Point Cloud Library (PCL) 1.15.0
Loading...
Searching...
No Matches
greedy_verification.h
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2010-2011, Willow Garage, 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 Willow Garage, Inc. 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#pragma once
38
39#include <pcl/pcl_macros.h>
40#include <pcl/recognition/hv/hypotheses_verification.h>
41
42#include <memory>
43
44namespace pcl
45{
46
47 /**
48 * \brief A greedy hypothesis verification method
49 * \author Aitor Aldoma
50 * \ingroup recognition
51 */
52
53 template<typename ModelT, typename SceneT>
54 class PCL_EXPORTS GreedyVerification : public HypothesisVerification<ModelT, SceneT>
55 {
56 using HypothesisVerification<ModelT, SceneT>::mask_;
57 using HypothesisVerification<ModelT, SceneT>::scene_cloud_downsampled_;
58 using HypothesisVerification<ModelT, SceneT>::scene_downsampled_tree_;
59 using HypothesisVerification<ModelT, SceneT>::visible_models_;
60 using HypothesisVerification<ModelT, SceneT>::resolution_;
61 using HypothesisVerification<ModelT, SceneT>::inliers_threshold_;
62
63 /*
64 * \brief Recognition model using during the verification
65 */
66 class RecognitionModel
67 {
68 public:
69 std::vector<int> explained_;
70 typename pcl::PointCloud<ModelT>::Ptr cloud_;
71 int bad_information_;
72 int good_information_;
73 int id_;
74 float regularizer_;
75 };
76
77 using RecognitionModelPtr = std::shared_ptr<RecognitionModel>;
78
79 /*
80 * \brief Sorts recognition models based on the number of explained scene points and visible outliers
81 */
82 struct sortModelsClass
83 {
84 bool
85 operator() (const RecognitionModelPtr & n1, const RecognitionModelPtr & n2)
86 {
87 float val1 = static_cast<float>(n1->good_information_) - static_cast<float>(n1->bad_information_) * n1->regularizer_;
88 float val2 = static_cast<float>(n2->good_information_) - static_cast<float>(n2->bad_information_) * n2->regularizer_;
89 return val1 > val2;
90 }
91 } sortModelsOp;
92
93
94 /*
95 * \brief Recognition model indices to keep track of the sorted recognition hypotheses
96 */
97 struct modelIndices
98 {
99 int index_;
100 RecognitionModelPtr model_;
101 };
102
103 /*
104 * \brief Sorts model indices similar to sortModelsClass
105 */
106 struct sortModelIndicesClass
107 {
108 bool
109 operator() (const modelIndices & n1, const modelIndices & n2)
110 {
111 float val1 = static_cast<float>(n1.model_->good_information_) - static_cast<float>(n1.model_->bad_information_) * n1.model_->regularizer_;
112 float val2 = static_cast<float>(n2.model_->good_information_) - static_cast<float>(n2.model_->bad_information_) * n2.model_->regularizer_;
113 return val1 > val2;
114 }
115 } sortModelsIndicesOp;
116
117 /** \brief Recognition model and indices */
118 std::vector<modelIndices> indices_models_;
119
120 /** \brief Recognition models (hypotheses to be verified) */
121 std::vector<RecognitionModelPtr> recognition_models_;
122
123 /** \brief Recognition models that explain a scene points. */
124 std::vector<std::vector<RecognitionModelPtr>> points_explained_by_rm_;
125
126 /** \brief Weighting for outliers */
127 float regularizer_;
128
129 /** \brief Initialize the data structures */
130 void
131 initialize ();
132
133 /** \brief Sorts the hypotheses for the greedy approach */
134 void
135 sortModels ()
136 {
137 indices_models_.clear ();
138 for (std::size_t i = 0; i < recognition_models_.size (); i++)
139 {
140 modelIndices mi;
141 mi.index_ = static_cast<int> (i);
142 mi.model_ = recognition_models_[i];
143 indices_models_.push_back (mi);
144 }
145
146 std::sort (indices_models_.begin (), indices_models_.end (), sortModelsIndicesOp);
147 //sort also recognition models
148 std::sort (recognition_models_.begin (), recognition_models_.end (), sortModelsOp);
149 }
150
151 /** \brief Updates conflicting recognition hypotheses when a hypothesis is accepted */
152 void
153 updateGoodInformation (int i)
154 {
155 for (std::size_t k = 0; k < recognition_models_[i]->explained_.size (); k++)
156 {
157 //update good_information_ for all hypotheses that were explaining the same points as hypothesis i
158 for (std::size_t kk = 0; kk < points_explained_by_rm_[recognition_models_[i]->explained_[k]].size (); kk++)
159 {
160 (points_explained_by_rm_[recognition_models_[i]->explained_[k]])[kk]->good_information_--;
161 (points_explained_by_rm_[recognition_models_[i]->explained_[k]])[kk]->bad_information_++;
162 }
163 }
164 }
165
166 public:
167
168 /** \brief Constructor
169 * \param[in] reg Regularizer value
170 **/
171 GreedyVerification (float reg = 1.5f) :
172 HypothesisVerification<ModelT, SceneT> ()
173 {
174 regularizer_ = reg;
175 }
176
177 /** \brief Starts verification */
178 void
179 verify () override;
180 };
181}
182
183#ifdef PCL_NO_PRECOMPILE
184#include <pcl/recognition/impl/hv/greedy_verification.hpp>
185#endif
A greedy hypothesis verification method.
GreedyVerification(float reg=1.5f)
Constructor.
Abstract class for hypotheses verification methods.
shared_ptr< PointCloud< PointT > > Ptr
Defines all the PCL and non-PCL macros used.