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 { 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 // Graph optimizers
//////////////////////////////////////////// ////////////////////////////////////////////
@@ -118,9 +131,18 @@ public:
const std::multimap<int, Link> & links, const std::multimap<int, Link> & links,
const std::map<int, CameraModel> & models, // in case of stereo, Tx should be set const std::map<int, CameraModel> & models, // in case of stereo, Tx should be set
std::map<int, cv::Point3f> & points3DMap, 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::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( std::map<int, Transform> optimizeBA(
int rootId, int rootId,
const std::map<int, Transform> & poses, const std::map<int, Transform> & poses,
@@ -131,7 +153,7 @@ public:
const Link & link, const Link & link,
const CameraModel & model, const CameraModel & model,
std::map<int, cv::Point3f> & points3DMap, 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); std::set<int> * outliers = 0);
void computeBACorrespondences( void computeBACorrespondences(
@@ -139,7 +161,7 @@ public:
const std::multimap<int, Link> & links, 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, 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: protected:
Optimizer( 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_ #define ODOMETRYF2M_H_
#include <rtabmap/core/Odometry.h> #include <rtabmap/core/Odometry.h>
#include <rtabmap/core/Optimizer.h>
#include <pcl/point_cloud.h> #include <pcl/point_cloud.h>
#include <pcl/point_types.h> #include <pcl/point_types.h>
#include <pcl/pcl_base.h> #include <pcl/pcl_base.h>
@@ -74,7 +75,7 @@ private:
int lastFrameOldestNewId_; int lastFrameOldestNewId_;
std::vector<std::pair<pcl::PointCloud<pcl::PointNormal>::Ptr, pcl::IndicesPtr> > scansBuffer_; 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::map<int, Transform> bundlePoses_;
std::multimap<int, Link> bundleLinks_; std::multimap<int, Link> bundleLinks_;
std::map<int, CameraModel> bundleModels_; 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::multimap<int, Link> & links,
const std::map<int, CameraModel> & models, const std::map<int, CameraModel> & models,
std::map<int, cv::Point3f> & points3DMap, 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); 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::multimap<int, Link> & links,
const std::map<int, CameraModel> & models, // in case of stereo, Tx should be set const std::map<int, CameraModel> & models, // in case of stereo, Tx should be set
std::map<int, cv::Point3f> & points3DMap, 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); std::set<int> * outliers = 0);
bool saveGraph( bool saveGraph(

6
corelib/src/Memory.cpp
View File

@@ -2599,7 +2599,7 @@ Transform Memory::computeTransform(
std::map<int, Transform> bundlePoses; std::map<int, Transform> bundlePoses;
std::multimap<int, Link> bundleLinks; std::multimap<int, Link> bundleLinks;
std::map<int, CameraModel> bundleModels; 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(); 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()))); 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); std::multimap<int, cv::Point3f>::const_iterator kter = s->getWords3().find(jter->first);
cv::Point3f pt3d = util3d::transformPoint(kter->second, invLocalTransform); cv::Point3f pt3d = util3d::transformPoint(kter->second, invLocalTransform);
wordReferences.insert(std::make_pair(jter->first, std::map<int, cv::Point3f>())); wordReferences.insert(std::make_pair(jter->first, std::map<int, FeatureBA>()));
wordReferences.at(jter->first).insert(std::make_pair(id, cv::Point3f(jter->second.pt.x, jter->second.pt.y, pt3d.z))); 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::multimap<int, Link> & links,
const std::map<int, CameraModel> & models, const std::map<int, CameraModel> & models,
std::map<int, cv::Point3f> & points3DMap, 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::set<int> * outliers)
{ {
UERROR("Optimizer %d doesn't implement optimizeBA() method.", (int)this->type()); UERROR("Optimizer %d doesn't implement optimizeBA() method.", (int)this->type());
@@ -370,7 +370,9 @@ std::map<int, Transform> Optimizer::optimizeBA(
int rootId, int rootId,
const std::map<int, Transform> & poses, const std::map<int, Transform> & poses,
const std::multimap<int, Link> & links, 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(""); UDEBUG("");
std::map<int, CameraModel> models; std::map<int, CameraModel> models;
@@ -415,18 +417,27 @@ std::map<int, Transform> Optimizer::optimizeBA(
} }
// compute correspondences // compute correspondences
std::map<int, cv::Point3f> points3DMap;
std::map<int, std::map<int, cv::Point3f> > wordReferences;
this->computeBACorrespondences(poses, links, signatures, points3DMap, wordReferences); this->computeBACorrespondences(poses, links, signatures, points3DMap, wordReferences);
return optimizeBA(rootId, poses, links, models, 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( Transform Optimizer::optimizeBA(
const Link & link, const Link & link,
const CameraModel & model, const CameraModel & model,
std::map<int, cv::Point3f> & points3DMap, 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::set<int> * outliers)
{ {
std::map<int, Transform> poses; std::map<int, Transform> poses;
@@ -453,7 +464,7 @@ void Optimizer::computeBACorrespondences(
const std::multimap<int, Link> & links, 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, 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(""); UDEBUG("");
int wordCount = 0; int wordCount = 0;
@@ -465,7 +476,8 @@ void Optimizer::computeBACorrespondences(
{ {
link = link.inverse(); link = link.inverse();
} }
if(uContains(signatures, link.from()) && if(link.to() != link.from() &&
uContains(signatures, link.from()) &&
uContains(signatures, link.to()) && uContains(signatures, link.to()) &&
uContains(poses, link.from())) uContains(poses, link.from()))
{ {
@@ -513,13 +525,14 @@ void Optimizer::computeBACorrespondences(
{ {
int wordId = ++wordCount; 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; cv::KeyPoint ptFrom = sFrom.getWords().lower_bound(info.inliersIDs[i])->second;
wordReferences.at(wordId).insert(std::make_pair(sFrom.id(), cv::Point3f(pt.x, pt.y, p.x))); 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)));
cv::KeyPoint ptTo = sTo.getWords().lower_bound(info.inliersIDs[i])->second;
pt = sTo.getWords().lower_bound(info.inliersIDs[i])->second.pt; cv::Mat descriptorTo = sTo.getWordsDescriptors().lower_bound(info.inliersIDs[i])->second;
float depth = 0.0f; float depth = 0.0f;
std::multimap<int, cv::Point3f>::const_iterator iterTo = sTo.getWords3().lower_bound(info.inliersIDs[i]); std::multimap<int, cv::Point3f>::const_iterator iterTo = sTo.getWords3().lower_bound(info.inliersIDs[i]);
if( iterTo!=sTo.getWords3().end() && if( iterTo!=sTo.getWords3().end() &&
@@ -527,7 +540,7 @@ void Optimizer::computeBACorrespondences(
{ {
depth = iterTo->second.x; 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); p = util3d::transformPoint(p, pose);
points3DMap.insert(std::make_pair(wordId, p)); 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])) 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; int matchedIndex = -1;
if(indices[i].size() >= 2) if(indices[i].size() >= 2)
{ {
@@ -837,7 +839,9 @@ Transform RegistrationVis::computeTransformationImpl(
} }
for(unsigned int j=0; j<indices[i].size(); ++j) 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)); descriptorsFrom.row(projectedIndexToDescIndex[indices[i].at(j)]).copyTo(descriptors.row(oi));
descriptorsIndices[oi++] = indices[i].at(j); descriptorsIndices[oi++] = indices[i].at(j);
@@ -861,10 +865,14 @@ Transform RegistrationVis::computeTransformationImpl(
matchedIndex = descriptorsIndices[0]; matchedIndex = descriptorsIndices[0];
} }
} }
else if(indices[i].size() == 1 && else if(indices[i].size() == 1)
kptsFrom.at(projectedIndexToDescIndex[indices[i].at(0)]).octave == octave)
{ {
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) if(matchedIndex >= 0)
@@ -972,6 +980,10 @@ Transform RegistrationVis::computeTransformationImpl(
if(util3d::isFinite(kptsFrom3D[matchedIndexFrom])) 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; int matchedIndexTo = -1;
if(indices[i].size() >= 2) if(indices[i].size() >= 2)
{ {
@@ -985,8 +997,9 @@ Transform RegistrationVis::computeTransformationImpl(
std::list<int> indicesToIgnoretmp; std::list<int> indicesToIgnoretmp;
for(unsigned int j=0; j<indices[i].size(); ++j) for(unsigned int j=0; j<indices[i].size(); ++j)
{ {
int octave = kptsTo[indices[i].at(j)].octave; int octave = kptsTo[indices[i].at(j)].octave & 255;
if(kptsFrom.at(matchedIndexFrom).octave==octave) octave = octave < 128 ? octave : (-128 | octave);
if(octaveFrom==octave)
{ {
descriptorsTo.row(indices[i].at(j)).copyTo(descriptors.row(oi)); descriptorsTo.row(indices[i].at(j)).copyTo(descriptors.row(oi));
descriptorsIndices[oi++] = indices[i].at(j); descriptorsIndices[oi++] = indices[i].at(j);
@@ -1017,8 +1030,9 @@ Transform RegistrationVis::computeTransformationImpl(
} }
else if(indices[i].size() == 1) else if(indices[i].size() == 1)
{ {
int octave = kptsTo[indices[i].at(0)].octave; int octave = kptsTo[indices[i].at(0)].octave & 255;
if(kptsFrom.at(matchedIndexFrom).octave == octave) octave = octave < 128 ? octave : (-128 | octave);
if(octaveFrom == octave)
{ {
matchedIndexTo = indices[i].at(0); 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(1, cameraModelFrom.isValidForProjection()?cameraModelFrom:cameraModelTo));
models.insert(std::make_pair(2, 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) for(unsigned int i=0; i<allInliers.size(); ++i)
{ {
int wordId = allInliers[i]; int wordId = allInliers[i];
const cv::Point3f & pt3D = fromSignature.getWords3().find(wordId)->second; const cv::Point3f & pt3D = fromSignature.getWords3().find(wordId)->second;
points3DMap.insert(std::make_pair(wordId, pt3D)); points3DMap.insert(std::make_pair(wordId, pt3D));
std::map<int, cv::Point3f> ptMap; std::map<int, FeatureBA> ptMap;
if(fromSignature.getWords().size() && cameraModelFrom.isValidForProjection()) if(fromSignature.getWords().size() && cameraModelFrom.isValidForProjection())
{ {
float depthFrom = util3d::transformPoint(pt3D, invLocalTransformFrom).z; float depthFrom = util3d::transformPoint(pt3D, invLocalTransformFrom).z;
const cv::Point2f & kpt = fromSignature.getWords().find(wordId)->second.pt; const cv::KeyPoint & kpt = fromSignature.getWords().find(wordId)->second;
ptMap.insert(std::make_pair(1,cv::Point3f(kpt.x, kpt.y, depthFrom))); ptMap.insert(std::make_pair(1,FeatureBA(kpt, depthFrom)));
} }
if(toSignature.getWords().size() && cameraModelTo.isValidForProjection()) if(toSignature.getWords().size() && cameraModelTo.isValidForProjection())
{ {
float depthTo = util3d::transformPoint(toSignature.getWords3().find(wordId)->second, invLocalTransformTo).z; 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()); 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)); 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(); Transform invLocalTransform = model.localTransform().inverse();
UDEBUG("Fill matches (%d)", (int)regInfo.inliersIDs.size()); 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) for(unsigned int i=0; i<regInfo.inliersIDs.size(); ++i)
{ {
int wordId =regInfo.inliersIDs[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); std::multimap<int, cv::KeyPoint>::const_iterator iter2D = lastFrame_->getWords().find(wordId);
// all other references // 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()); 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; int step = bundleMaxFrames_>0?(refIter->second.size() / bundleMaxFrames_):1;
if(step == 0) if(step == 0)
{ {
step = 1; step = 1;
} }
int oi=0; 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()) 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()); UASSERT(lastFrame_->getWords3().find(wordId) != lastFrame_->getWords3().end());
//move back point in camera frame (to get depth along z) //move back point in camera frame (to get depth along z)
cv::Point3f pt3d = util3d::transformPoint(lastFrame_->getWords3().find(wordId)->second, invLocalTransform); 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)); wordReferences.insert(std::make_pair(wordId, references));
@@ -589,13 +589,13 @@ Transform OdometryF2M::computeTransform(
cv::Point3f pt3d = util3d::transformPoint(iter->second, invLocalTransform); cv::Point3f pt3d = util3d::transformPoint(iter->second, invLocalTransform);
if(bundleWordReferences_.find(iter->first) == bundleWordReferences_.end()) if(bundleWordReferences_.find(iter->first) == bundleWordReferences_.end())
{ {
std::map<int, cv::Point3f> framePt; std::map<int, FeatureBA> framePt;
framePt.insert(std::make_pair(lastFrame_->id(), cv::Point3f(iter2D->second.pt.x, iter2D->second.pt.y, pt3d.z))); framePt.insert(std::make_pair(lastFrame_->id(), FeatureBA(iter2D->second, pt3d.z)));
bundleWordReferences_.insert(std::make_pair(iter->first, framePt)); bundleWordReferences_.insert(std::make_pair(iter->first, framePt));
} }
else 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); cv::Point3f pt3d = util3d::transformPoint(iter->second.second.second.first, invLocalTransform);
if(bundleWordReferences_.find(iter->second.first) == bundleWordReferences_.end()) if(bundleWordReferences_.find(iter->second.first) == bundleWordReferences_.end())
{ {
std::map<int, cv::Point3f> framePt; std::map<int, FeatureBA> framePt;
framePt.insert(std::make_pair(lastFrame_->id(), cv::Point3f(iter->second.second.first.pt.x, iter->second.second.first.pt.y, pt3d.z))); framePt.insert(std::make_pair(lastFrame_->id(), FeatureBA(iter->second.second.first, pt3d.z)));
bundleWordReferences_.insert(std::make_pair(iter->second.first, framePt)); bundleWordReferences_.insert(std::make_pair(iter->second.first, framePt));
} }
else 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); int id = ids.at(i);
if(inliers.find(id) == inliers.end()) 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()) 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()) if(bundlePoseReferences_.find(iterFrame->first) != bundlePoseReferences_.end())
{ {
@@ -697,10 +697,10 @@ Transform OdometryF2M::computeTransform(
{ {
if(inliers.find(iter->first) == inliers.end()) 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()) 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()) if(bundlePoseReferences_.find(iterFrame->first) != bundlePoseReferences_.end())
{ {
@@ -983,7 +983,7 @@ Transform OdometryF2M::computeTransform(
if(words.count(iter->first) == 1) if(words.count(iter->first) == 1)
{ {
UASSERT(bundleWordReferences_.find(iter->first) == bundleWordReferences_.end()); UASSERT(bundleWordReferences_.find(iter->first) == bundleWordReferences_.end());
std::map<int, cv::Point3f> framePt; std::map<int, FeatureBA> framePt;
//get depth //get depth
float d = 0.0f; 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)); 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::multimap<int, Link> & links,
const std::map<int, CameraModel> & models, const std::map<int, CameraModel> & models,
std::map<int, cv::Point3f> & points3DMap, 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) std::set<int> * outliers)
{ {
#ifdef RTABMAP_CVSBA #ifdef RTABMAP_CVSBA
@@ -131,14 +131,14 @@ std::map<int, Transform> OptimizerCVSBA::optimizeBA(
{ {
points[i] = kter->second; 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()) 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()) 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; 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::multimap<int, Link> & links,
const std::map<int, CameraModel> & models, const std::map<int, CameraModel> & models,
std::map<int, cv::Point3f> & points3DMap, 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::set<int> * outliers)
{ {
std::map<int, Transform> optimizedPoses; std::map<int, Transform> optimizedPoses;
@@ -1070,7 +1070,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
} }
UDEBUG("stepVertexId=%d, negVertexOffset=%d", stepVertexId, negVertexOffset); UDEBUG("stepVertexId=%d, negVertexOffset=%d", stepVertexId, negVertexOffset);
std::list<g2o::OptimizableGraph::Edge*> edges; 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; int id = iter->first;
if(points3DMap.find(id) != points3DMap.end()) 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); //UDEBUG("Added 3D point %d (%f,%f,%f)", vpt3d->id()-stepVertexId, pt3d.x, pt3d.y, pt3d.z);
// set observations // 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; int camId = jter->first;
if(poses.find(camId) != poses.end() && optimizer.vertex(camId) != 0) if(poses.find(camId) != poses.end() && optimizer.vertex(camId) != 0)
{ {
const cv::Point3f & pt = jter->second; const FeatureBA & pt = jter->second;
double depth = pt.z; 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); //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 #ifdef RTABMAP_ORB_SLAM2
g2o::EdgeStereoSE3ProjectXYZ* es = new g2o::EdgeStereoSE3ProjectXYZ(); g2o::EdgeStereoSE3ProjectXYZ* es = new g2o::EdgeStereoSE3ProjectXYZ();
float disparity = baseline * iterModel->second.fx() / depth; 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); es->setMeasurement(obs);
//variance *= log(exp(1)+disparity); //variance *= log(exp(1)+disparity);
es->setInformation(Eigen::Matrix3d::Identity() / variance); es->setInformation(Eigen::Matrix3d::Identity() / variance);
@@ -1136,7 +1136,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
#else #else
g2o::EdgeProjectP2SC* es = new g2o::EdgeProjectP2SC(); g2o::EdgeProjectP2SC* es = new g2o::EdgeProjectP2SC();
float disparity = baseline * vcam->estimate().Kcam(0,0) / depth; 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); es->setMeasurement(obs);
//variance *= log(exp(1)+disparity); //variance *= log(exp(1)+disparity);
es->setInformation(Eigen::Matrix3d::Identity() / variance); es->setInformation(Eigen::Matrix3d::Identity() / variance);
@@ -1155,7 +1155,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
// mono edge // mono edge
#ifdef RTABMAP_ORB_SLAM2 #ifdef RTABMAP_ORB_SLAM2
g2o::EdgeSE3ProjectXYZ* em = new g2o::EdgeSE3ProjectXYZ(); 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->setMeasurement(obs);
em->setInformation(Eigen::Matrix2d::Identity() / variance); em->setInformation(Eigen::Matrix2d::Identity() / variance);
em->fx = iterModel->second.fx(); em->fx = iterModel->second.fx();
@@ -1166,7 +1166,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
#else #else
g2o::EdgeProjectP2MC* em = new g2o::EdgeProjectP2MC(); 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->setMeasurement(obs);
em->setInformation(Eigen::Matrix2d::Identity() / variance); em->setInformation(Eigen::Matrix2d::Identity() / variance);
e = em; e = em;

10
guilib/include/rtabmap/gui/ExportBundlerDialog.h
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/gui/RtabmapGuiExp.h" // DLL export/import defines
#include <rtabmap/core/Signature.h>
#include <QDialog> #include <QDialog>
#include <QSettings> #include <QSettings>
@@ -51,11 +52,10 @@ public:
void setWorkingDirectory(const QString & path); void setWorkingDirectory(const QString & path);
QString outputPath() const; void exportBundler(
const std::map<int, Transform> & poses,
double maxLinearSpeed() const; const std::multimap<int, Link> & links,
double maxAngularSpeed() const; const QMap<int, Signature> & signatures);
double laplacianThreshold() const;
Q_SIGNALS: Q_SIGNALS:
void configChanged(); void configChanged();

378
guilib/src/ExportBundlerDialog.cpp
View File

@@ -27,9 +27,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/gui/ExportBundlerDialog.h" #include "rtabmap/gui/ExportBundlerDialog.h"
#include "ui_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 <QFileDialog>
#include <QPushButton> #include <QPushButton>
#include <QMessageBox>
#include <QTextStream>
namespace rtabmap { namespace rtabmap {
@@ -47,6 +51,9 @@ ExportBundlerDialog::ExportBundlerDialog(QWidget * parent) :
connect(_ui->doubleSpinBox_laplacianVariance, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); connect(_ui->doubleSpinBox_laplacianVariance, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
connect(_ui->doubleSpinBox_linearSpeed, 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->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()); _ui->lineEdit_path->setText(QDir::currentPath());
} }
@@ -62,9 +69,10 @@ void ExportBundlerDialog::saveSettings(QSettings & settings, const QString & gro
{ {
settings.beginGroup(group); settings.beginGroup(group);
} }
settings.setValue("maxLinearSpeed", this->maxLinearSpeed()); settings.setValue("maxLinearSpeed", _ui->doubleSpinBox_linearSpeed->value());
settings.setValue("maxAngularSpeed", this->maxAngularSpeed()); settings.setValue("maxAngularSpeed", _ui->doubleSpinBox_angularSpeed->value());
settings.setValue("laplacianThr", this->laplacianThreshold()); settings.setValue("laplacianThr", _ui->doubleSpinBox_laplacianVariance->value());
settings.setValue("exportPoints", _ui->checkBox_export_points->isChecked());
if(!group.isEmpty()) if(!group.isEmpty())
{ {
settings.endGroup(); settings.endGroup();
@@ -77,9 +85,10 @@ void ExportBundlerDialog::loadSettings(QSettings & settings, const QString & gro
{ {
settings.beginGroup(group); settings.beginGroup(group);
} }
_ui->doubleSpinBox_linearSpeed->setValue(settings.value("maxLinearSpeed", this->maxLinearSpeed()).toDouble()); _ui->doubleSpinBox_linearSpeed->setValue(settings.value("maxLinearSpeed", _ui->doubleSpinBox_linearSpeed->value()).toDouble());
_ui->doubleSpinBox_angularSpeed->setValue(settings.value("maxAngularSpeed", this->maxAngularSpeed()).toDouble()); _ui->doubleSpinBox_angularSpeed->setValue(settings.value("maxAngularSpeed", _ui->doubleSpinBox_angularSpeed->value()).toDouble());
_ui->doubleSpinBox_laplacianVariance->setValue(settings.value("laplacianThr", this->laplacianThreshold()).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()) if(!group.isEmpty())
{ {
settings.endGroup(); settings.endGroup();
@@ -96,6 +105,7 @@ void ExportBundlerDialog::restoreDefaults()
_ui->doubleSpinBox_linearSpeed->setValue(0); _ui->doubleSpinBox_linearSpeed->setValue(0);
_ui->doubleSpinBox_angularSpeed->setValue(0); _ui->doubleSpinBox_angularSpeed->setValue(0);
_ui->doubleSpinBox_laplacianVariance->setValue(0); _ui->doubleSpinBox_laplacianVariance->setValue(0);
_ui->checkBox_export_points->setChecked(false);
} }
void ExportBundlerDialog::getPath() 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 std::map<int, cv::Point3f> points3DMap;
{ std::map<int, std::map<int, FeatureBA> > wordReferences;
return _ui->doubleSpinBox_linearSpeed->value(); std::map<int, Transform> newPoses = poses;
} if(_ui->checkBox_export_points->isEnabled() && _ui->checkBox_export_points->isChecked())
double ExportBundlerDialog::maxAngularSpeed() const {
{ std::map<int, Transform> posesOut;
return _ui->doubleSpinBox_angularSpeed->value(); std::multimap<int, Link> linksOut;
} Optimizer * sba = Optimizer::create(Optimizer::kTypeG2O);
double ExportBundlerDialog::laplacianThreshold() const sba->getConnectedGraph(poses.begin()->first, poses, links, posesOut, linksOut);
{ newPoses = sba->optimizeBA(
return _ui->doubleSpinBox_laplacianVariance->value(); 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"));
}
}
} }
} }

10
guilib/src/KeypointItem.cpp
View File

@@ -74,6 +74,10 @@ void KeypointItem::showDescription()
} }
QGraphicsTextItem * text = new QGraphicsTextItem(_placeHolder); QGraphicsTextItem * text = new QGraphicsTextItem(_placeHolder);
text->setDefaultTextColor(this->pen().color().rgb()); 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) if(_depth <= 0)
{ {
text->setPlainText(QString( "Id = %1\n" text->setPlainText(QString( "Id = %1\n"
@@ -82,7 +86,8 @@ void KeypointItem::showDescription()
"X = %5\n" "X = %5\n"
"Y = %6\n" "Y = %6\n"
"Size = %7\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 else
{ {
@@ -93,7 +98,8 @@ void KeypointItem::showDescription()
"Y = %6\n" "Y = %6\n"
"Size = %7\n" "Size = %7\n"
"Octave = %8\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()); _placeHolder->setRect(text->boundingRect());
} }

164
guilib/src/MainWindow.cpp
View File

@@ -7038,166 +7038,10 @@ void MainWindow::exportBundlerFormat()
if(poses.size()) if(poses.size())
{ {
if(_exportBundlerDialog->exec() != QDialog::Accepted) _exportBundlerDialog->exportBundler(
{ poses,
return; _currentLinksMap,
} _cachedSignatures);
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"));
}
}
} }
else else
{ {

24
guilib/src/ui/exportBundlerDialog.ui
View File

@@ -6,8 +6,8 @@
<rect> <rect>
<x>0</x> <x>0</x>
<y>0</y> <y>0</y>
<width>521</width> <width>524</width>
<height>320</height> <height>363</height>
</rect> </rect>
</property> </property>
<property name="windowTitle"> <property name="windowTitle">
@@ -119,6 +119,26 @@
</property> </property>
</widget> </widget>
</item> </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> </layout>
</item> </item>
</layout> </layout>