i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-03 01:50:24 +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

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())
{
UDEBUG("Pose %d not found in current added poses, it be added to map", iter->first);
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);
UDEBUG("Adding grid %d: ground=%d obstacles=%d empty=%d", iter->first, pair.first.first.cols, pair.first.second.cols, pair.second.cols);
//ground
if(pair.first.first.cols)
{

18
corelib/src/Rtabmap.cpp
View File

@@ -662,6 +662,24 @@ Transform Rtabmap::getPose(int locationId) const
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 mapId = -1;