八、ROS小车闭环控制：将DSO算法得到相机位姿信息作为ROS节点广播

DSO算法鼎鼎大名，不用多说。

一般的相机都能用（我用Kinect V2，现在想想，只说DSO的话，它并不是必须的，一般的相机只要能标定就行），CSDN都有相关的教程，我这里说一下如何将DSO算法得到相机位姿信息作为ROS节点广播。

一句话来说，就是请参考这个链接里面我的回答，也就是二楼：https://github.com/rancheng/dso_ros/issues/1

后续我有时间会把相关完整代码放出来，我的描述相信找到这里的人基本也能看懂。站在前人的肩膀上。

附：我的回复

Here is a brief description of what I've done:

1. Use any DSO copy you like(the original is best), just remember:
   (1)sampleoutput=1 may be different in some Rosrun command example, be careful;
   (2)referring to the DSO_main of this author, add the function ' getCameraPose()' correctly, this is all we need for the main DSO!

2. For the ROS client, we don't really need to write the wrapper like in this project, we can:
   (1)refer to this project: https://github.com/BluewhaleRobot/dso_ros , then add what is relevant to ' getCameraPose()' like in this project, pay attention to headers:
   ` std::vector wraps = fullSystem->outputWrapper;
   if (wraps.size() != 0) {
   std::cout << frameID << std::endl;
   IOWrap::Output3DWrapper *wrap = wraps.back();
   std::vector *poses = wrap->getCameraPose();
   if (poses->size() != 0) {
   SE3 pose = poses->back();
   std::cout << "----------Pose Size: " << poses->size() << "--------"
   << std::endl;
   // std::cout << "-----------------------Camera
   // Pose!!!!!----------------------" << std::endl;
   std::cout << pose.matrix3x4() << std::endl;
   std::cout << "----------1" << std::endl;
   auto rt_matrix = pose.matrix3x4();
   geometry_msgs::Transform dso_est_tf;

   dso_est_tf.translation.x = rt_matrix(0, 3);
   dso_est_tf.translation.y = rt_matrix(1, 3);
   dso_est_tf.translation.z = rt_matrix(2, 3);
   dso_est_tf.rotation.x = pose.unit_quaternion().x();
   dso_est_tf.rotation.y = pose.unit_quaternion().y();
   dso_est_tf.rotation.z = pose.unit_quaternion().z();
   dso_est_tf.rotation.w = pose.unit_quaternion().w();
   std::cout << dso_est_tf.translation.x << dso_est_tf.translation.y
   << dso_est_tf.translation.z << std::endl;
   std::cout << "----------2" << std::endl;
   }
   }`
   (2)catkin_make it and remember to export the DSO path.
   This an example pic:
   

Now your terminal should show the exact camera poses with other stuff.
Just focus on the camera pose for now.
The next thing is to publish the pose, which you should know what to do.