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

Increased version to 0.6.2

Browse Source 
Memory management tested on RGBD SLAM
Fixed some crashes
New parameter RGBD/LocalLoopDetectionMaxDiffID
New parameter LccIcp2/VoxelSize
Updated parameter LccIcp/Type (added "No ICP" type)
Updated parameter kLccBowMinInliers (inliers minimum reduced to 1)
TORO optimization: The graph root is explicitly defined as the last node added (fixed a TORO crash when a root could not be found)
TORO optimization: only one constraint between neighbors and loop closures
TORO optimization: added initial tree guess
Map correction/Map transform: now only used in localization mode (map correction is always identity on mapping mode)
New statistics: local loop space diff, last loop closure parent and child ids
Database: Fixed empty signatures with no poses loaded
ICP: fixed transform multiplication order
Dump memory: added 3D words
DatabaseViewer: added Export option (added ExportDialog object)
DataRecorder: Option recording in RAM or Hard drive
GUI: added a dock widget (MapVisibilityWidget) to change visibility of nodes in the 3D map
GUI: fixed progress bar step count when generating the map
Menu option: generate TORO graph (full/current map, optimized/not optimized)
Menu option: download map (full/current map, optimized/not optimized)
Preferences: added Reset all settings button
Preferences: A QGroupBox can be a boolean parameter

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1066 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-01-22 19:49:28 +00:00
parent 2489397b49
commit 22e3082adc
33 changed files with 2103 additions and 857 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -71,7 +71,7 @@ public:
std::list<int> cleanup(const std::list<int> & ignoredIds = std::list<int>());
void emptyTrash();
void joinTrashThread();
bool addLoopClosureLink(int oldId, int newId, const Transform & transform);
bool addLoopClosureLink(int oldId, int newId, const Transform & transform, bool global);
void updateNeighborLink(int fromId, int toId, const Transform & transform);
std::map<int, int> getNeighborsId(int signatureId,
unsigned int margin,
@@ -87,7 +87,6 @@ public:
const std::set<int> & getStMem() const {return _stMem;}
int getMaxStMemSize() const {return _maxStMemSize;}
void getPose(int locationId,
int targetMapId,
Transform & pose,
bool lookInDatabase = false) const;
std::map<int, Transform> getNeighborLinks(int signatureId,
@@ -119,12 +118,14 @@ public:
bool isInWM(int signatureId) const {return _workingMem.find(signatureId) != _workingMem.end();}
bool isInLTM(int signatureId) const {return !this->isInSTM(signatureId) && !this->isInWM(signatureId);}
bool isIDsGenerated() const {return _generateIds;}
int getLastGlobalLoopClosureParentId() const {return _lastGlobalLoopClosureParentId;}
int getLastGlobalLoopClosureChildId() const {return _lastGlobalLoopClosureChildId;}
void setRoi(const std::string & roi);
void dumpMemoryTree(const char * fileNameTree) const;
virtual void dumpMemory(std::string directory) const;
virtual void dumpSignatures(const char * fileNameSign) const;
virtual void dumpSignatures(const char * fileNameSign, bool words3D) const;
void dumpDictionary(const char * fileNameRef, const char * fileNameDesc) const;
void generateGraph(const std::string & fileName, std::set<int> ids = std::set<int>());
@@ -142,14 +143,13 @@ public:
void getMetricConstraints(
const std::vector<int> & ids,
int targetMapId,
std::map<int, Transform> & poses,
std::multimap<int, std::pair<int, Transform> > & links,
bool lookInDatabase = false);
Transform computeVisualTransform(int oldId, int newId) const;
Transform computeVisualTransform(const Signature & oldS, const Signature & newS) const;
Transform computeIcpTransform(int oldId, int newId, Transform guess);
Transform computeIcpTransform(const Signature & oldS, const Signature & newS, Transform guess) const;
Transform computeIcpTransform(int oldId, int newId, Transform guess, bool icp3D);
Transform computeIcpTransform(const Signature & oldS, const Signature & newS, Transform guess, bool icp3D) const;
Transform computeScanMatchingTransform(
int newId,
int oldId,
@@ -201,7 +201,8 @@ private:
int _idCount;
int _idMapCount;
Signature * _lastSignature;
int _lastLoopClosureId;
int _lastGlobalLoopClosureParentId;
int _lastGlobalLoopClosureChildId;
bool _memoryChanged; // False by default, become true when Memory::update() is called.
int _signaturesAdded;
@@ -220,7 +221,6 @@ private:
std::vector<float> _roiRatios; // size 4
// RGBD-SLAM stuff
int _icpType;
int _bowMinInliers;
float _bowInlierDistance;
int _bowIterations;
@@ -236,6 +236,7 @@ private:
int _icp2MaxIterations;
float _icp2MaxFitness;
float _icp2CorrespondenceRatio;
float _icp2VoxelSize;
};
} // namespace rtabmap

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

@@ -210,6 +210,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, LocalLoopDetectionRadius, float, 15, "Maximum radius for space detection.");
RTABMAP_PARAM(RGBD, LocalLoopDetectionNeighbors, int, 20, "Maximum nearest neighbor.");
RTABMAP_PARAM(RGBD, LocalLoopDetectionMaxDiffID, int, 0, "Maximum ID difference between the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore.")
// Odometry
RTABMAP_PARAM(Odom, Type, int, 0, "0=BOW 1=Binary.");
@@ -235,8 +236,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(OdomICP, MaxFitness, float, 0.01, "");
// Loop closure constraint
RTABMAP_PARAM(LccIcp, Enabled, bool, false, "Enable ICP");
RTABMAP_PARAM(LccIcp, Type, int, 0, "0=ICP 3D, 1=ICP 2D");
RTABMAP_PARAM(LccIcp, Type, int, 0, "0=No ICP, 0=ICP 3D, 1=ICP 2D");
RTABMAP_PARAM(LccBow, MinInliers, int, 20, "Minimum visual word correspondences to compute geometry transform.");
RTABMAP_PARAM(LccBow, InlierDistance, float, 0.01, "Maximum distance for visual word correspondences.");
@@ -255,7 +255,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(LccIcp2, Iterations, int, 30, "ICP 2D: Max iterations.");
RTABMAP_PARAM(LccIcp2, MaxFitness, float, 1.0, "ICP 2D: Maximum fitness to accept the computed transform.");
RTABMAP_PARAM(LccIcp2, CorrespondenceRatio, float, 0.7, "ICP 2D: Ratio of matching correspondences to accept the transform.");
RTABMAP_PARAM(LccIcp2, VoxelSize, float, 0.005, "Voxel size to be used for ICP computation.");
public:
virtual ~Parameters();

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

@@ -75,7 +75,6 @@ public:
int getTotalMemSize() const;
double getLastProcessTime() const {return _lastProcessTime;};
std::multimap<int, cv::KeyPoint> getWords(int locationId) const;
std::map<int, int> getNeighbors(int nodeId, int margin, bool lookInLTM = false) const;// <Id,Margin> including nodeId
bool isInSTM(int locationId) const;
bool isIDsGenerated() const;
const Statistics & getStatistics() const;
@@ -87,6 +86,7 @@ public:
void triggerNewMap();
void generateGraph(const std::string & path, int id=0, int margin=5);
void generateTOROGraph(const std::string & path, bool optimized, bool full);
void resetMemory(bool dbOverwritten = false);
void dumpPrediction() const;
void dumpData() const;
@@ -101,15 +101,20 @@ public:
std::map<int, float> & depthConstants,
std::map<int, Transform> & localTransforms,
std::map<int, Transform> & poses,
Transform & mapCorrection) const;
bool optimized,
bool full) const;
std::map<int, Transform> getOptimizedWMPosesInRadius(int fromId, int maxNearestNeighbors, float radius, int & nearestId) const;
std::map<int, Transform> getOptimizedWMPosesInRadius(int fromId, int maxNearestNeighbors, float radius, int maxDiffID, int & nearestId) const;
void adjustLikelihood(std::map<int, float> & likelihood) const;
std::pair<int, float> selectHypothesis(const std::map<int, float> & posterior,
const std::map<int, float> & likelihood) const;
private:
void optimizeCurrentMap(int id, bool lookInDatabase, std::map<int, Transform> & optimizedPoses, Transform & mapCorrection) const;
void optimizeCurrentMap(int id,
bool lookInDatabase,
std::map<int, Transform> & optimizedPoses,
std::multimap<int, std::pair<int, Transform> > * constraints = 0) const;
void setupLogFiles(bool overwrite = false);
void flushStatisticLogs();
@@ -130,11 +135,13 @@ private:
bool _rgbdSlamMode;
float _rgbdLinearUpdate;
float _rgbdAngularUpdate;
int _globalLoopClosureIcpType;
int _scanMatchingSize;
bool _localLoopClosureDetectionTime;
bool _localLoopClosureDetectionSpace;
float _localDetectRadius;
float _localDetectMaxNeighbors;
int _localDetectMaxDiffID;
bool _icpEnabled;
std::string _databasePath;
@@ -160,8 +167,8 @@ private:
std::string _wDir;
std::map<int, Transform> _optimizedPoses;
Transform _mapCorrection;
Transform _mapTransform;
Transform _mapCorrection; // for localization mode
Transform _mapTransform; // for localization mode
};
#endif /* RTABMAP_H_ */

22
corelib/include/rtabmap/core/RtabmapEvent.h
View File

@@ -52,11 +52,14 @@ public:
kCmdResetMemory,
kCmdDumpMemory,
kCmdDumpPrediction,
kCmdGenerateGraph,
kCmdGenerateLocalGraph,
kCmdDeleteMemory,
kCmdGenerateGraph, // params: path
kCmdGenerateLocalGraph, // params: path, id, margin
kCmdGenerateTOROGraph, // params: path, optimized
kCmdGenerateTOROGraphFull, // params: path, optimized
kCmdDeleteMemory, // params: path [optional]
kCmdCleanDataBuffer,
kCmdPublish3DMap,
kCmdPublish3DMapFull,
kCmdTriggerNewMap,
kCmdPause};
public:
@@ -75,6 +78,11 @@ public:
_cmd(cmd),
_strValue(value),
_intValue(0){}
RtabmapEventCmd(Cmd cmd, const std::string & strValue, int intValue) :
UEvent(0),
_cmd(cmd),
_strValue(strValue),
_intValue(intValue){}
virtual ~RtabmapEventCmd() {}
Cmd getCmd() const {return _cmd;}
@@ -139,16 +147,14 @@ public:
const std::map<int, std::vector<unsigned char> > & depths2d,
const std::map<int, float> & depthConstants,
const std::map<int, Transform> & localTransforms,
const std::map<int, Transform> & poses,
const Transform & mapCorrection) :
const std::map<int, Transform> & poses) :
UEvent(0),
_images(images),
_depths(depths),
_depths2d(depths2d),
_depthConstants(depthConstants),
_localTransforms(localTransforms),
_poses(poses),
_mapCorrection(mapCorrection)
_poses(poses)
{}
virtual ~RtabmapEvent3DMap() {}
@@ -159,7 +165,6 @@ public:
const std::map<int, float> & getDepthConstants() const {return _depthConstants;}
const std::map<int, Transform> & getLocalTransforms() const {return _localTransforms;}
const std::map<int, Transform> & getPoses() const {return _poses;}
const Transform & getMapCorrection() const {return _mapCorrection;}
virtual std::string getClassName() const {return std::string("RtabmapEvent3DMap");}
@@ -170,7 +175,6 @@ private:
std::map<int, float> _depthConstants;
std::map<int, Transform> _localTransforms;
std::map<int, Transform> _poses;
Transform _mapCorrection;
};
} // namespace rtabmap

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

@@ -53,9 +53,12 @@ public:
kStateDumpingPrediction,
kStateGeneratingGraph,
kStateGeneratingLocalGraph,
kStateGeneratingTOROGraph,
kStateGeneratingTOROGraphFull,
kStateDeletingMemory,
kStateCleanDataBuffer,
kStatePublishingMap,
kStatePublishingMapFull,
kStateTriggeringMap
};
@@ -76,7 +79,7 @@ private:
void getImage(Image & image);
void pushNewState(State newState, const ParametersMap & parameters = ParametersMap());
void setDataBufferSize(int size);
void publishMap() const;
void publishMap(bool optimized, bool full) const;
private:
UMutex _stateMutex;

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

@@ -50,11 +50,14 @@ class RTABMAP_EXP Statistics
RTABMAP_STATS(Loop, ReactivateId,);
RTABMAP_STATS(Loop, Hypothesis_ratio,);
RTABMAP_STATS(Loop, Hypothesis_reactivated,);
RTABMAP_STATS(Loop, Last_loop_closure_parent,);
RTABMAP_STATS(Loop, Last_loop_closure_child,);
RTABMAP_STATS(LocalLoop, Scan_matching_success,);
RTABMAP_STATS(LocalLoop, Odom_corrected,);
RTABMAP_STATS(LocalLoop, Time_closures,);
RTABMAP_STATS(LocalLoop, Space_closure_id,);
RTABMAP_STATS(LocalLoop, Space_neighbors,);
RTABMAP_STATS(LocalLoop, Space_diff_id,);
RTABMAP_STATS(Memory, Working_memory_size,);
RTABMAP_STATS(Memory, Short_time_memory_size,);
@@ -63,7 +66,6 @@ class RTABMAP_EXP Statistics
RTABMAP_STATS(Memory, Images_buffered,);
RTABMAP_STATS(Memory, Rehearsal_sim,);
RTABMAP_STATS(Memory, Rehearsal_merged,);
RTABMAP_STATS(Memory, Last_loop_closure,);
RTABMAP_STATS(Timing, Memory_update, ms);
RTABMAP_STATS(Timing, Scan_matching, ms);

161
corelib/src/DBDriverSqlite3.cpp
View File

@@ -940,74 +940,110 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
std::stringstream query;
std::multimap<int, cv::KeyPoint> visualWords;
std::multimap<int, pcl::PointXYZ> visualWords3;
unsigned int loaded = 0;
// Prepare the query... Get the map from signature and visual words
query << "SELECT id, map_id, weight, pose, word_id, pos_x, pos_y, size, dir, response, depth_x, depth_y, depth_z "
"FROM Map_Node_Word "
"INNER JOIN Node "
"ON Node.id = node_id "
"WHERE node_id = ? ";
query << " ORDER BY word_id"; // Needed for fast insertion below
query << ";";
// Load nodes information
query << "SELECT id, map_id, weight, pose "
<< "FROM Node "
<< "WHERE id=?;";
rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
int id = 0;
int mapId = 0;
int weight = 0;
const void * data = 0;
int dataSize = 0;
Transform pose;
int visualWordId = 0;
cv::KeyPoint kpt;
pcl::PointXYZ depth;
for(std::list<int>::const_iterator iter=ids.begin(); iter!=ids.end(); ++iter)
{
ULOGGER_DEBUG("Loading %d...", *iter);
ULOGGER_DEBUG("Loading node %d...", *iter);
// bind id
rc = sqlite3_bind_int(ppStmt, 1, *iter);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
id = 0;
mapId = 0;
weight = 0;
data = 0;
dataSize = 0;
pose = Transform();
visualWordId = 0;
kpt = cv::KeyPoint();
visualWords.clear();
depth = pcl::PointXYZ(0,0,0);
int id = 0;
int mapId = 0;
int weight = 0;
Transform pose;
const void * data = 0;
int dataSize = 0;
// Process the result if one
rc = sqlite3_step(ppStmt);
if(rc == SQLITE_ROW)
{
int index = 0;
id = sqlite3_column_int(ppStmt, index++); // Signature Id
mapId = sqlite3_column_int(ppStmt, index++); // Map Id
weight = sqlite3_column_int(ppStmt, index++); // weight
data = sqlite3_column_blob(ppStmt, index); // pose
dataSize = sqlite3_column_bytes(ppStmt, index++);
if((unsigned int)dataSize == pose.size()*sizeof(float) && data)
{
memcpy(pose.data(), data, dataSize);
}
rc = sqlite3_step(ppStmt);
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
// create the node
if(id)
{
ULOGGER_DEBUG("Creating %d (map=%d)", *iter, mapId);
Signature * s = new Signature(
id,
mapId,
std::multimap<int, cv::KeyPoint>(),
std::multimap<int, pcl::PointXYZ>(),
pose);
s->setWeight(weight);
s->setSaved(true);
nodes.push_back(s);
++loaded;
}
else
{
UERROR("Signature %d not found in database!", *iter);
}
//reset
rc = sqlite3_reset(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
}
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
ULOGGER_DEBUG("Time=%fs", timer.ticks());
// Prepare the query... Get the map from signature and visual words
std::stringstream query2;
query2 << "SELECT word_id, pos_x, pos_y, size, dir, response, depth_x, depth_y, depth_z "
"FROM Map_Node_Word "
"WHERE node_id = ? ";
query2 << " ORDER BY word_id"; // Needed for fast insertion below
query2 << ";";
rc = sqlite3_prepare_v2(_ppDb, query2.str().c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
for(std::list<Signature*>::const_iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
{
ULOGGER_DEBUG("Loading words of %d...", (*iter)->id());
// bind id
rc = sqlite3_bind_int(ppStmt, 1, (*iter)->id());
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
int visualWordId = 0;
cv::KeyPoint kpt;
std::multimap<int, cv::KeyPoint> visualWords;
std::multimap<int, pcl::PointXYZ> visualWords3;
pcl::PointXYZ depth(0,0,0);
// Process the result if one
rc = sqlite3_step(ppStmt);
while(rc == SQLITE_ROW)
{
int index = 0;
if(id==0)
{
id = sqlite3_column_int(ppStmt, index++); // Signature Id
mapId = sqlite3_column_int(ppStmt, index++); // Map Id
weight = sqlite3_column_int(ppStmt, index++); // weight
data = sqlite3_column_blob(ppStmt, index); // pose
dataSize = sqlite3_column_bytes(ppStmt, index++);
if((unsigned int)dataSize == pose.size()*sizeof(float) && data)
{
memcpy(pose.data(), data, dataSize);
}
}
else
{
index = 4;
}
visualWordId = sqlite3_column_int(ppStmt, index++);
kpt.pt.x = sqlite3_column_double(ppStmt, index++);
kpt.pt.y = sqlite3_column_double(ppStmt, index++);
@@ -1023,23 +1059,16 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
// create the node
if(id==0)
if(visualWords.size()==0)
{
UWARN("Empty signature detected! (id=%d)", *iter);
UWARN("Empty signature detected! (id=%d)", (*iter)->id());
}
else
{
(*iter)->setWords(visualWords);
(*iter)->setWords3(visualWords3);
ULOGGER_DEBUG("Add %d keypoints and %d 3d points to node %d", visualWords.size(), visualWords3.size(), (*iter)->id());
}
ULOGGER_DEBUG("Creating %d (map=%d) with %d keypoints", *iter, mapId, visualWords.size());
Signature * s = new Signature(
*iter,
mapId,
visualWords,
visualWords3,
pose);
s->setWeight(weight);
s->setSaved(true);
nodes.push_back(s);
++loaded;
//reset
rc = sqlite3_reset(ppStmt);
@@ -1079,7 +1108,6 @@ void DBDriverSqlite3::loadLastNodesQuery(std::list<Signature *> & nodes) const
std::string query;
std::list<int> ids;
// Get the map from signature and visual words
query = "SELECT n.id "
"FROM Node AS n "
"WHERE n.time_enter >= (SELECT MAX(time_enter) FROM Statistics) "
@@ -1412,6 +1440,7 @@ void DBDriverSqlite3::loadLinksQuery(std::list<Signature *> & signatures) const
void DBDriverSqlite3::updateQuery(const std::list<Signature *> & nodes) const
{
UDEBUG("nodes = %d", nodes.size());
if(_ppDb && nodes.size())
{
UTimer timer;

186
corelib/src/Memory.cpp
View File

@@ -58,7 +58,8 @@ Memory::Memory(const ParametersMap & parameters) :
_idCount(kIdStart),
_idMapCount(kIdStart),
_lastSignature(0),
_lastLoopClosureId(0),
_lastGlobalLoopClosureParentId(0),
_lastGlobalLoopClosureChildId(0),
_memoryChanged(false),
_signaturesAdded(0),
@@ -70,8 +71,6 @@ Memory::Memory(const ParametersMap & parameters) :
_wordsPerImageTarget(Parameters::defaultKpWordsPerImage()),
_roiRatios(std::vector<float>(4, 0.0f)),
_icpType(Parameters::defaultLccIcpType()),
_bowMinInliers(Parameters::defaultLccBowMinInliers()),
_bowInlierDistance(Parameters::defaultLccBowInlierDistance()),
_bowIterations(Parameters::defaultLccBowIterations()),
@@ -88,7 +87,8 @@ Memory::Memory(const ParametersMap & parameters) :
_icp2MaxCorrespondenceDistance(Parameters::defaultLccIcp2MaxCorrespondenceDistance()),
_icp2MaxIterations(Parameters::defaultLccIcp2Iterations()),
_icp2MaxFitness(Parameters::defaultLccIcp2MaxFitness()),
_icp2CorrespondenceRatio(Parameters::defaultLccIcp2CorrespondenceRatio())
_icp2CorrespondenceRatio(Parameters::defaultLccIcp2CorrespondenceRatio()),
_icp2VoxelSize(Parameters::defaultLccIcp2VoxelSize())
{
_vwd = new VWDictionary(parameters);
this->parseParameters(parameters);
@@ -314,20 +314,6 @@ void Memory::parseParameters(const ParametersMap & parameters)
_dbDriver->parseParameters(parameters);
}
// RGB-D SLAM stuff
if((iter=parameters.find(Parameters::kLccIcpType())) != parameters.end())
{
int icpType = std::atoi((*iter).second.c_str());
if(icpType >= 0 && icpType <= 1)
{
_icpType = icpType;
}
else
{
UERROR("Icp type must be 0 or 1 (value=%d)", icpType);
}
}
Parameters::parse(parameters, Parameters::kLccBowMinInliers(), _bowMinInliers);
Parameters::parse(parameters, Parameters::kLccBowInlierDistance(), _bowInlierDistance);
Parameters::parse(parameters, Parameters::kLccBowIterations(), _bowIterations);
@@ -343,8 +329,9 @@ void Memory::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kLccIcp2Iterations(), _icp2MaxIterations);
Parameters::parse(parameters, Parameters::kLccIcp2MaxFitness(), _icp2MaxFitness);
Parameters::parse(parameters, Parameters::kLccIcp2CorrespondenceRatio(), _icp2CorrespondenceRatio);
Parameters::parse(parameters, Parameters::kLccIcp2VoxelSize(), _icp2VoxelSize);
UASSERT_MSG(_bowMinInliers >= 8, uFormat("value=%d", _bowMinInliers).c_str());
UASSERT_MSG(_bowMinInliers >= 1, uFormat("value=%d", _bowMinInliers).c_str());
UASSERT_MSG(_bowInlierDistance > 0.0f, uFormat("value=%f", _bowInlierDistance).c_str());
UASSERT_MSG(_bowIterations > 0, uFormat("value=%d", _bowIterations).c_str());
UASSERT_MSG(_bowMaxDepth >= 0.0f, uFormat("value=%f", _bowMaxDepth).c_str());
@@ -359,6 +346,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
UASSERT_MSG(_icp2MaxIterations > 0, uFormat("value=%d", _icp2MaxIterations).c_str());
UASSERT_MSG(_icp2MaxFitness > 0.0f, uFormat("value=%f", _icp2MaxFitness).c_str());
UASSERT_MSG(_icp2CorrespondenceRatio >=0.0f && _icp2CorrespondenceRatio <=1.0f, uFormat("value=%f", _icp2MaxFitness).c_str());
UASSERT_MSG(_icp2VoxelSize >= 0, uFormat("value=%d", _icp2VoxelSize).c_str());
// Keypoint stuff
if(_vwd)
@@ -511,7 +499,8 @@ bool Memory::update(const Image & image, Statistics * stats)
UDEBUG("totalTimer = %fs", totalTimer.ticks());
if(stats) stats->addStatistic(Statistics::kMemoryLast_loop_closure(), _lastLoopClosureId);
if(stats) stats->addStatistic(Statistics::kLoopLast_loop_closure_parent(), _lastGlobalLoopClosureParentId);
if(stats) stats->addStatistic(Statistics::kLoopLast_loop_closure_child(), _lastGlobalLoopClosureChildId);
return true;
}
@@ -629,7 +618,7 @@ int Memory::getVWDictionarySize() const
return _vwd->getVisualWords().size();
}
void Memory::getPose(int locationId, int targetMapId, Transform & pose, bool lookInDatabase) const
void Memory::getPose(int locationId, Transform & pose, bool lookInDatabase) const
{
const Signature * s = getSignature(locationId);
int mapId = -1;
@@ -637,6 +626,11 @@ void Memory::getPose(int locationId, int targetMapId, Transform & pose, bool loo
{
pose = s->getPose();
mapId = s->mapId();
if(pose.isNull())
{
UERROR("Pose of %d is null?!?", locationId);
}
}
else if(lookInDatabase && _dbDriver)
{
@@ -722,7 +716,7 @@ std::map<int, Transform> Memory::getNeighborLinks(int signatureId, bool ignoreNe
// maxCheckedInDatabase = -1 means no limit to check in database (default)
// maxCheckedInDatabase = 0 means don't check in database
std::map<int, int> Memory::getNeighborsId(int signatureId,
unsigned int margin,
unsigned int margin, // 0 means infinite margin
int maxCheckedInDatabase, // default -1 (no limit)
bool incrementMarginOnLoop, // default false
bool ignoreLoopIds, // default false
@@ -745,7 +739,7 @@ std::map<int, int> Memory::getNeighborsId(int signatureId,
std::set<int> nextMargin;
nextMargin.insert(signatureId);
unsigned int m = 0;
while(m < margin && nextMargin.size())
while((margin == 0 || m < margin) && nextMargin.size())
{
curentMarginList = std::list<int>(nextMargin.begin(), nextMargin.end());
nextMargin.clear();
@@ -1001,7 +995,8 @@ void Memory::clear()
}
UDEBUG("");
_lastSignature = 0;
_lastLoopClosureId = 0;
_lastGlobalLoopClosureParentId = 0;
_lastGlobalLoopClosureChildId = 0;
_idCount = kIdStart;
_idMapCount = kIdStart;
_memoryChanged = false;
@@ -1299,10 +1294,10 @@ std::list<Signature *> Memory::getRemovableSignatures(int count, const std::set<
bool recentWmImmunized = false;
// look for the position of the lastLoopClosureId in WM
int currentRecentWmSize = 0;
if(_lastLoopClosureId > 0 && _stMem.find(_lastLoopClosureId) == _stMem.end())
if(_lastGlobalLoopClosureParentId > 0 && _stMem.find(_lastGlobalLoopClosureParentId) == _stMem.end())
{
// If set, it must be in WM
std::set<int>::const_iterator iter = _workingMem.find(_lastLoopClosureId);
std::set<int>::const_iterator iter = _workingMem.find(_lastGlobalLoopClosureParentId);
while(iter != _workingMem.end())
{
++currentRecentWmSize;
@@ -1314,9 +1309,9 @@ std::list<Signature *> Memory::getRemovableSignatures(int count, const std::set<
}
else if(currentRecentWmSize == 0 && _workingMem.size() > 1)
{
UERROR("Last loop closure id not found in WM (%d)", _lastLoopClosureId);
UERROR("Last loop closure id not found in WM (%d)", _lastGlobalLoopClosureParentId);
}
UDEBUG("currentRecentWmSize=%d, recentWmMaxSize=%d, _recentWmRatio=%f, end recent wM = %d", currentRecentWmSize, recentWmMaxSize, _recentWmRatio, _lastLoopClosureId);
UDEBUG("currentRecentWmSize=%d, recentWmMaxSize=%d, _recentWmRatio=%f, end recent wM = %d", currentRecentWmSize, recentWmMaxSize, _recentWmRatio, _lastGlobalLoopClosureParentId);
}
// Ignore neighbor of the last location in STM (for neighbor links redirection issue during Rehearsal).
@@ -1328,8 +1323,8 @@ std::list<Signature *> Memory::getRemovableSignatures(int count, const std::set<
for(std::set<int>::const_iterator memIter = wm.begin(); memIter != wm.end(); ++memIter)
{
if( (recentWmImmunized && *memIter > _lastLoopClosureId) ||
*memIter == _lastLoopClosureId)
if( (recentWmImmunized && *memIter > _lastGlobalLoopClosureParentId) ||
*memIter == _lastGlobalLoopClosureParentId)
{
// ignore recent memory
}
@@ -1403,7 +1398,7 @@ std::list<Signature *> Memory::getRemovableSignatures(int count, const std::set<
removableSignatures.push_back(iter->second);
addedSignatures.insert(iter->second->id());
if(iter->second->id() > _lastLoopClosureId)
if(iter->second->id() > _lastGlobalLoopClosureParentId)
{
++recentWmCount;
if(currentRecentWmSize - recentWmCount < recentWmMaxSize)
@@ -1413,7 +1408,7 @@ std::list<Signature *> Memory::getRemovableSignatures(int count, const std::set<
}
}
}
else if(iter->second->id() < _lastLoopClosureId)
else if(iter->second->id() < _lastGlobalLoopClosureParentId)
{
UDEBUG("weight=%d, id=%d, lcCount=%d, lcId=%d, childId=%d",
iter->first.weight,
@@ -1575,9 +1570,10 @@ void Memory::rejectLoopClosure(int oldId, int newId)
break;
}
}
if(newS->getChildLoopClosureIds().size() == 0 && newId == _lastLoopClosureId)
if(newS->getChildLoopClosureIds().size() == 0 && newId == _lastGlobalLoopClosureParentId)
{
_lastLoopClosureId = 0;
_lastGlobalLoopClosureParentId = 0;
_lastGlobalLoopClosureChildId = 0;
}
}
else
@@ -1627,6 +1623,7 @@ Transform Memory::computeVisualTransform(const Signature & oldS, const Signature
if((int)inliersOld->size() >= _bowMinInliers)
{
UDEBUG("Correspondences = %d", (int)inliersOld->size());
int inliersCount = 0;
Transform t = util3d::transformFromXYZCorrespondences(
inliersOld,
@@ -1656,7 +1653,7 @@ Transform Memory::computeVisualTransform(const Signature & oldS, const Signature
}
// compute transform newId -> oldId
Transform Memory::computeIcpTransform(int oldId, int newId, Transform guess)
Transform Memory::computeIcpTransform(int oldId, int newId, Transform guess, bool icp3D)
{
Signature * oldS = this->_getSignature(oldId);
Signature * newS = this->_getSignature(newId);
@@ -1665,7 +1662,7 @@ Transform Memory::computeIcpTransform(int oldId, int newId, Transform guess)
{
std::list<Signature*> depthToLoad;
std::set<int> added;
if(_icpType == 0)
if(icp3D)
{
//Depth required, if not in RAM, load it from LTM
if(oldS->getDepth().empty())
@@ -1679,7 +1676,7 @@ Transform Memory::computeIcpTransform(int oldId, int newId, Transform guess)
added.insert(newS->id());
}
}
if(_icpType == 1)
else
{
//Depth required, if not in RAM, load it from LTM
if(oldS->getDepth2D().size() == 0 && added.find(oldS->id()) == added.end())
@@ -1699,13 +1696,13 @@ Transform Memory::computeIcpTransform(int oldId, int newId, Transform guess)
Transform t;
if(oldS && newS)
{
t = computeIcpTransform(*oldS, *newS, guess);
t = computeIcpTransform(*oldS, *newS, guess, icp3D);
}
return t;
}
// get transform from the new to old node
Transform Memory::computeIcpTransform(const Signature & oldS, const Signature & newS, Transform guess) const
Transform Memory::computeIcpTransform(const Signature & oldS, const Signature & newS, Transform guess, bool icp3D) const
{
if(guess.isNull())
{
@@ -1722,7 +1719,7 @@ Transform Memory::computeIcpTransform(const Signature & oldS, const Signature &
Transform transform;
// ICP with guess transform
if(_icpType == 0)
if(icp3D)
{
UDEBUG("3D ICP");
util3d::CompressionThread ctOld(oldS.getDepth(), true);
@@ -1786,7 +1783,7 @@ Transform Memory::computeIcpTransform(const Signature & oldS, const Signature &
if(hasConverged && (_icpMaxFitness == 0 || fitness < _icpMaxFitness))
{
transform = guess * icpT;
transform = icpT * guess;
transform = transform.inverse();
}
else
@@ -1800,7 +1797,7 @@ Transform Memory::computeIcpTransform(const Signature & oldS, const Signature &
UERROR("Depths 3D empty?!?");
}
}
else //_icp2DEnabled
else // icp 2D
{
UDEBUG("2D ICP");
@@ -1826,6 +1823,14 @@ Transform Memory::computeIcpTransform(const Signature & oldS, const Signature &
// 2D
pcl::PointCloud<pcl::PointXYZ>::Ptr oldCloud = util3d::cvMat2Cloud(oldDepth2D);
pcl::PointCloud<pcl::PointXYZ>::Ptr newCloud = util3d::cvMat2Cloud(newDepth2D, guess);
//voxelize
if(_icp2VoxelSize > 0.0f)
{
oldCloud = util3d::voxelize(oldCloud, _icp2VoxelSize);
newCloud = util3d::voxelize(newCloud, _icp2VoxelSize);
}
double fitness = 0.0f;
bool hasConverged = false;
Transform icpT;
@@ -1839,10 +1844,10 @@ Transform Memory::computeIcpTransform(const Signature & oldS, const Signature &
hasConverged,
fitness);
//pcl::io::savePCDFile("old.pcd", *oldCloud);
//pcl::io::savePCDFile("newguess.pcd", *newCloud);
//pcl::io::savePCDFile("lccold.pcd", *oldCloud);
//pcl::io::savePCDFile("lccnewguess.pcd", *newCloud);
newCloud = util3d::transformPointCloud(newCloud, icpT);
//pcl::io::savePCDFile("newicp.pcd", *newCloud);
//pcl::io::savePCDFile("lccnewicp.pcd", *newCloud);
// verify if there are enough correspondences
int correspondences = util3d::getCorrespondencesCount(newCloud, oldCloud, _icp2MaxCorrespondenceDistance);
@@ -1864,7 +1869,7 @@ Transform Memory::computeIcpTransform(const Signature & oldS, const Signature &
(_icp2MaxFitness == 0 || fitness < _icp2MaxFitness) &&
correspondencesRatio >= _icp2CorrespondenceRatio)
{
transform = guess * icpT;
transform = icpT * guess;
transform = transform.inverse();
}
else
@@ -1881,6 +1886,7 @@ Transform Memory::computeIcpTransform(const Signature & oldS, const Signature &
return transform;
}
// poses of newId and oldId must be in "poses"
Transform Memory::computeScanMatchingTransform(
int newId,
int oldId,
@@ -1920,9 +1926,9 @@ Transform Memory::computeScanMatchingTransform(
}
//voxelize
if(assembledOldClouds->size())
if(assembledOldClouds->size() && _icp2VoxelSize > 0.0f)
{
assembledOldClouds = util3d::voxelize(assembledOldClouds, 0.01);
assembledOldClouds = util3d::voxelize(assembledOldClouds, _icp2VoxelSize);
}
// get the new cloud
@@ -1932,9 +1938,9 @@ Transform Memory::computeScanMatchingTransform(
newCloud = util3d::cvMat2Cloud(util3d::uncompressData(newS->getDepth2D()), poses.at(newId));
//voxelize
if(newCloud->size())
if(newCloud->size() && _icp2VoxelSize > 0.0f)
{
newCloud = util3d::voxelize(newCloud, 0.01);
newCloud = util3d::voxelize(newCloud, _icp2VoxelSize);
}
//pcl::io::savePCDFile("old.pcd", *assembledOldClouds);
@@ -1991,7 +1997,7 @@ Transform Memory::computeScanMatchingTransform(
}
// Transform from new to old
bool Memory::addLoopClosureLink(int oldId, int newId, const Transform & transform)
bool Memory::addLoopClosureLink(int oldId, int newId, const Transform & transform, bool global)
{
ULOGGER_INFO("old=%d, new=%d transform: %s", oldId, newId, transform.prettyPrint().c_str());
Signature * oldS = _getSignature(oldId);
@@ -2011,9 +2017,10 @@ bool Memory::addLoopClosureLink(int oldId, int newId, const Transform & transfor
oldS->addLoopClosureId(newS->id(), transform.inverse());
newS->addChildLoopClosureId(oldS->id(), transform);
if(_incrementalMemory)
if(_incrementalMemory && global)
{
_lastLoopClosureId = newS->id();
_lastGlobalLoopClosureParentId = newS->id();
_lastGlobalLoopClosureChildId = oldS->id();
// udpate weights only if the memory is incremental
newS->setWeight(newS->getWeight() + oldS->getWeight());
@@ -2058,7 +2065,8 @@ void Memory::dumpMemory(std::string directory) const
{
UINFO("Dumping memory to directory \"%s\"", directory.c_str());
this->dumpDictionary((directory+"DumpMemoryWordRef.txt").c_str(), (directory+"DumpMemoryWordDesc.txt").c_str());
this->dumpSignatures((directory + "DumpMemorySign.txt").c_str());
this->dumpSignatures((directory + "DumpMemorySign.txt").c_str(), false);
this->dumpSignatures((directory + "DumpMemorySign3.txt").c_str(), true);
this->dumpMemoryTree((directory + "DumpMemoryTree.txt").c_str());
}
@@ -2070,7 +2078,7 @@ void Memory::dumpDictionary(const char * fileNameRef, const char * fileNameDesc)
}
}
void Memory::dumpSignatures(const char * fileNameSign) const
void Memory::dumpSignatures(const char * fileNameSign, bool words3D) const
{
FILE* foutSign = 0;
#ifdef _MSC_VER
@@ -2081,7 +2089,14 @@ void Memory::dumpSignatures(const char * fileNameSign) const
if(foutSign)
{
fprintf(foutSign, "SignatureID WordsID...\n");
if(words3D)
{
fprintf(foutSign, "SignatureID WordsID... (Max features depth=%f)\n", _bowMaxDepth);
}
else
{
fprintf(foutSign, "SignatureID WordsID...\n");
}
const std::map<int, Signature *> & signatures = this->getSignatures();
for(std::map<int, Signature *>::const_iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
{
@@ -2089,10 +2104,27 @@ void Memory::dumpSignatures(const char * fileNameSign) const
const Signature * ss = dynamic_cast<const Signature *>(iter->second);
if(ss)
{
const std::multimap<int, cv::KeyPoint> & ref = ss->getWords();
for(std::multimap<int, cv::KeyPoint>::const_iterator jter=ref.begin(); jter!=ref.end(); ++jter)
if(words3D)
{
fprintf(foutSign, "%d ", (*jter).first);
const std::multimap<int, pcl::PointXYZ> & ref = ss->getWords3();
for(std::multimap<int, pcl::PointXYZ>::const_iterator jter=ref.begin(); jter!=ref.end(); ++jter)
{
//show only valid point according to current parameters
if(pcl::isFinite(jter->second) &&
(jter->second.x != 0 || jter->second.y != 0 || jter->second.z != 0) &&
(_bowMaxDepth <= 0 || jter->second.x <= _bowMaxDepth))
{
fprintf(foutSign, "%d ", (*jter).first);
}
}
}
else
{
const std::multimap<int, cv::KeyPoint> & ref = ss->getWords();
for(std::multimap<int, cv::KeyPoint>::const_iterator jter=ref.begin(); jter!=ref.end(); ++jter)
{
fprintf(foutSign, "%d ", (*jter).first);
}
}
}
fprintf(foutSign, "\n");
@@ -2208,9 +2240,9 @@ bool Memory::rehearsalMerge(int oldId, int newId)
oldS->addLoopClosureId(newS->id()); // to keep track of the merged location
if(_lastLoopClosureId == oldS->id())
if(_lastGlobalLoopClosureParentId == oldS->id())
{
_lastLoopClosureId = newS->id();
_lastGlobalLoopClosureParentId = newS->id();
}
}
else
@@ -2406,7 +2438,7 @@ void Memory::generateGraph(const std::string & fileName, std::set<int> ids)
{
weightNeighbor = _signatures.find(iter->first)->second->getWeight();
}
UDEBUG("Add neighbor link from %d to %d", id, iter->first);
//UDEBUG("Add neighbor link from %d to %d", id, iter->first);
fprintf(fout, " \"%d\\n%d\" -> \"%d\\n%d\"\n",
id,
weight,
@@ -2427,7 +2459,7 @@ void Memory::generateGraph(const std::string & fileName, std::set<int> ids)
{
weightNeighbor = _signatures.find(iter->first)->second->getWeight();
}
UDEBUG("Add loop link from %d to %d", id, iter->first);
//UDEBUG("Add loop link from %d to %d", id, iter->first);
fprintf(fout, " \"%d\\n%d\" -> \"%d\\n%d\" [label=\"L\", fontcolor=%s, fontsize=8];\n",
id,
weight,
@@ -2446,7 +2478,7 @@ void Memory::generateGraph(const std::string & fileName, std::set<int> ids)
{
weightNeighbor = _signatures.find(iter->first)->second->getWeight();
}
UDEBUG("Add child link from %d to %d", id, iter->first);
//UDEBUG("Add child link from %d to %d", id, iter->first);
fprintf(fout, " \"%d\\n%d\" -> \"%d\\n%d\" [label=\"C\", fontcolor=%s, fontsize=8];\n",
id,
weight,
@@ -2481,7 +2513,7 @@ void Memory::generateGraph(const std::string & fileName, std::set<int> ids)
{
_dbDriver->getWeight(iter->first, weightNeighbor);
}
UDEBUG("Add neighbor link from %d to %d", id, iter->first);
//UDEBUG("Add neighbor link from %d to %d", id, iter->first);
fprintf(fout, " \"%d\\n%d\" -> \"%d\\n%d\";\n",
id,
weight,
@@ -2502,7 +2534,7 @@ void Memory::generateGraph(const std::string & fileName, std::set<int> ids)
{
weightNeighbor = _signatures.find(iter->first)->second->getWeight();
}
UDEBUG("Add loop link from %d to %d", id, iter->first);
//UDEBUG("Add loop link from %d to %d", id, iter->first);
fprintf(fout, " \"%d\\n%d\" -> \"%d\\n%d\" [label=\"L\", fontcolor=%s, fontsize=8];\n",
id,
weight,
@@ -2524,7 +2556,7 @@ void Memory::generateGraph(const std::string & fileName, std::set<int> ids)
{
weightNeighbor = _signatures.find(iter->first)->second->getWeight();
}
UDEBUG("Add child link from %d to %d", id, iter->first);
//UDEBUG("Add child link from %d to %d", id, iter->first);
fprintf(fout, " \"%d\\n%d\" -> \"%d\\n%d\" [label=\"C\", fontcolor=%s, fontsize=8];\n",
id,
weight,
@@ -3165,16 +3197,14 @@ std::set<int> Memory::reactivateSignatures(const std::list<int> & ids, unsigned
void Memory::getMetricConstraints(
const std::vector<int> & ids,
int targetMapId,
std::map<int, Transform> & poses,
std::multimap<int, std::pair<int, Transform> > & links,
bool lookInDatabase)
{
UDEBUG("targetMap = %d", targetMapId);
for(unsigned int i=0; i<ids.size(); ++i)
{
Transform pose;
this->getPose(ids[i], targetMapId, pose, lookInDatabase);
this->getPose(ids[i], pose, lookInDatabase);
if(!pose.isNull())
{
poses.insert(std::make_pair(ids[i], pose));
@@ -3190,9 +3220,23 @@ void Memory::getMetricConstraints(
{
if(!jter->second.isNull() && uContains(poses, jter->first))
{
links.insert(std::make_pair(ids[i], *jter));
bool edgeAlreadyAdded = false;
for(std::multimap<int, std::pair<int, Transform> >::iterator iter = links.lower_bound(jter->first);
iter != links.end() && iter->first == jter->first;
++iter)
{
if(iter->second.first == ids[i])
{
edgeAlreadyAdded = true;
}
}
if(!edgeAlreadyAdded)
{
links.insert(std::make_pair(ids[i], *jter));
}
}
}
std::map<int, Transform> loops, children;
this->getLoopClosureIds(ids[i], loops, children, lookInDatabase);
for(std::map<int, Transform>::iterator jter=children.begin(); jter!=children.end(); ++jter)

268
corelib/src/Rtabmap.cpp
View File

@@ -79,12 +79,13 @@ Rtabmap::Rtabmap() :
_rgbdSlamMode(Parameters::defaultRGBDEnabled()),
_rgbdLinearUpdate(Parameters::defaultRGBDLinearUpdate()),
_rgbdAngularUpdate(Parameters::defaultRGBDAngularUpdate()),
_globalLoopClosureIcpType(Parameters::defaultLccIcpType()),
_scanMatchingSize(Parameters::defaultRGBDScanMatchingSize()),
_localLoopClosureDetectionTime(Parameters::defaultRGBDLocalLoopDetectionTime()),
_localLoopClosureDetectionSpace(Parameters::defaultRGBDLocalLoopDetectionSpace()),
_localDetectRadius(Parameters::defaultRGBDLocalLoopDetectionRadius()),
_localDetectMaxNeighbors(Parameters::defaultRGBDLocalLoopDetectionNeighbors()),
_icpEnabled(Parameters::defaultLccIcpEnabled()),
_localDetectMaxDiffID(Parameters::defaultRGBDLocalLoopDetectionMaxDiffID()),
_databasePath(""),
_lcHypothesisId(0),
_lcHypothesisValue(0),
@@ -330,7 +331,21 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionSpace(), _localLoopClosureDetectionSpace);
Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionRadius(), _localDetectRadius);
Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionNeighbors(), _localDetectMaxNeighbors);
Parameters::parse(parameters, Parameters::kLccIcpEnabled(), _icpEnabled);
Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionMaxDiffID(), _localDetectMaxDiffID);
// RGB-D SLAM stuff
if((iter=parameters.find(Parameters::kLccIcpType())) != parameters.end())
{
int icpType = std::atoi((*iter).second.c_str());
if(icpType >= 0 && icpType <= 2)
{
_globalLoopClosureIcpType = icpType;
}
else
{
UERROR("Icp type must be 0, 1 or 2 (value=%d)", icpType);
}
}
// By default, we create our strategies if they are not already created.
// If they already exists, we check the parameters if a change is requested
@@ -343,7 +358,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
//generate map
if(_memory->getLastWorkingSignature())
{
optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), false, _optimizedPoses, _mapCorrection);
optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), false, _optimizedPoses);
}
}
else
@@ -482,15 +497,6 @@ std::multimap<int, cv::KeyPoint> Rtabmap::getWords(int locationId) const
return std::multimap<int, cv::KeyPoint>();
}
std::map<int, int> Rtabmap::getNeighbors(int nodeId, int margin, bool lookInLTM) const
{
if(_memory)
{
return _memory->getNeighborsId(nodeId, margin, lookInLTM?-1:0);
}
return std::map<int, int>();
}
bool Rtabmap::isInSTM(int locationId) const
{
if(_memory)
@@ -551,6 +557,7 @@ void Rtabmap::triggerNewMap()
{
int mapId = _memory->incrementMapId();
UINFO("New map triggerred, new map = %d", mapId);
_optimizedPoses.clear();
}
}
@@ -586,6 +593,27 @@ void Rtabmap::generateGraph(const std::string & path, int id, int margin)
}
}
void Rtabmap::generateTOROGraph(const std::string & path, bool optimized, bool full)
{
if(_memory && _memory->getLastWorkingSignature())
{
std::map<int, Transform> poses;
std::multimap<int, std::pair<int, Transform> > constraints;
if(optimized)
{
this->optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), full, poses, &constraints);
}
else
{
std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, full?-1:0, true);
_memory->getMetricConstraints(uKeys(ids), poses, constraints, full);
}
util3d::saveTOROGraph(path, poses, constraints);
}
}
void Rtabmap::resetMemory(bool dbOverwritten)
{
_retrievedId = 0;
@@ -601,7 +629,7 @@ void Rtabmap::resetMemory(bool dbOverwritten)
_memory->init(getDatabasePath(), dbOverwritten);
if(_memory->getLastWorkingSignature())
{
optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), false, _optimizedPoses, _mapCorrection);
optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), false, _optimizedPoses);
}
if(_bayesFilter)
{
@@ -610,7 +638,6 @@ void Rtabmap::resetMemory(bool dbOverwritten)
}
else if(dbOverwritten)
{
// FIXME May be not work with other database type.
// May be memory should be already created here, and use init above...
UINFO("Erasing file : \"%s\"", getDatabasePath().c_str());
UFile::erase(getDatabasePath());
@@ -712,10 +739,12 @@ bool Rtabmap::process(const Image & image)
Transform lastPoseToNewPose = lastPose.inverse() * image.pose();
float x,y,z, roll,pitch,yaw;
lastPoseToNewPose.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
// TODO Increment map id also if there is a big position change
if(!lastPose.isIdentity() && image.pose().isIdentity())
{
int mapId = _memory->incrementMapId();
UWARN("Odometry is reset (transform identity detected). A new map (%d) is created!", mapId);
_optimizedPoses.clear();
}
}
}
@@ -774,6 +803,13 @@ bool Rtabmap::process(const Image & image)
}
}
// Reset map correction if we are mapping!
if(_memory->isIncremental() && !_mapCorrection.isIdentity())
{
UWARN("Reset map correction because we are now mapping!");
_mapCorrection.setIdentity();
}
Transform newPose = _mapCorrection * signature->getPose();
_optimizedPoses.insert(std::make_pair(signature->id(), newPose));
@@ -785,7 +821,7 @@ bool Rtabmap::process(const Image & image)
signature->getDepth2D().size() &&
rehearsedId == 0) // don't do it if rehearsal happened
{
UINFO("Scan matching...");
UINFO("Odometry correction by scan matching (size=%d)...", _scanMatchingSize);
const std::set<int> & stm = _memory->getStMem();
std::map<int, Transform> poses;
int count = _scanMatchingSize;
@@ -794,7 +830,7 @@ bool Rtabmap::process(const Image & image)
if(_memory->getSignature(*iter)->mapId() == signature->mapId())
{
std::map<int, Transform>::iterator jter = _optimizedPoses.find(*iter);
UASSERT(jter != _optimizedPoses.end());
UASSERT_MSG(jter != _optimizedPoses.end(), uFormat("%d not found in optimized poses!", *iter).c_str());
poses.insert(*jter);
if(*iter != signature->id())
{
@@ -844,9 +880,9 @@ bool Rtabmap::process(const Image & image)
{
UDEBUG("Check local transform between %d and %d", signature->id(), *iter);
Transform transform = _memory->computeVisualTransform(*iter, signature->id());
if(!transform.isNull() && _icpEnabled)
if(!transform.isNull() && _globalLoopClosureIcpType > 0)
{
transform = _memory->computeIcpTransform(*iter, signature->id(), transform);
transform = _memory->computeIcpTransform(*iter, signature->id(), transform, _globalLoopClosureIcpType==1);
}
if(!transform.isNull())
{
@@ -855,7 +891,7 @@ bool Rtabmap::process(const Image & image)
*iter,
transform.prettyPrint().c_str());
// Add a loop constraint
if(_memory->addLoopClosureLink(*iter, signature->id(), transform))
if(_memory->addLoopClosureLink(*iter, signature->id(), transform, false))
{
++localLoopClosuresInTimeFound;
UINFO("Local loop closure found between %d and %d with t=%s",
@@ -1170,9 +1206,9 @@ bool Rtabmap::process(const Image & image)
if(_rgbdSlamMode)
{
transform = _memory->computeVisualTransform(_lcHypothesisId, signature->id());
if(!transform.isNull() && _icpEnabled)
if(!transform.isNull() && _globalLoopClosureIcpType > 0)
{
transform = _memory->computeIcpTransform(_lcHypothesisId, signature->id(), transform);
transform = _memory->computeIcpTransform(_lcHypothesisId, signature->id(), transform, _globalLoopClosureIcpType == 1);
}
rejectedHypothesis = transform.isNull();
if(rejectedHypothesis)
@@ -1182,7 +1218,7 @@ bool Rtabmap::process(const Image & image)
}
if(!rejectedHypothesis)
{
rejectedHypothesis = !_memory->addLoopClosureLink(_lcHypothesisId, signature->id(), transform);
rejectedHypothesis = !_memory->addLoopClosureLink(_lcHypothesisId, signature->id(), transform, true);
}
if(rejectedHypothesis)
@@ -1219,12 +1255,17 @@ bool Rtabmap::process(const Image & image)
signature->getDepth2D().size())
{
//============================================================
// Scan matching
// Scan matching LOCAL LOOP CLOSURE SPACE
//============================================================
// get all nodes in radius of the current node
std::map<int, Transform> poses;
int oldId = 0;
poses = this->getOptimizedWMPosesInRadius(signature->id(), _localDetectMaxNeighbors, _localDetectRadius, oldId);
poses = this->getOptimizedWMPosesInRadius(signature->id(), _localDetectMaxNeighbors, _localDetectRadius, _localDetectMaxDiffID, oldId);
// add current node to poses
UASSERT(_optimizedPoses.find(signature->id()) != _optimizedPoses.end());
poses.insert(std::make_pair(signature->id(), _optimizedPoses.at(signature->id())));
localSpaceDetectionPosesCount = poses.size()-1;
//The nearest will be the reference for a loop closure transform
if(poses.size() &&
@@ -1239,7 +1280,7 @@ bool Rtabmap::process(const Image & image)
signature->id(),
oldId,
t.prettyPrint().c_str());
_memory->addLoopClosureLink(oldId, signature->id(), t);
_memory->addLoopClosureLink(oldId, signature->id(), t, false);
}
}
}
@@ -1260,7 +1301,7 @@ bool Rtabmap::process(const Image & image)
{
UINFO("Update map correction: SLAM mode");
// SLAM mode!
optimizeCurrentMap(signature->id(), false, _optimizedPoses, _mapCorrection);
optimizeCurrentMap(signature->id(), false, _optimizedPoses);
}
else if(_lcHypothesisId > 0 || localSpaceClosureId > 0 || signaturesRetrieved.size())
{
@@ -1270,7 +1311,7 @@ bool Rtabmap::process(const Image & image)
if(signaturesRetrieved.size() || _optimizedPoses.find(oldId) == _optimizedPoses.end())
{
// update optimized poses
optimizeCurrentMap(_retrievedId, false, _optimizedPoses, _mapCorrection);
optimizeCurrentMap(_retrievedId, false, _optimizedPoses);
}
if(_optimizedPoses.find(oldId) == _optimizedPoses.end())
@@ -1340,13 +1381,19 @@ bool Rtabmap::process(const Image & image)
statistics_.addStatistic(Statistics::kLoopReactivateId(), _retrievedId);
statistics_.addStatistic(Statistics::kLoopHypothesis_ratio(), hypothesisRatio);
statistics_.addStatistic(Statistics::kLocalLoopScan_matching_success(), scanMatchingSuccess?1:0);
statistics_.addStatistic(Statistics::kLocalLoopOdom_corrected(), scanMatchingSuccess?1:0);
statistics_.addStatistic(Statistics::kLocalLoopTime_closures(), localLoopClosuresInTimeFound);
statistics_.addStatistic(Statistics::kLocalLoopSpace_neighbors(), localSpaceDetectionPosesCount);
statistics_.addStatistic(Statistics::kLocalLoopSpace_closure_id(), localSpaceClosureId);
if(localSpaceClosureId)
{
statistics_.setLocalLoopClosureId(localSpaceClosureId);
int d1 = abs(signature->id() - localSpaceClosureId);
int d2 = abs(localSpaceClosureId - _memory->getLastGlobalLoopClosureChildId());
int d3 = abs(signature->id() - _memory->getLastGlobalLoopClosureParentId());
int d = d1<=d2?d1:d2;
d = d <= d3?d:d3;
statistics_.addStatistic(Statistics::kLocalLoopSpace_diff_id(), d);
}
if(_lcHypothesisId || localSpaceClosureId)
{
@@ -1503,13 +1550,14 @@ bool Rtabmap::process(const Image & image)
}
_lastProcessTime = totalTime;
//Removed optimized poses from signatures transferred
//Remove optimized poses from signatures transferred
for(std::list<int>::iterator iter = signaturesRemoved.begin(); iter!=signaturesRemoved.end(); ++iter)
{
UDEBUG("removing optimized pose %d...", *iter);
_optimizedPoses.erase(*iter);
}
timeRealTimeLimitReachedProcess = timer.ticks();
ULOGGER_INFO("Time limit reached processing = %f...", timeRealTimeLimitReachedProcess);
@@ -1710,14 +1758,17 @@ void Rtabmap::dumpData() const
}
}
// fromId must be in _memory and in _optimizedPoses
std::map<int, Transform> Rtabmap::getOptimizedWMPosesInRadius(
int fromId,
int maxNearestNeighbors,
float radius,
int maxDiffID, // 0 means ignore
int & nearestId) const
{
UDEBUG("");
const Signature * fromS = _memory->getSignature(fromId);
UASSERT(fromS != 0);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
cloud->resize(_optimizedPoses.size());
@@ -1726,8 +1777,9 @@ std::map<int, Transform> Rtabmap::getOptimizedWMPosesInRadius(
const std::set<int> & stm = _memory->getStMem();
for(std::map<int, Transform>::const_iterator iter = _optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
{
// Only locations in Working Memory and in the same map.
if(stm.find(iter->first) == stm.end() && fromS->mapId() == _memory->getSignature(iter->first)->mapId())
// Only locations in Working Memory with ID not too far from the last loop closure child id
bool diffIdOk = maxDiffID == 0 || abs(fromId - iter->first) <= maxDiffID || (abs(iter->first - _memory->getLastGlobalLoopClosureChildId()) <= maxDiffID && abs(fromId - _memory->getLastGlobalLoopClosureParentId()) <= maxDiffID);
if(stm.find(iter->first) == stm.end() && diffIdOk)
{
(*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
ids[oi++] = iter->first;
@@ -1737,11 +1789,11 @@ std::map<int, Transform> Rtabmap::getOptimizedWMPosesInRadius(
cloud->resize(oi);
ids.resize(oi);
UASSERT(_optimizedPoses.find(fromId) != _optimizedPoses.end());
Transform fromT = _optimizedPoses.at(fromId);
nearestId = 0;
std::map<int, Transform> poses;
poses.insert(*_optimizedPoses.find(fromId)); // add fromId
float minDistance = -1;
if(cloud->size())
{
@@ -1789,6 +1841,7 @@ std::map<int, Transform> Rtabmap::getOptimizedWMPosesInRadius(
}
}
}
//if(inliers.size())
//{
// pcl::io::savePCDFile("radiusInliers.pcd", inliers);
@@ -1800,29 +1853,130 @@ std::map<int, Transform> Rtabmap::getOptimizedWMPosesInRadius(
// c.push_back(pcl::PointXYZ(ct.x(), ct.y(), ct.z()));
// pcl::io::savePCDFile("radiusNearestPt.pcd", c);
//}
if(nearestId > 0)
{
// Only take nodes linked with the nearest node
std::map<int, int> neighbors = _memory->getNeighborsId(nearestId, maxNearestNeighbors, 0, true, true);
for(std::map<int, Transform>::iterator iter = poses.begin(); iter!= poses.end();)
{
if(!uContains(neighbors, iter->first))
{
poses.erase(iter++);
}
else
{
++iter;
}
}
}
else if(poses.size())
{
UWARN("Flushing poses (%d) because nearest id of %d can't be found!", (int)poses.size(), fromId);
poses.clear();
}
}
}
UDEBUG("nearestId = %d, minDistance=%f poses=%d", nearestId, minDistance, (int)poses.size());
return poses;
}
void Rtabmap::optimizeCurrentMap(int id, bool lookInDatabase, std::map<int, Transform> & optimizedPoses, Transform & mapCorrection) const
void Rtabmap::optimizeCurrentMap(
int id,
bool lookInDatabase,
std::map<int, Transform> & optimizedPoses,
std::multimap<int, std::pair<int, Transform> > * constraints) const
{
//Optimize the map
optimizedPoses.clear();
UDEBUG("Optimize map: around location %d", id);
if(_memory && id > 0)
{
std::map<int, int> ids = _memory->getNeighborsId(id, 999, lookInDatabase?-1:0);
std::map<int, int> ids = _memory->getNeighborsId(id, 0, lookInDatabase?-1:0, true);
UDEBUG("ids=%d", (int)ids.size());
std::map<int, Transform> poses;
std::multimap<int, std::pair<int, Transform> > edgeConstraints;
_memory->getMetricConstraints(uKeys(ids), _memory->getSignature(id)->mapId(), poses, edgeConstraints, false);
if(poses.size() > 1 && edgeConstraints.size() > 0)
_memory->getMetricConstraints(uKeys(ids), poses, edgeConstraints, lookInDatabase);
UDEBUG("poses=%d, edgeConstraints=%d", (int)poses.size(), (int)edgeConstraints.size());
if(constraints)
{
util3d::optimizeTOROGraph(poses, edgeConstraints, 100, optimizedPoses, mapCorrection);
*constraints = edgeConstraints;
}
// Modify IDs using the margin from the current signature (TORO root will be the last signature)
int m = 0;
int toroId = 1;
std::map<int, int> rtabmapToToro; // <RTAB-Map ID, TORO ID>
std::map<int, int> toroToRtabmap; // <TORO ID, RTAB-Map ID>
while(ids.size())
{
for(std::map<int, int>::iterator iter = ids.begin(); iter!=ids.end();)
{
if(m == iter->second)
{
rtabmapToToro.insert(std::make_pair(iter->first, toroId));
toroToRtabmap.insert(std::make_pair(toroId, iter->first));
++toroId;
ids.erase(iter++);
}
else
{
++iter;
}
}
++m;
}
//
std::map<int, Transform> posesToro;
std::multimap<int, std::pair<int, Transform> > edgeConstraintsToro;
for(std::map<int, Transform>::iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
posesToro.insert(std::make_pair(rtabmapToToro.at(iter->first), iter->second));
}
for(std::multimap<int, std::pair<int, Transform> >::iterator iter = edgeConstraints.begin();
iter!=edgeConstraints.end();
++iter)
{
edgeConstraintsToro.insert(std::make_pair(rtabmapToToro.at(iter->first), std::make_pair(rtabmapToToro.at(iter->second.first), iter->second.second)));
}
if(posesToro.size() > 1 && edgeConstraintsToro.size() > 0)
{
///UDEBUG("TORO optimize begin");
//util3d::saveTOROGraph("toroIdRtabmap.graph", poses, edgeConstraints);
//util3d::saveTOROGraph("toroIdToro.graph", posesToro, edgeConstraintsToro);
//UDEBUG("graph saved");
std::map<int, Transform> optimizedPosesToro;
Transform mapCorrectionToro;
util3d::optimizeTOROGraph(posesToro, edgeConstraintsToro, 100, optimizedPosesToro, mapCorrectionToro);
//UDEBUG("saving optimized graph...");
//util3d::saveTOROGraph("toroOptimized.graph", optimizedPosesToro, edgeConstraintsToro);
//UDEBUG("TORO optimize end");
for(std::map<int, Transform>::iterator iter=optimizedPosesToro.begin(); iter!=optimizedPosesToro.end(); ++iter)
{
optimizedPoses.insert(std::make_pair(toroToRtabmap.at(iter->first), iter->second));
}
//util3d::saveTOROGraph("toro.graph", poses, edgeConstraints);
//util3d::saveTOROGraph("toroOptimized.graph", _optimizedPoses, edgeConstraints);
}
else if(poses.size() == 1 && edgeConstraints.size() == 0)
{
optimizedPoses = poses;
}
else if(poses.size() || edgeConstraints.size())
{
UFATAL("Poses=%d and edges=%d (poses must "
"not be null if there are edges, and edges must be null if poses <= 1)",
poses.size(), edgeConstraints.size());
}
}
}
@@ -1936,44 +2090,50 @@ void Rtabmap::get3DMap(std::map<int, std::vector<unsigned char> > & images,
std::map<int, std::vector<unsigned char> > & depths2d,
std::map<int, float> & depthConstants,
std::map<int, Transform> & localTransforms,
std::map<int, Transform> & optimizedPoses,
Transform & mapCorrection) const
std::map<int, Transform> & poses,
bool optimized,
bool full) const
{
if(_memory)
if(_memory && _memory->getLastWorkingSignature())
{
//Optimize the map
UDEBUG("Optimize map");
optimizedPoses = _optimizedPoses;
mapCorrection = _mapCorrection;
std::set<int> ids = _memory->getAllSignatureIds();
for(std::set<int>::iterator iter = ids.begin(); iter!=ids.end(); ++iter)
if(optimized)
{
UDEBUG("Adding %d ... ", *iter);
this->optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), full, poses);
}
else
{
std::multimap<int, std::pair<int, Transform> > constraints;
std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, full?-1:0, true);
_memory->getMetricConstraints(uKeys(ids), poses, constraints, full);
}
for(std::map<int, Transform>::iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
UDEBUG("Adding %d ... ", iter->first);
std::vector<unsigned char> image, depth, depth2d;
float depthConstant;
Transform localTransform;
_memory->getImageDepth(*iter, image, depth, depth2d, depthConstant, localTransform);
_memory->getImageDepth(iter->first, image, depth, depth2d, depthConstant, localTransform);
if(image.size())
{
images.insert(std::make_pair(*iter, image));
images.insert(std::make_pair(iter->first, image));
}
if(depth.size())
{
depths.insert(std::make_pair(*iter, depth));
depths.insert(std::make_pair(iter->first, depth));
}
if(depth2d.size())
{
depths2d.insert(std::make_pair(*iter, depth2d));
depths2d.insert(std::make_pair(iter->first, depth2d));
}
if(depthConstant > 0)
{
depthConstants.insert(std::make_pair(*iter, depthConstant));
depthConstants.insert(std::make_pair(iter->first, depthConstant));
}
if(!localTransform.isNull())
{
localTransforms.insert(std::make_pair(*iter, localTransform));
localTransforms.insert(std::make_pair(iter->first, localTransform));
}
}
}

85
corelib/src/RtabmapThread.cpp
View File

@@ -29,6 +29,7 @@
#include <rtabmap/utilite/UEventsManager.h>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UConversion.h>
namespace rtabmap {
@@ -76,7 +77,7 @@ void RtabmapThread::clearBufferedData()
_imageMutex.unlock();
}
void RtabmapThread::publishMap() const
void RtabmapThread::publishMap(bool optimized, bool full) const
{
std::map<int, std::vector<unsigned char> > images;
std::map<int, std::vector<unsigned char> > depths;
@@ -84,7 +85,6 @@ void RtabmapThread::publishMap() const
std::map<int, float> depthConstants;
std::map<int, Transform> localTransforms;
std::map<int, Transform> poses;
Transform mapCorrection;
_rtabmap->get3DMap(images,
depths,
@@ -92,15 +92,15 @@ void RtabmapThread::publishMap() const
depthConstants,
localTransforms,
poses,
mapCorrection);
optimized,
full);
this->post(new RtabmapEvent3DMap(images,
depths,
depths2d,
depthConstants,
localTransforms,
poses,
mapCorrection));
poses));
}
void RtabmapThread::mainLoopKill()
@@ -156,6 +156,12 @@ void RtabmapThread::mainLoop()
case kStateGeneratingLocalGraph:
_rtabmap->generateGraph(parameters.at("path"), atoi(parameters.at("id").c_str()), atoi(parameters.at("margin").c_str()));
break;
case kStateGeneratingTOROGraph:
_rtabmap->generateTOROGraph(parameters.at("path"), atoi(parameters.at("optimized").c_str())!=0, false);
break;
case kStateGeneratingTOROGraphFull:
_rtabmap->generateTOROGraph(parameters.at("path"), atoi(parameters.at("optimized").c_str())!=0, true);
break;
case kStateDeletingMemory:
if(!parameters.at("path").empty())
{
@@ -168,7 +174,10 @@ void RtabmapThread::mainLoop()
this->clearBufferedData();
break;
case kStatePublishingMap:
this->publishMap();
this->publishMap(atoi(parameters.at("optimized").c_str())!=0, false);
break;
case kStatePublishingMapFull:
this->publishMap(atoi(parameters.at("optimized").c_str())!=0, true);
break;
case kStateTriggeringMap:
_rtabmap->triggerNewMap();
@@ -223,26 +232,47 @@ void RtabmapThread::handleEvent(UEvent* event)
}
else if(cmd == RtabmapEventCmd::kCmdGenerateGraph)
{
if(!rtabmapEvent->getStr().empty())
{
ULOGGER_DEBUG("CMD_GENERATE_GRAPH");
ParametersMap param;
param.insert(ParametersPair("path", rtabmapEvent->getStr()));
pushNewState(kStateGeneratingGraph, param);
}
UASSERT(!rtabmapEvent->getStr().empty());
ULOGGER_DEBUG("CMD_GENERATE_GRAPH");
ParametersMap param;
param.insert(ParametersPair("path", rtabmapEvent->getStr()));
pushNewState(kStateGeneratingGraph, param);
}
else if(cmd == RtabmapEventCmd::kCmdGenerateLocalGraph)
{
std::list<std::string> values = uSplit(rtabmapEvent->getStr(), ';');
if(values.size() == 3)
{
ULOGGER_DEBUG("CMD_GENERATE_LOCAL_GRAPH");
ParametersMap param;
param.insert(ParametersPair("path", *values.begin()));
param.insert(ParametersPair("id", *(++values.begin())));
param.insert(ParametersPair("margin", *values.rbegin()));
pushNewState(kStateGeneratingLocalGraph, param);
}
UASSERT(values.size() == 3);
ULOGGER_DEBUG("CMD_GENERATE_LOCAL_GRAPH");
ParametersMap param;
param.insert(ParametersPair("path", *values.begin()));
param.insert(ParametersPair("id", *(++values.begin())));
param.insert(ParametersPair("margin", *values.rbegin()));
pushNewState(kStateGeneratingLocalGraph, param);
}
else if(cmd == RtabmapEventCmd::kCmdGenerateTOROGraph)
{
UASSERT(!rtabmapEvent->getStr().empty());
ULOGGER_DEBUG("CMD_GENERATE_TORO_GRAPH");
ParametersMap param;
param.insert(ParametersPair("path", rtabmapEvent->getStr()));
param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt())));
pushNewState(kStateGeneratingTOROGraph, param);
}
else if(cmd == RtabmapEventCmd::kCmdGenerateTOROGraphFull)
{
UASSERT(!rtabmapEvent->getStr().empty());
ULOGGER_DEBUG("CMD_GENERATE_TORO_GRAPH_FULL");
ParametersMap param;
param.insert(ParametersPair("path", rtabmapEvent->getStr()));
param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt())));
pushNewState(kStateGeneratingTOROGraphFull, param);
}
else if(cmd == RtabmapEventCmd::kCmdDeleteMemory)
{
@@ -259,7 +289,16 @@ void RtabmapThread::handleEvent(UEvent* event)
else if(cmd == RtabmapEventCmd::kCmdPublish3DMap)
{
ULOGGER_DEBUG("CMD_PUBLISH_MAP");
pushNewState(kStatePublishingMap);
ParametersMap param;
param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt())));
pushNewState(kStatePublishingMap, param);
}
else if(cmd == RtabmapEventCmd::kCmdPublish3DMapFull)
{
ULOGGER_DEBUG("CMD_PUBLISH_MAP_FULL");
ParametersMap param;
param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt())));
pushNewState(kStatePublishingMapFull, param);
}
else if(cmd == RtabmapEventCmd::kCmdTriggerNewMap)
{

12
corelib/src/util3d.cpp
View File

@@ -1266,12 +1266,12 @@ int getCorrespondencesCount(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & clo
pcl::search::KdTree<pcl::PointXYZ>::Ptr kdTree(new pcl::search::KdTree<pcl::PointXYZ>);
kdTree->setInputCloud(cloud_target);
int count = 0;
float sqrdMaxDistance = maxDistance * maxDistance;
for(unsigned int i=0; i<cloud_source->size(); ++i)
{
std::vector<int> ind(1);
std::vector<float> dist(1);
kdTree->nearestKSearch(cloud_source->at(i), 1, ind, dist);
if(dist[0] < maxDistance)
if(kdTree->nearestKSearch(cloud_source->at(i), 1, ind, dist) && dist[0] < sqrdMaxDistance)
{
++count;
}
@@ -1545,16 +1545,22 @@ void optimizeTOROGraph(
return;
}
}
pg.buildMST(pg.vertices.begin()->first); // pg.buildSimpleTree();
pg.buildMST(pg.vertices.begin()->first);
UDEBUG("Initial guess...");
pg.initializeOnTree(); // optional
pg.initializeTreeParameters();
UDEBUG("Building TORO tree... (if a crash happens just after this msg, "
"TORO is not able to find the root of the graph!)");
pg.initializeOptimization();
UDEBUG("TORO iterate begin");
for (int i=0; i<toroIterations; i++)
{
pg.iterate();
}
UDEBUG("TORO iterate end");
optimizedPoses.clear();
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)