From 4bc234e49060f24e8aa7d22250d417930b728c41 Mon Sep 17 00:00:00 2001 From: Christian Forster Date: Fri, 23 May 2014 15:31:48 +0200 Subject: [PATCH] apply rotation to dense-input-msgs --- svo_ros/src/visualizer.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/svo_ros/src/visualizer.cpp b/svo_ros/src/visualizer.cpp index a06827e..6f50461 100644 --- a/svo_ros/src/visualizer.cpp +++ b/svo_ros/src/visualizer.cpp @@ -298,8 +298,11 @@ void Visualizer::exportToDense(const FramePtr& frame) msg.min_depth = (float) min_z; msg.max_depth = (float) max_z; - Quaterniond q(frame->T_f_w_.inverse().rotation_matrix()); - Vector3d p(frame->T_f_w_.inverse().translation()); + // 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()); + Vector3d p(T_world_from_cam.translation()); + msg.pose.position.x = p[0]; msg.pose.position.y = p[1]; msg.pose.position.z = p[2];