视觉slam十四讲第8章课后习题3+稀疏直接法程序注释

112 阅读 0 评论 74 点赞

我是靠谱客的博主危机大雁，这篇文章主要介绍视觉slam十四讲第8章课后习题3+稀疏直接法程序注释，现在分享给大家，希望可以做个参考。

版权声明：本文为博主原创文章，转载请注明出处: http://www.cnblogs.com/newneul/p/8571653.html
3、题目回顾:
在稀疏直接法中，假设单个像素周围小块的光度也不变，是否可以提高算法的健壮性？请编程实现、
分析：根据直接法的思想：基于灰度不变假设。因为题目假设了周围小块光度也不变，那么我们可以用单个像素周围的3x3或5x5小块的平均灰度值作为单个像素的灰度值，从一定程度上调高了健壮性，但是效果提升有限。
下面程序集成了direct_sparse.cpp程序的解释和利用半稠密直接法思路结合稀疏直接法以及本题小块思想的结合。共有四种配置方案。默认为稀疏直接法+小块思路也就是本题目的解法。当然也可以对应修改配置信息。执行相应的功能查看效果。
程序代码如下:

复制代码

1 #include <iostream>

2 #include <fstream>

3 #include <list>

4 #include <vector>

5 #include <chrono>

6 #include <ctime>

7 #include <climits>

9 #include <opencv2/core/core.hpp>
 10 #include <opencv2/imgproc/imgproc.hpp>
 11 #include <opencv2/highgui/highgui.hpp>
 12 #include <opencv2/features2d/features2d.hpp>
 13
 14 #include <g2o/core/base_unary_edge.h>
 15 #include <g2o/core/block_solver.h>
 16 #include <g2o/core/optimization_algorithm_levenberg.h>
 17 #include <g2o/solvers/dense/linear_solver_dense.h>
 18 #include <g2o/core/robust_kernel.h>
 19 #include <g2o/types/sba/types_six_dof_expmap.h>
 20
 21 using namespace std;
 22 using namespace g2o;
 23
 24 /*+++++++++++++++++++++++++++参数配置区+++++++++++++++++++++++++++++++++++++++=*/
 25
 26 //下面两种配置参数 可以任意组合 互不冲突 下面列出4种情况:
 27 /*
SEMIDENSE
BookExercise
 28  *
0
0
: 采用direct_sparse 例子 稀疏直接法（提取FAST特征）
 29  *
0
1
: 采用书上课后习题3（小块均值法）+ 稀疏直接法（提取FAST特征）
 30  *
1
0
: 采用稀疏直接法(提取FAST特征) +半稠密直接法（选取梯度大的）
 31  *
1
1
: 采用半稠密直接法（提取FAST特征）+课后习题3思路（小块均值法）+稀疏直接法（提取FAST特征）
 32  *
从枚举器中选择合适的值，对应修改下面的两个值:
 33  *
GradientThread
梯度域值
 34  *
PATCH_RADIUS
小块半径
 35  *
*/
 36 #define
SEMIDENSE
0
// 1 (稀疏直接法+半稠密法) 表示利用半稠密方法思想 筛选梯度比较大的点
这里就是从FAST关键点中筛选出梯度大的点
 37
// 0 表示仅仅用稀疏直接法
 38 #define
BookExercise
1
// 1 表示运行课后习题3的做法
 39
// 0 表示不采用课后习题3的做法
 40
 41 #if
SEMIDENSE
 42 enum GradientThreadChoice{
 43
GRADIENT_10 = 10,
//1170点
 44
GRADIENT_15 = 15,
//984点
 45
GRADIENT_20 = 20,
//805点
 46
GRADIENT_25 = 25,
//656点
 47
GRADIENT_30 = 30,
//514点
 48
GRADIENT_50 = 50,
//201点
 49
GRADIENT_100 = 100
//33点
 50 };
 51 #define
GradientThread
GRADIENT_50 // 默认筛选的梯度域值,通过调节域值（默认50） 可以增加关键点的个数
 52
 53 #endif
 54
 55 #if
BookExercise
 56 enum PatchRadiusChoices{
// 块大小选取类
 57
PATCH_RADIUS_ONE = 1,
// 表示以像素为圆心 半径为1大小的块
 58
PATCH_RADIUS_TWO = 2
// 最多半径为2 否则计算量太大（因为边计算误差函数会进行插值查找 块越大 计算量成平方增加）
 59 };
 60 PatchRadiusChoices
PATCH_RADIUS = PATCH_RADIUS_ONE;
//将全局变量置为该选项半径为1
 61 #endif
// 1对应3x3小块
2对应5x5小块
 62
 63 /*+++++++++++++++++++++++++++END参数配置取++++++++++++++++++++++++++++++++++++=*/
 64
 65 /********************************************
 66  * 本节演示了RGBD上的稀疏直接法
 67  ********************************************/
 68 //获取小块平均灰度值
 69 // gray:灰度矩阵 x,y表示以(x,y)为中心
计算的小块的平均灰度
patchRadius 表示块的半径
 70 #if BookExercise
 71 float getPatchAverageGray(const cv::Mat &gray ,float u, float v,int patchRadius);
 72 // 一次测量的值，包括一个世界坐标系下三维点（以第一帧为参考系）与一个灰度值（以第一帧为参考的3D点对应灰度图像的灰度值，灰度图是由color图像转换到对应的gray图像得到的 ）
 73 #endif
 74 struct Measurement
 75 {
 76
Measurement ( Eigen::Vector3d p, float g ) : pos_world ( p ), grayscale ( g ) {}
 77 
Eigen::Vector3d pos_world;
 78
float grayscale;
 79 };
 80
 81 //转换成相机坐标系下坐标
 82 inline Eigen::Vector3d project2Dto3D ( int x, int y, int d, float fx, float fy, float cx, float cy, float scale )
 83 {
 84
float zz = float ( d ) /scale;
 85
float xx = zz* ( x-cx ) /fx;
 86
float yy = zz* ( y-cy ) /fy;
 87
return Eigen::Vector3d ( xx, yy, zz );
 88 }
 89
 90 inline Eigen::Vector2d project3Dto2D ( float x, float y, float z, float fx, float fy, float cx, float cy )
 91 {
 92
float u = fx*x/z+cx;
 93
float v = fy*y/z+cy;
 94
return Eigen::Vector2d ( u,v );
 95 }
 96
 97 // 直接法估计位姿
 98 // 输入：测量值（空间点的灰度），新的灰度图，相机内参； 输出：相机位姿
 99 // 返回：true为成功，false失败
这里并没有设置返回值信息！
100 bool poseEstimationDirect ( const vector<Measurement>& measurements, cv::Mat* gray, Eigen::Matrix3f& intrinsics, Eigen::Isometry3d& Tcw );
101
102 // project a 3d point into an image plane, the error is photometric error
103 // an unary edge with one vertex SE3Expmap (the pose of camera)
104
//误差值维度 误差类型 顶点类型
105 class EdgeSE3ProjectDirect: public BaseUnaryEdge< 1, double, VertexSE3Expmap>
106 {
107 public:
108 
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
109
110
EdgeSE3ProjectDirect() = default; //代替下面的方式 默认产生合成的构造函数
111
//EdgeSE3ProjectDirect(){}
112
EdgeSE3ProjectDirect ( Eigen::Vector3d point, float fx, float fy, float cx, float cy, cv::Mat* image )
113
: x_world_ ( point ), fx_ ( fx ), fy_ ( fy ), cx_ ( cx ), cy_ ( cy ), image_ ( image )
//灰度图像指针
114 
{}
115
116
virtual void computeError()override
117 
{
118
const VertexSE3Expmap* v
=static_cast<const VertexSE3Expmap*> ( _vertices[0] );
119
Eigen::Vector3d x_local = v->estimate().map ( x_world_ );
120
float x = x_local[0]*fx_/x_local[2] + cx_;
//世界坐标转换到当期帧像素坐标
121
float y = x_local[1]*fy_/x_local[2] + cy_;
122
// check x,y is in the image
123
//距离图像四条边4个像素大小的区域内作为有效投影区域 对于不在该范围内的点误差值设为0 为了防止计算的误差太大 拉低内点对误差的影响 导致估计的RT严重偏离真值
124
if ( x-4<0 || ( x+4 ) >image_->cols || ( y-4 ) <0 || ( y+4 ) >image_->rows )
125 
{
126
_error ( 0,0 ) = 0.0;
127
this->setLevel ( 1 );//???????????????????????????????????????????????????
128 
}
129
else
130 
{
131 #if BookExercise
//表示运行课后习题3
132
//选取小块的大小为2x2
133
float sumValue = 0.0;
134
for(int i = x-PATCH_RADIUS ; i<= x+PATCH_RADIUS ; ++i)
135
for (int j = y-PATCH_RADIUS; j <= y+PATCH_RADIUS ; ++j) {
136
sumValue += getPixelValue(i,j);
137 
}
138
sumValue /=( (2*PATCH_RADIUS +1)*(2*PATCH_RADIUS+1) );
//求得元素周围小块的平均灰度值
139
_error (0,0) = sumValue - _measurement;
140 #else
141
_error ( 0,0 ) = getPixelValue ( x,y ) - _measurement;//经过在灰度图中插值获得的像素值 减去测量值
142 #endif
143 
}
144 
}
145
146
// plus in manifold
147
//提供误差关于位姿的雅克比矩阵 书上8.16式子 只不过负号去掉了 因为用的是当前帧灰度值 - 世界坐标下的测量值
148
virtual void linearizeOplus( )override
149 
{
150
if ( level() == 1 )
151 
{
152
_jacobianOplusXi = Eigen::Matrix<double, 1, 6>::Zero();
153
return;
154 
}
155
VertexSE3Expmap* vtx = dynamic_cast<VertexSE3Expmap*> ( _vertices[0] );
156
Eigen::Vector3d xyz_trans = vtx->estimate().map ( x_world_ );
// q in book 转换到第二帧坐标系下
157
158
double x = xyz_trans[0];
159
double y = xyz_trans[1];
160
double invz = 1.0/xyz_trans[2];
161
double invz_2 = invz*invz;
162
163
float u = x*fx_*invz + cx_;//投影到第二帧像素坐标系
164
float v = y*fy_*invz + cy_;
165
166
// jacobian from se3 to u,v
167
// NOTE that in g2o the Lie algebra is (omega, epsilon), where omega is so(3) and epsilon the translation
168
Eigen::Matrix<double, 2, 6> jacobian_uv_ksai;
169
170
//书上8.15式子
171
jacobian_uv_ksai ( 0,0 ) = - x*y*invz_2 *fx_;
172
jacobian_uv_ksai ( 0,1 ) = ( 1+ ( x*x*invz_2 ) ) *fx_;
173
jacobian_uv_ksai ( 0,2 ) = - y*invz *fx_;
174
jacobian_uv_ksai ( 0,3 ) = invz *fx_;
175
jacobian_uv_ksai ( 0,4 ) = 0;
176
jacobian_uv_ksai ( 0,5 ) = -x*invz_2 *fx_;
177
178
jacobian_uv_ksai ( 1,0 ) = - ( 1+y*y*invz_2 ) *fy_;
179
jacobian_uv_ksai ( 1,1 ) = x*y*invz_2 *fy_;
180
jacobian_uv_ksai ( 1,2 ) = x*invz *fy_;
181
jacobian_uv_ksai ( 1,3 ) = 0;
182
jacobian_uv_ksai ( 1,4 ) = invz *fy_;
183
jacobian_uv_ksai ( 1,5 ) = -y*invz_2 *fy_;
184
185
Eigen::Matrix<double, 1, 2> jacobian_pixel_uv;
186
187
//书上I2对像素坐标系的偏导数
这里很有可能 计算出来的梯度为0
因为FAST角点的梯度没有限制
188
//这也是半稠密法主要改进的地方 就是选关键点的时候 选择梯度大的点 因此这里的梯度就不可能为0了
189
jacobian_pixel_uv ( 0,0 ) = ( getPixelValue ( u+1,v )-getPixelValue ( u-1,v ) ) /2;
190
jacobian_pixel_uv ( 0,1 ) = ( getPixelValue ( u,v+1 )-getPixelValue ( u,v-1 ) ) /2;
191
192
_jacobianOplusXi = jacobian_pixel_uv*jacobian_uv_ksai;//书上8.16式子
193 
}
194
195
// dummy read and write functions because we don't care...
196
virtual bool read ( std::istream& in ) {}
197
virtual bool write ( std::ostream& out ) const {}
198
199 protected:
200
// get a gray scale value from reference image (bilinear interpolated)
201
//cv::Mat中成员变量代表的含义:http://blog.csdn.net/dcrmg/article/details/52294259
202
//下面的方式 针对单通道的灰度图
203
inline float getPixelValue ( float x, float y )//通过双线性插值获取浮点坐标对应的插值后的像素值
204 
{
205
uchar* data = & image_->data[ int ( y ) * image_->step + int ( x ) ];//step表示图像矩阵一行的所有字节（包括所有通道的总和），data表示存储图像的开始指针
206
float xx = x - floor ( x );
//取整函数
207
float yy = y - floor ( y );
208
return float (
//公式f(i+u,j+v) = (1-u)(1-v)f(i,j) + u(1-v)f(i+1,j) + (1-u)vf(i,j+1) + uvf(i+1,j+1)
209
//这里的xx 就是u
yy就是v
210
( 1-xx ) * ( 1-yy ) * data[0] +
211
xx* ( 1-yy ) * data[1] +
212
( 1-xx ) *yy*data[ image_->step ] +
//I(i+1,j) //这里相当于像素的周期是image_->step，即每一行存储像素的个数为image_->step
213
xx*yy*data[image_->step+1]
//I(i+1,j+1)
//data[image_->step]是I(i,j)对应的下一行像素为I(i+1,j)
214 
);
215 
}
216 public:
217
Eigen::Vector3d x_world_;
// 3D point in world frame
218
float cx_=0, cy_=0, fx_=0, fy_=0; // Camera intrinsics
219
cv::Mat* image_=nullptr;
// reference image
220 };
221
222 int main ( int argc, char** argv )
223 {
224
if ( argc != 2 )
225 
{
226
cout<<"usage: useLK path_to_dataset"<<endl;
227
return 1;
228 
}
229
srand ( ( unsigned int ) time ( 0 ) );
230
string path_to_dataset = argv[1];
231
string associate_file = path_to_dataset + "/associate.txt";
232
233 
ifstream fin ( associate_file );
234
235
string rgb_file, depth_file, time_rgb, time_depth;
236 
cv::Mat color, depth, gray;
237
vector<Measurement> measurements;//Measurement类 存储世界坐标点（以第一帧为参考的FAST关键点） 和 对应的灰度图像（由color->gray）的灰度值
238
// 相机内参
239
float cx = 325.5;
240
float cy = 253.5;
241
float fx = 518.0;
242
float fy = 519.0;
243
float depth_scale = 1000.0;
244 
Eigen::Matrix3f K;
245
K<<fx,0.f,cx,0.f,fy,cy,0.f,0.f,1.0f;
246
247
Eigen::Isometry3d Tcw = Eigen::Isometry3d::Identity();//三维变换矩阵T 4X4 初始时刻是单位R矩阵+0平移向量
248
249 
cv::Mat prev_color;
250
// 我们以第一个图像为参考，对后续图像和参考图像做直接法 ，每一副图像 都会与第一帧图像做直接法计算第一帧到当前帧的RT 但是经过更多的帧后 关键点的数量会减少，
251
//所以实际应用时 应当规定关键点的数量少于多少 就该从新设定参考系，再次利用直接法 ，但是会累计的误差需要解决？？？？
252
for ( int index=0; index<10; index++ )//总共10帧
253 
{
254
cout<<"*********** loop "<<index<<" ************"<<endl;
255
fin>>time_rgb>>rgb_file>>time_depth>>depth_file;
256
color = cv::imread ( path_to_dataset+"/"+rgb_file );
257
depth = cv::imread ( path_to_dataset+"/"+depth_file, -1 );//-1 按原图像的方式存储 detph 16位存储
258
if ( color.data==nullptr || depth.data==nullptr )
259
continue;
260
//转换后的灰度图为g2o优化需要的边提供灰度值
261
cv::cvtColor ( color, gray, cv::COLOR_BGR2GRAY );
//将颜色图3通道 转换为灰度图单通道 8位无符号 对应边类的双线性插值计算放法以单通道计算的
262
263
//第一帧为世界坐标系 计算FAST关键点 为之后与当前帧用直接法计算RT做准备
264
if ( index ==0 )//以第一帧为参考系 计算关键点后存储测量值（关键点对应的灰度值） 以此为基准跟踪后面的图像 计算位姿
265 
{
266
// 对第一帧提取FAST特征点
267
vector<cv::KeyPoint> keypoints;
268
cv::Ptr<cv::FastFeatureDetector> detector = cv::FastFeatureDetector::create();
269
detector->detect ( color, keypoints );
270
//对于2D关键点获取 3D信息 并去掉范围外的点 存储符合要求的关键点的深度值和3D信息
271
//对所有关键点挑选出符合要求且有深度值的 存储到vector<Measurement> measurements中 为g2o边提供灰度测量值和空间点坐标
272
for ( auto kp:keypoints )
273 
{
274 #if SEMIDENSE
//表示利用半稠密法的思想
只不过结果由原来的1402个点 变为了201个点 特征点数目降低了 但是看起来精度还是很高 可以适当调整梯度域值
275
Eigen::Vector2d delta (
//计算像素坐标系下 两个方向的变化量
276
gray.ptr<uchar>(int ( kp.pt.y))[ int(kp.pt.x+1)] - gray.ptr<uchar>(int(kp.pt.y))[int(kp.pt.x-1)],
277
gray.ptr<uchar>(int(kp.pt.y+1))[int(kp.pt.x)] - gray.ptr<uchar>(int(kp.pt.y-1))[int(kp.pt.x)]
278 
);
279
//cout<<" keypoints坐标值: "<<kp.pt.x<<" "<<kp.pt.y<<endl;//可以看出点虽然存储方式是浮点数 但是实际的值都是int类型
280
if ( delta.norm() < GradientThread )//可以转变为变化量的2范数小于50/16 默认域值为30 可调
281
continue;
282 #endif
283
// 去掉邻近边缘处的点 在离图像四条边20个像素构成的内矩阵范围内是符合要求的关键点
284
if ( kp.pt.x < 20 || kp.pt.y < 20 || ( kp.pt.x+20 ) >color.cols || ( kp.pt.y+20 ) >color.rows )
285
continue;
286
//depth.ptr<ushort>( kp.pt.y)获取行指针
cvRound(kp.pt,y) 表示返回跟参数值最接近的整数值 因为像素量化后是整数，而kp.pt.y存储方式是float，所以强制转换一下即可
287
ushort d = depth.ptr<ushort> ( cvRound ( kp.pt.y ) ) [ cvRound ( kp.pt.x ) ];//16位深度图
288
if ( d==0 )
289
continue;
290
Eigen::Vector3d p3d = project2Dto3D ( kp.pt.x, kp.pt.y, d, fx, fy, cx, cy, depth_scale );
//3D相机坐标系（第一帧 也是世界帧）
291 #if BookExercise
//计算小块平均灰度值作为对应单一像素的测量值 增加算法健壮性
292
float grayscale = getPatchAverageGray( gray , kp.pt.x , kp.pt.y , PATCH_RADIUS_ONE );
293 #else
//否则是正常以单个像素的灰度值作为测量值
294
float grayscale = float ( gray.ptr<uchar> ( cvRound ( kp.pt.y ) ) [ cvRound ( kp.pt.x ) ] );
//8位无符号也取整数 因为灰度图 是对应整数（int）的像素的 kp.pt.y是float
295 #endif
296 
measurements.push_back ( Measurement ( p3d, grayscale ) );
297 
}
298
prev_color = color.clone();
//深拷贝color图像
299
continue;
300 
}
301
// 使用直接法计算相机运动
302
//从第二帧开始计算相机位姿g2o优化
303
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
304
//优化过程中要提供灰度图像 边里面计算误差函数需要 为getPixelValue()该函数提供灰度值查找
305
poseEstimationDirect ( measurements, &gray, K, Tcw );//Tcw为世界坐标到下一帧坐标的累计值 最后Tcw的结果是从世界坐标 到当前帧下的转换
306
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
307
chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>> ( t2-t1 );
308
cout<<"direct method costs time: "<<time_used.count() <<" seconds."<<endl;
309
cout<<"Tcw="<<Tcw.matrix() <<endl;
310
311
// plot the feature points
312
cv::Mat img_show ( color.rows*2, color.cols, CV_8UC3 );//目的是为了之后对比前后两帧图像的关键点数量 所以建立一个可以存储pre_color 和color 大小的矩阵
313
//Rect(参数)表示坐标0,0 到cols,rows 那么大的矩形
314
//img_show.operator(const Rect &roi 参数:表示这个矩阵的某个兴趣区域)
315
//img_show.opertor返回一个构造的矩阵 Mat(const Mat& m, const Rect& roi);这个构造函数返回引用m矩阵中roi那部分感兴趣的范围
316
//最终结果是:prev_color矩阵元素拷贝到了 img_show矩阵对应Rect兴趣区域 因为img_show 是一个2*row行 cols列 可以包含两个prev_color矩阵
317
prev_color.copyTo ( img_show ( cv::Rect ( 0,0,color.cols, color.rows ) ) );//0列 0行 ->cols列 rows行 大小 //实际上就是把第一帧的图像拷贝到img_show中
318
//因为我们针对每一帧图像都会把第一帧图像拷贝到这里 所以这里实际上执行一次即可
319
//可以修改 前加上仅仅对第二帧执行一次即可
320
color.copyTo ( img_show ( cv::Rect ( 0,color.rows,color.cols, color.rows ) ) );//0列 rows行 ->cols列 rows行 大小
321
322
//在measurements容器中 随机挑选出符合要求的测量值 在img_show矩阵中对应部分进行标记（因为img_show上半部分是第一帧图像，下半部分是当前图像）
323
for ( Measurement m:measurements )
324 
{
325
if ( rand() > RAND_MAX/5 )
326
continue;
327
Eigen::Vector3d p = m.pos_world;
328
Eigen::Vector2d pixel_prev = project3Dto2D ( float( p ( 0,0 ) ), p ( 1,0 ), p ( 2,0 ), fx, fy, cx, cy );//世界坐标系下的 图像坐标2D
329
Eigen::Vector3d p2 = Tcw*m.pos_world;//将空间点转换到下一帧相机坐标系下
330
Eigen::Vector2d pixel_now = project3Dto2D ( p2 ( 0,0 ), p2 ( 1,0 ), p2 ( 2,0 ), fx, fy, cx, cy );//当前帧坐标系下的图像像素坐标
331
//对于超出下一帧图像像素坐标轴范围的点 舍弃不画
332
if ( pixel_now(0,0)<0 || pixel_now(0,0)>=color.cols || pixel_now(1,0)<0 || pixel_now(1,0)>=color.rows )
333
continue;
334
//随机获取bgr颜色 在cv::circle中 为关键点用不同的颜色圆来画出
335
float b = 255*float ( rand() ) /RAND_MAX;
336
float g = 255*float ( rand() ) /RAND_MAX;
337
float r = 255*float ( rand() ) /RAND_MAX;
338
//在img_show包含两帧图像上 以关键点为圆心画圆 半径为8个像素 颜色为bgr随机组合
2表示外轮廓线宽度为2 如果为负数则表示填充圆
339
//pixel_prev 都是世界坐标系下的坐标 (以第一帧为参考系) 和当前帧下的对比 可以看出关键点的数量会逐渐减少
340
cv::circle ( img_show, cv::Point2d ( pixel_prev ( 0,0 ), pixel_prev ( 1,0 ) ), 8, cv::Scalar ( b,g,r ), 2 );
341
cv::circle ( img_show, cv::Point2d ( pixel_now ( 0,0 ), pixel_now ( 1,0 ) +color.rows ), 8, cv::Scalar ( b,g,r ), 2 );//注意这里+color.rows 当前帧在img_show的下半部分
342
//连接前后两针匹配好的点
343
cv::line ( img_show, cv::Point2d ( pixel_prev ( 0,0 ), pixel_prev ( 1,0 ) ), cv::Point2d ( pixel_now ( 0,0 ), pixel_now ( 1,0 ) +color.rows ), cv::Scalar ( 0,0,250 ), 1 );
344 
}
345
cv::imshow ( "result", img_show );
346
cv::waitKey ( 0 );
347
348 
}
349
return 0;
350 }
351
352 bool poseEstimationDirect ( const vector< Measurement >& measurements, cv::Mat* gray, Eigen::Matrix3f& K, Eigen::Isometry3d& Tcw )
353 {
354
// 初始化g2o
355
typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,1>> DirectBlock;
// 求解的向量是6＊1的 因为一元边 所以后面的1可以是其他的数字
356
auto
linearSolver = g2o::make_unique<g2o::LinearSolverDense< DirectBlock::PoseMatrixType >>();
357
auto
solver_ptr = g2o::make_unique<DirectBlock>( std::move(linearSolver) );
358
g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg ( std::move(solver_ptr) );
359 //
DirectBlock::LinearSolverType* linearSolver = new g2o::LinearSolverDense< DirectBlock::PoseMatrixType > ();
360 //
DirectBlock* solver_ptr = new DirectBlock ( linearSolver );
361
// g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr ); // G-N
362 //
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr ); // L-M
363 
g2o::SparseOptimizer optimizer;
364 
optimizer.setAlgorithm ( solver );
365
optimizer.setVerbose( true );
366
367
auto pose = new g2o::VertexSE3Expmap();
368
pose->setEstimate ( g2o::SE3Quat ( Tcw.rotation(), Tcw.translation() ) );
369
pose->setId ( 0 );
370 
optimizer.addVertex ( pose );
371
372
// 添加边
373
int id=1;
374
for ( Measurement m: measurements )
375 
{
376
auto edge = new EdgeSE3ProjectDirect (
377 
m.pos_world,
378
K ( 0,0 ), K ( 1,1 ), K ( 0,2 ), K ( 1,2 ), gray
379 
);
380
edge->setVertex ( 0, pose );//设置一元边链接的顶点
381
edge->setMeasurement ( m.grayscale );//设置测量值 即把前一帧关键点的灰度值作为测量值 供给下一帧进行匹配计算RT
382
edge->setInformation ( Eigen::Matrix<double,1,1>::Identity() );//因为误差维度是1 所以信心矩阵为1x1
383
edge->setId ( id++ );
384 
optimizer.addEdge ( edge );
385 
}
386
cout<<"edges in graph: "<<optimizer.edges().size() <<endl;//边的个数 实际上反应了关键点的个数
387 
optimizer.initializeOptimization();
388
optimizer.optimize ( 30 );
389
Tcw = pose->estimate();
390 }
391
392 #if
BookExercise
393 //获取小块平均灰度值
394 // gray:灰度矩阵 x,y表示以(x,y)为中心
计算的小块的平均灰度
patchSize 表示块的半径
395 float getPatchAverageGray(const cv::Mat &gray ,float u, float v,int patchRadius){
396
int x = cvRound(u);
397
int y = cvRound(v);
398
if( (patchRadius < 0) || ( (2*patchRadius+1) > 5 ) ){
399
std::cout<<"Error:请修改PATCH_RADIUS为指定值1 or 2! "<<std::endl;
400
exit(1);
401 
}
402
float grayscale = 0.0;
403 //
y - patchRadius;//代表y坐标
404 //
x - patchRadius;//代表x坐标
405
for (int j = y-patchRadius; j <= y+patchRadius ; ++j)
406
for(auto i = x-patchRadius;i<= (x+patchRadius); ++i){
407
grayscale += float ( gray.ptr<uchar> (j)[i] );
408 
}
409
grayscale/= ( (2*patchRadius + 1)*(2*patchRadius +1) );
410
return grayscale;
411 }
412 #endif

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780

1 #include <iostream>

2 #include <fstream>

3 #include <list>

4 #include <vector>

5 #include <chrono>

6 #include <ctime>

7 #include <climits>

8

9 #include <opencv2/core/core.hpp>
 10 #include <opencv2/imgproc/imgproc.hpp>
 11 #include <opencv2/highgui/highgui.hpp>
 12 #include <opencv2/features2d/features2d.hpp>
 13
 14 #include <g2o/core/base_unary_edge.h>
 15 #include <g2o/core/block_solver.h>
 16 #include <g2o/core/optimization_algorithm_levenberg.h>
 17 #include <g2o/solvers/dense/linear_solver_dense.h>
 18 #include <g2o/core/robust_kernel.h>
 19 #include <g2o/types/sba/types_six_dof_expmap.h>
 20
 21 using namespace std;
 22 using namespace g2o;
 23
 24 /*+++++++++++++++++++++++++++参数配置区+++++++++++++++++++++++++++++++++++++++=*/
 25
 26 //下面两种配置参数 可以任意组合 互不冲突 下面列出4种情况:
 27 /*
SEMIDENSE
BookExercise
 28  *
0
0
: 采用direct_sparse 例子 稀疏直接法（提取FAST特征）
 29  *
0
1
: 采用书上课后习题3（小块均值法）+ 稀疏直接法（提取FAST特征）
 30  *
1
0
: 采用稀疏直接法(提取FAST特征) +半稠密直接法（选取梯度大的）
 31  *
1
1
: 采用半稠密直接法（提取FAST特征）+课后习题3思路（小块均值法）+稀疏直接法（提取FAST特征）
 32  *
从枚举器中选择合适的值，对应修改下面的两个值:
 33  *
GradientThread
梯度域值
 34  *
PATCH_RADIUS
小块半径
 35  *
*/
 36 #define
SEMIDENSE
0
// 1 (稀疏直接法+半稠密法) 表示利用半稠密方法思想 筛选梯度比较大的点
这里就是从FAST关键点中筛选出梯度大的点
 37
// 0 表示仅仅用稀疏直接法
 38 #define
BookExercise
1
// 1 表示运行课后习题3的做法
 39
// 0 表示不采用课后习题3的做法
 40
 41 #if
SEMIDENSE
 42 enum GradientThreadChoice{
 43
GRADIENT_10 = 10,
//1170点
 44
GRADIENT_15 = 15,
//984点
 45
GRADIENT_20 = 20,
//805点
 46
GRADIENT_25 = 25,
//656点
 47
GRADIENT_30 = 30,
//514点
 48
GRADIENT_50 = 50,
//201点
 49
GRADIENT_100 = 100
//33点
 50 };
 51 #define
GradientThread
GRADIENT_50 // 默认筛选的梯度域值,通过调节域值（默认50） 可以增加关键点的个数
 52
 53 #endif
 54
 55 #if
BookExercise
 56 enum PatchRadiusChoices{
// 块大小选取类
 57
PATCH_RADIUS_ONE = 1,
// 表示以像素为圆心 半径为1大小的块
 58
PATCH_RADIUS_TWO = 2
// 最多半径为2 否则计算量太大（因为边计算误差函数会进行插值查找 块越大 计算量成平方增加）
 59 };
 60 PatchRadiusChoices
PATCH_RADIUS = PATCH_RADIUS_ONE;
//将全局变量置为该选项半径为1
 61 #endif
// 1对应3x3小块
2对应5x5小块
 62
 63 /*+++++++++++++++++++++++++++END参数配置取++++++++++++++++++++++++++++++++++++=*/
 64
 65 /********************************************
 66  * 本节演示了RGBD上的稀疏直接法
 67  ********************************************/
 68 //获取小块平均灰度值
 69 // gray:灰度矩阵 x,y表示以(x,y)为中心
计算的小块的平均灰度
patchRadius 表示块的半径
 70 #if BookExercise
 71 float getPatchAverageGray(const cv::Mat &gray ,float u, float v,int patchRadius);
 72 // 一次测量的值，包括一个世界坐标系下三维点（以第一帧为参考系）与一个灰度值（以第一帧为参考的3D点对应灰度图像的灰度值，灰度图是由color图像转换到对应的gray图像得到的 ）
 73 #endif
 74 struct Measurement
 75 {
 76
Measurement ( Eigen::Vector3d p, float g ) : pos_world ( p ), grayscale ( g ) {}
 77 
Eigen::Vector3d pos_world;
 78
float grayscale;
 79 };
 80
 81 //转换成相机坐标系下坐标
 82 inline Eigen::Vector3d project2Dto3D ( int x, int y, int d, float fx, float fy, float cx, float cy, float scale )
 83 {
 84
float zz = float ( d ) /scale;
 85
float xx = zz* ( x-cx ) /fx;
 86
float yy = zz* ( y-cy ) /fy;
 87
return Eigen::Vector3d ( xx, yy, zz );
 88 }
 89
 90 inline Eigen::Vector2d project3Dto2D ( float x, float y, float z, float fx, float fy, float cx, float cy )
 91 {
 92
float u = fx*x/z+cx;
 93
float v = fy*y/z+cy;
 94
return Eigen::Vector2d ( u,v );
 95 }
 96
 97 // 直接法估计位姿
 98 // 输入：测量值（空间点的灰度），新的灰度图，相机内参； 输出：相机位姿
 99 // 返回：true为成功，false失败
这里并没有设置返回值信息！
100 bool poseEstimationDirect ( const vector<Measurement>& measurements, cv::Mat* gray, Eigen::Matrix3f& intrinsics, Eigen::Isometry3d& Tcw );
101
102 // project a 3d point into an image plane, the error is photometric error
103 // an unary edge with one vertex SE3Expmap (the pose of camera)
104
//误差值维度 误差类型 顶点类型
105 class EdgeSE3ProjectDirect: public BaseUnaryEdge< 1, double, VertexSE3Expmap>
106 {
107 public:
108 
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
109
110
EdgeSE3ProjectDirect() = default; //代替下面的方式 默认产生合成的构造函数
111
//EdgeSE3ProjectDirect(){}
112
EdgeSE3ProjectDirect ( Eigen::Vector3d point, float fx, float fy, float cx, float cy, cv::Mat* image )
113
: x_world_ ( point ), fx_ ( fx ), fy_ ( fy ), cx_ ( cx ), cy_ ( cy ), image_ ( image )
//灰度图像指针
114 
{}
115
116
virtual void computeError()override
117 
{
118
const VertexSE3Expmap* v
=static_cast<const VertexSE3Expmap*> ( _vertices[0] );
119
Eigen::Vector3d x_local = v->estimate().map ( x_world_ );
120
float x = x_local[0]*fx_/x_local[2] + cx_;
//世界坐标转换到当期帧像素坐标
121
float y = x_local[1]*fy_/x_local[2] + cy_;
122
// check x,y is in the image
123
//距离图像四条边4个像素大小的区域内作为有效投影区域 对于不在该范围内的点误差值设为0 为了防止计算的误差太大 拉低内点对误差的影响 导致估计的RT严重偏离真值
124
if ( x-4<0 || ( x+4 ) >image_->cols || ( y-4 ) <0 || ( y+4 ) >image_->rows )
125 
{
126
_error ( 0,0 ) = 0.0;
127
this->setLevel ( 1 );//???????????????????????????????????????????????????
128 
}
129
else
130 
{
131 #if BookExercise
//表示运行课后习题3
132
//选取小块的大小为2x2
133
float sumValue = 0.0;
134
for(int i = x-PATCH_RADIUS ; i<= x+PATCH_RADIUS ; ++i)
135
for (int j = y-PATCH_RADIUS; j <= y+PATCH_RADIUS ; ++j) {
136
sumValue += getPixelValue(i,j);
137 
}
138
sumValue /=( (2*PATCH_RADIUS +1)*(2*PATCH_RADIUS+1) );
//求得元素周围小块的平均灰度值
139
_error (0,0) = sumValue - _measurement;
140 #else
141
_error ( 0,0 ) = getPixelValue ( x,y ) - _measurement;//经过在灰度图中插值获得的像素值 减去测量值
142 #endif
143 
}
144 
}
145
146
// plus in manifold
147
//提供误差关于位姿的雅克比矩阵 书上8.16式子 只不过负号去掉了 因为用的是当前帧灰度值 - 世界坐标下的测量值
148
virtual void linearizeOplus( )override
149 
{
150
if ( level() == 1 )
151 
{
152
_jacobianOplusXi = Eigen::Matrix<double, 1, 6>::Zero();
153
return;
154 
}
155
VertexSE3Expmap* vtx = dynamic_cast<VertexSE3Expmap*> ( _vertices[0] );
156
Eigen::Vector3d xyz_trans = vtx->estimate().map ( x_world_ );
// q in book 转换到第二帧坐标系下
157
158
double x = xyz_trans[0];
159
double y = xyz_trans[1];
160
double invz = 1.0/xyz_trans[2];
161
double invz_2 = invz*invz;
162
163
float u = x*fx_*invz + cx_;//投影到第二帧像素坐标系
164
float v = y*fy_*invz + cy_;
165
166
// jacobian from se3 to u,v
167
// NOTE that in g2o the Lie algebra is (omega, epsilon), where omega is so(3) and epsilon the translation
168
Eigen::Matrix<double, 2, 6> jacobian_uv_ksai;
169
170
//书上8.15式子
171
jacobian_uv_ksai ( 0,0 ) = - x*y*invz_2 *fx_;
172
jacobian_uv_ksai ( 0,1 ) = ( 1+ ( x*x*invz_2 ) ) *fx_;
173
jacobian_uv_ksai ( 0,2 ) = - y*invz *fx_;
174
jacobian_uv_ksai ( 0,3 ) = invz *fx_;
175
jacobian_uv_ksai ( 0,4 ) = 0;
176
jacobian_uv_ksai ( 0,5 ) = -x*invz_2 *fx_;
177
178
jacobian_uv_ksai ( 1,0 ) = - ( 1+y*y*invz_2 ) *fy_;
179
jacobian_uv_ksai ( 1,1 ) = x*y*invz_2 *fy_;
180
jacobian_uv_ksai ( 1,2 ) = x*invz *fy_;
181
jacobian_uv_ksai ( 1,3 ) = 0;
182
jacobian_uv_ksai ( 1,4 ) = invz *fy_;
183
jacobian_uv_ksai ( 1,5 ) = -y*invz_2 *fy_;
184
185
Eigen::Matrix<double, 1, 2> jacobian_pixel_uv;
186
187
//书上I2对像素坐标系的偏导数
这里很有可能 计算出来的梯度为0
因为FAST角点的梯度没有限制
188
//这也是半稠密法主要改进的地方 就是选关键点的时候 选择梯度大的点 因此这里的梯度就不可能为0了
189
jacobian_pixel_uv ( 0,0 ) = ( getPixelValue ( u+1,v )-getPixelValue ( u-1,v ) ) /2;
190
jacobian_pixel_uv ( 0,1 ) = ( getPixelValue ( u,v+1 )-getPixelValue ( u,v-1 ) ) /2;
191
192
_jacobianOplusXi = jacobian_pixel_uv*jacobian_uv_ksai;//书上8.16式子
193 
}
194
195
// dummy read and write functions because we don't care...
196
virtual bool read ( std::istream& in ) {}
197
virtual bool write ( std::ostream& out ) const {}
198
199 protected:
200
// get a gray scale value from reference image (bilinear interpolated)
201
//cv::Mat中成员变量代表的含义:http://blog.csdn.net/dcrmg/article/details/52294259
202
//下面的方式 针对单通道的灰度图
203
inline float getPixelValue ( float x, float y )//通过双线性插值获取浮点坐标对应的插值后的像素值
204 
{
205
uchar* data = & image_->data[ int ( y ) * image_->step + int ( x ) ];//step表示图像矩阵一行的所有字节（包括所有通道的总和），data表示存储图像的开始指针
206
float xx = x - floor ( x );
//取整函数
207
float yy = y - floor ( y );
208
return float (
//公式f(i+u,j+v) = (1-u)(1-v)f(i,j) + u(1-v)f(i+1,j) + (1-u)vf(i,j+1) + uvf(i+1,j+1)
209
//这里的xx 就是u
yy就是v
210
( 1-xx ) * ( 1-yy ) * data[0] +
211
xx* ( 1-yy ) * data[1] +
212
( 1-xx ) *yy*data[ image_->step ] +
//I(i+1,j) //这里相当于像素的周期是image_->step，即每一行存储像素的个数为image_->step
213
xx*yy*data[image_->step+1]
//I(i+1,j+1)
//data[image_->step]是I(i,j)对应的下一行像素为I(i+1,j)
214 
);
215 
}
216 public:
217
Eigen::Vector3d x_world_;
// 3D point in world frame
218
float cx_=0, cy_=0, fx_=0, fy_=0; // Camera intrinsics
219
cv::Mat* image_=nullptr;
// reference image
220 };
221
222 int main ( int argc, char** argv )
223 {
224
if ( argc != 2 )
225 
{
226
cout<<"usage: useLK path_to_dataset"<<endl;
227
return 1;
228 
}
229
srand ( ( unsigned int ) time ( 0 ) );
230
string path_to_dataset = argv[1];
231
string associate_file = path_to_dataset + "/associate.txt";
232
233 
ifstream fin ( associate_file );
234
235
string rgb_file, depth_file, time_rgb, time_depth;
236 
cv::Mat color, depth, gray;
237
vector<Measurement> measurements;//Measurement类 存储世界坐标点（以第一帧为参考的FAST关键点） 和 对应的灰度图像（由color->gray）的灰度值
238
// 相机内参
239
float cx = 325.5;
240
float cy = 253.5;
241
float fx = 518.0;
242
float fy = 519.0;
243
float depth_scale = 1000.0;
244 
Eigen::Matrix3f K;
245
K<<fx,0.f,cx,0.f,fy,cy,0.f,0.f,1.0f;
246
247
Eigen::Isometry3d Tcw = Eigen::Isometry3d::Identity();//三维变换矩阵T 4X4 初始时刻是单位R矩阵+0平移向量
248
249 
cv::Mat prev_color;
250
// 我们以第一个图像为参考，对后续图像和参考图像做直接法 ，每一副图像 都会与第一帧图像做直接法计算第一帧到当前帧的RT 但是经过更多的帧后 关键点的数量会减少，
251
//所以实际应用时 应当规定关键点的数量少于多少 就该从新设定参考系，再次利用直接法 ，但是会累计的误差需要解决？？？？
252
for ( int index=0; index<10; index++ )//总共10帧
253 
{
254
cout<<"*********** loop "<<index<<" ************"<<endl;
255
fin>>time_rgb>>rgb_file>>time_depth>>depth_file;
256
color = cv::imread ( path_to_dataset+"/"+rgb_file );
257
depth = cv::imread ( path_to_dataset+"/"+depth_file, -1 );//-1 按原图像的方式存储 detph 16位存储
258
if ( color.data==nullptr || depth.data==nullptr )
259
continue;
260
//转换后的灰度图为g2o优化需要的边提供灰度值
261
cv::cvtColor ( color, gray, cv::COLOR_BGR2GRAY );
//将颜色图3通道 转换为灰度图单通道 8位无符号 对应边类的双线性插值计算放法以单通道计算的
262
263
//第一帧为世界坐标系 计算FAST关键点 为之后与当前帧用直接法计算RT做准备
264
if ( index ==0 )//以第一帧为参考系 计算关键点后存储测量值（关键点对应的灰度值） 以此为基准跟踪后面的图像 计算位姿
265 
{
266
// 对第一帧提取FAST特征点
267
vector<cv::KeyPoint> keypoints;
268
cv::Ptr<cv::FastFeatureDetector> detector = cv::FastFeatureDetector::create();
269
detector->detect ( color, keypoints );
270
//对于2D关键点获取 3D信息 并去掉范围外的点 存储符合要求的关键点的深度值和3D信息
271
//对所有关键点挑选出符合要求且有深度值的 存储到vector<Measurement> measurements中 为g2o边提供灰度测量值和空间点坐标
272
for ( auto kp:keypoints )
273 
{
274 #if SEMIDENSE
//表示利用半稠密法的思想
只不过结果由原来的1402个点 变为了201个点 特征点数目降低了 但是看起来精度还是很高 可以适当调整梯度域值
275
Eigen::Vector2d delta (
//计算像素坐标系下 两个方向的变化量
276
gray.ptr<uchar>(int ( kp.pt.y))[ int(kp.pt.x+1)] - gray.ptr<uchar>(int(kp.pt.y))[int(kp.pt.x-1)],
277
gray.ptr<uchar>(int(kp.pt.y+1))[int(kp.pt.x)] - gray.ptr<uchar>(int(kp.pt.y-1))[int(kp.pt.x)]
278 
);
279
//cout<<" keypoints坐标值: "<<kp.pt.x<<" "<<kp.pt.y<<endl;//可以看出点虽然存储方式是浮点数 但是实际的值都是int类型
280
if ( delta.norm() < GradientThread )//可以转变为变化量的2范数小于50/16 默认域值为30 可调
281
continue;
282 #endif
283
// 去掉邻近边缘处的点 在离图像四条边20个像素构成的内矩阵范围内是符合要求的关键点
284
if ( kp.pt.x < 20 || kp.pt.y < 20 || ( kp.pt.x+20 ) >color.cols || ( kp.pt.y+20 ) >color.rows )
285
continue;
286
//depth.ptr<ushort>( kp.pt.y)获取行指针
cvRound(kp.pt,y) 表示返回跟参数值最接近的整数值 因为像素量化后是整数，而kp.pt.y存储方式是float，所以强制转换一下即可
287
ushort d = depth.ptr<ushort> ( cvRound ( kp.pt.y ) ) [ cvRound ( kp.pt.x ) ];//16位深度图
288
if ( d==0 )
289
continue;
290
Eigen::Vector3d p3d = project2Dto3D ( kp.pt.x, kp.pt.y, d, fx, fy, cx, cy, depth_scale );
//3D相机坐标系（第一帧 也是世界帧）
291 #if BookExercise
//计算小块平均灰度值作为对应单一像素的测量值 增加算法健壮性
292
float grayscale = getPatchAverageGray( gray , kp.pt.x , kp.pt.y , PATCH_RADIUS_ONE );
293 #else
//否则是正常以单个像素的灰度值作为测量值
294
float grayscale = float ( gray.ptr<uchar> ( cvRound ( kp.pt.y ) ) [ cvRound ( kp.pt.x ) ] );
//8位无符号也取整数 因为灰度图 是对应整数（int）的像素的 kp.pt.y是float
295 #endif
296 
measurements.push_back ( Measurement ( p3d, grayscale ) );
297 
}
298
prev_color = color.clone();
//深拷贝color图像
299
continue;
300 
}
301
// 使用直接法计算相机运动
302
//从第二帧开始计算相机位姿g2o优化
303
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
304
//优化过程中要提供灰度图像 边里面计算误差函数需要 为getPixelValue()该函数提供灰度值查找
305
poseEstimationDirect ( measurements, &gray, K, Tcw );//Tcw为世界坐标到下一帧坐标的累计值 最后Tcw的结果是从世界坐标 到当前帧下的转换
306
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
307
chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>> ( t2-t1 );
308
cout<<"direct method costs time: "<<time_used.count() <<" seconds."<<endl;
309
cout<<"Tcw="<<Tcw.matrix() <<endl;
310
311
// plot the feature points
312
cv::Mat img_show ( color.rows*2, color.cols, CV_8UC3 );//目的是为了之后对比前后两帧图像的关键点数量 所以建立一个可以存储pre_color 和color 大小的矩阵
313
//Rect(参数)表示坐标0,0 到cols,rows 那么大的矩形
314
//img_show.operator(const Rect &roi 参数:表示这个矩阵的某个兴趣区域)
315
//img_show.opertor返回一个构造的矩阵 Mat(const Mat& m, const Rect& roi);这个构造函数返回引用m矩阵中roi那部分感兴趣的范围
316
//最终结果是:prev_color矩阵元素拷贝到了 img_show矩阵对应Rect兴趣区域 因为img_show 是一个2*row行 cols列 可以包含两个prev_color矩阵
317
prev_color.copyTo ( img_show ( cv::Rect ( 0,0,color.cols, color.rows ) ) );//0列 0行 ->cols列 rows行 大小 //实际上就是把第一帧的图像拷贝到img_show中
318
//因为我们针对每一帧图像都会把第一帧图像拷贝到这里 所以这里实际上执行一次即可
319
//可以修改 前加上仅仅对第二帧执行一次即可
320
color.copyTo ( img_show ( cv::Rect ( 0,color.rows,color.cols, color.rows ) ) );//0列 rows行 ->cols列 rows行 大小
321
322
//在measurements容器中 随机挑选出符合要求的测量值 在img_show矩阵中对应部分进行标记（因为img_show上半部分是第一帧图像，下半部分是当前图像）
323
for ( Measurement m:measurements )
324 
{
325
if ( rand() > RAND_MAX/5 )
326
continue;
327
Eigen::Vector3d p = m.pos_world;
328
Eigen::Vector2d pixel_prev = project3Dto2D ( float( p ( 0,0 ) ), p ( 1,0 ), p ( 2,0 ), fx, fy, cx, cy );//世界坐标系下的 图像坐标2D
329
Eigen::Vector3d p2 = Tcw*m.pos_world;//将空间点转换到下一帧相机坐标系下
330
Eigen::Vector2d pixel_now = project3Dto2D ( p2 ( 0,0 ), p2 ( 1,0 ), p2 ( 2,0 ), fx, fy, cx, cy );//当前帧坐标系下的图像像素坐标
331
//对于超出下一帧图像像素坐标轴范围的点 舍弃不画
332
if ( pixel_now(0,0)<0 || pixel_now(0,0)>=color.cols || pixel_now(1,0)<0 || pixel_now(1,0)>=color.rows )
333
continue;
334
//随机获取bgr颜色 在cv::circle中 为关键点用不同的颜色圆来画出
335
float b = 255*float ( rand() ) /RAND_MAX;
336
float g = 255*float ( rand() ) /RAND_MAX;
337
float r = 255*float ( rand() ) /RAND_MAX;
338
//在img_show包含两帧图像上 以关键点为圆心画圆 半径为8个像素 颜色为bgr随机组合
2表示外轮廓线宽度为2 如果为负数则表示填充圆
339
//pixel_prev 都是世界坐标系下的坐标 (以第一帧为参考系) 和当前帧下的对比 可以看出关键点的数量会逐渐减少
340
cv::circle ( img_show, cv::Point2d ( pixel_prev ( 0,0 ), pixel_prev ( 1,0 ) ), 8, cv::Scalar ( b,g,r ), 2 );
341
cv::circle ( img_show, cv::Point2d ( pixel_now ( 0,0 ), pixel_now ( 1,0 ) +color.rows ), 8, cv::Scalar ( b,g,r ), 2 );//注意这里+color.rows 当前帧在img_show的下半部分
342
//连接前后两针匹配好的点
343
cv::line ( img_show, cv::Point2d ( pixel_prev ( 0,0 ), pixel_prev ( 1,0 ) ), cv::Point2d ( pixel_now ( 0,0 ), pixel_now ( 1,0 ) +color.rows ), cv::Scalar ( 0,0,250 ), 1 );
344 
}
345
cv::imshow ( "result", img_show );
346
cv::waitKey ( 0 );
347
348 
}
349
return 0;
350 }
351
352 bool poseEstimationDirect ( const vector< Measurement >& measurements, cv::Mat* gray, Eigen::Matrix3f& K, Eigen::Isometry3d& Tcw )
353 {
354
// 初始化g2o
355
typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,1>> DirectBlock;
// 求解的向量是6＊1的 因为一元边 所以后面的1可以是其他的数字
356
auto
linearSolver = g2o::make_unique<g2o::LinearSolverDense< DirectBlock::PoseMatrixType >>();
357
auto
solver_ptr = g2o::make_unique<DirectBlock>( std::move(linearSolver) );
358
g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg ( std::move(solver_ptr) );
359 //
DirectBlock::LinearSolverType* linearSolver = new g2o::LinearSolverDense< DirectBlock::PoseMatrixType > ();
360 //
DirectBlock* solver_ptr = new DirectBlock ( linearSolver );
361
// g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr ); // G-N
362 //
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr ); // L-M
363 
g2o::SparseOptimizer optimizer;
364 
optimizer.setAlgorithm ( solver );
365
optimizer.setVerbose( true );
366
367
auto pose = new g2o::VertexSE3Expmap();
368
pose->setEstimate ( g2o::SE3Quat ( Tcw.rotation(), Tcw.translation() ) );
369
pose->setId ( 0 );
370 
optimizer.addVertex ( pose );
371
372
// 添加边
373
int id=1;
374
for ( Measurement m: measurements )
375 
{
376
auto edge = new EdgeSE3ProjectDirect (
377 
m.pos_world,
378
K ( 0,0 ), K ( 1,1 ), K ( 0,2 ), K ( 1,2 ), gray
379 
);
380
edge->setVertex ( 0, pose );//设置一元边链接的顶点
381
edge->setMeasurement ( m.grayscale );//设置测量值 即把前一帧关键点的灰度值作为测量值 供给下一帧进行匹配计算RT
382
edge->setInformation ( Eigen::Matrix<double,1,1>::Identity() );//因为误差维度是1 所以信心矩阵为1x1
383
edge->setId ( id++ );
384 
optimizer.addEdge ( edge );
385 
}
386
cout<<"edges in graph: "<<optimizer.edges().size() <<endl;//边的个数 实际上反应了关键点的个数
387 
optimizer.initializeOptimization();
388
optimizer.optimize ( 30 );
389
Tcw = pose->estimate();
390 }
391
392 #if
BookExercise
393 //获取小块平均灰度值
394 // gray:灰度矩阵 x,y表示以(x,y)为中心
计算的小块的平均灰度
patchSize 表示块的半径
395 float getPatchAverageGray(const cv::Mat &gray ,float u, float v,int patchRadius){
396
int x = cvRound(u);
397
int y = cvRound(v);
398
if( (patchRadius < 0) || ( (2*patchRadius+1) > 5 ) ){
399
std::cout<<"Error:请修改PATCH_RADIUS为指定值1 or 2! "<<std::endl;
400
exit(1);
401 
}
402
float grayscale = 0.0;
403 //
y - patchRadius;//代表y坐标
404 //
x - patchRadius;//代表x坐标
405
for (int j = y-patchRadius; j <= y+patchRadius ; ++j)
406
for(auto i = x-patchRadius;i<= (x+patchRadius); ++i){
407
grayscale += float ( gray.ptr<uchar> (j)[i] );
408 
}
409
grayscale/= ( (2*patchRadius + 1)*(2*patchRadius +1) );
410
return grayscale;
411 }
412 #endif