12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fixed forgotten compose

release/4.3a0
dellaert 2016-02-08 18:58:43 -08:00
parent 3052afe42b
commit 6eeeb3fef1
1 changed files with 1 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam_unstable/timing/timeInertialNavFactor_GlobalVelocity.cpp
View File

@ -75,7 +75,7 @@ int main() {
Rot3 R2(0.473618898, 0.119523052, 0.872582019, Rot3 R2(0.473618898, 0.119523052, 0.872582019,
0.609241153, 0.67099888, -0.422594037, 0.609241153, 0.67099888, -0.422594037,
-0.636011287, 0.731761397, 0.244979388); -0.636011287, 0.731761397, 0.244979388);
Point3 t2 = t1.compose( Point3(Vel1*measurement_dt) ); Point3 t2 = t1 + Point3(Vel1*measurement_dt);
Pose3 Pose2(R2, t2); Pose3 Pose2(R2, t2);
Vector dv = measurement_dt * (R1.matrix() * measurement_acc + world_g); Vector dv = measurement_dt * (R1.matrix() * measurement_acc + world_g);
Vector3 Vel2 = Vel1 + dv; Vector3 Vel2 = Vel1 + dv;