Point Cloud Library (PCL)  1.7.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 
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_ */
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
Abstract CorrespondenceEstimationBase class.
Eigen::Matrix4f getSourceTransformation() const
Reads back the transformation from the source point cloud to the target point cloud.
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:429
void setCameraCenters(const float cx, const float cy)
Sets the camera center parameters of the target camera.
void setFocalLengths(const float fx, const float fy)
Sets the focal length parameters of the target camera.
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
boost::shared_ptr< const CorrespondenceEstimationOrganizedProjection< PointSource, PointTarget, Scalar > > ConstPtr
void setSourceTransformation(const Eigen::Matrix4f &src_to_tgt_transformation)
Sets the transformation from the source point cloud to the target point cloud.
PCL base class.
Definition: pcl_base.h:68
void determineCorrespondences(Correspondences &correspondences, double max_distance)
Computes the correspondences, applying a maximum Euclidean distance threshold.
CorrespondenceEstimationOrganizedProjection computes correspondences by projecting the source point c...
void getCameraCenters(float &cx, float &cy) const
Reads back the camera center parameters of the target camera.
void determineReciprocalCorrespondences(Correspondences &correspondences, double max_distance)
Computes the correspondences, applying a maximum Euclidean distance threshold.
float getDepthThreshold() const
Reads back the depth threshold; after projecting the source points in the image space of the target c...
CorrespondenceEstimationOrganizedProjection()
Empty constructor that sets all the intrinsic calibration to the default Kinect values.
void setDepthThreshold(const float depth_threshold)
Sets the depth threshold; after projecting the source points in the image space of the target camera...
boost::shared_ptr< CorrespondenceEstimationOrganizedProjection< PointSource, PointTarget, Scalar > > Ptr
void getFocalLengths(float &fx, float &fy) const
Reads back the focal length parameters of the target camera.