后段优化（BA图优化）"/>
SLAM高翔十四讲（九）第九讲 后段优化（BA图优化）
文章目录
- 一、概述
- 1.1 问题引入
- 1.2 相关问题
- 二、滤波器方法
- 2.1 线性系统和KF
- 2.2 非线性系统和EKF
- 三、BA与图优化
- 3.1 投影模型和求解BA代价函数
- 3.2 稀疏性和边缘化
- 3.3 鲁棒核函数
- 四、实践：Ceres求解BA
- 4.1 CMakeList.txt
- 4.2 自定义的库文件
- 4.3 代码演示
- 4.4 结果
- 五、实践：g2o求解BA
- 5.1 CMakeList.txt
- 5.2 代码演示
- 5.3 结果
一、概述
1.1 问题引入
前端视觉里程计能给出短时间内的轨迹和地图，但由于误差的积累，长时间是不准确的。因此，在前端的基础上。我们希望构建一个尺度、规模更大的优化问题，考虑长时间内最优的轨迹和地图。
1.2 相关问题
- 在后端优化优化中，使用过去和未来的信息更新自己的状态，我们称这种处理方式为“批量的”；当前的状态由过去某时刻甚至前一个时刻决定，我们称为“渐近的”。
- 优化方法：
1）扩展卡尔曼滤波EKF：当前状态只与前一时刻状态有关
2）非线性优化：当前状态与之前所有状态有关
二、滤波器方法
2.1 线性系统和KF
- 我们假设运动方程和观测方程可以由线性方程描述，使用卡尔曼滤波器KF构成线性系统的最优无偏估计。
- 算法过程分成“预测”和“更新”。参见课本P239
2.2 非线性系统和EKF
- 事实上，SLAM的动方程和观测方程是非线性的，一个高斯分布经过非线性变换后不再是高斯分布，因此，我们取一定近似，使得非高斯分布近似成高斯分布。卡尔曼滤波器的结果拓展到非线性系统中，成为非卡尔曼滤波器EKF。
- 算法过程分成“预测”和“更新”。参见课本P240
- EKF的局限
1）假设的马尔可夫性当（前状态只与前一时刻状态有关）局限，如果和很久以前的数据有关，滤波器将会很难处理；
2）由于使用一阶泰勒展开，存在非线性误差；
3）不适用大型系统
4）没有异常检测机制
三、BA与图优化
所谓BA(Bundle Adjustment)，是指从视觉推向中提炼出最优的3D模型和相机参数(内参和外参)。
3.1 投影模型和求解BA代价函数
参见这篇博客
3.2 稀疏性和边缘化
边缘化的目的是利用H的稀疏性加速计算。
参见这篇博客
3.3 鲁棒核函数
- BA问题中将最小化误差项的二范数平方和作为目标函数，当出现误匹配时，会造成很大误差。
- 鲁棒核函数：当误差超过某个阈值时，替换成一个增长比较慢的函数，同时保持光滑性（求导）。
- 种类：Huber函数、Cauchy函数、Tukey函数等。
四、实践：Ceres求解BA
4.1 CMakeList.txt
cmake_minimum_required(VERSION 2.8)project(bundle_adjustment)
set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_FLAGS "-std=c++14 -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_LIBRARIES} fmt)
target_link_libraries(bundle_adjustment_g2o ${G2O_LIBS} bal_common ${FMT_LIBRARIES} fmt)
4.2 自定义的库文件
- SnavelyReprojectionError.h用来构建误差模型
#ifndef SnavelyReprojection_H
#define SnavelyReprojection_H#include <iostream>
#include "ceres/ceres.h"
#include "rotation.h"// 定义投影误差模型
class SnavelyReprojectionError {
public:SnavelyReprojectionError(double observation_x, double observation_y) : observed_x(observation_x),observed_y(observation_y) {}// 残差的计算template<typename T>// 重载“()”运算bool operator()(const T *const camera,const T *const point,T *residuals) const {// camera[0,1,2] are the angle-axis rotationT predictions[2];CamProjectionWithDistortion(camera, point, predictions);residuals[0] = predictions[0] - T(observed_x);residuals[1] = predictions[1] - T(observed_y);return true;}// camera : 9 dims array// [0-2] : angle-axis rotation// [3-5] : translateion// [6-8] : camera parameter, [6] focal length, [7-8] second and forth order radial distortion// point : 3D location.// predictions : 2D predictions with center of the image plane.template<typename T>static inline bool CamProjectionWithDistortion(const T *camera, const T *point, T *predictions) {// Rodrigues' formulaT p[3];AngleAxisRotatePoint(camera, point, p);// camera[3,4,5] are the translationp[0] += camera[3];p[1] += camera[4];p[2] += camera[5];// Compute the center fo distortionT xp = -p[0] / p[2];T yp = -p[1] / p[2];// Apply second and fourth order radial distortionconst T &l1 = camera[7];const T &l2 = camera[8];T r2 = xp * xp + yp * yp;T distortion = T(1.0) + r2 * (l1 + l2 * r2);const T &focal = camera[6];predictions[0] = focal * distortion * xp;predictions[1] = focal * distortion * yp;return true;}static ceres::CostFunction *Create(const double observed_x, const double observed_y) {return (new ceres::AutoDiffCostFunction<SnavelyReprojectionError, 2, 9, 3>( //ceres的自动求导机制new SnavelyReprojectionError(observed_x, observed_y))); //构建一个误差模型}private:double observed_x;double observed_y;
};#endif // SnavelyReprojection.h- common.h构建一个BAL类，用来对BAL数据集处理
#pragma once/// 从文件读入BAL dataset
class BALProblem {
public:/// load bal data from text fileexplicit BALProblem(const std::string &filename, bool use_quaternions = false);~BALProblem() {delete[] point_index_;delete[] camera_index_;delete[] observations_;delete[] parameters_;}/// save results to text filevoid WriteToFile(const std::string &filename) const;/// save results to ply pointcloudvoid WriteToPLYFile(const std::string &filename) const;void Normalize();void Perturb(const double rotation_sigma,const double translation_sigma,const double point_sigma);int camera_block_size() const { return use_quaternions_ ? 10 : 9; }int point_block_size() const { return 3; }int num_cameras() const { return num_cameras_; }int num_points() const { return num_points_; }int num_observations() const { return num_observations_; }int num_parameters() const { return num_parameters_; }const int *point_index() const { return point_index_; }const int *camera_index() const { return camera_index_; }const double *observations() const { return observations_; }const double *parameters() const { return parameters_; }const double *cameras() const { return parameters_; }const double *points() const { return parameters_ + camera_block_size() * num_cameras_; }/// camera参数的起始地址double *mutable_cameras() { return parameters_; }double *mutable_points() { return parameters_ + camera_block_size() * num_cameras_; }double *mutable_camera_for_observation(int i) {return mutable_cameras() + camera_index_[i] * camera_block_size();}double *mutable_point_for_observation(int i) {return mutable_points() + point_index_[i] * point_block_size();}const double *camera_for_observation(int i) const {return cameras() + camera_index_[i] * camera_block_size();}const double *point_for_observation(int i) const {return points() + point_index_[i] * point_block_size();}private:void CameraToAngelAxisAndCenter(const double *camera,double *angle_axis,double *center) const;void AngleAxisAndCenterToCamera(const double *angle_axis,const double *center,double *camera) const;int num_cameras_;int num_points_;int num_observations_;int num_parameters_;bool use_quaternions_;int *point_index_; // 每个observation对应的point indexint *camera_index_; // 每个observation对应的camera indexdouble *observations_;double *parameters_;
};
- rotation.h旋转矩阵、四元数等的转换
#ifndef ROTATION_H
#define ROTATION_H#include <algorithm>
#include <cmath>
#include <limits>//
// 旋转转换所需的数学函数。
// math functions needed for rotation conversion. // dot and cross production template<typename T>
inline T DotProduct(const T x[3], const T y[3]) {return (x[0] * y[0] + x[1] * y[1] + x[2] * y[2]);
}template<typename T>
inline void CrossProduct(const T x[3], const T y[3], T result[3]) {result[0] = x[1] * y[2] - x[2] * y[1];result[1] = x[2] * y[0] - x[0] * y[2];result[2] = x[0] * y[1] - x[1] * y[0];
}//// 将角度anxis转换为四元数：
// Converts from a angle anxis to quaternion :
template<typename T>
inline void AngleAxisToQuaternion(const T *angle_axis, T *quaternion) {const T &a0 = angle_axis[0];const T &a1 = angle_axis[1];const T &a2 = angle_axis[2];const T theta_squared = a0 * a0 + a1 * a1 + a2 * a2;if (theta_squared > T(std::numeric_limits<double>::epsilon())) {const T theta = sqrt(theta_squared);const T half_theta = theta * T(0.5);const T k = sin(half_theta) / theta;quaternion[0] = cos(half_theta);quaternion[1] = a0 * k;quaternion[2] = a1 * k;quaternion[3] = a2 * k;} else { // in case if theta_squared is zeroconst T k(0.5);quaternion[0] = T(1.0);quaternion[1] = a0 * k;quaternion[2] = a1 * k;quaternion[3] = a2 * k;}
}// 将四元数转换为角度anxis：
template<typename T>
inline void QuaternionToAngleAxis(const T *quaternion, T *angle_axis) {const T &q1 = quaternion[1];const T &q2 = quaternion[2];const T &q3 = quaternion[3];const T sin_squared_theta = q1 * q1 + q2 * q2 + q3 * q3;// For quaternions representing non-zero rotation, the conversion// is numercially stableif (sin_squared_theta > T(std::numeric_limits<double>::epsilon())) {const T sin_theta = sqrt(sin_squared_theta);const T &cos_theta = quaternion[0];// If cos_theta is negative, theta is greater than pi/2, which// means that angle for the angle_axis vector which is 2 * theta// would be greater than pi...const T two_theta = T(2.0) * ((cos_theta < 0.0)? atan2(-sin_theta, -cos_theta): atan2(sin_theta, cos_theta));const T k = two_theta / sin_theta;angle_axis[0] = q1 * k;angle_axis[1] = q2 * k;angle_axis[2] = q3 * k;} else {// For zero rotation, sqrt() will produce NaN in derivative since// the argument is zero. By approximating with a Taylor series,// and truncating at one term, the value and first derivatives will be// computed correctly when Jets are used..const T k(2.0);angle_axis[0] = q1 * k;angle_axis[1] = q2 * k;angle_axis[2] = q3 * k;}}// 将角度anxis转换为旋转点：
template<typename T>
inline void AngleAxisRotatePoint(const T angle_axis[3], const T pt[3], T result[3]) {const T theta2 = DotProduct(angle_axis, angle_axis);if (theta2 > T(std::numeric_limits<double>::epsilon())) {// Away from zero, use the rodriguez formula//// result = pt costheta +// (w x pt) * sintheta +// w (w . pt) (1 - costheta)//// We want to be careful to only evaluate the square root if the// norm of the angle_axis vector is greater than zero. Otherwise// we get a division by zero.//const T theta = sqrt(theta2);const T costheta = cos(theta);const T sintheta = sin(theta);const T theta_inverse = 1.0 / theta;const T w[3] = {angle_axis[0] * theta_inverse,angle_axis[1] * theta_inverse,angle_axis[2] * theta_inverse};// Explicitly inlined evaluation of the cross product for// performance reasons./*const T w_cross_pt[3] = { w[1] * pt[2] - w[2] * pt[1],w[2] * pt[0] - w[0] * pt[2],w[0] * pt[1] - w[1] * pt[0] };*/T w_cross_pt[3];CrossProduct(w, pt, w_cross_pt);const T tmp = DotProduct(w, pt) * (T(1.0) - costheta);// (w[0] * pt[0] + w[1] * pt[1] + w[2] * pt[2]) * (T(1.0) - costheta);result[0] = pt[0] * costheta + w_cross_pt[0] * sintheta + w[0] * tmp;result[1] = pt[1] * costheta + w_cross_pt[1] * sintheta + w[1] * tmp;result[2] = pt[2] * costheta + w_cross_pt[2] * sintheta + w[2] * tmp;} else {// Near zero, the first order Taylor approximation of the rotation// matrix R corresponding to a vector w and angle w is//// R = I + hat(w) * sin(theta)//// But sintheta ~ theta and theta * w = angle_axis, which gives us//// R = I + hat(w)//// and actually performing multiplication with the point pt, gives us// R * pt = pt + w x pt.//// Switching to the Taylor expansion near zero provides meaningful// derivatives when evaluated using Jets.//// Explicitly inlined evaluation of the cross product for// performance reasons./*const T w_cross_pt[3] = { angle_axis[1] * pt[2] - angle_axis[2] * pt[1],angle_axis[2] * pt[0] - angle_axis[0] * pt[2],angle_axis[0] * pt[1] - angle_axis[1] * pt[0] };*/T w_cross_pt[3];CrossProduct(angle_axis, pt, w_cross_pt);result[0] = pt[0] + w_cross_pt[0];result[1] = pt[1] + w_cross_pt[1];result[2] = pt[2] + w_cross_pt[2];}
}#endif // rotation.h
4.3 代码演示
#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;//}// 1. BALProblem自定义类，用于对BAL数据集的处理BALProblem bal_problem("/home/robot/桌面/slambook2-master/ch9/problem-16-22106-pre.txt");// 2. 模拟投影过程bal_problem.Normalize(); // 归一化bal_problem.Perturb(0.1, 0.5, 0.5); // 噪声处理// 3. CeresBA求解bal_problem.WriteToPLYFile("initial.ply");SolveBA(bal_problem); // 非线性优化BAbal_problem.WriteToPLYFile("final.ply");return 0;
}void SolveBA(BALProblem &bal_problem) {// 1. 初始化以及设置一些参数，这是是对BAL数据集问题的处理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(); // // 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();ceres::Problem problem;// 2. 构建最小二乘问题for (int i = 0; i < bal_problem.num_observations(); ++i) {ceres::CostFunction *cost_function;// 1）定义投影误差模型// 每个残差块以一个点和一个相机作为输入，并输出二维残差cost_function = SnavelyReprojectionError::Create(observations[2 * i + 0], observations[2 * i + 1]);// 2）使用Huber核函数ceres::LossFunction *loss_function = new ceres::HuberLoss(1.0);// 3）每个观测对应于一对相机和一个点，它们分别由camera_index（）[i]和point_index）[i]标识double *camera = cameras + camera_block_size * bal_problem.camera_index()[i];double *point = points + point_block_size * bal_problem.point_index()[i];// 4）将上述加入到Ceres的Problem中problem.AddResidualBlock(cost_function, loss_function, camera, point);}// 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;// 3. 配置求解器ceres::Solver::Options options;// 增量方程如何求解：这里采用边缘化\Schur消元options.linear_solver_type = ceres::LinearSolverType::SPARSE_SCHUR; // 输出到coutoptions.minimizer_progress_to_stdout = true;// 4. 优化信息ceres::Solver::Summary summary;// 5. 开始优化（求解器，最小二乘问题，优化信息）ceres::Solve(options, &problem, &summary);std::cout << summary.FullReport() << "\n";
}
4.4 结果
总误差随着迭代次数增加而减小，我们将优化前与优化后的点云输出到initial.ply和final.ply，用Meshlab可直接打开查看
Header: 16 22106 83718
BAL 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 6.76e-02 1.99e-011 1.449093e+06 1.70e+07 1.75e+06 2.16e+03 1.84e+00 3.00e+04 1 1.54e-01 3.53e-012 5.848543e+04 1.39e+06 1.30e+06 1.55e+03 1.87e+00 9.00e+04 1 1.17e-01 4.70e-013 1.581483e+04 4.27e+04 4.98e+05 4.98e+02 1.29e+00 2.70e+05 1 1.11e-01 5.81e-014 1.251823e+04 3.30e+03 4.64e+04 9.96e+01 1.11e+00 8.10e+05 1 1.50e-01 7.31e-015 1.240936e+04 1.09e+02 9.78e+03 1.33e+01 1.42e+00 2.43e+06 1 1.16e-01 8.47e-016 1.237699e+04 3.24e+01 3.91e+03 5.04e+00 1.70e+00 7.29e+06 1 1.28e-01 9.75e-017 1.236187e+04 1.51e+01 1.96e+03 3.40e+00 1.75e+00 2.19e+07 1 1.41e-01 1.12e+008 1.235405e+04 7.82e+00 1.03e+03 2.40e+00 1.76e+00 6.56e+07 1 1.10e-01 1.23e+00
......
五、实践：g2o求解BA
5.1 CMakeList.txt
同4.1
5.2 代码演示
#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;// 我们用节点表示相机和路标，用边表示它们之间的观测
/*** 1. PoseAndIntrinsics：定义位姿和内参的结构体* 2. VertexPoseAndIntrinsics：定义模型的位姿加相机内参的顶点* 3. VertexPoint：定义模型的顶点* 4. EdgeProjection：定义误差模型* */
// 定义位姿和内参的结构体
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];}// 将估计值放入内存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
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 {}
};
// 定义模型的顶点，模板参数：优化变量维度和数据类型
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 {}
};
// 定义误差模型 模板参数：观测值维度，类型，连接顶点类型 这里BaseBinaryEdge表示二元边
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;//}// 1. BALProblem自定义类，用于对BAL数据集的处理BALProblem bal_problem("/home/robot/桌面/slambook2-master/ch9/problem-16-22106-pre.txt");// 2. 模拟投影过程bal_problem.Normalize(); // 归一化bal_problem.Perturb(0.1, 0.5, 0.5); // 噪声处理、// 3. CeresBA求解bal_problem.WriteToPLYFile("initial.ply");SolveBA(bal_problem); // 非线性优化g2oBAbal_problem.WriteToPLYFile("final.ply");return 0;
}void SolveBA(BALProblem &bal_problem) {// 1. 初始化以及设置一些参数，这是是对BAL数据集问题的处理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();// 2. 构建图优化，先设定g2o// 每个误差项优化变量维度为9，误差值维度为3// pose dimension 9, landmark is 3typedef g2o::BlockSolver<g2o::BlockSolverTraits<9, 3>> BlockSolverType;// 线性方程求解器typedef g2o::LinearSolverCSparse<BlockSolverType::PoseMatrixType> LinearSolverType;// 选择优化算法，从GN, LM, DogLeg 中选auto solver = new g2o::OptimizationAlgorithmLevenberg(g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));g2o::SparseOptimizer optimizer;// 图模型optimizer.setAlgorithm(solver);// 设置求解器:线性方程求解器->矩阵块求解器->优化算法->g2ooptimizer.setVerbose(true);// 打开调试输出// 3. 往图中增加顶点和边const double *observations = bal_problem.observations();// 顶点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中需要手动设置待边缘化的顶点v->setMarginalized(true);optimizer.addVertex(v);vertex_points.push_back(v);}// 边for (int i = 0; i < bal_problem.num_observations(); ++i) {EdgeProjection *edge = new EdgeProjection;edge->setVertex(0, vertex_pose_intrinsics[bal_problem.camera_index()[i]]);edge->setVertex(1, vertex_points[bal_problem.point_index()[i]]);edge->setMeasurement(Vector2d(observations[2 * i + 0], observations[2 * i + 1]));edge->setInformation(Matrix2d::Identity());edge->setRobustKernel(new g2o::RobustKernelHuber());optimizer.addEdge(edge);}// 6. 执行优化optimizer.initializeOptimization();optimizer.optimize(40);// 7. 设置BAL问题，方便下一步WriteToPLYFile写到文件中for (int i = 0; i < bal_problem.num_cameras(); ++i) {double *camera = cameras + camera_block_size * i;auto vertex = vertex_pose_intrinsics[i];auto estimate = vertex->estimate();estimate.set_to(camera);}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];}
}
5.3 结果
更多推荐
SLAM高翔十四讲（九）第九讲 后段优化（BA图优化）
发布评论