i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-05 17:27:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added g2o optimization option (along with TORO). Some refactoring: updated graph optimization parameter names, new graph::Optimizer class and new parameter RGBD/OptimizeSlam2d

Browse Source
This commit is contained in:
Mathieu Labbe
2015-03-12 17:00:56 -04:00
parent 05d4276ba0
commit aa8fe2e55c
22 changed files with 3254 additions and 820 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Transform.cpp
+36
View File
@@ -131,6 +131,13 @@ void Transform::setIdentity()
*this = getIdentity();
}
float Transform::theta() const
{
float roll, pitch, yaw;
this->getEulerAngles(roll, pitch, yaw);
return yaw;
}
Transform Transform::inverse() const
{
return fromEigen4f(toEigen4f().inverse());
@@ -155,6 +162,12 @@ void Transform::getTranslationAndEulerAngles(float & x, float & y, float & z, fl
pcl::getTranslationAndEulerAngles(toEigen3f(), x, y, z, roll, pitch, yaw);
}
void Transform::getEulerAngles(float & roll, float & pitch, float & yaw) const
{
float x,y,z;
pcl::getTranslationAndEulerAngles(toEigen3f(), x, y, z, roll, pitch, yaw);
}
void Transform::getTranslation(float & x, float & y, float & z) const
{
x = this->x();
@@ -252,6 +265,16 @@ Eigen::Affine3d Transform::toEigen3d() const
return Eigen::Affine3d(toEigen4d());
}
Eigen::Quaternionf Transform::getQuaternionf() const
{
return Eigen::Quaternionf(this->toEigen3f().rotation()).normalized();
}
Eigen::Quaterniond Transform::getQuaterniond() const
{
return Eigen::Quaterniond(this->toEigen3d().rotation()).normalized();
}
Transform Transform::getIdentity()
{
return Transform(1,0,0,0, 0,1,0,0, 0,0,1,0);
@@ -283,4 +306,17 @@ Transform Transform::fromEigen3d(const Eigen::Affine3d & matrix)
matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
}
Transform Transform::fromEigen3f(const Eigen::Isometry3f & matrix)
{
return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
}
Transform Transform::fromEigen3d(const Eigen::Isometry3d & matrix)
{
return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
}
}