MSCKF7讲：特征管理与优化

MSCKF7讲：特征管理与优化

​

​ 写在前面，MSCKF里面维度一个feature.hpp，用来管理特征

std::map<StateIDType, Eigen::Vector4d, std::less<StateIDType>,Eigen::aligned_allocator<std::pair<const StateIDType, Eigen::Vector4d>>>observations;	// 存放了观测到当前特征（路标点）的所有帧id，以及对应坐标系下的归一化坐标，因为这里是双目相机，所以是Vector4d// 3d postion of the feature in the world frame.
Eigen::Vector3d position;

​ msckf是一个vio系统，他不会对路标点进行优化更新，也不关注全局map，不会保存地图信息，只会把要删除的点去做更新，所以下面MapServer中只是保存了一段时间的特征（路标点）

// 地图----特征(路标点)管理
typedef Feature::FeatureIDType FeatureIDType;	// int
typedef std::map<FeatureIDType, Feature, std::less<int>,	// 存储了特征的map容器Eigen::aligned_allocator<std::pair<const FeatureIDType, Feature>>>MapServer;

状态量	含义
MapServer	存储满足要求的特征点，键-特征id，值-对应特征（路标点）
FeatureIDType	长整型long long int
Feature	特征结构体，描述一个路标点应该有的东西

​ MSCKF中特征参数使用了逆深度的形式，即$\mathbf{\lambda }={\left[\begin{array}{ccc}\alpha & \beta & \rho \end{array}\right]}^{\top }$。（X/Z, Y/Z, 1/Z）

使用逆深度

好处：能够表示无穷远点，当特征点较远时， z 数值比较大，直接估计 z 会存在数值问题

坏处：当z趋于0时，ρ趋于无穷大，存在数值奇异，只能用在相机坐标系中，因为相机系下坐标深度都是大于0的，不会趋于0；特别是在非相机坐标系中，因为在那里点的深度可以是负值或零。

1 Feature.h

​ 记录观测特征（路标点）的类，一个特征对应的一些成员（特征id、三维点坐标）、三角化函数等

/** * @brief Feature Salient part of an image.* 一个特征可以理解为一个三维点，由多帧观测到*/
struct Feature
{EIGEN_MAKE_ALIGNED_OPERATOR_NEWtypedef long long int FeatureIDType;struct OptimizationConfig{};	// 优化参数配置// 构造函数:初始化特征id，空间位置Feature() : id(0), position(Eigen::Vector3d::Zero()),is_initialized(false) {}Feature(const FeatureIDType &new_id): id(new_id),position(Eigen::Vector3d::Zero()),is_initialized(false) {}/*** @brief 计算三维点的重投影误差* @param T_c0_c1 即Tcic0* @param x The current estimation.要投影的三维点* @param z The ith measurement of the feature j in ci frame.观测* @return e The cost of this observation.重投影误差*/inline void cost(const Eigen::Isometry3d &T_c0_ci,const Eigen::Vector3d &x, const Eigen::Vector2d &z,double &e) const;/*** @brief 计算观测z对三维点的雅可比(这里的三维点不是传统意义上的三维点，具体看程序吧)* @param T_c0_c1 即Tcic0* @param x The current estimation.要投影的三维点* @param z 第j个特征在第ci帧中的观测* @return J The computed Jacobian.* @return r The computed residual.残差* @return w Weight induced by huber kernel.高斯核*/inline void jacobian(const Eigen::Isometry3d &T_c0_ci,const Eigen::Vector3d &x, const Eigen::Vector2d &z,Eigen::Matrix<double, 2, 3> &J, Eigen::Vector2d &r,double &w) const;/*** @brief 使用两帧(观测到该点的第一帧(左)核最后一帧(右))去三角化初始的三维点坐标* @param z1: feature observation in c1 frame.* @param z2: feature observation in c2 frame.* @return p: Computed feature position in c1 frame.*/inline void generateInitialGuess(const Eigen::Isometry3d &T_c1_c2, const Eigen::Vector2d &z1,const Eigen::Vector2d &z2, Eigen::Vector3d &p) const;/*** @brief 检验三角化是否合理* @param cam_states : input camera poses.* @return True if the translation between the input camera*    poses is sufficient.*/inline bool checkMotion(const CamStateServer &cam_states) const;/*** @brief InitializePosition Intialize the feature position*    based on all current available measurements.供外部调用* @param cam_states:std::map<StateIDType, CAMState>* @return The computed 3d position is used to set the position*    member variable. Note the resulted position is in world*    frame.* @return True if the estimated 3d position of the feature*    is valid.*/inline bool initializePosition(const CamStateServer &cam_states);// 特征idFeatureIDType id;// id for next featurestatic FeatureIDType next_id;// 升序排序的map// 键:观测到这个特征点的 帧id// 值:注意这里4表示的是左右相机的特征点对应坐标std::map<StateIDType, Eigen::Vector4d, std::less<StateIDType>,Eigen::aligned_allocator<std::pair<const StateIDType, Eigen::Vector4d>>>observations;// 对应在世界系下的坐标Eigen::Vector3d position;bool is_initialized;// Noise for a normalized feature measurement.static double observation_noise;// Optimization configuration for solving the 3d position.static OptimizationConfig optimization_config;
};

2 OptimizationConfig

​ 主要设置了最小二乘法—列文伯格算法中的参数、平移量阈值等

/** 优化配置* @brief OptimizationConfig Configuration parameters*    for 3d feature position optimization.* 优化参数*/
struct OptimizationConfig
{// 位移是否足够，用于判断点是否能做三角化double translation_threshold;// huber参数double huber_epsilon;// 修改量阈值，优化的每次迭代都会有更新量，这个量如果太小则表示与目标值接近double estimation_precision;// LM算法lambda的初始值double initial_damping;// 内外轮最大迭代次数int outer_loop_max_iteration;int inner_loop_max_iteration;OptimizationConfig(): translation_threshold(0.2),huber_epsilon(0.01),estimation_precision(5e-7),initial_damping(1e-3),outer_loop_max_iteration(10),inner_loop_max_iteration(10){return;}
};

3 initializePosition三角化+LM优化

功能：feature主函数，利用观测到的特征点来进行三角化（利用左目不同时刻的帧，第一次观测帧、最后一次观测帧），返回是否三角化成功

/*** @brief initializePosition 三角化+LM优化* @param cam_states 所有参与计算的相机位姿 是一个map容器，键-id 值-相机状态* @return 是否三角化成功*/
bool Feature::initializePosition(const CamStateServer &cam_states)
{// Organize camera poses and feature observations properly.// 存放每个观测以及每个对应相机的pos，注意这块是左右目独立存放std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> cam_poses(0);std::vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> measurements(0);// 1. 添加每一个观测对应的帧信息（左右双目）for (auto &m : observations){// TODO: This should be handled properly. Normally, the//    required camera states should all be available in//    the input cam_states buffer.// 找到对应观测到该的特征点的帧auto cam_state_iter = cam_states.find(m.first);if (cam_state_iter == cam_states.end())continue;// Add the measurement.// 1.1 添加归一化坐标measurements.push_back(m.second.head<2>());measurements.push_back(m.second.tail<2>());// This camera pose will take a vector from this camera frame// to the world frame.// TwcEigen::Isometry3d cam0_pose;cam0_pose.linear() =quaternionToRotation(cam_state_iter->second.orientation).transpose();cam0_pose.translation() = cam_state_iter->second.position;Eigen::Isometry3d cam1_pose;cam1_pose = cam0_pose * CAMState::T_cam0_cam1.inverse();// 1.2 添加相机位姿cam_poses.push_back(cam0_pose);cam_poses.push_back(cam1_pose);}// All camera poses should be modified such that it takes a// vector from the first camera frame in the buffer to this// camera frame.// 2. 中心化位姿，提高计算精度---实际上就是把第一帧当基准，找到第一帧到其它帧(包括了右目)的位姿变换Eigen::Isometry3d T_c0_w = cam_poses[0];        // Twc0for (auto &pose : cam_poses)	// 注意这里是引用pose = pose.inverse() * T_c0_w;  // Tcic0 = Tciw *Twc0// Generate initial guess// 3. 使用首末位姿粗略计算出一个三维点坐标----初始解// 这里计算了一个是第一帧坐标系下的三维坐标点初始解Eigen::Vector3d initial_position(0.0, 0.0, 0.0);generateInitialGuess(cam_poses[cam_poses.size() - 1], measurements[0],measurements[measurements.size() - 1], initial_position);// 弄成逆深度形式---归一化三维点--原则上和measurements[0]的前两维度是同样的值。因为估算的时候也是在最后用measurements[0]*depth，只不过这里第三维变成了深度的倒数，即逆深度形式Eigen::Vector3d solution(initial_position(0) / initial_position(2),initial_position(1) / initial_position(2),1.0 / initial_position(2));// 列文伯格算法的相关参数double lambda = optimization_config.initial_damping;int inner_loop_cntr = 0;int outer_loop_cntr = 0;bool is_cost_reduced = false;double delta_norm = 0;// Compute the initial cost.// 4. 利用初计算的三维点(有深度)计算在各个相机下的误差，作为初始误差double total_cost = 0.0;for (int i = 0; i < cam_poses.size(); ++i){double this_cost = 0.0;// 计算投影误差（归一化坐标）cost(cam_poses[i], solution, measurements[i], this_cost);total_cost += this_cost;}// Outer loop.// 5. LM优化开始， 优化三维点坐标，不优化位姿，比较简单do{// A是  J^t * J  B是 J^t * r// 可能有同学疑问自己当初学的时候是 -J^t * r// 这个无所谓，因为这里是负的更新就是正的，而这里是正的，所以更新是负的// 总之就是有一个是负的，总不能误差越来越大吧Eigen::Matrix3d A = Eigen::Matrix3d::Zero();Eigen::Vector3d b = Eigen::Vector3d::Zero();// 5.1 计算AB矩阵for (int i = 0; i < cam_poses.size(); ++i){Eigen::Matrix<double, 2, 3> J;Eigen::Vector2d r;double w;// 计算一目相机观测的雅可比与误差// J 归一化坐标误差相对于三维点的雅可比// r// w 权重，同信息矩阵jacobian(cam_poses[i], solution, measurements[i], J, r, w);// 鲁棒核约束if (w == 1){A += J.transpose() * J;b += J.transpose() * r;}else{double w_square = w * w;A += w_square * J.transpose() * J;b += w_square * J.transpose() * r;}}// Inner loop.// Solve for the delta that can reduce the total cost.// 这层是在同一个雅可比下多次迭代，如果效果不好说明需要调整阻尼因子了，因为线性化不是很好// 如果多次一直误差不下降，退出循环重新计算雅可比do{// LM算法中的lambdaEigen::Matrix3d damper = lambda * Eigen::Matrix3d::Identity();Eigen::Vector3d delta = (A + damper).ldlt().solve(b);// 更新Eigen::Vector3d new_solution = solution - delta;// 统计本次修改量的大小，如果太小表示接近目标值或者陷入局部极小值，那么就没必要继续了delta_norm = delta.norm();// 计算更新后的误差double new_cost = 0.0;for (int i = 0; i < cam_poses.size(); ++i){double this_cost = 0.0;cost(cam_poses[i], new_solution, measurements[i], this_cost);new_cost += this_cost;}// 如果更新后误差比之前小，说明确实是往好方向发展// 我们高斯牛顿的JtJ比较接近真实情况所以减少阻尼，增大步长，delta变大，加快收敛if (new_cost < total_cost){is_cost_reduced = true;solution = new_solution;total_cost = new_cost;lambda = lambda / 10 > 1e-10 ? lambda / 10 : 1e-10;}// 如果不行，那么不要这次迭代的结果// 说明高斯牛顿的JtJ不接近二阶的海森矩阵// 那么增大阻尼，减小步长，delta变小// 并且算法接近一阶的最速下降法else{is_cost_reduced = false;lambda = lambda * 10 < 1e12 ? lambda * 10 : 1e12;}} while (inner_loop_cntr++ <optimization_config.inner_loop_max_iteration &&!is_cost_reduced);inner_loop_cntr = 0;// 直到迭代次数到了或者更新量足够小了} while (outer_loop_cntr++ <optimization_config.outer_loop_max_iteration &&delta_norm > optimization_config.estimation_precision);// Covert the feature position from inverse depth// representation to its 3d coordinate.// 取出最后的结果Eigen::Vector3d final_position(solution(0) / solution(2),solution(1) / solution(2), 1.0 / solution(2));// Check if the solution is valid. Make sure the feature// is in front of every camera frame observing it.// 6. 深度验证bool is_valid_solution = true;for (const auto &pose : cam_poses){Eigen::Vector3d position =pose.linear() * final_position + pose.translation();if (position(2) <= 0){is_valid_solution = false;break;}}// 7. 更新结果// Convert the feature position to the world frame.position = T_c0_w.linear() * final_position + T_c0_w.translation();if (is_valid_solution)is_initialized = true;return is_valid_solution;
}

3.1 计算归一化坐标深度初值generateInitialGuess

这里和常规的三角化并不相同，它只是利用两个归一化坐标的一个旋转变换关系，求取某一个归一化坐标的深度。而不是真正的进行三角化，三角化路标点其中XYZ位置，需要两个匹配点投影才可以求取。

① 理论推导

② 代码分析

这里计算了一个是第一帧坐标系下的三维坐标点初始解

归一化坐标（非像素坐标）

/*** @brief generateInitialGuess 两帧做一次三角化* @param T_c1_c2 两帧间的相对位姿* @param z1 观测1（坐标系1下归一化坐标（不是像素坐标））* @param z2 观测2 都是归一化坐标* @param p 三位点坐标*/
void Feature::generateInitialGuess(const Eigen::Isometry3d &T_c1_c2, const Eigen::Vector2d &z1,const Eigen::Vector2d &z2, Eigen::Vector3d &p) const
{// 列出方程// P2 = R21 * P1 + t21  下面省略21// 两边左乘P2的反对称矩阵// P2^ * (R * P1 + t) = 0// 其中左右可以除以P2的深度，这样P2就成了z2，且P1可以分成z1（归一化平面） 乘上我们要求的深度d// 令m = R * z1// | 0   -1   z2y |    ( | m0 |         )// | 1    0  -z2x | *  ( | m1 | * d + t )  =  0// |-z2y z2x   0  |    ( | m2 |         )// 会发现这三行里面两行是线性相关的，所以只取前两行// Construct a least square problem to solve the depth.Eigen::Vector3d m = T_c1_c2.linear() * Eigen::Vector3d(z1(0), z1(1), 1.0);Eigen::Vector2d A(0.0, 0.0);A(0) = m(0) - z2(0) * m(2);  // 对应第二行// 按照上面推导这里应该是负的但是不影响，因为我们下边b(1)也给取负了A(1) = m(1) - z2(1) * m(2);  // 对应第一行Eigen::Vector2d b(0.0, 0.0);b(0) = z2(0) * T_c1_c2.translation()(2) - T_c1_c2.translation()(0);b(1) = z2(1) * T_c1_c2.translation()(2) - T_c1_c2.translation()(1);// Solve for the depth.// 解方程得出p1的深度值double depth = (A.transpose() * A).inverse() * A.transpose() * b;p(0) = z1(0) * depth;p(1) = z1(1) * depth;p(2) = depth;		// 实际返回的是第一帧坐标系下的三维坐标点return;
}

3.2 计算归一化误差cost

函数功能：计算归一化坐标残差

计算c0系下路标点在每一个ci系下的归一化坐标（预测），并计算归一化坐标z（观测）与预测z_hat的残差e（函数功能参考原作者代码英文注释）

① 理论推导

② 代码分析

/*** @brief cost 计算投影误差（归一化坐标）* @param T_c0_ci 相对位姿，Tcic0 每一个单相机到第一个观测的相机的T* @param x 三维点坐标(x/z, y/z, 1/z)* @param z ci帧下的观测归一化坐标* @param e 误差*/
void Feature::cost(const Eigen::Isometry3d &T_c0_ci,const Eigen::Vector3d &x, const Eigen::Vector2d &z,double &e) const
{// Compute hi1, hi2, and hi3 as Equation (37).const double &alpha = x(0);const double &beta = x(1);const double &rho = x(2);// h 等于  (R * P + t) * 1/Pz// h1    | R11 R12 R13    alpha / rho       t1    |// h2 =  | R21 R22 R23 *  beta  / rho   +   t2    |   *  rho// h3    | R31 R32 R33    1 / rho           t3    |Eigen::Vector3d h =T_c0_ci.linear() * Eigen::Vector3d(alpha, beta, 1.0) +		// T_c0_ci.linear()获取旋转矩阵rho * T_c0_ci.translation();double &h1 = h(0);double &h2 = h(1);double &h3 = h(2);// Predict the feature observation in ci frame.// 求出在另一个相机ci下的归一化坐标Eigen::Vector2d z_hat(h1 / h3, h2 / h3);// Compute the residual.// 两个归一化坐标算误差e = (z_hat - z).squaredNorm();return;
}

3.3 计算雅可比jacobian()

上面计算的是残差，而这里计算残差对α、β、ρ的雅可比，即对(x/z, y/z, 1/z)的雅可比。本质是求解对三维点的雅可比，但不是直接对xyz求雅可比

① 理论推导

② 代码分析

/*** @brief jacobian 求一个观测对应的雅可比* @param T_c0_ci 相对位姿，Tcic0 每一个单相机到第一个观测的相机的T* @param x 三维点坐标(x/z, y/z, 1/z)* @param z 归一化坐标* @param J 雅可比 归一化坐标误差相对于三维点的* @param r 误差* @param w 权重* @return 是否三角化成功*/
void Feature::jacobian(const Eigen::Isometry3d &T_c0_ci,const Eigen::Vector3d &x, const Eigen::Vector2d &z,Eigen::Matrix<double, 2, 3> &J, Eigen::Vector2d &r,double &w) const
{// Compute hi1, hi2, and hi3 as Equation (37).const double &alpha = x(0);  // x/zconst double &beta = x(1);  // y/zconst double &rho = x(2);  // 1/z// h 等于 (R * P + t) * 1/Pz// h1    | R11 R12 R13    alpha / rho       t1    |// h2 =  | R21 R22 R23 *  beta  / rho   +   t2    |   *  rho// h3    | R31 R32 R33    1 / rho           t3    |Eigen::Vector3d h = T_c0_ci.linear() * Eigen::Vector3d(alpha, beta, 1.0) +rho * T_c0_ci.translation();double &h1 = h(0);double &h2 = h(1);double &h3 = h(2);// Compute the Jacobian.// 首先明确一下误差与三维点的关系// 下面的r是误差 r = z_hat - z;  Eigen::Vector2d z_hat(h1 / h3, h2 / h3)// 我们要求r对三维点的雅可比，其中z是观测，与三维点坐标无关// 因此相当于求归一化坐标相对于 alpha beta rho的雅可比，首先要求出他们之间的关系// 归一化坐标设为x y// x = h1/h3 y = h2/h3// 先写出h与alpha beta rho的关系，也就是上面写的// 然后求h1 相对于alpha beta rho的导数 再求h2 的  在求 h3 的// R11 R12 t1// R21 R22 t2// R31 R32 t3// 链式求导法则// ∂x/∂alpha = ∂x/∂h1 * ∂h1/∂alpha + ∂x/∂h3 * ∂h3/∂alpha// ∂x/∂h1 = 1/h3       ∂h1/∂alpha = R11// ∂x/∂h3 = -h1/h3^2   ∂h3/∂alpha = R31// 剩下的就好求了Eigen::Matrix3d W;W.leftCols<2>() = T_c0_ci.linear().leftCols<2>();W.rightCols<1>() = T_c0_ci.translation();// h1 / h3 相对于 alpha beta rho的J.row(0) = 1 / h3 * W.row(0) - h1 / (h3 * h3) * W.row(2);// h1 / h3 相对于 alpha beta rho的J.row(1) = 1 / h3 * W.row(1) - h2 / (h3 * h3) * W.row(2);// Compute the residual.// 求取误差Eigen::Vector2d z_hat(h1 / h3, h2 / h3);r = z_hat - z;// Compute the weight based on the residual.// 使用鲁棒核函数约束double e = r.norm();if (e <= optimization_config.huber_epsilon)w = 1.0;// 如果误差大于optimization_config.huber_epsilon但是没超过他的2倍，那么会放大权重w>1// 如果误差大的离谱，超过他的2倍，缩小他的权重elsew = std::sqrt(2.0 * optimization_config.huber_epsilon / e);return;
}

3.4 计算视差checkMotion—能否三角化

checkMotion Check the input camera poses to ensure there is enough translation to triangulate the feature positon

① 理论分析

​ 代码写的很简单，实际上就是说这角度和基线O1O2不能太小，也不能是180°，要不然达不到三角化条件。

注意：下面feature_direction经过旋转之后还是$O_{1}P$方向，所以简单代替，实际转换后，其为世界系下坐标，不再等同$O_{1}P$，但方向不变！因为$R_{wc}{P}_c=P_w-t_{wc}$，所以方向还是不变的！(或者说一个空间矢量不会因为在不同坐标系下而改变，改变的只是其在不同系下坐标)。
还有这里为什么$O_{1}P$要乘以一个旋转，因为$O_1{O}_2$也是世界系下的，我们要求的点P是世界系下的，即要求这个三角形矢量也都必须在世界系。

② 代码分析

/*** @brief checkMotion 查看是否有足够视差.供外部调用* @param cam_states 所有参与计算的相机位姿* @return 是否有足够视差*/
bool Feature::checkMotion(const CamStateServer &cam_states) const
{// observations存储了所有观测到该特征的帧id和特征坐标// 1. 取出对应的始末帧idconst StateIDType &first_cam_id = observations.begin()->first;// -- 是递减运算符，用于将迭代器向前移动一个位置。--observations.end() 用于获取容器中倒数第1个元素的迭代器// observations.end()指向了容器的末尾，不是最后一个元素！const StateIDType &last_cam_id = (--observations.end())->first;// 2. 分别赋值位姿Eigen::Isometry3d first_cam_pose;// Rwc---相机状态里面记录的是qcw，所以这里转R后取了转置first_cam_pose.linear() = quaternionToRotation(cam_states.find(first_cam_id)->second.orientation).transpose();// twc---相机状态原本记录即twcfirst_cam_pose.translation() = cam_states.find(first_cam_id)->second.position;Eigen::Isometry3d last_cam_pose;// Rwclast_cam_pose.linear() = quaternionToRotation(cam_states.find(last_cam_id)->second.orientation).transpose();// twclast_cam_pose.translation() = cam_states.find(last_cam_id)->second.position;// Get the direction of the feature when it is first observed.// This direction is represented in the world frame.// 这里的意思应该是:找到第一次观测到这个特征的帧，那么把这一帧作为世界系，然后根据计算求出观测的最后一帧，// 3. 求出投影射线在世界坐标系啊下的方向// 在第一帧的左相机上的归一化坐标Eigen::Vector3d feature_direction(observations.begin()->second(0),observations.begin()->second(1), 1.0);// 归一化保证了模长是1feature_direction = feature_direction / feature_direction.norm();// 转到世界坐标系下，求出了这个射线的方向feature_direction = first_cam_pose.linear() * feature_direction;// Compute the translation between the first frame// and the last frame. We assume the first frame and// the last frame will provide the largest motion to// speed up the checking process.// 4. 求出始末两帧在世界坐标系下的位移----始指向末的向量// -----就是两帧相机光心在世界系向量O1O2 = twc2-twc1Eigen::Vector3d translation = last_cam_pose.translation() - first_cam_pose.translation();     // 这里相当于两个向量点乘 这个结果等于夹角的cos值乘上位移的模// 也相当于translation 在 feature_direction上的投影// 其实就是translation在feature_direction方向上的长度double parallel_translation = translation.transpose() * feature_direction;// 这块直接理解比较抽象，使用带入法，分别带入 0° 180° 跟90°// 当两个向量的方向相同 0°，这个值是0// 两个方向相反时 180°，这个值也是0// 90°时， cos为0，也就是看translation是否足够大，后面项是0，如果translation小于阈值，也不行// 所以这块的判断即考虑了角度，同时考虑了位移。即使90°但是位移不够也不做三角化Eigen::Vector3d orthogonal_translation = translation - parallel_translation * feature_direction;if (orthogonal_translation.norm() > optimization_config.translation_threshold)return true;elsereturn false;
}

3.5 初始化Position

/*** @brief InitializePosition Intialize the feature position*    based on all current available measurements.供外部调用* @param cam_states: A map containing the camera poses with its*    ID as the associated key value.* @return The computed 3d position is used to set the position*    member variable. Note the resulted position is in world*    frame.* @return True if the estimated 3d position of the feature*    is valid.*/
inline bool initializePosition(const CamStateServer &cam_states);/*** @brief initializePosition 三角化+LM优化* @param cam_states 所有参与计算的相机位姿* @return 是否三角化成功*/
bool Feature::initializePosition(const CamStateServer &cam_states)
{// Organize camera poses and feature observations properly.// 存放每个观测以及每个对应相机的pos，注意这块是左右目独立存放std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> cam_poses(0);std::vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> measurements(0);// 1. 准备数据for (auto &m : observations){// TODO: This should be handled properly. Normally, the//    required camera states should all be available in//    the input cam_states buffer.auto cam_state_iter = cam_states.find(m.first);if (cam_state_iter == cam_states.end())continue;// Add the measurement.// 1.1 添加归一化坐标measurements.push_back(m.second.head<2>());measurements.push_back(m.second.tail<2>());// This camera pose will take a vector from this camera frame// to the world frame.// TwcEigen::Isometry3d cam0_pose;cam0_pose.linear() =quaternionToRotation(cam_state_iter->second.orientation).transpose();cam0_pose.translation() = cam_state_iter->second.position;Eigen::Isometry3d cam1_pose;cam1_pose = cam0_pose * CAMState::T_cam0_cam1.inverse();// 1.2 添加相机位姿cam_poses.push_back(cam0_pose);cam_poses.push_back(cam1_pose);}// All camera poses should be modified such that it takes a// vector from the first camera frame in the buffer to this// camera frame.// 2. 中心化位姿，提高计算精度Eigen::Isometry3d T_c0_w = cam_poses[0];for (auto &pose : cam_poses)pose = pose.inverse() * T_c0_w;// Generate initial guess// 3. 使用首末位姿粗略计算出一个三维点坐标Eigen::Vector3d initial_position(0.0, 0.0, 0.0);generateInitialGuess(cam_poses[cam_poses.size() - 1], measurements[0],measurements[measurements.size() - 1], initial_position);// 弄成逆深度形式Eigen::Vector3d solution(initial_position(0) / initial_position(2),initial_position(1) / initial_position(2),1.0 / initial_position(2));// Apply Levenberg-Marquart method to solve for the 3d position.double lambda = optimization_config.initial_damping;int inner_loop_cntr = 0;int outer_loop_cntr = 0;bool is_cost_reduced = false;double delta_norm = 0;// Compute the initial cost.// 4. 利用初计算的点计算在各个相机下的误差，作为初始误差double total_cost = 0.0;for (int i = 0; i < cam_poses.size(); ++i){double this_cost = 0.0;// 计算投影误差（归一化坐标）cost(cam_poses[i], solution, measurements[i], this_cost);total_cost += this_cost;}// Outer loop.// 5. LM优化开始， 优化三维点坐标，不优化位姿，比较简单do{// A是  J^t * J  B是 J^t * r// 可能有同学疑问自己当初学的时候是 -J^t * r// 这个无所谓，因为这里是负的更新就是正的，而这里是正的，所以更新是负的// 总之就是有一个是负的，总不能误差越来越大吧Eigen::Matrix3d A = Eigen::Matrix3d::Zero();Eigen::Vector3d b = Eigen::Vector3d::Zero();// 5.1 计算AB矩阵for (int i = 0; i < cam_poses.size(); ++i){Eigen::Matrix<double, 2, 3> J;Eigen::Vector2d r;double w;// 计算一目相机观测的雅可比与误差// J 归一化坐标误差相对于三维点的雅可比// r// w 权重，同信息矩阵jacobian(cam_poses[i], solution, measurements[i], J, r, w);// 鲁棒核约束if (w == 1){A += J.transpose() * J;b += J.transpose() * r;}else{double w_square = w * w;A += w_square * J.transpose() * J;b += w_square * J.transpose() * r;}}// Inner loop.// Solve for the delta that can reduce the total cost.// 这层是在同一个雅可比下多次迭代，如果效果不好说明需要调整阻尼因子了，因为线性化不是很好// 如果多次一直误差不下降，退出循环重新计算雅可比do{// LM算法中的lambdaEigen::Matrix3d damper = lambda * Eigen::Matrix3d::Identity();Eigen::Vector3d delta = (A + damper).ldlt().solve(b);// 更新Eigen::Vector3d new_solution = solution - delta;// 统计本次修改量的大小，如果太小表示接近目标值或者陷入局部极小值，那么就没必要继续了delta_norm = delta.norm();// 计算更新后的误差double new_cost = 0.0;for (int i = 0; i < cam_poses.size(); ++i){double this_cost = 0.0;cost(cam_poses[i], new_solution, measurements[i], this_cost);new_cost += this_cost;}// 如果更新后误差比之前小，说明确实是往好方向发展// 我们高斯牛顿的JtJ比较接近真实情况所以减少阻尼，增大步长，delta变大，加快收敛if (new_cost < total_cost){is_cost_reduced = true;solution = new_solution;total_cost = new_cost;lambda = lambda / 10 > 1e-10 ? lambda / 10 : 1e-10;}// 如果不行，那么不要这次迭代的结果// 说明高斯牛顿的JtJ不接近二阶的海森矩阵// 那么增大阻尼，减小步长，delta变小// 并且算法接近一阶的最速下降法else{is_cost_reduced = false;lambda = lambda * 10 < 1e12 ? lambda * 10 : 1e12;}} while (inner_loop_cntr++ <optimization_config.inner_loop_max_iteration &&!is_cost_reduced);inner_loop_cntr = 0;// 直到迭代次数到了或者更新量足够小了} while (outer_loop_cntr++ <optimization_config.outer_loop_max_iteration &&delta_norm > optimization_config.estimation_precision);// Covert the feature position from inverse depth// representation to its 3d coordinate.// 取出最后的结果Eigen::Vector3d final_position(solution(0) / solution(2),solution(1) / solution(2), 1.0 / solution(2));// Check if the solution is valid. Make sure the feature// is in front of every camera frame observing it.// 6. 深度验证bool is_valid_solution = true;for (const auto &pose : cam_poses){Eigen::Vector3d position =pose.linear() * final_position + pose.translation();if (position(2) <= 0){is_valid_solution = false;break;}}// 7. 更新结果// Convert the feature position to the world frame.position = T_c0_w.linear() * final_position + T_c0_w.translation();if (is_valid_solution)is_initialized = true;return is_valid_solution;
}

4 特征管理总结

4.1 特征投影

$$\begin{array}{rl}{\mathbf{z}}_{m,k}& =\mathbf{h}({\mathbf{x}}_k)+{\mathbf{n}}_k\\ & ={\mathbf{h}}_d({\mathbf{z}}_{n,k},\mathbf{\zeta })+{\mathbf{n}}_k\\ & ={\mathbf{h}}_d({\mathbf{h}}_p{(}^{C_k}{\mathbf{p}}_f),\mathbf{\zeta })+{\mathbf{n}}_k\\ & ={\mathbf{h}}_d({\mathbf{h}}_p({\mathbf{h}}_t{(}^G{\mathbf{p}}_f{,}_G^{C_k}\mathbf{R}{,}^G{\mathbf{p}}_{C_k})),\mathbf{\zeta })+{\mathbf{n}}_k\\ & ={\mathbf{h}}_d({\mathbf{h}}_p({\mathbf{h}}_t({\mathbf{h}}_r(\mathbf{\lambda },\cdots ){,}_G^{C_k}\mathbf{R}{,}^G{\mathbf{p}}_{C_k})),\mathbf{\zeta })+{\mathbf{n}}_k\end{array}$$

1. 特征参数转换为特征的全局坐标：${}^G{p}_f=h_r\left(\lambda \right)$
2. 特征的全局坐标转到观测相机坐标系：${}^{C_k}{p}_f=h_t\left({}^G{p}_f\right)$
3. 相机坐标系投影到normalize平面：$z_{n,k}=h_p\left({}^{C_k}{p}_f\right)$
4. normalize平面转到图像平面：$z_{m,k}=h_d\left(z_{n,k}\right)$

链式法则求导：
$$\frac{\partial z_{m,k}}{\partial \lambda }=\frac{\partial z_{m,k}}{\partial z_{n,k}}\frac{\partial z_{n,k}}{{\partial }^{C_k}{p}_f}\frac{{\partial }^{C_k}{p}_f}{{\partial }^G{p}_f}\frac{{\partial }^G{p}_f}{\partial \lambda }$$

关于链式法则的详细推导，参考

4.2 特征参数

从上面的特征投影就能够看出，整个过程多了特征参数转换为特征全局坐标的一个步骤

对于不同的特征参数表示$\lambda$，都先将其转换成世界系下的$XYZ$，即${}^G{p}_f=h_r\left(\lambda \right)$，再投影到图像上。这样做的好处在于${}^G{p}_f$到$z_{m,k}$的转换关系和Jacobian保持不变，只需要构造不同的$h_r\left(\lambda \right)$并推导$\frac{{\partial }^G{p}_f}{\partial \lambda }$，就能很快得到$\frac{\partial z_{m,k}}{\partial \lambda }$。

特征参数的表示类型由5种（openVINS文档）：

① Global XYZ：$^Gp_f=h_r\left(\lambda\right)=\lambda $，推出$\frac{\partial^{G}p_{f}}{\partial\lambda}=I$

② Global Inverse Depth（球坐标）：见4.2.2

③ Anchored XYZ：$\lambda ={}^{C_a}{p}_f,{}^G{p}_f=h_r\left(\lambda \right)={}_{C_a}^{G}R\lambda +{}^G{p}_{C_a}$，推出$\frac{{\partial }^G{p}_f}{\partial \lambda }{=}_{C_a}^{G}R$

④ Anchored Inverse Depth（球坐标）：见4.2.2

⑤ Anchored Inverse Depth (MSCKF Version)：见4.2.2

注意，只有球坐标逆深度可以用在全局坐标系，因为MSCKF的逆深度当z趋于0时ρ趋于无穷，其只能用在相机坐标系，只有相机系下的特征（路标点）的深度一定大于0！在全局坐标系下，它的深度并不一定大于0。

注意

​ openvins将世界系中的相机位姿拆分成了IMU位姿以及相机到IMU的外参，这样是为了分别对IMU位姿和相机外参求Jacobian。

4.2.1 基于坐标系划分

Global vs Anchored：特征点的表示是全局坐标系的坐标还是局部相机坐标系的坐标。

Anchored XYZ表示选择某个相机坐标系作为Anbchor坐标系$C_a$。对于MSCKF里面就使用了观测到当前特征的第一帧作为anchored坐标系

4.2.2 基于XYZ或者逆深度

​ XYZ即直接使用空间三维点坐标进行建模，当特征较远时， Z 数值比较大，直接估计Z会存在数值问题，逆深度优势在于能够建模无穷远点。

XYZ vs Inverse Depth：使用的XYZ还是逆深度

XYZ

对于世界系

$${}^G{p}_f=h_r\left(\lambda \right)=\lambda$$

对于局部相机坐标系

$$\lambda ={}^{C_a}{p}_f,{}^G{p}_f=h_r\left(\lambda \right)={}_{C_a}^{G}R\lambda +{}^G{p}_{C_a}$$

逆深度1：球坐标逆深度

$$\begin{array}{rl}& p_f=\left[\begin{array}{c}{x}_f\\ y_f\\ z_f\end{array}\right]=\frac{1}{\rho }\left[\begin{array}{c}\cos (\theta )\sin (\varphi )\\ \sin (\theta )\sin (\varphi )\\ \cos (\varphi )\end{array}\right]\\ & \lambda =\left[\begin{array}{c}\theta \\ \varphi \\ \rho \end{array}\right]=\left[\begin{array}{c}\arctan (y_f/x_f)\\ \arccos \left(z_f/\sqrt{x_f^2+y_f^2+z_f^2}\right)\\ 1/\sqrt{x_f^2+y_f^2+z_f^2}\end{array}\right]\\ & \frac{\partial p_f}{\partial \lambda }=[\begin{array}{ccc}-\frac{1}{\rho }\sin (\theta )\sin (\varphi )& \frac{1}{\rho }\cos (\theta )\cos (\varphi )& -\frac{1}{{\rho }^2}\cos (\theta )\sin (\varphi )\\ \frac{1}{\rho }\cos (\theta )\sin (\varphi )& \frac{1}{\rho }\sin (\theta )\cos (\varphi )& -\frac{1}{{\rho }^2}\sin (\theta )\sin (\varphi )\\ 0& -\frac{1}{\rho }\sin (\varphi )& -\frac{1}{{\rho }^2}\cos (\varphi )\end{array}]\end{array}$$

逆深度2：MSCKF逆深度

对应了MSCKF中α、β、ρ ----（x/z, y/z, 1/z）

但MSCKF逆深度有个缺点是当z→0时，ρ→∞，存在数值奇异，所以只能用在相机坐标系，因为相机系下特征点的深度都大于0。而球坐标逆深度仅在x,y,z都趋近于0时才存在数值奇异，所以能用在全局坐标系。

$$\begin{array}{c}{p}_f=\left[\begin{array}{c}{x}_f\\ y_f\\ z_f\end{array}\right]=\frac{1}{\rho }\left[\begin{array}{c}\alpha \\ \beta \\ 1\end{array}\right]\\ \lambda =\left[\begin{array}{c}\alpha \\ \beta \\ \rho \end{array}\right]=\left[\begin{array}{c}{x}_f/z_f\\ y_f/z_f\\ 1/z_f\end{array}\right]\\ \frac{\partial p_f}{\partial \lambda }=\left[\begin{array}{ccc}\frac{1}{\rho }& 0& -\frac{\alpha }{{\rho }^2}\\ 0& \frac{1}{\rho }& -\frac{\beta }{{\rho }^2}\\ 0& 0& -\frac{1}{{\rho }^2}\end{array}\right]\end{array}$$

如果是全局坐标系，那么上面的推到就是$\frac{{\partial }^G{p}_f}{\partial \lambda }$；如果是局部相机坐标系，那么我们还需要对构建局部坐标系到世界系的变换，然后进行链式法则求导（即左乘一个旋转矩阵${}_{C_a}^{G}R$）。

$$\begin{array}{rl}& {}^G{p}_f=h_r\left(\lambda \right){=}_{C_a}^G{R}^{C_a}{p}_f{+}^G{p}_{C_a}\\ & \frac{{\partial }^G{p}_f}{\partial \lambda }=\frac{{\partial }^G{p}_f}{{\partial }^{C_a}{p}_f}\frac{{\partial }^{C_a}{p}_f}{\partial \lambda }\end{array}$$

下面求导参考openvins推导

对球坐标

对msckf

4.3 MSCKF逆深度

​ MSCKF中特征参数使用了逆深度的形式，即$\mathbf{\lambda }={\left[\begin{array}{ccc}\alpha & \beta & \rho \end{array}\right]}^{\top }$。

使用逆深度

好处：能够表示无穷远点，当特征点较远时， z 数值比较大，直接估计 z 会存在数值问题

坏处：当z趋于0时，ρ趋于无穷大，存在数值奇异，只能用在相机坐标系中，因为相机系下坐标深度都是大于0的，不会趋于0；特别是在非相机坐标系中，因为在那里点的深度可以是负值或零。

4.4 VINS逆深度

对于VINS：仅仅使用了深度的逆作为特征参数，

$$\lambda =\left[\begin{array}{c}\rho \end{array}\right]$$

这对应了VINS里面的优化量：姿态、外参、逆深度

$$\left[\begin{array}{c}{p}_{b_i}^w,q_{b_i}^w\end{array}\right],\left[\begin{array}{c}{p}_{b_j}^w,q_{b_j}^w\end{array}\right],[p_c^b,q_c^b],{\lambda }_l\circ$$