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

increased version to 0.8.4: Database Link's "variance" field split into "rot_variance" and "trans_variance". Added Parameter LccIcp/HighTransitionalVariance

Browse Source
This commit is contained in:
Mathieu Labbe
2015-02-24 16:06:02 -05:00
parent f514cfa5fe
commit eb96fe1249
24 changed files with 317 additions and 267 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
corelib/include/rtabmap/core/CameraEvent.h
View File

@@ -57,7 +57,7 @@ public:
CameraEvent(const cv::Mat & rgb, const cv::Mat & depth, float fx, float fy, float cx, float cy, const Transform & localTransform, int id) :
UEvent(kCodeImageDepth),
data_(rgb, depth, fx, fy, cx, cy, localTransform, Transform(), 1.0f, id)
data_(rgb, depth, fx, fy, cx, cy, localTransform, Transform(), 1.0f, 1.0f, id)
{
}

16
corelib/include/rtabmap/core/Link.h
View File

@@ -40,15 +40,17 @@ public:
from_(0),
to_(0),
type_(kUndef),
variance_(1.0f)
rotVariance_(1.0f),
transVariance_(1.0f)
{
}
Link(int from, int to, Type type, const Transform & transform, float variance) :
Link(int from, int to, Type type, const Transform & transform, float rotVariance, float transVariance) :
from_(from),
to_(to),
transform_(transform),
type_(type),
variance_(variance)
rotVariance_(rotVariance),
transVariance_(transVariance)
{
}
@@ -58,20 +60,22 @@ public:
int to() const {return to_;}
const Transform & transform() const {return transform_;}
Type type() const {return type_;}
float variance() const {return variance_;}
float rotVariance() const {return rotVariance_;}
float transVariance() const {return transVariance_;}
void setFrom(int from) {from_ = from;}
void setTo(int to) {to_ = to;}
void setTransform(const Transform & transform) {transform_ = transform;}
void setType(Type type) {type_ = type;}
void setVariance(float variance) {variance_ = variance;}
void setVariance(float rotVariance, float transVariance) {rotVariance_ = rotVariance; transVariance_ = transVariance;}
private:
int from_;
int to_;
Transform transform_;
Type type_;
float variance_;
float rotVariance_;
float transVariance_;
};
}

6
corelib/include/rtabmap/core/Memory.h
View File

@@ -80,8 +80,8 @@ public:
std::list<int> cleanup(const std::list<int> & ignoredIds = std::list<int>());
void emptyTrash();
void joinTrashThread();
bool addLink(int to, int from, const Transform & transform, Link::Type type, float variance);
void updateLink(int fromId, int toId, const Transform & transform, float variance);
bool addLink(int to, int from, const Transform & transform, Link::Type type, float rotVariance, float transVariance);
void updateLink(int fromId, int toId, const Transform & transform, float rotVariance, float transVariance);
void removeAllVirtualLinks();
std::map<int, int> getNeighborsId(int signatureId,
int margin,
@@ -167,7 +167,7 @@ public:
private:
void preUpdate();
void addSignatureToStm(Signature * signature, float odomVariance);
void addSignatureToStm(Signature * signature, float poseRotVariance, float poseTransVariance);
void clear();
void moveToTrash(Signature * s, bool saveToDatabase = true, std::list<int> * deletedWords = 0);

15
corelib/include/rtabmap/core/Parameters.h
View File

@@ -332,6 +332,7 @@ class RTABMAP_EXP Parameters
// Loop closure constraint
RTABMAP_PARAM(LccIcp, Type, int, 0, "0=No ICP, 1=ICP 3D, 2=ICP 2D");
RTABMAP_PARAM(LccIcp, MaxDistance, float, 0.2, "Maximum ICP correction distance accepted (m).");
RTABMAP_PARAM(LccIcp, HighTransitionalVariance, bool, true, "Set high transitional variance on odometry pose correction and on local loop closure in space. Particularly useful when the robot is moving in hallways with short-range laser rangefinder.");
RTABMAP_PARAM(LccBow, MinInliers, int, 20, "Minimum visual word correspondences to compute geometry transform.");
RTABMAP_PARAM(LccBow, InlierDistance, float, 0.02, "Maximum distance for visual word correspondences.");
@@ -349,14 +350,14 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(LccIcp3, VoxelSize, float, 0.01, "Voxel size to be used for ICP computation.");
RTABMAP_PARAM(LccIcp3, Samples, int, 0, "Random samples to be used for ICP computation. Not used if voxelSize is set.");
RTABMAP_PARAM(LccIcp3, MaxCorrespondenceDistance, float, 0.05, "ICP 3D: Max distance for point correspondences.");
RTABMAP_PARAM(LccIcp3, Iterations, int, 30, "ICP 3D: Max iterations.");
RTABMAP_PARAM(LccIcp3, CorrespondenceRatio, float, 0.7, "ICP 3D: Ratio of matching correspondences to accept the transform.");
RTABMAP_PARAM(LccIcp3, PointToPlane, bool, false, "ICP 3D: Use point to plane ICP.");
RTABMAP_PARAM(LccIcp3, PointToPlaneNormalNeighbors, int, 20, "ICP 3D: Number of neighbors to compute normals for point to plane.");
RTABMAP_PARAM(LccIcp3, Iterations, int, 30, "Max iterations.");
RTABMAP_PARAM(LccIcp3, CorrespondenceRatio, float, 0.7, "Ratio of matching correspondences to accept the transform.");
RTABMAP_PARAM(LccIcp3, PointToPlane, bool, false, "Use point to plane ICP.");
RTABMAP_PARAM(LccIcp3, PointToPlaneNormalNeighbors, int, 20, "Number of neighbors to compute normals for point to plane.");
RTABMAP_PARAM(LccIcp2, MaxCorrespondenceDistance, float, 0.1, "ICP 2D: Max distance for point correspondences.");
RTABMAP_PARAM(LccIcp2, Iterations, int, 30, "ICP 2D: Max iterations.");
RTABMAP_PARAM(LccIcp2, CorrespondenceRatio, float, 0.7, "ICP 2D: Ratio of matching correspondences to accept the transform.");
RTABMAP_PARAM(LccIcp2, MaxCorrespondenceDistance, float, 0.1, "Max distance for point correspondences.");
RTABMAP_PARAM(LccIcp2, Iterations, int, 30, "Max iterations.");
RTABMAP_PARAM(LccIcp2, CorrespondenceRatio, float, 0.7, "Ratio of matching correspondences to accept the transform.");
RTABMAP_PARAM(LccIcp2, VoxelSize, float, 0.005, "Voxel size to be used for ICP computation.");
// Stereo disparity

1
corelib/include/rtabmap/core/Rtabmap.h
View File

@@ -183,6 +183,7 @@ private:
bool _startNewMapOnLoopClosure;
float _goalReachedRadius; // meters
bool _planWithNearNodesLinked;
bool _icpHighTransVariance;
std::pair<int, float> _loopClosureHypothesis;
std::pair<int, float> _highestHypothesis;

3
corelib/include/rtabmap/core/RtabmapThread.h
View File

@@ -110,7 +110,8 @@ private:
Rtabmap * _rtabmap;
bool _paused;
Transform lastPose_;
float _variance;
float _rotVariance;
float _transVariance;
};
} /* namespace rtabmap */

18
corelib/include/rtabmap/core/SensorData.h
View File

@@ -54,8 +54,9 @@ public:
float cy,
const Transform & localTransform,
const Transform & pose,
float poseVariance,
int id = 0);
float poseRotVariance,
float poseTransVariance,
int id);
// Metric constructor + 2d laser scan
SensorData(const cv::Mat & laserScan,
@@ -67,8 +68,9 @@ public:
float cy,
const Transform & localTransform,
const Transform & pose,
float poseVariance,
int id = 0);
float poseRotVariance,
float poseTransVariance,
int id);
virtual ~SensorData() {}
@@ -82,7 +84,7 @@ public:
void setId(int id) {_id = id;}
bool isMetric() const {return !_depthOrRightImage.empty() || _fx != 0.0f || _fyOrBaseline != 0.0f || !_pose.isNull();}
void setPose(const Transform & pose, float variance) {_pose = pose; _poseVariance=variance;}
void setPose(const Transform & pose, float rotVariance, float transVariance) {_pose = pose; _poseRotVariance=rotVariance; _poseTransVariance = transVariance;}
cv::Mat depth() const {return (_depthOrRightImage.type()==CV_32FC1 || _depthOrRightImage.type()==CV_16UC1)?_depthOrRightImage:cv::Mat();}
cv::Mat rightImage() const {return _depthOrRightImage.type()==CV_8UC1?_depthOrRightImage:cv::Mat();}
const cv::Mat & depthOrRightImage() const {return _depthOrRightImage;}
@@ -95,7 +97,8 @@ public:
float fyOrBaseline() const {return _fyOrBaseline;}
const Transform & pose() const {return _pose;}
const Transform & localTransform() const {return _localTransform;}
float poseVariance() const {return _poseVariance;}
float poseRotVariance() const {return _poseRotVariance;}
float poseTransVariance() const {return _poseTransVariance;}
void setFeatures(const std::vector<cv::KeyPoint> & keypoints, const cv::Mat & descriptors)
{
@@ -118,7 +121,8 @@ private:
float _cy;
Transform _pose;
Transform _localTransform;
float _poseVariance;
float _poseRotVariance;
float _poseTransVariance;
// features
std::vector<cv::KeyPoint> _keypoints;

77
corelib/src/DBDriverSqlite3.cpp
View File

@@ -1360,7 +1360,11 @@ void DBDriverSqlite3::loadLinksQuery(
sqlite3_stmt * ppStmt = 0;
std::stringstream query;
if(uStrNumCmp(_version, "0.7.4") >= 0)
if(uStrNumCmp(_version, "0.8.4") >= 0)
{
query << "SELECT to_id, type, transform, rot_variance, trans_variance FROM Link ";
}
else if(uStrNumCmp(_version, "0.7.4") >= 0)
{
query << "SELECT to_id, type, transform, variance FROM Link ";
}
@@ -1391,7 +1395,8 @@ void DBDriverSqlite3::loadLinksQuery(
int toId = -1;
int type = Link::kUndef;
float variance = 1.0f;
float rotVariance = 1.0f;
float transVariance = 1.0f;
const void * data = 0;
int dataSize = 0;
@@ -1417,15 +1422,21 @@ void DBDriverSqlite3::loadLinksQuery(
UERROR("Error while loading link transform from %d to %d! Setting to null...", signatureId, toId);
}
if(uStrNumCmp(_version, "0.7.4") >= 0)
if(uStrNumCmp(_version, "0.8.4") >= 0)
{
variance = sqlite3_column_double(ppStmt, index++);
neighbors.insert(neighbors.end(), std::make_pair(toId, Link(signatureId, toId, (Link::Type)type, transform, variance)));
rotVariance = sqlite3_column_double(ppStmt, index++);
transVariance = sqlite3_column_double(ppStmt, index++);
neighbors.insert(neighbors.end(), std::make_pair(toId, Link(signatureId, toId, (Link::Type)type, transform, rotVariance, transVariance)));
}
else if(uStrNumCmp(_version, "0.7.4") >= 0)
{
rotVariance = transVariance = sqlite3_column_double(ppStmt, index++);
neighbors.insert(neighbors.end(), std::make_pair(toId, Link(signatureId, toId, (Link::Type)type, transform, rotVariance, transVariance)));
}
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)));
neighbors.insert(neighbors.end(), std::make_pair(toId, Link(signatureId, toId, type==0?Link::kNeighbor:Link::kGlobalClosure, transform, rotVariance, transVariance)));
}
rc = sqlite3_step(ppStmt);
@@ -1455,7 +1466,13 @@ void DBDriverSqlite3::loadLinksQuery(std::list<Signature *> & signatures) const
std::stringstream query;
int totalLinksLoaded = 0;
if(uStrNumCmp(_version, "0.7.4") >= 0)
if(uStrNumCmp(_version, "0.8.4") >= 0)
{
query << "SELECT to_id, type, rot_variance, trans_variance, transform FROM Link "
<< "WHERE from_id = ? "
<< "ORDER BY to_id";
}
else if(uStrNumCmp(_version, "0.7.4") >= 0)
{
query << "SELECT to_id, type, variance, transform FROM Link "
<< "WHERE from_id = ? "
@@ -1479,7 +1496,8 @@ void DBDriverSqlite3::loadLinksQuery(std::list<Signature *> & signatures) const
int toId = -1;
int linkType = -1;
float variance = 1.0f;
float rotVariance = 1.0f;
float transVariance = 1.0f;
std::list<Link> links;
const void * data = 0;
int dataSize = 0;
@@ -1492,9 +1510,14 @@ 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)
if(uStrNumCmp(_version, "0.8.4") >= 0)
{
variance = sqlite3_column_double(ppStmt, index++);
rotVariance = sqlite3_column_double(ppStmt, index++);
transVariance = sqlite3_column_double(ppStmt, index++);
}
else if(uStrNumCmp(_version, "0.7.4") >= 0)
{
rotVariance = transVariance = sqlite3_column_double(ppStmt, index++);
}
//transform
@@ -1514,11 +1537,11 @@ void DBDriverSqlite3::loadLinksQuery(std::list<Signature *> & signatures) const
{
if(uStrNumCmp(_version, "0.7.4") >= 0)
{
links.push_back(Link((*iter)->id(), toId, (Link::Type)linkType, transform, variance));
links.push_back(Link((*iter)->id(), toId, (Link::Type)linkType, transform, rotVariance, transVariance));
}
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));
links.push_back(Link((*iter)->id(), toId, linkType == 0?Link::kNeighbor:Link::kGlobalClosure, transform, rotVariance, transVariance));
}
}
else
@@ -1631,7 +1654,7 @@ void DBDriverSqlite3::updateQuery(const std::list<Signature *> & nodes, bool upd
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, i->second.type(), i->second.variance(), i->second.transform());
stepLink(ppStmt, (*j)->id(), i->first, i->second.type(), i->second.rotVariance(), i->second.transVariance(), i->second.transform());
}
}
}
@@ -1739,7 +1762,7 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures) const
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, i->second.type(), i->second.variance(), i->second.transform());
stepLink(ppStmt, (*jter)->id(), i->first, i->second.type(), i->second.rotVariance(), i->second.transVariance(), i->second.transform());
}
}
// Finalize (delete) the statement
@@ -2027,7 +2050,11 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt,
std::string DBDriverSqlite3::queryStepLink() const
{
if(uStrNumCmp(_version, "0.7.4") >= 0)
if(uStrNumCmp(_version, "0.8.4") >= 0)
{
return "INSERT INTO Link(from_id, to_id, type, rot_variance, trans_variance, transform) VALUES(?,?,?,?,?,?);";
}
else if(uStrNumCmp(_version, "0.7.4") >= 0)
{
return "INSERT INTO Link(from_id, to_id, type, variance, transform) VALUES(?,?,?,?,?);";
}
@@ -2036,7 +2063,14 @@ std::string DBDriverSqlite3::queryStepLink() const
return "INSERT INTO Link(from_id, to_id, type, transform) VALUES(?,?,?,?);";
}
}
void DBDriverSqlite3::stepLink(sqlite3_stmt * ppStmt, int fromId, int toId, Link::Type type, float variance, const Transform & transform) const
void DBDriverSqlite3::stepLink(
sqlite3_stmt * ppStmt,
int fromId,
int toId,
Link::Type type,
float rotVariance,
float transVariance,
const Transform & transform) const
{
if(!ppStmt)
{
@@ -2060,9 +2094,16 @@ void DBDriverSqlite3::stepLink(sqlite3_stmt * ppStmt, int fromId, int toId, Link
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)
if(uStrNumCmp(_version, "0.8.4") >= 0)
{
rc = sqlite3_bind_double(ppStmt, index++, variance);
rc = sqlite3_bind_double(ppStmt, index++, rotVariance);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_bind_double(ppStmt, index++, transVariance);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
}
else if(uStrNumCmp(_version, "0.7.4") >= 0)
{
rc = sqlite3_bind_double(ppStmt, index++, rotVariance<transVariance?rotVariance:transVariance);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
}

2
corelib/src/DBDriverSqlite3.h
View File

@@ -109,7 +109,7 @@ private:
float cx,
float cy,
const Transform & localTransform) const;
void stepLink(sqlite3_stmt * ppStmt, int fromId, int toId, Link::Type type, float variance, const Transform & transform) 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;

9
corelib/src/DBReader.cpp
View File

@@ -193,7 +193,8 @@ SensorData DBReader::getNextData()
int mapId;
float fx,fy,cx,cy;
Transform localTransform, pose;
float variance = 1.0f;
float rotVariance = 1.0f;
float transVariance = 1.0f;
_dbDriver->getNodeData(*_currentId, imageBytes, depthBytes, laserScanBytes, fx, fy, cx, cy, localTransform);
if(!_odometryIgnored)
{
@@ -203,7 +204,8 @@ SensorData DBReader::getNextData()
if(links.size())
{
// assume the first is the backward neighbor, take its variance
variance = links.begin()->second.variance();
rotVariance = links.begin()->second.rotVariance();
transVariance = links.begin()->second.transVariance();
}
}
int seq = *_currentId;
@@ -229,7 +231,8 @@ SensorData DBReader::getNextData()
fx,fy,cx,cy,
localTransform,
pose,
variance,
rotVariance,
transVariance,
seq);
UDEBUG("Laser=%d RGB/Left=%d Depth=%d Right=%d",
data.laserScan().empty()?0:1,

34
corelib/src/Graph.cpp
View File

@@ -185,7 +185,7 @@ void optimizeTOROGraph(
{
UASSERT(uContains(rtabmapToToro, iter->first) && uContains(rtabmapToToro, iter->second.to()));
UASSERT(!iter->second.transform().isNull());
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())));
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.rotVariance(), iter->second.transVariance())));
}
}
@@ -285,14 +285,20 @@ void optimizeTOROGraph(
pcl::getTranslationAndEulerAngles(iter->second.transform().toEigen3f(), x,y,z, roll,pitch,yaw);
AISNavigation::TreePoseGraph3::Pose p(x, y, z, roll, pitch, yaw);
AISNavigation::TreePoseGraph3::InformationMatrix inf = DMatrix<double>::I(6);
if(!ignoreCovariance && iter->second.variance()>0)
if(!ignoreCovariance)
{
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
if(iter->second.rotVariance()>0)
{
inf[0][0] = 1.0f/iter->second.rotVariance(); // roll
inf[1][1] = 1.0f/iter->second.rotVariance(); // pitch
inf[2][2] = 1.0f/iter->second.rotVariance(); // yaw
}
if(iter->second.transVariance()>0)
{
inf[3][3] = 1.0f/iter->second.transVariance(); // x
inf[4][4] = 1.0f/iter->second.transVariance(); // y
inf[5][5] = 1.0f/iter->second.transVariance(); // z
}
}
AISNavigation::TreePoseGraph<AISNavigation::Operations3D<double> >::Vertex* v1=pg.vertex(id1);
@@ -409,12 +415,12 @@ bool saveTOROGraph(
roll,
pitch,
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());
1.0f/iter->second.rotVariance(),
1.0f/iter->second.rotVariance(),
1.0f/iter->second.rotVariance(),
1.0f/iter->second.transVariance(),
1.0f/iter->second.transVariance(),
1.0f/iter->second.transVariance());
}
UINFO("Graph saved to %s", fileName.c_str());
fclose(file);

28
corelib/src/Memory.cpp
View File

@@ -548,7 +548,7 @@ bool Memory::update(const SensorData & data, Statistics * stats)
UDEBUG("time creating signature=%f ms", t);
// It will be added to the short-term memory, no need to delete it...
this->addSignatureToStm(signature, data.poseVariance());
this->addSignatureToStm(signature, data.poseRotVariance(), data.poseTransVariance());
_lastSignature = signature;
@@ -657,7 +657,7 @@ void Memory::setRoi(const std::string & roi)
}
}
void Memory::addSignatureToStm(Signature * signature, float poseVariance)
void Memory::addSignatureToStm(Signature * signature, float poseRotVariance, float poseTransVariance)
{
UTimer timer;
// add signature on top of the short-term memory
@@ -674,13 +674,13 @@ void Memory::addSignatureToStm(Signature * signature, float poseVariance)
!_signatures.at(*_stMem.rbegin())->getPose().isNull())
{
motionEstimate = _signatures.at(*_stMem.rbegin())->getPose().inverse() * signature->getPose();
_signatures.at(*_stMem.rbegin())->addLink(Link(*_stMem.rbegin(), signature->id(), Link::kNeighbor, motionEstimate, poseVariance));
signature->addLink(Link(signature->id(), *_stMem.rbegin(), Link::kNeighbor, motionEstimate.inverse(), poseVariance));
_signatures.at(*_stMem.rbegin())->addLink(Link(*_stMem.rbegin(), signature->id(), Link::kNeighbor, motionEstimate, poseRotVariance, poseTransVariance));
signature->addLink(Link(signature->id(), *_stMem.rbegin(), Link::kNeighbor, motionEstimate.inverse(), poseRotVariance, poseTransVariance));
}
else
{
_signatures.at(*_stMem.rbegin())->addLink(Link(*_stMem.rbegin(), signature->id(), Link::kNeighbor, Transform(), 1.0f));
signature->addLink(Link(signature->id(), *_stMem.rbegin(), Link::kNeighbor, Transform(), 1.0f));
_signatures.at(*_stMem.rbegin())->addLink(Link(*_stMem.rbegin(), signature->id(), Link::kNeighbor, Transform(), 1.0f, 1.0f));
signature->addLink(Link(signature->id(), *_stMem.rbegin(), Link::kNeighbor, Transform(), 1.0f, 1.0f));
}
UDEBUG("Min STM id = %d", *_stMem.begin());
}
@@ -2200,7 +2200,7 @@ Transform Memory::computeScanMatchingTransform(
}
// Transform from new to old
bool Memory::addLink(int oldId, int newId, const Transform & transform, Link::Type type, float variance)
bool Memory::addLink(int oldId, int newId, const Transform & transform, Link::Type type, float rotVariance, float transVariance)
{
UASSERT(type > Link::kNeighbor && type != Link::kUndef);
@@ -2218,8 +2218,8 @@ bool Memory::addLink(int oldId, int newId, const Transform & transform, Link::Ty
UDEBUG("Add link between %d and %d", oldS->id(), newS->id());
oldS->addLink(Link(oldS->id(), newS->id(), type, transform.inverse(), variance));
newS->addLink(Link(newS->id(), oldS->id(), type, transform, variance));
oldS->addLink(Link(oldS->id(), newS->id(), type, transform.inverse(), rotVariance, transVariance));
newS->addLink(Link(newS->id(), oldS->id(), type, transform, rotVariance, transVariance));
if(type!=Link::kVirtualClosure)
{
@@ -2258,7 +2258,7 @@ bool Memory::addLink(int oldId, int newId, const Transform & transform, Link::Ty
return false;
}
void Memory::updateLink(int fromId, int toId, const Transform & transform, float variance)
void Memory::updateLink(int fromId, int toId, const Transform & transform, float rotVariance, float transVariance)
{
Signature * fromS = this->_getSignature(fromId);
Signature * toS = this->_getSignature(toId);
@@ -2269,8 +2269,8 @@ void Memory::updateLink(int fromId, int toId, const Transform & transform, float
fromS->removeLink(toId);
toS->removeLink(fromId);
fromS->addLink(Link(fromId, toId, type, transform, variance));
toS->addLink(Link(toId, fromId, type, transform.inverse(), variance));
fromS->addLink(Link(fromId, toId, type, transform, rotVariance, transVariance));
toS->addLink(Link(toId, fromId, type, transform.inverse(), rotVariance, transVariance));
if(type!=Link::kVirtualClosure)
{
@@ -2535,7 +2535,7 @@ bool Memory::rehearsalMerge(int oldId, int newId)
}
oldS->removeLinks(); // remove all links
oldS->addLink(Link(oldS->id(), newS->id(), Link::kGlobalClosure, Transform(), 1.0f)); // to keep track of the merged location
oldS->addLink(Link(oldS->id(), newS->id(), Link::kGlobalClosure, Transform(), 1.0f, 1.0f)); // to keep track of the merged location
// Set old image to new signature
this->copyData(oldS, newS);
@@ -2550,7 +2550,7 @@ bool Memory::rehearsalMerge(int oldId, int newId)
}
else
{
newS->addLink(Link(newS->id(), oldS->id(), Link::kGlobalClosure, Transform(), 1.0f)); // to keep track of the merged location
newS->addLink(Link(newS->id(), oldS->id(), Link::kGlobalClosure, Transform(), 1.0f, 1.0f)); // to keep track of the merged location
// update weight
oldS->setWeight(newS->getWeight() + 1 + oldS->getWeight());

2
corelib/src/Odometry.cpp
View File

@@ -1301,7 +1301,7 @@ void OdometryThread::mainLoop()
{
OdometryInfo info;
Transform pose = _odometry->process(data, &info);
data.setPose(pose, info.variance); // a null pose notify that odometry could not be computed
data.setPose(pose, info.variance, info.variance); // a null pose notify that odometry could not be computed
this->post(new OdometryEvent(data, info));
}
}

15
corelib/src/Rtabmap.cpp
View File

@@ -58,6 +58,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#define LOG_I "LogI.txt"
#define GRAPH_FILE_NAME "Graph.dot"
#define HIGH_VARIANCE 10000
//
//
@@ -110,6 +111,7 @@ Rtabmap::Rtabmap() :
_startNewMapOnLoopClosure(Parameters::defaultRtabmapStartNewMapOnLoopClosure()),
_goalReachedRadius(Parameters::defaultRGBDGoalReachedRadius()),
_planWithNearNodesLinked(Parameters::defaultRGBDPlanWithNearNodesLinked()),
_icpHighTransVariance(Parameters::defaultLccIcpHighTransitionalVariance()),
_loopClosureHypothesis(0,0.0f),
_highestHypothesis(0,0.0f),
_lastProcessTime(0.0),
@@ -376,6 +378,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kRtabmapStartNewMapOnLoopClosure(), _startNewMapOnLoopClosure);
Parameters::parse(parameters, Parameters::kRGBDGoalReachedRadius(), _goalReachedRadius);
Parameters::parse(parameters, Parameters::kRGBDPlanWithNearNodesLinked(), _planWithNearNodesLinked);
Parameters::parse(parameters, Parameters::kLccIcpHighTransitionalVariance(), _icpHighTransVariance);
// RGB-D SLAM stuff
if((iter=parameters.find(Parameters::kLccIcpType())) != parameters.end())
@@ -870,7 +873,7 @@ bool Rtabmap::process(const SensorData & data)
oldId,
signature->getLinks().at(oldId).transform().prettyPrint().c_str(),
t.prettyPrint().c_str());
_memory->updateLink(signature->id(), oldId, t, variance);
_memory->updateLink(signature->id(), oldId, t, variance, _icpHighTransVariance?HIGH_VARIANCE:variance);
}
else
{
@@ -928,7 +931,7 @@ bool Rtabmap::process(const SensorData & data)
*iter,
transform.prettyPrint().c_str());
// Add a loop constraint
if(_memory->addLink(*iter, signature->id(), transform, Link::kLocalTimeClosure, variance))
if(_memory->addLink(*iter, signature->id(), transform, Link::kLocalTimeClosure, variance, variance))
{
++localLoopClosuresInTimeFound;
UINFO("Local loop closure found between %d and %d with t=%s",
@@ -1421,7 +1424,7 @@ bool Rtabmap::process(const SensorData & data)
if(!rejectedHypothesis)
{
// Make the new one the parent of the old one
rejectedHypothesis = !_memory->addLink(_loopClosureHypothesis.first, signature->id(), transform, Link::kGlobalClosure, variance);
rejectedHypothesis = !_memory->addLink(_loopClosureHypothesis.first, signature->id(), transform, Link::kGlobalClosure, variance, variance);
}
if(rejectedHypothesis)
@@ -1483,7 +1486,7 @@ bool Rtabmap::process(const SensorData & data)
signature->id(),
localSpaceNearestId,
t.prettyPrint().c_str());
_memory->addLink(localSpaceNearestId, signature->id(), t, Link::kLocalSpaceClosure, variance);
_memory->addLink(localSpaceNearestId, signature->id(), t, Link::kLocalSpaceClosure, variance, _icpHighTransVariance?HIGH_VARIANCE:variance);
// Old map -> new map, used for localization correction on loop closure
const Signature * oldS = _memory->getSignature(localSpaceNearestId);
@@ -1565,7 +1568,7 @@ bool Rtabmap::process(const SensorData & data)
Transform virtualLoop = _optimizedPoses.at(signature->id()).inverse() * _optimizedPoses.at(_path[_pathCurrentIndex].first);
if(_localRadius > 0.0f && virtualLoop.getNorm() < _localRadius)
{
_memory->addLink(_path[_pathCurrentIndex].first, signature->id(), virtualLoop, Link::kVirtualClosure, 99999);
_memory->addLink(_path[_pathCurrentIndex].first, signature->id(), virtualLoop, Link::kVirtualClosure, HIGH_VARIANCE, HIGH_VARIANCE);
}
}
}
@@ -2605,7 +2608,7 @@ void Rtabmap::updateGoalIndex()
if(!s->hasLink(_path[i-1].first) && _memory->getSignature(_path[i-1].first) != 0)
{
Transform virtualLoop = _path[i].second.inverse() * _path[i-1].second;
_memory->addLink(_path[i-1].first, _path[i].first, virtualLoop, Link::kVirtualClosure, 99999);
_memory->addLink(_path[i-1].first, _path[i].first, virtualLoop, Link::kVirtualClosure, HIGH_VARIANCE, HIGH_VARIANCE);
UINFO("Added Virtual link between %d and %d", _path[i-1].first, _path[i].first);
}
}

30
corelib/src/RtabmapThread.cpp
View File

@@ -48,7 +48,8 @@ RtabmapThread::RtabmapThread(Rtabmap * rtabmap) :
_rtabmap(rtabmap),
_paused(false),
lastPose_(Transform::getIdentity()),
_variance(0)
_rotVariance(0),
_transVariance(0)
{
UASSERT(rtabmap != 0);
@@ -85,7 +86,8 @@ void RtabmapThread::clearBufferedData()
{
_dataBuffer.clear();
lastPose_.setIdentity();
_variance = 0;
_rotVariance = 0;
_transVariance = 0;
}
_dataMutex.unlock();
}
@@ -437,13 +439,18 @@ void RtabmapThread::addData(const SensorData & sensorData)
{
UWARN("Odometry is reset (identity pose detected). Increment map id!");
pushNewState(kStateTriggeringMap);
_variance = 0;
_rotVariance = 0;
_transVariance = 0;
}
lastPose_ = sensorData.pose();
if(sensorData.poseVariance() > _variance)
if(sensorData.poseRotVariance() > _rotVariance)
{
_variance = sensorData.poseVariance();
_rotVariance = sensorData.poseRotVariance();
}
if(sensorData.poseTransVariance() > _transVariance)
{
_transVariance = sensorData.poseTransVariance();
}
if(_rate>0.0f)
@@ -459,12 +466,17 @@ void RtabmapThread::addData(const SensorData & sensorData)
_dataMutex.lock();
{
_dataBuffer.push_back(sensorData);
if(_variance <= 0)
if(_rotVariance <= 0)
{
_variance = 1.0f;
_rotVariance = 1.0f;
}
_dataBuffer.back().setPose(_dataBuffer.back().pose(), _variance);
_variance = 0;
if(_transVariance <= 0)
{
_transVariance = 1.0f;
}
_dataBuffer.back().setPose(_dataBuffer.back().pose(), _rotVariance, _transVariance);
_rotVariance = 0;
_transVariance = 0;
while(_dataBufferMaxSize > 0 && _dataBuffer.size() > (unsigned int)_dataBufferMaxSize)
{
ULOGGER_WARN("Data buffer is full, the oldest data is removed to add the new one.");

18
corelib/src/SensorData.cpp
View File

@@ -43,7 +43,8 @@ SensorData::SensorData() :
_cx(0.0f),
_cy(0.0f),
_localTransform(Transform::getIdentity()),
_poseVariance(1.0f)
_poseRotVariance(1.0f),
_poseTransVariance(1.0f)
{
}
@@ -56,7 +57,8 @@ SensorData::SensorData(const cv::Mat & image,
_cx(0.0f),
_cy(0.0f),
_localTransform(Transform::getIdentity()),
_poseVariance(1.0f)
_poseRotVariance(1.0f),
_poseTransVariance(1.0f)
{
UASSERT(image.type() == CV_8UC1 || // Mono
image.type() == CV_8UC3); // RGB
@@ -71,7 +73,8 @@ SensorData::SensorData(const cv::Mat & image,
float cy,
const Transform & localTransform,
const Transform & pose,
float poseVariance,
float poseRotVariance,
float poseTransVariance,
int id) :
_image(image),
_id(id),
@@ -82,7 +85,8 @@ SensorData::SensorData(const cv::Mat & image,
_cy(cy),
_pose(pose),
_localTransform(localTransform),
_poseVariance(poseVariance)
_poseRotVariance(poseRotVariance),
_poseTransVariance(poseTransVariance)
{
UASSERT(image.type() == CV_8UC1 || // Mono
image.type() == CV_8UC3); // RGB
@@ -103,7 +107,8 @@ SensorData::SensorData(const cv::Mat & laserScan,
float cy,
const Transform & localTransform,
const Transform & pose,
float poseVariance,
float poseRotVariance,
float poseTransVariance,
int id) :
_image(image),
_id(id),
@@ -115,7 +120,8 @@ SensorData::SensorData(const cv::Mat & laserScan,
_cy(cy),
_pose(pose),
_localTransform(localTransform),
_poseVariance(poseVariance)
_poseRotVariance(poseRotVariance),
_poseTransVariance(poseTransVariance)
{
UASSERT(_laserScan.empty() || _laserScan.type() == CV_32FC2);
UASSERT(image.type() == CV_8UC1 || // Mono

9
corelib/src/Signature.cpp
View File

@@ -229,7 +229,8 @@ void Signature::setDepthCompressed(const cv::Mat & bytes, float fx, float fy, fl
SensorData Signature::toSensorData()
{
this->uncompressData();
float variance = 1.0f;
float rotVariance = 1.0f;
float transVariance = 1.0f;
if(_links.size())
{
for(std::map<int, Link>::iterator iter = _links.begin(); iter!=_links.end(); ++iter)
@@ -239,7 +240,8 @@ SensorData Signature::toSensorData()
//Assume the first neighbor to be the backward neighbor link
if(iter->second.to() < iter->second.from())
{
variance = iter->second.variance();
rotVariance = iter->second.rotVariance();
transVariance = iter->second.transVariance();
break;
}
}
@@ -254,7 +256,8 @@ SensorData Signature::toSensorData()
_cy,
_localTransform,
_pose,
variance,
rotVariance,
transVariance,
_id);
}

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

@@ -46,7 +46,8 @@ 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,
rot_variance FLOAT NOT NULL,
trans_variance FLOAT NOT NULL,
transform BLOB,
FOREIGN KEY (from_id) REFERENCES Node(id),
FOREIGN KEY (to_id) REFERENCES Node(id)