时间戳

首页 > 技术文章 > 软件篇-03-基于ORB_SLAM2手写SLAM稠密地图构建实现

软件篇-03-基于ORB_SLAM2手写SLAM稠密地图构建实现

QiQi-Robotics 2020-08-27 09:29 原文

本文使用的方法不是从内部修改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<sensor_msgs::PointCloud2> ( n, "/zed2/zed_node/point_cloud/cloud_registered", 1); 
4 carTF_zed2_sub = new message_filters::Subscriber<geometry_msgs::PoseStamped> (n, "/zed2/zed_node/pose", 1); 
5 sync_ = new message_filters::Synchronizer<sync_pol> (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; i<cloud_xyz->width; 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 }
 
三、点云滤波
 
 
可以看到，图中绿色的点云原本是墙面和窗帘，但是在边缘却有很多&amp;quot;飞点&amp;quot;
我们获取的原始点云有很多的噪声点并且密度太大，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<pcl::PCLPointCloud2> 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<pcl::PCLPointCloud2> 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<pcl::PointXYZRGB, pcl::PointXYZRGB> 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 }
我们下期再见
./下期就有小车实际的实验了
 

推荐阅读