Torus ransac model (#5816)

* First torus commit

* Torus ransac

* integrating projectPoints

* Implementing more functions

* Slight progress
 * Adding optimizeModelCoefficients
 * Adding an example to test during implementation
 * Fix a bit of the mess between Vector3 and Vector4 float   / double types

* Progress

* Optimizer now works!

* Important milestone:
  * optimizeModelCoefficients is ready
  * Moved the implementation towards a "normal based" approach
  * computeModelCoefficients is also ready

* Removes normals

* Reverting unnecessary changes..

* Several changes:

  * Removing torus example to test
  * Unsupressing errors, fixing them but some things are still not adressed

* Restores unrequired file modifications

* Restoring unnecessary modifications

* Updating doxygen

* Adds clangformat to new files

* Implements a direct approach to torus SAC, with normals

* Cleans up a bit, fixes some implementation issues

* Fixes equation, tests are now passing

* Fixed some bugs, added tests, cleanup

* Fixes clang-format

* Implements isModelValid

* implements doSamplesVerifyModel

* Adds more tests, adds normals to ML optimizer

* Adds more tests, adds normals to ML optimizer

* Rolls back formatting, sorry

* Review day 1

* Fixes closest

* Review stage + cleanup

* Adds const correctness to variables

* Corrects a silly mistake

* Normalize direction after optimizemodelcoeffs

* Format review

Author: lasdasdas

sample_consensus/CMakeLists.txt

@@ -27,6 +27,7 @@ set(srcs
   src/sac_model_registration.cpp
   src/sac_model_sphere.cpp
   src/sac_model_ellipse3d.cpp
+  src/sac_model_torus.cpp
 )
 
 set(incs
@@ -58,6 +59,7 @@ set(incs
   "include/pcl/${SUBSYS_NAME}/sac_model_registration_2d.h"
   "include/pcl/${SUBSYS_NAME}/sac_model_sphere.h"
   "include/pcl/${SUBSYS_NAME}/sac_model_ellipse3d.h"
+  "include/pcl/${SUBSYS_NAME}/sac_model_torus.h"
 )
 
 set(impl_incs
@@ -85,6 +87,7 @@ set(impl_incs
   "include/pcl/${SUBSYS_NAME}/impl/sac_model_registration_2d.hpp"
   "include/pcl/${SUBSYS_NAME}/impl/sac_model_sphere.hpp"
   "include/pcl/${SUBSYS_NAME}/impl/sac_model_ellipse3d.hpp"
+  "include/pcl/${SUBSYS_NAME}/impl/sac_model_torus.hpp"
 )
 
 set(LIB_NAME "pcl_${SUBSYS_NAME}")