From 8d59f7cd9ef04d1a4f24d1fc206c794b3bfd63a4 Mon Sep 17 00:00:00 2001 From: Devansh Batra Date: Tue, 30 Jun 2020 18:59:08 +0530 Subject: [PATCH 01/13] initial commit for ptcloud object fitting --- modules/ptcloud/CMakeLists.txt | 9 + modules/ptcloud/README.md | 14 + modules/ptcloud/doc/ptcloud.bib | 0 modules/ptcloud/include/opencv2/ptcloud.hpp | 10 + .../opencv2/ptcloud/sac_segmentation.hpp | 114 +++++ modules/ptcloud/src/precomp.hpp | 14 + modules/ptcloud/src/sac_segmentation.cpp | 442 ++++++++++++++++++ modules/ptcloud/test/test_main.cpp | 9 + modules/ptcloud/test/test_precomp.hpp | 15 + 9 files changed, 627 insertions(+) create mode 100644 modules/ptcloud/CMakeLists.txt create mode 100644 modules/ptcloud/README.md create mode 100644 modules/ptcloud/doc/ptcloud.bib create mode 100644 modules/ptcloud/include/opencv2/ptcloud.hpp create mode 100644 modules/ptcloud/include/opencv2/ptcloud/sac_segmentation.hpp create mode 100644 modules/ptcloud/src/precomp.hpp create mode 100644 modules/ptcloud/src/sac_segmentation.cpp create mode 100644 modules/ptcloud/test/test_main.cpp create mode 100644 modules/ptcloud/test/test_precomp.hpp diff --git a/modules/ptcloud/CMakeLists.txt b/modules/ptcloud/CMakeLists.txt new file mode 100644 index 00000000000..0382b48bc91 --- /dev/null +++ b/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) \ No newline at end of file diff --git a/modules/ptcloud/README.md b/modules/ptcloud/README.md new file mode 100644 index 00000000000..c719346cdaa --- /dev/null +++ b/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 + +//! @} \ No newline at end of file diff --git a/modules/ptcloud/doc/ptcloud.bib b/modules/ptcloud/doc/ptcloud.bib new file mode 100644 index 00000000000..e69de29bb2d diff --git a/modules/ptcloud/include/opencv2/ptcloud.hpp b/modules/ptcloud/include/opencv2/ptcloud.hpp new file mode 100644 index 00000000000..18ac3c30803 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/modules/ptcloud/include/opencv2/ptcloud/sac_segmentation.hpp b/modules/ptcloud/include/opencv2/ptcloud/sac_segmentation.hpp new file mode 100644 index 00000000000..20c04b997bf --- /dev/null +++ b/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 +#include +#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 ModelCoefficients; + // vector inliers; + SACModel(); + SACModel(std::vector 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 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 coefficients, Size2d size=Size2d(2.0, 2.0)); + + viz::WPlane WindowWidget (); + + std::pair getInliers(Mat cloud, std::vector indices, const double threshold, std::vector& inliers); + }; + + + class CV_EXPORTS_W SACSphereModel : public SACModel { + public: + Point3d center; + double radius; + + SACSphereModel() { + } + + SACSphereModel(Point3d center, double radius); + + SACSphereModel(const std::vector Coefficients); + + SACSphereModel(Vec4d coefficients); + // SACSphereModel(std::vector coefficients); + viz::WSphere WindowWidget (); + + double euclideanDist(Point3d& p, Point3d& q); + + std::pair getInliers(Mat cloud, std::vector indices, const double threshold, std::vector& 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> inliers; + vector 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&, Point3d&, double&); + + Vec4d getPlaneFromPoints(const Vec3f*&, const std::vector&, cv::Point3d&); + + double euclideanDist(Point3d& p, Point3d& q); + +} // ptcloud +} // cv +#endif \ No newline at end of file diff --git a/modules/ptcloud/src/precomp.hpp b/modules/ptcloud/src/precomp.hpp new file mode 100644 index 00000000000..29c8ef6d2b0 --- /dev/null +++ b/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 +#include "opencv2/ptcloud/sac_segmentation.hpp" + +#include +#include +#include +#include + +#endif \ No newline at end of file diff --git a/modules/ptcloud/src/sac_segmentation.cpp b/modules/ptcloud/src/sac_segmentation.cpp new file mode 100644 index 00000000000..aef759b61c8 --- /dev/null +++ b/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 +#include + + +using namespace std; +using namespace cv; + +namespace cv +{ +namespace ptcloud +{ + + bool getSphereFromPoints(const Vec3f* &points, const std::vector &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(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(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(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(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(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 &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 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 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 SACPlaneModel::getInliers(Mat cloud, vector indices, const double threshold, vector& inliers) { + pair result; + inliers.clear(); + const Vec3f* points = cloud.ptr(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 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 SACSphereModel::getInliers(Mat cloud, vector indices, const double threshold, vector& inliers) { + pair result; + inliers.clear(); + const Vec3f* points = cloud.ptr(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(0); + unsigned num_points = cloud.cols; + std::vector indices(num_points); + std::iota(std::begin(indices), std::end(indices), 0); + + + vector inliers_indices; + + // Initialize the best plane model. + SACModel bestModel; + pair 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 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 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 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 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 diff --git a/modules/ptcloud/test/test_main.cpp b/modules/ptcloud/test/test_main.cpp new file mode 100644 index 00000000000..ffa182a039a --- /dev/null +++ b/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") +) diff --git a/modules/ptcloud/test/test_precomp.hpp b/modules/ptcloud/test/test_precomp.hpp new file mode 100644 index 00000000000..27fc705be8d --- /dev/null +++ b/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 From 325ce02cde39c998aa3cc8f4a0095f25a109577d Mon Sep 17 00:00:00 2001 From: Devansh Batra Date: Tue, 30 Jun 2020 22:10:40 +0530 Subject: [PATCH 02/13] corrected build issue and added sample data and code --- .../opencv2/ptcloud/sac_segmentation.hpp | 32 ++- modules/ptcloud/samples/sample_plane.cpp | 49 +++++ modules/ptcloud/samples/sample_sphere.cpp | 43 ++++ modules/ptcloud/src/sac_segmentation.cpp | 194 +++++++++--------- 4 files changed, 212 insertions(+), 106 deletions(-) create mode 100755 modules/ptcloud/samples/sample_plane.cpp create mode 100755 modules/ptcloud/samples/sample_sphere.cpp diff --git a/modules/ptcloud/include/opencv2/ptcloud/sac_segmentation.hpp b/modules/ptcloud/include/opencv2/ptcloud/sac_segmentation.hpp index 20c04b997bf..c43569d3815 100644 --- a/modules/ptcloud/include/opencv2/ptcloud/sac_segmentation.hpp +++ b/modules/ptcloud/include/opencv2/ptcloud/sac_segmentation.hpp @@ -24,16 +24,17 @@ namespace ptcloud public: std::vector ModelCoefficients; // vector inliers; - SACModel(); + SACModel() { + + } + SACModel(std::vector 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 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> inliers; vector 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&, Point3d&, double&); diff --git a/modules/ptcloud/samples/sample_plane.cpp b/modules/ptcloud/samples/sample_plane.cpp new file mode 100755 index 00000000000..5bc711f63f3 --- /dev/null +++ b/modules/ptcloud/samples/sample_plane.cpp @@ -0,0 +1,49 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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(0); + vector inlier_vec = planar_segmentation.inliers.at(0); + cv::Mat fit_cloud(1, inlier_vec.size(), CV_32FC3); + for(int j=0; j(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 model_coefficients = planar_segmentation.model_instances.at(0).ModelCoefficients; + cv::ptcloud::SACPlaneModel SACplane (model_coefficients); + + window.spin(); + fitted.spin(); + waitKey(1); + +} \ No newline at end of file diff --git a/modules/ptcloud/samples/sample_sphere.cpp b/modules/ptcloud/samples/sample_sphere.cpp new file mode 100755 index 00000000000..53cc3123592 --- /dev/null +++ b/modules/ptcloud/samples/sample_sphere.cpp @@ -0,0 +1,43 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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 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); + +} \ No newline at end of file diff --git a/modules/ptcloud/src/sac_segmentation.cpp b/modules/ptcloud/src/sac_segmentation.cpp index aef759b61c8..6f54bfe5093 100644 --- a/modules/ptcloud/src/sac_segmentation.cpp +++ b/modules/ptcloud/src/sac_segmentation.cpp @@ -138,32 +138,6 @@ namespace ptcloud return coefficients; } - - - SACPlaneModel::SACPlaneModel(vector 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(0); - unsigned num_points = cloud.cols; - std::vector 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(0); + unsigned num_points = cloud.cols; + std::vector indices(num_points); + std::iota(std::begin(indices), std::end(indices), 0); - vector inliers_indices; - // Initialize the best plane model. - SACModel bestModel; - pair bestResult(0, 0); + vector 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 current_model_inliers; - SACModel model; + // Initialize the best plane model. + SACModel bestModel; + pair 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 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 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 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 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 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 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 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 From cb009e40451e068a529b0def59d36cbb077463b2 Mon Sep 17 00:00:00 2001 From: Devansh Batra Date: Tue, 30 Jun 2020 22:35:29 +0530 Subject: [PATCH 03/13] added highgui as dependency and removed imgproc --- modules/ptcloud/CMakeLists.txt | 2 +- modules/ptcloud/samples/sample_plane.cpp | 1 - modules/ptcloud/samples/sample_sphere.cpp | 1 - 3 files changed, 1 insertion(+), 3 deletions(-) diff --git a/modules/ptcloud/CMakeLists.txt b/modules/ptcloud/CMakeLists.txt index 0382b48bc91..3a67716a9d2 100644 --- a/modules/ptcloud/CMakeLists.txt +++ b/modules/ptcloud/CMakeLists.txt @@ -1,5 +1,5 @@ set(the_description "Point Cloud Object Fitting API") -ocv_define_module(ptcloud opencv_core opencv_viz WRAP python) +ocv_define_module(ptcloud opencv_core opencv_highgui opencv_viz WRAP python) # add test data from samples dir to contrib/ptcloud ocv_add_testdata(samples/ contrib/ptcloud FILES_MATCHING PATTERN "*.ply") diff --git a/modules/ptcloud/samples/sample_plane.cpp b/modules/ptcloud/samples/sample_plane.cpp index 5bc711f63f3..6e6cc35082e 100755 --- a/modules/ptcloud/samples/sample_plane.cpp +++ b/modules/ptcloud/samples/sample_plane.cpp @@ -1,5 +1,4 @@ #include -#include #include #include #include diff --git a/modules/ptcloud/samples/sample_sphere.cpp b/modules/ptcloud/samples/sample_sphere.cpp index 53cc3123592..9616e06ce74 100755 --- a/modules/ptcloud/samples/sample_sphere.cpp +++ b/modules/ptcloud/samples/sample_sphere.cpp @@ -1,5 +1,4 @@ #include -#include #include #include #include From 1a40d52f6fed6ca8bfffaec67481ad42ff30e8ce Mon Sep 17 00:00:00 2001 From: Devansh Batra Date: Wed, 1 Jul 2020 01:43:02 +0530 Subject: [PATCH 04/13] include DOXYGEN docs --- .../opencv2/ptcloud/sac_segmentation.hpp | 76 +++++++++++++++++-- 1 file changed, 70 insertions(+), 6 deletions(-) diff --git a/modules/ptcloud/include/opencv2/ptcloud/sac_segmentation.hpp b/modules/ptcloud/include/opencv2/ptcloud/sac_segmentation.hpp index c43569d3815..59621166e86 100644 --- a/modules/ptcloud/include/opencv2/ptcloud/sac_segmentation.hpp +++ b/modules/ptcloud/include/opencv2/ptcloud/sac_segmentation.hpp @@ -23,7 +23,7 @@ namespace ptcloud class CV_EXPORTS_W SACModel: public Algorithm { public: std::vector ModelCoefficients; - // vector inliers; + SACModel() { } @@ -52,10 +52,27 @@ namespace ptcloud } + /** @brief Create a plane model based on the given coefficients and a center point. + + @param coefficients coefficients in the plane equations of type Ax + By + Cz + D = 0. Also obtained using SACModelFitting. + @param center the center point of the plane. + @param size the size of the plane. + */ + SACPlaneModel(Vec4d coefficients, Point3d center, Size2d size=Size2d(2.0, 2.0)); + /** @brief Create a plane model based on the given coefficients and an arbitrary center point. + + @param coefficients coefficients in the plane equations Ax + By + Cz + D = 0. + @param size the size of the plane. + */ SACPlaneModel(Vec4d coefficients, Size2d size=Size2d(2.0, 2.0)); + /** @brief Create a plane model based on the given coefficients and an arbitrary center point. + + @param coefficients coefficients in the plane equations Ax + By + Cz + D = 0. + @param size the size of the plane. + */ SACPlaneModel(std::vector coefficients, Size2d size=Size2d(2.0, 2.0)); viz::WPlane WindowWidget (); @@ -78,12 +95,26 @@ namespace ptcloud } + /** @brief Create a spherical model based on the given center and radius. + + @param center the center point of the sphere + @param radius the radius of the sphere. + */ + SACSphereModel(Point3d center, double radius); + /** @brief Create a spherical model based on the parametric coefficients. + + This is very helpful for creating a model for the fit models using SACModelFitting class. + + @param Coefficients parametric coefficients for the Sphere model + @param radius the radius of the sphere. + */ + SACSphereModel(const std::vector Coefficients); SACSphereModel(Vec4d coefficients); - // SACSphereModel(std::vector coefficients); + viz::WSphere WindowWidget (); double euclideanDist(Point3d& p, Point3d& q); @@ -106,19 +137,52 @@ namespace ptcloud vector> inliers; vector model_instances; - // viz::Viz3d window; + /** @brief Initializes SACModelFitting class. + + Threshold and Iterations may also be set separately. + + @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 - currently, only RANSAC is supported (use value SAC_METHOD_RANSAC). + @param threshold set the threshold while choosing inliers. + @param max_iters number of iterations for Sampling. + */ + 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) {} + /** @brief Initializes SACModelFitting class. + + Threshold and Iterations may also be set separately. + + @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 - currently, only RANSAC is supported (use value SAC_METHOD_RANSAC). + @param threshold set the threshold while choosing inliers. + @param max_iters number of iterations for Sampling. + */ 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). + /** @brief Get one model (plane), this function would get the best fitting model on the given set of points. + + note: This stores the model in the public class member model_instances, and the mask for inliers in inliers. + */ void fit_once(); - void set_threshold (double); + /** @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. + */ + void set_threshold (double threshold); + + /** @brief Set the number of iterations for the fitting. - void set_iterations (long unsigned); + This may be helpful when multiple fitting operations are to be performed. + @param iterations the threshold to set. + */ + void set_iterations (long unsigned iterations); }; bool getSphereFromPoints(const Vec3f*&, const vector&, Point3d&, double&); From 3b0990b674401723ef9955603f62c491e91b24c8 Mon Sep 17 00:00:00 2001 From: Devansh Batra Date: Wed, 1 Jul 2020 02:26:45 +0530 Subject: [PATCH 05/13] remove redundant doxygen param --- modules/ptcloud/include/opencv2/ptcloud/sac_segmentation.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/modules/ptcloud/include/opencv2/ptcloud/sac_segmentation.hpp b/modules/ptcloud/include/opencv2/ptcloud/sac_segmentation.hpp index 59621166e86..99144400dde 100644 --- a/modules/ptcloud/include/opencv2/ptcloud/sac_segmentation.hpp +++ b/modules/ptcloud/include/opencv2/ptcloud/sac_segmentation.hpp @@ -108,7 +108,6 @@ namespace ptcloud This is very helpful for creating a model for the fit models using SACModelFitting class. @param Coefficients parametric coefficients for the Sphere model - @param radius the radius of the sphere. */ SACSphereModel(const std::vector Coefficients); From 806549f99569dcb192ff2275959293a6fcb2b616 Mon Sep 17 00:00:00 2001 From: Devansh Batra Date: Sat, 25 Jul 2020 20:02:25 +0530 Subject: [PATCH 06/13] completed cylinder fitting --- modules/ptcloud/CMakeLists.txt | 2 +- .../opencv2/ptcloud/sac_segmentation.hpp | 56 +++- modules/ptcloud/src/sac_segmentation.cpp | 303 ++++++++++++++++-- 3 files changed, 339 insertions(+), 22 deletions(-) diff --git a/modules/ptcloud/CMakeLists.txt b/modules/ptcloud/CMakeLists.txt index 3a67716a9d2..9a80ab5e812 100644 --- a/modules/ptcloud/CMakeLists.txt +++ b/modules/ptcloud/CMakeLists.txt @@ -1,5 +1,5 @@ set(the_description "Point Cloud Object Fitting API") -ocv_define_module(ptcloud opencv_core opencv_highgui opencv_viz WRAP python) +ocv_define_module(ptcloud opencv_core opencv_highgui opencv_viz opencv_surface_matching WRAP python) # add test data from samples dir to contrib/ptcloud ocv_add_testdata(samples/ contrib/ptcloud FILES_MATCHING PATTERN "*.ply") diff --git a/modules/ptcloud/include/opencv2/ptcloud/sac_segmentation.hpp b/modules/ptcloud/include/opencv2/ptcloud/sac_segmentation.hpp index 99144400dde..09d98959bef 100644 --- a/modules/ptcloud/include/opencv2/ptcloud/sac_segmentation.hpp +++ b/modules/ptcloud/include/opencv2/ptcloud/sac_segmentation.hpp @@ -121,13 +121,54 @@ namespace ptcloud std::pair getInliers(Mat cloud, std::vector indices, const double threshold, std::vector& inliers); }; + class CV_EXPORTS_W SACCylinderModel : public SACModel { + public: + Point3d pt_on_axis; + Vec3d axis_dir; + double radius; + double size = 20; + + ~ SACCylinderModel() + { + + } + + SACCylinderModel() { + + } + + // /** @brief Create a spherical model based on the given center and radius. + + // @param center the center point of the sphere + // @param radius the radius of the sphere. + // */ + + // SACCylinderModel(const std::vector Coefficients); + + /** @brief Create a spherical model based on the parametric coefficients. + + This is very helpful for creating a model for the fit models using SACModelFitting class. + + @param Coefficients parametric coefficients for the Sphere model + */ + + SACCylinderModel(const std::vector Coefficients); + + viz::WCylinder WindowWidget (); + + std::pair getInliers(Mat cloud, Mat normals, std::vector indices, const double threshold, std::vector& inliers, double normal_distance_weight_ = 0); + }; + class CV_EXPORTS_W SACModelFitting { private: Mat cloud; + Mat normals; + bool normals_available = false; int model_type; int method_type; double threshold; long unsigned max_iters; + double normal_distance_weight_ = 0; public: // cv::Mat remainingCloud; // will be used while segmentation @@ -168,6 +209,10 @@ namespace ptcloud */ void fit_once(); + void setCloud(Mat cloud); + + void setCloud(Mat cloud, bool with_normals=false); + /** @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. @@ -182,12 +227,21 @@ namespace ptcloud @param iterations the threshold to set. */ void set_iterations (long unsigned iterations); + + /** @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). + */ + void set_normal_distance_weight(double weight); }; bool getSphereFromPoints(const Vec3f*&, const vector&, Point3d&, double&); Vec4d getPlaneFromPoints(const Vec3f*&, const std::vector&, cv::Point3d&); - + + bool getCylinderFromPoints(Mat cloud, Mat normal, + const std::vector &inliers, vector & coefficients) ; + double euclideanDist(Point3d& p, Point3d& q); } // ptcloud diff --git a/modules/ptcloud/src/sac_segmentation.cpp b/modules/ptcloud/src/sac_segmentation.cpp index 6f54bfe5093..9e33e3d4e8b 100644 --- a/modules/ptcloud/src/sac_segmentation.cpp +++ b/modules/ptcloud/src/sac_segmentation.cpp @@ -4,7 +4,10 @@ #include "precomp.hpp" #include +#include "opencv2/surface_matching.hpp" +#include "opencv2/surface_matching/ppf_helpers.hpp" #include +#include using namespace std; @@ -15,7 +18,7 @@ namespace cv namespace ptcloud { - bool getSphereFromPoints(const Vec3f* &points, const std::vector &inliers, Point3d& center, double& radius) { + bool getSphereFromPoints(const Vec3f* &points, const std::vector &inliers, Point3d& center, double& radius) { // assert that size of points is 3. Mat temp(5,5,CV_32FC1); // Vec4f temp; @@ -138,21 +141,95 @@ namespace ptcloud return coefficients; } + bool getCylinderFromPoints(const Mat cloud, const Mat normals_cld, + const std::vector &inliers, vector & model_coefficients) { + assert(inliers.size() == 2); + unsigned int num_points = cloud.cols; + + Mat _pointsAndNormals; + + assert(normals_cld.cols == cloud.cols); + + const Point3d* points = cloud.ptr(0); + const Vec3f* normals = normals_cld.ptr(0); + num_points = cloud.cols; + + if (fabs (points[inliers[0]].x - points[inliers[1]].x) <= std::numeric_limits::epsilon () && + fabs (points[inliers[0]].y - points[inliers[1]].y) <= std::numeric_limits::epsilon () && + fabs (points[inliers[0]].z - points[inliers[1]].z) <= std::numeric_limits::epsilon ()) + { + return (false); + } + Vec3f p1 (points[inliers[0]].x, points[inliers[0]].y, points[inliers[0]].z); + Vec3f p2 (points[inliers[1]].x, points[inliers[1]].y, points[inliers[1]].z); + + Vec3f n1 (normals[inliers[0]] [0], normals[inliers[0]] [1], normals[inliers[0]] [2]); + Vec3f n2 (normals[inliers[1]] [0], normals[inliers[1]] [1], normals[inliers[1]] [2]); + Vec3f w = n1 + p1 - p2; + + float a = n1.dot (n1); + float b = n1.dot (n2); + float c = n2.dot (n2); + float d = n1.dot (w); + float e = n2.dot (w); + float denominator = a*c - b*b; + float sc, tc; + // Compute the line parameters of the two closest points + if (denominator < 1e-8) // The lines are almost parallel + { + sc = 0.0f; + tc = (b > c ? d / b : e / c); // Use the largest denominator + } + else + { + sc = (b*e - c*d) / denominator; + tc = (a*e - b*d) / denominator; + } + + // point_on_axis, axis_direction + Vec3f line_pt = p1 + n1 + sc * n1; + Vec3f line_dir = p2 + tc * n2 - line_pt; + + model_coefficients.resize (7); + // point on line + model_coefficients[0] = line_pt[0]; + model_coefficients[1] = line_pt[1]; + model_coefficients[2] = line_pt[2]; + + double divide_by = std::sqrt (line_dir[0] * line_dir[0] + + line_dir[1] * line_dir[1] + + line_dir[2] * line_dir[2]); + // direction of line; + model_coefficients[3] = line_dir[0] / divide_by; + model_coefficients[4] = line_dir[1] / divide_by; + model_coefficients[5] = line_dir[2] / divide_by; + + double radius_squared = fabs((line_dir.cross(line_pt - p1)).dot(line_dir.cross(line_pt - p1)) / line_dir.dot(line_dir)); + + // radius of cylinder + model_coefficients[6] = sqrt(radius_squared); + + if (radius_squared == 0) return false; + + return (true); + } + SACPlaneModel::SACPlaneModel(Vec4d coefficients, Point3d set_center, Size2d set_size) { - this->ModelCoefficients.reserve(4); + this -> ModelCoefficients.reserve(4); for (int i = 0; i < 4; i++) { - this->ModelCoefficients.push_back(coefficients[i]); + this -> ModelCoefficients.push_back(coefficients[i]); } - this->size = set_size; + this -> size = set_size; - this-> normal = Vec3d(coefficients[0], coefficients[1], coefficients[2]); + 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); + this -> ModelCoefficients.reserve(4); for (int i = 0; i < 4; i++) { this->ModelCoefficients.push_back(coefficients[i]); } @@ -202,10 +279,7 @@ namespace ptcloud 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()); } @@ -218,10 +292,7 @@ namespace ptcloud double magnitude_abc = sqrt(ModelCoefficients[0]*ModelCoefficients[0] + ModelCoefficients[1]* ModelCoefficients[1] + ModelCoefficients[2] * ModelCoefficients[2]); - if (magnitude_abc == 0) { - //something is wrong - } - + assert (magnitude_abc == 0); Vec4d NormalisedCoefficients (ModelCoefficients[0]/magnitude_abc, ModelCoefficients[1]/magnitude_abc, ModelCoefficients[2]/magnitude_abc, ModelCoefficients[3]/magnitude_abc); double fitness = 0; @@ -255,7 +326,7 @@ namespace ptcloud this -> center = set_center; this -> radius = set_radius; - this->ModelCoefficients.reserve(4); + this -> ModelCoefficients.reserve(4); this -> ModelCoefficients.push_back(center.x); this -> ModelCoefficients.push_back(center.y); this -> ModelCoefficients.push_back(center.z); @@ -267,7 +338,7 @@ namespace ptcloud SACSphereModel::SACSphereModel(Vec4d coefficients) { this->ModelCoefficients.reserve(4); for (int i = 0; i < 4; i++) { - this->ModelCoefficients.push_back(coefficients[i]); + this -> ModelCoefficients.push_back(coefficients[i]); } this -> center = Point3d(coefficients[0], coefficients[1], coefficients[2]); this -> radius = coefficients[3]; @@ -330,6 +401,128 @@ namespace ptcloud return result; } + viz::WCylinder SACCylinderModel::WindowWidget () { + Point3d first_point = Point3d( Vec3d(pt_on_axis) + size * (axis_dir)); + Point3d second_point = Point3d(Vec3d(pt_on_axis) - size * (axis_dir)); + + return viz::WCylinder (first_point, second_point, radius, 40, viz::Color::green()); + } + + SACCylinderModel::SACCylinderModel(const vector coefficients) { + assert(coefficients.size() == 7); + for (int i = 0; i < 7; i++) { + this -> ModelCoefficients.push_back(coefficients[i]); + } + this -> pt_on_axis = Point3d(coefficients[0], coefficients[1], coefficients[2]); + this -> axis_dir = Vec3d(coefficients[3], coefficients[4], coefficients[5]); + this -> radius = coefficients[6]; + + } + + std::pair SACCylinderModel::getInliers(Mat cloud, Mat normal_cloud, std::vector indices, const double threshold, std::vector& inliers, double normal_distance_weight_) { + pair result; + inliers.clear(); + const Vec3f* points = cloud.ptr(0); + const Vec3f* normals = normal_cloud.ptr(0); + const unsigned num_points = indices.size(); + + double fitness = 0; + double rmse = 0; + axis_dir = (axis_dir); + + // for (int i = 0; i < num_points; i++) { + // cout << i << " " << points[i] << endl; + // } + 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]); + Vec3d normal (normals[ind][0], normals[ind][1], normals[ind][2]); + normal = normal / sqrt(normal[0] * normal[0] + normal[1] * normal[1] + normal[2] * normal[2]); + + double distanceFromAxis = fabs((axis_dir.cross(pt_on_axis - pt)).dot(axis_dir.cross(pt_on_axis - pt)) / axis_dir.dot(axis_dir)); + + double distanceFromSurface = fabs(distanceFromAxis - radius*radius); + if (distanceFromSurface > threshold) continue; + + // Calculate the point's projection on the cylinder axis + float dist = (pt.dot (axis_dir) - pt_on_axis.dot(axis_dir)); + Vec3d pt_proj = Vec3d(pt_on_axis) + dist * axis_dir; + Vec3f dir = Vec3d(pt) - pt_proj; + dir = dir / sqrt(dir[0] * dir[0] + dir[1] * dir[1] + dir[2] * dir[2]); + + // Calculate the angular distance between the point normal and the (dir=pt_proj->pt) vector + double rad = normalize(normal).dot(dir); + if (rad < -1.0) rad = -1.0; + if (rad > 1.0) rad = 1.0; + double d_normal = fabs(acos (rad)); + + // convert 0 to PI/2 + d_normal = (std::min) (d_normal, M_PI - d_normal); + + // calculate overall distance as weighted sum of the two distances. + double distance = fabs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * distanceFromSurface); + + if (distance > threshold) continue; + + inliers.emplace_back(ind); + + fitness += 1; + rmse += max(0., distance); + } + } + + 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; + } + + void SACModelFitting::setCloud(Mat inp_cloud, bool with_normals) { + if (! with_normals) { + // normals are not required. + // the cloud should have three channels. + assert(inp_cloud.channels() == 3 || (inp_cloud.channels() == 1 && (inp_cloud.cols == 3 || inp_cloud.rows == 3))); + if (inp_cloud.rows == 1 && inp_cloud.channels() == 3) { + cloud = inp_cloud; + return; + } + + if (inp_cloud.channels() != 3 && inp_cloud.rows == 3) { + inp_cloud = inp_cloud.t(); + } + const long unsigned num_points = inp_cloud.rows; + cloud = inp_cloud.reshape(3, num_points); + cloud = cloud.t(); + + const Vec3f* points = cloud.ptr(0); + } + else { + assert(inp_cloud.channels() == 1 && (inp_cloud.cols == 6 || inp_cloud.rows == 6)); + if (inp_cloud.rows == 6) { + inp_cloud = inp_cloud.t(); + } + Mat _cld; + inp_cloud.colRange(0, 3).copyTo(_cld); + + Mat _normals; + inp_cloud.colRange(3, 6).copyTo(_normals); + + this -> cloud = Mat(_cld).reshape(3, 1); + this -> normals = Mat(_normals).reshape(3, 1); + this -> normals_available = true; + } + } + 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) {} @@ -361,11 +554,11 @@ namespace ptcloud SACModel model; for (unsigned j = 0; j < num_rnd_model_points; ++j) { - std::swap(indices[i], indices[rng.uniform(0, num_points)]); + std::swap(indices[j], indices[rng.uniform(0, num_points)]); } for (unsigned j = 0; j < num_rnd_model_points; j++) { - unsigned idx = indices[i]; + unsigned idx = indices[j]; current_model_inliers.emplace_back(idx); } @@ -395,11 +588,11 @@ namespace ptcloud SACModel model; for (unsigned j = 0; j < num_rnd_model_points; ++j) { - std::swap(indices[i], indices[rng.uniform(0, num_points)]); + std::swap(indices[j], indices[rng.uniform(0, num_points)]); } for (unsigned j = 0; j < num_rnd_model_points; j++) { - unsigned idx = indices[i]; + unsigned idx = indices[j]; current_model_inliers.emplace_back(idx); } @@ -424,6 +617,66 @@ namespace ptcloud inliers.push_back(inliers_indices); model_instances.push_back(bestModel); } + + if (model_type == CYLINDER_MODEL) { + assert(this->normals_available == true); + RNG rng((uint64)-1); + const unsigned num_rnd_model_points = 2; + double bestRadius = 10000000; + + if (!normals_available) { + // Reshape the cloud for Compute Normals Function + Mat _pointsAndNormals; + Vec3d viewpoint(0, 0, 0); + Mat _cld_reshaped = Mat(cloud).t(); + _cld_reshaped = _cld_reshaped.reshape(1); + ppf_match_3d::computeNormalsPC3d(_cld_reshaped, _pointsAndNormals, 12, false, viewpoint); + + Mat(_pointsAndNormals.colRange(3,6)).copyTo(normals); + long unsigned num_points = cloud.cols; + normals = normals.reshape(3, num_points); + normals = normals.t(); + // cout << cloud.cols << endl; + // cout << normals.size(); + } + for (unsigned i = 0; i < max_iters; ++i) { + vector current_model_inliers; + SACModel model; + + for (unsigned j = 0; j < num_rnd_model_points; ++j) { + std::swap(indices[j], indices[rng.uniform(0, num_points)]); + } + + for (unsigned j = 0; j < num_rnd_model_points; j++) { + unsigned idx = indices[j]; + current_model_inliers.emplace_back(idx); + } + + Point3d center; + double radius; + vector coefficients; + bool valid_model = getCylinderFromPoints(cloud, normals, current_model_inliers, coefficients); + + if (!valid_model) continue; + + SACCylinderModel cylinderModel (coefficients); + + pair result = cylinderModel.getInliers(cloud, normals, indices, threshold, current_model_inliers, normal_distance_weight_); + + // Compare fitness first. + if (bestResult.first < result.first || (bestResult.first == result.first && bestResult.second > result.second)) { + // if (bestResult.first == result.first && bestModel.ModelCoefficients.size() == 7) continue; + bestResult = result; + bestModel.ModelCoefficients = cylinderModel.ModelCoefficients; + bestModel.ModelCoefficients = cylinderModel.ModelCoefficients; + inliers_indices = current_model_inliers; + cout << bestResult.first << endl; + } + + } + inliers.push_back(inliers_indices); + model_instances.push_back(bestModel); + } } void SACModelFitting::set_threshold (double threshold_value) { @@ -434,5 +687,15 @@ namespace ptcloud max_iters = iterations; } + void SACModelFitting::set_normal_distance_weight(double weight) { + if (weight > 1) { + normal_distance_weight_ = 1; + } else if (weight < 0) { + normal_distance_weight_ = 0; + } else { + normal_distance_weight_ = weight; + } + } + } // ptcloud } // cv From 1283fee16d6f16aaf16ef45033d7d2e23f5f6710 Mon Sep 17 00:00:00 2001 From: Devansh Batra Date: Thu, 30 Jul 2020 13:55:31 +0530 Subject: [PATCH 07/13] added code and sample program for segmentation --- .../opencv2/ptcloud/sac_segmentation.hpp | 16 ++- .../samples/sample_cylinder_fitting.cpp | 68 +++++++++++++ .../ptcloud/samples/sample_plane_fitting.cpp | 48 +++++++++ .../samples/sample_plane_segmentation.cpp | 61 ++++++++++++ .../ptcloud/samples/sample_sphere_fitting.cpp | 42 ++++++++ modules/ptcloud/src/sac_segmentation.cpp | 98 +++++++++++++++---- 6 files changed, 312 insertions(+), 21 deletions(-) create mode 100644 modules/ptcloud/samples/sample_cylinder_fitting.cpp create mode 100755 modules/ptcloud/samples/sample_plane_fitting.cpp create mode 100755 modules/ptcloud/samples/sample_plane_segmentation.cpp create mode 100755 modules/ptcloud/samples/sample_sphere_fitting.cpp diff --git a/modules/ptcloud/include/opencv2/ptcloud/sac_segmentation.hpp b/modules/ptcloud/include/opencv2/ptcloud/sac_segmentation.hpp index 09d98959bef..b285d5f955e 100644 --- a/modules/ptcloud/include/opencv2/ptcloud/sac_segmentation.hpp +++ b/modules/ptcloud/include/opencv2/ptcloud/sac_segmentation.hpp @@ -203,11 +203,21 @@ namespace ptcloud 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) {} - /** @brief Get one model (plane), this function would get the best fitting model on the given set of points. + /** @brief Fit one model, this function would get the best fitting model on the given set of points. - note: This stores the model in the public class member model_instances, and the mask for inliers in inliers. + This stores the model in the public class member model_instances, and the mask for inliers in inliers. */ - void fit_once(); + bool fit_once(vector remaining_indices = {}); + + /** @brief Fit 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 public class member model_instances, and the corresponding masks for inliers in inliers. + + Returns False if no valid model could be fit. + + @param remaining_cloud_threshold set the threshold for the remaining cloud (from 0 to 1) until which the segmentation should continue. + */ + void segment(float remaining_cloud_threshold = 0.3); void setCloud(Mat cloud); diff --git a/modules/ptcloud/samples/sample_cylinder_fitting.cpp b/modules/ptcloud/samples/sample_cylinder_fitting.cpp new file mode 100644 index 00000000000..674ee8a19eb --- /dev/null +++ b/modules/ptcloud/samples/sample_cylinder_fitting.cpp @@ -0,0 +1,68 @@ +#include +#include +#include +#include +#include "opencv2/surface_matching.hpp" +#include "opencv2/surface_matching/ppf_helpers.hpp" +#include +#include +#include +#include +#include + +using namespace cv; +using namespace std; + +int main() { + // Mat cloud = cv::ppf_match_3d::loadPLYSimple("./data/semi-cylinder-with-normals-usingOpenCV2.ply", true); + Mat cloud = cv::ppf_match_3d::loadPLYSimple("./data/cylinder-big.ply", false); + Mat ptset; + Mat(cloud.colRange(0,3)).copyTo(ptset); + long unsigned num_points = ptset.rows; + ptset = ptset.reshape(3, num_points); + ptset = ptset.t(); + + cv::ptcloud::SACModelFitting cylinder_segmentation(CYLINDER_MODEL); + cylinder_segmentation.setCloud(cloud, false); + + // add original cloud to window + viz::Viz3d window("original cloud"); + viz::WCloud original_cloud(ptset); + window.showWidget("cloud", original_cloud); + + cylinder_segmentation.set_threshold(0.5); + cylinder_segmentation.set_iterations(80000); + cylinder_segmentation.set_normal_distance_weight(0.5); + cylinder_segmentation.fit_once(); + + cout << cylinder_segmentation.inliers.size(); + vector inlier_vec = cylinder_segmentation.inliers.at(0); + + + vector model_coefficients = cylinder_segmentation.model_instances.at(0).ModelCoefficients; + cout << cylinder_segmentation.model_instances.at(0).ModelCoefficients.size(); + cv::ptcloud::SACCylinderModel cylinder (model_coefficients); + cout << cylinder.pt_on_axis << endl; + cout << cylinder.axis_dir << endl; + cout << cylinder.radius << endl; + + viz::WCylinder model = cylinder.WindowWidget(); + window.showWidget("model", model); + + const Vec3f* points = ptset.ptr(0); + cout << endl << endl << inlier_vec.size(); + cv::Mat fit_cloud(1, inlier_vec.size(), CV_32FC3); + for(int j=0; j(0, j) = points[(j)]; + } + viz::Viz3d fitted("fitted cloud"); + viz::WCloud cloud_widget2(fit_cloud, viz::Color::red()); + fitted.showWidget("fit_cloud", cloud_widget2); + window.showWidget("fit_cloud", cloud_widget2); + fitted.spin(); + + + window.spin(); + waitKey(1); + +} \ No newline at end of file diff --git a/modules/ptcloud/samples/sample_plane_fitting.cpp b/modules/ptcloud/samples/sample_plane_fitting.cpp new file mode 100755 index 00000000000..6e6cc35082e --- /dev/null +++ b/modules/ptcloud/samples/sample_plane_fitting.cpp @@ -0,0 +1,48 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +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(0); + vector inlier_vec = planar_segmentation.inliers.at(0); + cv::Mat fit_cloud(1, inlier_vec.size(), CV_32FC3); + for(int j=0; j(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 model_coefficients = planar_segmentation.model_instances.at(0).ModelCoefficients; + cv::ptcloud::SACPlaneModel SACplane (model_coefficients); + + window.spin(); + fitted.spin(); + waitKey(1); + +} \ No newline at end of file diff --git a/modules/ptcloud/samples/sample_plane_segmentation.cpp b/modules/ptcloud/samples/sample_plane_segmentation.cpp new file mode 100755 index 00000000000..1a664046a4a --- /dev/null +++ b/modules/ptcloud/samples/sample_plane_segmentation.cpp @@ -0,0 +1,61 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +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.segment(); + + + const Vec3f* points = cloud.ptr(0); + + // Initialise a colors array. These colors will be used (in a cyclic order) to visualise all the segmented planes. + const vector colors({viz::Color::green(), viz::Color::blue(), viz::Color::red(), viz::Color::yellow(), viz::Color::orange(),viz::Color::olive()}); + + // Adds segmented planes to window + for (int model_idx = 0; model_idx < planar_segmentation.inliers.size(); model_idx++) { + vector inlier_vec = planar_segmentation.inliers.at(model_idx); + cv::Mat fit_cloud(1, inlier_vec.size(), CV_32FC3); + for(int j=0; j(0, j) = points[inlier_vec.at(j)]; + + viz::Viz3d fitted("fit cloud " + to_string(model_idx + 1)); + fitted.showWidget("cloud", original_cloud); + + // Assign a color to this cloud from the colors array in a cyclic order. + viz::Color cloud_color = colors[model_idx % colors.size()]; + viz::WCloud cloud_widget2(fit_cloud, cloud_color); + fitted.showWidget("fit plane", cloud_widget2); + window.showWidget("fit plane " + to_string(model_idx + 1), cloud_widget2); + + + vector model_coefficients = planar_segmentation.model_instances.at(0).ModelCoefficients; + cv::ptcloud::SACPlaneModel SACplane (model_coefficients); + + fitted.spin(); + } + + window.spin(); + + // waitKey(1); + +} \ No newline at end of file diff --git a/modules/ptcloud/samples/sample_sphere_fitting.cpp b/modules/ptcloud/samples/sample_sphere_fitting.cpp new file mode 100755 index 00000000000..9616e06ce74 --- /dev/null +++ b/modules/ptcloud/samples/sample_sphere_fitting.cpp @@ -0,0 +1,42 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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 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); + +} \ No newline at end of file diff --git a/modules/ptcloud/src/sac_segmentation.cpp b/modules/ptcloud/src/sac_segmentation.cpp index 9e33e3d4e8b..095f28a3dd9 100644 --- a/modules/ptcloud/src/sac_segmentation.cpp +++ b/modules/ptcloud/src/sac_segmentation.cpp @@ -493,7 +493,7 @@ namespace ptcloud // the cloud should have three channels. assert(inp_cloud.channels() == 3 || (inp_cloud.channels() == 1 && (inp_cloud.cols == 3 || inp_cloud.rows == 3))); if (inp_cloud.rows == 1 && inp_cloud.channels() == 3) { - cloud = inp_cloud; + cloud = inp_cloud.clone(); return; } @@ -524,21 +524,29 @@ namespace ptcloud } 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) {} + :cloud(set_cloud.clone()), 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() { + bool SACModelFitting::fit_once(vector labels /* = {} */) { - if (method_type != SAC_METHOD_RANSAC) return; // Only RANSAC supported ATM, need to integrate with Maksym's framework. + if (method_type != SAC_METHOD_RANSAC) return false; // 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(0); unsigned num_points = cloud.cols; - std::vector indices(num_points); - std::iota(std::begin(indices), std::end(indices), 0); + std::vector indices; + + if (labels.size() != num_points) { + indices = std::vector (num_points); + std::iota(std::begin(indices), std::end(indices), 0); + } else { + for (unsigned i = 0; i < num_points; i++) { + if (labels[i] == -1) indices.push_back(i); + } + } vector inliers_indices; @@ -551,10 +559,11 @@ namespace ptcloud RNG rng((uint64)-1); for (unsigned i = 0; i < max_iters; ++i) { vector current_model_inliers; - SACModel model; + SACModel model; - for (unsigned j = 0; j < num_rnd_model_points; ++j) { + for (unsigned j = 0; j < num_rnd_model_points;) { std::swap(indices[j], indices[rng.uniform(0, num_points)]); + j++; } for (unsigned j = 0; j < num_rnd_model_points; j++) { @@ -564,6 +573,7 @@ namespace ptcloud Point3d center; Vec4d coefficients = getPlaneFromPoints(points, current_model_inliers, center); + if (coefficients == Vec4d(0, 0, 0, 0)) continue; SACPlaneModel planeModel (coefficients, center); pair result = planeModel.getInliers(cloud, indices, threshold, current_model_inliers); @@ -575,8 +585,11 @@ namespace ptcloud } } - inliers.push_back(inliers_indices); - model_instances.push_back(bestModel); + if (bestModel.ModelCoefficients.size()) { + inliers.push_back(inliers_indices); + model_instances.push_back(bestModel); + return true; + } } if (model_type == SPHERE_MODEL) { @@ -587,8 +600,9 @@ namespace ptcloud vector current_model_inliers; SACModel model; - for (unsigned j = 0; j < num_rnd_model_points; ++j) { + for (unsigned j = 0; j < num_rnd_model_points;) { std::swap(indices[j], indices[rng.uniform(0, num_points)]); + j++; } for (unsigned j = 0; j < num_rnd_model_points; j++) { @@ -614,8 +628,11 @@ namespace ptcloud } } - inliers.push_back(inliers_indices); - model_instances.push_back(bestModel); + if (bestModel.ModelCoefficients.size()) { + inliers.push_back(inliers_indices); + model_instances.push_back(bestModel); + return true; + } } if (model_type == CYLINDER_MODEL) { @@ -636,15 +653,14 @@ namespace ptcloud long unsigned num_points = cloud.cols; normals = normals.reshape(3, num_points); normals = normals.t(); - // cout << cloud.cols << endl; - // cout << normals.size(); } for (unsigned i = 0; i < max_iters; ++i) { vector current_model_inliers; SACModel model; - for (unsigned j = 0; j < num_rnd_model_points; ++j) { + for (unsigned j = 0; j < num_rnd_model_points;) { std::swap(indices[j], indices[rng.uniform(0, num_points)]); + j++; } for (unsigned j = 0; j < num_rnd_model_points; j++) { @@ -674,8 +690,54 @@ namespace ptcloud } } - inliers.push_back(inliers_indices); - model_instances.push_back(bestModel); + if (bestModel.ModelCoefficients.size()) { + inliers.push_back(inliers_indices); + model_instances.push_back(bestModel); + return true; + } + } + return false; + } + + void SACModelFitting::segment(float remaining_cloud_threshold /*=0.3*/) { + const Vec3f* points = cloud.ptr(0); + unsigned num_points = cloud.cols; + + std::vector indices (num_points); + std::iota(std::begin(indices), std::end(indices), 0); + + std::vector point_labels (num_points, -1); + + unsigned long num_segmented_points = 0; + + unsigned label = 0; + while ( (float) num_segmented_points / num_points < (1 - remaining_cloud_threshold )) { + label = label + 1; + bool successful_fitting = fit_once(point_labels); + + if (!successful_fitting) { + cout << "Could not fit the required model" << endl; + break; + } + vector latest_model_inliers = inliers.back(); + num_segmented_points += latest_model_inliers.size(); + + // This loop erases all occurences of latest inliers in the indices array + // for(int i = 0; i < latest_model_inliers.size(); i++) + // { + // auto iter = std::find(indices.begin(),indices.end(),latest_model_inliers[i]); + // if(iter != indices.end()) + // { + // indices.erase(iter); + // } + // } + + // This loop erases all occurences of latest inliers in the indices array + for(int i = 0; i < latest_model_inliers.size(); i++) + { + point_labels[latest_model_inliers[i]] = label; + } + label++; } } From 5eea16512339ddd582e9435f5a1e8f6810dc58e7 Mon Sep 17 00:00:00 2001 From: Devansh Batra Date: Thu, 30 Jul 2020 16:47:58 +0530 Subject: [PATCH 08/13] removed trailing whitespace and minor warning corrections --- .../opencv2/ptcloud/sac_segmentation.hpp | 14 ++-- .../ptcloud/samples/sample_plane_fitting.cpp | 1 - .../samples/sample_plane_segmentation.cpp | 9 +-- modules/ptcloud/src/sac_segmentation.cpp | 75 +++++++++---------- 4 files changed, 45 insertions(+), 54 deletions(-) diff --git a/modules/ptcloud/include/opencv2/ptcloud/sac_segmentation.hpp b/modules/ptcloud/include/opencv2/ptcloud/sac_segmentation.hpp index b285d5f955e..a892f251afd 100644 --- a/modules/ptcloud/include/opencv2/ptcloud/sac_segmentation.hpp +++ b/modules/ptcloud/include/opencv2/ptcloud/sac_segmentation.hpp @@ -158,7 +158,7 @@ namespace ptcloud std::pair getInliers(Mat cloud, Mat normals, std::vector indices, const double threshold, std::vector& inliers, double normal_distance_weight_ = 0); }; - + class CV_EXPORTS_W SACModelFitting { private: Mat cloud; @@ -207,12 +207,12 @@ namespace ptcloud This stores the model in the public class member model_instances, and the mask for inliers in inliers. */ - bool fit_once(vector remaining_indices = {}); + bool fit_once(vector remaining_indices = {}); /** @brief Fit 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 public class member model_instances, and the corresponding masks for inliers in inliers. - + This stores the models in the public class member model_instances, and the corresponding masks for inliers in inliers. + Returns False if no valid model could be fit. @param remaining_cloud_threshold set the threshold for the remaining cloud (from 0 to 1) until which the segmentation should continue. @@ -239,7 +239,7 @@ namespace ptcloud void set_iterations (long unsigned iterations); /** @brief Set the weight given to normal alignment before comparing overall error with threshold. - * By default it is set to 0. + * By default it is set to 0. @param weight the desired normal alignment weight (between 0 to 1). */ void set_normal_distance_weight(double weight); @@ -248,10 +248,10 @@ namespace ptcloud bool getSphereFromPoints(const Vec3f*&, const vector&, Point3d&, double&); Vec4d getPlaneFromPoints(const Vec3f*&, const std::vector&, cv::Point3d&); - + bool getCylinderFromPoints(Mat cloud, Mat normal, const std::vector &inliers, vector & coefficients) ; - + double euclideanDist(Point3d& p, Point3d& q); } // ptcloud diff --git a/modules/ptcloud/samples/sample_plane_fitting.cpp b/modules/ptcloud/samples/sample_plane_fitting.cpp index 6e6cc35082e..5a62dbe8bb6 100755 --- a/modules/ptcloud/samples/sample_plane_fitting.cpp +++ b/modules/ptcloud/samples/sample_plane_fitting.cpp @@ -19,7 +19,6 @@ int main() { 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(); diff --git a/modules/ptcloud/samples/sample_plane_segmentation.cpp b/modules/ptcloud/samples/sample_plane_segmentation.cpp index 1a664046a4a..1654c303e96 100755 --- a/modules/ptcloud/samples/sample_plane_segmentation.cpp +++ b/modules/ptcloud/samples/sample_plane_segmentation.cpp @@ -12,7 +12,7 @@ using namespace std; int main() { Mat cloud = cv::viz::readCloud("./data/CobbleStones.obj"); - + cv::ptcloud::SACModelFitting planar_segmentation(cloud); // add original cloud to window @@ -25,14 +25,14 @@ int main() { planar_segmentation.set_iterations(1000); planar_segmentation.segment(); - + const Vec3f* points = cloud.ptr(0); - + // Initialise a colors array. These colors will be used (in a cyclic order) to visualise all the segmented planes. const vector colors({viz::Color::green(), viz::Color::blue(), viz::Color::red(), viz::Color::yellow(), viz::Color::orange(),viz::Color::olive()}); // Adds segmented planes to window - for (int model_idx = 0; model_idx < planar_segmentation.inliers.size(); model_idx++) { + for (unsigned model_idx = 0; model_idx < planar_segmentation.inliers.size(); model_idx++) { vector inlier_vec = planar_segmentation.inliers.at(model_idx); cv::Mat fit_cloud(1, inlier_vec.size(), CV_32FC3); for(int j=0; j model_coefficients = planar_segmentation.model_instances.at(0).ModelCoefficients; cv::ptcloud::SACPlaneModel SACplane (model_coefficients); diff --git a/modules/ptcloud/src/sac_segmentation.cpp b/modules/ptcloud/src/sac_segmentation.cpp index 095f28a3dd9..34925092e4f 100644 --- a/modules/ptcloud/src/sac_segmentation.cpp +++ b/modules/ptcloud/src/sac_segmentation.cpp @@ -144,19 +144,17 @@ namespace ptcloud bool getCylinderFromPoints(const Mat cloud, const Mat normals_cld, const std::vector &inliers, vector & model_coefficients) { assert(inliers.size() == 2); - unsigned int num_points = cloud.cols; - + Mat _pointsAndNormals; assert(normals_cld.cols == cloud.cols); const Point3d* points = cloud.ptr(0); const Vec3f* normals = normals_cld.ptr(0); - num_points = cloud.cols; - if (fabs (points[inliers[0]].x - points[inliers[1]].x) <= std::numeric_limits::epsilon () && - fabs (points[inliers[0]].y - points[inliers[1]].y) <= std::numeric_limits::epsilon () && - fabs (points[inliers[0]].z - points[inliers[1]].z) <= std::numeric_limits::epsilon ()) + if (fabs (points[inliers[0]].x - points[inliers[1]].x) <= std::numeric_limits::epsilon () && + fabs (points[inliers[0]].y - points[inliers[1]].y) <= std::numeric_limits::epsilon () && + fabs (points[inliers[0]].z - points[inliers[1]].z) <= std::numeric_limits::epsilon ()) { return (false); } @@ -205,10 +203,10 @@ namespace ptcloud model_coefficients[5] = line_dir[2] / divide_by; double radius_squared = fabs((line_dir.cross(line_pt - p1)).dot(line_dir.cross(line_pt - p1)) / line_dir.dot(line_dir)); - + // radius of cylinder model_coefficients[6] = sqrt(radius_squared); - + if (radius_squared == 0) return false; return (true); @@ -416,7 +414,7 @@ namespace ptcloud this -> pt_on_axis = Point3d(coefficients[0], coefficients[1], coefficients[2]); this -> axis_dir = Vec3d(coefficients[3], coefficients[4], coefficients[5]); this -> radius = coefficients[6]; - + } std::pair SACCylinderModel::getInliers(Mat cloud, Mat normal_cloud, std::vector indices, const double threshold, std::vector& inliers, double normal_distance_weight_) { @@ -489,6 +487,7 @@ namespace ptcloud void SACModelFitting::setCloud(Mat inp_cloud, bool with_normals) { if (! with_normals) { + // normals are not required. // the cloud should have three channels. assert(inp_cloud.channels() == 3 || (inp_cloud.channels() == 1 && (inp_cloud.cols == 3 || inp_cloud.rows == 3))); @@ -500,20 +499,21 @@ namespace ptcloud if (inp_cloud.channels() != 3 && inp_cloud.rows == 3) { inp_cloud = inp_cloud.t(); } + const long unsigned num_points = inp_cloud.rows; cloud = inp_cloud.reshape(3, num_points); cloud = cloud.t(); - const Vec3f* points = cloud.ptr(0); } else { assert(inp_cloud.channels() == 1 && (inp_cloud.cols == 6 || inp_cloud.rows == 6)); if (inp_cloud.rows == 6) { inp_cloud = inp_cloud.t(); } + Mat _cld; inp_cloud.colRange(0, 3).copyTo(_cld); - + Mat _normals; inp_cloud.colRange(3, 6).copyTo(_normals); @@ -529,9 +529,10 @@ namespace ptcloud 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) {} - bool SACModelFitting::fit_once(vector labels /* = {} */) { + bool SACModelFitting::fit_once(vector labels /* = {} */) { - if (method_type != SAC_METHOD_RANSAC) return false; // Only RANSAC supported ATM, need to integrate with Maksym's framework. + // Only RANSAC supported ATM, need to integrate with Maksym's framework. + if (method_type != SAC_METHOD_RANSAC) return false; // 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(0); @@ -559,7 +560,7 @@ namespace ptcloud RNG rng((uint64)-1); for (unsigned i = 0; i < max_iters; ++i) { vector current_model_inliers; - SACModel model; + SACModel model; for (unsigned j = 0; j < num_rnd_model_points;) { std::swap(indices[j], indices[rng.uniform(0, num_points)]); @@ -585,7 +586,7 @@ namespace ptcloud } } - if (bestModel.ModelCoefficients.size()) { + if (bestModel.ModelCoefficients.size()) { inliers.push_back(inliers_indices); model_instances.push_back(bestModel); return true; @@ -616,6 +617,7 @@ namespace ptcloud getSphereFromPoints(points, current_model_inliers, center, radius); SACSphereModel sphereModel (center, radius); pair 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)) { @@ -628,7 +630,7 @@ namespace ptcloud } } - if (bestModel.ModelCoefficients.size()) { + if (bestModel.ModelCoefficients.size()) { inliers.push_back(inliers_indices); model_instances.push_back(bestModel); return true; @@ -639,7 +641,6 @@ namespace ptcloud assert(this->normals_available == true); RNG rng((uint64)-1); const unsigned num_rnd_model_points = 2; - double bestRadius = 10000000; if (!normals_available) { // Reshape the cloud for Compute Normals Function @@ -648,12 +649,12 @@ namespace ptcloud Mat _cld_reshaped = Mat(cloud).t(); _cld_reshaped = _cld_reshaped.reshape(1); ppf_match_3d::computeNormalsPC3d(_cld_reshaped, _pointsAndNormals, 12, false, viewpoint); - + Mat(_pointsAndNormals.colRange(3,6)).copyTo(normals); - long unsigned num_points = cloud.cols; normals = normals.reshape(3, num_points); normals = normals.t(); } + for (unsigned i = 0; i < max_iters; ++i) { vector current_model_inliers; SACModel model; @@ -669,16 +670,15 @@ namespace ptcloud } Point3d center; - double radius; vector coefficients; bool valid_model = getCylinderFromPoints(cloud, normals, current_model_inliers, coefficients); - + if (!valid_model) continue; SACCylinderModel cylinderModel (coefficients); pair result = cylinderModel.getInliers(cloud, normals, indices, threshold, current_model_inliers, normal_distance_weight_); - + // Compare fitness first. if (bestResult.first < result.first || (bestResult.first == result.first && bestResult.second > result.second)) { // if (bestResult.first == result.first && bestModel.ModelCoefficients.size() == 7) continue; @@ -690,31 +690,32 @@ namespace ptcloud } } - if (bestModel.ModelCoefficients.size()) { + + if (bestModel.ModelCoefficients.size()) { inliers.push_back(inliers_indices); model_instances.push_back(bestModel); return true; } + } return false; } void SACModelFitting::segment(float remaining_cloud_threshold /*=0.3*/) { - const Vec3f* points = cloud.ptr(0); unsigned num_points = cloud.cols; std::vector indices (num_points); std::iota(std::begin(indices), std::end(indices), 0); - std::vector point_labels (num_points, -1); + std::vector point_labels (num_points, -1); - unsigned long num_segmented_points = 0; - - unsigned label = 0; + long num_segmented_points = 0; + + int label = 0; while ( (float) num_segmented_points / num_points < (1 - remaining_cloud_threshold )) { label = label + 1; bool successful_fitting = fit_once(point_labels); - + if (!successful_fitting) { cout << "Could not fit the required model" << endl; break; @@ -722,18 +723,10 @@ namespace ptcloud vector latest_model_inliers = inliers.back(); num_segmented_points += latest_model_inliers.size(); - // This loop erases all occurences of latest inliers in the indices array - // for(int i = 0; i < latest_model_inliers.size(); i++) - // { - // auto iter = std::find(indices.begin(),indices.end(),latest_model_inliers[i]); - // if(iter != indices.end()) - // { - // indices.erase(iter); - // } - // } - - // This loop erases all occurences of latest inliers in the indices array - for(int i = 0; i < latest_model_inliers.size(); i++) + // This loop is for implementation purposes only, and maps each point to a label. + // All the points still labelled with -1 are non-segmented. + // This way, complexity of the finding non-segmented is decreased to O(n). + for(unsigned long i = 0; i < latest_model_inliers.size(); i++) { point_labels[latest_model_inliers[i]] = label; } From a3d6b0afe86454ae3e28fb2b9b81869c4428b359 Mon Sep 17 00:00:00 2001 From: berak Date: Mon, 1 Mar 2021 17:33:43 +0100 Subject: [PATCH 09/13] initial ptcloud --- modules/ptcloud/README.md | 26 +++++++++++++++++++++++--- 1 file changed, 23 insertions(+), 3 deletions(-) diff --git a/modules/ptcloud/README.md b/modules/ptcloud/README.md index c719346cdaa..f63a6f086ef 100644 --- a/modules/ptcloud/README.md +++ b/modules/ptcloud/README.md @@ -4,11 +4,31 @@ Point Cloud Module, Object Fitting API ======================================= +Try to segement geometric prmitives 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 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 ----------------------------------------- -- Cylinder Model Fitting -- Segmentation -- Integrate with Maksym's work +- Integrate (better) with Maksym's work //! @} \ No newline at end of file From 1f38693133fc32121b3cdcee8af259b7d81b0023 Mon Sep 17 00:00:00 2001 From: berak Date: Mon, 1 Mar 2021 18:42:49 +0100 Subject: [PATCH 10/13] ptcloud: update --- modules/ptcloud/CMakeLists.txt | 11 +- .../opencv2/ptcloud/sac_segmentation.hpp | 393 ++-- .../include/opencv2/ptcloud/sac_utils.hpp | 38 + .../ptcloud/misc/python/pyopencv_ptcloud.hpp | 34 + modules/ptcloud/misc/python/test/test_sac.py | 35 + modules/ptcloud/perf/perf_main.cpp | 45 + modules/ptcloud/perf/perf_precomp.hpp | 15 + modules/ptcloud/perf/perf_ptcloud_sac.cpp | 52 + modules/ptcloud/samples/sac_demo.cpp | 117 + modules/ptcloud/samples/sac_demo.py | 63 + .../{ => viz}/sample_cylinder_fitting.cpp | 12 +- .../sample_plane.txt} | 0 .../sample_plane_fitting.txt} | 0 .../sample_plane_segmentation.txt} | 0 .../sample_sphere.txt} | 0 .../sample_sphere_fitting.txt} | 0 modules/ptcloud/samples/viz/viz.txt | 34 + modules/ptcloud/src/sac_segmentation.cpp | 1884 +++++++++++------ modules/ptcloud/src/sac_utils.cpp | 60 + modules/ptcloud/test/test_main.cpp | 6 +- modules/ptcloud/test/test_precomp.hpp | 15 +- modules/ptcloud/test/test_sac.cpp | 252 +++ 22 files changed, 2195 insertions(+), 871 deletions(-) create mode 100644 modules/ptcloud/include/opencv2/ptcloud/sac_utils.hpp create mode 100644 modules/ptcloud/misc/python/pyopencv_ptcloud.hpp create mode 100644 modules/ptcloud/misc/python/test/test_sac.py create mode 100644 modules/ptcloud/perf/perf_main.cpp create mode 100644 modules/ptcloud/perf/perf_precomp.hpp create mode 100644 modules/ptcloud/perf/perf_ptcloud_sac.cpp create mode 100644 modules/ptcloud/samples/sac_demo.cpp create mode 100644 modules/ptcloud/samples/sac_demo.py rename modules/ptcloud/samples/{ => viz}/sample_cylinder_fitting.cpp (93%) rename modules/ptcloud/samples/{sample_plane.cpp => viz/sample_plane.txt} (100%) mode change 100755 => 100644 rename modules/ptcloud/samples/{sample_plane_fitting.cpp => viz/sample_plane_fitting.txt} (100%) mode change 100755 => 100644 rename modules/ptcloud/samples/{sample_plane_segmentation.cpp => viz/sample_plane_segmentation.txt} (100%) mode change 100755 => 100644 rename modules/ptcloud/samples/{sample_sphere.cpp => viz/sample_sphere.txt} (100%) mode change 100755 => 100644 rename modules/ptcloud/samples/{sample_sphere_fitting.cpp => viz/sample_sphere_fitting.txt} (100%) mode change 100755 => 100644 create mode 100644 modules/ptcloud/samples/viz/viz.txt create mode 100644 modules/ptcloud/src/sac_utils.cpp create mode 100644 modules/ptcloud/test/test_sac.cpp diff --git a/modules/ptcloud/CMakeLists.txt b/modules/ptcloud/CMakeLists.txt index 9a80ab5e812..b6ef587325a 100644 --- a/modules/ptcloud/CMakeLists.txt +++ b/modules/ptcloud/CMakeLists.txt @@ -1,9 +1,2 @@ -set(the_description "Point Cloud Object Fitting API") -ocv_define_module(ptcloud opencv_core opencv_highgui opencv_viz opencv_surface_matching 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) \ No newline at end of file +set(the_description "Point cloud functionality.") +ocv_define_module(ptcloud opencv_core opencv_calib3d opencv_surface_matching OPTIONAL opencv_viz WRAP python) diff --git a/modules/ptcloud/include/opencv2/ptcloud/sac_segmentation.hpp b/modules/ptcloud/include/opencv2/ptcloud/sac_segmentation.hpp index a892f251afd..900d0e3bd95 100644 --- a/modules/ptcloud/include/opencv2/ptcloud/sac_segmentation.hpp +++ b/modules/ptcloud/include/opencv2/ptcloud/sac_segmentation.hpp @@ -6,254 +6,187 @@ #define OPENCV_PTCLOUD_SAC_SEGMENTATION #include -#include -#include "opencv2/viz.hpp" -#define PLANE_MODEL 1 -#define SPHERE_MODEL 2 -#define CYLINDER_MODEL 3 -#define SAC_METHOD_RANSAC 1 -using namespace std; +#include + namespace cv { namespace ptcloud { - //! @addtogroup ptcloud - //! @{ - class CV_EXPORTS_W SACModel: public Algorithm { - public: - std::vector ModelCoefficients; - - SACModel() { - - } - - SACModel(std::vector ModelCoefficients); - - virtual ~SACModel() - { - - } - - }; - - class CV_EXPORTS_W SACPlaneModel : public SACModel { - private: - Point3d center; - Vec3d normal; - Size2d size = Size2d(2.0, 2.0); - public: - ~ SACPlaneModel() - { - - } - - SACPlaneModel() { - - } - - /** @brief Create a plane model based on the given coefficients and a center point. - - @param coefficients coefficients in the plane equations of type Ax + By + Cz + D = 0. Also obtained using SACModelFitting. - @param center the center point of the plane. - @param size the size of the plane. - */ - - SACPlaneModel(Vec4d coefficients, Point3d center, Size2d size=Size2d(2.0, 2.0)); - - /** @brief Create a plane model based on the given coefficients and an arbitrary center point. - - @param coefficients coefficients in the plane equations Ax + By + Cz + D = 0. - @param size the size of the plane. - */ - SACPlaneModel(Vec4d coefficients, Size2d size=Size2d(2.0, 2.0)); - - /** @brief Create a plane model based on the given coefficients and an arbitrary center point. - - @param coefficients coefficients in the plane equations Ax + By + Cz + D = 0. - @param size the size of the plane. - */ - SACPlaneModel(std::vector coefficients, Size2d size=Size2d(2.0, 2.0)); - - viz::WPlane WindowWidget (); - - std::pair getInliers(Mat cloud, std::vector indices, const double threshold, std::vector& inliers); - }; - - - class CV_EXPORTS_W SACSphereModel : public SACModel { - public: - Point3d center; - double radius; - - ~ SACSphereModel() - { - - } - - SACSphereModel() { - - } - - /** @brief Create a spherical model based on the given center and radius. - - @param center the center point of the sphere - @param radius the radius of the sphere. - */ - - SACSphereModel(Point3d center, double radius); - - /** @brief Create a spherical model based on the parametric coefficients. - - This is very helpful for creating a model for the fit models using SACModelFitting class. - - @param Coefficients parametric coefficients for the Sphere model - */ - - SACSphereModel(const std::vector Coefficients); +//! @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 SACScore; + +/** @brief structure to hold the segmentation results +*/ +class CV_EXPORTS_W SACModel { +public: + CV_PROP std::vector coefficients; //!< the "model" + CV_PROP std::vector indices; //!< into the original cloud + CV_PROP std::vector 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 Probabilistic 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. - SACSphereModel(Vec4d coefficients); + 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; - viz::WSphere WindowWidget (); + /** @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). - double euclideanDist(Point3d& p, Point3d& q); + @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; - std::pair getInliers(Mat cloud, std::vector indices, const double threshold, std::vector& inliers); - }; + /** + @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; - class CV_EXPORTS_W SACCylinderModel : public SACModel { - public: - Point3d pt_on_axis; - Vec3d axis_dir; - double radius; - double size = 20; + /** + @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. - ~ SACCylinderModel() - { + @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 &model_instances, OutputArray new_cloud=noArray()) = 0; - } + /** @brief Initializes a SACModelFitting instance. - SACCylinderModel() { + @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 create(InputArray cloud, int model_type = PLANE_MODEL, int method_type = SAC_METHOD_RANSAC, double threshold = 20, int max_iters = 1000); +}; - } - // /** @brief Create a spherical model based on the given center and radius. +/** @brief Cluster (remaining) points into blobs - // @param center the center point of the sphere - // @param radius the radius of the sphere. - // */ + This is using cv::partition() internally to seperate blobs. - // SACCylinderModel(const std::vector Coefficients); + @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 &models, OutputArray new_cloud=noArray()); - /** @brief Create a spherical model based on the parametric coefficients. - - This is very helpful for creating a model for the fit models using SACModelFitting class. - - @param Coefficients parametric coefficients for the Sphere model - */ - - SACCylinderModel(const std::vector Coefficients); - - viz::WCylinder WindowWidget (); - - std::pair getInliers(Mat cloud, Mat normals, std::vector indices, const double threshold, std::vector& inliers, double normal_distance_weight_ = 0); - }; - - class CV_EXPORTS_W SACModelFitting { - private: - Mat cloud; - Mat normals; - bool normals_available = false; - int model_type; - int method_type; - double threshold; - long unsigned max_iters; - double normal_distance_weight_ = 0; - - public: - // 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> inliers; - vector model_instances; - - /** @brief Initializes SACModelFitting class. - - Threshold and Iterations may also be set separately. - - @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 - currently, only RANSAC is supported (use value SAC_METHOD_RANSAC). - @param threshold set the threshold while choosing inliers. - @param max_iters number of iterations for Sampling. - */ - - 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) {} - - /** @brief Initializes SACModelFitting class. - - Threshold and Iterations may also be set separately. - - @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 - currently, only RANSAC is supported (use value SAC_METHOD_RANSAC). - @param threshold set the threshold while choosing inliers. - @param max_iters number of iterations for Sampling. - */ - 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) {} - - /** @brief Fit one model, this function would get the best fitting model on the given set of points. - - This stores the model in the public class member model_instances, and the mask for inliers in inliers. - */ - bool fit_once(vector remaining_indices = {}); - - /** @brief Fit 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 public class member model_instances, and the corresponding masks for inliers in inliers. - - Returns False if no valid model could be fit. - - @param remaining_cloud_threshold set the threshold for the remaining cloud (from 0 to 1) until which the segmentation should continue. - */ - void segment(float remaining_cloud_threshold = 0.3); - - void setCloud(Mat cloud); - - void setCloud(Mat cloud, bool with_normals=false); - - /** @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. - */ - void set_threshold (double threshold); - - /** @brief Set the number of iterations for the fitting. - - This may be helpful when multiple fitting operations are to be performed. - @param iterations the threshold to set. - */ - void set_iterations (long unsigned iterations); - - /** @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). - */ - void set_normal_distance_weight(double weight); - }; - - bool getSphereFromPoints(const Vec3f*&, const vector&, Point3d&, double&); - - Vec4d getPlaneFromPoints(const Vec3f*&, const std::vector&, cv::Point3d&); - - bool getCylinderFromPoints(Mat cloud, Mat normal, - const std::vector &inliers, vector & coefficients) ; +} // ptcloud +} // cv - double euclideanDist(Point3d& p, Point3d& q); -} // ptcloud -} // cv #endif \ No newline at end of file diff --git a/modules/ptcloud/include/opencv2/ptcloud/sac_utils.hpp b/modules/ptcloud/include/opencv2/ptcloud/sac_utils.hpp new file mode 100644 index 00000000000..9cca09ccabc --- /dev/null +++ b/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 &coeffs, int N=256); +CV_EXPORTS void generateSphere(Mat &cloud, const std::vector &coeffs, int N=256); +CV_EXPORTS void generateCylinder(Mat &cloud, const std::vector &coeffs, int N=256); +CV_EXPORTS void generateRandom(Mat &cloud, const std::vector &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 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 optimizeModel(int model_type, const Mat &cloud, const std::vector &inliers_indices, const std::vector &old_coeff); +CV_EXPORTS SACScore getInliers(int model_type, const std::vector &coeffs, const Mat &cloud, const Mat &normals, const std::vector &indices, const double threshold, const double best_inliers, std::vector& inliers, double normal_distance_weight_, Ptr sprt=Ptr()); +CV_EXPORTS bool generateHypothesis(int model_type, const Mat &cloud, const std::vector ¤t_model_inliers, const Mat &normals, double max_radius, double normal_distance_weight_, std::vector &coefficients); + +} // ptcloud +} // cv + +#endif // OPENCV_SAC_IMPL_HPP diff --git a/modules/ptcloud/misc/python/pyopencv_ptcloud.hpp b/modules/ptcloud/misc/python/pyopencv_ptcloud.hpp new file mode 100644 index 00000000000..7ba1e0ac828 --- /dev/null +++ b/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& 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& src) +{ + int i, n = (int)src.size(); + PyObject* seq = PyList_New(n); + for( i = 0; i < n; i++ ) + { + Ptr 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 vector_SACModel; diff --git a/modules/ptcloud/misc/python/test/test_sac.py b/modules/ptcloud/misc/python/test/test_sac.py new file mode 100644 index 00000000000..eb690e381f0 --- /dev/null +++ b/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(): + 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() diff --git a/modules/ptcloud/perf/perf_main.cpp b/modules/ptcloud/perf/perf_main.cpp new file mode 100644 index 00000000000..1c4f59d5194 --- /dev/null +++ b/modules/ptcloud/perf/perf_main.cpp @@ -0,0 +1,45 @@ +/*M/////////////////////////////////////////////////////////////////////////////////////// +// +// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. +// +// By downloading, copying, installing or using the software you agree to this license. +// If you do not agree to this license, do not download, install, +// copy or use the software. +// +// +// License Agreement +// For Open Source Computer Vision Library +// +// Copyright (C) 2010-2012, Multicoreware, Inc., all rights reserved. +// Copyright (C) 2010-2012, Advanced Micro Devices, Inc., all rights reserved. +// Third party copyrights are property of their respective owners. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// * Redistribution's of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// +// * Redistribution's in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * The name of the copyright holders may not be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// This software is provided by the copyright holders and contributors as is and +// any express or implied warranties, including, but not limited to, the implied +// warranties of merchantability and fitness for a particular purpose are disclaimed. +// In no event shall the Intel Corporation or contributors be liable for any direct, +// indirect, incidental, special, exemplary, or consequential damages +// (including, but not limited to, procurement of substitute goods or services; +// loss of use, data, or profits; or business interruption) however caused +// and on any theory of liability, whether in contract, strict liability, +// or tort (including negligence or otherwise) arising in any way out of +// the use of this software, even if advised of the possibility of such damage. +// +//M*/ + +#include "perf_precomp.hpp" + +CV_PERF_TEST_MAIN(ptcloud) diff --git a/modules/ptcloud/perf/perf_precomp.hpp b/modules/ptcloud/perf/perf_precomp.hpp new file mode 100644 index 00000000000..9a6244b619b --- /dev/null +++ b/modules/ptcloud/perf/perf_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_PRECOMP_HPP__ +#define __OPENCV_TEST_PRECOMP_HPP__ + +#include "opencv2/opencv_modules.hpp" +#include "opencv2/ts.hpp" +#include "opencv2/ts/ts_perf.hpp" +#include "opencv2/ptcloud/sac_segmentation.hpp" +#include "opencv2/ptcloud/sac_utils.hpp" + +using namespace perf; + +#endif diff --git a/modules/ptcloud/perf/perf_ptcloud_sac.cpp b/modules/ptcloud/perf/perf_ptcloud_sac.cpp new file mode 100644 index 00000000000..0250b0dd8e5 --- /dev/null +++ b/modules/ptcloud/perf/perf_ptcloud_sac.cpp @@ -0,0 +1,52 @@ +// 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" + +using namespace std; +using namespace cv; +using namespace perf; + +namespace opencv_test { namespace { + +// sprt(-1)/ransac(0)/preemptive(500) mode, inlier size, outlier size +typedef perf::TestBaseWithParam< tuple > ptcloud_sac; +#define PREC testing::Values(-1,0,500) +#define MODL testing::Values(64,1024) +#define RAND testing::Values(64,1024) + +// +// if the inlier ratio is high, plain ransac is still faster than the preemptive version +// (i guess, there is a high bailout rate then) +// +PERF_TEST_P(ptcloud_sac, segment, testing::Combine(PREC, MODL, RAND)) +{ + int pc = get<0>(GetParam()); + int s1 = get<1>(GetParam()); + int s2 = get<2>(GetParam()); + + Mat cloud; + ptcloud::generatePlane(cloud, std::vector{0.98, 0.13, 0.13, -10.829}, s1); + ptcloud::generateRandom(cloud, std::vector{0,0,0,100}, s2); + + Ptr fit = ptcloud::SACModelFitting::create(cloud); + fit->set_threshold(0.0015); + if (pc == -1) { // test sprt + fit->set_use_sprt(true); + fit->set_preemptive_count(0); + } else { + fit->set_preemptive_count(pc); + } + + std::vector mdl; + + size_t count = 0; // each cycle should segment exactly 1 model + TEST_CYCLE() { fit->segment(mdl); count++; } + + ASSERT_TRUE(mdl.size() == count); + + SANITY_CHECK_NOTHING(); +} + +}} \ No newline at end of file diff --git a/modules/ptcloud/samples/sac_demo.cpp b/modules/ptcloud/samples/sac_demo.cpp new file mode 100644 index 00000000000..472ab53a693 --- /dev/null +++ b/modules/ptcloud/samples/sac_demo.cpp @@ -0,0 +1,117 @@ +#include "opencv2/ptcloud/sac_segmentation.hpp" +#include "opencv2/ptcloud/sac_utils.hpp" +#include "opencv2/surface_matching/ppf_helpers.hpp" + +#include +using namespace std; +using namespace cv; +using namespace cv::ptcloud; + + +int main(int argc,char **argv) { + CommandLineParser parser (argc,argv, + "{help h | | print this help}" + "{model_type m | 1 | 1:plane 2:sphere 3:cylinder 4:cluster 0:all}" + "{use_sprt u | false | use sprt evaluation/termination with non-preemptive ransac}" + "{ply P | | load a .ply file}" + "{threshold t | 0.0015 | rejection threshold .001 for planes, 0.1 for spheres, 2.5 for cylinders}" + "{iters i | 10000 | ransac iterations}" + "{cloud c | 15 | or'ed synthetic model types to generate 0:none 1:planes 2:sphere 4:cylinder 8:random 16:noise distortion 32:from ply}" + "{min_inliers M | 60 | rejection inlier count}" + "{min_distance d | 6 | distance for clustering (partition)}" + "{sac_method s | 0 | SacMethodType (0:RANSAC or 1:MSAC)}" + "{preemptive p | 0 | number of hypotheses for preemptive evaluation. set to 0 to disable}" + "{napsac n | 0 | radius for napsac sampling. set to 0 to disable}" + "{max_sphere S | 50 | (sphere only) reject larger spheres}" + "{normal_weight w | 0.5 | (cylinder only) interpolate between point and normal(dot) distance. setting it to 0 will not use (or generate) normals}" + ""); + if (parser.has("help")) { + parser.printMessage(); + return 0; + } + int typ = parser.get("model_type"); + int iters = parser.get("iters"); + int elems = parser.get("cloud"); + int method = parser.get("sac_method"); + int napsac = parser.get("napsac"); + int min_inliers = parser.get("min_inliers"); + int min_distance = parser.get("min_distance"); + int preemptive = parser.get("preemptive"); + int max_sphere = parser.get("max_sphere"); + float thresh = parser.get("threshold"); + float normal_weight = parser.get("normal_weight"); + bool sprt = parser.get("use_sprt"); + string ply = parser.get("ply"); + + Mat_ pts; + if (! ply.empty()) { + pts = cv::ppf_match_3d::loadPLYSimple(ply.c_str(), false); + } + if (elems & 1) { + generatePlane(pts, vector{0.1,.1,.8, .76}, 64); + generatePlane(pts, vector{-.2,-.7,.3, .16}, 64); + } + if (elems & 2) { + generateSphere(pts, vector{12,15,-12, 5}, 128); + generateSphere(pts, vector{-12,15,-12, 5}, 128); + } + if (elems & 4) { + generateCylinder(pts, vector{-12,-15,-12, 0,0,1, 5}, 256); + generateCylinder(pts, vector{20,5,12, 0,1,0, 5}, 256); + } + if (elems & 8) { + generateRandom(pts, vector{-12,31,-5, 5}, 32); + generateRandom(pts, vector{12,-21,15, 5}, 32); + generateRandom(pts, vector{1,-2,1, 25}, 64); + } + if (elems & 16) { + Mat fuzz(pts.size(), pts.type()); + float F = 0.001; + randu(fuzz,Scalar(-F,-F,-F), Scalar(F,F,F)); + pts += fuzz; + } + cout << pts.size() << " points." << endl; + cv::ppf_match_3d::writePLY(pts, "cloud.ply"); + + auto segment = [napsac,normal_weight,max_sphere,preemptive,sprt](const Mat &cloud, std::vector &models, int model_type, float threshold, int max_iters, int min_inlier, int method_type) -> Mat { + Ptr fitting = SACModelFitting::create(cloud, model_type, method_type, threshold, max_iters); + fitting->set_normal_distance_weight(normal_weight); + fitting->set_max_napsac_radius(napsac); + fitting->set_max_sphere_radius(max_sphere); + fitting->set_preemptive_count(preemptive); + fitting->set_min_inliers(min_inlier); + fitting->set_use_sprt(sprt); + + Mat new_cloud; + fitting->segment(models, new_cloud); + + return new_cloud; + }; + + std::vector models; + if (typ==4) { // cluster only + cv::ptcloud::cluster(pts, min_distance, min_inliers, models, pts); + } else + if (typ==0) { // end to end + pts = segment(pts, models, 1, thresh, iters, min_inliers, method); + cout << pts.total() << " points left." << endl; + pts = segment(pts, models, 2, 0.145, iters, min_inliers, method); + cout << pts.total() << " points left." << endl; + pts = segment(pts, models, 3, 5.0, iters, min_inliers, method); + cout << pts.total() << " points left." << endl; + cv::ptcloud::cluster(pts, 7, 20, models, pts); + } else // single model type + pts = segment(pts, models, typ, thresh, iters, min_inliers, method); + + string names[] = {"", "plane","sphere","cylinder","blob"}; + for (size_t i=0; i #include #include @@ -29,7 +30,7 @@ int main() { viz::Viz3d window("original cloud"); viz::WCloud original_cloud(ptset); window.showWidget("cloud", original_cloud); - + cylinder_segmentation.set_threshold(0.5); cylinder_segmentation.set_iterations(80000); cylinder_segmentation.set_normal_distance_weight(0.5); @@ -59,10 +60,15 @@ int main() { viz::WCloud cloud_widget2(fit_cloud, viz::Color::red()); fitted.showWidget("fit_cloud", cloud_widget2); window.showWidget("fit_cloud", cloud_widget2); - fitted.spin(); + fitted.spin(); window.spin(); waitKey(1); -} \ No newline at end of file +} +#else +int main() { + return CV_ERROR(-215, "this sample needs to be build with opencv's viz module"); +} +#endif \ No newline at end of file diff --git a/modules/ptcloud/samples/sample_plane.cpp b/modules/ptcloud/samples/viz/sample_plane.txt old mode 100755 new mode 100644 similarity index 100% rename from modules/ptcloud/samples/sample_plane.cpp rename to modules/ptcloud/samples/viz/sample_plane.txt diff --git a/modules/ptcloud/samples/sample_plane_fitting.cpp b/modules/ptcloud/samples/viz/sample_plane_fitting.txt old mode 100755 new mode 100644 similarity index 100% rename from modules/ptcloud/samples/sample_plane_fitting.cpp rename to modules/ptcloud/samples/viz/sample_plane_fitting.txt diff --git a/modules/ptcloud/samples/sample_plane_segmentation.cpp b/modules/ptcloud/samples/viz/sample_plane_segmentation.txt old mode 100755 new mode 100644 similarity index 100% rename from modules/ptcloud/samples/sample_plane_segmentation.cpp rename to modules/ptcloud/samples/viz/sample_plane_segmentation.txt diff --git a/modules/ptcloud/samples/sample_sphere.cpp b/modules/ptcloud/samples/viz/sample_sphere.txt old mode 100755 new mode 100644 similarity index 100% rename from modules/ptcloud/samples/sample_sphere.cpp rename to modules/ptcloud/samples/viz/sample_sphere.txt diff --git a/modules/ptcloud/samples/sample_sphere_fitting.cpp b/modules/ptcloud/samples/viz/sample_sphere_fitting.txt old mode 100755 new mode 100644 similarity index 100% rename from modules/ptcloud/samples/sample_sphere_fitting.cpp rename to modules/ptcloud/samples/viz/sample_sphere_fitting.txt diff --git a/modules/ptcloud/samples/viz/viz.txt b/modules/ptcloud/samples/viz/viz.txt new file mode 100644 index 00000000000..e799b933351 --- /dev/null +++ b/modules/ptcloud/samples/viz/viz.txt @@ -0,0 +1,34 @@ +// 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. + +#ifdef HAVE_OPENCV_VIZ +#include +#include +#include + + +using namespace std; +using namespace cv; + +namespace cv +{ +namespace ptcloud +{ + viz::WPlane WindowWidget (const SACPlaneModel &mdl) { + return viz::WPlane (mdl.center, mdl.normal, Vec3d(1, 0, 0), mdl.size, viz::Color::green()); + } + + viz::WSphere WindowWidget (const SACSphereModel &mdl) { + return viz::WSphere(mdl.center, mdl.radius, 10, viz::Color::green());; + } + + viz::WCylinder WindowWidget (const SACCylinderModel &mdl) { + Point3d first_point = Point3d(Vec3d(mdl.pt_on_axis) + size * (mdl.axis_dir)); + Point3d second_point = Point3d(Vec3d(mdl.pt_on_axis) - size * (mdl.axis_dir)); + + return viz::WCylinder (first_point, second_point, mdl.radius, 40, viz::Color::green()); + } +} // ptcloud +} // cv +#endif diff --git a/modules/ptcloud/src/sac_segmentation.cpp b/modules/ptcloud/src/sac_segmentation.cpp index 34925092e4f..5245ad14c81 100644 --- a/modules/ptcloud/src/sac_segmentation.cpp +++ b/modules/ptcloud/src/sac_segmentation.cpp @@ -2,755 +2,1403 @@ // 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 -#include "opencv2/surface_matching.hpp" -#include "opencv2/surface_matching/ppf_helpers.hpp" +#include "opencv2/calib3d.hpp" // lmsolver +#include "opencv2/ptcloud/sac_segmentation.hpp" // public api +#include "opencv2/ptcloud/sac_utils.hpp" // api for tests & samples +#include "opencv2/surface_matching/ppf_helpers.hpp" // ply,normals + #include +#include #include +#include + +namespace cv { +namespace ptcloud { + +SACModel::SACModel(int typ) { + type = typ; + score = std::make_pair(0,1000); +} + +inline +bool fequal(float a, float b) { + return std::abs(a - b) <= std::numeric_limits::epsilon(); +} + +inline +bool fequal(const Vec3f &p1, const Vec3f &p2) { + return (fequal(p1[0], p2[0]) && fequal(p1[1], p2[1]) && fequal(p1[2], p2[2])); +} + +struct box { + Point3f m, M; + double r; +}; + +static box bbox(const Mat &cloud) { + box b; + b.m = Point3f(9999,9999,9999); + b.M = Point3f(-9999,-9999,-9999); + for (size_t i=0; i(i); + b.M.x = std::max(b.M.x,p.x); + b.M.y = std::max(b.M.y,p.y); + b.M.z = std::max(b.M.z,p.z); + b.m.x = std::min(b.m.x,p.x); + b.m.y = std::min(b.m.y,p.y); + b.m.z = std::min(b.m.z,p.z); + } + b.r = (norm(b.M - b.m) / 2); + return b; +} -using namespace std; -using namespace cv; - -namespace cv -{ -namespace ptcloud -{ - - bool getSphereFromPoints(const Vec3f* &points, const std::vector &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(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(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(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(i); - - tempi[2] = tempi[1]; - tempi[1] = (float) points[point_idx][1]; +template +static void knuth_shuffle(std::vector &vec) { + for (size_t t=0; t pts; + Mat_ model; + PlaneModel(const Mat &cloud) : pts(cloud) {} + + virtual void setModelParameters (const Mat &model_) CV_OVERRIDE {model = model_;} + virtual float getError (int point_idx) CV_OVERRIDE { + Point3f p = pts(point_idx); + float a = model(0) * p.x; + float b = model(1) * p.y; + float c = model(2) * p.z; + float d = a + b + c + model(3); + return abs(d); + } +}; - } - double m14 = determinant(temp); +struct SphereModel : public Error_ { + Mat_ pts; + Mat_ model; + SphereModel(const Mat &cloud) : pts(cloud) {} - for(int i = 0; i < 4; i++) - { - unsigned point_idx = inliers[i]; - float* tempi = temp.ptr(i); + virtual void setModelParameters (const Mat &model_) CV_OVERRIDE {model = model_;} + virtual float getError (int point_idx) CV_OVERRIDE { + Point3f p = pts(point_idx); + Point3f center(model(0),model(1),model(2)); + double distanceFromCenter = norm(p - center); + double distanceFromSurface = fabs(distanceFromCenter - model(3)); - 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]; + return distanceFromSurface; + } +}; + +struct CylinderModel : public Error_ { + Mat_ pts, normals; + Mat_ model; + double weight; + Point3d pt_on_axis, axis_dir; + double rsqr; + CylinderModel(const Mat &cloud, const Mat &nrm, double weight_) : pts(cloud), normals(nrm), weight(weight_) {} + + virtual void setModelParameters (const Mat &model_) CV_OVERRIDE { + model = model_; + pt_on_axis = Point3d(model(0),model(1),model(2)); + axis_dir = Point3d(model(3),model(4),model(5)); + rsqr = model(6)*model(6); + } + virtual float getError (int point_idx) CV_OVERRIDE { + Point3d pt = pts(point_idx); + Point3d a_cross = axis_dir.cross(pt_on_axis - pt); + double distanceFromAxis = fabs(a_cross.dot(a_cross) / axis_dir.dot(axis_dir)); + double distanceFromSurface = fabs(distanceFromAxis - rsqr); + + double d_normal = 0.0; + if ((! normals.empty()) && (weight > 0)) { + // Calculate the point's projection on the cylinder axis + float dist = (pt.dot(axis_dir) - pt_on_axis.dot(axis_dir)); + Point3d pt_proj = pt_on_axis + dist * axis_dir; + Point3d dir = pt - pt_proj; + dir = dir / sqrt(dir.dot(dir)); + // Calculate the angular distance between the point normal and the (dir=pt_proj->pt) vector + double rad = normals(point_idx).dot(dir); + if (rad < -1.0) rad = -1.0; + if (rad > 1.0) rad = 1.0; + d_normal = fabs(acos (rad)); + // convert range 0 to PI/2 + d_normal = (std::min) (d_normal, CV_PI - d_normal); } - 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); + // calculate overall distance as weighted sum of the two distances. + return fabs (weight * d_normal + (1 - weight) * distanceFromSurface); + } +}; + + + +struct SPRT_history { + /* + * delta: + * The probability of a data point being consistent + * with a ‘bad’ model is modeled as a probability of + * a random event with Bernoulli distribution with parameter + * δ : p(1|Hb) = δ. + + * epsilon: + * The probability p(1|Hg) = ε + * that any randomly chosen data point is consistent with a ‘good’ model + * is approximated by the fraction of inliers ε among the data + * points + + * A is the decision threshold, the only parameter of the Adapted SPRT + */ + double epsilon, delta, A; + // number of samples processed by test + int tested_samples; // k + SPRT_history () : epsilon(0), delta(0), A(0) { + tested_samples = 0; + } +}; + +///////////////////////////////////// SPRT IMPL ////////////////////////////////////////// +struct SPRTImpl : public SPRT { + RNG rng; + const int points_size; + int highest_inlier_number, current_sprt_idx; // i + // time t_M needed to instantiate a model hypothesis given a sample + // Let m_S be the number of models that are verified per sample + const double inlier_threshold, norm_thr, one_over_thr, t_M, m_S; + + double lowest_sum_errors, current_epsilon, current_delta, current_A, + delta_to_epsilon, complement_delta_to_complement_epsilon; + + std::vector sprt_histories; + + bool last_model_is_good; + + const double log_eta_0; + const int sample_size, MAX_ITERATIONS; + double lambda, sum_errors; + int tested_inliers; + + SACScore score; + const ScoreMethod score_type; + + SPRTImpl (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_) + : rng(state), + points_size(points_size_), + inlier_threshold (inlier_threshold_), + norm_thr(inlier_threshold_*9/4), + one_over_thr (1/norm_thr), + t_M(time_sample), + m_S(avg_num_models), + log_eta_0(log(1-.99/*confidence*/)), + sample_size (sample_size_), + MAX_ITERATIONS(max_iterations_), + score_type(score_type_) { + // reserve (approximately) some space for sprt vector. + sprt_histories.reserve(20); + + createTest(prob_pt_of_good_model, prob_pt_of_bad_model); + + highest_inlier_number = 0; + lowest_sum_errors = std::numeric_limits::max(); + last_model_is_good = false; } - Vec4d getPlaneFromPoints(const Vec3f* &points, - const std::vector &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); + /* + * add a data point, and recalculate lambda + * returns: whether to continue evaluating this model + * @model: model to verify + * p(x(r)|Hb) p(x(j)|Hb) + * lambda(j) = Product (----------) = lambda(j-1) * ---------- + * p(x(r)|Hg) p(x(j)|Hg) + * Set j = 1 + * 1. Check whether j-th data point is consistent with the + * model + * 2. Compute the likelihood ratio λj eq. (1) + * 3. If λj > A, decide the model is ’bad’ (model ”re-jected”), + * else increment j or continue testing + * 4. If j = N the number of correspondences decide model ”accepted” + */ + bool addDataPoint(int tested_point, double error) CV_OVERRIDE { + if (tested_point==0) { + lambda = 1; + sum_errors = 0; + tested_inliers = 0; + lowest_sum_errors = std::numeric_limits::max(); } - - 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); + if (error < inlier_threshold) { + tested_inliers++; + lambda *= delta_to_epsilon; } else { - abc = Vec3d(xy * yz - xz * yy, xy * xz - yz * xx, det_z); + lambda *= complement_delta_to_complement_epsilon; + // since delta is always higher than epsilon, then lambda can increase only + // when point is not consistent with model + if (lambda > current_A) { + return false; + } } + if (score_type == ScoreMethod::SCORE_METHOD_MSAC) { + if (error < norm_thr) + sum_errors -= (1 - error * one_over_thr); + if (sum_errors - points_size + tested_point > lowest_sum_errors) { + return false; + } + } else if (score_type == ScoreMethod::SCORE_METHOD_RANSAC) { + if (tested_inliers + points_size - tested_point < highest_inlier_number) { + return false; + } + }// else errors[points_random_pool[random_pool_idx-1]] = (float)error; + return true; + } - 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); + /* Verifies model and returns model score. + * Return: true if model is good, false - otherwise. + */ + bool isModelGood(int tested_point) CV_OVERRIDE { + last_model_is_good = tested_point == points_size; + // increase number of samples processed by current test + sprt_histories[current_sprt_idx].tested_samples++; + if (last_model_is_good) { + score.first = tested_inliers; + if (score_type == ScoreMethod::SCORE_METHOD_MSAC) { + score.second = sum_errors; + if (lowest_sum_errors > sum_errors) + lowest_sum_errors = sum_errors; + } else if (score_type == ScoreMethod::SCORE_METHOD_RANSAC) + score.second = -static_cast(tested_inliers); + + const double new_epsilon = static_cast(tested_inliers) / points_size; + if (new_epsilon > current_epsilon) { + highest_inlier_number = tested_inliers; // update max inlier number + /* + * Model accepted and the largest support so far: + * design (i+1)-th test (εi + 1= εˆ, δi+1 = δ, i := i + 1). + * Store the current model parameters θ + */ + createTest(new_epsilon, current_delta); + } + } else { + /* + * Since almost all tested models are ‘bad’, the probability + * δ can be estimated as the average fraction of consistent data points + * in rejected models. + */ + // add 1 to tested_point, because loop over tested_point starts from 0 + const double delta_estimated = static_cast (tested_inliers) / (tested_point+1); + if (delta_estimated > 0 && fabs(current_delta - delta_estimated) + / current_delta > 0.05) { + /* + * Model rejected: re-estimate δ. If the estimate δ_ differs + * from δi by more than 5% design (i+1)-th test (εi+1 = εi, + * δi+1 = δˆ, i := i + 1) + */ + createTest(current_epsilon, delta_estimated); + } } - abc /= magnitude_abc; - double d = -abc.dot(centroid); - - Vec4d coefficients (abc[0], abc[1], abc[2], d); - center = Point3d (centroid); - return coefficients; + return last_model_is_good; } - bool getCylinderFromPoints(const Mat cloud, const Mat normals_cld, - const std::vector &inliers, vector & model_coefficients) { - assert(inliers.size() == 2); - - Mat _pointsAndNormals; - - assert(normals_cld.cols == cloud.cols); - - const Point3d* points = cloud.ptr(0); - const Vec3f* normals = normals_cld.ptr(0); - - if (fabs (points[inliers[0]].x - points[inliers[1]].x) <= std::numeric_limits::epsilon () && - fabs (points[inliers[0]].y - points[inliers[1]].y) <= std::numeric_limits::epsilon () && - fabs (points[inliers[0]].z - points[inliers[1]].z) <= std::numeric_limits::epsilon ()) - { - return (false); - } - Vec3f p1 (points[inliers[0]].x, points[inliers[0]].y, points[inliers[0]].z); - Vec3f p2 (points[inliers[1]].x, points[inliers[1]].y, points[inliers[1]].z); - - Vec3f n1 (normals[inliers[0]] [0], normals[inliers[0]] [1], normals[inliers[0]] [2]); - Vec3f n2 (normals[inliers[1]] [0], normals[inliers[1]] [1], normals[inliers[1]] [2]); - Vec3f w = n1 + p1 - p2; - - float a = n1.dot (n1); - float b = n1.dot (n2); - float c = n2.dot (n2); - float d = n1.dot (w); - float e = n2.dot (w); - float denominator = a*c - b*b; - float sc, tc; - // Compute the line parameters of the two closest points - if (denominator < 1e-8) // The lines are almost parallel - { - sc = 0.0f; - tc = (b > c ? d / b : e / c); // Use the largest denominator - } - else - { - sc = (b*e - c*d) / denominator; - tc = (a*e - b*d) / denominator; + SACScore getScore() CV_OVERRIDE { + return score; + } + /* + * Termination criterion: + * l is number of tests + * n(l) = Product from i = 0 to l ( 1 - P_g (1 - A(i)^(-h(i)))^k(i) ) + * log n(l) = sum from i = 0 to l k(i) * ( 1 - P_g (1 - A(i)^(-h(i))) ) + * + * log (n0) - log (n(l-1)) + * k(l) = ----------------------- (9) + * log (1 - P_g*A(l)^-1) + * + * A is decision threshold + * P_g is probability of good model. + * k(i) is number of samples verified by i-th sprt. + * n0 is typically set to 0.05 + * this equation does not have to be evaluated before nR < n0 + * nR = (1 - P_g)^k + */ + int update (int inlier_size) CV_OVERRIDE { + if (sprt_histories.empty()) + return std::min(MAX_ITERATIONS, getStandardUpperBound(inlier_size)); + + const double epsilon = static_cast(inlier_size) / points_size; // inlier probability + const double P_g = pow (epsilon, sample_size); // probability of good sample + + double log_eta_lmin1 = 0; + + int total_number_of_tested_samples = 0; + const int sprts_size_min1 = static_cast(sprt_histories.size())-1; + if (sprts_size_min1 < 0) return getStandardUpperBound(inlier_size); + // compute log n(l-1), l is number of tests + for (int test = 0; test < sprts_size_min1; test++) { + log_eta_lmin1 += log (1 - P_g * (1 - pow (sprt_histories[test].A, + -computeExponentH(sprt_histories[test].epsilon, epsilon,sprt_histories[test].delta)))) + * sprt_histories[test].tested_samples; + total_number_of_tested_samples += sprt_histories[test].tested_samples; } - // point_on_axis, axis_direction - Vec3f line_pt = p1 + n1 + sc * n1; - Vec3f line_dir = p2 + tc * n2 - line_pt; - - model_coefficients.resize (7); - // point on line - model_coefficients[0] = line_pt[0]; - model_coefficients[1] = line_pt[1]; - model_coefficients[2] = line_pt[2]; - - double divide_by = std::sqrt (line_dir[0] * line_dir[0] + - line_dir[1] * line_dir[1] + - line_dir[2] * line_dir[2]); - // direction of line; - model_coefficients[3] = line_dir[0] / divide_by; - model_coefficients[4] = line_dir[1] / divide_by; - model_coefficients[5] = line_dir[2] / divide_by; - - double radius_squared = fabs((line_dir.cross(line_pt - p1)).dot(line_dir.cross(line_pt - p1)) / line_dir.dot(line_dir)); - - // radius of cylinder - model_coefficients[6] = sqrt(radius_squared); - - if (radius_squared == 0) return false; + // Implementation note: since η > ηR the equation (9) does not have to be evaluated + // before ηR < η0 is satisfied. + if (std::pow(1 - P_g, total_number_of_tested_samples) < log_eta_0) + return std::min(MAX_ITERATIONS, getStandardUpperBound(inlier_size)); + // use decision threshold A for last test (l-th) + const double predicted_iters_sprt = (log_eta_0 - log_eta_lmin1) / + log (1 - P_g * (1 - 1 / sprt_histories[sprts_size_min1].A)); // last A + //cout << "pred " << predicted_iters_sprt << endl; + if (std::isnan(predicted_iters_sprt) || std::isinf(predicted_iters_sprt)) + return getStandardUpperBound(inlier_size); + + if (predicted_iters_sprt < 0) return 0; + // compare with standard upper bound + if (predicted_iters_sprt < MAX_ITERATIONS) + return std::min(static_cast(predicted_iters_sprt), + getStandardUpperBound(inlier_size)); + return getStandardUpperBound(inlier_size); + } - return (true); +private: + // Saves sprt test to sprt history and update current epsilon, delta and threshold. + void createTest (double epsilon, double delta) { + // if epsilon is closed to 1 then set them to 0.99 to avoid numerical problems + if (epsilon > 0.999999) epsilon = 0.999; + // delta can't be higher than epsilon, because ratio delta / epsilon will be greater than 1 + if (epsilon < delta) delta = epsilon-0.0001; + // avoid delta going too high as it is very unlikely + // e.g., 30% of points are consistent with bad model is not very real + if (delta > 0.3) delta = 0.3; + + SPRT_history new_sprt_history; + new_sprt_history.epsilon = epsilon; + new_sprt_history.delta = delta; + new_sprt_history.A = estimateThresholdA (epsilon, delta); + + sprt_histories.emplace_back(new_sprt_history); + + current_A = new_sprt_history.A; + current_delta = delta; + current_epsilon = epsilon; + + delta_to_epsilon = delta / epsilon; + complement_delta_to_complement_epsilon = (1 - delta) / (1 - epsilon); + current_sprt_idx = static_cast(sprt_histories.size()) - 1; } - 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]); + /* + * A(0) = K1/K2 + 1 + * A(n+1) = K1/K2 + 1 + log (A(n)) + * K1 = t_M / P_g + * K2 = m_S/(P_g*C) + * t_M is time needed to instantiate a model hypotheses given a sample + * P_g = epsilon ^ m, m is the number of data point in the Ransac sample. + * m_S is the number of models that are verified per sample. + * p (0|Hb) p (1|Hb) + * C = p(0|Hb) log (---------) + p(1|Hb) log (---------) + * p (0|Hg) p (1|Hg) + */ + double estimateThresholdA (double epsilon, double delta) { + const double C = (1 - delta) * log ((1 - delta) / (1 - epsilon)) + + delta * (log(delta / epsilon)); + // K = K1/K2 + 1 = (t_M / P_g) / (m_S / (C * P_g)) + 1 = (t_M * C)/m_S + 1 + const double K = t_M * C / m_S + 1; + double An, An_1 = K; + // compute A using a recursive relation + // A* = lim(n->inf)(An), the series typically converges within 4 iterations + for (int i = 0; i < 10; i++) { + An = K + log(An_1); + if (fabs(An - An_1) < FLT_EPSILON) + break; + An_1 = An; } - this -> size = set_size; - - this -> normal = Vec3d(coefficients[0], coefficients[1], coefficients[2]); - this -> center = set_center; + return An; + } - // Assign normal vector - for (unsigned i = 0; i < 3; i++) normal[i] = coefficients[i]; + inline int getStandardUpperBound(int inlier_size) const { + const double predicted_iters = log_eta_0 / log(1 - std::pow + (static_cast(inlier_size) / points_size, sample_size)); + return (! std::isinf(predicted_iters) && predicted_iters < MAX_ITERATIONS) ? + static_cast(predicted_iters) : MAX_ITERATIONS; + } + /* + * h(i) must hold + * + * δ(i) 1 - δ(i) + * ε (-----)^h(i) + (1 - ε) (--------)^h(i) = 1 + * ε(i) 1 - ε(i) + * + * ε * a^h + (1 - ε) * b^h = 1 + * Has numerical solution. + */ + static double computeExponentH (double epsilon, double epsilon_new, double delta) { + const double a = log (delta / epsilon); // log likelihood ratio + const double b = log ((1 - delta) / (1 - epsilon)); + + const double x0 = log (1 / (1 - epsilon_new)) / b; + const double v0 = epsilon_new * exp (x0 * a); + const double x1 = log ((1 - 2*v0) / (1 - epsilon_new)) / b; + const double v1 = epsilon_new * exp (x1 * a) + (1 - epsilon_new) * exp(x1 * b); + const double h = x0 - (x0 - x1) / (1 + v0 - v1) * v0; + + if (std::isnan(h)) + // The equation always has solution for h = 0 + // ε * a^0 + (1 - ε) * b^0 = 1 + // ε + 1 - ε = 1 -> 1 = 1 + return 0; + return h; } +}; + +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_) { + return makePtr(state, points_size_, + inlier_threshold_, prob_pt_of_good_model, prob_pt_of_bad_model, + time_sample, avg_num_models, sample_size_, max_iterations_, score_type_); +} + +SACScore getInliers(int model_type, const std::vector &coeffs, const Mat &cloud, const Mat &normals, const std::vector &indices, const double threshold, const double best_inliers, std::vector& inliers, double normal_distance_weight_, Ptr sprt) { + inliers.clear(); + + Ptr mdl; + switch (model_type) { + case PLANE_MODEL: mdl = makePtr(cloud); break; + case SPHERE_MODEL: mdl = makePtr(cloud); break; + case CYLINDER_MODEL: mdl = makePtr(cloud, normals, normal_distance_weight_); break; + default: CV_Error(215, format("unsupported model type %d", model_type).c_str()); + } + mdl->setModelParameters(Mat(coeffs)); - 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; + double msac=0; + double norm_thr = (threshold*9/4); + double one_over_thr = (1/norm_thr); + size_t num_points = indices.size(); + + size_t i; + for (i=0; i < num_points; i++) { + if (i == num_points/4) { + if (inliers.size() < best_inliers/8) { + break; + } } - } + double distance = mdl->getError(indices[i]); - SACPlaneModel::SACPlaneModel(vector 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]; - - // 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; + if (!sprt.empty()) { + if (! sprt->addDataPoint(i,distance)) { + break; + } } + + if (distance < norm_thr) + msac -= (1 - distance * one_over_thr); + + if (distance < threshold) + inliers.emplace_back(indices[i]); } - viz::WPlane SACPlaneModel::WindowWidget () { - return viz::WPlane (this->center, this->normal, Vec3d(1, 0, 0), this->size, viz::Color::green()); + if (!sprt.empty()) { + if (sprt->isModelGood(i)) + return sprt->getScore(); } - pair SACPlaneModel::getInliers(Mat cloud, vector indices, const double threshold, vector& inliers) { - pair result; - inliers.clear(); - const Vec3f* points = cloud.ptr(0); - const unsigned num_points = indices.size(); + return SACScore(inliers.size(), msac); +} - double magnitude_abc = sqrt(ModelCoefficients[0]*ModelCoefficients[0] + ModelCoefficients[1]* ModelCoefficients[1] + ModelCoefficients[2] * ModelCoefficients[2]); - assert (magnitude_abc == 0); +// http://ambrsoft.com/TrigoCalc/Sphere/Spher3D_.htm +static bool getSphereFromPoints(const Mat &cloud, const std::vector &inliers, std::vector &coeffs) { + const Vec3f* points = cloud.ptr(0); - 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); + Mat temp(4, 4, CV_32FC1, 1.0f); // last column must be 1 + for (int i = 0; i < 4; i++) { + size_t point_idx = inliers[i]; + float* tempi = temp.ptr(i); + tempi[0] = (float) points[point_idx][0]; + tempi[1] = (float) points[point_idx][1]; + tempi[2] = (float) points[point_idx][2]; + tempi[3] = 1; + } + double m11 = determinant(temp); + if (fequal(m11, 0)) return false; // no sphere exists - fitness+=1; - rmse += distanceFromPlane; - } + for (int i = 0; i < 4; i++) { + size_t point_idx = inliers[i]; + float* tempi = temp.ptr(i); - unsigned num_inliers = fitness; - if (num_inliers == 0) { - result.first = 0; - result.second = 0; - } else { - rmse /= num_inliers; - fitness /= num_points; + 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); - result.first = fitness; - result.second = rmse; - } + for(int i = 0; i < 4; i++) { + size_t point_idx = inliers[i]; + float* tempi = temp.ptr(i); - return result; + tempi[1] = tempi[0]; + tempi[0] = (float) points[point_idx][0]; } - SACSphereModel::SACSphereModel(Point3d set_center, double set_radius) { - this -> center = set_center; - this -> radius = set_radius; + double m13 = determinant(temp); - this -> ModelCoefficients.reserve(4); - this -> ModelCoefficients.push_back(center.x); - this -> ModelCoefficients.push_back(center.y); - this -> ModelCoefficients.push_back(center.z); + for (int i = 0; i < 4; i++) { + size_t point_idx = inliers[i]; + float* tempi = temp.ptr(i); - this -> ModelCoefficients.push_back(radius); + tempi[2] = tempi[1]; + tempi[1] = (float) points[point_idx][1]; } + double m14 = determinant(temp); + for (int i = 0; i < 4; i++) { + size_t point_idx = inliers[i]; + float* tempi = temp.ptr(i); - 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]; + 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]; } - - SACSphereModel::SACSphereModel(vector 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]; + double m15 = determinant(temp); + + Point3d center( + 0.5 * m12 / m11, + 0.5 * m13 / m11, + 0.5 * m14 / m11); + + double radius = std::sqrt( + center.x * center.x + + center.y * center.y + + center.z * center.z - m15 / m11); + + if (std::isnan(radius)) return false; + + coeffs = std::vector {center.x, center.y, center.z, radius}; + return true; +} + +static bool getPlaneFromPoints(const Mat &cloud, const std::vector &inliers, std::vector &coeffs) { + // REF: https://www.ilikebigbits.com/2015_03_04_plane_from_points.html + const Vec3f* points = cloud.ptr(0); + Vec3d abc; + Vec3f centroid(0, 0, 0); + for (size_t 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; - viz::WSphere SACSphereModel::WindowWidget () { - return viz::WSphere(this->center, this->radius, 10, viz::Color::green());; + 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]); - 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); - } + // Return invalid plane if the points don't span a plane. + if (fequal(magnitude_abc, 0)) + return false; - pair SACSphereModel::getInliers(Mat cloud, vector indices, const double threshold, vector& inliers) { - pair result; - inliers.clear(); - const Vec3f* points = cloud.ptr(0); - const unsigned num_points = indices.size(); + abc /= magnitude_abc; + double d = -abc.dot(centroid); - 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); + coeffs = std::vector{abc[0], abc[1], abc[2], d}; + return true; +} - double distanceFromSurface = distanceFromCenter - radius; - if (distanceFromSurface > threshold) continue; - inliers.emplace_back(ind); +// from 2 points & normals +static bool getCylinderFromPoints(const Mat &cloud, const Mat &normals_cld, const std::vector &inliers, std::vector & model_coefficients) { + CV_Assert(inliers.size() == 2); - fitness+=1; - rmse += max(0., distanceFromSurface); - } - } + const Vec3f* points = cloud.ptr(0); + const Vec3f* normals = normals_cld.ptr(0); + Vec3f p1 = points[inliers[0]]; + Vec3f p2 = points[inliers[1]]; - 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; - } + if (fequal(p1, p2)) + return false; - return result; + Vec3f n1 (normals[inliers[0]]); + Vec3f n2 (normals[inliers[1]]); + Vec3f w = n1 + p1 - p2; + + float a = n1.dot(n1); + float b = n1.dot(n2); + float c = n2.dot(n2); + float d = n1.dot(w); + float e = n2.dot(w); + float denominator = a*c - b*b; + float sc, tc; + // Compute the line parameters of the two closest points + if (denominator < 1e-8) { // The lines are almost parallel + sc = 0.0f; + tc = (b > c ? d / b : e / c); // Use the largest denominator + } else { + sc = (b*e - c*d) / denominator; + tc = (a*e - b*d) / denominator; } - viz::WCylinder SACCylinderModel::WindowWidget () { - Point3d first_point = Point3d( Vec3d(pt_on_axis) + size * (axis_dir)); - Point3d second_point = Point3d(Vec3d(pt_on_axis) - size * (axis_dir)); + // point_on_axis, axis_direction + Vec3f line_pt = p1 + n1 + sc * n1; + Vec3f line_dir = p2 + tc * n2 - line_pt; - return viz::WCylinder (first_point, second_point, radius, 40, viz::Color::green()); - } + model_coefficients.resize (7); + // point on line + model_coefficients[0] = line_pt[0]; + model_coefficients[1] = line_pt[1]; + model_coefficients[2] = line_pt[2]; - SACCylinderModel::SACCylinderModel(const vector coefficients) { - assert(coefficients.size() == 7); - for (int i = 0; i < 7; i++) { - this -> ModelCoefficients.push_back(coefficients[i]); - } - this -> pt_on_axis = Point3d(coefficients[0], coefficients[1], coefficients[2]); - this -> axis_dir = Vec3d(coefficients[3], coefficients[4], coefficients[5]); - this -> radius = coefficients[6]; + double line_len_sqr = line_dir.dot(line_dir); + double divide_by = std::sqrt (line_len_sqr); - } + // direction of line; + model_coefficients[3] = line_dir[0] / divide_by; + model_coefficients[4] = line_dir[1] / divide_by; + model_coefficients[5] = line_dir[2] / divide_by; + Vec3f line_normal = line_dir.cross(line_pt - p1); - std::pair SACCylinderModel::getInliers(Mat cloud, Mat normal_cloud, std::vector indices, const double threshold, std::vector& inliers, double normal_distance_weight_) { - pair result; - inliers.clear(); - const Vec3f* points = cloud.ptr(0); - const Vec3f* normals = normal_cloud.ptr(0); - const unsigned num_points = indices.size(); + // radius of cylinder + double radius_squared = fabs(line_normal.dot(line_normal)) / line_len_sqr; + if (fequal(radius_squared, 0)) + return false; + model_coefficients[6] = sqrt(radius_squared); - double fitness = 0; - double rmse = 0; - axis_dir = (axis_dir); + return (true); +} - // for (int i = 0; i < num_points; i++) { - // cout << i << " " << points[i] << endl; - // } - 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]); - Vec3d normal (normals[ind][0], normals[ind][1], normals[ind][2]); - normal = normal / sqrt(normal[0] * normal[0] + normal[1] * normal[1] + normal[2] * normal[2]); +// from 3 points +static bool getCylinderFromPoints(const Mat &cloud, const std::vector &inliers, std::vector & model_coefficients) { + CV_Assert(inliers.size() == 3); - double distanceFromAxis = fabs((axis_dir.cross(pt_on_axis - pt)).dot(axis_dir.cross(pt_on_axis - pt)) / axis_dir.dot(axis_dir)); + const Vec3f* points = cloud.ptr(0); - double distanceFromSurface = fabs(distanceFromAxis - radius*radius); - if (distanceFromSurface > threshold) continue; + // two points form a line on the hull of the cylinder + Vec3f p1 = points[inliers[0]]; + Vec3f p2 = points[inliers[1]]; + // a third point on the opposite side + Vec3f p3 = points[inliers[2]]; + // degenerate if 2 points are same + if (fequal(p1, p2) || fequal(p1, p3) || fequal(p2, p3)) + return false; - // Calculate the point's projection on the cylinder axis - float dist = (pt.dot (axis_dir) - pt_on_axis.dot(axis_dir)); - Vec3d pt_proj = Vec3d(pt_on_axis) + dist * axis_dir; - Vec3f dir = Vec3d(pt) - pt_proj; - dir = dir / sqrt(dir[0] * dir[0] + dir[1] * dir[1] + dir[2] * dir[2]); + Vec3f dr = p2 - p1; + normalize(dr, dr); - // Calculate the angular distance between the point normal and the (dir=pt_proj->pt) vector - double rad = normalize(normal).dot(dir); - if (rad < -1.0) rad = -1.0; - if (rad > 1.0) rad = 1.0; - double d_normal = fabs(acos (rad)); + // distance from p3 to line p1p2 + double a = p3.dot(dr); + Vec3f n = dr - a*p3; + double r = std::sqrt(n.dot(n)); - // convert 0 to PI/2 - d_normal = (std::min) (d_normal, M_PI - d_normal); + // move line_point halfway along the normal (to the center of the cylinder) + model_coefficients = std::vector { + p1[0] + n[0]/2, p1[1] + n[1]/2, p1[2]+n[2]/2, + dr[0], dr[1], dr[2], + r/2 + }; + return (true); +} - // calculate overall distance as weighted sum of the two distances. - double distance = fabs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * distanceFromSurface); +// pca on inlier points +// https://pointclouds.org/documentation/centroid_8hpp_source.html#l00485 +static std::vector optimizePlane(const Mat &pointcloud, + const std::vector &indices, + const std::vector &old_coeffs = {}) { + CV_UNUSED(old_coeffs); - if (distance > threshold) continue; + const Point3f *cloud = pointcloud.ptr(0); - inliers.emplace_back(ind); + // de-mean before accumulating values + Point3f centroid(0, 0, 0); + for (int idx : indices) { + centroid += cloud[idx]; + } + centroid /= double(indices.size()); + + std::vector accu(6, 0); + for (const auto &index : indices) { + const Point3f p = cloud[index] - centroid; + accu[0] += p.x * p.x; + accu[1] += p.x * p.y; + accu[2] += p.x * p.z; + accu[3] += p.y * p.y; + accu[4] += p.y * p.z; + accu[5] += p.z * p.z; + } + Mat(accu) /= float(indices.size()); + + Mat_ covariance_matrix(3,3); + covariance_matrix << + accu[0], accu[1], accu[2], + accu[1], accu[3], accu[4], + accu[2], accu[4], accu[5]; + + Mat_ evec, eval; + eigen(covariance_matrix, eval, evec); + // the evec corresponding to the *smallest* eval + Mat_ ev = evec.row(2); + std::vector coeffs = { + ev(0), ev(1), ev(2), + -1 * (ev(0)*centroid.x + ev(1)*centroid.y + ev(2)*centroid.z) + }; + + return coeffs; +} + +struct SphereSolverCallback : LMSolver::Callback { + Mat_ P; + SphereSolverCallback(const Mat &pts) : P(pts) {} + + bool compute (InputArray param, OutputArray err, OutputArray J) const CV_OVERRIDE { + Mat_ in = param.getMat(); + + Point3f c(in(0),in(1),in(2)); + double r = in(3); + + // the levmarquard solver needs an error metrics for each callback + int count = P.total(); + err.create(count,1,CV_64F); + Mat_ e = err.getMat(); + + // but the jacobian matrix needs only to be (re)generated, if the error falls beyond some threshold + Mat_ j; + if (J.needed()) { + J.create(count, in.total(), CV_64F); + j = J.getMat(); + } - fitness += 1; - rmse += max(0., distance); + for (int i=0; i optimizeSphere(const Mat &pointcloud, + const std::vector &indices, + const std::vector &old_coeffs = {}) { + std::vector new_coeffs = old_coeffs; + if (new_coeffs.empty()) + new_coeffs = std::vector(4,1.0); + + Mat_ pts; + for(auto i:indices) + pts.push_back(pointcloud.at(i)); + + Ptr cb = makePtr(pts); + Ptr lm = LMSolver::create(cb, 100); + int n = lm->run(new_coeffs); + CV_Check(n, n<100, "failed to converge"); + CV_Assert(!std::isnan(new_coeffs[4])); + return new_coeffs; +} + +// https://www.ncbi.nlm.nih.gov/pmc/articles/PMC4890955/ +struct CylinderSolverCallback : LMSolver::Callback { + Mat_ P; + CylinderSolverCallback(const Mat &pts) : P(pts) {} + + bool compute (InputArray param, OutputArray err, OutputArray J) const CV_OVERRIDE { + Mat_ in = param.getMat(); + + double x=in(0), y=in(1), z=in(2); // point on axis + double a=in(3), b=in(4), c=in(5); // axis dir + double r=in(6); // radius + + // normalize dir vec + double len = sqrt(a*a+b*b+c*c) + 0.000001; + a /= len; b /= len; c /= len; + + int count = P.total(); + err.create(count, 1, CV_64F); + Mat_ e = err.getMat(); + + Mat_ j; + if (J.needed()) { + J.create(count, in.total(), CV_64F); + j = J.getMat(); } - return result; + for (int i=0; i optimizeCylinder(const Mat &pointcloud, + const std::vector &indices, + const std::vector &old_coeffs = {}) { + std::vector new_coeffs = old_coeffs; + if (new_coeffs.empty()) + new_coeffs = std::vector(7,1.0); + + Mat_ pts; + for (auto i:indices) + pts.push_back(pointcloud.at(i)); + + Ptr cb = makePtr(pts); + Ptr lm = LMSolver::create(cb, 100); + int n = lm->run(new_coeffs); + CV_Check(n, n<100, "failed to converge"); + + // normalize direction vector: + double len = norm(Point3d(new_coeffs[3],new_coeffs[4],new_coeffs[5])); + new_coeffs[3] /= len; new_coeffs[4] /= len; new_coeffs[5] /= len; + + return new_coeffs; +} + +std::vector optimizeModel(int model_type, const Mat &cloud, const std::vector &inliers_indices, const std::vector &old_coeff) { + switch (model_type) { + case PLANE_MODEL: + return optimizePlane(cloud, inliers_indices, old_coeff); + case SPHERE_MODEL: + return optimizeSphere(cloud, inliers_indices, old_coeff); + case CYLINDER_MODEL: + return optimizeCylinder(cloud, inliers_indices, old_coeff); + default: + CV_Error(215, format("invalid model_type %d", model_type).c_str()); + } + return std::vector(); +} + + +// how many points to sample for a minimal hypothesis +static size_t rnd_model_points(int model_type, const Mat &normals) { + size_t num_rnd_model_points = + model_type==PLANE_MODEL ? 3 : + model_type==SPHERE_MODEL ? 4 : + model_type==CYLINDER_MODEL ? + size_t(normals.empty() + ? 3 // from 3 points + : 2 // 2 points and 2 normals + ) : 0; // invalid + CV_Assert(num_rnd_model_points != 0); // invalid model enum + return num_rnd_model_points; +} + +static std::vector sampleHypothesis(RNG &rng, const Mat &cloud, const std::vector &indices, size_t num_rnd_model_points, double max_napsac_radius) { + std::vector current_model_inliers; + if (max_napsac_radius > 0) { + // NAPSAC like sampling strategy, + // assume good hypothesis inliers are locally close to each other + // (enforce spatial proximity) + int start = rng.uniform(0, indices.size()); + current_model_inliers.emplace_back(start); + Point3f a = cloud.at(indices[start]); + for (int n=0; n<100; n++) { + int next = rng.uniform(0, indices.size()); + Point3f b = cloud.at(indices[next]); + if (norm(a-b) > max_napsac_radius) + continue; + + current_model_inliers.emplace_back(next); + if (current_model_inliers.size() == num_rnd_model_points) + break; + } + } else { + // uniform sample from indices + for (size_t j = 0; j < num_rnd_model_points; j++) + current_model_inliers.emplace_back(rng.uniform(0, indices.size())); } - void SACModelFitting::setCloud(Mat inp_cloud, bool with_normals) { - if (! with_normals) { + return current_model_inliers; +} - // normals are not required. - // the cloud should have three channels. - assert(inp_cloud.channels() == 3 || (inp_cloud.channels() == 1 && (inp_cloud.cols == 3 || inp_cloud.rows == 3))); - if (inp_cloud.rows == 1 && inp_cloud.channels() == 3) { - cloud = inp_cloud.clone(); - return; - } +bool generateHypothesis(int model_type, const Mat &cloud, const std::vector ¤t_model_inliers, const Mat &normals, double max_radius, double normal_distance_weight_, std::vector &coefficients) { + if (model_type == PLANE_MODEL) { + return getPlaneFromPoints(cloud, current_model_inliers, coefficients); + } - if (inp_cloud.channels() != 3 && inp_cloud.rows == 3) { - inp_cloud = inp_cloud.t(); - } + if (model_type == SPHERE_MODEL) { + bool valid = getSphereFromPoints(cloud, current_model_inliers, coefficients); + if (!valid) return false; + if (coefficients[3] > max_radius) return false; + return true; + } - const long unsigned num_points = inp_cloud.rows; - cloud = inp_cloud.reshape(3, num_points); - cloud = cloud.t(); + if (model_type == CYLINDER_MODEL) { + if ((normals.total() > 0) && (normal_distance_weight_ > 0)) + return getCylinderFromPoints(cloud, normals, current_model_inliers, coefficients); + else + return getCylinderFromPoints(cloud, current_model_inliers, coefficients); + } - } - else { - assert(inp_cloud.channels() == 1 && (inp_cloud.cols == 6 || inp_cloud.rows == 6)); - if (inp_cloud.rows == 6) { - inp_cloud = inp_cloud.t(); - } + CV_Error(215, format("unsupported model type %d", model_type).c_str()); + return false; +} + +Mat generateNormals(const Mat &cloud) { + // Reshape the cloud for Compute Normals Function + Mat _cld_reshaped = cloud; + if (_cld_reshaped.rows < _cld_reshaped.cols) + _cld_reshaped = _cld_reshaped.t(); + _cld_reshaped = _cld_reshaped.reshape(1); + + // calculate normals from Knn cloud neighbours + Mat _pointsAndNormals; + ppf_match_3d::computeNormalsPC3d(_cld_reshaped, _pointsAndNormals, 8, false, Vec3d(0,0,0)); + + Mat normals; + Mat(_pointsAndNormals.colRange(3,6)).copyTo(normals); + normals = normals.reshape(3, cloud.total()); + return normals.t(); +} + + +class SACModelFittingImpl : public SACModelFitting { +private: + Mat cloud; + Mat normals; + int model_type; + int method_type; + int preemptive_count = 0; + int min_inliers = 10; + int max_iters; + double threshold; + double normal_distance_weight_ = 0; + double max_sphere_radius = 60; + double max_napsac_radius = 0; + + bool use_sprt = false; + Ptr sprt; + + bool fit_ransac(const std::vector& indices, std::vector &model_instances); + bool fit_preemptive(const std::vector &indices, std::vector &model_instances); + +public: + SACModelFittingImpl (InputArray cloud, int model_type = PLANE_MODEL, int method_type = SAC_METHOD_RANSAC, double threshold = 20,int max_iters = 1000); + + void segment(std::vector &model_instances, OutputArray new_cloud=noArray()) CV_OVERRIDE; + + void set_cloud(InputArray cloud, bool with_normals=false) CV_OVERRIDE; + void set_model_type(int type) CV_OVERRIDE; + void set_method_type(int type) CV_OVERRIDE; + void set_threshold (double threshold) CV_OVERRIDE; + void set_iterations (int iterations) CV_OVERRIDE; + void set_normal_distance_weight(double weight) CV_OVERRIDE; + void set_max_sphere_radius(double max_radius) CV_OVERRIDE; + void set_max_napsac_radius(double max_radius) CV_OVERRIDE; + void set_preemptive_count(int preemptive) CV_OVERRIDE; + void set_min_inliers(int min_inliers) CV_OVERRIDE; + void set_use_sprt(bool sprt) CV_OVERRIDE; +}; + + +Ptr SACModelFitting::create(InputArray cloud, int model_type, int method_type, double threshold, int max_iters) { + return makePtr(cloud, model_type, method_type, threshold, max_iters); +} + +SACModelFittingImpl::SACModelFittingImpl (InputArray inp_cloud, 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), max_iters(set_max_iters), threshold(set_threshold) { + // if there are normals, keep them + Mat input_cloud = inp_cloud.getMat(); + bool with_normals = input_cloud.channels() == 1 && (input_cloud.cols == 6 || input_cloud.rows == 6); + set_cloud(input_cloud, with_normals); +} + +bool SACModelFittingImpl::fit_ransac(const std::vector &indices, std::vector &model_instances) { + RNG rng((uint64)-1); + // Initialize the best future model. + SACModel bestModel, curModel; + + size_t num_rnd_model_points = rnd_model_points(model_type, normals); + CV_Assert(indices.size() >= num_rnd_model_points); + + int stop = stopping(min_inliers, indices.size(), num_rnd_model_points); + + if (use_sprt) { + sprt = SPRT::create(9999, indices.size(), + threshold, 0.01, 0.008, + 200, 3, num_rnd_model_points, max_iters, (ScoreMethod)method_type); + } - Mat _cld; - inp_cloud.colRange(0, 3).copyTo(_cld); + for (int i = 0; i < std::min(stop, max_iters); ++i) { + // generate model hypothesis + curModel = SACModel(SacModelType(model_type)); - Mat _normals; - inp_cloud.colRange(3, 6).copyTo(_normals); + std::vector hypothesis_inliers = sampleHypothesis(rng, cloud, indices, num_rnd_model_points, max_napsac_radius); + if (hypothesis_inliers.size() != num_rnd_model_points) + continue; // bad starting point for napsac - this -> cloud = Mat(_cld).reshape(3, 1); - this -> normals = Mat(_normals).reshape(3, 1); - this -> normals_available = true; - } - } + bool valid_model = generateHypothesis(model_type, cloud, hypothesis_inliers, normals, max_sphere_radius, normal_distance_weight_, curModel.coefficients); + if (!valid_model) + continue; - SACModelFitting::SACModelFitting (Mat set_cloud, int set_model_type, int set_method_type, double set_threshold, int set_max_iters) - :cloud(set_cloud.clone()), model_type(set_model_type), method_type(set_method_type), threshold(set_threshold), max_iters(set_max_iters) {} + // check inliers for the current hypothesis + curModel.score = getInliers(model_type, curModel.coefficients, cloud, normals, indices, threshold, bestModel.score.first, curModel.indices, normal_distance_weight_, sprt); - 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) {} + if (curModel.indices.size() < size_t(min_inliers)) + continue; - bool SACModelFitting::fit_once(vector labels /* = {} */) { + bool better = false; + if (method_type == SAC_METHOD_RANSAC) + better = bestModel.score.first < curModel.score.first; + if (method_type == SAC_METHOD_MSAC) + better = bestModel.score.second > curModel.score.second; - // Only RANSAC supported ATM, need to integrate with Maksym's framework. - if (method_type != SAC_METHOD_RANSAC) return false; + if (better) { + // apply local optimization + // (this could run on a fraction of the indices, but right now it's fast enough to run on all of them) + std::vector new_coeff = optimizeModel(model_type, cloud, curModel.indices, curModel.coefficients); - // 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(0); - unsigned num_points = cloud.cols; + // check again if it improved + std::vector new_inliers; + SACScore new_result = getInliers(model_type, new_coeff, cloud, normals, indices, threshold, curModel.score.first, new_inliers, normal_distance_weight_, sprt); - std::vector indices; + if (method_type == SAC_METHOD_RANSAC) + better = new_result.first > curModel.score.first; + if (method_type == SAC_METHOD_MSAC) + better = new_result.second < curModel.score.second; - if (labels.size() != num_points) { - indices = std::vector (num_points); - std::iota(std::begin(indices), std::end(indices), 0); - } else { - for (unsigned i = 0; i < num_points; i++) { - if (labels[i] == -1) indices.push_back(i); + if (better) { + curModel.score = new_result; + curModel.coefficients = new_coeff; + curModel.indices = new_inliers; } - } - vector inliers_indices; - - // Initialize the best plane model. - SACModel bestModel; - pair 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 current_model_inliers; - SACModel model; - - for (unsigned j = 0; j < num_rnd_model_points;) { - std::swap(indices[j], indices[rng.uniform(0, num_points)]); - j++; - } - - for (unsigned j = 0; j < num_rnd_model_points; j++) { - unsigned idx = indices[j]; - current_model_inliers.emplace_back(idx); - } - - Point3d center; - Vec4d coefficients = getPlaneFromPoints(points, current_model_inliers, center); - if (coefficients == Vec4d(0, 0, 0, 0)) continue; - SACPlaneModel planeModel (coefficients, center); - pair 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; - } + if (!sprt.empty()) + stop = i + sprt->update(curModel.score.first); + else + stop = i + stopping(curModel.score.first, indices.size(), num_rnd_model_points); - } - if (bestModel.ModelCoefficients.size()) { - inliers.push_back(inliers_indices); - model_instances.push_back(bestModel); - return true; - } + bestModel = curModel; } + } - 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 current_model_inliers; - SACModel model; - - for (unsigned j = 0; j < num_rnd_model_points;) { - std::swap(indices[j], indices[rng.uniform(0, num_points)]); - j++; - } - - for (unsigned j = 0; j < num_rnd_model_points; j++) { - unsigned idx = indices[j]; - current_model_inliers.emplace_back(idx); - } - - Point3d center; - double radius; - - getSphereFromPoints(points, current_model_inliers, center, radius); - SACSphereModel sphereModel (center, radius); - pair 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; - } + if (bestModel.coefficients.size() && (bestModel.indices.size() > size_t(min_inliers))) { + model_instances.push_back(bestModel); + return true; + } + return false; +} + +// how many models to retain after each data block +// RaguramECCV08.pdf (5) +inline +double preemptive_func(int i, int M, int B) { + return M * std::pow(2.0, -(double(i)/B)); +} + +// D. Nister, Preemptive RANSAC for live structure and motion estimation, in International Conference on Computer Vision (ICCV), 2003. +bool SACModelFittingImpl::fit_preemptive(const std::vector &indices, std::vector &model_instances) { + RNG rng((uint64)-1); + + size_t num_rnd_model_points = rnd_model_points(model_type, normals); + if (indices.size() < num_rnd_model_points) + return false; - } - if (bestModel.ModelCoefficients.size()) { - inliers.push_back(inliers_indices); - model_instances.push_back(bestModel); - return true; - } - } + // generate model hypotheses in parallel + std::vector preemptive; + while (preemptive.size() < (size_t)preemptive_count) { + SACModel model = SACModel(SacModelType(model_type)); + std::vector hypothesis_inliers = sampleHypothesis(rng, cloud, indices, num_rnd_model_points, max_napsac_radius); + if (hypothesis_inliers.size() != num_rnd_model_points) + continue; // bad starting point for napsac - if (model_type == CYLINDER_MODEL) { - assert(this->normals_available == true); - RNG rng((uint64)-1); - const unsigned num_rnd_model_points = 2; - - if (!normals_available) { - // Reshape the cloud for Compute Normals Function - Mat _pointsAndNormals; - Vec3d viewpoint(0, 0, 0); - Mat _cld_reshaped = Mat(cloud).t(); - _cld_reshaped = _cld_reshaped.reshape(1); - ppf_match_3d::computeNormalsPC3d(_cld_reshaped, _pointsAndNormals, 12, false, viewpoint); - - Mat(_pointsAndNormals.colRange(3,6)).copyTo(normals); - normals = normals.reshape(3, num_points); - normals = normals.t(); - } + bool valid_model = generateHypothesis(model_type, cloud, hypothesis_inliers, normals, max_sphere_radius, normal_distance_weight_, model.coefficients); + if (!valid_model) + continue; - for (unsigned i = 0; i < max_iters; ++i) { - vector current_model_inliers; - SACModel model; + preemptive.push_back(model); + } - for (unsigned j = 0; j < num_rnd_model_points;) { - std::swap(indices[j], indices[rng.uniform(0, num_points)]); - j++; - } + size_t preemptive_pos = 0; // "i", the current data position + size_t preemptive_step = 100; // "B", the block size + while ((preemptive.size() > 1) && (preemptive_pos < indices.size())) { + + // slice a data block + std::vector partial_set( + indices.begin() + preemptive_pos, + indices.begin() + std::min((preemptive_pos + preemptive_step), indices.size())); + preemptive_pos += preemptive_step; + + // parallel evaluation of each data block + for (size_t j=0; j current_model_inliers; + SACScore score = getInliers(model_type, model.coefficients, cloud, normals, partial_set, threshold, 0, current_model_inliers, normal_distance_weight_); + model.score.first += score.first; + model.score.second += score.second; + model.indices.insert(model.indices.end(), current_model_inliers.begin(), current_model_inliers.end()); + } - for (unsigned j = 0; j < num_rnd_model_points; j++) { - unsigned idx = indices[j]; - current_model_inliers.emplace_back(idx); - } + // sort descending + if (method_type == SAC_METHOD_RANSAC) { + std::sort(preemptive.begin(), preemptive.end(), [](const SACModel &a,const SACModel &b) -> bool { + return b.score.first < a.score.first; // RANSAC + }); + } else { + std::sort(preemptive.begin(), preemptive.end(), [](const SACModel &a,const SACModel &b) -> bool { + return b.score.second > a.score.second; // MSAC + }); + } - Point3d center; - vector coefficients; - bool valid_model = getCylinderFromPoints(cloud, normals, current_model_inliers, coefficients); + // prune models + int retain = preemptive_func(preemptive_pos, preemptive_count, preemptive_step); + preemptive.erase(preemptive.begin() + retain + 1, preemptive.end()); + } - if (!valid_model) continue; + SACModel &model = preemptive[0]; + if (model.score.first < min_inliers) + return false; - SACCylinderModel cylinderModel (coefficients); + // "polish" the current best model, it might not have seen all data available. + std::vector new_coeff = optimizeModel(model_type, cloud, model.indices, model.coefficients); + std::vector new_inliers; + SACScore new_result = getInliers(model_type, new_coeff, cloud, normals, indices, threshold, 0, new_inliers, normal_distance_weight_); + if (new_inliers.size() >= model.indices.size()) { + model.score = new_result; + model.indices = new_inliers; + model.coefficients = new_coeff; + } - pair result = cylinderModel.getInliers(cloud, normals, indices, threshold, current_model_inliers, normal_distance_weight_); + model_instances.push_back(model); + return true; +} - // Compare fitness first. - if (bestResult.first < result.first || (bestResult.first == result.first && bestResult.second > result.second)) { - // if (bestResult.first == result.first && bestModel.ModelCoefficients.size() == 7) continue; - bestResult = result; - bestModel.ModelCoefficients = cylinderModel.ModelCoefficients; - bestModel.ModelCoefficients = cylinderModel.ModelCoefficients; - inliers_indices = current_model_inliers; - cout << bestResult.first << endl; - } - } +void SACModelFittingImpl::segment(std::vector &model_instances, OutputArray new_cloud) { + size_t num_points = cloud.total(); - if (bestModel.ModelCoefficients.size()) { - inliers.push_back(inliers_indices); - model_instances.push_back(bestModel); - return true; - } + // optionally generate normals for the Cylinder model + if ((model_type == CYLINDER_MODEL) && (normals.empty()) && (normal_distance_weight_ > 0)) { + normals = generateNormals(cloud); + } + // a "mask" for already segmented points (0 == not_segmented) + std::vector point_labels(cloud.total(), 0); + long num_segmented_points = 0; + int label = 0; // to mark segmented points + while (true) { + label++; + + // filter unused point indices + std::vector indices; + for (size_t i = 0; i < num_points; i++) { + if (point_labels[i] == 0) indices.push_back(i); + } + if (indices.empty()) + break; + + // randomize indices, so we can assume, possible inliers are distributed uniformly. + knuth_shuffle(indices); + + bool successful_fitting = (preemptive_count == 0) + ? fit_ransac(indices, model_instances) + : fit_preemptive(indices, model_instances); + if (!successful_fitting) + break; + + SACModel &latest = model_instances.back(); + num_segmented_points += latest.indices.size(); + + if (num_segmented_points == 0) + break; + + // This loop is for implementation purposes only, and maps each point to a label. + // All the points still labelled with 0 are non-segmented. + // This way, complexity of the finding non-segmented is decreased to O(n). + for(size_t i = 0; i < latest.indices.size(); i++) { + int idx = latest.indices[i]; + point_labels[idx] = label; + latest.points.push_back(cloud.at(idx)); } - return false; } - void SACModelFitting::segment(float remaining_cloud_threshold /*=0.3*/) { - unsigned num_points = cloud.cols; + // optionally, copy remaining non-segmented points + if (new_cloud.needed()) { + Mat _cloud; + for (size_t i=0; i(i)); + } + new_cloud.assign(_cloud); + } +} - std::vector indices (num_points); - std::iota(std::begin(indices), std::end(indices), 0); - std::vector point_labels (num_points, -1); +void SACModelFittingImpl::set_cloud(InputArray inp_cloud, bool with_normals) { + Mat input_cloud = inp_cloud.getMat(); - long num_segmented_points = 0; + if (! with_normals) { + // normals are not required. + normals.release(); + // the cloud should have three channels. + CV_Assert(input_cloud.channels() == 3 || (input_cloud.channels() == 1 && (input_cloud.cols == 3 || input_cloud.rows == 3))); + if (input_cloud.rows == 1 && input_cloud.channels() == 3) { + cloud = input_cloud.clone(); + return; + } - int label = 0; - while ( (float) num_segmented_points / num_points < (1 - remaining_cloud_threshold )) { - label = label + 1; - bool successful_fitting = fit_once(point_labels); + if (input_cloud.channels() != 3 && input_cloud.rows == 3) { + cloud = input_cloud.t(); + } else { + cloud = input_cloud.clone(); + } - if (!successful_fitting) { - cout << "Could not fit the required model" << endl; - break; - } - vector latest_model_inliers = inliers.back(); - num_segmented_points += latest_model_inliers.size(); - - // This loop is for implementation purposes only, and maps each point to a label. - // All the points still labelled with -1 are non-segmented. - // This way, complexity of the finding non-segmented is decreased to O(n). - for(unsigned long i = 0; i < latest_model_inliers.size(); i++) - { - point_labels[latest_model_inliers[i]] = label; - } - label++; + cloud = cloud.reshape(3, input_cloud.rows); + + } else { // with_normals + Mat _cld, _normals; + CV_Assert(input_cloud.channels() == 1 && (input_cloud.cols == 6 || input_cloud.rows == 6)); + if (input_cloud.rows == 6) { + input_cloud.rowRange(0, 3).copyTo(_cld); + input_cloud.rowRange(3, 6).copyTo(_normals); + } else { + input_cloud.colRange(0, 3).copyTo(_cld); + input_cloud.colRange(3, 6).copyTo(_normals); } + cloud = Mat(_cld).reshape(3, 1); + normals = Mat(_normals).reshape(3, 1); } - - void SACModelFitting::set_threshold (double threshold_value) { - threshold = threshold_value; +} + +void SACModelFittingImpl::set_model_type(int type) { + model_type = type; +} + +void SACModelFittingImpl::set_method_type(int type) { + method_type = type; +} + +void SACModelFittingImpl::set_use_sprt(bool sprt_) { + use_sprt = sprt_; +} + +void SACModelFittingImpl::set_threshold (double threshold_value) { + threshold = threshold_value; +} + +void SACModelFittingImpl::set_preemptive_count(int preemptive) { + preemptive_count = preemptive; +} + +void SACModelFittingImpl::set_max_sphere_radius(double max_radius) { + max_sphere_radius = max_radius; +} + +void SACModelFittingImpl::set_max_napsac_radius(double max_radius) { + max_napsac_radius = max_radius; +} + +void SACModelFittingImpl::set_iterations (int iterations) { + max_iters = iterations; +} + +void SACModelFittingImpl::set_min_inliers (int inliers) { + min_inliers = inliers; +} + +void SACModelFittingImpl::set_normal_distance_weight(double weight) { + if (weight > 1) { + normal_distance_weight_ = 1; + } else if (weight < 0) { + normal_distance_weight_ = 0; + } else { + normal_distance_weight_ = weight; } +} + + +void cluster(InputArray _cloud, double distance, int min_inliers, std::vector &models, OutputArray _new_cloud) { + Mat cloud = _cloud.getMat(); + std::vector pts(cloud.begin(), cloud.end()); - void SACModelFitting::set_iterations (long unsigned iterations) { - max_iters = iterations; + std::vector cluster_indices; + int n = cv::partition(pts, cluster_indices, [distance](const Point3f &a, const Point3f &b) -> bool{ + return norm(a-b) < distance; + }); + if (n==0) return; + + std::vector mdl(n); + for (size_t i=0; i 1) { - normal_distance_weight_ = 1; - } else if (weight < 0) { - normal_distance_weight_ = 0; - } else { - normal_distance_weight_ = weight; + Mat_ new_cloud; + for (size_t i=0; i {c.x, c.y, c.z, b.r}; + mdl[i].type = BLOB_MODEL; + models.push_back(mdl[i]); } + _new_cloud.assign(new_cloud); +} } // ptcloud -} // cv +} // cv diff --git a/modules/ptcloud/src/sac_utils.cpp b/modules/ptcloud/src/sac_utils.cpp new file mode 100644 index 00000000000..8109257d197 --- /dev/null +++ b/modules/ptcloud/src/sac_utils.cpp @@ -0,0 +1,60 @@ +#include "opencv2/ptcloud/sac_utils.hpp" + + +namespace cv { +namespace ptcloud { + + +// neither of a,b,c may be 0 ! +void generatePlane(Mat &cloud, const std::vector &coeffs, int N) { + int n=sqrt(N); + for (int i=0; i &coeffs, int N) { + for (int i=0; i &coeffs, int N) { + float R=20; + Point3f cen(coeffs[0],coeffs[1],coeffs[2]); + Point3f dir(coeffs[3],coeffs[4],coeffs[5]); + for (int i=0; i &coeffs, int N) { + float R = coeffs[3]; + for (int i=0; i // std::iota -#include "opencv2/core.hpp" #include "opencv2/ts.hpp" -#include "opencv2/ptcloud.hpp" +#include "opencv2/surface_matching/ppf_helpers.hpp" // normals + +#include "opencv2/ptcloud/sac_segmentation.hpp" +#include "opencv2/ptcloud/sac_utils.hpp" -namespace opencv_test { -using namespace cv::ptcloud; -} #endif diff --git a/modules/ptcloud/test/test_sac.cpp b/modules/ptcloud/test/test_sac.cpp new file mode 100644 index 00000000000..c6513fd6dbf --- /dev/null +++ b/modules/ptcloud/test/test_sac.cpp @@ -0,0 +1,252 @@ +// This file is part of the 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" + + +namespace opencv_test { namespace { + +using namespace cv::ptcloud; + +struct BaseModel { + std::vector coefs; + std::vector indices; + Mat_ cloud; + size_t NPOINTS = 128; + double THRESHOLD = 0.001; + + void generateIndices() { + indices.resize(cloud.total()); + std::iota(indices.begin(),indices.end(),0); + } +}; + +struct PlaneModel : public BaseModel { + PlaneModel() { + NPOINTS = 64; + coefs = std::vector{0.98, 0.13, 0.13, -10.829}; + generatePlane(cloud, coefs, NPOINTS); + generateIndices(); + } +}; + +struct SphereModel : public BaseModel { + SphereModel() { + coefs = std::vector{-8, -4, 10, 5.0}; + generateSphere(cloud, coefs, NPOINTS); + generateIndices(); + } +}; + +struct CylinderModel : public BaseModel { + Point3f dir; + CylinderModel() { + coefs = std::vector{-10,-20,3, 0,0,1, 5.0}; + dir = Point3f(coefs[3],coefs[4],coefs[5]); + generateCylinder(cloud, coefs, NPOINTS); + generateIndices(); + } +}; + +PlaneModel plane; +SphereModel sphere; +CylinderModel cylinder; + +// +// Part 1, check the internal building blocks: +// coefficient generation, inliers, optimization +// + +TEST(SAC, plane) +{ + // 1. see if we get our coefficients back + vector new_coefs; + std::vector hypothesis_inliers {1,8,11}; + bool valid_model = generateHypothesis(PLANE_MODEL, plane.cloud, hypothesis_inliers, Mat(), 50, 0, new_coefs); + double d = cv::norm(Mat(plane.coefs),Mat(new_coefs)); + EXPECT_TRUE(valid_model); + EXPECT_LE(d, 0.05); + + // 2. all plane points should be inliers + std::vector new_indices; + SACScore score = getInliers(PLANE_MODEL, plane.coefs, plane.cloud, Mat(), plane.indices, plane.THRESHOLD, 0, new_indices, 0); + EXPECT_EQ(score.first, plane.NPOINTS); + EXPECT_LE(score.second, plane.THRESHOLD * plane.NPOINTS); + + // 3. no sphere points should be inliers + score = getInliers(PLANE_MODEL, plane.coefs, sphere.cloud, Mat(), sphere.indices, plane.THRESHOLD, 0, new_indices, 0); + EXPECT_EQ(score.first, 0); + + // 4. no cylinder points should be inliers + score = getInliers(PLANE_MODEL, plane.coefs, cylinder.cloud, Mat(), cylinder.indices, plane.THRESHOLD, 0, new_indices, 0); + EXPECT_EQ(score.first, 0); + + // 5. check optimization + new_coefs = optimizeModel(PLANE_MODEL, plane.cloud, plane.indices, plane.coefs); + double d1 = cv::norm(Mat(plane.coefs),Mat(new_coefs)); + EXPECT_LE(d1, 0.05); +} + +TEST(SAC, sphere) +{ + // 1. see if we get our coefficients back + vector new_coefs; + std::vector hypothesis_inliers {1,8,11,22}; + bool valid_model = generateHypothesis(SPHERE_MODEL, sphere.cloud, hypothesis_inliers, Mat(), 50, 0, new_coefs); + double d = cv::norm(Mat(sphere.coefs),Mat(new_coefs)); + EXPECT_TRUE(valid_model); + EXPECT_LE(d, 0.05); + + // 2. all sphere points should be inliers + std::vector new_indices; + SACScore score = getInliers(SPHERE_MODEL, sphere.coefs, sphere.cloud, Mat(), sphere.indices, sphere.THRESHOLD, 0, new_indices, 0); + EXPECT_EQ(score.first, sphere.NPOINTS); + EXPECT_LE(score.second, sphere.THRESHOLD * sphere.NPOINTS); + + // 3. no plane points should be inliers + score = getInliers(SPHERE_MODEL, sphere.coefs, plane.cloud, Mat(), plane.indices, sphere.THRESHOLD, 0, new_indices, 0); + EXPECT_EQ(score.first, 0); + + // 4. no cylinder points should be inliers + score = getInliers(SPHERE_MODEL, sphere.coefs, cylinder.cloud, Mat(), cylinder.indices, sphere.THRESHOLD, 0, new_indices, 0); + EXPECT_EQ(score.first, 0); + + // 5. check optimization + new_coefs = optimizeModel(SPHERE_MODEL, sphere.cloud, sphere.indices, sphere.coefs); + double d1 = cv::norm(Mat(sphere.coefs),Mat(new_coefs)); + EXPECT_LE(d1, 0.05); +} + +TEST(SAC, cylinder) +{ + // 1. see if we get our coefficients back + Mat normals = generateNormals(cylinder.cloud); + vector new_coefs; + std::vector hypothesis_inliers {1,22}; + bool valid_model = generateHypothesis(CYLINDER_MODEL, cylinder.cloud, hypothesis_inliers, normals, 50, 0.5, new_coefs); + // any point on the axis is actually valid, so we can only check the dir and the radius + Point3f dir(new_coefs[3],new_coefs[4],new_coefs[5]); + double d = cylinder.dir.dot(dir); // check if they are parallel + EXPECT_TRUE(valid_model); + EXPECT_GE(abs(d), 0.95); + EXPECT_LE(abs(cylinder.coefs[6] - new_coefs[6]), 0.6); // radius + + // 2. all cylinder points should be inliers + std::vector new_indices; + SACScore score = getInliers(CYLINDER_MODEL, cylinder.coefs, cylinder.cloud, Mat(), cylinder.indices, cylinder.THRESHOLD, 0, new_indices, 0); + EXPECT_EQ(score.first, cylinder.NPOINTS); + EXPECT_LE(score.second, cylinder.THRESHOLD * cylinder.NPOINTS); + + // 3. no plane points should be inliers + score = getInliers(CYLINDER_MODEL, cylinder.coefs, plane.cloud, Mat(), plane.indices, cylinder.THRESHOLD, 0, new_indices, 0); + EXPECT_EQ(score.first, 0); + + // 4. no sphere points should be inliers + score = getInliers(CYLINDER_MODEL, cylinder.coefs, sphere.cloud, Mat(), sphere.indices, cylinder.THRESHOLD, 0, new_indices, 0); + EXPECT_EQ(score.first, 0); + + // 5. check optimization + new_coefs = optimizeModel(CYLINDER_MODEL, cylinder.cloud, cylinder.indices, cylinder.coefs); + double d1 = cv::norm(Mat(cylinder.coefs),Mat(new_coefs)); + EXPECT_LE(d1, 0.05); +} + + +// +// Part 2, check the segmentation: +// + +Mat generateScene() +{ + Mat_ pts; + pts.push_back(plane.cloud); + pts.push_back(sphere.cloud); + pts.push_back(cylinder.cloud); + theRNG().state=99999999; + generateRandom(pts, vector{-12,31,-5, 5}, 32); // these 2 form distinct blobs + generateRandom(pts, vector{12,-21,15, 5}, 32); // that can be retrieved via clustering + generateRandom(pts, vector{1,-2,1, 25}, 64); // general noisy outliers + return pts; +} + +TEST(SAC, segment_ransac) +{ + Mat cloud = generateScene(); + std::vector models; + Ptr fitting = SACModelFitting::create(cloud); + fitting->set_threshold(plane.THRESHOLD); + fitting->segment(models); + EXPECT_EQ(models.size(), size_t(1)); + EXPECT_EQ(models[0].indices.size(), plane.NPOINTS); + double d1 = cv::norm(Mat(plane.coefs), Mat(models[0].coefficients)); + EXPECT_LE(d1, 0.05); + + fitting->set_model_type(SPHERE_MODEL); + fitting->set_threshold(sphere.THRESHOLD); + fitting->segment(models); + EXPECT_EQ(models.size(), size_t(2)); + EXPECT_EQ(models[1].indices.size(), sphere.NPOINTS); + double d2 = cv::norm(Mat(sphere.coefs), Mat(models[1].coefficients)); + EXPECT_LE(d2, 0.05); + + fitting->set_model_type(CYLINDER_MODEL); + fitting->set_threshold(2.62); + fitting->set_normal_distance_weight(0.5); + fitting->set_min_inliers(cylinder.NPOINTS/2); + fitting->segment(models); + EXPECT_EQ(models.size(), size_t(3)); + EXPECT_GE(models[2].indices.size(), cylinder.NPOINTS*0.6); + Point3f dir(models[2].coefficients[3],models[2].coefficients[4],models[2].coefficients[5]); + double d3 = cylinder.dir.dot(dir); // check if they are parallel + EXPECT_GE(abs(d3), 0.65); + EXPECT_LE(abs(cylinder.coefs[6] - models[2].coefficients[6]), 0.6); // radius +} + +TEST(SAC, segment_preemptive) +{ + Mat cloud = generateScene(); + std::vector models; + Ptr fitting = SACModelFitting::create(cloud); + fitting->set_threshold(plane.THRESHOLD); + fitting->set_preemptive_count(500); + fitting->segment(models); + EXPECT_EQ(models.size(), size_t(1)); + EXPECT_EQ(models[0].indices.size(), plane.NPOINTS); + + fitting->set_model_type(SPHERE_MODEL); + fitting->set_threshold(sphere.THRESHOLD); + fitting->segment(models); + EXPECT_EQ(models.size(), size_t(2)); + EXPECT_EQ(models[1].indices.size(), sphere.NPOINTS); + double d2 = cv::norm(Mat(sphere.coefs), Mat(models[1].coefficients)); + EXPECT_LE(d2, 0.05); + + fitting->set_model_type(CYLINDER_MODEL); + fitting->set_threshold(3.62); + fitting->set_normal_distance_weight(0.5); + fitting->set_min_inliers(85); + fitting->segment(models); + EXPECT_EQ(models.size(), size_t(3)); + EXPECT_GE(models[2].indices.size(), cylinder.NPOINTS*0.7); + Point3f dir(models[2].coefficients[3],models[2].coefficients[4],models[2].coefficients[5]); + double d3 = cylinder.dir.dot(dir); // check if they are parallel + EXPECT_GE(abs(d3), 0.65); + EXPECT_LE(abs(cylinder.coefs[6] - models[2].coefficients[6]), 0.6); // radius +} + +TEST(SAC, cluster) +{ + Mat cloud = generateScene(); + cv::ppf_match_3d::writePLY(Mat(cloud), "cloud.ply"); + std::vector models; + Mat new_cloud; + cluster(cloud, 5, 20, models, new_cloud); + EXPECT_EQ(models.size(), size_t(4)); // sphere, cylinder, 2 blobs + // the plane needs a larger distance, so do a 2nd pass on the leftover points + cluster(new_cloud, 8, 20, models, new_cloud); + EXPECT_EQ(models.size(), size_t(5)); + EXPECT_LE(new_cloud.total(), size_t(64)); // the last large random blob +} + +}} // namespace From 39dd595a896c53e7493441079114684fa965445f Mon Sep 17 00:00:00 2001 From: berak Date: Tue, 2 Mar 2021 11:18:33 +0100 Subject: [PATCH 11/13] ptcloud: viz samples --- .../samples/viz/sample_cylinder_fitting.cpp | 44 ++++++------- modules/ptcloud/samples/viz/sample_plane.txt | 48 --------------- .../samples/viz/sample_plane_fitting.cpp | 44 +++++++++++++ .../samples/viz/sample_plane_fitting.txt | 47 -------------- .../samples/viz/sample_plane_segmentation.txt | 60 ------------------ modules/ptcloud/samples/viz/sample_sphere.txt | 42 ------------- .../samples/viz/sample_sphere_fitting.cpp | 61 +++++++++++++++++++ .../samples/viz/sample_sphere_fitting.txt | 42 ------------- modules/ptcloud/samples/viz/viz.txt | 34 ----------- 9 files changed, 125 insertions(+), 297 deletions(-) delete mode 100644 modules/ptcloud/samples/viz/sample_plane.txt create mode 100644 modules/ptcloud/samples/viz/sample_plane_fitting.cpp delete mode 100644 modules/ptcloud/samples/viz/sample_plane_fitting.txt delete mode 100644 modules/ptcloud/samples/viz/sample_plane_segmentation.txt delete mode 100644 modules/ptcloud/samples/viz/sample_sphere.txt create mode 100644 modules/ptcloud/samples/viz/sample_sphere_fitting.cpp delete mode 100644 modules/ptcloud/samples/viz/sample_sphere_fitting.txt delete mode 100644 modules/ptcloud/samples/viz/viz.txt diff --git a/modules/ptcloud/samples/viz/sample_cylinder_fitting.cpp b/modules/ptcloud/samples/viz/sample_cylinder_fitting.cpp index 97c1c3a877b..2350bf3636a 100644 --- a/modules/ptcloud/samples/viz/sample_cylinder_fitting.cpp +++ b/modules/ptcloud/samples/viz/sample_cylinder_fitting.cpp @@ -1,4 +1,5 @@ #ifdef HAVE_OPENCV_VIZ + #include #include #include @@ -17,55 +18,50 @@ using namespace std; int main() { // Mat cloud = cv::ppf_match_3d::loadPLYSimple("./data/semi-cylinder-with-normals-usingOpenCV2.ply", true); Mat cloud = cv::ppf_match_3d::loadPLYSimple("./data/cylinder-big.ply", false); + Mat ptset; Mat(cloud.colRange(0,3)).copyTo(ptset); long unsigned num_points = ptset.rows; ptset = ptset.reshape(3, num_points); ptset = ptset.t(); - cv::ptcloud::SACModelFitting cylinder_segmentation(CYLINDER_MODEL); - cylinder_segmentation.setCloud(cloud, false); + Ptr cylinder_segmentation = cv::ptcloud::SACModelFitting::create(cloud, CYLINDER_MODEL); // add original cloud to window viz::Viz3d window("original cloud"); viz::WCloud original_cloud(ptset); window.showWidget("cloud", original_cloud); - cylinder_segmentation.set_threshold(0.5); - cylinder_segmentation.set_iterations(80000); - cylinder_segmentation.set_normal_distance_weight(0.5); - cylinder_segmentation.fit_once(); + cylinder_segmentation->set_threshold(0.5); + cylinder_segmentation->set_iterations(80000); + cylinder_segmentation->set_normal_distance_weight(0.5); - cout << cylinder_segmentation.inliers.size(); - vector inlier_vec = cylinder_segmentation.inliers.at(0); + vector models; + cylinder_segmentation->segment(models); + cout << models[0].points.size(); + vector model_coefficients = models.at(0).coefficients; + cout << model_coefficients.size(); - vector model_coefficients = cylinder_segmentation.model_instances.at(0).ModelCoefficients; - cout << cylinder_segmentation.model_instances.at(0).ModelCoefficients.size(); - cv::ptcloud::SACCylinderModel cylinder (model_coefficients); - cout << cylinder.pt_on_axis << endl; - cout << cylinder.axis_dir << endl; - cout << cylinder.radius << endl; - - viz::WCylinder model = cylinder.WindowWidget(); + double size = 10; + double radius = model_coefficients[6]; + Point3d pt_on_axis(model_coefficients[0], model_coefficients[1], model_coefficients[2]); + Point3d axis_dir(model_coefficients[3], model_coefficients[4], model_coefficients[5]); + Point3d first_point = Point3d(Vec3d(pt_on_axis) + size * (axis_dir)); + Point3d second_point = Point3d(Vec3d(pt_on_axis) - size * (axis_dir)); + viz::WCylinder model(first_point, second_point, radius, 40, viz::Color::green()); window.showWidget("model", model); - const Vec3f* points = ptset.ptr(0); - cout << endl << endl << inlier_vec.size(); - cv::Mat fit_cloud(1, inlier_vec.size(), CV_32FC3); - for(int j=0; j(0, j) = points[(j)]; - } viz::Viz3d fitted("fitted cloud"); - viz::WCloud cloud_widget2(fit_cloud, viz::Color::red()); + viz::WCloud cloud_widget2(models[0].points, viz::Color::red()); fitted.showWidget("fit_cloud", cloud_widget2); window.showWidget("fit_cloud", cloud_widget2); fitted.spin(); - window.spin(); waitKey(1); + return 0; } #else int main() { diff --git a/modules/ptcloud/samples/viz/sample_plane.txt b/modules/ptcloud/samples/viz/sample_plane.txt deleted file mode 100644 index 6e6cc35082e..00000000000 --- a/modules/ptcloud/samples/viz/sample_plane.txt +++ /dev/null @@ -1,48 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include - -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(0); - vector inlier_vec = planar_segmentation.inliers.at(0); - cv::Mat fit_cloud(1, inlier_vec.size(), CV_32FC3); - for(int j=0; j(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 model_coefficients = planar_segmentation.model_instances.at(0).ModelCoefficients; - cv::ptcloud::SACPlaneModel SACplane (model_coefficients); - - window.spin(); - fitted.spin(); - waitKey(1); - -} \ No newline at end of file diff --git a/modules/ptcloud/samples/viz/sample_plane_fitting.cpp b/modules/ptcloud/samples/viz/sample_plane_fitting.cpp new file mode 100644 index 00000000000..82ef6f5acac --- /dev/null +++ b/modules/ptcloud/samples/viz/sample_plane_fitting.cpp @@ -0,0 +1,44 @@ +#ifdef HAVE_OPENCV_VIZ +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace cv; +using namespace std; + +int main() { + Mat cloud = cv::viz::readCloud("./data/CobbleStones.obj"); + Ptr planar_segmentation = cv::ptcloud::SACModelFitting::create(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); + + vector models; + planar_segmentation->segment(models); + CV_Assert(models.size()>0); + + viz::Viz3d fitted("fitted cloud"); + viz::WCloud cloud_widget2(Mat(models[0].points), viz::Color::green()); + fitted.showWidget("fit plane", cloud_widget2); + window.showWidget("fit plane", cloud_widget2); + + window.spin(); + fitted.spin(); + waitKey(1); + return 0; +} +#else +int main() { + return CV_ERROR(-215, "this sample needs to be build with opencv's viz module"); +} +#endif \ No newline at end of file diff --git a/modules/ptcloud/samples/viz/sample_plane_fitting.txt b/modules/ptcloud/samples/viz/sample_plane_fitting.txt deleted file mode 100644 index 5a62dbe8bb6..00000000000 --- a/modules/ptcloud/samples/viz/sample_plane_fitting.txt +++ /dev/null @@ -1,47 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include - -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(0); - vector inlier_vec = planar_segmentation.inliers.at(0); - cv::Mat fit_cloud(1, inlier_vec.size(), CV_32FC3); - for(int j=0; j(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 model_coefficients = planar_segmentation.model_instances.at(0).ModelCoefficients; - cv::ptcloud::SACPlaneModel SACplane (model_coefficients); - - window.spin(); - fitted.spin(); - waitKey(1); - -} \ No newline at end of file diff --git a/modules/ptcloud/samples/viz/sample_plane_segmentation.txt b/modules/ptcloud/samples/viz/sample_plane_segmentation.txt deleted file mode 100644 index 1654c303e96..00000000000 --- a/modules/ptcloud/samples/viz/sample_plane_segmentation.txt +++ /dev/null @@ -1,60 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include - -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.segment(); - - - const Vec3f* points = cloud.ptr(0); - - // Initialise a colors array. These colors will be used (in a cyclic order) to visualise all the segmented planes. - const vector colors({viz::Color::green(), viz::Color::blue(), viz::Color::red(), viz::Color::yellow(), viz::Color::orange(),viz::Color::olive()}); - - // Adds segmented planes to window - for (unsigned model_idx = 0; model_idx < planar_segmentation.inliers.size(); model_idx++) { - vector inlier_vec = planar_segmentation.inliers.at(model_idx); - cv::Mat fit_cloud(1, inlier_vec.size(), CV_32FC3); - for(int j=0; j(0, j) = points[inlier_vec.at(j)]; - - viz::Viz3d fitted("fit cloud " + to_string(model_idx + 1)); - fitted.showWidget("cloud", original_cloud); - - // Assign a color to this cloud from the colors array in a cyclic order. - viz::Color cloud_color = colors[model_idx % colors.size()]; - viz::WCloud cloud_widget2(fit_cloud, cloud_color); - fitted.showWidget("fit plane", cloud_widget2); - window.showWidget("fit plane " + to_string(model_idx + 1), cloud_widget2); - - vector model_coefficients = planar_segmentation.model_instances.at(0).ModelCoefficients; - cv::ptcloud::SACPlaneModel SACplane (model_coefficients); - - fitted.spin(); - } - - window.spin(); - - // waitKey(1); - -} \ No newline at end of file diff --git a/modules/ptcloud/samples/viz/sample_sphere.txt b/modules/ptcloud/samples/viz/sample_sphere.txt deleted file mode 100644 index 9616e06ce74..00000000000 --- a/modules/ptcloud/samples/viz/sample_sphere.txt +++ /dev/null @@ -1,42 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include - -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 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); - -} \ No newline at end of file diff --git a/modules/ptcloud/samples/viz/sample_sphere_fitting.cpp b/modules/ptcloud/samples/viz/sample_sphere_fitting.cpp new file mode 100644 index 00000000000..5e8958705c5 --- /dev/null +++ b/modules/ptcloud/samples/viz/sample_sphere_fitting.cpp @@ -0,0 +1,61 @@ +#ifdef HAVE_OPENCV_VIZ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace cv; +using namespace std; + +int main() { + Mat cloud = cv::viz::readCloud("./data/sphere-big.obj"); + Ptr sphere_segmentation = cv::ptcloud::SACModelFitting::create(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); + sphere_segmentation->set_max_radius(10000); + + viz::Viz3d fitted("fitted cloud"); + + vector models; + sphere_segmentation->segment(models); + CV_Assert(models.size()>0); + + viz::Viz3d fitted("fitted cloud"); + viz::WCloud cloud_widget2(Mat(models[0].points), viz::Color::green()); + fitted.showWidget("fit plane", cloud_widget2); + + vector model_coefficients = models.at(0).coefficients; + cout << model_coefficients.size(); + + Point3d center(model_coefficients[0],model_coefficients[1],model_coefficients[2]); + double radius(model_coefficients[3]); + cout << center; + cout << radius; + radius *= 0.75; + + viz::WSphere sphere(center, radius, 10, viz::Color::green());; + window.showWidget("model", sphere); + + window.spin(); + fitted.spin(); + + waitKey(1); + + return 0; +} +#else +int main() { + return CV_ERROR(-215, "this sample needs to be build with opencv's viz module"); +} +#endif \ No newline at end of file diff --git a/modules/ptcloud/samples/viz/sample_sphere_fitting.txt b/modules/ptcloud/samples/viz/sample_sphere_fitting.txt deleted file mode 100644 index 9616e06ce74..00000000000 --- a/modules/ptcloud/samples/viz/sample_sphere_fitting.txt +++ /dev/null @@ -1,42 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include - -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 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); - -} \ No newline at end of file diff --git a/modules/ptcloud/samples/viz/viz.txt b/modules/ptcloud/samples/viz/viz.txt deleted file mode 100644 index e799b933351..00000000000 --- a/modules/ptcloud/samples/viz/viz.txt +++ /dev/null @@ -1,34 +0,0 @@ -// 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. - -#ifdef HAVE_OPENCV_VIZ -#include -#include -#include - - -using namespace std; -using namespace cv; - -namespace cv -{ -namespace ptcloud -{ - viz::WPlane WindowWidget (const SACPlaneModel &mdl) { - return viz::WPlane (mdl.center, mdl.normal, Vec3d(1, 0, 0), mdl.size, viz::Color::green()); - } - - viz::WSphere WindowWidget (const SACSphereModel &mdl) { - return viz::WSphere(mdl.center, mdl.radius, 10, viz::Color::green());; - } - - viz::WCylinder WindowWidget (const SACCylinderModel &mdl) { - Point3d first_point = Point3d(Vec3d(mdl.pt_on_axis) + size * (mdl.axis_dir)); - Point3d second_point = Point3d(Vec3d(mdl.pt_on_axis) - size * (mdl.axis_dir)); - - return viz::WCylinder (first_point, second_point, mdl.radius, 40, viz::Color::green()); - } -} // ptcloud -} // cv -#endif From 1f37ca344430b8f7b08a5850c184b7ee7927ef02 Mon Sep 17 00:00:00 2001 From: berak Date: Tue, 2 Mar 2021 12:45:10 +0100 Subject: [PATCH 12/13] ptcloud: bib --- modules/ptcloud/README.md | 3 ++- modules/ptcloud/doc/ptcloud.bib | 24 ++++++++++++++++++++++++ 2 files changed, 26 insertions(+), 1 deletion(-) diff --git a/modules/ptcloud/README.md b/modules/ptcloud/README.md index f63a6f086ef..3b8fb47d2a9 100644 --- a/modules/ptcloud/README.md +++ b/modules/ptcloud/README.md @@ -4,7 +4,7 @@ Point Cloud Module, Object Fitting API ======================================= -Try to segement geometric prmitives like planes, spheres and cylinders from a 3d point cloud +Try to segment geometric primitives like planes, spheres and cylinders from a 3d point cloud 2 alternative ransac strategies are implemented here: @@ -12,6 +12,7 @@ Try to segement geometric prmitives like planes, spheres and cylinders from a 3d + 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) diff --git a/modules/ptcloud/doc/ptcloud.bib b/modules/ptcloud/doc/ptcloud.bib index e69de29bb2d..35915bb128b 100644 --- a/modules/ptcloud/doc/ptcloud.bib +++ b/modules/ptcloud/doc/ptcloud.bib @@ -0,0 +1,24 @@ +@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{RA13 + 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} +} \ No newline at end of file From 398a9c774706382a3cd78f71f2f318a6d772f675 Mon Sep 17 00:00:00 2001 From: berak Date: Wed, 17 Mar 2021 09:00:58 +0100 Subject: [PATCH 13/13] fix test and float conversions --- modules/ptcloud/doc/ptcloud.bib | 13 +- .../opencv2/ptcloud/sac_segmentation.hpp | 2 +- modules/ptcloud/misc/python/test/test_sac.py | 2 +- modules/ptcloud/perf/perf_main.cpp | 44 +----- modules/ptcloud/samples/sac_demo.cpp | 148 +++++++++--------- .../samples/viz/sample_cylinder_fitting.cpp | 3 +- .../samples/viz/sample_plane_fitting.cpp | 1 + .../samples/viz/sample_sphere_fitting.cpp | 1 + modules/ptcloud/src/sac_segmentation.cpp | 87 +++++----- modules/ptcloud/src/sac_utils.cpp | 43 ++--- modules/ptcloud/test/test_sac.cpp | 75 +++++---- 11 files changed, 199 insertions(+), 220 deletions(-) diff --git a/modules/ptcloud/doc/ptcloud.bib b/modules/ptcloud/doc/ptcloud.bib index 35915bb128b..7112d48f924 100644 --- a/modules/ptcloud/doc/ptcloud.bib +++ b/modules/ptcloud/doc/ptcloud.bib @@ -15,10 +15,19 @@ @inproceedings{NI03 year={2003} } -@article{RA13 +@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}, + 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} } \ No newline at end of file diff --git a/modules/ptcloud/include/opencv2/ptcloud/sac_segmentation.hpp b/modules/ptcloud/include/opencv2/ptcloud/sac_segmentation.hpp index 900d0e3bd95..bc1f88b2bb1 100644 --- a/modules/ptcloud/include/opencv2/ptcloud/sac_segmentation.hpp +++ b/modules/ptcloud/include/opencv2/ptcloud/sac_segmentation.hpp @@ -94,7 +94,7 @@ class CV_EXPORTS_W SACModelFitting { */ CV_WRAP virtual void set_method_type(int method_type) = 0; - /** @brief Use Wald's Sequential Probabilistic Ratio Test with ransac fitting + /** @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 diff --git a/modules/ptcloud/misc/python/test/test_sac.py b/modules/ptcloud/misc/python/test/test_sac.py index eb690e381f0..f2cbc2de8e4 100644 --- a/modules/ptcloud/misc/python/test/test_sac.py +++ b/modules/ptcloud/misc/python/test/test_sac.py @@ -14,7 +14,7 @@ class sac_test(NewOpenCVTests): - def test_plane(): + def test_plane(self): N = 64; plane = np.zeros((N,N,3),np.float32) for i in range(0,N): diff --git a/modules/ptcloud/perf/perf_main.cpp b/modules/ptcloud/perf/perf_main.cpp index 1c4f59d5194..7852d0939ec 100644 --- a/modules/ptcloud/perf/perf_main.cpp +++ b/modules/ptcloud/perf/perf_main.cpp @@ -1,44 +1,6 @@ -/*M/////////////////////////////////////////////////////////////////////////////////////// -// -// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. -// -// By downloading, copying, installing or using the software you agree to this license. -// If you do not agree to this license, do not download, install, -// copy or use the software. -// -// -// License Agreement -// For Open Source Computer Vision Library -// -// Copyright (C) 2010-2012, Multicoreware, Inc., all rights reserved. -// Copyright (C) 2010-2012, Advanced Micro Devices, Inc., all rights reserved. -// Third party copyrights are property of their respective owners. -// -// Redistribution and use in source and binary forms, with or without modification, -// are permitted provided that the following conditions are met: -// -// * Redistribution's of source code must retain the above copyright notice, -// this list of conditions and the following disclaimer. -// -// * Redistribution's in binary form must reproduce the above copyright notice, -// this list of conditions and the following disclaimer in the documentation -// and/or other materials provided with the distribution. -// -// * The name of the copyright holders may not be used to endorse or promote products -// derived from this software without specific prior written permission. -// -// This software is provided by the copyright holders and contributors as is and -// any express or implied warranties, including, but not limited to, the implied -// warranties of merchantability and fitness for a particular purpose are disclaimed. -// In no event shall the Intel Corporation or contributors be liable for any direct, -// indirect, incidental, special, exemplary, or consequential damages -// (including, but not limited to, procurement of substitute goods or services; -// loss of use, data, or profits; or business interruption) however caused -// and on any theory of liability, whether in contract, strict liability, -// or tort (including negligence or otherwise) arising in any way out of -// the use of this software, even if advised of the possibility of such damage. -// -//M*/ +// 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" diff --git a/modules/ptcloud/samples/sac_demo.cpp b/modules/ptcloud/samples/sac_demo.cpp index 472ab53a693..c3196443b84 100644 --- a/modules/ptcloud/samples/sac_demo.cpp +++ b/modules/ptcloud/samples/sac_demo.cpp @@ -9,26 +9,26 @@ using namespace cv::ptcloud; int main(int argc,char **argv) { - CommandLineParser parser (argc,argv, - "{help h | | print this help}" - "{model_type m | 1 | 1:plane 2:sphere 3:cylinder 4:cluster 0:all}" - "{use_sprt u | false | use sprt evaluation/termination with non-preemptive ransac}" - "{ply P | | load a .ply file}" - "{threshold t | 0.0015 | rejection threshold .001 for planes, 0.1 for spheres, 2.5 for cylinders}" - "{iters i | 10000 | ransac iterations}" - "{cloud c | 15 | or'ed synthetic model types to generate 0:none 1:planes 2:sphere 4:cylinder 8:random 16:noise distortion 32:from ply}" - "{min_inliers M | 60 | rejection inlier count}" - "{min_distance d | 6 | distance for clustering (partition)}" - "{sac_method s | 0 | SacMethodType (0:RANSAC or 1:MSAC)}" - "{preemptive p | 0 | number of hypotheses for preemptive evaluation. set to 0 to disable}" - "{napsac n | 0 | radius for napsac sampling. set to 0 to disable}" - "{max_sphere S | 50 | (sphere only) reject larger spheres}" - "{normal_weight w | 0.5 | (cylinder only) interpolate between point and normal(dot) distance. setting it to 0 will not use (or generate) normals}" - ""); - if (parser.has("help")) { + CommandLineParser parser (argc,argv, + "{help h | | print this help}" + "{model_type m | 1 | 1:plane 2:sphere 3:cylinder 4:cluster 0:all}" + "{use_sprt u | false | use sprt evaluation/termination with non-preemptive ransac}" + "{ply P | | load a .ply file}" + "{threshold t | 0.0015 | rejection threshold .001 for planes, 0.1 for spheres, 2.5 for cylinders}" + "{iters i | 10000 | ransac iterations}" + "{cloud c | 15 | or'ed synthetic model types to generate 0:none 1:planes 2:sphere 4:cylinder 8:random 16:noise distortion 32:from ply}" + "{min_inliers M | 60 | rejection inlier count}" + "{min_distance d | 6 | distance for clustering (partition)}" + "{sac_method s | 0 | SacMethodType (0:RANSAC or 1:MSAC)}" + "{preemptive p | 0 | number of hypotheses for preemptive evaluation. set to 0 to disable}" + "{napsac n | 0 | radius for napsac sampling. set to 0 to disable}" + "{max_sphere S | 50 | (sphere only) reject larger spheres}" + "{normal_weight w | 0.5 | (cylinder only) interpolate between point and normal(dot) distance. setting it to 0 will not use (or generate) normals}" + ""); + if (parser.has("help")) { parser.printMessage(); return 0; - } + } int typ = parser.get("model_type"); int iters = parser.get("iters"); int elems = parser.get("cloud"); @@ -43,75 +43,75 @@ int main(int argc,char **argv) { bool sprt = parser.get("use_sprt"); string ply = parser.get("ply"); - Mat_ pts; - if (! ply.empty()) { - pts = cv::ppf_match_3d::loadPLYSimple(ply.c_str(), false); - } - if (elems & 1) { - generatePlane(pts, vector{0.1,.1,.8, .76}, 64); - generatePlane(pts, vector{-.2,-.7,.3, .16}, 64); - } - if (elems & 2) { - generateSphere(pts, vector{12,15,-12, 5}, 128); - generateSphere(pts, vector{-12,15,-12, 5}, 128); - } - if (elems & 4) { - generateCylinder(pts, vector{-12,-15,-12, 0,0,1, 5}, 256); - generateCylinder(pts, vector{20,5,12, 0,1,0, 5}, 256); - } - if (elems & 8) { - generateRandom(pts, vector{-12,31,-5, 5}, 32); - generateRandom(pts, vector{12,-21,15, 5}, 32); - generateRandom(pts, vector{1,-2,1, 25}, 64); - } - if (elems & 16) { - Mat fuzz(pts.size(), pts.type()); - float F = 0.001; - randu(fuzz,Scalar(-F,-F,-F), Scalar(F,F,F)); - pts += fuzz; - } - cout << pts.size() << " points." << endl; - cv::ppf_match_3d::writePLY(pts, "cloud.ply"); + Mat_ pts; + if (! ply.empty()) { + pts = cv::ppf_match_3d::loadPLYSimple(ply.c_str(), false); + } + if (elems & 1) { + generatePlane(pts, vector{0.1,.1,.8, .76}, 64); + generatePlane(pts, vector{-.2,-.7,.3, .16}, 64); + } + if (elems & 2) { + generateSphere(pts, vector{12,15,-12, 5}, 128); + generateSphere(pts, vector{-12,15,-12, 5}, 128); + } + if (elems & 4) { + generateCylinder(pts, vector{-12,-15,-12, 0,0,1, 5}, 256); + generateCylinder(pts, vector{20,5,12, 0,1,0, 5}, 256); + } + if (elems & 8) { + generateRandom(pts, vector{-12,31,-5, 5}, 32); + generateRandom(pts, vector{12,-21,15, 5}, 32); + generateRandom(pts, vector{1,-2,1, 25}, 64); + } + if (elems & 16) { + Mat fuzz(pts.size(), pts.type()); + float F = 0.001f; + randu(fuzz,Scalar(-F,-F,-F), Scalar(F,F,F)); + pts += fuzz; + } + cout << pts.size() << " points." << endl; + cv::ppf_match_3d::writePLY(pts, "cloud.ply"); - auto segment = [napsac,normal_weight,max_sphere,preemptive,sprt](const Mat &cloud, std::vector &models, int model_type, float threshold, int max_iters, int min_inlier, int method_type) -> Mat { - Ptr fitting = SACModelFitting::create(cloud, model_type, method_type, threshold, max_iters); - fitting->set_normal_distance_weight(normal_weight); - fitting->set_max_napsac_radius(napsac); - fitting->set_max_sphere_radius(max_sphere); - fitting->set_preemptive_count(preemptive); - fitting->set_min_inliers(min_inlier); - fitting->set_use_sprt(sprt); + auto segment = [napsac,normal_weight,max_sphere,preemptive,sprt](const Mat &cloud, std::vector &models, int model_type, float threshold, int max_iters, int min_inlier, int method_type) -> Mat { + Ptr fitting = SACModelFitting::create(cloud, model_type, method_type, threshold, max_iters); + fitting->set_normal_distance_weight(normal_weight); + fitting->set_max_napsac_radius(napsac); + fitting->set_max_sphere_radius(max_sphere); + fitting->set_preemptive_count(preemptive); + fitting->set_min_inliers(min_inlier); + fitting->set_use_sprt(sprt); - Mat new_cloud; - fitting->segment(models, new_cloud); + Mat new_cloud; + fitting->segment(models, new_cloud); - return new_cloud; - }; + return new_cloud; + }; std::vector models; - if (typ==4) { // cluster only + if (typ==4) { // cluster only cv::ptcloud::cluster(pts, min_distance, min_inliers, models, pts); - } else + } else if (typ==0) { // end to end pts = segment(pts, models, 1, thresh, iters, min_inliers, method); cout << pts.total() << " points left." << endl; - pts = segment(pts, models, 2, 0.145, iters, min_inliers, method); - cout << pts.total() << " points left." << endl; - pts = segment(pts, models, 3, 5.0, iters, min_inliers, method); - cout << pts.total() << " points left." << endl; - cv::ptcloud::cluster(pts, 7, 20, models, pts); + pts = segment(pts, models, 2, 0.145f, iters, min_inliers, method); + cout << pts.total() << " points left." << endl; + pts = segment(pts, models, 3, 5.0f, iters, min_inliers, method); + cout << pts.total() << " points left." << endl; + cv::ptcloud::cluster(pts, 7, 20, models, pts); } else // single model type - pts = segment(pts, models, typ, thresh, iters, min_inliers, method); + pts = segment(pts, models, typ, thresh, iters, min_inliers, method); string names[] = {"", "plane","sphere","cylinder","blob"}; for (size_t i=0; i #include #include @@ -16,7 +16,6 @@ using namespace cv; using namespace std; int main() { - // Mat cloud = cv::ppf_match_3d::loadPLYSimple("./data/semi-cylinder-with-normals-usingOpenCV2.ply", true); Mat cloud = cv::ppf_match_3d::loadPLYSimple("./data/cylinder-big.ply", false); Mat ptset; diff --git a/modules/ptcloud/samples/viz/sample_plane_fitting.cpp b/modules/ptcloud/samples/viz/sample_plane_fitting.cpp index 82ef6f5acac..6405d51c082 100644 --- a/modules/ptcloud/samples/viz/sample_plane_fitting.cpp +++ b/modules/ptcloud/samples/viz/sample_plane_fitting.cpp @@ -1,3 +1,4 @@ +#include "opencv2/opencv_modules.hpp" #ifdef HAVE_OPENCV_VIZ #include #include diff --git a/modules/ptcloud/samples/viz/sample_sphere_fitting.cpp b/modules/ptcloud/samples/viz/sample_sphere_fitting.cpp index 5e8958705c5..e11c6a5f529 100644 --- a/modules/ptcloud/samples/viz/sample_sphere_fitting.cpp +++ b/modules/ptcloud/samples/viz/sample_sphere_fitting.cpp @@ -1,3 +1,4 @@ +#include "opencv2/opencv_modules.hpp" #ifdef HAVE_OPENCV_VIZ #include #include diff --git a/modules/ptcloud/src/sac_segmentation.cpp b/modules/ptcloud/src/sac_segmentation.cpp index 5245ad14c81..79c60b90218 100644 --- a/modules/ptcloud/src/sac_segmentation.cpp +++ b/modules/ptcloud/src/sac_segmentation.cpp @@ -40,7 +40,7 @@ static box bbox(const Mat &cloud) { b.m = Point3f(9999,9999,9999); b.M = Point3f(-9999,-9999,-9999); for (size_t i=0; i(i); + const Point3f &p = cloud.at(int(i)); b.M.x = std::max(b.M.x,p.x); b.M.y = std::max(b.M.y,p.y); b.M.z = std::max(b.M.z,p.z); @@ -56,7 +56,7 @@ static box bbox(const Mat &cloud) { template static void knuth_shuffle(std::vector &vec) { for (size_t t=0; t 0)) { // Calculate the point's projection on the cylinder axis - float dist = (pt.dot(axis_dir) - pt_on_axis.dot(axis_dir)); + double dist = (pt.dot(axis_dir) - pt_on_axis.dot(axis_dir)); Point3d pt_proj = pt_on_axis + dist * axis_dir; Point3d dir = pt - pt_proj; dir = dir / sqrt(dir.dot(dir)); @@ -149,7 +149,7 @@ struct CylinderModel : public Error_ { } // calculate overall distance as weighted sum of the two distances. - return fabs (weight * d_normal + (1 - weight) * distanceFromSurface); + return (float)fabs(weight * d_normal + (1 - weight) * distanceFromSurface); } }; @@ -270,8 +270,7 @@ struct SPRTImpl : public SPRT { if (tested_inliers + points_size - tested_point < highest_inlier_number) { return false; } - }// else errors[points_random_pool[random_pool_idx-1]] = (float)error; - + } return true; } @@ -492,7 +491,7 @@ SACScore getInliers(int model_type, const std::vector &coeffs, const Mat } mdl->setModelParameters(Mat(coeffs)); - double msac=0; + double msac=0; // use same calculation as SPRT double norm_thr = (threshold*9/4); double one_over_thr = (1/norm_thr); size_t num_points = indices.size(); @@ -507,7 +506,7 @@ SACScore getInliers(int model_type, const std::vector &coeffs, const Mat double distance = mdl->getError(indices[i]); if (!sprt.empty()) { - if (! sprt->addDataPoint(i,distance)) { + if (! sprt->addDataPoint(int(i),distance)) { break; } } @@ -520,11 +519,11 @@ SACScore getInliers(int model_type, const std::vector &coeffs, const Mat } if (!sprt.empty()) { - if (sprt->isModelGood(i)) + if (sprt->isModelGood(int(i))) return sprt->getScore(); } - return SACScore(inliers.size(), msac); + return SACScore((double)inliers.size(), msac); } @@ -542,7 +541,7 @@ static bool getSphereFromPoints(const Mat &cloud, const std::vector &inlier tempi[3] = 1; } double m11 = determinant(temp); - if (fequal(m11, 0)) return false; // no sphere exists + if (fequal(float(m11), 0)) return false; // no sphere exists for (int i = 0; i < 4; i++) { size_t point_idx = inliers[i]; @@ -635,7 +634,7 @@ static bool getPlaneFromPoints(const Mat &cloud, const std::vector &inliers 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 (fequal(magnitude_abc, 0)) + if (fequal(float(magnitude_abc), 0)) return false; abc /= magnitude_abc; @@ -699,7 +698,7 @@ static bool getCylinderFromPoints(const Mat &cloud, const Mat &normals_cld, cons // radius of cylinder double radius_squared = fabs(line_normal.dot(line_normal)) / line_len_sqr; - if (fequal(radius_squared, 0)) + if (fequal(float(radius_squared), 0)) return false; model_coefficients[6] = sqrt(radius_squared); @@ -791,18 +790,18 @@ struct SphereSolverCallback : LMSolver::Callback { bool compute (InputArray param, OutputArray err, OutputArray J) const CV_OVERRIDE { Mat_ in = param.getMat(); - Point3f c(in(0),in(1),in(2)); + Point3f c(float(in(0)),float(in(1)),float(in(2))); double r = in(3); // the levmarquard solver needs an error metrics for each callback - int count = P.total(); + int count = (int)P.total(); err.create(count,1,CV_64F); Mat_ e = err.getMat(); // but the jacobian matrix needs only to be (re)generated, if the error falls beyond some threshold Mat_ j; if (J.needed()) { - J.create(count, in.total(), CV_64F); + J.create(count, (int)in.total(), CV_64F); j = J.getMat(); } @@ -856,13 +855,13 @@ struct CylinderSolverCallback : LMSolver::Callback { double len = sqrt(a*a+b*b+c*c) + 0.000001; a /= len; b /= len; c /= len; - int count = P.total(); + int count = (int)P.total(); err.create(count, 1, CV_64F); Mat_ e = err.getMat(); Mat_ j; if (J.needed()) { - J.create(count, in.total(), CV_64F); + J.create(count, (int)in.total(), CV_64F); j = J.getMat(); } @@ -920,8 +919,8 @@ std::vector optimizeModel(int model_type, const Mat &cloud, const std::v return optimizeSphere(cloud, inliers_indices, old_coeff); case CYLINDER_MODEL: return optimizeCylinder(cloud, inliers_indices, old_coeff); - default: - CV_Error(215, format("invalid model_type %d", model_type).c_str()); + // default: + // CV_Error(215, format("invalid model_type %d", model_type).c_str()); } return std::vector(); } @@ -947,11 +946,11 @@ static std::vector sampleHypothesis(RNG &rng, const Mat &cloud, const std:: // NAPSAC like sampling strategy, // assume good hypothesis inliers are locally close to each other // (enforce spatial proximity) - int start = rng.uniform(0, indices.size()); + int start = rng.uniform(0, (int)indices.size()); current_model_inliers.emplace_back(start); Point3f a = cloud.at(indices[start]); for (int n=0; n<100; n++) { - int next = rng.uniform(0, indices.size()); + int next = rng.uniform(0, (int)indices.size()); Point3f b = cloud.at(indices[next]); if (norm(a-b) > max_napsac_radius) continue; @@ -963,7 +962,7 @@ static std::vector sampleHypothesis(RNG &rng, const Mat &cloud, const std:: } else { // uniform sample from indices for (size_t j = 0; j < num_rnd_model_points; j++) - current_model_inliers.emplace_back(rng.uniform(0, indices.size())); + current_model_inliers.emplace_back(rng.uniform(0, (int)indices.size())); } return current_model_inliers; @@ -980,7 +979,6 @@ bool generateHypothesis(int model_type, const Mat &cloud, const std::vector if (coefficients[3] > max_radius) return false; return true; } - if (model_type == CYLINDER_MODEL) { if ((normals.total() > 0) && (normal_distance_weight_ > 0)) return getCylinderFromPoints(cloud, normals, current_model_inliers, coefficients); @@ -988,7 +986,6 @@ bool generateHypothesis(int model_type, const Mat &cloud, const std::vector return getCylinderFromPoints(cloud, current_model_inliers, coefficients); } - CV_Error(215, format("unsupported model type %d", model_type).c_str()); return false; } @@ -1005,7 +1002,7 @@ Mat generateNormals(const Mat &cloud) { Mat normals; Mat(_pointsAndNormals.colRange(3,6)).copyTo(normals); - normals = normals.reshape(3, cloud.total()); + normals = normals.reshape(3, (int)cloud.total()); return normals.t(); } @@ -1069,12 +1066,12 @@ bool SACModelFittingImpl::fit_ransac(const std::vector &indices, std::vecto size_t num_rnd_model_points = rnd_model_points(model_type, normals); CV_Assert(indices.size() >= num_rnd_model_points); - int stop = stopping(min_inliers, indices.size(), num_rnd_model_points); + int stop = (int)stopping(min_inliers, indices.size(), num_rnd_model_points); if (use_sprt) { - sprt = SPRT::create(9999, indices.size(), + sprt = SPRT::create(9999, (int)indices.size(), threshold, 0.01, 0.008, - 200, 3, num_rnd_model_points, max_iters, (ScoreMethod)method_type); + 200, 3, (int)num_rnd_model_points, max_iters, (ScoreMethod)method_type); } for (int i = 0; i < std::min(stop, max_iters); ++i) { @@ -1122,9 +1119,9 @@ bool SACModelFittingImpl::fit_ransac(const std::vector &indices, std::vecto } if (!sprt.empty()) - stop = i + sprt->update(curModel.score.first); + stop = i + (int)sprt->update((int)curModel.score.first); else - stop = i + stopping(curModel.score.first, indices.size(), num_rnd_model_points); + stop = i + (int)stopping((size_t)curModel.score.first, indices.size(), num_rnd_model_points); bestModel = curModel; } @@ -1140,7 +1137,7 @@ bool SACModelFittingImpl::fit_ransac(const std::vector &indices, std::vecto // how many models to retain after each data block // RaguramECCV08.pdf (5) inline -double preemptive_func(int i, int M, int B) { +double preemptive_func(size_t i, size_t M, size_t B) { return M * std::pow(2.0, -(double(i)/B)); } @@ -1199,7 +1196,7 @@ bool SACModelFittingImpl::fit_preemptive(const std::vector &indices, std::v } // prune models - int retain = preemptive_func(preemptive_pos, preemptive_count, preemptive_step); + int retain = (int)preemptive_func(preemptive_pos, (size_t)preemptive_count, preemptive_step); preemptive.erase(preemptive.begin() + retain + 1, preemptive.end()); } @@ -1223,6 +1220,8 @@ bool SACModelFittingImpl::fit_preemptive(const std::vector &indices, std::v void SACModelFittingImpl::segment(std::vector &model_instances, OutputArray new_cloud) { + sprt.release(); // make sure it's only used in fit_ransac() and only if use_sprt is on. + size_t num_points = cloud.total(); // optionally generate normals for the Cylinder model @@ -1240,7 +1239,7 @@ void SACModelFittingImpl::segment(std::vector &model_instances, Output // filter unused point indices std::vector indices; for (size_t i = 0; i < num_points; i++) { - if (point_labels[i] == 0) indices.push_back(i); + if (point_labels[i] == 0) indices.push_back(int(i)); } if (indices.empty()) break; @@ -1255,7 +1254,7 @@ void SACModelFittingImpl::segment(std::vector &model_instances, Output break; SACModel &latest = model_instances.back(); - num_segmented_points += latest.indices.size(); + num_segmented_points += (long)latest.indices.size(); if (num_segmented_points == 0) break; @@ -1275,7 +1274,7 @@ void SACModelFittingImpl::segment(std::vector &model_instances, Output Mat _cloud; for (size_t i=0; i(i)); + if (p == 0) _cloud.push_back(cloud.at(int(i))); } new_cloud.assign(_cloud); } @@ -1377,7 +1376,7 @@ void cluster(InputArray _cloud, double distance, int min_inliers, std::vector mdl(n); for (size_t i=0; i &coeffs, int N) { - int n=sqrt(N); + int n=(int)sqrt(N); for (int i=0; i &coeffs, int N) { for (int i=0; i &coeffs, int N) { - float R=20; - Point3f cen(coeffs[0],coeffs[1],coeffs[2]); - Point3f dir(coeffs[3],coeffs[4],coeffs[5]); + double R=20; + Point3d cen(coeffs[0],coeffs[1],coeffs[2]); + Point3d dir(coeffs[3],coeffs[4],coeffs[5]); for (int i=0; i &coeffs, int N) { - float R = coeffs[3]; + float R = float(coeffs[3]); for (int i=0; i coefs; std::vector indices; Mat_ cloud; - size_t NPOINTS = 128; + int NPOINTS = 128; double THRESHOLD = 0.001; void generateIndices() { @@ -43,7 +43,7 @@ struct CylinderModel : public BaseModel { Point3f dir; CylinderModel() { coefs = std::vector{-10,-20,3, 0,0,1, 5.0}; - dir = Point3f(coefs[3],coefs[4],coefs[5]); + dir = Point3f(float(coefs[3]),float(coefs[4]),float(coefs[5])); generateCylinder(cloud, coefs, NPOINTS); generateIndices(); } @@ -64,28 +64,33 @@ TEST(SAC, plane) vector new_coefs; std::vector hypothesis_inliers {1,8,11}; bool valid_model = generateHypothesis(PLANE_MODEL, plane.cloud, hypothesis_inliers, Mat(), 50, 0, new_coefs); - double d = cv::norm(Mat(plane.coefs),Mat(new_coefs)); EXPECT_TRUE(valid_model); - EXPECT_LE(d, 0.05); + Point3d dir1(new_coefs[0],new_coefs[1],new_coefs[2]); + Point3d dir2(plane.coefs[0],plane.coefs[1],plane.coefs[2]); + double d2 = dir1.dot(dir2); // check if they are parallel + EXPECT_GE(abs(d2), 0.95); + EXPECT_LE(abs(abs(plane.coefs[3]) - abs(new_coefs[3])), 0.1); // length // 2. all plane points should be inliers std::vector new_indices; SACScore score = getInliers(PLANE_MODEL, plane.coefs, plane.cloud, Mat(), plane.indices, plane.THRESHOLD, 0, new_indices, 0); - EXPECT_EQ(score.first, plane.NPOINTS); - EXPECT_LE(score.second, plane.THRESHOLD * plane.NPOINTS); + EXPECT_EQ((int)score.first, plane.NPOINTS); + EXPECT_LE(score.second, double(plane.THRESHOLD * plane.NPOINTS)); // 3. no sphere points should be inliers score = getInliers(PLANE_MODEL, plane.coefs, sphere.cloud, Mat(), sphere.indices, plane.THRESHOLD, 0, new_indices, 0); - EXPECT_EQ(score.first, 0); + EXPECT_EQ(score.first, 0.0); // 4. no cylinder points should be inliers score = getInliers(PLANE_MODEL, plane.coefs, cylinder.cloud, Mat(), cylinder.indices, plane.THRESHOLD, 0, new_indices, 0); - EXPECT_EQ(score.first, 0); + EXPECT_EQ(score.first, 0.0); // 5. check optimization new_coefs = optimizeModel(PLANE_MODEL, plane.cloud, plane.indices, plane.coefs); - double d1 = cv::norm(Mat(plane.coefs),Mat(new_coefs)); - EXPECT_LE(d1, 0.05); + Point3d dir3(new_coefs[0],new_coefs[1],new_coefs[2]); + double d3 = dir3.dot(dir2); // check if they are parallel + EXPECT_GE(abs(d3), 0.95); + EXPECT_LE(abs(abs(plane.coefs[3]) - abs(new_coefs[3])), 0.1); // length } TEST(SAC, sphere) @@ -101,16 +106,16 @@ TEST(SAC, sphere) // 2. all sphere points should be inliers std::vector new_indices; SACScore score = getInliers(SPHERE_MODEL, sphere.coefs, sphere.cloud, Mat(), sphere.indices, sphere.THRESHOLD, 0, new_indices, 0); - EXPECT_EQ(score.first, sphere.NPOINTS); - EXPECT_LE(score.second, sphere.THRESHOLD * sphere.NPOINTS); + EXPECT_EQ((int)score.first, sphere.NPOINTS); + EXPECT_LE(score.second, double(sphere.THRESHOLD * sphere.NPOINTS)); // 3. no plane points should be inliers score = getInliers(SPHERE_MODEL, sphere.coefs, plane.cloud, Mat(), plane.indices, sphere.THRESHOLD, 0, new_indices, 0); - EXPECT_EQ(score.first, 0); + EXPECT_EQ(score.first, 0.0); // 4. no cylinder points should be inliers score = getInliers(SPHERE_MODEL, sphere.coefs, cylinder.cloud, Mat(), cylinder.indices, sphere.THRESHOLD, 0, new_indices, 0); - EXPECT_EQ(score.first, 0); + EXPECT_EQ(score.first, 0.0); // 5. check optimization new_coefs = optimizeModel(SPHERE_MODEL, sphere.cloud, sphere.indices, sphere.coefs); @@ -125,18 +130,18 @@ TEST(SAC, cylinder) vector new_coefs; std::vector hypothesis_inliers {1,22}; bool valid_model = generateHypothesis(CYLINDER_MODEL, cylinder.cloud, hypothesis_inliers, normals, 50, 0.5, new_coefs); + EXPECT_TRUE(valid_model); // any point on the axis is actually valid, so we can only check the dir and the radius - Point3f dir(new_coefs[3],new_coefs[4],new_coefs[5]); + Point3d dir(new_coefs[3],new_coefs[4],new_coefs[5]); double d = cylinder.dir.dot(dir); // check if they are parallel - EXPECT_TRUE(valid_model); EXPECT_GE(abs(d), 0.95); - EXPECT_LE(abs(cylinder.coefs[6] - new_coefs[6]), 0.6); // radius + EXPECT_LE(abs(abs(cylinder.coefs[6]) - abs(new_coefs[6])), 2.0); // radius // 2. all cylinder points should be inliers std::vector new_indices; SACScore score = getInliers(CYLINDER_MODEL, cylinder.coefs, cylinder.cloud, Mat(), cylinder.indices, cylinder.THRESHOLD, 0, new_indices, 0); - EXPECT_EQ(score.first, cylinder.NPOINTS); - EXPECT_LE(score.second, cylinder.THRESHOLD * cylinder.NPOINTS); + EXPECT_EQ((int)score.first, cylinder.NPOINTS); + EXPECT_LE(score.second, double(cylinder.THRESHOLD * cylinder.NPOINTS)); // 3. no plane points should be inliers score = getInliers(CYLINDER_MODEL, cylinder.coefs, plane.cloud, Mat(), plane.indices, cylinder.THRESHOLD, 0, new_indices, 0); @@ -144,12 +149,14 @@ TEST(SAC, cylinder) // 4. no sphere points should be inliers score = getInliers(CYLINDER_MODEL, cylinder.coefs, sphere.cloud, Mat(), sphere.indices, cylinder.THRESHOLD, 0, new_indices, 0); - EXPECT_EQ(score.first, 0); + EXPECT_EQ(score.first, 0.0); // 5. check optimization new_coefs = optimizeModel(CYLINDER_MODEL, cylinder.cloud, cylinder.indices, cylinder.coefs); - double d1 = cv::norm(Mat(cylinder.coefs),Mat(new_coefs)); - EXPECT_LE(d1, 0.05); + Point3d dir2(new_coefs[3],new_coefs[4],new_coefs[5]); + double d2 = cylinder.dir.dot(dir2); // check if they are parallel + EXPECT_GE(abs(d2), 0.95); + EXPECT_LE(abs(abs(cylinder.coefs[6]) - abs(new_coefs[6])), 2.0); // radius } @@ -167,7 +174,7 @@ Mat generateScene() generateRandom(pts, vector{-12,31,-5, 5}, 32); // these 2 form distinct blobs generateRandom(pts, vector{12,-21,15, 5}, 32); // that can be retrieved via clustering generateRandom(pts, vector{1,-2,1, 25}, 64); // general noisy outliers - return pts; + return Mat(pts); } TEST(SAC, segment_ransac) @@ -178,7 +185,7 @@ TEST(SAC, segment_ransac) fitting->set_threshold(plane.THRESHOLD); fitting->segment(models); EXPECT_EQ(models.size(), size_t(1)); - EXPECT_EQ(models[0].indices.size(), plane.NPOINTS); + EXPECT_EQ(models[0].indices.size(), (size_t)plane.NPOINTS); double d1 = cv::norm(Mat(plane.coefs), Mat(models[0].coefficients)); EXPECT_LE(d1, 0.05); @@ -186,18 +193,18 @@ TEST(SAC, segment_ransac) fitting->set_threshold(sphere.THRESHOLD); fitting->segment(models); EXPECT_EQ(models.size(), size_t(2)); - EXPECT_EQ(models[1].indices.size(), sphere.NPOINTS); + EXPECT_EQ(models[1].indices.size(), (size_t)sphere.NPOINTS); double d2 = cv::norm(Mat(sphere.coefs), Mat(models[1].coefficients)); EXPECT_LE(d2, 0.05); fitting->set_model_type(CYLINDER_MODEL); fitting->set_threshold(2.62); fitting->set_normal_distance_weight(0.5); - fitting->set_min_inliers(cylinder.NPOINTS/2); + fitting->set_min_inliers(int(cylinder.NPOINTS/2)); fitting->segment(models); EXPECT_EQ(models.size(), size_t(3)); - EXPECT_GE(models[2].indices.size(), cylinder.NPOINTS*0.6); - Point3f dir(models[2].coefficients[3],models[2].coefficients[4],models[2].coefficients[5]); + EXPECT_GE(models[2].indices.size(), size_t(cylinder.NPOINTS*0.5)); + Point3d dir(models[2].coefficients[3],models[2].coefficients[4],models[2].coefficients[5]); double d3 = cylinder.dir.dot(dir); // check if they are parallel EXPECT_GE(abs(d3), 0.65); EXPECT_LE(abs(cylinder.coefs[6] - models[2].coefficients[6]), 0.6); // radius @@ -212,27 +219,27 @@ TEST(SAC, segment_preemptive) fitting->set_preemptive_count(500); fitting->segment(models); EXPECT_EQ(models.size(), size_t(1)); - EXPECT_EQ(models[0].indices.size(), plane.NPOINTS); + EXPECT_EQ(models[0].indices.size(), (size_t)plane.NPOINTS); fitting->set_model_type(SPHERE_MODEL); fitting->set_threshold(sphere.THRESHOLD); fitting->segment(models); EXPECT_EQ(models.size(), size_t(2)); - EXPECT_EQ(models[1].indices.size(), sphere.NPOINTS); + EXPECT_EQ(models[1].indices.size(), (size_t)sphere.NPOINTS); double d2 = cv::norm(Mat(sphere.coefs), Mat(models[1].coefficients)); EXPECT_LE(d2, 0.05); fitting->set_model_type(CYLINDER_MODEL); fitting->set_threshold(3.62); fitting->set_normal_distance_weight(0.5); - fitting->set_min_inliers(85); + fitting->set_min_inliers(90); fitting->segment(models); EXPECT_EQ(models.size(), size_t(3)); - EXPECT_GE(models[2].indices.size(), cylinder.NPOINTS*0.7); - Point3f dir(models[2].coefficients[3],models[2].coefficients[4],models[2].coefficients[5]); + EXPECT_GE(models[2].indices.size(), size_t(cylinder.NPOINTS*0.7)); + Point3d dir(models[2].coefficients[3],models[2].coefficients[4],models[2].coefficients[5]); double d3 = cylinder.dir.dot(dir); // check if they are parallel EXPECT_GE(abs(d3), 0.65); - EXPECT_LE(abs(cylinder.coefs[6] - models[2].coefficients[6]), 0.6); // radius + EXPECT_LE(abs(abs(cylinder.coefs[6]) - abs(models[2].coefficients[6])), 0.6); // radius } TEST(SAC, cluster)