二维ICP配准算法-C++ 高斯牛顿实现

给算法爸爸调试

已于 2024-07-05 17:30:46 修改

阅读量1.3k

点赞数 18

文章标签：自动驾驶图像处理人工智能

于 2024-07-04 14:10:34 首次发布

本文链接：https://blog.youkuaiyun.com/qq_52157273/article/details/140034983

版权

二维ICP配准算法-C++ 高斯牛顿实现

1 原理
2 C++实现
3 测试

1 原理

1.1 2D的ICP算法的基本原理

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

1.2 目标函数

$\min F(R,T) = \frac{1}{n} \sum_{i=1}^n||f(R,T)||^2 \\ f(R,T) =P_i-(R\cdot Q_i+T)$

1.3 Gauss-Newton 雅可比矩阵

高斯牛顿法采用f(R,T)的一阶泰勒展开式代替f(R,T)。因此注意此处的雅可比矩阵应该为f(R, T)的，而非F(R, T)的，如果为F(R, T)的话就变成牛顿法了。
根据如下SE(2)的定义
$R=\left[ \begin{matrix} cos(\theta) & -sin(\theta) & t_x \\ sin(\theta) & cos(\theta) & t_y \\ 0 & 0 & 1 \end{matrix} \right]$
f(R,T)可以表达成如下形式
$f(R,T)=f(t_x,t_y,\theta)= \left[ \begin{matrix} P_i^x \\ P_i^y \\ 1 \end{matrix} \right] - \left[ \begin{matrix} cos(\theta) & -sin(\theta) & t_x \\ sin(\theta) & cos(\theta) & t_y \\ 0 & 0 & 1 \end{matrix} \right] \left[ \begin{matrix} Q_i^x \\ Q_i^y \\ 1 \end{matrix} \right] \\ =\left[ \begin{matrix} P_i^x - cos(\theta) Q_i^x + sin(\theta)Q_i^y -t_x \\ P_i^y - sin(\theta) Q_i^x - cos(\theta)Q_i^y -t_y \\ 1 \end{matrix} \right]$
对上述公式求导可得雅可比矩阵
$J=\left[ \begin{matrix} -1 & 0 \\ 0 & -1 \\ sin(\theta) Q_i^x + cos(\theta)Q_i^y & - cos(\theta) Q_i^x + sin(\theta)Q_i^y \end{matrix} \right]$

2 C++实现

// icp2d_gn.h
#ifndef ICP2D_GN_H
#define ICP2D_GN_H
#include <opencv2/opencv.hpp>

class ICP2DGN
{
public:
    ICP2DGN() = default;
    ~ICP2DGN() = 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);
};

#endif

// icp2d_gn.cc
#include "icp2d_gn.h"
#include <Eigen/Dense>

double ICP2DGN::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);

    for (int i = 0; i < iter_num; ++i)
    {
        Eigen::Matrix3d H = Eigen::Matrix3d::Zero();
        Eigen::Vector3d g = Eigen::Vector3d::Zero();

        for (int j = 0; j < target_.size(); ++j)
        {
            int nn_idx = bfnn_point_(source, target_[j]);

            Eigen::Matrix<double, 3, 2> J;
            J << -1, 0, 0, -1,
                std::sin(theta) * target_[j].x + std::cos(theta) * target_[j].y,
                std::sin(theta) * target_[j].y - std::cos(theta) * target_[j].x;

            H += J * J.transpose();

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

        Eigen::Vector3d dx = H.ldlt().solve(g);

        T.x += dx[0];
        T.y += dx[1];
        theta += dx[2];

        // Update target points
        cv::Matx22f R_dx;
        R_dx << std::cos(dx[2]), -std::sin(dx[2]),
            std::sin(dx[2]), std::cos(dx[2]);
        for (int j = 0; j < target_.size(); ++j)
        {
            target_[j] = R_dx * target_[j] + cv::Point2f(dx[0], dx[1]);
        }

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

        std::cout << "iter R: " << dx[2] / CV_PI * 180.f << std::endl;
    }

    return theta / CV_PI * 180.f;
}

int ICP2DGN::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();
}

3 测试

#include <opencv2/opencv.hpp>
#include "icp2d_gn.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_gn()
{
    ICP2DGN 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_gn, 1);

    return 0;
}

/* Terminal print
iter R: -28.6479
iter R: -1.19412
iter R: -0.137148
iter R: -0.0180555
iter R: -0.00240684
iter R: -0.000322292
angle final: -30
run cost: 0.0013694
*/