diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..3489a2c --- /dev/null +++ b/.gitignore @@ -0,0 +1,178 @@ +# Project files +# Ignore everything in the /data/ folder +/data/* +# Except the data folder itself +!/data/ +# And the demo_hallway folder and its contents +!/data/demo_hallway/ +/data/demo_hallway/* +!/data/demo_hallway/crazyflie-poses/ +!/data/demo_hallway/crazyflie-rgb-images/ + +/utils/calibration/calibration_pictures/* +/utils/calibration/calibration_output/* +/utils/calibration/flydrone_calibration_pictures/* +/utils/calibration/output/* +/utils/calibration/undistort_output/* + +# Byte-compiled / optimized / DLL files +cache/* +__pycache__/ +*.py[cod] +*$py.class + +# C extensions +*.so + +# Distribution / packaging +.Python +build/ +develop-eggs/ +dist/ +downloads/ +eggs/ +.eggs/ +lib/ +lib64/ +parts/ +sdist/ +var/ +wheels/ +share/python-wheels/ +*.egg-info/ +.installed.cfg +*.egg +MANIFEST + +# PyInstaller +# Usually these files are written by a python script from a template +# before PyInstaller builds the exe, so as to inject date/other infos into it. +*.manifest +*.spec + +# Installer logs +pip-log.txt +pip-delete-this-directory.txt + +# Unit test / coverage reports +htmlcov/ +.tox/ +.nox/ +.coverage +.coverage.* +.cache +nosetests.xml +coverage.xml +*.cover +*.py,cover +.hypothesis/ +.pytest_cache/ +cover/ + +# Translations +*.mo +*.pot + +# Django stuff: +*.log +local_settings.py +db.sqlite3 +db.sqlite3-journal + +# Flask stuff: +instance/ +.webassets-cache + +# Scrapy stuff: +.scrapy + +# Sphinx documentation +docs/_build/ + +# PyBuilder +.pybuilder/ +target/ + +# Jupyter Notebook +.ipynb_checkpoints + +# IPython +profile_default/ +ipython_config.py + +# pyenv +# For a library or package, you might want to ignore these files since the code is +# intended to run in multiple environments; otherwise, check them in: +# .python-version + +# pipenv +# According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. +# However, in case of collaboration, if having platform-specific dependencies or dependencies +# having no cross-platform support, pipenv may install dependencies that don't work, or not +# install all needed dependencies. +#Pipfile.lock + +# poetry +# Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control. +# This is especially recommended for binary packages to ensure reproducibility, and is more +# commonly ignored for libraries. +# https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control +#poetry.lock + +# pdm +# Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control. +#pdm.lock +# pdm stores project-wide configurations in .pdm.toml, but it is recommended to not include it +# in version control. +# https://pdm.fming.dev/#use-with-ide +.pdm.toml + +# PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm +__pypackages__/ + +# Celery stuff +celerybeat-schedule +celerybeat.pid + +# SageMath parsed files +*.sage.py + +# Environments +.env +.venv +env/ +venv/ +ENV/ +env.bak/ +venv.bak/ + +# Spyder project settings +.spyderproject +.spyproject + +# Rope project settings +.ropeproject + +# mkdocs documentation +/site + +# mypy +.mypy_cache/ +.dmypy.json +dmypy.json + +# Pyre type checker +.pyre/ + +# pytype static type analyzer +.pytype/ + +# Cython debug symbols +cython_debug/ + +# PyCharm +# JetBrains specific template is maintained in a separate JetBrains.gitignore that can +# be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore +# and can be added to the global gitignore or merged into this file. For a more nuclear +# option (not recommended) you can uncomment the following to ignore the entire idea folder. +#.idea/ diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..554dcd6 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "ZoeDepth"] + path = ZoeDepth + url = https://github.com/isl-org/ZoeDepth.git diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..d2bb0dd --- /dev/null +++ b/LICENSE @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2023 Nate Simon + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/README.md b/README.md new file mode 100644 index 0000000..7612e87 --- /dev/null +++ b/README.md @@ -0,0 +1,184 @@ +# MonoNav: MAV Navigation via Monocular
Depth Estimation and Reconstruction + +**Authors**: [Nathaniel Simon](https://natesimon.github.io/) and [Anirudha Majumdar](https://irom-lab.princeton.edu/majumdar/) + +_[Intelligent Robot Motion Lab](https://irom-lab.princeton.edu/), Princeton University_ + +[Project Page](https://natesimon.github.io/mononav) | [Paper (arXiv)](https://arxiv.org/pdf/2311.14100.pdf) | [Video](https://www.youtube.com/watch?v=msWLSfOmTpI) | + +--- + +MonoNav is a monocular navigation stack that uses RGB images and camera poses to generate a 3D reconstruction, enabling the use of conventional planning techniques. MonoNav leverages pre-trained depth-estimation ([ZoeDepth](https://github.com/isl-org/ZoeDepth)) and off-the-shelf fusion ([Open3D](https://github.com/isl-org/Open3D)) to generate a real-time 3D reconstruction of the environment. At each planning step, MonoNav selects from a library of motion primitives to navigate collision-free towards the goal. While the robot executes each motion primitive, new images and poses are integrated into the reconstruction. In our [paper](https://arxiv.org/pdf/2311.14100.pdf), we demonstrate MonoNav on a 37 gram micro aerial vehicle (MAV) navigating hallways at 0.5 m/s (see [project page](https://natesimon.github.io/mononav) and [video](https://www.youtube.com/watch?v=msWLSfOmTpI)). + +## Overview + + + +This repository contains code to run the following: +**MonoNav pipeline** ([`mononav_cf.py`](mononav_cf.py)): Monocular hallway navigation using a Crazyflie + FPV camera, as seen in our paper. We encourage you to adapt this script to other vehicles / scenes! + +We also offer scripts that break MonoNav into sub-parts, which can be run independently & offline: +1. **Data collection pipeline** ([`collect_dataset.py`](collect_dataset.py)): Collect images and poses from your own camera / robot. +2. **Depth estimation pipeline** ([`estimate_depth.py`](estimate_depth.py)): Estimate depths from RGB images using ZoeDepth. +3. **Fusion pipeline** ([`fuse_depth.py`](fuse_depth.py)): Fuse the estimated depth images and camera poses into a 3D reconstruction. +4. **Simulate MonoNav** ([`simulate.py`](simulate.py)): Step through the 3D reconstruction and visualize the motion primitives chosen by the MonoNav planner. This is a useful way to replay and debug MonoNav trials. + +These scripts (run in sequence) form a demo, which we highly recommend before adapting MonoNav for your system. We include a sample dataset ([`data/demo_hallway`](data/demo_hallway)), so no robot is needed to run the demo! + +In addition, we include the following resources in the `/utils/` directory: +- [`utils/test_camera.py`](utils/test_camera.py) to test your camera, +- [`utils/generate_primitives.py`](utils/generate_primitives.py) to generate and visualize new motion primitives, +- [`utils/calibration/take_pictures.py`](utils/calibration/take_pictures.py) to take pictures of a calibration target, +- [`utils/calibration/calibrate.py`](utils/calibration/calibrate.py) to calibrate the camera with OpenCV and save the camera intrinsics to file, +- [`utils/calibration/transform.py`](utils/calibration/transform.py) to test the undistortion and transformation pipeline. + +We hope you enjoy MonoNav! + +## Installation and Configuration + +Clone the repository and its submodules (ZoeDepth): + +``` +git clone --recurse-submodules https://github.com/natesimon/MonoNav.git +``` + +Install the dependencies from `environment.yml` using [mamba](https://github.com/mamba-org/mamba) (fastest): +```bash +mamba env create -n mononav --file environment.yml +mamba activate mononav +``` +or conda : + +```bash +conda env create -n mononav --file environment.yml +conda activate mononav +``` +**Note:** If the installation gets stuck at `Solving environment: ...`, we recommend updating your system, re-installing conda / miniconda, or using mamba. + +**Tested on:** (release / driver / GPU) +- Ubuntu 22.04 / NVIDIA 535 / RTX 4090 +- Ubuntu 22.04 / NVIDIA 535 / Titan RTX +- Ubuntu 20.04 / NVIDIA 530 / Titan Xp +- Ubuntu 18.04 / NVIDIA 470 / Titan Xp + +If you do not have access to GPU, set `device = "CPU:0"` in `config.yml`. This will reduce the speed of both depth estimation and fusion, and may not be fast enough for real-time operations. + +## Demo: Out of the Box! + +We include a demo dataset ([`data/demo_hallway`](data/demo_hallway)) to try MonoNav out of the box - no robot needed! From a series of ([occasionally noisy](data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000030.rgb.jpg)) images and poses, we will transform the images, estimate depth, and fuse them into a [3D reconstuction](utils/reconstruction.gif). +1. The dataset includes just RGB images and camera poses from a Crazyflie (see [Hardware](#mononav-hardware)): + ``` + ├── + │ ├── # camera poses + │ ├── # raw camera images + ``` +1. Set the dataset path in `config.yml`. By default, `data_dir: 'data/demo_hallway`, but make sure to change this if you want to process your own dataset. + ``` + data_dir: 'data/demo_hallway' # change to whichever directory you want to process + ``` +1. To demonstrate ZoeDepth, run `python estimate_depth.py`. This reads in the crazyflie images and transforms them to match the camera intrinsics used in the ZoeDepth training dataset. This is crucial for depth estimation accuracy (see [Calibration](#camera-calibration) for more details). The transformed images are saved in `` and used to estimate depth. The estimated depths are saved as numpy arrays and colormaps (for visualization) in ``. After running, take a look at the resulting images and note the loss of peripheral information as the raw images are undistorted. +1. To demonstrate fusion, run: `python fuse_depth.py`. This script reads in the (transformed) images, poses, and depths, and integrates them using Open3D's TSDF Fusion. After completion, a reconstruction should be displayed with coordinate frames to mark the camera poses throughout the run. The reconstruction is saved to file as a VoxelBlockGrid (`vbg.npz`) and pointcloud (`pointcloud.ply` - which can be opened using MeshLab). +1. Next, run `python simulate.py`. This loads the reconstruction (`vbg.npz`) and executes the MonoNav planner. The planner is executed at each of the camera poses, and does the following: + 1. visualizes (in black) the available motion primitives in the trajectory library (`utils/trajlib`), + 1. chooses a motion primitive according to the planner: `choose_primitive()` in `utils/utils.py` selects the primitive that makes the most progress towards `goal_position` while remaining `min_dist2obs` from all obstacles in the reconstruction, + 1. paints the chosen primitive green. +`simulate.py` is useful for debugging and de-briefing, and also to anticipate how changes in the trajectory library or planner affect performance. For example, by changing `min_dist2obs` in`config.yml`, it is possible to see how increasing/decreasing the distance threshold to obstacles affects planner performance. + +1. Finally, try changing the motion primitives to see how they affect planner performance! To modify and generate the trajectory library, open `utils/generate_primitives.py`. Try changing `num_trajectories` from `7` to `11`, and run `generate_primitives.py.` This will display the new motion primitives and update the trajectory library. Note that each motion primitive is defined by a set of gentle turns left, right, or straight. An "extension" segment is added to the primitive (but not flown) to encourage foresight in the planner. See our paper for more details. Feel free to re-run `simulate.py` to try out the new primitives! + +The tutorial should result in the additional files added to `data/demo_hallway`: +``` +├── +│ ├── # images transformed to match kinect intrinsics +│ ├── # estimated depth (.npy for fusion and .jpg for visualization) +│ ├── vbg.npz / pointcloud.ply # reconstructions generated by fuse_depth.py +``` +If you are unable to execute the full tutorial but want to reference the output, you can download it from [Google Drive](https://drive.google.com/file/d/1r9KgkOoOsSP_7FARzuB4NcOaF94TS1HF/view?usp=sharing). If you have made it through the tutorials, you can try MonoNav on your own dataset! + +## Collect your own Dataset + +To run MonoNav on your own dataset, there are two crucial steps: +1. Collect your own dataset (RGB images and poses). We provide [`collect_dataset.py`](collect_dataset.py) which works for the Crazyflie, but you may have to modify it for your system. Ensure that you are transforming the pose (rotation + translation) into the Open3D frame correctly, and saving it in homogeneous form. See `get_crazyflie_pose` in `utils/utils.py` for reference. Make sure to update `data_dir` in `config.yml` to point to your collected dataset. +2. Provide the camera intrinsics. This is crucial for the image transformation step and can affect depth estimation quality dramatically. We recommend that you follow our provided calibration sequence to automatically generate a `.json` of camera intrinsics and distortion coefficients. `config.yml` should then be updated to point to the intrinsics json file path: `camera_calibration_path: 'utils/calibration/intrinsics.json'`. + +With those steps complete, you can run `estimate_depth.py`, `fuse_depth.py`, and `simulate.py` to reconstruct and try MonoNav on your own dataset! + +## Camera Calibration +A key aspect to MonoNav is using a pre-trained depth estimation network on a different camera than the one used during training. The micro FPV camera ([Wolfwhoop WT05](https://a.co/d/bhTwelB)) that we use has significant barrel distortion (fish-eye), and thus the images must be first undistorted to better match the camera intrinsics used to collect the training data. To maintain the metric depth estimation accuracy of the model, we must transform the input image to match the intrinsics of the training dataset. ZoeDepth is trained on [NYU-Depth-v2](https://cs.nyu.edu/~silberman/datasets/nyu_depth_v2.html), which used the Microsoft [Kinect](https://en.wikipedia.org/wiki/Kinect). + +The `transform_image()` function in `utils/utils.py` performs the transformation: resizing the image and undistorting it to match the Kinect's intrinsics. + +In `utils/calibration`, we provide scripts to generate `intrinsics.json` for your own camera. Steps to calibrate: + +1. Make a chessboard calibration target. +1. `take_pictures.py`: Take many pictures (recommended: 80+) of the chessboard by pressing the spacebar. Saves them to `utils/calibration/calibration_pictures/`. +1. `calibrate.py`: Based on the [OpenCV sample](https://github.com/opencv/opencv/blob/4.x/samples/python/calibrate.py).You need to provide several arguments, including the structure and dimensions of your chessboard target. Example: + ``` + MonoNav/utils/calibration$ python calibrate.py -w 6 -h 8 -t chessboard --square_size=35 ./calibration_pictures/frame*.jpg + + ``` + The intrinsics are printed and saved to `utils/calibration/intrinsics.json`. +1. `transform.py`: This script loads the intrinsics from `intrinsics.json` and transforms your `calibration_pictures` to the Kinect's dimensions (640x480) and intrinsics. This operation may involve resizing your image. The transformed images are saved in `utils/calibration/transform_output` and should be inspected. +1. Finally, we recommend re-running calibration on `transform_output` to ensure that the intrinsics match the Kinect. + ``` + MonoNav/utils/calibration$ python calibrate.py -w 6 -h 8 -t chessboard --square_size=35 ./transform_output/frame*.jpg + + ``` + `transform.py` will save the intrinsics of the transformed images to `check_intrinsics.json`, which should roughly match those of the Kinect: + ``` + [[525. 0. 319.5] + [ 0. 525. 239.5] + [ 0. 0. 1. ]] + ``` + +## MonoNav: Hardware +To run MonoNav as shown in our paper, you need a monocular robot with pose (position & orientation) estimation. We used the [Crazyflie 2.1](https://www.bitcraze.io/products/crazyflie-2-1/) micro aerial vehicle modified with an FPV camera. Our hardware setup follows closely the one used in Princeton's [Introduction to Robotics](https://irom-lab.princeton.edu/intro-to-robotics/) course. If you are using the Crazyflie, we recommend that you follow the [Bitcraze tutorials](https://www.bitcraze.io/documentation/tutorials/getting-started-with-crazyflie-2-x/) to ensure that the vehicle flies and commmunicates properly. + +**List of parts:** +- [Crazyflie 2.1](https://www.bitcraze.io/products/crazyflie-2-1/) micro aerial vehicle, +- [Flowdeck v2](https://www.bitcraze.io/products/flow-deck-v2/) for position and velocity estimation (pose), +- 5.8 GHz micro FPV camera (e.g., [Wolfwhoop WT05](https://a.co/d/bhTwelB)), +- Custom PCB with [long pins](https://store.bitcraze.io/collections/spare-parts-crazyflie-2-0/products/male-long-deck-connector), to attach the camera to the Crazyflie, +- [Crazyradio PA](https://store.bitcraze.io/collections/kits/products/crazyradio-pa) for communication with the vehicle, +- 5.8 GHz video receiver (e.g., [Skydroid](https://a.co/d/9lO7Md8)). + +## Running `mononav_cf.py` + +The `mononav_cf.py` ("cf" for "crazyflie") script performs the image transformation, depth estimation, fusion, and planning process simultaneously for goal-directed obstacle avoidance. If `FLY_CRAZYFLIE: True`, the Crazyflie will takeoff, if `False`, the pipeline will execute without the Crazyflie starting its motors (useful for testing). + +After takeoff, the Crazyflie can be controlled manually by the following key commands: +``` +w: choose MIDDLE primitive (typically FORWARD) +a: choose FIRST primitive (typically LEFT) +d: choose LAST primitive (typically RIGHT) +c: end control (stop and land) +q: end control immediately (EMERGENCY stop and land) +g: start MonoNav +``` + +Manual control is an excellent way to check that the pipeline is working, as it should produce a sensible reconstruction after landing. As mentioned in the paper, it is HIGHLY RECOMMENDED to manually fly forward 3x (press `w, w, w`) before starting MonoNav (press `g`). This is due to the narrow field of view of the transformed images, which discards peripheral information; to make an informed decision, the planner needs information collected 3 primitives ago. + +After MonoNav is started, the Crazyflie will choose and execute primitives according to the planner. If collision seems imminent, you can manually choose a primitive (by `w, a, d`) or stop the planner (`c` or `q`). During the run, a `crazyflie_trajectories.csv` log is produced, which includes the `frame_number` and `time_elapsed` during replanning, as well as the `chosen_traj_idx`. + +## Future Work + +MonoNav is a "work in progress" - there are many exciting directions for future work! If you find any bugs or implement any exciting features, please submit a pull request as we'd love to continue improving the system. Once you get MonoNav running on your robot, send us a video! We'd love to see it. + +**Areas of future work in the pipeline:** +- Run MonoNav on new platforms and in new settings! +- Integrate with ROS for easier interoperability on new systems. +- Integrate improved depth estimation pipelines, which might take in multiple frames or metadata to improve the depth estimate. +- Improve the planner to treat space as unsafe UNTIL explored, which could prevent crashes into occluded obstacles. (The current planner treats unseen space as free.) +- Improve the motion primitive control from open-loop to closed-loop, for more reliable and accurate primitive tracking. + +## Citation +``` +@inproceedings{simon2023mononav, + author = {Nathaniel Simon and Anirudha Majumdar}, + title = {{MonoNav: MAV Navigation via Monocular Depth Estimation and Reconstruction}}, + booktitle = {Symposium on Experimental Robotics (ISER)}, + year = {2023}, + url = {https://arxiv.org/abs/2311.14100} +} +``` + diff --git a/ZoeDepth b/ZoeDepth new file mode 160000 index 0000000..edb6daf --- /dev/null +++ b/ZoeDepth @@ -0,0 +1 @@ +Subproject commit edb6daf45458569e24f50250ef1ed08c015f17a7 diff --git a/collect_dataset.py b/collect_dataset.py new file mode 100644 index 0000000..2ec74a3 --- /dev/null +++ b/collect_dataset.py @@ -0,0 +1,105 @@ +""" + __ __ _ _ + | \/ | ___ _ __ ___ | \ | | __ ___ __ + | |\/| |/ _ \| '_ \ / _ \| \| |/ _` \ \ / / + | | | | (_) | | | | (_) | |\ | (_| |\ V / + |_| |_|\___/|_| |_|\___/|_| \_|\__,_| \_/ +Copyright (c) 2023 Nate Simon +License: MIT +Authors: Nate Simon and Anirudha Majumdar, Princeton University +Project Page: https://natesimon.github.io/mononav + +The purpose of this script is to collect a custom dataset for the MonoNav demo. +This script saves images and poses from a Crazyflie to file. +Those images and poses can then be used in the demo (depth estimation, fusion, and simulated planning). + +""" +import cv2 +import numpy as np +import time +import os +import time + +# For Craziflie logging +import logging +import cflib.crtp +from cflib.crazyflie import Crazyflie +from cflib.crazyflie.log import LogConfig +from cflib.crazyflie.syncCrazyflie import SyncCrazyflie +from cflib.utils import uri_helper + +# sys.path.append('/home/nsimon/Documents/MonoNav/') +from utils.utils import reset_estimator, VideoCapture, get_crazyflie_pose, load_config +''' +This file collects the following synchronized images: +- Crazyflie (front-facing camera) +- Crazyflie pose +- Timestamps +''' +# LOAD CONFIG +CONFIG_PATH = "config.yml" +config = load_config("config.yml") + +URI = uri_helper.uri_from_env(default=config["radio_uri"]) +logging.basicConfig(level=logging.ERROR) +height = config["height"] +FLY_CRAZYFLIE = config["FLY_CRAZYFLIE"] +camera_num = config["camera_num"] + +# Make directories for data +save_dir = 'data/trial-' + time.strftime("%Y-%m-%d-%H-%M-%S") +os.mkdir(save_dir) if not os.path.exists(save_dir) else None + +crazyflie_img_dir = os.path.join(save_dir, "crazyflie-rgb-images") +crazyflie_pose_dir = os.path.join(save_dir, "crazyflie-poses") + +os.mkdir(crazyflie_img_dir) if not os.path.exists(crazyflie_img_dir) else None +os.mkdir(crazyflie_pose_dir) if not os.path.exists(crazyflie_pose_dir) else None + +print("Saving files to: " + save_dir) + +# Drone object +# Initialize the low-level drivers +cflib.crtp.init_drivers() + +# Set up log conf +logstate = LogConfig(name='state', period_in_ms=10) +logstate.add_variable('stateEstimate.x', 'float') +logstate.add_variable('stateEstimate.y', 'float') +logstate.add_variable('stateEstimate.z', 'float') +logstate.add_variable('stateEstimate.roll', 'float') +logstate.add_variable('stateEstimate.pitch', 'float') +logstate.add_variable('stateEstimate.yaw', 'float') + +with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: + cf = scf.cf + + reset_estimator(cf) + + # Camera Object + cap = VideoCapture(camera_num) + + # Initialize counter for images + frame_number = 0 + + while True: + + start_time = time.time() + + crazyflie_rgb = cap.read() + + # Save images and depth array + cv2.imwrite(crazyflie_img_dir + "/crazyflie_frame-%06d.rgb.jpg"%(frame_number), crazyflie_rgb) + camera_position = get_crazyflie_pose(scf, logstate) + np.savetxt(crazyflie_pose_dir + "/crazyflie_frame-%06d.pose.txt"%(frame_number), camera_position) + + # Update counter + frame_number += 1 + + # Show images + cv2.imshow('crazyflie', crazyflie_rgb) + if chr(cv2.waitKey(1)&255) == 'q': + break + time.sleep(0.1) +cap.cap.release() +cv2.destroyAllWindows() \ No newline at end of file diff --git a/config.yml b/config.yml new file mode 100644 index 0000000..ee1cb73 --- /dev/null +++ b/config.yml @@ -0,0 +1,41 @@ +# Directory Settings +data_dir: 'data/demo_hallway' +save_dir_prefix: 'data/mononav-' +# Camera Settings +camera_num: 0 # camera number index (test with utils/test_camera.py), try (-1, 0, 1, 2, ...) +camera_source: 'crazyflie' # used for naming files +camera_calibration_path: 'utils/calibration/demo_intrinsics.json' # camera calibration file (for undistortion) + +# ZoeDepth Settings +zoedepth_mode: "eval" # "eval" or "infer" - see ZoeDepth documentation for other settings +num_prezoe_frames: 5 # how many times to run ZoeDepth before starting (the first few inferences are much slower) + +# Reconstruction settings +filterYvals: True #this helps filter out the ground / ceiling (for planar navigation) +filterWeights: True # this helps filter out low-weight voxels +filterTSDF: True +weight_threshold: 3. # determine the weight threshold for filtering (higher threshold -> fewer points) +VoxelBlockGrid: + depth_scale: 1000.0 # 1000.0 is default (mm to m), scale factor for depth values during integration + depth_max: 7.0 # max depth to include in the reconstruction + trunc_voxel_multiplier: 8.0 + device: "CUDA:0" # "CUDA:0" (faster) or "CPU:0" (if no GPU is available) + +# Planning Settings +trajlib_dir: 'utils/trajlib/' # where to save & retrieve the trajectory library (motion primitives) +# comment out goal position for undirected exploration +goal_position: # goal position in meters + - -10.0 # +X RIGHT + - -1.0 # +Y DOWN (HEIGHT) + - 10.0 # +Z FORWARD +min_dist2obs: 0.5 # minimum acceptable distance from obstacles, meters (for primitive selection) +min_dist2goal: 1.0 # termination condition for reaching the goal, meters + + +# Crazyflie Settings +radio_uri: 'radio://0/4/2M/E7E7E7E7E7' # update the drone ID# to match your vehicle, check using cfclient +height: 1.0 # crazyflie flight altitude, meters +FLY_CRAZYFLIE: True # whether to actually fly the crazyflie +# Control gain settings (TUNED FOR CRAZYFLIE) +yvel_gain: 0.015 # yvel = -yawrate*yvel_gain +yawrate_gain: 1.25 # yawrate = yawrate*yawrate_gain \ No newline at end of file diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000000.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000000.pose.txt new file mode 100644 index 0000000..4efe1d2 --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000000.pose.txt @@ -0,0 +1,4 @@ +9.973858499659662424e-01 -4.017458997433130674e-03 7.214794737808424840e-02 -1.190161481499671936e-01 +4.290575044141858169e-03 9.999842035743383706e-01 -3.630918284212129865e-03 -1.066727876663208008e+00 +-7.213222063306701237e-02 3.930982701562352015e-03 9.973873320438466639e-01 2.025074213743209839e-01 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000001.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000001.pose.txt new file mode 100644 index 0000000..e01e4a8 --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000001.pose.txt @@ -0,0 +1,4 @@ +9.973231373594130256e-01 -1.583031948437643374e-02 7.138599773484928523e-02 -1.235054507851600647e-01 +6.707956845582694304e-03 9.919743706079799939e-01 1.262610445539708859e-01 -1.062878727912902832e+00 +-7.281183284697025082e-02 -1.254442068886586215e-01 9.894251805748327522e-01 2.397804558277130127e-01 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000002.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000002.pose.txt new file mode 100644 index 0000000..bcbee59 --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000002.pose.txt @@ -0,0 +1,4 @@ +9.975257703010298460e-01 -5.153753894615985091e-03 7.011259805577305104e-02 -1.180345341563224792e-01 +-2.185217524087768558e-03 9.945543120639754875e-01 1.041966658743187640e-01 -1.061743021011352539e+00 +-7.026779069893318574e-02 -1.040920706670096646e-01 9.920822941745028878e-01 3.021039664745330811e-01 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000003.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000003.pose.txt new file mode 100644 index 0000000..4419aff --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000003.pose.txt @@ -0,0 +1,4 @@ +9.981825273087893136e-01 -7.393070220438345379e-03 5.980789820879305052e-02 -1.117867603898048401e-01 +5.369992913698918256e-03 9.994102418227833740e-01 3.391654044610115104e-02 -1.064025521278381348e+00 +-6.002337337691465713e-02 -3.353373007049557930e-02 9.976335417356486657e-01 3.606672883033752441e-01 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000004.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000004.pose.txt new file mode 100644 index 0000000..acd4120 --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000004.pose.txt @@ -0,0 +1,4 @@ +9.991764452100482252e-01 -1.481897976480400780e-02 3.777339243623332316e-02 -1.035078987479209900e-01 +1.569222025239149637e-02 9.996139746801602222e-01 -2.292718577763568077e-02 -1.084493637084960938e+00 +-3.741905344823400620e-02 2.350105237775779177e-02 9.990232804975948122e-01 4.531985223293304443e-01 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000005.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000005.pose.txt new file mode 100644 index 0000000..6999d16 --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000005.pose.txt @@ -0,0 +1,4 @@ +9.999322530339349591e-01 -4.178664047195069370e-03 1.086407424768178961e-02 -1.003286167979240417e-01 +4.579489743263776003e-03 9.992997506329315272e-01 -3.713538283971151710e-02 -1.106485486030578613e+00 +-1.070129039741493995e-02 3.718261894677778179e-02 9.992511872557813257e-01 5.365260243415832520e-01 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000006.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000006.pose.txt new file mode 100644 index 0000000..ecff640 --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000006.pose.txt @@ -0,0 +1,4 @@ +9.998149747159122969e-01 9.091321174341769618e-03 -1.695182034841612867e-02 -8.217989653348922729e-02 +-9.229623847170614209e-03 9.999246132365710027e-01 -8.098264464320626202e-03 -1.106362223625183105e+00 +1.687691848234593173e-02 8.253225005978191350e-03 9.998235113756533954e-01 6.322807669639587402e-01 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000007.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000007.pose.txt new file mode 100644 index 0000000..7666abe --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000007.pose.txt @@ -0,0 +1,4 @@ +9.996674927158817336e-01 2.086202064726471253e-02 -1.515520048218982167e-02 -6.418107450008392334e-02 +-2.047873281830910400e-02 9.994771629693761650e-01 2.502043574446938257e-02 -1.104636311531066895e+00 +1.566925362927588591e-02 -2.470175696584986369e-02 9.995720572792645697e-01 7.002592682838439941e-01 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000008.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000008.pose.txt new file mode 100644 index 0000000..71abd76 --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000008.pose.txt @@ -0,0 +1,4 @@ +9.999037680827647101e-01 1.204631343878742253e-02 -6.880472834267944982e-03 -7.072885334491729736e-02 +-1.189533807403226132e-02 9.996964115362398928e-01 2.157743482597852813e-02 -1.099065184593200684e+00 +7.138312545208991625e-03 -2.149351283758339087e-02 9.997435038048047007e-01 7.894267439842224121e-01 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000009.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000009.pose.txt new file mode 100644 index 0000000..a7a2e1a --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000009.pose.txt @@ -0,0 +1,4 @@ +9.999974945440481822e-01 -5.593867418218791392e-04 -2.167485201707290005e-03 -7.114133238792419434e-02 +5.870567561399048923e-04 9.999180780975077676e-01 1.278641693953479035e-02 -1.093339920043945312e+00 +2.160155085084559598e-03 -1.278765734056186258e-02 9.999159012385736567e-01 8.717800378799438477e-01 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000010.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000010.pose.txt new file mode 100644 index 0000000..6049ab1 --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000010.pose.txt @@ -0,0 +1,4 @@ +9.999461039591166500e-01 -8.223223965381732878e-03 -6.337804398902351956e-03 -6.862983107566833496e-02 +8.220563689088003848e-03 9.999661112750125591e-01 -4.456839272942523383e-04 -1.095187544822692871e+00 +6.341254577543964203e-03 3.935595819849268618e-04 9.999798165969342456e-01 9.364930391311645508e-01 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000011.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000011.pose.txt new file mode 100644 index 0000000..cebe87c --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000011.pose.txt @@ -0,0 +1,4 @@ +9.998024450089053605e-01 -4.722052839336554272e-03 -1.930733464766789392e-02 -5.910723283886909485e-02 +4.837187766033845468e-03 9.999707716873225127e-01 5.920927763166157314e-03 -1.091253280639648438e+00 +1.927881139309828296e-02 -6.013151257287054942e-03 9.997960639266519900e-01 9.967969059944152832e-01 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000012.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000012.pose.txt new file mode 100644 index 0000000..cabb598 --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000012.pose.txt @@ -0,0 +1,4 @@ +9.993049444368701240e-01 2.505196133939321251e-02 -2.760484118906599352e-02 -4.156760871410369873e-02 +-2.425197715817516142e-02 9.992868105808396795e-01 2.894325142572218290e-02 -1.069482684135437012e+00 +2.831023892416588070e-02 -2.825366227983138606e-02 9.991998103181535251e-01 1.074659585952758789e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000013.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000013.pose.txt new file mode 100644 index 0000000..985644a --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000013.pose.txt @@ -0,0 +1,4 @@ +9.994348560344542554e-01 1.915117220353811101e-02 -2.762609539221409088e-02 -3.268329799175262451e-02 +-1.848503295604886354e-02 9.995369541851228945e-01 2.416983191792709817e-02 -1.067682862281799316e+00 +2.807618385755213108e-02 -2.364550319949840218e-02 9.993260819564547814e-01 1.151242017745971680e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000014.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000014.pose.txt new file mode 100644 index 0000000..7b95a18 --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000014.pose.txt @@ -0,0 +1,4 @@ +9.993506077392594333e-01 3.689227145422810297e-03 -3.584344311366449709e-02 -3.555935621261596680e-02 +-3.376380431820839943e-03 9.999557130724356924e-01 8.784756625914479436e-03 -1.072049617767333984e+00 +3.587426468030591176e-02 -8.658030773011060263e-03 9.993188058056253764e-01 1.227163791656494141e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000015.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000015.pose.txt new file mode 100644 index 0000000..799a97f --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000015.pose.txt @@ -0,0 +1,4 @@ +9.991771655777602623e-01 2.139272389019468770e-03 -4.050204070955430724e-02 -3.083843924105167389e-02 +-2.246320097794118724e-03 9.999941029943016924e-01 -2.597695640344983327e-03 -1.068561792373657227e+00 +4.049624469023098416e-02 2.686538715001153370e-03 9.991760789148834698e-01 1.325551271438598633e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000016.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000016.pose.txt new file mode 100644 index 0000000..64a2d68 --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000016.pose.txt @@ -0,0 +1,4 @@ +9.994073360650970184e-01 3.031424367846272550e-03 -3.428975190298865633e-02 -2.892226167023181915e-02 +-3.203532100456695734e-03 9.999825409825875422e-01 -4.965391433647593951e-03 -1.068861246109008789e+00 +3.427410102902524874e-02 5.072296946160115109e-03 9.993995986602868253e-01 1.403288245201110840e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000017.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000017.pose.txt new file mode 100644 index 0000000..544a7a3 --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000017.pose.txt @@ -0,0 +1,4 @@ +9.995003547925970233e-01 5.656228651207249707e-03 -3.109739292799992838e-02 -2.721573412418365479e-02 +-5.434422493726832372e-03 9.999592224201426793e-01 7.212527231404792724e-03 -1.064140915870666504e+00 +3.113692055475032749e-02 -7.039927155116171287e-03 9.994903359232736317e-01 1.509033083915710449e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000018.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000018.pose.txt new file mode 100644 index 0000000..5245237 --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000018.pose.txt @@ -0,0 +1,4 @@ +9.995652814381734697e-01 -3.130907126895011040e-03 -2.931630201760758794e-02 -3.390425443649291992e-02 +3.244286523991553577e-03 9.999874384041229636e-01 3.820685659721460067e-03 -1.052048802375793457e+00 +2.930397154610739005e-02 -3.914135220315266041e-03 9.995628828628551066e-01 1.651141881942749023e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000019.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000019.pose.txt new file mode 100644 index 0000000..f2acce4 --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000019.pose.txt @@ -0,0 +1,4 @@ +9.996881602417060364e-01 -3.180018490113281290e-03 -2.476832160150686454e-02 -2.798685990273952484e-02 +3.300082668394602008e-03 9.999829955555384897e-01 4.808123766457165181e-03 -1.047238826751708984e+00 +2.475261050747770350e-02 -4.888361911146342324e-03 9.996816554238107111e-01 1.712441802024841309e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000020.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000020.pose.txt new file mode 100644 index 0000000..deebf96 --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000020.pose.txt @@ -0,0 +1,4 @@ +9.996857038076845114e-01 4.082434249795299265e-03 -2.473514368526847501e-02 -1.468169875442981720e-02 +-3.862873398278169300e-03 9.999527747428713642e-01 8.917762787924317022e-03 -1.044185996055603027e+00 +2.477038174198479409e-02 -8.819411240491686552e-03 9.996542633199378969e-01 1.775212168693542480e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000021.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000021.pose.txt new file mode 100644 index 0000000..725f401 --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000021.pose.txt @@ -0,0 +1,4 @@ +9.995729046367036741e-01 1.463715999302989124e-02 -2.529351425726483402e-02 -2.451116219162940979e-03 +-1.421503884934546477e-02 9.997580035399140641e-01 1.678889598506730363e-02 -1.039546370506286621e+00 +2.553313507299120530e-02 -1.642217723763372134e-02 9.995390793301284216e-01 1.841038942337036133e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000022.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000022.pose.txt new file mode 100644 index 0000000..74ad056 --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000022.pose.txt @@ -0,0 +1,4 @@ +9.995053090821061081e-01 1.353134424745025370e-02 -2.839084076847010404e-02 4.270572215318679810e-03 +-1.320920871154655958e-02 9.998465813719954465e-01 1.150350050848315125e-02 -1.035794973373413086e+00 +2.854214291006251444e-02 -1.112278929005060951e-02 9.995307046992157884e-01 1.892839074134826660e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000023.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000023.pose.txt new file mode 100644 index 0000000..6ec1257 --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000023.pose.txt @@ -0,0 +1,4 @@ +9.997594452052825931e-01 9.750642440430702662e-03 -1.964628959432940941e-02 1.060404255986213684e-02 +-9.617802241490058801e-03 9.999303210536191200e-01 6.844773016656921678e-03 -1.033187508583068848e+00 +1.971166159584151339e-02 -6.654172345591708761e-03 9.997835627712260420e-01 1.981160163879394531e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000024.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000024.pose.txt new file mode 100644 index 0000000..99149cf --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000024.pose.txt @@ -0,0 +1,4 @@ +9.998021571735085100e-01 -1.034123185281389870e-02 -1.699133411375334732e-02 4.152660258114337921e-03 +1.029866735988329424e-02 9.999436113586583907e-01 -2.590666635209668434e-03 -1.034368634223937988e+00 +1.701716667983614248e-02 2.415165992361878369e-03 9.998522805951987058e-01 2.135844230651855469e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000025.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000025.pose.txt new file mode 100644 index 0000000..e966c2c --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000025.pose.txt @@ -0,0 +1,4 @@ +9.998415562092889397e-01 -3.133681176441133545e-03 -1.752262877742586863e-02 7.980545982718467712e-03 +3.115556964018594346e-03 9.999945832097653087e-01 -1.061534705823487809e-03 -1.035024285316467285e+00 +1.752586037234720243e-02 1.006773764125303916e-03 9.998459034395233980e-01 2.213798284530639648e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000026.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000026.pose.txt new file mode 100644 index 0000000..97f97d9 --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000026.pose.txt @@ -0,0 +1,4 @@ +9.997782667083996433e-01 1.585468227517542600e-02 -1.385808311063507423e-02 2.007640525698661804e-02 +-1.570496273996747905e-02 9.998178356406620670e-01 1.084664372781993756e-02 -1.034354090690612793e+00 +1.402752875186018267e-02 -1.062659798690345457e-02 9.998451399353504021e-01 2.299170970916748047e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000027.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000027.pose.txt new file mode 100644 index 0000000..98c1529 --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000027.pose.txt @@ -0,0 +1,4 @@ +9.999032164167211967e-01 1.050390968440758493e-02 -9.122811016191646749e-03 2.037889324128627777e-02 +-1.041280049221450857e-02 9.998960041954539912e-01 9.977694116058774196e-03 -1.029215216636657715e+00 +9.226667079974024827e-03 -9.881734428029581921e-03 9.999086057931939608e-01 2.402371644973754883e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000028.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000028.pose.txt new file mode 100644 index 0000000..fdc34f8 --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000028.pose.txt @@ -0,0 +1,4 @@ +9.999924240114005380e-01 3.167812476843815620e-03 2.262053030966810260e-03 2.056029438972473145e-02 +-3.167799851235118206e-03 9.999949824674708720e-01 -9.164339572669566469e-06 -1.019667744636535645e+00 +-2.262070711951385139e-03 1.998538888754832607e-06 9.999974415127770300e-01 2.481690406799316406e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000029.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000029.pose.txt new file mode 100644 index 0000000..1e09d48 --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000029.pose.txt @@ -0,0 +1,4 @@ +9.998556692074460628e-01 -8.503492315082515282e-03 1.470820764664943631e-02 2.253521792590618134e-02 +8.500342359977887050e-03 9.999638331636192534e-01 2.766667385285464338e-04 -1.022908806800842285e+00 +-1.471002833079494698e-02 -1.516020065007365027e-04 9.998917901869873726e-01 2.557897806167602539e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000030.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000030.pose.txt new file mode 100644 index 0000000..a01244f --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000030.pose.txt @@ -0,0 +1,4 @@ +9.999833250718316302e-01 -3.446883244156994405e-04 5.764613451254040240e-03 3.635245189070701599e-02 +3.136486600988682284e-04 9.999854539879843296e-01 5.384555409946420630e-03 -1.012791633605957031e+00 +-5.766385592499490183e-03 -5.382657559586767441e-03 9.999688874134008421e-01 2.709801435470581055e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000031.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000031.pose.txt new file mode 100644 index 0000000..71a86f7 --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000031.pose.txt @@ -0,0 +1,4 @@ +9.999974143691278128e-01 2.149614991462924559e-03 -7.418965207274091358e-04 4.960525408387184143e-02 +-2.150171584356366013e-03 9.999974069406326604e-01 -7.502494042653954981e-04 -1.013996005058288574e+00 +7.402818495789413459e-04 7.518426692147748116e-04 9.999994433575370989e-01 2.788298368453979492e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000032.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000032.pose.txt new file mode 100644 index 0000000..609ea05 --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000032.pose.txt @@ -0,0 +1,4 @@ +9.999408019225315414e-01 9.186806898944955418e-03 -5.830542815747330118e-03 5.613939464092254639e-02 +-9.192202173565630630e-03 9.999573464981347248e-01 -8.992238930718566940e-04 -1.020525813102722168e+00 +5.822033126413894005e-03 9.527661890901531136e-04 9.999825979320160485e-01 2.893432378768920898e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000033.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000033.pose.txt new file mode 100644 index 0000000..52339fc --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000033.pose.txt @@ -0,0 +1,4 @@ +9.998731096908743954e-01 1.144297050868037062e-02 -1.108255128742637909e-02 6.200755387544631958e-02 +-1.144355316084969508e-02 9.999345203429750306e-01 1.084064863568179159e-05 -1.027031302452087402e+00 +1.108194965499174726e-02 1.159844917530817156e-04 9.999385865839173215e-01 2.974056005477905273e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000034.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000034.pose.txt new file mode 100644 index 0000000..b0d17c1 --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000034.pose.txt @@ -0,0 +1,4 @@ +9.998259023713802485e-01 5.085403285126527916e-03 -1.795281651114247201e-02 6.072515249252319336e-02 +-5.059870842061368716e-03 9.999861222225027158e-01 1.467334134911861456e-03 -1.024879455566406250e+00 +1.796002935177953841e-02 -1.376239742720972853e-03 9.998377584937737650e-01 3.054310083389282227e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000035.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000035.pose.txt new file mode 100644 index 0000000..6b37b82 --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000035.pose.txt @@ -0,0 +1,4 @@ +9.998942258234719827e-01 2.064347992795994517e-04 -1.454285218082587705e-02 5.960584431886672974e-02 +-1.635812439586646215e-04 9.999956417394900710e-01 2.947837037922902692e-03 -1.018476366996765137e+00 +1.454339733543474900e-02 -2.945146295037230806e-03 9.998899017928146726e-01 3.147603273391723633e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000036.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000036.pose.txt new file mode 100644 index 0000000..d193eb3 --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000036.pose.txt @@ -0,0 +1,4 @@ +9.999745815914624636e-01 3.921546654452082951e-04 -7.119156248996927859e-03 5.576657503843307495e-02 +-3.158540256715611921e-04 9.999425362705005593e-01 1.071561445521598994e-02 -1.014068484306335449e+00 +7.122949333929697209e-03 -1.071309346718940687e-02 9.999172431862295429e-01 3.271452665328979492e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000037.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000037.pose.txt new file mode 100644 index 0000000..793f31a --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000037.pose.txt @@ -0,0 +1,4 @@ +9.999280120177254094e-01 -6.643202525755591802e-03 -9.991928866919500343e-03 5.828869342803955078e-02 +6.674749800334742710e-03 9.999728336470082857e-01 3.127248483233122361e-03 -1.019704341888427734e+00 +9.970882477630354027e-03 -3.193716984134173115e-03 9.999451893351168907e-01 3.356578826904296875e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000038.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000038.pose.txt new file mode 100644 index 0000000..6bebfbd --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000038.pose.txt @@ -0,0 +1,4 @@ +9.998547267989706944e-01 -4.973341634265564595e-03 -1.630310310168976012e-02 5.957882478833198547e-02 +4.946069216320495357e-03 9.999863013830052605e-01 -1.712730464638427324e-03 -1.021987199783325195e+00 +1.631139776545260933e-02 1.631845374419562235e-03 9.998656286639775459e-01 3.449634075164794922e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000039.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000039.pose.txt new file mode 100644 index 0000000..34fd411 --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000039.pose.txt @@ -0,0 +1,4 @@ +9.997078143762182512e-01 7.821520211057255215e-03 -2.287159147747174978e-02 6.452596187591552734e-02 +-7.754266467287477919e-03 9.999653515775650536e-01 3.027704726465732174e-03 -1.018171310424804688e+00 +2.289448026661964827e-02 -2.849467659824345828e-03 9.997338262294502842e-01 3.535241127014160156e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000040.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000040.pose.txt new file mode 100644 index 0000000..ed2842c --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000040.pose.txt @@ -0,0 +1,4 @@ +9.997434427622041708e-01 1.101374163936315320e-02 -1.979257813112297176e-02 7.232656329870223999e-02 +-1.093846190501656171e-02 9.999325377679926241e-01 3.907680132044705212e-03 -1.024409055709838867e+00 +1.983428105900868210e-02 -3.690177226534485828e-03 9.997964712314746372e-01 3.638313055038452148e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000041.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000041.pose.txt new file mode 100644 index 0000000..021548b --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000041.pose.txt @@ -0,0 +1,4 @@ +9.999279356183896628e-01 5.622657021395925485e-03 -1.060704001902782433e-02 7.152191549539566040e-02 +-5.595726627316837017e-03 9.999810492828815622e-01 2.566888898942383448e-03 -1.022719860076904297e+00 +1.062127174390374053e-02 -2.507349821409729593e-03 9.999404491185538424e-01 3.739703893661499023e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000042.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000042.pose.txt new file mode 100644 index 0000000..94b446f --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000042.pose.txt @@ -0,0 +1,4 @@ +9.999783264757302614e-01 -5.823665319029565357e-03 -3.071074901356145997e-03 7.494855672121047974e-02 +5.811163058523360903e-03 9.999748555998574373e-01 -4.064302148150077815e-03 -1.019419193267822266e+00 +3.094666816486197486e-03 4.046367543382110507e-03 9.999870248893225622e-01 3.892185211181640625e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000043.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000043.pose.txt new file mode 100644 index 0000000..17b3899 --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000043.pose.txt @@ -0,0 +1,4 @@ +9.999990641746410169e-01 -1.367697523104576115e-03 3.245501071840636007e-05 7.826871424913406372e-02 +1.367930642882112620e-03 9.999601390918607668e-01 -8.823207644784213446e-03 -1.013227224349975586e+00 +-2.038623779059672012e-05 8.823243784006428933e-03 9.999610742191565871e-01 3.981158971786499023e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000044.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000044.pose.txt new file mode 100644 index 0000000..b157f67 --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000044.pose.txt @@ -0,0 +1,4 @@ +9.939219454112930840e-01 1.040060994733258970e-01 -3.608181955188900086e-02 7.804459333419799805e-02 +-1.037203622023552541e-01 9.945591221172745966e-01 9.707681387861488709e-03 -1.017664432525634766e+00 +3.689516085400191409e-02 -5.906258177614343539e-03 9.993016877899769668e-01 4.071835994720458984e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000045.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000045.pose.txt new file mode 100644 index 0000000..2fdc1d5 --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000045.pose.txt @@ -0,0 +1,4 @@ +9.785032016270708288e-01 1.544142461126458699e-01 -1.367030541101212637e-01 5.203760042786598206e-02 +-1.543444818909324168e-01 9.879539015724865259e-01 1.117449227127919296e-02 -1.029380559921264648e+00 +1.367818164647302270e-01 1.016508559553112000e-02 9.905490425614694727e-01 4.155334472656250000e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000046.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000046.pose.txt new file mode 100644 index 0000000..890078a --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000046.pose.txt @@ -0,0 +1,4 @@ +9.452153695302628611e-01 1.079871037244409387e-01 -3.080692951804450130e-01 -4.348916187882423401e-02 +-1.165010955646049312e-01 9.931468219046636348e-01 -9.321205550455052141e-03 -1.003115296363830566e+00 +3.049514714442547181e-01 4.470095714717839397e-02 9.513182561551685179e-01 4.261658191680908203e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000047.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000047.pose.txt new file mode 100644 index 0000000..99f41a7 --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000047.pose.txt @@ -0,0 +1,4 @@ +9.066948097688729558e-01 1.376987564278810833e-03 -4.217850469651986400e-01 -1.240393817424774170e-01 +-1.015836819378726670e-02 9.997758984302798080e-01 -1.857311157469817425e-02 -9.635425209999084473e-01 +4.216649493304209240e-01 2.112479167174335926e-02 9.065056059854229575e-01 4.301921367645263672e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000048.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000048.pose.txt new file mode 100644 index 0000000..4151e4e --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000048.pose.txt @@ -0,0 +1,4 @@ +8.553734777261109423e-01 -9.755144520955456866e-02 -5.087434806857633607e-01 -2.673747539520263672e-01 +9.878894569784867075e-02 9.948029352991030194e-01 -2.465490069329698622e-02 -1.000057816505432129e+00 +5.085046290946091396e-01 -2.916908393858306039e-02 8.605650508424903533e-01 4.379816532135009766e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000049.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000049.pose.txt new file mode 100644 index 0000000..5a7fcbf --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000049.pose.txt @@ -0,0 +1,4 @@ +8.434016355431376288e-01 -6.267041099583016417e-02 -5.336160611795475184e-01 -3.163306713104248047e-01 +6.214089633601792978e-02 9.978868940337405347e-01 -1.898040353235351876e-02 -1.010152220726013184e+00 +5.336779835872162980e-01 -1.715127695853675346e-02 8.455138340281457277e-01 4.441175460815429688e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000050.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000050.pose.txt new file mode 100644 index 0000000..4938f2f --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000050.pose.txt @@ -0,0 +1,4 @@ +8.219043011819492373e-01 7.596630223215317113e-02 -5.645373686690583481e-01 -3.488609492778778076e-01 +-8.514453470400037971e-02 9.963173455087906971e-01 1.010718795490152602e-02 -1.003106355667114258e+00 +5.632261782877731049e-01 3.976013032535974379e-02 8.253456270725976207e-01 4.518645286560058594e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000051.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000051.pose.txt new file mode 100644 index 0000000..b1b846a --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000051.pose.txt @@ -0,0 +1,4 @@ +7.547658688330078913e-01 1.426831824143778293e-01 -6.402889915505807217e-01 -3.939827084541320801e-01 +-1.626159143306936117e-01 9.862893308566833550e-01 2.809662336772771052e-02 -1.024992704391479492e+00 +6.355191166685285609e-01 8.291480744945867176e-02 7.676200798926979818e-01 4.582563877105712891e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000052.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000052.pose.txt new file mode 100644 index 0000000..68f575d --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000052.pose.txt @@ -0,0 +1,4 @@ +6.478089765133857592e-01 7.965663028472014240e-02 -7.576267888613517076e-01 -4.997257888317108154e-01 +-1.253267102250951726e-01 9.921114307938517785e-01 -2.850367052046304250e-03 -1.012748241424560547e+00 +7.514231468705463435e-01 9.679736638906956403e-02 6.526817939909965727e-01 4.611841201782226562e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000053.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000053.pose.txt new file mode 100644 index 0000000..050b338 --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000053.pose.txt @@ -0,0 +1,4 @@ +5.371939625302799559e-01 -1.041829802510530964e-02 -8.433943951007003559e-01 -6.208071708679199219e-01 +-1.650933190204102730e-02 9.996022742378583015e-01 -2.286340522866017397e-02 -9.778800010681152344e-01 +8.432971531916637620e-01 2.620596124475806435e-02 5.368083075121637959e-01 4.600412845611572266e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000054.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000054.pose.txt new file mode 100644 index 0000000..1ae438b --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000054.pose.txt @@ -0,0 +1,4 @@ +3.940586042625802143e-01 -2.306294286781833749e-02 -9.187959061036909958e-01 -9.707878828048706055e-01 +1.806992787217798560e-02 9.996862918967927003e-01 -1.734345698915486927e-02 -1.003086924552917480e+00 +9.189076635404240800e-01 -9.768237298311655686e-03 3.943517305994261868e-01 4.656989574432373047e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000055.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000055.pose.txt new file mode 100644 index 0000000..0d2995c --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000055.pose.txt @@ -0,0 +1,4 @@ +3.930767687452925152e-01 7.962933689209503174e-02 -9.160512117665065013e-01 -1.011528611183166504e+00 +2.555666974599245939e-02 9.949121554986136706e-01 9.745080539737221992e-02 -1.021275520324707031e+00 +9.191504286591254402e-01 -6.171686598675819174e-02 3.890417946037654229e-01 4.655695438385009766e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000056.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000056.pose.txt new file mode 100644 index 0000000..caf07f7 --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000056.pose.txt @@ -0,0 +1,4 @@ +3.945314145372949555e-01 8.638267210788827533e-02 -9.148130939720431343e-01 -1.063736200332641602e+00 +1.524115438550072837e-02 9.948192600072465197e-01 1.005104329491821602e-01 -1.020944356918334961e+00 +9.187560449630784998e-01 -5.359733088630228015e-02 3.911708782177755750e-01 4.671943664550781250e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000057.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000057.pose.txt new file mode 100644 index 0000000..26a5437 --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000057.pose.txt @@ -0,0 +1,4 @@ +4.034783809012283395e-01 2.768374489948119063e-02 -9.145702851140878886e-01 -1.146941661834716797e+00 +9.697042019340369051e-03 9.993566778385057914e-01 3.452821796241672958e-02 -1.003813862800598145e+00 +9.149377921593350171e-01 -2.280001596327202104e-02 4.029503638795678411e-01 4.696274280548095703e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000058.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000058.pose.txt new file mode 100644 index 0000000..410d3d0 --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000058.pose.txt @@ -0,0 +1,4 @@ +4.099324266797771399e-01 -7.698018021556098101e-04 -9.121155699589909016e-01 -1.252198934555053711e+00 +2.114230543207376578e-04 9.999996971842185500e-01 -7.489537793598228903e-04 -9.974044561386108398e-01 +9.121158703019707215e-01 1.141781805497321645e-04 4.099324652996171325e-01 4.731480121612548828e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000059.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000059.pose.txt new file mode 100644 index 0000000..8f20bd8 --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000059.pose.txt @@ -0,0 +1,4 @@ +4.064038059090948063e-01 -2.273588124936104515e-02 -9.134106558642819396e-01 -1.371684432029724121e+00 +1.959633772774792629e-03 9.997097480711116813e-01 -2.401206877948733331e-02 -1.008401870727539062e+00 +9.136914722038721459e-01 7.968645770090584945e-03 4.063304004212722753e-01 4.770505905151367188e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000060.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000060.pose.txt new file mode 100644 index 0000000..50b9c39 --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000060.pose.txt @@ -0,0 +1,4 @@ +3.985314097350763918e-01 -6.698761035873260217e-03 -9.171302208820496515e-01 -1.536085367202758789e+00 +2.833760618786106827e-03 9.999775470549572276e-01 -6.072494273874277752e-03 -1.011939287185668945e+00 +9.171503067956355837e-01 -1.788477986588606072e-04 3.985414442164538773e-01 4.811849117279052734e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000061.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000061.pose.txt new file mode 100644 index 0000000..2b2f11c --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000061.pose.txt @@ -0,0 +1,4 @@ +3.990067903248953329e-01 -3.339600154760257854e-03 -9.169418892958436373e-01 -1.619540929794311523e+00 +8.134500944350043714e-03 9.999669091709086910e-01 -1.022622404069366449e-04 -1.014479517936706543e+00 +9.169118884434923622e-01 -7.418061336074950954e-03 3.990207528400031012e-01 4.829958915710449219e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000062.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000062.pose.txt new file mode 100644 index 0000000..bdccaa9 --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000062.pose.txt @@ -0,0 +1,4 @@ +4.055091647622474649e-01 -1.606785846466235110e-03 -9.140895664720541935e-01 -1.707392334938049316e+00 +1.042535423408640777e-02 9.999415438455983018e-01 2.867207836082835866e-03 -1.020306468009948730e+00 +9.140315253222496272e-01 -1.069238658696336150e-02 4.055022115675285810e-01 4.848496437072753906e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000063.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000063.pose.txt new file mode 100644 index 0000000..9a79f52 --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000063.pose.txt @@ -0,0 +1,4 @@ +4.097820193322233018e-01 -2.412971909752960331e-03 -9.121802860172805172e-01 -1.789994120597839355e+00 +1.403238302507445884e-02 9.998948471164990082e-01 3.658816271760728135e-03 -1.009362459182739258e+00 +9.120755390090464765e-01 -1.429938028152416551e-02 4.097727893173501168e-01 4.867924690246582031e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000064.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000064.pose.txt new file mode 100644 index 0000000..c230afb --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000064.pose.txt @@ -0,0 +1,4 @@ +4.175215153796766332e-01 6.020775387901536324e-03 -9.086470901614041029e-01 -1.892759680747985840e+00 +6.414718281564949984e-03 9.999336004302334935e-01 9.573198002663145523e-03 -1.000824689865112305e+00 +9.086443944604658540e-01 -9.825731237850764133e-03 4.174551705527371759e-01 4.895465850830078125e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000065.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000065.pose.txt new file mode 100644 index 0000000..c19f9c4 --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000065.pose.txt @@ -0,0 +1,4 @@ +4.248551341164387152e-01 -2.664152213236660460e-03 -9.052574315121020287e-01 -1.973486185073852539e+00 +1.262765018598234132e-02 9.999158165233439366e-01 2.983675122322243349e-03 -1.002540111541748047e+00 +9.051732748295675535e-01 -1.269890386764978632e-02 4.248530103163644656e-01 4.921638488769531250e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000066.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000066.pose.txt new file mode 100644 index 0000000..29447fd --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000066.pose.txt @@ -0,0 +1,4 @@ +4.255269589167116617e-01 1.755041316656018400e-03 -9.049440463725214601e-01 -2.103090047836303711e+00 +8.697083256235747448e-03 9.999440051908194960e-01 6.028866044031584714e-03 -1.003678202629089355e+00 +9.049039551123255753e-01 -1.043581874696966616e-02 4.254878678753968302e-01 4.972989082336425781e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000067.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000067.pose.txt new file mode 100644 index 0000000..b88442a --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000067.pose.txt @@ -0,0 +1,4 @@ +4.180487957770303575e-01 2.658545976911063334e-03 -9.084206825489300474e-01 -2.180528879165649414e+00 +-1.007466583150977207e-03 9.999964595147801116e-01 2.462918794647709752e-03 -1.005797266960144043e+00 +9.084240140817827491e-01 -1.144167550878412085e-04 4.180499940776791767e-01 5.006124973297119141e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000068.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000068.pose.txt new file mode 100644 index 0000000..125bfb2 --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000068.pose.txt @@ -0,0 +1,4 @@ +4.166970858238667197e-01 6.874147209180384542e-03 -9.090194083549834048e-01 -2.265179157257080078e+00 +-7.433786123337760531e-03 9.999637400085422678e-01 4.154213764546904435e-03 -1.011166691780090332e+00 +9.090150039959572492e-01 5.026407094097886698e-03 4.167330773312274972e-01 5.048423290252685547e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000069.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000069.pose.txt new file mode 100644 index 0000000..6fef5f6 --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000069.pose.txt @@ -0,0 +1,4 @@ +4.209717287750189274e-01 2.295709067706027794e-03 -9.070708535125843186e-01 -2.353878259658813477e+00 +9.764268664072664255e-04 9.999950710023930434e-01 2.984051188122335569e-03 -1.005983233451843262e+00 +9.070732330758891626e-01 -2.141889538521666182e-03 4.209674122143640118e-01 5.079937934875488281e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000070.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000070.pose.txt new file mode 100644 index 0000000..23487fb --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000070.pose.txt @@ -0,0 +1,4 @@ +4.215337146767577092e-01 -4.770412535525603379e-04 -9.068126045785072087e-01 -2.429828166961669922e+00 +2.633268363514648311e-03 9.999962893271130415e-01 6.980184311606598118e-04 -1.006750583648681641e+00 +9.068089067099744938e-01 -2.682119245472897387e-03 4.215334066806728508e-01 5.112838745117187500e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000071.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000071.pose.txt new file mode 100644 index 0000000..45e2f5f --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000071.pose.txt @@ -0,0 +1,4 @@ +4.223223583182480478e-01 -9.469123378880151508e-05 -9.064457053227641392e-01 -2.522770643234252930e+00 +-1.524762656751428016e-03 9.999985055444047255e-01 -8.148667361039657869e-04 -9.997978210449218750e-01 +9.064444278406447175e-01 1.726251003555380427e-03 4.223215827945794310e-01 5.154627323150634766e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000072.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000072.pose.txt new file mode 100644 index 0000000..6191750 --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000072.pose.txt @@ -0,0 +1,4 @@ +4.309755521628136909e-01 -3.439916087665978537e-03 -9.023570470801828280e-01 -2.664690494537353516e+00 +1.036386635731869829e-02 9.999456461863331924e-01 1.137957435032775633e-03 -9.963304996490478516e-01 +9.023040860552972164e-01 -9.842339676424782802e-03 4.309877778285694516e-01 5.208714485168457031e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000073.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000073.pose.txt new file mode 100644 index 0000000..f2c2972 --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000073.pose.txt @@ -0,0 +1,4 @@ +4.305278934050991713e-01 -2.089819975800788834e-04 -9.025772483985468053e-01 -2.736328840255737305e+00 +6.833385550019455308e-03 9.999720676958295806e-01 3.027981184206155439e-03 -9.997515082359313965e-01 +9.025514045427507348e-01 -7.471288687489504969e-03 4.305172958235892744e-01 5.241302490234375000e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000074.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000074.pose.txt new file mode 100644 index 0000000..e1e2366 --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000074.pose.txt @@ -0,0 +1,4 @@ +4.298087768538886855e-01 2.140676747116990446e-03 -9.029174008969083065e-01 -2.818843841552734375e+00 +4.047377022017431070e-04 9.999966323202039709e-01 2.563500661781375166e-03 -1.004641294479370117e+00 +9.029198477864779360e-01 -1.467259798021372129e-03 4.298064629829670413e-01 5.280450344085693359e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000075.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000075.pose.txt new file mode 100644 index 0000000..7f250e8 --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000075.pose.txt @@ -0,0 +1,4 @@ +4.318247922527075344e-01 5.951730005318475389e-03 -9.019378724201848607e-01 -2.874640464782714844e+00 +-2.954618274041734619e-03 9.999821974501907906e-01 5.184111644439512098e-03 -1.007314443588256836e+00 +9.019526700591115054e-01 4.262541860280666915e-04 4.318346897605706003e-01 5.313178062438964844e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000076.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000076.pose.txt new file mode 100644 index 0000000..cf4f37f --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000076.pose.txt @@ -0,0 +1,4 @@ +4.296222301939966459e-01 2.277820019860517155e-03 -9.030058420957718379e-01 -2.942108392715454102e+00 +-1.121123997821456015e-03 9.999973933099157719e-01 1.989083798010573851e-03 -1.010257840156555176e+00 +9.030080190142932839e-01 1.578269024024945427e-04 4.296236640206761437e-01 5.352435111999511719e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000077.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000077.pose.txt new file mode 100644 index 0000000..56ad7b6 --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000077.pose.txt @@ -0,0 +1,4 @@ +4.284522513730398829e-01 3.276551928702437378e-03 -9.035584831657725857e-01 -3.022695779800415039e+00 +-7.671003720276570181e-03 9.999705773543572374e-01 -1.129234084727270132e-05 -1.007953882217407227e+00 +9.035318610847637988e-01 6.936038714711382364e-03 4.284647796163076894e-01 5.399482727050781250e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000078.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000078.pose.txt new file mode 100644 index 0000000..945e2ba --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000078.pose.txt @@ -0,0 +1,4 @@ +4.307432704760594255e-01 6.045195849203887500e-03 -9.024542595316020765e-01 -3.199549198150634766e+00 +-4.464796625368041248e-04 9.999788700910932082e-01 6.485370247752893572e-03 -1.001620888710021973e+00 +9.024743960886083549e-01 -2.390602117514642466e-03 4.307368679669949518e-01 5.485281944274902344e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000079.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000079.pose.txt new file mode 100644 index 0000000..e6372b0 --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000079.pose.txt @@ -0,0 +1,4 @@ +4.321286688069273518e-01 -4.234194136784047782e-03 -9.018020210640274037e-01 -3.279738426208496094e+00 +7.918103140517211702e-03 9.999682455198644071e-01 -9.008854422439867976e-04 -9.974331855773925781e-01 +9.017771993335208514e-01 -6.751262988207410928e-03 4.321484735715898085e-01 5.528159141540527344e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000080.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000080.pose.txt new file mode 100644 index 0000000..757a194 --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000080.pose.txt @@ -0,0 +1,4 @@ +4.333342481405469959e-01 1.028189415376936288e-03 -9.012326959309636676e-01 -3.359243154525756836e+00 +-1.194725448244552150e-03 9.999991259013479850e-01 5.664164927585211944e-04 -9.909830093383789062e-01 +9.012324905481216009e-01 8.312779715948476027e-04 4.333350977677250526e-01 5.575683593750000000e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000081.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000081.pose.txt new file mode 100644 index 0000000..ca735ed --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000081.pose.txt @@ -0,0 +1,4 @@ +4.330660501850991384e-01 1.032027618082353819e-02 -9.013031055514170164e-01 -3.444309711456298828e+00 +-1.372626222374601253e-02 9.998940092399084545e-01 4.853865625135789230e-03 -9.939182996749877930e-01 +9.012576689839827404e-01 1.026947835546859582e-02 4.331618080023496020e-01 5.635225772857666016e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000082.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000082.pose.txt new file mode 100644 index 0000000..3c3e835 --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000082.pose.txt @@ -0,0 +1,4 @@ +4.367913841997465019e-01 7.312133918390341381e-03 -8.995331118899567713e-01 -3.519419193267822266e+00 +-8.631042594651382183e-03 9.999549999005133971e-01 3.937420182554761741e-03 -9.923428297042846680e-01 +8.995214237540978264e-01 6.044077392307410646e-03 4.368348398833099555e-01 5.673442840576171875e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000083.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000083.pose.txt new file mode 100644 index 0000000..671b05e --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000083.pose.txt @@ -0,0 +1,4 @@ +4.382546522581045600e-01 -1.037527954300726226e-03 -8.988502563329840989e-01 -3.604137420654296875e+00 +4.809835266115190799e-03 9.999877235764732530e-01 1.190874071915934982e-03 -9.943543076515197754e-01 +8.988379861014108574e-01 -4.845227764137778788e-03 4.382542623969252205e-01 5.710685729980468750e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000084.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000084.pose.txt new file mode 100644 index 0000000..02ae960 --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000084.pose.txt @@ -0,0 +1,4 @@ +4.333074288272629748e-01 1.962688777366686406e-03 -9.012440401888210939e-01 -3.746492862701416016e+00 +-9.111194828203158375e-03 9.999560657264265906e-01 -2.202894851960419693e-03 -1.009130716323852539e+00 +9.012001210895994596e-01 9.165940742197091137e-03 4.333062742199811912e-01 5.796766281127929688e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000085.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000085.pose.txt new file mode 100644 index 0000000..6adbfde --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000085.pose.txt @@ -0,0 +1,4 @@ +4.316766648385159111e-01 7.441245368227824078e-03 -9.019977189002561513e-01 -3.814902782440185547e+00 +-6.532173884538477679e-03 9.999655406740541874e-01 5.123296671902595467e-03 -1.002568125724792480e+00 +9.020047603744880682e-01 3.680398323008350625e-03 4.317103970602818586e-01 5.833820343017578125e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000086.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000086.pose.txt new file mode 100644 index 0000000..fff8c6b --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000086.pose.txt @@ -0,0 +1,4 @@ +4.349203131297774694e-01 7.682633088212165347e-03 -9.004361711836816884e-01 -3.890194892883300781e+00 +-8.469015480386640066e-03 9.999542751196608403e-01 4.441108724948230821e-03 -9.980048537254333496e-01 +9.004291182563397555e-01 5.694279475556802278e-03 4.349654907890567190e-01 5.867496013641357422e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000087.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000087.pose.txt new file mode 100644 index 0000000..95e4c8e --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000087.pose.txt @@ -0,0 +1,4 @@ +4.383049601922210381e-01 3.337547946426305118e-03 -8.988201280815873995e-01 -3.947441577911376953e+00 +1.134323628286870245e-03 9.999902556706059542e-01 4.266365401925042121e-03 -9.952256083488464355e-01 +8.988256088812790479e-01 -2.889522026518999941e-03 4.382969033448176899e-01 5.894556999206542969e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000088.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000088.pose.txt new file mode 100644 index 0000000..0c27349 --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000088.pose.txt @@ -0,0 +1,4 @@ +4.380005944790599792e-01 1.712677631534345341e-03 -8.989730507480856447e-01 -4.013761043548583984e+00 +7.108700962210383334e-03 9.999603214758785441e-01 5.368598000150808652e-03 -9.932868480682373047e-01 +8.989465755019147020e-01 -8.741979706439266876e-03 4.379710403487795944e-01 5.923881053924560547e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000089.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000089.pose.txt new file mode 100644 index 0000000..314ad67 --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000089.pose.txt @@ -0,0 +1,4 @@ +4.337279595028018520e-01 1.892275161959062601e-03 -9.010418838435021449e-01 -4.111992835998535156e+00 +-1.913799676863626661e-03 9.999974738271268926e-01 1.178859686822490791e-03 -9.988889098167419434e-01 +9.010418383828426858e-01 1.213109259634657065e-03 4.337304852665897492e-01 5.978406429290771484e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-poses/crazyflie_frame-000090.pose.txt b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000090.pose.txt new file mode 100644 index 0000000..fd92873 --- /dev/null +++ b/data/demo_hallway/crazyflie-poses/crazyflie_frame-000090.pose.txt @@ -0,0 +1,4 @@ +4.333489309029525116e-01 2.067286334707929985e-03 -9.012238514445110038e-01 -4.177628993988037109e+00 +-8.549019479731495563e-03 9.999618057047559017e-01 -1.816974853719429198e-03 -1.005701780319213867e+00 +9.011856736273624602e-01 8.491964371934631814e-03 4.333500527170875416e-01 6.014142513275146484e+00 +0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00 diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000000.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000000.rgb.jpg new file mode 100644 index 0000000..1599357 Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000000.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000001.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000001.rgb.jpg new file mode 100644 index 0000000..9ca683b Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000001.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000002.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000002.rgb.jpg new file mode 100644 index 0000000..13d1891 Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000002.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000003.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000003.rgb.jpg new file mode 100644 index 0000000..152c5d9 Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000003.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000004.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000004.rgb.jpg new file mode 100644 index 0000000..de7a6c3 Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000004.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000005.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000005.rgb.jpg new file mode 100644 index 0000000..fd6dea1 Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000005.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000006.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000006.rgb.jpg new file mode 100644 index 0000000..c934ac0 Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000006.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000007.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000007.rgb.jpg new file mode 100644 index 0000000..d6d07a7 Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000007.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000008.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000008.rgb.jpg new file mode 100644 index 0000000..3bdf5d5 Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000008.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000009.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000009.rgb.jpg new file mode 100644 index 0000000..b96a2ab Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000009.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000010.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000010.rgb.jpg new file mode 100644 index 0000000..2a24f9f Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000010.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000011.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000011.rgb.jpg new file mode 100644 index 0000000..d4ef0be Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000011.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000012.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000012.rgb.jpg new file mode 100644 index 0000000..8f2343f Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000012.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000013.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000013.rgb.jpg new file mode 100644 index 0000000..673ef4c Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000013.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000014.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000014.rgb.jpg new file mode 100644 index 0000000..33d05bd Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000014.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000015.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000015.rgb.jpg new file mode 100644 index 0000000..c5e9b89 Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000015.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000016.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000016.rgb.jpg new file mode 100644 index 0000000..0c19752 Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000016.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000017.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000017.rgb.jpg new file mode 100644 index 0000000..2a65afa Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000017.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000018.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000018.rgb.jpg new file mode 100644 index 0000000..24fdf54 Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000018.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000019.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000019.rgb.jpg new file mode 100644 index 0000000..f914b75 Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000019.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000020.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000020.rgb.jpg new file mode 100644 index 0000000..f9c714a Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000020.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000021.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000021.rgb.jpg new file mode 100644 index 0000000..8e8f354 Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000021.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000022.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000022.rgb.jpg new file mode 100644 index 0000000..1a1eb9d Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000022.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000023.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000023.rgb.jpg new file mode 100644 index 0000000..e5273b0 Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000023.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000024.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000024.rgb.jpg new file mode 100644 index 0000000..4ae224c Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000024.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000025.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000025.rgb.jpg new file mode 100644 index 0000000..402164b Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000025.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000026.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000026.rgb.jpg new file mode 100644 index 0000000..31ca33f Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000026.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000027.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000027.rgb.jpg new file mode 100644 index 0000000..8228690 Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000027.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000028.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000028.rgb.jpg new file mode 100644 index 0000000..143d192 Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000028.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000029.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000029.rgb.jpg new file mode 100644 index 0000000..f343b73 Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000029.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000030.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000030.rgb.jpg new file mode 100644 index 0000000..b98bd74 Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000030.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000031.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000031.rgb.jpg new file mode 100644 index 0000000..3dccc40 Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000031.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000032.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000032.rgb.jpg new file mode 100644 index 0000000..c41fcf2 Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000032.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000033.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000033.rgb.jpg new file mode 100644 index 0000000..a54f298 Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000033.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000034.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000034.rgb.jpg new file mode 100644 index 0000000..f349b39 Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000034.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000035.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000035.rgb.jpg new file mode 100644 index 0000000..51175f9 Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000035.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000036.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000036.rgb.jpg new file mode 100644 index 0000000..aeab10e Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000036.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000037.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000037.rgb.jpg new file mode 100644 index 0000000..6c8a343 Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000037.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000038.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000038.rgb.jpg new file mode 100644 index 0000000..2551ea2 Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000038.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000039.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000039.rgb.jpg new file mode 100644 index 0000000..f63fd2c Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000039.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000040.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000040.rgb.jpg new file mode 100644 index 0000000..387c70d Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000040.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000041.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000041.rgb.jpg new file mode 100644 index 0000000..e4b1f67 Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000041.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000042.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000042.rgb.jpg new file mode 100644 index 0000000..f65a71a Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000042.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000043.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000043.rgb.jpg new file mode 100644 index 0000000..3c42785 Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000043.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000044.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000044.rgb.jpg new file mode 100644 index 0000000..7b7bdcd Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000044.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000045.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000045.rgb.jpg new file mode 100644 index 0000000..7b115b3 Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000045.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000046.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000046.rgb.jpg new file mode 100644 index 0000000..87135a6 Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000046.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000047.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000047.rgb.jpg new file mode 100644 index 0000000..5bcf25a Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000047.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000048.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000048.rgb.jpg new file mode 100644 index 0000000..91b339d Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000048.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000049.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000049.rgb.jpg new file mode 100644 index 0000000..a87069a Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000049.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000050.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000050.rgb.jpg new file mode 100644 index 0000000..1227f06 Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000050.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000051.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000051.rgb.jpg new file mode 100644 index 0000000..a897ed6 Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000051.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000052.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000052.rgb.jpg new file mode 100644 index 0000000..831ea19 Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000052.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000053.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000053.rgb.jpg new file mode 100644 index 0000000..e882186 Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000053.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000054.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000054.rgb.jpg new file mode 100644 index 0000000..e5eee09 Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000054.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000055.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000055.rgb.jpg new file mode 100644 index 0000000..06005a0 Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000055.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000056.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000056.rgb.jpg new file mode 100644 index 0000000..6a28978 Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000056.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000057.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000057.rgb.jpg new file mode 100644 index 0000000..9db7892 Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000057.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000058.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000058.rgb.jpg new file mode 100644 index 0000000..39d4ffa Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000058.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000059.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000059.rgb.jpg new file mode 100644 index 0000000..26e0c2f Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000059.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000060.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000060.rgb.jpg new file mode 100644 index 0000000..0ae162b Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000060.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000061.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000061.rgb.jpg new file mode 100644 index 0000000..3ef5af5 Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000061.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000062.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000062.rgb.jpg new file mode 100644 index 0000000..d167c66 Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000062.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000063.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000063.rgb.jpg new file mode 100644 index 0000000..625a362 Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000063.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000064.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000064.rgb.jpg new file mode 100644 index 0000000..e177ec3 Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000064.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000065.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000065.rgb.jpg new file mode 100644 index 0000000..5b5e317 Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000065.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000066.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000066.rgb.jpg new file mode 100644 index 0000000..a1845a0 Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000066.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000067.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000067.rgb.jpg new file mode 100644 index 0000000..826539f Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000067.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000068.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000068.rgb.jpg new file mode 100644 index 0000000..d7f5bc7 Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000068.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000069.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000069.rgb.jpg new file mode 100644 index 0000000..3d193f6 Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000069.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000070.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000070.rgb.jpg new file mode 100644 index 0000000..084828d Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000070.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000071.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000071.rgb.jpg new file mode 100644 index 0000000..9c48386 Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000071.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000072.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000072.rgb.jpg new file mode 100644 index 0000000..a83ff9c Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000072.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000073.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000073.rgb.jpg new file mode 100644 index 0000000..ec7bb86 Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000073.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000074.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000074.rgb.jpg new file mode 100644 index 0000000..7c11d7f Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000074.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000075.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000075.rgb.jpg new file mode 100644 index 0000000..458e5e5 Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000075.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000076.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000076.rgb.jpg new file mode 100644 index 0000000..becbe44 Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000076.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000077.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000077.rgb.jpg new file mode 100644 index 0000000..0f98591 Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000077.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000078.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000078.rgb.jpg new file mode 100644 index 0000000..9d5775e Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000078.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000079.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000079.rgb.jpg new file mode 100644 index 0000000..85ba70a Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000079.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000080.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000080.rgb.jpg new file mode 100644 index 0000000..4ca7b1d Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000080.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000081.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000081.rgb.jpg new file mode 100644 index 0000000..25b57ee Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000081.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000082.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000082.rgb.jpg new file mode 100644 index 0000000..e1366c9 Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000082.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000083.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000083.rgb.jpg new file mode 100644 index 0000000..a5db066 Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000083.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000084.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000084.rgb.jpg new file mode 100644 index 0000000..2d4ff41 Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000084.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000085.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000085.rgb.jpg new file mode 100644 index 0000000..82e4d7a Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000085.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000086.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000086.rgb.jpg new file mode 100644 index 0000000..d6ef445 Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000086.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000087.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000087.rgb.jpg new file mode 100644 index 0000000..18f3b4c Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000087.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000088.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000088.rgb.jpg new file mode 100644 index 0000000..41e7d8f Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000088.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000089.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000089.rgb.jpg new file mode 100644 index 0000000..cf9c97a Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000089.rgb.jpg differ diff --git a/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000090.rgb.jpg b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000090.rgb.jpg new file mode 100644 index 0000000..7764752 Binary files /dev/null and b/data/demo_hallway/crazyflie-rgb-images/crazyflie_frame-000090.rgb.jpg differ diff --git a/environment.yml b/environment.yml new file mode 100644 index 0000000..175b578 --- /dev/null +++ b/environment.yml @@ -0,0 +1,34 @@ +name: test2_mononav +channels: + - pytorch + - nvidia + - conda-forge +dependencies: + - cuda=11.7.1 + - h5py=3.7.0 + - matplotlib=3.6.2 + - matplotlib-base=3.6.2 + - numpy=1.24.1 + - opencv=4.6.0 + - pip=22.3.1 + - python=3.9.7 + - pytorch=1.13.1 + - pytorch-cuda=11.7 + - pytorch-mutex=1.0 + - scipy=1.10.0 + - torchaudio=0.13.1 + - torchvision=0.14.1 + - ffmpeg=5.1.2 + - pip: + - huggingface-hub==0.11.1 + - timm==0.6.12 + - tqdm==4.64.1 + - wandb==0.13.9 + - cflib==0.1.23 # crazyflie + - cfclient==2023.6 # crazyflie + - open3d==0.18.0 # mononav + - Pillow==9.4.0 # mononav + - pynput==1.7.6 # mononav + - PyYAML==5.4.1 # mononav + - trimesh==3.22.1 # mononav + diff --git a/estimate_depth.py b/estimate_depth.py new file mode 100644 index 0000000..be8c945 --- /dev/null +++ b/estimate_depth.py @@ -0,0 +1,92 @@ +""" + __ __ _ _ + | \/ | ___ _ __ ___ | \ | | __ ___ __ + | |\/| |/ _ \| '_ \ / _ \| \| |/ _` \ \ / / + | | | | (_) | | | | (_) | |\ | (_| |\ V / + |_| |_|\___/|_| |_|\___/|_| \_|\__,_| \_/ +Copyright (c) 2023 Nate Simon +License: MIT +Authors: Nate Simon and Anirudha Majumdar, Princeton University +Project Page: https://natesimon.github.io/mononav + +This script reads in a stream of RGB images, transforms them to the Kinect intrinsics, +and estimates metric depth using ZoeDepth. + +The following are saved to file: +│ ├── # images transformed to match kinect intrinsics +│ ├── # estimated depth (.npy for fusion and .jpg for visualization) + +""" + +import time +import os +import sys +import torch +import cv2 + +# Add ZoeDepth to path +sys.path.insert(0, "ZoeDepth") +from zoedepth.models.builder import build_model +from zoedepth.utils.config import get_config + +import open3d as o3d +from PIL import Image +import numpy as np + +from utils.utils import compute_depth, load_config, get_calibration_values, transform_image + +"""" +This script runs a depth estimation model on a directory of RGB images and saves the depth images. +""" + +# LOAD CONFIG +CONFIG_PATH = "config.yml" +config = load_config("config.yml") +data_dir = config["data_dir"] # parent directory to look for RGB images, and save depth images +camera_source = config["camera_source"] # what camera was used for the RGB images? +print("Loading" + camera_source + "images from: ", data_dir, ".") + +# Set & create directories for images +rgb_dir = os.path.join(data_dir, camera_source + "-rgb-images") +kinect_img_dir = os.path.join(data_dir, "kinect-rgb-images") +os.mkdir(kinect_img_dir) if not os.path.exists(kinect_img_dir) else None +kinect_depth_dir = os.path.join(data_dir, "kinect-depth-images") +os.mkdir(kinect_depth_dir) if not os.path.exists(kinect_depth_dir) else None +print("Saving Depth images to: ", kinect_depth_dir) + +# Load the calibration values +camera_calibration_path = config["camera_calibration_path"] +mtx, dist = get_calibration_values(camera_calibration_path) +# Kinect intrinsic matrix +kinect = o3d.camera.PinholeCameraIntrinsic(o3d.camera.PinholeCameraIntrinsicParameters.PrimeSenseDefault) + +# Load the ZoeDepth model +conf = get_config("zoedepth", config["zoedepth_mode"]) # NOTE: "eval" runs slightly slower, but is stated to be more metrically accurate +model_zoe = build_model(conf) +DEVICE = "cuda" if torch.cuda.is_available() else "cpu" +print("Device is: ", DEVICE) +zoe = model_zoe.to(DEVICE) + +# Figure out how many images are in folder by counting .jpg files +end_frame = len([name for name in os.listdir(rgb_dir) if os.path.isfile(os.path.join(rgb_dir, name)) and name.endswith(".jpg")]) +end_frame = end_frame - 1 # Ignore last image since sometimes pose information is not saved for it + +start_time = time.time() + +for frame_number in range(0, end_frame): + print("Applying ZoeDepth to: %d/%d"%(frame_number+1,end_frame)) + filename = rgb_dir + "/" + camera_source + "_frame-%06d.rgb.jpg"%(frame_number) + # Read in image with Pillow and convert to RGB + crazyflie_rgb = Image.open(filename)#.convert("RGB") # load + # Resize, Undistort, and Warp image to kinect's dimensions and intrinsics + kinect_rgb = transform_image(np.asarray(crazyflie_rgb), mtx, dist, kinect) + kinect_rgb = cv2.cvtColor(kinect_rgb, cv2.COLOR_BGR2RGB) + # Compute depth + depth_numpy, depth_colormap = compute_depth(kinect_rgb, zoe) + # Save images + cv2.imwrite(kinect_img_dir + "/kinect_frame-%06d.rgb.jpg"%(frame_number), kinect_rgb) + cv2.imwrite(kinect_depth_dir + "/" + "kinect_frame-%06d.depth.jpg"%(frame_number), depth_colormap) + np.save(kinect_depth_dir + "/" + "kinect_frame-%06d.depth.npy"%(frame_number), depth_numpy) # saved in meters + +print("Time to compute depth for %d images: %f"%(end_frame, time.time()-start_time)) +# On Nvidia GeForce RTX 4090: 13.6 s for 80 images \ No newline at end of file diff --git a/fuse_depth.py b/fuse_depth.py new file mode 100644 index 0000000..ec76c72 --- /dev/null +++ b/fuse_depth.py @@ -0,0 +1,118 @@ +""" + __ __ _ _ + | \/ | ___ _ __ ___ | \ | | __ ___ __ + | |\/| |/ _ \| '_ \ / _ \| \| |/ _` \ \ / / + | | | | (_) | | | | (_) | |\ | (_| |\ V / + |_| |_|\___/|_| |_|\___/|_| \_|\__,_| \_/ +Copyright (c) 2023 Nate Simon +License: MIT +Authors: Nate Simon and Anirudha Majumdar, Princeton University +Project Page: https://natesimon.github.io/mononav + +The purpose of this script is to fuse depth images and poses into a 3D reconstruction. +Here, we use Open3D's tensor reconstruction system: the VoxelBlockGrid. + +After fusion, the reconstruction is visualized (in addition to the camera poses), and saved to file. + +""" + +import numpy as np +import time +import os + +import open3d as o3d +from PIL import Image +import numpy as np +import yaml +from utils.utils import * +##################################################################### + +addPose = True + +CONFIG_PATH = "config.yml" +with open(CONFIG_PATH, "r") as f: + config = yaml.safe_load(f) + +data_dir = config["data_dir"] # parent directory to look for RGB images, and save depth images + +source = "kinect" # meaning: crazyflie images have been undistorted to match kinect +rgb_dir = data_dir + "/" + source + "-rgb-images/" +depth_dir = data_dir + "/" + source + "-depth-images" +pose_dir = data_dir + "/crazyflie-poses/" +##################################################################### + +# Initialize TSDF VoxelBlockGrid +depth_scale = config["VoxelBlockGrid"]["depth_scale"] +depth_max = config["VoxelBlockGrid"]["depth_max"] +trunc_voxel_multiplier = config["VoxelBlockGrid"]["trunc_voxel_multiplier"] +weight_threshold = config["weight_threshold"] # for planning and visualization (!! important !!) +device = config["VoxelBlockGrid"]["device"] + +vbg = VoxelBlockGrid(depth_scale, depth_max, trunc_voxel_multiplier, o3d.core.Device(device)) + +##################################################################### + +poses = [] # for visualization +t_start = time.time() + +depth_files = [name for name in os.listdir(depth_dir) if os.path.isfile(os.path.join(depth_dir, name)) and name.endswith(".jpg")] +depth_files = sorted(depth_files) + +# Get last frame +first_frame = split_filename(depth_files[0]) +end_frame = split_filename(depth_files[-1]) + +for filename in depth_files: + # Get the frame number from the depth filename + frame_number = split_filename(filename) + print("Integrating frame %d/%d"%(frame_number,end_frame)) + # Get rbg_file + rgb_file = rgb_dir + source + "_frame-%06d.rgb.jpg"%(frame_number) + + # Read in camera pose + pose_file = data_dir + "/crazyflie-poses/crazyflie_frame-%06d.pose.txt"%(frame_number) + cam_pose = np.loadtxt(pose_file) + poses.append(cam_pose) + + # Get color image with Pillow and convert to RGB + color = Image.open(rgb_file).convert("RGB") # load + + # Integrate + depth_file = depth_dir + "/" + source + "_frame-%06d.depth.npy"%(frame_number) + depth_numpy = np.load(depth_file) # mm + vbg.integration_step(color, depth_numpy, cam_pose) + +##################################################################### +# Print out timing information +t_end = time.time() +print("Time taken (s): ", t_end - t_start) +print("FPS: ", end_frame/(t_end - t_start)) + +pcd = vbg.vbg.extract_point_cloud(weight_threshold) + +if addPose: + pose_lineset = get_poses_lineset(poses) + visualizer = o3d.visualization.Visualizer() + visualizer.create_window() + visualizer.add_geometry(pcd.to_legacy()) + visualizer.add_geometry(pose_lineset) + for pose in poses: + # Add coordinate frame ( The x, y, z axis will be rendered as red, green, and blue arrows respectively.) + coordinate_frame = o3d.geometry.TriangleMesh.create_coordinate_frame().scale(0.5, center=(0, 0, 0)) + visualizer.add_geometry(coordinate_frame.transform(pose)) + visualizer.run() + visualizer.destroy_window() +else: + o3d.visualization.draw([pcd]) + +##################################################################### + +npz_filename = os.path.join(data_dir, "vbg.npz") +ply_filename = os.path.join(data_dir, "pointcloud.ply") +print('Saving npz to {}...'.format(npz_filename)) +print('Saving ply to {}...'.format(ply_filename)) + +vbg.vbg.save(npz_filename) +o3d.io.write_point_cloud(ply_filename, pcd.to_legacy()) + +print('Saving finished') \ No newline at end of file diff --git a/mononav_cf.py b/mononav_cf.py new file mode 100644 index 0000000..f82d587 --- /dev/null +++ b/mononav_cf.py @@ -0,0 +1,311 @@ +""" + __ __ _ _ + | \/ | ___ _ __ ___ | \ | | __ ___ __ + | |\/| |/ _ \| '_ \ / _ \| \| |/ _` \ \ / / + | | | | (_) | | | | (_) | |\ | (_| |\ V / + |_| |_|\___/|_| |_|\___/|_| \_|\__,_| \_/ +Copyright (c) 2023 Nate Simon +License: MIT +Authors: Nate Simon and Anirudha Majumdar, Princeton University +Project Page: https://natesimon.github.io/mononav + +This script runs the MonoNav navigation pipeline on a Crazyflie micro aerial vehicle (MAV). +Essentially, the collect_dataset, estimate_depth, fuse_depth, and simulate scripts are combined +into a single script that: +- Collects synchronized images and poses from the Crazyflie, +- Estimates depth using ZoeDepth, +- Fuses depth images and poses into a 3D reconstruction, +- Chooses a motion primitive according to the planner, +- Executes the motion primitive while collecting & fusing new images, +- Repeats until the goal is reached or no primitive satisfies the obstacle avoidance constraint. + +This script is designed for the Crazyflie platform, and may require modification for your specific hardware! + +""" + +import cv2 +import torch +import numpy as np +import time +import os +import open3d as o3d +import sys +# Add path to ZoeDepth +sys.path.insert(0, "ZoeDepth") +from zoedepth.models.builder import build_model +from zoedepth.utils.config import get_config + +# For Craziflie logging +import logging +import time + +import cflib.crtp +from cflib.crazyflie import Crazyflie +from cflib.crazyflie.log import LogConfig +from cflib.crazyflie.syncCrazyflie import SyncCrazyflie +from cflib.utils import uri_helper + +# Keyboard control +from pynput import keyboard + +# helper functions +from utils.utils import * + +# GLOBAL VARIABLES +last_key_pressed = None # store the last key pressed +shouldStop = False + +# LOAD VALUES FROM CONFIG FILE +CONFIG_PATH = "config.yml" +config = load_config("config.yml") + +URI = uri_helper.uri_from_env(default=config["radio_uri"]) +logging.basicConfig(level=logging.ERROR) +height = config["height"] +FLY_CRAZYFLIE = config["FLY_CRAZYFLIE"] + +# Camera Settings for Undistortion +camera_num = config["camera_num"] +# Intrinsics for undistortion +camera_calibration_path = config["camera_calibration_path"] +mtx, dist = get_calibration_values(camera_calibration_path) # for the robot's camera +kinect = o3d.camera.PinholeCameraIntrinsic(o3d.camera.PinholeCameraIntrinsicParameters.PrimeSenseDefault) # for the kinect + +# Initialize Zoedepth Model & Move to device +conf = get_config("zoedepth", config["zoedepth_mode"]) # NOTE: "eval" runs slightly slower, but is stated to be more metrically accurate +model_zoe = build_model(conf) +DEVICE = "cuda" if torch.cuda.is_available() else "cpu" +print("Device is: ", DEVICE) +zoe = model_zoe.to(DEVICE) + +# Initialize VoxelBlockGrid +depth_scale = config["VoxelBlockGrid"]["depth_scale"] +depth_max = config["VoxelBlockGrid"]["depth_max"] +trunc_voxel_multiplier = config["VoxelBlockGrid"]["trunc_voxel_multiplier"] +weight_threshold = config["weight_threshold"] # for planning and visualization (!! important !!) +device = config["VoxelBlockGrid"]["device"] +vbg = VoxelBlockGrid(depth_scale, depth_max, trunc_voxel_multiplier, o3d.core.Device(device)) + +# Initialize Trajectory Library (Motion Primitives) +trajlib_dir = config["trajlib_dir"] +traj_list = get_trajlist(trajlib_dir) +traj_linesets, period, forward_speed, amplitudes = get_traj_linesets(traj_list) +max_traj_idx = int(len(traj_list)/2) # set initial value to that of FORWARD flight (should be median value) +print("Initial trajectory chosen: %d out of %d"%(max_traj_idx, len(traj_list))) + +# Planning presets +filterYvals = config["filterYvals"] +filterWeights = config["filterWeights"] +filterTSDF = config["filterTSDF"] +if "goal_position" in config: + goal_position = np.array(config["goal_position"]).reshape(1, 3)#np.array([-5., -0.4, 10.0]).reshape(1, 3) # OpenCV frame: +X RIGHT, +Y DOWN, +Z FORWARD +else: + goal_position = None # non-directed exploration +print("Goal position: ", goal_position) +min_dist2obs = config["min_dist2obs"] +min_dist2goal = config["min_dist2goal"] + +# Make directories for data +time_string = time.strftime("%Y-%m-%d-%H-%M-%S") +save_dir = config["save_dir_prefix"] + time_string +print("Saving files to: " + save_dir) +npz_save_filename = save_dir + "/vbg.npz" + +crazyflie_img_dir = os.path.join(save_dir, "crazyflie-rgb-images") +crazyflie_pose_dir = os.path.join(save_dir, "crazyflie-poses") +kinect_img_dir = os.path.join(save_dir, "kinect-rgb-images") +kinect_depth_dir = os.path.join(save_dir, "kinect-depth-images") + +os.makedirs(crazyflie_img_dir, exist_ok=True) +os.makedirs(crazyflie_pose_dir, exist_ok=True) +os.makedirs(kinect_img_dir, exist_ok=True) +os.makedirs(kinect_depth_dir, exist_ok=True) + +# Save the run information to a csv +header = ["frame_number", "chosen_traj_idx", "time_elapsed"] +with open(save_dir + "/crazyflie_trajectories.csv", "w") as file: + file.write(",".join(header) + "\n") + +# key press callback function (for manual control) +def on_press(key): + global last_key_pressed + try: + last_key_pressed = key.char + except AttributeError: + last_key_pressed = key +# start keyboard listener +listener = keyboard.Listener(on_press=on_press) +listener.start() + +# MAIN MONONAV CONTROL LOOP +def main(): + global shouldStop + global last_key_pressed + global max_traj_idx + + # Run ZoeDepth a few times (the first inference is slow), and skip the first few frames + cap = VideoCapture(camera_num) + num_prezoe_frames = config["num_prezoe_frames"] + for i in range(0,num_prezoe_frames): + crazyflie_rgb = cap.read() + # COMPUTE DEPTH + start_time_test = time.time() + depth_numpy, depth_colormap = compute_depth(crazyflie_rgb, zoe) + print("TIME TO COMPUTE DEPTH:",time.time()-start_time_test) + cv2.imshow('frame', crazyflie_rgb) + cv2.waitKey(1) + cv2.destroyAllWindows() + + # CRAZYFLIE CONTROL + # Initialize the low-level drivers + cflib.crtp.init_drivers() + + with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: + cf = scf.cf + reset_estimator(cf) + # Set up log conf + logstate = LogConfig(name='state', period_in_ms=10) + logstate.add_variable('stateEstimate.x', 'float') + logstate.add_variable('stateEstimate.y', 'float') + logstate.add_variable('stateEstimate.z', 'float') + logstate.add_variable('stateEstimate.roll', 'float') + logstate.add_variable('stateEstimate.pitch', 'float') + logstate.add_variable('stateEstimate.yaw', 'float') + + # Initialize lists and frame counter. + frame_number = 0 + + start_flight_time = time.time() + if FLY_CRAZYFLIE: + print("Taking off.") + # Takeoff Sequence + for y in np.linspace(0, height, 21): + cf.commander.send_hover_setpoint(0, 0, 0, y) + time.sleep(0.1) + + for _ in range(20): + cf.commander.send_hover_setpoint(0, 0, 0, height) + time.sleep(0.1) + + ########################################## + print("Starting control.") + traj_counter = 0 # how many trajectory iterations have we done? + start_time = time.time() # seconds + + while not shouldStop: + cv2.imshow('frame', cap.read()) + cv2.waitKey(1) + print("shouldStop: ", shouldStop) + if last_key_pressed == 'a': + print("Pressed a. Going left.") + traj_index = 0 # left + elif last_key_pressed == 'w': + print("Pressed w. Going straight.") + traj_index = int(len(traj_list)/2) # straight + elif last_key_pressed == 'd': + print("Pressed d. Going right.") + traj_index = len(traj_list)-1 # right + elif last_key_pressed == 'g': + print("Pressed g. Using MonoNav.") + traj_index = max_traj_idx + elif last_key_pressed == 'c': #end control and land + print("Pressed c. Ending control.") + break + elif last_key_pressed == 'q': #end flight immediately + print("Pressed q. EMERGENCY STOP.") + cf.commander.send_stop_setpoint() + break + else: + print("Else: Staying put.") + start_time = time.time() + while time.time() - start_time < period: + if FLY_CRAZYFLIE: + cf.commander.send_hover_setpoint(0, 0, 0, height) + time.sleep(0.1) + continue + + # Save trajectory information + row = np.array([frame_number, int(max_traj_idx), time.time()-start_flight_time]) # time since start of flight + with open(save_dir + "/crazyflie_trajectories.csv", "a") as file: + np.savetxt(file, row.reshape(1, -1), delimiter=',', fmt='%s') + + # Fly the selected trajectory, as applicable. + start_time = time.time() + while time.time() - start_time < period: + # WARNING: This controller is tuned to work for the Crazyflie 2.1. + # You must check whether your robot follows the open-loop trajectory. + yawrate = -amplitudes[traj_index]*np.sin(np.pi/period*(time.time() - start_time))*180/np.pi # deg/s + yvel = -yawrate*config["yvel_gain"] + yawrate = yawrate*config["yawrate_gain"] + print("last_key_pressed = ", last_key_pressed) + if FLY_CRAZYFLIE: + cf.commander.send_hover_setpoint(forward_speed, yvel, yawrate, height) + # get camera capture and transform intrinsics + crazyflie_rgb = cap.read() + camera_position = get_crazyflie_pose(scf, logstate) # get camera position immediately + if goal_position is not None: + dist_to_goal = np.linalg.norm(camera_position[0:-1, -1]-goal_position[0]) + print("dist_to_goal: ", dist_to_goal) + if dist_to_goal < min_dist2goal: + print("Reached goal!") + shouldStop = True + last_key_pressed = 'q' + break + # Transform Crazyflie Image to Kinect Image + kinect_rgb = transform_image(np.asarray(crazyflie_rgb), mtx, dist, kinect) + kinect_bgr = cv2.cvtColor(kinect_rgb, cv2.COLOR_RGB2BGR) + # compute depth + depth_numpy, depth_colormap = compute_depth(kinect_rgb, zoe) + + # SAVE DATA TO FILE + cv2.imwrite(crazyflie_img_dir + "/crazyflie_frame-%06d.rgb.jpg"%(frame_number), crazyflie_rgb) + cv2.imwrite(kinect_img_dir + "/kinect_frame-%06d.rgb.jpg"%(frame_number), kinect_rgb) + cv2.imwrite(kinect_depth_dir + "/" + "kinect_frame-%06d.depth.jpg"%(frame_number), depth_colormap) + np.save(kinect_depth_dir + "/" + "kinect_frame-%06d.depth.npy"%(frame_number), depth_numpy) # saved in meters + np.savetxt(crazyflie_pose_dir + "/crazyflie_frame-%06d.pose.txt"%(frame_number), camera_position) + + # integrate the vbg (prefers bgr) + vbg.integration_step(kinect_bgr, depth_numpy, camera_position) + + frame_number += 1 + traj_counter += 1 + + # if crazyflie is not in "GO" (g) mode, reset to stopping mode + if last_key_pressed != 'g': + last_key_pressed = None + + shouldStop, max_traj_idx = choose_primitive(vbg.vbg, camera_position, traj_linesets, goal_position, min_dist2obs, filterYvals, filterWeights, filterTSDF, weight_threshold) + print("SELECTED max_traj_idx: ", max_traj_idx) + + # Exited while(!shouldStop); end control! + print("shouldStop: ", shouldStop) + print("Reached goal OR too close to obstacles.") + print("End control.") + + if FLY_CRAZYFLIE: + # Stopping sequence + for _ in range(10): + cf.commander.send_hover_setpoint(0, 0, 0, height) + time.sleep(0.1) + print("Landing.") + # Landing Sequence + for y in np.linspace(height, 0, 21): + cf.commander.send_hover_setpoint(0, 0, 0, y) + time.sleep(0.1) + + cf.commander.send_stop_setpoint() + + print("Releasing camera capture.") + cap.cap.release() + cv2.destroyAllWindows() + + # save and view vbg + print('Saving to {}...'.format(npz_save_filename)) + vbg.vbg.save(npz_save_filename) + print('Saving finished') + print("Visualize raw pointcloud.") + pcd = vbg.vbg.extract_point_cloud(weight_threshold) + o3d.visualization.draw([pcd.cpu()]) + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/simulate.py b/simulate.py new file mode 100644 index 0000000..f7a7d71 --- /dev/null +++ b/simulate.py @@ -0,0 +1,95 @@ +""" + __ __ _ _ + | \/ | ___ _ __ ___ | \ | | __ ___ __ + | |\/| |/ _ \| '_ \ / _ \| \| |/ _` \ \ / / + | | | | (_) | | | | (_) | |\ | (_| |\ V / + |_| |_|\___/|_| |_|\___/|_| \_|\__,_| \_/ +Copyright (c) 2023 Nate Simon +License: MIT +Authors: Nate Simon and Anirudha Majumdar, Princeton University +Project Page: https://natesimon.github.io/mononav + +The purpose of this script is to step through the 3D reconstruction and to execute the MonoNav planner. +Steps: +1) load the reconstruction, poses, and trajectory library, +2) for each pose, choose the optimal motion primitive according to the planner, +3) visualize the reconstruction, poses, and motion primitives (both available and chosen). + +This script is a useful way to debug and debrief the planner, as well as to see how changes to the planner +and trajectory library affect the planning performance. + +""" + +import os +import open3d as o3d +import numpy as np +import copy + +from utils.utils import load_config, poses_from_posedir, get_poses_lineset, get_trajlist, get_traj_linesets, choose_primitive + +CONFIG_PATH = "config.yml" +config = load_config("config.yml") + +data_dir = config["data_dir"] # parent directory to look for RGB images, and save depth images +pose_dir = os.path.join(data_dir, "crazyflie-poses") +trajlib_dir = config["trajlib_dir"] + +# Load the VoxelBlockGrid from file. +files = [file for file in os.listdir(data_dir) if file.endswith('.npz')] +assert len(files) > 0, "No *.npz files found." +npz_filename = files[0] # if there are multiple files, change the index +print("Loading ", npz_filename, " with Open3D.") +vbg = o3d.t.geometry.VoxelBlockGrid.load(os.path.join(data_dir, npz_filename)).cpu() +pcd = vbg.extract_point_cloud(config["weight_threshold"]) + +# Planning presets +filterYvals = config["filterYvals"] +filterWeights = config["filterWeights"] +filterTSDF = config["filterTSDF"] +if "goal_position" in config: + goal_position = np.array(config["goal_position"]).reshape(1, 3) # OpenCV frame: +X RIGHT, +Y DOWN, +Z FORWARD +else: + goal_position = None +print("Goal position: ", goal_position) +min_dist2obs = config["min_dist2obs"] +weight_threshold = config["weight_threshold"] # for planning and visualization + +# Load poses from directory. +poses = poses_from_posedir(pose_dir) +# Get pose lineset +pose_lineset = get_poses_lineset(poses) + +# Load the trajectory linesets from the trajlib directory +traj_list = get_trajlist(trajlib_dir) +traj_linesets, period, forward_speed, amplitudes = get_traj_linesets(traj_list) + + +# Create the visualizer and add components +visualizer = o3d.visualization.Visualizer() +visualizer.create_window() +visualizer.add_geometry(pcd.to_legacy()) +visualizer.add_geometry(pose_lineset) + +# For each pose, compute the optimal motion primitive. +# Paint the optimal motion primitive green, and the rest black. +n = 5 # iterate over every n poses +for i in range(0, len(poses), n): + pose = poses[i] + shouldStop, max_traj_idx = choose_primitive(vbg, pose, traj_linesets, goal_position, min_dist2obs, filterYvals, filterWeights, filterTSDF, weight_threshold) + for traj_idx, traj_lineset in enumerate(traj_linesets): + traj_lineset_copy = copy.deepcopy(traj_lineset) + traj_lineset_copy.transform(pose) + + if traj_idx == max_traj_idx: + traj_lineset_copy.paint_uniform_color([0, 1, 0]) + else: + traj_lineset_copy.paint_uniform_color([0, 0, 0]) + + visualizer.add_geometry(traj_lineset_copy) + + # # (Optional) Uncomment to add coordinate frame, which may look busy. + # coordinate_frame = o3d.geometry.TriangleMesh.create_coordinate_frame().scale(0.5, center=(0, 0, 0)) + # visualizer.add_geometry(coordinate_frame.transform(pose)) + +visualizer.run() +visualizer.destroy_window() \ No newline at end of file diff --git a/utils/calibration/calibrate.py b/utils/calibration/calibrate.py new file mode 100644 index 0000000..6e0e0c8 --- /dev/null +++ b/utils/calibration/calibrate.py @@ -0,0 +1,181 @@ +#!/usr/bin/env python + +''' +ADAPTED FROM OPENCV SAMPLES +https://github.com/opencv/opencv/blob/4.x/samples/python/calibrate.py +SAMPLE CALL: +python calibrate.py --debug ./calibration_output -w 6 -h 8 -t chessboard --square_size=35 ./calibration_pictures/frame*.jpg + + +SEE INSTRUCTIONS FROM OPENCV: +camera calibration for distorted images with chess board samples +reads distorted images, calculates the calibration and write undistorted images + +usage: + calibrate.py [--debug ] [-w ] [-h ] [-t ] [--square_size=] [] + +usage example: + calibrate.py -w 4 -h 6 -t chessboard --square_size=50 ../data/left*.jpg + +default values: + --debug: ./output/ + -w: 4 + -h: 6 + -t: chessboard + --square_size: 10 + --marker_size: 5 + --threads: 4 + +NOTE: Chessboard size is defined in inner corners. Charuco board size is defined in units, and has been removed from this sample. +''' + +import numpy as np +import cv2 as cv +import json +import os + +def splitfn(fn): + path, fn = os.path.split(fn) + name, ext = os.path.splitext(fn) + return path, name, ext + +def main(): + import sys + import getopt + + args, img_names = getopt.getopt(sys.argv[1:], 'w:h:t:', ['debug=','square_size=', 'threads=', ]) + args = dict(args) + args.setdefault('--debug', './output/') + args.setdefault('-w', 4) + args.setdefault('-h', 6) + args.setdefault('-t', 'chessboard') + args.setdefault('--square_size', 10) + args.setdefault('--threads', 4) + + assert img_names, 'Did you provide a path for images?' + + debug_dir = args.get('--debug') + if debug_dir and not os.path.isdir(debug_dir): + os.mkdir(debug_dir) + + height = int(args.get('-h')) + width = int(args.get('-w')) + pattern_type = str(args.get('-t')) + square_size = float(args.get('--square_size')) + + pattern_size = (width, height) + if pattern_type == 'chessboard': + pattern_points = np.zeros((np.prod(pattern_size), 3), np.float32) + pattern_points[:, :2] = np.indices(pattern_size).T.reshape(-1, 2) + pattern_points *= square_size + + obj_points = [] + img_points = [] + h, w = cv.imread(img_names[0], cv.IMREAD_GRAYSCALE).shape[:2] + + def processImage(fn): + print('processing %s... ' % fn) + img = cv.imread(fn, cv.IMREAD_GRAYSCALE) + if img is None: + print("Failed to load", fn) + return None + + assert w == img.shape[1] and h == img.shape[0], ("size: %d x %d ... " % (img.shape[1], img.shape[0])) + found = False + corners = 0 + if pattern_type == 'chessboard': + found, corners = cv.findChessboardCorners(img, pattern_size) + if found: + term = (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_COUNT, 30, 0.1) + cv.cornerSubPix(img, corners, (5, 5), (-1, -1), term) + frame_img_points = corners.reshape(-1, 2) + frame_obj_points = pattern_points + else: + print("unknown pattern type", pattern_type) + return None + + if debug_dir: + vis = cv.cvtColor(img, cv.COLOR_GRAY2BGR) + if pattern_type == 'chessboard': + cv.drawChessboardCorners(vis, pattern_size, corners, found) + _path, name, _ext = splitfn(fn) + outfile = os.path.join(debug_dir, name + '_board.png') + cv.imwrite(outfile, vis) + + if not found: + print('pattern not found') + return None + + print(' %s... OK' % fn) + return (frame_img_points, frame_obj_points) + + threads_num = int(args.get('--threads')) + if threads_num <= 1: + chessboards = [processImage(fn) for fn in img_names] + else: + print("Run with %d threads..." % threads_num) + from multiprocessing.dummy import Pool as ThreadPool + pool = ThreadPool(threads_num) + chessboards = pool.map(processImage, img_names) + + chessboards = [x for x in chessboards if x is not None] + for idx, (corners, pattern_points) in enumerate(chessboards): + if len(corners) < 4: + print("Not enough obj/img points for %d, skipping image!" % idx) + else: + img_points.append(corners) + obj_points.append(pattern_points) + + rms, camera_matrix, dist_coefs, _rvecs, _tvecs = cv.calibrateCamera(obj_points, img_points, (w, h), None, None) + newcameramtx, roi = cv.getOptimalNewCameraMatrix(camera_matrix, dist_coefs, (w, h), 1, (w, h)) + + print("\nRMS:", rms) + print("camera matrix:\n", camera_matrix) + print("distortion coefficients: ", dist_coefs.ravel()) + print("newcameramtx:\n", newcameramtx) + print("roi: ", roi) + + + data = { + "RMS": rms, + "CameraMatrix": camera_matrix.tolist(), + "DistortionCoefficients": dist_coefs.ravel().tolist(), + "NewCameraMatrix": newcameramtx.tolist(), + "ROI": roi + } + input_dirname = img_names[0].split('/')[1].split('.')[0] + if input_dirname == 'transform_output': + file_path = "check_intrinsics.json" + else: + file_path = "intrinsics.json" + + with open(file_path, "w") as json_file: + json.dump(data, json_file) + + # undistort the image with the calibration + print('') + for fn in img_names if debug_dir else []: + _path, name, _ext = splitfn(fn) + # img_found = os.path.join(debug_dir, name + '_board.png') + outfile = os.path.join(debug_dir, name + '_undistorted.png') + + img = cv.imread(fn) + if img is None: + continue + + dst = cv.undistort(img, camera_matrix, dist_coefs, None, newcameramtx) + + # # crop and save the image + # x, y, w, h = roi + # dst = dst[y:y+h, x:x+w] + + print('Undistorted image written to: %s' % outfile) + cv.imwrite(outfile, dst) + print('Done') + + +if __name__ == '__main__': + print(__doc__) + main() + cv.destroyAllWindows() + diff --git a/utils/calibration/demo_intrinsics.json b/utils/calibration/demo_intrinsics.json new file mode 100644 index 0000000..90491ab --- /dev/null +++ b/utils/calibration/demo_intrinsics.json @@ -0,0 +1,50 @@ +{ + "RMS": 0.33129824379597467, + "CameraMatrix": [ + [ + 210.24118591854983, + 0.0, + 318.51182284207556 + ], + [ + 0.0, + 211.4846805698751, + 252.48457990317036 + ], + [ + 0.0, + 0.0, + 1.0 + ] + ], + "DistortionCoefficients": [ + 0.19941623511989434, + -0.20736409267316344, + 0.0007040531507409564, + 0.0008060182732909919, + 0.047875485130274044 + ], + "NewCameraMatrix": [ + [ + 200.40865481935182, + 0.0, + 319.71648713379267 + ], + [ + 0.0, + 200.89201451631303, + 255.18999581949225 + ], + [ + 0.0, + 0.0, + 1.0 + ] + ], + "ROI": [ + 84, + 71, + 469, + 350 + ] +} \ No newline at end of file diff --git a/utils/calibration/take_pictures.py b/utils/calibration/take_pictures.py new file mode 100644 index 0000000..ac82c4d --- /dev/null +++ b/utils/calibration/take_pictures.py @@ -0,0 +1,52 @@ +""" +The purpose of this file is to take pictures of a calibration target. +Press "spacebar" to capture a frame and "q" to quit the program. +The calibration target should be a checkerboard pattern. +In calibrate_camera, the captured pictures will be used to get a calibration matrix. +""" +import cv2 +import os +import yaml + +# Read the camera number from the config file +CONFIG_PATH = "../../config.yml" +with open(CONFIG_PATH, "r") as f: + config = yaml.safe_load(f) +camera_num = config["camera_num"] # you may have to try 0, 1, 2, ... (depending on # of cameras on system) + +# Receive the image +cap = cv2.VideoCapture(camera_num) + +# Define the directory where you want to save the captured images +calibration_pictures = "calibration_pictures" +print("Saving calibration images to: ", calibration_pictures) +# Create the directory if it doesn't exist +os.makedirs(calibration_pictures, exist_ok=True) + + +frame_count = 0 +print("Press space to capture frame. Press q to quit.") +while True: + # Capture a frame from the camera + ret, frame = cap.read() + # Display the frame + cv2.imshow('Press Space to Capture, q to quit', frame) + # Check for the spacebar key press (ASCII code 32) + key = cv2.waitKey(1) + if key == 32: # 32 is the ASCII code for spacebar + filename = os.path.join(calibration_pictures, f"frame-{frame_count:04d}.jpg") + + # Increment the frame count for the next captured frame + frame_count += 1 + + # Save the captured frame as a .jpg image + cv2.imwrite(filename, frame) + print(f"Image saved as {filename}") + + # Check for the 'q' key press to quit the program + elif key == ord('q'): + break + +# Release the camera and close all OpenCV windows +cap.release() +cv2.destroyAllWindows() \ No newline at end of file diff --git a/utils/calibration/transform.py b/utils/calibration/transform.py new file mode 100644 index 0000000..f4dd384 --- /dev/null +++ b/utils/calibration/transform.py @@ -0,0 +1,49 @@ +""" +This script is used to transform the images from the robot's camera to the kinect's camera. +It uses the camera intrinsics from calibrate.py, and OpenCV's undistort function. +The transform_image function also resizes the image (and intrinsic matrix) appropriately +to match the kinect's dimensions and intrinsics. This is crucial for the performance of +the depth estimation model. +""" + +import os +import open3d as o3d +import numpy as np +import cv2 + +import os, sys +sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) +from utils import load_config, get_calibration_values, transform_image + +# LOAD CONFIG +CONFIG_PATH = "../../config.yml" +config = load_config(CONFIG_PATH) + +# Load the calibration images from "calibration_pictures" +calibration_dir = "calibration_pictures" +print("Loading images from: ", calibration_dir) +calibration_files = [file for file in os.listdir(calibration_dir) if file.endswith('.jpg')] +calibration_files = sorted(calibration_files) + +# Load the calibration values +camera_calibration_path = "intrinsics.json"#os.path.join('../../',config["camera_calibration_path"]) +intrinsic_filename = os.path.basename(camera_calibration_path) +print("Loading intrinsics from: ", intrinsic_filename) +mtx, dist = get_calibration_values(camera_calibration_path) # for the robot's camera +# Kinect intrinsic matrix +kinect = o3d.camera.PinholeCameraIntrinsic(o3d.camera.PinholeCameraIntrinsicParameters.PrimeSenseDefault) + + +# Set directories for images +transform_dir = "transform_output" +os.mkdir(transform_dir) if not os.path.exists(transform_dir) else None +print("Saving transformed images to: ", transform_dir) + + +for filename in calibration_files: + # Read in the image + img = cv2.imread(os.path.join(calibration_dir, filename)) + # transform + transformed_image = transform_image(np.asarray(img), mtx, dist, kinect) + # write image + cv2.imwrite(os.path.join(transform_dir, filename), transformed_image) \ No newline at end of file diff --git a/utils/generate_primitives.py b/utils/generate_primitives.py new file mode 100644 index 0000000..c5882fc --- /dev/null +++ b/utils/generate_primitives.py @@ -0,0 +1,128 @@ +""" + __ __ _ _ + | \/ | ___ _ __ ___ | \ | | __ ___ __ + | |\/| |/ _ \| '_ \ / _ \| \| |/ _` \ \ / / + | | | | (_) | | | | (_) | |\ | (_| |\ V / + |_| |_|\___/|_| |_|\___/|_| \_|\__,_| \_/ +Copyright (c) 2023 Nate Simon +License: MIT +Authors: Nate Simon and Anirudha Majumdar, Princeton University +Project Page: https://natesimon.github.io/mononav + +The purpose of this script is to generate a library of motion primitives (trajectories) +as described in the paper. Each motion primitive is defined by a sinusoidal yawrate profile, +which keeps the initial and final yawrates zero (for smooth chaining of primitives). +The trajectory library is visualized in trajlib/visualization.png. +By changing the trajectory constants and extension segment, you can generate a diverse variety of primitives. +If you change the trajlib_dir, be sure to update `trajlib_dir: 'utils/trajlib/'` in config.yml. +""" + +import os +import numpy as np +import matplotlib.pyplot as plt + +# Trajectory Constants +T = 1.0 # s, period of the primitive +V = 0.5 # m/s, forward speed +max_yawrate = 0.7 # rad/s +num_trajectories = 7 # number of trajectories should be ODD (e.g., 11) to ensure a straight line is included +num_commands = 65 # number of points in the trajectory +num_points = 8 # how many points should be in each primitive and each extension segment? (for primitive evaluation) + +# Extension segment - straight line at the end of the trajectory (to encourage foresight) +x_ext = np.linspace(0., 1.0, num_points) # extension segment in the body frame (x = forward) +y_ext = np.zeros_like(x_ext) +ext = np.vstack((x_ext, y_ext)) + +# Assertion statements +assert num_trajectories % 2 == 1, "num_trajectories should be odd" +assert max_yawrate > 0, "max_yawrate should be positive" + +# Derived quantities +omega = np.pi/T # 1/s, angular frequency +t = np.linspace(0,T,num_commands) # s, time vector +Avals = np.linspace(max_yawrate, -max_yawrate, num_trajectories) # rad/s, yawrate amplitude vector + + +# Create a subplot +fig, (ax2, ax1) = plt.subplots(2, 1)#, sharex=True) +fig.tight_layout() + +traj_num = 0 +trajlib_dir = './trajlib/' +# os.mkdir(trajlib_dir) if not os.path.exists(trajlib_dir) else None + +if os.path.exists(trajlib_dir): + # Delete the existing trajectory library\ + for filename in os.listdir(trajlib_dir): + file_path = os.path.join(trajlib_dir, filename) + os.remove(file_path) +else: + # Create the directory + os.mkdir(trajlib_dir) + +trajlist = [] + +# For each yawrate profile, determine the associate xdot, ydot commands (which are used for the Crazyflie) +# numerically integrate to obtain x(t) and y(t) (for collision avoidance) +for A in Avals: + yawrate = A*np.sin(omega*t) + psi = A*(1 - np.cos(omega*t)) + # Calculate xdot and ydot + xdot = V * np.cos(psi) + ydot = V * np.sin(psi) + # Numerical integration to obtain x(t) and y(t) + x = np.cumsum(xdot) * (t[1] - t[0]) + y = np.cumsum(ydot) * (t[1] - t[0]) + + # rotate and add the extension segment + yaw = psi[-1] + R = np.array([[np.cos(yaw), -np.sin(yaw)], + [np.sin(yaw), np.cos(yaw)]]) + ext_rot = np.matmul(R,ext) + np.array([[x[-1]],[y[-1]]]) + # sample every num_points for efficient primitive evaluation + sample = int(num_commands/num_points) + x_sample = np.hstack((x[::sample], ext_rot[0,:])) + y_sample = np.hstack((y[::sample], ext_rot[1,:])) + + trajectory = { + 'xvals': x, + 'yvals': y, + 'yawvals': psi, + 'x_sample': x_sample, + 'y_sample': y_sample, + 'yawrates': yawrate, + 'times': t, + 'period': T, + 'forward_speed': V, + 'amplitude': A + } + + # Save to file + np.savez(trajlib_dir + 'traj-%02d.npz'%(traj_num), **trajectory) + traj_num += 1 + + # Top plot: yawrate and psi + ax2.plot(t, yawrate, label=r'$\dot{\psi}_{max}=$ %.2f' % A) + + # Bottom plot + ax1.plot(x,y)#, label=r'$\dot{\psi}_{max}=$ %.2f' % A) + # Plot samples and distance + ax1.plot(x_sample, y_sample, 'o', color = 'red') # slice every other value + +ax1.set_title('Trajectory @ V = %.2f m/s' % V) +ax1.set_xlabel('X position (m)') +ax1.set_ylabel('Y position (m)') +ax1.axis('equal') +#ax1.legend(loc='center left', bbox_to_anchor=(1, 0.5)) +ax2.set_xlabel('Time (s)') +ax2.set_ylabel('Yawrates rad/s') +ax2.set_title('Yawrates') +ax2.legend(loc='center left', bbox_to_anchor=(1, 0.5)) + +# Suggested plotting adjustment +plt.subplots_adjust(0.125, 0.1, 0.75, 0.9, 0.2, 0.5) + +plt.savefig(trajlib_dir + 'visualization.png',dpi=300) + +plt.show() \ No newline at end of file diff --git a/utils/reconstruction.gif b/utils/reconstruction.gif new file mode 100644 index 0000000..fe843c6 Binary files /dev/null and b/utils/reconstruction.gif differ diff --git a/utils/test_camera.py b/utils/test_camera.py new file mode 100644 index 0000000..5b9116e --- /dev/null +++ b/utils/test_camera.py @@ -0,0 +1,35 @@ +""" +This script tests the camera feed. +""" + +import cv2 +import yaml + +CONFIG_PATH = "../config.yml" +with open(CONFIG_PATH, "r") as f: + config = yaml.safe_load(f) + +# If you have multiple cameras hooked up to your desktop, +# The camera number may change. If so, try +# a different small, positive integer, e.g. -1, 0, 1, 2, 3, ... +# If you're still having issues, ensure you have ffmpeg installed. + +camera = config["camera_num"] +cap = cv2.VideoCapture(camera) + +while(True): + # Capture frame-by-frame + ret, frame = cap.read() + if frame is None: + print("No frame captured.") + break + # Display the resulting frame + cv2.imshow('frame', frame) + + # Hit q to quit. + if cv2.waitKey(1) & 0xFF == ord('q'): + break + +# Release the capture +cap.release() +cv2.destroyAllWindows() \ No newline at end of file diff --git a/utils/trajlib/traj-00.npz b/utils/trajlib/traj-00.npz new file mode 100644 index 0000000..7da90e7 Binary files /dev/null and b/utils/trajlib/traj-00.npz differ diff --git a/utils/trajlib/traj-01.npz b/utils/trajlib/traj-01.npz new file mode 100644 index 0000000..2da7cf2 Binary files /dev/null and b/utils/trajlib/traj-01.npz differ diff --git a/utils/trajlib/traj-02.npz b/utils/trajlib/traj-02.npz new file mode 100644 index 0000000..36ed77d Binary files /dev/null and b/utils/trajlib/traj-02.npz differ diff --git a/utils/trajlib/traj-03.npz b/utils/trajlib/traj-03.npz new file mode 100644 index 0000000..8e39d1d Binary files /dev/null and b/utils/trajlib/traj-03.npz differ diff --git a/utils/trajlib/traj-04.npz b/utils/trajlib/traj-04.npz new file mode 100644 index 0000000..c2be380 Binary files /dev/null and b/utils/trajlib/traj-04.npz differ diff --git a/utils/trajlib/traj-05.npz b/utils/trajlib/traj-05.npz new file mode 100644 index 0000000..79a4ab0 Binary files /dev/null and b/utils/trajlib/traj-05.npz differ diff --git a/utils/trajlib/traj-06.npz b/utils/trajlib/traj-06.npz new file mode 100644 index 0000000..342a88c Binary files /dev/null and b/utils/trajlib/traj-06.npz differ diff --git a/utils/trajlib/visualization.png b/utils/trajlib/visualization.png new file mode 100644 index 0000000..80b3401 Binary files /dev/null and b/utils/trajlib/visualization.png differ diff --git a/utils/utils.py b/utils/utils.py new file mode 100644 index 0000000..b22d922 --- /dev/null +++ b/utils/utils.py @@ -0,0 +1,399 @@ +""" + __ __ _ _ + | \/ | ___ _ __ ___ | \ | | __ ___ __ + | |\/| |/ _ \| '_ \ / _ \| \| |/ _` \ \ / / + | | | | (_) | | | | (_) | |\ | (_| |\ V / + |_| |_|\___/|_| |_|\___/|_| \_|\__,_| \_/ +Copyright (c) 2023 Nate Simon +License: MIT +Authors: Nate Simon and Anirudha Majumdar, Princeton University +Project Page: https://natesimon.github.io/mononav + +Helper functions for the MonoNav project. +Functionality should be concentrated here and shared between the scripts. + +""" +import time +import cv2 as cv2 +import numpy as np +from scipy.spatial.transform import Rotation as Rotation +from scipy.spatial import distance +import os +import open3d as o3d +import open3d.core as o3c +import copy +import yaml, json + +# For Craziflie logging +from cflib.crazyflie.log import LogConfig +from cflib.crazyflie.syncLogger import SyncLogger + +# For bufferless video capture +import queue, threading + +""" +VoxelBlockGrid class (adapted from Open3D) for ease of initialization and integration. +You can read more about the VoxelBlockGrid here: +https://www.open3d.org/docs/latest/tutorial/t_reconstruction_system/voxel_block_grid.html +""" +class VoxelBlockGrid: + def __init__(self, depth_scale=1000.0, depth_max=5.0, trunc_voxel_multiplier=8.0, device=o3d.core.Device("CUDA:0")): + # Reconstruction Information + self.depth_scale = depth_scale + self.depth_max = depth_max + self.trunc_voxel_multiplier = trunc_voxel_multiplier + self.device = device + self.camera = o3d.camera.PinholeCameraIntrinsic(o3d.camera.PinholeCameraIntrinsicParameters.PrimeSenseDefault) # Kinect Intrinsics (default) + self.depth_intrinsic = o3d.core.Tensor(self.camera.intrinsic_matrix, o3d.core.Dtype.Float64) + + # Initialize the VoxelBlockGrid + self.vbg = o3d.t.geometry.VoxelBlockGrid( + attr_names=('tsdf', 'weight', 'color'), + attr_dtypes=(o3c.float32, o3c.float32, o3c.float32), + attr_channels=(1, 1, 3), + voxel_size=3.0 / 64, # this sets the resolution of the voxel grid + block_resolution=1, + block_count=50000, + device=device) + + def integration_step(self, color, depth_numpy, cam_pose): + # Integration Step (TSDF Fusion) + depth_numpy = depth_numpy.astype(np.uint16) # Convert to uint16 + depth = o3d.t.geometry.Image(depth_numpy).to(self.device) + extrinsic = o3d.core.Tensor(np.linalg.inv(cam_pose), o3d.core.Dtype.Float64) + frustum_block_coords = self.vbg.compute_unique_block_coordinates( + depth, self.depth_intrinsic, extrinsic, self.depth_scale, self.depth_max, self.trunc_voxel_multiplier) + color = o3d.t.geometry.Image(np.asarray(color)).to(self.device) + color_intrinsic = o3d.core.Tensor(self.camera.intrinsic_matrix, o3d.core.Dtype.Float64) + self.vbg.integrate(frustum_block_coords, depth, color, self.depth_intrinsic, + color_intrinsic, extrinsic, self.depth_scale, self.depth_max, self.trunc_voxel_multiplier) + + +""" +Bufferless VideoCapture, courtesy of Ulrich Stern (https://stackoverflow.com/a/54577746) +Otherwise, a lag builds up in the video stream. +""" +class VideoCapture: + + def __init__(self, name): + self.cap = cv2.VideoCapture(name) + self.q = queue.Queue() + t = threading.Thread(target=self._reader) + t.daemon = True + t.start() + + # read frames as soon as they are available, keeping only most recent one + def _reader(self): + while True: + ret, frame = self.cap.read() + if not ret: + break + if not self.q.empty(): + try: + self.q.get_nowait() # discard previous (unprocessed) frame + except queue.Empty: + pass + self.q.put(frame) + + def read(self): + return self.q.get() + +""" +Get the global Crazyflie (camera) pose from the logger, convert to the Open3D frame +Crazyflie frame: (X, Y, Z) is FRONT LEFT UP (FLU) +Open3D frame: (X, Y, Z) is RIGHT DOWN FRONT (RDF) +""" +def get_crazyflie_pose(scf, logstate): + with SyncLogger(scf, logstate) as logger: + for log_entry in logger: + data = log_entry[1] + _x = data['stateEstimate.x'] + _y = data['stateEstimate.y'] + _z = data['stateEstimate.z'] + _roll = data['stateEstimate.roll'] + _pitch = data['stateEstimate.pitch'] + _yaw = data['stateEstimate.yaw'] + # Convert position from CF to TSDF frame + xyz = np.array([-_y, -_z, _x]) # Convert to TSDF frame + # Convert rotation from CF to TSDF frame + r = Rotation.from_euler('xyz', [_roll, -_pitch, _yaw], degrees=True) + R = r.as_matrix() + M_change = np.array([[0,-1,0],[0,0,-1],[1,0,0]]) + R = M_change @ R @ M_change.T + + # Create a homogeneous matrix + Hmtrx = np.hstack((R, xyz.reshape(3,1))) + # return camera position + return np.vstack((Hmtrx, np.array([0, 0, 0, 1]))) + +""" +Compute depth from an RGB image using ZoeDepth +Returns depth_numpy (uint16 in mm), depth_colormap (for visualization) +""" +def compute_depth(color, zoe): + # Compute depth + depth = zoe.infer_pil(color, output_type="tensor") # as torch tensor + depth_numpy = np.asarray(depth) # Convert to numpy array + depth_numpy = 1000*depth_numpy # Convert to mm + depth_numpy = depth_numpy.astype(np.uint16) # Convert to uint16 + + # Save images and depth array + depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(depth_numpy, alpha=0.03), cv2.COLORMAP_JET) + + return depth_numpy, depth_colormap + +""" +Load the poses (after navigation, for analysis) from the posedir. +Returns a list of pose arrays. +""" +def poses_from_posedir(posedir): + poses = [] + pose_files = [name for name in os.listdir(posedir) if os.path.isfile(os.path.join(posedir, name)) and name.endswith(".txt")] + pose_files = sorted(pose_files) + + for pose_file in pose_files: + cam_pose = np.loadtxt(posedir +"/"+pose_file) + poses.append(cam_pose) + return poses + +""" +Convert a list of poses (after navigation, for analysis) into a trajectory lineset. +This object is used to visualize the trajectory in Open3D. +Returns a list of of lineset objects representing the camera's pose. +""" +def get_poses_lineset(poses): + points = [] + lines = [] + for pose in poses: + position = pose[0:3,3] # meters + points.append(position) + lines.append([len(points)-1, len(points)]) + + pose_lineset = o3d.geometry.LineSet( + points=o3d.utility.Vector3dVector(points), + lines=o3d.utility.Vector2iVector(lines[:-1]), + ) + pose_lineset.paint_uniform_color([1, 0, 0]) #optional: change the color here + return pose_lineset + +""" +Load the trajectory primitives (before navigation). +Read a list of motion primitives (trajectories) from a the "trajlib_dir" (trajectory library) directory. +Returns a list of trajectory objects. +""" +def get_trajlist(trajlib_dir): + # Get the list of files in the directory + file_list = os.listdir(trajlib_dir) + # Filter only .npz files + npz_files = [file for file in file_list if file.endswith('.npz')] + # Sort the list of .npz files - important for indexing! + sorted_files = sorted(npz_files) + # Iterate over the sorted list of .npz files + traj_list = [] + for trajfile in sorted_files: + file_path = os.path.join(trajlib_dir, trajfile) + traj_list.append(np.load(file_path)) + + return traj_list + +""" +Convert the trajectory list into a list of trajectory linesets. +These are used for visualizing the possible trajectories at each step. +Returns a list of trajectory lineset objects. +""" +def get_traj_linesets(traj_list): + traj_linesets = [] + amplitudes = [] + for traj in traj_list: + # traj_dict = {key: traj[key] for key in traj.files} + z_tsdf = traj['x_sample'] + x_tsdf = -traj['y_sample'] + points = [] + lines = [] + for i in range(len(x_tsdf)): + points.append([x_tsdf[i], 0, z_tsdf[i]]) + lines.append([len(points)-1, len(points)]) + traj_lineset = o3d.geometry.LineSet( + points=o3d.utility.Vector3dVector(points), + lines=o3d.utility.Vector2iVector(lines[:-1]), + ) + traj_linesets.append(traj_lineset) + amplitudes.append(traj['amplitude']) + # get traj info + period = traj['period'] + forward_speed = traj['forward_speed'] + + return traj_linesets, period, forward_speed, amplitudes + + +""" +MonoNav Planner: Return the chosen trajectory index given the current position, current reconstruction, trajectory library, and goal position. +""" +def choose_primitive(vbg, camera_position, traj_linesets, goal_position, dist_threshold, filterYvals, filterWeights, filterTSDF, weight_threshold): + + # Boolean for stopping criteria + shouldStop = False + + # Get weights and tsdf values from the voxel block grid + weights = vbg.attribute("weight").reshape((-1)) + tsdf = vbg.attribute("tsdf").reshape((-1)) + # Get the voxel_coords, voxel_indices + voxel_coords, voxel_indices = vbg.voxel_coordinates_and_flattened_indices() + + # IMPORTANT + # Use voxel_indices to rearrange weights and tsdf to match voxel_coords + # Otherwise, the ordering of voxels from the hashmap is non-deterministic + weights = weights[voxel_indices] + tsdf = tsdf[voxel_indices] + + # Generate mask to filter out y values (vertical) (+y is DOWN) + # This is useful to filter out the floor, and avoid obstacles in-plane + if filterYvals: + mask = voxel_coords[:, 1] < -0.3 + # Apply mask to voxel_coords and weights + voxel_coords = voxel_coords[mask] + weights = weights[mask] + tsdf = tsdf[mask] + + # Generate mask to filter by weights + # This rejects voxels below a certain weight threshold + if filterWeights: + mask = weights > weight_threshold + # Apply mask to voxel_coords and weights + voxel_coords = voxel_coords[mask,:] + tsdf = tsdf[mask] + + # Generate mask to filter by tsdf value + if filterTSDF: + # Generate mask to filter by tsdf values + mask = tsdf < 0.0 + voxel_coords = voxel_coords[mask,:] + + # transfer to cpu for cdist + voxel_coords_numpy = voxel_coords.cpu().numpy() + + # NOW WE HAVE A FILTERED SET OF VOXELS THAT REPRESENT OBSTACLES + # NEXT, WE DETERMINE THE BEST TRAJECTORY ACCORDING TO A COST FUNCTION + + # Initialize scoring variables to evaluate the trajectories + max_traj_score = -np.inf # track best trajectory + min_goal_score = np.inf # track proximity to goal + max_traj_idx = None # track the index of the best trajectory + + # iterate over the sorted traj linesets + for traj_idx, traj_linset in enumerate(traj_linesets): + traj_lineset_copy = copy.deepcopy(traj_linset) + traj_lineset_copy.transform(camera_position) # transform the lineset (copy) to the camera position + pts = np.asarray(traj_lineset_copy.points) # meters # extract the points from the lineset + tmp = distance.cdist(voxel_coords_numpy, pts, "sqeuclidean") # compute the distance between all voxels and all points in the trajectory + voxel_idx, pt_idx = np.unravel_index(np.argmin(tmp), tmp.shape) # extract indices of the nearest voxel to and nearest point in the trajectory + nearest_voxel_dist = np.sqrt(tmp[voxel_idx, pt_idx]) + if nearest_voxel_dist > dist_threshold: + # the trajectory meets the dist_threshold criterion + if goal_position is not None: + # the trajectory satisfies the dist_threshold; let's compute the goal score + tmp_to_goal = distance.cdist(goal_position, pts, "sqeuclidean") + dst_to_goal = np.sqrt(np.min(tmp_to_goal)) + if dst_to_goal < min_goal_score: + # we have a trajectory that gets us closer to the goal + # print("traj %d gets us closer to the goal: %f"%(traj_idx, dst_to_goal)) + max_traj_idx = traj_idx + min_goal_score = dst_to_goal + else: + # no goal position, choose the index that maximizes distance from the obstacles + if max_traj_score < nearest_voxel_dist: + # we have found a trajectory that gets us closer to goal + max_traj_idx = traj_idx + max_traj_score = nearest_voxel_dist + + if max_traj_idx is None: + # No trajectory meets the dist_threshold criterion, crazyflie should stop. + shouldStop = True + return shouldStop, max_traj_idx + + +""" +Upon Crazyflie startup, these helper functions ensure the EKF is properly initialized before takeoff. +# Copyright (C) 2018 Bitcraze AB +Taken from several Crazyflie examples, e.g., https://github.com/bitcraze/crazyflie-lib-python/blob/master/examples/autonomy/autonomous_sequence_high_level.py +""" +def reset_estimator(scf): + scf.param.set_value('kalman.resetEstimation', '1') + time.sleep(0.1) + scf.param.set_value('kalman.resetEstimation', '0') + + print('Waiting for estimator to find position...') + + log_config = LogConfig(name='Kalman Variance', period_in_ms=500) + log_config.add_variable('kalman.varPX', 'float') + log_config.add_variable('kalman.varPY', 'float') + log_config.add_variable('kalman.varPZ', 'float') + + var_y_history = [1000] * 10 + var_x_history = [1000] * 10 + var_z_history = [1000] * 10 + + threshold = 0.001 + + with SyncLogger(scf, log_config) as logger: + for log_entry in logger: + data = log_entry[1] + + var_x_history.append(data['kalman.varPX']) + var_x_history.pop(0) + var_y_history.append(data['kalman.varPY']) + var_y_history.pop(0) + var_z_history.append(data['kalman.varPZ']) + var_z_history.pop(0) + + min_x = min(var_x_history) + max_x = max(var_x_history) + min_y = min(var_y_history) + max_y = max(var_y_history) + min_z = min(var_z_history) + max_z = max(var_z_history) + + if (max_x - min_x) < threshold and ( + max_y - min_y) < threshold and ( + max_z - min_z) < threshold: + break + +""" +Load config.yml file +""" +def load_config(config_path): + with open(config_path, "r") as f: + config = yaml.safe_load(f) + return config + +""" +Read in the intrinsics.json file and return the camera matrix and distortion coefficients +""" +def get_calibration_values(camera_calibration_path): + # Load the camera calibration file + with open(camera_calibration_path, "r") as json_file: + data = json.load(json_file) + mtx = np.array(data['CameraMatrix']) + dist = np.array(data['DistortionCoefficients']) + return mtx, dist + +""" +Transform the raw image to match the kinect image: dimensions and intrinsics. +This involves resizing the image, scaling the camera matrix, and undistorting the image. +""" +def transform_image(image, mtx, dist, kinect): + if image.shape[0] != kinect.height or image.shape[1] != kinect.width: + # Resize the camera matrix to match new dimensions + scale_vec = np.array([kinect.width / image.shape[1], kinect.height / image.shape[0], 1]).reshape((3,1)) + mtx = mtx * scale_vec + # Resize image to match the kinect dimensions & new intrinsics + image = cv2.resize(image, (kinect.width, kinect.height)) + # Transform to the kinect camera matrix + transformed_image = cv2.undistort(np.asarray(image), mtx, dist, None, kinect.intrinsic_matrix) + return transformed_image + +""" +Helper function to extract the image frame number from the filename string. +""" +def split_filename(filename): + return int(filename.split("-")[-1].split(".")[0]) \ No newline at end of file