平面点云匹配姿态计算
本来想写个ICP的,发现用不着。。
对于已经对应好的点云,直接使用第一点与中心点进行变换姿态估计就可以了;
如果想精度更准确些,那就依次加入第二点,第三点….直到达到最终误差限;
#include "stdafx.h"
#include <eigen/dense>
#include <iostream>
#define M_T M_PI / 180
bool getRT(Eigen::MatrixXf src0, Eigen::MatrixXf src1, Eigen::MatrixXf &Rr, Eigen::MatrixXf &Tr, float &error)
{
Rr = Eigen::MatrixXf::Identity(2, 2);
Tr = Eigen::MatrixXf::Zero(2, 1);
Eigen::MatrixXf center0 = Eigen::MatrixXf::Zero(2, 1);
Eigen::MatrixXf center1 = Eigen::MatrixXf::Zero(2, 1);
Eigen::MatrixXf t0 = Eigen::MatrixXf(2, 1);
Eigen::MatrixXf t1 = Eigen::MatrixXf(2, 1);
float theta = 0;
center0(0, 0) = src0.col(0).sum() / src0.rows();
center0(1, 0) = src0.col(1).sum() / src0.rows();
center1(0, 0) = src1.col(0).sum() / src1.rows();
center1(1, 0) = src1.col(1).sum() / src1.rows();
t0 = center0 - src0.row(0).transpose();
t1 = center1 - src1.row(0).transpose();
theta = acosf((t0(0, 0) * t1(0, 0) + t0(1, 0) * t1(1, 0)) / (t0.norm() * t1.norm()));
Rr << cosf(theta), sinf(theta), -sinf(theta), cosf(theta);
Rr = Rr.inverse();
src1 = src1 * Rr;
center0(0, 0) = src0.col(0).sum() / src0.rows();
center0(1, 0) = src0.col(1).sum() / src0.rows();
center1(0, 0) = src1.col(0).sum() / src1.rows();
center1(1, 0) = src1.col(1).sum() / src1.rows();
Tr = center0 - center1;
error = 0;
src1.col(0) = src1.col(0).array() + Tr(0, 0);
src1.col(1) = src1.col(1).array() + Tr(1, 0);
for (int i = 0; i < src0.rows(); ++i)
{
error += (src0.row(i) - src1.row(i)).norm();
}
return true;
}
int _tmain(int argc, _TCHAR* argv[])
{
int r = 10;
int c = 10;
int N = r * c;
float theta = 20.0;
float sx = 10;
float sy = 20;
Eigen::MatrixXf src0 = Eigen::MatrixXf::Zero(N, 2);
Eigen::MatrixXf src1 = Eigen::MatrixXf::Zero(N, 2);
for (int i = 0; i < N; ++i)
{
src0(i, 0) = i / c;
src0(i, 1) = i % c;
}
Eigen::MatrixXf R = Eigen::MatrixXf::Identity(2, 2);
R << cosf(theta * M_T), sinf(theta * M_T),
-sinf(theta * M_T), cosf(theta * M_T);
src1 = src0 * R;
src1.col(0) = src1.col(0).array() + sx;
src1.col(1) = src1.col(1).array() + sy;
std::cout << src0 << "\n\n";
std::cout << src1 << "\n\n";
Eigen::MatrixXf Rr, Tr;
float error;
getRT(src0, src1, Rr, Tr, error);
std::cout << error << "\n\n";
src1 = (src1 * Rr);
src1.col(0) = src1.col(0).array() + Tr(0, 0);
src1.col(1) = src1.col(1).array() + Tr(1, 0);
std::cout << src1 << "\n\n";
getchar();
return 0;
}