1. 实验5：基于STDR的ICP仿真
1.1 实验内容
1、在ROS中，仿真激光雷达，通过ROS消息，获得前后两组激光雷达消息(点云数据)A1与A2。
2、借鉴PPT中的ICP算法模型，求解A1与A2的平移矩阵与旋转矩阵。
3、在ROS中绘制出来两个点云A1与A2的变换，在实验效果上验证ICP的有效性。
安装ROS（含有rviz）
PCL（Point Cloud Library）-ros 是ROS中点云和3D几何处理的接口和工具。 使用下面的命令安装：
发布点云：
#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
int main(int argc, char** argv)
{
ros::init (argc, argv, "pub_pcl");
ros::NodeHandle nh;
ros::Publisher pub = nh.advertise<PointCloud> ("points2", 1);
PointCloud::Ptr msg (new PointCloud);
msg->header.frame_id = "some_tf_frame";
msg->height = msg->width = 1;
msg->points.push_back (pcl::PointXYZ(1.0, 2.0, 3.0));
ros::Rate loop_rate(4);
while (nh.ok())
{
msg->header.stamp = ros::Time::now().toNSec();
pub.publish (msg);
ros::spinOnce ();
loop_rate.sleep ();
}
}
#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <boost/foreach.hpp>
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
void callback(const PointCloud::ConstPtr& msg)
{
printf ("Cloud: width = %d, height = %d\n", msg->width, msg->height);
BOOST_FOREACH (const pcl::PointXYZ& pt, msg->points)
printf ("\t(%f, %f, %f)\n", pt.x, pt.y, pt.z);
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "sub_pcl");
ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe<PointCloud>("points2", 1, callback);
ros::spin();
}
mkdir -p ~/rslidar_ws/src
cd ~/rslidar_ws/src
git clone https://github.com/RoboSense-LiDAR/ros_rslidar
cd ..
catkin_make #编译和安装
启用主节点
发布激光点云
rosbag play rslidar_test.bag
rosbag play -r 100 rslidar_test.bag #100倍速度播放
rosbag play -r 100 rslidar_test.bag >> bagtime.txt
运行