多帧点云数据拼接合并_一起做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& camera )
{
static ParameterReader pd;
vector< cv::DMatch > matches;
cv::FlannBasedMatcher matcher;
matcher.match( frame1.desp, frame2.desp, matches );
cout<
vector< cv::DMatch > goodMatches;
double minDis = 9999;
double good_match_threshold = atof( pd.getData( "good_match_threshold" ).c_str() );
for ( size_t i=0; i
{
if ( matches[i].distance < minDis )
minDis = matches[i].distance;
}
for ( size_t i=0; i
{
if (matches[i].distance < good_match_threshold*minDis)
goodMatches.push_back( matches[i] );
}
cout<
// 第一个帧的三维点
vector<:point3f> pts_obj;
// 第二个帧的图像点
vector< cv::Point2f > pts_img;
// 相机内参
for (size_t i=0; i
{
// query 是第一个, train 是第二个
cv::Point2f p = frame1.kp[goodMatches[i].queryIdx].pt;
// 获取d是要小心！x是向右的，y是向下的，所以y才是行，x是列！
ushort d = frame1.depth.ptr( int(p.y) )[ int(p.x) ];
if (d == 0)
continue;
pts_img.push_back( cv::Point2f( frame2.kp[goodMatches[i].trainIdx].pt ) );
// 将(u,v,d)转成(x,y,z)
cv::Point3f pt ( p.x, p.y, d );
cv::Point3f pd = point2dTo3d( pt, camera );
pts_obj.push_back( pd );
}
double camera_matrix_data[3][3] = {
{camera.fx, 0, camera.cx},
{0, camera.fy, camera.cy},
{0, 0, 1}
};
cout<
// 构建相机矩阵
cv::Mat cameraMatrix( 3, 3, CV_64F, camera_matrix_data );
cv::Mat rvec, tvec, inliers;
// 求解pnp
cv::solvePnPRansac( pts_obj, pts_img, cameraMatrix, cv::Mat(), rvec, tvec, false, 100, 1.0, 100, inliers );
RESULT_OF_PNP result;
result.rvec = rvec;
result.tvec = tvec;
result.inliers = inliers.rows;
return result;
}
我们还实现了一个简单的参数读取类。
这个类读取一个参数的文本文件，能够以关键字的形式提供文本文件中的变量：
vim include/slamBase.h
代码如下：
// 参数读取类
class ParameterReader
{
public:
ParameterReader( string filename="./parameters.txt" )
{
ifstream fin( filename.c_str() );
if (!fin)
{
cerr<
return;
}
while(!fin.eof())
{
string str;
getline( fin, str );
if (str[0] == '#')
{
// 以‘＃’开头的是注释
continue;
}
int pos = str.find("=");
if (pos == -1)
continue;
string key = str.substr( 0, pos );
string value = str.substr( pos+1, str.length() );
data[key] = value;
if ( !fin.good() )
break;
}
}
string getData( string key )
{
map::iterator iter = data.find(key);
if (iter == data.end())
{
cerr<
return string("NOT_FOUND");
}
return iter->second;
}
public:
map data;
};
它读的参数文件是长这个样子的：
这是一个参数文件
part 4 里定义的参数
detector=SIFT
descriptor=SIFT
good_match_threshold=4
camera
camera.cx=325.5;
camera.cy=253.5;
camera.fx=518.0;
camera.fy=519.0;
camera.scale=1000.0;
参数文件里，以“变量名=值”的形式定义变量。以井号开头的是注释啦
拼接点云
点云变换
要把OpenCV里的旋转向量、位移向量转换成这个矩阵
OpenCV认为旋转矩阵R，虽然有3×3那么大，自由变量却只有三个，不够节省空间。
所以在OpenCV里使用了一个向量来表达旋转。向量的方向是旋转轴，大小则是转过的弧度
先用罗德里格斯变换(Rodrigues)将旋转向量转换为矩阵，然后“组装”成变换矩阵
代码如下：vim src/joinPointCloud.cpp
/*************************************************************************
File Name: src/jointPointCloud.cpp
Author: Xiang gao
Created Time: 2015年07月22日 星期三 20时46分08秒
************************************************************************/
#include
using namespace std;
#include "slamBase.h"
#include
#include
#include
// Eigen !
#include
#include
int main( int argc, char** argv )
{
//本节要拼合data中的两对图像
ParameterReader pd;
// 声明两个帧，FRAME结构请见include/slamBase.h
FRAME frame1, frame2;
//读取图像
frame1.rgb = cv::imread( "./data/rgb1.png" );
frame1.depth = cv::imread( "./data/depth1.png", -1);
frame2.rgb = cv::imread( "./data/rgb2.png" );
frame2.depth = cv::imread( "./data/depth2.png", -1 );
// 提取特征并计算描述子
cout<
string detecter = pd.getData( "detector" );
string descriptor = pd.getData( "descriptor" );
computeKeyPointsAndDesp( frame1, detecter, descriptor );
computeKeyPointsAndDesp( frame2, detecter, descriptor );
// 相机内参
CAMERA_INTRINSIC_PARAMETERS camera;
camera.fx = atof( pd.getData( "camera.fx" ).c_str());
camera.fy = atof( pd.getData( "camera.fy" ).c_str());
camera.cx = atof( pd.getData( "camera.cx" ).c_str());
camera.cy = atof( pd.getData( "camera.cy" ).c_str());
camera.scale = atof( pd.getData( "camera.scale" ).c_str() );
cout<
// 求解pnp
RESULT_OF_PNP result = estimateMotion( frame1, frame2, camera );
cout<
// 处理result
// 将旋转向量转化为旋转矩阵
cv::Mat R;
cv::Rodrigues( result.rvec, R );
Eigen::Matrix3d r;
cv::cv2eigen(R, r);
// 将平移向量和旋转矩阵转换成变换矩阵
Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
Eigen::AngleAxisd angle(r);
cout<
Eigen::Translation trans(result.tvec.at(0,0), result.tvec.at(0,1), result.tvec.at(0,2));
T = angle;
T(0,3) = result.tvec.at(0,0);
T(1,3) = result.tvec.at(0,1);
T(2,3) = result.tvec.at(0,2);
// 转换点云
cout<
PointCloud::Ptr cloud1 = image2PointCloud( frame1.rgb, frame1.depth, camera );
PointCloud::Ptr cloud2 = image2PointCloud( frame2.rgb, frame2.depth, camera );
// 合并点云
cout<
PointCloud::Ptr output (new PointCloud());
pcl::transformPointCloud( *cloud1, *output, T.matrix() );
*output += *cloud2;
pcl::io::savePCDFile("data/result.pcd", *output);
cout<
pcl::visualization::CloudViewer viewer( "viewer" );
viewer.showCloud( output );
while( !viewer.wasStopped() )
{
}
return 0;
}
重点在于59至73行，讲述了这个转换的过程
变换完毕后，我们就得到了拼合的点云
额外：
已经实现了一个只有两帧的SLAM程序。然而，也许你还不知道，这已经是一个视觉里程计(Visual Odometry)啦！
只要不断地把进来的数据与上一帧对比，就可以得到完整的运动轨迹以及地图了呢
要做完整的SLAM，还需要一些东西。以两两匹配为基础的里程计有明显的累积误差，我们需要通过回环检测来消除它。
先讲讲关键帧的处理，因为把每个图像都放进地图，会导致地图规模增长地太快，所以需要关键帧技术。
然后呢，我们要做一个SLAM后端，就要用到g2o