*把T265定点过程中的坐标变换彻底弄清楚（包含vision_to_mavros代码分析）px4_command T265定点过程的坐标系转换也理清

把T265定点过程中的坐标变换彻底弄清楚

 

但是我看普罗米修斯的px4_pos_estimator.cpp的代码感觉里面没有做坐标变换

https://gitee.com/maxibooksiyi/Prometheus/blob/master/Modules/control/src/px4_pos_estimator.cpp

 

 

那我去板载计算机上打印看看 /t265/odom/sample  和  /mavros/vision_pose/pose  看看经过px4_pos_estimate之后有什么变化？

 

实际我看了下似乎真的不需要坐标变换，下面左边是/t265/odom/sample  右边是 /mavros/vision_pose/pose  ，原始的odom信息似乎就已经符合东北天坐标系了！！！！！莫非是realsense2-camera已经考虑到这一点了？确实px4_pos_estimator.cpp里我看代码也没有做任何的坐标变换。

（下面这个图的数据是我再上电后把无人机向前移动了几厘米）

 

（下面的数据是我再上电后把无人机向前移动了几厘米，向左移动了一段距离，可以看到数据都是符合东北天坐标系的）

 

 

我顺带把/tf消息也打出来了，也是符合东北天坐标系的。所以不需要坐标系变换啊，难道那个APM教程或者vision_to_mavros说错了。你可以再去分析一下PX4给的VIO的代码，里面是不是也没有做任何坐标系变换。

那等于也就是实质做了个消息类型转变，px4_pos_estimator.cpp在T265定点中就起了这么一个作用。把nav_msgs::Odometry类型的消息转为geometry_msgs::PoseStamped类型的消息，并改个名字，好让MAVROS接收。

至少可以确定，T265向前（摄像头前方）是X变大，向左是Y变大，向上是Z变大，

 

 

那我去回看一下我之前做APMT265树莓派无人机的时候的录屏（之前的录屏似乎没有移动无人机，所以看不出来），看看那个时候打印的/tf消息和打印的/mavros/vison_pose/pose消息是不是就是一样的，你当然还可以进一步看一下vision_to_mavros的源码，看看它到底有没有做坐标系变换。

 

我看了下vision_to_mavros的代码似乎也是没有做坐标系变换的，似乎就是做了个消息类型的转变，如它的注释所说  Create PoseStamped message to be sent

https://github.com/thien94/vision_to_mavros/blob/master/src/vision_to_mavros.cpp

vision_to_mavros它是监听tf消息（所以没有写subscribe订阅，似乎/tf不是用来订阅的？），然后发布geometry_msgs::PoseStamped类型的vision_pose

vision_to_mavros中间这么一大坨都是用来打印终端消息的。

 

不过我好像在vision_to_mavros里面看到了有做坐标系转换。

 

 

 

普罗米修斯跑T265定点时的节点图

 

px4_command跑T265定点时的节点图

 

 

下面是跑px4_command时打印的消息，最左边是/tf (vision_to_mavros接收的）    中间/visionPose（vision_to_mavros发布的，也是px4_pos_extimate订阅的）   最右边是/mavros/vision_pose/pose（px4_pos_extimate发布的）

这里对比可以看到vision_to_mavros确实做了坐标系变换，但是px4_pos_extimate又将其变回去了。可能原本我的APM树莓派T265默认正方向朝北，这里默认正方向朝东，所以阿木的这个就实际不需要多T265输出的原始位置数据做坐标系变换，是不是这个道理。确实第二个/visionPose特就是经过vision_to_mavros变换过后的，就变成y是朝向机头的了，在东北天坐标系下就是朝北，这个就是符合我的APMT265树莓派无人机的，所以那个时候我每次起飞机头要朝向正北方向，这样才符合现实的磁场角度。因为当时我无人机飞控用的现实的磁场，如果对不上，飞机自身发送期望指令就会有问题，飞机实际偏向一个方向，但是无人机认为偏向另一个方向，再负反馈控制就会有问题，这个细节我之前分析过，后面有时间再写一遍。

之前有个弄错了，阿木应该是没有改vision_to_mavros的核心代码的，虽然他们代码行数少些好像删去的是一些打印终端消息的。

之前我说阿木改了vision_to_mavros是在这篇博文里面，我现在去改过来 https://blog.csdn.net/sinat_16643223/article/details/113139495

下面的位置是我把无人机向前移动几厘米

现在再看vision_to_mavros这里注释的第一行，说是变成y向前知道什么意思了吧。它发布的/visionPose就是这样！！！！！我的APMT265树莓派无人机就是这样。

对了找找vision_to_mavros是在哪里写的发布/mavros/vision_pose/pose的

 

再看下面的坐标系的图

 

 

对于机体是朝前，对于世界是朝北。也就是x是朝北的，现在看明白那个图了。
对于世界就是东北天坐标系。
机体的x和世界的y对应上
这就是那句话的意思。

 

之所以这么写，因为对于机体或者摄像头而言，是没有北的概念的，只有前，而对于世界坐标系而言，是东北天坐标系，是没有前的概念的，只有东北天。

划红线的地方的意思就是 机体的x和世界的y对应上

 

我现在非常彻底清除为什么上面的坐标系图那么画了！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！之前的东西都对得上了。

 

我之前的APMT265树莓派无人机默认机头也就是摄像头前方朝北，而阿木的无人机默认机头方向也就是摄像头方向朝东，这就是区别，所以为什么我需要用vision_to_mavros，而他们不需要做坐标系转换。

 

 

现在还有两个地方没弄懂，vision_to_mavros是在哪里发布的/mavros/vision_pose/pose消息，我只看到发布/visionPose消息。第二个是px4_command的px4_pos_extimate讲道理是对订阅的视觉位置数据做了坐标系变换了，为什么我没有看到呢。

又发布，我弄错了，只是阿木的怎么把消息变成/visionPose了呢，难道他们改了名字，我去看看板载计算机上的代码，是的，确实就是改了vision_to_mavros.cpp发布消息的名称。

看了下阿木的px4_command里面的vision_to_mavros确实修改了发布的消息名称，和原版的vision_to_mavros相比。

 

 

节点名称的改名和发布消息的改名原来是放在Launch文件里面改的。这下面是官方vision_to_mavros的，这也可以明白为什么看节点图看到的节点名称是t265_to_mavros而不是vision_to_mavros。也可以明白为什么代码里写发布消息只写了/vision_pose，而不是/mavros/vision_pose/pose了。阿木应该是在launch文件里面把他们vision_to_mavros节点发布的消息最后改为/visionPose，这样再看上面px4_command的T265定点的节点图就清楚了。这么来讲阿木对vision_to_mavros还是有些改动的，只是坐标系变换部分没有改动。（launch文件里面可以改发布的话题消息名称，但是后来发现阿木是在vision_to_mavros.cpp里面改的，由于P200的TX2上有两个vision_to_mavros.cpp，我之前找错了。）

当然上面的分析可以让我们很好理解，我之前做APMT265树莓派的时候，为什么vision_to_mavros发布出来的消息是/mavros/vision_pose/pose，而代码里面写的发布消息的名称却是/vision_pose，因为在lunach文件中修改了。这是不是侧面可以体现出，消息的名称是可以随便改的，消息的本质类型就是那样，名字可以随便取，所以普罗米修斯里面就把/camera改为/t265了，是这样吧。

后来看了下，原来这种改名就是所说的重映射！！！！之前看到重映射以为是什么很高级的东西，现在才明白。<remap>

而且可能是ros里面的基础操作，普罗米修斯把/camera改为/t265也是基础操作，应该为了防止重名，并不是什么为了防止别人拿去能直接用。。。。

 

 

 

px4_command中的px4_pos_estimate.cpp里面对视觉位置数据做坐标系变换的地方找到了，虽然上面有个备注时上面vicon，但是实际那些变量pos_drone_vio就是来自于视觉位置数据的回调函数里面的！！！！！！那么现在OK了。我知道px4_command在T265定点的时候怎么做坐标系变换的了。再看上面那三个话题消息的打印，都可以解释清楚了。对vision_to_mavros的坐标转换也清楚了，也清楚为什么普罗米修斯在T265定点的时候不需要做坐标转换了。是的，都清楚了。

https://github.com/amov-lab/px4_command/blob/master/src/px4_pos_estimator.cpp

 

 

 

 

 

目前就剩最后一个问题 在px4_command定点的时候，阿木在哪里把vision_to_mavros发布的话题消息/vision_xyz改为/visionPose的，应该是在t265_tf_to_mavros.launch这个launch文件里面，但是我目前还没有找到明确写了改了这个的。

这个问题我找到答案了，他们就是在vision_to_mavros.cpp里面改的话题消息名称，只是我之前找到另一个vision_to_mavros.cpp去了，他们实际用的这个vision_to_mavros.cpp，所以launch文件里面的改不改无所谓。那么现在最后一个问题也彻底解决了！！！！！！！！！！！！！！！！！

 

 

现在可能能真正自己写个T265定点了，就做个消息类型转换。

昨天本身对消息也有了进一步认识。

就像肖昆自己写个transfer.py确实不难。

 

 

关于阿木为什么默认机头朝东好好想想，可能东北天，x就是东，这样方便发布指点坐标，好看些。要是机头朝北确实也不方便。

或者px4 有没有规定？应该没有。
 

 

 

 

 

 

 

我发现vision_to_mavros这一段话你还没有真正理解，根本可能是对TF没有理解。（结合这篇博文来看 https://blog.csdn.net/sinat_16643223/article/details/113889533 ）

 结合它下面的解释可能就好些，就是从相机坐标系到世界坐标系的变换，你就可以理解为相机相对于世界的位姿。就像下面cartographer也是发布的/tf，意思就是机器人相对地图的位姿，不就是位置么。感觉就是一个意思，换种表达方式。

https://blog.csdn.net/xiekaikaibing/article/details/80118134

 

 

而且我还看到local_position/pose还可以以TF的类型发出去

t265节点为什么要发布TF消息？

 

还有我发现普罗米修斯里面px4_pos_eatimate阅激光雷达的也是订阅的/tf，这里面应该有一定道理。可能都是用/tf来表示的

https://gitee.com/maxibooksiyi/Prometheus/blob/master/Modules/control/src/px4_pos_estimator.cpp

 https://blog.csdn.net/qq_38649880/article/details/88372390

https://blog.csdn.net/xiekaikaibing/article/details/80118134