RegistrationVis.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 
29 
30 #include <rtabmap/core/RegistrationVis.h>
31 #include <rtabmap/core/util3d_motion_estimation.h>
32 #include <rtabmap/core/util3d_features.h>
33 #include <rtabmap/core/util3d.h>
34 #include <rtabmap/core/VWDictionary.h>
35 #include <rtabmap/core/util2d.h>
36 #include <rtabmap/core/Features2d.h>
37 #include <rtabmap/core/VisualWord.h>
38 #include <rtabmap/core/Optimizer.h>
39 #include <rtabmap/core/util3d_transforms.h>
40 #include <rtabmap/utilite/ULogger.h>
41 #include <rtabmap/utilite/UConversion.h>
42 #include <rtabmap/utilite/UStl.h>
43 #include <rtabmap/utilite/UTimer.h>
44 #include <rtabmap/utilite/UMath.h>
45 
46 #include <rtflann/flann.hpp>
47 
48 namespace rtabmap {
49 
50 RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration * child) :
51  Registration(parameters, child),
52  _minInliers(Parameters::defaultVisMinInliers()),
53  _inlierDistance(Parameters::defaultVisInlierDistance()),
54  _iterations(Parameters::defaultVisIterations()),
55  _refineIterations(Parameters::defaultVisRefineIterations()),
56  _epipolarGeometryVar(Parameters::defaultVisEpipolarGeometryVar()),
57  _estimationType(Parameters::defaultVisEstimationType()),
58  _forwardEstimateOnly(Parameters::defaultVisForwardEstOnly()),
59  _PnPReprojError(Parameters::defaultVisPnPReprojError()),
60  _PnPFlags(Parameters::defaultVisPnPFlags()),
61  _PnPRefineIterations(Parameters::defaultVisPnPRefineIterations()),
62  _correspondencesApproach(Parameters::defaultVisCorType()),
63  _flowWinSize(Parameters::defaultVisCorFlowWinSize()),
64  _flowIterations(Parameters::defaultVisCorFlowIterations()),
65  _flowEps(Parameters::defaultVisCorFlowEps()),
66  _flowMaxLevel(Parameters::defaultVisCorFlowMaxLevel()),
67  _nndr(Parameters::defaultVisCorNNDR()),
68  _guessWinSize(Parameters::defaultVisCorGuessWinSize()),
69  _guessMatchToProjection(Parameters::defaultVisCorGuessMatchToProjection()),
70  _bundleAdjustment(Parameters::defaultVisBundleAdjustment()),
71  _depthAsMask(Parameters::defaultVisDepthAsMask())
72 {
73  _featureParameters = Parameters::getDefaultParameters();
74  uInsert(_featureParameters, ParametersPair(Parameters::kKpNNStrategy(), _featureParameters.at(Parameters::kVisCorNNType())));
75  uInsert(_featureParameters, ParametersPair(Parameters::kKpNndrRatio(), _featureParameters.at(Parameters::kVisCorNNDR())));
76  uInsert(_featureParameters, ParametersPair(Parameters::kKpDetectorStrategy(), _featureParameters.at(Parameters::kVisFeatureType())));
77  uInsert(_featureParameters, ParametersPair(Parameters::kKpMaxFeatures(), _featureParameters.at(Parameters::kVisMaxFeatures())));
78  uInsert(_featureParameters, ParametersPair(Parameters::kKpMaxDepth(), _featureParameters.at(Parameters::kVisMaxDepth())));
79  uInsert(_featureParameters, ParametersPair(Parameters::kKpMinDepth(), _featureParameters.at(Parameters::kVisMinDepth())));
80  uInsert(_featureParameters, ParametersPair(Parameters::kKpRoiRatios(), _featureParameters.at(Parameters::kVisRoiRatios())));
81  uInsert(_featureParameters, ParametersPair(Parameters::kKpSubPixEps(), _featureParameters.at(Parameters::kVisSubPixWinSize())));
82  uInsert(_featureParameters, ParametersPair(Parameters::kKpSubPixIterations(), _featureParameters.at(Parameters::kVisSubPixIterations())));
83  uInsert(_featureParameters, ParametersPair(Parameters::kKpSubPixWinSize(), _featureParameters.at(Parameters::kVisSubPixEps())));
84  uInsert(_featureParameters, ParametersPair(Parameters::kKpGridRows(), _featureParameters.at(Parameters::kVisGridRows())));
85  uInsert(_featureParameters, ParametersPair(Parameters::kKpGridCols(), _featureParameters.at(Parameters::kVisGridCols())));
86  uInsert(_featureParameters, ParametersPair(Parameters::kKpNewWordsComparedTogether(), "false"));
87 
88  this->parseParameters(parameters);
89 }
90 
91 void RegistrationVis::parseParameters(const ParametersMap & parameters)
92 {
93  Registration::parseParameters(parameters);
94 
95  Parameters::parse(parameters, Parameters::kVisMinInliers(), _minInliers);
96  Parameters::parse(parameters, Parameters::kVisInlierDistance(), _inlierDistance);
97  Parameters::parse(parameters, Parameters::kVisIterations(), _iterations);
98  Parameters::parse(parameters, Parameters::kVisRefineIterations(), _refineIterations);
99  Parameters::parse(parameters, Parameters::kVisEstimationType(), _estimationType);
100  Parameters::parse(parameters, Parameters::kVisForwardEstOnly(), _forwardEstimateOnly);
101  Parameters::parse(parameters, Parameters::kVisEpipolarGeometryVar(), _epipolarGeometryVar);
102  Parameters::parse(parameters, Parameters::kVisPnPReprojError(), _PnPReprojError);
103  Parameters::parse(parameters, Parameters::kVisPnPFlags(), _PnPFlags);
104  Parameters::parse(parameters, Parameters::kVisPnPRefineIterations(), _PnPRefineIterations);
105  Parameters::parse(parameters, Parameters::kVisCorType(), _correspondencesApproach);
106  Parameters::parse(parameters, Parameters::kVisCorFlowWinSize(), _flowWinSize);
107  Parameters::parse(parameters, Parameters::kVisCorFlowIterations(), _flowIterations);
108  Parameters::parse(parameters, Parameters::kVisCorFlowEps(), _flowEps);
109  Parameters::parse(parameters, Parameters::kVisCorFlowMaxLevel(), _flowMaxLevel);
110  Parameters::parse(parameters, Parameters::kVisCorNNDR(), _nndr);
111  Parameters::parse(parameters, Parameters::kVisCorGuessWinSize(), _guessWinSize);
112  Parameters::parse(parameters, Parameters::kVisCorGuessMatchToProjection(), _guessMatchToProjection);
113  Parameters::parse(parameters, Parameters::kVisBundleAdjustment(), _bundleAdjustment);
114  Parameters::parse(parameters, Parameters::kVisDepthAsMask(), _depthAsMask);
115  uInsert(_bundleParameters, parameters);
116 
117  UASSERT_MSG(_minInliers >= 1, uFormat("value=%d", _minInliers).c_str());
118  UASSERT_MSG(_inlierDistance > 0.0f, uFormat("value=%f", _inlierDistance).c_str());
119  UASSERT_MSG(_iterations > 0, uFormat("value=%d", _iterations).c_str());
120 
121  // override feature parameters
122  for(ParametersMap::const_iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
123  {
124  std::string group = uSplit(iter->first, '/').front();
125  if(Parameters::isFeatureParameter(iter->first) || group.compare("Stereo") == 0)
126  {
127  uInsert(_featureParameters, ParametersPair(iter->first, iter->second));
128  }
129  }
130 
131  if(uContains(parameters, Parameters::kVisCorNNType()))
132  {
133  uInsert(_featureParameters, ParametersPair(Parameters::kKpNNStrategy(), parameters.at(Parameters::kVisCorNNType())));
134  }
135  if(uContains(parameters, Parameters::kVisCorNNDR()))
136  {
137  uInsert(_featureParameters, ParametersPair(Parameters::kKpNndrRatio(), parameters.at(Parameters::kVisCorNNDR())));
138  }
139  if(uContains(parameters, Parameters::kVisFeatureType()))
140  {
141  uInsert(_featureParameters, ParametersPair(Parameters::kKpDetectorStrategy(), parameters.at(Parameters::kVisFeatureType())));
142  }
143  if(uContains(parameters, Parameters::kVisMaxFeatures()))
144  {
145  uInsert(_featureParameters, ParametersPair(Parameters::kKpMaxFeatures(), parameters.at(Parameters::kVisMaxFeatures())));
146  }
147  if(uContains(parameters, Parameters::kVisMaxDepth()))
148  {
149  uInsert(_featureParameters, ParametersPair(Parameters::kKpMaxDepth(), parameters.at(Parameters::kVisMaxDepth())));
150  }
151  if(uContains(parameters, Parameters::kVisMinDepth()))
152  {
153  uInsert(_featureParameters, ParametersPair(Parameters::kKpMinDepth(), parameters.at(Parameters::kVisMinDepth())));
154  }
155  if(uContains(parameters, Parameters::kVisRoiRatios()))
156  {
157  uInsert(_featureParameters, ParametersPair(Parameters::kKpRoiRatios(), parameters.at(Parameters::kVisRoiRatios())));
158  }
159  if(uContains(parameters, Parameters::kVisSubPixEps()))
160  {
161  uInsert(_featureParameters, ParametersPair(Parameters::kKpSubPixEps(), parameters.at(Parameters::kVisSubPixEps())));
162  }
163  if(uContains(parameters, Parameters::kVisSubPixIterations()))
164  {
165  uInsert(_featureParameters, ParametersPair(Parameters::kKpSubPixIterations(), parameters.at(Parameters::kVisSubPixIterations())));
166  }
167  if(uContains(parameters, Parameters::kVisSubPixWinSize()))
168  {
169  uInsert(_featureParameters, ParametersPair(Parameters::kKpSubPixWinSize(), parameters.at(Parameters::kVisSubPixWinSize())));
170  }
171  if(uContains(parameters, Parameters::kVisGridRows()))
172  {
173  uInsert(_featureParameters, ParametersPair(Parameters::kKpGridRows(), parameters.at(Parameters::kVisGridRows())));
174  }
175  if(uContains(parameters, Parameters::kVisGridCols()))
176  {
177  uInsert(_featureParameters, ParametersPair(Parameters::kKpGridCols(), parameters.at(Parameters::kVisGridCols())));
178  }
179 }
180 
181 RegistrationVis::~RegistrationVis()
182 {
183 }
184 
185 Feature2D * RegistrationVis::createFeatureDetector() const
186 {
187  return Feature2D::create(_featureParameters);
188 }
189 
190 Transform RegistrationVis::computeTransformationImpl(
191  Signature & fromSignature,
192  Signature & toSignature,
193  Transform guess, // (flowMaxLevel is set to 0 when guess is used)
194  RegistrationInfo & info) const
195 {
196  UDEBUG("%s=%d", Parameters::kVisMinInliers().c_str(), _minInliers);
197  UDEBUG("%s=%f", Parameters::kVisInlierDistance().c_str(), _inlierDistance);
198  UDEBUG("%s=%d", Parameters::kVisIterations().c_str(), _iterations);
199  UDEBUG("%s=%d", Parameters::kVisEstimationType().c_str(), _estimationType);
200  UDEBUG("%s=%d", Parameters::kVisForwardEstOnly().c_str(), _forwardEstimateOnly);
201  UDEBUG("%s=%f", Parameters::kVisEpipolarGeometryVar().c_str(), _epipolarGeometryVar);
202  UDEBUG("%s=%f", Parameters::kVisPnPReprojError().c_str(), _PnPReprojError);
203  UDEBUG("%s=%d", Parameters::kVisPnPFlags().c_str(), _PnPFlags);
204  UDEBUG("%s=%d", Parameters::kVisCorType().c_str(), _correspondencesApproach);
205  UDEBUG("%s=%d", Parameters::kVisCorFlowWinSize().c_str(), _flowWinSize);
206  UDEBUG("%s=%d", Parameters::kVisCorFlowIterations().c_str(), _flowIterations);
207  UDEBUG("%s=%f", Parameters::kVisCorFlowEps().c_str(), _flowEps);
208  UDEBUG("%s=%d", Parameters::kVisCorFlowMaxLevel().c_str(), _flowMaxLevel);
209  UDEBUG("guess=%s", guess.prettyPrint().c_str());
210 
211  UDEBUG("Input(%d): from=%d words, %d 3D words, %d words descriptors, %d kpts, %d kpts3D, %d descriptors, image=%dx%d",
212  fromSignature.id(),
213  (int)fromSignature.getWords().size(),
214  (int)fromSignature.getWords3().size(),
215  (int)fromSignature.getWordsDescriptors().size(),
216  (int)fromSignature.sensorData().keypoints().size(),
217  (int)fromSignature.sensorData().keypoints3D().size(),
218  fromSignature.sensorData().descriptors().rows,
219  fromSignature.sensorData().imageRaw().cols,
220  fromSignature.sensorData().imageRaw().rows);
221 
222  UDEBUG("Input(%d): to=%d words, %d 3D words, %d words descriptors, %d kpts, %d kpts3D, %d descriptors, image=%dx%d",
223  toSignature.id(),
224  (int)toSignature.getWords().size(),
225  (int)toSignature.getWords3().size(),
226  (int)toSignature.getWordsDescriptors().size(),
227  (int)toSignature.sensorData().keypoints().size(),
228  (int)toSignature.sensorData().keypoints3D().size(),
229  toSignature.sensorData().descriptors().rows,
230  toSignature.sensorData().imageRaw().cols,
231  toSignature.sensorData().imageRaw().rows);
232 
233  std::string msg;
234  info.projectedIDs.clear();
235 
237  // Find correspondences
239  //recompute correspondences if descriptors are provided
240  if((fromSignature.getWordsDescriptors().empty() && toSignature.getWordsDescriptors().empty()) &&
241  (_estimationType<2 || fromSignature.getWords().size()) && // required only for 2D->2D
242  (_estimationType==0 || toSignature.getWords().size()) && // required only for 3D->2D or 2D->2D
243  fromSignature.getWords3().size() && // required in all estimation approaches
244  (_estimationType==1 || toSignature.getWords3().size())) // required only for 3D->3D and 2D->2D
245  {
246  // no need to extract new features, we have all the data we need
247  UDEBUG("");
248  }
249  else
250  {
251  UDEBUG("");
252  // just some checks to make sure that input data are ok
253  UASSERT(fromSignature.getWords().empty() ||
254  fromSignature.getWords3().empty() ||
255  (fromSignature.getWords().size() == fromSignature.getWords3().size()));
256  UASSERT((int)fromSignature.sensorData().keypoints().size() == fromSignature.sensorData().descriptors().rows ||
257  fromSignature.getWords().size() == fromSignature.getWordsDescriptors().size() ||
258  fromSignature.sensorData().descriptors().rows == 0 ||
259  fromSignature.getWordsDescriptors().size() == 0);
260  UASSERT((toSignature.getWords().empty() && toSignature.getWords3().empty())||
261  (toSignature.getWords().size() && toSignature.getWords3().empty())||
262  (toSignature.getWords().size() == toSignature.getWords3().size()));
263  UASSERT((int)toSignature.sensorData().keypoints().size() == toSignature.sensorData().descriptors().rows ||
264  toSignature.getWords().size() == toSignature.getWordsDescriptors().size() ||
265  toSignature.sensorData().descriptors().rows == 0 ||
266  toSignature.getWordsDescriptors().size() == 0);
267  UASSERT(fromSignature.sensorData().imageRaw().empty() ||
268  fromSignature.sensorData().imageRaw().type() == CV_8UC1 ||
269  fromSignature.sensorData().imageRaw().type() == CV_8UC3);
270  UASSERT(toSignature.sensorData().imageRaw().empty() ||
271  toSignature.sensorData().imageRaw().type() == CV_8UC1 ||
272  toSignature.sensorData().imageRaw().type() == CV_8UC3);
273 
274  Feature2D * detector = createFeatureDetector();
275  std::vector<cv::KeyPoint> kptsFrom;
276  cv::Mat imageFrom = fromSignature.sensorData().imageRaw();
277  cv::Mat imageTo = toSignature.sensorData().imageRaw();
278 
279  std::vector<int> orignalWordsFromIds;
280  if(fromSignature.getWords().empty())
281  {
282  if(fromSignature.sensorData().keypoints().empty())
283  {
284  if(!imageFrom.empty())
285  {
286  if(imageFrom.channels() > 1)
287  {
288  cv::Mat tmp;
289  cv::cvtColor(imageFrom, tmp, cv::COLOR_BGR2GRAY);
290  imageFrom = tmp;
291  }
292 
293  cv::Mat depthMask;
294  if(!fromSignature.sensorData().depthRaw().empty() && _depthAsMask)
295  {
296  if(imageFrom.rows % fromSignature.sensorData().depthRaw().rows == 0 &&
297  imageFrom.cols % fromSignature.sensorData().depthRaw().cols == 0 &&
298  imageFrom.rows/fromSignature.sensorData().depthRaw().rows == fromSignature.sensorData().imageRaw().cols/fromSignature.sensorData().depthRaw().cols)
299  {
300  depthMask = util2d::interpolate(fromSignature.sensorData().depthRaw(), fromSignature.sensorData().imageRaw().rows/fromSignature.sensorData().depthRaw().rows, 0.1f);
301  }
302  }
303 
304  kptsFrom = detector->generateKeypoints(
305  imageFrom,
306  depthMask);
307  }
308  }
309  else
310  {
311  kptsFrom = fromSignature.sensorData().keypoints();
312  }
313  }
314  else
315  {
316  kptsFrom.resize(fromSignature.getWords().size());
317  orignalWordsFromIds.resize(fromSignature.getWords().size());
318  int i=0;
319  bool allUniques = true;
320  for(std::multimap<int, cv::KeyPoint>::const_iterator iter=fromSignature.getWords().begin(); iter!=fromSignature.getWords().end(); ++iter)
321  {
322  kptsFrom[i] = iter->second;
323  orignalWordsFromIds[i] = iter->first;
324  if(i>0 && iter->first==orignalWordsFromIds[i-1])
325  {
326  allUniques = false;
327  }
328  ++i;
329  }
330  if(!allUniques)
331  {
332  UDEBUG("IDs are not unique, IDs will be regenerated!");
333  orignalWordsFromIds.clear();
334  }
335  }
336 
337  std::multimap<int, cv::KeyPoint> wordsFrom;
338  std::multimap<int, cv::KeyPoint> wordsTo;
339  std::multimap<int, cv::Point3f> words3From;
340  std::multimap<int, cv::Point3f> words3To;
341  std::multimap<int, cv::Mat> wordsDescFrom;
342  std::multimap<int, cv::Mat> wordsDescTo;
343  if(_correspondencesApproach == 1) //Optical Flow
344  {
345  UDEBUG("");
346  // convert to grayscale
347  if(imageFrom.channels() > 1)
348  {
349  cv::Mat tmp;
350  cv::cvtColor(imageFrom, tmp, cv::COLOR_BGR2GRAY);
351  imageFrom = tmp;
352  }
353  if(imageTo.channels() > 1)
354  {
355  cv::Mat tmp;
356  cv::cvtColor(imageTo, tmp, cv::COLOR_BGR2GRAY);
357  imageTo = tmp;
358  }
359 
360  std::vector<cv::Point3f> kptsFrom3D;
361  if(kptsFrom.size() == fromSignature.getWords3().size())
362  {
363  kptsFrom3D = uValues(fromSignature.getWords3());
364  }
365  else if(kptsFrom.size() == fromSignature.sensorData().keypoints3D().size())
366  {
367  kptsFrom3D = fromSignature.sensorData().keypoints3D();
368  }
369  else
370  {
371  kptsFrom3D = detector->generateKeypoints3D(fromSignature.sensorData(), kptsFrom);
372  }
373 
374  if(!imageFrom.empty() && !imageTo.empty())
375  {
376  std::vector<cv::Point2f> cornersFrom;
377  cv::KeyPoint::convert(kptsFrom, cornersFrom);
378  std::vector<cv::Point2f> cornersTo;
379  bool guessSet = !guess.isIdentity() && !guess.isNull();
380  if(guessSet)
381  {
382  Transform localTransform = fromSignature.sensorData().cameraModels().size()?fromSignature.sensorData().cameraModels()[0].localTransform():fromSignature.sensorData().stereoCameraModel().left().localTransform();
383  Transform guessCameraRef = (guess * localTransform).inverse();
384  cv::Mat R = (cv::Mat_<double>(3,3) <<
385  (double)guessCameraRef.r11(), (double)guessCameraRef.r12(), (double)guessCameraRef.r13(),
386  (double)guessCameraRef.r21(), (double)guessCameraRef.r22(), (double)guessCameraRef.r23(),
387  (double)guessCameraRef.r31(), (double)guessCameraRef.r32(), (double)guessCameraRef.r33());
388  cv::Mat rvec(1,3, CV_64FC1);
389  cv::Rodrigues(R, rvec);
390  cv::Mat tvec = (cv::Mat_<double>(1,3) << (double)guessCameraRef.x(), (double)guessCameraRef.y(), (double)guessCameraRef.z());
391  cv::Mat K = fromSignature.sensorData().cameraModels().size()?fromSignature.sensorData().cameraModels()[0].K():fromSignature.sensorData().stereoCameraModel().left().K();
392  cv::projectPoints(kptsFrom3D, rvec, tvec, K, cv::Mat(), cornersTo);
393  }
394 
395  // Find features in the new left image
396  UDEBUG("guessSet = %d", guessSet?1:0);
397  std::vector<unsigned char> status;
398  std::vector<float> err;
399  UDEBUG("cv::calcOpticalFlowPyrLK() begin");
400  cv::calcOpticalFlowPyrLK(
401  imageFrom,
402  imageTo,
403  cornersFrom,
404  cornersTo,
405  status,
406  err,
407  cv::Size(_flowWinSize, _flowWinSize),
408  guessSet?0:_flowMaxLevel,
409  cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, _flowIterations, _flowEps),
410  cv::OPTFLOW_LK_GET_MIN_EIGENVALS | (guessSet?cv::OPTFLOW_USE_INITIAL_FLOW:0), 1e-4);
411  UDEBUG("cv::calcOpticalFlowPyrLK() end");
412 
413  UASSERT(kptsFrom.size() == kptsFrom3D.size());
414  std::vector<cv::KeyPoint> kptsTo(kptsFrom.size());
415  std::vector<cv::Point3f> kptsFrom3DKept(kptsFrom3D.size());
416  std::vector<int> orignalWordsFromIdsCpy = orignalWordsFromIds;
417  int ki = 0;
418  for(unsigned int i=0; i<status.size(); ++i)
419  {
420  if(status[i] &&
421  uIsInBounds(cornersTo[i].x, 0.0f, float(imageTo.cols)) &&
422  uIsInBounds(cornersTo[i].y, 0.0f, float(imageTo.rows)))
423  {
424  if(orignalWordsFromIdsCpy.size())
425  {
426  orignalWordsFromIds[ki] = orignalWordsFromIdsCpy[i];
427  }
428  kptsFrom[ki] = cv::KeyPoint(cornersFrom[i], 1);
429  kptsFrom3DKept[ki] = kptsFrom3D[i];
430  kptsTo[ki++] = cv::KeyPoint(cornersTo[i], 1);
431  }
432  }
433  if(orignalWordsFromIds.size())
434  {
435  orignalWordsFromIds.resize(ki);
436  }
437  kptsFrom.resize(ki);
438  kptsTo.resize(ki);
439  kptsFrom3DKept.resize(ki);
440  kptsFrom3D = kptsFrom3DKept;
441 
442  std::vector<cv::Point3f> kptsTo3D;
443  if(_estimationType == 0 || _estimationType == 1 || !_forwardEstimateOnly)
444  {
445  kptsTo3D = detector->generateKeypoints3D(toSignature.sensorData(), kptsTo);
446  }
447 
448  UASSERT(kptsFrom.size() == kptsFrom3DKept.size());
449  UASSERT(kptsFrom.size() == kptsTo.size());
450  UASSERT(kptsTo3D.size() == 0 || kptsTo.size() == kptsTo3D.size());
451  for(unsigned int i=0; i< kptsFrom3DKept.size(); ++i)
452  {
453  int id = orignalWordsFromIds.size()?orignalWordsFromIds[i]:i;
454  wordsFrom.insert(std::make_pair(id, kptsFrom[i]));
455  words3From.insert(std::make_pair(id, kptsFrom3DKept[i]));
456  wordsTo.insert(std::make_pair(id, kptsTo[i]));
457  if(kptsTo3D.size())
458  {
459  words3To.insert(std::make_pair(id, kptsTo3D[i]));
460  }
461  }
462  toSignature.sensorData().setFeatures(kptsTo, kptsTo3D, cv::Mat());
463  }
464  else
465  {
466  if(imageFrom.empty())
467  {
468  UERROR("Optical flow correspondences requires images in data!");
469  }
470  UASSERT(kptsFrom.size() == kptsFrom3D.size());
471  for(unsigned int i=0; i< kptsFrom3D.size(); ++i)
472  {
473  if(util3d::isFinite(kptsFrom3D[i]))
474  {
475  int id = orignalWordsFromIds.size()?orignalWordsFromIds[i]:i;
476  wordsFrom.insert(std::make_pair(id, kptsFrom[i]));
477  words3From.insert(std::make_pair(id, kptsFrom3D[i]));
478  }
479  }
480  toSignature.sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
481  }
482 
483  fromSignature.sensorData().setFeatures(kptsFrom, kptsFrom3D, cv::Mat());
484  }
485  else // Features Matching
486  {
487  UDEBUG("");
488  std::vector<cv::KeyPoint> kptsTo;
489  if(toSignature.getWords().empty())
490  {
491  if(toSignature.sensorData().keypoints().empty() &&
492  !imageTo.empty())
493  {
494  if(imageTo.channels() > 1)
495  {
496  cv::Mat tmp;
497  cv::cvtColor(imageTo, tmp, cv::COLOR_BGR2GRAY);
498  imageTo = tmp;
499  }
500 
501  cv::Mat depthMask;
502  if(!toSignature.sensorData().depthRaw().empty() && _depthAsMask)
503  {
504  if(imageTo.rows % toSignature.sensorData().depthRaw().rows == 0 &&
505  imageTo.cols % toSignature.sensorData().depthRaw().cols == 0 &&
506  imageTo.rows/toSignature.sensorData().depthRaw().rows == imageTo.cols/toSignature.sensorData().depthRaw().cols)
507  {
508  depthMask = util2d::interpolate(toSignature.sensorData().depthRaw(), imageTo.rows/toSignature.sensorData().depthRaw().rows, 0.1f);
509  }
510  }
511 
512  kptsTo = detector->generateKeypoints(
513  imageTo,
514  depthMask);
515  }
516  else
517  {
518  kptsTo = toSignature.sensorData().keypoints();
519  }
520  }
521  else
522  {
523  kptsTo = uValues(toSignature.getWords());
524  }
525 
526  // extract descriptors
527  UDEBUG("kptsFrom=%d", (int)kptsFrom.size());
528  UDEBUG("kptsTo=%d", (int)kptsTo.size());
529  cv::Mat descriptorsFrom;
530  if(fromSignature.getWordsDescriptors().size() &&
531  ((kptsFrom.empty() && fromSignature.getWordsDescriptors().size()) ||
532  fromSignature.getWordsDescriptors().size() == kptsFrom.size()))
533  {
534  descriptorsFrom = cv::Mat(fromSignature.getWordsDescriptors().size(),
535  fromSignature.getWordsDescriptors().begin()->second.cols,
536  fromSignature.getWordsDescriptors().begin()->second.type());
537  int i=0;
538  for(std::multimap<int, cv::Mat>::const_iterator iter=fromSignature.getWordsDescriptors().begin();
539  iter!=fromSignature.getWordsDescriptors().end();
540  ++iter, ++i)
541  {
542  iter->second.copyTo(descriptorsFrom.row(i));
543  }
544  }
545  else if(fromSignature.sensorData().descriptors().rows == (int)kptsFrom.size())
546  {
547  descriptorsFrom = fromSignature.sensorData().descriptors();
548  }
549  else if(!imageFrom.empty())
550  {
551  if(imageFrom.channels() > 1)
552  {
553  cv::Mat tmp;
554  cv::cvtColor(imageFrom, tmp, cv::COLOR_BGR2GRAY);
555  imageFrom = tmp;
556  }
557  orignalWordsFromIds.clear();
558  descriptorsFrom = detector->generateDescriptors(imageFrom, kptsFrom);
559  }
560 
561  cv::Mat descriptorsTo;
562  if(kptsTo.size())
563  {
564  if(toSignature.getWordsDescriptors().size() == kptsTo.size())
565  {
566  descriptorsTo = cv::Mat(toSignature.getWordsDescriptors().size(),
567  toSignature.getWordsDescriptors().begin()->second.cols,
568  toSignature.getWordsDescriptors().begin()->second.type());
569  int i=0;
570  for(std::multimap<int, cv::Mat>::const_iterator iter=toSignature.getWordsDescriptors().begin();
571  iter!=toSignature.getWordsDescriptors().end();
572  ++iter, ++i)
573  {
574  iter->second.copyTo(descriptorsTo.row(i));
575  }
576  }
577  else if(toSignature.sensorData().descriptors().rows == (int)kptsTo.size())
578  {
579  descriptorsTo = toSignature.sensorData().descriptors();
580  }
581  else if(!imageTo.empty())
582  {
583  if(imageTo.channels() > 1)
584  {
585  cv::Mat tmp;
586  cv::cvtColor(imageTo, tmp, cv::COLOR_BGR2GRAY);
587  imageTo = tmp;
588  }
589 
590  descriptorsTo = detector->generateDescriptors(imageTo, kptsTo);
591  }
592  }
593 
594  // create 3D keypoints
595  std::vector<cv::Point3f> kptsFrom3D;
596  std::vector<cv::Point3f> kptsTo3D;
597  if(kptsFrom.size() == fromSignature.getWords3().size())
598  {
599  kptsFrom3D = uValues(fromSignature.getWords3());
600  }
601  else if(kptsFrom.size() == fromSignature.sensorData().keypoints3D().size())
602  {
603  kptsFrom3D = fromSignature.sensorData().keypoints3D();
604  }
605  else
606  {
607  if(fromSignature.getWords3().size() && kptsFrom.size() != fromSignature.getWords3().size())
608  {
609  UWARN("kptsFrom (%d) is not the same size as fromSignature.getWords3() (%d), there "
610  "is maybe a problem with the logic above (getWords3() should be null or equal to kptsfrom). Regenerating kptsFrom3D...",
611  kptsFrom.size(),
612  fromSignature.getWords3().size());
613  }
614  else if(fromSignature.sensorData().keypoints3D().size() && kptsFrom.size() != fromSignature.sensorData().keypoints3D().size())
615  {
616  UWARN("kptsFrom (%d) is not the same size as fromSignature.sensorData().keypoints3D() (%d), there "
617  "is maybe a problem with the logic above (keypoints3D should be null or equal to kptsfrom). Regenerating kptsFrom3D...",
618  kptsFrom.size(),
619  fromSignature.sensorData().keypoints3D().size());
620  }
621  kptsFrom3D = detector->generateKeypoints3D(fromSignature.sensorData(), kptsFrom);
622  UDEBUG("generated kptsFrom3D=%d", (int)kptsFrom3D.size());
623  if(detector->getMinDepth() > 0.0f || detector->getMaxDepth() > 0.0f)
624  {
625  //remove all keypoints/descriptors with no valid 3D points
626  UASSERT((int)kptsFrom.size() == descriptorsFrom.rows &&
627  kptsFrom3D.size() == kptsFrom.size());
628  std::vector<cv::KeyPoint> validKeypoints(kptsFrom.size());
629  std::vector<cv::Point3f> validKeypoints3D(kptsFrom.size());
630  cv::Mat validDescriptors(descriptorsFrom.size(), descriptorsFrom.type());
631  std::vector<int> validKeypointsIds;
632  if(orignalWordsFromIds.size())
633  {
634  validKeypointsIds.resize(kptsFrom.size());
635  }
636 
637  int oi=0;
638  for(unsigned int i=0; i<kptsFrom3D.size(); ++i)
639  {
640  if(util3d::isFinite(kptsFrom3D[i]))
641  {
642  validKeypoints[oi] = kptsFrom[i];
643  validKeypoints3D[oi] = kptsFrom3D[i];
644  if(orignalWordsFromIds.size())
645  {
646  validKeypointsIds[oi] = orignalWordsFromIds[i];
647  }
648  descriptorsFrom.row(i).copyTo(validDescriptors.row(oi));
649  ++oi;
650  }
651  }
652  UDEBUG("Removed %d invalid 3D points", (int)kptsFrom3D.size()-oi);
653  validKeypoints.resize(oi);
654  validKeypoints3D.resize(oi);
655  kptsFrom = validKeypoints;
656  kptsFrom3D = validKeypoints3D;
657 
658  if(orignalWordsFromIds.size())
659  {
660  validKeypointsIds.resize(oi);
661  orignalWordsFromIds = validKeypointsIds;
662  }
663  descriptorsFrom = validDescriptors.rowRange(0, oi).clone();
664  }
665  }
666 
667  if(kptsTo.size() == toSignature.getWords3().size())
668  {
669  kptsTo3D = uValues(toSignature.getWords3());
670  }
671  else if(kptsTo.size() == toSignature.sensorData().keypoints3D().size())
672  {
673  kptsTo3D = toSignature.sensorData().keypoints3D();
674  }
675  else
676  {
677  if(toSignature.getWords3().size() && kptsTo.size() != toSignature.getWords3().size())
678  {
679  UWARN("kptsTo (%d) is not the same size as toSignature.getWords3() (%d), there "
680  "is maybe a problem with the logic above (getWords3() should be null or equal to kptsTo). Regenerating kptsTo3D...",
681  (int)kptsTo.size(),
682  (int)toSignature.getWords3().size());
683  }
684  else if(toSignature.sensorData().keypoints3D().size() && kptsTo.size() != toSignature.sensorData().keypoints3D().size())
685  {
686  UWARN("kptsTo (%d) is not the same size as toSignature.sensorData().keypoints3D() (%d), there "
687  "is maybe a problem with the logic above (keypoints3D() should be null or equal to kptsTo). Regenerating kptsTo3D...",
688  (int)kptsTo.size(),
689  (int)toSignature.sensorData().keypoints3D().size());
690  }
691  kptsTo3D = detector->generateKeypoints3D(toSignature.sensorData(), kptsTo);
692  if(kptsTo3D.size() && (detector->getMinDepth() > 0.0f || detector->getMaxDepth() > 0.0f))
693  {
694  UDEBUG("");
695  //remove all keypoints/descriptors with no valid 3D points
696  UASSERT((int)kptsTo.size() == descriptorsTo.rows &&
697  kptsTo3D.size() == kptsTo.size());
698  std::vector<cv::KeyPoint> validKeypoints(kptsTo.size());
699  std::vector<cv::Point3f> validKeypoints3D(kptsTo.size());
700  cv::Mat validDescriptors(descriptorsTo.size(), descriptorsTo.type());
701 
702  int oi=0;
703  for(unsigned int i=0; i<kptsTo3D.size(); ++i)
704  {
705  if(util3d::isFinite(kptsTo3D[i]))
706  {
707  validKeypoints[oi] = kptsTo[i];
708  validKeypoints3D[oi] = kptsTo3D[i];
709  descriptorsTo.row(i).copyTo(validDescriptors.row(oi));
710  ++oi;
711  }
712  }
713  UDEBUG("Removed %d invalid 3D points", (int)kptsTo3D.size()-oi);
714  validKeypoints.resize(oi);
715  validKeypoints3D.resize(oi);
716  kptsTo = validKeypoints;
717  kptsTo3D = validKeypoints3D;
718  descriptorsTo = validDescriptors.rowRange(0, oi).clone();
719  }
720  }
721 
722  UASSERT(kptsFrom.empty() || descriptorsFrom.rows == 0 || int(kptsFrom.size()) == descriptorsFrom.rows);
723 
724  fromSignature.sensorData().setFeatures(kptsFrom, kptsFrom3D, descriptorsFrom);
725  toSignature.sensorData().setFeatures(kptsTo, kptsTo3D, descriptorsTo);
726 
727  UDEBUG("descriptorsFrom=%d", descriptorsFrom.rows);
728  UDEBUG("descriptorsTo=%d", descriptorsTo.rows);
729 
730  // We have all data we need here, so match!
731  if(descriptorsFrom.rows > 0 && descriptorsTo.rows > 0)
732  {
733  cv::Size imageSize = imageTo.size();
734  bool isCalibrated = false; // multiple cameras not supported.
735  if(imageSize.height == 0 || imageSize.width == 0)
736  {
737  imageSize = toSignature.sensorData().cameraModels().size() == 1?toSignature.sensorData().cameraModels()[0].imageSize():toSignature.sensorData().stereoCameraModel().left().imageSize();
738  }
739 
740  isCalibrated = imageSize.height != 0 && imageSize.width != 0 &&
741  (toSignature.sensorData().cameraModels().size()==1?toSignature.sensorData().cameraModels()[0].isValidForProjection():toSignature.sensorData().stereoCameraModel().isValidForProjection());
742 
743  // If guess is set, limit the search of matches using optical flow window size
744  bool guessSet = !guess.isIdentity() && !guess.isNull();
745  if(guessSet && _guessWinSize > 0 && kptsFrom3D.size() &&
746  isCalibrated) // needed for projection
747  {
748  UDEBUG("");
749  UASSERT((int)kptsTo.size() == descriptorsTo.rows);
750  UASSERT((int)kptsFrom3D.size() == descriptorsFrom.rows);
751 
752  // Use guess to project 3D "from" keypoints into "to" image
753  if(toSignature.sensorData().cameraModels().size() > 1)
754  {
755  UFATAL("Guess reprojection feature matching is not supported for multiple cameras.");
756  }
757 
758  Transform localTransform = toSignature.sensorData().cameraModels().size()?toSignature.sensorData().cameraModels()[0].localTransform():toSignature.sensorData().stereoCameraModel().left().localTransform();
759  Transform guessCameraRef = (guess * localTransform).inverse();
760  cv::Mat R = (cv::Mat_<double>(3,3) <<
761  (double)guessCameraRef.r11(), (double)guessCameraRef.r12(), (double)guessCameraRef.r13(),
762  (double)guessCameraRef.r21(), (double)guessCameraRef.r22(), (double)guessCameraRef.r23(),
763  (double)guessCameraRef.r31(), (double)guessCameraRef.r32(), (double)guessCameraRef.r33());
764  cv::Mat rvec(1,3, CV_64FC1);
765  cv::Rodrigues(R, rvec);
766  cv::Mat tvec = (cv::Mat_<double>(1,3) << (double)guessCameraRef.x(), (double)guessCameraRef.y(), (double)guessCameraRef.z());
767  cv::Mat K = toSignature.sensorData().cameraModels().size()?toSignature.sensorData().cameraModels()[0].K():toSignature.sensorData().stereoCameraModel().left().K();
768  std::vector<cv::Point2f> projected;
769  cv::projectPoints(kptsFrom3D, rvec, tvec, K, cv::Mat(), projected);
770  UDEBUG("Projected points=%d", (int)projected.size());
771  //remove projected points outside of the image
772  UASSERT((int)projected.size() == descriptorsFrom.rows);
773  std::vector<cv::Point2f> cornersProjected(projected.size());
774  std::vector<int> projectedIndexToDescIndex(projected.size());
775  int oi=0;
776  for(unsigned int i=0; i<projected.size(); ++i)
777  {
778  if(uIsInBounds(projected[i].x, 0.0f, float(imageSize.width-1)) &&
779  uIsInBounds(projected[i].y, 0.0f, float(imageSize.height-1)) &&
780  util3d::transformPoint(kptsFrom3D[i], guessCameraRef).z > 0.0)
781  {
782  projectedIndexToDescIndex[oi] = i;
783  cornersProjected[oi++] = projected[i];
784  }
785  }
786  projectedIndexToDescIndex.resize(oi);
787  cornersProjected.resize(oi);
788  UDEBUG("corners in frame=%d", (int)cornersProjected.size());
789 
790  // For each projected feature guess of "from" in "to", find its matching feature in
791  // the radius around the projected guess.
792  // TODO: do cross-check?
793  if(cornersProjected.size())
794  {
795  if(_guessMatchToProjection)
796  {
797  // match frame to projected
798  // Create kd-tree for projected keypoints
799  rtflann::Matrix<float> cornersProjectedMat((float*)cornersProjected.data(), cornersProjected.size(), 2);
800  rtflann::Index<rtflann::L2_Simple<float> > index(cornersProjectedMat, rtflann::KDTreeIndexParams());
801  index.buildIndex();
802 
803  std::vector< std::vector<size_t> > indices;
804  std::vector<std::vector<float> > dists;
805  float radius = (float)_guessWinSize; // pixels
806  std::vector<cv::Point2f> pointsTo;
807  cv::KeyPoint::convert(kptsTo, pointsTo);
808  rtflann::Matrix<float> pointsToMat((float*)pointsTo.data(), pointsTo.size(), 2);
809  index.radiusSearch(pointsToMat, indices, dists, radius*radius, rtflann::SearchParams());
810 
811  UASSERT(indices.size() == pointsToMat.rows);
812  UASSERT(descriptorsFrom.cols == descriptorsTo.cols);
813  UASSERT(descriptorsFrom.rows == (int)kptsFrom.size());
814  UASSERT((int)pointsToMat.rows == descriptorsTo.rows);
815  UASSERT(pointsToMat.rows == kptsTo.size());
816  UDEBUG("radius search done for guess");
817 
818  // Process results (Nearest Neighbor Distance Ratio)
819  int newToId = orignalWordsFromIds.size()?orignalWordsFromIds.back():descriptorsFrom.rows;
820  std::map<int,int> addedWordsFrom; //<id, index>
821  std::map<int, int> duplicates; //<fromId, toId>
822  int newWords = 0;
823  cv::Mat descriptors(10, descriptorsTo.cols, descriptorsTo.type());
824  for(unsigned int i = 0; i < pointsToMat.rows; ++i)
825  {
826  if(kptsTo3D.empty() || util3d::isFinite(kptsTo3D[i]))
827  {
828  int octave = kptsTo[i].octave;
829  int matchedIndex = -1;
830  if(indices[i].size() >= 2)
831  {
832  std::vector<int> descriptorsIndices(indices[i].size());
833  int oi=0;
834  if((int)indices[i].size() > descriptors.rows)
835  {
836  descriptors.resize(indices[i].size());
837  }
838  for(unsigned int j=0; j<indices[i].size(); ++j)
839  {
840  if(kptsFrom.at(projectedIndexToDescIndex[indices[i].at(j)]).octave==octave)
841  {
842  descriptorsFrom.row(projectedIndexToDescIndex[indices[i].at(j)]).copyTo(descriptors.row(oi));
843  descriptorsIndices[oi++] = indices[i].at(j);
844  }
845  }
846  descriptorsIndices.resize(oi);
847  if(oi >=2)
848  {
849  std::vector<std::vector<cv::DMatch> > matches;
850  cv::BFMatcher matcher(descriptors.type()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR);
851  matcher.knnMatch(descriptorsTo.row(i), cv::Mat(descriptors, cv::Range(0, oi)), matches, 2);
852  UASSERT(matches.size() == 1);
853  UASSERT(matches[0].size() == 2);
854  if(matches[0].at(0).distance < _nndr * matches[0].at(1).distance)
855  {
856  matchedIndex = descriptorsIndices.at(matches[0].at(0).trainIdx);
857  }
858  }
859  else if(oi == 1)
860  {
861  matchedIndex = descriptorsIndices[0];
862  }
863  }
864  else if(indices[i].size() == 1 &&
865  kptsFrom.at(projectedIndexToDescIndex[indices[i].at(0)]).octave == octave)
866  {
867  matchedIndex = indices[i].at(0);
868  }
869 
870  if(matchedIndex >= 0)
871  {
872  matchedIndex = projectedIndexToDescIndex[matchedIndex];
873  int id = orignalWordsFromIds.size()?orignalWordsFromIds[matchedIndex]:matchedIndex;
874 
875  if(addedWordsFrom.find(matchedIndex) != addedWordsFrom.end())
876  {
877  id = addedWordsFrom.at(matchedIndex);
878  duplicates.insert(std::make_pair(matchedIndex, id));
879  }
880  else
881  {
882  addedWordsFrom.insert(std::make_pair(matchedIndex, id));
883 
884  if(kptsFrom.size())
885  {
886  wordsFrom.insert(std::make_pair(id, kptsFrom[matchedIndex]));
887  }
888  words3From.insert(std::make_pair(id, kptsFrom3D[matchedIndex]));
889  wordsDescFrom.insert(std::make_pair(id, descriptorsFrom.row(matchedIndex)));
890  }
891 
892  wordsTo.insert(std::make_pair(id, kptsTo[i]));
893  wordsDescTo.insert(std::make_pair(id, descriptorsTo.row(i)));
894  if(kptsTo3D.size())
895  {
896  words3To.insert(std::make_pair(id, kptsTo3D[i]));
897  }
898  }
899  else
900  {
901  // gen fake ids
902  wordsTo.insert(wordsTo.end(), std::make_pair(newToId, kptsTo[i]));
903  wordsDescTo.insert(wordsDescTo.end(), std::make_pair(newToId, descriptorsTo.row(i)));
904  if(kptsTo3D.size())
905  {
906  words3To.insert(words3To.end(), std::make_pair(newToId, kptsTo3D[i]));
907  }
908 
909  ++newToId;
910  ++newWords;
911  }
912  }
913  }
914  UDEBUG("addedWordsFrom=%d/%d (duplicates=%d, newWords=%d), kptsTo=%d, wordsTo=%d, words3From=%d",
915  (int)addedWordsFrom.size(), (int)cornersProjected.size(), (int)duplicates.size(), newWords,
916  (int)kptsTo.size(), (int)wordsTo.size(), (int)words3From.size());
917 
918  // create fake ids for not matched words from "from"
919  int addWordsFromNotMatched = 0;
920  for(unsigned int i=0; i<kptsFrom3D.size(); ++i)
921  {
922  if(util3d::isFinite(kptsFrom3D[i]) && addedWordsFrom.find(i) == addedWordsFrom.end())
923  {
924  int id = orignalWordsFromIds.size()?orignalWordsFromIds[i]:i;
925  wordsFrom.insert(wordsFrom.end(), std::make_pair(id, kptsFrom[i]));
926  wordsDescFrom.insert(wordsDescFrom.end(), std::make_pair(id, descriptorsFrom.row(i)));
927  words3From.insert(words3From.end(), std::make_pair(id, kptsFrom3D[i]));
928 
929  ++addWordsFromNotMatched;
930  }
931  }
932  UDEBUG("addWordsFromNotMatched=%d -> words3From=%d", addWordsFromNotMatched, (int)words3From.size());
933  }
934  else
935  {
936  // match projected to frame
937  std::vector<cv::Point2f> pointsTo;
938  cv::KeyPoint::convert(kptsTo, pointsTo);
939  rtflann::Matrix<float> pointsToMat((float*)pointsTo.data(), pointsTo.size(), 2);
940  rtflann::Index<rtflann::L2_Simple<float> > index(pointsToMat, rtflann::KDTreeIndexParams());
941  index.buildIndex();
942 
943  std::vector< std::vector<size_t> > indices;
944  std::vector<std::vector<float> > dists;
945  float radius = (float)_guessWinSize; // pixels
946  rtflann::Matrix<float> cornersProjectedMat((float*)cornersProjected.data(), cornersProjected.size(), 2);
947  index.radiusSearch(cornersProjectedMat, indices, dists, radius*radius, rtflann::SearchParams());
948 
949  UASSERT(indices.size() == cornersProjectedMat.rows);
950  UASSERT(descriptorsFrom.cols == descriptorsTo.cols);
951  UASSERT(descriptorsFrom.rows == (int)kptsFrom.size());
952  UASSERT((int)pointsToMat.rows == descriptorsTo.rows);
953  UASSERT(pointsToMat.rows == kptsTo.size());
954  UDEBUG("radius search done for guess");
955 
956  // Process results (Nearest Neighbor Distance Ratio)
957  std::set<int> addedWordsTo;
958  std::set<int> addedWordsFrom;
959  std::set<int> indicesToIgnore;
960  double bruteForceTotalTime = 0.0;
961  double bruteForceDescCopy = 0.0;
962  UTimer bruteForceTimer;
963  cv::Mat descriptors(10, descriptorsTo.cols, descriptorsTo.type());
964  for(unsigned int i = 0; i < cornersProjectedMat.rows; ++i)
965  {
966  int matchedIndexFrom = projectedIndexToDescIndex[i];
967 
968  if(indices[i].size())
969  {
970  info.projectedIDs.push_back(orignalWordsFromIds.size()?orignalWordsFromIds[matchedIndexFrom]:matchedIndexFrom);
971  }
972 
973  if(util3d::isFinite(kptsFrom3D[matchedIndexFrom]))
974  {
975  int matchedIndexTo = -1;
976  if(indices[i].size() >= 2)
977  {
978  bruteForceTimer.restart();
979  std::vector<int> descriptorsIndices(indices[i].size());
980  int oi=0;
981  if((int)indices[i].size() > descriptors.rows)
982  {
983  descriptors.resize(indices[i].size());
984  }
985  std::list<int> indicesToIgnoretmp;
986  for(unsigned int j=0; j<indices[i].size(); ++j)
987  {
988  int octave = kptsTo[indices[i].at(j)].octave;
989  if(kptsFrom.at(matchedIndexFrom).octave==octave)
990  {
991  descriptorsTo.row(indices[i].at(j)).copyTo(descriptors.row(oi));
992  descriptorsIndices[oi++] = indices[i].at(j);
993 
994  indicesToIgnoretmp.push_back(indices[i].at(j));
995  }
996  }
997  bruteForceDescCopy += bruteForceTimer.ticks();
998  if(oi >=2)
999  {
1000  std::vector<std::vector<cv::DMatch> > matches;
1001  cv::BFMatcher matcher(descriptors.type()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR);
1002  matcher.knnMatch(descriptorsFrom.row(matchedIndexFrom), cv::Mat(descriptors, cv::Range(0, oi)), matches, 2);
1003  UASSERT(matches.size() == 1);
1004  UASSERT(matches[0].size() == 2);
1005  bruteForceTotalTime+=bruteForceTimer.elapsed();
1006  if(matches[0].at(0).distance < _nndr * matches[0].at(1).distance)
1007  {
1008  matchedIndexTo = descriptorsIndices.at(matches[0].at(0).trainIdx);
1009 
1010  indicesToIgnore.insert(indicesToIgnore.begin(), indicesToIgnore.end());
1011  }
1012  }
1013  else if(oi == 1)
1014  {
1015  matchedIndexTo = descriptorsIndices[0];
1016  }
1017  }
1018  else if(indices[i].size() == 1)
1019  {
1020  int octave = kptsTo[indices[i].at(0)].octave;
1021  if(kptsFrom.at(matchedIndexFrom).octave == octave)
1022  {
1023  matchedIndexTo = indices[i].at(0);
1024  }
1025  }
1026 
1027  int id = orignalWordsFromIds.size()?orignalWordsFromIds[matchedIndexFrom]:matchedIndexFrom;
1028  addedWordsFrom.insert(addedWordsFrom.end(), matchedIndexFrom);
1029 
1030  if(kptsFrom.size())
1031  {
1032  wordsFrom.insert(wordsFrom.end(), std::make_pair(id, kptsFrom[matchedIndexFrom]));
1033  }
1034  words3From.insert(words3From.end(), std::make_pair(id, kptsFrom3D[matchedIndexFrom]));
1035  wordsDescFrom.insert(wordsDescFrom.end(), std::make_pair(id, descriptorsFrom.row(matchedIndexFrom)));
1036 
1037  if((kptsTo3D.empty() || util3d::isFinite(kptsTo3D[matchedIndexTo])) &&
1038  matchedIndexTo >= 0 &&
1039  addedWordsTo.find(matchedIndexTo) == addedWordsTo.end())
1040  {
1041  addedWordsTo.insert(matchedIndexTo);
1042 
1043  wordsTo.insert(wordsTo.end(), std::make_pair(id, kptsTo[matchedIndexTo]));
1044  wordsDescTo.insert(wordsDescTo.end(), std::make_pair(id, descriptorsTo.row(matchedIndexTo)));
1045  if(kptsTo3D.size())
1046  {
1047  words3To.insert(words3To.end(), std::make_pair(id, kptsTo3D[matchedIndexTo]));
1048  }
1049  }
1050  }
1051  }
1052  UDEBUG("bruteForceDescCopy=%fs, bruteForceTotalTime=%fs", bruteForceDescCopy, bruteForceTotalTime);
1053 
1054  // create fake ids for not matched words from "from"
1055  for(unsigned int i=0; i<kptsFrom3D.size(); ++i)
1056  {
1057  if(util3d::isFinite(kptsFrom3D[i]) && addedWordsFrom.find(i) == addedWordsFrom.end())
1058  {
1059  int id = orignalWordsFromIds.size()?orignalWordsFromIds[i]:i;
1060  wordsFrom.insert(wordsFrom.end(), std::make_pair(id, kptsFrom[i]));
1061  wordsDescFrom.insert(wordsDescFrom.end(), std::make_pair(id, descriptorsFrom.row(i)));
1062  words3From.insert(words3From.end(), std::make_pair(id, kptsFrom3D[i]));
1063  }
1064  }
1065 
1066  int newToId = orignalWordsFromIds.size()?orignalWordsFromIds.back():descriptorsFrom.rows;
1067  for(unsigned int i = 0; i < kptsTo.size(); ++i)
1068  {
1069  if(addedWordsTo.find(i) == addedWordsTo.end() && indicesToIgnore.find(i) == indicesToIgnore.end())
1070  {
1071  wordsTo.insert(wordsTo.end(), std::make_pair(newToId, kptsTo[i]));
1072  wordsDescTo.insert(wordsDescTo.end(), std::make_pair(newToId, descriptorsTo.row(i)));
1073  if(kptsTo3D.size())
1074  {
1075  words3To.insert(words3To.end(), std::make_pair(newToId, kptsTo3D[i]));
1076  }
1077  ++newToId;
1078  }
1079  }
1080  }
1081  }
1082  else
1083  {
1084  UWARN("All projected points are outside the camera. Guess (%s) is wrong or images are not overlapping.", guess.prettyPrint().c_str());
1085  }
1086  UDEBUG("");
1087  }
1088  else
1089  {
1090  if(guessSet && _guessWinSize > 0 && kptsFrom3D.size() && !isCalibrated)
1091  {
1092  if(fromSignature.sensorData().cameraModels().size() > 1 || toSignature.sensorData().cameraModels().size() > 1)
1093  {
1094  UWARN("Finding correspondences with the guess cannot "
1095  "be done with multiple cameras, global matching is "
1096  "done instead. Please set \"%s\" to 0 to avoid this warning.",
1097  Parameters::kVisCorGuessWinSize().c_str());
1098  }
1099  else
1100  {
1101  UWARN("Calibration not found! Finding correspondences "
1102  "with the guess cannot be done, global matching is "
1103  "done instead.");
1104  }
1105  }
1106 
1107  UDEBUG("");
1108  // match between all descriptors
1109  VWDictionary dictionary(_featureParameters);
1110  std::list<int> fromWordIds;
1111  for (int i = 0; i < descriptorsFrom.rows; ++i)
1112  {
1113  int id = orignalWordsFromIds.size() ? orignalWordsFromIds[i] : i;
1114  dictionary.addWord(new VisualWord(id, descriptorsFrom.row(i), 1));
1115  fromWordIds.push_back(id);
1116  }
1117 
1118  std::list<int> toWordIds;
1119  if(descriptorsTo.rows)
1120  {
1121  dictionary.update();
1122  toWordIds = dictionary.addNewWords(descriptorsTo, 2);
1123  }
1124  dictionary.clear(false);
1125 
1126  std::multiset<int> fromWordIdsSet(fromWordIds.begin(), fromWordIds.end());
1127  std::multiset<int> toWordIdsSet(toWordIds.begin(), toWordIds.end());
1128 
1129  UASSERT(kptsFrom3D.empty() || fromWordIds.size() == kptsFrom3D.size());
1130  UASSERT(int(fromWordIds.size()) == descriptorsFrom.rows);
1131  int i=0;
1132  for(std::list<int>::iterator iter=fromWordIds.begin(); iter!=fromWordIds.end(); ++iter)
1133  {
1134  if(fromWordIdsSet.count(*iter) == 1)
1135  {
1136  if (kptsFrom.size())
1137  {
1138  wordsFrom.insert(std::make_pair(*iter, kptsFrom[i]));
1139  }
1140  if(kptsFrom3D.size())
1141  {
1142  words3From.insert(std::make_pair(*iter, kptsFrom3D[i]));
1143  }
1144  wordsDescFrom.insert(std::make_pair(*iter, descriptorsFrom.row(i)));
1145  }
1146  ++i;
1147  }
1148  UASSERT(kptsTo3D.size() == 0 || kptsTo3D.size() == kptsTo.size());
1149  UASSERT(toWordIds.size() == kptsTo.size());
1150  UASSERT(int(toWordIds.size()) == descriptorsTo.rows);
1151  i=0;
1152  for(std::list<int>::iterator iter=toWordIds.begin(); iter!=toWordIds.end(); ++iter)
1153  {
1154  if(toWordIdsSet.count(*iter) == 1)
1155  {
1156  wordsTo.insert(std::make_pair(*iter, kptsTo[i]));
1157  wordsDescTo.insert(std::make_pair(*iter, descriptorsTo.row(i)));
1158  if(kptsTo3D.size())
1159  {
1160  words3To.insert(std::make_pair(*iter, kptsTo3D[i]));
1161  }
1162  }
1163  ++i;
1164  }
1165  }
1166  }
1167  else if(descriptorsFrom.rows)
1168  {
1169  //just create fake words
1170  UASSERT(kptsFrom3D.empty() || int(kptsFrom3D.size()) == descriptorsFrom.rows);
1171  for(int i=0; i<descriptorsFrom.rows; ++i)
1172  {
1173  wordsFrom.insert(std::make_pair(i, kptsFrom[i]));
1174  wordsDescFrom.insert(std::make_pair(i, descriptorsFrom.row(i)));
1175  if(kptsFrom3D.size())
1176  {
1177  words3From.insert(std::make_pair(i, kptsFrom3D[i]));
1178  }
1179  }
1180  }
1181  }
1182  fromSignature.setWords(wordsFrom);
1183  fromSignature.setWords3(words3From);
1184  fromSignature.setWordsDescriptors(wordsDescFrom);
1185  toSignature.setWords(wordsTo);
1186  toSignature.setWords3(words3To);
1187  toSignature.setWordsDescriptors(wordsDescTo);
1188  delete detector;
1189  }
1190 
1192  // Motion estimation
1194  Transform transform;
1195  cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
1196  int inliersCount = 0;
1197  int matchesCount = 0;
1198  info.inliersIDs.clear();
1199  info.matchesIDs.clear();
1200  if(toSignature.getWords().size())
1201  {
1202  Transform transforms[2];
1203  std::vector<int> inliers[2];
1204  std::vector<int> matches[2];
1205  cv::Mat covariances[2];
1206  covariances[0] = cv::Mat::eye(6,6,CV_64FC1);
1207  covariances[1] = cv::Mat::eye(6,6,CV_64FC1);
1208  for(int dir=0; dir<(!_forwardEstimateOnly?2:1); ++dir)
1209  {
1210  // A to B
1211  Signature * signatureA;
1212  Signature * signatureB;
1213  if(dir == 0)
1214  {
1215  signatureA = &fromSignature;
1216  signatureB = &toSignature;
1217  }
1218  else
1219  {
1220  signatureA = &toSignature;
1221  signatureB = &fromSignature;
1222  }
1223  if(_estimationType == 2) // Epipolar Geometry
1224  {
1225  UDEBUG("");
1226  if(!signatureB->sensorData().stereoCameraModel().isValidForProjection() &&
1227  (signatureB->sensorData().cameraModels().size() != 1 ||
1228  !signatureB->sensorData().cameraModels()[0].isValidForProjection()))
1229  {
1230  UERROR("Calibrated camera required (multi-cameras not supported).");
1231  }
1232  else if((int)signatureA->getWords().size() >= _minInliers &&
1233  (int)signatureB->getWords().size() >= _minInliers)
1234  {
1235  UASSERT(signatureA->sensorData().stereoCameraModel().isValidForProjection() || (signatureA->sensorData().cameraModels().size() == 1 && signatureA->sensorData().cameraModels()[0].isValidForProjection()));
1236  const CameraModel & cameraModel = signatureA->sensorData().stereoCameraModel().isValidForProjection()?signatureA->sensorData().stereoCameraModel().left():signatureA->sensorData().cameraModels()[0];
1237 
1238  // we only need the camera transform, send guess words3 for scale estimation
1239  Transform cameraTransform;
1240  double variance = 1.0f;
1241  std::map<int, cv::Point3f> inliers3D = util3d::generateWords3DMono(
1242  uMultimapToMapUnique(signatureA->getWords()),
1243  uMultimapToMapUnique(signatureB->getWords()),
1244  cameraModel,
1245  cameraTransform,
1246  _iterations,
1247  _PnPReprojError,
1248  _PnPFlags, // cv::SOLVEPNP_ITERATIVE
1249  _PnPRefineIterations,
1250  1.0f,
1251  0.99f,
1252  uMultimapToMapUnique(signatureA->getWords3()), // for scale estimation
1253  &variance);
1254  covariances[dir] *= variance;
1255  inliers[dir] = uKeys(inliers3D);
1256 
1257  if(!cameraTransform.isNull())
1258  {
1259  if((int)inliers3D.size() >= _minInliers)
1260  {
1261  if(variance <= _epipolarGeometryVar)
1262  {
1263  if(this->force3DoF())
1264  {
1265  transforms[dir] = cameraTransform.to3DoF();
1266  }
1267  else
1268  {
1269  transforms[dir] = cameraTransform;
1270  }
1271  }
1272  else
1273  {
1274  msg = uFormat("Variance is too high! (max inlier distance=%f, variance=%f)", _epipolarGeometryVar, variance);
1275  UINFO(msg.c_str());
1276  }
1277  }
1278  else
1279  {
1280  msg = uFormat("Not enough inliers %d < %d", (int)inliers3D.size(), _minInliers);
1281  UINFO(msg.c_str());
1282  }
1283  }
1284  else
1285  {
1286  msg = uFormat("No camera transform found");
1287  UINFO(msg.c_str());
1288  }
1289  }
1290  else if(signatureA->getWords().size() == 0)
1291  {
1292  msg = uFormat("No enough features (%d)", (int)signatureA->getWords().size());
1293  UWARN(msg.c_str());
1294  }
1295  else
1296  {
1297  msg = uFormat("No camera model");
1298  UWARN(msg.c_str());
1299  }
1300  }
1301  else if(_estimationType == 1) // PnP
1302  {
1303  UDEBUG("");
1304  if(!signatureB->sensorData().stereoCameraModel().isValidForProjection() &&
1305  (signatureB->sensorData().cameraModels().size() != 1 ||
1306  !signatureB->sensorData().cameraModels()[0].isValidForProjection()))
1307  {
1308  UERROR("Calibrated camera required (multi-cameras not supported). Id=%d Models=%d StereoModel=%d weight=%d",
1309  signatureB->id(),
1310  (int)signatureB->sensorData().cameraModels().size(),
1311  signatureB->sensorData().stereoCameraModel().isValidForProjection()?1:0,
1312  signatureB->getWeight());
1313  }
1314  else
1315  {
1316  UDEBUG("words from3D=%d to2D=%d", (int)signatureA->getWords3().size(), (int)signatureB->getWords().size());
1317  // 3D to 2D
1318  if((int)signatureA->getWords3().size() >= _minInliers &&
1319  (int)signatureB->getWords().size() >= _minInliers)
1320  {
1321  UASSERT(signatureB->sensorData().stereoCameraModel().isValidForProjection() || (signatureB->sensorData().cameraModels().size() == 1 && signatureB->sensorData().cameraModels()[0].isValidForProjection()));
1322  const CameraModel & cameraModel = signatureB->sensorData().stereoCameraModel().isValidForProjection()?signatureB->sensorData().stereoCameraModel().left():signatureB->sensorData().cameraModels()[0];
1323 
1324  std::vector<int> inliersV;
1325  std::vector<int> matchesV;
1326  transforms[dir] = util3d::estimateMotion3DTo2D(
1327  uMultimapToMapUnique(signatureA->getWords3()),
1328  uMultimapToMapUnique(signatureB->getWords()),
1329  cameraModel,
1330  _minInliers,
1331  _iterations,
1332  _PnPReprojError,
1333  _PnPFlags,
1334  _PnPRefineIterations,
1335  dir==0?(!guess.isNull()?guess:Transform::getIdentity()):!transforms[0].isNull()?transforms[0].inverse():(!guess.isNull()?guess.inverse():Transform::getIdentity()),
1336  uMultimapToMapUnique(signatureB->getWords3()),
1337  &covariances[dir],
1338  &matchesV,
1339  &inliersV);
1340  inliers[dir] = inliersV;
1341  matches[dir] = matchesV;
1342  if(transforms[dir].isNull())
1343  {
1344  msg = uFormat("Not enough inliers %d/%d (matches=%d) between %d and %d",
1345  (int)inliers[dir].size(), _minInliers, (int)matches[dir].size(), signatureA->id(), signatureB->id());
1346  UINFO(msg.c_str());
1347  }
1348  else if(this->force3DoF())
1349  {
1350  transforms[dir] = transforms[dir].to3DoF();
1351  }
1352  }
1353  else
1354  {
1355  msg = uFormat("Not enough features in images (old=%d, new=%d, min=%d)",
1356  (int)signatureA->getWords3().size(), (int)signatureB->getWords().size(), _minInliers);
1357  UINFO(msg.c_str());
1358  }
1359  }
1360 
1361  }
1362  else
1363  {
1364  UDEBUG("");
1365  // 3D -> 3D
1366  if((int)signatureA->getWords3().size() >= _minInliers &&
1367  (int)signatureB->getWords3().size() >= _minInliers)
1368  {
1369  std::vector<int> inliersV;
1370  std::vector<int> matchesV;
1371  transforms[dir] = util3d::estimateMotion3DTo3D(
1372  uMultimapToMapUnique(signatureA->getWords3()),
1373  uMultimapToMapUnique(signatureB->getWords3()),
1374  _minInliers,
1375  _inlierDistance,
1376  _iterations,
1377  _refineIterations,
1378  &covariances[dir],
1379  &matchesV,
1380  &inliersV);
1381  inliers[dir] = inliersV;
1382  matches[dir] = matchesV;
1383  if(transforms[dir].isNull())
1384  {
1385  msg = uFormat("Not enough inliers %d/%d (matches=%d) between %d and %d",
1386  (int)inliers[dir].size(), _minInliers, (int)matches[dir].size(), signatureA->id(), signatureB->id());
1387  UINFO(msg.c_str());
1388  }
1389  else if(this->force3DoF())
1390  {
1391  transforms[dir] = transforms[dir].to3DoF();
1392  }
1393  }
1394  else
1395  {
1396  msg = uFormat("Not enough 3D features in images (old=%d, new=%d, min=%d)",
1397  (int)signatureA->getWords3().size(), (int)signatureB->getWords3().size(), _minInliers);
1398  UINFO(msg.c_str());
1399  }
1400  }
1401  }
1402 
1403  if(!_forwardEstimateOnly)
1404  {
1405  UDEBUG("from->to=%s", transforms[0].prettyPrint().c_str());
1406  UDEBUG("to->from=%s", transforms[1].prettyPrint().c_str());
1407  }
1408 
1409  std::vector<int> allInliers = inliers[0];
1410  if(inliers[1].size())
1411  {
1412  std::set<int> allInliersSet(allInliers.begin(), allInliers.end());
1413  unsigned int oi = allInliers.size();
1414  allInliers.resize(allInliers.size() + inliers[1].size());
1415  for(unsigned int i=0; i<inliers[1].size(); ++i)
1416  {
1417  if(allInliersSet.find(inliers[1][i]) == allInliersSet.end())
1418  {
1419  allInliers[oi++] = inliers[1][i];
1420  }
1421  }
1422  allInliers.resize(oi);
1423  }
1424  std::vector<int> allMatches = matches[0];
1425  if(matches[1].size())
1426  {
1427  std::set<int> allMatchesSet(allMatches.begin(), allMatches.end());
1428  unsigned int oi = allMatches.size();
1429  allMatches.resize(allMatches.size() + matches[1].size());
1430  for(unsigned int i=0; i<matches[1].size(); ++i)
1431  {
1432  if(allMatchesSet.find(matches[1][i]) == allMatchesSet.end())
1433  {
1434  allMatches[oi++] = matches[1][i];
1435  }
1436  }
1437  allMatches.resize(oi);
1438  }
1439 
1440  if(_bundleAdjustment > 0 &&
1441  _estimationType < 2 &&
1442  !transforms[0].isNull() &&
1443  allInliers.size() &&
1444  fromSignature.getWords3().size() &&
1445  toSignature.getWords().size() &&
1446  fromSignature.sensorData().cameraModels().size() <= 1 &&
1447  toSignature.sensorData().cameraModels().size() <= 1)
1448  {
1449  Optimizer * sba = Optimizer::create(_bundleAdjustment==2?Optimizer::kTypeCVSBA:Optimizer::kTypeG2O, _bundleParameters);
1450 
1451  std::map<int, Transform> poses;
1452  std::multimap<int, Link> links;
1453  std::map<int, cv::Point3f> points3DMap;
1454 
1455  poses.insert(std::make_pair(1, Transform::getIdentity()));
1456  poses.insert(std::make_pair(2, transforms[0]));
1457 
1458  for(int i=0;i<2;++i)
1459  {
1460  UASSERT(covariances[i].cols==6 && covariances[i].rows == 6 && covariances[i].type() == CV_64FC1);
1461  if(covariances[i].at<double>(0,0)<=COVARIANCE_EPSILON)
1462  covariances[i].at<double>(0,0) = COVARIANCE_EPSILON; // epsilon if exact transform
1463  if(covariances[i].at<double>(1,1)<=COVARIANCE_EPSILON)
1464  covariances[i].at<double>(1,1) = COVARIANCE_EPSILON; // epsilon if exact transform
1465  if(covariances[i].at<double>(2,2)<=COVARIANCE_EPSILON)
1466  covariances[i].at<double>(2,2) = COVARIANCE_EPSILON; // epsilon if exact transform
1467  if(covariances[i].at<double>(3,3)<=COVARIANCE_EPSILON)
1468  covariances[i].at<double>(3,3) = COVARIANCE_EPSILON; // epsilon if exact transform
1469  if(covariances[i].at<double>(4,4)<=COVARIANCE_EPSILON)
1470  covariances[i].at<double>(4,4) = COVARIANCE_EPSILON; // epsilon if exact transform
1471  if(covariances[i].at<double>(5,5)<=COVARIANCE_EPSILON)
1472  covariances[i].at<double>(5,5) = COVARIANCE_EPSILON; // epsilon if exact transform
1473  }
1474 
1475  cv::Mat cov = covariances[0].clone();
1476 
1477  links.insert(std::make_pair(1, Link(1, 2, Link::kNeighbor, transforms[0], cov.inv())));
1478  if(!transforms[1].isNull() && inliers[1].size())
1479  {
1480  cov = covariances[1].clone();
1481  links.insert(std::make_pair(2, Link(2, 1, Link::kNeighbor, transforms[1], cov.inv())));
1482  }
1483 
1484  std::map<int, Transform> optimizedPoses;
1485 
1486  UASSERT(toSignature.sensorData().stereoCameraModel().isValidForProjection() ||
1487  (toSignature.sensorData().cameraModels().size() == 1 && toSignature.sensorData().cameraModels()[0].isValidForProjection()));
1488 
1489  std::map<int, CameraModel> models;
1490 
1491  Transform invLocalTransformFrom;
1492  CameraModel cameraModelFrom;
1493  if(fromSignature.sensorData().stereoCameraModel().isValidForProjection())
1494  {
1495  cameraModelFrom = fromSignature.sensorData().stereoCameraModel().left();
1496  // Set Tx=-baseline*fx for Stereo BA
1497  cameraModelFrom = CameraModel(cameraModelFrom.fx(),
1498  cameraModelFrom.fy(),
1499  cameraModelFrom.cx(),
1500  cameraModelFrom.cy(),
1501  cameraModelFrom.localTransform(),
1502  -fromSignature.sensorData().stereoCameraModel().baseline()*cameraModelFrom.fy());
1503  invLocalTransformFrom = toSignature.sensorData().stereoCameraModel().localTransform().inverse();
1504  }
1505  else if(fromSignature.sensorData().cameraModels().size() == 1)
1506  {
1507  cameraModelFrom = fromSignature.sensorData().cameraModels()[0];
1508  invLocalTransformFrom = toSignature.sensorData().cameraModels()[0].localTransform().inverse();
1509  }
1510 
1511  Transform invLocalTransformTo = Transform::getIdentity();
1512  CameraModel cameraModelTo;
1513  if(toSignature.sensorData().stereoCameraModel().isValidForProjection())
1514  {
1515  cameraModelTo = toSignature.sensorData().stereoCameraModel().left();
1516  // Set Tx=-baseline*fx for Stereo BA
1517  cameraModelTo = CameraModel(cameraModelTo.fx(),
1518  cameraModelTo.fy(),
1519  cameraModelTo.cx(),
1520  cameraModelTo.cy(),
1521  cameraModelTo.localTransform(),
1522  -toSignature.sensorData().stereoCameraModel().baseline()*cameraModelTo.fy());
1523  invLocalTransformTo = toSignature.sensorData().stereoCameraModel().localTransform().inverse();
1524  }
1525  else if(toSignature.sensorData().cameraModels().size() == 1)
1526  {
1527  cameraModelTo = toSignature.sensorData().cameraModels()[0];
1528  invLocalTransformTo = toSignature.sensorData().cameraModels()[0].localTransform().inverse();
1529  }
1530  if(invLocalTransformFrom.isNull())
1531  {
1532  invLocalTransformFrom = invLocalTransformTo;
1533  }
1534 
1535  models.insert(std::make_pair(1, cameraModelFrom.isValidForProjection()?cameraModelFrom:cameraModelTo));
1536  models.insert(std::make_pair(2, cameraModelTo));
1537 
1538  std::map<int, std::map<int, cv::Point3f> > wordReferences;
1539  for(unsigned int i=0; i<allInliers.size(); ++i)
1540  {
1541  int wordId = allInliers[i];
1542  const cv::Point3f & pt3D = fromSignature.getWords3().find(wordId)->second;
1543  points3DMap.insert(std::make_pair(wordId, pt3D));
1544 
1545  std::map<int, cv::Point3f> ptMap;
1546  if(fromSignature.getWords().size() && cameraModelFrom.isValidForProjection())
1547  {
1548  float depthFrom = util3d::transformPoint(pt3D, invLocalTransformFrom).z;
1549  const cv::Point2f & kpt = fromSignature.getWords().find(wordId)->second.pt;
1550  ptMap.insert(std::make_pair(1,cv::Point3f(kpt.x, kpt.y, depthFrom)));
1551  }
1552  if(toSignature.getWords().size() && cameraModelTo.isValidForProjection())
1553  {
1554  float depthTo = util3d::transformPoint(toSignature.getWords3().find(wordId)->second, invLocalTransformTo).z;
1555  const cv::Point2f & kpt = toSignature.getWords().find(wordId)->second.pt;
1556  UASSERT(toSignature.getWords3().find(wordId) != toSignature.getWords3().end());
1557  ptMap.insert(std::make_pair(2,cv::Point3f(kpt.x, kpt.y, depthTo)));
1558  }
1559 
1560  wordReferences.insert(std::make_pair(wordId, ptMap));
1561 
1562  //UDEBUG("%d (%f,%f,%f)", wordId, points3DMap.at(wordId).x, points3DMap.at(wordId).y, points3DMap.at(wordId).z);
1563  //for(std::map<int, cv::Point3f>::iterator iter=ptMap.begin(); iter!=ptMap.end(); ++iter)
1564  //{
1565  // UDEBUG("%d (%f,%f) d=%f", iter->first, iter->second.x, iter->second.y, iter->second.z);
1566  //}
1567  }
1568 
1569  std::set<int> sbaOutliers;
1570  optimizedPoses = sba->optimizeBA(1, poses, links, models, points3DMap, wordReferences, &sbaOutliers);
1571  delete sba;
1572 
1573  //update transform
1574  if(optimizedPoses.size() == 2 &&
1575  !optimizedPoses.begin()->second.isNull() &&
1576  !optimizedPoses.rbegin()->second.isNull())
1577  {
1578  UDEBUG("Pose optimization: %s -> %s", transforms[0].prettyPrint().c_str(), optimizedPoses.rbegin()->second.prettyPrint().c_str());
1579 
1580  if(sbaOutliers.size())
1581  {
1582  std::vector<int> newInliers(allInliers.size());
1583  int oi=0;
1584  for(unsigned int i=0; i<allInliers.size(); ++i)
1585  {
1586  if(sbaOutliers.find(allInliers[i]) == sbaOutliers.end())
1587  {
1588  newInliers[oi++] = allInliers[i];
1589  }
1590  }
1591  newInliers.resize(oi);
1592  UDEBUG("BA outliers ratio %f", float(sbaOutliers.size())/float(allInliers.size()));
1593  allInliers = newInliers;
1594  }
1595  if((int)allInliers.size() < _minInliers)
1596  {
1597  msg = uFormat("Not enough inliers after bundle adjustment %d/%d (matches=%d) between %d and %d",
1598  (int)allInliers.size(), _minInliers, fromSignature.id(), toSignature.id());
1599  transforms[0].setNull();
1600  }
1601  else
1602  {
1603  transforms[0] = optimizedPoses.rbegin()->second;
1604  }
1605  // update 3D points, both from and to signatures
1606  /*std::multimap<int, cv::Point3f> cpyWordsFrom3 = fromSignature.getWords3();
1607  std::multimap<int, cv::Point3f> cpyWordsTo3 = toSignature.getWords3();
1608  Transform invT = transforms[0].inverse();
1609  for(std::map<int, cv::Point3f>::iterator iter=points3DMap.begin(); iter!=points3DMap.end(); ++iter)
1610  {
1611  cpyWordsFrom3.find(iter->first)->second = iter->second;
1612  if(cpyWordsTo3.find(iter->first) != cpyWordsTo3.end())
1613  {
1614  cpyWordsTo3.find(iter->first)->second = util3d::transformPoint(iter->second, invT);
1615  }
1616  }
1617  fromSignature.setWords3(cpyWordsFrom3);
1618  toSignature.setWords3(cpyWordsTo3);*/
1619  }
1620  else
1621  {
1622  transforms[0].setNull();
1623  }
1624  transforms[1].setNull();
1625  }
1626 
1627  info.inliersIDs = allInliers;
1628  info.matchesIDs = allMatches;
1629  inliersCount = (int)allInliers.size();
1630  matchesCount = (int)allMatches.size();
1631  if(!transforms[1].isNull())
1632  {
1633  transforms[1] = transforms[1].inverse();
1634  if(transforms[0].isNull())
1635  {
1636  transform = transforms[1];
1637  covariance = covariances[1];
1638  }
1639  else
1640  {
1641  transform = transforms[0].interpolate(0.5f, transforms[1]);
1642  covariance = (covariances[0]+covariances[1])/2.0f;
1643  }
1644  }
1645  else
1646  {
1647  transform = transforms[0];
1648  covariance = covariances[0];
1649  }
1650  }
1651  else if(toSignature.sensorData().isValid())
1652  {
1653  UWARN("Missing correspondences for registration (%d->%d). fromWords = %d fromImageEmpty=%d toWords = %d toImageEmpty=%d",
1654  fromSignature.id(), toSignature.id(),
1655  (int)fromSignature.getWords().size(), fromSignature.sensorData().imageRaw().empty()?1:0,
1656  (int)toSignature.getWords().size(), toSignature.sensorData().imageRaw().empty()?1:0);
1657  }
1658 
1659  info.inliers = inliersCount;
1660  info.matches = matchesCount;
1661  info.rejectedMsg = msg;
1662  info.covariance = covariance;
1663 
1664  UDEBUG("transform=%s", transform.prettyPrint().c_str());
1665  return transform;
1666 }
1667 
1668 }
rtabmap::Parameters::isFeatureParameter
static bool isFeatureParameter(const std::string &param)
Definition: Parameters.cpp:150
rtabmap::Signature::getWords3
const std::multimap< int, cv::Point3f > & getWords3() const
Definition: Signature.h:128
rtabmap::util3d::isFinite
bool RTABMAP_EXP isFinite(const cv::Point3f &pt)
Definition: util3d.cpp:3035
rtabmap::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:446
rtabmap::RegistrationVis::_bundleParameters
ParametersMap _bundleParameters
Definition: RegistrationVis.h:90
uMultimapToMapUnique
std::map< K, V > uMultimapToMapUnique(const std::multimap< K, V > &m)
Definition: UStl.h:505
UTimer::restart
double restart()
Definition: UTimer.h:94
rtabmap::util3d::estimateMotion3DTo2D
Transform RTABMAP_EXP estimateMotion3DTo2D(const std::map< int, cv::Point3f > &words3A, const std::map< int, cv::KeyPoint > &words2B, const CameraModel &cameraModel, int minInliers=10, int iterations=100, double reprojError=5., int flagsPnP=0, int pnpRefineIterations=1, const Transform &guess=Transform::getIdentity(), const std::map< int, cv::Point3f > &words3B=std::map< int, cv::Point3f >(), cv::Mat *covariance=0, std::vector< int > *matchesOut=0, std::vector< int > *inliersOut=0)
Definition: util3d_motion_estimation.cpp:48
rtabmap::Transform::r13
float r13() const
Definition: Transform.h:63
rtabmap::util3d::generateWords3DMono
std::map< int, cv::Point3f > RTABMAP_EXP generateWords3DMono(const std::map< int, cv::KeyPoint > &kpts, const std::map< int, cv::KeyPoint > &previousKpts, const CameraModel &cameraModel, Transform &cameraTransform, int pnpIterations=100, float pnpReprojError=8.0f, int pnpFlags=0, int pnpRefineIterations=1, float ransacParam1=3.0f, float ransacParam2=0.99f, const std::map< int, cv::Point3f > &refGuess3D=std::map< int, cv::Point3f >(), double *variance=0)
Definition: util3d_features.cpp:206
UTimer
Definition: UTimer.h:46
rtabmap::Transform::r23
float r23() const
Definition: Transform.h:66
rtabmap::Transform::prettyPrint
std::string prettyPrint() const
Definition: Transform.cpp:274
rtabmap::CameraModel::imageSize
const cv::Size & imageSize() const
Definition: CameraModel.h:112
rtabmap::util3d::transformPoint
cv::Point3f RTABMAP_EXP transformPoint(const cv::Point3f &pt, const Transform &transform)
Definition: util3d_transforms.cpp:193
uKeys
std::vector< K > uKeys(const std::multimap< K, V > &mm)
Definition: UStl.h:67
UTimer::elapsed
double elapsed()
Definition: UTimer.h:75
rtabmap::Signature::getWords
const std::multimap< int, cv::KeyPoint > & getWords() const
Definition: Signature.h:108
rtabmap::Signature::setWords3
void setWords3(const std::multimap< int, cv::Point3f > &words3)
Definition: Signature.h:115
f
f
uIsInBounds
bool uIsInBounds(const T &value, const T &low, const T &high)
Definition: UMath.h:934
rtabmap::Signature::setWords
void setWords(const std::multimap< int, cv::KeyPoint > &words)
Definition: Signature.cpp:231
util3d_motion_estimation.h
rtabmap::Registration::COVARIANCE_EPSILON
static double COVARIANCE_EPSILON
Definition: Registration.h:48
rtabmap::VWDictionary::update
virtual void update()
Definition: VWDictionary.cpp:348
rtabmap::ParametersPair
std::pair< std::string, std::string > ParametersPair
Definition: Parameters.h:42
rtabmap::Transform::getIdentity
static Transform getIdentity()
Definition: Transform.cpp:364
glm::e
GLM_FUNC_DECL genType e()
rtabmap::RegistrationVis::createFeatureDetector
Feature2D * createFeatureDetector() const
Definition: RegistrationVis.cpp:185
rtabmap::SensorData::descriptors
const cv::Mat & descriptors() const
Definition: SensorData.h:229
rtabmap::VWDictionary::addNewWords
virtual std::list< int > addNewWords(const cv::Mat &descriptors, int signatureId)
Definition: VWDictionary.cpp:634
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:41
glm::distance
GLM_FUNC_DECL genType::value_type distance(genType const &p0, genType const &p1)
rtabmap::Registration::force3DoF
bool force3DoF() const
Definition: Registration.h:68
rtabmap::SensorData::keypoints
const std::vector< cv::KeyPoint > & keypoints() const
Definition: SensorData.h:227
rtabmap::Signature::getWordsDescriptors
const std::multimap< int, cv::Mat > & getWordsDescriptors() const
Definition: Signature.h:111
UMath.h
Basic mathematics functions.
rtabmap::RegistrationInfo::inliersIDs
std::vector< int > inliersIDs
Definition: RegistrationInfo.h:70
UConversion.h
Some conversion functions.
rtabmap::SensorData::imageRaw
const cv::Mat & imageRaw() const
Definition: SensorData.h:161
UFATAL
#define UFATAL(...)
rtabmap::Transform::isIdentity
bool isIdentity() const
Definition: Transform.cpp:136
uSplit
std::list< std::string > uSplit(const std::string &str, char separator= ' ')
Definition: UStl.h:566
glm::isNull
GLM_FUNC_DECL bool isNull(detail::tmat2x2< T, P > const &m, T const &epsilon)
rtabmap::util2d::interpolate
cv::Mat RTABMAP_EXP interpolate(const cv::Mat &image, int factor, float depthErrorRatio=0.02f)
Definition: util2d.cpp:1245
UASSERT
#define UASSERT(condition)
rtabmap::SensorData::isValid
bool isValid() const
Definition: SensorData.h:134
UStl.h
Wrappers of STL for convenient functions.
rtabmap::StereoCameraModel::left
const CameraModel & left() const
Definition: StereoCameraModel.h:119
rtabmap::CameraModel::cx
double cx() const
Definition: CameraModel.h:97
rtabmap::Feature2D::create
static Feature2D * create(const ParametersMap &parameters=ParametersMap())
Definition: Features2d.cpp:405
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
rtabmap::CameraModel::fy
double fy() const
Definition: CameraModel.h:96
rtabmap::RegistrationInfo::projectedIDs
std::vector< int > projectedIDs
Definition: RegistrationInfo.h:73
rtabmap::Transform::r12
float r12() const
Definition: Transform.h:62
rtabmap::Registration::parseParameters
virtual void parseParameters(const ParametersMap &parameters)
Definition: Registration.cpp:78
rtabmap::Transform::r31
float r31() const
Definition: Transform.h:67
rtabmap::Signature::setWordsDescriptors
void setWordsDescriptors(const std::multimap< int, cv::Mat > &descriptors)
Definition: Signature.h:112
rtabmap::VWDictionary::clear
void clear(bool printWarningsIfNotEmpty=true)
Definition: VWDictionary.cpp:561
rtabmap::Signature::id
int id() const
Definition: Signature.h:70
rtabmap::util3d::estimateMotion3DTo3D
Transform RTABMAP_EXP estimateMotion3DTo3D(const std::map< int, cv::Point3f > &words3A, const std::map< int, cv::Point3f > &words3B, int minInliers=10, double inliersDistance=0.1, int iterations=100, int refineIterations=5, cv::Mat *covariance=0, std::vector< int > *matchesOut=0, std::vector< int > *inliersOut=0)
Definition: util3d_motion_estimation.cpp:212
rtabmap::Signature::getWeight
int getWeight() const
Definition: Signature.h:74
rtabmap::Transform::r21
float r21() const
Definition: Transform.h:64
util3d_transforms.h
rtabmap::RegistrationVis::computeTransformationImpl
virtual Transform computeTransformationImpl(Signature &from, Signature &to, Transform guess, RegistrationInfo &info) const
Definition: RegistrationVis.cpp:190
uContains
bool uContains(const std::list< V > &list, const V &value)
Definition: UStl.h:409
rtabmap::CameraModel::cy
double cy() const
Definition: CameraModel.h:98
rtabmap::CameraModel::isValidForProjection
bool isValidForProjection() const
Definition: CameraModel.h:80
rtabmap::Transform::r33
float r33() const
Definition: Transform.h:69
rtabmap::Parameters::getDefaultParameters
static const ParametersMap & getDefaultParameters()
Definition: Parameters.h:695
rtabmap::SensorData::cameraModels
const std::vector< CameraModel > & cameraModels() const
Definition: SensorData.h:193
rtabmap::Transform::to3DoF
Transform to3DoF() const
Definition: Transform.cpp:189
uValues
std::vector< V > uValues(const std::multimap< K, V > &mm)
Definition: UStl.h:100
rtabmap::Optimizer::optimizeBA
virtual std::map< int, Transform > optimizeBA(int rootId, const std::map< int, Transform > &poses, const std::multimap< int, Link > &links, const std::map< int, CameraModel > &models, std::map< int, cv::Point3f > &points3DMap, const std::map< int, std::map< int, cv::Point3f > > &wordReferences, std::set< int > *outliers=0)
Definition: Optimizer.cpp:362
UDEBUG
#define UDEBUG(...)
rtabmap::CameraModel::fx
double fx() const
Definition: CameraModel.h:95
rtabmap::Signature::sensorData
SensorData & sensorData()
Definition: Signature.h:134
rtabmap::Transform::r22
float r22() const
Definition: Transform.h:65
UERROR
#define UERROR(...)
rtabmap::RegistrationVis::parseParameters
virtual void parseParameters(const ParametersMap &parameters)
Definition: RegistrationVis.cpp:91
ULogger.h
ULogger class and convenient macros.
rtabmap::SensorData::stereoCameraModel
const StereoCameraModel & stereoCameraModel() const
Definition: SensorData.h:194
UWARN
#define UWARN(...)
UTimer::ticks
double ticks()
Definition: UTimer.cpp:110
rtabmap::RegistrationVis::_featureParameters
ParametersMap _featureParameters
Definition: RegistrationVis.h:89
rtabmap::RegistrationInfo::matchesIDs
std::vector< int > matchesIDs
Definition: RegistrationInfo.h:72
util3d_features.h
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:169
rtabmap::CameraModel::K
cv::Mat K() const
Definition: CameraModel.h:103
rtabmap::VWDictionary::addWord
virtual void addWord(VisualWord *vw)
Definition: VWDictionary.cpp:1251
rtabmap::StereoCameraModel::localTransform
const Transform & localTransform() const
Definition: StereoCameraModel.h:116
rtabmap::Transform::r11
float r11() const
Definition: Transform.h:61
uFormat
std::string UTILITE_EXP uFormat(const char *fmt,...)
Definition: UConversion.cpp:350
rtabmap::Optimizer::create
static Optimizer * create(const ParametersMap &parameters)
Definition: Optimizer.cpp:67
rtabmap::SensorData::setFeatures
void setFeatures(const std::vector< cv::KeyPoint > &keypoints, const std::vector< cv::Point3f > &keypoints3D, const cv::Mat &descriptors)
Definition: SensorData.cpp:828
rtabmap::RegistrationVis::RegistrationVis
RegistrationVis(const ParametersMap &parameters=ParametersMap(), Registration *child=0)
Definition: RegistrationVis.cpp:50
rtabmap::SensorData::keypoints3D
const std::vector< cv::Point3f > & keypoints3D() const
Definition: SensorData.h:228
rtabmap::SensorData::depthRaw
cv::Mat depthRaw() const
Definition: SensorData.h:172
rtabmap::Transform::r32
float r32() const
Definition: Transform.h:68
uInsert
void uInsert(std::map< K, V > &map, const std::pair< K, V > &pair)
Definition: UStl.h:443
rtabmap::CameraModel::localTransform
const Transform & localTransform() const
Definition: CameraModel.h:109
glm::inverse
GLM_FUNC_DECL matType< T, P > inverse(matType< T, P > const &m)
rtabmap::Transform::interpolate
Transform interpolate(float t, const Transform &other) const
Definition: Transform.cpp:251
UINFO
#define UINFO(...)

rtabmap
Author(s): Mathieu Labbe
autogenerated on Wed Jun 5 2019 22:41:32