i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-12 20:49:50 +08:00
Code Issues Packages Projects Releases Wiki Activity

fixed #172

Browse Source
This commit is contained in:
matlabbe
2017-05-23 18:03:43 -04:00
parent 77f0172942
commit 47e6c8dfdc
1 changed files with 7 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/MsgConversion.cpp
+7
View File
@@ -69,6 +69,13 @@ void transformToGeometryMsg(const rtabmap::Transform & transform, geometry_msgs:
if(!transform.isNull()) if(!transform.isNull())
{ {
tf::transformEigenToMsg(transform.toEigen3d(), msg); tf::transformEigenToMsg(transform.toEigen3d(), msg);
// make sure the quaternion is normalized
long double recipNorm = 1.0 / sqrt(msg.rotation.x * msg.rotation.x + msg.rotation.y * msg.rotation.y + msg.rotation.z * msg.rotation.z + msg.rotation.w * msg.rotation.w);
msg.rotation.x *= recipNorm;
msg.rotation.y *= recipNorm;
msg.rotation.z *= recipNorm;
msg.rotation.w *= recipNorm;
} }
else else
{ {