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

Updated version to 0.8.11. Added param RGBD/LocalLoopDetectionPathOdomPosesUsed. DatabaseViewer: export option at a specified framerate. DBReader: option to read database at a rate specified by the stamps saved. Database: Added new column "data2d_max_pts" in Depth table

Browse Source
This commit is contained in:
Mathieu Labbe
2015-05-01 07:30:09 -04:00
parent d09e8f237a
commit abb7eb15ac
29 changed files with 622 additions and 275 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
corelib/src/DBDriver.cpp
View File

@@ -419,7 +419,8 @@ void DBDriver::getNodeData(
float & fy,
float & cx,
float & cy,
Transform & localTransform) const
Transform & localTransform,
int & laserScanMaxPts) const
{
bool found = false;
// look in the trash
@@ -437,6 +438,7 @@ void DBDriver::getNodeData(
cx = s->getCx();
cy = s->getCy();
localTransform = s->getLocalTransform();
laserScanMaxPts = s->getLaserScanMaxPts();
found = true;
}
}
@@ -445,7 +447,7 @@ void DBDriver::getNodeData(
if(!found)
{
_dbSafeAccessMutex.lock();
this->getNodeDataQuery(signatureId, imageCompressed, depthCompressed, laserScanCompressed, fx, fy, cx, cy, localTransform);
this->getNodeDataQuery(signatureId, imageCompressed, depthCompressed, laserScanCompressed, fx, fy, cx, cy, localTransform, laserScanMaxPts);
_dbSafeAccessMutex.unlock();
}
}

61
corelib/src/DBDriverSqlite3.cpp
View File

@@ -458,7 +458,17 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
if(loadMetricData)
{
if(uStrNumCmp(_version, "0.7.0") >= 0)
if(uStrNumCmp(_version, "0.8.11") >= 0)
{
query << "SELECT Image.data, "
"Depth.data, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.local_transform, Depth.data2d_max_pts, Depth.data2d "
<< "FROM Image "
<< "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
<< "ON Image.id = Depth.id "
<< "WHERE Image.id = ?"
<<";";
}
else if(uStrNumCmp(_version, "0.7.0") >= 0)
{
query << "SELECT Image.data, "
"Depth.data, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.local_transform, Depth.data2d "
@@ -553,14 +563,19 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
}
(*iter)->setLocalTransform(localTransform);
int laserScanMaxPts = 0;
if(uStrNumCmp(_version, "0.8.11") >= 0)
{
laserScanMaxPts = sqlite3_column_int(ppStmt, index++);
}
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
//Create the laserScan
if(dataSize>4 && data)
{
(*iter)->setLaserScanCompressed(cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone()); // depth2d
(*iter)->setLaserScanCompressed(cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(), laserScanMaxPts); // depth2d
}
}
rc = sqlite3_step(ppStmt); // next result...
@@ -588,7 +603,8 @@ void DBDriverSqlite3::getNodeDataQuery(
float & fy,
float & cx,
float & cy,
Transform & localTransform) const
Transform & localTransform,
int & laserScanMaxPts) const
{
if(_ppDb)
{
@@ -598,7 +614,17 @@ void DBDriverSqlite3::getNodeDataQuery(
sqlite3_stmt * ppStmt = 0;
std::stringstream query;
if(uStrNumCmp(_version, "0.7.0") >= 0)
if(uStrNumCmp(_version, "0.8.11") >= 0)
{
query << "SELECT Image.data, "
"Depth.data, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.local_transform, Depth.data2d_max_pts, Depth.data2d "
<< "FROM Image "
<< "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
<< "ON Image.id = Depth.id "
<< "WHERE Image.id = " << signatureId
<<";";
}
else if(uStrNumCmp(_version, "0.7.0") >= 0)
{
query << "SELECT Image.data, "
"Depth.data, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.local_transform, Depth.data2d "
@@ -675,6 +701,12 @@ void DBDriverSqlite3::getNodeDataQuery(
memcpy(localTransform.data(), data, dataSize);
}
laserScanMaxPts = 0;
if(uStrNumCmp(_version, "0.8.11") >= 0)
{
laserScanMaxPts = sqlite3_column_int(ppStmt, index++);
}
data = sqlite3_column_blob(ppStmt, index); // depth2d
dataSize = sqlite3_column_bytes(ppStmt, index++);
//Create the depth2d
@@ -1255,7 +1287,7 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
if(visualWords.size()==0)
{
UINFO("Empty signature detected! (id=%d)", (*iter)->id());
UDEBUG("Empty signature detected! (id=%d)", (*iter)->id());
}
else
{
@@ -2044,7 +2076,7 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures) const
//metric
if(!(*i)->getDepthCompressed().empty() || !(*i)->getLaserScanCompressed().empty())
{
stepDepth(ppStmt, (*i)->id(), (*i)->getDepthCompressed(), (*i)->getLaserScanCompressed(), (*i)->getFx(), (*i)->getFy(), (*i)->getCx(), (*i)->getCy(), (*i)->getLocalTransform());
stepDepth(ppStmt, (*i)->id(), (*i)->getDepthCompressed(), (*i)->getLaserScanCompressed(), (*i)->getFx(), (*i)->getFy(), (*i)->getCx(), (*i)->getCy(), (*i)->getLocalTransform(), (*i)->getLaserScanMaxPts());
}
}
// Finalize (delete) the statement
@@ -2222,7 +2254,11 @@ void DBDriverSqlite3::stepImage(sqlite3_stmt * ppStmt,
std::string DBDriverSqlite3::queryStepDepth() const
{
if(uStrNumCmp(_version, "0.7.0") >= 0)
if(uStrNumCmp(_version, "0.8.11") >= 0)
{
return "INSERT INTO Depth(id, data, fx, fy, cx, cy, local_transform, data2d, data2d_max_pts) VALUES(?,?,?,?,?,?,?,?,?);";
}
else if(uStrNumCmp(_version, "0.7.0") >= 0)
{
return "INSERT INTO Depth(id, data, fx, fy, cx, cy, local_transform, data2d) VALUES(?,?,?,?,?,?,?,?);";
}
@@ -2239,7 +2275,8 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt,
float fy,
float cx,
float cy,
const Transform & localTransform) const
const Transform & localTransform,
int depth2dMaxPts) const
{
UDEBUG("Save depth %d (size=%d) depth2d = %d", id, (int)depthBytes.cols, (int)depth2dBytes.cols);
if(!ppStmt)
@@ -2293,6 +2330,12 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt,
}
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
if(uStrNumCmp(_version, "0.8.11") >= 0)
{
rc = sqlite3_bind_int(ppStmt, index++, depth2dMaxPts);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
}
//step
rc=sqlite3_step(ppStmt);
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

6
corelib/src/DBDriverSqlite3.h
View File

@@ -80,7 +80,8 @@ private:
float & fy,
float & cx,
float & cy,
Transform & localTransform) const;
Transform & localTransform,
int & laserScanMaxPts) const;
virtual void getNodeDataQuery(int signatureId, cv::Mat & imageCompressed) const;
virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, std::vector<unsigned char> & userData) const;
virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren) const;
@@ -110,7 +111,8 @@ private:
float fy,
float cx,
float cy,
const Transform & localTransform) const;
const Transform & localTransform,
int depth2dMaxPts) const;
void stepLink(sqlite3_stmt * ppStmt, int fromId, int toId, Link::Type type, float rotVariance, float transVariance, const Transform & transform) const;
void stepWordsChanged(sqlite3_stmt * ppStmt, int signatureId, int oldWordId, int newWordId) const;
void stepKeypoint(sqlite3_stmt * ppStmt, int signatureId, int wordId, const cv::KeyPoint & kp, const pcl::PointXYZ & pt) const;

132
corelib/src/DBReader.cpp
View File

@@ -75,6 +75,7 @@ bool DBReader::init(int startIndex)
}
_ids.clear();
_currentId=_ids.end();
_previousStamp = 0;
if(!UFile::exists(_path))
{
@@ -118,10 +119,7 @@ bool DBReader::init(int startIndex)
void DBReader::setFrameRate(float frameRate)
{
if(frameRate >= 0.0f)
{
_frameRate = frameRate;
}
_frameRate = frameRate;
}
void DBReader::mainLoopBegin()
@@ -210,26 +208,6 @@ SensorData DBReader::getNextData()
SensorData data;
if(_dbDriver)
{
float frameRate = _frameRate;
if(frameRate>0.0f)
{
int sleepTime = (1000.0f/frameRate - 1000.0f*_timer.getElapsedTime());
if(sleepTime > 2)
{
uSleep(sleepTime-2);
}
// Add precision at the cost of a small overhead
while(_timer.getElapsedTime() < 1.0/double(frameRate)-0.000001)
{
//
}
double slept = _timer.getElapsedTime();
_timer.start();
UDEBUG("slept=%fs vs target=%fs", slept, 1.0/double(frameRate));
}
if(!this->isKilled() && _currentId != _ids.end())
{
cv::Mat imageBytes;
@@ -241,7 +219,8 @@ SensorData DBReader::getNextData()
float rotVariance = 1.0f;
float transVariance = 1.0f;
std::vector<unsigned char> userData;
_dbDriver->getNodeData(*_currentId, imageBytes, depthBytes, laserScanBytes, fx, fy, cx, cy, localTransform);
int laserScanMaxPts = 0;
_dbDriver->getNodeData(*_currentId, imageBytes, depthBytes, laserScanBytes, fx, fy, cx, cy, localTransform, laserScanMaxPts);
// info
int weight;
@@ -272,32 +251,83 @@ SensorData DBReader::getNextData()
UWARN("No image loaded from the database for id=%d!", *_currentId);
}
rtabmap::CompressionThread ctImage(imageBytes, true);
rtabmap::CompressionThread ctDepth(depthBytes, true);
rtabmap::CompressionThread ctLaserScan(laserScanBytes, false);
ctImage.start();
ctDepth.start();
ctLaserScan.start();
ctImage.join();
ctDepth.join();
ctLaserScan.join();
data = SensorData(
ctLaserScan.getUncompressedData(),
ctImage.getUncompressedData(),
ctDepth.getUncompressedData(),
fx,fy,cx,cy,
localTransform,
pose,
rotVariance,
transVariance,
seq,
stamp,
userData);
UDEBUG("Laser=%d RGB/Left=%d Depth=%d Right=%d",
data.laserScan().empty()?0:1,
data.image().empty()?0:1,
data.depth().empty()?0:1,
data.rightImage().empty()?0:1);
// Frame rate
if(_frameRate < 0.0f)
{
if(stamp == 0)
{
UERROR("The option to use database stamps is set (framerate<0), but there are no stamps saved in the database! Aborting...");
this->kill();
}
else if(_previousStamp > 0)
{
int sleepTime = 1000.0*(stamp-_previousStamp) - 1000.0*_timer.getElapsedTime();
if(sleepTime > 2)
{
uSleep(sleepTime-2);
}
// Add precision at the cost of a small overhead
while(_timer.getElapsedTime() < (stamp-_previousStamp)-0.000001)
{
//
}
double slept = _timer.getElapsedTime();
_timer.start();
UDEBUG("slept=%fs vs target=%fs", slept, stamp-_previousStamp);
}
_previousStamp = stamp;
}
else if(_frameRate>0.0f)
{
int sleepTime = (1000.0f/_frameRate - 1000.0f*_timer.getElapsedTime());
if(sleepTime > 2)
{
uSleep(sleepTime-2);
}
// Add precision at the cost of a small overhead
while(_timer.getElapsedTime() < 1.0/double(_frameRate)-0.000001)
{
//
}
double slept = _timer.getElapsedTime();
_timer.start();
UDEBUG("slept=%fs vs target=%fs", slept, 1.0/double(_frameRate));
}
if(!this->isKilled())
{
rtabmap::CompressionThread ctImage(imageBytes, true);
rtabmap::CompressionThread ctDepth(depthBytes, true);
rtabmap::CompressionThread ctLaserScan(laserScanBytes, false);
ctImage.start();
ctDepth.start();
ctLaserScan.start();
ctImage.join();
ctDepth.join();
ctLaserScan.join();
data = SensorData(
ctLaserScan.getUncompressedData(),
laserScanMaxPts,
ctImage.getUncompressedData(),
ctDepth.getUncompressedData(),
fx,fy,cx,cy,
localTransform,
pose,
rotVariance,
transVariance,
seq,
stamp,
userData);
UDEBUG("Laser=%d RGB/Left=%d Depth=%d Right=%d",
data.laserScan().empty()?0:1,
data.image().empty()?0:1,
data.depth().empty()?0:1,
data.rightImage().empty()?0:1);
}
}
}
else

98
corelib/src/Memory.cpp
View File

@@ -2006,7 +2006,8 @@ Transform Memory::computeIcpTransform(
bool icp3D,
std::string * rejectedMsg,
int * inliers,
double * variance)
double * variance,
float * inliersRatio)
{
Signature * oldS = this->_getSignature(oldId);
Signature * newS = this->_getSignature(newId);
@@ -2064,7 +2065,7 @@ Transform Memory::computeIcpTransform(
newS->uncompressData(0, 0, &tmp2);
}
t = computeIcpTransform(*oldS, *newS, guess, icp3D, rejectedMsg, inliers, variance);
t = computeIcpTransform(*oldS, *newS, guess, icp3D, rejectedMsg, inliers, variance, inliersRatio);
}
else
{
@@ -2085,8 +2086,9 @@ Transform Memory::computeIcpTransform(
Transform guess,
bool icp3D,
std::string * rejectedMsg,
int * inliers,
double * variance) const
int * correspondencesOut,
double * varianceOut,
float * correspondencesRatioOut) const
{
if(guess.isNull())
{
@@ -2145,6 +2147,7 @@ Transform Memory::computeIcpTransform(
Transform icpT;
int correspondences = 0;
float correspondencesRatio = -1.0f;
double variance = 1;
if(_icpPointToPlane)
{
pcl::PointCloud<pcl::PointNormal>::Ptr oldCloud = util3d::computeNormals(oldCloudXYZ, _icpPointToPlaneNormalNeighbors);
@@ -2161,7 +2164,7 @@ Transform Memory::computeIcpTransform(
_icpMaxCorrespondenceDistance,
_icpMaxIterations,
&hasConverged,
variance,
&variance,
&correspondences);
}
}
@@ -2172,23 +2175,31 @@ Transform Memory::computeIcpTransform(
_icpMaxCorrespondenceDistance,
_icpMaxIterations,
&hasConverged,
variance,
&variance,
&correspondences);
}
// verify if there are enough correspondences
correspondencesRatio = float(correspondences)/float(oldCloudXYZ->size()>newCloudXYZ->size()?oldCloudXYZ->size():newCloudXYZ->size());
correspondencesRatio = float(correspondences)/float(newS.getDepthRaw().total());
UDEBUG("%d->%d hasConverged=%s, variance=%f, correspondences=%d/%d (%f%%)",
hasConverged?"true":"false",
variance?*variance:-1,
variance,
correspondences,
(int)(oldCloudXYZ->size()>newCloudXYZ->size()?oldCloudXYZ->size():newCloudXYZ->size()),
correspondencesRatio*100.0f);
if(inliers)
if(varianceOut)
{
*inliers = correspondences;
*varianceOut = variance;
}
if(correspondencesOut)
{
*correspondencesOut = correspondences;
}
if(correspondencesRatioOut)
{
*correspondencesRatioOut = correspondencesRatio;
}
if(!icpT.isNull() && hasConverged &&
@@ -2217,8 +2228,8 @@ Transform Memory::computeIcpTransform(
}
else
{
msg = uFormat("Cannot compute transform (converged=%s var=%f corrRatio=%f/%f)",
hasConverged?"true":"false", variance?*variance:-1, correspondencesRatio, _icpCorrespondenceRatio);
msg = uFormat("Cannot compute transform (converged=%s var=%f corr=%d corrRatio=%f/%f)",
hasConverged?"true":"false", variance, correspondences, correspondencesRatio, _icpCorrespondenceRatio);
UINFO(msg.c_str());
}
}
@@ -2267,21 +2278,30 @@ Transform Memory::computeIcpTransform(
bool hasConverged = false;
float correspondencesRatio = -1.0f;
int correspondences = 0;
double variance = 1;
icpT = util3d::icp2D(newCloud,
oldCloud,
_icp2MaxCorrespondenceDistance,
_icp2MaxIterations,
&hasConverged,
variance,
&variance,
&correspondences);
// verify if there are enough correspondences
correspondencesRatio = float(correspondences)/float(oldCloud->size()>newCloud->size()?oldCloud->size():newCloud->size());
if(newS.getLaserScanMaxPts())
{
correspondencesRatio = float(correspondences)/float(newS.getLaserScanMaxPts());
}
else
{
correspondencesRatio = float(correspondences)/float(oldCloud->size()>newCloud->size()?oldCloud->size():newCloud->size());
}
UDEBUG("%d->%d hasConverged=%s, variance=%f, correspondences=%d/%d (%f%%)",
newS.id(), oldS.id(),
hasConverged?"true":"false",
variance?*variance:-1,
variance,
correspondences,
(int)(oldCloud->size()>newCloud->size()?oldCloud->size():newCloud->size()),
correspondencesRatio*100.0f);
@@ -2296,12 +2316,22 @@ Transform Memory::computeIcpTransform(
// UWARN("saved newCloudFinal.pcd");
//}
if(inliers)
if(varianceOut)
{
*inliers = correspondences;
*varianceOut = variance;
}
if(correspondencesOut)
{
*correspondencesOut = correspondences;
}
if(correspondencesRatioOut)
{
*correspondencesRatioOut = correspondencesRatio;
}
if(!icpT.isNull() && hasConverged && correspondencesRatio >= _icp2CorrespondenceRatio)
if(!icpT.isNull() &&
hasConverged &&
correspondencesRatio >= _icp2CorrespondenceRatio)
{
float ix,iy,iz, iroll,ipitch,iyaw;
icpT.getTranslationAndEulerAngles(ix,iy,iz,iroll,ipitch,iyaw);
@@ -2326,8 +2356,8 @@ Transform Memory::computeIcpTransform(
}
else
{
msg = uFormat("Cannot compute transform (converged=%s var=%f corrRatio=%f/%f)",
hasConverged?"true":"false", variance?*variance:-1, correspondencesRatio, _icp2CorrespondenceRatio);
msg = uFormat("Cannot compute transform (converged=%s var=%f cor=%d corrRatio=%f/%f)",
hasConverged?"true":"false", variance, correspondences, correspondencesRatio, _icp2CorrespondenceRatio);
UINFO(msg.c_str());
}
}
@@ -2362,6 +2392,9 @@ Transform Memory::computeScanMatchingTransform(
int * inliers,
double * variance)
{
UASSERT(uContains(poses, newId) && uContains(_signatures, newId));
UASSERT(uContains(poses, oldId) && uContains(_signatures, oldId));
// make sure that all depth2D are loaded
std::list<Signature*> depthToLoad;
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
@@ -2407,7 +2440,6 @@ Transform Memory::computeScanMatchingTransform(
// get the new cloud
Signature * newS = _getSignature(newId);
pcl::PointCloud<pcl::PointXYZ>::Ptr newCloud;
UASSERT(uContains(poses, newId));
cv::Mat newScan;
newS->uncompressData(0, 0, &newScan);
newCloud = util3d::cvMat2Cloud(newScan, poses.at(newId));
@@ -2452,10 +2484,10 @@ Transform Memory::computeScanMatchingTransform(
{
transform = poses.at(newId).inverse()*icpT.inverse() * poses.at(oldId);
//pcl::io::savePCDFile("old.pcd", *assembledOldClouds);
//pcl::io::savePCDFile("new.pcd", *newCloud);
//pcl::io::savePCDFile("old.pcd", *assembledOldClouds, true);
//pcl::io::savePCDFile("new.pcd", *newCloud, true);
//newCloud = util3d::transformPointCloud<pcl::PointXYZ>(newCloud, icpT);
//pcl::io::savePCDFile("newFinal.pcd", *newCloud);
//pcl::io::savePCDFile("newFinal.pcd", *newCloud, true);
//UWARN("local scan matching old.pcd, new.pcd and newFinal.pcd saved!");
}
else
@@ -3322,11 +3354,12 @@ void Memory::copyData(const Signature * from, Signature * to)
cv::Mat laserScan;
float fx, fy, cx, cy;
Transform localTransform;
_dbDriver->getNodeData(from->id(), image, depth, laserScan, fx, fy, cx, cy, localTransform);
int laserScanMaxPts = 0;
_dbDriver->getNodeData(from->id(), image, depth, laserScan, fx, fy, cx, cy, localTransform, laserScanMaxPts);
to->setImageCompressed(image);
to->setDepthCompressed(depth, fx, fy, cx, cy);
to->setLaserScanCompressed(laserScan);
to->setLaserScanCompressed(laserScan, laserScanMaxPts);
to->setLocalTransform(localTransform);
UDEBUG("Loaded image data from database");
@@ -3872,7 +3905,8 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
fyOrBaseline,
cx,
cy,
data.localTransform());
data.localTransform(),
data.laserScanMaxPts());
}
else
{
@@ -3885,7 +3919,15 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
words3D,
data.pose(),
data.userData(),
rtabmap::compressData2(laserScan));
rtabmap::compressData2(laserScan),
cv::Mat(),
cv::Mat(),
0,
0,
0,
0,
Transform(),
data.laserScanMaxPts());
}
if(this->isRawDataKept())
{

102
corelib/src/Rtabmap.cpp
View File

@@ -98,6 +98,7 @@ Rtabmap::Rtabmap() :
_localRadius(Parameters::defaultRGBDLocalRadius()),
_localDetectMaxDiffID(Parameters::defaultRGBDLocalLoopDetectionMaxDiffID()),
_localPathFilteringRadius(Parameters::defaultRGBDLocalLoopDetectionPathFilteringRadius()),
_localPathOdomPosesUsed(Parameters::defaultRGBDLocalLoopDetectionPathOdomPosesUsed()),
_databasePath(""),
_optimizeFromGraphEnd(Parameters::defaultRGBDOptimizeFromGraphEnd()),
_reextractLoopClosureFeatures(Parameters::defaultLccReextractActivated()),
@@ -386,6 +387,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kRGBDLocalRadius(), _localRadius);
Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionMaxDiffID(), _localDetectMaxDiffID);
Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionPathFilteringRadius(), _localPathFilteringRadius);
Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionPathOdomPosesUsed(), _localPathOdomPosesUsed);
Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), _optimizeFromGraphEnd);
Parameters::parse(parameters, Parameters::kLccReextractActivated(), _reextractLoopClosureFeatures);
Parameters::parse(parameters, Parameters::kLccReextractNNType(), _reextractNNType);
@@ -951,7 +953,9 @@ bool Rtabmap::process(const SensorData & data)
std::string rejectedMsg;
Transform guess = signature->getLinks().begin()->second.transform();
double variance = -1.0;
Transform t = _memory->computeIcpTransform(oldId, signature->id(), guess, false, &rejectedMsg, 0, &variance);
int inliers = 0;
float inliersRatio = 0;
Transform t = _memory->computeIcpTransform(oldId, signature->id(), guess, false, &rejectedMsg, &inliers, &variance, &inliersRatio);
if(!t.isNull())
{
scanMatchingSuccess = true;
@@ -966,6 +970,10 @@ bool Rtabmap::process(const SensorData & data)
{
UINFO("Scan matching rejected: %s", rejectedMsg.c_str());
}
statistics_.addStatistic(Statistics::kOdomCorrectionAccepted(), scanMatchingSuccess?1.0f:0);
statistics_.addStatistic(Statistics::kOdomCorrectionInliers(), inliers);
statistics_.addStatistic(Statistics::kOdomCorrectionInliers_ratio(), inliersRatio);
statistics_.addStatistic(Statistics::kOdomCorrectionVariance(), variance);
}
timeScanMatching = timer.ticks();
ULOGGER_INFO("timeScanMatching=%fs", timeScanMatching);
@@ -1554,17 +1562,6 @@ bool Rtabmap::process(const SensorData & data)
if(_localPathFilteringRadius <= 0.0f ||
_optimizedPoses.at(signature->id()).getDistance(_optimizedPoses.at(nearestId)) < _localPathFilteringRadius)
{
// path filtering
if(_localPathFilteringRadius > 0.0f)
{
std::map<int, Transform> filteredPath = graph::radiusPosesFiltering(path, _localPathFilteringRadius, CV_PI, true);
// make sure the nearest and farthest poses are still here
filteredPath.insert(*_optimizedPoses.find(nearestId));
filteredPath.insert(*path.begin());
filteredPath.insert(*path.rbegin());
path = filteredPath;
}
// 1) look for loop closures based on visual correspondences
double variance = 1.0;
Transform transform = _memory->computeVisualTransform(nearestId, signature->id(), 0, 0, &variance);
@@ -1576,9 +1573,31 @@ bool Rtabmap::process(const SensorData & data)
}
if(transform.isNull())
{
// 2) Assemble scans in the path and do ICP only
if(_localPathOdomPosesUsed)
{
//optimize the path's poses locally
path = optimizeGraph(nearestId, uKeys(path), false);
// transform local poses in optimized graph referential
Transform t = _optimizedPoses.at(nearestId) * path.at(nearestId).inverse();
for(std::map<int, Transform>::iterator jter=path.begin(); jter!=path.end(); ++jter)
{
jter->second = t * jter->second;
}
}
if(_localPathFilteringRadius > 0.0f)
{
// path filtering
std::map<int, Transform> filteredPath = graph::radiusPosesFiltering(path, _localPathFilteringRadius, CV_PI, true);
// make sure the nearest and farthest poses are still here
filteredPath.insert(*path.find(nearestId));
filteredPath.insert(*path.begin());
filteredPath.insert(*path.rbegin());
path = filteredPath;
}
if(path.size() > 2) // more than current+nearest
{
// 2) Assemble scans in the path and do ICP only
// add current node to poses
path.insert(std::make_pair(signature->id(), _optimizedPoses.at(signature->id())));
//The nearest will be the reference for a loop closure transform
@@ -1742,7 +1761,6 @@ bool Rtabmap::process(const SensorData & data)
statistics_.addStatistic(Statistics::kLoopVisualInliers(), loopClosureVisualInliers);
statistics_.addStatistic(Statistics::kLoopLast_id(), _memory->getLastGlobalLoopClosureId());
statistics_.addStatistic(Statistics::kLocalLoopOdom_corrected(), scanMatchingSuccess?1:0);
statistics_.addStatistic(Statistics::kLocalLoopTime_closures(), localLoopClosuresInTimeFound);
statistics_.addStatistic(Statistics::kLocalLoopSpace_closures_added(), localSpaceClosuresAdded);
statistics_.addStatistic(Statistics::kLocalLoopSpace_closures_added_icp_only(), localSpaceClosuresAddedByICPOnly);
@@ -2295,36 +2313,50 @@ void Rtabmap::optimizeCurrentMap(
}
UINFO("get ids time %f s", timer.ticks());
std::map<int, Transform> poses;
std::multimap<int, Link> edgeConstraints;
_memory->getMetricConstraints(uKeys(ids), poses, edgeConstraints, lookInDatabase);
UINFO("get constraints (%d poses, %d edges) time %f s", (int)poses.size(), (int)edgeConstraints.size(), timer.ticks());
if(constraints)
{
*constraints = edgeConstraints;
}
UASSERT(_graphOptimizer!=0);
if(_graphOptimizer->iterations() == 0)
{
// Optimization desactivated! Return not optimized poses.
optimizedPoses = poses;
}
else
{
optimizedPoses = _graphOptimizer->optimize(id, poses, edgeConstraints);
}
UINFO("optimize time %f s", timer.ticks());
optimizedPoses = Rtabmap::optimizeGraph(id, uKeys(ids), lookInDatabase, constraints);
if(_memory->getSignature(id) && uContains(optimizedPoses, id))
{
Transform t = optimizedPoses.at(id) * _memory->getSignature(id)->getPose().inverse();
UINFO("Correction (from node %d) %s", id, t.prettyPrint().c_str());
}
UINFO("optimize time %f s", timer.ticks());
}
}
std::map<int, Transform> Rtabmap::optimizeGraph(
int fromId,
const std::vector<int> & ids,
bool lookInDatabase,
std::multimap<int, Link> * constraints) const
{
UTimer timer;
std::map<int, Transform> optimizedPoses;
std::map<int, Transform> poses;
std::multimap<int, Link> edgeConstraints;
UDEBUG("ids=%d", (int)ids.size());
_memory->getMetricConstraints(ids, poses, edgeConstraints, lookInDatabase);
UDEBUG("get constraints (%d poses, %d edges) time %f s", (int)poses.size(), (int)edgeConstraints.size(), timer.ticks());
if(constraints)
{
*constraints = edgeConstraints;
}
UASSERT(_graphOptimizer!=0);
if(_graphOptimizer->iterations() == 0)
{
// Optimization desactivated! Return not optimized poses.
optimizedPoses = poses;
}
else
{
optimizedPoses = _graphOptimizer->optimize(fromId, poses, edgeConstraints);
}
return optimizedPoses;
}
void Rtabmap::adjustLikelihood(std::map<int, float> & likelihood) const
{
ULOGGER_DEBUG("likelihood.size()=%d", likelihood.size());

7
corelib/src/SensorData.cpp
View File

@@ -45,7 +45,8 @@ SensorData::SensorData() :
_cy(0.0f),
_localTransform(Transform::getIdentity()),
_poseRotVariance(1.0f),
_poseTransVariance(1.0f)
_poseTransVariance(1.0f),
_laserScanMaxPts(0)
{
}
@@ -63,6 +64,7 @@ SensorData::SensorData(const cv::Mat & image,
_localTransform(Transform::getIdentity()),
_poseRotVariance(1.0f),
_poseTransVariance(1.0f),
_laserScanMaxPts(0),
_userData(userData)
{
UASSERT(image.type() == CV_8UC1 || // Mono
@@ -95,6 +97,7 @@ SensorData::SensorData(const cv::Mat & image,
_localTransform(localTransform),
_poseRotVariance(poseRotVariance),
_poseTransVariance(poseTransVariance),
_laserScanMaxPts(0),
_userData(userData)
{
UASSERT(image.type() == CV_8UC1 || // Mono
@@ -109,6 +112,7 @@ SensorData::SensorData(const cv::Mat & image,
// Metric constructor + 2d depth
SensorData::SensorData(const cv::Mat & laserScan,
int laserScanMaxPts,
const cv::Mat & image,
const cv::Mat & depthOrRightImage,
float fx,
@@ -135,6 +139,7 @@ SensorData::SensorData(const cv::Mat & laserScan,
_localTransform(localTransform),
_poseRotVariance(poseRotVariance),
_poseTransVariance(poseTransVariance),
_laserScanMaxPts(laserScanMaxPts),
_userData(userData)
{
UASSERT(_laserScan.empty() || _laserScan.type() == CV_32FC2);

10
corelib/src/Signature.cpp
View File

@@ -48,7 +48,8 @@ Signature::Signature() :
_fx(0.0f),
_fy(0.0f),
_cx(0.0f),
_cy(0.0f)
_cy(0.0f),
_laserScanMaxPts(0)
{
}
@@ -69,7 +70,8 @@ Signature::Signature(
float fy,
float cx,
float cy,
const Transform & localTransform) :
const Transform & localTransform,
int laserScanMaxPts) :
_id(id),
_mapId(mapId),
_stamp(stamp),
@@ -90,7 +92,8 @@ Signature::Signature(
_cy(cy),
_pose(pose),
_localTransform(localTransform),
_words3(words3)
_words3(words3),
_laserScanMaxPts(laserScanMaxPts)
{
}
@@ -273,6 +276,7 @@ SensorData Signature::toSensorData()
}
}
return SensorData(_laserScanRaw,
_laserScanMaxPts,
_imageRaw,
_depthRaw,
_fx,

1
corelib/src/resources/DatabaseSchema.sql.in
View File

@@ -42,6 +42,7 @@ CREATE TABLE Depth (
cy FLOAT,
local_transform BLOB,
data2d BLOB, -- compressed data (Laser scan)
data2d_max_pts INTEGER, -- Laser scan max points
time_enter DATE,
PRIMARY KEY (id)
);

36
corelib/src/util3d.cpp
View File

@@ -1933,7 +1933,7 @@ Transform icp(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
int maximumIterations,
bool * hasConvergedOut,
double * variance,
int * inliers)
int * correspondencesOut)
{
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
// Set the input source and target
@@ -1956,7 +1956,7 @@ Transform icp(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
bool hasConverged = icp.hasConverged();
// compute variance
if((inliers || variance) && hasConverged)
if((correspondencesOut || variance) && hasConverged)
{
pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>::Ptr est;
est.reset(new pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>);
@@ -1986,16 +1986,16 @@ Transform icp(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
}
}
if(inliers)
if(correspondencesOut)
{
*inliers = (int)correspondences.size();
*correspondencesOut = (int)correspondences.size();
}
}
else
{
if(inliers)
if(correspondencesOut)
{
*inliers = 0;
*correspondencesOut = 0;
}
if(variance)
{
@@ -2019,7 +2019,7 @@ Transform icpPointToPlane(
int maximumIterations,
bool * hasConvergedOut,
double * variance,
int * inliers)
int * correspondencesOut)
{
pcl::IterativeClosestPoint<pcl::PointNormal, pcl::PointNormal> icp;
// Set the input source and target
@@ -2046,7 +2046,7 @@ Transform icpPointToPlane(
bool hasConverged = icp.hasConverged();
// compute variance
if((inliers || variance) && hasConverged)
if((correspondencesOut || variance) && hasConverged)
{
pcl::registration::CorrespondenceEstimation<pcl::PointNormal, pcl::PointNormal>::Ptr est;
est.reset(new pcl::registration::CorrespondenceEstimation<pcl::PointNormal, pcl::PointNormal>);
@@ -2076,16 +2076,16 @@ Transform icpPointToPlane(
}
}
if(inliers)
if(correspondencesOut)
{
*inliers = (int)correspondences.size();
*correspondencesOut = (int)correspondences.size();
}
}
else
{
if(inliers)
if(correspondencesOut)
{
*inliers = 0;
*correspondencesOut = 0;
}
if(variance)
{
@@ -2108,7 +2108,7 @@ Transform icp2D(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
int maximumIterations,
bool * hasConvergedOut,
double * variance,
int * inliers)
int * correspondencesOut)
{
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
// Set the input source and target
@@ -2135,7 +2135,7 @@ Transform icp2D(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
bool hasConverged = icp.hasConverged();
// compute variance
if((inliers || variance) && hasConverged)
if((correspondencesOut || variance) && hasConverged)
{
pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>::Ptr est;
est.reset(new pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>);
@@ -2165,16 +2165,16 @@ Transform icp2D(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
}
}
if(inliers)
if(correspondencesOut)
{
*inliers = (int)correspondences.size();
*correspondencesOut = (int)correspondences.size();
}
}
else
{
if(inliers)
if(correspondencesOut)
{
*inliers = 0;
*correspondencesOut = 0;
}
if(variance)
{