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

Fixed SIFT octave issue causing registration to always fail. Added Bundler export points option.

Browse Source
This commit is contained in:
matlabbe
2018-10-24 20:01:57 -04:00
parent 299bec15ff
commit c14e20330f
15 changed files with 513 additions and 257 deletions
Download Patch File Download Diff File Expand all files Collapse all files

28
corelib/include/rtabmap/core/Optimizer.h
View File

@@ -38,6 +38,19 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap {
class FeatureBA
{
public:
FeatureBA(const cv::KeyPoint & kptIn, const float & depthIn = 0.0f, const cv::Mat & descriptorIn = cv::Mat()):
kpt(kptIn),
depth(depthIn),
descriptor(descriptorIn)
{}
cv::KeyPoint kpt;
float depth;
cv::Mat descriptor;
};
////////////////////////////////////////////
// Graph optimizers
////////////////////////////////////////////
@@ -118,9 +131,18 @@ public:
const std::multimap<int, Link> & links,
const std::map<int, CameraModel> & models, // in case of stereo, Tx should be set
std::map<int, cv::Point3f> & points3DMap,
const std::map<int, std::map<int, cv::Point3f> > & wordReferences, // <ID words, IDs frames + keypoint(x,y,depth)>
const std::map<int, std::map<int, FeatureBA> > & wordReferences, // <ID words, IDs frames + keypoint/depth/descriptor>
std::set<int> * outliers = 0);
std::map<int, Transform> optimizeBA(
int rootId,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & links,
const std::map<int, Signature> & signatures,
std::map<int, cv::Point3f> & points3DMap,
std::map<int, std::map<int, FeatureBA> > & wordReferences); // <ID words, IDs frames + keypoint/depth/descriptor>
std::map<int, Transform> optimizeBA(
int rootId,
const std::map<int, Transform> & poses,
@@ -131,7 +153,7 @@ public:
const Link & link,
const CameraModel & model,
std::map<int, cv::Point3f> & points3DMap,
const std::map<int, std::map<int, cv::Point3f> > & wordReferences,
const std::map<int, std::map<int, FeatureBA> > & wordReferences,
std::set<int> * outliers = 0);
void computeBACorrespondences(
@@ -139,7 +161,7 @@ public:
const std::multimap<int, Link> & links,
const std::map<int, Signature> & signatures,
std::map<int, cv::Point3f> & points3DMap,
std::map<int, std::map<int, cv::Point3f> > & wordReferences); // <ID words, IDs frames + keypoint/depth>
std::map<int, std::map<int, FeatureBA > > & wordReferences); // <ID words, IDs frames + keypoint/depth/descriptor>
protected:
Optimizer(

3
corelib/include/rtabmap/core/odometry/OdometryF2M.h
View File

@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#define ODOMETRYF2M_H_
#include <rtabmap/core/Odometry.h>
#include <rtabmap/core/Optimizer.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/pcl_base.h>
@@ -74,7 +75,7 @@ private:
int lastFrameOldestNewId_;
std::vector<std::pair<pcl::PointCloud<pcl::PointNormal>::Ptr, pcl::IndicesPtr> > scansBuffer_;
std::map<int, std::map<int, cv::Point3f> > bundleWordReferences_; //<WordId, <FrameId, pt2D+depth>>
std::map<int, std::map<int, FeatureBA> > bundleWordReferences_; //<WordId, <FrameId, pt2D+depth>>
std::map<int, Transform> bundlePoses_;
std::multimap<int, Link> bundleLinks_;
std::map<int, CameraModel> bundleModels_;

2
corelib/include/rtabmap/core/optimizer/OptimizerCVSBA.h
View File

@@ -57,7 +57,7 @@ public:
const std::multimap<int, Link> & links,
const std::map<int, CameraModel> & models,
std::map<int, cv::Point3f> & points3DMap,
const std::map<int, std::map<int, cv::Point3f> > & wordReferences, // <ID words, IDs frames + keypoint(x,y,depth)>
const std::map<int, std::map<int, FeatureBA> > & wordReferences, // <ID words, IDs frames + keypoint(x,y,depth)>
std::set<int> * outliers = 0);
};

2
corelib/include/rtabmap/core/optimizer/OptimizerG2O.h
View File

@@ -73,7 +73,7 @@ public:
const std::multimap<int, Link> & links,
const std::map<int, CameraModel> & models, // in case of stereo, Tx should be set
std::map<int, cv::Point3f> & points3DMap,
const std::map<int, std::map<int, cv::Point3f> > & wordReferences, // <ID words, IDs frames + keypoint(x,y,depth)>
const std::map<int, std::map<int, FeatureBA> > & wordReferences, // <ID words, IDs frames + keypoint(x,y,depth)>
std::set<int> * outliers = 0);
bool saveGraph(

6
corelib/src/Memory.cpp
View File

@@ -2599,7 +2599,7 @@ Transform Memory::computeTransform(
std::map<int, Transform> bundlePoses;
std::multimap<int, Link> bundleLinks;
std::map<int, CameraModel> bundleModels;
std::map<int, std::map<int, cv::Point3f> > wordReferences;
std::map<int, std::map<int, FeatureBA> > wordReferences;
std::map<int, Link> links = fromS.getLinks();
links.insert(std::make_pair(toS.id(), Link(fromS.id(), toS.id(), Link::kGlobalClosure, transform, info->covariance.inv())));
@@ -2656,8 +2656,8 @@ Transform Memory::computeTransform(
{
std::multimap<int, cv::Point3f>::const_iterator kter = s->getWords3().find(jter->first);
cv::Point3f pt3d = util3d::transformPoint(kter->second, invLocalTransform);
wordReferences.insert(std::make_pair(jter->first, std::map<int, cv::Point3f>()));
wordReferences.at(jter->first).insert(std::make_pair(id, cv::Point3f(jter->second.pt.x, jter->second.pt.y, pt3d.z)));
wordReferences.insert(std::make_pair(jter->first, std::map<int, FeatureBA>()));
wordReferences.at(jter->first).insert(std::make_pair(id, FeatureBA(jter->second, pt3d.z)));
}
}
}

39
corelib/src/Optimizer.cpp
View File

@@ -359,7 +359,7 @@ std::map<int, Transform> Optimizer::optimizeBA(
const std::multimap<int, Link> & links,
const std::map<int, CameraModel> & models,
std::map<int, cv::Point3f> & points3DMap,
const std::map<int, std::map<int, cv::Point3f> > & wordReferences,
const std::map<int, std::map<int, FeatureBA> > & wordReferences,
std::set<int> * outliers)
{
UERROR("Optimizer %d doesn't implement optimizeBA() method.", (int)this->type());
@@ -370,7 +370,9 @@ std::map<int, Transform> Optimizer::optimizeBA(
int rootId,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & links,
const std::map<int, Signature> & signatures)
const std::map<int, Signature> & signatures,
std::map<int, cv::Point3f> & points3DMap,
std::map<int, std::map<int, FeatureBA> > & wordReferences)
{
UDEBUG("");
std::map<int, CameraModel> models;
@@ -415,18 +417,27 @@ std::map<int, Transform> Optimizer::optimizeBA(
}
// compute correspondences
std::map<int, cv::Point3f> points3DMap;
std::map<int, std::map<int, cv::Point3f> > wordReferences;
this->computeBACorrespondences(poses, links, signatures, points3DMap, wordReferences);
return optimizeBA(rootId, poses, links, models, points3DMap, wordReferences);
}
std::map<int, Transform> Optimizer::optimizeBA(
int rootId,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & links,
const std::map<int, Signature> & signatures)
{
std::map<int, cv::Point3f> points3DMap;
std::map<int, std::map<int, FeatureBA> > wordReferences;
return optimizeBA(rootId, poses, links, signatures, points3DMap, wordReferences);
}
Transform Optimizer::optimizeBA(
const Link & link,
const CameraModel & model,
std::map<int, cv::Point3f> & points3DMap,
const std::map<int, std::map<int, cv::Point3f> > & wordReferences,
const std::map<int, std::map<int, FeatureBA> > & wordReferences,
std::set<int> * outliers)
{
std::map<int, Transform> poses;
@@ -453,7 +464,7 @@ void Optimizer::computeBACorrespondences(
const std::multimap<int, Link> & links,
const std::map<int, Signature> & signatures,
std::map<int, cv::Point3f> & points3DMap,
std::map<int, std::map<int, cv::Point3f> > & wordReferences) // <ID words, IDs frames + keypoint/depth>
std::map<int, std::map<int, FeatureBA> > & wordReferences)
{
UDEBUG("");
int wordCount = 0;
@@ -465,7 +476,8 @@ void Optimizer::computeBACorrespondences(
{
link = link.inverse();
}
if(uContains(signatures, link.from()) &&
if(link.to() != link.from() &&
uContains(signatures, link.from()) &&
uContains(signatures, link.to()) &&
uContains(poses, link.from()))
{
@@ -513,13 +525,14 @@ void Optimizer::computeBACorrespondences(
{
int wordId = ++wordCount;
wordReferences.insert(std::make_pair(wordId, std::map<int, cv::Point3f>()));
wordReferences.insert(std::make_pair(wordId, std::map<int, FeatureBA>()));
cv::Point2f pt = sFrom.getWords().lower_bound(info.inliersIDs[i])->second.pt;
wordReferences.at(wordId).insert(std::make_pair(sFrom.id(), cv::Point3f(pt.x, pt.y, p.x)));
cv::KeyPoint ptFrom = sFrom.getWords().lower_bound(info.inliersIDs[i])->second;
cv::Mat descriptorFrom = sFrom.getWordsDescriptors().lower_bound(info.inliersIDs[i])->second;
wordReferences.at(wordId).insert(std::make_pair(sFrom.id(), FeatureBA(ptFrom, p.x, descriptorFrom)));
pt = sTo.getWords().lower_bound(info.inliersIDs[i])->second.pt;
cv::KeyPoint ptTo = sTo.getWords().lower_bound(info.inliersIDs[i])->second;
cv::Mat descriptorTo = sTo.getWordsDescriptors().lower_bound(info.inliersIDs[i])->second;
float depth = 0.0f;
std::multimap<int, cv::Point3f>::const_iterator iterTo = sTo.getWords3().lower_bound(info.inliersIDs[i]);
if( iterTo!=sTo.getWords3().end() &&
@@ -527,7 +540,7 @@ void Optimizer::computeBACorrespondences(
{
depth = iterTo->second.x;
}
wordReferences.at(wordId).insert(std::make_pair(sTo.id(), cv::Point3f(pt.x, pt.y, depth)));
wordReferences.at(wordId).insert(std::make_pair(sTo.id(), FeatureBA(ptTo, depth, descriptorTo)));
p = util3d::transformPoint(p, pose);
points3DMap.insert(std::make_pair(wordId, p));

44
corelib/src/RegistrationVis.cpp
View File

@@ -825,7 +825,9 @@ Transform RegistrationVis::computeTransformationImpl(
{
if(kptsTo3D.empty() || util3d::isFinite(kptsTo3D[i]))
{
int octave = kptsTo[i].octave;
// Make octave compatible with SIFT packed octave (https://github.com/opencv/opencv/issues/4554)
int octave = kptsTo[i].octave & 255;
octave = octave < 128 ? octave : (-128 | octave);
int matchedIndex = -1;
if(indices[i].size() >= 2)
{
@@ -837,7 +839,9 @@ Transform RegistrationVis::computeTransformationImpl(
}
for(unsigned int j=0; j<indices[i].size(); ++j)
{
if(kptsFrom.at(projectedIndexToDescIndex[indices[i].at(j)]).octave==octave)
int octaveFrom = kptsFrom.at(projectedIndexToDescIndex[indices[i].at(j)]).octave & 255;
octaveFrom = octaveFrom < 128 ? octaveFrom : (-128 | octaveFrom);
if(octaveFrom==octave)
{
descriptorsFrom.row(projectedIndexToDescIndex[indices[i].at(j)]).copyTo(descriptors.row(oi));
descriptorsIndices[oi++] = indices[i].at(j);
@@ -861,10 +865,14 @@ Transform RegistrationVis::computeTransformationImpl(
matchedIndex = descriptorsIndices[0];
}
}
else if(indices[i].size() == 1 &&
kptsFrom.at(projectedIndexToDescIndex[indices[i].at(0)]).octave == octave)
else if(indices[i].size() == 1)
{
matchedIndex = indices[i].at(0);
int octaveFrom = kptsFrom.at(projectedIndexToDescIndex[indices[i].at(0)]).octave & 255;
octaveFrom = octaveFrom < 128 ? octaveFrom : (-128 | octaveFrom);
if(octaveFrom == octave)
{
matchedIndex = indices[i].at(0);
}
}
if(matchedIndex >= 0)
@@ -972,6 +980,10 @@ Transform RegistrationVis::computeTransformationImpl(
if(util3d::isFinite(kptsFrom3D[matchedIndexFrom]))
{
// Make octave compatible with SIFT packed octave (https://github.com/opencv/opencv/issues/4554)
int octaveFrom = kptsFrom.at(matchedIndexFrom).octave & 255;
octaveFrom = octaveFrom < 128 ? octaveFrom : (-128 | octaveFrom);
int matchedIndexTo = -1;
if(indices[i].size() >= 2)
{
@@ -985,8 +997,9 @@ Transform RegistrationVis::computeTransformationImpl(
std::list<int> indicesToIgnoretmp;
for(unsigned int j=0; j<indices[i].size(); ++j)
{
int octave = kptsTo[indices[i].at(j)].octave;
if(kptsFrom.at(matchedIndexFrom).octave==octave)
int octave = kptsTo[indices[i].at(j)].octave & 255;
octave = octave < 128 ? octave : (-128 | octave);
if(octaveFrom==octave)
{
descriptorsTo.row(indices[i].at(j)).copyTo(descriptors.row(oi));
descriptorsIndices[oi++] = indices[i].at(j);
@@ -1017,8 +1030,9 @@ Transform RegistrationVis::computeTransformationImpl(
}
else if(indices[i].size() == 1)
{
int octave = kptsTo[indices[i].at(0)].octave;
if(kptsFrom.at(matchedIndexFrom).octave == octave)
int octave = kptsTo[indices[i].at(0)].octave & 255;
octave = octave < 128 ? octave : (-128 | octave);
if(octaveFrom == octave)
{
matchedIndexTo = indices[i].at(0);
}
@@ -1535,26 +1549,26 @@ Transform RegistrationVis::computeTransformationImpl(
models.insert(std::make_pair(1, cameraModelFrom.isValidForProjection()?cameraModelFrom:cameraModelTo));
models.insert(std::make_pair(2, cameraModelTo));
std::map<int, std::map<int, cv::Point3f> > wordReferences;
std::map<int, std::map<int, FeatureBA> > wordReferences;
for(unsigned int i=0; i<allInliers.size(); ++i)
{
int wordId = allInliers[i];
const cv::Point3f & pt3D = fromSignature.getWords3().find(wordId)->second;
points3DMap.insert(std::make_pair(wordId, pt3D));
std::map<int, cv::Point3f> ptMap;
std::map<int, FeatureBA> 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, depthFrom)));
const cv::KeyPoint & kpt = fromSignature.getWords().find(wordId)->second;
ptMap.insert(std::make_pair(1,FeatureBA(kpt, 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;
const cv::KeyPoint & kpt = toSignature.getWords().find(wordId)->second;
UASSERT(toSignature.getWords3().find(wordId) != toSignature.getWords3().end());
ptMap.insert(std::make_pair(2,cv::Point3f(kpt.x, kpt.y, depthTo)));
ptMap.insert(std::make_pair(2,FeatureBA(kpt, depthTo)));
}
wordReferences.insert(std::make_pair(wordId, ptMap));

34
corelib/src/odometry/OdometryF2M.cpp
View File

@@ -338,7 +338,7 @@ Transform OdometryF2M::computeTransform(
Transform invLocalTransform = model.localTransform().inverse();
UDEBUG("Fill matches (%d)", (int)regInfo.inliersIDs.size());
std::map<int, std::map<int, cv::Point3f> > wordReferences;
std::map<int, std::map<int, FeatureBA> > wordReferences;
for(unsigned int i=0; i<regInfo.inliersIDs.size(); ++i)
{
int wordId =regInfo.inliersIDs[i];
@@ -351,17 +351,17 @@ Transform OdometryF2M::computeTransform(
std::multimap<int, cv::KeyPoint>::const_iterator iter2D = lastFrame_->getWords().find(wordId);
// all other references
std::map<int, std::map<int, cv::Point3f> >::iterator refIter = bundleWordReferences_.find(wordId);
std::map<int, std::map<int, FeatureBA> >::iterator refIter = bundleWordReferences_.find(wordId);
UASSERT_MSG(refIter != bundleWordReferences_.end(), uFormat("wordId=%d", wordId).c_str());
std::map<int, cv::Point3f> references;
std::map<int, FeatureBA> references;
int step = bundleMaxFrames_>0?(refIter->second.size() / bundleMaxFrames_):1;
if(step == 0)
{
step = 1;
}
int oi=0;
for(std::map<int, cv::Point3f>::iterator jter=refIter->second.begin(); jter!=refIter->second.end(); ++jter)
for(std::map<int, FeatureBA>::iterator jter=refIter->second.begin(); jter!=refIter->second.end(); ++jter)
{
if(oi++ % step == 0 && bundlePoses.find(jter->first)!=bundlePoses.end())
{
@@ -383,7 +383,7 @@ Transform OdometryF2M::computeTransform(
UASSERT(lastFrame_->getWords3().find(wordId) != lastFrame_->getWords3().end());
//move back point in camera frame (to get depth along z)
cv::Point3f pt3d = util3d::transformPoint(lastFrame_->getWords3().find(wordId)->second, invLocalTransform);
references.insert(std::make_pair(lastFrame_->id(), cv::Point3f(iter2D->second.pt.x, iter2D->second.pt.y, pt3d.z)));
references.insert(std::make_pair(lastFrame_->id(), FeatureBA(iter2D->second, pt3d.z)));
}
wordReferences.insert(std::make_pair(wordId, references));
@@ -589,13 +589,13 @@ Transform OdometryF2M::computeTransform(
cv::Point3f pt3d = util3d::transformPoint(iter->second, invLocalTransform);
if(bundleWordReferences_.find(iter->first) == bundleWordReferences_.end())
{
std::map<int, cv::Point3f> framePt;
framePt.insert(std::make_pair(lastFrame_->id(), cv::Point3f(iter2D->second.pt.x, iter2D->second.pt.y, pt3d.z)));
std::map<int, FeatureBA> framePt;
framePt.insert(std::make_pair(lastFrame_->id(), FeatureBA(iter2D->second, pt3d.z)));
bundleWordReferences_.insert(std::make_pair(iter->first, framePt));
}
else
{
bundleWordReferences_.find(iter->first)->second.insert(std::make_pair(lastFrame_->id(), cv::Point3f(iter2D->second.pt.x, iter2D->second.pt.y, pt3d.z)));
bundleWordReferences_.find(iter->first)->second.insert(std::make_pair(lastFrame_->id(), FeatureBA(iter2D->second, pt3d.z)));
}
}
}
@@ -622,13 +622,13 @@ Transform OdometryF2M::computeTransform(
cv::Point3f pt3d = util3d::transformPoint(iter->second.second.second.first, invLocalTransform);
if(bundleWordReferences_.find(iter->second.first) == bundleWordReferences_.end())
{
std::map<int, cv::Point3f> framePt;
framePt.insert(std::make_pair(lastFrame_->id(), cv::Point3f(iter->second.second.first.pt.x, iter->second.second.first.pt.y, pt3d.z)));
std::map<int, FeatureBA> framePt;
framePt.insert(std::make_pair(lastFrame_->id(), FeatureBA(iter->second.second.first, pt3d.z)));
bundleWordReferences_.insert(std::make_pair(iter->second.first, framePt));
}
else
{
bundleWordReferences_.find(iter->second.first)->second.insert(std::make_pair(lastFrame_->id(), cv::Point3f(iter->second.second.first.pt.x, iter->second.second.first.pt.y, pt3d.z)));
bundleWordReferences_.find(iter->second.first)->second.insert(std::make_pair(lastFrame_->id(), FeatureBA(iter->second.second.first, pt3d.z)));
}
}
}
@@ -669,10 +669,10 @@ Transform OdometryF2M::computeTransform(
int id = ids.at(i);
if(inliers.find(id) == inliers.end())
{
std::map<int, std::map<int, cv::Point3f> >::iterator iterRef = bundleWordReferences_.find(id);
std::map<int, std::map<int, FeatureBA> >::iterator iterRef = bundleWordReferences_.find(id);
if(iterRef != bundleWordReferences_.end())
{
for(std::map<int, cv::Point3f>::iterator iterFrame = iterRef->second.begin(); iterFrame != iterRef->second.end(); ++iterFrame)
for(std::map<int, FeatureBA>::iterator iterFrame = iterRef->second.begin(); iterFrame != iterRef->second.end(); ++iterFrame)
{
if(bundlePoseReferences_.find(iterFrame->first) != bundlePoseReferences_.end())
{
@@ -697,10 +697,10 @@ Transform OdometryF2M::computeTransform(
{
if(inliers.find(iter->first) == inliers.end())
{
std::map<int, std::map<int, cv::Point3f> >::iterator iterRef = bundleWordReferences_.find(iter->first);
std::map<int, std::map<int, FeatureBA> >::iterator iterRef = bundleWordReferences_.find(iter->first);
if(iterRef != bundleWordReferences_.end())
{
for(std::map<int, cv::Point3f>::iterator iterFrame = iterRef->second.begin(); iterFrame != iterRef->second.end(); ++iterFrame)
for(std::map<int, FeatureBA>::iterator iterFrame = iterRef->second.begin(); iterFrame != iterRef->second.end(); ++iterFrame)
{
if(bundlePoseReferences_.find(iterFrame->first) != bundlePoseReferences_.end())
{
@@ -983,7 +983,7 @@ Transform OdometryF2M::computeTransform(
if(words.count(iter->first) == 1)
{
UASSERT(bundleWordReferences_.find(iter->first) == bundleWordReferences_.end());
std::map<int, cv::Point3f> framePt;
std::map<int, FeatureBA> framePt;
//get depth
float d = 0.0f;
@@ -995,7 +995,7 @@ Transform OdometryF2M::computeTransform(
}
framePt.insert(std::make_pair(lastFrame_->id(), cv::Point3f(iter->second.pt.x, iter->second.pt.y, d)));
framePt.insert(std::make_pair(lastFrame_->id(), FeatureBA(iter->second, d)));
bundleWordReferences_.insert(std::make_pair(iter->first, framePt));
}
}

8
corelib/src/optimizer/OptimizerCVSBA.cpp
View File

@@ -59,7 +59,7 @@ std::map<int, Transform> OptimizerCVSBA::optimizeBA(
const std::multimap<int, Link> & links,
const std::map<int, CameraModel> & models,
std::map<int, cv::Point3f> & points3DMap,
const std::map<int, std::map<int, cv::Point3f> > & wordReferences, // <ID words, IDs frames + keypoint/Disparity>)
const std::map<int, std::map<int, FeatureBA> > & wordReferences, // <ID words, IDs frames + keypoint/Disparity>)
std::set<int> * outliers)
{
#ifdef RTABMAP_CVSBA
@@ -131,14 +131,14 @@ std::map<int, Transform> OptimizerCVSBA::optimizeBA(
{
points[i] = kter->second;
std::map<int, std::map<int, cv::Point3f> >::const_iterator iter = wordReferences.find(kter->first);
std::map<int, std::map<int, FeatureBA> >::const_iterator iter = wordReferences.find(kter->first);
if(iter != wordReferences.end())
{
for(std::map<int, cv::Point3f>::const_iterator jter=iter->second.begin(); jter!=iter->second.end(); ++jter)
for(std::map<int, FeatureBA>::const_iterator jter=iter->second.begin(); jter!=iter->second.end(); ++jter)
{
if(frameIdToIndex.find(jter->first) != frameIdToIndex.end())
{
imagePoints[frameIdToIndex.at(jter->first)][i] = cv::Point2f(jter->second.x, jter->second.y);
imagePoints[frameIdToIndex.at(jter->first)][i] = cv::Point2f(jter->second.kpt.pt.x, jter->second.kpt.pt.y);
visibility[frameIdToIndex.at(jter->first)][i] = 1;
}
}

18
corelib/src/optimizer/OptimizerG2O.cpp
View File

@@ -866,7 +866,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
const std::multimap<int, Link> & links,
const std::map<int, CameraModel> & models,
std::map<int, cv::Point3f> & points3DMap,
const std::map<int, std::map<int, cv::Point3f> > & wordReferences,
const std::map<int, std::map<int, FeatureBA> > & wordReferences,
std::set<int> * outliers)
{
std::map<int, Transform> optimizedPoses;
@@ -1070,7 +1070,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
}
UDEBUG("stepVertexId=%d, negVertexOffset=%d", stepVertexId, negVertexOffset);
std::list<g2o::OptimizableGraph::Edge*> edges;
for(std::map<int, std::map<int, cv::Point3f> >::const_iterator iter = wordReferences.begin(); iter!=wordReferences.end(); ++iter)
for(std::map<int, std::map<int, FeatureBA> >::const_iterator iter = wordReferences.begin(); iter!=wordReferences.end(); ++iter)
{
int id = iter->first;
if(points3DMap.find(id) != points3DMap.end())
@@ -1094,13 +1094,13 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
//UDEBUG("Added 3D point %d (%f,%f,%f)", vpt3d->id()-stepVertexId, pt3d.x, pt3d.y, pt3d.z);
// set observations
for(std::map<int, cv::Point3f>::const_iterator jter=iter->second.begin(); jter!=iter->second.end(); ++jter)
for(std::map<int, FeatureBA>::const_iterator jter=iter->second.begin(); jter!=iter->second.end(); ++jter)
{
int camId = jter->first;
if(poses.find(camId) != poses.end() && optimizer.vertex(camId) != 0)
{
const cv::Point3f & pt = jter->second;
double depth = pt.z;
const FeatureBA & pt = jter->second;
double depth = pt.depth;
//UDEBUG("Added observation pt=%d to cam=%d (%f,%f) depth=%f", vpt3d->id()-stepVertexId, camId, pt.x, pt.y, depth);
@@ -1123,7 +1123,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
#ifdef RTABMAP_ORB_SLAM2
g2o::EdgeStereoSE3ProjectXYZ* es = new g2o::EdgeStereoSE3ProjectXYZ();
float disparity = baseline * iterModel->second.fx() / depth;
Eigen::Vector3d obs( pt.x, pt.y, pt.x-disparity);
Eigen::Vector3d obs( pt.kpt.pt.x, pt.kpt.pt.y, pt.kpt.pt.x-disparity);
es->setMeasurement(obs);
//variance *= log(exp(1)+disparity);
es->setInformation(Eigen::Matrix3d::Identity() / variance);
@@ -1136,7 +1136,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
#else
g2o::EdgeProjectP2SC* es = new g2o::EdgeProjectP2SC();
float disparity = baseline * vcam->estimate().Kcam(0,0) / depth;
Eigen::Vector3d obs( pt.x, pt.y, pt.x-disparity);
Eigen::Vector3d obs( pt.kpt.pt.x, pt.kpt.pt.y, pt.kpt.pt.x-disparity);
es->setMeasurement(obs);
//variance *= log(exp(1)+disparity);
es->setInformation(Eigen::Matrix3d::Identity() / variance);
@@ -1155,7 +1155,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
// mono edge
#ifdef RTABMAP_ORB_SLAM2
g2o::EdgeSE3ProjectXYZ* em = new g2o::EdgeSE3ProjectXYZ();
Eigen::Vector2d obs( pt.x, pt.y);
Eigen::Vector2d obs( pt.kpt.pt.x, pt.kpt.pt.y);
em->setMeasurement(obs);
em->setInformation(Eigen::Matrix2d::Identity() / variance);
em->fx = iterModel->second.fx();
@@ -1166,7 +1166,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
#else
g2o::EdgeProjectP2MC* em = new g2o::EdgeProjectP2MC();
Eigen::Vector2d obs( pt.x, pt.y);
Eigen::Vector2d obs( pt.kpt.pt.x, pt.kpt.pt.y);
em->setMeasurement(obs);
em->setInformation(Eigen::Matrix2d::Identity() / variance);
e = em;