1
$\begingroup$

I'd like to know how to form my pose graph if the only information I have available is that from a camera,

(1) What are my poses? Are they just the accumulated transformations from pairwise matching?

(2) And then what are my edges?

(3) If I have already detected the loops, how would I loop correct?

Thank you.

|improve this question
$\endgroup$
1
$\begingroup$

(1) Usually, the poses are just your absolute camera poses, i.e $[R,T]$ where $R$ is a rotation matrix and $T$ is a translation, expressed in world coordinates. You can add a scale factor to that if you want.

(2) Usually, the edges are the relative displacements between your cameras. That is , if you have cameras $C_1=[R_1,T_1]$ and $C_2=[R_2,T_2]$, then, noting $X_w$ a point in world coordinates and $X_1$, $X_2$ its representations in (respectively) cameras $C_1$ and $C_2$, we will have

$X_w=R_1X_1+T_1$

$X_w=R_2X_2+T_2$

$\Rightarrow X_1=R_1^{t}R_2X_2+R_1^t(T_2-T_1)\triangleq R'X_2+T'.$

Now, your edges are just defined as the relative transformation $[R',T']$. Once again, you can add a scale factor.

(3) Let $P_1,...,P_n$ be the relative poses computed between cameras $1,...n,n+1$ by your SLAM algorithm. Now, you detect a loop between cameras $n$ and $n+1$, and this says that your relative transformation should be $T_n$, not $P_n$ . The difference between $T_n$ and $R_n$ is often important, and you want to propagate this large error to previous cameras. Thus, what you want to do is this: you want to change all absolute camera poses in a way that does not significantly change the relative poses $P_1,...,P_{n-1}$, all the while making the transformation between cameras $n$ and $n+1$ as close to $T_n$ as possible. Let us write $A_i$ the absolute pose of camera $i$, and assume that $A_i^{-1}A_j$ gives us the relative transformation between $i$ and $j$. Now, we can state our goal clearly:

$\{P_1,...,P_{n-1}\}\cup{T_n} \ \ \ \text{these are our constants}$

$\{A_1,...,A_n,A_{n+1}\} \ \ \ \ \text{These are the variable we optimize}$

$argmin_{A_i}\Sigma_{i=1}^{n-1}d(A_i^{-1}A_{i+1},P_i)+d(A_{n}^{-1}A_{n+1},T_n) \ \ \ \text{what we want to solve}$

where $d$ is a function measuring the distance between its two arguments. Its actual definition will depend on how you actually parametrize your transformations (e.g Lie algera, quaternions, etc...). Usually, any iterative minimization algorithm will do.

A really great resource to understand loop closure is Hauke Strasdat's phd thesis.

|improve this answer
$\endgroup$
  • $\begingroup$ Hi, thank you for the answer, it helps a lot. Still have a doubt though, so the poses in my case will be the accumulated transformations since I only have data from the pairwiase 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 Apr 17 '17 at 17:47
  • $\begingroup$ My pleasure! The difference will be Identity at first. But don't forget that you minimize the cost function iteratively. So, after the first iteration, the large error between the expect poses and predicted relative poses on the last two cameras will make everything move around a bit. So, the pose of camera i will become $A_iS$ where $S$ is a perturbation. Thus, the difference wont be the Identity anymore. As for the first part of your question, yes, the poses will be the ones you already have computed before the loop is detected. $\endgroup$ – Ash Apr 17 '17 at 19:23
  • $\begingroup$ Thank you so much. :-) That really helped and so did Mr Strasdat's thesis. $\endgroup$ – arunabh sharma Apr 18 '17 at 1:08

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

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