From 325ce02cde39c998aa3cc8f4a0095f25a109577d Mon Sep 17 00:00:00 2001 From: Devansh Batra Date: Tue, 30 Jun 2020 22:10:40 +0530 Subject: [PATCH] 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 20c04b997..c43569d38 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 000000000..5bc711f63 --- /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 000000000..53cc31235 --- /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 aef759b61..6f54bfe50 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