mirror of https://github.com/CGAL/cgal
376 lines
11 KiB
C++
376 lines
11 KiB
C++
#include "include/utils.h"
|
|
#include "include/Saver.h"
|
|
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
|
|
#include <CGAL/Shape_detection/Efficient_RANSAC.h>
|
|
#include <CGAL/Shape_regularization/regularize_planes.h>
|
|
|
|
using Kernel = CGAL::Exact_predicates_inexact_constructions_kernel;
|
|
|
|
using Point = typename Kernel::Point_3;
|
|
using Vector = typename Kernel::Vector_3;
|
|
using Plane = typename Kernel::Plane_3;
|
|
|
|
using Point_with_normal = std::pair<Point, Vector>;
|
|
using Pwn_vector = std::vector<Point_with_normal>;
|
|
using Point_map = CGAL::First_of_pair_property_map<Point_with_normal>;
|
|
using Normal_map = CGAL::Second_of_pair_property_map<Point_with_normal>;
|
|
using Transform = CGAL::Aff_transformation_3<Kernel>;
|
|
|
|
using Traits = CGAL::Shape_detection::Efficient_RANSAC_traits<
|
|
Kernel, Pwn_vector, Point_map, Normal_map>;
|
|
using Efficient_RANSAC = CGAL::Shape_detection::Efficient_RANSAC<Traits>;
|
|
|
|
template<typename OutputIterator>
|
|
void generate_random_points(
|
|
const Point& origin,
|
|
const Vector& base1,
|
|
const Vector& base2,
|
|
const std::size_t nb_pts,
|
|
OutputIterator output) {
|
|
|
|
Vector normal = CGAL::cross_product(base1, base2);
|
|
normal /= std::sqrt(normal * normal);
|
|
|
|
// Vector noise = 0.00001 * normal;
|
|
for (std::size_t i = 0; i < nb_pts; ++i) {
|
|
const 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.0; }
|
|
double to_deg(const double rad) { return rad * 180.0 / CGAL_PI; }
|
|
|
|
void rotate_vector(
|
|
Vector& vector,
|
|
const int axis,
|
|
const double angle) {
|
|
|
|
const double cos_a = std::cos(angle);
|
|
const double sin_a = std::sin(angle);
|
|
|
|
Transform transform;
|
|
if (axis == 0) { // around x
|
|
transform = Transform(
|
|
1.0, 0.0, 0.0,
|
|
0.0, cos_a, -sin_a,
|
|
0.0, sin_a, cos_a);
|
|
}
|
|
else if (axis == 1) { // around y
|
|
transform = Transform(
|
|
cos_a, 0.0, sin_a,
|
|
0.0, 1.0, 0.0,
|
|
-sin_a, 0.0, cos_a);
|
|
}
|
|
else if (axis == 2) { // around z
|
|
transform = Transform(
|
|
cos_a, -sin_a, 0.0,
|
|
sin_a, cos_a, 0.0,
|
|
0.0, 0.0, 1.0);
|
|
}
|
|
else abort();
|
|
vector = transform(vector);
|
|
}
|
|
|
|
std::vector<Plane> get_ransac_planes(
|
|
const Efficient_RANSAC& ransac) {
|
|
|
|
std::vector<Plane> planes;
|
|
for (const auto& shape : ransac.shapes()) {
|
|
planes.push_back((Plane)(*(dynamic_cast<
|
|
CGAL::Shape_detection::Plane<Traits>*>(shape.get()))));
|
|
}
|
|
return planes;
|
|
}
|
|
|
|
bool planes_difference(
|
|
const Plane& a,
|
|
const Plane& b) {
|
|
|
|
return (
|
|
(CGAL::abs(a.a() - b.a()) > 1e-6) ||
|
|
(CGAL::abs(a.b() - b.b()) > 1e-6) ||
|
|
(CGAL::abs(a.c() - b.c()) > 1e-6) ||
|
|
(CGAL::abs(a.d() - b.d()) > 1e-6) );
|
|
}
|
|
|
|
void check_ransac_size(
|
|
const Efficient_RANSAC& ransac,
|
|
const 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_difference(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_difference(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.0, 0.0),
|
|
vy(0.0, 1.0, 0.0),
|
|
vz(0.0, 0.0, 1.0);
|
|
|
|
Vector dvx = vx;
|
|
rotate_vector(dvx, 1, to_rad(10.0));
|
|
|
|
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.0, 0.0), vx, vy,
|
|
5000, std::back_inserter(points));
|
|
|
|
generate_random_points(
|
|
Point(0.0, 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);
|
|
|
|
const std::vector<Plane> before = get_ransac_planes(ransac);
|
|
|
|
// Test regularization.
|
|
Efficient_RANSAC::Plane_range planes = ransac.planes();
|
|
CGAL::Shape_regularization::Planes::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.0);
|
|
|
|
std::vector<Plane> after = get_ransac_planes(ransac);
|
|
|
|
std::cerr << "* nothing should change now..." << std::endl;
|
|
check_planes_unchanged(before, after);
|
|
|
|
CGAL::Shape_regularization::Planes::regularize_planes(
|
|
planes,
|
|
points,
|
|
CGAL::parameters::
|
|
plane_map(CGAL::Shape_detection::Plane_map<Traits>()).
|
|
point_map(Point_map()).
|
|
plane_index_map(
|
|
CGAL::Shape_detection::Point_to_shape_index_map<Traits>(points, planes)).
|
|
regularize_parallelism(true).
|
|
regularize_orthogonality(false).
|
|
regularize_coplanarity(false).
|
|
regularize_axis_symmetry(false).
|
|
maximum_angle(15.0));
|
|
|
|
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.0, 0.0), vy, vz,
|
|
5000, std::back_inserter(points));
|
|
|
|
generate_random_points(
|
|
Point(0.5, 0.0, 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);
|
|
|
|
const std::vector<Plane> before = get_ransac_planes(ransac);
|
|
|
|
Efficient_RANSAC::Plane_range planes = ransac.planes();
|
|
CGAL::Shape_regularization::Planes::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.0);
|
|
|
|
std::vector<Plane> after = get_ransac_planes(ransac);
|
|
|
|
std::cerr << "* nothing should change now..." << std::endl;
|
|
check_planes_unchanged(before, after);
|
|
|
|
CGAL::Shape_regularization::Planes::regularize_planes(
|
|
planes,
|
|
CGAL::Shape_detection::Plane_map<Traits>(),
|
|
points,
|
|
Point_map(),
|
|
CGAL::parameters::plane_index_map(
|
|
CGAL::Shape_detection::Point_to_shape_index_map<Traits>(points, planes)).
|
|
regularize_parallelism(false).
|
|
regularize_orthogonality(true).
|
|
regularize_coplanarity(false).
|
|
regularize_axis_symmetry(true).
|
|
maximum_angle(15.0));
|
|
|
|
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.0, 0.0), vx, vy,
|
|
5000, std::back_inserter(points));
|
|
|
|
generate_random_points(
|
|
Point(1.0, 1.0, 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);
|
|
|
|
const std::vector<Plane> before = get_ransac_planes(ransac);
|
|
|
|
Efficient_RANSAC::Plane_range planes = ransac.planes();
|
|
CGAL::Shape_regularization::Planes::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, 0.1);
|
|
|
|
std::vector<Plane> after = get_ransac_planes(ransac);
|
|
|
|
std::cerr << "* nothing should change now..." << std::endl;
|
|
check_planes_unchanged(before, after);
|
|
|
|
CGAL::Shape_regularization::Planes::regularize_planes(
|
|
planes,
|
|
points,
|
|
CGAL::parameters::
|
|
point_map(Point_map()).
|
|
plane_map(CGAL::Shape_detection::Plane_map<Traits>()).
|
|
plane_index_map(
|
|
CGAL::Shape_detection::Point_to_shape_index_map<Traits>(points, planes)).
|
|
regularize_parallelism(true).
|
|
regularize_orthogonality(false).
|
|
regularize_coplanarity(true).
|
|
regularize_axis_symmetry(false).
|
|
maximum_angle(5.0).
|
|
maximum_offset(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.0));
|
|
rotate_vector(dvx2, 1, to_rad(-50.0));
|
|
|
|
generate_random_points(
|
|
Point(0.0, 0.0, -0.5), dvx1, vy,
|
|
5000, std::back_inserter(points));
|
|
|
|
generate_random_points(
|
|
Point(0.0, 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);
|
|
|
|
const std::vector<Plane> before = get_ransac_planes(ransac);
|
|
|
|
Efficient_RANSAC::Plane_range planes = ransac.planes();
|
|
CGAL::Shape_regularization::Planes::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, 0.01, Vector(1.0, 0.0, 0.0));
|
|
|
|
std::vector<Plane> after = get_ransac_planes(ransac);
|
|
|
|
std::cerr << "* nothing should change now..." << std::endl;
|
|
check_planes_unchanged(before, after);
|
|
|
|
CGAL::Shape_regularization::Planes::regularize_planes(
|
|
planes,
|
|
CGAL::Shape_detection::Plane_map<Traits>(),
|
|
points,
|
|
Point_map(),
|
|
CGAL::parameters::plane_index_map(
|
|
CGAL::Shape_detection::Point_to_shape_index_map<Traits>(points, planes)).
|
|
regularize_parallelism(false).
|
|
regularize_orthogonality(false).
|
|
regularize_coplanarity(false).
|
|
regularize_axis_symmetry(true).
|
|
maximum_angle(15.0).
|
|
symmetry_direction(Vector(1.0, 0.0, 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;
|
|
}
|