求座標轉換矩陣之奇異值分解法c++實現

功能說明

已知
處於不同座標系下的兩個座標點集$A$和$B$,
$a_i\in R^2$, $b_i \in R^2$
並且滿足以下條件$T\cdot P_{ai}=P_{bi}$
求
轉換矩陣 $T$

c++實現

數據文件 sample.xy
前兩列爲集合$A$的座標點$(x_{ai},y_{ai})$,
後兩列爲集合$B$的座標點$(x_{bi},y_{bi})$

-0.0252359227429 -0.0476232909431 330674.269915 3357812.427190
-0.0407901595465 0.083445758589 330674.275657 3357812.416351
-0.0609007045329 0.214495280716 330674.281479 3357812.405183
-0.083873836225 0.345326492203 330674.287303 3357812.393994
-0.10907351247 0.476001609724 330674.293129 3357812.382816
-0.135304033057 0.606518110971 330674.298937 3357812.371967
-0.163586705944 0.736986275985 330674.304782 3357812.360789
-0.193198426386 0.867483556297 330674.310639 3357812.349578
-0.223337949791 0.99869281889 330674.316531 3357812.338344
-0.254186255713 1.13033062908 330674.322378 3357812.327396

源碼文件 convert_frame.cc

#include <iostream>
#include "Eigen/SVD"
#include <Eigen/Core>
#include <Eigen/Dense>
#include <sstream>
#include <string>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>

using namespace std;
using namespace Eigen;

int main(int argc, char** argv)
{

    int pointsNum = 0;
    int ch;
    while ((ch = getopt(argc, argv, "c:")) != -1){
       switch (ch) {
            case 'h':
                std::cout<<"eg.:\n  cat sample.xy |./convert_frame -c 10 \n";
                return 0;
            break;
            case 'c': 
                pointsNum = atoi(optarg);
            break;
            default:
            break;
        }
    }

    int mRow = pointsNum *2;
    MatrixXf m(mRow,4); 
    VectorXf rhs(mRow);

    std::string line;
    int i=0;
    while(getline(cin,line)){
        std::stringstream ss(line);
        double ax,ay,bx,by;
        ss>>ax;
        ss>>ay;
        ss>>bx;
        ss>>by;
        m(i,0) = ax;
        m(i,1) = -ay;
        m(i,2) = 1;
        m(i,3) = 0;

        m(i+1,0) = ay;
        m(i+1,1) = ax;
        m(i+1,2) = 0;
        m(i+1,3) = 1;

        rhs(i) = bx;
        rhs(i+1) = by;
        i += 2;
    }
    JacobiSVD<MatrixXf> svd(m, ComputeThinU | ComputeThinV);
    VectorXf x(4);
    x = svd.solve(rhs);

    MatrixXf A(3,3);
    A(0,0) = x(0);
    A(0,1) = -x(1);
    A(0,2) = x(2);
    A(1,0) = x(1);
    A(1,1) = x(0);
    A(1,2) = x(3);
    A(2,0) = 0;
    A(2,1) = 0;
    A(2,2) = 1;
    
    cout << "transform matrix is:"<<endl << A <<endl;
    return 0;
    
}

CMakeLists.txt

cmake_minimum_required( VERSION 2.8   )
project( convert   )
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
include_directories( "./include" )
find_package(Eigen3 REQUIRED)
include_directories(${EIGEN3_INCLUDE_DIRS})
add_executable( convert_frame convert_frame.cc   )