diff --git a/voxgraph/include/voxgraph/frontend/voxgraph_mapper.h b/voxgraph/include/voxgraph/frontend/voxgraph_mapper.h index 0ea1dd42..5642056b 100644 --- a/voxgraph/include/voxgraph/frontend/voxgraph_mapper.h +++ b/voxgraph/include/voxgraph/frontend/voxgraph_mapper.h @@ -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_; diff --git a/voxgraph/src/frontend/voxgraph_mapper.cpp b/voxgraph/src/frontend/voxgraph_mapper.cpp index 047ce40e..5bd321d8 100644 --- a/voxgraph/src/frontend/voxgraph_mapper.cpp +++ b/voxgraph/src/frontend/voxgraph_mapper.cpp @@ -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(submap_config_)), @@ -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: " + << (loop_closure_constraints_enabled_ ? "enabled" : "disabled")); nh_measurement_params.param("height/enabled", height_constraints_enabled_, height_constraints_enabled_); ROS_INFO_STREAM_COND( @@ -242,9 +250,11 @@ void VoxgraphMapper::loopClosureCallback( Transformation T_t1_t2(translation.cast(), rotation.cast()); 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 + 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();