SRT3D: compute mean probability for region modality

Author: paroj

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();

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()) {