说在前面
- 书籍参考:《视觉SLAM十四讲》高翔著
- 书籍代码地址:Github
- 数据集:TUM
- 环境搭建:【SLAM】在WSL中搭建环境(Linux子系统)
- 前篇:【SLAM】视觉SLAM十四讲-设计前端v0.2
- 其他说明:学习记录
代码理解
-
g2o优化
-
前置知识点
《视觉SLAM十四讲》第四讲、第六讲、7.7.3、7.8 -
Bundle Adjustment
重投影误差示意图-《视觉SLAM十四讲》 重投影误差:点 p 2 p_2 p2是空间中3D点P在该帧中的投影,而 p 2 ^ \hat{p_2} p2^为通过相机姿态计算出来的点P的投影点;点 p 2 、 p 2 ^ p_2、\hat{p_2} p2、p2^之间的误差即为重投影误差。
问题提出:对于n个这样的点P,我们希望得到一个最佳的相机姿态,使得这n个点的平均重投影误差最小。
解决方法:最小二乘问题的解决思路。(详细推导见书)
对于:
ξ ∗ = * a r g   m i n ξ 1 2 ∑ i = 1 n ∥ u i − 1 s i K e x p ( ξ ˄ ) P i ∥ \xi^*=\operatorname*{arg\,min}_{\xi}\frac{1}{2}\sum_{i=1}^n\parallel u_i-\frac{1}{s_i}Kexp(\xi^˄)P_i \parallel ξ∗=*argminξ21i=1∑n∥ui−si1Kexp(ξ˄)Pi∥
使用优化方法前,需要求每个误差项(旋转、平移)关于优化变量的导数,也就是线性化:
e ( x + Δ x ) = e ( x ) + J Δ x e(x+\Delta x)=e(x)+J\Delta x e(x+Δx)=e(x)+JΔx
经过一系列推导,得到 J J J如下
∂ e ∂ δ ξ = − [ f x Z ′ 0 − f x X ′ Z ′ 2 − f x X ′ Y ′ Z ′ 2 f x + f x X ′ 2 Z ′ 2 − f x Y ′ Z ′ 0 f y Z ′ − f x Y ′ Z ′ 2 − f y − f y Y ′ 2 Z ′ 2 f y X ′ Y ′ Z ′ 2 f y X ′ Z ′ ] \frac{\partial e}{\partial \delta\xi}=- \left[ \begin{matrix} \frac{f_x}{Z'} & 0 & -\frac{f_xX'}{Z'^2} & -\frac{f_xX'Y'}{Z'^2} & f_x+\frac{f_xX'^2}{Z'^2} & -\frac{f_xY'}{Z'}\\ 0 & \frac{f_y}{Z'} & -\frac{f_xY'}{Z'^2} & -f_y-\frac{f_yY'^2}{Z'^2} & \frac{f_yX'Y'}{Z'^2} & \frac{f_yX'}{Z'} \end{matrix} \right] ∂δξ∂e=−[Z′fx00Z′fy−Z′2fxX′−Z′2fxY′−Z′2fxX′Y′−fy−Z′2fyY′2fx+Z′2fxX′2Z′2fyX′Y′−Z′fxY′Z′fyX′]
注:平移在前旋转在后。
然后我们还需优化特征点的空间位置。经过推导:
∂ e ∂ P = − [ f x Z ′ 0 − f x X ′ Z ′ 2 0 f y Z ′ − f y Y ′ Z ′ 2 ] R \frac{\partial e}{\partial P}=- \left[ \begin{matrix} \frac{f_x}{Z'} & 0 & -\frac{f_xX'}{Z'^2}\\ 0 & \frac{f_y}{Z'} & -\frac{f_yY'}{Z'^2} \end{matrix} \right]R ∂P∂e=−[Z′fx00Z′fy−Z′2fxX′−Z′2fyY′]R
其中 f x 、 f y f_x、f_y fx、fy为相机内参, P ′ ( X ′ , Y ′ , X ′ ) P'(X',Y',X') P′(X′,Y′,X′)为相机坐标系中P的坐标。
-
-
Code
-
g2otype类
可以看到实现的正是上述的2x6的雅可比矩阵;void EdgeProjectXYZ2UVPoseOnly::linearizeOplus() { g2o::VertexSE3Expmap* pose = static_cast<g2o::VertexSE3Expmap*> ( _vertices[0] ); g2o::SE3Quat T ( pose->estimate() ); Vector3d xyz_trans = T.map ( point_ ); double x = xyz_trans[0]; double y = xyz_trans[1]; double z = xyz_trans[2]; double z_2 = z*z; _jacobianOplusXi ( 0,0 ) = x*y/z_2 *camera_->fx_; _jacobianOplusXi ( 0,1 ) = - ( 1+ ( x*x/z_2 ) ) *camera_->fx_; _jacobianOplusXi ( 0,2 ) = y/z * camera_->fx_; _jacobianOplusXi ( 0,3 ) = -1./z * camera_->fx_; _jacobianOplusXi ( 0,4 ) = 0; _jacobianOplusXi ( 0,5 ) = x/z_2 * camera_->fx_; _jacobianOplusXi ( 1,0 ) = ( 1+y*y/z_2 ) *camera_->fy_; _jacobianOplusXi ( 1,1 ) = -x*y/z_2 *camera_->fy_; _jacobianOplusXi ( 1,2 ) = -x/z *camera_->fy_; _jacobianOplusXi ( 1,3 ) = 0; _jacobianOplusXi ( 1,4 ) = -1./z *camera_->fy_; _jacobianOplusXi ( 1,5 ) = y/z_2 *camera_->fy_; }
-
相对于前一版本,主要是在使用PnP求解的时候添加了Bundle Adjustment
- 节点:第二个相机的位姿节点 ξ ∈ s e ( 3 ) ξ ∈ se(3) ξ∈se(3),以及所有特征点的空间位置 P ∈ R 3 P ∈R3 P∈R3
- 边:每个 3D 点在第二个相机中的投影,以观测方程来描述:
z j = h ( ξ , P j ) zj = h(ξ,Pj) zj=h(ξ,Pj)
void VisualOdometry::poseEstimationPnP() { // construct the 3d 2d observations vector<cv::Point3f> pts3d; vector<cv::Point2f> pts2d; for ( cv::DMatch m:feature_matches_ ) { pts3d.push_back( pts_3d_ref_[m.queryIdx] ); pts2d.push_back( keypoints_curr_[m.trainIdx].pt ); } Mat K = ( cv::Mat_<double>(3,3)<< ref_->camera_->fx_, 0, ref_->camera_->cx_, 0, ref_->camera_->fy_, ref_->camera_->cy_, 0,0,1 ); Mat rvec, tvec, inliers;//inliers存储PnP使用到的点对数据的序号(pts3d\pts2d) cv::solvePnPRansac( pts3d, pts2d, K, Mat(), rvec, tvec, false, 100, 4.0, 0.99, inliers ); num_inliers_ = inliers.rows; cout<<"pnp inliers: "<<num_inliers_<<endl; T_c_r_estimated_ = SE3( SO3(rvec.at<double>(0,0), rvec.at<double>(1,0), rvec.at<double>(2,0)), Vector3d( tvec.at<double>(0,0), tvec.at<double>(1,0), tvec.at<double>(2,0)) ); // using bundle adjustment to optimize the pose // 初始化优化器 typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,2>> Block; Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>(); Block* solver_ptr = new Block( linearSolver );//这里需要改一下,见注意 g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr );//这里需要改一下,见注意 g2o::SparseOptimizer optimizer; optimizer.setAlgorithm ( solver ); g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap(); pose->setId ( 0 ); pose->setEstimate ( g2o::SE3Quat ( T_c_r_estimated_.rotation_matrix(), T_c_r_estimated_.translation() ) ); optimizer.addVertex ( pose ); // edges // 添加点对数据,即边数据 for ( int i=0; i<inliers.rows; i++ ) { int index = inliers.at<int>(i,0); // 3D -> 2D projection EdgeProjectXYZ2UVPoseOnly* edge = new EdgeProjectXYZ2UVPoseOnly(); edge->setId(i); edge->setVertex(0, pose); edge->camera_ = curr_->camera_.get(); edge->point_ = Vector3d( pts3d[index].x, pts3d[index].y, pts3d[index].z ); edge->setMeasurement( Vector2d(pts2d[index].x, pts2d[index].y) ); edge->setInformation( Eigen::Matrix2d::Identity() ); optimizer.addEdge( edge ); } // 开始进行优化 optimizer.initializeOptimization(); optimizer.optimize(10); T_c_r_estimated_ = SE3 ( pose->estimate().rotation(), pose->estimate().translation() ); }
注意:将原代码中的下述:
Block* solver_ptr = new Block( linearSolver ); g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr );
改成:
Block* solver_ptr = new Block( std::unique_ptr<Block::LinearSolverType>(linearSolver) ); g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( std::unique_ptr<Block>(solver_ptr) );
-
运行结果
-
我,,,,哭了,这个错误真的是,,,
double free or corruption (out) Aborted (core dumped)
-
最后,还真有人找到解决方式了
哭了/(ㄒoㄒ)/~~ -
结果