diff --git a/modules/ptcloud/CMakeLists.txt b/modules/ptcloud/CMakeLists.txt new file mode 100644 index 00000000000..b6ef587325a --- /dev/null +++ b/modules/ptcloud/CMakeLists.txt @@ -0,0 +1,2 @@ +set(the_description "Point cloud functionality.") +ocv_define_module(ptcloud opencv_core opencv_calib3d opencv_surface_matching OPTIONAL opencv_viz WRAP python) diff --git a/modules/ptcloud/README.md b/modules/ptcloud/README.md new file mode 100644 index 00000000000..3b8fb47d2a9 --- /dev/null +++ b/modules/ptcloud/README.md @@ -0,0 +1,35 @@ +//! @addtogroup ptcloud +//! @{ + +Point Cloud Module, Object Fitting API +======================================= + +Try to segment geometric primitives like planes, spheres and cylinders from a 3d point cloud + +2 alternative ransac strategies are implemented here: + +- plain ransac loop: + + generate random minimal hypothesis + + find inliers for the model, + - bail out if at 1/4 of the data it does not have more than 1/8 inliers of the current best model + - if sprt is used, bail out if the probability of finding inliers goes low + + if this model is the current best one + - best model = current + - update stopping criterion (optional SPRT) + - apply local optimization (generate a non-minimal model from the inliers) + + if it improved, take that instead + +- preemptive ransac loop: + + generate M minimal random models in advance + + slice the data into blocks(of size B), for each block: + - evaluate all M models in parallel + - sort descending (by inliers or accumulated distance) + - prune model list, M' = M * (2 ^ -(i/B)) + - stop if there is only one model left, or the last data block reached + + polish/optimize the last remaining model + +To Do +----------------------------------------- +- Integrate (better) 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..7112d48f924 --- /dev/null +++ b/modules/ptcloud/doc/ptcloud.bib @@ -0,0 +1,33 @@ +@article{FB81 + title={Random sample consensus: A paradigm for model fitting with applications to image analysis and automated cartography}, + author={Fischler, M. A. and Bolles, R.}, + journal={Communications of the ACM}, + year={1981}, + volume={24}, + number={2}, + pages={381-395} +} + +@inproceedings{NI03 + title={Preemptive RANSAC for live structure and motion estimation}, + author={Nister, D.}, + booktitle={International Conference on Computer Vision (ICCV)}, + year={2003} +} + +@article{USAC13 + title={USAC: A Universal Framework for Random Sample Consensus}, + author={Raguram,Rahul and Chum,Ondrej and Pollefeys,Marc and Matas,Jiri and Frahm,Jan-Michael}, + journal={IEEE Transactions on Software Engineering}, + year={2013}, + organization={IEEE} +} + +@article{SPRT05 + title={Randomized RANSAC with Sequential Probability Ratio Test}, + author={Matas,Jirı and Chum,Ondrej}, + booktitle={International Conference on Computer Vision (ICCV)}, + year={2005}, + volume={2}, + pages={1727–1732} +} \ No newline at end of file diff --git a/modules/ptcloud/include/opencv2/ptcloud.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..bc1f88b2bb1 --- /dev/null +++ b/modules/ptcloud/include/opencv2/ptcloud/sac_segmentation.hpp @@ -0,0 +1,192 @@ +// 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 + + +namespace cv +{ +namespace ptcloud +{ +//! @addtogroup ptcloud +//! @{ + +enum SacModelType { + PLANE_MODEL = 1, + SPHERE_MODEL = 2, + CYLINDER_MODEL = 3, + BLOB_MODEL = 4 +}; + +// +//! follows ScoreMethod from calib3d +// +enum SacMethodType { + SAC_METHOD_RANSAC = 0, //!< maximize inlier count + SAC_METHOD_MSAC = 1 //!< minimize inlier distance +}; + +typedef std::pair SACScore; + +/** @brief structure to hold the segmentation results +*/ +class CV_EXPORTS_W SACModel { +public: + CV_PROP std::vector coefficients; //!< the "model" + CV_PROP std::vector indices; //!< into the original cloud + CV_PROP std::vector points; //!< (output) copies (original cloud might no more exist) + CV_PROP int type; //!< see SacModelType + CV_PROP SACScore score; //!< first:inlier_count(ransac), second:accumulated distance(msac) + + CV_WRAP SACModel(int typ=PLANE_MODEL); +}; + + +/** @brief The SACModelFitting class to segment geometric primitives like planes,spheres,cylinders from 3d point clouds. + +2 alternative ransac strategies are implemented here: + +- ransac loop: + + generate random minimal hypothesis + + find inliers for the model, + - bail out if at 1/4 of the data it does not have more than 1/8 inliers of the current best model + - if sprt is enabled, bail out if the accumulated probability of a bad model crosses a threshold + + if this model is the current best one + - best model = current + - update stopping criterion (using sprt if enabled) + - apply local optimization (generate a non-minimal model from the inliers) + + if it improved, take that instead + +- preemptive ransac loop: + + generate M minimal random models in advance + + slice the data into blocks(of size B), for each block: + - evaluate all M models in parallel + - sort descending (by inliers or accumulated distance) + - prune model list, M' = M * (2 ^ -(i/B)) + - stop if there is only one model left, or the last data block reached + + polish/optimize the last remaining model +*/ +class CV_EXPORTS_W SACModelFitting { +public: + virtual ~SACModelFitting() {} + + /** @brief set a new point cloud to segment + + @param cloud either a 3channel or 1 channel / 3cols or rows Mat + @param with_normals if enabled, the cloud should have either 6 rows or 6 cols, and a single channel + */ + CV_WRAP virtual void set_cloud(InputArray cloud, bool with_normals=false) = 0; + + /** @brief Set the type of model to be fitted + + @param model_type see SacModelType enum. + */ + CV_WRAP virtual void set_model_type(int model_type) = 0; + + /** @brief Set the type of ransac method + + @param method_type see SacMethodType enum. + */ + CV_WRAP virtual void set_method_type(int method_type) = 0; + + /** @brief Use Wald's Sequential Probability Ratio Test with ransac fitting + + This will result in less iterations and less evaluated data points, and thus be + much faster, but it might miss some inliers + (not used in the preemptive ransac) + @param sprt true or false. + */ + CV_WRAP virtual void set_use_sprt(bool sprt) = 0; + + /** @brief Set the threshold for the fitting. + The threshold is usually the distance from the boundary of model, but may vary from model to model. + + This may be helpful when multiple fitting operations are to be performed. + @param threshold the threshold to set. + */ + CV_WRAP virtual void set_threshold(double threshold) = 0; + + /** @brief Set the number of iterations for the (non preemptive) ransac fitting. + + This may be helpful when multiple fitting operations are to be performed. + @param iterations the threshold to set. + */ + CV_WRAP virtual void set_iterations(int iterations) = 0; + + /** @brief Set the weight given to normal alignment before comparing overall error with threshold. + + By default it is set to 0. + @param weight the desired normal alignment weight (between 0 to 1). + */ + CV_WRAP virtual void set_normal_distance_weight(double weight) = 0; + + /** @brief Set the maximal radius for the sphere hypothesis generation. + + A radius larger than this is considered degenerate. + @param max_radius the maximum valid sphere radius. + */ + CV_WRAP virtual void set_max_sphere_radius(double max_radius) = 0; + + /** @brief Set the maximal radius for the (optional) napsac hypothesis sampling. + + Assume good hypothesis inliers are locally close to each other + (enforce spatial proximity for hypothesis inliers). + + @param max_radius the maximum valid sphere radius for napsac sampling. set it to 0 to disable this strategy, and sample uniformly instead. + */ + CV_WRAP virtual void set_max_napsac_radius(double max_radius) = 0; + + /** + @param preemptive the number of models generated with preemptive ransac. + set to 0 to disable preemptive hypothesis generation and do plain ransac instead + */ + CV_WRAP virtual void set_preemptive_count(int preemptive) = 0; + + /** + @param min_inliers reject a model if it has less inliers than this. + */ + CV_WRAP virtual void set_min_inliers(int min_inliers) = 0; + + /** @brief Segment multiple models of the same type, this function would get the best fitting models on the given set of points. + + This stores the models in the model_instances, and optionally, the remaining cloud points. + + @param model_instances a vector of SACModels. + @param new_cloud (optional) the remaining non-segmented cloud points. + */ + CV_WRAP virtual void segment(CV_OUT std::vector &model_instances, OutputArray new_cloud=noArray()) = 0; + + /** @brief Initializes a SACModelFitting instance. + + @param cloud input Point Cloud. + @param model_type type of model fitting to attempt - values can be either PLANE_MODEL, SPHERE_MODEL, or CYLINDER_MODEL. + @param method_type which method to use - (use value SAC_METHOD_RANSAC or SAC_METHOD_MSAC). + @param threshold set the threshold while choosing inliers. + @param max_iters number of iterations for Sampling. + */ + CV_WRAP static Ptr create(InputArray cloud, int model_type = PLANE_MODEL, int method_type = SAC_METHOD_RANSAC, double threshold = 20, int max_iters = 1000); +}; + + +/** @brief Cluster (remaining) points into blobs + + This is using cv::partition() internally to seperate blobs. + + @param cloud the input point cloud + @param distance max distance to the next inlier point in the blob + @param min_inliers reject blobs with less inliers than this + @param models a vector of SACModels to hold the resulting blobs + @param new_cloud optionally return non segmented points +*/ +CV_EXPORTS_W void cluster(InputArray cloud, double distance, int min_inliers, CV_OUT std::vector &models, OutputArray new_cloud=noArray()); + +} // ptcloud +} // cv + + +#endif \ No newline at end of file diff --git a/modules/ptcloud/include/opencv2/ptcloud/sac_utils.hpp b/modules/ptcloud/include/opencv2/ptcloud/sac_utils.hpp new file mode 100644 index 00000000000..9cca09ccabc --- /dev/null +++ b/modules/ptcloud/include/opencv2/ptcloud/sac_utils.hpp @@ -0,0 +1,38 @@ +#ifndef OPENCV_SAC_IMPL_HPP +#define OPENCV_SAC_IMPL_HPP + +#include "opencv2/calib3d.hpp" +#include "opencv2/ptcloud/sac_segmentation.hpp" + +namespace cv { +namespace ptcloud { + +// testing & samples +CV_EXPORTS void generatePlane(Mat &cloud, const std::vector &coeffs, int N=256); +CV_EXPORTS void generateSphere(Mat &cloud, const std::vector &coeffs, int N=256); +CV_EXPORTS void generateCylinder(Mat &cloud, const std::vector &coeffs, int N=256); +CV_EXPORTS void generateRandom(Mat &cloud, const std::vector &coeffs, int N=256); + + +// for testing +struct CV_EXPORTS SPRT { + virtual ~SPRT() {} + virtual bool addDataPoint(int tested_point, double error) = 0; + virtual bool isModelGood(int tested_point) = 0; + virtual int update (int inlier_size) = 0; + virtual SACScore getScore() = 0; + + static Ptr create(int state, int points_size_, + double inlier_threshold_, double prob_pt_of_good_model, double prob_pt_of_bad_model, + double time_sample, double avg_num_models, int sample_size_, int max_iterations_, ScoreMethod score_type_); +}; + +CV_EXPORTS Mat generateNormals(const Mat &cloud); +CV_EXPORTS std::vector optimizeModel(int model_type, const Mat &cloud, const std::vector &inliers_indices, const std::vector &old_coeff); +CV_EXPORTS SACScore getInliers(int model_type, const std::vector &coeffs, const Mat &cloud, const Mat &normals, const std::vector &indices, const double threshold, const double best_inliers, std::vector& inliers, double normal_distance_weight_, Ptr sprt=Ptr()); +CV_EXPORTS bool generateHypothesis(int model_type, const Mat &cloud, const std::vector ¤t_model_inliers, const Mat &normals, double max_radius, double normal_distance_weight_, std::vector &coefficients); + +} // ptcloud +} // cv + +#endif // OPENCV_SAC_IMPL_HPP diff --git a/modules/ptcloud/misc/python/pyopencv_ptcloud.hpp b/modules/ptcloud/misc/python/pyopencv_ptcloud.hpp new file mode 100644 index 00000000000..7ba1e0ac828 --- /dev/null +++ b/modules/ptcloud/misc/python/pyopencv_ptcloud.hpp @@ -0,0 +1,34 @@ +#include "opencv2/ptcloud/sac_segmentation.hpp" + +// SACScore +template<> +PyObject* pyopencv_from(const std::pair& src) +{ + return Py_BuildValue("(dd)", src.first, src.second); +} + +// we have a vector of structs in c++, but python needs a list of Ptrs +// (in the end, this makes dreadful copies of anything) +template<> +PyObject* pyopencv_from(const std::vector& src) +{ + int i, n = (int)src.size(); + PyObject* seq = PyList_New(n); + for( i = 0; i < n; i++ ) + { + Ptr ptr(new ptcloud::SACModel()); + *ptr = src[i]; + PyObject* item = pyopencv_from(ptr); + if(!item) + break; + PyList_SetItem(seq, i, item); + } + if( i < n ) + { + Py_DECREF(seq); + return 0; + } + return seq; +} + +typedef std::vector vector_SACModel; diff --git a/modules/ptcloud/misc/python/test/test_sac.py b/modules/ptcloud/misc/python/test/test_sac.py new file mode 100644 index 00000000000..f2cbc2de8e4 --- /dev/null +++ b/modules/ptcloud/misc/python/test/test_sac.py @@ -0,0 +1,35 @@ +#!/usr/bin/env python + +''' +sac segmentation +''' + +# Python 2/3 compatibility +from __future__ import print_function + +import numpy as np +import cv2 as cv + +from tests_common import NewOpenCVTests + +class sac_test(NewOpenCVTests): + + def test_plane(self): + N = 64; + plane = np.zeros((N,N,3),np.float32) + for i in range(0,N): + for j in range(0,N): + plane[i,j] = (i,j,0) + + fit = cv.ptcloud.SACModelFitting_create(plane) + mdl,left = fit.segment() + + self.assertEqual(len(mdl), 1) + self.assertEqual(len(mdl[0].indices), N*N) + self.assertEqual(len(mdl[0].points), N*N) + self.assertEqual(len(mdl[0].coefficients), 4) + self.assertEqual(np.shape(left), ()) + + +if __name__ == '__main__': + NewOpenCVTests.bootstrap() diff --git a/modules/ptcloud/perf/perf_main.cpp b/modules/ptcloud/perf/perf_main.cpp new file mode 100644 index 00000000000..7852d0939ec --- /dev/null +++ b/modules/ptcloud/perf/perf_main.cpp @@ -0,0 +1,7 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html. + +#include "perf_precomp.hpp" + +CV_PERF_TEST_MAIN(ptcloud) diff --git a/modules/ptcloud/perf/perf_precomp.hpp b/modules/ptcloud/perf/perf_precomp.hpp new file mode 100644 index 00000000000..9a6244b619b --- /dev/null +++ b/modules/ptcloud/perf/perf_precomp.hpp @@ -0,0 +1,15 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html. +#ifndef __OPENCV_TEST_PRECOMP_HPP__ +#define __OPENCV_TEST_PRECOMP_HPP__ + +#include "opencv2/opencv_modules.hpp" +#include "opencv2/ts.hpp" +#include "opencv2/ts/ts_perf.hpp" +#include "opencv2/ptcloud/sac_segmentation.hpp" +#include "opencv2/ptcloud/sac_utils.hpp" + +using namespace perf; + +#endif diff --git a/modules/ptcloud/perf/perf_ptcloud_sac.cpp b/modules/ptcloud/perf/perf_ptcloud_sac.cpp new file mode 100644 index 00000000000..0250b0dd8e5 --- /dev/null +++ b/modules/ptcloud/perf/perf_ptcloud_sac.cpp @@ -0,0 +1,52 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html. + +#include "perf_precomp.hpp" + +using namespace std; +using namespace cv; +using namespace perf; + +namespace opencv_test { namespace { + +// sprt(-1)/ransac(0)/preemptive(500) mode, inlier size, outlier size +typedef perf::TestBaseWithParam< tuple > ptcloud_sac; +#define PREC testing::Values(-1,0,500) +#define MODL testing::Values(64,1024) +#define RAND testing::Values(64,1024) + +// +// if the inlier ratio is high, plain ransac is still faster than the preemptive version +// (i guess, there is a high bailout rate then) +// +PERF_TEST_P(ptcloud_sac, segment, testing::Combine(PREC, MODL, RAND)) +{ + int pc = get<0>(GetParam()); + int s1 = get<1>(GetParam()); + int s2 = get<2>(GetParam()); + + Mat cloud; + ptcloud::generatePlane(cloud, std::vector{0.98, 0.13, 0.13, -10.829}, s1); + ptcloud::generateRandom(cloud, std::vector{0,0,0,100}, s2); + + Ptr fit = ptcloud::SACModelFitting::create(cloud); + fit->set_threshold(0.0015); + if (pc == -1) { // test sprt + fit->set_use_sprt(true); + fit->set_preemptive_count(0); + } else { + fit->set_preemptive_count(pc); + } + + std::vector mdl; + + size_t count = 0; // each cycle should segment exactly 1 model + TEST_CYCLE() { fit->segment(mdl); count++; } + + ASSERT_TRUE(mdl.size() == count); + + SANITY_CHECK_NOTHING(); +} + +}} \ No newline at end of file diff --git a/modules/ptcloud/samples/sac_demo.cpp b/modules/ptcloud/samples/sac_demo.cpp new file mode 100644 index 00000000000..c3196443b84 --- /dev/null +++ b/modules/ptcloud/samples/sac_demo.cpp @@ -0,0 +1,117 @@ +#include "opencv2/ptcloud/sac_segmentation.hpp" +#include "opencv2/ptcloud/sac_utils.hpp" +#include "opencv2/surface_matching/ppf_helpers.hpp" + +#include +using namespace std; +using namespace cv; +using namespace cv::ptcloud; + + +int main(int argc,char **argv) { + CommandLineParser parser (argc,argv, + "{help h | | print this help}" + "{model_type m | 1 | 1:plane 2:sphere 3:cylinder 4:cluster 0:all}" + "{use_sprt u | false | use sprt evaluation/termination with non-preemptive ransac}" + "{ply P | | load a .ply file}" + "{threshold t | 0.0015 | rejection threshold .001 for planes, 0.1 for spheres, 2.5 for cylinders}" + "{iters i | 10000 | ransac iterations}" + "{cloud c | 15 | or'ed synthetic model types to generate 0:none 1:planes 2:sphere 4:cylinder 8:random 16:noise distortion 32:from ply}" + "{min_inliers M | 60 | rejection inlier count}" + "{min_distance d | 6 | distance for clustering (partition)}" + "{sac_method s | 0 | SacMethodType (0:RANSAC or 1:MSAC)}" + "{preemptive p | 0 | number of hypotheses for preemptive evaluation. set to 0 to disable}" + "{napsac n | 0 | radius for napsac sampling. set to 0 to disable}" + "{max_sphere S | 50 | (sphere only) reject larger spheres}" + "{normal_weight w | 0.5 | (cylinder only) interpolate between point and normal(dot) distance. setting it to 0 will not use (or generate) normals}" + ""); + if (parser.has("help")) { + parser.printMessage(); + return 0; + } + int typ = parser.get("model_type"); + int iters = parser.get("iters"); + int elems = parser.get("cloud"); + int method = parser.get("sac_method"); + int napsac = parser.get("napsac"); + int min_inliers = parser.get("min_inliers"); + int min_distance = parser.get("min_distance"); + int preemptive = parser.get("preemptive"); + int max_sphere = parser.get("max_sphere"); + float thresh = parser.get("threshold"); + float normal_weight = parser.get("normal_weight"); + bool sprt = parser.get("use_sprt"); + string ply = parser.get("ply"); + + Mat_ pts; + if (! ply.empty()) { + pts = cv::ppf_match_3d::loadPLYSimple(ply.c_str(), false); + } + if (elems & 1) { + generatePlane(pts, vector{0.1,.1,.8, .76}, 64); + generatePlane(pts, vector{-.2,-.7,.3, .16}, 64); + } + if (elems & 2) { + generateSphere(pts, vector{12,15,-12, 5}, 128); + generateSphere(pts, vector{-12,15,-12, 5}, 128); + } + if (elems & 4) { + generateCylinder(pts, vector{-12,-15,-12, 0,0,1, 5}, 256); + generateCylinder(pts, vector{20,5,12, 0,1,0, 5}, 256); + } + if (elems & 8) { + generateRandom(pts, vector{-12,31,-5, 5}, 32); + generateRandom(pts, vector{12,-21,15, 5}, 32); + generateRandom(pts, vector{1,-2,1, 25}, 64); + } + if (elems & 16) { + Mat fuzz(pts.size(), pts.type()); + float F = 0.001f; + randu(fuzz,Scalar(-F,-F,-F), Scalar(F,F,F)); + pts += fuzz; + } + cout << pts.size() << " points." << endl; + cv::ppf_match_3d::writePLY(pts, "cloud.ply"); + + auto segment = [napsac,normal_weight,max_sphere,preemptive,sprt](const Mat &cloud, std::vector &models, int model_type, float threshold, int max_iters, int min_inlier, int method_type) -> Mat { + Ptr fitting = SACModelFitting::create(cloud, model_type, method_type, threshold, max_iters); + fitting->set_normal_distance_weight(normal_weight); + fitting->set_max_napsac_radius(napsac); + fitting->set_max_sphere_radius(max_sphere); + fitting->set_preemptive_count(preemptive); + fitting->set_min_inliers(min_inlier); + fitting->set_use_sprt(sprt); + + Mat new_cloud; + fitting->segment(models, new_cloud); + + return new_cloud; + }; + + std::vector models; + if (typ==4) { // cluster only + cv::ptcloud::cluster(pts, min_distance, min_inliers, models, pts); + } else + if (typ==0) { // end to end + pts = segment(pts, models, 1, thresh, iters, min_inliers, method); + cout << pts.total() << " points left." << endl; + pts = segment(pts, models, 2, 0.145f, iters, min_inliers, method); + cout << pts.total() << " points left." << endl; + pts = segment(pts, models, 3, 5.0f, iters, min_inliers, method); + cout << pts.total() << " points left." << endl; + cv::ptcloud::cluster(pts, 7, 20, models, pts); + } else // single model type + pts = segment(pts, models, typ, thresh, iters, min_inliers, method); + + string names[] = {"", "plane","sphere","cylinder","blob"}; + for (size_t i=0; i +#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/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(); + + Ptr cylinder_segmentation = cv::ptcloud::SACModelFitting::create(cloud, CYLINDER_MODEL); + + // add original cloud to window + viz::Viz3d window("original cloud"); + viz::WCloud original_cloud(ptset); + window.showWidget("cloud", original_cloud); + + cylinder_segmentation->set_threshold(0.5); + cylinder_segmentation->set_iterations(80000); + cylinder_segmentation->set_normal_distance_weight(0.5); + + vector models; + cylinder_segmentation->segment(models); + cout << models[0].points.size(); + + vector model_coefficients = models.at(0).coefficients; + cout << model_coefficients.size(); + + double size = 10; + double radius = model_coefficients[6]; + Point3d pt_on_axis(model_coefficients[0], model_coefficients[1], model_coefficients[2]); + Point3d axis_dir(model_coefficients[3], model_coefficients[4], model_coefficients[5]); + Point3d first_point = Point3d(Vec3d(pt_on_axis) + size * (axis_dir)); + Point3d second_point = Point3d(Vec3d(pt_on_axis) - size * (axis_dir)); + viz::WCylinder model(first_point, second_point, radius, 40, viz::Color::green()); + window.showWidget("model", model); + + viz::Viz3d fitted("fitted cloud"); + viz::WCloud cloud_widget2(models[0].points, viz::Color::red()); + fitted.showWidget("fit_cloud", cloud_widget2); + window.showWidget("fit_cloud", cloud_widget2); + fitted.spin(); + + window.spin(); + waitKey(1); + + return 0; +} +#else +int main() { + return CV_ERROR(-215, "this sample needs to be build with opencv's viz module"); +} +#endif \ No newline at end of file diff --git a/modules/ptcloud/samples/viz/sample_plane_fitting.cpp b/modules/ptcloud/samples/viz/sample_plane_fitting.cpp new file mode 100644 index 00000000000..6405d51c082 --- /dev/null +++ b/modules/ptcloud/samples/viz/sample_plane_fitting.cpp @@ -0,0 +1,45 @@ +#include "opencv2/opencv_modules.hpp" +#ifdef HAVE_OPENCV_VIZ +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace cv; +using namespace std; + +int main() { + Mat cloud = cv::viz::readCloud("./data/CobbleStones.obj"); + Ptr planar_segmentation = cv::ptcloud::SACModelFitting::create(cloud); + + // add original cloud to window + viz::Viz3d window("original cloud"); + viz::WCloud original_cloud(cloud); + window.showWidget("cloud", original_cloud); + + planar_segmentation->set_threshold(0.001); + planar_segmentation->set_iterations(1000); + + vector models; + planar_segmentation->segment(models); + CV_Assert(models.size()>0); + + viz::Viz3d fitted("fitted cloud"); + viz::WCloud cloud_widget2(Mat(models[0].points), viz::Color::green()); + fitted.showWidget("fit plane", cloud_widget2); + window.showWidget("fit plane", cloud_widget2); + + window.spin(); + fitted.spin(); + waitKey(1); + return 0; +} +#else +int main() { + return CV_ERROR(-215, "this sample needs to be build with opencv's viz module"); +} +#endif \ No newline at end of file diff --git a/modules/ptcloud/samples/viz/sample_sphere_fitting.cpp b/modules/ptcloud/samples/viz/sample_sphere_fitting.cpp new file mode 100644 index 00000000000..e11c6a5f529 --- /dev/null +++ b/modules/ptcloud/samples/viz/sample_sphere_fitting.cpp @@ -0,0 +1,62 @@ +#include "opencv2/opencv_modules.hpp" +#ifdef HAVE_OPENCV_VIZ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace cv; +using namespace std; + +int main() { + Mat cloud = cv::viz::readCloud("./data/sphere-big.obj"); + Ptr sphere_segmentation = cv::ptcloud::SACModelFitting::create(cloud, 2); + + /// Adds original cloud to window + viz::Viz3d window("original cloud"); + viz::WCloud cloud_widget1(cloud); + window.showWidget("cloud 1", cloud_widget1); + + sphere_segmentation->set_threshold(0.001); + sphere_segmentation->set_iterations(10000); + sphere_segmentation->set_max_radius(10000); + + viz::Viz3d fitted("fitted cloud"); + + vector models; + sphere_segmentation->segment(models); + CV_Assert(models.size()>0); + + viz::Viz3d fitted("fitted cloud"); + viz::WCloud cloud_widget2(Mat(models[0].points), viz::Color::green()); + fitted.showWidget("fit plane", cloud_widget2); + + vector model_coefficients = models.at(0).coefficients; + cout << model_coefficients.size(); + + Point3d center(model_coefficients[0],model_coefficients[1],model_coefficients[2]); + double radius(model_coefficients[3]); + cout << center; + cout << radius; + radius *= 0.75; + + viz::WSphere sphere(center, radius, 10, viz::Color::green());; + window.showWidget("model", sphere); + + window.spin(); + fitted.spin(); + + waitKey(1); + + return 0; +} +#else +int main() { + return CV_ERROR(-215, "this sample needs to be build with opencv's viz module"); +} +#endif \ No newline at end of file diff --git a/modules/ptcloud/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..79c60b90218 --- /dev/null +++ b/modules/ptcloud/src/sac_segmentation.cpp @@ -0,0 +1,1403 @@ +// 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 "opencv2/calib3d.hpp" // lmsolver +#include "opencv2/ptcloud/sac_segmentation.hpp" // public api +#include "opencv2/ptcloud/sac_utils.hpp" // api for tests & samples +#include "opencv2/surface_matching/ppf_helpers.hpp" // ply,normals + +#include +#include +#include +#include + +namespace cv { +namespace ptcloud { + +SACModel::SACModel(int typ) { + type = typ; + score = std::make_pair(0,1000); +} + +inline +bool fequal(float a, float b) { + return std::abs(a - b) <= std::numeric_limits::epsilon(); +} + +inline +bool fequal(const Vec3f &p1, const Vec3f &p2) { + return (fequal(p1[0], p2[0]) && fequal(p1[1], p2[1]) && fequal(p1[2], p2[2])); +} + +struct box { + Point3f m, M; + double r; +}; + +static box bbox(const Mat &cloud) { + box b; + b.m = Point3f(9999,9999,9999); + b.M = Point3f(-9999,-9999,-9999); + for (size_t i=0; i(int(i)); + b.M.x = std::max(b.M.x,p.x); + b.M.y = std::max(b.M.y,p.y); + b.M.z = std::max(b.M.z,p.z); + b.m.x = std::min(b.m.x,p.x); + b.m.y = std::min(b.m.y,p.y); + b.m.z = std::min(b.m.z,p.z); + } + b.r = (norm(b.M - b.m) / 2); + return b; +} + + +template +static void knuth_shuffle(std::vector &vec) { + for (size_t t=0; t pts; + Mat_ model; + PlaneModel(const Mat &cloud) : pts(cloud) {} + + virtual void setModelParameters (const Mat &model_) CV_OVERRIDE {model = model_;} + virtual float getError (int point_idx) CV_OVERRIDE { + Point3f p = pts(point_idx); + double a = model(0) * p.x; + double b = model(1) * p.y; + double c = model(2) * p.z; + double d = a + b + c + model(3); + return (float)abs(d); + } +}; + +struct SphereModel : public Error_ { + Mat_ pts; + Mat_ model; + SphereModel(const Mat &cloud) : pts(cloud) {} + + virtual void setModelParameters (const Mat &model_) CV_OVERRIDE {model = model_;} + virtual float getError (int point_idx) CV_OVERRIDE { + Point3f p = pts(point_idx); + Point3f center(float(model(0)),float(model(1)),float(model(2))); + double distanceFromCenter = norm(p - center); + double distanceFromSurface = fabs(distanceFromCenter - model(3)); + + return (float)distanceFromSurface; + } +}; + +struct CylinderModel : public Error_ { + Mat_ pts, normals; + Mat_ model; + double weight; + Point3d pt_on_axis, axis_dir; + double rsqr; + CylinderModel(const Mat &cloud, const Mat &nrm, double weight_) : pts(cloud), normals(nrm), weight(weight_) {} + + virtual void setModelParameters (const Mat &model_) CV_OVERRIDE { + model = model_; + pt_on_axis = Point3d(model(0),model(1),model(2)); + axis_dir = Point3d(model(3),model(4),model(5)); + rsqr = model(6)*model(6); + } + virtual float getError (int point_idx) CV_OVERRIDE { + Point3d pt = pts(point_idx); + Point3d a_cross = axis_dir.cross(pt_on_axis - pt); + double distanceFromAxis = fabs(a_cross.dot(a_cross) / axis_dir.dot(axis_dir)); + double distanceFromSurface = fabs(distanceFromAxis - rsqr); + + double d_normal = 0.0; + if ((! normals.empty()) && (weight > 0)) { + // Calculate the point's projection on the cylinder axis + double dist = (pt.dot(axis_dir) - pt_on_axis.dot(axis_dir)); + Point3d pt_proj = pt_on_axis + dist * axis_dir; + Point3d dir = pt - pt_proj; + dir = dir / sqrt(dir.dot(dir)); + // Calculate the angular distance between the point normal and the (dir=pt_proj->pt) vector + double rad = normals(point_idx).dot(dir); + if (rad < -1.0) rad = -1.0; + if (rad > 1.0) rad = 1.0; + d_normal = fabs(acos (rad)); + // convert range 0 to PI/2 + d_normal = (std::min) (d_normal, CV_PI - d_normal); + } + + // calculate overall distance as weighted sum of the two distances. + return (float)fabs(weight * d_normal + (1 - weight) * distanceFromSurface); + } +}; + + + +struct SPRT_history { + /* + * delta: + * The probability of a data point being consistent + * with a ‘bad’ model is modeled as a probability of + * a random event with Bernoulli distribution with parameter + * δ : p(1|Hb) = δ. + + * epsilon: + * The probability p(1|Hg) = ε + * that any randomly chosen data point is consistent with a ‘good’ model + * is approximated by the fraction of inliers ε among the data + * points + + * A is the decision threshold, the only parameter of the Adapted SPRT + */ + double epsilon, delta, A; + // number of samples processed by test + int tested_samples; // k + SPRT_history () : epsilon(0), delta(0), A(0) { + tested_samples = 0; + } +}; + +///////////////////////////////////// SPRT IMPL ////////////////////////////////////////// +struct SPRTImpl : public SPRT { + RNG rng; + const int points_size; + int highest_inlier_number, current_sprt_idx; // i + // time t_M needed to instantiate a model hypothesis given a sample + // Let m_S be the number of models that are verified per sample + const double inlier_threshold, norm_thr, one_over_thr, t_M, m_S; + + double lowest_sum_errors, current_epsilon, current_delta, current_A, + delta_to_epsilon, complement_delta_to_complement_epsilon; + + std::vector sprt_histories; + + bool last_model_is_good; + + const double log_eta_0; + const int sample_size, MAX_ITERATIONS; + double lambda, sum_errors; + int tested_inliers; + + SACScore score; + const ScoreMethod score_type; + + SPRTImpl (int state, int points_size_, + double inlier_threshold_, double prob_pt_of_good_model, double prob_pt_of_bad_model, + double time_sample, double avg_num_models, int sample_size_, int max_iterations_, ScoreMethod score_type_) + : rng(state), + points_size(points_size_), + inlier_threshold (inlier_threshold_), + norm_thr(inlier_threshold_*9/4), + one_over_thr (1/norm_thr), + t_M(time_sample), + m_S(avg_num_models), + log_eta_0(log(1-.99/*confidence*/)), + sample_size (sample_size_), + MAX_ITERATIONS(max_iterations_), + score_type(score_type_) { + // reserve (approximately) some space for sprt vector. + sprt_histories.reserve(20); + + createTest(prob_pt_of_good_model, prob_pt_of_bad_model); + + highest_inlier_number = 0; + lowest_sum_errors = std::numeric_limits::max(); + last_model_is_good = false; + } + + /* + * add a data point, and recalculate lambda + * returns: whether to continue evaluating this model + * @model: model to verify + * p(x(r)|Hb) p(x(j)|Hb) + * lambda(j) = Product (----------) = lambda(j-1) * ---------- + * p(x(r)|Hg) p(x(j)|Hg) + * Set j = 1 + * 1. Check whether j-th data point is consistent with the + * model + * 2. Compute the likelihood ratio λj eq. (1) + * 3. If λj > A, decide the model is ’bad’ (model ”re-jected”), + * else increment j or continue testing + * 4. If j = N the number of correspondences decide model ”accepted” + */ + bool addDataPoint(int tested_point, double error) CV_OVERRIDE { + if (tested_point==0) { + lambda = 1; + sum_errors = 0; + tested_inliers = 0; + lowest_sum_errors = std::numeric_limits::max(); + } + if (error < inlier_threshold) { + tested_inliers++; + lambda *= delta_to_epsilon; + } else { + lambda *= complement_delta_to_complement_epsilon; + // since delta is always higher than epsilon, then lambda can increase only + // when point is not consistent with model + if (lambda > current_A) { + return false; + } + } + if (score_type == ScoreMethod::SCORE_METHOD_MSAC) { + if (error < norm_thr) + sum_errors -= (1 - error * one_over_thr); + if (sum_errors - points_size + tested_point > lowest_sum_errors) { + return false; + } + } else if (score_type == ScoreMethod::SCORE_METHOD_RANSAC) { + if (tested_inliers + points_size - tested_point < highest_inlier_number) { + return false; + } + } + return true; + } + + /* Verifies model and returns model score. + * Return: true if model is good, false - otherwise. + */ + bool isModelGood(int tested_point) CV_OVERRIDE { + last_model_is_good = tested_point == points_size; + // increase number of samples processed by current test + sprt_histories[current_sprt_idx].tested_samples++; + if (last_model_is_good) { + score.first = tested_inliers; + if (score_type == ScoreMethod::SCORE_METHOD_MSAC) { + score.second = sum_errors; + if (lowest_sum_errors > sum_errors) + lowest_sum_errors = sum_errors; + } else if (score_type == ScoreMethod::SCORE_METHOD_RANSAC) + score.second = -static_cast(tested_inliers); + + const double new_epsilon = static_cast(tested_inliers) / points_size; + if (new_epsilon > current_epsilon) { + highest_inlier_number = tested_inliers; // update max inlier number + /* + * Model accepted and the largest support so far: + * design (i+1)-th test (εi + 1= εˆ, δi+1 = δ, i := i + 1). + * Store the current model parameters θ + */ + createTest(new_epsilon, current_delta); + } + } else { + /* + * Since almost all tested models are ‘bad’, the probability + * δ can be estimated as the average fraction of consistent data points + * in rejected models. + */ + // add 1 to tested_point, because loop over tested_point starts from 0 + const double delta_estimated = static_cast (tested_inliers) / (tested_point+1); + if (delta_estimated > 0 && fabs(current_delta - delta_estimated) + / current_delta > 0.05) { + /* + * Model rejected: re-estimate δ. If the estimate δ_ differs + * from δi by more than 5% design (i+1)-th test (εi+1 = εi, + * δi+1 = δˆ, i := i + 1) + */ + createTest(current_epsilon, delta_estimated); + } + } + return last_model_is_good; + } + + SACScore getScore() CV_OVERRIDE { + return score; + } + /* + * Termination criterion: + * l is number of tests + * n(l) = Product from i = 0 to l ( 1 - P_g (1 - A(i)^(-h(i)))^k(i) ) + * log n(l) = sum from i = 0 to l k(i) * ( 1 - P_g (1 - A(i)^(-h(i))) ) + * + * log (n0) - log (n(l-1)) + * k(l) = ----------------------- (9) + * log (1 - P_g*A(l)^-1) + * + * A is decision threshold + * P_g is probability of good model. + * k(i) is number of samples verified by i-th sprt. + * n0 is typically set to 0.05 + * this equation does not have to be evaluated before nR < n0 + * nR = (1 - P_g)^k + */ + int update (int inlier_size) CV_OVERRIDE { + if (sprt_histories.empty()) + return std::min(MAX_ITERATIONS, getStandardUpperBound(inlier_size)); + + const double epsilon = static_cast(inlier_size) / points_size; // inlier probability + const double P_g = pow (epsilon, sample_size); // probability of good sample + + double log_eta_lmin1 = 0; + + int total_number_of_tested_samples = 0; + const int sprts_size_min1 = static_cast(sprt_histories.size())-1; + if (sprts_size_min1 < 0) return getStandardUpperBound(inlier_size); + // compute log n(l-1), l is number of tests + for (int test = 0; test < sprts_size_min1; test++) { + log_eta_lmin1 += log (1 - P_g * (1 - pow (sprt_histories[test].A, + -computeExponentH(sprt_histories[test].epsilon, epsilon,sprt_histories[test].delta)))) + * sprt_histories[test].tested_samples; + total_number_of_tested_samples += sprt_histories[test].tested_samples; + } + + // Implementation note: since η > ηR the equation (9) does not have to be evaluated + // before ηR < η0 is satisfied. + if (std::pow(1 - P_g, total_number_of_tested_samples) < log_eta_0) + return std::min(MAX_ITERATIONS, getStandardUpperBound(inlier_size)); + // use decision threshold A for last test (l-th) + const double predicted_iters_sprt = (log_eta_0 - log_eta_lmin1) / + log (1 - P_g * (1 - 1 / sprt_histories[sprts_size_min1].A)); // last A + //cout << "pred " << predicted_iters_sprt << endl; + if (std::isnan(predicted_iters_sprt) || std::isinf(predicted_iters_sprt)) + return getStandardUpperBound(inlier_size); + + if (predicted_iters_sprt < 0) return 0; + // compare with standard upper bound + if (predicted_iters_sprt < MAX_ITERATIONS) + return std::min(static_cast(predicted_iters_sprt), + getStandardUpperBound(inlier_size)); + return getStandardUpperBound(inlier_size); + } + +private: + // Saves sprt test to sprt history and update current epsilon, delta and threshold. + void createTest (double epsilon, double delta) { + // if epsilon is closed to 1 then set them to 0.99 to avoid numerical problems + if (epsilon > 0.999999) epsilon = 0.999; + // delta can't be higher than epsilon, because ratio delta / epsilon will be greater than 1 + if (epsilon < delta) delta = epsilon-0.0001; + // avoid delta going too high as it is very unlikely + // e.g., 30% of points are consistent with bad model is not very real + if (delta > 0.3) delta = 0.3; + + SPRT_history new_sprt_history; + new_sprt_history.epsilon = epsilon; + new_sprt_history.delta = delta; + new_sprt_history.A = estimateThresholdA (epsilon, delta); + + sprt_histories.emplace_back(new_sprt_history); + + current_A = new_sprt_history.A; + current_delta = delta; + current_epsilon = epsilon; + + delta_to_epsilon = delta / epsilon; + complement_delta_to_complement_epsilon = (1 - delta) / (1 - epsilon); + current_sprt_idx = static_cast(sprt_histories.size()) - 1; + } + + /* + * A(0) = K1/K2 + 1 + * A(n+1) = K1/K2 + 1 + log (A(n)) + * K1 = t_M / P_g + * K2 = m_S/(P_g*C) + * t_M is time needed to instantiate a model hypotheses given a sample + * P_g = epsilon ^ m, m is the number of data point in the Ransac sample. + * m_S is the number of models that are verified per sample. + * p (0|Hb) p (1|Hb) + * C = p(0|Hb) log (---------) + p(1|Hb) log (---------) + * p (0|Hg) p (1|Hg) + */ + double estimateThresholdA (double epsilon, double delta) { + const double C = (1 - delta) * log ((1 - delta) / (1 - epsilon)) + + delta * (log(delta / epsilon)); + // K = K1/K2 + 1 = (t_M / P_g) / (m_S / (C * P_g)) + 1 = (t_M * C)/m_S + 1 + const double K = t_M * C / m_S + 1; + double An, An_1 = K; + // compute A using a recursive relation + // A* = lim(n->inf)(An), the series typically converges within 4 iterations + for (int i = 0; i < 10; i++) { + An = K + log(An_1); + if (fabs(An - An_1) < FLT_EPSILON) + break; + An_1 = An; + } + return An; + } + + inline int getStandardUpperBound(int inlier_size) const { + const double predicted_iters = log_eta_0 / log(1 - std::pow + (static_cast(inlier_size) / points_size, sample_size)); + return (! std::isinf(predicted_iters) && predicted_iters < MAX_ITERATIONS) ? + static_cast(predicted_iters) : MAX_ITERATIONS; + } + /* + * h(i) must hold + * + * δ(i) 1 - δ(i) + * ε (-----)^h(i) + (1 - ε) (--------)^h(i) = 1 + * ε(i) 1 - ε(i) + * + * ε * a^h + (1 - ε) * b^h = 1 + * Has numerical solution. + */ + static double computeExponentH (double epsilon, double epsilon_new, double delta) { + const double a = log (delta / epsilon); // log likelihood ratio + const double b = log ((1 - delta) / (1 - epsilon)); + + const double x0 = log (1 / (1 - epsilon_new)) / b; + const double v0 = epsilon_new * exp (x0 * a); + const double x1 = log ((1 - 2*v0) / (1 - epsilon_new)) / b; + const double v1 = epsilon_new * exp (x1 * a) + (1 - epsilon_new) * exp(x1 * b); + const double h = x0 - (x0 - x1) / (1 + v0 - v1) * v0; + + if (std::isnan(h)) + // The equation always has solution for h = 0 + // ε * a^0 + (1 - ε) * b^0 = 1 + // ε + 1 - ε = 1 -> 1 = 1 + return 0; + return h; + } +}; + +Ptr SPRT::create(int state, int points_size_, + double inlier_threshold_, double prob_pt_of_good_model, double prob_pt_of_bad_model, + double time_sample, double avg_num_models, int sample_size_, int max_iterations_, ScoreMethod score_type_) { + return makePtr(state, points_size_, + inlier_threshold_, prob_pt_of_good_model, prob_pt_of_bad_model, + time_sample, avg_num_models, sample_size_, max_iterations_, score_type_); +} + +SACScore getInliers(int model_type, const std::vector &coeffs, const Mat &cloud, const Mat &normals, const std::vector &indices, const double threshold, const double best_inliers, std::vector& inliers, double normal_distance_weight_, Ptr sprt) { + inliers.clear(); + + Ptr mdl; + switch (model_type) { + case PLANE_MODEL: mdl = makePtr(cloud); break; + case SPHERE_MODEL: mdl = makePtr(cloud); break; + case CYLINDER_MODEL: mdl = makePtr(cloud, normals, normal_distance_weight_); break; + default: CV_Error(215, format("unsupported model type %d", model_type).c_str()); + } + mdl->setModelParameters(Mat(coeffs)); + + double msac=0; // use same calculation as SPRT + double norm_thr = (threshold*9/4); + double one_over_thr = (1/norm_thr); + size_t num_points = indices.size(); + + size_t i; + for (i=0; i < num_points; i++) { + if (i == num_points/4) { + if (inliers.size() < best_inliers/8) { + break; + } + } + double distance = mdl->getError(indices[i]); + + if (!sprt.empty()) { + if (! sprt->addDataPoint(int(i),distance)) { + break; + } + } + + if (distance < norm_thr) + msac -= (1 - distance * one_over_thr); + + if (distance < threshold) + inliers.emplace_back(indices[i]); + } + + if (!sprt.empty()) { + if (sprt->isModelGood(int(i))) + return sprt->getScore(); + } + + return SACScore((double)inliers.size(), msac); +} + + +// http://ambrsoft.com/TrigoCalc/Sphere/Spher3D_.htm +static bool getSphereFromPoints(const Mat &cloud, const std::vector &inliers, std::vector &coeffs) { + const Vec3f* points = cloud.ptr(0); + + Mat temp(4, 4, CV_32FC1, 1.0f); // last column must be 1 + for (int i = 0; i < 4; i++) { + size_t point_idx = inliers[i]; + float* tempi = temp.ptr(i); + tempi[0] = (float) points[point_idx][0]; + tempi[1] = (float) points[point_idx][1]; + tempi[2] = (float) points[point_idx][2]; + tempi[3] = 1; + } + double m11 = determinant(temp); + if (fequal(float(m11), 0)) return false; // no sphere exists + + for (int i = 0; i < 4; i++) { + size_t point_idx = inliers[i]; + float* tempi = temp.ptr(i); + + tempi[0] = (float) points[point_idx][0] * (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++) { + size_t 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++) { + size_t 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++) { + size_t 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); + + Point3d center( + 0.5 * m12 / m11, + 0.5 * m13 / m11, + 0.5 * m14 / m11); + + double radius = std::sqrt( + center.x * center.x + + center.y * center.y + + center.z * center.z - m15 / m11); + + if (std::isnan(radius)) return false; + + coeffs = std::vector {center.x, center.y, center.z, radius}; + return true; +} + +static bool getPlaneFromPoints(const Mat &cloud, const std::vector &inliers, std::vector &coeffs) { + // REF: https://www.ilikebigbits.com/2015_03_04_plane_from_points.html + const Vec3f* points = cloud.ptr(0); + Vec3d abc; + Vec3f centroid(0, 0, 0); + for (size_t idx : inliers) + centroid += points[idx]; + + centroid /= double(inliers.size()); + + double xx = 0, xy = 0, xz = 0, yy = 0, yz = 0, zz = 0; + + for (size_t idx : inliers) { + Vec3f r = points[idx] - centroid; + xx += r(0) * r(0); + xy += r(0) * r(1); + xz += r(0) * r(2); + yy += r(1) * r(1); + yz += r(1) * r(2); + zz += r(2) * r(2); + } + + double det_x = yy * zz - yz * yz; + double det_y = xx * zz - xz * xz; + double det_z = xx * yy - xy * xy; + + 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 (fequal(float(magnitude_abc), 0)) + return false; + + abc /= magnitude_abc; + double d = -abc.dot(centroid); + + coeffs = std::vector{abc[0], abc[1], abc[2], d}; + return true; +} + +// from 2 points & normals +static bool getCylinderFromPoints(const Mat &cloud, const Mat &normals_cld, const std::vector &inliers, std::vector & model_coefficients) { + CV_Assert(inliers.size() == 2); + + const Vec3f* points = cloud.ptr(0); + const Vec3f* normals = normals_cld.ptr(0); + + Vec3f p1 = points[inliers[0]]; + Vec3f p2 = points[inliers[1]]; + + if (fequal(p1, p2)) + return false; + + Vec3f n1 (normals[inliers[0]]); + Vec3f n2 (normals[inliers[1]]); + Vec3f w = n1 + p1 - p2; + + float a = n1.dot(n1); + float b = n1.dot(n2); + float c = n2.dot(n2); + float d = n1.dot(w); + float e = n2.dot(w); + float denominator = a*c - b*b; + float sc, tc; + // Compute the line parameters of the two closest points + if (denominator < 1e-8) { // The lines are almost parallel + sc = 0.0f; + tc = (b > c ? d / b : e / c); // Use the largest denominator + } else { + sc = (b*e - c*d) / denominator; + tc = (a*e - b*d) / denominator; + } + + // 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 line_len_sqr = line_dir.dot(line_dir); + double divide_by = std::sqrt (line_len_sqr); + + // direction of line; + model_coefficients[3] = line_dir[0] / divide_by; + model_coefficients[4] = line_dir[1] / divide_by; + model_coefficients[5] = line_dir[2] / divide_by; + Vec3f line_normal = line_dir.cross(line_pt - p1); + + // radius of cylinder + double radius_squared = fabs(line_normal.dot(line_normal)) / line_len_sqr; + if (fequal(float(radius_squared), 0)) + return false; + model_coefficients[6] = sqrt(radius_squared); + + return (true); +} + +// from 3 points +static bool getCylinderFromPoints(const Mat &cloud, const std::vector &inliers, std::vector & model_coefficients) { + CV_Assert(inliers.size() == 3); + + const Vec3f* points = cloud.ptr(0); + + // two points form a line on the hull of the cylinder + Vec3f p1 = points[inliers[0]]; + Vec3f p2 = points[inliers[1]]; + // a third point on the opposite side + Vec3f p3 = points[inliers[2]]; + // degenerate if 2 points are same + if (fequal(p1, p2) || fequal(p1, p3) || fequal(p2, p3)) + return false; + + Vec3f dr = p2 - p1; + normalize(dr, dr); + + // distance from p3 to line p1p2 + double a = p3.dot(dr); + Vec3f n = dr - a*p3; + double r = std::sqrt(n.dot(n)); + + // move line_point halfway along the normal (to the center of the cylinder) + model_coefficients = std::vector { + p1[0] + n[0]/2, p1[1] + n[1]/2, p1[2]+n[2]/2, + dr[0], dr[1], dr[2], + r/2 + }; + return (true); +} + +// pca on inlier points +// https://pointclouds.org/documentation/centroid_8hpp_source.html#l00485 +static std::vector optimizePlane(const Mat &pointcloud, + const std::vector &indices, + const std::vector &old_coeffs = {}) { + CV_UNUSED(old_coeffs); + + const Point3f *cloud = pointcloud.ptr(0); + + // de-mean before accumulating values + Point3f centroid(0, 0, 0); + for (int idx : indices) { + centroid += cloud[idx]; + } + centroid /= double(indices.size()); + + std::vector accu(6, 0); + for (const auto &index : indices) { + const Point3f p = cloud[index] - centroid; + accu[0] += p.x * p.x; + accu[1] += p.x * p.y; + accu[2] += p.x * p.z; + accu[3] += p.y * p.y; + accu[4] += p.y * p.z; + accu[5] += p.z * p.z; + } + Mat(accu) /= float(indices.size()); + + Mat_ covariance_matrix(3,3); + covariance_matrix << + accu[0], accu[1], accu[2], + accu[1], accu[3], accu[4], + accu[2], accu[4], accu[5]; + + Mat_ evec, eval; + eigen(covariance_matrix, eval, evec); + // the evec corresponding to the *smallest* eval + Mat_ ev = evec.row(2); + std::vector coeffs = { + ev(0), ev(1), ev(2), + -1 * (ev(0)*centroid.x + ev(1)*centroid.y + ev(2)*centroid.z) + }; + + return coeffs; +} + +struct SphereSolverCallback : LMSolver::Callback { + Mat_ P; + SphereSolverCallback(const Mat &pts) : P(pts) {} + + bool compute (InputArray param, OutputArray err, OutputArray J) const CV_OVERRIDE { + Mat_ in = param.getMat(); + + Point3f c(float(in(0)),float(in(1)),float(in(2))); + double r = in(3); + + // the levmarquard solver needs an error metrics for each callback + int count = (int)P.total(); + err.create(count,1,CV_64F); + Mat_ e = err.getMat(); + + // but the jacobian matrix needs only to be (re)generated, if the error falls beyond some threshold + Mat_ j; + if (J.needed()) { + J.create(count, (int)in.total(), CV_64F); + j = J.getMat(); + } + + for (int i=0; i optimizeSphere(const Mat &pointcloud, + const std::vector &indices, + const std::vector &old_coeffs = {}) { + std::vector new_coeffs = old_coeffs; + if (new_coeffs.empty()) + new_coeffs = std::vector(4,1.0); + + Mat_ pts; + for(auto i:indices) + pts.push_back(pointcloud.at(i)); + + Ptr cb = makePtr(pts); + Ptr lm = LMSolver::create(cb, 100); + int n = lm->run(new_coeffs); + CV_Check(n, n<100, "failed to converge"); + CV_Assert(!std::isnan(new_coeffs[4])); + return new_coeffs; +} + +// https://www.ncbi.nlm.nih.gov/pmc/articles/PMC4890955/ +struct CylinderSolverCallback : LMSolver::Callback { + Mat_ P; + CylinderSolverCallback(const Mat &pts) : P(pts) {} + + bool compute (InputArray param, OutputArray err, OutputArray J) const CV_OVERRIDE { + Mat_ in = param.getMat(); + + double x=in(0), y=in(1), z=in(2); // point on axis + double a=in(3), b=in(4), c=in(5); // axis dir + double r=in(6); // radius + + // normalize dir vec + double len = sqrt(a*a+b*b+c*c) + 0.000001; + a /= len; b /= len; c /= len; + + int count = (int)P.total(); + err.create(count, 1, CV_64F); + Mat_ e = err.getMat(); + + Mat_ j; + if (J.needed()) { + J.create(count, (int)in.total(), CV_64F); + j = J.getMat(); + } + + for (int i=0; i optimizeCylinder(const Mat &pointcloud, + const std::vector &indices, + const std::vector &old_coeffs = {}) { + std::vector new_coeffs = old_coeffs; + if (new_coeffs.empty()) + new_coeffs = std::vector(7,1.0); + + Mat_ pts; + for (auto i:indices) + pts.push_back(pointcloud.at(i)); + + Ptr cb = makePtr(pts); + Ptr lm = LMSolver::create(cb, 100); + int n = lm->run(new_coeffs); + CV_Check(n, n<100, "failed to converge"); + + // normalize direction vector: + double len = norm(Point3d(new_coeffs[3],new_coeffs[4],new_coeffs[5])); + new_coeffs[3] /= len; new_coeffs[4] /= len; new_coeffs[5] /= len; + + return new_coeffs; +} + +std::vector optimizeModel(int model_type, const Mat &cloud, const std::vector &inliers_indices, const std::vector &old_coeff) { + switch (model_type) { + case PLANE_MODEL: + return optimizePlane(cloud, inliers_indices, old_coeff); + case SPHERE_MODEL: + return optimizeSphere(cloud, inliers_indices, old_coeff); + case CYLINDER_MODEL: + return optimizeCylinder(cloud, inliers_indices, old_coeff); + // default: + // CV_Error(215, format("invalid model_type %d", model_type).c_str()); + } + return std::vector(); +} + + +// how many points to sample for a minimal hypothesis +static size_t rnd_model_points(int model_type, const Mat &normals) { + size_t num_rnd_model_points = + model_type==PLANE_MODEL ? 3 : + model_type==SPHERE_MODEL ? 4 : + model_type==CYLINDER_MODEL ? + size_t(normals.empty() + ? 3 // from 3 points + : 2 // 2 points and 2 normals + ) : 0; // invalid + CV_Assert(num_rnd_model_points != 0); // invalid model enum + return num_rnd_model_points; +} + +static std::vector sampleHypothesis(RNG &rng, const Mat &cloud, const std::vector &indices, size_t num_rnd_model_points, double max_napsac_radius) { + std::vector current_model_inliers; + if (max_napsac_radius > 0) { + // NAPSAC like sampling strategy, + // assume good hypothesis inliers are locally close to each other + // (enforce spatial proximity) + int start = rng.uniform(0, (int)indices.size()); + current_model_inliers.emplace_back(start); + Point3f a = cloud.at(indices[start]); + for (int n=0; n<100; n++) { + int next = rng.uniform(0, (int)indices.size()); + Point3f b = cloud.at(indices[next]); + if (norm(a-b) > max_napsac_radius) + continue; + + current_model_inliers.emplace_back(next); + if (current_model_inliers.size() == num_rnd_model_points) + break; + } + } else { + // uniform sample from indices + for (size_t j = 0; j < num_rnd_model_points; j++) + current_model_inliers.emplace_back(rng.uniform(0, (int)indices.size())); + } + + return current_model_inliers; +} + +bool generateHypothesis(int model_type, const Mat &cloud, const std::vector ¤t_model_inliers, const Mat &normals, double max_radius, double normal_distance_weight_, std::vector &coefficients) { + if (model_type == PLANE_MODEL) { + return getPlaneFromPoints(cloud, current_model_inliers, coefficients); + } + + if (model_type == SPHERE_MODEL) { + bool valid = getSphereFromPoints(cloud, current_model_inliers, coefficients); + if (!valid) return false; + if (coefficients[3] > max_radius) return false; + return true; + } + if (model_type == CYLINDER_MODEL) { + if ((normals.total() > 0) && (normal_distance_weight_ > 0)) + return getCylinderFromPoints(cloud, normals, current_model_inliers, coefficients); + else + return getCylinderFromPoints(cloud, current_model_inliers, coefficients); + } + + return false; +} + +Mat generateNormals(const Mat &cloud) { + // Reshape the cloud for Compute Normals Function + Mat _cld_reshaped = cloud; + if (_cld_reshaped.rows < _cld_reshaped.cols) + _cld_reshaped = _cld_reshaped.t(); + _cld_reshaped = _cld_reshaped.reshape(1); + + // calculate normals from Knn cloud neighbours + Mat _pointsAndNormals; + ppf_match_3d::computeNormalsPC3d(_cld_reshaped, _pointsAndNormals, 8, false, Vec3d(0,0,0)); + + Mat normals; + Mat(_pointsAndNormals.colRange(3,6)).copyTo(normals); + normals = normals.reshape(3, (int)cloud.total()); + return normals.t(); +} + + +class SACModelFittingImpl : public SACModelFitting { +private: + Mat cloud; + Mat normals; + int model_type; + int method_type; + int preemptive_count = 0; + int min_inliers = 10; + int max_iters; + double threshold; + double normal_distance_weight_ = 0; + double max_sphere_radius = 60; + double max_napsac_radius = 0; + + bool use_sprt = false; + Ptr sprt; + + bool fit_ransac(const std::vector& indices, std::vector &model_instances); + bool fit_preemptive(const std::vector &indices, std::vector &model_instances); + +public: + SACModelFittingImpl (InputArray cloud, int model_type = PLANE_MODEL, int method_type = SAC_METHOD_RANSAC, double threshold = 20,int max_iters = 1000); + + void segment(std::vector &model_instances, OutputArray new_cloud=noArray()) CV_OVERRIDE; + + void set_cloud(InputArray cloud, bool with_normals=false) CV_OVERRIDE; + void set_model_type(int type) CV_OVERRIDE; + void set_method_type(int type) CV_OVERRIDE; + void set_threshold (double threshold) CV_OVERRIDE; + void set_iterations (int iterations) CV_OVERRIDE; + void set_normal_distance_weight(double weight) CV_OVERRIDE; + void set_max_sphere_radius(double max_radius) CV_OVERRIDE; + void set_max_napsac_radius(double max_radius) CV_OVERRIDE; + void set_preemptive_count(int preemptive) CV_OVERRIDE; + void set_min_inliers(int min_inliers) CV_OVERRIDE; + void set_use_sprt(bool sprt) CV_OVERRIDE; +}; + + +Ptr SACModelFitting::create(InputArray cloud, int model_type, int method_type, double threshold, int max_iters) { + return makePtr(cloud, model_type, method_type, threshold, max_iters); +} + +SACModelFittingImpl::SACModelFittingImpl (InputArray inp_cloud, int set_model_type, int set_method_type, double set_threshold, int set_max_iters) + : model_type(set_model_type), method_type(set_method_type), max_iters(set_max_iters), threshold(set_threshold) { + // if there are normals, keep them + Mat input_cloud = inp_cloud.getMat(); + bool with_normals = input_cloud.channels() == 1 && (input_cloud.cols == 6 || input_cloud.rows == 6); + set_cloud(input_cloud, with_normals); +} + +bool SACModelFittingImpl::fit_ransac(const std::vector &indices, std::vector &model_instances) { + RNG rng((uint64)-1); + // Initialize the best future model. + SACModel bestModel, curModel; + + size_t num_rnd_model_points = rnd_model_points(model_type, normals); + CV_Assert(indices.size() >= num_rnd_model_points); + + int stop = (int)stopping(min_inliers, indices.size(), num_rnd_model_points); + + if (use_sprt) { + sprt = SPRT::create(9999, (int)indices.size(), + threshold, 0.01, 0.008, + 200, 3, (int)num_rnd_model_points, max_iters, (ScoreMethod)method_type); + } + + for (int i = 0; i < std::min(stop, max_iters); ++i) { + // generate model hypothesis + curModel = SACModel(SacModelType(model_type)); + + std::vector hypothesis_inliers = sampleHypothesis(rng, cloud, indices, num_rnd_model_points, max_napsac_radius); + if (hypothesis_inliers.size() != num_rnd_model_points) + continue; // bad starting point for napsac + + bool valid_model = generateHypothesis(model_type, cloud, hypothesis_inliers, normals, max_sphere_radius, normal_distance_weight_, curModel.coefficients); + if (!valid_model) + continue; + + // check inliers for the current hypothesis + curModel.score = getInliers(model_type, curModel.coefficients, cloud, normals, indices, threshold, bestModel.score.first, curModel.indices, normal_distance_weight_, sprt); + + if (curModel.indices.size() < size_t(min_inliers)) + continue; + + bool better = false; + if (method_type == SAC_METHOD_RANSAC) + better = bestModel.score.first < curModel.score.first; + if (method_type == SAC_METHOD_MSAC) + better = bestModel.score.second > curModel.score.second; + + if (better) { + // apply local optimization + // (this could run on a fraction of the indices, but right now it's fast enough to run on all of them) + std::vector new_coeff = optimizeModel(model_type, cloud, curModel.indices, curModel.coefficients); + + // check again if it improved + std::vector new_inliers; + SACScore new_result = getInliers(model_type, new_coeff, cloud, normals, indices, threshold, curModel.score.first, new_inliers, normal_distance_weight_, sprt); + + if (method_type == SAC_METHOD_RANSAC) + better = new_result.first > curModel.score.first; + if (method_type == SAC_METHOD_MSAC) + better = new_result.second < curModel.score.second; + + if (better) { + curModel.score = new_result; + curModel.coefficients = new_coeff; + curModel.indices = new_inliers; + } + + if (!sprt.empty()) + stop = i + (int)sprt->update((int)curModel.score.first); + else + stop = i + (int)stopping((size_t)curModel.score.first, indices.size(), num_rnd_model_points); + + bestModel = curModel; + } + } + + if (bestModel.coefficients.size() && (bestModel.indices.size() > size_t(min_inliers))) { + model_instances.push_back(bestModel); + return true; + } + return false; +} + +// how many models to retain after each data block +// RaguramECCV08.pdf (5) +inline +double preemptive_func(size_t i, size_t M, size_t B) { + return M * std::pow(2.0, -(double(i)/B)); +} + +// D. Nister, Preemptive RANSAC for live structure and motion estimation, in International Conference on Computer Vision (ICCV), 2003. +bool SACModelFittingImpl::fit_preemptive(const std::vector &indices, std::vector &model_instances) { + RNG rng((uint64)-1); + + size_t num_rnd_model_points = rnd_model_points(model_type, normals); + if (indices.size() < num_rnd_model_points) + return false; + + // generate model hypotheses in parallel + std::vector preemptive; + while (preemptive.size() < (size_t)preemptive_count) { + SACModel model = SACModel(SacModelType(model_type)); + std::vector hypothesis_inliers = sampleHypothesis(rng, cloud, indices, num_rnd_model_points, max_napsac_radius); + if (hypothesis_inliers.size() != num_rnd_model_points) + continue; // bad starting point for napsac + + bool valid_model = generateHypothesis(model_type, cloud, hypothesis_inliers, normals, max_sphere_radius, normal_distance_weight_, model.coefficients); + if (!valid_model) + continue; + + preemptive.push_back(model); + } + + size_t preemptive_pos = 0; // "i", the current data position + size_t preemptive_step = 100; // "B", the block size + while ((preemptive.size() > 1) && (preemptive_pos < indices.size())) { + + // slice a data block + std::vector partial_set( + indices.begin() + preemptive_pos, + indices.begin() + std::min((preemptive_pos + preemptive_step), indices.size())); + preemptive_pos += preemptive_step; + + // parallel evaluation of each data block + for (size_t j=0; j current_model_inliers; + SACScore score = getInliers(model_type, model.coefficients, cloud, normals, partial_set, threshold, 0, current_model_inliers, normal_distance_weight_); + model.score.first += score.first; + model.score.second += score.second; + model.indices.insert(model.indices.end(), current_model_inliers.begin(), current_model_inliers.end()); + } + + // sort descending + if (method_type == SAC_METHOD_RANSAC) { + std::sort(preemptive.begin(), preemptive.end(), [](const SACModel &a,const SACModel &b) -> bool { + return b.score.first < a.score.first; // RANSAC + }); + } else { + std::sort(preemptive.begin(), preemptive.end(), [](const SACModel &a,const SACModel &b) -> bool { + return b.score.second > a.score.second; // MSAC + }); + } + + // prune models + int retain = (int)preemptive_func(preemptive_pos, (size_t)preemptive_count, preemptive_step); + preemptive.erase(preemptive.begin() + retain + 1, preemptive.end()); + } + + SACModel &model = preemptive[0]; + if (model.score.first < min_inliers) + return false; + + // "polish" the current best model, it might not have seen all data available. + std::vector new_coeff = optimizeModel(model_type, cloud, model.indices, model.coefficients); + std::vector new_inliers; + SACScore new_result = getInliers(model_type, new_coeff, cloud, normals, indices, threshold, 0, new_inliers, normal_distance_weight_); + if (new_inliers.size() >= model.indices.size()) { + model.score = new_result; + model.indices = new_inliers; + model.coefficients = new_coeff; + } + + model_instances.push_back(model); + return true; +} + + +void SACModelFittingImpl::segment(std::vector &model_instances, OutputArray new_cloud) { + sprt.release(); // make sure it's only used in fit_ransac() and only if use_sprt is on. + + size_t num_points = cloud.total(); + + // optionally generate normals for the Cylinder model + if ((model_type == CYLINDER_MODEL) && (normals.empty()) && (normal_distance_weight_ > 0)) { + normals = generateNormals(cloud); + } + + // a "mask" for already segmented points (0 == not_segmented) + std::vector point_labels(cloud.total(), 0); + long num_segmented_points = 0; + int label = 0; // to mark segmented points + while (true) { + label++; + + // filter unused point indices + std::vector indices; + for (size_t i = 0; i < num_points; i++) { + if (point_labels[i] == 0) indices.push_back(int(i)); + } + if (indices.empty()) + break; + + // randomize indices, so we can assume, possible inliers are distributed uniformly. + knuth_shuffle(indices); + + bool successful_fitting = (preemptive_count == 0) + ? fit_ransac(indices, model_instances) + : fit_preemptive(indices, model_instances); + if (!successful_fitting) + break; + + SACModel &latest = model_instances.back(); + num_segmented_points += (long)latest.indices.size(); + + if (num_segmented_points == 0) + break; + + // This loop is for implementation purposes only, and maps each point to a label. + // All the points still labelled with 0 are non-segmented. + // This way, complexity of the finding non-segmented is decreased to O(n). + for(size_t i = 0; i < latest.indices.size(); i++) { + int idx = latest.indices[i]; + point_labels[idx] = label; + latest.points.push_back(cloud.at(idx)); + } + } + + // optionally, copy remaining non-segmented points + if (new_cloud.needed()) { + Mat _cloud; + for (size_t i=0; i(int(i))); + } + new_cloud.assign(_cloud); + } +} + + +void SACModelFittingImpl::set_cloud(InputArray inp_cloud, bool with_normals) { + Mat input_cloud = inp_cloud.getMat(); + + if (! with_normals) { + // normals are not required. + normals.release(); + // the cloud should have three channels. + CV_Assert(input_cloud.channels() == 3 || (input_cloud.channels() == 1 && (input_cloud.cols == 3 || input_cloud.rows == 3))); + if (input_cloud.rows == 1 && input_cloud.channels() == 3) { + cloud = input_cloud.clone(); + return; + } + + if (input_cloud.channels() != 3 && input_cloud.rows == 3) { + cloud = input_cloud.t(); + } else { + cloud = input_cloud.clone(); + } + + cloud = cloud.reshape(3, input_cloud.rows); + + } else { // with_normals + Mat _cld, _normals; + CV_Assert(input_cloud.channels() == 1 && (input_cloud.cols == 6 || input_cloud.rows == 6)); + if (input_cloud.rows == 6) { + input_cloud.rowRange(0, 3).copyTo(_cld); + input_cloud.rowRange(3, 6).copyTo(_normals); + } else { + input_cloud.colRange(0, 3).copyTo(_cld); + input_cloud.colRange(3, 6).copyTo(_normals); + } + cloud = Mat(_cld).reshape(3, 1); + normals = Mat(_normals).reshape(3, 1); + } +} + +void SACModelFittingImpl::set_model_type(int type) { + model_type = type; +} + +void SACModelFittingImpl::set_method_type(int type) { + method_type = type; +} + +void SACModelFittingImpl::set_use_sprt(bool sprt_) { + use_sprt = sprt_; +} + +void SACModelFittingImpl::set_threshold (double threshold_value) { + threshold = threshold_value; +} + +void SACModelFittingImpl::set_preemptive_count(int preemptive) { + preemptive_count = preemptive; +} + +void SACModelFittingImpl::set_max_sphere_radius(double max_radius) { + max_sphere_radius = max_radius; +} + +void SACModelFittingImpl::set_max_napsac_radius(double max_radius) { + max_napsac_radius = max_radius; +} + +void SACModelFittingImpl::set_iterations (int iterations) { + max_iters = iterations; +} + +void SACModelFittingImpl::set_min_inliers (int inliers) { + min_inliers = inliers; +} + +void SACModelFittingImpl::set_normal_distance_weight(double weight) { + if (weight > 1) { + normal_distance_weight_ = 1; + } else if (weight < 0) { + normal_distance_weight_ = 0; + } else { + normal_distance_weight_ = weight; + } +} + + +void cluster(InputArray _cloud, double distance, int min_inliers, std::vector &models, OutputArray _new_cloud) { + Mat cloud = _cloud.getMat(); + std::vector pts(cloud.begin(), cloud.end()); + + std::vector cluster_indices; + int n = cv::partition(pts, cluster_indices, [distance](const Point3f &a, const Point3f &b) -> bool{ + return norm(a-b) < distance; + }); + if (n==0) return; + + std::vector mdl(n); + for (size_t i=0; i new_cloud; + for (size_t i=0; i {c.x, c.y, c.z, b.r}; + mdl[i].type = BLOB_MODEL; + models.push_back(mdl[i]); + } + _new_cloud.assign(new_cloud); +} + +} // ptcloud +} // cv diff --git a/modules/ptcloud/src/sac_utils.cpp b/modules/ptcloud/src/sac_utils.cpp new file mode 100644 index 00000000000..94cb17cad4a --- /dev/null +++ b/modules/ptcloud/src/sac_utils.cpp @@ -0,0 +1,61 @@ +#include "opencv2/ptcloud/sac_utils.hpp" + + +namespace cv { +namespace ptcloud { + + +// neither of a,b,c may be 0 ! +void generatePlane(Mat &cloud, const std::vector &coeffs, int N) { + int n=(int)sqrt(N); + for (int i=0; i &coeffs, int N) { + for (int i=0; i &coeffs, int N) { + double R=20; + Point3d cen(coeffs[0],coeffs[1],coeffs[2]); + Point3d dir(coeffs[3],coeffs[4],coeffs[5]); + for (int i=0; i &coeffs, int N) { + float R = float(coeffs[3]); + for (int i=0; i // std::iota + +#include "opencv2/ts.hpp" +#include "opencv2/surface_matching/ppf_helpers.hpp" // normals + +#include "opencv2/ptcloud/sac_segmentation.hpp" +#include "opencv2/ptcloud/sac_utils.hpp" + + +#endif diff --git a/modules/ptcloud/test/test_sac.cpp b/modules/ptcloud/test/test_sac.cpp new file mode 100644 index 00000000000..95a2355d626 --- /dev/null +++ b/modules/ptcloud/test/test_sac.cpp @@ -0,0 +1,259 @@ +// This file is part of the OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html. + +#include "test_precomp.hpp" + + +namespace opencv_test { namespace { + +using namespace cv::ptcloud; + +struct BaseModel { + std::vector coefs; + std::vector indices; + Mat_ cloud; + int NPOINTS = 128; + double THRESHOLD = 0.001; + + void generateIndices() { + indices.resize(cloud.total()); + std::iota(indices.begin(),indices.end(),0); + } +}; + +struct PlaneModel : public BaseModel { + PlaneModel() { + NPOINTS = 64; + coefs = std::vector{0.98, 0.13, 0.13, -10.829}; + generatePlane(cloud, coefs, NPOINTS); + generateIndices(); + } +}; + +struct SphereModel : public BaseModel { + SphereModel() { + coefs = std::vector{-8, -4, 10, 5.0}; + generateSphere(cloud, coefs, NPOINTS); + generateIndices(); + } +}; + +struct CylinderModel : public BaseModel { + Point3f dir; + CylinderModel() { + coefs = std::vector{-10,-20,3, 0,0,1, 5.0}; + dir = Point3f(float(coefs[3]),float(coefs[4]),float(coefs[5])); + generateCylinder(cloud, coefs, NPOINTS); + generateIndices(); + } +}; + +PlaneModel plane; +SphereModel sphere; +CylinderModel cylinder; + +// +// Part 1, check the internal building blocks: +// coefficient generation, inliers, optimization +// + +TEST(SAC, plane) +{ + // 1. see if we get our coefficients back + vector new_coefs; + std::vector hypothesis_inliers {1,8,11}; + bool valid_model = generateHypothesis(PLANE_MODEL, plane.cloud, hypothesis_inliers, Mat(), 50, 0, new_coefs); + EXPECT_TRUE(valid_model); + Point3d dir1(new_coefs[0],new_coefs[1],new_coefs[2]); + Point3d dir2(plane.coefs[0],plane.coefs[1],plane.coefs[2]); + double d2 = dir1.dot(dir2); // check if they are parallel + EXPECT_GE(abs(d2), 0.95); + EXPECT_LE(abs(abs(plane.coefs[3]) - abs(new_coefs[3])), 0.1); // length + + // 2. all plane points should be inliers + std::vector new_indices; + SACScore score = getInliers(PLANE_MODEL, plane.coefs, plane.cloud, Mat(), plane.indices, plane.THRESHOLD, 0, new_indices, 0); + EXPECT_EQ((int)score.first, plane.NPOINTS); + EXPECT_LE(score.second, double(plane.THRESHOLD * plane.NPOINTS)); + + // 3. no sphere points should be inliers + score = getInliers(PLANE_MODEL, plane.coefs, sphere.cloud, Mat(), sphere.indices, plane.THRESHOLD, 0, new_indices, 0); + EXPECT_EQ(score.first, 0.0); + + // 4. no cylinder points should be inliers + score = getInliers(PLANE_MODEL, plane.coefs, cylinder.cloud, Mat(), cylinder.indices, plane.THRESHOLD, 0, new_indices, 0); + EXPECT_EQ(score.first, 0.0); + + // 5. check optimization + new_coefs = optimizeModel(PLANE_MODEL, plane.cloud, plane.indices, plane.coefs); + Point3d dir3(new_coefs[0],new_coefs[1],new_coefs[2]); + double d3 = dir3.dot(dir2); // check if they are parallel + EXPECT_GE(abs(d3), 0.95); + EXPECT_LE(abs(abs(plane.coefs[3]) - abs(new_coefs[3])), 0.1); // length +} + +TEST(SAC, sphere) +{ + // 1. see if we get our coefficients back + vector new_coefs; + std::vector hypothesis_inliers {1,8,11,22}; + bool valid_model = generateHypothesis(SPHERE_MODEL, sphere.cloud, hypothesis_inliers, Mat(), 50, 0, new_coefs); + double d = cv::norm(Mat(sphere.coefs),Mat(new_coefs)); + EXPECT_TRUE(valid_model); + EXPECT_LE(d, 0.05); + + // 2. all sphere points should be inliers + std::vector new_indices; + SACScore score = getInliers(SPHERE_MODEL, sphere.coefs, sphere.cloud, Mat(), sphere.indices, sphere.THRESHOLD, 0, new_indices, 0); + EXPECT_EQ((int)score.first, sphere.NPOINTS); + EXPECT_LE(score.second, double(sphere.THRESHOLD * sphere.NPOINTS)); + + // 3. no plane points should be inliers + score = getInliers(SPHERE_MODEL, sphere.coefs, plane.cloud, Mat(), plane.indices, sphere.THRESHOLD, 0, new_indices, 0); + EXPECT_EQ(score.first, 0.0); + + // 4. no cylinder points should be inliers + score = getInliers(SPHERE_MODEL, sphere.coefs, cylinder.cloud, Mat(), cylinder.indices, sphere.THRESHOLD, 0, new_indices, 0); + EXPECT_EQ(score.first, 0.0); + + // 5. check optimization + new_coefs = optimizeModel(SPHERE_MODEL, sphere.cloud, sphere.indices, sphere.coefs); + double d1 = cv::norm(Mat(sphere.coefs),Mat(new_coefs)); + EXPECT_LE(d1, 0.05); +} + +TEST(SAC, cylinder) +{ + // 1. see if we get our coefficients back + Mat normals = generateNormals(cylinder.cloud); + vector new_coefs; + std::vector hypothesis_inliers {1,22}; + bool valid_model = generateHypothesis(CYLINDER_MODEL, cylinder.cloud, hypothesis_inliers, normals, 50, 0.5, new_coefs); + EXPECT_TRUE(valid_model); + // any point on the axis is actually valid, so we can only check the dir and the radius + Point3d dir(new_coefs[3],new_coefs[4],new_coefs[5]); + double d = cylinder.dir.dot(dir); // check if they are parallel + EXPECT_GE(abs(d), 0.95); + EXPECT_LE(abs(abs(cylinder.coefs[6]) - abs(new_coefs[6])), 2.0); // radius + + // 2. all cylinder points should be inliers + std::vector new_indices; + SACScore score = getInliers(CYLINDER_MODEL, cylinder.coefs, cylinder.cloud, Mat(), cylinder.indices, cylinder.THRESHOLD, 0, new_indices, 0); + EXPECT_EQ((int)score.first, cylinder.NPOINTS); + EXPECT_LE(score.second, double(cylinder.THRESHOLD * cylinder.NPOINTS)); + + // 3. no plane points should be inliers + score = getInliers(CYLINDER_MODEL, cylinder.coefs, plane.cloud, Mat(), plane.indices, cylinder.THRESHOLD, 0, new_indices, 0); + EXPECT_EQ(score.first, 0); + + // 4. no sphere points should be inliers + score = getInliers(CYLINDER_MODEL, cylinder.coefs, sphere.cloud, Mat(), sphere.indices, cylinder.THRESHOLD, 0, new_indices, 0); + EXPECT_EQ(score.first, 0.0); + + // 5. check optimization + new_coefs = optimizeModel(CYLINDER_MODEL, cylinder.cloud, cylinder.indices, cylinder.coefs); + Point3d dir2(new_coefs[3],new_coefs[4],new_coefs[5]); + double d2 = cylinder.dir.dot(dir2); // check if they are parallel + EXPECT_GE(abs(d2), 0.95); + EXPECT_LE(abs(abs(cylinder.coefs[6]) - abs(new_coefs[6])), 2.0); // radius +} + + +// +// Part 2, check the segmentation: +// + +Mat generateScene() +{ + Mat_ pts; + pts.push_back(plane.cloud); + pts.push_back(sphere.cloud); + pts.push_back(cylinder.cloud); + theRNG().state=99999999; + generateRandom(pts, vector{-12,31,-5, 5}, 32); // these 2 form distinct blobs + generateRandom(pts, vector{12,-21,15, 5}, 32); // that can be retrieved via clustering + generateRandom(pts, vector{1,-2,1, 25}, 64); // general noisy outliers + return Mat(pts); +} + +TEST(SAC, segment_ransac) +{ + Mat cloud = generateScene(); + std::vector models; + Ptr fitting = SACModelFitting::create(cloud); + fitting->set_threshold(plane.THRESHOLD); + fitting->segment(models); + EXPECT_EQ(models.size(), size_t(1)); + EXPECT_EQ(models[0].indices.size(), (size_t)plane.NPOINTS); + double d1 = cv::norm(Mat(plane.coefs), Mat(models[0].coefficients)); + EXPECT_LE(d1, 0.05); + + fitting->set_model_type(SPHERE_MODEL); + fitting->set_threshold(sphere.THRESHOLD); + fitting->segment(models); + EXPECT_EQ(models.size(), size_t(2)); + EXPECT_EQ(models[1].indices.size(), (size_t)sphere.NPOINTS); + double d2 = cv::norm(Mat(sphere.coefs), Mat(models[1].coefficients)); + EXPECT_LE(d2, 0.05); + + fitting->set_model_type(CYLINDER_MODEL); + fitting->set_threshold(2.62); + fitting->set_normal_distance_weight(0.5); + fitting->set_min_inliers(int(cylinder.NPOINTS/2)); + fitting->segment(models); + EXPECT_EQ(models.size(), size_t(3)); + EXPECT_GE(models[2].indices.size(), size_t(cylinder.NPOINTS*0.5)); + Point3d dir(models[2].coefficients[3],models[2].coefficients[4],models[2].coefficients[5]); + double d3 = cylinder.dir.dot(dir); // check if they are parallel + EXPECT_GE(abs(d3), 0.65); + EXPECT_LE(abs(cylinder.coefs[6] - models[2].coefficients[6]), 0.6); // radius +} + +TEST(SAC, segment_preemptive) +{ + Mat cloud = generateScene(); + std::vector models; + Ptr fitting = SACModelFitting::create(cloud); + fitting->set_threshold(plane.THRESHOLD); + fitting->set_preemptive_count(500); + fitting->segment(models); + EXPECT_EQ(models.size(), size_t(1)); + EXPECT_EQ(models[0].indices.size(), (size_t)plane.NPOINTS); + + fitting->set_model_type(SPHERE_MODEL); + fitting->set_threshold(sphere.THRESHOLD); + fitting->segment(models); + EXPECT_EQ(models.size(), size_t(2)); + EXPECT_EQ(models[1].indices.size(), (size_t)sphere.NPOINTS); + double d2 = cv::norm(Mat(sphere.coefs), Mat(models[1].coefficients)); + EXPECT_LE(d2, 0.05); + + fitting->set_model_type(CYLINDER_MODEL); + fitting->set_threshold(3.62); + fitting->set_normal_distance_weight(0.5); + fitting->set_min_inliers(90); + fitting->segment(models); + EXPECT_EQ(models.size(), size_t(3)); + EXPECT_GE(models[2].indices.size(), size_t(cylinder.NPOINTS*0.7)); + Point3d dir(models[2].coefficients[3],models[2].coefficients[4],models[2].coefficients[5]); + double d3 = cylinder.dir.dot(dir); // check if they are parallel + EXPECT_GE(abs(d3), 0.65); + EXPECT_LE(abs(abs(cylinder.coefs[6]) - abs(models[2].coefficients[6])), 0.6); // radius +} + +TEST(SAC, cluster) +{ + Mat cloud = generateScene(); + cv::ppf_match_3d::writePLY(Mat(cloud), "cloud.ply"); + std::vector models; + Mat new_cloud; + cluster(cloud, 5, 20, models, new_cloud); + EXPECT_EQ(models.size(), size_t(4)); // sphere, cylinder, 2 blobs + // the plane needs a larger distance, so do a 2nd pass on the leftover points + cluster(new_cloud, 8, 20, models, new_cloud); + EXPECT_EQ(models.size(), size_t(5)); + EXPECT_LE(new_cloud.total(), size_t(64)); // the last large random blob +} + +}} // namespace