Skip to content
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

added loop_closure_enabled_, to enable/disable loop closure constrains #36

Open
wants to merge 1 commit into
base: feature/temporal_window
Choose a base branch
from
+14 −3
Open

added loop_closure_enabled_, to enable/disable loop closure constrains #36

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
1 change: 1 addition & 0 deletions voxgraph/include/voxgraph/frontend/voxgraph_mapper.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -135,6 +135,7 @@ class VoxgraphMapper {
bool registration_constraints_enabled_;
bool odometry_constraints_enabled_;
bool height_constraints_enabled_;
bool loop_closure_constraints_enabled_;

// Instantiate the submap collection
VoxgraphSubmap::Config submap_config_;
Expand Down
16 changes: 13 additions & 3 deletions voxgraph/src/frontend/voxgraph_mapper.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -46,6 +46,7 @@ VoxgraphMapper::VoxgraphMapper(const ros::NodeHandle& nh,
registration_constraints_enabled_(false),
odometry_constraints_enabled_(false),
height_constraints_enabled_(false),
loop_closure_constraints_enabled_(false),
submap_config_(submap_config),
submap_collection_ptr_(
std::make_shared<VoxgraphSubmapCollection>(submap_config_)),
Expand Down Expand Up @@ -124,6 +125,13 @@ void VoxgraphMapper::getParametersFromRos() {
verbose_,
"Odometry constraints: " << (odometry_constraints_enabled_ ? "enabled"
: "disabled"));
nh_measurement_params.param("loop_closure/enabled",
loop_closure_constraints_enabled_,
loop_closure_constraints_enabled_);
ROS_INFO_STREAM_COND(
verbose_,
"Loop Closure registration constraints: "
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Strictly speaking, the loop closures are added to the pose graph as relative pose constraints. Registration constraints are added automatically for all overlapping submaps, so implicitly loop closures will also lead to registration constraints - but only if they cause submaps to overlap.
Could you change this INFO message to something like "Loop closure constraints: "? Just to reduce the change of other users getting confused.

<< (loop_closure_constraints_enabled_ ? "enabled" : "disabled"));
nh_measurement_params.param("height/enabled", height_constraints_enabled_,
height_constraints_enabled_);
ROS_INFO_STREAM_COND(
Expand Down Expand Up @@ -242,9 +250,11 @@ void VoxgraphMapper::loopClosureCallback(
Transformation T_t1_t2(translation.cast<voxblox::FloatingPoint>(),
rotation.cast<voxblox::FloatingPoint>());
Transformation T_AB = T_A_t1 * T_t1_t2 * T_B_t2.inverse();
pose_graph_interface_.addLoopClosureMeasurement(submap_id_A, submap_id_B,
T_AB);

// Check if loop closure constraints is enabled
Copy link
Member

@victorreijgwart victorreijgwart Aug 31, 2020 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

With the suggested implementation of this callback, the loop closure constraints would be shown in Rviz despite not being used in the optimization.
This could be confusing to other users. It'd be cleaner if the first block of code at the top of this method was:

if (!loop_closure_constraints_enabled_) {
    ROS_WARN_THROTTLE(1.0, "Received loop closure constraint, but loop_closure_constraints_enabled is set to false. Constraint will be ignored.");
    return;
}

So that if loop closures are disabled, they're ignored entirely - such that the visuals and optimization stay consistent.

Alternatively, you could start the method with:

if (!loop_closure_constraints_enabled_) {
    ROS_WARN_THROTTLE(1.0, "Received loop closure constraint, but loop_closure_constraints_enabled is set to false. Constraint will be ignored by the optimization but still shown in Rviz.");
}

But there'd still be a high chance that people don't see the warning message (e.g. when running it on a robot, or when there are many other messages being published to the same log), so unless there's a strong reason not to, I'd rather go with the first option.

if (loop_closure_constraints_enabled_) {
pose_graph_interface_.addLoopClosureMeasurement(submap_id_A, submap_id_B,
T_AB);
}
// Visualize the loop closure link
const Transformation& T_M_A = submap_A.getPose();
const Transformation& T_M_B = submap_B.getPose();
Expand Down