OdometryF2F.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 
34 #include "rtabmap/utilite/UTimer.h"
35 #include "rtabmap/utilite/UStl.h"
36 
37 namespace rtabmap {
38 
40  Odometry(parameters),
41  keyFrameThr_(Parameters::defaultOdomKeyFrameThr()),
42  visKeyFrameThr_(Parameters::defaultOdomVisKeyFrameThr()),
43  scanKeyFrameThr_(Parameters::defaultOdomScanKeyFrameThr())
44 {
46  Parameters::parse(parameters, Parameters::kOdomKeyFrameThr(), keyFrameThr_);
47  Parameters::parse(parameters, Parameters::kOdomVisKeyFrameThr(), visKeyFrameThr_);
48  Parameters::parse(parameters, Parameters::kOdomScanKeyFrameThr(), scanKeyFrameThr_);
49  UASSERT(keyFrameThr_>=0.0f && keyFrameThr_<=1.0f);
52 
53  parameters_ = parameters;
54 }
55 
57 {
58  delete registrationPipeline_;
59 }
60 
61 void OdometryF2F::reset(const Transform & initialPose)
62 {
63  Odometry::reset(initialPose);
64  refFrame_ = Signature();
66 }
67 
68 // return not null transform if odometry is correctly computed
70  SensorData & data,
71  const Transform & guess,
72  OdometryInfo * info)
73 {
74  UTimer timer;
75  Transform output;
76  if(!data.rightRaw().empty() &&
77  (data.stereoCameraModels().empty() || !data.stereoCameraModels()[0].isValidForProjection()))
78  {
79  UERROR("Calibrated stereo camera required.");
80  return output;
81  }
82  if(!data.depthRaw().empty() &&
83  (data.cameraModels().empty() || !data.cameraModels()[0].isValidForProjection()))
84  {
85  UERROR("Calibrated camera required.");
86  return output;
87  }
88 
89  bool addKeyFrame = false;
90  RegistrationInfo regInfo;
91 
92  UASSERT(!this->getPose().isNull());
94  {
95  lastKeyFramePose_ = this->getPose(); // reset to current pose
96  }
97  Transform motionSinceLastKeyFrame = lastKeyFramePose_.inverse()*this->getPose();
98 
99  Signature newFrame(data);
101  {
102  float maxCorrespondenceDistance = 0.0f;
103  float outlierRatio = 0.0f;
104  if(guess.isNull() &&
107  this->framesProcessed() < 2)
108  {
109  // only on initialization (first frame to register), increase icp max correspondences in case the robot is already moving
110  maxCorrespondenceDistance = Parameters::defaultIcpMaxCorrespondenceDistance();
111  outlierRatio = Parameters::defaultIcpOutlierRatio();
112  Parameters::parse(parameters_, Parameters::kIcpMaxCorrespondenceDistance(), maxCorrespondenceDistance);
113  Parameters::parse(parameters_, Parameters::kIcpOutlierRatio(), outlierRatio);
115  params.insert(ParametersPair(Parameters::kIcpMaxCorrespondenceDistance(), uNumber2Str(maxCorrespondenceDistance*3.0f)));
116  params.insert(ParametersPair(Parameters::kIcpOutlierRatio(), uNumber2Str(0.95f)));
118  }
119 
120  Signature tmpRefFrame = refFrame_;
122  tmpRefFrame,
123  newFrame,
124  // special case for ICP-only odom, set guess to identity if we just started or reset
125  !guess.isNull()?motionSinceLastKeyFrame*guess:!registrationPipeline_->isImageRequired()&&this->framesProcessed()<2?motionSinceLastKeyFrame:Transform(),
126  &regInfo);
127 
128  if(maxCorrespondenceDistance>0.0f)
129  {
130  // set it back
132  params.insert(ParametersPair(Parameters::kIcpMaxCorrespondenceDistance(), uNumber2Str(maxCorrespondenceDistance)));
133  params.insert(ParametersPair(Parameters::kIcpOutlierRatio(), uNumber2Str(outlierRatio)));
135  }
136 
137  if(output.isNull() && !guess.isNull() && registrationPipeline_->isImageRequired())
138  {
139  tmpRefFrame = refFrame_;
140  // reset matches, but keep already extracted features in newFrame.sensorData()
141  newFrame.removeAllWords();
142  UWARN("Failed to find a transformation with the provided guess (%s), trying again without a guess.", guess.prettyPrint().c_str());
143  // If optical flow is used, switch temporary to feature matching
144  int visCorTypeBackup = Parameters::defaultVisCorType();
145  Parameters::parse(parameters_, Parameters::kVisCorType(), visCorTypeBackup);
146  if(visCorTypeBackup == 1)
147  {
149  params.insert(ParametersPair(Parameters::kVisCorType(), "0"));
151  }
152 
154  tmpRefFrame,
155  newFrame,
156  Transform(), // null guess
157  &regInfo);
158 
159  if(visCorTypeBackup == 1)
160  {
162  params.insert(ParametersPair(Parameters::kVisCorType(), "1"));
164  }
165 
166  if(output.isNull())
167  {
168  UWARN("Trial with no guess still fail.");
169  }
170  else
171  {
172  UWARN("Trial with no guess succeeded.");
173  }
174  }
175 
176  if(info && this->isInfoDataFilled())
177  {
178  std::list<std::pair<int, std::pair<int, int> > > pairs;
179  EpipolarGeometry::findPairsUnique(tmpRefFrame.getWords(), newFrame.getWords(), pairs);
180  info->refCorners.resize(pairs.size());
181  info->newCorners.resize(pairs.size());
182  std::map<int, int> idToIndex;
183  int i=0;
184  for(std::list<std::pair<int, std::pair<int, int> > >::iterator iter=pairs.begin();
185  iter!=pairs.end();
186  ++iter)
187  {
188  info->refCorners[i] = tmpRefFrame.getWordsKpts()[iter->second.first].pt;
189  info->newCorners[i] = newFrame.getWordsKpts()[iter->second.second].pt;
190  idToIndex.insert(std::make_pair(iter->first, i));
191  ++i;
192  }
193  info->cornerInliers.resize(regInfo.inliersIDs.size(), 1);
194  i=0;
195  for(; i<(int)regInfo.inliersIDs.size(); ++i)
196  {
197  info->cornerInliers[i] = idToIndex.at(regInfo.inliersIDs[i]);
198  }
199 
200  Transform t = this->getPose()*motionSinceLastKeyFrame.inverse();
201  if(!tmpRefFrame.getWords3().empty())
202  {
203  for(std::multimap<int, int>::const_iterator iter=tmpRefFrame.getWords().begin(); iter!=tmpRefFrame.getWords().end(); ++iter)
204  {
205  info->localMap.insert(std::make_pair(iter->first, util3d::transformPoint(tmpRefFrame.getWords3()[iter->second], t)));
206  }
207  }
208  info->localMapSize = tmpRefFrame.getWords3().size();
209  if(!newFrame.getWordsKpts().empty())
210  {
211  for(std::multimap<int, int>::const_iterator iter=newFrame.getWords().begin(); iter!=newFrame.getWords().end(); ++iter)
212  {
213  info->words.insert(std::make_pair(iter->first, newFrame.getWordsKpts()[iter->second]));
214  }
215  }
216 
217  info->localScanMapSize = tmpRefFrame.sensorData().laserScanRaw().size();
218 
219  info->localScanMap = util3d::transformLaserScan(tmpRefFrame.sensorData().laserScanRaw(), tmpRefFrame.sensorData().laserScanRaw().localTransform().inverse()*t*tmpRefFrame.sensorData().laserScanRaw().localTransform());
220  }
221  }
222  else
223  {
224  //return Identity
225  output = Transform::getIdentity();
226  // a very high variance tells that the new pose is not linked with the previous one
227  regInfo.covariance = cv::Mat::eye(6,6,CV_64FC1)*9999.0;
228  }
229 
230  if(!output.isNull())
231  {
232  output = motionSinceLastKeyFrame.inverse() * output;
233 
234  // new key-frame?
236  (keyFrameThr_ == 0.0f ||
237  visKeyFrameThr_ == 0 ||
238  float(regInfo.inliers) <= keyFrameThr_*float(refFrame_.sensorData().keypoints().size()) ||
239  regInfo.inliers <= visKeyFrameThr_)) ||
241  {
242  UDEBUG("Update key frame");
243  int features = newFrame.getWordsDescriptors().rows;
244  if(!refFrame_.sensorData().isValid() || (features==0 && registrationPipeline_->isImageRequired()))
245  {
246  newFrame = Signature(data);
247  // this will generate features only for the first frame or if optical flow was used (no 3d words)
248  // For scan, we want to use reading filters, so set dummy's scan and set back to reference afterwards
250  dummy.sensorData().setLaserScan(newFrame.sensorData().laserScanRaw());
251  newFrame.sensorData().setLaserScan(LaserScan());
253  newFrame,
254  dummy);
255  features = (int)newFrame.sensorData().keypoints().size();
256  newFrame.sensorData().setLaserScan(dummy.sensorData().laserScanRaw(), true);
257  }
258 
261  (newFrame.sensorData().laserScanRaw().size() &&
263  {
264  refFrame_ = newFrame;
265 
267 
268  //reset motion
270 
271  addKeyFrame = true;
272  }
273  else
274  {
275  if (!refFrame_.sensorData().isValid())
276  {
277  // Don't send odometry if we don't have a keyframe yet
278  output.setNull();
279  }
280 
281  if(features < registrationPipeline_->getMinVisualCorrespondences())
282  {
283  UWARN("Too low 2D features (%d), keeping last key frame...", features);
284  }
285 
287  {
288  UWARN("Too low scan points (%d), keeping last key frame...", newFrame.sensorData().laserScanRaw().size());
289  }
291  {
292  UWARN("Too low scan points ratio (%d < %d), keeping last key frame...", float(newFrame.sensorData().laserScanRaw().size())/float(newFrame.sensorData().laserScanRaw().maxPoints()), registrationPipeline_->getMinGeometryCorrespondencesRatio());
293  }
294  }
295  }
296  }
297  else if(!regInfo.rejectedMsg.empty())
298  {
299  UWARN("Registration failed: \"%s\"", regInfo.rejectedMsg.c_str());
300  }
301 
302  data.setFeatures(newFrame.sensorData().keypoints(), newFrame.sensorData().keypoints3D(), newFrame.sensorData().descriptors());
303  data.setLaserScan(newFrame.sensorData().laserScanRaw());
304 
305  if(info)
306  {
307  info->type = kTypeF2F;
308  info->features = newFrame.sensorData().keypoints().size();
309  info->keyFrameAdded = addKeyFrame;
310  if(this->isInfoDataFilled())
311  {
312  info->reg = regInfo;
313  }
314  else
315  {
316  info->reg = regInfo.copyWithoutData();
317  }
318  }
319 
320  UINFO("Odom update time = %fs lost=%s inliers=%d, ref frame corners=%d, transform accepted=%s",
321  timer.elapsed(),
322  output.isNull()?"true":"false",
323  (int)regInfo.inliers,
324  (int)newFrame.sensorData().keypoints().size(),
325  !output.isNull()?"true":"false");
326 
327  return output;
328 }
329 
330 } // namespace rtabmap
rtabmap::OdometryF2F::scanKeyFrameThr_
float scanKeyFrameThr_
Definition: OdometryF2F.h:57
rtabmap::SensorData
Definition: SensorData.h:51
rtabmap::OdometryF2F::~OdometryF2F
virtual ~OdometryF2F()
Definition: OdometryF2F.cpp:56
rtabmap::ParametersPair
std::pair< std::string, std::string > ParametersPair
Definition: Parameters.h:44
int
int
rtabmap::LaserScan::localTransform
Transform localTransform() const
Definition: LaserScan.h:127
rtabmap::SensorData::setLaserScan
void setLaserScan(const LaserScan &laserScan, bool clearPreviousData=true)
Definition: SensorData.cpp:381
UINFO
#define UINFO(...)
rtabmap::Registration::computeTransformationMod
Transform computeTransformationMod(Signature &from, Signature &to, Transform guess=Transform::getIdentity(), RegistrationInfo *info=0) const
Definition: Registration.cpp:189
OdometryInfo.h
rtabmap::LaserScan::maxPoints
int maxPoints() const
Definition: LaserScan.h:120
rtabmap::RegistrationInfo::inliersIDs
std::vector< int > inliersIDs
Definition: RegistrationInfo.h:84
timer
Registration.h
rtabmap::Signature::getWords
const std::multimap< int, int > & getWords() const
Definition: Signature.h:112
rtabmap::RegistrationInfo
Definition: RegistrationInfo.h:34
rtabmap::Transform::getIdentity
static Transform getIdentity()
Definition: Transform.cpp:411
this
this
rtabmap::Signature::getWordsKpts
const std::vector< cv::KeyPoint > & getWordsKpts() const
Definition: Signature.h:113
rtabmap::OdometryF2F::visKeyFrameThr_
int visKeyFrameThr_
Definition: OdometryF2F.h:56
rtabmap::OdometryF2F::OdometryF2F
OdometryF2F(const rtabmap::ParametersMap &parameters=rtabmap::ParametersMap())
Definition: OdometryF2F.cpp:39
rtabmap::util3d::transformLaserScan
LaserScan RTABMAP_CORE_EXPORT transformLaserScan(const LaserScan &laserScan, const Transform &transform)
Definition: util3d_transforms.cpp:39
rtabmap::Signature::getWordsDescriptors
const cv::Mat & getWordsDescriptors() const
Definition: Signature.h:116
rtabmap::SensorData::isValid
bool isValid() const
Definition: SensorData.h:156
rtabmap::OdometryF2F::reset
virtual void reset(const Transform &initialPose=Transform::getIdentity())
Definition: OdometryF2F.cpp:61
iterator
rtabmap::Transform::setNull
void setNull()
Definition: Transform.cpp:152
rtabmap::LaserScan
Definition: LaserScan.h:37
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
UTimer.h
EpipolarGeometry.h
rtabmap::LaserScan::size
int size() const
Definition: LaserScan.h:131
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
dummy
dummy
rtabmap::RegistrationInfo::copyWithoutData
RegistrationInfo copyWithoutData() const
Definition: RegistrationInfo.h:55
rtabmap::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:500
OdometryF2F.h
glm::isNull
GLM_FUNC_DECL bool isNull(detail::tmat2x2< T, P > const &m, T const &epsilon)
rtabmap::Odometry::getPose
const Transform & getPose() const
Definition: Odometry.h:76
rtabmap::util3d::transformPoint
cv::Point3f RTABMAP_CORE_EXPORT transformPoint(const cv::Point3f &pt, const Transform &transform)
Definition: util3d_transforms.cpp:211
rtabmap::EpipolarGeometry::findPairsUnique
static int findPairsUnique(const std::multimap< int, T > &wordsA, const std::multimap< int, T > &wordsB, std::list< std::pair< int, std::pair< T, T > > > &pairs, bool ignoreNegativeIds=true)
Definition: EpipolarGeometry.h:158
rtabmap::Registration::getMinGeometryCorrespondencesRatio
float getMinGeometryCorrespondencesRatio() const
Definition: Registration.cpp:144
rtabmap::Transform::prettyPrint
std::string prettyPrint() const
Definition: Transform.cpp:326
rtabmap::SensorData::laserScanRaw
const LaserScan & laserScanRaw() const
Definition: SensorData.h:185
data
int data[]
rtabmap::Registration::isImageRequired
bool isImageRequired() const
Definition: Registration.cpp:90
util3d_transforms.h
uNumber2Str
std::string UTILITE_EXPORT uNumber2Str(unsigned int number)
Definition: UConversion.cpp:91
rtabmap::SensorData::descriptors
const cv::Mat & descriptors() const
Definition: SensorData.h:276
rtabmap::RegistrationInfo::rejectedMsg
std::string rejectedMsg
Definition: RegistrationInfo.h:76
rtabmap::Signature::getWords3
const std::vector< cv::Point3f > & getWords3() const
Definition: Signature.h:132
rtabmap::Odometry::reset
virtual void reset(const Transform &initialPose=Transform::getIdentity())
Definition: Odometry.cpp:210
rtabmap::RegistrationInfo::covariance
cv::Mat covariance
Definition: RegistrationInfo.h:75
info
else if n * info
rtabmap::RegistrationInfo::icpInliersRatio
float icpInliersRatio
Definition: RegistrationInfo.h:90
rtabmap::RegistrationInfo::inliers
int inliers
Definition: RegistrationInfo.h:80
UASSERT
#define UASSERT(condition)
params
SmartProjectionParams params(gtsam::HESSIAN, gtsam::ZERO_ON_DEGENERACY)
rtabmap::Parameters
Definition: Parameters.h:170
rtabmap::Registration::isScanRequired
bool isScanRequired() const
Definition: Registration.cpp:100
rtabmap::OdometryF2F::keyFrameThr_
float keyFrameThr_
Definition: OdometryF2F.h:55
rtabmap::OdometryF2F::registrationPipeline_
Registration * registrationPipeline_
Definition: OdometryF2F.h:59
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:178
UWARN
#define UWARN(...)
f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
timer::elapsed
double elapsed() const
ULogger.h
ULogger class and convenient macros.
rtabmap::Transform
Definition: Transform.h:41
rtabmap::Registration::parseParameters
virtual void parseParameters(const ParametersMap &parameters)
Definition: Registration.cpp:79
rtabmap::OdometryF2F::computeTransform
virtual Transform computeTransform(SensorData &image, const Transform &guess=Transform(), OdometryInfo *info=0)
Definition: OdometryF2F.cpp:69
iter
iterator iter(handle obj)
UStl.h
Wrappers of STL for convenient functions.
rtabmap::Registration::create
static Registration * create(const ParametersMap &parameters)
Definition: Registration.cpp:39
UDEBUG
#define UDEBUG(...)
rtabmap::Odometry
Definition: Odometry.h:42
rtabmap::OdometryInfo::reg
RegistrationInfo reg
Definition: OdometryInfo.h:99
UTimer
Definition: UTimer.h:46
rtabmap::OdometryInfo
Definition: OdometryInfo.h:40
float
float
rtabmap::OdometryF2F::lastKeyFramePose_
Transform lastKeyFramePose_
Definition: OdometryF2F.h:61
rtabmap::SensorData::keypoints
const std::vector< cv::KeyPoint > & keypoints() const
Definition: SensorData.h:274
rtabmap::Registration::getMinVisualCorrespondences
int getMinVisualCorrespondences() const
Definition: Registration.cpp:130
rtabmap::OdometryF2F::refFrame_
Signature refFrame_
Definition: OdometryF2F.h:60
rtabmap::OdometryF2F::parameters_
ParametersMap parameters_
Definition: OdometryF2F.h:62
rtabmap::Odometry::kTypeF2F
@ kTypeF2F
Definition: Odometry.h:48
t
Point2 t(10, 10)
rtabmap::Signature::sensorData
SensorData & sensorData()
Definition: Signature.h:138
rtabmap
Definition: CameraARCore.cpp:35
UERROR
#define UERROR(...)
rtabmap::SensorData::keypoints3D
const std::vector< cv::Point3f > & keypoints3D() const
Definition: SensorData.h:275
rtabmap::Signature::removeAllWords
void removeAllWords()
Definition: Signature.cpp:346
i
int i
rtabmap::Signature
Definition: Signature.h:48
rtabmap::Odometry::isInfoDataFilled
bool isInfoDataFilled() const
Definition: Odometry.h:77


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