對3d座標和四元數做線性插值計算c++實現

功能介紹

設位姿點$P$的向量爲$(t,x,y,z,qx,qy,qz,qw)$,
其中 $(t)$代表時間戳，$(x,y,z)$代表$3d$位置座標，$(qx,qy,qz,qw)$代表姿態四元數，
所要求的是按照指定的時間間隔，在兩個位姿點$P_1,P_2$之間做線性插值計算，插值包括$3d$位置插值和四元數插值兩個部分。

數據源 data.pose 如下

     1586747345.016527891 330676.282541113 3357811.940754859 0.010018029 0.007638817 -0.011094164 -0.002806535 0.999905341   
     1586747345.116585016 330676.362531093 3357811.833798940 0.006964320 0.007859637 -0.011136416 0.006601951 0.999885304   
     1586747345.216607094 330676.446372186 3357811.728910104 0.006729199 0.007697294 -0.012205158 0.015607504 0.999774070   
     1586747345.316587925 330676.532460048 3357811.625497681 0.007449332 0.007413975 -0.013260547 0.024201023 0.999591667   
     1586747345.416523218 330676.620261295 3357811.523244015 0.007768851 0.007214256 -0.014193941 0.032397247 0.999348240   
     1586747345.516441107 330676.708745489 3357811.421607558 0.007307768 0.007162372 -0.014664308 0.040147410 0.999060481   
     1586747345.616352081 330676.798857846 3357811.321017583 0.006542085 0.007244681 -0.014875008 0.047409182 0.998738513   
     1586747345.716264009 330676.890037565 3357811.221070574 0.005610467 0.007316182 -0.015100238 0.054259991 0.998385852   
     1586747345.816192150 330676.981991540 3357811.120774384 0.005154748 0.007306883 -0.015472638 0.060730675 0.998007511   
     1586747345.916131258 330677.074755255 3357811.020436957 0.005439263 0.007023194 -0.015447904 0.066805610 0.997621696

c++ 代碼如下

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


using namespace std;
using namespace Eigen;
int main(int argc, char** argv)
{

    
    int ch;
    while ((ch = getopt(argc, argv, "hp:")) != -1){
       switch (ch) {
            case 'h':
                std::cout<<"eg.:\n  echo \"p0(x y z qx qy qz qw) p1(x y z qx qy qz qw) omega\" |./interpolate \n";
                return 0;
            break;
            default:
            break;
        }
    }

    //input local pose (x,y,z)
    std::string line;
    int i=0;
    cout << setprecision(12)<<std::fixed;
    while(getline(cin,line)){
        std::stringstream ss(line);
	    double p0x,p0y,p0z,qr0x,qr0y,qr0z,qr0w;
	    double p1x,p1y,p1z,qr1x,qr1y,qr1z,qr1w;
	    double omega;
        ss>>p0x >>p0y >>p0z >>qr0x >>qr0y >>qr0z >>qr0w;
        ss>>p1x >>p1y >>p1z >>qr1x >>qr1y >>qr1z >>qr1w;
        ss>>omega;

 		Eigen::Translation3d p0Trans(Eigen::Vector3d(p0x, p0y, p0z));
 		Eigen::Quaterniond p0Quat(qr0w, qr0x, qr0y, qr0z);

 		Eigen::Translation3d p1Trans(Eigen::Vector3d(p1x, p1y, p1z));
 		Eigen::Quaterniond p1Quat(qr1w, qr1x, qr1y, qr1z);

 		Eigen::Quaterniond slerp_quat = p0Quat.slerp(1 - omega, p1Quat);

 		Eigen::Translation3d interpTrans;
        interpTrans.x() = p1Trans.x() * omega + p0Trans.x() * (1 - omega);
        interpTrans.y() = p1Trans.y() * omega + p0Trans.y() * (1 - omega);
        interpTrans.z() = p1Trans.z() * omega + p0Trans.z() * (1 - omega);

        std::cout << interpTrans.x()<<" "<<interpTrans.y()<<" "<<interpTrans.z()<<" "
        		  << slerp_quat.x() <<" "<< slerp_quat.y() <<" "<< slerp_quat.z() <<" "<< slerp_quat.w() 
        		  << std::endl;

    }
}

CMakeLists.txt

cmake_minimum_required( VERSION 2.8   )
project( interpolate   )
set( CMAKE_BUILD_TYPE "Debug"  )
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
include_directories( "./include" )
#include_directories( "/usr/local/include/eigen3"  )
find_package(Eigen3 REQUIRED)
include_directories(${EIGEN3_INCLUDE_DIRS})
add_executable( interpolate interpolate.cc  )