i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-08 12:30:20 +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
CMakeLists.txt
+1 -1
View File
@@ -20,7 +20,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
#######################
SET(RTABMAP_MAJOR_VERSION 0)
SET(RTABMAP_MINOR_VERSION 8)
SET(RTABMAP_PATCH_VERSION 3)
SET(RTABMAP_PATCH_VERSION 4)
SET(RTABMAP_VERSION
${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
corelib/include/rtabmap/core/CameraEvent.h
+1 -1
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)
{
}
corelib/include/rtabmap/core/Link.h
+10 -6
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_;
};
}
corelib/include/rtabmap/core/Memory.h
+3 -3
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);
corelib/include/rtabmap/core/Parameters.h
+8 -7
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
corelib/include/rtabmap/core/Rtabmap.h
+1
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;
corelib/include/rtabmap/core/RtabmapThread.h
+2 -1
View File
@@ -110,7 +110,8 @@ private:
Rtabmap * _rtabmap;
bool _paused;
Transform lastPose_;
float _variance;
float _rotVariance;
float _transVariance;
};
} /* namespace rtabmap */
corelib/include/rtabmap/core/SensorData.h
+11 -7
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;
corelib/src/DBDriverSqlite3.cpp
+59 -18
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());
}
corelib/src/DBDriverSqlite3.h
+1 -1
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;
corelib/src/DBReader.cpp
+6 -3
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,
corelib/src/Graph.cpp
+20 -14
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);
corelib/src/Memory.cpp
+14 -14
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());
corelib/src/Odometry.cpp
+1 -1
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));
}
}
corelib/src/Rtabmap.cpp
+9 -6
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);
}
}
corelib/src/RtabmapThread.cpp
+21 -9
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.");
corelib/src/SensorData.cpp
+12 -6
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
corelib/src/Signature.cpp
+6 -3
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);
}
corelib/src/resources/DatabaseSchema.sql.in
+2 -1
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)
guilib/src/DatabaseViewer.cpp
+24 -7
View File
@@ -244,11 +244,23 @@ void DatabaseViewer::closeEvent(QCloseEvent* event)
std::multimap<int, rtabmap::Link>::iterator refinedIter = rtabmap::graph::findLink(linksRefined_, iter->second.from(), iter->second.to());
if(refinedIter != linksRefined_.end())
{
memory_->addLink(refinedIter->second.to(), refinedIter->second.from(), refinedIter->second.transform(), refinedIter->second.type(), refinedIter->second.variance());
memory_->addLink(
refinedIter->second.to(),
refinedIter->second.from(),
refinedIter->second.transform(),
refinedIter->second.type(),
refinedIter->second.rotVariance(),
refinedIter->second.transVariance());
}
else
{
memory_->addLink(iter->second.to(), iter->second.from(), iter->second.transform(), iter->second.type(), iter->second.variance());
memory_->addLink(
iter->second.to(),
iter->second.from(),
iter->second.transform(),
iter->second.type(),
iter->second.rotVariance(),
iter->second.transVariance());
}
}
@@ -257,7 +269,12 @@ void DatabaseViewer::closeEvent(QCloseEvent* event)
{
if(!containsLink(linksAdded_, iter->second.from(), iter->second.to()))
{
memory_->updateLink(iter->second.from(), iter->second.to(), iter->second.transform(), iter->second.variance());
memory_->updateLink(
iter->second.from(),
iter->second.to(),
iter->second.transform(),
iter->second.rotVariance(),
iter->second.transVariance());
}
}
@@ -1431,7 +1448,7 @@ void DatabaseViewer::updateConstraintView(const rtabmap::Link & linkIn,
ui_->checkBox_showOptimized->setEnabled(false);
UASSERT(!t.isNull() && memory_);
ui_->label_constraint->setText(QString("%1 (%2=%3)").arg(t.prettyPrint().c_str()).arg(QChar(0xc3, 0x03)).arg(sqrt(link.variance())));
ui_->label_constraint->setText(QString("%1 (%2r=%3 %4t=%5)").arg(t.prettyPrint().c_str()).arg(QChar(0xc3, 0x03)).arg(sqrt(link.rotVariance())).arg(QChar(0xc3, 0x03)).arg(sqrt(link.transVariance())));
if(link.type() == Link::kNeighbor &&
graphes_.size() &&
(int)graphes_.size()-1 == ui_->horizontalSlider_iterations->maximum())
@@ -2042,7 +2059,7 @@ void DatabaseViewer::refineConstraint(int from, int to, bool updateGraph)
if(hasConverged && !transform.isNull())
{
Link newLink(currentLink.from(), currentLink.to(), currentLink.type(), transform*t, variance);
Link newLink(currentLink.from(), currentLink.to(), currentLink.type(), transform*t, variance, variance);
bool updated = false;
std::multimap<int, Link>::iterator iter = linksRefined_.find(currentLink.from());
@@ -2147,7 +2164,7 @@ void DatabaseViewer::refineConstraintVisually(int from, int to, bool updateGraph
if(!t.isNull())
{
Link newLink(currentLink.from(), currentLink.to(), currentLink.type(), t, variance);
Link newLink(currentLink.from(), currentLink.to(), currentLink.type(), t, variance, variance);
bool updated = false;
std::multimap<int, Link>::iterator iter = linksRefined_.find(currentLink.from());
@@ -2275,7 +2292,7 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool updateGra
}
// transform is valid, make a link
linksAdded_.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, t, variance)));
linksAdded_.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, t, variance, variance)));
updateSlider = true;
}
}
guilib/src/MainWindow.cpp
+2 -2
View File
@@ -3193,7 +3193,7 @@ void MainWindow::postProcessing()
UINFO("Added new loop closure between %d and %d.", from, to);
addedLinks.insert(from);
addedLinks.insert(to);
_currentLinksMap.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, transform, variance)));
_currentLinksMap.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, transform, variance, variance)));
++loopClosuresAdded;
_initProgressDialog->appendText(tr("Detected loop closure %1->%2! (%3/%4)").arg(from).arg(to).arg(i+1).arg(clusters.size()));
}
@@ -3349,7 +3349,7 @@ void MainWindow::postProcessing()
if(!transform.isNull() && hasConverged &&
correspondencesRatio >= correspondenceRatio)
{
Link newLink(from, to, iter->second.type(), transform*iter->second.transform(), variance);
Link newLink(from, to, iter->second.type(), transform*iter->second.transform(), variance, variance);
iter->second = newLink;
}
else
guilib/src/PreferencesDialog.cpp
+2 -2
View File
@@ -495,6 +495,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->loopClosure_icp2Iterations->setObjectName(Parameters::kLccIcp2Iterations().c_str());
_ui->loopClosure_icp2Ratio->setObjectName(Parameters::kLccIcp2CorrespondenceRatio().c_str());
_ui->loopClosure_icp2Voxel->setObjectName(Parameters::kLccIcp2VoxelSize().c_str());
_ui->loopClosure_highICPTransitionalVariance->setObjectName(Parameters::kLccIcpHighTransitionalVariance().c_str());
//Odometry
_ui->odom_strategy->setObjectName(Parameters::kOdomStrategy().c_str());
@@ -2478,7 +2480,6 @@ void PreferencesDialog::addParameter(const QObject * object, bool value)
this->addParameters(_ui->groupBox_graphOptimization);
this->addParameters(_ui->groupBox_localDetection_time);
this->addParameters(_ui->groupBox_localDetection_space);
this->addParameters(_ui->groupBox_globalConstraints);
this->addParameters(_ui->groupBox_visualTransform2);
}
@@ -2487,7 +2488,6 @@ void PreferencesDialog::addParameter(const QObject * object, bool value)
// RGBD panel
if(value && groupBox == _ui->groupBox_localDetection_time)
{
this->addParameters(_ui->groupBox_globalConstraints);
this->addParameters(_ui->groupBox_visualTransform2);
}
if(value && groupBox == _ui->groupBox_localDetection_space)
guilib/src/ui/preferencesDialog.ui
+100 -153
View File
@@ -63,9 +63,9 @@
<property name="geometry">
<rect>
<x>0</x>
<y>-522</y>
<y>0</y>
<width>744</width>
<height>1179</height>
<height>1101</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>3</number>
<number>21</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29">
@@ -4986,7 +4986,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
<item row="3" column="1">
<widget class="QLabel" name="label_scanMatching">
<property name="text">
<string>Laser scan matching for odometry pose correction. ICP 2D only is used here.</string>
<string>Odometry pose correction using laser scan matching. ICP 2D only is used here.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -5029,125 +5029,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</layout>
</widget>
</item>
<item>
<widget class="QGroupBox" name="groupBox_globalConstraints">
<property name="title">
<string>Global loop closure constraints</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout_20">
<item>
<widget class="QLabel" name="label_55">
<property name="text">
<string>When a global loop closure is detected by the Bayes filter, a transform is computed between the current location and the old one. If a transform cannot be computed, the loop closure is rejected. The computation of the transform is done in 2 steps: </string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item>
<layout class="QGridLayout" name="gridLayout_10" columnstretch="0,1">
<item row="0" column="1">
<widget class="QLabel" name="label_57">
<property name="text">
<string>A transformation is computed using the 3D visual word correspondences. See &quot;Loop closure constraint&quot; panel for parameters.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QLabel" name="label_17">
<property name="text">
<string>1)</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_64">
<property name="text">
<string>Then if enabled, the visual transform is used as a guess for ICP estimation (3D or 2D). See &quot;ICP&quot; panel for parameters.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QLabel" name="label_18">
<property name="text">
<string>2)</string>
</property>
</widget>
</item>
</layout>
</item>
<item>
<layout class="QGridLayout" name="gridLayout_48" columnstretch="0,1">
<item row="0" column="0">
<widget class="QComboBox" name="globalDetection_icpType">
<property name="sizeAdjustPolicy">
<enum>QComboBox::AdjustToContents</enum>
</property>
<item>
<property name="text">
<string>No ICP</string>
</property>
</item>
<item>
<property name="text">
<string>ICP 3D (e.g. Kinect depth)</string>
</property>
</item>
<item>
<property name="text">
<string>ICP 2D (e.g. Laser scan)</string>
</property>
</item>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_19">
<property name="text">
<string>Use ICP estimation. If ICP 2D is selected, laser scans are required.</string>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QDoubleSpinBox" name="globalDetection_icpMaxDistance">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>2</number>
</property>
<property name="maximum">
<double>1.000000000000000</double>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
</property>
<property name="value">
<double>0.200000000000000</double>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_51">
<property name="text">
<string>Maximum ICP correction distance accepted. A large translation difference between the visual transformation and ICP transformation results in wrong transformations in most cases.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
</layout>
</item>
</layout>
</widget>
</item>
<item>
<widget class="QGroupBox" name="groupBox_graphOptimization">
<property name="title">
@@ -5405,7 +5286,7 @@ Warning when set to false: when some nodes are transferred, the first referentia
<item>
<widget class="QLabel" name="label_16">
<property name="text">
<string>Parameters to compute a transform from 3D visual word correspondences.</string>
<string>When a global loop closure is detected by the Bayes filter, a transform is computed between the current location and the old one. If a transform cannot be computed, the loop closure is rejected. A transformation is computed using the 3D visual word correspondences.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -5523,12 +5404,44 @@ Warning when set to false: when some nodes are transferred, the first referentia
</property>
</widget>
</item>
<item row="5" column="0">
<widget class="QComboBox" name="globalDetection_icpType">
<property name="sizeAdjustPolicy">
<enum>QComboBox::AdjustToContents</enum>
</property>
<item>
<property name="text">
<string>No ICP</string>
</property>
</item>
<item>
<property name="text">
<string>ICP 3D (e.g. Kinect depth)</string>
</property>
</item>
<item>
<property name="text">
<string>ICP 2D (e.g. Laser scan)</string>
</property>
</item>
</widget>
</item>
<item row="5" column="1">
<widget class="QLabel" name="label_57">
<property name="text">
<string>When enabled, the visual transform is used as a guess for ICP estimation (3D or 2D). See &quot;ICP&quot; panel for parameters.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
</layout>
</item>
<item>
<widget class="QGroupBox" name="groupBox_reextract">
<property name="title">
<string>Re-extract features</string>
<string>Re-extract features on global loop closure</string>
</property>
<property name="checkable">
<bool>true</bool>
@@ -5537,16 +5450,6 @@ Warning when set to false: when some nodes are transferred, the first referentia
<bool>true</bool>
</property>
<layout class="QVBoxLayout" name="verticalLayout_46">
<item>
<widget class="QLabel" name="label_171">
<property name="text">
<string>If not activated, when a loop closure constraint must be computed, the words already extracted for the loop closure detector are used. These words are limited (see Visual Word-&gt;Words Per Image) and matched to the loop closure detection vocabulary. When the vocabulary is large, there maybe less corresponding words on a loop closure.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="label_182">
<property name="text">
@@ -5558,9 +5461,9 @@ Warning when set to false: when some nodes are transferred, the first referentia
</widget>
</item>
<item>
<widget class="QLabel" name="label_183">
<widget class="QLabel" name="label_171">
<property name="text">
<string>Note that this is used only for global loop closure, so not for local loop closure in time.</string>
<string>If not activated, when a loop closure constraint must be computed, the words already extracted for the loop closure detector are used. These words are limited (see Visual Word-&gt;Words Per Image) and matched to the loop closure detection vocabulary. When the vocabulary is large, there maybe less corresponding words on a loop closure.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -5749,7 +5652,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
<property name="title">
<string>ICP</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout_51">
<layout class="QVBoxLayout" name="verticalLayout_20">
<item>
<widget class="QLabel" name="label_7">
<property name="text">
@@ -5757,15 +5660,62 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</property>
</widget>
</item>
<item>
<layout class="QGridLayout" name="gridLayout_48" columnstretch="0,1">
<item row="1" column="0">
<widget class="QCheckBox" name="loopClosure_highICPTransitionalVariance">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QDoubleSpinBox" name="globalDetection_icpMaxDistance">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>2</number>
</property>
<property name="maximum">
<double>1.000000000000000</double>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
</property>
<property name="value">
<double>0.200000000000000</double>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_51">
<property name="text">
<string>Maximum ICP correction distance accepted. A large translation difference between the visual transformation and ICP transformation results in wrong transformations in most cases.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_80">
<property name="text">
<string>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. Only used with ICP 2D.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
</layout>
</item>
<item>
<widget class="QGroupBox" name="groupBox_loopClosure_icp3">
<property name="title">
<string>ICP 3D - Point To Plane (e.g. Kinect depth)</string>
</property>
<layout class="QFormLayout" name="formLayout_3">
<property name="fieldGrowthPolicy">
<enum>QFormLayout::AllNonFixedFieldsGrow</enum>
</property>
<layout class="QGridLayout" name="gridLayout_53" columnstretch="0,1">
<item row="0" column="0">
<widget class="QSpinBox" name="loopClosure_icpDecimation">
<property name="minimum">
@@ -5936,6 +5886,13 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</property>
</widget>
</item>
<item row="7" column="0">
<widget class="QCheckBox" name="loopClosure_icpPointToPlane">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="7" column="1">
<widget class="QLabel" name="label_144">
<property name="text">
@@ -5946,13 +5903,6 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</property>
</widget>
</item>
<item row="7" column="0">
<widget class="QCheckBox" name="loopClosure_icpPointToPlane">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="8" column="0">
<widget class="QSpinBox" name="loopClosure_icpPointToPlaneNormals">
<property name="minimum">
@@ -5984,10 +5934,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
<property name="title">
<string>ICP 2D (e.g. Laser scan)</string>
</property>
<layout class="QFormLayout" name="formLayout_4">
<property name="fieldGrowthPolicy">
<enum>QFormLayout::AllNonFixedFieldsGrow</enum>
</property>
<layout class="QGridLayout" name="gridLayout_52" columnstretch="0,1">
<item row="0" column="0">
<widget class="QDoubleSpinBox" name="loopClosure_icp2MaxCorrespondenceDistance">
<property name="minimum">
package.xml
+1 -1
View File
@@ -1,7 +1,7 @@
<?xml version="1.0"?>
<package>
<name>rtabmap</name>
<version>0.8.3</version>
<version>0.8.4</version>
<description>RTAB-Map's standalone library. RTAB-Map is an RGB-D SLAM approach with real-time constraints.</description>
<maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
<author>Mathieu Labbe</author>