i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 17:40:23 +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

29
corelib/src/Graph.cpp
View File

@@ -744,9 +744,8 @@ void calcRelativeErrors (
// compute rotational and translational errors
Transform pose_delta_gt = poses_gt[i].inverse()*poses_gt[i+1];
Transform pose_delta_result = poses_result[i].inverse()*poses_result[i+1];
Transform pose_error = pose_delta_result.inverse()*pose_delta_gt;
float r_err = pose_error.getAngle();
float t_err = pose_error.getNorm();
float r_err = pose_delta_result.getAngle(pose_delta_gt);
float t_err = pose_delta_result.getDistance(pose_delta_gt);
// write to file
err.push_back(errors(i,r_err,t_err,0,0));
@@ -993,15 +992,21 @@ void computeMaxGraphErrors(
if(iter->second.type() != Link::kLandmark ||
1.0 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) < 9999.0)
{
float opt_roll,opt_pitch,opt_yaw;
float link_roll,link_pitch,link_yaw;
t.getEulerAngles(opt_roll, opt_pitch, opt_yaw);
linkT.getEulerAngles(link_roll, link_pitch, link_yaw);
float angularError = uMax3(
force3DoF?0:fabs(opt_roll - link_roll),
force3DoF?0:fabs(opt_pitch - link_pitch),
fabs(opt_yaw - link_yaw));
angularError = angularError>M_PI?2*M_PI-angularError:angularError;
float angularError = 0.0f;
if(force3DoF)
{
float opt_roll,opt_pitch,opt_yaw;
float link_roll,link_pitch,link_yaw;
t.getEulerAngles(opt_roll, opt_pitch, opt_yaw);
linkT.getEulerAngles(link_roll, link_pitch, link_yaw);
angularError = fabs(opt_yaw - link_yaw);
angularError = angularError>M_PI?2*M_PI-angularError:angularError;
}
else
{
angularError = t.getAngle(linkT);
}
UASSERT(iter->second.rotVariance(false)>0.0);
float stddevAngular = sqrt(iter->second.rotVariance(false));
float angularErrorRatio = angularError/stddevAngular;

5
corelib/src/OdometryInfo.cpp
View File

@@ -191,9 +191,8 @@ std::map<std::string, float> OdometryInfo::statistics(const Transform & pose)
{
if(!transform.isNull())
{
rtabmap::Transform diff = transformGroundTruth.inverse()*transform;
stats.insert(std::make_pair("Odometry/TG_error_lin/m", diff.getNorm()));
stats.insert(std::make_pair("Odometry/TG_error_ang/deg", diff.getAngle()*180.0/CV_PI));
stats.insert(std::make_pair("Odometry/TG_error_lin/m", transformGroundTruth.getDistance(transform)));
stats.insert(std::make_pair("Odometry/TG_error_ang/deg", transformGroundTruth.getAngle(transform)*180.0/CV_PI));
}
transformGroundTruth.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);

13
corelib/src/Rtabmap.cpp
View File

@@ -1701,7 +1701,7 @@ bool Rtabmap::process(
{
distanceToClosestNodeInTheGraph = sqrt(sqrdDistance);
UDEBUG("Last localization pose = %s, closest node=%d (%f m)", newPose.prettyPrint().c_str(), closestNode, distanceToClosestNodeInTheGraph);
angleToClosestNodeInTheGraph = (newPose.inverse() * _optimizedPoses.at(closestNode)).getAngle();
angleToClosestNodeInTheGraph = newPose.getAngle(_optimizedPoses.at(closestNode));
}
}
@@ -3954,7 +3954,7 @@ bool Rtabmap::process(
{
distanceToClosestNodeInTheGraph = _lastLocalizationPose.getDistance(_optimizedPoses.at(closestNode));
UDEBUG("Last localization pose = %s, updated closest node=%d (%f m)", _lastLocalizationPose.prettyPrint().c_str(), closestNode, distanceToClosestNodeInTheGraph);
angleToClosestNodeInTheGraph = (_lastLocalizationPose.inverse() * _optimizedPoses.at(closestNode)).getAngle();
angleToClosestNodeInTheGraph = _lastLocalizationPose.getAngle(_optimizedPoses.at(closestNode));
}
}
_lastLocalizationPose = _optimizedPoses.at(signature->id()); // update
@@ -4103,16 +4103,15 @@ bool Rtabmap::process(
if(!sLoop->getGroundTruthPose().isNull() && !signature->getGroundTruthPose().isNull())
{
Transform transformGT = sLoop->getGroundTruthPose().inverse() * signature->getGroundTruthPose();
Transform error = loopIter->second.transform().inverse() * transformGT;
statistics_.addStatistic(Statistics::kGtLocalization_linear_error(), error.getNorm());
statistics_.addStatistic(Statistics::kGtLocalization_angular_error(), error.getAngle(1,0,0)*180/M_PI);
statistics_.addStatistic(Statistics::kGtLocalization_linear_error(), loopIter->second.transform().getDistance(transformGT));
statistics_.addStatistic(Statistics::kGtLocalization_angular_error(), loopIter->second.transform().getAngle(transformGT)*180/M_PI);
}
}
_distanceTravelledSinceLastLocalization = 0.0f;
statistics_.addStatistic(Statistics::kLoopMapToOdom_norm(), _mapCorrection.getNorm());
statistics_.addStatistic(Statistics::kLoopMapToOdom_angle(), _mapCorrection.getAngle()*180.0f/M_PI);
statistics_.addStatistic(Statistics::kLoopMapToOdom_angle(), _mapCorrection.getAngle(Transform::getIdentity())*180.0f/M_PI);
_mapCorrection.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
statistics_.addStatistic(Statistics::kLoopMapToOdom_x(), x);
statistics_.addStatistic(Statistics::kLoopMapToOdom_y(), y);
@@ -4126,7 +4125,7 @@ bool Rtabmap::process(
{
Transform odomCorrection = (previousMapCorrection*odomPose).inverse()*_mapCorrection*odomPose;
statistics_.addStatistic(Statistics::kLoopOdom_correction_norm(), odomCorrection.getNorm());
statistics_.addStatistic(Statistics::kLoopOdom_correction_angle(), odomCorrection.getAngle()*180.0f/M_PI);
statistics_.addStatistic(Statistics::kLoopOdom_correction_angle(), odomCorrection.getAngle(Transform::getIdentity())*180.0f/M_PI);
odomCorrection.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
statistics_.addStatistic(Statistics::kLoopOdom_correction_x(), x);
statistics_.addStatistic(Statistics::kLoopOdom_correction_y(), y);

6
corelib/src/Transform.cpp
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

2
corelib/src/odometry/OdometryDVO.cpp
View File

@@ -217,7 +217,7 @@ Transform OdometryDVO::computeTransform(
t = motionFromKeyFrame_.inverse() * t;
// TODO make parameters?
if(currentMotion.getNorm() > 0.01 || currentMotion.getAngle() > 0.01)
if(currentMotion.getNorm() > 0.01 || currentMotion.getAngle(Transform::getIdentity()) > 0.01)
{
if(info)
{