标签：mat 答案 相机 tran lis 坐标系 cmak col main
设有?萝?1?号和?萝??号位于世界坐标系中。
?萝??号的位姿为：q1 = [0.55, 0.3, 0.2, 0.2], t1 = [0.7, 1.1, 0.2]T（q 的第?项为实部）。这?的 q 和 t 表达的是 Tcw，也就是世界到相机的变换关系。
?萝? ?号的位姿为 q2 = [?0.1, 0.3, ?0.7, 0.2], t2 = [?0.1, 0.4, 0.8]T。
现在，?萝??号看到某个点在??的坐 标系下，坐标为 p1 = [0.5, ?0.1, 0.2]T，求该向量在?萝??号坐标系下的坐标。
请编程实现此事，并提交 你的程序。
下面是我的程序：hw3_3.cpp
#include <iostream> #include <Eigen/Core> #include <Eigen/Geometry> #include <cmath> using namespace std; int main() { /*1、先求出p点在世界坐标系下的坐标pw=Twc*pc1=Tcw`*pc1 *先根据q1和t1，得到Twc(四元数使用之前要归一化) *再求出Tcw，再代入公式得到Pw */ Eigen::Quaterniond q1(0.55,0.3,0.2,0.2); Eigen::Vector3d t1; t1<<0.7,1.1,0.2; Eigen::Vector3d pc1(0.5,-0.1,0.2); // 变换矩阵T用 Eigen::Isometry Eigen::Isometry3d Twc1=Eigen::Isometry3d::Identity(); // 虽然称为3d，实质上是4＊4的矩阵 Twc1.rotate ( q1.normalized() ); // 按照q1进行旋转 Twc1.pretranslate ( t1 ); // 按照t1进行平移 // 用变换矩阵进行坐标变换 Eigen::Vector3d pw = Twc1.inverse()*pc1; // 相当于R*v+t //2、再求出pw在小萝卜二号坐标系下的坐标pc2=Tcw*pw Eigen::Quaterniond q2(-0.1,0.3,-0.7,0.2); Eigen::Vector3d t2; t2<<-0.1,0.4,0.8; // 变换矩阵T用 Eigen::Isometry Eigen::Isometry3d Tcw2= Eigen::Isometry3d::Identity(); // 虽然称为3d，实质上是4＊4的矩阵 Tcw2.rotate ( q2.normalized() ); // 按照q2进行旋转 Tcw2.pretranslate (t2 ); // 按照t2进行平移 Eigen::Vector3d pc2=Tcw2*pw; cout << "pc2= " <<pc2.transpose()<<endl; return 0; }
然后它对应的CMakeLists.txt文件对应为：
cmake_minimum_required( VERSION 2.8 ) project( hw3 ) set( CMAKE_BUILD_TYPE "Release" ) set( CMAKE_CXX_FLAGS "-O3" ) # 添加Eigen头文件 include_directories( "/usr/include/eigen3" ) add_executable( hw3_3 hw3_3.cpp )
然后cmake make得到答案是：
pc2= 1.08228 0.663509 0.686957
标签：mat 答案 相机 tran lis 坐标系 cmak col main
原文地址：https://www.cnblogs.com/wongyi/p/8777953.html
