Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/registration/include/pcl/registration/correspondence_estimation_organized_projection.h
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2012-, Open Perception, Inc.
00006  *
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of the copyright holder(s) nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  * $Id$
00037  *
00038  */
00039 
00040 
00041 #ifndef PCL_REGISTRATION_CORRESPONDENCE_ESTIMATION_ORGANIZED_PROJECTION_H_
00042 #define PCL_REGISTRATION_CORRESPONDENCE_ESTIMATION_ORGANIZED_PROJECTION_H_
00043 
00044 #include <pcl/registration/correspondence_estimation.h>
00045 
00046 namespace pcl
00047 {
00048   namespace registration
00049   {
00050     /** \brief CorrespondenceEstimationOrganizedProjection computes correspondences by projecting the source point cloud
00051       * onto the target point cloud using the camera intrinsic and extrinsic parameters. The correspondences can be trimmed
00052       * by a depth threshold and by a distance threshold.
00053       * It is not as precise as a nearest neighbor search, but it is much faster, as it avoids the usage of any additional
00054       * structures (i.e., kd-trees).
00055       * \note The target point cloud must be organized (no restrictions on the source) and the target point cloud must be
00056       * given in the camera coordinate frame. Any other transformation is specified by the src_to_tgt_transformation_
00057       * variable.
00058       * \author Alex Ichim
00059       */
00060     template <typename PointSource, typename PointTarget, typename Scalar = float>
00061     class CorrespondenceEstimationOrganizedProjection : public CorrespondenceEstimationBase <PointSource, PointTarget, Scalar>
00062     {
00063       public:
00064         using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::initCompute;
00065         using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::input_transformed_;
00066         using PCLBase<PointSource>::deinitCompute;
00067         using PCLBase<PointSource>::input_;
00068         using PCLBase<PointSource>::indices_;
00069         using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::getClassName;
00070         using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::point_representation_;
00071         using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::target_cloud_updated_;
00072 
00073         typedef pcl::PointCloud<PointSource> PointCloudSource;
00074         typedef typename PointCloudSource::Ptr PointCloudSourcePtr;
00075         typedef typename PointCloudSource::ConstPtr PointCloudSourceConstPtr;
00076 
00077         typedef pcl::PointCloud<PointTarget> PointCloudTarget;
00078         typedef typename PointCloudTarget::Ptr PointCloudTargetPtr;
00079         typedef typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr;
00080 
00081         typedef boost::shared_ptr< CorrespondenceEstimationOrganizedProjection<PointSource, PointTarget, Scalar> > Ptr;
00082         typedef boost::shared_ptr< const CorrespondenceEstimationOrganizedProjection<PointSource, PointTarget, Scalar> > ConstPtr;
00083 
00084 
00085 
00086         /** \brief Empty constructor that sets all the intrinsic calibration to the default Kinect values. */
00087         CorrespondenceEstimationOrganizedProjection ()
00088           : fx_ (525.f)
00089           , fy_ (525.f)
00090           , cx_ (319.5f)
00091           , cy_ (239.5f)
00092           , src_to_tgt_transformation_ (Eigen::Matrix4f::Identity ())
00093           , depth_threshold_ (std::numeric_limits<float>::max ())
00094           , projection_matrix_ (Eigen::Matrix3f::Identity ())
00095         { }
00096 
00097 
00098         /** \brief Sets the focal length parameters of the target camera.
00099           * \param[in] fx the focal length in pixels along the x-axis of the image
00100           * \param[in] fy the focal length in pixels along the y-axis of the image
00101           */
00102         inline void
00103         setFocalLengths (const float fx, const float fy)
00104         { fx_ = fx; fy_ = fy; }
00105 
00106         /** \brief Reads back the focal length parameters of the target camera.
00107           * \param[out] fx the focal length in pixels along the x-axis of the image
00108           * \param[out] fy the focal length in pixels along the y-axis of the image
00109           */
00110         inline void
00111         getFocalLengths (float &fx, float &fy) const
00112         { fx = fx_; fy = fy_; }
00113 
00114 
00115         /** \brief Sets the camera center parameters of the target camera.
00116           * \param[in] cx the x-coordinate of the camera center
00117           * \param[in] cy the y-coordinate of the camera center
00118           */
00119         inline void
00120         setCameraCenters (const float cx, const float cy)
00121         { cx_ = cx; cy_ = cy; }
00122 
00123         /** \brief Reads back the camera center parameters of the target camera.
00124           * \param[out] cx the x-coordinate of the camera center
00125           * \param[out] cy the y-coordinate of the camera center
00126           */
00127         inline void
00128         getCameraCenters (float &cx, float &cy) const
00129         { cx = cx_; cy = cy_; }
00130 
00131         /** \brief Sets the transformation from the source point cloud to the target point cloud.
00132           * \note The target point cloud must be in its local camera coordinates, so use this transformation to correct
00133           * for that.
00134           * \param[in] src_to_tgt_transformation the transformation
00135           */
00136         inline void
00137         setSourceTransformation (const Eigen::Matrix4f &src_to_tgt_transformation)
00138         { src_to_tgt_transformation_ = src_to_tgt_transformation; }
00139 
00140         /** \brief Reads back the transformation from the source point cloud to the target point cloud.
00141           * \note The target point cloud must be in its local camera coordinates, so use this transformation to correct
00142           * for that.
00143           * \param[out] src_to_tgt_transformation the transformation
00144           */
00145         inline Eigen::Matrix4f
00146         getSourceTransformation () const
00147         { return (src_to_tgt_transformation_); }
00148 
00149         /** \brief Sets the depth threshold; after projecting the source points in the image space of the target camera,
00150           * this threshold is applied on the depths of corresponding dexels to eliminate the ones that are too far from
00151           * each other.
00152           * \param[in] depth_threshold the depth threshold
00153           */
00154         inline void
00155         setDepthThreshold (const float depth_threshold)
00156         { depth_threshold_ = depth_threshold; }
00157 
00158         /** \brief Reads back the depth threshold; after projecting the source points in the image space of the target
00159           * camera, this threshold is applied on the depths of corresponding dexels to eliminate the ones that are too
00160           * far from each other.
00161           * \param[out] depth_threshold the depth threshold
00162           */
00163         inline float
00164         getDepthThreshold () const
00165         { return (depth_threshold_); }
00166 
00167         /** \brief Computes the correspondences, applying a maximum Euclidean distance threshold.
00168           * \param[in] max_distance Euclidean distance threshold above which correspondences will be rejected
00169           */
00170         void
00171         determineCorrespondences (Correspondences &correspondences, double max_distance);
00172 
00173         /** \brief Computes the correspondences, applying a maximum Euclidean distance threshold.
00174           * \param[in] max_distance Euclidean distance threshold above which correspondences will be rejected
00175           */
00176         void
00177         determineReciprocalCorrespondences (Correspondences &correspondences, double max_distance);
00178 
00179       protected:
00180         using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::target_;
00181 
00182         bool
00183         initCompute ();
00184 
00185         float fx_, fy_;
00186         float cx_, cy_;
00187         Eigen::Matrix4f src_to_tgt_transformation_;
00188         float depth_threshold_;
00189 
00190         Eigen::Matrix3f projection_matrix_;
00191 
00192       public:
00193         EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00194     };
00195   }
00196 }
00197 
00198 #include <pcl/registration/impl/correspondence_estimation_organized_projection.hpp>
00199 
00200 #endif /* PCL_REGISTRATION_CORRESPONDENCE_ESTIMATION_ORGANIZED_PROJECTION_H_ */