File improve_model_precision.patch of Package pcl

Overview Repositories Revisions Requests Users Attributes Meta

File improve_model_precision.patch of Package pcl (Revision 0558bf018e5e3e29ca0d36702f7619af)

Currently displaying revision 0558bf018e5e3e29ca0d36702f7619af , Show latest

From f1a5291d7f9bf10ffed9921ef267a93f871df446 Mon Sep 17 00:00:00 2001
From: Markus Vieth <mvieth@techfak.uni-bielefeld.de>
Date: Fri, 27 May 2022 11:39:02 +0200
Subject: [PATCH] Change sphere model computation to double precision m11 to
 m15 can be quite small (around 1e-5), so small changes (noise) can result in
 very different model coefficients. Maybe we can find a different, more stable
 algorithm to compute the coefficients. But for now, simply switching to
 double is a sufficient solution.

---
 .../pcl/sample_consensus/impl/sac_model_sphere.hpp  | 13 +++++++------
 1 file changed, 7 insertions(+), 6 deletions(-)

diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_sphere.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_sphere.hpp
index 8bc1472d0d5..3b77afdb75b 100644
--- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_sphere.hpp
+++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_sphere.hpp
@@ -68,7 +68,8 @@ pcl::SampleConsensusModelSphere<PointT>::computeModelCoefficients (
     return (false);
   }
 
-  Eigen::Matrix4f temp;
+  // TODO maybe find a more stable algorithm for this?
+  Eigen::Matrix4d temp;
   for (int i = 0; i < 4; i++)
   {
     temp (i, 0) = (*input_)[samples[i]].x;
@@ -76,7 +77,7 @@ pcl::SampleConsensusModelSphere<PointT>::computeModelCoefficients (
     temp (i, 2) = (*input_)[samples[i]].z;
     temp (i, 3) = 1;
   }
-  float m11 = temp.determinant ();
+  const double m11 = temp.determinant ();
   if (m11 == 0)
   {
     return (false);             // the points don't define a sphere!
@@ -88,21 +89,21 @@ pcl::SampleConsensusModelSphere<PointT>::computeModelCoefficients (
                   ((*input_)[samples[i]].y) * ((*input_)[samples[i]].y) +
                   ((*input_)[samples[i]].z) * ((*input_)[samples[i]].z);
   }
-  float m12 = temp.determinant ();
+  const double m12 = temp.determinant ();
 
   for (int i = 0; i < 4; ++i)
   {
     temp (i, 1) = temp (i, 0);
     temp (i, 0) = (*input_)[samples[i]].x;
   }
-  float m13 = temp.determinant ();
+  const double m13 = temp.determinant ();
 
   for (int i = 0; i < 4; ++i)
   {
     temp (i, 2) = temp (i, 1);
     temp (i, 1) = (*input_)[samples[i]].y;
   }
-  float m14 = temp.determinant ();
+  const double m14 = temp.determinant ();
 
   for (int i = 0; i < 4; ++i)
   {
@@ -111,7 +112,7 @@ pcl::SampleConsensusModelSphere<PointT>::computeModelCoefficients (
     temp (i, 2) = (*input_)[samples[i]].y;
     temp (i, 3) = (*input_)[samples[i]].z;
   }
-  float m15 = temp.determinant ();
+  const double m15 = temp.determinant ();
 
   // Center (x , y, z)
   model_coefficients.resize (model_size_);

Places

File improve_model_precision.patch of Package pcl (Revision 0558bf018e5e3e29ca0d36702f7619af)

Places