i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-10 21:40:19 +08:00
Code Issues Packages Projects Releases Wiki Activity

Localization: Refactored getConnectedGraph() for 2d slam and tags (https://github.com/introlab/rtabmap_ros/issues/1057). Transform: Fixed is3DoF() and is 4DoF()

Browse Source
This commit is contained in:
matlabbe
2023-11-16 22:44:32 -08:00
parent 757d3eb90b
commit 9c56a429cc
4 changed files with 41 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Transform.cpp
+13 -3
View File
@@ -211,14 +211,24 @@ Transform Transform::to3DoF() const
{
float x,y,z,roll,pitch,yaw;
this->getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
return Transform(x,y,0, 0,0,yaw);
float A = std::cos(yaw);
float B = std::sin(yaw);
return Transform(
A,-B, 0, x,
B, A, 0, y,
0, 0, 1, 0);
}
Transform Transform::to4DoF() const
{
float x,y,z,roll,pitch,yaw;
this->getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
return Transform(x,y,z, 0,0,yaw);
float A = std::cos(yaw);
float B = std::sin(yaw);
return Transform(
A,-B, 0, x,
B, A, 0, y,
0, 0, 1, z);
}
bool Transform::is3DoF() const
@@ -232,7 +242,7 @@ bool Transform::is4DoF() const
r23() == 0.0 &&
r31() == 0.0 &&
r32() == 0.0 &&
r33() == 0.0;
r33() == 1.0;
}
cv::Mat Transform::rotationMatrix() const