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

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

Browse Source
This commit is contained in:
Mathieu Labbe
2015-05-01 07:30:09 -04:00
parent d09e8f237a
commit abb7eb15ac
29 changed files with 622 additions and 275 deletions
Download Patch File Download Diff File Expand all files Collapse all files
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 10)
SET(RTABMAP_PATCH_VERSION 11)
SET(RTABMAP_VERSION
${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
corelib/include/rtabmap/core/DBDriver.h
+2 -2
View File
@@ -95,7 +95,7 @@ public:
// Specific queries...
void loadNodeData(std::list<Signature *> & signatures, bool loadMetricData) const;
void getNodeData(int signatureId, cv::Mat & imageCompressed, cv::Mat & depthCompressed, cv::Mat & laserScanCompressed, float & fx, float & fy, float & cx, float & cy, Transform & localTransform) const;
void getNodeData(int signatureId, cv::Mat & imageCompressed, cv::Mat & depthCompressed, cv::Mat & laserScanCompressed, float & fx, float & fy, float & cx, float & cy, Transform & localTransform, int & laserScanMaxPts) const;
void getNodeData(int signatureId, cv::Mat & imageCompressed) const;
bool getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, std::vector<unsigned char> & userData) const;
void loadLinks(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const;
@@ -134,7 +134,7 @@ private:
virtual void loadLinksQuery(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const = 0;
virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool loadMetricData) const = 0;
virtual void getNodeDataQuery(int signatureId, cv::Mat & imageCompressed, cv::Mat & depthCompressed, cv::Mat & laserScanCompressed, float & fx, float & fy, float & cx, float & cy, Transform & localTransform) const = 0;
virtual void getNodeDataQuery(int signatureId, cv::Mat & imageCompressed, cv::Mat & depthCompressed, cv::Mat & laserScanCompressed, float & fx, float & fy, float & cx, float & cy, Transform & localTransform, int & laserScanMaxPts) const = 0;
virtual void getNodeDataQuery(int signatureId, cv::Mat & imageCompressed) const = 0;
virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, std::vector<unsigned char> & userData) const = 0;
virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren) const = 0;
corelib/include/rtabmap/core/DBReader.h
+2 -1
View File
@@ -62,7 +62,7 @@ protected:
private:
std::string _path;
float _frameRate;
float _frameRate; // -1 = use Database stamps, 0 = inf
bool _odometryIgnored;
bool _ignoreGoalDelay;
@@ -70,6 +70,7 @@ private:
UTimer _timer;
std::set<int> _ids;
std::set<int>::iterator _currentId;
double _previousStamp;
};
} /* namespace rtabmap */
corelib/include/rtabmap/core/Memory.h
+2 -2
View File
@@ -165,8 +165,8 @@ public:
bool getBowForce2D() const {return _bowForce2D;}
Transform computeVisualTransform(int oldId, int newId, std::string * rejectedMsg = 0, int * inliers = 0, double * variance = 0) const;
Transform computeVisualTransform(const Signature & oldS, const Signature & newS, std::string * rejectedMsg = 0, int * inliers = 0, double * variance = 0) const;
Transform computeIcpTransform(int oldId, int newId, Transform guess, bool icp3D, std::string * rejectedMsg = 0, int * inliers = 0, double * variance = 0);
Transform computeIcpTransform(const Signature & oldS, const Signature & newS, Transform guess, bool icp3D, std::string * rejectedMsg = 0, int * inliers = 0, double * variance = 0) const;
Transform computeIcpTransform(int oldId, int newId, Transform guess, bool icp3D, std::string * rejectedMsg = 0, int * correspondences = 0, double * variance = 0, float * correspondencesRatio = 0);
Transform computeIcpTransform(const Signature & oldS, const Signature & newS, Transform guess, bool icp3D, std::string * rejectedMsg = 0, int * correspondences = 0, double * variance = 0, float * correspondencesRatio = 0) const;
Transform computeScanMatchingTransform(
int newId,
int oldId,
corelib/include/rtabmap/core/Parameters.h
+4 -3
View File
@@ -299,6 +299,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(RGBD, LocalLoopDetectionSpace, bool, false, "Detection over locations (in Working Memory or STM) near in space.");
RTABMAP_PARAM(RGBD, LocalLoopDetectionMaxDiffID, int, 50, "Maximum ID difference between the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore.");
RTABMAP_PARAM(RGBD, LocalLoopDetectionPathFilteringRadius, float, 0.25, "Path filtering radius.");
RTABMAP_PARAM(RGBD, LocalLoopDetectionPathOdomPosesUsed, bool, true, "When comparing to a local path, merge the scan using the odometry poses instead of the ones in the optimized local graph.");
// Graph optimization
RTABMAP_PARAM(RGBD, OptimizeStrategy, int, 0, "Graph optimization strategy: 0=TORO and 1=g2o.");
@@ -373,10 +374,10 @@ class RTABMAP_EXP Parameters
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, "Max distance for point correspondences.");
RTABMAP_PARAM(LccIcp2, MaxCorrespondenceDistance, float, 0.05, "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.05, "Voxel size to be used for ICP computation.");
RTABMAP_PARAM(LccIcp2, CorrespondenceRatio, float, 0.3, "Ratio of matching correspondences to accept the transform.");
RTABMAP_PARAM(LccIcp2, VoxelSize, float, 0.025, "Voxel size to be used for ICP computation.");
// Stereo disparity
RTABMAP_PARAM(Stereo, WinSize, int, 16, "See cv::calcOpticalFlowPyrLK().");
corelib/include/rtabmap/core/Rtabmap.h
+6
View File
@@ -149,6 +149,11 @@ private:
bool lookInDatabase,
std::map<int, Transform> & optimizedPoses,
std::multimap<int, Link> * constraints = 0) const;
std::map<int, Transform> optimizeGraph(
int fromId,
const std::vector<int> & ids,
bool lookInDatabase,
std::multimap<int, Link> * constraints = 0) const;
void updateGoalIndex();
bool computePath(int targetNode, const std::map<int, Transform> & nodes, const std::multimap<int, rtabmap::Link> & constraints);
@@ -181,6 +186,7 @@ private:
float _localRadius;
int _localDetectMaxDiffID;
float _localPathFilteringRadius;
bool _localPathOdomPosesUsed;
std::string _databasePath;
bool _optimizeFromGraphEnd;
bool _reextractLoopClosureFeatures;
corelib/include/rtabmap/core/SensorData.h
+3
View File
@@ -62,6 +62,7 @@ public:
// Metric constructor + 2d laser scan
SensorData(const cv::Mat & laserScan,
int laserScanMaxPts,
const cv::Mat & image,
const cv::Mat & depthOrRightImage,
float fx,
@@ -95,6 +96,7 @@ public:
cv::Mat rightImage() const {return _depthOrRightImage.type()==CV_8UC1?_depthOrRightImage:cv::Mat();}
const cv::Mat & depthOrRightImage() const {return _depthOrRightImage;}
const cv::Mat & laserScan() const {return _laserScan;}
int laserScanMaxPts() const {return _laserScanMaxPts;}
float fx() const {return _fx;}
float fy() const {return (_depthOrRightImage.type()==CV_8UC1)?0:_fyOrBaseline;}
float cx() const {return _cx;}
@@ -133,6 +135,7 @@ private:
Transform _localTransform;
float _poseRotVariance;
float _poseTransVariance;
int _laserScanMaxPts;
// features
std::vector<cv::KeyPoint> _keypoints;
corelib/include/rtabmap/core/Signature.h
+6 -3
View File
@@ -68,7 +68,8 @@ public:
float fy = 0.0f,
float cx = 0.0f,
float cy = 0.0f,
const Transform & localTransform =Transform::getIdentity());
const Transform & localTransform =Transform::getIdentity(),
int laserScanMaxPts = 0);
virtual ~Signature();
/**
@@ -128,7 +129,7 @@ public:
//metric stuff
void setWords3(const std::multimap<int, pcl::PointXYZ> & words3) {_words3 = words3;}
void setDepthCompressed(const cv::Mat & bytes, float fx, float fy, float cx, float cy);
void setLaserScanCompressed(const cv::Mat & bytes) {_laserScanCompressed = bytes;}
void setLaserScanCompressed(const cv::Mat & bytes, int maxPts = 0) {_laserScanCompressed = bytes; _laserScanMaxPts=maxPts;}
void setLocalTransform(const Transform & t) {_localTransform = t;}
void setPose(const Transform & pose) {_pose = pose;}
const std::multimap<int, pcl::PointXYZ> & getWords3() const {return _words3;}
@@ -146,8 +147,9 @@ public:
const Transform & getLocalTransform() const {return _localTransform;}
void setDepthRaw(const cv::Mat & depth) {_depthRaw = depth;}
const cv::Mat & getDepthRaw() const {return _depthRaw;}
void setLaserScanRaw(const cv::Mat & depth2D) {_laserScanRaw = depth2D;}
void setLaserScanRaw(const cv::Mat & depth2D, int maxPts = 0) {_laserScanRaw = depth2D; _laserScanMaxPts=maxPts;}
const cv::Mat & getLaserScanRaw() const {return _laserScanRaw;}
int getLaserScanMaxPts() const {return _laserScanMaxPts;}
SensorData toSensorData();
void uncompressData();
@@ -183,6 +185,7 @@ private:
Transform _pose;
Transform _localTransform; // camera_link -> base_link
std::multimap<int, pcl::PointXYZ> _words3; // word <id, keypoint>
int _laserScanMaxPts;
cv::Mat _imageRaw; // CV_8UC1 or CV_8UC3
cv::Mat _depthRaw; // depth CV_16UC1 or CV_32FC1, right image CV_8UC1
corelib/include/rtabmap/core/Statistics.h
+5 -1
View File
@@ -63,13 +63,17 @@ class RTABMAP_EXP Statistics
RTABMAP_STATS(Loop, VisualInliers,);
RTABMAP_STATS(Loop, Last_id,);
RTABMAP_STATS(LocalLoop, Odom_corrected,);
RTABMAP_STATS(LocalLoop, Time_closures,);
RTABMAP_STATS(LocalLoop, Space_last_closure_id,);
RTABMAP_STATS(LocalLoop, Space_paths,);
RTABMAP_STATS(LocalLoop, Space_closures_added,);
RTABMAP_STATS(LocalLoop, Space_closures_added_icp_only,);
RTABMAP_STATS(OdomCorrection, Accepted,);
RTABMAP_STATS(OdomCorrection, Inliers,);
RTABMAP_STATS(OdomCorrection, Inliers_ratio,);
RTABMAP_STATS(OdomCorrection, Variance,);
RTABMAP_STATS(Memory, Working_memory_size,);
RTABMAP_STATS(Memory, Short_time_memory_size,);
RTABMAP_STATS(Memory, Signatures_removed,);
corelib/include/rtabmap/core/util3d.h
+3 -3
View File
@@ -286,7 +286,7 @@ Transform RTABMAP_EXP icp(
int maximumIterations,
bool * hasConverged = 0,
double * variance = 0,
int * inliers = 0);
int * correspondences = 0);
Transform RTABMAP_EXP icpPointToPlane(
const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloud_source,
@@ -295,7 +295,7 @@ Transform RTABMAP_EXP icpPointToPlane(
int maximumIterations,
bool * hasConverged = 0,
double * variance = 0,
int * inliers = 0);
int * correspondences = 0);
Transform RTABMAP_EXP icp2D(
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
@@ -304,7 +304,7 @@ Transform RTABMAP_EXP icp2D(
int maximumIterations,
bool * hasConverged = 0,
double * variance = 0,
int * inliers = 0);
int * correspondences = 0);
pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP computeNormals(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
corelib/src/DBDriver.cpp
+4 -2
View File
@@ -419,7 +419,8 @@ void DBDriver::getNodeData(
float & fy,
float & cx,
float & cy,
Transform & localTransform) const
Transform & localTransform,
int & laserScanMaxPts) const
{
bool found = false;
// look in the trash
@@ -437,6 +438,7 @@ void DBDriver::getNodeData(
cx = s->getCx();
cy = s->getCy();
localTransform = s->getLocalTransform();
laserScanMaxPts = s->getLaserScanMaxPts();
found = true;
}
}
@@ -445,7 +447,7 @@ void DBDriver::getNodeData(
if(!found)
{
_dbSafeAccessMutex.lock();
this->getNodeDataQuery(signatureId, imageCompressed, depthCompressed, laserScanCompressed, fx, fy, cx, cy, localTransform);
this->getNodeDataQuery(signatureId, imageCompressed, depthCompressed, laserScanCompressed, fx, fy, cx, cy, localTransform, laserScanMaxPts);
_dbSafeAccessMutex.unlock();
}
}
corelib/src/DBDriverSqlite3.cpp
+52 -9
View File
@@ -458,7 +458,17 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
if(loadMetricData)
{
if(uStrNumCmp(_version, "0.7.0") >= 0)
if(uStrNumCmp(_version, "0.8.11") >= 0)
{
query << "SELECT Image.data, "
"Depth.data, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.local_transform, Depth.data2d_max_pts, Depth.data2d "
<< "FROM Image "
<< "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
<< "ON Image.id = Depth.id "
<< "WHERE Image.id = ?"
<<";";
}
else if(uStrNumCmp(_version, "0.7.0") >= 0)
{
query << "SELECT Image.data, "
"Depth.data, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.local_transform, Depth.data2d "
@@ -553,14 +563,19 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
}
(*iter)->setLocalTransform(localTransform);
int laserScanMaxPts = 0;
if(uStrNumCmp(_version, "0.8.11") >= 0)
{
laserScanMaxPts = sqlite3_column_int(ppStmt, index++);
}
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
//Create the laserScan
if(dataSize>4 && data)
{
(*iter)->setLaserScanCompressed(cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone()); // depth2d
(*iter)->setLaserScanCompressed(cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(), laserScanMaxPts); // depth2d
}
}
rc = sqlite3_step(ppStmt); // next result...
@@ -588,7 +603,8 @@ void DBDriverSqlite3::getNodeDataQuery(
float & fy,
float & cx,
float & cy,
Transform & localTransform) const
Transform & localTransform,
int & laserScanMaxPts) const
{
if(_ppDb)
{
@@ -598,7 +614,17 @@ void DBDriverSqlite3::getNodeDataQuery(
sqlite3_stmt * ppStmt = 0;
std::stringstream query;
if(uStrNumCmp(_version, "0.7.0") >= 0)
if(uStrNumCmp(_version, "0.8.11") >= 0)
{
query << "SELECT Image.data, "
"Depth.data, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.local_transform, Depth.data2d_max_pts, Depth.data2d "
<< "FROM Image "
<< "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
<< "ON Image.id = Depth.id "
<< "WHERE Image.id = " << signatureId
<<";";
}
else if(uStrNumCmp(_version, "0.7.0") >= 0)
{
query << "SELECT Image.data, "
"Depth.data, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.local_transform, Depth.data2d "
@@ -675,6 +701,12 @@ void DBDriverSqlite3::getNodeDataQuery(
memcpy(localTransform.data(), data, dataSize);
}
laserScanMaxPts = 0;
if(uStrNumCmp(_version, "0.8.11") >= 0)
{
laserScanMaxPts = sqlite3_column_int(ppStmt, index++);
}
data = sqlite3_column_blob(ppStmt, index); // depth2d
dataSize = sqlite3_column_bytes(ppStmt, index++);
//Create the depth2d
@@ -1255,7 +1287,7 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
if(visualWords.size()==0)
{
UINFO("Empty signature detected! (id=%d)", (*iter)->id());
UDEBUG("Empty signature detected! (id=%d)", (*iter)->id());
}
else
{
@@ -2044,7 +2076,7 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures) const
//metric
if(!(*i)->getDepthCompressed().empty() || !(*i)->getLaserScanCompressed().empty())
{
stepDepth(ppStmt, (*i)->id(), (*i)->getDepthCompressed(), (*i)->getLaserScanCompressed(), (*i)->getFx(), (*i)->getFy(), (*i)->getCx(), (*i)->getCy(), (*i)->getLocalTransform());
stepDepth(ppStmt, (*i)->id(), (*i)->getDepthCompressed(), (*i)->getLaserScanCompressed(), (*i)->getFx(), (*i)->getFy(), (*i)->getCx(), (*i)->getCy(), (*i)->getLocalTransform(), (*i)->getLaserScanMaxPts());
}
}
// Finalize (delete) the statement
@@ -2222,7 +2254,11 @@ void DBDriverSqlite3::stepImage(sqlite3_stmt * ppStmt,
std::string DBDriverSqlite3::queryStepDepth() const
{
if(uStrNumCmp(_version, "0.7.0") >= 0)
if(uStrNumCmp(_version, "0.8.11") >= 0)
{
return "INSERT INTO Depth(id, data, fx, fy, cx, cy, local_transform, data2d, data2d_max_pts) VALUES(?,?,?,?,?,?,?,?,?);";
}
else if(uStrNumCmp(_version, "0.7.0") >= 0)
{
return "INSERT INTO Depth(id, data, fx, fy, cx, cy, local_transform, data2d) VALUES(?,?,?,?,?,?,?,?);";
}
@@ -2239,7 +2275,8 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt,
float fy,
float cx,
float cy,
const Transform & localTransform) const
const Transform & localTransform,
int depth2dMaxPts) const
{
UDEBUG("Save depth %d (size=%d) depth2d = %d", id, (int)depthBytes.cols, (int)depth2dBytes.cols);
if(!ppStmt)
@@ -2293,6 +2330,12 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt,
}
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
if(uStrNumCmp(_version, "0.8.11") >= 0)
{
rc = sqlite3_bind_int(ppStmt, index++, depth2dMaxPts);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
}
//step
rc=sqlite3_step(ppStmt);
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
corelib/src/DBDriverSqlite3.h
+4 -2
View File
@@ -80,7 +80,8 @@ private:
float & fy,
float & cx,
float & cy,
Transform & localTransform) const;
Transform & localTransform,
int & laserScanMaxPts) const;
virtual void getNodeDataQuery(int signatureId, cv::Mat & imageCompressed) const;
virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, std::vector<unsigned char> & userData) const;
virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren) const;
@@ -110,7 +111,8 @@ private:
float fy,
float cx,
float cy,
const Transform & localTransform) const;
const Transform & localTransform,
int depth2dMaxPts) const;
void stepLink(sqlite3_stmt * ppStmt, int fromId, int toId, Link::Type type, float rotVariance, float transVariance, const Transform & transform) const;
void stepWordsChanged(sqlite3_stmt * ppStmt, int signatureId, int oldWordId, int newWordId) const;
void stepKeypoint(sqlite3_stmt * ppStmt, int signatureId, int wordId, const cv::KeyPoint & kp, const pcl::PointXYZ & pt) const;
corelib/src/DBReader.cpp
+81 -51
View File
@@ -75,6 +75,7 @@ bool DBReader::init(int startIndex)
}
_ids.clear();
_currentId=_ids.end();
_previousStamp = 0;
if(!UFile::exists(_path))
{
@@ -118,10 +119,7 @@ bool DBReader::init(int startIndex)
void DBReader::setFrameRate(float frameRate)
{
if(frameRate >= 0.0f)
{
_frameRate = frameRate;
}
_frameRate = frameRate;
}
void DBReader::mainLoopBegin()
@@ -210,26 +208,6 @@ SensorData DBReader::getNextData()
SensorData data;
if(_dbDriver)
{
float frameRate = _frameRate;
if(frameRate>0.0f)
{
int sleepTime = (1000.0f/frameRate - 1000.0f*_timer.getElapsedTime());
if(sleepTime > 2)
{
uSleep(sleepTime-2);
}
// Add precision at the cost of a small overhead
while(_timer.getElapsedTime() < 1.0/double(frameRate)-0.000001)
{
//
}
double slept = _timer.getElapsedTime();
_timer.start();
UDEBUG("slept=%fs vs target=%fs", slept, 1.0/double(frameRate));
}
if(!this->isKilled() && _currentId != _ids.end())
{
cv::Mat imageBytes;
@@ -241,7 +219,8 @@ SensorData DBReader::getNextData()
float rotVariance = 1.0f;
float transVariance = 1.0f;
std::vector<unsigned char> userData;
_dbDriver->getNodeData(*_currentId, imageBytes, depthBytes, laserScanBytes, fx, fy, cx, cy, localTransform);
int laserScanMaxPts = 0;
_dbDriver->getNodeData(*_currentId, imageBytes, depthBytes, laserScanBytes, fx, fy, cx, cy, localTransform, laserScanMaxPts);
// info
int weight;
@@ -272,32 +251,83 @@ SensorData DBReader::getNextData()
UWARN("No image loaded from the database for id=%d!", *_currentId);
}
rtabmap::CompressionThread ctImage(imageBytes, true);
rtabmap::CompressionThread ctDepth(depthBytes, true);
rtabmap::CompressionThread ctLaserScan(laserScanBytes, false);
ctImage.start();
ctDepth.start();
ctLaserScan.start();
ctImage.join();
ctDepth.join();
ctLaserScan.join();
data = SensorData(
ctLaserScan.getUncompressedData(),
ctImage.getUncompressedData(),
ctDepth.getUncompressedData(),
fx,fy,cx,cy,
localTransform,
pose,
rotVariance,
transVariance,
seq,
stamp,
userData);
UDEBUG("Laser=%d RGB/Left=%d Depth=%d Right=%d",
data.laserScan().empty()?0:1,
data.image().empty()?0:1,
data.depth().empty()?0:1,
data.rightImage().empty()?0:1);
// Frame rate
if(_frameRate < 0.0f)
{
if(stamp == 0)
{
UERROR("The option to use database stamps is set (framerate<0), but there are no stamps saved in the database! Aborting...");
this->kill();
}
else if(_previousStamp > 0)
{
int sleepTime = 1000.0*(stamp-_previousStamp) - 1000.0*_timer.getElapsedTime();
if(sleepTime > 2)
{
uSleep(sleepTime-2);
}
// Add precision at the cost of a small overhead
while(_timer.getElapsedTime() < (stamp-_previousStamp)-0.000001)
{
//
}
double slept = _timer.getElapsedTime();
_timer.start();
UDEBUG("slept=%fs vs target=%fs", slept, stamp-_previousStamp);
}
_previousStamp = stamp;
}
else if(_frameRate>0.0f)
{
int sleepTime = (1000.0f/_frameRate - 1000.0f*_timer.getElapsedTime());
if(sleepTime > 2)
{
uSleep(sleepTime-2);
}
// Add precision at the cost of a small overhead
while(_timer.getElapsedTime() < 1.0/double(_frameRate)-0.000001)
{
//
}
double slept = _timer.getElapsedTime();
_timer.start();
UDEBUG("slept=%fs vs target=%fs", slept, 1.0/double(_frameRate));
}
if(!this->isKilled())
{
rtabmap::CompressionThread ctImage(imageBytes, true);
rtabmap::CompressionThread ctDepth(depthBytes, true);
rtabmap::CompressionThread ctLaserScan(laserScanBytes, false);
ctImage.start();
ctDepth.start();
ctLaserScan.start();
ctImage.join();
ctDepth.join();
ctLaserScan.join();
data = SensorData(
ctLaserScan.getUncompressedData(),
laserScanMaxPts,
ctImage.getUncompressedData(),
ctDepth.getUncompressedData(),
fx,fy,cx,cy,
localTransform,
pose,
rotVariance,
transVariance,
seq,
stamp,
userData);
UDEBUG("Laser=%d RGB/Left=%d Depth=%d Right=%d",
data.laserScan().empty()?0:1,
data.image().empty()?0:1,
data.depth().empty()?0:1,
data.rightImage().empty()?0:1);
}
}
}
else
corelib/src/Memory.cpp
+70 -28
View File
@@ -2006,7 +2006,8 @@ Transform Memory::computeIcpTransform(
bool icp3D,
std::string * rejectedMsg,
int * inliers,
double * variance)
double * variance,
float * inliersRatio)
{
Signature * oldS = this->_getSignature(oldId);
Signature * newS = this->_getSignature(newId);
@@ -2064,7 +2065,7 @@ Transform Memory::computeIcpTransform(
newS->uncompressData(0, 0, &tmp2);
}
t = computeIcpTransform(*oldS, *newS, guess, icp3D, rejectedMsg, inliers, variance);
t = computeIcpTransform(*oldS, *newS, guess, icp3D, rejectedMsg, inliers, variance, inliersRatio);
}
else
{
@@ -2085,8 +2086,9 @@ Transform Memory::computeIcpTransform(
Transform guess,
bool icp3D,
std::string * rejectedMsg,
int * inliers,
double * variance) const
int * correspondencesOut,
double * varianceOut,
float * correspondencesRatioOut) const
{
if(guess.isNull())
{
@@ -2145,6 +2147,7 @@ Transform Memory::computeIcpTransform(
Transform icpT;
int correspondences = 0;
float correspondencesRatio = -1.0f;
double variance = 1;
if(_icpPointToPlane)
{
pcl::PointCloud<pcl::PointNormal>::Ptr oldCloud = util3d::computeNormals(oldCloudXYZ, _icpPointToPlaneNormalNeighbors);
@@ -2161,7 +2164,7 @@ Transform Memory::computeIcpTransform(
_icpMaxCorrespondenceDistance,
_icpMaxIterations,
&hasConverged,
variance,
&variance,
&correspondences);
}
}
@@ -2172,23 +2175,31 @@ Transform Memory::computeIcpTransform(
_icpMaxCorrespondenceDistance,
_icpMaxIterations,
&hasConverged,
variance,
&variance,
&correspondences);
}
// verify if there are enough correspondences
correspondencesRatio = float(correspondences)/float(oldCloudXYZ->size()>newCloudXYZ->size()?oldCloudXYZ->size():newCloudXYZ->size());
correspondencesRatio = float(correspondences)/float(newS.getDepthRaw().total());
UDEBUG("%d->%d hasConverged=%s, variance=%f, correspondences=%d/%d (%f%%)",
hasConverged?"true":"false",
variance?*variance:-1,
variance,
correspondences,
(int)(oldCloudXYZ->size()>newCloudXYZ->size()?oldCloudXYZ->size():newCloudXYZ->size()),
correspondencesRatio*100.0f);
if(inliers)
if(varianceOut)
{
*inliers = correspondences;
*varianceOut = variance;
}
if(correspondencesOut)
{
*correspondencesOut = correspondences;
}
if(correspondencesRatioOut)
{
*correspondencesRatioOut = correspondencesRatio;
}
if(!icpT.isNull() && hasConverged &&
@@ -2217,8 +2228,8 @@ Transform Memory::computeIcpTransform(
}
else
{
msg = uFormat("Cannot compute transform (converged=%s var=%f corrRatio=%f/%f)",
hasConverged?"true":"false", variance?*variance:-1, correspondencesRatio, _icpCorrespondenceRatio);
msg = uFormat("Cannot compute transform (converged=%s var=%f corr=%d corrRatio=%f/%f)",
hasConverged?"true":"false", variance, correspondences, correspondencesRatio, _icpCorrespondenceRatio);
UINFO(msg.c_str());
}
}
@@ -2267,21 +2278,30 @@ Transform Memory::computeIcpTransform(
bool hasConverged = false;
float correspondencesRatio = -1.0f;
int correspondences = 0;
double variance = 1;
icpT = util3d::icp2D(newCloud,
oldCloud,
_icp2MaxCorrespondenceDistance,
_icp2MaxIterations,
&hasConverged,
variance,
&variance,
&correspondences);
// verify if there are enough correspondences
correspondencesRatio = float(correspondences)/float(oldCloud->size()>newCloud->size()?oldCloud->size():newCloud->size());
if(newS.getLaserScanMaxPts())
{
correspondencesRatio = float(correspondences)/float(newS.getLaserScanMaxPts());
}
else
{
correspondencesRatio = float(correspondences)/float(oldCloud->size()>newCloud->size()?oldCloud->size():newCloud->size());
}
UDEBUG("%d->%d hasConverged=%s, variance=%f, correspondences=%d/%d (%f%%)",
newS.id(), oldS.id(),
hasConverged?"true":"false",
variance?*variance:-1,
variance,
correspondences,
(int)(oldCloud->size()>newCloud->size()?oldCloud->size():newCloud->size()),
correspondencesRatio*100.0f);
@@ -2296,12 +2316,22 @@ Transform Memory::computeIcpTransform(
// UWARN("saved newCloudFinal.pcd");
//}
if(inliers)
if(varianceOut)
{
*inliers = correspondences;
*varianceOut = variance;
}
if(correspondencesOut)
{
*correspondencesOut = correspondences;
}
if(correspondencesRatioOut)
{
*correspondencesRatioOut = correspondencesRatio;
}
if(!icpT.isNull() && hasConverged && correspondencesRatio >= _icp2CorrespondenceRatio)
if(!icpT.isNull() &&
hasConverged &&
correspondencesRatio >= _icp2CorrespondenceRatio)
{
float ix,iy,iz, iroll,ipitch,iyaw;
icpT.getTranslationAndEulerAngles(ix,iy,iz,iroll,ipitch,iyaw);
@@ -2326,8 +2356,8 @@ Transform Memory::computeIcpTransform(
}
else
{
msg = uFormat("Cannot compute transform (converged=%s var=%f corrRatio=%f/%f)",
hasConverged?"true":"false", variance?*variance:-1, correspondencesRatio, _icp2CorrespondenceRatio);
msg = uFormat("Cannot compute transform (converged=%s var=%f cor=%d corrRatio=%f/%f)",
hasConverged?"true":"false", variance, correspondences, correspondencesRatio, _icp2CorrespondenceRatio);
UINFO(msg.c_str());
}
}
@@ -2362,6 +2392,9 @@ Transform Memory::computeScanMatchingTransform(
int * inliers,
double * variance)
{
UASSERT(uContains(poses, newId) && uContains(_signatures, newId));
UASSERT(uContains(poses, oldId) && uContains(_signatures, oldId));
// make sure that all depth2D are loaded
std::list<Signature*> depthToLoad;
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
@@ -2407,7 +2440,6 @@ Transform Memory::computeScanMatchingTransform(
// get the new cloud
Signature * newS = _getSignature(newId);
pcl::PointCloud<pcl::PointXYZ>::Ptr newCloud;
UASSERT(uContains(poses, newId));
cv::Mat newScan;
newS->uncompressData(0, 0, &newScan);
newCloud = util3d::cvMat2Cloud(newScan, poses.at(newId));
@@ -2452,10 +2484,10 @@ Transform Memory::computeScanMatchingTransform(
{
transform = poses.at(newId).inverse()*icpT.inverse() * poses.at(oldId);
//pcl::io::savePCDFile("old.pcd", *assembledOldClouds);
//pcl::io::savePCDFile("new.pcd", *newCloud);
//pcl::io::savePCDFile("old.pcd", *assembledOldClouds, true);
//pcl::io::savePCDFile("new.pcd", *newCloud, true);
//newCloud = util3d::transformPointCloud<pcl::PointXYZ>(newCloud, icpT);
//pcl::io::savePCDFile("newFinal.pcd", *newCloud);
//pcl::io::savePCDFile("newFinal.pcd", *newCloud, true);
//UWARN("local scan matching old.pcd, new.pcd and newFinal.pcd saved!");
}
else
@@ -3322,11 +3354,12 @@ void Memory::copyData(const Signature * from, Signature * to)
cv::Mat laserScan;
float fx, fy, cx, cy;
Transform localTransform;
_dbDriver->getNodeData(from->id(), image, depth, laserScan, fx, fy, cx, cy, localTransform);
int laserScanMaxPts = 0;
_dbDriver->getNodeData(from->id(), image, depth, laserScan, fx, fy, cx, cy, localTransform, laserScanMaxPts);
to->setImageCompressed(image);
to->setDepthCompressed(depth, fx, fy, cx, cy);
to->setLaserScanCompressed(laserScan);
to->setLaserScanCompressed(laserScan, laserScanMaxPts);
to->setLocalTransform(localTransform);
UDEBUG("Loaded image data from database");
@@ -3872,7 +3905,8 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
fyOrBaseline,
cx,
cy,
data.localTransform());
data.localTransform(),
data.laserScanMaxPts());
}
else
{
@@ -3885,7 +3919,15 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
words3D,
data.pose(),
data.userData(),
rtabmap::compressData2(laserScan));
rtabmap::compressData2(laserScan),
cv::Mat(),
cv::Mat(),
0,
0,
0,
0,
Transform(),
data.laserScanMaxPts());
}
if(this->isRawDataKept())
{
corelib/src/Rtabmap.cpp
+67 -35
View File
@@ -98,6 +98,7 @@ Rtabmap::Rtabmap() :
_localRadius(Parameters::defaultRGBDLocalRadius()),
_localDetectMaxDiffID(Parameters::defaultRGBDLocalLoopDetectionMaxDiffID()),
_localPathFilteringRadius(Parameters::defaultRGBDLocalLoopDetectionPathFilteringRadius()),
_localPathOdomPosesUsed(Parameters::defaultRGBDLocalLoopDetectionPathOdomPosesUsed()),
_databasePath(""),
_optimizeFromGraphEnd(Parameters::defaultRGBDOptimizeFromGraphEnd()),
_reextractLoopClosureFeatures(Parameters::defaultLccReextractActivated()),
@@ -386,6 +387,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kRGBDLocalRadius(), _localRadius);
Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionMaxDiffID(), _localDetectMaxDiffID);
Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionPathFilteringRadius(), _localPathFilteringRadius);
Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionPathOdomPosesUsed(), _localPathOdomPosesUsed);
Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), _optimizeFromGraphEnd);
Parameters::parse(parameters, Parameters::kLccReextractActivated(), _reextractLoopClosureFeatures);
Parameters::parse(parameters, Parameters::kLccReextractNNType(), _reextractNNType);
@@ -951,7 +953,9 @@ bool Rtabmap::process(const SensorData & data)
std::string rejectedMsg;
Transform guess = signature->getLinks().begin()->second.transform();
double variance = -1.0;
Transform t = _memory->computeIcpTransform(oldId, signature->id(), guess, false, &rejectedMsg, 0, &variance);
int inliers = 0;
float inliersRatio = 0;
Transform t = _memory->computeIcpTransform(oldId, signature->id(), guess, false, &rejectedMsg, &inliers, &variance, &inliersRatio);
if(!t.isNull())
{
scanMatchingSuccess = true;
@@ -966,6 +970,10 @@ bool Rtabmap::process(const SensorData & data)
{
UINFO("Scan matching rejected: %s", rejectedMsg.c_str());
}
statistics_.addStatistic(Statistics::kOdomCorrectionAccepted(), scanMatchingSuccess?1.0f:0);
statistics_.addStatistic(Statistics::kOdomCorrectionInliers(), inliers);
statistics_.addStatistic(Statistics::kOdomCorrectionInliers_ratio(), inliersRatio);
statistics_.addStatistic(Statistics::kOdomCorrectionVariance(), variance);
}
timeScanMatching = timer.ticks();
ULOGGER_INFO("timeScanMatching=%fs", timeScanMatching);
@@ -1554,17 +1562,6 @@ bool Rtabmap::process(const SensorData & data)
if(_localPathFilteringRadius <= 0.0f ||
_optimizedPoses.at(signature->id()).getDistance(_optimizedPoses.at(nearestId)) < _localPathFilteringRadius)
{
// path filtering
if(_localPathFilteringRadius > 0.0f)
{
std::map<int, Transform> filteredPath = graph::radiusPosesFiltering(path, _localPathFilteringRadius, CV_PI, true);
// make sure the nearest and farthest poses are still here
filteredPath.insert(*_optimizedPoses.find(nearestId));
filteredPath.insert(*path.begin());
filteredPath.insert(*path.rbegin());
path = filteredPath;
}
// 1) look for loop closures based on visual correspondences
double variance = 1.0;
Transform transform = _memory->computeVisualTransform(nearestId, signature->id(), 0, 0, &variance);
@@ -1576,9 +1573,31 @@ bool Rtabmap::process(const SensorData & data)
}
if(transform.isNull())
{
// 2) Assemble scans in the path and do ICP only
if(_localPathOdomPosesUsed)
{
//optimize the path's poses locally
path = optimizeGraph(nearestId, uKeys(path), false);
// transform local poses in optimized graph referential
Transform t = _optimizedPoses.at(nearestId) * path.at(nearestId).inverse();
for(std::map<int, Transform>::iterator jter=path.begin(); jter!=path.end(); ++jter)
{
jter->second = t * jter->second;
}
}
if(_localPathFilteringRadius > 0.0f)
{
// path filtering
std::map<int, Transform> filteredPath = graph::radiusPosesFiltering(path, _localPathFilteringRadius, CV_PI, true);
// make sure the nearest and farthest poses are still here
filteredPath.insert(*path.find(nearestId));
filteredPath.insert(*path.begin());
filteredPath.insert(*path.rbegin());
path = filteredPath;
}
if(path.size() > 2) // more than current+nearest
{
// 2) Assemble scans in the path and do ICP only
// add current node to poses
path.insert(std::make_pair(signature->id(), _optimizedPoses.at(signature->id())));
//The nearest will be the reference for a loop closure transform
@@ -1742,7 +1761,6 @@ bool Rtabmap::process(const SensorData & data)
statistics_.addStatistic(Statistics::kLoopVisualInliers(), loopClosureVisualInliers);
statistics_.addStatistic(Statistics::kLoopLast_id(), _memory->getLastGlobalLoopClosureId());
statistics_.addStatistic(Statistics::kLocalLoopOdom_corrected(), scanMatchingSuccess?1:0);
statistics_.addStatistic(Statistics::kLocalLoopTime_closures(), localLoopClosuresInTimeFound);
statistics_.addStatistic(Statistics::kLocalLoopSpace_closures_added(), localSpaceClosuresAdded);
statistics_.addStatistic(Statistics::kLocalLoopSpace_closures_added_icp_only(), localSpaceClosuresAddedByICPOnly);
@@ -2295,36 +2313,50 @@ void Rtabmap::optimizeCurrentMap(
}
UINFO("get ids time %f s", timer.ticks());
std::map<int, Transform> poses;
std::multimap<int, Link> edgeConstraints;
_memory->getMetricConstraints(uKeys(ids), poses, edgeConstraints, lookInDatabase);
UINFO("get constraints (%d poses, %d edges) time %f s", (int)poses.size(), (int)edgeConstraints.size(), timer.ticks());
if(constraints)
{
*constraints = edgeConstraints;
}
UASSERT(_graphOptimizer!=0);
if(_graphOptimizer->iterations() == 0)
{
// Optimization desactivated! Return not optimized poses.
optimizedPoses = poses;
}
else
{
optimizedPoses = _graphOptimizer->optimize(id, poses, edgeConstraints);
}
UINFO("optimize time %f s", timer.ticks());
optimizedPoses = Rtabmap::optimizeGraph(id, uKeys(ids), lookInDatabase, constraints);
if(_memory->getSignature(id) && uContains(optimizedPoses, id))
{
Transform t = optimizedPoses.at(id) * _memory->getSignature(id)->getPose().inverse();
UINFO("Correction (from node %d) %s", id, t.prettyPrint().c_str());
}
UINFO("optimize time %f s", timer.ticks());
}
}
std::map<int, Transform> Rtabmap::optimizeGraph(
int fromId,
const std::vector<int> & ids,
bool lookInDatabase,
std::multimap<int, Link> * constraints) const
{
UTimer timer;
std::map<int, Transform> optimizedPoses;
std::map<int, Transform> poses;
std::multimap<int, Link> edgeConstraints;
UDEBUG("ids=%d", (int)ids.size());
_memory->getMetricConstraints(ids, poses, edgeConstraints, lookInDatabase);
UDEBUG("get constraints (%d poses, %d edges) time %f s", (int)poses.size(), (int)edgeConstraints.size(), timer.ticks());
if(constraints)
{
*constraints = edgeConstraints;
}
UASSERT(_graphOptimizer!=0);
if(_graphOptimizer->iterations() == 0)
{
// Optimization desactivated! Return not optimized poses.
optimizedPoses = poses;
}
else
{
optimizedPoses = _graphOptimizer->optimize(fromId, poses, edgeConstraints);
}
return optimizedPoses;
}
void Rtabmap::adjustLikelihood(std::map<int, float> & likelihood) const
{
ULOGGER_DEBUG("likelihood.size()=%d", likelihood.size());
corelib/src/SensorData.cpp
+6 -1
View File
@@ -45,7 +45,8 @@ SensorData::SensorData() :
_cy(0.0f),
_localTransform(Transform::getIdentity()),
_poseRotVariance(1.0f),
_poseTransVariance(1.0f)
_poseTransVariance(1.0f),
_laserScanMaxPts(0)
{
}
@@ -63,6 +64,7 @@ SensorData::SensorData(const cv::Mat & image,
_localTransform(Transform::getIdentity()),
_poseRotVariance(1.0f),
_poseTransVariance(1.0f),
_laserScanMaxPts(0),
_userData(userData)
{
UASSERT(image.type() == CV_8UC1 || // Mono
@@ -95,6 +97,7 @@ SensorData::SensorData(const cv::Mat & image,
_localTransform(localTransform),
_poseRotVariance(poseRotVariance),
_poseTransVariance(poseTransVariance),
_laserScanMaxPts(0),
_userData(userData)
{
UASSERT(image.type() == CV_8UC1 || // Mono
@@ -109,6 +112,7 @@ SensorData::SensorData(const cv::Mat & image,
// Metric constructor + 2d depth
SensorData::SensorData(const cv::Mat & laserScan,
int laserScanMaxPts,
const cv::Mat & image,
const cv::Mat & depthOrRightImage,
float fx,
@@ -135,6 +139,7 @@ SensorData::SensorData(const cv::Mat & laserScan,
_localTransform(localTransform),
_poseRotVariance(poseRotVariance),
_poseTransVariance(poseTransVariance),
_laserScanMaxPts(laserScanMaxPts),
_userData(userData)
{
UASSERT(_laserScan.empty() || _laserScan.type() == CV_32FC2);
corelib/src/Signature.cpp
+7 -3
View File
@@ -48,7 +48,8 @@ Signature::Signature() :
_fx(0.0f),
_fy(0.0f),
_cx(0.0f),
_cy(0.0f)
_cy(0.0f),
_laserScanMaxPts(0)
{
}
@@ -69,7 +70,8 @@ Signature::Signature(
float fy,
float cx,
float cy,
const Transform & localTransform) :
const Transform & localTransform,
int laserScanMaxPts) :
_id(id),
_mapId(mapId),
_stamp(stamp),
@@ -90,7 +92,8 @@ Signature::Signature(
_cy(cy),
_pose(pose),
_localTransform(localTransform),
_words3(words3)
_words3(words3),
_laserScanMaxPts(laserScanMaxPts)
{
}
@@ -273,6 +276,7 @@ SensorData Signature::toSensorData()
}
}
return SensorData(_laserScanRaw,
_laserScanMaxPts,
_imageRaw,
_depthRaw,
_fx,
corelib/src/resources/DatabaseSchema.sql.in
+1
View File
@@ -42,6 +42,7 @@ CREATE TABLE Depth (
cy FLOAT,
local_transform BLOB,
data2d BLOB, -- compressed data (Laser scan)
data2d_max_pts INTEGER, -- Laser scan max points
time_enter DATE,
PRIMARY KEY (id)
);
corelib/src/util3d.cpp
+18 -18
View File
@@ -1933,7 +1933,7 @@ Transform icp(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
int maximumIterations,
bool * hasConvergedOut,
double * variance,
int * inliers)
int * correspondencesOut)
{
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
// Set the input source and target
@@ -1956,7 +1956,7 @@ Transform icp(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
bool hasConverged = icp.hasConverged();
// compute variance
if((inliers || variance) && hasConverged)
if((correspondencesOut || variance) && hasConverged)
{
pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>::Ptr est;
est.reset(new pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>);
@@ -1986,16 +1986,16 @@ Transform icp(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
}
}
if(inliers)
if(correspondencesOut)
{
*inliers = (int)correspondences.size();
*correspondencesOut = (int)correspondences.size();
}
}
else
{
if(inliers)
if(correspondencesOut)
{
*inliers = 0;
*correspondencesOut = 0;
}
if(variance)
{
@@ -2019,7 +2019,7 @@ Transform icpPointToPlane(
int maximumIterations,
bool * hasConvergedOut,
double * variance,
int * inliers)
int * correspondencesOut)
{
pcl::IterativeClosestPoint<pcl::PointNormal, pcl::PointNormal> icp;
// Set the input source and target
@@ -2046,7 +2046,7 @@ Transform icpPointToPlane(
bool hasConverged = icp.hasConverged();
// compute variance
if((inliers || variance) && hasConverged)
if((correspondencesOut || variance) && hasConverged)
{
pcl::registration::CorrespondenceEstimation<pcl::PointNormal, pcl::PointNormal>::Ptr est;
est.reset(new pcl::registration::CorrespondenceEstimation<pcl::PointNormal, pcl::PointNormal>);
@@ -2076,16 +2076,16 @@ Transform icpPointToPlane(
}
}
if(inliers)
if(correspondencesOut)
{
*inliers = (int)correspondences.size();
*correspondencesOut = (int)correspondences.size();
}
}
else
{
if(inliers)
if(correspondencesOut)
{
*inliers = 0;
*correspondencesOut = 0;
}
if(variance)
{
@@ -2108,7 +2108,7 @@ Transform icp2D(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
int maximumIterations,
bool * hasConvergedOut,
double * variance,
int * inliers)
int * correspondencesOut)
{
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
// Set the input source and target
@@ -2135,7 +2135,7 @@ Transform icp2D(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
bool hasConverged = icp.hasConverged();
// compute variance
if((inliers || variance) && hasConverged)
if((correspondencesOut || variance) && hasConverged)
{
pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>::Ptr est;
est.reset(new pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>);
@@ -2165,16 +2165,16 @@ Transform icp2D(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
}
}
if(inliers)
if(correspondencesOut)
{
*inliers = (int)correspondences.size();
*correspondencesOut = (int)correspondences.size();
}
}
else
{
if(inliers)
if(correspondencesOut)
{
*inliers = 0;
*correspondencesOut = 0;
}
if(variance)
{
guilib/include/rtabmap/gui/PreferencesDialog.h
+1
View File
@@ -178,6 +178,7 @@ public:
bool getSourceDatabaseOdometryIgnored() const; //Database group
bool getSourceDatabaseGoalDelayIgnored() const; //Database group
int getSourceDatabaseStartPos() const; //Database group
bool getSourceDatabaseStampsUsed() const;//Database group
Src getSourceRGBD() const; // Openni group
bool getSourceOpenni2AutoWhiteBalance() const; //Openni group
bool getSourceOpenni2AutoExposure() const; //Openni group
guilib/src/DatabaseViewer.cpp
+41 -27
View File
@@ -671,44 +671,47 @@ void DatabaseViewer::exportDatabase()
if(!dialog.outputPath().isEmpty())
{
int framesIgnored = dialog.framesIgnored();
double frameRate = dialog.targetFramerate();
int sessionExported = dialog.sessionExported();
QString path = dialog.outputPath();
rtabmap::DataRecorder recorder;
QList<int> ids;
if(sessionExported < 0)
double previousStamp = 0;
for(int i=0; i<ids_.size(); i+=1+framesIgnored)
{
ids = ids_;
}
else
{
for(int i=0; i<ids_.size(); ++i)
Transform odomPose;
int weight = -1;
int mapId = -1;
std::string label;
double stamp = 0;
std::vector<unsigned char> userData;
if(memory_->getNodeInfo(ids_[i], odomPose, mapId, weight, label, stamp, userData, true))
{
Transform odomPose;
int weight = -1;
int mapId = -1;
std::string label;
double stamp = 0;
std::vector<unsigned char> userData;
if(memory_->getNodeInfo(ids_[i], odomPose, mapId, weight, label, stamp, userData, true))
if(frameRate == 0 ||
previousStamp == 0 ||
stamp == 0 ||
stamp - previousStamp >= 1.0/frameRate)
{
if(sessionExported == mapId)
if(sessionExported < 0 || sessionExported == mapId)
{
ids.push_back(ids_[i]);
}
else if(mapId > sessionExported)
{
break;
}
previousStamp = stamp;
}
if(sessionExported >= 0 && mapId > sessionExported)
{
break;
}
}
}
if(recorder.init(path, false))
{
rtabmap::DetailedProgressDialog progressDialog(this);
progressDialog.setMaximumSteps(ids.size() / (1+framesIgnored) + 1);
progressDialog.setMaximumSteps(ids.size());
progressDialog.show();
for(int i=0; i<ids.size(); i+=1+framesIgnored)
for(int i=0; i<ids.size(); ++i)
{
int id = ids.at(i);
@@ -1465,6 +1468,11 @@ void DatabaseViewer::update(int value,
1);
}
view3D->addOrUpdateCloud("0", cloud, data.getLocalTransform());
//add scan
pcl::PointCloud<pcl::PointXYZ>::Ptr scan = util3d::laserScanToPointCloud(data.getLaserScanRaw());
view3D->addOrUpdateCloud("1", scan);
view3D->update();
}
}
@@ -1842,13 +1850,16 @@ void DatabaseViewer::sliderLoopValueChanged(int value)
void DatabaseViewer::updateConstraintView()
{
Link link = this->findActiveLink(ui_->horizontalSlider_A->value(), ui_->horizontalSlider_B->value());
if(link.type() == Link::kNeighbor)
if(link.isValid())
{
this->updateConstraintView(neighborLinks_.at(ui_->horizontalSlider_neighbors->value()), false);
}
else
{
this->updateConstraintView(loopLinks_.at(ui_->horizontalSlider_loops->value()), false);
if(link.type() == Link::kNeighbor)
{
this->updateConstraintView(neighborLinks_.at(ui_->horizontalSlider_neighbors->value()), false);
}
else
{
this->updateConstraintView(loopLinks_.at(ui_->horizontalSlider_loops->value()), false);
}
}
}
@@ -2042,7 +2053,10 @@ void DatabaseViewer::updateConstraintView(
if(cloudTo->size())
{
cloudTo = rtabmap::util3d::removeNaNFromPointCloud<pcl::PointXYZ>(cloudTo);
cloudTo = rtabmap::util3d::transformPointCloud<pcl::PointXYZ>(cloudTo, t);
if(cloudTo->size())
{
cloudTo = rtabmap::util3d::transformPointCloud<pcl::PointXYZ>(cloudTo, t);
}
}
if(cloudFrom->size())
guilib/src/ExportDialog.cpp
+63 -1
View File
@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "ui_exportDialog.h"
#include <QFileDialog>
#include <QPushButton>
namespace rtabmap {
@@ -40,7 +41,11 @@ ExportDialog::ExportDialog(QWidget * parent) :
connect(_ui->toolButton_path, SIGNAL(clicked()), this, SLOT(getPath()));
restoreDefaults();
connect(_ui->buttonBox->button(QDialogButtonBox::RestoreDefaults), SIGNAL(clicked()), this, SLOT(restoreDefaults()));
connect(_ui->spinBox_ignored, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
connect(_ui->doubleSpinBox_framerate, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
connect(_ui->spinBox_session, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
connect(_ui->checkBox_rgb, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
connect(_ui->checkBox_depth, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
@@ -48,7 +53,7 @@ ExportDialog::ExportDialog(QWidget * parent) :
connect(_ui->checkBox_odom, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
connect(_ui->checkBox_userData, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
_ui->lineEdit_path->setText(QDir::homePath()+QDir::separator()+"output.db");
_ui->lineEdit_path->setText(QDir::currentPath()+QDir::separator()+"output.db");
}
ExportDialog::~ExportDialog()
@@ -56,6 +61,58 @@ ExportDialog::~ExportDialog()
delete _ui;
}
void ExportDialog::saveSettings(QSettings & settings, const QString & group) const
{
if(!group.isEmpty())
{
settings.beginGroup(group);
}
settings.setValue("framesIgnored", this->framesIgnored());
settings.setValue("targetFramerate", this->targetFramerate());
settings.setValue("sessionExported", this->sessionExported());
settings.setValue("rgbExported", this->isRgbExported());
settings.setValue("depthExported", this->isDepthExported());
settings.setValue("depth2dExported", this->isDepth2dExported());
settings.setValue("odomExported", this->isOdomExported());
settings.setValue("userDataExported", this->isUserDataExported());
if(!group.isEmpty())
{
settings.endGroup();
}
}
void ExportDialog::loadSettings(QSettings & settings, const QString & group)
{
if(!group.isEmpty())
{
settings.beginGroup(group);
}
_ui->spinBox_ignored->setValue(settings.value("framesIgnored", this->framesIgnored()).toInt());
_ui->doubleSpinBox_framerate->setValue(settings.value("targetFramerate", this->targetFramerate()).toDouble());
_ui->spinBox_session->setValue(settings.value("sessionExported", this->sessionExported()).toInt());
_ui->checkBox_rgb->setChecked(settings.value("rgbExported", this->isRgbExported()).toBool());
_ui->checkBox_depth->setChecked(settings.value("depthExported", this->isDepthExported()).toBool());
_ui->checkBox_depth2d->setChecked(settings.value("depth2dExported", this->isDepth2dExported()).toBool());
_ui->checkBox_odom->setChecked(settings.value("odomExported", this->isOdomExported()).toBool());
_ui->checkBox_userData->setChecked(settings.value("userDataExported", this->isUserDataExported()).toBool());
if(!group.isEmpty())
{
settings.endGroup();
}
}
void ExportDialog::restoreDefaults()
{
_ui->spinBox_ignored->setValue(0);
_ui->doubleSpinBox_framerate->setValue(0);
_ui->spinBox_session->setValue(-1);
_ui->checkBox_rgb->setChecked(true);
_ui->checkBox_depth->setChecked(true);
_ui->checkBox_depth2d->setChecked(true);
_ui->checkBox_odom->setChecked(true);
_ui->checkBox_userData->setChecked(false);
}
void ExportDialog::getPath()
{
QString path = QFileDialog::getSaveFileName(this, tr("Output database path..."), _ui->lineEdit_path->text(), tr("RTAB-Map database (*.db)"));
@@ -75,6 +132,11 @@ int ExportDialog::framesIgnored() const
return _ui->spinBox_ignored->value();
}
double ExportDialog::targetFramerate() const
{
return _ui->doubleSpinBox_framerate->value();
}
int ExportDialog::sessionExported() const
{
return _ui->spinBox_session->value();
guilib/src/ExportDialog.h
+6
View File
@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#define EXPORTDIALOG_H_
#include <QDialog>
#include <QSettings>
class Ui_ExportDialog;
@@ -43,8 +44,12 @@ public:
virtual ~ExportDialog();
void saveSettings(QSettings & settings, const QString & group) const;
void loadSettings(QSettings & settings, const QString & group);
QString outputPath() const;
int framesIgnored() const;
double targetFramerate() const;
int sessionExported() const;
bool isRgbExported() const;
bool isDepthExported() const;
@@ -57,6 +62,7 @@ signals:
private slots:
void getPath();
void restoreDefaults();
private:
Ui_ExportDialog * _ui;
guilib/src/MainWindow.cpp
+3 -3
View File
@@ -981,7 +981,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
int rehearsed = (int)uValue(stat.data(), Statistics::kMemoryRehearsal_merged(), 0.0f);
int localTimeClosures = (int)uValue(stat.data(), Statistics::kLocalLoopTime_closures(), 0.0f);
bool scanMatchingSuccess = (bool)uValue(stat.data(), Statistics::kLocalLoopOdom_corrected(), 0.0f);
bool scanMatchingSuccess = (bool)uValue(stat.data(), Statistics::kOdomCorrectionAccepted(), 0.0f);
_ui->label_matchId->clear();
_ui->label_stats_imageNumber->setText(QString("%1 [%2]").arg(stat.refImageId()).arg(refMapId));
@@ -2035,7 +2035,7 @@ void MainWindow::applyPrefSettings(PreferencesDialog::PANEL_FLAGS flags)
}
if(_dbReader)
{
_dbReader->setFrameRate(_preferencesDialog->getGeneralInputRate());
_dbReader->setFrameRate( _preferencesDialog->getSourceDatabaseStampsUsed()?-1:_preferencesDialog->getGeneralInputRate());
}
}//This will update the statistics toolbox
@@ -2735,7 +2735,7 @@ void MainWindow::startDetection()
else if(_preferencesDialog->isSourceDatabaseUsed())
{
_dbReader = new DBReader(_preferencesDialog->getSourceDatabasePath().toStdString(),
_preferencesDialog->getGeneralInputRate(),
_preferencesDialog->getSourceDatabaseStampsUsed()?-1:_preferencesDialog->getGeneralInputRate(),
_preferencesDialog->getSourceDatabaseOdometryIgnored(),
_preferencesDialog->getSourceDatabaseGoalDelayIgnored());
guilib/src/PreferencesDialog.cpp
+11
View File
@@ -299,6 +299,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
connect(_ui->source_checkBox_ignoreOdometry, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->source_checkBox_ignoreGoalDelay, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->source_spinBox_databaseStartPos, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->source_checkBox_useDbStamps, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
//openni group
connect(_ui->groupBox_sourceOpenni, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->comboBox_cameraRGBD, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
@@ -489,6 +491,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->localDetection_radius->setObjectName(Parameters::kRGBDLocalRadius().c_str());
_ui->localDetection_maxDiffID->setObjectName(Parameters::kRGBDLocalLoopDetectionMaxDiffID().c_str());
_ui->localDetection_pathFilteringRadius->setObjectName(Parameters::kRGBDLocalLoopDetectionPathFilteringRadius().c_str());
_ui->checkBox_localSpacePathOdomPosesUsed->setObjectName(Parameters::kRGBDLocalLoopDetectionPathOdomPosesUsed().c_str());
_ui->loopClosure_bowMinInliers->setObjectName(Parameters::kLccBowMinInliers().c_str());
_ui->loopClosure_bowInlierDistance->setObjectName(Parameters::kLccBowInlierDistance().c_str());
@@ -940,6 +943,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
_ui->source_checkBox_ignoreOdometry->setChecked(false);
_ui->source_checkBox_ignoreGoalDelay->setChecked(false);
_ui->source_spinBox_databaseStartPos->setValue(0);
_ui->source_checkBox_useDbStamps->setChecked(false);
_ui->groupBox_sourceOpenni->setChecked(true);
#ifdef _WIN32
@@ -1215,6 +1219,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
_ui->source_checkBox_ignoreOdometry->setChecked(settings.value("ignoreOdometry", _ui->source_checkBox_ignoreOdometry->isChecked()).toBool());
_ui->source_checkBox_ignoreGoalDelay->setChecked(settings.value("ignoreGoalDelay", _ui->source_checkBox_ignoreGoalDelay->isChecked()).toBool());
_ui->source_spinBox_databaseStartPos->setValue(settings.value("startPos", _ui->source_spinBox_databaseStartPos->value()).toInt());
_ui->source_checkBox_useDbStamps->setChecked(settings.value("useDatabaseStamps", _ui->source_checkBox_useDbStamps->isChecked()).toBool());
settings.endGroup(); // Database
settings.beginGroup("Openni");
@@ -1482,6 +1487,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
settings.setValue("ignoreOdometry", _ui->source_checkBox_ignoreOdometry->isChecked());
settings.setValue("ignoreGoalDelay", _ui->source_checkBox_ignoreGoalDelay->isChecked());
settings.setValue("startPos", _ui->source_spinBox_databaseStartPos->value());
settings.setValue("useDatabaseStamps", _ui->source_checkBox_useDbStamps->isChecked());
settings.endGroup();
settings.beginGroup("Openni");
@@ -2046,6 +2052,7 @@ void PreferencesDialog::selectSourceDatabase(bool user)
_ui->source_checkBox_ignoreGoalDelay->setChecked(false);
_ui->source_database_lineEdit_path->setText(path);
_ui->source_spinBox_databaseStartPos->setValue(0);
_ui->source_checkBox_useDbStamps->setChecked(false);
}
if(_ui->groupBox_sourceDatabase->isChecked())
@@ -3060,6 +3067,10 @@ int PreferencesDialog::getSourceDatabaseStartPos() const
{
return _ui->source_spinBox_databaseStartPos->value();
}
bool PreferencesDialog::getSourceDatabaseStampsUsed() const
{
return _ui->source_checkBox_useDbStamps->isChecked();
}
PreferencesDialog::Src PreferencesDialog::getSourceRGBD() const
{
guilib/src/ui/exportDialog.ui
+36 -2
View File
@@ -7,7 +7,7 @@
<x>0</x>
<y>0</y>
<width>382</width>
<height>291</height>
<height>331</height>
</rect>
</property>
<property name="windowTitle">
@@ -45,6 +45,40 @@
</item>
</layout>
</item>
<item>
<layout class="QHBoxLayout" name="horizontalLayout_4">
<item>
<widget class="QDoubleSpinBox" name="doubleSpinBox_framerate">
<property name="suffix">
<string> Hz</string>
</property>
<property name="decimals">
<number>1</number>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="label_3">
<property name="text">
<string>Target frame rate</string>
</property>
</widget>
</item>
<item>
<spacer name="horizontalSpacer_3">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
</layout>
</item>
<item>
<layout class="QHBoxLayout" name="horizontalLayout_3">
<item>
@@ -172,7 +206,7 @@
<enum>Qt::Horizontal</enum>
</property>
<property name="standardButtons">
<set>QDialogButtonBox::Cancel|QDialogButtonBox::Ok</set>
<set>QDialogButtonBox::Cancel|QDialogButtonBox::Ok|QDialogButtonBox::RestoreDefaults</set>
</property>
</widget>
</item>
guilib/src/ui/preferencesDialog.ui
+116 -76
View File
@@ -63,9 +63,9 @@
<property name="geometry">
<rect>
<x>0</x>
<y>-806</y>
<y>0</y>
<width>760</width>
<height>1462</height>
<height>1029</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>19</number>
<number>21</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29">
@@ -1603,6 +1603,20 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<bool>false</bool>
</property>
<layout class="QGridLayout" name="gridLayout_9">
<item row="0" column="2">
<widget class="QToolButton" name="toolButton_dbViewer">
<property name="toolTip">
<string>Open database viewer</string>
</property>
<property name="text">
<string/>
</property>
<property name="icon">
<iconset resource="../GuiLib.qrc">
<normaloff>:/images/mag_glass.png</normaloff>:/images/mag_glass.png</iconset>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QCheckBox" name="source_checkBox_ignoreOdometry">
<property name="text">
@@ -1620,14 +1634,14 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<item row="0" column="1">
<widget class="QLineEdit" name="source_database_lineEdit_path"/>
</item>
<item row="3" column="1">
<item row="4" column="1">
<widget class="QLabel" name="label_58">
<property name="text">
<string>Start position (index)</string>
</property>
</widget>
</item>
<item row="3" column="0">
<item row="4" column="0">
<widget class="QSpinBox" name="source_spinBox_databaseStartPos">
<property name="minimum">
<number>0</number>
@@ -1637,20 +1651,6 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="0" column="2">
<widget class="QToolButton" name="toolButton_dbViewer">
<property name="toolTip">
<string>Open database viewer</string>
</property>
<property name="text">
<string/>
</property>
<property name="icon">
<iconset resource="../GuiLib.qrc">
<normaloff>:/images/mag_glass.png</normaloff>:/images/mag_glass.png</iconset>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_72">
<property name="text">
@@ -1661,7 +1661,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="2" column="1">
<item row="3" column="1">
<widget class="QLabel" name="label_80">
<property name="text">
<string>Ignore goal delay.</string>
@@ -1671,13 +1671,30 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="2" column="0">
<item row="3" column="0">
<widget class="QCheckBox" name="source_checkBox_ignoreGoalDelay">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_90">
<property name="text">
<string>Use database stamps as input rate.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QCheckBox" name="source_checkBox_useDbStamps">
<property name="text">
<string/>
</property>
</widget>
</item>
</layout>
</widget>
</item>
@@ -5244,14 +5261,14 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="2" column="0">
<item row="3" column="0">
<widget class="QCheckBox" name="checkBox_localSpaceLinksKeptInWM">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="2" column="1">
<item row="3" column="1">
<widget class="QLabel" name="label_scanMatching_2">
<property name="text">
<string>If local space links are kept in WM.</string>
@@ -5271,6 +5288,23 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_scanMatching_4">
<property name="text">
<string>When comparing to a local path, merge the scan using the odometry poses instead of the ones in the optimized local graph.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QCheckBox" name="checkBox_localSpacePathOdomPosesUsed">
<property name="text">
<string/>
</property>
</widget>
</item>
</layout>
</item>
</layout>
@@ -5900,6 +5934,45 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
<string>ICP 3D (e.g. Kinect depth)</string>
</property>
<layout class="QGridLayout" name="gridLayout_53" columnstretch="0,1">
<item row="5" column="0">
<widget class="QSpinBox" name="loopClosure_icpIterations">
<property name="minimum">
<number>1</number>
</property>
<property name="maximum">
<number>1000</number>
</property>
<property name="value">
<number>10</number>
</property>
</widget>
</item>
<item row="5" column="1">
<widget class="QLabel" name="label_124">
<property name="text">
<string>Max iterations.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QSpinBox" name="loopClosure_icpSamples">
<property name="minimum">
<number>0</number>
</property>
<property name="maximum">
<number>999999</number>
</property>
<property name="singleStep">
<number>100</number>
</property>
<property name="value">
<number>5000</number>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QSpinBox" name="loopClosure_icpDecimation">
<property name="minimum">
@@ -5910,6 +5983,22 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QDoubleSpinBox" name="loopClosure_icpMaxCorrespondenceDistance">
<property name="decimals">
<number>3</number>
</property>
<property name="minimum">
<double>0.010000000000000</double>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
</property>
<property name="value">
<double>0.100000000000000</double>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_109">
<property name="text">
@@ -5972,22 +6061,6 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QSpinBox" name="loopClosure_icpSamples">
<property name="minimum">
<number>0</number>
</property>
<property name="maximum">
<number>999999</number>
</property>
<property name="singleStep">
<number>100</number>
</property>
<property name="value">
<number>5000</number>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_118">
<property name="text">
@@ -5998,19 +6071,6 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QDoubleSpinBox" name="loopClosure_icpMaxCorrespondenceDistance">
<property name="minimum">
<double>0.010000000000000</double>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
</property>
<property name="value">
<double>0.100000000000000</double>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel" name="label_122">
<property name="text">
@@ -6021,29 +6081,6 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</property>
</widget>
</item>
<item row="5" column="0">
<widget class="QSpinBox" name="loopClosure_icpIterations">
<property name="minimum">
<number>1</number>
</property>
<property name="maximum">
<number>1000</number>
</property>
<property name="value">
<number>10</number>
</property>
</widget>
</item>
<item row="5" column="1">
<widget class="QLabel" name="label_124">
<property name="text">
<string>Max iterations.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="6" column="0">
<widget class="QDoubleSpinBox" name="loopClosure_icpRatio">
<property name="minimum">
@@ -6121,6 +6158,9 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
<layout class="QGridLayout" name="gridLayout_52" columnstretch="0,1">
<item row="0" column="0">
<widget class="QDoubleSpinBox" name="loopClosure_icp2MaxCorrespondenceDistance">
<property name="decimals">
<number>3</number>
</property>
<property name="minimum">
<double>0.010000000000000</double>
</property>
@@ -6184,7 +6224,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
<item row="2" column="1">
<widget class="QLabel" name="label_148">
<property name="text">
<string>Ratio of matching correspondences to accept the transform.</string>
<string>Minimum ratio of correspondences on laser scan maximum size to accept the transform.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
package.xml
+1 -1
View File
@@ -1,7 +1,7 @@
<?xml version="1.0"?>
<package>
<name>rtabmap</name>
<version>0.8.10</version>
<version>0.8.11</version>
<description>RTAB-Map's standalone library. RTAB-Map is a RGB-D SLAM approach with real-time constraints.</description>
<maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
<author>Mathieu Labbe</author>