From a85455f651f7a63163f073a866b260e2ae756f98 Mon Sep 17 00:00:00 2001 From: Pierre Willenbrock Date: Tue, 8 Aug 2023 15:39:31 +0200 Subject: [PATCH 1/3] fix: Stop emiting propertyChanged when nothing changed This would break endless recursion if the receiver of propertyChanged has other callers than the visualisation and also wants to keep the visualisation updated, which seems to not have been an issue, yet. Also a slight performance benefit. --- viz/RigidBodyStateVisualization.cpp | 38 +++++++++++++++++++++++++---- 1 file changed, 33 insertions(+), 5 deletions(-) diff --git a/viz/RigidBodyStateVisualization.cpp b/viz/RigidBodyStateVisualization.cpp index d8556b1d..f4c8bbfb 100644 --- a/viz/RigidBodyStateVisualization.cpp +++ b/viz/RigidBodyStateVisualization.cpp @@ -52,11 +52,21 @@ void RigidBodyStateVisualization::setColor(const Vec4d& color, Geode* geode) bool RigidBodyStateVisualization::isPositionDisplayForced() const { return forcePositionDisplay; } void RigidBodyStateVisualization::setPositionDisplayForceFlag(bool flag) -{ forcePositionDisplay = flag; emit propertyChanged("forcePositionDisplay"); } +{ + if(forcePositionDisplay == flag) + return; + forcePositionDisplay = flag; + emit propertyChanged("forcePositionDisplay"); +} bool RigidBodyStateVisualization::isOrientationDisplayForced() const { return forceOrientationDisplay; } void RigidBodyStateVisualization::setOrientationDisplayForceFlag(bool flag) -{ forceOrientationDisplay = flag; emit propertyChanged("forceOrientationDisplay"); } +{ + if(forceOrientationDisplay == flag) + return; + forceOrientationDisplay = flag; + emit propertyChanged("forceOrientationDisplay"); +} void RigidBodyStateVisualization::setTexture(QString const& path) { return setTexture(path.toStdString()); } @@ -223,6 +233,8 @@ double RigidBodyStateVisualization::getMainSphereSize() const void RigidBodyStateVisualization::setMainSphereSize(double size) { + if(main_size == size) + return; main_size = size; emit propertyChanged("sphereSize"); // This triggers an update of the model if we don't have a custom model @@ -236,6 +248,8 @@ double RigidBodyStateVisualization::getTextSize() const void RigidBodyStateVisualization::setTextSize(double size) { + if(text_size == size) + return; text_size = size; emit propertyChanged("textSize"); // This triggers an update of the model if we don't have a custom model @@ -244,6 +258,8 @@ void RigidBodyStateVisualization::setTextSize(double size) void RigidBodyStateVisualization::setSize(double size) { + if(total_size == size) + return; total_size = size; emit propertyChanged("size"); if (body_type == BODY_SIMPLE) @@ -288,6 +304,9 @@ void RigidBodyStateVisualization::loadModel(QString const& path) void RigidBodyStateVisualization::loadModel(std::string const& path) { + if(model_path == QString::fromStdString(path)) + return; + model_path = QString::fromStdString(path); if (path == "sphere") { resetModelSphere(total_size); @@ -309,7 +328,6 @@ void RigidBodyStateVisualization::loadModel(std::string const& path) else if (!body_model->asGeode()->getDrawable(0)->asGeometry()) std::cerr << "model does not contain a mesh, using bump mapping will not be possible" << std::endl; - model_path = QString::fromStdString(path); //set plugin name if(vizkit3d_plugin_name.isEmpty()) { @@ -349,7 +367,12 @@ void RigidBodyStateVisualization::setRotation(QQuaternion const& q) } void RigidBodyStateVisualization::displayCovariance(bool enable) -{ covariance = enable; emit propertyChanged("displayCovariance"); } +{ + if(covariance == enable) + return; + covariance = enable; + emit propertyChanged("displayCovariance"); +} bool RigidBodyStateVisualization::isCovarianceDisplayed() const { return covariance; } @@ -357,7 +380,12 @@ void RigidBodyStateVisualization::setColor(base::Vector3d const& color) { this->color = color; } void RigidBodyStateVisualization::displayCovarianceWithSamples(bool enable) -{ covariance_with_samples = enable; emit propertyChanged("displayCovarianceWithSamples"); } +{ + if(covariance_with_samples == enable) + return; + covariance_with_samples = enable; + emit propertyChanged("displayCovarianceWithSamples"); +} bool RigidBodyStateVisualization::isCovarianceDisplayedWithSamples() const { return covariance_with_samples; } From 8ae24cc0b2c6a83e5d7547668e830b3602172fdd Mon Sep 17 00:00:00 2001 From: Pierre Willenbrock Date: Tue, 8 Aug 2023 15:41:05 +0200 Subject: [PATCH 2/3] fix: RigidBodyStateVisualization: add updateModel, use it for more properties --- viz/RigidBodyStateVisualization.cpp | 22 ++++++++++++++-------- viz/RigidBodyStateVisualization.hpp | 1 + 2 files changed, 15 insertions(+), 8 deletions(-) diff --git a/viz/RigidBodyStateVisualization.cpp b/viz/RigidBodyStateVisualization.cpp index f4c8bbfb..56f62c41 100644 --- a/viz/RigidBodyStateVisualization.cpp +++ b/viz/RigidBodyStateVisualization.cpp @@ -57,6 +57,7 @@ void RigidBodyStateVisualization::setPositionDisplayForceFlag(bool flag) return; forcePositionDisplay = flag; emit propertyChanged("forcePositionDisplay"); + updateModel(); } bool RigidBodyStateVisualization::isOrientationDisplayForced() const { return forceOrientationDisplay; } @@ -66,6 +67,7 @@ void RigidBodyStateVisualization::setOrientationDisplayForceFlag(bool flag) return; forceOrientationDisplay = flag; emit propertyChanged("forceOrientationDisplay"); + updateModel(); } void RigidBodyStateVisualization::setTexture(QString const& path) @@ -226,6 +228,13 @@ ref_ptr RigidBodyStateVisualization::createSimpleBody(double size) return group; } +void RigidBodyStateVisualization::updateModel() { + if (body_type == BODY_SIMPLE) + resetModel(total_size); + else if (body_type == BODY_SPHERE) + resetModelSphere(total_size); +} + double RigidBodyStateVisualization::getMainSphereSize() const { return main_size; @@ -237,8 +246,7 @@ void RigidBodyStateVisualization::setMainSphereSize(double size) return; main_size = size; emit propertyChanged("sphereSize"); - // This triggers an update of the model if we don't have a custom model - setSize(total_size); + updateModel(); } double RigidBodyStateVisualization::getTextSize() const @@ -252,8 +260,7 @@ void RigidBodyStateVisualization::setTextSize(double size) return; text_size = size; emit propertyChanged("textSize"); - // This triggers an update of the model if we don't have a custom model - setSize(total_size); + updateModel(); } void RigidBodyStateVisualization::setSize(double size) @@ -262,10 +269,7 @@ void RigidBodyStateVisualization::setSize(double size) return; total_size = size; emit propertyChanged("size"); - if (body_type == BODY_SIMPLE) - resetModel(size); - else if (body_type == BODY_SPHERE) - resetModelSphere(size); + updateModel(); } double RigidBodyStateVisualization::getSize() const @@ -372,6 +376,7 @@ void RigidBodyStateVisualization::displayCovariance(bool enable) return; covariance = enable; emit propertyChanged("displayCovariance"); + updateModel(); } bool RigidBodyStateVisualization::isCovarianceDisplayed() const { return covariance; } @@ -385,6 +390,7 @@ void RigidBodyStateVisualization::displayCovarianceWithSamples(bool enable) return; covariance_with_samples = enable; emit propertyChanged("displayCovarianceWithSamples"); + updateModel(); } bool RigidBodyStateVisualization::isCovarianceDisplayedWithSamples() const { return covariance_with_samples; } diff --git a/viz/RigidBodyStateVisualization.hpp b/viz/RigidBodyStateVisualization.hpp index 13c02ee4..082fb2ae 100644 --- a/viz/RigidBodyStateVisualization.hpp +++ b/viz/RigidBodyStateVisualization.hpp @@ -137,6 +137,7 @@ class RigidBodyStateVisualization : public Vizkit3DPlugin body_model; osg::ref_ptr createSimpleBody(double size); osg::ref_ptr createSimpleSphere(double size); + void updateModel(); osg::ref_ptr image; osg::ref_ptr texture; From 4a653f18df9ecdd45d23eba4044052d97d027889 Mon Sep 17 00:00:00 2001 From: Pierre Willenbrock Date: Tue, 8 Aug 2023 15:47:03 +0200 Subject: [PATCH 3/3] feat: RigidBodyStateVisualization: allow displaying inverse transform This allows display of the coordinate system of the targetframe as viewed from the sourceframe. Previously, the only useful way to setup the view in regards to the other transformations was to set the frame of the visualisation to the targetFrame, and the model/coordinate cross would appear at the origin of the sourceFrame. Setting displayTargetInSource to true, one would then set the visualisation frame to the sourceFrame and get the coordinate cross(and model) at the origin of the targetFrame. --- viz/RigidBodyStateVisualization.cpp | 63 ++++++++++++++++++++++++----- viz/RigidBodyStateVisualization.hpp | 5 +++ 2 files changed, 58 insertions(+), 10 deletions(-) diff --git a/viz/RigidBodyStateVisualization.cpp b/viz/RigidBodyStateVisualization.cpp index 56f62c41..aaccdd70 100644 --- a/viz/RigidBodyStateVisualization.cpp +++ b/viz/RigidBodyStateVisualization.cpp @@ -27,6 +27,7 @@ RigidBodyStateVisualization::RigidBodyStateVisualization(QObject* parent) , bump_mapping_dirty(false) , forcePositionDisplay(false) , forceOrientationDisplay(false) + , display_target_in_source(false) { state = base::samples::RigidBodyState::invalid(); state.position = base::Vector3d::Zero(); @@ -381,6 +382,17 @@ void RigidBodyStateVisualization::displayCovariance(bool enable) bool RigidBodyStateVisualization::isCovarianceDisplayed() const { return covariance; } +void RigidBodyStateVisualization::setDisplayTargetInSource(bool enable) { + if(display_target_in_source == enable) + return; + display_target_in_source = enable; + emit propertyChanged("displayTargetInSource"); + updateModel(); +} +bool RigidBodyStateVisualization::isTargetInSourceShown() const { + return display_target_in_source; +} + void RigidBodyStateVisualization::setColor(base::Vector3d const& color) { this->color = color; } @@ -462,12 +474,25 @@ void RigidBodyStateVisualization::updateMainNode(Node* node) if (bump_mapping_dirty) updateBumpMapping(); + Eigen::Affine3d transform_inv; + if(display_target_in_source) { + transform_inv = state.getTransform().inverse(); + } + if (forcePositionDisplay || state.hasValidPosition()) { - osg::Vec3d pos( - state.position.x(), state.position.y(), state.position.z()); + if(display_target_in_source) { + base::Position position(transform_inv.translation()); + osg::Vec3d pos( + position.x(), position.y(), position.z()); - body_pose->setPosition(pos + translation); + body_pose->setPosition(pos + translation); + } else { + osg::Vec3d pos( + state.position.x(), state.position.y(), state.position.z()); + + body_pose->setPosition(pos + translation); + } } if (needs_uncertainty) { @@ -476,16 +501,34 @@ void RigidBodyStateVisualization::updateMainNode(Node* node) else uncertainty->hideSamples(); - uncertainty->setMean(static_cast(state.position)); - uncertainty->setCovariance(static_cast(state.cov_position)); + if(display_target_in_source) { + base::Position position(transform_inv.translation()); + uncertainty->setMean(static_cast(position)); + uncertainty->setCovariance(static_cast( + transform_inv.linear() * + state.orientation.toRotationMatrix().inverse() * + state.cov_position)); + } else { + uncertainty->setMean(static_cast(state.position)); + uncertainty->setCovariance(static_cast(state.cov_position)); + } } if (forceOrientationDisplay || state.hasValidOrientation()) { - osg::Quat orientation(state.orientation.x(), - state.orientation.y(), - state.orientation.z(), - state.orientation.w()); - body_pose->setAttitude(rotation * orientation); + if(display_target_in_source) { + Eigen::Quaterniond transform_orientation(transform_inv.linear()); + osg::Quat orientation(transform_orientation.x(), + transform_orientation.y(), + transform_orientation.z(), + transform_orientation.w()); + body_pose->setAttitude(rotation * orientation); + } else { + osg::Quat orientation(state.orientation.x(), + state.orientation.y(), + state.orientation.z(), + state.orientation.w()); + body_pose->setAttitude(rotation * orientation); + } } } diff --git a/viz/RigidBodyStateVisualization.hpp b/viz/RigidBodyStateVisualization.hpp index 082fb2ae..cf55050b 100644 --- a/viz/RigidBodyStateVisualization.hpp +++ b/viz/RigidBodyStateVisualization.hpp @@ -28,6 +28,7 @@ class RigidBodyStateVisualization : public Vizkit3DPlugin