Merge pull request laboshinl#31 from StefanGlaser/hotfix

Fixed copy-paste error (introduced in PR laboshinl#20) in TransformToStartIMU …
hitbg-yjm · Dec 1, 2017 · 965ea30 · 965ea30
2 parents a7c6cf9 + 45461e8
commit 965ea30
Showing 1 changed file with 3 additions and 3 deletions.
diff --git a/src/scanRegistration.cpp b/src/scanRegistration.cpp
@@ -147,9 +147,9 @@ void TransformToStartIMU(PointType *p)
   Vector3 v2 = rotateX( v1, imuPitchCur);
   Vector3 v3 = rotateY( v2, imuYawCur);
 
-  Vector3 v4 = rotateY( v2, -imuYawStart);
-  Vector3 v5 = rotateX( v1, -imuPitchStart);
-  Vector3 v6 = rotateZ( *p, -imuRollStart);
+  Vector3 v4 = rotateY( v3, -imuYawStart);
+  Vector3 v5 = rotateX( v4, -imuPitchStart);
+  Vector3 v6 = rotateZ( v5, -imuRollStart);
 
   v6 += imuShiftFromStartCur;