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

Fix 6DoF graph angular check (#1511)

Browse Source 
* Fix 6DoF graph angular check

* Updated getAngle to use better version
This commit is contained in:
matlabbe
2025-05-24 13:25:26 -07:00
committed by GitHub
parent a02621ef30
commit 478e909742
10 changed files with 36 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Transform.cpp
+2 -4
View File
@@ -273,11 +273,9 @@ void Transform::getTranslation(float & x, float & y, float & z) const
z = this->z();
}
float Transform::getAngle(float x, float y, float z) const
float Transform::getAngle(const Transform & t) const
{
Eigen::Vector3f vA(x,y,z);
Eigen::Vector3f vB = this->toEigen3f().linear()*Eigen::Vector3f(1,0,0);
return pcl::getAngle3D(Eigen::Vector4f(vA[0], vA[1], vA[2], 0), Eigen::Vector4f(vB[0], vB[1], vB[2], 0));
return getQuaternionf().angularDistance(t.getQuaternionf());
}
float Transform::getNorm() const