Help: Unary factor (GPS reading)

[Ilinca Burdulea]

Hello!

I started running the LocalizationExample.cpp and now I'd like to adapt it on my use case of position estimation using GPS measurements (I have them in lat/lon but I transform them in x/y coordinates before using GTSAM). I basically kept the same structure of the code as the LocalizationExample.cpp, the only difference is that I use a loop that iterates over the nodes (around 500) while building the graph. Thus my states are (x,y,theta) and my GPS measurements (x,y).

I have tried different use cases: 

- for odometry: Pose2(0,0,0), Pose2(delta_x, delta_y, 0) with delta_x and delta_y calculated from x/y measurements, Pose2(delta_x, delta_y, delta_theta) with delta_theta calculated geometrically from x/y measurements

- for initial estimates: Pose2(x, y, 0), Pose2(x, y, theta)with theta calculated geometrically from x/y measurements

- for noise: I tried adding high noise for odometry and low noise for measurements, and the opposite

- for the Hessian: the initial Hessian from the LocalizationExample.cpp and also (Matrix(2,3) << 1.0,0.0,0.0, 0.0,1.0,0.0) (given the measurement function of the GPS measurement)

After optimizing and transforming back to lat/lon space, I am plotting the results vs the initial trace (see the image attached). I was expecting a smoothing effect from the result of FGO but instead I get this chainsaw effect on my original trajectory. 

Is this the expected effect (as it goes a bit against my intuition of how the error function is working)? How can I improve it? 

I am new to GTSAM and any help is greatly appreciated. 

[Ilinca Burdulea]

I printed the error statements and it seems that the error is decreasing until a threshold of 1241,.. and stops after 5 iterations (Levenberg-Marquardt is giving up because cannot decrease error with maximum lambda).

[Graham Fletcher]

Depending on which example code you are referencing, there was previously a bug in the unary factor example that didn’t define the Jacobian properly. This may be a contributor here. Check the sample code in Section 3.2 here: 

https://gtsam.org/tutorials/intro.html#magicparlabel-65484

Another related discussion: 

https://groups.google.com/g/gtsam-users/c/p9-rmuMa-40/m/bKKNWdXdAgAJ?utm_medium=email&utm_source=footer

Graham

--
You received this message because you are subscribed to the Google Groups "gtsam users" group.
To unsubscribe from this group and stop receiving emails from it, send an email to gtsam-users...@googlegroups.com.
To view this discussion on the web visit https://groups.google.com/d/msgid/gtsam-users/88a2e7f7-a5a3-4460-9263-7993aa43ebd5n%40googlegroups.com.