Velocity factor iSAM2

[Erik Strömberg]

Hi,

I'm trying to use GTSAM for SLAM with a quadrotor in MATLAB. My motion model includes the body-velocity and I would like to track this in iSAM2 together with the pose and orientation, but haven't found a good way of doing it.

Is something like this available?

[Erik Strömberg]

I noticed that you can define it as a LieVector. Closing.

[Marco Camurri]

Hi, I'm also interested in that. Could you please provide more details on how did you solve the issue? Maybe a code snippet as well? Thanks.

[Erik Strömberg]

Hi Marco, I think I used a NavState. You can define an initial point, where your robot is at rest and aligned with the coordinate system as:

Rot3 rot0 = Rot3::RzRyRx(0.0, 0.0, 0.0);
Point3 point0 = Point3(0.0, 0.0, 0.0);
Pose3 pose0 = Pose3(rot0, point0);
Vector3 vel0 = Vector3(0.0,0.0,0.0);

NavState state0 = NavState(pose0, vel0);

I haven't tried this, but hope it helps.