目录
1、数据结构解析
1.1 类定义
1.2 相机内参、帧结构、PnP结果3个结构体定义
1.3 参数读取类
1.4 函数接口定义
2、函数调用流程解析
1、数据结构解析
数据结构类CAMERA_INTRINSIC_PARAMETERS、FRAME、RESULT_OF_PNP、ParameterReader都在slambase.h文件中定义,我们首先进行slambase.h的解析。
1.1 类定义
定义我们使用的点云中的点数据结构和点云数据结构:
// 类定义
typedef pcl::PointXYZRGBA PointT;
typedef pcl::PointCloud 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<:keypoint> kp; // 关键点
};
PnP结果结构体定义如下:
// PnP结果
struct RESULT_OF_PNP
{
cv::Mat rvec, tvec;
int inliers; // 内点数量
};
1.3 参数读取类
参数读取类是一切工作开展的基石。这里高博采用的是直接读取.txt文件的做法,包含头文件
// 参数读取类
class ParameterReader
{
public:
ParameterReader(string filename="/home/zlc/WorkCode/rgbd-slam/parameters.txt")
{
ifstream fin(filename.c_str());
if (!fin)
{
cerr ::iterator iter = data.find(key);
if (iter == data.end())
{
cerr second;
}
public:
map 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(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(m)[n*3];
p.g = rgb.ptr(m)[n*3+1];
p.r = rgb.ptr(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<:featuredetector> _detector;
cv::Ptr<:descriptorextractor> _descriptor;
if (detector == "ORB")
{
_detector = cv::ORB::create();
}
if (descriptor == "ORB")
{
_descriptor = cv::ORB::create();
}
if (!_detector || !_descriptor)
{
cerr 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<:dmatch> matches; // 存储匹配点
// 第1步:对两针的特征点使用Hamming距离进行暴力匹配
cv::BFMatcher matcher;
matcher.match(frame1.desp, frame2.desp, matches);
// 第2步:匹配点对筛选:找出最小匹配距离,并进行数量检查
RESULT_OF_PNP result; // 待返回值
vector<:dmatch> goodMatches; // 存储好的匹配点
double minDis = 9999;
double good_match_threshold = atof(pd.getData("good_match_threshold").c_str());
for (size_t i=0; i pts_obj; // 第一帧的三维点
vector<:point2f> pts_img; // 第二帧的图像点
for (size_t i=0; i( 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. 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(i, j);
}
// 将平移向量 和 旋转矩阵 转换成 变换矩阵
Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
Eigen::AngleAxisd angle(r); // 直接使用旋转矩阵来对旋转向量赋值
T = angle; // “=”重载,使用旋转向量初始化变换矩阵旋转矩阵部分
T(0, 3) = tvec.at(0, 0); // 矩阵右上角的元素,这里开始平移部分
T(1, 3) = tvec.at(1, 0);
T(2, 3) = tvec.at(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 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<: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 > filename;
f.rgb = cv::imread(filename);
ss.clear();
filename.clear();
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 = max_norm)
return TOO_FAR_AWAY; // too far away, may be error
}
else
{
if (norm >= max_norm_lp)
return TOO_FAR_AWAY;
}
if (norm 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 information = Eigen::Matrix::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& 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()
(6)checkRandomLoops()函数:随机检测闭环,代码如下:
void checkRandomLoops(vector& 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()
关于流程调用,具体的代码解析,下篇再讲。