LM算法原理推导 + C++实现(2D ICP配准算法)

1. 原理

1.1 2D的ICP算法原理

算法的基本原理介绍见前文 二维ICP配准算法-C++ 直接法实现

1.2 LM算法

1.2.1 增量方程推导

LM算法与高斯牛顿法的主要区别，是其在泰勒展开的近似目标函数上增加了一个惩罚项
$$\begin{gathered} \underset{\Delta x}{\mathrm{argmin}}M(\Delta x)=\frac{1}{2}||f(x_n)+J(x_n{)}^T\Delta x|{|}^2+\frac{1}{2}\mu {\Delta x}^T\Delta x \\ =\frac{1}{2}(f(x_n)+J(x_n{)}^T\Delta x{)}^T(f(x_n)+J(x_n{)}^T\Delta x)+\frac{1}{2}\mu \Delta x^T\Delta x \\ =\frac{1}{2}(||f(x_n)|{|}^2+2f(x_n)J(x_n{)}^T\Delta x+\Delta x^{T}J(x_n)J(x_n{)}^T\Delta x+\mu \Delta x^T\Delta x) \end{gathered}$$
对上述目标函数求导，并令其导数=0，得：
$$\begin{gathered} f(x_n)J(x_n{)}^T+J(x_n)J(x_n{)}^T\Delta x+\mu \Delta x=0 \\ J(x_n)J(x_n{)}^T\Delta x+\mu \Delta x=-f(x_n)J(x_n{)}^T \end{gathered}$$
令$J(x_n)J(x_n{)}^T$为H, $-f(x_n)J(x_n{)}^T$为g，简化后得
$$(H+\mu I)\Delta x=g$$
从如上公式可以看出当$\mu$比较小的时候H占据主导地位，此时LM算法接近高斯牛顿算法；当$\mu$比较大的时候$\mu I$占据主导，此时更接近最速下降法(一阶梯度下降法)。因此LM算法能够在一定程度上解决H奇异和病态的问题。但由于阻尼的存在，其收敛速度可能会比高斯牛顿更慢。

1.2.2 阻尼系数调节

上述公式中的$\mu$可以通过如下规则调节，首先定义增益率$\rho$
$$\rho =\frac{F(x_n)-F(x_n+\Delta x)}{M(x_n)-M(x_n+\Delta x)}$$
增益率$\rho$表达了一阶泰勒展开近似M($\Delta x$)与真实函数F($\Delta x$)的近似程度

• $\rho$越接近于1，说明近似程度越高
• 分子越大，$\rho$越大，说明此时一阶近似函数相比真实函数下降的慢，应该减小阻尼$\mu$，使其接近高斯牛顿，提升收敛速度
• 分母越大，$\rho$越小，说明此时一阶近似函数相比真实函数下降的快，距离最优点远，应该增大阻尼$\mu$，使其接近最速下降算法

如果 $\rho$ > 0:
$$\mu =\mu \times \min (\frac{1}{3},1-(2\rho -1{)}^3);v=2$$
否则：
$$\mu =v\times \mu ;v=2\times v$$

1.2.3 增益率$\rho$的计算

1. 分子
   $$cos{t}_{real}=||F(x_n)|{|}^2-||F(x_n+\Delta x)|{|}^2$$
   是真实函数误差的下降值。
2. 分母
   $$cos{t}_{approximate}=||M(x_n)|{|}^2-||M(x_n+\Delta x)|{|}^2$$
   当更新前$\Delta$x为0的时候，$M(x_n)=f(x_n)$;
   当更新后$\Delta$x非常小的时候,$M(x+\Delta x)$可以用$f(x_n)$一阶近似代替
   $$M(x_n+\Delta x)=f(x_n)+J(x_n{)}^T\Delta x$$
   故：
   $$\begin{gathered} cos{t}_{approximate}=||f(x_n)|{|}^2-||f(x_n)+J(x_n{)}^T\Delta x|{|}^2 \\ =||f(x_n)|{|}^2-(f(x_n)+J(x_n{)}^T\Delta x{)}^T(f(x_n)+J(x_n{)}^T\Delta x) \\ =||f(x_n)|{|}^2-(||f(x_n)|{|}^2+2J(x_n{)}^T\Delta xf(x_n)+\Delta x^{T}J(x_n)J(x_n{)}^T\Delta x) \\ =-2J(x_n{)}^T\Delta xf(x_n)-\Delta x^{T}J(x_n)J(x_n{)}^T\Delta x \end{gathered}$$
   由$J(x_n)J(x_n{)}^T=H$, $-f(x_n)J(x_n{)}^T=g$得：
   $$cos{t}_{approximate}=\Delta x^T(2g-(H+\mu I-\mu I)\Delta x)$$
   又由于$H+\mu I=g$,得：
   $$cos{t}_{approximate}=\Delta x^T(2g-(g-\mu I)\Delta x)=\Delta x^T(g+uI\Delta x)$$

1.3 目标函数和雅可比矩阵推导

LM算法又称阻尼牛顿法，因此其所用的目标函数和雅可比矩阵与前文一致二维ICP配准算法-C++ 高斯牛顿实现，这里不再过多介绍。

2. C++实现

2.1 函数声明

#ifndef ICP2d_LM_H
#define ICP2d_LM_H

#include <opencv2/opencv.hpp>
#include <Eigen/Dense>

class ICP2DLM
{
public:
    ICP2DLM() = default;
    ~ICP2DLM() = default;

    double registration(const std::vector<cv::Point2f> &source,
                        const std::vector<cv::Point2f> &target,
                        const int &iter_num, const double &eps);

private:
    int bfnn_point_(const std::vector<cv::Point2f> &points,
                    const cv::Point2f &point);

    double calculate_H_g_(const std::vector<cv::Point2f> &source,
                          const std::vector<cv::Point2f> &target,
                          const double &theta,
                          Eigen::Matrix3d &H, Eigen::Vector3d &g);
};

#endif

2.2 暴力搜索最近邻

int ICP2DLM::bfnn_point_(const std::vector<cv::Point2f> &points,
                         const cv::Point2f &point)
{
    return std::min_element(points.begin(), points.end(),
                            [&point](const cv::Point2f &p1, const cv::Point2f &p2)
                            { return cv::norm(p1 - point) < cv::norm(p2 - point); }) -
           points.begin();
}

2.3 C++17多线程计算H和g

double ICP2DLM::calculate_H_g_(const std::vector<cv::Point2f> &source,
                               const std::vector<cv::Point2f> &target,
                               const double &theta,
                               Eigen::Matrix3d &H, Eigen::Vector3d &g)
{
    H = Eigen::Matrix3d::Zero();
    g = Eigen::Vector3d::Zero();
    double cost = 0.f;

    // C++ 17 multi thread process
    std::for_each(std::execution::par_unseq, target.begin(), target.end(),
                  [&](const cv::Point2f &p)
                  {
                      int nn_idx = bfnn_point_(source, p);
                      Eigen::Matrix<double, 3, 2> J;
                      J << -1, 0, 0, -1,
                          std::sin(theta) * p.x + std::cos(theta) * p.y,
                          std::sin(theta) * p.y - std::cos(theta) * p.x;
                      H += J * J.transpose();

                      Eigen::Vector2d e(source[nn_idx].x - p.x,
                                        source[nn_idx].y - p.y);
                      g += -J * e;
                      cost += e.dot(e);
                  });

    return cost;
}

2.4 LM算法优化SE(2)

double ICP2DLM::registration(const std::vector<cv::Point2f> &source,
                             const std::vector<cv::Point2f> &target,
                             const int &iter_num, const double &eps)
{
    auto target_ = target;

    double theta = 0.f;
    cv::Point2f T(0.f, 0.f);

    double cost = 0.0;
    double v = 2.f;
    double rho = 0.f;

    // Initialize
    double tao = 1e-10;
    Eigen::Matrix3d H;
    Eigen::Vector3d g;
    cost = calculate_H_g_(source, target_, theta, H, g);
    double u = tao * H.maxCoeff();
    Eigen::Matrix3d I = Eigen::Matrix3d::Identity();

    // Start iterate
    for (int i = 0; i < iter_num; ++i)
    {
        Eigen::Matrix3d H_lm = H + u * I; // LM core
        Eigen::Vector3d dx = H_lm.ldlt().solve(g);

        // converg by R
        if (std::abs(dx[2] / CV_PI * 180.f) < eps)
        {
            break;
        }

        // Update SE(2)
        T.x += dx[0];
        T.y += dx[1];
        theta += dx[2];

        // Update point cloud
        cv::Matx22f R_dx;
        R_dx << std::cos(dx[2]), -std::sin(dx[2]),
            std::sin(dx[2]), std::cos(dx[2]);
        std::transform(target_.begin(), target_.end(), target_.begin(),
                       [=](cv::Point2f &p)
                       { return R_dx * p + cv::Point2f(dx[0], dx[1]); });

        // Calculate new cost, rho
        double cost_new = 0.f;
        for (int j = 0; j < target_.size(); ++j)
        {
            int nn_idx = bfnn_point_(source, target_[j]);
            Eigen::Vector2d e(source[nn_idx].x - target_[j].x,
                              source[nn_idx].y - target_[j].y);
            cost_new += e.dot(e);
        }
        rho = (cost - cost_new) / (dx.transpose() * (u * dx + g));

        // LM core
        if (rho > 0)
        {
            cost = calculate_H_g_(source, target_, theta, H, g);

            u = u * std::max(0.3333333, (1 - (2 * rho - 1) * (2 * rho - 1) * (2 * rho - 1)));
            v = 2;
        }
        else
        {
            u = u * v;
            v = 2 * v;
        }

        std::cout << std::to_string(i + 1) << " iter R: " << dx[2] / CV_PI * 180.f << std::endl;
    }

    return theta / CV_PI * 180.f;
}

3 测试代码

#include <opencv2/opencv.hpp>
#include "icp2d_lm.h"

template <typename FuncT>
void evaluate_and_call(FuncT &&func, int run_num)
{
    double total_time = 0.f;
    for (int i = 0; i < run_num; ++i)
    {
        auto t1 = std::chrono::high_resolution_clock::now();
        func();
        auto t2 = std::chrono::high_resolution_clock::now();
        total_time +=  std::chrono::duration_cast<std::chrono::duration<double>>(t2 - t1).count() * 1000;;
    }

    std::cout << "run cost: " << total_time / 1000.f << std::endl;
}

void test_icp_lm()
{
    ICP2DLM icp;

    std::vector<cv::Point2f> points = {{100, 100}, {200, 200}, {300, 300}};
    cv::Matx22f R;
    double angle = 30.f;
    R << std::cos(angle / 180.f * CV_PI), -std::sin(angle / 180.f * CV_PI),
        std::sin(angle / 180.f * CV_PI), std::cos(angle / 180.f * CV_PI);
    cv::Point2f T(10, 20);

    std::vector<cv::Point2f> points_RT;
    for (int i = 0; i < points.size(); ++i)
    {
        points_RT.push_back(R * points[i] + T);
    }

    double angle_final = icp.registration(points, points_RT, 100, 0.0001);
    std::cout << "angle final: " << angle_final << std::endl;
}


int main()
{
    evaluate_and_call(test_icp_lm, 1);

    return 0;
}

4 运行结果

1 iter R: -28.6477
2 iter R: -1.19418
3 iter R: -0.137277
4 iter R: -0.0180692
5 iter R: -0.00241103
6 iter R: -0.000322292
angle final: -29.9999
run cost: 0.0021303

• 本文只用其计算角度，因此收敛条件设置的为$\Delta R$足够小