Initial Commit (tested training, testing, and TRT conversion)

2024-10-20 17:01:07 +08:00
parent 86d2f311f8
commit 5738088bae
221 changed files with 59249 additions and 6 deletions
--- a/.clang-format
+++ b/.clang-format
@@ -0,0 +1,31 @@
+---
+BasedOnStyle: Google
+UseTab: ForIndentation
+TabWidth: 4
+ColumnLimit: 150
+ConstructorInitializerAllOnOneLineOrOnePerLine: true
+ConstructorInitializerIndentWidth: 4
+ContinuationIndentWidth: 4
+Cpp11BracedListStyle: true
+IndentWidth: 4
+Language: Cpp
+MaxEmptyLinesToKeep: 2
+NamespaceIndentation: None
+SortUsingDeclarations: true
+SpaceAfterTemplateKeyword: false
+SpaceInEmptyParentheses: false
+Standard: Cpp11
+#圆括号之后,多行内容,进行对齐
+AlignAfterOpenBracket: Align
+#连续赋值时，对齐所有等号
+AlignConsecutiveAssignments: true
+#连续声明时，对齐所有声明的变量名
+#AlignConsecutiveDeclarations: true
+AllowShortLoopsOnASingleLine: false
+#对齐连续的尾随的注释
+AlignTrailingComments: true
+#将简单的语句块放到一个单行
+AllowShortBlocksOnASingleLine: false
+#if (a) return;放单行 属性：Never、WithoutElse没有else的可以放单行、OnlyFirstIf只有第一个if放单行、AllIfsAndElse总是把简短的if, else if和else语句放在同一行。
+AllowShortIfStatementsOnASingleLine: false
+---
--- a/.gitignore
+++ b/.gitignore
@@ -0,0 +1,33 @@
+# Python
+dist
+*.egg-info
+*.so
+__pycache__
+
+# Pycharm
+.idea/
+
+# VScode
+.vscode/
+.vscode/*
+**/.vscode/*
+!.vscode/extensions.json
+
+# General
+_build/
+.sass-cache
+_posts
+_site
+
+# saved
+run/saved/*
+!run/saved/YOPO_1
+run/yopo_trt.pth
+externals/
+
+#logs
+run/utils/*.csv
+
+# dataset
+/run/yopo_sim/*
+
--- a/25
+++ b/25
@@ -0,0 +1,25 @@
+MIT License
+
+Copyright (c) 2024, TJU-Aerial-Robotics 
+Tianjin University, China
+
+This work is developed based on Flightmare Simulator.
+The original LICENSE can be found in the LICENSE_FLIGHTMARE file.
+
+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.
--- a/22
+++ b/22
@@ -0,0 +1,22 @@
+MIT License
+
+Copyright (c) 2020 Robotics and Perception Group, 
+University of Zurich, Switzerland
+
+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.
--- a/README.md
+++ b/README.md
@@ -1,9 +1,226 @@
-# YOPO
-### Code for Paper: You Only Plan Once: A Learning-based One-stage Planner with Guidance Learning
-More details can be found in the video on [YouTube](https://youtu.be/m7u1MYIuIn4) or [bilibili](https://www.bilibili.com/video/BV15M4m1d7j5/).

-**The code is to be released...**
+# You Only Plan Once

-A simple demonstration with random goals clicked in rviz at the flight speed of 4m/s (the video is 3x speed-up and the map is unavailable).
+Paper: [You Only Plan Once: A Learning-Based One-Stage Planner With Guidance Learning](https://ieeexplore.ieee.org/document/10528860)

-![demo](click_in_rviz.gif)
+Video of this paper can be found: [YouTube](https://youtu.be/m7u1MYIuIn4), [bilibili](https://www.bilibili.com/video/BV15M4m1d7j5)
+
+Some realworld experiment: [YouTube](https://youtu.be/LHvtbKmTwvE), [bilibili](https://www.bilibili.com/video/BV1jBpve5EkP)
+
+## Introduction:
+We proposed a learning-based planner for autonomous navigation in obstacle-dense environments which intergrats (i) perception and mapping, (ii) front-end path searching, and (iii) back-end optimization of classical methods into a single network. 
+
+Considering the multi-modal nature of the navigation problem and to avoid local minima around initial values, our approach adopts a set of motion primitives as anchor to cover the searching space, and predicts the offsets and scores of primitives for further improvement (like the one-stage object detector YOLO). 
+
+Compared to giving expert demonstrations for imitation in imitation learning or exploring by trial-and-error in reinforcement learning, we directly back-propagate the numerical gradient (e.g. from ESDF) to the weights of neural network in the training process, which is realistic, accurate, and timely.
+
+<table>
+    <tr>
+        <td align="center" style="border: none;"><img src="docs/primitive_trajectories.png" alt="Fig1" style="width: 80%;"></td>
+        <td align="center" style="border: none;"><img src="docs/predicted_trajectories.png" alt="Fig2" style="width: 80%;"></td>
+		<td align="center" style="border: none;"><img src="docs/proposed_guidance_learning.png" alt="Fig3" style="width: 100%;"></td>
+    </tr>
+    <tr>
+        <td align="center" style="border: none;">primitive anchors</td>
+        <td align="center" style="border: none;">predicted traj and scores</td>
+		<td align="center" style="border: none;">learning method</td>
+    </tr>
+</table>
+
+## Acknowledgements: 
+
+This project is developed based on the open-source simulator [Flightmare](https://github.com/uzh-rpg/flightmare) and the gradient computation is modified from [grad_traj_optimization](https://github.com/HKUST-Aerial-Robotics/grad_traj_optimization), thanks for their excellent work!
+
+## Installation
+
+The project was tested with Ubuntu 20.04 and Jetson Orin/Xavier NX.
+
+**1. Flightmare Dependencies**
+
+```
+sudo apt-get update && apt-get install -y --no-install-recommends \
+   build-essential \
+   cmake \
+   libzmqpp-dev \
+   libopencv-dev \
+   libpcl-dev
+```
+
+**2. Add sourcing of your catkin workspace as FLIGHTMARE_PATH environment variable:**
+```
+# modify "~/YOPO" to your path
+echo "export FLIGHTMARE_PATH=~/YOPO" >> ~/.bashrc
+source ~/.bashrc
+```
+**3. Unity:** 
+
+Download the Flightmare Standalone uploaded by [uzh-rpg
+/agile_autonomy](https://zenodo.org/records/5517791/files/standalone.tar), extract it and put in the `flightrender` folder.
+It should looks like:
+```
+flightrender/
+├── CMakeLists.txt
+└── RPG_Flightmare/
+    ├── flightmare.x86_64
+    └── ...
+```
+
+**4. Create a conda virtual environment.**
+```
+conda create --name yopo python=3.8
+conda activate yopo
+
+conda install pytorch torchvision torchaudio pytorch-cuda=11.8 -c pytorch -c nvidia
+pip install opencv-python
+pip install gym stable-baselines3==1.5.0 
+pip install scipy scikit-build ruamel-yaml==0.17.21 numpy==1.22.3 tensorboard==2.9.1 empy catkin_pkg
+```
+**5. build the flightlib** 
+```
+conda activate yopo
+cd YOPO/flightlib/build
+cmake ..
+make -j8
+pip install .
+```
+**6. Some issues may arise when we test on different devices.**
+
+6.1. No module named 'flightpolicy' 
+
+```
+# modify "~/YOPO/flightpolicy" to your path
+echo "export PYTHONPATH=$PYTHONPATH:~/YOPO" >> ~/.bashrc
+source ~/.bashrc
+```
+6.2. No module named 'flightgym'
+```
+cd YOPO/flightlib/build
+pip install -e .
+```
+
+## Train the Policy
+**1. Data Collection:** For efficiency, we proactively collect dataset (images and states) by randomly initializing the drone's state (position and orientation). We randomly sample multiple velocities and accelerations for each image during the training process. The distribution of random sampled velocity is as `/docs/distribution_of_sampled_velocity.png`. It may take nearly 1 hour for collection with default dataset size but you only need to collect once. The data will be saved at `run/yopo_sim`.
+```
+cd ~/YOPO/run
+conda activate yopo
+python data_collection_simulation.py
+```
+
+**2. Training:** 
+```
+cd ~/YOPO/run
+conda activate yopo
+python run_yopo.py --train=1
+```
+It may take 2-3 hours to traing with default dataset size and training epoch. If everything goes well, the training log is as follows:
+
+<p align="center">
+    <img src="/docs/train_log.png" alt="train_log" />
+</p>
+
+Besides, you can refer to [quadrotor_env.yaml](./flightlib/configs/quadrotor_env.yaml), [vec_env.yaml](./flightlib/config/vec_env.yaml) and [traj_opt.yaml](./flightlib/config/traj_opt.yaml) for some modifications of quadrotor, environment, and trajectory optimization.
+
+## Test the Policy
+
+You can test the policy using pre-trained weights we provide at `run/saved/YOPO_1/Policy/epoch0_iter0.pth`. (For example, to use the weights of `YOPO_1/Policy/epoch2_iter3.pth`, you should use `--trial=1 --epoch=2 --iter=3`)
+
+**1. Test without dynamics model and controller (simple but not recommended).**
+
+
+```
+cd ~/YOPO/run
+conda activate yopo
+python run_yopo.py --train=0 --render=1 --trial=1 --epoch=0 --iter=0 
+```
+
+It will take a while for unity setup, and then you will see:
+<p align="center">
+    <img src="/docs/simple_demo.gif" alt="demo" />
+</p>
+
+**2. Test with dynamics model and controller (recommended).**
+
+**Prapare:** We did not use the built-in dynamics of Flightmare; instead, we used a ROS-based simulator and controller from [Fast Planner](https://github.com/HKUST-Aerial-Robotics/Fast-Planner). For your convenience, we have extracted only the relevant sections from the project, which you can refer to [UAV_Simulator](https://github.com/TJU-Aerial-Robotics/UAV_Simulator) for installation.
+
+Besides, we recommend using tmux & tmuxinator for terminal management.
+
+**2.1** Start the simulation environment with Unity and the ROS interface (It will take some time to load Unity and randomly generate a new environment)
+
+```
+cd ~/YOPO/flightrender/RPG_Flightmare
+./flightmare.x86_64
+```
+
+```
+cd ~/YOPO/flightlib/build
+./flightros_node
+```
+
+Besides, you can refer to [quadrotor_ros.yaml](./flightlib/configs/quadrotor_ros.yaml) for some modifications.
+
+**2.2** Start the dynamics simulation and controller of UAV
+```
+cd ~/UAV_Simulator
+source devel/setup.bash
+roslaunch so3_quadrotor_simulator simulator.launch
+```
+
+**2.3** Start the YOPO inference and the planner (the implementation of yopo_planner_node will be moved to test_yopo_ros.py in the future)
+
+```
+cd ~/YOPO/run
+conda activate yopo
+python test_yopo_ros.py --trial=1 --epoch=0 --iter=0
+```
+
+```
+cd ~/YOPO/flightlib/build
+./yopo_planner_node
+```
+
+**2.4** Visualization: start the RVIZ and publish the map.
+Then you can click the `2D Nav Goal` on RVIZ as the goal at will, just like the following GIF.
+```
+cd ~/YOPO/
+rviz -d yopo.rviz
+```
+
+```
+cd ~/YOPO/flightlib/build
+./map_visual_node
+```
+
+<p align="center">
+    <img src="/docs/click_in_rviz.gif" alt="click_in_rviz" />
+</p>
+
+
+## TensorRT Deployment
+We highly recommend using TensorRT for acceleration when flying in real world. It only takes 1ms for inference on NVIDIA Orin NX.
+
+**1. Prepera:**
+```
+conda activate yopo
+pip install -U nvidia-tensorrt --index-url https://pypi.ngc.nvidia.com
+
+git clone https://github.com/NVIDIA-AI-IOT/torch2trt
+cd torch2trt
+python setup.py install
+```
+**2. PyTorch model to TensorRT model**
+```
+cd ~/YOPO/
+conda activate yopo
+python yopo_trt_transfer.py --trial=1 --epoch=0 --iter=0
+```
+**3. TensorRT Inference**
+```
+cd ~/YOPO/
+conda activate yopo
+python test_yopo_ros.py --use_tensorrt=True
+```
+
+## Finally
+We are still working on improving and refactoring the code to improve the readability, reliability, and efficiency. For any technical issues, please feel free to contact me (lqzx1998@tju.edu.cn) 😀 We are very open and enjoy collaboration!
+
+If you find this work useful or interesting, please kindly give us a star ⭐; If our repository supports your academic projects, please cite our paper. Thank you!
--- a/docs/click_in_rviz.gif
+++ b/docs/click_in_rviz.gif
--- a/docs/depth_egb_stereo_image.png
+++ b/docs/depth_egb_stereo_image.png
--- a/docs/distribution_of_sampled_velocity.png
+++ b/docs/distribution_of_sampled_velocity.png
--- a/docs/executed_trajectory.png
+++ b/docs/executed_trajectory.png
--- a/docs/predicted_trajectories.png
+++ b/docs/predicted_trajectories.png
--- a/docs/primitive_trajectories.png
+++ b/docs/primitive_trajectories.png
--- a/docs/proposed_guidance_learning.png
+++ b/docs/proposed_guidance_learning.png
--- a/docs/simple_demo.gif
+++ b/docs/simple_demo.gif
--- a/docs/train_log.png
+++ b/docs/train_log.png
--- a/flightlib/CMakeLists.txt
+++ b/flightlib/CMakeLists.txt
@@ -0,0 +1,321 @@
+cmake_minimum_required(VERSION 3.0)
+
+project(flightlib VERSION 0.1.0)
+
+message(STATUS "======================  !Flightmare!  ======================")
+
+# ###############################################################################
+# Options
+# ###############################################################################
+option(ENABLE_FAST "Build with optimizations for speed" ON)
+option(ENABLE_BLAS "Build using BLAS and LAPACK libraries" OFF)
+option(ENABLE_PARALLEL "Build using openmp parallelization" ON)
+option(EIGEN_FROM_SYSTTEM "Use the system-provided Eigen" ON)
+set(
+	EIGEN_ALTERNATIVE "" CACHE STRING
+	"Path to alternative Eigen, autodownload if blank"
+)
+
+# ###############################################################################
+# Finding Dependencies
+# ###############################################################################
+
+# Eigen3
+message(STATUS "======> Setup Dependencies")
+
+if(EIGEN_FROM_SYSTTEM)
+	find_package(Eigen3 3.3.4 QUIET)
+
+	if(EIGEN3_FOUND)
+		message(STATUS "Using system provided Eigen.")
+		message(${EIGEN3_INCLUDE_DIR})
+	else()
+		message(STATUS "No sufficient Eigen version (3.3.4) found.")
+		message(STATUS "Restoring to download Eigen sources.")
+		include(cmake/eigen.cmake)
+	endif()
+elseif(EIGEN_ALTERNATIVE STREQUAL "")
+	include(cmake/eigen.cmake)
+else()
+	set(EIGEN_INCLUDE_DIR ${EIGEN_ALTERNATIVE})
+endif()
+
+message(STATUS "Eigen3 include dir: ${EIGEN3_INCLUDE_DIR}")
+
+# PCL (Many VTK errors are reported, but do not affect compilation)
+find_package(PCL 1.10 REQUIRED)
+message(STATUS "Using system provided PCL.")
+message(${PCL_INCLUDE_DIRS})
+include_directories(${PCL_INCLUDE_DIRS})
+link_directories(${PCL_LIBRARY_DIRS})
+add_definitions(${PCL_DEFINITIONS})
+
+# Including dependencies
+find_package(OpenCV REQUIRED)
+include_directories(${OpenCV_INCLUDE_DIRS})
+
+# Including dependencies
+include(cmake/pybind11.cmake)
+include(cmake/yaml.cmake)
+
+# Including OpenMP & CUDA
+find_package(OpenMP REQUIRED)
+find_package(CUDA REQUIRED)
+
+if(ENABLE_BLAS)
+	set(BLA_VENDOR "Generic")
+	find_package(BLAS REQUIRED)
+
+	if(BLAS_FOUND)
+		message(STATUS "Found BLAS: ${BLAS_LIBRARIES}")
+	else()
+		message(ERROR "Could not enable BLAS because BLAS was not found")
+	endif()
+
+	find_package(LAPACK REQUIRED)
+
+	if(LAPACK_FOUND)
+		message(STATUS "Found Lapack: ${LAPACK_LIBRARIES}")
+	else()
+		message(ERROR "Could not enable LAPACK because LAPACK was not found")
+	endif()
+endif()
+
+# Check for ccache
+if(NOT DEFINED CATKIN_DEVEL_PREFIX)
+	find_program(CCACHE_PROGRAM ccache)
+
+	if(CCACHE_PROGRAM)
+		set_property(GLOBAL PROPERTY RULE_LAUNCH_COMPILE "${CCACHE_PROGRAM}")
+	else()
+		message(INFO "Build time could be improved with ccache!")
+		message(INFO "sudo apt install ccache")
+	endif()
+endif()
+
+# ###############################################################################
+# Setup Compilation Flag
+# ###############################################################################
+message(STATUS "======> Setup Compilation ")
+
+add_definitions(-DEIGEN_STACK_ALLOCATION_LIMIT=1048576)
+include_directories(${EIGEN_INCLUDE_DIR} "tests")
+include_directories(${OpenCV_INCLUDE_DIRS})
+
+# Set default build type
+if(NOT EXISTS ${CMAKE_BINARY_DIR}/CMakeCache.txt)
+	if(NOT CMAKE_BUILD_TYPE)
+		set(CMAKE_BUILD_TYPE "Release" CACHE STRING "" FORCE)
+	endif()
+endif()
+
+cuda_select_nvcc_arch_flags(CUDA_ARCH_FLAGS)
+message("CUDA Architecture: ${CUDA_ARCH_FLAGS}")
+
+if(${CMAKE_BUILD_TYPE} STREQUAL Release)
+	SET(CUDA_NVCC_FLAGS "${CUDA_ARCH_FLAGS};-O3;-use_fast_math" CACHE STRING "nvcc flags" FORCE)
+	message("build CUDA with -O3")
+else()
+	message("build CUDA with Debug")
+	SET(CUDA_NVCC_FLAGS "-g ;-G ;${CUDA_ARCH_FLAGS}" CACHE STRING "nvcc flags" FORCE)
+	SET(CUDA_VERBOSE_BUILD ON CACHE BOOL "nvcc verbose" FORCE)
+endif()
+
+# Add c++ flags
+set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS} -fPIC -Wall -DNDEBUG  -fopenmp")
+set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS} -fPIC -Wall -g -fopenmp")
+set(CMAKE_CXX_STANDARD 17)
+
+# Architectural flags
+if("${CMAKE_HOST_SYSTEM_PROCESSOR}" STREQUAL "armv7l")
+	message(STATUS "Using ARMv7 optimized flags!")
+	set(CMAKE_CXX_ARCH_FLAGS " -Wno-psabi -march=armv7-a -mfpu=neon -mfloat-abi=hard -funsafe-math-optimizations")
+elseif("${CMAKE_HOST_SYSTEM_PROCESSOR}" STREQUAL "aarch64")
+	message(STATUS "Using ARM aarch64 optimized flags!")
+	set(CMAKE_CXX_ARCH_FLAGS " -Wno-psabi -march=armv8-a+crypto -mcpu=cortex-a57+crypto")
+else()
+	set(CMAKE_CXX_ARCH_FLAGS " -march=native")
+endif()
+
+# Optimized flags
+if(ENABLE_FAST)
+	message(STATUS "Enabling fast optimization flags!")
+	set(CMAKE_CXX_FAST_FLAGS " -Ofast")
+else()
+	set(CMAKE_CXX_FAST_FLAGS " -O0")
+endif()
+
+# BLAS Flags
+if(BLAS_FOUND AND LAPACK_FOUND)
+	message(STATUS "Enabling BLAS and LAPACK")
+	set(CMAKE_CXX_BLAS_FLAGS " -DEIGEN_USE_BLAS -DEIGEN_USE_LAPACK -DEIGEN_USE_LAPACKE")
+else()
+	set(CMAKE_CXX_BLAS_FLAGS "")
+endif()
+
+# Summarize Flags
+set(CMAKE_CXX_FLAGS_RELEASE
+	"${CMAKE_CXX_FLAGS_RELEASE} ${CMAKE_CXX_FAST_FLAGS} ${CMAKE_CXX_ARCH_FLAGS} ${CMAKE_CXX_PAR_FLAGS}")
+string(REPLACE "-DNDEBUG" "" CMAKE_CXX_FLAGS_RELWITHDEBINFO "${CMAKE_CXX_FLAGS_RELWITHDEBINFO}")
+message(STATUS "The activated CXX RELEASE configuration is:\n ${CMAKE_CXX_FLAGS_RELEASE}")
+message(STATUS "The activated CXX DEBUG configuration is:\n ${CMAKE_CXX_FLAGS_DEBUG}")
+
+# ###############################################################################
+# Specify Build Resources
+# ###############################################################################
+message(STATUS "======>  Setup Build  ")
+
+add_subdirectory(${PROJECT_SOURCE_DIR}/third_party/arc_utilities)
+add_subdirectory(${PROJECT_SOURCE_DIR}/third_party/sdf_tools)
+
+# Create file lists for flightlib source
+file(GLOB_RECURSE FLIGHTLIB_SOURCES
+	src/bridges/*.cpp
+	src/dynamics/*.cpp
+	src/objects/*.cpp
+	src/sensors/*.cpp
+	src/sensors/*.cu
+	src/envs/*.cpp
+	src/common/*.cpp
+	src/grad_traj_optimization/*.cpp
+)
+
+# Create file lists for flightlib_gym source
+file(GLOB_RECURSE FLIGHTLIB_GYM_SOURCES
+	src/wrapper/*.cpp
+)
+
+# ###############################################################################
+# Optional Catkin Build
+# ###############################################################################
+if(DEFINED CATKIN_DEVEL_PREFIX)
+	message(STATUS "======>  Building with -- catkin -- ")
+	include(cmake/catkin.cmake)
+	return()
+endif()
+
+find_package(catkin REQUIRED COMPONENTS
+	roscpp
+	sdf_tools
+	pcl_ros
+	pcl_conversions
+	sensor_msgs
+)
+
+# ###############################################################################
+# Setup Build
+# ###############################################################################
+
+# Library and Executables
+include_directories(include)
+include_directories(${PROJECT_SOURCE_DIR}/third_party/sdf_tools/include)
+include_directories(${PROJECT_SOURCE_DIR}/third_party/arc_utilities/include)
+
+# 1. The Training Lib
+if(NOT FLIGHTLIB_SOURCES)
+	set(LIBRARY_NAME)
+else()
+	# flightlib -CUDA
+	cuda_add_library(${PROJECT_NAME} ${FLIGHTLIB_SOURCES})
+	target_link_libraries(${PROJECT_NAME} # PRIVATE
+		${OpenCV_LIBRARIES}
+		${PCL_LIBRARIES}
+		${CUDA_curand_LIBRARY}
+		${CUDA_cublas_LIBRARY}
+		${CUDA_LIBRARIES}
+		${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PROJECT_SOURCE_DIR}
+		yaml-cpp
+		zmq
+		zmqpp
+		stdc++fs)
+	set(LIBRARY_NAME ${PROJECT_NAME})
+	set_target_properties(${PROJECT_NAME} PROPERTIES POSITION_INDEPENDENT_CODE TRUE)
+endif()
+
+if(FLIGHTLIB_GYM_SOURCES)
+	# flightlib_gym (python3 binding with Pybind11)
+	pybind11_add_module(flightgym MODULE
+		${FLIGHTLIB_GYM_SOURCES})
+
+	if(EIGEN3_FOUND)
+		target_include_directories(flightgym PRIVATE
+
+			# ${PROJECT_SOURCE_DIR}/externals/pybind11-src/include
+			${PYBIND11_INCLUDE_DIR}
+			${PROJECT_SOURCE_DIR}/include
+			${EIGEN3_INCLUDE_DIR})
+	else()
+		target_include_directories(flightgym PRIVATE
+
+			# ${PROJECT_SOURCE_DIR}/externals/pybind11-src/include
+			${PYBIND11_INCLUDE_DIR}
+			${PROJECT_SOURCE_DIR}/externals/eigen/eigen3 # pybind11 use #include <Eigen/Core>, however, flightmare use #include <eigen3/Eigen/Core>
+			${PROJECT_SOURCE_DIR}/include)
+	endif()
+
+	target_link_libraries(flightgym PRIVATE ${LIBRARY_NAME})
+endif()
+
+if(ENABLE_BLAS AND BLAS_FOUND AND LAPACK_FOUND)
+	message(STATUS "Linking standard BLAS ${BLAS_LIBRARIES}")
+	target_link_libraries(${LIBRARY_NAME}
+		${BLAS_LIBRARIES}
+		${LAPACK_LIBRARIES}
+		${LAPACKE_LIBRARIES}
+	)
+endif()
+
+# Build Other ROS PKG
+find_package(catkin REQUIRED COMPONENTS
+	tf
+	nav_msgs
+	cv_bridge
+)
+
+include_directories(
+	${catkin_INCLUDE_DIRS}
+	${PROJECT_SOURCE_DIR}/include
+)
+
+# 2. ROS Simulation Node
+add_executable(flightros_node
+	src/ros_nodes/flight_pilot.cpp
+	src/ros_nodes/flight_pilot_node.cpp
+)
+
+target_link_libraries(flightros_node
+	${LIBRARY_NAME}
+	${catkin_LIBRARIES}
+	${OpenCV_LIBRARIES}
+	zmqpp
+	stdc++fs
+)
+
+# 3. The Planner
+add_executable(yopo_planner_node
+	src/ros_nodes/yopo_planner_node.cpp
+)
+target_link_libraries(yopo_planner_node
+	${LIBRARY_NAME}
+	${catkin_LIBRARIES}
+)
+
+# 4. Tool: Trajectory Evaluation
+add_executable(traj_eval_node
+	src/ros_nodes/traj_eval_node.cpp
+)
+target_link_libraries(traj_eval_node
+	${LIBRARY_NAME}
+	${catkin_LIBRARIES}
+)
+
+# 5. Tool: Map Visualization
+add_executable(map_visual_node
+	src/ros_nodes/map_visual_node.cpp
+)
+target_link_libraries(map_visual_node
+	${LIBRARY_NAME}
+	${catkin_LIBRARIES}
+)
+
+message(STATUS "================  !Done. No more nightmare!  ================")
--- a/flightlib/build/.clang-format
+++ b/flightlib/build/.clang-format
@@ -0,0 +1,4 @@
+---
+DisableFormat: true
+SortIncludes: false
+---
--- a/flightlib/build/.gitignore
+++ b/flightlib/build/.gitignore
@@ -0,0 +1,6 @@
+# Ignore everything in this directory
+*
+# Except this file
+!.gitignore
+!.clang-format
+!setup.py
--- a/flightlib/build/setup.py
+++ b/flightlib/build/setup.py
@@ -0,0 +1,15 @@
+from setuptools import setup, Extension
+
+#
+setup(name='flightgym',
+      version='0.0.1',
+      author="Junjie Lu / Yunlong Song",
+      author_email='lqzx1998@tju.edu.cn / song@ifi.uzh.ch',
+      description="Flightmare: A Quadrotor Simulator",
+      long_description='This project is modified based on Flightmare by Yunlong Song, Thanks for his excellent work!',
+      packages=[''],
+      package_dir={'': './'},
+      package_data={'': ['flightgym.cpython-36m-x86_64-linux-gnu.so']},
+      zip_fase=True,
+      url=None,
+)
--- a/flightlib/cmake/catkin.cmake
+++ b/flightlib/cmake/catkin.cmake
@@ -0,0 +1,29 @@
+# Setup catkin simple
+find_package(catkin_simple REQUIRED)
+
+catkin_simple()
+
+add_definitions(-std=c++17)
+
+# Library and Executables
+cs_add_library(${PROJECT_NAME} ${FLIGHTLIB_SOURCES})
+target_link_libraries(${PROJECT_NAME}
+  ${catkin_LIBRARIES}
+  ${BLAS_LIBRARIES}
+  ${LAPACK_LIBRARIES}
+  ${LAPACKE_LIBRARIES}
+  ${OpenCV_LIBRARIES}
+  yaml-cpp
+  zmq
+  zmqpp
+)
+
+# Build tests
+if(BUILD_TESTS)
+  catkin_add_gtest(flightlib_tests ${FLIGHTLIB_TEST_SOURCES})
+  target_link_libraries(flightlib_tests ${PROJECT_NAME} gtest gtest_main)
+endif()
+
+# Finish catkin simple
+cs_install()
+cs_export()
--- a/flightlib/cmake/eigen.cmake
+++ b/flightlib/cmake/eigen.cmake
@@ -0,0 +1,25 @@
+# Download and unpack eigen at configure time
+message(STATUS "Getting Eigen...")
+
+configure_file(
+  cmake/eigen_download.cmake
+  ${PROJECT_SOURCE_DIR}/externals/eigen/CMakeLists.txt)
+
+execute_process(COMMAND ${CMAKE_COMMAND} -G "${CMAKE_GENERATOR}" .
+  RESULT_VARIABLE result
+  WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/externals/eigen
+  OUTPUT_QUIET)
+if(result)
+  message(FATAL_ERROR "Download of Eigen failed: ${result}")
+endif()
+execute_process(COMMAND ${CMAKE_COMMAND} --build .
+  RESULT_VARIABLE result
+  WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/externals/eigen
+  OUTPUT_QUIET)
+if(result)
+  message(FATAL_ERROR "Build step for eigen failed: ${result}")
+endif()
+
+message(STATUS "Eigen downloaded!")
+
+set(EIGEN_INCLUDE_DIR ${PROJECT_SOURCE_DIR}/externals/eigen)
--- a/flightlib/cmake/eigen_download.cmake
+++ b/flightlib/cmake/eigen_download.cmake
@@ -0,0 +1,15 @@
+cmake_minimum_required(VERSION 3.0.0)
+
+project(eigen-external)
+
+include(ExternalProject)
+ExternalProject_Add(eigen
+  GIT_REPOSITORY    https://gitlab.com/libeigen/eigen.git
+  GIT_TAG           3.3.4
+  SOURCE_DIR        "${PROJECT_SOURCE_DIR}/externals/eigen/eigen3"
+  CONFIGURE_COMMAND ""
+  BUILD_COMMAND     ""
+  INSTALL_COMMAND   ""
+  TEST_COMMAND      ""
+  UPDATE_DISCONNECTED ON
+)
--- a/flightlib/cmake/gtest.cmake
+++ b/flightlib/cmake/gtest.cmake
@@ -0,0 +1,37 @@
+# Download and unpack googletest at configure time
+message(STATUS "Getting gtests...")
+
+configure_file(cmake/gtest_download.cmake ${PROJECT_SOURCE_DIR}/externals/googletest-download/CMakeLists.txt)
+execute_process(COMMAND ${CMAKE_COMMAND} -G "${CMAKE_GENERATOR}" .
+  RESULT_VARIABLE result
+  WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/externals/googletest-download
+  OUTPUT_QUIET)
+if(result)
+  message(FATAL_ERROR "CMake step for googletest failed: ${result}")
+endif()
+execute_process(COMMAND ${CMAKE_COMMAND} --build .
+  RESULT_VARIABLE result
+  WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/externals/googletest-download
+  OUTPUT_QUIET)
+if(result)
+  message(FATAL_ERROR "Build step for googletest failed: ${result}")
+endif()
+
+message(STATUS "gtests downloaded!")
+
+# Prevent overriding the parent project's compiler/linker
+# settings on Windows
+set(gtest_force_shared_crt ON CACHE BOOL "" FORCE)
+
+# Add googletest directly to our build. This defines
+# the gtest and gtest_main targets.
+add_subdirectory(${PROJECT_SOURCE_DIR}/externals/googletest-src
+                 ${PROJECT_SOURCE_DIR}/externals/googletest-build
+                 EXCLUDE_FROM_ALL)
+
+# The gtest/gtest_main targets carry header search path
+# dependencies automatically when using CMake 2.8.11 or
+# later. Otherwise we have to add them here ourselves.
+if (CMAKE_VERSION VERSION_LESS 2.8.11)
+  include_directories("${gtest_SOURCE_DIR}/include")
+endif()
--- a/flightlib/cmake/gtest_download.cmake
+++ b/flightlib/cmake/gtest_download.cmake
@@ -0,0 +1,16 @@
+cmake_minimum_required(VERSION 3.0.0)
+
+project(googletest-download NONE)
+
+include(ExternalProject)
+ExternalProject_Add(googletest
+  GIT_REPOSITORY    https://github.com/google/googletest.git
+  GIT_TAG           main
+  SOURCE_DIR        "${PROJECT_SOURCE_DIR}/externals/googletest-src"
+  BINARY_DIR        "${PROJECT_SOURCE_DIR}/externals/googletest-build"
+  CONFIGURE_COMMAND ""
+  BUILD_COMMAND     ""
+  INSTALL_COMMAND   ""
+  TEST_COMMAND      ""
+  UPDATE_DISCONNECTED ON
+)
--- a/flightlib/cmake/pybind11.cmake
+++ b/flightlib/cmake/pybind11.cmake
@@ -0,0 +1,33 @@
+# Download and unpack pybind11 at configure time
+message(STATUS "Getting Pybind11...")
+
+# set(PYBIND11_PYTHON_VERSION 3.6)
+set(PYBIND11_PYTHON_VERSION ${PYTHON_VERSION_STRING})   
+
+configure_file(
+  cmake/pybind11_download.cmake
+  ${PROJECT_SOURCE_DIR}/externals/pybind11-download/CMakeLists.txt)
+
+execute_process(COMMAND ${CMAKE_COMMAND} -G "${CMAKE_GENERATOR}" . 
+  RESULT_VARIABLE result
+  WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/externals/pybind11-download
+  OUTPUT_QUIET)
+if(result)
+  message(FATAL_ERROR "Cmake Step for Pybind11 failed: ${result}")
+endif()
+
+execute_process(COMMAND ${CMAKE_COMMAND} --build .
+  RESULT_VARIABLE result
+  WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/externals/pybind11-download
+  OUTPUT_QUIET)
+if(result)
+  message(FATAL_ERROR "Build step for eigen failed: ${result}")
+endif()
+
+message(STATUS "Pybind11 downloaded!")
+
+set(PYBIND11_INCLUDE_DIR ${PROJECT_SOURCE_DIR}/externals/pybind11-src/include)
+add_subdirectory(${PROJECT_SOURCE_DIR}/externals/pybind11-src
+                 EXCLUDE_FROM_ALL)
+
+include_directories(SYSTEM "${PROJECT_SOURCE_DIR}/externals/pybind11-src/include")
--- a/flightlib/cmake/pybind11_download.cmake
+++ b/flightlib/cmake/pybind11_download.cmake
@@ -0,0 +1,17 @@
+cmake_minimum_required(VERSION 3.0.0)
+
+project(pybind11-download)
+
+include(ExternalProject)
+ExternalProject_Add(pybind11
+  GIT_REPOSITORY    https://cf.ghproxy.cc/https://github.com/pybind/pybind11
+  GIT_TAG           master 
+  SOURCE_DIR        "${PROJECT_SOURCE_DIR}/externals/pybind11-src"
+  BINARY_DIR        "${PROJECT_SOURCE_DIR}/externals/pybind11-bin"
+  CONFIGURE_COMMAND ""
+  CMAKE_ARGS        ""
+  BUILD_COMMAND     ""
+  INSTALL_COMMAND   ""
+  TEST_COMMAND      ""
+  UPDATE_DISCONNECTED ON
+)
--- a/flightlib/cmake/yaml.cmake
+++ b/flightlib/cmake/yaml.cmake
@@ -0,0 +1,33 @@
+# Download and unpack eigen at configure time
+message(STATUS "Getting yaml-cpp...")
+
+configure_file(
+  cmake/yaml_download.cmake
+  ${PROJECT_SOURCE_DIR}/externals/yaml-download/CMakeLists.txt)
+
+execute_process(COMMAND ${CMAKE_COMMAND} -G "${CMAKE_GENERATOR}" . 
+  RESULT_VARIABLE result
+  WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/externals/yaml-download
+  OUTPUT_QUIET
+  ERROR_QUIET)
+if(result)
+  message(FATAL_ERROR "CMake step for yaml-cpp failed: ${result}")
+endif()
+execute_process(COMMAND ${CMAKE_COMMAND} --build .
+  RESULT_VARIABLE result
+  WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/externals/yaml-download
+  OUTPUT_QUIET
+  ERROR_QUIET)
+if(result)
+  message(FATAL_ERROR "Build step for yaml failed: ${result}")
+endif()
+
+message(STATUS "Yaml downloaded!")
+
+add_subdirectory(${PROJECT_SOURCE_DIR}/externals/yaml-src
+                 ${PROJECT_SOURCE_DIR}/externals/yaml-build
+                 EXCLUDE_FROM_ALL)
+target_compile_options(yaml-cpp PUBLIC -fPIC -w)
+
+include_directories(SYSTEM "${PROJECT_SOURCE_DIR}/externals/yaml-src/include")
+link_directories("${PROJECT_SOURCE_DIR}/externals/yaml-build")
--- a/flightlib/cmake/yaml_download.cmake
+++ b/flightlib/cmake/yaml_download.cmake
@@ -0,0 +1,18 @@
+cmake_minimum_required(VERSION 3.0.0)
+
+project(yaml-download)
+
+include(ExternalProject)
+ExternalProject_Add(yaml
+  GIT_REPOSITORY    https://cf.ghproxy.cc/https://github.com/jbeder/yaml-cpp
+  GIT_TAG           master
+  SOURCE_DIR        "${PROJECT_SOURCE_DIR}/externals/yaml-src"
+  BINARY_DIR        "${PROJECT_SOURCE_DIR}/externals/yaml-bin"
+  CONFIGURE_COMMAND ""
+  CMAKE_ARGS        "-DBUILD_TESTING=OFF -DYAML_CPP_INSTALL=OFF"
+  CMAKE_CACHE_ARGS  -DBUILD_TESTING:BOOL=OFF -DYAML_CPP_INSTALLC:BOOL=ON
+  BUILD_COMMAND     ""
+  INSTALL_COMMAND   ""
+  TEST_COMMAND      ""
+  UPDATE_DISCONNECTED ON
+)
--- a/flightlib/configs/quadrotor_env.yaml
+++ b/flightlib/configs/quadrotor_env.yaml
@@ -0,0 +1,26 @@
+quadrotor_env:
+  collect_data: yes       # yes in Data Collection (random init state); no in Imitation Learning and Testing
+  bounding_box: [60.0, 60.0, 2.0]     # spawn quadrotor within this bounding box
+  bounding_box_origin: [-10, 20, 2.5]
+  sim_dt: 0.1             # sim_dt in imitation learning and testing
+
+data_collection:
+  roll_var: 0.01
+  pitch_var: 0.01
+
+rgb_camera_left:
+  on: yes 
+  t_BC: [0.0, 0.0, 0.1]   # translational vector of the camera with repect to the body frame
+  r_BC: [0.0, 0.0, -90]   # rotational angle (roll, pitch, yaw) of the camera in degree.
+  width: 160
+  height: 90
+  fov: 90.0               # Horizontal FOV
+  enable_depth: yes 
+  enable_segmentation: no # not used
+  enable_opticalflow: no  # not used
+
+# Enable Stereo depth when rgb_camera_right on.
+rgb_camera_right:
+  on: no
+  t_BC: [0.0, -0.2, 0.1]  # translational vector of the camera with repect to the body frame
+  r_BC: [0.0, 0.0, -90]   # rotational angle (roll, pitch, yaw) of the camera in degree.
--- a/flightlib/configs/quadrotor_ros.yaml
+++ b/flightlib/configs/quadrotor_ros.yaml
@@ -0,0 +1,30 @@
+main_loop_freq: 30
+unity_render: yes
+scene_id: 4               # 0 wasteland, 1 japanese street, 4 emptyforest in standalone (SR)
+odom_topic: /juliett/ground_truth/odom
+quad_size: 0.1
+ply_path: "/flightrender/RPG_Flightmare/pointcloud_data/"
+
+rgb_camera_left:
+  on: yes 
+  t_BC: [0.0, 0.0, 0.1]   # translational vector of the camera with repect to the body frame
+  r_BC: [0.0, -5.0, -90]  # rotational angle (roll, pitch, yaw) of the camera in degree.
+  width: 160
+  height: 90
+  fov: 90.0               # Horizontal FOV
+  enable_depth: yes
+  enable_segmentation: no
+  enable_opticalflow: no
+
+# enable stereo depth when rgb_camera_right on (If use, please use larger resolution (e.g., 640x360)).
+rgb_camera_right:
+  on: no
+  t_BC: [0.0, -0.2, 0.1]  # translational vector of the camera with repect to the body frame
+  r_BC: [0.0, -5.0, -90]  # rotational angle (roll, pitch, yaw) of the camera in degree.
+
+unity:
+  spawn_trees: true
+  avg_tree_spacing: 4.0
+  bounding_box: [80.0, 80.0, 11.0]    # spawn objects within this bounding box
+  bounding_box_origin: [-10, 20, 2.5]
+  pointcloud_resolution: 0.2
--- a/flightlib/configs/traj_opt.yaml
+++ b/flightlib/configs/traj_opt.yaml
@@ -0,0 +1,64 @@
+vel_max: 6.0
+# segment_time = 2 * radio / vel_max
+
+# IMPORTANT PARAM: weight of penalties (6m/s)
+ws: 0.00004
+wc: 0.001
+wl: 0.00
+wg: 0.0002
+
+#ws: 0.00004
+#wc: 0.001
+#wl: 0.02
+#wg: 0.0001
+
+# trajectory and primitive parma
+horizon_num: 5
+vertical_num: 3
+horizon_camera_fov: 90.0
+vertical_camera_fov: 60.0
+horizon_anchor_fov: 30
+vertical_anchor_fov: 30
+goal_length: 10
+radio_range: 4.0    # planning horizon: 2 * radio_range
+vel_fov: 90.0       # not use currently
+radio_num: 1        # 1 just ok
+vel_num: 1          # 1 just ok
+vel_prefile: 0.0    # 0 just ok
+
+# For data efficiency, we randomly sample multiple vel and acc for each depth image with the following the distribution.
+# values at normalized speed (actual speed can be denormalized by multiplying v_multiple)
+# 单位数据倍数： v_multiple = 0.5 * v_max = radio / time
+# v数据的均值： v_mean = v_multiple * v_mean_unit
+# v数据的方差： v_var = v_multiple^2 * v_var_unit
+# a数据的均值： v_mean = v_multiple^2 * a_mean_unit
+# a数据的方差： v_var = v_multiple^4 * a_var_unit
+vx_mean_unit: 1.5
+vy_mean_unit: 0.0
+vz_mean_unit: 0.0
+vx_var_unit: 0.15
+vy_var_unit: 0.45
+vz_var_unit: 0.1
+ax_mean_unit: 0.0
+ay_mean_unit: 0.0
+az_mean_unit: 0.0
+ax_var_unit: 0.0278
+ay_var_unit: 0.05
+az_var_unit: 0.0278
+
+# penalties
+alpha: 10.0
+d0: 1.2
+r: 0.6
+
+alphav: 2.0
+v0: 3.5
+rv: 1.5
+
+alphaa: 2.0
+a0: 3.5
+ra: 1.5
+
+# deprecated weight
+wv: 0.0
+wa: 0.0
--- a/flightlib/configs/vec_env.yaml
+++ b/flightlib/configs/vec_env.yaml
@@ -0,0 +1,15 @@
+env:
+  seed: 1
+  scene_id: 4   # 0 wasteland, 1 japanese street, 4 emptyforest in SR standalone
+  num_envs: 16  # Important: same to batch size!
+  num_threads: 16
+  dataset_path: "/run/yopo_sim/"
+  ply_path: "/run/yopo_sim/"
+
+unity:
+  spawn_trees: true
+  avg_tree_spacing: 4
+  # larger than the position to generate the drone to ensure the completeness of the point cloud in edge.
+  bounding_box: [80.0, 80.0, 11]      # spawn objects within this bounding box
+  bounding_box_origin: [-10, 20, 2.5]
+  pointcloud_resolution: 0.2
--- a/flightlib/include/flightlib/bridges/unity_bridge.hpp
+++ b/flightlib/include/flightlib/bridges/unity_bridge.hpp
@@ -0,0 +1,115 @@
+//
+// This bridge was originally from FlightGoggles.
+// We made several changes on top of it.
+//
+#pragma once
+
+// std libs
+#include <opencv2/imgproc/types_c.h>
+#include <sys/time.h>
+#include <unistd.h>
+
+#include <experimental/filesystem>
+#include <fstream>
+#include <map>
+#include <random>
+#include <string>
+#include <thread>
+#include <unordered_map>
+
+// Include ZMQ bindings for communications with Unity.
+#include <zmqpp/zmqpp.hpp>
+
+// flightlib
+#include "flightlib/bridges/unity_message_types.hpp"
+#include "flightlib/common/logger.hpp"
+#include "flightlib/common/math.hpp"
+#include "flightlib/common/quad_state.hpp"
+#include "flightlib/common/types.hpp"
+#include "flightlib/objects/quadrotor.hpp"
+#include "flightlib/objects/static_object.hpp"
+#include "flightlib/objects/unity_camera.hpp"
+#include "flightlib/sensors/rgb_camera.hpp"
+
+using json = nlohmann::json;
+
+namespace flightlib {
+
+class UnityBridge {
+   public:
+	// constructor & destructor
+	UnityBridge();
+	~UnityBridge() {};
+
+	// connect function
+	bool initializeConnections(void);
+	bool connectUnity(const SceneID scene_id);
+	bool disconnectUnity(void);
+
+	// public get functions
+	bool getRender(const FrameID frame_id);
+	bool handleOutput(FrameID &frameID);
+	void refreshUnity(FrameID id);
+	void generatePointcloud(const Ref<Vector<3>> &min_corner, const Ref<Vector<3>> &max_corner, int ply_idx, std::string path, SceneID scene_id,
+	                        Scalar pc_resolution_ = 0.2);
+
+	// public set functions
+	bool setScene(const SceneID &scene_id);
+
+	// add object
+	bool addQuadrotor(std::shared_ptr<Quadrotor> quad);
+	bool addCamera(std::shared_ptr<UnityCamera> unity_camera);
+	bool addStaticObject(std::shared_ptr<StaticObject> static_object);
+
+	// public auxiliary functions
+	inline void setPubPort(const std::string &pub_port) { pub_port_ = pub_port; };
+	inline void setSubPort(const std::string &sub_port) { sub_port_ = sub_port; };
+	// create unity bridge
+	static std::shared_ptr<UnityBridge> getInstance(void) {
+		static std::shared_ptr<UnityBridge> bridge_ptr = std::make_shared<UnityBridge>();
+		return bridge_ptr;
+	};
+
+	// add tree
+	bool placeTrees(TreeMessage_t &tree_msg);
+	void rmTrees();
+	void pointCloudGenerator(PointCloudMessage_t &pcd_msg);
+	bool spawnTrees(Ref<Vector<3>> bounding_box_, Ref<Vector<3>> bounding_box_origin_, Scalar avg_tree_spacing_);
+
+   private:
+	template<typename T>
+	std::vector<double> convertToDoubleVector(const std::vector<T> &input);
+	//
+	SettingsMessage_t settings_;
+	PubMessage_t pub_msg_;
+	Logger logger_{"UnityBridge"};
+
+	std::vector<std::shared_ptr<Quadrotor>> unity_quadrotors_;
+	std::vector<std::shared_ptr<RGBCamera>> rgb_cameras_;
+	std::vector<std::shared_ptr<StaticObject>> static_objects_;
+
+	// ZMQ variables and functions
+	std::string client_address_;
+	std::string pub_port_;
+	std::string sub_port_;
+	zmqpp::context context_;
+	zmqpp::socket pub_{context_, zmqpp::socket_type::publish};
+	zmqpp::socket sub_{context_, zmqpp::socket_type::subscribe};
+	bool sendInitialSettings(void);
+	bool handleSettings(void);
+
+	// timing variables
+	int64_t num_frames_;
+	int64_t last_downloaded_utime_;
+	int64_t last_download_debug_utime_;
+	int64_t u_packet_latency_;
+
+	// axuiliary variables
+	const Scalar unity_connection_time_out_{60.0};
+	bool unity_ready_{false};
+
+	// Used for trees
+	std::random_device random_device_;
+	std::default_random_engine generator_;
+};
+}  // namespace flightlib
--- a/flightlib/include/flightlib/bridges/unity_message_types.hpp
+++ b/flightlib/include/flightlib/bridges/unity_message_types.hpp
@@ -0,0 +1,309 @@
+//
+// This bridge message types was originally from FlightGoggles.
+// We made several changes on top of it.
+//
+#pragma once
+
+// std
+#include <string>
+
+// opencv
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/opencv.hpp>
+
+// flightlib
+#include "flightlib/common/types.hpp"
+#include "flightlib/json/json.hpp"
+
+using json = nlohmann::json;
+
+namespace flightlib {
+
+enum UnityScene {
+  WAREHOUSE = 0,
+  NATUREFOREST = 3,
+  SCENE_EMPTYFOREST = 4,
+  SCENE_GRANDCANYON = 5,
+  SCENE_WASTELAND = 6,
+  SCENE_JAPANESE = 7,
+  // total number of environment
+  SceneNum = 6
+};
+
+struct Camera_t {
+  std::string ID;
+  // frame Metadata
+  // int64_t ntime; // deprecated
+  int channels{3};
+  int width{1024};
+  int height{768};
+  double fov{70.0f};
+  double depth_scale{0.20};  // 0.xx corresponds to xx cm resolution
+  // metadata
+  bool is_depth{false};
+  int output_index{0};
+  //
+  std::vector<std::string> post_processing;
+  // Transformation matrix from camera to vehicle body 4 x 4
+  // use 1-D vector for json convention
+  std::vector<double> T_BC{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+                           0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+};
+
+struct Lidar_t {
+  std::string ID;
+  int num_beams{10};
+  double max_distance{10.0};
+  double start_scan_angle{-M_PI / 2};
+  double end_scan_angle{M_PI / 2};
+  // Transformation matrix from lidar to vehicle body 4 x 4
+  // use 1-D vector for json convention
+  std::vector<double> T_BS{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+                           0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+};
+
+// Unity Vechicle, e.g., quadrotor
+struct Vehicle_t {
+  std::string ID;
+  // unity coordinate system left-handed, y up
+  std::vector<double> position{0.0, 0.0, 0.0};
+  // unity quaternion (x, y, z, w)
+  std::vector<double> rotation{0.0, 0.0, 0.0, 1.0};
+  std::vector<double> size{1.0, 1.0, 1.0};  // scale
+  // sensors attached on the vehicle
+  std::vector<Camera_t> cameras;
+  std::vector<Lidar_t> lidars;
+  // collision check
+  bool has_collision_check = true;
+};
+
+// Unity object, e.g., gate, light, etc...
+struct Object_t {
+  std::string ID;
+  std::string prefab_ID;
+  // unity coordinate system left hand, y up
+  std::vector<double> position{0.0, 0.0, 0.0};
+  // unity quaternion (x, y, z, w)
+  std::vector<double> rotation{0.0, 0.0, 0.0, 1.0};
+  std::vector<double> size{1.0, 1.0, 1.0};  // scale
+};
+
+struct SettingsMessage_t {
+  // scene/render settings
+  size_t scene_id = UnityScene::WAREHOUSE;
+
+  //
+  std::vector<Vehicle_t> vehicles;
+  std::vector<Object_t> objects;
+};
+
+struct PubMessage_t {
+  FrameID ntime{0};
+  std::vector<Vehicle_t> vehicles;
+  std::vector<Object_t> objects;
+};
+
+//
+struct Sub_Vehicle_t {
+  bool collision;
+  std::vector<double> lidar_ranges;
+};
+
+struct SubMessage_t {
+  //
+  FrameID ntime{0};
+  //
+  std::vector<Sub_Vehicle_t> sub_vehicles;
+};
+
+struct PointCloudMessage_t {
+  int scene_id{0};
+  std::vector<double> bounding_box_scale{100.0, 100.0, 100.0};
+  std::vector<double> bounding_box_origin{0.0, 0.0, 0.0};
+  double resolution_z{0.1};
+  double ground_z_limit{0.2};
+  double resolution_above_ground{0.1};
+  double resolution_below_ground{0.1};
+  std::string path{"point_clouds_data/"};
+  std::string file_name{"default"};
+  double unity_ground_offset{0.3};
+};
+
+struct TreeMessage_t {
+  std::vector<double> bounding_area{25.0, 25.0};
+  std::vector<double> bounding_origin{0.0, 0.0};
+  double density{4.0};
+  int seed{-1};
+};
+
+struct ObjectMessage_t {
+  std::vector<std::string> name = {"Cube"};  // Cube, Sphere, Cylinder
+  std::vector<double> bounding_area{25.0, 25.0, 25.0};
+  std::vector<double> bounding_origin{0.0, 0.0, 0.0};
+  double density{4.0};
+  double rand_size{0.0};
+  std::vector<double> scale_min{
+      0.1,
+      0.3,
+      2.0,
+  };
+  std::vector<double> scale_max{1.0, 0.3, 10.0};
+
+  std::vector<double> rot_min{0.0, 0.0, 0.0};
+  std::vector<double> rot_max{360.0, 360.0, 360.0};
+  int seed{-1};
+};
+
+struct FixRatioObjectMessage_t {
+  std::string name = "Cube";  // Cube, Sphere, Cylinder
+  std::vector<double> bounding_area{25.0, 25.0};
+  std::vector<double> bounding_origin{0.0, 0.0};
+  double density{4.0};
+  double scale_min{0.1};
+  double scale_max{2.0};
+  double rot_min{0.0};  // rotation around z-axix
+  double rot_max{360.0};
+  int seed{-1};
+};
+
+struct LightMessage_t {
+  double red{0};
+  double green{0};
+  double blue{0};
+  double intensity{1.0};
+};
+
+/*********************
+ * JSON constructors *
+ *********************/
+// Camera_t
+inline void to_json(json &j, const Camera_t &o) {
+  j = json{{"ID", o.ID},
+           {"channels", o.channels},
+           {"width", o.width},
+           {"height", o.height},
+           {"fov", o.fov},
+           {"T_BC", o.T_BC},
+           {"isDepth", o.is_depth},
+           {"post_processing", o.post_processing},
+           {"depthScale", o.depth_scale},
+           {"outputIndex", o.output_index}};
+}
+
+// Lidar
+inline void to_json(json &j, const Lidar_t &o) {
+  j = json{{"ID", o.ID},
+           {"num_beams", o.num_beams},
+           {"max_distance", o.max_distance},
+           {"start_angle", o.start_scan_angle},
+           {"end_angle", o.end_scan_angle},
+           {"T_BS", o.T_BS}};
+}
+// Vehicle_t
+inline void to_json(json &j, const Vehicle_t &o) {
+  j = json{{"ID", o.ID},
+           {"position", o.position},
+           {"rotation", o.rotation},
+           {"size", o.size},
+           {"cameras", o.cameras},
+           {"lidars", o.lidars},
+           {"hasCollisionCheck", o.has_collision_check}};
+}
+
+// Object_t
+inline void to_json(json &j, const Object_t &o) {
+  j = json{{"ID", o.ID},
+           {"prefabID", o.prefab_ID},
+           {"position", o.position},
+           {"rotation", o.rotation},
+           {"size", o.size}};
+}
+
+// Setting messages, pub to unity
+inline void to_json(json &j, const SettingsMessage_t &o) {
+  j = json{{"scene_id", o.scene_id},
+           {"vehicles", o.vehicles},
+           {"objects", o.objects}};
+}
+
+
+// Publish messages to unity
+inline void to_json(json &j, const PubMessage_t &o) {
+  j = json{
+      {"ntime", o.ntime}, {"vehicles", o.vehicles}, {"objects", o.objects}};
+}
+
+// Publish messages to unity
+inline void from_json(const json &j, Sub_Vehicle_t &o) {
+  o.collision = j.at("collision").get<bool>();
+  o.lidar_ranges = j.at("lidar_ranges").get<std::vector<double>>();
+}
+
+// json to our sub message data type
+// note: pub_vechicles is defined in Unity which corresponding
+// to our sub_vehicles in ROS.
+inline void from_json(const json &j, SubMessage_t &o) {
+  o.ntime = j.at("ntime").get<uint64_t>();
+  o.sub_vehicles = j.at("pub_vehicles").get<std::vector<Sub_Vehicle_t>>();
+}
+
+inline void to_json(json &j, const PointCloudMessage_t &o) {
+  j = json{{"scene_id", o.scene_id},
+           {"bounding_box_scale", o.bounding_box_scale},
+           {"bounding_box_origin", o.bounding_box_origin},
+           {"resolution_z", o.resolution_z},
+           {"ground_z_limit", o.ground_z_limit},
+           {"resolution_above_ground", o.resolution_above_ground},
+           {"resolution_below_ground", o.resolution_below_ground},
+           {"path", o.path},
+           {"file_name", o.file_name},
+           {"unity_ground_offset", o.unity_ground_offset}};
+}
+
+inline void to_json(json &j, const TreeMessage_t &o) {
+  j = json{{"bounding_area", o.bounding_area},
+           {"bounding_origin", o.bounding_origin},
+           {"density", o.density},
+           {"seed", o.seed}};
+}
+
+inline void to_json(json &j, const ObjectMessage_t &o) {
+  j = json{{"bounding_area", o.bounding_area},
+           {"bounding_origin", o.bounding_origin},
+           {"density", o.density},
+           {"rand_size", o.rand_size},
+           {"seed", o.seed},
+           {"name", o.name},
+           {"scale_min", o.scale_min},
+           {"scale_max", o.scale_max},
+           {"rot_min", o.rot_min},
+           {"rot_max", o.rot_max}};
+}
+
+inline void to_json(json &j, const FixRatioObjectMessage_t &o) {
+  j = json{{"bounding_area", o.bounding_area},
+           {"bounding_origin", o.bounding_origin},
+           {"density", o.density},
+           {"seed", o.seed},
+           {"name", o.name},
+           {"scale_min", o.scale_min},
+           {"scale_max", o.scale_max},
+           {"rot_min", o.rot_min},
+           {"rot_max", o.rot_max}};
+}
+
+inline void to_json(json &j, const LightMessage_t &o) {
+  j = json{{"red", o.red},
+           {"green", o.green},
+           {"blue", o.blue},
+           {"intensity", o.intensity}};
+}
+
+// Struct for outputting parsed received messages to handler functions
+struct RenderMessage_t {
+  SubMessage_t sub_msg;
+  std::vector<cv::Mat> images;
+};
+
+}  // namespace flightlib
--- a/flightlib/include/flightlib/common/command.hpp
+++ b/flightlib/include/flightlib/common/command.hpp
@@ -0,0 +1,37 @@
+
+#pragma once
+
+#include <cmath>
+
+#include "flightlib/common/types.hpp"
+
+namespace flightlib {
+
+struct Command {
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  Command();
+
+  Command(const Scalar t, const Scalar thrust, const Vector<3>& omega);
+
+  Command(const Scalar t, const Vector<4>& thrusts);
+
+  bool valid() const;
+  bool isSingleRotorThrusts() const;
+  bool isRatesThrust() const;
+
+  /// time in [s]
+  Scalar t{NAN};
+
+  /// Collective mass-normalized thrust in [m/s^2]
+  Scalar collective_thrust{NAN};
+
+  /// Bodyrates in [rad/s]
+  Vector<3> omega{NAN, NAN, NAN};
+
+  /// Single rotor thrusts in [N]
+  Vector<4> thrusts{NAN, NAN, NAN, NAN};
+
+};
+
+}  // namespace flightlib
--- a/flightlib/include/flightlib/common/integrator_base.hpp
+++ b/flightlib/include/flightlib/common/integrator_base.hpp
@@ -0,0 +1,32 @@
+#pragma once
+
+#include <functional>
+
+#include "flightlib/common/quad_state.hpp"
+#include "flightlib/common/types.hpp"
+
+namespace flightlib {
+
+class IntegratorBase {
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ public:
+  using DynamicsFunction =
+    std::function<bool(const Ref<const Vector<>>, Ref<Vector<>>)>;
+  IntegratorBase(DynamicsFunction function, const Scalar dt_max = 1e-3);
+
+  bool integrate(const QuadState& initial, QuadState* const final) const;
+
+  bool integrate(const Ref<const Vector<>> initial, const Scalar dt,
+                 Ref<Vector<>> final) const;
+
+  virtual bool step(const Ref<const Vector<>> initial, const Scalar dt,
+                    Ref<Vector<>> final) const = 0;
+
+  Scalar dtMax() const;
+
+ protected:
+  DynamicsFunction dynamics_;
+  Scalar dt_max_;
+};
+
+}  // namespace flightlib
--- a/flightlib/include/flightlib/common/integrator_euler.hpp
+++ b/flightlib/include/flightlib/common/integrator_euler.hpp
@@ -0,0 +1,17 @@
+#pragma once
+
+#include "flightlib/common/integrator_base.hpp"
+
+namespace flightlib {
+
+class IntegratorEuler : public IntegratorBase {
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  using IntegratorBase::DynamicsFunction;
+  using IntegratorBase::IntegratorBase;
+
+  bool step(const Ref<const Vector<>> initial, const Scalar dt,
+            Ref<Vector<>> final) const;
+};
+
+}  // namespace flightlib
--- a/flightlib/include/flightlib/common/integrator_rk4.hpp
+++ b/flightlib/include/flightlib/common/integrator_rk4.hpp
@@ -0,0 +1,17 @@
+#pragma once
+
+#include "flightlib/common/integrator_base.hpp"
+
+namespace flightlib {
+
+class IntegratorRK4 : public IntegratorBase {
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  using IntegratorBase::DynamicsFunction;
+  using IntegratorBase::IntegratorBase;
+
+  bool step(const Ref<const Vector<>> initial, const Scalar dt,
+            Ref<Vector<>> final) const;
+};
+
+}  // namespace flightlib
--- a/flightlib/include/flightlib/common/logger.hpp
+++ b/flightlib/include/flightlib/common/logger.hpp
@@ -0,0 +1,102 @@
+// """credit: Philipp Foehn """
+#pragma once
+
+#include <cstdio>
+#include <fstream>
+#include <iostream>
+#include <string>
+
+namespace flightlib {
+
+class Logger {
+ public:
+  Logger(const std::string& name, const bool color = true);
+  Logger(const std::string& name, const std::string& filename);
+  ~Logger();
+
+  inline std::streamsize precision(const std::streamsize n);
+  inline void scientific(const bool on = true);
+
+  template<class... Args>
+  void info(const std::string& message, const Args&... args) const;
+  void info(const std::string& message) const;
+
+  template<class... Args>
+  void warn(const std::string& message, const Args&... args) const;
+  void warn(const std::string& message) const;
+
+  template<class... Args>
+  void error(const std::string& message, const Args&... args) const;
+  void error(const std::string& message) const;
+
+  template<class... Args>
+  void fatal(const std::string& message, const Args&... args) const;
+  void fatal(const std::string& message) const;
+
+  template<typename T>
+  std::ostream& operator<<(const T& printable) const;
+
+  static constexpr int MAX_CHARS = 256;
+
+ private:
+  static constexpr int DEFAULT_PRECISION = 3;
+  static constexpr int NAME_PADDING = 15;
+  static constexpr char RESET[] = "\033[0m";
+  static constexpr char RED[] = "\033[31m";
+  static constexpr char YELLOW[] = "\033[33m";
+  static constexpr char INFO[] = "Info:    ";
+  static constexpr char WARN[] = "Warning: ";
+  static constexpr char ERROR[] = "Error:   ";
+  static constexpr char FATAL[] = "Fatal:   ";
+  //
+  std::string name_;
+  mutable std::ostream sink_;
+  const bool colored_;
+};
+
+template<class... Args>
+void Logger::info(const std::string& message, const Args&... args) const {
+  char buf[MAX_CHARS];
+  const int n = std::snprintf(buf, MAX_CHARS, message.c_str(), args...);
+  if (n >= 0 && n < MAX_CHARS)
+    info(buf);
+  else
+    error("=== Logging error ===\n");
+}
+
+template<class... Args>
+void Logger::warn(const std::string& message, const Args&... args) const {
+  char buf[MAX_CHARS];
+  const int n = std::snprintf(buf, MAX_CHARS, message.c_str(), args...);
+  if (n >= 0 && n < MAX_CHARS)
+    warn(buf);
+  else
+    error("=== Logging error ===\n");
+}
+
+template<class... Args>
+void Logger::error(const std::string& message, const Args&... args) const {
+  char buf[MAX_CHARS];
+  const int n = std::snprintf(buf, MAX_CHARS, message.c_str(), args...);
+  if (n >= 0 && n < MAX_CHARS)
+    error(buf);
+  else
+    error("=== Logging error ===\n");
+}
+
+template<class... Args>
+void Logger::fatal(const std::string& message, const Args&... args) const {
+  char buf[MAX_CHARS];
+  const int n = std::snprintf(buf, MAX_CHARS, message.c_str(), args...);
+  if (n >= 0 && n < MAX_CHARS)
+    fatal(buf);
+  else
+    fatal("=== Logging error ===\n");
+}
+
+template<typename T>
+std::ostream& Logger::operator<<(const T& printable) const {
+  return sink_ << name_ << printable;
+}
+
+}  // namespace flightlib
--- a/flightlib/include/flightlib/common/math.hpp
+++ b/flightlib/include/flightlib/common/math.hpp
@@ -0,0 +1,54 @@
+#pragma once
+
+#include "flightlib/common/types.hpp"
+
+namespace flightlib {
+
+Matrix<3, 3> skew(const Vector<3>& v);
+
+Matrix<4, 4> Q_left(const Quaternion& q);
+
+Matrix<4, 4> Q_right(const Quaternion& q);
+
+Matrix<4, 3> qFromQeJacobian(const Quaternion& q);
+
+Matrix<4, 4> qConjugateJacobian();
+
+Matrix<3, 3> qeRotJacobian(const Quaternion& q, const Matrix<3, 1>& t);
+
+Matrix<3, 3> qeInvRotJacobian(const Quaternion& q, const Matrix<3, 1>& t);
+
+void matrixToTripletList(const SparseMatrix& matrix,
+                         std::vector<SparseTriplet>* const list,
+                         const int row_offset = 0, const int col_offset = 0);
+
+void matrixToTripletList(const Matrix<>& matrix,
+                         std::vector<SparseTriplet>* const list,
+                         const int row_offset = 0, const int col_offset = 0);
+
+void insert(const SparseMatrix& from, SparseMatrix* const into,
+            const int row_offset = 0, const int col_offset = 0);
+
+void insert(const Matrix<>& from, SparseMatrix* const into,
+            const int row_offset = 0, const int col_offset = 0);
+
+void insert(const Matrix<>& from, Matrix<>* const into,
+            const int row_offset = 0, const int col_offset = 0);
+
+void quaternionToEuler(const Quaternion& quat, Ref<Vector<3>> euler);
+
+std::vector<Scalar> transformationRos2Unity(const Matrix<4, 4>& ros_tran_mat);
+
+std::vector<Scalar> positionRos2Unity(const Vector<3>& ros_pos_vec);
+
+std::vector<Scalar> quaternionRos2Unity(const Quaternion& ros_quat);
+
+std::vector<Scalar> scalarRos2Unity(const Vector<3>& ros_scale);
+
+void get_euler_from_R(Vector<3> &e, const Matrix<3,3> &R);
+float calculate_yaw(float yaw_cur, float yaw_ref, float sim_t);
+
+double wrapMinusPiToPi(const double angle);
+double wrapZeroToTwoPi(const double angle);
+
+}  // namespace flightlib
--- a/flightlib/include/flightlib/common/parameter_base.hpp
+++ b/flightlib/include/flightlib/common/parameter_base.hpp
@@ -0,0 +1,21 @@
+#pragma once
+
+
+#include <yaml-cpp/yaml.h>
+
+namespace flightlib {
+
+struct ParameterBase {
+  ParameterBase();
+  ParameterBase(const std::string& cfg_path);
+  ParameterBase(const YAML::Node& cfg_node);
+
+  virtual ~ParameterBase();
+
+  virtual bool valid() = 0;
+  virtual bool loadParam() = 0;
+
+  YAML::Node cfg_node_;
+};
+
+}  // namespace flightlib
--- a/flightlib/include/flightlib/common/pend_state.hpp
+++ b/flightlib/include/flightlib/common/pend_state.hpp
@@ -0,0 +1,92 @@
+#pragma once
+
+#include <memory>
+
+#include "flightlib/common/types.hpp"
+
+namespace flightlib {
+
+struct PendState {
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  enum IDX : int {
+    POS = 0,
+    POSX = 0,
+    POSY = 1,
+    POSZ = 2,
+    NPOS = 3,
+    ATT = 3,
+    ATTW = 3,
+    ATTX = 4,
+    ATTY = 5,
+    ATTZ = 6,
+    NATT = 4,
+    VEL = 7,
+    VELX = 7,
+    VELY = 8,
+    VELZ = 9,
+    NVEL = 3,
+    OME = 10,
+    OMEX = 10,
+    OMEY = 11,
+    OMEZ = 12,
+    NOME = 3,
+    ACC = 13,
+    ACCX = 13,
+    ACCY = 14,
+    ACCZ = 15,
+    NACC = 3,
+    TAU = 16,
+    TAUX = 16,
+    TAUY = 17,
+    TAUZ = 18,
+    NTAU = 3,
+    BOME = 19,
+    BOMEX = 19,
+    BOMEY = 20,
+    BOMEZ = 21,
+    NBOME = 3,
+    BACC = 22,
+    BACCX = 22,
+    BACCY = 23,
+    BACCZ = 24,
+    NBACC = 3,
+    SIZE = 25,
+    DYN = 19
+  };
+
+  PendState();
+  PendState(const Vector<IDX::SIZE>& x, const Scalar t = NAN);
+  PendState(const PendState& state);
+  ~PendState();
+
+  inline static int size() { return SIZE; }
+  Quaternion q() const;
+  void q(const Quaternion quaternion);
+  Matrix<3, 3> R() const;
+  void setZero();
+
+  inline bool valid() const { return x.allFinite() && std::isfinite(t); }
+
+  Vector<IDX::SIZE> x = Vector<IDX::SIZE>::Constant(NAN);
+  Scalar t{NAN};
+
+  Ref<Vector<3>> p{x.segment<IDX::NPOS>(IDX::POS)};
+  Ref<Vector<4>> qx{x.segment<IDX::NATT>(IDX::ATT)};
+  Ref<Vector<3>> v{x.segment<IDX::NVEL>(IDX::VEL)};
+  Ref<Vector<3>> w{x.segment<IDX::NOME>(IDX::OME)};
+  Ref<Vector<3>> a{x.segment<IDX::NACC>(IDX::ACC)};
+  Ref<Vector<3>> tau{x.segment<IDX::NTAU>(IDX::TAU)};
+  Ref<Vector<3>> bw{x.segment<IDX::NBOME>(IDX::BOME)};
+  Ref<Vector<3>> ba{x.segment<IDX::NBACC>(IDX::BACC)};
+
+  bool operator==(const PendState& rhs) const {
+    return t == rhs.t && x.isApprox(rhs.x, 1e-5);
+  }
+
+  friend std::ostream& operator<<(std::ostream& os, const PendState& state);
+};
+
+using PS = PendState;
+
+}  // namespace flightlib
--- a/flightlib/include/flightlib/common/quad_state.hpp
+++ b/flightlib/include/flightlib/common/quad_state.hpp
@@ -0,0 +1,109 @@
+#pragma once
+
+#include <memory>
+
+#include "flightlib/common/types.hpp"
+
+namespace flightlib {
+
+struct QuadState {
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  enum IDX : int {
+    // position
+    POS = 0,
+    POSX = 0,
+    POSY = 1,
+    POSZ = 2,
+    NPOS = 3,
+    // quaternion
+    ATT = 3,
+    ATTW = 3,
+    ATTX = 4,
+    ATTY = 5,
+    ATTZ = 6,
+    NATT = 4,
+    // linear velocity
+    VEL = 7,
+    VELX = 7,
+    VELY = 8,
+    VELZ = 9,
+    NVEL = 3,
+    // body rate
+    OME = 10,
+    OMEX = 10,
+    OMEY = 11,
+    OMEZ = 12,
+    NOME = 3,
+    // linear acceleration
+    ACC = 13,
+    ACCX = 13,
+    ACCY = 14,
+    ACCZ = 15,
+    NACC = 3,
+    // body-torque
+    TAU = 16,
+    TAUX = 16,
+    TAUY = 17,
+    TAUZ = 18,
+    NTAU = 3,
+    //
+    BOME = 19,
+    BOMEX = 19,
+    BOMEY = 20,
+    BOMEZ = 21,
+    NBOME = 3,
+    //
+    BACC = 22,
+    BACCX = 22,
+    BACCY = 23,
+    BACCZ = 24,
+    NBACC = 3,
+    //
+    SIZE = 25,
+    NDYM = 19
+  };
+
+  QuadState();
+  QuadState(const Vector<IDX::SIZE>& x, const Scalar t = NAN);
+  QuadState(const QuadState& state);
+  ~QuadState();
+
+  inline static int size() { return SIZE; }
+  Quaternion q() const;
+  void q(const Quaternion quaternion);
+  Matrix<3, 3> R() const;
+  void setZero();
+
+  inline bool valid() const { return x.allFinite() && std::isfinite(t); }
+
+  Vector<IDX::SIZE> x = Vector<IDX::SIZE>::Constant(NAN);
+  Scalar t{NAN};
+
+  // position
+  Ref<Vector<3>> p{x.segment<IDX::NPOS>(IDX::POS)};
+  // orientation (quaternion)
+  Ref<Vector<4>> qx{x.segment<IDX::NATT>(IDX::ATT)};
+  // linear velocity
+  Ref<Vector<3>> v{x.segment<IDX::NVEL>(IDX::VEL)};
+  // angular velocity
+  Ref<Vector<3>> w{x.segment<IDX::NOME>(IDX::OME)};
+  // linear accleration
+  Ref<Vector<3>> a{x.segment<IDX::NACC>(IDX::ACC)};
+  // body torque
+  Ref<Vector<3>> tau{x.segment<IDX::NTAU>(IDX::TAU)};
+  //
+  Ref<Vector<3>> bw{x.segment<IDX::NBOME>(IDX::BOME)};
+  //
+  Ref<Vector<3>> ba{x.segment<IDX::NBACC>(IDX::BACC)};
+
+  bool operator==(const QuadState& rhs) const {
+    return t == rhs.t && x.isApprox(rhs.x, 1e-5);
+  }
+
+  friend std::ostream& operator<<(std::ostream& os, const QuadState& state);
+};
+
+using QS = QuadState;
+
+}  // namespace flightlib
--- a/flightlib/include/flightlib/common/timer.hpp
+++ b/flightlib/include/flightlib/common/timer.hpp
@@ -0,0 +1,87 @@
+#pragma once
+
+#include <chrono>
+#include <iostream>
+#include <string>
+
+#include "flightlib/common/types.hpp"
+
+namespace flightlib {
+
+/*
+ * Timer class to perform runtime analytics.
+ *
+ * This timer class provides a simple solution to time code.
+ * Simply construct a timer and call it's `tic()` and `toc()` functions to time
+ * code. It is intended to be used to time multiple calls of a function and not
+ * only reports the `last()` timing, but also statistics such as the `mean()`,
+ * `min()`, `max()` time, the `count()` of calls to the timer , and even
+ * standard deviation `std()`.
+ *
+ * The constructor can take a name for the timer (like "update") and a name for
+ * the module (like "Filter").
+ * After construction it can be `reset()` if needed.
+ *
+ * A simple way to get the timing and stats is `std::cout << timer;` which can
+ * output to arbitrary streams, overloading the stream operator,
+ * or `print()` which always prints to console.
+ *
+ */
+class Timer {
+ public:
+  Timer(const std::string name = "", const std::string module = "");
+  Timer(const Timer& other);
+  ~Timer() {}
+
+  /// Start the timer.
+  void tic();
+
+  /// Stops timer, calculates timing, also tics again.
+  Scalar toc();
+
+  /// Reset saved timings and calls;
+  void reset();
+
+  // Accessors
+  Scalar operator()() const;
+  Scalar mean() const;
+  Scalar last() const;
+  Scalar min() const;
+  Scalar max() const;
+  Scalar std() const;
+  int count() const;
+
+  /// Custom stream operator for outputs.
+  friend std::ostream& operator<<(std::ostream& os, const Timer& timer);
+
+  /// Print timing information to console.
+  void print() const;
+
+ private:
+  std::string name_, module_;
+  using TimePoint = std::chrono::high_resolution_clock::time_point;
+  TimePoint t_start_;
+
+  // Initialize timing to impossible values.
+  Scalar timing_mean_;
+  Scalar timing_last_;
+  Scalar timing_S_;
+  Scalar timing_min_;
+  Scalar timing_max_;
+
+  int n_samples_;
+};
+
+/*
+ * Simple class to time scopes.
+ *
+ * This effectively instantiates a timer and calls `tic()` in its constructor
+ * and `toc()` and ` print()` in its destructor.
+ */
+class ScopedTimer : public Timer {
+ public:
+  ScopedTimer(const std::string name = "", const std::string module = "");
+  ~ScopedTimer();
+};
+
+}  // namespace flightlib
--- a/flightlib/include/flightlib/common/types.hpp
+++ b/flightlib/include/flightlib/common/types.hpp
@@ -0,0 +1,101 @@
+#pragma once
+
+#include <eigen3/Eigen/Eigen>
+
+namespace flightlib {
+
+// ------------ General Stuff-------------
+
+// Define the scalar type used.
+using Scalar = float;  // numpy float32
+
+// Define frame id for unity
+using FrameID = uint64_t;
+
+using USecs = uint64_t;
+
+// Define frame id for unity
+using SceneID = size_t;
+
+
+// ------------ Eigen Stuff-------------
+// Define `Dynamic` matrix size.
+static constexpr int Dynamic = Eigen::Dynamic;
+
+// Using shorthand for 'DepthImg<ros, cols>' with scalar type.
+template<int rows = Dynamic, int cols = Dynamic>
+using DepthImgMatrix = Eigen::Matrix<float_t, rows, cols>;
+
+// Using shorthand for `DepthImgVector<ros>` with scalar type.
+template<int rows = Dynamic>
+using DepthImgVector = DepthImgMatrix<rows, 1>;
+
+// Using shorthand for 'DepthImgRowMajor<ros, cols>' with scalar type.
+template<int rows = Dynamic, int cols = Dynamic>
+using DepthImgMatrixRowMajor =
+  Eigen::Matrix<float_t, rows, cols, Eigen::RowMajor>;
+
+// Using shorthand for 'ImgMatrix<ros, cols>' with scalar type.
+template<int rows = Dynamic, int cols = Dynamic>
+using ImgMatrix = Eigen::Matrix<uint8_t, rows, cols>;
+
+// Using shorthand for `ImgVector<ros>` with scalar type.
+template<int rows = Dynamic>
+using ImgVector = ImgMatrix<rows, 1>;
+
+// Using shorthand for 'ImgMatrixRowMajor<ros, cols>' with scalar type.
+template<int rows = Dynamic, int cols = Dynamic>
+using ImgMatrixRowMajor = Eigen::Matrix<uint8_t, rows, cols, Eigen::RowMajor>;
+
+// Using shorthand for `Matrix<rows, cols>` with scalar type.
+template<int rows = Dynamic, int cols = Dynamic>
+using Matrix = Eigen::Matrix<Scalar, rows, cols>;
+
+// Using shorthand for `Matrix<rows, cols>` with scalar type.
+template<int rows = Dynamic, int cols = Dynamic>
+using MatrixRowMajor = Eigen::Matrix<Scalar, rows, cols, Eigen::RowMajor>;
+
+// Using shorthand for `Vector<rows>` with scalar type.
+template<int rows = Dynamic>
+using Vector = Matrix<rows, 1>;
+
+// Vector bool
+template<int rows = Dynamic>
+using BoolVector = Eigen::Matrix<bool, -1, 1>;
+
+// Vector int
+template<int rows = Dynamic>
+using IntVector = Eigen::Matrix<int, -1, 1>;
+
+// Using shorthand for `Array<rows, cols>` with scalar type.
+template<int rows = Dynamic, int cols = rows>
+using Array = Eigen::Array<Scalar, rows, cols>;
+
+// Using `SparseMatrix` with type.
+using SparseMatrix = Eigen::SparseMatrix<Scalar>;
+
+// Using SparseTriplet with type.
+using SparseTriplet = Eigen::Triplet<Scalar>;
+
+// Using `Quaternion` with type.
+using Quaternion = Eigen::Quaternion<Scalar>;
+
+// Using 'AngleAxis' with type
+using AngleAxis = Eigen::AngleAxis<Scalar>;
+
+// Using `Ref` for modifier references.
+template<class Derived>
+using Ref = Eigen::Ref<Derived>;
+
+// // Using `ConstRef` for constant references.
+template<class Derived>
+using ConstRef = const Eigen::Ref<const Derived>;
+
+// Using `Map`.
+template<class Derived>
+using Map = Eigen::Map<Derived>;
+
+static constexpr Scalar Gz = -9.81;
+const Vector<3> GVEC{0.0, 0.0, Gz};
+
+}  // namespace flightlib
--- a/flightlib/include/flightlib/controller/PositionCommand.h
+++ b/flightlib/include/flightlib/controller/PositionCommand.h
@@ -0,0 +1,400 @@
+// Generated by gencpp from file quadrotor_msgs/PositionCommand.msg
+// DO NOT EDIT!
+
+
+#ifndef QUADROTOR_MSGS_MESSAGE_POSITIONCOMMAND_H
+#define QUADROTOR_MSGS_MESSAGE_POSITIONCOMMAND_H
+
+
+#include <string>
+#include <vector>
+#include <map>
+
+#include <ros/types.h>
+#include <ros/serialization.h>
+#include <ros/builtin_message_traits.h>
+#include <ros/message_operations.h>
+
+#include <std_msgs/Header.h>
+#include <geometry_msgs/Point.h>
+#include <geometry_msgs/Vector3.h>
+#include <geometry_msgs/Vector3.h>
+
+namespace quadrotor_msgs
+{
+template <class ContainerAllocator>
+struct PositionCommand_
+{
+  typedef PositionCommand_<ContainerAllocator> Type;
+
+  PositionCommand_()
+    : header()
+    , position()
+    , velocity()
+    , acceleration()
+    , yaw(0.0)
+    , yaw_dot(0.0)
+    , kx()
+    , kv()
+    , trajectory_id(0)
+    , trajectory_flag(0)  {
+      kx.assign(0.0);
+
+      kv.assign(0.0);
+  }
+  PositionCommand_(const ContainerAllocator& _alloc)
+    : header(_alloc)
+    , position(_alloc)
+    , velocity(_alloc)
+    , acceleration(_alloc)
+    , yaw(0.0)
+    , yaw_dot(0.0)
+    , kx()
+    , kv()
+    , trajectory_id(0)
+    , trajectory_flag(0)  {
+  (void)_alloc;
+      kx.assign(0.0);
+
+      kv.assign(0.0);
+  }
+
+
+
+   typedef  ::std_msgs::Header_<ContainerAllocator>  _header_type;
+  _header_type header;
+
+   typedef  ::geometry_msgs::Point_<ContainerAllocator>  _position_type;
+  _position_type position;
+
+   typedef  ::geometry_msgs::Vector3_<ContainerAllocator>  _velocity_type;
+  _velocity_type velocity;
+
+   typedef  ::geometry_msgs::Vector3_<ContainerAllocator>  _acceleration_type;
+  _acceleration_type acceleration;
+
+   typedef double _yaw_type;
+  _yaw_type yaw;
+
+   typedef double _yaw_dot_type;
+  _yaw_dot_type yaw_dot;
+
+   typedef boost::array<double, 3>  _kx_type;
+  _kx_type kx;
+
+   typedef boost::array<double, 3>  _kv_type;
+  _kv_type kv;
+
+   typedef uint32_t _trajectory_id_type;
+  _trajectory_id_type trajectory_id;
+
+   typedef uint8_t _trajectory_flag_type;
+  _trajectory_flag_type trajectory_flag;
+
+
+
+// reducing the odds to have name collisions with Windows.h 
+#if defined(_WIN32) && defined(TRAJECTORY_STATUS_EMPTY)
+  #undef TRAJECTORY_STATUS_EMPTY
+#endif
+#if defined(_WIN32) && defined(TRAJECTORY_STATUS_READY)
+  #undef TRAJECTORY_STATUS_READY
+#endif
+#if defined(_WIN32) && defined(TRAJECTORY_STATUS_COMPLETED)
+  #undef TRAJECTORY_STATUS_COMPLETED
+#endif
+#if defined(_WIN32) && defined(TRAJECTROY_STATUS_ABORT)
+  #undef TRAJECTROY_STATUS_ABORT
+#endif
+#if defined(_WIN32) && defined(TRAJECTORY_STATUS_ILLEGAL_START)
+  #undef TRAJECTORY_STATUS_ILLEGAL_START
+#endif
+#if defined(_WIN32) && defined(TRAJECTORY_STATUS_ILLEGAL_FINAL)
+  #undef TRAJECTORY_STATUS_ILLEGAL_FINAL
+#endif
+#if defined(_WIN32) && defined(TRAJECTORY_STATUS_IMPOSSIBLE)
+  #undef TRAJECTORY_STATUS_IMPOSSIBLE
+#endif
+
+  enum {
+    TRAJECTORY_STATUS_EMPTY = 0u,
+    TRAJECTORY_STATUS_READY = 1u,
+    TRAJECTORY_STATUS_COMPLETED = 3u,
+    TRAJECTROY_STATUS_ABORT = 4u,
+    TRAJECTORY_STATUS_ILLEGAL_START = 5u,
+    TRAJECTORY_STATUS_ILLEGAL_FINAL = 6u,
+    TRAJECTORY_STATUS_IMPOSSIBLE = 7u,
+  };
+
+
+  typedef boost::shared_ptr< ::quadrotor_msgs::PositionCommand_<ContainerAllocator> > Ptr;
+  typedef boost::shared_ptr< ::quadrotor_msgs::PositionCommand_<ContainerAllocator> const> ConstPtr;
+
+}; // struct PositionCommand_
+
+typedef ::quadrotor_msgs::PositionCommand_<std::allocator<void> > PositionCommand;
+
+typedef boost::shared_ptr< ::quadrotor_msgs::PositionCommand > PositionCommandPtr;
+typedef boost::shared_ptr< ::quadrotor_msgs::PositionCommand const> PositionCommandConstPtr;
+
+// constants requiring out of line definition
+
+   
+
+   
+
+   
+
+   
+
+   
+
+   
+
+   
+
+
+
+template<typename ContainerAllocator>
+std::ostream& operator<<(std::ostream& s, const ::quadrotor_msgs::PositionCommand_<ContainerAllocator> & v)
+{
+ros::message_operations::Printer< ::quadrotor_msgs::PositionCommand_<ContainerAllocator> >::stream(s, "", v);
+return s;
+}
+
+
+template<typename ContainerAllocator1, typename ContainerAllocator2>
+bool operator==(const ::quadrotor_msgs::PositionCommand_<ContainerAllocator1> & lhs, const ::quadrotor_msgs::PositionCommand_<ContainerAllocator2> & rhs)
+{
+  return lhs.header == rhs.header &&
+    lhs.position == rhs.position &&
+    lhs.velocity == rhs.velocity &&
+    lhs.acceleration == rhs.acceleration &&
+    lhs.yaw == rhs.yaw &&
+    lhs.yaw_dot == rhs.yaw_dot &&
+    lhs.kx == rhs.kx &&
+    lhs.kv == rhs.kv &&
+    lhs.trajectory_id == rhs.trajectory_id &&
+    lhs.trajectory_flag == rhs.trajectory_flag;
+}
+
+template<typename ContainerAllocator1, typename ContainerAllocator2>
+bool operator!=(const ::quadrotor_msgs::PositionCommand_<ContainerAllocator1> & lhs, const ::quadrotor_msgs::PositionCommand_<ContainerAllocator2> & rhs)
+{
+  return !(lhs == rhs);
+}
+
+
+} // namespace quadrotor_msgs
+
+namespace ros
+{
+namespace message_traits
+{
+
+
+
+
+
+template <class ContainerAllocator>
+struct IsMessage< ::quadrotor_msgs::PositionCommand_<ContainerAllocator> >
+  : TrueType
+  { };
+
+template <class ContainerAllocator>
+struct IsMessage< ::quadrotor_msgs::PositionCommand_<ContainerAllocator> const>
+  : TrueType
+  { };
+
+template <class ContainerAllocator>
+struct IsFixedSize< ::quadrotor_msgs::PositionCommand_<ContainerAllocator> >
+  : FalseType
+  { };
+
+template <class ContainerAllocator>
+struct IsFixedSize< ::quadrotor_msgs::PositionCommand_<ContainerAllocator> const>
+  : FalseType
+  { };
+
+template <class ContainerAllocator>
+struct HasHeader< ::quadrotor_msgs::PositionCommand_<ContainerAllocator> >
+  : TrueType
+  { };
+
+template <class ContainerAllocator>
+struct HasHeader< ::quadrotor_msgs::PositionCommand_<ContainerAllocator> const>
+  : TrueType
+  { };
+
+
+template<class ContainerAllocator>
+struct MD5Sum< ::quadrotor_msgs::PositionCommand_<ContainerAllocator> >
+{
+  static const char* value()
+  {
+    return "4712f0609ca29a79af79a35ca3e3967a";
+  }
+
+  static const char* value(const ::quadrotor_msgs::PositionCommand_<ContainerAllocator>&) { return value(); }
+  static const uint64_t static_value1 = 0x4712f0609ca29a79ULL;
+  static const uint64_t static_value2 = 0xaf79a35ca3e3967aULL;
+};
+
+template<class ContainerAllocator>
+struct DataType< ::quadrotor_msgs::PositionCommand_<ContainerAllocator> >
+{
+  static const char* value()
+  {
+    return "quadrotor_msgs/PositionCommand";
+  }
+
+  static const char* value(const ::quadrotor_msgs::PositionCommand_<ContainerAllocator>&) { return value(); }
+};
+
+template<class ContainerAllocator>
+struct Definition< ::quadrotor_msgs::PositionCommand_<ContainerAllocator> >
+{
+  static const char* value()
+  {
+    return "Header header\n"
+"geometry_msgs/Point position\n"
+"geometry_msgs/Vector3 velocity\n"
+"geometry_msgs/Vector3 acceleration\n"
+"float64 yaw\n"
+"float64 yaw_dot\n"
+"float64[3] kx\n"
+"float64[3] kv \n"
+"\n"
+"uint32 trajectory_id\n"
+"\n"
+"uint8 TRAJECTORY_STATUS_EMPTY = 0\n"
+"uint8 TRAJECTORY_STATUS_READY = 1\n"
+"uint8 TRAJECTORY_STATUS_COMPLETED = 3\n"
+"uint8 TRAJECTROY_STATUS_ABORT = 4\n"
+"uint8 TRAJECTORY_STATUS_ILLEGAL_START = 5\n"
+"uint8 TRAJECTORY_STATUS_ILLEGAL_FINAL = 6\n"
+"uint8 TRAJECTORY_STATUS_IMPOSSIBLE = 7\n"
+"\n"
+"# Its ID number will start from 1, allowing you comparing it with 0.\n"
+"uint8 trajectory_flag\n"
+"\n"
+"================================================================================\n"
+"MSG: std_msgs/Header\n"
+"# Standard metadata for higher-level stamped data types.\n"
+"# This is generally used to communicate timestamped data \n"
+"# in a particular coordinate frame.\n"
+"# \n"
+"# sequence ID: consecutively increasing ID \n"
+"uint32 seq\n"
+"#Two-integer timestamp that is expressed as:\n"
+"# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n"
+"# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n"
+"# time-handling sugar is provided by the client library\n"
+"time stamp\n"
+"#Frame this data is associated with\n"
+"string frame_id\n"
+"\n"
+"================================================================================\n"
+"MSG: geometry_msgs/Point\n"
+"# This contains the position of a point in free space\n"
+"float64 x\n"
+"float64 y\n"
+"float64 z\n"
+"\n"
+"================================================================================\n"
+"MSG: geometry_msgs/Vector3\n"
+"# This represents a vector in free space. \n"
+"# It is only meant to represent a direction. Therefore, it does not\n"
+"# make sense to apply a translation to it (e.g., when applying a \n"
+"# generic rigid transformation to a Vector3, tf2 will only apply the\n"
+"# rotation). If you want your data to be translatable too, use the\n"
+"# geometry_msgs/Point message instead.\n"
+"\n"
+"float64 x\n"
+"float64 y\n"
+"float64 z\n"
+;
+  }
+
+  static const char* value(const ::quadrotor_msgs::PositionCommand_<ContainerAllocator>&) { return value(); }
+};
+
+} // namespace message_traits
+} // namespace ros
+
+namespace ros
+{
+namespace serialization
+{
+
+  template<class ContainerAllocator> struct Serializer< ::quadrotor_msgs::PositionCommand_<ContainerAllocator> >
+  {
+    template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
+    {
+      stream.next(m.header);
+      stream.next(m.position);
+      stream.next(m.velocity);
+      stream.next(m.acceleration);
+      stream.next(m.yaw);
+      stream.next(m.yaw_dot);
+      stream.next(m.kx);
+      stream.next(m.kv);
+      stream.next(m.trajectory_id);
+      stream.next(m.trajectory_flag);
+    }
+
+    ROS_DECLARE_ALLINONE_SERIALIZER
+  }; // struct PositionCommand_
+
+} // namespace serialization
+} // namespace ros
+
+namespace ros
+{
+namespace message_operations
+{
+
+template<class ContainerAllocator>
+struct Printer< ::quadrotor_msgs::PositionCommand_<ContainerAllocator> >
+{
+  template<typename Stream> static void stream(Stream& s, const std::string& indent, const ::quadrotor_msgs::PositionCommand_<ContainerAllocator>& v)
+  {
+    s << indent << "header: ";
+    s << std::endl;
+    Printer< ::std_msgs::Header_<ContainerAllocator> >::stream(s, indent + "  ", v.header);
+    s << indent << "position: ";
+    s << std::endl;
+    Printer< ::geometry_msgs::Point_<ContainerAllocator> >::stream(s, indent + "  ", v.position);
+    s << indent << "velocity: ";
+    s << std::endl;
+    Printer< ::geometry_msgs::Vector3_<ContainerAllocator> >::stream(s, indent + "  ", v.velocity);
+    s << indent << "acceleration: ";
+    s << std::endl;
+    Printer< ::geometry_msgs::Vector3_<ContainerAllocator> >::stream(s, indent + "  ", v.acceleration);
+    s << indent << "yaw: ";
+    Printer<double>::stream(s, indent + "  ", v.yaw);
+    s << indent << "yaw_dot: ";
+    Printer<double>::stream(s, indent + "  ", v.yaw_dot);
+    s << indent << "kx[]" << std::endl;
+    for (size_t i = 0; i < v.kx.size(); ++i)
+    {
+      s << indent << "  kx[" << i << "]: ";
+      Printer<double>::stream(s, indent + "  ", v.kx[i]);
+    }
+    s << indent << "kv[]" << std::endl;
+    for (size_t i = 0; i < v.kv.size(); ++i)
+    {
+      s << indent << "  kv[" << i << "]: ";
+      Printer<double>::stream(s, indent + "  ", v.kv[i]);
+    }
+    s << indent << "trajectory_id: ";
+    Printer<uint32_t>::stream(s, indent + "  ", v.trajectory_id);
+    s << indent << "trajectory_flag: ";
+    Printer<uint8_t>::stream(s, indent + "  ", v.trajectory_flag);
+  }
+};
+
+} // namespace message_operations
+} // namespace ros
+
+#endif // QUADROTOR_MSGS_MESSAGE_POSITIONCOMMAND_H
--- a/flightlib/include/flightlib/controller/ctrl_ref.h
+++ b/flightlib/include/flightlib/controller/ctrl_ref.h
@@ -0,0 +1,290 @@
+// Generated by gencpp from file quad_pos_ctrl/ctrl_ref.msg
+// DO NOT EDIT!
+
+
+#ifndef QUAD_POS_CTRL_MESSAGE_CTRL_REF_H
+#define QUAD_POS_CTRL_MESSAGE_CTRL_REF_H
+
+
+#include <string>
+#include <vector>
+#include <map>
+
+#include <ros/types.h>
+#include <ros/serialization.h>
+#include <ros/builtin_message_traits.h>
+#include <ros/message_operations.h>
+
+#include <std_msgs/Header.h>
+
+namespace quad_pos_ctrl
+{
+template <class ContainerAllocator>
+struct ctrl_ref_
+{
+  typedef ctrl_ref_<ContainerAllocator> Type;
+
+  ctrl_ref_()
+    : header()
+    , pos_ref()
+    , vel_ref()
+    , acc_ref()
+    , yaw_ref(0.0)
+    , ref_mask(0)  {
+      pos_ref.assign(0.0);
+
+      vel_ref.assign(0.0);
+
+      acc_ref.assign(0.0);
+  }
+  ctrl_ref_(const ContainerAllocator& _alloc)
+    : header(_alloc)
+    , pos_ref()
+    , vel_ref()
+    , acc_ref()
+    , yaw_ref(0.0)
+    , ref_mask(0)  {
+  (void)_alloc;
+      pos_ref.assign(0.0);
+
+      vel_ref.assign(0.0);
+
+      acc_ref.assign(0.0);
+  }
+
+
+
+   typedef  ::std_msgs::Header_<ContainerAllocator>  _header_type;
+  _header_type header;
+
+   typedef boost::array<float, 3>  _pos_ref_type;
+  _pos_ref_type pos_ref;
+
+   typedef boost::array<float, 3>  _vel_ref_type;
+  _vel_ref_type vel_ref;
+
+   typedef boost::array<float, 3>  _acc_ref_type;
+  _acc_ref_type acc_ref;
+
+   typedef float _yaw_ref_type;
+  _yaw_ref_type yaw_ref;
+
+   typedef uint8_t _ref_mask_type;
+  _ref_mask_type ref_mask;
+
+
+
+  enum {
+    POS_CTRL_VALIED = 1u,
+    VEL_CTRL_VALIED = 2u,
+    ACC_CTRL_VALIED = 4u,
+  };
+
+
+  typedef boost::shared_ptr< ::quad_pos_ctrl::ctrl_ref_<ContainerAllocator> > Ptr;
+  typedef boost::shared_ptr< ::quad_pos_ctrl::ctrl_ref_<ContainerAllocator> const> ConstPtr;
+
+}; // struct ctrl_ref_
+
+typedef ::quad_pos_ctrl::ctrl_ref_<std::allocator<void> > ctrl_ref;
+
+typedef boost::shared_ptr< ::quad_pos_ctrl::ctrl_ref > ctrl_refPtr;
+typedef boost::shared_ptr< ::quad_pos_ctrl::ctrl_ref const> ctrl_refConstPtr;
+
+// constants requiring out of line definition
+
+   
+
+   
+
+   
+
+
+
+template<typename ContainerAllocator>
+std::ostream& operator<<(std::ostream& s, const ::quad_pos_ctrl::ctrl_ref_<ContainerAllocator> & v)
+{
+ros::message_operations::Printer< ::quad_pos_ctrl::ctrl_ref_<ContainerAllocator> >::stream(s, "", v);
+return s;
+}
+
+} // namespace quad_pos_ctrl
+
+namespace ros
+{
+namespace message_traits
+{
+
+
+
+// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': True}
+// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/opt/ros/kinetic/share/geometry_msgs/cmake/../msg'], 'quad_pos_ctrl': ['/home/yiqianlingqi/Work/pos_px4lidar_ctrl_ws/src/quad_pos_ctrl/msg']}
+
+// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types']
+
+
+
+
+template <class ContainerAllocator>
+struct IsFixedSize< ::quad_pos_ctrl::ctrl_ref_<ContainerAllocator> >
+  : FalseType
+  { };
+
+template <class ContainerAllocator>
+struct IsFixedSize< ::quad_pos_ctrl::ctrl_ref_<ContainerAllocator> const>
+  : FalseType
+  { };
+
+template <class ContainerAllocator>
+struct IsMessage< ::quad_pos_ctrl::ctrl_ref_<ContainerAllocator> >
+  : TrueType
+  { };
+
+template <class ContainerAllocator>
+struct IsMessage< ::quad_pos_ctrl::ctrl_ref_<ContainerAllocator> const>
+  : TrueType
+  { };
+
+template <class ContainerAllocator>
+struct HasHeader< ::quad_pos_ctrl::ctrl_ref_<ContainerAllocator> >
+  : TrueType
+  { };
+
+template <class ContainerAllocator>
+struct HasHeader< ::quad_pos_ctrl::ctrl_ref_<ContainerAllocator> const>
+  : TrueType
+  { };
+
+
+template<class ContainerAllocator>
+struct MD5Sum< ::quad_pos_ctrl::ctrl_ref_<ContainerAllocator> >
+{
+  static const char* value()
+  {
+    return "c44a0c7d669f499d943b0196aea84d57";
+  }
+
+  static const char* value(const ::quad_pos_ctrl::ctrl_ref_<ContainerAllocator>&) { return value(); }
+  static const uint64_t static_value1 = 0xc44a0c7d669f499dULL;
+  static const uint64_t static_value2 = 0x943b0196aea84d57ULL;
+};
+
+template<class ContainerAllocator>
+struct DataType< ::quad_pos_ctrl::ctrl_ref_<ContainerAllocator> >
+{
+  static const char* value()
+  {
+    return "quad_pos_ctrl/ctrl_ref";
+  }
+
+  static const char* value(const ::quad_pos_ctrl::ctrl_ref_<ContainerAllocator>&) { return value(); }
+};
+
+template<class ContainerAllocator>
+struct Definition< ::quad_pos_ctrl::ctrl_ref_<ContainerAllocator> >
+{
+  static const char* value()
+  {
+    return "# enum mask\n\
+uint8 POS_CTRL_VALIED = 1\n\
+uint8 VEL_CTRL_VALIED = 2\n\
+uint8 ACC_CTRL_VALIED = 4\n\
+\n\
+# \n\
+Header     header\n\
+float32[3] pos_ref\n\
+float32[3] vel_ref\n\
+float32[3] acc_ref\n\
+float32    yaw_ref\n\
+uint8      ref_mask\n\
+================================================================================\n\
+MSG: std_msgs/Header\n\
+# Standard metadata for higher-level stamped data types.\n\
+# This is generally used to communicate timestamped data \n\
+# in a particular coordinate frame.\n\
+# \n\
+# sequence ID: consecutively increasing ID \n\
+uint32 seq\n\
+#Two-integer timestamp that is expressed as:\n\
+# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n\
+# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n\
+# time-handling sugar is provided by the client library\n\
+time stamp\n\
+#Frame this data is associated with\n\
+# 0: no frame\n\
+# 1: global frame\n\
+string frame_id\n\
+";
+  }
+
+  static const char* value(const ::quad_pos_ctrl::ctrl_ref_<ContainerAllocator>&) { return value(); }
+};
+
+} // namespace message_traits
+} // namespace ros
+
+namespace ros
+{
+namespace serialization
+{
+
+  template<class ContainerAllocator> struct Serializer< ::quad_pos_ctrl::ctrl_ref_<ContainerAllocator> >
+  {
+    template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
+    {
+      stream.next(m.header);
+      stream.next(m.pos_ref);
+      stream.next(m.vel_ref);
+      stream.next(m.acc_ref);
+      stream.next(m.yaw_ref);
+      stream.next(m.ref_mask);
+    }
+
+    ROS_DECLARE_ALLINONE_SERIALIZER
+  }; // struct ctrl_ref_
+
+} // namespace serialization
+} // namespace ros
+
+namespace ros
+{
+namespace message_operations
+{
+
+template<class ContainerAllocator>
+struct Printer< ::quad_pos_ctrl::ctrl_ref_<ContainerAllocator> >
+{
+  template<typename Stream> static void stream(Stream& s, const std::string& indent, const ::quad_pos_ctrl::ctrl_ref_<ContainerAllocator>& v)
+  {
+    s << indent << "header: ";
+    s << std::endl;
+    Printer< ::std_msgs::Header_<ContainerAllocator> >::stream(s, indent + "  ", v.header);
+    s << indent << "pos_ref[]" << std::endl;
+    for (size_t i = 0; i < v.pos_ref.size(); ++i)
+    {
+      s << indent << "  pos_ref[" << i << "]: ";
+      Printer<float>::stream(s, indent + "  ", v.pos_ref[i]);
+    }
+    s << indent << "vel_ref[]" << std::endl;
+    for (size_t i = 0; i < v.vel_ref.size(); ++i)
+    {
+      s << indent << "  vel_ref[" << i << "]: ";
+      Printer<float>::stream(s, indent + "  ", v.vel_ref[i]);
+    }
+    s << indent << "acc_ref[]" << std::endl;
+    for (size_t i = 0; i < v.acc_ref.size(); ++i)
+    {
+      s << indent << "  acc_ref[" << i << "]: ";
+      Printer<float>::stream(s, indent + "  ", v.acc_ref[i]);
+    }
+    s << indent << "yaw_ref: ";
+    Printer<float>::stream(s, indent + "  ", v.yaw_ref);
+    s << indent << "ref_mask: ";
+    Printer<uint8_t>::stream(s, indent + "  ", v.ref_mask);
+  }
+};
+
+} // namespace message_operations
+} // namespace ros
+
+#endif // QUAD_POS_CTRL_MESSAGE_CTRL_REF_H
--- a/flightlib/include/flightlib/dynamics/dynamics_base.hpp
+++ b/flightlib/include/flightlib/dynamics/dynamics_base.hpp
@@ -0,0 +1,21 @@
+#pragma once
+
+#include "flightlib/common/types.hpp"
+
+namespace flightlib {
+
+class DynamicsBase {
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+   public:
+	using DynamicsFunction = std::function<bool(const Ref<const Vector<>>, Ref<Vector<>>)>;
+
+	DynamicsBase();
+	virtual ~DynamicsBase();
+
+	// public get function
+	virtual DynamicsFunction getDynamicsFunction() const = 0;
+
+   private:
+};
+
+}  // namespace flightlib
--- a/flightlib/include/flightlib/dynamics/quadrotor_dynamics.hpp
+++ b/flightlib/include/flightlib/dynamics/quadrotor_dynamics.hpp
@@ -0,0 +1,87 @@
+#pragma once
+
+#include <yaml-cpp/yaml.h>
+
+#include <iostream>
+#include <memory>
+
+#include "flightlib/common/logger.hpp"
+#include "flightlib/common/math.hpp"
+#include "flightlib/common/quad_state.hpp"
+#include "flightlib/dynamics/dynamics_base.hpp"
+
+namespace flightlib {
+
+class QuadrotorDynamics : DynamicsBase {
+   public:
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+	QuadrotorDynamics(const Scalar mass = 1.0, const Scalar arm_l = 0.2);
+	~QuadrotorDynamics();
+
+	// dynamics function
+	bool dState(const QuadState& state, QuadState* derivative) const;
+	bool dState(const Ref<const Vector<QuadState::SIZE>> state, Ref<Vector<QuadState::SIZE>> derivative) const;
+
+	// public get function
+	DynamicsFunction getDynamicsFunction() const;
+
+	// help functions
+	bool valid() const;
+	bool updateParams(const YAML::Node& params);
+
+	// Helpers to apply limits.
+	Vector<4> clampThrust(const Vector<4> thrusts) const;
+	Scalar clampThrust(const Scalar thrust) const;
+	Scalar clampCollectiveThrust(const Scalar thrust) const;
+
+	Vector<4> clampMotorOmega(const Vector<4>& omega) const;
+	Vector<3> clampBodyrates(const Vector<3>& omega) const;
+
+	inline Scalar collective_thrust_min() const { return 4.0 * thrust_min_; }
+	inline Scalar collective_thrust_max() const { return 4.0 * thrust_max_; }
+
+	// Helpers for conversion
+	Vector<4> motorOmegaToThrust(const Vector<4>& omega) const;
+	Vector<4> motorThrustToOmega(const Vector<4>& thrusts) const;
+	Matrix<4, 4> getAllocationMatrix() const;
+
+	//
+	inline Scalar getMass(void) const { return mass_; };
+	inline Scalar getArmLength(void) const { return arm_l_; };
+	inline Scalar getMotorTauInv() const { return motor_tau_inv_; };
+	inline Matrix<3, 3> getJ(void) const { return J_; };
+	inline Matrix<3, 3> getJInv(void) const { return J_inv_; };
+	inline Vector<3> getOmegaMax(void) const { return omega_max_; };
+
+	bool setMass(const Scalar mass);
+	bool setArmLength(const Scalar arm_length);
+	bool setMotortauInv(const Scalar tau_inv);
+
+	friend std::ostream& operator<<(std::ostream& os, const QuadrotorDynamics& quad_dymaics);
+
+   private:
+	bool updateInertiaMarix();
+	Scalar mass_;
+	Scalar arm_l_;
+	Matrix<3, 4> t_BM_;
+	Matrix<3, 3> J_;
+	Matrix<3, 3> J_inv_;
+
+	// motors
+	Scalar motor_omega_min_;
+	Scalar motor_omega_max_;
+	Scalar motor_tau_inv_;
+
+	// Propellers
+	Vector<3> thrust_map_;
+	Scalar kappa_;
+	Scalar thrust_min_;
+	Scalar thrust_max_;
+	Scalar collective_thrust_min_;
+	Scalar collective_thrust_max_;
+
+	// Quadrotor limits
+	Vector<3> omega_max_;
+};
+
+}  // namespace flightlib
--- a/flightlib/include/flightlib/envs/env_base.hpp
+++ b/flightlib/include/flightlib/envs/env_base.hpp
@@ -0,0 +1,70 @@
+//
+// This is inspired by RaiGym, thanks.
+//
+#pragma once
+
+#include <unistd.h>
+#include <yaml-cpp/yaml.h>
+
+#include <memory>
+#include <random>
+#include <unordered_map>
+#include <vector>
+
+#include "flightlib/common/types.hpp"
+
+namespace flightlib {
+
+class EnvBase {
+   public:
+	EnvBase() : obs_dim_(0), act_dim_(0), rew_dim_(0), sim_dt_(0.0), img_width_(0), img_height_(0) {};
+	virtual ~EnvBase() {};
+
+	// (pure virtual) public methods (has to be implemented by child classes)
+	virtual bool reset(Ref<Vector<>> obs, const bool random = true)                     = 0;
+	virtual bool step(const Ref<Vector<>> act, Ref<Vector<>> obs, Ref<Vector<>> reward) = 0;
+	virtual bool getObs(Ref<Vector<>> obs)                                              = 0;
+
+	// (virtual) public methods (implementations are optional.)
+	virtual bool getDepthImage(Ref<DepthImgVector<>> img) { return false; }
+	virtual void close() {};
+	// virtual void render();
+	virtual void updateExtraInfo() {};
+	virtual bool isTerminalState(Scalar &reward) {
+		reward = 0.f;
+		return false;
+	}
+
+	// auxilirary functions
+	inline void setSeed(const int seed) { std::srand(seed); };
+	inline int getObsDim() { return obs_dim_; };
+	inline int getActDim() { return act_dim_; };
+	inline int getRewDim() { return rew_dim_; };
+	inline int getImgWidth() { return img_width_; };
+	inline int getImgHeight() { return img_height_; };
+	inline Scalar getSimTimeStep() { return sim_dt_; };
+	inline int getExtraInfoDim() { return extra_info_.size(); };
+
+	// public variables
+	std::unordered_map<std::string, float> extra_info_;
+
+   protected:
+	// observation and action dimenstions (for Reinforcement learning)
+	int obs_dim_;
+	int act_dim_;
+	int rew_dim_;
+
+	int img_width_;
+	int img_height_;
+
+	// control time step
+	Scalar sim_dt_{0.02};
+
+	// random variable generator
+	std::normal_distribution<Scalar> norm_dist_{0.0, 1.0};
+	std::uniform_real_distribution<Scalar> uniform_dist_{-1.0, 1.0};
+	std::random_device rd_;
+	std::mt19937 random_gen_{rd_()};
+};
+
+}  // namespace flightlib
--- a/flightlib/include/flightlib/envs/quadrotor_env.hpp
+++ b/flightlib/include/flightlib/envs/quadrotor_env.hpp
@@ -0,0 +1,119 @@
+#pragma once
+
+// std lib
+#include <pcl/point_types.h>
+#include <pcl/search/kdtree.h>
+#include <stdlib.h>
+#include <time.h>
+#include <unistd.h>
+#include <yaml-cpp/yaml.h>
+
+#include <cmath>
+#include <iostream>
+// flightlib
+#include "flightlib/bridges/unity_bridge.hpp"
+#include "flightlib/common/command.hpp"
+#include "flightlib/common/logger.hpp"
+#include "flightlib/common/math.hpp"
+#include "flightlib/common/quad_state.hpp"
+#include "flightlib/common/types.hpp"
+#include "flightlib/envs/env_base.hpp"
+#include "flightlib/grad_traj_optimization/traj_optimization_bridge.h"
+#include "flightlib/objects/quadrotor.hpp"
+#include "flightlib/sensors/sgm_gpu/sgm_gpu.h"
+
+namespace flightlib {
+
+enum Ctl : int {
+	kNObs = 13,  // observation dim
+	kNAct = 9,   // action dim
+};
+
+class QuadrotorEnv final : public EnvBase {
+   public:
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+	QuadrotorEnv();
+	QuadrotorEnv(const std::string &cfg_path);
+	~QuadrotorEnv();
+
+	/* set functions */
+	bool reset(Ref<Vector<>> obs, const bool random = true) override;
+	void setState(ConstRef<Vector<>> state);
+	void setGoal(ConstRef<Vector<>> goal);
+	void setMapID(int id);
+	void setDAggerMode(bool dagger_mode) { dagger_mode_ = dagger_mode; }
+	bool step(const Ref<Vector<>> act, Ref<Vector<>> obs, Ref<Vector<>> reward) override;
+	void setInputCloud(const pcl::PointCloud<pcl::PointXYZ> &point_in);
+	void setESDF(const std::vector<float> &esdf_map, const pcl::PointCloud<pcl::PointXYZ> &point_in);
+	void addKdtree(std::shared_ptr<pcl::search::KdTree<pcl::PointXYZ>> kdtree) { kdtrees.push_back(kdtree); }
+	void addESDFMap(std::shared_ptr<sdf_tools::SignedDistanceField> esdf_map) { esdf_maps.push_back(esdf_map); }
+	void addMapSize(Eigen::Vector3d map_boundary_min, Eigen::Vector3d map_boundary_max) {
+		min_map_boundaries.push_back(map_boundary_min);
+		max_map_boundaries.push_back(map_boundary_max);
+	}
+
+	/* get functions */
+	bool getObs(Ref<Vector<>> obs) override;
+	bool getAct(Ref<Vector<>> act) const;
+	bool getRGBImage(Ref<ImgVector<>> img, const bool rgb);
+	bool getDepthImage(Ref<DepthImgVector<>> img) override;
+	bool getStereoImage(Ref<DepthImgVector<>> img);
+	int getMapNum() { return kdtrees.size(); }
+	void getCostAndGradient(ConstRef<Vector<>> dp, int id, float &cost, Ref<Vector<>> grad);
+	inline std::vector<std::string> getRewardNames() { return reward_names_; }
+	void getWorldBox(Ref<Vector<>> world_box) {
+		world_box << world_box_(0), world_box_(1), world_box_(2), world_box_(3), world_box_(4), world_box_(5);  // xyz_min, xyz_max
+	};
+
+	/* other functions */
+	void collisionCheck(float dis = 0.2);
+	bool configCamera(const YAML::Node &cfg_node);
+	bool loadParam(const YAML::Node &cfg);
+	void computeDepthImage(const cv::Mat &left_frame, const cv::Mat &right_frame, cv::Mat *const depth);
+	bool isTerminalState(Scalar &reward) override;
+	void addObjectsToUnity(std::shared_ptr<UnityBridge> bridge) { bridge->addQuadrotor(quadrotor_ptr_); }
+	void runControlAndUpdateState(Eigen::Vector3f p_ref, Eigen::Vector3f v_ref, Eigen::Vector3f a_ref);
+
+   private:
+	// quadrotor state and observation
+	std::shared_ptr<Quadrotor> quadrotor_ptr_;
+	QuadState quad_state_;
+	Vector<kNObs> quad_obs_;
+	Vector<kNAct> quad_act_;
+	Logger logger_{"QaudrotorEnv"};
+	Eigen::Vector3f desired_p_, desired_v_, desired_a_;
+
+	// map
+	Matrix<3, 2> world_box_;
+	Vector<3> center_, scale_;
+	std::vector<std::shared_ptr<pcl::search::KdTree<pcl::PointXYZ>>> kdtrees;
+	std::vector<std::shared_ptr<sdf_tools::SignedDistanceField>> esdf_maps;
+	std::vector<Eigen::Vector3d> min_map_boundaries, max_map_boundaries;
+
+	// camera params
+	Scalar fov_;
+	int width_, height_;
+	Scalar stereo_baseline_;
+	std::shared_ptr<sgm_gpu::SgmGpu> sgm_;
+	cv::Mat rgb_img_, gray_img_, depth_img_;
+	std::shared_ptr<RGBCamera> rgb_camera_left, rgb_camera_right;
+
+	// trajectory optimization
+	int map_idx_{0};
+	Vector<3> goal_;
+	TrajOptimizationBridge *traj_opt_bridge;
+
+	// data collection
+	Scalar roll_var_, pitch_var_;
+
+	// others
+	int steps_;
+	YAML::Node cfg_;
+	bool is_collision_;
+	Scalar nearest_obstacle{10};
+	std::vector<std::string> reward_names_;
+	bool collect_data_, dagger_mode_{false};
+};
+
+}  // namespace flightlib
--- a/flightlib/include/flightlib/envs/vec_env.hpp
+++ b/flightlib/include/flightlib/envs/vec_env.hpp
@@ -0,0 +1,106 @@
+//
+// This is inspired by RaiGym, thanks.
+//
+#pragma once
+
+// std
+#include <omp.h>
+#include <time.h>
+
+#include <filesystem>
+#include <memory>
+#include <regex>
+
+// pcl
+#include <pcl/io/ply_io.h>
+#include <pcl/point_types.h>
+#include <pcl/search/kdtree.h>
+
+// flightlib
+#include "flightlib/bridges/unity_bridge.hpp"
+#include "flightlib/common/logger.hpp"
+#include "flightlib/common/types.hpp"
+#include "flightlib/envs/env_base.hpp"
+#include "flightlib/envs/quadrotor_env.hpp"
+#include "flightlib/grad_traj_optimization/traj_optimization_bridge.h"
+
+namespace flightlib {
+
+template<typename EnvBase>
+class VecEnv {
+   public:
+	VecEnv();
+	VecEnv(const std::string& cfgs, const bool from_file = true);
+	VecEnv(const YAML::Node& cfgs_node);
+	~VecEnv();
+
+	/* unity functions */
+	bool connectUnity();
+	void disconnectUnity();
+	bool setUnity(bool render);
+	void render();
+	bool spawnTrees();
+	bool savePointcloud(int ply_id);
+	bool spawnTreesAndSavePointcloud(int ply_id_in = -1, float spacing = -1);
+	void close();
+	void setSeed(const int seed);
+
+	/* set functions */
+	bool reset(Ref<MatrixRowMajor<>> obs);
+	bool step(Ref<MatrixRowMajor<>> act, Ref<MatrixRowMajor<>> obs, Ref<MatrixRowMajor<>> reward, Ref<BoolVector<>> done);
+	void perAgentStep(int agent_id, Ref<MatrixRowMajor<>> act, Ref<MatrixRowMajor<>> obs, Ref<MatrixRowMajor<>> reward, Ref<BoolVector<>> done);
+	bool setState(ConstRef<MatrixRowMajor<>> state);  // World Frame
+	bool setGoal(ConstRef<MatrixRowMajor<>> goal);    // World Frame
+	void setMapID(ConstRef<IntVector<>> id);
+
+	/* get functions */
+	inline int getObsDim(void) { return obs_dim_; };
+	inline int getActDim(void) { return act_dim_; };
+	inline int getRewDim(void) const { return rew_dim_; };
+	inline int getImgHeight(void) const { return img_height_; };
+	inline int getImgWidth(void) const { return img_width_; };
+	inline int getNumOfEnvs(void) { return envs_.size(); };
+	inline std::vector<std::string> getRewardNames(void) { return envs_[0]->getRewardNames(); };
+	inline void getWorldBox(Ref<Vector<>> world_box) { envs_[0]->getWorldBox(world_box); };
+	void getObs(Ref<MatrixRowMajor<>> obs);
+	bool getRGBImage(Ref<ImgMatrixRowMajor<>> img, const bool rgb_image);
+	bool getStereoImage(Ref<DepthImgMatrixRowMajor<>> img);
+	bool getDepthImage(Ref<DepthImgMatrixRowMajor<>> img);
+	void getCostAndGradient(ConstRef<MatrixRowMajor<>> dp, ConstRef<IntVector<>> traj_id, Ref<Vector<>> cost,
+	                        Ref<MatrixRowMajor<>> grad);  // Body Frame
+
+	/* other functions */
+	void init(void);
+	void generateMaps();
+	int extract_number(const std::string& filename);
+
+   private:
+	// create objects
+	Logger logger_{"VecEnv"};
+	std::vector<std::unique_ptr<EnvBase>> envs_;
+
+	// Flightmare(Unity3D)
+	std::shared_ptr<UnityBridge> unity_bridge_ptr_;
+	SceneID scene_id_{UnityScene::WAREHOUSE};
+	bool unity_ready_{false};
+	bool unity_render_{false};
+	FrameID frameID{1};
+	RenderMessage_t unity_output_;
+	uint16_t receive_id_{0};
+
+	// scenario generation
+	Scalar avg_tree_spacing_;
+	Vector<3> bounding_box_, bounding_box_origin_;
+	Scalar pointcloud_resolution_;
+
+	// other variables
+	std::string ply_path_;
+	bool dagger_mode_{false}, supervised_mode_{false};
+	int seed_, num_envs_, obs_dim_, act_dim_, rew_dim_, num_threads_;
+	int img_width_, img_height_;
+
+	// yaml configurations
+	YAML::Node cfg_;
+};
+
+}  // namespace flightlib
--- a/flightlib/include/flightlib/grad_traj_optimization/grad_traj_optimizer.h
+++ b/flightlib/include/flightlib/grad_traj_optimization/grad_traj_optimizer.h
@@ -0,0 +1,115 @@
+/*
+    This code is modified from https://github.com/HKUST-Aerial-Robotics/grad_traj_optimization, thanks to their excellent work.
+*/
+
+#ifndef _GRAD_TRAJ_OPTIMIZER_H_
+#define _GRAD_TRAJ_OPTIMIZER_H_
+
+#include <yaml-cpp/yaml.h>
+
+#include <Eigen/Eigen>
+
+#include "flightlib/grad_traj_optimization/qp_generator.h"
+#include "sdf_tools/collision_map.hpp"
+#include "sdf_tools/sdf.hpp"
+
+#define GDTB getDistanceToBoundary
+
+using namespace std;
+
+class GradTrajOptimizer {
+   public:
+	GradTrajOptimizer(const YAML::Node &cfg);
+
+	void getCoefficient(Eigen::MatrixXd &coeff) { coeff = this->coeff; };
+
+	double getCost() { return this->min_f; }
+
+	void getSegmentTime(Eigen::VectorXd &T) { T = this->Time; }
+
+	void setSignedDistanceField(std::shared_ptr<sdf_tools::SignedDistanceField> s, double res);
+
+	void setGoal(Eigen::Vector3d goal);
+
+	void setBoundary(Eigen::Vector3d min, Eigen::Vector3d max);
+
+	void getCostAndGradient(const std::vector<double> &df, const std::vector<double> &dp, double &cost, std::vector<double> &grad) const;
+
+	/** convert derivatives of end points to polynomials coefficient */
+	void getCoefficientFromDerivative(Eigen::MatrixXd &coeff, const std::vector<double> &_dp) const;
+
+   private:
+	/** signed distance field */
+	double resolution;
+	Eigen::Vector3d map_boundary_min, map_boundary_max;
+	std::shared_ptr<sdf_tools::SignedDistanceField> sdf;
+	mutable Eigen::VectorXd boundary;  // min x max x... min z,max z
+
+	/** coefficient of polynomials*/
+	mutable Eigen::MatrixXd coeff;
+
+	/** important matrix and variables*/
+	Eigen::MatrixXd A;
+	Eigen::MatrixXd C;
+	Eigen::MatrixXd L;
+	Eigen::MatrixXd R;
+	Eigen::MatrixXd Rff;
+	Eigen::MatrixXd Rpp;
+	Eigen::MatrixXd Rpf;
+	Eigen::MatrixXd Rfp;
+	Eigen::VectorXd Time;
+	Eigen::MatrixXd V;
+	mutable Eigen::MatrixXd Df;
+	Eigen::MatrixXd Dp;
+	Eigen::MatrixXd path;
+	Eigen::Vector3d goal;
+
+	/** tractory params */
+	double sgm_time;
+	int num_dp;
+	int num_df;
+	int num_point;
+	double min_f;
+
+	// weight of cost
+	double w_smooth;
+	double w_collision;
+	double w_vel;
+	double w_acc;
+	double w_goal;
+	double w_long;
+
+	// params of cost
+	double d0;
+	double r;
+	double alpha;
+
+	double v0;
+	double rv;
+	double alphav;
+
+	double a0;
+	double ra;
+	double alphaa;
+
+	/** get distance and gradient in signed distance field ,by position query*/
+	void getDistanceAndGradient(Eigen::Vector3d &pos, double &dist, Eigen::Vector3d &grad) const;
+	void getPositionFromCoeff(Eigen::Vector3d &pos, const Eigen::MatrixXd &coeff, const int &index, const double &time) const;
+	void getVelocityFromCoeff(Eigen::Vector3d &vel, const Eigen::MatrixXd &coeff, const int &index, const double &time) const;
+	void getAccelerationFromCoeff(Eigen::Vector3d &acc, const Eigen::MatrixXd &coeff, const int &index, const double &time) const;
+
+	/** penalty and gradient */
+	void getDistancePenalty(const double &distance, double &cost) const;
+	void getDistancePenaltyGradient(const double &distance, double &grad) const;
+	void getVelocityPenalty(const double &distance, double &cost) const;
+	void getVelocityPenaltyGradient(const double &vel, double &grad) const;
+	void getAccelerationPenalty(const double &distance, double &cost) const;
+	void getAccelerationPenaltyGradient(const double &acc, double &grad) const;
+
+	void getTimeMatrix(const double &t, Eigen::MatrixXd &T) const;
+	void constrains(double &n, double min, double max) const;
+	double getDistanceToBoundary(const double &x, const double &y, const double &z) const;
+	void recaluculateGradient(const double &x, const double &y, const double &z, Eigen ::Vector3d &grad) const;
+};
+
+#endif
--- a/flightlib/include/flightlib/grad_traj_optimization/opt_utile.h
+++ b/flightlib/include/flightlib/grad_traj_optimization/opt_utile.h
@@ -0,0 +1,18 @@
+#ifndef _LATTICE_NODE_H_
+#define _LATTICE_NODE_H_
+#include <Eigen/Eigen>
+
+// body frame
+void getLatticeGuiding(std::vector<std::pair<Eigen::Vector3d, Eigen::Vector3d>> &lattice_nodes, int horizon_num, int vertical_num, int radio_num,
+                       int vel_num, double horizon_fov, double vertical_fov, double radio_range, double vel_fov, double vel_prefile);
+
+void getPositionFromCoeff(Eigen::Vector3d &pos, Eigen::MatrixXd coeff, int index, double time);
+
+void getVelocityFromCoeff(Eigen::Vector3d &vel, Eigen::MatrixXd coeff, int index, double time);
+
+void getAccelerationFromCoeff(Eigen::Vector3d &acc, Eigen::MatrixXd coeff, int index, double time);
+
+Eigen::MatrixXd solveCoeffFromBoundaryState(const Eigen::Vector3d &Pos_init, const Eigen::Vector3d &Vel_init, const Eigen::Vector3d &Acc_init,
+                                            const Eigen::Vector3d &Pos_end, const Eigen::Vector3d &Vel_end, const Eigen::Vector3d &Acc_end,
+                                            double Time);
+#endif
--- a/flightlib/include/flightlib/grad_traj_optimization/qp_generator.h
+++ b/flightlib/include/flightlib/grad_traj_optimization/qp_generator.h
@@ -0,0 +1,53 @@
+#ifndef _TRAJECTORY_GENERATOR_H_
+#define _TRAJECTORY_GENERATOR_H_
+#include <Eigen/Eigen>
+#include <vector>
+
+
+class TrajectoryGenerator {
+   private:
+	int m = 1;           // number of segments in the trajectory
+	Eigen::MatrixXd _A;  // Mapping matrix
+	Eigen::MatrixXd _Q;  // Hessian matrix
+	Eigen::MatrixXd _C;  // Selection matrix
+	Eigen::MatrixXd _L;  // A.inv() * C.transpose()
+
+	Eigen::MatrixXd _R;
+	Eigen::MatrixXd _Rff;
+	Eigen::MatrixXd _Rpp;
+	Eigen::MatrixXd _Rpf;
+	Eigen::MatrixXd _Rfp;
+
+	Eigen::VectorXd _Pxi;
+	Eigen::VectorXd _Pyi;
+	Eigen::VectorXd _Pzi;
+
+	Eigen::VectorXd _Dx;
+	Eigen::VectorXd _Dy;
+	Eigen::VectorXd _Dz;
+
+   public:
+	Eigen::MatrixXd _Path;
+	Eigen::VectorXd _Time;
+
+	TrajectoryGenerator();
+
+	~TrajectoryGenerator();
+
+	void QPGeneration(const Eigen::VectorXd &Time);
+
+	void StackOptiDep();  // Stack the optimization's dependency, the intermediate matrix and initial derivatives
+
+	Eigen::MatrixXd getA();
+	Eigen::MatrixXd getQ();
+	Eigen::MatrixXd getC();
+	Eigen::MatrixXd getL();
+
+	Eigen::MatrixXd getR();
+	Eigen::MatrixXd getRpp();
+	Eigen::MatrixXd getRff();
+	Eigen::MatrixXd getRfp();
+	Eigen::MatrixXd getRpf();
+};
+
+#endif
--- a/flightlib/include/flightlib/grad_traj_optimization/traj_optimization_bridge.h
+++ b/flightlib/include/flightlib/grad_traj_optimization/traj_optimization_bridge.h
@@ -0,0 +1,71 @@
+#ifndef _TRAJ_OPTIMIZATION_BRIDGE_H_
+#define _TRAJ_OPTIMIZATION_BRIDGE_H_
+
+#include <pcl/common/common.h>
+#include <pcl/io/ply_io.h>
+#include <pcl_ros/point_cloud.h>
+#include <sensor_msgs/PointCloud.h>
+#include <stdlib.h>
+#include <yaml-cpp/yaml.h>
+
+#include <Eigen/Eigen>
+
+#include "flightlib/grad_traj_optimization/grad_traj_optimizer.h"
+#include "flightlib/grad_traj_optimization/opt_utile.h"
+
+namespace traj_opt {
+std::shared_ptr<sdf_tools::SignedDistanceField> SdfConstruction(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, Eigen::Vector3d &map_boundary_min_sdf,
+                                                                Eigen::Vector3d &map_boundary_max_sdf);
+}
+
+class TrajOptimizationBridge {
+   private:
+	YAML::Node cfg_;
+	// state of uav in world frame
+	Eigen::Vector3d pos_;
+	Eigen::Vector3d vel_;
+	Eigen::Vector3d acc_;
+	Eigen::Quaterniond q_wb_;
+	Eigen::Vector3d goal_;
+	Eigen::MatrixXd pred_coeff_;
+
+	// std::string ply_file;
+	double resolution;
+	std::shared_ptr<sdf_tools::SignedDistanceField> sdf_;
+	Eigen::Vector3d map_boundary_min_, map_boundary_max_;
+
+	// x_pva, y_pva, z_pva in world frame
+	std::vector<double> dp_;  // df refers to the initial_state currently
+	std::vector<double> df_;  // dp refers to the end_state currently
+
+	double goal_length;
+	int horizon_num, vertical_num, radio_num, vel_num;
+	double horizon_fov, vertical_fov, radio_range, vel_fov, vel_prefile;
+	std::vector<std::pair<Eigen::Vector3d, Eigen::Vector3d>> lattice_nodes;
+
+	void loadParam(YAML::Node &cfg);
+
+   public:
+	TrajOptimizationBridge();
+	~TrajOptimizationBridge();
+
+	void setMap(std::shared_ptr<sdf_tools::SignedDistanceField> sdf_for_traj_optimization, Eigen::Vector3d &map_boundary_min,
+	            Eigen::Vector3d &map_boundary_max);
+
+	// in world frame
+	void setState(Eigen::Vector3d pos, Eigen::Quaterniond q, Eigen::Vector3d vel, Eigen::Vector3d acc);
+
+	void setGoal(Eigen::Vector3d goal);
+
+	// dp in body frame, grad in body frame
+	void getCostAndGradient(const std::vector<double> &dp, int id, double &cost, std::vector<double> &grad);
+
+	void getNextStateAndCost(const std::vector<double> &dp, double &cost, Eigen::Vector3d &pos, Eigen::Vector3d &vel, Eigen::Vector3d &acc,
+	                         double sim_t);
+
+	void solveBVP(const std::vector<double> &dp);
+
+	void getNextState(Eigen::Vector3d &pos, Eigen::Vector3d &vel, Eigen::Vector3d &acc, double sim_t);
+};
+
+#endif
--- a/flightlib/include/flightlib/json/json.hpp
+++ b/flightlib/include/flightlib/json/json.hpp
--- a/flightlib/include/flightlib/objects/object_base.hpp
+++ b/flightlib/include/flightlib/objects/object_base.hpp
@@ -0,0 +1,19 @@
+#pragma once
+
+#include "flightlib/common/types.hpp"
+
+namespace flightlib {
+
+class ObjectBase {
+ public:
+  ObjectBase();
+  virtual ~ObjectBase();
+
+  // reset
+  virtual bool reset(void) = 0;
+
+  // run
+  virtual bool run(const Scalar dt) = 0;
+};
+
+}  // namespace flightlib
--- a/flightlib/include/flightlib/objects/quadrotor.hpp
+++ b/flightlib/include/flightlib/objects/quadrotor.hpp
@@ -0,0 +1,110 @@
+#pragma once
+
+/*
+    Explanation:
+        We retain this class but do not use most of its functionalities.
+        For efficiency, we do not use the built-in dynamics model and instead
+        directly use the desired attitude given by the controller as actual state.
+        Because the proposed approach (YOPO) only focuses on the trajectory performance,
+        while control is preformed by an external controller.
+*/
+
+#include <stdlib.h>
+
+#include "flightlib/common/command.hpp"
+#include "flightlib/common/integrator_rk4.hpp"
+#include "flightlib/common/types.hpp"
+#include "flightlib/dynamics/quadrotor_dynamics.hpp"
+#include "flightlib/objects/object_base.hpp"
+#include "flightlib/sensors/imu.hpp"
+#include "flightlib/sensors/rgb_camera.hpp"
+
+namespace flightlib {
+
+class Quadrotor : ObjectBase {
+   public:
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+	Quadrotor(const std::string& cfg_path);
+	Quadrotor(const QuadrotorDynamics& dynamics = QuadrotorDynamics(1.0, 0.25));
+	~Quadrotor();
+
+	// reset
+	bool reset(void) override;
+	bool reset(const QuadState& state);
+	void init(void);
+
+	// run the quadrotor
+	bool setState(const Ref<Vector<>> p, const Ref<Vector<>> v, const Quaternion q_, const Ref<Vector<>> a_, const Scalar ctl_dt);
+	bool run(const Scalar dt) override;
+	bool run(const Command& cmd, const Scalar dt);
+	void runSimpleFlight(const Eigen::Vector3f& ref_acc, float ref_yaw, Eigen::Quaternionf& quat_des);
+
+	// public get functions
+	bool getState(QuadState* const state) const;
+	bool getMotorThrusts(Ref<Vector<4>> motor_thrusts) const;
+	bool getMotorOmega(Ref<Vector<4>> motor_omega) const;
+	bool getDynamics(QuadrotorDynamics* const dynamics) const;
+
+	const QuadrotorDynamics& getDynamics();
+	Vector<3> getSize(void) const;
+	Vector<3> getPosition(void) const;
+	Quaternion getQuaternion(void) const;
+	std::vector<std::shared_ptr<RGBCamera>> getCameras(void) const;
+	bool getCamera(const size_t cam_id, std::shared_ptr<RGBCamera> camera) const;
+	int getNumCamera() const;
+	bool getCollision() const;
+
+	// public set functions
+	bool setState(const QuadState& state);
+	bool setCommand(const Command& cmd);
+	bool updateDynamics(const QuadrotorDynamics& dynamics);
+	bool addRGBCamera(std::shared_ptr<RGBCamera> camera);
+
+	// low-level controller
+	Vector<4> runFlightCtl(const Scalar sim_dt, const Vector<3>& omega, const Command& cmd);
+
+	// simulate motors
+	void runMotors(const Scalar sim_dt, const Vector<4>& motor_thrust_des);
+
+	// constrain world box
+	bool setWorldBox(const Ref<Matrix<3, 2>> box);
+	bool constrainInWorldBox(const QuadState& old_state);
+
+	//
+	inline Scalar getMass(void) { return dynamics_.getMass(); };
+	inline void setSize(const Ref<Vector<3>> size) { size_ = size; };
+	inline void setCollision(const bool collision) { collision_ = collision; };
+
+	float getSimT() { return state_.t; }
+
+   private:
+	// quadrotor dynamics, integrators
+	QuadrotorDynamics dynamics_;
+	IMU imu_;
+	std::unique_ptr<IntegratorRK4> integrator_ptr_;
+	std::vector<std::shared_ptr<RGBCamera>> rgb_cameras_;
+
+	// quad control command
+	Command cmd_;
+
+	// quad state
+	QuadState state_;
+	Vector<3> size_;
+	bool collision_;
+
+	// auxiliar variablers
+	Vector<4> motor_omega_;
+	Vector<4> motor_thrusts_;
+	Matrix<4, 4> B_allocation_;
+	Matrix<4, 4> B_allocation_inv_;
+
+	// P gain for body-rate control
+	const Matrix<3, 3> Kinv_ang_vel_tau_ = Vector<3>(16.6, 16.6, 5.0).asDiagonal();
+	// gravity
+	const Vector<3> gz_{0.0, 0.0, Gz};
+
+	// auxiliary variables
+	Matrix<3, 2> world_box_;
+};
+
+}  // namespace flightlib
--- a/flightlib/include/flightlib/objects/static_gate.hpp
+++ b/flightlib/include/flightlib/objects/static_gate.hpp
@@ -0,0 +1,13 @@
+#pragma once
+
+#include "flightlib/objects/static_object.hpp"
+
+namespace flightlib {
+class StaticGate : public StaticObject {
+ public:
+  StaticGate(const std::string& id, const std::string& prefab_id = "rpg_gate")
+    : StaticObject(id, prefab_id) {}
+  ~StaticGate() {}
+};
+
+}  // namespace flightlib
--- a/flightlib/include/flightlib/objects/static_object.hpp
+++ b/flightlib/include/flightlib/objects/static_object.hpp
@@ -0,0 +1,36 @@
+#pragma once
+
+#include "flightlib/common/types.hpp"
+
+namespace flightlib {
+class StaticObject {
+ public:
+  StaticObject(const std::string& id, const std::string& prefab_id)
+    : id_(id), prefab_id_(prefab_id){};
+  virtual ~StaticObject(){};
+
+  // public set functions
+  virtual void setPosition(const Vector<3>& position) { position_ = position; };
+  virtual void setQuaternion(const Quaternion& quaternion) {
+    quat_ = quaternion;
+  };
+  virtual void setSize(const Vector<3>& size) { size_ = size; };
+
+  // public get functions
+  virtual Vector<3> getPosition(void) { return position_; };
+  virtual Quaternion getQuaternion(void) { return quat_; };
+  virtual Vector<3> getSize(void) { return size_; };
+  const std::string& getID(void) { return id_; };
+  const std::string& getPrefabID(void) { return prefab_id_; };
+
+ private:
+  std::string id_;
+  std::string prefab_id_;
+
+ protected:
+  Vector<3> position_{0.0, 0.0, 0.0};
+  Quaternion quat_{1.0, 0.0, 0.0, 0.0};
+  Vector<3> size_{1.0, 1.0, 1.0};
+};
+
+}  // namespace flightlib
--- a/flightlib/include/flightlib/objects/unity_camera.hpp
+++ b/flightlib/include/flightlib/objects/unity_camera.hpp
@@ -0,0 +1,15 @@
+#pragma once
+
+#include "flightlib/common/types.hpp"
+#include "flightlib/sensors/rgb_camera.hpp"
+
+namespace flightlib {
+
+class UnityCamera : RGBCamera {
+ public:
+  UnityCamera();
+  ~UnityCamera();
+
+ private:
+};
+}  // namespace flightlib
--- a/flightlib/include/flightlib/ros_nodes/flight_pilot.hpp
+++ b/flightlib/include/flightlib/ros_nodes/flight_pilot.hpp
@@ -0,0 +1,92 @@
+
+#pragma once
+
+#include <cv_bridge/cv_bridge.h>
+#include <nav_msgs/Odometry.h>
+#include <ros/ros.h>
+#include <sensor_msgs/CameraInfo.h>
+#include <yaml-cpp/yaml.h>
+
+#include <cmath>
+#include <filesystem>
+#include <memory>
+
+#include "std_msgs/Empty.h"
+// flightlib
+#include "flightlib/bridges/unity_bridge.hpp"
+#include "flightlib/common/quad_state.hpp"
+#include "flightlib/common/types.hpp"
+#include "flightlib/objects/quadrotor.hpp"
+#include "flightlib/sensors/rgb_camera.hpp"
+#include "flightlib/sensors/sgm_gpu/sgm_gpu.h"
+
+using namespace flightlib;
+
+namespace flightros {
+
+class FlightPilot {
+   public:
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+	FlightPilot(const ros::NodeHandle& nh, const ros::NodeHandle& pnh);
+	~FlightPilot();
+
+	// callbacks
+	void mainLoopCallback(const ros::TimerEvent& event);
+	void spawnTreeCallback(const std_msgs::Empty::ConstPtr& msg);
+	void clearTreeCallback(const std_msgs::Empty::ConstPtr& msg);
+	void savePointcloudCallback(const std_msgs::Empty::ConstPtr& msg);
+	void poseCallback(const nav_msgs::Odometry::ConstPtr& msg);
+
+	// unity
+	bool setUnity(const bool render);
+	bool connectUnity(void);
+	bool disconnectUnity();
+	bool loadParams(const YAML::Node& cfg);
+	bool configCamera(const YAML::Node& cfg);
+	void computeDepthImage(const cv::Mat& left_frame, const cv::Mat& right_frame, cv::Mat* const depth);
+	bool spawnTreesAndSavePointCloud();
+
+   private:
+	// ros nodes
+	ros::NodeHandle nh_;
+	ros::NodeHandle pnh_;
+
+	// publisher & subscriber
+	std::string odom_topic_;
+	ros::Publisher stereo_pub, left_img_pub, depth_pub, cam_info_pub;
+	ros::Subscriber state_est_sub_, spawn_tree_sub_, clear_tree_sub_, save_pc_sub_;
+
+	// main loop timer
+	ros::Timer timer_main_loop_;
+	ros::Time timestamp;
+	Scalar main_loop_freq_{50.0};
+
+	// unity & quadrotor
+	Vector<3> quad_size_;
+	QuadState quad_state_;
+	std::shared_ptr<Quadrotor> quad_ptr_;
+	std::shared_ptr<UnityBridge> unity_bridge_ptr_;
+	SceneID scene_id_{UnityScene::WAREHOUSE};
+	bool unity_ready_{false};
+	bool unity_render_{false};
+	RenderMessage_t unity_output_;
+	uint16_t receive_id_{0};
+	FrameID frameID{0};
+
+	// camera param
+	Scalar stereo_baseline_;
+	Scalar fov_;
+	int width_;
+	int height_;
+	bool use_depth, use_stereo;
+	std::shared_ptr<RGBCamera> rgb_camera_left, rgb_camera_right;
+	std::shared_ptr<sgm_gpu::SgmGpu> sgm_;
+
+	// tree generation
+	int ply_id_{0};
+	Scalar avg_tree_spacing_;
+	Vector<3> bounding_box_, bounding_box_origin_;
+	Scalar pointcloud_resolution_;
+	std::string ply_path_;
+};
+}  // namespace flightros
--- a/flightlib/include/flightlib/sensors/imu.hpp
+++ b/flightlib/include/flightlib/sensors/imu.hpp
@@ -0,0 +1,16 @@
+#pragma once
+
+#include "flightlib/common/types.hpp"
+#include "flightlib/sensors/sensor_base.hpp"
+
+namespace flightlib {
+
+class IMU : SensorBase {
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  IMU();
+  ~IMU();
+
+ private:
+};
+}  // namespace flightlib
--- a/flightlib/include/flightlib/sensors/rgb_camera.hpp
+++ b/flightlib/include/flightlib/sensors/rgb_camera.hpp
@@ -0,0 +1,126 @@
+#pragma once
+
+#include <yaml-cpp/yaml.h>
+#include <deque>
+#include <functional>
+#include <memory>
+#include <mutex>
+
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
+
+#include "flightlib/common/logger.hpp"
+#include "flightlib/common/types.hpp"
+#include "flightlib/sensors/sensor_base.hpp"
+
+namespace flightlib {
+
+enum CameraLayer { RGB = 0, DepthMap = 1, Segmentation = 2, OpticalFlow = 3 };
+
+namespace RGBCameraTypes {
+  typedef int8_t Intensity_t;
+  typedef cv::Mat Image_t;
+
+  struct RGBImage_t {
+    Image_t image;
+    USecs elapsed_useconds;
+  };
+  struct Depthmap_t {
+    Image_t image;
+    USecs elapsed_useconds;
+  };
+  struct Segement_t {
+    Image_t image;
+    USecs elapsed_useconds;
+  };
+
+  struct OpticFlow_t {
+    Image_t image;
+    USecs elapsed_useconds;
+  };
+
+  typedef Eigen::Matrix4d Mat4_t;
+  typedef Eigen::Vector3d Vec3_t;
+
+  typedef std::function<Eigen::Vector3d()> GetPos_t;
+  typedef std::function<Eigen::Vector3d()> GetVel_t;
+  typedef std::function<Eigen::Vector3d()> GetAcc_t;
+  typedef std::function<Eigen::Quaterniond()> GetQuat_t;
+  typedef std::function<Eigen::Vector3d()> GetOmega_t;
+  typedef std::function<Eigen::Vector3d()> GetPsi_t;
+
+  const std::string RGB = "rgb";
+  // image post processing
+  typedef std::string PostProcessingID;
+  const PostProcessingID Depth = "depth";
+  const PostProcessingID OpticalFlow = "optical_flow";
+  const PostProcessingID ObjectSegment = "object_segment";      // object segmentation
+  const PostProcessingID CategorySegment = "category_segment";  // category segmentation
+}  // namespace RGBCameraTypes
+
+class RGBCamera : SensorBase {
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  RGBCamera();
+  ~RGBCamera();
+
+  // public set functions
+  bool setRelPose(const Ref<Vector<3>> B_r_BC, const Ref<Matrix<3, 3>> R_BC);
+  bool setWidth(const int width);
+  bool setHeight(const int height);
+  bool setFOV(const Scalar fov);
+  bool setDepthScale(const Scalar depth_scale);
+  bool setPostProcesscing(const std::vector<bool>& enabled_layers);
+  bool feedImageQueue(const int image_layer, const cv::Mat& image_mat);
+  void clearImageQueue();
+
+  // public get functions
+  std::vector<bool> getEnabledLayers(void) const;
+  std::vector<std::string> GetPostProcessing(void);
+  Matrix<4, 4> getRelPose(void) const;
+  int getChannels(void) const;
+  int getWidth(void) const;
+  int getHeight(void) const;
+  Scalar getFOV(void) const;
+  Scalar getDepthScale(void) const;
+  bool getRGBImage(cv::Mat& rgb_img);
+  bool getDepthMap(cv::Mat& depth_map);
+  bool getSegmentation(cv::Mat& segmentation);
+  bool getOpticalFlow(cv::Mat& opticalflow);
+
+  // auxiliary functions
+  void enableDepth(const bool on);
+  void enableOpticalFlow(const bool on);
+  void enableSegmentation(const bool on);
+
+ private:
+  Logger logger_{"RBGCamera"};
+
+  // camera parameters
+  int channels_;
+  int width_;
+  int height_;
+  Scalar fov_;
+  Scalar depth_scale_;
+
+  // Camera relative
+  Vector<3> B_r_BC_;
+  Matrix<4, 4> T_BC_;
+
+  // image data buffer
+  std::mutex queue_mutex_;
+  const int queue_size_ = 0;  // 1
+
+  // TODO：不要用队列，就单纯的Mat就好了，也不会有滞留的问题，也不会有弹空的问题；先不改了省的出错
+  std::deque<cv::Mat> rgb_queue_;
+  std::deque<cv::Mat> depth_queue_;
+  std::deque<cv::Mat> opticalflow_queue_;
+  std::deque<cv::Mat> segmentation_queue_;
+
+  // [rgb, depth, segmentation, optical flow]
+  std::vector<bool> enabled_layers_;
+  // [depth, optical flow, segmentation, segmentation]
+  std::unordered_map<RGBCameraTypes::PostProcessingID, bool> post_processing_;
+};
+
+}  // namespace flightlib
--- a/flightlib/include/flightlib/sensors/sensor_base.hpp
+++ b/flightlib/include/flightlib/sensors/sensor_base.hpp
@@ -0,0 +1,14 @@
+#pragma once
+
+#include "flightlib/common/types.hpp"
+
+namespace flightlib {
+class SensorBase {
+ public:
+  SensorBase();
+  virtual ~SensorBase();
+
+ private:
+};
+
+}  // namespace flightlib
--- a/flightlib/include/flightlib/sensors/sgm_gpu/configuration.h
+++ b/flightlib/include/flightlib/sensors/sgm_gpu/configuration.h
@@ -0,0 +1,39 @@
+/***********************************************************************
+  Copyright (C) 2020 Hironori Fujimoto
+
+  This program is free software: you can redistribute it and/or modify
+  it under the terms of the GNU General Public License as published by
+  the Free Software Foundation, either version 3 of the License, or
+  (at your option) any later version.
+ 
+  This program is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  GNU General Public License for more details.
+  You should have received a copy of the GNU General Public License
+  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+***********************************************************************/
+
+#ifndef SGM_GPU__CONFIGURATION_H_
+#define SGM_GPU__CONFIGURATION_H_
+
+#include <stdint.h>
+
+#define	MAX_DISPARITY 128
+#define CENSUS_WIDTH  9
+#define CENSUS_HEIGHT 7
+
+#define TOP  (CENSUS_HEIGHT-1)/2
+#define LEFT (CENSUS_WIDTH-1)/2
+
+namespace sgm_gpu
+{
+
+typedef uint32_t cost_t;
+
+}
+
+#define COSTAGG_BLOCKSIZE       GPU_THREADS_PER_BLOCK
+#define COSTAGG_BLOCKSIZE_HORIZ GPU_THREADS_PER_BLOCK
+
+#endif // SGM_GPU__CONFIGURATION_H_
--- a/flightlib/include/flightlib/sensors/sgm_gpu/cost_aggregation.h
+++ b/flightlib/include/flightlib/sensors/sgm_gpu/cost_aggregation.h
@@ -0,0 +1,511 @@
+/***********************************************************************
+  Copyright (C) 2019 Hironori Fujimoto
+
+  This program is free software: you can redistribute it and/or modify
+  it under the terms of the GNU General Public License as published by
+  the Free Software Foundation, either version 3 of the License, or
+  (at your option) any later version.
+
+  This program is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  GNU General Public License for more details.
+  You should have received a copy of the GNU General Public License
+  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+***********************************************************************/
+#include "flightlib/sensors/sgm_gpu/util.h"
+
+#ifndef COST_AGGREGATION_H_
+#define COST_AGGREGATION_H_
+
+#define ITER_COPY			0
+#define ITER_NORMAL			1
+
+#define MIN_COMPUTE			0
+#define MIN_NOCOMPUTE		1
+
+#define DIR_UPDOWN			0
+#define DIR_DOWNUP			1
+#define DIR_LEFTRIGHT		2
+#define DIR_RIGHTLEFT		3
+
+namespace sgm_gpu
+{
+
+template<int add_col, bool recompute, bool join_dispcomputation>
+__device__ __forceinline__ void CostAggregationGenericIndexesIncrement(int *index, int *index_im, int *col, const int add_index, const int add_imindex) {
+  *index += add_index;
+  if(recompute || join_dispcomputation) {
+    *index_im += add_imindex;
+    if(recompute) {
+      *col += add_col;
+    }
+  }
+}
+
+template<int add_index, bool recompute, bool join_dispcomputation>
+__device__ __forceinline__ void CostAggregationDiagonalGenericIndexesIncrement(int *index, int *index_im, int *col, const int cols, const int initial_row, const int i, const int dis) {
+  *col += add_index;
+  if(add_index > 0 && *col >= cols) {
+    *col = 0;
+  } else if(*col < 0) {
+    *col = cols-1;
+  }
+  *index = abs(initial_row-i)*cols*MAX_DISPARITY+*col*MAX_DISPARITY+dis;
+  if(recompute || join_dispcomputation) {
+    *index_im = abs(initial_row-i)*cols+*col;
+  }
+}
+
+template<class T, int iter_type, int min_type, int dir_type, bool first_iteration, bool recompute, bool join_dispcomputation>
+__device__ __forceinline__ void CostAggregationGenericIteration(int index, int index_im, int col, uint32_t *old_values, int *old_value1, int *old_value2, int *old_value3, int *old_value4, uint32_t *min_cost, uint32_t *min_cost_p2, uint8_t* d_cost, uint8_t *d_L, uint16_t *d_s, const int p1_vector, const int p2_vector, const T *_d_transform0, const T *_d_transform1, const int lane, const int MAX_PAD, const int dis, T *rp0, T *rp1, T *rp2, T *rp3, uint8_t* __restrict__ d_disparity, const uint8_t* d_L0, const uint8_t* d_L1, const uint8_t* d_L2, const uint8_t* d_L3, const uint8_t* d_L4, const uint8_t* d_L5, const uint8_t* d_L6) {
+  const T __restrict__ *d_transform0 = _d_transform0;
+  const T __restrict__ *d_transform1 = _d_transform1;
+  uint32_t costs, next_dis, prev_dis;
+
+  if(iter_type == ITER_NORMAL) {
+    // First shuffle
+    int prev_dis1 = shfl_up_32(*old_value4, 1);
+    if(lane == 0) {
+      prev_dis1 = MAX_PAD;
+    }
+
+    // Second shuffle
+    int next_dis4 = shfl_down_32(*old_value1, 1);
+    if(lane == 31) {
+      next_dis4 = MAX_PAD;
+    }
+
+    // Shift + rotate
+    //next_dis = __funnelshift_r(next_dis4, *old_values, 8);
+    next_dis = __byte_perm(*old_values, next_dis4, 0x4321);
+    prev_dis = __byte_perm(*old_values, prev_dis1, 0x2104);
+
+    next_dis = next_dis + p1_vector;
+    prev_dis = prev_dis + p1_vector;
+  }
+  if(recompute) {
+    const int dif = col - dis;
+    if(dir_type == DIR_LEFTRIGHT) {
+      if(lane == 0) {
+        // lane = 0 is dis = 0, no need to subtract dis
+        *rp0 = d_transform1[index_im];
+      }
+    } else if(dir_type == DIR_RIGHTLEFT) {
+      // First iteration, load D pixels
+      if(first_iteration) {
+        const uint4 right = reinterpret_cast<const uint4*>(&d_transform1[index_im-dis-3])[0];
+        *rp3 = right.x;
+        *rp2 = right.y;
+        *rp1 = right.z;
+        *rp0 = right.w;
+      } else if(lane == 31 && dif >= 3) {
+        *rp3 = d_transform1[index_im-dis-3];
+      }
+    } else {
+  /*
+      __shared__ T right_p[MAX_DISPARITY+32];
+      const int warp_id = threadIdx.x / WARP_SIZE;
+      if(warp_id < 5) {
+        const int block_imindex = index_im - warp_id + 32;
+        const int rp_index = warp_id*WARP_SIZE+lane;
+        const int col_cpy = col-warp_id+32;
+        right_p[rp_index] = ((col_cpy-(159-rp_index)) >= 0) ? ld_gbl_cs(&d_transform1[block_imindex-(159-rp_index)]) : 0;
+      }*/
+
+      __shared__ T right_p[128+32];
+      const int warp_id = threadIdx.x / WARP_SIZE;
+      const int block_imindex = index_im - warp_id + 2;
+      const int rp_index = warp_id*WARP_SIZE+lane;
+      const int col_cpy = col-warp_id+2;
+      right_p[rp_index] = ((col_cpy-(129-rp_index)) >= 0) ? d_transform1[block_imindex-(129-rp_index)] : 0;
+      right_p[rp_index+64] = ((col_cpy-(129-rp_index-64)) >= 0) ? d_transform1[block_imindex-(129-rp_index-64)] : 0;
+      //right_p[rp_index+128] = ld_gbl_cs(&d_transform1[block_imindex-(129-rp_index-128)]);
+      if(warp_id == 0) {
+        right_p[128+lane] = ld_gbl_cs(&d_transform1[block_imindex-(129-lane)]);
+      }
+      __syncthreads();
+
+      const int px = MAX_DISPARITY+warp_id-dis-1;
+      *rp0 = right_p[px];
+      *rp1 = right_p[px-1];
+      *rp2 = right_p[px-2];
+      *rp3 = right_p[px-3];
+    }
+    const T left_pixel = d_transform0[index_im];
+    *old_value1 = popcount(left_pixel ^ *rp0);
+    *old_value2 = popcount(left_pixel ^ *rp1);
+    *old_value3 = popcount(left_pixel ^ *rp2);
+    *old_value4 = popcount(left_pixel ^ *rp3);
+    if(iter_type == ITER_COPY) {
+      *old_values = uchars_to_uint32(*old_value1, *old_value2, *old_value3, *old_value4);
+    } else {
+      costs = uchars_to_uint32(*old_value1, *old_value2, *old_value3, *old_value4);
+    }
+    // Prepare for next iteration
+    if(dir_type == DIR_LEFTRIGHT) {
+      *rp3 = shfl_up_32(*rp3, 1);
+    } else if(dir_type == DIR_RIGHTLEFT) {
+      *rp0 = shfl_down_32(*rp0, 1);
+    }
+  } else {
+    if(iter_type == ITER_COPY) {
+      *old_values = ld_gbl_ca(reinterpret_cast<const uint32_t*>(&d_cost[index]));
+    } else {
+      costs = ld_gbl_ca(reinterpret_cast<const uint32_t*>(&d_cost[index]));
+    }
+  }
+
+  if(iter_type == ITER_NORMAL) {
+    const uint32_t min1 = __vminu4(*old_values, prev_dis);
+    const uint32_t min2 = __vminu4(next_dis, *min_cost_p2);
+    const uint32_t min_prev = __vminu4(min1, min2);
+    *old_values = costs + (min_prev - *min_cost);
+  }
+  if(iter_type == ITER_NORMAL || !recompute) {
+    uint32_to_uchars(*old_values, old_value1, old_value2, old_value3, old_value4);
+  }
+
+  if(join_dispcomputation) {
+    const uint32_t L0_costs = *((uint32_t*) (d_L0+index));
+    const uint32_t L1_costs = *((uint32_t*) (d_L1+index));
+    const uint32_t L2_costs = *((uint32_t*) (d_L2+index));
+    const uint32_t L3_costs = *((uint32_t*) (d_L3+index));
+    const uint32_t L4_costs = *((uint32_t*) (d_L4+index));
+    const uint32_t L5_costs = *((uint32_t*) (d_L5+index));
+    const uint32_t L6_costs = *((uint32_t*) (d_L6+index));
+
+    int l0_x, l0_y, l0_z, l0_w;
+    int l1_x, l1_y, l1_z, l1_w;
+    int l2_x, l2_y, l2_z, l2_w;
+    int l3_x, l3_y, l3_z, l3_w;
+    int l4_x, l4_y, l4_z, l4_w;
+    int l5_x, l5_y, l5_z, l5_w;
+    int l6_x, l6_y, l6_z, l6_w;
+
+    uint32_to_uchars(L0_costs, &l0_x, &l0_y, &l0_z, &l0_w);
+    uint32_to_uchars(L1_costs, &l1_x, &l1_y, &l1_z, &l1_w);
+    uint32_to_uchars(L2_costs, &l2_x, &l2_y, &l2_z, &l2_w);
+    uint32_to_uchars(L3_costs, &l3_x, &l3_y, &l3_z, &l3_w);
+    uint32_to_uchars(L4_costs, &l4_x, &l4_y, &l4_z, &l4_w);
+    uint32_to_uchars(L5_costs, &l5_x, &l5_y, &l5_z, &l5_w);
+    uint32_to_uchars(L6_costs, &l6_x, &l6_y, &l6_z, &l6_w);
+
+    const uint16_t val1 = l0_x + l1_x + l2_x + l3_x + l4_x + l5_x + l6_x + *old_value1;
+    const uint16_t val2 = l0_y + l1_y + l2_y + l3_y + l4_y + l5_y + l6_y + *old_value2;
+    const uint16_t val3 = l0_z + l1_z + l2_z + l3_z + l4_z + l5_z + l6_z + *old_value3;
+    const uint16_t val4 = l0_w + l1_w + l2_w + l3_w + l4_w + l5_w + l6_w + *old_value4;
+
+    int min_idx1 = dis;
+    uint16_t min1 = val1;
+    if(val1 > val2) {
+      min1 = val2;
+      min_idx1 = dis+1;
+    }
+
+    int min_idx2 = dis+2;
+    uint16_t min2 = val3;
+    if(val3 > val4) {
+      min2 = val4;
+      min_idx2 = dis+3;
+    }
+
+    uint16_t minval = min1;
+    int min_idx = min_idx1;
+    if(min1 > min2) {
+      minval = min2;
+      min_idx = min_idx2;
+    }
+
+    const int min_warpindex = warpReduceMinIndex(minval, min_idx);
+    if(lane == 0) {
+      d_disparity[index_im] = min_warpindex;
+    }
+    
+    // Save smoothed cost to obtain right disparity
+    d_s[index] = val1;
+    d_s[index+1] = val2;
+    d_s[index+2] = val3;
+    d_s[index+3] = val4;
+  } else {
+    st_gbl_cs(reinterpret_cast<uint32_t*>(&d_L[index]), *old_values);
+  }
+  if(min_type == MIN_COMPUTE) {
+    int min_cost_scalar = min(min(*old_value1, *old_value2), min(*old_value3, *old_value4));
+    *min_cost = uchar_to_uint32(warpReduceMin(min_cost_scalar));
+    *min_cost_p2 = *min_cost + p2_vector;
+  }
+}
+
+template<class T, int add_col, int dir_type, bool recompute, bool join_dispcomputation>
+__device__ __forceinline__ void CostAggregationGeneric(uint8_t* d_cost, uint8_t *d_L, uint16_t *d_s, const int P1, const int P2, const int initial_row, const int initial_col, const int max_iter, const int cols, int add_index, const T *_d_transform0, const T *_d_transform1, const int add_imindex, uint8_t* __restrict__ d_disparity, const uint8_t* d_L0, const uint8_t* d_L1, const uint8_t* d_L2, const uint8_t* d_L3, const uint8_t* d_L4, const uint8_t* d_L5, const uint8_t* d_L6) {
+  const int lane = threadIdx.x % WARP_SIZE;
+  const int dis = 4*lane;
+  int index = initial_row*cols*MAX_DISPARITY+initial_col*MAX_DISPARITY+dis;
+  int col, index_im;
+  if(recompute || join_dispcomputation) {
+    if(recompute) {
+      col = initial_col;
+    }
+    index_im = initial_row*cols+initial_col;
+  }
+
+  const int MAX_PAD = UCHAR_MAX-P1;
+  const uint32_t p1_vector = uchars_to_uint32(P1, P1, P1, P1);
+  const uint32_t p2_vector = uchars_to_uint32(P2, P2, P2, P2);
+  int old_value1;
+  int old_value2;
+  int old_value3;
+  int old_value4;
+  uint32_t min_cost, min_cost_p2, old_values;
+  T rp0, rp1, rp2, rp3;
+
+  if(recompute) {
+    if(dir_type == DIR_LEFTRIGHT) {
+      CostAggregationGenericIteration<T, ITER_COPY, MIN_COMPUTE, dir_type, true, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp0, &rp1, &rp2, &rp3, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
+      CostAggregationGenericIndexesIncrement<add_col, recompute, join_dispcomputation>(&index, &index_im, &col, add_index, add_imindex);
+      CostAggregationGenericIteration<T, ITER_NORMAL, MIN_COMPUTE, dir_type, false, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp3, &rp0, &rp1, &rp2, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
+      CostAggregationGenericIndexesIncrement<add_col, recompute, join_dispcomputation>(&index, &index_im, &col, add_index, add_imindex);
+      CostAggregationGenericIteration<T, ITER_NORMAL, MIN_COMPUTE, dir_type, false, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp2, &rp3, &rp0, &rp1, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
+      CostAggregationGenericIndexesIncrement<add_col, recompute, join_dispcomputation>(&index, &index_im, &col, add_index, add_imindex);
+      CostAggregationGenericIteration<T, ITER_NORMAL, MIN_COMPUTE, dir_type, false, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp1, &rp2, &rp3, &rp0, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
+      for(int i = 4; i < max_iter-3; i+=4) {
+        CostAggregationGenericIndexesIncrement<add_col, recompute, join_dispcomputation>(&index, &index_im, &col, add_index, add_imindex);
+        CostAggregationGenericIteration<T, ITER_NORMAL, MIN_COMPUTE, dir_type, false, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp0, &rp1, &rp2, &rp3, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
+        CostAggregationGenericIndexesIncrement<add_col, recompute, join_dispcomputation>(&index, &index_im, &col, add_index, add_imindex);
+        CostAggregationGenericIteration<T, ITER_NORMAL, MIN_COMPUTE, dir_type, false, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp3, &rp0, &rp1, &rp2, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
+        CostAggregationGenericIndexesIncrement<add_col, recompute, join_dispcomputation>(&index, &index_im, &col, add_index, add_imindex);
+        CostAggregationGenericIteration<T, ITER_NORMAL, MIN_COMPUTE, dir_type, false, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp2, &rp3, &rp0, &rp1, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
+        CostAggregationGenericIndexesIncrement<add_col, recompute, join_dispcomputation>(&index, &index_im, &col, add_index, add_imindex);
+        CostAggregationGenericIteration<T, ITER_NORMAL, MIN_COMPUTE, dir_type, false, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp1, &rp2, &rp3, &rp0, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
+      }
+      CostAggregationGenericIndexesIncrement<add_col, recompute, join_dispcomputation>(&index, &index_im, &col, add_index, add_imindex);
+      CostAggregationGenericIteration<T, ITER_NORMAL, MIN_COMPUTE, dir_type, false, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp0, &rp1, &rp2, &rp3, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
+      CostAggregationGenericIndexesIncrement<add_col, recompute, join_dispcomputation>(&index, &index_im, &col, add_index, add_imindex);
+      CostAggregationGenericIteration<T, ITER_NORMAL, MIN_COMPUTE, dir_type, false, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp3, &rp0, &rp1, &rp2, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
+      CostAggregationGenericIndexesIncrement<add_col, recompute, join_dispcomputation>(&index, &index_im, &col, add_index, add_imindex);
+      CostAggregationGenericIteration<T, ITER_NORMAL, MIN_COMPUTE, dir_type, false, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp2, &rp3, &rp0, &rp1, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
+      CostAggregationGenericIndexesIncrement<add_col, recompute, join_dispcomputation>(&index, &index_im, &col, add_index, add_imindex);
+      CostAggregationGenericIteration<T, ITER_NORMAL, MIN_NOCOMPUTE, dir_type, false, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp1, &rp2, &rp3, &rp0, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
+    } else if(dir_type == DIR_RIGHTLEFT) {
+      CostAggregationGenericIteration<T, ITER_COPY, MIN_COMPUTE, dir_type, true, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp0, &rp1, &rp2, &rp3, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
+      CostAggregationGenericIndexesIncrement<add_col, recompute, join_dispcomputation>(&index, &index_im, &col, add_index, add_imindex);
+      CostAggregationGenericIteration<T, ITER_NORMAL, MIN_COMPUTE, dir_type, false, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp1, &rp2, &rp3, &rp0, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
+      CostAggregationGenericIndexesIncrement<add_col, recompute, join_dispcomputation>(&index, &index_im, &col, add_index, add_imindex);
+      CostAggregationGenericIteration<T, ITER_NORMAL, MIN_COMPUTE, dir_type, false, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp2, &rp3, &rp0, &rp1, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
+      CostAggregationGenericIndexesIncrement<add_col, recompute, join_dispcomputation>(&index, &index_im, &col, add_index, add_imindex);
+      CostAggregationGenericIteration<T, ITER_NORMAL, MIN_COMPUTE, dir_type, false, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp3, &rp0, &rp1, &rp2, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
+      for(int i = 4; i < max_iter-3; i+=4) {
+        CostAggregationGenericIndexesIncrement<add_col, recompute, join_dispcomputation>(&index, &index_im, &col, add_index, add_imindex);
+        CostAggregationGenericIteration<T, ITER_NORMAL, MIN_COMPUTE, dir_type, false, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp0, &rp1, &rp2, &rp3, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
+        CostAggregationGenericIndexesIncrement<add_col, recompute, join_dispcomputation>(&index, &index_im, &col, add_index, add_imindex);
+        CostAggregationGenericIteration<T, ITER_NORMAL, MIN_COMPUTE, dir_type, false, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp1, &rp2, &rp3, &rp0, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
+        CostAggregationGenericIndexesIncrement<add_col, recompute, join_dispcomputation>(&index, &index_im, &col, add_index, add_imindex);
+        CostAggregationGenericIteration<T, ITER_NORMAL, MIN_COMPUTE, dir_type, false, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp2, &rp3, &rp0, &rp1, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
+        CostAggregationGenericIndexesIncrement<add_col, recompute, join_dispcomputation>(&index, &index_im, &col, add_index, add_imindex);
+        CostAggregationGenericIteration<T, ITER_NORMAL, MIN_COMPUTE, dir_type, false, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp3, &rp0, &rp1, &rp2, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
+      }
+      CostAggregationGenericIndexesIncrement<add_col, recompute, join_dispcomputation>(&index, &index_im, &col, add_index, add_imindex);
+      CostAggregationGenericIteration<T, ITER_NORMAL, MIN_COMPUTE, dir_type, false, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp0, &rp1, &rp2, &rp3, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
+      CostAggregationGenericIndexesIncrement<add_col, recompute, join_dispcomputation>(&index, &index_im, &col, add_index, add_imindex);
+      CostAggregationGenericIteration<T, ITER_NORMAL, MIN_COMPUTE, dir_type, false, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp1, &rp2, &rp3, &rp0, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
+      CostAggregationGenericIndexesIncrement<add_col, recompute, join_dispcomputation>(&index, &index_im, &col, add_index, add_imindex);
+      CostAggregationGenericIteration<T, ITER_NORMAL, MIN_COMPUTE, dir_type, false, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp2, &rp3, &rp0, &rp1, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
+      CostAggregationGenericIndexesIncrement<add_col, recompute, join_dispcomputation>(&index, &index_im, &col, add_index, add_imindex);
+      CostAggregationGenericIteration<T, ITER_NORMAL, MIN_NOCOMPUTE, dir_type, false, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp3, &rp0, &rp1, &rp2, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
+    } else {
+      CostAggregationGenericIteration<T, ITER_COPY, MIN_COMPUTE, dir_type, true, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp0, &rp1, &rp2, &rp3, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
+      for(int i = 1; i < max_iter; i++) {
+        CostAggregationGenericIndexesIncrement<add_col, recompute, join_dispcomputation>(&index, &index_im, &col, add_index, add_imindex);
+        CostAggregationGenericIteration<T, ITER_NORMAL, MIN_COMPUTE, dir_type, false, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp0, &rp1, &rp2, &rp3, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
+      }
+      CostAggregationGenericIndexesIncrement<add_col, recompute, join_dispcomputation>(&index, &index_im, &col, add_index, add_imindex);
+      CostAggregationGenericIteration<T, ITER_NORMAL, MIN_NOCOMPUTE, dir_type, false, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp0, &rp1, &rp2, &rp3, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
+    }
+  } else {
+    CostAggregationGenericIteration<T, ITER_COPY, MIN_COMPUTE, dir_type, true, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp0, &rp1, &rp2, &rp3, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
+
+    for(int i = 1; i < max_iter; i++) {
+      CostAggregationGenericIndexesIncrement<add_col, recompute, join_dispcomputation>(&index, &index_im, &col, add_index, add_imindex);
+      CostAggregationGenericIteration<T, ITER_NORMAL, MIN_COMPUTE, dir_type, false, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp0, &rp1, &rp2, &rp3, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
+    }
+    CostAggregationGenericIndexesIncrement<add_col, recompute, join_dispcomputation>(&index, &index_im, &col, add_index, add_imindex);
+    CostAggregationGenericIteration<T, ITER_NORMAL, MIN_NOCOMPUTE, dir_type, false, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp0, &rp1, &rp2, &rp3, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
+  }
+}
+
+template<int add_index, class T, int dir_type, bool recompute, bool join_dispcomputation>
+__device__ __forceinline__ void CostAggregationDiagonalGeneric(uint8_t* d_cost, uint8_t *d_L, uint16_t *d_s, const int P1, const int P2, const int initial_row, const int initial_col, const int max_iter, const int col_nomin, const int col_copycost, const int cols, const T *_d_transform0, const T *_d_transform1, uint8_t* __restrict__ d_disparity, const uint8_t* d_L0, const uint8_t* d_L1, const uint8_t* d_L2, const uint8_t* d_L3, const uint8_t* d_L4, const uint8_t* d_L5, const uint8_t* d_L6) {
+  const int lane = threadIdx.x % WARP_SIZE;
+  const int dis = 4*lane;
+  int col = initial_col;
+  int index = initial_row*cols*MAX_DISPARITY+initial_col*MAX_DISPARITY+dis;
+  int index_im;
+  if(recompute || join_dispcomputation) {
+    index_im = initial_row*cols+col;
+  }
+  const int MAX_PAD = UCHAR_MAX-P1;
+  const uint32_t p1_vector = uchars_to_uint32(P1, P1, P1, P1);
+  const uint32_t p2_vector = uchars_to_uint32(P2, P2, P2, P2);
+  int old_value1;
+  int old_value2;
+  int old_value3;
+  int old_value4;
+  uint32_t min_cost, min_cost_p2, old_values;
+  T rp0, rp1, rp2, rp3;
+
+  CostAggregationGenericIteration<T, ITER_COPY, MIN_COMPUTE, dir_type, true, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp0, &rp1, &rp2, &rp3, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
+  for(int i = 1; i < max_iter; i++) {
+    CostAggregationDiagonalGenericIndexesIncrement<add_index, recompute, join_dispcomputation>(&index, &index_im, &col, cols, initial_row, i, dis);
+    if(col == col_copycost) {
+      CostAggregationGenericIteration<T, ITER_COPY, MIN_COMPUTE, dir_type, false, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp0, &rp1, &rp2, &rp3, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
+    } else {
+      CostAggregationGenericIteration<T, ITER_NORMAL, MIN_COMPUTE, dir_type, false, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp0, &rp1, &rp2, &rp3, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
+    }
+  }
+
+  CostAggregationDiagonalGenericIndexesIncrement<add_index, recompute, join_dispcomputation>(&index, &index_im, &col, cols, max_iter, initial_row, dis);
+  if(col == col_copycost) {
+    CostAggregationGenericIteration<T, ITER_COPY, MIN_NOCOMPUTE, dir_type, false, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp0, &rp1, &rp2, &rp3, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
+  } else {
+    CostAggregationGenericIteration<T, ITER_NORMAL, MIN_NOCOMPUTE, dir_type, false, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp0, &rp1, &rp2, &rp3, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
+  }
+}
+
+template<class T>
+
+__global__ void CostAggregationKernelDiagonalDownUpRightLeft(uint8_t* d_cost, uint8_t *d_L, uint16_t *d_s, const int P1, const int P2, const int rows, const int cols, const T *d_transform0, const T *d_transform1, uint8_t* __restrict__ d_disparity, const uint8_t* d_L0, const uint8_t* d_L1, const uint8_t* d_L2, const uint8_t* d_L3, const uint8_t* d_L4, const uint8_t* d_L5, const uint8_t* d_L6) {
+  const int initial_col = cols - (blockIdx.x*(blockDim.x/WARP_SIZE) + (threadIdx.x / WARP_SIZE)) - 1;
+  if(initial_col < cols) {
+    const int initial_row = rows-1;
+    const int add_index = -1;
+    const int col_nomin = 0;
+    const int col_copycost = cols-1;
+    const int max_iter = rows-1;
+    const bool recompute = false;
+    const bool join_dispcomputation = false;
+
+    CostAggregationDiagonalGeneric<add_index, T, DIR_DOWNUP, recompute, join_dispcomputation>(d_cost, d_L, d_s, P1, P2, initial_row, initial_col, max_iter, col_nomin, col_copycost, cols, d_transform0, d_transform1, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
+  }
+}
+
+template<class T>
+__global__ void CostAggregationKernelDiagonalDownUpLeftRight(uint8_t* d_cost, uint8_t *d_L, uint16_t *d_s, const int P1, const int P2, const int rows, const int cols, const T *d_transform0, const T *d_transform1, uint8_t* __restrict__ d_disparity, const uint8_t* d_L0, const uint8_t* d_L1, const uint8_t* d_L2, const uint8_t* d_L3, const uint8_t* d_L4, const uint8_t* d_L5, const uint8_t* d_L6) {
+  const int initial_col = cols - (blockIdx.x*(blockDim.x/WARP_SIZE) + (threadIdx.x / WARP_SIZE)) - 1;
+  if(initial_col >= 0) {
+    const int initial_row = rows-1;
+    const int add_index = 1;
+    const int col_nomin = cols-1;
+    const int col_copycost = 0;
+    const int max_iter = rows-1;
+    const bool recompute = false;
+    const bool join_dispcomputation = false;
+
+    CostAggregationDiagonalGeneric<add_index, T, DIR_DOWNUP, recompute, join_dispcomputation>(d_cost, d_L, d_s, P1, P2, initial_row, initial_col, max_iter, col_nomin, col_copycost, cols, d_transform0, d_transform1, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
+  }
+}
+
+template<class T>
+
+__global__ void CostAggregationKernelDiagonalUpDownRightLeft(uint8_t* d_cost, uint8_t *d_L, uint16_t *d_s, const int P1, const int P2, const int rows, const int cols, const T *d_transform0, const T *d_transform1, uint8_t* __restrict__ d_disparity, const uint8_t* d_L0, const uint8_t* d_L1, const uint8_t* d_L2, const uint8_t* d_L3, const uint8_t* d_L4, const uint8_t* d_L5, const uint8_t* d_L6) {
+  const int initial_col = blockIdx.x*(blockDim.x/WARP_SIZE) + (threadIdx.x / WARP_SIZE);
+  if(initial_col < cols) {
+    const int initial_row = 0;
+    const int add_index = -1;
+    const int col_nomin = 0;
+    const int col_copycost = cols-1;
+    const int max_iter = rows-1;
+    const bool recompute = false;
+    const bool join_dispcomputation = true;
+
+    CostAggregationDiagonalGeneric<add_index, T, DIR_UPDOWN, recompute, join_dispcomputation>(d_cost, d_L, d_s, P1, P2, initial_row, initial_col, max_iter, col_nomin, col_copycost, cols, d_transform0, d_transform1, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
+  }
+}
+
+template<class T>
+
+__global__ void CostAggregationKernelDiagonalUpDownLeftRight(uint8_t* d_cost, uint8_t *d_L, uint16_t *d_s, const int P1, const int P2, const int rows, const int cols, const T *d_transform0, const T *d_transform1, uint8_t* __restrict__ d_disparity, const uint8_t* d_L0, const uint8_t* d_L1, const uint8_t* d_L2, const uint8_t* d_L3, const uint8_t* d_L4, const uint8_t* d_L5, const uint8_t* d_L6) {
+  const int initial_col = blockIdx.x*(blockDim.x/WARP_SIZE) + (threadIdx.x / WARP_SIZE);
+  if(initial_col < cols) {
+    const int initial_row = 0;
+    const int add_index = 1;
+    const int col_nomin = cols-1;
+    const int col_copycost = 0;
+    const int max_iter = rows-1;
+    const bool recompute = false;
+    const bool join_dispcomputation = false;
+
+    CostAggregationDiagonalGeneric<add_index, T, DIR_UPDOWN, recompute, join_dispcomputation>(d_cost, d_L, d_s, P1, P2, initial_row, initial_col, max_iter, col_nomin, col_copycost, cols, d_transform0, d_transform1, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
+  }
+}
+
+template<class T>
+
+__global__ void CostAggregationKernelLeftToRight(uint8_t* d_cost, uint8_t *d_L, uint16_t *d_s, const int P1, const int P2, const int rows, const int cols, const T *d_transform0, const T *d_transform1, uint8_t* __restrict__ d_disparity, const uint8_t* d_L0, const uint8_t* d_L1, const uint8_t* d_L2, const uint8_t* d_L3, const uint8_t* d_L4, const uint8_t* d_L5, const uint8_t* d_L6) {
+  const int initial_row = blockIdx.x*(blockDim.x/WARP_SIZE) + (threadIdx.x / WARP_SIZE);
+  if(initial_row < rows) {
+    const int initial_col = 0;
+    const int add_index = MAX_DISPARITY;
+    const int add_imindex = 1;
+    const int max_iter = cols-1;
+    const int add_col = 1;
+    const bool recompute = true;
+    const bool join_dispcomputation = false;
+
+    CostAggregationGeneric<T, add_col, DIR_LEFTRIGHT, recompute, join_dispcomputation>(d_cost, d_L, d_s, P1, P2, initial_row, initial_col, max_iter, cols, add_index, d_transform0, d_transform1, add_imindex, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
+  }
+}
+
+template<class T>
+
+__global__ void CostAggregationKernelRightToLeft(uint8_t* d_cost, uint8_t *d_L, uint16_t *d_s, const int P1, const int P2, const int rows, const int cols, const T *d_transform0, const T *d_transform1, uint8_t* __restrict__ d_disparity, const uint8_t* d_L0, const uint8_t* d_L1, const uint8_t* d_L2, const uint8_t* d_L3, const uint8_t* d_L4, const uint8_t* d_L5, const uint8_t* d_L6) {
+  const int initial_row = blockIdx.x*(blockDim.x/WARP_SIZE) + (threadIdx.x / WARP_SIZE);
+  if(initial_row < rows) {
+    const int initial_col = cols-1;
+    const int add_index = -MAX_DISPARITY;
+    const int add_imindex = -1;
+    const int max_iter = cols-1;
+    const int add_col = -1;
+    const bool recompute = true;
+    const bool join_dispcomputation = false;
+
+    CostAggregationGeneric<T, add_col, DIR_RIGHTLEFT, recompute, join_dispcomputation>(d_cost, d_L, d_s, P1, P2, initial_row, initial_col, max_iter, cols, add_index, d_transform0, d_transform1, add_imindex, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
+  }
+}
+
+template<class T>
+__global__ void CostAggregationKernelDownToUp(uint8_t* d_cost, uint8_t *d_L, uint16_t *d_s, const int P1, const int P2, const int rows, const int cols, const T *d_transform0, const T *d_transform1, uint8_t* __restrict__ d_disparity, const uint8_t* d_L0, const uint8_t* d_L1, const uint8_t* d_L2, const uint8_t* d_L3, const uint8_t* d_L4, const uint8_t* d_L5, const uint8_t* d_L6) {
+  const int initial_col = blockIdx.x*(blockDim.x/WARP_SIZE) + (threadIdx.x / WARP_SIZE);
+  if(initial_col < cols) {
+    const int initial_row = rows-1;
+    const int add_index = -cols*MAX_DISPARITY;
+    const int add_imindex = -cols;
+    const int max_iter = rows-1;
+    const int add_col = 0;
+    const bool recompute = false;
+    const bool join_dispcomputation = false;
+
+    CostAggregationGeneric<T, add_col, DIR_DOWNUP, recompute, join_dispcomputation>(d_cost, d_L, d_s, P1, P2, initial_row, initial_col, max_iter, cols, add_index, d_transform0, d_transform1, add_imindex, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
+  }
+}
+
+template<class T>
+//__launch_bounds__(64, 16)
+__global__ void CostAggregationKernelUpToDown(uint8_t* d_cost, uint8_t *d_L, uint16_t* d_s, const int P1, const int P2, const int rows, const int cols, const T *d_transform0, const T *d_transform1, uint8_t* __restrict__ d_disparity, const uint8_t* d_L0, const uint8_t* d_L1, const uint8_t* d_L2, const uint8_t* d_L3, const uint8_t* d_L4, const uint8_t* d_L5, const uint8_t* d_L6) {
+  const int initial_col = blockIdx.x*(blockDim.x/WARP_SIZE) + (threadIdx.x / WARP_SIZE);
+  if(initial_col < cols) {
+    const int initial_row = 0;
+    const int add_index = cols*MAX_DISPARITY;
+    const int add_imindex = cols;
+    const int max_iter = rows-1;
+    const int add_col = 0;
+    const bool recompute = false;
+    const bool join_dispcomputation = false;
+
+    CostAggregationGeneric<T, add_col, DIR_UPDOWN, recompute, join_dispcomputation>(d_cost, d_L, d_s, P1, P2, initial_row, initial_col, max_iter, cols, add_index, d_transform0, d_transform1, add_imindex, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
+  }
+}
+
+} // namespace sgm_gpu
+
+#endif /* COST_AGGREGATION_H_ */
--- a/flightlib/include/flightlib/sensors/sgm_gpu/costs.h
+++ b/flightlib/include/flightlib/sensors/sgm_gpu/costs.h
@@ -0,0 +1,30 @@
+/***********************************************************************
+  Copyright (C) 2020 Hironori Fujimoto
+
+  This program is free software: you can redistribute it and/or modify
+  it under the terms of the GNU General Public License as published by
+  the Free Software Foundation, either version 3 of the License, or
+  (at your option) any later version.
+ 
+  This program is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  GNU General Public License for more details.
+  You should have received a copy of the GNU General Public License
+  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+***********************************************************************/
+#ifndef SGM_GPU__COSTS_H_
+#define SGM_GPU__COSTS_H_
+
+#include <stdint.h>
+#include "flightlib/sensors/sgm_gpu/configuration.h"
+
+namespace sgm_gpu
+{
+
+__global__ void CenterSymmetricCensusKernelSM2(const uint8_t *im, const uint8_t *im2, cost_t *transform, cost_t *transform2, const uint32_t rows, const uint32_t cols);
+
+}
+
+#endif // SGM_GPU__COSTS_H_
+
--- a/flightlib/include/flightlib/sensors/sgm_gpu/hamming_cost.h
+++ b/flightlib/include/flightlib/sensors/sgm_gpu/hamming_cost.h
@@ -0,0 +1,33 @@
+/***********************************************************************
+  Copyright (C) 2020 Hironori Fujimoto
+
+  This program is free software: you can redistribute it and/or modify
+  it under the terms of the GNU General Public License as published by
+  the Free Software Foundation, either version 3 of the License, or
+  (at your option) any later version.
+
+  This program is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  GNU General Public License for more details.
+  You should have received a copy of the GNU General Public License
+  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+***********************************************************************/
+
+#ifndef SGM_GPU__HAMMING_COST_H_
+#define SGM_GPU__HAMMING_COST_H_
+
+#include "flightlib/sensors/sgm_gpu/configuration.h"
+#include "flightlib/sensors/sgm_gpu/util.h"
+#include <stdint.h>
+
+namespace sgm_gpu
+{
+
+__global__ void
+HammingDistanceCostKernel(const cost_t *d_transform0, const cost_t *d_transform1, uint8_t *d_cost, const int rows, const int cols );
+
+}
+
+#endif // SGM_GPU__HAMMING_COST_H_ 
+
--- a/flightlib/include/flightlib/sensors/sgm_gpu/left_right_consistency.h
+++ b/flightlib/include/flightlib/sensors/sgm_gpu/left_right_consistency.h
@@ -0,0 +1,31 @@
+/***********************************************************************
+  Copyright (C) 2020 Hironori Fujimoto
+
+  This program is free software: you can redistribute it and/or modify
+  it under the terms of the GNU General Public License as published by
+  the Free Software Foundation, either version 3 of the License, or
+  (at your option) any later version.
+ 
+  This program is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  GNU General Public License for more details.
+  You should have received a copy of the GNU General Public License
+  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+***********************************************************************/
+
+#ifndef SGM_GPU__LEFT_RIGHT_CONSISTENCY_H_
+#define SGM_GPU__LEFT_RIGHT_CONSISTENCY_H_
+
+#include <stdint.h>
+
+namespace sgm_gpu
+{
+
+__global__ void ChooseRightDisparity(uint8_t *right_disparity, const uint16_t *smoothed_cost, const uint32_t rows, const uint32_t cols);
+__global__ void LeftRightConsistencyCheck(uint8_t *disparity, const uint8_t *disparity_right, const uint32_t rows, const uint32_t cols);
+
+}
+
+#endif // SGM_GPU__LEFT_RIGHT_CONSISTENCY_H_
+
--- a/flightlib/include/flightlib/sensors/sgm_gpu/median_filter.h
+++ b/flightlib/include/flightlib/sensors/sgm_gpu/median_filter.h
@@ -0,0 +1,62 @@
+/***********************************************************************
+  Copyright (C) 2020 Hironori Fujimoto
+
+  This program is free software: you can redistribute it and/or modify
+  it under the terms of the GNU General Public License as published by
+  the Free Software Foundation, either version 3 of the License, or
+  (at your option) any later version.
+
+  This program is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  GNU General Public License for more details.
+  You should have received a copy of the GNU General Public License
+  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+***********************************************************************/
+
+#ifndef SGM_GPU__MEDIAN_FILTER_H_
+#define SGM_GPU__MEDIAN_FILTER_H_
+
+#include <stdint.h>
+
+namespace sgm_gpu
+{
+
+__global__ void MedianFilter3x3(const uint8_t* __restrict__ d_input, uint8_t* __restrict__ d_out, const uint32_t rows, const uint32_t cols);
+
+template<int n, typename T>
+__inline__ __device__ void MedianFilter(const T* __restrict__ d_input, T* __restrict__ d_out, const uint32_t rows, const uint32_t cols) {
+  const uint32_t idx = blockIdx.x*blockDim.x+threadIdx.x;
+  const uint32_t row = idx / cols;
+  const uint32_t col = idx % cols;
+  T window[n*n];
+  int half = n/2;
+
+  if(row >= half && col >= half && row < rows-half && col < cols-half) {
+    for(uint32_t i = 0; i < n; i++) {
+      for(uint32_t j = 0; j < n; j++) {
+        window[i*n+j] = d_input[(row-half+i)*cols+col-half+j];
+      }
+    }
+
+    for(uint32_t i = 0; i < (n*n/2)+1; i++) {
+      uint32_t min_idx = i;
+      for(uint32_t j = i+1; j < n*n; j++) {
+        if(window[j] < window[min_idx]) {
+          min_idx = j;
+        }
+      }
+      const T tmp = window[i];
+      window[i] = window[min_idx];
+      window[min_idx] = tmp;
+    }
+    d_out[idx] = window[n*n/2];
+  } else if(row < rows && col < cols) {
+    d_out[idx] = d_input[idx];
+  }
+}
+
+} // namespace sgm_gpu
+
+#endif // SGM_GPU__MEDIAN_FILTER_H_
+
--- a/flightlib/include/flightlib/sensors/sgm_gpu/sgm_gpu.h
+++ b/flightlib/include/flightlib/sensors/sgm_gpu/sgm_gpu.h
@@ -0,0 +1,90 @@
+/***********************************************************************
+  Copyright (C) 2020 Hironori Fujimoto
+
+  This program is free software: you can redistribute it and/or modify
+  it under the terms of the GNU General Public License as published by
+  the Free Software Foundation, either version 3 of the License, or
+  (at your option) any later version.
+
+  This program is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  GNU General Public License for more details.
+  You should have received a copy of the GNU General Public License
+  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+***********************************************************************/
+#ifndef SGM_GPU__SGM_GPU_H_
+#define SGM_GPU__SGM_GPU_H_
+
+#include "flightlib/sensors/sgm_gpu/configuration.h"
+
+#include <opencv2/opencv.hpp>
+#include <opencv2/imgproc/types_c.h>
+
+namespace sgm_gpu {
+
+class SgmGpu {
+ private:
+  /**
+   * @brief Parameter used in SGM algorithm
+   *
+   * See SGM paper.
+   */
+  uint8_t p1_, p2_;
+  /**
+   * @brief Enable/disable left-right consistency check
+   */
+  bool check_consistency_;
+
+  // Memory for disparity computation
+  // d_: for device
+  uint8_t *d_im0_;
+  uint8_t *d_im1_;
+  uint32_t *d_transform0_;
+  uint32_t *d_transform1_;
+  uint8_t *d_cost_;
+  uint8_t *d_disparity_;
+  uint8_t *d_disparity_filtered_uchar_;
+  uint8_t *d_disparity_right_;
+  uint8_t *d_disparity_right_filtered_uchar_;
+  uint8_t *d_L0_;
+  uint8_t *d_L1_;
+  uint8_t *d_L2_;
+  uint8_t *d_L3_;
+  uint8_t *d_L4_;
+  uint8_t *d_L5_;
+  uint8_t *d_L6_;
+  uint8_t *d_L7_;
+  uint16_t *d_s_;
+
+  bool memory_allocated_;
+
+  uint32_t cols_, rows_;
+
+  void allocateMemory(uint32_t cols, uint32_t rows);
+  void freeMemory();
+
+  /**
+   * @brief Resize images to be width and height divisible by 4 for limit of
+   * CUDA code
+   */
+  void resizeToDivisibleBy4(cv::Mat &left_image, cv::Mat &right_image);
+
+//  void convertToMsg(const cv::Mat_<unsigned char> &disparity,
+//                    const sensor_msgs::CameraInfo &left_camera_info,
+//                    const sensor_msgs::CameraInfo &right_camera_info,
+//                    stereo_msgs::DisparityImage &disparity_msg);
+
+ public:
+  /**
+   * @brief Constructor which use namespace <parent>/libsgm_gpu for ROS param
+   */
+  SgmGpu(const int cols, const int rows);
+  ~SgmGpu();
+
+  bool computeDisparity(const cv::Mat &left_image, const cv::Mat &right_image, cv::Mat &disparity_out);
+};
+
+}  // namespace sgm_gpu
+
+#endif  // SGM_GPU__SGM_GPU_H_
--- a/flightlib/include/flightlib/sensors/sgm_gpu/util.h
+++ b/flightlib/include/flightlib/sensors/sgm_gpu/util.h
@@ -0,0 +1,362 @@
+/***********************************************************************
+  Copyright (C) 2020 Hironori Fujimoto
+
+  This program is free software: you can redistribute it and/or modify
+  it under the terms of the GNU General Public License as published by
+  the Free Software Foundation, either version 3 of the License, or
+  (at your option) any later version.
+
+  This program is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  GNU General Public License for more details.
+  You should have received a copy of the GNU General Public License
+  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+***********************************************************************/
+
+#ifndef SGM_GPU__UTIL_H_
+#define SGM_GPU__UTIL_H_
+
+#include <iostream>
+#include <dirent.h>
+#include <stdio.h>
+
+#define FERMI false
+
+#define GPU_THREADS_PER_BLOCK_FERMI 256
+#define GPU_THREADS_PER_BLOCK_MAXWELL 64
+
+/* Defines related to GPU Architecture */
+#if FERMI
+  #define GPU_THREADS_PER_BLOCK   GPU_THREADS_PER_BLOCK_FERMI
+#else
+  #define GPU_THREADS_PER_BLOCK   GPU_THREADS_PER_BLOCK_MAXWELL
+#endif
+
+#define WARP_SIZE		32
+
+namespace sgm_gpu
+{
+
+static void CheckCudaErrorAux (const char *, unsigned, const char *, cudaError_t);
+#define CUDA_CHECK_RETURN(value) CheckCudaErrorAux(__FILE__,__LINE__, #value, value)
+
+/**
+* Check the return value of the CUDA runtime API call and exit
+* the application if the call has failed.
+*/
+static void CheckCudaErrorAux (const char *file, unsigned line, const char *statement, cudaError_t err) {
+  if (err == cudaSuccess)
+    return;
+  std::cerr << statement<<" returned " << cudaGetErrorString(err) << "("<<err<< ") at "<<file<<":"<<line << std::endl;
+  exit (1);
+}
+
+/*************************************
+GPU Side defines (ASM instructions)
+**************************************/
+
+// output temporal carry in internal register
+#define UADD__CARRY_OUT(c, a, b) \
+  asm volatile("add.cc.u32 %0, %1, %2;" : "=r"(c) : "r"(a) , "r"(b))
+
+// add & output with temporal carry of internal register
+#define UADD__IN_CARRY_OUT(c, a, b) \
+  asm volatile("addc.cc.u32 %0, %1, %2;" : "=r"(c) : "r"(a) , "r"(b))
+
+// add with temporal carry of internal register
+#define UADD__IN_CARRY(c, a, b) \
+  asm volatile("addc.u32 %0, %1, %2;" : "=r"(c) : "r"(a) , "r"(b))
+
+// packing and unpacking: from uint64_t to uint2
+#define V2S_B64(v,s) \
+  asm("mov.b64 %0, {%1,%2};" : "=l"(s) : "r"(v.x), "r"(v.y))
+
+// packing and unpacking: from uint2 to uint64_t
+#define S2V_B64(s,v) \
+  asm("mov.b64 {%0,%1}, %2;" : "=r"(v.x), "=r"(v.y) : "l"(s))
+
+
+/*************************************
+DEVICE side basic block primitives
+**************************************/
+
+#if FERMI
+  #define LDG(ptr)  (* ptr)
+#else
+  #define LDG(ptr)  __ldg(ptr)
+#endif
+
+#if FERMI
+__shared__ int interBuff[GPU_THREADS_PER_BLOCK];
+__inline__ __device__ int __emulated_shfl(const int scalarValue, const uint32_t source_lane) {
+  const int warpIdx = threadIdx.x / WARP_SIZE;
+  const int laneIdx = threadIdx.x % WARP_SIZE;
+  volatile int *interShuffle = interBuff + (warpIdx * WARP_SIZE);
+  interShuffle[laneIdx] = scalarValue;
+  return(interShuffle[source_lane % WARP_SIZE]);
+}
+#endif
+
+__inline__ __device__ int shfl_32(int scalarValue, const int lane) {
+  #if FERMI
+    return __emulated_shfl(scalarValue, (uint32_t)lane);
+  #else
+    return __shfl_sync(0xffffffff, scalarValue, lane);
+  #endif
+}
+
+__inline__ __device__ int shfl_up_32(int scalarValue, const int n) {
+  #if FERMI
+    int lane = threadIdx.x % WARP_SIZE;
+    lane -= n;
+    return shfl_32(scalarValue, lane);
+  #else
+    return __shfl_up_sync(0xffffffff, scalarValue, n);
+  #endif
+}
+
+__inline__ __device__ int shfl_down_32(int scalarValue, const int n) {
+  #if FERMI
+    int lane = threadIdx.x % WARP_SIZE;
+    lane += n;
+    return shfl_32(scalarValue, lane);
+  #else
+    return __shfl_down_sync(0xffffffff, scalarValue, n);
+  #endif
+}
+
+__inline__ __device__ int shfl_xor_32(int scalarValue, const int n) {
+  #if FERMI
+    int lane = threadIdx.x % WARP_SIZE;
+    lane = lane ^ n;
+    return shfl_32(scalarValue, lane);
+  #else
+    return __shfl_xor_sync(0xffffffff, scalarValue, n);
+  #endif
+}
+
+__device__ __forceinline__ uint32_t ld_gbl_ca(const __restrict__ uint32_t *addr) {
+  uint32_t return_value;
+  asm("ld.global.ca.u32 %0, [%1];" : "=r"(return_value) : "l"(addr));
+  return return_value;
+}
+
+__device__ __forceinline__ uint32_t ld_gbl_cs(const __restrict__ uint32_t *addr) {
+  uint32_t return_value;
+  asm("ld.global.cs.u32 %0, [%1];" : "=r"(return_value) : "l"(addr));
+  return return_value;
+}
+
+__device__ __forceinline__ void st_gbl_wt(const __restrict__ uint32_t *addr, const uint32_t value) {
+  asm("st.global.wt.u32 [%0], %1;" :: "l"(addr), "r"(value));
+}
+
+__device__ __forceinline__ void st_gbl_cs(const __restrict__ uint32_t *addr, const uint32_t value) {
+  asm("st.global.cs.u32 [%0], %1;" :: "l"(addr), "r"(value));
+}
+
+__device__ __forceinline__ uint32_t gpu_get_sm_idx(){
+  uint32_t smid;
+  asm volatile("mov.u32 %0, %%smid;" : "=r"(smid));
+  return(smid);
+}
+
+__device__ __forceinline__ void uint32_to_uchars(const uint32_t s, int *u1, int *u2, int *u3, int *u4) {
+  //*u1 = s & 0xff;
+  *u1 = __byte_perm(s, 0, 0x4440);
+  //*u2 = (s>>8) & 0xff;
+  *u2 = __byte_perm(s, 0, 0x4441);
+  //*u3 = (s>>16) & 0xff;
+  *u3 = __byte_perm(s, 0, 0x4442);
+  //*u4 = s>>24;
+  *u4 = __byte_perm(s, 0, 0x4443);
+}
+
+__device__ __forceinline__ uint32_t uchars_to_uint32(int u1, int u2, int u3, int u4) {
+  //return u1 | (u2<<8) | (u3<<16) | (u4<<24);
+  //return __byte_perm(u1, u2, 0x7740) + __byte_perm(u3, u4, 0x4077);
+  return u1 | (u2<<8) | __byte_perm(u3, u4, 0x4077);
+}
+
+__device__ __forceinline__ uint32_t uchar_to_uint32(int u1) {
+  return __byte_perm(u1, u1, 0x0);
+}
+
+__device__ __forceinline__ unsigned int vcmpgeu4(unsigned int a, unsigned int b) {
+    unsigned int r, c;
+    c = a-b;
+    asm ("prmt.b32 %0,%1,0,0xba98;" : "=r"(r) : "r"(c));// build mask from msbs
+    return r;           // byte-wise unsigned gt-eq comparison with mask result
+}
+
+__device__ __forceinline__ unsigned int vminu4(unsigned int a, unsigned int b) {
+    unsigned int r, s;
+    s = vcmpgeu4 (b, a);// mask = 0xff if a >= b
+    r = a & s;          // select a when b >= a
+    s = b & ~s;         // select b when b < a
+    r = r | s;          // combine byte selections
+    return r;
+}
+
+__device__ __forceinline__ void print_uchars(const char* str, const uint32_t s) {
+  int u1, u2, u3, u4;
+  uint32_to_uchars(s, &u1, &u2, &u3, &u4);
+  printf("%s: %d %d %d %d\n", str, u1, u2, u3, u4);
+}
+
+template<class T>
+__device__ __forceinline__ int popcount(T n) {
+#if CSCT or CSCT_RECOMPUTE
+  return __popc(n);
+#else
+  return __popcll(n);
+#endif
+}
+
+__inline__ __device__ uint8_t minu8_index4(int *min_idx, const uint8_t val1, const int dis, const uint8_t val2, const int dis2, const uint8_t val3, const int dis3, const uint8_t val4, const int dis4) {
+  int min_idx1 = dis;
+  uint8_t min1 = val1;
+  if(val1 > val2) {
+    min1 = val2;
+    min_idx1 = dis2;
+  }
+
+  int min_idx2 = dis3;
+  uint8_t min2 = val3;
+  if(val3 > val4) {
+    min2 = val4;
+    min_idx2 = dis4;
+  }
+
+  uint8_t minval = min1;
+  *min_idx = min_idx1;
+  if(min1 > min2) {
+    minval = min2;
+    *min_idx = min_idx2;
+  }
+  return minval;
+}
+
+__inline__ __device__ uint8_t minu8_index8(int *min_idx, const uint8_t val1, const int dis, const uint8_t val2, const int dis2, const uint8_t val3, const int dis3, const uint8_t val4, const int dis4, const uint8_t val5, const int dis5, const uint8_t val6, const int dis6, const uint8_t val7, const int dis7, const uint8_t val8, const int dis8) {
+  int min_idx1, min_idx2;
+  uint8_t minval1, minval2;
+
+  minval1 = minu8_index4(&min_idx1, val1, dis, val2, dis2, val3, dis3, val4, dis4);
+  minval2 = minu8_index4(&min_idx2, val5, dis5, val6, dis6, val7, dis7, val8, dis8);
+
+  *min_idx = min_idx1;
+  uint8_t minval = minval1;
+  if(minval1 > minval2) {
+    *min_idx = min_idx2;
+    minval = minval2;
+  }
+  return minval;
+}
+
+__inline__ __device__ int warpReduceMinIndex2(int *val, int idx) {
+  for(int d = 1; d < WARP_SIZE; d *= 2) {
+    int tmp = shfl_xor_32(*val, d);
+    int tmp_idx = shfl_xor_32(idx, d);
+    if(*val > tmp) {
+      *val = tmp;
+      idx = tmp_idx;
+    }
+  }
+  return idx;
+}
+
+__inline__ __device__ int warpReduceMinIndex(int val, int idx) {
+  for(int d = 1; d < WARP_SIZE; d *= 2) {
+    int tmp = shfl_xor_32(val, d);
+    int tmp_idx = shfl_xor_32(idx, d);
+    if(val > tmp) {
+      val = tmp;
+      idx = tmp_idx;
+    }
+  }
+  return idx;
+}
+
+__inline__ __device__ int warpReduceMin(int val) {
+  val = min(val, shfl_xor_32(val, 1));
+  val = min(val, shfl_xor_32(val, 2));
+  val = min(val, shfl_xor_32(val, 4));
+  val = min(val, shfl_xor_32(val, 8));
+  val = min(val, shfl_xor_32(val, 16));
+  return val;
+}
+
+__inline__ __device__ int blockReduceMin(int val) {
+  static __shared__ int shared[WARP_SIZE]; // Shared mem for WARP_SIZE partial sums
+  const int lane = threadIdx.x % WARP_SIZE;
+  const int wid = threadIdx.x / WARP_SIZE;
+
+  val = warpReduceMin(val);     // Each warp performs partial reduction
+
+  if (lane==0) shared[wid]=val; // Write reduced value to shared memory
+
+  __syncthreads();              // Wait for all partial reductions
+
+  //read from shared memory only if that warp existed
+  val = (threadIdx.x < blockDim.x / warpSize) ? shared[lane] : INT_MAX;
+
+  if (wid==0) val = warpReduceMin(val); //Final reduce within first warp
+
+  return val;
+}
+
+__inline__ __device__ int blockReduceMinIndex(int val, int idx) {
+  static __shared__ int shared_val[WARP_SIZE]; // Shared mem for WARP_SIZE partial mins
+  static __shared__ int shared_idx[WARP_SIZE]; // Shared mem for WARP_SIZE indexes
+  const int lane = threadIdx.x % WARP_SIZE;
+  const int wid = threadIdx.x / WARP_SIZE;
+
+  idx = warpReduceMinIndex2(&val, idx);     // Each warp performs partial reduction
+
+  if (lane==0) {
+    shared_val[wid]=val;
+    shared_idx[wid]=idx;
+  }
+
+  __syncthreads();              // Wait for all partial reductions
+
+  //read from shared memory only if that warp existed
+  val = (threadIdx.x < blockDim.x / WARP_SIZE) ? shared_val[lane] : INT_MAX;
+  idx = (threadIdx.x < blockDim.x / WARP_SIZE) ? shared_idx[lane] : INT_MAX;
+
+  if (wid==0) {
+    idx = warpReduceMinIndex2(&val, idx); //Final reduce within first warp
+  }
+
+  return idx;
+}
+
+
+__inline__ __device__ bool blockAny(bool local_condition) {
+  __shared__ bool conditions[WARP_SIZE];
+  const int lane = threadIdx.x % WARP_SIZE;
+  const int wid = threadIdx.x / WARP_SIZE;
+
+  local_condition = __any_sync(0xffffffff, local_condition);     // Each warp performs __any
+
+  if (lane==0) {
+    conditions[wid]=local_condition;
+  }
+
+  __syncthreads();              // Wait for all partial __any
+
+  //read from shared memory only if that warp existed
+  local_condition = (threadIdx.x < blockDim.x / WARP_SIZE) ? conditions[lane] : false;
+
+  if (wid==0) {
+    local_condition = __any_sync(0xffffffff, local_condition); //Final __any within first warp
+  }
+
+  return local_condition;
+}
+
+} //namespace sgm_gpu
+
+#endif // SGM_GPU__UTIL_H_
+
--- a/flightlib/package.xml
+++ b/flightlib/package.xml
@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>flightlib</name>
+  <version>0.0.1</version>
+  <description>Flightmare: An Open Flexible Quadrotor</description>
+
+  <maintainer email="song@ifi.uzh.ch">Yunlong Song</maintainer>
+
+  <license>GNU GPL</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>catkin_simple</buildtool_depend>
+
+  <depend>gtest</depend>
+
+</package>
--- a/flightlib/src/bridges/unity_bridge.cpp
+++ b/flightlib/src/bridges/unity_bridge.cpp
@@ -0,0 +1,471 @@
+#include "flightlib/bridges/unity_bridge.hpp"
+
+namespace flightlib {
+
+// constructor
+UnityBridge::UnityBridge()
+    : client_address_("tcp://*"),
+      pub_port_("10253"),
+      sub_port_("10254"),
+      num_frames_(0),
+      last_downloaded_utime_(0),
+      last_download_debug_utime_(0),
+      u_packet_latency_(0),
+      unity_ready_(false) {
+	// initialize connections upon creating unity bridge
+	initializeConnections();
+	generator_ = std::default_random_engine(random_device_());
+}
+
+bool UnityBridge::initializeConnections() {
+	logger_.info("Initializing ZMQ connection...");
+
+	// create and bind an upload socket
+	pub_.set(zmqpp::socket_option::send_high_water_mark, 6);
+	pub_.bind(client_address_ + ":" + pub_port_);
+	std::cout << "pub_port_" << pub_port_ << std::endl;
+	// create and bind a download_socket
+	sub_.set(zmqpp::socket_option::receive_high_water_mark, 6);
+	sub_.bind(client_address_ + ":" + sub_port_);
+	std::cout << "sub_port_" << sub_port_ << std::endl;
+	// subscribe all messages from ZMQ
+	sub_.subscribe("");
+
+	logger_.info("Initializing ZMQ connections done!");
+	return true;
+}
+
+bool UnityBridge::connectUnity(const SceneID scene_id) {
+	Scalar time_out_count = 0;
+	Scalar sleep_useconds = 0.2 * 1e5;
+	setScene(scene_id);
+	// try to connect unity
+	logger_.info("Trying to Connect Unity.");
+	std::cout << "[";
+	while (!unity_ready_) {
+		// if time out
+		if (time_out_count / 1e6 > unity_connection_time_out_) {
+			std::cout << "]" << std::endl;
+			logger_.warn(
+			    "Unity Connection time out! Make sure that Unity Standalone "
+			    "or Unity Editor is running the Flightmare.");
+			return false;
+		}
+		// initialize Scene settings
+		sendInitialSettings();
+		// check if setting is done
+		unity_ready_ = handleSettings();
+		// sleep
+		usleep(sleep_useconds);
+		// increase time out counter
+		time_out_count += sleep_useconds;
+		// print something
+		std::cout << ".";
+		std::cout.flush();
+	}
+	logger_.info("Flightmare Unity is connected.");
+	// wait 1 seconds. until to environment is fully loaded.
+	usleep(1 * 1e6);
+	// wait until point cloud is created.
+	// Unity is frozen as long as point cloud is created
+	// check if it's possible to send and receive a frame again and then continue
+	FrameID send_id = 1;
+	getRender(send_id);
+	FrameID receive_id = 0;
+	while (send_id != receive_id) {
+		handleOutput(receive_id);
+	}
+	logger_.info("Flightmare Unity is ready.");
+	return unity_ready_;
+}
+
+bool UnityBridge::disconnectUnity() {
+	unity_ready_ = false;
+	// create new message object
+	std::string client_address_dis_{"tcp://*"};
+	std::string pub_port_dis_{"10255"};
+	zmqpp::context context_dis_;
+	zmqpp::socket pub_dis_{context_dis_, zmqpp::socket_type::publish};
+	pub_dis_.set(zmqpp::socket_option::send_high_water_mark, 6);
+	pub_dis_.bind(client_address_dis_ + ":" + pub_port_dis_);
+
+	// wait until publisher is properly connected
+	usleep(1000000);
+	zmqpp::message msg_dis_;
+	printf("Disconnect from Unity!\n");
+	msg_dis_ << "DISCONNECT";
+	pub_dis_.send(msg_dis_, true);
+	FrameID send_id = 1;
+	getRender(send_id);
+
+	pub_.close();
+	sub_.close();
+	pub_dis_.close();
+	printf("Disconnected!\n");
+	return true;
+}
+
+bool UnityBridge::sendInitialSettings(void) {
+	// create new message object
+	zmqpp::message msg;
+	// add topic header
+	msg << "Pose";
+	// create JSON object for initial settings
+	json json_mesg = settings_;
+	msg << json_mesg.dump();
+	// send message without blocking
+	pub_.send(msg, true);
+	return true;
+};
+
+bool UnityBridge::handleSettings(void) {
+	// create new message object
+	zmqpp::message msg;
+
+	bool done = false;
+	// Unpack message metadata
+	if (sub_.receive(msg, true)) {
+		std::string metadata_string = msg.get(0);
+		// Parse metadata
+		if (json::parse(metadata_string).size() > 1) {
+			return false;  // hack
+		}
+		done = json::parse(metadata_string).at("ready").get<bool>();
+	}
+	return done;
+};
+
+bool UnityBridge::getRender(const FrameID frame_id) {
+	pub_msg_.ntime = frame_id;
+	QuadState quad_state;
+	for (size_t idx = 0; idx < pub_msg_.vehicles.size(); idx++) {
+		unity_quadrotors_[idx]->getState(&quad_state);
+		// 传给unity的飞机位置 = 实际飞机 - 相机和飞机的位姿差, 使得图像渲染位置=飞机位置,使视野无飞机机身遮挡。请确保第0个camera是左目
+		Matrix<4, 4> cam_pose = rgb_cameras_[0]->getRelPose();
+		Vector<3> delta_pose  = cam_pose.block<3, 1>(0, 3);
+		// printf("camera pose to body: %f, %f, %f \n",delta_pose(0),delta_pose(1),delta_pose(2));
+		pub_msg_.vehicles[idx].position = convertToDoubleVector(positionRos2Unity(quad_state.p - delta_pose));
+		pub_msg_.vehicles[idx].rotation = convertToDoubleVector(quaternionRos2Unity(quad_state.q()));
+	}
+
+	for (size_t idx = 0; idx < pub_msg_.objects.size(); idx++) {
+		std::shared_ptr<StaticObject> gate = static_objects_[idx];
+		pub_msg_.objects[idx].position     = convertToDoubleVector(positionRos2Unity(gate->getPosition()));
+		pub_msg_.objects[idx].rotation     = convertToDoubleVector(quaternionRos2Unity(gate->getQuaternion()));
+	}
+
+	// create new message object
+	zmqpp::message msg;
+	// add topic header
+	msg << "Pose";
+	// create JSON object for pose update and append
+	json json_msg = pub_msg_;
+	msg << json_msg.dump();
+	// send message without blocking
+	pub_.send(msg, true);
+	return true;
+}
+
+bool UnityBridge::setScene(const SceneID& scene_id) {
+	if (scene_id >= UnityScene::SceneNum) {
+		logger_.warn("Scene ID is not defined, cannot set scene.");
+		return false;
+	}
+	// logger_.info("Scene ID is set to %d.", scene_id);
+	settings_.scene_id = scene_id;
+	return true;
+}
+
+bool UnityBridge::addQuadrotor(std::shared_ptr<Quadrotor> quad) {
+	Vehicle_t vehicle_t;
+	// get quadrotor state
+	QuadState quad_state;
+	if (!quad->getState(&quad_state)) {
+		logger_.error("Cannot get Quadrotor state.");
+		return false;
+	}
+
+	vehicle_t.ID       = "quadrotor" + std::to_string(settings_.vehicles.size());
+	vehicle_t.position = convertToDoubleVector(positionRos2Unity(quad_state.p));
+	vehicle_t.rotation = convertToDoubleVector(quaternionRos2Unity(quad_state.q()));
+	vehicle_t.size     = convertToDoubleVector(scalarRos2Unity(quad->getSize()));
+
+	// get camera
+	std::vector<std::shared_ptr<RGBCamera>> rgb_cameras = quad->getCameras();
+	for (size_t cam_idx = 0; cam_idx < rgb_cameras.size(); cam_idx++) {
+		std::shared_ptr<RGBCamera> cam = rgb_cameras[cam_idx];
+		Camera_t camera_t;
+		camera_t.ID                     = vehicle_t.ID + "_" + std::to_string(cam_idx);
+		std::vector<Scalar> T_BC_Scalar = transformationRos2Unity(rgb_cameras[cam_idx]->getRelPose());
+		std::vector<double> T_BC_double(T_BC_Scalar.begin(), T_BC_Scalar.end());
+		camera_t.T_BC            = T_BC_double;
+		camera_t.channels        = rgb_cameras[cam_idx]->getChannels();
+		camera_t.width           = rgb_cameras[cam_idx]->getWidth();
+		camera_t.height          = rgb_cameras[cam_idx]->getHeight();
+		camera_t.fov             = rgb_cameras[cam_idx]->getFOV();
+		camera_t.depth_scale     = rgb_cameras[cam_idx]->getDepthScale();
+		camera_t.post_processing = rgb_cameras[cam_idx]->GetPostProcessing();
+		camera_t.is_depth        = false;
+		camera_t.output_index    = cam_idx;
+		vehicle_t.cameras.push_back(camera_t);
+
+		// add rgb_cameras
+		rgb_cameras_.push_back(rgb_cameras[cam_idx]);
+	}
+	unity_quadrotors_.push_back(quad);
+
+	settings_.vehicles.push_back(vehicle_t);
+	pub_msg_.vehicles.push_back(vehicle_t);
+	return true;
+}
+
+bool UnityBridge::addStaticObject(std::shared_ptr<StaticObject> static_object) {
+	Object_t object_t;
+	object_t.ID        = static_object->getID();
+	object_t.prefab_ID = static_object->getPrefabID();
+	object_t.position  = convertToDoubleVector(positionRos2Unity(static_object->getPosition()));
+	object_t.rotation  = convertToDoubleVector(quaternionRos2Unity(static_object->getQuaternion()));
+	object_t.size      = convertToDoubleVector(scalarRos2Unity(static_object->getSize()));
+
+	static_objects_.push_back(static_object);
+	settings_.objects.push_back(object_t);
+	pub_msg_.objects.push_back(object_t);
+
+	return true;
+}
+
+bool UnityBridge::handleOutput(FrameID& frameID) {
+	// create new message object
+	zmqpp::message msg;
+	sub_.receive(msg);
+	// unpack message metadata
+	std::string json_sub_msg = msg.get(0);
+	// parse metadata
+	SubMessage_t sub_msg = json::parse(json_sub_msg);
+	frameID              = sub_msg.ntime;
+
+	size_t image_i = 1;
+	// ensureBufferIsAllocated(sub_msg);
+	for (size_t idx = 0; idx < settings_.vehicles.size(); idx++) {
+		// update vehicle collision flag
+		unity_quadrotors_[idx]->setCollision(sub_msg.sub_vehicles[idx].collision);
+
+		// feed image data to RGB camera
+		for (const auto& cam : settings_.vehicles[idx].cameras) {
+			// 1、RGB图-----------------------------------------
+			{
+				uint32_t image_len = cam.width * cam.height * cam.channels;
+				// Get raw image bytes from ZMQ message.
+				// WARNING: This is a zero-copy operation that also casts the input to an array of unit8_t. when the message is deleted, this pointer
+				// is also dereferenced.
+				const uint8_t* image_data;
+				msg.get(image_data, image_i);
+				image_i = image_i + 1;
+				// Pack image into cv::Mat
+				cv::Mat new_image = cv::Mat(cam.height, cam.width, CV_MAKETYPE(CV_8U, cam.channels));
+				memcpy(new_image.data, image_data, image_len);
+				// Flip image since OpenCV origin is upper left, but Unity's is lower left.
+				cv::flip(new_image, new_image, 0);
+				// Tell OpenCv that the input is RGB.
+				if (cam.channels == 3) {
+					cv::cvtColor(new_image, new_image, CV_RGB2BGR);
+				}
+				unity_quadrotors_[idx]->getCameras()[cam.output_index]->feedImageQueue(0, new_image);
+			}
+
+			// 之前Flightmare的layer_idx： 0 是RGB, 1是Depth, 2是Seg, 3是光流
+			// 现在的post_processing： 0 是RGB, 后面按设置打开的Denpth、Seg等排列
+
+			// 2、附加开启的图-------------------------------------------
+			for (size_t layer_idx = 0; layer_idx < cam.post_processing.size(); layer_idx++) {
+				if (cam.post_processing[layer_idx] == RGBCameraTypes::Depth) {
+					// depth
+					uint32_t image_len = cam.width * cam.height * 4;
+					// Get raw image bytes from ZMQ message.
+					// WARNING: This is a zero-copy operation that also casts the input to an array of unit8_t. when the message is deleted, this
+					// pointer is also dereferenced.
+					const uint8_t* image_data;
+					msg.get(image_data, image_i);
+					image_i = image_i + 1;
+					// Pack image into cv::Mat
+					cv::Mat new_image = cv::Mat(cam.height, cam.width, CV_32FC1);
+					memcpy(new_image.data, image_data, image_len);
+					// Flip image since OpenCV origin is upper left, but Unity's is lower left.
+					new_image = new_image * (1.f);
+					cv::flip(new_image, new_image, 0);
+
+					unity_quadrotors_[idx]->getCameras()[cam.output_index]->feedImageQueue(CameraLayer::DepthMap, new_image);
+				}
+			}
+		}
+	}
+	return true;
+}
+
+bool UnityBridge::spawnTrees(Ref<Vector<3>> bounding_box_, Ref<Vector<3>> bounding_box_origin_, Scalar avg_tree_spacing_) {
+	printf("Start Spawn Trees... \n");
+	rmTrees();
+	// 循环多次避免偶尔一次没render上，后面树再也无法生成
+	for (size_t i = 0; i < 3; i++)
+		refreshUnity(10086 + i);
+
+	TreeMessage_t tree_msg;
+	// compute the requested tree density for Poisson
+	Scalar density = 1.0 / (avg_tree_spacing_ * avg_tree_spacing_);
+	int num_trees  = static_cast<int>(bounding_box_[0] * bounding_box_[1] * density);
+	// draw sample from poisson distribution
+	std::poisson_distribution<int> poisson_dist(num_trees);
+	tree_msg.density = static_cast<double>(poisson_dist(generator_));
+	printf("Tree Spacing is %f. \n", avg_tree_spacing_);
+	// generate random seed
+	std::uniform_int_distribution<int> distribution(1, 1 << 20);
+	tree_msg.seed = distribution(generator_);
+
+	tree_msg.bounding_origin[0] = bounding_box_origin_[0];
+	tree_msg.bounding_origin[1] = bounding_box_origin_[1];
+	tree_msg.bounding_area[0]   = bounding_box_[0];
+	tree_msg.bounding_area[1]   = bounding_box_[1];
+	bool spawned                = placeTrees(tree_msg);
+	std::cout << "Tree Spawned" << std::endl;
+	return spawned;
+}
+
+void UnityBridge::generatePointcloud(const Ref<Vector<3>>& min_corner, const Ref<Vector<3>>& max_corner, int ply_idx, std::string path,
+                                     SceneID scene_id, Scalar pc_resolution_) {
+	printf("Start creating pointcloud... \n");
+	PointCloudMessage_t pcd_msg;
+	pcd_msg.scene_id = scene_id;
+	pcd_msg.bounding_box_scale =
+	    std::vector<double>{(max_corner.x() - min_corner.x()), (max_corner.y() - min_corner.y()), (max_corner.z() - min_corner.z())};
+	printf("Scale pointcloud: [%.2f, %.2f, %.2f]\n", pcd_msg.bounding_box_scale.at(0), pcd_msg.bounding_box_scale.at(1),
+	       pcd_msg.bounding_box_scale.at(2));
+
+	pcd_msg.bounding_box_origin = std::vector<double>{(max_corner.x() + min_corner.x()) / 2.0, (max_corner.y() + min_corner.y()) / 2.0,
+	                                                  (max_corner.z() + min_corner.z()) / 2.0};
+	printf("Origin pointcloud: [%.2f, %.2f, %.2f]\n", pcd_msg.bounding_box_origin.at(0), pcd_msg.bounding_box_origin.at(1),
+	       pcd_msg.bounding_box_origin.at(2));
+
+	pcd_msg.path                    = path;
+	pcd_msg.file_name               = "pointcloud-" + std::to_string(ply_idx);
+	pcd_msg.unity_ground_offset     = 0.0;
+	pcd_msg.resolution_above_ground = pc_resolution_;
+	pcd_msg.resolution_below_ground = pc_resolution_;
+
+	std::cout << "Save file name: " << pcd_msg.path + pcd_msg.file_name + ".ply" << std::endl;
+
+	while (std::experimental::filesystem::exists(pcd_msg.path + pcd_msg.file_name + ".ply")) {
+		std::cout << "file already exists, delete! " << std::endl;
+		std::experimental::filesystem::remove(pcd_msg.path + pcd_msg.file_name + ".ply");
+		usleep(1 * 1e6);
+	}
+	std::cout << "Pointcloud Saving...";
+
+	pointCloudGenerator(pcd_msg);
+
+	// render Unity until point cloud exists
+	FrameID frameID = 10086;
+	while (!std::experimental::filesystem::exists(pcd_msg.path + pcd_msg.file_name + ".ply")) {
+		std::cout << ".";
+		std::cout.flush();
+		refreshUnity(frameID);  // render, not usleep (BUG: Flightmare must render continuously to refresh the sense and generate pointcloud.)
+		frameID++;
+	}
+	usleep(5 * 1e6);
+	printf("Pointcloud saved!\n");
+}
+
+template<typename T>
+std::vector<double> UnityBridge::convertToDoubleVector(const std::vector<T>& input) {
+	std::vector<double> output(input.size());
+	std::transform(input.begin(), input.end(), output.begin(), [](T value) { return static_cast<double>(value); });
+	return output;
+}
+
+void UnityBridge::refreshUnity(FrameID id = 10086) {
+	FrameID frameID_test = id;
+	getRender(frameID_test);
+	FrameID frameID_rt;
+	handleOutput(frameID_rt);
+	while (frameID_test != frameID_rt)
+		handleOutput(frameID_rt);
+}
+
+bool UnityBridge::placeTrees(TreeMessage_t& tree_msg) {
+	std::string client_address_{"tcp://*"};
+	std::string pub_tree_port_{"10255"};
+	zmqpp::context context_;
+	zmqpp::socket pub_tree_{context_, zmqpp::socket_type::publish};
+	pub_tree_.set(zmqpp::socket_option::send_high_water_mark, 6);
+	pub_tree_.bind(client_address_ + ":" + pub_tree_port_);
+
+	std::string sub_tree_port_{"10256"};
+	zmqpp::socket sub_tree_{context_, zmqpp::socket_type::subscribe};
+	sub_tree_.set(zmqpp::socket_option::receive_high_water_mark, 6);
+	sub_tree_.bind(client_address_ + ":" + sub_tree_port_);
+	sub_tree_.subscribe("PLACETREE");
+
+	// wait until publisher is properly connected
+	usleep(1000000);
+	zmqpp::message msg;
+	msg << "PLACETREE";
+
+	// check if seed is not initialized
+	if (tree_msg.seed == -1)
+		tree_msg.seed = rand();
+
+	json json_msg = tree_msg;
+	msg << json_msg.dump();
+	pub_tree_.send(msg, true);
+	pub_tree_.close();
+
+	double sleep_useconds = 0.2 * 1e5;
+	FrameID frameID       = 10086;
+	//  Wait until response received
+	while (true) {
+		if (sub_tree_.receive(msg, true)) {
+			break;
+		}
+		// render, not usleep (BUG: Flightmare must render continuously to refresh the sense and tree.)
+		refreshUnity(frameID);
+		frameID++;
+	}
+	sub_tree_.close();
+	return true;
+}
+
+void UnityBridge::rmTrees() {
+	std::string client_address_{"tcp://*"};
+	std::string pub_tree_port_{"10255"};
+	zmqpp::context context_;
+	zmqpp::socket pub_tree_{context_, zmqpp::socket_type::publish};
+	pub_tree_.set(zmqpp::socket_option::send_high_water_mark, 6);
+	pub_tree_.bind(client_address_ + ":" + pub_tree_port_);
+
+	// wait until publisher is properly connected
+	usleep(1000000);
+	zmqpp::message msg;
+	msg << "RMTREE";
+	pub_tree_.send(msg, true);
+	pub_tree_.close();
+}
+
+void UnityBridge::pointCloudGenerator(PointCloudMessage_t& pcd_msg) {
+	std::string client_address_{"tcp://*"};
+	std::string pub_pc_port_{"10255"};
+	zmqpp::context context_;
+	zmqpp::socket pub_pc_{context_, zmqpp::socket_type::publish};
+	pub_pc_.set(zmqpp::socket_option::send_high_water_mark, 6);
+	pub_pc_.bind(client_address_ + ":" + pub_pc_port_);
+
+	// wait until publisher is properly connected
+	usleep(1000000);
+	zmqpp::message msg;
+	msg << "PCD";
+	json json_msg = pcd_msg;
+	msg << json_msg.dump();
+	pub_pc_.send(msg, true);
+
+	pub_pc_.close();
+}
+
+}  // namespace flightlib
--- a/flightlib/src/common/command.cpp
+++ b/flightlib/src/common/command.cpp
@@ -0,0 +1,29 @@
+#include "flightlib/common/command.hpp"
+
+
+namespace flightlib {
+
+Command::Command() {}
+
+Command::Command(const Scalar t, const Scalar thrust, const Vector<3>& omega)
+  : t(t), collective_thrust(thrust), omega(omega) {}
+
+Command::Command(const Scalar t, const Vector<4>& thrusts)
+  : t(t), thrusts(thrusts) {}
+
+bool Command::valid() const {
+  return std::isfinite(t) &&
+         ((std::isfinite(collective_thrust) && omega.allFinite()) ^
+          thrusts.allFinite());
+}
+
+bool Command::isSingleRotorThrusts() const {
+  return std::isfinite(t) && thrusts.allFinite();
+}
+
+bool Command::isRatesThrust() const {
+  return std::isfinite(t) && std::isfinite(collective_thrust) &&
+         omega.allFinite();
+}
+
+}  // namespace flightlib
--- a/flightlib/src/common/integrator_base.cpp
+++ b/flightlib/src/common/integrator_base.cpp
@@ -0,0 +1,33 @@
+#include "flightlib/common/integrator_base.hpp"
+
+namespace flightlib {
+
+IntegratorBase::IntegratorBase(IntegratorBase::DynamicsFunction function,
+                               const Scalar dt_max)
+  : dynamics_(function), dt_max_(dt_max) {}
+
+bool IntegratorBase::integrate(const QuadState& initial,
+                               QuadState* const final) const {
+  if (std::isnan(initial.t) || std::isnan(final->t)) return false;
+  if (initial.t >= final->t) return false;
+  return integrate(initial.x, final->t - initial.t, final->x);
+}
+
+bool IntegratorBase::integrate(const Ref<const Vector<>> initial,
+                               const Scalar dt, Ref<Vector<>> final) const {
+  Scalar dt_remaining = dt;
+  Vector<> state = initial;
+
+  do {
+    const Scalar dt_this = std::min(dt_remaining, dt_max_);
+    if (!step(state, dt_this, final)) return false;
+    state = final;
+    dt_remaining -= dt_this;
+  } while (dt_remaining > 0.0);
+
+  return true;
+}
+
+Scalar IntegratorBase::dtMax() const { return dt_max_; }
+
+}  // namespace flightlib
--- a/flightlib/src/common/integrator_euler.cpp
+++ b/flightlib/src/common/integrator_euler.cpp
@@ -0,0 +1,15 @@
+#include "flightlib/common/integrator_euler.hpp"
+
+namespace flightlib {
+
+bool IntegratorEuler::step(const Ref<const Vector<>> initial, const Scalar dt,
+                           Ref<Vector<>> final) const {
+  Vector<> derivative(initial.rows());
+  if (!this->dynamics_(initial, derivative)) return false;
+
+  final = initial + dt * derivative;
+
+  return true;
+}
+
+}  // namespace flightlib
--- a/flightlib/src/common/integrator_rk4.cpp
+++ b/flightlib/src/common/integrator_rk4.cpp
@@ -0,0 +1,34 @@
+#include "flightlib/common/integrator_rk4.hpp"
+
+namespace flightlib {
+
+bool IntegratorRK4::step(const Ref<const Vector<>> initial, const Scalar dt,
+                         Ref<Vector<>> final) const {
+  static const Vector<4> rk4_sum_vec{1.0 / 6.0, 2.0 / 6.0, 2.0 / 6.0,
+                                     1.0 / 6.0};
+  Matrix<> k = Matrix<>::Zero(initial.rows(), 4);
+
+  final = initial;
+
+  // k_1
+  if (!this->dynamics_(final, k.col(0))) return false;
+
+  // k_2
+  final = initial + 0.5 * dt * k.col(0);
+  if (!this->dynamics_(final, k.col(1))) return false;
+
+  // k_3
+  final = initial + 0.5 * dt * k.col(1);
+  if (!this->dynamics_(final, k.col(2))) return false;
+
+  // k_4
+  final = initial + dt * k.col(2);
+  if (!this->dynamics_(final, k.col(3))) return false;
+
+
+  final = initial + dt * k * rk4_sum_vec;
+
+  return true;
+}
+
+}  // namespace flightlib
--- a/flightlib/src/common/logger.cpp
+++ b/flightlib/src/common/logger.cpp
@@ -0,0 +1,63 @@
+#include "flightlib/common/logger.hpp"
+
+namespace flightlib {
+
+Logger::Logger(const std::string& name, const bool color)
+  : sink_(std::cout.rdbuf()), colored_(color) {
+  name_ = "[" + name + "]";
+
+  if (name_.size() < NAME_PADDING)
+    name_ = name_ + std::string(NAME_PADDING - name_.size(), ' ');
+  else
+    name_ = name_ + " ";
+
+  sink_.precision(DEFAULT_PRECISION);
+}
+
+Logger::Logger(const std::string& name, const std::string& filename)
+  : Logger(name, false) {
+  if (!filename.empty()) {
+    std::filebuf* fbuf = new std::filebuf;
+    if (fbuf->open(filename, std::ios::out))
+      sink_.rdbuf(fbuf);
+    else
+      warn("Could not open file %s. Logging to console!", filename);
+  }
+  sink_.precision(DEFAULT_PRECISION);
+}
+
+Logger::~Logger() {}
+
+inline std::streamsize Logger::precision(const std::streamsize n) {
+  return sink_.precision(n);
+}
+
+inline void Logger::scientific(const bool on) {
+  if (on)
+    sink_ << std::scientific;
+  else
+    sink_ << std::fixed;
+}
+
+void Logger::info(const std::string& message) const {
+  if (colored_)
+    sink_ << name_ << message << std::endl;
+  else
+    sink_ << name_ << INFO << message << std::endl;
+}
+
+void Logger::warn(const std::string& message) const {
+  if (colored_)
+    sink_ << YELLOW << name_ << message << RESET << std::endl;
+  else
+    sink_ << name_ << WARN << message << std::endl;
+}
+
+void Logger::error(const std::string& message) const {
+  if (colored_)
+    sink_ << RED << name_ << message << RESET << std::endl;
+  else
+    sink_ << name_ << ERROR << message << std::endl;
+}
+
+}  // namespace flightlib
--- a/flightlib/src/common/math.cpp
+++ b/flightlib/src/common/math.cpp
@@ -0,0 +1,232 @@
+#include "flightlib/common/math.hpp"
+
+#include "iostream"
+
+namespace flightlib {
+
+Matrix<3, 3> skew(const Vector<3>& v) { return (Matrix<3, 3>() << 0, -v.z(), v.y(), v.z(), 0, -v.x(), -v.y(), v.x(), 0).finished(); }
+
+Matrix<4, 4> Q_left(const Quaternion& q) {
+	return (Matrix<4, 4>() << q.w(), -q.x(), -q.y(), -q.z(), q.x(), q.w(), -q.z(), q.y(), q.y(), q.z(), q.w(), -q.x(), q.z(), -q.y(), q.x(), q.w())
+	    .finished();
+}
+
+Matrix<4, 4> Q_right(const Quaternion& q) {
+	return (Matrix<4, 4>() << q.w(), -q.x(), -q.y(), -q.z(), q.x(), q.w(), q.z(), -q.y(), q.y(), -q.z(), q.w(), q.x(), q.z(), q.y(), -q.x(), q.w())
+	    .finished();
+}
+
+Matrix<4, 3> qFromQeJacobian(const Quaternion& q) {
+	return (Matrix<4, 3>() << -1.0 / q.w() * q.vec().transpose(), Matrix<3, 3>::Identity()).finished();
+}
+
+Matrix<4, 4> qConjugateJacobian() { return Matrix<4, 1>(1, -1, -1, -1).asDiagonal(); }
+
+Matrix<3, 3> qeRotJacobian(const Quaternion& q, const Matrix<3, 1>& t) {
+	return 2.0 * (Matrix<3, 3>() << (q.y() + q.z() * q.x() / q.w()) * t.y() + (q.z() - q.y() * q.x() / q.w()) * t.z(),        // entry 0,0
+	              -2.0 * q.y() * t.x() + (q.x() + q.z() * q.y() / q.w()) * t.y() + (q.w() - q.y() * q.y() / q.w()) * t.z(),   // entry 0,1
+	              -2.0 * q.z() * t.x() + (-q.w() + q.z() * q.z() / q.w()) * t.y() + (q.x() - q.y() * q.z() / q.w()) * t.z(),  // entry 0,2
+
+	              (q.y() - q.z() * q.x() / q.w()) * t.x() + (-2.0 * q.x()) * t.y() + (-q.w() + q.x() * q.x() / q.w()) * t.z(),  // entry 1,0
+	              (q.x() - q.z() * q.y() / q.w()) * t.x() + (q.z() + q.x() * q.y() / q.w()) * t.z(),                            // entry 1,1
+	              (q.w() - q.z() * q.z() / q.w()) * t.x() + (-2.0 * q.z()) * t.y() + (q.y() + q.x() * q.z() / q.w()) * t.z(),   // entry 1,2
+
+	              (q.z() + q.y() * q.x() / q.w()) * t.x() + (q.w() - q.x() * q.x() / q.w()) * t.y() + (-2.0 * q.x()) * t.z(),   // entry 2,0
+	              (-q.w() + q.y() * q.y() / q.w()) * t.x() + (q.z() - q.x() * q.y() / q.w()) * t.y() + (-2.0 * q.y()) * t.z(),  // entry 2,1
+	              (q.x() + q.y() * q.z() / q.w()) * t.x() + (q.y() - q.x() * q.z() / q.w()) * t.y()                             // entry 2,2
+	              )
+	                 .finished();
+}
+
+Matrix<3, 3> qeInvRotJacobian(const Quaternion& q, const Matrix<3, 1>& t) {
+	return 2.0 * (Matrix<3, 3>() << (q.y() - q.z() * q.x() / q.w()) * t.y() + (q.z() + q.y() * q.x() / q.w()) * t.z(),       // entry 0,0
+	              -2.0 * q.y() * t.x() + (q.x() - q.z() * q.y() / q.w()) * t.y() - (q.w() - q.y() * q.y() / q.w()) * t.z(),  // entry 0,1
+	              -2.0 * q.z() * t.x() + (q.w() - q.z() * q.z() / q.w()) * t.y() + (q.x() + q.y() * q.z() / q.w()) * t.z(),  // entry 0,2
+
+	              (q.y() + q.z() * q.x() / q.w()) * t.x() - 2.0 * q.x() * t.y() + (q.w() - q.x() * q.x() / q.w()) * t.z(),   // entry 1,0
+	              (q.x() + q.z() * q.y() / q.w()) * t.x() + (q.z() - q.x() * q.y() / q.w()) * t.z(),                         // entry 1,1
+	              -(q.w() - q.z() * q.z() / q.w()) * t.x() - 2.0 * q.z() * t.y() + (q.y() - q.x() * q.z() / q.w()) * t.z(),  // entry 1,2
+
+	              (q.z() - q.y() * q.x() / q.w()) * t.x() - (q.w() - q.x() * q.x() / q.w()) * t.y() - 2.0 * q.x() * t.z(),  // entry 2,0
+	              (q.w() - q.y() * q.y() / q.w()) * t.x() + (q.z() + q.x() * q.y() / q.w()) * t.y() - 2.0 * q.y() * t.z(),  // entry 2,1
+	              (q.x() - q.y() * q.z() / q.w()) * t.x() + (q.y() + q.x() * q.z() / q.w()) * t.y()                         // entry 2,2
+	              )
+	                 .finished();
+}
+
+void matrixToTripletList(const SparseMatrix& matrix, std::vector<SparseTriplet>* const list, const int row_offset, const int col_offset) {
+	list->reserve((size_t)matrix.nonZeros() + list->size());
+
+	for (int i = 0; i < matrix.outerSize(); i++) {
+		for (typename SparseMatrix::InnerIterator it(matrix, i); it; ++it) {
+			list->emplace_back(it.row() + row_offset, it.col() + col_offset, it.value());
+		}
+	}
+}
+
+void matrixToTripletList(const Matrix<Dynamic, Dynamic>& matrix, std::vector<SparseTriplet>* const list, const int row_offset, const int col_offset) {
+	const SparseMatrix sparse = matrix.sparseView();
+	matrixToTripletList(sparse, list, row_offset, col_offset);
+}
+
+void insert(const SparseMatrix& from, SparseMatrix* const into, const int row_offset, const int col_offset) {
+	std::vector<SparseTriplet> v;
+
+	matrixToTripletList(*into, &v);
+	matrixToTripletList(from, &v, row_offset, col_offset);
+
+	into->setFromTriplets(v.begin(), v.end(), [](const Scalar& older, const Scalar& newer) { return newer; });
+}
+
+void insert(const Matrix<>& from, SparseMatrix* const into, const int row_offset, const int col_offset) {
+	const SparseMatrix from_sparse = from.sparseView();
+	insert(from_sparse, into, row_offset, col_offset);
+}
+
+inline void insert(const Matrix<>& from, Matrix<>* const into, const int row_offset, const int col_offset) {
+	into->block(row_offset, col_offset, from.rows(), from.cols()) = from;
+}
+
+void quaternionToEuler(const Quaternion& quat, Ref<Vector<3>> euler) {
+	euler.x() = std::atan2(2 * quat.w() * quat.x() + 2 * quat.y() * quat.z(),
+	                       quat.w() * quat.w() - quat.x() * quat.x() - quat.y() * quat.y() + quat.z() * quat.z());
+	euler.y() = -std::asin(2 * quat.x() * quat.z() - 2 * quat.w() * quat.y());
+	euler.z() = std::atan2(2 * quat.w() * quat.z() + 2 * quat.x() * quat.y(),
+	                       quat.w() * quat.w() + quat.x() * quat.x() - quat.y() * quat.y() - quat.z() * quat.z());
+}
+
+
+std::vector<Scalar> transformationRos2Unity(const Matrix<4, 4>& ros_tran_mat) {
+	/// [ Transformation Matrix ] from ROS coordinate system (right hand)
+	/// to Unity coordinate system (left hand)
+	Matrix<4, 4> tran_mat = Matrix<4, 4>::Zero();
+	tran_mat(0, 0)        = 1.0;
+	tran_mat(1, 2)        = 1.0;
+	tran_mat(2, 1)        = 1.0;
+	tran_mat(3, 3)        = 1.0;
+	//
+	Matrix<4, 4> unity_tran_mat = tran_mat * ros_tran_mat * tran_mat.transpose();
+	// std::vector<Scalar> unity_tran_mat_vec(
+	//   unity_tran_mat.data(),
+	//   unity_tran_mat.data() + unity_tran_mat.rows() * unity_tran_mat.cols());
+	std::vector<Scalar> tran_unity;
+	for (int i = 0; i < 4; ++i) {
+		for (int j = 0; j < 4; ++j) {
+			tran_unity.push_back(unity_tran_mat(i, j));
+		}
+	}
+	return tran_unity;
+}
+
+std::vector<Scalar> quaternionRos2Unity(const Quaternion& ros_quat) {
+	/// [ Quaternion ] from ROS coordinate system (right hand)
+	/// to Unity coordinate system (left hand)
+	Matrix<3, 3> rot_mat = Matrix<3, 3>::Zero();
+	rot_mat(0, 0)        = 1.0;
+	rot_mat(1, 2)        = 1.0;
+	rot_mat(2, 1)        = 1.0;
+	//
+	Matrix<3, 3> unity_rot_mat = rot_mat * ros_quat.toRotationMatrix() * rot_mat.transpose();
+	Quaternion unity_quat(unity_rot_mat);
+	std::vector<Scalar> unity_quat_vec{unity_quat.x(), unity_quat.y(), unity_quat.z(), unity_quat.w()};
+	return unity_quat_vec;
+}
+
+std::vector<Scalar> positionRos2Unity(const Vector<3>& ros_pos_vec) {
+	/// [ Position Vector ] from ROS coordinate system (right hand)
+	/// to Unity coordinate system (left hand)
+	std::vector<Scalar> unity_position{ros_pos_vec(0), ros_pos_vec(2), ros_pos_vec(1)};
+	return unity_position;
+}
+
+std::vector<Scalar> scalarRos2Unity(const Vector<3>& ros_scalar) {
+	/// [ Object Scalar Vector ] from ROS coordinate system (right hand)
+	/// to Unity coordinate system (left hand)
+	std::vector<Scalar> unity_scalar{ros_scalar(0), ros_scalar(2), ros_scalar(1)};
+	return unity_scalar;
+}
+
+// rpy顺序
+void get_euler_from_R(Vector<3>& e, const Matrix<3, 3>& R) {
+	float phi   = atan2(R(2, 1), R(2, 2));
+	float theta = asin(-R(2, 0));
+	float psi   = atan2(R(1, 0), R(0, 0));
+	float pi    = M_PI;
+
+	if (fabs(theta - pi / 2.0f) < 1.0e-3) {
+		phi = 0.0f;
+		psi = atan2(R(1, 2), R(0, 2));
+	} else if (fabs(theta + pi / 2.0f) < 1.0e-3) {
+		phi = 0.0f;
+		psi = atan2(-R(1, 2), -R(0, 2));
+	}
+	e(0) = phi;
+	e(1) = theta;
+	e(2) = psi;
+}
+
+double wrapMinusPiToPi(const double angle) {
+	if (angle >= -M_PIl && angle <= M_PIl) {
+		return angle;
+	}
+	double wrapped_angle = angle + M_PIl;
+	wrapped_angle        = wrapZeroToTwoPi(wrapped_angle);
+	wrapped_angle -= M_PIl;
+	return wrapped_angle;
+}
+
+double wrapZeroToTwoPi(const double angle) {
+	if (angle >= 0.0 && angle <= 2.0 * M_PIl) {
+		return angle;
+	}
+	double wrapped_angle = fmod(angle, 2.0 * M_PIl);
+	if (wrapped_angle < 0.0) {
+		wrapped_angle += 2.0 * M_PIl;
+	}
+	return wrapped_angle;
+}
+
+// calculate and constrain the yaw angle per sim_t
+float calculate_yaw(float yaw_cur, float yaw_ref, float sim_t)  // yaw [-pi,pi]
+{
+	float PI                  = 3.1415926;
+	float YAW_DOT_MAX_PER_SEC = 0.3 * PI;
+	float max_yaw_change      = YAW_DOT_MAX_PER_SEC * sim_t;
+	float yaw_temp            = yaw_ref;
+	float last_yaw_           = yaw_cur;
+	float yaw                 = 0;
+
+	if (yaw_temp - last_yaw_ > PI) {
+		if (yaw_temp - last_yaw_ - 2 * PI < -max_yaw_change) {
+			yaw = last_yaw_ - max_yaw_change;
+			if (yaw < -PI)
+				yaw += 2 * PI;
+		} else {
+			yaw = yaw_temp;
+		}
+	} else if (yaw_temp - last_yaw_ < -PI) {
+		if (yaw_temp - last_yaw_ + 2 * PI > max_yaw_change) {
+			yaw = last_yaw_ + max_yaw_change;
+			if (yaw > PI)
+				yaw -= 2 * PI;
+		} else {
+			yaw = yaw_temp;
+		}
+	} else {
+		if (yaw_temp - last_yaw_ < -max_yaw_change) {
+			yaw = last_yaw_ - max_yaw_change;
+			if (yaw < -PI)
+				yaw += 2 * PI;
+		} else if (yaw_temp - last_yaw_ > max_yaw_change) {
+			yaw = last_yaw_ + max_yaw_change;
+			if (yaw > PI)
+				yaw -= 2 * PI;
+		} else {
+			yaw = yaw_temp;
+		}
+	}
+
+	return yaw;
+}
+
+}  // namespace flightlib
--- a/flightlib/src/common/parameter_base.cpp
+++ b/flightlib/src/common/parameter_base.cpp
@@ -0,0 +1,15 @@
+#include "flightlib/common/parameter_base.hpp"
+
+namespace flightlib {
+
+ParameterBase::ParameterBase() {}
+
+ParameterBase::ParameterBase(const YAML::Node& cfg_node)
+  : cfg_node_(cfg_node) {}
+
+ParameterBase::ParameterBase(const std::string& cfg_path)
+  : cfg_node_(YAML::Node(cfg_path)) {}
+
+ParameterBase::~ParameterBase() {}
+
+}  // namespace flightlib
--- a/flightlib/src/common/pend_state.cpp
+++ b/flightlib/src/common/pend_state.cpp
@@ -0,0 +1,41 @@
+#include "flightlib/common/pend_state.hpp"
+
+namespace flightlib {
+
+PendState::PendState() {}
+
+PendState::PendState(const Vector<IDX::SIZE>& x, const Scalar t) : x(x), t(t) {}
+
+PendState::PendState(const PendState& state) : x(state.x), t(state.t) {}
+
+PendState::~PendState() {}
+
+Quaternion PendState::q() const {
+  return Quaternion(x(ATTW), x(ATTX), x(ATTY), x(ATTZ));
+}
+
+void PendState::q(const Quaternion quaternion) {
+  x(IDX::ATTW) = quaternion.w();
+  x(IDX::ATTX) = quaternion.x();
+  x(IDX::ATTY) = quaternion.y();
+  x(IDX::ATTZ) = quaternion.z();
+}
+
+Matrix<3, 3> PendState::R() const {
+  return Quaternion(x(ATTW), x(ATTX), x(ATTY), x(ATTZ)).toRotationMatrix();
+}
+
+void PendState::setZero() {
+  t = 0.0;
+  x.setZero();
+  x(ATTW) = 1.0;
+}
+
+std::ostream& operator<<(std::ostream& os, const PendState& state) {
+  os.precision(3);
+  os << "State at " << state.t << "s: [" << state.x.transpose() << "]";
+  os.precision();
+  return os;
+}
+
+}  // namespace flightlib
--- a/flightlib/src/common/quad_state.cpp
+++ b/flightlib/src/common/quad_state.cpp
@@ -0,0 +1,41 @@
+#include "flightlib/common/quad_state.hpp"
+
+namespace flightlib {
+
+QuadState::QuadState() {}
+
+QuadState::QuadState(const Vector<IDX::SIZE>& x, const Scalar t) : x(x), t(t) {}
+
+QuadState::QuadState(const QuadState& state) : x(state.x), t(state.t) {}
+
+QuadState::~QuadState() {}
+
+Quaternion QuadState::q() const {
+  return Quaternion(x(ATTW), x(ATTX), x(ATTY), x(ATTZ));
+}
+
+void QuadState::q(const Quaternion quaternion) {
+  x(IDX::ATTW) = quaternion.w();
+  x(IDX::ATTX) = quaternion.x();
+  x(IDX::ATTY) = quaternion.y();
+  x(IDX::ATTZ) = quaternion.z();
+}
+
+Matrix<3, 3> QuadState::R() const {
+  return Quaternion(x(ATTW), x(ATTX), x(ATTY), x(ATTZ)).toRotationMatrix();
+}
+
+void QuadState::setZero() {
+  t = 0.0;
+  x.setZero();
+  x(ATTW) = 1.0;
+}
+
+std::ostream& operator<<(std::ostream& os, const QuadState& state) {
+  os.precision(3);
+  os << "State at " << state.t << "s: [" << state.x.transpose() << "]";
+  os.precision();
+  return os;
+}
+
+}  // namespace flightlib
--- a/flightlib/src/common/timer.cpp
+++ b/flightlib/src/common/timer.cpp
@@ -0,0 +1,117 @@
+#include "flightlib/common/timer.hpp"
+
+#include <cmath>
+#include <limits>
+
+namespace flightlib {
+
+Timer::Timer(const std::string name, const std::string module)
+  : name_(name),
+    module_(module),
+    timing_mean_(0.0),
+    timing_last_(0.0),
+    timing_S_(0.0),
+    timing_min_(std::numeric_limits<Scalar>::max()),
+    timing_max_(0.0),
+    n_samples_(0) {}
+
+Timer::Timer(const Timer& other)
+  : name_(other.name_),
+    module_(other.module_),
+    t_start_(other.t_start_),
+    timing_mean_(other.timing_mean_),
+    timing_last_(other.timing_last_),
+    timing_S_(other.timing_S_),
+    timing_min_(other.timing_min_),
+    timing_max_(other.timing_max_),
+    n_samples_(other.n_samples_) {}
+
+void Timer::tic() { t_start_ = std::chrono::high_resolution_clock::now(); }
+
+Scalar Timer::toc() {
+  // Calculate timing.
+  const TimePoint t_end = std::chrono::high_resolution_clock::now();
+  timing_last_ = 1e-9 * std::chrono::duration_cast<std::chrono::nanoseconds>(
+                          t_end - t_start_)
+                          .count();
+  n_samples_++;
+
+  // Set timing, filter if already initialized.
+  if (timing_mean_ <= 0.0) {
+    timing_mean_ = timing_last_;
+  } else {
+    const Scalar timing_mean_prev = timing_mean_;
+    timing_mean_ =
+      timing_mean_prev + (timing_last_ - timing_mean_prev) / n_samples_;
+    timing_S_ = timing_S_ + (timing_last_ - timing_mean_prev) *
+                              (timing_last_ - timing_mean_);
+  }
+  timing_min_ = (timing_last_ < timing_min_) ? timing_last_ : timing_min_;
+  timing_max_ = (timing_last_ > timing_max_) ? timing_last_ : timing_max_;
+
+  t_start_ = t_end;
+
+  return timing_mean_;
+}
+
+Scalar Timer::operator()() const { return timing_mean_; }
+
+Scalar Timer::mean() const { return timing_mean_; }
+
+Scalar Timer::last() const { return timing_last_; }
+
+Scalar Timer::min() const { return timing_min_; }
+
+Scalar Timer::max() const { return timing_max_; }
+
+Scalar Timer::std() const { return std::sqrt(timing_S_ / n_samples_); }
+
+int Timer::count() const { return n_samples_; }
+
+void Timer::reset() {
+  n_samples_ = 0u;
+  t_start_ = TimePoint();
+  timing_mean_ = 0.0;
+  timing_last_ = 0.0;
+  timing_S_ = 0.0;
+  timing_min_ = std::numeric_limits<Scalar>::max();
+  timing_max_ = 0.0;
+}
+
+void Timer::print() const { std::cout << *this; }
+
+std::ostream& operator<<(std::ostream& os, const Timer& timer) {
+  if (!timer.module_.empty()) os << "[" << timer.module_ << "] ";
+
+  if (timer.n_samples_ < 1) {
+    os << "Timing " << timer.name_ << " has no call yet." << std::endl;
+    return os;
+  }
+
+  const std::streamsize prec = os.precision();
+  os.precision(3);
+
+  os << "Timing " << timer.name_ << " in " << timer.n_samples_ << " calls"
+     << std::endl;
+
+  if (!timer.module_.empty()) os << "[" << timer.module_ << "] ";
+  os << "mean|std:  " << 1000 * timer.timing_mean_ << " | "
+     << 1000 * timer.timing_S_ << " ms    "
+     << "[min|max:  " << 1000 * timer.timing_min_ << " | "
+     << 1000 * timer.timing_max_ << " ms]" << std::endl;
+
+  os.precision(prec);
+  return os;
+}
+
+ScopedTimer::ScopedTimer(const std::string name, const std::string module)
+  : Timer(name, module) {
+  this->tic();
+}
+
+ScopedTimer::~ScopedTimer() {
+  this->toc();
+  this->print();
+}
+
+}  // namespace flightlib
--- a/flightlib/src/dynamics/dynamics_base.cpp
+++ b/flightlib/src/dynamics/dynamics_base.cpp
@@ -0,0 +1,9 @@
+#include "flightlib/dynamics/dynamics_base.hpp"
+
+namespace flightlib {
+
+DynamicsBase::DynamicsBase() {}
+
+DynamicsBase::~DynamicsBase() {}
+
+}  // namespace flightlib
--- a/flightlib/src/dynamics/quadrotor_dynamics.cpp
+++ b/flightlib/src/dynamics/quadrotor_dynamics.cpp
@@ -0,0 +1,228 @@
+#include "flightlib/dynamics/quadrotor_dynamics.hpp"
+
+namespace flightlib {
+
+QuadrotorDynamics::QuadrotorDynamics(const Scalar mass, const Scalar arm_l)
+  : mass_(mass),
+    arm_l_(arm_l),
+    t_BM_(
+      arm_l * sqrt(0.5) *
+      (Matrix<3, 4>() << 1, -1, -1, 1, -1, -1, 1, 1, 0, 0, 0, 0).finished()),
+    J_(mass / 12.0 * arm_l * arm_l * Vector<3>(4.5, 4.5, 7).asDiagonal()),
+    J_inv_(J_.inverse()),
+    motor_omega_min_(150.0),
+    motor_omega_max_(2000.0),
+    motor_tau_inv_(1.0 / 0.05),
+    thrust_map_(1.3298253500372892e-06, 0.0038360810526746033,
+                -1.7689986848125325),
+    kappa_(0.016),
+    thrust_min_(0.0),
+    thrust_max_(motor_omega_max_ * motor_omega_max_ * thrust_map_(0) +
+                motor_omega_max_ * thrust_map_(1) + thrust_map_(2)),
+    collective_thrust_min_(4.0 * thrust_min_ / mass_),
+    collective_thrust_max_(4.0 * thrust_max_ / mass_),            
+    omega_max_(6.0, 6.0, 2.0) {}
+
+QuadrotorDynamics::~QuadrotorDynamics() {}
+
+bool QuadrotorDynamics::dState(const QuadState& state,
+                               QuadState* dstate) const {
+  return dState(state.x, dstate->x);
+}
+
+bool QuadrotorDynamics::dState(const Ref<const Vector<QuadState::SIZE>> state,
+                               Ref<Vector<QuadState::SIZE>> dstate) const {
+  if (!state.segment<QS::NDYM>(0).allFinite()) return false;
+
+  dstate.setZero();
+  //
+  const Vector<3> omega(state(QS::OMEX), state(QS::OMEY), state(QS::OMEZ));
+  const Quaternion q_omega(0, omega.x(), omega.y(), omega.z());
+  const Vector<3> body_torque = state.segment<QS::NTAU>(QS::TAU);
+
+  // linear velocity = dx / dt
+  dstate.segment<QS::NPOS>(QS::POS) = state.segment<QS::NVEL>(QS::VEL);
+
+  // differentiate quaternion = dq / dt
+  dstate.segment<QS::NATT>(QS::ATT) =
+    0.5 * Q_right(q_omega) * state.segment<QS::NATT>(QS::ATT);
+
+  // linear acceleration = dv / dt
+  dstate.segment<QS::NVEL>(QS::VEL) = state.segment<QS::NACC>(QS::ACC);
+
+  // angular accleration = domega / dt
+  dstate.segment<QS::NOME>(QS::OME) =
+    J_inv_ * (body_torque - omega.cross(J_ * omega));
+  //
+  return true;
+}
+
+QuadrotorDynamics::DynamicsFunction QuadrotorDynamics::getDynamicsFunction()
+  const {
+  return std::bind(
+    static_cast<bool (QuadrotorDynamics::*)(const Ref<const Vector<QS::SIZE>>,
+                                            Ref<Vector<QS::SIZE>>) const>(
+      &QuadrotorDynamics::dState),
+    this, std::placeholders::_1, std::placeholders::_2);
+}
+
+bool QuadrotorDynamics::valid() const {
+  bool check = true;
+
+  check &= mass_ > 0.0;
+  check &= mass_ < 100.0;  // limit maximum mass
+  check &= t_BM_.allFinite();
+  check &= J_.allFinite();
+  check &= J_inv_.allFinite();
+
+  check &= motor_omega_min_ >= 0.0;
+  check &= (motor_omega_max_ > motor_omega_min_);
+  check &= motor_tau_inv_ > 0.0;
+
+  check &= thrust_map_.allFinite();
+  check &= kappa_ > 0.0;
+  check &= thrust_min_ >= 0.0;
+  check &= (thrust_max_ > thrust_min_);
+
+  check &= (omega_max_.array() > 0).all();
+
+  return check;
+}
+
+Vector<4> QuadrotorDynamics::clampThrust(const Vector<4> thrusts) const {
+  return thrusts.cwiseMax(thrust_min_).cwiseMin(thrust_max_);
+}
+
+Scalar QuadrotorDynamics::clampThrust(const Scalar thrust) const {
+  return std::clamp(thrust, thrust_min_, thrust_max_);
+}
+
+Scalar QuadrotorDynamics::clampCollectiveThrust(const Scalar thrust) const {
+  return std::clamp(thrust, collective_thrust_min_, collective_thrust_max_);
+}
+
+Vector<4> QuadrotorDynamics::clampMotorOmega(const Vector<4>& omega) const {
+  return omega.cwiseMax(motor_omega_min_).cwiseMin(motor_omega_max_);
+}
+
+Vector<3> QuadrotorDynamics::clampBodyrates(const Vector<3>& omega) const {
+  return omega.cwiseMax(-omega_max_).cwiseMin(omega_max_);
+}
+
+Vector<4> QuadrotorDynamics::motorOmegaToThrust(const Vector<4>& omega) const {
+  const Matrix<4, 3> omega_poly =
+    (Matrix<4, 3>() << omega.cwiseProduct(omega), omega, Vector<4>::Ones())
+      .finished();
+  return omega_poly * thrust_map_;
+}
+
+Vector<4> QuadrotorDynamics::motorThrustToOmega(
+  const Vector<4>& thrusts) const {
+  const Scalar scale = 1.0 / (2.0 * thrust_map_[0]);
+  const Scalar offset = -thrust_map_[1] * scale;
+
+  const Array<4, 1> root =
+    (std::pow(thrust_map_[1], 2) -
+     4.0 * thrust_map_[0] * (thrust_map_[2] - thrusts.array()))
+      .sqrt();
+
+  return offset + scale * root;
+}
+
+Matrix<4, 4> QuadrotorDynamics::getAllocationMatrix() const {
+  return (Matrix<4, 4>() << Vector<4>::Ones().transpose(), t_BM_.topRows<2>(),
+          kappa_ * Vector<4>(1, -1, 1, -1).transpose())
+    .finished();
+}
+
+bool QuadrotorDynamics::setMass(const Scalar mass) {
+  if (mass < 0.0 || mass >= 100.) {
+    return false;
+  }
+  mass_ = mass;
+  // update inertial matrix and its inverse
+  updateInertiaMarix();
+  return true;
+}
+
+bool QuadrotorDynamics::setArmLength(const Scalar arm_length) {
+  if (arm_length < 0.0) {
+    return false;
+  }
+  arm_l_ = arm_length;
+  // update torque mapping matrix, inertial matrix and its inverse
+  updateInertiaMarix();
+  return true;
+}
+
+bool QuadrotorDynamics::setMotortauInv(const Scalar tau_inv) {
+  if (tau_inv < 1.0) {
+    return false;
+  }
+  motor_tau_inv_ = tau_inv;
+  return true;
+}
+
+bool QuadrotorDynamics::updateParams(const YAML::Node& params) {
+  if (params["quadrotor_dynamics"]) {
+    // load parameters from a yaml configuration file
+    mass_ = params["quadrotor_dynamics"]["mass"].as<Scalar>();
+    arm_l_ = params["quadrotor_dynamics"]["arm_l"].as<Scalar>();
+    motor_omega_min_ =
+      params["quadrotor_dynamics"]["motor_omega_min"].as<Scalar>();
+    motor_omega_max_ =
+      params["quadrotor_dynamics"]["motor_omega_max"].as<Scalar>();
+    motor_tau_inv_ =
+      (1.0 / params["quadrotor_dynamics"]["motor_tau"].as<Scalar>());
+    std::vector<Scalar> thrust_map;
+    thrust_map =
+      params["quadrotor_dynamics"]["thrust_map"].as<std::vector<Scalar>>();
+    thrust_map_ = Map<Vector<3>>(thrust_map.data());
+    kappa_ = params["quadrotor_dynamics"]["kappa"].as<Scalar>();
+    std::vector<Scalar> omega_max;
+    omega_max =
+      params["quadrotor_dynamics"]["omega_max"].as<std::vector<Scalar>>();
+    omega_max_ = Map<Vector<3>>(omega_max.data());
+
+    // update relevant variables
+    updateInertiaMarix();
+    return valid();
+  } else {
+    return false;
+  }
+}
+
+bool QuadrotorDynamics::updateInertiaMarix() {
+  if (!valid()) return false;
+  t_BM_ = arm_l_ * sqrt(0.5) *
+          (Matrix<3, 4>() << 1, -1, -1, 1, -1, -1, 1, 1, 0, 0, 0, 0).finished();
+  J_ = mass_ / 12.0 * arm_l_ * arm_l_ * Vector<3>(4.5, 4.5, 7).asDiagonal();
+  J_inv_ = J_.inverse();
+  return true;
+}
+
+std::ostream& operator<<(std::ostream& os, const QuadrotorDynamics& quad) {
+  os.precision(3);
+  os << "Quadrotor Dynamics:\n"
+     << "mass =             [" << quad.mass_ << "]\n"
+     << "arm_l =            [" << quad.arm_l_ << "]\n"
+     << "t_BM =             [" << quad.t_BM_.row(0) << "]\n"
+     << "                   [" << quad.t_BM_.row(1) << "]\n"
+     << "                   [" << quad.t_BM_.row(2) << "]\n"
+     << "inertia =          [" << quad.J_.row(0) << "]\n"
+     << "                   [" << quad.J_.row(1) << "]\n"
+     << "                   [" << quad.J_.row(2) << "]\n"
+     << "motor_omega_min =  [" << quad.motor_omega_min_ << "]\n"
+     << "motor_omega_max =  [" << quad.motor_omega_max_ << "]\n"
+     << "motor_tau_inv =    [" << quad.motor_tau_inv_ << "]\n"
+     << "thrust_map =       [" << quad.thrust_map_.transpose() << "]\n"
+     << "kappa =            [" << quad.kappa_ << "]\n"
+     << "thrust_min =       [" << quad.thrust_min_ << "]\n"
+     << "thrust_max =       [" << quad.thrust_max_ << "]\n"
+     << "omega_max =        [" << quad.omega_max_.transpose() << "]"
+     << std::endl;
+  os.precision();
+  return os;
+}
+
+}  // namespace flightlib
--- a/flightlib/src/envs/quadrotor_env.cpp
+++ b/flightlib/src/envs/quadrotor_env.cpp
@@ -0,0 +1,481 @@
+#include "flightlib/envs/quadrotor_env.hpp"
+
+namespace flightlib {
+
+QuadrotorEnv::QuadrotorEnv() : QuadrotorEnv(getenv("FLIGHTMARE_PATH") + std::string("/flightlib/configs/quadrotor_env.yaml")) {}
+
+QuadrotorEnv::QuadrotorEnv(const std::string &cfg_path) : EnvBase() {
+	// load configuration file
+	YAML::Node cfg_ = YAML::LoadFile(cfg_path);
+	loadParam(cfg_);
+
+	quadrotor_ptr_ = std::make_shared<Quadrotor>();
+
+	// 1、define a bounding box (z is defined manually, different from spawn box)
+	// x_min, x_max, y_min, y_max, z_min, z_max
+	world_box_ << center_(0) - 0.5 * scale_(0), center_(0) + 0.5 * scale_(0), center_(1) - 0.5 * scale_(1), center_(1) + 0.5 * scale_(1), 0.0, 5.0;
+	if (!quadrotor_ptr_->setWorldBox(world_box_)) {
+		logger_.error("cannot set wolrd box");
+	};
+
+	// 2、define input and output dimension for the environment
+	obs_dim_ = kNObs;
+	act_dim_ = kNAct;
+	rew_dim_ = 1;
+
+	// 3、define planner
+	traj_opt_bridge = new TrajOptimizationBridge();
+
+	// 5、add camera
+	sgm_.reset(new sgm_gpu::SgmGpu(width_, height_));
+
+	if (!configCamera(cfg_)) {
+		logger_.error("Cannot config RGB Camera. Something wrong with the config file");
+	}
+
+	Vector<3> quad_size(0.01, 0.01, 0.01);
+	quadrotor_ptr_->setSize(quad_size);
+	is_collision_ = false;
+	steps_        = 0;
+}
+
+QuadrotorEnv::~QuadrotorEnv() { delete traj_opt_bridge; }
+
+bool QuadrotorEnv::reset(Ref<Vector<>> obs, const bool random) {
+	quad_state_.setZero();
+	quad_act_.setZero();
+	is_collision_    = false;
+	steps_           = 0;
+	nearest_obstacle = 10;
+
+	// Dagger Training
+	if (random && !collect_data_) {
+		// 1.reset position.
+		do {
+			is_collision_           = false;
+			quad_state_.x(QS::POSX) = 0.40 * scale_(0) * uniform_dist_(random_gen_) + center_(0);
+			quad_state_.x(QS::POSY) = 0.40 * scale_(1) * uniform_dist_(random_gen_) + center_(1);
+			quad_state_.x(QS::POSZ) = 0.20 * scale_(2) * uniform_dist_(random_gen_) + center_(2);
+			collisionCheck(1.5);
+		} while (is_collision_);
+
+		// 2.reset orientation
+		float roll  = 0.0;
+		float pitch = 0.0;
+		float yaw   = 3.14159 * uniform_dist_(random_gen_);
+		Eigen::Quaternionf q_;
+		q_ = Eigen::AngleAxisf(yaw, Eigen::Vector3f::UnitZ()) * Eigen::AngleAxisf(pitch, Eigen::Vector3f::UnitY()) *
+		     Eigen::AngleAxisf(roll, Eigen::Vector3f::UnitX());
+		quad_state_.q(q_);
+	}
+
+	// Offline Data Collection
+	if (collect_data_) {
+		// 1.reset position.
+		do {
+			is_collision_           = false;
+			quad_state_.x(QS::POSX) = 0.5 * scale_(0) * uniform_dist_(random_gen_) + center_(0);
+			quad_state_.x(QS::POSY) = 0.5 * scale_(1) * uniform_dist_(random_gen_) + center_(1);
+			quad_state_.x(QS::POSZ) = 0.5 * scale_(2) * uniform_dist_(random_gen_) + center_(2);
+			collisionCheck(0.5);
+		} while (is_collision_);
+
+		// 2.reset orientation
+		float roll  = (norm_dist_(random_gen_) * sqrt(roll_var_)) + 0.0;
+		float pitch = (norm_dist_(random_gen_) * sqrt(pitch_var_)) + 0.0;
+		float yaw   = 3.14159 * uniform_dist_(random_gen_);
+		Eigen::Quaternionf q_;
+		q_ = Eigen::AngleAxisf(yaw, Eigen::Vector3f::UnitZ()) * Eigen::AngleAxisf(pitch, Eigen::Vector3f::UnitY()) *
+		     Eigen::AngleAxisf(roll, Eigen::Vector3f::UnitX());
+		quad_state_.q(q_);
+	}
+
+	// reset quadrotor with random states
+	quadrotor_ptr_->reset(quad_state_);
+	// Currently, since there is no controller, the desired state is equal to the actual state.
+	desired_p_ = Eigen::Vector3f(quad_state_.p(0), quad_state_.p(1), quad_state_.p(2));
+	desired_v_ = Eigen::Vector3f(quad_state_.v(0), quad_state_.v(1), quad_state_.v(2));
+	desired_a_ = Eigen::Vector3f(quad_state_.a(0), quad_state_.a(1), quad_state_.a(2));
+
+	// obtain observations
+	getObs(obs);
+	return true;
+}
+
+void QuadrotorEnv::setState(ConstRef<Vector<>> state) {
+	if (state.rows() != 13) {
+		std::cout << "ERROR: state must be 13 dim (P_xyz, V_xyz, A_xyz, Q_wxyz)" << std::endl;
+		return;
+	}
+	quad_state_.setZero();
+	quad_state_.x(QS::POSX) = state(0);
+	quad_state_.x(QS::POSY) = state(1);
+	quad_state_.x(QS::POSZ) = state(2);
+	quad_state_.x(QS::VELX) = state(3);
+	quad_state_.x(QS::VELY) = state(4);
+	quad_state_.x(QS::VELZ) = state(5);
+	quad_state_.x(QS::ACCX) = state(6);
+	quad_state_.x(QS::ACCY) = state(7);
+	quad_state_.x(QS::ACCZ) = state(8);
+	quad_state_.x(QS::ATTW) = state(9);
+	quad_state_.x(QS::ATTX) = state(10);
+	quad_state_.x(QS::ATTY) = state(11);
+	quad_state_.x(QS::ATTZ) = state(12);
+	quadrotor_ptr_->reset(quad_state_);
+}
+
+void QuadrotorEnv::setGoal(ConstRef<Vector<>> goal) {
+	if (goal.rows() != 3) {
+		std::cout << "ERROR: goal must be 3 dim (xyz)" << std::endl;
+		return;
+	}
+	goal_ = goal;
+}
+
+bool QuadrotorEnv::getObs(Ref<Vector<>> obs) {
+	// The actual position.
+	Eigen::Vector3f Pw(quad_state_.p(0), quad_state_.p(1), quad_state_.p(2));
+	Eigen::Quaternionf Qw = quad_state_.q();
+	// The desired state, same with the real flight.
+	Eigen::Matrix3f Rwb = quad_state_.R();
+	Eigen::Vector3f Vw(desired_v_(0), desired_v_(1), desired_v_(2));
+	Eigen::Vector3f Vb = Rwb.inverse() * Vw;
+	Eigen::Vector3f Aw(desired_a_(0), desired_a_(1), desired_a_(2));
+	Eigen::Vector3f Ab = Rwb.inverse() * Aw;
+
+	// Observation: p, q_wxyz in the world frame (for training); v, a in the body frame (for testing).
+	quad_obs_ << Pw(0), Pw(1), Pw(2), Vb(0), Vb(1), Vb(2), Ab(0), Ab(1), Ab(2), Qw.w(), Qw.x(), Qw.y(), Qw.z();
+	obs.segment<kNObs>(0) = quad_obs_;
+	return true;
+}
+
+bool QuadrotorEnv::getDepthImage(Ref<DepthImgVector<>> depth_img) {
+	if (!rgb_camera_left || !rgb_camera_left->getEnabledLayers()[1]) {
+		logger_.error("No RGB Camera or depth map is not enabled. Cannot retrieve depth images.");
+		return false;
+	}
+	rgb_camera_left->getDepthMap(depth_img_);
+
+	depth_img = Map<DepthImgVector<>>((float_t *)depth_img_.data, depth_img_.rows * depth_img_.cols);
+	return true;
+}
+
+bool QuadrotorEnv::getStereoImage(Ref<DepthImgVector<>> stereo_img) {
+	if (!rgb_camera_left || !rgb_camera_right) {
+		logger_.error("No Stereo Camera enabled. Cannot retrieve depth map.");
+		return false;
+	}
+	cv::Mat left_img, right_img;
+	rgb_camera_left->getRGBImage(left_img);
+	rgb_camera_right->getRGBImage(right_img);
+
+	// compute disparity image
+	cv::Mat stereo_(height_, width_, CV_32FC1);
+	computeDepthImage(left_img, right_img, &stereo_);
+
+	// fix the nan of stereo by gt depth (make it closer to RealSense 435, as the depth from 435 is better than from SGM directly)
+	if (rgb_camera_left->getEnabledLayers()[1]) {
+		if (rgb_camera_left->getDepthMap(depth_img_)) {
+			cv::Mat mask, mask1, mask2;
+			cv::compare(stereo_, 0, mask1, cv::CMP_EQ);   // 将 A 中为 0 的位置置为 255，其余位置置为 0
+			cv::compare(stereo_, 20, mask2, cv::CMP_GT);  // 将 A 中大于 20 的位置置为 255，其余位置置为 0
+			mask = mask1 | mask2;                         // 将两个掩码进行逻辑或操作
+			depth_img_.copyTo(stereo_, mask);             // 将 B 中 mask 为 255 的位置的值复制到 A 中
+		}
+	}
+
+	stereo_img = Map<DepthImgVector<>>((float_t *)stereo_.data, stereo_.rows * stereo_.cols);
+	return true;
+}
+
+void QuadrotorEnv::setMapID(int id) {
+	// -1 represent using the latest map in imitation learning
+	if (id < 0)
+		map_idx_ = kdtrees.size() - 1;
+	else
+		map_idx_ = id;
+}
+
+void QuadrotorEnv::getCostAndGradient(ConstRef<Vector<>> endstate, int traj_id, float &cost, Ref<Vector<>> grad) {
+	if (endstate.rows() != 9) {
+		std::cout << "ERROR: endstate must be 9 dim (X_pva, Y_pva, Z_pva)" << std::endl;
+		return;
+	}
+	std::vector<double> endstate_, grad_;
+	double cost_;
+	for (size_t i = 0; i < endstate.rows(); i++) {
+		endstate_.push_back(static_cast<double>(endstate(i)));
+	}
+
+	traj_opt_bridge->setMap(esdf_maps[map_idx_], min_map_boundaries[map_idx_], max_map_boundaries[map_idx_]);
+	traj_opt_bridge->setState(quad_state_.p.cast<double>(), quad_state_.q().cast<double>(), quad_state_.v.cast<double>(),
+	                          quad_state_.a.cast<double>());
+	traj_opt_bridge->setGoal(goal_.cast<double>());
+
+	traj_opt_bridge->getCostAndGradient(endstate_, traj_id, cost_, grad_);
+
+	for (size_t i = 0; i < grad_.size(); i++) {
+		grad(i) = static_cast<float>(grad_[i]);
+	}
+	cost = static_cast<float>(cost_);
+}
+
+bool QuadrotorEnv::step(const Ref<Vector<>> act, Ref<Vector<>> obs, Ref<Vector<>> reward) {
+	// python：setGoal -> step
+	if (!act.allFinite() || act.rows() != 9 || reward.rows() != 1) {
+		std::cout << "ERROR: endstate must be 9 dim" << std::endl;
+		return false;
+	}
+
+	steps_++;
+
+	std::vector<double> endstate_;
+	for (size_t i = 0; i < act.rows(); i++) {
+		endstate_.push_back(static_cast<double>(act(i)));
+	}
+
+	traj_opt_bridge->setMap(esdf_maps[map_idx_], min_map_boundaries[map_idx_], max_map_boundaries[map_idx_]);
+	traj_opt_bridge->setState(quad_state_.p.cast<double>(), quad_state_.q().cast<double>(), quad_state_.v.cast<double>(),
+	                          quad_state_.a.cast<double>());
+	traj_opt_bridge->setGoal(goal_.cast<double>());
+
+	double cost_;
+	Eigen::Vector3d next_pos, next_vel, next_acc;
+	traj_opt_bridge->getNextStateAndCost(endstate_, cost_, next_pos, next_vel, next_acc, sim_dt_);
+	desired_p_ = next_pos.cast<float>();
+	desired_v_ = next_vel.cast<float>();
+	desired_a_ = next_acc.cast<float>();
+	reward(0)  = static_cast<float>(cost_);
+
+	// calculate the state based on the desired state.
+	runControlAndUpdateState(desired_p_, desired_v_, desired_a_);
+	quadrotor_ptr_->getState(&quad_state_);
+
+	getObs(obs);
+	return true;
+}
+
+bool QuadrotorEnv::isTerminalState(Scalar &reward) {
+	// 1.if out of boundary
+	if (quad_state_.x(QS::POSX) <= (world_box_(0)) || quad_state_.x(QS::POSY) <= (world_box_(1)) || quad_state_.x(QS::POSZ) <= (world_box_(2)) ||
+	    quad_state_.x(QS::POSX) >= (world_box_(3)) || quad_state_.x(QS::POSY) >= (world_box_(4)) || quad_state_.x(QS::POSZ) >= (world_box_(5))) {
+		reward = 0;
+		// std::cout<<"越界"<<std::endl;
+		return true;
+	}
+
+	// 2.if collision
+	if (is_collision_) {
+		reward = -1;
+		// std::cout<<"碰撞"<<std::endl;
+		return true;
+	}
+
+	// 3.prevent uav being trapped
+	if (steps_ > 10 && quad_state_.v.norm() < 0.6 && dagger_mode_) {
+		reward = 0;
+		return true;
+	}
+
+	// 4.if reach the goal
+	Eigen::Vector3f Pw(quad_state_.p(0), quad_state_.p(1), quad_state_.p(2));
+	Eigen::Vector3f Gw(goal_(0), goal_(1), goal_(2));
+	float dist = (Pw - Gw).norm();
+	if (dist < 4) {
+		reward = 0;
+		// std::cout<<"到达"<<std::endl;
+		return true;
+	}
+
+	reward = 0.0;
+	return false;
+}
+
+void QuadrotorEnv::collisionCheck(float dis) {
+	pcl::PointXYZ drone_;
+	drone_.x = quad_state_.x(QS::POSX);
+	drone_.y = quad_state_.x(QS::POSY);
+	drone_.z = quad_state_.x(QS::POSZ);
+
+	int K = 1;
+	std::vector<int> indices(K);
+	std::vector<float> distances(K);  // the square of the distances to the neighboring points.
+	kdtrees[map_idx_]->nearestKSearch(drone_, K, indices, distances);
+
+	nearest_obstacle = distances[0];
+	if (distances[0] < dis * dis)
+		is_collision_ = true;
+}
+
+bool QuadrotorEnv::loadParam(const YAML::Node &cfg) {
+	// camera
+	width_  = cfg["rgb_camera_left"]["width"].as<int>();
+	height_ = cfg["rgb_camera_left"]["height"].as<int>();
+	fov_    = cfg["rgb_camera_left"]["fov"].as<Scalar>();
+	// train or test
+	collect_data_ = cfg["quadrotor_env"]["collect_data"].as<bool>();
+	sim_dt_       = cfg["quadrotor_env"]["sim_dt"].as<Scalar>();
+	// data_collection
+	roll_var_  = cfg["data_collection"]["roll_var"].as<Scalar>();
+	pitch_var_ = cfg["data_collection"]["pitch_var"].as<Scalar>();
+	// world box
+	for (int i = 0; i < 3; ++i) {
+		scale_(i)  = cfg["quadrotor_env"]["bounding_box"][i].as<Scalar>();
+		center_(i) = cfg["quadrotor_env"]["bounding_box_origin"][i].as<Scalar>();
+	}
+	return true;
+}
+
+bool QuadrotorEnv::getAct(Ref<Vector<>> act) const {
+	if (quad_act_.allFinite()) {
+		act = quad_act_;
+		return true;
+	}
+	return false;
+}
+
+bool QuadrotorEnv::configCamera(const YAML::Node &cfg) {
+	if (!cfg["rgb_camera_left"]) {
+		logger_.error("Cannot config RGB Camera");
+		return false;
+	}
+
+	if (!cfg["rgb_camera_left"]["on"].as<bool>()) {
+		logger_.warn("Camera is off. Please turn it on.");
+		return false;
+	}
+
+	if (quadrotor_ptr_->getNumCamera() >= 2) {
+		logger_.warn("Camera has been added. Skipping the camera configuration.");
+		return false;
+	}
+
+	// create left camera --------------------------------------------
+	rgb_camera_left = std::make_shared<RGBCamera>();
+
+	// load camera settings
+	std::vector<Scalar> t_BC_vec = cfg["rgb_camera_left"]["t_BC"].as<std::vector<Scalar>>();
+	std::vector<Scalar> r_BC_vec = cfg["rgb_camera_left"]["r_BC"].as<std::vector<Scalar>>();
+
+	Vector<3> t_BC(t_BC_vec.data());
+	Matrix<3, 3> r_BC = (AngleAxis(r_BC_vec[2] * M_PI / 180.0, Vector<3>::UnitZ()) * AngleAxis(r_BC_vec[1] * M_PI / 180.0, Vector<3>::UnitY()) *
+	                     AngleAxis(r_BC_vec[0] * M_PI / 180.0, Vector<3>::UnitX()))
+	                        .toRotationMatrix();
+
+	// Flightmare's FOV is vertical, while usually is horizontal, so convert the input horizontal FOV to vertical FOV.
+	Scalar rgb_fov_deg_    = cfg["rgb_camera_left"]["fov"].as<Scalar>();
+	double hor_fov_radians = (M_PI * (rgb_fov_deg_ / 180.0));
+	Scalar img_rows_       = cfg["rgb_camera_left"]["height"].as<Scalar>();
+	Scalar img_cols_       = cfg["rgb_camera_left"]["width"].as<Scalar>();
+	double flightmare_fov  = 2. * std::atan(std::tan(hor_fov_radians / 2) * img_rows_ / img_cols_);
+	flightmare_fov         = (flightmare_fov / M_PI) * 180.0;
+	rgb_camera_left->setFOV(flightmare_fov);
+	rgb_camera_left->setWidth(cfg["rgb_camera_left"]["width"].as<int>());
+	rgb_camera_left->setHeight(cfg["rgb_camera_left"]["height"].as<int>());
+	rgb_camera_left->setRelPose(t_BC, r_BC);
+	rgb_camera_left->enableOpticalFlow(cfg["rgb_camera_left"]["enable_opticalflow"].as<bool>());
+	rgb_camera_left->enableSegmentation(cfg["rgb_camera_left"]["enable_segmentation"].as<bool>());
+	rgb_camera_left->enableDepth(cfg["rgb_camera_left"]["enable_depth"].as<bool>());
+
+	// add camera to the quadrotor
+	quadrotor_ptr_->addRGBCamera(rgb_camera_left);
+
+	// create right camera --------------------------------------------
+	bool have_right_camera = cfg["rgb_camera_right"]["on"].as<bool>();
+	if (have_right_camera) {
+		rgb_camera_right = std::make_shared<RGBCamera>();
+
+		// load camera settings
+		std::vector<Scalar> t_BC_vec_r = cfg["rgb_camera_right"]["t_BC"].as<std::vector<Scalar>>();
+		std::vector<Scalar> r_BC_vec_r = cfg["rgb_camera_right"]["r_BC"].as<std::vector<Scalar>>();
+
+		Vector<3> t_BC_r(t_BC_vec_r.data());
+		Matrix<3, 3> r_BC_r =
+		    (AngleAxis(r_BC_vec_r[2] * M_PI / 180.0, Vector<3>::UnitZ()) * AngleAxis(r_BC_vec_r[1] * M_PI / 180.0, Vector<3>::UnitY()) *
+		     AngleAxis(r_BC_vec_r[0] * M_PI / 180.0, Vector<3>::UnitX()))
+		        .toRotationMatrix();
+
+		rgb_camera_right->setFOV(flightmare_fov);
+		rgb_camera_right->setWidth(cfg["rgb_camera_left"]["width"].as<int>());
+		rgb_camera_right->setHeight(cfg["rgb_camera_left"]["height"].as<int>());
+		rgb_camera_right->setRelPose(t_BC_r, r_BC_r);
+		rgb_camera_right->enableOpticalFlow(false);
+		rgb_camera_right->enableSegmentation(false);
+		rgb_camera_right->enableDepth(false);
+
+		// add camera to the quadrotor
+		quadrotor_ptr_->addRGBCamera(rgb_camera_right);
+		stereo_baseline_ = fabs(t_BC(1) - t_BC_r(1));
+	}
+	// adapt parameters
+	img_width_  = rgb_camera_left->getWidth();
+	img_height_ = rgb_camera_left->getHeight();
+	rgb_img_    = cv::Mat::zeros(img_height_, img_width_, CV_MAKETYPE(CV_8U, rgb_camera_left->getChannels()));
+	depth_img_  = cv::Mat::zeros(img_height_, img_width_, CV_32FC1);
+	return true;
+}
+
+void QuadrotorEnv::computeDepthImage(const cv::Mat &left_frame, const cv::Mat &right_frame, cv::Mat *const depth) {
+	cv::Mat disparity(height_, width_, CV_8UC1);
+	sgm_->computeDisparity(left_frame, right_frame, disparity);
+	disparity.convertTo(disparity, CV_32FC1);
+
+	// compute depth from disparity
+	float f = (width_ / 2.0) / std::tan((M_PI * (fov_ / 180.0)) / 2.0);
+	//  depth = stereo_baseline_ * f / disparity
+	for (int r = 0; r < height_; ++r) {
+		for (int c = 0; c < width_; ++c) {
+			if (disparity.at<float>(r, c) == 0.0f) {
+				depth->at<float>(r, c) = 0.0f;
+			} else if (disparity.at<float>(r, c) == 255.0f) {
+				depth->at<float>(r, c) = 0.0f;
+			} else {
+				depth->at<float>(r, c) = static_cast<float>(stereo_baseline_) * f / disparity.at<float>(r, c);
+			}
+		}
+	}
+}
+
+bool QuadrotorEnv::getRGBImage(Ref<ImgVector<>> img, const bool rgb) {
+	if (!rgb_camera_left) {
+		logger_.error("No Camera! Cannot retrieve Images.");
+		return false;
+	}
+
+	rgb_camera_left->getRGBImage(rgb_img_);
+
+	if (rgb_img_.rows != height_ || rgb_img_.cols != width_) {
+		logger_.error("Image resolution mismatch. Aborting.. Image rows %d != %d, Image cols %d != %d", rgb_img_.rows, height_, rgb_img_.cols,
+		              width_);
+		return false;
+	}
+
+	if (!rgb) {
+		// converting rgb image to gray image
+		cvtColor(rgb_img_, gray_img_, CV_RGB2GRAY);
+		// map cv::Mat data to Eiegn::Vector
+		img = Map<ImgVector<>>(gray_img_.data, gray_img_.rows * gray_img_.cols);
+	} else {
+		img = Map<ImgVector<>>(rgb_img_.data, rgb_img_.rows * rgb_img_.cols * rgb_camera_left->getChannels());
+	}
+	return true;
+}
+
+void QuadrotorEnv::runControlAndUpdateState(Eigen::Vector3f p_ref, Eigen::Vector3f v_ref, Eigen::Vector3f a_ref) {
+	Eigen::Vector3f p_cur;
+	p_cur = quad_state_.p;
+
+	Eigen::Vector3f dir_vel  = v_ref;
+	Eigen::Vector3f dir_goal = goal_ - p_cur;
+	Eigen::Vector3f dir_des  = dir_vel.normalized() + dir_goal.normalized();
+	float yaw_ref            = atan2(dir_des(1), dir_des(0));
+	Vector<3> rpy_cur;
+	get_euler_from_R(rpy_cur, quad_state_.R());
+	yaw_ref = calculate_yaw(rpy_cur(2), yaw_ref, sim_dt_);  // limit the yaw (required by controller)
+
+	Eigen::Quaternionf q_ref;
+	quadrotor_ptr_->runSimpleFlight(a_ref, yaw_ref, q_ref);
+	quadrotor_ptr_->setState(p_ref, v_ref, q_ref, a_ref, sim_dt_);
+}
+
+}  // namespace flightlib
--- a/flightlib/src/envs/vec_env.cpp
+++ b/flightlib/src/envs/vec_env.cpp
@@ -0,0 +1,406 @@
+#include "flightlib/envs/vec_env.hpp"
+
+namespace flightlib {
+
+template<typename EnvBase>
+VecEnv<EnvBase>::VecEnv() : VecEnv(getenv("FLIGHTMARE_PATH") + std::string("/flightlib/configs/vec_env.yaml")) {}
+
+template<typename EnvBase>
+VecEnv<EnvBase>::VecEnv(const YAML::Node& cfg_node) : cfg_(cfg_node) {
+	init();
+}
+
+template<typename EnvBase>
+VecEnv<EnvBase>::VecEnv(const std::string& cfgs, const bool from_file) {
+	// load environment configuration
+	if (from_file)
+		cfg_ = YAML::LoadFile(cfgs);
+	else
+		cfg_ = YAML::Load(cfgs);
+
+	init();
+}
+
+template<typename EnvBase>
+void VecEnv<EnvBase>::init(void) {
+	// note that the cfg are input from python, and many have changed from vec_end.yaml
+	unity_render_          = cfg_["env"]["render"].as<bool>();
+	supervised_mode_       = cfg_["env"]["supervised"].as<bool>();
+	dagger_mode_           = cfg_["env"]["imitation"].as<bool>();
+	seed_                  = cfg_["env"]["seed"].as<int>();
+	num_envs_              = cfg_["env"]["num_envs"].as<int>();
+	num_threads_           = cfg_["env"]["num_threads"].as<int>();
+	scene_id_              = cfg_["env"]["scene_id"].as<SceneID>();
+	ply_path_              = getenv("FLIGHTMARE_PATH") + cfg_["env"]["ply_path"].as<std::string>();
+	avg_tree_spacing_      = cfg_["unity"]["avg_tree_spacing"].as<Scalar>();
+	pointcloud_resolution_ = cfg_["unity"]["pointcloud_resolution"].as<Scalar>();
+	for (int i = 0; i < 3; ++i) {
+		bounding_box_(i)        = cfg_["unity"]["bounding_box"][i].as<Scalar>();
+		bounding_box_origin_(i) = cfg_["unity"]["bounding_box_origin"][i].as<Scalar>();
+	}
+
+	// set threads
+	omp_set_num_threads(cfg_["env"]["num_threads"].as<int>());
+
+	// create & setup environments
+	const bool render = false;
+	for (int i = 0; i < num_envs_; i++) {
+		envs_.push_back(std::make_unique<EnvBase>());
+	}
+
+	// set Unity (init unity_bridge_ptr_ and add quadrotors to envs)
+	setUnity(unity_render_);
+
+	obs_dim_    = envs_[0]->getObsDim();
+	act_dim_    = envs_[0]->getActDim();
+	rew_dim_    = envs_[0]->getRewDim();
+	img_width_  = envs_[0]->getImgWidth();
+	img_height_ = envs_[0]->getImgHeight();
+
+	// if supervised_mode, then generate map from .ply
+	if (supervised_mode_)
+		generateMaps();
+	// set dagger_mode to each env
+	for (int i = 0; i < num_envs_; i++)
+		envs_[i]->setDAggerMode(dagger_mode_);
+
+	std::cout << "Vectorized Environment:\n"
+	          << "dagger mode       =        [" << dagger_mode_ << "]\n"
+	          << "supervised mode   =        [" << supervised_mode_ << "]\n"
+	          << "obs dim           =        [" << obs_dim_ << "]\n"
+	          << "act dim           =        [" << act_dim_ << "]\n"
+	          << "img width         =        [" << img_width_ << "]\n"
+	          << "img height        =        [" << img_height_ << "]\n"
+	          << "num_envs          =        [" << num_envs_ << "]\n"
+	          << "num_thread        =        [" << num_threads_ << "]\n"
+	          << "scene_id          =        [" << scene_id_ << "]" << std::endl;
+}
+
+template<typename EnvBase>
+VecEnv<EnvBase>::~VecEnv() {}
+
+// ======================	set functions	===================== //
+
+template<typename EnvBase>
+bool VecEnv<EnvBase>::reset(Ref<MatrixRowMajor<>> obs) {
+	if (obs.rows() != num_envs_ || obs.cols() != obs_dim_) {
+		logger_.error("Input matrix dimensions do not match with that of the environment.");
+		return false;
+	}
+
+	for (int i = 0; i < num_envs_; i++) {
+		envs_[i]->reset(obs.row(i));
+	}
+
+	if (unity_render_ && unity_ready_) {
+		frameID = 1;
+		FrameID frameID_rt;
+		unity_bridge_ptr_->getRender(frameID);
+		unity_bridge_ptr_->handleOutput(frameID_rt);
+		while (frameID != frameID_rt)
+			unity_bridge_ptr_->handleOutput(frameID_rt);
+	}
+	return true;
+}
+
+template<typename EnvBase>
+bool VecEnv<EnvBase>::setState(ConstRef<MatrixRowMajor<>> state) {
+	if (state.rows() != num_envs_ || state.cols() != 13) {  // 13: pvaq
+		logger_.error("Input state dimensions do not match with state.");
+		return false;
+	}
+
+	for (int i = 0; i < num_envs_; i++) {
+		envs_[i]->setState(state.row(i));
+	}
+
+	return true;
+}
+
+template<typename EnvBase>
+bool VecEnv<EnvBase>::setGoal(ConstRef<MatrixRowMajor<>> goal) {
+	if (goal.rows() != num_envs_ || goal.cols() != 3) {
+		logger_.error("Input goal dimensions do not match with 3.");
+		return false;
+	}
+
+	for (int i = 0; i < num_envs_; i++) {
+		envs_[i]->setGoal(goal.row(i));
+	}
+
+	return true;
+}
+
+template<typename EnvBase>
+bool VecEnv<EnvBase>::step(Ref<MatrixRowMajor<>> act, Ref<MatrixRowMajor<>> obs, Ref<MatrixRowMajor<>> reward, Ref<BoolVector<>> done) {
+	if (act.rows() != num_envs_ || act.cols() != act_dim_ || obs.rows() != num_envs_ || obs.cols() != obs_dim_ || reward.rows() != num_envs_ ||
+	    reward.cols() != rew_dim_ || done.rows() != num_envs_ || done.cols() != 1) {
+		logger_.error("Input matrix dimensions do not match with that of the environment.");
+		return false;
+	}
+
+#pragma omp parallel for schedule(dynamic)
+	for (int i = 0; i < num_envs_; i++) {
+		perAgentStep(i, act, obs, reward, done);
+	}
+
+	if (unity_render_ && unity_ready_) {
+		frameID++;
+		FrameID frameID_rt;
+		unity_bridge_ptr_->getRender(frameID);
+		unity_bridge_ptr_->handleOutput(frameID_rt);
+		while (frameID != frameID_rt)
+			unity_bridge_ptr_->handleOutput(frameID_rt);
+	}
+	return true;
+}
+
+template<typename EnvBase>
+void VecEnv<EnvBase>::perAgentStep(int agent_id, Ref<MatrixRowMajor<>> act, Ref<MatrixRowMajor<>> obs, Ref<MatrixRowMajor<>> reward,
+                                   Ref<BoolVector<>> done) {
+	envs_[agent_id]->step(act.row(agent_id), obs.row(agent_id), reward.row(agent_id));
+
+	// use larger collision threshold in training and lower in testing
+	if (dagger_mode_)
+		envs_[agent_id]->collisionCheck(0.3);
+	else
+		envs_[agent_id]->collisionCheck(0.1);
+
+	Scalar terminal_reward = 0;
+	done(agent_id)         = envs_[agent_id]->isTerminalState(terminal_reward);
+
+	if (done[agent_id]) {
+		envs_[agent_id]->reset(obs.row(agent_id));
+	}
+}
+
+template<typename EnvBase>
+void VecEnv<EnvBase>::setMapID(ConstRef<IntVector<>> id) {
+	if (id.rows() != num_envs_) {
+		logger_.error("Input matrix dimensions do not match with that of the environment.");
+		return;
+	}
+
+	for (int i = 0; i < num_envs_; i++) {
+		envs_[i]->setMapID(id(i));
+	}
+}
+
+template<typename EnvBase>
+void VecEnv<EnvBase>::setSeed(const int seed) {
+	int seed_inc = seed;
+	for (int i = 0; i < num_envs_; i++)
+		envs_[i]->setSeed(seed_inc++);
+}
+
+// ======================	set functions	===================== //
+
+template<typename EnvBase>
+void VecEnv<EnvBase>::getObs(Ref<MatrixRowMajor<>> obs) {
+	for (int i = 0; i < num_envs_; i++)
+		envs_[i]->getObs(obs.row(i));
+}
+
+template<typename EnvBase>
+bool VecEnv<EnvBase>::getRGBImage(Ref<ImgMatrixRowMajor<>> img, const bool rgb_image) {
+	bool valid_img = true;
+	for (int i = 0; i < num_envs_; i++) {
+		valid_img &= envs_[i]->getRGBImage(img.row(i), rgb_image);
+	}
+	return valid_img;
+}
+
+template<typename EnvBase>
+bool VecEnv<EnvBase>::getStereoImage(Ref<DepthImgMatrixRowMajor<>> stereo_img) {
+	bool valid_img = true;
+	for (int i = 0; i < num_envs_; i++) {
+		valid_img &= envs_[i]->getStereoImage(stereo_img.row(i));
+	}
+	return valid_img;
+}
+
+template<typename EnvBase>
+bool VecEnv<EnvBase>::getDepthImage(Ref<DepthImgMatrixRowMajor<>> depth_img) {
+	bool valid_img = true;
+	for (int i = 0; i < num_envs_; i++) {
+		valid_img &= envs_[i]->getDepthImage(depth_img.row(i));
+	}
+	return valid_img;
+}
+
+
+template<typename EnvBase>
+void VecEnv<EnvBase>::getCostAndGradient(ConstRef<MatrixRowMajor<>> dp, ConstRef<IntVector<>> traj_id, Ref<Vector<>> cost,
+                                         Ref<MatrixRowMajor<>> grad) {
+#pragma omp parallel for schedule(dynamic) num_threads(num_threads_)
+	for (int i = 0; i < num_envs_; i++) {
+		envs_[i]->getCostAndGradient(dp.row(i), traj_id(i), cost(i), grad.row(i));
+	}
+}
+
+
+// ======================	unity functions	===================== //
+
+template<typename EnvBase>
+bool VecEnv<EnvBase>::setUnity(bool render) {
+	unity_render_ = render;
+	if (unity_render_ && unity_bridge_ptr_ == nullptr) {
+		// create unity bridge
+		unity_bridge_ptr_ = UnityBridge::getInstance();
+		// add objects to Unity
+		for (int i = 0; i < num_envs_; i++) {
+			envs_[i]->addObjectsToUnity(unity_bridge_ptr_);
+		}
+		logger_.info("Flightmare Bridge is created.");
+	}
+	return true;
+}
+
+template<typename EnvBase>
+bool VecEnv<EnvBase>::spawnTrees() {
+	if (!unity_ready_ || unity_bridge_ptr_ == nullptr)
+		return false;
+	bool spawned = unity_bridge_ptr_->spawnTrees(bounding_box_, bounding_box_origin_, avg_tree_spacing_);
+	return spawned;
+}
+
+template<typename EnvBase>
+bool VecEnv<EnvBase>::savePointcloud(int ply_id) {
+	if (!unity_ready_ || unity_bridge_ptr_ == nullptr)
+		return false;
+	Vector<3> min_corner = bounding_box_origin_ - 0.5 * bounding_box_;
+	Vector<3> max_corner = bounding_box_origin_ + 0.5 * bounding_box_;
+	unity_bridge_ptr_->generatePointcloud(min_corner, max_corner, ply_id, ply_path_, scene_id_, pointcloud_resolution_);
+	return true;
+}
+
+template<typename EnvBase>
+bool VecEnv<EnvBase>::spawnTreesAndSavePointcloud(int ply_id_in, float spacing) {
+	Scalar avg_tree_spacing = avg_tree_spacing_;
+	if (spacing > 0)
+		avg_tree_spacing = spacing;
+	int ply_id = envs_[0]->getMapNum();
+	if (ply_id_in >= 0)
+		ply_id = ply_id_in;
+
+	if (!unity_ready_ || unity_bridge_ptr_ == nullptr)
+		return false;
+
+	bool spawned = unity_bridge_ptr_->spawnTrees(bounding_box_, bounding_box_origin_, avg_tree_spacing);
+
+	Vector<3> min_corner = bounding_box_origin_ - 0.5 * bounding_box_;
+	Vector<3> max_corner = bounding_box_origin_ + 0.5 * bounding_box_;
+	unity_bridge_ptr_->generatePointcloud(min_corner, max_corner, ply_id, ply_path_, scene_id_, pointcloud_resolution_);
+
+	usleep(1 * 1e6);  // waitting 1s for generating completely
+
+	// KDtree, for collision detection
+	pcl::search::KdTree<pcl::PointXYZ> kdtree;
+	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+	std::cout << "Map Path: " << ply_path_ + "pointcloud-" + std::to_string(ply_id) + ".ply" << std::endl;
+	pcl::io::loadPLYFile<pcl::PointXYZ>(ply_path_ + "pointcloud-" + std::to_string(ply_id) + ".ply", *cloud);
+	kdtree.setInputCloud(cloud);  // 0.3s
+
+	// ESDF, for gradient calculation (map_id is required)
+	Eigen::Vector3d map_boundary_min, map_boundary_max;
+	std::shared_ptr<sdf_tools::SignedDistanceField> esdf_map = traj_opt::SdfConstruction(cloud, map_boundary_min, map_boundary_max);
+
+	for (int i = 0; i < num_envs_; i++) {
+		envs_[i]->addKdtree(std::make_shared<pcl::search::KdTree<pcl::PointXYZ>>(kdtree));
+		envs_[i]->addESDFMap(esdf_map);
+		envs_[i]->addMapSize(map_boundary_min, map_boundary_max);
+	}
+
+	return true;
+}
+
+// For supervised learning
+template<typename EnvBase>
+void VecEnv<EnvBase>::generateMaps() {
+	std::vector<std::string> ply_files;
+	for (const auto& entry : std::filesystem::directory_iterator(ply_path_)) {
+		if (entry.is_regular_file() && entry.path().extension() == ".ply") {
+			ply_files.push_back(entry.path().string());
+		}
+	}
+
+	// Sort according to the number of the filename.
+	std::sort(ply_files.begin(), ply_files.end(), [this](const std::string& a, const std::string& b) {
+		return extract_number(std::filesystem::path(a).filename().string()) < extract_number(std::filesystem::path(b).filename().string());
+	});
+
+	for (auto ply_file : ply_files) {
+		std::cout << "load ply file: " << ply_file << std::endl;
+		pcl::search::KdTree<pcl::PointXYZ> kdtree;
+		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+		pcl::io::loadPLYFile<pcl::PointXYZ>(ply_file, *cloud);
+		// KDtree, for collision detection
+		kdtree.setInputCloud(cloud);  // 0.3s
+		// ESDF, for gradient calculation
+		Eigen::Vector3d map_boundary_min, map_boundary_max;
+		std::shared_ptr<sdf_tools::SignedDistanceField> esdf_map = traj_opt::SdfConstruction(cloud, map_boundary_min, map_boundary_max);
+
+		std::cout << "pc min:" << map_boundary_min.transpose() << " pc max:" << map_boundary_max.transpose() << std::endl;
+		for (int i = 0; i < num_envs_; i++) {
+			envs_[i]->addKdtree(std::make_shared<pcl::search::KdTree<pcl::PointXYZ>>(kdtree));
+			envs_[i]->addESDFMap(esdf_map);
+			envs_[i]->addMapSize(map_boundary_min, map_boundary_max);
+		}
+	}
+}
+
+template<typename EnvBase>
+bool VecEnv<EnvBase>::connectUnity(void) {
+	if (unity_bridge_ptr_ == nullptr)
+		return false;
+	unity_ready_ = unity_bridge_ptr_->connectUnity(scene_id_);
+	return unity_ready_;
+}
+
+template<typename EnvBase>
+void VecEnv<EnvBase>::render(void) {
+	if (unity_render_ && unity_ready_) {
+		frameID++;
+		FrameID frameID_rt;
+		unity_bridge_ptr_->getRender(frameID);
+		unity_bridge_ptr_->handleOutput(frameID_rt);
+		while (frameID != frameID_rt)
+			unity_bridge_ptr_->handleOutput(frameID_rt);
+	}
+}
+
+template<typename EnvBase>
+void VecEnv<EnvBase>::disconnectUnity(void) {
+	if (unity_bridge_ptr_ != nullptr) {
+		unity_bridge_ptr_->disconnectUnity();
+		unity_ready_ = false;
+	} else {
+		logger_.warn("Flightmare Unity Bridge is not initialized.");
+	}
+}
+
+template<typename EnvBase>
+void VecEnv<EnvBase>::close() {
+	for (int i = 0; i < num_envs_; i++) {
+		envs_[i]->close();
+	}
+}
+
+// ======================	other functions	===================== //
+
+// Extract the number from the filename (e.g., pointcloud-1.ply)
+template<typename EnvBase>
+int VecEnv<EnvBase>::extract_number(const std::string& filename) {
+	std::regex number_regex("pointcloud-(\\d+)\\.ply");
+	std::smatch match;
+	if (std::regex_search(filename, match, number_regex)) {
+		return std::stoi(match[1]);
+	}
+	return -1;  // If no number is found, return -1
+}
+
+// IMPORTANT. Otherwise:
+// Segmentation fault (core dumped)
+template class VecEnv<QuadrotorEnv>;
+
+}  // namespace flightlib
--- a/flightlib/src/grad_traj_optimization/grad_traj_optimizer.cpp
+++ b/flightlib/src/grad_traj_optimization/grad_traj_optimizer.cpp
@@ -0,0 +1,375 @@
+#include "flightlib/grad_traj_optimization/grad_traj_optimizer.h"
+
+GradTrajOptimizer::GradTrajOptimizer(const YAML::Node &cfg) {
+	//-------------------------get parameter from server--------------------
+	this->w_smooth    = cfg["ws"].as<double>();
+	this->w_goal      = cfg["wg"].as<double>();
+	this->w_long      = cfg["wl"].as<double>();
+	this->w_vel       = cfg["wv"].as<double>();
+	this->w_acc       = cfg["wa"].as<double>();
+	this->w_collision = cfg["wc"].as<double>();
+
+	this->alpha  = cfg["alpha"].as<double>();
+	this->d0     = cfg["d0"].as<double>();
+	this->r      = cfg["r"].as<double>();
+	this->alphav = cfg["alphav"].as<double>();
+	this->v0     = cfg["v0"].as<double>();
+	this->rv     = cfg["rv"].as<double>();
+	this->alphaa = cfg["alphaa"].as<double>();
+	this->a0     = cfg["a0"].as<double>();
+	this->ra     = cfg["ra"].as<double>();
+
+	this->sgm_time = 2 * cfg["radio_range"].as<double>() / cfg["vel_max"].as<double>();
+
+	//------------------------generate optimization dependency------------------
+	Time    = Eigen::VectorXd::Zero(1);
+	Time(0) = sgm_time;
+
+	TrajectoryGenerator generator;
+	generator.QPGeneration(Time);
+	R   = generator.getR();
+	Rff = generator.getRff();
+	Rpp = generator.getRpp();
+	Rpf = generator.getRpf();
+	Rfp = generator.getRfp();
+	L   = generator.getL();
+	A   = generator.getA();
+	C   = generator.getC();
+
+	int m = Time.size();                      // number of segments in the trajectory
+	Dp    = Eigen::MatrixXd::Zero(3, m * 3);  // optimized x_pva, y_pva, z_pva (end state)
+	Df    = Eigen::MatrixXd::Zero(3, m * 3);  // fixed x_pva, y_pva, z_pva (init state)
+
+	V = Eigen::MatrixXd::Zero(6, 6);
+	for (int i = 0; i < 5; ++i)
+		V(i, i + 1) = i + 1;
+
+	num_dp    = Dp.cols();
+	num_df    = Df.cols();
+	num_point = Time.rows() + 1;
+	boundary  = Eigen::VectorXd::Zero(6);
+}
+
+void GradTrajOptimizer::setSignedDistanceField(std::shared_ptr<sdf_tools::SignedDistanceField> s, double res) {
+	this->sdf        = s;
+	this->resolution = res;
+}
+
+void GradTrajOptimizer::constrains(double &n, double min, double max) const {
+	if (n > max)
+		n = max;
+	else if (n < min)
+		n = min;
+}
+
+void GradTrajOptimizer::setGoal(Eigen::Vector3d goal) { this->goal = goal; }
+
+void GradTrajOptimizer::setBoundary(Eigen::Vector3d min, Eigen::Vector3d max) {
+	this->map_boundary_min = min;
+	this->map_boundary_max = max;
+	boundary(0)            = map_boundary_min(0);
+	boundary(1)            = map_boundary_max(0);
+	boundary(2)            = map_boundary_min(1);
+	boundary(3)            = map_boundary_max(1);
+	boundary(4)            = map_boundary_min(2);
+	boundary(5)            = map_boundary_max(2);
+}
+
+
+void GradTrajOptimizer::getCoefficientFromDerivative(Eigen::MatrixXd &coefficient, const std::vector<double> &_dp) const {
+	coefficient.resize(num_point - 1, 18);
+
+	for (int i = 0; i < 3; ++i) {
+		//-----------------------merge df and dp -> d(df,dp)-----------------------
+		Eigen::VectorXd df(num_df);
+		Eigen::VectorXd dp(num_dp);
+		Eigen::VectorXd d(num_df + num_dp);
+
+		df = Df.row(i);
+		for (int j = 0; j < num_dp; j++) {
+			dp(j) = _dp[j + num_dp * i];
+		}
+
+		d.segment(0, 3)      = df;
+		d.segment(3, num_dp) = dp;
+
+		// ----------------------convert derivative to coefficient------------------
+		Eigen::VectorXd coe(6 * (num_point - 1));
+		coe = L * d;
+
+		for (int j = 0; j < (num_point - 1); j++) {
+			coefficient.block(j, 6 * i, 1, 6) = coe.segment(6 * j, 6).transpose();
+		}
+	}
+}
+
+void GradTrajOptimizer::getCostAndGradient(const std::vector<double> &df, const std::vector<double> &dp, double &cost,
+                                           std::vector<double> &_grad) const {
+	cost               = 0;
+	double cost_smooth = 0;
+	double cost_colli  = 0;
+	double cost_goal   = 0;
+	double cost_long   = 0;
+	double cost_vel    = 0;  // deprecated
+	double cost_acc    = 0;  // deprecated
+
+	Eigen::MatrixXd gradient = Eigen::MatrixXd::Zero(3, num_dp);
+	Eigen::MatrixXd g_smooth = Eigen::MatrixXd::Zero(3, num_dp);
+	Eigen::MatrixXd g_colli  = Eigen::MatrixXd::Zero(3, num_dp);
+	Eigen::MatrixXd g_goal   = Eigen::MatrixXd::Zero(3, num_dp);
+	Eigen::MatrixXd g_long   = Eigen::MatrixXd::Zero(3, num_dp);
+	Eigen::MatrixXd g_vel    = Eigen::MatrixXd::Zero(3, num_dp);  // deprecated
+	Eigen::MatrixXd g_acc    = Eigen::MatrixXd::Zero(3, num_dp);  // deprecated
+
+	for (int i = 0; i < num_df; ++i) {
+		Df(0, i) = df[i];
+		Df(1, i) = df[i + num_df];
+		Df(2, i) = df[i + 2 * num_df];
+	}
+
+	// ------------------------- 1. get smoothness cost -----------------------------
+	{
+		// 平滑度的Cost，基于MinimalSnap，有：Js = d * R * d，其中d = [dF,dP]
+		Eigen::VectorXd dfx = Df.block(0, 0, 1, 3).transpose();
+		Eigen::VectorXd dfy = Df.block(1, 0, 1, 3).transpose();
+		Eigen::VectorXd dfz = Df.block(2, 0, 1, 3).transpose();
+
+		Eigen::VectorXd dpx = Eigen::VectorXd::Zero(num_dp);
+		Eigen::VectorXd dpy = Eigen::VectorXd::Zero(num_dp);
+		Eigen::VectorXd dpz = Eigen::VectorXd::Zero(num_dp);
+
+		for (int i = 0; i < num_dp; ++i) {
+			dpx(i) = dp[i];
+			dpy(i) = dp[i + num_dp];
+			dpz(i) = dp[i + 2 * num_dp];
+		}
+
+		Eigen::VectorXd dx    = Eigen::VectorXd::Zero(num_dp + num_df);
+		Eigen::VectorXd dy    = Eigen::VectorXd::Zero(num_dp + num_df);
+		Eigen::VectorXd dz    = Eigen::VectorXd::Zero(num_dp + num_df);
+		dx.segment(0, 3)      = dfx;
+		dx.segment(3, num_dp) = dpx;
+		dy.segment(0, 3)      = dfy;
+		dy.segment(3, num_dp) = dpy;
+		dz.segment(0, 3)      = dfz;
+		dz.segment(3, num_dp) = dpz;
+
+		// ------------------- 1.1 get smoothness cost,fs= d'Rd ---------------------
+		cost_smooth = double(dx.transpose() * R * dx) + double(dy.transpose() * R * dy) + (dz.transpose() * R * dz);
+
+		//-------------------- 1.2 get smoothness gradient --------------------------
+		Eigen::MatrixXd gx_smooth = 2 * Rfp.transpose() * dfx + 2 * Rpp * dpx;
+		Eigen::MatrixXd gy_smooth = 2 * Rfp.transpose() * dfy + 2 * Rpp * dpy;
+		Eigen::MatrixXd gz_smooth = 2 * Rfp.transpose() * dfz + 2 * Rpp * dpz;
+
+		g_smooth.row(0) = gx_smooth.transpose();
+		g_smooth.row(1) = gy_smooth.transpose();
+		g_smooth.row(2) = gz_smooth.transpose();
+	}
+
+	// -------------------------- 2. get collision cost -----------------------------
+	{
+		Eigen::MatrixXd coe;
+		getCoefficientFromDerivative(coe, dp);
+
+		Eigen::MatrixXd Ldp(6, num_dp);
+
+		// only single-segment polynomial here
+		for (int s = 0; s < Time.size(); s++) {
+			Ldp = L.block(6 * s, 3, 6, num_dp);
+
+			// discrete time step
+			double dt = Time(s) / 30.0;
+			for (double t = 1e-3; t < Time(s); t += dt) {
+				// get position, velocity
+				Eigen::Vector3d pos, vel;
+				getPositionFromCoeff(pos, coe, s, t);
+				getVelocityFromCoeff(vel, coe, s, t);
+
+				// get information from signed distance field
+				double dist = 0, gd = 0, cd = 0;
+				Eigen::Vector3d grad;
+				getDistanceAndGradient(pos, dist, grad);  // 在sdf地图中的距离障碍物dist和grad（梯度方向）
+				getDistancePenalty(dist, cd);             // 计算障碍物距离惩罚cost
+				getDistancePenaltyGradient(dist, gd);     // 计算障碍物距离惩罚的梯度值
+				// time Matrix T
+				Eigen::MatrixXd T(1, 6);
+				getTimeMatrix(t, T);
+
+				// ------------------------ 2.1 collision cost-------------------------
+				cost_colli += cd * dt;  // 碰撞cost = 障碍物距离惩罚c * 速度norm * 时间间隔
+
+				// ------------------ 2.2 gradient of collision cost-------------------
+				for (int k = 0; k < 3; k++) {  // 每一行对应xyz三个轴，一行的各列对应具体轴上对p,v,a的梯度
+					g_colli.row(k) = g_colli.row(k) + (gd * grad(k) * T * Ldp) * dt;
+				}
+
+				// ---------- 3. Deprecated: get velocity and accleration cost --------
+				if (0) {
+					double cv = 0, ca = 0, gv = 0, ga = 0;
+					Eigen::Vector3d acc;
+					getAccelerationFromCoeff(acc, coe, s, t);
+
+					for (int k = 0; k < 3; k++) {
+						getVelocityPenalty(vel(k), cv);
+						cost_vel += cv * dt;
+						getAccelerationPenalty(acc(k), ca);
+						cost_acc += ca * dt;
+					}
+
+					for (int k = 0; k < 3; k++) {
+						getVelocityPenaltyGradient(vel(k), gv);
+						g_vel.row(k) = g_vel.row(k) + (gv * T * V * Ldp) * dt;
+						getAccelerationPenaltyGradient(acc(k), ga);
+						g_acc.row(k) = g_acc.row(k) + (ga * T * V * V * Ldp) * dt;
+					}
+				}
+			}
+		}
+	}
+
+	// ---------------------------- 4. get goal cost ---------------------------------
+	// 4.1 make the trajectry longer
+	Eigen::Vector3d start_pos(df[0], df[num_dp], df[2 * num_dp]);
+	Eigen::Vector3d end_pos(dp[0], dp[num_dp], dp[2 * num_dp]);
+	Eigen::Vector3d delta_pos = end_pos - start_pos;
+	double goal_r             = 100;  // param can be moved to config
+	cost_long                 = exp(-(delta_pos(0) * delta_pos(0) + delta_pos(1) * delta_pos(1)) / goal_r);
+	g_long(0, 0)              = -2 / goal_r * delta_pos(0) * cost_long;
+	g_long(1, 0)              = -2 / goal_r * delta_pos(1) * cost_long;
+
+	// 4.2 make the trajectry approach the goal
+	cost_goal    = (end_pos - this->goal).norm() * (end_pos - this->goal).norm();
+	g_goal(0, 0) = dp[0] - this->goal(0);
+	g_goal(1, 0) = dp[num_dp] - this->goal(1);
+	g_goal(2, 0) = dp[2 * num_dp] - this->goal(2);
+	g_goal       = 2 * g_goal;
+
+	//------------------------ sum up all cost and gradient -----------------------------
+	double ws = this->w_smooth, wc = this->w_collision, wg = this->w_goal, wv = this->w_vel, wa = this->w_acc, wl = this->w_long;
+	cost     = ws * cost_smooth + wc * cost_colli + wv * cost_vel + wa * cost_acc + wg * cost_goal + wl * cost_long + 1e-3;
+	gradient = ws * g_smooth + wc * g_colli + wg * g_goal + wv * g_vel + wa * g_acc + wl * g_long;
+
+	// gradient: 3x3 每一行对应xyz三个轴，一行的各列对应具体轴上对p,v,a的梯度
+	_grad.resize(num_dp * 3);
+	for (int i = 0; i < num_dp; ++i) {
+		_grad[i]              = gradient(0, i);
+		_grad[i + num_dp]     = gradient(1, i);
+		_grad[i + 2 * num_dp] = gradient(2, i);
+	}
+
+	// cout << "smooth cost:" << ws * cost_smooth << " collision cost:" << wc * cost_colli << " goal cost:" << wg * cost_goal << endl;
+	// cout << "smooth grad:" << ws * g_smooth(0) << " collision grad:" << wc * g_colli(0) << " goal grad:" << wg * g_goal(0) << endl;
+}
+
+// get position from coefficient
+void GradTrajOptimizer::getPositionFromCoeff(Eigen::Vector3d &pos, const Eigen::MatrixXd &coeff, const int &index, const double &time) const {
+	int s    = index;
+	double t = time;
+	float x  = coeff(s, 0) + coeff(s, 1) * t + coeff(s, 2) * pow(t, 2) + coeff(s, 3) * pow(t, 3) + coeff(s, 4) * pow(t, 4) + coeff(s, 5) * pow(t, 5);
+	float y = coeff(s, 6) + coeff(s, 7) * t + coeff(s, 8) * pow(t, 2) + coeff(s, 9) * pow(t, 3) + coeff(s, 10) * pow(t, 4) + coeff(s, 11) * pow(t, 5);
+	float z =
+	    coeff(s, 12) + coeff(s, 13) * t + coeff(s, 14) * pow(t, 2) + coeff(s, 15) * pow(t, 3) + coeff(s, 16) * pow(t, 4) + coeff(s, 17) * pow(t, 5);
+
+	pos(0) = x;
+	pos(1) = y;
+	pos(2) = z;
+}
+
+// get velocity from cofficient
+void GradTrajOptimizer::getVelocityFromCoeff(Eigen::Vector3d &vel, const Eigen::MatrixXd &coeff, const int &index, const double &time) const {
+	int s    = index;
+	double t = time;
+	float vx = coeff(s, 1) + 2 * coeff(s, 2) * pow(t, 1) + 3 * coeff(s, 3) * pow(t, 2) + 4 * coeff(s, 4) * pow(t, 3) + 5 * coeff(s, 5) * pow(t, 4);
+	float vy = coeff(s, 7) + 2 * coeff(s, 8) * pow(t, 1) + 3 * coeff(s, 9) * pow(t, 2) + 4 * coeff(s, 10) * pow(t, 3) + 5 * coeff(s, 11) * pow(t, 4);
+	float vz =
+	    coeff(s, 13) + 2 * coeff(s, 14) * pow(t, 1) + 3 * coeff(s, 15) * pow(t, 2) + 4 * coeff(s, 16) * pow(t, 3) + 5 * coeff(s, 17) * pow(t, 4);
+
+	vel(0) = vx;
+	vel(1) = vy;
+	vel(2) = vz;
+}
+
+// get acceleration from coefficient
+void GradTrajOptimizer::getAccelerationFromCoeff(Eigen::Vector3d &acc, const Eigen::MatrixXd &coeff, const int &index, const double &time) const {
+	int s    = index;
+	double t = time;
+	float ax = 2 * coeff(s, 2) + 6 * coeff(s, 3) * pow(t, 1) + 12 * coeff(s, 4) * pow(t, 2) + 20 * coeff(s, 5) * pow(t, 3);
+	float ay = 2 * coeff(s, 8) + 6 * coeff(s, 9) * pow(t, 1) + 12 * coeff(s, 10) * pow(t, 2) + 20 * coeff(s, 11) * pow(t, 3);
+	float az = 2 * coeff(s, 14) + 6 * coeff(s, 15) * pow(t, 1) + 12 * coeff(s, 16) * pow(t, 2) + 20 * coeff(s, 17) * pow(t, 3);
+
+	acc(0) = ax;
+	acc(1) = ay;
+	acc(2) = az;
+}
+
+inline void GradTrajOptimizer::getDistancePenalty(const double &d, double &cost) const { cost = this->alpha * exp(-(d - this->d0) / this->r); }
+
+inline void GradTrajOptimizer::getDistancePenaltyGradient(const double &d, double &grad) const {
+	grad = -(this->alpha / this->r) * exp(-(d - this->d0) / this->r);
+}
+
+inline void GradTrajOptimizer::getVelocityPenalty(const double &v, double &cost) const { cost = alphav * exp((abs(v) - v0) / rv); }
+
+inline void GradTrajOptimizer::getVelocityPenaltyGradient(const double &v, double &grad) const { grad = (alphav / rv) * exp((abs(v) - v0) / rv); }
+
+inline void GradTrajOptimizer::getAccelerationPenalty(const double &a, double &cost) const { cost = alphaa * exp((abs(a) - a0) / ra); }
+
+inline void GradTrajOptimizer::getAccelerationPenaltyGradient(const double &a, double &grad) const { grad = (alphaa / ra) * exp((abs(a) - a0) / ra); }
+
+// get distance in signed distance field ,by position query
+void GradTrajOptimizer::getDistanceAndGradient(Eigen::Vector3d &pos, double &dist, Eigen::Vector3d &grad) const {
+	// get sdf directly from sdf_tools
+	Eigen::Vector3d ori_pos = pos;
+	// 1、限定在地图边界内 2、后面越界的惩罚回来
+	constrains(pos(0), map_boundary_min(0), map_boundary_max(0));
+	constrains(pos(1), map_boundary_min(1), map_boundary_max(1));
+	constrains(pos(2), map_boundary_min(2), map_boundary_max(2));
+	std::vector<double> location_gradient_query = this->sdf->GetGradient(pos(0), pos(1), pos(2), true);
+	grad(0)                                     = location_gradient_query[0];
+	grad(1)                                     = location_gradient_query[1];
+	grad(2)                                     = location_gradient_query[2];
+	std::pair<float, bool> location_sdf_query   = this->sdf->GetSafe(pos(0), pos(1), pos(2));
+	dist                                        = location_sdf_query.first;
+
+	// update distance and gradient using boundary
+	double dtb = getDistanceToBoundary(ori_pos(0), ori_pos(1), ori_pos(2));
+	// 1. 点在边界内时：把点推向内部; 2. 如果在Boundary外: (dtb<0)梯度是指向Boundary的,同样推向内部
+	if (dtb < dist) {
+		dist = dtb;
+		recaluculateGradient(ori_pos(0), ori_pos(1), ori_pos(2), grad);
+	}
+}
+
+double GradTrajOptimizer::getDistanceToBoundary(const double &x, const double &y, const double &z) const {
+	double dist_x = std::min(x - boundary(0), boundary(1) - x);
+	double dist_y = std::min(y - boundary(2), boundary(3) - y);
+	double dist_z = std::min(z - boundary(4), boundary(5) - z);
+	double dtb    = std::min(dist_x, dist_y);
+	dtb           = std::min(dtb, dist_z);
+
+	return dtb;
+}
+
+void GradTrajOptimizer::recaluculateGradient(const double &x, const double &y, const double &z, Eigen::Vector3d &grad) const {
+	double r = this->resolution;
+
+	grad(0) = (10 * (GDTB(x + r, y, z) - GDTB(x - r, y, z)) + 3 * (GDTB(x + r, y + r, z) - GDTB(x - r, y + r, z)) +
+	           3 * (GDTB(x + r, y - r, z) - GDTB(x - r, y - r, z))) /
+	          (32 * r);
+	grad(1) = (10 * (GDTB(x, y + r, z) - GDTB(x, y - r, z)) + 3 * (GDTB(x + r, y + r, z) - GDTB(x + r, y - r, z)) +
+	           3 * (GDTB(x - r, y + r, z) - GDTB(x - r, y - r, z))) /
+	          (32 * r);
+	grad(2) = (10 * (GDTB(x, y, z + r) - GDTB(x, y, z - r)) + 3 * (GDTB(x, y + r, z + r) - GDTB(x, y + r, z - r)) +
+	           3 * (GDTB(x, y - r, z + r) - GDTB(x, y - r, z - r))) /
+	          (32 * r);
+}
+
+void GradTrajOptimizer::getTimeMatrix(const double &t, Eigen::MatrixXd &T) const {
+	T.resize(1, 6);
+	T.setZero();
+
+	for (int i = 0; i < 6; ++i) {
+		T(0, i) = pow(t, i);
+	}
+}
--- a/flightlib/src/grad_traj_optimization/opt_utile.cpp
+++ b/flightlib/src/grad_traj_optimization/opt_utile.cpp
@@ -0,0 +1,145 @@
+#include "flightlib/grad_traj_optimization/opt_utile.h"
+
+/*
+    Front-End Guiding Path:
+    We evenly sample vertical_num * horizon_num * radio_num * vel_num primitives here with different position, length, and velocity direction.
+    But in practical, only vertical_num * horizon_num primitives are sampled (radio_num = vel_num = 1).
+*/
+void getLatticeGuiding(std::vector<std::pair<Eigen::Vector3d, Eigen::Vector3d>> &lattice_nodes, int horizon_num, int vertical_num, int radio_num,
+                       int vel_num, double horizon_fov, double vertical_fov, double radio_range, double vel_fov, double vel_prefile) {
+	double direction_diff, altitude_diff, radio_diff, vel_dir_diff;
+	if (horizon_num == 1)
+		direction_diff = 0;
+	else
+		direction_diff = (horizon_fov / 180.0 * M_PI) / (horizon_num - 1);
+	if (vertical_num == 1)
+		altitude_diff = 0;
+	else
+		altitude_diff = (vertical_fov / 180.0 * M_PI) / (vertical_num - 1);
+	radio_diff = radio_range / radio_num;
+	if (vel_num == 1)
+		vel_dir_diff = 0;
+	else
+		vel_dir_diff = (vel_fov / 180.0f * M_PI) / (vel_num - 1);
+	// if (vel_num == 1)   // be 0 looks like better
+	//     vel_prefile = 0.0;
+	lattice_nodes.clear();
+
+	for (int h = 0; h < radio_num; h++) {
+		for (int i = 0; i < vertical_num; i++) {
+			for (int j = 0; j < horizon_num; j++) {
+				for (int k = 0; k < vel_num; k++) {
+					double search_radio = (h + 1) * radio_diff;
+					double alpha        = -direction_diff * (horizon_num - 1) / 2 + j * direction_diff;  // 位置偏航角（从右往左）
+					double beta         = -altitude_diff * (vertical_num - 1) / 2 + i * altitude_diff;   // 高度偏移角（从下往上）
+					double gamma        = -vel_dir_diff * (vel_num - 1) / 2 + k * vel_dir_diff;          // 速度偏航
+					Eigen::Vector3d lattice_node_pos(cos(beta) * cos(alpha) * search_radio, cos(beta) * sin(alpha) * search_radio,
+					                                 sin(beta) * search_radio);
+					Eigen::Vector3d lattice_node_vel(cos(alpha + gamma) * vel_prefile, sin(alpha + gamma) * vel_prefile, 0.0);
+					std::pair<Eigen::Vector3d, Eigen::Vector3d> lattice_node(lattice_node_pos, lattice_node_vel);
+					lattice_nodes.push_back(lattice_node);
+				}
+			}
+		}
+	}
+}
+
+// TODO: 改为硬编码
+Eigen::MatrixXd solveCoeffFromBoundaryState(const Eigen::Vector3d &Pos_init, const Eigen::Vector3d &Vel_init, const Eigen::Vector3d &Acc_init,
+                                            const Eigen::Vector3d &Pos_end, const Eigen::Vector3d &Vel_end, const Eigen::Vector3d &Acc_end,
+                                            double Time) {
+	Eigen::MatrixXd PolyCoeff(1, 3 * 6);
+	Eigen::VectorXd Px(6), Py(6), Pz(6);
+
+	const static auto Factorial = [](int x) {
+		int fac = 1;
+		for (int i = x; i > 0; i--)
+			fac = fac * i;
+		return fac;
+	};
+
+	/*   Produce Mapping Matrix A to the entire trajectory.   */
+	Eigen::MatrixXd A = Eigen::MatrixXd::Zero(6, 6);
+	for (int i = 0; i < 3; i++) {
+		A(2 * i, i) = Factorial(i);
+		for (int j = i; j < 6; j++)
+			A(2 * i + 1, j) = Factorial(j) / Factorial(j - i) * pow(Time, j - i);
+	}
+
+	/*   Produce the dereivatives in X, Y and Z axis directly.  */
+	Eigen::VectorXd Dx = Eigen::VectorXd::Zero(6);
+	Eigen::VectorXd Dy = Eigen::VectorXd::Zero(6);
+	Eigen::VectorXd Dz = Eigen::VectorXd::Zero(6);
+
+	Dx(0) = Pos_init(0);
+	Dy(0) = Pos_init(1);
+	Dz(0) = Pos_init(2);
+
+	Dx(1) = Pos_end(0);
+	Dy(1) = Pos_end(1);
+	Dz(1) = Pos_end(2);
+
+	Dx(2) = Vel_init(0);
+	Dy(2) = Vel_init(1);
+	Dz(2) = Vel_init(2);
+
+	Dx(3) = Vel_end(0);
+	Dy(3) = Vel_end(1);
+	Dz(3) = Vel_end(2);
+
+	Dx(4) = Acc_init(0);
+	Dy(4) = Acc_init(1);
+	Dz(4) = Acc_init(2);
+
+	Dx(5) = Acc_end(0);
+	Dy(5) = Acc_end(1);
+	Dz(5) = Acc_end(2);
+
+	Px = A.inverse() * Dx;
+	Py = A.inverse() * Dy;
+	Pz = A.inverse() * Dz;
+
+	PolyCoeff.block(0, 0, 1, 6)  = Px.segment(0, 6).transpose();
+	PolyCoeff.block(0, 6, 1, 6)  = Py.segment(0, 6).transpose();
+	PolyCoeff.block(0, 12, 1, 6) = Pz.segment(0, 6).transpose();
+
+	return PolyCoeff;
+}
+
+void getPositionFromCoeff(Eigen::Vector3d &pos, Eigen::MatrixXd coeff, int index, double time) {
+	int s    = index;
+	double t = time;
+	float x  = coeff(s, 0) + coeff(s, 1) * t + coeff(s, 2) * pow(t, 2) + coeff(s, 3) * pow(t, 3) + coeff(s, 4) * pow(t, 4) + coeff(s, 5) * pow(t, 5);
+	float y = coeff(s, 6) + coeff(s, 7) * t + coeff(s, 8) * pow(t, 2) + coeff(s, 9) * pow(t, 3) + coeff(s, 10) * pow(t, 4) + coeff(s, 11) * pow(t, 5);
+	float z =
+	    coeff(s, 12) + coeff(s, 13) * t + coeff(s, 14) * pow(t, 2) + coeff(s, 15) * pow(t, 3) + coeff(s, 16) * pow(t, 4) + coeff(s, 17) * pow(t, 5);
+
+	pos(0) = x;
+	pos(1) = y;
+	pos(2) = z;
+}
+
+void getVelocityFromCoeff(Eigen::Vector3d &vel, Eigen::MatrixXd coeff, int index, double time) {
+	int s    = index;
+	double t = time;
+	float vx = coeff(s, 1) + 2 * coeff(s, 2) * pow(t, 1) + 3 * coeff(s, 3) * pow(t, 2) + 4 * coeff(s, 4) * pow(t, 3) + 5 * coeff(s, 5) * pow(t, 4);
+	float vy = coeff(s, 7) + 2 * coeff(s, 8) * pow(t, 1) + 3 * coeff(s, 9) * pow(t, 2) + 4 * coeff(s, 10) * pow(t, 3) + 5 * coeff(s, 11) * pow(t, 4);
+	float vz =
+	    coeff(s, 13) + 2 * coeff(s, 14) * pow(t, 1) + 3 * coeff(s, 15) * pow(t, 2) + 4 * coeff(s, 16) * pow(t, 3) + 5 * coeff(s, 17) * pow(t, 4);
+
+	vel(0) = vx;
+	vel(1) = vy;
+	vel(2) = vz;
+}
+
+void getAccelerationFromCoeff(Eigen::Vector3d &acc, Eigen::MatrixXd coeff, int index, double time) {
+	int s    = index;
+	double t = time;
+	float ax = 2 * coeff(s, 2) + 6 * coeff(s, 3) * pow(t, 1) + 12 * coeff(s, 4) * pow(t, 2) + 20 * coeff(s, 5) * pow(t, 3);
+	float ay = 2 * coeff(s, 8) + 6 * coeff(s, 9) * pow(t, 1) + 12 * coeff(s, 10) * pow(t, 2) + 20 * coeff(s, 11) * pow(t, 3);
+	float az = 2 * coeff(s, 14) + 6 * coeff(s, 15) * pow(t, 1) + 12 * coeff(s, 16) * pow(t, 2) + 20 * coeff(s, 17) * pow(t, 3);
+
+	acc(0) = ax;
+	acc(1) = ay;
+	acc(2) = az;
+}
--- a/flightlib/src/grad_traj_optimization/qp_generator.cpp
+++ b/flightlib/src/grad_traj_optimization/qp_generator.cpp
@@ -0,0 +1,125 @@
+/*
+    Currently, only 5-order single-segment polynomial is used,
+    but the functionality for piecewise polynomials is retained (i.e. m, num_f, num_p and other variables).
+*/
+
+#include "flightlib/grad_traj_optimization/qp_generator.h"
+
+#include <stdio.h>
+
+#include <fstream>
+#include <iostream>
+#include <string>
+using namespace std;
+using namespace Eigen;
+
+TrajectoryGenerator::TrajectoryGenerator() {}
+
+TrajectoryGenerator::~TrajectoryGenerator() {}
+
+void TrajectoryGenerator::QPGeneration(const Eigen::VectorXd &Time) {
+	m = Time.size();
+
+	// 阶乘
+	const static auto Factorial = [](int x) {
+		int fac = 1;
+
+		for (int i = x; i > 0; i--)
+			fac = fac * i;
+
+		return fac;
+	};
+
+	/*   Produce Mapping Matrix A to the entire trajectory.   */
+	MatrixXd Ab;                                // 每一段矩阵的A（论文中M）
+	MatrixXd A = MatrixXd::Zero(m * 6, m * 6);  // m是段数
+
+	// Ab 的组成为6行，第1行Tk位置，第二行Tk+1位置，第三行Tk速度，第四行Tk+1速度，五六为加速度
+	// 采用5次多项式，所以每段轨迹有6个多项式系数（列）
+	for (int k = 0; k < m; k++) {
+		Ab = Eigen::MatrixXd::Zero(6, 6);
+		for (int i = 0; i < 3; i++) {
+			Ab(2 * i, i) = Factorial(i);
+			for (int j = i; j < 6; j++)
+				Ab(2 * i + 1, j) = Factorial(j) / Factorial(j - i) * pow(Time(k), j - i);
+		}
+		A.block(k * 6, k * 6, 6, 6) = Ab;
+	}
+
+	_A = A;
+
+	/*   Produce the Minimum Snap cost function, the Hessian Matrix   */
+	MatrixXd H = MatrixXd::Zero(m * 6, m * 6);
+	// min snap 的cost function（四阶导数的积分 和 系数的关系 的矩阵），论文中间的矩阵Q
+	for (int k = 0; k < m; k++) {
+		for (int i = 3; i < 6; i++) {
+			for (int j = 3; j < 6; j++) {
+				H(k * 6 + i, k * 6 + j) = i * (i - 1) * (i - 2) * j * (j - 1) * (j - 2) / (i + j - 5) * pow(Time(k), (i + j - 5));
+			}
+		}
+	}
+
+	_Q = H;  // Only minumum snap term is considered here inf the Hessian matrix
+
+	StackOptiDep();
+}
+
+void TrajectoryGenerator::StackOptiDep() {
+	double num_f = 3;      // 4 + 4 : only start and target position has fixed derivatives
+	double num_p = 3 * m;  // All other derivatives are free
+	double num_d = 6 * m;
+
+	MatrixXd Ct;
+	Ct       = MatrixXd::Zero(num_d, num_f + num_p);
+	Ct(0, 0) = 1;
+	Ct(2, 1) = 1;
+	Ct(4, 2) = 1;  // Stack the start point
+
+	Ct(6 * (m - 1) + 1, 3) = 1;  // Stack the end point
+	Ct(6 * (m - 1) + 3, 4) = 1;
+	Ct(6 * (m - 1) + 5, 5) = 1;
+
+	if (m > 1) {
+		Ct(1, 6)                       = 1;
+		Ct(3, 7)                       = 1;
+		Ct(5, 8)                       = 1;
+		Ct(6 * (m - 1) + 0, 3 * m + 0) = 1;
+		Ct(6 * (m - 1) + 2, 3 * m + 1) = 1;
+		Ct(6 * (m - 1) + 4, 3 * m + 2) = 1;
+		for (int j = 2; j < m; j++) {
+			Ct(6 * (j - 1) + 0, 6 + 3 * (j - 2) + 0) = 1;
+			Ct(6 * (j - 1) + 1, 6 + 3 * (j - 1) + 0) = 1;
+			Ct(6 * (j - 1) + 2, 6 + 3 * (j - 2) + 1) = 1;
+			Ct(6 * (j - 1) + 3, 6 + 3 * (j - 1) + 1) = 1;
+			Ct(6 * (j - 1) + 4, 6 + 3 * (j - 2) + 2) = 1;
+			Ct(6 * (j - 1) + 5, 6 + 3 * (j - 1) + 2) = 1;
+		}
+	}
+
+	_C = Ct.transpose();
+	_L = _A.inverse() * Ct;
+
+	MatrixXd B = _A.inverse();
+	_R         = _C * B.transpose() * _Q * (_A.inverse()) * Ct;
+
+	_Rff.resize(3, 3);
+	_Rfp.resize(3, 3 * m);
+	_Rpf.resize(3 * m, 3);
+	_Rpp.resize(3 * m, 3 * m);
+
+	_Rff = _R.block(0, 0, 3, 3);
+	_Rfp = _R.block(0, 3, 3, 3 * m);
+	_Rpf = _R.block(3, 0, 3 * m, 3);
+	_Rpp = _R.block(3, 3, 3 * m, 3 * m);
+}
+
+Eigen::MatrixXd TrajectoryGenerator::getA() { return _A; }
+Eigen::MatrixXd TrajectoryGenerator::getQ() { return _Q; }
+Eigen::MatrixXd TrajectoryGenerator::getC() { return _C; }
+Eigen::MatrixXd TrajectoryGenerator::getL() { return _L; }
+Eigen::MatrixXd TrajectoryGenerator::getR() { return _R; }
+
+Eigen::MatrixXd TrajectoryGenerator::getRff() { return _Rff; }
+Eigen::MatrixXd TrajectoryGenerator::getRpp() { return _Rpp; }
+Eigen::MatrixXd TrajectoryGenerator::getRpf() { return _Rpf; }
+Eigen::MatrixXd TrajectoryGenerator::getRfp() { return _Rfp; }
--- a/flightlib/src/grad_traj_optimization/traj_optimization_bridge.cpp
+++ b/flightlib/src/grad_traj_optimization/traj_optimization_bridge.cpp
@@ -0,0 +1,235 @@
+#include "flightlib/grad_traj_optimization/traj_optimization_bridge.h"
+
+namespace traj_opt {
+
+std::shared_ptr<sdf_tools::SignedDistanceField> SdfConstruction(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, Eigen::Vector3d &map_boundary_min_sdf,
+                                                                Eigen::Vector3d &map_boundary_max_sdf) {
+	pcl::PointXYZ min, max;
+	pcl::getMinMax3D(*cloud, min, max);
+
+	// sdf collision map parameter
+	const double x_size = max.x - min.x;
+	const double z_size = max.z - min.z;
+	const double y_size = max.y - min.y;
+	Eigen::Translation3d origin_translation(min.x, min.y, min.z);
+	Eigen::Quaterniond origin_rotation(1.0, 0.0, 0.0, 0.0);
+	const Eigen::Isometry3d origin_transform = origin_translation * origin_rotation;
+	const std ::string frame                 = "world";
+	map_boundary_min_sdf                     = Eigen::Vector3d(min.x, min.y, min.z);
+	map_boundary_max_sdf                     = Eigen::Vector3d(max.x, max.y, max.z);
+
+	// create map
+	sdf_tools ::COLLISION_CELL cell;
+	cell.occupancy                           = 0.0;
+	cell.component                           = 0;
+	const sdf_tools::COLLISION_CELL oob_cell = cell;
+	double resolution_sdf                    = 0.2;
+	sdf_tools::CollisionMapGrid collision_map(origin_transform, frame, resolution_sdf, x_size, y_size, z_size, oob_cell);
+
+	// add obstacles set in launch file
+	std::cout << "Generate map..." << std::endl;
+	sdf_tools::COLLISION_CELL obstacle_cell(1.0);
+
+	// add the generated obstacles into collision map （flightmare点云直接建图行列偶尔有全空的情况, 但不影响）
+	// 点云分辨率改为0.1地图分辨率0.2，就不存在空行的问题; 地图分辨率0.2时，用pt.x + 0.001, pt.y + 0.001, pt.z + 0.001可避免空行且不会造成地图偏移
+	for (int i = 0; i < cloud->points.size(); i++) {
+		pcl::PointXYZ pt = cloud->points[i];
+		collision_map.Set(pt.x, pt.y, pt.z, obstacle_cell);
+	}
+
+	// Build the signed distance field
+	float oob_value                                                                       = INFINITY;
+	std::pair<sdf_tools::SignedDistanceField, std::pair<double, double>> sdf_with_extrema = collision_map.ExtractSignedDistanceField(oob_value);
+
+	sdf_tools::SignedDistanceField sdf_for_traj_optimization = sdf_with_extrema.first;
+	cout << "Signed Distance Field Build!" << endl;
+	return std::make_shared<sdf_tools::SignedDistanceField>(sdf_for_traj_optimization);
+}
+
+}  // namespace traj_opt
+
+TrajOptimizationBridge::TrajOptimizationBridge() {
+	std::string cfg_path = getenv("FLIGHTMARE_PATH") + std::string("/flightlib/configs/traj_opt.yaml");
+	cfg_                 = YAML::LoadFile(cfg_path);
+	loadParam(cfg_);
+
+	resolution = 0.2;  // Must be the same as the map in SdfConstruction()
+	df_.resize(9);
+	dp_.resize(9);
+
+	getLatticeGuiding(lattice_nodes, horizon_num, vertical_num, radio_num, vel_num, horizon_fov, vertical_fov, radio_range, vel_fov, vel_prefile);
+}
+
+TrajOptimizationBridge::~TrajOptimizationBridge() {}
+
+// Explanation: In grad_traj_optimization of the current project, dp refers to the end_state, and df refers to the initial_state
+void TrajOptimizationBridge::getCostAndGradient(const std::vector<double> &dp, int id, double &cost, std::vector<double> &grad) {
+	// dp: the predicted X_pva, Y_pva, Z_pva in Body Frame
+	if (dp_.size() != dp.size()) {
+		std::cout << "Error: size of dp dose not match !" << std::endl;
+		return;
+	}
+	std::vector<double> dp_b = dp;
+	// Transform to world frame.
+	Eigen::Vector3d Pb, Vb, Ab, Pw, Vw, Aw;
+	for (int i = 0; i < 3; i++) {
+		Pb(i) = dp_b[3 * i];
+		Vb(i) = dp_b[3 * i + 1];
+		Ab(i) = dp_b[3 * i + 2];
+	}
+	Eigen::Matrix3d Rwb = q_wb_.toRotationMatrix();
+	Pw                  = Rwb * Pb + pos_;
+	Vw                  = Rwb * Vb;
+	Aw                  = Rwb * Ab;
+	for (int i = 0; i < 3; i++) {
+		dp_[3 * i]     = Pw(i);
+		dp_[3 * i + 1] = Vw(i);
+		dp_[3 * i + 2] = Aw(i);
+	}
+
+	// ----------------------------main optimization procedure--------------------------
+	GradTrajOptimizer grad_traj_opt(cfg_);
+	grad_traj_opt.setSignedDistanceField(sdf_, resolution);
+	grad_traj_opt.setBoundary(map_boundary_min_, map_boundary_max_);
+	grad_traj_opt.setGoal(goal_);
+
+	double cost_;
+	std::vector<double> grad_w, grad_b;
+	grad_traj_opt.getCostAndGradient(df_, dp_, cost_, grad_w);
+
+	Eigen::Vector3d grad_pb, grad_vb, grad_ab, grad_pw, grad_vw, grad_aw;
+	for (int i = 0; i < 3; i++) {
+		grad_pw(i) = grad_w[3 * i];
+		grad_vw(i) = grad_w[3 * i + 1];
+		grad_aw(i) = grad_w[3 * i + 2];
+	}
+	grad_pb = Rwb.transpose() * grad_pw;
+	grad_vb = Rwb.transpose() * grad_vw;
+	grad_ab = Rwb.transpose() * grad_aw;
+	grad_b.resize(grad_w.size());
+	for (int i = 0; i < 3; i++) {
+		grad_b[3 * i]     = grad_pb(i);
+		grad_b[3 * i + 1] = grad_vb(i);
+		grad_b[3 * i + 2] = grad_ab(i);
+	}
+
+	cost = cost_;
+	grad = grad_b;  // x_pva, y_pva, z_pva
+}
+
+void TrajOptimizationBridge::setMap(std::shared_ptr<sdf_tools::SignedDistanceField> sdf_for_traj_optimization, Eigen::Vector3d &map_boundary_min,
+                                    Eigen::Vector3d &map_boundary_max) {
+	map_boundary_min_ = map_boundary_min;
+	map_boundary_max_ = map_boundary_max;
+	sdf_              = sdf_for_traj_optimization;
+}
+
+void TrajOptimizationBridge::setState(Eigen::Vector3d pos, Eigen::Quaterniond q, Eigen::Vector3d vel, Eigen::Vector3d acc) {
+	pos_  = pos;
+	q_wb_ = q;
+	vel_  = vel;
+	acc_  = acc;
+	for (int i = 0; i < 3; i++) {
+		df_[3 * i]     = pos(i);
+		df_[3 * i + 1] = vel(i);
+		df_[3 * i + 2] = acc(i);
+	}
+}
+
+// Explanation: In grad_traj_optimization of the current project, dp refers to the end_state, and df refers to the initial_state
+void TrajOptimizationBridge::getNextStateAndCost(const std::vector<double> &dp, double &cost, Eigen::Vector3d &pos, Eigen::Vector3d &vel,
+                                                 Eigen::Vector3d &acc, double sim_t) {
+	// dp: xyz_pva (in Body Frame)
+	if (dp_.size() != dp.size()) {
+		std::cout << "Error: size of dp dose not match !" << std::endl;
+		return;
+	}
+	std::vector<double> dp_b = dp;
+	Eigen::Vector3d Pb, Vb, Ab, Pw, Vw, Aw;
+	for (int i = 0; i < 3; i++) {
+		Pb(i) = dp_b[3 * i];
+		Vb(i) = dp_b[3 * i + 1];
+		Ab(i) = dp_b[3 * i + 2];
+	}
+	Eigen::Matrix3d Rwb = q_wb_.toRotationMatrix();
+	Pw                  = Rwb * Pb + pos_;
+	Vw                  = Rwb * Vb;
+	Aw                  = Rwb * Ab;
+	for (int i = 0; i < 3; i++) {
+		dp_[3 * i]     = Pw(i);
+		dp_[3 * i + 1] = Vw(i);
+		dp_[3 * i + 2] = Aw(i);
+	}
+
+	GradTrajOptimizer grad_traj_opt(cfg_);
+	grad_traj_opt.setSignedDistanceField(sdf_, resolution);
+	grad_traj_opt.setBoundary(map_boundary_min_, map_boundary_max_);
+	grad_traj_opt.setGoal(goal_);
+
+	double cost_;
+	std::vector<double> grad_w;
+	grad_traj_opt.getCostAndGradient(df_, dp_, cost_, grad_w);     // Df is set here
+	grad_traj_opt.getCoefficientFromDerivative(pred_coeff_, dp_);  // get coefficient by Dp and Df
+
+	cost = cost_;
+	getPositionFromCoeff(pos, pred_coeff_, 0, sim_t);
+	getVelocityFromCoeff(vel, pred_coeff_, 0, sim_t);
+	getAccelerationFromCoeff(acc, pred_coeff_, 0, sim_t);
+}
+
+/**
+    set dp and get the coeffs for getNextState() function.
+    dp is in the world frame because this func only used in real flight and
+    the prediction must be tramsformed to world frame in python to avoid the attitude inconsistency caused by latency
+*/
+void TrajOptimizationBridge::solveBVP(const std::vector<double> &dp) {
+	// dp: xyz_pva given by python（in World Frame, 除了位置没加机身的偏移）
+	if (dp_.size() != dp.size()) {
+		std::cout << "Error: size of dp dose not match !" << std::endl;
+		return;
+	}
+
+	std::vector<double> dp_w = dp;
+	Eigen::Vector3d Pb, Vb, Ab, Pw, Vw, Aw;
+	for (int i = 0; i < 3; i++) {
+		Pw(i) = dp_w[3 * i];
+		Vw(i) = dp_w[3 * i + 1];
+		Aw(i) = dp_w[3 * i + 2];
+	}
+
+	Pw = Pw + pos_;
+	// Eigen::Matrix3d Rwb = q_wb_.toRotationMatrix();
+	// Pw = Rwb * Pb + pos_;
+	// Vw = Rwb * Vb;
+	// Aw = Rwb * Ab;
+
+	double traj_time = 2 * cfg_["radio_range"].as<double>() / cfg_["vel_max"].as<double>();
+	pred_coeff_      = solveCoeffFromBoundaryState(pos_, vel_, acc_, Pw, Vw, Aw, traj_time);
+}
+
+// get the next state in world frame
+void TrajOptimizationBridge::getNextState(Eigen::Vector3d &pos, Eigen::Vector3d &vel, Eigen::Vector3d &acc, double sim_t) {
+	getPositionFromCoeff(pos, pred_coeff_, 0, sim_t);
+	getVelocityFromCoeff(vel, pred_coeff_, 0, sim_t);
+	getAccelerationFromCoeff(acc, pred_coeff_, 0, sim_t);
+}
+
+void TrajOptimizationBridge::setGoal(Eigen::Vector3d goal) {
+	Eigen::Vector3d goal_dir  = (goal - pos_) / (goal - pos_).norm();
+	Eigen::Vector3d temp_goal = goal;
+	temp_goal                 = pos_ + goal_length * goal_dir;
+	goal_                     = temp_goal;
+}
+
+void TrajOptimizationBridge::loadParam(YAML::Node &cfg) {
+	horizon_num  = cfg["horizon_num"].as<int>();
+	vertical_num = cfg["vertical_num"].as<int>();
+	vel_num      = cfg["vel_num"].as<int>();
+	radio_num    = cfg["radio_num"].as<int>();
+	horizon_fov  = cfg["horizon_camera_fov"].as<double>() * (horizon_num - 1) / horizon_num;
+	vertical_fov = cfg["vertical_camera_fov"].as<double>() * (vertical_num - 1) / vertical_num;
+	vel_fov      = cfg["vel_fov"].as<double>();
+	radio_range  = cfg["radio_range"].as<double>();
+	vel_prefile  = cfg["vel_prefile"].as<double>();
+	goal_length  = cfg["goal_length"].as<double>();
+}
--- a/flightlib/src/objects/object_base.cpp
+++ b/flightlib/src/objects/object_base.cpp
@@ -0,0 +1,9 @@
+#include "flightlib/objects/object_base.hpp"
+
+namespace flightlib {
+
+ObjectBase::ObjectBase() {}
+
+ObjectBase::~ObjectBase() {}
+
+}  // namespace flightlib
--- a/flightlib/src/objects/quadrotor.cpp
+++ b/flightlib/src/objects/quadrotor.cpp
@@ -0,0 +1,308 @@
+#include "flightlib/objects/quadrotor.hpp"
+
+namespace flightlib {
+
+Quadrotor::Quadrotor(const std::string &cfg_path)
+    : world_box_((Matrix<3, 2>() << -100, 100, -100, 100, -100, 100).finished()), size_(1.0, 1.0, 1.0), collision_(false) {
+	//
+	YAML::Node cfg = YAML::LoadFile(cfg_path);
+
+	// create quadrotor dynamics and update the parameters
+	dynamics_.updateParams(cfg);
+	init();
+}
+
+Quadrotor::Quadrotor(const QuadrotorDynamics &dynamics)
+    : world_box_((Matrix<3, 2>() << -100, 100, -100, 100, -100, 100).finished()), dynamics_(dynamics), size_(1.0, 1.0, 1.0), collision_(false) {
+	init();
+}
+
+Quadrotor::~Quadrotor() {}
+
+bool Quadrotor::setState(const Ref<Vector<>> p_, const Ref<Vector<>> v_, const Quaternion q_, const Ref<Vector<>> a_, const Scalar ctl_dt) {
+	QuadState old_state = state_;
+	state_.p            = p_;
+	state_.v            = v_;
+	state_.q(q_);
+	state_.a = a_;
+	state_.t += ctl_dt;
+	constrainInWorldBox(old_state);
+	return true;
+}
+
+bool Quadrotor::run(const Command &cmd, const Scalar ctl_dt) {
+	if (!setCommand(cmd))
+		return false;  // 限幅
+	return run(ctl_dt);
+}
+
+bool Quadrotor::run(const Scalar ctl_dt) {
+	if (!state_.valid())
+		return false;
+	if (!cmd_.valid())
+		return false;
+
+	QuadState old_state  = state_;
+	QuadState next_state = state_;
+
+	// time
+	const Scalar max_dt  = integrator_ptr_->dtMax();
+	Scalar remain_ctl_dt = ctl_dt;
+
+	// simulation loop
+	while (remain_ctl_dt > 0.0) {
+		const Scalar sim_dt = std::min(remain_ctl_dt, max_dt);
+
+		const Vector<4> motor_thrusts_des = cmd_.isSingleRotorThrusts() ? cmd_.thrusts : runFlightCtl(sim_dt, state_.w, cmd_);
+
+		runMotors(sim_dt, motor_thrusts_des);
+		// motor_thrusts_ = cmd_.thrusts;
+
+		const Vector<4> force_torques = B_allocation_ * motor_thrusts_;
+
+		// Compute linear acceleration and body torque
+		const Vector<3> force(0.0, 0.0, force_torques[0]);
+		state_.a = state_.q() * force * 1.0 / dynamics_.getMass() + gz_;
+
+		// compute body torque
+		state_.tau = force_torques.segment<3>(1);
+
+		// dynamics integration
+		integrator_ptr_->step(state_.x, sim_dt, next_state.x);
+
+		// update state and sim time
+		state_.qx /= state_.qx.norm();
+
+		//
+		state_.x = next_state.x;
+		remain_ctl_dt -= sim_dt;
+	}
+	state_.t += ctl_dt;
+	//
+	constrainInWorldBox(old_state);
+	return true;
+}
+
+void Quadrotor::init(void) {
+	// reset
+	updateDynamics(dynamics_);
+	reset();
+}
+
+bool Quadrotor::reset(void) {
+	state_.setZero();
+	motor_omega_.setZero();
+	motor_thrusts_.setZero();
+	return true;
+}
+
+bool Quadrotor::reset(const QuadState &state) {
+	if (!state.valid())
+		return false;
+	state_ = state;
+	motor_omega_.setZero();
+	motor_thrusts_.setZero();
+	return true;
+}
+
+/*
+    There is no controller (or using an ideal controller). The attitude is simply obtained from the desired acceleration.
+    This is because our algorithm is only concerned with the quality of the trajectory, while control is performed by external controller.
+*/
+void Quadrotor::runSimpleFlight(const Eigen::Vector3f &ref_acc, float ref_yaw, Eigen::Quaternionf &quat_des) {
+	float mass_ = 1.0;
+	float ONE_G = 9.8;
+	// float M_PI             = 3.1415925;
+	Eigen::Vector3f force_ = mass_ * ONE_G * Eigen::Vector3f(0, 0, 1);
+	force_.noalias() += mass_ * ref_acc;
+
+	// Limit control angle to theta degree
+	float theta = M_PI / 4;
+	float c     = cos(theta);
+	Eigen::Vector3f f;
+	f.noalias() = force_ - mass_ * ONE_G * Eigen::Vector3f(0, 0, 1);
+	if (Eigen::Vector3f(0, 0, 1).dot(force_ / force_.norm()) < c) {
+		float nf         = f.norm();
+		float A          = c * c * nf * nf - f(2) * f(2);
+		float B          = 2 * (c * c - 1) * f(2) * mass_ * ONE_G;
+		float C          = (c * c - 1) * mass_ * mass_ * ONE_G * ONE_G;
+		float s          = (-B + sqrt(B * B - 4 * A * C)) / (2 * A);
+		force_.noalias() = s * f + mass_ * ONE_G * Eigen::Vector3f(0, 0, 1);
+	}
+
+	Eigen::Vector3f b1c, b2c, b3c;
+	Eigen::Vector3f b1d(cos(ref_yaw), sin(ref_yaw), 0);
+
+	if (force_.norm() > 1e-6)
+		b3c.noalias() = force_.normalized();
+	else
+		b3c.noalias() = Eigen::Vector3f(0, 0, 1);
+
+	b2c.noalias() = b3c.cross(b1d).normalized();
+	b1c.noalias() = b2c.cross(b3c).normalized();
+
+	Eigen::Matrix3f R;
+	R << b1c, b2c, b3c;
+
+	quat_des = Eigen::Quaternionf(R);
+}
+
+Vector<4> Quadrotor::runFlightCtl(const Scalar sim_dt, const Vector<3> &omega, const Command &command) {
+	const Scalar force = dynamics_.getMass() * command.collective_thrust;
+
+	const Vector<3> omega_err = command.omega - omega;
+
+	const Vector<3> body_torque_des = dynamics_.getJ() * Kinv_ang_vel_tau_ * omega_err + state_.w.cross(dynamics_.getJ() * state_.w);
+
+	const Vector<4> thrust_and_torque(force, body_torque_des.x(), body_torque_des.y(), body_torque_des.z());
+
+	const Vector<4> motor_thrusts_des = B_allocation_inv_ * thrust_and_torque;
+
+	return dynamics_.clampThrust(motor_thrusts_des);
+}
+
+void Quadrotor::runMotors(const Scalar sim_dt, const Vector<4> &motor_thruts_des) {
+	const Vector<4> motor_omega_des     = dynamics_.motorThrustToOmega(motor_thruts_des);
+	const Vector<4> motor_omega_clamped = dynamics_.clampMotorOmega(motor_omega_des);
+
+	// simulate motors as a first-order system
+	const Scalar c = std::exp(-sim_dt * dynamics_.getMotorTauInv());
+	motor_omega_   = c * motor_omega_ + (1.0 - c) * motor_omega_clamped;
+
+	motor_thrusts_ = dynamics_.motorOmegaToThrust(motor_omega_);
+	motor_thrusts_ = dynamics_.clampThrust(motor_thrusts_);
+}
+
+bool Quadrotor::setCommand(const Command &cmd) {
+	if (!cmd.valid())
+		return false;
+	cmd_ = cmd;
+
+	// 推力角速率
+	if (std::isfinite(cmd_.collective_thrust))
+		cmd_.collective_thrust = dynamics_.clampCollectiveThrust(cmd_.collective_thrust);
+
+	if (cmd_.omega.allFinite())
+		cmd_.omega = dynamics_.clampBodyrates(cmd_.omega);
+
+	// 转子推力
+	if (cmd_.thrusts.allFinite())
+		cmd_.thrusts = dynamics_.clampThrust(cmd_.thrusts);
+
+	return true;
+}
+
+bool Quadrotor::setState(const QuadState &state) {
+	if (!state.valid())
+		return false;
+	state_ = state;
+	return true;
+}
+
+bool Quadrotor::setWorldBox(const Ref<Matrix<3, 2>> box) {
+	if (box(0, 0) >= box(0, 1) || box(1, 0) >= box(1, 1) || box(2, 0) >= box(2, 1)) {
+		return false;
+	}
+	world_box_ = box;
+	return true;
+}
+
+
+bool Quadrotor::constrainInWorldBox(const QuadState &old_state) {
+	if (!old_state.valid())
+		return false;
+
+	// violate world box constraint in the x-axis
+	if (state_.x(QS::POSX) < world_box_(0, 0) || state_.x(QS::POSX) > world_box_(0, 1)) {
+		state_.x(QS::POSX) = old_state.x(QS::POSX);
+		state_.x(QS::VELX) = 0.0;
+	}
+
+	// violate world box constraint in the y-axis
+	if (state_.x(QS::POSY) < world_box_(1, 0) || state_.x(QS::POSY) > world_box_(1, 1)) {
+		state_.x(QS::POSY) = old_state.x(QS::POSY);
+		state_.x(QS::VELY) = 0.0;
+	}
+
+	// violate world box constraint in the x-axis
+	if (state_.x(QS::POSZ) <= world_box_(2, 0) || state_.x(QS::POSZ) > world_box_(2, 1)) {
+		//
+		state_.x(QS::POSZ) = world_box_(2, 0);
+
+		// reset velocity to zero
+		state_.x(QS::VELX) = 0.0;
+		state_.x(QS::VELY) = 0.0;
+
+		// reset acceleration to zero
+		state_.a << 0.0, 0.0, 0.0;
+		// reset angular velocity to zero
+		state_.w << 0.0, 0.0, 0.0;
+	}
+	return true;
+}
+
+bool Quadrotor::getState(QuadState *const state) const {
+	if (!state_.valid())
+		return false;
+
+	*state = state_;
+	return true;
+}
+
+bool Quadrotor::getMotorThrusts(Ref<Vector<4>> motor_thrusts) const {
+	motor_thrusts = motor_thrusts_;
+	return true;
+}
+
+bool Quadrotor::getMotorOmega(Ref<Vector<4>> motor_omega) const {
+	motor_omega = motor_omega_;
+	return true;
+}
+
+bool Quadrotor::getDynamics(QuadrotorDynamics *const dynamics) const {
+	if (!dynamics_.valid())
+		return false;
+	*dynamics = dynamics_;
+	return true;
+}
+
+const QuadrotorDynamics &Quadrotor::getDynamics() { return dynamics_; }
+
+bool Quadrotor::updateDynamics(const QuadrotorDynamics &dynamics) {
+	if (!dynamics.valid()) {
+		std::cout << "[Quadrotor] dynamics is not valid!" << std::endl;
+		return false;
+	}
+	dynamics_       = dynamics;
+	integrator_ptr_ = std::make_unique<IntegratorRK4>(dynamics_.getDynamicsFunction(), 2.5e-3);
+
+	B_allocation_     = dynamics_.getAllocationMatrix();
+	B_allocation_inv_ = B_allocation_.inverse();
+	return true;
+}
+
+bool Quadrotor::addRGBCamera(std::shared_ptr<RGBCamera> camera) {
+	rgb_cameras_.push_back(camera);
+	return true;
+}
+
+Vector<3> Quadrotor::getSize(void) const { return size_; }
+
+Vector<3> Quadrotor::getPosition(void) const { return state_.p; }
+
+std::vector<std::shared_ptr<RGBCamera>> Quadrotor::getCameras(void) const { return rgb_cameras_; };
+
+bool Quadrotor::getCamera(const size_t cam_id, std::shared_ptr<RGBCamera> camera) const {
+	if (cam_id <= rgb_cameras_.size()) {
+		return false;
+	}
+
+	camera = rgb_cameras_[cam_id];
+	return true;
+}
+
+bool Quadrotor::getCollision() const { return collision_; }
+
+int Quadrotor::getNumCamera() const { return rgb_cameras_.size(); }
+
+}  // namespace flightlib
--- a/flightlib/src/objects/unity_camera.cpp
+++ b/flightlib/src/objects/unity_camera.cpp
@@ -0,0 +1,9 @@
+#include "flightlib/objects/unity_camera.hpp"
+
+namespace flightlib {
+
+UnityCamera::UnityCamera() {}
+
+UnityCamera::~UnityCamera() {}
+
+}  // namespace flightlib
--- a/flightlib/src/ros_nodes/flight_pilot.cpp
+++ b/flightlib/src/ros_nodes/flight_pilot.cpp
@@ -0,0 +1,293 @@
+#include "flightlib/ros_nodes/flight_pilot.hpp"
+
+namespace flightros {
+
+FlightPilot::FlightPilot(const ros::NodeHandle& nh, const ros::NodeHandle& pnh)
+    : nh_(nh), pnh_(pnh), scene_id_(4), unity_ready_(false), unity_render_(false), receive_id_(0), frameID(0), main_loop_freq_(50.0) {
+	// quad initialization
+	quad_ptr_ = std::make_shared<Quadrotor>();
+	// load parameters
+	std::string cfg_path = getenv("FLIGHTMARE_PATH") + std::string("/flightlib/configs/quadrotor_ros.yaml");
+	YAML::Node cfg_      = YAML::LoadFile(cfg_path);
+	loadParams(cfg_);
+	configCamera(cfg_);
+	quad_ptr_->setSize(quad_size_);
+
+	// initialization
+	quad_state_.setZero();
+	if (scene_id_ == UnityScene::NATUREFOREST) {
+		quad_state_.x(QS::POSX) = 51;   // 41-61
+		quad_state_.x(QS::POSY) = 108;  // 98-118
+		quad_state_.x(QS::POSZ) = 34;
+	}
+	quad_ptr_->reset(quad_state_);
+
+	sgm_.reset(new sgm_gpu::SgmGpu(width_, height_));
+
+	// initialize subscriber and publisher
+	left_img_pub = nh_.advertise<sensor_msgs::Image>("RGB_image", 1);
+	stereo_pub   = nh_.advertise<sensor_msgs::Image>("stereo_image", 1);
+	depth_pub    = nh_.advertise<sensor_msgs::Image>("depth_image", 1);
+	cam_info_pub = nh_.advertise<sensor_msgs::CameraInfo>("camera_info", 1);
+
+	state_est_sub_  = nh_.subscribe(odom_topic_, 1, &FlightPilot::poseCallback, this);
+	spawn_tree_sub_ = nh_.subscribe("/spawn_tree", 1, &FlightPilot::spawnTreeCallback, this);
+	clear_tree_sub_ = nh_.subscribe("/clear_tree", 1, &FlightPilot::clearTreeCallback, this);
+	save_pc_sub_    = nh_.subscribe("/save_pc", 1, &FlightPilot::savePointcloudCallback, this);
+	timestamp       = ros::Time::now();
+
+	// connect unity and setup unity
+	setUnity(unity_render_);
+	connectUnity();
+	if (!unity_ready_)
+		ROS_ERROR("Start the gazebo and unity first!");
+	spawnTreesAndSavePointCloud();
+
+	timer_main_loop_ = nh_.createTimer(ros::Rate(main_loop_freq_), &FlightPilot::mainLoopCallback, this);
+}
+
+FlightPilot::~FlightPilot() { disconnectUnity(); }
+
+void FlightPilot::spawnTreeCallback(const std_msgs::Empty::ConstPtr& msg) {
+	if (!unity_ready_ || unity_bridge_ptr_ == nullptr)
+		return;
+	unity_bridge_ptr_->spawnTrees(bounding_box_, bounding_box_origin_, avg_tree_spacing_);
+}
+
+void FlightPilot::clearTreeCallback(const std_msgs::Empty::ConstPtr& msg) { unity_bridge_ptr_->rmTrees(); }
+
+// If the point cloud is not saved when init, it also can be saved by this callback.
+void FlightPilot::savePointcloudCallback(const std_msgs::Empty::ConstPtr& msg) {
+	Vector<3> min_corner = bounding_box_origin_ - 0.5 * bounding_box_;
+	Vector<3> max_corner = bounding_box_origin_ + 0.5 * bounding_box_;
+	unity_bridge_ptr_->generatePointcloud(min_corner, max_corner, ply_id_, ply_path_, scene_id_, pointcloud_resolution_);
+}
+
+void FlightPilot::poseCallback(const nav_msgs::Odometry::ConstPtr& msg) {
+	quad_state_.x[QS::POSX] = (Scalar)msg->pose.pose.position.x;
+	quad_state_.x[QS::POSY] = (Scalar)msg->pose.pose.position.y;
+	quad_state_.x[QS::POSZ] = (Scalar)msg->pose.pose.position.z;
+	quad_state_.x[QS::ATTW] = (Scalar)msg->pose.pose.orientation.w;
+	quad_state_.x[QS::ATTX] = (Scalar)msg->pose.pose.orientation.x;
+	quad_state_.x[QS::ATTY] = (Scalar)msg->pose.pose.orientation.y;
+	quad_state_.x[QS::ATTZ] = (Scalar)msg->pose.pose.orientation.z;
+
+	quad_ptr_->setState(quad_state_);
+	timestamp = msg->header.stamp;
+}
+
+void FlightPilot::mainLoopCallback(const ros::TimerEvent& event) {
+	if (!unity_render_ || !unity_ready_)
+		return;
+
+	frameID++;
+	ros::Time timestamp_ = timestamp;
+	unity_bridge_ptr_->getRender(frameID);  // 1ms
+
+	FrameID frameID_rt;
+	unity_bridge_ptr_->handleOutput(frameID_rt);  // 30ms
+	while (frameID != frameID_rt)
+		unity_bridge_ptr_->handleOutput(frameID_rt);
+
+	cv::Mat left_img, right_img, depth_img;
+	// publish RGB image
+	rgb_camera_left->getRGBImage(left_img);
+	sensor_msgs::ImagePtr img_msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", left_img).toImageMsg();
+	img_msg->header.stamp         = timestamp_;
+	left_img_pub.publish(img_msg);
+
+	// publish camera info
+	int width                 = rgb_camera_left->getWidth();
+	int hight                 = rgb_camera_left->getHeight();
+	float fov                 = rgb_camera_left->getFOV();
+	float cx                  = width / 2.0;
+	float cy                  = hight / 2.0;
+	float fx                  = cy / tan(0.5 * fov * M_PI / 180.0);  // 他这个特殊，fov是竖直方向的
+	float fy                  = fx;
+	boost::array<double, 9> K = {fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0};
+	sensor_msgs::CameraInfo cam_msg;
+	cam_msg.height = hight;
+	cam_msg.width  = width;
+	cam_msg.K      = K;
+	cam_info_pub.publish(cam_msg);
+
+	// publish depth image
+	if (use_depth) {
+		rgb_camera_left->getDepthMap(depth_img);
+		depth_img                       = depth_img * 1000.0;
+		depth_img                       = cv::min(depth_img, 20.0);
+		sensor_msgs::ImagePtr depth_msg = cv_bridge::CvImage(std_msgs::Header(), "32FC1", depth_img).toImageMsg();
+		depth_msg->header.stamp         = timestamp_;
+		depth_pub.publish(depth_msg);
+	}
+
+	// publish stereo image
+	if (use_stereo) {
+		rgb_camera_right->getRGBImage(right_img);
+		cv::Mat stereo_(height_, width_, CV_32FC1);
+		computeDepthImage(left_img, right_img, &stereo_);
+		if (use_depth) {
+			// Complete the NaN values in the depth map, as the RealSense performs better than SGM.
+			cv::Mat mask, mask1, mask2;
+			cv::compare(stereo_, 0, mask1, cv::CMP_EQ);   // 将 A 中为 0 的位置置为 255，其余位置置为 0
+			cv::compare(stereo_, 20, mask2, cv::CMP_GT);  // 将 A 中大于 20 的位置置为 255，其余位置置为 0
+			mask = mask1 | mask2;                         // 将两个掩码进行逻辑或操作
+			depth_img.copyTo(stereo_, mask);              // 将 B 中 mask 为 255 的位置的值复制到 A 中
+		}
+
+		sensor_msgs::ImagePtr stereo_msg = cv_bridge::CvImage(std_msgs::Header(), "32FC1", stereo_).toImageMsg();
+		stereo_msg->header.stamp         = timestamp_;
+		stereo_pub.publish(stereo_msg);
+	}
+}
+
+bool FlightPilot::setUnity(const bool render) {
+	unity_render_ = render;
+	if (unity_render_ && unity_bridge_ptr_ == nullptr) {
+		// create unity bridge
+		unity_bridge_ptr_ = UnityBridge::getInstance();
+		unity_bridge_ptr_->addQuadrotor(quad_ptr_);
+		ROS_INFO("[%s] Unity Bridge is created.", pnh_.getNamespace().c_str());
+	}
+	return true;
+}
+
+bool FlightPilot::spawnTreesAndSavePointCloud() {
+	if (!unity_ready_ || unity_bridge_ptr_ == nullptr)
+		return false;
+
+	unity_bridge_ptr_->spawnTrees(bounding_box_, bounding_box_origin_, avg_tree_spacing_);
+
+	// Saving point cloud during the testing is much time-consuming, but can be used for evaluation.
+	// The following can be commented if evaluation is not needed.
+	Vector<3> min_corner = bounding_box_origin_ - 0.5 * bounding_box_;
+	Vector<3> max_corner = bounding_box_origin_ + 0.5 * bounding_box_;
+	unity_bridge_ptr_->generatePointcloud(min_corner, max_corner, ply_id_, ply_path_, scene_id_, pointcloud_resolution_);
+	return true;
+}
+
+bool FlightPilot::connectUnity() {
+	if (!unity_render_ || unity_bridge_ptr_ == nullptr)
+		return false;
+	unity_ready_ = unity_bridge_ptr_->connectUnity(scene_id_);
+	return unity_ready_;
+}
+
+bool FlightPilot::disconnectUnity() {
+	if (unity_render_ && unity_bridge_ptr_ != nullptr)
+		;
+	unity_bridge_ptr_->disconnectUnity();
+	unity_ready_ = false;
+}
+
+bool FlightPilot::loadParams(const YAML::Node& cfg) {
+	// ros
+	main_loop_freq_ = cfg["main_loop_freq"].as<int>();
+	odom_topic_     = cfg["odom_topic"].as<std::string>();
+	// camera
+	width_     = cfg["rgb_camera_left"]["width"].as<int>();
+	height_    = cfg["rgb_camera_left"]["height"].as<int>();
+	fov_       = cfg["rgb_camera_left"]["fov"].as<Scalar>();
+	use_depth  = cfg["rgb_camera_left"]["enable_depth"].as<bool>();
+	use_stereo = cfg["rgb_camera_right"]["on"].as<bool>();
+	// scence
+	scene_id_          = cfg["scene_id"].as<int>();
+	unity_render_      = cfg["unity_render"].as<bool>();
+	Scalar quad_size_i = cfg["quad_size"].as<Scalar>();
+	quad_size_         = Vector<3>(quad_size_i, quad_size_i, quad_size_i);
+	avg_tree_spacing_  = cfg["unity"]["avg_tree_spacing"].as<Scalar>();
+	for (int i = 0; i < 3; ++i) {
+		bounding_box_(i)        = cfg["unity"]["bounding_box"][i].as<Scalar>();
+		bounding_box_origin_(i) = cfg["unity"]["bounding_box_origin"][i].as<Scalar>();
+	}
+	pointcloud_resolution_ = cfg["unity"]["pointcloud_resolution"].as<Scalar>();
+	ply_path_              = getenv("FLIGHTMARE_PATH") + cfg["ply_path"].as<std::string>();
+	if (!std::filesystem::exists(ply_path_)) {
+		std::filesystem::create_directories(ply_path_);
+		std::cout << "Directory created: " << ply_path_ << std::endl;
+	}
+	return true;
+}
+
+void FlightPilot::computeDepthImage(const cv::Mat& left_frame, const cv::Mat& right_frame, cv::Mat* const depth) {
+	cv::Mat disparity(height_, width_, CV_8UC1);
+	sgm_->computeDisparity(left_frame, right_frame, disparity);
+	disparity.convertTo(disparity, CV_32FC1);
+
+	// compute depth from disparity
+	float f = (width_ / 2.0) / std::tan((M_PI * (fov_ / 180.0)) / 2.0);
+	//  depth = stereo_baseline_ * f / disparity
+	for (int r = 0; r < height_; ++r) {
+		for (int c = 0; c < width_; ++c) {
+			if (disparity.at<float>(r, c) == 0.0f) {
+				depth->at<float>(r, c) = 0.0f;
+			} else if (disparity.at<float>(r, c) == 255.0f) {
+				depth->at<float>(r, c) = 0.0f;
+			} else {
+				depth->at<float>(r, c) = static_cast<float>(stereo_baseline_) * f / disparity.at<float>(r, c);
+			}
+		}
+	}
+}
+
+bool FlightPilot::configCamera(const YAML::Node& cfg) {
+	if (!cfg["rgb_camera_left"] || !cfg["rgb_camera_right"]) {
+		ROS_ERROR("Cannot config stereo Camera");
+		return false;
+	}
+	// create left camera --------------------------------------------
+	rgb_camera_left = std::make_shared<RGBCamera>();
+
+	// load camera settings
+	std::vector<Scalar> t_BC_vec = cfg["rgb_camera_left"]["t_BC"].as<std::vector<Scalar>>();
+	std::vector<Scalar> r_BC_vec = cfg["rgb_camera_left"]["r_BC"].as<std::vector<Scalar>>();
+	Vector<3> t_BC(t_BC_vec.data());
+	Matrix<3, 3> r_BC = (AngleAxis(r_BC_vec[0] * M_PI / 180.0, Vector<3>::UnitX()) * AngleAxis(r_BC_vec[1] * M_PI / 180.0, Vector<3>::UnitY()) *
+	                     AngleAxis(r_BC_vec[2] * M_PI / 180.0, Vector<3>::UnitZ()))
+	                        .toRotationMatrix();  // the rotation order has been verified
+	// Convert the horizontal FOV (usually used) to vertical FOV (flightmare).
+	Scalar rgb_fov_deg_    = cfg["rgb_camera_left"]["fov"].as<Scalar>();
+	double hor_fov_radians = (M_PI * (rgb_fov_deg_ / 180.0));
+	Scalar img_rows_       = cfg["rgb_camera_left"]["height"].as<Scalar>();
+	Scalar img_cols_       = cfg["rgb_camera_left"]["width"].as<Scalar>();
+	double flightmare_fov  = 2. * std::atan(std::tan(hor_fov_radians / 2) * img_rows_ / img_cols_);
+	flightmare_fov         = (flightmare_fov / M_PI) * 180.0;
+	rgb_camera_left->setFOV(flightmare_fov);
+	rgb_camera_left->setWidth(cfg["rgb_camera_left"]["width"].as<int>());
+	rgb_camera_left->setHeight(cfg["rgb_camera_left"]["height"].as<int>());
+	rgb_camera_left->setRelPose(t_BC, r_BC);
+	rgb_camera_left->enableOpticalFlow(cfg["rgb_camera_left"]["enable_opticalflow"].as<bool>());
+	rgb_camera_left->enableSegmentation(cfg["rgb_camera_left"]["enable_segmentation"].as<bool>());
+	rgb_camera_left->enableDepth(cfg["rgb_camera_left"]["enable_depth"].as<bool>());
+	// add camera to the quadrotor
+	quad_ptr_->addRGBCamera(rgb_camera_left);
+
+	// create right camera --------------------------------------------
+	if (use_stereo) {
+		rgb_camera_right = std::make_shared<RGBCamera>();
+
+		// load camera settings
+		std::vector<Scalar> t_BC_vec_r = cfg["rgb_camera_right"]["t_BC"].as<std::vector<Scalar>>();
+		std::vector<Scalar> r_BC_vec_r = cfg["rgb_camera_right"]["r_BC"].as<std::vector<Scalar>>();
+
+		Vector<3> t_BC_r(t_BC_vec_r.data());
+		Matrix<3, 3> r_BC_r = (AngleAxis(r_BC_vec[0] * M_PI / 180.0, Vector<3>::UnitX()) * AngleAxis(r_BC_vec[1] * M_PI / 180.0, Vector<3>::UnitY()) *
+		                       AngleAxis(r_BC_vec[2] * M_PI / 180.0, Vector<3>::UnitZ()))
+		                          .toRotationMatrix();
+
+		rgb_camera_right->setFOV(flightmare_fov);
+		rgb_camera_right->setWidth(cfg["rgb_camera_left"]["width"].as<int>());
+		rgb_camera_right->setHeight(cfg["rgb_camera_left"]["height"].as<int>());
+		rgb_camera_right->setRelPose(t_BC_r, r_BC_r);
+		rgb_camera_right->enableOpticalFlow(false);
+		rgb_camera_right->enableSegmentation(false);
+		rgb_camera_right->enableDepth(false);
+		// add camera to the quadrotor
+		quad_ptr_->addRGBCamera(rgb_camera_right);
+
+		stereo_baseline_ = fabs(t_BC(1) - t_BC_r(1));
+	}
+	return true;
+}
+
+}  // namespace flightros
--- a/flightlib/src/ros_nodes/flight_pilot_node.cpp
+++ b/flightlib/src/ros_nodes/flight_pilot_node.cpp
@@ -0,0 +1,13 @@
+#include <ros/ros.h>
+
+#include "flightlib/ros_nodes/flight_pilot.hpp"
+
+int main(int argc, char** argv) {
+	ros::init(argc, argv, "flight_pilot");
+	flightros::FlightPilot pilot(ros::NodeHandle(), ros::NodeHandle("~"));
+
+	// spin the ros
+	ros::spin();
+
+	return 0;
+}
--- a/flightlib/src/ros_nodes/map_visual_node.cpp
+++ b/flightlib/src/ros_nodes/map_visual_node.cpp
@@ -0,0 +1,101 @@
+// Publish the surrounding point cloud map based on the drone's position for visualization.
+#include <nav_msgs/Odometry.h>
+#include <pcl/common/common.h>
+#include <pcl/io/ply_io.h>
+#include <pcl/point_types.h>
+#include <pcl_ros/point_cloud.h>
+#include <ros/ros.h>
+#include <tf/transform_broadcaster.h>
+#include <time.h>
+#include <yaml-cpp/yaml.h>
+
+#include <Eigen/Core>
+
+#include "visualization_msgs/Marker.h"
+
+namespace map_visual {
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+
+void pcl_input() {
+	std::string cfg_path  = getenv("FLIGHTMARE_PATH") + std::string("/flightlib/configs/quadrotor_ros.yaml");
+	YAML::Node cfg_       = YAML::LoadFile(cfg_path);
+	std::string ply_path_ = getenv("FLIGHTMARE_PATH") + cfg_["ply_path"].as<std::string>() + "pointcloud-0.ply";
+	pcl::io::loadPLYFile<pcl::PointXYZ>(ply_path_, *cloud);
+	std::cout << "size of pointcloud: " << cloud->points.size() << std::endl;
+}
+
+void odom_cb(const nav_msgs::Odometry::ConstPtr odom_msg, ros::Publisher* local_map_pub, ros::Publisher* mesh_pub,
+             tf::TransformBroadcaster* uav_tf_br) {
+	// 1. publish tf
+	tf::Transform transform;
+	transform.setOrigin(tf::Vector3(odom_msg->pose.pose.position.x, odom_msg->pose.pose.position.y, odom_msg->pose.pose.position.z));
+	tf::Quaternion q(0, 0, 0, 1);
+	// tf::Quaternion q(odom_msg->pose.pose.orientation.x, odom_msg->pose.pose.orientation.y,
+	//                  odom_msg->pose.pose.orientation.z, odom_msg->pose.pose.orientation.w);
+	transform.setRotation(q);
+	uav_tf_br->sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "uav"));
+
+	// 2. publish map
+	Eigen::Vector3f pose_cur(odom_msg->pose.pose.position.x, odom_msg->pose.pose.position.y, odom_msg->pose.pose.position.z);
+	Eigen::Vector3f local_map_half_size(8, 8, 1);
+	pcl::PointCloud<pcl::PointXYZ>::Ptr local_map(new pcl::PointCloud<pcl::PointXYZ>);
+	for (auto& x : cloud->points) {
+		if (x.z > pose_cur(2) - 6 && x.z < pose_cur(2) + local_map_half_size(2) && x.x > pose_cur(0) - local_map_half_size(0) &&
+		    x.x < pose_cur(0) + local_map_half_size(0) && x.y > pose_cur(1) - local_map_half_size(1) && x.y < pose_cur(1) + local_map_half_size(1)) {
+			local_map->points.push_back(x);
+		}
+	}
+	pcl_conversions::toPCL(ros::Time::now(), local_map->header.stamp);
+	local_map->header.frame_id = "world";
+	local_map_pub->publish(local_map);
+
+	// 3. publish UAV model
+	std::string mesh_resource = std::string("file://") + getenv("FLIGHTMARE_PATH") + std::string("/flightlib/src/ros_nodes/model/uav.dae");
+
+	visualization_msgs::Marker meshROS;
+	meshROS.header.frame_id = "world";
+	meshROS.header.stamp    = ros::Time::now();
+	meshROS.ns              = "mesh";
+	meshROS.id              = 0;
+	meshROS.type            = visualization_msgs::Marker::MESH_RESOURCE;
+	meshROS.action          = visualization_msgs::Marker::ADD;
+
+	meshROS.pose.position.x = odom_msg->pose.pose.position.x - 0.2;
+	meshROS.pose.position.y = odom_msg->pose.pose.position.y;
+	meshROS.pose.position.z = odom_msg->pose.pose.position.z;
+
+	meshROS.pose.orientation.w = odom_msg->pose.pose.orientation.w;
+	meshROS.pose.orientation.x = odom_msg->pose.pose.orientation.x;
+	meshROS.pose.orientation.y = odom_msg->pose.pose.orientation.y;
+	meshROS.pose.orientation.z = odom_msg->pose.pose.orientation.z;
+
+	float scale     = 2;
+	meshROS.scale.x = scale;
+	meshROS.scale.y = scale;
+	meshROS.scale.z = scale;
+	meshROS.color.a = 1;
+	meshROS.color.r = 1;
+	meshROS.color.g = 1;
+	meshROS.color.b = 1;
+
+	meshROS.mesh_resource               = mesh_resource;
+	meshROS.mesh_use_embedded_materials = true;
+	mesh_pub->publish(meshROS);
+}
+
+}  // namespace map_visual
+
+using namespace map_visual;
+int main(int argc, char** argv) {
+	pcl_input();
+	ros::init(argc, argv, "map_visual");
+	ros::NodeHandle nh;
+	tf::TransformBroadcaster uav_tf_br;
+	ros::Publisher local_map_pub = nh.advertise<pcl::PointCloud<pcl::PointXYZ>>("/local_map", 1);
+	ros::Publisher mesh_pub      = nh.advertise<visualization_msgs::Marker>("/uav_mesh", 1);
+	ros::Subscriber odom_sub =
+	    nh.subscribe<nav_msgs::Odometry>("/juliett/ground_truth/odom", 1, boost::bind(&odom_cb, _1, &local_map_pub, &mesh_pub, &uav_tf_br));
+	std::cout << "Map visual node OK!" << std::endl;
+	ros::spin();
+}
--- a/flightlib/src/ros_nodes/model/uav.dae
+++ b/flightlib/src/ros_nodes/model/uav.dae
--- a/Show More
+++ b/Show More