Kinematic ICP Threshold (#15)

* We didnt have proper flagging in the online node

* Clean launch common args

* Add new threshold

* Update CMakeLists.txt (#7)

Make *USE_SYSTEM_SOPHUS* consistent with the rest of the options.

* Update README.md (#6)

More accurate statement in the readme, the system does not need to follow a unicycle motion model, just the pose correction needs to do that, which applies to much more robotics platforms.

* Add python checks to pre-commit (#9)

* Add black and isort

* Format

* use black for isort

* Fixing ROS launch system (#8)

* We didnt have proper flagging in the online node

* Clean launch common args

* Fix formatting python

* Tiny modification, but makes sense

* Renaming and add configuration for the correspondence threshold

* Fix pipeline

* Consistency in the CMakeLists

* Remove ternary operator and make the two functions consistent

* Update cpp/kinematic_icp/correspondence_threshold/CorrespondenceThreshold.hpp

Co-authored-by: Benedikt Mersch <mersch@igg.uni-bonn.de>

* Renaming according to Ben's review

* Why a ref

* Odometry Regularization (#19)

* Natural extension to make also the odometry regularization optional

* Consistency in the CMakeLists

* Remove ternary operator and make the two functions consistent

* Update cpp/kinematic_icp/correspondence_threshold/CorrespondenceThreshold.hpp

Co-authored-by: Benedikt Mersch <mersch@igg.uni-bonn.de>

* Renaming according to Ben's review

* Why a ref

* Use the same default

---------

Co-authored-by: Benedikt Mersch <mersch@igg.uni-bonn.de>
Co-authored-by: Benedikt Mersch <benedikt.mersch@gmail.com>

---------

Co-authored-by: Benedikt Mersch <mersch@igg.uni-bonn.de>
Co-authored-by: Benedikt Mersch <benedikt.mersch@gmail.com>

cpp/kinematic_icp/correspondence_threshold/CMakeLists.txt

@@ -20,6 +20,7 @@
 # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 # SOFTWARE.
-add_library(kinematic_icp_threshold CorrespondenceThreshold.cpp)
-target_link_libraries(kinematic_icp_threshold Sophus::Sophus)
+add_library(kinematic_icp_threshold STATIC)
+target_sources(kinematic_icp_threshold PRIVATE CorrespondenceThreshold.cpp)
+target_link_libraries(kinematic_icp_threshold PUBLIC Sophus::Sophus)
 set_global_target_properties(kinematic_icp_threshold)

cpp/kinematic_icp/correspondence_threshold/CorrespondenceThreshold.cpp

@@ -26,7 +26,7 @@
 #include <sophus/se3.hpp>
 
 namespace {
-double ModelError(const Sophus::SE3d &pose, const double max_range) {
+double OdometryErrorInPointSpace(const Sophus::SE3d &pose, const double max_range) {
     const double &theta = pose.so3().logAndTheta().theta;
     const double &delta_rot = 2.0 * max_range * std::sin(theta / 2.0);
     const double &delta_trans = pose.translation().norm();
@@ -43,20 +43,23 @@ CorrespondenceThreshold::CorrespondenceThreshold(const double map_discretization
       max_range_(max_range),
       use_adaptive_threshold_(use_adaptive_threshold),
       fixed_threshold_(fixed_threshold),
-      model_sse_(0.0),
+      odom_sse_(0.0),
       num_samples_(1e-8) {}
 
 double CorrespondenceThreshold::ComputeThreshold() const {
-    const double moder_error_variance = std::sqrt(model_sse_ / num_samples_);
-    const double adaptive_threshold = 3.0 * (map_discretization_error_ + moder_error_variance);
-    return use_adaptive_threshold_ ? adaptive_threshold : fixed_threshold_;
+    if (!use_adaptive_threshold_) return fixed_threshold_;
+
+    const double sigma_odom = std::sqrt(odom_sse_ / num_samples_);
+    const double &sigma_map = map_discretization_error_;  // <-- Renaming for clarity
+    const double adaptive_threshold = 3.0 * (sigma_map + sigma_odom);
+    return adaptive_threshold;
 }
 
-void CorrespondenceThreshold::UpdateModelError(const Sophus::SE3d &current_deviation) {
+void CorrespondenceThreshold::UpdateOdometryError(const Sophus::SE3d &odometry_error) {
     if (!use_adaptive_threshold_) return;
 
-    const double &model_error = ModelError(current_deviation, max_range_);
-    model_sse_ += model_error * model_error;
+    const double &odom_error_in_point_space = OdometryErrorInPointSpace(odometry_error, max_range_);
+    odom_sse_ += odom_error_in_point_space * odom_error_in_point_space;
     num_samples_ += 1.0;
 }
 

cpp/kinematic_icp/correspondence_threshold/CorrespondenceThreshold.hpp

@@ -33,12 +33,20 @@ struct CorrespondenceThreshold {
                                      const bool use_adaptive_threshold,
                                      const double fixed_threshold);
 
+<<<<<<< HEAD
     void UpdateModelError(const Sophus::SE3d &current_deviation);
+=======
+    void UpdateOdometryError(const Sophus::SE3d &odometry_error);
+>>>>>>> 59bc9fc (Kinematic ICP Threshold (#15))
 
     double ComputeThreshold() const;
 
     inline void Reset() {
+<<<<<<< HEAD
         model_sse_ = 0.0;
+=======
+        odom_sse_ = 0.0;
+>>>>>>> 59bc9fc (Kinematic ICP Threshold (#15))
         num_samples_ = 1e-8;
     }
 
@@ -49,8 +57,13 @@ struct CorrespondenceThreshold {
     bool use_adaptive_threshold_;
     double fixed_threshold_;  // <-- Ignored if use_adaptive_threshold_ = true
 
+<<<<<<< HEAD
     // Local cache for ccomputation
     double model_sse_;
+=======
+    // Local cache for computation
+    double odom_sse_;
+>>>>>>> 59bc9fc (Kinematic ICP Threshold (#15))
     double num_samples_;
 };
 }  // namespace kinematic_icp

cpp/kinematic_icp/pipeline/CMakeLists.txt

@@ -23,5 +23,5 @@
 add_library(kinematic_icp_pipeline STATIC)
 target_sources(kinematic_icp_pipeline PRIVATE KinematicICP.cpp)
 target_link_libraries(kinematic_icp_pipeline PUBLIC kinematic_icp_registration kinematic_icp_threshold
-  kiss_icp_pipeline Eigen3::Eigen Sophus::Sophus)
+                                                    kiss_icp_pipeline)
 set_global_target_properties(kinematic_icp_pipeline)