多帧点云数据拼接合并_一起做RGB-D SLAM (4)-点云拼接

一起做RGB-D SLAM (4)-点云拼接

说明：

介绍利用上节得到一个旋转向量与平移向量，把两张图像的点云给拼接起来，形成更大的点云

函数封装

上节函数封装进slamBase库中，编辑slamBase.h

vim include/slamBase.h

代码如下：

// 帧结构

struct FRAME

{

cv::Mat rgb, depth; //该帧对应的彩色图与深度图

cv::Mat desp; //特征描述子

vector<:keypoint> kp; //关键点

};

// PnP 结果

struct RESULT_OF_PNP

{

cv::Mat rvec, tvec;

int inliers;

};

// computeKeyPointsAndDesp 同时提取关键点与特征描述子

void computeKeyPointsAndDesp( FRAME& frame, string detector, string descriptor );

// estimateMotion 计算两个帧之间的运动

// 输入：帧1和帧2, 相机内参

RESULT_OF_PNP estimateMotion( FRAME& frame1, FRAME& frame2, CAMERA_INTRINSIC_PARAMETERS& camera );

我们把关键帧和PnP的结果都封成了结构体，以便将来别的程序调用, 编辑slamBase.cpp

vim src/slamBase.cpp

这两个函数的实现如下：

// computeKeyPointsAndDesp 同时提取关键点与特征描述子

void computeKeyPointsAndDesp( FRAME& frame, string detector, string descriptor )

{

cv::Ptr<:featuredetector> _detector;

cv::Ptr<:descriptorextractor> _descriptor;

cv::initModule_nonfree();

_detector = cv::FeatureDetector::create( detector.c_str() );

_descriptor = cv::DescriptorExtractor::create( descriptor.c_str() );

if (!_detector || !_descriptor)

{

cerr<

return;

}

_detector->detect( frame.rgb, frame.kp );

_descriptor->compute( frame.rgb, frame.kp, frame.desp );

return;

}

// estimateMotion 计算两个帧之间的运动

// 输入：帧1和帧2

// 输出：rvec 和 tvec

RESULT_OF_PNP estimateMotion( FRAME& frame1, FRAME& frame2, CAMERA_INTRINSIC_PARAMETERS& camer