文章目錄

• 9.1理論部分
• 9.2實踐部分
• • 9.2.1 利用ceres進行BA優化（多個相機和路標點）
  • 9.2.2利用g2o進行BA優化（多個相機和路標點）
  • 調試遇到問題bug
• 參考博客

9.1理論部分

推薦參考博文推導:

視覺SLAM十四講學習筆記——第九講 后端優化（1）

slam十四講-ch9（后端1）-卡爾曼濾波器公式推導及BA優化代碼實現【注釋】（應該是ch13前面最難的部分了）

9.2實踐部分

9.2.1 利用ceres進行BA優化（多個相機和路標點）

代碼及詳細注釋如下:

#include <iostream> #include <ceres/ceres.h> #include "common.h" #include "SnavelyReprojectionError.h"using namespace std; //求解最小二乘的函數 void SolveBA(BALProblem &bal_problem);int main(int argc, char **argv) {if (argc != 2) {cout << "usage: bundle_adjustment_ceres bal_data.txt" << endl;return 1;}BALProblem bal_problem(argv[1]);//讀入數據bal_problem.Normalize();//進行歸一化處理bal_problem.Perturb(0.1, 0.5, 0.5);//利用Perturb加入噪聲bal_problem.WriteToPLYFile("initial.ply");//將優化前的數據（相機和3d點） 保存在initial.ply文件中SolveBA(bal_problem);//求解最小二乘問題bal_problem.WriteToPLYFile("final.ply");//將優化后的數據（相機和3d點） 保存在final.ply文件中return 0; } //重點 void SolveBA(BALProblem &bal_problem) {const int point_block_size = bal_problem.point_block_size();const int camera_block_size = bal_problem.camera_block_size();//注意這里獲得待優化系數首地址的時候要用mutable_points()和mutable_cameras()// 因為這兩個函數指向的地址的內容是允許改變的（優化系數肯定要變的啦）double *points = bal_problem.mutable_points();//獲得待優化系數3d點 points指向3d點的首地址double *cameras = bal_problem.mutable_cameras();//獲得待優化系數相機 cameras指向相機的首地址// Observations is 2 * num_observations long array observations// [u_1, u_2, ... u_n], where each u_i is two dimensional, the x// and y position of the observation.const double *observations = bal_problem.observations();//獲得觀測數據 observations指向觀測數據的首地址ceres::Problem problem;//要用循環for (int i = 0; i < bal_problem.num_observations(); ++i) {ceres::CostFunction *cost_function;// Each Residual block takes a point and a camera as input// and outputs a 2 dimensional Residualcost_function = SnavelyReprojectionError::Create(observations[2 * i + 0], observations[2 * i + 1]);// If enabled use Huber's loss function.ceres::LossFunction *loss_function = new ceres::HuberLoss(1.0);//核函數// Each observation corresponds to a pair of a camera and a point// which are identified by camera_index()[i] and point_index()[i]// respectively.//bal_Problem.point_index()這返回的是一個地址指向索引號的首地址double *camera = cameras + camera_block_size * bal_problem.camera_index()[i];double *point = points + point_block_size * bal_problem.point_index()[i];//構建最小二乘問題problem.AddResidualBlock(cost_function, loss_function, camera, point);}/*cost_function,//代價函數loss_function,//核函數camera,//待優化的相機point//待優化的3d點*/// show some information here ...std::cout << "bal problem file loaded..." << std::endl;std::cout << "bal problem have " << bal_problem.num_cameras() << " cameras and "<< bal_problem.num_points() << " points. " << std::endl;std::cout << "Forming " << bal_problem.num_observations() << " observations. " << std::endl;std::cout << "Solving ceres BA ... " << endl;//配置求解器ceres::Solver::Options options;//這里有很多配置選項可以填options.linear_solver_type = ceres::LinearSolverType::SPARSE_SCHUR;//消元options.minimizer_progress_to_stdout = true;//輸出到coutceres::Solver::Summary summary;ceres::Solve(options, &problem, &summary);std::cout << summary.FullReport() << "\n"; }

結果如下:

Header: 16 22106 83718bal problem file loaded... bal problem have 16 cameras and 22106 points. Forming 83718 observations. Solving ceres BA ... iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time0 1.842900e+07 0.00e+00 2.04e+06 0.00e+00 0.00e+00 1.00e+04 0 9.82e-02 3.01e-011 1.449093e+06 1.70e+07 1.75e+06 2.16e+03 1.84e+00 3.00e+04 1 2.54e-01 5.56e-012 5.848543e+04 1.39e+06 1.30e+06 1.55e+03 1.87e+00 9.00e+04 1 1.55e-01 7.11e-013 1.581483e+04 4.27e+04 4.98e+05 4.98e+02 1.29e+00 2.70e+05 1 1.56e-01 8.67e-014 1.251823e+04 3.30e+03 4.64e+04 9.96e+01 1.11e+00 8.10e+05 1 1.53e-01 1.02e+005 1.240936e+04 1.09e+02 9.78e+03 1.33e+01 1.42e+00 2.43e+06 1 1.62e-01 1.18e+006 1.237699e+04 3.24e+01 3.91e+03 5.04e+00 1.70e+00 7.29e+06 1 1.65e-01 1.35e+007 1.236187e+04 1.51e+01 1.96e+03 3.40e+00 1.75e+00 2.19e+07 1 1.52e-01 1.50e+008 1.235405e+04 7.82e+00 1.03e+03 2.40e+00 1.76e+00 6.56e+07 1 1.53e-01 1.65e+009 1.234934e+04 4.71e+00 5.04e+02 1.67e+00 1.87e+00 1.97e+08 1 1.49e-01 1.80e+0010 1.234610e+04 3.24e+00 4.31e+02 1.15e+00 1.88e+00 5.90e+08 1 1.49e-01 1.95e+0011 1.234386e+04 2.24e+00 3.27e+02 8.44e-01 1.90e+00 1.77e+09 1 1.54e-01 2.10e+0012 1.234232e+04 1.54e+00 3.44e+02 6.69e-01 1.82e+00 5.31e+09 1 1.57e-01 2.26e+0013 1.234126e+04 1.07e+00 2.21e+02 5.45e-01 1.91e+00 1.59e+10 1 1.55e-01 2.42e+0014 1.234047e+04 7.90e-01 1.12e+02 4.84e-01 1.87e+00 4.78e+10 1 1.62e-01 2.58e+0015 1.233986e+04 6.07e-01 1.02e+02 4.22e-01 1.95e+00 1.43e+11 1 1.57e-01 2.74e+0016 1.233934e+04 5.22e-01 1.03e+02 3.82e-01 1.97e+00 4.30e+11 1 1.60e-01 2.90e+0017 1.233891e+04 4.25e-01 1.07e+02 3.46e-01 1.93e+00 1.29e+12 1 1.52e-01 3.05e+0018 1.233855e+04 3.59e-01 1.04e+02 3.15e-01 1.96e+00 3.87e+12 1 1.53e-01 3.20e+0019 1.233825e+04 3.06e-01 9.27e+01 2.88e-01 1.98e+00 1.16e+13 1 1.51e-01 3.35e+0020 1.233799e+04 2.61e-01 1.17e+02 2.16e-01 1.97e+00 3.49e+13 1 1.50e-01 3.50e+0021 1.233777e+04 2.18e-01 1.22e+02 1.15e-01 1.97e+00 1.05e+14 1 1.47e-01 3.65e+0022 1.233760e+04 1.73e-01 1.10e+02 9.32e-02 1.89e+00 3.14e+14 1 1.50e-01 3.80e+0023 1.233746e+04 1.37e-01 1.14e+02 1.27e-01 1.98e+00 9.41e+14 1 1.46e-01 3.95e+0024 1.233735e+04 1.13e-01 1.17e+02 3.82e-01 1.96e+00 2.82e+15 1 1.53e-01 4.10e+00 WARNING: Logging before InitGoogleLogging() is written to STDERR W0429 14:34:55.045905 11724 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.25 1.233735e+04 0.00e+00 1.17e+02 0.00e+00 0.00e+00 1.41e+15 1 6.03e-02 4.16e+00 W0429 14:34:55.095809 11724 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.26 1.233735e+04 0.00e+00 1.17e+02 0.00e+00 0.00e+00 3.53e+14 1 4.98e-02 4.21e+0027 1.233725e+04 9.50e-02 1.20e+02 2.02e-01 1.99e+00 1.06e+15 1 1.43e-01 4.35e+0028 1.233718e+04 6.92e-02 5.84e+01 4.77e-01 1.70e+00 3.18e+15 1 2.16e-01 4.57e+00 W0429 14:34:55.524346 11724 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.29 1.233718e+04 0.00e+00 5.84e+01 0.00e+00 0.00e+00 1.59e+15 1 6.97e-02 4.64e+0030 1.233714e+04 3.65e-02 6.13e+01 7.39e-01 1.93e+00 4.77e+15 1 1.50e-01 4.79e+00 W0429 14:34:55.733259 11724 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.31 1.233714e+04 0.00e+00 6.13e+01 0.00e+00 0.00e+00 2.38e+15 1 5.86e-02 4.85e+0032 1.233711e+04 3.32e-02 6.05e+01 5.42e-01 2.00e+00 7.15e+15 1 1.45e-01 4.99e+00 W0429 14:34:55.938537 11724 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.33 1.233711e+04 0.00e+00 6.05e+01 0.00e+00 0.00e+00 3.57e+15 1 6.03e-02 5.05e+00 W0429 14:34:55.988092 11724 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.34 1.233711e+04 0.00e+00 6.05e+01 0.00e+00 0.00e+00 8.94e+14 1 4.95e-02 5.10e+0035 1.233708e+04 3.14e-02 6.14e+01 2.30e-01 2.00e+00 2.68e+15 1 1.40e-01 5.24e+0036 1.233706e+04 2.41e-02 3.85e+02 6.15e+00 1.62e+00 8.04e+15 1 1.50e-01 5.39e+00 W0429 14:34:56.335259 11724 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.37 1.233706e+04 0.00e+00 3.85e+02 0.00e+00 0.00e+00 4.02e+15 1 5.77e-02 5.45e+0038 1.233756e+04 -5.04e-01 3.85e+02 3.06e+01 -5.59e+01 1.01e+15 1 7.26e-02 5.52e+0039 1.233704e+04 1.68e-02 2.03e+01 3.41e-01 1.86e+00 3.02e+15 1 1.49e-01 5.67e+0040 1.234161e+04 -4.57e+00 2.03e+01 5.84e+01 -6.04e+02 1.51e+15 1 8.34e-02 5.75e+0041 1.233702e+04 1.51e-02 2.10e+01 2.66e-01 2.00e+00 4.52e+15 1 1.45e-01 5.90e+00 W0429 14:34:56.842923 11724 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.42 1.233702e+04 0.00e+00 2.10e+01 0.00e+00 0.00e+00 2.26e+15 1 5.78e-02 5.96e+00 W0429 14:34:56.895454 11724 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.43 1.233702e+04 0.00e+00 2.10e+01 0.00e+00 0.00e+00 5.65e+14 1 5.25e-02 6.01e+0044 1.233701e+04 1.48e-02 2.08e+01 1.18e-01 1.99e+00 1.70e+15 1 1.41e-01 6.15e+00 W0429 14:34:57.093436 11724 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.45 1.233701e+04 0.00e+00 2.08e+01 0.00e+00 0.00e+00 8.48e+14 1 5.71e-02 6.21e+0046 1.233700e+04 1.42e-02 2.08e+01 1.47e-01 1.99e+00 2.54e+15 1 1.49e-01 6.35e+00 W0429 14:34:57.300102 11724 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.47 1.233700e+04 0.00e+00 2.08e+01 0.00e+00 0.00e+00 1.27e+15 1 5.78e-02 6.41e+0048 1.233698e+04 1.39e-02 2.19e+01 5.90e-01 2.00e+00 3.82e+15 1 1.62e-01 6.57e+00 W0429 14:34:57.515491 11724 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.49 1.233698e+04 0.00e+00 2.19e+01 0.00e+00 0.00e+00 1.91e+15 1 5.29e-02 6.63e+00 W0429 14:34:57.563660 11724 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.50 1.233698e+04 0.00e+00 2.19e+01 0.00e+00 0.00e+00 4.77e+14 1 4.82e-02 6.68e+00Solver Summary (v 2.0.0-eigen-(3.3.4)-lapack-suitesparse-(5.1.2)-cxsparse-(3.1.9)-eigensparse-no_openmp)Original Reduced Parameter blocks 22122 22122 Parameters 66462 66462 Residual blocks 83718 83718 Residuals 167436 167436Minimizer TRUST_REGIONSparse linear algebra library SUITE_SPARSE Trust region strategy LEVENBERG_MARQUARDTGiven Used Linear solver SPARSE_SCHUR SPARSE_SCHUR Threads 1 1 Linear solver ordering AUTOMATIC 22106,16 Schur structure 2,3,9 2,3,9Cost: Initial 1.842900e+07 Final 1.233698e+04 Change 1.841667e+07Minimizer iterations 51 Successful steps 36 Unsuccessful steps 15Time (in seconds): Preprocessor 0.203083Residual only evaluation 0.549531 (37)Jacobian & residual evaluation 2.340715 (36)Linear solver 3.013938 (50) Minimizer 6.475674Postprocessor 0.005911 Total 6.684669Termination: NO_CONVERGENCE (Maximum number of iterations reached. Number of iterations: 50.)

9.2.2利用g2o進行BA優化（多個相機和路標點）

代碼及詳細注釋如下:

#include <g2o/core/base_vertex.h> #include <g2o/core/base_binary_edge.h> #include <g2o/core/block_solver.h> #include <g2o/core/optimization_algorithm_levenberg.h> #include <g2o/solvers/csparse/linear_solver_csparse.h> #include <g2o/core/robust_kernel_impl.h> #include <iostream>#include "common.h" #include "sophus/se3.hpp"using namespace Sophus; using namespace Eigen; using namespace std;/// 姿態和內參的結構,定義一個結構體表示相機 相機9維 struct PoseAndIntrinsics {PoseAndIntrinsics() {}//顯示構造進行賦值，把數據集的內容賦值過去/// set from given data addressexplicit PoseAndIntrinsics(double *data_addr) {rotation = SO3d::exp(Vector3d(data_addr[0], data_addr[1], data_addr[2]));translation = Vector3d(data_addr[3], data_addr[4], data_addr[5]);focal = data_addr[6];k1 = data_addr[7];k2 = data_addr[8];}/// 將估計值放入內存 //set_to函數是將優化的系數放進內存void set_to(double *data_addr) {auto r = rotation.log();for (int i = 0; i < 3; ++i) data_addr[i] = r[i];for (int i = 0; i < 3; ++i) data_addr[i + 3] = translation[i];data_addr[6] = focal;data_addr[7] = k1;data_addr[8] = k2;}SO3d rotation; //李群 旋轉Vector3d translation = Vector3d::Zero(); //平移double focal = 0;//焦距double k1 = 0, k2 = 0; //畸變系數 };/// 位姿加相機內參的頂點，9維，前三維為so3，接下去為t, f, k1, k2 //定義兩個頂點 一個是 相機（PoseAndIntrinsics） 一個是路標點（3d點） //頂點 ：相機（PoseAndIntrinsics） class VertexPoseAndIntrinsics : public g2o::BaseVertex<9, PoseAndIntrinsics> { public:EIGEN_MAKE_ALIGNED_OPERATOR_NEW;VertexPoseAndIntrinsics() {}//重置virtual void setToOriginImpl() override {_estimate = PoseAndIntrinsics();//給待優化系數賦上初始值}//更新virtual void oplusImpl(const double *update) override {_estimate.rotation = SO3d::exp(Vector3d(update[0], update[1], update[2])) * _estimate.rotation;_estimate.translation += Vector3d(update[3], update[4], update[5]);_estimate.focal += update[6];_estimate.k1 += update[7];_estimate.k2 += update[8];}/// 根據估計值投影一個點Vector2d project(const Vector3d &point) {Vector3d pc = _estimate.rotation * point + _estimate.translation;pc = -pc / pc[2]; //把相機坐標歸一化double r2 = pc.squaredNorm(); //相機坐標歸一化后的模的平方double distortion = 1.0 + r2 * (_estimate.k1 + _estimate.k2 * r2);return Vector2d(_estimate.focal * distortion * pc[0],_estimate.focal * distortion * pc[1]);}//存盤和讀盤 ： 留空virtual bool read(istream &in) {}virtual bool write(ostream &out) const {} };//頂點 ：路標點（3d點） class VertexPoint : public g2o::BaseVertex<3, Vector3d> { public:EIGEN_MAKE_ALIGNED_OPERATOR_NEW;VertexPoint() {}//重置virtual void setToOriginImpl() override {_estimate = Vector3d(0, 0, 0); //待優化系數的初始化}//更新virtual void oplusImpl(const double *update) override {_estimate += Vector3d(update[0], update[1], update[2]);}//存盤和讀盤 ： 留空virtual bool read(istream &in) {}virtual bool write(ostream &out) const {} };//定義邊 這里面邊的定義比前幾章的要簡單很多 class EdgeProjection :public g2o::BaseBinaryEdge<2, Vector2d, VertexPoseAndIntrinsics, VertexPoint> { public:EIGEN_MAKE_ALIGNED_OPERATOR_NEW;//計算殘差virtual void computeError() override {auto v0 = (VertexPoseAndIntrinsics *) _vertices[0];auto v1 = (VertexPoint *) _vertices[1];auto proj = v0->project(v1->estimate());_error = proj - _measurement;}//存盤和讀盤 ： 留空// use numeric derivativesvirtual bool read(istream &in) {}virtual bool write(ostream &out) const {}};void SolveBA(BALProblem &bal_problem);int main(int argc, char **argv) {if (argc != 2) {cout << "usage: bundle_adjustment_g2o bal_data.txt" << endl;return 1;}BALProblem bal_problem(argv[1]);//傳入數據bal_problem.Normalize();//對數據進行歸一化bal_problem.Perturb(0.1,0.5,0.5);//給數據加上噪聲（相機旋轉、相機平移、路標點）bal_problem.WriteToPLYFile("initial_g2o.ply");SolveBA(bal_problem);//求解BAbal_problem.WriteToPLYFile("final_g2o.ply");return 0; }void SolveBA(BALProblem &bal_problem) {/獲得 相機和點的維度const int point_block_size = bal_problem.point_block_size();const int camera_block_size = bal_problem.camera_block_size();//獲得相機和點各自參數的首地址double *points = bal_problem.mutable_points();double *cameras = bal_problem.mutable_cameras();//構建圖優化// pose dimension 9, landmark is 3typedef g2o::BlockSolver<g2o::BlockSolverTraits<9, 3>> BlockSolverType;//兩個頂點的維度typedef g2o::LinearSolverCSparse<BlockSolverType::PoseMatrixType> LinearSolverType;// use LMauto solver = new g2o::OptimizationAlgorithmLevenberg(g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));g2o::SparseOptimizer optimizer;optimizer.setAlgorithm(solver);//設置求解器optimizer.setVerbose(true);//打開調試輸出/// build g2o problem//獲得觀測值的首地址const double *observations=bal_problem.observations();//加入頂點//因為頂點有很多個，所以需要容器//容器 vertex_pose_intrinsics 和 vertex_points存放兩頂點的地址vector<VertexPoseAndIntrinsics *> vertex_pose_intrinsics;vector<VertexPoint *> vertex_points;for (int i = 0; i < bal_problem.num_cameras(); ++i) {VertexPoseAndIntrinsics *v = new VertexPoseAndIntrinsics();double *camera = cameras + camera_block_size * i;//獲得每個相機的首地址v->setId(i);//設置編號v->setEstimate(PoseAndIntrinsics(camera));//傳入待優化的系數 此處為相機optimizer.addVertex(v);//加入頂點vertex_pose_intrinsics.push_back(v);}for (int i = 0; i < bal_problem.num_points(); ++i) {VertexPoint *v = new VertexPoint();//獲得每個路標點的首地址double *point = points + point_block_size * i;v->setId(i + bal_problem.num_cameras());v->setEstimate(Vector3d(point[0], point[1], point[2]));//傳入待優化的系數 此處為路標點// g2o在BA中需要手動設置待Marg的頂點v->setMarginalized(true);//設置邊緣化optimizer.addVertex(v);//加入頂點vertex_points.push_back(v);//將頂點一個一個放回到容器里面}// edgefor (int i = 0; i < bal_problem.num_observations(); ++i) {EdgeProjection *edge = new EdgeProjection;// edge->setId(i);//設置編號edge->setVertex(0,vertex_pose_intrinsics[bal_problem.camera_index()[i]]);//加入頂點edge->setVertex(1,vertex_points[bal_problem.point_index()[i]]);//加入頂點edge->setMeasurement(Sophus::Vector2d(observations[2*i+0],observations[2*i+1]));//設置觀測數據edge->setInformation(Eigen::Matrix2d::Identity());//設置信息矩陣edge->setRobustKernel(new g2o::RobustKernelHuber());//設置核函數optimizer.addEdge(edge);//加入邊}optimizer.initializeOptimization();optimizer.optimize(40);//優化后在存到內從中去// set to bal problemfor (int i = 0; i < bal_problem.num_cameras(); ++i) {double *camera =cameras + camera_block_size*i;auto vertex = vertex_pose_intrinsics[i];//把優化后的頂點地址給vertexauto estimate = vertex->estimate();//這樣estimate就指向了優化后的相機結構體 此時estimate本質上指向了 相機結構體estimate.set_to(camera);//這樣camera就指向了優化后的相機（利用了相機結構體的set_to()函數）}for (int i = 0; i < bal_problem.num_points(); ++i) {double *point = points + point_block_size * i;auto vertex = vertex_points[i];for (int k = 0; k < 3; ++k) point[k] = vertex->estimate()[k];}//經過上面的兩個循環后，原來待優化的系數就被優化完畢了，并且優化后的系數 還是存放在 bal_problem.mutable_cameras()和 bal_problem.mutable_points() 所對應的地址中}

結果如下:

Header: 16 22106 83718iteration= 0 chi2= 8894423.022949 time= 0.324563 cumTime= 0.324563 edges= 83718 schur= 1 lambda= 227.832660 levenbergIter= 1 iteration= 1 chi2= 1772145.050517 time= 0.284179 cumTime= 0.608742 edges= 83718 schur= 1 lambda= 75.944220 levenbergIter= 1 iteration= 2 chi2= 752585.293391 time= 0.30358 cumTime= 0.912323 edges= 83718 schur= 1 lambda= 25.314740 levenbergIter= 1 iteration= 3 chi2= 402814.243627 time= 0.293005 cumTime= 1.20533 edges= 83718 schur= 1 lambda= 8.438247 levenbergIter= 1 iteration= 4 chi2= 284879.378894 time= 0.289516 cumTime= 1.49484 edges= 83718 schur= 1 lambda= 2.812749 levenbergIter= 1 iteration= 5 chi2= 238356.214415 time= 0.283434 cumTime= 1.77828 edges= 83718 schur= 1 lambda= 0.937583 levenbergIter= 1 iteration= 6 chi2= 193550.755079 time= 0.286116 cumTime= 2.06439 edges= 83718 schur= 1 lambda= 0.312528 levenbergIter= 1 iteration= 7 chi2= 146859.909574 time= 0.281158 cumTime= 2.34555 edges= 83718 schur= 1 lambda= 0.104176 levenbergIter= 1 iteration= 8 chi2= 122887.700218 time= 0.272719 cumTime= 2.61827 edges= 83718 schur= 1 lambda= 0.069451 levenbergIter= 1 iteration= 9 chi2= 97810.139925 time= 0.275721 cumTime= 2.89399 edges= 83718 schur= 1 lambda= 0.046300 levenbergIter= 1 iteration= 10 chi2= 80329.940265 time= 0.267307 cumTime= 3.1613 edges= 83718 schur= 1 lambda= 0.030867 levenbergIter= 1 iteration= 11 chi2= 65663.994405 time= 0.321365 cumTime= 3.48266 edges= 83718 schur= 1 lambda= 0.020578 levenbergIter= 1 iteration= 12 chi2= 55960.726637 time= 0.29458 cumTime= 3.77724 edges= 83718 schur= 1 lambda= 0.013719 levenbergIter= 1 iteration= 13 chi2= 53275.547797 time= 0.282115 cumTime= 4.05936 edges= 83718 schur= 1 lambda= 0.009146 levenbergIter= 1 iteration= 14 chi2= 35983.312124 time= 0.395526 cumTime= 4.45488 edges= 83718 schur= 1 lambda= 0.006097 levenbergIter= 2 iteration= 15 chi2= 32091.891518 time= 0.57566 cumTime= 5.03054 edges= 83718 schur= 1 lambda= 0.016259 levenbergIter= 3 iteration= 16 chi2= 31156.262647 time= 0.383858 cumTime= 5.4144 edges= 83718 schur= 1 lambda= 0.021679 levenbergIter= 2 iteration= 17 chi2= 30773.139623 time= 0.311891 cumTime= 5.72629 edges= 83718 schur= 1 lambda= 0.014453 levenbergIter= 1 iteration= 18 chi2= 29079.563460 time= 0.381047 cumTime= 6.10734 edges= 83718 schur= 1 lambda= 0.012488 levenbergIter= 2 iteration= 19 chi2= 28484.154313 time= 0.420185 cumTime= 6.52752 edges= 83718 schur= 1 lambda= 0.016651 levenbergIter= 2 iteration= 20 chi2= 28445.405201 time= 0.316267 cumTime= 6.84379 edges= 83718 schur= 1 lambda= 0.011101 levenbergIter= 1 iteration= 21 chi2= 27170.592543 time= 0.334324 cumTime= 7.17811 edges= 83718 schur= 1 lambda= 0.011118 levenbergIter= 2 iteration= 22 chi2= 26748.191194 time= 0.333374 cumTime= 7.51149 edges= 83718 schur= 1 lambda= 0.014824 levenbergIter= 2 iteration= 23 chi2= 26675.118188 time= 0.26431 cumTime= 7.7758 edges= 83718 schur= 1 lambda= 0.009883 levenbergIter= 1 iteration= 24 chi2= 26087.985781 time= 0.338879 cumTime= 8.11468 edges= 83718 schur= 1 lambda= 0.010281 levenbergIter= 2 iteration= 25 chi2= 25875.818536 time= 0.410122 cumTime= 8.5248 edges= 83718 schur= 1 lambda= 0.013708 levenbergIter= 2 iteration= 26 chi2= 25831.564925 time= 0.285147 cumTime= 8.80994 edges= 83718 schur= 1 lambda= 0.009139 levenbergIter= 1 iteration= 27 chi2= 25568.344873 time= 0.341496 cumTime= 9.15144 edges= 83718 schur= 1 lambda= 0.011118 levenbergIter= 2 iteration= 28 chi2= 25455.865005 time= 0.34611 cumTime= 9.49755 edges= 83718 schur= 1 lambda= 0.011781 levenbergIter= 2 iteration= 29 chi2= 25454.942053 time= 0.331617 cumTime= 9.82917 edges= 83718 schur= 1 lambda= 0.007854 levenbergIter= 1 iteration= 30 chi2= 25260.709796 time= 0.356176 cumTime= 10.1853 edges= 83718 schur= 1 lambda= 0.009148 levenbergIter= 2 iteration= 31 chi2= 25171.392636 time= 0.345109 cumTime= 10.5305 edges= 83718 schur= 1 lambda= 0.009425 levenbergIter= 2 iteration= 32 chi2= 25104.160294 time= 0.420982 cumTime= 10.9514 edges= 83718 schur= 1 lambda= 0.008637 levenbergIter= 2 iteration= 33 chi2= 25042.986799 time= 0.406712 cumTime= 11.3581 edges= 83718 schur= 1 lambda= 0.008765 levenbergIter= 2 iteration= 34 chi2= 24984.677998 time= 0.43978 cumTime= 11.7979 edges= 83718 schur= 1 lambda= 0.005949 levenbergIter= 2 iteration= 35 chi2= 24943.879912 time= 0.419768 cumTime= 12.2177 edges= 83718 schur= 1 lambda= 0.007933 levenbergIter= 2 iteration= 36 chi2= 24886.075504 time= 0.426047 cumTime= 12.6437 edges= 83718 schur= 1 lambda= 0.005674 levenbergIter= 2 iteration= 37 chi2= 24868.088225 time= 0.414774 cumTime= 13.0585 edges= 83718 schur= 1 lambda= 0.007565 levenbergIter= 2 iteration= 38 chi2= 24833.053138 time= 0.431343 cumTime= 13.4899 edges= 83718 schur= 1 lambda= 0.008448 levenbergIter= 2 iteration= 39 chi2= 24815.047826 time= 0.440435 cumTime= 13.9303 edges= 83718 schur= 1 lambda= 0.009766 levenbergIter= 2

利用MeshLab顯示點云文件,final.py.initial.py

調試遇到問題bug

本章調試遇到bug和第8章基本一致,此外還遇到fmt報錯問題,都可以通過修改CmakeList調試通過
修改如下:set(CMAKE_CXX_FLAGS "-O3 -std=c++11")改為set(CMAKE_CXX_FLAGS "-std=c++14 -O2 ${SSE_FLAGS} -msse4"),在每一個target_link_libraries末尾加上 fmt.

cmake_minimum_required(VERSION 2.8)project(bundle_adjustment) set(CMAKE_BUILD_TYPE "Release") #set(CMAKE_CXX_FLAGS "-O3 -std=c++11") set(CMAKE_CXX_FLAGS "-std=c++14 -O2 ${SSE_FLAGS} -msse4")LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)Find_Package(G2O REQUIRED) Find_Package(Eigen3 REQUIRED) Find_Package(Ceres REQUIRED) Find_Package(Sophus REQUIRED) Find_Package(CSparse REQUIRED)SET(G2O_LIBS g2o_csparse_extension g2o_stuff g2o_core cxsparse)include_directories(${PROJECT_SOURCE_DIR} ${EIGEN3_INCLUDE_DIR} ${CSPARSE_INCLUDE_DIR})add_library(bal_common common.cpp) add_executable(bundle_adjustment_g2o bundle_adjustment_g2o.cpp) add_executable(bundle_adjustment_ceres bundle_adjustment_ceres.cpp)target_link_libraries(bundle_adjustment_ceres ${CERES_LIBRARIES} bal_common fmt) target_link_libraries(bundle_adjustment_g2o ${G2O_LIBS} bal_common fmt)

參考博客

slam十四講-ch9（后端1）-卡爾曼濾波器公式推導及BA優化代碼實現【注釋】（應該是ch13前面最難的部分了）

總結

以上是生活随笔為你收集整理的视觉SLAM十四讲学习笔记——ch9后端1的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。