Add Drawer option to Settings.yaml

.gitignore

@@ -145,4 +145,5 @@ Dataset/
 .bash_history
 docker_*.sh
 results/**
-save*.ipynb
+save*.ipynb
+save*.py

docs/source/config.rst

@@ -29,3 +29,21 @@ add your own settings
 if you add your method with :ref:`this <addmethod>` you can add in **settings.yaml** the params needed for your application execution in the form ::
 
     params_name : 'params_values'
+
+---------------------------------
+Drawer Params
+---------------------------------
+See **setting.yaml** for examples values
+
+- **Drawer.eye.x ** determines the x view point about the origin of this scene. 
+- **Drawer.eye.y ** determines the y view point about the origin of this scene.
+- **Drawer.eye.z ** determines the z view point about the origin of this scene.
+- **Drawer.center.x ** determines the x plane translation about the origin of this scene.
+- **Drawer.center.y ** determines the y plane translation about the origin of this scene.
+- **Drawer.scale_grade.x ** determines the zoom about x plane.
+- **Drawer.scale_grade.y ** determines the zoom about y plane.
+- **Drawer.scale_grade.z **  (float): determines the zoom about z plane.
+- **Drawer.aspectratio.x** to set the x-axis aspecratio. 
+- **Drawer.aspectratio.y** to set the y-axis aspecratio.
+- **Drawer.aspectratio.z** to set the z-axis aspecratio. 
+- **Drawer.point_size** the size of the marker point in pixel.

settings.yaml

@@ -8,3 +8,21 @@ SLAM.alg: "OrbSlam2"
 #--------------------------------------------------------------------------------------------
 SLAM.vocab_path: "slam_method/Settings/ORBvoc.bin"
 SLAM.settings_path: "slam_method/Settings/OrbSlam2_KITTI_02.yaml"
+
+#--------------------------------------------------------------------------------------------
+# Drawer Params: the extra params of trajectory_drawer class
+# Params Guide https://slampy.readthedocs.io/en/latest/config.html
+#--------------------------------------------------------------------------------------------
+
+Drawer.eye.x: -18.0
+Drawer.eye.y: -13.0
+Drawer.eye.z: -55.0
+Drawer.center.x: -17.0
+Drawer.center.y: -8.0
+Drawer.scale_grade.x: 1.0
+Drawer.scale_grade.y: 1.0
+Drawer.scale_grade.z: 10.0
+Drawer.aspectratio.x: 50
+Drawer.aspectratio.y: 50
+Drawer.aspectratio.z: 100
+Drawer.point_size: 2

slampy.py

@@ -307,6 +307,7 @@ def get_state(self):
     def shutdown(self):
         """Shutdown the SLAM system"""
         self.slam.shutdown()
+        self.pose_array = []
 
     def reset(self):
         """Reset SLAM system"""

trajectory_drawer.py

@@ -1,5 +1,6 @@
 import slampy
 import numpy as np
+import yaml
 import plotly.graph_objects as go
 import time
 
@@ -9,48 +10,33 @@ class TrajectoryDrawer:
 
     def __init__(
         self,
-        eye_x=-18.0,
-        eye_y=-13.0,
-        eye_z=-55.0,
-        center_x=-17.0,
-        center_y=-8.0,
-        scale_grade_x=1.0,
-        scale_grade_y=1.0,
-        scale_grade_z=10.0,
-        aspectratio=dict(x=50, y=50, z=100),
+        params_file,
         width=None,
         height=None,
-        point_size=2,
         drawpointcloud=True,
         useFigureWidget=True,
     ):
         """Build the Trajectory drawer
 
         Args:
-            eye_x (float): determines the x view point about the origin of this scene. Defaults to -18.0
-            eye_y (float): determines the y view point about the origin of this scene. Defaults to -13.0
-            eye_z (float): determines the z view point about the origin of this scene. Defaults to -55.0
-            center_x (float): determines the x plane translation about the origin of this scene. Defaults to -17.0
-            center_y (float): determines the y plane translation about the origin of this scene. Defaults to -8.0
-            scale_grade_x (float): determines the zoom about x plane. Defaults to 1
-            scale_grade_y (float): determines the zoom about y plane. Defaults to 1
-            scale_grade_z (float): determines the zoom about z plane. Defaults to 10
-            aspectratio (dict): a dict in the form (x=(int), y=(int), z=(int)) to set the scene aspecratio Defaults to dict(x=50, y=50, z=100)
+            params_file (str): the Path to the .yaml file.
             width(int): the width of figure in pixel. Defaults to None
             height(int): the height of figure in pixel. Defaults to None
-            point_size (int): the size of the marker point in pixel. Defauts to 2
             drawpointcloud (bool): if is false the plot show only trajectory and not the point cloud. Defaults to True
             useFigureWidget (bool): use the plotily.graph_object.FigureWidget instance if false it used the plotily.graph_object.Figure
         """
-        self.eye_x = eye_x
-        self.eye_y = eye_y
-        self.eye_z = eye_z
-        self.center_x = center_x
-        self.center_y = center_y
-        self.scale_grade_x = scale_grade_x
-        self.scale_grade_y = scale_grade_y
-        self.scale_grade_z = scale_grade_z
-        self.point_size = point_size
+        with open(params_file) as fs:
+            self.params = yaml.safe_load(fs)
+
+        self.eye_x = self.params["Drawer.eye.x"]
+        self.eye_y = self.params["Drawer.eye.y"]
+        self.eye_z = self.params["Drawer.eye.z"]
+        self.center_x = self.params["Drawer.center.x"]
+        self.center_y = self.params["Drawer.center.y"]
+        self.scale_grade_x = self.params["Drawer.scale_grade.x"]
+        self.scale_grade_y = self.params["Drawer.scale_grade.y"]
+        self.scale_grade_z = self.params["Drawer.scale_grade.z"]
+        self.point_size = self.params["Drawer.point_size"]
         self.drawpointcloud = drawpointcloud
         # initialize the figure
         if useFigureWidget == True:
@@ -64,7 +50,11 @@ def __init__(
             height=height,
             scene=dict(
                 aspectmode="manual",
-                aspectratio=aspectratio,
+                aspectratio=dict(
+                    x=self.params["Drawer.aspectratio.x"],
+                    y=self.params["Drawer.aspectratio.y"],
+                    z=self.params["Drawer.aspectratio.z"],
+                ),
                 xaxis=dict(
                     showticklabels=False,
                     showgrid=False,
@@ -114,23 +104,23 @@ def plot_trajcetory(self, slampy_app):
 
                 # draw the point cloud
                 self.figure.add_scatter3d(
-                    x=wp[..., 0],
+                    x=wp[..., 0] * -1,
                     y=wp[..., 1] * -1,
                     z=wp[..., 2],
                     mode="markers",
                     marker=dict(
                         size=self.point_size,
                         color=colors,
                     ),
+                    hoverinfo="skip",
                 )
 
             # get the camera center in absolute coordinates
             camera_center = pose[0:3, 3].flatten()
             if self.prec_camera_center is not None:
                 self.figure.add_scatter3d(
-                    x=np.array(
-                        [camera_center[0], self.prec_camera_center[0]]
-                    ).flatten(),
+                    x=np.array([camera_center[0], self.prec_camera_center[0]]).flatten()
+                    * -1,
                     y=np.array([camera_center[1], self.prec_camera_center[1]]).flatten()
                     * -1,
                     z=np.array(
@@ -140,6 +130,7 @@ def plot_trajcetory(self, slampy_app):
                         size=self.point_size * 2, color="red", symbol="diamond"
                     ),
                     line=dict(width=self.point_size, color="red"),
+                    hoverinfo="skip",
                 )
                 self.figure.update_layout(
                     scene_camera=dict(

trajectory_example.ipynb

@@ -20,7 +20,6 @@
     "import cv2\n",
     "import numpy as np\n",
     "import time\n",
-    "import os\n",
     "from utils import load_images_KITTI\n",
     "import plotly.graph_objects as go"
    ]
@@ -42,7 +41,7 @@
    "metadata": {},
    "outputs": [],
    "source": [
-    "image_folder = 'Dataset/image_02'\n",
+    "image_folder = 'Dataset/KITTI_RAW/2011_09_26/2011_09_26_drive_0002_sync/image_02/'\n",
     "setting_file ='settings.yaml'"
    ]
   },
@@ -95,7 +94,7 @@
    "metadata": {},
    "outputs": [],
    "source": [
-    "drawer = TrajectoryDrawer(height=600)\n",
+    "drawer = TrajectoryDrawer(setting_file,height=600)\n",
     "drawer.get_figure()"
    ]
   },
@@ -109,30 +108,27 @@
   {
    "cell_type": "code",
    "execution_count": null,
-   "metadata": {
-    "scrolled": false
-   },
+   "metadata": {},
    "outputs": [],
    "source": [
     "print('images in the sequences {}'.format(num_images))\n",
     "#initialize the figure and the array of coordinates\n",
-    "prec_camera_center = None\n",
-    "start_eye=-num_images /2\n",
     "for idx in range(num_images):\n",
     "    # load and convert to RGB image '\n",
     "    name = image_filenames[idx]\n",
     "    image = cv2.imread(name)\n",
     "    image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)\n",
-    "\n",
     "    if image is None:\n",
     "        print(\"failed to load image at {0}\".format(self.idx))\n",
     "        exit\n",
     "    t1 = time.time()\n",
     "    state = app.process_image_mono(image,timestamps[idx])\n",
     "    t2 = time.time()\n",
     "    if state == slampy.State.OK:\n",
+    "        print(idx)\n",
     "        #compute and plot the trajecotry\n",
     "        drawer.plot_trajcetory(app)\n",
+    "        time.sleep(1)\n",
     "    #sleep the execution if the time is less than the image acquisition\n",
     "    ttrack = t2 - t1\n",
     "    t = 0\n",
@@ -151,8 +147,20 @@
    "display_name": "Python 3",
    "language": "python",
    "name": "python3"
+  },
+  "language_info": {
+   "codemirror_mode": {
+    "name": "ipython",
+    "version": 3
+   },
+   "file_extension": ".py",
+   "mimetype": "text/x-python",
+   "name": "python",
+   "nbconvert_exporter": "python",
+   "pygments_lexer": "ipython3",
+   "version": "3.8.6"
   }
  },
  "nbformat": 4,
  "nbformat_minor": 4
-}
+}