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

Binary descriptors (ORB, BRIEF, FREAK) can now be used for the visual dictionnary: maybe not as discriminative as SIFT/SURF on large environments, the advantage is that RTAB-Map will work without Patent/noncommercial licenses of SIFT and SURF.

Browse Source 
A new option is added to re-extract features when a loop closure hypothesis is found. 
Another new option is to force 2D (3DoF) transform on visual odometry and loop closures.

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1679 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-09-21 16:51:51 +00:00
parent 02850ec756
commit a98f819821
10 changed files with 463 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files

99
corelib/src/Memory.cpp
View File

@@ -84,6 +84,7 @@ Memory::Memory(const ParametersMap & parameters) :
_bowInlierDistance(Parameters::defaultLccBowInlierDistance()),
_bowIterations(Parameters::defaultLccBowIterations()),
_bowMaxDepth(Parameters::defaultLccBowMaxDepth()),
_bowForce2D(Parameters::defaultLccBowForce2D()),
_icpDecimation(Parameters::defaultLccIcp3Decimation()),
_icpMaxDepth(Parameters::defaultLccIcp3MaxDepth()),
@@ -324,6 +325,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kLccBowInlierDistance(), _bowInlierDistance);
Parameters::parse(parameters, Parameters::kLccBowIterations(), _bowIterations);
Parameters::parse(parameters, Parameters::kLccBowMaxDepth(), _bowMaxDepth);
Parameters::parse(parameters, Parameters::kLccBowForce2D(), _bowForce2D);
Parameters::parse(parameters, Parameters::kLccIcp3Decimation(), _icpDecimation);
Parameters::parse(parameters, Parameters::kLccIcp3MaxDepth(), _icpMaxDepth);
Parameters::parse(parameters, Parameters::kLccIcp3VoxelSize(), _icpVoxelSize);
@@ -1636,7 +1638,7 @@ void Memory::rejectLoopClosure(int oldId, int newId)
}
// compute transform newId -> oldId
Transform Memory::computeVisualTransform(int oldId, int newId, std::string * rejectedMsg) const
Transform Memory::computeVisualTransform(int oldId, int newId, std::string * rejectedMsg, int * inliers) const
{
const Signature * oldS = this->getSignature(oldId);
const Signature * newS = this->getSignature(newId);
@@ -1645,7 +1647,7 @@ Transform Memory::computeVisualTransform(int oldId, int newId, std::string * rej
if(oldS && newId)
{
return computeVisualTransform(*oldS, *newS, rejectedMsg);
return computeVisualTransform(*oldS, *newS, rejectedMsg, inliers);
}
else
{
@@ -1660,7 +1662,7 @@ Transform Memory::computeVisualTransform(int oldId, int newId, std::string * rej
}
// compute transform newId -> oldId
Transform Memory::computeVisualTransform(const Signature & oldS, const Signature & newS, std::string * rejectedMsg) const
Transform Memory::computeVisualTransform(const Signature & oldS, const Signature & newS, std::string * rejectedMsg, int * inliers) const
{
Transform transform;
std::string msg;
@@ -1690,6 +1692,13 @@ Transform Memory::computeVisualTransform(const Signature & oldS, const Signature
if(!t.isNull() && inliersCount >= _bowMinInliers)
{
transform = t;
if(_bowForce2D)
{
UDEBUG("Forcing 2D...");
float x,y,z,r,p,yaw;
transform.getTranslationAndEulerAngles(x,y,z, r,p,yaw);
transform = util3d::transformFromEigen3f(pcl::getTransformation(x,y,0, 0, 0, yaw));
}
}
else if(inliersCount < _bowMinInliers)
{
@@ -1701,6 +1710,11 @@ Transform Memory::computeVisualTransform(const Signature & oldS, const Signature
msg = uFormat("Rejected identity with full inliers.");
UINFO(msg.c_str());
}
if(inliers)
{
*inliers = inliersCount;
}
}
else
{
@@ -2489,9 +2503,9 @@ void Memory::getImageDepth(
float & fy,
float & cx,
float & cy,
Transform & localTransform) const
Transform & localTransform)
{
const Signature * s = this->getSignature(locationId);
Signature * s = this->_getSignature(locationId);
if(s)
{
rgb = s->getImage();
@@ -2506,6 +2520,79 @@ void Memory::getImageDepth(
if(rgb.empty() && this->isRawDataKept() && _dbDriver)
{
_dbDriver->getNodeData(locationId, rgb, depth, depth2d, fx, fy, cx, cy, localTransform);
if(s)
{
// keep in cache
if(!rgb.empty())
{
s->setImage(rgb);
}
if(!depth.empty())
{
s->setDepth(depth, fx, fy, cx, cy);
}
if(!depth2d.empty())
{
s->setDepth2D(depth2d);
}
if(!localTransform.isNull())
{
s->setLocalTransform(localTransform);
}
}
}
}
void Memory::getImageDepthRaw(
int locationId,
cv::Mat & rgb,
cv::Mat & depth,
float & fx,
float & fy,
float & cx,
float & cy,
Transform & localTransform)
{
Signature * s = this->_getSignature(locationId);
if(s)
{
rgb = s->getImageRaw();
depth = s->getDepthRaw();
fx = s->getDepthFx();
fy = s->getDepthFy();
cx = s->getDepthCx();
cy = s->getDepthCy();
localTransform = s->getLocalTransform();
}
if(rgb.empty())
{
std::vector<unsigned char> compressedRgb;
std::vector<unsigned char> compressedDepth;
std::vector<unsigned char> comressedDepth2d;
getImageDepth(locationId, compressedRgb, compressedDepth, comressedDepth2d, fx, fy, cx, cy, localTransform);
//uncomressed data
util3d::CompressionThread ctImage(compressedRgb, true);
util3d::CompressionThread ctDepth(compressedDepth, true);
ctImage.start();
ctDepth.start();
ctImage.join();
ctDepth.join();
rgb = ctImage.getUncompressedData();
depth = ctDepth.getUncompressedData();
if(s)
{
//save it uncompressed in the signature
if(!rgb.empty())
{
s->setImageRaw(rgb);
}
if(!depth.empty())
{
s->setDepthRaw(depth);
}
}
}
}
@@ -3110,6 +3197,8 @@ Signature * Memory::createSignature(const SensorData & data, bool keepRawData)
data.depthCx(),
data.depthCy(),
data.localTransform());
s->setImageRaw(data.image());
s->setDepthRaw(data.depth());
}
else
{

72
corelib/src/Rtabmap.cpp
View File

@@ -100,6 +100,11 @@ Rtabmap::Rtabmap() :
_toroIterations(Parameters::defaultRGBDToroIterations()),
_databasePath(""),
_optimizeFromGraphEnd(Parameters::defaultRGBDOptimizeFromGraphEnd()),
_reextractLoopClosureFeatures(Parameters::defaultLccReextractLoopClosureFeatures()),
_reextractNNType(Parameters::defaultLccReextractNNType()),
_reextractNNDR(Parameters::defaultLccReextractNNDR()),
_reextractFeatureType(Parameters::defaultLccReextractFeatureType()),
_reextractMaxWords(Parameters::defaultLccReextractMaxWords()),
_lcHypothesisId(0),
_lcHypothesisValue(0),
_retrievedId(0),
@@ -349,6 +354,11 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionMaxDiffID(), _localDetectMaxDiffID);
Parameters::parse(parameters, Parameters::kRGBDToroIterations(), _toroIterations);
Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), _optimizeFromGraphEnd);
Parameters::parse(parameters, Parameters::kLccReextractLoopClosureFeatures(), _reextractLoopClosureFeatures);
Parameters::parse(parameters, Parameters::kLccReextractNNType(), _reextractNNType);
Parameters::parse(parameters, Parameters::kLccReextractNNDR(), _reextractNNDR);
Parameters::parse(parameters, Parameters::kLccReextractFeatureType(), _reextractFeatureType);
Parameters::parse(parameters, Parameters::kLccReextractMaxWords(), _reextractMaxWords);
// RGB-D SLAM stuff
if((iter=parameters.find(Parameters::kLccIcpType())) != parameters.end())
@@ -1254,6 +1264,7 @@ bool Rtabmap::process(const SensorData & data)
// Update loop closure links
// (updated: place this after retrieval to be sure that neighbors of the loop closure are in RAM)
//=============================================================
int loopClosureVisualInliers = 0; // for statistics
if(_lcHypothesisId>0)
{
//Compute transform if metric data are present
@@ -1261,7 +1272,65 @@ bool Rtabmap::process(const SensorData & data)
if(_rgbdSlamMode)
{
std::string rejectedMsg;
transform = _memory->computeVisualTransform(_lcHypothesisId, signature->id(), &rejectedMsg);
if(_reextractLoopClosureFeatures)
{
ParametersMap customParameters;
customParameters.insert(ParametersPair(Parameters::kLccBowInlierDistance(), uNumber2Str(_memory->getBowInlierDistance())));
customParameters.insert(ParametersPair(Parameters::kLccBowIterations(), uNumber2Str(_memory->getBowIterations())));
customParameters.insert(ParametersPair(Parameters::kLccBowMinInliers(), uNumber2Str(_memory->getBowMinInliers())));
customParameters.insert(ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(_memory->getBowMaxDepth())));
customParameters.insert(ParametersPair(Parameters::kLccBowForce2D(), uNumber2Str(_memory->getBowForce2D())));
customParameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
customParameters.insert(ParametersPair(Parameters::kMemImageKept(), "false"));
customParameters.insert(ParametersPair(Parameters::kMemSTMSize(), "0"));
customParameters.insert(ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(_reextractNNType))); // bruteforce
customParameters.insert(ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(_reextractNNDR)));
customParameters.insert(ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(_reextractFeatureType))); // FAST/BRIEF
customParameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(_reextractMaxWords)));
Memory memory(customParameters);
UTimer timeT;
// Add signatures
float fxA, fyA, cxA, cyA;
float fxB, fyB, cxB, cyB;
rtabmap::Transform localTransformA, localTransformB;
cv::Mat imageA, depthA;
_memory->getImageDepthRaw(signature->id(), imageA, depthA, fxA, fyA, cxA, cyA, localTransformA);
SensorData dataFrom(imageA, depthA, fxA, fyA, cxA, cyA, Transform::getIdentity(), localTransformA, 1);
UDEBUG("timeA = %fs", timeT.ticks());
cv::Mat imageB, depthB;
_memory->getImageDepthRaw(_lcHypothesisId, imageB, depthB, fxB, fyB, cxB, cyB, localTransformB);
SensorData dataTo(imageB, depthB, fxB, fyB, cxB, cyB, Transform::getIdentity(), localTransformB, 2);
UDEBUG("timeB = %fs", timeT.ticks());
if(dataFrom.isValid() && dataFrom.isMetric() && dataTo.isValid() && dataTo.isMetric())
{
memory.update(dataFrom);
UDEBUG("timeUpA = %fs", timeT.ticks());
memory.update(dataTo);
UDEBUG("timeUpB = %fs", timeT.ticks());
transform = memory.computeVisualTransform(2, 1, &rejectedMsg, &loopClosureVisualInliers);
UDEBUG("timeTransform = %fs", timeT.ticks());
}
else
{
// Fallback to normal way (raw data not kept in database...)
UWARN("Loop closure: Some images not found in memory for re-extracting "
"features, is Mem/RawDataKept=false? Falling back with already extracted 3D features.");
transform = _memory->computeVisualTransform(_lcHypothesisId, signature->id(), &rejectedMsg, &loopClosureVisualInliers);
}
}
else
{
transform = _memory->computeVisualTransform(_lcHypothesisId, signature->id(), &rejectedMsg, &loopClosureVisualInliers);
}
if(!transform.isNull() && _globalLoopClosureIcpType > 0)
{
Transform icpTransform = _memory->computeIcpTransform(_lcHypothesisId, signature->id(), transform, _globalLoopClosureIcpType == 1, &rejectedMsg);
@@ -1451,6 +1520,7 @@ bool Rtabmap::process(const SensorData & data)
statistics_.addStatistic(Statistics::kLoopVp_hypothesis(), vpHypothesis);
statistics_.addStatistic(Statistics::kLoopReactivateId(), _retrievedId);
statistics_.addStatistic(Statistics::kLoopHypothesis_ratio(), hypothesisRatio);
statistics_.addStatistic(Statistics::kLoopVisualInliers(), loopClosureVisualInliers);
statistics_.addStatistic(Statistics::kLocalLoopOdom_corrected(), scanMatchingSuccess?1:0);
statistics_.addStatistic(Statistics::kLocalLoopTime_closures(), localLoopClosuresInTimeFound);