Update python API and examples

doc/source/pymrpt_example_lines-3d-geometry-example.rst

@@ -9,3 +9,9 @@ This example illustrates some geometry functions:
 .. literalinclude:: ../../python-examples/lines-3d-geometry-example.py
    :language: python
    :linenos:
+
+Output:
+
+.. literalinclude:: ../../python-examples/lines-3d-geometry-example.out
+   :linenos:
+

doc/source/pymrpt_example_matrices.rst

@@ -0,0 +1,19 @@
+.. _pyexample_matrices:
+
+====================================================
+Python example: matrices.py
+====================================================
+
+This example shows how to create, manipulate, and print MRPT matrix classes in Python, 
+as well as converting from/to numpy matrices.
+
+.. literalinclude:: ../../python-examples/matrices.py
+   :language: python
+   :linenos:
+
+
+Output:
+
+.. literalinclude:: ../../python-examples/matrices.out
+   :linenos:
+

doc/source/pymrpt_example_ros-poses-convert.rst

@@ -10,3 +10,9 @@ and ROS 1 or ROS 2 (both are compatible with this same code) Pose and PoseWithCo
 .. literalinclude:: ../../python-examples/ros-poses-convert.py
    :language: python
    :linenos:
+
+
+Output:
+
+.. literalinclude:: ../../python-examples/ros-poses-convert.out
+   :linenos:

doc/source/pymrpt_example_se2-poses-example.rst

@@ -10,3 +10,8 @@ in the plane (x,y,phi):
 .. literalinclude:: ../../python-examples/se2-poses-example.py
    :language: python
    :linenos:
+
+Output:
+
+.. literalinclude:: ../../python-examples/se2-poses-example.out
+   :linenos:

doc/source/pymrpt_example_se3-poses-example.rst

@@ -10,3 +10,8 @@ in the space (x,y,z,yaw,pitch,roll):
 .. literalinclude:: ../../python-examples/se3-poses-example.py
    :language: python
    :linenos:
+
+Output:
+
+.. literalinclude:: ../../python-examples/se3-poses-example.out
+   :linenos:

doc/source/python_examples.rst

@@ -7,6 +7,8 @@ Python examples
 The source code for all these Python examples can be found
 under `MRPT/python-examples <https://github.com/MRPT/mrpt/tree/develop/python-examples>`_.
 
+See also the `Python API documentation <https://mrpt.github.io/pymrpt-docs/mrpt.pymrpt.mrpt.html>`_.
+
 C++ examples are `here <examples.html>`_.
 
 
@@ -16,6 +18,7 @@ C++ examples are `here <examples.html>`_.
   pymrpt_example_global_localization.rst
   pymrpt_example_hwdriver_tao_imu_usb.rst
   pymrpt_example_lines-3d-geometry-example.rst
+  pymrpt_example_matrices.rst
   pymrpt_example_rbpf_slam.rst
   pymrpt_example_ros-poses-convert.rst
   pymrpt_example_se2-poses-example.rst

python-examples/lines-3d-geometry-example.out

@@ -0,0 +1,3 @@
+l1             : P=[   0.00000,    0.00000,    0.00000] u=[   1.00000,    0.00000,    0.00000]
+l2             : P=[   1.00000,    1.00000,    1.00000] u=[   1.00000,    1.00000,   -1.00000]
+dist(l1,l2)    : 1.414213562373095

python-examples/matrices.out

@@ -0,0 +1,29 @@
+m1_np   :
+[[1 2 3]
+ [4 5 6]
+ [7 8 9]]
+
+m1_mrpt :
+11 12 13
+14 15 16
+17 18 19
+m1_mrpt.size() :(3, 3)
+
+m2_data: [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]
+m2_np:
+[[1. 0. 0.]
+ [0. 1. 0.]
+ [0. 0. 1.]]
+
+m2 modified:
+ 1  0 99
+ 0  1  0
+ 0  0  1
+m2[1,1]=1.0
+
+m3_np:
+[[1 2]
+ [3 4]]
+m3_mrpt:
+1 2
+3 4

python-examples/matrices.py

@@ -0,0 +1,48 @@
+#!/usr/bin/env python3
+
+# ---------------------------------------------------------------------
+# Install python3-pymrpt, ros-$ROS_DISTRO-mrpt2, or test with a local build with:
+# export PYTHONPATH=$HOME/code/mrpt/build-Release/:$PYTHONPATH
+# ---------------------------------------------------------------------
+
+# More matrix classes available in the module mrpt.math.
+# See: https://mrpt.github.io/pymrpt-docs/mrpt.pymrpt.mrpt.math.html
+
+from mrpt import pymrpt
+import numpy as np
+mrpt = pymrpt.mrpt
+
+# Create a numpy matrix from a list:
+m1_np = np.array([[1, 2, 3], [4, 5, 6], [7, 8, 9]])
+
+print('m1_np   :\n' + str(m1_np))
+print()
+
+# Create an MRPT matrix from a list:
+m1_mrpt = mrpt.math.CMatrixDynamic_double_t(
+    [[11, 12, 13], [14, 15, 16], [17, 18, 19]])
+
+print('m1_mrpt :\n' + str(m1_mrpt))
+print('m1_mrpt.size() :' + str(m1_mrpt.size()))
+print()
+
+# Convert an MRPT matrix to numpy via an intermediary list:
+m2_mrpt = mrpt.math.CMatrixDynamic_double_t.Identity(3)
+m2_data = m2_mrpt.to_list()
+print('m2_data: {}'.format(m2_data))
+m2_np = np.array(m2_data)
+print('m2_np:\n{}'.format(m2_np))
+print()
+
+# Read/write access an MRPT matrix (0-based indices)
+m2_mrpt[0, 2] = 99.0  # modify the entry (0,2)
+print('m2 modified:\n{}'.format(m2_mrpt))
+m2_mrpt[0, 2] = 99.0  # modify the entry (0,2)
+print('m2[1,1]={}'.format(m2_mrpt[1, 1]))
+print()
+
+# Convert a numpy matrix to MRPT:
+m3_np = np.array([[1, 2], [3, 4]])
+m3_mrpt = mrpt.math.CMatrixDynamic_double_t(m3_np.tolist())
+print('m3_np:\n{}'.format(m3_np))
+print('m3_mrpt:\n{}'.format(m3_mrpt))

python-examples/ros-poses-convert.py

@@ -55,7 +55,7 @@
                  0.0, 0.0, 0.0,  0.0, 1.0, 0.0,
                  0.0, 0.0, 0.0,  0.0, 0.0, 1.0]
 
-mr3 = ros_bridge.ROS_PoseWithCovariance_msg_to_CPose3DPDFGaussian()
+mr3 = ros_bridge.ROS_PoseWithCovariance_msg_to_CPose3DPDFGaussian(r3)
 
 mr3b = mr3
 mr3b.mean.x(mr3b.mean.x()+1.0)

python-examples/se2-poses-example.out

@@ -0,0 +1,4 @@
+p1             : (1.000,2.000,90.00deg)
+p2             : (3.000,0.000,0.00deg)
+p1(+)p2        : (1.000,5.000,90.00deg)
+(p1(+)p2)(-)p1 : (3.000,-0.000,0.00deg)

python-examples/se3-poses-example.out

@@ -0,0 +1,4 @@
+p1             : (x,y,z,yaw,pitch,roll)=(1.0000,2.0000,0.0000,90.00deg,0.00deg,0.00deg)
+p2             : (x,y,z,yaw,pitch,roll)=(3.0000,0.0000,0.0000,0.00deg,0.00deg,0.00deg)
+p1(+)p2        : (x,y,z,yaw,pitch,roll)=(1.0000,5.0000,0.0000,90.00deg,-0.00deg,0.00deg)
+(p1(+)p2)(-)p1 : (x,y,z,yaw,pitch,roll)=(3.0000,-0.0000,0.0000,0.00deg,-0.00deg,0.00deg)

python/ros_bridge.py

@@ -87,15 +87,13 @@ def ROS_PoseWithCovariance_msg_to_CPose3DPDFGaussian(p: PoseWithCovariance) -> _
     Converts to mrpt::poses::CPose3DPDFGaussian from ROS geometry_msgs.PoseWithCovariance
     """
     # rearrange covariance (see notes above)
-    ind_map = [0, 1, 2, 5, 4, 3] # X,Y,Z,YAW,PITCH,ROLL
- 
+    ind_map = [0, 1, 2, 5, 4, 3]  # X,Y,Z,YAW,PITCH,ROLL
+
     ret = _mrpt.poses.CPose3DPDFGaussian()
-    ret.mean = ROS_Pose_msg_to_CPose3D(p.mean)
-    
+    ret.mean = ROS_Pose_msg_to_CPose3D(p.pose)
+
     for i in range(0, 6):
         for j in range(0, 6):
-            # WIP!!
-            #ret.cov(i,j) =
-            p.covariance[ind_map[i] * 6 + ind_map[j]]
+            ret.cov[i, j] = p.covariance[ind_map[i] * 6 + ind_map[j]]
 
     return ret