songsenand
/
cgal
mirror of https://github.com/CGAL/cgal
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
cgal/Kinetic_space_partition/include/CGAL/Kinetic_shape_reconstructio...

1914 lines
67 KiB
C++
Raw Blame History

// Copyright (c) 2023 GeometryFactory Sarl (France).
// All rights reserved.
//
// This file is part of CGAL (www.cgal.org).
//
// $URL$
// $Id$
// SPDX-License-Identifier: GPL-3.0-or-later OR LicenseRef-Commercial
//
//
// Author(s) : Sven Oesau, Florent Lafarge, Dmitry Anisimov, Simon Giraudot
#ifndef CGAL_KINETIC_SHAPE_RECONSTRUCTION_3_H
#define CGAL_KINETIC_SHAPE_RECONSTRUCTION_3_H
#include <CGAL/license/Kinetic_shape_partition.h>
#include <CGAL/Kinetic_shape_partition_3.h>
#include <CGAL/KSP_3/Graphcut.h>
#include <CGAL/IO/PLY.h>
#include <CGAL/Point_set_3.h>
#include <CGAL/Point_set_3/IO.h>
#include <CGAL/Shape_detection/Region_growing/Region_growing.h>
#include <CGAL/Shape_detection/Region_growing/Point_set.h>
#include <CGAL/Delaunay_triangulation_2.h>
#include <CGAL/Delaunay_triangulation_3.h>
#include <CGAL/Triangulation_face_base_with_info_2.h>
#include <CGAL/Triangulation_vertex_base_with_info_2.h>
#include <CGAL/KSP/debug.h>
#include <CGAL/Shape_regularization/regularize_planes.h>
#include <CGAL/bounding_box.h>
#include <boost/range/adaptor/transformed.hpp>
#include <CGAL/AABB_face_graph_triangle_primitive.h>
#include <CGAL/AABB_tree.h>
#include <CGAL/AABB_traits.h>
#include <CGAL/boost/graph/helpers.h>
namespace CGAL
{
#ifndef DOXYGEN_RUNNING
/*!
* \ingroup PkgKineticShapePartition
\brief Piece-wise linear reconstruction via inside/outside labeling of a kinetic partition using graph cut.
\tparam GeomTraits
must be a model of `KineticPartitionTraits`.
\tparam NormalMap
must be a model of `ConstRange` whose iterator type is `RandomAccessIterator`. It must map the elements in `KineticShapePartitionTraits_3::Input_range` to `Vector_3`.
*/
template<typename GeomTraits, typename PointSet, typename PointMap, typename NormalMap, typename IntersectionKernel = CGAL::Exact_predicates_exact_constructions_kernel>
class Kinetic_shape_reconstruction_3 {
public:
using Kernel = GeomTraits;
using Intersection_kernel = IntersectionKernel;
using FT = typename Kernel::FT;
using Point_2 = typename Kernel::Point_2;
using Point_3 = typename Kernel::Point_3;
using Vector_3 = typename Kernel::Vector_3;
using Plane_3 = typename Kernel::Plane_3;
using Triangle_2 = typename Kernel::Triangle_2;
using Point_set = PointSet;
using Indices = std::vector<std::size_t>;
using Polygon_3 = std::vector<Point_3>;
using KSP = Kinetic_shape_partition_3<Kernel, Intersection_kernel>;
using Point_map = PointMap;
using Normal_map = NormalMap;
using Region_type = CGAL::Shape_detection::Point_set::Least_squares_plane_fit_region_for_point_set<Point_set>;
using Neighbor_query = CGAL::Shape_detection::Point_set::K_neighbor_query_for_point_set<Point_set>;
using Sorting = CGAL::Shape_detection::Point_set::Least_squares_plane_fit_sorting_for_point_set<Point_set, Neighbor_query>;
using Region_growing = CGAL::Shape_detection::Region_growing<Neighbor_query, Region_type>;
using From_exact = typename CGAL::Cartesian_converter<Intersection_kernel, Kernel>;
/*!
\brief Creates a Kinetic_shape_reconstruction_3 object.
\param ps
an instance of `InputRange` with 3D points and corresponding 3D normal vectors.
*/
template<typename NamedParameters = parameters::Default_named_parameters>
Kinetic_shape_reconstruction_3(PointSet& ps,
const NamedParameters& np = CGAL::parameters::default_values()) : m_points(ps), m_ground_polygon_index(-1), m_kinetic_partition(np) {}
/*!
\brief Detects shapes in the provided point cloud
\tparam NamedParameters
a sequence of \ref bgl_namedparameters "Named Parameters"
\param np
an instance of `NamedParameters`.
\cgalNamedParamsBegin
\cgalParamNBegin{point_map}
\cgalParamDescription{a property map associating points to the elements of `input_range`}
\cgalParamDefault{`PointMap()`}
\cgalParamNEnd
\cgalParamNBegin{normal_map}
\cgalParamDescription{a property map associating normals to the elements of `input_range`}
\cgalParamDefault{`NormalMap()`}
\cgalParamNEnd
\cgalParamNBegin{k_neighbors}
\cgalParamDescription{the number of returned neighbors per each query point}
\cgalParamType{`std::size_t`}
\cgalParamDefault{12}
\cgalParamNEnd
\cgalParamNBegin{angle_tolerance}
\cgalParamDescription{maximum angle in degrees between the normal of a point and the plane normal}
\cgalParamType{`GeomTraits::FT`}
\cgalParamDefault{25 degrees}
\cgalParamNEnd
\cgalParamNBegin{minimum_region_size}
\cgalParamDescription{minimum number of 3D points a region must have}
\cgalParamType{`std::size_t`}
\cgalParamDefault{5}
\cgalParamNEnd
\cgalNamedParamsEnd
*/
template<
typename CGAL_NP_TEMPLATE_PARAMETERS>
std::size_t detect_planar_shapes(bool estimate_ground = false,
const CGAL_NP_CLASS& np = parameters::default_values()) {
if (m_verbose)
std::cout << std::endl << "--- DETECTING PLANAR SHAPES: " << std::endl;
m_planes.clear();
m_polygons.clear();
m_region_map.clear();
m_point_map = Point_set_processing_3_np_helper<Point_set, CGAL_NP_CLASS, Point_map>::get_point_map(m_points, np);
m_normal_map = Point_set_processing_3_np_helper<Point_set, CGAL_NP_CLASS, Normal_map>::get_normal_map(m_points, np);
create_planar_shapes(estimate_ground, np);
CGAL_assertion(m_planes.size() == m_polygons.size());
CGAL_assertion(m_polygons.size() == m_region_map.size());
return m_polygons.size();
}
/*!
\brief Retrieves the detected shapes.
@return
vector with a plane equation for each detected planar shape.
\pre `successful shape detection`
*/
const std::vector<Plane_3>& detected_planar_shapes() {
return m_planes;
}
/*!
\brief Retrieves the indices of detected shapes.
@return
indices into `input_range` for each detected planar shape in vectors.
\pre `successful shape detection`
*/
const std::vector<std::vector<std::size_t> >& detected_planar_shape_indices() {
return m_planar_regions;
}
/*!
\brief initializes the kinetic partition.
\param np
a sequence of \ref bgl_namedparameters "Named Parameters"
among the ones listed below
\cgalNamedParamsBegin
\cgalParamNBegin{reorient_bbox}
\cgalParamDescription{Use the oriented bounding box instead of the axis-aligned bounding box.}
\cgalParamType{bool}
\cgalParamDefault{false}
\cgalParamNEnd
\cgalParamNBegin{bbox_dilation_ratio}
\cgalParamDescription{Factor for extension of the bounding box of the input data to be used for the partition.}
\cgalParamType{FT}
\cgalParamDefault{1.1}
\cgalParamNEnd
\cgalParamNBegin{angle_tolerance}
\cgalParamDescription{The tolerance angle to snap the planes of two input polygons into one plane.}
\cgalParamType{FT}
\cgalParamDefault{5}
\cgalParamNEnd
\cgalNamedParamsEnd
\pre `successful shape detection`
*/
template<typename CGAL_NP_TEMPLATE_PARAMETERS>
void initialize_partition(const CGAL_NP_CLASS& np = parameters::default_values()) {
m_kinetic_partition.insert(m_polygon_pts, m_polygon_indices, np);
m_kinetic_partition.initialize(np);
}
/*!
\brief Propagates the kinetic polygons in the initialized partition.
\param k
maximum number of allowed intersections for each input polygon before its expansion stops.
@return
success of kinetic partition.
\pre `successful initialization`
*/
void partition(std::size_t k, FT& partition_time, FT& finalization_time, FT& conformal_time) {
m_kinetic_partition.partition(k, partition_time, finalization_time, conformal_time);
std::cout << "Bounding box partitioned into " << m_kinetic_partition.number_of_volumes() << " volumes" << std::endl;
m_kinetic_partition.get_linear_cell_complex(m_lcc);
}
/*!
\brief Access to the kinetic partition.
@return
created kinetic partition data structure
\pre `successful partition`
*/
const Kinetic_shape_partition_3<Kernel, Intersection_kernel>& partition() const {
return m_kinetic_partition;
}
/*!
\brief Creates the visibility (data-) and regularity energy terms from the input point cloud and the kinetic partition.
\pre `successful initialization`
*/
void setup_energyterms() {
if (m_lcc.template one_dart_per_cell<3>().size() == 0) {
std::cout << "Kinetic partition is not constructed or does not have volumes" << std::endl;
return;
}
m_face_area.clear();
m_face_inliers.clear();
auto face_range = m_lcc.template one_dart_per_cell<2>();
m_faces_lcc.reserve(face_range.size());
for (auto& d : face_range) {
m_faces_lcc.push_back(m_lcc.dart_descriptor(d));
std::size_t id = m_lcc.attribute<2>(m_faces_lcc.back());
auto p = m_attrib2index_lcc.emplace(std::make_pair(m_lcc.attribute<2>(m_faces_lcc.back()), m_faces_lcc.size() - 1));
CGAL_assertion(p.second);
}
// Create value arrays for graph cut
m_face_inliers.resize(m_faces_lcc.size());
m_face_area.resize(m_faces_lcc.size());
m_face_area_lcc.resize(m_faces_lcc.size());
m_face_neighbors_lcc.resize(m_faces_lcc.size(), std::pair<int, int>(-1, -1));
m_cost_matrix.resize(2);
m_cost_matrix[0].resize(m_kinetic_partition.number_of_volumes() + 6, 0);
m_cost_matrix[1].resize(m_kinetic_partition.number_of_volumes() + 6, 0);
for (std::size_t i = 0; i < m_faces_lcc.size(); i++) {
auto n = m_lcc.template one_dart_per_incident_cell<3, 2>(m_faces_lcc[i]);
assert(n.size() == 1 || n.size() == 2);
auto it = n.begin();
auto& finf = m_lcc.info<2>(m_faces_lcc[i]);
int first = m_lcc.info<3>(m_lcc.dart_descriptor(*it)).volume_id;
auto& inf1 = m_lcc.info<3>(m_lcc.dart_descriptor(*it++));
auto inf2 = inf1;
if (n.size() == 2)
inf2 = m_lcc.info<3>(m_lcc.dart_descriptor(*it));
int second;
if (n.size() == 2)
second = m_lcc.info<3>(m_lcc.dart_descriptor(*it)).volume_id;
if (n.size() == 2)
m_face_neighbors_lcc[i] = std::make_pair(first + 6, m_lcc.info<3>(m_lcc.dart_descriptor(*it)).volume_id + 6);
else
m_face_neighbors_lcc[i] = std::make_pair(first + 6, -m_lcc.info<2>(m_faces_lcc[i]).input_polygon_index - 1);
if (m_face_neighbors_lcc[i].first > m_face_neighbors_lcc[i].second)
m_face_neighbors_lcc[i] = std::make_pair(m_face_neighbors_lcc[i].second, m_face_neighbors_lcc[i].first);
if (m_face_neighbors_lcc[i].first < m_face_neighbors_lcc[i].second) {
auto it = m_neighbors2index_lcc.emplace(std::make_pair(m_face_neighbors_lcc[i], i));
assert(it.second);
}
}
check_ground();
m_face_inliers.clear();
m_face_inliers.resize(m_faces_lcc.size());
collect_points_for_faces_lcc();
count_volume_votes_lcc();
std::cout << "* computing data term ... ";
std::size_t max_inside = 0, max_outside = 0;
for (std::size_t i = 0; i < m_volumes.size(); i++) {
max_inside = (std::max<double>)(m_cost_matrix[0][i + 6], max_inside);
max_outside = (std::max<double>)(m_cost_matrix[1][i + 6], max_outside);
}
// Dump volumes colored by votes
/*
if (false) {
namespace fs = boost::filesystem;
for (fs::directory_iterator end_dir_it, it("gc/i"); it != end_dir_it; ++it) {
fs::remove_all(it->path());
}
for (fs::directory_iterator end_dir_it, it("gc/o"); it != end_dir_it; ++it) {
fs::remove_all(it->path());
}
for (fs::directory_iterator end_dir_it, it("gc/n"); it != end_dir_it; ++it) {
fs::remove_all(it->path());
}
for (fs::directory_iterator end_dir_it, it("gc/all"); it != end_dir_it; ++it) {
fs::remove_all(it->path());
}
for (std::size_t i = 0; i < m_volumes.size(); i++) {
// skip 0/0 volumes? Maybe safe them a few seconds later to be able to separate them?
CGAL::Color c;
int diff = int(m_cost_matrix[0][i + 6]) - int(m_cost_matrix[1][i + 6]);
if (diff > 0) {
std::size_t m = (std::max<int>)(50, (diff * 255) / max_inside);
c = CGAL::Color(0, m, 0);
}
else {
std::size_t m = (std::max<int>)(50, (-diff * 255) / max_outside);
c = CGAL::Color(0, 0, m);
}
if (diff < 0) {
dump_volume(i, "gc/o/" + std::to_string(i) + "-vol-" + std::to_string(m_cost_matrix[0][i + 6]) + "-" + std::to_string(m_cost_matrix[1][i + 6]), c);
dump_volume(i, "gc/all/" + std::to_string(i) + "-vol-" + std::to_string(m_cost_matrix[0][i + 6]) + "-" + std::to_string(m_cost_matrix[1][i + 6]), c);
}
else if (diff > 0) {
dump_volume(i, "gc/i/" + std::to_string(i) + "-vol-" + std::to_string(m_cost_matrix[0][i + 6]) + "-" + std::to_string(m_cost_matrix[1][i + 6]), c);
dump_volume(i, "gc/all/" + std::to_string(i) + "-vol-" + std::to_string(m_cost_matrix[0][i + 6]) + "-" + std::to_string(m_cost_matrix[1][i + 6]), c);
}
else {
dump_volume(i, "gc/n/" + std::to_string(i) + "-vol-0-0", CGAL::Color(255, 255, 255));
dump_volume(i, "gc/all/" + std::to_string(i) + "-vol-0-0", CGAL::Color(255, 255, 255));
}
}
}
*/
}
/*!
\brief Provides the data and regularity energy terms for reconstruction via graph-cut.
\param edges
contains a vector of pairs of volume indices. Indicates which volumes should be connected in the graph cut formulation.
\param edge_costs
contains the cost for each edge specified in `edges` for two labels with different labels. For equal labels, the cost is 0. Needs to be index compatible to the `edges` parameter.
\param cost_matrix
provides the cost of a label for each volume cell. The first index corresponds to the label and the second index corresponds to the volume index.
@return
fails if the dimensions of parameters does not match the kinetic partition.
\pre `successful initialization`
*/
template<typename NamedParameters>
bool setup_energyterms(
const std::vector< std::pair<std::size_t, std::size_t> >& edges,
const std::vector<double>& edge_costs,
const std::vector< std::vector<double> >& cost_matrix);
/*!
\brief Uses graph-cut to solve an solid/empty labeling of the volumes of the kinetic partition.
\param lambda
trades the impact of the data term for impact of the regularization term. Should be in the range [0, 1).
@return
success of reconstruction.
\pre `successful initialization`
*/
bool reconstruct(FT lambda) {
KSR_3::Graphcut<Kernel> gc(lambda);
gc.solve(m_face_neighbors_lcc, m_face_area_lcc, m_cost_matrix, m_labels);
return true;
}
/*!
\brief Provides the reconstructed surface as a list of indexed polygons.
\param pit
an output iterator taking Point_3.
\param triit
an output iterator taking std::vector<std::size_t>.
\pre `successful reconstruction`
*/
template<class OutputPointIterator, class OutputPolygonIterator>
void reconstructed_model_polylist(OutputPointIterator pit, OutputPolygonIterator polyit) {
if (m_labels.empty())
return;
From_exact from_exact;
std::map<Point_3, std::size_t> pt2idx;
for (std::size_t i = 0; i < m_faces_lcc.size(); i++) {
const auto& n = m_face_neighbors_lcc[i];
// Do not extract boundary faces.
if (n.second < 6)
continue;
if (m_labels[n.first] != m_labels[n.second]) {
std::vector<Point_3> face;
for (const auto& vd : m_lcc.one_dart_per_incident_cell<0, 2>(m_faces_lcc[i]))
face.push_back(from_exact(m_lcc.point(m_lcc.dart_descriptor(vd))));
std::vector<std::size_t> indices(face.size());
for (std::size_t i = 0; i < face.size(); i++) {
auto p = pt2idx.emplace(face[i], pt2idx.size());
if (p.second)
*pit++ = face[i];
indices[i] = p.first->second;
}
*polyit++ = std::move(indices);
}
}
}
/*!
\brief Provides the reconstructed surface as a list of indexed polygons.
\param pit
an output iterator taking Point_3.
\param triit
an output iterator taking std::vector<std::size_t>.
\pre `successful reconstruction`
*/
template<class OutputPointIterator, class OutputPolygonIterator>
void reconstructed_model_polylist_lcc(OutputPointIterator pit, OutputPolygonIterator polyit) {
if (m_labels.empty())
return;
From_exact from_exact;
std::map<Point_3, std::size_t> pt2idx;
std::vector<int> region_index(m_faces_lcc.size(), -1);
std::size_t region = 0;
std::vector<std::vector<std::vector<Point_3> > > polygon_regions;
std::vector<typename Intersection_kernel::Plane_3> planes;
for (std::size_t i = 0; i < m_faces_lcc.size(); i++) {
const auto& n = m_face_neighbors_lcc[i];
if (n.second < 6)
continue;
if (m_labels[n.first] != m_labels[n.second]) {
Face_attribute fa = m_lcc.attribute<2>(m_faces_lcc[i]);
if (region_index[fa] == -1) {
std::vector<std::vector<Point_3> > faces;
collect_connected_component(m_faces_lcc[i], region_index, region++, faces);
planes.push_back(m_lcc.info_of_attribute<2>(fa).plane);
polygon_regions.push_back(std::move(faces));
}
}
}
KSP_3::dump_polygons(polygon_regions, "faces_by_region.ply");
std::vector<std::vector<std::size_t> > borders;
std::vector<std::vector<std::size_t> > borders_per_region;
collect_connected_border(borders, region_index, borders_per_region);
for (std::size_t i = 0; i < region; i++) {
if (borders_per_region[i].size() > 0) {
// ToDo: remove after -->
std::size_t outer = -1;
typename Intersection_kernel::FT min = (std::numeric_limits<double>::max)();
for (std::size_t j = 0; j < borders_per_region[i].size(); j++)
for (std::size_t k = 0; k < borders[borders_per_region[i][j]].size(); k++) {
const typename Intersection_kernel::Point_3& p = m_lcc.point(m_lcc.dart_of_attribute<0>(borders[borders_per_region[i][j]][k]));
if (p.x() < min) {
min = p.x();
outer = j;
}
}
/*
for (std::size_t j = 0; j < borders_per_region[i].size(); j++) {
std::string fn;
if (j == outer)
fn = "merged_borders/" + std::to_string(i) + "-outer.polylines.txt";
else
fn = "merged_borders/" + std::to_string(i) + "-" + std::to_string(j) + ".polylines.txt";
std::ofstream vout(fn);
vout << (borders[borders_per_region[i][j]].size() + 1);
for (std::size_t k = 0; k < borders[borders_per_region[i][j]].size(); k++) {
vout << " " << from_exact(m_lcc.point(m_lcc.dart_of_attribute<0>(borders[borders_per_region[i][j]][k])));
}
vout << " " << from_exact(m_lcc.point(m_lcc.dart_of_attribute<0>(borders[borders_per_region[i][j]][0]))) << std::endl;
vout.close();
}*/
if (borders_per_region[i].size() > 1) {
std::vector<std::vector<std::size_t> > polygons;
polygons.reserve(borders_per_region[i].size());
for (std::size_t j = 0; j < borders_per_region[i].size(); j++)
polygons.push_back(std::move(borders[borders_per_region[i][j]]));
insert_ghost_edges_cdt(polygons, planes[i]);
/*
std::ofstream vout("merged_borders/" + std::to_string(i) + "-merged.polylines.txt");
vout << (polygons[0].size() + 1);
for (std::size_t k = 0; k < polygons[0].size(); k++) {
vout << " " << from_exact(m_lcc.point(m_lcc.dart_of_attribute<0>(polygons[0][k])));
}
vout << " " << from_exact(m_lcc.point(m_lcc.dart_of_attribute<0>(polygons[0][0]))) << std::endl;
vout.close();*/
borders_per_region[i].resize(1);
CGAL_assertion(borders[borders_per_region[i][0]].empty());
CGAL_assertion(!polygons[0].empty());
borders[borders_per_region[i][0]] = std::move(polygons[0]);
}
}
/*
else {
for (std::size_t k = 0;k<region_index.size();k++)
if (region_index[k] == i) {
write_face(m_lcc.dart_of_attribute<2>(k), std::to_string(i) + "-" + std::to_string(k) + "_missing.ply");
}
}*/
}
std::map<std::size_t, std::size_t> attrib2idx;
for (std::size_t i = 0; i < borders.size(); i++) {
if (borders[i].empty())
continue;
/* if (false) {*/
std::vector<std::size_t> indices(borders[i].size());
for (std::size_t j = 0; j != borders[i].size(); j++) {
auto p = attrib2idx.emplace(borders[i][j], attrib2idx.size());
if (p.second)
*pit++ = from_exact(m_lcc.point(m_lcc.dart_of_attribute<0>(borders[i][j])));
indices[j] = p.first->second;
}
*polyit++ = std::move(indices);
/*
}
else {
std::vector<std::size_t> indices(borders[i].size());
for (std::size_t j = borders[i].size() - 1; j != std::size_t(-1); j--) {
auto p = attrib2idx.emplace(borders[i][j], attrib2idx.size());
if (p.second)
*pit++ = from_exact(m_lcc.point(m_lcc.dart_of_attribute<0>(borders[i][j])));
indices[j] = p.first->second;
}
*polyit++ = std::move(indices);
}*/
}
}
/*!
\brief Provides the reconstructed surface as a list of indexed triangles.
\param pit
an output iterator taking Point_3.
\param triit
an output iterator taking std::size_t.
\pre `successful reconstruction`
*/
template<class OutputPointIterator, class OutputTriangleIterator>
void reconstructed_model_trilist(OutputPointIterator pit, OutputTriangleIterator triit) {
if (m_labels.empty())
return;
std::map<Point_3, std::size_t> pt2idx;
for (std::size_t i = 0; i < m_faces_lcc.size(); i++) {
const auto& n = m_face_neighbors_lcc[i];
// Do not extract boundary faces.
if (n.second < 6)
continue;
if (m_labels[n.first] != m_labels[n.second]) {
std::vector<Point_3> face;
m_kinetic_partition.vertices(m_faces_lcc[i], std::back_inserter(face));
std::vector<std::size_t> indices(face.size());
for (std::size_t i = 0; i < face.size(); i++) {
auto p = pt2idx.emplace(face[i], pt2idx.size());
if (p.second)
*pit++ = face[i];
indices[i] = p.first->second;
}
for (std::size_t i = 2; i < face.size(); i++) {
*triit++ = indices[0];
*triit++ = indices[i - 1];
*triit++ = indices[i];
}
}
}
}
private:
struct Vertex_info { FT z = FT(0); };
struct Face_info { };
using Fbi = CGAL::Triangulation_face_base_with_info_2<Face_info, Kernel>;
//using Fb = CGAL::Alpha_shape_face_base_2<Kernel, Fbi>;
using Vbi = CGAL::Triangulation_vertex_base_with_info_2<Vertex_info, Kernel>;
//using Vb = CGAL::Alpha_shape_vertex_base_2<Kernel, Vbi>;
using Tds = CGAL::Triangulation_data_structure_2<Vbi, Fbi>;
using Delaunay_2 = CGAL::Delaunay_triangulation_2<Kernel, Tds>;
using Delaunay_3 = CGAL::Delaunay_triangulation_3<Kernel>;
typedef CGAL::Linear_cell_complex_traits<3, CGAL::Exact_predicates_exact_constructions_kernel> Traits;
using LCC = CGAL::Linear_cell_complex_for_combinatorial_map<3, 3, Traits, typename KSP::Linear_cell_complex_min_items>;
using Dart_descriptor = typename LCC::Dart_descriptor;
using Dart = typename LCC::Dart;
struct VI {
VI() : i(-1), j(-1) {}
int i, j;
Dart_descriptor dh;
typename Intersection_kernel::Point_2 p;
};
typedef CGAL::Triangulation_vertex_base_with_info_2<VI, typename Intersection_kernel> Vbi2;
typedef CGAL::Constrained_triangulation_face_base_2<typename Intersection_kernel> Fb;
typedef CGAL::Triangulation_data_structure_2<Vbi2, Fb> Tds2;
typedef CGAL::Exact_intersections_tag Itag;
typedef CGAL::Constrained_Delaunay_triangulation_2<typename Intersection_kernel, Tds2, Itag> CDT;
typedef typename CDT::Vertex_handle Vertex_handle;
typedef typename CDT::Face_handle Face_handle;
typedef typename CDT::Finite_vertices_iterator Finite_vertices_iterator;
typedef typename CDT::Finite_edges_iterator Finite_edges_iterator;
typedef typename CDT::Finite_faces_iterator Finite_faces_iterator;
//using Visibility = KSR_3::Visibility<Kernel, Intersection_kernel, Point_map, Normal_map>;
using Index = typename KSP::Index;
using Face_attribute = typename LCC::Base::template Attribute_descriptor<2>::type;
using Volume_attribute = typename LCC::Base::template Attribute_descriptor<3>::type;
bool m_verbose;
bool m_debug;
Point_set &m_points;
Point_map m_point_map;
Normal_map m_normal_map;
std::vector<std::vector<std::size_t> > m_planar_regions;
std::vector<typename Region_growing::Primitive_and_region> m_regions;
std::map<std::size_t, Indices> m_region_map;
double m_detection_distance_tolerance;
std::size_t m_ground_polygon_index;
Plane_3 m_ground_plane;
std::vector<Plane_3> m_planes;
std::vector<Point_3> m_polygon_pts;
std::vector<std::vector<std::size_t> > m_polygon_indices;
std::vector<Polygon_3> m_polygons;
KSP m_kinetic_partition;
LCC m_lcc;
std::vector<typename LCC::Dart_const_descriptor> m_faces_lcc;
std::map<Face_attribute, std::size_t> m_attrib2index_lcc;
std::vector<std::size_t> lcc2index;
std::vector<std::size_t> index2lcc;
// Face indices are now of type Indices and are not in a range 0 to n
std::vector<Indices> m_face_inliers;
std::vector<FT> m_face_area, m_face_area_lcc;
std::vector<std::pair<std::size_t, std::size_t> > m_face_neighbors_lcc;
std::map<std::pair<std::size_t, std::size_t>, std::size_t> m_neighbors2index_lcc;
std::vector<std::pair<std::size_t, std::size_t> > m_volume_votes; // pair<inside, outside> votes
std::vector<bool> m_volume_below_ground;
std::vector<std::vector<double> > m_cost_matrix;
std::vector<FT> m_volumes; // normalized volume of each kinetic volume
std::vector<std::size_t> m_labels;
std::size_t m_total_inliers;
std::size_t add_convex_hull_shape(
const std::vector<std::size_t>& region, const Plane_3& plane) {
std::vector<Point_2> points;
points.reserve(region.size());
for (const std::size_t idx : region) {
CGAL_assertion(idx < m_points.size());
const auto& p = get(m_point_map, idx);
const auto q = plane.projection(p);
const auto point = plane.to_2d(q);
points.push_back(point);
}
CGAL_assertion(points.size() == region.size());
std::vector<Point_2> ch;
CGAL::convex_hull_2(points.begin(), points.end(), std::back_inserter(ch));
std::vector<Point_3> polygon;
for (const auto& p : ch) {
const auto point = plane.to_3d(p);
polygon.push_back(point);
}
const std::size_t shape_idx = m_polygons.size();
m_polygons.push_back(polygon);
m_planes.push_back(plane);
m_polygon_indices.push_back(std::vector<std::size_t>());
m_polygon_indices.back().resize(polygon.size());
std::iota(std::begin(m_polygon_indices.back()), std::end(m_polygon_indices.back()), m_polygon_pts.size());
std::copy(polygon.begin(), polygon.end(), std::back_inserter(m_polygon_pts));
return shape_idx;
}
void store_convex_hull_shape(const std::vector<std::size_t>& region, const Plane_3& plane, std::vector<std::vector<Point_3> > &polys) {
std::vector<Point_2> points;
points.reserve(region.size());
for (const std::size_t idx : region) {
CGAL_assertion(idx < m_points.size());
const auto& p = get(m_point_map, idx);
const auto q = plane.projection(p);
const auto point = plane.to_2d(q);
points.push_back(point);
}
CGAL_assertion(points.size() == region.size());
std::vector<Point_2> ch;
CGAL::convex_hull_2(points.begin(), points.end(), std::back_inserter(ch));
std::vector<Point_3> polygon;
for (const auto& p : ch) {
const auto point = plane.to_3d(p);
polygon.push_back(point);
}
polys.push_back(polygon);
}
std::pair<int, int> make_canonical_pair(int i, int j)
{
if (i > j) return std::make_pair(j, i);
return std::make_pair(i, j);
}
void check_ground() {
std::size_t num_volumes = m_kinetic_partition.number_of_volumes();
// Set all volumes to not be below the ground, this leads to the standard 6 outside node connection.
m_volume_below_ground.resize(num_volumes, false);
From_exact from_exact;
if (m_ground_polygon_index != -1)
for (const auto &vd : m_lcc.template one_dart_per_cell<3>()) {
const auto& info = m_lcc.info<3>(m_lcc.dart_descriptor(vd));
m_volume_below_ground[info.volume_id] = (from_exact(info.barycenter) - m_regions[m_ground_polygon_index].first.projection(from_exact(info.barycenter))).z() < 0;
}
}
void collect_connected_component(typename LCC::Dart_descriptor face, std::vector<int>& region_index, std::size_t region, std::vector<std::vector<Point_3> > &faces) {
std::queue<std::size_t> face_queue;
face_queue.push(face);
From_exact from_exact;
auto& finfo = m_lcc.info<2>(face);
int ip = m_lcc.info<2>(face).input_polygon_index;
typename Intersection_kernel::Plane_3 pl = m_lcc.info<2>(face).plane;
// if (debug)
// std::cout << ip << std::endl;
while (!face_queue.empty()) {
Dart_descriptor cur_fdh(face_queue.front());
Face_attribute cur_fa = m_lcc.attribute<2>(cur_fdh);
face_queue.pop();
if (region_index[cur_fa] == region)
continue;
// if (debug)
// write_face(cur_fdh, std::to_string(region) + "-inside-" + std::to_string(cur_fa) + ".ply");
region_index[cur_fa] = region;
Dart_descriptor n = cur_fdh;
std::vector<Point_3> f;
do {
f.push_back(from_exact(m_lcc.point(n)));
n = m_lcc.beta(n, 1);
} while (n != cur_fdh);
faces.push_back(std::move(f));
// Iterate over edges of face.
Dart_descriptor edh = cur_fdh;
do {
Dart_descriptor fdh = m_lcc.beta<2, 3>(edh);
do {
Face_attribute fa = m_lcc.attribute<2>(fdh);
if (fa == m_lcc.null_descriptor)
break;
auto& finfo2 = m_lcc.info<2>(fdh);
if (fa == cur_fa) {
fdh = m_lcc.beta<2, 3>(fdh);
continue;
}
auto& inf = m_lcc.info<2>(fdh);
bool added = false;
// if (debug)
// write_face(fdh, std::to_string(region) + "-" + std::to_string(fa) + ".ply");
const auto& n = m_face_neighbors_lcc[m_attrib2index_lcc[fa]];
// Belongs to reconstruction?
bool internal = m_labels[n.first] == m_labels[n.second];
if (m_labels[n.first] == m_labels[n.second]) {
fdh = m_lcc.beta<2, 3>(fdh);
continue;
}
// Already segmented?
if (region_index[fa] != -1) {
if (!internal)
break;
fdh = m_lcc.beta<2, 3>(fdh);
continue;
}
// If the face is part of the reconstruction, but on the inside volume, switch to the mirror face on the outside.
if (n.first >= 6 && n.second >= 6 && m_labels[m_lcc.info<3>(fdh).volume_id + 6] == 0) {
fdh = m_lcc.beta<3>(fdh);
fa = m_lcc.attribute<2>(fdh);
finfo2 = m_lcc.info<2>(fdh);
}
if (ip != -7) {
if (m_lcc.info<2>(fdh).input_polygon_index == ip) {
if (internal)
break;
added = true;
face_queue.push(fdh);
// if (debug)
// std::cout << ip << std::endl;
//
// if (debug)
// write_face(fdh, std::to_string(region) + "-inside-" + std::to_string(fa) + ".ply");
}
else
if (!internal)
break;
}
else
if (m_lcc.info<2>(fdh).plane == pl || m_lcc.info<2>(fdh).plane == pl.opposite()) {
if (internal)
break;
added = true;
Plane_3 pla = from_exact(pl);
// if (debug)
// std::cout << ip << " " << pl.a() << " " << pl.b() << " " << pl.c() << " " << pl.d() << std::endl;
//
// if (debug)
// write_face(fdh, std::to_string(region) + "-inside-" + std::to_string(fa) + ".ply");
face_queue.push(fdh);
}
else
if (!internal)
break;
// if (!added)
// border_edges.push_back(edh);
break;
} while (fdh != edh);
edh = m_lcc.beta<1>(edh);
} while (edh != cur_fdh);
}
}
bool is_border_edge(typename LCC::Dart_descriptor dh) {
Face_attribute& fa = m_lcc.attribute<2>(dh);
auto& finfo = m_lcc.info_of_attribute<2>(fa);
if (!m_labels[m_lcc.info<3>(dh).volume_id + 6] == 1) {
write_face(dh, "flipface.ply");
std::cout << "is_border_edge called on dart of outside volume, dh " << dh << " volume_id " << m_lcc.info<3>(dh).volume_id << std::endl;
}
Dart_descriptor edh = m_lcc.beta<2, 3>(dh);
do {
Face_attribute fa2 = m_lcc.attribute<2>(edh);
if (fa2 == m_lcc.null_descriptor)
return true;
// if (debug)
// write_face(edh, "cur_is_border.ply");
if (fa2 == fa) {
std::cout << "should not happen" << std::endl;
edh = m_lcc.beta<2, 3>(edh);
continue;
}
const auto& n = m_face_neighbors_lcc[m_attrib2index_lcc[fa2]];
bool internal = (m_labels[n.first] == m_labels[n.second]);
auto& finfo2 = m_lcc.info_of_attribute<2>(fa2);
// Is neighbor face on same support plane?
if (finfo2.input_polygon_index != finfo.input_polygon_index)
if (!internal)
return true;
else {
edh = m_lcc.beta<2, 3>(edh);
continue;
}
if (finfo2.input_polygon_index == -7)
if (finfo2.plane != finfo.plane && finfo2.plane != finfo.plane.opposite())
if (!internal)
return true;
else {
edh = m_lcc.beta<2, 3>(edh);
continue;
};
return internal;
} while (edh != dh);
// If there is no neighbor face on the same support plane, this is a border edge.
return true;
}
void insert_ghost_edges_cdt(std::vector<std::vector<std::size_t> >& polygons, const typename Intersection_kernel::Plane_3 pl) const {
CDT cdt;
From_exact from_exact;
std::unordered_map<std::size_t, std::size_t> va2vh;
std::vector<Vertex_handle> vertices;
std::size_t num_vertices = 0;
for (std::size_t i = 0; i < polygons.size(); i++) {
num_vertices += polygons[i].size();
for (std::size_t j = 0; j < polygons[i].size(); j++) {
vertices.push_back(cdt.insert(pl.to_2d(m_lcc.point(m_lcc.dart_of_attribute<0>(polygons[i][j])))));
auto it = va2vh.insert(std::make_pair(polygons[i][j], vertices.size() - 1));
CGAL_assertion(it.second);
vertices.back()->info().i = i;
vertices.back()->info().j = j;
vertices.back()->info().p = pl.to_2d(m_lcc.point(m_lcc.dart_of_attribute<0>(polygons[i][j])));
vertices.back()->info().dh = polygons[i][j];
if (j >= 1)
cdt.insert_constraint(vertices[vertices.size() - 2], vertices.back());
}
cdt.insert_constraint(vertices.back(), vertices[vertices.size() - polygons[i].size()]);
}
// check infinitive edges for outer polygon
int outer = -1;
auto& e = *(cdt.incident_edges(cdt.infinite_vertex()));
auto a = e.first->vertex((e.second + 1) % 3);
auto b = e.first->vertex((e.second + 2) % 3);
if (a == cdt.infinite_vertex())
outer = b->info().i;
else
outer = a->info().i;
CGAL_assertion(outer != -1);
// Distance matrix
std::vector<FT> dist(polygons.size()* polygons.size(), (std::numeric_limits<FT>::max)());
std::vector<std::pair<std::size_t, std::size_t> > closest_pts(polygons.size() * polygons.size(), std::make_pair(-1, -1));
for (auto& edge : cdt.finite_edges()) {
auto v1 = edge.first->vertex((edge.second + 1) % 3);
auto v2 = edge.first->vertex((edge.second + 2) % 3);
if (v1->info().i != v2->info().i) {
std::size_t idx;
if (v1->info().i == -1 || v2->info().i == -1)
continue;
if (v1->info().i < v2->info().i)
idx = v1->info().i * polygons.size() + v2->info().i;
else
idx = v2->info().i * polygons.size() + v1->info().i;
FT d = from_exact((v1->info().p - v2->info().p).squared_length());
if (dist[idx] > d) {
dist[idx] = d;
closest_pts[idx] = std::make_pair(v1->info().dh, v2->info().dh);
}
}
}
std::vector<bool> merged(polygons.size(), false);
for (std::size_t i = 0; i < polygons.size(); i++) {
if (i == outer)
continue;
std::size_t idx;
if (i < outer)
idx = i * polygons.size() + outer;
else
idx = outer * polygons.size() + i;
// For now merge all polygons into outer if possible
if (dist[idx] < (std::numeric_limits<FT>::max)()) {
std::size_t in_target, in_source;
for (in_target = 0; in_target < polygons[outer].size(); in_target++)
if (polygons[outer][in_target] == closest_pts[idx].first || polygons[outer][in_target] == closest_pts[idx].second)
break;
for (in_source = 0; in_source < polygons[i].size(); in_source++)
if (polygons[i][in_source] == closest_pts[idx].first || polygons[i][in_source] == closest_pts[idx].second)
break;
std::size_t former_end = polygons[outer].size() - 1;
polygons[outer].resize(polygons[outer].size() + polygons[i].size() + 2);
for (std::size_t j = 0; j != former_end - in_target + 1; j++)
polygons[outer][polygons[outer].size() - j - 1] = polygons[outer][former_end - j];
for (std::size_t j = 0; j < polygons[i].size() + 1; j++) {
std::size_t idx = (in_source + j) % polygons[i].size();
polygons[outer][in_target + j + 1] = polygons[i][idx];
}
}
else {
std::cout << "ghost edge could not be placed" << std::endl;
// Do I need a minimum spanning tree? https://www.boost.org/doc/libs/1_75_0/libs/graph/example/kruskal-example.cpp
}
polygons[i].clear();
}
if (outer != 0)
polygons[0] = std::move(polygons[outer]);
polygons.resize(1);
}
typename LCC::Dart_descriptor circulate_vertex_2d(typename LCC::Dart_descriptor dh) {
CGAL_assertion(!is_border_edge(dh));
//beta3(beta2(dh)) until I am on a face that is on the same input polygon
// is_border_edge should handle if there is no coplanar neighbor face
// However, the dart should be pointing towards the vertex
Face_attribute& fa = m_lcc.attribute<2>(dh);
auto& finfo = m_lcc.info_of_attribute<2>(fa);
From_exact from_exact;
typename LCC::Dart_descriptor dh2 = m_lcc.beta<2>(dh);
//write_face(dh, std::to_string(dh) + "c0.ply");
std::size_t idx = 1;
do {
//write_face(dh2, "c" + std::to_string(idx) + ".ply");
Face_attribute fa2 = m_lcc.attribute<2>(dh2);
auto& finfo2 = m_lcc.info_of_attribute<2>(fa2);
if (finfo2.input_polygon_index == finfo.input_polygon_index) {
CGAL_assertion(fa != fa2);
if (finfo2.input_polygon_index == -7) {
if (finfo2.plane == finfo.plane || finfo2.plane == finfo.plane.opposite())
return dh2;
}
else return dh2;
}
dh2 = m_lcc.beta<3, 2>(dh2);
idx++;
} while (dh2 != dh);
// dh is a border edge
CGAL_assertion(false);
// std::ofstream vout("c0.polylines.txt");
// vout << "2 " << from_exact(m_lcc.point(dh)) << " " << from_exact(m_lcc.point(m_lcc.beta<1>(dh))) << std::endl;
// vout.close();
/*
typename Face_attribute::Info finfo2;
do {
dh2 = m_lcc.beta<3, 2>(dh2);
CGAL_assertion(dh2 != dh); // Should be prevented in is_border_edge
Face_attribute& fa2 = m_lcc.attribute<2>(dh2);
finfo2 = m_lcc.info_of_attribute<2>(fa2);
} while (finfo.input_polygon_index == finfo2.input_polygon_index);
// dh2 is still on the same edge as dh and points in the same direction.
// beta3 gives the mirrored dart on the same edge&face, beta1 then proceeds to the next edge of that vertex.
dh2 = m_lcc.beta<1, 3>(dh2);
CGAL_assertion(dh2 != m_lcc.null_dart_descriptor);*/
return dh2;
}
void collect_border(typename LCC::Dart_descriptor dh, std::vector<bool>& processed, std::vector<std::vector<std::size_t> >& borders) {
// Iterate clockwise around target vertex of dh
// It seems the dart associated with a vertex are pointing away from it
// -> beta_1(beta_2(dh)) circulates around a vertex
processed[dh] = true;
From_exact from_exact;
if (!m_labels[m_lcc.info<3>(dh).volume_id + 6] == 1)
std::cout << "collect_border called on dart of outside volume, dh " << dh << " volume_id " << m_lcc.info<3>(dh).volume_id << std::endl;
std::vector<std::size_t> border;
border.push_back(m_lcc.attribute<0>(dh));
// std::ofstream vout("b.polylines.txt");
// vout << "2 " << from_exact(m_lcc.point(dh)) << " " << from_exact(m_lcc.point(m_lcc.beta<1>(dh))) << std::endl;
// vout.close();
Face_attribute& fa = m_lcc.attribute<2>(dh);
auto& finfo = m_lcc.info_of_attribute<2>(fa);
// The central element of the loop is the current edge/vertex?
// // dh = beta1(dh) // progressing to the next vertex
// while !is_border(dh) do dh = beta1(beta2(dh)) (wrong, this is 2D thinking...beta3(beta2(dh))
// add vertex
// dh = beta1(dh)
// if attribute<0>(dh) is equal to first element in border -> stop
// How to identify inner loops after this?
// Do I need connected component for faces and then also the looping stuff?
typename LCC::Dart_descriptor cur = dh;
cur = m_lcc.beta<1>(cur);
std::size_t idx = 0;
do {
/*
if (debug) {
std::ofstream vout("0-" + std::to_string(idx) + ".xyz");
for (std::size_t p : border)
vout << from_exact(m_lcc.point(m_lcc.dart_of_attribute<0>(p))) << std::endl;
vout.close();
}
if (debug)
write_edge(cur, "cur.polylines.txt");*/
if (is_border_edge(cur)) {
CGAL_assertion(!processed[cur]);
processed[cur] = true;
border.push_back(m_lcc.attribute<0>(cur));
if (!m_labels[m_lcc.info<3>(cur).volume_id + 6] == 1)
std::cout << "border collected from dart of outside volume, dh " << cur << " volume_id " << m_lcc.info<3>(cur).volume_id << std::endl;
}
else
cur = circulate_vertex_2d(cur);
cur = m_lcc.beta<1>(cur);
idx++;
} while(cur != dh);
borders.push_back(std::move(border));
}
void write_face(const typename LCC::Dart_descriptor dh, const std::string& fn) {
std::vector<Point_3> face;
From_exact from_exact;
Dart_descriptor n = dh;
do {
face.push_back(from_exact(m_lcc.point(n)));
n = m_lcc.beta(n, 1);
} while (n != dh);
KSP_3::dump_polygon(face, fn);
}
void write_edge(typename LCC::Dart_descriptor dh, const std::string& fn) {
From_exact from_exact;
std::ofstream vout(fn);
vout << "2 " << from_exact(m_lcc.point(dh)) << " " << from_exact(m_lcc.point(m_lcc.beta<1>(dh))) << std::endl;
vout.close();
}
void write_border(std::vector<std::size_t> &border, const std::string& fn) {
From_exact from_exact;
std::ofstream vout(fn);
vout << (border.size() + 1);
for (std::size_t k = 0; k < border.size(); k++) {
vout << " " << from_exact(m_lcc.point(m_lcc.dart_of_attribute<0>(border[k])));
}
vout << " " << from_exact(m_lcc.point(m_lcc.dart_of_attribute<0>(border[0]))) << std::endl;
vout.close();
}
void collect_connected_border(std::vector<std::vector<std::size_t> >& borders, const std::vector<int> &region_index, std::vector<std::vector<std::size_t> > &borders_per_region) {
// Start extraction of a border from each dart (each dart is a 1/n-edge)
// Search starting darts by searching faces
//borders contains Attribute<0> handles casted to std::size_t
std::vector<bool> processed(m_lcc.number_of_darts(), false);
From_exact from_exact;
borders_per_region.resize(region_index.size());
for (std::size_t i = 0;i<region_index.size();i++) {
if (region_index[i] == -1)
continue;
/*
if (region_index[i] == 3)
std::cout << std::endl;
if (i == 1043)
std::cout << std::endl;*/
typename LCC::Dart_descriptor dh = m_lcc.dart_of_attribute<2>(i);
if (m_labels[m_lcc.info<3>(dh).volume_id + 6] == 0)
dh = m_lcc.beta<3>(dh);
Volume_attribute va = m_lcc.attribute<3>(dh);
Face_attribute &fa = m_lcc.attribute<2>(dh);
auto finfo = m_lcc.info_of_attribute<2>(fa);
const auto& n = m_face_neighbors_lcc[m_attrib2index_lcc[fa]];
// Do not segment bbox surface
if (n.second < 6)
continue;
// Belongs to reconstruction?
if (m_labels[n.first] == m_labels[n.second]) {
std::cout << "face skipped" << std::endl;
continue;
}
std::size_t num_edges = m_lcc.template one_dart_per_incident_cell<1, 2>(dh).size();
typename LCC::Dart_descriptor dh2 = dh;
do {
if (va != m_lcc.attribute<3>(dh2)) {
std::cout << "volume attribute mismatch" << std::endl;
}
if (!processed[dh2] && is_border_edge(dh2)) {
borders_per_region[region_index[fa]].push_back(borders.size());
collect_border(dh2, processed, borders);
}
dh2 = m_lcc.beta<1>(dh2);
} while (dh2 != dh);
}
}
void collect_points_for_faces_lcc() {
FT total_area = 0;
m_total_inliers = 0;
From_exact from_exact;
std::vector<std::vector<Dart_descriptor> > poly2faces(m_kinetic_partition.input_planes().size());
std::vector<Dart_descriptor> other_faces;
for (auto& d : m_lcc.template one_dart_per_cell<2>()) {
Dart_descriptor dh = m_lcc.dart_descriptor(d);
if (m_lcc.info<2>(dh).input_polygon_index >= 0)
poly2faces[m_lcc.info<2>(dh).input_polygon_index].push_back(dh);
else
other_faces.push_back(dh); // Contains faces originating from the octree decomposition as well as bbox faces
}
assert(m_kinetic_partition.input_planes().size() == m_regions.size());
std::size_t next = 0, step = 1;
for (std::size_t i = 0; i < m_kinetic_partition.input_planes().size(); i++) {
std::vector<std::pair<Dart_descriptor, std::vector<std::size_t> > > mapping;
std::vector<Point_3> pts;
pts.reserve(m_regions[i].second.size());
for (std::size_t j = 0; j < m_regions[i].second.size(); j++)
pts.emplace_back(get(m_point_map, m_regions[i].second[j]));
map_points_to_faces(i, pts, mapping);
// Remap from mapping to m_face_inliers
for (auto p : mapping) {
//m_face_inliers[m_attrib2index_lcc[m_lcc.attribute<2>(p.first)]].clear();
Face_attribute& f = m_lcc.attribute<2>(p.first);
std::size_t id = m_attrib2index_lcc[f];
assert(m_face_inliers[id].size() == 0);
m_face_inliers[m_attrib2index_lcc[m_lcc.attribute<2>(p.first)]].resize(p.second.size());
for (std::size_t k = 0; k < p.second.size(); k++)
m_face_inliers[m_attrib2index_lcc[m_lcc.attribute<2>(p.first)]][k] = m_regions[i].second[p.second[k]];
m_total_inliers += p.second.size();
}
Plane_3 pl = from_exact(m_kinetic_partition.input_planes()[i]);
for (std::size_t j = 0; j < poly2faces[i].size(); j++) {
std::size_t idx = m_attrib2index_lcc[m_lcc.attribute<2>(poly2faces[i][j])];
m_face_area_lcc[idx] = 0;
//multiple regions per input polygon
Delaunay_2 tri;
Dart_descriptor n = poly2faces[i][j];
do {
tri.insert(pl.to_2d(from_exact(m_lcc.point(n))));
n = m_lcc.beta(n, 0);
} while (n != poly2faces[i][j]);
// Get area
for (auto fit = tri.finite_faces_begin(); fit != tri.finite_faces_end(); ++fit) {
const Triangle_2 triangle(
fit->vertex(0)->point(),
fit->vertex(1)->point(),
fit->vertex(2)->point());
m_face_area_lcc[idx] += triangle.area();
}
total_area += m_face_area_lcc[idx];
}
}
set_outside_volumes(m_cost_matrix);
// Handling face generated by the octree partition. They are not associated with an input polygon.
for (std::size_t i = 0; i < other_faces.size(); i++) {
std::vector<Point_3> face;
std::size_t idx = m_attrib2index_lcc[m_lcc.attribute<2>(other_faces[i])];
Dart_descriptor n = other_faces[i];
do {
face.push_back(from_exact(m_lcc.point(n)));
n = m_lcc.beta(n, 0);
} while (n != other_faces[i]);
Plane_3 pl;
CGAL::linear_least_squares_fitting_3(face.begin(), face.end(), pl, CGAL::Dimension_tag<0>());
Delaunay_2 tri;
for (const Point_3& p : face)
tri.insert(pl.to_2d(p));
// Get area
for (auto fit = tri.finite_faces_begin(); fit != tri.finite_faces_end(); ++fit) {
const Triangle_2 triangle(
fit->vertex(0)->point(),
fit->vertex(1)->point(),
fit->vertex(2)->point());
m_face_area_lcc[idx] += triangle.area();
}
total_area += m_face_area_lcc[idx];
}
m_face_area_lcc.resize(m_faces_lcc.size(), 0);
for (std::size_t i = 0; i < m_faces_lcc.size(); i++)
m_face_area_lcc[i] = m_face_area_lcc[i] * 2.0 * m_total_inliers / total_area;
std::size_t redirected = 0;
for (std::size_t i = 0; i < m_face_neighbors_lcc.size(); i++) {
// Check if the face is on a bbox face besides the top face.
// If so and the connected volume is below the ground, redirect the face to the bottom face node.
if (m_face_neighbors_lcc[i].second < 5 && m_volume_below_ground[m_face_neighbors_lcc[i].first - 6]) {
redirected++;
m_face_neighbors_lcc[i].second = 0;
}
}
std::cout << redirected << " faces redirected to below ground" << std::endl;
}
void count_volume_votes_lcc() {
const int debug_volume = -1;
FT total_volume = 0;
std::size_t num_volumes = m_kinetic_partition.number_of_volumes();
m_volume_votes.clear();
m_volume_votes.resize(num_volumes, std::make_pair(0, 0));
m_volumes.resize(num_volumes, 0);
for (std::size_t i = 0; i < num_volumes; i++) {
m_cost_matrix[0][i] = m_cost_matrix[1][i] = 0;
m_volumes[i] = 0;
}
std::size_t count_faces = 0;
std::size_t count_points = 0;
From_exact from_exact;
std::size_t idx = 0;
for (std::size_t i = 0; i < m_faces_lcc.size(); i++) {
std::size_t v[] = { -1, -1 };
Point_3 c[2];
std::size_t in[] = {0, 0}, out[] = {0, 0};
std::size_t idx = 0;
for (auto& vd : m_lcc.one_dart_per_incident_cell<3, 2>(m_faces_lcc[i])) {
typename LCC::Dart_descriptor vdh = m_lcc.dart_descriptor(vd);
v[idx] = m_lcc.info<3>(vdh).volume_id;
c[idx] = from_exact(m_lcc.info<3>(vdh).barycenter);
idx++;
}
for (std::size_t p : m_face_inliers[i]) {
const auto& point = get(m_point_map, p);
const auto& normal = get(m_normal_map, p);
count_points++;
for (std::size_t j = 0; j < idx; j++) {
const Vector_3 vec(point, c[j]);
const FT dot_product = vec * normal;
if (dot_product < FT(0))
in[j]++;
else
out[j]++;
}
}
for (std::size_t j = 0; j < idx; j++) {
m_volume_votes[v[j]].first += in[j];
m_volume_votes[v[j]].second += out[j];
m_cost_matrix[0][v[j] + 6] += static_cast<double>(in[j]);
m_cost_matrix[1][v[j] + 6] += static_cast<double>(out[j]);
}
}
for (auto &d : m_lcc.template one_dart_per_cell<3>()) {
typename LCC::Dart_descriptor dh = m_lcc.dart_descriptor(d);
std::vector<Point_3> volume_vertices;
std::size_t volume_index = m_lcc.info<3>(dh).volume_id;
// Collect all vertices of volume to calculate volume
for (auto &fd : m_lcc.one_dart_per_incident_cell<2, 3>(dh)) {
typename LCC::Dart_descriptor fdh = m_lcc.dart_descriptor(fd);
for (const auto &vd : m_lcc.one_dart_per_incident_cell<0, 2>(fdh))
volume_vertices.push_back(from_exact(m_lcc.point(m_lcc.dart_descriptor(vd))));
}
Delaunay_3 tri;
for (const Point_3& p : volume_vertices)
tri.insert(p);
m_volumes[volume_index] = FT(0);
for (auto cit = tri.finite_cells_begin(); cit != tri.finite_cells_end(); ++cit) {
const auto& tet = tri.tetrahedron(cit);
m_volumes[volume_index] += tet.volume();
}
total_volume += m_volumes[volume_index];
}
// Normalize volumes
for (FT& v : m_volumes)
v /= total_volume;
// for (std::size_t i = 0; i < m_volumes.size(); i++)
// std::cout << i << ": " << m_cost_matrix[0][i] << " o: " << m_cost_matrix[1][i] << " v: " << m_volumes[i] << std::endl;
}
template<typename NamedParameters>
void create_planar_shapes(bool estimate_ground, const NamedParameters& np) {
if (m_points.size() < 3) {
if (m_verbose) std::cout << "* no points found, skipping" << std::endl;
return;
}
if (m_verbose) std::cout << "* getting planar shapes using region growing" << std::endl;
// Parameters.
const std::size_t k = parameters::choose_parameter(
parameters::get_parameter(np, internal_np::k_neighbors), 12);
const FT max_distance_to_plane = parameters::choose_parameter(
parameters::get_parameter(np, internal_np::maximum_distance), FT(1));
const FT max_accepted_angle = parameters::choose_parameter(
parameters::get_parameter(np, internal_np::maximum_angle), FT(15));
const std::size_t min_region_size = parameters::choose_parameter(
parameters::get_parameter(np, internal_np::minimum_region_size), 50);
m_detection_distance_tolerance = max_distance_to_plane;
// Region growing.
Neighbor_query neighbor_query = CGAL::Shape_detection::Point_set::make_k_neighbor_query(
m_points, CGAL::parameters::k_neighbors(k));
Region_type region_type = CGAL::Shape_detection::Point_set::make_least_squares_plane_fit_region(
m_points,
CGAL::parameters::
maximum_distance(max_distance_to_plane).
maximum_angle(max_accepted_angle).
minimum_region_size(min_region_size));
Sorting sorting = CGAL::Shape_detection::Point_set::make_least_squares_plane_fit_sorting(m_points, neighbor_query);
sorting.sort();
Region_growing region_growing(
m_points, sorting.ordered(), neighbor_query, region_type);
region_growing.detect(std::back_inserter(m_regions));
std::size_t unassigned = 0;
region_growing.unassigned_items(m_points, boost::make_function_output_iterator([&](const auto&) { ++unassigned; }));
std::vector<std::vector<Point_3> > polys_debug;
for (std::size_t i = 0; i < m_regions.size(); i++) {
Indices region;
for (auto& j : m_regions[i].second)
region.push_back(j);
store_convex_hull_shape(region, m_regions[i].first, polys_debug);
//KSR_3::dump_polygon(polys_debug[i], std::to_string(i) + "-detected-region.ply");
}
KSP_3::dump_polygons(polys_debug, "detected-" + std::to_string(m_regions.size()) + "-polygons.ply");
// Convert indices.
m_planar_regions.clear();
m_planar_regions.reserve(m_regions.size());
// Copy planes for regularization.
std::vector<Plane_3> planes(m_regions.size());
for (std::size_t i = 0; i < m_regions.size(); i++)
planes[i] = m_regions[i].first;
auto range = m_regions | boost::adaptors::transformed([](typename Region_growing::Primitive_and_region& pr)->Plane_3& {return pr.first; });
std::size_t num_shapes = m_regions.size();
const bool regularize_axis_symmetry = parameters::choose_parameter(
parameters::get_parameter(np, internal_np::regularize_axis_symmetry), false);
const bool regularize_coplanarity = parameters::choose_parameter(
parameters::get_parameter(np, internal_np::regularize_coplanarity), false);
const bool regularize_orthogonality = parameters::choose_parameter(
parameters::get_parameter(np, internal_np::regularize_orthogonality), false);
const bool regularize_parallelism = parameters::choose_parameter(
parameters::get_parameter(np, internal_np::regularize_parallelism), false);
const FT angle_tolerance = parameters::choose_parameter(
parameters::get_parameter(np, internal_np::angle_tolerance), 25);
const FT maximum_offset = parameters::choose_parameter(
parameters::get_parameter(np, internal_np::maximum_offset), 0.01);
// Regularize detected planes.
CGAL::Shape_regularization::Planes::regularize_planes(range, m_points,
CGAL::parameters::plane_index_map(region_growing.region_map())
.point_map(m_point_map)
.regularize_axis_symmetry(regularize_axis_symmetry)
.regularize_orthogonality(regularize_orthogonality)
.regularize_parallelism(regularize_parallelism)
.regularize_coplanarity(regularize_coplanarity)
.maximum_angle(angle_tolerance)
.maximum_offset(maximum_offset));
polys_debug.clear();
for (std::size_t i = 0; i < m_regions.size(); i++) {
Indices region;
for (auto& j : m_regions[i].second)
region.push_back(j);
store_convex_hull_shape(region, m_regions[i].first, polys_debug);
//KSR_3::dump_polygon(polys_debug[i], std::to_string(i) + "-detected-region.ply");
}
//KSR_3::dump_polygons(polys_debug, "regularized-" + std::to_string(m_regions.size()) + "-polygons.ply");
// Merge coplanar regions
for (std::size_t i = 0; i < m_regions.size() - 1; i++) {
for (std::size_t j = i + 1; j < m_regions.size(); j++) {
if (m_regions[i].first == m_regions[j].first || m_regions[i].first.opposite() == m_regions[j].first) {
std::move(m_regions[j].second.begin(), m_regions[j].second.end(), std::back_inserter(m_regions[i].second));
m_regions.erase(m_regions.begin() + j);
j--;
}
}
}
if (estimate_ground) {
// How to estimate ground plane? Find low z peak
std::vector<std::size_t> candidates;
FT low_z_peak = (std::numeric_limits<FT>::max)();
FT bbox_min[] = { (std::numeric_limits<FT>::max)(), (std::numeric_limits<FT>::max)(), (std::numeric_limits<FT>::max)() };
FT bbox_max[] = { -(std::numeric_limits<FT>::max)(), -(std::numeric_limits<FT>::max)(), -(std::numeric_limits<FT>::max)() };
for (const auto& p : m_points) {
const auto& point = get(m_point_map, p);
for (std::size_t i = 0; i < 3; i++) {
bbox_min[i] = (std::min)(point[i], bbox_min[i]);
bbox_max[i] = (std::max)(point[i], bbox_max[i]);
}
}
FT bbox_center[] = { 0.5 * (bbox_min[0] + bbox_max[0]), 0.5 * (bbox_min[1] + bbox_max[1]), 0.5 * (bbox_min[2] + bbox_max[2]) };
for (std::size_t i = 0; i < m_regions.size(); i++) {
Vector_3 d = m_regions[i].first.orthogonal_vector();
if (abs(d.z()) > 0.98) {
candidates.push_back(i);
FT z = m_regions[i].first.projection(Point_3(bbox_center[0], bbox_center[1], bbox_center[2])).z();
low_z_peak = (std::min<FT>)(z, low_z_peak);
}
}
m_ground_polygon_index = -1;
std::vector<std::size_t> other_ground;
for (std::size_t i = 0; i < candidates.size(); i++) {
Vector_3 d = m_regions[candidates[i]].first.orthogonal_vector();
FT z = m_regions[candidates[i]].first.projection(Point_3(bbox_center[0], bbox_center[1], bbox_center[2])).z();
if (z - low_z_peak < max_distance_to_plane) {
if (m_ground_polygon_index == -1)
m_ground_polygon_index = candidates[i];
else
other_ground.push_back(candidates[i]);
}
}
for (std::size_t i = 0; i < other_ground.size(); i++)
std::move(m_regions[other_ground[i]].second.begin(), m_regions[other_ground[i]].second.end(), std::back_inserter(m_regions[m_ground_polygon_index].second));
std::cout << "ground polygon " << m_ground_polygon_index << ", merging other polygons";
while (other_ground.size() != 0) {
m_regions.erase(m_regions.begin() + other_ground.back());
std::cout << " " << other_ground.back();
other_ground.pop_back();
}
std::cout << std::endl;
std::vector<Point_3> ground_plane;
ground_plane.reserve(m_regions[m_ground_polygon_index].second.size());
for (std::size_t i = 0; i < m_regions[m_ground_polygon_index].second.size(); i++) {
ground_plane.push_back(get(m_point_map, m_regions[m_ground_polygon_index].second[i]));
}
CGAL::linear_least_squares_fitting_3(ground_plane.begin(), ground_plane.end(), m_regions[m_ground_polygon_index].first, CGAL::Dimension_tag<0>());
if (m_regions[m_ground_polygon_index].first.orthogonal_vector().z() < 0)
m_regions[m_ground_polygon_index].first = m_regions[m_ground_polygon_index].first.opposite();
}
polys_debug.clear();
for (std::size_t i = 0; i < m_regions.size(); i++) {
Indices region;
for (auto& j : m_regions[i].second)
region.push_back(j);
store_convex_hull_shape(region, m_regions[i].first, polys_debug);
//KSR_3::dump_polygon(polys_debug[i], std::to_string(i) + "-detected-region.ply");
}
//KSR_3::dump_polygons(polys_debug, "merged-" + std::to_string(m_regions.size()) + "-polygons.ply");
std::vector<Plane_3> pl;
std::size_t idx = 0;
for (const auto& p : m_regions) {
bool exists = false;
for (std::size_t i = 0; i < pl.size(); i++)
if (pl[i] == p.first || pl[i].opposite() == p.first)
exists = true;
if (!exists)
pl.push_back(p.first);
idx++;
}
for (const auto& pair : m_regions) {
Indices region;
for (auto& i : pair.second)
region.push_back(i);
m_planar_regions.push_back(region);
const std::size_t shape_idx = add_convex_hull_shape(region, pair.first);
CGAL_assertion(shape_idx != std::size_t(-1));
m_region_map[shape_idx] = region;
}
CGAL_assertion(m_planar_regions.size() == m_regions.size());
std::cout << "found " << num_shapes << " planar shapes regularized into " << m_planar_regions.size() << std::endl;
std::cout << "from " << m_points.size() << " input points " << unassigned << " remain unassigned" << std::endl;
}
void map_points_to_faces(const std::size_t polygon_index, const std::vector<Point_3>& pts, std::vector<std::pair<typename LCC::Dart_descriptor, std::vector<std::size_t> > >& face_to_points) {
std::vector<Index> faces;
if (polygon_index >= m_kinetic_partition.input_planes().size())
assert(false);
From_exact from_exact;
const typename Intersection_kernel::Plane_3& pl = m_kinetic_partition.input_planes()[polygon_index];
const Plane_3 inexact_pl = from_exact(pl);
std::vector<Point_2> pts2d;
pts2d.reserve(pts.size());
for (const Point_3& p : pts)
pts2d.push_back(inexact_pl.to_2d(p));
// Iterate over all faces of the lcc
for (Dart& d : m_lcc.template one_dart_per_cell<2>()) {
Dart_descriptor dd = m_lcc.dart_descriptor(d);
if (m_lcc.info<2>(m_lcc.dart_descriptor(d)).input_polygon_index != polygon_index || !m_lcc.info<2>(m_lcc.dart_descriptor(d)).part_of_initial_polygon)
continue;
// No filtering of points per partition
face_to_points.push_back(std::make_pair(m_lcc.dart_descriptor(d), std::vector<std::size_t>()));
auto& info = m_lcc.info<2>(m_lcc.dart_descriptor(d));
std::vector<Point_2> vts2d;
vts2d.reserve(m_lcc.one_dart_per_incident_cell<0, 2>(m_lcc.dart_descriptor(d)).size());
typename LCC::Dart_descriptor n = dd;
do {
vts2d.push_back(inexact_pl.to_2d(from_exact(m_lcc.point(n))));
n = m_lcc.beta(n, 0);
} while (n != dd);
Polygon_2<Kernel> poly(vts2d.begin(), vts2d.end());
if (poly.is_clockwise_oriented())
std::reverse(vts2d.begin(), vts2d.end());
for (std::size_t i = 0; i < pts2d.size(); i++) {
const auto& pt = pts2d[i];
bool outside = false;
// poly, vertices and edges in IFace are oriented ccw
std::size_t idx = 0;
for (std::size_t i = 0; i < vts2d.size(); i++) {
Vector_2 ts = (vts2d[(i + vts2d.size() - 1) % vts2d.size()]) - pt;
Vector_2 tt = (vts2d[i]) - pt;
bool ccw = (tt.x() * ts.y() - tt.y() * ts.x()) <= 0;
if (!ccw) {
outside = true;
break;
}
}
if (outside)
continue;
face_to_points.back().second.push_back(i);
}
}
}
/*
const Plane_3 fit_plane(const std::vector<std::size_t>& region) const {
std::vector<Point_3> points;
points.reserve(region.size());
for (const std::size_t idx : region) {
CGAL_assertion(idx < m_points.size());
points.push_back(get(m_point_map, idx));
}
CGAL_assertion(points.size() == region.size());
Plane_3 fitted_plane;
Point_3 fitted_centroid;
CGAL::linear_least_squares_fitting_3(
points.begin(), points.end(),
fitted_plane, fitted_centroid,
CGAL::Dimension_tag<0>());
const Plane_3 plane(
static_cast<FT>(fitted_plane.a()),
static_cast<FT>(fitted_plane.b()),
static_cast<FT>(fitted_plane.c()),
static_cast<FT>(fitted_plane.d()));
return plane;
}
*/
void set_outside_volumes(std::vector<std::vector<double> >& cost_matrix) const {
// Setting preferred outside label for bbox plane nodes
// Order:
// 0 zmin
// 1 ymin
// 2 xmax
// 3 ymax
// 4 xmin
// 5 zmax
const std::size_t force = m_total_inliers * 3;
// 0 - cost for labelled as outside
cost_matrix[0][0] = 0;
cost_matrix[0][1] = 0;
cost_matrix[0][2] = 0;
cost_matrix[0][3] = 0;
cost_matrix[0][4] = 0;
cost_matrix[0][5] = 0;
// 1 - cost for labelled as inside
cost_matrix[1][0] = 0;
cost_matrix[1][1] = force;
cost_matrix[1][2] = force;
cost_matrix[1][3] = force;
cost_matrix[1][4] = force;
cost_matrix[1][5] = force;
if (m_ground_polygon_index != -1) {
std::cout << "using estimated ground plane for reconstruction" << std::endl;
cost_matrix[0][1] = 0;
cost_matrix[0][2] = 0;
cost_matrix[0][3] = 0;
cost_matrix[0][4] = 0;
cost_matrix[1][1] = force;
cost_matrix[1][2] = force;
cost_matrix[1][3] = force;
cost_matrix[1][4] = force;
}
}
/*
void dump_volume(std::size_t i, const std::string& filename, const CGAL::Color &color) const {
std::vector<KSP::Index> faces;
m_kinetic_partition.faces(i, std::back_inserter(faces));
std::vector<std::vector<Point_3> > pts(faces.size());
std::vector<CGAL::Color> col(faces.size(), color);
for (std::size_t j = 0; j < faces.size(); j++) {
m_kinetic_partition.vertices(faces[j], std::back_inserter(pts[j]));
}
CGAL::KSR_3::dump_polygons(pts, col, filename);
}
void dump_face(std::size_t i, const std::string& filename, const CGAL::Color& color) const {
std::vector<Point_3> face;
m_kinetic_partition.vertices(m_faces[i], std::back_inserter(face));
}*/
};
#endif
} // namespace CGAL
#endif // CGAL_KINETIC_SHAPE_RECONSTRUCTION_3_H
Reference in New Issue View Git Blame Copy Permalink