mirror of https://github.com/CGAL/cgal
331 lines
11 KiB
C++
331 lines
11 KiB
C++
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
|
|
#include <CGAL/Point_with_normal_3.h>
|
|
#include <CGAL/property_map.h>
|
|
|
|
#include <CGAL/Shape_detection/Efficient_RANSAC.h>
|
|
#include <CGAL/Regularization/regularize_planes.h>
|
|
#include <CGAL/Random.h>
|
|
#include <CGAL/Aff_transformation_3.h>
|
|
|
|
#include <iostream>
|
|
#include <fstream>
|
|
|
|
typedef CGAL::Exact_predicates_inexact_constructions_kernel Kernel;
|
|
typedef Kernel::Point_3 Point;
|
|
typedef Kernel::Vector_3 Vector;
|
|
typedef Kernel::Plane_3 Plane;
|
|
typedef std::pair<Point, Vector> Point_with_normal;
|
|
typedef std::vector<Point_with_normal> Pwn_vector;
|
|
typedef CGAL::First_of_pair_property_map<Point_with_normal> Point_map;
|
|
typedef CGAL::Second_of_pair_property_map<Point_with_normal> Normal_map;
|
|
|
|
typedef CGAL::Shape_detection::Efficient_RANSAC_traits<Kernel, Pwn_vector, Point_map, Normal_map> Traits;
|
|
typedef CGAL::Shape_detection::Efficient_RANSAC<Traits> Efficient_ransac;
|
|
|
|
typedef CGAL::Aff_transformation_3<Kernel> Transform;
|
|
|
|
|
|
template <typename OutputIterator>
|
|
void generate_random_points (const Point& origin, const Vector& base1, const Vector& base2,
|
|
std::size_t nb_pts, OutputIterator output)
|
|
{
|
|
Vector normal = CGAL::cross_product (base1, base2);
|
|
normal = normal / std::sqrt (normal * normal);
|
|
// Vector noise = 0.00001 * normal;
|
|
|
|
for (std::size_t i = 0; i < nb_pts; ++ i)
|
|
{
|
|
Point point = origin
|
|
+ CGAL::get_default_random().get_double() * base1
|
|
+ CGAL::get_default_random().get_double() * base2;
|
|
// + CGAL::get_default_random().get_double() * noise;
|
|
*(output ++) = std::make_pair (point, normal);
|
|
}
|
|
}
|
|
|
|
double to_rad (const double& deg) { return deg * CGAL_PI / 180.; }
|
|
double to_deg (const double& rad) { return rad * 180. / CGAL_PI; }
|
|
|
|
void rotate_vector (Vector& vector, int axis, double angle)
|
|
{
|
|
double cos_a = std::cos (angle);
|
|
double sin_a = std::sin (angle);
|
|
|
|
Transform trans;
|
|
if (axis == 0) // around x
|
|
{
|
|
trans = Transform (1., 0., 0.,
|
|
0., cos_a, -sin_a,
|
|
0., sin_a, cos_a);
|
|
}
|
|
else if (axis == 1) // around y
|
|
{
|
|
trans = Transform ( cos_a, 0., sin_a,
|
|
0., 1., 0.,
|
|
-sin_a, 0., cos_a);
|
|
}
|
|
else if (axis == 2) // around z
|
|
{
|
|
trans = Transform ( cos_a, -sin_a, 0.,
|
|
sin_a, cos_a, 0.,
|
|
0., 0., 1.);
|
|
}
|
|
else
|
|
abort();
|
|
|
|
vector = trans (vector);
|
|
|
|
}
|
|
|
|
std::vector<Plane> get_ransac_planes (const Efficient_ransac& ransac)
|
|
{
|
|
std::vector<Plane> out;
|
|
for (const auto& shape : ransac.shapes()) {
|
|
out.push_back ((Plane)(*(dynamic_cast<CGAL::Shape_detection::Plane<Traits>*>(shape.get ()))));
|
|
}
|
|
return out;
|
|
}
|
|
|
|
bool planes_differ (const Plane& a, const Plane& b)
|
|
{
|
|
return ((std::fabs (a.a() - b.a()) > 1e-6) ||
|
|
(std::fabs (a.b() - b.b()) > 1e-6) ||
|
|
(std::fabs (a.c() - b.c()) > 1e-6) ||
|
|
(std::fabs (a.d() - b.d()) > 1e-6));
|
|
}
|
|
|
|
void check_ransac_size (const Efficient_ransac& ransac, std::size_t nb)
|
|
{
|
|
if (ransac.shapes().size() != nb)
|
|
{
|
|
std::cerr << "Error: " << ransac.shapes().size() << " detected plane(s) instead of " << nb << "." << std::endl;
|
|
abort();
|
|
}
|
|
}
|
|
|
|
void check_planes_unchanged (const std::vector<Plane>& before, const std::vector<Plane>& after)
|
|
{
|
|
for (std::size_t i = 0; i < before.size(); ++ i)
|
|
if (planes_differ (before[i], after[i]))
|
|
std::cerr << "Error: [" << before[i] << "] was altered as [" << after[i] << "]" << std::endl;
|
|
}
|
|
|
|
void check_planes_changed (const std::vector<Plane>& before, const std::vector<Plane>& after)
|
|
{
|
|
for (std::size_t i = 0; i < before.size(); ++ i)
|
|
if (planes_differ (before[i], after[i]))
|
|
return;
|
|
std::cerr << "Error: no plane has been altered by regularization while at least one should have." << std::endl;
|
|
}
|
|
|
|
int main()
|
|
{
|
|
Vector vx (1., 0., 0.),
|
|
vy (0., 1., 0.),
|
|
vz (0., 0., 1.);
|
|
|
|
Vector dvx = vx;
|
|
rotate_vector (dvx, 1, to_rad(10.));
|
|
|
|
Efficient_ransac ransac;
|
|
|
|
const std::size_t nb_pts = 5000;
|
|
|
|
Efficient_ransac::Parameters op;
|
|
op.probability = 0.05;
|
|
op.min_points = nb_pts / 2;
|
|
op.epsilon = 0.02;
|
|
op.cluster_epsilon = 0.2;
|
|
op.normal_threshold = 0.8;
|
|
|
|
// Test parallelism.
|
|
std::cerr << "Testing parallelism..." << std::endl;
|
|
{
|
|
Pwn_vector points;
|
|
|
|
generate_random_points (Point (0., 0., 0.), vx, vy,
|
|
5000, std::back_inserter (points));
|
|
|
|
|
|
generate_random_points (Point (0., 0., 0.5), dvx, vy,
|
|
5000, std::back_inserter (points));
|
|
|
|
ransac.set_input(points);
|
|
ransac.add_shape_factory<CGAL::Shape_detection::Plane<Traits> >();
|
|
ransac.detect(op);
|
|
check_ransac_size (ransac, 2);
|
|
|
|
std::vector<Plane> before = get_ransac_planes(ransac);
|
|
|
|
// Test regularization.
|
|
Efficient_ransac::Plane_range planes = ransac.planes();
|
|
CGAL::regularize_planes (points,
|
|
Point_map(),
|
|
planes,
|
|
CGAL::Shape_detection::Plane_map<Traits>(),
|
|
CGAL::Shape_detection::Point_to_shape_index_map<Traits>(points, planes),
|
|
true, false, false, false, 5.);
|
|
|
|
std::vector<Plane> after = get_ransac_planes(ransac);
|
|
|
|
std::cerr << " * Nothing should change now..." << std::endl;
|
|
check_planes_unchanged (before, after);
|
|
|
|
CGAL::regularize_planes (points,
|
|
Point_map(),
|
|
planes,
|
|
CGAL::Shape_detection::Plane_map<Traits>(),
|
|
CGAL::Shape_detection::Point_to_shape_index_map<Traits>(points, planes),
|
|
true, false, false, false, 15.);
|
|
|
|
after = get_ransac_planes(ransac);
|
|
|
|
std::cerr << " * Something should change now..." << std::endl;
|
|
check_planes_changed (before, after);
|
|
ransac.clear ();
|
|
}
|
|
|
|
// Test orthogonality.
|
|
std::cerr << "Testing orthogonality..." << std::endl;
|
|
{
|
|
Pwn_vector points;
|
|
|
|
generate_random_points (Point (0., 0., 0.), vy, vz,
|
|
5000, std::back_inserter (points));
|
|
|
|
generate_random_points (Point (0.5, 0., 0.), dvx, vy,
|
|
5000, std::back_inserter (points));
|
|
|
|
|
|
ransac.set_input(points);
|
|
ransac.add_shape_factory<CGAL::Shape_detection::Plane<Traits> >();
|
|
ransac.detect(op);
|
|
check_ransac_size (ransac, 2);
|
|
|
|
std::vector<Plane> before = get_ransac_planes(ransac);
|
|
|
|
Efficient_ransac::Plane_range planes = ransac.planes();
|
|
CGAL::regularize_planes (points,
|
|
Point_map(),
|
|
planes,
|
|
CGAL::Shape_detection::Plane_map<Traits>(),
|
|
CGAL::Shape_detection::Point_to_shape_index_map<Traits>(points, planes),
|
|
false, true, false, true, 5.);
|
|
|
|
std::vector<Plane> after = get_ransac_planes(ransac);
|
|
|
|
std::cerr << " * Nothing should change now..." << std::endl;
|
|
check_planes_unchanged (before, after);
|
|
|
|
CGAL::regularize_planes (points,
|
|
Point_map(),
|
|
planes,
|
|
CGAL::Shape_detection::Plane_map<Traits>(),
|
|
CGAL::Shape_detection::Point_to_shape_index_map<Traits>(points, planes),
|
|
false, true, false, true, 15.);
|
|
|
|
after = get_ransac_planes(ransac);
|
|
|
|
std::cerr << " * Something should change now..." << std::endl;
|
|
check_planes_changed (before, after);
|
|
ransac.clear ();
|
|
}
|
|
|
|
// Test coplanarity.
|
|
std::cerr << "Testing coplanarity..." << std::endl;
|
|
{
|
|
Pwn_vector points;
|
|
|
|
generate_random_points (Point (0., 0., 0.), vx, vy,
|
|
5000, std::back_inserter (points));
|
|
|
|
generate_random_points (Point (1., 1., 0.2), vx, vy,
|
|
5000, std::back_inserter (points));
|
|
|
|
ransac.set_input(points);
|
|
ransac.add_shape_factory<CGAL::Shape_detection::Plane<Traits> >();
|
|
ransac.detect(op);
|
|
check_ransac_size (ransac, 2);
|
|
|
|
std::vector<Plane> before = get_ransac_planes(ransac);
|
|
|
|
Efficient_ransac::Plane_range planes = ransac.planes();
|
|
CGAL::regularize_planes (points,
|
|
Point_map(),
|
|
planes,
|
|
CGAL::Shape_detection::Plane_map<Traits>(),
|
|
CGAL::Shape_detection::Point_to_shape_index_map<Traits>(points, planes),
|
|
true, false, true, false, 5., 0.1);
|
|
|
|
std::vector<Plane> after = get_ransac_planes(ransac);
|
|
|
|
std::cerr << " * Nothing should change now..." << std::endl;
|
|
check_planes_unchanged (before, after);
|
|
|
|
CGAL::regularize_planes (points,
|
|
Point_map(),
|
|
planes,
|
|
CGAL::Shape_detection::Plane_map<Traits>(),
|
|
CGAL::Shape_detection::Point_to_shape_index_map<Traits>(points, planes),
|
|
true, false, true, false, 5., 0.3);
|
|
|
|
after = get_ransac_planes(ransac);
|
|
|
|
std::cerr << " * Something should change now..." << std::endl;
|
|
check_planes_changed (before, after);
|
|
ransac.clear ();
|
|
}
|
|
|
|
// Test symmetry.
|
|
std::cerr << "Testing symmetry..." << std::endl;
|
|
{
|
|
Pwn_vector points;
|
|
|
|
Vector dvx1 = vx;
|
|
Vector dvx2 = vx;
|
|
rotate_vector (dvx1, 1, to_rad(40.));
|
|
rotate_vector (dvx2, 1, to_rad(-50.));
|
|
|
|
generate_random_points (Point (0., 0., -0.5), dvx1, vy,
|
|
5000, std::back_inserter (points));
|
|
|
|
generate_random_points (Point (0., 0., 0.5), dvx2, vy,
|
|
5000, std::back_inserter (points));
|
|
|
|
ransac.set_input(points);
|
|
ransac.add_shape_factory<CGAL::Shape_detection::Plane<Traits> >();
|
|
ransac.detect(op);
|
|
check_ransac_size (ransac, 2);
|
|
|
|
std::vector<Plane> before = get_ransac_planes(ransac);
|
|
Efficient_ransac::Plane_range planes = ransac.planes();
|
|
CGAL::regularize_planes (points,
|
|
Point_map(),
|
|
planes,
|
|
CGAL::Shape_detection::Plane_map<Traits>(),
|
|
CGAL::Shape_detection::Point_to_shape_index_map<Traits>(points, planes),
|
|
false, false, false, true, 5., 0.01, Vector(1., 0., 0.));
|
|
|
|
std::vector<Plane> after = get_ransac_planes(ransac);
|
|
|
|
std::cerr << " * Nothing should change now..." << std::endl;
|
|
check_planes_unchanged (before, after);
|
|
|
|
CGAL::regularize_planes (points,
|
|
Point_map(),
|
|
planes,
|
|
CGAL::Shape_detection::Plane_map<Traits>(),
|
|
CGAL::Shape_detection::Point_to_shape_index_map<Traits>(points, planes),
|
|
false, false, false, true, 15., 0.01, Vector(1., 0., 0.));
|
|
|
|
after = get_ransac_planes(ransac);
|
|
|
|
std::cerr << " * Something should change now..." << std::endl;
|
|
check_planes_changed (before, after);
|
|
|
|
ransac.clear ();
|
|
}
|
|
|
|
return EXIT_SUCCESS;
|
|
}
|