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_NEW
    typedef 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);

    // 特征id
    FeatureIDType id;

    // id for next feature
    static 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.
        // Twc
        Eigen::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];        // Twc0
    for (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算法中的lambda
            Eigen::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/z
    const double &beta = x(1);  // y/z
    const 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倍，缩小他的权重
    else
        w = 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. 取出对应的始末帧id
    const 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---相机状态原本记录即twc
    first_cam_pose.translation() = cam_states.find(first_cam_id)->second.position;

    Eigen::Isometry3d last_cam_pose;
    // Rwc
    last_cam_pose.linear() = quaternionToRotation(cam_states.find(last_cam_id)->second.orientation).transpose();
    // twc
    last_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);
    // 归一化保证了模长是1
    feature_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-twc1
    Eigen::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;
    else
        return 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.
        // Twc
        Eigen::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算法中的lambda
            Eigen::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$$