Rodrigues' fomula 使用Sophus庫與自己編寫程序的消耗時間對比

廢話不多說,直接上代碼.相機位姿用6維李代數表示.發現自己寫的代碼比用Sophus會快很多.

#include <iostream>
#include <cmath>
#include <ctime>
using namespace std; 

#include <Eigen/Core>
#include <Eigen/Geometry>

#include "sophus/so3.h"
#include "sophus/se3.h"

   //test for compute time

    Eigen::Matrix<double,6,1> camera;
    camera(0) = 1.5741515942940262e-02;
    camera(1) = -1.2790936163850642e-02;
    camera(2) = -4.4008498081980789e-03;
    camera(3) = -3.4093839577186584e-02;
    camera(4) = -1.0751387104921525e-01;
    camera(5) = 1.1202240291236032e+00;
    Eigen::Vector3d pw(1.0,2.0,3.0);
    Eigen::Vector4d pw_h(1.0,2.0,3.0,1);

    clock_t s_time,e_time;
    s_time = clock();
    Eigen::Vector3d v3d(camera(0),camera(1),camera(2));
    Eigen::Vector3d m_t(camera(3),camera(4),camera(5));
    Sophus::SO3 m_so3 = Sophus::SO3::exp(v3d);
    Sophus::SE3 m_se3(m_so3,m_t);
    Eigen::Vector4d p_c = m_se3.matrix()*pw_h;
    cout<<"result of using  Sophus lib : "<<p_c(0)<<" "<<p_c(1)<<" "<<p_c(2)<<endl;

    e_time = clock() -s_time;
    cout<<"time used for Sphus is "<<e_time<<" ms"<<endl;

    //compute by my own code
    s_time = clock();
    double p[3];
    const double theta2 = camera(0)*camera(0)+ camera(1)* camera(1)+camera(2)*camera(2);
    if (theta2 > double(std::numeric_limits<double>::epsilon())) {

        const double theta = sqrt(theta2);
        const double costheta = cos(theta);
        const double sintheta = sin(theta);
        const double theta_inverse = 1.0 / theta;

        const double w[3] = { camera[0] * theta_inverse,

                              camera[1] * theta_inverse,
                              camera[2] * theta_inverse };

        double w_cross_pt[3];

        w_cross_pt[0] = w[1] * pw[2] - w[2] * pw[1];
        w_cross_pt[1] = w[2] * pw[0] - w[0] * pw[2];
        w_cross_pt[2] = w[0] * pw[1] - w[1] * pw[0];


        const double tmp = (w[0]*pw(0)+ w[1]* pw(1)+w[2]*pw(2)) * (double(1.0) - costheta);
        //    (w[0] * pt[0] + w[1] * pt[1] + w[2] * pt[2]) * (T(1.0) - costheta);

        p[0] = pw[0] * costheta + w_cross_pt[0] * sintheta + w[0] * tmp;
        p[1] = pw[1] * costheta + w_cross_pt[1] * sintheta + w[1] * tmp;
        p[2] = pw[2] * costheta + w_cross_pt[2] * sintheta + w[2] * tmp;
    } else {

        double w_cross_pt[3];
        //CrossProduct(angle_axis, pt, w_cross_pt);

        w_cross_pt[0] = camera[1] * pw[2] - camera[2] * pw[1];
        w_cross_pt[1] = camera[2] * pw[0] - camera[0] * pw[2];
        w_cross_pt[2] = camera[0] * pw[1] - camera[1] * pw[0];
        p[0] = pw[0] + w_cross_pt[0];
        p[1] = pw[1] + w_cross_pt[1];
        p[2] = pw[2] + w_cross_pt[2];
    }
    p[0] += m_t[0]; p[1] += m_t[1]; p[2] += m_t[2];
    cout<<"result of using my code is "<<p[0]<<" "<<p[1]<<" "<<p[2]<<endl;
    e_time = clock() -s_time;
    cout<<"time used for my code is "<<e_time<<" ms"<<endl;
    return 0;

}

結果如下:

result of using  Sophus lib : 0.93594 1.84058 4.1639
time used for Sphus is 59 ms
result of using my code is 0.93594 1.84058 4.1639
time used for my code is 2 ms

可以看到計算結果是一樣的,但是速度提高到了28倍.