使用 levenberg-marquardt 优化 欧式空间中的三维点变换关系

1. 前言

最近涉及到一部分将原先matlab的计算机视觉的相关代码转化成C++实现，其中，有一段是关于空间点变换的。简单来讲，已知欧式空间中的一组空间点W1，在经过某个欧式变换RT之后，得到另一组空间点W2，现在在已知W1，W2的前提下，我们需要求解对应的RT。

2. 基本思路

其实这个过程是非常简单的，显然，W2=RT∗W1， 于是， RT=W2∗pinv(W1), 然而，由于实际过程中存在数据噪声，因而求解得到的RT 可能是不符合实际物理要求的，这就需要涉及到对RT 的优化了。

3. matlab 代码

3.1 main

首先生成一组对应的空间点，计算得到他们的变换关系rt 的初值， 然后使用LM优化得到符合物理意义的RT

clear all
clc
close all
warning off

%% generate w3d points   w2(4xn) = rtx(4x4) * w1(4xn)
[w3d1, w3d2, rtx] = generateW3DPoints(0.01);

rtx_c = w3d2 * pinv(w3d1);

x0 = getRESFromRt(rtx_c);
options=optimset('LargeScale','off','Display','iter','TolFun',1e-30,...
'TolX',1e-35,'Algorithm','levenberg-marquardt','MaxFunEvals',1e+100,'MaxIter',1000);
fun = @(x)RTx_youhua(x, w3d1, w3d2);
[x, fval] = lsqnonlin(fun, x0, [], [], options);

disp(fval)
rtx_f = getRtFromRES(x)
rtx

w3d1_t = w3d1';
w3d2_t = w3d2';
w3d1_t(:, 4) = 1:size(w3d1_t, 1);
w3d2_t(:, 4) = 1:size(w3d2_t, 1);
save('w3d1.txt', 'w3d1_t', '-ascii', '-double')
save('w3d2.txt', 'w3d2_t', '-ascii', '-double')

3.2 generateW3DPoints

使用meshgrid生成空间点, noise_level 是误差水平的参数

function [w3d1, w3d2, rtx] = generateW3DPoints(noise_level)
[X, Y, Z] = meshgrid(1:10, 1:10, 1:2);
w3d1 = [X(:)'; Y(:)'; Z(:)'; ones(1, length(X(:)))];
figure, plot3(w3d1(1, :), w3d1(2, :), w3d1(3, :), '*')

rtx = [getRtFromRES([0.1, 0.2, 0.3, 10, 20, 30]); 0 0 0 1 ];
w3d2 = rtx * w3d1;

w3d2 = w3d2 + noise_level * addnoise(w3d2);
figure, plot3(w3d2(1, :), w3d2(2, :), w3d2(3, :), '*')
end

% @author       :       zhyh2010
% @date         :       20150806
% @output       :
% @description  :       add noise

function [noise_new] = addnoise(x1)
noise = rand(size(x1, 1), size(x1, 2));
noise_mean = mean(noise, 2);
noise_std = std(noise, 1, 2);
noise_new = (noise - repmat(noise_mean, [1 size(x1, 2)])) ./ repmat(noise_std, [1 size(x1, 2)]);
noise_new_mean = mean(noise_new , 2);
noise_new_std = std(noise_new, 1, 2);
assert(all(abs(noise_new_mean) < 1e-6), 'noise did not normalized!!!')
assert(all(abs(noise_new_std - 1) < 1e-6), 'noise did not normalized!!!')
end

3.3 getRtFromRES

使用欧拉角对 RT 进行描述

function [ Rt ] = getRtFromRES( res )
x = res(1);
y = res(2);
z = res(3);
t1 = res(4);
t2 = res(5);
t3 = res(6);

Rt = [cos(y)*cos(z)-sin(x)*sin(y)*sin(z), -cos(x)*sin(z), sin(y)*cos(z)+sin(x)*cos(y)*sin(z), t1
cos(y)*sin(z)+sin(x)*sin(y)*cos(z), cos(x)*cos(z), sin(y)*sin(z)-sin(x)*cos(y)*cos(z), t2
-cos(x)*sin(y), sin(x), cos(x)*cos(y), t3];
end

3.4 RTx_youhua

优化的目标方程， 最小化变换前后的三维点之间的误差minarg||W2−RT∗W1||

function [err] = RTx_youhua(x, w1, w2)
rtx = [getRtFromRES(x); 0 0 0 1 ];
w2_c = rtx * w1;
err = abs(w2_c - w2);
end

3.5 getRESFromRt

function [ res,err ] = getRESFromRt( Rt )
a = real(asin(Rt(3,2)));
b = real(asin(-Rt(3,1)/cos(a)));
c = real(asin(-Rt(1,2)/cos(a)));

res = [a,b,c,Rt(1:3,end)'];
fun = @(res)ce(Rt,res);
[res , ~] = lsqnonlin(fun, res, [], [], optimset('LargeScale','off','Display','off','TolFun',1e-30,...
'TolX',1e-35,'Algorithm','levenberg-marquardt','MaxFunEvals',1e+100,'MaxIter',1000));
err = ce(Rt,res);

end

function err = ce(Rt,res)
x = res(1);
y = res(2);
z = res(3);
t1 = res(4);
t2 = res(5);
t3 = res(6);

Rt2 = [cos(y)*cos(z)-sin(x)*sin(y)*sin(z) -cos(x)*sin(z) sin(y)*cos(z)+sin(x)*cos(y)*sin(z) t1
cos(y)*sin(z)+sin(x)*sin(y)*cos(z) cos(x)*cos(z) sin(y)*sin(z)-sin(x)*cos(y)*cos(z) t2
-cos(x)*sin(y) sin(x) cos(x)*cos(y) t3];

err = Rt2-Rt(1:3, :);
end

4. C++ 版本代码

4.1 项目工程代码

https://code.csdn.net/zhyh1435589631/calcguangbiballcenter/tree/master

4.2 LM优化部分

其实这部分代码中最核心的就是这个LM算法了https://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm

对照着上面维基百科中的描述，我们可以给出如下实现代码，当然也可以选择使用现有的实现框架等

// levenberg-marquardt法
void CComputeBallCenterFrom2W3dPoints::LM_RTxFrom2W3dPoints(const cv::Mat & w3d1, const cv::Mat & w3d2, cv::Mat & res, double err_thresh){
Mat err, Jacobi, step, err_0, Jacobi_pinv, step_len;
int num = 0;
double uk = 0.15;
double v = 5;
Mat J1;
double err_last_loop = 99999;
do{
getErrAndJacobiForNewton(w3d1, w3d2, res, err, Jacobi);

J1 = Jacobi.t()*Jacobi + uk*Mat::eye(Size(Jacobi.cols, Jacobi.cols), CV_64F);
invert(J1, Jacobi_pinv, DECOMP_SVD);
Jacobi_pinv = Jacobi_pinv*Jacobi.t();
step = Jacobi_pinv*err;
reduce(abs(err), err_0, 0, CV_REDUCE_AVG);
if (err_0.at<double>(0, 0) < err_last_loop){
uk = uk / v;
}
else{
uk = uk * v;
}
err_last_loop = err_0.at<double>(0, 0);

reduce(abs(step), step_len, 0, CV_REDUCE_AVG);
cout << step_len.at<double>(0, 0) << endl;
if (step_len.at<double>(0, 0) < err_thresh){
break;
}
res = res - step.t();
} while (++num < 500);
}

理论值是【0.1， 0.2， 0.3， 10， 20， 30】

由于加入了0.01mm的物点误差，

相应的matlab版本给出的结果为：