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 * 00037 */ 00038 00039 #ifndef PCL_REGISTRATION_CORRESPONDENCE_REJECTION_SAMPLE_CONSENSUS_2D_H_ 00040 #define PCL_REGISTRATION_CORRESPONDENCE_REJECTION_SAMPLE_CONSENSUS_2D_H_ 00041 00042 #include <pcl/registration/correspondence_rejection_sample_consensus.h> 00043 00044 namespace pcl 00045 { 00046 namespace registration 00047 { 00048 /** \brief CorrespondenceRejectorSampleConsensus2D implements a pixel-based 00049 * correspondence rejection using Random Sample Consensus to identify inliers 00050 * (and reject outliers) 00051 * \author Radu B. Rusu 00052 * \ingroup registration 00053 */ 00054 template <typename PointT> 00055 class CorrespondenceRejectorSampleConsensus2D: public CorrespondenceRejectorSampleConsensus<PointT> 00056 { 00057 typedef pcl::PointCloud<PointT> PointCloud; 00058 typedef typename PointCloud::Ptr PointCloudPtr; 00059 typedef typename PointCloud::ConstPtr PointCloudConstPtr; 00060 00061 public: 00062 using CorrespondenceRejectorSampleConsensus<PointT>::refine_; 00063 using CorrespondenceRejectorSampleConsensus<PointT>::input_; 00064 using CorrespondenceRejectorSampleConsensus<PointT>::target_; 00065 using CorrespondenceRejectorSampleConsensus<PointT>::input_correspondences_; 00066 using CorrespondenceRejectorSampleConsensus<PointT>::rejection_name_; 00067 using CorrespondenceRejectorSampleConsensus<PointT>::getClassName; 00068 using CorrespondenceRejectorSampleConsensus<PointT>::inlier_threshold_; 00069 using CorrespondenceRejectorSampleConsensus<PointT>::max_iterations_; 00070 using CorrespondenceRejectorSampleConsensus<PointT>::best_transformation_; 00071 00072 typedef boost::shared_ptr<CorrespondenceRejectorSampleConsensus2D> Ptr; 00073 typedef boost::shared_ptr<const CorrespondenceRejectorSampleConsensus2D> ConstPtr; 00074 00075 /** \brief Empty constructor. Sets the inlier threshold to 5cm (0.05m), 00076 * and the maximum number of iterations to 1000. 00077 */ 00078 CorrespondenceRejectorSampleConsensus2D () 00079 : projection_matrix_ (Eigen::Matrix3f::Identity ()) 00080 { 00081 rejection_name_ = "CorrespondenceRejectorSampleConsensus2D"; 00082 // Put the projection matrix together 00083 //projection_matrix_ (0, 0) = 525.f; 00084 //projection_matrix_ (1, 1) = 525.f; 00085 //projection_matrix_ (0, 2) = 320.f; 00086 //projection_matrix_ (1, 2) = 240.f; 00087 } 00088 00089 /** \brief Get a list of valid correspondences after rejection from the original set of correspondences. 00090 * \param[in] original_correspondences the set of initial correspondences given 00091 * \param[out] remaining_correspondences the resultant filtered set of remaining correspondences 00092 */ 00093 inline void 00094 getRemainingCorrespondences (const pcl::Correspondences& original_correspondences, 00095 pcl::Correspondences& remaining_correspondences); 00096 00097 /** \brief Sets the focal length parameters of the target camera. 00098 * \param[in] fx the focal length in pixels along the x-axis of the image 00099 * \param[in] fy the focal length in pixels along the y-axis of the image 00100 */ 00101 inline void 00102 setFocalLengths (const float fx, const float fy) 00103 { 00104 projection_matrix_ (0, 0) = fx; 00105 projection_matrix_ (1, 1) = fy; 00106 } 00107 00108 /** \brief Reads back the focal length parameters of the target camera. 00109 * \param[out] fx the focal length in pixels along the x-axis of the image 00110 * \param[out] fy the focal length in pixels along the y-axis of the image 00111 */ 00112 inline void 00113 getFocalLengths (float &fx, float &fy) const 00114 { 00115 fx = projection_matrix_ (0, 0); 00116 fy = projection_matrix_ (1, 1); 00117 } 00118 00119 00120 /** \brief Sets the camera center parameters of the target camera. 00121 * \param[in] cx the x-coordinate of the camera center 00122 * \param[in] cy the y-coordinate of the camera center 00123 */ 00124 inline void 00125 setCameraCenters (const float cx, const float cy) 00126 { 00127 projection_matrix_ (0, 2) = cx; 00128 projection_matrix_ (1, 2) = cy; 00129 } 00130 00131 /** \brief Reads back the camera center parameters of the target camera. 00132 * \param[out] cx the x-coordinate of the camera center 00133 * \param[out] cy the y-coordinate of the camera center 00134 */ 00135 inline void 00136 getCameraCenters (float &cx, float &cy) const 00137 { 00138 cx = projection_matrix_ (0, 2); 00139 cy = projection_matrix_ (1, 2); 00140 } 00141 00142 protected: 00143 00144 /** \brief Apply the rejection algorithm. 00145 * \param[out] correspondences the set of resultant correspondences. 00146 */ 00147 inline void 00148 applyRejection (pcl::Correspondences &correspondences) 00149 { 00150 getRemainingCorrespondences (*input_correspondences_, correspondences); 00151 } 00152 00153 /** \brief Camera projection matrix. */ 00154 Eigen::Matrix3f projection_matrix_; 00155 00156 public: 00157 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00158 }; 00159 } 00160 } 00161 00162 #include <pcl/registration/impl/correspondence_rejection_sample_consensus_2d.hpp> 00163 00164 #endif // PCL_REGISTRATION_CORRESPONDENCE_REJECTION_SAMPLE_CONSENSUS_2D_H_ 00165