Uncertainty Pose Representation and Propagation of uncertainty in Robotics

From : A probabilistic framework for tracking uncertainties in robotic manipulation Huy Nguyen and Quang-Cuong Pham

In early works by [11] and [12], the uncertainty of a pose is simply represented by worst-case bounds which include all possible errors (min-max approach).This simple approach usually results in conservative estimates which make it difficult to apply in decision-making process.
To address this limitation, the probabilistic approach which uses the calculus of probability theory and assign probabilities to all potential positions of the object has been proposed.
The pose of an object is now represented by a probability distribution over the space [9], [13].
As a result, the probabilistic representation can make use of probability theory and, thus, provide more uncertainty manipulation methodologies (e.g. propagation, fusion, etc.) [3], [4], [14], [15].
Recently, this probabilistic approach has been further investigated in [5], [6], [16], [17] where they provided a rigorous treatment of representing and propagating uncertainty on SO(3) and SE(3)

Uncertainty of Pose by worst-case bounds

[11] R. H. Taylor, “The synthesis of manipulator control programs from task- level specifications.” Ph.D. dissertation, Stanford University, Stanford, CA, USA, 1976, aAI7707174.
[12] R. A. Brooks, “Symbolic error analysis and robot planning,” The International Journal of Robotics Research, vol. 1, no. 4, pp. 29–78,

Pose of object as probability distribution over space (probabilistic representation)

[9] S. Su and C. Lee, “Uncertainty manipulation and propagation and verification of applicability of actions in assembly tasks,” in IEEE International Conference on Robotics and Automation, vol. 3, 1991, pp. 2471–2476
[13] S.-F. Su and C. G. Lee, “Manipulation and propagation of uncertainty and verification of applicability of actions in assembly tasks,” Systems Science and Cybernetics, IEEE Transactions on, vol. 22, no. 6, pp. 1376– 1389, 1992.

Application of probability pose for applications (propagation, fusion, etc)

[3] R. Smith, M. Self, and P. Cheeseman, “Estimating uncertain spatial relationships in robotics,” in Autonomous robot vehicles. Springer, 1990, pp. 167–193.
[4] H. F. Durrant-Whyte, “Uncertainty geometry in robotics,” IEEE Journal of Robotics and Automation, vol. 4, no. 1, pp. 23–31, 1988.
[14] R. A. Brooks, “Visual map making for a mobile robot,” in Robotics and Automation, 1985. Proceedings. IEEE International Conference on, vol. 2. IEEE, 1985, pp. 824–829.
[15] R. C. Smith and P. Cheeseman, “On the representation and estimation of spatial uncertainty,” IEEE Journal of Robotics and Automation, vol. 5, no. 4, pp. 56–68, 1986.

Rigours treatment of probabilistic representation and propagation uncertainty on SO(3) SE(3)

[5] Y. Wang and G. S. Chirikjian, “Nonparametric second-order theory of error propagation on motion groups,” International Journal of Robotics Research, vol. 27, no. 11-12, pp. 1258–1273, 2008.
[6] T. D. Barfoot and P. T. Furgale, “Associating uncertainty with three- dimensional poses for use in estimation problems,” IEEE Transactions on Robotics, vol. 30, no. 3, pp. 679 – 693, June 2014.
[16] G. Chirikjian, Stochastic Models, Information Theory, and Lie Groups, Volume 1: Classical Results and Geometric Methods. Springer Science & Business Media, 2009, vol. 1.
[17] ——, Stochastic Models, Information Theory, and Lie Groups, Volume 2: Analytic Methods and Modern Applications. Springer Science & Business Media, 2011, vol. 2.

!git clone https://github.com/dinhhuy2109/python-cope.git

Cloning into 'python-cope'...
remote: Enumerating objects: 361, done.
remote: Total 361 (delta 0), reused 0 (delta 0), pack-reused 361
Receiving objects: 100% (361/361), 1.17 MiB | 3.46 MiB/s, done.
Resolving deltas: 100% (199/199), done.

!pip install trimesh -q

     ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━ 0.0/685.4 kB ? eta -:--:--
     ━━━━╺━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━ 71.7/685.4 kB 1.9 MB/s eta 0:00:01
     ━━━━━━━━━━━━━━━━━━╺━━━━━━━━━━━━━━━━━━━━ 317.4/685.4 kB 4.4 MB/s eta 0:00:01
     ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━╸━━━━ 614.4/685.4 kB 5.8 MB/s eta 0:00:01
     ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━ 685.4/685.4 kB 5.1 MB/s eta 0:00:00

from importlib import reload  # Python 3.4+

!cd python-cope && sudo pip install -e .

Obtaining file:///content/python-cope
  Preparing metadata (setup.py) ... done
Installing collected packages: cope
  Attempting uninstall: cope
    Found existing installation: cope 1.0.0
    Uninstalling cope-1.0.0:
      Successfully uninstalled cope-1.0.0
  Running setup.py develop for cope
Successfully installed cope-1.0.0

import numpy  as np
import cope
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt
import time

starttime = time.time()
tiny = 1e-5

T1 = np.eye(4)
sigma1 = np.diag([tiny,tiny,tiny,tiny,tiny,tiny],0)

T2 = np.eye(4)
T2[:3,3] = np.array([0,0.15,0])
sigma2 = np.diag([tiny,tiny,tiny,0.01,tiny,0.01],0)

T3 = np.eye(4)
T3[:3,3] = np.array([0,0,-0.03])
sigma3 = np.diag([tiny,tiny,tiny,tiny,tiny,0.1],0)

T4 = np.eye(4)
T4[:3,3] = np.array([0,0.14,0])
sigma4 = np.diag([tiny,tiny,tiny,tiny,tiny,tiny],0)



T12, sigma12 = cope.Propagating(T1,sigma1,T2,sigma2)

T23, sigma23 = cope.Propagating(T12,sigma12,T3,sigma3)

T34, sigma34 = cope.Propagating(T23,sigma23,T4,sigma4)
print('Propagation took %.4f seconds' % (time.time() - starttime))

Tv = T34
Tv[:3,3] = Tv[:3,3] + np.array([0.001,0.002,-0.001])
sigmav = np.diag([tiny,tiny,tiny,0.01,0.2,tiny])

Te,sigmae,iters = cope.Fusing([T34,Tv],[sigma34,sigmav], maxiterations=10, retiter=True)

print('Took %.4f seconds and %d iterations' % (time.time() - starttime, iters))

cope.Visualize([T12,T34,Te],[sigma12, sigma34,sigmae],100)
# visualize(T,sigma,100,2)

---------------------------------------------------------------------------
ModuleNotFoundError                       Traceback (most recent call last)
<ipython-input-20-712d9140bd3b> in <cell line: 2>()
      1 import numpy  as np
----> 2 import cope
      3 from mpl_toolkits.mplot3d import Axes3D
      4 import matplotlib.pyplot as plt
      5 import time

ModuleNotFoundError: No module named 'cope'

---------------------------------------------------------------------------
NOTE: If your import is failing due to a missing package, you can
manually install dependencies using either !pip or !apt.

To view examples of installing some common dependencies, click the
"Open Examples" button below.
---------------------------------------------------------------------------


def Visualize(Tlist,sigmalist, nsamples = 100):
  """
  Visualize an estimation (a point will be used to represent the translation position of a transformation)
  @param Tlist:     a list of Transformations
  @param sigmalist: a list of corresponding sigmas
  @param nsamples:  the number of samples generated for each (T,sigma)
  """
  import matplotlib.cm as cm
  fig = plt.figure()
  ax = fig.add_subplot(111, projection='3d')
  cholsigmalist = []
  colors = iter(cm.rainbow(np.linspace(0, 1, len(Tlist))))
  for i in range(len(sigmalist)):
    color = next(colors)
    cholsigma = np.linalg.cholesky(sigmalist[i]).T
    Tsample = []
    for k in range(nsamples):
      vecsample = np.dot(cholsigma,np.random.randn(6,1))
      #vecsample = np.dot(cholsigma, np.random.uniform(-1,1,size = 6))
      vecsample.resize(6)
      Tsample = np.dot(VecToTran(vecsample), Tlist[i])
      ax.scatter(Tsample[0,3],Tsample[1,3],Tsample[2,3], c = color)

  ax.set_autoscaley_on(False)
  ax.set_xlim([-0.5, 0.5])
  ax.set_ylim([-0.5, 0.5])
  ax.set_zlim([-0.5, 0.5])
  ax.set_xlabel('X Label')
  ax.set_ylabel('Y Label')
  ax.set_zlabel('Z Label')
  print("plotting now ")
  plt.savefig('/content/plot.png')
  plt.show()
  return True

python3: can't open file '/content/setup.py': [Errno 2] No such file or directory