Point Cloud Library (PCL)  1.14.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  : src_to_tgt_transformation_(Eigen::Matrix4f::Identity())
94  , depth_threshold_(std::numeric_limits<float>::max())
95  , projection_matrix_(Eigen::Matrix3f::Identity())
96  {}
97 
98  /** \brief Sets the focal length parameters of the target camera.
99  * \param[in] fx the focal length in pixels along the x-axis of the image
100  * \param[in] fy the focal length in pixels along the y-axis of the image
101  */
102  inline void
103  setFocalLengths(const float fx, const float fy)
104  {
105  fx_ = fx;
106  fy_ = fy;
107  }
108 
109  /** \brief Reads back the focal length parameters of the target camera.
110  * \param[out] fx the focal length in pixels along the x-axis of the image
111  * \param[out] fy the focal length in pixels along the y-axis of the image
112  */
113  inline void
114  getFocalLengths(float& fx, float& fy) const
115  {
116  fx = fx_;
117  fy = fy_;
118  }
119 
120  /** \brief Sets the camera center parameters of the target camera.
121  * \param[in] cx the x-coordinate of the camera center
122  * \param[in] cy the y-coordinate of the camera center
123  */
124  inline void
125  setCameraCenters(const float cx, const float cy)
126  {
127  cx_ = cx;
128  cy_ = cy;
129  }
130 
131  /** \brief Reads back the camera center parameters of the target camera.
132  * \param[out] cx the x-coordinate of the camera center
133  * \param[out] cy the y-coordinate of the camera center
134  */
135  inline void
136  getCameraCenters(float& cx, float& cy) const
137  {
138  cx = cx_;
139  cy = cy_;
140  }
141 
142  /** \brief Sets the transformation from the source point cloud to the target point
143  * cloud. \note The target point cloud must be in its local camera coordinates, so use
144  * this transformation to correct for that. \param[in] src_to_tgt_transformation the
145  * transformation
146  */
147  inline void
148  setSourceTransformation(const Eigen::Matrix4f& src_to_tgt_transformation)
149  {
150  src_to_tgt_transformation_ = src_to_tgt_transformation;
151  }
152 
153  /** \brief Reads back the transformation from the source point cloud to the target
154  * point cloud. \note The target point cloud must be in its local camera coordinates,
155  * so use this transformation to correct for that. \return the transformation
156  */
157  inline Eigen::Matrix4f
159  {
161  }
162 
163  /** \brief Sets the depth threshold; after projecting the source points in the image
164  * space of the target camera, this threshold is applied on the depths of
165  * corresponding dexels to eliminate the ones that are too far from each other.
166  * \param[in] depth_threshold the depth threshold
167  */
168  inline void
169  setDepthThreshold(const float depth_threshold)
170  {
171  depth_threshold_ = depth_threshold;
172  }
173 
174  /** \brief Reads back the depth threshold; after projecting the source points in the
175  * image space of the target camera, this threshold is applied on the depths of
176  * corresponding dexels to eliminate the ones that are too far from each other.
177  * \return the depth threshold
178  */
179  inline float
181  {
182  return (depth_threshold_);
183  }
184 
185  /** \brief Computes the correspondences, applying a maximum Euclidean distance
186  * threshold.
187  * \param[out] correspondences the found correspondences (index of query point, index
188  * of target point, distance)
189  * \param[in] max_distance Euclidean distance threshold above which correspondences
190  * will be rejected
191  */
192  void
193  determineCorrespondences(Correspondences& correspondences, double max_distance);
194 
195  /** \brief Computes the correspondences, applying a maximum Euclidean distance
196  * threshold.
197  * \param[out] correspondences the found correspondences (index of query and target
198  * point, distance)
199  * \param[in] max_distance Euclidean distance threshold above which correspondences
200  * will be rejected
201  */
202  void
204  double max_distance);
205 
206  /** \brief Clone and cast to CorrespondenceEstimationBase */
208  clone() const
209  {
211  PointTarget,
212  Scalar>(*this));
213  return (copy);
214  }
215 
216 protected:
218 
219  bool
220  initCompute();
221 
222  float fx_{525.f}, fy_{525.f};
223  float cx_{319.5f}, cy_{239.5f};
224  Eigen::Matrix4f src_to_tgt_transformation_;
226 
227  Eigen::Matrix3f projection_matrix_;
228 
229 public:
231 };
232 } // namespace registration
233 } // namespace pcl
234 
235 #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.