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_LINEAR_EPSILON = 0.00000001; // 0.1 mm
37 double Registration::COVARIANCE_ANGULAR_EPSILON = 0.00000003; // 0.01 deg
38 
40 {
41  int regTypeInt = Parameters::defaultRegStrategy();
42  Parameters::parse(parameters, Parameters::kRegStrategy(), regTypeInt);
44  return create(type, parameters);
45 }
46 
48 {
49  UDEBUG("type=%d", (int)type);
50  Registration * reg = 0;
51  switch(type)
52  {
54  reg = new RegistrationIcp(parameters);
55  break;
57  reg = new RegistrationVis(parameters, new RegistrationIcp(parameters));
58  break;
59  default: // kTypeVis
60  reg = new RegistrationVis(parameters);
62  break;
63  }
64  return reg;
65 }
66 
68  repeatOnce_(Parameters::defaultRegRepeatOnce()),
69  force3DoF_(Parameters::defaultRegForce3DoF()),
70  child_(child)
71 {
72  this->parseParameters(parameters);
73 }
74 
76 {
77  delete child_;
78 }
80 {
81  Parameters::parse(parameters, Parameters::kRegRepeatOnce(), repeatOnce_);
82  Parameters::parse(parameters, Parameters::kRegForce3DoF(), force3DoF_);
83 
84  if(child_)
85  {
86  child_->parseParameters(parameters);
87  }
88 }
89 
91 {
92  bool val = isImageRequiredImpl();
93  if(!val && child_)
94  {
95  val = child_->isImageRequired();
96  }
97  return val;
98 }
99 
101 {
102  bool val = isScanRequiredImpl();
103  if(!val && child_)
104  {
105  val = child_->isScanRequired();
106  }
107  return val;
108 }
109 
111 {
112  bool val = isUserDataRequiredImpl();
113  if(!val && child_)
114  {
115  val = child_->isUserDataRequired();
116  }
117  return val;
118 }
119 
121 {
122  bool val = canUseGuessImpl();
123  if(!val && child_)
124  {
125  val = child_->canUseGuess();
126  }
127  return val;
128 }
129 
131 {
132  int min = this->getMinVisualCorrespondencesImpl();
133  if(child_)
134  {
135  int childMin = child_->getMinVisualCorrespondences();
136  if(min == 0 || childMin > min)
137  {
138  min = childMin;
139  }
140  }
141  return min;
142 }
143 
145 {
147  if(child_)
148  {
149  float childMin = child_->getMinGeometryCorrespondencesRatio();
150  if(min == 0 || childMin > min)
151  {
152  min = childMin;
153  }
154  }
155  return min;
156 }
157 
159 {
160  if(child_)
161  {
162  delete child_;
163  }
164  child_ = child;
165 }
166 
168  const Signature & from,
169  const Signature & to,
170  Transform guess,
171  RegistrationInfo * infoOut) const
172 {
173  Signature fromCopy(from);
174  Signature toCopy(to);
175  return computeTransformationMod(fromCopy, toCopy, guess, infoOut);
176 }
177 
179  const SensorData & from,
180  const SensorData & to,
181  Transform guess,
182  RegistrationInfo * infoOut) const
183 {
184  Signature fromCopy(from);
185  Signature toCopy(to);
186  return computeTransformationMod(fromCopy, toCopy, guess, infoOut);
187 }
188 
190  Signature & from,
191  Signature & to,
192  Transform guess,
193  RegistrationInfo * infoOut) const
194 {
195  UTimer time;
197  if(infoOut)
198  {
199  info = *infoOut;
200  }
201 
202  if(!guess.isNull() && force3DoF_)
203  {
204  guess = guess.to3DoF();
205  }
206 
207  Transform t = computeTransformationImpl(from, to, guess, info);
208 
209  if(child_)
210  {
211  if(!t.isNull())
212  {
213  t = child_->computeTransformationMod(from, to, force3DoF_?t.to3DoF():t, &info);
214  }
215  else if(!guess.isNull())
216  {
217  UDEBUG("This registration approach failed, continue with the guess for the next registration");
218  t = child_->computeTransformationMod(from, to, guess, &info);
219  }
220  }
221  else if(repeatOnce_ && guess.isNull() && !t.isNull() && this->canUseGuess())
222  {
223  // redo with guess to get a more accurate transform
224  t = computeTransformationImpl(from, to, t, info);
225 
226  if(!t.isNull() && force3DoF_)
227  {
228  t = t.to3DoF();
229  }
230  }
231  else if(!t.isNull() && force3DoF_)
232  {
233  t = t.to3DoF();
234  }
235 
236  if(info.covariance.empty())
237  {
238  info.covariance = cv::Mat::eye(6,6,CV_64FC1);
239  }
240 
241  if(info.covariance.at<double>(0,0)<=COVARIANCE_LINEAR_EPSILON)
242  info.covariance.at<double>(0,0) = COVARIANCE_LINEAR_EPSILON; // epsilon if exact transform
243  if(info.covariance.at<double>(1,1)<=COVARIANCE_LINEAR_EPSILON)
244  info.covariance.at<double>(1,1) = COVARIANCE_LINEAR_EPSILON; // epsilon if exact transform
245  if(info.covariance.at<double>(2,2)<=COVARIANCE_LINEAR_EPSILON)
246  info.covariance.at<double>(2,2) = COVARIANCE_LINEAR_EPSILON; // epsilon if exact transform
247  if(info.covariance.at<double>(3,3)<=COVARIANCE_ANGULAR_EPSILON)
248  info.covariance.at<double>(3,3) = COVARIANCE_ANGULAR_EPSILON; // epsilon if exact transform
249  if(info.covariance.at<double>(4,4)<=COVARIANCE_ANGULAR_EPSILON)
250  info.covariance.at<double>(4,4) = COVARIANCE_ANGULAR_EPSILON; // epsilon if exact transform
251  if(info.covariance.at<double>(5,5)<=COVARIANCE_ANGULAR_EPSILON)
252  info.covariance.at<double>(5,5) = COVARIANCE_ANGULAR_EPSILON; // epsilon if exact transform
253 
254 
255  if(infoOut)
256  {
257  *infoOut = info;
258  infoOut->totalTime = time.ticks();
259  }
260  return t;
261 }
262 
263 }
rtabmap::SensorData
Definition: SensorData.h:51
glm::min
GLM_FUNC_DECL genType min(genType const &x, genType const &y)
rtabmap::Registration::~Registration
virtual ~Registration()
Definition: Registration.cpp:75
rtabmap::Registration::computeTransformationMod
Transform computeTransformationMod(Signature &from, Signature &to, Transform guess=Transform::getIdentity(), RegistrationInfo *info=0) const
Definition: Registration.cpp:189
rtabmap::RegistrationIcp
Definition: RegistrationIcp.h:39
rtabmap::Registration
Definition: Registration.h:39
rtabmap::Registration::repeatOnce_
bool repeatOnce_
Definition: Registration.h:111
rtabmap::RegistrationInfo
Definition: RegistrationInfo.h:34
this
this
rtabmap::Registration::kTypeIcp
@ kTypeIcp
Definition: Registration.h:45
type
rtabmap::Registration::computeTransformationImpl
virtual Transform computeTransformationImpl(Signature &from, Signature &to, Transform guess, RegistrationInfo &info) const =0
rtabmap::Registration::computeTransformation
Transform computeTransformation(const Signature &from, const Signature &to, Transform guess=Transform::getIdentity(), RegistrationInfo *info=0) const
Definition: Registration.cpp:167
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
UTimer.h
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
rtabmap::Registration::COVARIANCE_LINEAR_EPSILON
static double COVARIANCE_LINEAR_EPSILON
Definition: Registration.h:48
rtabmap::Registration::getMinVisualCorrespondencesImpl
virtual int getMinVisualCorrespondencesImpl() const
Definition: Registration.h:107
rtabmap::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:500
rtabmap::Registration::getMinGeometryCorrespondencesRatio
float getMinGeometryCorrespondencesRatio() const
Definition: Registration.cpp:144
rtabmap::Registration::isImageRequired
bool isImageRequired() const
Definition: Registration.cpp:90
rtabmap::RegistrationVis
Definition: RegistrationVis.h:45
rtabmap::Registration::isImageRequiredImpl
virtual bool isImageRequiredImpl() const
Definition: Registration.h:103
info
else if n * info
rtabmap::Registration::Type
Type
Definition: Registration.h:42
rtabmap::Registration::child_
Registration * child_
Definition: Registration.h:113
rtabmap::Registration::getMinGeometryCorrespondencesRatioImpl
virtual float getMinGeometryCorrespondencesRatioImpl() const
Definition: Registration.h:108
rtabmap::Registration::isScanRequiredImpl
virtual bool isScanRequiredImpl() const
Definition: Registration.h:104
rtabmap::Registration::isUserDataRequired
bool isUserDataRequired() const
Definition: Registration.cpp:110
rtabmap::Parameters
Definition: Parameters.h:170
rtabmap::Registration::kTypeVisIcp
@ kTypeVisIcp
Definition: Registration.h:46
rtabmap::Registration::isScanRequired
bool isScanRequired() const
Definition: Registration.cpp:100
time
#define time
rtabmap::Registration::isUserDataRequiredImpl
virtual bool isUserDataRequiredImpl() const
Definition: Registration.h:105
ULogger.h
ULogger class and convenient macros.
rtabmap::RegistrationInfo::totalTime
double totalTime
Definition: RegistrationInfo.h:77
rtabmap::Transform
Definition: Transform.h:41
rtabmap::Registration::parseParameters
virtual void parseParameters(const ParametersMap &parameters)
Definition: Registration.cpp:79
rtabmap::Registration::kTypeVis
@ kTypeVis
Definition: Registration.h:44
rtabmap::Registration::canUseGuessImpl
virtual bool canUseGuessImpl() const
Definition: Registration.h:106
RegistrationVis.h
rtabmap::Registration::create
static Registration * create(const ParametersMap &parameters)
Definition: Registration.cpp:39
UDEBUG
#define UDEBUG(...)
RegistrationIcp.h
UTimer
Definition: UTimer.h:46
rtabmap::Registration::getMinVisualCorrespondences
int getMinVisualCorrespondences() const
Definition: Registration.cpp:130
rtabmap::Transform::to3DoF
Transform to3DoF() const
Definition: Transform.cpp:210
t
Point2 t(10, 10)
rtabmap
Definition: CameraARCore.cpp:35
rtabmap::Registration::canUseGuess
bool canUseGuess() const
Definition: Registration.cpp:120
rtabmap::Registration::force3DoF_
bool force3DoF_
Definition: Registration.h:112
rtabmap::Registration::setChildRegistration
void setChildRegistration(Registration *child)
Definition: Registration.cpp:158
rtabmap::Registration::COVARIANCE_ANGULAR_EPSILON
static double COVARIANCE_ANGULAR_EPSILON
Definition: Registration.h:49
rtabmap::Signature
Definition: Signature.h:48
rtabmap::Registration::Registration
Registration(const ParametersMap &parameters=ParametersMap(), Registration *child=0)
Definition: Registration.cpp:67


rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:15