Skip to content

RigidBodyStateVisualisation changes #166

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 3 commits into
base: master
Choose a base branch
from
+106 −23
Open

RigidBodyStateVisualisation changes #166

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
a85455f
fix: Stop emiting propertyChanged when nothing changed
pierrewillenbrockdfki Aug 8, 2023
8ae24cc
fix: RigidBodyStateVisualization: add updateModel, use it for more pr…
pierrewillenbrockdfki Aug 8, 2023
4a653f1
feat: RigidBodyStateVisualization: allow displaying inverse transform
pierrewillenbrockdfki Aug 8, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
123 changes: 100 additions & 23 deletions viz/RigidBodyStateVisualization.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand All @@ -52,11 +53,23 @@ 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");
updateModel();
}
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");
updateModel();
}

void RigidBodyStateVisualization::setTexture(QString const& path)
{ return setTexture(path.toStdString()); }
Expand Down Expand Up @@ -216,17 +229,25 @@ ref_ptr<Group> 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;
}

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
setSize(total_size);
updateModel();
}

double RigidBodyStateVisualization::getTextSize() const
Expand All @@ -236,20 +257,20 @@ 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
setSize(total_size);
updateModel();
}

void RigidBodyStateVisualization::setSize(double size)
{
if(total_size == 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
Expand Down Expand Up @@ -288,6 +309,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);
Expand All @@ -309,7 +333,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())
{
Expand Down Expand Up @@ -349,15 +372,38 @@ 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");
updateModel();
}
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; }

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");
updateModel();
}
bool RigidBodyStateVisualization::isCovarianceDisplayedWithSamples() const
{ return covariance_with_samples; }

Expand Down Expand Up @@ -428,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)
{
Expand All @@ -442,16 +501,34 @@ void RigidBodyStateVisualization::updateMainNode(Node* node)
else
uncertainty->hideSamples();

uncertainty->setMean(static_cast<Eigen::Vector3d>(state.position));
uncertainty->setCovariance(static_cast<Eigen::Matrix3d>(state.cov_position));
if(display_target_in_source) {
base::Position position(transform_inv.translation());
uncertainty->setMean(static_cast<Eigen::Vector3d>(position));
uncertainty->setCovariance(static_cast<Eigen::Matrix3d>(
transform_inv.linear() *
state.orientation.toRotationMatrix().inverse() *
state.cov_position));
} else {
uncertainty->setMean(static_cast<Eigen::Vector3d>(state.position));
uncertainty->setCovariance(static_cast<Eigen::Matrix3d>(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);
}
}
}

Expand Down
6 changes: 6 additions & 0 deletions viz/RigidBodyStateVisualization.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,7 @@ class RigidBodyStateVisualization : public Vizkit3DPlugin<base::samples::RigidBo
Q_PROPERTY(bool forcePositionDisplay READ isPositionDisplayForced WRITE setPositionDisplayForceFlag)
Q_PROPERTY(bool forceOrientationDisplay READ isOrientationDisplayForced WRITE setOrientationDisplayForceFlag)
Q_PROPERTY(QString modelPath READ getModelPath WRITE loadModel)
Q_PROPERTY(bool displayTargetInSource READ isTargetInSourceShown WRITE setDisplayTargetInSource)

public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Expand Down Expand Up @@ -93,6 +94,9 @@ class RigidBodyStateVisualization : public Vizkit3DPlugin<base::samples::RigidBo
void displayCovarianceWithSamples(bool enable);
bool isCovarianceDisplayedWithSamples() const;

void setDisplayTargetInSource(bool enable);
bool isTargetInSourceShown() const;

/** Sets the color of the default body model in R, G, B
*
* Values must be between 0 and 1
Expand Down Expand Up @@ -137,6 +141,7 @@ class RigidBodyStateVisualization : public Vizkit3DPlugin<base::samples::RigidBo
osg::ref_ptr<osg::Node> body_model;
osg::ref_ptr<osg::Group> createSimpleBody(double size);
osg::ref_ptr<osg::Group> createSimpleSphere(double size);
void updateModel();

osg::ref_ptr<osg::Image> image;
osg::ref_ptr<osg::Texture2D> texture;
Expand All @@ -156,6 +161,7 @@ class RigidBodyStateVisualization : public Vizkit3DPlugin<base::samples::RigidBo

QString model_path;

bool display_target_in_source;
};

}
Expand Down