-
【slam十四讲第二版】【课本例题代码向】【第十讲~后端2】
0 前言
1 实践：位姿图优化
1.1 g2o原生位姿图
- 代码的实现需要一个
cmake_modules/FindG2O.cmake文件，链接: https://pan.baidu.com/s/15j-IP1MNoI5_1uPIV5kLOg 提取码: i19h - 需要一个
sphere.g2o文件，链接: https://pan.baidu.com/s/1FyoEwOIB39oC2tUJ639d1A 提取码: m5bw - 整个工程代码自取：链接: https://pan.baidu.com/s/1E7tIuVPoSbX8k80WKXOPrg 提取码: s6l1
1.1.1 pose_graph_g2o_SE3.cpp
#include#include #include #include #include #include #include using namespace std; /************************************************ * 本程序演示如何用g2o solver进行位姿图优化 * sphere.g2o是人工生成的一个Pose graph，我们来优化它。 * 尽管可以直接通过load函数读取整个图，但我们还是自己来实现读取代码，以期获得更深刻的理解 * 这里使用g2o/types/slam3d/中的SE3表示位姿，它实质上是四元数而非李代数. * **********************************************/ int main(int argc, char **argv) { if (argc != 2) { cout << "Usage: pose_graph_g2o_SE3 sphere.g2o" << endl; return 1; } ifstream fin(argv[1]); if (!fin) { cout << "file " << argv[1] << " does not exist." << endl; return 1; } // 设定g2o typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 6>> BlockSolverType; typedef g2o::LinearSolverEigen<BlockSolverType::PoseMatrixType> LinearSolverType; auto solver = new g2o::OptimizationAlgorithmLevenberg( g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>())); g2o::SparseOptimizer optimizer; // 图模型 optimizer.setAlgorithm(solver); // 设置求解器 optimizer.setVerbose(true); // 打开调试输出 int vertexCnt = 0, edgeCnt = 0; // 顶点和边的数量 while (!fin.eof()) { string name; fin >> name; if (name == "VERTEX_SE3:QUAT") { // SE3 顶点 g2o::VertexSE3 *v = new g2o::VertexSE3(); int index = 0; fin >> index; v->setId(index); v->read(fin); optimizer.addVertex(v); vertexCnt++; if (index == 0) v->setFixed(true); } else if (name == "EDGE_SE3:QUAT") { // SE3-SE3 边 g2o::EdgeSE3 *e = new g2o::EdgeSE3(); int idx1, idx2; // 关联的两个顶点 fin >> idx1 >> idx2; e->setId(edgeCnt++); e->setVertex(0, optimizer.vertices()[idx1]); e->setVertex(1, optimizer.vertices()[idx2]); e->read(fin); optimizer.addEdge(e); } if (!fin.good()) break; } cout << "read total " << vertexCnt << " vertices, " << edgeCnt << " edges." << endl; cout << "optimizing ..." << endl; optimizer.initializeOptimization(); optimizer.optimize(30); cout << "saving optimization results ..." << endl; optimizer.save("result.g2o"); return 0; } - 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
- 59
- 60
- 61
- 62
- 63
- 64
- 65
- 66
- 67
- 68
- 69
- 70
- 71
- 72
- 73
- 74
- 75
- 76
- 77
- 78
1.1.2 CMakeLists.txt
cmake_minimum_required(VERSION 2.8) project(pose_graph_g2o_SE3) set(CMAKE_BUILD_TYPE "Release") set(CMAKE_CXX_STANDARD 14) list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules) # Eigen include_directories("/usr/include/eigen3") # sophus find_package(Sophus REQUIRED) include_directories(${Sophus_INCLUDE_DIRS}) # g2o find_package(G2O REQUIRED) include_directories(${G2O_INCLUDE_DIRS}) SET(G2O_LIBS g2o_core g2o_stuff g2o_types_sba g2o_csparse_extension g2o_types_slam3d cxsparse) add_executable(pose_graph_g2o_SE3 src/pose_graph_g2o_SE3.cpp) target_link_libraries(pose_graph_g2o_SE3 ${G2O_LIBS} ${CHOLMOD_LIBRARIES} fmt )- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
1.1.3 输出
/home/bupo/my_study/slam14/slam14_my/cap10/pose_graph_g2o_SE3/cmake-build-debug/pose_graph_g2o_SE3 ./src/sphere.g2o read total 2500 vertices, 9799 edges. optimizing ... iteration= 0 chi2= 1023011093.967642 time= 0.391243 cumTime= 0.391243 edges= 9799 schur= 0 lambda= 805.622433 levenbergIter= 1 iteration= 1 chi2= 385118688.233188 time= 0.288333 cumTime= 0.679577 edges= 9799 schur= 0 lambda= 537.081622 levenbergIter= 1 iteration= 2 chi2= 166223726.693658 time= 0.286221 cumTime= 0.965798 edges= 9799 schur= 0 lambda= 358.054415 levenbergIter= 1 iteration= 3 chi2= 86610874.269316 time= 0.272452 cumTime= 1.23825 edges= 9799 schur= 0 lambda= 238.702943 levenbergIter= 1 iteration= 4 chi2= 40582782.710190 time= 0.271862 cumTime= 1.51011 edges= 9799 schur= 0 lambda= 159.135295 levenbergIter= 1 iteration= 5 chi2= 15055383.753040 time= 0.271552 cumTime= 1.78166 edges= 9799 schur= 0 lambda= 101.425210 levenbergIter= 1 iteration= 6 chi2= 6715193.487654 time= 0.27709 cumTime= 2.05875 edges= 9799 schur= 0 lambda= 37.664667 levenbergIter= 1 iteration= 7 chi2= 2171949.168383 time= 0.27673 cumTime= 2.33548 edges= 9799 schur= 0 lambda= 12.554889 levenbergIter= 1 iteration= 8 chi2= 740566.827049 time= 0.282254 cumTime= 2.61774 edges= 9799 schur= 0 lambda= 4.184963 levenbergIter= 1 iteration= 9 chi2= 313641.802464 time= 0.272131 cumTime= 2.88987 edges= 9799 schur= 0 lambda= 2.583432 levenbergIter= 1 iteration= 10 chi2= 82659.743578 time= 0.277022 cumTime= 3.16689 edges= 9799 schur= 0 lambda= 0.861144 levenbergIter= 1 iteration= 11 chi2= 58220.369189 time= 0.293291 cumTime= 3.46018 edges= 9799 schur= 0 lambda= 0.287048 levenbergIter= 1 iteration= 12 chi2= 52214.188561 time= 0.326622 cumTime= 3.7868 edges= 9799 schur= 0 lambda= 0.095683 levenbergIter= 1 iteration= 13 chi2= 50948.580336 time= 0.28764 cumTime= 4.07444 edges= 9799 schur= 0 lambda= 0.031894 levenbergIter= 1 iteration= 14 chi2= 50587.776729 time= 0.335924 cumTime= 4.41037 edges= 9799 schur= 0 lambda= 0.016436 levenbergIter= 1 iteration= 15 chi2= 50233.038802 time= 0.279265 cumTime= 4.68963 edges= 9799 schur= 0 lambda= 0.010957 levenbergIter= 1 iteration= 16 chi2= 49995.082836 time= 0.274312 cumTime= 4.96394 edges= 9799 schur= 0 lambda= 0.007305 levenbergIter= 1 iteration= 17 chi2= 48876.738968 time= 0.547949 cumTime= 5.51189 edges= 9799 schur= 0 lambda= 0.009298 levenbergIter= 2 iteration= 18 chi2= 48806.625520 time= 0.277708 cumTime= 5.7896 edges= 9799 schur= 0 lambda= 0.006199 levenbergIter= 1 iteration= 19 chi2= 47790.891374 time= 0.534136 cumTime= 6.32374 edges= 9799 schur= 0 lambda= 0.008265 levenbergIter= 2 iteration= 20 chi2= 47713.626578 time= 0.271654 cumTime= 6.59539 edges= 9799 schur= 0 lambda= 0.005510 levenbergIter= 1 iteration= 21 chi2= 46869.323691 time= 0.534435 cumTime= 7.12983 edges= 9799 schur= 0 lambda= 0.007347 levenbergIter= 2 iteration= 22 chi2= 46802.585509 time= 0.273815 cumTime= 7.40364 edges= 9799 schur= 0 lambda= 0.004898 levenbergIter= 1 iteration= 23 chi2= 46128.758046 time= 0.53324 cumTime= 7.93688 edges= 9799 schur= 0 lambda= 0.006489 levenbergIter= 2 iteration= 24 chi2= 46069.133544 time= 0.270033 cumTime= 8.20692 edges= 9799 schur= 0 lambda= 0.004326 levenbergIter= 1 iteration= 25 chi2= 45553.862168 time= 0.53346 cumTime= 8.74038 edges= 9799 schur= 0 lambda= 0.005595 levenbergIter= 2 iteration= 26 chi2= 45511.762622 time= 0.273562 cumTime= 9.01394 edges= 9799 schur= 0 lambda= 0.003730 levenbergIter= 1 iteration= 27 chi2= 45122.763002 time= 0.535634 cumTime= 9.54957 edges= 9799 schur= 0 lambda= 0.004690 levenbergIter= 2 iteration= 28 chi2= 45095.174401 time= 0.26904 cumTime= 9.81861 edges= 9799 schur= 0 lambda= 0.003127 levenbergIter= 1 iteration= 29 chi2= 44811.248507 time= 0.532803 cumTime= 10.3514 edges= 9799 schur= 0 lambda= 0.003785 levenbergIter= 2 saving optimization results ... 进程已结束,退出代码0- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
-
sphere.g2o可视化为
-
result.g2o可视化为
1.2 李代数上的位姿图优化
- 代码的实现需要一个
cmake_modules/FindG2O.cmake文件，链接: https://pan.baidu.com/s/15j-IP1MNoI5_1uPIV5kLOg 提取码: i19h - 需要一个
sphere.g2o文件，链接: https://pan.baidu.com/s/1FyoEwOIB39oC2tUJ639d1A 提取码: m5bw - 整个工程代码自取：链接: https://pan.baidu.com/s/1y1lnVsG8aG0m5i0_YsvalQ 提取码: mv68
1.2.1 pose_graph_g2o_lie_algebra.cpp
#include#include #include #include #include #include #include #include #include #include using namespace std; using namespace Eigen; using Sophus::SE3d; using Sophus::SO3d; /************************************************ * 本程序演示如何用g2o solver进行位姿图优化 * sphere.g2o是人工生成的一个Pose graph，我们来优化它。 * 尽管可以直接通过load函数读取整个图，但我们还是自己来实现读取代码，以期获得更深刻的理解 * 本节使用李代数表达位姿图，节点和边的方式为自定义 * **********************************************/ typedef Matrix<double, 6, 6> Matrix6d; // 给定误差求J_R^{-1}的近似 Matrix6d JRInv(const SE3d &e) { Matrix6d J; J.block(0, 0, 3, 3) = SO3d::hat(e.so3().log()); J.block(0, 3, 3, 3) = SO3d::hat(e.translation()); J.block(3, 0, 3, 3) = Matrix3d::Zero(3, 3); J.block(3, 3, 3, 3) = SO3d::hat(e.so3().log()); //J = J * 0.5 + Matrix6d::Identity(); J = Matrix6d::Identity(); // try Identity if you want return J; } // 李代数顶点 typedef Matrix<double, 6, 1> Vector6d; class VertexSE3LieAlgebra : public g2o::BaseVertex<6, SE3d> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW virtual bool read(istream &is) override { double data[7]; for (int i = 0; i < 7; i++) is >> data[i]; setEstimate(SE3d( Quaterniond(data[6], data[3], data[4], data[5]), Vector3d(data[0], data[1], data[2]) )); } virtual bool write(ostream &os) const override { os << id() << " "; Quaterniond q = _estimate.unit_quaternion(); os << _estimate.translation().transpose() << " "; os << q.coeffs()[0] << " " << q.coeffs()[1] << " " << q.coeffs()[2] << " " << q.coeffs()[3] << endl; return true; } virtual void setToOriginImpl() override { _estimate = SE3d(); } // 左乘更新 virtual void oplusImpl(const double *update) override { Vector6d upd; upd << update[0], update[1], update[2], update[3], update[4], update[5]; _estimate = SE3d::exp(upd) * _estimate; } }; // 两个李代数节点之边 class EdgeSE3LieAlgebra : public g2o::BaseBinaryEdge<6, SE3d, VertexSE3LieAlgebra, VertexSE3LieAlgebra> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW virtual bool read(istream &is) override { double data[7]; for (int i = 0; i < 7; i++) is >> data[i]; Quaterniond q(data[6], data[3], data[4], data[5]); q.normalize(); setMeasurement(SE3d(q, Vector3d(data[0], data[1], data[2]))); for (int i = 0; i < information().rows() && is.good(); i++) for (int j = i; j < information().cols() && is.good(); j++) { is >> information()(i, j); if (i != j) information()(j, i) = information()(i, j); } return true; } virtual bool write(ostream &os) const override { VertexSE3LieAlgebra *v1 = static_cast<VertexSE3LieAlgebra *> (_vertices[0]); VertexSE3LieAlgebra *v2 = static_cast<VertexSE3LieAlgebra *> (_vertices[1]); os << v1->id() << " " << v2->id() << " "; SE3d m = _measurement; Eigen::Quaterniond q = m.unit_quaternion(); os << m.translation().transpose() << " "; os << q.coeffs()[0] << " " << q.coeffs()[1] << " " << q.coeffs()[2] << " " << q.coeffs()[3] << " "; // information matrix for (int i = 0; i < information().rows(); i++) for (int j = i; j < information().cols(); j++) { os << information()(i, j) << " "; } os << endl; return true; } // 误差计算与书中推导一致 virtual void computeError() override { SE3d v1 = (static_cast<VertexSE3LieAlgebra *> (_vertices[0]))->estimate(); SE3d v2 = (static_cast<VertexSE3LieAlgebra *> (_vertices[1]))->estimate(); _error = (_measurement.inverse() * v1.inverse() * v2).log(); } // 雅可比计算 virtual void linearizeOplus() override { SE3d v1 = (static_cast<VertexSE3LieAlgebra *> (_vertices[0]))->estimate(); SE3d v2 = (static_cast<VertexSE3LieAlgebra *> (_vertices[1]))->estimate(); Matrix6d J = JRInv(SE3d::exp(_error)); // 尝试把J近似为I？ _jacobianOplusXi = -J * v2.inverse().Adj(); _jacobianOplusXj = J * v2.inverse().Adj(); } }; int main(int argc, char **argv) { if (argc != 2) { cout << "Usage: pose_graph_g2o_SE3_lie sphere.g2o" << endl; return 1; } ifstream fin(argv[1]); if (!fin) { cout << "file " << argv[1] << " does not exist." << endl; return 1; } // 设定g2o typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 6>> BlockSolverType; typedef g2o::LinearSolverEigen<BlockSolverType::PoseMatrixType> LinearSolverType; auto solver = new g2o::OptimizationAlgorithmLevenberg( g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>())); g2o::SparseOptimizer optimizer; // 图模型 optimizer.setAlgorithm(solver); // 设置求解器 optimizer.setVerbose(true); // 打开调试输出 int vertexCnt = 0, edgeCnt = 0; // 顶点和边的数量 vector<VertexSE3LieAlgebra *> vectices; vector<EdgeSE3LieAlgebra *> edges; while (!fin.eof()) { string name; fin >> name; if (name == "VERTEX_SE3:QUAT") { // 顶点 VertexSE3LieAlgebra *v = new VertexSE3LieAlgebra(); int index = 0; fin >> index; v->setId(index); v->read(fin); optimizer.addVertex(v); vertexCnt++; vectices.push_back(v); if (index == 0) v->setFixed(true); } else if (name == "EDGE_SE3:QUAT") { // SE3-SE3 边 EdgeSE3LieAlgebra *e = new EdgeSE3LieAlgebra(); int idx1, idx2; // 关联的两个顶点 fin >> idx1 >> idx2; e->setId(edgeCnt++); e->setVertex(0, optimizer.vertices()[idx1]); e->setVertex(1, optimizer.vertices()[idx2]); e->read(fin); optimizer.addEdge(e); edges.push_back(e); } if (!fin.good()) break; } cout << "read total " << vertexCnt << " vertices, " << edgeCnt << " edges." << endl; cout << "optimizing ..." << endl; optimizer.initializeOptimization(); optimizer.optimize(30); cout << "saving optimization results ..." << endl; // 因为用了自定义顶点且没有向g2o注册，这里保存自己来实现 // 伪装成 SE3 顶点和边，让 g2o_viewer 可以认出 ofstream fout("result_lie.g2o"); for (VertexSE3LieAlgebra *v:vectices) { fout << "VERTEX_SE3:QUAT "; v->write(fout); } for (EdgeSE3LieAlgebra *e:edges) { fout << "EDGE_SE3:QUAT "; e->write(fout); } fout.close(); return 0; } - 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
- 59
- 60
- 61
- 62
- 63
- 64
- 65
- 66
- 67
- 68
- 69
- 70
- 71
- 72
- 73
- 74
- 75
- 76
- 77
- 78
- 79
- 80
- 81
- 82
- 83
- 84
- 85
- 86
- 87
- 88
- 89
- 90
- 91
- 92
- 93
- 94
- 95
- 96
- 97
- 98
- 99
- 100
- 101
- 102
- 103
- 104
- 105
- 106
- 107
- 108
- 109
- 110
- 111
- 112
- 113
- 114
- 115
- 116
- 117
- 118
- 119
- 120
- 121
- 122
- 123
- 124
- 125
- 126
- 127
- 128
- 129
- 130
- 131
- 132
- 133
- 134
- 135
- 136
- 137
- 138
- 139
- 140
- 141
- 142
- 143
- 144
- 145
- 146
- 147
- 148
- 149
- 150
- 151
- 152
- 153
- 154
- 155
- 156
- 157
- 158
- 159
- 160
- 161
- 162
- 163
- 164
- 165
- 166
- 167
- 168
- 169
- 170
- 171
- 172
- 173
- 174
- 175
- 176
- 177
- 178
- 179
- 180
- 181
- 182
- 183
- 184
- 185
- 186
- 187
- 188
- 189
- 190
- 191
- 192
- 193
- 194
- 195
- 196
- 197
- 198
- 199
- 200
- 201
- 202
- 203
- 204
- 205
- 206
- 207
- 208
- 209
1.2.2 CMakeLists.txt
cmake_minimum_required(VERSION 2.8) project(pose_graph) set(CMAKE_BUILD_TYPE "Release") set(CMAKE_CXX_STANDARD 14) list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules) # Eigen include_directories("/usr/include/eigen3") # sophus find_package(Sophus REQUIRED) include_directories(${Sophus_INCLUDE_DIRS}) # g2o find_package(G2O REQUIRED) include_directories(${G2O_INCLUDE_DIRS}) SET(G2O_LIBS g2o_core g2o_stuff g2o_types_sba g2o_csparse_extension g2o_types_slam3d cxsparse) add_executable(pose_graph_g2o_lie src/pose_graph_g2o_lie_algebra.cpp) target_link_libraries(pose_graph_g2o_lie ${G2O_LIBS} ${CHOLMOD_LIBRARIES} ${Sophus_LIBRARIES} fmt )- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
1.2.3 输出
/home/bupo/my_study/slam14/slam14_my/cap10/pose_graph_g2o_lie_algebra/cmake-build-debug/pose_graph_g2o_lie ./src/sphere.g2o read total 2500 vertices, 9799 edges. optimizing ... iteration= 0 chi2= 674837160.579970 time= 0.313929 cumTime= 0.313929 edges= 9799 schur= 0 lambda= 6658.554263 levenbergIter= 1 iteration= 1 chi2= 234706314.970484 time= 0.291987 cumTime= 0.605915 edges= 9799 schur= 0 lambda= 2219.518088 levenbergIter= 1 iteration= 2 chi2= 142146174.348537 time= 0.277532 cumTime= 0.883447 edges= 9799 schur= 0 lambda= 739.839363 levenbergIter= 1 iteration= 3 chi2= 83834595.145595 time= 0.279928 cumTime= 1.16337 edges= 9799 schur= 0 lambda= 246.613121 levenbergIter= 1 iteration= 4 chi2= 41878079.903257 time= 0.273487 cumTime= 1.43686 edges= 9799 schur= 0 lambda= 82.204374 levenbergIter= 1 iteration= 5 chi2= 16598628.119947 time= 0.272396 cumTime= 1.70926 edges= 9799 schur= 0 lambda= 27.401458 levenbergIter= 1 iteration= 6 chi2= 6137666.739405 time= 0.272809 cumTime= 1.98207 edges= 9799 schur= 0 lambda= 9.133819 levenbergIter= 1 iteration= 7 chi2= 2182986.250593 time= 0.275608 cumTime= 2.25767 edges= 9799 schur= 0 lambda= 3.044606 levenbergIter= 1 iteration= 8 chi2= 732676.668220 time= 0.27199 cumTime= 2.52967 edges= 9799 schur= 0 lambda= 1.014869 levenbergIter= 1 iteration= 9 chi2= 284457.115176 time= 0.273163 cumTime= 2.80283 edges= 9799 schur= 0 lambda= 0.338290 levenbergIter= 1 iteration= 10 chi2= 170796.109734 time= 0.272436 cumTime= 3.07526 edges= 9799 schur= 0 lambda= 0.181974 levenbergIter= 1 iteration= 11 chi2= 145466.315841 time= 0.270867 cumTime= 3.34613 edges= 9799 schur= 0 lambda= 0.060658 levenbergIter= 1 iteration= 12 chi2= 142373.179500 time= 0.271743 cumTime= 3.61787 edges= 9799 schur= 0 lambda= 0.020219 levenbergIter= 1 iteration= 13 chi2= 137485.756901 time= 0.276317 cumTime= 3.89419 edges= 9799 schur= 0 lambda= 0.006740 levenbergIter= 1 iteration= 14 chi2= 131202.175668 time= 0.27323 cumTime= 4.16742 edges= 9799 schur= 0 lambda= 0.002247 levenbergIter= 1 iteration= 15 chi2= 128006.202530 time= 0.272473 cumTime= 4.43989 edges= 9799 schur= 0 lambda= 0.000749 levenbergIter= 1 iteration= 16 chi2= 127587.860945 time= 0.272907 cumTime= 4.7128 edges= 9799 schur= 0 lambda= 0.000250 levenbergIter= 1 iteration= 17 chi2= 127578.599359 time= 0.272402 cumTime= 4.9852 edges= 9799 schur= 0 lambda= 0.000083 levenbergIter= 1 iteration= 18 chi2= 127578.573853 time= 0.27214 cumTime= 5.25734 edges= 9799 schur= 0 lambda= 0.000028 levenbergIter= 1 iteration= 19 chi2= 127578.573840 time= 0.272582 cumTime= 5.52992 edges= 9799 schur= 0 lambda= 0.000018 levenbergIter= 1 iteration= 20 chi2= 127578.573840 time= 0.270247 cumTime= 5.80017 edges= 9799 schur= 0 lambda= 0.000012 levenbergIter= 1 iteration= 21 chi2= 127578.573840 time= 0.275518 cumTime= 6.07569 edges= 9799 schur= 0 lambda= 0.000008 levenbergIter= 1 iteration= 22 chi2= 127578.573840 time= 0.793899 cumTime= 6.86959 edges= 9799 schur= 0 lambda= 0.000044 levenbergIter= 3 iteration= 23 chi2= 127578.573840 time= 0.791677 cumTime= 7.66127 edges= 9799 schur= 0 lambda= 0.000234 levenbergIter= 3 iteration= 24 chi2= 127578.573840 time= 2.63651 cumTime= 10.2978 edges= 9799 schur= 0 lambda= 5483030743.383683 levenbergIter= 10 saving optimization results ... 进程已结束,退出代码0- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- sphere.g2o的可视化
- result_lie.g2o的可视化
- 代码的实现需要一个
-
相关阅读:
网站架构演变、LNP+Mariadb数据库分离、Web服务器集群、Keepalived高可用
threejs(12)-着色器打造烟雾水云效果
鸿蒙原生应用元服务-访问控制（权限）开发应用权限列表二
[算法日志]图论： 深度优先搜索(DFS)
[BPU部署教程] 教你搞定YOLOV5部署 (版本: 6.2)
Vue路由重复点击报错解决
Can 通信-协议
c++语法拾遗，一些细节与特性
python笔记Ⅴ----元组、字典、集合
【博客529】linux cpu的 “steal time“含义
- 原文地址：https://blog.csdn.net/qq_45954434/article/details/126035397