Point Cloud Library (PCL)  1.13.1-dev
correspondence_estimation_organized_projection.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2012-, 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  * $Id$
37  *
38  */
39 
40 #pragma once
41 
42 #include <pcl/registration/correspondence_estimation.h>
43 #include <pcl/memory.h>
44 #include <pcl/pcl_macros.h>
45 
46 namespace pcl {
47 namespace registration {
48 /** \brief CorrespondenceEstimationOrganizedProjection computes correspondences by
49  * projecting the source point cloud onto the target point cloud using the camera
50  * intrinsic and extrinsic parameters. The correspondences can be trimmed by a depth
51  * threshold and by a distance threshold. It is not as precise as a nearest neighbor
52  * search, but it is much faster, as it avoids the usage of any additional structures
53  * (i.e., kd-trees). \note The target point cloud must be organized (no restrictions on
54  * the source) and the target point cloud must be given in the camera coordinate frame.
55  * Any other transformation is specified by the src_to_tgt_transformation_ variable.
56  * \author Alex Ichim
57  * \ingroup registration
58  */
59 template <typename PointSource, typename PointTarget, typename Scalar = float>
61 : public CorrespondenceEstimationBase<PointSource, PointTarget, Scalar> {
62 public:
74 
78 
82 
83  using Ptr = shared_ptr<
85  using ConstPtr =
86  shared_ptr<const CorrespondenceEstimationOrganizedProjection<PointSource,
87  PointTarget,
88  Scalar>>;
89 
90  /** \brief Empty constructor that sets all the intrinsic calibration to the default
91  * Kinect values. */
93  : fx_(525.f)
94  , fy_(525.f)
95  , cx_(319.5f)
96  , cy_(239.5f)
97  , src_to_tgt_transformation_(Eigen::Matrix4f::Identity())
98  , depth_threshold_(std::numeric_limits<float>::max())
99  , projection_matrix_(Eigen::Matrix3f::Identity())
100  {}
101 
102  /** \brief Sets the focal length parameters of the target camera.
103  * \param[in] fx the focal length in pixels along the x-axis of the image
104  * \param[in] fy the focal length in pixels along the y-axis of the image
105  */
106  inline void
107  setFocalLengths(const float fx, const float fy)
108  {
109  fx_ = fx;
110  fy_ = fy;
111  }
112 
113  /** \brief Reads back the focal length parameters of the target camera.
114  * \param[out] fx the focal length in pixels along the x-axis of the image
115  * \param[out] fy the focal length in pixels along the y-axis of the image
116  */
117  inline void
118  getFocalLengths(float& fx, float& fy) const
119  {
120  fx = fx_;
121  fy = fy_;
122  }
123 
124  /** \brief Sets the camera center parameters of the target camera.
125  * \param[in] cx the x-coordinate of the camera center
126  * \param[in] cy the y-coordinate of the camera center
127  */
128  inline void
129  setCameraCenters(const float cx, const float cy)
130  {
131  cx_ = cx;
132  cy_ = cy;
133  }
134 
135  /** \brief Reads back the camera center parameters of the target camera.
136  * \param[out] cx the x-coordinate of the camera center
137  * \param[out] cy the y-coordinate of the camera center
138  */
139  inline void
140  getCameraCenters(float& cx, float& cy) const
141  {
142  cx = cx_;
143  cy = cy_;
144  }
145 
146  /** \brief Sets the transformation from the source point cloud to the target point
147  * cloud. \note The target point cloud must be in its local camera coordinates, so use
148  * this transformation to correct for that. \param[in] src_to_tgt_transformation the
149  * transformation
150  */
151  inline void
152  setSourceTransformation(const Eigen::Matrix4f& src_to_tgt_transformation)
153  {
154  src_to_tgt_transformation_ = src_to_tgt_transformation;
155  }
156 
157  /** \brief Reads back the transformation from the source point cloud to the target
158  * point cloud. \note The target point cloud must be in its local camera coordinates,
159  * so use this transformation to correct for that. \return the transformation
160  */
161  inline Eigen::Matrix4f
163  {
165  }
166 
167  /** \brief Sets the depth threshold; after projecting the source points in the image
168  * space of the target camera, this threshold is applied on the depths of
169  * corresponding dexels to eliminate the ones that are too far from each other.
170  * \param[in] depth_threshold the depth threshold
171  */
172  inline void
173  setDepthThreshold(const float depth_threshold)
174  {
175  depth_threshold_ = depth_threshold;
176  }
177 
178  /** \brief Reads back the depth threshold; after projecting the source points in the
179  * image space of the target camera, this threshold is applied on the depths of
180  * corresponding dexels to eliminate the ones that are too far from each other.
181  * \return the depth threshold
182  */
183  inline float
185  {
186  return (depth_threshold_);
187  }
188 
189  /** \brief Computes the correspondences, applying a maximum Euclidean distance
190  * threshold.
191  * \param[out] correspondences the found correspondences (index of query point, index
192  * of target point, distance)
193  * \param[in] max_distance Euclidean distance threshold above which correspondences
194  * will be rejected
195  */
196  void
197  determineCorrespondences(Correspondences& correspondences, double max_distance);
198 
199  /** \brief Computes the correspondences, applying a maximum Euclidean distance
200  * threshold.
201  * \param[out] correspondences the found correspondences (index of query and target
202  * point, distance)
203  * \param[in] max_distance Euclidean distance threshold above which correspondences
204  * will be rejected
205  */
206  void
208  double max_distance);
209 
210  /** \brief Clone and cast to CorrespondenceEstimationBase */
212  clone() const
213  {
215  PointTarget,
216  Scalar>(*this));
217  return (copy);
218  }
219 
220 protected:
222 
223  bool
224  initCompute();
225 
226  float fx_, fy_;
227  float cx_, cy_;
228  Eigen::Matrix4f src_to_tgt_transformation_;
230 
231  Eigen::Matrix3f projection_matrix_;
232 
233 public:
235 };
236 } // namespace registration
237 } // namespace pcl
238 
239 #include <pcl/registration/impl/correspondence_estimation_organized_projection.hpp>
PCL base class.
Definition: pcl_base.h:70
shared_ptr< PointCloud< PointSource > > Ptr
Definition: point_cloud.h:413
shared_ptr< const PointCloud< PointSource > > ConstPtr
Definition: point_cloud.h:414
Abstract CorrespondenceEstimationBase class.
PointCloudTargetPtr input_transformed_
The transformed input source point cloud dataset.
PointRepresentationConstPtr point_representation_
The target point representation used (internal).
bool target_cloud_updated_
Variable that stores whether we have a new target cloud, meaning we need to pre-process it again.
shared_ptr< CorrespondenceEstimationBase< PointSource, PointTarget, Scalar > > Ptr
CorrespondenceEstimationOrganizedProjection computes correspondences by projecting the source point c...
virtual CorrespondenceEstimationBase< PointSource, PointTarget, Scalar >::Ptr clone() const
Clone and cast to CorrespondenceEstimationBase.
void setFocalLengths(const float fx, const float fy)
Sets the focal length parameters of the target camera.
shared_ptr< CorrespondenceEstimationOrganizedProjection< PointSource, PointTarget, Scalar > > Ptr
void determineCorrespondences(Correspondences &correspondences, double max_distance)
Computes the correspondences, applying a maximum Euclidean distance threshold.
void getCameraCenters(float &cx, float &cy) const
Reads back the camera center parameters of the target camera.
Eigen::Matrix4f getSourceTransformation() const
Reads back the transformation from the source point cloud to the target point cloud.
void setSourceTransformation(const Eigen::Matrix4f &src_to_tgt_transformation)
Sets the transformation from the source point cloud to the target point cloud.
CorrespondenceEstimationOrganizedProjection()
Empty constructor that sets all the intrinsic calibration to the default Kinect values.
shared_ptr< const CorrespondenceEstimationOrganizedProjection< PointSource, PointTarget, Scalar > > ConstPtr
float getDepthThreshold() const
Reads back the depth threshold; after projecting the source points in the image space of the target c...
void determineReciprocalCorrespondences(Correspondences &correspondences, double max_distance)
Computes the correspondences, applying a maximum Euclidean distance threshold.
void setCameraCenters(const float cx, const float cy)
Sets the camera center parameters of the target camera.
void getFocalLengths(float &fx, float &fy) const
Reads back the focal length parameters of the target camera.
void setDepthThreshold(const float depth_threshold)
Sets the depth threshold; after projecting the source points in the image space of the target camera,...
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition: memory.h:63
Defines functions, macros and traits for allocating and using memory.
Definition: bfgs.h:10
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
Defines all the PCL and non-PCL macros used.