mirror of
https://github.com/introlab/rtabmap.git
synced 2026-09-10 21:40:19 +08:00
0.15.1: CameraImages: added ground truth time diff, fixed memory leak when loading binary scans. CameraRGBDImages and CameraStereoImages: fixed start id. Feature2D: added grid rows and cols parameters (Kp/GridRows, Kp/GridCols, Vis/GridRows, Vis/GridCols). OdomInfo: publish bundle frames. OdometryORBSLAM2: added OdomORBSLAM2/Fps and OdomORBSLAM2/MaxFeatures parameters. Registration: added Reg/RepeatOnce parameter and removed variance normalization. For util2d::getDepth() and util3d::projectDepthTo3D(), maxZError parameter is now depthErrorRatio to be dependent of the sensor range. Database: save image width and height from stereo calibration. OptimizerG2O: fixed SBA optimization when using g2o built from ORBSLAM2 library. OptimizerGTSAM: to increase optimization stability, all rotations in information matrix are divided by 100000. Added rtabmap-report tool.
This commit is contained in:
matlabbe
+283
-145
@@ -79,6 +79,8 @@ RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration
|
||||
uInsert(_featureParameters, ParametersPair(Parameters::kKpSubPixEps(), _featureParameters.at(Parameters::kVisSubPixWinSize())));
|
||||
uInsert(_featureParameters, ParametersPair(Parameters::kKpSubPixIterations(), _featureParameters.at(Parameters::kVisSubPixIterations())));
|
||||
uInsert(_featureParameters, ParametersPair(Parameters::kKpSubPixWinSize(), _featureParameters.at(Parameters::kVisSubPixEps())));
|
||||
uInsert(_featureParameters, ParametersPair(Parameters::kKpGridRows(), _featureParameters.at(Parameters::kVisGridRows())));
|
||||
uInsert(_featureParameters, ParametersPair(Parameters::kKpGridCols(), _featureParameters.at(Parameters::kVisGridCols())));
|
||||
uInsert(_featureParameters, ParametersPair(Parameters::kKpNewWordsComparedTogether(), "false"));
|
||||
|
||||
this->parseParameters(parameters);
|
||||
@@ -162,6 +164,14 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
|
||||
{
|
||||
uInsert(_featureParameters, ParametersPair(Parameters::kKpSubPixWinSize(), parameters.at(Parameters::kVisSubPixWinSize())));
|
||||
}
|
||||
if(uContains(parameters, Parameters::kVisGridRows()))
|
||||
{
|
||||
uInsert(_featureParameters, ParametersPair(Parameters::kKpGridRows(), parameters.at(Parameters::kVisGridRows())));
|
||||
}
|
||||
if(uContains(parameters, Parameters::kVisGridCols()))
|
||||
{
|
||||
uInsert(_featureParameters, ParametersPair(Parameters::kKpGridCols(), parameters.at(Parameters::kVisGridCols())));
|
||||
}
|
||||
}
|
||||
|
||||
RegistrationVis::~RegistrationVis()
|
||||
@@ -425,7 +435,7 @@ Transform RegistrationVis::computeTransformationImpl(
|
||||
kptsFrom3D = kptsFrom3DKept;
|
||||
|
||||
std::vector<cv::Point3f> kptsTo3D;
|
||||
if(_estimationType == 0 || (_estimationType == 1 && !varianceFromInliersCount()) || !_forwardEstimateOnly)
|
||||
if(_estimationType == 0 || _estimationType == 1 || !_forwardEstimateOnly)
|
||||
{
|
||||
kptsTo3D = detector->generateKeypoints3D(toSignature.sensorData(), kptsTo);
|
||||
}
|
||||
@@ -721,7 +731,9 @@ Transform RegistrationVis::computeTransformationImpl(
|
||||
{
|
||||
imageSize = toSignature.sensorData().cameraModels().size() == 1?toSignature.sensorData().cameraModels()[0].imageSize():toSignature.sensorData().stereoCameraModel().left().imageSize();
|
||||
}
|
||||
isCalibrated = imageSize.height != 0 && imageSize.width != 0 && toSignature.sensorData().cameraModels().size()==1?toSignature.sensorData().cameraModels()[0].isValidForProjection():toSignature.sensorData().stereoCameraModel().isValidForProjection();
|
||||
|
||||
isCalibrated = imageSize.height != 0 && imageSize.width != 0 &&
|
||||
(toSignature.sensorData().cameraModels().size()==1?toSignature.sensorData().cameraModels()[0].isValidForProjection():toSignature.sensorData().stereoCameraModel().isValidForProjection());
|
||||
|
||||
// If guess is set, limit the search of matches using optical flow window size
|
||||
bool guessSet = !guess.isIdentity() && !guess.isNull();
|
||||
@@ -756,12 +768,11 @@ Transform RegistrationVis::computeTransformationImpl(
|
||||
std::vector<cv::Point2f> cornersProjected(projected.size());
|
||||
std::vector<int> projectedIndexToDescIndex(projected.size());
|
||||
int oi=0;
|
||||
Transform guessInv = guess.inverse();
|
||||
for(unsigned int i=0; i<projected.size(); ++i)
|
||||
{
|
||||
if(uIsInBounds(projected[i].x, 0.0f, float(imageSize.width-1)) &&
|
||||
uIsInBounds(projected[i].y, 0.0f, float(imageSize.height-1)) &&
|
||||
util3d::transformPoint(kptsFrom3D[i], guessInv).x > 0.0)
|
||||
util3d::transformPoint(kptsFrom3D[i], guessCameraRef).z > 0.0)
|
||||
{
|
||||
projectedIndexToDescIndex[oi] = i;
|
||||
cornersProjected[oi++] = projected[i];
|
||||
@@ -780,159 +791,273 @@ Transform RegistrationVis::computeTransformationImpl(
|
||||
if(cornersProjected.size())
|
||||
{
|
||||
|
||||
// Create kd-tree for projected keypoints
|
||||
rtflann::Matrix<float> cornersProjectedMat((float*)cornersProjected.data(), cornersProjected.size(), 2);
|
||||
rtflann::Index<rtflann::L2_Simple<float> > index(cornersProjectedMat, rtflann::KDTreeIndexParams());
|
||||
index.buildIndex();
|
||||
|
||||
std::vector< std::vector<size_t> > indices;
|
||||
std::vector<std::vector<float> > dists;
|
||||
float radius = (float)_guessWinSize; // pixels
|
||||
std::vector<cv::Point2f> pointsTo;
|
||||
cv::KeyPoint::convert(kptsTo, pointsTo);
|
||||
rtflann::Matrix<float> pointsToMat((float*)pointsTo.data(), pointsTo.size(), 2);
|
||||
index.radiusSearch(pointsToMat, indices, dists, radius*radius, rtflann::SearchParams());
|
||||
|
||||
UASSERT(indices.size() == pointsToMat.rows);
|
||||
UASSERT(descriptorsFrom.cols == descriptorsTo.cols);
|
||||
UASSERT(descriptorsFrom.rows == (int)kptsFrom.size());
|
||||
UASSERT((int)pointsToMat.rows == descriptorsTo.rows);
|
||||
UASSERT(pointsToMat.rows == kptsTo.size());
|
||||
UDEBUG("radius search done for guess");
|
||||
|
||||
// Process results (Nearest Neighbor Distance Ratio)
|
||||
int newToId = orignalWordsFromIds.size()?orignalWordsFromIds.back():descriptorsFrom.rows;
|
||||
std::map<int,int> addedWordsFrom; //<id, index>
|
||||
std::map<int, int> duplicates; //<fromId, toId>
|
||||
int newWords = 0;
|
||||
for(unsigned int i = 0; i < pointsToMat.rows; ++i)
|
||||
bool matchToProjected = false;
|
||||
if(matchToProjected)
|
||||
{
|
||||
if(kptsTo3D.empty() || util3d::isFinite(kptsTo3D[i]))
|
||||
// match frame to projected
|
||||
// Create kd-tree for projected keypoints
|
||||
rtflann::Matrix<float> cornersProjectedMat((float*)cornersProjected.data(), cornersProjected.size(), 2);
|
||||
rtflann::Index<rtflann::L2_Simple<float> > index(cornersProjectedMat, rtflann::KDTreeIndexParams());
|
||||
index.buildIndex();
|
||||
|
||||
std::vector< std::vector<size_t> > indices;
|
||||
std::vector<std::vector<float> > dists;
|
||||
float radius = (float)_guessWinSize; // pixels
|
||||
std::vector<cv::Point2f> pointsTo;
|
||||
cv::KeyPoint::convert(kptsTo, pointsTo);
|
||||
rtflann::Matrix<float> pointsToMat((float*)pointsTo.data(), pointsTo.size(), 2);
|
||||
index.radiusSearch(pointsToMat, indices, dists, radius*radius, rtflann::SearchParams());
|
||||
|
||||
UASSERT(indices.size() == pointsToMat.rows);
|
||||
UASSERT(descriptorsFrom.cols == descriptorsTo.cols);
|
||||
UASSERT(descriptorsFrom.rows == (int)kptsFrom.size());
|
||||
UASSERT((int)pointsToMat.rows == descriptorsTo.rows);
|
||||
UASSERT(pointsToMat.rows == kptsTo.size());
|
||||
UDEBUG("radius search done for guess");
|
||||
|
||||
// Process results (Nearest Neighbor Distance Ratio)
|
||||
int newToId = orignalWordsFromIds.size()?orignalWordsFromIds.back():descriptorsFrom.rows;
|
||||
std::map<int,int> addedWordsFrom; //<id, index>
|
||||
std::map<int, int> duplicates; //<fromId, toId>
|
||||
int newWords = 0;
|
||||
for(unsigned int i = 0; i < pointsToMat.rows; ++i)
|
||||
{
|
||||
int octave = kptsTo[i].octave;
|
||||
int matchedIndex = -1;
|
||||
if(indices[i].size() >= 2)
|
||||
if(kptsTo3D.empty() || util3d::isFinite(kptsTo3D[i]))
|
||||
{
|
||||
cv::Mat descriptors;
|
||||
std::vector<int> descriptorsIndices(indices[i].size());
|
||||
int oi=0;
|
||||
for(unsigned int j=0; j<indices[i].size(); ++j)
|
||||
int octave = kptsTo[i].octave;
|
||||
int matchedIndex = -1;
|
||||
if(indices[i].size() >= 2)
|
||||
{
|
||||
if(kptsFrom.at(projectedIndexToDescIndex[indices[i].at(j)]).octave==octave)
|
||||
cv::Mat descriptors;
|
||||
std::vector<int> descriptorsIndices(indices[i].size());
|
||||
int oi=0;
|
||||
for(unsigned int j=0; j<indices[i].size(); ++j)
|
||||
{
|
||||
descriptors.push_back(descriptorsFrom.row(projectedIndexToDescIndex[indices[i].at(j)]));
|
||||
descriptorsIndices[oi++] = indices[i].at(j);
|
||||
if(kptsFrom.at(projectedIndexToDescIndex[indices[i].at(j)]).octave==octave)
|
||||
{
|
||||
descriptors.push_back(descriptorsFrom.row(projectedIndexToDescIndex[indices[i].at(j)]));
|
||||
descriptorsIndices[oi++] = indices[i].at(j);
|
||||
}
|
||||
}
|
||||
descriptorsIndices.resize(oi);
|
||||
if(oi >=2)
|
||||
{
|
||||
std::vector<std::vector<cv::DMatch> > matches;
|
||||
cv::BFMatcher matcher(descriptors.type()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR);
|
||||
matcher.knnMatch(descriptorsTo.row(i), descriptors, matches, 2);
|
||||
UASSERT(matches.size() == 1);
|
||||
UASSERT(matches[0].size() == 2);
|
||||
if(matches[0].at(0).distance < _nndr * matches[0].at(1).distance)
|
||||
{
|
||||
matchedIndex = descriptorsIndices.at(matches[0].at(0).trainIdx);
|
||||
}
|
||||
}
|
||||
else if(oi == 1)
|
||||
{
|
||||
matchedIndex = descriptorsIndices[0];
|
||||
}
|
||||
}
|
||||
descriptorsIndices.resize(oi);
|
||||
if(oi >=2)
|
||||
else if(indices[i].size() == 1 &&
|
||||
kptsFrom.at(projectedIndexToDescIndex[indices[i].at(0)]).octave == octave)
|
||||
{
|
||||
std::vector<std::vector<cv::DMatch> > matches;
|
||||
cv::BFMatcher matcher(descriptors.type()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR);
|
||||
matcher.knnMatch(descriptorsTo.row(i), descriptors, matches, 2);
|
||||
UASSERT(matches.size() == 1);
|
||||
UASSERT(matches[0].size() == 2);
|
||||
if(matches[0].at(0).distance < _nndr * matches[0].at(1).distance)
|
||||
matchedIndex = indices[i].at(0);
|
||||
}
|
||||
|
||||
if(matchedIndex >= 0)
|
||||
{
|
||||
matchedIndex = projectedIndexToDescIndex[matchedIndex];
|
||||
int id = orignalWordsFromIds.size()?orignalWordsFromIds[matchedIndex]:matchedIndex;
|
||||
|
||||
if(addedWordsFrom.find(matchedIndex) != addedWordsFrom.end())
|
||||
{
|
||||
matchedIndex = descriptorsIndices.at(matches[0].at(0).trainIdx);
|
||||
id = addedWordsFrom.at(matchedIndex);
|
||||
duplicates.insert(std::make_pair(matchedIndex, id));
|
||||
}
|
||||
}
|
||||
else if(oi == 1)
|
||||
{
|
||||
matchedIndex = descriptorsIndices[0];
|
||||
}
|
||||
}
|
||||
else if(indices[i].size() == 1 &&
|
||||
kptsFrom.at(projectedIndexToDescIndex[indices[i].at(0)]).octave == octave)
|
||||
{
|
||||
matchedIndex = indices[i].at(0);
|
||||
}
|
||||
else
|
||||
{
|
||||
addedWordsFrom.insert(std::make_pair(matchedIndex, id));
|
||||
|
||||
if(matchedIndex >= 0)
|
||||
{
|
||||
matchedIndex = projectedIndexToDescIndex[matchedIndex];
|
||||
int id = orignalWordsFromIds.size()?orignalWordsFromIds[matchedIndex]:matchedIndex;
|
||||
if(kptsFrom.size())
|
||||
{
|
||||
wordsFrom.insert(std::make_pair(id, kptsFrom[matchedIndex]));
|
||||
}
|
||||
words3From.insert(std::make_pair(id, kptsFrom3D[matchedIndex]));
|
||||
wordsDescFrom.insert(std::make_pair(id, descriptorsFrom.row(matchedIndex)));
|
||||
}
|
||||
|
||||
if(addedWordsFrom.find(matchedIndex) != addedWordsFrom.end())
|
||||
{
|
||||
id = addedWordsFrom.at(matchedIndex);
|
||||
duplicates.insert(std::make_pair(matchedIndex, id));
|
||||
wordsTo.insert(std::make_pair(id, kptsTo[i]));
|
||||
wordsDescTo.insert(std::make_pair(id, descriptorsTo.row(i)));
|
||||
if(kptsTo3D.size())
|
||||
{
|
||||
words3To.insert(std::make_pair(id, kptsTo3D[i]));
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
addedWordsFrom.insert(std::make_pair(matchedIndex, id));
|
||||
|
||||
if(kptsFrom.size())
|
||||
// gen fake ids
|
||||
wordsTo.insert(std::make_pair(newToId, kptsTo[i]));
|
||||
wordsDescTo.insert(std::make_pair(newToId, descriptorsTo.row(i)));
|
||||
if(kptsTo3D.size())
|
||||
{
|
||||
wordsFrom.insert(std::make_pair(id, kptsFrom[matchedIndex]));
|
||||
words3To.insert(std::make_pair(newToId, kptsTo3D[i]));
|
||||
}
|
||||
words3From.insert(std::make_pair(id, kptsFrom3D[matchedIndex]));
|
||||
wordsDescFrom.insert(std::make_pair(id, descriptorsFrom.row(matchedIndex)));
|
||||
}
|
||||
|
||||
wordsTo.insert(std::make_pair(id, kptsTo[i]));
|
||||
wordsDescTo.insert(std::make_pair(id, descriptorsTo.row(i)));
|
||||
if(kptsTo3D.size())
|
||||
{
|
||||
words3To.insert(std::make_pair(id, kptsTo3D[i]));
|
||||
++newToId;
|
||||
++newWords;
|
||||
}
|
||||
}
|
||||
else
|
||||
}
|
||||
UDEBUG("addedWordsFrom=%d/%d (duplicates=%d, newWords=%d), kptsTo=%d, wordsTo=%d, words3From=%d",
|
||||
(int)addedWordsFrom.size(), (int)cornersProjected.size(), (int)duplicates.size(), newWords,
|
||||
(int)kptsTo.size(), (int)wordsTo.size(), (int)words3From.size());
|
||||
|
||||
// create fake ids for not matched words from "from"
|
||||
int addWordsFromNotMatched = 0;
|
||||
for(unsigned int i=0; i<kptsFrom3D.size(); ++i)
|
||||
{
|
||||
if(util3d::isFinite(kptsFrom3D[i]) && addedWordsFrom.find(i) == addedWordsFrom.end())
|
||||
{
|
||||
int id = orignalWordsFromIds.size()?orignalWordsFromIds[i]:i;
|
||||
wordsFrom.insert(std::make_pair(id, kptsFrom[i]));
|
||||
wordsDescFrom.insert(std::make_pair(id, descriptorsFrom.row(i)));
|
||||
words3From.insert(std::make_pair(id, kptsFrom3D[i]));
|
||||
|
||||
++addWordsFromNotMatched;
|
||||
}
|
||||
}
|
||||
UDEBUG("addWordsFromNotMatched=%d -> words3From=%d", addWordsFromNotMatched, (int)words3From.size());
|
||||
}
|
||||
else
|
||||
{
|
||||
// match projected to frame
|
||||
std::vector<cv::Point2f> pointsTo;
|
||||
cv::KeyPoint::convert(kptsTo, pointsTo);
|
||||
rtflann::Matrix<float> pointsToMat((float*)pointsTo.data(), pointsTo.size(), 2);
|
||||
rtflann::Index<rtflann::L2_Simple<float> > index(pointsToMat, rtflann::KDTreeIndexParams());
|
||||
index.buildIndex();
|
||||
|
||||
std::vector< std::vector<size_t> > indices;
|
||||
std::vector<std::vector<float> > dists;
|
||||
float radius = (float)_guessWinSize; // pixels
|
||||
rtflann::Matrix<float> cornersProjectedMat((float*)cornersProjected.data(), cornersProjected.size(), 2);
|
||||
index.radiusSearch(cornersProjectedMat, indices, dists, radius*radius, rtflann::SearchParams());
|
||||
|
||||
UASSERT(indices.size() == cornersProjectedMat.rows);
|
||||
UASSERT(descriptorsFrom.cols == descriptorsTo.cols);
|
||||
UASSERT(descriptorsFrom.rows == (int)kptsFrom.size());
|
||||
UASSERT((int)pointsToMat.rows == descriptorsTo.rows);
|
||||
UASSERT(pointsToMat.rows == kptsTo.size());
|
||||
UDEBUG("radius search done for guess");
|
||||
|
||||
// Process results (Nearest Neighbor Distance Ratio)
|
||||
std::set<int> addedWordsTo;
|
||||
std::set<int> addedWordsFrom;
|
||||
std::set<int> indicesToIgnore;
|
||||
for(unsigned int i = 0; i < cornersProjectedMat.rows; ++i)
|
||||
{
|
||||
int matchedIndexFrom = projectedIndexToDescIndex[i];
|
||||
if(util3d::isFinite(kptsFrom3D[matchedIndexFrom]))
|
||||
{
|
||||
int matchedIndexTo = -1;
|
||||
if(indices[i].size() >= 2)
|
||||
{
|
||||
cv::Mat descriptors;
|
||||
std::vector<int> descriptorsIndices(indices[i].size());
|
||||
int oi=0;
|
||||
for(unsigned int j=0; j<indices[i].size(); ++j)
|
||||
{
|
||||
int octave = kptsTo[indices[i].at(j)].octave;
|
||||
if(kptsFrom.at(matchedIndexFrom).octave==octave)
|
||||
{
|
||||
descriptors.push_back(descriptorsTo.row(indices[i].at(j)));
|
||||
descriptorsIndices[oi++] = indices[i].at(j);
|
||||
|
||||
if(dists[i].at(j) < radius)
|
||||
{
|
||||
indicesToIgnore.insert(indices[i].at(j));
|
||||
}
|
||||
}
|
||||
}
|
||||
descriptorsIndices.resize(oi);
|
||||
if(oi >=2)
|
||||
{
|
||||
std::vector<std::vector<cv::DMatch> > matches;
|
||||
cv::BFMatcher matcher(descriptors.type()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR);
|
||||
matcher.knnMatch(descriptorsFrom.row(matchedIndexFrom), descriptors, matches, 2);
|
||||
UASSERT(matches.size() == 1);
|
||||
UASSERT(matches[0].size() == 2);
|
||||
if(matches[0].at(0).distance < _nndr * matches[0].at(1).distance)
|
||||
{
|
||||
matchedIndexTo = descriptorsIndices.at(matches[0].at(0).trainIdx);
|
||||
}
|
||||
}
|
||||
else if(oi == 1)
|
||||
{
|
||||
matchedIndexTo = descriptorsIndices[0];
|
||||
}
|
||||
}
|
||||
else if(indices[i].size() == 1)
|
||||
{
|
||||
int octave = kptsTo[indices[i].at(0)].octave;
|
||||
if(kptsFrom.at(matchedIndexFrom).octave == octave)
|
||||
{
|
||||
matchedIndexTo = indices[i].at(0);
|
||||
}
|
||||
}
|
||||
|
||||
int id = orignalWordsFromIds.size()?orignalWordsFromIds[matchedIndexFrom]:matchedIndexFrom;
|
||||
addedWordsFrom.insert(matchedIndexFrom);
|
||||
|
||||
if(kptsFrom.size())
|
||||
{
|
||||
wordsFrom.insert(std::make_pair(id, kptsFrom[matchedIndexFrom]));
|
||||
}
|
||||
words3From.insert(std::make_pair(id, kptsFrom3D[matchedIndexFrom]));
|
||||
wordsDescFrom.insert(std::make_pair(id, descriptorsFrom.row(matchedIndexFrom)));
|
||||
|
||||
if((kptsTo3D.empty() || util3d::isFinite(kptsTo3D[matchedIndexTo])) &&
|
||||
matchedIndexTo >= 0 &&
|
||||
addedWordsTo.find(matchedIndexTo) == addedWordsTo.end())
|
||||
{
|
||||
addedWordsTo.insert(matchedIndexTo);
|
||||
|
||||
wordsTo.insert(std::make_pair(id, kptsTo[matchedIndexTo]));
|
||||
wordsDescTo.insert(std::make_pair(id, descriptorsTo.row(matchedIndexTo)));
|
||||
if(kptsTo3D.size())
|
||||
{
|
||||
words3To.insert(std::make_pair(id, kptsTo3D[matchedIndexTo]));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// create fake ids for not matched words from "from"
|
||||
for(unsigned int i=0; i<kptsFrom3D.size(); ++i)
|
||||
{
|
||||
if(util3d::isFinite(kptsFrom3D[i]) && addedWordsFrom.find(i) == addedWordsFrom.end())
|
||||
{
|
||||
int id = orignalWordsFromIds.size()?orignalWordsFromIds[i]:i;
|
||||
wordsFrom.insert(std::make_pair(id, kptsFrom[i]));
|
||||
wordsDescFrom.insert(std::make_pair(id, descriptorsFrom.row(i)));
|
||||
words3From.insert(std::make_pair(id, kptsFrom3D[i]));
|
||||
}
|
||||
}
|
||||
|
||||
int newToId = orignalWordsFromIds.size()?orignalWordsFromIds.back():descriptorsFrom.rows;
|
||||
for(unsigned int i = 0; i < kptsTo.size(); ++i)
|
||||
{
|
||||
if(addedWordsTo.find(i) == addedWordsTo.end() && indicesToIgnore.find(i) == indicesToIgnore.end())
|
||||
{
|
||||
// gen fake ids
|
||||
wordsTo.insert(std::make_pair(newToId, kptsTo[i]));
|
||||
wordsDescTo.insert(std::make_pair(newToId, descriptorsTo.row(i)));
|
||||
if(kptsTo3D.size())
|
||||
{
|
||||
words3To.insert(std::make_pair(newToId, kptsTo3D[i]));
|
||||
}
|
||||
|
||||
++newToId;
|
||||
++newWords;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
UDEBUG("addedWordsFrom=%d/%d (duplicates=%d, newWords=%d), kptsTo=%d, wordsTo=%d, words3From=%d",
|
||||
(int)addedWordsFrom.size(), (int)cornersProjected.size(), (int)duplicates.size(), newWords,
|
||||
(int)kptsTo.size(), (int)wordsTo.size(), (int)words3From.size());
|
||||
|
||||
// create fake ids for not matched words from "from"
|
||||
int addWordsFromNotMatched = 0;
|
||||
for(unsigned int i=0; i<kptsFrom3D.size(); ++i)
|
||||
{
|
||||
if(util3d::isFinite(kptsFrom3D[i]) && addedWordsFrom.find(i) == addedWordsFrom.end())
|
||||
{
|
||||
int id = orignalWordsFromIds.size()?orignalWordsFromIds[i]:i;
|
||||
wordsFrom.insert(std::make_pair(id, kptsFrom[i]));
|
||||
wordsDescFrom.insert(std::make_pair(id, descriptorsFrom.row(i)));
|
||||
words3From.insert(std::make_pair(id, kptsFrom3D[i]));
|
||||
|
||||
++addWordsFromNotMatched;
|
||||
}
|
||||
}
|
||||
UDEBUG("addWordsFromNotMatched=%d -> words3From=%d", addWordsFromNotMatched, (int)words3From.size());
|
||||
|
||||
/*std::vector<cv::KeyPoint> matches(wordsTo.size());
|
||||
int oi=0;
|
||||
for(std::multimap<int, cv::KeyPoint>::iterator iter = wordsTo.begin(); iter!=wordsTo.end(); ++iter)
|
||||
{
|
||||
if(iter->first < (orignalWordsFromIds.size()?orignalWordsFromIds.back():descriptorsFrom.rows) && wordsTo.count(iter->first) <= 1)
|
||||
{
|
||||
matches[oi++] = iter->second;
|
||||
}
|
||||
}
|
||||
matches.resize(oi);
|
||||
UDEBUG("guess=%s", guess.prettyPrint().c_str());
|
||||
std::vector<cv::KeyPoint> projectedKpts;
|
||||
cv::KeyPoint::convert(cornersProjected, projectedKpts);
|
||||
cv::Mat image = toSignature.sensorData().imageRaw().clone();
|
||||
drawKeypoints(image, kptsTo, image, cv::Scalar(0,0,255));
|
||||
drawKeypoints(image, projectedKpts, image, cv::Scalar(0,255,255)); // BGR
|
||||
drawKeypoints(image, matches, image, cv::Scalar(0,255,0));
|
||||
cv::imwrite("projected.bmp", image);
|
||||
UWARN("saved projected.bmp");*/
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
@@ -1180,7 +1305,7 @@ Transform RegistrationVis::computeTransformationImpl(
|
||||
_PnPRefineIterations,
|
||||
dir==0?(!guess.isNull()?guess:Transform::getIdentity()):!transforms[0].isNull()?transforms[0].inverse():(!guess.isNull()?guess.inverse():Transform::getIdentity()),
|
||||
uMultimapToMapUnique(signatureB->getWords3()),
|
||||
varianceFromInliersCount()?0:&covariances[dir],
|
||||
&covariances[dir],
|
||||
&matchesV,
|
||||
&inliersV);
|
||||
inliers[dir] = inliersV;
|
||||
@@ -1236,20 +1361,6 @@ Transform RegistrationVis::computeTransformationImpl(
|
||||
UINFO(msg.c_str());
|
||||
}
|
||||
}
|
||||
|
||||
double epsilon = 0.000001;
|
||||
if(covariances[dir].at<double>(0,0)<=epsilon)
|
||||
covariances[dir].at<double>(0,0) = epsilon; // epsilon if exact transform
|
||||
if(covariances[dir].at<double>(1,1)<=epsilon)
|
||||
covariances[dir].at<double>(1,1) = epsilon; // epsilon if exact transform
|
||||
if(covariances[dir].at<double>(2,2)<=epsilon)
|
||||
covariances[dir].at<double>(2,2) = epsilon; // epsilon if exact transform
|
||||
if(covariances[dir].at<double>(3,3)<=epsilon)
|
||||
covariances[dir].at<double>(3,3) = epsilon; // epsilon if exact transform
|
||||
if(covariances[dir].at<double>(4,4)<=epsilon)
|
||||
covariances[dir].at<double>(4,4) = epsilon; // epsilon if exact transform
|
||||
if(covariances[dir].at<double>(5,5)<=epsilon)
|
||||
covariances[dir].at<double>(5,5) = epsilon; // epsilon if exact transform
|
||||
}
|
||||
|
||||
if(!_forwardEstimateOnly)
|
||||
@@ -1307,14 +1418,29 @@ Transform RegistrationVis::computeTransformationImpl(
|
||||
poses.insert(std::make_pair(1, Transform::getIdentity()));
|
||||
poses.insert(std::make_pair(2, transforms[0]));
|
||||
|
||||
for(int i=0;i<2;++i)
|
||||
{
|
||||
UASSERT(covariances[i].cols==6 && covariances[i].rows == 6 && covariances[i].type() == CV_64FC1);
|
||||
if(covariances[i].at<double>(0,0)<=COVARIANCE_EPSILON)
|
||||
covariances[i].at<double>(0,0) = COVARIANCE_EPSILON; // epsilon if exact transform
|
||||
if(covariances[i].at<double>(1,1)<=COVARIANCE_EPSILON)
|
||||
covariances[i].at<double>(1,1) = COVARIANCE_EPSILON; // epsilon if exact transform
|
||||
if(covariances[i].at<double>(2,2)<=COVARIANCE_EPSILON)
|
||||
covariances[i].at<double>(2,2) = COVARIANCE_EPSILON; // epsilon if exact transform
|
||||
if(covariances[i].at<double>(3,3)<=COVARIANCE_EPSILON)
|
||||
covariances[i].at<double>(3,3) = COVARIANCE_EPSILON; // epsilon if exact transform
|
||||
if(covariances[i].at<double>(4,4)<=COVARIANCE_EPSILON)
|
||||
covariances[i].at<double>(4,4) = COVARIANCE_EPSILON; // epsilon if exact transform
|
||||
if(covariances[i].at<double>(5,5)<=COVARIANCE_EPSILON)
|
||||
covariances[i].at<double>(5,5) = COVARIANCE_EPSILON; // epsilon if exact transform
|
||||
}
|
||||
|
||||
cv::Mat cov = covariances[0].clone();
|
||||
normalizeCovariance(cov, transforms[0]);
|
||||
|
||||
links.insert(std::make_pair(1, Link(1, 2, Link::kNeighbor, transforms[0], cov.inv())));
|
||||
if(!transforms[1].isNull() && inliers[1].size())
|
||||
{
|
||||
cov = covariances[1].clone();
|
||||
normalizeCovariance(cov, transforms[1]);
|
||||
links.insert(std::make_pair(2, Link(2, 1, Link::kNeighbor, transforms[1], cov.inv())));
|
||||
}
|
||||
|
||||
@@ -1325,6 +1451,7 @@ Transform RegistrationVis::computeTransformationImpl(
|
||||
|
||||
std::map<int, CameraModel> models;
|
||||
|
||||
Transform invLocalTransformFrom;
|
||||
CameraModel cameraModelFrom;
|
||||
if(fromSignature.sensorData().stereoCameraModel().isValidForProjection())
|
||||
{
|
||||
@@ -1336,12 +1463,15 @@ Transform RegistrationVis::computeTransformationImpl(
|
||||
cameraModelFrom.cy(),
|
||||
cameraModelFrom.localTransform(),
|
||||
-fromSignature.sensorData().stereoCameraModel().baseline()*cameraModelFrom.fy());
|
||||
invLocalTransformFrom = toSignature.sensorData().stereoCameraModel().localTransform().inverse();
|
||||
}
|
||||
else if(fromSignature.sensorData().cameraModels().size() == 1)
|
||||
{
|
||||
cameraModelFrom = fromSignature.sensorData().cameraModels()[0];
|
||||
invLocalTransformFrom = toSignature.sensorData().cameraModels()[0].localTransform().inverse();
|
||||
}
|
||||
|
||||
Transform invLocalTransformTo = Transform::getIdentity();
|
||||
CameraModel cameraModelTo;
|
||||
if(toSignature.sensorData().stereoCameraModel().isValidForProjection())
|
||||
{
|
||||
@@ -1353,10 +1483,16 @@ Transform RegistrationVis::computeTransformationImpl(
|
||||
cameraModelTo.cy(),
|
||||
cameraModelTo.localTransform(),
|
||||
-toSignature.sensorData().stereoCameraModel().baseline()*cameraModelTo.fy());
|
||||
invLocalTransformTo = toSignature.sensorData().stereoCameraModel().localTransform().inverse();
|
||||
}
|
||||
else if(toSignature.sensorData().cameraModels().size() == 1)
|
||||
{
|
||||
cameraModelTo = toSignature.sensorData().cameraModels()[0];
|
||||
invLocalTransformTo = toSignature.sensorData().cameraModels()[0].localTransform().inverse();
|
||||
}
|
||||
if(invLocalTransformFrom.isNull())
|
||||
{
|
||||
invLocalTransformFrom = invLocalTransformTo;
|
||||
}
|
||||
|
||||
models.insert(std::make_pair(1, cameraModelFrom.isValidForProjection()?cameraModelFrom:cameraModelTo));
|
||||
@@ -1372,14 +1508,16 @@ Transform RegistrationVis::computeTransformationImpl(
|
||||
std::map<int, cv::Point3f> ptMap;
|
||||
if(fromSignature.getWords().size() && cameraModelFrom.isValidForProjection())
|
||||
{
|
||||
float depthFrom = util3d::transformPoint(pt3D, invLocalTransformFrom).z;
|
||||
const cv::Point2f & kpt = fromSignature.getWords().find(wordId)->second.pt;
|
||||
ptMap.insert(std::make_pair(1,cv::Point3f(kpt.x, kpt.y, pt3D.x)));
|
||||
ptMap.insert(std::make_pair(1,cv::Point3f(kpt.x, kpt.y, depthFrom)));
|
||||
}
|
||||
if(toSignature.getWords().size() && cameraModelTo.isValidForProjection())
|
||||
{
|
||||
float depthTo = util3d::transformPoint(toSignature.getWords3().find(wordId)->second, invLocalTransformTo).z;
|
||||
const cv::Point2f & kpt = toSignature.getWords().find(wordId)->second.pt;
|
||||
UASSERT(toSignature.getWords3().find(wordId) != toSignature.getWords3().end());
|
||||
ptMap.insert(std::make_pair(2,cv::Point3f(kpt.x, kpt.y, toSignature.getWords3().find(wordId)->second.x)));
|
||||
ptMap.insert(std::make_pair(2,cv::Point3f(kpt.x, kpt.y, depthTo)));
|
||||
}
|
||||
|
||||
wordReferences.insert(std::make_pair(wordId, ptMap));
|
||||
|
||||
Reference in New Issue
Block a user