Merge 5eea165123 into ce3c6681c9
Devansh Batra
2 months ago
committed by
GitHub
commit
46de410b19
15 changed files with 1393 additions and 0 deletions
@ -0,0 +1,9 @@ |
||||
set(the_description "Point Cloud Object Fitting API") |
||||
ocv_define_module(ptcloud opencv_core opencv_highgui opencv_viz opencv_surface_matching WRAP python) |
||||
|
||||
# add test data from samples dir to contrib/ptcloud |
||||
ocv_add_testdata(samples/ contrib/ptcloud FILES_MATCHING PATTERN "*.ply") |
||||
|
||||
# add data point cloud files to installation |
||||
file(GLOB POINTCLOUD_DATA samples/*.ply) |
||||
install(FILES ${POINTCLOUD_DATA} DESTINATION ${OPENCV_OTHER_INSTALL_PATH}/ptcloud COMPONENT libs) |
||||
@ -0,0 +1,14 @@ |
||||
//! @addtogroup ptcloud |
||||
//! @{ |
||||
|
||||
Point Cloud Module, Object Fitting API |
||||
======================================= |
||||
|
||||
|
||||
To Do |
||||
----------------------------------------- |
||||
- Cylinder Model Fitting |
||||
- Segmentation |
||||
- Integrate with Maksym's work |
||||
|
||||
//! @} |
||||
@ -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 |
||||
@ -0,0 +1,259 @@ |
||||
// This file is part of OpenCV project.
|
||||
// It is subject to the license terms in the LICENSE file found in the top-level directory
|
||||
// of this distribution and at http://opencv.org/license.html.
|
||||
|
||||
#ifndef OPENCV_PTCLOUD_SAC_SEGMENTATION |
||||
#define OPENCV_PTCLOUD_SAC_SEGMENTATION |
||||
|
||||
#include <vector> |
||||
#include <utility> |
||||
#include "opencv2/viz.hpp" |
||||
#define PLANE_MODEL 1 |
||||
#define SPHERE_MODEL 2 |
||||
#define CYLINDER_MODEL 3 |
||||
#define SAC_METHOD_RANSAC 1 |
||||
using namespace std; |
||||
|
||||
namespace cv |
||||
{ |
||||
namespace ptcloud |
||||
{ |
||||
//! @addtogroup ptcloud
|
||||
//! @{
|
||||
class CV_EXPORTS_W SACModel: public Algorithm { |
||||
public: |
||||
std::vector<double> ModelCoefficients; |
||||
|
||||
SACModel() { |
||||
|
||||
} |
||||
|
||||
SACModel(std::vector<double> ModelCoefficients); |
||||
|
||||
virtual ~SACModel() |
||||
{ |
||||
|
||||
} |
||||
|
||||
}; |
||||
|
||||
class CV_EXPORTS_W SACPlaneModel : public SACModel { |
||||
private: |
||||
Point3d center; |
||||
Vec3d normal; |
||||
Size2d size = Size2d(2.0, 2.0); |
||||
public: |
||||
~ SACPlaneModel() |
||||
{ |
||||
|
||||
} |
||||
|
||||
SACPlaneModel() { |
||||
|
||||
} |
||||
|
||||
/** @brief Create a plane model based on the given coefficients and a center point.
|
||||
|
||||
@param coefficients coefficients in the plane equations of type Ax + By + Cz + D = 0. Also obtained using SACModelFitting. |
||||
@param center the center point of the plane. |
||||
@param size the size of the plane. |
||||
*/ |
||||
|
||||
SACPlaneModel(Vec4d coefficients, Point3d center, Size2d size=Size2d(2.0, 2.0)); |
||||
|
||||
/** @brief Create a plane model based on the given coefficients and an arbitrary center point.
|
||||
|
||||
@param coefficients coefficients in the plane equations Ax + By + Cz + D = 0. |
||||
@param size the size of the plane. |
||||
*/ |
||||
SACPlaneModel(Vec4d coefficients, Size2d size=Size2d(2.0, 2.0)); |
||||
|
||||
/** @brief Create a plane model based on the given coefficients and an arbitrary center point.
|
||||
|
||||
@param coefficients coefficients in the plane equations Ax + By + Cz + D = 0. |
||||
@param size the size of the plane. |
||||
*/ |
||||
SACPlaneModel(std::vector<double> coefficients, Size2d size=Size2d(2.0, 2.0)); |
||||
|
||||
viz::WPlane WindowWidget (); |
||||
|
||||
std::pair<double, double> getInliers(Mat cloud, std::vector<unsigned> indices, const double threshold, std::vector<unsigned>& inliers); |
||||
}; |
||||
|
||||
|
||||
class CV_EXPORTS_W SACSphereModel : public SACModel { |
||||
public: |
||||
Point3d center; |
||||
double radius; |
||||
|
||||
~ SACSphereModel() |
||||
{ |
||||
|
||||
} |
||||
|
||||
SACSphereModel() { |
||||
|
||||
} |
||||
|
||||
/** @brief Create a spherical model based on the given center and radius.
|
||||
|
||||
@param center the center point of the sphere |
||||
@param radius the radius of the sphere. |
||||
*/ |
||||
|
||||
SACSphereModel(Point3d center, double radius); |
||||
|
||||
/** @brief Create a spherical model based on the parametric coefficients.
|
||||
|
||||
This is very helpful for creating a model for the fit models using SACModelFitting class. |
||||
|
||||
@param Coefficients parametric coefficients for the Sphere model |
||||
*/ |
||||
|
||||
SACSphereModel(const std::vector<double> Coefficients); |
||||
|
||||
SACSphereModel(Vec4d coefficients); |
||||
|
||||
viz::WSphere WindowWidget (); |
||||
|
||||
double euclideanDist(Point3d& p, Point3d& q); |
||||
|
||||
std::pair<double, double> getInliers(Mat cloud, std::vector<unsigned> indices, const double threshold, std::vector<unsigned>& inliers); |
||||
}; |
||||
|
||||
class CV_EXPORTS_W SACCylinderModel : public SACModel { |
||||
public: |
||||
Point3d pt_on_axis; |
||||
Vec3d axis_dir; |
||||
double radius; |
||||
double size = 20; |
||||
|
||||
~ SACCylinderModel() |
||||
{ |
||||
|
||||
} |
||||
|
||||
SACCylinderModel() { |
||||
|
||||
} |
||||
|
||||
// /** @brief Create a spherical model based on the given center and radius.
|
||||
|
||||
// @param center the center point of the sphere
|
||||
// @param radius the radius of the sphere.
|
||||
// */
|
||||
|
||||
// SACCylinderModel(const std::vector<double> Coefficients);
|
||||
|
||||
/** @brief Create a spherical model based on the parametric coefficients.
|
||||
|
||||
This is very helpful for creating a model for the fit models using SACModelFitting class. |
||||
|
||||
@param Coefficients parametric coefficients for the Sphere model |
||||
*/ |
||||
|
||||
SACCylinderModel(const std::vector<double> Coefficients); |
||||
|
||||
viz::WCylinder WindowWidget (); |
||||
|
||||
std::pair<double, double> getInliers(Mat cloud, Mat normals, std::vector<unsigned> indices, const double threshold, std::vector<unsigned>& inliers, double normal_distance_weight_ = 0); |
||||
}; |
||||
|
||||
class CV_EXPORTS_W SACModelFitting { |
||||
private: |
||||
Mat cloud; |
||||
Mat normals; |
||||
bool normals_available = false; |
||||
int model_type; |
||||
int method_type; |
||||
double threshold; |
||||
long unsigned max_iters; |
||||
double normal_distance_weight_ = 0; |
||||
|
||||
public: |
||||
// cv::Mat remainingCloud; // will be used while segmentation
|
||||
|
||||
// Inlier indices only, not the points themselves. It would work like a mask output for segmentation in 2d.
|
||||
vector<vector<unsigned>> inliers; |
||||
vector<SACModel> model_instances; |
||||
|
||||
/** @brief Initializes SACModelFitting class.
|
||||
|
||||
Threshold and Iterations may also be set separately. |
||||
|
||||
@param cloud input Point Cloud. |
||||
@param model_type type of model fitting to attempt - values can be either PLANE_MODEL, SPHERE_MODEL, or CYLINDER_MODEL. |
||||
@param method_type which method to use - currently, only RANSAC is supported (use value SAC_METHOD_RANSAC). |
||||
@param threshold set the threshold while choosing inliers. |
||||
@param max_iters number of iterations for Sampling. |
||||
*/ |
||||
|
||||
SACModelFitting (Mat cloud, int model_type = PLANE_MODEL, int method_type = SAC_METHOD_RANSAC, double threshold = 20,int max_iters = 1000); |
||||
// :cloud(cloud), model_type(model_type), method_type(method_type), threshold(threshold), max_iters(max_iters) {}
|
||||
|
||||
/** @brief Initializes SACModelFitting class.
|
||||
|
||||
Threshold and Iterations may also be set separately. |
||||
|
||||
@param model_type type of model fitting to attempt - values can be either PLANE_MODEL, SPHERE_MODEL, or CYLINDER_MODEL. |
||||
@param method_type which method to use - currently, only RANSAC is supported (use value SAC_METHOD_RANSAC). |
||||
@param threshold set the threshold while choosing inliers. |
||||
@param max_iters number of iterations for Sampling. |
||||
*/ |
||||
SACModelFitting (int model_type = PLANE_MODEL, int method_type = SAC_METHOD_RANSAC, double threshold = 20,int max_iters = 1000); |
||||
// :model_type(model_type), method_type(method_type), threshold(threshold), max_iters(max_iters) {}
|
||||
|
||||
/** @brief Fit one model, this function would get the best fitting model on the given set of points.
|
||||
|
||||
This stores the model in the public class member model_instances, and the mask for inliers in inliers. |
||||
*/ |
||||
bool fit_once(vector<int> remaining_indices = {}); |
||||
|
||||
/** @brief Fit multiple models of the same type, this function would get the best fitting models on the given set of points.
|
||||
|
||||
This stores the models in the public class member model_instances, and the corresponding masks for inliers in inliers. |
||||
|
||||
Returns False if no valid model could be fit. |
||||
|
||||
@param remaining_cloud_threshold set the threshold for the remaining cloud (from 0 to 1) until which the segmentation should continue. |
||||
*/ |
||||
void segment(float remaining_cloud_threshold = 0.3); |
||||
|
||||
void setCloud(Mat cloud); |
||||
|
||||
void setCloud(Mat cloud, bool with_normals=false); |
||||
|
||||
/** @brief Set the threshold for the fitting.
|
||||
The threshold is usually the distance from the boundary of model, but may vary from model to model. |
||||
|
||||
This may be helpful when multiple fitting operations are to be performed. |
||||
@param threshold the threshold to set. |
||||
*/ |
||||
void set_threshold (double threshold); |
||||
|
||||
/** @brief Set the number of iterations for the fitting.
|
||||
|
||||
This may be helpful when multiple fitting operations are to be performed. |
||||
@param iterations the threshold to set. |
||||
*/ |
||||
void set_iterations (long unsigned iterations); |
||||
|
||||
/** @brief Set the weight given to normal alignment before comparing overall error with threshold.
|
||||
* By default it is set to 0. |
||||
@param weight the desired normal alignment weight (between 0 to 1). |
||||
*/ |
||||
void set_normal_distance_weight(double weight); |
||||
}; |
||||
|
||||
bool getSphereFromPoints(const Vec3f*&, const vector<unsigned int>&, Point3d&, double&); |
||||
|
||||
Vec4d getPlaneFromPoints(const Vec3f*&, const std::vector<unsigned int>&, cv::Point3d&); |
||||
|
||||
bool getCylinderFromPoints(Mat cloud, Mat normal, |
||||
const std::vector<unsigned> &inliers, vector<double> & coefficients) ; |
||||
|
||||
double euclideanDist(Point3d& p, Point3d& q); |
||||
|
||||
} // ptcloud
|
||||
} // cv
|
||||
#endif |
||||
@ -0,0 +1,68 @@ |
||||
#include <opencv2/viz.hpp> |
||||
#include <opencv2/highgui.hpp> |
||||
#include <opencv2/viz/widgets.hpp> |
||||
#include <opencv2/ptcloud.hpp> |
||||
#include "opencv2/surface_matching.hpp" |
||||
#include "opencv2/surface_matching/ppf_helpers.hpp" |
||||
#include <cassert> |
||||
#include <numeric> |
||||
#include <cmath> |
||||
#include <iostream> |
||||
#include <string> |
||||
|
||||
using namespace cv; |
||||
using namespace std; |
||||
|
||||
int main() { |
||||
// Mat cloud = cv::ppf_match_3d::loadPLYSimple("./data/semi-cylinder-with-normals-usingOpenCV2.ply", true);
|
||||
Mat cloud = cv::ppf_match_3d::loadPLYSimple("./data/cylinder-big.ply", false); |
||||
Mat ptset; |
||||
Mat(cloud.colRange(0,3)).copyTo(ptset); |
||||
long unsigned num_points = ptset.rows; |
||||
ptset = ptset.reshape(3, num_points); |
||||
ptset = ptset.t(); |
||||
|
||||
cv::ptcloud::SACModelFitting cylinder_segmentation(CYLINDER_MODEL); |
||||
cylinder_segmentation.setCloud(cloud, false); |
||||
|
||||
// add original cloud to window
|
||||
viz::Viz3d window("original cloud"); |
||||
viz::WCloud original_cloud(ptset); |
||||
window.showWidget("cloud", original_cloud); |
||||
|
||||
cylinder_segmentation.set_threshold(0.5); |
||||
cylinder_segmentation.set_iterations(80000); |
||||
cylinder_segmentation.set_normal_distance_weight(0.5); |
||||
cylinder_segmentation.fit_once(); |
||||
|
||||
cout << cylinder_segmentation.inliers.size(); |
||||
vector<unsigned> inlier_vec = cylinder_segmentation.inliers.at(0); |
||||
|
||||
|
||||
vector<double> model_coefficients = cylinder_segmentation.model_instances.at(0).ModelCoefficients; |
||||
cout << cylinder_segmentation.model_instances.at(0).ModelCoefficients.size(); |
||||
cv::ptcloud::SACCylinderModel cylinder (model_coefficients); |
||||
cout << cylinder.pt_on_axis << endl; |
||||
cout << cylinder.axis_dir << endl; |
||||
cout << cylinder.radius << endl; |
||||
|
||||
viz::WCylinder model = cylinder.WindowWidget(); |
||||
window.showWidget("model", model); |
||||
|
||||
const Vec3f* points = ptset.ptr<Vec3f>(0); |
||||
cout << endl << endl << inlier_vec.size(); |
||||
cv::Mat fit_cloud(1, inlier_vec.size(), CV_32FC3); |
||||
for(int j=0; j<fit_cloud.cols; ++j){ |
||||
fit_cloud.at<Vec3f>(0, j) = points[(j)]; |
||||
} |
||||
viz::Viz3d fitted("fitted cloud"); |
||||
viz::WCloud cloud_widget2(fit_cloud, viz::Color::red()); |
||||
fitted.showWidget("fit_cloud", cloud_widget2); |
||||
window.showWidget("fit_cloud", cloud_widget2); |
||||
fitted.spin();
|
||||
|
||||
|
||||
window.spin(); |
||||
waitKey(1); |
||||
|
||||
} |
||||
@ -0,0 +1,48 @@ |
||||
#include <opencv2/viz.hpp> |
||||
#include <opencv2/highgui.hpp> |
||||
#include <opencv2/viz/widgets.hpp> |
||||
#include <opencv2/ptcloud.hpp> |
||||
#include <cassert> |
||||
#include <numeric> |
||||
#include <cmath> |
||||
#include <string> |
||||
|
||||
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<Vec3f>(0); |
||||
vector<unsigned> inlier_vec = planar_segmentation.inliers.at(0); |
||||
cv::Mat fit_cloud(1, inlier_vec.size(), CV_32FC3); |
||||
for(int j=0; j<fit_cloud.cols; ++j) |
||||
fit_cloud.at<Vec3f>(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<double> model_coefficients = planar_segmentation.model_instances.at(0).ModelCoefficients; |
||||
cv::ptcloud::SACPlaneModel SACplane (model_coefficients); |
||||
|
||||
window.spin(); |
||||
fitted.spin(); |
||||
waitKey(1); |
||||
|
||||
} |
||||
@ -0,0 +1,47 @@ |
||||
#include <opencv2/viz.hpp> |
||||
#include <opencv2/highgui.hpp> |
||||
#include <opencv2/viz/widgets.hpp> |
||||
#include <opencv2/ptcloud.hpp> |
||||
#include <cassert> |
||||
#include <numeric> |
||||
#include <cmath> |
||||
#include <string> |
||||
|
||||
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<Vec3f>(0); |
||||
vector<unsigned> inlier_vec = planar_segmentation.inliers.at(0); |
||||
cv::Mat fit_cloud(1, inlier_vec.size(), CV_32FC3); |
||||
for(int j=0; j<fit_cloud.cols; ++j) |
||||
fit_cloud.at<Vec3f>(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<double> model_coefficients = planar_segmentation.model_instances.at(0).ModelCoefficients; |
||||
cv::ptcloud::SACPlaneModel SACplane (model_coefficients); |
||||
|
||||
window.spin(); |
||||
fitted.spin(); |
||||
waitKey(1); |
||||
|
||||
} |
||||
@ -0,0 +1,60 @@ |
||||
#include <opencv2/viz.hpp> |
||||
#include <opencv2/highgui.hpp> |
||||
#include <opencv2/viz/widgets.hpp> |
||||
#include <opencv2/ptcloud.hpp> |
||||
#include <cassert> |
||||
#include <numeric> |
||||
#include <cmath> |
||||
#include <string> |
||||
|
||||
using namespace cv; |
||||
using namespace std; |
||||
|
||||
int main() { |
||||
Mat cloud = cv::viz::readCloud("./data/CobbleStones.obj"); |
||||
|
||||
cv::ptcloud::SACModelFitting planar_segmentation(cloud); |
||||
|
||||
// add original cloud to window
|
||||
viz::Viz3d window("original cloud"); |
||||
viz::WCloud original_cloud(cloud); |
||||
window.showWidget("cloud", original_cloud); |
||||
|
||||
|
||||
planar_segmentation.set_threshold(0.001); |
||||
planar_segmentation.set_iterations(1000); |
||||
planar_segmentation.segment(); |
||||
|
||||
|
||||
const Vec3f* points = cloud.ptr<Vec3f>(0); |
||||
|
||||
// Initialise a colors array. These colors will be used (in a cyclic order) to visualise all the segmented planes.
|
||||
const vector<viz::Color> colors({viz::Color::green(), viz::Color::blue(), viz::Color::red(), viz::Color::yellow(), viz::Color::orange(),viz::Color::olive()}); |
||||
|
||||
// Adds segmented planes to window
|
||||
for (unsigned model_idx = 0; model_idx < planar_segmentation.inliers.size(); model_idx++) { |
||||
vector<unsigned> inlier_vec = planar_segmentation.inliers.at(model_idx); |
||||
cv::Mat fit_cloud(1, inlier_vec.size(), CV_32FC3); |
||||
for(int j=0; j<fit_cloud.cols; ++j) |
||||
fit_cloud.at<Vec3f>(0, j) = points[inlier_vec.at(j)]; |
||||
|
||||
viz::Viz3d fitted("fit cloud " + to_string(model_idx + 1)); |
||||
fitted.showWidget("cloud", original_cloud); |
||||
|
||||
// Assign a color to this cloud from the colors array in a cyclic order.
|
||||
viz::Color cloud_color = colors[model_idx % colors.size()]; |
||||
viz::WCloud cloud_widget2(fit_cloud, cloud_color); |
||||
fitted.showWidget("fit plane", cloud_widget2); |
||||
window.showWidget("fit plane " + to_string(model_idx + 1), cloud_widget2); |
||||
|
||||
vector<double> model_coefficients = planar_segmentation.model_instances.at(0).ModelCoefficients; |
||||
cv::ptcloud::SACPlaneModel SACplane (model_coefficients); |
||||
|
||||
fitted.spin(); |
||||
} |
||||
|
||||
window.spin(); |
||||
|
||||
// waitKey(1);
|
||||
|
||||
} |
||||
@ -0,0 +1,42 @@ |
||||
#include <opencv2/viz.hpp> |
||||
#include <opencv2/highgui.hpp> |
||||
#include <opencv2/viz/widgets.hpp> |
||||
#include <opencv2/ptcloud.hpp> |
||||
#include <cassert> |
||||
#include <numeric> |
||||
#include <cmath> |
||||
#include <iostream> |
||||
#include <string> |
||||
|
||||
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<double> 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); |
||||
|
||||
} |
||||
@ -0,0 +1,42 @@ |
||||
#include <opencv2/viz.hpp> |
||||
#include <opencv2/highgui.hpp> |
||||
#include <opencv2/viz/widgets.hpp> |
||||
#include <opencv2/ptcloud.hpp> |
||||
#include <cassert> |
||||
#include <numeric> |
||||
#include <cmath> |
||||
#include <iostream> |
||||
#include <string> |
||||
|
||||
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<double> 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); |
||||
|
||||
} |
||||
@ -0,0 +1,14 @@ |
||||
// This file is part of OpenCV project.
|
||||
// It is subject to the license terms in the LICENSE file found in the top-level directory
|
||||
// of this distribution and at http://opencv.org/license.html.
|
||||
#ifndef OPENCV_PTCLOUD_PRECOMP_HPP |
||||
#define OPENCV_PTCLOUD_PRECOMP_HPP |
||||
#include <opencv2/core.hpp> |
||||
#include "opencv2/ptcloud/sac_segmentation.hpp" |
||||
|
||||
#include <iostream> |
||||
#include <stdio.h> |
||||
#include <vector> |
||||
#include <algorithm> |
||||
|
||||
#endif |
||||
@ -0,0 +1,756 @@ |
||||
// This file is part of OpenCV project.
|
||||
// It is subject to the license terms in the LICENSE file found in the top-level directory
|
||||
// of this distribution and at http://opencv.org/license.html.
|
||||
|
||||
#include "precomp.hpp" |
||||
#include <cassert> |
||||
#include "opencv2/surface_matching.hpp" |
||||
#include "opencv2/surface_matching/ppf_helpers.hpp" |
||||
#include <numeric> |
||||
#include <limits> |
||||
|
||||
|
||||
using namespace std; |
||||
using namespace cv; |
||||
|
||||
namespace cv |
||||
{ |
||||
namespace ptcloud |
||||
{ |
||||
|
||||
bool getSphereFromPoints(const Vec3f* &points, const std::vector<unsigned> &inliers, Point3d& center, double& radius) { |
||||
// assert that size of points is 3.
|
||||
Mat temp(5,5,CV_32FC1); |
||||
// Vec4f temp;
|
||||
for(int i = 0; i < 4; i++) |
||||
{ |
||||
unsigned point_idx = inliers[i]; |
||||
float* tempi = temp.ptr<float>(i); |
||||
for(int j = 0; j < 3; j++) { |
||||
tempi[j] = (float) points[point_idx][j]; |
||||
} |
||||
tempi[3] = 1; |
||||
} |
||||
double m11 = determinant(temp); |
||||
if (m11 == 0) return false; // no sphere exists
|
||||
|
||||
for(int i = 0; i < 4; i++) |
||||
{ |
||||
unsigned point_idx = inliers[i]; |
||||
float* tempi = temp.ptr<float>(i); |
||||
|
||||
tempi[0] = (float) points[point_idx][0] * (float) points[point_idx][0] |
||||
+ (float) points[point_idx][1] * (float) points[point_idx][1] |
||||
+ (float) points[point_idx][2] * (float) points[point_idx][2]; |
||||
|
||||
} |
||||
double m12 = determinant(temp); |
||||
|
||||
for(int i = 0; i < 4; i++) |
||||
{ |
||||
unsigned point_idx = inliers[i]; |
||||
float* tempi = temp.ptr<float>(i); |
||||
|
||||
tempi[1] = tempi[0]; |
||||
tempi[0] = (float) points[point_idx][0]; |
||||
|
||||
} |
||||
double m13 = determinant(temp); |
||||
|
||||
for(int i = 0; i < 4; i++) |
||||
{ |
||||
unsigned point_idx = inliers[i]; |
||||
float* tempi = temp.ptr<float>(i); |
||||
|
||||
tempi[2] = tempi[1]; |
||||
tempi[1] = (float) points[point_idx][1]; |
||||
|
||||
} |
||||
double m14 = determinant(temp); |
||||
|
||||
for(int i = 0; i < 4; i++) |
||||
{ |
||||
unsigned point_idx = inliers[i]; |
||||
float* tempi = temp.ptr<float>(i); |
||||
|
||||
tempi[0] = tempi[2]; |
||||
tempi[1] = (float) points[point_idx][0]; |
||||
tempi[2] = (float) points[point_idx][1]; |
||||
tempi[3] = (float) points[point_idx][2]; |
||||
} |
||||
double m15 = determinant(temp); |
||||
|
||||
center.x = 0.5 * m12 / m11; |
||||
center.y = 0.5 * m13 / m11; |
||||
center.z = 0.5 * m14 / m11; |
||||
// Radius
|
||||
radius = std::sqrt (center.x * center.x + |
||||
center.y * center.y + |
||||
center.z * center.z - m15 / m11); |
||||
|
||||
return (true); |
||||
|
||||
} |
||||
|
||||
Vec4d getPlaneFromPoints(const Vec3f* &points, |
||||
const std::vector<unsigned> &inliers, Point3d& center) { |
||||
// REF: https://www.ilikebigbits.com/2015_03_04_plane_from_points.html
|
||||
Vec3f centroid(0, 0, 0); |
||||
for (unsigned idx : inliers) { |
||||
centroid += points[idx]; |
||||
} |
||||
centroid /= double(inliers.size()); |
||||
|
||||
double xx = 0, xy = 0, xz = 0, yy = 0, yz = 0, zz = 0; |
||||
|
||||
for (size_t idx : inliers) { |
||||
Vec3f r = points[idx] - centroid; |
||||
xx += r(0) * r(0); |
||||
xy += r(0) * r(1); |
||||
xz += r(0) * r(2); |
||||
yy += r(1) * r(1); |
||||
yz += r(1) * r(2); |
||||
zz += r(2) * r(2); |
||||
} |
||||
|
||||
double det_x = yy * zz - yz * yz; |
||||
double det_y = xx * zz - xz * xz; |
||||
double det_z = xx * yy - xy * xy; |
||||
|
||||
Vec3d abc; |
||||
if (det_x > det_y && det_x > det_z) { |
||||
abc = Vec3d(det_x, xz * yz - xy * zz, xy * yz - xz * yy); |
||||
} else if (det_y > det_z) { |
||||
abc = Vec3d(xz * yz - xy * zz, det_y, xy * xz - yz * xx); |
||||
} else { |
||||
abc = Vec3d(xy * yz - xz * yy, xy * xz - yz * xx, det_z); |
||||
} |
||||
|
||||
|
||||
double magnitude_abc = sqrt(abc[0]*abc[0] + abc[1]* abc[1] + abc[2] * abc[2]); |
||||
|
||||
// Return invalid plane if the points don't span a plane.
|
||||
if (magnitude_abc == 0) { |
||||
return Vec4d (0, 0, 0, 0); |
||||
} |
||||
abc /= magnitude_abc; |
||||
double d = -abc.dot(centroid); |
||||
|
||||
Vec4d coefficients (abc[0], abc[1], abc[2], d); |
||||
center = Point3d (centroid); |
||||
return coefficients; |
||||
} |
||||
|
||||
bool getCylinderFromPoints(const Mat cloud, const Mat normals_cld, |
||||
const std::vector<unsigned> &inliers, vector<double> & model_coefficients) { |
||||
assert(inliers.size() == 2); |
||||
|
||||
Mat _pointsAndNormals; |
||||
|
||||
assert(normals_cld.cols == cloud.cols); |
||||
|
||||
const Point3d* points = cloud.ptr<Point3d>(0); |
||||
const Vec3f* normals = normals_cld.ptr<Vec3f>(0); |
||||
|
||||
if (fabs (points[inliers[0]].x - points[inliers[1]].x) <= std::numeric_limits<float>::epsilon () && |
||||
fabs (points[inliers[0]].y - points[inliers[1]].y) <= std::numeric_limits<float>::epsilon () && |
||||
fabs (points[inliers[0]].z - points[inliers[1]].z) <= std::numeric_limits<float>::epsilon ()) |
||||
{ |
||||
return (false); |
||||
} |
||||
Vec3f p1 (points[inliers[0]].x, points[inliers[0]].y, points[inliers[0]].z); |
||||
Vec3f p2 (points[inliers[1]].x, points[inliers[1]].y, points[inliers[1]].z); |
||||
|
||||
Vec3f n1 (normals[inliers[0]] [0], normals[inliers[0]] [1], normals[inliers[0]] [2]); |
||||
Vec3f n2 (normals[inliers[1]] [0], normals[inliers[1]] [1], normals[inliers[1]] [2]); |
||||
Vec3f w = n1 + p1 - p2; |
||||
|
||||
float a = n1.dot (n1); |
||||
float b = n1.dot (n2); |
||||
float c = n2.dot (n2); |
||||
float d = n1.dot (w); |
||||
float e = n2.dot (w); |
||||
float denominator = a*c - b*b; |
||||
float sc, tc; |
||||
// Compute the line parameters of the two closest points
|
||||
if (denominator < 1e-8) // The lines are almost parallel
|
||||
{ |
||||
sc = 0.0f; |
||||
tc = (b > c ? d / b : e / c); // Use the largest denominator
|
||||
} |
||||
else |
||||
{ |
||||
sc = (b*e - c*d) / denominator; |
||||
tc = (a*e - b*d) / denominator; |
||||
} |
||||
|
||||
// point_on_axis, axis_direction
|
||||
Vec3f line_pt = p1 + n1 + sc * n1; |
||||
Vec3f line_dir = p2 + tc * n2 - line_pt; |
||||
|
||||
model_coefficients.resize (7); |
||||
// point on line
|
||||
model_coefficients[0] = line_pt[0]; |
||||
model_coefficients[1] = line_pt[1]; |
||||
model_coefficients[2] = line_pt[2]; |
||||
|
||||
double divide_by = std::sqrt (line_dir[0] * line_dir[0] + |
||||
line_dir[1] * line_dir[1] + |
||||
line_dir[2] * line_dir[2]); |
||||
// direction of line;
|
||||
model_coefficients[3] = line_dir[0] / divide_by; |
||||
model_coefficients[4] = line_dir[1] / divide_by; |
||||
model_coefficients[5] = line_dir[2] / divide_by; |
||||
|
||||
double radius_squared = fabs((line_dir.cross(line_pt - p1)).dot(line_dir.cross(line_pt - p1)) / line_dir.dot(line_dir)); |
||||
|
||||
// radius of cylinder
|
||||
model_coefficients[6] = sqrt(radius_squared); |
||||
|
||||
if (radius_squared == 0) return false; |
||||
|
||||
return (true); |
||||
} |
||||
|
||||
SACPlaneModel::SACPlaneModel(Vec4d coefficients, Point3d set_center, Size2d set_size) { |
||||
this -> ModelCoefficients.reserve(4); |
||||
for (int i = 0; i < 4; i++) { |
||||
this -> ModelCoefficients.push_back(coefficients[i]); |
||||
} |
||||
this -> size = set_size; |
||||
|
||||
this -> normal = Vec3d(coefficients[0], coefficients[1], coefficients[2]); |
||||
this -> center = set_center; |
||||
|
||||
// Assign normal vector
|
||||
for (unsigned i = 0; i < 3; i++) normal[i] = coefficients[i]; |
||||
} |
||||
|
||||
SACPlaneModel::SACPlaneModel(Vec4d coefficients, Size2d set_size) { |
||||
this -> ModelCoefficients.reserve(4); |
||||
for (int i = 0; i < 4; i++) { |
||||
this->ModelCoefficients.push_back(coefficients[i]); |
||||
} |
||||
this->size = set_size; |
||||
|
||||
this-> normal = Vec3d(coefficients[0], coefficients[1], coefficients[2]); |
||||
this -> center = Point3d(0, 0, - coefficients[3] / coefficients[2]); |
||||
// Assign normal vector
|
||||
for (unsigned i = 0; i < 3; i++) normal[i] = coefficients[i]; |
||||
|
||||
if (coefficients[2] != 0) { |
||||
center.x = 0; |
||||
center.y = 0; |
||||
center.z = -coefficients[3] / coefficients[2]; |
||||
} else if (coefficients[1] != 0) { |
||||
center.x = 0; |
||||
center.y = -coefficients[3] / coefficients[1]; |
||||
center.z = 0; |
||||
} else if (coefficients[0] != 0) { |
||||
center.x = -coefficients[3] / coefficients[0]; |
||||
center.y = 0; |
||||
center.z = 0; |
||||
} |
||||
} |
||||
|
||||
SACPlaneModel::SACPlaneModel(vector<double> coefficients, Size2d set_size) { |
||||
assert(coefficients.size() == 4); |
||||
this->ModelCoefficients = coefficients; |
||||
this->size = set_size; |
||||
|
||||
// Assign normal vector
|
||||
for (unsigned i = 0; i < 3; i++) normal[i] = coefficients[i]; |
||||
|
||||
// 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; |
||||
} |
||||
} |
||||
|
||||
viz::WPlane SACPlaneModel::WindowWidget () { |
||||
return viz::WPlane (this->center, this->normal, Vec3d(1, 0, 0), this->size, viz::Color::green()); |
||||
} |
||||
|
||||
pair<double, double> SACPlaneModel::getInliers(Mat cloud, vector<unsigned> indices, const double threshold, vector<unsigned>& inliers) { |
||||
pair<double, double> result; |
||||
inliers.clear(); |
||||
const Vec3f* points = cloud.ptr<Vec3f>(0); |
||||
const unsigned num_points = indices.size(); |
||||
|
||||
double magnitude_abc = sqrt(ModelCoefficients[0]*ModelCoefficients[0] + ModelCoefficients[1]* ModelCoefficients[1] + ModelCoefficients[2] * ModelCoefficients[2]); |
||||
|
||||
assert (magnitude_abc == 0); |
||||
|
||||
Vec4d NormalisedCoefficients (ModelCoefficients[0]/magnitude_abc, ModelCoefficients[1]/magnitude_abc, ModelCoefficients[2]/magnitude_abc, ModelCoefficients[3]/magnitude_abc); |
||||
double fitness = 0; |
||||
double rmse = 0; |
||||
for (unsigned i = 0; i < num_points; i++) { |
||||
unsigned ind = indices[i]; |
||||
Vec4d point4d (points[ind][0], points[ind][1], points[ind][2], 1); |
||||
double distanceFromPlane = point4d.dot(NormalisedCoefficients); |
||||
if (abs(distanceFromPlane) > threshold) continue; |
||||
inliers.emplace_back(ind); |
||||
|
||||
fitness+=1; |
||||
rmse += distanceFromPlane; |
||||
} |
||||
|
||||
unsigned num_inliers = fitness; |
||||
if (num_inliers == 0) { |
||||
result.first = 0; |
||||
result.second = 0; |
||||
} else { |
||||
rmse /= num_inliers; |
||||
fitness /= num_points; |
||||
|
||||
result.first = fitness; |
||||
result.second = rmse; |
||||
} |
||||
|
||||
return result; |
||||
} |
||||
SACSphereModel::SACSphereModel(Point3d set_center, double set_radius) { |
||||
this -> center = set_center; |
||||
this -> radius = set_radius; |
||||
|
||||
this -> ModelCoefficients.reserve(4); |
||||
this -> ModelCoefficients.push_back(center.x); |
||||
this -> ModelCoefficients.push_back(center.y); |
||||
this -> ModelCoefficients.push_back(center.z); |
||||
|
||||
this -> ModelCoefficients.push_back(radius); |
||||
} |
||||
|
||||
|
||||
SACSphereModel::SACSphereModel(Vec4d coefficients) { |
||||
this->ModelCoefficients.reserve(4); |
||||
for (int i = 0; i < 4; i++) { |
||||
this -> ModelCoefficients.push_back(coefficients[i]); |
||||
} |
||||
this -> center = Point3d(coefficients[0], coefficients[1], coefficients[2]); |
||||
this -> radius = coefficients[3]; |
||||
} |
||||
|
||||
SACSphereModel::SACSphereModel(vector<double> coefficients) { |
||||
assert(coefficients.size() == 4); |
||||
for (int i = 0; i < 4; i++) { |
||||
this->ModelCoefficients.push_back(coefficients[i]); |
||||
} |
||||
this -> center = Point3d(coefficients[0], coefficients[1], coefficients[2]); |
||||
this -> radius = coefficients[3]; |
||||
} |
||||
|
||||
|
||||
viz::WSphere SACSphereModel::WindowWidget () { |
||||
return viz::WSphere(this->center, this->radius, 10, viz::Color::green());; |
||||
} |
||||
|
||||
double 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); |
||||
} |
||||
|
||||
pair<double, double> SACSphereModel::getInliers(Mat cloud, vector<unsigned> indices, const double threshold, vector<unsigned>& inliers) { |
||||
pair<double, double> result; |
||||
inliers.clear(); |
||||
const Vec3f* points = cloud.ptr<Vec3f>(0); |
||||
const unsigned num_points = indices.size(); |
||||
|
||||
double fitness = 0; |
||||
double rmse = 0; |
||||
if(!isnan(radius)) { // radius may come out to be nan if selected points form a plane
|
||||
for (unsigned i = 0; i < num_points; i++) { |
||||
unsigned ind = indices[i]; |
||||
Point3d pt (points[ind][0], points[ind][1], points[ind][2]); |
||||
double distanceFromCenter = euclideanDist(pt, center); |
||||
|
||||
double distanceFromSurface = distanceFromCenter - radius; |
||||
if (distanceFromSurface > threshold) continue; |
||||
inliers.emplace_back(ind); |
||||
|
||||
fitness+=1; |
||||
rmse += max(0., distanceFromSurface); |
||||
} |
||||
} |
||||
|
||||
|
||||
unsigned num_inliers = fitness; |
||||
if (num_inliers == 0) { |
||||
result.first = 0; |
||||
result.second = 0; |
||||
} else { |
||||
rmse /= num_inliers; |
||||
fitness /= num_points; |
||||
result.first = fitness; |
||||
result.second = rmse; |
||||
} |
||||
|
||||
return result; |
||||
} |
||||
|
||||
viz::WCylinder SACCylinderModel::WindowWidget () { |
||||
Point3d first_point = Point3d( Vec3d(pt_on_axis) + size * (axis_dir)); |
||||
Point3d second_point = Point3d(Vec3d(pt_on_axis) - size * (axis_dir)); |
||||
|
||||
return viz::WCylinder (first_point, second_point, radius, 40, viz::Color::green()); |
||||
} |
||||
|
||||
SACCylinderModel::SACCylinderModel(const vector<double> coefficients) { |
||||
assert(coefficients.size() == 7); |
||||
for (int i = 0; i < 7; i++) { |
||||
this -> ModelCoefficients.push_back(coefficients[i]); |
||||
} |
||||
this -> pt_on_axis = Point3d(coefficients[0], coefficients[1], coefficients[2]); |
||||
this -> axis_dir = Vec3d(coefficients[3], coefficients[4], coefficients[5]); |
||||
this -> radius = coefficients[6]; |
||||
|
||||
} |
||||
|
||||
std::pair<double, double> SACCylinderModel::getInliers(Mat cloud, Mat normal_cloud, std::vector<unsigned> indices, const double threshold, std::vector<unsigned>& inliers, double normal_distance_weight_) { |
||||
pair<double, double> result; |
||||
inliers.clear(); |
||||
const Vec3f* points = cloud.ptr<Vec3f>(0); |
||||
const Vec3f* normals = normal_cloud.ptr<Vec3f>(0); |
||||
const unsigned num_points = indices.size(); |
||||
|
||||
double fitness = 0; |
||||
double rmse = 0; |
||||
axis_dir = (axis_dir); |
||||
|
||||
// for (int i = 0; i < num_points; i++) {
|
||||
// cout << i << " " << points[i] << endl;
|
||||
// }
|
||||
if(!isnan(radius)) { // radius may come out to be nan if selected points form a plane
|
||||
for (unsigned i = 0; i < num_points; i++) { |
||||
unsigned ind = indices[i]; |
||||
Point3d pt (points[ind][0], points[ind][1], points[ind][2]); |
||||
Vec3d normal (normals[ind][0], normals[ind][1], normals[ind][2]); |
||||
normal = normal / sqrt(normal[0] * normal[0] + normal[1] * normal[1] + normal[2] * normal[2]); |
||||
|
||||
double distanceFromAxis = fabs((axis_dir.cross(pt_on_axis - pt)).dot(axis_dir.cross(pt_on_axis - pt)) / axis_dir.dot(axis_dir)); |
||||
|
||||
double distanceFromSurface = fabs(distanceFromAxis - radius*radius); |
||||
if (distanceFromSurface > threshold) continue; |
||||
|
||||
// Calculate the point's projection on the cylinder axis
|
||||
float dist = (pt.dot (axis_dir) - pt_on_axis.dot(axis_dir)); |
||||
Vec3d pt_proj = Vec3d(pt_on_axis) + dist * axis_dir; |
||||
Vec3f dir = Vec3d(pt) - pt_proj; |
||||
dir = dir / sqrt(dir[0] * dir[0] + dir[1] * dir[1] + dir[2] * dir[2]); |
||||
|
||||
// Calculate the angular distance between the point normal and the (dir=pt_proj->pt) vector
|
||||
double rad = normalize(normal).dot(dir); |
||||
if (rad < -1.0) rad = -1.0; |
||||
if (rad > 1.0) rad = 1.0; |
||||
double d_normal = fabs(acos (rad)); |
||||
|
||||
// convert 0 to PI/2
|
||||
d_normal = (std::min) (d_normal, M_PI - d_normal); |
||||
|
||||
// calculate overall distance as weighted sum of the two distances.
|
||||
double distance = fabs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * distanceFromSurface); |
||||
|
||||
if (distance > threshold) continue; |
||||
|
||||
inliers.emplace_back(ind); |
||||
|
||||
fitness += 1; |
||||
rmse += max(0., distance); |
||||
} |
||||
} |
||||
|
||||
unsigned num_inliers = fitness; |
||||
if (num_inliers == 0) { |
||||
result.first = 0; |
||||
result.second = 0; |
||||
} else { |
||||
rmse /= num_inliers; |
||||
fitness /= num_points; |
||||
|
||||
result.first = fitness; |
||||
result.second = rmse; |
||||
} |
||||
|
||||
return result; |
||||
} |
||||
|
||||
void SACModelFitting::setCloud(Mat inp_cloud, bool with_normals) { |
||||
if (! with_normals) { |
||||
|
||||
// normals are not required.
|
||||
// the cloud should have three channels.
|
||||
assert(inp_cloud.channels() == 3 || (inp_cloud.channels() == 1 && (inp_cloud.cols == 3 || inp_cloud.rows == 3))); |
||||
if (inp_cloud.rows == 1 && inp_cloud.channels() == 3) { |
||||
cloud = inp_cloud.clone(); |
||||
return; |
||||
} |
||||
|
||||
if (inp_cloud.channels() != 3 && inp_cloud.rows == 3) { |
||||
inp_cloud = inp_cloud.t(); |
||||
} |
||||
|
||||
const long unsigned num_points = inp_cloud.rows; |
||||
cloud = inp_cloud.reshape(3, num_points); |
||||
cloud = cloud.t(); |
||||
|
||||
} |
||||
else { |
||||
assert(inp_cloud.channels() == 1 && (inp_cloud.cols == 6 || inp_cloud.rows == 6)); |
||||
if (inp_cloud.rows == 6) { |
||||
inp_cloud = inp_cloud.t(); |
||||
} |
||||
|
||||
Mat _cld; |
||||
inp_cloud.colRange(0, 3).copyTo(_cld); |
||||
|
||||
Mat _normals; |
||||
inp_cloud.colRange(3, 6).copyTo(_normals); |
||||
|
||||
this -> cloud = Mat(_cld).reshape(3, 1); |
||||
this -> normals = Mat(_normals).reshape(3, 1); |
||||
this -> normals_available = true; |
||||
} |
||||
} |
||||
|
||||
SACModelFitting::SACModelFitting (Mat set_cloud, int set_model_type, int set_method_type, double set_threshold, int set_max_iters) |
||||
:cloud(set_cloud.clone()), model_type(set_model_type), method_type(set_method_type), threshold(set_threshold), max_iters(set_max_iters) {} |
||||
|
||||
SACModelFitting::SACModelFitting (int set_model_type, int set_method_type, double set_threshold, int set_max_iters) |
||||
:model_type(set_model_type), method_type(set_method_type), threshold(set_threshold), max_iters(set_max_iters) {} |
||||
|
||||
bool SACModelFitting::fit_once(vector<int> labels /* = {} */) { |
||||
|
||||
// Only RANSAC supported ATM, need to integrate with Maksym's framework.
|
||||
if (method_type != SAC_METHOD_RANSAC) return false; |
||||
|
||||
// creates an array of indices for the points in the point cloud which will be appended as masks to denote inliers and outliers.
|
||||
const Vec3f* points = cloud.ptr<Vec3f>(0); |
||||
unsigned num_points = cloud.cols; |
||||
|
||||
std::vector<unsigned> indices; |
||||
|
||||
if (labels.size() != num_points) { |
||||
indices = std::vector<unsigned> (num_points); |
||||
std::iota(std::begin(indices), std::end(indices), 0); |
||||
} else { |
||||
for (unsigned i = 0; i < num_points; i++) { |
||||
if (labels[i] == -1) indices.push_back(i); |
||||
} |
||||
} |
||||
|
||||
vector<unsigned> inliers_indices; |
||||
|
||||
// Initialize the best plane model.
|
||||
SACModel bestModel; |
||||
pair<double, double> bestResult(0, 0); |
||||
|
||||
if (model_type == PLANE_MODEL) { |
||||
const unsigned num_rnd_model_points = 3; |
||||
RNG rng((uint64)-1); |
||||
for (unsigned i = 0; i < max_iters; ++i) { |
||||
vector<unsigned> current_model_inliers; |
||||
SACModel model; |
||||
|
||||
for (unsigned j = 0; j < num_rnd_model_points;) { |
||||
std::swap(indices[j], indices[rng.uniform(0, num_points)]); |
||||
j++; |
||||
} |
||||
|
||||
for (unsigned j = 0; j < num_rnd_model_points; j++) { |
||||
unsigned idx = indices[j]; |
||||
current_model_inliers.emplace_back(idx); |
||||
} |
||||
|
||||
Point3d center; |
||||
Vec4d coefficients = getPlaneFromPoints(points, current_model_inliers, center); |
||||
if (coefficients == Vec4d(0, 0, 0, 0)) continue; |
||||
SACPlaneModel planeModel (coefficients, center); |
||||
pair<double, double> result = planeModel.getInliers(cloud, indices, threshold, current_model_inliers); |
||||
|
||||
// Compare fitness first.
|
||||
if (bestResult.first < result.first || (bestResult.first == result.first && bestResult.second > result.second )) { |
||||
bestResult = result; |
||||
bestModel.ModelCoefficients = planeModel.ModelCoefficients; |
||||
inliers_indices = current_model_inliers; |
||||
} |
||||
|
||||
} |
||||
if (bestModel.ModelCoefficients.size()) { |
||||
inliers.push_back(inliers_indices); |
||||
model_instances.push_back(bestModel); |
||||
return true; |
||||
} |
||||
} |
||||
|
||||
if (model_type == SPHERE_MODEL) { |
||||
RNG rng((uint64)-1); |
||||
const unsigned num_rnd_model_points = 4; |
||||
double bestRadius = 10000000; |
||||
for (unsigned i = 0; i < max_iters; ++i) { |
||||
vector<unsigned> current_model_inliers; |
||||
SACModel model; |
||||
|
||||
for (unsigned j = 0; j < num_rnd_model_points;) { |
||||
std::swap(indices[j], indices[rng.uniform(0, num_points)]); |
||||
j++; |
||||
} |
||||
|
||||
for (unsigned j = 0; j < num_rnd_model_points; j++) { |
||||
unsigned idx = indices[j]; |
||||
current_model_inliers.emplace_back(idx); |
||||
} |
||||
|
||||
Point3d center; |
||||
double radius; |
||||
|
||||
getSphereFromPoints(points, current_model_inliers, center, radius); |
||||
SACSphereModel sphereModel (center, radius); |
||||
pair<double, double> result = sphereModel.getInliers(cloud, indices, threshold, current_model_inliers); |
||||
|
||||
// Compare fitness first.
|
||||
if (bestResult.first < result.first || (bestResult.first == result.first && bestResult.second > result.second) |
||||
|| (bestResult.first == result.first)) { |
||||
|
||||
if (bestResult.first == result.first && bestModel.ModelCoefficients.size() == 4 && sphereModel.radius > bestRadius) continue; |
||||
bestResult = result; |
||||
bestModel.ModelCoefficients = sphereModel.ModelCoefficients; |
||||
bestModel.ModelCoefficients = sphereModel.ModelCoefficients; |
||||
inliers_indices = current_model_inliers; |
||||
} |
||||
|
||||
} |
||||
if (bestModel.ModelCoefficients.size()) { |
||||
inliers.push_back(inliers_indices); |
||||
model_instances.push_back(bestModel); |
||||
return true; |
||||
} |
||||
} |
||||
|
||||
if (model_type == CYLINDER_MODEL) { |
||||
assert(this->normals_available == true); |
||||
RNG rng((uint64)-1); |
||||
const unsigned num_rnd_model_points = 2; |
||||
|
||||
if (!normals_available) { |
||||
// Reshape the cloud for Compute Normals Function
|
||||
Mat _pointsAndNormals; |
||||
Vec3d viewpoint(0, 0, 0); |
||||
Mat _cld_reshaped = Mat(cloud).t(); |
||||
_cld_reshaped = _cld_reshaped.reshape(1); |
||||
ppf_match_3d::computeNormalsPC3d(_cld_reshaped, _pointsAndNormals, 12, false, viewpoint); |
||||
|
||||
Mat(_pointsAndNormals.colRange(3,6)).copyTo(normals); |
||||
normals = normals.reshape(3, num_points); |
||||
normals = normals.t(); |
||||
} |
||||
|
||||
for (unsigned i = 0; i < max_iters; ++i) { |
||||
vector<unsigned> current_model_inliers; |
||||
SACModel model; |
||||
|
||||
for (unsigned j = 0; j < num_rnd_model_points;) { |
||||
std::swap(indices[j], indices[rng.uniform(0, num_points)]); |
||||
j++; |
||||
} |
||||
|
||||
for (unsigned j = 0; j < num_rnd_model_points; j++) { |
||||
unsigned idx = indices[j]; |
||||
current_model_inliers.emplace_back(idx); |
||||
} |
||||
|
||||
Point3d center; |
||||
vector<double> coefficients; |
||||
bool valid_model = getCylinderFromPoints(cloud, normals, current_model_inliers, coefficients); |
||||
|
||||
if (!valid_model) continue; |
||||
|
||||
SACCylinderModel cylinderModel (coefficients); |
||||
|
||||
pair<double, double> result = cylinderModel.getInliers(cloud, normals, indices, threshold, current_model_inliers, normal_distance_weight_); |
||||
|
||||
// Compare fitness first.
|
||||
if (bestResult.first < result.first || (bestResult.first == result.first && bestResult.second > result.second)) { |
||||
// if (bestResult.first == result.first && bestModel.ModelCoefficients.size() == 7) continue;
|
||||
bestResult = result; |
||||
bestModel.ModelCoefficients = cylinderModel.ModelCoefficients; |
||||
bestModel.ModelCoefficients = cylinderModel.ModelCoefficients; |
||||
inliers_indices = current_model_inliers; |
||||
cout << bestResult.first << endl; |
||||
} |
||||
|
||||
} |
||||
|
||||
if (bestModel.ModelCoefficients.size()) { |
||||
inliers.push_back(inliers_indices); |
||||
model_instances.push_back(bestModel); |
||||
return true; |
||||
} |
||||
|
||||
} |
||||
return false; |
||||
} |
||||
|
||||
void SACModelFitting::segment(float remaining_cloud_threshold /*=0.3*/) { |
||||
unsigned num_points = cloud.cols; |
||||
|
||||
std::vector<unsigned> indices (num_points); |
||||
std::iota(std::begin(indices), std::end(indices), 0); |
||||
|
||||
std::vector<int> point_labels (num_points, -1); |
||||
|
||||
long num_segmented_points = 0; |
||||
|
||||
int label = 0; |
||||
while ( (float) num_segmented_points / num_points < (1 - remaining_cloud_threshold )) { |
||||
label = label + 1; |
||||
bool successful_fitting = fit_once(point_labels); |
||||
|
||||
if (!successful_fitting) { |
||||
cout << "Could not fit the required model" << endl; |
||||
break; |
||||
} |
||||
vector<unsigned> latest_model_inliers = inliers.back(); |
||||
num_segmented_points += latest_model_inliers.size(); |
||||
|
||||
// This loop is for implementation purposes only, and maps each point to a label.
|
||||
// All the points still labelled with -1 are non-segmented.
|
||||
// This way, complexity of the finding non-segmented is decreased to O(n).
|
||||
for(unsigned long i = 0; i < latest_model_inliers.size(); i++) |
||||
{ |
||||
point_labels[latest_model_inliers[i]] = label; |
||||
} |
||||
label++; |
||||
} |
||||
} |
||||
|
||||
void SACModelFitting::set_threshold (double threshold_value) { |
||||
threshold = threshold_value; |
||||
} |
||||
|
||||
void SACModelFitting::set_iterations (long unsigned iterations) { |
||||
max_iters = iterations; |
||||
} |
||||
|
||||
void SACModelFitting::set_normal_distance_weight(double weight) { |
||||
if (weight > 1) { |
||||
normal_distance_weight_ = 1; |
||||
} else if (weight < 0) { |
||||
normal_distance_weight_ = 0; |
||||
} else { |
||||
normal_distance_weight_ = weight; |
||||
} |
||||
} |
||||
|
||||
} // ptcloud
|
||||
} // cv
|
||||
@ -0,0 +1,9 @@ |
||||
// This file is part of OpenCV project.
|
||||
// It is subject to the license terms in the LICENSE file found in the top-level directory
|
||||
// of this distribution and at http://opencv.org/license.html.
|
||||
#include "test_precomp.hpp" |
||||
|
||||
CV_TEST_MAIN("", |
||||
cvtest::addDataSearchSubDirectory("contrib"), |
||||
cvtest::addDataSearchSubDirectory("contrib/ptcloud") |
||||
) |
||||
@ -0,0 +1,15 @@ |
||||
// This file is part of OpenCV project.
|
||||
// It is subject to the license terms in the LICENSE file found in the top-level directory
|
||||
// of this distribution and at http://opencv.org/license.html.
|
||||
#ifndef __OPENCV_TEST_PTCLOUD_PRECOMP_HPP__ |
||||
#define __OPENCV_TEST_PTCLOUD_PRECOMP_HPP__ |
||||
|
||||
#include "opencv2/core.hpp" |
||||
#include "opencv2/ts.hpp" |
||||
#include "opencv2/ptcloud.hpp" |
||||
|
||||
namespace opencv_test { |
||||
using namespace cv::ptcloud; |
||||
} |
||||
|
||||
#endif |
||||
Loading…
Reference in new issue