Point Cloud Library (PCL)  1.14.1-dev
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 #pragma once
40 
41 #include <pcl/registration/correspondence_rejection_sample_consensus.h>
42 #include <pcl/memory.h>
43 
44 namespace pcl {
45 namespace registration {
46 /** \brief CorrespondenceRejectorSampleConsensus2D implements a pixel-based
47  * correspondence rejection using Random Sample Consensus to identify inliers
48  * (and reject outliers)
49  * \author Radu B. Rusu
50  * \ingroup registration
51  */
52 template <typename PointT>
56  using PointCloudPtr = typename PointCloud::Ptr;
57  using PointCloudConstPtr = typename PointCloud::ConstPtr;
58 
59 public:
69 
70  using Ptr = shared_ptr<CorrespondenceRejectorSampleConsensus2D<PointT>>;
71  using ConstPtr = shared_ptr<const CorrespondenceRejectorSampleConsensus2D<PointT>>;
72 
73  /** \brief Empty constructor. Sets the inlier threshold to 5cm (0.05m),
74  * and the maximum number of iterations to 1000.
75  */
77  : projection_matrix_(Eigen::Matrix3f::Identity())
78  {
79  rejection_name_ = "CorrespondenceRejectorSampleConsensus2D";
80  // Put the projection matrix together
81  // projection_matrix_ (0, 0) = 525.f;
82  // projection_matrix_ (1, 1) = 525.f;
83  // projection_matrix_ (0, 2) = 320.f;
84  // projection_matrix_ (1, 2) = 240.f;
85  }
86 
87  /** \brief Get a list of valid correspondences after rejection from the original set
88  * of correspondences. \param[in] original_correspondences the set of initial
89  * correspondences given \param[out] remaining_correspondences the resultant filtered
90  * set of remaining correspondences
91  */
92  inline void
93  getRemainingCorrespondences(const pcl::Correspondences& original_correspondences,
94  pcl::Correspondences& remaining_correspondences);
95 
96  /** \brief Sets the focal length parameters of the target camera.
97  * \param[in] fx the focal length in pixels along the x-axis of the image
98  * \param[in] fy the focal length in pixels along the y-axis of the image
99  */
100  inline void
101  setFocalLengths(const float fx, const float fy)
102  {
103  projection_matrix_(0, 0) = fx;
104  projection_matrix_(1, 1) = fy;
105  }
106 
107  /** \brief Reads back the focal length parameters of the target camera.
108  * \param[out] fx the focal length in pixels along the x-axis of the image
109  * \param[out] fy the focal length in pixels along the y-axis of the image
110  */
111  inline void
112  getFocalLengths(float& fx, float& fy) const
113  {
114  fx = projection_matrix_(0, 0);
115  fy = projection_matrix_(1, 1);
116  }
117 
118  /** \brief Sets the camera center parameters of the target camera.
119  * \param[in] cx the x-coordinate of the camera center
120  * \param[in] cy the y-coordinate of the camera center
121  */
122  inline void
123  setCameraCenters(const float cx, const float cy)
124  {
125  projection_matrix_(0, 2) = cx;
126  projection_matrix_(1, 2) = cy;
127  }
128 
129  /** \brief Reads back the camera center parameters of the target camera.
130  * \param[out] cx the x-coordinate of the camera center
131  * \param[out] cy the y-coordinate of the camera center
132  */
133  inline void
134  getCameraCenters(float& cx, float& cy) const
135  {
136  cx = projection_matrix_(0, 2);
137  cy = projection_matrix_(1, 2);
138  }
139 
140 protected:
141  /** \brief Apply the rejection algorithm.
142  * \param[out] correspondences the set of resultant correspondences.
143  */
144  inline void
146  {
148  }
149 
150  /** \brief Camera projection matrix. */
151  Eigen::Matrix3f projection_matrix_;
152 
153 public:
155 };
156 } // namespace registration
157 } // namespace pcl
158 
159 #include <pcl/registration/impl/correspondence_rejection_sample_consensus_2d.hpp>
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:413
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:414
shared_ptr< const CorrespondenceRejector > ConstPtr
shared_ptr< CorrespondenceRejector > Ptr
CorrespondenceRejectorSampleConsensus2D implements a pixel-based correspondence rejection using Rando...
void getCameraCenters(float &cx, float &cy) const
Reads back the camera center parameters of the target camera.
void getRemainingCorrespondences(const pcl::Correspondences &original_correspondences, pcl::Correspondences &remaining_correspondences)
Get a list of valid correspondences after rejection from the original set of correspondences.
void setFocalLengths(const float fx, const float fy)
Sets the focal length parameters of the target camera.
void setCameraCenters(const float cx, const float cy)
Sets the camera center parameters of the target camera.
void getFocalLengths(float &fx, float &fy) const
Reads back the focal length parameters of the target camera.
void applyRejection(pcl::Correspondences &correspondences)
Apply the rejection algorithm.
CorrespondenceRejectorSampleConsensus implements a correspondence rejection using Random Sample Conse...
CorrespondencesConstPtr input_correspondences_
The input correspondences.
std::string rejection_name_
The name of the rejection method.
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition: memory.h:63
Defines functions, macros and traits for allocating and using memory.
Definition: bfgs.h:10
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences