From 143041369acbf4329b9e1fd6204dac8683cc6098 Mon Sep 17 00:00:00 2001 From: Jackson Campolattaro Date: Thu, 23 Jul 2020 10:42:08 -0400 Subject: [PATCH] Reinstate original RANSAC tree code --- .../Shape_detection/Efficient_RANSAC/Octree.h | 705 +++++++++++++++++- 1 file changed, 699 insertions(+), 6 deletions(-) diff --git a/Shape_detection/include/CGAL/Shape_detection/Efficient_RANSAC/Octree.h b/Shape_detection/include/CGAL/Shape_detection/Efficient_RANSAC/Octree.h index 9e024de7ad8..1f83a9b156c 100644 --- a/Shape_detection/include/CGAL/Shape_detection/Efficient_RANSAC/Octree.h +++ b/Shape_detection/include/CGAL/Shape_detection/Efficient_RANSAC/Octree.h @@ -15,17 +15,710 @@ #define CGAL_SHAPE_DETECTION_EFFICIENT_RANSAC_OCTREE_H #include -#include + +#include +#include + +#include +#include +#include + +extern int scoreTime; namespace CGAL { -namespace Shape_detection { + namespace Shape_detection { -template -class Octree : CGAL::Octree::Octree { + template + class Efficient_RANSAC; -}; + namespace internal { + + const std::size_t size_t_max = (std::numeric_limits::max)(); + + template + class DirectPointAccessor { + public: + typedef Sdt Sd_traits; + typedef typename Sd_traits::Input_range::iterator Input_iterator; + + DirectPointAccessor() {} + DirectPointAccessor(const Input_iterator &begin, + const Input_iterator &beyond, + std::size_t offset) : m_first(begin), m_offset(offset) { + m_beyond = (beyond == begin) ? begin : beyond - 1; + } + + Input_iterator at(std::size_t i) { + return m_first + i; + } + + std::size_t index(std::size_t i) { + return i + m_offset; + } + + std::size_t offset() { + return m_offset; + } + + std::size_t size() { + return m_beyond - m_first + 1; + } + + Input_iterator first() { + return m_first; + } + + Input_iterator beyond() { + return m_beyond; + } + + void setData(Input_iterator &begin, Input_iterator &beyond) { + m_beyond = (beyond == begin) ? begin : beyond - 1; + } + + void swap(std::size_t a, std::size_t b) { + typename std::iterator_traits::value_type tmp; + tmp = m_first[a]; + m_first[a] = m_first[b]; + m_first[b] = tmp; + } + + protected: + Input_iterator m_first; + + private: + Input_iterator m_beyond; + std::size_t m_offset; + }; + + template + class IndexedPointAccessor { + public: + typedef Sdt Sd_traits; + typedef typename Sd_traits::Input_range::iterator Input_iterator; + + IndexedPointAccessor() {} + IndexedPointAccessor(const Input_iterator &begin, + const Input_iterator &beyond, std::size_t) + : m_first(begin) { + m_beyond = (beyond == begin) ? begin : beyond - 1; + m_indices.resize(size()); + for (std::size_t i = 0;i m_indices; + Input_iterator m_beyond; + }; + + template + class Octree : public PointAccessor { + + typedef typename PointAccessor::Sd_traits Sd_traits; + typedef typename Sd_traits::Input_range::iterator Input_iterator; + typedef Shape_base Shape; + typedef typename Sd_traits::Point_3 Point_3; + typedef typename Sd_traits::Vector_3 Vector_3; + typedef typename Sd_traits::FT FT; + typedef typename Sd_traits::Point_map Point_map; + typedef typename Sd_traits::Normal_map Normal_map; + + template + friend class ::CGAL::Shape_detection::Efficient_RANSAC; + + struct Cell { + std::size_t first, last; + Cell *child[8]; + Point_3 center; + std::size_t level; + + Cell(std::size_t first, std::size_t last, Point_3 center, std::size_t level) + : first(first), last(last), center(center), level(level) { + memset(child, 0, sizeof(Cell *) * 8); + } + + bool isLeaf() const { + for (std::size_t i = 0;i<8;i++) { + if (child[i]) + return false; + } + return true; + } + + std::size_t size() const { + if (first == size_t_max || last == size_t_max) + return 0; + else return (last - first + 1); + } + }; + + // -------------------------------------------------------------------------- + // Utilities + // -------------------------------------------------------------------------- + FT get_x(const Vector_3& v){ return m_traits.compute_x_3_object()(v); } + FT get_y(const Vector_3& v){ return m_traits.compute_y_3_object()(v); } + FT get_z(const Vector_3& v){ return m_traits.compute_z_3_object()(v); } + FT get_x(const Point_3& p){ return m_traits.compute_x_3_object()(p); } + FT get_y(const Point_3& p){ return m_traits.compute_y_3_object()(p); } + FT get_z(const Point_3& p){ return m_traits.compute_z_3_object()(p); } + FT get_coord(const Point_3& p, unsigned int d) + { + CGAL_assertion(d < 3); + switch (d) + { + case 0: return get_x(p); + case 1: return get_y(p); + case 2: return get_z(p); + default: return FT(0); + } + } + + Point_3 constr_pt(FT x, FT y, FT z) const + { return m_traits.construct_point_3_object()(x, y, z); } + Vector_3 constr_vec(const Point_3& p, const Point_3& q) const + { return m_traits.construct_vector_3_object()(p, q); } + + Point_3 transl(const Point_3& p, const Vector_3 &v) + { return m_traits.construct_translated_point_3_object()(p, v); } + + public: + Octree(Sd_traits const& traits) + : m_traits(traits), m_bucket_size(20), m_set_max_level(10), m_root(nullptr) {} + Octree(Sd_traits const& traits, + const Input_iterator &first, + const Input_iterator &beyond, + Point_map& point_pmap, + Normal_map& normal_pmap, + std::size_t offset = 0, + std::size_t bucketSize = 20, + std::size_t maxLevel = 10) + : PointAccessor(first, beyond, offset), + m_traits(traits), + m_root(nullptr), + m_bucket_size(bucketSize), + m_set_max_level(maxLevel), + m_point_pmap (point_pmap), + m_normal_pmap (normal_pmap) {} + + ~Octree() { + if (!m_root) + return; + + std::stack stack; + stack.push(m_root); + while (!stack.empty()) { + Cell *cell = stack.top(); + stack.pop(); + + for (std::size_t i = 0;i<8;i++) + if (cell->child[i]) + stack.push(cell->child[i]); + + delete cell; + } + } + + // Sorting data in a way such that points of one cell + // are always in one range and ordered child-wise: + // +---+---+ + // | 1.| 0.| + // +---+---+ + // | 3.| 2.| + // +---+---+ + // z max before z min, then y max before y min, then x max before x min + void createTree(double cluster_epsilon_for_max_level_recomputation = -1.) { + buildBoundingCube(); + std::size_t count = 0; + m_max_level = 0; + + if (cluster_epsilon_for_max_level_recomputation > 0.) + { + FT bbox_diagonal = (FT) CGAL::sqrt( + (m_bBox.xmax() - m_bBox.xmin()) * (m_bBox.xmax() - m_bBox.xmin()) + + (m_bBox.ymax() - m_bBox.ymin()) * (m_bBox.ymax() - m_bBox.ymin()) + + (m_bBox.zmax() - m_bBox.zmin()) * (m_bBox.zmax() - m_bBox.zmin())); + + m_set_max_level = std::size_t (std::log (bbox_diagonal + / cluster_epsilon_for_max_level_recomputation) + / std::log (2.0)); + } + + std::stack stack; + m_root = new Cell(0, this->size() - 1, m_center, 0); + stack.push(m_root); + while (!stack.empty()) { + Cell *cell= stack.top(); + stack.pop(); + + m_max_level = std::max(m_max_level, cell->level); + if (cell->level == m_set_max_level) + continue; + + std::size_t zLowYHighXSplit, zLowYLowXSplit, zLowYSplit; + std::size_t zHighYSplit, zHighYHighXSplit, zHighYLowXSplit; + + std::size_t zSplit = split(cell->first, cell->last, 2, get_z(cell->center)); + + if (zSplit != size_t_max) { + + zLowYSplit = split(cell->first, zSplit, 1, get_y(cell->center)); + if (zLowYSplit != size_t_max) { + zLowYLowXSplit = split(cell->first, + zLowYSplit, 0, get_x(cell->center)); + zLowYHighXSplit = split(zLowYSplit + 1, + zSplit, 0, get_x(cell->center)); + } + else { + zLowYLowXSplit = size_t_max; + zLowYHighXSplit = split(cell->first, zSplit, 0, get_x(cell->center)); + } + + zHighYSplit = split(zSplit + 1, cell->last, 1, get_y(cell->center)); + + if (zHighYSplit != size_t_max) { + zHighYHighXSplit = split(zHighYSplit + 1, + cell->last, 0, get_x(cell->center)); + zHighYLowXSplit = split(zSplit + 1, + zHighYSplit, 0, get_x(cell->center)); + } + else { + zHighYLowXSplit = size_t_max; + zHighYHighXSplit = split(zSplit + 1, + cell->last, 0, get_x(cell->center)); + } + } + else { + zLowYSplit = size_t_max; + zLowYLowXSplit = size_t_max; + zLowYHighXSplit = size_t_max; + + zHighYSplit = split(cell->first, + cell->last, + 1, + get_y(cell->center)); + + if (zHighYSplit != size_t_max) { + zHighYHighXSplit = split(zHighYSplit + 1, + cell->last, + 0, + get_x(cell->center)); + + zHighYLowXSplit = split(cell->first, + zHighYSplit, + 0, + get_x(cell->center)); + } + else { + zHighYLowXSplit = size_t_max; + zHighYHighXSplit = split(cell->first, + cell->last, + 0, + get_x(cell->center)); + } + } + + FT width = m_width / (1<<(cell->level + 1)); + + if (zSplit != size_t_max) { + if (zLowYSplit != size_t_max) { + if (zLowYLowXSplit != size_t_max) { + + if (cell->first <= zLowYLowXSplit) { + //--- + cell->child[7] = new Cell(cell->first, + zLowYLowXSplit, + transl(cell->center, constr_vec( + ORIGIN, constr_pt(-width,-width,-width))), + cell->level + 1); + + if (cell->child[7]->size() > m_bucket_size) + stack.push(cell->child[7]); + } + } + else zLowYLowXSplit = cell->first - 1; + + if (zLowYLowXSplit < zLowYSplit || zLowYLowXSplit == size_t_max) { + //+-- + cell->child[6] = new Cell(zLowYLowXSplit + 1, + zLowYSplit, + transl(cell->center, constr_vec( + ORIGIN, constr_pt(width,-width,-width))), + cell->level + 1); + + if (cell->child[6]->size() > m_bucket_size) + stack.push(cell->child[6]); + } + } + else zLowYSplit = cell->first - 1; + + if (zLowYHighXSplit != size_t_max) { + + if (zLowYSplit < zLowYHighXSplit || zLowYSplit == size_t_max) { + //-+- + cell->child[5] = new Cell(zLowYSplit + 1, + zLowYHighXSplit, + transl(cell->center, constr_vec( + ORIGIN, constr_pt(-width, width,-width))), + cell->level + 1); + + if (cell->child[5]->size() > m_bucket_size) + stack.push(cell->child[5]); + } + } + else zLowYHighXSplit = zLowYSplit; + + if (zLowYHighXSplit < zSplit || zLowYHighXSplit == size_t_max) { + //++- + cell->child[4] = new Cell(zLowYHighXSplit + 1, + zSplit, + transl(cell->center, constr_vec( + ORIGIN, constr_pt(width, width,-width))), + cell->level + 1); + + if (cell->child[4]->size() > m_bucket_size) + stack.push(cell->child[4]); + } + } + else zSplit = cell->first - 1; + + if (zHighYSplit != size_t_max) { + if (zHighYLowXSplit != size_t_max) { + + if (zSplit < zHighYLowXSplit || zSplit == size_t_max) { + //--+ + cell->child[3] = new Cell(zSplit + 1, + zHighYLowXSplit, + transl(cell->center, constr_vec( + ORIGIN, constr_pt(-width,-width, width))), + cell->level + 1); + + if (cell->child[3]->size() > m_bucket_size) + stack.push(cell->child[3]); + } + } + else zHighYLowXSplit = zSplit; + + if (zHighYLowXSplit < zHighYSplit || zHighYLowXSplit == size_t_max) { + //+-+ + cell->child[2] = new Cell(zHighYLowXSplit + 1, + zHighYSplit, + transl(cell->center, constr_vec( + ORIGIN, constr_pt(width,-width, width))), + cell->level + 1); + + if (cell->child[2]->size() > m_bucket_size) + stack.push(cell->child[2]); + } + + } + else zHighYSplit = zSplit; + + if (zHighYHighXSplit != size_t_max) { + if (zHighYSplit < zHighYHighXSplit || zHighYSplit == size_t_max) { + //-++ + cell->child[1] = new Cell(zHighYSplit + 1, + zHighYHighXSplit, + transl(cell->center, constr_vec( + ORIGIN, constr_pt(-width, width, width))), + cell->level + 1); + + if (cell->child[1]->size() > m_bucket_size) + stack.push(cell->child[1]); + } + } + else zHighYHighXSplit = zHighYSplit; + + if (zHighYHighXSplit <= cell->last || zHighYHighXSplit == size_t_max) { + if (zHighYHighXSplit < cell->last || zHighYHighXSplit == size_t_max) { + //+++ + cell->child[0] = new Cell(zHighYHighXSplit + 1, + cell->last, + transl(cell->center, constr_vec( + ORIGIN, constr_pt(width, width, width))), + cell->level + 1); + + if (cell->child[0]->size() > m_bucket_size) + stack.push(cell->child[0]); + } + } + + std::size_t sum = 0; + for (std::size_t i = 0;i<8;i++) + sum += (cell->child[i]) ? cell->child[i]->size() : 0; + + count++; + } + } + + bool drawSamplesFromCellContainingPoint(const Point_3 &p, + std::size_t level, + std::set& indices, + const std::vector& shapeIndex, + std::size_t requiredSamples) { + + bool upperZ, upperY, upperX; + Cell *cur = m_root; + + while (cur && cur->level < level) { + upperX = get_x(cur->center) <= get_x(p); + upperY = get_y(cur->center) <= get_y(p); + upperZ = get_z(cur->center) <= get_z(p); + + if (upperZ) { + if (upperY) + cur = (upperX) ? cur->child[0] : cur->child[1]; + else cur = (upperX) ? cur->child[2] : cur->child[3]; + } + else { + if (upperY) + cur = (upperX) ? cur->child[4] : cur->child[5]; + else cur = (upperX) ? cur->child[6] : cur->child[7]; + } + } + + if (cur) { + std::size_t enough = 0; + for (std::size_t i = cur->first;i<=cur->last;i++) { + std::size_t j = this->index(i); + if (shapeIndex[j] == -1) { + enough++; + if (enough >= requiredSamples) + break; + } + } + if (enough >= requiredSamples) { + do { + std::size_t p = CGAL::get_default_random(). + uniform_int(0, cur->size() - 1); + std::size_t j = this->index(cur->first + p); + if (shapeIndex[j] == -1) + indices.insert(j); + } while (indices.size() < requiredSamples); + + return true; + } + else return false; + } + else return false; + } + + std::size_t maxLevel() { + return m_max_level; + } + + std::size_t fullScore(Shape *candidate, + std::vector &shapeIndex, + FT epsilon, + FT normal_threshold) { + + std::vector indices(m_root->size()); + for (std::size_t i = 0;isize();i++) { + indices[i] = index(m_root->first + i); + } + + candidate->cost_function(this->begin() + m_root->first, + this->begin() + m_root->last, + shapeIndex, + epsilon, + normal_threshold, + indices); + + return candidate->m_indices.size(); + } + + std::size_t score(Shape *candidate, + std::vector &shapeIndex, + FT epsilon, + FT normal_threshold) { + + std::stack stack; + stack.push(m_root); + + while(!stack.empty()) { + Cell *cell = stack.top(); + stack.pop(); + + FT width = m_width / (1<<(cell->level)); + + FT diag = CGAL::sqrt(FT(3) * width * width) + epsilon; + + FT dist = candidate->squared_distance(cell->center); + + if (dist > (diag * diag)) + continue; + + // differ between full or partial overlap? + // if full overlap further traversal of this branch is not necessary + if (cell->isLeaf()) { + std::vector indices; + indices.reserve(cell->size()); + for (std::size_t i = 0;isize();i++) { + if (shapeIndex[this->index(cell->first + i)] == -1) { + indices.push_back(this->index(cell->first + i)); + } + } + + candidate->cost_function(epsilon, + normal_threshold, + indices); + } + else { + for (std::size_t i = 0;i<8;i++) + if (cell->child[i]) + stack.push(cell->child[i]); + } + + } + + return candidate->m_indices.size(); + } + + void setBucketSize(std::size_t bucketSize) { + m_bucket_size = bucketSize; + } + + const Bbox_3 &boundingBox() { + return m_bBox; + } + + const Bbox_3 &buildBoundingCube() { + FT min[] = {std::numeric_limits::infinity(), + std::numeric_limits::infinity(), + std::numeric_limits::infinity()}; + FT max[] = {-std::numeric_limits::infinity(), + -std::numeric_limits::infinity(), + -std::numeric_limits::infinity()}; + + for (std::size_t i = 0;isize();i++) { + Point_3 p = get(m_point_pmap, *this->at(i)); + min[0] = (std::min)(min[0], get_x(p)); + min[1] = (std::min)(min[1], get_y(p)); + min[2] = (std::min)(min[2], get_z(p)); + max[0] = (std::max)(max[0], get_x(p)); + max[1] = (std::max)(max[1], get_y(p)); + max[2] = (std::max)(max[2], get_z(p)); + } + + m_bBox = Bbox_3(min[0], min[1], min[2], max[0], max[1], max[2]); + + m_width = (std::max)(max[0] - min[0], + (std::max)(max[1] - min[1], max[2] - min[2])) * (FT) 0.5; + m_center = constr_pt((min[0] + max[0]) * (FT) 0.5, + (min[1] + max[1]) * (FT) 0.5, + (min[2] + max[2]) * (FT) 0.5); + + return m_bBox; + } + + // returns index of last point below threshold + std::size_t split(std::size_t first, std::size_t last, std::size_t dimension, FT threshold) { + if (last == size_t_max || first == size_t_max) + return size_t_max; + + if (first > last) + return first - 1; + + std::size_t origFirst = first; + + while(first < last) { + // find first above threshold + while (get_coord( + get(m_point_pmap, *this->at(first)), + static_cast(dimension)) < threshold + && first < last) { + first++; + } + + // check if last has been reached + if (first == last) { + return (get_coord( + get(m_point_pmap, *this->at(first)), + static_cast(dimension)) < threshold) ? + first : (first == origFirst) ? size_t_max : first - 1; + } + + // find last below threshold + while (get_coord( + get(m_point_pmap, *this->at(last)), + static_cast(dimension)) >= threshold + && last > first) { + last--; + } + + // check if first has been reached + if (last == first) { + return (get_coord( + get(m_point_pmap, *this->at(first)), + static_cast(dimension)) < threshold) ? + first : (first == origFirst) ? size_t_max : first - 1; + } + + this->swap(first, last); + first++; + last--; + } + + return (get_coord( + get(m_point_pmap, *this->at(first)), + static_cast(dimension)) < threshold) ? + first : (first == origFirst) ? size_t_max : first - 1; + } + + Sd_traits m_traits; + Bbox_3 m_bBox; + Cell *m_root; + Point_3 m_center; + FT m_width; + std::size_t m_bucket_size; + std::size_t m_set_max_level; + std::size_t m_max_level; + Point_map m_point_pmap; + Normal_map m_normal_pmap; + }; + } + } -} } #endif