Point Cloud Library (PCL)
1.7.0
|
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_ */