八、直接法(semidense与sparse)_RGBD
semidense
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
using namespace std;
using namespace g2o;
1. 坐标转换project2Dto3D和project3Dto2D
// 一次测量的值，包括一个世界坐标系下三维点与一个灰度值
struct Measurement
{
Measurement ( Eigen::Vector3d p, float g ) : pos_world ( p ), grayscale ( g ) {} //初始化构造函数
Eigen::Vector3d pos_world;
float grayscale;
};
// 2d像素坐标到3D坐标，RGBD照片d单位毫米，空间点zz单位米，scale单位换算
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 );
}
//3D到2d像素坐标RGB照片
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 );
}
2、直接法估计相机运动poseEstimationDirect（使用g2o）
// 输入：测量值（空间点的灰度），新的灰度图，相机内参； 输出：相机位姿
// 返回：true为成功，false失败
bool poseEstimationDirect ( const vector< Measurement >& measurements, cv::Mat* gray, Eigen::Matrix3f& K, Eigen::Isometry3d& Tcw )
{
// 1. 初始化g2o
typedef g2o::BlockSolver
DirectBlock::LinearSolverType* linearSolver = new g2o::LinearSolverDense< DirectBlock::PoseMatrixType > ();
DirectBlock* solver_ptr = new DirectBlock ( linearSolver );
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr ); // L-M
g2o::SparseOptimizer optimizer;
optimizer.setAlgorithm ( solver );
optimizer.setVerbose( true );
// 2.添加顶点相机位姿李代数pose
g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap();
pose->setEstimate ( g2o::SE3Quat ( Tcw.rotation(), Tcw.translation() ) );
pose->setId ( 0 );
optimizer.addVertex ( pose );
// 3. 添加边，光度误差，一帧图上很多像素代表很多一元边
int id=1;
for ( Measurement m: measurements )
{
EdgeSE3ProjectDirect* edge = new EdgeSE3ProjectDirect (
m.pos_world,
K ( 0,0 ), K ( 1,1 ), K ( 0,2 ), K ( 1,2 ), gray
);
edge->setVertex ( 0, pose );//这些边（误差），对应的顶点都是ID为0那一个pose顶点
//整个过程测量值只有第一帧的灰度值，后面的每一帧根据位姿找出像素点，再找到灰度值，都是估计值
edge->setMeasurement ( m.grayscale );
//信息矩阵设置为单位阵，表征每个边的权重都一样
edge->setInformation ( Eigen::Matrix
edge->setId ( id++ );//依次增加，给边设置一个ID
optimizer.addEdge ( edge );
}
cout<<"edges in graph: "< // 4. 开始优化 optimizer.initializeOptimization(); optimizer.optimize ( 30 ); Tcw = pose->estimate(); } 3.直接法位姿估计的边，构造图优化 EdgeSE3ProjectDirect //自定义的光度误差 边EdgeSE3ProjectDirect，顶点为g2o中李代数位姿节点VertexSE3Expmap class EdgeSE3ProjectDirect: public BaseUnaryEdge< 1, double, VertexSE3Expmap>//一元边 { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW 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 ) {} //光度误差计算，3D点投影到图像平面 virtual void computeError() { //v是位姿pose, x_local是3D估计值 const VertexSE3Expmap* v =static_cast Eigen::Vector3d x_local = v->estimate().map ( x_world_ ); //3D到2D像素点转换（u,v） float x = x_local[0]*fx_/x_local[2] + cx_; //x_local为估计值Vector 3d类型的（x,y,z） float y = x_local[1]*fy_/x_local[2] + cy_; //检查x y是否出界，距离图像四条边4个像素大小内为有效区域 if ( x-4<0 || ( x+4 ) >image_->cols || ( y-4 ) <0 || ( y+4 ) >image_->rows )//rows行；cols列 { _error ( 0,0 ) = 0.0; this->setLevel ( 1 );//! sets the level of the edge } else { //经过在灰度图中插值获取得的灰度值getPixelValue（x,y）减去测量值灰度值 _error ( 0,0 ) = getPixelValue ( x,y ) - _measurement; } } // 计算线性增量，雅可比矩阵J virtual void linearizeOplus( ) { //先判断是否出界，重置 if ( level() == 1 ) { _jacobianOplusXi = Eigen::Matrix return; } // 位姿估计，得到空间坐标系3D坐标 VertexSE3Expmap* vtx = static_cast Eigen::Vector3d xyz_trans = vtx->estimate().map ( x_world_ ); // q in book（x,y,z） //3D转换为2D像素坐标 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（注意g2o的jacobian矩阵前3列与后3列相对于书上公式是对调的） Eigen::Matrix //公式后三列 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 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;//整体最终jacobian矩阵 } // 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)getPixelValue函数通过双线性差值获得浮点坐标对应插值后的像素值 inline float getPixelValue ( float x, float y ) { uchar* data = & image_->data[ int ( y ) * image_->step + int ( x ) ]; float xx = x - floor ( x ); float yy = y - floor ( y ); return float ( ( 1-xx ) * ( 1-yy ) * data[0] + xx* ( 1-yy ) * data[1] + ( 1-xx ) *yy*data[ image_->step ] + xx*yy*data[image_->step+1] ); } public: //变量声明，3D点、内参、灰度图像指针image 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"< 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; //Measurement类存储世界坐标点（以第一帧为参考的FAST关键点）和对应的灰度图像（由color->gray）的灰度值 vector // 相机内参 float cx = 325.5; float cy = 253.5; float fx = 518.0; float fy = 519.0; float depth_scale = 1000.0;//RGBD相机单位毫米，3D点单位米 Eigen::Matrix3f K; K< // 位姿，变换矩阵T Eigen::Isometry3d Tcw = Eigen::Isometry3d::Identity(); cv::Mat prev_color; // 我们以第一个图像为参考，对后续图像和参考图像做直接法 //每一副图像都会与第一帧图像做直接法计算第一帧到当前帧的R,t。 // 参考帧一直是第一帧，而不是循环流动当前帧的上一帧为参考帧 for ( int index=0; index<10; index++ ) { cout<<"*********** loop "< //读入颜色和深度图像 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优化需要的边提供灰度值 //将颜色图3通道 转换为灰度图单通道 8位无符号 对应边类的双线性插值计算放法以单通道计算的 cv::cvtColor ( color, gray, cv::COLOR_BGR2GRAY );//转化为灰度图 if ( index ==0 ) { // select the pixels with high gradiants for ( int x=10; x for ( int y=10; y { Eigen::Vector2d delta (//计算像素坐标系下 两个方向的变化量 gray.ptr gray.ptr ); if ( delta.norm() < 50 ) //自定义阈值，梯度大小 continue; ushort d = depth.ptr if ( d==0 ) continue; Eigen::Vector3d p3d = project2Dto3D ( x, y, d, fx, fy, cx, cy, depth_scale ); float grayscale = float ( gray.ptr measurements.push_back ( Measurement ( p3d, grayscale ) ); } prev_color = color.clone(); cout<<"add total "< continue; } // 使用直接法计算相机运动 poseEstimationDirect ( measurements, &gray, K, Tcw ); cout<<"Tcw="< // plot the feature points //两张对比图拼接显示 cv::Mat img_show ( color.rows*2, color.cols, CV_8UC3 ); // cv::Rect() 行，列到行，列的矩形区域 prev_color.copyTo ( img_show ( cv::Rect ( 0,0,color.cols, color.rows ) ) ); color.copyTo ( img_show ( cv::Rect ( 0,color.rows,color.cols, color.rows ) ) ); //摆完两张图后画上直接法跟踪的关键点和连线 for ( Measurement m:measurements ) { if ( rand() > RAND_MAX/5 ) continue; //上一帧空间点3D坐标 Eigen::Vector3d p = m.pos_world; //上一帧坐标系下的图像像素坐标 Eigen::Vector2d pixel_prev = project3Dto2D ( p ( 0,0 ), p ( 1,0 ), p ( 2,0 ), fx, fy, cx, cy ); //将空间点转换到下一帧相机坐标系下 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; /************上色*************************/ float b = 0; float g = 250; float r = 0; //上图 img_show.ptr img_show.ptr img_show.ptr //下图 img_show.ptr img_show.ptr img_show.ptr /****************************************/ //在img_show包含两帧图像上 以关键点为圆心画圆 半径为4个像素 颜色为bgr随机组合 2表示外轮廓线宽度为2 如果为负数则表示填充圆 cv::circle ( img_show, cv::Point2d ( pixel_prev ( 0,0 ), pixel_prev ( 1,0 ) ), 4, cv::Scalar ( b,g,r ), 2 );//画圆 cv::circle ( img_show, cv::Point2d ( pixel_now ( 0,0 ), pixel_now ( 1,0 ) +color.rows ), 4, 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 ) +color.rows ), cv::Scalar ( 0,0,250 ), 1 ); cv::imshow ( "result", img_show ); cv::waitKey ( 0 ); } return 0; } /-----------------------------------------------------------------------------------------------------------------------------------/ sparse直接法 只有main函数略有不同 int main ( int argc, char** argv ) { if ( argc != 2 ) { cout<<"usage: useLK path_to_dataset"< 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; //Measurement类存储世界坐标点（以第一帧为参考的FAST关键点）和对应的灰度图像（由color->gray）的灰度值 vector // 相机内参 float cx = 325.5; float cy = 253.5; float fx = 518.0; float fy = 519.0; float depth_scale = 1000.0;//RGBD相机单位毫米，3D点单位米 Eigen::Matrix3f K; K< // 位姿，变换矩阵T Eigen::Isometry3d Tcw = Eigen::Isometry3d::Identity(); cv::Mat prev_color； // 我们以第一个图像为参考，对后续图像和参考图像做直接法 //每一副图像都会与第一帧图像做直接法计算第一帧到当前帧的R,t。 // 参考帧一直是第一帧，而不是循环流动当前帧的上一帧为参考帧 for ( int index=0; index<10; index++ ) { cout<<"*********** loop "< //读入颜色和深度图像 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 ); if ( color.data==nullptr || depth.data==nullptr ) continue; cv::cvtColor ( color, gray, cv::COLOR_BGR2GRAY ); if ( index ==0 ) { // 对第一帧提取FAST特征点 //遍历第一帧特征点数组，筛选，把3D坐标和灰度值放到measurement中 vector cv::Ptr detector->detect ( color, keypoints ); for ( auto kp:keypoints ) { // 去掉邻近边缘处的点 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 d = depth.ptr if ( d==0 ) continue; // 找到深度值d后，2D点投影到3D世界坐标系 Eigen::Vector3d p3d = project2Dto3D ( kp.pt.x, kp.pt.y, d, fx, fy, cx, cy, depth_scale ); // 灰度图上关键点灰度值 float grayscale = float ( gray.ptr //得到三维3D坐标和对应的灰度值，作为测量值 measurements.push_back ( Measurement ( p3d, grayscale ) ); } prev_color = color.clone(); continue; } // 使用直接法计算相机运动 //measurements是不变的，之后不断读入的fray灰度图变化 poseEstimationDirect ( measurements, &gray, K, Tcw ); cout<<"Tcw="< // plot the feature points cv::Mat img_show ( color.rows*2, color.cols, CV_8UC3 ); prev_color.copyTo ( img_show ( cv::Rect ( 0,0,color.cols, color.rows ) ) ); color.copyTo ( img_show ( cv::Rect ( 0,color.rows,color.cols, color.rows ) ) ); for ( Measurement m:measurements ) { if ( rand() > RAND_MAX/5 ) continue; Eigen::Vector3d p = m.pos_world; Eigen::Vector2d pixel_prev = project3Dto2D ( p ( 0,0 ), p ( 1,0 ), p ( 2,0 ), fx, fy, cx, cy ); 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; 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 ); 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 ( b,g,r ), 1 ); } cv::imshow ( "result", img_show ); cv::waitKey ( 0 ); } return 0; }