DS-SLAM代码解析和问题整理 (三)
目录
-
- ORBextractor::DeleteOneRowOfMat
- ORBextractor::CheckMovingKeyPoints
- ORBextractor::ProcessDesp
- PointCloudMapping::PointCloudMapping
- PointCloudMapping::shutdown
- PointCloudMapping::insertKeyFrame
- PointCloudMapping::generatePointCloud
- PointCloudMapping::viewer
- PointCloudMapping::Cloud_transform
接着我们来看ORBextractor.cc修改了哪些内容
ORBextractor::DeleteOneRowOfMat
系统中没有使用
ORBextractor::CheckMovingKeyPoints
一个通过分割图 imgS 和潜在的动态点 T 去除关键点 mvKeysT 中的位于人身上的动态点的函数。
int ORBextractor::CheckMovingKeyPoints( const cv::Mat &imGray, // 灰度图
const cv::Mat &imS, // 分割图
std::vector<std::vector<cv::KeyPoint>>& mvKeysT, // 关键点
std::vector<cv::Point2f> T) // 光流点T矩阵
为了方便理解,我在这里贴一下这个函数的源头:
ros_tum_realtime中的TrackRGBD -> System::TrackRGBD ->Tracking.::GrabImageRGBD ->Frame::CalculEverything -> ORBextractor::CheckMovingKeyPoints
这个函数主要执行了
- step1 :检查剔除区域（T_M集中每个像素点及周围15像素）,查看是否具有动态先验“人”的标签
- step2 :如果存在"人",就将位于人身上的部分提取的ORB特征点全部剔除
我们现在来具体的看一下函数:
- 设定"人"是否存在的flag
int flag_orb_mov =0;
- 检查剔除区域,如果在T_M 离群点的 15*15 的像素块内发现 “人”，那么flag_orb_mov 设置为1.
for (int i = 0; i < T.size(); i++)
{
for(int m = -15; m < 15; m++)
{
for(int n = -15; n < 15; n++)
{
// 确定 mx ,my 的范围不能出界
int my = ((int)T[i].y + n) ;
int mx = ((int)T[i].x + m) ;
if( ((int)T[i].y + n) > (Camera::height -1) ) my = (Camera::height - 1) ;
if( ((int)T[i].y + n) < 1 ) my = 0;
if( ((int)T[i].x + m) > (Camera::width -1) ) mx = (Camera::width - 1) ;
if( ((int)T[i].x + m) < 1 ) mx = 0;
// The label of peopel is 15
if((int)imS.ptr<uchar>(my)[mx] == PEOPLE_LABLE)
{
flag_orb_mov=1;
break;
}
}
if(flag_orb_mov==1)
break;
}
if(flag_orb_mov==1)
break;
}
- 如果有"人"，那么就将位于人身上的部分提取的ORB特征点全部剔除。(在每一层特征点金字塔内删除该特征点)
if(flag_orb_mov==1)
{
for (int level = 0; level < nlevels; ++level)
{
vector<cv::KeyPoint>& mkeypoints = mvKeysT[level]; // 提取每一层的金字塔的特征点
int nkeypointsLevel = (int)mkeypoints.size();
if(nkeypointsLevel==0)
continue;
if (level != 0) //获取非零层金字塔的缩放系数
scale = mvScaleFactor[level];
else
scale = 1;
vector<cv::KeyPoint>::iterator keypoint = mkeypoints.begin();
// 遍历每个特征点
while(keypoint != mkeypoints.end())
{
cv::Point2f search_coord = keypoint->pt * scale; // 对点进行缩放
// Search in the semantic image
if(search_coord.x >= (Camera::width -1))
search_coord.x=(Camera::width -1);
if(search_coord.y >= (Camera::height -1))
search_coord.y=(Camera::height -1) ;
// 用来访问灰度图像的单个像素。对于灰度图像，每个像素只存储一个值
int label_coord =(int)imS.ptr<uchar>((int)search_coord.y)[(int)search_coord.x]; //获得语义分割图该坐标点的像素值(标签值??)
// 发现这个特征点的坐标落在"人"身上，则把这个特征点删除
if(label_coord == PEOPLE_LABLE)
{
keypoint=mkeypoints.erase(keypoint); // 将这个特征点删除掉
}
else
{
keypoint++;
}
}
}
}
return flag_orb_mov; /// 至此，动态特征点的删除工作就已经做完了，保留的特征点认为是静态的点，可以参与后续的描述子计算，去畸变和Track部分
ORBextractor::ProcessDesp
把计算ORB描述子单独放在了一个函数中.就不细说了
void ORBextractor::ProcessDesp(cv::InputArray _image, cv::InputArray _mask, vector<vector<cv::KeyPoint>>& _allKeypoints,
vector<cv::KeyPoint>& _mKeypoints,cv::OutputArray _descriptors)
{
cv::Mat descriptors;
//统计整个图像金字塔中的特征点
int nkeypoints = 0;
//开始遍历每层图像金字塔，并且累加每层的特征点个数
for (int level = 0; level < nlevels; ++level)
nkeypoints += (int)_allKeypoints[level].size();
//如果本图像金字塔中没有任何的特征点
if( nkeypoints == 0 )
_descriptors.release();
else
{
//如果图像金字塔中有特征点，那么就创建这个存储描述子的矩阵，注意这个矩阵是存储整个图像金字塔中特征点的描述子的
_descriptors.create(nkeypoints, 32, CV_8U);
//获取这个描述子的矩阵信息
descriptors = _descriptors.getMat();
}
//清空用作返回特征点提取结果的vector容器
_mKeypoints.clear();
//并预分配正确大小的空间
_mKeypoints.reserve(nkeypoints);
//因为遍历是一层一层进行的，但是描述子那个矩阵是存储整个图像金字塔中特征点的描述子，所以在这里设置了Offset变量来保存“寻址”时的偏移量，
int offset = 0;
//开始遍历每一层图像
for (int level = 0; level < nlevels; ++level)
{
//获取在allKeypoints中当前层特征点容器的句柄
vector<cv::KeyPoint>& keypoints = _allKeypoints[level];
//本层的特征点数
int nkeypointsLevel = (int)keypoints.size();
//如果特征点数目为0，跳出本次循环，继续下一层金字塔
if(nkeypointsLevel==0)
continue;
// Preprocess the resized image
// 对图像进行高斯模糊
// 深拷贝当前金字塔所在层级的图像
cv::Mat workingMat = mvImagePyramid[level].clone();
// 注意：提取特征点的时候，使用的是清晰的原图像；这里计算描述子的时候，为了避免图像噪声的影响，使用了高斯模糊
GaussianBlur(workingMat, workingMat, cv::Size(7, 7), 2, 2, cv::BORDER_REFLECT_101);
// Compute the descriptors
// desc存储当前图层的描述子
cv::Mat desc = descriptors.rowRange(offset, offset + nkeypointsLevel);
// 计算高斯模糊后图像的描述子
computeDescriptors(workingMat, keypoints, desc, pattern);
// cv::imshow("desc",desc);
// cv::waitKey(0);
// 更新偏移量的值
offset += nkeypointsLevel;
// Scale keypoint coordinates
// 对非第0层图像中的特征点的坐标恢复到第0层图像（原图像）的坐标系下
if (level != 0)
{
// 获取当前图层上的缩放系数
float scale = mvScaleFactor[level];
for (vector<cv::KeyPoint>::iterator keypoint = keypoints.begin(),
keypointEnd = keypoints.end(); keypoint != keypointEnd; ++keypoint)
// 特征点本身直接乘缩放倍数就可以了
keypoint->pt *= scale;
}
// And add the keypoints to the output
// 将keypoints中内容插入到_keypoints 的末尾
_mKeypoints.insert(_mKeypoints.end(), keypoints.begin(), keypoints.end());
}
}
接着我们来看pointcloudmapping.cc里面的内容
PointCloudMapping::PointCloudMapping
PointCloudMapping类的初始化函数
PointCloudMapping::PointCloudMapping(double resolution_)
{
this->resolution = resolution_;
voxel.setLeafSize( resolution, resolution, resolution); //设置滤波时创建的体素为5cm的立方体
this->sor.setMeanK(50); //设置在统计时考虑的临近点个数
this->sor.setStddevMulThresh(1.0); //设置判断是否为离群点的阈值,用来倍乘标准差
//如果一个点得到距离超过平均距离加上一个标准差以上,则该点被标记为离群点
globalMap = boost::make_shared< PointCloud >( ); //定义为一个指针ptr管理globalMap
KfMap = boost::make_shared< PointCloud >( ); //定义为一个指针ptr管理KfMap
// 这里创建了一个 thread 线程对象，执行 PointCloudMapping::viewer函数
viewerThread = boost::make_shared<thread>( bind(&PointCloudMapping::viewer, this ) ); // viewer是一个死循环会一直显示点云数据。
}
PointCloudMapping::shutdown
将PointCloudMapping线程关闭
void PointCloudMapping::shutdown()
{
{
unique_lock<mutex> lck(shutDownMutex);
shutDownFlag = true;
keyFrameUpdated.notify_one(); // 唤醒一次
}
viewerThread->join();
}
PointCloudMapping::insertKeyFrame
我们将Track线程传递过来的 keyframe,语义分割图，深度图等放置到对应的容器里面
void PointCloudMapping::insertKeyFrame(KeyFrame* kf, // 关键帧
cv::Mat& semantic_color, // 彩色语义分割图
cv::Mat& semantic, // 语义分割图
cv::Mat& color, // RGB 图
cv::Mat& depth) // 深度图
{
unique_lock<mutex> lck(keyframeMutex);
keyframes.push_back( kf );
semanticImgs.push_back(semantic.clone() );
semanticImgs_color.push_back(semantic_color.clone() );
colorImgs.push_back( color.clone() );
depthImgs.push_back( depth.clone() );
keyFrameUpdated.notify_one();
}
PointCloudMapping::generatePointCloud
这个函数主要执行了:
-
step 1 :遍历所有像素点
-
step 1.1 在当前像素点20*20的范围内搜索是否有人
-
step 1.2 如果有人存在就不建人那部分的图像
-
step 1.3 将像素点反投影到三维点
-
step 1.4 对没有语义信息的点云赋予其原本的颜色
-
step 1.5 对于有语义标签的点云,将语义分割图片的颜色赋给点云，颜色信息从2D映射到3D
-
step 2 得到当前关键帧的位姿
-
step 3 将点云通过位姿矩阵进行变换
pcl::PointCloud< PointCloudMapping::PointT >::Ptr PointCloudMapping::generatePointCloud(KeyFrame* kf, cv::Mat& semantic_color,cv::Mat& semantic, cv::Mat& color, cv::Mat& depth)
{
PointCloud::Ptr tmp( new PointCloud() );
// step 1 遍历所有像素点
for ( int m=0; m<depth.rows; m+=1)
{
for ( int n=0; n<depth.cols; n+=1)
{
float d = depth.ptr<float>(m)[n];
if (d < 0.01 || d > 8)
continue;
int flag_exist=0; //人的标志位
// step 1.1在当前像素点20*20的范围内搜索是否有人
for (int i=-20;i <= 20; i+=3)
{
for (int j=-20;j <= 20; j+=3)
{
int tempx = m + i;
int tempy = n + j ;
if( tempx <= 0 ) tempx = 0;
if( tempx >= (Camera::height -1) ) tempx = Camera::height-1;
if( tempy <= 0 ) tempy = 0;
if( tempy >= (Camera::width -1) ) tempy = Camera::width -1;
if((int)semantic.ptr<uchar>(tempx)[tempy] == PEOPLE_LABLE)
{
flag_exist=1;
break;
}
}
if(flag_exist==1)
break;
}
// step 1.2 如果有人存在就不建人那部分的图像
if(flag_exist == 1)
continue;
// step 1.3 将像素点反投影到三维点
PointT p;
p.z = d;
p.x = ( n - Camera::cx) * p.z / Camera::fx;
p.y = ( m - Camera::cy) * p.z / Camera::fy;
// step 1.4 对没有语义信息的点云赋予其原本的颜色
// Deal with color
if((int)semantic.ptr<uchar>(m)[n]==0) //普通颜色
{
p.b = color.ptr<uchar>(m)[n*3];
p.g = color.ptr<uchar>(m)[n*3+1];
p.r = color.ptr<uchar>(m)[n*3+2];
}
else //step 1.5 对于有语义标签的点云,将语义分割图片的颜色赋给点云，颜色信息从2D映射到3D
{
p.b = semantic_color.ptr<uchar>(m)[n*3];
p.g = semantic_color.ptr<uchar>(m)[n*3+1];
p.r = semantic_color.ptr<uchar>(m)[n*3+2];
}
tmp->points.push_back(p);
}
}
// step 2 得到当前关键帧的位姿
Eigen::Isometry3d T = ORB_SLAM2::Converter::toSE3Quat( kf->GetPose() );
PointCloud::Ptr cloud(new PointCloud);
// step 3 将点云通过位姿矩阵进行变换 TODO:目的是什么?
pcl::transformPointCloud( *tmp, *cloud, T.inverse().matrix());
cloud->is_dense = false;
cout<<"Generate point cloud for kf "<<kf->mnId<<", size="<<cloud->points.size()<<endl;
return cloud;
} // 返回点云后，将该关键帧的点云以topic的形式发布出去。Rviz会自行对于点云信息进行拼接，至此，整个语义建图部分完成。
PointCloudMapping::viewer
viewer是一个死循环会一直显示点云数据
void PointCloudMapping::viewer()
{
ros::NodeHandle n; // 创建一个ros句柄
// 发布了消息类型为sensor_msgs::PointCloud2的消息，话题为/ORB_SLAM2_PointMap_SegNetM/Point_Clouds" ，消息队列的大小为100000
pclPoint_pub = n.advertise<sensor_msgs::PointCloud2>("/ORB_SLAM2_PointMap_SegNetM/Point_Clouds",100000);
ros::Rate r(5);
while(1)
{
{
unique_lock<mutex> lck_shutdown( shutDownMutex );
if (shutDownFlag) //如果需要停止,则viewer停止
{
break;
}
}
{
unique_lock<mutex> lck_keyframeUpdated( keyFrameUpdateMutex );
keyFrameUpdated.wait( lck_keyframeUpdated );
}
size_t N=0; // 关键帧的数目
{
unique_lock<mutex> lck( keyframeMutex );
N = keyframes.size();
}
if(N==0) // 如果关键帧数目为0
{
cout<<"Keyframes miss!"<<endl;
usleep(1000);
continue;
}
KfMap->clear();
for ( size_t i=lastKeyframeSize; i<N ; i++ )
{
//生成点云
PointCloud::Ptr p = generatePointCloud( keyframes[i],semanticImgs_color[i], semanticImgs[i],colorImgs[i], depthImgs[i] );
*KfMap += *p;
*globalMap += *p;
}
PointCloud::Ptr tmp1(new PointCloud());
voxel.setInputCloud( KfMap ); // 设置需要过滤的点云给滤波对象
voxel.filter( *tmp1 ); // 执行滤波操作
KfMap->swap( *tmp1 ); // 将点云与其他云进行短暂交换
pcl_cloud_kf = *KfMap; // 将滤波后的点云赋值给pcl_cloud_kf
Cloud_transform(pcl_cloud_kf,pcl_filter);
pcl::toROSMsg(pcl_filter, pcl_point); // sensor_msgs::PointCloud2类型转化为pcl::PointCloud类型
pcl_point.header.frame_id = "/pointCloud"; // 赋予frame_id
pclPoint_pub.publish(pcl_point); // 发布消息
lastKeyframeSize = N; //更新上一帧的关键帧数目?
cout << "Keyframe map publish time ="<<endl;
}
}
PointCloudMapping::Cloud_transform
void PointCloudMapping::Cloud_transform(pcl::PointCloud<pcl::PointXYZRGBA>& source, pcl::PointCloud<pcl::PointXYZRGBA>& out)
{
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_filtered;
Eigen::Matrix4f m;
// 不知道这个矩阵是做什么 ...
m<< 0,0,1,0,
-1,0,0,0,
0,-1,0,0;
Eigen::Affine3f transform(m);
pcl::transformPointCloud (source, out, transform);
}
不知道这个m的意义是什么,希望有知道的同学可以告知一下,感谢!
PointCloudMapping这个程序全是PCL相关的,以后要系统的学一下…嗨呀