PL-ICP帧间匹配lesson3

一、scan_tools  lesson3 scan_match_plicp

本篇文章对应的代码为lesson3  

ros中有人使用使用csm包进行了ros下的实现，进行了扫描匹配与位姿累加的实现，但是没有发布odometry的topic与tf．包的名字为 laser_scan_matcher，是scan_tools包集中的一个．

首先介绍一下scan_tools包集，这个包里提供了很多操作二维激光雷达数据的功能，虽然不一定能直接用，但是其处理数据的思路是非常值得借鉴的．其wiki的地址为: scan_tools - ROS Wiki

1.2 功能简介 

其包含的功能包名字如下所示，并对其功能进行了简要介绍．

laser_ortho_projector: 将切斜的雷达数据投影到平面上．
laser_scan_matcher: 基于pl-icp的扫描匹配的实现，并进行了位姿累加
laser_scan_sparsifier: 对雷达数据进行稀疏处理
laser_scan_splitter: 将一帧雷达数据分段，并发布出去
ncd_parser: 读取New College Dataset，转换成ros的scan 与 odometry 发布出去
polar_scan_matcher: 基于Polar Scan Matcher的扫描匹配器的ros实现
scan_to_cloud_converter: 将 sensor_msgs/LaserScan 数据转成 sensor_msgs/PointCloud2 的数据格式．
楼主前的那篇将雷达数据转成 sensor_msgs/PointCloud2 的文章，就是参考scan_to_cloud_converter包来实现的．

如果想要深入学习一下的请去wiki的网址，看介绍及源码，这里就不再过多介绍了．

1.3依赖

sudo apt-get install ros-noetic-csm

1.4CMakeLists

需要在CMakeLists.txt中添加如下内容，以在程序中使用csm

# Find csm project
find_package(PkgConfig)
pkg_check_modules(csm REQUIRED csm)
include_directories(
  include
  ${catkin_INCLUDE_DIRS}
  ${csm_INCLUDE_DIRS} 
)
link_directories(${csm_LIBRARY_DIRS})

#Create library
add_executable(${PROJECT_NAME}_scan_match_plicp_node src/scan_match_plicp.cc)

#Note we don't link against pcl as we're using header-only parts of the library
target_link_libraries( ${PROJECT_NAME}_scan_match_plicp_node 
${catkin_LIBRARIES} 
${csm_LIBRARIES}
)

add_dependencies(${PROJECT_NAME}_scan_match_plicp_node 
${csm_EXPORTED_TARGETS} 
${catkin_EXPORTED_TARGETS}
)

1.5 csm算法 

scan_match_plicp 

1.5 .1回调函数

首先要进行初始化，初始化的工作做了第一帧雷达数据的赋值，第一帧时间的赋值．

并且计算了雷达数据对应的每个角度值的cos与sin，用于节省计算量．

之后再将雷达数据转换成csm需要的格式

1.6

类中的私有成员函数， 可以 通过类中的公共函数 来调用其他的私有成员函数

私有成员函数特性， 类外不能直接调用， 类内可以访问

一般封装的话    回调函数放在类的 public 中，  其他的放在private中 ， 在回调函数Callback中访问  

//公共权限  public     类内可以访问  类外可以访问
//保护权限  protected  类内可以访问  类外不可以访问
//私有权限  private    类内可以访问  类外不可以访问

1.7编译和运行

由于是新增的包，所以需要通过 rospack profile 命令让ros找到这个新的包．

roslaunch lesson3 scan_match_plicp.launch

 二、基于PL-ICP的激光雷达里程计 lesson3 plicp_odometry

1.TF2

1.1坐标系

map: 地图坐标系，也被称为世界坐标系，是静止不动的
odom: 里程计坐标系，相对于map来说一般情况下是静止的，有些情况下会变动（定位节点为了修正机器人的位姿从而改变了map->odom间的坐标变换）
base_link: 代表机器人的旋转中心的坐标系，相对于odom来说base_link是运动的
laser_link: 激光雷达的坐标系，相对于base_link来说是静止的，因为雷达装在机器人上，雷达不会自己飞起来

可以看到，上边的坐标系，是单方向依赖的， laser_link 依赖于 base_link，base_link 依赖于 odom 。也可以说成是一个坐标系指向下一个坐标系的，连起来的话也成了：

map -> odom -> base_link -> laser_link

tf2常用数据类型与常用函数汇总_李太白lx的博客-CSDN博客

使用私有句柄 private_node_ 获取节点内部参数，参数写在配置文件中，配置文件稍后讲解 

2、 lookupTransform

// 获取tf并不是瞬间就能获取到的，要给1秒的缓冲时间让其找到tf

虽然在tf树中可以看到各个坐标之间的已经存在，但是却 Buffer::lookupTransform（）时报错，主要是因为在使用时，大家都默认不填写第四个参数timeout或填写的时间比较短而导致的。这个参数表示最长等待监听时间，如果没有这个参数，那么监听一次，没有查询到数据，就失败，但是，有时候lookupTransform（）函数处理时间比较长，所以导致出错。
 

bool ScanMatchPLICP::GetBaseToLaserTf(const std::string &frame_id)
{
    ros::Time t = ros::Time::now();

    geometry_msgs::TransformStamped transformStamped;
    // 获取tf并不是瞬间就能获取到的，要给1秒的缓冲时间让其找到tf
    try
    {
        transformStamped = tfBuffer_.lookupTransform(base_frame_, frame_id,
                                                     t, ros::Duration(1.0));
    }
    catch (tf2::TransformException &ex)
    {
        ROS_WARN("%s", ex.what());
        ros::Duration(1.0).sleep();
        return false;
    }

    // 将获取的tf存到base_to_laser_中
    tf2::fromMsg(transformStamped.transform, base_to_laser_);
    laser_to_base_ = base_to_laser_.inverse();

    return true;
}

3、 ScanMatchWithPLICP()

增加了 基于匀速模型的位姿预测，位姿累加，发布TF以及odom话题，新建关键帧，这4个功能

2、坐标转化

  if (output_.valid)
    {
        // 雷达坐标系下的坐标变换
        tf2::Transform corr_ch_l;
        CreateTfFromXYTheta(output_.x[0], output_.x[1], output_.x[2], corr_ch_l);

        // 将雷达坐标系下的坐标变换 转换成 base_link坐标系下的坐标变换
        corr_ch = base_to_laser_ * corr_ch_l * laser_to_base_;

        // 更新 base_link 在 odom 坐标系下 的坐标
        base_in_odom_ = base_in_odom_keyframe_ * corr_ch;

        latest_velocity_.linear.x = corr_ch.getOrigin().getX() / dt;
        latest_velocity_.angular.z = tf2::getYaw(corr_ch.getRotation()) / dt;
    }

3运行

3.1 生成新bag文件

1、由于 lesson1 这个数据包中已经有了tf，而本篇文章的代码也要发布TF，为了让流程更加清晰，现在我重新录制一个没有tf的数据包．通过分别在3个终端中执行如下命令进行新bag的录制

2也可以不重新录制包，只要将我们代码的TF的odom坐标系，改个名字如odom_plicp，再将Rviz的Fixed Frame设置成odom_plicp也可以

# 终端1
cd ~/bagfiles
roscore
# 终端2
rosparam set use_sim_time true
cd ~/bagfiles
rosbag play --clock lesson1.bag
# 终端3
cd ~/bagfiles
rosbag record /laser_scan /odom
 

use_sim_time

这个参数当回放bag数据集是设置为true，此时说明系统使用的是仿真时间Time；如果设置为false，则系统使用walltime。（此参数可以通过launch文件中设置，或者通过节点设置）

• 发布时钟
  默认/clock话题是没有消息的。

rosbag play --clock bag_name.bag #默认/clock的话题频率100hz rosbag play --clock --hz 200 bag_name.bag #指定/clock的时钟为200hz

三、ros

1、roll,pitch,yall

 //位姿信息中没有四元数， 但有偏航角度，又已知乌龟是2d，没有翻滚与付仰， 所以为 0  0  角度   ，偏航
   tf2::Quaternion qtn;
   qtn.setRPY(0,0,pose->theta);

2、nav_msgs geometry_msgs 两者区别

【ROS】rosbag使用_Amelie_xiao的博客-CSDN博客_rosbag

红色轨迹为通过小车自身编码器累加出来的里程计，黄色轨迹为通过PL-ICP算法累加出来的激光雷达里程计