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/core/VisualWord.h>
00038 #include <rtabmap/core/Optimizer.h>
00039 #include <rtabmap/core/util3d_transforms.h>
00040 #include <rtabmap/utilite/ULogger.h>
00041 #include <rtabmap/utilite/UConversion.h>
00042 #include <rtabmap/utilite/UStl.h>
00043 #include <rtabmap/utilite/UTimer.h>
00044 #include <rtabmap/utilite/UMath.h>
00045
00046 #include <rtflann/flann.hpp>
00047
00048 namespace rtabmap {
00049
00050 RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration * child) :
00051 Registration(parameters, child),
00052 _minInliers(Parameters::defaultVisMinInliers()),
00053 _inlierDistance(Parameters::defaultVisInlierDistance()),
00054 _iterations(Parameters::defaultVisIterations()),
00055 _refineIterations(Parameters::defaultVisRefineIterations()),
00056 _epipolarGeometryVar(Parameters::defaultVisEpipolarGeometryVar()),
00057 _estimationType(Parameters::defaultVisEstimationType()),
00058 _forwardEstimateOnly(Parameters::defaultVisForwardEstOnly()),
00059 _PnPReprojError(Parameters::defaultVisPnPReprojError()),
00060 _PnPFlags(Parameters::defaultVisPnPFlags()),
00061 _PnPRefineIterations(Parameters::defaultVisPnPRefineIterations()),
00062 _correspondencesApproach(Parameters::defaultVisCorType()),
00063 _flowWinSize(Parameters::defaultVisCorFlowWinSize()),
00064 _flowIterations(Parameters::defaultVisCorFlowIterations()),
00065 _flowEps(Parameters::defaultVisCorFlowEps()),
00066 _flowMaxLevel(Parameters::defaultVisCorFlowMaxLevel()),
00067 _nndr(Parameters::defaultVisCorNNDR()),
00068 _guessWinSize(Parameters::defaultVisCorGuessWinSize()),
00069 _guessMatchToProjection(Parameters::defaultVisCorGuessMatchToProjection()),
00070 _bundleAdjustment(Parameters::defaultVisBundleAdjustment()),
00071 _depthAsMask(Parameters::defaultVisDepthAsMask())
00072 {
00073 _featureParameters = Parameters::getDefaultParameters();
00074 uInsert(_featureParameters, ParametersPair(Parameters::kKpNNStrategy(), _featureParameters.at(Parameters::kVisCorNNType())));
00075 uInsert(_featureParameters, ParametersPair(Parameters::kKpNndrRatio(), _featureParameters.at(Parameters::kVisCorNNDR())));
00076 uInsert(_featureParameters, ParametersPair(Parameters::kKpDetectorStrategy(), _featureParameters.at(Parameters::kVisFeatureType())));
00077 uInsert(_featureParameters, ParametersPair(Parameters::kKpMaxFeatures(), _featureParameters.at(Parameters::kVisMaxFeatures())));
00078 uInsert(_featureParameters, ParametersPair(Parameters::kKpMaxDepth(), _featureParameters.at(Parameters::kVisMaxDepth())));
00079 uInsert(_featureParameters, ParametersPair(Parameters::kKpMinDepth(), _featureParameters.at(Parameters::kVisMinDepth())));
00080 uInsert(_featureParameters, ParametersPair(Parameters::kKpRoiRatios(), _featureParameters.at(Parameters::kVisRoiRatios())));
00081 uInsert(_featureParameters, ParametersPair(Parameters::kKpSubPixEps(), _featureParameters.at(Parameters::kVisSubPixWinSize())));
00082 uInsert(_featureParameters, ParametersPair(Parameters::kKpSubPixIterations(), _featureParameters.at(Parameters::kVisSubPixIterations())));
00083 uInsert(_featureParameters, ParametersPair(Parameters::kKpSubPixWinSize(), _featureParameters.at(Parameters::kVisSubPixEps())));
00084 uInsert(_featureParameters, ParametersPair(Parameters::kKpGridRows(), _featureParameters.at(Parameters::kVisGridRows())));
00085 uInsert(_featureParameters, ParametersPair(Parameters::kKpGridCols(), _featureParameters.at(Parameters::kVisGridCols())));
00086 uInsert(_featureParameters, ParametersPair(Parameters::kKpNewWordsComparedTogether(), "false"));
00087
00088 this->parseParameters(parameters);
00089 }
00090
00091 void RegistrationVis::parseParameters(const ParametersMap & parameters)
00092 {
00093 Registration::parseParameters(parameters);
00094
00095 Parameters::parse(parameters, Parameters::kVisMinInliers(), _minInliers);
00096 Parameters::parse(parameters, Parameters::kVisInlierDistance(), _inlierDistance);
00097 Parameters::parse(parameters, Parameters::kVisIterations(), _iterations);
00098 Parameters::parse(parameters, Parameters::kVisRefineIterations(), _refineIterations);
00099 Parameters::parse(parameters, Parameters::kVisEstimationType(), _estimationType);
00100 Parameters::parse(parameters, Parameters::kVisForwardEstOnly(), _forwardEstimateOnly);
00101 Parameters::parse(parameters, Parameters::kVisEpipolarGeometryVar(), _epipolarGeometryVar);
00102 Parameters::parse(parameters, Parameters::kVisPnPReprojError(), _PnPReprojError);
00103 Parameters::parse(parameters, Parameters::kVisPnPFlags(), _PnPFlags);
00104 Parameters::parse(parameters, Parameters::kVisPnPRefineIterations(), _PnPRefineIterations);
00105 Parameters::parse(parameters, Parameters::kVisCorType(), _correspondencesApproach);
00106 Parameters::parse(parameters, Parameters::kVisCorFlowWinSize(), _flowWinSize);
00107 Parameters::parse(parameters, Parameters::kVisCorFlowIterations(), _flowIterations);
00108 Parameters::parse(parameters, Parameters::kVisCorFlowEps(), _flowEps);
00109 Parameters::parse(parameters, Parameters::kVisCorFlowMaxLevel(), _flowMaxLevel);
00110 Parameters::parse(parameters, Parameters::kVisCorNNDR(), _nndr);
00111 Parameters::parse(parameters, Parameters::kVisCorGuessWinSize(), _guessWinSize);
00112 Parameters::parse(parameters, Parameters::kVisCorGuessMatchToProjection(), _guessMatchToProjection);
00113 Parameters::parse(parameters, Parameters::kVisBundleAdjustment(), _bundleAdjustment);
00114 Parameters::parse(parameters, Parameters::kVisDepthAsMask(), _depthAsMask);
00115 uInsert(_bundleParameters, parameters);
00116
00117 UASSERT_MSG(_minInliers >= 1, uFormat("value=%d", _minInliers).c_str());
00118 UASSERT_MSG(_inlierDistance > 0.0f, uFormat("value=%f", _inlierDistance).c_str());
00119 UASSERT_MSG(_iterations > 0, uFormat("value=%d", _iterations).c_str());
00120
00121
00122 for(ParametersMap::const_iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
00123 {
00124 std::string group = uSplit(iter->first, '/').front();
00125 if(Parameters::isFeatureParameter(iter->first) || group.compare("Stereo") == 0)
00126 {
00127 uInsert(_featureParameters, ParametersPair(iter->first, iter->second));
00128 }
00129 }
00130
00131 if(uContains(parameters, Parameters::kVisCorNNType()))
00132 {
00133 uInsert(_featureParameters, ParametersPair(Parameters::kKpNNStrategy(), parameters.at(Parameters::kVisCorNNType())));
00134 }
00135 if(uContains(parameters, Parameters::kVisCorNNDR()))
00136 {
00137 uInsert(_featureParameters, ParametersPair(Parameters::kKpNndrRatio(), parameters.at(Parameters::kVisCorNNDR())));
00138 }
00139 if(uContains(parameters, Parameters::kVisFeatureType()))
00140 {
00141 uInsert(_featureParameters, ParametersPair(Parameters::kKpDetectorStrategy(), parameters.at(Parameters::kVisFeatureType())));
00142 }
00143 if(uContains(parameters, Parameters::kVisMaxFeatures()))
00144 {
00145 uInsert(_featureParameters, ParametersPair(Parameters::kKpMaxFeatures(), parameters.at(Parameters::kVisMaxFeatures())));
00146 }
00147 if(uContains(parameters, Parameters::kVisMaxDepth()))
00148 {
00149 uInsert(_featureParameters, ParametersPair(Parameters::kKpMaxDepth(), parameters.at(Parameters::kVisMaxDepth())));
00150 }
00151 if(uContains(parameters, Parameters::kVisMinDepth()))
00152 {
00153 uInsert(_featureParameters, ParametersPair(Parameters::kKpMinDepth(), parameters.at(Parameters::kVisMinDepth())));
00154 }
00155 if(uContains(parameters, Parameters::kVisRoiRatios()))
00156 {
00157 uInsert(_featureParameters, ParametersPair(Parameters::kKpRoiRatios(), parameters.at(Parameters::kVisRoiRatios())));
00158 }
00159 if(uContains(parameters, Parameters::kVisSubPixEps()))
00160 {
00161 uInsert(_featureParameters, ParametersPair(Parameters::kKpSubPixEps(), parameters.at(Parameters::kVisSubPixEps())));
00162 }
00163 if(uContains(parameters, Parameters::kVisSubPixIterations()))
00164 {
00165 uInsert(_featureParameters, ParametersPair(Parameters::kKpSubPixIterations(), parameters.at(Parameters::kVisSubPixIterations())));
00166 }
00167 if(uContains(parameters, Parameters::kVisSubPixWinSize()))
00168 {
00169 uInsert(_featureParameters, ParametersPair(Parameters::kKpSubPixWinSize(), parameters.at(Parameters::kVisSubPixWinSize())));
00170 }
00171 if(uContains(parameters, Parameters::kVisGridRows()))
00172 {
00173 uInsert(_featureParameters, ParametersPair(Parameters::kKpGridRows(), parameters.at(Parameters::kVisGridRows())));
00174 }
00175 if(uContains(parameters, Parameters::kVisGridCols()))
00176 {
00177 uInsert(_featureParameters, ParametersPair(Parameters::kKpGridCols(), parameters.at(Parameters::kVisGridCols())));
00178 }
00179 }
00180
00181 RegistrationVis::~RegistrationVis()
00182 {
00183 }
00184
00185 Feature2D * RegistrationVis::createFeatureDetector() const
00186 {
00187 return Feature2D::create(_featureParameters);
00188 }
00189
00190 Transform RegistrationVis::computeTransformationImpl(
00191 Signature & fromSignature,
00192 Signature & toSignature,
00193 Transform guess,
00194 RegistrationInfo & info) const
00195 {
00196 UDEBUG("%s=%d", Parameters::kVisMinInliers().c_str(), _minInliers);
00197 UDEBUG("%s=%f", Parameters::kVisInlierDistance().c_str(), _inlierDistance);
00198 UDEBUG("%s=%d", Parameters::kVisIterations().c_str(), _iterations);
00199 UDEBUG("%s=%d", Parameters::kVisEstimationType().c_str(), _estimationType);
00200 UDEBUG("%s=%d", Parameters::kVisForwardEstOnly().c_str(), _forwardEstimateOnly);
00201 UDEBUG("%s=%f", Parameters::kVisEpipolarGeometryVar().c_str(), _epipolarGeometryVar);
00202 UDEBUG("%s=%f", Parameters::kVisPnPReprojError().c_str(), _PnPReprojError);
00203 UDEBUG("%s=%d", Parameters::kVisPnPFlags().c_str(), _PnPFlags);
00204 UDEBUG("%s=%d", Parameters::kVisCorType().c_str(), _correspondencesApproach);
00205 UDEBUG("%s=%d", Parameters::kVisCorFlowWinSize().c_str(), _flowWinSize);
00206 UDEBUG("%s=%d", Parameters::kVisCorFlowIterations().c_str(), _flowIterations);
00207 UDEBUG("%s=%f", Parameters::kVisCorFlowEps().c_str(), _flowEps);
00208 UDEBUG("%s=%d", Parameters::kVisCorFlowMaxLevel().c_str(), _flowMaxLevel);
00209 UDEBUG("guess=%s", guess.prettyPrint().c_str());
00210
00211 UDEBUG("Input(%d): from=%d words, %d 3D words, %d words descriptors, %d kpts, %d kpts3D, %d descriptors, image=%dx%d",
00212 fromSignature.id(),
00213 (int)fromSignature.getWords().size(),
00214 (int)fromSignature.getWords3().size(),
00215 (int)fromSignature.getWordsDescriptors().size(),
00216 (int)fromSignature.sensorData().keypoints().size(),
00217 (int)fromSignature.sensorData().keypoints3D().size(),
00218 fromSignature.sensorData().descriptors().rows,
00219 fromSignature.sensorData().imageRaw().cols,
00220 fromSignature.sensorData().imageRaw().rows);
00221
00222 UDEBUG("Input(%d): to=%d words, %d 3D words, %d words descriptors, %d kpts, %d kpts3D, %d descriptors, image=%dx%d",
00223 toSignature.id(),
00224 (int)toSignature.getWords().size(),
00225 (int)toSignature.getWords3().size(),
00226 (int)toSignature.getWordsDescriptors().size(),
00227 (int)toSignature.sensorData().keypoints().size(),
00228 (int)toSignature.sensorData().keypoints3D().size(),
00229 toSignature.sensorData().descriptors().rows,
00230 toSignature.sensorData().imageRaw().cols,
00231 toSignature.sensorData().imageRaw().rows);
00232
00233 std::string msg;
00234 info.projectedIDs.clear();
00235
00237
00239
00240 if((fromSignature.getWordsDescriptors().empty() && toSignature.getWordsDescriptors().empty()) &&
00241 (_estimationType<2 || fromSignature.getWords().size()) &&
00242 (_estimationType==0 || toSignature.getWords().size()) &&
00243 fromSignature.getWords3().size() &&
00244 (_estimationType==1 || toSignature.getWords3().size()))
00245 {
00246
00247 UDEBUG("");
00248 }
00249 else
00250 {
00251 UDEBUG("");
00252
00253 UASSERT(fromSignature.getWords().empty() ||
00254 fromSignature.getWords3().empty() ||
00255 (fromSignature.getWords().size() == fromSignature.getWords3().size()));
00256 UASSERT((int)fromSignature.sensorData().keypoints().size() == fromSignature.sensorData().descriptors().rows ||
00257 fromSignature.getWords().size() == fromSignature.getWordsDescriptors().size() ||
00258 fromSignature.sensorData().descriptors().rows == 0 ||
00259 fromSignature.getWordsDescriptors().size() == 0);
00260 UASSERT((toSignature.getWords().empty() && toSignature.getWords3().empty())||
00261 (toSignature.getWords().size() && toSignature.getWords3().empty())||
00262 (toSignature.getWords().size() == toSignature.getWords3().size()));
00263 UASSERT((int)toSignature.sensorData().keypoints().size() == toSignature.sensorData().descriptors().rows ||
00264 toSignature.getWords().size() == toSignature.getWordsDescriptors().size() ||
00265 toSignature.sensorData().descriptors().rows == 0 ||
00266 toSignature.getWordsDescriptors().size() == 0);
00267 UASSERT(fromSignature.sensorData().imageRaw().empty() ||
00268 fromSignature.sensorData().imageRaw().type() == CV_8UC1 ||
00269 fromSignature.sensorData().imageRaw().type() == CV_8UC3);
00270 UASSERT(toSignature.sensorData().imageRaw().empty() ||
00271 toSignature.sensorData().imageRaw().type() == CV_8UC1 ||
00272 toSignature.sensorData().imageRaw().type() == CV_8UC3);
00273
00274 Feature2D * detector = createFeatureDetector();
00275 std::vector<cv::KeyPoint> kptsFrom;
00276 cv::Mat imageFrom = fromSignature.sensorData().imageRaw();
00277 cv::Mat imageTo = toSignature.sensorData().imageRaw();
00278
00279 std::vector<int> orignalWordsFromIds;
00280 if(fromSignature.getWords().empty())
00281 {
00282 if(fromSignature.sensorData().keypoints().empty())
00283 {
00284 if(!imageFrom.empty())
00285 {
00286 if(imageFrom.channels() > 1)
00287 {
00288 cv::Mat tmp;
00289 cv::cvtColor(imageFrom, tmp, cv::COLOR_BGR2GRAY);
00290 imageFrom = tmp;
00291 }
00292
00293 cv::Mat depthMask;
00294 if(!fromSignature.sensorData().depthRaw().empty() && _depthAsMask)
00295 {
00296 if(imageFrom.rows % fromSignature.sensorData().depthRaw().rows == 0 &&
00297 imageFrom.cols % fromSignature.sensorData().depthRaw().cols == 0 &&
00298 imageFrom.rows/fromSignature.sensorData().depthRaw().rows == fromSignature.sensorData().imageRaw().cols/fromSignature.sensorData().depthRaw().cols)
00299 {
00300 depthMask = util2d::interpolate(fromSignature.sensorData().depthRaw(), fromSignature.sensorData().imageRaw().rows/fromSignature.sensorData().depthRaw().rows, 0.1f);
00301 }
00302 }
00303
00304 kptsFrom = detector->generateKeypoints(
00305 imageFrom,
00306 depthMask);
00307 }
00308 }
00309 else
00310 {
00311 kptsFrom = fromSignature.sensorData().keypoints();
00312 }
00313 }
00314 else
00315 {
00316 kptsFrom.resize(fromSignature.getWords().size());
00317 orignalWordsFromIds.resize(fromSignature.getWords().size());
00318 int i=0;
00319 bool allUniques = true;
00320 for(std::multimap<int, cv::KeyPoint>::const_iterator iter=fromSignature.getWords().begin(); iter!=fromSignature.getWords().end(); ++iter)
00321 {
00322 kptsFrom[i] = iter->second;
00323 orignalWordsFromIds[i] = iter->first;
00324 if(i>0 && iter->first==orignalWordsFromIds[i-1])
00325 {
00326 allUniques = false;
00327 }
00328 ++i;
00329 }
00330 if(!allUniques)
00331 {
00332 UDEBUG("IDs are not unique, IDs will be regenerated!");
00333 orignalWordsFromIds.clear();
00334 }
00335 }
00336
00337 std::multimap<int, cv::KeyPoint> wordsFrom;
00338 std::multimap<int, cv::KeyPoint> wordsTo;
00339 std::multimap<int, cv::Point3f> words3From;
00340 std::multimap<int, cv::Point3f> words3To;
00341 std::multimap<int, cv::Mat> wordsDescFrom;
00342 std::multimap<int, cv::Mat> wordsDescTo;
00343 if(_correspondencesApproach == 1)
00344 {
00345 UDEBUG("");
00346
00347 if(imageFrom.channels() > 1)
00348 {
00349 cv::Mat tmp;
00350 cv::cvtColor(imageFrom, tmp, cv::COLOR_BGR2GRAY);
00351 imageFrom = tmp;
00352 }
00353 if(imageTo.channels() > 1)
00354 {
00355 cv::Mat tmp;
00356 cv::cvtColor(imageTo, tmp, cv::COLOR_BGR2GRAY);
00357 imageTo = tmp;
00358 }
00359
00360 std::vector<cv::Point3f> kptsFrom3D;
00361 if(kptsFrom.size() == fromSignature.getWords3().size())
00362 {
00363 kptsFrom3D = uValues(fromSignature.getWords3());
00364 }
00365 else if(kptsFrom.size() == fromSignature.sensorData().keypoints3D().size())
00366 {
00367 kptsFrom3D = fromSignature.sensorData().keypoints3D();
00368 }
00369 else
00370 {
00371 kptsFrom3D = detector->generateKeypoints3D(fromSignature.sensorData(), kptsFrom);
00372 }
00373
00374 if(!imageFrom.empty() && !imageTo.empty())
00375 {
00376 std::vector<cv::Point2f> cornersFrom;
00377 cv::KeyPoint::convert(kptsFrom, cornersFrom);
00378 std::vector<cv::Point2f> cornersTo;
00379 bool guessSet = !guess.isIdentity() && !guess.isNull();
00380 if(guessSet)
00381 {
00382 Transform localTransform = fromSignature.sensorData().cameraModels().size()?fromSignature.sensorData().cameraModels()[0].localTransform():fromSignature.sensorData().stereoCameraModel().left().localTransform();
00383 Transform guessCameraRef = (guess * localTransform).inverse();
00384 cv::Mat R = (cv::Mat_<double>(3,3) <<
00385 (double)guessCameraRef.r11(), (double)guessCameraRef.r12(), (double)guessCameraRef.r13(),
00386 (double)guessCameraRef.r21(), (double)guessCameraRef.r22(), (double)guessCameraRef.r23(),
00387 (double)guessCameraRef.r31(), (double)guessCameraRef.r32(), (double)guessCameraRef.r33());
00388 cv::Mat rvec(1,3, CV_64FC1);
00389 cv::Rodrigues(R, rvec);
00390 cv::Mat tvec = (cv::Mat_<double>(1,3) << (double)guessCameraRef.x(), (double)guessCameraRef.y(), (double)guessCameraRef.z());
00391 cv::Mat K = fromSignature.sensorData().cameraModels().size()?fromSignature.sensorData().cameraModels()[0].K():fromSignature.sensorData().stereoCameraModel().left().K();
00392 cv::projectPoints(kptsFrom3D, rvec, tvec, K, cv::Mat(), cornersTo);
00393 }
00394
00395
00396 UDEBUG("guessSet = %d", guessSet?1:0);
00397 std::vector<unsigned char> status;
00398 std::vector<float> err;
00399 UDEBUG("cv::calcOpticalFlowPyrLK() begin");
00400 cv::calcOpticalFlowPyrLK(
00401 imageFrom,
00402 imageTo,
00403 cornersFrom,
00404 cornersTo,
00405 status,
00406 err,
00407 cv::Size(_flowWinSize, _flowWinSize),
00408 guessSet?0:_flowMaxLevel,
00409 cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, _flowIterations, _flowEps),
00410 cv::OPTFLOW_LK_GET_MIN_EIGENVALS | (guessSet?cv::OPTFLOW_USE_INITIAL_FLOW:0), 1e-4);
00411 UDEBUG("cv::calcOpticalFlowPyrLK() end");
00412
00413 UASSERT(kptsFrom.size() == kptsFrom3D.size());
00414 std::vector<cv::KeyPoint> kptsTo(kptsFrom.size());
00415 std::vector<cv::Point3f> kptsFrom3DKept(kptsFrom3D.size());
00416 std::vector<int> orignalWordsFromIdsCpy = orignalWordsFromIds;
00417 int ki = 0;
00418 for(unsigned int i=0; i<status.size(); ++i)
00419 {
00420 if(status[i] &&
00421 uIsInBounds(cornersTo[i].x, 0.0f, float(imageTo.cols)) &&
00422 uIsInBounds(cornersTo[i].y, 0.0f, float(imageTo.rows)))
00423 {
00424 if(orignalWordsFromIdsCpy.size())
00425 {
00426 orignalWordsFromIds[ki] = orignalWordsFromIdsCpy[i];
00427 }
00428 kptsFrom[ki] = cv::KeyPoint(cornersFrom[i], 1);
00429 kptsFrom3DKept[ki] = kptsFrom3D[i];
00430 kptsTo[ki++] = cv::KeyPoint(cornersTo[i], 1);
00431 }
00432 }
00433 if(orignalWordsFromIds.size())
00434 {
00435 orignalWordsFromIds.resize(ki);
00436 }
00437 kptsFrom.resize(ki);
00438 kptsTo.resize(ki);
00439 kptsFrom3DKept.resize(ki);
00440 kptsFrom3D = kptsFrom3DKept;
00441
00442 std::vector<cv::Point3f> kptsTo3D;
00443 if(_estimationType == 0 || _estimationType == 1 || !_forwardEstimateOnly)
00444 {
00445 kptsTo3D = detector->generateKeypoints3D(toSignature.sensorData(), kptsTo);
00446 }
00447
00448 UASSERT(kptsFrom.size() == kptsFrom3DKept.size());
00449 UASSERT(kptsFrom.size() == kptsTo.size());
00450 UASSERT(kptsTo3D.size() == 0 || kptsTo.size() == kptsTo3D.size());
00451 for(unsigned int i=0; i< kptsFrom3DKept.size(); ++i)
00452 {
00453 int id = orignalWordsFromIds.size()?orignalWordsFromIds[i]:i;
00454 wordsFrom.insert(std::make_pair(id, kptsFrom[i]));
00455 words3From.insert(std::make_pair(id, kptsFrom3DKept[i]));
00456 wordsTo.insert(std::make_pair(id, kptsTo[i]));
00457 if(kptsTo3D.size())
00458 {
00459 words3To.insert(std::make_pair(id, kptsTo3D[i]));
00460 }
00461 }
00462 toSignature.sensorData().setFeatures(kptsTo, kptsTo3D, cv::Mat());
00463 }
00464 else
00465 {
00466 if(imageFrom.empty())
00467 {
00468 UERROR("Optical flow correspondences requires images in data!");
00469 }
00470 UASSERT(kptsFrom.size() == kptsFrom3D.size());
00471 for(unsigned int i=0; i< kptsFrom3D.size(); ++i)
00472 {
00473 if(util3d::isFinite(kptsFrom3D[i]))
00474 {
00475 int id = orignalWordsFromIds.size()?orignalWordsFromIds[i]:i;
00476 wordsFrom.insert(std::make_pair(id, kptsFrom[i]));
00477 words3From.insert(std::make_pair(id, kptsFrom3D[i]));
00478 }
00479 }
00480 toSignature.sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
00481 }
00482
00483 fromSignature.sensorData().setFeatures(kptsFrom, kptsFrom3D, cv::Mat());
00484 }
00485 else
00486 {
00487 UDEBUG("");
00488 std::vector<cv::KeyPoint> kptsTo;
00489 if(toSignature.getWords().empty())
00490 {
00491 if(toSignature.sensorData().keypoints().empty() &&
00492 !imageTo.empty())
00493 {
00494 if(imageTo.channels() > 1)
00495 {
00496 cv::Mat tmp;
00497 cv::cvtColor(imageTo, tmp, cv::COLOR_BGR2GRAY);
00498 imageTo = tmp;
00499 }
00500
00501 cv::Mat depthMask;
00502 if(!toSignature.sensorData().depthRaw().empty() && _depthAsMask)
00503 {
00504 if(imageTo.rows % toSignature.sensorData().depthRaw().rows == 0 &&
00505 imageTo.cols % toSignature.sensorData().depthRaw().cols == 0 &&
00506 imageTo.rows/toSignature.sensorData().depthRaw().rows == imageTo.cols/toSignature.sensorData().depthRaw().cols)
00507 {
00508 depthMask = util2d::interpolate(toSignature.sensorData().depthRaw(), imageTo.rows/toSignature.sensorData().depthRaw().rows, 0.1f);
00509 }
00510 }
00511
00512 kptsTo = detector->generateKeypoints(
00513 imageTo,
00514 depthMask);
00515 }
00516 else
00517 {
00518 kptsTo = toSignature.sensorData().keypoints();
00519 }
00520 }
00521 else
00522 {
00523 kptsTo = uValues(toSignature.getWords());
00524 }
00525
00526
00527 UDEBUG("kptsFrom=%d", (int)kptsFrom.size());
00528 UDEBUG("kptsTo=%d", (int)kptsTo.size());
00529 cv::Mat descriptorsFrom;
00530 if(fromSignature.getWordsDescriptors().size() &&
00531 ((kptsFrom.empty() && fromSignature.getWordsDescriptors().size()) ||
00532 fromSignature.getWordsDescriptors().size() == kptsFrom.size()))
00533 {
00534 descriptorsFrom = cv::Mat(fromSignature.getWordsDescriptors().size(),
00535 fromSignature.getWordsDescriptors().begin()->second.cols,
00536 fromSignature.getWordsDescriptors().begin()->second.type());
00537 int i=0;
00538 for(std::multimap<int, cv::Mat>::const_iterator iter=fromSignature.getWordsDescriptors().begin();
00539 iter!=fromSignature.getWordsDescriptors().end();
00540 ++iter, ++i)
00541 {
00542 iter->second.copyTo(descriptorsFrom.row(i));
00543 }
00544 }
00545 else if(fromSignature.sensorData().descriptors().rows == (int)kptsFrom.size())
00546 {
00547 descriptorsFrom = fromSignature.sensorData().descriptors();
00548 }
00549 else if(!imageFrom.empty())
00550 {
00551 if(imageFrom.channels() > 1)
00552 {
00553 cv::Mat tmp;
00554 cv::cvtColor(imageFrom, tmp, cv::COLOR_BGR2GRAY);
00555 imageFrom = tmp;
00556 }
00557 orignalWordsFromIds.clear();
00558 descriptorsFrom = detector->generateDescriptors(imageFrom, kptsFrom);
00559 }
00560
00561 cv::Mat descriptorsTo;
00562 if(kptsTo.size())
00563 {
00564 if(toSignature.getWordsDescriptors().size() == kptsTo.size())
00565 {
00566 descriptorsTo = cv::Mat(toSignature.getWordsDescriptors().size(),
00567 toSignature.getWordsDescriptors().begin()->second.cols,
00568 toSignature.getWordsDescriptors().begin()->second.type());
00569 int i=0;
00570 for(std::multimap<int, cv::Mat>::const_iterator iter=toSignature.getWordsDescriptors().begin();
00571 iter!=toSignature.getWordsDescriptors().end();
00572 ++iter, ++i)
00573 {
00574 iter->second.copyTo(descriptorsTo.row(i));
00575 }
00576 }
00577 else if(toSignature.sensorData().descriptors().rows == (int)kptsTo.size())
00578 {
00579 descriptorsTo = toSignature.sensorData().descriptors();
00580 }
00581 else if(!imageTo.empty())
00582 {
00583 if(imageTo.channels() > 1)
00584 {
00585 cv::Mat tmp;
00586 cv::cvtColor(imageTo, tmp, cv::COLOR_BGR2GRAY);
00587 imageTo = tmp;
00588 }
00589
00590 descriptorsTo = detector->generateDescriptors(imageTo, kptsTo);
00591 }
00592 }
00593
00594
00595 std::vector<cv::Point3f> kptsFrom3D;
00596 std::vector<cv::Point3f> kptsTo3D;
00597 if(kptsFrom.size() == fromSignature.getWords3().size())
00598 {
00599 kptsFrom3D = uValues(fromSignature.getWords3());
00600 }
00601 else if(kptsFrom.size() == fromSignature.sensorData().keypoints3D().size())
00602 {
00603 kptsFrom3D = fromSignature.sensorData().keypoints3D();
00604 }
00605 else
00606 {
00607 if(fromSignature.getWords3().size() && kptsFrom.size() != fromSignature.getWords3().size())
00608 {
00609 UWARN("kptsFrom (%d) is not the same size as fromSignature.getWords3() (%d), there "
00610 "is maybe a problem with the logic above (getWords3() should be null or equal to kptsfrom). Regenerating kptsFrom3D...",
00611 kptsFrom.size(),
00612 fromSignature.getWords3().size());
00613 }
00614 else if(fromSignature.sensorData().keypoints3D().size() && kptsFrom.size() != fromSignature.sensorData().keypoints3D().size())
00615 {
00616 UWARN("kptsFrom (%d) is not the same size as fromSignature.sensorData().keypoints3D() (%d), there "
00617 "is maybe a problem with the logic above (keypoints3D should be null or equal to kptsfrom). Regenerating kptsFrom3D...",
00618 kptsFrom.size(),
00619 fromSignature.sensorData().keypoints3D().size());
00620 }
00621 kptsFrom3D = detector->generateKeypoints3D(fromSignature.sensorData(), kptsFrom);
00622 UDEBUG("generated kptsFrom3D=%d", (int)kptsFrom3D.size());
00623 if(detector->getMinDepth() > 0.0f || detector->getMaxDepth() > 0.0f)
00624 {
00625
00626 UASSERT((int)kptsFrom.size() == descriptorsFrom.rows &&
00627 kptsFrom3D.size() == kptsFrom.size());
00628 std::vector<cv::KeyPoint> validKeypoints(kptsFrom.size());
00629 std::vector<cv::Point3f> validKeypoints3D(kptsFrom.size());
00630 cv::Mat validDescriptors(descriptorsFrom.size(), descriptorsFrom.type());
00631 std::vector<int> validKeypointsIds;
00632 if(orignalWordsFromIds.size())
00633 {
00634 validKeypointsIds.resize(kptsFrom.size());
00635 }
00636
00637 int oi=0;
00638 for(unsigned int i=0; i<kptsFrom3D.size(); ++i)
00639 {
00640 if(util3d::isFinite(kptsFrom3D[i]))
00641 {
00642 validKeypoints[oi] = kptsFrom[i];
00643 validKeypoints3D[oi] = kptsFrom3D[i];
00644 if(orignalWordsFromIds.size())
00645 {
00646 validKeypointsIds[oi] = orignalWordsFromIds[i];
00647 }
00648 descriptorsFrom.row(i).copyTo(validDescriptors.row(oi));
00649 ++oi;
00650 }
00651 }
00652 UDEBUG("Removed %d invalid 3D points", (int)kptsFrom3D.size()-oi);
00653 validKeypoints.resize(oi);
00654 validKeypoints3D.resize(oi);
00655 kptsFrom = validKeypoints;
00656 kptsFrom3D = validKeypoints3D;
00657
00658 if(orignalWordsFromIds.size())
00659 {
00660 validKeypointsIds.resize(oi);
00661 orignalWordsFromIds = validKeypointsIds;
00662 }
00663 descriptorsFrom = validDescriptors.rowRange(0, oi).clone();
00664 }
00665 }
00666
00667 if(kptsTo.size() == toSignature.getWords3().size())
00668 {
00669 kptsTo3D = uValues(toSignature.getWords3());
00670 }
00671 else if(kptsTo.size() == toSignature.sensorData().keypoints3D().size())
00672 {
00673 kptsTo3D = toSignature.sensorData().keypoints3D();
00674 }
00675 else
00676 {
00677 if(toSignature.getWords3().size() && kptsTo.size() != toSignature.getWords3().size())
00678 {
00679 UWARN("kptsTo (%d) is not the same size as toSignature.getWords3() (%d), there "
00680 "is maybe a problem with the logic above (getWords3() should be null or equal to kptsTo). Regenerating kptsTo3D...",
00681 (int)kptsTo.size(),
00682 (int)toSignature.getWords3().size());
00683 }
00684 else if(toSignature.sensorData().keypoints3D().size() && kptsTo.size() != toSignature.sensorData().keypoints3D().size())
00685 {
00686 UWARN("kptsTo (%d) is not the same size as toSignature.sensorData().keypoints3D() (%d), there "
00687 "is maybe a problem with the logic above (keypoints3D() should be null or equal to kptsTo). Regenerating kptsTo3D...",
00688 (int)kptsTo.size(),
00689 (int)toSignature.sensorData().keypoints3D().size());
00690 }
00691 kptsTo3D = detector->generateKeypoints3D(toSignature.sensorData(), kptsTo);
00692 if(kptsTo3D.size() && (detector->getMinDepth() > 0.0f || detector->getMaxDepth() > 0.0f))
00693 {
00694 UDEBUG("");
00695
00696 UASSERT((int)kptsTo.size() == descriptorsTo.rows &&
00697 kptsTo3D.size() == kptsTo.size());
00698 std::vector<cv::KeyPoint> validKeypoints(kptsTo.size());
00699 std::vector<cv::Point3f> validKeypoints3D(kptsTo.size());
00700 cv::Mat validDescriptors(descriptorsTo.size(), descriptorsTo.type());
00701
00702 int oi=0;
00703 for(unsigned int i=0; i<kptsTo3D.size(); ++i)
00704 {
00705 if(util3d::isFinite(kptsTo3D[i]))
00706 {
00707 validKeypoints[oi] = kptsTo[i];
00708 validKeypoints3D[oi] = kptsTo3D[i];
00709 descriptorsTo.row(i).copyTo(validDescriptors.row(oi));
00710 ++oi;
00711 }
00712 }
00713 UDEBUG("Removed %d invalid 3D points", (int)kptsTo3D.size()-oi);
00714 validKeypoints.resize(oi);
00715 validKeypoints3D.resize(oi);
00716 kptsTo = validKeypoints;
00717 kptsTo3D = validKeypoints3D;
00718 descriptorsTo = validDescriptors.rowRange(0, oi).clone();
00719 }
00720 }
00721
00722 UASSERT(kptsFrom.empty() || descriptorsFrom.rows == 0 || int(kptsFrom.size()) == descriptorsFrom.rows);
00723
00724 fromSignature.sensorData().setFeatures(kptsFrom, kptsFrom3D, descriptorsFrom);
00725 toSignature.sensorData().setFeatures(kptsTo, kptsTo3D, descriptorsTo);
00726
00727 UDEBUG("descriptorsFrom=%d", descriptorsFrom.rows);
00728 UDEBUG("descriptorsTo=%d", descriptorsTo.rows);
00729
00730
00731 if(descriptorsFrom.rows > 0 && descriptorsTo.rows > 0)
00732 {
00733 cv::Size imageSize = imageTo.size();
00734 bool isCalibrated = false;
00735 if(imageSize.height == 0 || imageSize.width == 0)
00736 {
00737 imageSize = toSignature.sensorData().cameraModels().size() == 1?toSignature.sensorData().cameraModels()[0].imageSize():toSignature.sensorData().stereoCameraModel().left().imageSize();
00738 }
00739
00740 isCalibrated = imageSize.height != 0 && imageSize.width != 0 &&
00741 (toSignature.sensorData().cameraModels().size()==1?toSignature.sensorData().cameraModels()[0].isValidForProjection():toSignature.sensorData().stereoCameraModel().isValidForProjection());
00742
00743
00744 bool guessSet = !guess.isIdentity() && !guess.isNull();
00745 if(guessSet && _guessWinSize > 0 && kptsFrom3D.size() &&
00746 isCalibrated)
00747 {
00748 UDEBUG("");
00749 UASSERT((int)kptsTo.size() == descriptorsTo.rows);
00750 UASSERT((int)kptsFrom3D.size() == descriptorsFrom.rows);
00751
00752
00753 if(toSignature.sensorData().cameraModels().size() > 1)
00754 {
00755 UFATAL("Guess reprojection feature matching is not supported for multiple cameras.");
00756 }
00757
00758 Transform localTransform = toSignature.sensorData().cameraModels().size()?toSignature.sensorData().cameraModels()[0].localTransform():toSignature.sensorData().stereoCameraModel().left().localTransform();
00759 Transform guessCameraRef = (guess * localTransform).inverse();
00760 cv::Mat R = (cv::Mat_<double>(3,3) <<
00761 (double)guessCameraRef.r11(), (double)guessCameraRef.r12(), (double)guessCameraRef.r13(),
00762 (double)guessCameraRef.r21(), (double)guessCameraRef.r22(), (double)guessCameraRef.r23(),
00763 (double)guessCameraRef.r31(), (double)guessCameraRef.r32(), (double)guessCameraRef.r33());
00764 cv::Mat rvec(1,3, CV_64FC1);
00765 cv::Rodrigues(R, rvec);
00766 cv::Mat tvec = (cv::Mat_<double>(1,3) << (double)guessCameraRef.x(), (double)guessCameraRef.y(), (double)guessCameraRef.z());
00767 cv::Mat K = toSignature.sensorData().cameraModels().size()?toSignature.sensorData().cameraModels()[0].K():toSignature.sensorData().stereoCameraModel().left().K();
00768 std::vector<cv::Point2f> projected;
00769 cv::projectPoints(kptsFrom3D, rvec, tvec, K, cv::Mat(), projected);
00770 UDEBUG("Projected points=%d", (int)projected.size());
00771
00772 UASSERT((int)projected.size() == descriptorsFrom.rows);
00773 std::vector<cv::Point2f> cornersProjected(projected.size());
00774 std::vector<int> projectedIndexToDescIndex(projected.size());
00775 int oi=0;
00776 for(unsigned int i=0; i<projected.size(); ++i)
00777 {
00778 if(uIsInBounds(projected[i].x, 0.0f, float(imageSize.width-1)) &&
00779 uIsInBounds(projected[i].y, 0.0f, float(imageSize.height-1)) &&
00780 util3d::transformPoint(kptsFrom3D[i], guessCameraRef).z > 0.0)
00781 {
00782 projectedIndexToDescIndex[oi] = i;
00783 cornersProjected[oi++] = projected[i];
00784 }
00785 }
00786 projectedIndexToDescIndex.resize(oi);
00787 cornersProjected.resize(oi);
00788 UDEBUG("corners in frame=%d", (int)cornersProjected.size());
00789
00790
00791
00792
00793 if(cornersProjected.size())
00794 {
00795 if(_guessMatchToProjection)
00796 {
00797
00798
00799 rtflann::Matrix<float> cornersProjectedMat((float*)cornersProjected.data(), cornersProjected.size(), 2);
00800 rtflann::Index<rtflann::L2_Simple<float> > index(cornersProjectedMat, rtflann::KDTreeIndexParams());
00801 index.buildIndex();
00802
00803 std::vector< std::vector<size_t> > indices;
00804 std::vector<std::vector<float> > dists;
00805 float radius = (float)_guessWinSize;
00806 std::vector<cv::Point2f> pointsTo;
00807 cv::KeyPoint::convert(kptsTo, pointsTo);
00808 rtflann::Matrix<float> pointsToMat((float*)pointsTo.data(), pointsTo.size(), 2);
00809 index.radiusSearch(pointsToMat, indices, dists, radius*radius, rtflann::SearchParams());
00810
00811 UASSERT(indices.size() == pointsToMat.rows);
00812 UASSERT(descriptorsFrom.cols == descriptorsTo.cols);
00813 UASSERT(descriptorsFrom.rows == (int)kptsFrom.size());
00814 UASSERT((int)pointsToMat.rows == descriptorsTo.rows);
00815 UASSERT(pointsToMat.rows == kptsTo.size());
00816 UDEBUG("radius search done for guess");
00817
00818
00819 int newToId = orignalWordsFromIds.size()?orignalWordsFromIds.back():descriptorsFrom.rows;
00820 std::map<int,int> addedWordsFrom;
00821 std::map<int, int> duplicates;
00822 int newWords = 0;
00823 cv::Mat descriptors(10, descriptorsTo.cols, descriptorsTo.type());
00824 for(unsigned int i = 0; i < pointsToMat.rows; ++i)
00825 {
00826 if(kptsTo3D.empty() || util3d::isFinite(kptsTo3D[i]))
00827 {
00828 int octave = kptsTo[i].octave;
00829 int matchedIndex = -1;
00830 if(indices[i].size() >= 2)
00831 {
00832 std::vector<int> descriptorsIndices(indices[i].size());
00833 int oi=0;
00834 if((int)indices[i].size() > descriptors.rows)
00835 {
00836 descriptors.resize(indices[i].size());
00837 }
00838 for(unsigned int j=0; j<indices[i].size(); ++j)
00839 {
00840 if(kptsFrom.at(projectedIndexToDescIndex[indices[i].at(j)]).octave==octave)
00841 {
00842 descriptorsFrom.row(projectedIndexToDescIndex[indices[i].at(j)]).copyTo(descriptors.row(oi));
00843 descriptorsIndices[oi++] = indices[i].at(j);
00844 }
00845 }
00846 descriptorsIndices.resize(oi);
00847 if(oi >=2)
00848 {
00849 std::vector<std::vector<cv::DMatch> > matches;
00850 cv::BFMatcher matcher(descriptors.type()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR);
00851 matcher.knnMatch(descriptorsTo.row(i), cv::Mat(descriptors, cv::Range(0, oi)), matches, 2);
00852 UASSERT(matches.size() == 1);
00853 UASSERT(matches[0].size() == 2);
00854 if(matches[0].at(0).distance < _nndr * matches[0].at(1).distance)
00855 {
00856 matchedIndex = descriptorsIndices.at(matches[0].at(0).trainIdx);
00857 }
00858 }
00859 else if(oi == 1)
00860 {
00861 matchedIndex = descriptorsIndices[0];
00862 }
00863 }
00864 else if(indices[i].size() == 1 &&
00865 kptsFrom.at(projectedIndexToDescIndex[indices[i].at(0)]).octave == octave)
00866 {
00867 matchedIndex = indices[i].at(0);
00868 }
00869
00870 if(matchedIndex >= 0)
00871 {
00872 matchedIndex = projectedIndexToDescIndex[matchedIndex];
00873 int id = orignalWordsFromIds.size()?orignalWordsFromIds[matchedIndex]:matchedIndex;
00874
00875 if(addedWordsFrom.find(matchedIndex) != addedWordsFrom.end())
00876 {
00877 id = addedWordsFrom.at(matchedIndex);
00878 duplicates.insert(std::make_pair(matchedIndex, id));
00879 }
00880 else
00881 {
00882 addedWordsFrom.insert(std::make_pair(matchedIndex, id));
00883
00884 if(kptsFrom.size())
00885 {
00886 wordsFrom.insert(std::make_pair(id, kptsFrom[matchedIndex]));
00887 }
00888 words3From.insert(std::make_pair(id, kptsFrom3D[matchedIndex]));
00889 wordsDescFrom.insert(std::make_pair(id, descriptorsFrom.row(matchedIndex)));
00890 }
00891
00892 wordsTo.insert(std::make_pair(id, kptsTo[i]));
00893 wordsDescTo.insert(std::make_pair(id, descriptorsTo.row(i)));
00894 if(kptsTo3D.size())
00895 {
00896 words3To.insert(std::make_pair(id, kptsTo3D[i]));
00897 }
00898 }
00899 else
00900 {
00901
00902 wordsTo.insert(wordsTo.end(), std::make_pair(newToId, kptsTo[i]));
00903 wordsDescTo.insert(wordsDescTo.end(), std::make_pair(newToId, descriptorsTo.row(i)));
00904 if(kptsTo3D.size())
00905 {
00906 words3To.insert(words3To.end(), std::make_pair(newToId, kptsTo3D[i]));
00907 }
00908
00909 ++newToId;
00910 ++newWords;
00911 }
00912 }
00913 }
00914 UDEBUG("addedWordsFrom=%d/%d (duplicates=%d, newWords=%d), kptsTo=%d, wordsTo=%d, words3From=%d",
00915 (int)addedWordsFrom.size(), (int)cornersProjected.size(), (int)duplicates.size(), newWords,
00916 (int)kptsTo.size(), (int)wordsTo.size(), (int)words3From.size());
00917
00918
00919 int addWordsFromNotMatched = 0;
00920 for(unsigned int i=0; i<kptsFrom3D.size(); ++i)
00921 {
00922 if(util3d::isFinite(kptsFrom3D[i]) && addedWordsFrom.find(i) == addedWordsFrom.end())
00923 {
00924 int id = orignalWordsFromIds.size()?orignalWordsFromIds[i]:i;
00925 wordsFrom.insert(wordsFrom.end(), std::make_pair(id, kptsFrom[i]));
00926 wordsDescFrom.insert(wordsDescFrom.end(), std::make_pair(id, descriptorsFrom.row(i)));
00927 words3From.insert(words3From.end(), std::make_pair(id, kptsFrom3D[i]));
00928
00929 ++addWordsFromNotMatched;
00930 }
00931 }
00932 UDEBUG("addWordsFromNotMatched=%d -> words3From=%d", addWordsFromNotMatched, (int)words3From.size());
00933 }
00934 else
00935 {
00936
00937 std::vector<cv::Point2f> pointsTo;
00938 cv::KeyPoint::convert(kptsTo, pointsTo);
00939 rtflann::Matrix<float> pointsToMat((float*)pointsTo.data(), pointsTo.size(), 2);
00940 rtflann::Index<rtflann::L2_Simple<float> > index(pointsToMat, rtflann::KDTreeIndexParams());
00941 index.buildIndex();
00942
00943 std::vector< std::vector<size_t> > indices;
00944 std::vector<std::vector<float> > dists;
00945 float radius = (float)_guessWinSize;
00946 rtflann::Matrix<float> cornersProjectedMat((float*)cornersProjected.data(), cornersProjected.size(), 2);
00947 index.radiusSearch(cornersProjectedMat, indices, dists, radius*radius, rtflann::SearchParams());
00948
00949 UASSERT(indices.size() == cornersProjectedMat.rows);
00950 UASSERT(descriptorsFrom.cols == descriptorsTo.cols);
00951 UASSERT(descriptorsFrom.rows == (int)kptsFrom.size());
00952 UASSERT((int)pointsToMat.rows == descriptorsTo.rows);
00953 UASSERT(pointsToMat.rows == kptsTo.size());
00954 UDEBUG("radius search done for guess");
00955
00956
00957 std::set<int> addedWordsTo;
00958 std::set<int> addedWordsFrom;
00959 std::set<int> indicesToIgnore;
00960 double bruteForceTotalTime = 0.0;
00961 double bruteForceDescCopy = 0.0;
00962 UTimer bruteForceTimer;
00963 cv::Mat descriptors(10, descriptorsTo.cols, descriptorsTo.type());
00964 for(unsigned int i = 0; i < cornersProjectedMat.rows; ++i)
00965 {
00966 int matchedIndexFrom = projectedIndexToDescIndex[i];
00967
00968 if(indices[i].size())
00969 {
00970 info.projectedIDs.push_back(orignalWordsFromIds.size()?orignalWordsFromIds[matchedIndexFrom]:matchedIndexFrom);
00971 }
00972
00973 if(util3d::isFinite(kptsFrom3D[matchedIndexFrom]))
00974 {
00975 int matchedIndexTo = -1;
00976 if(indices[i].size() >= 2)
00977 {
00978 bruteForceTimer.restart();
00979 std::vector<int> descriptorsIndices(indices[i].size());
00980 int oi=0;
00981 if((int)indices[i].size() > descriptors.rows)
00982 {
00983 descriptors.resize(indices[i].size());
00984 }
00985 std::list<int> indicesToIgnoretmp;
00986 for(unsigned int j=0; j<indices[i].size(); ++j)
00987 {
00988 int octave = kptsTo[indices[i].at(j)].octave;
00989 if(kptsFrom.at(matchedIndexFrom).octave==octave)
00990 {
00991 descriptorsTo.row(indices[i].at(j)).copyTo(descriptors.row(oi));
00992 descriptorsIndices[oi++] = indices[i].at(j);
00993
00994 indicesToIgnoretmp.push_back(indices[i].at(j));
00995 }
00996 }
00997 bruteForceDescCopy += bruteForceTimer.ticks();
00998 if(oi >=2)
00999 {
01000 std::vector<std::vector<cv::DMatch> > matches;
01001 cv::BFMatcher matcher(descriptors.type()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR);
01002 matcher.knnMatch(descriptorsFrom.row(matchedIndexFrom), cv::Mat(descriptors, cv::Range(0, oi)), matches, 2);
01003 UASSERT(matches.size() == 1);
01004 UASSERT(matches[0].size() == 2);
01005 bruteForceTotalTime+=bruteForceTimer.elapsed();
01006 if(matches[0].at(0).distance < _nndr * matches[0].at(1).distance)
01007 {
01008 matchedIndexTo = descriptorsIndices.at(matches[0].at(0).trainIdx);
01009
01010 indicesToIgnore.insert(indicesToIgnore.begin(), indicesToIgnore.end());
01011 }
01012 }
01013 else if(oi == 1)
01014 {
01015 matchedIndexTo = descriptorsIndices[0];
01016 }
01017 }
01018 else if(indices[i].size() == 1)
01019 {
01020 int octave = kptsTo[indices[i].at(0)].octave;
01021 if(kptsFrom.at(matchedIndexFrom).octave == octave)
01022 {
01023 matchedIndexTo = indices[i].at(0);
01024 }
01025 }
01026
01027 int id = orignalWordsFromIds.size()?orignalWordsFromIds[matchedIndexFrom]:matchedIndexFrom;
01028 addedWordsFrom.insert(addedWordsFrom.end(), matchedIndexFrom);
01029
01030 if(kptsFrom.size())
01031 {
01032 wordsFrom.insert(wordsFrom.end(), std::make_pair(id, kptsFrom[matchedIndexFrom]));
01033 }
01034 words3From.insert(words3From.end(), std::make_pair(id, kptsFrom3D[matchedIndexFrom]));
01035 wordsDescFrom.insert(wordsDescFrom.end(), std::make_pair(id, descriptorsFrom.row(matchedIndexFrom)));
01036
01037 if((kptsTo3D.empty() || util3d::isFinite(kptsTo3D[matchedIndexTo])) &&
01038 matchedIndexTo >= 0 &&
01039 addedWordsTo.find(matchedIndexTo) == addedWordsTo.end())
01040 {
01041 addedWordsTo.insert(matchedIndexTo);
01042
01043 wordsTo.insert(wordsTo.end(), std::make_pair(id, kptsTo[matchedIndexTo]));
01044 wordsDescTo.insert(wordsDescTo.end(), std::make_pair(id, descriptorsTo.row(matchedIndexTo)));
01045 if(kptsTo3D.size())
01046 {
01047 words3To.insert(words3To.end(), std::make_pair(id, kptsTo3D[matchedIndexTo]));
01048 }
01049 }
01050 }
01051 }
01052 UDEBUG("bruteForceDescCopy=%fs, bruteForceTotalTime=%fs", bruteForceDescCopy, bruteForceTotalTime);
01053
01054
01055 for(unsigned int i=0; i<kptsFrom3D.size(); ++i)
01056 {
01057 if(util3d::isFinite(kptsFrom3D[i]) && addedWordsFrom.find(i) == addedWordsFrom.end())
01058 {
01059 int id = orignalWordsFromIds.size()?orignalWordsFromIds[i]:i;
01060 wordsFrom.insert(wordsFrom.end(), std::make_pair(id, kptsFrom[i]));
01061 wordsDescFrom.insert(wordsDescFrom.end(), std::make_pair(id, descriptorsFrom.row(i)));
01062 words3From.insert(words3From.end(), std::make_pair(id, kptsFrom3D[i]));
01063 }
01064 }
01065
01066 int newToId = orignalWordsFromIds.size()?orignalWordsFromIds.back():descriptorsFrom.rows;
01067 for(unsigned int i = 0; i < kptsTo.size(); ++i)
01068 {
01069 if(addedWordsTo.find(i) == addedWordsTo.end() && indicesToIgnore.find(i) == indicesToIgnore.end())
01070 {
01071 wordsTo.insert(wordsTo.end(), std::make_pair(newToId, kptsTo[i]));
01072 wordsDescTo.insert(wordsDescTo.end(), std::make_pair(newToId, descriptorsTo.row(i)));
01073 if(kptsTo3D.size())
01074 {
01075 words3To.insert(words3To.end(), std::make_pair(newToId, kptsTo3D[i]));
01076 }
01077 ++newToId;
01078 }
01079 }
01080 }
01081 }
01082 else
01083 {
01084 UWARN("All projected points are outside the camera. Guess (%s) is wrong or images are not overlapping.", guess.prettyPrint().c_str());
01085 }
01086 UDEBUG("");
01087 }
01088 else
01089 {
01090 if(guessSet && _guessWinSize > 0 && kptsFrom3D.size() && !isCalibrated)
01091 {
01092 if(fromSignature.sensorData().cameraModels().size() > 1 || toSignature.sensorData().cameraModels().size() > 1)
01093 {
01094 UWARN("Finding correspondences with the guess cannot "
01095 "be done with multiple cameras, global matching is "
01096 "done instead. Please set \"%s\" to 0 to avoid this warning.",
01097 Parameters::kVisCorGuessWinSize().c_str());
01098 }
01099 else
01100 {
01101 UWARN("Calibration not found! Finding correspondences "
01102 "with the guess cannot be done, global matching is "
01103 "done instead.");
01104 }
01105 }
01106
01107 UDEBUG("");
01108
01109 VWDictionary dictionary(_featureParameters);
01110 std::list<int> fromWordIds;
01111 for (int i = 0; i < descriptorsFrom.rows; ++i)
01112 {
01113 int id = orignalWordsFromIds.size() ? orignalWordsFromIds[i] : i;
01114 dictionary.addWord(new VisualWord(id, descriptorsFrom.row(i), 1));
01115 fromWordIds.push_back(id);
01116 }
01117
01118 std::list<int> toWordIds;
01119 if(descriptorsTo.rows)
01120 {
01121 dictionary.update();
01122 toWordIds = dictionary.addNewWords(descriptorsTo, 2);
01123 }
01124 dictionary.clear(false);
01125
01126 std::multiset<int> fromWordIdsSet(fromWordIds.begin(), fromWordIds.end());
01127 std::multiset<int> toWordIdsSet(toWordIds.begin(), toWordIds.end());
01128
01129 UASSERT(kptsFrom3D.empty() || fromWordIds.size() == kptsFrom3D.size());
01130 UASSERT(int(fromWordIds.size()) == descriptorsFrom.rows);
01131 int i=0;
01132 for(std::list<int>::iterator iter=fromWordIds.begin(); iter!=fromWordIds.end(); ++iter)
01133 {
01134 if(fromWordIdsSet.count(*iter) == 1)
01135 {
01136 if (kptsFrom.size())
01137 {
01138 wordsFrom.insert(std::make_pair(*iter, kptsFrom[i]));
01139 }
01140 if(kptsFrom3D.size())
01141 {
01142 words3From.insert(std::make_pair(*iter, kptsFrom3D[i]));
01143 }
01144 wordsDescFrom.insert(std::make_pair(*iter, descriptorsFrom.row(i)));
01145 }
01146 ++i;
01147 }
01148 UASSERT(kptsTo3D.size() == 0 || kptsTo3D.size() == kptsTo.size());
01149 UASSERT(toWordIds.size() == kptsTo.size());
01150 UASSERT(int(toWordIds.size()) == descriptorsTo.rows);
01151 i=0;
01152 for(std::list<int>::iterator iter=toWordIds.begin(); iter!=toWordIds.end(); ++iter)
01153 {
01154 if(toWordIdsSet.count(*iter) == 1)
01155 {
01156 wordsTo.insert(std::make_pair(*iter, kptsTo[i]));
01157 wordsDescTo.insert(std::make_pair(*iter, descriptorsTo.row(i)));
01158 if(kptsTo3D.size())
01159 {
01160 words3To.insert(std::make_pair(*iter, kptsTo3D[i]));
01161 }
01162 }
01163 ++i;
01164 }
01165 }
01166 }
01167 else if(descriptorsFrom.rows)
01168 {
01169
01170 UASSERT(kptsFrom3D.empty() || int(kptsFrom3D.size()) == descriptorsFrom.rows);
01171 for(int i=0; i<descriptorsFrom.rows; ++i)
01172 {
01173 wordsFrom.insert(std::make_pair(i, kptsFrom[i]));
01174 wordsDescFrom.insert(std::make_pair(i, descriptorsFrom.row(i)));
01175 if(kptsFrom3D.size())
01176 {
01177 words3From.insert(std::make_pair(i, kptsFrom3D[i]));
01178 }
01179 }
01180 }
01181 }
01182 fromSignature.setWords(wordsFrom);
01183 fromSignature.setWords3(words3From);
01184 fromSignature.setWordsDescriptors(wordsDescFrom);
01185 toSignature.setWords(wordsTo);
01186 toSignature.setWords3(words3To);
01187 toSignature.setWordsDescriptors(wordsDescTo);
01188 delete detector;
01189 }
01190
01192
01194 Transform transform;
01195 cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
01196 int inliersCount = 0;
01197 int matchesCount = 0;
01198 info.inliersIDs.clear();
01199 info.matchesIDs.clear();
01200 if(toSignature.getWords().size())
01201 {
01202 Transform transforms[2];
01203 std::vector<int> inliers[2];
01204 std::vector<int> matches[2];
01205 cv::Mat covariances[2];
01206 covariances[0] = cv::Mat::eye(6,6,CV_64FC1);
01207 covariances[1] = cv::Mat::eye(6,6,CV_64FC1);
01208 for(int dir=0; dir<(!_forwardEstimateOnly?2:1); ++dir)
01209 {
01210
01211 Signature * signatureA;
01212 Signature * signatureB;
01213 if(dir == 0)
01214 {
01215 signatureA = &fromSignature;
01216 signatureB = &toSignature;
01217 }
01218 else
01219 {
01220 signatureA = &toSignature;
01221 signatureB = &fromSignature;
01222 }
01223 if(_estimationType == 2)
01224 {
01225 UDEBUG("");
01226 if(!signatureB->sensorData().stereoCameraModel().isValidForProjection() &&
01227 (signatureB->sensorData().cameraModels().size() != 1 ||
01228 !signatureB->sensorData().cameraModels()[0].isValidForProjection()))
01229 {
01230 UERROR("Calibrated camera required (multi-cameras not supported).");
01231 }
01232 else if((int)signatureA->getWords().size() >= _minInliers &&
01233 (int)signatureB->getWords().size() >= _minInliers)
01234 {
01235 UASSERT(signatureA->sensorData().stereoCameraModel().isValidForProjection() || (signatureA->sensorData().cameraModels().size() == 1 && signatureA->sensorData().cameraModels()[0].isValidForProjection()));
01236 const CameraModel & cameraModel = signatureA->sensorData().stereoCameraModel().isValidForProjection()?signatureA->sensorData().stereoCameraModel().left():signatureA->sensorData().cameraModels()[0];
01237
01238
01239 Transform cameraTransform;
01240 double variance = 1.0f;
01241 std::map<int, cv::Point3f> inliers3D = util3d::generateWords3DMono(
01242 uMultimapToMapUnique(signatureA->getWords()),
01243 uMultimapToMapUnique(signatureB->getWords()),
01244 cameraModel,
01245 cameraTransform,
01246 _iterations,
01247 _PnPReprojError,
01248 _PnPFlags,
01249 _PnPRefineIterations,
01250 1.0f,
01251 0.99f,
01252 uMultimapToMapUnique(signatureA->getWords3()),
01253 &variance);
01254 covariances[dir] *= variance;
01255 inliers[dir] = uKeys(inliers3D);
01256
01257 if(!cameraTransform.isNull())
01258 {
01259 if((int)inliers3D.size() >= _minInliers)
01260 {
01261 if(variance <= _epipolarGeometryVar)
01262 {
01263 if(this->force3DoF())
01264 {
01265 transforms[dir] = cameraTransform.to3DoF();
01266 }
01267 else
01268 {
01269 transforms[dir] = cameraTransform;
01270 }
01271 }
01272 else
01273 {
01274 msg = uFormat("Variance is too high! (max inlier distance=%f, variance=%f)", _epipolarGeometryVar, variance);
01275 UINFO(msg.c_str());
01276 }
01277 }
01278 else
01279 {
01280 msg = uFormat("Not enough inliers %d < %d", (int)inliers3D.size(), _minInliers);
01281 UINFO(msg.c_str());
01282 }
01283 }
01284 else
01285 {
01286 msg = uFormat("No camera transform found");
01287 UINFO(msg.c_str());
01288 }
01289 }
01290 else if(signatureA->getWords().size() == 0)
01291 {
01292 msg = uFormat("No enough features (%d)", (int)signatureA->getWords().size());
01293 UWARN(msg.c_str());
01294 }
01295 else
01296 {
01297 msg = uFormat("No camera model");
01298 UWARN(msg.c_str());
01299 }
01300 }
01301 else if(_estimationType == 1)
01302 {
01303 UDEBUG("");
01304 if(!signatureB->sensorData().stereoCameraModel().isValidForProjection() &&
01305 (signatureB->sensorData().cameraModels().size() != 1 ||
01306 !signatureB->sensorData().cameraModels()[0].isValidForProjection()))
01307 {
01308 UERROR("Calibrated camera required (multi-cameras not supported). Id=%d Models=%d StereoModel=%d weight=%d",
01309 signatureB->id(),
01310 (int)signatureB->sensorData().cameraModels().size(),
01311 signatureB->sensorData().stereoCameraModel().isValidForProjection()?1:0,
01312 signatureB->getWeight());
01313 }
01314 else
01315 {
01316 UDEBUG("words from3D=%d to2D=%d", (int)signatureA->getWords3().size(), (int)signatureB->getWords().size());
01317
01318 if((int)signatureA->getWords3().size() >= _minInliers &&
01319 (int)signatureB->getWords().size() >= _minInliers)
01320 {
01321 UASSERT(signatureB->sensorData().stereoCameraModel().isValidForProjection() || (signatureB->sensorData().cameraModels().size() == 1 && signatureB->sensorData().cameraModels()[0].isValidForProjection()));
01322 const CameraModel & cameraModel = signatureB->sensorData().stereoCameraModel().isValidForProjection()?signatureB->sensorData().stereoCameraModel().left():signatureB->sensorData().cameraModels()[0];
01323
01324 std::vector<int> inliersV;
01325 std::vector<int> matchesV;
01326 transforms[dir] = util3d::estimateMotion3DTo2D(
01327 uMultimapToMapUnique(signatureA->getWords3()),
01328 uMultimapToMapUnique(signatureB->getWords()),
01329 cameraModel,
01330 _minInliers,
01331 _iterations,
01332 _PnPReprojError,
01333 _PnPFlags,
01334 _PnPRefineIterations,
01335 dir==0?(!guess.isNull()?guess:Transform::getIdentity()):!transforms[0].isNull()?transforms[0].inverse():(!guess.isNull()?guess.inverse():Transform::getIdentity()),
01336 uMultimapToMapUnique(signatureB->getWords3()),
01337 &covariances[dir],
01338 &matchesV,
01339 &inliersV);
01340 inliers[dir] = inliersV;
01341 matches[dir] = matchesV;
01342 if(transforms[dir].isNull())
01343 {
01344 msg = uFormat("Not enough inliers %d/%d (matches=%d) between %d and %d",
01345 (int)inliers[dir].size(), _minInliers, (int)matches[dir].size(), signatureA->id(), signatureB->id());
01346 UINFO(msg.c_str());
01347 }
01348 else if(this->force3DoF())
01349 {
01350 transforms[dir] = transforms[dir].to3DoF();
01351 }
01352 }
01353 else
01354 {
01355 msg = uFormat("Not enough features in images (old=%d, new=%d, min=%d)",
01356 (int)signatureA->getWords3().size(), (int)signatureB->getWords().size(), _minInliers);
01357 UINFO(msg.c_str());
01358 }
01359 }
01360
01361 }
01362 else
01363 {
01364 UDEBUG("");
01365
01366 if((int)signatureA->getWords3().size() >= _minInliers &&
01367 (int)signatureB->getWords3().size() >= _minInliers)
01368 {
01369 std::vector<int> inliersV;
01370 std::vector<int> matchesV;
01371 transforms[dir] = util3d::estimateMotion3DTo3D(
01372 uMultimapToMapUnique(signatureA->getWords3()),
01373 uMultimapToMapUnique(signatureB->getWords3()),
01374 _minInliers,
01375 _inlierDistance,
01376 _iterations,
01377 _refineIterations,
01378 &covariances[dir],
01379 &matchesV,
01380 &inliersV);
01381 inliers[dir] = inliersV;
01382 matches[dir] = matchesV;
01383 if(transforms[dir].isNull())
01384 {
01385 msg = uFormat("Not enough inliers %d/%d (matches=%d) between %d and %d",
01386 (int)inliers[dir].size(), _minInliers, (int)matches[dir].size(), signatureA->id(), signatureB->id());
01387 UINFO(msg.c_str());
01388 }
01389 else if(this->force3DoF())
01390 {
01391 transforms[dir] = transforms[dir].to3DoF();
01392 }
01393 }
01394 else
01395 {
01396 msg = uFormat("Not enough 3D features in images (old=%d, new=%d, min=%d)",
01397 (int)signatureA->getWords3().size(), (int)signatureB->getWords3().size(), _minInliers);
01398 UINFO(msg.c_str());
01399 }
01400 }
01401 }
01402
01403 if(!_forwardEstimateOnly)
01404 {
01405 UDEBUG("from->to=%s", transforms[0].prettyPrint().c_str());
01406 UDEBUG("to->from=%s", transforms[1].prettyPrint().c_str());
01407 }
01408
01409 std::vector<int> allInliers = inliers[0];
01410 if(inliers[1].size())
01411 {
01412 std::set<int> allInliersSet(allInliers.begin(), allInliers.end());
01413 unsigned int oi = allInliers.size();
01414 allInliers.resize(allInliers.size() + inliers[1].size());
01415 for(unsigned int i=0; i<inliers[1].size(); ++i)
01416 {
01417 if(allInliersSet.find(inliers[1][i]) == allInliersSet.end())
01418 {
01419 allInliers[oi++] = inliers[1][i];
01420 }
01421 }
01422 allInliers.resize(oi);
01423 }
01424 std::vector<int> allMatches = matches[0];
01425 if(matches[1].size())
01426 {
01427 std::set<int> allMatchesSet(allMatches.begin(), allMatches.end());
01428 unsigned int oi = allMatches.size();
01429 allMatches.resize(allMatches.size() + matches[1].size());
01430 for(unsigned int i=0; i<matches[1].size(); ++i)
01431 {
01432 if(allMatchesSet.find(matches[1][i]) == allMatchesSet.end())
01433 {
01434 allMatches[oi++] = matches[1][i];
01435 }
01436 }
01437 allMatches.resize(oi);
01438 }
01439
01440 if(_bundleAdjustment > 0 &&
01441 _estimationType < 2 &&
01442 !transforms[0].isNull() &&
01443 allInliers.size() &&
01444 fromSignature.getWords3().size() &&
01445 toSignature.getWords().size() &&
01446 fromSignature.sensorData().cameraModels().size() <= 1 &&
01447 toSignature.sensorData().cameraModels().size() <= 1)
01448 {
01449 Optimizer * sba = Optimizer::create(_bundleAdjustment==2?Optimizer::kTypeCVSBA:Optimizer::kTypeG2O, _bundleParameters);
01450
01451 std::map<int, Transform> poses;
01452 std::multimap<int, Link> links;
01453 std::map<int, cv::Point3f> points3DMap;
01454
01455 poses.insert(std::make_pair(1, Transform::getIdentity()));
01456 poses.insert(std::make_pair(2, transforms[0]));
01457
01458 for(int i=0;i<2;++i)
01459 {
01460 UASSERT(covariances[i].cols==6 && covariances[i].rows == 6 && covariances[i].type() == CV_64FC1);
01461 if(covariances[i].at<double>(0,0)<=COVARIANCE_EPSILON)
01462 covariances[i].at<double>(0,0) = COVARIANCE_EPSILON;
01463 if(covariances[i].at<double>(1,1)<=COVARIANCE_EPSILON)
01464 covariances[i].at<double>(1,1) = COVARIANCE_EPSILON;
01465 if(covariances[i].at<double>(2,2)<=COVARIANCE_EPSILON)
01466 covariances[i].at<double>(2,2) = COVARIANCE_EPSILON;
01467 if(covariances[i].at<double>(3,3)<=COVARIANCE_EPSILON)
01468 covariances[i].at<double>(3,3) = COVARIANCE_EPSILON;
01469 if(covariances[i].at<double>(4,4)<=COVARIANCE_EPSILON)
01470 covariances[i].at<double>(4,4) = COVARIANCE_EPSILON;
01471 if(covariances[i].at<double>(5,5)<=COVARIANCE_EPSILON)
01472 covariances[i].at<double>(5,5) = COVARIANCE_EPSILON;
01473 }
01474
01475 cv::Mat cov = covariances[0].clone();
01476
01477 links.insert(std::make_pair(1, Link(1, 2, Link::kNeighbor, transforms[0], cov.inv())));
01478 if(!transforms[1].isNull() && inliers[1].size())
01479 {
01480 cov = covariances[1].clone();
01481 links.insert(std::make_pair(2, Link(2, 1, Link::kNeighbor, transforms[1], cov.inv())));
01482 }
01483
01484 std::map<int, Transform> optimizedPoses;
01485
01486 UASSERT(toSignature.sensorData().stereoCameraModel().isValidForProjection() ||
01487 (toSignature.sensorData().cameraModels().size() == 1 && toSignature.sensorData().cameraModels()[0].isValidForProjection()));
01488
01489 std::map<int, CameraModel> models;
01490
01491 Transform invLocalTransformFrom;
01492 CameraModel cameraModelFrom;
01493 if(fromSignature.sensorData().stereoCameraModel().isValidForProjection())
01494 {
01495 cameraModelFrom = fromSignature.sensorData().stereoCameraModel().left();
01496
01497 cameraModelFrom = CameraModel(cameraModelFrom.fx(),
01498 cameraModelFrom.fy(),
01499 cameraModelFrom.cx(),
01500 cameraModelFrom.cy(),
01501 cameraModelFrom.localTransform(),
01502 -fromSignature.sensorData().stereoCameraModel().baseline()*cameraModelFrom.fy());
01503 invLocalTransformFrom = toSignature.sensorData().stereoCameraModel().localTransform().inverse();
01504 }
01505 else if(fromSignature.sensorData().cameraModels().size() == 1)
01506 {
01507 cameraModelFrom = fromSignature.sensorData().cameraModels()[0];
01508 invLocalTransformFrom = toSignature.sensorData().cameraModels()[0].localTransform().inverse();
01509 }
01510
01511 Transform invLocalTransformTo = Transform::getIdentity();
01512 CameraModel cameraModelTo;
01513 if(toSignature.sensorData().stereoCameraModel().isValidForProjection())
01514 {
01515 cameraModelTo = toSignature.sensorData().stereoCameraModel().left();
01516
01517 cameraModelTo = CameraModel(cameraModelTo.fx(),
01518 cameraModelTo.fy(),
01519 cameraModelTo.cx(),
01520 cameraModelTo.cy(),
01521 cameraModelTo.localTransform(),
01522 -toSignature.sensorData().stereoCameraModel().baseline()*cameraModelTo.fy());
01523 invLocalTransformTo = toSignature.sensorData().stereoCameraModel().localTransform().inverse();
01524 }
01525 else if(toSignature.sensorData().cameraModels().size() == 1)
01526 {
01527 cameraModelTo = toSignature.sensorData().cameraModels()[0];
01528 invLocalTransformTo = toSignature.sensorData().cameraModels()[0].localTransform().inverse();
01529 }
01530 if(invLocalTransformFrom.isNull())
01531 {
01532 invLocalTransformFrom = invLocalTransformTo;
01533 }
01534
01535 models.insert(std::make_pair(1, cameraModelFrom.isValidForProjection()?cameraModelFrom:cameraModelTo));
01536 models.insert(std::make_pair(2, cameraModelTo));
01537
01538 std::map<int, std::map<int, cv::Point3f> > wordReferences;
01539 for(unsigned int i=0; i<allInliers.size(); ++i)
01540 {
01541 int wordId = allInliers[i];
01542 const cv::Point3f & pt3D = fromSignature.getWords3().find(wordId)->second;
01543 points3DMap.insert(std::make_pair(wordId, pt3D));
01544
01545 std::map<int, cv::Point3f> ptMap;
01546 if(fromSignature.getWords().size() && cameraModelFrom.isValidForProjection())
01547 {
01548 float depthFrom = util3d::transformPoint(pt3D, invLocalTransformFrom).z;
01549 const cv::Point2f & kpt = fromSignature.getWords().find(wordId)->second.pt;
01550 ptMap.insert(std::make_pair(1,cv::Point3f(kpt.x, kpt.y, depthFrom)));
01551 }
01552 if(toSignature.getWords().size() && cameraModelTo.isValidForProjection())
01553 {
01554 float depthTo = util3d::transformPoint(toSignature.getWords3().find(wordId)->second, invLocalTransformTo).z;
01555 const cv::Point2f & kpt = toSignature.getWords().find(wordId)->second.pt;
01556 UASSERT(toSignature.getWords3().find(wordId) != toSignature.getWords3().end());
01557 ptMap.insert(std::make_pair(2,cv::Point3f(kpt.x, kpt.y, depthTo)));
01558 }
01559
01560 wordReferences.insert(std::make_pair(wordId, ptMap));
01561
01562
01563
01564
01565
01566
01567 }
01568
01569 std::set<int> sbaOutliers;
01570 optimizedPoses = sba->optimizeBA(1, poses, links, models, points3DMap, wordReferences, &sbaOutliers);
01571 delete sba;
01572
01573
01574 if(optimizedPoses.size() == 2 &&
01575 !optimizedPoses.begin()->second.isNull() &&
01576 !optimizedPoses.rbegin()->second.isNull())
01577 {
01578 UDEBUG("Pose optimization: %s -> %s", transforms[0].prettyPrint().c_str(), optimizedPoses.rbegin()->second.prettyPrint().c_str());
01579
01580 if(sbaOutliers.size())
01581 {
01582 std::vector<int> newInliers(allInliers.size());
01583 int oi=0;
01584 for(unsigned int i=0; i<allInliers.size(); ++i)
01585 {
01586 if(sbaOutliers.find(allInliers[i]) == sbaOutliers.end())
01587 {
01588 newInliers[oi++] = allInliers[i];
01589 }
01590 }
01591 newInliers.resize(oi);
01592 UDEBUG("BA outliers ratio %f", float(sbaOutliers.size())/float(allInliers.size()));
01593 allInliers = newInliers;
01594 }
01595 if((int)allInliers.size() < _minInliers)
01596 {
01597 msg = uFormat("Not enough inliers after bundle adjustment %d/%d (matches=%d) between %d and %d",
01598 (int)allInliers.size(), _minInliers, fromSignature.id(), toSignature.id());
01599 transforms[0].setNull();
01600 }
01601 else
01602 {
01603 transforms[0] = optimizedPoses.rbegin()->second;
01604 }
01605
01606
01607
01608
01609
01610
01611
01612
01613
01614
01615
01616
01617
01618
01619 }
01620 else
01621 {
01622 transforms[0].setNull();
01623 }
01624 transforms[1].setNull();
01625 }
01626
01627 info.inliersIDs = allInliers;
01628 info.matchesIDs = allMatches;
01629 inliersCount = (int)allInliers.size();
01630 matchesCount = (int)allMatches.size();
01631 if(!transforms[1].isNull())
01632 {
01633 transforms[1] = transforms[1].inverse();
01634 if(transforms[0].isNull())
01635 {
01636 transform = transforms[1];
01637 covariance = covariances[1];
01638 }
01639 else
01640 {
01641 transform = transforms[0].interpolate(0.5f, transforms[1]);
01642 covariance = (covariances[0]+covariances[1])/2.0f;
01643 }
01644 }
01645 else
01646 {
01647 transform = transforms[0];
01648 covariance = covariances[0];
01649 }
01650 }
01651 else if(toSignature.sensorData().isValid())
01652 {
01653 UWARN("Missing correspondences for registration (%d->%d). fromWords = %d fromImageEmpty=%d toWords = %d toImageEmpty=%d",
01654 fromSignature.id(), toSignature.id(),
01655 (int)fromSignature.getWords().size(), fromSignature.sensorData().imageRaw().empty()?1:0,
01656 (int)toSignature.getWords().size(), toSignature.sensorData().imageRaw().empty()?1:0);
01657 }
01658
01659 info.inliers = inliersCount;
01660 info.matches = matchesCount;
01661 info.rejectedMsg = msg;
01662 info.covariance = covariance;
01663
01664 UDEBUG("transform=%s", transform.prettyPrint().c_str());
01665 return transform;
01666 }
01667
01668 }