视觉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>

8

9 #include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>

#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/core/robust_kernel.h>
#include <g2o/types/sba/types_six_dof_expmap.h>

using namespace std;
using namespace g2o;

/*+++++++++++++++++++++++++++参数配置区+++++++++++++++++++++++++++++++++++++++=*/

//下面两种配置参数 可以任意组合 互不冲突 下面列出4种情况:
/*
SEMIDENSE
BookExercise
 *
0
0
: 采用direct_sparse 例子 稀疏直接法（提取FAST特征）
 *
0
1
: 采用书上课后习题3（小块均值法）+ 稀疏直接法（提取FAST特征）
 *
1
0
: 采用稀疏直接法(提取FAST特征) +半稠密直接法（选取梯度大的）
 *
1
1
: 采用半稠密直接法（提取FAST特征）+课后习题3思路（小块均值法）+稀疏直接法（提取FAST特征）
 *
从枚举器中选择合适的值，对应修改下面的两个值:
 *
GradientThread
梯度域值
 *
PATCH_RADIUS
小块半径
 *
*/
#define
SEMIDENSE
0
// 1 (稀疏直接法+半稠密法) 表示利用半稠密方法思想 筛选梯度比较大的点
这里就是从FAST关键点中筛选出梯度大的点

// 0 表示仅仅用稀疏直接法
#define
BookExercise
1
// 1 表示运行课后习题3的做法

// 0 表示不采用课后习题3的做法

#if
SEMIDENSE
enum GradientThreadChoice{

GRADIENT_10 = 10,
//1170点

GRADIENT_15 = 15,
//984点

GRADIENT_20 = 20,
//805点

GRADIENT_25 = 25,
//656点

GRADIENT_30 = 30,
//514点

GRADIENT_50 = 50,
//201点

GRADIENT_100 = 100
//33点
};
#define
GradientThread
GRADIENT_50 // 默认筛选的梯度域值,通过调节域值（默认50） 可以增加关键点的个数

#endif

#if
BookExercise
enum PatchRadiusChoices{
// 块大小选取类

PATCH_RADIUS_ONE = 1,
// 表示以像素为圆心 半径为1大小的块

PATCH_RADIUS_TWO = 2
// 最多半径为2 否则计算量太大（因为边计算误差函数会进行插值查找 块越大 计算量成平方增加）
};
PatchRadiusChoices
PATCH_RADIUS = PATCH_RADIUS_ONE;
//将全局变量置为该选项半径为1
#endif
// 1对应3x3小块
2对应5x5小块

/*+++++++++++++++++++++++++++END参数配置取++++++++++++++++++++++++++++++++++++=*/

/********************************************
 * 本节演示了RGBD上的稀疏直接法
 ********************************************/
//获取小块平均灰度值
// gray:灰度矩阵 x,y表示以(x,y)为中心
计算的小块的平均灰度
patchRadius 表示块的半径
#if BookExercise
float getPatchAverageGray(const cv::Mat &gray ,float u, float v,int patchRadius);
// 一次测量的值，包括一个世界坐标系下三维点（以第一帧为参考系）与一个灰度值（以第一帧为参考的3D点对应灰度图像的灰度值，灰度图是由color图像转换到对应的gray图像得到的 ）
#endif
struct Measurement
{

Measurement ( Eigen::Vector3d p, float g ) : pos_world ( p ), grayscale ( g ) {}

Eigen::Vector3d pos_world;

float grayscale;
};

//转换成相机坐标系下坐标
inline Eigen::Vector3d project2Dto3D ( int x, int y, int d, float fx, float fy, float cx, float cy, float scale )
{

float zz = float ( d ) /scale;

float xx = zz* ( x-cx ) /fx;

float yy = zz* ( y-cy ) /fy;

return Eigen::Vector3d ( xx, yy, zz );
}

inline Eigen::Vector2d project3Dto2D ( float x, float y, float z, float fx, float fy, float cx, float cy )
{

float u = fx*x/z+cx;

float v = fy*y/z+cy;

return Eigen::Vector2d ( u,v );
}

// 直接法估计位姿
// 输入：测量值（空间点的灰度），新的灰度图，相机内参； 输出：相机位姿
// 返回：true为成功，false失败
这里并没有设置返回值信息！
bool poseEstimationDirect ( const vector<Measurement>& measurements, cv::Mat* gray, Eigen::Matrix3f& intrinsics, Eigen::Isometry3d& Tcw );

// project a 3d point into an image plane, the error is photometric error
// an unary edge with one vertex SE3Expmap (the pose of camera)

//误差值维度 误差类型 顶点类型
class EdgeSE3ProjectDirect: public BaseUnaryEdge< 1, double, VertexSE3Expmap>
{
public:

EIGEN_MAKE_ALIGNED_OPERATOR_NEW


EdgeSE3ProjectDirect() = default; //代替下面的方式 默认产生合成的构造函数

//EdgeSE3ProjectDirect(){}

EdgeSE3ProjectDirect ( Eigen::Vector3d point, float fx, float fy, float cx, float cy, cv::Mat* image )

: x_world_ ( point ), fx_ ( fx ), fy_ ( fy ), cx_ ( cx ), cy_ ( cy ), image_ ( image )
//灰度图像指针

{}


virtual void computeError()override

{

const VertexSE3Expmap* v
=static_cast<const VertexSE3Expmap*> ( _vertices[0] );

Eigen::Vector3d x_local = v->estimate().map ( x_world_ );

float x = x_local[0]*fx_/x_local[2] + cx_;
//世界坐标转换到当期帧像素坐标

float y = x_local[1]*fy_/x_local[2] + cy_;

// check x,y is in the image

//距离图像四条边4个像素大小的区域内作为有效投影区域 对于不在该范围内的点误差值设为0 为了防止计算的误差太大 拉低内点对误差的影响 导致估计的RT严重偏离真值

if ( x-4<0 || ( x+4 ) >image_->cols || ( y-4 ) <0 || ( y+4 ) >image_->rows )

{

_error ( 0,0 ) = 0.0;

this->setLevel ( 1 );//???????????????????????????????????????????????????

}

else

{
#if BookExercise
//表示运行课后习题3

//选取小块的大小为2x2

float sumValue = 0.0;

for(int i = x-PATCH_RADIUS ; i<= x+PATCH_RADIUS ; ++i)

for (int j = y-PATCH_RADIUS; j <= y+PATCH_RADIUS ; ++j) {

sumValue += getPixelValue(i,j);

}

sumValue /=( (2*PATCH_RADIUS +1)*(2*PATCH_RADIUS+1) );
//求得元素周围小块的平均灰度值

_error (0,0) = sumValue - _measurement;
#else

_error ( 0,0 ) = getPixelValue ( x,y ) - _measurement;//经过在灰度图中插值获得的像素值 减去测量值
#endif

}

}


// plus in manifold

//提供误差关于位姿的雅克比矩阵 书上8.16式子 只不过负号去掉了 因为用的是当前帧灰度值 - 世界坐标下的测量值

virtual void linearizeOplus( )override

{

if ( level() == 1 )

{

_jacobianOplusXi = Eigen::Matrix<double, 1, 6>::Zero();

return;

}

VertexSE3Expmap* vtx = dynamic_cast<VertexSE3Expmap*> ( _vertices[0] );

Eigen::Vector3d xyz_trans = vtx->estimate().map ( x_world_ );
// q in book 转换到第二帧坐标系下


double x = xyz_trans[0];

double y = xyz_trans[1];

double invz = 1.0/xyz_trans[2];

double invz_2 = invz*invz;


float u = x*fx_*invz + cx_;//投影到第二帧像素坐标系

float v = y*fy_*invz + cy_;


// jacobian from se3 to u,v

// NOTE that in g2o the Lie algebra is (omega, epsilon), where omega is so(3) and epsilon the translation

Eigen::Matrix<double, 2, 6> jacobian_uv_ksai;


//书上8.15式子

jacobian_uv_ksai ( 0,0 ) = - x*y*invz_2 *fx_;

jacobian_uv_ksai ( 0,1 ) = ( 1+ ( x*x*invz_2 ) ) *fx_;

jacobian_uv_ksai ( 0,2 ) = - y*invz *fx_;

jacobian_uv_ksai ( 0,3 ) = invz *fx_;

jacobian_uv_ksai ( 0,4 ) = 0;

jacobian_uv_ksai ( 0,5 ) = -x*invz_2 *fx_;


jacobian_uv_ksai ( 1,0 ) = - ( 1+y*y*invz_2 ) *fy_;

jacobian_uv_ksai ( 1,1 ) = x*y*invz_2 *fy_;

jacobian_uv_ksai ( 1,2 ) = x*invz *fy_;

jacobian_uv_ksai ( 1,3 ) = 0;

jacobian_uv_ksai ( 1,4 ) = invz *fy_;

jacobian_uv_ksai ( 1,5 ) = -y*invz_2 *fy_;


Eigen::Matrix<double, 1, 2> jacobian_pixel_uv;


//书上I2对像素坐标系的偏导数
这里很有可能 计算出来的梯度为0
因为FAST角点的梯度没有限制

//这也是半稠密法主要改进的地方 就是选关键点的时候 选择梯度大的点 因此这里的梯度就不可能为0了

jacobian_pixel_uv ( 0,0 ) = ( getPixelValue ( u+1,v )-getPixelValue ( u-1,v ) ) /2;

jacobian_pixel_uv ( 0,1 ) = ( getPixelValue ( u,v+1 )-getPixelValue ( u,v-1 ) ) /2;


_jacobianOplusXi = jacobian_pixel_uv*jacobian_uv_ksai;//书上8.16式子

}


// dummy read and write functions because we don't care...

virtual bool read ( std::istream& in ) {}

virtual bool write ( std::ostream& out ) const {}

protected:

// get a gray scale value from reference image (bilinear interpolated)

//cv::Mat中成员变量代表的含义:http://blog.csdn.net/dcrmg/article/details/52294259

//下面的方式 针对单通道的灰度图

inline float getPixelValue ( float x, float y )//通过双线性插值获取浮点坐标对应的插值后的像素值

{

uchar* data = & image_->data[ int ( y ) * image_->step + int ( x ) ];//step表示图像矩阵一行的所有字节（包括所有通道的总和），data表示存储图像的开始指针

float xx = x - floor ( x );
//取整函数

float yy = y - floor ( y );

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)

//这里的xx 就是u
yy就是v

( 1-xx ) * ( 1-yy ) * data[0] +

xx* ( 1-yy ) * data[1] +

( 1-xx ) *yy*data[ image_->step ] +
//I(i+1,j) //这里相当于像素的周期是image_->step，即每一行存储像素的个数为image_->step

xx*yy*data[image_->step+1]
//I(i+1,j+1)
//data[image_->step]是I(i,j)对应的下一行像素为I(i+1,j)

);

}
public:

Eigen::Vector3d x_world_;
// 3D point in world frame

float cx_=0, cy_=0, fx_=0, fy_=0; // Camera intrinsics

cv::Mat* image_=nullptr;
// reference image
};

int main ( int argc, char** argv )
{

if ( argc != 2 )

{

cout<<"usage: useLK path_to_dataset"<<endl;

return 1;

}

srand ( ( unsigned int ) time ( 0 ) );

string path_to_dataset = argv[1];

string associate_file = path_to_dataset + "/associate.txt";


ifstream fin ( associate_file );


string rgb_file, depth_file, time_rgb, time_depth;

cv::Mat color, depth, gray;

vector<Measurement> measurements;//Measurement类 存储世界坐标点（以第一帧为参考的FAST关键点） 和 对应的灰度图像（由color->gray）的灰度值

// 相机内参

float cx = 325.5;

float cy = 253.5;

float fx = 518.0;

float fy = 519.0;

float depth_scale = 1000.0;

Eigen::Matrix3f K;

K<<fx,0.f,cx,0.f,fy,cy,0.f,0.f,1.0f;


Eigen::Isometry3d Tcw = Eigen::Isometry3d::Identity();//三维变换矩阵T 4X4 初始时刻是单位R矩阵+0平移向量


cv::Mat prev_color;

// 我们以第一个图像为参考，对后续图像和参考图像做直接法 ，每一副图像 都会与第一帧图像做直接法计算第一帧到当前帧的RT 但是经过更多的帧后 关键点的数量会减少，

//所以实际应用时 应当规定关键点的数量少于多少 就该从新设定参考系，再次利用直接法 ，但是会累计的误差需要解决？？？？

for ( int index=0; index<10; index++ )//总共10帧

{

cout<<"*********** loop "<<index<<" ************"<<endl;

fin>>time_rgb>>rgb_file>>time_depth>>depth_file;

color = cv::imread ( path_to_dataset+"/"+rgb_file );

depth = cv::imread ( path_to_dataset+"/"+depth_file, -1 );//-1 按原图像的方式存储 detph 16位存储

if ( color.data==nullptr || depth.data==nullptr )

continue;

//转换后的灰度图为g2o优化需要的边提供灰度值

cv::cvtColor ( color, gray, cv::COLOR_BGR2GRAY );
//将颜色图3通道 转换为灰度图单通道 8位无符号 对应边类的双线性插值计算放法以单通道计算的


//第一帧为世界坐标系 计算FAST关键点 为之后与当前帧用直接法计算RT做准备

if ( index ==0 )//以第一帧为参考系 计算关键点后存储测量值（关键点对应的灰度值） 以此为基准跟踪后面的图像 计算位姿

{

// 对第一帧提取FAST特征点

vector<cv::KeyPoint> keypoints;

cv::Ptr<cv::FastFeatureDetector> detector = cv::FastFeatureDetector::create();

detector->detect ( color, keypoints );

//对于2D关键点获取 3D信息 并去掉范围外的点 存储符合要求的关键点的深度值和3D信息

//对所有关键点挑选出符合要求且有深度值的 存储到vector<Measurement> measurements中 为g2o边提供灰度测量值和空间点坐标

for ( auto kp:keypoints )

{
#if SEMIDENSE
//表示利用半稠密法的思想
只不过结果由原来的1402个点 变为了201个点 特征点数目降低了 但是看起来精度还是很高 可以适当调整梯度域值

Eigen::Vector2d delta (
//计算像素坐标系下 两个方向的变化量

gray.ptr<uchar>(int ( kp.pt.y))[ int(kp.pt.x+1)] - gray.ptr<uchar>(int(kp.pt.y))[int(kp.pt.x-1)],

gray.ptr<uchar>(int(kp.pt.y+1))[int(kp.pt.x)] - gray.ptr<uchar>(int(kp.pt.y-1))[int(kp.pt.x)]

);

//cout<<" keypoints坐标值: "<<kp.pt.x<<" "<<kp.pt.y<<endl;//可以看出点虽然存储方式是浮点数 但是实际的值都是int类型

if ( delta.norm() < GradientThread )//可以转变为变化量的2范数小于50/16 默认域值为30 可调

continue;
#endif

// 去掉邻近边缘处的点 在离图像四条边20个像素构成的内矩阵范围内是符合要求的关键点

if ( kp.pt.x < 20 || kp.pt.y < 20 || ( kp.pt.x+20 ) >color.cols || ( kp.pt.y+20 ) >color.rows )

continue;

//depth.ptr<ushort>( kp.pt.y)获取行指针
cvRound(kp.pt,y) 表示返回跟参数值最接近的整数值 因为像素量化后是整数，而kp.pt.y存储方式是float，所以强制转换一下即可

ushort d = depth.ptr<ushort> ( cvRound ( kp.pt.y ) ) [ cvRound ( kp.pt.x ) ];//16位深度图

if ( d==0 )

continue;

Eigen::Vector3d p3d = project2Dto3D ( kp.pt.x, kp.pt.y, d, fx, fy, cx, cy, depth_scale );
//3D相机坐标系（第一帧 也是世界帧）
#if BookExercise
//计算小块平均灰度值作为对应单一像素的测量值 增加算法健壮性

float grayscale = getPatchAverageGray( gray , kp.pt.x , kp.pt.y , PATCH_RADIUS_ONE );
#else
//否则是正常以单个像素的灰度值作为测量值

float grayscale = float ( gray.ptr<uchar> ( cvRound ( kp.pt.y ) ) [ cvRound ( kp.pt.x ) ] );
//8位无符号也取整数 因为灰度图 是对应整数（int）的像素的 kp.pt.y是float
#endif

measurements.push_back ( Measurement ( p3d, grayscale ) );

}

prev_color = color.clone();
//深拷贝color图像

continue;

}

// 使用直接法计算相机运动

//从第二帧开始计算相机位姿g2o优化

chrono::steady_clock::time_point t1 = chrono::steady_clock::now();

//优化过程中要提供灰度图像 边里面计算误差函数需要 为getPixelValue()该函数提供灰度值查找

poseEstimationDirect ( measurements, &gray, K, Tcw );//Tcw为世界坐标到下一帧坐标的累计值 最后Tcw的结果是从世界坐标 到当前帧下的转换

chrono::steady_clock::time_point t2 = chrono::steady_clock::now();

chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>> ( t2-t1 );

cout<<"direct method costs time: "<<time_used.count() <<" seconds."<<endl;

cout<<"Tcw="<<Tcw.matrix() <<endl;


// plot the feature points

cv::Mat img_show ( color.rows*2, color.cols, CV_8UC3 );//目的是为了之后对比前后两帧图像的关键点数量 所以建立一个可以存储pre_color 和color 大小的矩阵

//Rect(参数)表示坐标0,0 到cols,rows 那么大的矩形

//img_show.operator(const Rect &roi 参数:表示这个矩阵的某个兴趣区域)

//img_show.opertor返回一个构造的矩阵 Mat(const Mat& m, const Rect& roi);这个构造函数返回引用m矩阵中roi那部分感兴趣的范围

//最终结果是:prev_color矩阵元素拷贝到了 img_show矩阵对应Rect兴趣区域 因为img_show 是一个2*row行 cols列 可以包含两个prev_color矩阵

prev_color.copyTo ( img_show ( cv::Rect ( 0,0,color.cols, color.rows ) ) );//0列 0行 ->cols列 rows行 大小 //实际上就是把第一帧的图像拷贝到img_show中

//因为我们针对每一帧图像都会把第一帧图像拷贝到这里 所以这里实际上执行一次即可

//可以修改 前加上仅仅对第二帧执行一次即可

color.copyTo ( img_show ( cv::Rect ( 0,color.rows,color.cols, color.rows ) ) );//0列 rows行 ->cols列 rows行 大小


//在measurements容器中 随机挑选出符合要求的测量值 在img_show矩阵中对应部分进行标记（因为img_show上半部分是第一帧图像，下半部分是当前图像）

for ( Measurement m:measurements )

{

if ( rand() > RAND_MAX/5 )

continue;

Eigen::Vector3d p = m.pos_world;

Eigen::Vector2d pixel_prev = project3Dto2D ( float( p ( 0,0 ) ), p ( 1,0 ), p ( 2,0 ), fx, fy, cx, cy );//世界坐标系下的 图像坐标2D

Eigen::Vector3d p2 = Tcw*m.pos_world;//将空间点转换到下一帧相机坐标系下

Eigen::Vector2d pixel_now = project3Dto2D ( p2 ( 0,0 ), p2 ( 1,0 ), p2 ( 2,0 ), fx, fy, cx, cy );//当前帧坐标系下的图像像素坐标

//对于超出下一帧图像像素坐标轴范围的点 舍弃不画

if ( pixel_now(0,0)<0 || pixel_now(0,0)>=color.cols || pixel_now(1,0)<0 || pixel_now(1,0)>=color.rows )

continue;

//随机获取bgr颜色 在cv::circle中 为关键点用不同的颜色圆来画出

float b = 255*float ( rand() ) /RAND_MAX;

float g = 255*float ( rand() ) /RAND_MAX;

float r = 255*float ( rand() ) /RAND_MAX;

//在img_show包含两帧图像上 以关键点为圆心画圆 半径为8个像素 颜色为bgr随机组合
2表示外轮廓线宽度为2 如果为负数则表示填充圆

//pixel_prev 都是世界坐标系下的坐标 (以第一帧为参考系) 和当前帧下的对比 可以看出关键点的数量会逐渐减少

cv::circle ( img_show, cv::Point2d ( pixel_prev ( 0,0 ), pixel_prev ( 1,0 ) ), 8, cv::Scalar ( b,g,r ), 2 );

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的下半部分

//连接前后两针匹配好的点

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 );

}

cv::imshow ( "result", img_show );

cv::waitKey ( 0 );


}

return 0;
}

bool poseEstimationDirect ( const vector< Measurement >& measurements, cv::Mat* gray, Eigen::Matrix3f& K, Eigen::Isometry3d& Tcw )
{

// 初始化g2o

typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,1>> DirectBlock;
// 求解的向量是6＊1的 因为一元边 所以后面的1可以是其他的数字

auto
linearSolver = g2o::make_unique<g2o::LinearSolverDense< DirectBlock::PoseMatrixType >>();

auto
solver_ptr = g2o::make_unique<DirectBlock>( std::move(linearSolver) );

g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg ( std::move(solver_ptr) );
//
DirectBlock::LinearSolverType* linearSolver = new g2o::LinearSolverDense< DirectBlock::PoseMatrixType > ();
//
DirectBlock* solver_ptr = new DirectBlock ( linearSolver );

// g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr ); // G-N
//
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr ); // L-M

g2o::SparseOptimizer optimizer;

optimizer.setAlgorithm ( solver );

optimizer.setVerbose( true );


auto pose = new g2o::VertexSE3Expmap();

pose->setEstimate ( g2o::SE3Quat ( Tcw.rotation(), Tcw.translation() ) );

pose->setId ( 0 );

optimizer.addVertex ( pose );


// 添加边

int id=1;

for ( Measurement m: measurements )

{

auto edge = new EdgeSE3ProjectDirect (

m.pos_world,

K ( 0,0 ), K ( 1,1 ), K ( 0,2 ), K ( 1,2 ), gray

);

edge->setVertex ( 0, pose );//设置一元边链接的顶点

edge->setMeasurement ( m.grayscale );//设置测量值 即把前一帧关键点的灰度值作为测量值 供给下一帧进行匹配计算RT

edge->setInformation ( Eigen::Matrix<double,1,1>::Identity() );//因为误差维度是1 所以信心矩阵为1x1

edge->setId ( id++ );

optimizer.addEdge ( edge );

}

cout<<"edges in graph: "<<optimizer.edges().size() <<endl;//边的个数 实际上反应了关键点的个数

optimizer.initializeOptimization();

optimizer.optimize ( 30 );

Tcw = pose->estimate();
}

#if
BookExercise
//获取小块平均灰度值
// gray:灰度矩阵 x,y表示以(x,y)为中心
计算的小块的平均灰度
patchSize 表示块的半径
float getPatchAverageGray(const cv::Mat &gray ,float u, float v,int patchRadius){

int x = cvRound(u);

int y = cvRound(v);

if( (patchRadius < 0) || ( (2*patchRadius+1) > 5 ) ){

std::cout<<"Error:请修改PATCH_RADIUS为指定值1 or 2! "<<std::endl;

exit(1);

}

float grayscale = 0.0;
//
y - patchRadius;//代表y坐标
//
x - patchRadius;//代表x坐标

for (int j = y-patchRadius; j <= y+patchRadius ; ++j)

for(auto i = x-patchRadius;i<= (x+patchRadius); ++i){

grayscale += float ( gray.ptr<uchar> (j)[i] );

}

grayscale/= ( (2*patchRadius + 1)*(2*patchRadius +1) );

return grayscale;
}
#endif