Change structure_point_set() with generalized API

This commit is contained in:
Simon Giraudot 2017-05-17 12:47:09 +02:00
parent f1738645b9
commit 85634b7a27
4 changed files with 132 additions and 167 deletions

View File

@ -140,10 +140,15 @@ int main (int argc, char* argv[])
ransac.detect(op); // Plane detection
typename Efficient_ransac::Plane_range planes = ransac.planes();
std::cerr << "done\nPoint set structuring... ";
Pwn_vector structured_pts;
Structure pss (points.begin (), points.end (), ransac,
Structure pss (points, Point_map(), Normal_map(),
planes,
CGAL::Shape_detection_3::Plane_map<Traits>(),
CGAL::Shape_detection_3::Point_to_shape_index_map<Traits>(points, planes),
op.cluster_epsilon); // Same parameter as RANSAC
for (std::size_t i = 0; i < pss.size(); ++ i)

View File

@ -51,12 +51,16 @@ int main (int argc, char** argv)
ransac.add_shape_factory<Plane>();
ransac.detect();
typename Efficient_ransac::Plane_range planes = ransac.planes();
Pwn_vector structured_pts;
CGAL::structure_point_set (points.begin (), points.end (), // input points
std::back_inserter (structured_pts),
ransac, // shape detection engine
0.015); // epsilon for structuring points
CGAL::structure_point_set<Traits> (points, Point_map(), Normal_map(),
planes,
CGAL::Shape_detection_3::Plane_map<Traits>(),
CGAL::Shape_detection_3::Point_to_shape_index_map<Traits>(points, planes),
std::back_inserter (structured_pts),
0.015); // epsilon for structuring points
std::cerr << structured_pts.size ()
<< " structured point(s) generated." << std::endl;

View File

@ -24,7 +24,6 @@
#include <CGAL/license/Point_set_processing_3.h>
#include <CGAL/property_map.h>
#include <CGAL/point_set_processing_assertions.h>
#include <CGAL/assertions.h>
@ -73,13 +72,8 @@ class Point_set_with_structure
typedef typename Traits::FT FT;
typedef typename Traits::Segment_3 Segment;
typedef typename Traits::Line_3 Line;
typedef typename Traits::Plane_3 Plane;
typedef typename Traits::Point_2 Point_2;
typedef Shape_detection_3::Shape_base<Traits> Shape;
enum Point_status { POINT, RESIDUS, PLANE, EDGE, CORNER, SKIPPED };
public:
@ -87,11 +81,7 @@ public:
typedef typename Traits::Point_3 Point;
typedef typename Traits::Vector_3 Vector;
typedef typename Traits::Point_map Point_map;
typedef typename Traits::Normal_map Normal_map;
typedef typename Traits::Input_range Input_range;
typedef typename Input_range::iterator Input_iterator;
typedef Shape_detection_3::Plane<Traits> Plane_shape;
typedef typename Traits::Plane_3 Plane;
/// Tag classifying the coherence of a triplet of points with
/// respect to an inferred surface
@ -147,16 +137,13 @@ private:
};
Traits m_traits;
std::vector<Point> m_points;
std::vector<Vector> m_normals;
std::vector<std::size_t> m_indices;
std::vector<Point_status> m_status;
Point_map m_point_map;
Normal_map m_normal_map;
std::vector<boost::shared_ptr<Plane_shape> > m_planes;
std::vector<Plane> m_planes;
std::vector<std::vector<std::size_t> > m_indices_of_assigned_points;
std::vector<Edge> m_edges;
std::vector<Corner> m_corners;
@ -170,66 +157,49 @@ public:
\note Both property maps can be omitted if the default constructors of these property maps can be safely used.
*/
Point_set_with_structure (Input_iterator begin, ///< iterator over the first input point.
Input_iterator end, ///< past-the-end iterator over the input points.
Point_map point_map, ///< property map: value_type of InputIterator -> Point_3.
Normal_map normal_map, ///< property map: value_type of InputIterator -> Vector_3.
const Shape_detection_3::Efficient_RANSAC<Traits>&
shape_detection, ///< shape detection object
template <typename PointRange,
typename PointMap,
typename NormalMap,
typename PlaneRange,
typename PlaneMap,
typename IndexMap>
Point_set_with_structure (const PointRange& points,
PointMap point_map,
NormalMap normal_map,
const PlaneRange& planes,
PlaneMap plane_map,
IndexMap index_map,
double epsilon, ///< size parameter
double attraction_factor = 3.) ///< attraction factor
: m_traits (shape_detection.traits()),
m_point_map(point_map), m_normal_map (normal_map)
{
constructor (begin, end, shape_detection, epsilon, attraction_factor);
}
m_points.reserve(points.size());
m_normals.reserve(points.size());
m_indices_of_assigned_points.resize (planes.size());
/// \cond SKIP_IN_MANUAL
Point_set_with_structure (Input_iterator begin, ///< iterator over the first input point.
Input_iterator end, ///< past-the-end iterator over the input points.
const Shape_detection_3::Efficient_RANSAC<Traits>&
shape_detection, ///< shape detection object
double epsilon, ///< size parameter
double attraction_factor = 3.) ///< attraction factor
: m_traits (shape_detection.traits())
{
constructor (begin, end, shape_detection, epsilon, attraction_factor);
}
m_indices.resize (points.size (), (std::numeric_limits<std::size_t>::max)());
m_status.resize (points.size (), POINT);
void constructor(Input_iterator begin, ///< iterator over the first input point.
Input_iterator end, ///< past-the-end iterator over the input points.
const Shape_detection_3::Efficient_RANSAC<Traits>&
shape_detection, ///< shape detection object
double epsilon, ///< size parameter
double attraction_factor = 3.) ///< attraction factor
{
m_points.reserve(end - begin);
m_normals.reserve(end - begin);
for (Input_iterator it = begin; it != end; ++ it)
std::size_t idx = 0;
for (typename PointRange::const_iterator it = points.begin();
it != points.end(); ++ it)
{
m_points.push_back (get(point_map, *it));
m_normals.push_back (get(normal_map, *it));
int plane_index = get (index_map, idx);
if (plane_index != -1)
{
m_points.push_back (get(m_point_map, *it));
m_normals.push_back (get(m_normal_map, *it));
m_indices_of_assigned_points[std::size_t(plane_index)].push_back(idx);
m_indices[idx] = std::size_t(plane_index);
m_status[idx] = PLANE;
}
++ idx;
}
m_indices.resize (m_points.size (), (std::numeric_limits<std::size_t>::max)());
m_status.resize (m_points.size (), POINT);
BOOST_FOREACH (boost::shared_ptr<Shape> shape, shape_detection.shapes())
{
boost::shared_ptr<Plane_shape> pshape
= boost::dynamic_pointer_cast<Plane_shape>(shape);
// Ignore all shapes other than plane
if (pshape == boost::shared_ptr<Plane_shape>())
continue;
m_planes.push_back (pshape);
for (std::size_t i = 0; i < pshape->indices_of_assigned_points().size (); ++ i)
{
m_indices[pshape->indices_of_assigned_points()[i]] = m_planes.size () - 1;
m_status[pshape->indices_of_assigned_points()[i]] = PLANE;
}
}
m_planes.reserve (planes.size());
for (typename PlaneRange::const_iterator it = planes.begin();
it != planes.end(); ++ it)
m_planes.push_back (get (plane_map, *it));
run (epsilon, attraction_factor);
clean ();
@ -253,23 +223,21 @@ public:
vertices.
*/
std::vector<boost::shared_ptr<Plane_shape> > adjacency (std::size_t i) const
template <typename OutputIterator>
void adjacency (std::size_t i, OutputIterator output) const
{
std::vector<boost::shared_ptr<Plane_shape> > out;
if (m_status[i] == PLANE || m_status[i] == RESIDUS)
out.push_back (m_planes[m_indices[i]]);
*(output ++) = m_planes[m_indices[i]];
else if (m_status[i] == EDGE)
{
out.push_back (m_planes[m_edges[m_indices[i]].planes[0]]);
out.push_back (m_planes[m_edges[m_indices[i]].planes[1]]);
*(output ++) = m_planes[m_edges[m_indices[i]].planes[0]];
*(output ++) = m_planes[m_edges[m_indices[i]].planes[1]];
}
else if (m_status[i] == CORNER)
{
for (std::size_t j = 0; j < m_corners[m_indices[i]].planes.size(); ++ j)
out.push_back (m_planes[m_corners[m_indices[i]].planes[j]]);
*(output ++) = m_planes[m_corners[m_indices[i]].planes[j]];
}
return out;
}
/*!
@ -546,11 +514,11 @@ private:
void project_inliers ()
{
for(std::size_t i = 0; i < m_planes.size (); ++ i)
for (std::size_t j = 0; j < m_planes[i]->indices_of_assigned_points ().size(); ++ j)
for(std::size_t i = 0; i < m_indices_of_assigned_points.size (); ++ i)
for (std::size_t j = 0; j < m_indices_of_assigned_points[i].size(); ++ j)
{
std::size_t ind = m_planes[i]->indices_of_assigned_points ()[j];
m_points[ind] = static_cast<Plane> (*(m_planes[i])).projection (m_points[ind]);
std::size_t ind = m_indices_of_assigned_points[i][j];
m_points[ind] = m_planes[i].projection (m_points[ind]);
}
}
@ -561,7 +529,7 @@ private:
for (std::size_t c = 0; c < m_planes.size (); ++ c)
{
//plane attributes and 2D projection vectors
Plane plane = static_cast<Plane> (*(m_planes[c]));
const Plane& plane = m_planes[c];
Vector vortho = plane.orthogonal_vector();
Vector b1 = plane.base1();
Vector b2 = plane.base2();
@ -572,9 +540,9 @@ private:
std::vector<Point_2> points_2d;
//storage of the 2D points in "pt_2d"
for (std::size_t j = 0; j < m_planes[c]->indices_of_assigned_points ().size(); ++ j)
for (std::size_t j = 0; j < m_indices_of_assigned_points[c].size(); ++ j)
{
std::size_t ind = m_planes[c]->indices_of_assigned_points ()[j];
std::size_t ind = m_indices_of_assigned_points[c][j];
const Point& pt = m_points[ind];
points_2d.push_back (Point_2 (b1.x() * pt.x() + b1.y() * pt.y() + b1.z() * pt.z(),
b2.x() * pt.x() + b2.y() * pt.y() + b2.z() * pt.z()));
@ -597,7 +565,7 @@ private:
std::size_t ind_x = static_cast<std::size_t>((points_2d[i].x() - box_2d.xmin()) / grid_length);
std::size_t ind_y = static_cast<std::size_t>((points_2d[i].y() - box_2d.ymin()) / grid_length);
Mask[ind_x][ind_y] = true;
point_map[ind_x][ind_y].push_back (m_planes[c]->indices_of_assigned_points ()[i]);
point_map[ind_x][ind_y].push_back (m_indices_of_assigned_points[c][i]);
}
//hole filing in Mask in 4-connexity
@ -657,7 +625,7 @@ private:
std::size_t index_pt = point_map[i][j][0];
m_points[index_pt] = Point (X1, X2, X3);
m_normals[index_pt] = m_planes[c]->plane_normal();
m_normals[index_pt] = m_planes[c].orthogonal_vector();
m_status[index_pt] = PLANE;
for (std::size_t np = 1; np < point_map[i][j].size(); ++ np)
@ -692,7 +660,7 @@ private:
FT X3 = x2pt * b1.z() + y2pt * b2.z() - plane.d() * vortho.z();
m_points.push_back (Point (X1, X2, X3));
m_normals.push_back (m_planes[c]->plane_normal());
m_normals.push_back (m_planes[c].orthogonal_vector());
m_indices.push_back (c);
m_status.push_back (RESIDUS);
}
@ -754,15 +722,14 @@ private:
{
for (std::size_t i = 0; i < m_edges.size(); ++ i)
{
boost::shared_ptr<Plane_shape> plane1 = m_planes[m_edges[i].planes[0]];
boost::shared_ptr<Plane_shape> plane2 = m_planes[m_edges[i].planes[1]];
const Plane& plane1 = m_planes[m_edges[i].planes[0]];
const Plane& plane2 = m_planes[m_edges[i].planes[1]];
double angle_A = std::acos (CGAL::abs (plane1->plane_normal() * plane2->plane_normal()));
double angle_A = std::acos (CGAL::abs (plane1.orthogonal_vector() * plane2.orthogonal_vector()));
double angle_B = CGAL_PI - angle_A;
typename cpp11::result_of<typename Traits::Intersect_3(Plane, Plane)>::type
result = CGAL::intersection(static_cast<Plane>(*plane1),
static_cast<Plane>(*plane2));
result = CGAL::intersection(plane1, plane2);
if (!result)
{
@ -783,9 +750,9 @@ private:
}
Vector direction_p1 (0., 0., 0.);
for (std::size_t k = 0; k < plane1->indices_of_assigned_points ().size(); ++ k)
for (std::size_t k = 0; k < m_indices_of_assigned_points[m_edges[i].planes[0]].size(); ++ k)
{
std::size_t index_point = plane1->indices_of_assigned_points ()[k];
std::size_t index_point = m_indices_of_assigned_points[m_edges[i].planes[0]][k];
const Point& point = m_points[index_point];
Point projected = m_edges[i].support.projection (point);
@ -798,9 +765,9 @@ private:
direction_p1 = direction_p1 / std::sqrt (direction_p1 * direction_p1);
Vector direction_p2 (0., 0., 0.);
for (std::size_t k = 0; k < plane2->indices_of_assigned_points ().size(); ++ k)
for (std::size_t k = 0; k < m_indices_of_assigned_points[m_edges[i].planes[1]].size(); ++ k)
{
std::size_t index_point = plane2->indices_of_assigned_points ()[k];
std::size_t index_point = m_indices_of_assigned_points[m_edges[i].planes[1]][k];
const Point& point = m_points[index_point];
Point projected = m_edges[i].support.projection (point);
@ -831,8 +798,8 @@ private:
for (std::size_t i = 0; i < m_edges.size(); ++ i)
{
boost::shared_ptr<Plane_shape> plane1 = m_planes[m_edges[i].planes[0]];
boost::shared_ptr<Plane_shape> plane2 = m_planes[m_edges[i].planes[1]];
const Plane& plane1 = m_planes[m_edges[i].planes[0]];
const Plane& plane2 = m_planes[m_edges[i].planes[1]];
const Line& line = m_edges[i].support;
@ -841,21 +808,21 @@ private:
continue;
}
Vector normal = 0.5 * plane1->plane_normal () + 0.5 * plane2->plane_normal();
Vector normal = 0.5 * plane1.orthogonal_vector () + 0.5 * plane2.orthogonal_vector();
//find set of points close (<attraction_radius) to the edge and store in intersection_points
std::vector<std::size_t> intersection_points;
for (std::size_t k = 0; k < plane1->indices_of_assigned_points().size(); ++ k)
for (std::size_t k = 0; k < m_indices_of_assigned_points[m_edges[i].planes[0]].size(); ++ k)
{
std::size_t index_point = plane1->indices_of_assigned_points()[k];
std::size_t index_point = m_indices_of_assigned_points[m_edges[i].planes[0]][k];
const Point& point = m_points[index_point];
Point projected = line.projection (point);
if (CGAL::squared_distance (point, projected) < radius * radius)
intersection_points.push_back (index_point);
}
for (std::size_t k = 0; k < plane2->indices_of_assigned_points().size(); ++ k)
for (std::size_t k = 0; k < m_indices_of_assigned_points[m_edges[i].planes[1]].size(); ++ k)
{
std::size_t index_point = plane2->indices_of_assigned_points()[k];
std::size_t index_point = m_indices_of_assigned_points[m_edges[i].planes[1]][k];
const Point& point = m_points[index_point];
Point projected = line.projection (point);
if (CGAL::squared_distance (point, projected) < radius * radius)
@ -991,7 +958,7 @@ private:
}
typename cpp11::result_of<typename Traits::Intersect_3(Plane, Plane)>::type
result = CGAL::intersection (static_cast<Plane> (*plane1), ortho);
result = CGAL::intersection (plane1, ortho);
if (result)
{
if (const Line* l = boost::get<Line>(&*result))
@ -1008,7 +975,7 @@ private:
Point anchor1 = anchor + vecp1 * r_edge;
m_points.push_back (anchor1);
m_normals.push_back (m_planes[m_edges[i].planes[0]]->plane_normal());
m_normals.push_back (m_planes[m_edges[i].planes[0]].orthogonal_vector());
m_indices.push_back (m_edges[i].planes[0]);
m_status.push_back (PLANE);
}
@ -1028,7 +995,7 @@ private:
}
result = CGAL::intersection (static_cast<Plane> (*plane2),ortho);
result = CGAL::intersection (plane2,ortho);
if (result)
{
if (const Line* l = boost::get<Line>(&*result))
@ -1045,7 +1012,7 @@ private:
Point anchor2 = anchor + vecp2 * r_edge;
m_points.push_back (anchor2);
m_normals.push_back (m_planes[m_edges[i].planes[1]]->plane_normal());
m_normals.push_back (m_planes[m_edges[i].planes[1]].orthogonal_vector());
m_indices.push_back (m_edges[i].planes[1]);
m_status.push_back (PLANE);
}
@ -1151,9 +1118,9 @@ private:
for (std::size_t i = 0; i < m_corners.size (); ++ i)
{
//calcul pt d'intersection des 3 plans
Plane plane1 = static_cast<Plane> (*(m_planes[m_corners[i].planes[0]]));
Plane plane2 = static_cast<Plane> (*(m_planes[m_corners[i].planes[1]]));
Plane plane3 = static_cast<Plane> (*(m_planes[m_corners[i].planes[2]]));
const Plane& plane1 = m_planes[m_corners[i].planes[0]];
const Plane& plane2 = m_planes[m_corners[i].planes[1]];
const Plane& plane3 = m_planes[m_corners[i].planes[2]];
typename cpp11::result_of<typename Traits::Intersect_3(Plane, Plane)>::type
result = CGAL::intersection(plane1, plane2);
@ -1307,7 +1274,7 @@ private:
Vector normal (0., 0., 0.);
for (std::size_t i = 0; i < m_corners[k].planes.size(); ++ i)
normal = normal + (1. / (double)(m_corners[k].planes.size()))
* m_planes[m_corners[k].planes[i]]->plane_normal();
* m_planes[m_corners[k].planes[i]].orthogonal_vector();
m_points.push_back (m_corners[k].support);
m_normals.push_back (normal);
@ -1415,8 +1382,8 @@ private:
{
Point new_edge = m_corners[k].support + m_corners[k].directions[ed] * d_DeltaEdge;
m_points.push_back (new_edge);
m_normals.push_back (0.5 * m_planes[m_edges[m_corners[k].edges[ed]].planes[0]]->plane_normal()
+ 0.5 * m_planes[m_edges[m_corners[k].edges[ed]].planes[1]]->plane_normal());
m_normals.push_back (0.5 * m_planes[m_edges[m_corners[k].edges[ed]].planes[0]].orthogonal_vector()
+ 0.5 * m_planes[m_edges[m_corners[k].edges[ed]].planes[1]].orthogonal_vector());
m_status.push_back (EDGE);
m_indices.push_back (m_corners[k].edges[ed]);
edge.indices.push_back (m_points.size() - 1);
@ -1425,8 +1392,8 @@ private:
//rajouter un edge a 1/3 epsilon du cote dominant
Point new_edge = m_corners[k].support + m_corners[k].directions[ed] * d_DeltaEdge / 3;
m_points.push_back (new_edge);
m_normals.push_back (0.5 * m_planes[m_edges[m_corners[k].edges[ed]].planes[0]]->plane_normal()
+ 0.5 * m_planes[m_edges[m_corners[k].edges[ed]].planes[1]]->plane_normal());
m_normals.push_back (0.5 * m_planes[m_edges[m_corners[k].edges[ed]].planes[0]].orthogonal_vector()
+ 0.5 * m_planes[m_edges[m_corners[k].edges[ed]].planes[1]].orthogonal_vector());
m_status.push_back (EDGE);
m_indices.push_back (m_corners[k].edges[ed]);
edge.indices.push_back (m_points.size() - 1);
@ -1474,21 +1441,27 @@ private:
///
/// @note Both property maps can be omitted if the default constructors of these property maps can be safely used.
template <typename Traits,
typename PointRange,
typename PointMap,
typename NormalMap,
typename PlaneRange,
typename PlaneMap,
typename IndexMap,
typename OutputIterator
>
OutputIterator
structure_point_set (typename Traits::Input_range::iterator first, ///< iterator over the first input point.
typename Traits::Input_range::iterator beyond, ///< past-the-end iterator over the input points.
typename Traits::Point_map point_map, ///< property map: value_type of InputIterator -> Point_3.
typename Traits::Normal_map normal_map, ///< property map: value_type of InputIterator -> Vector_3.
structure_point_set (const PointRange& points,
PointMap point_map,
NormalMap normal_map,
const PlaneRange& planes,
PlaneMap plane_map,
IndexMap index_map,
OutputIterator output, ///< output iterator where output points are written
Shape_detection_3::Efficient_RANSAC<Traits>&
shape_detection, ///< shape detection object
double epsilon, ///< size parameter
double attraction_factor = 3.) ///< attraction factor
{
Point_set_with_structure<Traits> pss (first, beyond, point_map, normal_map,
shape_detection, epsilon, attraction_factor);
Point_set_with_structure<Traits> pss (points, point_map, normal_map, planes, plane_map, index_map,
epsilon, attraction_factor);
for (std::size_t i = 0; i < pss.size(); ++ i)
*(output ++) = pss[i];
@ -1497,28 +1470,6 @@ structure_point_set (typename Traits::Input_range::iterator first, ///< iterato
}
/// \cond SKIP_IN_MANUAL
template <typename Traits,
typename OutputIterator
>
OutputIterator
structure_point_set (typename Traits::Input_range::iterator first, ///< iterator over the first input point.
typename Traits::Input_range::iterator beyond, ///< past-the-end iterator over the input points.
OutputIterator output, ///< output iterator where output points are written
Shape_detection_3::Efficient_RANSAC<Traits>&
shape_detection, ///< shape detection object
double epsilon, ///< size parameter
double attraction_factor = 3.) ///< attraction factor
{
return structure_point_set (first, beyond,
typename Traits::Point_map(),
typename Traits::Normal_map(),
output,
shape_detection,
epsilon,
attraction_factor);
}
/// \endcond

View File

@ -25,7 +25,7 @@
#include <CGAL/Delaunay_triangulation_2.h>
#include <CGAL/Alpha_shape_2.h>
//#include <CGAL/structure_point_set.h>
#include <CGAL/structure_point_set.h>
#include <QObject>
#include <QAction>
@ -409,23 +409,28 @@ private:
{
std::cerr << "Structuring point set... ";
// Scene_points_with_normal_item *pts_full = new Scene_points_with_normal_item;
// pts_full->point_set()->add_normal_map();
// CGAL::structure_point_set (points->begin (), points->end (),
// points->point_map(), points->normal_map(),
// boost::make_function_output_iterator (build_from_pair ((*(pts_full->point_set())))),
// shape_detection,
// op.cluster_epsilon);
// if (pts_full->point_set ()->empty ())
// delete pts_full;
// else
// {
// pts_full->point_set ()->unselect_all();
// pts_full->setName(tr("%1 (structured)").arg(item->name()));
// pts_full->setRenderingMode(PointsPlusNormals);
// pts_full->setColor(Qt::blue);
// scene->addItem (pts_full);
// }
Scene_points_with_normal_item *pts_full = new Scene_points_with_normal_item;
pts_full->point_set()->add_normal_map();
typename Shape_detection::Plane_range planes = shape_detection.planes();
CGAL::structure_point_set<Traits> (*points,
points->point_map(), points->normal_map(),
planes,
CGAL::Shape_detection_3::Plane_map<Traits>(),
CGAL::Shape_detection_3::Point_to_shape_index_map<Traits>(*points, planes),
boost::make_function_output_iterator (build_from_pair ((*(pts_full->point_set())))),
op.cluster_epsilon);
if (pts_full->point_set ()->empty ())
delete pts_full;
else
{
pts_full->point_set ()->unselect_all();
pts_full->setName(tr("%1 (structured)").arg(item->name()));
pts_full->setRenderingMode(PointsPlusNormals);
pts_full->setColor(Qt::blue);
scene->addItem (pts_full);
}
std::cerr << "done" << std::endl;
}