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

OdometryROS: set initial pose to guess transform when provided.

Browse Source
This commit is contained in:
matlabbe
2021-02-01 11:34:26 -05:00
parent 4b878e45b9
commit 5a0ddaf9f5
1 changed files with 12 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/OdometryROS.cpp
+12 -1
View File
@@ -617,7 +617,18 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp,
if(!guessFrameId_.empty())
{
guessCurrentPose = this->getTransform(guessFrameId_, frameId_, stamp);
Transform previousPose = guessPreviousPose_.isNull()?guessCurrentPose:guessPreviousPose_;
Transform previousPose = guessPreviousPose_;
if(guessPreviousPose_.isNull())
{
previousPose = guessCurrentPose;
if(!guessCurrentPose.isNull() && odometry_->getPose().isIdentity())
{
ROS_INFO("Odometry: init pose with guess %s", guessCurrentPose.prettyPrint().c_str());
odometry_->reset(guessCurrentPose);
}
}
if(!previousPose.isNull() && !guessCurrentPose.isNull())
{
if(guess_.isNull())