Fixed sac_model_plane to make sure a valid plane is optimized

PointCloudLibrary · Jul 11, 2013 · 02ca563 · 02ca563
1 parent 77f87e9
commit 02ca563
Showing 1 changed file with 6 additions and 0 deletions.
diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_plane.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_plane.hpp
@@ -248,6 +248,12 @@ pcl::SampleConsensusModelPlane<PointT>::optimizeModelCoefficients (
   optimized_coefficients[2] = eigen_vector [2];
   optimized_coefficients[3] = 0;
   optimized_coefficients[3] = -1 * optimized_coefficients.dot (xyz_centroid);
+
+  // Make sure it results in a valid model
+  if (!isModelValid (optimized_coefficients))
+  {
+    optimized_coefficients = model_coefficients;
+  }
 }
 
 //////////////////////////////////////////////////////////////////////////