Point Cloud Library (PCL)  1.10.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/pcl_macros.h>
43 #include <pcl/registration/correspondence_estimation.h>
44 
45 namespace pcl
46 {
47  namespace registration
48  {
49  /** \brief CorrespondenceEstimationOrganizedProjection computes correspondences by projecting the source point cloud
50  * onto the target point cloud using the camera intrinsic and extrinsic parameters. The correspondences can be trimmed
51  * by a depth threshold and by a distance threshold.
52  * It is not as precise as a nearest neighbor search, but it is much faster, as it avoids the usage of any additional
53  * structures (i.e., kd-trees).
54  * \note The target point cloud must be organized (no restrictions on the source) and the target point cloud must be
55  * given in the camera coordinate frame. Any other transformation is specified by the src_to_tgt_transformation_
56  * variable.
57  * \author Alex Ichim
58  */
59  template <typename PointSource, typename PointTarget, typename Scalar = float>
60  class CorrespondenceEstimationOrganizedProjection : public CorrespondenceEstimationBase <PointSource, PointTarget, Scalar>
61  {
62  public:
71 
75 
79 
82 
83 
84 
85  /** \brief Empty constructor that sets all the intrinsic calibration to the default Kinect values. */
87  : fx_ (525.f)
88  , fy_ (525.f)
89  , cx_ (319.5f)
90  , cy_ (239.5f)
91  , src_to_tgt_transformation_ (Eigen::Matrix4f::Identity ())
92  , depth_threshold_ (std::numeric_limits<float>::max ())
93  , projection_matrix_ (Eigen::Matrix3f::Identity ())
94  { }
95 
96 
97  /** \brief Sets the focal length parameters of the target camera.
98  * \param[in] fx the focal length in pixels along the x-axis of the image
99  * \param[in] fy the focal length in pixels along the y-axis of the image
100  */
101  inline void
102  setFocalLengths (const float fx, const float fy)
103  { fx_ = fx; fy_ = fy; }
104 
105  /** \brief Reads back the focal length parameters of the target camera.
106  * \param[out] fx the focal length in pixels along the x-axis of the image
107  * \param[out] fy the focal length in pixels along the y-axis of the image
108  */
109  inline void
110  getFocalLengths (float &fx, float &fy) const
111  { fx = fx_; fy = fy_; }
112 
113 
114  /** \brief Sets the camera center parameters of the target camera.
115  * \param[in] cx the x-coordinate of the camera center
116  * \param[in] cy the y-coordinate of the camera center
117  */
118  inline void
119  setCameraCenters (const float cx, const float cy)
120  { cx_ = cx; cy_ = cy; }
121 
122  /** \brief Reads back the camera center parameters of the target camera.
123  * \param[out] cx the x-coordinate of the camera center
124  * \param[out] cy the y-coordinate of the camera center
125  */
126  inline void
127  getCameraCenters (float &cx, float &cy) const
128  { cx = cx_; cy = cy_; }
129 
130  /** \brief Sets the transformation from the source point cloud to the target point cloud.
131  * \note The target point cloud must be in its local camera coordinates, so use this transformation to correct
132  * for that.
133  * \param[in] src_to_tgt_transformation the transformation
134  */
135  inline void
136  setSourceTransformation (const Eigen::Matrix4f &src_to_tgt_transformation)
137  { src_to_tgt_transformation_ = src_to_tgt_transformation; }
138 
139  /** \brief Reads back the transformation from the source point cloud to the target point cloud.
140  * \note The target point cloud must be in its local camera coordinates, so use this transformation to correct
141  * for that.
142  * \return the transformation
143  */
144  inline Eigen::Matrix4f
146  { return (src_to_tgt_transformation_); }
147 
148  /** \brief Sets the depth threshold; after projecting the source points in the image space of the target camera,
149  * this threshold is applied on the depths of corresponding dexels to eliminate the ones that are too far from
150  * each other.
151  * \param[in] depth_threshold the depth threshold
152  */
153  inline void
154  setDepthThreshold (const float depth_threshold)
155  { depth_threshold_ = depth_threshold; }
156 
157  /** \brief Reads back the depth threshold; after projecting the source points in the image space of the target
158  * camera, this threshold is applied on the depths of corresponding dexels to eliminate the ones that are too
159  * far from each other.
160  * \return the depth threshold
161  */
162  inline float
164  { return (depth_threshold_); }
165 
166  /** \brief Computes the correspondences, applying a maximum Euclidean distance threshold.
167  * \param correspondences
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 correspondences
175  * \param[in] max_distance Euclidean distance threshold above which correspondences will be rejected
176  */
177  void
178  determineReciprocalCorrespondences (Correspondences &correspondences, double max_distance);
179 
180  /** \brief Clone and cast to CorrespondenceEstimationBase */
182  clone () const
183  {
185  return (copy);
186  }
187 
188  protected:
190 
191  bool
192  initCompute ();
193 
194  float fx_, fy_;
195  float cx_, cy_;
196  Eigen::Matrix4f src_to_tgt_transformation_;
198 
199  Eigen::Matrix3f projection_matrix_;
200 
201  public:
203  };
204  }
205 }
206 
207 #include <pcl/registration/impl/correspondence_estimation_organized_projection.hpp>
pcl::registration::CorrespondenceEstimationOrganizedProjection::CorrespondenceEstimationOrganizedProjection
CorrespondenceEstimationOrganizedProjection()
Empty constructor that sets all the intrinsic calibration to the default Kinect values.
Definition: correspondence_estimation_organized_projection.h:86
pcl_macros.h
Defines all the PCL and non-PCL macros used.
pcl::registration::CorrespondenceEstimationOrganizedProjection::getFocalLengths
void getFocalLengths(float &fx, float &fy) const
Reads back the focal length parameters of the target camera.
Definition: correspondence_estimation_organized_projection.h:110
pcl
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
pcl::registration::CorrespondenceEstimationOrganizedProjection::setFocalLengths
void setFocalLengths(const float fx, const float fy)
Sets the focal length parameters of the target camera.
Definition: correspondence_estimation_organized_projection.h:102
Eigen
Definition: bfgs.h:9
pcl::registration::CorrespondenceEstimationOrganizedProjection
CorrespondenceEstimationOrganizedProjection computes correspondences by projecting the source point c...
Definition: correspondence_estimation_organized_projection.h:60
pcl::registration::CorrespondenceEstimationOrganizedProjection::initCompute
bool initCompute()
Definition: correspondence_estimation_organized_projection.hpp:46
pcl::registration::CorrespondenceEstimationOrganizedProjection::setDepthThreshold
void setDepthThreshold(const float depth_threshold)
Sets the depth threshold; after projecting the source points in the image space of the target camera,...
Definition: correspondence_estimation_organized_projection.h:154
pcl::registration::CorrespondenceEstimationBase::PointCloudTargetPtr
typename PointCloudTarget::Ptr PointCloudTargetPtr
Definition: correspondence_estimation.h:85
pcl::registration::CorrespondenceEstimationOrganizedProjection::cy_
float cy_
Definition: correspondence_estimation_organized_projection.h:195
pcl::PCLBase
PCL base class.
Definition: pcl_base.h:69
pcl::registration::CorrespondenceEstimationBase
Abstract CorrespondenceEstimationBase class.
Definition: correspondence_estimation.h:62
pcl::registration::CorrespondenceEstimationOrganizedProjection::setSourceTransformation
void setSourceTransformation(const Eigen::Matrix4f &src_to_tgt_transformation)
Sets the transformation from the source point cloud to the target point cloud.
Definition: correspondence_estimation_organized_projection.h:136
pcl::registration::CorrespondenceEstimationOrganizedProjection::determineCorrespondences
void determineCorrespondences(Correspondences &correspondences, double max_distance)
Computes the correspondences, applying a maximum Euclidean distance threshold.
Definition: correspondence_estimation_organized_projection.hpp:71
pcl::PointCloud< PointSource >
pcl::registration::CorrespondenceEstimationOrganizedProjection::projection_matrix_
Eigen::Matrix3f projection_matrix_
Definition: correspondence_estimation_organized_projection.h:199
pcl::registration::CorrespondenceEstimationOrganizedProjection::fx_
float fx_
Definition: correspondence_estimation_organized_projection.h:194
pcl::registration::CorrespondenceEstimationOrganizedProjection::cx_
float cx_
Definition: correspondence_estimation_organized_projection.h:195
pcl::registration::CorrespondenceEstimationBase::PointCloudTargetConstPtr
typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr
Definition: correspondence_estimation.h:86
pcl::registration::CorrespondenceEstimationBase::PointCloudSourcePtr
typename PointCloudSource::Ptr PointCloudSourcePtr
Definition: correspondence_estimation.h:81
pcl::registration::CorrespondenceEstimationOrganizedProjection::getCameraCenters
void getCameraCenters(float &cx, float &cy) const
Reads back the camera center parameters of the target camera.
Definition: correspondence_estimation_organized_projection.h:127
pcl::registration::CorrespondenceEstimationOrganizedProjection::determineReciprocalCorrespondences
void determineReciprocalCorrespondences(Correspondences &correspondences, double max_distance)
Computes the correspondences, applying a maximum Euclidean distance threshold.
Definition: correspondence_estimation_organized_projection.hpp:118
pcl::registration::CorrespondenceEstimationOrganizedProjection::getDepthThreshold
float getDepthThreshold() const
Reads back the depth threshold; after projecting the source points in the image space of the target c...
Definition: correspondence_estimation_organized_projection.h:163
pcl::registration::CorrespondenceEstimationBase::Ptr
shared_ptr< CorrespondenceEstimationBase< PointSource, PointTarget, Scalar > > Ptr
Definition: correspondence_estimation.h:65
pcl::registration::CorrespondenceEstimationBase::ConstPtr
shared_ptr< const CorrespondenceEstimationBase< PointSource, PointTarget, Scalar > > ConstPtr
Definition: correspondence_estimation.h:66
pcl::registration::CorrespondenceEstimationOrganizedProjection::fy_
float fy_
Definition: correspondence_estimation_organized_projection.h:194
PCL_MAKE_ALIGNED_OPERATOR_NEW
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition: pcl_macros.h:389
pcl::registration::CorrespondenceEstimationOrganizedProjection::src_to_tgt_transformation_
Eigen::Matrix4f src_to_tgt_transformation_
Definition: correspondence_estimation_organized_projection.h:196
pcl::registration::CorrespondenceEstimationOrganizedProjection::clone
virtual CorrespondenceEstimationBase< PointSource, PointTarget, Scalar >::Ptr clone() const
Clone and cast to CorrespondenceEstimationBase.
Definition: correspondence_estimation_organized_projection.h:182
pcl::PointCloud< PointSource >::Ptr
shared_ptr< PointCloud< PointSource > > Ptr
Definition: point_cloud.h:415
pcl::registration::CorrespondenceEstimationOrganizedProjection::setCameraCenters
void setCameraCenters(const float cx, const float cy)
Sets the camera center parameters of the target camera.
Definition: correspondence_estimation_organized_projection.h:119
pcl::registration::CorrespondenceEstimationBase::PointCloudSourceConstPtr
typename PointCloudSource::ConstPtr PointCloudSourceConstPtr
Definition: correspondence_estimation.h:82
pcl::PointCloud< PointSource >::ConstPtr
shared_ptr< const PointCloud< PointSource > > ConstPtr
Definition: point_cloud.h:416
pcl::Correspondences
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
Definition: correspondence.h:88
pcl::registration::CorrespondenceEstimationOrganizedProjection::depth_threshold_
float depth_threshold_
Definition: correspondence_estimation_organized_projection.h:197
pcl::shared_ptr
boost::shared_ptr< T > shared_ptr
Alias for boost::shared_ptr.
Definition: pcl_macros.h:108
pcl::registration::CorrespondenceEstimationOrganizedProjection::getSourceTransformation
Eigen::Matrix4f getSourceTransformation() const
Reads back the transformation from the source point cloud to the target point cloud.
Definition: correspondence_estimation_organized_projection.h:145