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

apply rotation to dense-input-msgs

Browse Source
This commit is contained in:
Christian Forster
2014-05-23 15:31:48 +02:00
parent e8d6757e9e
commit 4bc234e490
1 changed files with 5 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
svo_ros/src/visualizer.cpp
+5 -2
View File
@@ -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];