平面点云匹配姿态计算

平面点云匹配姿态计算

本来想写个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;
}