参考网址:
xiangli0608/Creating-2D-laser-slam-from-scratch: 从零开始创建二维激光SLAM (github.com)
李想的博客_李太白lx_CSDN博客-从零开始搭二维激光SLAM,激光SLAM,cartographer领域博主
(107条消息) ROS中点云学习(九):sensor_msgs::LaserScan转sensor_msgs::PointCloud2_00 2的博客-CSDN博客
lesson2
格式转换(sensor_msgs/LaserScan -> sensor_msgs/PointCloud2 )
1 2 3 4 5 6 7 8 9 10 11
| # sensor_msgs/LaserScan std_msgs/Header header float32 angle_min float32 angle_max float32 angle_increment float32 time_increment float32 scan_time float32 range_min float32 range_max float32[] ranges float32[] intensities
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22
| # sensor_msgs/PointCloud2 std_msgs/Header header # width - 以点数指定点云数据集的宽度。 WIDTH有两个含义: # * - 它可以为 无组织数据集 指定云中的总点数; # * - 它可以指定 有组织的点云数据集 的宽度(一行中的总点数). uint32 height # height - 以点数指定点云数据集的高度。HEIGHT有两个含义: # * - 对于 无组织数据集,它设置为1(因此用于检查数据集是否ordered)。 # * - 它可以指定 有组织的点云数据集 的高度(总行数); uint32 width # 描述二进制数据blob中的通道及其布局。 sensor_msgs/PointField[] fields # 是大端数据格式吗? bool is_bigendian # 以字节为单位的点的长度 uint32 point_step # 行的长度(以字节为单位) uint32 row_step # 实际数据(二进制的数据流),大小为(row_step * height) uint8[] data # 如果包含Inf / NaN值,则为false,否则为真 bool is_dense
|
1 2 3 4 5 6
| # ros中自动做了 pcl::PointCloud<PointT> 到 sensor_msgs/PointCloud2 的数据类型的转换 std_msgs/Header header uint32 height uint32 width PointT* point_tmp bool is_dense
|
相对来说比较简单,就是将scan数据转化为PointT数据发布,且ros自带转化成pointcloud2功能,需要注意的是对于无效数据需要特殊处理
1 2 3 4
|
pointcloud2_publisher_ = node_handle_.advertise<PointCloudT>( "pointcloud2_converted", 1, this);
|
无效点处理
1 2 3 4 5 6 7
| if (!std::isfinite(range)) { point_tmp = invalid_point_; continue; }
|
point坐标
1 2 3 4 5 6 7 8 9 10 11 12 13 14
|
if (range > scan_msg->range_min && range < scan_msg->range_max) { float angle = scan_msg->angle_min + i * scan_msg->angle_increment; point_tmp.x = range * cos(angle); point_tmp.y = range * sin(angle); point_tmp.z = 0.0; } else
point_tmp = invalid_point_;
|
发布
1 2 3 4 5 6 7 8 9
| cloud_msg->width = scan_msg->ranges.size(); cloud_msg->height = 1; cloud_msg->is_dense = false; // contains nans // 将scan_msg的消息头 赋值到 PointCloudT的消息头 pcl_conversions::toPCL(scan_msg->header, cloud_msg->header);
// 由于ros中自动做了 pcl::PointCloud<PointT> 到 sensor_msgs/PointCloud2 的数据类型的转换 // 所以这里直接发布 pcl::PointCloud<PointT> 即可 pointcloud2_publisher_.publish(cloud_msg);
|
icp算法
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25
|
icp_.setInputSource(last_pointcloud_); icp_.setInputTarget(current_pointcloud_);
pcl::PointCloud<pcl::PointXYZ> unused_result; icp_.align(unused_result);
if (icp_.hasConverged() == false){ std::cout << "not Converged" << std::endl; return; } else { Eigen::Affine3f transfrom; transfrom = icp_.getFinalTransformation();
float x, y, z, roll, pitch, yaw; pcl::getTranslationAndEulerAngles(transfrom, x, y, z, roll, pitch, yaw); std::cout << "transfrom: (" << x << ", " << y << ", " << yaw * 180 / M_PI << ")" << std::endl; }
|
总结
1 2 3 4 5 6 7 8 9 10 11 12
| 迭代最近点(Iterative Closest Point, 下简称ICP)算法是一种点云匹配算法。
其求解思路为: 首先对于一幅点云中的每个点,在另一幅点云中计算匹配点(最近点) 极小化匹配点间的匹配误差,计算位姿 然后将计算的位姿作用于点云 再重新计算匹配点 如此迭代,直到迭代次数达到阈值,或者极小化函数的变化量小于设定阈值
首先,ICP对初值比较敏感,初值给的不好,就需要花费更多的迭代次数进行匹配. 其次,由于它是迭代很多次的,所以其花费的时间很长,这一点是非常致命的,之后我会通过程序来让大家体验ICP的费时. 再次,精度与速度是矛盾的,ICP算法理论上可以实现很高的精度,但是要很多很多的迭代次数,以及很长的时间.所以,当限制了迭代次数的情况下,精度就不一定能保证了.
|