From 340248fed09b487098196e248d7a41d4efe53267 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 14 Dec 2024 16:33:29 -0800 Subject: [PATCH] Refactored gravity links visualization (fixed rotation in some cases). --- guilib/src/DatabaseViewer.cpp | 13 +++---------- guilib/src/MainWindow.cpp | 4 ++-- 2 files changed, 5 insertions(+), 12 deletions(-) diff --git a/guilib/src/DatabaseViewer.cpp b/guilib/src/DatabaseViewer.cpp index 640bdc96de..69beddeb8a 100644 --- a/guilib/src/DatabaseViewer.cpp +++ b/guilib/src/DatabaseViewer.cpp @@ -4870,9 +4870,7 @@ void DatabaseViewer::update(int value, dbDriver_->loadLinks(id, gravityLink, Link::kGravity); if(!gravityLink.empty()) { - float roll,pitch,yaw; - gravityLink.begin()->second.transform().getEulerAngles(roll, pitch, yaw); - Eigen::Vector3d v = Transform(0,0,0,roll,pitch,0).toEigen3d() * -Eigen::Vector3d::UnitZ(); + Eigen::Vector3f v = gravityLink.begin()->second.transform().inverse().toEigen3f() * -Eigen::Vector3f::UnitZ(); labelGravity->setText(QString("x=%1 y=%2 z=%3").arg(v[0]).arg(v[1]).arg(v[2])); labelGravity->setToolTip(QString("roll=%1 pitch=%2 yaw=%3").arg(roll).arg(pitch).arg(yaw)); } @@ -5098,13 +5096,8 @@ void DatabaseViewer::update(int value, if(!gravityLink.empty() && ui_->checkBox_gravity_3dview->isChecked()) { Transform gravityT = gravityLink.begin()->second.transform(); - Eigen::Vector3f gravity(0,0,-1); - if(pose.isIdentity()) - { - gravityT = gravityT.inverse(); - } - gravity = (gravityT.rotation()*(pose).rotation().inverse()).toEigen3f()*gravity; - cloudViewer_->addOrUpdateLine("gravity", pose, (pose).translation()*Transform(gravity[0], gravity[1], gravity[2], 0, 0, 0)*pose.rotation().inverse(), Qt::yellow, true, false); + Eigen::Vector3f gravity = gravityT.inverse().toEigen3f()*-Eigen::Vector3f::UnitZ(); + cloudViewer_->addOrUpdateLine("gravity", pose, pose*Transform(gravity[0], gravity[1], gravity[2], 0, 0, 0), Qt::yellow, true, false); } //add scan diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index 03544dd766..2a1e6284b9 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -3094,8 +3094,8 @@ void MainWindow::updateMapCloud( { Transform gravityT = linkIter->second.transform(); Eigen::Vector3f gravity(0,0,-_preferencesDialog->getIMUGravityLength(0)); - gravity = (gravityT.rotation()*(iter->second).rotation().inverse()).toEigen3f()*gravity; - _cloudViewer->addOrUpdateLine(gravityName, iter->second, (iter->second).translation()*Transform(gravity[0], gravity[1], gravity[2], 0, 0, 0)*iter->second.rotation().inverse(), Qt::yellow, false, false); + gravity = gravityT.inverse().toEigen3f()*gravity; + _cloudViewer->addOrUpdateLine(gravityName, iter->second, iter->second*Transform(gravity[0], gravity[1], gravity[2], 0, 0, 0), Qt::yellow, false, false); } } else if(viewerLines.find(gravityName)!=viewerLines.end())