From 7623ede21fd9ef6ffdb9ca69a5b1131debf88a2a Mon Sep 17 00:00:00 2001
From: Pavel Rojtberg <rojtberg@gmail.com>
Date: Tue, 6 Jun 2023 17:46:35 +0200
Subject: [PATCH] SRT3D: compute mean probability for region modality

---
 SRT3D/include/srt3d/region_modality.h | 2 ++
 SRT3D/src/region_modality.cpp         | 5 +++++
 2 files changed, 7 insertions(+)

diff --git a/SRT3D/include/srt3d/region_modality.h b/SRT3D/include/srt3d/region_modality.h
index 9b2ad0b..d84e8af 100644
--- a/SRT3D/include/srt3d/region_modality.h
+++ b/SRT3D/include/srt3d/region_modality.h
@@ -118,6 +118,8 @@ class RegionModality {
   bool imshow_result() const;
   bool set_up() const;
 
+  float probability_;
+
  private:
   // Helper methods for precalculation of internal data
   void PrecalculateFunctionLookup();
diff --git a/SRT3D/src/region_modality.cpp b/SRT3D/src/region_modality.cpp
index 0bc42ae..15de8ff 100644
--- a/SRT3D/src/region_modality.cpp
+++ b/SRT3D/src/region_modality.cpp
@@ -301,6 +301,7 @@ bool RegionModality::CalculatePoseUpdate(int corr_iteration,
   gradient.setZero();
   hessian.setZero();
 
+  probability_ = 0;
   // Iterate over correspondence lines
   for (auto &data_line : data_lines_) {
     // Calculate point coordinates in camera frame
@@ -356,9 +357,13 @@ bool RegionModality::CalculatePoseUpdate(int corr_iteration,
     ddelta_cs_dtheta /= data_line.standard_deviation;
     hessian.triangularView<Eigen::Lower>() -=
         ddelta_cs_dtheta.transpose() * ddelta_cs_dtheta;
+    
+    probability_ += data_line.distribution[distribution_length_plus_1_half_];
   }
   hessian = hessian.selfadjointView<Eigen::Lower>();
 
+  probability_ /= std::max(int(data_lines_.size()), 1); // mean probability
+
   // Optimize and update pose
   Eigen::FullPivLU<Eigen::Matrix<float, 6, 6>> lu{tikhonov_matrix_ - hessian};
   if (lu.isInvertible()) {