constraining pitch and roll in 3D pose graph SLAM

[Maurice Fallon]

Hi Folks,

I'm looking to constrain pitch and roll in our 3D pose-graph SLAM system. The SLAM system is fed with 3D odometry from a VINS system (which is gravity aligned using the pre-integration factors)

I don't have access to the pre-integration factor so I wanted to directly constrain these two variables for every pose in the SLAM system.

I've tried adding a prior factor with very large uncertainty on the other variables - that was unstable:

> Pose3 prior_odometry; // the odometry from the VINS systems

> noiseModel::Diagonal::shared_ptr pitch_roll_noise = noiseModel::Diagonal::Sigmas(

>   (Vector(6) << 1e-3, 1e-3, 1e10, 1e10, 1e10, 1e10).finished()); // noise 

> factor_graph_.emplace_shared<PriorFactor<Pose3> >(headID, prior_input_odometry , pitch_roll_noise) ;

While using a PartialPriorFactor was also unstable or didnt have an effect for different noise levels:

> std::vector<size_t> mask; // A mask with the pitch and roll indices

> mask.resize(2);

> mask[0] = 0;

> mask[1] = 1;

> noiseModel::Diagonal::shared_ptr pitch_roll_noise_2d = noiseModel::Diagonal::Sigmas((Vector(2) << 10., 10.).finished()); 

> gtsam::Vector2 nerf = Vector2(prior_odometry_roll, prior_odometry_pitch); // the pitch/roll values from the VINS system

> factor_graph_.emplace_shared<PartialPriorFactor<Pose3> >(headID, mask, nerf , pitch_roll_noise_2d); // the factor

Can anyone suggest what type of factor I should use or why these two examples don't work?

The factor graph I'm trying to implement is attached - the orange nodes in particular.

best wishes,

- Maurice

[José Luis Blanco-Claraco]

Hi,

I think you should:
- use numbers not so wildly different in range. From my experience,
max/min sigmas of 1e6 is enough.
- set smart=false in the noise model constructor (it often helps to
avoid numerical issues, at least when *all* sigmas are small).
- are you sure those two sigmas constrain pitch & roll? :-) I think
it's correct, but I recommend you to write and run a toy test problem
to validate; see:
* https://twitter.com/mmattamala/status/1364941299061825540
* https://github.com/borglab/gtsam/issues/504

All in all, I think the factor to use is correct, PriorFactor<Pose3>,
the key is the noise model.

JL

> --
> 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/a234cd9b-c635-48a9-a4d9-9f9c2b05a3fbn%40googlegroups.com.



--

/**
* Jose Luis Blanco-Claraco
* Universidad de Almería - Departamento de Ingeniería
* [Homepage]( https://w3.ual.es/~jlblanco/ )
* [GH profile]( https://github.com/jlblancoc )
*/

[José Luis Blanco-Claraco]

On Sun, Mar 14, 2021 at 6:37 PM José Luis Blanco-Claraco
<jlbl...@ual.es> wrote:
> - are you sure those two sigmas constrain pitch & roll? :-) I think
> it's correct,

On a second though, if those Prior are given wrt the "global" frame of
reference, each frame should have a different covariance matrix,
modifying the one you build here:

(Vector(6) << 1e-3, 1e-3, 1e10, 1e10, 1e10, 1e10).finished())

with the SE(3) adjoint [1], multiplying on the left and the right
(transpose) by the Adj of each particular pose where you want to add
an observation.

At least, that's my understanding...

JL

[1] https://gtsam.org/2021/02/23/uncertainties-part3.html

[Dellaert, Frank]

Ha, I had forgotten PartialPriorFactor existed. I could have used that a few weeks ago, when facing a similar situation. 

But, in my case (legged animal tracking) using large uncertainties worked just fine. And indeed, JL's comments is technically correct, but suspecting your pitch and roll are close to zero I think that might not make much of a difference, and your diagonal should work...

Sorry I can't be of more help without knowing why things go "unstable". It might be due to other issues.

Frank

---

From: gtsam...@googlegroups.com <gtsam...@googlegroups.com> on behalf of Maurice Fallon <mauric...@gmail.com>
Sent: Sunday, March 14, 2021 11:42
To: gtsam users <gtsam...@googlegroups.com>
Subject: [GTSAM] constraining pitch and roll in 3D pose graph SLAM

 

[Phạm Ngọc Sơn]

Hi everyone.
I also meet the issue when create the custom factor to constraint the roll, pitch angle.
I still find how to define the Jacobian Matrix in evaluateError function.
Can you help me?
Thanks

Vào lúc 05:08:49 UTC+7 ngày Thứ Hai, 15 tháng 3, 2021, Dellaert, Frank đã viết: