00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #include <rtabmap/core/RegistrationVis.h>
00031 #include <rtabmap/core/util3d_motion_estimation.h>
00032 #include <rtabmap/core/util3d_features.h>
00033 #include <rtabmap/core/util3d.h>
00034 #include <rtabmap/core/VWDictionary.h>
00035 #include <rtabmap/core/util2d.h>
00036 #include <rtabmap/core/Features2d.h>
00037 #include <rtabmap/utilite/ULogger.h>
00038 #include <rtabmap/utilite/UConversion.h>
00039 #include <rtabmap/utilite/UStl.h>
00040 #include <rtabmap/utilite/UTimer.h>
00041 #include <rtabmap/utilite/UMath.h>
00042
00043 #include <rtflann/flann.hpp>
00044
00045 namespace rtabmap {
00046
00047 RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration * child) :
00048 Registration(parameters, child),
00049 _minInliers(Parameters::defaultVisMinInliers()),
00050 _inlierDistance(Parameters::defaultVisInlierDistance()),
00051 _iterations(Parameters::defaultVisIterations()),
00052 _refineIterations(Parameters::defaultVisRefineIterations()),
00053 _epipolarGeometryVar(Parameters::defaultVisEpipolarGeometryVar()),
00054 _estimationType(Parameters::defaultVisEstimationType()),
00055 _forwardEstimateOnly(Parameters::defaultVisForwardEstOnly()),
00056 _PnPReprojError(Parameters::defaultVisPnPReprojError()),
00057 _PnPFlags(Parameters::defaultVisPnPFlags()),
00058 _PnPRefineIterations(Parameters::defaultVisPnPRefineIterations()),
00059 _correspondencesApproach(Parameters::defaultVisCorType()),
00060 _flowWinSize(Parameters::defaultVisCorFlowWinSize()),
00061 _flowIterations(Parameters::defaultVisCorFlowIterations()),
00062 _flowEps(Parameters::defaultVisCorFlowEps()),
00063 _flowMaxLevel(Parameters::defaultVisCorFlowMaxLevel()),
00064 _nndr(Parameters::defaultVisCorNNDR()),
00065 _guessWinSize(Parameters::defaultVisCorGuessWinSize())
00066 {
00067 _featureParameters = Parameters::getDefaultParameters();
00068 uInsert(_featureParameters, ParametersPair(Parameters::kKpNNStrategy(), _featureParameters.at(Parameters::kVisCorNNType())));
00069 uInsert(_featureParameters, ParametersPair(Parameters::kKpNndrRatio(), _featureParameters.at(Parameters::kVisCorNNDR())));
00070 uInsert(_featureParameters, ParametersPair(Parameters::kKpDetectorStrategy(), _featureParameters.at(Parameters::kVisFeatureType())));
00071 uInsert(_featureParameters, ParametersPair(Parameters::kKpMaxFeatures(), _featureParameters.at(Parameters::kVisMaxFeatures())));
00072 uInsert(_featureParameters, ParametersPair(Parameters::kKpMaxDepth(), _featureParameters.at(Parameters::kVisMaxDepth())));
00073 uInsert(_featureParameters, ParametersPair(Parameters::kKpMinDepth(), _featureParameters.at(Parameters::kVisMinDepth())));
00074 uInsert(_featureParameters, ParametersPair(Parameters::kKpRoiRatios(), _featureParameters.at(Parameters::kVisRoiRatios())));
00075 uInsert(_featureParameters, ParametersPair(Parameters::kKpSubPixEps(), _featureParameters.at(Parameters::kVisSubPixWinSize())));
00076 uInsert(_featureParameters, ParametersPair(Parameters::kKpSubPixIterations(), _featureParameters.at(Parameters::kVisSubPixIterations())));
00077 uInsert(_featureParameters, ParametersPair(Parameters::kKpSubPixWinSize(), _featureParameters.at(Parameters::kVisSubPixEps())));
00078 uInsert(_featureParameters, ParametersPair(Parameters::kKpNewWordsComparedTogether(), "false"));
00079
00080 this->parseParameters(parameters);
00081 }
00082
00083 void RegistrationVis::parseParameters(const ParametersMap & parameters)
00084 {
00085 Registration::parseParameters(parameters);
00086
00087 Parameters::parse(parameters, Parameters::kVisMinInliers(), _minInliers);
00088 Parameters::parse(parameters, Parameters::kVisInlierDistance(), _inlierDistance);
00089 Parameters::parse(parameters, Parameters::kVisIterations(), _iterations);
00090 Parameters::parse(parameters, Parameters::kVisRefineIterations(), _refineIterations);
00091 Parameters::parse(parameters, Parameters::kVisEstimationType(), _estimationType);
00092 Parameters::parse(parameters, Parameters::kVisEpipolarGeometryVar(), _epipolarGeometryVar);
00093 Parameters::parse(parameters, Parameters::kVisPnPReprojError(), _PnPReprojError);
00094 Parameters::parse(parameters, Parameters::kVisPnPFlags(), _PnPFlags);
00095 Parameters::parse(parameters, Parameters::kVisPnPRefineIterations(), _PnPRefineIterations);
00096 Parameters::parse(parameters, Parameters::kVisCorType(), _correspondencesApproach);
00097 Parameters::parse(parameters, Parameters::kVisCorFlowWinSize(), _flowWinSize);
00098 Parameters::parse(parameters, Parameters::kVisCorFlowIterations(), _flowIterations);
00099 Parameters::parse(parameters, Parameters::kVisCorFlowEps(), _flowEps);
00100 Parameters::parse(parameters, Parameters::kVisCorFlowMaxLevel(), _flowMaxLevel);
00101 Parameters::parse(parameters, Parameters::kVisCorNNDR(), _nndr);
00102 Parameters::parse(parameters, Parameters::kVisCorGuessWinSize(), _guessWinSize);
00103
00104 UASSERT_MSG(_minInliers >= 1, uFormat("value=%d", _minInliers).c_str());
00105 UASSERT_MSG(_inlierDistance > 0.0f, uFormat("value=%f", _inlierDistance).c_str());
00106 UASSERT_MSG(_iterations > 0, uFormat("value=%d", _iterations).c_str());
00107
00108
00109 for(ParametersMap::const_iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
00110 {
00111 std::string group = uSplit(iter->first, '/').front();
00112 if(Parameters::isFeatureParameter(iter->first) || group.compare("Stereo") == 0)
00113 {
00114 uInsert(_featureParameters, ParametersPair(iter->first, iter->second));
00115 }
00116 }
00117
00118 if(uContains(parameters, Parameters::kVisCorNNType()))
00119 {
00120 uInsert(_featureParameters, ParametersPair(Parameters::kKpNNStrategy(), parameters.at(Parameters::kVisCorNNType())));
00121 }
00122 if(uContains(parameters, Parameters::kVisCorNNDR()))
00123 {
00124 uInsert(_featureParameters, ParametersPair(Parameters::kKpNndrRatio(), parameters.at(Parameters::kVisCorNNDR())));
00125 }
00126 if(uContains(parameters, Parameters::kVisFeatureType()))
00127 {
00128 uInsert(_featureParameters, ParametersPair(Parameters::kKpDetectorStrategy(), parameters.at(Parameters::kVisFeatureType())));
00129 }
00130 if(uContains(parameters, Parameters::kVisMaxFeatures()))
00131 {
00132 uInsert(_featureParameters, ParametersPair(Parameters::kKpMaxFeatures(), parameters.at(Parameters::kVisMaxFeatures())));
00133 }
00134 if(uContains(parameters, Parameters::kVisMaxDepth()))
00135 {
00136 uInsert(_featureParameters, ParametersPair(Parameters::kKpMaxDepth(), parameters.at(Parameters::kVisMaxDepth())));
00137 }
00138 if(uContains(parameters, Parameters::kVisMinDepth()))
00139 {
00140 uInsert(_featureParameters, ParametersPair(Parameters::kKpMinDepth(), parameters.at(Parameters::kVisMinDepth())));
00141 }
00142 if(uContains(parameters, Parameters::kVisRoiRatios()))
00143 {
00144 uInsert(_featureParameters, ParametersPair(Parameters::kKpRoiRatios(), parameters.at(Parameters::kVisRoiRatios())));
00145 }
00146 if(uContains(parameters, Parameters::kVisSubPixEps()))
00147 {
00148 uInsert(_featureParameters, ParametersPair(Parameters::kKpSubPixEps(), parameters.at(Parameters::kVisSubPixEps())));
00149 }
00150 if(uContains(parameters, Parameters::kVisSubPixIterations()))
00151 {
00152 uInsert(_featureParameters, ParametersPair(Parameters::kKpSubPixIterations(), parameters.at(Parameters::kVisSubPixIterations())));
00153 }
00154 if(uContains(parameters, Parameters::kVisSubPixWinSize()))
00155 {
00156 uInsert(_featureParameters, ParametersPair(Parameters::kKpSubPixWinSize(), parameters.at(Parameters::kVisSubPixWinSize())));
00157 }
00158 }
00159
00160 RegistrationVis::~RegistrationVis()
00161 {
00162 }
00163
00164 Feature2D * RegistrationVis::createFeatureDetector() const
00165 {
00166 return Feature2D::create(_featureParameters);
00167 }
00168
00169 Transform RegistrationVis::computeTransformationImpl(
00170 Signature & fromSignature,
00171 Signature & toSignature,
00172 Transform guess,
00173 RegistrationInfo & info) const
00174 {
00175 UDEBUG("%s=%d", Parameters::kVisMinInliers().c_str(), _minInliers);
00176 UDEBUG("%s=%f", Parameters::kVisInlierDistance().c_str(), _inlierDistance);
00177 UDEBUG("%s=%d", Parameters::kVisIterations().c_str(), _iterations);
00178 UDEBUG("%s=%d", Parameters::kVisEstimationType().c_str(), _estimationType);
00179 UDEBUG("%s=%d", Parameters::kVisForwardEstOnly().c_str(), _forwardEstimateOnly);
00180 UDEBUG("%s=%f", Parameters::kVisEpipolarGeometryVar().c_str(), _epipolarGeometryVar);
00181 UDEBUG("%s=%f", Parameters::kVisPnPReprojError().c_str(), _PnPReprojError);
00182 UDEBUG("%s=%d", Parameters::kVisPnPFlags().c_str(), _PnPFlags);
00183 UDEBUG("%s=%d", Parameters::kVisCorType().c_str(), _correspondencesApproach);
00184 UDEBUG("%s=%d", Parameters::kVisCorFlowWinSize().c_str(), _flowWinSize);
00185 UDEBUG("%s=%d", Parameters::kVisCorFlowIterations().c_str(), _flowIterations);
00186 UDEBUG("%s=%f", Parameters::kVisCorFlowEps().c_str(), _flowEps);
00187 UDEBUG("%s=%d", Parameters::kVisCorFlowMaxLevel().c_str(), _flowMaxLevel);
00188
00189 UDEBUG("Input(%d): from=%d words, %d 3D words, %d words descriptors, %d kpts, %d descriptors",
00190 fromSignature.id(),
00191 (int)fromSignature.getWords().size(),
00192 (int)fromSignature.getWords3().size(),
00193 (int)fromSignature.getWordsDescriptors().size(),
00194 (int)fromSignature.sensorData().keypoints().size(),
00195 fromSignature.sensorData().descriptors().rows);
00196
00197 UDEBUG("Input(%d): to=%d words, %d 3D words, %d words descriptors, %d kpts, %d descriptors",
00198 toSignature.id(),
00199 (int)toSignature.getWords().size(),
00200 (int)toSignature.getWords3().size(),
00201 (int)toSignature.getWordsDescriptors().size(),
00202 (int)toSignature.sensorData().keypoints().size(),
00203 toSignature.sensorData().descriptors().rows);
00204
00205 std::string msg;
00206
00208
00210
00211 if((fromSignature.getWordsDescriptors().empty() && toSignature.getWordsDescriptors().empty()) &&
00212 (_estimationType<2 || fromSignature.getWords().size()) &&
00213 (_estimationType==0 || toSignature.getWords().size()) &&
00214 fromSignature.getWords3().size() &&
00215 (_estimationType==1 || toSignature.getWords3().size()))
00216 {
00217
00218 UDEBUG("");
00219 }
00220 else
00221 {
00222 UDEBUG("");
00223
00224 UASSERT(fromSignature.getWords().empty() ||
00225 fromSignature.getWords3().empty() ||
00226 (fromSignature.getWords().size() == fromSignature.getWords3().size()));
00227 UASSERT((int)fromSignature.sensorData().keypoints().size() == fromSignature.sensorData().descriptors().rows ||
00228 fromSignature.getWords().size() == fromSignature.getWordsDescriptors().size() ||
00229 fromSignature.sensorData().descriptors().rows == 0 ||
00230 fromSignature.getWordsDescriptors().size() == 0);
00231 UASSERT((toSignature.getWords().empty() && toSignature.getWords3().empty())||
00232 (toSignature.getWords().size() == toSignature.getWords3().size()));
00233 UASSERT((int)toSignature.sensorData().keypoints().size() == toSignature.sensorData().descriptors().rows ||
00234 toSignature.getWords().size() == toSignature.getWordsDescriptors().size() ||
00235 toSignature.sensorData().descriptors().rows == 0 ||
00236 toSignature.getWordsDescriptors().size() == 0);
00237 UASSERT(fromSignature.sensorData().imageRaw().empty() ||
00238 fromSignature.sensorData().imageRaw().type() == CV_8UC1 ||
00239 fromSignature.sensorData().imageRaw().type() == CV_8UC3);
00240 UASSERT(toSignature.sensorData().imageRaw().empty() ||
00241 toSignature.sensorData().imageRaw().type() == CV_8UC1 ||
00242 toSignature.sensorData().imageRaw().type() == CV_8UC3);
00243
00244 Feature2D * detector = createFeatureDetector();
00245 std::vector<cv::KeyPoint> kptsFrom;
00246 cv::Mat imageFrom = fromSignature.sensorData().imageRaw();
00247 cv::Mat imageTo = toSignature.sensorData().imageRaw();
00248
00249 if(fromSignature.getWords().empty())
00250 {
00251 if(fromSignature.sensorData().keypoints().empty())
00252 {
00253 if(!imageFrom.empty())
00254 {
00255 if(imageFrom.channels() > 1)
00256 {
00257 cv::Mat tmp;
00258 cv::cvtColor(imageFrom, tmp, cv::COLOR_BGR2GRAY);
00259 imageFrom = tmp;
00260 }
00261
00262 cv::Mat depthMask;
00263 if(!fromSignature.sensorData().depthRaw().empty())
00264 {
00265 if(imageFrom.rows % fromSignature.sensorData().depthRaw().rows == 0 &&
00266 imageFrom.cols % fromSignature.sensorData().depthRaw().cols == 0 &&
00267 imageFrom.rows/fromSignature.sensorData().depthRaw().rows == fromSignature.sensorData().imageRaw().cols/fromSignature.sensorData().depthRaw().cols)
00268 {
00269 depthMask = util2d::interpolate(fromSignature.sensorData().depthRaw(), fromSignature.sensorData().imageRaw().rows/fromSignature.sensorData().depthRaw().rows, 0.1f);
00270 }
00271 }
00272
00273 kptsFrom = detector->generateKeypoints(
00274 imageFrom,
00275 depthMask);
00276 }
00277 }
00278 else
00279 {
00280 kptsFrom = fromSignature.sensorData().keypoints();
00281 }
00282 }
00283 else
00284 {
00285 kptsFrom = uValues(fromSignature.getWords());
00286 }
00287
00288 std::multimap<int, cv::KeyPoint> wordsFrom;
00289 std::multimap<int, cv::KeyPoint> wordsTo;
00290 std::multimap<int, cv::Point3f> words3From;
00291 std::multimap<int, cv::Point3f> words3To;
00292 std::multimap<int, cv::Mat> wordsDescFrom;
00293 std::multimap<int, cv::Mat> wordsDescTo;
00294 if(_correspondencesApproach == 1 &&
00295 !imageFrom.empty() &&
00296 !imageTo.empty())
00297 {
00298 UDEBUG("");
00299
00300 if(imageFrom.channels() > 1)
00301 {
00302 cv::Mat tmp;
00303 cv::cvtColor(imageFrom, tmp, cv::COLOR_BGR2GRAY);
00304 imageFrom = tmp;
00305 }
00306 if(imageTo.channels() > 1)
00307 {
00308 cv::Mat tmp;
00309 cv::cvtColor(imageTo, tmp, cv::COLOR_BGR2GRAY);
00310 imageTo = tmp;
00311 }
00312
00313 std::vector<cv::Point3f> kptsFrom3D;
00314 if(fromSignature.getWords3().empty())
00315 {
00316 kptsFrom3D = detector->generateKeypoints3D(fromSignature.sensorData(), kptsFrom);
00317 }
00318 else
00319 {
00320 kptsFrom3D = uValues(fromSignature.getWords3());
00321 }
00322
00323 if(!imageTo.empty())
00324 {
00325 std::vector<cv::Point2f> cornersFrom;
00326 cv::KeyPoint::convert(kptsFrom, cornersFrom);
00327 std::vector<cv::Point2f> cornersTo;
00328 bool guessSet = !guess.isIdentity() && !guess.isNull();
00329 if(guessSet)
00330 {
00331 Transform localTransform = fromSignature.sensorData().cameraModels().size()?fromSignature.sensorData().cameraModels()[0].localTransform():fromSignature.sensorData().stereoCameraModel().left().localTransform();
00332 Transform guessCameraRef = (guess * localTransform).inverse();
00333 cv::Mat R = (cv::Mat_<double>(3,3) <<
00334 (double)guessCameraRef.r11(), (double)guessCameraRef.r12(), (double)guessCameraRef.r13(),
00335 (double)guessCameraRef.r21(), (double)guessCameraRef.r22(), (double)guessCameraRef.r23(),
00336 (double)guessCameraRef.r31(), (double)guessCameraRef.r32(), (double)guessCameraRef.r33());
00337 cv::Mat rvec(1,3, CV_64FC1);
00338 cv::Rodrigues(R, rvec);
00339 cv::Mat tvec = (cv::Mat_<double>(1,3) << (double)guessCameraRef.x(), (double)guessCameraRef.y(), (double)guessCameraRef.z());
00340 cv::Mat K = fromSignature.sensorData().cameraModels().size()?fromSignature.sensorData().cameraModels()[0].K():fromSignature.sensorData().stereoCameraModel().left().K();
00341 cv::projectPoints(kptsFrom3D, rvec, tvec, K, cv::Mat(), cornersTo);
00342 }
00343
00344
00345 UDEBUG("guessSet = %d", guessSet?1:0);
00346 std::vector<unsigned char> status;
00347 std::vector<float> err;
00348 UDEBUG("cv::calcOpticalFlowPyrLK() begin");
00349 cv::calcOpticalFlowPyrLK(
00350 imageFrom,
00351 imageTo,
00352 cornersFrom,
00353 cornersTo,
00354 status,
00355 err,
00356 cv::Size(_flowWinSize, _flowWinSize),
00357 guessSet?0:_flowMaxLevel,
00358 cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, _flowIterations, _flowEps),
00359 cv::OPTFLOW_LK_GET_MIN_EIGENVALS | (guessSet?cv::OPTFLOW_USE_INITIAL_FLOW:0), 1e-4);
00360 UDEBUG("cv::calcOpticalFlowPyrLK() end");
00361
00362 UASSERT(kptsFrom.size() == kptsFrom3D.size());
00363 std::vector<cv::KeyPoint> kptsTo(kptsFrom.size());
00364 std::vector<cv::Point3f> kptsFrom3DKept(kptsFrom3D.size());
00365 int ki = 0;
00366 for(unsigned int i=0; i<status.size(); ++i)
00367 {
00368 if(status[i] &&
00369 uIsInBounds(cornersTo[i].x, 0.0f, float(imageTo.cols)) &&
00370 uIsInBounds(cornersTo[i].y, 0.0f, float(imageTo.rows)))
00371 {
00372 kptsFrom[ki] = cv::KeyPoint(cornersFrom[i], 1);
00373 kptsFrom3DKept[ki] = kptsFrom3D[i];
00374 kptsTo[ki++] = cv::KeyPoint(cornersTo[i], 1);
00375 }
00376 }
00377 kptsFrom.resize(ki);
00378 kptsTo.resize(ki);
00379 kptsFrom3DKept.resize(ki);
00380
00381 std::vector<cv::Point3f> kptsTo3D;
00382 if(_estimationType == 0 || (_estimationType == 1 && !varianceFromInliersCount()) || !_forwardEstimateOnly)
00383 {
00384 kptsTo3D = detector->generateKeypoints3D(toSignature.sensorData(), kptsTo);
00385 }
00386
00387 UASSERT(kptsFrom.size() == kptsFrom3DKept.size());
00388 UASSERT(kptsFrom.size() == kptsTo.size());
00389 UASSERT(kptsTo3D.size() == 0 || kptsTo.size() == kptsTo3D.size());
00390 for(unsigned int i=0; i< kptsFrom3DKept.size(); ++i)
00391 {
00392 wordsFrom.insert(std::make_pair(i, kptsFrom[i]));
00393 words3From.insert(std::make_pair(i, kptsFrom3DKept[i]));
00394 wordsTo.insert(std::make_pair(i, kptsTo[i]));
00395 if(kptsTo3D.size())
00396 {
00397 words3To.insert(std::make_pair(i, kptsTo3D[i]));
00398 }
00399 }
00400 toSignature.sensorData().setFeatures(kptsTo, cv::Mat());
00401 }
00402 else
00403 {
00404 UASSERT(kptsFrom.size() == kptsFrom3D.size());
00405 for(unsigned int i=0; i< kptsFrom3D.size(); ++i)
00406 {
00407 if(util3d::isFinite(kptsFrom3D[i]))
00408 {
00409 wordsFrom.insert(std::make_pair(i, kptsFrom[i]));
00410 words3From.insert(std::make_pair(i, kptsFrom3D[i]));
00411 }
00412 }
00413 toSignature.sensorData().setFeatures(std::vector<cv::KeyPoint>(), cv::Mat());
00414 }
00415
00416 fromSignature.sensorData().setFeatures(kptsFrom, cv::Mat());
00417 }
00418 else
00419 {
00420 UDEBUG("");
00421 std::vector<cv::KeyPoint> kptsTo;
00422 if(toSignature.getWords().empty())
00423 {
00424 if(toSignature.sensorData().keypoints().empty() &&
00425 !imageTo.empty())
00426 {
00427 if(imageTo.channels() > 1)
00428 {
00429 cv::Mat tmp;
00430 cv::cvtColor(imageTo, tmp, cv::COLOR_BGR2GRAY);
00431 imageTo = tmp;
00432 }
00433
00434 cv::Mat depthMask;
00435 if(!toSignature.sensorData().depthRaw().empty())
00436 {
00437 if(imageTo.rows % toSignature.sensorData().depthRaw().rows == 0 &&
00438 imageTo.cols % toSignature.sensorData().depthRaw().cols == 0 &&
00439 imageTo.rows/toSignature.sensorData().depthRaw().rows == imageTo.cols/toSignature.sensorData().depthRaw().cols)
00440 {
00441 depthMask = util2d::interpolate(toSignature.sensorData().depthRaw(), imageTo.rows/toSignature.sensorData().depthRaw().rows, 0.1f);
00442 }
00443 }
00444
00445 kptsTo = detector->generateKeypoints(
00446 imageTo,
00447 depthMask);
00448 }
00449 else
00450 {
00451 kptsTo = toSignature.sensorData().keypoints();
00452 }
00453 }
00454 else
00455 {
00456 kptsTo = uValues(toSignature.getWords());
00457 }
00458
00459
00460 UDEBUG("kptsFrom=%d", (int)kptsFrom.size());
00461 UDEBUG("kptsTo=%d", (int)kptsTo.size());
00462 cv::Mat descriptorsFrom;
00463 if((kptsFrom.empty() && fromSignature.getWordsDescriptors().size()) ||
00464 fromSignature.getWordsDescriptors().size() == kptsFrom.size())
00465 {
00466 descriptorsFrom = cv::Mat(fromSignature.getWordsDescriptors().size(),
00467 fromSignature.getWordsDescriptors().begin()->second.cols,
00468 fromSignature.getWordsDescriptors().begin()->second.type());
00469 int i=0;
00470 for(std::multimap<int, cv::Mat>::const_iterator iter=fromSignature.getWordsDescriptors().begin();
00471 iter!=fromSignature.getWordsDescriptors().end();
00472 ++iter, ++i)
00473 {
00474 iter->second.copyTo(descriptorsFrom.row(i));
00475 }
00476 }
00477 else if(fromSignature.sensorData().descriptors().rows == (int)kptsFrom.size())
00478 {
00479 descriptorsFrom = fromSignature.sensorData().descriptors();
00480 }
00481 else if(!imageFrom.empty())
00482 {
00483 if(imageFrom.channels() > 1)
00484 {
00485 cv::Mat tmp;
00486 cv::cvtColor(imageFrom, tmp, cv::COLOR_BGR2GRAY);
00487 imageFrom = tmp;
00488 }
00489 descriptorsFrom = detector->generateDescriptors(imageFrom, kptsFrom);
00490 }
00491
00492 cv::Mat descriptorsTo;
00493 if(kptsTo.size())
00494 {
00495 if(toSignature.getWordsDescriptors().size() == kptsTo.size())
00496 {
00497 descriptorsTo = cv::Mat(toSignature.getWordsDescriptors().size(),
00498 toSignature.getWordsDescriptors().begin()->second.cols,
00499 toSignature.getWordsDescriptors().begin()->second.type());
00500 int i=0;
00501 for(std::multimap<int, cv::Mat>::const_iterator iter=toSignature.getWordsDescriptors().begin();
00502 iter!=toSignature.getWordsDescriptors().end();
00503 ++iter, ++i)
00504 {
00505 iter->second.copyTo(descriptorsTo.row(i));
00506 }
00507 }
00508 else if(toSignature.sensorData().descriptors().rows == (int)kptsTo.size())
00509 {
00510 descriptorsTo = toSignature.sensorData().descriptors();
00511 }
00512 else if(!imageTo.empty())
00513 {
00514 if(imageTo.channels() > 1)
00515 {
00516 cv::Mat tmp;
00517 cv::cvtColor(imageTo, tmp, cv::COLOR_BGR2GRAY);
00518 imageTo = tmp;
00519 }
00520
00521 descriptorsTo = detector->generateDescriptors(imageTo, kptsTo);
00522 }
00523 }
00524
00525
00526 std::vector<cv::Point3f> kptsFrom3D;
00527 std::vector<cv::Point3f> kptsTo3D;
00528 if(fromSignature.getWords3().empty() || kptsFrom.size() != fromSignature.getWords3().size())
00529 {
00530 if(fromSignature.getWords3().size() && kptsFrom.size() != fromSignature.getWords3().size())
00531 {
00532 UWARN("kptsFrom (%d) is not the same size as fromSignature.getWords3() (%d), there "
00533 "is maybe a problem with the logic above (getWords3() should be null or equal to kptsfrom).");
00534 }
00535 kptsFrom3D = detector->generateKeypoints3D(fromSignature.sensorData(), kptsFrom);
00536 if(detector->getMinDepth() > 0.0f || detector->getMaxDepth() > 0.0f)
00537 {
00538 UDEBUG("");
00539
00540 UASSERT((int)kptsFrom.size() == descriptorsFrom.rows &&
00541 kptsFrom3D.size() == kptsFrom.size());
00542 std::vector<cv::KeyPoint> validKeypoints(kptsFrom.size());
00543 std::vector<cv::Point3f> validKeypoints3D(kptsFrom.size());
00544 cv::Mat validDescriptors(descriptorsFrom.size(), descriptorsFrom.type());
00545
00546 int oi=0;
00547 for(unsigned int i=0; i<kptsFrom3D.size(); ++i)
00548 {
00549 if(util3d::isFinite(kptsFrom3D[i]))
00550 {
00551 validKeypoints[oi] = kptsFrom[i];
00552 validKeypoints3D[oi] = kptsFrom3D[i];
00553 descriptorsFrom.row(i).copyTo(validDescriptors.row(oi));
00554 ++oi;
00555 }
00556 }
00557 UDEBUG("Removed %d invalid 3D points", (int)kptsFrom3D.size()-oi);
00558 validKeypoints.resize(oi);
00559 validKeypoints3D.resize(oi);
00560 kptsFrom = validKeypoints;
00561 kptsFrom3D = validKeypoints3D;
00562 descriptorsFrom = validDescriptors.rowRange(0, oi).clone();
00563 }
00564 }
00565 else
00566 {
00567 kptsFrom3D = uValues(fromSignature.getWords3());
00568 }
00569 if(toSignature.getWords3().empty() || kptsTo.size() != toSignature.getWords3().size())
00570 {
00571 if(toSignature.getWords3().size() && kptsTo.size() != toSignature.getWords3().size())
00572 {
00573 UWARN("kptsTo (%d) is not the same size as toSignature.getWords3() (%d), there "
00574 "is maybe a problem with the logic above (getWords3() should be null or equal to kptsTo).");
00575 }
00576 kptsTo3D = detector->generateKeypoints3D(toSignature.sensorData(), kptsTo);
00577 if(detector->getMinDepth() > 0.0f || detector->getMaxDepth() > 0.0f)
00578 {
00579 UDEBUG("");
00580
00581 UASSERT((int)kptsTo.size() == descriptorsTo.rows &&
00582 kptsTo3D.size() == kptsTo.size());
00583 std::vector<cv::KeyPoint> validKeypoints(kptsTo.size());
00584 std::vector<cv::Point3f> validKeypoints3D(kptsTo.size());
00585 cv::Mat validDescriptors(descriptorsTo.size(), descriptorsTo.type());
00586
00587 int oi=0;
00588 for(unsigned int i=0; i<kptsTo3D.size(); ++i)
00589 {
00590 if(util3d::isFinite(kptsTo3D[i]))
00591 {
00592 validKeypoints[oi] = kptsTo[i];
00593 validKeypoints3D[oi] = kptsTo3D[i];
00594 descriptorsTo.row(i).copyTo(validDescriptors.row(oi));
00595 ++oi;
00596 }
00597 }
00598 UDEBUG("Removed %d invalid 3D points", (int)kptsTo3D.size()-oi);
00599 validKeypoints.resize(oi);
00600 validKeypoints3D.resize(oi);
00601 kptsTo = validKeypoints;
00602 kptsTo3D = validKeypoints3D;
00603 descriptorsTo = validDescriptors.rowRange(0, oi).clone();
00604 }
00605 }
00606 else
00607 {
00608 kptsTo3D = uValues(toSignature.getWords3());
00609 }
00610
00611 UASSERT(kptsFrom.empty() || int(kptsFrom.size()) == descriptorsFrom.rows);
00612
00613 fromSignature.sensorData().setFeatures(kptsFrom, descriptorsFrom);
00614 toSignature.sensorData().setFeatures(kptsTo, descriptorsTo);
00615
00616 UDEBUG("descriptorsFrom=%d", descriptorsFrom.rows);
00617 UDEBUG("descriptorsTo=%d", descriptorsTo.rows);
00618
00619
00620 if(descriptorsFrom.rows > 0 && descriptorsTo.rows > 0)
00621 {
00622 cv::Size imageSize = imageTo.size();
00623 bool isCalibrated = false;
00624 if(imageSize.height == 0 || imageSize.width == 0)
00625 {
00626 imageSize = fromSignature.sensorData().cameraModels().size() == 1?fromSignature.sensorData().cameraModels()[0].imageSize():fromSignature.sensorData().stereoCameraModel().left().imageSize();
00627 }
00628 isCalibrated = imageSize.height != 0 && imageSize.width != 0 && fromSignature.sensorData().cameraModels().size()==1?fromSignature.sensorData().cameraModels()[0].isValidForProjection():fromSignature.sensorData().stereoCameraModel().isValidForProjection();
00629
00630
00631 bool guessSet = !guess.isIdentity() && !guess.isNull();
00632 if(guessSet && _guessWinSize > 0 && kptsFrom3D.size() &&
00633 isCalibrated)
00634 {
00635 UDEBUG("");
00636 UASSERT((int)kptsTo.size() == descriptorsTo.rows);
00637 UASSERT((int)kptsFrom3D.size() == descriptorsFrom.rows);
00638
00639
00640 if(fromSignature.sensorData().cameraModels().size() > 1)
00641 {
00642 UFATAL("Guess reprojection feature matching is not supported for multiple cameras.");
00643 }
00644
00645 Transform localTransform = fromSignature.sensorData().cameraModels().size()?fromSignature.sensorData().cameraModels()[0].localTransform():fromSignature.sensorData().stereoCameraModel().left().localTransform();
00646 Transform guessCameraRef = (guess * localTransform).inverse();
00647 cv::Mat R = (cv::Mat_<double>(3,3) <<
00648 (double)guessCameraRef.r11(), (double)guessCameraRef.r12(), (double)guessCameraRef.r13(),
00649 (double)guessCameraRef.r21(), (double)guessCameraRef.r22(), (double)guessCameraRef.r23(),
00650 (double)guessCameraRef.r31(), (double)guessCameraRef.r32(), (double)guessCameraRef.r33());
00651 cv::Mat rvec(1,3, CV_64FC1);
00652 cv::Rodrigues(R, rvec);
00653 cv::Mat tvec = (cv::Mat_<double>(1,3) << (double)guessCameraRef.x(), (double)guessCameraRef.y(), (double)guessCameraRef.z());
00654 cv::Mat K = fromSignature.sensorData().cameraModels().size()?fromSignature.sensorData().cameraModels()[0].K():fromSignature.sensorData().stereoCameraModel().left().K();
00655 std::vector<cv::Point2f> projected;
00656 cv::projectPoints(kptsFrom3D, rvec, tvec, K, cv::Mat(), projected);
00657
00658
00659 UASSERT((int)projected.size() == descriptorsFrom.rows);
00660 std::vector<cv::Point2f> cornersProjected(projected.size());
00661 std::vector<int> projectedIndexToDescIndex(projected.size());
00662 int oi=0;
00663 for(unsigned int i=0; i<projected.size(); ++i)
00664 {
00665 if(uIsInBounds(projected[i].x, 0.0f, float(imageSize.width-1)) &&
00666 uIsInBounds(projected[i].y, 0.0f, float(imageSize.height-1)))
00667 {
00668 projectedIndexToDescIndex[oi] = i;
00669 cornersProjected[oi++] = projected[i];
00670 }
00671 }
00672 projectedIndexToDescIndex.resize(oi);
00673 cornersProjected.resize(oi);
00674
00675
00676
00677 UDEBUG("cornersProjected=%d", (int)cornersProjected.size());
00678
00679
00680
00681
00682 if(cornersProjected.size())
00683 {
00684
00685
00686 rtflann::Matrix<float> cornersProjectedMat((float*)cornersProjected.data(), cornersProjected.size(), 2);
00687 rtflann::Index<rtflann::L2<float> > index(cornersProjectedMat, rtflann::KDTreeIndexParams());
00688 index.buildIndex();
00689
00690 std::vector< std::vector<size_t> > indices;
00691 std::vector<std::vector<float> > dists;
00692 float radius = (float)_guessWinSize;
00693 std::vector<cv::Point2f> pointsTo;
00694 cv::KeyPoint::convert(kptsTo, pointsTo);
00695 rtflann::Matrix<float> pointsToMat((float*)pointsTo.data(), pointsTo.size(), 2);
00696 index.radiusSearch(pointsToMat, indices, dists, radius*radius, rtflann::SearchParams());
00697
00698 UASSERT(indices.size() == pointsToMat.rows);
00699 UASSERT(descriptorsFrom.cols == descriptorsTo.cols);
00700 UASSERT(descriptorsFrom.rows == (int)kptsFrom.size());
00701 UASSERT((int)pointsToMat.rows == descriptorsTo.rows);
00702 UASSERT(pointsToMat.rows == kptsTo.size());
00703 UDEBUG("");
00704
00705
00706 int matchedID = descriptorsFrom.rows+descriptorsTo.rows;
00707 int newToId = descriptorsFrom.rows;
00708 int notMatchedFromId = 0;
00709 std::map<int,int> addedWordsFrom;
00710 std::map<int, int> duplicates;
00711 int newWords = 0;
00712 for(unsigned int i = 0; i < pointsToMat.rows; ++i)
00713 {
00714 if(kptsTo3D.empty() || util3d::isFinite(kptsTo3D[i]))
00715 {
00716 int octave = kptsTo[i].octave;
00717 int matchedIndex = -1;
00718 if(indices[i].size() >= 2)
00719 {
00720 cv::Mat descriptors;
00721 std::vector<int> descriptorsIndices(indices[i].size());
00722 int oi=0;
00723 for(unsigned int j=0; j<indices[i].size(); ++j)
00724 {
00725 if(kptsFrom.at(projectedIndexToDescIndex[indices[i].at(j)]).octave>=octave-1 &&
00726 kptsFrom.at(projectedIndexToDescIndex[indices[i].at(j)]).octave<=octave+1)
00727 {
00728 descriptors.push_back(descriptorsFrom.row(projectedIndexToDescIndex[indices[i].at(j)]));
00729 descriptorsIndices[oi++] = indices[i].at(j);
00730 }
00731 }
00732 descriptorsIndices.resize(oi);
00733 if(oi >=2)
00734 {
00735 std::vector<std::vector<cv::DMatch> > matches;
00736 cv::BFMatcher matcher(descriptors.type()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR);
00737 matcher.knnMatch(descriptorsTo.row(i), descriptors, matches, 2);
00738 UASSERT(matches.size() == 1);
00739 UASSERT(matches[0].size() == 2);
00740 if(matches[0].at(0).distance < _nndr * matches[0].at(1).distance)
00741 {
00742 matchedIndex = descriptorsIndices.at(matches[0].at(0).trainIdx);
00743 }
00744 }
00745 else if(oi == 1)
00746 {
00747 matchedIndex = descriptorsIndices[0];
00748 }
00749 }
00750 else if(indices[i].size() == 1 &&
00751 kptsFrom.at(projectedIndexToDescIndex[indices[i].at(0)]).octave >= octave-1 &&
00752 kptsFrom.at(projectedIndexToDescIndex[indices[i].at(0)]).octave <= octave+1)
00753 {
00754 matchedIndex = indices[i].at(0);
00755 }
00756
00757 if(matchedIndex >= 0)
00758 {
00759 matchedIndex = projectedIndexToDescIndex[matchedIndex];
00760 int id = matchedID++;
00761
00762 if(addedWordsFrom.find(matchedIndex) != addedWordsFrom.end())
00763 {
00764 id = addedWordsFrom.at(matchedIndex);
00765 duplicates.insert(std::make_pair(matchedIndex, id));
00766 }
00767 else
00768 {
00769 addedWordsFrom.insert(std::make_pair(matchedIndex, id));
00770
00771 if(kptsFrom.size())
00772 {
00773 wordsFrom.insert(std::make_pair(id, kptsFrom[matchedIndex]));
00774 }
00775 words3From.insert(std::make_pair(id, kptsFrom3D[matchedIndex]));
00776 wordsDescFrom.insert(std::make_pair(id, descriptorsFrom.row(matchedIndex)));
00777 }
00778
00779 wordsTo.insert(std::make_pair(id, kptsTo[i]));
00780 wordsDescTo.insert(std::make_pair(id, descriptorsTo.row(i)));
00781 if(kptsTo3D.size())
00782 {
00783 words3To.insert(std::make_pair(id, kptsTo3D[i]));
00784 }
00785 }
00786 else
00787 {
00788
00789 wordsTo.insert(std::make_pair(newToId, kptsTo[i]));
00790 wordsDescTo.insert(std::make_pair(newToId, descriptorsTo.row(i)));
00791 if(kptsTo3D.size())
00792 {
00793 words3To.insert(std::make_pair(newToId, kptsTo3D[i]));
00794 }
00795
00796 ++newToId;
00797 ++newWords;
00798 }
00799 }
00800 }
00801
00802 UDEBUG("addedWordsFrom=%d/%d (duplicates=%d, newWords=%d), kptsTo=%d, wordsTo=%d, words3From=%d",
00803 (int)addedWordsFrom.size(), (int)cornersProjected.size(), (int)duplicates.size(), newWords,
00804 (int)kptsTo.size(), (int)wordsTo.size(), (int)words3From.size());
00805
00806
00807 int addWordsFromNotMatched = 0;
00808 for(unsigned int i=0; i<kptsFrom3D.size(); ++i)
00809 {
00810 if(util3d::isFinite(kptsFrom3D[i]) && addedWordsFrom.find(i) == addedWordsFrom.end())
00811 {
00812 wordsFrom.insert(std::make_pair(notMatchedFromId, kptsFrom[i]));
00813 wordsDescFrom.insert(std::make_pair(notMatchedFromId, descriptorsFrom.row(i)));
00814 words3From.insert(std::make_pair(notMatchedFromId, kptsFrom3D[i]));
00815
00816 ++notMatchedFromId;
00817 ++addWordsFromNotMatched;
00818 }
00819 }
00820 UDEBUG("addWordsFromNotMatched=%d -> words3From=%d", addWordsFromNotMatched, (int)words3From.size());
00821
00822
00823
00824
00825
00826
00827
00828
00829
00830
00831
00832
00833
00834
00835
00836
00837
00838
00839
00840
00841
00842 }
00843 UDEBUG("");
00844 }
00845 else
00846 {
00847 if(guessSet && _guessWinSize > 0 && kptsFrom3D.size() && !isCalibrated)
00848 {
00849 if(fromSignature.sensorData().cameraModels().size() > 1 || toSignature.sensorData().cameraModels().size() > 1)
00850 {
00851 UWARN("Finding correspondences with the guess cannot "
00852 "be done with multiple cameras, global matching is "
00853 "done instead. Please set \"%s\" to 0 to avoid this warning.",
00854 Parameters::kVisCorGuessWinSize().c_str());
00855 }
00856 else
00857 {
00858 UWARN("Calibration not found! Finding correspondences "
00859 "with the guess cannot be done, global matching is "
00860 "done instead.");
00861 }
00862 }
00863
00864 UDEBUG("");
00865
00866 VWDictionary dictionary(_featureParameters);
00867 std::list<int> fromWordIds = dictionary.addNewWords(descriptorsFrom, 1);
00868 std::list<int> toWordIds;
00869
00870 if(descriptorsTo.rows)
00871 {
00872 dictionary.update();
00873 toWordIds = dictionary.addNewWords(descriptorsTo, 2);
00874 }
00875 dictionary.clear(false);
00876
00877 std::multiset<int> fromWordIdsSet(fromWordIds.begin(), fromWordIds.end());
00878 std::multiset<int> toWordIdsSet(toWordIds.begin(), toWordIds.end());
00879
00880 UASSERT(kptsFrom3D.empty() || fromWordIds.size() == kptsFrom3D.size());
00881 UASSERT(int(fromWordIds.size()) == descriptorsFrom.rows);
00882 int i=0;
00883 for(std::list<int>::iterator iter=fromWordIds.begin(); iter!=fromWordIds.end(); ++iter)
00884 {
00885 if(fromWordIdsSet.count(*iter) == 1)
00886 {
00887 wordsFrom.insert(std::make_pair(*iter, kptsFrom[i]));
00888 if(kptsFrom3D.size())
00889 {
00890 words3From.insert(std::make_pair(*iter, kptsFrom3D[i]));
00891 }
00892 wordsDescFrom.insert(std::make_pair(*iter, descriptorsFrom.row(i)));
00893 }
00894 ++i;
00895 }
00896 UASSERT(kptsTo3D.size() == 0 || kptsTo3D.size() == kptsTo.size());
00897 UASSERT(toWordIds.size() == kptsTo.size());
00898 UASSERT(int(toWordIds.size()) == descriptorsTo.rows);
00899 i=0;
00900 for(std::list<int>::iterator iter=toWordIds.begin(); iter!=toWordIds.end(); ++iter)
00901 {
00902 if(toWordIdsSet.count(*iter) == 1)
00903 {
00904 wordsTo.insert(std::make_pair(*iter, kptsTo[i]));
00905 wordsDescTo.insert(std::make_pair(*iter, descriptorsTo.row(i)));
00906 if(kptsTo3D.size())
00907 {
00908 words3To.insert(std::make_pair(*iter, kptsTo3D[i]));
00909 }
00910 }
00911 ++i;
00912 }
00913 }
00914 }
00915 else if(descriptorsFrom.rows)
00916 {
00917
00918 UASSERT(kptsFrom3D.empty() || int(kptsFrom3D.size()) == descriptorsFrom.rows);
00919 for(int i=0; i<descriptorsFrom.rows; ++i)
00920 {
00921 wordsFrom.insert(std::make_pair(i, kptsFrom[i]));
00922 wordsDescFrom.insert(std::make_pair(i, descriptorsFrom.row(i)));
00923 if(kptsFrom3D.size())
00924 {
00925 words3From.insert(std::make_pair(i, kptsFrom3D[i]));
00926 }
00927 }
00928 }
00929 }
00930 fromSignature.setWords(wordsFrom);
00931 fromSignature.setWords3(words3From);
00932 fromSignature.setWordsDescriptors(wordsDescFrom);
00933 toSignature.setWords(wordsTo);
00934 toSignature.setWords3(words3To);
00935 toSignature.setWordsDescriptors(wordsDescTo);
00936 delete detector;
00937 }
00938
00940
00942 Transform transform;
00943 float variance = 1.0f;
00944 int inliersCount = 0;
00945 int matchesCount = 0;
00946 if(toSignature.getWords().size())
00947 {
00948 Transform transforms[2];
00949 std::vector<int> inliers[2];
00950 std::vector<int> matches[2];
00951 double variances[2] = {1.0f};
00952 for(int dir=0; dir<(!_forwardEstimateOnly?2:1); ++dir)
00953 {
00954
00955 const Signature * signatureA;
00956 const Signature * signatureB;
00957 if(dir == 0)
00958 {
00959 signatureA = &fromSignature;
00960 signatureB = &toSignature;
00961 }
00962 else
00963 {
00964 signatureA = &toSignature;
00965 signatureB = &fromSignature;
00966 }
00967 if(_estimationType == 2)
00968 {
00969 UDEBUG("");
00970 if(!signatureB->sensorData().stereoCameraModel().isValidForProjection() &&
00971 (signatureB->sensorData().cameraModels().size() != 1 ||
00972 !signatureB->sensorData().cameraModels()[0].isValidForProjection()))
00973 {
00974 UERROR("Calibrated camera required (multi-cameras not supported).");
00975 }
00976 else if((int)signatureA->getWords().size() >= _minInliers &&
00977 (int)signatureB->getWords().size() >= _minInliers)
00978 {
00979 UASSERT(signatureA->sensorData().stereoCameraModel().isValidForProjection() || (signatureA->sensorData().cameraModels().size() == 1 && signatureA->sensorData().cameraModels()[0].isValidForProjection()));
00980 const CameraModel & cameraModel = signatureA->sensorData().stereoCameraModel().isValidForProjection()?signatureA->sensorData().stereoCameraModel().left():signatureA->sensorData().cameraModels()[0];
00981
00982
00983 Transform cameraTransform;
00984 std::map<int, cv::Point3f> inliers3D = util3d::generateWords3DMono(
00985 uMultimapToMapUnique(signatureA->getWords()),
00986 uMultimapToMapUnique(signatureB->getWords()),
00987 cameraModel,
00988 cameraTransform,
00989 _iterations,
00990 _PnPReprojError,
00991 _PnPFlags,
00992 _PnPRefineIterations,
00993 1.0f,
00994 0.99f,
00995 uMultimapToMapUnique(signatureA->getWords3()),
00996 &variances[dir]);
00997
00998 inliers[dir] = uKeys(inliers3D);
00999
01000 if(!cameraTransform.isNull())
01001 {
01002 if((int)inliers3D.size() >= _minInliers)
01003 {
01004 if(variances[dir] <= _epipolarGeometryVar)
01005 {
01006 transforms[dir] = cameraTransform;
01007 }
01008 else
01009 {
01010 msg = uFormat("Variance is too high! (max inlier distance=%f, variance=%f)", _epipolarGeometryVar, variances[dir]);
01011 UINFO(msg.c_str());
01012 }
01013 }
01014 else
01015 {
01016 msg = uFormat("Not enough inliers %d < %d", (int)inliers3D.size(), _minInliers);
01017 UINFO(msg.c_str());
01018 }
01019 }
01020 else
01021 {
01022 msg = uFormat("No camera transform found");
01023 UINFO(msg.c_str());
01024 }
01025 }
01026 else if(signatureA->getWords().size() == 0)
01027 {
01028 msg = uFormat("No enough features (%d)", (int)signatureA->getWords().size());
01029 UWARN(msg.c_str());
01030 }
01031 else
01032 {
01033 msg = uFormat("No camera model");
01034 UWARN(msg.c_str());
01035 }
01036 }
01037 else if(_estimationType == 1)
01038 {
01039 UDEBUG("");
01040 if(!signatureB->sensorData().stereoCameraModel().isValidForProjection() &&
01041 (signatureB->sensorData().cameraModels().size() != 1 ||
01042 !signatureB->sensorData().cameraModels()[0].isValidForProjection()))
01043 {
01044 UERROR("Calibrated camera required (multi-cameras not supported). Id=%d Models=%d StereoModel=%d weight=%d",
01045 signatureB->id(),
01046 (int)signatureB->sensorData().cameraModels().size(),
01047 signatureB->sensorData().stereoCameraModel().isValidForProjection()?1:0,
01048 signatureB->getWeight());
01049 }
01050 else
01051 {
01052 UDEBUG("words from3D=%d to2D=%d", (int)signatureA->getWords3().size(), (int)signatureB->getWords().size());
01053
01054 if((int)signatureA->getWords3().size() >= _minInliers &&
01055 (int)signatureB->getWords().size() >= _minInliers)
01056 {
01057 UASSERT(signatureB->sensorData().stereoCameraModel().isValidForProjection() || (signatureB->sensorData().cameraModels().size() == 1 && signatureB->sensorData().cameraModels()[0].isValidForProjection()));
01058 const CameraModel & cameraModel = signatureB->sensorData().stereoCameraModel().isValidForProjection()?signatureB->sensorData().stereoCameraModel().left():signatureB->sensorData().cameraModels()[0];
01059
01060 std::vector<int> inliersV;
01061 std::vector<int> matchesV;
01062 transforms[dir] = util3d::estimateMotion3DTo2D(
01063 uMultimapToMapUnique(signatureA->getWords3()),
01064 uMultimapToMapUnique(signatureB->getWords()),
01065 cameraModel,
01066 _minInliers,
01067 _iterations,
01068 _PnPReprojError,
01069 _PnPFlags,
01070 _PnPRefineIterations,
01071 dir==0?(!guess.isNull()?guess:Transform::getIdentity()):!transforms[0].isNull()?transforms[0].inverse():(!guess.isNull()?guess.inverse():Transform::getIdentity()),
01072 uMultimapToMapUnique(signatureB->getWords3()),
01073 varianceFromInliersCount()?0:&variances[dir],
01074 &matchesV,
01075 &inliersV);
01076 inliers[dir] = inliersV;
01077 matches[dir] = matchesV;
01078 if(transforms[dir].isNull())
01079 {
01080 msg = uFormat("Not enough inliers %d/%d (matches=%d) between %d and %d",
01081 (int)inliers[dir].size(), _minInliers, (int)matches[dir].size(), signatureA->id(), signatureB->id());
01082 UINFO(msg.c_str());
01083 }
01084 }
01085 else
01086 {
01087 msg = uFormat("Not enough features in images (old=%d, new=%d, min=%d)",
01088 (int)signatureA->getWords3().size(), (int)signatureB->getWords().size(), _minInliers);
01089 UINFO(msg.c_str());
01090 }
01091 }
01092
01093 }
01094 else
01095 {
01096 UDEBUG("");
01097
01098 if((int)signatureA->getWords3().size() >= _minInliers &&
01099 (int)signatureB->getWords3().size() >= _minInliers)
01100 {
01101 std::vector<int> inliersV;
01102 std::vector<int> matchesV;
01103 transforms[dir] = util3d::estimateMotion3DTo3D(
01104 uMultimapToMapUnique(signatureA->getWords3()),
01105 uMultimapToMapUnique(signatureB->getWords3()),
01106 _minInliers,
01107 _inlierDistance,
01108 _iterations,
01109 _refineIterations,
01110 &variances[dir],
01111 &matchesV,
01112 &inliersV);
01113 inliers[dir] = inliersV;
01114 matches[dir] = matchesV;
01115 if(transforms[dir].isNull())
01116 {
01117 msg = uFormat("Not enough inliers %d/%d between %d and %d",
01118 (int)inliers[dir].size(), _minInliers, signatureA->id(), signatureB->id());
01119 UINFO(msg.c_str());
01120 }
01121 }
01122 else
01123 {
01124 msg = uFormat("Not enough 3D features in images (old=%d, new=%d, min=%d)",
01125 (int)signatureA->getWords3().size(), (int)signatureB->getWords3().size(), _minInliers);
01126 UINFO(msg.c_str());
01127 }
01128 }
01129 }
01130
01131 if(!transforms[1].isNull())
01132 {
01133 transforms[1] = transforms[1].inverse();
01134 }
01135
01136 if(!_forwardEstimateOnly)
01137 {
01138 UDEBUG("from->to=%s", transforms[0].prettyPrint().c_str());
01139 UDEBUG("from->from=%s", transforms[1].prettyPrint().c_str());
01140 }
01141 if(!transforms[1].isNull())
01142 {
01143 if(transforms[0].isNull())
01144 {
01145 transform = transforms[1];
01146 info.inliersIDs = inliers[1];
01147 info.matchesIDs = matches[1];
01148
01149 variance = variances[1];
01150 inliersCount = (int)inliers[1].size();
01151 matchesCount = (int)matches[1].size();
01152 }
01153 else
01154 {
01155 transform = transforms[0].interpolate(0.5f, transforms[1]);
01156 info.inliersIDs = inliers[0];
01157 info.matchesIDs = matches[0];
01158
01159 variance = (variances[0]+variances[1])/2.0f;
01160 inliersCount = (int)(inliers[0].size()+inliers[1].size())/2;
01161 matchesCount = (int)(matches[0].size()+matches[1].size())/2;
01162 }
01163 }
01164 else
01165 {
01166 transform = transforms[0];
01167 info.inliersIDs = inliers[0];
01168 info.matchesIDs = matches[0];
01169
01170 variance = variances[0];
01171 inliersCount = (int)inliers[0].size();
01172 matchesCount = (int)matches[0].size();
01173 }
01174 }
01175 else if(toSignature.sensorData().isValid())
01176 {
01177 UWARN("Missing correspondences for registration. toWords = %d toImageEmpty=%d",
01178 (int)toSignature.getWords().size(), toSignature.sensorData().imageRaw().empty()?1:0);
01179 }
01180
01181 info.inliers = inliersCount;
01182 info.matches = matchesCount;
01183 info.rejectedMsg = msg;
01184 info.variance = variance>0.0f?variance:0.0001f;
01185
01186 UDEBUG("transform=%s", transform.prettyPrint().c_str());
01187 return transform;
01188 }
01189
01190 }