Skip to content

Commit 8182d25

Browse files
aliosciapetrellitaketwo
aliosciapetrelli
authored and
taketwo
committed
FLARELocalReferenceFrameEstimation class added (#1571)
Add implementation of the Fast LocAl Reference framE (FLARE) algorithm
1 parent 857a197 commit 8182d25

File tree

8 files changed

+891
-1
lines changed

8 files changed

+891
-1
lines changed

‎common/include/pcl/common/geometry.h

Lines changed: 57 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -43,6 +43,7 @@
4343
#endif
4444

4545
#include <Eigen/Core>
46+
#include <pcl/console/print.h>
4647

4748
/**
4849
* \file common/geometry.h
@@ -101,6 +102,62 @@ namespace pcl
101102
float lambda = plane_normal.dot(po);
102103
projected = point - (lambda * plane_normal);
103104
}
105+
106+
107+
/** \brief Given a plane defined by plane_origin and plane_normal, find the unit vector pointing from plane_origin to the projection of point on the plane.
108+
*
109+
* \param[in] point Point projected on the plane
110+
* \param[in] plane_origin The plane origin
111+
* \param[in] plane_normal The plane normal
112+
* \return unit vector pointing from plane_origin to the projection of point on the plane.
113+
* \ingroup geometry
114+
*/
115+
inline Eigen::Vector3f
116+
projectedAsUnitVector (Eigen::Vector3f const &point,
117+
Eigen::Vector3f const &plane_origin,
118+
Eigen::Vector3f const &plane_normal)
119+
{
120+
Eigen::Vector3f projection;
121+
project (point, plane_origin, plane_normal, projection);
122+
Eigen::Vector3f projected_as_unit_vector = projection - plane_origin;
123+
projected_as_unit_vector.normalize ();
124+
return projected_as_unit_vector;
125+
}
126+
127+
128+
/** \brief Define a random unit vector orthogonal to axis.
129+
*
130+
* \param[in] axis Axis
131+
* \return random unit vector orthogonal to axis
132+
* \ingroup geometry
133+
*/
134+
inline Eigen::Vector3f
135+
randomOrthogonalAxis (Eigen::Vector3f const &axis)
136+
{
137+
Eigen::Vector3f rand_ortho_axis;
138+
rand_ortho_axis.setRandom();
139+
if (std::abs (axis.z ()) > 1E-8f)
140+
{
141+
rand_ortho_axis.z () = -(axis.x () * rand_ortho_axis.x () + axis.y () * rand_ortho_axis.y ()) / axis.z ();
142+
}
143+
else if (std::abs (axis.y ()) > 1E-8f)
144+
{
145+
rand_ortho_axis.y () = -(axis.x () * rand_ortho_axis.x () + axis.z () * rand_ortho_axis.z ()) / axis.y ();
146+
}
147+
else if (std::abs (axis.x ()) > 1E-8f)
148+
{
149+
rand_ortho_axis.x () = -(axis.y () * rand_ortho_axis.y () + axis.z () * rand_ortho_axis.z ()) / axis.x ();
150+
}
151+
else
152+
{
153+
PCL_WARN ("[pcl::randomOrthogonalAxis] provided axis has norm < 1E-8f");
154+
}
155+
156+
rand_ortho_axis.normalize ();
157+
return rand_ortho_axis;
158+
}
159+
160+
104161
}
105162
}
106163

‎features/CMakeLists.txt

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,7 @@ if(build)
1515
"include/pcl/${SUBSYS_NAME}/boost.h"
1616
"include/pcl/${SUBSYS_NAME}/eigen.h"
1717
"include/pcl/${SUBSYS_NAME}/board.h"
18+
"include/pcl/${SUBSYS_NAME}/flare.h"
1819
"include/pcl/${SUBSYS_NAME}/brisk_2d.h"
1920
"include/pcl/${SUBSYS_NAME}/cppf.h"
2021
"include/pcl/${SUBSYS_NAME}/cvfh.h"
@@ -66,6 +67,7 @@ if(build)
6667

6768
set(impl_incs
6869
"include/pcl/${SUBSYS_NAME}/impl/board.hpp"
70+
"include/pcl/${SUBSYS_NAME}/impl/flare.hpp"
6971
"include/pcl/${SUBSYS_NAME}/impl/brisk_2d.hpp"
7072
"include/pcl/${SUBSYS_NAME}/impl/cppf.hpp"
7173
"include/pcl/${SUBSYS_NAME}/impl/cvfh.hpp"
@@ -114,6 +116,7 @@ if(build)
114116

115117
set(srcs
116118
src/board.cpp
119+
src/flare.cpp
117120
src/brisk_2d.cpp
118121
src/boundary.cpp
119122
src/cppf.cpp

‎features/include/pcl/features/flare.h

Lines changed: 293 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,293 @@
1+
/*
2+
* Software License Agreement (BSD License)
3+
*
4+
* Point Cloud Library (PCL) - www.pointclouds.org
5+
* Copyright (c) 2016-, Open Perception, Inc.
6+
*
7+
* All rights reserved.
8+
*
9+
* Redistribution and use in source and binary forms, with or without
10+
* modification, are permitted provided that the following conditions
11+
* are met:
12+
*
13+
* * Redistributions of source code must retain the above copyright
14+
* notice, this list of conditions and the following disclaimer.
15+
* * Redistributions in binary form must reproduce the above
16+
* copyright notice, this list of conditions and the following
17+
* disclaimer in the documentation and/or other materials provided
18+
* with the distribution.
19+
* * Neither the name of the copyright holder(s) nor the names of its
20+
* contributors may be used to endorse or promote products derived
21+
* from this software without specific prior written permission.
22+
*
23+
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24+
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25+
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26+
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27+
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28+
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29+
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30+
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31+
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32+
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33+
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34+
* POSSIBILITY OF SUCH DAMAGE.
35+
*
36+
*
37+
*/
38+
39+
#ifndef PCL_FLARE_H_
40+
#define PCL_FLARE_H_
41+
42+
#include <pcl/point_types.h>
43+
#include <pcl/features/feature.h>
44+
#include <pcl/features/normal_3d.h>
45+
46+
47+
namespace pcl
48+
{
49+
50+
/** \brief FLARELocalReferenceFrameEstimation implements the Fast LocAl Reference framE algorithm
51+
* for local reference frame estimation as described here:
52+
*
53+
* - A. Petrelli, L. Di Stefano,
54+
* "A repeatable and efficient canonical reference for surface matching",
55+
* 3DimPVT, 2012
56+
*
57+
* FLARE algorithm is deployed in ReLOC algorithm proposed in:
58+
*
59+
* Petrelli A., Di Stefano L., "Pairwise registration by local orientation cues", Computer Graphics Forum, 2015.
60+
*
61+
* \author Alioscia Petrelli
62+
* \ingroup features
63+
*/
64+
template<typename PointInT, typename PointNT, typename PointOutT = ReferenceFrame, typename SignedDistanceT = float>
65+
class FLARELocalReferenceFrameEstimation : public FeatureFromNormals<PointInT, PointNT, PointOutT>
66+
{
67+
protected:
68+
using Feature<PointInT, PointOutT>::feature_name_;
69+
using Feature<PointInT, PointOutT>::input_;
70+
using Feature<PointInT, PointOutT>::indices_;
71+
using Feature<PointInT, PointOutT>::surface_;
72+
using Feature<PointInT, PointOutT>::tree_;
73+
using Feature<PointInT, PointOutT>::search_parameter_;
74+
using FeatureFromNormals<PointInT, PointNT, PointOutT>::normals_;
75+
using Feature<PointInT, PointOutT>::fake_surface_;
76+
using Feature<PointInT, PointOutT>::getClassName;
77+
78+
using typename Feature<PointInT, PointOutT>::PointCloudIn;
79+
using typename Feature<PointInT, PointOutT>::PointCloudOut;
80+
81+
using typename Feature<PointInT, PointOutT>::PointCloudInConstPtr;
82+
83+
using typename Feature<PointInT, PointOutT>::KdTreePtr;
84+
85+
typedef typename pcl::PointCloud<SignedDistanceT> PointCloudSignedDistance;
86+
typedef typename PointCloudSignedDistance::Ptr PointCloudSignedDistancePtr;
87+
88+
typedef boost::shared_ptr<FLARELocalReferenceFrameEstimation<PointInT, PointNT, PointOutT> > Ptr;
89+
typedef boost::shared_ptr<const FLARELocalReferenceFrameEstimation<PointInT, PointNT, PointOutT> > ConstPtr;
90+
91+
public:
92+
/** \brief Constructor. */
93+
FLARELocalReferenceFrameEstimation () :
94+
tangent_radius_ (0.0f),
95+
margin_thresh_ (0.85f),
96+
min_neighbors_for_normal_axis_ (6),
97+
min_neighbors_for_tangent_axis_ (6),
98+
sampled_surface_ (),
99+
sampled_tree_ (),
100+
fake_sampled_surface_ (false)
101+
{
102+
feature_name_ = "FLARELocalReferenceFrameEstimation";
103+
}
104+
105+
//Getters/Setters
106+
107+
/** \brief Set the maximum distance of the points used to estimate the x_axis of the FLARE Reference Frame for a given point.
108+
*
109+
* \param[in] radius The search radius for x axis.
110+
*/
111+
inline void
112+
setTangentRadius (float radius)
113+
{
114+
tangent_radius_ = radius;
115+
}
116+
117+
/** \brief Get the maximum distance of the points used to estimate the x_axis of the FLARE Reference Frame for a given point.
118+
*
119+
* \return The search radius for x axis.
120+
*/
121+
inline float
122+
getTangentRadius () const
123+
{
124+
return (tangent_radius_);
125+
}
126+
127+
/** \brief Set the percentage of the search tangent radius after which a point is considered part of the support.
128+
*
129+
* \param[in] margin_thresh the percentage of the search tangent radius after which a point is considered part of the support.
130+
*/
131+
inline void
132+
setMarginThresh (float margin_thresh)
133+
{
134+
margin_thresh_ = margin_thresh;
135+
}
136+
137+
/** \brief Get the percentage of the search tangent radius after which a point is considered part of the support.
138+
*
139+
* \return The percentage of the search tangent radius after which a point is considered part of the support.
140+
*/
141+
inline float
142+
getMarginThresh () const
143+
{
144+
return (margin_thresh_);
145+
}
146+
147+
148+
/** \brief Set min number of neighbours required for the computation of Z axis.
149+
*
150+
* \param[in] min_neighbors_for_normal_axis min number of neighbours required for the computation of Z axis.
151+
*/
152+
inline void
153+
setMinNeighboursForNormalAxis (int min_neighbors_for_normal_axis)
154+
{
155+
min_neighbors_for_normal_axis_ = min_neighbors_for_normal_axis;
156+
}
157+
158+
/** \brief Get min number of neighbours required for the computation of Z axis.
159+
*
160+
* \return min number of neighbours required for the computation of Z axis.
161+
*/
162+
inline int
163+
getMinNeighboursForNormalAxis () const
164+
{
165+
return (min_neighbors_for_normal_axis_);
166+
}
167+
168+
169+
/** \brief Set min number of neighbours required for the computation of X axis.
170+
*
171+
* \param[in] min_neighbors_for_tangent_axis min number of neighbours required for the computation of X axis.
172+
*/
173+
inline void
174+
setMinNeighboursForTangentAxis (int min_neighbors_for_tangent_axis)
175+
{
176+
min_neighbors_for_tangent_axis_ = min_neighbors_for_tangent_axis;
177+
}
178+
179+
/** \brief Get min number of neighbours required for the computation of X axis.
180+
*
181+
* \return min number of neighbours required for the computation of X axis.
182+
*/
183+
inline int
184+
getMinNeighboursForTangentAxis () const
185+
{
186+
return (min_neighbors_for_tangent_axis_);
187+
}
188+
189+
190+
/** \brief Provide a pointer to the dataset used for the estimation of X axis.
191+
* As the estimation of x axis is negligibly affected by surface downsampling,
192+
* this method lets to consider a downsampled version of surface_ in the estimation of x axis.
193+
* This is optional, if this is not set, it will only use the data in the
194+
* surface_ cloud to estimate the x axis.
195+
* \param[in] cloud a pointer to a PointCloud
196+
*/
197+
inline void
198+
setSearchSampledSurface(const PointCloudInConstPtr &cloud)
199+
{
200+
sampled_surface_ = cloud;
201+
fake_sampled_surface_ = false;
202+
}
203+
204+
/** \brief Get a pointer to the sampled_surface_ cloud dataset. */
205+
inline const PointCloudInConstPtr&
206+
getSearchSampledSurface() const
207+
{
208+
return (sampled_surface_);
209+
}
210+
211+
/** \brief Provide a pointer to the search object linked to sampled_surface.
212+
* \param[in] tree a pointer to the spatial search object linked to sampled_surface.
213+
*/
214+
inline void
215+
setSearchMethodForSampledSurface (const KdTreePtr &tree) { sampled_tree_ = tree; }
216+
217+
/** \brief Get a pointer to the search method used for the extimation of x axis. */
218+
inline const KdTreePtr&
219+
getSearchMethodForSampledSurface () const
220+
{
221+
return (sampled_tree_);
222+
}
223+
224+
/** \brief Get the signed distances of the highest points from the fitted planes. */
225+
inline const std::vector<SignedDistanceT> &
226+
getSignedDistancesFromHighestPoints () const
227+
{
228+
return (signed_distances_from_highest_points_);
229+
}
230+
231+
protected:
232+
/** \brief This method should get called before starting the actual computation. */
233+
virtual bool
234+
initCompute ();
235+
236+
/** \brief This method should get called after the actual computation is ended. */
237+
virtual bool
238+
deinitCompute ();
239+
240+
/** \brief Estimate the LRF descriptor for a given point based on its spatial neighborhood of 3D points with normals
241+
* \param[in] index the index of the point in input_
242+
* \param[out] lrf the resultant local reference frame
243+
* \return signed distance of the highest point from the fitted plane. Max if the lrf is not computable.
244+
*/
245+
SignedDistanceT
246+
computePointLRF (const int index, Eigen::Matrix3f &lrf);
247+
248+
/** \brief Abstract feature estimation method.
249+
* \param[out] output the resultant features
250+
*/
251+
virtual void
252+
computeFeature (PointCloudOut &output);
253+
254+
255+
private:
256+
/** \brief Radius used to find tangent axis. */
257+
float tangent_radius_;
258+
259+
/** \brief Threshold that define if a support point is near the margins. */
260+
float margin_thresh_;
261+
262+
/** \brief Min number of neighbours required for the computation of Z axis. Otherwise, feature point normal is used. */
263+
int min_neighbors_for_normal_axis_;
264+
265+
/** \brief Min number of neighbours required for the computation of X axis. Otherwise, a random X axis is set */
266+
int min_neighbors_for_tangent_axis_;
267+
268+
/** \brief An input point cloud describing the surface that is to be used
269+
* for nearest neighbor searches for the estimation of X axis.
270+
*/
271+
PointCloudInConstPtr sampled_surface_;
272+
273+
/** \brief A pointer to the spatial search object used for the estimation of X axis. */
274+
KdTreePtr sampled_tree_;
275+
276+
/** \brief Class for normal estimation. */
277+
NormalEstimation<PointInT, PointNT> normal_estimation_;
278+
279+
/** \brief Signed distances of the highest points from the fitted planes.*/
280+
std::vector<SignedDistanceT> signed_distances_from_highest_points_;
281+
282+
/** \brief If no sampled_surface_ is given, we use surface_ as the sampled surface. */
283+
bool fake_sampled_surface_;
284+
285+
};
286+
287+
}
288+
289+
#ifdef PCL_NO_PRECOMPILE
290+
#include <pcl/features/impl/flare.hpp>
291+
#endif
292+
293+
#endif //#ifndef PCL_FLARE_H_

0 commit comments

Comments
 (0)