軟件篇-03-基於ORB_SLAM2手寫SLAM稠密地圖構建實現

本文使用的方法不是從內部修改ORBSLAM2源碼以獲取稠密點雲，而是先從ZED2 sdk獲取以攝像頭坐標系為描述的三維點雲/作為點雲地圖的一個子集，然后融合IMU與ORB_SLAM2進行實時定位，通過點雲濾波，點雲融合建圖。

 

以上是在室內實驗的demo，由於是純雙目，沒有深度傳感器，在白牆和地板上有些失真，下次等移動平台到了我會去室外實驗。

 

一、獲取實時坐標和點雲圖

 

使用ORBSLAM2獲取當前姿態，同時ZED2 利用其IMU數據對速度加速度積分得出另一個姿態，考慮到ORBSLAM2的響應及時性和IMU數據的漂移，當兩者數據相差較大時停止建圖，等待恢復正常，否則以ORB_SLAM2的姿態信息為准，同時手動添加損失量對IMU姿態信息進行校准。在某些情況下ORB_SLAM2可能會跟丟，此時通過IMU數據積分獲取迪卡爾空間位移變化量，並回到之前的位置重新確定位置。

 

if(!startTimer) 
{ 
　　timeLast = ros::Time::now().toSec(); startTimer = 1; 
　　ROS_ERROR("\noffect between two poseMsgs is too big, stop mapping...");
　　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); 
}else if(timeNow = ros::Time::now().toSec() - timeLast > 10) 
{ 
　　startTimer = 0; 
　　timeLast = timeNow = 0;
　　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; 
　　z_bias = carTF_zed2.pose.position.z - carTF_orb.pose.position.z; 
}

 

這里我訂閱ZED2 sdk輸出的實時點雲，有一點需要注意的是實時的點雲和姿態信息必需要時間戳同步，不然融合出來的地圖會發生很大的偏移和扭曲。使用ros message_filters管理消息同步，可以設置弱同步和強同步。

 

imu_sub = n.subscribe("/zed/zed_node/imu/data", 1, &MapBuild::imuCallback,this); 
carTF_orb_sub = n.subscribe("/orb_slam2_stereo/pose", 1, &MapBuild::carTF_orb_Callback,this); 
pointCloud_sub = new message_filters::Subscriber<sensor_msgs::PointCloud2> ( n, "/zed2/zed_node/point_cloud/cloud_registered", 1); 
carTF_zed2_sub = new message_filters::Subscriber<geometry_msgs::PoseStamped> (n, "/zed2/zed_node/pose", 1); 
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里有關四元數的寫法順序是顛倒的，大家注意一下。

Quaterniond quaternion(carTF_zed2.pose.orientation.w, carTF_zed2.pose.orientation.x, carTF_zed2.pose.orientation.y, carTF_zed2.pose.orientation.z); 
Matrix3d rotation_matrix; rotation_matrix=quaternion.toRotationMatrix(); 

// transform the cloud link to the "map" frame

Vector3d position_transform (carTF_zed2.pose.position.x - x_bias, carTF_zed2.pose.position.y - y_bias, carTF_zed2.pose.position.z - z_bias); 

for (int i=0; i<cloud_xyz->width; i++) 
{ 
　　Vector3d position_(cloud_xyz->at(i).x,cloud_xyz->at(i).y,cloud_xyz->at(i).z); 
　　Vector3d position = rotation_matrix*position_ + position_transform; 
　　cloud_xyz->at(i).x = position[0]; 
　　cloud_xyz->at(i).y = position[1]; 
　　cloud_xyz->at(i).z = position[2];
}

 

三、點雲濾波

 

 

可以看到，圖中綠色的點雲原本是牆面和窗簾，但是在邊緣卻有很多"飛點"

我們獲取的原始點雲有很多的噪聲點並且密度太大，TX2吃不消，因此這里要對這些點雲進行濾波，這里使用pcl的體素濾波和直通濾波。

 

// Perform the actual filtering 
// VoxelGrid(decrease the memory occupation) & PassThrough(delete some incorrect points) 

pcl::PCLPointCloud2* cloud2 = new pcl::PCLPointCloud2; 
pcl::PCLPointCloud2ConstPtr cloudPtr(cloud2); 
pcl_conversions::toPCL(*cloud, *cloud2); 

// VoxelGrid pcl::PCLPointCloud2* cloud_filtered_1 = new pcl::PCLPointCloud2; 
pcl::PCLPointCloud2ConstPtr cloud_filter_1_Ptr(cloud_filtered_1); 
pcl::VoxelGrid<pcl::PCLPointCloud2> filter_1; 
filter_1.setInputCloud (cloudPtr); 
filter_1.setLeafSize (0.03, 0.03, 0.03); filter_1.filter(*cloud_filtered_1); 
// 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);

 

四、點雲融合

 

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

 

五、去除重復的點雲

 

實驗過程中不可避免的會回到某個之前來過的姿態，這個時候不能任由重復的點雲在我的地圖上大行其道，因此需要實時判斷當前的點雲是否已經添加到地圖中去了，使用pcl::registration::CorrespondenceEstimation判斷當前點雲和地圖有多少重復的點，該數目與點雲總體數目之比如果大於某個閾值，則丟棄該點雲。

pcl::registration::CorrespondenceEstimation<pcl::PointXYZRGB, pcl::PointXYZRGB> est; cloud_xyzFusedPtr = cloud_xyzFused->makeShared(); 
cloud_xyzPtr = cloud_xyz->makeShared(); 
est.setInputSource (cloud_xyzPtr); 
est.setInputTarget (cloud_xyzFusedPtr);
pcl::Correspondences all_correspondences; 
// Determine all reciprocal correspondences 

est.determineReciprocalCorrespondences (all_correspondences); 
// filter the reciprocal points cloud 

if(1.0*all_correspondences.size()/cloud_xyz->width < 0.9) { 
　　// fused the current cloud to the fused cloud 
　　*cloud_xyzFused += *cloud_xyz; pcl::toROSMsg(*cloud_xyzFused, mPointcloudFusedMsg); 
　　mPointcloudFusedMsg.header.frame_id = "map"; 
　　pointCloudFused_pub.publish(mPointcloudFusedMsg); 
}

我們下期再見

./下期就有小車實際的實驗了