本文使用的方法不是从内部修改ORBSLAM2源码以获取稠密点云，而是先从ZED2 sdk获取以摄像头坐标系为描述的三维点云/作为点云地图的一个子集，然后融合IMU与ORB_SLAM2进行实时定位，通过点云滤波，点云融合建图。

以上是在室内实验的demo，由于是纯双目，没有深度传感器，在白墙和地板上有些失真，下次等移动平台到了我会去室外实验。

一、获取实时坐标和点云图

使用ORBSLAM2获取当前姿态，同时ZED2 利用其IMU数据对速度加速度积分得出另一个姿态，考虑到ORBSLAM2的响应及时性和IMU数据的漂移，当两者数据相差较大时停止建图，等待恢复正常，否则以ORB_SLAM2的姿态信息为准，同时手动添加损失量对IMU姿态信息进行校准。在某些情况下ORB_SLAM2可能会跟丢，此时通过IMU数据积分获取迪卡尔空间位移变化量，并回到之前的位置重新确定位置。

1 if(!startTimer)
2 {
3 　　timeLast = ros::Time::now().toSec(); startTimer = 1;
4 　　ROS_ERROR("\noffect between two poseMsgs is too big, stop mapping…");
5 　　ROS_WARN("the offset from zed2Pose to orbPose2 is:\nx:%f y:%f z:%f \n-------------" ,carTF_zed2.pose.position.x - carTF_orb.pose.position.x ,carTF_zed2.pose.position.y - carTF_orb.pose.position.y ,carTF_zed2.pose.position.z - carTF_orb.pose.position.z);
6 }else if(timeNow = ros::Time::now().toSec() - timeLast > 10)
7 {
8 　　startTimer = 0;
9 　　timeLast = timeNow = 0;
10 　　ROS_WARN("Don't warry, it seems that something wrong happend, trying to fix it…"); x_bias = carTF_zed2.pose.position.x - carTF_orb.pose.position.x; y_bias = carTF_zed2.pose.position.y - carTF_orb.pose.position.y;
11 　　z_bias = carTF_zed2.pose.position.z - carTF_orb.pose.position.z;
12 }

这里我订阅ZED2 sdk输出的实时点云，有一点需要注意的是实时的点云和姿态信息必需要时间戳同步，不然融合出来的地图会发生很大的偏移和扭曲。使用ros message_filters管理消息同步，可以设置弱同步和强同步。

1 imu_sub = n.subscribe("/zed/zed_node/imu/data", 1, &MapBuild::imuCallback,this);
2 carTF_orb_sub = n.subscribe("/orb_slam2_stereo/pose", 1, &MapBuild::carTF_orb_Callback,this);
3 pointCloud_sub = new message_filters::Subscriber ( n, "/zed2/zed_node/point_cloud/cloud_registered", 1);
4 carTF_zed2_sub = new message_filters::Subscriber (n, "/zed2/zed_node/pose", 1);
5 sync_ = new message_filters::Synchronizer (sync_pol(10), *pointCloud_sub, *carTF_zed2_sub); sync_->registerCallback(boost::bind(&MapBuild::buildMap_callback, this, _1, _2));

二、点云坐标系变换

我们当前的点云是相对摄像头坐标系的，但是建图就要将这些点转换到世界坐标系，点的坐标系变换我这里就不讲了，不懂的去看一下《机器人学导论》/题外话“英文的原汁原味”。要用到的工具当然是Eigen了。这里吐槽一下，Eigen和geometry_msgs::PoseStamped里有关四元数的写法顺序是颠倒的，大家注意一下。

1 Quaterniond quaternion(carTF_zed2.pose.orientation.w, carTF_zed2.pose.orientation.x, carTF_zed2.pose.orientation.y, carTF_zed2.pose.orientation.z);
2 Matrix3d rotation_matrix; rotation_matrix=quaternion.toRotationMatrix();
3
4 // transform the cloud link to the "map" frame
5
6 Vector3d position_transform (carTF_zed2.pose.position.x - x_bias, carTF_zed2.pose.position.y - y_bias, carTF_zed2.pose.position.z - z_bias);
7
8 for (int i=0; iwidth; i++)
9 {
10 　　Vector3d position_(cloud_xyz->at(i).x,cloud_xyz->at(i).y,cloud_xyz->at(i).z);
11 　　Vector3d position = rotation_matrix*position_ + position_transform;
12 　　cloud_xyz->at(i).x = position[0];
13 　　cloud_xyz->at(i).y = position[1];
14 　　cloud_xyz->at(i).z = position[2];
15 }

三、点云滤波

可以看到，图中绿色的点云原本是墙面和窗帘，但是在边缘却有很多"飞点"

我们获取的原始点云有很多的噪声点并且密度太大，TX2吃不消，因此这里要对这些点云进行滤波，这里使用pcl的体素滤波和直通滤波。

1 // Perform the actual filtering
2 // VoxelGrid(decrease the memory occupation) & PassThrough(delete some incorrect points)
3
4 pcl::PCLPointCloud2* cloud2 = new pcl::PCLPointCloud2;
5 pcl::PCLPointCloud2ConstPtr cloudPtr(cloud2);
6 pcl_conversions::toPCL(*cloud, *cloud2);
7
8 // VoxelGrid pcl::PCLPointCloud2* cloud_filtered_1 = new pcl::PCLPointCloud2;
9 pcl::PCLPointCloud2ConstPtr cloud_filter_1_Ptr(cloud_filtered_1);
10 pcl::VoxelGrid filter_1;
11 filter_1.setInputCloud (cloudPtr);
12 filter_1.setLeafSize (0.03, 0.03, 0.03); filter_1.filter(*cloud_filtered_1);
13 // PassThrough pcl::PCLPointCloud2* cloud_filtered_2 = new pcl::PCLPointCloud2; pcl::PCLPointCloud2ConstPtr cloud_filter_2_Ptr(cloud_filtered_2); pcl::PassThrough filter_2; filter_2.setInputCloud (cloud_filter_1_Ptr); filter_2.setFilterFieldName ("y"); filter_2.setFilterLimits (-1.2, 1.2); // filter_2.setFilterLimitsNegative (true); filter_2.filter(*cloud_filtered_2); pcl::PCLPointCloud2 cloud_filtered_3; filter_2.setInputCloud (cloud_filter_2_Ptr); filter_2.setFilterFieldName ("z"); filter_2.setFilterLimits (-2,2);// filter_2.setFilterLimitsNegative (true); filter_2.filter(cloud_filtered_3);

四、点云融合

1 // fused the current cloud to the fused cloud
2 *cloud_xyzFused += *cloud_xyz; pcl::toROSMsg(*cloud_xyzFused, mPointcloudFusedMsg);
3 mPointcloudFusedMsg.header.frame_id = "map"; pointCloudFused_pub.publish(mPointcloudFusedMsg);

五、去除重复的点云

实验过程中不可避免的会回到某个之前来过的姿态，这个时候不能任由重复的点云在我的地图上大行其道，因此需要实时判断当前的点云是否已经添加到地图中去了，使用pcl::registration::CorrespondenceEstimation判断当前点云和地图有多少重复的点，该数目与点云总体数目之比如果大于某个阈值，则丢弃该点云。

1 pcl::registration::CorrespondenceEstimation est; cloud_xyzFusedPtr = cloud_xyzFused->makeShared();
2 cloud_xyzPtr = cloud_xyz->makeShared();
3 est.setInputSource (cloud_xyzPtr);
4 est.setInputTarget (cloud_xyzFusedPtr);
5 pcl::Correspondences all_correspondences;
6 // Determine all reciprocal correspondences
7
8 est.determineReciprocalCorrespondences (all_correspondences);
9 // filter the reciprocal points cloud
10
11 if(1.0*all_correspondences.size()/cloud_xyz->width < 0.9) {
12 　　// fused the current cloud to the fused cloud
13 　　*cloud_xyzFused += *cloud_xyz; pcl::toROSMsg(*cloud_xyzFused, mPointcloudFusedMsg);
14 　　mPointcloudFusedMsg.header.frame_id = "map";
15 　　pointCloudFused_pub.publish(mPointcloudFusedMsg);
16 }

我们下期再见

./下期就有小车实际的实验了