Skip to content

Commit

Permalink
Change error line brightness based on loss
Browse files Browse the repository at this point in the history
  • Loading branch information
@efernandez
efernandez committed Feb 14, 2020
1 parent 6192d3a commit 0fe9f53
Show file tree
Hide file tree
Showing 4 changed files with 76 additions and 68 deletions.
9 changes: 3 additions & 6 deletions fuse_viz/include/fuse_viz/relative_pose_2d_stamped_constraint_property.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -106,8 +106,7 @@ private Q_SLOTS:
void updateColor();
void updateErrorLineAlpha();
void updateErrorLineWidth();
void updateLossErrorLineAlpha();
void updateLossErrorLineWidth();
void updateLossMinBrightness();
void updateRelativePoseAxesAlpha();
void updateRelativePoseAxesScale();
void updateRelativePoseLineAlpha();
Expand All @@ -119,8 +118,7 @@ private Q_SLOTS:
void updateColor(const VisualPtr& constraint);
void updateErrorLineAlpha(const VisualPtr& constraint);
void updateErrorLineWidth(const VisualPtr& constraint);
void updateLossErrorLineAlpha(const VisualPtr& constraint);
void updateLossErrorLineWidth(const VisualPtr& constraint);
void updateLossMinBrightness(const VisualPtr& constraint);
void updateRelativePoseAxesAlpha(const VisualPtr& constraint);
void updateRelativePoseAxesScale(const VisualPtr& constraint);
void updateRelativePoseLineAlpha(const VisualPtr& constraint);
Expand All @@ -140,8 +138,7 @@ private Q_SLOTS:
FloatProperty* relative_pose_line_width_property_;
FloatProperty* error_line_alpha_property_;
FloatProperty* error_line_width_property_;
FloatProperty* loss_error_line_alpha_property_;
FloatProperty* loss_error_line_width_property_;
FloatProperty* loss_min_brightness_property_;
MappedCovarianceProperty* covariance_property_;
};

Expand Down
12 changes: 7 additions & 5 deletions fuse_viz/include/fuse_viz/relative_pose_2d_stamped_constraint_visual.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -140,14 +140,12 @@ class RelativePose2DStampedConstraintVisual : public rviz::Object

void setErrorLineWidth(const float line_width);

void setLossErrorLineWidth(const float line_width);
void setLossMinBrightness(const float min_brightness);

void setRelativePoseLineColor(const float r, const float g, const float b, const float a);

void setErrorLineColor(const float r, const float g, const float b, const float a);

void setLossErrorLineColor(const float r, const float g, const float b, const float a);

void setRelativePoseAxesAlpha(const float alpha);

void setRelativePoseAxesScale(const Ogre::Vector3& scale);
Expand Down Expand Up @@ -192,18 +190,20 @@ class RelativePose2DStampedConstraintVisual : public rviz::Object
Ogre::SceneNode* root_node_;
Ogre::SceneNode* relative_pose_line_node_;
Ogre::SceneNode* error_line_node_;
Ogre::SceneNode* loss_error_line_node_;
Ogre::SceneNode* relative_pose_axes_node_;
Ogre::SceneNode* text_node_;

std::shared_ptr<BillboardLine> relative_pose_line_;
std::shared_ptr<BillboardLine> error_line_;
std::shared_ptr<BillboardLine> loss_error_line_;
std::shared_ptr<Axes> relative_pose_axes_;
MovableText* text_;
CovarianceVisualPtr covariance_;
std::string source_;

float loss_scale_{ -1.0 };
float min_brightness_{ 0.0 };
Ogre::ColourValue error_line_color_;

bool visible_;

private:
Expand All @@ -214,6 +214,8 @@ class RelativePose2DStampedConstraintVisual : public rviz::Object
const Ogre::Vector3& getPosition() override;
const Ogre::Quaternion& getOrientation() override;

Ogre::ColourValue computeLossErrorLineColor(const Ogre::ColourValue& color, const float loss_scale);

// Make RelativePose2DStampedConstraintProperty friend class so it create RelativePose2DStampedConstraintVisual
// objects
friend class RelativePose2DStampedConstraintProperty;
Expand Down
43 changes: 12 additions & 31 deletions fuse_viz/src/relative_pose_2d_stamped_constraint_property.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -88,16 +88,12 @@ RelativePose2DStampedConstraintProperty::RelativePose2DStampedConstraintProperty
SLOT(updateErrorLineWidth()));
error_line_width_property_->setMin(0.0);

loss_error_line_alpha_property_ =
new FloatProperty("Loss Error Line Alpha", 0.75, "Alpha of constraint error line scaled by loss cost.", this,
SLOT(updateLossErrorLineAlpha()));
loss_error_line_alpha_property_->setMin(0.0);
loss_error_line_alpha_property_->setMax(1.0);

loss_error_line_width_property_ =
new FloatProperty("Loss Error Line Width", 0.5, "Line width of constraint error line scaled by loss cost.", this,
SLOT(updateLossErrorLineWidth()));
loss_error_line_width_property_->setMin(0.0);
loss_min_brightness_property_ = new FloatProperty("Loss Min Brightness", 0.25,
"Min brightness to show the loss impact on the constraint error "
"line.",
this, SLOT(updateLossMinBrightness()));
loss_min_brightness_property_->setMin(0.0);
loss_min_brightness_property_->setMax(1.0);

show_text_property_ =
new BoolProperty("Show Text", false, "Show constraint source, type and UUID.", this, SLOT(updateShowText()));
Expand Down Expand Up @@ -141,8 +137,7 @@ RelativePose2DStampedConstraintProperty::VisualPtr RelativePose2DStampedConstrai
updateColor(visual);
updateErrorLineAlpha(visual);
updateErrorLineWidth(visual);
updateLossErrorLineAlpha(visual);
updateLossErrorLineWidth(visual);
updateLossMinBrightness(visual);
updateRelativePoseAxesAlpha(visual);
updateRelativePoseAxesScale(visual);
updateRelativePoseLineAlpha(visual);
Expand Down Expand Up @@ -203,19 +198,11 @@ void RelativePose2DStampedConstraintProperty::updateErrorLineWidth()
}
}

void RelativePose2DStampedConstraintProperty::updateLossErrorLineAlpha()
void RelativePose2DStampedConstraintProperty::updateLossMinBrightness()
{
for (auto& entry : constraints_)
{
updateLossErrorLineAlpha(entry.second);
}
}

void RelativePose2DStampedConstraintProperty::updateLossErrorLineWidth()
{
for (auto& entry : constraints_)
{
updateLossErrorLineWidth(entry.second);
updateLossMinBrightness(entry.second);
}
}

Expand Down Expand Up @@ -288,17 +275,11 @@ void RelativePose2DStampedConstraintProperty::updateErrorLineWidth(const VisualP
constraint->setErrorLineWidth(error_line_width_property_->getFloat());
}

void RelativePose2DStampedConstraintProperty::updateLossErrorLineAlpha(const VisualPtr& constraint)
void RelativePose2DStampedConstraintProperty::updateLossMinBrightness(const VisualPtr& constraint)
{
const auto color = color_property_->getColor();
constraint->setLossMinBrightness(loss_min_brightness_property_->getFloat());

constraint->setLossErrorLineColor(color.redF(), color.greenF(), color.blueF(),
loss_error_line_alpha_property_->getFloat());
}

void RelativePose2DStampedConstraintProperty::updateLossErrorLineWidth(const VisualPtr& constraint)
{
constraint->setLossErrorLineWidth(loss_error_line_width_property_->getFloat());
updateErrorLineAlpha(constraint);
}

void RelativePose2DStampedConstraintProperty::updateRelativePoseAxesAlpha(const VisualPtr& constraint)
Expand Down
80 changes: 54 additions & 26 deletions fuse_viz/src/relative_pose_2d_stamped_constraint_visual.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,6 +50,8 @@
#include <OgreSceneManager.h>
#include <OgreSceneNode.h>

#include <algorithm>

namespace rviz
{

Expand All @@ -73,12 +75,6 @@ RelativePose2DStampedConstraintVisual::RelativePose2DStampedConstraintVisual(
error_line_->setMaxPointsPerLine(2);
error_line_->setNumLines(1);

// Create constraint loss error line:
loss_error_line_node_ = root_node_->createChildSceneNode();
loss_error_line_ = std::make_shared<BillboardLine>(scene_manager_, loss_error_line_node_);
loss_error_line_->setMaxPointsPerLine(2);
loss_error_line_->setNumLines(1);

// Create constraint relative pose axes:
relative_pose_axes_node_ = root_node_->createChildSceneNode();
relative_pose_axes_ = std::make_shared<rviz::Axes>(scene_manager_, relative_pose_axes_node_, 10.0, 1.0);
Expand All @@ -104,7 +100,6 @@ RelativePose2DStampedConstraintVisual::~RelativePose2DStampedConstraintVisual()
delete text_;
scene_manager_->destroySceneNode(relative_pose_line_node_->getName());
scene_manager_->destroySceneNode(error_line_node_->getName());
scene_manager_->destroySceneNode(loss_error_line_node_->getName());
scene_manager_->destroySceneNode(relative_pose_axes_node_->getName());
scene_manager_->destroySceneNode(text_node_->getName());
scene_manager_->destroySceneNode(root_node_->getName());
Expand Down Expand Up @@ -145,9 +140,7 @@ void RelativePose2DStampedConstraintVisual::setConstraint(
error_line_->addPoint(absolute_position_ogre);
error_line_->addPoint(toOgre(pose2.getOrigin()));

// Update constraint loss error line:
loss_error_line_->clear();

// Set error line color brightness based on the loss function impact on the constraint cost:
auto loss_function = constraint.lossFunction();
if (loss_function)
{
Expand Down Expand Up @@ -194,12 +187,12 @@ void RelativePose2DStampedConstraintVisual::setConstraint(
//
// Remember that in principle `rho[0] <= squared_norm`, with `rho[0] == squared_norm` for the inlier region, and
// `rho[0] < squared_norm` for the outlier region:
const auto loss_scale = squared_norm == 0.0 ? 0.0 : rho[0] / squared_norm;
loss_scale_ = squared_norm == 0.0 ? 0.0 : rho[0] / squared_norm;

const auto loss_position = absolute_pose.getOrigin().lerp(pose2.getOrigin(), loss_scale);

loss_error_line_->addPoint(absolute_position_ogre);
loss_error_line_->addPoint(toOgre(loss_position));
// Compute error line color with the loss function impact:
const auto loss_error_line_color = computeLossErrorLineColor(error_line_color_, loss_scale_);
error_line_->setColor(loss_error_line_color.r, loss_error_line_color.g, loss_error_line_color.b,
error_line_color_.a);
}

// Update constraint relative pose axes:
Expand All @@ -214,7 +207,6 @@ void RelativePose2DStampedConstraintVisual::setUserData(const Ogre::Any& data)
{
relative_pose_line_->setUserData(data);
error_line_->setUserData(data);
loss_error_line_->setUserData(data);
relative_pose_axes_->setUserData(data);
covariance_->setUserData(data);
}
Expand All @@ -229,9 +221,9 @@ void RelativePose2DStampedConstraintVisual::setErrorLineWidth(const float line_w
error_line_->setLineWidth(line_width);
}

void RelativePose2DStampedConstraintVisual::setLossErrorLineWidth(const float line_width)
void RelativePose2DStampedConstraintVisual::setLossMinBrightness(const float min_brightness)
{
loss_error_line_->setLineWidth(line_width);
min_brightness_ = min_brightness;
}

void RelativePose2DStampedConstraintVisual::setRelativePoseLineColor(const float r, const float g, const float b,
Expand All @@ -243,13 +235,19 @@ void RelativePose2DStampedConstraintVisual::setRelativePoseLineColor(const float
void RelativePose2DStampedConstraintVisual::setErrorLineColor(const float r, const float g, const float b,
const float a)
{
error_line_->setColor(r, g, b, a);
}

void RelativePose2DStampedConstraintVisual::setLossErrorLineColor(const float r, const float g, const float b,
const float a)
{
loss_error_line_->setColor(r, g, b, a);
// Cache error line color w/o the loss function impact, so we can change its darkness based on the loss function
// impact on the constraint cost:
// Note that we cannot recover/retrieve the color from the Ogre::BillboarrdLine error_line_ because its API does NOT
// support that.
error_line_color_.r = r;
error_line_color_.g = g;
error_line_color_.b = b;
error_line_color_.a = a;

// Compute error line color with the impact of the loss function, in case the constraint has one:
const auto loss_error_line_color = computeLossErrorLineColor(error_line_color_, loss_scale_);
error_line_->setColor(loss_error_line_color.r, loss_error_line_color.r, loss_error_line_color.b,
loss_error_line_color.a);
}

void RelativePose2DStampedConstraintVisual::setRelativePoseAxesAlpha(const float alpha)
Expand Down Expand Up @@ -285,7 +283,6 @@ void RelativePose2DStampedConstraintVisual::setVisible(const bool visible)
{
relative_pose_line_node_->setVisible(visible);
error_line_node_->setVisible(visible);
loss_error_line_node_->setVisible(visible);
relative_pose_axes_node_->setVisible(visible);
}

Expand All @@ -309,4 +306,35 @@ void RelativePose2DStampedConstraintVisual::setOrientation(const Ogre::Quaternio
root_node_->setOrientation(orientation);
}

Ogre::ColourValue RelativePose2DStampedConstraintVisual::computeLossErrorLineColor(const Ogre::ColourValue& color,
const float loss_scale)
{
// Skip if the loss scale is negative, which means the constraint has no loss:
if (loss_scale < 0.0)
{
return color;
}

// Get the error line color as HSB:
Ogre::ColourValue error_line_color(color.r, color.g, color.b);
Ogre::Real hue, saturation, brightness;
error_line_color.getHSB(&hue, &saturation, &brightness);

// We should correct the color brightness if it is smaller than minimum brightness. Otherwise, we would get an
// incorrect loss brightness.
//
// However, we cannot do this because it changes the color of the error line, which should be consistent for all
// constraints visuals. Instead, we clamp the minium brightness:
const auto min_brightness = std::min(min_brightness_, brightness);

// Scale brightness by the loss scale within the [min_brightness, 1] range:
const auto loss_brightness = min_brightness + (brightness - min_brightness) * loss_scale;

// Set error line color with the loss brightness:
Ogre::ColourValue loss_error_line_color;
loss_error_line_color.setHSB(hue, saturation, loss_brightness);

return Ogre::ColourValue(loss_error_line_color.r, loss_error_line_color.g, loss_error_line_color.b, color.a);
}

} // namespace rviz

0 comments on commit 0fe9f53

Please sign in to comment.