目录
1、数据结构解析
数据结构类CAMERA_INTRINSIC_PARAMETERS、FRAME、RESULT_OF_PNP、ParameterReader都在slambase.h文件中定义，我们首先进行slambase.h的解析。
1.1 类定义
定义我们使用的点云中的点数据结构和点云数据结构：
// 类定义
typedef pcl::PointXYZRGBA PointT;
typedef pcl::PointCloud<PointT> PointCloud;1.2 相机内参、帧结构、PnP结果3个结构体定义
相机内参结构定义如下：
// 相机内参结构
struct CAMERA_INTRINSIC_PARAMETERS
{
double cx, cy, fx, fy, scale;
};帧结构定义如下：
// 帧结构
struct FRAME
{
int frameID; // 帧号
cv::Mat rgb, depth; // 该帧对应的彩色图与深度图
cv::Mat desp; // 特征描述子
vector<cv::KeyPoint> kp; // 关键点
};PnP结果结构体定义如下：
// PnP结果
struct RESULT_OF_PNP
{
cv::Mat rvec, tvec;
int inliers; // 内点数量
};1.3 参数读取类
参数读取类是一切工作开展的基石。这里高博采用的是直接读取.txt文件的做法，包含头文件<fstream>，然后使用定义文件流ifstream进行文件操作，具体代码如下：
// 参数读取类
class ParameterReader
{
public:
ParameterReader(string filename="/home/zlc/WorkCode/rgbd-slam/parameters.txt")
{
ifstream fin(filename.c_str());
if (!fin)
{
cerr << "parameter file does not exist." << endl;
return;
}
while(!fin.eof())
{
string str;
getline(fin, str); // 一次读一行，存入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<string, string>::iterator iter = data.find(key);
if (iter == data.end())
{
cerr << "Parameter name " << key << " not found!" << endl;
return string("NOT_FOUND");
}
return iter->second;
}
public:
map<string, string> data;
};很巧妙的应用map结构，找到“=”作为行分割点，分割点以前是关键字key，分割点后面是键值value，使用data[key]=value进行数组初始组装。
获取数据也比较简单，使用字符串操作，直接在data中遍历找到关键字即可，注意iter的类型应该就是map，所以最后返回的是iter->second。
1.4 函数接口定义
除了上面的几个结构体外，slambase.h还定义了几个函数接口：
1. image2PointCloud，将rgb图转换为点云：
// 函数接口：image2PointCloud 将rgb图转换为点云
PointCloud::Ptr image2PointCloud(cv::Mat& rgb, cv::Mat& depth, CAMERA_INTRINSIC_PARAMETERS& camera);
在slamBase.cpp中可以找到对应的函数实现如下，直接将当前帧颜色rgb图和深度depth图转换成点云：
// 将RGB图像像素转换为点云
PointCloud::Ptr image2PointCloud(cv::Mat& rgb, cv::Mat& depth, CAMERA_INTRINSIC_PARAMETERS& camera)
{
PointCloud::Ptr cloud(new PointCloud);
// 注意：Mat访问都是行优先
for (int m=0; m<depth.rows; m += 2)
for (int n=0; n<depth.cols; n += 2)
{
// 获取深度图中(m, n)处的值
ushort d = depth.ptr<ushort>(m)[n];
// d可能没有值，若如此，跳过此点
if (d == 0)
continue;
// d存在值，则向点云中增加一个点
PointT p;
// 计算这个点的空间坐标
p.z = double(d) / camera.scale;
p.x = (n - camera.cx) * p.z / camera.fx;
p.y = (m - camera.cy) * p.z / camera.fy;
// 从rgb图像中获取它的颜色
// rgb是三通道的BGR格式图，所以按下面的顺序获取颜色
p.b = rgb.ptr<uchar>(m)[n*3];
p.g = rgb.ptr<uchar>(m)[n*3+1];
p.r = rgb.ptr<uchar>(m)[n*3+2];
// 把p加入到点云中
cloud->points.push_back( p );
}
// 设置并保存点云
cloud->height = 1;
cloud->width = cloud->points.size();
cloud->is_dense = false;
return cloud;
}这里要注意思路，以深度图的行列大小为判断条件，先访问深度图获取深度值，然后进行坐标转换，将像素坐标系转换为空间坐标，此外还需要获取相机的rgb颜色，并将点p加入到点云中，最后三行设置了点云的存储性质。
2. point2dTo3d，将单个点从图像坐标转换为空间坐标：
// point2dTo3d 将单个点从图像坐标转换为空间坐标
// input：3 维点 Point3f(u, v, d)
cv::Point3f point2dTo3d(cv::Point3f& point, CAMERA_INTRINSIC_PARAMETERS& camera);
在slamBase.cpp中可以找到对应的函数实现如下：
cv::Point3f point2dTo3d(cv::Point3f& point, CAMERA_INTRINSIC_PARAMETERS& camera)
{
cv::Point3f p; // 3D点
p.z = double(point.z) / camera.scale;
p.x = (point.x - camera.cx) * p.z / camera.fx;
p.y = (point.y - camera.cy) * p.z / camera.fy;
return p;
}这里的转换太过经典，不再解析。
3. computeKeyPointsAndDesp，同时提取关键点和特征描述子
// computeKeyPointsAndDesp 同时提取关键点和特征描述子
void computeKeyPointsAndDesp(FRAME& frame, string detector, string descriptor);在slamBase.cpp中可以找到对应的函数实现如下：
// computeKeyPointsAndDesp 同时提取关键点与特征点描述子
void computeKeyPointsAndDesp(FRAME& frame, string detector, string descriptor)
{
cv::Ptr<cv::FeatureDetector> _detector;
cv::Ptr<cv::DescriptorExtractor> _descriptor;
if (detector == "ORB")
{
_detector = cv::ORB::create();
}
if (descriptor == "ORB")
{
_descriptor = cv::ORB::create();
}
if (!_detector || !_descriptor)
{
cerr << "Unknown detector or descriptor type !" << detector << ", " <<descriptor << endl;
return ;
}
// 第一步：检测Oriented FAST角点位置
_detector->detect(frame.rgb, frame.kp);
// 第二步：根据角点位置计算BRIEF描述子
_descriptor->compute(frame.rgb, frame.kp, frame.desp);
return ;
}本函数是重中之重，因为opencv版本的问题，这里选择这种方式建立特征检测器和描述子，本实现中只支持了ORB特征点及其描述子，只针对一帧进行检测，检测的结果都会使用引用，存在frame.kp和frame.desp中，这样也就明白了上面的帧结构结构体应该包含哪些成员。
4. estimateMotion，使用PnP估计相机两帧之间的运动。
// estimateMotion 计算两个帧之间的运动
// 输入：帧1和帧2，相机内参
RESULT_OF_PNP estimateMotion(FRAME& frame1, FRAME& frame2, CAMERA_INTRINSIC_PARAMETERS& camera);
slamBase.cpp文件中的定义如下，函数功能是输入两帧连续图像帧1和帧2，输出两帧之间的变换，先总结一些流程：
（1）特征点匹配：对两帧的特征点使用Hamming距离进行暴力匹配；
（2）匹配点筛选：找出最小匹配距离，准备筛选匹配点，并进行数量检查；
（3）对求解出的特征点进行PnP求解问题构造：（PnP是求解3D到2D点对运动的方法）；
（4）求解PnP。
代码如下：
// estimateMotion 计算两个帧之间的运动
// 输入：帧1和帧2
// 输出：rvec 和 tvec
RESULT_OF_PNP estimateMotion(FRAME& frame1, FRAME& frame2, CAMERA_INTRINSIC_PARAMETERS& camera)
{
static ParameterReader pd;
vector<cv::DMatch> matches; // 存储匹配点
// 第1步：对两针的特征点使用Hamming距离进行暴力匹配
cv::BFMatcher matcher;
matcher.match(frame1.desp, frame2.desp, matches);
// 第2步：匹配点对筛选：找出最小匹配距离，并进行数量检查
RESULT_OF_PNP result; // 待返回值
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<matches.size(); i ++) // 找出所有匹配点对中的最小距离，即醉相思的两组点之间的距离，作为后续筛选的阈值
{
if (matches[i].distance < minDis)
minDis = matches[i].distance;
}
if (minDis < 10) // 描述子匹配最小距离，即最相似的点
minDis = 10;
for (size_t i=0; i<matches.size(); i ++)
{
// if (matches[i].distance < max(30.0, good_match_threshold*minDis)) // 工程经验
if (matches[i].distance < good_match_threshold*minDis) // 工程经验
goodMatches.push_back(matches[i]);
}
if (goodMatches.size() <= 5)
{
result.inliers = -1;
return result;
}
// 第3步：对筛选出的特征点进行PnP求解问题构造： （PnP是 求解3D点到2D点对运动 的方法）
vector<cv::Point3f> pts_obj; // 第一帧的三维点
vector<cv::Point2f> pts_img; // 第二帧的图像点
for (size_t i=0; i<goodMatches.size(); i ++) // 需要相机内参
{
// ①第1个匹配点转换为相机坐标系下的3维空间点 query是第一个，train是第二个
cv::Point2f p = frame1.kp[goodMatches[i].queryIdx].pt;
// 获取 深度d 时要小心！！！ x是向右的列，y是向下的行
ushort d = frame1.depth.ptr<ushort>( int(p.y) )[int(p.x)];
if (d == 0)
continue;
// 将(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);
// ②第2个匹配点直接存
pts_img.push_back( cv::Point2f(frame2.kp[goodMatches[i].trainIdx].pt) );
}
if (pts_obj.size() <= 5 || pts_img.size() <= 5)
{
result.inliers = -1;
return result;
}
// 相机内参矩阵
double camera_matrix_data[3][3] = {
{camera.fx, 0, camera.cx},
{0, camera.fy, camera.cy},
{0, 0, 1}
};
cv::Mat cameraMatrix(3, 3, CV_64F, camera_matrix_data); // 构建相机矩阵，cv::Mat类型的矩阵
cv::Mat rvec, tvec, inliers;
// 第4步：求解PnP
cv::solvePnPRansac(pts_obj, pts_img, cameraMatrix, cv::Mat(), rvec, tvec, false, 100, 1.0, 0.99, inliers);
result.rvec = rvec;
result.tvec = tvec;
result.inliers = inliers.rows; // 记录内点数量
return result;
}5. cvMat2Eigen，进行数据转换，因为使用opencv接口函数，得到的都是cv结构体，要进一步处理的话，比方进行点云处理，还是要使用eigen，所以要进行转换，函数定义如下：
// cvMat2Eigen
Eigen::Isometry3d cvMat2Eigen(cv::Mat& rvec, cv::Mat& tvec);在slamBase.cpp中可以找到对应的函数实现如下，这里需要注意的是一些转换关系，调用opencv中的罗德里格斯公式，进行旋转向量（李代数）和旋转矩阵（李群）之间的转换：
// cvMat 2 Eigen 注意：这里解析出的变换矩阵是 T_21， 即第一帧转换到第二帧
Eigen::Isometry3d cvMat2Eigen(cv::Mat& rvec, cv::Mat& tvec)
{
cv::Mat R;
cv::Rodrigues(rvec, R);
Eigen::Matrix3d r;
for (int i=0; i<3; i ++)
{
for (int j=0; j<3; j ++)
r(i, j) = R.at<double>(i, j);
}
// 将平移向量 和 旋转矩阵 转换成 变换矩阵
Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
Eigen::AngleAxisd angle(r); // 直接使用旋转矩阵来对旋转向量赋值
T = angle; // “=”重载，使用旋转向量初始化变换矩阵旋转矩阵部分
T(0, 3) = tvec.at<double>(0, 0); // 矩阵右上角的元素，这里开始平移部分
T(1, 3) = tvec.at<double>(1, 0);
T(2, 3) = tvec.at<double>(2, 0);
return T;
}中间的变换关系看似简单，背后有初始化和“=”操作符重载操作。
6. jointPointCloud，点云拼接
// jointPointCloud
PointCloud::Ptr jointPointCloud(PointCloud::Ptr original, FRAME& newFrame, Eigen::Isometry3d T, CAMERA_INTRINSIC_PARAMETERS& camera);
在slamBase.cpp中可以找到对应的函数实现如下，但这个函数实际上没有用到：
// jointPointCloud
// 输入：原始点云，新来的帧以及它的位姿
// 输出：将新来帧加到原始帧后的图像
PointCloud::Ptr jointPointCloud(PointCloud::Ptr original, FRAME& newFrame, Eigen::Isometry3d T, CAMERA_INTRINSIC_PARAMETERS& camera)
{
PointCloud::Ptr newCloud = image2PointCloud(newFrame.rgb, newFrame.depth, camera);
// 合并点云
PointCloud::Ptr output(new PointCloud());
pcl::transformPointCloud(*original, *output, T.matrix()); // 将原始点云变换到当前帧坐标系下
*newCloud += *output; // 拼接融合
// Voxel grid 滤波降采样
static pcl::VoxelGrid<PointT> voxel;
static ParameterReader pd;
double gridsize = atof(pd.getData("voxel_grid").c_str());
voxel.setLeafSize(gridsize, gridsize, gridsize);
voxel.setInputCloud( newCloud );
PointCloud::Ptr tmp( new PointCloud() ); // 滤波后的点云
voxel.filter(*tmp);
return tmp;
}7. getDefaultCamera，获取相机内参
需要注意的是，这里就标准应用了上面的参数获取函数，但需要注意最后有C++string转换为
字符串的过程，使用的是.c_str()函数，然后使用atof()函数进行浮点数转换。
inline static CAMERA_INTRINSIC_PARAMETERS getDefaultCamera()
{
ParameterReader pd;
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());
return camera; // Bug
}2、函数调用流程解析
函数框架如下：
函数调用流程如下：
这里将一些函数接口进行说明：
（1）g2o块求解的定义：
// 把g2o的定义放到前面
typedef g2o::BlockSolver_6_3 SlamBlockSolver;
typedef g2o::LinearSolverEigen<SlamBlockSolver::PoseMatrixType> SlamLinearSolver;
// 注意这里的求解方法是LinearSolverEigen，不是LinearSolverCSparse（2）readFrame()：读取一帧数据：
// 给定index，读取一帧数据
FRAME readFrame(int index, ParameterReader& pd);实际代码如下：
// 读取一帧rgb图像和对应的深度depth图
FRAME readFrame(int index, ParameterReader& pd)
{
FRAME f;
string rgbDir = pd.getData("rgb_dir");
string depthDir = pd.getData("depth_dir");
string rgbExt = pd.getData("rgb_extension"); // .png
string depthExt = pd.getData("depth_extension"); // .png
stringstream ss;
ss << rgbDir << index << rgbExt;
string filename;
ss >> filename;
f.rgb = cv::imread(filename);
ss.clear();
filename.clear();
ss << depthDir << index << depthExt;
ss >> filename;
f.depth = cv::imread(filename, -1);
f.frameID = index;
return f;
}cv::imread()的读取参数如下，第二个参数为flag，具体含义如下：
- flags=-1：imread按解码得到的方式读入图像；
- flags=0：imread按单通道的方式读入图像，即灰白图像；
- flags=1：imread按三通道方式读入图像，即彩色图像。
（3）normofTransform()：估计一个运动的大小，主要在关键帧检查中用到：
// 估计一个运动的大小
double normofTransform(cv::Mat rvec, cv::Mat tvec);对应代码如下：
double normofTransform(cv::Mat rvec, cv::Mat tvec)
{
return fabs( min(cv::norm(rvec), 2*M_PI - cv::norm(rvec)) ) + fabs(cv::norm(tvec));
}（4）检测两个帧，结果定义如下：
// 检测两个帧，结果定义
enum CHECK_RESULT {NOT_MATCHED=0, TOO_FAR_AWAY, TOO_CLOSE, KEYFRAME};应用上面这个帧的函数主要是checkKeyframes，检测是否为关键帧，代码如下：
// 检测是否为关键帧，
CHECK_RESULT checkKeyframes(FRAME& f1, FRAME& f2, g2o::SparseOptimizer& opti, bool is_loops)
{
static ParameterReader pd;
static int min_inliers = atoi( pd.getData("min_inliers").c_str() );
static double max_norm = atof( pd.getData("max_norm").c_str() );
static double keyframe_threshold = atof( pd.getData("keyframe_threshold").c_str() );
static double max_norm_lp = atof( pd.getData("max_norm_lp").c_str() );
static CAMERA_INTRINSIC_PARAMETERS camera = getDefaultCamera();
// 比较f1和f2，对两帧进行暴力匹配，挑选出好的匹配点，对第一个匹配帧关键点进行2D=》3D转换，对第二个匹配帧关键点直接存储，构建PnP问题并求解，得到运动估计
RESULT_OF_PNP result = estimateMotion(f1, f2, camera);
if ( result.inliers < min_inliers ) // inliers不够，放弃该帧
return NOT_MATCHED;
// 计算运动范围是否太大
double norm = normofTransform( result.rvec, result.tvec );
if (is_loops == false)
{
if (norm >= max_norm)
return TOO_FAR_AWAY; // too far away, may be error
}
else
{
if (norm >= max_norm_lp)
return TOO_FAR_AWAY;
}
if (norm <= keyframe_threshold)
return TOO_CLOSE;
// 向g2o中增加这个顶点与上一帧联系的边
// 顶点部分：顶点只需设定id即可
if (is_loops == false)
{
g2o::VertexSE3* v = new g2o::VertexSE3();
v->setId(f2.frameID);
v->setEstimate(Eigen::Isometry3d::Identity());
opti.addVertex(v);
}
// 边部分
g2o::EdgeSE3* edge = new g2o::EdgeSE3();
edge->setVertex(0, opti.vertex(f1.frameID)); // 连接此边的两个顶点id
edge->setVertex(1, opti.vertex(f2.frameID));
edge->setRobustKernel( new g2o::RobustKernelHuber() );
// 信息矩阵
Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
// 信息矩阵是协方差矩阵的逆，表示我们对边的精度的预先估计
// 因此pose为6D的，信息矩阵是6*6的矩阵，假设位置和角度的估计精度均为0.1且互相独立
// 那么协方差则为对角为0.01的矩阵，信息阵则为100的矩阵
information(0, 0) = information(1, 1) = information(2, 2) = 100;
information(3, 3) = information(4, 4) = information(5, 5) = 100;
// 也可以将角度设大一些，表示对角度的估计更加准确
edge->setInformation(information);
// 边的估计即是 pnp求解的结果
Eigen::Isometry3d T = cvMat2Eigen(result.rvec, result.tvec);
// edge->setMeasurement( T );
edge->setMeasurement(T.inverse());
// 将此边加入图中
opti.addEdge(edge);
return KEYFRAME;
}（5）checkKeyframes()函数：最近闭环检测：
// 函数声明
CHECK_RESULT checkKeyframes(FRAME& f1, FRAME& f2, g2o::SparseOptimizer& opti, bool is_loops=false);代码如下：
void checkNearbyLoops(vector<FRAME>& frames, FRAME& currFrame, g2o::SparseOptimizer& opti)
{
static ParameterReader pd;
static int nearby_loops = atoi(pd.getData("nearby_loops").c_str()); // 5
// 就是把currFrame 和 frames里末尾几个测一遍
if ( frames.size() <= nearby_loops )
{
// no enough keyframes, check everyone
for (size_t i=0; i<frames.size(); i ++)
{
checkKeyframes(frames[i], currFrame, opti, true);
}
}
else
{
// check the nearest ones
for (size_t i=frames.size()-nearby_loops; i<frames.size(); i ++)
{
checkKeyframes(frames[i], currFrame, opti, true);
}
}
}（6）checkRandomLoops()函数：随机检测闭环，代码如下：
void checkRandomLoops(vector<FRAME>& frames, FRAME& currFrame, g2o::SparseOptimizer& opti)
{
static ParameterReader pd;
static int random_loops = atoi(pd.getData("random_loops").c_str());
srand((unsigned int)time(NULL));
// 随机取一些帧进行检测
if (frames.size() <= random_loops)
{
// no enough keyframes, check everyone
for (size_t i=0; i<frames.size(); i ++)
{
checkKeyframes(frames[i], currFrame, opti, true);
}
}
else
{
// randomly check loops
for (int i=0; i<random_loops; i ++)
{
int index = rand()%frames.size();
checkKeyframes(frames[index], currFrame, opti, true);
}
}
}关于流程调用，具体的代码解析，下篇再讲。
1535
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
