i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 17:40:23 +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) void OdometryF2M::reset(const Transform & initialPose)
{ {
UDEBUG("initialPose=%s", initialPose.prettyPrint().c_str());
Odometry::reset(initialPose); Odometry::reset(initialPose);
*lastFrame_ = Signature(1); *lastFrame_ = Signature(1);
*map_ = Signature(-1); *map_ = Signature(-1);
@@ -1142,7 +1143,7 @@ Transform OdometryF2M::computeTransform(
0, 0,
0.0f, 0.0f,
LaserScan::kXYNormal, 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 else
{ {
@@ -1153,7 +1154,7 @@ Transform OdometryF2M::computeTransform(
0, 0,
0.0f, 0.0f,
LaserScan::kXYZNormal, LaserScan::kXYZNormal,
newFramePose.translation())); newFramePose));
} }
addKeyFrame = true; addKeyFrame = true;
} }