Point Cloud Library (PCL)  1.7.1
correspondence_rejection_sample_consensus_2d.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  *
37  */
38 
39 #ifndef PCL_REGISTRATION_CORRESPONDENCE_REJECTION_SAMPLE_CONSENSUS_2D_H_
40 #define PCL_REGISTRATION_CORRESPONDENCE_REJECTION_SAMPLE_CONSENSUS_2D_H_
41 
42 #include <pcl/registration/correspondence_rejection_sample_consensus.h>
43 
44 namespace pcl
45 {
46  namespace registration
47  {
48  /** \brief CorrespondenceRejectorSampleConsensus2D implements a pixel-based
49  * correspondence rejection using Random Sample Consensus to identify inliers
50  * (and reject outliers)
51  * \author Radu B. Rusu
52  * \ingroup registration
53  */
54  template <typename PointT>
56  {
58  typedef typename PointCloud::Ptr PointCloudPtr;
59  typedef typename PointCloud::ConstPtr PointCloudConstPtr;
60 
61  public:
71 
72  typedef boost::shared_ptr<CorrespondenceRejectorSampleConsensus2D> Ptr;
73  typedef boost::shared_ptr<const CorrespondenceRejectorSampleConsensus2D> ConstPtr;
74 
75  /** \brief Empty constructor. Sets the inlier threshold to 5cm (0.05m),
76  * and the maximum number of iterations to 1000.
77  */
79  : projection_matrix_ (Eigen::Matrix3f::Identity ())
80  {
81  rejection_name_ = "CorrespondenceRejectorSampleConsensus2D";
82  // Put the projection matrix together
83  //projection_matrix_ (0, 0) = 525.f;
84  //projection_matrix_ (1, 1) = 525.f;
85  //projection_matrix_ (0, 2) = 320.f;
86  //projection_matrix_ (1, 2) = 240.f;
87  }
88 
89  /** \brief Get a list of valid correspondences after rejection from the original set of correspondences.
90  * \param[in] original_correspondences the set of initial correspondences given
91  * \param[out] remaining_correspondences the resultant filtered set of remaining correspondences
92  */
93  inline void
94  getRemainingCorrespondences (const pcl::Correspondences& original_correspondences,
95  pcl::Correspondences& remaining_correspondences);
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  {
104  projection_matrix_ (0, 0) = fx;
105  projection_matrix_ (1, 1) = fy;
106  }
107 
108  /** \brief Reads back the focal length parameters of the target camera.
109  * \param[out] fx the focal length in pixels along the x-axis of the image
110  * \param[out] fy the focal length in pixels along the y-axis of the image
111  */
112  inline void
113  getFocalLengths (float &fx, float &fy) const
114  {
115  fx = projection_matrix_ (0, 0);
116  fy = projection_matrix_ (1, 1);
117  }
118 
119 
120  /** \brief Sets the camera center parameters of the target camera.
121  * \param[in] cx the x-coordinate of the camera center
122  * \param[in] cy the y-coordinate of the camera center
123  */
124  inline void
125  setCameraCenters (const float cx, const float cy)
126  {
127  projection_matrix_ (0, 2) = cx;
128  projection_matrix_ (1, 2) = cy;
129  }
130 
131  /** \brief Reads back the camera center parameters of the target camera.
132  * \param[out] cx the x-coordinate of the camera center
133  * \param[out] cy the y-coordinate of the camera center
134  */
135  inline void
136  getCameraCenters (float &cx, float &cy) const
137  {
138  cx = projection_matrix_ (0, 2);
139  cy = projection_matrix_ (1, 2);
140  }
141 
142  protected:
143 
144  /** \brief Apply the rejection algorithm.
145  * \param[out] correspondences the set of resultant correspondences.
146  */
147  inline void
149  {
151  }
152 
153  /** \brief Camera projection matrix. */
154  Eigen::Matrix3f projection_matrix_;
155 
156  public:
157  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
158  };
159  }
160 }
161 
162 #include <pcl/registration/impl/correspondence_rejection_sample_consensus_2d.hpp>
163 
164 #endif // PCL_REGISTRATION_CORRESPONDENCE_REJECTION_SAMPLE_CONSENSUS_2D_H_
165