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();