not publishing pose in relocalization mode and fix in dense input mes…

…sage
ancientbees · May 23, 2014 · 4001f6a · 4001f6a
1 parent ab12143
commit 4001f6a
Showing 1 changed file with 2 additions and 2 deletions.
diff --git a/svo_ros/src/visualizer.cpp b/svo_ros/src/visualizer.cpp
@@ -161,7 +161,7 @@ void Visualizer::publishMinimal(
     pub_images_.publish(img_msg.toImageMsg());
   }
 
-  if(pub_pose_.getNumSubscribers() > 0)
+  if(pub_pose_.getNumSubscribers() > 0 && slam.stage() == FrameHandlerBase::STAGE_DEFAULT_FRAME)
   {
     Quaterniond q;
     Vector3d p;
@@ -300,7 +300,7 @@ void Visualizer::exportToDense(const FramePtr& frame)
 
     // publish cam in world frame
     SE3 T_world_from_cam(T_world_from_vision_*frame->T_f_w_.inverse());
-    Quaterniond q(T_world_from_cam.rotation_matrix()*T_world_from_vision_.rotation_matrix().transpose());
+    Quaterniond q(T_world_from_cam.rotation_matrix());
     Vector3d p(T_world_from_cam.translation());
 
     msg.pose.position.x = p[0];