further work on Ptcloud module

modules/ptcloud/CMakeLists.txt

@@ -0,0 +1,2 @@
+set(the_description "Point cloud functionality.")
+ocv_define_module(ptcloud opencv_core opencv_calib3d opencv_surface_matching OPTIONAL opencv_viz WRAP python)

modules/ptcloud/README.md

@@ -0,0 +1,35 @@
+//! @addtogroup ptcloud
+//! @{
+
+Point Cloud Module, Object Fitting API
+=======================================
+
+Try to segment geometric primitives like planes, spheres and cylinders from a 3d point cloud
+
+2 alternative ransac strategies are implemented here:
+
+- plain ransac loop:
+  + generate random minimal hypothesis
+  + find inliers for the model,
+    - bail out if at 1/4 of the data it does not have more than 1/8 inliers of the current best model
+    - if sprt is used, bail out if the probability of finding inliers goes low
+  + if this model is the current best one
+    - best model = current
+    - update stopping criterion (optional SPRT)
+    - apply local optimization (generate a non-minimal model from the inliers)
+      + if it improved, take that instead
+
+- preemptive ransac loop:
+  + generate M minimal random models in advance
+  + slice the data into blocks(of size B), for each block:
+    - evaluate all M models in parallel
+    - sort descending (by inliers or accumulated distance)
+    - prune model list, M' = M * (2 ^ -(i/B))
+    - stop if there is only one model left, or the last data block reached
+  + polish/optimize the last remaining model
+
+To Do
+-----------------------------------------
+- Integrate (better) with Maksym's work
+
+//! @}

modules/ptcloud/doc/ptcloud.bib

@@ -0,0 +1,33 @@
+@article{FB81
+  title={Random sample consensus:  A paradigm for model fitting with applications to image analysis and automated cartography},
+  author={Fischler, M. A. and Bolles, R.},
+  journal={Communications of the ACM},
+  year={1981},
+  volume={24},
+  number={2},
+  pages={381-395}
+}
+
+@inproceedings{NI03
+  title={Preemptive RANSAC for live structure and motion estimation},
+  author={Nister, D.},
+  booktitle={International Conference on Computer Vision (ICCV)},
+  year={2003}
+}
+
+@article{USAC13
+  title={USAC: A Universal Framework for Random Sample Consensus},
+  author={Raguram,Rahul and Chum,Ondrej and Pollefeys,Marc and Matas,Jiri and Frahm,Jan-Michael},
+  journal={IEEE Transactions on Software Engineering},
+  year={2013},
+  organization={IEEE}
+}
+
+@article{SPRT05
+  title={Randomized RANSAC with Sequential Probability Ratio Test},
+  author={Matas,Jirı and Chum,Ondrej},
+  booktitle={International Conference on Computer Vision (ICCV)},
+  year={2005},
+  volume={2},
+  pages={1727–1732}
+}

modules/ptcloud/include/opencv2/ptcloud.hpp

@@ -0,0 +1,10 @@
+// This file is part of OpenCV project.
+// It is subject to the license terms in the LICENSE file found in the top-level directory
+// of this distribution and at http://opencv.org/license.html.
+
+#ifndef OPENCV_PTCLOUD_HPP
+#define OPENCV_PTCLOUD_HPP
+
+#include "opencv2/ptcloud/sac_segmentation.hpp"
+
+#endif

modules/ptcloud/include/opencv2/ptcloud/sac_segmentation.hpp

@@ -0,0 +1,192 @@
+// This file is part of OpenCV project.
+// It is subject to the license terms in the LICENSE file found in the top-level directory
+// of this distribution and at http://opencv.org/license.html.
+
+#ifndef OPENCV_PTCLOUD_SAC_SEGMENTATION
+#define OPENCV_PTCLOUD_SAC_SEGMENTATION
+
+#include <vector>
+#include <opencv2/core.hpp>
+
+
+namespace cv
+{
+namespace ptcloud
+{
+//! @addtogroup ptcloud
+//! @{
+
+enum SacModelType {
+    PLANE_MODEL = 1,
+    SPHERE_MODEL = 2,
+    CYLINDER_MODEL = 3,
+    BLOB_MODEL = 4
+};
+
+//
+//! follows ScoreMethod from calib3d
+//
+enum SacMethodType {
+    SAC_METHOD_RANSAC = 0,     //!< maximize inlier count
+    SAC_METHOD_MSAC   = 1      //!< minimize inlier distance
+};
+
+typedef std::pair<double, double> SACScore;
+
+/** @brief structure to hold the segmentation results
+*/
+class CV_EXPORTS_W SACModel {
+public:
+    CV_PROP std::vector<double> coefficients; //!< the "model"
+    CV_PROP std::vector<int> indices; //!< into the original cloud
+    CV_PROP std::vector<Point3f> points; //!< (output) copies (original cloud might no more exist)
+    CV_PROP int type; //!< see SacModelType
+    CV_PROP SACScore score; //!< first:inlier_count(ransac), second:accumulated distance(msac)
+
+    CV_WRAP SACModel(int typ=PLANE_MODEL);
+};
+
+
+/**  @brief The SACModelFitting class to segment geometric primitives like planes,spheres,cylinders from 3d point clouds.
+
+2 alternative ransac strategies are implemented here:
+
+- ransac loop:
+  + generate random minimal hypothesis
+  + find inliers for the model,
+    - bail out if at 1/4 of the data it does not have more than 1/8 inliers of the current best model
+    - if sprt is enabled, bail out if the accumulated probability of a bad model crosses a threshold
+  + if this model is the current best one
+    - best model = current
+    - update stopping criterion (using sprt if enabled)
+    - apply local optimization (generate a non-minimal model from the inliers)
+      + if it improved, take that instead
+
+- preemptive ransac loop:
+  + generate M minimal random models in advance
+  + slice the data into blocks(of size B), for each block:
+    - evaluate all M models in parallel
+    - sort descending (by inliers or accumulated distance)
+    - prune model list, M' = M * (2 ^ -(i/B))
+    - stop if there is only one model left, or the last data block reached
+  + polish/optimize the last remaining model
+*/
+class CV_EXPORTS_W SACModelFitting {
+public:
+    virtual ~SACModelFitting() {}
+
+    /** @brief set a new point cloud to segment
+
+    @param cloud either a 3channel or 1 channel / 3cols or rows Mat
+    @param with_normals if enabled, the cloud should have either 6 rows or 6 cols, and a single channel
+    */
+    CV_WRAP virtual void set_cloud(InputArray cloud, bool with_normals=false) = 0;
+
+    /** @brief Set the type of model to be fitted
+
+    @param model_type see SacModelType enum.
+    */
+    CV_WRAP virtual void set_model_type(int model_type) = 0;
+
+    /** @brief Set the type of ransac method
+
+    @param method_type see SacMethodType enum.
+    */
+    CV_WRAP virtual void set_method_type(int method_type) = 0;
+
+    /** @brief Use Wald's Sequential Probability Ratio Test with ransac fitting
+
+    This will result in less iterations and less evaluated data points, and thus be
+    much faster, but it might miss some inliers
+    (not used in the preemptive ransac)
+    @param sprt true or false.
+    */
+    CV_WRAP virtual void set_use_sprt(bool sprt) = 0;
+
+    /** @brief Set the threshold for the fitting.
+    The threshold is usually the distance from the boundary of model, but may vary from model to model.
+
+    This may be helpful when multiple fitting operations are to be performed.
+    @param threshold the threshold to set.
+    */
+    CV_WRAP virtual void set_threshold(double threshold) = 0;
+
+    /** @brief Set the number of iterations for the (non preemptive) ransac fitting.
+
+    This may be helpful when multiple fitting operations are to be performed.
+    @param iterations the threshold to set.
+    */
+    CV_WRAP virtual void set_iterations(int iterations) = 0;
+
+    /** @brief Set the weight given to normal alignment before comparing overall error with threshold.
+
+    By default it is set to 0.
+    @param weight the desired normal alignment weight (between 0 to 1).
+    */
+    CV_WRAP virtual void set_normal_distance_weight(double weight) = 0;
+
+    /** @brief Set the maximal radius for the sphere hypothesis generation.
+
+    A radius larger than this is considered degenerate.
+    @param max_radius the maximum valid sphere radius.
+    */
+    CV_WRAP virtual void set_max_sphere_radius(double max_radius) = 0;
+
+    /** @brief Set the maximal radius for the (optional) napsac hypothesis sampling.
+
+    Assume good hypothesis inliers are locally close to each other
+    (enforce spatial proximity for hypothesis inliers).
+
+    @param max_radius the maximum valid sphere radius for napsac sampling. set it to 0 to disable this strategy, and sample uniformly instead.
+    */
+    CV_WRAP virtual void set_max_napsac_radius(double max_radius) = 0;
+
+    /**
+    @param preemptive the number of models generated with preemptive ransac.
+        set to 0 to disable preemptive hypothesis generation and do plain ransac instead
+    */
+    CV_WRAP virtual void set_preemptive_count(int preemptive) = 0;
+
+    /**
+    @param min_inliers reject a model if it has less inliers than this.
+    */
+    CV_WRAP virtual void set_min_inliers(int min_inliers) = 0;
+
+    /** @brief Segment multiple models of the same type, this function would get the best fitting models on the given set of points.
+
+    This stores the models in the model_instances, and optionally, the remaining cloud points.
+
+    @param model_instances a vector of SACModels.
+    @param new_cloud (optional) the remaining non-segmented cloud points.
+    */
+    CV_WRAP virtual void segment(CV_OUT std::vector<SACModel> &model_instances, OutputArray new_cloud=noArray()) = 0;
+
+    /** @brief Initializes a SACModelFitting instance.
+
+    @param cloud input Point Cloud.
+    @param model_type type of model fitting to attempt - values can be either PLANE_MODEL, SPHERE_MODEL, or CYLINDER_MODEL.
+    @param method_type which method to use - (use value SAC_METHOD_RANSAC or SAC_METHOD_MSAC).
+    @param threshold set the threshold while choosing inliers.
+    @param max_iters number of iterations for Sampling.
+    */
+    CV_WRAP static Ptr<SACModelFitting> create(InputArray cloud, int model_type = PLANE_MODEL, int method_type = SAC_METHOD_RANSAC, double threshold = 20, int max_iters = 1000);
+};
+
+
+/** @brief Cluster (remaining) points into blobs
+
+    This is using cv::partition() internally to seperate blobs.
+
+    @param cloud the input point cloud
+    @param distance max distance to the next inlier point in the blob
+    @param min_inliers reject blobs with less inliers than this
+    @param models a vector of SACModels to hold the resulting blobs
+    @param new_cloud optionally return non segmented points
+*/
+CV_EXPORTS_W void cluster(InputArray cloud, double distance, int min_inliers, CV_OUT std::vector<SACModel> &models, OutputArray new_cloud=noArray());
+
+} // ptcloud
+} // cv
+
+
+#endif

modules/ptcloud/include/opencv2/ptcloud/sac_utils.hpp

@@ -0,0 +1,38 @@
+#ifndef OPENCV_SAC_IMPL_HPP
+#define OPENCV_SAC_IMPL_HPP
+
+#include "opencv2/calib3d.hpp"
+#include "opencv2/ptcloud/sac_segmentation.hpp"
+
+namespace cv {
+namespace ptcloud {
+
+// testing & samples
+CV_EXPORTS void generatePlane(Mat &cloud, const std::vector<double> &coeffs, int N=256);
+CV_EXPORTS void generateSphere(Mat &cloud, const std::vector<double> &coeffs, int N=256);
+CV_EXPORTS void generateCylinder(Mat &cloud, const std::vector<double> &coeffs, int N=256);
+CV_EXPORTS void generateRandom(Mat &cloud, const std::vector<double> &coeffs, int N=256);
+
+
+// for testing
+struct CV_EXPORTS SPRT {
+	virtual ~SPRT() {}
+    virtual bool addDataPoint(int tested_point, double error) = 0;
+    virtual bool isModelGood(int tested_point) = 0;
+    virtual int update (int inlier_size) = 0;
+    virtual SACScore getScore() = 0;
+
+    static Ptr<SPRT> create(int state, int points_size_,
+          double inlier_threshold_, double prob_pt_of_good_model, double prob_pt_of_bad_model,
+          double time_sample, double avg_num_models, int sample_size_, int max_iterations_, ScoreMethod score_type_);
+};
+
+CV_EXPORTS Mat generateNormals(const Mat &cloud);
+CV_EXPORTS std::vector<double> optimizeModel(int model_type, const Mat &cloud, const std::vector<int> &inliers_indices, const std::vector<double> &old_coeff);
+CV_EXPORTS SACScore getInliers(int model_type, const std::vector<double> &coeffs, const Mat &cloud, const Mat &normals, const std::vector<int> &indices, const double threshold, const double best_inliers, std::vector<int>& inliers, double normal_distance_weight_, Ptr<SPRT> sprt=Ptr<SPRT>());
+CV_EXPORTS bool generateHypothesis(int model_type, const Mat &cloud, const std::vector<int> &current_model_inliers, const Mat &normals, double max_radius, double normal_distance_weight_, std::vector<double> &coefficients);
+
+} // ptcloud
+} // cv
+
+#endif // OPENCV_SAC_IMPL_HPP

modules/ptcloud/misc/python/pyopencv_ptcloud.hpp

@@ -0,0 +1,34 @@
+#include "opencv2/ptcloud/sac_segmentation.hpp"
+
+// SACScore
+template<>
+PyObject* pyopencv_from(const std::pair<double, double>& src)
+{
+    return Py_BuildValue("(dd)", src.first, src.second);
+}
+
+// we have a vector of structs in c++, but python needs a list of Ptrs
+// (in the end, this makes dreadful copies of anything)
+template<>
+PyObject* pyopencv_from(const std::vector<cv::ptcloud::SACModel>& src)
+{
+    int i, n = (int)src.size();
+    PyObject* seq = PyList_New(n);
+    for( i = 0; i < n; i++ )
+    {
+        Ptr<ptcloud::SACModel> ptr(new ptcloud::SACModel());
+        *ptr = src[i];
+        PyObject* item = pyopencv_from(ptr);
+        if(!item)
+            break;
+        PyList_SetItem(seq, i, item);
+    }
+    if( i < n )
+    {
+        Py_DECREF(seq);
+        return 0;
+    }
+    return seq;
+}
+
+typedef std::vector<ptcloud::SACModel> vector_SACModel;

modules/ptcloud/misc/python/test/test_sac.py

@@ -0,0 +1,35 @@
+#!/usr/bin/env python
+
+'''
+sac segmentation
+'''
+
+# Python 2/3 compatibility
+from __future__ import print_function
+
+import numpy as np
+import cv2 as cv
+
+from tests_common import NewOpenCVTests
+
+class sac_test(NewOpenCVTests):
+
+    def test_plane(self):
+        N = 64;
+        plane = np.zeros((N,N,3),np.float32)
+        for i in range(0,N):
+            for j in range(0,N):
+                 plane[i,j] = (i,j,0)
+
+        fit = cv.ptcloud.SACModelFitting_create(plane)
+        mdl,left = fit.segment()
+
+        self.assertEqual(len(mdl), 1)
+        self.assertEqual(len(mdl[0].indices), N*N)
+        self.assertEqual(len(mdl[0].points), N*N)
+        self.assertEqual(len(mdl[0].coefficients), 4)
+        self.assertEqual(np.shape(left), ())
+
+
+if __name__ == '__main__':
+    NewOpenCVTests.bootstrap()

modules/ptcloud/perf/perf_main.cpp

@@ -0,0 +1,7 @@
+// This file is part of OpenCV project.
+// It is subject to the license terms in the LICENSE file found in the top-level directory
+// of this distribution and at http://opencv.org/license.html.
+
+#include "perf_precomp.hpp"
+
+CV_PERF_TEST_MAIN(ptcloud)