二维ICP配准算法-C++ 直接法实现

1. 原理

1.1 2D的ICP算法

2D的ICP算法的原理与3D的一致，区别在于其SO(2)的自由度只有1。因此并不需要借用SVD来进行求解，只需要直接求导即可。

1.2 目标函数

$$\min D(R,T)=\frac{1}{n}\sum _{i=1}^n||P_i-(R\cdot Q_i+T)|{|}^2$$

1.3 简化目标函数

上述目标函数包含两个变量比较难以求解
可以通过对点集进行归一化，不考虑T
$$\begin{gathered} P_{mean}=\frac{1}{n}\cdot \sum _{i=1}^n{P}_{(i)},Q_{mean}=\frac{1}{n}\cdot \sum _{i=1}^n{Q}_{(i)} \\ {P}_i=Pi-P_{mean}{Q}_i=Q_i-Q_{mean} \end{gathered}$$
这时目标函数可以简化为
$$\min D(R,T)=\frac{1}{n}\sum _{i=1}^n||P_i-R\cdot Q_i|{|}^2$$
对目标函数进行展开可得
$$\min D(R,T)=\frac{1}{n}\sum _{i=1}^n(P_i^2-2P_i^{t}R{Q}_i+Q_i^2)$$
由于第一项和第三项为常量，则目标函数可以转化为
$$\max G(R,T)=\frac{1}{n}\sum _{i=1}^n{P}_i^{t}R{Q}_i$$

1.4 求解SO(2)

因SO(2)有如下定义
$$R=\left[\begin{array}{cc}cos(\theta )& -sin(\theta )\\ sin(\theta )& cos(\theta )\end{array}\right]$$
则目标函数G可以表达为如下形式
$$\begin{gathered} G(R,T)=\frac{1}{n}\sum _{i=1}^n[p_i^x,p_i^y]\left[\begin{array}{cc}cos(\theta )& -sin(\theta )\\ sin(\theta )& cos(\theta )\end{array}\right]\left[\begin{array}{c}{q}_i^x\\ q_i^y]\end{array}\right]= \\ \frac{1}{n}\sum _{i=1}^{n}cos(\theta )\cdot (p_i^x{q}_i^x+p_i^y{q}_i^y)+sin(\theta )\cdot (p_i^y{q}_i^x-p_i^x{q}_i^y) \end{gathered}$$
对$\theta$求导,令导数等于0
$$0=\frac{1}{n}\sum _{i=1}^n-sin(\theta )\cdot (p_i^x{q}_i^x+p_i^y{q}_i^y)+cos(\theta )\cdot (p_i^y{q}_i^x-p_i^x{q}_i^y)$$
移项得
$$\begin{gathered} tan(\theta )=\frac{\frac{1}{n}{\sum }_{i=1}^n{p}_i^y{q}_i^x-p_i^x{q}_i^y}{\frac{1}{n}{\sum }_{i=1}^n{p}_i^x{q}_i^x+p_i^y{q}_i^y}=a \\ \theta =atan(a) \end{gathered}$$
求出$\theta$,根据SO(2)的形式就可求出R

1.5求解T

$$T=P_{mean}-R\cdot Q_{mean}$$

2. C++实现

// ICP2D.h
#ifndef ICP2D_H_
#define ICP2D_H_

#include <opencv2/opencv.hpp>

class ICP2D
{
public:
    ICP2D() = default;
    ~ICP2D() = default;
    
    // ICP配准
    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);
    
    // 点集去中心化
    cv::Point2f normalize_(std::vector<cv::Point2f> &points);
};

#endif

// ICP2D.cc
#include "ICP2D.h"

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

    auto source_mean = normalize_(source_);

    cv::Matx22f R = cv::Matx22f::eye();
    double res_angle = 0.f;
    cv::Point2f T(0.f, 0.f);

    for (int i = 0; i < iter_num; ++i)
    {
        auto target_mean = normalize_(target_);

        double t_nume = 0.f, t_deno = 0.f;

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

            t_nume += source_[closet_index].y * target_[j].x - source_[closet_index].x * target_[j].y;
            t_deno += source_[closet_index].x * target_[j].x + source_[closet_index].y * target_[j].y;
        }

        double theta = std::atan2(t_nume, t_deno);
        res_angle += theta / CV_PI * 180.f;

        cv::Matx22f R_iter;
        R_iter << std::cos(theta), -std::sin(theta),
            std::sin(theta), std::cos(theta);

        cv::Point2f T_iter = source_mean - R_iter * target_mean;

        for (int j = 0; j < target_.size(); ++j)
        {
            target_[j] = R_iter * target_[j] + T_iter;
        }

        R = R_iter * R;
        T = R_iter * T + T_iter;

        std::cout << "R iter: " << theta / CV_PI * 180.f << std::endl;
        std::cout << "T iter: " << T_iter << std::endl;

        if (std::abs(theta / CV_PI * 180.f) < eps)
        {
            break;
        }
    }

    return res_angle;
}

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

cv::Point2f ICP2D::normalize_(std::vector<cv::Point2f> &points)
{
    cv::Point2f mean(0.f, 0.f);
    for (const auto &p : points)
    {
        mean += p;
    }

    mean.x /= points.size();
    mean.y /= points.size();

    for (auto &p : points)
    {
        p -= mean;
    }

    return mean;
}

3. 性能测试

// test.cc
#include <opencv2/opencv.hpp>
#include "ICP2D.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 / run_num << " ms" << std::endl;
}

void test_icp()
{
    ICP2D icp;
    std::vector<cv::Point2f> points = {{1, 1}, {2, 2}, {3, 3}};
    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.001);
    std::cout << "angle final: " << angle_final << std::endl;
}

int main()
{
    evaluate_and_call(test_icp, 10);

    return 0;
}

/* Terminal print
R iter: -30
T iter: [-18.6603, -12.3205]
R iter: 1.36604e-05
T iter: [20.6602, 14.3205]
angle final: -30
run cost: 1.1759 ms
*/

• 笔者在项目中只用其求解角度，所以registration函数的返回值只有角度，且收敛条件设置为$\Delta$R足够小