i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-08 12:30:20 +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
corelib/include/rtabmap/core/Optimizer.h
+25 -3
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(
corelib/include/rtabmap/core/odometry/OdometryF2M.h
+2 -1
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_;
corelib/include/rtabmap/core/optimizer/OptimizerCVSBA.h
+1 -1
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);
};
corelib/include/rtabmap/core/optimizer/OptimizerG2O.h
+1 -1
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(
corelib/src/Memory.cpp
+3 -3
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)));
}
}
}
corelib/src/Optimizer.cpp
+26 -13
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));
corelib/src/RegistrationVis.cpp
+29 -15
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));
corelib/src/odometry/OdometryF2M.cpp
+17 -17
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));
}
}
corelib/src/optimizer/OptimizerCVSBA.cpp
+4 -4
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;
}
}
corelib/src/optimizer/OptimizerG2O.cpp
+9 -9
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;
guilib/include/rtabmap/gui/ExportBundlerDialog.h
+5 -5
View File
@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines
#include <rtabmap/core/Signature.h>
#include <QDialog>
#include <QSettings>
@@ -51,11 +52,10 @@ public:
void setWorkingDirectory(const QString & path);
QString outputPath() const;
double maxLinearSpeed() const;
double maxAngularSpeed() const;
double laplacianThreshold() const;
void exportBundler(
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & links,
const QMap<int, Signature> & signatures);
Q_SIGNALS:
void configChanged();
guilib/src/ExportBundlerDialog.cpp
+357 -21
View File
@@ -27,9 +27,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/gui/ExportBundlerDialog.h"
#include "ui_exportBundlerDialog.h"
#include <rtabmap/utilite/UMath.h>
#include <rtabmap/core/Optimizer.h>
#include <rtabmap/core/util3d_transforms.h>
#include <QFileDialog>
#include <QPushButton>
#include <QMessageBox>
#include <QTextStream>
namespace rtabmap {
@@ -47,6 +51,9 @@ ExportBundlerDialog::ExportBundlerDialog(QWidget * parent) :
connect(_ui->doubleSpinBox_laplacianVariance, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
connect(_ui->doubleSpinBox_linearSpeed, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
connect(_ui->doubleSpinBox_angularSpeed, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
connect(_ui->checkBox_export_points, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
_ui->checkBox_export_points->setEnabled(Optimizer::isAvailable(Optimizer::kTypeG2O));
_ui->lineEdit_path->setText(QDir::currentPath());
}
@@ -62,9 +69,10 @@ void ExportBundlerDialog::saveSettings(QSettings & settings, const QString & gro
{
settings.beginGroup(group);
}
settings.setValue("maxLinearSpeed", this->maxLinearSpeed());
settings.setValue("maxAngularSpeed", this->maxAngularSpeed());
settings.setValue("laplacianThr", this->laplacianThreshold());
settings.setValue("maxLinearSpeed", _ui->doubleSpinBox_linearSpeed->value());
settings.setValue("maxAngularSpeed", _ui->doubleSpinBox_angularSpeed->value());
settings.setValue("laplacianThr", _ui->doubleSpinBox_laplacianVariance->value());
settings.setValue("exportPoints", _ui->checkBox_export_points->isChecked());
if(!group.isEmpty())
{
settings.endGroup();
@@ -77,9 +85,10 @@ void ExportBundlerDialog::loadSettings(QSettings & settings, const QString & gro
{
settings.beginGroup(group);
}
_ui->doubleSpinBox_linearSpeed->setValue(settings.value("maxLinearSpeed", this->maxLinearSpeed()).toDouble());
_ui->doubleSpinBox_angularSpeed->setValue(settings.value("maxAngularSpeed", this->maxAngularSpeed()).toDouble());
_ui->doubleSpinBox_laplacianVariance->setValue(settings.value("laplacianThr", this->laplacianThreshold()).toDouble());
_ui->doubleSpinBox_linearSpeed->setValue(settings.value("maxLinearSpeed", _ui->doubleSpinBox_linearSpeed->value()).toDouble());
_ui->doubleSpinBox_angularSpeed->setValue(settings.value("maxAngularSpeed", _ui->doubleSpinBox_angularSpeed->value()).toDouble());
_ui->doubleSpinBox_laplacianVariance->setValue(settings.value("laplacianThr", _ui->doubleSpinBox_laplacianVariance->value()).toDouble());
_ui->checkBox_export_points->setChecked(settings.value("exportPoints", _ui->checkBox_export_points->isChecked()).toBool());
if(!group.isEmpty())
{
settings.endGroup();
@@ -96,6 +105,7 @@ void ExportBundlerDialog::restoreDefaults()
_ui->doubleSpinBox_linearSpeed->setValue(0);
_ui->doubleSpinBox_angularSpeed->setValue(0);
_ui->doubleSpinBox_laplacianVariance->setValue(0);
_ui->checkBox_export_points->setChecked(false);
}
void ExportBundlerDialog::getPath()
@@ -107,22 +117,348 @@ void ExportBundlerDialog::getPath()
}
}
QString ExportBundlerDialog::outputPath() const
void ExportBundlerDialog::exportBundler(
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & links,
const QMap<int, Signature> & signatures)
{
return _ui->lineEdit_path->text();
}
if(this->exec() != QDialog::Accepted)
{
return;
}
QString path = _ui->lineEdit_path->text();
if(!path.isEmpty())
{
if(!QDir(path).mkpath("."))
{
QMessageBox::warning(this, tr("Exporting cameras..."), tr("Failed creating directory %1.").arg(path));
return;
}
double ExportBundlerDialog::maxLinearSpeed() const
{
return _ui->doubleSpinBox_linearSpeed->value();
}
double ExportBundlerDialog::maxAngularSpeed() const
{
return _ui->doubleSpinBox_angularSpeed->value();
}
double ExportBundlerDialog::laplacianThreshold() const
{
return _ui->doubleSpinBox_laplacianVariance->value();
std::map<int, cv::Point3f> points3DMap;
std::map<int, std::map<int, FeatureBA> > wordReferences;
std::map<int, Transform> newPoses = poses;
if(_ui->checkBox_export_points->isEnabled() && _ui->checkBox_export_points->isChecked())
{
std::map<int, Transform> posesOut;
std::multimap<int, Link> linksOut;
Optimizer * sba = Optimizer::create(Optimizer::kTypeG2O);
sba->getConnectedGraph(poses.begin()->first, poses, links, posesOut, linksOut);
newPoses = sba->optimizeBA(
posesOut.begin()->first,
posesOut,
linksOut,
signatures.toStdMap(),
points3DMap,
wordReferences);
delete sba;
if(newPoses.empty())
{
QMessageBox::warning(this, tr("Exporting cameras..."), tr("SBA optimization failed! Cannot export with 3D points.").arg(path));
return;
}
}
// export cameras and images
QFile fileOut(path+QDir::separator()+"cameras.out");
QFile fileList(path+QDir::separator()+"list.txt");
QFile fileListKeys(path+QDir::separator()+"list_keys.txt");
QDir(path).mkdir("images");
if(wordReferences.size())
{
QDir(path).mkdir("keys");
}
if(fileOut.open(QIODevice::WriteOnly | QIODevice::Text))
{
if(fileList.open(QIODevice::WriteOnly | QIODevice::Text))
{
std::map<int, Transform> cameras;
std::map<int, int> cameraIndexes;
int camIndex = 0;
for(std::map<int, Transform>::const_iterator iter=newPoses.begin(); iter!=newPoses.end(); ++iter)
{
if(signatures.find(iter->first) != signatures.end())
{
cv::Mat image = signatures[iter->first].sensorData().imageRaw();
if(image.empty())
{
signatures[iter->first].sensorData().uncompressDataConst(&image, 0, 0, 0);
}
double maxLinearVel = _ui->doubleSpinBox_linearSpeed->value();
double maxAngularVel = _ui->doubleSpinBox_angularSpeed->value();
double laplacianThr = _ui->doubleSpinBox_laplacianVariance->value();
bool blurryImage = false;
const std::vector<float> & velocity = signatures[iter->first].getVelocity();
if(maxLinearVel>0.0 || maxAngularVel>0.0)
{
if(velocity.size() == 6)
{
float transVel = uMax3(fabs(velocity[0]), fabs(velocity[1]), fabs(velocity[2]));
float rotVel = uMax3(fabs(velocity[3]), fabs(velocity[4]), fabs(velocity[5]));
if(maxLinearVel>0.0 && transVel > maxLinearVel)
{
UWARN("Fast motion detected for camera %d (speed=%f m/s > thr=%f m/s), camera is ignored for texturing.", iter->first, transVel, maxLinearVel);
blurryImage = true;
}
else if(maxAngularVel>0.0 && rotVel > maxAngularVel)
{
UWARN("Fast motion detected for camera %d (speed=%f rad/s > thr=%f rad/s), camera is ignored for texturing.", iter->first, rotVel, maxAngularVel);
blurryImage = true;
}
}
else
{
UWARN("Camera motion filtering is set, but velocity of camera %d is not available.", iter->first);
}
}
if(!blurryImage && !image.empty() && laplacianThr>0.0)
{
cv::Mat imgLaplacian;
cv::Laplacian(image, imgLaplacian, CV_16S);
cv::Mat m, s;
cv::meanStdDev(imgLaplacian, m, s);
double stddev_pxl = s.at<double>(0);
double var = stddev_pxl*stddev_pxl;
if(var < laplacianThr)
{
blurryImage = true;
UWARN("Camera's image %d is detected as blurry (var=%f < thr=%f), camera is ignored for texturing.", iter->first, var, laplacianThr);
}
}
if(!blurryImage)
{
cameras.insert(*iter);
cameraIndexes.insert(std::make_pair(iter->first, camIndex++));
QString p = QString("images")+QDir::separator()+tr("%1.jpg").arg(iter->first);
p = path+QDir::separator()+p;
if(cv::imwrite(p.toStdString(), image))
{
UINFO("saved image %s", p.toStdString().c_str());
}
else
{
UERROR("Failed to save image %s", p.toStdString().c_str());
}
//
// Descriptors
//
// The file format starts with 2 integers giving the total number of
// keypoints and the length of the descriptor vector for each keypoint
// (128). Then the location of each keypoint in the image is specified by
// 4 floating point numbers giving subpixel row and column location,
// scale, and orientation (in radians from -PI to PI). Obviously, these
// numbers are not invariant to viewpoint, but can be used in later
// stages of processing to check for geometric consistency among matches.
// Finally, the invariant descriptor vector for the keypoint is given as
// a list of 128 integers in range [0,255]. Keypoints from a new image
// can be matched to those from previous images by simply looking for the
// descriptor vector with closest Euclidean distance among all vectors
// from previous images.
//
if(wordReferences.size())
{
std::list<FeatureBA> descriptors;
for(std::map<int, std::map<int, FeatureBA> >::iterator jter=wordReferences.begin(); jter!=wordReferences.end(); ++jter)
{
for(std::map<int, FeatureBA>::iterator kter=jter->second.begin(); kter!=jter->second.end(); ++kter)
{
if(kter->first == iter->first)
{
descriptors.push_back(kter->second);
}
}
}
if(descriptors.size())
{
QString p = QString("keys")+QDir::separator()+tr("%1.key").arg(iter->first);
p = path+QDir::separator()+p;
QFile fileKey(p);
if(fileKey.open(QIODevice::WriteOnly | QIODevice::Text))
{
QTextStream key(&fileKey);
key << descriptors.size() << " " << descriptors.front().descriptor.cols << "\n";
for(std::list<FeatureBA>::iterator dter=descriptors.begin(); dter!=descriptors.end(); ++dter)
{
// unpack octave value to get the scale set by SIFT (https://github.com/opencv/opencv/issues/4554)
int octave = dter->kpt.octave & 255;
octave = octave < 128 ? octave : (-128 | octave);
float scale = octave >= 0 ? 1.f/(1 << octave) : (float)(1 << -octave);
key << dter->kpt.pt.x << " " << dter->kpt.pt.y << " " << scale << " " << dter->kpt.angle << "\n";
for(int i=0; i<dter->descriptor.cols; ++i)
{
if(dter->descriptor.type() == CV_8U)
{
key << " " << (int)dter->descriptor.at<unsigned char>(i);
}
else // assume CV_32F
{
key << " " << (int)dter->descriptor.at<float>(i);
}
if((i+1)%20 == 0 && i+1 < dter->descriptor.cols)
{
key << "\n";
}
}
key << "\n";
}
fileKey.close();
}
}
}
}
}
else
{
UWARN("Could not find signature data for pose %d", iter->first);
}
}
static const Transform opengl_world_T_rtabmap_world(
0.0f, -1.0f, 0.0f, 0.0f,
0.0f, 0.0f, 1.0f, 0.0f,
-1.0f, 0.0f, 0.0f, 0.0f);
static const Transform optical_rotation_inv(
0.0f, -1.0f, 0.0f, 0.0f,
0.0f, 0.0f, -1.0f, 0.0f,
1.0f, 0.0f, 0.0f, 0.0f);
QTextStream out(&fileOut);
QTextStream list(&fileList);
out << "# Bundle file v0.3\n";
out << cameras.size() << " " << points3DMap.size() << "\n";
//
// Each camera entry <cameraI> contains the estimated camera intrinsics and extrinsics, and has the form:
//
// <f> <k1> <k2> [the focal length, followed by two radial distortion coeffs]
// <R> [a 3x3 matrix representing the camera rotation]
// <t> [a 3-vector describing the camera translation]
//
// The cameras are specified in the order they appear in the list of images.
//
for(std::map<int, Transform>::iterator iter=cameras.begin(); iter!=cameras.end(); ++iter)
{
QString p = QString("images")+QDir::separator()+tr("%1.jpg").arg(iter->first);
list << p << "\n";
Transform localTransform;
if(signatures[iter->first].sensorData().cameraModels().size())
{
out << signatures[iter->first].sensorData().cameraModels().at(0).fx() << " 0 0\n";
localTransform = signatures[iter->first].sensorData().cameraModels().at(0).localTransform();
}
else
{
out << signatures[iter->first].sensorData().stereoCameraModel().left().fx() << " 0 0\n";
localTransform = signatures[iter->first].sensorData().stereoCameraModel().left().localTransform();
}
Transform pose = iter->second;
if(!localTransform.isNull())
{
pose*=localTransform*optical_rotation_inv;
}
Transform poseGL = opengl_world_T_rtabmap_world*pose.inverse();
out << poseGL.r11() << " " << poseGL.r12() << " " << poseGL.r13() << "\n";
out << poseGL.r21() << " " << poseGL.r22() << " " << poseGL.r23() << "\n";
out << poseGL.r31() << " " << poseGL.r32() << " " << poseGL.r33() << "\n";
out << poseGL.x() << " " << poseGL.y() << " " << poseGL.z() << "\n";
}
if(wordReferences.size())
{
if(fileListKeys.open(QIODevice::WriteOnly | QIODevice::Text))
{
QTextStream listKeys(&fileListKeys);
for(std::map<int, Transform>::iterator iter=cameras.begin(); iter!=cameras.end(); ++iter)
{
QString p = QString("keys")+QDir::separator()+tr("%1.key").arg(iter->first);
listKeys << p << "\n";
}
fileListKeys.close();
}
}
//
// Each point entry has the form:
//
// <position> [a 3-vector describing the 3D position of the point]
// <color> [a 3-vector describing the RGB color of the point]
// <view list> [a list of views the point is visible in]
//
// The view list begins with the length of the list (i.e., the number of cameras
// the point is visible in). The list is then given as a list of quadruplets
// <camera> <key> <x> <y>, where <camera> is a camera index, <key> the index
// of the SIFT keypoint where the point was detected in that camera, and <x>
// and <y> are the detected positions of that keypoint. Both indices are
// 0-based (e.g., if camera 0 appears in the list, this corresponds to the
// first camera in the scene file and the first image in "list.txt"). The
// pixel positions are floating point numbers in a coordinate system where
// the origin is the center of the image, the x-axis increases to the right,
// and the y-axis increases towards the top of the image. Thus, (-w/2, -h/2)
// is the lower-left corner of the image, and (w/2, h/2) is the top-right
// corner (where w and h are the width and height of the image).
//
std::map<int, int> descriptorIndexes;
for(std::map<int, cv::Point3f>::iterator iter=points3DMap.begin(); iter!=points3DMap.end(); ++iter)
{
std::map<int, std::map<int, FeatureBA> >::iterator jter = wordReferences.find(iter->first);
cv::Point3f pt3d = util3d::transformPoint(iter->second, opengl_world_T_rtabmap_world);
out << pt3d.x << " " << pt3d.y << " " << pt3d.z << "\n";
out << 255 << " " << 0 << " " << 0 << "\n"; // make them all red for now
out << jter->second.size();
for(std::map<int, FeatureBA>::iterator kter = jter->second.begin(); kter!=jter->second.end(); ++kter)
{
// <camera> <key> <x> <y>
int camId = kter->first;
UASSERT(signatures.contains(camId));
UASSERT(cameraIndexes.find(camId) != cameraIndexes.end());
const Signature & s = signatures[camId];
cv::Point2f pt;
if(signatures[camId].sensorData().cameraModels().size())
{
pt.x = kter->second.kpt.pt.x - s.sensorData().cameraModels().at(0).cx();
pt.y = kter->second.kpt.pt.y - s.sensorData().cameraModels().at(0).cy();
}
else
{
pt.x = kter->second.kpt.pt.x - s.sensorData().stereoCameraModel().left().cx();
pt.y = kter->second.kpt.pt.y - s.sensorData().stereoCameraModel().left().cy();
}
descriptorIndexes.insert(std::make_pair(camId, 0));
out << " " << cameraIndexes.at(camId) << " " << descriptorIndexes.at(camId)++ << " " << pt.x << " " << -pt.y;
}
out << "\n";
}
fileList.close();
fileOut.close();
QMessageBox::information(this,
tr("Exporting cameras in Bundler format..."),
tr("%1 cameras/images and %2 points exported to directory \"%3\".%4")
.arg(newPoses.size())
.arg(points3DMap.size())
.arg(path)
.arg(newPoses.size()>cameras.size()?tr(" %1/%2 cameras ignored for too fast motion and/or blur level.").arg(newPoses.size()-cameras.size()).arg(newPoses.size()):""));
}
else
{
fileOut.close();
QMessageBox::warning(this, tr("Exporting cameras..."), tr("Failed opening file %1 for writing.").arg(path+QDir::separator()+"list.txt"));
}
}
else
{
QMessageBox::warning(this, tr("Exporting cameras..."), tr("Failed opening file %1 for writing.").arg(path+QDir::separator()+"cameras.out"));
}
}
}
}
guilib/src/KeypointItem.cpp
+8 -2
View File
@@ -74,6 +74,10 @@ void KeypointItem::showDescription()
}
QGraphicsTextItem * text = new QGraphicsTextItem(_placeHolder);
text->setDefaultTextColor(this->pen().color().rgb());
// Make octave compatible with SIFT packed octave (https://github.com/opencv/opencv/issues/4554)
int octave = _kpt.octave & 255;
octave = octave < 128 ? octave : (-128 | octave);
float scale = octave >= 0 ? 1.f/(1 << octave) : (float)(1 << -octave);
if(_depth <= 0)
{
text->setPlainText(QString( "Id = %1\n"
@@ -82,7 +86,8 @@ void KeypointItem::showDescription()
"X = %5\n"
"Y = %6\n"
"Size = %7\n"
"Octave = %8").arg(_id).arg(_kpt.angle).arg(_kpt.response).arg(_kpt.pt.x).arg(_kpt.pt.y).arg(_kpt.size).arg(_kpt.octave));
"Octave = %8\n"
"Scale = %9").arg(_id).arg(_kpt.angle).arg(_kpt.response).arg(_kpt.pt.x).arg(_kpt.pt.y).arg(_kpt.size).arg(octave).arg(scale));
}
else
{
@@ -93,7 +98,8 @@ void KeypointItem::showDescription()
"Y = %6\n"
"Size = %7\n"
"Octave = %8\n"
"Depth = %9 m").arg(_id).arg(_kpt.angle).arg(_kpt.response).arg(_kpt.pt.x).arg(_kpt.pt.y).arg(_kpt.size).arg(_kpt.octave).arg(_depth));
"Scale = %9\n"
"Depth = %10 m").arg(_id).arg(_kpt.angle).arg(_kpt.response).arg(_kpt.pt.x).arg(_kpt.pt.y).arg(_kpt.size).arg(octave).arg(scale).arg(_depth));
}
_placeHolder->setRect(text->boundingRect());
}
guilib/src/MainWindow.cpp
+4 -160
View File
@@ -7038,166 +7038,10 @@ void MainWindow::exportBundlerFormat()
if(poses.size())
{
if(_exportBundlerDialog->exec() != QDialog::Accepted)
{
return;
}
QString path = _exportBundlerDialog->outputPath();
if(!path.isEmpty())
{
if(!QDir(path).mkpath("."))
{
QMessageBox::warning(this, tr("Exporting cameras..."), tr("Failed creating directory %1.").arg(path));
return;
}
// export cameras and images
QFile fileOut(path+QDir::separator()+"cameras.out");
QFile fileList(path+QDir::separator()+"list.txt");
QDir(path).mkdir("images");
if(fileOut.open(QIODevice::WriteOnly | QIODevice::Text))
{
if(fileList.open(QIODevice::WriteOnly | QIODevice::Text))
{
std::set<int> ignoredCameras;
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
QString p = QString("images")+QDir::separator()+tr("%1.jpg").arg(iter->first);
p = path+QDir::separator()+p;
cv::Mat image = _cachedSignatures[iter->first].sensorData().imageRaw();
if(image.empty())
{
_cachedSignatures[iter->first].sensorData().uncompressDataConst(&image, 0, 0, 0);
}
double maxLinearVel = _exportBundlerDialog->maxLinearSpeed();
double maxAngularVel = _exportBundlerDialog->maxAngularSpeed();
double laplacianThr = _exportBundlerDialog->laplacianThreshold();
bool blurryImage = false;
const std::vector<float> & velocity = _cachedSignatures[iter->first].getVelocity();
if(maxLinearVel>0.0 || maxAngularVel>0.0)
{
if(velocity.size() == 6)
{
float transVel = uMax3(fabs(velocity[0]), fabs(velocity[1]), fabs(velocity[2]));
float rotVel = uMax3(fabs(velocity[3]), fabs(velocity[4]), fabs(velocity[5]));
if(maxLinearVel>0.0 && transVel > maxLinearVel)
{
UWARN("Fast motion detected for camera %d (speed=%f m/s > thr=%f m/s), camera is ignored for texturing.", iter->first, transVel, maxLinearVel);
blurryImage = true;
}
else if(maxAngularVel>0.0 && rotVel > maxAngularVel)
{
UWARN("Fast motion detected for camera %d (speed=%f rad/s > thr=%f rad/s), camera is ignored for texturing.", iter->first, rotVel, maxAngularVel);
blurryImage = true;
}
}
else
{
UWARN("Camera motion filtering is set, but velocity of camera %d is not available.", iter->first);
}
}
if(!blurryImage && !image.empty() && laplacianThr>0.0)
{
cv::Mat imgLaplacian;
cv::Laplacian(image, imgLaplacian, CV_16S);
cv::Mat m, s;
cv::meanStdDev(imgLaplacian, m, s);
double stddev_pxl = s.at<double>(0);
double var = stddev_pxl*stddev_pxl;
if(var < laplacianThr)
{
blurryImage = true;
UWARN("Camera's image %d is detected as blurry (var=%f < thr=%f), camera is ignored for texturing.", iter->first, var, laplacianThr);
}
}
if(blurryImage)
{
ignoredCameras.insert(iter->first);
}
else
{
if(cv::imwrite(p.toStdString(), image))
{
UINFO("saved image %s", p.toStdString().c_str());
}
else
{
UERROR("Failed to save image %s", p.toStdString().c_str());
}
}
}
QTextStream out(&fileOut);
QTextStream list(&fileList);
out << "# Bundle file v0.3\n";
out << poses.size()-ignoredCameras.size() << " 0\n";
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
if(ignoredCameras.find(iter->first) == ignoredCameras.end())
{
QString p = QString("images")+QDir::separator()+tr("%1.jpg").arg(iter->first);
list << p << "\n";
Transform localTransform;
if(_cachedSignatures[iter->first].sensorData().cameraModels().size())
{
out << _cachedSignatures[iter->first].sensorData().cameraModels().at(0).fx() << " 0 0\n";
localTransform = _cachedSignatures[iter->first].sensorData().cameraModels().at(0).localTransform();
}
else
{
out << _cachedSignatures[iter->first].sensorData().stereoCameraModel().left().fx() << " 0 0\n";
localTransform = _cachedSignatures[iter->first].sensorData().stereoCameraModel().left().localTransform();
}
static const Transform opengl_world_T_rtabmap_world(
0.0f, -1.0f, 0.0f, 0.0f,
0.0f, 0.0f, 1.0f, 0.0f,
-1.0f, 0.0f, 0.0f, 0.0f);
static const Transform optical_rotation_inv(
0.0f, -1.0f, 0.0f, 0.0f,
0.0f, 0.0f, -1.0f, 0.0f,
1.0f, 0.0f, 0.0f, 0.0f);
Transform pose = iter->second;
if(!localTransform.isNull())
{
pose*=localTransform*optical_rotation_inv;
}
Transform poseGL = opengl_world_T_rtabmap_world*pose.inverse();
out << poseGL.r11() << " " << poseGL.r12() << " " << poseGL.r13() << "\n";
out << poseGL.r21() << " " << poseGL.r22() << " " << poseGL.r23() << "\n";
out << poseGL.r31() << " " << poseGL.r32() << " " << poseGL.r33() << "\n";
out << poseGL.x() << " " << poseGL.y() << " " << poseGL.z() << "\n";
}
}
fileList.close();
fileOut.close();
QMessageBox::information(this,
tr("Exporting cameras in Bundler format..."),
tr("%1 cameras/images exported to directory \"%2\".%3")
.arg(poses.size())
.arg(path)
.arg(ignoredCameras.size()>0?tr(" %1/%2 cameras ignored for too fast motion and/or blur level.").arg(ignoredCameras.size()).arg(poses.size()):""));
}
else
{
fileOut.close();
QMessageBox::warning(this, tr("Exporting cameras..."), tr("Failed opening file %1 for writing.").arg(path+QDir::separator()+"list.txt"));
}
}
else
{
QMessageBox::warning(this, tr("Exporting cameras..."), tr("Failed opening file %1 for writing.").arg(path+QDir::separator()+"cameras.out"));
}
}
_exportBundlerDialog->exportBundler(
poses,
_currentLinksMap,
_cachedSignatures);
}
else
{
guilib/src/ui/exportBundlerDialog.ui
+22 -2
View File
@@ -6,8 +6,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>521</width>
<height>320</height>
<width>524</width>
<height>363</height>
</rect>
</property>
<property name="windowTitle">
@@ -119,6 +119,26 @@
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_43">
<property name="text">
<string>Export 3D points. RTAB-Map must be built with g2o.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QCheckBox" name="checkBox_export_points">
<property name="text">
<string/>
</property>
</widget>
</item>
</layout>
</item>
</layout>