Point Cloud Library (PCL)  1.11.1
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/memory.h>
43 #include <pcl/pcl_macros.h>
44 #include <pcl/registration/correspondence_estimation.h>
45 
46 namespace pcl
47 {
48  namespace registration
49  {
50  /** \brief CorrespondenceEstimationOrganizedProjection computes correspondences by projecting the source point cloud
51  * onto the target point cloud using the camera intrinsic and extrinsic parameters. The correspondences can be trimmed
52  * by a depth threshold and by a distance threshold.
53  * It is not as precise as a nearest neighbor search, but it is much faster, as it avoids the usage of any additional
54  * structures (i.e., kd-trees).
55  * \note The target point cloud must be organized (no restrictions on the source) and the target point cloud must be
56  * given in the camera coordinate frame. Any other transformation is specified by the src_to_tgt_transformation_
57  * variable.
58  * \author Alex Ichim
59  */
60  template <typename PointSource, typename PointTarget, typename Scalar = float>
61  class CorrespondenceEstimationOrganizedProjection : public CorrespondenceEstimationBase <PointSource, PointTarget, Scalar>
62  {
63  public:
72 
76 
80 
81  using Ptr = shared_ptr< CorrespondenceEstimationOrganizedProjection<PointSource, PointTarget, Scalar> >;
82  using ConstPtr = shared_ptr< const CorrespondenceEstimationOrganizedProjection<PointSource, PointTarget, Scalar> >;
83 
84 
85 
86  /** \brief Empty constructor that sets all the intrinsic calibration to the default Kinect values. */
88  : fx_ (525.f)
89  , fy_ (525.f)
90  , cx_ (319.5f)
91  , cy_ (239.5f)
92  , src_to_tgt_transformation_ (Eigen::Matrix4f::Identity ())
93  , depth_threshold_ (std::numeric_limits<float>::max ())
94  , projection_matrix_ (Eigen::Matrix3f::Identity ())
95  { }
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  { fx_ = fx; fy_ = fy; }
105 
106  /** \brief Reads back the focal length parameters of the target camera.
107  * \param[out] fx the focal length in pixels along the x-axis of the image
108  * \param[out] fy the focal length in pixels along the y-axis of the image
109  */
110  inline void
111  getFocalLengths (float &fx, float &fy) const
112  { fx = fx_; fy = fy_; }
113 
114 
115  /** \brief Sets the camera center parameters of the target camera.
116  * \param[in] cx the x-coordinate of the camera center
117  * \param[in] cy the y-coordinate of the camera center
118  */
119  inline void
120  setCameraCenters (const float cx, const float cy)
121  { cx_ = cx; cy_ = cy; }
122 
123  /** \brief Reads back the camera center parameters of the target camera.
124  * \param[out] cx the x-coordinate of the camera center
125  * \param[out] cy the y-coordinate of the camera center
126  */
127  inline void
128  getCameraCenters (float &cx, float &cy) const
129  { cx = cx_; cy = cy_; }
130 
131  /** \brief Sets the transformation from the source point cloud to the target point cloud.
132  * \note The target point cloud must be in its local camera coordinates, so use this transformation to correct
133  * for that.
134  * \param[in] src_to_tgt_transformation the transformation
135  */
136  inline void
137  setSourceTransformation (const Eigen::Matrix4f &src_to_tgt_transformation)
138  { src_to_tgt_transformation_ = src_to_tgt_transformation; }
139 
140  /** \brief Reads back the transformation from the source point cloud to the target point cloud.
141  * \note The target point cloud must be in its local camera coordinates, so use this transformation to correct
142  * for that.
143  * \return the transformation
144  */
145  inline Eigen::Matrix4f
147  { return (src_to_tgt_transformation_); }
148 
149  /** \brief Sets the depth threshold; after projecting the source points in the image space of the target camera,
150  * this threshold is applied on the depths of corresponding dexels to eliminate the ones that are too far from
151  * each other.
152  * \param[in] depth_threshold the depth threshold
153  */
154  inline void
155  setDepthThreshold (const float depth_threshold)
156  { depth_threshold_ = depth_threshold; }
157 
158  /** \brief Reads back the depth threshold; after projecting the source points in the image space of the target
159  * camera, this threshold is applied on the depths of corresponding dexels to eliminate the ones that are too
160  * far from each other.
161  * \return the depth threshold
162  */
163  inline float
165  { return (depth_threshold_); }
166 
167  /** \brief Computes the correspondences, applying a maximum Euclidean distance threshold.
168  * \param correspondences
169  * \param[in] max_distance Euclidean distance threshold above which correspondences will be rejected
170  */
171  void
172  determineCorrespondences (Correspondences &correspondences, double max_distance);
173 
174  /** \brief Computes the correspondences, applying a maximum Euclidean distance threshold.
175  * \param correspondences
176  * \param[in] max_distance Euclidean distance threshold above which correspondences will be rejected
177  */
178  void
179  determineReciprocalCorrespondences (Correspondences &correspondences, double max_distance);
180 
181  /** \brief Clone and cast to CorrespondenceEstimationBase */
183  clone () const
184  {
186  return (copy);
187  }
188 
189  protected:
191 
192  bool
193  initCompute ();
194 
195  float fx_, fy_;
196  float cx_, cy_;
197  Eigen::Matrix4f src_to_tgt_transformation_;
199 
200  Eigen::Matrix3f projection_matrix_;
201 
202  public:
204  };
205  }
206 }
207 
208 #include <pcl/registration/impl/correspondence_estimation_organized_projection.hpp>
PCL base class.
Definition: pcl_base.h:73
shared_ptr< PointCloud< PointSource > > Ptr
Definition: point_cloud.h:429
shared_ptr< const PointCloud< PointSource > > ConstPtr
Definition: point_cloud.h:430
Abstract CorrespondenceEstimationBase class.
shared_ptr< CorrespondenceEstimationBase< PointSource, PointTarget, Scalar > > Ptr
CorrespondenceEstimationOrganizedProjection computes correspondences by projecting the source point c...
shared_ptr< const CorrespondenceEstimationOrganizedProjection< PointSource, PointTarget, Scalar > > ConstPtr
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.
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.