corrected build issue and added sample data and code

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

@@ -24,16 +24,17 @@ namespace ptcloud
         public:
             std::vector<double> ModelCoefficients;
             // vector<Point3f> inliers;
-            SACModel();
+            SACModel() {
+
+            }
+
             SACModel(std::vector<double> ModelCoefficients);
 
             virtual ~SACModel()
             {
 
             }
-            // virtual viz::Widget3D WindowWidget () = 0;
 
-            virtual void getModelFromPoints(Mat inliers);
     };
 
     class CV_EXPORTS_W SACPlaneModel : public SACModel {
@@ -42,9 +43,14 @@ namespace ptcloud
             Vec3d normal;
             Size2d size = Size2d(2.0, 2.0);
         public:
-            SACPlaneModel();
+            ~ SACPlaneModel()
+            {
 
-            SACPlaneModel(const std::vector<double> Coefficients);
+            }
+
+            SACPlaneModel() {
+
+            }
 
             SACPlaneModel(Vec4d coefficients, Point3d center, Size2d size=Size2d(2.0, 2.0));
 
@@ -63,7 +69,13 @@ namespace ptcloud
             Point3d center;
             double radius;
 
+            ~ SACSphereModel()
+            {
+
+            }
+
             SACSphereModel() {
+
             }
 
             SACSphereModel(Point3d center, double radius);
@@ -88,7 +100,9 @@ namespace ptcloud
             long unsigned max_iters;
 
         public:
-            cv::Mat remainingCloud;
+            // cv::Mat remainingCloud; // will be used while segmentation
+
+            // Inlier indices only, not the points themselves. It would work like a mask output for segmentation in 2d.
             vector<vector<unsigned>> inliers;
             vector<SACModel> model_instances;
 
@@ -101,7 +115,11 @@ namespace ptcloud
 
             // Get one model (plane), this function would call RANSAC on the given set of points and get the biggest model (plane).
             void fit_once();
-            };
+
+            void set_threshold (double);
+
+            void set_iterations (long unsigned);
+    };
 
     bool getSphereFromPoints(const Vec3f*&, const vector<unsigned int>&, Point3d&, double&);
 

modules/ptcloud/samples/sample_plane.cpp

@@ -0,0 +1,49 @@
+#include <opencv2/viz.hpp>
+#include <opencv2/imgproc.hpp>
+#include <opencv2/highgui.hpp>
+#include <opencv2/viz/widgets.hpp>
+#include <opencv2/ptcloud.hpp>
+#include <cassert>
+#include <numeric>
+#include <cmath>
+#include <string>
+
+using namespace cv;
+using namespace std;
+
+int main() {
+     Mat cloud = cv::viz::readCloud("./data/CobbleStones.obj");
+    cv::ptcloud::SACModelFitting planar_segmentation(cloud);
+
+    // add original cloud to window
+    viz::Viz3d window("original cloud");
+    viz::WCloud original_cloud(cloud);
+    window.showWidget("cloud", original_cloud);
+
+
+    planar_segmentation.set_threshold(0.001);
+    planar_segmentation.set_iterations(1000);
+    planar_segmentation.fit_once();
+
+    // Adds segmented (int this case fit, since only once) plane to window
+
+    const Vec3f* points = cloud.ptr<Vec3f>(0);
+    vector<unsigned> inlier_vec =  planar_segmentation.inliers.at(0);
+    cv::Mat fit_cloud(1, inlier_vec.size(), CV_32FC3);
+    for(int j=0; j<fit_cloud.cols; ++j)
+        fit_cloud.at<Vec3f>(0, j) = points[inlier_vec.at(j)];
+
+    viz::Viz3d fitted("fitted cloud");
+    viz::WCloud cloud_widget2(fit_cloud, viz::Color::green());
+    fitted.showWidget("fit plane", cloud_widget2);
+
+    window.showWidget("fit plane", cloud_widget2);
+
+    vector<double> model_coefficients = planar_segmentation.model_instances.at(0).ModelCoefficients;
+    cv::ptcloud::SACPlaneModel SACplane (model_coefficients);
+
+    window.spin();
+    fitted.spin();
+    waitKey(1);
+
+}

modules/ptcloud/samples/sample_sphere.cpp

@@ -0,0 +1,43 @@
+#include <opencv2/viz.hpp>
+#include <opencv2/imgproc.hpp>
+#include <opencv2/highgui.hpp>
+#include <opencv2/viz/widgets.hpp>
+#include <opencv2/ptcloud.hpp>
+#include <cassert>
+#include <numeric>
+#include <cmath>
+#include <iostream>
+#include <string>
+
+using namespace cv;
+using namespace std;
+
+int main() {
+    Mat cloud = cv::viz::readCloud("./data/sphere-big.obj");
+    cv::ptcloud::SACModelFitting sphere_segmentation(cloud, 2);
+
+    /// Adds original cloud to window
+    viz::Viz3d window("original cloud");
+    viz::WCloud cloud_widget1(cloud);
+    window.showWidget("cloud 1", cloud_widget1);
+
+    sphere_segmentation.set_threshold(0.001);
+    sphere_segmentation.set_iterations(10000);
+
+    viz::Viz3d fitted("fitted cloud");
+
+    sphere_segmentation.fit_once();
+    vector<double> model_coefficients = sphere_segmentation.model_instances.at(0).ModelCoefficients;
+    cout << sphere_segmentation.model_instances.at(0).ModelCoefficients.size();
+    cv::ptcloud::SACSphereModel sphere (model_coefficients);
+        cout << sphere.center;
+        cout << sphere.radius;
+    sphere.radius *= 0.75;
+
+    viz::WSphere model = sphere.WindowWidget();
+    window.showWidget("model", model);
+
+    window.spin();
+    waitKey(1);
+
+}

modules/ptcloud/src/sac_segmentation.cpp

@@ -138,32 +138,6 @@ namespace ptcloud
         return coefficients;
     }
 
-
-
-    SACPlaneModel::SACPlaneModel(vector<double> Coefficients ) {
-        this->ModelCoefficients.reserve(4);
-        this->ModelCoefficients = Coefficients;
-
-        for (unsigned i = 0; i < 3; i++) normal[i] = Coefficients[i];
-
-        // Since the plane viz widget would be finite, it must have a center, we give it an arbitrary center
-        // from the model coefficients.
-        if (Coefficients[2] != 0) {
-            center.x = 0;
-            center.y = 0;
-            center.z = -Coefficients[3] / Coefficients[2];
-        } else if (Coefficients[1] != 0) {
-            center.x = 0;
-            center.y = -Coefficients[3] / Coefficients[1];
-            center.z = 0;
-        } else if (Coefficients[0] != 0) {
-            center.x = -Coefficients[3] / Coefficients[0];
-            center.y = 0;
-            center.z = 0;
-        }
-
-    }
-
     SACPlaneModel::SACPlaneModel(Vec4d coefficients, Point3d set_center, Size2d set_size) {
         this->ModelCoefficients.reserve(4);
         for (int i = 0; i < 4; i++) {
@@ -212,9 +186,21 @@ namespace ptcloud
         // Assign normal vector
         for (unsigned i = 0; i < 3; i++) normal[i] = coefficients[i];
 
-        center.x = 0;
-        center.y = 0;
-        center.z = 0;
+        // Since the plane viz widget would be finite, it must have a center, we give it an arbitrary center
+        // from the model coefficients.
+        if (coefficients[2] != 0) {
+            center.x = 0;
+            center.y = 0;
+            center.z = -coefficients[3] / coefficients[2];
+        } else if (coefficients[1] != 0) {
+            center.x = 0;
+            center.y = -coefficients[3] / coefficients[1];
+            center.z = 0;
+        } else if (coefficients[0] != 0) {
+            center.x = -coefficients[3] / coefficients[0];
+            center.y = 0;
+            center.z = 0;
+        }
     }
     // void SACPlaneModel::addToWindow(viz::Viz3d & window) {
     //     viz::WPlane plane(this->center, this->normal, Vec3d(1, 0, 0), this->size, viz::Color::green());
@@ -301,7 +287,7 @@ namespace ptcloud
         return viz::WSphere(this->center, this->radius, 10, viz::Color::green());;
     }
 
-    double euclideanDist(Point3d& p, Point3d& q) {
+    double SACSphereModel::euclideanDist(Point3d& p, Point3d& q) {
         Point3d diff = p - q;
         return cv::sqrt(diff.x*diff.x + diff.y*diff.y + diff.z*diff.z);
     }
@@ -352,91 +338,101 @@ namespace ptcloud
 
     void SACModelFitting::fit_once() {
 
-            // creates an array of indices for the points in the point cloud which will be appended as masks to denote inliers and outliers.
-            const Vec3f* points = cloud.ptr<Vec3f>(0);
-            unsigned num_points = cloud.cols;
-            std::vector<unsigned> indices(num_points);
-            std::iota(std::begin(indices), std::end(indices), 0);
+        if (method_type != SAC_METHOD_RANSAC) return; // Only RANSAC supported ATM, need to integrate with Maksym's framework.
 
+        // creates an array of indices for the points in the point cloud which will be appended as masks to denote inliers and outliers.
+        const Vec3f* points = cloud.ptr<Vec3f>(0);
+        unsigned num_points = cloud.cols;
+        std::vector<unsigned> indices(num_points);
+        std::iota(std::begin(indices), std::end(indices), 0);
 
-            vector<unsigned> inliers_indices;
 
-            // Initialize the best plane model.
-            SACModel bestModel;
-            pair<double, double> bestResult(0, 0);
+        vector<unsigned> inliers_indices;
 
-            if (model_type == PLANE_MODEL) {
-                const unsigned num_rnd_model_points = 3;
-                RNG rng((uint64)-1);
-                for (unsigned i = 0; i < max_iters; ++i) {
-                    vector<unsigned> current_model_inliers;
-                    SACModel model;
+        // Initialize the best plane model.
+        SACModel bestModel;
+        pair<double, double> bestResult(0, 0);
 
-                    for (unsigned j = 0; j < num_rnd_model_points; ++j) {
-                        std::swap(indices[i], indices[rng.uniform(0, num_points)]);
-                    }
+        if (model_type == PLANE_MODEL) {
+            const unsigned num_rnd_model_points = 3;
+            RNG rng((uint64)-1);
+            for (unsigned i = 0; i < max_iters; ++i) {
+                vector<unsigned> current_model_inliers;
+                SACModel model;
 
-                    for (unsigned j = 0; j < num_rnd_model_points; j++) {
-                        unsigned idx = indices[i];
-                        current_model_inliers.emplace_back(idx);
-                    }
+                for (unsigned j = 0; j < num_rnd_model_points; ++j) {
+                    std::swap(indices[i], indices[rng.uniform(0, num_points)]);
+                }
 
-                    Point3d center;
-                    Vec4d coefficients = getPlaneFromPoints(points, current_model_inliers, center);
-                    SACPlaneModel planeModel (coefficients, center);
-                    pair<double, double> result = planeModel.getInliers(cloud, indices, threshold, current_model_inliers);
+                for (unsigned j = 0; j < num_rnd_model_points; j++) {
+                    unsigned idx = indices[i];
+                    current_model_inliers.emplace_back(idx);
+                }
 
-                    // Compare fitness first.
-                    if (bestResult.first < result.first || (bestResult.first == result.first && bestResult.second > result.second )) {
-                        bestResult = result;
-                        bestModel.ModelCoefficients = planeModel.ModelCoefficients;
-                        inliers_indices = current_model_inliers;
-                    }
+                Point3d center;
+                Vec4d coefficients = getPlaneFromPoints(points, current_model_inliers, center);
+                SACPlaneModel planeModel (coefficients, center);
+                pair<double, double> result = planeModel.getInliers(cloud, indices, threshold, current_model_inliers);
 
+                // Compare fitness first.
+                if (bestResult.first < result.first || (bestResult.first == result.first && bestResult.second > result.second )) {
+                    bestResult = result;
+                    bestModel.ModelCoefficients = planeModel.ModelCoefficients;
+                    inliers_indices = current_model_inliers;
                 }
-                inliers.push_back(inliers_indices);
-                model_instances.push_back(bestModel);
+
             }
+            inliers.push_back(inliers_indices);
+            model_instances.push_back(bestModel);
+        }
+
+        if (model_type == SPHERE_MODEL) {
+            RNG rng((uint64)-1);
+            const unsigned num_rnd_model_points = 4;
+            double bestRadius = 10000000;
+            for (unsigned i = 0; i < max_iters; ++i) {
+                vector<unsigned> current_model_inliers;
+                SACModel model;
+
+                for (unsigned j = 0; j < num_rnd_model_points; ++j) {
+                    std::swap(indices[i], indices[rng.uniform(0, num_points)]);
+                }
 
-            if (model_type == SPHERE_MODEL) {
-                RNG rng((uint64)-1);
-                const unsigned num_rnd_model_points = 4;
-                double bestRadius = 10000000;
-                for (unsigned i = 0; i < max_iters; ++i) {
-                    vector<unsigned> current_model_inliers;
-                    SACModel model;
-
-                    for (unsigned j = 0; j < num_rnd_model_points; ++j) {
-                        std::swap(indices[i], indices[rng.uniform(0, num_points)]);
-                    }
-
-                    for (unsigned j = 0; j < num_rnd_model_points; j++) {
-                        unsigned idx = indices[i];
-                        current_model_inliers.emplace_back(idx);
-                    }
-
-                    Point3d center;
-                    double radius;
-
-                    getSphereFromPoints(points, current_model_inliers, center, radius);
-                    SACSphereModel sphereModel (center, radius);
-                    pair<double, double> result = sphereModel.getInliers(cloud, indices, threshold, current_model_inliers);
-                    // Compare fitness first.
-                    if (bestResult.first < result.first || (bestResult.first == result.first && bestResult.second > result.second)
-                        || (bestResult.first == result.first)) {
-
-                        if (bestResult.first == result.first && bestModel.ModelCoefficients.size() == 4 && sphereModel.radius > bestRadius) continue;
-                        bestResult = result;
-                        bestModel.ModelCoefficients = sphereModel.ModelCoefficients;
-                        bestModel.ModelCoefficients = sphereModel.ModelCoefficients;
-                        inliers_indices = current_model_inliers;
-                    }
+                for (unsigned j = 0; j < num_rnd_model_points; j++) {
+                    unsigned idx = indices[i];
+                    current_model_inliers.emplace_back(idx);
+                }
 
+                Point3d center;
+                double radius;
+
+                getSphereFromPoints(points, current_model_inliers, center, radius);
+                SACSphereModel sphereModel (center, radius);
+                pair<double, double> result = sphereModel.getInliers(cloud, indices, threshold, current_model_inliers);
+                // Compare fitness first.
+                if (bestResult.first < result.first || (bestResult.first == result.first && bestResult.second > result.second)
+                    || (bestResult.first == result.first)) {
+
+                    if (bestResult.first == result.first && bestModel.ModelCoefficients.size() == 4 && sphereModel.radius > bestRadius) continue;
+                    bestResult = result;
+                    bestModel.ModelCoefficients = sphereModel.ModelCoefficients;
+                    bestModel.ModelCoefficients = sphereModel.ModelCoefficients;
+                    inliers_indices = current_model_inliers;
                 }
-                inliers.push_back(inliers_indices);
-                model_instances.push_back(bestModel);
+
             }
+            inliers.push_back(inliers_indices);
+            model_instances.push_back(bestModel);
         }
+    }
+
+    void SACModelFitting::set_threshold (double threshold_value) {
+        threshold = threshold_value;
+    }
+
+    void SACModelFitting::set_iterations (long unsigned iterations) {
+        max_iters = iterations;
+    }
 
 } // ptcloud
 }   // cv