2
$\begingroup$

So I'm trying to incorporate pose graph optimization into a mapping framework using a Lidar. So I basically have all the relative transformations between the pointclouds and I have pairs of pointclouds which satisfy my place recognition algorithm so I know which poses to complete the loop with, now the question I have is given that I only have these relative transformations (1) how do I calculate the error

e = z- z

where $\hat{z}$ is the ground truth since I only have one set of measurements which are my R,t estimate from consecutive pointclouds?

(2) How do I loop close using g2o?

(3) What will my information matrix be isn't it supposed to be a property of the sensor itself?

Thank you.

Improve this question
$\endgroup$

1 Answer 1

Reset to default
1
$\begingroup$

  1. In the context of a pose graph / factor graph SLAM backend, $e\left(i,j\right)$ actually reduces to the difference between the current estimate of pose $x_j$ and the predicted pose $\hat{x}_j$ given the current estimate of pose $x_i$ and the constraint / pose graph edge $c_{ij}$: $$ e\left(i,j\right)=x_j-\hat{x}_j=x_j-x_i\cdot c_{ij}=x_j-\left(R_{ij}\cdot x_i+t_{ij}\right) $$ where $R_{ij},t_{ij}$ are the parametrization of the transformation between poses $x_i$ and $x_j$ as given by constraint / edge $c_{ij}$. If you are using g2o, then this is not something you need to calculate yourself, you only need to provide an initial estimate for every pose, as well as the edge measurements ($R,t$) and their information matrices $\mathcal{I}$.

  2. Depending on whether you are operating in SE3 or SE2, add an edge (EdgeSE2 / EdgeSE3) between the two vertices where the loop closure was determined, using the registration data (transformation and information matrix) of the two associated point clouds to specify the edge constraint.

    I would strongly advise you to look into robust methods like Dynamic Covariance Scaling (Agarwal et al., ICRA 2013), Switchable Constraints (Sünderhauf et al., IROS 2012) and RRR (Lasif et al., IJRR 2013), as erroneous loop closure constraints will wreak havok on the shape of your optimization problem / pose graph.

  3. If you are talking about the information matrix of the edges / constraints between poses, then they will come from your registration algorithm, odometry system, IMU, etc. Anything that measures the geometric offset between two poses should provide an estimate of the certainty or uncertainty of this measurement. In Pose Graph SLAM you will typically have constraints generated by Odometry or IMU, and / or from registration algorithms such as ICP.

    Depending on your parametrization of the transformation, this uncertainty is represented as a covariance matrix $\Sigma^{n\times n}$ where $n$ is the dimensionality of your transformation representation (e.g. in 2D we would have $n=3$). The Fisher information matrix $\mathcal{I}$ required by e.g. g2o is simply the inverse of the covariance matrix $\Sigma$ , i.e. $$ \mathcal{I}_{ij}={\Sigma_{ij}}^{-1} $$
    Hope this helps.
Improve this answer
$\endgroup$
3
  • $\begingroup$ Hi Christian. Thank you for the answer, still have a doubt though.will my poses be the accumulated transformations since I only have data from the pairwise registrations? And if so, won't the difference between my expected and predicted measurement always be identity because the way I'm calculating my poses and the way I'm calculating my relative transformations is the same. $\endgroup$
    – arunabh sharma
    Commented Apr 17, 2017 at 17:49
  • $\begingroup$ Hi Arunabh, you are correct there - pose $x_i$ will be the accumulation of all relative transforms leading up to timestep $i$, i.e. $x_i = R_{\left(i-1\right), i}\cdot x_{\left(i-1\right)}+t_{\left(i-1\right), i}$, starting with $x_0 = 0^n$ as the origin. These concatenated poses are only the initialization of your optimization problem though. Without loop closure constraints, the absolute error of that problem is 0. As soon as you add a single loop closure constraints that disagrees with your pairwise registration constraints, the error function will have a different minimum. $\endgroup$
    – Christian R.
    Commented Apr 18, 2017 at 11:18
  • $\begingroup$ On stack exchange, it is better to edit your answer to add information requested in comments, rather than adding more comments. Comments are for helping to improve questions and answers, and are distracting, so we try to keep them to a minimum. If all of the information needed to answer the question is contained within it, the comments can be tidied up (deleted). $\endgroup$
    – Mark Booth
    Commented Apr 19, 2017 at 10:08

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.