稀疏直接法视觉里程计中的特征点优化：基于光度误差最小化的灰度图像线性插值技术

特征点直接法 的 稀疏直接法 视觉里程计 最小化光度误差 灰度二维线性插值

github连接 点击打开链接

#include //输入输出流
#include //文件流
#include //列表
#include //容器
#include //计时
#include //时间
#include //限制
//opencv
#include
#include //图像处理
#include
#include //二维特征
//g2o 非线性凸优化
#include //一元边
#include //矩阵快求解器 矩阵分解
#include //LM 数字优化算法
#include //线性方程求解器
#include //
#include //系统定义的顶点类型 6自由度位姿using namespace std;
using namespace g2o;/********************************************* 本节演示了RGBD上的稀疏直接法 ********************************************/// 一次测量的值&＃xff0c;包括一个世界坐标系下三维点与一个灰度值
struct Measurement//结构体
{Measurement ( Eigen::Vector3d p, float g ) : pos_world ( p ), grayscale ( g ) {}//直接赋值初始化Eigen::Vector3d pos_world;//世界坐标系下三维点 双整数float grayscale;//点对应的灰度值 浮点型
};// 2D坐标 由 相机内参数 和深度信息 得到 像极坐标系下的三维坐标
// 相机归一化 平面下坐标 x &＃61; K逆* px y &＃61; K逆* py 相机坐标系下 归一化平面上的点 //相机内参数 K&＃61;
// [fx 0 cx
// 0 fy cy
// 0 0 1]
// x&＃61; (px -cx)/fx
// y&＃61;(py-cy)/fy
// z &＃61; 1
// 而深度值 为 dd&＃61; d/scale mm 转成m
// 则相机坐标系下的坐标为 xx &＃61; x*dd yy &＃61; y * dd zz &＃61; z * dd
inline Eigen::Vector3d project2Dto3D ( int x, int y, int d, float fx, float fy, float cx, float cy, float scale )
{float zz &＃61; float ( d ) /scale;// 深度 mm 转成mfloat xx &＃61; zz* ( x-cx ) /fx; //反过来 像素坐标 x &＃61; xx/zz * fx &＃43; cxfloat yy &＃61; zz* ( y-cy ) /fy;// 像素坐标 y &＃61; yy/zz * fy &＃43; cy return Eigen::Vector3d ( xx, yy, zz );
}// 相机坐标系下 三维坐标 (x, y, z)转化成 像素坐标(u, v)
// 像素坐标 x &＃61; xx/zz * fx &＃43; cx
// 像素坐标 y &＃61; yy/zz * fy &＃43; cy
inline Eigen::Vector2d project3Dto2D ( float x, float y, float z, float fx, float fy, float cx, float cy )
{float u &＃61; fx*x/z&＃43;cx;float v &＃61; fy*y/z&＃43;cy;return Eigen::Vector2d ( u,v );
}// 直接法估计位姿
// 输入&＃xff1a;测量值&＃xff08;空间点的灰度&＃xff09;&＃xff0c;新的灰度图&＃xff0c;相机内参&＃xff1b; 输出&＃xff1a;相机位姿 4*4 转换矩阵形式
// 返回&＃xff1a;true为成功&＃xff0c;false失败
bool poseEstimationDirect ( const vector& measurements, cv::Mat* gray, Eigen::Matrix3f& intrinsics, Eigen::Isometry3d& Tcw );//g20图优化
// 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 // 类成员 有Eigen 变量时需要 显示 加此句话 宏定义EdgeSE3ProjectDirect() {}//默认构造函数//自定义构造函数&＃xff0c;参数为://一个3d点世界坐标系下坐标//内参矩阵的4个参数//参考图&＃xff0c;灰度图EdgeSE3ProjectDirect ( Eigen::Vector3d point, float fx, float fy, float cx, float cy, cv::Mat* image )//成员变量 image 灰度图: x_world_ ( point ), fx_ ( fx ), fy_ ( fy ), cx_ ( cx ), cy_ ( cy ), image_ ( image ) {}//计算误差 覆写 计算误差的 虚函数virtual void computeError(){const VertexSE3Expmap* v &＃61;static_cast ( _vertices[0] );//类型强转 相机位姿 Eigen::Vector3d x_local &＃61; v->estimate().map ( x_world_ );//第二帧下 相机坐标系下的三维坐标x_local[0]&＃xff0c;x_local[1]&＃xff0c;x_local[2]// 相机坐标系下 三维坐标 (x, y, z)转化成 像素坐标(u, v)float x &＃61; x_local[0]*fx_/x_local[2] &＃43; cx_;float y &＃61; x_local[1]*fy_/x_local[2] &＃43; cy_;// check x,y is in the imageif ( x-4<0 || ( x&＃43;4 ) >image_->cols || ( y-4 ) <0 || ( y&＃43;4 ) >image_->rows )//像素点坐标超出 有效范围{_error ( 0,0 ) &＃61; 0.0;//误差0this->setLevel ( 1 );//边 效果差 不考虑}else{// 这里 误差为e &＃61; I(p2) - I2(p1) 原来e &＃61; I(p1) - I2(p2) 所以 雅克比 相差一个 负号 // 上面计算出来的 x&＃xff0c;y 为浮点数形式 // 需要得到 整数形式 的 坐标值 对应图像的亮度值 需要进行插值运算 这里使用了 双线性插值_error ( 0,0 ) &＃61; getPixelValue ( x,y ) - _measurement;//根据 像素坐标 和 灰度图得到 灰度值 - 测量值}}//覆写求雅克比矩阵 的虚函数
// plus in manifoldvirtual void linearizeOplus( ){if ( level() &＃61;&＃61; 1 ){_jacobianOplusXi &＃61; Eigen::Matrix::Zero();return;}VertexSE3Expmap* vtx &＃61; static_cast ( _vertices[0] );Eigen::Vector3d xyz_trans &＃61; vtx->estimate().map ( x_world_ ); // q in bookdouble x &＃61; xyz_trans[0];//Xdouble y &＃61; xyz_trans[1];//Ydouble invz &＃61; 1.0/xyz_trans[2];// 1/Zdouble invz_2 &＃61; invz*invz;// 1/Z^2float u &＃61; x*fx_*invz &＃43; cx_;float v &＃61; y*fy_*invz &＃43; 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// 旋转在前 平移在后 g2o u对∇f的偏导数 像素坐标 对 变换矩阵李代数增量 的导数
// J1&＃61; [ fx *X*Y/Z^2 -fx *(1 &＃43; X^2/Z^2) fx*Y/Z -fx/Z 0 fx * X/Z^2 // fy *(1 &＃43; Y^2/Z^2) -fy * X*Y/Z^2 -fy*X/Z 0 -fy/Z fy* Y/Z^2 ]
// 上面 误差为e &＃61; I(p2) - I2(p1) 原来e &＃61; I(p1) - I2(p2) 所以 雅克比 相差一个 负号 Eigen::Matrix jacobian_uv_ksai;jacobian_uv_ksai ( 0,0 ) &＃61; - x*y*invz_2 *fx_;jacobian_uv_ksai ( 0,1 ) &＃61; ( 1&＃43; ( x*x*invz_2 ) ) *fx_;jacobian_uv_ksai ( 0,2 ) &＃61; - y*invz *fx_;jacobian_uv_ksai ( 0,3 ) &＃61; invz *fx_;jacobian_uv_ksai ( 0,4 ) &＃61; 0;jacobian_uv_ksai ( 0,5 ) &＃61; -x*invz_2 *fx_;jacobian_uv_ksai ( 1,0 ) &＃61; - ( 1&＃43;y*y*invz_2 ) *fy_;jacobian_uv_ksai ( 1,1 ) &＃61; x*y*invz_2 *fy_;jacobian_uv_ksai ( 1,2 ) &＃61; x*invz *fy_;jacobian_uv_ksai ( 1,3 ) &＃61; 0;jacobian_uv_ksai ( 1,4 ) &＃61; invz *fy_;jacobian_uv_ksai ( 1,5 ) &＃61; -y*invz_2 *fy_;// I2对u偏导 J2 图像灰度梯度可以得到(x方向梯度 y方向梯度 离散求解 坐标前后灰度值作差/2) //这里由于各个像素点其实是离散值&＃xff0c;其实求的是差分&＃xff0c;前一个像素灰度值减后一个像素灰度值&＃xff0c;除以2&＃xff0c;即认为是这个方向上的梯度Eigen::Matrix jacobian_pixel_uv;jacobian_pixel_uv ( 0,0 ) &＃61; ( getPixelValue ( u&＃43;1,v )-getPixelValue ( u-1,v ) ) /2;//灰度梯度 x方向 离散形式jacobian_pixel_uv ( 0,1 ) &＃61; ( getPixelValue ( u,v&＃43;1 )-getPixelValue ( u,v-1 ) ) /2;// 灰度梯度 y方向_jacobianOplusXi &＃61; jacobian_pixel_uv*jacobian_uv_ksai;//最后的 雅克比矩阵 }// dummy read and write functions because we don&＃39;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)
// x&＃xff0c;y 为浮点数形式 需要得到 整数形式 的 坐标值 对应图像的亮度值 需要进行插值运算 这里使用了 双线性插值inline float getPixelValue ( float x, float y ){//这里先说一下各个参数的类型&＃xff1a;//image_为Mat*类型&＃xff0c;图像指针&＃xff0c;所以调用data时用->符号&＃xff0c;//data为图像矩阵首地址&＃xff0c;支持数组形式访问&＃xff0c;data[]就是访问到像素的值了&＃xff0c;此处为像素的灰度值&＃xff0c;类型为uchar//关于step有点复杂&＃xff0c;data[]中括号的式子有点复杂&＃xff0c;总的意思就是y行乘上每行内存数&＃xff0c;定位到行&＃xff0c;然后在加上x&＃xff0c;定位到像素//step具体解释在最后面有一些资料//image_->data[int(y)*image_->step &＃43; int(x)]这一步读到了x,y处的灰度值&＃xff0c;类型为uchar&＃xff0c;//但是后面由于线性插值&＃xff0c;需要定位这个像素的位置&＃xff0c;而不是他的灰度值&＃xff0c;所以取其地址&＃xff0c;赋值给data_ptr&＃xff0c;记住它的位置&＃xff0c;后面使用uchar* data_ptr &＃61; & image_->data[ int ( y ) * image_->step &＃43; int ( x ) ];//对应的 灰度图 的灰度值 的地址 行 * step &＃43; 列 对应的 灰度值uchar* data &＃61; data_ptr ;//地址//由于x,y这里有可能带小数&＃xff0c;但是像素位置肯定是整数&＃xff0c;所以&＃xff0c;问题来了&＃xff0c;(1.2, 4.5)像素坐标处的灰度值为多少呢?OK,线性插值&＃xff01;//说一下floor(),std中的cmath函数。向下取整,返回不大于x的整数。例floor(4.9)&＃61;4//xx和yy&＃xff0c;就是取到小数部分。例&＃xff1a;x&＃61;4.9的话&＃xff0c;xx&＃61;x-floor(x)就为0.9。y同理// I(1.2, 4.5) 飞整数的像素值 为周围四点 的 二维线性插值 按距离四点距离大小为权重 // 1-xx xx// 1-yy I(1,4) I(1,5)// yy I(2,4) I(2,5)////float xx &＃61; x - floor ( x );// 计算出来的坐标的 小数部分 float yy &＃61; y - floor ( y );//return float (( 1-xx ) * ( 1-yy ) * data[0] &＃43;xx* ( 1-yy ) * data[1] &＃43;( 1-xx ) *yy*data[ image_->step ] &＃43;xx*yy*data[image_->step&＃43;1]);}public://公开变量Eigen::Vector3d x_world_; // 3D point in world framefloat cx_&＃61;0, cy_&＃61;0, fx_&＃61;0, fy_&＃61;0; // Camera intrinsics 相机内参cv::Mat* image_&＃61;nullptr; // reference image 图像 image 灰度图
};int main ( int argc, char** argv )
{if ( argc !&＃61; 2 ){cout<<"用法&＃xff1a;./direct_sparse path_to_dataset"< measurements;// 相机内参float cx &＃61; 325.5;float cy &＃61; 253.5;float fx &＃61; 518.0;float fy &＃61; 519.0;float depth_scale &＃61; 1000.0;// mm 变成 m Eigen::Matrix3f K;K<>time_rgb>>rgb_file>>time_depth>>depth_file;color &＃61; cv::imread ( path_to_dataset&＃43;"/"&＃43;rgb_file );// rgb 图像depth &＃61; cv::imread ( path_to_dataset&＃43;"/"&＃43;depth_file, -1 );// 深度图if ( color.data&＃61;&＃61;nullptr || depth.data&＃61;&＃61;nullptr )continue; cv::cvtColor ( color, gray, cv::COLOR_BGR2GRAY );//彩色图到灰度图if ( index &＃61;&＃61;0 )//第一帧{// 对第一帧提取FAST特征点vector keypoints;cv::Ptr detector &＃61; cv::FastFeatureDetector::create();detector->detect ( color, keypoints );//检测 特征点for ( auto kp:keypoints ){// 去掉邻近图像边缘处的点if ( kp.pt.x <20 || kp.pt.y <20 || ( kp.pt.x&＃43;20 ) >color.cols || ( kp.pt.y&＃43;20 ) >color.rows )continue;//跳过以下ushort d &＃61; depth.ptr ( cvRound ( kp.pt.y ) ) [ cvRound ( kp.pt.x ) ];//对于特征点的深度if ( d&＃61;&＃61;0 )continue;//跳过Eigen::Vector3d p3d &＃61; project2Dto3D ( kp.pt.x, kp.pt.y, d, fx, fy, cx, cy, depth_scale );//2D像素坐标 转换成 相机坐标系下的 三维点 3Dfloat grayscale &＃61; float ( gray.ptr ( cvRound ( kp.pt.y ) ) [ cvRound ( kp.pt.x ) ] );//特征点 对应的灰度值 坐标值为整数 需要取整measurements.push_back ( Measurement ( p3d, grayscale ) );//测量值为 三维点 和 对应图像的灰度值}prev_color &＃61; color.clone();//赋值 图像continue;//第一幅图 跳过 以下}// 使用直接法计算相机运动chrono::steady_clock::time_point t1 &＃61; chrono::steady_clock::now();//计时开始poseEstimationDirect ( measurements, &gray, K, Tcw );//测量值chrono::steady_clock::time_point t2 &＃61; chrono::steady_clock::now();//计时结束chrono::duration time_used &＃61; chrono::duration_cast> ( t2-t1 );cout<<"直接法耗时 direct method costs time: "< RAND_MAX/5 )continue;Eigen::Vector3d p &＃61; m.pos_world;//测量值的 三维点 pEigen::Vector2d pixel_prev &＃61; project3Dto2D ( p ( 0,0 ), p ( 1,0 ), p ( 2,0 ), fx, fy, cx, cy );//转换成 2d像素坐标Eigen::Vector3d p2 &＃61; Tcw*m.pos_world;//变换到 第二帧图像的坐标系下 Eigen::Vector2d pixel_now &＃61; project3Dto2D ( p2 ( 0,0 ), p2 ( 1,0 ), p2 ( 2,0 ), fx, fy, cx, cy );//转化成 2d像素坐标if ( pixel_now(0,0)<0 || pixel_now(0,0)>&＃61;color.cols || pixel_now(1,0)<0 || pixel_now(1,0)>&＃61;color.rows )// 超出范围的 跳过continue;float b &＃61; 255*float ( rand() ) /RAND_MAX;//随机颜色 分量float g &＃61; 255*float ( rand() ) /RAND_MAX;float r &＃61; 255*float ( rand() ) /RAND_MAX;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 ) &＃43;color.rows ), 8, cv::Scalar ( b,g,r ), 2 );cv::line ( img_show, cv::Point2d ( pixel_prev ( 0,0 ), pixel_prev ( 1,0 ) ), cv::Point2d ( pixel_now ( 0,0 ), pixel_now ( 1,0 ) &＃43;color.rows ), cv::Scalar ( b,g,r ), 1 );}cv::imshow ( "result", img_show );cv::waitKey ( 0 );//等待按键}return 0;
}bool poseEstimationDirect ( const vector& measurements, cv::Mat* gray, Eigen::Matrix3f& K, Eigen::Isometry3d& Tcw )
{// 初始化g2otypedef g2o::BlockSolver> DirectBlock; // 求解的向量 顶点(姿态) 是6&＃xff0a;1的DirectBlock::LinearSolverType* linearSolver &＃61; new g2o::LinearSolverDense ();DirectBlock* solver_ptr &＃61; new DirectBlock ( linearSolver );// g2o::OptimizationAlgorithmGaussNewton* solver &＃61; new g2o::OptimizationAlgorithmGaussNewton( solver_ptr ); // G-N 高斯牛顿g2o::OptimizationAlgorithmLevenberg* solver &＃61; new g2o::OptimizationAlgorithmLevenberg ( solver_ptr ); // L-M g2o::SparseOptimizer optimizer; optimizer.setAlgorithm ( solver );optimizer.setVerbose( true );// 添加顶点g2o::VertexSE3Expmap* pose &＃61; new g2o::VertexSE3Expmap();//位姿pose->setEstimate ( g2o::SE3Quat ( Tcw.rotation(), Tcw.translation() ) );//旋转矩阵 和 平移向量pose->setId ( 0 );//idoptimizer.addVertex ( pose );//添加顶点// 添加边int id&＃61;1;for ( Measurement m: measurements ){EdgeSE3ProjectDirect* edge &＃61; new EdgeSE3ProjectDirect (m.pos_world,//3D 位置K ( 0,0 ), K ( 1,1 ), K ( 0,2 ), K ( 1,2 ), gray//相机内参数 灰度图);edge->setVertex ( 0, pose );//顶点edge->setMeasurement ( m.grayscale );//测量值为真是灰度值edge->setInformation ( Eigen::Matrix::Identity() );//误差 权重 信息矩阵edge->setId ( id&＃43;&＃43; );optimizer.addEdge ( edge );}cout<<"边的数量 edges in graph: "<estimate();// 变换矩阵
}