From 255d8a7c2bc41c1e829f1c06acfb43535496ae3f Mon Sep 17 00:00:00 2001 From: giordanolaminetti Date: Wed, 13 Jan 2021 17:05:18 +0100 Subject: [PATCH] Add Drawer option to Settings.yaml --- .gitignore | 3 +- docs/source/config.rst | 18 ++++++++++++ settings.yaml | 18 ++++++++++++ slampy.py | 1 + trajectory_drawer.py | 59 +++++++++++++++++----------------------- trajectory_example.ipynb | 28 ++++++++++++------- 6 files changed, 82 insertions(+), 45 deletions(-) diff --git a/.gitignore b/.gitignore index b152c5a..f91ef4c 100644 --- a/.gitignore +++ b/.gitignore @@ -145,4 +145,5 @@ Dataset/ .bash_history docker_*.sh results/** -save*.ipynb \ No newline at end of file +save*.ipynb +save*.py \ No newline at end of file diff --git a/docs/source/config.rst b/docs/source/config.rst index 1b85eac..723d7f8 100644 --- a/docs/source/config.rst +++ b/docs/source/config.rst @@ -29,3 +29,21 @@ add your own settings if you add your method with :ref:`this ` 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. \ No newline at end of file diff --git a/settings.yaml b/settings.yaml index b7a5cbc..4ff48ef 100644 --- a/settings.yaml +++ b/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 diff --git a/slampy.py b/slampy.py index e89de58..33563f2 100644 --- a/slampy.py +++ b/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""" diff --git a/trajectory_drawer.py b/trajectory_drawer.py index 155e375..2f98ad1 100644 --- a/trajectory_drawer.py +++ b/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,7 +104,7 @@ 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", @@ -122,15 +112,15 @@ def plot_trajcetory(self, slampy_app): 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( diff --git a/trajectory_example.ipynb b/trajectory_example.ipynb index 0ef337b..25a74e0 100644 --- a/trajectory_example.ipynb +++ b/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,21 +108,16 @@ { "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", @@ -131,8 +125,10 @@ " 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 -} \ No newline at end of file +}