YOPO

Simulator/src/CMakeLists.txt

@@ -0,0 +1,63 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(sensor_simulator)
+
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+## Find catkin and any catkin packages
+find_package(catkin REQUIRED COMPONENTS
+        roscpp
+        nav_msgs
+        std_msgs
+        sensor_msgs
+        cv_bridge
+)
+find_package(Eigen3 REQUIRED)
+find_package(PCL REQUIRED)
+find_package(OpenCV REQUIRED)
+find_package(OpenMP)
+find_package(yaml-cpp REQUIRED)
+message(WARNING "OpenCV_VERSION: ${OpenCV_VERSION}")
+
+# CUDA
+find_package(CUDA REQUIRED)
+# Query the GPU architecture
+cuda_select_nvcc_arch_flags(ARCH_FLAGS)
+message(WARNING "CUDA Architecture: ${ARCH_FLAGS}")
+set(CUDA_NVCC_FLAGS "${CUDA_NVCC_FLAGS} ${ARCH_FLAGS}")
+
+set(CMAKE_BUILD_TYPE Release)
+set(CMAKE_CXX_FLAGS "-O3")
+set(CMAKE_CUDA_FLAGS_RELEASE "-O3")
+
+## Declare a catkin package
+catkin_package(
+        INCLUDE_DIRS include
+        LIBRARIES sensor_simulator
+        CATKIN_DEPENDS roscpp
+        DEPENDS OpenCV)
+
+## Specify additional locations of header files
+include_directories(
+        include
+        ${catkin_INCLUDE_DIRS}
+        ${EIGEN3_INCLUDE_DIR}  # Eigen库路径
+        ${PCL_INCLUDE_DIRS}
+        ${OpenCV_INCLUDE_DIRS}
+)
+
+add_executable(sensor_simulator src/test_simulator.cpp src/sensor_simulator.cpp)
+target_link_libraries(sensor_simulator ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES} OpenMP::OpenMP_CXX yaml-cpp)
+target_compile_definitions(sensor_simulator PRIVATE CONFIG_FILE_PATH="${CMAKE_SOURCE_DIR}/config/config.yaml")
+
+
+cuda_add_library(raycast_cuda src/sensor_simulator.cu)
+target_link_libraries(raycast_cuda ${CUDA_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES})
+
+add_executable(sensor_simulator_cuda src/test_simulator_cuda.cpp src/maps.cpp src/perlinnoise.cpp)
+target_link_libraries(sensor_simulator_cuda ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES} raycast_cuda yaml-cpp)
+target_compile_definitions(sensor_simulator_cuda PRIVATE CONFIG_FILE_PATH="${CMAKE_SOURCE_DIR}/config/config.yaml")
+
+add_executable(dataset_generator src/dataset_generator.cpp src/maps.cpp src/perlinnoise.cpp)
+target_link_libraries(dataset_generator ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES} raycast_cuda yaml-cpp)
+target_compile_definitions(dataset_generator PRIVATE CONFIG_FILE_PATH="${CMAKE_SOURCE_DIR}/config/config.yaml")

Simulator/src/config/config.yaml

@@ -0,0 +1,81 @@
+# 1. ROS节点参数 =============================================================
+odom_topic: "/sim/odom"
+depth_topic: "/depth_image"
+lidar_topic: "/lidar_points"
+
+render_depth: true
+depth_fps: 30
+
+render_lidar: true
+lidar_fps: 10
+
+# 2. 仿真环境参数 =============================================================
+random_map: true       # 使用预先建立的点云地图还是随机地图
+resolution: 0.1        # 地图分辨率
+
+# 2.1 预先建立的点云地图 (此分支已弃用)
+ply_file: "src/pointcloud/forest.ply"
+expand_x_times: 0      # x方向复制次数（cuda版不使用）
+expand_y_times: 0      # y方向复制次数（cuda版不使用）
+occupy_threshold: 0    # 大于多少个点才认为占据（cpu版不使用）
+
+# 2.2 随机地图
+seed: 1
+x_length: 60           # 真正随机范围(XY更大范围处为其镜像)
+y_length: 60
+z_length: 15
+maze_type: 5           # 1: 溶洞 2: 柱子 3:迷宫 5:森林 6:房间  (当前策略支持1,2,5)
+# 溶洞
+complexity: 0.02       # 复杂度 0.0-0.5
+fill: 0.1              # 填充率 0.0-0.4
+fractal: 1
+attenuation: 0.1
+# 柱子
+width_min: 0.6
+width_max: 1.5
+obstacle_number: 100
+# 迷宫
+road_width: 3
+add_wall_x: 1
+add_wall_y: 1
+# 森林
+tree_file: "src/pointcloud/tree.ply"
+tree_dist: 4
+# 房间
+room_number: 4         # 每个方向上的房间数 (房间大小 = x_length / room_number)
+max_windows: 2         # 每面墙最多窗口数
+window_size_min: 2.0
+window_size_max: 2.8
+add_ceiling: 0         # 是否添加天花板
+
+# 3. 传感器参数 ==============================================================
+camera:
+  fx: 80.0             # focal length x
+  fy: 80.0             # focal length y
+  cx: 80.0             # principal point x (image center)
+  cy: 45.0              # principal point y (image center)
+  image_width: 160
+  image_height: 90
+  max_depth_dist: 20.0
+  normalize_depth: false
+  pitch: -0.0           # 相机相对机体的俯仰角 (仰为负, cpu版尚未添加)
+
+lidar:
+  vertical_lines: 16             # 纵向16线
+  vertical_angle_start: -15.0    # 起始垂直角度
+  vertical_angle_end: 15.0       # 结束垂直角度
+  horizontal_num: 360            # 水平360点
+  horizontal_resolution: 1.0     # 水平分辨率为1度，horizontal_resolution * horizontal_num = 360
+  max_lidar_dist: 20.0
+
+# 4. 数据收集参数 =============================================================
+save_path: "../dataset/"
+env_num: 10             # 地图数量
+image_num: 10000        # 每个地图收集图像数量
+roll_range: 30          # 采样点的roll角度范围
+pitch_range: 30         # 采样点的pitch角度范围
+x_range: 40             # x方向采样范围 (请小于地图大小x_length并保留一定边缘间距)
+y_range: 40             # y方向采样范围 (请小于地图大小y_length并保留一定边缘间距)
+z_range: [0.5, 4]       # z方向采样范围 (请小于地图大小z_length并保留一定边缘间距)
+safe_dist: 0.5          # 安全距离，采样点与障碍的最小距离
+ply_res: 0.1            # 保存点云滤波的体素网格大小

Simulator/src/include/cuda_toolkit/matrix.cuh

@@ -0,0 +1,103 @@
+// This file is part of REMODE - REgularized MOnocular Depth Estimation.
+//
+// Copyright (C) 2014 Matia Pizzoli <matia dot pizzoli at gmail dot com>
+// Robotics and Perception Group, University of Zurich, Switzerland
+// http://rpg.ifi.uzh.ch
+//
+// REMODE 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 any later version.
+//
+// REMODE 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/>.
+
+#pragma once
+
+#include <cuda_runtime.h>
+#include <ostream>
+#include <iomanip>
+
+namespace cudaMat {
+template<typename Type, unsigned R, unsigned C>
+struct Matrix
+{
+  __host__ __device__ __forceinline__
+  Type operator()(int row, int col) const
+  {
+    return data[row*C+col];
+  }
+
+  __host__ __device__ __forceinline__
+  Type & operator()(int row, int col)
+  {
+    return data[row*C+col];
+  }
+
+  __host__ __device__ __forceinline__
+  Type operator[](int ind) const
+  {
+    return data[ind];
+  }
+
+  __host__ __device__ __forceinline__
+  Type & operator[](int ind)
+  {
+    return data[ind];
+  }
+
+  __host__
+  friend std::ostream & operator<<(std::ostream &out, const Matrix<Type, R, C> &m)
+  {
+    for(size_t row=0; row<R; ++row)
+    {
+      for(size_t col=0; col<C; ++col)
+      {
+        out << std::setprecision(9) << m(row, col) << " ";
+      }
+      out << std::endl;
+    }
+    return out;
+  }
+
+  Type data[R*C];
+};
+
+template<typename Type, unsigned R, unsigned CR, unsigned C>
+__host__ __device__ __forceinline__
+Matrix<Type, R, C> operator*(const Matrix<Type, R, CR> & lhs,
+                             const Matrix<Type, CR, C> & rhs)
+{
+  Matrix<Type, R, C> result;
+  for(size_t row=0; row<R; ++row)
+  {
+    for(size_t col=0; col<C; ++col)
+    {
+      result(row, col) = 0;
+      for(size_t i=0; i<CR; ++i)
+      {
+        result(row, col) += lhs(row,i) * rhs(i,col);
+      }
+    }
+  }
+  return result;
+}
+
+template<typename Type>
+__host__ __device__ __forceinline__
+Matrix<Type, 2, 2> inv(const Matrix<Type, 2, 2> & in)
+{
+  Matrix<Type, 2, 2> out;
+  float det = in[0]*in[3] - in[1]*in[2];
+  out[0] =  in[3] / det;
+  out[1] = -in[1] / det;
+  out[2] = -in[2] / det;
+  out[3] =  in[0] / det;
+  return out;
+}
+
+}
+

Simulator/src/include/cuda_toolkit/se3.cuh

@@ -0,0 +1,202 @@
+// This file is part of REMODE - REgularized MOnocular Depth Estimation.
+//
+// Copyright (C) 2014 Matia Pizzoli <matia dot pizzoli at gmail dot com>
+// Robotics and Perception Group, University of Zurich, Switzerland
+// http://rpg.ifi.uzh.ch
+//
+// REMODE 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 any later version.
+//
+// REMODE 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/>.
+
+#pragma once
+
+#include "./helper_math.h"
+#include "./matrix.cuh"
+
+namespace cudaMat {
+template<typename Type>
+struct SE3
+{
+  __host__ __device__ __forceinline__
+  SE3()
+  {
+    data(0, 0) = (Type)1;
+    data(0, 1) = 0;
+    data(0, 2) = 0;
+    data(1, 0) = 0;
+    data(1, 1) = (Type)1;
+    data(1, 2) = 0;
+    data(2, 0) = 0;
+    data(2, 1) = 0;
+    data(2, 2) = (Type)1;
+
+    data(0, 3) = 0;
+    data(1, 3) = 0;
+    data(2, 3) = 0;
+  }
+
+  /// Constructor from a normalized quaternion and a translation vector
+  __host__ __device__ __forceinline__
+  SE3(Type qw, Type qx, Type qy, Type qz, Type tx, Type ty, Type tz)
+  {
+    const Type x  = 2*qx;
+    const Type y  = 2*qy;
+    const Type z  = 2*qz;
+    const Type wx = x*qw;
+    const Type wy = y*qw;
+    const Type wz = z*qw;
+    const Type xx = x*qx;
+    const Type xy = y*qx;
+    const Type xz = z*qx;
+    const Type yy = y*qy;
+    const Type yz = z*qy;
+    const Type zz = z*qz;
+
+    data(0, 0) = 1-(yy+zz);
+    data(0, 1) = xy-wz;
+    data(0, 2) = xz+wy;
+    data(1, 0) = xy+wz;
+    data(1, 1) = 1-(xx+zz);
+    data(1, 2) = yz-wx;
+    data(2, 0) = xz-wy;
+    data(2, 1) = yz+wx;
+    data(2, 2) = 1-(xx+yy);
+
+    data(0, 3) = tx;
+    data(1, 3) = ty;
+    data(2, 3) = tz;
+  }
+
+  /// Construct from C arrays
+  /// r is rotation matrix row major
+  /// t is the translation vector (x y z)
+  __host__ __device__ __forceinline__
+  SE3(Type *r, Type *t)
+  {
+    data[0]=r[0]; data[1]=r[1]; data[2] =r[2]; data[3] =t[0];
+    data[4]=r[3]; data[5]=r[4]; data[6] =r[5]; data[7] =t[1];
+    data[8]=r[6]; data[9]=r[7]; data[10]=r[8]; data[11]=t[2];
+  }
+
+  __host__ __device__ __forceinline__
+  SE3<Type> inv() const
+  {
+    SE3<Type> result;
+    result.data[0]  = data[0];
+    result.data[1]  = data[4];
+    result.data[2]  = data[8];
+    result.data[4]  = data[1];
+    result.data[5]  = data[5];
+    result.data[6]  = data[9];
+    result.data[8]  = data[2];
+    result.data[9]  = data[6];
+    result.data[10] = data[10];
+    result.data[3]  = -data[0]*data[3] -data[4]*data[7] -data[8] *data[11];
+    result.data[7]  = -data[1]*data[3] -data[5]*data[7] -data[9] *data[11];
+    result.data[11] = -data[2]*data[3] -data[6]*data[7] -data[10]*data[11];
+    return result;
+  }
+
+  __host__ __device__ __forceinline__
+  Type operator()(int r, int c) const
+  {
+    return data(r, c);
+  }
+
+  __host__ __device__ __forceinline__
+  Type & operator()(int r, int c)
+  {
+    return data(r, c);
+  }
+
+  __host__ __device__ __forceinline__
+  float3 rotate(const float3 &p) const
+  {
+    return make_float3(data(0,0)*p.x + data(0,1)*p.y + data(0,2)*p.z,
+                       data(1,0)*p.x + data(1,1)*p.y + data(1,2)*p.z,
+                       data(2,0)*p.x + data(2,1)*p.y + data(2,2)*p.z);
+  }
+
+  __host__ __device__ __forceinline__
+  float rotate_1_row(const float3 &p) const
+  {
+    return (data(0,0)*p.x + data(0,1)*p.y + data(0,2)*p.z);
+  }
+  __host__ __device__ __forceinline__
+  float rotate_2_row(const float3 &p) const
+  {
+    return (data(1,0)*p.x + data(1,1)*p.y + data(1,2)*p.z);
+  }
+  __host__ __device__ __forceinline__
+  float rotate_3_row(const float3 &p) const
+  {
+    return (data(2,0)*p.x + data(2,1)*p.y + data(2,2)*p.z);
+  }
+
+  __host__ __device__ __forceinline__
+  float3 translate(const float3 &p) const
+  {
+    return make_float3(p.x + data(0,3),
+                       p.y + data(1,3),
+                       p.z + data(2,3));
+  }
+
+  __host__ __device__ __forceinline__
+  float3 getTranslation() const
+  {
+    return make_float3(data(0,3),
+                       data(1,3),
+                       data(2,3));
+  }
+
+  __host__ __device__ __forceinline__
+  float3 getinvTranslation() const
+  {
+    SE3<Type> inv_ = this->inv();
+    return inv_.getTranslation();
+  }
+  __host__
+  friend std::ostream & operator<<(std::ostream &out, const SE3 &m)
+  {
+    out << m.data;
+    return out;
+  }
+
+  Matrix<Type, 3, 4> data;
+};
+
+template<typename Type>
+__host__ __device__ __forceinline__
+SE3<Type> operator*(const SE3<Type> &lhs, const SE3<Type> &rhs)
+{
+  SE3<Type> result;
+  result.data[0]  = lhs.data[0]*rhs.data[0] + lhs.data[1]*rhs.data[4] + lhs.data[2]*rhs.data[8];
+  result.data[1]  = lhs.data[0]*rhs.data[1] + lhs.data[1]*rhs.data[5] + lhs.data[2]*rhs.data[9];
+  result.data[2]  = lhs.data[0]*rhs.data[2] + lhs.data[1]*rhs.data[6] + lhs.data[2]*rhs.data[10];
+  result.data[3]  = lhs.data[3] + lhs.data[0]*rhs.data[3] + lhs.data[1]*rhs.data[7] + lhs.data[2]*rhs.data[11];
+  result.data[4]  = lhs.data[4]*rhs.data[0] + lhs.data[5]*rhs.data[4] + lhs.data[6]*rhs.data[8];
+  result.data[5]  = lhs.data[4]*rhs.data[1] + lhs.data[5]*rhs.data[5] + lhs.data[6]*rhs.data[9];
+  result.data[6]  = lhs.data[4]*rhs.data[2] + lhs.data[5]*rhs.data[6] + lhs.data[6]*rhs.data[10];
+  result.data[7]  = lhs.data[7] + lhs.data[4]*rhs.data[3] + lhs.data[5]*rhs.data[7] + lhs.data[6]*rhs.data[11];
+  result.data[8]  = lhs.data[8]*rhs.data[0] + lhs.data[9]*rhs.data[4] + lhs.data[10]*rhs.data[8];
+  result.data[9]  = lhs.data[8]*rhs.data[1] + lhs.data[9]*rhs.data[5] + lhs.data[10]*rhs.data[9];
+  result.data[10] = lhs.data[8]*rhs.data[2] + lhs.data[9]*rhs.data[6] + lhs.data[10]*rhs.data[10];
+  result.data[11] = lhs.data[11] + lhs.data[8]*rhs.data[3] + lhs.data[9]*rhs.data[7] + lhs.data[10]*rhs.data[11];
+  return result;
+}
+
+__host__ __device__ __forceinline__
+float3 operator*(const SE3<float> &se3, const float3 &p)
+{
+  return se3.translate(se3.rotate(p));
+}
+}
+
+

Simulator/src/include/maps.hpp

@@ -0,0 +1,106 @@
+#ifndef MAPS_HPP
+#define MAPS_HPP
+#include <yaml-cpp/yaml.h>
+#include <pcl/point_cloud.h>
+#include <pcl/io/ply_io.h>
+#include <pcl/common/transforms.h>
+#include <pcl/point_types.h>
+#include <pcl/common/common.h>
+#include <pcl_conversions/pcl_conversions.h>
+
+#include <algorithm>
+#include <iostream>
+#include <random>
+#include <vector>
+#include <Eigen/Core>
+#include "perlinnoise.hpp"
+
+namespace mocka {
+
+class Maps {
+public:
+  typedef struct BasicInfo {
+    int sizeX;
+    int sizeY;
+    int sizeZ;
+    int seed;
+    double scale;
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
+  } BasicInfo;
+
+  BasicInfo getInfo() const;
+  void setInfo(const BasicInfo &value);
+  void setParam(const YAML::Node& config);
+  Maps() {}
+  void generate(int type);
+
+private:
+  BasicInfo info;
+  // perlin3D
+  double complexity;
+  double fill;
+  int    fractal;
+  double attenuation;
+  // randomMap
+  double _w_l, _w_h;
+  int    _ObsNum;
+  // maze2D
+  double width;
+  int    addWallX;
+  int    addWallY;
+  // tree
+  std::string tree_file;
+  double tree_dist;
+  // room
+  int room_number;
+  int max_windows;
+  int add_ceiling;
+  double window_size_min, window_size_max;
+
+  std::uniform_real_distribution<double> dis_window_x, dis_window_z, dis_window_size;
+  std::default_random_engine window_eng;
+
+  void perlin3D();
+  void maze2D();
+  void randomMapGenerate();
+  void Maze3DGen();
+  void recursiveDivision(int xl, int xh, int yl, int yh, Eigen::MatrixXi &maze);
+  void recursizeDivisionMaze(Eigen::MatrixXi &maze);
+  void optimizeMap();
+
+  void forest();
+  void generatePoissonPoints(float map_width, float map_height, float dist, std::vector<Eigen::Vector2f> &positions);
+  void scaleAndTranslateCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, float scale_factor, Eigen::Vector2f position, Eigen::Matrix3f &rotation);
+  pcl::PointCloud<pcl::PointXYZ>::Ptr generateGround(const pcl::PointCloud<pcl::PointXYZ>::Ptr &forest_cloud, float grid_size, float hight = 0.0);
+
+  void room();
+  void transformPointCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud,
+                           const Eigen::Matrix3f &rotation, const Eigen::Vector3f &translation);
+  void generateWallWithWindows(pcl::PointCloud<pcl::PointXYZ>::Ptr wall, float L, float W, float H, int num_windows);
+};
+
+class MazePoint {
+private:
+  pcl::PointXYZ point;
+  double dist1;
+  double dist2;
+  int point1;
+  int point2;
+  bool isdoor;
+
+public:
+  pcl::PointXYZ getPoint();
+  int getPoint1();
+  int getPoint2();
+  double getDist1();
+  double getDist2();
+  void setPoint(pcl::PointXYZ p);
+  void setPoint1(int p);
+  void setPoint2(int p);
+  void setDist1(double set);
+  void setDist2(double set);
+};
+
+} // namespace mocka
+
+#endif // MAPS_HPP

Simulator/src/include/perlinnoise.hpp

@@ -0,0 +1,33 @@
+#ifndef PERLINNOISE_HPP
+#define PERLINNOISE_HPP
+
+#include <vector>
+
+// THIS CLASS IS A TRANSLATION TO C++11 FROM THE REFERENCE
+// JAVA IMPLEMENTATION OF THE IMPROVED PERLIN FUNCTION (see
+// http://mrl.nyu.edu/~perlin/noise/)
+// THE ORIGINAL JAVA IMPLEMENTATION IS COPYRIGHT 2002 KEN PERLIN
+
+// I ADDED AN EXTRA METHOD THAT GENERATES A NEW PERMUTATION VECTOR (THIS IS NOT
+// PRESENT IN THE ORIGINAL IMPLEMENTATION)
+
+class PerlinNoise
+{
+  // The permutation vector
+  std::vector<int> p;
+
+public:
+  // Initialize with the reference values for the permutation vector
+  PerlinNoise();
+  // Generate a new permutation vector based on the value of seed
+  PerlinNoise(unsigned int seed);
+  // Get a noise value, for 2D images z can have any value
+  double noise(double x, double y, double z);
+
+private:
+  double fade(double t);
+  double lerp(double t, double a, double b);
+  double grad(int hash, double x, double y, double z);
+};
+
+#endif // PERLINNOISE_HPP

Simulator/src/include/sensor_simulator.cuh

@@ -0,0 +1,84 @@
+#ifndef CUDA_UTILS_CUH
+#define CUDA_UTILS_CUH
+
+#include <cuda_runtime.h>
+#include "cuda_toolkit/se3.cuh"
+#include <cmath>
+#include <vector>
+#include <iostream>
+#include <opencv2/opencv.hpp>
+#include <pcl/common/common.h> // For pcl::getMinMax3D
+#include <pcl/point_cloud.h>   // For pcl::PointCloud
+#include <pcl/point_types.h>   // For pcl::PointXYZ
+#include <chrono>
+
+namespace raycast
+{
+    struct Vector3f
+    {
+        float x, y, z;
+        __device__ __host__ Vector3f() : x(0.0f), y(0.0f), z(0.0f) {}
+        __device__ __host__ Vector3f(float x_val, float y_val, float z_val)
+            : x(x_val), y(y_val), z(z_val) {}
+    };
+
+    struct Vector3i
+    {
+        int x, y, z;
+        __device__ __host__ Vector3i() : x(0), y(0), z(0) {}
+        __device__ __host__ Vector3i(int x_val, int y_val, int z_val)
+            : x(x_val), y(y_val), z(z_val) {}
+    };
+
+    struct CameraParams
+    {
+        float fx = 80.0f; // focal length x
+        float fy = 80.0f; // focal length y
+        float cx = 80.0f; // principal point x (image center)
+        float cy = 45.0f; // principal point y (image center)
+        int image_width = 160;
+        int image_height = 90;
+        float max_depth_dist{20};
+        bool normalize_depth{false};
+    };
+
+    struct LidarParams
+    {
+        int vertical_lines = 16;            // 纵向16线
+        float vertical_angle_start = -15.0; // 起始垂直角度
+        float vertical_angle_end = 15.0;    // 结束垂直角度
+        int horizontal_num = 360;           // 水平360点
+        float horizontal_resolution = 1.0;  // 水平分辨率为1度
+        float max_lidar_dist{50};
+    };
+
+    class GridMap
+    {
+        public:
+            GridMap(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, float resolution, int occupy_threshold);
+            ~GridMap() {};
+            void freeGridMap() {cudaFree(map_cuda_);}
+            __host__ __device__ Vector3i Pos2Vox(const Vector3f &pos);
+            __host__ __device__ Vector3f Vox2Pos(const Vector3i &vox);
+            __host__ __device__ int Vox2Idx(const Vector3i &vox);
+            __host__ __device__ Vector3i Idx2Vox(int idx);
+            __device__ int symmetricIndex(int index, int length);
+            __device__ int mapQuery(const Vector3f &pos);
+
+            float raycast_step_; // raycast step
+        private:
+            // map param
+            int *map_cuda_;
+            float resolution_;                                           // grid resolution
+            float origin_x_, origin_y_, origin_z_;                       // origin coordinates
+            int grid_size_x_, grid_size_y_, grid_size_z_, grid_size_yz_; // grid sizes
+            int occupy_threshold_;                                        // occupancy threshold
+    };
+
+    __global__ void cameraRaycastKernel(float *depth_values, GridMap grid_map, CameraParams camera_param, cudaMat::SE3<float> T_wc);
+    __global__ void lidarRaycastKernel(Vector3f* point_values, GridMap grid_map, LidarParams lidar_param, cudaMat::SE3<float> T_wc);
+
+    void renderDepthImage(GridMap *grid_map, CameraParams *camera_param, cudaMat::SE3<float>& T_wc, cv::Mat &depth_image);
+    void renderLidarPointcloud(GridMap *grid_map, LidarParams *lidar_param, cudaMat::SE3<float>& T_wc, pcl::PointCloud<pcl::PointXYZ>& lidar_points);
+}
+#endif // CUDA_UTILS_CUH

Simulator/src/include/sensor_simulator.h

@@ -0,0 +1,144 @@
+#ifndef SENSOR_SIMULATOR_H
+#define SENSOR_SIMULATOR_H
+#include <pcl/io/pcd_io.h>
+#include <pcl/io/ply_io.h>
+#include <pcl/point_cloud.h>
+#include <pcl/common/common.h>
+#include <pcl/common/eigen.h>
+#include <pcl/octree/octree_search.h>
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+#include <opencv2/opencv.hpp>
+#include <ros/ros.h>
+#include <nav_msgs/Odometry.h>
+#include <sensor_msgs/Image.h>
+#include <pcl_ros/point_cloud.h>
+#include <cv_bridge/cv_bridge.h>
+#include <iostream>
+#include <vector>
+#include <chrono>
+#include <omp.h>
+#include <yaml-cpp/yaml.h>
+
+class SensorSimulator {
+public:
+    SensorSimulator(ros::NodeHandle &nh) : nh_(nh) {
+        YAML::Node config = YAML::LoadFile(CONFIG_FILE_PATH);
+        // 读取camera参数
+        fx = config["camera"]["fx"].as<float>();
+        fy = config["camera"]["fy"].as<float>();
+        cx = config["camera"]["cx"].as<float>();
+        cy = config["camera"]["cy"].as<float>();
+        image_width = config["camera"]["image_width"].as<int>();
+        image_height = config["camera"]["image_height"].as<int>();
+        max_depth_dist = config["camera"]["max_depth_dist"].as<float>();
+        normalize_depth = config["camera"]["normalize_depth"].as<bool>();
+
+        // 读取lidar参数
+        vertical_lines = config["lidar"]["vertical_lines"].as<int>();
+        vertical_angle_start = config["lidar"]["vertical_angle_start"].as<float>();
+        vertical_angle_end = config["lidar"]["vertical_angle_end"].as<float>();
+        horizontal_num = config["lidar"]["horizontal_num"].as<int>();
+        horizontal_resolution = config["lidar"]["horizontal_resolution"].as<float>();
+        max_lidar_dist = config["lidar"]["max_lidar_dist"].as<float>();
+
+        render_lidar = config["render_lidar"].as<bool>();
+        render_depth = config["render_depth"].as<bool>();
+        float depth_fps = config["depth_fps"].as<float>();
+        float lidar_fps = config["lidar_fps"].as<float>();
+
+        std::string ply_file = config["ply_file"].as<std::string>();
+        std::string odom_topic = config["odom_topic"].as<std::string>();
+        std::string depth_topic = config["depth_topic"].as<std::string>();
+        std::string lidar_topic = config["lidar_topic"].as<std::string>();
+
+        float resolution = config["resolution"].as<float>();
+        int expand_y_times = config["expand_y_times"].as<int>();
+        int expand_x_times = config["expand_x_times"].as<int>();
+
+        pcl::PointCloud<pcl::PointXYZ>::Ptr orig_cloud(new pcl::PointCloud<pcl::PointXYZ>());
+        printf("1.Reading Point Cloud... \n");
+
+        // if (pcl::io::loadPCDFile("/home/lu/用完删除/test/map.pcd", *cloud) == -1) {
+        //     PCL_ERROR("Couldn't read PLY file \n");
+        //     return;
+        // }
+
+        if (pcl::io::loadPLYFile(ply_file, *orig_cloud) == -1) {
+            PCL_ERROR("Couldn't read PLY file \n");
+            return;
+        }
+        
+        printf("2.Processing... \n");
+
+        cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>());
+        *cloud = *orig_cloud;
+        for (int i = 0; i < expand_x_times; ++i) {
+            expand_cloud(cloud, 0);
+        }
+        for (int i = 0; i < expand_y_times; ++i) {
+            expand_cloud(cloud, 1);
+        }
+
+        octree = pcl::octree::OctreePointCloudSearch<pcl::PointXYZ>::Ptr(
+            new pcl::octree::OctreePointCloudSearch<pcl::PointXYZ>(resolution));
+        octree->setInputCloud(cloud);
+        octree->addPointsFromInputCloud();
+
+        image_pub_ = nh_.advertise<sensor_msgs::Image>(depth_topic, 1);
+        point_cloud_pub_ = nh_.advertise<sensor_msgs::PointCloud2>(lidar_topic, 1);
+        odom_sub_ = nh_.subscribe(odom_topic, 1, &SensorSimulator::odomCallback, this);
+        timer_depth_ = nh_.createTimer(ros::Duration(1 / depth_fps), &SensorSimulator::timerDepthCallback, this);
+        timer_lidar_ = nh_.createTimer(ros::Duration(1 / lidar_fps), &SensorSimulator::timerLidarCallback, this);
+        printf("3.Simulation Ready! \n");
+        ros::spin();
+    }
+
+    void odomCallback(const nav_msgs::Odometry::ConstPtr &msg);
+
+    cv::Mat renderDepthImage();
+
+    pcl::PointCloud<pcl::PointXYZ> renderLidarPointcloud();
+
+    void timerDepthCallback(const ros::TimerEvent &);
+
+    void timerLidarCallback(const ros::TimerEvent &);
+
+    void expand_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr expanded_cloud, int direction = 0);
+
+private:
+    bool render_depth{false};
+    bool render_lidar{false};
+    bool odom_init{false};
+    Eigen::Quaternionf quat;
+    Eigen::Vector3f pos;
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
+    pcl::octree::OctreePointCloudSearch<pcl::PointXYZ>::Ptr octree;
+
+    // camera param
+    float fx = 80.0f; // focal length x
+    float fy = 80.0f; // focal length y
+    float cx = 80.0f; // principal point x (image center)
+    float cy = 45.0f; // principal point y (image center)
+    int image_width = 160;
+    int image_height = 90;
+    float max_depth_dist{20};
+    bool normalize_depth{false};
+
+    // lidar param
+    int vertical_lines = 16;            // 纵向16线
+    float vertical_angle_start = -15.0; // 起始垂直角度
+    float vertical_angle_end = 15.0;    // 结束垂直角度
+    int horizontal_num = 360;           // 水平360点
+    float horizontal_resolution = 1.0;  // 水平分辨率为1度
+    float max_lidar_dist{50};
+
+
+    ros::NodeHandle nh_;
+    ros::Publisher image_pub_, point_cloud_pub_;
+    ros::Subscriber odom_sub_;
+    ros::Timer timer_depth_, timer_lidar_;
+};
+
+
+#endif //SENSOR_SIMULATOR_H

Simulator/src/package.xml

@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<package format="2">
+    <name>sensor_simulator</name>
+    <version>0.0.1</version>
+    <description>A simple ROS node that publishes odometry with quaternion orientation using AngleAxis for rotations.</description>
+
+    <maintainer email="your_email@example.com">Your Name</maintainer>
+    <license>BSD</license>
+
+    <!-- Specify dependencies on other catkin packages -->
+    <buildtool_depend>catkin</buildtool_depend>
+    <build_depend>roscpp</build_depend>
+    <build_depend>nav_msgs</build_depend>
+    <build_depend>std_msgs</build_depend>
+
+    <exec_depend>roscpp</exec_depend>
+    <exec_depend>nav_msgs</exec_depend>
+    <exec_depend>std_msgs</exec_depend>
+
+    <!-- Eigen is commonly available, but if not, ensure its installation -->
+    <build_depend>eigen</build_depend>
+    <exec_depend>eigen</exec_depend>
+
+    <export>
+        <!-- Other tools can request additional information be placed here -->
+    </export>
+</package>

Simulator/src/readme.md

@@ -0,0 +1,102 @@
+# 真实环境点云/深度图仿真(支持CUDA)
+
+### 1 依赖
+
+CUDA; ROS; OpenCV; PCL; (如果已经安装ROS依赖基本都满足) yaml-cpp
+```angular2html
+sudo apt-get install libyaml-cpp-dev
+```
+
+### 2 编译
+```angular2html
+catkin build
+```
+
+### 3 运行
+```angular2html
+source devel/setup.bash
+# CPU版本 (已弃用)
+rosrun sensor_simulator sensor_simulator
+# GPU版本 (推荐, RTX 3060 深度输出 > 1000fps)
+rosrun sensor_simulator sensor_simulator_cuda
+```
+
+传感器参数以及点云环境修改见[config](config/config.yaml)，重要参数说明:
+```
+# 一些话题
+odom_topic: "/sim/odom"
+depth_topic: "/depth_image"
+lidar_topic: "/lidar_points"
+# 使用预先构建的点云地图还是随机地图
+random_map: true
+# 点云地图文件
+ply_file:
+# 随机地图配置  
+maze_type: 5   # 1: 溶洞 2: 柱子 3:迷宫 5:森林(也需设置树的点云文件) 6:房间
+```
+
+如果使用预先构建的点云地图，可下载我们收集的一个树林的示例: [谷歌云盘](https://drive.google.com/file/d/1WT3vh0m7Gjn0mt4ri-D35mVDgRCT0mNc/view?usp=sharing)
+
+### 4 仿真位置发布与简单可视化（可选）
+```angular2html
+cd src/sensor_simulator
+python sim_odom.py
+
+cd src/sensor_simulator
+rviz -d rviz.rviz
+```
+
+
+### 5 实时性与资源占用
+
+cpu 版本 (i7-9700)：
+深度图0.02s, 点云0.01s
+
+gpu 版本 (RTX 3060)：
+深度图0.001s, 点云0.001s
+
+GPU版资源占用(开30HZ)：
+![Demo GIF](img/resource.png)
+
+### 6 示例场景
+
+<table>
+  <tr>
+    <td align="center">
+      <img src="img/forest.png" alt="Image 1" style="width:100%;"/>
+      <p>1. realworld forest</p>
+    </td>
+    <td align="center">
+      <img src="img/building.png" alt="Image 2" style="width:82%;"/>
+      <p>2. realworld building</p>
+    </td>
+  </tr>
+  <tr>
+    <td align="center">
+      <img src="img/perlin3D.png" alt="Image 3" style="width:100%;"/>
+      <p>3. 3D perlin</p>
+    </td>
+    <td align="center">
+      <img src="img/random_forest.png" alt="Image 4"style="width:100%;"/>
+      <p>4. random forest</p>
+    </td>
+  </tr>
+  <tr>
+    <td align="center">
+      <img src="img/random_room.png" alt="Image 5" style="width:100%;"/>
+      <p>5. random room</p>
+    </td>
+    <td align="center">
+      <img src="img/random_maze.png" alt="Image 6" style="width:100%;"/>
+      <p>6. random maze</p>
+    </td>
+  </tr>
+</table>
+
+**注释:**
+
+1. GPU版本地图无边界可无限延伸; CPU版本地图有边界（可选择复制地图几份，已弃用）
+
+### acknowledgment
+
+Some maps (3D Perlin, random maze) are generated based on: https://github.com/HKUST-Aerial-Robotics/mockamap, thanks for their excellent work!

Simulator/src/rviz.rviz

@@ -0,0 +1,197 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+      Splitter Ratio: 0.5
+    Tree Height: 286
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Name: Time
+    SyncMode: 0
+    SyncSource: PointCloud2
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: false
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: false
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 4.587052345275879
+        Min Value: -1.6129478216171265
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: AxisColor
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Min Color: 0; 0; 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.10000000149011612
+      Style: Points
+      Topic: /lidar_points
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Class: rviz/Image
+      Enabled: true
+      Image Topic: /depth_image
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Image
+      Normalize Range: true
+      Queue Size: 2
+      Transport Hint: raw
+      Unreliable: false
+      Value: true
+    - Alpha: 1
+      Class: rviz/Axes
+      Enabled: true
+      Length: 1
+      Name: Axes
+      Radius: 0.10000000149011612
+      Reference Frame: <Fixed Frame>
+      Show Trail: false
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 11.853899955749512
+        Min Value: -0.3999969959259033
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: AxisColor
+      Decay Time: 0
+      Enabled: false
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Min Color: 0; 0; 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.10000000149011612
+      Style: Boxes
+      Topic: /mock_map
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: false
+  Enabled: true
+  Global Options:
+    Background Color: 50; 50; 50
+    Default Light: true
+    Fixed Frame: odom
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
+      Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 44.84852600097656
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Field of View: 0.7853981852531433
+      Focal Point:
+        X: 2.0842127799987793
+        Y: -1.169637680053711
+        Z: 2.9521546363830566
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 1.5697963237762451
+      Target Frame: <Fixed Frame>
+      Yaw: 3.250494956970215
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1016
+  Hide Left Dock: false
+  Hide Right Dock: true
+  Image:
+    collapsed: false
+  QMainWindow State: 000000ff00000000fd0000000400000000000002a20000035afc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001a9000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000001ec000001ab0000001600ffffff000000010000010f00000375fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000375000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d0065010000000000000738000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000004900000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1848
+  X: 72
+  Y: 387

Simulator/src/sim_odom.py

@@ -0,0 +1,54 @@
+#!/usr/bin/env python
+import rospy
+from nav_msgs.msg import Odometry
+from geometry_msgs.msg import Point, Quaternion
+import tf
+
+def publish_odometry():
+    rospy.init_node('odom_publisher', anonymous=True)
+    odom_pub = rospy.Publisher('/sim/odom', Odometry, queue_size=10)
+    rate = rospy.Rate(30)  # 30 Hz
+
+    odom_msg = Odometry()
+    odom_msg.header.frame_id = "world"
+    odom_msg.child_frame_id = "base_link"
+
+    x_position = 0.0
+    y_position = 2.0
+    z_position = 1.6
+    velocity = 2.0  # m/s
+
+    roll = 0.0
+    pitch = 0.0
+    yaw = 0.0
+
+    start_time = rospy.Time.now()
+
+    while not rospy.is_shutdown():
+        current_time = rospy.Time.now()
+        elapsed_time = (current_time - start_time).to_sec()
+        x_position = velocity * elapsed_time  # x = v * t
+
+        # Update odometry message
+        odom_msg.header.stamp = current_time
+        odom_msg.pose.pose.position = Point(x_position, y_position, z_position)
+        q = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
+        odom_msg.pose.pose.orientation = Quaternion(*q)
+        odom_msg.twist.twist.linear.x = velocity
+        odom_msg.twist.twist.linear.y = 0
+        odom_msg.twist.twist.linear.z = 0
+        odom_msg.twist.twist.angular.x = 0
+        odom_msg.twist.twist.angular.y = 0
+        odom_msg.twist.twist.angular.z = 0
+
+        # Publish the odometry message
+        odom_pub.publish(odom_msg)
+
+        # Sleep to maintain the loop rate
+        rate.sleep()
+
+if __name__ == '__main__':
+    try:
+        publish_odometry()
+    except rospy.ROSInterruptException:
+        pass

Simulator/src/src/dataset_generator.cpp

@@ -0,0 +1,234 @@
+#include <pcl/io/ply_io.h>
+#include <pcl/point_types.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/kdtree/kdtree_flann.h>
+#include <opencv2/opencv.hpp>
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+#include <yaml-cpp/yaml.h>
+#include <iostream>
+#include <filesystem>
+#include <fstream>
+#include <iomanip>
+#include "sensor_simulator.cuh"
+#include "maps.hpp"
+
+using namespace raycast;
+namespace fs = std::filesystem;
+
+void prepareSavePath(const std::string &path, bool print=false)
+{
+    if (fs::exists(path))
+    {
+        if (print)
+            std::cout << "Directory exists. Removing: " << path << std::endl;
+        fs::remove_all(path);
+    }
+    fs::create_directories(path);
+    if (print)
+        std::cout << "Created new dataset directory: " << path << std::endl;
+}
+
+void savePointCloudAsPLY(const pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud, const std::string &path)
+{
+    if (pcl::io::savePLYFileBinary(path, *cloud) == -1)
+        std::cerr << "Failed to save ply file to " << path << std::endl;
+}
+
+void saveDepthAs16BitPNG(const cv::Mat &depth_float, float max_depth_dist, const std::string &filepath)
+{
+    cv::Mat depth_scaled;
+    depth_scaled = depth_float / max_depth_dist; // 归一化0~1
+
+    // clip [0,1]
+    cv::threshold(depth_scaled, depth_scaled, 1.0, 1.0, cv::THRESH_TRUNC);
+    cv::threshold(depth_scaled, depth_scaled, 0.0, 0.0, cv::THRESH_TOZERO);
+
+    // 转成uint16
+    depth_scaled.convertTo(depth_scaled, CV_16UC1, 65535.0);
+
+    cv::imwrite(filepath, depth_scaled);
+}
+
+Eigen::Quaternionf RPY2Quat(float roll_deg, float pitch_deg, float yaw_deg)
+{
+    float roll = roll_deg * M_PI / 180.0f;
+    float pitch = pitch_deg * M_PI / 180.0f;
+    float yaw = yaw_deg * M_PI / 180.0f;
+    Eigen::AngleAxisf rollAngle(roll, Eigen::Vector3f::UnitX());
+    Eigen::AngleAxisf pitchAngle(pitch, Eigen::Vector3f::UnitY());
+    Eigen::AngleAxisf yawAngle(yaw, Eigen::Vector3f::UnitZ());
+    return yawAngle * pitchAngle * rollAngle;
+}
+
+void printProgressBar(int current, int total, int bar_width = 50)
+{
+    float progress = static_cast<float>(current) / total;
+    int pos = static_cast<int>(bar_width * progress);
+
+    std::cout << "\r[";
+    for (int i = 0; i < bar_width; ++i)
+    {
+        if (i < pos)
+            std::cout << "=";
+        else if (i == pos)
+            std::cout << ">";
+        else
+            std::cout << " ";
+    }
+    std::cout << "] " << int(progress * 100.0f) << "%";
+    std::cout.flush();
+}
+
+int main(int argc, char **argv)
+{
+    YAML::Node config = YAML::LoadFile(CONFIG_FILE_PATH);
+
+    // 1. 相机参数
+    CameraParams camera;
+    camera.fx = config["camera"]["fx"].as<float>();
+    camera.fy = config["camera"]["fy"].as<float>();
+    camera.cx = config["camera"]["cx"].as<float>();
+    camera.cy = config["camera"]["cy"].as<float>();
+    camera.image_width = config["camera"]["image_width"].as<int>();
+    camera.image_height = config["camera"]["image_height"].as<int>();
+    camera.max_depth_dist = config["camera"]["max_depth_dist"].as<float>();
+    camera.normalize_depth = config["camera"]["normalize_depth"].as<bool>();
+    float pitch = config["camera"]["pitch"].as<float>() * M_PI / 180.0;
+    Eigen::AngleAxisf angle_axis(pitch, Eigen::Vector3f::UnitY());
+    Eigen::Quaternionf quat_bc(angle_axis);
+
+    // 2. 地图参数
+    float resolution = config["resolution"].as<float>();
+    int occupy_threshold = config["occupy_threshold"].as<int>();
+    int seed = config["seed"].as<int>();
+    int sizeX = config["x_length"].as<int>();
+    int sizeY = config["y_length"].as<int>();
+    int sizeZ = config["z_length"].as<int>();
+    double scale = 1 / resolution;
+    sizeX *= scale;
+    sizeY *= scale;
+    sizeZ *= scale;
+
+    // 3. 数据集参数
+    std::string save_path = config["save_path"].as<std::string>();
+    int env_num = config["env_num"].as<int>();
+    int image_num = config["image_num"].as<int>();
+    float roll_range = config["roll_range"].as<float>();
+    float pitch_range = config["pitch_range"].as<float>();
+    float x_range = config["x_range"].as<float>();
+    float y_range = config["y_range"].as<float>();
+    float z_min = config["z_range"][0].as<float>();
+    float z_max = config["z_range"][1].as<float>();
+    float safe_dist = config["safe_dist"].as<float>();
+    float ply_res = config["ply_res"].as<float>();
+
+    // 中心对齐，计算偏移量
+    int dataset_num = env_num * image_num;
+    float x_min = -x_range / 2.0f;
+    float y_min = -y_range / 2.0f;
+
+    std::cout << "地图范围 (m): "
+              << "X: [" << -sizeX * resolution / 2.0 << ", " << sizeX * resolution / 2.0 << "], "
+              << "Y: [" << -sizeY * resolution / 2.0 << ", " << sizeY * resolution / 2.0 << "], "
+              << "Z: [" << -sizeZ * resolution / 2.0 << ", " << sizeZ * resolution / 2.0 << "]" << std::endl;
+
+    std::cout << "采集范围 (m): "
+              << "X: [" << x_min << ", " << x_min + x_range << "], "
+              << "Y: [" << y_min << ", " << y_min + y_range << "], "
+              << "Z: [" << z_min << ", " << z_max << "]" << std::endl;
+
+    std::cout << "角度范围 (度): "
+              << "Roll: [" << -roll_range << ", " << roll_range << "], "
+              << "Pitch: [" << -pitch_range << ", " << pitch_range << "], "
+              << "Yaw: [0, 360]" << std::endl;
+
+    // 收集所有数据
+    std::default_random_engine generator(std::random_device{}());
+    std::normal_distribution<float> normal_distribution(0.0f, 1.0f); // 均值0，标准差1
+    std::uniform_real_distribution<float> uniform_uniform(0.0f, 1.0f);
+    prepareSavePath(save_path, true);
+    for (int map_i = 0; map_i < env_num; ++map_i)
+    {
+        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
+        mocka::Maps::BasicInfo info;
+        info.sizeX = sizeX;
+        info.sizeY = sizeY;
+        info.sizeZ = sizeZ;
+        info.seed = seed + map_i; // 每个环境使用不同的随机种子
+        info.scale = scale;
+        info.cloud = cloud;
+
+        mocka::Maps map;
+        map.setParam(config);
+        map.setInfo(info);
+        map.generate(config["maze_type"].as<int>());
+
+        // 构建 GridMap
+        GridMap grid_map(cloud, resolution, occupy_threshold);
+
+        // 保存地图 (先滤波)
+        pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>());
+        pcl::VoxelGrid<pcl::PointXYZ> sor;
+        sor.setInputCloud(cloud);
+        sor.setLeafSize(ply_res, ply_res, ply_res);
+        sor.filter(*filtered_cloud);
+        pcl::PointXYZ min_pt, max_pt;
+        pcl::getMinMax3D(*filtered_cloud, min_pt, max_pt);
+
+        std::string image_path = save_path + std::to_string(map_i) + "/";
+        prepareSavePath(image_path);
+
+        savePointCloudAsPLY(filtered_cloud, save_path + "pointcloud-" + std::to_string(map_i) + ".ply");
+
+        pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
+        kdtree.setInputCloud(filtered_cloud);
+
+        // 收集当前环境的数据
+        std::ofstream pose_file(save_path + "pose-" + std::to_string(map_i) + ".csv");
+        pose_file << "px,py,pz,qw,qx,qy,qz\n";
+        for (int image_i = 0; image_i < image_num; ++image_i)
+        {
+            Eigen::Vector3f pos;
+            float dist;
+            do{
+                pos.x() = x_min + uniform_uniform(generator) * x_range;
+                pos.y() = y_min + uniform_uniform(generator) * y_range;
+                pos.z() = z_min + uniform_uniform(generator) * (z_max - z_min);
+                pcl::PointXYZ searchPoint(pos.x(), pos.y(), pos.z());
+                std::vector<int> pointIdxNKNSearch(1);
+                std::vector<float> pointNKNSquaredDistance(1);
+                int found_num = kdtree.nearestKSearch(searchPoint, 1, pointIdxNKNSearch, pointNKNSquaredDistance);
+                dist = sqrt(pointNKNSquaredDistance[0]);
+            } while (dist < safe_dist);
+
+            float roll = normal_distribution(generator) * roll_range / 3.0f;   // 3 * sigmoid = range
+            float pitch = normal_distribution(generator) * pitch_range / 3.0f; // 3 * sigmoid = range
+            float yaw = uniform_uniform(generator) * 360.0f;
+
+            Eigen::Quaternionf quat = RPY2Quat(roll, pitch, yaw);
+            Eigen::Quaternionf quat_wc = quat * quat_bc;
+
+            cudaMat::SE3<float> T_wc(quat_wc.w(), quat_wc.x(), quat_wc.y(), quat_wc.z(),
+                                     pos.x(), pos.y(), pos.z());
+
+            cv::Mat depth_image;
+            renderDepthImage(&grid_map, &camera, T_wc, depth_image);
+
+            std::string filename = image_path + "/img_" + std::to_string(image_i) + ".png";
+            saveDepthAs16BitPNG(depth_image, camera.max_depth_dist, filename);
+
+            pose_file << std::fixed << std::setprecision(6)
+                      << pos.x() << "," << pos.y() << "," << pos.z() << ","
+                      << quat_wc.w() << "," << quat_wc.x() << ","
+                      << quat_wc.y() << "," << quat_wc.z() << "\n";
+
+            printProgressBar(map_i * image_num + image_i + 1, dataset_num);
+        }
+        pose_file.close();
+    }
+
+    std::cout << "\nDataset generation completed!" << std::endl;
+
+    return 0;
+}

Simulator/src/src/perlinnoise.cpp

@@ -0,0 +1,123 @@
+#include "perlinnoise.hpp"
+
+#include <algorithm>
+#include <cmath>
+#include <numeric>
+#include <random>
+
+// THIS IS A DIRECT TRANSLATION TO C++11 FROM THE REFERENCE
+// JAVA IMPLEMENTATION OF THE IMPROVED PERLIN FUNCTION (see
+// http://mrl.nyu.edu/~perlin/noise/)
+// THE ORIGINAL JAVA IMPLEMENTATION IS COPYRIGHT 2002 KEN PERLIN
+
+// I ADDED AN EXTRA METHOD THAT GENERATES A NEW PERMUTATION VECTOR (THIS IS NOT
+// PRESENT IN THE ORIGINAL IMPLEMENTATION)
+
+// Initialize with the reference values for the permutation vector
+PerlinNoise::PerlinNoise()
+{
+
+  // Initialize the permutation vector with the reference values
+  p = { 151, 160, 137, 91,  90,  15,  131, 13,  201, 95,  96,  53,  194, 233,
+        7,   225, 140, 36,  103, 30,  69,  142, 8,   99,  37,  240, 21,  10,
+        23,  190, 6,   148, 247, 120, 234, 75,  0,   26,  197, 62,  94,  252,
+        219, 203, 117, 35,  11,  32,  57,  177, 33,  88,  237, 149, 56,  87,
+        174, 20,  125, 136, 171, 168, 68,  175, 74,  165, 71,  134, 139, 48,
+        27,  166, 77,  146, 158, 231, 83,  111, 229, 122, 60,  211, 133, 230,
+        220, 105, 92,  41,  55,  46,  245, 40,  244, 102, 143, 54,  65,  25,
+        63,  161, 1,   216, 80,  73,  209, 76,  132, 187, 208, 89,  18,  169,
+        200, 196, 135, 130, 116, 188, 159, 86,  164, 100, 109, 198, 173, 186,
+        3,   64,  52,  217, 226, 250, 124, 123, 5,   202, 38,  147, 118, 126,
+        255, 82,  85,  212, 207, 206, 59,  227, 47,  16,  58,  17,  182, 189,
+        28,  42,  223, 183, 170, 213, 119, 248, 152, 2,   44,  154, 163, 70,
+        221, 153, 101, 155, 167, 43,  172, 9,   129, 22,  39,  253, 19,  98,
+        108, 110, 79,  113, 224, 232, 178, 185, 112, 104, 218, 246, 97,  228,
+        251, 34,  242, 193, 238, 210, 144, 12,  191, 179, 162, 241, 81,  51,
+        145, 235, 249, 14,  239, 107, 49,  192, 214, 31,  181, 199, 106, 157,
+        184, 84,  204, 176, 115, 121, 50,  45,  127, 4,   150, 254, 138, 236,
+        205, 93,  222, 114, 67,  29,  24,  72,  243, 141, 128, 195, 78,  66,
+        215, 61,  156, 180 };
+  // Duplicate the permutation vector
+  p.insert(p.end(), p.begin(), p.end());
+}
+
+// Generate a new permutation vector based on the value of seed
+PerlinNoise::PerlinNoise(unsigned int seed)
+{
+  p.resize(256);
+
+  // Fill p with values from 0 to 255
+  std::iota(p.begin(), p.end(), 0);
+
+  // Initialize a random engine with seed
+  std::default_random_engine engine(seed);
+
+  // Suffle  using the above random engine
+  std::shuffle(p.begin(), p.end(), engine);
+
+  // Duplicate the permutation vector
+  p.insert(p.end(), p.begin(), p.end());
+}
+
+double
+PerlinNoise::noise(double x, double y, double z)
+{
+  // Find the unit cube that contains the point
+  int X = (int)floor(x) & 255;
+  int Y = (int)floor(y) & 255;
+  int Z = (int)floor(z) & 255;
+
+  // Find relative x, y,z of point in cube
+  x -= floor(x);
+  y -= floor(y);
+  z -= floor(z);
+
+  // Compute fade curves for each of x, y, z
+  double u = fade(x);
+  double v = fade(y);
+  double w = fade(z);
+
+  // Hash coordinates of the 8 cube corners
+  int A  = p[X] + Y;
+  int AA = p[A] + Z;
+  int AB = p[A + 1] + Z;
+  int B  = p[X + 1] + Y;
+  int BA = p[B] + Z;
+  int BB = p[B + 1] + Z;
+
+  // Add blended results from 8 corners of cube
+  double res =
+    lerp(w,      //
+         lerp(v, //
+              lerp(u, grad(p[AA], x, y, z), grad(p[BA], x - 1, y, z)),
+              lerp(u, grad(p[AB], x, y - 1, z), grad(p[BB], x - 1, y - 1, z))),
+         lerp(v,                                 //
+              lerp(u,                            //
+                   grad(p[AA + 1], x, y, z - 1), //
+                   grad(p[BA + 1], x - 1, y, z - 1)),
+              lerp(u, //
+                   grad(p[AB + 1], x, y - 1, z - 1),
+                   grad(p[BB + 1], x - 1, y - 1, z - 1))));
+  return (res + 1.0) / 2.0;
+}
+
+double
+PerlinNoise::fade(double t)
+{
+  return t * t * t * (t * (t * 6 - 15) + 10);
+}
+
+double
+PerlinNoise::lerp(double t, double a, double b)
+{
+  return a + t * (b - a);
+}
+
+double
+PerlinNoise::grad(int hash, double x, double y, double z)
+{
+  int h = hash & 15;
+  // Convert lower 4 bits of hash into 12 gradient directions
+  double u = h < 8 ? x : y, v = h < 4 ? y : h == 12 || h == 14 ? x : z;
+  return ((h & 1) == 0 ? u : -u) + ((h & 2) == 0 ? v : -v);
+}

Simulator/src/src/sensor_simulator.cpp

@@ -0,0 +1,176 @@
+#include "sensor_simulator.h"
+
+cv::Mat SensorSimulator::renderDepthImage(){
+
+    cv::Mat depth_image(image_height, image_width, CV_32FC1, cv::Scalar(std::numeric_limits<float>::max()));
+    Eigen::Matrix3f R_wc = quat.toRotationMatrix();
+    Eigen::Matrix3f R_cw = R_wc.inverse();
+
+    auto start = std::chrono::high_resolution_clock::now();
+#pragma omp parallel for
+    for (int v = 0; v < image_height; ++v) {
+        for (int u = 0; u < image_width; ++u) {
+            // 计算射线方向（图像平面坐标系）
+            float y = -(u - cx) / fx;
+            float z = -(v - cy) / fy;
+            float x = 1.0f;
+            Eigen::Vector3f d(x, y, z);
+            d.normalize();
+
+            // 转换到世界坐标系下
+            Eigen::Vector3f ray_direction = R_wc * d;  // 考虑相机旋转
+            Eigen::Vector3f ray_origin = pos;         // 相机的位置
+
+            // 使用Octree查找射线方向上最近的点
+            std::vector<int> pointIdxVec;
+            if (octree->getIntersectedVoxelIndices(ray_origin, ray_direction, pointIdxVec, 1)) {
+                pcl::PointXYZ closest_point = cloud->points[pointIdxVec[0]];
+                Eigen::Vector3f point_in_world = closest_point.getVector3fMap();
+                Eigen::Vector3f closest_point_camera = R_cw * (point_in_world - pos);
+                float distance = closest_point_camera(0);
+                if (distance < 0) distance = 0;
+                if (distance > max_depth_dist) distance = max_depth_dist;
+                if (normalize_depth) distance = distance / max_depth_dist;
+                depth_image.at<float>(v, u) = distance;
+            }
+        }
+    }
+    auto end = std::chrono::high_resolution_clock::now();
+    std::chrono::duration<double> elapsed = end - start;
+    // std::cout << "生成图像耗时: " << elapsed.count() << " 秒" << std::endl; // 输出耗时
+
+    // 将无效值设置为0
+    for (int v = 0; v < image_height; ++v) {
+        for (int u = 0; u < image_width; ++u) {
+            if (depth_image.at<float>(v, u) == std::numeric_limits<float>::max())
+                depth_image.at<float>(v, u) = max_depth_dist;
+        }
+    }
+
+    // 再做一遍插值更接近真实
+    // cv::Mat resized_depth_image;
+    // cv::resize(depth_image, resized_depth_image, cv::Size(48, 27));
+
+    return depth_image;
+}
+
+pcl::PointCloud<pcl::PointXYZ> SensorSimulator::renderLidarPointcloud() {
+    Eigen::Matrix3f R_wc = quat.toRotationMatrix();
+    Eigen::Matrix3f R_cw = R_wc.inverse();
+    pcl::PointCloud<pcl::PointXYZ> lidar_points;
+    float vertical_resolution = (vertical_angle_end - vertical_angle_start) / (vertical_lines - 1);
+    std::vector<pcl::PointCloud<pcl::PointXYZ>> line_clouds(vertical_lines);
+
+    auto start = std::chrono::high_resolution_clock::now();
+#pragma omp parallel for
+    for (int v = 0; v < vertical_lines; ++v) {
+        float vertical_angle = vertical_angle_start + v * vertical_resolution;
+        float sin_vert = std::sin(vertical_angle * M_PI / 180.0);
+        float cos_vert = std::cos(vertical_angle * M_PI / 180.0);
+
+        for (int h = 0; h < horizontal_num; ++h) {
+            float horizontal_angle = h * horizontal_resolution;
+            float sin_horz = std::sin(horizontal_angle * M_PI / 180.0);
+            float cos_horz = std::cos(horizontal_angle * M_PI / 180.0);
+
+            Eigen::Vector3f ray_direction(cos_vert * cos_horz, cos_vert * sin_horz, sin_vert);
+            ray_direction = R_wc * ray_direction;
+            Eigen::Vector3f ray_origin = pos;
+
+            std::vector<int> pointIdxVec;
+            if (octree->getIntersectedVoxelIndices(ray_origin, ray_direction, pointIdxVec, 1)) {
+                pcl::PointXYZ point = cloud->points[pointIdxVec[0]];
+                Eigen::Vector3f point_in_world = point.getVector3fMap();
+                Eigen::Vector3f point_in_body = R_cw * (point_in_world - pos);
+                if (max_lidar_dist > point_in_body.norm())
+                    line_clouds[v].points.push_back(pcl::PointXYZ(point_in_body.x(), point_in_body.y(), point_in_body.z()));
+            }
+        }
+    }
+    auto end = std::chrono::high_resolution_clock::now();
+    std::chrono::duration<double> elapsed = end - start;
+    // std::cout << "生成雷达耗时: " << elapsed.count() << " 秒" << std::endl;
+
+    for (int i = 0; i < vertical_lines; ++i) {
+        lidar_points += line_clouds[i];
+    }
+    lidar_points.width = lidar_points.points.size();
+    lidar_points.height = 1;
+    lidar_points.is_dense = true;
+    return lidar_points;
+}
+
+// TODO: 不能像python那样用一个向量一次性全算吗？
+void SensorSimulator::expand_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr expanded_cloud, int direction) {
+    auto start = std::chrono::high_resolution_clock::now();
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_temp(new pcl::PointCloud<pcl::PointXYZ>());
+    *cloud_temp = *expanded_cloud;
+    pcl::PointXYZ min_point, max_point;
+    pcl::getMinMax3D(*expanded_cloud, min_point, max_point);
+
+    float min_value = (direction == 0) ? min_point.x : min_point.y;
+    float max_value = (direction == 0) ? max_point.x : max_point.y;
+
+    // 镜像原始点云并添加到扩展点云中
+    for (const auto& point : cloud_temp->points) {
+        pcl::PointXYZ mirrored_point = point;
+        if (direction == 0) {
+            mirrored_point.x = 2 * min_value - point.x;  // 以 x 轴最小值为轴进行镜像
+        } else {
+            mirrored_point.y = 2 * min_value - point.y;  // 以 y 轴最小值为轴进行镜像
+        }
+        expanded_cloud->push_back(mirrored_point);
+    }
+
+    // 计算偏移量，保证方向上的最小值为0
+    float offset = max_value - min_value;
+    for (auto& point : expanded_cloud->points) {
+        if (direction == 0) {
+            point.x += offset;
+        } else {
+            point.y += offset;
+        }
+    }
+    auto end = std::chrono::high_resolution_clock::now();
+    std::chrono::duration<double> elapsed = end - start;
+    // std::cout << "点云扩张一次耗时: " << elapsed.count() << " 秒" << std::endl; // 输出耗时
+}
+
+
+void SensorSimulator::timerDepthCallback(const ros::TimerEvent&) {
+    if (!odom_init || !render_depth)
+        return;
+    cv::Mat depth_iamge = renderDepthImage();
+    sensor_msgs::Image ros_image;
+    cv_bridge::CvImage cv_image;
+    cv_image.header.stamp = ros::Time::now();
+    cv_image.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
+    cv_image.image = depth_iamge;
+    cv_image.toImageMsg(ros_image);
+    image_pub_.publish(ros_image);
+}
+
+void SensorSimulator::timerLidarCallback(const ros::TimerEvent&) {
+    if (!odom_init || !render_lidar)
+        return;
+    pcl::PointCloud<pcl::PointXYZ> lidar_points = renderLidarPointcloud();
+    sensor_msgs::PointCloud2 output;
+    pcl::toROSMsg(lidar_points, output);
+    output.header.stamp = ros::Time::now();
+    output.header.frame_id = "world";
+
+    point_cloud_pub_.publish(output);
+}
+
+void SensorSimulator::odomCallback(const nav_msgs::Odometry::ConstPtr& msg) {
+    quat.x() = msg->pose.pose.orientation.x;
+    quat.y() = msg->pose.pose.orientation.y;
+    quat.z() = msg->pose.pose.orientation.z;
+    quat.w() = msg->pose.pose.orientation.w;
+
+    pos.x() = msg->pose.pose.position.x;
+    pos.y() = msg->pose.pose.position.y;
+    pos.z() = msg->pose.pose.position.z;
+
+    odom_init = true;
+}

Simulator/src/src/sensor_simulator.cu

@@ -0,0 +1,282 @@
+#include "sensor_simulator.cuh"
+
+namespace raycast
+{   
+    GridMap::GridMap(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, float resolution, int occupy_threshold = 1){
+        
+        Eigen::Vector4f min_pt, max_pt;
+        pcl::getMinMax3D(*cloud, min_pt, max_pt);
+        float length = max_pt(0) - min_pt(0);   // X方向的长度
+        float width  = max_pt(1) - min_pt(1);   // Y方向的宽度
+        float height = max_pt(2) - min_pt(2);   // Z方向的高度
+        Vector3f origin(min_pt(0), min_pt(1), min_pt(2));
+        Vector3f map_size(length, width, height);
+        origin_x_ = origin.x;
+        origin_y_ = origin.y;
+        origin_z_ = origin.z;
+
+        Vector3i grid_size;
+        grid_size.x = ceil(map_size.x / resolution);
+        grid_size.y = ceil(map_size.y / resolution);
+        grid_size.z = ceil(map_size.z / resolution);
+        int grid_total_size = grid_size.x * grid_size.y * grid_size.z;
+
+        resolution_   = resolution;
+        grid_size_x_  = grid_size.x, 
+        grid_size_y_  = grid_size.y, 
+        grid_size_z_  = grid_size.z, 
+        grid_size_yz_ = grid_size.y * grid_size.z;
+        occupy_threshold_ = occupy_threshold;
+        raycast_step_ = resolution;
+
+        std::vector<int> h_map(grid_total_size, 0);
+        // 有时候会有全空的行，加个很小的偏移
+        for (size_t i = 0; i < cloud->points.size(); i++) {
+            Vector3f point(cloud->points[i].x + 0.001, cloud->points[i].y + 0.001, cloud->points[i].z + 0.001);
+            int idx = Vox2Idx(Pos2Vox(point));
+            if (idx < grid_total_size) {
+                h_map[idx]++;
+            }
+        }
+        cudaMalloc((void **)&map_cuda_, grid_total_size * sizeof(int));
+        cudaMemcpy(map_cuda_, h_map.data(), grid_total_size * sizeof(int), cudaMemcpyHostToDevice);
+    }
+
+    __host__ __device__ Vector3i GridMap::Pos2Vox(const Vector3f &pos)
+    {
+        Vector3i vox;
+        vox.x = floor((pos.x - origin_x_) / resolution_);
+        vox.y = floor((pos.y - origin_y_) / resolution_);
+        vox.z = floor((pos.z - origin_z_) / resolution_);
+        return vox;
+    }
+
+    __host__ __device__ Vector3f GridMap::Vox2Pos(const Vector3i &vox)
+    {
+        Vector3f pos;
+        pos.x = (vox.x + 0.5f) * resolution_ + origin_x_;
+        pos.y = (vox.y + 0.5f) * resolution_ + origin_y_;
+        pos.z = (vox.z + 0.5f) * resolution_ + origin_z_;
+        return pos;
+    }
+
+    __host__ __device__ int GridMap::Vox2Idx(const Vector3i &vox)
+    {
+        return vox.x * grid_size_yz_ + vox.y * grid_size_z_ + vox.z;
+    }
+
+    __host__ __device__ Vector3i GridMap::Idx2Vox(int idx)
+    {
+        return Vector3i(idx / grid_size_yz_, (idx % grid_size_yz_) / grid_size_z_, idx % grid_size_z_);
+    }
+
+    __device__ int GridMap::symmetricIndex(int index, int length)
+    {
+        index = index % (2 * length - 2);
+        if (index < 0)
+        {
+            index += (2 * length - 2);
+        }
+
+        if (index >= length)
+        {
+            index = 2 * length - 2 - index;
+        }
+        return index;
+    }
+
+    // -1: z越界; 0: 空闲; 1: 占据
+    __device__  int GridMap::mapQuery(const Vector3f &pos){
+        Vector3i vox = Pos2Vox(pos);
+        vox.x = symmetricIndex(vox.x, grid_size_x_);
+        vox.y = symmetricIndex(vox.y, grid_size_y_);
+
+        if (vox.z >= grid_size_z_)
+            return 0;
+        if (vox.z <= 0)
+            return 1;
+
+        int idx = Vox2Idx(vox);
+        if (map_cuda_[idx] > occupy_threshold_)
+            return 1;
+        return 0;        
+    }
+
+    __global__ void cameraRaycastKernel(float* depth_values, GridMap grid_map, CameraParams camera_param, cudaMat::SE3<float> T_wc)
+    {
+        int u = threadIdx.x;
+        int v = blockIdx.x;
+
+        // printf("u: %d, v: %d \n", u, v);
+
+        if (u < camera_param.image_width && v < camera_param.image_height)
+        {
+            // 计算射线方向
+            float y = -(u - camera_param.cx) / camera_param.fx;
+            float z = -(v - camera_param.cy) / camera_param.fy;
+            float x = 1.0f;
+
+            // 归一化射线方向
+            float length = sqrtf(x * x + y * y + z * z);
+            x /= length;
+            y /= length;
+            z /= length;
+
+            // 计算每个轴的增量比例 (x方向固定步长避免近距离处畸变; 0.5是瞎设的防止过于稀疏导致错误)
+            float dx = 0.5 * grid_map.raycast_step_;
+            float dy = (y / x) * dx;
+            float dz = (z / x) * dx;
+
+            // 递增射线方向上的每个轴
+            int scale = 0;
+            float depth = 0.0f;
+
+            while (1)
+            {
+                scale += 1;
+
+                float point_x = scale * dx;
+                float point_y = scale * dy;
+                float point_z = scale * dz;
+
+                float3 point_c = make_float3(point_x, point_y, point_z);
+                float3 point_w = T_wc * point_c;
+
+                Vector3f point(point_w.x, point_w.y, point_w.z);
+
+                int occupied = grid_map.mapQuery(point);
+
+                if (occupied == 1)
+                {
+                    // depth = point_x;  // 直接这样赋值会有一点误差
+                    // 栅格化避免平面变曲面 (有些冗余，但在机体系栅格化会有类似摩尔纹的东西)
+                    Vector3i occ_vox_w = grid_map.Pos2Vox(point);
+                    Vector3f occ_point_w = grid_map.Vox2Pos(occ_vox_w);
+                    float3 occ_point_w_ = make_float3(occ_point_w.x, occ_point_w.y, occ_point_w.z);
+                    float3 occ_point_c_ = T_wc.inv() * occ_point_w_;
+                    depth = occ_point_c_.x;
+                    break;
+                }
+
+                if (point_x >= camera_param.max_depth_dist){
+                    depth = camera_param.max_depth_dist;
+                    break;
+                }
+            }
+
+            // 将深度值存储到输出数组中
+            if (camera_param.normalize_depth)
+                depth = depth / camera_param.max_depth_dist;
+            depth_values[v * camera_param.image_width + u] = depth;
+        }
+    }
+
+    void renderDepthImage(GridMap* grid_map, CameraParams* camera_param, cudaMat::SE3<float>& T_wc, cv::Mat& depth_image)
+    {   
+        float* depth_values;
+        size_t num_elements = camera_param->image_width * camera_param->image_height;
+        cudaMallocManaged(&depth_values, num_elements * sizeof(float));
+
+        // 在GPU上启动核函数
+        cameraRaycastKernel<<<camera_param->image_height, camera_param->image_width>>>(depth_values, *grid_map, *camera_param, T_wc);
+        
+        cudaDeviceSynchronize();
+
+        depth_image.create(camera_param->image_height, camera_param->image_width, CV_32FC1);
+
+        cudaMemcpy(depth_image.data, depth_values, num_elements * sizeof(float), cudaMemcpyDeviceToHost);
+        
+        cudaFree(depth_values);
+        return;
+    }
+
+    __global__ void lidarRaycastKernel(Vector3f* point_values, GridMap grid_map, LidarParams lidar_param, cudaMat::SE3<float> T_wc)
+    {
+        int h = threadIdx.x;
+        int v = blockIdx.x;
+
+        // printf("u: %d, v: %d \n", u, v);
+        if (h < lidar_param.horizontal_num && v < lidar_param.vertical_lines)
+        {   
+            float vertical_resolution = (lidar_param.vertical_angle_end - lidar_param.vertical_angle_start) / (lidar_param.vertical_lines - 1);
+            float vertical_angle = lidar_param.vertical_angle_start + v * vertical_resolution;
+            float sin_vert = std::sin(vertical_angle * M_PI / 180.0);
+            float cos_vert = std::cos(vertical_angle * M_PI / 180.0);
+            float horizontal_angle = h * lidar_param.horizontal_resolution;
+            float sin_horz = std::sin(horizontal_angle * M_PI / 180.0);
+            float cos_horz = std::cos(horizontal_angle * M_PI / 180.0);
+            // 计算射线方向
+            Vector3f ray_direction(cos_vert * cos_horz, cos_vert * sin_horz, sin_vert);
+
+            // 计算每个轴的增量比例
+            float dx = ray_direction.x * grid_map.raycast_step_;
+            float dy = ray_direction.y * grid_map.raycast_step_;
+            float dz = ray_direction.z * grid_map.raycast_step_;
+
+            // 递增射线方向上的每个轴
+            int scale = 0;
+            Vector3f point_value(0, 0, 0);
+
+            while (1)
+            {
+                scale += 1;
+
+                float point_x = scale * dx;
+                float point_y = scale * dy;
+                float point_z = scale * dz;
+
+                float3 point_c = make_float3(point_x, point_y, point_z);
+                float3 point_w = T_wc * point_c;
+
+                Vector3f point(point_w.x, point_w.y, point_w.z);
+
+                int occupied = grid_map.mapQuery(point);
+
+                float ray_length = sqrtf(point_x * point_x + point_y * point_y + point_z * point_z);
+
+                if (occupied == 1)
+                {
+                    point_value = Vector3f(point_x, point_y, point_z);
+                    Vector3i vox_body = grid_map.Pos2Vox(point_value);  // 栅格化避免平面变曲面
+                    point_value = grid_map.Vox2Pos(vox_body);
+                    break;
+                }
+
+                if (ray_length > lidar_param.max_lidar_dist){
+                    break;
+                }
+            }
+
+            // 将点云值存储到输出数组中，(0, 0, 0)为无效值
+            point_values[v * lidar_param.horizontal_num + h] = point_value;
+        }
+    }
+
+    void renderLidarPointcloud(GridMap *grid_map, LidarParams *lidar_param, cudaMat::SE3<float>& T_wc, pcl::PointCloud<pcl::PointXYZ>& lidar_points){
+        Vector3f* point_values;
+        size_t num_elements = lidar_param->vertical_lines * lidar_param->horizontal_num;
+        cudaMallocManaged(&point_values, num_elements * sizeof(Vector3f));
+
+        // 在GPU上启动核函数
+        lidarRaycastKernel<<<lidar_param->vertical_lines, lidar_param->horizontal_num>>>(point_values, *grid_map, *lidar_param, T_wc);
+        
+        cudaDeviceSynchronize();
+
+        std::vector<Vector3f> cpu_points(num_elements);
+        cudaMemcpy(cpu_points.data(), point_values, num_elements * sizeof(Vector3f), cudaMemcpyDeviceToHost);
+        
+        lidar_points.points.clear();
+        lidar_points.points.reserve(num_elements);
+        
+        for (const auto& point : cpu_points) {
+            if (point.x != 0 || point.y != 0 || point.z != 0) {
+                lidar_points.points.emplace_back(point.x, point.y, point.z);
+            }
+        }
+        cudaFree(point_values);
+        return;
+    }
+
+
+    
+}

Simulator/src/src/test_simulator.cpp

@@ -0,0 +1,10 @@
+#include "sensor_simulator.h"
+
+int main(int argc, char** argv) {
+    ros::init(argc, argv, "sensor_simulator_node");
+    ros::NodeHandle nh;
+
+    SensorSimulator sensor_simulator(nh);
+
+    return 0;
+}

Simulator/src/src/test_simulator_cuda.cpp

@@ -0,0 +1,217 @@
+#include <pcl/io/pcd_io.h>
+#include <pcl/io/ply_io.h>
+#include <pcl/point_cloud.h>
+#include <pcl/common/common.h>
+#include <pcl/common/eigen.h>
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+#include <opencv2/opencv.hpp>
+#include <ros/ros.h>
+#include <nav_msgs/Odometry.h>
+#include <sensor_msgs/Image.h>
+#include <pcl_ros/point_cloud.h>
+#include <cv_bridge/cv_bridge.h>
+#include <iostream>
+#include <vector>
+#include <yaml-cpp/yaml.h>
+#include "sensor_simulator.cuh"
+#include <chrono>
+#include "maps.hpp"
+
+using namespace raycast;
+
+class SensorSimulator {
+public:
+    SensorSimulator(ros::NodeHandle &nh) : nh_(nh) {
+        YAML::Node config = YAML::LoadFile(CONFIG_FILE_PATH);
+        // 读取camera参数
+        camera = new CameraParams();
+        camera->fx = config["camera"]["fx"].as<float>();
+        camera->fy = config["camera"]["fy"].as<float>();
+        camera->cx = config["camera"]["cx"].as<float>();
+        camera->cy = config["camera"]["cy"].as<float>();
+        camera->image_width = config["camera"]["image_width"].as<int>();
+        camera->image_height = config["camera"]["image_height"].as<int>();
+        camera->max_depth_dist = config["camera"]["max_depth_dist"].as<float>();
+        camera->normalize_depth = config["camera"]["normalize_depth"].as<bool>();
+        float pitch = config["camera"]["pitch"].as<float>() * M_PI / 180.0;
+        quat_bc = Eigen::AngleAxisf(pitch, Eigen::Vector3f::UnitY());
+
+        // 读取lidar参数
+        lidar = new LidarParams();
+        lidar->vertical_lines = config["lidar"]["vertical_lines"].as<int>();
+        lidar->vertical_angle_start = config["lidar"]["vertical_angle_start"].as<float>();
+        lidar->vertical_angle_end = config["lidar"]["vertical_angle_end"].as<float>();
+        lidar->horizontal_num = config["lidar"]["horizontal_num"].as<int>();
+        lidar->horizontal_resolution = config["lidar"]["horizontal_resolution"].as<float>();
+        lidar->max_lidar_dist = config["lidar"]["max_lidar_dist"].as<float>();
+
+        render_lidar = config["render_lidar"].as<bool>();
+        render_depth = config["render_depth"].as<bool>();
+        float depth_fps = config["depth_fps"].as<float>();
+        float lidar_fps = config["lidar_fps"].as<float>();
+        
+        std::string ply_file = config["ply_file"].as<std::string>();
+        std::string odom_topic = config["odom_topic"].as<std::string>();
+        std::string depth_topic = config["depth_topic"].as<std::string>();
+        std::string lidar_topic = config["lidar_topic"].as<std::string>();
+
+        // 读取地图参数
+        bool use_random_map = config["random_map"].as<bool>();
+        float resolution = config["resolution"].as<float>();
+        int occupy_threshold = config["occupy_threshold"].as<int>();
+        pcl_pub = nh.advertise<sensor_msgs::PointCloud2>("mock_map", 1);
+        int seed = config["seed"].as<int>();
+        int sizeX = config["x_length"].as<int>();
+        int sizeY = config["y_length"].as<int>();
+        int sizeZ = config["z_length"].as<int>();
+        int type = config["maze_type"].as<int>();
+        double scale = 1 / resolution;
+        sizeX = sizeX * scale;
+        sizeY = sizeY * scale;
+        sizeZ = sizeZ * scale;
+
+        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
+        if (use_random_map) {
+            printf("1.Generate Random Map... \n");
+            mocka::Maps::BasicInfo info;
+            info.sizeX      = sizeX;
+            info.sizeY      = sizeY;
+            info.sizeZ      = sizeZ;
+            info.seed       = seed;
+            info.scale      = scale;
+            info.cloud      = cloud;
+
+            mocka::Maps map;
+            map.setParam(config);
+            map.setInfo(info);
+            map.generate(type);
+        }
+        else {
+            printf("1.Reading Point Cloud %s... \n", ply_file.c_str());
+            if (pcl::io::loadPLYFile(ply_file, *cloud) == -1) {
+                PCL_ERROR("Couldn't read PLY file \n");
+            }
+        }
+        pcl::toROSMsg(*cloud, output);
+        output.header.frame_id = "world";
+
+        std::cout<<"Pointloud size:"<<cloud->points.size()<<std::endl;
+        printf("2.Mapping... \n");
+        grid_map = new GridMap(cloud, resolution, occupy_threshold);
+
+        // ROS
+        image_pub_ = nh_.advertise<sensor_msgs::Image>(depth_topic, 1);
+        point_cloud_pub_ = nh_.advertise<sensor_msgs::PointCloud2>(lidar_topic, 1);
+        odom_sub_ = nh_.subscribe(odom_topic, 1, &SensorSimulator::odomCallback, this);
+        timer_depth_ = nh_.createTimer(ros::Duration(1 / depth_fps), &SensorSimulator::timerDepthCallback, this);
+        timer_lidar_ = nh_.createTimer(ros::Duration(1 / lidar_fps), &SensorSimulator::timerLidarCallback, this);
+        timer_map_   = nh_.createTimer(ros::Duration(1), &SensorSimulator::timerMapCallback, this);
+
+        printf("3.Simulation Ready! \n");
+        ros::spin();
+    }
+
+    void odomCallback(const nav_msgs::Odometry::ConstPtr &msg);
+
+    void timerDepthCallback(const ros::TimerEvent &);
+
+    void timerLidarCallback(const ros::TimerEvent &);
+
+    void timerMapCallback(const ros::TimerEvent &);
+
+private:
+    bool render_depth{false};
+    bool render_lidar{false};
+    bool odom_init{false};
+    Eigen::Quaternionf quat;
+    Eigen::Quaternionf quat_bc, quat_wc;
+    Eigen::Vector3f pos;
+
+    CameraParams* camera;
+    LidarParams* lidar;
+    GridMap* grid_map;
+    sensor_msgs::PointCloud2 output;
+    
+    ros::NodeHandle nh_;
+    ros::Publisher image_pub_, point_cloud_pub_;
+    ros::Publisher pcl_pub;
+    ros::Subscriber odom_sub_;
+    ros::Timer timer_depth_, timer_lidar_, timer_map_;
+
+    // mocka::Maps map;
+};
+
+
+
+void SensorSimulator::timerDepthCallback(const ros::TimerEvent&) {
+    if (!odom_init || !render_depth)
+        return;
+
+    auto start = std::chrono::high_resolution_clock::now();
+
+    cudaMat::SE3<float> T_wc(quat_wc.w(), quat_wc.x(), quat_wc.y(), quat_wc.z(), pos.x(), pos.y(), pos.z());
+    cv::Mat depth_image;
+    renderDepthImage(grid_map, camera, T_wc, depth_image);
+    
+    auto end = std::chrono::high_resolution_clock::now();
+    std::chrono::duration<double> elapsed = end - start;
+    // std::cout << "生成图像耗时: " << elapsed.count() << " 秒" << std::endl;
+
+    sensor_msgs::Image ros_image;
+    cv_bridge::CvImage cv_image;
+    cv_image.header.stamp = ros::Time::now();
+    cv_image.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
+    cv_image.image = depth_image;
+    cv_image.toImageMsg(ros_image);
+    image_pub_.publish(ros_image);
+}
+
+void SensorSimulator::timerMapCallback(const ros::TimerEvent&) {
+    if (pcl_pub.getNumSubscribers() > 0)
+        pcl_pub.publish(output);    
+}
+
+
+void SensorSimulator::timerLidarCallback(const ros::TimerEvent&) {
+    if (!odom_init || !render_lidar)
+        return;
+
+    auto start = std::chrono::high_resolution_clock::now();
+
+    cudaMat::SE3<float> T_wc(quat.w(), quat.x(), quat.y(), quat.z(), pos.x(), pos.y(), pos.z());
+    pcl::PointCloud<pcl::PointXYZ> lidar_points;
+    renderLidarPointcloud(grid_map, lidar, T_wc, lidar_points);
+    
+    auto end = std::chrono::high_resolution_clock::now();
+    std::chrono::duration<double> elapsed = end - start;
+    // std::cout << "生成雷达耗时: " << elapsed.count() << " 秒" << std::endl;
+
+    sensor_msgs::PointCloud2 output;
+    pcl::toROSMsg(lidar_points, output);
+    output.header.stamp = ros::Time::now();
+    output.header.frame_id = "odom";
+    point_cloud_pub_.publish(output);
+}
+
+void SensorSimulator::odomCallback(const nav_msgs::Odometry::ConstPtr& msg) {
+    quat.x() = msg->pose.pose.orientation.x;
+    quat.y() = msg->pose.pose.orientation.y;
+    quat.z() = msg->pose.pose.orientation.z;
+    quat.w() = msg->pose.pose.orientation.w;
+    quat_wc = quat * quat_bc;
+
+    pos.x() = msg->pose.pose.position.x;
+    pos.y() = msg->pose.pose.position.y;
+    pos.z() = msg->pose.pose.position.z;
+
+    odom_init = true;
+}
+
+int main(int argc, char** argv) {
+    ros::init(argc, argv, "sensor_simulator_node");
+    ros::NodeHandle nh;
+
+    SensorSimulator sensor_simulator(nh);
+    return 0;
+}