i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-12 06:20:19 +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/include/rtabmap/core/Transform.h
+1 -1
View File
@@ -113,7 +113,7 @@ public:
void getTranslationAndEulerAngles(float & x, float & y, float & z, float & roll, float & pitch, float & yaw) const;
void getEulerAngles(float & roll, float & pitch, float & yaw) const;
void getTranslation(float & x, float & y, float & z) const;
float getAngle(float x=1.0f, float y=0.0f, float z=0.0f) const;
float getAngle(const Transform & t) const;
float getNorm() const;
float getNormSquared() const;
float getDistance(const Transform & t) const;
corelib/src/Graph.cpp
+17 -12
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;
corelib/src/OdometryInfo.cpp
+2 -3
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);
corelib/src/Rtabmap.cpp
+6 -7
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);
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
corelib/src/odometry/OdometryDVO.cpp
+1 -1
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)
{
guilib/src/DatabaseViewer.cpp
+2 -1
View File
@@ -6358,11 +6358,12 @@ void DatabaseViewer::updateConstraintView(
ui_->checkBox_showOptimized->setEnabled(true);
Transform topt = iterFrom->second.inverse()*iterTo->second;
float diff = topt.getDistance(t);
Transform v1 = t.rotation()*Transform(1,0,0,0,0,0);
Transform v2 = topt.rotation()*Transform(1,0,0,0,0,0);
float a = pcl::getAngle3D(Eigen::Vector4f(v1.x(), v1.y(), v1.z(), 0), Eigen::Vector4f(v2.x(), v2.y(), v2.z(), 0));
a = (a *180.0f) / CV_PI;
ui_->label_constraint_opt->setText(QString("%1\n(error=%2% a=%3)").arg(QString(topt.prettyPrint().c_str()).replace(" ", "\n")).arg((t.getNorm()>0?diff/t.getNorm():0)*100.0f).arg(a));
ui_->label_constraint_opt->setText(QString("%1\n(error=%2% a=%3 deg)").arg(QString(topt.prettyPrint().c_str()).replace(" ", "\n")).arg((t.getNorm()>0?diff/t.getNorm():0)*100.0f).arg(a));
if(ui_->checkBox_showOptimized->isChecked())
{
guilib/src/MainWindow.cpp
+2 -3
View File
@@ -1908,9 +1908,8 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
{
if(!odomT.isNull())
{
rtabmap::Transform diff = odom.info().transformGroundTruth.inverse()*odomT;
_ui->statsToolBox->updateStat("Odometry/TG_error_lin/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), diff.getNorm(), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/TG_error_ang/deg", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), diff.getAngle()*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/TG_error_lin/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), odom.info().transformGroundTruth.getDistance(odomT), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/TG_error_ang/deg", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), odom.info().transformGroundTruth.getAngle(odomT)*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
}
odom.info().transformGroundTruth.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
tools/Report/main.cpp
+2 -3
View File
@@ -1255,9 +1255,8 @@ int main(int argc, char * argv[])
t.r11(), t.r12(), t.r13(), t.x()*bestScale,
t.r21(), t.r22(), t.r23(), t.y()*bestScale,
t.r31(), t.r32(), t.r33(), t.z()*bestScale);
Transform diff = gtLink.inverse()*scaledLink;
sumDist += diff.getNorm();
sumAngle += diff.getAngle();
sumDist += gtLink.getDistance(scaledLink);
sumAngle += gtLink.getAngle(scaledLink);
++count;
}
}
tools/Reprocess/main.cpp
+1 -1
View File
@@ -1289,7 +1289,7 @@ int main(int argc, char * argv[])
!info.odomPose.isNull())
{
float distance = odomPose.getDistance(info.odomPose);
float angle = (odomPose.inverse()*info.odomPose).getAngle();
float angle = odomPose.getAngle(info.odomPose);
odomDistances.push_back(distance);
if(distance < linearUpdate && angle <= angularUpdate)
{