Point Cloud Library (PCL)  1.7.0
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 
41 #ifndef PCL_REGISTRATION_CORRESPONDENCE_ESTIMATION_ORGANIZED_PROJECTION_H_
42 #define PCL_REGISTRATION_CORRESPONDENCE_ESTIMATION_ORGANIZED_PROJECTION_H_
43 
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  typedef boost::shared_ptr< CorrespondenceEstimationOrganizedProjection<PointSource, PointTarget, Scalar> > Ptr;
82  typedef boost::shared_ptr< const CorrespondenceEstimationOrganizedProjection<PointSource, PointTarget, Scalar> > ConstPtr;
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  * \param[out] src_to_tgt_transformation 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  * \param[out] depth_threshold 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[in] max_distance Euclidean distance threshold above which correspondences will be rejected
169  */
170  void
171  determineCorrespondences (Correspondences &correspondences, double max_distance);
172 
173  /** \brief Computes the correspondences, applying a maximum Euclidean distance threshold.
174  * \param[in] max_distance Euclidean distance threshold above which correspondences will be rejected
175  */
176  void
177  determineReciprocalCorrespondences (Correspondences &correspondences, double max_distance);
178 
179  protected:
181 
182  bool
183  initCompute ();
184 
185  float fx_, fy_;
186  float cx_, cy_;
187  Eigen::Matrix4f src_to_tgt_transformation_;
189 
190  Eigen::Matrix3f projection_matrix_;
191 
192  public:
193  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
194  };
195  }
196 }
197 
198 #include <pcl/registration/impl/correspondence_estimation_organized_projection.hpp>
199 
200 #endif /* PCL_REGISTRATION_CORRESPONDENCE_ESTIMATION_ORGANIZED_PROJECTION_H_ */