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

OdometryF2M: fixed orientation ignored when setting initial pose for scans

Browse Source
This commit is contained in:
matlabbe
2019-03-14 14:15:53 -04:00
parent 8edd85a9df
commit 977f8eb262
1 changed files with 3 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
corelib/src/odometry/OdometryF2M.cpp
View File

@@ -158,6 +158,7 @@ OdometryF2M::~OdometryF2M()
void OdometryF2M::reset(const Transform & initialPose)
{
UDEBUG("initialPose=%s", initialPose.prettyPrint().c_str());
Odometry::reset(initialPose);
*lastFrame_ = Signature(1);
*map_ = Signature(-1);
@@ -1142,7 +1143,7 @@ Transform OdometryF2M::computeTransform(
0,
0.0f,
LaserScan::kXYNormal,
Transform(newFramePose.x(), newFramePose.y(), lastFrame_->sensorData().laserScanRaw().localTransform().z(),0,0,0)));
Transform(newFramePose.x(), newFramePose.y(), lastFrame_->sensorData().laserScanRaw().localTransform().z(),0,0,newFramePose.theta())));
}
else
{
@@ -1153,7 +1154,7 @@ Transform OdometryF2M::computeTransform(
0,
0.0f,
LaserScan::kXYZNormal,
newFramePose.translation()));
newFramePose));
}
addKeyFrame = true;
}