CRITICAL: Now publishing camera in world frame. before it was the opposite.

svo/include/svo/frame_handler_base.h

@@ -63,9 +63,6 @@ public:
 
   virtual ~FrameHandlerBase();
 
-  /// Set initial pose of the camera.
-  void setInitialPose(const SE3& T_frame_from_world) { T_f_w_init_ = T_frame_from_world; }
-
   /// Get the current map.
   const Map& map() const { return map_; }
 
@@ -92,7 +89,6 @@ protected:
   bool set_reset_;              //!< Flag that the user can set. Will reset the system before the next iteration.
   bool set_start_;              //!< Flag the user can set to start the system when the next image is received.
   Map map_;                     //!< Map of keyframes created by the slam system.
-  SE3 T_f_w_init_;              //!< Initial pose of the camera.
   vk::Timer timer_;             //!< Stopwatch to measure time to process frame.
   vk::RingBuffer<double> acc_frame_timings_;    //!< Total processing time of the last 10 frames, used to give some user feedback on the performance.
   vk::RingBuffer<size_t> acc_num_obs_;          //!< Number of observed features of the last 10 frames, used to give some user feedback on the tracking performance.

svo/src/frame_handler_base.cpp

@@ -120,14 +120,14 @@ int FrameHandlerBase::finishFrameProcessingCommon(
   num_obs_last_ = num_observations;
   SVO_STOP_TIMER("tot_time");
 
-  #ifdef SVO_TRACE
-    g_permon->writeToFile();
-    {
-      boost::unique_lock<boost::mutex> lock(map_.point_candidates_.mut_);
-      size_t n_candidates = map_.point_candidates_.candidates_.size();
-      SVO_LOG(n_candidates);
-    }
-  #endif
+#ifdef SVO_TRACE
+  g_permon->writeToFile();
+  {
+    boost::unique_lock<boost::mutex> lock(map_.point_candidates_.mut_);
+    size_t n_candidates = map_.point_candidates_.candidates_.size();
+    SVO_LOG(n_candidates);
+  }
+#endif
 
   // reset if failure or requested
   if(dropout == RESULT_FAILURE || set_reset_)

svo/src/frame_handler_mono.cpp

@@ -91,7 +91,7 @@ void FrameHandlerMono::setFirstFrame(const FramePtr& first_frame)
 
 FrameHandlerMono::UpdateResult FrameHandlerMono::processFirstFrame()
 {
-  new_frame_->T_f_w_ = T_f_w_init_;
+  new_frame_->T_f_w_ = SE3(Matrix3d::Identity(), Vector3d::Zero());
   if(klt_homography_init_.addFirstFrame(new_frame_) == initialization::FAILURE)
     return RESULT_NO_KEYFRAME;
   new_frame_->setKeyframe();

svo_ros/include/svo_ros/visualizer.h

@@ -59,6 +59,7 @@ public:
   ros::Publisher pub_dense_;
   image_transport::Publisher pub_images_;
   tf::TransformBroadcaster br_;
+  SE3 T_world_from_vision_;
 
   Visualizer();
   ~Visualizer() {};

svo_ros/src/visualizer.cpp

@@ -43,7 +43,8 @@ Visualizer() :
     trace_id_(0),
     img_pub_level_(vk::getParam<int>("svo/image_publishing_level", 0)),
     img_pub_nth_(vk::getParam<int>("svo/publish_every_nth_img", 1)),
-    dense_pub_nth_(vk::getParam<int>("svo/publish_every_nth_dense_input", 1))
+    dense_pub_nth_(vk::getParam<int>("svo/publish_every_nth_dense_input", 1)),
+    T_world_from_vision_(Matrix3d::Identity(), Vector3d::Zero())
 {
   // Init ROS Marker Publishers
   pub_frames_ = pnh_.advertise<visualization_msgs::Marker>("keyframes", 10);
@@ -159,22 +160,27 @@ publishMinimal(const cv::Mat& img, const FramePtr& frame, const FrameHandlerMono
     pub_images_.publish(img_msg.toImageMsg());
   }
 
-  // publish pose (world in camera frame)
+  // publish pose (camera in world frame)
   if(pub_pose_.getNumSubscribers() > 0 && frame != NULL)
   {
+    // transform from vision-frame to world-frame
+    SE3 T_world_from_cam(T_world_from_vision_*frame->T_f_w_.inverse());
+    Quaterniond q_world_from_cam(T_world_from_cam.rotation_matrix()*T_world_from_vision_.rotation_matrix().transpose());
+    const Vector3d& p_cam_in_world(T_world_from_cam.translation());
+    Matrix<double,6,6> Cov_in_world = T_world_from_cam.Adj()*frame->Cov_*T_world_from_cam.inverse().Adj();
+
+    // publish message
     geometry_msgs::PoseWithCovarianceStampedPtr msg_pose(new geometry_msgs::PoseWithCovarianceStamped);
-    Quaterniond q(frame->T_f_w_.rotation_matrix());
-    Vector3d p(frame->T_f_w_.translation());
     msg_pose->header = header_msg;
-    msg_pose->pose.pose.position.x = p[0];
-    msg_pose->pose.pose.position.y = p[1];
-    msg_pose->pose.pose.position.z = p[2];
-    msg_pose->pose.pose.orientation.x = q.x();
-    msg_pose->pose.pose.orientation.y = q.y();
-    msg_pose->pose.pose.orientation.z = q.z();
-    msg_pose->pose.pose.orientation.w = q.w();
+    msg_pose->pose.pose.position.x = p_cam_in_world[0];
+    msg_pose->pose.pose.position.y = p_cam_in_world[1];
+    msg_pose->pose.pose.position.z = p_cam_in_world[2];
+    msg_pose->pose.pose.orientation.x = q_world_from_cam.x();
+    msg_pose->pose.pose.orientation.y = q_world_from_cam.y();
+    msg_pose->pose.pose.orientation.z = q_world_from_cam.z();
+    msg_pose->pose.pose.orientation.w = q_world_from_cam.w();
     for(size_t i=0; i<36; ++i)
-      msg_pose->pose.covariance[i] = frame->Cov_(i%6, i/6);
+      msg_pose->pose.covariance[i] = Cov_in_world(i%6, i/6);
     pub_pose_.publish(msg_pose);
   }
 }

svo_ros/src/vo_node.cpp

@@ -74,18 +74,17 @@ VoNode() :
   if(!vk::camera_loader::loadFromRosNs("svo", cam_))
     throw std::runtime_error("Camera model not correctly specified.");
 
-  // Init camera
-  vo_ = new svo::FrameHandlerMono(cam_);
-
-  // Set initial position and orientation
-  Sophus::SE3 T_cam_from_world(
+  // Get initial position and orientation
+  visualizer_.T_world_from_vision_ = Sophus::SE3(
       vk::rpy2dcm(Vector3d(vk::getParam<double>("svo/init_rx", 0.0),
                            vk::getParam<double>("svo/init_ry", 0.0),
                            vk::getParam<double>("svo/init_rz", 0.0))),
       Eigen::Vector3d(vk::getParam<double>("svo/init_tx", 0.0),
                       vk::getParam<double>("svo/init_ty", 0.0),
                       vk::getParam<double>("svo/init_tz", 0.0)));
-  vo_->setInitialPose(T_cam_from_world);
+
+  // Init VO and start
+  vo_ = new svo::FrameHandlerMono(cam_);
   vo_->start();
 }