0
点赞
收藏
分享

微信扫一扫

SLAM高翔视频第二讲萝卜习题

产品喵dandan米娜 2022-04-04 阅读 17

个人认为第二讲主要是要理解时间坐标和相机坐标的转换、位姿的含义。

题目如下图:

1.有一号和二号的世界到相机的变换关系,就可以得到它们分别的变换矩阵T1、T2 。

2.有了小萝卜在一号相机坐标系的位置,要求在二号相机坐标的位置,首先要知道小萝卜的世界坐标的位置Pw,就可以通过公式P2=T2*Pw

3.Pw=T1的逆*p1,代入即可得到结果P2

以上是解题思路,代码如下

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

#include <Eigen/Core>
#include <Eigen/Dense> //
int main() {
Eigen::Vector3d t1,t2,p1;
t1 << 0.7,1.1,0.2;
t2 << -0.1,0.4,0.8;
p1 << 0.5,-0.1,0.2;

Eigen::Quaterniond q1 =Eigen::Quaterniond(0.55,0.3,0.2,0.2).normalized();
Eigen::Quaterniond q2 =Eigen::Quaterniond (-0.1,0.3,-0.7,0.2).normalized();

Eigen::Isometry3d T1 =Eigen::Isometry3d::Identity();
T1.rotate(q1);
T1.pretranslate(t1);
cout<<"T1= \n"<<T1.matrix()<<endl;
Eigen::Isometry3d T2 =Eigen::Isometry3d::Identity();
T2.rotate(q2);
T2.pretranslate(t2);
cout<<"T2= \n"<<T1.matrix()<<endl;

Eigen::Vector3d P2=T2*T1.inverse()*p1;
cout<<"p2=: \n"<<P2<<endl;
return 0;

}

刚开始include<eigen>时会报错,要在cmakelist里面find_package

Cmakelist代码为

cmake_minimum_required(VERSION 3.21)
project(luobo)

set(CMAKE_CXX_STANDARD 14)

find_package(Eigen3 REQUIRED)
include_directories("/usr/include/eigen3")

add_executable(luobo main.cpp)

再会main就不会有红线报错了。

结果为

p2=: 
 1.08228
0.663509
0.686957

举报

相关推荐

0 条评论