in this perfect tutorial i found how i can compose two poses with uncertainty and how i can transform one representation to another with uncertainty http://ingmec.ual.es/~jlblanco/papers/jlblanco2010geometry3D_techrep.pdf. Let's suppose I have one global point which represent 6 dof and 6 confidence and another one also representing by 6 dof and 6 confidence. How can I get "mean" point with uncertainty.
$\begingroup$
$\endgroup$
Add a comment
|
2 Answers
$\begingroup$
$\endgroup$
4
The tutorial you linked is a good one but does not deal with the uncertainty well. Unfortunately, what @tuskiomi suggested is for euclidian space variable with single-dimension. Following is a better way of doing your problem. Although it is two poses example, it can be extended to more poses easily.
Fusing multiple observations of a pose.
\begin{equation} p(\textbf{z} | \textbf{x}) = \prod_{i=1}^{k} p(\textbf{z}_i | \textbf{x}) \end{equation}
Two observation example
\begin{equation} ln(p(\textbf{z} | \textbf{x}))=ln(p(\textbf{z}_1 | \textbf{x})p(\textbf{z}_2 | \textbf{x})) \end{equation}
\begin{equation} ln(p(\textbf{z} | \textbf{x}))=c(\textbf{x}^T\Sigma^{-1}\textbf{x}-2\textbf{x}^T\Sigma^{-1}\textbf{z}+\textbf{z}^T\Sigma^{-1}\textbf{z}) \end{equation}
\begin{equation} \begin{split} ln(p(\textbf{z}_1 | \textbf{x})p(\textbf{z}_2 | \textbf{x}))&=c(\textbf{x}^T\Sigma_1^{-1}\textbf{x}-2\textbf{x}^T\Sigma_1^{-1}\textbf{z}_1+\textbf{z}_1^T\Sigma_1^{-1}\textbf{z}_1+\\ &\textbf{x}^T\Sigma_2^{-1}\textbf{x}-2\textbf{x}^T\Sigma_2^{-1}\textbf{z}_2+\textbf{z}_2^T\Sigma_2^{-1}\textbf{z}_2) \end{split} \end{equation}
Here is your new uncertainty \begin{equation} \begin{split} \textbf{x}^T\Sigma^{-1}\textbf{x}=\textbf{x}^T(\Sigma_1^{-1}+\Sigma_2^{-1})\textbf{x} \end{split} \end{equation}
\begin{equation} \Sigma=(\Sigma_1^{-1}+\Sigma_2^{-1})^{-1} \end{equation}
and mean. \begin{equation} \begin{split} \textbf{x}^T\Sigma^{-1}\textbf{z}=\textbf{x}^T(\Sigma_1^{-1}\textbf{z}_1+\Sigma_2^{-1}\textbf{z}_2) \end{split} \end{equation}
\begin{equation} \textbf{z}=\Sigma(\Sigma_1^{-1}\textbf{z}_1+\Sigma_2^{-1}\textbf{z}_2) \end{equation}
It might work but occasionally it will suffer from the singularity problem.
The proper way of doing this is fusing them on manifold. If you want to learn more about uncertainty fusion of SE3 poses you should read State estimation for robotics by Prof. Tim Barfoot.
-
$\begingroup$ web.kamihq.com/web/… i found similar solution in this book. But I also need update a new pose iterative. For this problem I can use The Convolution of Two Univariate Gaussian PDFs, isn't it? And one more question, does it work for SE3 matrix or quaternion? $\endgroup$ Aug 12, 2019 at 11:47
-
$\begingroup$ Not sure about the convolution. I am not sure if you need that. To iteratively update your final pose with a new pose, you just need to iterate the two pose fusion above which is referred to as a recursive estimation. As I mentioned that will fail occasionally. I strongly recommend using SE3 pose fusion. Have a look at the section V.A of my paper: arxiv.org/pdf/1901.07660.pdf I did exactly the same thing you mentioned. $\endgroup$ Aug 13, 2019 at 1:09
-
$\begingroup$ now correct me if I'm wrong, but this solution is the result of convoluding many distributions together correct? $\endgroup$– tuskiomiAug 19, 2019 at 16:12
-
1$\begingroup$ @tuskiomi I am not sure because I have no idea what is convolution in probability. In robotics, this is usually called Bayesian fusion or recursive fusion or MAP estimation. $\endgroup$ Aug 20, 2019 at 0:32
$\begingroup$
$\endgroup$
This is more of a statistics question. Convolude 2 normal distributions with the means and deviations as you describe, and you get something akin to the answer posted here.
Or to sum it up simply, you add the means of the points ($\mu_{A+B} = \mu_{a}+ \mu_{b}$), and those are your new means, and a variance of $\sigma^{2}_{a+b}=\sigma ^{2}_{a}+\sigma ^{2}_{b}$ .
I assume you know your standard deviation, as without it your confidence intervals are useless.
