mirror of
https://github.com/introlab/rtabmap.git
synced 2026-09-02 01:20:25 +08:00
New classes: Link and GraphViewer (TORO graph visualization and laser scans occupancy grid)
New parameter: RGBD/ToroIterations=100 Rtabmap: added getGraph() method to get TORO poses/link constraints Fixed ICP correspondences ratio over 1 CloudViewer: added frustum culling and camera view up z lock in render(), moving camera using arrow keys, Menu options: Camera far plane clipping, background color DatabaseViewer: added 3D Map view and Graph view MainWindow: Moved main widget (loop closure image status) to a dock widget, added Graph view. git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1075 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
42
corelib/include/rtabmap/core/Link.h
Normal file
42
corelib/include/rtabmap/core/Link.h
Normal file
@@ -0,0 +1,42 @@
|
||||
/*
|
||||
* Link.h
|
||||
*
|
||||
* Created on: 2014-01-29
|
||||
* Author: mathieu
|
||||
*/
|
||||
|
||||
#ifndef LINK_H_
|
||||
#define LINK_H_
|
||||
|
||||
#include <rtabmap/core/Transform.h>
|
||||
|
||||
namespace rtabmap {
|
||||
|
||||
class Link
|
||||
{
|
||||
public:
|
||||
enum Type {kNeighbor, kGlobalClosure, kLocalSpaceClosure, kLocalTimeClosure};
|
||||
Link(int from, int to, const Transform & transform, Type type) :
|
||||
from_(from),
|
||||
to_(to),
|
||||
transform_(transform),
|
||||
type_(type)
|
||||
{
|
||||
}
|
||||
|
||||
int from() const {return from_;}
|
||||
int to() const {return to_;}
|
||||
const Transform & transform() const {return transform_;}
|
||||
Type type() const {return type_;}
|
||||
|
||||
private:
|
||||
int from_;
|
||||
int to_;
|
||||
Transform transform_;
|
||||
Type type_;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
|
||||
#endif /* LINK_H_ */
|
||||
@@ -25,6 +25,7 @@
|
||||
#include "rtabmap/utilite/UEventsHandler.h"
|
||||
#include "rtabmap/core/Parameters.h"
|
||||
#include "rtabmap/core/Image.h"
|
||||
#include "rtabmap/core/Link.h"
|
||||
#include <typeinfo>
|
||||
#include <list>
|
||||
#include <map>
|
||||
@@ -144,7 +145,7 @@ public:
|
||||
void getMetricConstraints(
|
||||
const std::vector<int> & ids,
|
||||
std::map<int, Transform> & poses,
|
||||
std::multimap<int, std::pair<int, Transform> > & links,
|
||||
std::multimap<int, Link> & links,
|
||||
bool lookInDatabase = false);
|
||||
Transform computeVisualTransform(int oldId, int newId) const;
|
||||
Transform computeVisualTransform(const Signature & oldS, const Signature & newS) const;
|
||||
|
||||
@@ -204,6 +204,7 @@ class RTABMAP_EXP Parameters
|
||||
RTABMAP_PARAM(RGBD, ScanMatchingSize, int, 0, "Laser scan matching history for odometry correction (laser scans are required). Set to 0 to disable odometry correction.");
|
||||
RTABMAP_PARAM(RGBD, LinearUpdate, float, 0.0, "Min linear displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
|
||||
RTABMAP_PARAM(RGBD, AngularUpdate, float, 0.0, "Min angular displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
|
||||
RTABMAP_PARAM(RGBD, ToroIterations, int, 100, "TORO graph optimization iterations")
|
||||
|
||||
// Local loop closure detection
|
||||
RTABMAP_PARAM(RGBD, LocalLoopDetectionTime, bool, true, "Detection over all locations in STM.");
|
||||
|
||||
@@ -25,6 +25,7 @@
|
||||
#include "rtabmap/core/Parameters.h"
|
||||
#include "rtabmap/core/Image.h"
|
||||
#include "rtabmap/core/Statistics.h"
|
||||
#include "rtabmap/core/Link.h"
|
||||
|
||||
#include <opencv2/core/core.hpp>
|
||||
#include <list>
|
||||
@@ -101,8 +102,13 @@ public:
|
||||
std::map<int, float> & depthConstants,
|
||||
std::map<int, Transform> & localTransforms,
|
||||
std::map<int, Transform> & poses,
|
||||
std::multimap<int, Link> & constraints,
|
||||
bool optimized,
|
||||
bool full) const;
|
||||
void getGraph(std::map<int, Transform> & poses,
|
||||
std::multimap<int, Link> & constraints,
|
||||
bool optimized,
|
||||
bool full);
|
||||
|
||||
std::map<int, Transform> getOptimizedWMPosesInRadius(int fromId, int maxNearestNeighbors, float radius, int maxDiffID, int & nearestId) const;
|
||||
void adjustLikelihood(std::map<int, float> & likelihood) const;
|
||||
@@ -113,7 +119,7 @@ private:
|
||||
void optimizeCurrentMap(int id,
|
||||
bool lookInDatabase,
|
||||
std::map<int, Transform> & optimizedPoses,
|
||||
std::multimap<int, std::pair<int, Transform> > * constraints = 0) const;
|
||||
std::multimap<int, Link> * constraints = 0) const;
|
||||
|
||||
void setupLogFiles(bool overwrite = false);
|
||||
void flushStatisticLogs();
|
||||
@@ -142,7 +148,7 @@ private:
|
||||
float _localDetectRadius;
|
||||
float _localDetectMaxNeighbors;
|
||||
int _localDetectMaxDiffID;
|
||||
bool _icpEnabled;
|
||||
int _toroIterations;
|
||||
std::string _databasePath;
|
||||
|
||||
int _lcHypothesisId;
|
||||
@@ -167,6 +173,7 @@ private:
|
||||
std::string _wDir;
|
||||
|
||||
std::map<int, Transform> _optimizedPoses;
|
||||
std::multimap<int, Link> _constraints;
|
||||
Transform _mapCorrection; // for localization mode
|
||||
Transform _mapTransform; // for localization mode
|
||||
};
|
||||
|
||||
@@ -58,8 +58,10 @@ public:
|
||||
kCmdGenerateTOROGraphFull, // params: path, optimized
|
||||
kCmdDeleteMemory, // params: path [optional]
|
||||
kCmdCleanDataBuffer,
|
||||
kCmdPublish3DMap,
|
||||
kCmdPublish3DMapFull,
|
||||
kCmdPublish3DMap, // params: optimized
|
||||
kCmdPublish3DMapFull, // params: optimized
|
||||
kCmdPublishGraphFull, // params: optimized
|
||||
kCmdPublishGraph, // params: optimized
|
||||
kCmdTriggerNewMap,
|
||||
kCmdPause};
|
||||
public:
|
||||
@@ -147,14 +149,16 @@ 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 std::map<int, Transform> & poses,
|
||||
const std::multimap<int, Link> & constraints) :
|
||||
UEvent(0),
|
||||
_images(images),
|
||||
_depths(depths),
|
||||
_depths2d(depths2d),
|
||||
_depthConstants(depthConstants),
|
||||
_localTransforms(localTransforms),
|
||||
_poses(poses)
|
||||
_poses(poses),
|
||||
_constraints(constraints)
|
||||
{}
|
||||
|
||||
virtual ~RtabmapEvent3DMap() {}
|
||||
@@ -165,6 +169,7 @@ 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 std::multimap<int, Link> & getConstraints() const {return _constraints;}
|
||||
|
||||
virtual std::string getClassName() const {return std::string("RtabmapEvent3DMap");}
|
||||
|
||||
@@ -175,6 +180,7 @@ private:
|
||||
std::map<int, float> _depthConstants;
|
||||
std::map<int, Transform> _localTransforms;
|
||||
std::map<int, Transform> _poses;
|
||||
std::multimap<int, Link> _constraints;
|
||||
};
|
||||
|
||||
} // namespace rtabmap
|
||||
|
||||
@@ -59,6 +59,9 @@ public:
|
||||
kStateCleanDataBuffer,
|
||||
kStatePublishingMap,
|
||||
kStatePublishingMapFull,
|
||||
kStatePublishingGraph,
|
||||
kStatePublishingGraphFull,
|
||||
Graph,
|
||||
kStateTriggeringMap
|
||||
};
|
||||
|
||||
@@ -80,6 +83,7 @@ private:
|
||||
void pushNewState(State newState, const ParametersMap & parameters = ParametersMap());
|
||||
void setDataBufferSize(int size);
|
||||
void publishMap(bool optimized, bool full) const;
|
||||
void publishGraph(bool optimized, bool full) const;
|
||||
|
||||
private:
|
||||
UMutex _stateMutex;
|
||||
|
||||
@@ -27,7 +27,7 @@
|
||||
#include <opencv2/imgproc/imgproc.hpp>
|
||||
#include <list>
|
||||
#include <vector>
|
||||
#include <rtabmap/core/Transform.h>
|
||||
#include <rtabmap/core/Link.h>
|
||||
|
||||
namespace rtabmap {
|
||||
|
||||
@@ -56,6 +56,7 @@ class RTABMAP_EXP Statistics
|
||||
RTABMAP_STATS(LocalLoop, Odom_corrected,);
|
||||
RTABMAP_STATS(LocalLoop, Time_closures,);
|
||||
RTABMAP_STATS(LocalLoop, Space_closure_id,);
|
||||
RTABMAP_STATS(LocalLoop, Space_nearest_id,);
|
||||
RTABMAP_STATS(LocalLoop, Space_neighbors,);
|
||||
RTABMAP_STATS(LocalLoop, Space_diff_id,);
|
||||
|
||||
@@ -117,6 +118,7 @@ public:
|
||||
void setLocalTransforms(const std::map<int, Transform> & localTransforms) {_localTransforms = localTransforms;}
|
||||
|
||||
void setPoses(const std::map<int, Transform> & poses) {_poses = poses;}
|
||||
void setConstraints(const std::multimap<int, Link> & constraints) {_constraints = constraints;}
|
||||
void setCurrentPose(const Transform & pose) {_currentPose = pose;}
|
||||
void setMapCorrection(const Transform & mapCorrection) {_mapCorrection = mapCorrection;}
|
||||
void setLoopClosureTransform(const Transform & loopClosureTransform) {_loopClosureTransform = loopClosureTransform;}
|
||||
@@ -141,6 +143,7 @@ public:
|
||||
const std::map<int, Transform> & getLocalTransforms() const {return _localTransforms;}
|
||||
|
||||
const std::map<int, Transform> & poses() const {return _poses;}
|
||||
const std::multimap<int, Link> & constraints() const {return _constraints;}
|
||||
const Transform & currentPose() const {return _currentPose;}
|
||||
const Transform & mapCorrection() const {return _mapCorrection;}
|
||||
const Transform & loopClosureTransform() const {return _loopClosureTransform;}
|
||||
@@ -171,6 +174,7 @@ private:
|
||||
std::map<int, Transform> _localTransforms;
|
||||
|
||||
std::map<int, Transform> _poses;
|
||||
std::multimap<int, Link> _constraints;
|
||||
Transform _currentPose;
|
||||
Transform _mapCorrection;
|
||||
Transform _loopClosureTransform;
|
||||
|
||||
@@ -12,7 +12,7 @@
|
||||
#include <list>
|
||||
#include <string>
|
||||
|
||||
#include <rtabmap/core/Transform.h>
|
||||
#include <rtabmap/core/Link.h>
|
||||
#include <rtabmap/utilite/UThread.h>
|
||||
#include <pcl/common/eigen.h>
|
||||
#include <pcl/point_types.h>
|
||||
@@ -342,20 +342,32 @@ pcl::PolygonMesh::Ptr RTABMAP_EXP createMesh(const pcl::PointCloud<pcl::PointXYZ
|
||||
|
||||
void RTABMAP_EXP optimizeTOROGraph(
|
||||
const std::map<int, Transform> & poses,
|
||||
const std::multimap<int, std::pair<int, Transform> > & edgeConstraints,
|
||||
int toroIterations,
|
||||
const std::multimap<int, Link> & edgeConstraints,
|
||||
std::map<int, Transform> & optimizedPoses,
|
||||
Transform & mapCorrection);
|
||||
Transform & mapCorrection,
|
||||
int toroIterations = 100,
|
||||
bool toroInitialGuess = true,
|
||||
std::list<std::map<int, Transform> > * intermediateGraphes = 0);
|
||||
|
||||
bool RTABMAP_EXP saveTOROGraph(
|
||||
const std::string & fileName,
|
||||
const std::map<int, Transform> & poses,
|
||||
const std::multimap<int, std::pair<int, Transform> > & edgeConstraints);
|
||||
const std::multimap<int, Link> & edgeConstraints);
|
||||
|
||||
bool RTABMAP_EXP loadTOROGraph(const std::string & fileName,
|
||||
std::map<int, Transform> & poses,
|
||||
std::multimap<int, std::pair<int, Transform> > & edgeConstraints);
|
||||
|
||||
cv::Mat RTABMAP_EXP create2DMap(const std::map<int, Transform> & poses,
|
||||
const std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > & scans,
|
||||
float delta,
|
||||
float & xMin,
|
||||
float & yMin);
|
||||
|
||||
void RTABMAP_EXP rayTrace(const cv::Point2i & start,
|
||||
const cv::Point2i & end,
|
||||
cv::Mat & grid);
|
||||
|
||||
} // namespace util3d
|
||||
} // namespace rtabmap
|
||||
|
||||
|
||||
@@ -661,7 +661,6 @@ void DBDriverSqlite3::getPoseQuery(int signatureId, Transform & pose, int & mapI
|
||||
{
|
||||
if(_ppDb && signatureId)
|
||||
{
|
||||
UTimer timer;
|
||||
int rc = SQLITE_OK;
|
||||
sqlite3_stmt * ppStmt = 0;
|
||||
std::stringstream query;
|
||||
@@ -700,8 +699,6 @@ void DBDriverSqlite3::getPoseQuery(int signatureId, Transform & pose, int & mapI
|
||||
// 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());
|
||||
}
|
||||
}
|
||||
|
||||
@@ -864,8 +861,6 @@ void DBDriverSqlite3::loadLoopClosuresQuery(int nodeId, std::map<int, Transform>
|
||||
childIds.clear();
|
||||
if(_ppDb)
|
||||
{
|
||||
UTimer timer;
|
||||
timer.start();
|
||||
int rc = SQLITE_OK;
|
||||
sqlite3_stmt * ppStmt = 0;
|
||||
std::stringstream query;
|
||||
@@ -923,8 +918,6 @@ void DBDriverSqlite3::loadLoopClosuresQuery(int nodeId, std::map<int, Transform>
|
||||
// Finalize (delete) the statement
|
||||
rc = sqlite3_finalize(ppStmt);
|
||||
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
|
||||
|
||||
UDEBUG("time =%fs", timer.getElapsedTime());
|
||||
}
|
||||
}
|
||||
|
||||
@@ -952,7 +945,7 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
|
||||
|
||||
for(std::list<int>::const_iterator iter=ids.begin(); iter!=ids.end(); ++iter)
|
||||
{
|
||||
ULOGGER_DEBUG("Loading node %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());
|
||||
@@ -1028,7 +1021,7 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
|
||||
|
||||
for(std::list<Signature*>::const_iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
|
||||
{
|
||||
ULOGGER_DEBUG("Loading words of %d...", (*iter)->id());
|
||||
//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());
|
||||
@@ -1428,7 +1421,7 @@ void DBDriverSqlite3::loadLinksQuery(std::list<Signature *> & signatures) const
|
||||
//reset
|
||||
rc = sqlite3_reset(ppStmt);
|
||||
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
|
||||
UINFO("time=%fs, node=%d, neighbors.size=%d, loopIds=%d, childIds=%d", timer.ticks(), (*iter)->id(), neighbors.size(), loopIds.size(), childIds.size());
|
||||
UDEBUG("time=%fs, node=%d, neighbors.size=%d, loopIds=%d, childIds=%d", timer.ticks(), (*iter)->id(), neighbors.size(), loopIds.size(), childIds.size());
|
||||
}
|
||||
|
||||
// Finalize (delete) the statement
|
||||
|
||||
@@ -200,6 +200,7 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
|
||||
if(postInitEvents) UEventsManager::post(new RtabmapEventInit("Loading dictionary..."));
|
||||
_dbDriver->load(_vwd);
|
||||
UDEBUG("%d words loaded!", _vwd->getUnusedWordsSize());
|
||||
_vwd->update();
|
||||
if(postInitEvents) UEventsManager::post(new RtabmapEventInit(uFormat("Loading dictionary, done! (%d words)", (int)_vwd->getUnusedWordsSize())));
|
||||
}
|
||||
|
||||
@@ -1851,7 +1852,7 @@ Transform Memory::computeIcpTransform(const Signature & oldS, const Signature &
|
||||
|
||||
// verify if there are enough correspondences
|
||||
int correspondences = util3d::getCorrespondencesCount(newCloud, oldCloud, _icp2MaxCorrespondenceDistance);
|
||||
correspondencesRatio = float(correspondences)/float(oldCloud->size());
|
||||
correspondencesRatio = float(correspondences)/float(oldCloud->size()>newCloud->size()?oldCloud->size():newCloud->size());
|
||||
|
||||
UDEBUG("hasConverged=%s, fitness=%f, correspondences=%d/%d (%f%%)",
|
||||
hasConverged?"true":"false",
|
||||
@@ -3198,7 +3199,7 @@ std::set<int> Memory::reactivateSignatures(const std::list<int> & ids, unsigned
|
||||
void Memory::getMetricConstraints(
|
||||
const std::vector<int> & ids,
|
||||
std::map<int, Transform> & poses,
|
||||
std::multimap<int, std::pair<int, Transform> > & links,
|
||||
std::multimap<int, Link> & links,
|
||||
bool lookInDatabase)
|
||||
{
|
||||
for(unsigned int i=0; i<ids.size(); ++i)
|
||||
@@ -3221,18 +3222,18 @@ void Memory::getMetricConstraints(
|
||||
if(!jter->second.isNull() && uContains(poses, jter->first))
|
||||
{
|
||||
bool edgeAlreadyAdded = false;
|
||||
for(std::multimap<int, std::pair<int, Transform> >::iterator iter = links.lower_bound(jter->first);
|
||||
for(std::multimap<int, Link>::iterator iter = links.lower_bound(jter->first);
|
||||
iter != links.end() && iter->first == jter->first;
|
||||
++iter)
|
||||
{
|
||||
if(iter->second.first == ids[i])
|
||||
if(iter->second.to() == ids[i])
|
||||
{
|
||||
edgeAlreadyAdded = true;
|
||||
}
|
||||
}
|
||||
if(!edgeAlreadyAdded)
|
||||
{
|
||||
links.insert(std::make_pair(ids[i], *jter));
|
||||
links.insert(std::make_pair(ids[i], Link(ids[i], jter->first, jter->second, Link::kNeighbor)));
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -3243,7 +3244,7 @@ void Memory::getMetricConstraints(
|
||||
{
|
||||
if(!jter->second.isNull() && uContains(poses, jter->first))
|
||||
{
|
||||
links.insert(std::make_pair(ids[i], *jter));
|
||||
links.insert(std::make_pair(ids[i], Link(ids[i], jter->first, jter->second, Link::kGlobalClosure)));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -86,6 +86,7 @@ Rtabmap::Rtabmap() :
|
||||
_localDetectRadius(Parameters::defaultRGBDLocalLoopDetectionRadius()),
|
||||
_localDetectMaxNeighbors(Parameters::defaultRGBDLocalLoopDetectionNeighbors()),
|
||||
_localDetectMaxDiffID(Parameters::defaultRGBDLocalLoopDetectionMaxDiffID()),
|
||||
_toroIterations(Parameters::defaultRGBDToroIterations()),
|
||||
_databasePath(""),
|
||||
_lcHypothesisId(0),
|
||||
_lcHypothesisValue(0),
|
||||
@@ -332,6 +333,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
|
||||
Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionRadius(), _localDetectRadius);
|
||||
Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionNeighbors(), _localDetectMaxNeighbors);
|
||||
Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionMaxDiffID(), _localDetectMaxDiffID);
|
||||
Parameters::parse(parameters, Parameters::kRGBDToroIterations(), _toroIterations);
|
||||
|
||||
// RGB-D SLAM stuff
|
||||
if((iter=parameters.find(Parameters::kLccIcpType())) != parameters.end())
|
||||
@@ -358,7 +360,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
|
||||
//generate map
|
||||
if(_memory->getLastWorkingSignature())
|
||||
{
|
||||
optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), false, _optimizedPoses);
|
||||
optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), false, _optimizedPoses, &_constraints);
|
||||
}
|
||||
}
|
||||
else
|
||||
@@ -558,6 +560,7 @@ void Rtabmap::triggerNewMap()
|
||||
int mapId = _memory->incrementMapId();
|
||||
UINFO("New map triggerred, new map = %d", mapId);
|
||||
_optimizedPoses.clear();
|
||||
_constraints.clear();
|
||||
}
|
||||
}
|
||||
|
||||
@@ -598,7 +601,7 @@ void Rtabmap::generateTOROGraph(const std::string & path, bool optimized, bool f
|
||||
if(_memory && _memory->getLastWorkingSignature())
|
||||
{
|
||||
std::map<int, Transform> poses;
|
||||
std::multimap<int, std::pair<int, Transform> > constraints;
|
||||
std::multimap<int, Link> constraints;
|
||||
|
||||
if(optimized)
|
||||
{
|
||||
@@ -621,6 +624,7 @@ void Rtabmap::resetMemory(bool dbOverwritten)
|
||||
_lcHypothesisId = 0;
|
||||
_lastProcessTime = 0.0;
|
||||
_optimizedPoses.clear();
|
||||
_constraints.clear();
|
||||
_mapCorrection.setIdentity();
|
||||
_mapTransform.setIdentity();
|
||||
|
||||
@@ -629,7 +633,7 @@ void Rtabmap::resetMemory(bool dbOverwritten)
|
||||
_memory->init(getDatabasePath(), dbOverwritten);
|
||||
if(_memory->getLastWorkingSignature())
|
||||
{
|
||||
optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), false, _optimizedPoses);
|
||||
optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), false, _optimizedPoses, &_constraints);
|
||||
}
|
||||
if(_bayesFilter)
|
||||
{
|
||||
@@ -745,6 +749,7 @@ bool Rtabmap::process(const Image & image)
|
||||
int mapId = _memory->incrementMapId();
|
||||
UWARN("Odometry is reset (transform identity detected). A new map (%d) is created!", mapId);
|
||||
_optimizedPoses.clear();
|
||||
_constraints.clear();
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -864,6 +869,11 @@ bool Rtabmap::process(const Image & image)
|
||||
timeScanMatching = timer.ticks();
|
||||
ULOGGER_INFO("timeScanMatching=%fs", timeScanMatching);
|
||||
|
||||
if(signature->getNeighbors().size() == 1)
|
||||
{
|
||||
_constraints.insert(std::make_pair(signature->id(), Link(signature->id(), signature->getNeighbors().begin()->first, signature->getNeighbors().begin()->second, Link::kNeighbor)));
|
||||
}
|
||||
|
||||
//============================================================
|
||||
// Local loop closure in TIME
|
||||
//============================================================
|
||||
@@ -1250,6 +1260,7 @@ bool Rtabmap::process(const Image & image)
|
||||
|
||||
int localSpaceDetectionPosesCount = 0;
|
||||
int localSpaceClosureId = 0;
|
||||
int localSpaceNearestId = 0;
|
||||
if(_lcHypothesisId == 0 &&
|
||||
_localLoopClosureDetectionSpace &&
|
||||
signature->getDepth2D().size())
|
||||
@@ -1259,8 +1270,8 @@ bool Rtabmap::process(const Image & image)
|
||||
//============================================================
|
||||
// get all nodes in radius of the current node
|
||||
std::map<int, Transform> poses;
|
||||
int oldId = 0;
|
||||
poses = this->getOptimizedWMPosesInRadius(signature->id(), _localDetectMaxNeighbors, _localDetectRadius, _localDetectMaxDiffID, oldId);
|
||||
localSpaceNearestId = 0;
|
||||
poses = this->getOptimizedWMPosesInRadius(signature->id(), _localDetectMaxNeighbors, _localDetectRadius, _localDetectMaxDiffID, localSpaceNearestId);
|
||||
|
||||
// add current node to poses
|
||||
UASSERT(_optimizedPoses.find(signature->id()) != _optimizedPoses.end());
|
||||
@@ -1269,18 +1280,18 @@ bool Rtabmap::process(const Image & image)
|
||||
localSpaceDetectionPosesCount = poses.size()-1;
|
||||
//The nearest will be the reference for a loop closure transform
|
||||
if(poses.size() &&
|
||||
oldId &&
|
||||
signature->getChildLoopClosureIds().find(oldId) == signature->getChildLoopClosureIds().end())
|
||||
localSpaceNearestId &&
|
||||
signature->getChildLoopClosureIds().find(localSpaceNearestId) == signature->getChildLoopClosureIds().end())
|
||||
{
|
||||
Transform t = _memory->computeScanMatchingTransform(signature->id(), oldId, poses);
|
||||
Transform t = _memory->computeScanMatchingTransform(signature->id(), localSpaceNearestId, poses);
|
||||
if(!t.isNull())
|
||||
{
|
||||
localSpaceClosureId = oldId;
|
||||
localSpaceClosureId = localSpaceNearestId;
|
||||
UDEBUG("Add local loop closure in SPACE (%d->%d) %s",
|
||||
signature->id(),
|
||||
oldId,
|
||||
localSpaceNearestId,
|
||||
t.prettyPrint().c_str());
|
||||
_memory->addLoopClosureLink(oldId, signature->id(), t, false);
|
||||
_memory->addLoopClosureLink(localSpaceNearestId, signature->id(), t, false);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1301,7 +1312,7 @@ bool Rtabmap::process(const Image & image)
|
||||
{
|
||||
UINFO("Update map correction: SLAM mode");
|
||||
// SLAM mode!
|
||||
optimizeCurrentMap(signature->id(), false, _optimizedPoses);
|
||||
optimizeCurrentMap(signature->id(), false, _optimizedPoses, &_constraints);
|
||||
}
|
||||
else if(_lcHypothesisId > 0 || localSpaceClosureId > 0 || signaturesRetrieved.size())
|
||||
{
|
||||
@@ -1311,7 +1322,7 @@ bool Rtabmap::process(const Image & image)
|
||||
if(signaturesRetrieved.size() || _optimizedPoses.find(oldId) == _optimizedPoses.end())
|
||||
{
|
||||
// update optimized poses
|
||||
optimizeCurrentMap(_retrievedId, false, _optimizedPoses);
|
||||
optimizeCurrentMap(_retrievedId, false, _optimizedPoses, &_constraints);
|
||||
}
|
||||
|
||||
if(_optimizedPoses.find(oldId) == _optimizedPoses.end())
|
||||
@@ -1385,11 +1396,15 @@ bool Rtabmap::process(const Image & image)
|
||||
statistics_.addStatistic(Statistics::kLocalLoopTime_closures(), localLoopClosuresInTimeFound);
|
||||
statistics_.addStatistic(Statistics::kLocalLoopSpace_neighbors(), localSpaceDetectionPosesCount);
|
||||
statistics_.addStatistic(Statistics::kLocalLoopSpace_closure_id(), localSpaceClosureId);
|
||||
statistics_.addStatistic(Statistics::kLocalLoopSpace_nearest_id(), localSpaceNearestId);
|
||||
if(localSpaceClosureId)
|
||||
{
|
||||
statistics_.setLocalLoopClosureId(localSpaceClosureId);
|
||||
int d1 = abs(signature->id() - localSpaceClosureId);
|
||||
int d2 = abs(localSpaceClosureId - _memory->getLastGlobalLoopClosureChildId());
|
||||
}
|
||||
if(localSpaceNearestId)
|
||||
{
|
||||
int d1 = abs(signature->id() - localSpaceNearestId);
|
||||
int d2 = abs(localSpaceNearestId - _memory->getLastGlobalLoopClosureChildId());
|
||||
int d3 = abs(signature->id() - _memory->getLastGlobalLoopClosureParentId());
|
||||
int d = d1<=d2?d1:d2;
|
||||
d = d <= d3?d:d3;
|
||||
@@ -1555,6 +1570,7 @@ bool Rtabmap::process(const Image & image)
|
||||
{
|
||||
UDEBUG("removing optimized pose %d...", *iter);
|
||||
_optimizedPoses.erase(*iter);
|
||||
_constraints.erase(*iter);
|
||||
}
|
||||
|
||||
|
||||
@@ -1581,6 +1597,7 @@ bool Rtabmap::process(const Image & image)
|
||||
if(_rgbdSlamMode)
|
||||
{
|
||||
statistics_.setPoses(_optimizedPoses);
|
||||
statistics_.setConstraints(_constraints);
|
||||
statistics_.setMapCorrection(_mapCorrection);
|
||||
statistics_.setCurrentPose(_mapCorrection * currentRawOdomPose);
|
||||
UINFO("Set map correction = %s", _mapCorrection.prettyPrint().c_str());
|
||||
@@ -1885,7 +1902,7 @@ void Rtabmap::optimizeCurrentMap(
|
||||
int id,
|
||||
bool lookInDatabase,
|
||||
std::map<int, Transform> & optimizedPoses,
|
||||
std::multimap<int, std::pair<int, Transform> > * constraints) const
|
||||
std::multimap<int, Link> * constraints) const
|
||||
{
|
||||
//Optimize the map
|
||||
optimizedPoses.clear();
|
||||
@@ -1897,7 +1914,7 @@ void Rtabmap::optimizeCurrentMap(
|
||||
UDEBUG("ids=%d", (int)ids.size());
|
||||
|
||||
std::map<int, Transform> poses;
|
||||
std::multimap<int, std::pair<int, Transform> > edgeConstraints;
|
||||
std::multimap<int, Link> edgeConstraints;
|
||||
_memory->getMetricConstraints(uKeys(ids), poses, edgeConstraints, lookInDatabase);
|
||||
UDEBUG("poses=%d, edgeConstraints=%d", (int)poses.size(), (int)edgeConstraints.size());
|
||||
|
||||
@@ -1932,16 +1949,16 @@ void Rtabmap::optimizeCurrentMap(
|
||||
|
||||
//
|
||||
std::map<int, Transform> posesToro;
|
||||
std::multimap<int, std::pair<int, Transform> > edgeConstraintsToro;
|
||||
std::multimap<int, Link> 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();
|
||||
for(std::multimap<int, Link>::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)));
|
||||
edgeConstraintsToro.insert(std::make_pair(rtabmapToToro.at(iter->first), Link(rtabmapToToro.at(iter->first), rtabmapToToro.at(iter->second.to()), iter->second.transform(), iter->second.type())));
|
||||
}
|
||||
|
||||
if(posesToro.size() > 1 && edgeConstraintsToro.size() > 0)
|
||||
@@ -1953,7 +1970,7 @@ void Rtabmap::optimizeCurrentMap(
|
||||
|
||||
std::map<int, Transform> optimizedPosesToro;
|
||||
Transform mapCorrectionToro;
|
||||
util3d::optimizeTOROGraph(posesToro, edgeConstraintsToro, 100, optimizedPosesToro, mapCorrectionToro);
|
||||
util3d::optimizeTOROGraph(posesToro, edgeConstraintsToro, optimizedPosesToro, mapCorrectionToro, _toroIterations, true);
|
||||
|
||||
//UDEBUG("saving optimized graph...");
|
||||
//util3d::saveTOROGraph("toroOptimized.graph", optimizedPosesToro, edgeConstraintsToro);
|
||||
@@ -2091,6 +2108,7 @@ void Rtabmap::get3DMap(std::map<int, std::vector<unsigned char> > & images,
|
||||
std::map<int, float> & depthConstants,
|
||||
std::map<int, Transform> & localTransforms,
|
||||
std::map<int, Transform> & poses,
|
||||
std::multimap<int, Link> & constraints,
|
||||
bool optimized,
|
||||
bool full) const
|
||||
{
|
||||
@@ -2098,47 +2116,72 @@ void Rtabmap::get3DMap(std::map<int, std::vector<unsigned char> > & images,
|
||||
{
|
||||
if(optimized)
|
||||
{
|
||||
this->optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), full, poses);
|
||||
this->optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), full, poses, &constraints);
|
||||
}
|
||||
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)
|
||||
std::set<int> ids = _memory->getWorkingMem(); // STM + WM
|
||||
ids.insert(_memory->getStMem().begin(), _memory->getStMem().end());
|
||||
if(full)
|
||||
{
|
||||
ids = _memory->getAllSignatureIds(); // STM + WM + LTM
|
||||
}
|
||||
|
||||
for(std::set<int>::iterator iter = ids.begin(); iter!=ids.end(); ++iter)
|
||||
{
|
||||
UDEBUG("Adding %d ... ", iter->first);
|
||||
std::vector<unsigned char> image, depth, depth2d;
|
||||
float depthConstant;
|
||||
Transform localTransform;
|
||||
_memory->getImageDepth(iter->first, image, depth, depth2d, depthConstant, localTransform);
|
||||
_memory->getImageDepth(*iter, image, depth, depth2d, depthConstant, localTransform);
|
||||
|
||||
if(image.size())
|
||||
{
|
||||
images.insert(std::make_pair(iter->first, image));
|
||||
images.insert(std::make_pair(*iter, image));
|
||||
}
|
||||
if(depth.size())
|
||||
{
|
||||
depths.insert(std::make_pair(iter->first, depth));
|
||||
depths.insert(std::make_pair(*iter, depth));
|
||||
}
|
||||
if(depth2d.size())
|
||||
{
|
||||
depths2d.insert(std::make_pair(iter->first, depth2d));
|
||||
depths2d.insert(std::make_pair(*iter, depth2d));
|
||||
}
|
||||
if(depthConstant > 0)
|
||||
{
|
||||
depthConstants.insert(std::make_pair(iter->first, depthConstant));
|
||||
depthConstants.insert(std::make_pair(*iter, depthConstant));
|
||||
}
|
||||
if(!localTransform.isNull())
|
||||
{
|
||||
localTransforms.insert(std::make_pair(iter->first, localTransform));
|
||||
localTransforms.insert(std::make_pair(*iter, localTransform));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Rtabmap::getGraph(
|
||||
std::map<int, Transform> & poses,
|
||||
std::multimap<int, Link> & constraints,
|
||||
bool optimized,
|
||||
bool full)
|
||||
{
|
||||
if(_memory && _memory->getLastWorkingSignature())
|
||||
{
|
||||
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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Rtabmap::readParameters(const std::string & configFile, ParametersMap & parameters)
|
||||
{
|
||||
CSimpleIniA ini;
|
||||
|
||||
@@ -85,6 +85,7 @@ void RtabmapThread::publishMap(bool optimized, bool full) const
|
||||
std::map<int, float> depthConstants;
|
||||
std::map<int, Transform> localTransforms;
|
||||
std::map<int, Transform> poses;
|
||||
std::multimap<int, Link> constraints;
|
||||
|
||||
_rtabmap->get3DMap(images,
|
||||
depths,
|
||||
@@ -92,6 +93,7 @@ void RtabmapThread::publishMap(bool optimized, bool full) const
|
||||
depthConstants,
|
||||
localTransforms,
|
||||
poses,
|
||||
constraints,
|
||||
optimized,
|
||||
full);
|
||||
|
||||
@@ -100,9 +102,35 @@ void RtabmapThread::publishMap(bool optimized, bool full) const
|
||||
depths2d,
|
||||
depthConstants,
|
||||
localTransforms,
|
||||
poses));
|
||||
poses,
|
||||
constraints));
|
||||
}
|
||||
|
||||
void RtabmapThread::publishGraph(bool optimized, bool full) const
|
||||
{
|
||||
std::map<int, std::vector<unsigned char> > images;
|
||||
std::map<int, std::vector<unsigned char> > depths;
|
||||
std::map<int, std::vector<unsigned char> > depths2d;
|
||||
std::map<int, float> depthConstants;
|
||||
std::map<int, Transform> localTransforms;
|
||||
std::map<int, Transform> poses;
|
||||
std::multimap<int, Link> constraints;
|
||||
|
||||
_rtabmap->getGraph(poses,
|
||||
constraints,
|
||||
optimized,
|
||||
full);
|
||||
|
||||
this->post(new RtabmapEvent3DMap(images,
|
||||
depths,
|
||||
depths2d,
|
||||
depthConstants,
|
||||
localTransforms,
|
||||
poses,
|
||||
constraints));
|
||||
}
|
||||
|
||||
|
||||
void RtabmapThread::mainLoopKill()
|
||||
{
|
||||
this->clearBufferedData();
|
||||
@@ -179,6 +207,12 @@ void RtabmapThread::mainLoop()
|
||||
case kStatePublishingMapFull:
|
||||
this->publishMap(atoi(parameters.at("optimized").c_str())!=0, true);
|
||||
break;
|
||||
case kStatePublishingGraph:
|
||||
this->publishGraph(atoi(parameters.at("optimized").c_str())!=0, false);
|
||||
break;
|
||||
case kStatePublishingGraphFull:
|
||||
this->publishGraph(atoi(parameters.at("optimized").c_str())!=0, true);
|
||||
break;
|
||||
case kStateTriggeringMap:
|
||||
_rtabmap->triggerNewMap();
|
||||
break;
|
||||
@@ -300,6 +334,20 @@ void RtabmapThread::handleEvent(UEvent* event)
|
||||
param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt())));
|
||||
pushNewState(kStatePublishingMapFull, param);
|
||||
}
|
||||
else if(cmd == RtabmapEventCmd::kCmdPublishGraph)
|
||||
{
|
||||
ULOGGER_DEBUG("CMD_PUBLISH_GRAPH");
|
||||
ParametersMap param;
|
||||
param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt())));
|
||||
pushNewState(kStatePublishingGraph, param);
|
||||
}
|
||||
else if(cmd == RtabmapEventCmd::kCmdPublishGraphFull)
|
||||
{
|
||||
ULOGGER_DEBUG("CMD_PUBLISH_GRAPH_FULL");
|
||||
ParametersMap param;
|
||||
param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt())));
|
||||
pushNewState(kStatePublishingGraphFull, param);
|
||||
}
|
||||
else if(cmd == RtabmapEventCmd::kCmdTriggerNewMap)
|
||||
{
|
||||
ULOGGER_DEBUG("CMD_TRIGGER_NEW_MAP");
|
||||
|
||||
@@ -1500,10 +1500,12 @@ pcl::PolygonMesh::Ptr createMesh(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &
|
||||
//On success, optimizedPoses is cleared and new poses are inserted in
|
||||
void optimizeTOROGraph(
|
||||
const std::map<int, Transform> & poses,
|
||||
const std::multimap<int, std::pair<int, Transform> > & edgeConstraints,
|
||||
int toroIterations,
|
||||
const std::multimap<int, Link> & edgeConstraints,
|
||||
std::map<int, Transform> & optimizedPoses,
|
||||
Transform & mapCorrection)
|
||||
Transform & mapCorrection,
|
||||
int toroIterations,
|
||||
bool toroInitialGuess,
|
||||
std::list<std::map<int, Transform> > * intermediateGraphes) // contains poses after tree init to last one before the end
|
||||
{
|
||||
if(edgeConstraints.size()>=1 && poses.size()>=2)
|
||||
{
|
||||
@@ -1526,12 +1528,12 @@ void optimizeTOROGraph(
|
||||
}
|
||||
}
|
||||
|
||||
for(std::multimap<int, std::pair<int, Transform> >::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
|
||||
for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
|
||||
{
|
||||
int id1 = iter->first;
|
||||
int id2 = iter->second.first;
|
||||
int id2 = iter->second.to();
|
||||
float x,y,z, roll,pitch,yaw;
|
||||
pcl::getTranslationAndEulerAngles(transformToEigen3f(iter->second.second), x,y,z, roll,pitch,yaw);
|
||||
pcl::getTranslationAndEulerAngles(transformToEigen3f(iter->second.transform()), x,y,z, roll,pitch,yaw);
|
||||
AISNavigation::TreePoseGraph3::Pose p(x, y, z, roll, pitch, yaw);
|
||||
AISNavigation::TreePoseGraph3::InformationMatrix m;
|
||||
m=DMatrix<double>::I(6);
|
||||
@@ -1548,7 +1550,10 @@ void optimizeTOROGraph(
|
||||
pg.buildMST(pg.vertices.begin()->first); // pg.buildSimpleTree();
|
||||
|
||||
UDEBUG("Initial guess...");
|
||||
pg.initializeOnTree(); // optional
|
||||
if(toroInitialGuess)
|
||||
{
|
||||
pg.initializeOnTree(); // optional
|
||||
}
|
||||
|
||||
pg.initializeTreeParameters();
|
||||
UDEBUG("Building TORO tree... (if a crash happens just after this msg, "
|
||||
@@ -1558,6 +1563,20 @@ void optimizeTOROGraph(
|
||||
UDEBUG("TORO iterate begin");
|
||||
for (int i=0; i<toroIterations; i++)
|
||||
{
|
||||
if(intermediateGraphes && (toroInitialGuess || i>0))
|
||||
{
|
||||
std::map<int, Transform> tmpPoses;
|
||||
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
|
||||
{
|
||||
AISNavigation::TreePoseGraph<AISNavigation::Operations3D<double> >::Vertex* v=pg.vertex(iter->first);
|
||||
v->pose=v->transformation.toPoseType();
|
||||
Transform newPose = transformFromEigen3f(pcl::getTransformation(v->pose.x(), v->pose.y(), v->pose.z(), v->pose.roll(), v->pose.pitch(), v->pose.yaw()));
|
||||
|
||||
tmpPoses.insert(std::pair<int, Transform>(iter->first, newPose));
|
||||
}
|
||||
intermediateGraphes->push_back(tmpPoses);
|
||||
}
|
||||
|
||||
pg.iterate();
|
||||
}
|
||||
UDEBUG("TORO iterate end");
|
||||
@@ -1587,7 +1606,7 @@ void optimizeTOROGraph(
|
||||
bool saveTOROGraph(
|
||||
const std::string & fileName,
|
||||
const std::map<int, Transform> & poses,
|
||||
const std::multimap<int, std::pair<int, Transform> > & edgeConstraints)
|
||||
const std::multimap<int, Link> & edgeConstraints)
|
||||
{
|
||||
FILE * file = fopen(fileName.c_str(), "w");
|
||||
|
||||
@@ -1609,13 +1628,13 @@ bool saveTOROGraph(
|
||||
}
|
||||
|
||||
//EDGE3 observed_vertex_id observing_vertex_id x y z roll pitch yaw inf_11 inf_12 .. inf_16 inf_22 .. inf_66
|
||||
for(std::multimap<int, std::pair<int, Transform> >::const_iterator iter = edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
|
||||
for(std::multimap<int, Link>::const_iterator iter = edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
|
||||
{
|
||||
float x,y,z, yaw,pitch,roll;
|
||||
pcl::getTranslationAndEulerAngles(transformToEigen3f(iter->second.second), x,y,z, roll, pitch, yaw);
|
||||
pcl::getTranslationAndEulerAngles(transformToEigen3f(iter->second.transform()), x,y,z, roll, pitch, yaw);
|
||||
fprintf(file, "EDGE3 %d %d %f %f %f %f %f %f 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1\n",
|
||||
iter->first,
|
||||
iter->second.first,
|
||||
iter->second.to(),
|
||||
x,
|
||||
y,
|
||||
z,
|
||||
@@ -1706,6 +1725,110 @@ bool loadTOROGraph(const std::string & fileName,
|
||||
return true;
|
||||
}
|
||||
|
||||
cv::Mat create2DMap(const std::map<int, Transform> & poses,
|
||||
const std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > & scans,
|
||||
float delta,
|
||||
float & xMin,
|
||||
float & yMin)
|
||||
{
|
||||
std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > localScans;
|
||||
|
||||
pcl::PointCloud<pcl::PointXYZ> minMax;
|
||||
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
|
||||
{
|
||||
if(uContains(scans, iter->first))
|
||||
{
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::transformPointCloud(scans.at(iter->first), iter->second);
|
||||
pcl::PointXYZ min, max;
|
||||
pcl::getMinMax3D(*cloud, min, max);
|
||||
minMax.push_back(min);
|
||||
minMax.push_back(max);
|
||||
minMax.push_back(pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z()));
|
||||
localScans.insert(std::make_pair(iter->first, cloud));
|
||||
}
|
||||
}
|
||||
|
||||
cv::Mat map;
|
||||
if(minMax.size())
|
||||
{
|
||||
//Get map size
|
||||
pcl::PointXYZ min, max;
|
||||
pcl::getMinMax3D(minMax, min, max);
|
||||
|
||||
xMin = min.x-1.0f;
|
||||
yMin = min.y-1.0f;
|
||||
float xMax = max.x+1.0f;
|
||||
float yMax = max.y+1.0f;
|
||||
|
||||
map = cv::Mat::ones((yMax - yMin) / delta, (xMax - xMin) / delta, CV_8S)*-1;
|
||||
for(std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator iter = localScans.begin(); iter!=localScans.end(); ++iter)
|
||||
{
|
||||
for(unsigned int i=0; i<iter->second->size(); ++i)
|
||||
{
|
||||
const Transform & pose = poses.at(iter->first);
|
||||
cv::Point2i start((pose.x()-xMin)/delta + 0.5f, (pose.y()-yMin)/delta + 0.5f);
|
||||
cv::Point2i end((iter->second->points[i].x-xMin)/delta + 0.5f, (iter->second->points[i].y-yMin)/delta + 0.5f);
|
||||
|
||||
rayTrace(start, end, map); // trace free space
|
||||
|
||||
map.at<char>(end.y, end.x) = 100; // obstacle
|
||||
}
|
||||
}
|
||||
}
|
||||
return map;
|
||||
}
|
||||
|
||||
void rayTrace(const cv::Point2i & start, const cv::Point2i & end, cv::Mat & grid)
|
||||
{
|
||||
UASSERT_MSG(start.x >= 0 && start.x < grid.cols, uFormat("start.x=%d grid.cols=%d", start.x, grid.cols).c_str());
|
||||
UASSERT_MSG(start.y >= 0 && start.y < grid.rows, uFormat("start.y=%d grid.rows=%d", start.y, grid.rows).c_str());
|
||||
UASSERT_MSG(end.x >= 0 && end.x < grid.cols, uFormat("end.x=%d grid.cols=%d", end.x, grid.cols).c_str());
|
||||
UASSERT_MSG(end.y >= 0 && end.y < grid.rows, uFormat("end.x=%d grid.cols=%d", end.y, grid.rows).c_str());
|
||||
|
||||
cv::Point2i ptA, ptB;
|
||||
if(start.x > end.x)
|
||||
{
|
||||
ptA = end;
|
||||
ptB = start;
|
||||
}
|
||||
else
|
||||
{
|
||||
ptA = start;
|
||||
ptB = end;
|
||||
}
|
||||
|
||||
float slope = float(ptB.y - ptA.y)/float(ptB.x - ptA.x);
|
||||
float b = ptA.y - slope*ptA.x;
|
||||
|
||||
|
||||
//ROS_WARN("start=%d,%d end=%d,%d", ptA.x, ptA.y, ptB.x, ptB.y);
|
||||
|
||||
//ROS_WARN("y = %f*x + %f", slope, b);
|
||||
|
||||
for(int x=ptA.x; x<ptB.x; ++x)
|
||||
{
|
||||
float lowerbound = float(x)*slope + b;
|
||||
float upperbound = float(x+1)*slope + b;
|
||||
|
||||
if(lowerbound > upperbound)
|
||||
{
|
||||
float tmp = lowerbound;
|
||||
lowerbound = upperbound;
|
||||
upperbound = tmp;
|
||||
}
|
||||
|
||||
//ROS_WARN("lowerbound=%f upperbound=%f", lowerbound, upperbound);
|
||||
UASSERT_MSG(lowerbound >= 0 && lowerbound < grid.rows, uFormat("lowerbound=%f grid.cols=%d x=%d slope=%f b=%f", lowerbound, grid.cols, x, slope, b).c_str());
|
||||
UASSERT_MSG(upperbound >= 0 && upperbound < grid.rows, uFormat("upperbound=%f grid.cols=%d x+1=%d slope=%f b=%f", upperbound, grid.cols, x+1, slope, b).c_str());
|
||||
for(int y = lowerbound; y<=(int)upperbound; ++y)
|
||||
{
|
||||
//if(grid.at<char>(y, x) == -1)
|
||||
{
|
||||
grid.at<char>(y, x) = 0; // free space
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user