initial commit for ptcloud object fitting

modules/ptcloud/CMakeLists.txt

@@ -0,0 +1,9 @@
+set(the_description "Point Cloud Object Fitting API")
+ocv_define_module(ptcloud opencv_core opencv_viz WRAP python)
+
+# add test data from samples dir to contrib/ptcloud
+ocv_add_testdata(samples/ contrib/ptcloud FILES_MATCHING PATTERN "*.ply")
+
+# add data point cloud files to installation
+file(GLOB POINTCLOUD_DATA samples/*.ply)
+install(FILES ${POINTCLOUD_DATA} DESTINATION ${OPENCV_OTHER_INSTALL_PATH}/ptcloud COMPONENT libs)

modules/ptcloud/README.md

@@ -0,0 +1,14 @@
+//! @addtogroup ptcloud
+//! @{
+
+Point Cloud Module, Object Fitting API
+=======================================
+
+
+To Do
+-----------------------------------------
+- Cylinder Model Fitting
+- Segmentation
+- Integrate with Maksym's work
+
+//! @}

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,114 @@
+// 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 <utility>
+#include "opencv2/viz.hpp"
+#define PLANE_MODEL 1
+#define SPHERE_MODEL 2
+#define CYLINDER_MODEL 3
+#define SAC_METHOD_RANSAC 1
+using namespace std;
+
+namespace cv
+{
+namespace ptcloud
+{
+    //! @addtogroup ptcloud
+    //! @{
+    class CV_EXPORTS_W SACModel: public Algorithm {
+        public:
+            std::vector<double> ModelCoefficients;
+            // vector<Point3f> inliers;
+            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 {
+        private:
+            Point3d center;
+            Vec3d normal;
+            Size2d size = Size2d(2.0, 2.0);
+        public:
+            SACPlaneModel();
+
+            SACPlaneModel(const std::vector<double> Coefficients);
+
+            SACPlaneModel(Vec4d coefficients, Point3d center, Size2d size=Size2d(2.0, 2.0));
+
+            SACPlaneModel(Vec4d coefficients, Size2d size=Size2d(2.0, 2.0));
+
+            SACPlaneModel(std::vector<double> coefficients, Size2d size=Size2d(2.0, 2.0));
+
+            viz::WPlane WindowWidget ();
+
+            std::pair<double, double> getInliers(Mat cloud, std::vector<unsigned> indices, const double threshold, std::vector<unsigned>& inliers);
+    };
+
+
+    class CV_EXPORTS_W SACSphereModel : public SACModel {
+        public:
+            Point3d center;
+            double radius;
+
+            SACSphereModel() {
+            }
+
+            SACSphereModel(Point3d center, double radius);
+
+            SACSphereModel(const std::vector<double> Coefficients);
+
+            SACSphereModel(Vec4d coefficients);
+            // SACSphereModel(std::vector<double> coefficients);
+            viz::WSphere WindowWidget ();
+
+            double euclideanDist(Point3d& p, Point3d& q);
+
+            std::pair<double, double> getInliers(Mat cloud, std::vector<unsigned> indices, const double threshold, std::vector<unsigned>& inliers);
+    };
+
+    class CV_EXPORTS_W SACModelFitting {
+        private:
+            Mat cloud;
+            int model_type;
+            int method_type;
+            double threshold;
+            long unsigned max_iters;
+
+        public:
+            cv::Mat remainingCloud;
+            vector<vector<unsigned>> inliers;
+            vector<SACModel> model_instances;
+
+            // viz::Viz3d window;
+            SACModelFitting (Mat cloud, int model_type = PLANE_MODEL, int method_type = SAC_METHOD_RANSAC, double threshold = 20,int max_iters = 1000);
+                // :cloud(cloud), model_type(model_type), method_type(method_type), threshold(threshold), max_iters(max_iters) {}
+
+            SACModelFitting (int model_type = PLANE_MODEL, int method_type = SAC_METHOD_RANSAC, double threshold = 20,int max_iters = 1000);
+                // :model_type(model_type), method_type(method_type), threshold(threshold), max_iters(max_iters) {}
+
+            // Get one model (plane), this function would call RANSAC on the given set of points and get the biggest model (plane).
+            void fit_once();
+            };
+
+    bool getSphereFromPoints(const Vec3f*&, const vector<unsigned int>&, Point3d&, double&);
+
+    Vec4d getPlaneFromPoints(const Vec3f*&, const std::vector<unsigned int>&, cv::Point3d&);
+
+    double euclideanDist(Point3d& p, Point3d& q);
+
+} // ptcloud
+}   // cv
+#endif

modules/ptcloud/src/precomp.hpp

@@ -0,0 +1,14 @@
+// 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_PRECOMP_HPP
+#define OPENCV_PTCLOUD_PRECOMP_HPP
+#include <opencv2/core.hpp>
+#include "opencv2/ptcloud/sac_segmentation.hpp"
+
+#include <iostream>
+#include <stdio.h>
+#include <vector>
+#include <algorithm>
+
+#endif

modules/ptcloud/src/sac_segmentation.cpp

@@ -0,0 +1,442 @@
+// 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 "precomp.hpp"
+#include <cassert>
+#include <numeric>
+
+
+using namespace std;
+using namespace cv;
+
+namespace cv
+{
+namespace ptcloud
+{
+
+        bool getSphereFromPoints(const Vec3f* &points, const std::vector<unsigned> &inliers, Point3d& center, double& radius) {
+        // assert that size of points is 3.
+        Mat temp(5,5,CV_32FC1);
+        // Vec4f temp;
+        for(int i = 0; i < 4; i++)
+        {
+            unsigned point_idx = inliers[i];
+            float* tempi = temp.ptr<float>(i);
+            for(int j = 0; j < 3; j++) {
+                tempi[j] = (float) points[point_idx][j];
+            }
+            tempi[3] = 1;
+        }
+        double m11 = determinant(temp);
+        if (m11 == 0) return false; // no sphere exists
+
+        for(int i = 0; i < 4; i++)
+        {
+            unsigned point_idx = inliers[i];
+            float* tempi = temp.ptr<float>(i);
+
+            tempi[0] = (float) points[point_idx][0] * (float) points[point_idx][0]
+                        + (float) points[point_idx][1] * (float) points[point_idx][1]
+                        + (float) points[point_idx][2] * (float) points[point_idx][2];
+
+        }
+        double m12 = determinant(temp);
+
+        for(int i = 0; i < 4; i++)
+        {
+            unsigned point_idx = inliers[i];
+            float* tempi = temp.ptr<float>(i);
+
+            tempi[1] = tempi[0];
+            tempi[0] = (float) points[point_idx][0];
+
+        }
+        double m13 = determinant(temp);
+
+        for(int i = 0; i < 4; i++)
+        {
+            unsigned point_idx = inliers[i];
+            float* tempi = temp.ptr<float>(i);
+
+            tempi[2] = tempi[1];
+            tempi[1] = (float) points[point_idx][1];
+
+        }
+        double m14 = determinant(temp);
+
+        for(int i = 0; i < 4; i++)
+        {
+            unsigned point_idx = inliers[i];
+            float* tempi = temp.ptr<float>(i);
+
+            tempi[0] = tempi[2];
+            tempi[1] = (float) points[point_idx][0];
+            tempi[2] = (float) points[point_idx][1];
+            tempi[3] = (float) points[point_idx][2];
+        }
+        double m15 = determinant(temp);
+
+        center.x = 0.5 * m12 / m11;
+        center.y = 0.5 * m13 / m11;
+        center.z = 0.5 * m14 / m11;
+        // Radius
+        radius = std::sqrt (center.x * center.x +
+                                            center.y * center.y +
+                                            center.z * center.z - m15 / m11);
+
+    return (true);
+
+    }
+
+    Vec4d getPlaneFromPoints(const Vec3f* &points,
+                                    const std::vector<unsigned> &inliers, Point3d& center) {
+        // REF: https://www.ilikebigbits.com/2015_03_04_plane_from_points.html
+        Vec3f centroid(0, 0, 0);
+        for (unsigned idx : inliers) {
+            centroid += points[idx];
+        }
+        centroid /= double(inliers.size());
+
+        double xx = 0, xy = 0, xz = 0, yy = 0, yz = 0, zz = 0;
+
+        for (size_t idx : inliers) {
+            Vec3f r = points[idx] - centroid;
+            xx += r(0) * r(0);
+            xy += r(0) * r(1);
+            xz += r(0) * r(2);
+            yy += r(1) * r(1);
+            yz += r(1) * r(2);
+            zz += r(2) * r(2);
+        }
+
+        double det_x = yy * zz - yz * yz;
+        double det_y = xx * zz - xz * xz;
+        double det_z = xx * yy - xy * xy;
+
+        Vec3d abc;
+        if (det_x > det_y && det_x > det_z) {
+            abc = Vec3d(det_x, xz * yz - xy * zz, xy * yz - xz * yy);
+        } else if (det_y > det_z) {
+            abc = Vec3d(xz * yz - xy * zz, det_y, xy * xz - yz * xx);
+        } else {
+            abc = Vec3d(xy * yz - xz * yy, xy * xz - yz * xx, det_z);
+        }
+
+
+        double magnitude_abc = sqrt(abc[0]*abc[0] + abc[1]* abc[1] + abc[2] * abc[2]);
+
+        // Return invalid plane if the points don't span a plane.
+        if (magnitude_abc == 0) {
+            return Vec4d (0, 0, 0, 0);
+        }
+        abc /= magnitude_abc;
+        double d = -abc.dot(centroid);
+
+        Vec4d coefficients (abc[0], abc[1], abc[2], d);
+        center = Point3d (centroid);
+        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++) {
+            this->ModelCoefficients.push_back(coefficients[i]);
+        }
+        this->size = set_size;
+
+        this-> normal = Vec3d(coefficients[0], coefficients[1], coefficients[2]);
+        this -> center = set_center;
+        // Assign normal vector
+        for (unsigned i = 0; i < 3; i++) normal[i] = coefficients[i];
+    }
+
+    SACPlaneModel::SACPlaneModel(Vec4d coefficients, Size2d set_size) {
+        this->ModelCoefficients.reserve(4);
+        for (int i = 0; i < 4; i++) {
+            this->ModelCoefficients.push_back(coefficients[i]);
+        }
+        this->size = set_size;
+
+        this-> normal = Vec3d(coefficients[0], coefficients[1], coefficients[2]);
+        this -> center = Point3d(0, 0, - coefficients[3] / coefficients[2]);
+        // Assign normal vector
+        for (unsigned i = 0; i < 3; i++) normal[i] = coefficients[i];
+
+        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(vector<double> coefficients, Size2d set_size) {
+        assert(coefficients.size() == 4);
+        this->ModelCoefficients = coefficients;
+        this->size = set_size;
+
+        // Assign normal vector
+        for (unsigned i = 0; i < 3; i++) normal[i] = coefficients[i];
+
+        center.x = 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());
+    //     window.showWidget("planeD", plane);
+    // }
+    viz::WPlane SACPlaneModel::WindowWidget () {
+        return viz::WPlane (this->center, this->normal, Vec3d(1, 0, 0), this->size, viz::Color::green());
+    }
+
+    pair<double, double> SACPlaneModel::getInliers(Mat cloud, vector<unsigned> indices, const double threshold, vector<unsigned>& inliers) {
+        pair<double, double> result;
+        inliers.clear();
+        const Vec3f* points = cloud.ptr<Vec3f>(0);
+        const unsigned num_points = indices.size();
+
+        double magnitude_abc = sqrt(ModelCoefficients[0]*ModelCoefficients[0] + ModelCoefficients[1]* ModelCoefficients[1] + ModelCoefficients[2] * ModelCoefficients[2]);
+
+        if (magnitude_abc == 0) {
+            //something is wrong
+        }
+
+
+        Vec4d NormalisedCoefficients (ModelCoefficients[0]/magnitude_abc, ModelCoefficients[1]/magnitude_abc, ModelCoefficients[2]/magnitude_abc, ModelCoefficients[3]/magnitude_abc);
+        double fitness = 0;
+        double rmse = 0;
+        for (unsigned i = 0; i < num_points; i++) {
+            unsigned ind = indices[i];
+            Vec4d point4d (points[ind][0], points[ind][1], points[ind][2], 1);
+            double distanceFromPlane = point4d.dot(NormalisedCoefficients);
+            if (abs(distanceFromPlane) > threshold) continue;
+            inliers.emplace_back(ind);
+
+            fitness+=1;
+            rmse += distanceFromPlane;
+        }
+
+        unsigned num_inliers = fitness;
+        if (num_inliers == 0) {
+            result.first = 0;
+            result.second = 0;
+        } else {
+            rmse /= num_inliers;
+            fitness /= num_points;
+
+            result.first = fitness;
+            result.second = rmse;
+        }
+
+        return result;
+    }
+    SACSphereModel::SACSphereModel(Point3d set_center, double set_radius) {
+        this -> center = set_center;
+        this -> radius = set_radius;
+
+        this->ModelCoefficients.reserve(4);
+        this -> ModelCoefficients.push_back(center.x);
+        this -> ModelCoefficients.push_back(center.y);
+        this -> ModelCoefficients.push_back(center.z);
+
+        this -> ModelCoefficients.push_back(radius);
+    }
+
+
+    SACSphereModel::SACSphereModel(Vec4d coefficients) {
+        this->ModelCoefficients.reserve(4);
+        for (int i = 0; i < 4; i++) {
+            this->ModelCoefficients.push_back(coefficients[i]);
+        }
+        this -> center = Point3d(coefficients[0], coefficients[1], coefficients[2]);
+        this -> radius = coefficients[3];
+    }
+
+    SACSphereModel::SACSphereModel(vector<double> coefficients) {
+        assert(coefficients.size() == 4);
+        for (int i = 0; i < 4; i++) {
+            this->ModelCoefficients.push_back(coefficients[i]);
+        }
+        this -> center = Point3d(coefficients[0], coefficients[1], coefficients[2]);
+        this -> radius = coefficients[3];
+    }
+
+
+    viz::WSphere SACSphereModel::WindowWidget () {
+        return viz::WSphere(this->center, this->radius, 10, viz::Color::green());;
+    }
+
+    double euclideanDist(Point3d& p, Point3d& q) {
+        Point3d diff = p - q;
+        return cv::sqrt(diff.x*diff.x + diff.y*diff.y + diff.z*diff.z);
+    }
+
+    pair<double, double> SACSphereModel::getInliers(Mat cloud, vector<unsigned> indices, const double threshold, vector<unsigned>& inliers) {
+        pair<double, double> result;
+        inliers.clear();
+        const Vec3f* points = cloud.ptr<Vec3f>(0);
+        const unsigned num_points = indices.size();
+
+        double fitness = 0;
+        double rmse = 0;
+        if(!isnan(radius)) { // radius may come out to be nan if selected points form a plane
+            for (unsigned i = 0; i < num_points; i++) {
+                unsigned ind = indices[i];
+                Point3d pt (points[ind][0], points[ind][1], points[ind][2]);
+                double distanceFromCenter = euclideanDist(pt, center);
+
+                double distanceFromSurface = distanceFromCenter - radius;
+                if (distanceFromSurface > threshold) continue;
+                inliers.emplace_back(ind);
+
+                fitness+=1;
+                rmse += max(0., distanceFromSurface);
+            }
+        }
+
+
+        unsigned num_inliers = fitness;
+        if (num_inliers == 0) {
+            result.first = 0;
+            result.second = 0;
+        } else {
+            rmse /= num_inliers;
+            fitness /= num_points;
+            result.first = fitness;
+            result.second = rmse;
+        }
+
+        return result;
+    }
+
+    SACModelFitting::SACModelFitting (Mat set_cloud, int set_model_type, int set_method_type, double set_threshold, int set_max_iters)
+            :cloud(set_cloud), model_type(set_model_type), method_type(set_method_type), threshold(set_threshold), max_iters(set_max_iters) {}
+
+    SACModelFitting::SACModelFitting (int set_model_type, int set_method_type, double set_threshold, int set_max_iters)
+        :model_type(set_model_type), method_type(set_method_type), threshold(set_threshold), max_iters(set_max_iters) {}
+
+    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);
+
+
+            vector<unsigned> inliers_indices;
+
+            // Initialize the best plane model.
+            SACModel bestModel;
+            pair<double, double> bestResult(0, 0);
+
+            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) {
+                        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;
+                    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);
+            }
+
+            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;
+                    }
+
+                }
+                inliers.push_back(inliers_indices);
+                model_instances.push_back(bestModel);
+            }
+        }
+
+} // ptcloud
+}   // cv

modules/ptcloud/test/test_main.cpp

@@ -0,0 +1,9 @@
+// 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 "test_precomp.hpp"
+
+CV_TEST_MAIN("",
+    cvtest::addDataSearchSubDirectory("contrib"),
+    cvtest::addDataSearchSubDirectory("contrib/ptcloud")
+)

modules/ptcloud/test/test_precomp.hpp

@@ -0,0 +1,15 @@
+// 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_TEST_PTCLOUD_PRECOMP_HPP__
+#define __OPENCV_TEST_PTCLOUD_PRECOMP_HPP__
+
+#include "opencv2/core.hpp"
+#include "opencv2/ts.hpp"
+#include "opencv2/ptcloud.hpp"
+
+namespace opencv_test {
+using namespace cv::ptcloud;
+}
+
+#endif