From ea994650ec087a66334917128878cd2ae5b1330e Mon Sep 17 00:00:00 2001 From: Konstantinos Katrioplas Date: Thu, 26 Apr 2018 14:22:45 +0200 Subject: [PATCH] it works! --- .../Optimal_bounding_box/fitness_function.h | 57 +++++- .../Optimal_bounding_box/linear_algebra.h | 12 ++ .../include/CGAL/Optimal_bounding_box/obb.h | 168 ++++++++++++++++-- .../optimization_algorithms.h | 39 ++++ .../CGAL/Optimal_bounding_box/population.h | 5 +- .../test/Optimal_bounding_box/data/tetra3.off | 12 ++ .../test_optimization_algorithms.cpp | 132 +++++++++++++- 7 files changed, 401 insertions(+), 24 deletions(-) create mode 100644 Optimal_bounding_box/test/Optimal_bounding_box/data/tetra3.off diff --git a/Optimal_bounding_box/include/CGAL/Optimal_bounding_box/fitness_function.h b/Optimal_bounding_box/include/CGAL/Optimal_bounding_box/fitness_function.h index 93f7b12e9f5..6b2cd7b5bb5 100644 --- a/Optimal_bounding_box/include/CGAL/Optimal_bounding_box/fitness_function.h +++ b/Optimal_bounding_box/include/CGAL/Optimal_bounding_box/fitness_function.h @@ -24,7 +24,8 @@ #include #include - +#include +#include namespace CGAL { namespace Optimal_bounding_box { @@ -40,8 +41,6 @@ const double compute_fitness(const Matrix& R, const Matrix& data) CGAL_assertion(data.cols() == 3); CGAL_assertion(data.rows() >= 3); - CGAL_assertion(R.rows() == data.cols()); - // rotate points Matrix RT = R.transpose(); Matrix rotated_data = data * RT; @@ -56,7 +55,7 @@ const double compute_fitness(const Matrix& R, const Matrix& data) double zmin = rotated_data.col(2).minCoeff(); double zmax = rotated_data.col(2).maxCoeff(); - double x_dim = abs(xmax - xmin); + double x_dim = abs(xmax - xmin); // abs needed? double y_dim = abs(ymax - ymin); double z_dim = abs(zmax - zmin); @@ -65,6 +64,56 @@ const double compute_fitness(const Matrix& R, const Matrix& data) } +template +struct Fitness_map // -> a free function +{ + Fitness_map(Population& p, Matrix& points) : pop(p), points(points) + {} + + Matrix get_best() + { + std::size_t count_vertices = 0; + + std::size_t simplex_id; + std::size_t vertex_id; + double best_fitness = std::numeric_limits::max(); + for(std::size_t i = 0; i < pop.size(); ++i) + { + const std::vector simplex = pop[i]; + for(std::size_t j =0; j < 4; ++j) + { + const Matrix vertex = simplex[j]; + //std::cout << "i= "<< i << " j=" << j<<"\n vertex= " << vertex << std::endl; + ++count_vertices; + //std::cout << "vertex = " << vertex << std::endl; + + const double fitness = compute_fitness(vertex, points); + //std::cout << "fitness = " << fitness << std::endl; + if (fitness < best_fitness) + { + simplex_id = i; + vertex_id = j; + best_fitness = fitness; + std::cout << "best fitness = " << best_fitness << std::endl; + } + + } + } + + std::vector best_simplex = pop[simplex_id]; + Matrix temp = best_simplex[vertex_id]; + + return temp; + } + + + const Matrix points; + Population pop; +}; + + + + diff --git a/Optimal_bounding_box/include/CGAL/Optimal_bounding_box/linear_algebra.h b/Optimal_bounding_box/include/CGAL/Optimal_bounding_box/linear_algebra.h index 121e0124ebf..d78eda5fc31 100644 --- a/Optimal_bounding_box/include/CGAL/Optimal_bounding_box/linear_algebra.h +++ b/Optimal_bounding_box/include/CGAL/Optimal_bounding_box/linear_algebra.h @@ -24,6 +24,7 @@ #include #include +#include namespace CGAL { @@ -37,6 +38,17 @@ void qr_factorization(Matrix& A, Matrix& Q) Q = qr.householderQ(); } +template +void qr_factorization(std::vector& Simplex) +{ + for(int i = 0; i < Simplex.size(); ++i) + { + Eigen::HouseholderQR qr(Simplex[i]); + Matrix Q = qr.householderQ(); + Simplex[i] = Q; + } +} + template const Matrix reflection(const Matrix& S_centroid, const Matrix& S_worst) { diff --git a/Optimal_bounding_box/include/CGAL/Optimal_bounding_box/obb.h b/Optimal_bounding_box/include/CGAL/Optimal_bounding_box/obb.h index f2661db44f9..cd90c110f12 100644 --- a/Optimal_bounding_box/include/CGAL/Optimal_bounding_box/obb.h +++ b/Optimal_bounding_box/include/CGAL/Optimal_bounding_box/obb.h @@ -23,6 +23,7 @@ #define CGAL_OBB_H #include +#include #include #include #include @@ -37,25 +38,56 @@ namespace Optimal_bounding_box { -template -void find_global_minimum(Population& pop, Simplex points) +template +void evolution(Matrix& R, Matrix& points) { + std::size_t generations = 7; + std::size_t nelder_mead_iterations = 20; - genetic_algorithm(pop, points); + Population pop(50); - std::size_t nelder_mead_iterations = 1; + //std::cout << "initial pop" << std::endl; + //pop.show_population(); + //std::cout << std::endl; + //std::cin.get(); - for(Simplex s : pop) - nelder_mead(s, points, nelder_mead_iterations); + for(std::size_t t = 0; t < generations; ++t) + { + genetic_algorithm(pop, points); + + //std::cout << "pop after genetic" << std::endl; + // pop.show_population(); + // std::cout << std::endl; + //std::cin.get(); + + + for(std::size_t s = 0; s < pop.size(); ++s) + nelder_mead(pop[s], points, nelder_mead_iterations); + + //std::cout << "pop after nelder mead: " << std::endl; + //pop.show_population(); + //std::cout << std::endl; + //std::cin.get(); + + + // debugging + Fitness_map fitness_map(pop, points); + Matrix R_now = fitness_map.get_best(); + std::cout << "det= " << R_now.determinant() << std::endl; + + } + + // compute fitness of entire population + Fitness_map fitness_map(pop, points); + R = fitness_map.get_best(); - //optional random mutations } -template -void visualize_obb(Simplex data_points) -{ +template +void visualize_obb(Simplex data_points, std::string filename) +{ typedef CGAL::Simple_cartesian K; typedef K::Point_3 Point; @@ -78,13 +110,127 @@ void visualize_obb(Simplex data_points) Mesh mesh; CGAL::make_hexahedron(ic[0], ic[1], ic[2], ic[3], ic[4], ic[5], ic[6], ic[7], mesh); - std::ofstream out("/tmp/box.off"); + std::ofstream out(filename); out << mesh; out.close(); } +template +void find_and_rotate_aabb(SurfaceMesh& sm, Matrix& R, Matrix& OBB) +{ + // get vector + typedef typename boost::property_map::const_type Vpm; + typedef typename boost::property_traits::value_type Point; + typedef typename boost::graph_traits::vertex_descriptor vertex_descriptor; + Vpm vpm = get(boost::vertex_point, sm); + + std::vector points; + points.resize(vertices(sm).size()); + std::size_t i = 0; + for(vertex_descriptor v : vertices(sm)) + { + Point p = get(vpm, v); + points[i] = p; + ++i; + } + + CGAL_assertion(points.size() == vertices(sm).size()); + + + // get AABB + typedef CGAL::Simple_cartesian K; + CGAL::Bbox_3 bbox; + bbox = bbox_3(points.begin(), points.end()); + K::Iso_cuboid_3 ic(bbox); + + + + // rotate AABB -> OBB + Matrix aabb(8,3); //hexahedron + for(int i = 0; i < 8; ++i) + { + aabb(i, 0) = ic[i].x(); + aabb(i, 1) = ic[i].y(); + aabb(i, 2) = ic[i].z(); + } + + + + OBB = aabb * R.transpose(); + CGAL_assertion(OBB.cols() == aabb.cols()); + CGAL_assertion(OBB.rows() == aabb.rows()); + + //std::cout << OBB << std::endl; +} + + +template +void post_processing(Matrix& points, Matrix& R, Matrix& obb) +{ + + // 1) rotate points with R + + Matrix rotated_points(points.rows(), points.cols()); + rotated_points = points * R.transpose(); + + // 2) get AABB from rotated points + + typedef CGAL::Simple_cartesian K; + typedef K::Point_3 Point; + std::vector v_points; // Simplex -> std::vector + for(int i = 0; i < rotated_points.rows(); ++i) + { + Point p(rotated_points(i, 0), rotated_points(i, 1), rotated_points(i, 2)); + v_points.push_back(p); + } + CGAL::Bbox_3 bbox; + bbox = bbox_3(v_points.begin(), v_points.end()); + K::Iso_cuboid_3 ic(bbox); + + Matrix aabb(8, 3); + for(int i = 0; i < 8; ++i) + { + aabb(i, 0) = ic[i].x(); + aabb(i, 1) = ic[i].y(); + aabb(i, 2) = ic[i].z(); + } + + // 3) apply inverse rotation to rotated AABB + + obb = aabb * R; + + +} + +template +void matrix_to_mesh_and_draw(Matrix& data_points, std::string filename) +{ + + typedef CGAL::Simple_cartesian K; + typedef K::Point_3 Point; + typedef CGAL::Surface_mesh Mesh; + + // Simplex -> std::vector + std::vector points; + + for(int i = 0; i < data_points.rows(); ++i) + { + Point p(data_points(i, 0), data_points(i, 1), data_points(i, 2)); + points.push_back(p); + } + + + Mesh mesh; + CGAL::make_hexahedron(points[0], points[1], points[2], points[3], points[4], points[5], + points[6], points[7], mesh); + + std::ofstream out(filename); + out << mesh; + out.close(); + +} diff --git a/Optimal_bounding_box/include/CGAL/Optimal_bounding_box/optimization_algorithms.h b/Optimal_bounding_box/include/CGAL/Optimal_bounding_box/optimization_algorithms.h index 4bc152a72db..980b6c10491 100644 --- a/Optimal_bounding_box/include/CGAL/Optimal_bounding_box/optimization_algorithms.h +++ b/Optimal_bounding_box/include/CGAL/Optimal_bounding_box/optimization_algorithms.h @@ -226,6 +226,13 @@ void genetic_algorithm(Population& pop, Simplex& points) + /* + check_det(group1); + check_det(group2); + check_det(group3); + check_det(group4); + */ + // crossover I Population offspringsA(size_first_group); @@ -253,6 +260,14 @@ void genetic_algorithm(Population& pop, Simplex& points) } + /* + std::cout << "offspringsA: \n" ; + check_det(offspringsA); + std::cin.get(); + */ + + + // crossover II Population offspringsB(size_second_group); bias = 0.1; @@ -273,10 +288,22 @@ void genetic_algorithm(Population& pop, Simplex& points) offspring[j] = lambda * group3[i][j] + lambda * group4[i][j]; } + + // qr factorization of the offspring + qr_factorization(offspring); offspringsB[i] = offspring; } + + + /* + std::cout << "offspringsB: \n" ; + check_det(offspringsB); + std::cin.get(); + */ + + CGAL_assertion(offspringsA.size() == size_first_group); CGAL_assertion(offspringsB.size() == size_second_group); CGAL_assertion(offspringsA.size() + offspringsB.size() == pop.size()); @@ -297,6 +324,18 @@ void genetic_algorithm(Population& pop, Simplex& points) } +template +void check_det(Population& pop) +{ + for(int i = 0; i < pop.size(); ++i) + { + for(int j = 0; j < 4; ++j) + { + auto A = pop[i][j]; + std::cout << A.determinant() << std::endl; + } + } +} diff --git a/Optimal_bounding_box/include/CGAL/Optimal_bounding_box/population.h b/Optimal_bounding_box/include/CGAL/Optimal_bounding_box/population.h index 2d952554602..76231016eba 100644 --- a/Optimal_bounding_box/include/CGAL/Optimal_bounding_box/population.h +++ b/Optimal_bounding_box/include/CGAL/Optimal_bounding_box/population.h @@ -82,7 +82,7 @@ public: } - // access individual + // access simplex Individual& operator[](std::size_t i) { CGAL_assertion(i < n); @@ -95,7 +95,6 @@ public: return pop[i]; } - private: // create random population @@ -132,6 +131,8 @@ private: + Individual somebody; //temp + CGAL::Random random_generator; std::size_t n; diff --git a/Optimal_bounding_box/test/Optimal_bounding_box/data/tetra3.off b/Optimal_bounding_box/test/Optimal_bounding_box/data/tetra3.off new file mode 100644 index 00000000000..d83aa004308 --- /dev/null +++ b/Optimal_bounding_box/test/Optimal_bounding_box/data/tetra3.off @@ -0,0 +1,12 @@ +OFF +4 4 0 + +0 1 0 +1 0 0 +0 0 0 +0 0 1 +3 0 1 2 +3 2 3 0 +3 1 3 2 +3 0 3 1 + diff --git a/Optimal_bounding_box/test/Optimal_bounding_box/test_optimization_algorithms.cpp b/Optimal_bounding_box/test/Optimal_bounding_box/test_optimization_algorithms.cpp index 78c00d2032f..a29b7c092eb 100644 --- a/Optimal_bounding_box/test/Optimal_bounding_box/test_optimization_algorithms.cpp +++ b/Optimal_bounding_box/test/Optimal_bounding_box/test_optimization_algorithms.cpp @@ -13,6 +13,9 @@ typedef Eigen::MatrixXf MatrixXf; +typedef CGAL::Exact_predicates_inexact_constructions_kernel K; + + bool assert_doubles(double d1, double d2, double epsilon) { return (d1 < d2 + epsilon && d1 > d2 - epsilon) ? true : false; @@ -131,29 +134,144 @@ void test_genetic_algorithm() CGAL_assertion(pop.size() == 5); } -void test_visualization() + + + +void find_obb() { - MatrixXf data_points(4, 3); + + MatrixXf data_points(4, 3); // there are on the convex hull data_points << 0.866802, 0.740808, 0.895304, 0.912651, 0.761565, 0.160330, 0.093661, 0.892578, 0.737412, 0.166461, 0.149912, 0.364944; - CGAL::Optimal_bounding_box::visualize_obb(data_points); + CGAL::Optimal_bounding_box::visualize_obb(data_points, "/tmp/original.off"); + + + MatrixXf rotation(3, 3); + CGAL::Optimal_bounding_box::evolution(rotation, data_points); + + // rotate + MatrixXf rotated_points(4, 3); + rotated_points = data_points * rotation.transpose(); + + CGAL_assertion(rotated_points.cols() == data_points.cols()); + CGAL_assertion(rotated_points.rows() == data_points.rows()); + + std::cout << "rotation matrix= \n" << rotation << std::endl << std::endl; + std::cout << "rotated_points= \n" << rotated_points << std::endl; + + CGAL::Optimal_bounding_box::visualize_obb(rotated_points, "/tmp/rotated.off"); } + + +template +void sm_to_matrix(SurfaceMesh& sm, Matrix& mat) +{ + typedef typename boost::property_map::const_type Vpm; + typedef typename boost::property_traits::reference Point_ref; + typedef typename boost::graph_traits::vertex_descriptor vertex_descriptor; + Vpm vpm = get(boost::vertex_point, sm); + + mat.resize(vertices(sm).size(), 3); + std::size_t i = 0; + for(vertex_descriptor v : vertices(sm)) + { + Point_ref p = get(vpm, v); + mat(i, 0) = p.x(); + mat(i, 1) = p.y(); + mat(i, 2) = p.z(); + ++i; + } +} + + +void test_tetrahedron(const char* fname) +{ + std::ifstream input(fname); + CGAL::Surface_mesh mesh; + if (!input || !(input >> mesh) || mesh.is_empty()) { + std::cerr << fname << " is not a valid off file.\n"; + exit(1); + } + + // points in a matrix + MatrixXf points; + sm_to_matrix(mesh, points); + + MatrixXf R(3, 3); + CGAL::Optimal_bounding_box::evolution(R, points); + std:: cout << "R= " << R << std::endl; + std:: cout << "det(evolution)= " << R.determinant() << std::endl; + + // postprocessing + MatrixXf obb(8, 3); + CGAL::Optimal_bounding_box::post_processing(points, R, obb); + CGAL::Optimal_bounding_box::matrix_to_mesh_and_draw(obb, "data/OBB.off"); + + +} + + +void rotate_tetrahedron(const char* fname, const char* Rname) +{ + + std::ifstream input(fname); + CGAL::Surface_mesh mesh; + if (!input || !(input >> mesh) || mesh.is_empty()) { + std::cerr << fname << " is not a valid off file.\n"; + exit(1); + } + + MatrixXf R(3, 3); + std::ifstream input_R(Rname); + double x, y, z; + std::size_t i = 0; + while (input_R >> x >> y >> z) + { + R(i, 0) = x; + R(i, 1) = y; + R(i, 2) = z; + ++i; + } + std:: cout << "det(benchmark)= " << R.determinant() << std::endl; + + + // points in a matrix + MatrixXf points; + sm_to_matrix(mesh, points); + + // just rotate once + //MatrixXf rotated_points = points * R.transpose(); + //CGAL::Optimal_bounding_box::visualize_obb(rotated_points, "data/rotated_points_benchmark.off"); + + + // postprocessing + MatrixXf obb(8, 3); + CGAL::Optimal_bounding_box::post_processing(points, R, obb); + CGAL::Optimal_bounding_box::matrix_to_mesh_and_draw(obb, "data/inverse_rotated.off"); + + +} + + + int main() { - test_population(); - test_nelder_mead(); - test_genetic_algorithm(); + //test_population(); + //test_nelder_mead(); + //test_genetic_algorithm(); + //find_obb(); - test_visualization(); + test_tetrahedron("data/random_tetra.off"); + //rotate_tetrahedron("data/random_tetra.off", "data/rotation.dat");