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

Added Rtabmap::setInitialPose()

Browse Source
This commit is contained in:
matlabbe
2018-04-30 14:49:41 -04:00
parent a39d0840ce
commit 8fec570c13
3 changed files with 22 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -129,6 +129,7 @@ public:
float getTimeThreshold() const {return _maxTimeAllowed;} // in ms float getTimeThreshold() const {return _maxTimeAllowed;} // in ms
void setTimeThreshold(float maxTimeAllowed); // in ms void setTimeThreshold(float maxTimeAllowed); // in ms
void setInitialPose(const Transform & initialPose);
int triggerNewMap(); int triggerNewMap();
bool labelLocation(int id, const std::string & label); bool labelLocation(int id, const std::string & label);
/** /**

3
corelib/src/OccupancyGrid.cpp
View File

@@ -786,6 +786,7 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
{ {
if(addedNodes_.find(iter->first) == addedNodes_.end()) if(addedNodes_.find(iter->first) == addedNodes_.end())
{ {
UDEBUG("Pose %d not found in current added poses, it be added to map", iter->first);
poses.push_back(*iter); poses.push_back(*iter);
} }
} }
@@ -837,6 +838,8 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
{ {
const std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> & pair = cache_.at(iter->first); const std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> & pair = cache_.at(iter->first);
UDEBUG("Adding grid %d: ground=%d obstacles=%d empty=%d", iter->first, pair.first.first.cols, pair.first.second.cols, pair.second.cols);
//ground //ground
if(pair.first.first.cols) if(pair.first.first.cols)
{ {

18
corelib/src/Rtabmap.cpp
View File

@@ -662,6 +662,24 @@ Transform Rtabmap::getPose(int locationId) const
return Transform(); return Transform();
} }
void Rtabmap::setInitialPose(const Transform & initialPose)
{
if(_memory)
{
if(!_memory->isIncremental())
{
_lastLocalizationPose = initialPose;
_lastLocalizationNodeId = 0;
_mapCorrection.setIdentity();
_mapCorrectionBackup.setNull();
}
else
{
UWARN("Initial pose can only be set in localization mode (%s=false), ignoring it...", Parameters::kMemIncrementalMemory().c_str());
}
}
}
int Rtabmap::triggerNewMap() int Rtabmap::triggerNewMap()
{ {
int mapId = -1; int mapId = -1;