From 8d59f7cd9ef04d1a4f24d1fc206c794b3bfd63a4 Mon Sep 17 00:00:00 2001 From: Devansh Batra Date: Tue, 30 Jun 2020 18:59:08 +0530 Subject: [PATCH 1/8] 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 2/8] 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 3/8] 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 4/8] 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 5/8] 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 6/8] 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 7/8] 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 8/8] 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; }