i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

OdometryF2F: Fixed broken OpticalFlow approach

Browse Source
This commit is contained in:
matlabbe
2016-02-24 17:42:23 -05:00
parent 172e9857c7
commit a8d1b91083
8 changed files with 55 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
corelib/src/Memory.cpp
View File

@@ -3104,7 +3104,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
}
std::vector<cv::Point3f> keypoints3D;
if(!_useOdometryFeatures || data.keypoints().size() == 0)
if(!_useOdometryFeatures || (data.keypoints().size() != data.descriptors().rows))
{
if(_feature2D->getMaxFeatures() >= 0 && !data.imageRaw().empty() && !isIntermediateNode)
{

12
corelib/src/OdometryF2F.cpp
View File

@@ -84,11 +84,9 @@ Transform OdometryF2F::computeTransform(
output = registrationPipeline_->computeTransformationMod(
refFrame_,
newFrame,
guessFromMotion_?motionSinceLastKeyFrame_*this->previousTransform():Transform(),
guessFromMotion_&&!this->previousTransform().isNull()?motionSinceLastKeyFrame_*this->previousTransform():Transform(),
&regInfo);
data.setFeatures(newFrame.sensorData().keypoints(), newFrame.sensorData().descriptors());
if(info && this->isInfoDataFilled())
{
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
@@ -132,18 +130,16 @@ Transform OdometryF2F::computeTransform(
if(keyFrameThr_ <= 0 || (int)regInfo.inliers <= keyFrameThr_)
{
UDEBUG("Update key frame");
int features = newFrame.sensorData().keypoints().size();
int features = newFrame.getWordsDescriptors().size();
if(features == 0)
{
newFrame = Signature(data);
// this will generate features only for the first frame
// this will generate features only for the first frame or if optical flow was used (no 3d words)
Signature dummy;
registrationPipeline_->computeTransformationMod(
newFrame,
dummy);
features = (int)newFrame.sensorData().keypoints().size();
data.setFeatures(newFrame.sensorData().keypoints(), newFrame.sensorData().descriptors());
}
if((features >= registrationPipeline_->getMinVisualCorrespondences()) &&
@@ -183,6 +179,8 @@ Transform OdometryF2F::computeTransform(
UWARN("Registration failed: \"%s\"", regInfo.rejectedMsg.c_str());
}
data.setFeatures(newFrame.sensorData().keypoints(), newFrame.sensorData().descriptors());
if(info)
{
info->type = 1;

68
corelib/src/RegistrationVis.cpp
View File

@@ -564,15 +564,6 @@ Transform RegistrationVis::computeTransformationImpl(
std::vector<cv::Point2f> projected;
cv::projectPoints(kptsFrom3D, rvec, tvec, K, cv::Mat(), projected);
/*UDEBUG("guess=%s", guess.prettyPrint().c_str());
std::vector<cv::KeyPoint> projectedKpts;
cv::KeyPoint::convert(projected, projectedKpts);
cv::Mat image = toSignature.sensorData().imageRaw().clone();
drawKeypoints(image, projectedKpts, image, cv::Scalar(255,0,0));
drawKeypoints(image, kptsTo, image, cv::Scalar(0,0,255));
cv::imwrite("projected.bmp", image);
UWARN("saved projected.bmp");*/
//remove projected points outside of the image
UASSERT((int)projected.size() == descriptorsFrom.rows);
std::vector<cv::Point2f> cornersProjected(projected.size());
@@ -621,10 +612,11 @@ Transform RegistrationVis::computeTransformationImpl(
UDEBUG("");
// Process results (Nearest Neighbor Distance Ratio)
int notToMatchedUniqueId = descriptorsFrom.rows+descriptorsTo.rows; // make sure new words from "to" are after older one of "from"
int notFromMatchedUniqueId = descriptorsTo.rows;
int matchedID = descriptorsFrom.rows+descriptorsTo.rows;
int newToId = descriptorsFrom.rows;
int notMatchedFromId = 0;
std::map<int,int> addedWordsFrom; //<id, index>
std::set<int> duplicates;
std::map<int, int> duplicates; //<fromId, toId>
int newWords = 0;
for(unsigned int i = 0; i < pointsToMat.rows; ++i)
{
@@ -657,12 +649,12 @@ Transform RegistrationVis::computeTransformationImpl(
if(matchedIndex >= 0)
{
matchedIndex = projectedIndexToDescIndex[matchedIndex];
int id = i;
int id = matchedID++;
if(addedWordsFrom.find(matchedIndex) != addedWordsFrom.end())
{
id = addedWordsFrom.at(matchedIndex);
duplicates.insert(matchedIndex);
duplicates.insert(std::make_pair(matchedIndex, id));
}
else
{
@@ -686,14 +678,14 @@ Transform RegistrationVis::computeTransformationImpl(
else
{
// gen fake ids
wordsTo.insert(std::make_pair(notToMatchedUniqueId, kptsTo[i]));
wordsDescTo.insert(std::make_pair(notToMatchedUniqueId, descriptorsTo.row(i)));
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(notToMatchedUniqueId, kptsTo3D[i]));
words3To.insert(std::make_pair(newToId, kptsTo3D[i]));
}
++notToMatchedUniqueId;
++newToId;
++newWords;
}
}
@@ -703,17 +695,6 @@ Transform RegistrationVis::computeTransformationImpl(
(int)addedWordsFrom.size(), (int)cornersProjected.size(), (int)duplicates.size(), newWords,
(int)kptsTo.size(), (int)wordsTo.size(), (int)words3From.size());
//remove duplicates
for(std::set<int>::iterator iter=duplicates.begin(); iter!=duplicates.end(); ++iter)
{
wordsFrom.erase(*iter);
wordsDescFrom.erase(*iter);
words3From.erase(*iter);
wordsTo.erase(*iter);
wordsDescTo.erase(*iter);
words3To.erase(*iter);
}
// create fake ids for not matched words from "from"
int addWordsFromNotMatched = 0;
for(unsigned int i=0; i<kptsFrom3D.size(); ++i)
@@ -722,16 +703,37 @@ Transform RegistrationVis::computeTransformationImpl(
{
if(kptsFrom.size())
{
wordsFrom.insert(std::make_pair(notFromMatchedUniqueId, kptsFrom[i]));
wordsFrom.insert(std::make_pair(notMatchedFromId, kptsFrom[i]));
}
wordsDescFrom.insert(std::make_pair(notFromMatchedUniqueId, descriptorsFrom.row(i)));
words3From.insert(std::make_pair(notFromMatchedUniqueId, kptsFrom3D[i]));
wordsDescFrom.insert(std::make_pair(notMatchedFromId, descriptorsFrom.row(i)));
words3From.insert(std::make_pair(notMatchedFromId, kptsFrom3D[i]));
++notFromMatchedUniqueId;
++notMatchedFromId;
++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 >= descriptorsFrom.rows+descriptorsTo.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(projected, projectedKpts);
cv::Mat image = toSignature.sensorData().imageRaw().clone();
drawKeypoints(image, projectedKpts, image, cv::Scalar(255,0,0));
drawKeypoints(image, kptsTo, image, cv::Scalar(0,0,255));
drawKeypoints(image, matches, image, cv::Scalar(0,255,0));
cv::imwrite("projected.bmp", image);
UWARN("saved projected.bmp");*/
}
UDEBUG("");
}

4
corelib/src/Transform.cpp
View File

@@ -375,7 +375,7 @@ Transform Transform::fromEigen3d(const Eigen::Isometry3d & matrix)
Transform Transform::fromString(const std::string & string)
{
std::list<std::string> list = uSplit(string, ' ');
UASSERT_MSG(list.size() == 3 || list.size() == 6 || list.size() == 7 || list.size() == 9 || list.size() == 12,
UASSERT_MSG(list.empty() || list.size() == 3 || list.size() == 6 || list.size() == 7 || list.size() == 9 || list.size() == 12,
uFormat("Cannot parse \"%s\"", string.c_str()).c_str());
std::vector<float> numbers(list.size());
@@ -424,7 +424,7 @@ Transform Transform::fromString(const std::string & string)
bool Transform::canParseString(const std::string & string)
{
std::list<std::string> list = uSplit(string, ' ');
return list.size() == 3 || list.size() == 6 || list.size() == 7 || list.size() == 9 || list.size() == 12;
return list.size() == 0 || list.size() == 3 || list.size() == 6 || list.size() == 7 || list.size() == 9 || list.size() == 12;
}
}

7
corelib/src/util3d_motion_estimation.cpp
View File

@@ -144,7 +144,12 @@ Transform estimateMotion3DTo2D(
{
const cv::Point3f & objPt = objectPoints[inliers[i]];
cv::Point3f newPt = util3d::transformPoint(iter->second, transform);
errorSqrdDists[oi++] = uNormSquared(objPt.x-newPt.x, objPt.y-newPt.y, objPt.z-newPt.z);
errorSqrdDists[oi] = uNormSquared(objPt.x-newPt.x, objPt.y-newPt.y, objPt.z-newPt.z);
//ignore very very far features (stereo)
if(errorSqrdDists[oi] < 100.0f)
{
++oi;
}
}
}
errorSqrdDists.resize(oi);