it works!

Optimal_bounding_box/include/CGAL/Optimal_bounding_box/fitness_function.h

@@ -24,7 +24,8 @@
 
 #include <CGAL/Bbox_3.h>
 #include <vector>
-
+#include <CGAL/Optimal_bounding_box/population.h>
+#include <limits>
 
 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 <typename Matrix>
+struct Fitness_map // -> a free function
+{
+  Fitness_map(Population<Matrix>& 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<int>::max();
+    for(std::size_t i = 0; i < pop.size(); ++i)
+    {
+      const std::vector<Matrix> 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<Matrix> best_simplex = pop[simplex_id];
+    Matrix temp = best_simplex[vertex_id];
+
+    return temp;
+  }
+
+
+  const Matrix points;
+  Population<Matrix> pop;
+};
+
+
+
+
 
 
 

Optimal_bounding_box/include/CGAL/Optimal_bounding_box/linear_algebra.h

@@ -24,6 +24,7 @@
 
 #include <CGAL/assertions.h>
 #include <Eigen/QR>
+#include <vector>
 
 
 namespace CGAL {
@@ -37,6 +38,17 @@ void qr_factorization(Matrix& A, Matrix& Q)
   Q = qr.householderQ();
 }
 
+template<typename Matrix>
+void qr_factorization(std::vector<Matrix>& Simplex)
+{
+  for(int i = 0; i < Simplex.size(); ++i)
+  {
+    Eigen::HouseholderQR<Matrix> qr(Simplex[i]);
+    Matrix Q = qr.householderQ();
+    Simplex[i] = Q;
+  }
+}
+
 template <typename Matrix>
 const Matrix reflection(const Matrix& S_centroid, const Matrix& S_worst)
 {

Optimal_bounding_box/include/CGAL/Optimal_bounding_box/obb.h

@@ -23,6 +23,7 @@
 #define CGAL_OBB_H
 
 #include <CGAL/Optimal_bounding_box/optimization_algorithms.h>
+#include <CGAL/Optimal_bounding_box/population.h>
 #include <vector>
 #include <fstream>
 #include <CGAL/Surface_mesh.h>
@@ -37,25 +38,56 @@ namespace Optimal_bounding_box {
 
 
 
-template <typename Simplex>
+template <typename Matrix>
-void find_global_minimum(Population<Simplex>& pop, Simplex points)
+void evolution(Matrix& R, Matrix& points)
 {
+  std::size_t generations = 7;
+  std::size_t nelder_mead_iterations = 20;
 
+  Population<Matrix> pop(50);
+
+  //std::cout << "initial pop" << std::endl;
+  //pop.show_population();
+  //std::cout << std::endl;
+  //std::cin.get();
+
+  for(std::size_t t = 0; t < generations; ++t)
+  {
     genetic_algorithm(pop, points);
 
-  std::size_t nelder_mead_iterations = 1;
+    //std::cout << "pop after genetic" << std::endl;
+   // pop.show_population();
+   // std::cout << std::endl;
+    //std::cin.get();
 
-  for(Simplex s : pop)
-    nelder_mead(s, points, nelder_mead_iterations);
 
-  //optional random mutations
+    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<Matrix> 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<Matrix> fitness_map(pop, points);
+  R = fitness_map.get_best();
+
 }
 
 
-template <typename Simplex>
-void visualize_obb(Simplex data_points)
-{
 
+template <typename Simplex>
+void visualize_obb(Simplex data_points, std::string filename)
+{
 
   typedef CGAL::Simple_cartesian<double> 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 <typename SurfaceMesh, typename Matrix>
+void find_and_rotate_aabb(SurfaceMesh& sm, Matrix& R, Matrix& OBB)
+{
+  // get vector<points>
+  typedef typename boost::property_map<SurfaceMesh, boost::vertex_point_t>::const_type Vpm;
+  typedef typename boost::property_traits<Vpm>::value_type Point;
+  typedef typename boost::graph_traits<SurfaceMesh>::vertex_descriptor vertex_descriptor;
+  Vpm vpm = get(boost::vertex_point, sm);
+
+  std::vector<Point> 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<double> 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 <typename Matrix>
+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<double> K;
+  typedef K::Point_3 Point;
+  std::vector<Point> 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 <typename Matrix>
+void matrix_to_mesh_and_draw(Matrix& data_points, std::string filename)
+{
+
+  typedef CGAL::Simple_cartesian<double> K;
+  typedef K::Point_3 Point;
+  typedef CGAL::Surface_mesh<Point> Mesh;
+
+  // Simplex -> std::vector
+  std::vector<Point> 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();
+
+}
 
 
 

Optimal_bounding_box/include/CGAL/Optimal_bounding_box/optimization_algorithms.h

@@ -226,6 +226,13 @@ void genetic_algorithm(Population<Simplex>& pop, Simplex& points)
 
 
 
+  /*
+  check_det(group1);
+  check_det(group2);
+  check_det(group3);
+  check_det(group4);
+  */
+
 
   // crossover I
   Population<Simplex> offspringsA(size_first_group);
@@ -253,6 +260,14 @@ void genetic_algorithm(Population<Simplex>& pop, Simplex& points)
   }
 
 
+  /*
+  std::cout << "offspringsA: \n" ;
+  check_det(offspringsA);
+  std::cin.get();
+  */
+
+
+
   // crossover II
   Population<Simplex> offspringsB(size_second_group);
   bias = 0.1;
@@ -273,10 +288,22 @@ void genetic_algorithm(Population<Simplex>& 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<Simplex>& pop, Simplex& points)
 }
 
 
+template <typename Simplex>
+void check_det(Population<Simplex>& 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;
+    }
+  }
+}
 
 
 

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;

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
+

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 <typename SurfaceMesh, typename Matrix>
+void sm_to_matrix(SurfaceMesh& sm, Matrix& mat)
+{
+  typedef typename boost::property_map<SurfaceMesh, boost::vertex_point_t>::const_type Vpm;
+  typedef typename boost::property_traits<Vpm>::reference Point_ref;
+  typedef typename boost::graph_traits<SurfaceMesh>::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<K::Point_3> 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<K::Point_3> 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_population();
-  test_nelder_mead();
+  //test_nelder_mead();
-  test_genetic_algorithm();
+  //test_genetic_algorithm();
+  //find_obb();
 
 
-  test_visualization();
+  test_tetrahedron("data/random_tetra.off");
+  //rotate_tetrahedron("data/random_tetra.off", "data/rotation.dat");