├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── config ├── params.yaml └── params_azure.yaml ├── docker_support.pdf ├── img ├── dio.jpg ├── displayboard.png ├── hall.png ├── office.png └── setup.png ├── include ├── imuOptimizationFactor.h ├── imuPreintegrationClass.h ├── laserMappingClass.h ├── laserProcessingClass.h ├── lidarOptimizationFactor.h ├── odomEstimationClass.h ├── param.h ├── peac │ ├── AHCParamSet.hpp │ ├── AHCPlaneFitter.hpp │ ├── AHCPlaneSeg.hpp │ ├── AHCTypes.hpp │ ├── AHCUtils.hpp │ ├── DisjointSet.hpp │ ├── README.md │ └── eig33sym.hpp ├── poseOptimizationFactor.h └── utils.h ├── l515_imu_calibration ├── README.md ├── calibration.json ├── l515_calibration_manual.pdf └── rs-imu-calibration.py ├── launch ├── r2dio.launch ├── r2dio_L515.launch ├── r2dio_azure.launch └── rs_camera.launch ├── package.xml ├── rviz ├── r2dio.rviz └── r2dio_azure.rviz └── src ├── imuOptimizationFactor.cpp ├── imuPreintegrationClass.cpp ├── laserMappingClass.cpp ├── laserMappingNode.cpp ├── laserProcessingClass.cpp ├── laserProcessingNode.cpp ├── lidarOptimizationFactor.cpp ├── odomEstimationClass.cpp ├── odomEstimationNode.cpp ├── param.cpp ├── poseOptimizationFactor.cpp └── utils.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | # Prerequisites 2 | *.d 3 | 4 | # Compiled Object files 5 | *.slo 6 | *.lo 7 | *.o 8 | *.obj 9 | 10 | # Precompiled Headers 11 | *.gch 12 | *.pch 13 | 14 | # Compiled Dynamic libraries 15 | *.so 16 | *.dylib 17 | *.dll 18 | 19 | # Fortran module files 20 | *.mod 21 | *.smod 22 | 23 | # Compiled Static libraries 24 | *.lai 25 | *.la 26 | *.a 27 | *.lib 28 | 29 | # Executables 30 | *.exe 31 | *.out 32 | *.app 33 | 34 | PCD/scans.pcd 35 | rviz_cfg/loam_livox.rviz 36 | Log/imu_.txt 37 | .vscode 38 | .idea/ 39 | cmake-build-debug/ -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(r2dio) 3 | #add_definitions(-w) 4 | set(CMAKE_BUILD_TYPE "Release") 5 | set(CMAKE_CXX_STANDARD 14) 6 | 7 | find_package(catkin REQUIRED COMPONENTS 8 | geometry_msgs 9 | nav_msgs 10 | sensor_msgs 11 | roscpp 12 | rospy 13 | rosbag 14 | std_msgs 15 | tf 16 | eigen_conversions 17 | cv_bridge 18 | image_transport 19 | ) 20 | 21 | find_package(Eigen3) 22 | find_package(OpenCV 4.2 REQUIRED) 23 | find_package(PCL REQUIRED) 24 | find_package(Ceres REQUIRED) 25 | find_package(OpenMP QUIET) 26 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 27 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 28 | 29 | include_directories( 30 | include 31 | include/peac 32 | ${catkin_INCLUDE_DIRS} 33 | ${PCL_INCLUDE_DIRS} 34 | ${EIGEN3_INCLUDE_DIRS} 35 | ${CMAKE_CURRENT_SOURCE_DIR} 36 | ${CMAKE_CURRENT_BINARY_DIR} 37 | ) 38 | 39 | link_directories( 40 | include 41 | ${PCL_LIBRARY_DIRS} 42 | ) 43 | 44 | catkin_package( 45 | CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs 46 | DEPENDS EIGEN3 PCL Ceres 47 | INCLUDE_DIRS include 48 | ) 49 | 50 | add_executable(r2dio_laser_processing_node src/laserProcessingNode.cpp src/laserProcessingClass.cpp src/param.cpp src/utils.cpp ) 51 | target_link_libraries(r2dio_laser_processing_node ${EIGEN3_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES} ${OpenCV_LIBS}) 52 | 53 | add_executable(r2dio_odom_estimation_node src/utils.cpp src/odomEstimationNode.cpp src/param.cpp src/odomEstimationClass.cpp src/imuOptimizationFactor.cpp src/lidarOptimizationFactor.cpp src/imuPreintegrationClass.cpp src/param.cpp src/poseOptimizationFactor.cpp) 54 | target_link_libraries(r2dio_odom_estimation_node ${EIGEN3_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES} ${OpenCV_LIBS}) 55 | 56 | add_executable(r2dio_laser_mapping_node src/laserMappingNode.cpp src/utils.cpp src/laserMappingClass.cpp src/param.cpp) 57 | target_link_libraries(r2dio_laser_mapping_node ${EIGEN3_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES} ${OpenCV_LIBS}) 58 | 59 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # R2DIO 2 | ## R2DIO: Robust, Real-Time and Depth-Inertial Odometry Based on Multi-Modal Constraints (Intel Realsense L515 as an example) 3 | 4 | The RGB-D camera is widely applied on lightweight robots as an essential sensor in indoor SLAM. However, limited by computing cost, many RGB-D SLAM systems do not fully use the multi-modal information of cameras, resulting in degeneration and low accuracy in challenging scenes. To address this issue, this letter introduces a novel, lightweight, robust, real-time and depth-inertial odometry (R2DIO) for ToF RGB-D cameras. Texture and structure information is coupled simultaneously to improve robustness. It efficiently extracts line features from RGB images and plane features from depth images based on agglomerative clustering. When aligning features, the direction vectors of lines and planes are used to filter the mismatches and enhance real-time capability. The IMU measurements propagate states and are tightly coupled in the system by pre-integration. Finally, the system estimates states and builds dense, colored maps with the following constraints: line and plane matching constraints, IMU pre-integration constraints and historical odometry constraints. We demonstrate the robustness, accuracy and efficiency of R2DIO in the experiments. The competitive results indicate that our system is able to locate precisely in challenging scenes and run at 30 Hz on a low-power platform. The source code of R2DIO and experiment datasets is publicly available for the development of the community. 5 | 6 | A summary video demo can be found at [Youtube](https://www.youtube.com/watch?v=YqJZTE948sk) and [Bilibili](https://www.bilibili.com/video/BV1GW4y1u7sz/). 7 | 8 | **Author:** Xu Jie, Harbin Institute of Technology, China 9 | 10 | ## 1. SLAM examples with Intel RealSense L515 11 | ### 1.1 In a hall 12 |

13 | 14 | 15 | 16 |

17 | 18 | ### 1.2 In an office 19 |

20 | 21 | 22 | 23 |

24 | 25 | ### 1.3 Facing a display board 26 |

27 | 28 | 29 | 30 |

31 | 32 | ## 2. Prerequisites 33 | ### 2.1 **Ubuntu** and **ROS** 34 | Ubuntu 64-bit 20.04. 35 | 36 | ROS noetic. [ROS Installation](http://wiki.ros.org/ROS/Installation) 37 | 38 | ### 2.2. **Ceres Solver** 39 | Follow [Ceres Installation](http://ceres-solver.org/installation.html). 40 | 41 | Tested with 2.0 42 | 43 | ### 2.3. **PCL** 44 | Follow [PCL Installation](http://www.pointclouds.org/downloads/linux.html). 45 | 46 | Tested with 1.10 (inherent in ros) 47 | 48 | ### 2.4. **Trajectory visualization** 49 | For visualization purpose, this package uses hector trajectory sever, you may install the package by 50 | ``` 51 | sudo apt update 52 | sudo apt install ros-noetic-hector-trajectory-server 53 | ``` 54 | Alternatively, you may remove the hector trajectory server node if trajectory visualization is not needed 55 | 56 | ### 2.4. **OpenCV** 57 | Tested with 4.2.0 (inherent in ros) 58 | 59 | ## 3. Build 60 | ### 3.1 Clone repository: 61 | ``` 62 | cd ~/catkin_ws/src 63 | git clone https://github.com/jiejie567/R2DIO.git 64 | cd .. 65 | catkin_make 66 | source ~/catkin_ws/devel/setup.bash 67 | ``` 68 | 69 | ### 3.2 Download test rosbag 70 | You may download our [recorded data](https://drive.google.com/drive/folders/1S0Q351M9zEgg-Nme4obqexRQlYRJyco7?usp=sharing) (10GB). (one rosbag) 71 | 72 | If you are in China, you can download the recorded data via Baidu Netdisk 73 | : [office](https://pan.baidu.com/s/1LTos6MG4CUq3SJz6GV55tQ), [hall](https://pan.baidu.com/s/16hp1APONPAn46WgFgvkm_g), and [display board](https://pan.baidu.com/s/1Ys_a9dZR9E-d9ELlY6-wug). The extraction code is 0503. (ten rosbags) 74 | 75 | Note that due to the limitation of Google drive capacity, we only upload one rosbag. 76 | 77 | 78 | 79 | 80 | ### 3.3 Launch ROS 81 | ``` 82 | roslaunch r2dio r2dio.launch 83 | ``` 84 | Note that change the path to your datasets. 85 | 86 | ## 4. Sensor Setup 87 | If you have new Realsense L515 sensor, you may follow the below setup instructions 88 | 89 | ### 4.1 IMU calibration (optional) 90 | You may read official document [L515 Calibration Manual] (https://github.com/l515_calibration_manual.pdf) first 91 | 92 | use the following command to calibrate imu, note that the build-in imu is a low-grade imu, to get better accurate, you may use your own imu 93 | ``` 94 | cd ~/catkin_ws/src/r2dio/l515_imu_calibration 95 | python rs-imu-calibration.py 96 | ``` 97 | 98 | ### 4.2 L515 99 |

100 | 101 |

102 | 103 | ### 4.3 Librealsense 104 | Follow [Librealsense Installation](https://github.com/IntelRealSense/librealsense/blob/master/doc/installation.md) 105 | 106 | ### 4.4 Realsense_ros 107 | Copy [realsense_ros](https://github.com/IntelRealSense/realsense-ros) package to your catkin folder 108 | ``` 109 | cd ~/catkin_ws/src 110 | git clone https://github.com/IntelRealSense/realsense-ros.git 111 | cd .. 112 | catkin_make 113 | ``` 114 | 115 | ### 4.5 Launch ROS 116 | Make Lidar still for 1 sec to estimate the initial bias, otherwise will cause localization failure! 117 | ``` 118 | roslaunch r2dio r2dio_L515.launch 119 | ``` 120 | ### 4.6 Docker suppot 121 | You can see the details in docker_support.pdf. Thanks for [Yin Ao](https://github.com/coolaogege)'s work about the docker. 122 | ## 5. Citation 123 | If you use this work for your research, you may want to cite the paper below, your citation will be appreciated. 124 | ``` 125 | @ARTICLE{10268066, 126 | author={Xu, Jie and Li, Ruifeng and Huang, Song and Zhao, Xiongwei and Qiu, Shuxin and Chen, Zhijun and Zhao, Lijun}, 127 | journal={IEEE Transactions on Instrumentation and Measurement}, 128 | title={R2DIO: A Robust and Real-Time Depth-Inertial Odometry Leveraging Multimodal Constraints for Challenging Environments}, 129 | year={2023}, 130 | volume={72}, 131 | number={}, 132 | pages={1-11}, 133 | doi={10.1109/TIM.2023.3320753}} 134 | 135 | @article{wang2021lightweight, 136 | author={H. {Wang} and C. {Wang} and L. {Xie}}, 137 | journal={IEEE Robotics and Automation Letters}, 138 | title={Lightweight 3-D Localization and Mapping for Solid-State LiDAR}, 139 | year={2021}, 140 | volume={6}, 141 | number={2}, 142 | pages={1801-1807}, 143 | doi={10.1109/LRA.2021.3060392}} 144 | ``` 145 | ## 6. Acknowledgements 146 | The code is heavily derived from [SSL-SLAM3](https://github.com/wh200720041/ssl_slam3), thanks for Wang Han's open-source spirit. 147 | 148 | The datasets and video are processed by [Qiu Shuxin](https://github.com/1136958879) and Huang Song. 149 | 150 | The docker is created by [Yin Ao](https://github.com/coolaogege). 151 | 152 | What's more, the "DIO" in the title "R2DIO" means: 153 |

154 | 155 |

156 | 157 | -------------------------------------------------------------------------------- /config/params.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | common: 4 | core_num: 8 5 | init_frame: 10 6 | nearby_frame: 6 7 | 8 | # Transformation from body-frame (imu) to camera 9 | Tbl: !!opencv-matrix 10 | rows: 4 11 | cols: 4 12 | dt: d 13 | data: [1.0, 0.0, 0.0, 0.0, 14 | 0.0, 1.0, 0.0, 0.0, 15 | 0.0, 0.0, 1.0, 0.0, 16 | 0.0, 0.0, 0.0, 1.0] 17 | 18 | lidar: 19 | rgb_topic: "/camera/color/image_raw" 20 | depth_topic: "/camera/aligned_depth_to_color/image_raw" 21 | frequency: 30 22 | camera_factor: 1000.0 23 | camera_cx: 330.275 24 | camera_cy: 266.435 25 | camera_fx: 594.196 26 | camera_fy: 595.270 27 | min_distance: 0.2 28 | max_distance: 8.0 29 | odom_n_rpy: 1.0e-2 30 | odom_n_xyz: 1.0e-2 31 | local_map_resolution: 0.05 32 | local_map_size: 10.0 33 | edge_n: 1.0e-4 34 | plane_n: 1.0e-3 35 | 36 | # mapping thread 37 | map_resolution: 0.1 38 | map_cell_width: 50.0 39 | map_cell_height: 50.0 40 | map_cell_depth: 50.0 41 | map_cell_width_range: 4 42 | map_cell_height_range: 4 43 | map_cell_depth_range: 4 44 | 45 | gap_plane: 20 46 | gap_line: 20 47 | 48 | imu: 49 | # note that we follow the value unit from kalibr link:https://github.com/ethz-asl/kalibr/wiki/IMU-Noise-Model 50 | # if you use imu_utils, the result is in discrete time, you can simply set frequency to 1 51 | imu_topic: "/camera/imu" 52 | frequency: 200 53 | acc_n: 1.176e-2 # 1.176e-2m/s^2 * 1/sqrt(Hz) continuous time 54 | gyr_n: 2.443e-3 # 2.443e-3rad/s * 1/sqrt(Hz) continuous time 55 | acc_w: 1.0e-3 # m/s^3 * 1/sqrt(Hz) continuous time 56 | gyr_w: 1.0e-4 # rad/s^2 * 1/sqrt(Hz) continuous time -------------------------------------------------------------------------------- /config/params_azure.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | common: 4 | core_num: 8 5 | init_frame: 10 6 | nearby_frame: 6 7 | 8 | # Transformation from body-frame (imu) to camera 9 | Tbl: !!opencv-matrix 10 | rows: 4 11 | cols: 4 12 | dt: d 13 | # data: [ 1.0, 0.0, 0.0, 0.0, 14 | # 0.0, 1.0, 0.0, 0.0, 15 | # 0.0, 0.0, 1.0, 0.0, 16 | # 0.0, 0.0, 0.0, 1.0 ] 17 | data: [-0.00061492, 0.00730016, -0.999973, -0.047, 18 | -0.999996, -0.00270155, 0.000595212, 0.0285, 19 | -0.00269713, 0.99997, 0.00730179, 0.0033, 20 | 0.0, 0.0, 0.0, 1.0] 21 | # data: [ -0.00061492, -0.999996, -0.00269713, -0.0285, 22 | # 0.00730016, -0.00270155, 0.99997, -0.0031, 23 | # -0.999973, 0.000595212, 0.00730179, -0.0470, 24 | # 0.0, 0.0, 0.0, 1.0 ] 25 | 26 | # [ INFO] [1676362407.449727668]: IMU (Color to IMU): 27 | # [ INFO] [1676362407.449734727]: Extrinsics: 28 | # [ INFO] [1676362407.449750595]: Translation: -46.9783, -28.4947, 3.33461 29 | # [ INFO] [1676362407.449770564]: Rotation[0]: -0.00061492, 0.00730016, -0.999973 30 | # [ INFO] [1676362407.449785103]: Rotation[1]: -0.999996, -0.00270155, 0.000595212 31 | # [ INFO] [1676362407.449806962]: Rotation[2]: -0.00269713, 0.99997, 0.00730179 32 | 33 | # [ INFO] [1676273645.432006109]: IMU (IMU to Color): 34 | # [ INFO] [1676273645.432016211]: Extrinsics: 35 | # [ INFO] [1676273645.432029369]: Translation: -28.5145, -3.06854, -46.9844 36 | # [ INFO] [1676273645.432042705]: Rotation[0]: -0.00061492, -0.999996, -0.00269713 37 | # [ INFO] [1676273645.432055924]: Rotation[1]: 0.00730016, -0.00270155, 0.99997 38 | # [ INFO] [1676273645.432072033]: Rotation[2]: -0.999973, 0.000595212, 0.00730179 39 | 40 | lidar: 41 | rgb_topic: "/rgb/image_raw" 42 | depth_topic: "/depth_to_rgb/image_raw" 43 | frequency: 30 44 | camera_factor: 1000.0 45 | camera_cx: 330.275 46 | camera_cy: 266.435 47 | camera_fx: 594.196 48 | camera_fy: 595.270 49 | min_distance: 0.2 50 | max_distance: 8.0 51 | odom_n_rpy: 1.0e-2 52 | odom_n_xyz: 1.0e-2 53 | local_map_resolution: 0.05 54 | local_map_size: 10.0 55 | edge_n: 1.0e-4 56 | plane_n: 1.0e-3 57 | 58 | # mapping thread 59 | map_resolution: 0.1 60 | map_cell_width: 50.0 61 | map_cell_height: 50.0 62 | map_cell_depth: 50.0 63 | map_cell_width_range: 4 64 | map_cell_height_range: 4 65 | map_cell_depth_range: 4 66 | 67 | gap_plane: 20 68 | gap_line: 20 69 | 70 | imu: 71 | # note that we follow the value unit from kalibr link:https://github.com/ethz-asl/kalibr/wiki/IMU-Noise-Model 72 | # if you use imu_utils, the result is in discrete time, you can simply set frequency to 1 73 | imu_topic: "/imu" 74 | frequency: 200 75 | acc_n: 1.176e-2 # 1.176e-2m/s^2 * 1/sqrt(Hz) continuous time 76 | gyr_n: 2.443e-3 # 2.443e-3rad/s * 1/sqrt(Hz) continuous time 77 | acc_w: 1.0e-3 # m/s^3 * 1/sqrt(Hz) continuous time 78 | gyr_w: 1.0e-4 # rad/s^2 * 1/sqrt(Hz) continuous time -------------------------------------------------------------------------------- /docker_support.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jiejie567/R2DIO/40d2c86f53bd6bd152c5094aa9096aebfa61f58a/docker_support.pdf -------------------------------------------------------------------------------- /img/dio.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jiejie567/R2DIO/40d2c86f53bd6bd152c5094aa9096aebfa61f58a/img/dio.jpg -------------------------------------------------------------------------------- /img/displayboard.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jiejie567/R2DIO/40d2c86f53bd6bd152c5094aa9096aebfa61f58a/img/displayboard.png -------------------------------------------------------------------------------- /img/hall.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jiejie567/R2DIO/40d2c86f53bd6bd152c5094aa9096aebfa61f58a/img/hall.png -------------------------------------------------------------------------------- /img/office.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jiejie567/R2DIO/40d2c86f53bd6bd152c5094aa9096aebfa61f58a/img/office.png -------------------------------------------------------------------------------- /img/setup.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jiejie567/R2DIO/40d2c86f53bd6bd152c5094aa9096aebfa61f58a/img/setup.png -------------------------------------------------------------------------------- /include/imuOptimizationFactor.h: -------------------------------------------------------------------------------- 1 | // Author of SSL_SLAM3: Wang Han 2 | // Email wh200720041@gmail.com 3 | // Homepage https://wanghan.pro 4 | #ifndef _IMU_OPTIMIZATION_FACTOR_H_ 5 | #define _IMU_OPTIMIZATION_FACTOR_H_ 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include "utils.h" 13 | #include "imuPreintegrationClass.h" 14 | 15 | class ImuPreintegrationFactor : public ceres::SizedCostFunction<15, 15, 15>{ 16 | public: 17 | ImuPreintegrationFactor(ImuPreintegrationClass& imu_preintegrator_in):imu_preintegrator(imu_preintegrator_in){} 18 | 19 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 20 | ImuPreintegrationClass& imu_preintegrator; 21 | }; 22 | 23 | #endif // _IMU_OPTIMIZATION_FACTOR_H_ 24 | 25 | -------------------------------------------------------------------------------- /include/imuPreintegrationClass.h: -------------------------------------------------------------------------------- 1 | // Author of SSL_SLAM3: Wang Han 2 | // Email wh200720041@gmail.com 3 | // Homepage https://wanghan.pro 4 | #ifndef _IMU_PREINTEGRATION_CLASS_H_ 5 | #define _IMU_PREINTEGRATION_CLASS_H_ 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include "utils.h" 13 | 14 | class ImuPreintegrationClass{ 15 | public: 16 | double sum_dt; 17 | Eigen::Matrix3d delta_R; 18 | Eigen::Vector3d delta_p; 19 | Eigen::Vector3d delta_v; 20 | Eigen::Vector3d b_a; 21 | Eigen::Vector3d b_g; 22 | 23 | Eigen::Matrix covariance; 24 | Eigen::Matrix jacobian; 25 | 26 | ImuPreintegrationClass(const Eigen::Vector3d b_a_in, const Eigen::Vector3d b_g_in, 27 | const double ACC_N_in, const double GYR_N_in, const double ACC_W_in, const double GYR_W_in); 28 | void addImuData(const double dt, const Eigen::Vector3d acc, const Eigen::Vector3d gyr); 29 | void update(const Eigen::Vector3d& b_a_in, const Eigen::Vector3d& b_g_in); 30 | 31 | //call only during initialization 32 | std::vector getAcc(void); 33 | std::vector getGyr(void); 34 | private: 35 | 36 | std::vector acc_buf; 37 | std::vector gyr_buf; 38 | std::vector dt_buf; 39 | Eigen::Vector3d last_acc; 40 | Eigen::Vector3d last_gyr; 41 | Eigen::Matrix noise; 42 | 43 | void initWithImu(const Eigen::Vector3d& acc, const Eigen::Vector3d& gyr); 44 | void propagate(const double dt, const Eigen::Vector3d& acc, const Eigen::Vector3d& gyr); 45 | }; 46 | 47 | #endif // _IMU_PREINTEGRATION_CLASS_H_ 48 | 49 | -------------------------------------------------------------------------------- /include/laserMappingClass.h: -------------------------------------------------------------------------------- 1 | // Author of SSL_SLAM3: Wang Han 2 | // Email wh200720041@gmail.com 3 | // Homepage https://wanghan.pro 4 | 5 | #ifndef _LASER_MAPPING_H_ 6 | #define _LASER_MAPPING_H_ 7 | 8 | //PCL lib 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | //eigen lib 20 | #include 21 | #include 22 | 23 | //c++ lib 24 | #include 25 | #include 26 | #include 27 | 28 | //local lib 29 | #include "param.h" 30 | 31 | class LaserMappingClass 32 | { 33 | public: 34 | LaserMappingClass(); 35 | void init(std::string& file_path); 36 | void updateCurrentPointsToMap(const pcl::PointCloud::Ptr& pc_in, const Eigen::Isometry3d& pose_current); 37 | pcl::PointCloud::Ptr getMap(void); 38 | 39 | private: 40 | int origin_in_map_x; 41 | int origin_in_map_y; 42 | int origin_in_map_z; 43 | int map_width; 44 | int map_height; 45 | int map_depth; 46 | LidarParam lidar_param; 47 | std::vector::Ptr>>> map; 48 | pcl::VoxelGrid downSizeFilter; 49 | 50 | void addWidthCellNegative(void); 51 | void addWidthCellPositive(void); 52 | void addHeightCellNegative(void); 53 | void addHeightCellPositive(void); 54 | void addDepthCellNegative(void); 55 | void addDepthCellPositive(void); 56 | void checkPoints(int& x, int& y, int& z); 57 | void noiseFilter(const pcl::PointCloud::Ptr& pc_in, pcl::PointCloud::Ptr& pc_out); 58 | }; 59 | 60 | 61 | #endif // _LASER_MAPPING_H_ 62 | 63 | -------------------------------------------------------------------------------- /include/laserProcessingClass.h: -------------------------------------------------------------------------------- 1 | // Author of SSL_SLAM3: Wang Han 2 | // Email wh200720041@gmail.com 3 | // Homepage https://wanghan.pro 4 | 5 | #ifndef _LASER_PROCESSING_CLASS_H_ 6 | #define _LASER_PROCESSING_CLASS_H_ 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include // 拟合平面 19 | #include // 拟合直线 20 | 21 | #include 22 | #include 23 | #include "opencv2/ximgproc.hpp" 24 | #include "opencv2/imgcodecs.hpp" 25 | 26 | #include "param.h" 27 | #include 28 | #include "AHCPlaneFitter.hpp" 29 | #include "utils.h" 30 | 31 | 32 | //points covariance class 33 | class Double2d{ 34 | public: 35 | int id; 36 | double value; 37 | }; 38 | 39 | //points info class 40 | class PointsInfo{ 41 | public: 42 | int layer; 43 | double time; 44 | }; 45 | 46 | class LaserProcessingClass { 47 | public: 48 | LaserProcessingClass(){}; 49 | void init(std::string& file_path); 50 | void featureExtraction(cv::Mat& color_im,cv::Mat& depth_im, pcl::PointCloud::Ptr& pc_out_line, 51 | pcl::PointCloud::Ptr& pc_out_plane, pcl::PointCloud::Ptr& cloud_filter); 52 | void lineFilter(cv::Mat& color_im,cv::Mat_& cloud_peac,pcl::PointCloud::Ptr& pc_out_line); 53 | int frame_count; 54 | CommonParam common_param; 55 | LidarParam lidar_param; 56 | private: 57 | uint32_t num_of_plane; 58 | uint32_t num_of_line; 59 | int gap_plane; 60 | int gap_line; 61 | 62 | }; 63 | 64 | #endif // _LASER_PROCESSING_CLASS_H_ 65 | 66 | -------------------------------------------------------------------------------- /include/lidarOptimizationFactor.h: -------------------------------------------------------------------------------- 1 | // Author of SSL_SLAM3: Wang Han 2 | // Email wh200720041@gmail.com 3 | // Homepage https://wanghan.pro 4 | #ifndef _LIDAR_OPTIMIZATION_FACTOR_H_ 5 | #define _LIDAR_OPTIMIZATION_FACTOR_H_ 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include "utils.h" 13 | #include 14 | class LidarOdometryFactor : public ceres::SizedCostFunction<6, 15, 15>{ 15 | public: 16 | LidarOdometryFactor(Eigen::Isometry3d odom_in, Eigen::Matrix covariance_in); 17 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 18 | Eigen::Isometry3d odom; 19 | Eigen::Matrix sqrt_info; 20 | }; 21 | 22 | class LidarEdgeFactor : public ceres::SizedCostFunction<1, 15> { 23 | public: 24 | LidarEdgeFactor(Eigen::Vector3d curr_point_in, Eigen::Vector3d last_point_a_in, Eigen::Vector3d last_point_b_in, double covariance_in); 25 | virtual ~LidarEdgeFactor() {} 26 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 27 | 28 | Eigen::Vector3d curr_point; 29 | Eigen::Vector3d last_point_a; 30 | Eigen::Vector3d last_point_b; 31 | double sqrt_info; 32 | }; 33 | 34 | 35 | class LidarPlaneFactor : public ceres::SizedCostFunction<1, 15> { 36 | public: 37 | LidarPlaneFactor(Eigen::Vector3d curr_point_in, Eigen::Vector3d plane_unit_norm_in, double negative_OA_dot_norm_in, double covariance_in); 38 | virtual ~LidarPlaneFactor() {} 39 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 40 | 41 | Eigen::Vector3d curr_point; 42 | Eigen::Vector3d plane_unit_norm; 43 | double negative_OA_dot_norm; 44 | double sqrt_info; 45 | }; 46 | struct NumericDiffCostFunctor { 47 | Eigen::Vector4d current_plane_hessian_; 48 | Eigen::Vector4d target_plane_hessian_; 49 | double sqrt_info; 50 | int quantity_plane_matched_; 51 | 52 | NumericDiffCostFunctor(Eigen::Vector4d current_plane_hessian, Eigen::Vector4d traget_plane_hessian, double quantity_plane_matched, double covariance_in){ 53 | current_plane_hessian_ = current_plane_hessian; 54 | target_plane_hessian_ = traget_plane_hessian; 55 | quantity_plane_matched_ = quantity_plane_matched; 56 | sqrt_info = 1.0 /covariance_in; 57 | } 58 | bool operator()(const double* const parameters, double *residuals) const { 59 | Eigen::Vector3d ri(parameters[0], parameters[1], parameters[2]); 60 | Eigen::Vector3d Pi(parameters[3], parameters[4], parameters[5]); 61 | Eigen::Matrix3d Ri = Utils::so3ToR(ri); 62 | Eigen::Vector3d plane_n = current_plane_hessian_.head(3); 63 | double plane_d= current_plane_hessian_[3]; 64 | Eigen::Vector3d plane_n_transed = Ri*plane_n; 65 | auto pi_transepose = Pi.transpose(); 66 | double plane_d_transed = -pi_transepose*plane_n_transed+plane_d; 67 | if(plane_d_transed<0) 68 | { 69 | plane_d_transed = -plane_d_transed; 70 | plane_n_transed = -plane_n_transed; 71 | } 72 | Eigen::Vector3d current_plane_closest_point = plane_n_transed*plane_d_transed; 73 | Eigen::Vector3d target_plane_closest_point = target_plane_hessian_.head(3)*target_plane_hessian_[3]; 74 | 75 | Eigen::Isometry3d T_wb = Eigen::Isometry3d::Identity(); 76 | T_wb.linear() = Utils::so3ToR(Eigen::Vector3d(parameters[0],parameters[1],parameters[2])); 77 | T_wb.translation() = Eigen::Vector3d(parameters[3],parameters[4],parameters[5]); 78 | 79 | // residuals[0] = sqrt_info*quantity_plane_matched_*(T_wb.matrix().transpose().inverse()*current_plane_hessian_-target_plane_hessian_).norm(); 80 | 81 | // residuals[0] = sqrt_info*quantity_plane_matched_*(current_plane_closest_point-target_plane_closest_point).norm(); 82 | Eigen::Map > residuals_i(residuals); 83 | residuals_i[0] = sqrt_info*quantity_plane_matched_*(current_plane_closest_point-target_plane_closest_point).norm(); 84 | return true; 85 | } 86 | }; 87 | struct NumericDiffCostLineFunctor { 88 | Eigen::Vector3d curr_end_pt; 89 | Eigen::Vector3d last_point_a; 90 | Eigen::Vector3d last_point_b; 91 | double sqrt_info; 92 | double weight_; 93 | 94 | NumericDiffCostLineFunctor(Eigen::Vector4d curr_end_, Eigen::Vector4d last_point_a_in, 95 | Eigen::Vector4d last_point_b_in, double covariance_in, 96 | double weight){ 97 | curr_end_pt = curr_end_.head(3); 98 | last_point_a = last_point_a_in.head(3); 99 | last_point_b = last_point_b_in.head(3); 100 | sqrt_info = 1.0 / covariance_in; 101 | weight_ = weight; 102 | } 103 | bool operator()(const double* const parameters, double *residuals) const { 104 | Eigen::Vector3d ri(parameters[0], parameters[1], parameters[2]); 105 | Eigen::Vector3d Pi(parameters[3], parameters[4], parameters[5]); 106 | Eigen::Matrix3d Ri = Utils::so3ToR(ri); 107 | 108 | Eigen::Vector3d lp = Ri * curr_end_pt + Pi; 109 | Eigen::Vector3d nu = (lp - last_point_a).cross(lp - last_point_b); 110 | Eigen::Vector3d de = last_point_a - last_point_b; 111 | residuals[0] = weight_*sqrt_info * nu.norm() / de.norm(); 112 | return true; 113 | } 114 | }; 115 | #endif // _LIDAR_OPTIMIZATION_FACTOR_H_ 116 | 117 | -------------------------------------------------------------------------------- /include/odomEstimationClass.h: -------------------------------------------------------------------------------- 1 | // Author of SSL_SLAM3: Wang Han 2 | // Email wh200720041@gmail.com 3 | // Homepage https://wanghan.pro 4 | 5 | #ifndef _ODOM_ESTIMATION_CLASS_H_ 6 | #define _ODOM_ESTIMATION_CLASS_H_ 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | 15 | //PCL 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | #include 27 | 28 | // local lib 29 | #include "utils.h" 30 | #include "param.h" 31 | #include "imuOptimizationFactor.h" 32 | #include "lidarOptimizationFactor.h" 33 | #include "poseOptimizationFactor.h" 34 | 35 | #define POSE_BUFFER 50 36 | class OdomEstimationClass{ 37 | // pose_k*******************************************************pose_k+1 38 | // pose_q_arr[k]****imu_integrator_arr[k]*lidar_odom_arr[k]*****pose_q_arr[k+1] 39 | public: 40 | bool is_initialized; 41 | std::vector imu_preintegrator_arr; 42 | std::vector lidar_odom_arr; 43 | std::vector pose_r_arr; //world coordinate 44 | std::vector pose_t_arr; //world coordinate 45 | std::vector pose_v_arr; //world coordinate 46 | std::vector pose_b_a_arr; //imu coordinate 47 | std::vector pose_b_g_arr; //imu coordinate 48 | OdomEstimationClass(); 49 | void init(std::string& file_path); 50 | bool initialize(void); 51 | void initMapWithPoints(const pcl::PointCloud::Ptr edge_in, const pcl::PointCloud::Ptr plane_in); 52 | void addImuPreintegration(std::vector dt_arr, std::vector acc_arr, std::vector gyr_arr); 53 | void addLidarFeature(const pcl::PointCloud::Ptr edge_in, const pcl::PointCloud::Ptr plane_in); 54 | void optimize(void); 55 | ImuParam imu_param; 56 | 57 | 58 | private: 59 | CommonParam common_param; 60 | LidarParam lidar_param; 61 | 62 | // map points 63 | pcl::PointCloud::Ptr edge_map; 64 | pcl::PointCloud::Ptr plane_map; 65 | 66 | pcl::PointCloud::Ptr current_edge_points; 67 | pcl::PointCloud::Ptr current_plane_points; 68 | 69 | 70 | 71 | //plane 72 | int current_plane_num; 73 | std::map *pm_plane_info; 74 | std::map m_current_plane_info; 75 | //line 76 | int current_line_num; 77 | std::map *pm_line_info; 78 | std::map m_current_line_info; 79 | 80 | 81 | // kdtree for fast indexing 82 | pcl::KdTreeFLANN edge_kd_tree; 83 | pcl::KdTreeFLANN plane_kd_tree; 84 | 85 | //pose 86 | Eigen::Isometry3d last_pose; 87 | Eigen::Isometry3d current_pose; 88 | 89 | 90 | 91 | // points downsampling before add to map 92 | pcl::VoxelGrid edge_downsize_filter; 93 | pcl::VoxelGrid plane_downsize_filter; 94 | 95 | 96 | void addEdgeCost(ceres::Problem& problem, ceres::LossFunction *loss_function, double* pose, int cnt); 97 | 98 | void addPlaneCost(ceres::Problem& problem, ceres::LossFunction *loss_function, double* pose, int cnt); 99 | void addOdometryCost(const Eigen::Isometry3d& odom, ceres::Problem& problem, ceres::LossFunction *loss_function, double* pose1, double* pose2); 100 | void addImuCost(ImuPreintegrationClass& imu_integrator, ceres::Problem& problem, ceres::LossFunction *loss_function, double* pose1, double* pose2); 101 | void updateLocalMap(Eigen::Isometry3d& transform); 102 | }; 103 | #endif // _ODOM_ESTIMATION_CLASS_H_ 104 | 105 | -------------------------------------------------------------------------------- /include/param.h: -------------------------------------------------------------------------------- 1 | // Author of SSL_SLAM3: Wang Han 2 | // Email wh200720041@gmail.com 3 | // Homepage https://wanghan.pro 4 | #ifndef _PARAM_H_ 5 | #define _PARAM_H_ 6 | 7 | // opencv 8 | #include 9 | 10 | // eigen 11 | #include 12 | #include 13 | 14 | using namespace std; 15 | // define lidar parameter 16 | class CommonParam{ 17 | public: 18 | CommonParam(){}; 19 | void loadParam(std::string& path); 20 | int getCoreNum(void); 21 | int getInitFrame(void); 22 | int getNearbyFrame(void); 23 | Eigen::Isometry3d getTbl(void); 24 | private: 25 | int core_num; 26 | int init_frame; 27 | int nearby_frame; 28 | Eigen::Isometry3d Tbl; 29 | }; 30 | 31 | // define lidar parameter 32 | class LidarParam{ 33 | public: 34 | LidarParam(){}; 35 | void loadParam(std::string& path); 36 | double getMapResolution(void); 37 | Eigen::Matrix getOdomN(void); 38 | double getEdgeN(); 39 | double getPlaneN(); 40 | double getLocalMapSize(void); 41 | double getLocalMapResolution(void); 42 | double getMapCellWidth(void); 43 | double getMapCellHeight(void); 44 | double getMapCellDepth(void); 45 | int getMapCellWidthRange(void); 46 | int getMapCellHeightRange(void); 47 | int getMapCellDepthRange(void); 48 | string rgb_topic; 49 | string depth_topic; 50 | double camera_factor; 51 | double camera_cx; 52 | double camera_cy; 53 | double camera_fx; 54 | double camera_fy; 55 | double min_distance; 56 | double max_distance; 57 | int gap_plane; 58 | int gap_line; 59 | private: 60 | int frequency; 61 | double horizontal_angle; 62 | double map_resolution; 63 | Eigen::Matrix odom_n; 64 | double edge_n; 65 | double plane_n; 66 | double local_map_resolution; 67 | double local_map_size; 68 | double map_cell_width; 69 | double map_cell_height; 70 | double map_cell_depth; 71 | int map_cell_width_range; 72 | int map_cell_height_range; 73 | int map_cell_depth_range; 74 | }; 75 | 76 | class ImuParam{ 77 | public: 78 | ImuParam(){}; 79 | void loadParam(std::string& path); 80 | int getFrequency(void); 81 | double getAccN(void); 82 | double getGyrN(void); 83 | double getAccW(void); 84 | double getGyrW(void); 85 | string imu_topic; 86 | private: 87 | int frequency; 88 | double acc_n; 89 | double gyr_n; 90 | double acc_w; 91 | double gyr_w; 92 | }; 93 | 94 | 95 | #endif // _PARAM_H_ 96 | 97 | -------------------------------------------------------------------------------- /include/peac/AHCParamSet.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright 2014 Mitsubishi Electric Research Laboratories All 3 | // Rights Reserved. 4 | // 5 | // Permission to use, copy and modify this software and its 6 | // documentation without fee for educational, research and non-profit 7 | // purposes, is hereby granted, provided that the above copyright 8 | // notice, this paragraph, and the following three paragraphs appear 9 | // in all copies. 10 | // 11 | // To request permission to incorporate this software into commercial 12 | // products contact: Director; Mitsubishi Electric Research 13 | // Laboratories (MERL); 201 Broadway; Cambridge, MA 02139. 14 | // 15 | // IN NO EVENT SHALL MERL BE LIABLE TO ANY PARTY FOR DIRECT, 16 | // INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING 17 | // LOST PROFITS, ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS 18 | // DOCUMENTATION, EVEN IF MERL HAS BEEN ADVISED OF THE POSSIBILITY OF 19 | // SUCH DAMAGES. 20 | // 21 | // MERL SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT 22 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | // FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS ON AN 24 | // "AS IS" BASIS, AND MERL HAS NO OBLIGATIONS TO PROVIDE MAINTENANCE, 25 | // SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. 26 | // 27 | #pragma once 28 | 29 | #include "math.h" 30 | 31 | namespace ahc { 32 | 33 | #define MACRO_DEG2RAD(d) ((d)*M_PI/180.0) 34 | #define MACRO_RAD2DEG(r) ((r)*180.0/M_PI) 35 | 36 | enum InitType { 37 | INIT_STRICT=0, //no nan point is allowed in any valid init blocks 38 | INIT_LOOSE=1 //at most half of a init block can be nan point 39 | }; 40 | 41 | /** 42 | * \brief ParamSet is a struct representing a set of parameters used in ahc::PlaneFitter 43 | */ 44 | struct ParamSet { 45 | // related to T_mse 46 | double depthSigma; //\sigma in the paper, unit: u^-1 mm^-1 47 | double stdTol_init; //\epsilon in the paper, used when init graph, unit: u mm 48 | double stdTol_merge; //\epsilon in the paper, used when merging nodes, unit: u mm 49 | 50 | // related to T_ang 51 | double z_near, z_far; //unit: u mm, closest/farthest z to be considered 52 | double angle_near, angle_far; //unit: rad, corresponding normal deviation angle 53 | double similarityTh_merge; //unit: none, 1 means the same, 0 means perpendicular 54 | double similarityTh_refine; //unit: none 55 | 56 | // related to T_dz 57 | double depthAlpha; //unit: none, corresponds to the 2*\alpha in the paper 58 | double depthChangeTol; //unit: u mm 59 | 60 | InitType initType; 61 | 62 | enum Phase { 63 | P_INIT=0, 64 | P_MERGING=1, 65 | P_REFINE=2 66 | }; 67 | 68 | ParamSet() : depthSigma(1.6e-6), 69 | stdTol_init(5), stdTol_merge(8), 70 | z_near(250), z_far(9000), 71 | angle_near(MACRO_DEG2RAD(5.0)), angle_far(MACRO_DEG2RAD(90.0)), 72 | similarityTh_merge(std::cos(MACRO_DEG2RAD(60.0))), 73 | similarityTh_refine(std::cos(MACRO_DEG2RAD(30.0))), 74 | depthAlpha(0.04), depthChangeTol(0.02), 75 | initType(INIT_STRICT) 76 | {} 77 | 78 | /** 79 | * \brief Dynamic MSE threshold, depending on depth z 80 | * 81 | * \param [in] phase specify whether invoked in initGraph or when merging 82 | * \param [in] z current depth, unit: u mm 83 | * \return the MSE threshold at depth z, unit: u^2 mm^2 84 | * 85 | * \details Reference: 2012.Sensors.Khoshelham.Accuracy and Resolution of Kinect Depth Data for Indoor Mapping Applications 86 | */ 87 | inline double T_mse(const Phase phase, const double z=0) const { 88 | //theoretical point-plane distance std = sigma * z * z 89 | //sigma corresponds to \sigma * (m/f/b) in the 2012.Khoshelham paper 90 | //we add a stdTol to move the theoretical curve up as tolerances 91 | switch(phase) { 92 | case P_INIT: 93 | return std::pow(depthSigma*z*z+stdTol_init,2); 94 | case P_MERGING: 95 | case P_REFINE: 96 | default: 97 | return std::pow(depthSigma*z*z+stdTol_merge,2); 98 | } 99 | } 100 | 101 | /** 102 | * \brief Dynamic normal deviation threshold, depending on depth z 103 | * 104 | * \param [in] phase specify whether invoked in initGraph or when merging 105 | * \param [in] z current depth (z>=0) 106 | * \return cos of the normal deviation threshold at depth z 107 | * 108 | * \details This is simply a linear mapping from depth to thresholding angle 109 | * and the threshold will be used to reject edge when initialize the graph; 110 | * this function corresponds to T_{ANG} in our paper 111 | */ 112 | inline double T_ang(const Phase phase, const double z=0) const { 113 | switch(phase) { 114 | case P_INIT: 115 | {//linear maping z->thresholding angle, clipping z also 116 | double clipped_z = z; 117 | clipped_z=std::max(clipped_z,z_near); 118 | clipped_z=std::min(clipped_z,z_far); 119 | const double factor = (angle_far-angle_near)/(z_far-z_near); 120 | return std::cos(factor*clipped_z+angle_near-factor*z_near); 121 | } 122 | case P_MERGING: 123 | { 124 | return similarityTh_merge; 125 | } 126 | case P_REFINE: 127 | default: 128 | { 129 | return similarityTh_refine; 130 | } 131 | } 132 | } 133 | 134 | /** 135 | * \brief Dynamic threshold to test whether the two adjacent pixels are discontinuous in depth 136 | * 137 | * \param [in] z depth of the current pixel 138 | * \return the max depth change allowed at depth z to make the points connected in a single block 139 | * 140 | * \details This is modified from pcl's segmentation code as well as suggested in 2013.iros.Holzer 141 | * essentially returns factor*z+tolerance 142 | * (TODO: maybe change this to 3D-point distance threshold) 143 | */ 144 | inline double T_dz(const double z) const { 145 | return depthAlpha * fabs(z) + depthChangeTol; 146 | } 147 | };//ParamSet 148 | 149 | }//ahc -------------------------------------------------------------------------------- /include/peac/AHCPlaneSeg.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright 2014 Mitsubishi Electric Research Laboratories All 3 | // Rights Reserved. 4 | // 5 | // Permission to use, copy and modify this software and its 6 | // documentation without fee for educational, research and non-profit 7 | // purposes, is hereby granted, provided that the above copyright 8 | // notice, this paragraph, and the following three paragraphs appear 9 | // in all copies. 10 | // 11 | // To request permission to incorporate this software into commercial 12 | // products contact: Director; Mitsubishi Electric Research 13 | // Laboratories (MERL); 201 Broadway; Cambridge, MA 02139. 14 | // 15 | // IN NO EVENT SHALL MERL BE LIABLE TO ANY PARTY FOR DIRECT, 16 | // INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING 17 | // LOST PROFITS, ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS 18 | // DOCUMENTATION, EVEN IF MERL HAS BEEN ADVISED OF THE POSSIBILITY OF 19 | // SUCH DAMAGES. 20 | // 21 | // MERL SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT 22 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | // FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS ON AN 24 | // "AS IS" BASIS, AND MERL HAS NO OBLIGATIONS TO PROVIDE MAINTENANCE, 25 | // SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. 26 | // 27 | #pragma once 28 | 29 | #include //PlaneSeg::NbSet 30 | #include //mseseq 31 | #include //quiet_NaN 32 | 33 | #include "AHCTypes.hpp" //shared_ptr 34 | #include "eig33sym.hpp" //PlaneSeg::Stats::compute 35 | #include "AHCParamSet.hpp" //depthDisContinuous 36 | #include "DisjointSet.hpp" //PlaneSeg::mergeNbsFrom 37 | 38 | namespace ahc { 39 | 40 | //return true if d0 and d1 is discontinuous 41 | inline static bool depthDisContinuous(const double d0, const double d1, const ParamSet& params) 42 | { 43 | return fabs(d0-d1) > params.T_dz(d0); 44 | } 45 | 46 | /** 47 | * \brief PlaneSeg is a struct representing a Plane Segment as a node of a graph 48 | * 49 | * \details It is usually dynamically allocated and garbage collected by boost::shared_ptr 50 | */ 51 | struct PlaneSeg { 52 | typedef PlaneSeg* Ptr; 53 | typedef ahc::shared_ptr shared_ptr; 54 | 55 | /** 56 | * \brief An internal struct holding this PlaneSeg's member points' 1st and 2nd order statistics 57 | * 58 | * \details It is usually dynamically allocated and garbage collected by boost::shared_ptr 59 | */ 60 | struct Stats { 61 | double sx, sy, sz, //sum of x/y/z 62 | sxx, syy, szz, //sum of xx/yy/zz 63 | sxy, syz, sxz; //sum of xy/yz/xz 64 | int N; //#points in this PlaneSeg 65 | 66 | Stats() : sx(0), sy(0), sz(0), 67 | sxx(0), syy(0), szz(0), 68 | sxy(0), syz(0), sxz(0), N(0) {} 69 | 70 | //merge from two other Stats 71 | Stats(const Stats& a, const Stats& b) : 72 | sx(a.sx+b.sx), sy(a.sy+b.sy), sz(a.sz+b.sz), 73 | sxx(a.sxx+b.sxx), syy(a.syy+b.syy), szz(a.szz+b.szz), 74 | sxy(a.sxy+b.sxy), syz(a.syz+b.syz), sxz(a.sxz+b.sxz), N(a.N+b.N) {} 75 | 76 | inline void clear() { 77 | sx=sy=sz=sxx=syy=szz=sxy=syz=sxz=0; 78 | N=0; 79 | } 80 | 81 | //push a new point (x,y,z) into this Stats 82 | inline void push(const double x, const double y, const double z) { 83 | sx+=x; sy+=y; sz+=z; 84 | sxx+=x*x; syy+=y*y; szz+=z*z; 85 | sxy+=x*y; syz+=y*z; sxz+=x*z; 86 | ++N; 87 | } 88 | 89 | //push a new Stats into this Stats 90 | inline void push(const Stats& other) { 91 | sx+=other.sx; sy+=other.sy; sz+=other.sz; 92 | sxx+=other.sxx; syy+=other.syy; szz+=other.szz; 93 | sxy+=other.sxy; syz+=other.syz; sxz+=other.sxz; 94 | N+=other.N; 95 | } 96 | 97 | //caller is responsible to ensure (x,y,z) was collected in this stats 98 | inline void pop(const double x, const double y, const double z) { 99 | sx-=x; sy-=y; sz-=z; 100 | sxx-=x*x; syy-=y*y; szz-=z*z; 101 | sxy-=x*y; syz-=y*z; sxz-=x*z; 102 | --N; 103 | 104 | assert(N>=0); 105 | } 106 | 107 | //caller is responsible to ensure {other} were collected in this stats 108 | inline void pop(const Stats& other) { 109 | sx-=other.sx; sy-=other.sy; sz-=other.sz; 110 | sxx-=other.sxx; syy-=other.syy; szz-=other.szz; 111 | sxy-=other.sxy; syz-=other.syz; sxz-=other.sxz; 112 | N-=other.N; 113 | 114 | assert(N>=0); 115 | } 116 | 117 | /** 118 | * \brief PCA-based plane fitting 119 | * 120 | * \param [out] center center of mass of the PlaneSeg 121 | * \param [out] normal unit normal vector of the PlaneSeg (ensure normal.z>=0) 122 | * \param [out] mse mean-square-error of the plane fitting 123 | * \param [out] curvature defined as in pcl 124 | */ 125 | inline void compute(double center[3], double normal[3], 126 | double& mse, double& curvature) const 127 | { 128 | assert(N>=4); 129 | 130 | const double sc=((double)1.0)/this->N;//this->ids.size(); 131 | //calc plane equation: center, normal and mse 132 | center[0]=sx*sc; 133 | center[1]=sy*sc; 134 | center[2]=sz*sc; 135 | double K[3][3] = { 136 | {sxx-sx*sx*sc,sxy-sx*sy*sc,sxz-sx*sz*sc}, 137 | { 0,syy-sy*sy*sc,syz-sy*sz*sc}, 138 | { 0, 0,szz-sz*sz*sc} 139 | }; 140 | K[1][0]=K[0][1]; K[2][0]=K[0][2]; K[2][1]=K[1][2]; 141 | double sv[3]={0,0,0}; 142 | double V[3][3]={0}; 143 | LA::eig33sym(K, sv, V); //!!! first eval is the least one 144 | //LA.svd33(K, sv, V); 145 | if(V[0][0]*center[0]+V[1][0]*center[1]+V[2][0]*center[2]<=0) {//enforce dot(normal,center)<00 so normal always points towards camera 146 | normal[0]=V[0][0]; 147 | normal[1]=V[1][0]; 148 | normal[2]=V[2][0]; 149 | } else { 150 | normal[0]=-V[0][0]; 151 | normal[1]=-V[1][0]; 152 | normal[2]=-V[2][0]; 153 | } 154 | mse = sv[0]*sc; 155 | curvature=sv[0]/(sv[0]+sv[1]+sv[2]); 156 | } 157 | } stats; //member points' 1st & 2nd order statistics 158 | 159 | int rid; //root block id 160 | double mse; //mean square error 161 | double center[3]; //q: plane center (center of mass) 162 | double normal[3]; //n: plane equation n'p=q 163 | int N; //#member points, same as stats.N 164 | double curvature; 165 | bool nouse; //this PlaneSeg will be marked as nouse after merged with others to produce a new PlaneSeg node in the graph 166 | 167 | #ifdef DEBUG_INIT 168 | enum Type { 169 | TYPE_NORMAL=0, //default value 170 | TYPE_MISSING_DATA=1, 171 | TYPE_DEPTH_DISCONTINUE=2 172 | } type; 173 | #endif 174 | 175 | #if defined(DEBUG_INIT) || defined(DEBUG_CLUSTER) 176 | cv::Vec3b clr; 177 | cv::Vec3b normalClr; 178 | cv::Vec3b& getColor(bool useNormal=true) { 179 | if(useNormal) return normalClr; 180 | return clr; 181 | } 182 | #endif 183 | 184 | #ifdef DEBUG_CALC 185 | std::vector mseseq; 186 | #endif 187 | 188 | typedef std::set NbSet; //no ownership of its content 189 | NbSet nbs; //neighbors, i.e. adjacency list for a graph structure 190 | 191 | inline void update() { 192 | this->stats.compute(this->center, this->normal, this->mse, this->curvature); 193 | } 194 | 195 | /** 196 | * \brief construct a PlaneSeg during graph initialization 197 | * 198 | * \param [in] points organized point cloud adapter, see NullImage3D 199 | * \param [in] root_block_id initial window/block's id 200 | * \param [in] seed_row row index of the upper left pixel of the initial window/block 201 | * \param [in] seed_col row index of the upper left pixel of the initial window/block 202 | * \param [in] imgWidth width of the organized point cloud 203 | * \param [in] imgHeight height of the organized point cloud 204 | * \param [in] winWidth width of the initial window/block 205 | * \param [in] winHeight height of the initial window/block 206 | * \param [in] depthChangeFactor parameter to determine depth discontinuity 207 | * 208 | * \details if exist depth discontinuity in this initial PlaneSeg, nouse will be set true and N 0. 209 | */ 210 | template 211 | PlaneSeg(const Image3D& points, const int root_block_id, 212 | const int seed_row, const int seed_col, 213 | const int imgWidth, const int imgHeight, 214 | const int winWidth, const int winHeight, 215 | const ParamSet& params) 216 | { 217 | //assert(0<=seed_row && seed_row0 && winH>0); 218 | this->rid = root_block_id; 219 | 220 | bool windowValid=true; 221 | int nanCnt=0, nanCntTh=winHeight*winWidth/2; 222 | //calc stats 223 | for(int i=seed_row, icnt=0; icnttype=TYPE_MISSING_DATA; 233 | #endif 234 | windowValid=false; break; 235 | } 236 | double xn=0,yn=0,zn=10000; 237 | if(j+1type=TYPE_DEPTH_DISCONTINUE; 241 | #endif 242 | windowValid=false; break; 243 | } 244 | if(i+1type=TYPE_DEPTH_DISCONTINUE; 248 | #endif 249 | windowValid=false; break; 250 | } 251 | this->stats.push(x,y,z); 252 | } 253 | if(!windowValid) break; 254 | } 255 | if(windowValid) {//if nan or depth-discontinuity shows, this obj will be rejected 256 | this->nouse=false; 257 | this->N=this->stats.N; 258 | #ifdef DEBUG_INIT 259 | this->type=TYPE_NORMAL; 260 | #endif 261 | } else { 262 | this->N=0; 263 | this->stats.clear(); 264 | this->nouse=true; 265 | } 266 | 267 | if(this->N<4) { 268 | this->mse=this->curvature=std::numeric_limits::quiet_NaN(); 269 | } else { 270 | this->stats.compute(this->center, this->normal, this->mse, this->curvature); 271 | #ifdef DEBUG_CALC 272 | this->mseseq.push_back(cv::Vec2d(this->N,this->mse)); 273 | #endif 274 | //nbs information to be maintained outside the class 275 | //typically when initializing the graph structure 276 | } 277 | #if defined(DEBUG_INIT) || defined(DEBUG_CLUSTER) 278 | const uchar clx=uchar((this->normal[0]+1.0)*0.5*255.0); 279 | const uchar cly=uchar((this->normal[1]+1.0)*0.5*255.0); 280 | const uchar clz=uchar((this->normal[2]+1.0)*0.5*255.0); 281 | this->normalClr=cv::Vec3b(clx,cly,clz); 282 | this->clr=cv::Vec3b(rand()%255,rand()%255,rand()%255); 283 | #endif 284 | //std::cout<curvature<type=TYPE_NORMAL; 297 | #endif 298 | this->nouse=false; 299 | this->rid = pa.N>=pb.N ? pa.rid : pb.rid; 300 | this->N=this->stats.N; 301 | 302 | //ds.union(pa.rid, pb.rid) will be called later 303 | //in mergeNbsFrom(pa,pb) function, since 304 | //this object might not be accepted into the graph structure 305 | 306 | this->stats.compute(this->center, this->normal, this->mse, this->curvature); 307 | 308 | #if defined(DEBUG_CLUSTER) 309 | const uchar clx=uchar((this->normal[0]+1.0)*0.5*255.0); 310 | const uchar cly=uchar((this->normal[1]+1.0)*0.5*255.0); 311 | const uchar clz=uchar((this->normal[2]+1.0)*0.5*255.0); 312 | this->normalClr=cv::Vec3b(clx,cly,clz); 313 | this->clr=cv::Vec3b(rand()%255,rand()%255,rand()%255); 314 | #endif 315 | //nbs information to be maintained later if this node is accepted 316 | } 317 | 318 | /** 319 | * \brief similarity of two plane normals 320 | * 321 | * \param [in] p another PlaneSeg 322 | * \return abs(dot(this->normal, p->normal)) 323 | * 324 | * \details 1 means identical, 0 means perpendicular 325 | */ 326 | inline double normalSimilarity(const PlaneSeg& p) const { 327 | return std::abs(normal[0]*p.normal[0]+ 328 | normal[1]*p.normal[1]+ 329 | normal[2]*p.normal[2]); 330 | } 331 | 332 | /** 333 | * \brief signed distance between this plane and the point pt[3] 334 | */ 335 | inline double signedDist(const double pt[3]) const { 336 | return normal[0]*(pt[0]-center[0])+ 337 | normal[1]*(pt[1]-center[1])+ 338 | normal[2]*(pt[2]-center[2]); 339 | } 340 | 341 | /** 342 | * \brief connect this PlaneSeg to another PlaneSeg p in the graph 343 | * 344 | * \param [in] p the other PlaneSeg 345 | */ 346 | inline void connect(PlaneSeg::Ptr p) { 347 | if(p) { 348 | this->nbs.insert(p); 349 | p->nbs.insert(this); 350 | } 351 | } 352 | 353 | /** 354 | * \brief disconnect this PlaneSeg with all its neighbors 355 | * 356 | * \details after this call, this->nbs.nbs should not contain this, and this->nbs should be empty i.e. after this call this PlaneSeg node should be isolated in the graph 357 | */ 358 | inline void disconnectAllNbs() { 359 | NbSet::iterator itr = this->nbs.begin(); 360 | for(; itr!=this->nbs.end(); ++itr) { 361 | PlaneSeg::Ptr nb = (*itr); 362 | if(!nb->nbs.erase(this)) { 363 | std::cout<<"[PlaneSeg warn] this->nbs.nbs" 364 | " should have contained this!"<nbs.clear(); 368 | } 369 | 370 | /** 371 | * \brief finish merging PlaneSeg pa and pb to this 372 | * 373 | * \param [in] pa a parent PlaneSeg of this 374 | * \param [in] pb another parent PlaneSeg of this 375 | * \param [in] ds the disjoint set of initial window/block membership to be updated 376 | * 377 | * \details Only call this if this obj is accepted to be added to the graph of PlaneSeg pa and pb should not exist after this function is called, i.e. after this call this PlaneSeg node will be representing a merged node of pa and pb, and pa/pb will be isolated (and thus Garbage Collected) in the graph 378 | */ 379 | inline void mergeNbsFrom(PlaneSeg& pa, PlaneSeg& pb, DisjointSet& ds) { 380 | //now we are sure that merging pa and pb is accepted 381 | ds.Union(pa.rid, pb.rid); 382 | 383 | //the new neighbors should be pa.nbs+pb.nbs-pa-pb 384 | this->nbs.insert(pa.nbs.begin(), pa.nbs.end()); 385 | this->nbs.insert(pb.nbs.begin(), pb.nbs.end()); 386 | this->nbs.erase(&pa); 387 | this->nbs.erase(&pb); 388 | 389 | //pa and pb should be GC later after the following two steps 390 | pa.disconnectAllNbs(); 391 | pb.disconnectAllNbs(); 392 | 393 | //complete the neighborhood from the other side 394 | NbSet::iterator itr = this->nbs.begin(); 395 | for(; itr!=this->nbs.end(); ++itr) { 396 | PlaneSeg::Ptr nb = (*itr); 397 | nb->nbs.insert(this); 398 | } 399 | 400 | pa.nouse=pb.nouse=true; 401 | #ifdef DEBUG_CALC 402 | if(pa.N>=pb.N) { 403 | this->mseseq.swap(pa.mseseq); 404 | } else { 405 | this->mseseq.swap(pb.mseseq); 406 | } 407 | this->mseseq.push_back(cv::Vec2d(this->N,this->mse)); 408 | #endif 409 | } 410 | };//PlaneSeg 411 | 412 | }//ahc -------------------------------------------------------------------------------- /include/peac/AHCTypes.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright 2014 Mitsubishi Electric Research Laboratories All 3 | // Rights Reserved. 4 | // 5 | // Permission to use, copy and modify this software and its 6 | // documentation without fee for educational, research and non-profit 7 | // purposes, is hereby granted, provided that the above copyright 8 | // notice, this paragraph, and the following three paragraphs appear 9 | // in all copies. 10 | // 11 | // To request permission to incorporate this software into commercial 12 | // products contact: Director; Mitsubishi Electric Research 13 | // Laboratories (MERL); 201 Broadway; Cambridge, MA 02139. 14 | // 15 | // IN NO EVENT SHALL MERL BE LIABLE TO ANY PARTY FOR DIRECT, 16 | // INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING 17 | // LOST PROFITS, ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS 18 | // DOCUMENTATION, EVEN IF MERL HAS BEEN ADVISED OF THE POSSIBILITY OF 19 | // SUCH DAMAGES. 20 | // 21 | // MERL SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT 22 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | // FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS ON AN 24 | // "AS IS" BASIS, AND MERL HAS NO OBLIGATIONS TO PROVIDE MAINTENANCE, 25 | // SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. 26 | // 27 | #pragma once 28 | 29 | #ifndef USE_BOOST_SHARED_PTR 30 | #include 31 | #else 32 | #include 33 | #endif 34 | 35 | namespace ahc { 36 | #ifndef USE_BOOST_SHARED_PTR 37 | using std::shared_ptr; 38 | #else 39 | using boost::shared_ptr; 40 | #endif 41 | } -------------------------------------------------------------------------------- /include/peac/AHCUtils.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright 2014 Mitsubishi Electric Research Laboratories All 3 | // Rights Reserved. 4 | // 5 | // Permission to use, copy and modify this software and its 6 | // documentation without fee for educational, research and non-profit 7 | // purposes, is hereby granted, provided that the above copyright 8 | // notice, this paragraph, and the following three paragraphs appear 9 | // in all copies. 10 | // 11 | // To request permission to incorporate this software into commercial 12 | // products contact: Director; Mitsubishi Electric Research 13 | // Laboratories (MERL); 201 Broadway; Cambridge, MA 02139. 14 | // 15 | // IN NO EVENT SHALL MERL BE LIABLE TO ANY PARTY FOR DIRECT, 16 | // INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING 17 | // LOST PROFITS, ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS 18 | // DOCUMENTATION, EVEN IF MERL HAS BEEN ADVISED OF THE POSSIBILITY OF 19 | // SUCH DAMAGES. 20 | // 21 | // MERL SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT 22 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | // FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS ON AN 24 | // "AS IS" BASIS, AND MERL HAS NO OBLIGATIONS TO PROVIDE MAINTENANCE, 25 | // SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. 26 | // 27 | #pragma once 28 | 29 | #include 30 | #include "opencv2/opencv.hpp" 31 | 32 | namespace ahc { 33 | namespace utils { 34 | 35 | /** 36 | * \brief Generate pseudo-colors 37 | * 38 | * \param [in] ncolors number of colors to be generated (will be reset to 10 if ncolors<=0) 39 | * \return a vector of cv::Vec3b 40 | */ 41 | inline std::vector pseudocolor(int ncolors) { 42 | srand((unsigned int)time(0)); 43 | if(ncolors<=0) ncolors=10; 44 | std::vector ret(ncolors); 45 | for(int i=0; iscale = scale; 69 | startTick=0; 70 | } 71 | 72 | /** 73 | start record time, similar to matlab function "tic"; 74 | 75 | @return the start tick 76 | */ 77 | inline double tic() { 78 | return startTick = (double)cv::getTickCount(); 79 | } 80 | 81 | /** 82 | return duration from last tic, in (second * scale), similar to matlab function "toc" 83 | 84 | @return duration from last tic, in (second * scale) 85 | */ 86 | inline double toc() { 87 | return ((double)cv::getTickCount()-startTick)/cv::getTickFrequency() * scale; 88 | } 89 | inline double toc(std::string tag) { 90 | double time=toc(); 91 | std::cout< 30 | 31 | class DisjointSet 32 | { 33 | private: 34 | std::vector parent_; 35 | std::vector size_; 36 | 37 | public: 38 | DisjointSet(const int n) 39 | { 40 | parent_.reserve(n); 41 | size_.reserve(n); 42 | 43 | for (int i=0; i 110 | 111 | **Feel free to email any bugs or suggestions to help us improve the code. Thank you!** -------------------------------------------------------------------------------- /include/peac/eig33sym.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright 2014 Mitsubishi Electric Research Laboratories All 3 | // Rights Reserved. 4 | // 5 | // Permission to use, copy and modify this software and its 6 | // documentation without fee for educational, research and non-profit 7 | // purposes, is hereby granted, provided that the above copyright 8 | // notice, this paragraph, and the following three paragraphs appear 9 | // in all copies. 10 | // 11 | // To request permission to incorporate this software into commercial 12 | // products contact: Director; Mitsubishi Electric Research 13 | // Laboratories (MERL); 201 Broadway; Cambridge, MA 02139. 14 | // 15 | // IN NO EVENT SHALL MERL BE LIABLE TO ANY PARTY FOR DIRECT, 16 | // INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING 17 | // LOST PROFITS, ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS 18 | // DOCUMENTATION, EVEN IF MERL HAS BEEN ADVISED OF THE POSSIBILITY OF 19 | // SUCH DAMAGES. 20 | // 21 | // MERL SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT 22 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | // FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS ON AN 24 | // "AS IS" BASIS, AND MERL HAS NO OBLIGATIONS TO PROVIDE MAINTENANCE, 25 | // SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. 26 | // 27 | #pragma once 28 | // 29 | // Note: 30 | // If you use dsyevh3 library [http://www.mpi-hd.mpg.de/personalhomes/globes/3x3/], 31 | // this function can be accelerated by uncommenting the following line 32 | //#define USE_DSYEVH3 33 | // 34 | #ifdef USE_DSYEVH3 35 | #include "dsyevh3/dsyevh3.h" 36 | #else 37 | #include 38 | #include 39 | #endif 40 | 41 | #include 42 | #include 43 | 44 | namespace LA { 45 | //s[0]<=s[1]<=s[2], V[:][i] correspond to s[i] 46 | inline static bool eig33sym(double K[3][3], double s[3], double V[3][3]) 47 | { 48 | #ifdef USE_DSYEVH3 49 | double tmpV[3][3]; 50 | if(dsyevh3(K, tmpV, s)!=0) return false; 51 | 52 | int order[]={0,1,2}; 53 | for(int i=0; i<3; ++i) { 54 | for(int j=i+1; j<3; ++j) { 55 | if(s[i]>s[j]) { 56 | double tmp=s[i]; 57 | s[i]=s[j]; 58 | s[j]=tmp; 59 | int tmpor=order[i]; 60 | order[i]=order[j]; 61 | order[j]=tmpor; 62 | } 63 | } 64 | } 65 | V[0][0]=tmpV[0][order[0]]; V[0][1]=tmpV[0][order[1]]; V[0][2]=tmpV[0][order[2]]; 66 | V[1][0]=tmpV[1][order[0]]; V[1][1]=tmpV[1][order[1]]; V[1][2]=tmpV[1][order[2]]; 67 | V[2][0]=tmpV[2][order[0]]; V[2][1]=tmpV[2][order[1]]; V[2][2]=tmpV[2][order[2]]; 68 | #else 69 | //below we did not specify row major since it does not matter, K==K' 70 | Eigen::SelfAdjointEigenSolver es( 71 | Eigen::Map(K[0], 3, 3) ); 72 | Eigen::Map(s,3,1)=es.eigenvalues(); 73 | //below we need to specify row major since V!=V' 74 | Eigen::Map>(V[0],3,3)=es.eigenvectors(); 75 | #endif 76 | return true; 77 | } 78 | }//end of namespace LA -------------------------------------------------------------------------------- /include/poseOptimizationFactor.h: -------------------------------------------------------------------------------- 1 | // Author of SSL_SLAM3: Wang Han 2 | // Email wh200720041@gmail.com 3 | // Homepage https://wanghan.pro 4 | #ifndef _PARAMETER_FACTOR_H_ 5 | #define _PARAMETER_FACTOR_H_ 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include "utils.h" 13 | 14 | class PoseParameterization : public ceres::LocalParameterization { 15 | public: 16 | PoseParameterization() {} 17 | virtual ~PoseParameterization() {} 18 | virtual bool Plus(const double* x, const double* delta, double* x_plus_delta) const; 19 | virtual bool ComputeJacobian(const double* x, double* jacobian) const; 20 | virtual int GlobalSize() const { return 15; } 21 | virtual int LocalSize() const { return 15; } 22 | }; 23 | 24 | class ConstantPoseParameterization : public ceres::LocalParameterization { 25 | public: 26 | 27 | ConstantPoseParameterization() {} 28 | virtual ~ConstantPoseParameterization() {} 29 | virtual bool Plus(const double* x, const double* delta, double* x_plus_delta) const; 30 | virtual bool ComputeJacobian(const double* x, double* jacobian) const; 31 | virtual int GlobalSize() const { return 15; } 32 | virtual int LocalSize() const { return 15; } 33 | }; 34 | 35 | 36 | #endif // _PARAMETER_FACTOR_H_ 37 | 38 | -------------------------------------------------------------------------------- /include/utils.h: -------------------------------------------------------------------------------- 1 | // Author of SSL_SLAM3: Wang Han 2 | // Email wh200720041@gmail.com 3 | // Homepage https://wanghan.pro 4 | 5 | #ifndef _UTILS_H_ 6 | #define _UTILS_H_ 7 | 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | const double DEG2RAD = M_PI / 180.0; 16 | #define THETA_THRESHOLD 0.0001 //sin a = a 17 | 18 | class Utils{ 19 | //global variables 20 | public: 21 | static Eigen::Vector3d gravity; 22 | //mathematics 23 | public: 24 | 25 | static Eigen::Matrix3d skew(const Eigen::Vector3d &v); 26 | static Eigen::Vector3d RToso3(const Eigen::Matrix3d& R); 27 | static Eigen::Quaterniond RToq(const Eigen::Matrix3d& R); 28 | static Eigen::Matrix3d qToR(const Eigen::Quaterniond& q); 29 | static Eigen::Vector3d qToso3(const Eigen::Quaterniond& q); 30 | static Eigen::Quaterniond so3Toq(const Eigen::Vector3d& so3); 31 | static Eigen::Matrix3d so3ToR(const Eigen::Vector3d& so3); 32 | static Eigen::Matrix3d Jl_so3(const Eigen::Vector3d& so3); 33 | static Eigen::Matrix3d Jr_so3(const Eigen::Vector3d& so3); 34 | static Eigen::Matrix3d Jl_so3_inv(const Eigen::Vector3d& so3); 35 | static Eigen::Matrix3d Jr_so3_inv(const Eigen::Vector3d& so3); 36 | static Eigen::Matrix3d Jr_R_inv(const Eigen::Matrix3d& R); 37 | 38 | //[q]_L [p]_R = [p]_R [q]_L 39 | static Eigen::Matrix4d quatLeftMatrix(const Eigen::Quaterniond &q); 40 | static Eigen::Matrix4d quatRightMatrix(const Eigen::Quaterniond &q); 41 | static Eigen::Quaterniond deltaQ(const Eigen::Vector3d &theta); 42 | static Eigen::Isometry3d odomToEigen(const nav_msgs::OdometryConstPtr& odom_msg); 43 | static Eigen::Isometry3d so3ToT(const Eigen::Vector3d &rotation, const Eigen::Vector3d &translation); 44 | static Eigen::Matrix3d normalizeR(const Eigen::Matrix3d& R_in); 45 | }; // namespace utils 46 | 47 | class TicToc{ 48 | public: 49 | TicToc(std::string name_in); 50 | void tic(); 51 | void toc(int freq); 52 | 53 | private: 54 | std::chrono::time_point start, end; 55 | unsigned long total_frame; 56 | double total_time; 57 | std::string name; 58 | }; 59 | 60 | #endif //_UTILS_H_ -------------------------------------------------------------------------------- /l515_imu_calibration/README.md: -------------------------------------------------------------------------------- 1 | # rs-imu-calibration Tool: 2 | 3 | ## Goal 4 | The tool is intended to calibrate the IMU built in D435i and L515 cameras 5 | 6 | ## Description 7 | D435i and L515 cameras arrive from the factory with a calibrated IMU device. However the calibration accuracy can be further imporved by a calibration procedure. 8 | 9 | The rs-imu-calibration tool is a code example that walks you through the calibration steps and saves the calibration coefficients to the EEPROM, to be applied automatically by the driver. 10 | 11 | Detailed information, including installation, procedure, and sample calibration demonstration, is described in the following white paper: 12 | IMU Calibration Tool for Intel® RealSense™ Depth Camera White Paper 13 | https://dev.intelrealsense.com/docs/imu-calibration-tool-for-intel-realsense-depth-camera 14 | 15 | ## Limitations 16 | While the tool achieves good overall results, it has limitations that may impact accuracy. Please refer to the above white paper for further information. 17 | 18 | ## Command Line Parameters 19 | 20 | |Flag |Description |Default| 21 | |-----|---|---| 22 | |`-h `|Show help. || 23 | |`-i [gyro_filename]`| Load previously saved results to EEPROM| | 24 | |`-s serial_no`| calibrate device with given serial_no| calibrate first found device| 25 | |`-g `|show graph of data before and after calibration| || 26 | 27 | # Calibration Procedure: 28 | Running: 29 | 30 | `python rs-imu-calibration.py` 31 | 32 | The script runs you through the 6 main orientations of the camera. 33 | For each direction there are the following steps: 34 | * **Rotation:**
35 | * The script prints the following line, describing how to orient the camera:
36 | `Align to direction: [ 0. -1. 0.] Mounting screw pointing down, device facing out`
37 | * Then it prints the status (rotate) and the difference from the desired orientation:
38 | `Status.rotate: [ 1.0157 -0.1037 0.9945]: [False False False]`
39 | * You have to bring the numbers to [0,0,0] and then you are in the right direction and the script moves on to the next status.

40 | 41 | * **Wait to Stablize:**
42 | * The script waits for you to be stable for 3 seconds. Meanwhile there is a countdown message:
43 | `Status.wait_to_stable: 2.8 (secs)`
44 | * When waited for 3 seconds, the script begin to collect data:

45 | 46 | * **Collecting data:**
47 | * Status line is a line of dots. When reaching 20 dots, enough data is collected and script carries on to next orientation.
48 | * If camera is no longer in the precise orientation, data is not collected. 49 | * If camera is moved too much, going back to Rotation status. 50 | 51 | When done all 6 orientations, the following message appears, suggesting you to save the raw data gathered:
52 | `Would you like to save the raw data? Enter footer for saving files (accel_