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

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

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

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

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

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

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

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

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

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

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

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

flightlib/src/dynamics/dynamics_base.cpp

@@ -0,0 +1,9 @@
+#include "flightlib/dynamics/dynamics_base.hpp"
+
+namespace flightlib {
+
+DynamicsBase::DynamicsBase() {}
+
+DynamicsBase::~DynamicsBase() {}
+
+}  // namespace flightlib

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

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

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

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);
+	}
+}

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;
+}

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; }

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>();
+}

flightlib/src/objects/object_base.cpp

@@ -0,0 +1,9 @@
+#include "flightlib/objects/object_base.hpp"
+
+namespace flightlib {
+
+ObjectBase::ObjectBase() {}
+
+ObjectBase::~ObjectBase() {}
+
+}  // namespace flightlib

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

flightlib/src/objects/unity_camera.cpp

@@ -0,0 +1,9 @@
+#include "flightlib/objects/unity_camera.hpp"
+
+namespace flightlib {
+
+UnityCamera::UnityCamera() {}
+
+UnityCamera::~UnityCamera() {}
+
+}  // namespace flightlib

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

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;
+}

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();
+}

flightlib/src/ros_nodes/traj_eval_node.cpp

@@ -0,0 +1,145 @@
+// A very rough evaluation: Input the map point cloud, receive odometry, and calculate: execution time,
+// trajectory length, distance to the nearest obstacle, and dynamic cost (logging method to be optimized).
+#include <nav_msgs/Odometry.h>
+#include <pcl/common/common.h>
+#include <pcl/io/ply_io.h>
+#include <pcl/point_types.h>
+#include <pcl/search/kdtree.h>
+#include <ros/ros.h>
+#include <time.h>
+
+#include <Eigen/Core>
+
+#include "flightlib/controller/ctrl_ref.h"
+
+namespace traj_eval {
+
+float start_record  = -39;
+float finish_record = 18;
+// eval
+std::ofstream log_, log_x, ctrl_log;
+Eigen::Vector3f pose_last;
+Eigen::Vector3f acc_last;
+ros::Time start, end;
+bool odom_init   = false;
+bool odom_finish = false;
+bool first_cmd   = true;
+float length_    = 0;
+float dist_      = 0;
+float ctrl_cost_ = 0;
+int num_         = 0;
+// map
+pcl::search::KdTree<pcl::PointXYZ> kdtree;
+float resolution = 0.2;
+Eigen::Vector3i m_size;
+Eigen::Vector3i m_origin;
+
+void map_input() {
+	std::string ply_path_ = getenv("FLIGHTMARE_PATH") + std::string("/flightrender/RPG_Flightmare/pointcloud_data/pointcloud-0.ply");
+	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+	pcl::io::loadPLYFile<pcl::PointXYZ>(ply_path_, *cloud);
+
+	pcl::PointXYZ min, max;
+	pcl::getMinMax3D(*cloud, min, max);
+	m_size(0) = ceil((max.x - min.x) / resolution);
+	m_size(1) = ceil((max.y - min.y) / resolution);
+	m_size(2) = ceil((max.z - min.z + 0.01) / resolution);
+	m_origin  = Eigen::Vector3i(min.x, min.y, min.z);
+	kdtree.setInputCloud(cloud);
+}
+
+int to_id(int x, int y, int z) { return x * m_size(1) * m_size(2) + y * m_size(2) + z; }
+
+float distance_at(Eigen::Vector3f pose) {
+	pcl::PointXYZ drone_;
+	drone_.x = pose(0);
+	drone_.y = pose(1);
+	drone_.z = pose(2);
+
+	int K = 1;
+	std::vector<int> indices(K);
+	std::vector<float> distances(K);  // 存储近邻点对应距离的平方
+	kdtree.nearestKSearch(drone_, K, indices, distances);
+	return std::sqrt(distances[0]);
+}
+
+void odom_cb(const nav_msgs::Odometry::Ptr odom_msg) {
+	if (!odom_init && odom_msg->pose.pose.position.x > start_record) {
+		odom_init = true;
+		pose_last = Eigen::Vector3f(odom_msg->pose.pose.position.x, odom_msg->pose.pose.position.y, odom_msg->pose.pose.position.z);
+		start     = ros::Time::now();
+		std::cout << "start!" << std::endl;
+		return;
+	}
+	if (!odom_init || odom_finish)
+		return;
+
+	Eigen::Vector3f pose_cur(odom_msg->pose.pose.position.x, odom_msg->pose.pose.position.y, odom_msg->pose.pose.position.z);
+	length_ += (pose_cur - pose_last).norm();
+	dist_ += distance_at(pose_cur);
+	log_ << distance_at(pose_cur) << std::endl;
+	log_x << pose_cur(0) << std::endl;
+
+	pose_last = pose_cur;
+	num_++;
+	if (odom_msg->pose.pose.position.x > finish_record) {
+		odom_finish = true;
+		end         = ros::Time::now();
+		std::cout << "finish! \n time:" << (end - start).toSec() << " s,\nlength:" << length_ << " m,\ndist:" << dist_ / num_
+		          << " m,\nctrl cost:" << ctrl_cost_ << " m2/s5" << std::endl;
+		log_.close();
+		log_x.close();
+		ctrl_log.close();
+	}
+}
+
+void ctrl_cb(const quad_pos_ctrl::ctrl_ref& ctrl_msg) {
+	if (!odom_init || odom_finish)
+		return;
+	if (first_cmd) {
+		first_cmd = false;
+		acc_last  = Eigen::Vector3f(ctrl_msg.acc_ref[0], -ctrl_msg.acc_ref[1], -ctrl_msg.acc_ref[2]);
+		return;
+	}
+
+	Eigen::Vector3f cur_acc(ctrl_msg.acc_ref[0], -ctrl_msg.acc_ref[1], -ctrl_msg.acc_ref[2]);
+	Eigen::Vector3f d_acc = (acc_last - cur_acc) / 0.02;
+	float acc_norm2       = d_acc.dot(d_acc);
+	ctrl_cost_ += 0.02 * acc_norm2;
+	acc_last = cur_acc;
+
+	ctrl_log << ctrl_msg.pos_ref[0] << ',';
+	ctrl_log << ctrl_msg.pos_ref[1] << ',';
+	ctrl_log << ctrl_msg.pos_ref[2] << ',';
+
+	ctrl_log << ctrl_msg.vel_ref[0] << ',';
+	ctrl_log << ctrl_msg.vel_ref[1] << ',';
+	ctrl_log << ctrl_msg.vel_ref[2] << ',';
+
+	ctrl_log << ctrl_msg.acc_ref[0] << ',';
+	ctrl_log << ctrl_msg.acc_ref[1] << ',';
+	ctrl_log << ctrl_msg.acc_ref[2] << std::endl;
+}
+
+}  // namespace traj_eval
+
+using namespace traj_eval;
+int main(int argc, char** argv) {
+	map_input();
+	// Move to the same log file.
+	std::string log_file = getenv("FLIGHTMARE_PATH") + std::string("/run/utils/dist.csv");
+	std::cout << "log path:" << log_file << std::endl;
+	log_.open(log_file.c_str(), std::ios::out);
+
+	std::string log_file2 = getenv("FLIGHTMARE_PATH") + std::string("/run/utils/dist_x.csv");
+	log_x.open(log_file2.c_str(), std::ios::out);
+
+	std::string log_file3 = getenv("FLIGHTMARE_PATH") + std::string("/run/utils/ctrl_log.csv");
+	ctrl_log.open(log_file3.c_str(), std::ios::out);
+
+	ros::init(argc, argv, "traj_eval");
+	ros::NodeHandle nh;
+	ros::Subscriber odom_sub = nh.subscribe("/juliett/ground_truth/odom", 1, odom_cb);
+	ros::Subscriber ctrl_sub = nh.subscribe("/juliett_pos_ctrl_node/controller/ctrl_ref", 1, ctrl_cb);
+	ros::spin();
+}

flightlib/src/ros_nodes/yopo_planner_node.cpp

@@ -0,0 +1,347 @@
+#include <nav_msgs/Odometry.h>
+#include <pcl/point_types.h>
+#include <pcl_ros/point_cloud.h>
+#include <ros/ros.h>
+#include <std_msgs/Float32MultiArray.h>
+#include <yaml-cpp/yaml.h>
+
+#include <Eigen/Core>
+
+#include "flightlib/controller/PositionCommand.h"
+#include "flightlib/controller/ctrl_ref.h"
+#include "flightlib/grad_traj_optimization/opt_utile.h"
+#include "flightlib/grad_traj_optimization/traj_optimization_bridge.h"
+
+namespace yopo_net {
+
+nav_msgs::Odometry odom_msg;
+quad_pos_ctrl::ctrl_ref ctrl_ref_last;
+quadrotor_msgs::PositionCommand pos_cmd_last;
+bool odom_init     = false;
+bool odom_ref_init = false;
+bool yopo_init     = false;
+bool done          = false;
+float traj_time    = 2.0;
+float sample_t     = 0.0;
+float last_yaw_    = 0;  // NWU
+
+TrajOptimizationBridge* traj_opt_bridge;
+TrajOptimizationBridge* traj_opt_bridge_for_vis;
+std::vector<std::pair<Eigen::Vector3d, Eigen::Vector3d>> lattice_nodes;
+
+Eigen::Vector3d goal_(100, 0, 2);
+Eigen::Quaterniond quat_(1, 0, 0, 0);
+Eigen::Vector3d last_pos_(0, 0, 0), last_vel_(0, 0, 0), last_acc_(0, 0, 0);
+
+ros::Publisher trajs_visual_pub, best_traj_visual_pub, state_ref_pub, ctrl_pub, mpc_ctrl_pub, so3_ctrl_pub, lattice_trajs_visual_pub;
+ros::Subscriber odom_sub, odom_ref_sub, yopo_best_sub, yopo_all_sub, goal_sub;
+
+void odom_cb(const nav_msgs::Odometry::Ptr msg) {
+	odom_msg  = *msg;
+	odom_init = true;
+	quat_.w() = odom_msg.pose.pose.orientation.w;
+	quat_.x() = odom_msg.pose.pose.orientation.x;
+	quat_.y() = odom_msg.pose.pose.orientation.y;
+	quat_.z() = odom_msg.pose.pose.orientation.z;
+	if (!odom_ref_init) {
+		last_pos_ << odom_msg.pose.pose.position.x, odom_msg.pose.pose.position.y, odom_msg.pose.pose.position.z;
+		last_vel_ << 0, 0, 0;
+		last_acc_ << 0, 0, 0;
+	}
+
+	// check if reach the goal
+	Eigen::Vector3d dist(odom_msg.pose.pose.position.x - goal_(0), odom_msg.pose.pose.position.y - goal_(1),
+	                     odom_msg.pose.pose.position.z - goal_(2));
+	if (dist.norm() < 4) {
+		if (!done)
+			printf("Done!\n");
+		done = true;
+	}
+}
+
+void goal_cb(const std_msgs::Float32MultiArray::Ptr msg) {
+	Eigen::Vector3d last_goal = goal_;
+	goal_(0)                  = msg->data[0];
+	goal_(1)                  = msg->data[1];
+	goal_(2)                  = msg->data[2];
+	if (last_goal != goal_)
+		done = false;
+}
+
+void traj_to_pcl(TrajOptimizationBridge* traj_opt_bridge_input, pcl::PointCloud<pcl::PointXYZI>::Ptr cloud, double cost = 0.0) {
+	for (float dt = 0.0; dt < traj_time; dt = dt + 0.05) {
+		Eigen::Vector3d next_pos, next_vel, next_acc;
+		traj_opt_bridge_input->getNextState(next_pos, next_vel, next_acc, dt);
+		pcl::PointXYZI clrP;
+		clrP.x         = next_pos(0);
+		clrP.y         = next_pos(1);
+		clrP.z         = next_pos(2);
+		clrP.intensity = cost;
+		cloud->points.push_back(clrP);
+	}
+}
+
+void yopo_cb(const std_msgs::Float32MultiArray::ConstPtr msg) {
+	if (!odom_init)
+		return;
+	std::vector<double> endstate;
+	for (int i = 0; i < msg->data.size(); i++) {
+		endstate.push_back(static_cast<double>(msg->data[i]));
+	}
+
+	traj_opt_bridge->setState(last_pos_.cast<double>(), quat_.cast<double>(), last_vel_.cast<double>(), last_acc_.cast<double>());
+	traj_opt_bridge->solveBVP(endstate);
+	// int milliseconds_yopo_pub = msg->layout.data_offset;  // 预测时采用的状态和图像的时间戳(ms)
+	// int milliseconds_now = uint64_t(ros::Time::now().toSec() * 1000) % 1000000;
+	// double delta_t = double(milliseconds_now - milliseconds_yopo_pub) / 1000;
+	// std::cout<<"yopo开始预测到当前时间: "<<delta_t<<std::endl;
+	sample_t = 0.0;  // = delta_t
+
+	pcl::PointCloud<pcl::PointXYZI>::Ptr best_traj_cld(new pcl::PointCloud<pcl::PointXYZI>);
+	traj_to_pcl(traj_opt_bridge, best_traj_cld);
+	pcl_conversions::toPCL(ros::Time::now(), best_traj_cld->header.stamp);  // for test
+	best_traj_cld->header.frame_id = "world";
+	best_traj_visual_pub.publish(best_traj_cld);
+	yopo_init = true;
+}
+
+void trajs_vis_cb(const std_msgs::Float32MultiArray::ConstPtr msg) {
+	if (!odom_init)
+		return;
+
+	// ---------------- visualization of all trajs --------------------
+	std::vector<std::vector<double>> endstates_b;
+	endstates_b.resize(msg->layout.dim[0].size);
+	std::vector<double> scores;
+	for (int i = 0; i < msg->layout.dim[0].size; i++) {
+		for (int j = 0; j < msg->layout.dim[1].size - 1; j++) {
+			endstates_b[i].push_back(static_cast<double>(msg->data[i * msg->layout.dim[1].size + j]));
+		}
+		scores.push_back(static_cast<double>(msg->data[(i + 1) * msg->layout.dim[1].size - 1]));
+	}
+
+	traj_opt_bridge_for_vis->setState(last_pos_.cast<double>(), quat_.cast<double>(), last_vel_.cast<double>(), last_acc_.cast<double>());
+
+	pcl::PointCloud<pcl::PointXYZI>::Ptr trajs_cld(new pcl::PointCloud<pcl::PointXYZI>);
+	for (size_t i = 0; i < endstates_b.size(); i++) {
+		traj_opt_bridge_for_vis->solveBVP(endstates_b[i]);
+		traj_to_pcl(traj_opt_bridge_for_vis, trajs_cld, scores[i]);
+	}
+	pcl_conversions::toPCL(ros::Time::now(), trajs_cld->header.stamp);
+	trajs_cld->header.frame_id = "world";
+	trajs_visual_pub.publish(trajs_cld);
+
+	// ---------------- visualization of lattice ------------------------
+	pcl::PointCloud<pcl::PointXYZI>::Ptr lattice_trajs_cld(new pcl::PointCloud<pcl::PointXYZI>);
+	Eigen::Vector3d pos_1(0.0, 0.0, 0.0), vel_1(0.0, 0.0, 0.0), acc_1(0.0, 0.0, 0.0);
+	for (size_t i = 0; i < lattice_nodes.size(); i++) {
+		pos_1                          = lattice_nodes[i].first;
+		vel_1                          = lattice_nodes[i].second;
+		std::vector<double> endstate_lattice = {pos_1(0), vel_1(0), acc_1(0), pos_1(1), vel_1(1), acc_1(1), pos_1(2), vel_1(2), acc_1(2)};
+		traj_opt_bridge_for_vis->solveBVP(endstate_lattice);
+		traj_to_pcl(traj_opt_bridge_for_vis, lattice_trajs_cld);
+	}
+	pcl_conversions::toPCL(ros::Time::now(), lattice_trajs_cld->header.stamp);
+	lattice_trajs_cld->header.frame_id = "world";
+	lattice_trajs_visual_pub.publish(lattice_trajs_cld);
+}
+
+std::pair<float, float> calculate_yaw(float sample_t, float dt) {
+	constexpr float PI                  = 3.1415926;
+	constexpr float YAW_DOT_MAX_PER_SEC = 0.3 * PI;
+	std::pair<float, float> yaw_yawdot(0, 0);
+	float yaw    = 0;
+	float yawdot = 0;
+
+	// dir of vel
+	Eigen::Vector3d nxt_p, nxt_v, nxt_a;
+	traj_opt_bridge->getNextState(nxt_p, nxt_v, nxt_a, sample_t);
+	Eigen::Vector3d dir_vel = nxt_v / nxt_v.norm();
+	// dir of goal
+	Eigen::Vector3d dir_goal(goal_(0) - nxt_p(0), goal_(1) - nxt_p(1), goal_(2) - nxt_p(2));
+	float goal_dist = dir_goal.norm();
+	dir_goal        = dir_goal / goal_dist;
+	// or just dir_des = dir_vel
+	Eigen::Vector3d dir_des = dir_vel + dir_goal;
+
+	float yaw_temp       = goal_dist > 0.2 ? atan2(dir_des(1), dir_des(0)) : last_yaw_;
+	float max_yaw_change = YAW_DOT_MAX_PER_SEC * dt;
+
+	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;
+			yawdot = -YAW_DOT_MAX_PER_SEC;
+		} else {
+			yaw = yaw_temp;
+			if (yaw - last_yaw_ > PI)
+				yawdot = -YAW_DOT_MAX_PER_SEC;
+			else
+				yawdot = (yaw_temp - last_yaw_) / dt;
+		}
+	} 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;
+			yawdot = YAW_DOT_MAX_PER_SEC;
+		} else {
+			yaw = yaw_temp;
+			if (yaw - last_yaw_ < -PI)
+				yawdot = YAW_DOT_MAX_PER_SEC;
+			else
+				yawdot = (yaw_temp - last_yaw_) / dt;
+		}
+	} else {
+		if (yaw_temp - last_yaw_ < -max_yaw_change) {
+			yaw = last_yaw_ - max_yaw_change;
+			if (yaw < -PI)
+				yaw += 2 * PI;
+			yawdot = -YAW_DOT_MAX_PER_SEC;
+		} else if (yaw_temp - last_yaw_ > max_yaw_change) {
+			yaw = last_yaw_ + max_yaw_change;
+			if (yaw > PI)
+				yaw -= 2 * PI;
+			yawdot = YAW_DOT_MAX_PER_SEC;
+		} else {
+			yaw = yaw_temp;
+			if (yaw - last_yaw_ > PI)
+				yawdot = -YAW_DOT_MAX_PER_SEC;
+			else if (yaw - last_yaw_ < -PI)
+				yawdot = YAW_DOT_MAX_PER_SEC;
+			else
+				yawdot = (yaw_temp - last_yaw_) / dt;
+		}
+	}
+
+	last_yaw_         = yaw;
+	yaw_yawdot.first  = yaw;
+	yaw_yawdot.second = yawdot;
+	return yaw_yawdot;
+}
+
+void ref_pub_cb(const ros::TimerEvent&) {
+	if (!yopo_init)
+		return;
+
+	if (done) {
+		// single state control for smoother performance
+		ctrl_ref_last.header.stamp = ros::Time::now();
+		ctrl_ref_last.vel_ref      = {0, 0, 0};
+		ctrl_ref_last.acc_ref      = {0, 0, 0};
+		ctrl_ref_last.ref_mask     = 1;
+		ctrl_pub.publish(ctrl_ref_last);
+
+		// un-smooth, just for simpler demonstration
+		pos_cmd_last.header.stamp   = ros::Time::now();
+		pos_cmd_last.velocity.x     = 0.95 * pos_cmd_last.velocity.x;
+		pos_cmd_last.velocity.y     = 0.95 * pos_cmd_last.velocity.y;
+		pos_cmd_last.velocity.z     = 0.95 * pos_cmd_last.velocity.z;
+		pos_cmd_last.acceleration.x = 0.95 * pos_cmd_last.acceleration.x;
+		pos_cmd_last.acceleration.y = 0.95 * pos_cmd_last.acceleration.y;
+		pos_cmd_last.acceleration.z = 0.95 * pos_cmd_last.acceleration.z;
+		pos_cmd_last.yaw_dot        = 0.95 * pos_cmd_last.yaw_dot;
+		so3_ctrl_pub.publish(pos_cmd_last);
+		return;
+	}
+
+	sample_t += 0.02;
+	Eigen::Vector3d desired_p, desired_v, desired_a;
+	traj_opt_bridge->getNextState(desired_p, desired_v, desired_a, sample_t);
+	std::pair<float, float> yaw_yawdot(0, 0);
+	yaw_yawdot = calculate_yaw(sample_t, 0.02);
+
+	// Realworld & our PID Controller
+	quad_pos_ctrl::ctrl_ref ctrl_msg;
+	ctrl_msg.header.stamp = ros::Time::now();
+	ctrl_msg.pos_ref      = {desired_p(0), -desired_p(1), -desired_p(2)};
+	ctrl_msg.vel_ref      = {desired_v(0), -desired_v(1), -desired_v(2)};
+	ctrl_msg.acc_ref      = {desired_a(0), -desired_a(1), -desired_a(2)};
+	ctrl_msg.yaw_ref      = -yaw_yawdot.first;
+	ctrl_msg.ref_mask     = 7;
+	ctrl_ref_last         = ctrl_msg;
+	ctrl_pub.publish(ctrl_msg);
+
+	// SO3 Simulator & SO3 Controller
+	quadrotor_msgs::PositionCommand cmd;
+	cmd.header.frame_id = "world";
+	cmd.header.stamp    = ros::Time::now();
+	cmd.trajectory_flag = quadrotor_msgs::PositionCommand::TRAJECTORY_STATUS_READY;
+	cmd.position.x      = desired_p(0);
+	cmd.position.y      = desired_p(1);
+	cmd.position.z      = desired_p(2);
+	cmd.velocity.x      = desired_v(0);
+	cmd.velocity.y      = desired_v(1);
+	cmd.velocity.z      = desired_v(2);
+	cmd.acceleration.x  = desired_a(0);
+	cmd.acceleration.y  = desired_a(1);
+	cmd.acceleration.z  = desired_a(2);
+	cmd.yaw             = yaw_yawdot.first;
+	cmd.yaw_dot         = yaw_yawdot.second;
+	pos_cmd_last        = cmd;
+	so3_ctrl_pub.publish(cmd);
+
+	// update the desire state for next planning
+	last_pos_ = desired_p;
+	last_vel_ = desired_v;
+	last_acc_ = desired_a;
+
+	// for reference of yopo network
+	nav_msgs::Odometry odom_;
+	odom_.header.stamp          = ros::Time::now();
+	odom_.pose.pose.position.x  = desired_p(0);
+	odom_.pose.pose.position.y  = desired_p(1);
+	odom_.pose.pose.position.z  = desired_p(2);
+	odom_.twist.twist.linear.x  = desired_v(0);
+	odom_.twist.twist.linear.y  = desired_v(1);
+	odom_.twist.twist.linear.z  = desired_v(2);
+	odom_.twist.twist.angular.x = desired_a(0);
+	odom_.twist.twist.angular.y = desired_a(1);
+	odom_.twist.twist.angular.z = desired_a(2);
+	state_ref_pub.publish(odom_);
+	odom_ref_init = true;
+}
+
+}  // namespace yopo_net
+
+using namespace yopo_net;
+int main(int argc, char** argv) {
+	ros::init(argc, argv, "yopo_test");
+	ros::NodeHandle nh;
+
+	string cfg_path     = getenv("FLIGHTMARE_PATH") + std::string("/flightlib/configs/traj_opt.yaml");
+	YAML::Node cfg_     = YAML::LoadFile(cfg_path);
+	traj_time           = 2 * cfg_["radio_range"].as<double>() / cfg_["vel_max"].as<double>();
+	int horizon_num     = cfg_["horizon_num"].as<int>();
+	int vertical_num    = cfg_["vertical_num"].as<int>();
+	int vel_num         = cfg_["vel_num"].as<int>();
+	int radio_num       = cfg_["radio_num"].as<int>();
+	double horizon_fov  = cfg_["horizon_camera_fov"].as<double>() * (horizon_num - 1) / horizon_num;
+	double vertical_fov = cfg_["vertical_camera_fov"].as<double>() * (vertical_num - 1) / vertical_num;
+	double vel_fov      = cfg_["vel_fov"].as<double>();
+	double radio_range  = cfg_["radio_range"].as<double>();
+	double vel_prefile  = cfg_["vel_prefile"].as<double>();
+
+	getLatticeGuiding(lattice_nodes, horizon_num, vertical_num, radio_num, vel_num, horizon_fov, vertical_fov, radio_range, vel_fov, vel_prefile);
+	traj_opt_bridge         = new TrajOptimizationBridge();
+	traj_opt_bridge_for_vis = new TrajOptimizationBridge();
+
+	lattice_trajs_visual_pub = nh.advertise<pcl::PointCloud<pcl::PointXYZI>>("/yopo_net/lattice_trajs_visual", 1);
+	trajs_visual_pub         = nh.advertise<pcl::PointCloud<pcl::PointXYZI>>("/yopo_net/trajs_visual", 1);
+	best_traj_visual_pub     = nh.advertise<pcl::PointCloud<pcl::PointXYZI>>("/yopo_net/best_traj_visual", 1);
+	state_ref_pub            = nh.advertise<nav_msgs::Odometry>("/juliett/state_ref/odom", 10);
+
+	// our PID Controller (realworld) & SO3 Controller (simulation)
+	ctrl_pub     = nh.advertise<quad_pos_ctrl::ctrl_ref>("/juliett_pos_ctrl_node/controller/ctrl_ref", 10);
+	so3_ctrl_pub = nh.advertise<quadrotor_msgs::PositionCommand>("/so3_control/pos_cmd", 10);
+
+	odom_sub      = nh.subscribe("/juliett/ground_truth/odom", 1, yopo_net::odom_cb, ros::TransportHints().tcpNoDelay());
+	yopo_best_sub = nh.subscribe("/yopo_net/pred_endstate", 1, yopo_net::yopo_cb, ros::TransportHints().tcpNoDelay());
+	yopo_all_sub  = nh.subscribe("/yopo_net/pred_endstates", 1, trajs_vis_cb);
+	goal_sub      = nh.subscribe("/yopo_net/goal", 1, goal_cb);
+
+	ros::Timer ref_timer = nh.createTimer(ros::Duration(0.02), ref_pub_cb);
+	std::cout << "YOPO Planner Node OK!" << std::endl;
+	ros::spin();
+}

flightlib/src/sensors/imu.cpp

@@ -0,0 +1,10 @@
+#include "flightlib/sensors/imu.hpp"
+
+
+namespace flightlib {
+
+IMU::IMU() {}
+
+IMU::~IMU() {}
+
+}  // namespace flightlib

flightlib/src/sensors/rgb_camera.cpp

@@ -0,0 +1,200 @@
+#include "flightlib/sensors/rgb_camera.hpp"
+
+namespace flightlib {
+
+RGBCamera::RGBCamera() : channels_(3), width_(720), height_(480), fov_{70.0}, depth_scale_{0.2}, enabled_layers_({true, false, false, false}) {
+	post_processing_ = {{RGBCameraTypes::Depth, false},
+	                    {RGBCameraTypes::OpticalFlow, false},
+	                    {RGBCameraTypes::ObjectSegment, false},
+	                    {RGBCameraTypes::CategorySegment, false}};
+}
+
+RGBCamera::~RGBCamera() {}
+
+bool RGBCamera::feedImageQueue(const int image_layer, const cv::Mat& image_mat) {
+	queue_mutex_.lock();
+	// rgb_queue_.resize(queue_size_) queue_size_由1改为0
+	switch (image_layer) {
+		case 0:  // rgb image
+			if (rgb_queue_.size() > queue_size_)
+				rgb_queue_.resize(queue_size_);
+			rgb_queue_.push_back(image_mat);
+			break;
+		case CameraLayer::DepthMap:
+			if (depth_queue_.size() > queue_size_)
+				depth_queue_.resize(queue_size_);
+			depth_queue_.push_back(image_mat);
+			break;
+		case CameraLayer::Segmentation:
+			if (segmentation_queue_.size() > queue_size_)
+				segmentation_queue_.resize(queue_size_);
+			segmentation_queue_.push_back(image_mat);
+			break;
+		case CameraLayer::OpticalFlow:
+			if (opticalflow_queue_.size() > queue_size_)
+				opticalflow_queue_.resize(queue_size_);
+			opticalflow_queue_.push_back(image_mat);
+			break;
+	}
+	queue_mutex_.unlock();
+	return true;
+}
+
+bool RGBCamera::setRelPose(const Ref<Vector<3>> B_r_BC, const Ref<Matrix<3, 3>> R_BC) {
+	if (!B_r_BC.allFinite() || !R_BC.allFinite()) {
+		logger_.error(
+		    "The setting value for Camera Relative Pose Matrix is not valid, discard "
+		    "the setting.");
+		return false;
+	}
+	B_r_BC_                 = B_r_BC;
+	T_BC_.block<3, 3>(0, 0) = R_BC;
+	T_BC_.block<3, 1>(0, 3) = B_r_BC;
+	T_BC_.row(3) << 0.0, 0.0, 0.0, 1.0;
+	return true;
+}
+
+bool RGBCamera::setWidth(const int width) {
+	if (width <= 0.0) {
+		logger_.warn("The setting value for Image Width is not valid, discard the setting.");
+		return false;
+	}
+	width_ = width;
+	return true;
+}
+
+bool RGBCamera::setHeight(const int height) {
+	if (height <= 0.0) {
+		logger_.warn(
+		    "The setting value for Image Height is not valid, discard the "
+		    "setting.");
+		return false;
+	}
+	height_ = height;
+	return true;
+}
+
+bool RGBCamera::setFOV(const Scalar fov) {
+	if (fov <= 0.0) {
+		logger_.warn(
+		    "The setting value for Camera Field-of-View is not valid, discard the "
+		    "setting.");
+		return false;
+	}
+	fov_ = fov;
+	return true;
+}
+
+bool RGBCamera::setDepthScale(const Scalar depth_scale) {
+	if (depth_scale_ < 0.0 || depth_scale_ > 1.0) {
+		logger_.warn(
+		    "The setting value for Camera Depth Scale is not valid, discard the "
+		    "setting.");
+		return false;
+	}
+	depth_scale_ = depth_scale;
+	return true;
+}
+
+bool RGBCamera::setPostProcesscing(const std::vector<bool>& enabled_layers) {
+	if (enabled_layers_.size() != enabled_layers.size()) {
+		logger_.warn("Vector size does not match. The vector size should be equal to %d.", enabled_layers_.size());
+		return false;
+	}
+	enabled_layers_ = enabled_layers;
+	return true;
+}
+
+std::vector<bool> RGBCamera::getEnabledLayers(void) const { return enabled_layers_; }
+
+std::vector<std::string> RGBCamera::GetPostProcessing(void) {
+	std::vector<std::string> post_processing;
+	for (const auto& pp : post_processing_) {
+		if (pp.second) {
+			post_processing.push_back(pp.first);
+		}
+	}
+	return post_processing;
+}
+
+Matrix<4, 4> RGBCamera::getRelPose(void) const { return T_BC_; }
+
+int RGBCamera::getChannels(void) const { return channels_; }
+
+int RGBCamera::getWidth(void) const { return width_; }
+
+int RGBCamera::getHeight(void) const { return height_; }
+
+Scalar RGBCamera::getFOV(void) const { return fov_; }
+
+Scalar RGBCamera::getDepthScale(void) const { return depth_scale_; }
+
+void RGBCamera::enableDepth(const bool on) {
+	// if (enabled_layers_[CameraLayer::DepthMap] == on) {
+	// 	logger_.warn("Depth layer was already %s.", on ? "on" : "off");
+	// }
+	enabled_layers_[CameraLayer::DepthMap]  = on;
+	post_processing_[RGBCameraTypes::Depth] = on;
+}
+
+void RGBCamera::enableSegmentation(const bool on) {
+	// if (enabled_layers_[CameraLayer::Segmentation] == on) {
+	// 	logger_.warn("Segmentation layer was already %s.", on ? "on" : "off");
+	// }
+	enabled_layers_[CameraLayer::Segmentation]        = on;
+	post_processing_[RGBCameraTypes::ObjectSegment]   = on;
+	post_processing_[RGBCameraTypes::CategorySegment] = on;
+}
+
+void RGBCamera::enableOpticalFlow(const bool on) {
+	// if (enabled_layers_[CameraLayer::OpticalFlow] == on) {
+	// 	logger_.warn("Optical Flow layer was already %s.", on ? "on" : "off");
+	// }
+	enabled_layers_[CameraLayer::OpticalFlow]     = on;
+	post_processing_[RGBCameraTypes::OpticalFlow] = on;
+}
+
+bool RGBCamera::getRGBImage(cv::Mat& rgb_img) {
+	if (!rgb_queue_.empty()) {
+		rgb_img = rgb_queue_.front();
+		rgb_queue_.pop_front();
+		return true;
+	}
+	return false;
+}
+
+bool RGBCamera::getDepthMap(cv::Mat& depth_map) {
+	if (!depth_queue_.empty()) {
+		depth_map = depth_queue_.front();
+		depth_queue_.pop_front();
+		return true;
+	}
+	return false;
+}
+
+bool RGBCamera::getSegmentation(cv::Mat& segmentation) {
+	if (!segmentation_queue_.empty()) {
+		segmentation = segmentation_queue_.front();
+		segmentation_queue_.pop_front();
+		return true;
+	}
+	return false;
+}
+
+bool RGBCamera::getOpticalFlow(cv::Mat& opticalflow) {
+	if (!opticalflow_queue_.empty()) {
+		opticalflow = opticalflow_queue_.front();
+		opticalflow_queue_.pop_front();
+		return true;
+	}
+	return false;
+}
+
+void RGBCamera::clearImageQueue() {
+	rgb_queue_.clear();
+	depth_queue_.clear();
+	segmentation_queue_.clear();
+	opticalflow_queue_.clear();
+}
+
+}  // namespace flightlib

flightlib/src/sensors/sensor_base.cpp

@@ -0,0 +1,9 @@
+#include "flightlib/sensors/sensor_base.hpp"
+
+namespace flightlib {
+
+SensorBase::SensorBase() {}
+
+SensorBase::~SensorBase() {}
+
+}  // namespace flightlib

flightlib/src/sensors/sgm_gpu/costs.cu

@@ -0,0 +1,115 @@
+/***********************************************************************
+  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/costs.h"
+#include <stdio.h>
+
+namespace sgm_gpu
+{
+
+__global__ void 
+__launch_bounds__(1024, 2)
+CenterSymmetricCensusKernelSM2(const uint8_t *im, const uint8_t *im2, cost_t *transform, cost_t *transform2, const uint32_t rows, const uint32_t cols) {
+  const int idx = blockIdx.x*blockDim.x+threadIdx.x;
+  const int idy = blockIdx.y*blockDim.y+threadIdx.y;
+
+  const int win_cols = (32+LEFT*2); // 32+4*2 = 40
+  const int win_rows = (32+TOP*2); // 32+3*2 = 38
+
+  __shared__ uint8_t window[win_cols*win_rows];
+  __shared__ uint8_t window2[win_cols*win_rows];
+
+  const int id = threadIdx.y*blockDim.x+threadIdx.x;
+  const int sm_row = id / win_cols;
+  const int sm_col = id % win_cols;
+
+  const int im_row = blockIdx.y*blockDim.y+sm_row-TOP;
+  const int im_col = blockIdx.x*blockDim.x+sm_col-LEFT;
+  const bool boundaries = (im_row >= 0 && im_col >= 0 && im_row < rows && im_col < cols);
+  window[sm_row*win_cols+sm_col] = boundaries ? im[im_row*cols+im_col] : 0;
+  window2[sm_row*win_cols+sm_col] = boundaries ? im2[im_row*cols+im_col] : 0;
+
+  // Not enough threads to fill window and window2
+  const int block_size = blockDim.x*blockDim.y;
+  if(id < (win_cols*win_rows-block_size)) {
+    const int id = threadIdx.y*blockDim.x+threadIdx.x+block_size;
+    const int sm_row = id / win_cols;
+    const int sm_col = id % win_cols;
+
+    const int im_row = blockIdx.y*blockDim.y+sm_row-TOP;
+    const int im_col = blockIdx.x*blockDim.x+sm_col-LEFT;
+    const bool boundaries = (im_row >= 0 && im_col >= 0 && im_row < rows && im_col < cols);
+    window[sm_row*win_cols+sm_col] = boundaries ? im[im_row*cols+im_col] : 0;
+    window2[sm_row*win_cols+sm_col] = boundaries ? im2[im_row*cols+im_col] : 0;
+  }
+
+  __syncthreads();
+  cost_t census = 0;
+  cost_t census2 = 0;
+  if(idy < rows && idx < cols) {
+      for(int k = 0; k < CENSUS_HEIGHT/2; k++) {
+        for(int m = 0; m < CENSUS_WIDTH; m++) {
+          const uint8_t e1 = window[(threadIdx.y+k)*win_cols+threadIdx.x+m];
+          const uint8_t e2 = window[(threadIdx.y+2*TOP-k)*win_cols+threadIdx.x+2*LEFT-m];
+          const uint8_t i1 = window2[(threadIdx.y+k)*win_cols+threadIdx.x+m];
+          const uint8_t i2 = window2[(threadIdx.y+2*TOP-k)*win_cols+threadIdx.x+2*LEFT-m];
+
+          const int shft = k*CENSUS_WIDTH+m;
+          // Compare to the center
+          cost_t tmp = (e1 >= e2);
+          // Shift to the desired position
+          tmp <<= shft;
+          // Add it to its place
+          census |= tmp;
+          // Compare to the center
+          cost_t tmp2 = (i1 >= i2);
+          // Shift to the desired position
+          tmp2 <<= shft;
+          // Add it to its place
+          census2 |= tmp2;
+        }
+      }
+      if(CENSUS_HEIGHT % 2 != 0) {
+        const int k = CENSUS_HEIGHT/2;
+        for(int m = 0; m < CENSUS_WIDTH/2; m++) {
+          const uint8_t e1 = window[(threadIdx.y+k)*win_cols+threadIdx.x+m];
+          const uint8_t e2 = window[(threadIdx.y+2*TOP-k)*win_cols+threadIdx.x+2*LEFT-m];
+          const uint8_t i1 = window2[(threadIdx.y+k)*win_cols+threadIdx.x+m];
+          const uint8_t i2 = window2[(threadIdx.y+2*TOP-k)*win_cols+threadIdx.x+2*LEFT-m];
+
+          const int shft = k*CENSUS_WIDTH+m;
+          // Compare to the center
+          cost_t tmp = (e1 >= e2);
+          // Shift to the desired position
+          tmp <<= shft;
+          // Add it to its place
+          census |= tmp;
+          // Compare to the center
+          cost_t tmp2 = (i1 >= i2);
+          // Shift to the desired position
+          tmp2 <<= shft;
+          // Add it to its place
+          census2 |= tmp2;
+        }
+      }
+
+    transform[idy*cols+idx] = census;
+    transform2[idy*cols+idx] = census2;
+  }
+}
+
+} // namespace sgm_gpu
+

flightlib/src/sensors/sgm_gpu/hamming_cost.cu

@@ -0,0 +1,71 @@
+/***********************************************************************
+  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/hamming_cost.h"
+
+namespace sgm_gpu
+{
+
+//d_transform0, d_transform1, d_cost, rows, cols
+__global__ void
+HammingDistanceCostKernel (  const cost_t *d_transform0, const cost_t *d_transform1,
+    uint8_t *d_cost, const int rows, const int cols ) {
+  //const int Dmax=   blockDim.x;  // Dmax is CTA size
+  const int y=      blockIdx.x;  // y is CTA Identifier
+  const int THRid = threadIdx.x; // THRid is Thread Identifier
+
+  __shared__ cost_t SharedMatch[2*MAX_DISPARITY];
+  __shared__ cost_t SharedBase [MAX_DISPARITY];
+
+  SharedMatch [MAX_DISPARITY+THRid] = d_transform1[y*cols+0];  // init position
+
+  int n_iter = cols/MAX_DISPARITY;
+  for (int ix=0; ix<n_iter; ix++) {
+    const int x = ix*MAX_DISPARITY;
+    SharedMatch [THRid]      = SharedMatch [THRid + MAX_DISPARITY];
+    SharedMatch [THRid+MAX_DISPARITY] = d_transform1 [y*cols+x+THRid];
+    SharedBase  [THRid]      = d_transform0 [y*cols+x+THRid];
+
+    __syncthreads();
+    for (int i=0; i<MAX_DISPARITY; i++) {
+      const cost_t base  = SharedBase [i];
+      const cost_t match = SharedMatch[(MAX_DISPARITY-1-THRid)+1+i];
+      d_cost[(y*cols+x+i)*MAX_DISPARITY+THRid] = popcount( base ^ match );
+    }
+    __syncthreads();
+  }
+  // For images with cols not multiples of MAX_DISPARITY
+  const int x = MAX_DISPARITY*(cols/MAX_DISPARITY);
+  const int left = cols-x;
+  if(left > 0) {
+    SharedMatch [THRid]      = SharedMatch [THRid + MAX_DISPARITY];
+    if(THRid < left) {
+      SharedMatch [THRid+MAX_DISPARITY] = d_transform1 [y*cols+x+THRid];
+      SharedBase  [THRid]      = d_transform0 [y*cols+x+THRid];
+    }
+
+    __syncthreads();
+    for (int i=0; i<left; i++) {
+      const cost_t base  = SharedBase [i];
+      const cost_t match = SharedMatch[(MAX_DISPARITY-1-THRid)+1+i];
+      d_cost[(y*cols+x+i)*MAX_DISPARITY+THRid] = popcount( base ^ match );
+    }
+    __syncthreads();
+  }
+}
+
+} // namespace sgm_gpu
+

flightlib/src/sensors/sgm_gpu/left_right_consistency.cu

@@ -0,0 +1,69 @@
+/***********************************************************************
+  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/left_right_consistency.h"
+#include "flightlib/sensors/sgm_gpu/configuration.h"
+
+namespace sgm_gpu
+{
+
+__global__ void ChooseRightDisparity(uint8_t *right_disparity, const uint16_t *smoothed_cost, const uint32_t rows, const uint32_t cols) {
+  const int x = blockIdx.x*blockDim.x+threadIdx.x;
+  const int y = blockIdx.y*blockDim.y+threadIdx.y;
+  
+  if (x >= cols || y >= rows)
+    return;
+  
+  int min_cost_disparity = 0;
+  uint16_t min_cost = smoothed_cost[(y*cols + x)*MAX_DISPARITY + min_cost_disparity];
+  
+  for (int d = 1; d < MAX_DISPARITY; d++) {
+    if (x + d >= cols)
+      break;
+    uint16_t tmp_cost = smoothed_cost[(y*cols + (x+d))*MAX_DISPARITY + d];
+    if (tmp_cost < min_cost) {
+      min_cost = tmp_cost;
+      min_cost_disparity = d;
+    }
+  }
+  
+  right_disparity[y*cols+x] = min_cost_disparity;
+}
+
+__global__ void LeftRightConsistencyCheck(uint8_t* disparity, const uint8_t* disparity_right, uint32_t rows, uint32_t cols)
+{
+  const int x = blockIdx.x*blockDim.x+threadIdx.x;
+  const int y = blockIdx.y*blockDim.y+threadIdx.y;
+  
+  if (x >= cols || y >= rows)
+    return;
+    
+  const int x_right = x - disparity[y*cols + x];
+  
+  if (x_right < 0) {
+    disparity[y*cols + x] = 255;
+    return;
+  }
+  
+  int diff = disparity[y*cols + x] - disparity_right[y*cols + x_right];
+  diff = diff < 0 ? diff * -1 : diff;
+  if (diff > 1) {
+    disparity[y*cols + x] = 255;
+  }
+}
+
+} // namespace sgm_gpu
+

flightlib/src/sensors/sgm_gpu/median_filter.cu

@@ -0,0 +1,26 @@
+/***********************************************************************
+  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/median_filter.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) {
+  MedianFilter<3>(d_input, d_out, rows, cols);
+}
+
+}

flightlib/src/sensors/sgm_gpu/sgm_gpu.cu

@@ -0,0 +1,270 @@
+/***********************************************************************
+  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/>.
+***********************************************************************/
+#include "flightlib/sensors/sgm_gpu/sgm_gpu.h"
+
+#include "flightlib/sensors/sgm_gpu/cost_aggregation.h"
+#include "flightlib/sensors/sgm_gpu/costs.h"
+#include "flightlib/sensors/sgm_gpu/hamming_cost.h"
+#include "flightlib/sensors/sgm_gpu/left_right_consistency.h"
+#include "flightlib/sensors/sgm_gpu/median_filter.h"
+
+namespace sgm_gpu {
+// Variables which have CUDA-related type are put here
+//   in order to include sgm_gpu.h from non-CUDA package
+cudaStream_t stream1_;
+cudaStream_t stream2_;
+cudaStream_t stream3_;
+
+dim3 BLOCK_SIZE_;
+dim3 grid_size_;
+
+SgmGpu::SgmGpu(const int cols, const int rows)
+    : memory_allocated_(false), cols_(cols), rows_(rows) {
+  // Get parameters used in SGM algorithm
+  p1_ = 6;   // static_cast<uint8_t>(private_node_handle_->param("p1", 6));
+  p2_ = 96;  // static_cast<uint8_t>(private_node_handle_->param("p2", 96));
+  check_consistency_ = true;  // private_node_handle_->param("check_consistency", true);
+
+  // Create streams
+  cudaStreamCreate(&stream1_);
+  cudaStreamCreate(&stream2_);
+  cudaStreamCreate(&stream3_);
+}
+
+SgmGpu::~SgmGpu() {
+  freeMemory();
+
+  cudaStreamDestroy(stream1_);
+  cudaStreamDestroy(stream2_);
+  cudaStreamDestroy(stream3_);
+}
+
+void SgmGpu::allocateMemory(uint32_t cols, uint32_t rows) {
+  freeMemory();
+
+  cols_ = cols;
+  rows_ = rows;
+
+  int total_pixel = cols_ * rows_;
+  cudaMalloc((void **)&d_im0_, sizeof(uint8_t) * total_pixel);
+  cudaMalloc((void **)&d_im1_, sizeof(uint8_t) * total_pixel);
+
+  cudaMalloc((void **)&d_transform0_, sizeof(cost_t) * total_pixel);
+  cudaMalloc((void **)&d_transform1_, sizeof(cost_t) * total_pixel);
+
+  int cost_volume_size = total_pixel * MAX_DISPARITY;
+  cudaMalloc((void **)&d_cost_, sizeof(uint8_t) * cost_volume_size);
+
+  cudaMalloc((void **)&d_L0_, sizeof(uint8_t) * cost_volume_size);
+  cudaMalloc((void **)&d_L1_, sizeof(uint8_t) * cost_volume_size);
+  cudaMalloc((void **)&d_L2_, sizeof(uint8_t) * cost_volume_size);
+  cudaMalloc((void **)&d_L3_, sizeof(uint8_t) * cost_volume_size);
+  cudaMalloc((void **)&d_L4_, sizeof(uint8_t) * cost_volume_size);
+  cudaMalloc((void **)&d_L5_, sizeof(uint8_t) * cost_volume_size);
+  cudaMalloc((void **)&d_L6_, sizeof(uint8_t) * cost_volume_size);
+  cudaMalloc((void **)&d_L7_, sizeof(uint8_t) * cost_volume_size);
+
+  cudaMalloc((void **)&d_s_, sizeof(uint16_t) * cost_volume_size);
+
+  cudaMalloc((void **)&d_disparity_, sizeof(uint8_t) * total_pixel);
+  cudaMalloc((void **)&d_disparity_filtered_uchar_,
+             sizeof(uint8_t) * total_pixel);
+  cudaMalloc((void **)&d_disparity_right_, sizeof(uint8_t) * total_pixel);
+  cudaMalloc((void **)&d_disparity_right_filtered_uchar_,
+             sizeof(uint8_t) * total_pixel);
+
+  memory_allocated_ = true;
+}
+
+void SgmGpu::freeMemory() {
+  if (!memory_allocated_) return;
+
+  cudaFree(d_im0_);
+  cudaFree(d_im1_);
+  cudaFree(d_transform0_);
+  cudaFree(d_transform1_);
+  cudaFree(d_L0_);
+  cudaFree(d_L1_);
+  cudaFree(d_L2_);
+  cudaFree(d_L3_);
+  cudaFree(d_L4_);
+  cudaFree(d_L5_);
+  cudaFree(d_L6_);
+  cudaFree(d_L7_);
+  cudaFree(d_disparity_);
+  cudaFree(d_disparity_filtered_uchar_);
+  cudaFree(d_disparity_right_);
+  cudaFree(d_disparity_right_filtered_uchar_);
+  cudaFree(d_cost_);
+  cudaFree(d_s_);
+
+  memory_allocated_ = false;
+}
+
+bool SgmGpu::computeDisparity(const cv::Mat &left_image,
+                              const cv::Mat &right_image,
+                              cv::Mat &disparity_out) {
+  // Convert images to grayscale
+  cv::Mat left_mono8, right_mono8;
+  if (left_image.channels() > 1) {
+    cv::cvtColor(left_image, left_mono8, CV_RGB2GRAY);
+  }
+
+  if (right_image.channels() > 1) {
+    cv::cvtColor(right_image, right_mono8, CV_RGB2GRAY);
+  }
+
+  // Resize images to their width and height divisible by 4 for limit of CUDA
+  // code
+  resizeToDivisibleBy4(left_mono8, right_mono8);
+
+  // Reallocate memory if needed
+  bool size_changed = (cols_ != left_mono8.cols || rows_ != left_mono8.rows);
+  if (!memory_allocated_ || size_changed)
+    allocateMemory(left_mono8.cols, left_mono8.rows);
+
+  // Copy image to GPU device
+  size_t mono8_image_size = left_mono8.total() * sizeof(uint8_t);
+  cudaMemcpyAsync(d_im0_, left_mono8.ptr<uint8_t>(), mono8_image_size,
+                  cudaMemcpyHostToDevice, stream1_);
+  cudaMemcpyAsync(d_im1_, right_mono8.ptr<uint8_t>(), mono8_image_size,
+                  cudaMemcpyHostToDevice, stream1_);
+
+  BLOCK_SIZE_.x = 32;
+  BLOCK_SIZE_.y = 32;
+
+  grid_size_.x = (cols_ + BLOCK_SIZE_.x - 1) / BLOCK_SIZE_.x;
+  grid_size_.y = (rows_ + BLOCK_SIZE_.y - 1) / BLOCK_SIZE_.y;
+
+  CenterSymmetricCensusKernelSM2<<<grid_size_, BLOCK_SIZE_, 0, stream1_>>>(
+      d_im0_, d_im1_, d_transform0_, d_transform1_, rows_, cols_);
+
+  cudaStreamSynchronize(stream1_);
+  HammingDistanceCostKernel<<<rows_, MAX_DISPARITY, 0, stream1_>>>(
+      d_transform0_, d_transform1_, d_cost_, rows_, cols_);
+
+  const int PIXELS_PER_BLOCK = COSTAGG_BLOCKSIZE / WARP_SIZE;
+  const int PIXELS_PER_BLOCK_HORIZ = COSTAGG_BLOCKSIZE_HORIZ / WARP_SIZE;
+
+  // Cost Aggregation
+  CostAggregationKernelLeftToRight<<<(rows_ + PIXELS_PER_BLOCK_HORIZ - 1) /
+                                         PIXELS_PER_BLOCK_HORIZ,
+                                     COSTAGG_BLOCKSIZE_HORIZ, 0, stream2_>>>(
+      d_cost_, d_L0_, d_s_, p1_, p2_, rows_, cols_, d_transform0_,
+      d_transform1_, d_disparity_, d_L0_, d_L1_, d_L2_, d_L3_, d_L4_, d_L5_,
+      d_L6_);
+  CostAggregationKernelRightToLeft<<<(rows_ + PIXELS_PER_BLOCK_HORIZ - 1) /
+                                         PIXELS_PER_BLOCK_HORIZ,
+                                     COSTAGG_BLOCKSIZE_HORIZ, 0, stream3_>>>(
+      d_cost_, d_L1_, d_s_, p1_, p2_, rows_, cols_, d_transform0_,
+      d_transform1_, d_disparity_, d_L0_, d_L1_, d_L2_, d_L3_, d_L4_, d_L5_,
+      d_L6_);
+  CostAggregationKernelUpToDown<<<(cols_ + PIXELS_PER_BLOCK - 1) /
+                                      PIXELS_PER_BLOCK,
+                                  COSTAGG_BLOCKSIZE, 0, stream1_>>>(
+      d_cost_, d_L2_, d_s_, p1_, p2_, rows_, cols_, d_transform0_,
+      d_transform1_, d_disparity_, d_L0_, d_L1_, d_L2_, d_L3_, d_L4_, d_L5_,
+      d_L6_);
+  CostAggregationKernelDownToUp<<<(cols_ + PIXELS_PER_BLOCK - 1) /
+                                      PIXELS_PER_BLOCK,
+                                  COSTAGG_BLOCKSIZE, 0, stream1_>>>(
+      d_cost_, d_L3_, d_s_, p1_, p2_, rows_, cols_, d_transform0_,
+      d_transform1_, d_disparity_, d_L0_, d_L1_, d_L2_, d_L3_, d_L4_, d_L5_,
+      d_L6_);
+  CostAggregationKernelDiagonalDownUpLeftRight<<<
+      (cols_ + PIXELS_PER_BLOCK - 1) / PIXELS_PER_BLOCK, COSTAGG_BLOCKSIZE, 0,
+      stream1_>>>(d_cost_, d_L4_, d_s_, p1_, p2_, rows_, cols_, d_transform0_,
+                  d_transform1_, d_disparity_, d_L0_, d_L1_, d_L2_, d_L3_,
+                  d_L4_, d_L5_, d_L6_);
+  CostAggregationKernelDiagonalUpDownLeftRight<<<
+      (cols_ + PIXELS_PER_BLOCK - 1) / PIXELS_PER_BLOCK, COSTAGG_BLOCKSIZE, 0,
+      stream1_>>>(d_cost_, d_L5_, d_s_, p1_, p2_, rows_, cols_, d_transform0_,
+                  d_transform1_, d_disparity_, d_L0_, d_L1_, d_L2_, d_L3_,
+                  d_L4_, d_L5_, d_L6_);
+  CostAggregationKernelDiagonalDownUpRightLeft<<<
+      (cols_ + PIXELS_PER_BLOCK - 1) / PIXELS_PER_BLOCK, COSTAGG_BLOCKSIZE, 0,
+      stream1_>>>(d_cost_, d_L6_, d_s_, p1_, p2_, rows_, cols_, d_transform0_,
+                  d_transform1_, d_disparity_, d_L0_, d_L1_, d_L2_, d_L3_,
+                  d_L4_, d_L5_, d_L6_);
+  CostAggregationKernelDiagonalUpDownRightLeft<<<
+      (cols_ + PIXELS_PER_BLOCK - 1) / PIXELS_PER_BLOCK, COSTAGG_BLOCKSIZE, 0,
+      stream1_>>>(d_cost_, d_L7_, d_s_, p1_, p2_, rows_, cols_, d_transform0_,
+                  d_transform1_, d_disparity_, d_L0_, d_L1_, d_L2_, d_L3_,
+                  d_L4_, d_L5_, d_L6_);
+
+  int total_pixel = rows_ * cols_;
+  MedianFilter3x3<<<(total_pixel + MAX_DISPARITY - 1) / MAX_DISPARITY,
+                    MAX_DISPARITY, 0, stream1_>>>(
+      d_disparity_, d_disparity_filtered_uchar_, rows_, cols_);
+
+  if (check_consistency_) {
+    ChooseRightDisparity<<<grid_size_, BLOCK_SIZE_, 0, stream1_>>>(
+        d_disparity_right_, d_s_, rows_, cols_);
+    MedianFilter3x3<<<(total_pixel + MAX_DISPARITY - 1) / MAX_DISPARITY,
+                      MAX_DISPARITY, 0, stream1_>>>(
+        d_disparity_right_, d_disparity_right_filtered_uchar_, rows_, cols_);
+
+    LeftRightConsistencyCheck<<<grid_size_, BLOCK_SIZE_, 0, stream1_>>>(
+        d_disparity_filtered_uchar_, d_disparity_right_filtered_uchar_, rows_,
+        cols_);
+  }
+  cudaError_t err = cudaGetLastError();
+  if (err != cudaSuccess) {
+    printf("libsgm_gpu ERROR: %s %d\n", cudaGetErrorString(err), err);
+    return false;
+  }
+
+  cudaDeviceSynchronize();
+  cv::Mat disparity(rows_, cols_, CV_8UC1);
+  cudaMemcpy(disparity.data, d_disparity_filtered_uchar_,
+             sizeof(uint8_t) * total_pixel, cudaMemcpyDeviceToHost);
+
+  // Restore image size if resized to be divisible by 4
+  if (cols_ != left_image.cols || rows_ != left_image.rows) {
+    cv::Size input_size(left_image.cols, left_image.rows);
+    cv::resize(disparity, disparity, input_size, 0, 0, cv::INTER_AREA);
+  }
+
+  disparity_out = disparity;
+  //  convertToMsg(disparity, left_camera_info, right_camera_info,
+  //  disparity_msg);
+
+  return true;
+}
+
+void SgmGpu::resizeToDivisibleBy4(cv::Mat &left_image, cv::Mat &right_image) {
+  bool need_resize = false;
+  cv::Size original_size, resized_size;
+
+  original_size = cv::Size(left_image.cols, left_image.rows);
+  resized_size = original_size;
+  if (original_size.width % 4 != 0) {
+    need_resize = true;
+    resized_size.width = (original_size.width / 4 + 1) * 4;
+  }
+  if (original_size.height % 4 != 0) {
+    need_resize = true;
+    resized_size.height = (original_size.height / 4 + 1) * 4;
+  }
+
+  if (need_resize) {
+    cv::resize(left_image, left_image, resized_size, 0, 0, cv::INTER_LINEAR);
+    cv::resize(right_image, right_image, resized_size, 0, 0, cv::INTER_LINEAR);
+  }
+}
+
+
+}  // namespace sgm_gpu

flightlib/src/wrapper/pybind_wrapper.cpp

@@ -0,0 +1,49 @@
+
+// pybind11
+#include <pybind11/eigen.h>
+#include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
+// flightlib
+#include "flightlib/envs/env_base.hpp"
+#include "flightlib/envs/quadrotor_env.hpp"
+#include "flightlib/envs/vec_env.hpp"
+
+namespace py = pybind11;
+using namespace flightlib;
+
+// vec_env -> quadrotor_env
+PYBIND11_MODULE(flightgym, m) {
+	py::class_<VecEnv<QuadrotorEnv>>(m, "QuadrotorEnv_v1")
+	    .def(py::init<>())
+	    .def(py::init<const std::string&>())
+	    .def(py::init<const std::string&, const bool>())
+	    // unity
+	    .def("close", &VecEnv<QuadrotorEnv>::close)
+	    .def("connectUnity", &VecEnv<QuadrotorEnv>::connectUnity)
+	    .def("disconnectUnity", &VecEnv<QuadrotorEnv>::disconnectUnity)
+	    .def("render", &VecEnv<QuadrotorEnv>::render)
+	    .def("spawnTrees", &VecEnv<QuadrotorEnv>::spawnTrees)
+	    .def("savePointcloud", &VecEnv<QuadrotorEnv>::savePointcloud)
+	    .def("spawnTreesAndSavePointcloud", &VecEnv<QuadrotorEnv>::spawnTreesAndSavePointcloud)
+	    // set
+	    .def("step", &VecEnv<QuadrotorEnv>::step)
+	    .def("reset", &VecEnv<QuadrotorEnv>::reset)
+	    .def("setState", &VecEnv<QuadrotorEnv>::setState)
+	    .def("setGoal", &VecEnv<QuadrotorEnv>::setGoal)
+	    .def("setSeed", &VecEnv<QuadrotorEnv>::setSeed)
+	    .def("setMapID", &VecEnv<QuadrotorEnv>::setMapID)
+	    // get
+	    .def("getNumOfEnvs", &VecEnv<QuadrotorEnv>::getNumOfEnvs)
+	    .def("getWorldBox", &VecEnv<QuadrotorEnv>::getWorldBox)
+	    .def("getObsDim", &VecEnv<QuadrotorEnv>::getObsDim)
+	    .def("getActDim", &VecEnv<QuadrotorEnv>::getActDim)
+	    .def("getRewDim", &VecEnv<QuadrotorEnv>::getRewDim)
+	    .def("getRGBImage", &VecEnv<QuadrotorEnv>::getRGBImage)
+	    .def("getStereoImage", &VecEnv<QuadrotorEnv>::getStereoImage)
+	    .def("getDepthImage", &VecEnv<QuadrotorEnv>::getDepthImage)
+	    .def("getImgHeight", &VecEnv<QuadrotorEnv>::getImgHeight)
+	    .def("getImgWidth", &VecEnv<QuadrotorEnv>::getImgWidth)
+	    .def("getRewardNames", &VecEnv<QuadrotorEnv>::getRewardNames)
+	    .def("getCostAndGradient", &VecEnv<QuadrotorEnv>::getCostAndGradient)
+	    .def("__repr__", [](const VecEnv<QuadrotorEnv>& a) { return "Flightmare Environment"; });
+}