RPP算法-RobustPlanarPose-C++

【位姿估计】RPP算法-RobustPlanarPose-C++ 是1.1.2版本的，不过并不是vs项目的，是linux下的项目，包含Makefile文件，可以直接在linux下运行。可以自行修改为vs项目，除了OpenCV没有其他依赖项~~~~

#include 
#include 
#include “RPP.h“

using namespace cv;

inline double SQ（double x）
{
    return x*x;
}

int main（int argc char **argv）
{
    // Data from the original Matlab code we‘ll use it for ground truth. Verify the results with the Matlab output.
    // First row is x
    // Second row is y
    double model_data[20] = {0.0685 0.6383 0.4558 0.7411 -0.7219 0.7081 0.7061 0.2887 -0.9521 -0.2553
                             0.4636 0.0159 -0.1010 0.2817 0.6638 0.1582 0.3925 -0.7954 0.6965 -0.7795};


    double iprts_data[20] = {-0.0168 0.0377 0.0277 0.0373 -0.0824 0.0386 0.0317 0.0360 -0.1015 -0.0080
                              0.0866 0.1179 0.1233 0.1035  0.0667 0.1102 0.0969 0.1660  0.0622 0.1608};


    // Results from Matlab/Octave
    // R is rotation and t is translation you should get VERY similar results or even numerically exact
    /*
      R =

     0.85763  -0.31179   0.40898
     0.16047  -0.59331  -0.78882
     0.48859   0.74214  -0.45881

    t =
     -0.10825
      1.26601
     11.19855
    */

    Mat model = Mat::zeros（3 10 CV_64F）; // 3D points z is zero
    Mat iprts = Mat::ones（3 10 CV_64F）; // 2D points homogenous points
    Mat rotation;
    Mat translation;
    int iterations;
    double obj_err;
    double img_err;

    for（int i=0; i < 10; i++） {
        model.at（0i） = model_data[i];
        model.at（1i） = model_data[i+10];

        iprts.at（0i） = iprts_data[i];
        iprts.at（1i） = iprts_data[i+10];
    }

    if（!RPP::Rpp（model iprts rotation translation iterations obj_err img_err）） {
        fprintf（stderr “Error with RPP\n“）;
        return 1;
    }

	printf（“Input normalised image points （using 3x3 camera intrinsic matrix）:\n“）;
	for（int i=0; i < 10; i++） {
		printf（“%d: （%g %g）\n“ i iprts_data[i*2] iprts_data[i*2+1]）;
	}

	printf（“\n“）;
	printf（“Input model points （fixed 3D points chosen by the user z is fixed to 0.0）:\n“）;
	for（int i=0; i < 10; i++） {
		printf（“%d: （%g %g 0.0）\n“ i model_data[i*2] model_data[i*2+1]）;
	}

	printf（“\n“）;
	printf（“Pose found by RPP\n“）;

	printf（“\n“）;
    printf（“Rotation matrix\n“）;
    RPP::Print（rotation）;

    printf（“Translation matrix\n“）;
    RPP::Print（translation）;

    printf（“Number of iterations: %d\n“ iterations）;
    printf（“object error: %f\n“ obj_err）;
    printf（“Image error: %f\n“ img_err）;

    // Compare against ground truth
    double rot_err = 0.0;
    double tran_err = 0.0;

    // Using fabs instead of sum square in case the signs are opposite
    rot_err += SQ（rotation.at（00） -  0.85763）;
    rot_err += SQ（rotation.at（01） - -0.31179）;
    rot_err += SQ（rotation.at（02） - 0.40898）;
    rot_err += SQ（rotation.at（10） - 0.16047）;
    rot_err += SQ（rotation.at（11） - -0.59331）;
    rot_err += SQ（rotation.at（12） - -0.78882）;
    rot_err += SQ（rotation.at（20） - 0.

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     文件         660  2012-03-12 11:57  RobustPlanarPose\ChangeLog.txt
     文件        3759  2012-03-12 11:52  RobustPlanarPose\demo.cpp
     文件        1702  2011-08-11 13:02  RobustPlanarPose\LICENSE.txt
     文件         275  2011-03-24 10:57  RobustPlanarPose\Makefile
     文件        1034  2011-12-07 07:58  RobustPlanarPose\RobustPlanarPose.cbp
     文件         515  2011-12-07 08:23  RobustPlanarPose\RobustPlanarPose.depend
     文件         652  2011-12-07 12:00  RobustPlanarPose\RobustPlanarPose.layout
     文件       28995  2011-03-24 11:20  RobustPlanarPose\Rpoly.cpp
     文件        3146  2011-03-24 11:20  RobustPlanarPose\Rpoly.h
     文件       28884  2012-03-12 11:52  RobustPlanarPose\RPP.cpp
     文件        5299  2011-12-07 08:07  RobustPlanarPose\RPP.h