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