Registration.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 
32 #include <rtabmap/utilite/UTimer.h>
33 
34 namespace rtabmap {
35 
36 double Registration::COVARIANCE_EPSILON = 0.000000001;
37 
39 {
40  int regTypeInt = Parameters::defaultRegStrategy();
41  Parameters::parse(parameters, Parameters::kRegStrategy(), regTypeInt);
42  Registration::Type type = (Registration::Type)regTypeInt;
43  return create(type, parameters);
44 }
45 
47 {
48  UDEBUG("type=%d", (int)type);
49  Registration * reg = 0;
50  switch(type)
51  {
53  reg = new RegistrationIcp(parameters);
54  break;
56  reg = new RegistrationVis(parameters, new RegistrationIcp(parameters));
57  break;
58  default: // kTypeVis
59  reg = new RegistrationVis(parameters);
61  break;
62  }
63  return reg;
64 }
65 
67  repeatOnce_(Parameters::defaultRegRepeatOnce()),
68  force3DoF_(Parameters::defaultRegForce3DoF()),
69  child_(child)
70 {
71  this->parseParameters(parameters);
72 }
73 
75 {
76  delete child_;
77 }
79 {
80  Parameters::parse(parameters, Parameters::kRegRepeatOnce(), repeatOnce_);
81  Parameters::parse(parameters, Parameters::kRegForce3DoF(), force3DoF_);
82 
83  if(child_)
84  {
85  child_->parseParameters(parameters);
86  }
87 }
88 
90 {
91  bool val = isImageRequiredImpl();
92  if(!val && child_)
93  {
94  val = child_->isImageRequired();
95  }
96  return val;
97 }
98 
100 {
101  bool val = isScanRequiredImpl();
102  if(!val && child_)
103  {
104  val = child_->isScanRequired();
105  }
106  return val;
107 }
108 
110 {
111  bool val = isUserDataRequiredImpl();
112  if(!val && child_)
113  {
114  val = child_->isUserDataRequired();
115  }
116  return val;
117 }
118 
120 {
121  bool val = canUseGuessImpl();
122  if(!val && child_)
123  {
124  val = child_->canUseGuess();
125  }
126  return val;
127 }
128 
130 {
131  int min = this->getMinVisualCorrespondencesImpl();
132  if(child_)
133  {
134  int childMin = child_->getMinVisualCorrespondences();
135  if(min == 0 || childMin > min)
136  {
137  min = childMin;
138  }
139  }
140  return min;
141 }
142 
144 {
146  if(child_)
147  {
148  float childMin = child_->getMinGeometryCorrespondencesRatio();
149  if(min == 0 || childMin > min)
150  {
151  min = childMin;
152  }
153  }
154  return min;
155 }
156 
158 {
159  if(child_)
160  {
161  delete child_;
162  }
163  child_ = child;
164 }
165 
167  const Signature & from,
168  const Signature & to,
169  Transform guess,
170  RegistrationInfo * infoOut) const
171 {
172  Signature fromCopy(from);
173  Signature toCopy(to);
174  return computeTransformationMod(fromCopy, toCopy, guess, infoOut);
175 }
176 
178  const SensorData & from,
179  const SensorData & to,
180  Transform guess,
181  RegistrationInfo * infoOut) const
182 {
183  Signature fromCopy(from);
184  Signature toCopy(to);
185  return computeTransformationMod(fromCopy, toCopy, guess, infoOut);
186 }
187 
189  Signature & from,
190  Signature & to,
191  Transform guess,
192  RegistrationInfo * infoOut) const
193 {
194  UTimer time;
195  RegistrationInfo info;
196  if(infoOut)
197  {
198  info = *infoOut;
199  }
200 
201  if(!guess.isNull() && force3DoF_)
202  {
203  guess = guess.to3DoF();
204  }
205 
206  Transform t = computeTransformationImpl(from, to, guess, info);
207 
208  if(child_)
209  {
210  if(!t.isNull())
211  {
212  t = child_->computeTransformationMod(from, to, force3DoF_?t.to3DoF():t, &info);
213  }
214  else if(!guess.isNull())
215  {
216  UDEBUG("This registration approach failed, continue with the guess for the next registration");
217  t = child_->computeTransformationMod(from, to, guess, &info);
218  }
219  }
220  else if(repeatOnce_ && guess.isNull() && !t.isNull() && this->canUseGuess())
221  {
222  // redo with guess to get a more accurate transform
223  t = computeTransformationImpl(from, to, t, info);
224 
225  if(!t.isNull() && force3DoF_)
226  {
227  t = t.to3DoF();
228  }
229  }
230  else if(!t.isNull() && force3DoF_)
231  {
232  t = t.to3DoF();
233  }
234 
235  if(info.covariance.empty())
236  {
237  info.covariance = cv::Mat::eye(6,6,CV_64FC1);
238  }
239 
240  if(info.covariance.at<double>(0,0)<=COVARIANCE_EPSILON)
241  info.covariance.at<double>(0,0) = COVARIANCE_EPSILON; // epsilon if exact transform
242  if(info.covariance.at<double>(1,1)<=COVARIANCE_EPSILON)
243  info.covariance.at<double>(1,1) = COVARIANCE_EPSILON; // epsilon if exact transform
244  if(info.covariance.at<double>(2,2)<=COVARIANCE_EPSILON)
245  info.covariance.at<double>(2,2) = COVARIANCE_EPSILON; // epsilon if exact transform
246  if(info.covariance.at<double>(3,3)<=COVARIANCE_EPSILON)
247  info.covariance.at<double>(3,3) = COVARIANCE_EPSILON; // epsilon if exact transform
248  if(info.covariance.at<double>(4,4)<=COVARIANCE_EPSILON)
249  info.covariance.at<double>(4,4) = COVARIANCE_EPSILON; // epsilon if exact transform
250  if(info.covariance.at<double>(5,5)<=COVARIANCE_EPSILON)
251  info.covariance.at<double>(5,5) = COVARIANCE_EPSILON; // epsilon if exact transform
252 
253 
254  if(infoOut)
255  {
256  *infoOut = info;
257  infoOut->totalTime = time.ticks();
258  }
259  return t;
260 }
261 
262 }
void setChildRegistration(Registration *child)
bool isUserDataRequired() const
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:476
int getMinVisualCorrespondences() const
Definition: UTimer.h:46
GLM_FUNC_DECL genType min(genType const &x, genType const &y)
virtual bool isScanRequiredImpl() const
Definition: Registration.h:103
float getMinGeometryCorrespondencesRatio() const
Transform computeTransformationMod(Signature &from, Signature &to, Transform guess=Transform::getIdentity(), RegistrationInfo *info=0) const
Transform computeTransformation(const Signature &from, const Signature &to, Transform guess=Transform::getIdentity(), RegistrationInfo *info=0) const
static double COVARIANCE_EPSILON
Definition: Registration.h:48
virtual Transform computeTransformationImpl(Signature &from, Signature &to, Transform guess, RegistrationInfo &info) const =0
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
virtual bool isImageRequiredImpl() const
Definition: Registration.h:102
bool isNull() const
Definition: Transform.cpp:107
bool isScanRequired() const
virtual void parseParameters(const ParametersMap &parameters)
bool canUseGuess() const
virtual bool canUseGuessImpl() const
Definition: Registration.h:105
Transform to3DoF() const
Definition: Transform.cpp:210
static Registration * create(const ParametersMap &parameters)
#define UDEBUG(...)
Registration * child_
Definition: Registration.h:112
ULogger class and convenient macros.
virtual float getMinGeometryCorrespondencesRatioImpl() const
Definition: Registration.h:107
bool isImageRequired() const
double ticks()
Definition: UTimer.cpp:117
virtual int getMinVisualCorrespondencesImpl() const
Definition: Registration.h:106
virtual bool isUserDataRequiredImpl() const
Definition: Registration.h:104
Registration(const ParametersMap &parameters=ParametersMap(), Registration *child=0)


rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:35:00