二维ICP配准算法-C++ LM算法实现
1. 原理
1.1 2D的ICP算法原理
算法的基本原理介绍见前文 二维ICP配准算法-C++ 直接法实现
1.2 LM算法
1.2.1 增量方程推导
LM算法与高斯牛顿法的主要区别,是其在泰勒展开的近似目标函数上增加了一个惩罚项
arg min
Δ
x
M
(
Δ
x
)
=
1
2
∣
∣
f
(
x
n
)
+
J
(
x
n
)
T
Δ
x
∣
∣
2
+
1
2
μ
Δ
x
T
Δ
x
=
1
2
(
f
(
x
n
)
+
J
(
x
n
)
T
Δ
x
)
T
(
f
(
x
n
)
+
J
(
x
n
)
T
Δ
x
)
+
1
2
μ
Δ
x
T
Δ
x
=
1
2
(
∣
∣
f
(
x
n
)
∣
∣
2
+
2
f
(
x
n
)
J
(
x
n
)
T
Δ
x
+
Δ
x
T
J
(
x
n
)
J
(
x
n
)
T
Δ
x
+
μ
Δ
x
T
Δ
x
)
\argmin_{\Delta x} 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^TJ(x_n)J(x_n)^T\Delta x + \mu \Delta x^T \Delta x)
ΔxargminM(Δx)=21∣∣f(xn)+J(xn)TΔx∣∣2+21μΔxTΔx=21(f(xn)+J(xn)TΔx)T(f(xn)+J(xn)TΔx)+21μΔxTΔx=21(∣∣f(xn)∣∣2+2f(xn)J(xn)TΔx+ΔxTJ(xn)J(xn)TΔx+μΔxTΔx)
对上述目标函数求导,并令其导数=0,得:
f
(
x
n
)
J
(
x
n
)
T
+
J
(
x
n
)
J
(
x
n
)
T
Δ
x
+
μ
Δ
x
=
0
J
(
x
n
)
J
(
x
n
)
T
Δ
x
+
μ
Δ
x
=
−
f
(
x
n
)
J
(
x
n
)
T
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
f(xn)J(xn)T+J(xn)J(xn)TΔx+μΔx=0J(xn)J(xn)TΔx+μΔx=−f(xn)J(xn)T
令
J
(
x
n
)
J
(
x
n
)
T
J(x_n)J(x_n)^T
J(xn)J(xn)T为H,
−
f
(
x
n
)
J
(
x
n
)
T
-f(x_n)J(x_n)^T
−f(xn)J(xn)T为g,简化后得
(
H
+
μ
I
)
Δ
x
=
g
(H+\mu I)\Delta x = g
(H+μI)Δx=g
从如上公式可以看出当
μ
\mu
μ比较小的时候H占据主导地位,此时LM算法接近高斯牛顿算法;当
μ
\mu
μ比较大的时候
μ
I
\mu I
μI占据主导,此时更接近最速下降法(一阶梯度下降法)。因此LM算法能够在一定程度上解决H奇异和病态的问题。但由于阻尼的存在,其收敛速度可能会比高斯牛顿更慢。
1.2.2 阻尼系数调节
上述公式中的
μ
\mu
μ可以通过如下规则调节,首先定义增益率
ρ
\rho
ρ
ρ
=
F
(
x
n
)
−
F
(
x
n
+
Δ
x
)
M
(
x
n
)
−
M
(
x
n
+
Δ
x
)
\rho = \frac{F(x_n)-F(x_n+\Delta x)}{M(x_n)-M(x_n+\Delta x)}
ρ=M(xn)−M(xn+Δx)F(xn)−F(xn+Δx)
增益率
ρ
\rho
ρ表达了一阶泰勒展开近似M(
Δ
x
\Delta x
Δx)与真实函数F(
Δ
x
\Delta x
Δx)的近似程度
- ρ \rho ρ越接近于1,说明近似程度越高
- 分子越大, ρ \rho ρ越大,说明此时一阶近似函数相比真实函数下降的慢,应该减小阻尼 μ \mu μ,使其接近高斯牛顿,提升收敛速度
- 分母越大, ρ \rho ρ越小,说明此时一阶近似函数相比真实函数下降的快,距离最优点远,应该增大阻尼 μ \mu μ,使其接近最速下降算法
如果
ρ
\rho
ρ > 0:
μ
=
μ
×
min
(
1
3
,
1
−
(
2
ρ
−
1
)
3
)
;
v
=
2
\mu=\mu \times \min(\frac{1}{3},1-(2\rho-1)^3) ;v=2
μ=μ×min(31,1−(2ρ−1)3);v=2
否则:
μ
=
v
×
μ
;
v
=
2
×
v
\mu=v\times \mu;v=2\times v
μ=v×μ;v=2×v
1.2.3 增益率 ρ \rho ρ的计算
- 分子
c o s t r e a l = ∣ ∣ F ( x n ) ∣ ∣ 2 − ∣ ∣ F ( x n + Δ x ) ∣ ∣ 2 cost_{real}=||F(x_n)||^2-||F(x_n+\Delta x)||^2 costreal=∣∣F(xn)∣∣2−∣∣F(xn+Δx)∣∣2
是真实函数误差的下降值。 - 分母
c o s t a p p r o x i m a t e = ∣ ∣ M ( x n ) ∣ ∣ 2 − ∣ ∣ M ( x n + Δ x ) ∣ ∣ 2 cost_{approximate}=||M(x_n)||^2-||M(x_n+\Delta x)||^2 costapproximate=∣∣M(xn)∣∣2−∣∣M(xn+Δx)∣∣2
当更新前 Δ \Delta Δx为0的时候, M ( x n ) = f ( x n ) M(x_n)=f(x_n) M(xn)=f(xn);
当更新后 Δ \Delta Δx非常小的时候, M ( x + Δ x ) M(x+\Delta x) M(x+Δx)可以用 f ( x n ) f(x_n) f(xn)一阶近似代替
M ( x n + Δ x ) = f ( x n ) + J ( x n ) T Δ x M(x_n+\Delta x)=f(x_n)+J(x_n)^T\Delta x M(xn+Δx)=f(xn)+J(xn)TΔx
故:
c o s t a p p r o x i m a t e = ∣ ∣ f ( x n ) ∣ ∣ 2 − ∣ ∣ f ( x n ) + J ( x n ) T Δ x ∣ ∣ 2 = ∣ ∣ f ( x n ) ∣ ∣ 2 − ( f ( x n ) + J ( x n ) T Δ x ) T ( f ( x n ) + J ( x n ) T Δ x ) = ∣ ∣ f ( x n ) ∣ ∣ 2 − ( ∣ ∣ f ( x n ) ∣ ∣ 2 + 2 J ( x n ) T Δ x f ( x n ) + Δ x T J ( x n ) J ( x n ) T Δ x ) = − 2 J ( x n ) T Δ x f ( x n ) − Δ x T J ( x n ) J ( x n ) T Δ x cost_{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^TJ(x_n)J(x_n)^T\Delta x) \\ = -2J(x_n)^T\Delta xf(x_n)-\Delta x^TJ(x_n)J(x_n)^T\Delta x costapproximate=∣∣f(xn)∣∣2−∣∣f(xn)+J(xn)TΔx∣∣2=∣∣f(xn)∣∣2−(f(xn)+J(xn)TΔx)T(f(xn)+J(xn)TΔx)=∣∣f(xn)∣∣2−(∣∣f(xn)∣∣2+2J(xn)TΔxf(xn)+ΔxTJ(xn)J(xn)TΔx)=−2J(xn)TΔxf(xn)−ΔxTJ(xn)J(xn)TΔx
由 J ( x n ) J ( x n ) T = H J(x_n)J(x_n)^T=H J(xn)J(xn)T=H, − f ( x n ) J ( x n ) T = g -f(x_n)J(x_n)^T=g −f(xn)J(xn)T=g得:
c o s t a p p r o x i m a t e = Δ x T ( 2 g − ( H + μ I − μ I ) Δ x ) cost_{approximate}=\Delta x^T(2g-(H+\mu I-\mu I)\Delta x) costapproximate=ΔxT(2g−(H+μI−μI)Δx)
又由于 H + μ I = g H+\mu I=g H+μI=g,得:
c o s t a p p r o x i m a t e = Δ x T ( 2 g − ( g − μ I ) Δ x ) = Δ x T ( g + u I Δ x ) cost_{approximate}=\Delta x^T(2g-(g-\mu I)\Delta x) = \Delta x^T(g+uI\Delta x) costapproximate=ΔxT(2g−(g−μI)Δx)=ΔxT(g+uIΔ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
- 本文只用其计算角度,因此收敛条件设置的为 Δ R \Delta R ΔR足够小