public/vio
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

not publishing pose in relocalization mode and fix in dense input message

Browse Source
This commit is contained in:
Flavio Fontana
2014-05-23 17:02:43 +02:00
parent ab121434e4
commit 4001f6a736
1 changed files with 2 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
svo_ros/src/visualizer.cpp
+2 -2
View File
@@ -161,7 +161,7 @@ void Visualizer::publishMinimal(
pub_images_.publish(img_msg.toImageMsg()); pub_images_.publish(img_msg.toImageMsg());
} }
if(pub_pose_.getNumSubscribers() > 0) if(pub_pose_.getNumSubscribers() > 0 && slam.stage() == FrameHandlerBase::STAGE_DEFAULT_FRAME)
{ {
Quaterniond q; Quaterniond q;
Vector3d p; Vector3d p;
@@ -300,7 +300,7 @@ void Visualizer::exportToDense(const FramePtr& frame)
// publish cam in world frame // publish cam in world frame
SE3 T_world_from_cam(T_world_from_vision_*frame->T_f_w_.inverse()); 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()); Vector3d p(T_world_from_cam.translation());
msg.pose.position.x = p[0]; msg.pose.position.x = p[0];