Point Cloud Library (PCL)  1.8.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  * \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 */
182  virtual boost::shared_ptr< CorrespondenceEstimationBase<PointSource, PointTarget, Scalar> >
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:
203  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
204  };
205  }
206 }
207 
208 #include <pcl/registration/impl/correspondence_estimation_organized_projection.hpp>
209 
210 #endif /* PCL_REGISTRATION_CORRESPONDENCE_ESTIMATION_ORGANIZED_PROJECTION_H_ */
void determineCorrespondences(Correspondences &correspondences, double max_distance)
Computes the correspondences, applying a maximum Euclidean distance threshold.
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...
void setFocalLengths(const float fx, const float fy)
Sets the focal length parameters of the target camera.
Definition: bfgs.h:10
boost::shared_ptr< PointCloud< PointSource > > Ptr
Definition: point_cloud.h:428
float getDepthThreshold() const
Reads back the depth threshold; after projecting the source points in the image space of the target c...
PCL base class.
Definition: pcl_base.h:68
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
void getCameraCenters(float &cx, float &cy) const
Reads back the camera center parameters of the target camera.
void setCameraCenters(const float cx, const float cy)
Sets the camera center parameters of the target camera.
boost::shared_ptr< const PointCloud< PointSource > > ConstPtr
Definition: point_cloud.h:429
boost::shared_ptr< CorrespondenceEstimationOrganizedProjection< PointSource, PointTarget, Scalar > > Ptr
void determineReciprocalCorrespondences(Correspondences &correspondences, double max_distance)
Computes the correspondences, applying a maximum Euclidean distance threshold.
Eigen::Matrix4f getSourceTransformation() const
Reads back the transformation from the source point cloud to the target point cloud.
virtual boost::shared_ptr< CorrespondenceEstimationBase< PointSource, PointTarget, Scalar > > clone() const
Clone and cast to CorrespondenceEstimationBase.
CorrespondenceEstimationOrganizedProjection()
Empty constructor that sets all the intrinsic calibration to the default Kinect values.
boost::shared_ptr< const CorrespondenceEstimationOrganizedProjection< PointSource, PointTarget, Scalar > > ConstPtr
CorrespondenceEstimationOrganizedProjection computes correspondences by projecting the source point c...
void setSourceTransformation(const Eigen::Matrix4f &src_to_tgt_transformation)
Sets the transformation from the source point cloud to the target point cloud.
Abstract CorrespondenceEstimationBase class.