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.0

Browse Source 
Libraries are installed in lib directly with symbolic links, not in lib/rtabmap-0.8. Removed the need of RPATH in cmake.
Saving variance of each link in database (new field Link.variance). The variance is used to generate the constraint information matrices for TORO optimization.
ICP: computing variance instead of fitness.
ICP3: added correspondences ratio parameter
Added OdometryInfo class
Refactoring: renamed depth2d stuff to laserScan. rtabmap::Memory and rtabmap::Signature classes (no more distinct neighbor, loop closure or child loop closure links, only links with different types)
This commit is contained in:
Mathieu Labbe
2014-12-14 16:42:10 -05:00
parent 6acf374063
commit 744e2fb3c7
42 changed files with 1764 additions and 1460 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
corelib/src/DBDriver.cpp
View File

@@ -406,7 +406,7 @@ void DBDriver::getNodeData(
int signatureId,
cv::Mat & imageCompressed,
cv::Mat & depthCompressed,
cv::Mat & depth2dCompressed,
cv::Mat & laserScanCompressed,
float & fx,
float & fy,
float & cx,
@@ -414,7 +414,7 @@ void DBDriver::getNodeData(
Transform & localTransform) const
{
_dbSafeAccessMutex.lock();
this->getNodeDataQuery(signatureId, imageCompressed, depthCompressed, depth2dCompressed, fx, fy, cx, cy, localTransform);
this->getNodeDataQuery(signatureId, imageCompressed, depthCompressed, laserScanCompressed, fx, fy, cx, cy, localTransform);
_dbSafeAccessMutex.unlock();
}
@@ -435,10 +435,10 @@ void DBDriver::getPose(int signatureId, Transform & pose, int & mapId) const
}
//TODO Check also in the trash ?
void DBDriver::loadNeighbors(int signatureId, std::map<int, Transform> & neighbors) const
void DBDriver::loadLinks(int signatureId, std::map<int, Link> & links, Link::Type type) const
{
_dbSafeAccessMutex.lock();
this->loadNeighborsQuery(signatureId, neighbors);
this->loadLinksQuery(signatureId, links, type);
_dbSafeAccessMutex.unlock();
}
@@ -450,14 +450,6 @@ void DBDriver::getWeight(int signatureId, int & weight) const
_dbSafeAccessMutex.unlock();
}
//TODO Check also in the trash ?
void DBDriver::loadLoopClosures(int signatureId, std::map<int, Transform> & loopIds, std::map<int, Transform> & childIds) const
{
_dbSafeAccessMutex.lock();
this->loadLoopClosuresQuery(signatureId, loopIds, childIds);
_dbSafeAccessMutex.unlock();
}
//TODO Check also in the trash ?
void DBDriver::getAllNodeIds(std::set<int> & ids, bool ignoreChildren) const
{

255
corelib/src/DBDriverSqlite3.cpp
View File

@@ -555,11 +555,10 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
//Create the depth2d
cv::Mat depth2dCompressed;
//Create the laserScan
if(dataSize>4 && data)
{
(*iter)->setDepth2DCompressed(cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone()); // depth2d
(*iter)->setLaserScanCompressed(cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone()); // depth2d
}
}
@@ -584,7 +583,7 @@ void DBDriverSqlite3::getNodeDataQuery(
int signatureId,
cv::Mat & imageCompressed,
cv::Mat & depthCompressed,
cv::Mat & depth2dCompressed,
cv::Mat & laserScanCompressed,
float & fx,
float & fy,
float & cx,
@@ -681,7 +680,7 @@ void DBDriverSqlite3::getNodeDataQuery(
//Create the depth2d
if(dataSize>4 && data)
{
depth2dCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
laserScanCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
}
if(depthCompressed.empty() || fx <= 0 || fy <= 0 || cx < 0 || cy < 0)
@@ -820,7 +819,7 @@ void DBDriverSqlite3::getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildre
<< "FROM Node "
<< "LEFT OUTER JOIN Link "
<< "ON id = from_id "
<< "WHERE type!=1 "
<< "WHERE type==0 " // select only nodes with neighor links, ignore merged nodes
<< "ORDER BY id";
}
@@ -840,7 +839,7 @@ void DBDriverSqlite3::getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildre
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
ULOGGER_DEBUG("Time=%f", timer.ticks());
ULOGGER_DEBUG("Time=%f ids=%d", timer.ticks(), (int)ids.size());
}
}
@@ -962,72 +961,6 @@ void DBDriverSqlite3::getWeightQuery(int nodeId, int & weight) const
}
}
void DBDriverSqlite3::loadLoopClosuresQuery(int nodeId, std::map<int, Transform> & loopIds, std::map<int, Transform> & childIds) const
{
loopIds.clear();
childIds.clear();
if(_ppDb)
{
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
std::stringstream query;
query << "SELECT to_id, type, transform FROM Link WHERE from_id = "
<< nodeId
<< " AND type > 0"
<< ";";
rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
int toId = 0;
int type;
const void * data = 0;
int dataSize = 0;
// Process the result if one
rc = sqlite3_step(ppStmt);
while(rc == SQLITE_ROW)
{
int index = 0;
toId = sqlite3_column_int(ppStmt, index++);
type = sqlite3_column_int(ppStmt, index++);
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
Transform transform;
if((unsigned int)dataSize == transform.size()*sizeof(float) && data)
{
memcpy(transform.data(), data, dataSize);
}
if(nodeId == toId)
{
UERROR("Loop links cannot be auto-reference links (node=%d)", toId);
}
else if(type == 1)
{
UDEBUG("Load link from %d to %d, type=%d", nodeId, toId, 1);
//loop id
loopIds.insert(std::pair<int, Transform>(toId, transform));
}
else if(type == 2)
{
UDEBUG("Load link from %d to %d, type=%d", nodeId, toId, 2);
//loop id
childIds.insert(std::pair<int, Transform>(toId, transform));
}
rc = sqlite3_step(ppStmt);
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
}
}
//may be slower than the previous version but don't have a limit of words that can be loaded at the same time
void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & nodes) const
{
@@ -1413,7 +1346,10 @@ void DBDriverSqlite3::loadWordsQuery(const std::set<int> & wordIds, std::list<Vi
}
}
void DBDriverSqlite3::loadNeighborsQuery(int signatureId, std::map<int, Transform> & neighbors) const
void DBDriverSqlite3::loadLinksQuery(
int signatureId,
std::map<int, Link> & neighbors,
Link::Type typeIn) const
{
neighbors.clear();
if(_ppDb)
@@ -1424,15 +1360,38 @@ void DBDriverSqlite3::loadNeighborsQuery(int signatureId, std::map<int, Transfor
sqlite3_stmt * ppStmt = 0;
std::stringstream query;
query << "SELECT to_id, transform FROM Link "
<< "WHERE from_id = " << signatureId
<< " AND type = 0"
<< " ORDER BY to_id";
if(uStrNumCmp(_version, "0.7.4") >= 0)
{
query << "SELECT to_id, type, transform, variance FROM Link ";
}
else
{
query << "SELECT to_id, type, transform FROM Link ";
}
query << "WHERE from_id = " << signatureId;
if(typeIn != Link::kUndef)
{
if(uStrNumCmp(_version, "0.7.4") >= 0)
{
query << " AND type = " << typeIn;
}
else if(typeIn == Link::kNeighbor)
{
query << " AND type = 0";
}
else if(typeIn > Link::kNeighbor)
{
query << " AND type > 0";
}
}
query << " ORDER BY to_id";
rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
int toId = -1;
int type = Link::kUndef;
float variance = 1.0f;
const void * data = 0;
int dataSize = 0;
@@ -1443,6 +1402,7 @@ void DBDriverSqlite3::loadNeighborsQuery(int signatureId, std::map<int, Transfor
int index = 0;
toId = sqlite3_column_int(ppStmt, index++);
type = sqlite3_column_int(ppStmt, index++);
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
@@ -1453,7 +1413,17 @@ void DBDriverSqlite3::loadNeighborsQuery(int signatureId, std::map<int, Transfor
memcpy(transform.data(), data, dataSize);
}
neighbors.insert(neighbors.end(), std::pair<int, Transform>(toId, transform));
if(uStrNumCmp(_version, "0.7.4") >= 0)
{
variance = sqlite3_column_double(ppStmt, index++);
neighbors.insert(neighbors.end(), std::make_pair(toId, Link(signatureId, toId, (Link::Type)type, transform, variance)));
}
else
{
// neighbor is 0, loop closures are 1 and 2 (child)
neighbors.insert(neighbors.end(), std::make_pair(toId, Link(signatureId, toId, type==0?Link::kNeighbor:Link::kGlobalClosure, transform, variance)));
}
rc = sqlite3_step(ppStmt);
}
@@ -1481,9 +1451,18 @@ void DBDriverSqlite3::loadLinksQuery(std::list<Signature *> & signatures) const
std::stringstream query;
int totalLinksLoaded = 0;
query << "SELECT to_id, type, transform FROM Link "
<< "WHERE from_id = ? "
<< "ORDER BY to_id";
if(uStrNumCmp(_version, "0.7.4") >= 0)
{
query << "SELECT to_id, type, variance, transform FROM Link "
<< "WHERE from_id = ? "
<< "ORDER BY to_id";
}
else
{
query << "SELECT to_id, type, transform FROM Link "
<< "WHERE from_id = ? "
<< "ORDER BY to_id";
}
rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
@@ -1496,9 +1475,8 @@ void DBDriverSqlite3::loadLinksQuery(std::list<Signature *> & signatures) const
int toId = -1;
int linkType = -1;
std::map<int, Transform> neighbors;
std::map<int, Transform> loopIds;
std::map<int, Transform> childIds;
float variance = 1.0f;
std::list<Link> links;
const void * data = 0;
int dataSize = 0;
@@ -1510,34 +1488,33 @@ void DBDriverSqlite3::loadLinksQuery(std::list<Signature *> & signatures) const
toId = sqlite3_column_int(ppStmt, index++);
linkType = sqlite3_column_int(ppStmt, index++);
if(uStrNumCmp(_version, "0.7.4") >= 0)
{
variance = sqlite3_column_double(ppStmt, index++);
}
//transform
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
Transform transform;
if((unsigned int)dataSize == transform.size()*sizeof(float) && data)
UASSERT((unsigned int)dataSize == transform.size()*sizeof(float) && data);
memcpy(transform.data(), data, dataSize);
if(linkType >= 0 && linkType != Link::kUndef)
{
memcpy(transform.data(), data, dataSize);
if(uStrNumCmp(_version, "0.7.4") >= 0)
{
links.push_back(Link((*iter)->id(), toId, (Link::Type)linkType, transform, variance));
}
else // neighbor is 0, loop closures are 1 and 2 (child)
{
links.push_back(Link((*iter)->id(), toId, linkType == 0?Link::kNeighbor:Link::kGlobalClosure, transform, variance));
}
}
else
{
UFATAL("");
}
if(linkType == 1)
{
UDEBUG("Load link from %d to %d, type=%d", (*iter)->id(), toId, 1);
loopIds.insert(std::pair<int, Transform>(toId, transform));
}
else if(linkType == 2)
{
UDEBUG("Load link from %d to %d, type=%d", (*iter)->id(), toId, 2);
childIds.insert(std::pair<int, Transform>(toId, transform));
}
else if(linkType == 0)
{
UDEBUG("Load link from %d to %d, type=%d", (*iter)->id(), toId, 0);
neighbors.insert(neighbors.end(), std::pair<int, Transform>(toId, transform));
UFATAL("Not supported link type %d ! (fromId=%d, toId=%d)",
linkType, (*iter)->id(), toId);
}
++totalLinksLoaded;
@@ -1546,14 +1523,12 @@ void DBDriverSqlite3::loadLinksQuery(std::list<Signature *> & signatures) const
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
// add links
(*iter)->addNeighbors(neighbors);
(*iter)->setLoopClosureIds(loopIds);
(*iter)->setChildLoopClosureIds(childIds);
(*iter)->addLinks(links);
//reset
rc = sqlite3_reset(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
UDEBUG("time=%fs, node=%d, neighbors.size=%d, loopIds=%d, childIds=%d", timer.ticks(), (*iter)->id(), neighbors.size(), loopIds.size(), childIds.size());
UDEBUG("time=%fs, node=%d, links.size=%d", timer.ticks(), (*iter)->id(), links.size());
}
// Finalize (delete) the statement
@@ -1610,7 +1585,7 @@ void DBDriverSqlite3::updateQuery(const std::list<Signature *> & nodes) const
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
for(std::list<Signature *>::const_iterator j=nodes.begin(); j!=nodes.end(); ++j)
{
if((*j)->isNeighborsModified())
if((*j)->isLinksModified())
{
rc = sqlite3_bind_int(ppStmt, 1, (*j)->id());
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
@@ -1632,24 +1607,13 @@ void DBDriverSqlite3::updateQuery(const std::list<Signature *> & nodes) const
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
for(std::list<Signature *>::const_iterator j=nodes.begin(); j!=nodes.end(); ++j)
{
if((*j)->isNeighborsModified())
if((*j)->isLinksModified())
{
// Save neighbor links
const std::map<int, Transform> & neighbors = (*j)->getNeighbors();
for(std::map<int, Transform>::const_iterator i=neighbors.begin(); i!=neighbors.end(); ++i)
// Save links
const std::map<int, Link> & links = (*j)->getLinks();
for(std::map<int, Link>::const_iterator i=links.begin(); i!=links.end(); ++i)
{
stepLink(ppStmt, (*j)->id(), i->first, 0, i->second);
}
// save loop closure links
const std::map<int, Transform> & loopIds = (*j)->getLoopClosureIds();
for(std::map<int, Transform>::const_iterator i=loopIds.begin(); i!=loopIds.end(); ++i)
{
stepLink(ppStmt, (*j)->id(), i->first, 1, i->second);
}
const std::map<int, Transform> & childIds = (*j)->getChildLoopClosureIds();
for(std::map<int, Transform>::const_iterator i=childIds.begin(); i!=childIds.end(); ++i)
{
stepLink(ppStmt, (*j)->id(), i->first, 2, i->second);
stepLink(ppStmt, (*j)->id(), i->first, i->second.type(), i->second.variance(), i->second.transform());
}
}
}
@@ -1752,22 +1716,11 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures) const
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
for(std::list<Signature *>::const_iterator jter=signatures.begin(); jter!=signatures.end(); ++jter)
{
// Save neighbor links
const std::map<int, Transform> & neighbors = (*jter)->getNeighbors();
for(std::map<int, Transform>::const_iterator i=neighbors.begin(); i!=neighbors.end(); ++i)
// Save links
const std::map<int, Link> & links = (*jter)->getLinks();
for(std::map<int, Link>::const_iterator i=links.begin(); i!=links.end(); ++i)
{
stepLink(ppStmt, (*jter)->id(), i->first, 0, i->second);
}
// save loop closure links
const std::map<int, Transform> & loopIds = (*jter)->getLoopClosureIds();
for(std::map<int, Transform>::const_iterator i=loopIds.begin(); i!=loopIds.end(); ++i)
{
stepLink(ppStmt, (*jter)->id(), i->first, 1, i->second);
}
const std::map<int, Transform> & childIds = (*jter)->getChildLoopClosureIds();
for(std::map<int, Transform>::const_iterator i=childIds.begin(); i!=childIds.end(); ++i)
{
stepLink(ppStmt, (*jter)->id(), i->first, 2, i->second);
stepLink(ppStmt, (*jter)->id(), i->first, i->second.type(), i->second.variance(), i->second.transform());
}
}
// Finalize (delete) the statement
@@ -1833,9 +1786,9 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures) const
for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
{
//metric
if(!(*i)->getDepthCompressed().empty() || !(*i)->getDepth2DCompressed().empty())
if(!(*i)->getDepthCompressed().empty() || !(*i)->getLaserScanCompressed().empty())
{
stepDepth(ppStmt, (*i)->id(), (*i)->getDepthCompressed(), (*i)->getDepth2DCompressed(), (*i)->getDepthFx(), (*i)->getDepthFy(), (*i)->getDepthCx(), (*i)->getDepthCy(), (*i)->getLocalTransform());
stepDepth(ppStmt, (*i)->id(), (*i)->getDepthCompressed(), (*i)->getLaserScanCompressed(), (*i)->getDepthFx(), (*i)->getDepthFy(), (*i)->getDepthCx(), (*i)->getDepthCy(), (*i)->getLocalTransform());
}
}
// Finalize (delete) the statement
@@ -2055,9 +2008,16 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt,
std::string DBDriverSqlite3::queryStepLink() const
{
return "INSERT INTO Link(from_id, to_id, type, transform) VALUES(?,?,?,?);";
if(uStrNumCmp(_version, "0.7.4") >= 0)
{
return "INSERT INTO Link(from_id, to_id, type, variance, transform) VALUES(?,?,?,?,?);";
}
else
{
return "INSERT INTO Link(from_id, to_id, type, transform) VALUES(?,?,?,?);";
}
}
void DBDriverSqlite3::stepLink(sqlite3_stmt * ppStmt, int fromId, int toId, int type, const Transform & transform) const
void DBDriverSqlite3::stepLink(sqlite3_stmt * ppStmt, int fromId, int toId, int type, float variance, const Transform & transform) const
{
if(!ppStmt)
{
@@ -2072,6 +2032,13 @@ void DBDriverSqlite3::stepLink(sqlite3_stmt * ppStmt, int fromId, int toId, int
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_bind_int(ppStmt, index++, type);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
if(uStrNumCmp(_version, "0.7.4") >= 0)
{
rc = sqlite3_bind_double(ppStmt, index++, variance);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
}
rc = sqlite3_bind_blob(ppStmt, index++, transform.data(), transform.size()*sizeof(float), SQLITE_STATIC);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

10
corelib/src/DBDriverSqlite3.h
View File

@@ -68,18 +68,14 @@ private:
virtual void loadLastNodesQuery(std::list<Signature *> & signatures) const;
virtual void loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures) const;
virtual void loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const;
virtual void loadNeighborsQuery(int signatureId, std::map<int, Transform> & neighbors) const;
virtual void loadLoopClosuresQuery(
int signatureId,
std::map<int, Transform> & loopIds,
std::map<int, Transform> & childIds) const;
virtual void loadLinksQuery(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const;
virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool loadMetricData) const;
virtual void getNodeDataQuery(
int signatureId,
cv::Mat & imageCompressed,
cv::Mat & depthCompressed,
cv::Mat & depth2dCompressed,
cv::Mat & laserScanCompressed,
float & fx,
float & fy,
float & cx,
@@ -113,7 +109,7 @@ private:
float cx,
float cy,
const Transform & localTransform) const;
void stepLink(sqlite3_stmt * ppStmt, int fromId, int toId, int type, const Transform & transform) const;
void stepLink(sqlite3_stmt * ppStmt, int fromId, int toId, int type, float variance, 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;

89
corelib/src/DBReader.cpp
View File

@@ -131,36 +131,24 @@ void DBReader::mainLoopBegin()
void DBReader::mainLoop()
{
cv::Mat image, depth, depth2d;
float fx,fy,cx,cy;
Transform localTransform, pose;
int seq = 0;
this->getNextImage(image, depth, depth2d, fx, fy, cx, cy, localTransform, pose, seq);
if(!image.empty())
SensorData data = this->getNextData();
if(data.isValid())
{
if(depth.empty())
if(!_odometryIgnored)
{
this->post(new CameraEvent(image));
if(data.pose().isNull())
{
UWARN("Reading the database: odometry is null! "
"Please set \"Ignore odometry = true\" if there is "
"no odometry in the database.");
}
this->post(new OdometryEvent(data));
}
else
{
if(!_odometryIgnored)
{
SensorData data(image, depth, depth2d, fx, fy, cx, cy, pose, localTransform, seq);
this->post(new OdometryEvent(data));
if(pose.isNull())
{
UWARN("Reading the database: odometry is null! "
"Please set \"Ignore odometry = true\" if there is "
"no odometry in the database.");
}
}
else
{
// without odometry
this->post(new CameraEvent(image, depth, depth2d, fx, fy, cx, cy, localTransform, seq));
}
this->post(new CameraEvent(data));
}
}
else if(!this->isKilled())
{
@@ -171,18 +159,9 @@ void DBReader::mainLoop()
}
void DBReader::getNextImage(
cv::Mat & image,
cv::Mat & depth,
cv::Mat & depth2d,
float & fx,
float & fy,
float & cx,
float & cy,
Transform & localTransform,
Transform & pose,
int & seq)
SensorData DBReader::getNextData()
{
SensorData data;
if(_dbDriver)
{
float frameRate = _frameRate;
@@ -209,11 +188,24 @@ void DBReader::getNextImage(
{
cv::Mat imageBytes;
cv::Mat depthBytes;
cv::Mat depth2dBytes;
cv::Mat laserScanBytes;
int mapId;
_dbDriver->getNodeData(*_currentId, imageBytes, depthBytes, depth2dBytes, fx, fy, cx, cy, localTransform);
_dbDriver->getPose(*_currentId, pose, mapId);
seq = *_currentId;
float fx,fy,cx,cy;
Transform localTransform, pose;
float variance = 1.0f;
_dbDriver->getNodeData(*_currentId, imageBytes, depthBytes, laserScanBytes, fx, fy, cx, cy, localTransform);
if(!_odometryIgnored)
{
_dbDriver->getPose(*_currentId, pose, mapId);
std::map<int, Link> links;
_dbDriver->loadLinks(*_currentId, links, Link::kNeighbor);
if(links.size())
{
// assume the first is the backward neighbor, take its variance
variance = links.begin()->second.variance();
}
}
int seq = *_currentId;
++_currentId;
if(imageBytes.empty())
{
@@ -222,22 +214,29 @@ void DBReader::getNextImage(
util3d::CompressionThread ctImage(imageBytes, true);
util3d::CompressionThread ctDepth(depthBytes, true);
util3d::CompressionThread ctDepth2D(depth2dBytes, false);
util3d::CompressionThread ctLaserScan(laserScanBytes, false);
ctImage.start();
ctDepth.start();
ctDepth2D.start();
ctLaserScan.start();
ctImage.join();
ctDepth.join();
ctDepth2D.join();
image = ctImage.getUncompressedData();
depth = ctDepth.getUncompressedData();
depth2d = ctDepth2D.getUncompressedData();
ctLaserScan.join();
data = SensorData(
ctLaserScan.getUncompressedData(),
ctImage.getUncompressedData(),
ctDepth.getUncompressedData(),
fx,fy,cx,cy,
localTransform,
pose,
variance,
seq);
}
}
else
{
UERROR("Not initialized...");
}
return data;
}
} /* namespace rtabmap */

2
corelib/src/Features2d.cpp
View File

@@ -324,7 +324,7 @@ Feature2D * Feature2D::create(Feature2D::Type & type, const ParametersMap & para
if(RTABMAP_NONFREE == 0 &&
(type == Feature2D::kFeatureSurf || type == Feature2D::kFeatureSift))
{
UERROR("SURF/SIFT features cannot be used because OpenCV was not built with nonfree module. ORB is used instead.");
UWARN("SURF/SIFT features cannot be used because OpenCV was not built with nonfree module. ORB is used instead.");
type = Feature2D::kFeatureOrb;
}
Feature2D * feature2D = 0;

861
corelib/src/Memory.cpp
View File

File diff suppressed because it is too large Load Diff

162
corelib/src/Odometry.cpp
View File

@@ -118,14 +118,22 @@ bool Odometry::isLargeEnoughTransform(const Transform & transform)
fabs(yaw) > _angularUpdate;
}
Transform Odometry::process(SensorData & data, int * quality, int * features, int * localMapSize)
Transform Odometry::process(const SensorData & data, OdometryInfo * info)
{
UTimer time;
if(_pose.isNull())
{
_pose.setIdentity(); // initialized
}
Transform t = this->computeTransform(data, quality, features, localMapSize);
Transform t = this->computeTransform(data, info);
if(info)
{
info->time = time.elapsed();
info->lost = t.isNull();
}
if(!t.isNull())
{
_resetCurrentCount = _resetCountdown;
@@ -134,14 +142,10 @@ Transform Odometry::process(SensorData & data, int * quality, int * features, in
{
float x,y,z, roll,pitch,yaw;
t.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
_pose *= Transform(x,y,0,0,0,yaw);
}
else
{
_pose *= t;
t = Transform(x,y,0, 0,0,yaw);
}
return _pose;
return _pose *= t;
}
else if(_resetCurrentCount > 0)
{
@@ -231,11 +235,14 @@ void OdometryBOW::reset(const Transform & initialPose)
}
// return not null transform if odometry is correctly computed
Transform OdometryBOW::computeTransform(const SensorData & data, int * quality, int * features, int * localMapSize)
Transform OdometryBOW::computeTransform(
const SensorData & data,
OdometryInfo * info)
{
UTimer timer;
Transform output;
double variance = -1;
int inliers = 0;
int correspondences = 0;
int nFeatures = 0;
@@ -276,10 +283,9 @@ Transform OdometryBOW::computeTransform(const SensorData & data, int * quality,
UDEBUG("localMap=%d, new=%d, unique correspondences=%d", (int)localMap_.size(), (int)newSignature->getWords3().size(), (int)uniqueCorrespondences.size());
correspondences = (int)inliers1->size();
if((int)inliers1->size() >= this->getMinInliers())
{
correspondences = (int)inliers1->size();
// transform new words in local map referential
//inliers2 = util3d::transformPointCloud<pcl::PointXYZ>(inliers2, this->getPose());
@@ -291,7 +297,8 @@ Transform OdometryBOW::computeTransform(const SensorData & data, int * quality,
this->getInlierDistance(),
this->getIterations(),
this->getRefineIterations()>0, 3.0, this->getRefineIterations(),
&inliersV);
&inliersV,
&variance);
inliers = (int)inliersV.size();
if(!transform.isNull())
@@ -362,11 +369,6 @@ Transform OdometryBOW::computeTransform(const SensorData & data, int * quality,
transform = transform * icpT;
*/
if(quality)
{
*quality = inliers;
}
if(inliers < this->getMinInliers())
{
transform.setNull();
@@ -469,13 +471,13 @@ Transform OdometryBOW::computeTransform(const SensorData & data, int * quality,
_memory->emptyTrash();
}
if(features)
if(info)
{
*features = nFeatures;
}
if(localMapSize)
{
*localMapSize = (int)localMap_.size();
info->variance = variance;
info->inliers = inliers;
info->matches = correspondences;
info->features = nFeatures;
info->localMapSize = (int)localMap_.size();
}
UINFO("Odom update time = %fs out=[%s] features=%d inliers=%d/%d local_map=%d[%d] dict=%d nodes=%d",
@@ -547,32 +549,30 @@ void OdometryOpticalFlow::reset(const Transform & initialPose)
// return not null transform if odometry is correctly computed
Transform OdometryOpticalFlow::computeTransform(
const SensorData & data,
int * quality,
int * features,
int * localMapSize)
OdometryInfo * info)
{
UDEBUG("");
if(!data.rightImage().empty())
{
//stereo
return computeTransformStereo(data, quality, features);
return computeTransformStereo(data, info);
}
else
{
//rgbd
return computeTransformRGBD(data, quality, features);
return computeTransformRGBD(data, info);
}
}
Transform OdometryOpticalFlow::computeTransformStereo(
const SensorData & data,
int * quality,
int * features)
OdometryInfo * info)
{
UTimer timer;
Transform output;
double variance = -1;
int inliers = 0;
int correspondences = 0;
@@ -747,15 +747,11 @@ Transform OdometryOpticalFlow::computeTransformStereo(
this->getInlierDistance(),
this->getIterations(),
this->getRefineIterations()>0, 3.0, this->getRefineIterations(),
&inliersV);
&inliersV,
&variance);
UDEBUG("time RANSAC = %fs", timerRANSAC.ticks());
inliers = (int)inliersV.size();
if(quality)
{
*quality = inliers;
}
if(inliers < this->getMinInliers())
{
output.setNull();
@@ -841,6 +837,14 @@ Transform OdometryOpticalFlow::computeTransformStereo(
output.setNull();
}
if(info)
{
info->variance = variance;
info->inliers = inliers;
info->features = (int)newCorners.size();
info->matches = correspondences;
}
UINFO("Odom update time = %fs inliers=%d/%d, new corners=%d, transform accepted=%s",
timer.elapsed(),
inliers,
@@ -853,12 +857,12 @@ Transform OdometryOpticalFlow::computeTransformStereo(
Transform OdometryOpticalFlow::computeTransformRGBD(
const SensorData & data,
int * quality,
int * features)
OdometryInfo * info)
{
UTimer timer;
Transform output;
double variance = -1;
int inliers = 0;
int correspondences = 0;
@@ -967,15 +971,11 @@ Transform OdometryOpticalFlow::computeTransformRGBD(
this->getInlierDistance(),
this->getIterations(),
this->getRefineIterations()>0, 3.0, this->getRefineIterations(),
&inliersV);
&inliersV,
&variance);
UDEBUG("time RANSAC = %fs", timerRANSAC.ticks());
inliers = (int)inliersV.size();
if(quality)
{
*quality = inliers;
}
if(inliers < this->getMinInliers())
{
output.setNull();
@@ -1097,6 +1097,14 @@ Transform OdometryOpticalFlow::computeTransformRGBD(
output = Transform::getIdentity();
}
if(info)
{
info->variance = variance;
info->inliers = inliers;
info->features = (int)newCorners.size();
info->matches = correspondences;
}
UINFO("Odom update time = %fs inliers=%d/%d, new corners=%d, transform accepted=%s",
timer.elapsed(),
inliers,
@@ -1112,7 +1120,7 @@ OdometryICP::OdometryICP(int decimation,
int samples,
float maxCorrespondenceDistance,
int maxIterations,
float maxFitness,
float correspondenceRatio,
bool pointToPlane,
const ParametersMap & odometryParameter) :
Odometry(odometryParameter),
@@ -1121,7 +1129,7 @@ OdometryICP::OdometryICP(int decimation,
_samples(samples),
_maxCorrespondenceDistance(maxCorrespondenceDistance),
_maxIterations(maxIterations),
_maxFitness(maxFitness),
_correspondenceRatio(correspondenceRatio),
_pointToPlane(pointToPlane),
_previousCloudNormal(new pcl::PointCloud<pcl::PointNormal>),
_previousCloud(new pcl::PointCloud<pcl::PointXYZ>)
@@ -1136,13 +1144,13 @@ void OdometryICP::reset(const Transform & initialPose)
}
// return not null transform if odometry is correctly computed
Transform OdometryICP::computeTransform(const SensorData & data, int * quality, int * features, int * localMapSize)
Transform OdometryICP::computeTransform(const SensorData & data, OdometryInfo * info)
{
UTimer timer;
Transform output;
bool hasConverged = false;
double fitness = 0;
double variance = -1;
unsigned int minPoints = 100;
if(!data.depth().empty())
{
@@ -1177,27 +1185,28 @@ Transform OdometryICP::computeTransform(const SensorData & data, int * quality,
if(_previousCloudNormal->size() > minPoints && newCloud->size() > minPoints)
{
int correspondences = 0;
Transform transform = util3d::icpPointToPlane(newCloud,
_previousCloudNormal,
_maxCorrespondenceDistance,
_maxIterations,
hasConverged,
fitness);
&hasConverged,
&variance,
&correspondences);
//pcl::io::savePCDFile("old.pcd", *_previousCloud);
//pcl::io::savePCDFile("new.pcd", *newCloud);
//pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudTransformed = util3d::transformPointCloud(newCloud, transform);
//pcl::io::savePCDFile("newicp.pcd", *newCloudTransformed);
// verify if there are enough correspondences
float correspondencesRatio = float(correspondences)/float(_previousCloudNormal->size()>newCloud->size()?_previousCloudNormal->size():newCloud->size());
if(hasConverged && (_maxFitness == 0 || fitness < _maxFitness))
if(!transform.isNull() && hasConverged &&
correspondencesRatio >= _correspondenceRatio)
{
output = transform;
_previousCloudNormal = newCloud;
}
else
{
UWARN("Transform not valid (hasConverged=%s fitness = %f < %f)",
hasConverged?"true":"false", fitness, _maxFitness);
UWARN("Transform not valid (hasConverged=%s variance = %f)",
hasConverged?"true":"false", variance);
}
}
else if(newCloud->size() > minPoints)
@@ -1211,27 +1220,28 @@ Transform OdometryICP::computeTransform(const SensorData & data, int * quality,
//point to point
if(_previousCloud->size() > minPoints && newCloudXYZ->size() > minPoints)
{
int correspondences = 0;
Transform transform = util3d::icp(newCloudXYZ,
_previousCloud,
_maxCorrespondenceDistance,
_maxIterations,
hasConverged,
fitness);
&hasConverged,
&variance,
&correspondences);
//pcl::io::savePCDFile("old.pcd", *_previousCloudNormal);
//pcl::io::savePCDFile("new.pcd", *newCloud);
//pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudTransformed = util3d::transformPointCloud(newCloud, transform);
//pcl::io::savePCDFile("newicp.pcd", *newCloudTransformed);
// verify if there are enough correspondences
float correspondencesRatio = float(correspondences)/float(_previousCloud->size()>newCloudXYZ->size()?_previousCloud->size():newCloudXYZ->size());
if(hasConverged && (_maxFitness == 0 || fitness < _maxFitness))
if(!transform.isNull() && hasConverged &&
correspondencesRatio >= _correspondenceRatio)
{
output = transform;
_previousCloud = newCloudXYZ;
}
else
{
UWARN("Transform not valid (hasConverged=%s fitness = %f < %f)",
hasConverged?"true":"false", fitness, _maxFitness);
UWARN("Transform not valid (hasConverged=%s variance = %f)",
hasConverged?"true":"false", variance);
}
}
else if(newCloudXYZ->size() > minPoints)
@@ -1246,10 +1256,15 @@ Transform OdometryICP::computeTransform(const SensorData & data, int * quality,
UERROR("Depth is empty?!?");
}
UINFO("Odom update time = %fs hasConverged=%s fitness=%f cloud=%d",
if(info)
{
info->variance = variance;
}
UINFO("Odom update time = %fs hasConverged=%s variance=%f cloud=%d",
timer.elapsed(),
hasConverged?"true":"false",
fitness,
variance,
(int)(_pointToPlane?_previousCloudNormal->size():_previousCloud->size()));
return output;
@@ -1316,13 +1331,10 @@ void OdometryThread::mainLoop()
getData(data);
if(data.isValid())
{
int quality = -1;
int features = -1;
int localMapSize = -1;
UTimer time;
Transform pose = _odometry->process(data, &quality, &features, &localMapSize);
data.setPose(pose); // a null pose notify that odometry could not be computed
this->post(new OdometryEvent(data, quality, time.elapsed(), features, localMapSize));
OdometryInfo info;
Transform pose = _odometry->process(data, &info);
data.setPose(pose, info.variance); // a null pose notify that odometry could not be computed
this->post(new OdometryEvent(data, info));
}
}

98
corelib/src/Rtabmap.cpp
View File

@@ -98,6 +98,7 @@ Rtabmap::Rtabmap() :
_localDetectMaxNeighbors(Parameters::defaultRGBDLocalLoopDetectionNeighbors()),
_localDetectMaxDiffID(Parameters::defaultRGBDLocalLoopDetectionMaxDiffID()),
_toroIterations(Parameters::defaultRGBDToroIterations()),
_toroIgnoreVariance(Parameters::defaultRGBDToroIgnoreVariance()),
_databasePath(""),
_optimizeFromGraphEnd(Parameters::defaultRGBDOptimizeFromGraphEnd()),
_reextractLoopClosureFeatures(Parameters::defaultLccReextractActivated()),
@@ -358,6 +359,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionNeighbors(), _localDetectMaxNeighbors);
Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionMaxDiffID(), _localDetectMaxDiffID);
Parameters::parse(parameters, Parameters::kRGBDToroIterations(), _toroIterations);
Parameters::parse(parameters, Parameters::kRGBDToroIgnoreVariance(), _toroIgnoreVariance);
Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), _optimizeFromGraphEnd);
Parameters::parse(parameters, Parameters::kLccReextractActivated(), _reextractLoopClosureFeatures);
Parameters::parse(parameters, Parameters::kLccReextractNNType(), _reextractNNType);
@@ -831,11 +833,11 @@ bool Rtabmap::process(const SensorData & data)
//============================================================
// Minimum displacement required to add to Memory
//============================================================
const std::map<int, Transform> & neighbors = signature->getNeighbors();
if(neighbors.size() == 1)
const std::map<int, Link> & links = signature->getLinks();
if(links.size() == 1)
{
float x,y,z, roll,pitch,yaw;
neighbors.begin()->second.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
links.begin()->second.transform().getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
if(fabs(x) < _rgbdLinearUpdate &&
fabs(y) < _rgbdLinearUpdate &&
fabs(z) < _rgbdLinearUpdate &&
@@ -857,26 +859,27 @@ bool Rtabmap::process(const SensorData & data)
// Scan matching
//============================================================
if(_poseScanMatching &&
signature->getNeighbors().size() == 1 &&
!signature->getDepth2DCompressed().empty() &&
signature->getLinks().size() == 1 &&
!signature->getLaserScanCompressed().empty() &&
rehearsedId == 0) // don't do it if rehearsal happened
{
UINFO("Odometry correction by scan matching");
int oldId = signature->getNeighbors().begin()->first;
int oldId = signature->getLinks().begin()->first;
const Signature * oldS = _memory->getSignature(oldId);
UASSERT(oldS != 0);
std::string rejectedMsg;
Transform guess = signature->getNeighbors().begin()->second;
Transform t = _memory->computeIcpTransform(oldId, signature->id(), guess, false, &rejectedMsg);
Transform guess = signature->getLinks().begin()->second.transform();
double variance = -1.0;
Transform t = _memory->computeIcpTransform(oldId, signature->id(), guess, false, &rejectedMsg, 0, &variance);
if(!t.isNull())
{
scanMatchingSuccess = true;
UINFO("Scan matching: update neighbor link (%d->%d) from %s to %s",
signature->id(),
oldId,
signature->getNeighbors().at(oldId).prettyPrint().c_str(),
signature->getLinks().at(oldId).transform().prettyPrint().c_str(),
t.prettyPrint().c_str());
_memory->updateNeighborLink(signature->id(), oldId, t);
_memory->updateNeighborLink(signature->id(), oldId, t, variance);
}
else
{
@@ -886,9 +889,9 @@ bool Rtabmap::process(const SensorData & data)
timeScanMatching = timer.ticks();
ULOGGER_INFO("timeScanMatching=%fs", timeScanMatching);
if(signature->getNeighbors().size() == 1)
if(signature->getLinks().size() == 1)
{
_constraints.insert(std::make_pair(signature->id(), Link(signature->id(), signature->getNeighbors().begin()->first, signature->getNeighbors().begin()->second, Link::kNeighbor)));
_constraints.insert(std::make_pair(signature->id(), signature->getLinks().begin()->second));
}
//============================================================
@@ -902,15 +905,17 @@ bool Rtabmap::process(const SensorData & data)
for(std::set<int>::const_reverse_iterator iter = stm.rbegin(); iter!=stm.rend(); ++iter)
{
if(*iter != signature->id() &&
signature->getNeighbors().find(*iter) == signature->getNeighbors().end() &&
signature->getLinks().find(*iter) == signature->getLinks().end() &&
_memory->getSignature(*iter)->mapId() == signature->mapId())
{
std::string rejectedMsg;
UDEBUG("Check local transform between %d and %d", signature->id(), *iter);
Transform transform = _memory->computeVisualTransform(*iter, signature->id(), &rejectedMsg);
double variance = -1.0;
int inliers = -1;
Transform transform = _memory->computeVisualTransform(*iter, signature->id(), &rejectedMsg, &inliers, &variance);
if(!transform.isNull() && _globalLoopClosureIcpType > 0)
{
Transform icpTransform = _memory->computeIcpTransform(*iter, signature->id(), transform, _globalLoopClosureIcpType==1, &rejectedMsg);
Transform icpTransform = _memory->computeIcpTransform(*iter, signature->id(), transform, _globalLoopClosureIcpType==1, &rejectedMsg, 0, &variance);
float squaredNorm = (transform.inverse()*icpTransform).getNormSquared();
if(!icpTransform.isNull() &&
_globalLoopClosureIcpMaxDistance>0.0f &&
@@ -932,7 +937,7 @@ bool Rtabmap::process(const SensorData & data)
*iter,
transform.prettyPrint().c_str());
// Add a loop constraint
if(_memory->addLoopClosureLink(*iter, signature->id(), transform, false))
if(_memory->addLoopClosureLink(*iter, signature->id(), transform, Link::kLocalTimeClosure, variance))
{
++localLoopClosuresInTimeFound;
UINFO("Local loop closure found between %d and %d with t=%s",
@@ -1251,6 +1256,7 @@ bool Rtabmap::process(const SensorData & data)
{
//Compute transform if metric data are present
Transform transform;
double variance = -1;
if(_rgbdSlamMode)
{
std::string rejectedMsg;
@@ -1298,7 +1304,7 @@ bool Rtabmap::process(const SensorData & data)
memory.update(dataFrom);
UDEBUG("timeUpFrom = %fs", timeT.ticks());
transform = memory.computeVisualTransform(dataTo.id(), dataFrom.id(), &rejectedMsg, &loopClosureVisualInliers);
transform = memory.computeVisualTransform(dataTo.id(), dataFrom.id(), &rejectedMsg, &loopClosureVisualInliers, &variance);
UDEBUG("timeTransform = %fs", timeT.ticks());
}
else
@@ -1306,16 +1312,16 @@ bool Rtabmap::process(const SensorData & data)
// 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);
transform = _memory->computeVisualTransform(_lcHypothesisId, signature->id(), &rejectedMsg, &loopClosureVisualInliers, &variance);
}
}
else
{
transform = _memory->computeVisualTransform(_lcHypothesisId, signature->id(), &rejectedMsg, &loopClosureVisualInliers);
transform = _memory->computeVisualTransform(_lcHypothesisId, signature->id(), &rejectedMsg, &loopClosureVisualInliers, &variance);
}
if(!transform.isNull() && _globalLoopClosureIcpType > 0)
{
Transform icpTransform = _memory->computeIcpTransform(_lcHypothesisId, signature->id(), transform, _globalLoopClosureIcpType == 1, &rejectedMsg);
Transform icpTransform = _memory->computeIcpTransform(_lcHypothesisId, signature->id(), transform, _globalLoopClosureIcpType == 1, &rejectedMsg, 0, &variance);
float squaredNorm = (transform.inverse()*icpTransform).getNormSquared();
if(!icpTransform.isNull() &&
_globalLoopClosureIcpMaxDistance>0.0f &&
@@ -1339,7 +1345,7 @@ bool Rtabmap::process(const SensorData & data)
if(!rejectedHypothesis)
{
// Make the new one the parent of the old one
rejectedHypothesis = !_memory->addLoopClosureLink(_lcHypothesisId, signature->id(), transform, true);
rejectedHypothesis = !_memory->addLoopClosureLink(_lcHypothesisId, signature->id(), transform, Link::kGlobalClosure, variance);
}
if(rejectedHypothesis)
@@ -1362,7 +1368,7 @@ bool Rtabmap::process(const SensorData & data)
int localSpaceNearestId = 0;
if(_lcHypothesisId == 0 &&
_localLoopClosureDetectionSpace &&
!signature->getDepth2DCompressed().empty())
!signature->getLaserScanCompressed().empty())
{
if(_toroIterations == 0)
{
@@ -1386,10 +1392,11 @@ bool Rtabmap::process(const SensorData & data)
//The nearest will be the reference for a loop closure transform
if(poses.size() &&
localSpaceNearestId &&
signature->getChildLoopClosureIds().find(localSpaceNearestId) == signature->getChildLoopClosureIds().end())
signature->getLinks().find(localSpaceNearestId) == signature->getLinks().end())
{
double variance = 1.0;
std::string rejectedMsg;
Transform t = _memory->computeScanMatchingTransform(signature->id(), localSpaceNearestId, poses, &rejectedMsg);
Transform t = _memory->computeScanMatchingTransform(signature->id(), localSpaceNearestId, poses, &rejectedMsg, 0, &variance);
if(!t.isNull())
{
localSpaceClosureId = localSpaceNearestId;
@@ -1397,7 +1404,7 @@ bool Rtabmap::process(const SensorData & data)
signature->id(),
localSpaceNearestId,
t.prettyPrint().c_str());
_memory->addLoopClosureLink(localSpaceNearestId, signature->id(), t, false);
_memory->addLoopClosureLink(localSpaceNearestId, signature->id(), t, Link::kLocalSpaceClosure, variance);
// Old map -> new map, used for localization correction on loop closure
const Signature * oldS = _memory->getSignature(localSpaceNearestId);
@@ -1531,9 +1538,9 @@ bool Rtabmap::process(const SensorData & data)
}
if(_lcHypothesisId || localSpaceClosureId)
{
UASSERT(uContains(sLoop->getLoopClosureIds(), signature->id()));
UINFO("Set loop closure transform = %s", sLoop->getLoopClosureIds().at(signature->id()).prettyPrint().c_str());
statistics_.setLoopClosureTransform(sLoop->getLoopClosureIds().at(signature->id()));
UASSERT(uContains(sLoop->getLinks(), signature->id()));
UINFO("Set loop closure transform = %s", sLoop->getLinks().at(signature->id()).transform().prettyPrint().c_str());
statistics_.setLoopClosureTransform(sLoop->getLinks().at(signature->id()).transform());
}
if(!_rgbdSlamMode)
@@ -1602,10 +1609,9 @@ bool Rtabmap::process(const SensorData & data)
// global loop closure detection before starting the new map,
// otherwise it deletes the current node.
if(_startNewMapOnLoopClosure &&
_memory->isIncremental() && // only in mapping mode
signature->getChildLoopClosureIds().size() == 0 && // no loop closure
signature->getNeighbors().size() == 0 && // no neighbors, alone in the current map
_memory->getWorkingMem().size()>1) // The working memory should not be empty
_memory->isIncremental() && // only in mapping mode
signature->getLinks().size() == 0 && // alone in the current map
_memory->getWorkingMem().size()>1) // The working memory should not be empty
{
_memory->deleteLocation(signature->id());
}
@@ -1751,9 +1757,9 @@ bool Rtabmap::process(const SensorData & data)
return true;
}
bool Rtabmap::process(const cv::Mat & sensorData, int id)
bool Rtabmap::process(const cv::Mat & image, int id)
{
return this->process(SensorData(sensorData, id));
return this->process(SensorData(image, id));
}
// SETTERS
@@ -1908,7 +1914,7 @@ std::map<int, Transform> Rtabmap::getOptimizedWMPosesInRadius(
//inliers.push_back(pcl::PointXYZ(tmp.x(), tmp.y(), tmp.z()));
UDEBUG("Inlier %d: %s", ids[ind[i]], tmp.prettyPrint().c_str());
poses.insert(std::make_pair(ids[ind[i]], tmp));
if(fromS->getNeighbors().find(ids[ind[i]]) == fromS->getNeighbors().end() && // can't be a neighbor
if(fromS->getLinks().find(ids[ind[i]]) == fromS->getLinks().end() && // can't be a neighbor
(minDistance == -1 || minDistance > dist[i]))
{
nearestId = ids[ind[i]];
@@ -2001,7 +2007,7 @@ void Rtabmap::optimizeCurrentMap(
}
else
{
util3d::optimizeTOROGraph(ids, poses, edgeConstraints, optimizedPoses, _toroIterations, true);
util3d::optimizeTOROGraph(ids, poses, edgeConstraints, optimizedPoses, _toroIterations, true, _toroIgnoreVariance);
}
}
}
@@ -2030,7 +2036,7 @@ void Rtabmap::adjustLikelihood(std::map<int, float> & likelihood) const
UDEBUG("values.size=%d", values.size());
float mean = uMean(values);
float stdDev = uStdDev(values, mean);
float stdDev = std::sqrt(uVariance(values, mean));
//Adjust likelihood with mean and standard deviation (see Angeli phd)
@@ -2132,8 +2138,15 @@ void Rtabmap::get3DMap(std::map<int, Signature> & signatures,
std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true);
_memory->getMetricConstraints(uKeys(ids), poses, constraints, global);
}
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
mapIds.insert(std::make_pair(iter->first, _memory->getMapId(iter->first)));
}
}
// Get data
std::set<int> ids = _memory->getWorkingMem(); // STM + WM
//remove virtual signature
@@ -2151,7 +2164,6 @@ void Rtabmap::get3DMap(std::map<int, Signature> & signatures,
if(data.id() != Memory::kIdInvalid)
{
signatures.insert(std::make_pair(*iter, Signature())).first->second = data;
mapIds.insert(std::make_pair(*iter, _memory->getMapId(*iter)));
}
}
}
@@ -2185,6 +2197,11 @@ void Rtabmap::getGraph(
std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true);
_memory->getMetricConstraints(uKeys(ids), poses, constraints, global);
}
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
mapIds.insert(std::make_pair(iter->first, _memory->getMapId(iter->first)));
}
}
else
{
@@ -2197,11 +2214,6 @@ void Rtabmap::getGraph(
{
ids = _memory->getAllSignatureIds(); // STM + WM + LTM
}
for(std::set<int>::iterator iter = ids.begin(); iter!=ids.end(); ++iter)
{
mapIds.insert(std::make_pair(*iter, _memory->getMapId(*iter)));
}
}
else if(_memory && (_memory->getStMem().size() || _memory->getWorkingMem().size()))
{

25
corelib/src/SensorData.cpp
View File

@@ -42,7 +42,8 @@ SensorData::SensorData() :
_fyOrBaseline(0.0f),
_cx(0.0f),
_cy(0.0f),
_localTransform(Transform::getIdentity())
_localTransform(Transform::getIdentity()),
_poseVariance(1.0f)
{
}
@@ -54,7 +55,8 @@ SensorData::SensorData(const cv::Mat & image,
_fyOrBaseline(0.0f),
_cx(0.0f),
_cy(0.0f),
_localTransform(Transform::getIdentity())
_localTransform(Transform::getIdentity()),
_poseVariance(1.0f)
{
UASSERT(image.type() == CV_8UC1 || // Mono
image.type() == CV_8UC3); // RGB
@@ -67,8 +69,9 @@ SensorData::SensorData(const cv::Mat & image,
float fyOrBaseline,
float cx,
float cy,
const Transform & pose,
const Transform & localTransform,
const Transform & pose,
float poseVariance,
int id) :
_image(image),
_id(id),
@@ -78,7 +81,8 @@ SensorData::SensorData(const cv::Mat & image,
_cx(cx),
_cy(cy),
_pose(pose),
_localTransform(localTransform)
_localTransform(localTransform),
_poseVariance(poseVariance)
{
UASSERT(image.type() == CV_8UC1 || // Mono
image.type() == CV_8UC3); // RGB
@@ -90,27 +94,30 @@ SensorData::SensorData(const cv::Mat & image,
}
// Metric constructor + 2d depth
SensorData::SensorData(const cv::Mat & image,
SensorData::SensorData(const cv::Mat & laserScan,
const cv::Mat & image,
const cv::Mat & depthOrRightImage,
const cv::Mat & depth2d,
float fx,
float fyOrBaseline,
float cx,
float cy,
const Transform & pose,
const Transform & localTransform,
const Transform & pose,
float poseVariance,
int id) :
_image(image),
_id(id),
_depthOrRightImage(depthOrRightImage),
_depth2d(depth2d),
_laserScan(laserScan),
_fx(fx),
_fyOrBaseline(fyOrBaseline),
_cx(cx),
_cy(cy),
_pose(pose),
_localTransform(localTransform)
_localTransform(localTransform),
_poseVariance(poseVariance)
{
UASSERT(_laserScan.empty() || _laserScan.type() == CV_32FC2);
UASSERT(image.type() == CV_8UC1 || // Mono
image.type() == CV_8UC3); // RGB
UASSERT(depthOrRightImage.type() == CV_32FC1 || // Depth in meter

165
corelib/src/Signature.cpp
View File

@@ -42,7 +42,7 @@ Signature::Signature() :
_weight(-1),
_saved(false),
_modified(true),
_neighborsModified(true),
_linksModified(true),
_enabled(false),
_fx(0.0f),
_fy(0.0f),
@@ -57,7 +57,7 @@ Signature::Signature(
const std::multimap<int, cv::KeyPoint> & words,
const std::multimap<int, pcl::PointXYZ> & words3, // in base_link frame (localTransform applied)
const Transform & pose,
const cv::Mat & depth2DCompressed, // in base_link frame
const cv::Mat & laserScanCompressed, // in base_link frame
const cv::Mat & imageCompressed, // in camera_link frame
const cv::Mat & depthCompressed, // in camera_link frame
float fx,
@@ -70,12 +70,12 @@ Signature::Signature(
_weight(0),
_saved(false),
_modified(true),
_neighborsModified(true),
_linksModified(true),
_words(words),
_enabled(false),
_imageCompressed(imageCompressed),
_depthCompressed(depthCompressed),
_depth2DCompressed(depth2DCompressed),
_laserScanCompressed(laserScanCompressed),
_fx(fx),
_fy(fy),
_cx(cx),
@@ -91,80 +91,64 @@ Signature::~Signature()
//UDEBUG("id=%d", _id);
}
void Signature::addNeighbors(const std::map<int, Transform> & neighbors)
void Signature::addLinks(const std::list<Link> & links)
{
for(std::map<int, Transform>::const_iterator i=neighbors.begin(); i!=neighbors.end(); ++i)
for(std::list<Link>::const_iterator iter = links.begin(); iter!=links.end(); ++iter)
{
this->addNeighbor(i->first, i->second);
addLink(*iter);
}
}
void Signature::addLinks(const std::map<int, Link> & links)
{
for(std::map<int, Link>::const_iterator iter = links.begin(); iter!=links.end(); ++iter)
{
addLink(iter->second);
}
}
void Signature::addLink(const Link & link)
{
UDEBUG("Add link %d to %d (type=%d)", link.to(), this->id(), (int)link.type());
UASSERT(link.from() == this->id());
std::pair<std::map<int, Link>::iterator, bool> pair = _links.insert(std::make_pair(link.to(), link));
UASSERT_MSG(pair.second, uFormat("Link %d (type=%d) already added to signature %d!", link.to(), link.type(), this->id()).c_str());
_linksModified = true;
}
bool Signature::hasLink(int idTo) const
{
return _links.find(idTo) != _links.end();
}
void Signature::changeLinkIds(int idFrom, int idTo)
{
std::map<int, Link>::iterator iter = _links.find(idFrom);
if(iter != _links.end())
{
Link link = iter->second;
_links.erase(iter);
link.setTo(idTo);
_links.insert(std::make_pair(idTo, link));
_linksModified = true;
UDEBUG("(%d) neighbor ids changed from %d to %d", _id, idFrom, idTo);
}
}
void Signature::addNeighbor(int neighbor, const Transform & transform)
void Signature::removeLinks()
{
UDEBUG("Add neighbor %d to %d", neighbor, this->id());
_neighbors.insert(std::pair<int, Transform>(neighbor, transform));
_neighborsModified = true;
if(_links.size())
_linksModified = true;
_links.clear();
}
void Signature::removeNeighbor(int neighborId)
void Signature::removeLink(int idTo)
{
int count = (int)_neighbors.erase(neighborId);
int count = (int)_links.erase(idTo);
if(count)
{
_neighborsModified = true;
_linksModified = true;
}
}
void Signature::removeNeighbors()
{
if(_neighbors.size())
_neighborsModified = true;
_neighbors.clear();
}
void Signature::changeNeighborIds(int idFrom, int idTo)
{
std::map<int, Transform>::iterator iter = _neighbors.find(idFrom);
if(iter != _neighbors.end())
{
Transform t = iter->second;
_neighbors.erase(iter);
_neighbors.insert(std::pair<int, Transform>(idTo, t));
_neighborsModified = true;
}
UDEBUG("(%d) neighbor ids changed from %d to %d", _id, idFrom, idTo);
}
void Signature::addLoopClosureId(int loopClosureId, const Transform & transform)
{
if(loopClosureId && _loopClosureIds.insert(std::pair<int, Transform>(loopClosureId, transform)).second)
{
_neighborsModified=true;
}
}
void Signature::addChildLoopClosureId(int childLoopClosureId, const Transform & transform)
{
if(childLoopClosureId && _childLoopClosureIds.insert(std::pair<int, Transform>(childLoopClosureId, transform)).second)
{
_neighborsModified=true;
}
}
void Signature::changeLoopClosureId(int idFrom, int idTo)
{
std::map<int, Transform>::iterator iter = _loopClosureIds.find(idFrom);
if(iter != _loopClosureIds.end())
{
Transform t = iter->second;
_loopClosureIds.erase(iter);
_loopClosureIds.insert(std::pair<int, Transform>(idTo, t));
_neighborsModified = true;
}
UDEBUG("(%d) loop closure ids changed from %d to %d", _id, idFrom, idTo);
}
float Signature::compareTo(const Signature & s) const
{
float similarity = 0.0f;
@@ -230,26 +214,43 @@ void Signature::setDepthCompressed(const cv::Mat & bytes, float fx, float fy, fl
SensorData Signature::toSensorData()
{
this->uncompressData();
return SensorData(_imageRaw,
float variance = 1.0f;
if(_links.size())
{
for(std::map<int, Link>::iterator iter = _links.begin(); iter!=_links.end(); ++iter)
{
if(iter->second.kNeighbor)
{
//Assume the first neighbor to be the backward neighbor link
if(iter->second.to() < iter->second.from())
{
variance = iter->second.variance();
break;
}
}
}
}
return SensorData(_laserScanRaw,
_imageRaw,
_depthRaw,
_depth2DRaw,
_fx,
_fy,
_cx,
_cy,
_pose,
_localTransform,
_pose,
variance,
_id);
}
void Signature::uncompressData()
{
uncompressData(&_imageRaw, &_depthRaw, &_depth2DRaw);
uncompressData(&_imageRaw, &_depthRaw, &_laserScanRaw);
}
void Signature::uncompressData(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * depth2DRaw)
void Signature::uncompressData(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * laserScanRaw)
{
uncompressDataConst(imageRaw, depthRaw, depth2DRaw);
uncompressDataConst(imageRaw, depthRaw, laserScanRaw);
if(imageRaw && !imageRaw->empty() && _imageRaw.empty())
{
_imageRaw = *imageRaw;
@@ -258,13 +259,13 @@ void Signature::uncompressData(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat *
{
_depthRaw = *depthRaw;
}
if(depth2DRaw && !depth2DRaw->empty() && _depth2DRaw.empty())
if(laserScanRaw && !laserScanRaw->empty() && _laserScanRaw.empty())
{
_depth2DRaw = *depth2DRaw;
_laserScanRaw = *laserScanRaw;
}
}
void Signature::uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * depth2DRaw) const
void Signature::uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * laserScanRaw) const
{
if(imageRaw)
{
@@ -274,17 +275,17 @@ void Signature::uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::
{
*depthRaw = _depthRaw;
}
if(depth2DRaw)
if(laserScanRaw)
{
*depth2DRaw = _depth2DRaw;
*laserScanRaw = _laserScanRaw;
}
if( (imageRaw && imageRaw->empty()) ||
(depthRaw && depthRaw->empty()) ||
(depth2DRaw && depth2DRaw->empty()))
(laserScanRaw && laserScanRaw->empty()))
{
util3d::CompressionThread ctImage(_imageCompressed, true);
util3d::CompressionThread ctDepth(_depthCompressed, true);
util3d::CompressionThread ctDepth2D(_depth2DCompressed, false);
util3d::CompressionThread ctLaserScan(_laserScanCompressed, false);
if(imageRaw && imageRaw->empty())
{
ctImage.start();
@@ -293,13 +294,13 @@ void Signature::uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::
{
ctDepth.start();
}
if(depth2DRaw && depth2DRaw->empty())
if(laserScanRaw && laserScanRaw->empty())
{
ctDepth2D.start();
ctLaserScan.start();
}
ctImage.join();
ctDepth.join();
ctDepth2D.join();
ctLaserScan.join();
if(imageRaw && imageRaw->empty())
{
*imageRaw = ctImage.getUncompressedData();
@@ -308,9 +309,9 @@ void Signature::uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::
{
*depthRaw = ctDepth.getUncompressedData();
}
if(depth2DRaw && depth2DRaw->empty())
if(laserScanRaw && laserScanRaw->empty())
{
*depth2DRaw = ctDepth2D.getUncompressedData();
*laserScanRaw = ctLaserScan.getUncompressedData();
}
}
}

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

@@ -46,6 +46,7 @@ CREATE TABLE Link (
from_id INTEGER NOT NULL,
to_id INTEGER NOT NULL,
type INTEGER NOT NULL, -- neighbor=0, loop=1, child=2
variance FLOAT NOT NULL,
transform BLOB,
FOREIGN KEY (from_id) REFERENCES Node(id),
FOREIGN KEY (to_id) REFERENCES Node(id)

275
corelib/src/util3d.cpp
View File

@@ -1070,27 +1070,27 @@ cv::Mat depthFromDisparity(const cv::Mat & disparity,
return depth;
}
cv::Mat depth2DFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud)
cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud)
{
cv::Mat depth2d(1, (int)cloud.size(), CV_32FC2);
cv::Mat laserScan(1, (int)cloud.size(), CV_32FC2);
for(unsigned int i=0; i<cloud.size(); ++i)
{
depth2d.at<cv::Vec2f>(i)[0] = cloud.at(i).x;
depth2d.at<cv::Vec2f>(i)[1] = cloud.at(i).y;
laserScan.at<cv::Vec2f>(i)[0] = cloud.at(i).x;
laserScan.at<cv::Vec2f>(i)[1] = cloud.at(i).y;
}
return depth2d;
return laserScan;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr depth2DToPointCloud(const cv::Mat & depth2D)
pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const cv::Mat & laserScan)
{
UASSERT(depth2D.empty() || depth2D.type() == CV_32FC2);
UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2);
pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
output->resize(depth2D.cols);
for(int i=0; i<depth2D.cols; ++i)
output->resize(laserScan.cols);
for(int i=0; i<laserScan.cols; ++i)
{
output->at(i).x = depth2D.at<cv::Vec2f>(i)[0];
output->at(i).y = depth2D.at<cv::Vec2f>(i)[1];
output->at(i).x = laserScan.at<cv::Vec2f>(i)[0];
output->at(i).y = laserScan.at<cv::Vec2f>(i)[1];
}
return output;
}
@@ -1495,12 +1495,17 @@ Transform transformFromXYZCorrespondences(
bool refineModel,
double refineModelSigma,
int refineModelIterations,
std::vector<int> * inliersOut)
std::vector<int> * inliersOut,
double * varianceOut)
{
//NOTE: this method is a mix of two methods:
// - getRemainingCorrespondences() in pcl/registration/impl/correspondence_rejection_sample_consensus.hpp
// - refineModel() in pcl/sample_consensus/sac.h
if(varianceOut)
{
*varianceOut = 1.0f;
}
Transform transform;
if(cloud1->size() >=3 && cloud1->size() == cloud2->size())
{
@@ -1626,6 +1631,10 @@ Transform transformFromXYZCorrespondences(
{
*inliersOut = inliers;
}
if(varianceOut)
{
*varianceOut = model->computeVariance();
}
// get best transformation
Eigen::Matrix4f bestTransformation;
@@ -1661,8 +1670,9 @@ Transform icp(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_target,
double maxCorrespondenceDistance,
int maximumIterations,
bool & hasConverged,
double & fitnessScore)
bool * hasConvergedOut,
double * variance,
int * inliers)
{
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
// Set the input source and target
@@ -1677,13 +1687,65 @@ Transform icp(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
//icp.setTransformationEpsilon (transformationEpsilon);
// Set the euclidean distance difference epsilon (criterion 3)
//icp.setEuclideanFitnessEpsilon (1);
icp.setRANSACOutlierRejectionThreshold(maxCorrespondenceDistance);
//icp.setRANSACOutlierRejectionThreshold(maxCorrespondenceDistance);
// Perform the alignment
pcl::PointCloud<pcl::PointXYZ> cloud_source_registered;
icp.align (cloud_source_registered);
fitnessScore = icp.getFitnessScore();
hasConverged = icp.hasConverged();
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_source_registered(new pcl::PointCloud<pcl::PointXYZ>);
icp.align (*cloud_source_registered);
bool hasConverged = icp.hasConverged();
// compute variance
if((inliers || variance) && hasConverged)
{
pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>::Ptr est;
est.reset(new pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>);
est->setInputTarget(cloud_target);
est->setInputSource(cloud_source_registered);
pcl::Correspondences correspondences;
est->determineCorrespondences(correspondences, maxCorrespondenceDistance);
if(variance)
{
if(correspondences.size()>=3)
{
std::vector<double> distances(correspondences.size());
for(unsigned int i=0; i<correspondences.size(); ++i)
{
distances[i] = correspondences[i].distance;
}
//variance
std::sort(distances.begin (), distances.end ());
double median_error_sqr = distances[distances.size () >> 1];
*variance = (2.1981 * median_error_sqr);
}
else
{
hasConverged = false;
*variance = -1.0;
}
}
if(inliers)
{
*inliers = correspondences.size();
}
}
else
{
if(inliers)
{
*inliers = 0;
}
if(variance)
{
*variance = -1;
}
}
if(hasConvergedOut)
{
*hasConvergedOut = hasConverged;
}
return transformFromEigen4f(icp.getFinalTransformation());
}
@@ -1694,8 +1756,9 @@ Transform icpPointToPlane(
const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloud_target,
double maxCorrespondenceDistance,
int maximumIterations,
bool & hasConverged,
double & fitnessScore)
bool * hasConvergedOut,
double * variance,
int * inliers)
{
pcl::IterativeClosestPoint<pcl::PointNormal, pcl::PointNormal> icp;
// Set the input source and target
@@ -1714,13 +1777,65 @@ Transform icpPointToPlane(
//icp.setTransformationEpsilon (transformationEpsilon);
// Set the euclidean distance difference epsilon (criterion 3)
//icp.setEuclideanFitnessEpsilon (1);
icp.setRANSACOutlierRejectionThreshold(maxCorrespondenceDistance);
//icp.setRANSACOutlierRejectionThreshold(maxCorrespondenceDistance);
// Perform the alignment
pcl::PointCloud<pcl::PointNormal> cloud_source_registered;
icp.align (cloud_source_registered);
fitnessScore = icp.getFitnessScore();
hasConverged = icp.hasConverged();
pcl::PointCloud<pcl::PointNormal>::Ptr cloud_source_registered(new pcl::PointCloud<pcl::PointNormal>);
icp.align (*cloud_source_registered);
bool hasConverged = icp.hasConverged();
// compute variance
if((inliers || variance) && hasConverged)
{
pcl::registration::CorrespondenceEstimation<pcl::PointNormal, pcl::PointNormal>::Ptr est;
est.reset(new pcl::registration::CorrespondenceEstimation<pcl::PointNormal, pcl::PointNormal>);
est->setInputTarget(cloud_target);
est->setInputSource(cloud_source_registered);
pcl::Correspondences correspondences;
est->determineCorrespondences(correspondences, maxCorrespondenceDistance);
if(variance)
{
if(correspondences.size()>=3)
{
std::vector<double> distances(correspondences.size());
for(unsigned int i=0; i<correspondences.size(); ++i)
{
distances[i] = correspondences[i].distance;
}
//variance
std::sort(distances.begin (), distances.end ());
double median_error_sqr = distances[distances.size () >> 1];
*variance = (2.1981 * median_error_sqr);
}
else
{
hasConverged = false;
*variance = -1.0;
}
}
if(inliers)
{
*inliers = correspondences.size();
}
}
else
{
if(inliers)
{
*inliers = 0;
}
if(variance)
{
*variance = -1;
}
}
if(hasConvergedOut)
{
*hasConvergedOut = hasConverged;
}
return transformFromEigen4f(icp.getFinalTransformation());
}
@@ -1730,8 +1845,9 @@ Transform icp2D(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_target,
double maxCorrespondenceDistance,
int maximumIterations,
bool & hasConverged,
double & fitnessScore)
bool * hasConvergedOut,
double * variance,
int * inliers)
{
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
// Set the input source and target
@@ -1750,13 +1866,65 @@ Transform icp2D(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
//icp.setTransformationEpsilon (transformationEpsilon);
// Set the euclidean distance difference epsilon (criterion 3)
//icp.setEuclideanFitnessEpsilon (1);
icp.setRANSACOutlierRejectionThreshold(maxCorrespondenceDistance);
//icp.setRANSACOutlierRejectionThreshold(maxCorrespondenceDistance);
// Perform the alignment
pcl::PointCloud<pcl::PointXYZ> cloud_source_registered;
icp.align (cloud_source_registered);
fitnessScore = icp.getFitnessScore();
hasConverged = icp.hasConverged();
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_source_registered(new pcl::PointCloud<pcl::PointXYZ>);
icp.align (*cloud_source_registered);
bool hasConverged = icp.hasConverged();
// compute variance
if((inliers || variance) && hasConverged)
{
pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>::Ptr est;
est.reset(new pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>);
est->setInputTarget(cloud_target);
est->setInputSource(cloud_source_registered);
pcl::Correspondences correspondences;
est->determineCorrespondences(correspondences, maxCorrespondenceDistance);
if(variance)
{
if(correspondences.size()>=3)
{
std::vector<double> distances(correspondences.size());
for(unsigned int i=0; i<correspondences.size(); ++i)
{
distances[i] = correspondences[i].distance;
}
//variance
std::sort(distances.begin (), distances.end ());
double median_error_sqr = distances[distances.size () >> 1];
*variance = (2.1981 * median_error_sqr);
}
else
{
hasConverged = false;
*variance = -1.0;
}
}
if(inliers)
{
*inliers = correspondences.size();
}
}
else
{
if(inliers)
{
*inliers = 0;
}
if(variance)
{
*variance = -1;
}
}
if(hasConvergedOut)
{
*hasConvergedOut = hasConverged;
}
return transformFromEigen4f(icp.getFinalTransformation());
}
@@ -2145,6 +2313,7 @@ void optimizeTOROGraph(
std::map<int, Transform> & optimizedPoses,
int toroIterations,
bool toroInitialGuess,
bool ignoreCovariance,
std::list<std::map<int, Transform> > * intermediateGraphes)
{
optimizedPoses.clear();
@@ -2190,17 +2359,26 @@ void optimizeTOROGraph(
{
if(uContains(depthGraph, iter->second.from()) && uContains(depthGraph, iter->second.to()))
{
edgeConstraintsToro.insert(std::make_pair(rtabmapToToro.at(iter->first), rtabmap::Link(rtabmapToToro.at(iter->first), rtabmapToToro.at(iter->second.to()), iter->second.transform(), iter->second.type())));
edgeConstraintsToro.insert(std::make_pair(rtabmapToToro.at(iter->first), Link(rtabmapToToro.at(iter->first), rtabmapToToro.at(iter->second.to()), iter->second.type(), iter->second.transform(), iter->second.variance())));
}
}
std::map<int, rtabmap::Transform> optimizedPosesToro;
// Optimize!
if(posesToro.size() && edgeConstraintsToro.size())
{
std::list<std::map<int, rtabmap::Transform> > graphesToro;
rtabmap::util3d::optimizeTOROGraph(posesToro, edgeConstraintsToro, optimizedPosesToro, toroIterations, toroInitialGuess, &graphesToro);
// Optimize!
rtabmap::util3d::optimizeTOROGraph(
posesToro,
edgeConstraintsToro,
optimizedPosesToro,
toroIterations,
toroInitialGuess,
ignoreCovariance,
&graphesToro);
for(std::map<int, rtabmap::Transform>::iterator iter=optimizedPosesToro.begin(); iter!=optimizedPosesToro.end(); ++iter)
{
optimizedPoses.insert(std::make_pair(toroToRtabmap.at(iter->first), iter->second));
@@ -2242,6 +2420,7 @@ void optimizeTOROGraph(
std::map<int, Transform> & optimizedPoses,
int toroIterations,
bool toroInitialGuess,
bool ignoreCovariance,
std::list<std::map<int, Transform> > * intermediateGraphes) // contains poses after tree init to last one before the end
{
UASSERT(toroIterations>0);
@@ -2274,13 +2453,21 @@ void optimizeTOROGraph(
float x,y,z, roll,pitch,yaw;
pcl::getTranslationAndEulerAngles(transformToEigen3f(iter->second.transform()), x,y,z, roll,pitch,yaw);
AISNavigation::TreePoseGraph3::Pose p(x, y, z, roll, pitch, yaw);
AISNavigation::TreePoseGraph3::InformationMatrix m;
m=DMatrix<double>::I(6);
AISNavigation::TreePoseGraph3::InformationMatrix inf = DMatrix<double>::I(6);
if(!ignoreCovariance && iter->second.variance()>0)
{
inf[0][0] = 1.0f/iter->second.variance(); // x
inf[1][1] = 1.0f/iter->second.variance(); // y
inf[2][2] = 1.0f/iter->second.variance(); // z
inf[3][3] = 1.0f/iter->second.variance(); // roll
inf[4][4] = 1.0f/iter->second.variance(); // pitch
inf[5][5] = 1.0f/iter->second.variance(); // yaw
}
AISNavigation::TreePoseGraph<AISNavigation::Operations3D<double> >::Vertex* v1=pg.vertex(id1);
AISNavigation::TreePoseGraph<AISNavigation::Operations3D<double> >::Vertex* v2=pg.vertex(id2);
AISNavigation::TreePoseGraph3::Transformation t(p);
if (!pg.addEdge(v1, v2,t ,m))
if (!pg.addEdge(v1, v2, t, inf))
{
UERROR("Map: Edge already exits between nodes %d and %d, skipping", id1, id2);
return;
@@ -2380,7 +2567,7 @@ bool saveTOROGraph(
{
float x,y,z, yaw,pitch,roll;
pcl::getTranslationAndEulerAngles(transformToEigen3f(iter->second.transform()), x,y,z, roll, pitch, yaw);
fprintf(file, "EDGE3 %d %d %f %f %f %f %f %f 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1\n",
fprintf(file, "EDGE3 %d %d %f %f %f %f %f %f %f 0 0 0 0 0 %f 0 0 0 0 %f 0 0 0 %f 0 0 %f 0 %f\n",
iter->first,
iter->second.to(),
x,
@@ -2388,7 +2575,13 @@ bool saveTOROGraph(
z,
roll,
pitch,
yaw);
yaw,
1.0f/iter->second.variance(),
1.0f/iter->second.variance(),
1.0f/iter->second.variance(),
1.0f/iter->second.variance(),
1.0f/iter->second.variance(),
1.0f/iter->second.variance());
}
UINFO("Graph saved to %s", fileName.c_str());
fclose(file);