├── README.md ├── camera_model ├── CMakeLists.txt ├── cmake │ └── FindEigen.cmake ├── include │ └── camodocal │ │ ├── calib │ │ └── CameraCalibration.h │ │ ├── camera_models │ │ ├── Camera.h │ │ ├── CameraFactory.h │ │ ├── CataCamera.h │ │ ├── CostFunctionFactory.h │ │ ├── EquidistantCamera.h │ │ ├── PinholeCamera.h │ │ └── ScaramuzzaCamera.h │ │ ├── chessboard │ │ ├── Chessboard.h │ │ ├── ChessboardCorner.h │ │ ├── ChessboardQuad.h │ │ └── Spline.h │ │ ├── gpl │ │ ├── EigenQuaternionParameterization.h │ │ ├── EigenUtils.h │ │ └── gpl.h │ │ └── sparse_graph │ │ └── Transform.h ├── instruction ├── package.xml ├── readme.md └── src │ ├── calib │ └── CameraCalibration.cc │ ├── camera_models │ ├── Camera.cc │ ├── CameraFactory.cc │ ├── CataCamera.cc │ ├── CostFunctionFactory.cc │ ├── EquidistantCamera.cc │ ├── PinholeCamera.cc │ └── ScaramuzzaCamera.cc │ ├── chessboard │ └── Chessboard.cc │ ├── gpl │ ├── EigenQuaternionParameterization.cc │ └── gpl.cc │ ├── intrinsic_calib.cc │ └── sparse_graph │ └── Transform.cc ├── config ├── mynteye_leishen_indoor.yaml ├── mynteye_leishen_outdoor.yaml ├── vils_odom.rviz └── vils_rviz_config.rviz ├── feature_tracker_ ├── CMakeLists.txt ├── cmake │ └── FindEigen.cmake ├── package.xml └── src │ ├── feature_tracker.cpp │ ├── feature_tracker.h │ ├── feature_tracker_node.cpp │ ├── parameters.cpp │ ├── parameters.h │ └── tic_toc.h ├── fig ├── cover.png ├── device.png └── results.png ├── lidar_compensator ├── CMakeLists.txt ├── cmake │ ├── FindGflags.cmake │ └── FindGlog.cmake ├── package.xml └── src │ ├── CircularBuffer.h │ ├── PointProcessor.cc │ ├── PointProcessor.h │ ├── TicToc.h │ ├── common_ros.h │ ├── math_utils.h │ └── processor_node.cc ├── lidar_mapping ├── CMakeLists.txt ├── include │ ├── aloam_velodyne │ │ ├── common.h │ │ └── tic_toc.h │ ├── fast_gicp │ │ ├── cuda │ │ │ ├── brute_force_knn.cuh │ │ │ ├── compute_derivatives.cuh │ │ │ ├── compute_mahalanobis.cuh │ │ │ ├── covariance_estimation.cuh │ │ │ ├── covariance_regularization.cuh │ │ │ ├── fast_vgicp_cuda.cuh │ │ │ ├── find_voxel_correspondences.cuh │ │ │ ├── gaussian_voxelmap.cuh │ │ │ ├── ndt_compute_derivatives.cuh │ │ │ ├── ndt_cuda.cuh │ │ │ └── vector3_hash.cuh │ │ ├── fast_gicp.hpp │ │ ├── fast_gicp_st.hpp │ │ ├── fast_vgicp.hpp │ │ ├── gicp │ │ │ ├── experimental │ │ │ │ ├── fast_gicp_mp.hpp │ │ │ │ └── fast_gicp_mp_impl.hpp │ │ │ ├── fast_gicp.hpp │ │ │ ├── fast_gicp_st.hpp │ │ │ ├── fast_vgicp.hpp │ │ │ ├── fast_vgicp_cuda.hpp │ │ │ ├── fast_vgicp_voxel.hpp │ │ │ ├── gicp_settings.hpp │ │ │ ├── impl │ │ │ │ ├── fast_gicp_impl.hpp │ │ │ │ ├── fast_gicp_st_impl.hpp │ │ │ │ ├── fast_vgicp_cuda_impl.hpp │ │ │ │ ├── fast_vgicp_impl.hpp │ │ │ │ └── lsq_registration_impl.hpp │ │ │ └── lsq_registration.hpp │ │ ├── ndt │ │ │ ├── impl │ │ │ │ └── ndt_cuda_impl.hpp │ │ │ ├── ndt_cuda.hpp │ │ │ └── ndt_settings.hpp │ │ └── so3 │ │ │ └── so3.hpp │ ├── global_mapping │ │ ├── Pose6D.h │ │ ├── Quaternion.h │ │ ├── Vector3.h │ │ └── util.h │ ├── ikd_Tree │ │ ├── ikd_Tree.cpp │ │ └── ikd_Tree.h │ └── scancontext │ │ ├── KDTreeVectorOfVectorsAdaptor.h │ │ ├── Scancontext.cpp │ │ ├── Scancontext.h │ │ └── nanoflann.hpp ├── launch │ └── map.launch ├── package.xml └── src │ ├── globalMappingIkdTree.cpp │ ├── globalMappingOcTree.cpp │ ├── lidarFactor.hpp │ ├── localMapping.cpp │ └── scanRegistration.cpp └── vils_estimator ├── CMakeLists.txt ├── cmake └── FindEigen.cmake ├── launch ├── mynteye_leishen_indoor.launch ├── mynteye_leishen_odom.launch └── mynteye_leishen_outdoor.launch ├── package.xml └── src ├── estimator.cpp ├── estimator.h ├── estimator_node.cpp ├── factor ├── imu_factor.h ├── integration_base.h ├── marginalization_factor.cpp ├── marginalization_factor.h ├── pose_local_parameterization.cpp ├── pose_local_parameterization.h ├── projection_factor.cpp ├── projection_factor.h ├── projection_td_factor.cpp └── projection_td_factor.h ├── feature_manager.cpp ├── feature_manager.h ├── initial ├── initial_aligment.cpp ├── initial_alignment.h ├── initial_ex_rotation.cpp ├── initial_ex_rotation.h ├── initial_sfm.cpp ├── initial_sfm.h ├── solve_5pts.cpp └── solve_5pts.h ├── lidar_backend.cpp ├── lidar_backend.h ├── lidar_frontend.cpp ├── lidar_frontend.h ├── lidar_functions ├── cov_func_point_to_point.h ├── fast_gicp-master.zip └── fast_gicp │ ├── .clang-format │ ├── .gitignore │ ├── .gitmodules │ ├── .travis.yml │ └── include │ └── fast_gicp │ ├── cuda │ ├── brute_force_knn.cuh │ ├── compute_derivatives.cuh │ ├── compute_mahalanobis.cuh │ ├── covariance_estimation.cuh │ ├── covariance_regularization.cuh │ ├── fast_vgicp_cuda.cuh │ ├── find_voxel_correspondences.cuh │ ├── gaussian_voxelmap.cuh │ ├── ndt_compute_derivatives.cuh │ ├── ndt_cuda.cuh │ └── vector3_hash.cuh │ ├── gicp │ ├── experimental │ │ ├── fast_gicp_mp.hpp │ │ └── fast_gicp_mp_impl.hpp │ ├── fast_gicp.hpp │ ├── fast_gicp_st.hpp │ ├── fast_vgicp.hpp │ ├── fast_vgicp_cuda.hpp │ ├── fast_vgicp_voxel.hpp │ ├── gicp_settings.hpp │ ├── impl │ │ ├── fast_gicp_impl.hpp │ │ ├── fast_gicp_st_impl.hpp │ │ ├── fast_vgicp_cuda_impl.hpp │ │ ├── fast_vgicp_impl.hpp │ │ └── lsq_registration_impl.hpp │ └── lsq_registration.hpp │ ├── ndt │ ├── impl │ │ └── ndt_cuda_impl.hpp │ ├── ndt_cuda.hpp │ └── ndt_settings.hpp │ └── so3 │ └── so3.hpp ├── parameters.cpp ├── parameters.h └── utility ├── CameraPoseVisualization.cpp ├── CameraPoseVisualization.h ├── tic_toc.h ├── utility.cpp ├── utility.h ├── visualization.cpp └── visualization.h /README.md: -------------------------------------------------------------------------------- 1 | # mVIL-Fusion 2 | Monocular Visual-Inertial-LiDAR Simultaneous Localization and Mapping in Challenging Environments 3 | ## 1. Introduction 4 | 5 | ### 1.1 Video 6 |

7 | video 9 |

10 | 11 | ### 1.2 Dataset 12 | #### Device 13 |

14 | 15 | - MYNTEYE-D120 VI sensor https://www.mynteye.com/pages/mynt-eye-d 16 | - LeiShen-C16 3d spinning LiDAR https://www.leishen-lidar.com/ 17 | #### Topic: 18 | - imu_topic: `/mynteye/imu/data_raw` 200hz 19 | - image_topic: `/mynteye/left/image_mono` 30hz 20 | - lidar_topic: `/lslidar_point_cloud` 10hz 21 | #### Description 22 | | Name | Size | 23 | | :-----| ----: | 24 | | 3indoor.bag | 6.0G | 25 | | outdoor.bag | 9.1G | 26 | #### Download: 27 | - Link：https://pan.baidu.com/s/1ERiMsiewaP9C9_nWbJS-9w?pwd=h9m9
28 | - Pin：h9m9 29 | ## 2. Usage 30 | ### 2.1 Build 31 | ``` 32 | sudo add-apt-repository ppa:ubuntu-toolchain-r/test 33 | sudo apt-get update 34 | sudo apt-get install gcc-7 g++-7 35 | sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-5 60 --slave /usr/bin/g++ g++ /usr/bin/g++-5 36 | sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-7 50 --slave /usr/bin/g++ g++ /usr/bin/g++-7 37 | 38 | cd ~/catkin_ws/src 39 | git clone https://github.com/Stan994265/mVIL-Fusion 40 | cd ../ 41 | catkin_make -j1 42 | source ~/catkin_ws/devel/setup.bash 43 | ``` 44 | ### 2.2 Run Demo 45 | ``` 46 | For indoor: 47 | roslaunch vils_estimator mynteye_leishen_indoor.launch 48 | rosbag play ~/3indoor.bag 49 | 50 | For outdoor: 51 | roslaunch vils_estimator mynteye_leishen_outdoor.launch 52 | rosbag play ~/outdoor.bag 53 | 54 | For odometry only: 55 | (Comment line "#define FOR_GLOBAL" in lidar_mapping/src/localMapping.cpp.) 56 | roslaunch vils_estimator mynteye_leishen_odom.launch 57 | ``` 58 | ### 2.3. Save Results 59 | If you want to save global map, set the onboard parameter of the .launch file to 1. 60 |

61 | 62 | -------------------------------------------------------------------------------- /camera_model/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(camera_model) 3 | 4 | set(CMAKE_BUILD_TYPE "Release") 5 | set(CMAKE_CXX_FLAGS "-std=c++11") 6 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -fPIC") 7 | 8 | find_package(catkin REQUIRED COMPONENTS 9 | roscpp 10 | std_msgs 11 | ) 12 | 13 | find_package(Boost REQUIRED COMPONENTS filesystem program_options system) 14 | include_directories(${Boost_INCLUDE_DIRS}) 15 | 16 | find_package(OpenCV REQUIRED) 17 | 18 | # set(EIGEN_INCLUDE_DIR "/usr/local/include/eigen3") 19 | find_package(Ceres REQUIRED) 20 | include_directories(${CERES_INCLUDE_DIRS}) 21 | 22 | 23 | catkin_package( 24 | INCLUDE_DIRS include 25 | LIBRARIES camera_model 26 | CATKIN_DEPENDS roscpp std_msgs 27 | # DEPENDS system_lib 28 | ) 29 | 30 | include_directories( 31 | ${catkin_INCLUDE_DIRS} 32 | ) 33 | 34 | include_directories("include") 35 | 36 | 37 | add_executable(Calibration 38 | src/intrinsic_calib.cc 39 | src/chessboard/Chessboard.cc 40 | src/calib/CameraCalibration.cc 41 | src/camera_models/Camera.cc 42 | src/camera_models/CameraFactory.cc 43 | src/camera_models/CostFunctionFactory.cc 44 | src/camera_models/PinholeCamera.cc 45 | src/camera_models/CataCamera.cc 46 | src/camera_models/EquidistantCamera.cc 47 | src/camera_models/ScaramuzzaCamera.cc 48 | src/sparse_graph/Transform.cc 49 | src/gpl/gpl.cc 50 | src/gpl/EigenQuaternionParameterization.cc) 51 | 52 | add_library(camera_model 53 | src/chessboard/Chessboard.cc 54 | src/calib/CameraCalibration.cc 55 | src/camera_models/Camera.cc 56 | src/camera_models/CameraFactory.cc 57 | src/camera_models/CostFunctionFactory.cc 58 | src/camera_models/PinholeCamera.cc 59 | src/camera_models/CataCamera.cc 60 | src/camera_models/EquidistantCamera.cc 61 | src/camera_models/ScaramuzzaCamera.cc 62 | src/sparse_graph/Transform.cc 63 | src/gpl/gpl.cc 64 | src/gpl/EigenQuaternionParameterization.cc) 65 | 66 | target_link_libraries(Calibration ${Boost_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES}) 67 | target_link_libraries(camera_model ${Boost_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES}) 68 | -------------------------------------------------------------------------------- /camera_model/include/camodocal/calib/CameraCalibration.h: -------------------------------------------------------------------------------- 1 | #ifndef CAMERACALIBRATION_H 2 | #define CAMERACALIBRATION_H 3 | 4 | #include 5 | 6 | #include "camodocal/camera_models/Camera.h" 7 | 8 | namespace camodocal 9 | { 10 | 11 | class CameraCalibration 12 | { 13 | public: 14 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 15 | CameraCalibration(); 16 | 17 | CameraCalibration(Camera::ModelType modelType, 18 | const std::string& cameraName, 19 | const cv::Size& imageSize, 20 | const cv::Size& boardSize, 21 | float squareSize); 22 | 23 | void clear(void); 24 | 25 | void addChessboardData(const std::vector& corners); 26 | 27 | bool calibrate(void); 28 | 29 | int sampleCount(void) const; 30 | std::vector >& imagePoints(void); 31 | const std::vector >& imagePoints(void) const; 32 | std::vector >& scenePoints(void); 33 | const std::vector >& scenePoints(void) const; 34 | CameraPtr& camera(void); 35 | const CameraConstPtr camera(void) const; 36 | 37 | Eigen::Matrix2d& measurementCovariance(void); 38 | const Eigen::Matrix2d& measurementCovariance(void) const; 39 | 40 | cv::Mat& cameraPoses(void); 41 | const cv::Mat& cameraPoses(void) const; 42 | 43 | void drawResults(std::vector& images) const; 44 | 45 | void writeParams(const std::string& filename) const; 46 | 47 | bool writeChessboardData(const std::string& filename) const; 48 | bool readChessboardData(const std::string& filename); 49 | 50 | void setVerbose(bool verbose); 51 | 52 | private: 53 | bool calibrateHelper(CameraPtr& camera, 54 | std::vector& rvecs, std::vector& tvecs) const; 55 | 56 | void optimize(CameraPtr& camera, 57 | std::vector& rvecs, std::vector& tvecs) const; 58 | 59 | template 60 | void readData(std::ifstream& ifs, T& data) const; 61 | 62 | template 63 | void writeData(std::ofstream& ofs, T data) const; 64 | 65 | cv::Size m_boardSize; 66 | float m_squareSize; 67 | 68 | CameraPtr m_camera; 69 | cv::Mat m_cameraPoses; 70 | 71 | std::vector > m_imagePoints; 72 | std::vector > m_scenePoints; 73 | 74 | Eigen::Matrix2d m_measurementCovariance; 75 | 76 | bool m_verbose; 77 | }; 78 | 79 | } 80 | 81 | #endif 82 | -------------------------------------------------------------------------------- /camera_model/include/camodocal/camera_models/CameraFactory.h: -------------------------------------------------------------------------------- 1 | #ifndef CAMERAFACTORY_H 2 | #define CAMERAFACTORY_H 3 | 4 | #include 5 | #include 6 | 7 | #include "camodocal/camera_models/Camera.h" 8 | 9 | namespace camodocal 10 | { 11 | 12 | class CameraFactory 13 | { 14 | public: 15 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 16 | CameraFactory(); 17 | 18 | static boost::shared_ptr instance(void); 19 | 20 | CameraPtr generateCamera(Camera::ModelType modelType, 21 | const std::string& cameraName, 22 | cv::Size imageSize) const; 23 | 24 | CameraPtr generateCameraFromYamlFile(const std::string& filename); 25 | 26 | private: 27 | static boost::shared_ptr m_instance; 28 | }; 29 | 30 | } 31 | 32 | #endif 33 | -------------------------------------------------------------------------------- /camera_model/include/camodocal/camera_models/CostFunctionFactory.h: -------------------------------------------------------------------------------- 1 | #ifndef COSTFUNCTIONFACTORY_H 2 | #define COSTFUNCTIONFACTORY_H 3 | 4 | #include 5 | #include 6 | 7 | #include "camodocal/camera_models/Camera.h" 8 | 9 | namespace ceres 10 | { 11 | class CostFunction; 12 | } 13 | 14 | namespace camodocal 15 | { 16 | 17 | enum 18 | { 19 | CAMERA_INTRINSICS = 1 << 0, 20 | CAMERA_POSE = 1 << 1, 21 | POINT_3D = 1 << 2, 22 | ODOMETRY_INTRINSICS = 1 << 3, 23 | ODOMETRY_3D_POSE = 1 << 4, 24 | ODOMETRY_6D_POSE = 1 << 5, 25 | CAMERA_ODOMETRY_TRANSFORM = 1 << 6 26 | }; 27 | 28 | class CostFunctionFactory 29 | { 30 | public: 31 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 32 | CostFunctionFactory(); 33 | 34 | static boost::shared_ptr instance(void); 35 | 36 | ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera, 37 | const Eigen::Vector3d& observed_P, 38 | const Eigen::Vector2d& observed_p, 39 | int flags) const; 40 | 41 | ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera, 42 | const Eigen::Vector3d& observed_P, 43 | const Eigen::Vector2d& observed_p, 44 | const Eigen::Matrix2d& sqrtPrecisionMat, 45 | int flags) const; 46 | 47 | ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera, 48 | const Eigen::Vector2d& observed_p, 49 | int flags, bool optimize_cam_odo_z = true) const; 50 | 51 | ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera, 52 | const Eigen::Vector2d& observed_p, 53 | const Eigen::Matrix2d& sqrtPrecisionMat, 54 | int flags, bool optimize_cam_odo_z = true) const; 55 | 56 | ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera, 57 | const Eigen::Vector3d& odo_pos, 58 | const Eigen::Vector3d& odo_att, 59 | const Eigen::Vector2d& observed_p, 60 | int flags, bool optimize_cam_odo_z = true) const; 61 | 62 | ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera, 63 | const Eigen::Quaterniond& cam_odo_q, 64 | const Eigen::Vector3d& cam_odo_t, 65 | const Eigen::Vector3d& odo_pos, 66 | const Eigen::Vector3d& odo_att, 67 | const Eigen::Vector2d& observed_p, 68 | int flags) const; 69 | 70 | ceres::CostFunction* generateCostFunction(const CameraConstPtr& cameraLeft, 71 | const CameraConstPtr& cameraRight, 72 | const Eigen::Vector3d& observed_P, 73 | const Eigen::Vector2d& observed_p_left, 74 | const Eigen::Vector2d& observed_p_right) const; 75 | 76 | private: 77 | static boost::shared_ptr m_instance; 78 | }; 79 | 80 | } 81 | 82 | #endif 83 | -------------------------------------------------------------------------------- /camera_model/include/camodocal/chessboard/Chessboard.h: -------------------------------------------------------------------------------- 1 | #ifndef CHESSBOARD_H 2 | #define CHESSBOARD_H 3 | 4 | #include 5 | #include 6 | 7 | namespace camodocal 8 | { 9 | 10 | // forward declarations 11 | class ChessboardCorner; 12 | typedef boost::shared_ptr ChessboardCornerPtr; 13 | class ChessboardQuad; 14 | typedef boost::shared_ptr ChessboardQuadPtr; 15 | 16 | class Chessboard 17 | { 18 | public: 19 | Chessboard(cv::Size boardSize, cv::Mat& image); 20 | 21 | void findCorners(bool useOpenCV = false); 22 | const std::vector& getCorners(void) const; 23 | bool cornersFound(void) const; 24 | 25 | const cv::Mat& getImage(void) const; 26 | const cv::Mat& getSketch(void) const; 27 | 28 | private: 29 | bool findChessboardCorners(const cv::Mat& image, 30 | const cv::Size& patternSize, 31 | std::vector& corners, 32 | int flags, bool useOpenCV); 33 | 34 | bool findChessboardCornersImproved(const cv::Mat& image, 35 | const cv::Size& patternSize, 36 | std::vector& corners, 37 | int flags); 38 | 39 | void cleanFoundConnectedQuads(std::vector& quadGroup, cv::Size patternSize); 40 | 41 | void findConnectedQuads(std::vector& quads, 42 | std::vector& group, 43 | int group_idx, int dilation); 44 | 45 | // int checkQuadGroup(std::vector& quadGroup, 46 | // std::vector& outCorners, 47 | // cv::Size patternSize); 48 | 49 | void labelQuadGroup(std::vector& quad_group, 50 | cv::Size patternSize, bool firstRun); 51 | 52 | void findQuadNeighbors(std::vector& quads, int dilation); 53 | 54 | int augmentBestRun(std::vector& candidateQuads, int candidateDilation, 55 | std::vector& existingQuads, int existingDilation); 56 | 57 | void generateQuads(std::vector& quads, 58 | cv::Mat& image, int flags, 59 | int dilation, bool firstRun); 60 | 61 | bool checkQuadGroup(std::vector& quads, 62 | std::vector& corners, 63 | cv::Size patternSize); 64 | 65 | void getQuadrangleHypotheses(const std::vector< std::vector >& contours, 66 | std::vector< std::pair >& quads, 67 | int classId) const; 68 | 69 | bool checkChessboard(const cv::Mat& image, cv::Size patternSize) const; 70 | 71 | bool checkBoardMonotony(std::vector& corners, 72 | cv::Size patternSize); 73 | 74 | bool matchCorners(ChessboardQuadPtr& quad1, int corner1, 75 | ChessboardQuadPtr& quad2, int corner2) const; 76 | 77 | cv::Mat mImage; 78 | cv::Mat mSketch; 79 | std::vector mCorners; 80 | cv::Size mBoardSize; 81 | bool mCornersFound; 82 | }; 83 | 84 | } 85 | 86 | #endif 87 | -------------------------------------------------------------------------------- /camera_model/include/camodocal/chessboard/ChessboardCorner.h: -------------------------------------------------------------------------------- 1 | #ifndef CHESSBOARDCORNER_H 2 | #define CHESSBOARDCORNER_H 3 | 4 | #include 5 | #include 6 | 7 | namespace camodocal 8 | { 9 | 10 | class ChessboardCorner; 11 | typedef boost::shared_ptr ChessboardCornerPtr; 12 | 13 | class ChessboardCorner 14 | { 15 | public: 16 | ChessboardCorner() : row(0), column(0), needsNeighbor(true), count(0) {} 17 | 18 | float meanDist(int &n) const 19 | { 20 | float sum = 0; 21 | n = 0; 22 | for (int i = 0; i < 4; ++i) 23 | { 24 | if (neighbors[i].get()) 25 | { 26 | float dx = neighbors[i]->pt.x - pt.x; 27 | float dy = neighbors[i]->pt.y - pt.y; 28 | sum += sqrt(dx*dx + dy*dy); 29 | n++; 30 | } 31 | } 32 | return sum / std::max(n, 1); 33 | } 34 | 35 | cv::Point2f pt; // X and y coordinates 36 | int row; // Row and column of the corner 37 | int column; // in the found pattern 38 | bool needsNeighbor; // Does the corner require a neighbor? 39 | int count; // number of corner neighbors 40 | ChessboardCornerPtr neighbors[4]; // pointer to all corner neighbors 41 | }; 42 | 43 | } 44 | 45 | #endif 46 | -------------------------------------------------------------------------------- /camera_model/include/camodocal/chessboard/ChessboardQuad.h: -------------------------------------------------------------------------------- 1 | #ifndef CHESSBOARDQUAD_H 2 | #define CHESSBOARDQUAD_H 3 | 4 | #include 5 | 6 | #include "camodocal/chessboard/ChessboardCorner.h" 7 | 8 | namespace camodocal 9 | { 10 | 11 | class ChessboardQuad; 12 | typedef boost::shared_ptr ChessboardQuadPtr; 13 | 14 | class ChessboardQuad 15 | { 16 | public: 17 | ChessboardQuad() : count(0), group_idx(-1), edge_len(FLT_MAX), labeled(false) {} 18 | 19 | int count; // Number of quad neighbors 20 | int group_idx; // Quad group ID 21 | float edge_len; // Smallest side length^2 22 | ChessboardCornerPtr corners[4]; // Coordinates of quad corners 23 | ChessboardQuadPtr neighbors[4]; // Pointers of quad neighbors 24 | bool labeled; // Has this corner been labeled? 25 | }; 26 | 27 | } 28 | 29 | #endif 30 | -------------------------------------------------------------------------------- /camera_model/include/camodocal/gpl/EigenQuaternionParameterization.h: -------------------------------------------------------------------------------- 1 | #ifndef EIGENQUATERNIONPARAMETERIZATION_H 2 | #define EIGENQUATERNIONPARAMETERIZATION_H 3 | 4 | #include "ceres/local_parameterization.h" 5 | 6 | namespace camodocal 7 | { 8 | 9 | class EigenQuaternionParameterization : public ceres::LocalParameterization 10 | { 11 | public: 12 | virtual ~EigenQuaternionParameterization() {} 13 | virtual bool Plus(const double* x, 14 | const double* delta, 15 | double* x_plus_delta) const; 16 | virtual bool ComputeJacobian(const double* x, 17 | double* jacobian) const; 18 | virtual int GlobalSize() const { return 4; } 19 | virtual int LocalSize() const { return 3; } 20 | 21 | private: 22 | template 23 | void EigenQuaternionProduct(const T z[4], const T w[4], T zw[4]) const; 24 | }; 25 | 26 | 27 | template 28 | void 29 | EigenQuaternionParameterization::EigenQuaternionProduct(const T z[4], const T w[4], T zw[4]) const 30 | { 31 | zw[0] = z[3] * w[0] + z[0] * w[3] + z[1] * w[2] - z[2] * w[1]; 32 | zw[1] = z[3] * w[1] - z[0] * w[2] + z[1] * w[3] + z[2] * w[0]; 33 | zw[2] = z[3] * w[2] + z[0] * w[1] - z[1] * w[0] + z[2] * w[3]; 34 | zw[3] = z[3] * w[3] - z[0] * w[0] - z[1] * w[1] - z[2] * w[2]; 35 | } 36 | 37 | } 38 | 39 | #endif 40 | 41 | -------------------------------------------------------------------------------- /camera_model/include/camodocal/gpl/gpl.h: -------------------------------------------------------------------------------- 1 | #ifndef GPL_H 2 | #define GPL_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace camodocal 9 | { 10 | 11 | template 12 | const T clamp(const T& v, const T& a, const T& b) 13 | { 14 | return std::min(b, std::max(a, v)); 15 | } 16 | 17 | double hypot3(double x, double y, double z); 18 | float hypot3f(float x, float y, float z); 19 | 20 | template 21 | const T normalizeTheta(const T& theta) 22 | { 23 | T normTheta = theta; 24 | 25 | while (normTheta < - M_PI) 26 | { 27 | normTheta += 2.0 * M_PI; 28 | } 29 | while (normTheta > M_PI) 30 | { 31 | normTheta -= 2.0 * M_PI; 32 | } 33 | 34 | return normTheta; 35 | } 36 | 37 | double d2r(double deg); 38 | float d2r(float deg); 39 | double r2d(double rad); 40 | float r2d(float rad); 41 | 42 | double sinc(double theta); 43 | 44 | template 45 | const T square(const T& x) 46 | { 47 | return x * x; 48 | } 49 | 50 | template 51 | const T cube(const T& x) 52 | { 53 | return x * x * x; 54 | } 55 | 56 | template 57 | const T random(const T& a, const T& b) 58 | { 59 | return static_cast(rand()) / RAND_MAX * (b - a) + a; 60 | } 61 | 62 | template 63 | const T randomNormal(const T& sigma) 64 | { 65 | T x1, x2, w; 66 | 67 | do 68 | { 69 | x1 = 2.0 * random(0.0, 1.0) - 1.0; 70 | x2 = 2.0 * random(0.0, 1.0) - 1.0; 71 | w = x1 * x1 + x2 * x2; 72 | } 73 | while (w >= 1.0 || w == 0.0); 74 | 75 | w = sqrt((-2.0 * log(w)) / w); 76 | 77 | return x1 * w * sigma; 78 | } 79 | 80 | unsigned long long timeInMicroseconds(void); 81 | 82 | double timeInSeconds(void); 83 | 84 | void colorDepthImage(cv::Mat& imgDepth, 85 | cv::Mat& imgColoredDepth, 86 | float minRange, float maxRange); 87 | 88 | bool colormap(const std::string& name, unsigned char idx, 89 | float& r, float& g, float& b); 90 | 91 | std::vector bresLine(int x0, int y0, int x1, int y1); 92 | std::vector bresCircle(int x0, int y0, int r); 93 | 94 | void fitCircle(const std::vector& points, 95 | double& centerX, double& centerY, double& radius); 96 | 97 | std::vector intersectCircles(double x1, double y1, double r1, 98 | double x2, double y2, double r2); 99 | 100 | void LLtoUTM(double latitude, double longitude, 101 | double& utmNorthing, double& utmEasting, 102 | std::string& utmZone); 103 | void UTMtoLL(double utmNorthing, double utmEasting, 104 | const std::string& utmZone, 105 | double& latitude, double& longitude); 106 | 107 | long int timestampDiff(uint64_t t1, uint64_t t2); 108 | 109 | } 110 | 111 | #endif 112 | -------------------------------------------------------------------------------- /camera_model/include/camodocal/sparse_graph/Transform.h: -------------------------------------------------------------------------------- 1 | #ifndef TRANSFORM_H 2 | #define TRANSFORM_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace camodocal 9 | { 10 | 11 | class Transform 12 | { 13 | public: 14 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 15 | 16 | Transform(); 17 | Transform(const Eigen::Matrix4d& H); 18 | 19 | Eigen::Quaterniond& rotation(void); 20 | const Eigen::Quaterniond& rotation(void) const; 21 | double* rotationData(void); 22 | const double* const rotationData(void) const; 23 | 24 | Eigen::Vector3d& translation(void); 25 | const Eigen::Vector3d& translation(void) const; 26 | double* translationData(void); 27 | const double* const translationData(void) const; 28 | 29 | Eigen::Matrix4d toMatrix(void) const; 30 | 31 | private: 32 | Eigen::Quaterniond m_q; 33 | Eigen::Vector3d m_t; 34 | }; 35 | 36 | } 37 | 38 | #endif 39 | -------------------------------------------------------------------------------- /camera_model/instruction: -------------------------------------------------------------------------------- 1 | rosrun camera_model Calibration -w 8 -h 11 -s 70 -i ~/bag/PX/calib/ 2 | -------------------------------------------------------------------------------- /camera_model/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | camera_model 4 | 0.0.0 5 | The camera_model package 6 | 7 | 8 | 9 | 10 | dvorak 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | roscpp 44 | std_msgs 45 | roscpp 46 | std_msgs 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | -------------------------------------------------------------------------------- /camera_model/readme.md: -------------------------------------------------------------------------------- 1 | part of [camodocal](https://github.com/hengli/camodocal) 2 | 3 | [Google Ceres](http://ceres-solver.org) is needed. 4 | 5 | # Calibration: 6 | 7 | Use [intrinsic_calib.cc](https://github.com/dvorak0/camera_model/blob/master/src/intrinsic_calib.cc) to calibrate your camera. 8 | 9 | # Undistortion: 10 | 11 | See [Camera.h](https://github.com/dvorak0/camera_model/blob/master/include/camodocal/camera_models/Camera.h) for general interface: 12 | 13 | - liftProjective: Lift points from the image plane to the projective space. 14 | - spaceToPlane: Projects 3D points to the image plane (Pi function) 15 | 16 | -------------------------------------------------------------------------------- /camera_model/src/gpl/EigenQuaternionParameterization.cc: -------------------------------------------------------------------------------- 1 | #include "camodocal/gpl/EigenQuaternionParameterization.h" 2 | 3 | #include 4 | 5 | namespace camodocal 6 | { 7 | 8 | bool 9 | EigenQuaternionParameterization::Plus(const double* x, 10 | const double* delta, 11 | double* x_plus_delta) const 12 | { 13 | const double norm_delta = 14 | sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]); 15 | if (norm_delta > 0.0) 16 | { 17 | const double sin_delta_by_delta = (sin(norm_delta) / norm_delta); 18 | double q_delta[4]; 19 | q_delta[0] = sin_delta_by_delta * delta[0]; 20 | q_delta[1] = sin_delta_by_delta * delta[1]; 21 | q_delta[2] = sin_delta_by_delta * delta[2]; 22 | q_delta[3] = cos(norm_delta); 23 | EigenQuaternionProduct(q_delta, x, x_plus_delta); 24 | } 25 | else 26 | { 27 | for (int i = 0; i < 4; ++i) 28 | { 29 | x_plus_delta[i] = x[i]; 30 | } 31 | } 32 | return true; 33 | } 34 | 35 | bool 36 | EigenQuaternionParameterization::ComputeJacobian(const double* x, 37 | double* jacobian) const 38 | { 39 | jacobian[0] = x[3]; jacobian[1] = x[2]; jacobian[2] = -x[1]; // NOLINT 40 | jacobian[3] = -x[2]; jacobian[4] = x[3]; jacobian[5] = x[0]; // NOLINT 41 | jacobian[6] = x[1]; jacobian[7] = -x[0]; jacobian[8] = x[3]; // NOLINT 42 | jacobian[9] = -x[0]; jacobian[10] = -x[1]; jacobian[11] = -x[2]; // NOLINT 43 | return true; 44 | } 45 | 46 | } 47 | -------------------------------------------------------------------------------- /camera_model/src/sparse_graph/Transform.cc: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace camodocal 4 | { 5 | 6 | Transform::Transform() 7 | { 8 | m_q.setIdentity(); 9 | m_t.setZero(); 10 | } 11 | 12 | Transform::Transform(const Eigen::Matrix4d& H) 13 | { 14 | m_q = Eigen::Quaterniond(H.block<3,3>(0,0)); 15 | m_t = H.block<3,1>(0,3); 16 | } 17 | 18 | Eigen::Quaterniond& 19 | Transform::rotation(void) 20 | { 21 | return m_q; 22 | } 23 | 24 | const Eigen::Quaterniond& 25 | Transform::rotation(void) const 26 | { 27 | return m_q; 28 | } 29 | 30 | double* 31 | Transform::rotationData(void) 32 | { 33 | return m_q.coeffs().data(); 34 | } 35 | 36 | const double* const 37 | Transform::rotationData(void) const 38 | { 39 | return m_q.coeffs().data(); 40 | } 41 | 42 | Eigen::Vector3d& 43 | Transform::translation(void) 44 | { 45 | return m_t; 46 | } 47 | 48 | const Eigen::Vector3d& 49 | Transform::translation(void) const 50 | { 51 | return m_t; 52 | } 53 | 54 | double* 55 | Transform::translationData(void) 56 | { 57 | return m_t.data(); 58 | } 59 | 60 | const double* const 61 | Transform::translationData(void) const 62 | { 63 | return m_t.data(); 64 | } 65 | 66 | Eigen::Matrix4d 67 | Transform::toMatrix(void) const 68 | { 69 | Eigen::Matrix4d H; 70 | H.setIdentity(); 71 | H.block<3,3>(0,0) = m_q.toRotationMatrix(); 72 | H.block<3,1>(0,3) = m_t; 73 | 74 | return H; 75 | } 76 | 77 | } 78 | -------------------------------------------------------------------------------- /feature_tracker_/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(feature_tracker_) 3 | 4 | set(CMAKE_BUILD_TYPE "Release") 5 | set(CMAKE_CXX_FLAGS "-std=c++11") 6 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g") 7 | 8 | find_package(catkin REQUIRED COMPONENTS 9 | roscpp 10 | std_msgs 11 | sensor_msgs 12 | cv_bridge 13 | camera_model 14 | tf 15 | ) 16 | find_package(PCL REQUIRED) 17 | find_package(OpenCV REQUIRED) 18 | 19 | catkin_package() 20 | 21 | # include_directories( 22 | # ${catkin_INCLUDE_DIRS} 23 | # ${PCL_INCLUDE_DIRS} 24 | # ) 25 | set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) 26 | find_package(Eigen3) 27 | include_directories( 28 | ${catkin_INCLUDE_DIRS} 29 | ${EIGEN3_INCLUDE_DIR} 30 | ${PCL_INCLUDE_DIRS} 31 | ) 32 | 33 | add_executable(feature_tracker_ 34 | src/feature_tracker_node.cpp 35 | src/parameters.cpp 36 | src/feature_tracker.cpp 37 | ) 38 | 39 | target_link_libraries(feature_tracker_ ${catkin_LIBRARIES} ${OpenCV_LIBS} ${PCL_LIBRARIES}) 40 | -------------------------------------------------------------------------------- /feature_tracker_/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | feature_tracker_ 4 | 0.0.0 5 | The feature_tracker_package 6 | 7 | 8 | 9 | 10 | dvorak 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | roscpp 44 | camera_model 45 | message_generation 46 | roscpp 47 | camera_model 48 | message_runtime 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /feature_tracker_/src/parameters.cpp: -------------------------------------------------------------------------------- 1 | #include "parameters.h" 2 | 3 | std::string IMAGE_TOPIC; 4 | std::string IMU_TOPIC; 5 | std::string LIDAR_TOPIC; 6 | std::vector CAM_NAMES; 7 | std::string FISHEYE_MASK; 8 | int MAX_CNT; 9 | int MIN_DIST; 10 | int WINDOW_SIZE; 11 | int FREQ; 12 | double F_THRESHOLD; 13 | int SHOW_TRACK; 14 | int STEREO_TRACK; 15 | int EQUALIZE; 16 | int ROW; 17 | int COL; 18 | int FOCAL_LENGTH; 19 | int FISHEYE; 20 | bool PUB_THIS_FRAME; 21 | int SHOW_LIDAR2CAM; 22 | int LIDAR2CAM; 23 | 24 | Eigen::Matrix3d RLC; 25 | Eigen::Vector3d TLC; 26 | Eigen::Matrix4f TransFormLC; 27 | 28 | template 29 | T readParam(ros::NodeHandle &n, std::string name) 30 | { 31 | T ans; 32 | if (n.getParam(name, ans)) 33 | { 34 | ROS_INFO_STREAM("Loaded " << name << ": " << ans); 35 | } 36 | else 37 | { 38 | ROS_ERROR_STREAM("Failed to load " << name); 39 | n.shutdown(); 40 | } 41 | return ans; 42 | } 43 | 44 | void readParameters(ros::NodeHandle &n) 45 | { 46 | std::string config_file; 47 | config_file = readParam(n, "config_file"); 48 | cv::FileStorage fsSettings(config_file, cv::FileStorage::READ); 49 | if(!fsSettings.isOpened()) 50 | { 51 | std::cerr << "ERROR: Wrong path to settings" << std::endl; 52 | } 53 | std::string VINS_FOLDER_PATH = readParam(n, "vils_folder"); 54 | 55 | { 56 | cv::Mat cv_RLC, cv_TLC; 57 | fsSettings["gt_rlc"] >> cv_RLC; 58 | fsSettings["gt_tlc"] >> cv_TLC; 59 | Eigen::Matrix3d eigen_RLC; 60 | Eigen::Vector3d eigen_TLC; 61 | cv::cv2eigen(cv_RLC, eigen_RLC); 62 | cv::cv2eigen(cv_TLC, eigen_TLC); 63 | Eigen::Quaterniond QLC(eigen_RLC); 64 | eigen_RLC = QLC.normalized(); 65 | RLC = eigen_RLC; 66 | TLC = eigen_TLC; 67 | TransFormLC.setIdentity(); 68 | TransFormLC.block<3, 3>(0, 0) = RLC.cast(); 69 | TransFormLC.block<3, 1>(0, 3) = TLC.cast(); 70 | } 71 | 72 | fsSettings["image_topic"] >> IMAGE_TOPIC; 73 | fsSettings["imu_topic"] >> IMU_TOPIC; 74 | fsSettings["lidar_topic"] >> LIDAR_TOPIC; 75 | MAX_CNT = fsSettings["max_cnt"]; 76 | MIN_DIST = fsSettings["min_dist"]; 77 | ROW = fsSettings["image_height"]; 78 | COL = fsSettings["image_width"]; 79 | FREQ = fsSettings["freq"]; 80 | F_THRESHOLD = fsSettings["F_threshold"]; 81 | SHOW_TRACK = fsSettings["show_track"]; 82 | EQUALIZE = fsSettings["equalize"]; 83 | FISHEYE = fsSettings["fisheye"]; 84 | SHOW_LIDAR2CAM = fsSettings["show_lidar2cam"]; 85 | LIDAR2CAM = fsSettings["add_lidar2cam"]; 86 | 87 | if (FISHEYE == 1) 88 | FISHEYE_MASK = VINS_FOLDER_PATH + "config/fisheye_mask.jpg"; 89 | CAM_NAMES.push_back(config_file); 90 | 91 | WINDOW_SIZE = 20; 92 | STEREO_TRACK = false; 93 | FOCAL_LENGTH = 460; 94 | PUB_THIS_FRAME = false; 95 | 96 | if (FREQ == 0) 97 | FREQ = 100; 98 | 99 | fsSettings.release(); 100 | 101 | 102 | } 103 | 104 | float pointDistance(PointType p) 105 | { 106 | return sqrt(p.x*p.x + p.y*p.y + p.z*p.z); 107 | } 108 | 109 | 110 | float pointDistance(PointType p1, PointType p2) 111 | { 112 | return sqrt((p1.x-p2.x)*(p1.x-p2.x) + (p1.y-p2.y)*(p1.y-p2.y) + (p1.z-p2.z)*(p1.z-p2.z)); 113 | } 114 | 115 | -------------------------------------------------------------------------------- /feature_tracker_/src/parameters.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | #include 17 | 18 | #include 19 | #include 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include 53 | #include 54 | #include 55 | #include 56 | #include 57 | 58 | using namespace std; 59 | 60 | extern int ROW; 61 | extern int COL; 62 | extern int FOCAL_LENGTH; 63 | const int NUM_OF_CAM = 1; 64 | 65 | 66 | extern std::string IMAGE_TOPIC; 67 | extern std::string IMU_TOPIC; 68 | extern std::string LIDAR_TOPIC; 69 | extern std::string FISHEYE_MASK; 70 | extern std::vector CAM_NAMES; 71 | extern int MAX_CNT; 72 | extern int MIN_DIST; 73 | extern int WINDOW_SIZE; 74 | extern int FREQ; 75 | extern double F_THRESHOLD; 76 | extern int SHOW_TRACK; 77 | extern int STEREO_TRACK; 78 | extern int EQUALIZE; 79 | extern int FISHEYE; 80 | extern bool PUB_THIS_FRAME; 81 | extern int SHOW_LIDAR2CAM; 82 | extern int LIDAR2CAM; 83 | 84 | 85 | extern Eigen::Matrix3d RLC; 86 | extern Eigen::Vector3d TLC; 87 | extern Eigen::Matrix4f TransFormLC; 88 | 89 | void readParameters(ros::NodeHandle &n); 90 | 91 | typedef pcl::PointXYZI PointType; 92 | float pointDistance(PointType p); 93 | float pointDistance(PointType p1, PointType p2); 94 | void publishCloud(ros::Publisher *thisPub, pcl::PointCloud::Ptr thisCloud, ros::Time thisStamp, std::string thisFrame); -------------------------------------------------------------------------------- /feature_tracker_/src/tic_toc.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | class TicToc 8 | { 9 | public: 10 | TicToc() 11 | { 12 | tic(); 13 | } 14 | 15 | void tic() 16 | { 17 | start = std::chrono::system_clock::now(); 18 | } 19 | 20 | double toc() 21 | { 22 | end = std::chrono::system_clock::now(); 23 | std::chrono::duration elapsed_seconds = end - start; 24 | return elapsed_seconds.count() * 1000; 25 | } 26 | 27 | private: 28 | std::chrono::time_point start, end; 29 | }; 30 | -------------------------------------------------------------------------------- /fig/cover.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Stan994265/mVIL-Fusion/3dcb644e34f5c5908b46c78fb87505e9a800e803/fig/cover.png -------------------------------------------------------------------------------- /fig/device.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Stan994265/mVIL-Fusion/3dcb644e34f5c5908b46c78fb87505e9a800e803/fig/device.png -------------------------------------------------------------------------------- /fig/results.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Stan994265/mVIL-Fusion/3dcb644e34f5c5908b46c78fb87505e9a800e803/fig/results.png -------------------------------------------------------------------------------- /lidar_compensator/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(lidar_compensator) 3 | 4 | set(CMAKE_BUILD_TYPE "Release") 5 | set(CMAKE_CXX_FLAGS "-std=c++11") 6 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g") 7 | 8 | find_package(catkin REQUIRED COMPONENTS 9 | dynamic_reconfigure 10 | geometry_msgs 11 | message_generation 12 | message_runtime 13 | nav_msgs 14 | pcl_conversions 15 | pcl_ros 16 | rosbag 17 | roscpp 18 | rospy 19 | sensor_msgs 20 | std_msgs 21 | tf 22 | tf_conversions 23 | ) 24 | 25 | 26 | find_package(OpenCV REQUIRED) 27 | include_directories(${OpenCV_INCLUDE_DIR}) 28 | find_package(Eigen3 REQUIRED) 29 | include_directories(${EIGEN3_INCLUDE_DIR}) 30 | find_package(PCL REQUIRED) 31 | include_directories(${PCL_INCLUDE_DIRS}) 32 | 33 | 34 | catkin_package() 35 | 36 | include_directories( 37 | ${catkin_INCLUDE_DIRS} 38 | ) 39 | 40 | add_executable(lidar_compensator 41 | src/PointProcessor.cc 42 | src/processor_node.cc 43 | ) 44 | 45 | target_link_libraries(lidar_compensator ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES}) 46 | -------------------------------------------------------------------------------- /lidar_compensator/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | lidar_compensator 4 | 0.0.0 5 | The lidar_feature package 6 | wy 7 | 8 | TODO 9 | 10 | catkin 11 | dynamic_reconfigure 12 | geometry_msgs 13 | message_generation 14 | nav_msgs 15 | pcl_conversions 16 | pcl_ros 17 | rosbag 18 | roscpp 19 | rospy 20 | sensor_msgs 21 | std_msgs 22 | tf 23 | tf_conversions 24 | 25 | -------------------------------------------------------------------------------- /lidar_compensator/src/CircularBuffer.h: -------------------------------------------------------------------------------- 1 | #ifndef CIRCULARBUFFER_H_ 2 | #define CIRCULARBUFFER_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | template 9 | class CircularBuffer 10 | { 11 | public: 12 | CircularBuffer(const size_t &capacity = 200) 13 | : capacity_(capacity), 14 | size_(0), 15 | start_idx_(0) 16 | { 17 | buffer_ = new T[capacity]; 18 | }; 19 | 20 | ~CircularBuffer() 21 | { 22 | delete[] buffer_; 23 | buffer_ = NULL; 24 | } 25 | 26 | void Reset(size_t capacity = 1) 27 | { 28 | delete[] buffer_; 29 | buffer_ = NULL; 30 | 31 | capacity_ = capacity; 32 | size_ = 0; 33 | start_idx_ = 0; 34 | buffer_ = new T[capacity]; 35 | } 36 | 37 | /** \brief Retrieve the buffer size. 38 | * 39 | * @return the buffer size 40 | */ 41 | const size_t &size() 42 | { 43 | return size_; 44 | } 45 | 46 | /** \brief Retrieve the buffer capacity. 47 | * 48 | * @return the buffer capacity 49 | */ 50 | const size_t &capacity() 51 | { 52 | return capacity_; 53 | } 54 | 55 | /** \brief Ensure that this buffer has at least the required capacity. 56 | * 57 | * @param req_apacity the minimum required capacity 58 | */ 59 | void EnsureCapacity(const int &req_apacity) 60 | { 61 | if (req_apacity > 0 && capacity_ < req_apacity) 62 | { 63 | // create new buffer and copy (valid) entries 64 | T *new_buffer = new T[req_apacity]; 65 | for (size_t i = 0; i < size_; i++) 66 | { 67 | new_buffer[i] = (*this)[i]; 68 | } 69 | 70 | // switch buffer pointers and delete old buffer 71 | T *old_buffer = buffer_; 72 | buffer_ = new_buffer; 73 | start_idx_ = 0; 74 | 75 | delete[] old_buffer; 76 | } 77 | } 78 | 79 | /** \brief Check if the buffer is empty. 80 | * 81 | * @return true if the buffer is empty, false otherwise 82 | */ 83 | bool empty() 84 | { 85 | return size_ == 0; 86 | } 87 | 88 | /** \brief Retrieve the i-th element of the buffer. 89 | * 90 | * 91 | * @param i the buffer index 92 | * @return the element at the i-th position as no-constant 93 | */ 94 | T &operator[](const size_t &i) 95 | { 96 | return buffer_[(start_idx_ + i) % capacity_]; 97 | } 98 | 99 | /** \brief Retrieve the i-th element of the buffer. 100 | * 101 | * 102 | * @param i the buffer index 103 | * @return the element at the i-th position 104 | */ 105 | const T &operator[](const size_t &i) const 106 | { 107 | return buffer_[(start_idx_ + i) % capacity_]; 108 | } 109 | 110 | /** \brief Retrieve the first (oldest) element of the buffer. 111 | * 112 | * @return the first element 113 | */ 114 | const T &first() const 115 | { 116 | return buffer_[start_idx_]; 117 | } 118 | 119 | T &first() 120 | { 121 | return buffer_[start_idx_]; 122 | } 123 | 124 | /** \brief Retrieve the last (latest) element of the buffer. 125 | * 126 | * @return the last element 127 | */ 128 | const T &last() const 129 | { 130 | size_t idx = size_ == 0 ? 0 : (start_idx_ + size_ - 1) % capacity_; 131 | return buffer_[idx]; 132 | } 133 | 134 | T &last() 135 | { 136 | size_t idx = size_ == 0 ? 0 : (start_idx_ + size_ - 1) % capacity_; 137 | return buffer_[idx]; 138 | } 139 | 140 | /** \brief Push a new element to the buffer. 141 | * 142 | * If the buffer reached its capacity, the oldest element is overwritten. 143 | * 144 | * @param element the element to push 145 | */ 146 | void push(const T &element) 147 | { 148 | if (size_ < capacity_) 149 | { 150 | buffer_[size_] = element; 151 | ++size_; 152 | } 153 | else 154 | { 155 | buffer_[start_idx_] = element; 156 | start_idx_ = (start_idx_ + 1) % capacity_; 157 | } 158 | } 159 | 160 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 161 | 162 | private: 163 | size_t capacity_; ///< buffer capacity 164 | size_t size_; ///< current buffer size 165 | size_t start_idx_; ///< current start index 166 | T *buffer_; ///< internal element buffer 167 | }; 168 | 169 | #endif // CIRCULARBUFFER_H_ 170 | -------------------------------------------------------------------------------- /lidar_compensator/src/PointProcessor.h: -------------------------------------------------------------------------------- 1 | #ifndef POINTPROCESSOR_H_ 2 | #define POINTPROCESSOR_H_ 3 | 4 | #include 5 | 6 | #include 7 | 8 | #include "common_ros.h" 9 | // #include "TicToc.h" 10 | #include "CircularBuffer.h" 11 | #include "math_utils.h" 12 | #include 13 | 14 | using namespace std; 15 | typedef pcl::PointXYZI PointT; 16 | typedef typename pcl::PointCloud PointCloud; 17 | typedef typename pcl::PointCloud::Ptr PointCloudPtr; 18 | typedef typename pcl::PointCloud::ConstPtr PointCloudConstPtr; 19 | typedef std::pair IndexRange; 20 | 21 | // adapted from LOAM 22 | /** Point label options. */ 23 | enum PointLabel 24 | { 25 | CORNER_SHARP = 2, ///< sharp corner point 26 | CORNER_LESS_SHARP = 1, ///< less sharp corner point 27 | SURFACE_LESS_FLAT = 0, ///< less flat surface point 28 | SURFACE_FLAT = -1 ///< flat surface point 29 | }; 30 | 31 | struct PointProcessorConfig 32 | { 33 | bool deskew = false; 34 | double scan_period = 0.1; 35 | int num_scan_subregions = 8; 36 | int num_curvature_regions = 5; 37 | float surf_curv_th = 1.0; 38 | int max_corner_sharp = 3; 39 | int max_corner_less_sharp = 10 * max_corner_sharp; 40 | int max_surf_flat = 4; 41 | float less_flat_filter_size = 0.2; 42 | 43 | string capture_frame_id = "/laser_link"; // /laser_link /velodyne /base_link 44 | 45 | double rad_diff = 0.2; 46 | 47 | bool infer_start_ori_ = false; 48 | }; 49 | 50 | class PointProcessor 51 | { 52 | 53 | public: 54 | PointProcessor(); 55 | PointProcessor(float lower_bound, float upper_bound, int num_rings, bool uneven = false); 56 | 57 | // WARNING: is it useful to separate Process and SetInputCloud? 58 | // void Process(const PointCloudConstPtr &cloud_in, PointCloud &cloud_out); 59 | void Process(); 60 | 61 | void PointCloudHandler(const sensor_msgs::PointCloud2ConstPtr &raw_points_msg); 62 | 63 | void SetupConfig(PointProcessorConfig config) 64 | { 65 | config_ = config; 66 | } 67 | 68 | void SetupRos(ros::NodeHandle &nh); 69 | 70 | void SetInputCloud(const PointCloudConstPtr &cloud_in, ros::Time time_in = ros::Time::now()); 71 | 72 | void PointToRing(); 73 | void PointToRing(const PointCloudConstPtr &cloud_in, 74 | vector &ring_out, 75 | vector &intensity_out); 76 | 77 | inline int ElevationToRing(float rad) 78 | { 79 | double in = (RadToDeg(rad) - lower_bound_) * factor_ + 0.5; 80 | return int((RadToDeg(rad) - lower_bound_) * factor_ + 0.5); 81 | } 82 | 83 | void ExtractFeaturePoints(); 84 | 85 | void PublishResults(); 86 | 87 | // TODO: not necessary data? 88 | vector laser_scans; 89 | vector intensity_scans; 90 | vector scan_ranges; 91 | 92 | protected: 93 | ros::Time sweep_start_; 94 | ros::Time scan_time_; 95 | 96 | float lower_bound_; 97 | float upper_bound_; 98 | int num_rings_; 99 | float factor_; 100 | PointProcessorConfig config_; 101 | // TicToc tic_toc_; 102 | 103 | PointCloudConstPtr cloud_ptr_; 104 | PointCloud cloud_in_rings_; 105 | 106 | PointCloud corner_points_sharp_; 107 | PointCloud corner_points_less_sharp_; 108 | PointCloud surface_points_flat_; 109 | PointCloud surface_points_less_flat_; 110 | 111 | // the following will be assigened or resized 112 | vector scan_ring_mask_; 113 | vector > curvature_idx_pairs_; // in subregion 114 | vector subregion_labels_; ///< point label buffer 115 | 116 | void Reset(const ros::Time &scan_time, const bool &is_new_sweep = true); 117 | void PrepareRing(const PointCloud &scan); 118 | void PrepareSubregion(const PointCloud &scan, const size_t idx_start, const size_t idx_end); 119 | void MaskPickedInRing(const PointCloud &scan, const size_t in_scan_idx); 120 | 121 | ros::Subscriber sub_raw_points_; ///< input cloud message subscriber 122 | 123 | ros::Publisher pub_full_cloud_; ///< full resolution cloud message publisher 124 | ros::Publisher pub_corner_points_sharp_; ///< sharp corner cloud message publisher 125 | ros::Publisher pub_corner_points_less_sharp_; ///< less sharp corner cloud message publisher 126 | ros::Publisher pub_surf_points_flat_; ///< flat surface cloud message publisher 127 | ros::Publisher pub_surf_points_less_flat_; ///< less flat surface cloud message publisher 128 | 129 | bool is_ros_setup_ = false; 130 | 131 | bool uneven_ = false; 132 | 133 | private: 134 | float start_ori_, end_ori_; 135 | CircularBuffer start_ori_buf1_{10}; 136 | CircularBuffer start_ori_buf2_{10}; 137 | }; 138 | 139 | #endif //POINTPROCESSOR_H_ 140 | -------------------------------------------------------------------------------- /lidar_compensator/src/TicToc.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of LIO-mapping. 3 | * 4 | * Copyright (C) 2019 Haoyang Ye , 5 | * Robotics and Multiperception Lab (RAM-LAB ), 6 | * The Hong Kong University of Science and Technology 7 | * 8 | * For more information please see 9 | * or . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * LIO-mapping is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * LIO-mapping is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with LIO-mapping. If not, see . 25 | */ 26 | 27 | // 28 | // Created by hyye on 3/14/18. 29 | // 30 | 31 | #ifndef LIO_TICTOC_H 32 | #define LIO_TICTOC_H 33 | 34 | #include 35 | #include 36 | #include 37 | 38 | class TicToc { 39 | public: 40 | TicToc() { 41 | Tic(); 42 | } 43 | 44 | void Tic() { 45 | start_ = std::chrono::system_clock::now(); 46 | } 47 | 48 | double Toc() { 49 | end_ = std::chrono::system_clock::now(); 50 | elapsed_seconds_ = end_ - start_; 51 | return elapsed_seconds_.count() * 1000; 52 | } 53 | 54 | double GetLastStop() { 55 | return elapsed_seconds_.count() * 1000; 56 | } 57 | 58 | private: 59 | std::chrono::time_point start_, end_; 60 | std::chrono::duration elapsed_seconds_; 61 | }; 62 | 63 | #endif //LIO_TICTOC_H 64 | -------------------------------------------------------------------------------- /lidar_compensator/src/common_ros.h: -------------------------------------------------------------------------------- 1 | #ifndef COMMON_ROS_H_ 2 | #define COMMON_ROS_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | template 10 | inline void PublishCloudMsg(ros::Publisher &publisher, 11 | const pcl::PointCloud &cloud, 12 | const ros::Time &stamp, 13 | std::string frame_id) 14 | { 15 | sensor_msgs::PointCloud2 msg; 16 | pcl::toROSMsg(cloud, msg); 17 | msg.header.stamp = stamp; 18 | msg.header.frame_id = frame_id; 19 | publisher.publish(msg); 20 | } 21 | 22 | #endif //COMMON_ROS_H_ 23 | -------------------------------------------------------------------------------- /lidar_compensator/src/processor_node.cc: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | #include "PointProcessor.h" 10 | #include "TicToc.h" 11 | using namespace std; 12 | 13 | int main(int argc, char **argv) 14 | { 15 | 16 | ros::init(argc, argv, "point_processor"); 17 | 18 | ros::NodeHandle nh("~"); 19 | 20 | int sensor_type; 21 | double rad_diff; 22 | bool infer_start_ori; 23 | nh.param("sensor_type", sensor_type, 16); 24 | nh.param("rad_diff", rad_diff, 0.2); 25 | nh.param("infer_start_ori", infer_start_ori, false); 26 | double scan_period; 27 | nh.param("scan_period", scan_period, 0.1); 28 | 29 | PointProcessor processor; // Default sensor_type is 16 30 | 31 | if (sensor_type == 32) 32 | { 33 | processor = PointProcessor(-30.67f, 10.67f, 32); 34 | } 35 | else if (sensor_type == 64) 36 | { 37 | processor = PointProcessor(-24.9f, 2, 64); 38 | } 39 | else if (sensor_type == 320) 40 | { 41 | processor = PointProcessor(-25, 15, 32, true); 42 | } 43 | 44 | PointProcessorConfig config; 45 | config.rad_diff = rad_diff; 46 | config.infer_start_ori_ = infer_start_ori; 47 | config.scan_period = scan_period; 48 | processor.SetupConfig(config); 49 | processor.SetupRos(nh); 50 | ros::Rate r(100); 51 | // while (ros::ok()) 52 | // { 53 | // ros::spinOnce(); 54 | // r.sleep(); 55 | // } 56 | ros::AsyncSpinner spinner(1); 57 | spinner.start(); 58 | ros::waitForShutdown(); 59 | 60 | return 0; 61 | } -------------------------------------------------------------------------------- /lidar_mapping/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(vils_mapping) 3 | 4 | set(CMAKE_BUILD_TYPE "Release") 5 | # set(CMAKE_CXX_FLAGS "-std=c++11") 6 | set(CMAKE_CXX_STANDARD 17) 7 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g -pthread") 8 | 9 | find_package(catkin REQUIRED COMPONENTS 10 | geometry_msgs 11 | nav_msgs 12 | sensor_msgs 13 | roscpp 14 | rospy 15 | rosbag 16 | std_msgs 17 | image_transport 18 | cv_bridge 19 | tf 20 | ) 21 | 22 | #find_package(Eigen3 REQUIRED) 23 | find_package(PCL REQUIRED) 24 | find_package(OpenCV REQUIRED) 25 | find_package(Ceres REQUIRED) 26 | 27 | find_package(OpenMP REQUIRED) 28 | find_package(GTSAM REQUIRED QUIET) 29 | find_package(Boost REQUIRED COMPONENTS timer) 30 | include_directories( 31 | include 32 | ${catkin_INCLUDE_DIRS} 33 | ${PCL_INCLUDE_DIRS} 34 | ${CERES_INCLUDE_DIRS} 35 | ${OpenCV_INCLUDE_DIRS} 36 | ${GTSAM_INCLUDE_DIR} 37 | ) 38 | 39 | catkin_package( 40 | CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs 41 | DEPENDS EIGEN3 PCL 42 | INCLUDE_DIRS include 43 | ) 44 | 45 | 46 | add_executable(feature src/scanRegistration.cpp) 47 | target_link_libraries(feature ${catkin_LIBRARIES} ${PCL_LIBRARIES}) 48 | 49 | add_executable(local src/localMapping.cpp) 50 | target_link_libraries(local ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES}) 51 | 52 | include_directories(${PROJECT_SOURCE_DIR}/fast_gicp) 53 | 54 | add_executable(global_ikdtree 55 | src/globalMappingIkdTree.cpp 56 | include/ikd_Tree/ikd_Tree.cpp 57 | include/scancontext/Scancontext.cpp 58 | ) 59 | target_compile_options(global_ikdtree 60 | PRIVATE ${OpenMP_CXX_FLAGS} 61 | ) 62 | 63 | target_link_libraries(global_ikdtree 64 | ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenMP_CXX_FLAGS} 65 | # gtsam ${PROJECT_SOURCE_DIR}/include/fast_gicp/libfast_gicp.a 66 | gtsam ${PROJECT_SOURCE_DIR}/../vils_estimator/src/lidar_functions/fast_gicp/build/libfast_gicp.a 67 | ) 68 | 69 | add_executable(global_octree 70 | src/globalMappingOcTree.cpp 71 | include/scancontext/Scancontext.cpp 72 | ) 73 | target_compile_options(global_octree 74 | PRIVATE ${OpenMP_CXX_FLAGS} 75 | ) 76 | target_link_libraries(global_octree 77 | ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenMP_CXX_FLAGS} 78 | #gtsam ${PROJECT_SOURCE_DIR}/include/fast_gicp/libfast_gicp.a 79 | gtsam ${PROJECT_SOURCE_DIR}/../vils_estimator/src/lidar_functions/fast_gicp/build/libfast_gicp.a 80 | ) 81 | 82 | 83 | 84 | 85 | -------------------------------------------------------------------------------- /lidar_mapping/include/aloam_velodyne/common.h: -------------------------------------------------------------------------------- 1 | // This is an advanced implementation of the algorithm described in the following paper: 2 | // J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. 3 | // Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. 4 | 5 | // Modifier: Tong Qin qintonguav@gmail.com 6 | // Shaozu Cao saozu.cao@connect.ust.hk 7 | 8 | // Copyright 2013, Ji Zhang, Carnegie Mellon University 9 | // Further contributions copyright (c) 2016, Southwest Research Institute 10 | // All rights reserved. 11 | // 12 | // Redistribution and use in source and binary forms, with or without 13 | // modification, are permitted provided that the following conditions are met: 14 | // 15 | // 1. Redistributions of source code must retain the above copyright notice, 16 | // this list of conditions and the following disclaimer. 17 | // 2. Redistributions in binary form must reproduce the above copyright notice, 18 | // this list of conditions and the following disclaimer in the documentation 19 | // and/or other materials provided with the distribution. 20 | // 3. Neither the name of the copyright holder nor the names of its 21 | // contributors may be used to endorse or promote products derived from this 22 | // software without specific prior written permission. 23 | // 24 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 26 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 27 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 28 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 29 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 30 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 31 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 32 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 33 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | // POSSIBILITY OF SUCH DAMAGE. 35 | 36 | #pragma once 37 | 38 | #include 39 | 40 | #include 41 | 42 | typedef pcl::PointXYZI PointType; 43 | 44 | inline double rad2deg(double radians) 45 | { 46 | return radians * 180.0 / M_PI; 47 | } 48 | 49 | inline double deg2rad(double degrees) 50 | { 51 | return degrees * M_PI / 180.0; 52 | } 53 | 54 | struct Pose6D 55 | { 56 | double x; 57 | double y; 58 | double z; 59 | double roll; 60 | double pitch; 61 | double yaw; 62 | }; -------------------------------------------------------------------------------- /lidar_mapping/include/aloam_velodyne/tic_toc.h: -------------------------------------------------------------------------------- 1 | // Author: Tong Qin qintonguav@gmail.com 2 | // Shaozu Cao saozu.cao@connect.ust.hk 3 | 4 | #pragma once 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | class TicToc 11 | { 12 | public: 13 | TicToc() 14 | { 15 | tic(); 16 | } 17 | 18 | void tic() 19 | { 20 | start = std::chrono::system_clock::now(); 21 | } 22 | 23 | double toc() 24 | { 25 | end = std::chrono::system_clock::now(); 26 | std::chrono::duration elapsed_seconds = end - start; 27 | return elapsed_seconds.count() * 1000; 28 | } 29 | 30 | private: 31 | std::chrono::time_point start, end; 32 | }; 33 | 34 | class TicTocV2 35 | { 36 | public: 37 | TicTocV2() 38 | { 39 | tic(); 40 | } 41 | 42 | TicTocV2( bool _disp ) 43 | { 44 | disp_ = _disp; 45 | tic(); 46 | } 47 | 48 | void tic() 49 | { 50 | start = std::chrono::system_clock::now(); 51 | } 52 | 53 | void toc( std::string _about_task ) 54 | { 55 | end = std::chrono::system_clock::now(); 56 | std::chrono::duration elapsed_seconds = end - start; 57 | double elapsed_ms = elapsed_seconds.count() * 1000; 58 | 59 | if( disp_ ) 60 | { 61 | std::cout.precision(3); // 10 for sec, 3 for ms 62 | std::cout << _about_task << ": " << elapsed_ms << " msec." << std::endl; 63 | } 64 | } 65 | 66 | private: 67 | std::chrono::time_point start, end; 68 | bool disp_ = false; 69 | }; -------------------------------------------------------------------------------- /lidar_mapping/include/fast_gicp/cuda/brute_force_knn.cuh: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_CUDA_BRUTE_FORCE_KNN_CUH 2 | #define FAST_GICP_CUDA_BRUTE_FORCE_KNN_CUH 3 | 4 | #include 5 | 6 | #include 7 | 8 | namespace fast_gicp { 9 | namespace cuda { 10 | 11 | void brute_force_knn_search(const thrust::device_vector& source, const thrust::device_vector& target, int k, thrust::device_vector>& k_neighbors, bool do_sort=false); 12 | 13 | } 14 | } // namespace fast_gicp 15 | 16 | 17 | #endif -------------------------------------------------------------------------------- /lidar_mapping/include/fast_gicp/cuda/compute_derivatives.cuh: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_CUDA_COMPUTE_DERIVATIVES_CUH 2 | #define FAST_GICP_CUDA_COMPUTE_DERIVATIVES_CUH 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | namespace fast_gicp { 10 | namespace cuda { 11 | 12 | double compute_derivatives( 13 | const thrust::device_vector& src_points, 14 | const thrust::device_vector& src_covs, 15 | const GaussianVoxelMap& voxelmap, 16 | const thrust::device_vector>& voxel_correspondences, 17 | const thrust::device_ptr& linearized_x_ptr, 18 | const thrust::device_ptr& x_ptr, 19 | Eigen::Matrix* H, 20 | Eigen::Matrix* b); 21 | } 22 | } // namespace fast_gicp 23 | 24 | #endif -------------------------------------------------------------------------------- /lidar_mapping/include/fast_gicp/cuda/compute_mahalanobis.cuh: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_CUDA_COMPUTE_MAHALANOBIS_CUH 2 | #define FAST_GICP_CUDA_COMPUTE_MAHALANOBIS_CUH 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | namespace fast_gicp { 11 | namespace cuda { 12 | 13 | void compute_mahalanobis( 14 | const thrust::device_vector& src_points, 15 | const thrust::device_vector& src_covs, 16 | const GaussianVoxelMap& voxelmap, 17 | const thrust::device_vector& voxel_correspondences, 18 | const Eigen::Isometry3f& linearized_x, 19 | thrust::device_vector& mahalanobis 20 | ); 21 | 22 | } 23 | } // namespace fast_gicp 24 | 25 | #endif -------------------------------------------------------------------------------- /lidar_mapping/include/fast_gicp/cuda/covariance_estimation.cuh: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_CUDA_COVARIANCE_ESTIMATION_CUH 2 | #define FAST_GICP_CUDA_COVARIANCE_ESTIMATION_CUH 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace fast_gicp { 9 | namespace cuda { 10 | 11 | void covariance_estimation(const thrust::device_vector& points, int k, const thrust::device_vector& k_neighbors, thrust::device_vector& covariances); 12 | 13 | void covariance_estimation_rbf(const thrust::device_vector& points, double kernel_width, double max_dist, thrust::device_vector& covariances); 14 | } 15 | } // namespace fast_gicp 16 | 17 | #endif -------------------------------------------------------------------------------- /lidar_mapping/include/fast_gicp/cuda/covariance_regularization.cuh: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_CUDA_COVARIANCE_REGULARIZATION_CUH 2 | #define FAST_GICP_CUDA_COVARIANCE_REGULARIZATION_CUH 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace fast_gicp { 9 | namespace cuda { 10 | 11 | void covariance_regularization(thrust::device_vector& means, thrust::device_vector& covs, RegularizationMethod method); 12 | 13 | } // namespace cuda 14 | } // namespace fast_gicp 15 | 16 | #endif -------------------------------------------------------------------------------- /lidar_mapping/include/fast_gicp/cuda/fast_vgicp_cuda.cuh: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_FAST_VGICP_CUDA_CORE_CUH 2 | #define FAST_GICP_FAST_VGICP_CUDA_CORE_CUH 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | namespace thrust { 12 | 13 | template 14 | class pair; 15 | 16 | template 17 | class device_allocator; 18 | 19 | template 20 | class device_vector; 21 | } // namespace thrust 22 | 23 | namespace fast_gicp { 24 | namespace cuda { 25 | 26 | class GaussianVoxelMap; 27 | 28 | class FastVGICPCudaCore { 29 | public: 30 | using Points = thrust::device_vector>; 31 | using Indices = thrust::device_vector>; 32 | using Matrices = thrust::device_vector>; 33 | using Correspondences = thrust::device_vector, thrust::device_allocator>>; 34 | using VoxelCoordinates = thrust::device_vector>; 35 | 36 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 37 | FastVGICPCudaCore(); 38 | ~FastVGICPCudaCore(); 39 | 40 | void set_resolution(double resolution); 41 | void set_kernel_params(double kernel_width, double kernel_max_dist); 42 | void set_neighbor_search_method(fast_gicp::NeighborSearchMethod method, double radius); 43 | 44 | void swap_source_and_target(); 45 | void set_source_cloud(const std::vector>& cloud); 46 | void set_target_cloud(const std::vector>& cloud); 47 | 48 | void set_source_neighbors(int k, const std::vector& neighbors); 49 | void set_target_neighbors(int k, const std::vector& neighbors); 50 | void find_source_neighbors(int k); 51 | void find_target_neighbors(int k); 52 | 53 | void calculate_source_covariances(RegularizationMethod method); 54 | void calculate_target_covariances(RegularizationMethod method); 55 | 56 | void calculate_source_covariances_rbf(RegularizationMethod method); 57 | void calculate_target_covariances_rbf(RegularizationMethod method); 58 | 59 | void get_source_covariances(std::vector>& covs) const; 60 | void get_target_covariances(std::vector>& covs) const; 61 | 62 | void get_voxel_num_points(std::vector& num_points) const; 63 | void get_voxel_means(std::vector>& means) const; 64 | void get_voxel_covs(std::vector>& covs) const; 65 | void get_voxel_correspondences(std::vector>& correspondences) const; 66 | 67 | void create_target_voxelmap(); 68 | 69 | void update_correspondences(const Eigen::Isometry3d& trans); 70 | 71 | double compute_error(const Eigen::Isometry3d& trans, Eigen::Matrix* H, Eigen::Matrix* b) const; 72 | 73 | public: 74 | double resolution; 75 | double kernel_width; 76 | double kernel_max_dist; 77 | std::unique_ptr offsets; 78 | 79 | std::unique_ptr source_points; 80 | std::unique_ptr target_points; 81 | 82 | std::unique_ptr source_neighbors; 83 | std::unique_ptr target_neighbors; 84 | 85 | std::unique_ptr source_covariances; 86 | std::unique_ptr target_covariances; 87 | 88 | std::unique_ptr voxelmap; 89 | 90 | Eigen::Isometry3f linearized_x; 91 | std::unique_ptr voxel_correspondences; 92 | }; 93 | 94 | } // namespace cuda 95 | } // namespace fast_gicp 96 | 97 | #endif -------------------------------------------------------------------------------- /lidar_mapping/include/fast_gicp/cuda/find_voxel_correspondences.cuh: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_CUDA_FIND_VOXEL_CORRESPONDENCES_CUH 2 | #define FAST_GICP_CUDA_FIND_VOXEL_CORRESPONDENCES_CUH 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | namespace fast_gicp { 12 | namespace cuda { 13 | 14 | void find_voxel_correspondences( 15 | const thrust::device_vector& src_points, 16 | const GaussianVoxelMap& voxelmap, 17 | const thrust::device_ptr& x_ptr, 18 | const thrust::device_vector& offsets, 19 | thrust::device_vector>& correspondences) ; 20 | 21 | } // namespace cuda 22 | } // namespace fast_gicp 23 | 24 | #endif -------------------------------------------------------------------------------- /lidar_mapping/include/fast_gicp/cuda/gaussian_voxelmap.cuh: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_CUDA_GAUSSIAN_VOXELMAP_CUH 2 | #define FAST_GICP_CUDA_GAUSSIAN_VOXELMAP_CUH 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace fast_gicp { 9 | namespace cuda { 10 | 11 | struct VoxelMapInfo { 12 | int num_voxels; 13 | int num_buckets; 14 | int max_bucket_scan_count; 15 | float voxel_resolution; 16 | }; 17 | 18 | class GaussianVoxelMap { 19 | public: 20 | GaussianVoxelMap(float resolution, int init_num_buckets = 8192, int max_bucket_scan_count = 10); 21 | 22 | void create_voxelmap(const thrust::device_vector& points); 23 | void create_voxelmap(const thrust::device_vector& points, const thrust::device_vector& covariances); 24 | 25 | private: 26 | void create_bucket_table(cudaStream_t stream, const thrust::device_vector& points); 27 | 28 | public: 29 | const int init_num_buckets; 30 | VoxelMapInfo voxelmap_info; 31 | thrust::device_vector voxelmap_info_ptr; 32 | 33 | thrust::device_vector> buckets; 34 | 35 | // voxel data 36 | thrust::device_vector num_points; 37 | thrust::device_vector voxel_means; 38 | thrust::device_vector voxel_covs; 39 | }; 40 | 41 | } // namespace cuda 42 | } // namespace fast_gicp 43 | 44 | #endif -------------------------------------------------------------------------------- /lidar_mapping/include/fast_gicp/cuda/ndt_compute_derivatives.cuh: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_CUDA_NDT_COMPUTE_DERIVATIVES_CUH 2 | #define FAST_GICP_CUDA_NDT_COMPUTE_DERIVATIVES_CUH 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | namespace fast_gicp { 11 | namespace cuda { 12 | 13 | double p2d_ndt_compute_derivatives( 14 | const GaussianVoxelMap& target_voxelmap, 15 | const thrust::device_vector& source_points, 16 | const thrust::device_vector>& correspondences, 17 | const thrust::device_ptr& linearized_x_ptr, 18 | const thrust::device_ptr& x_ptr, 19 | Eigen::Matrix* H, 20 | Eigen::Matrix* b); 21 | 22 | double d2d_ndt_compute_derivatives( 23 | const GaussianVoxelMap& target_voxelmap, 24 | const GaussianVoxelMap& source_voxelmap, 25 | const thrust::device_vector>& correspondences, 26 | const thrust::device_ptr& linearized_x_ptr, 27 | const thrust::device_ptr& x_ptr, 28 | Eigen::Matrix* H, 29 | Eigen::Matrix* b); 30 | 31 | } // namespace cuda 32 | } // namespace fast_gicp 33 | 34 | #endif -------------------------------------------------------------------------------- /lidar_mapping/include/fast_gicp/cuda/ndt_cuda.cuh: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_NDT_CUDA_CUH 2 | #define FAST_GICP_NDT_CUDA_CUH 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | namespace thrust { 13 | template 14 | class pair; 15 | 16 | template 17 | class device_allocator; 18 | 19 | template 20 | class device_vector; 21 | } // namespace thrust 22 | 23 | namespace fast_gicp { 24 | namespace cuda { 25 | 26 | class GaussianVoxelMap; 27 | 28 | class NDTCudaCore { 29 | public: 30 | using Points = thrust::device_vector>; 31 | using Indices = thrust::device_vector>; 32 | using Matrices = thrust::device_vector>; 33 | using Correspondences = thrust::device_vector, thrust::device_allocator>>; 34 | using VoxelCoordinates = thrust::device_vector>; 35 | 36 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 37 | NDTCudaCore(); 38 | ~NDTCudaCore(); 39 | 40 | void set_distance_mode(fast_gicp::NDTDistanceMode mode); 41 | void set_resolution(double resolution); 42 | void set_neighbor_search_method(fast_gicp::NeighborSearchMethod method, double radius); 43 | 44 | void swap_source_and_target(); 45 | void set_source_cloud(const std::vector>& cloud); 46 | void set_target_cloud(const std::vector>& cloud); 47 | 48 | void create_voxelmaps(); 49 | void create_target_voxelmap(); 50 | void create_source_voxelmap(); 51 | 52 | void update_correspondences(const Eigen::Isometry3d& trans); 53 | double compute_error(const Eigen::Isometry3d& trans, Eigen::Matrix* H, Eigen::Matrix* b) const; 54 | 55 | public: 56 | fast_gicp::NDTDistanceMode distance_mode; 57 | double resolution; 58 | std::unique_ptr offsets; 59 | 60 | std::unique_ptr source_points; 61 | std::unique_ptr target_points; 62 | 63 | std::unique_ptr source_voxelmap; 64 | std::unique_ptr target_voxelmap; 65 | 66 | Eigen::Isometry3f linearized_x; 67 | std::unique_ptr correspondences; 68 | }; 69 | 70 | } // namespace cuda 71 | } // namespace fast_gicp 72 | 73 | #endif -------------------------------------------------------------------------------- /lidar_mapping/include/fast_gicp/cuda/vector3_hash.cuh: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_CUDA_VECTOR3_HASH_CUH 2 | #define FAST_GICP_CUDA_VECTOR3_HASH_CUH 3 | 4 | namespace fast_gicp { 5 | namespace cuda { 6 | 7 | // taken from boost/hash.hpp 8 | __host__ __device__ inline void hash_combine(uint64_t& h, uint64_t k) { 9 | const uint64_t m = UINT64_C(0xc6a4a7935bd1e995); 10 | const int r = 47; 11 | 12 | k *= m; 13 | k ^= k >> r; 14 | k *= m; 15 | 16 | h ^= k; 17 | h *= m; 18 | 19 | h += 0xe6546b64; 20 | } 21 | 22 | inline __host__ __device__ bool equal(const Eigen::Vector3i& lhs, const Eigen::Vector3i& rhs) { 23 | return lhs[0] == rhs[0] && lhs[1] == rhs[1] && lhs[2] == rhs[2]; 24 | } 25 | 26 | // compute vector3i hash 27 | __host__ __device__ inline uint64_t vector3i_hash(const Eigen::Vector3i& x) { 28 | uint64_t seed = 0; 29 | hash_combine(seed, x[0]); 30 | hash_combine(seed, x[1]); 31 | hash_combine(seed, x[2]); 32 | return seed; 33 | } 34 | 35 | __host__ __device__ inline Eigen::Vector3i calc_voxel_coord(const Eigen::Vector3f& x, float resolution) { 36 | Eigen::Vector3i coord = (x.array() / resolution - 0.5).floor().cast(); 37 | return coord; 38 | } 39 | 40 | } 41 | } // namespace fast_gicp 42 | 43 | #endif -------------------------------------------------------------------------------- /lidar_mapping/include/fast_gicp/fast_gicp.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_FAST_GICP_HPP 2 | #define FAST_GICP_FAST_GICP_HPP 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | namespace fast_gicp { 15 | 16 | /** 17 | * @brief Fast GICP algorithm optimized for multi threading with OpenMP 18 | */ 19 | template 20 | class FastGICP : public LsqRegistration { 21 | public: 22 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 23 | using Scalar = float; 24 | using Matrix4 = typename pcl::Registration::Matrix4; 25 | 26 | using PointCloudSource = typename pcl::Registration::PointCloudSource; 27 | using PointCloudSourcePtr = typename PointCloudSource::Ptr; 28 | using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr; 29 | 30 | using PointCloudTarget = typename pcl::Registration::PointCloudTarget; 31 | using PointCloudTargetPtr = typename PointCloudTarget::Ptr; 32 | using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr; 33 | 34 | protected: 35 | using pcl::Registration::reg_name_; 36 | using pcl::Registration::input_; 37 | using pcl::Registration::target_; 38 | using pcl::Registration::corr_dist_threshold_; 39 | 40 | public: 41 | FastGICP(); 42 | virtual ~FastGICP() override; 43 | 44 | void setNumThreads(int n); 45 | void setCorrespondenceRandomness(int k); 46 | void setRegularizationMethod(RegularizationMethod method); 47 | 48 | virtual void swapSourceAndTarget() override; 49 | virtual void clearSource() override; 50 | virtual void clearTarget() override; 51 | 52 | virtual void setInputSource(const PointCloudSourceConstPtr& cloud) override; 53 | virtual void setSourceCovariances(const std::vector>& covs); 54 | virtual void setInputTarget(const PointCloudTargetConstPtr& cloud) override; 55 | virtual void setTargetCovariances(const std::vector>& covs); 56 | 57 | const std::vector>& getSourceCovariances() const { 58 | return source_covs_; 59 | } 60 | 61 | const std::vector>& getTargetCovariances() const { 62 | return target_covs_; 63 | } 64 | 65 | protected: 66 | virtual void computeTransformation(PointCloudSource& output, const Matrix4& guess) override; 67 | 68 | virtual void update_correspondences(const Eigen::Isometry3d& trans); 69 | 70 | virtual double linearize(const Eigen::Isometry3d& trans, Eigen::Matrix* H, Eigen::Matrix* b) override; 71 | //virtual double linearize(const Eigen::Isometry3d& trans, Eigen::MatrixXd* H, Eigen::MatrixXd* b) override; 72 | 73 | virtual double compute_error(const Eigen::Isometry3d& trans) override; 74 | 75 | template 76 | bool calculate_covariances(const typename pcl::PointCloud::ConstPtr& cloud, pcl::search::KdTree& kdtree, std::vector>& covariances); 77 | 78 | protected: 79 | int num_threads_; 80 | int k_correspondences_; 81 | 82 | RegularizationMethod regularization_method_; 83 | 84 | std::shared_ptr> source_kdtree_; 85 | std::shared_ptr> target_kdtree_; 86 | 87 | std::vector> source_covs_; 88 | std::vector> target_covs_; 89 | 90 | std::vector> mahalanobis_; 91 | 92 | std::vector correspondences_; 93 | std::vector sq_distances_; 94 | }; 95 | } // namespace fast_gicp 96 | 97 | #endif 98 | -------------------------------------------------------------------------------- /lidar_mapping/include/fast_gicp/fast_gicp_st.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_FAST_GICP_ST_HPP 2 | #define FAST_GICP_FAST_GICP_ST_HPP 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | namespace fast_gicp { 15 | 16 | /** 17 | * @brief Fast GICP algorithm optimized for single threading 18 | */ 19 | template 20 | class FastGICPSingleThread : public FastGICP { 21 | public: 22 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 23 | using Scalar = float; 24 | using Matrix4 = typename pcl::Registration::Matrix4; 25 | 26 | using PointCloudSource = typename pcl::Registration::PointCloudSource; 27 | using PointCloudSourcePtr = typename PointCloudSource::Ptr; 28 | using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr; 29 | 30 | protected: 31 | using pcl::Registration::input_; 32 | using pcl::Registration::target_; 33 | 34 | using FastGICP::target_kdtree_; 35 | using FastGICP::correspondences_; 36 | using FastGICP::sq_distances_; 37 | using FastGICP::source_covs_; 38 | using FastGICP::target_covs_; 39 | using FastGICP::mahalanobis_; 40 | 41 | public: 42 | FastGICPSingleThread(); 43 | virtual ~FastGICPSingleThread() override; 44 | 45 | protected: 46 | virtual void computeTransformation(PointCloudSource& output, const Matrix4& guess) override; 47 | 48 | private: 49 | virtual void update_correspondences(const Eigen::Isometry3d& trans) override; 50 | 51 | virtual double linearize(const Eigen::Isometry3d& trans, Eigen::Matrix* H = nullptr, Eigen::Matrix* b = nullptr) override; 52 | 53 | virtual double compute_error(const Eigen::Isometry3d& trans) override; 54 | 55 | private: 56 | std::vector second_sq_distances_; 57 | std::vector> anchors_; 58 | }; 59 | } // namespace fast_gicp 60 | 61 | #endif 62 | -------------------------------------------------------------------------------- /lidar_mapping/include/fast_gicp/fast_vgicp.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_FAST_VGICP_HPP 2 | #define FAST_GICP_FAST_VGICP_HPP 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | #include 17 | 18 | namespace fast_gicp { 19 | 20 | /** 21 | * @brief Fast Voxelized GICP algorithm boosted with OpenMP 22 | */ 23 | template 24 | class FastVGICP : public FastGICP { 25 | public: 26 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 27 | using Scalar = float; 28 | using Matrix4 = typename pcl::Registration::Matrix4; 29 | 30 | using PointCloudSource = typename pcl::Registration::PointCloudSource; 31 | using PointCloudSourcePtr = typename PointCloudSource::Ptr; 32 | using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr; 33 | 34 | using PointCloudTarget = typename pcl::Registration::PointCloudTarget; 35 | using PointCloudTargetPtr = typename PointCloudTarget::Ptr; 36 | using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr; 37 | 38 | protected: 39 | using pcl::Registration::input_; 40 | using pcl::Registration::target_; 41 | 42 | using FastGICP::num_threads_; 43 | using FastGICP::source_kdtree_; 44 | using FastGICP::target_kdtree_; 45 | using FastGICP::source_covs_; 46 | using FastGICP::target_covs_; 47 | 48 | public: 49 | FastVGICP(); 50 | virtual ~FastVGICP() override; 51 | 52 | void setResolution(double resolution); 53 | void setVoxelAccumulationMode(VoxelAccumulationMode mode); 54 | void setNeighborSearchMethod(NeighborSearchMethod method); 55 | 56 | virtual void swapSourceAndTarget() override; 57 | virtual void setInputTarget(const PointCloudTargetConstPtr& cloud) override; 58 | 59 | protected: 60 | virtual void computeTransformation(PointCloudSource& output, const Matrix4& guess) override; 61 | virtual void update_correspondences(const Eigen::Isometry3d& trans) override; 62 | virtual double linearize(const Eigen::Isometry3d& trans, Eigen::Matrix* H = nullptr, Eigen::Matrix* b = nullptr) override; 63 | virtual double compute_error(const Eigen::Isometry3d& trans) override; 64 | 65 | protected: 66 | double voxel_resolution_; 67 | NeighborSearchMethod search_method_; 68 | VoxelAccumulationMode voxel_mode_; 69 | 70 | std::unique_ptr> voxelmap_; 71 | 72 | std::vector> voxel_correspondences_; 73 | std::vector> voxel_mahalanobis_; 74 | }; 75 | } // namespace fast_gicp 76 | 77 | #endif 78 | -------------------------------------------------------------------------------- /lidar_mapping/include/fast_gicp/gicp/experimental/fast_gicp_mp.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_FAST_GICP_MP_HPP 2 | #define FAST_GICP_FAST_GICP_MP_HPP 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | namespace fast_gicp { 14 | 15 | template 16 | class FastGICPMultiPoints : public pcl::Registration { 17 | public: 18 | using Scalar = float; 19 | using Matrix4 = typename pcl::Registration::Matrix4; 20 | 21 | using PointCloudSource = typename pcl::Registration::PointCloudSource; 22 | using PointCloudSourcePtr = typename PointCloudSource::Ptr; 23 | using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr; 24 | 25 | using PointCloudTarget = typename pcl::Registration::PointCloudTarget; 26 | using PointCloudTargetPtr = typename PointCloudTarget::Ptr; 27 | using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr; 28 | 29 | using pcl::Registration::reg_name_; 30 | using pcl::Registration::input_; 31 | using pcl::Registration::target_; 32 | 33 | using pcl::Registration::nr_iterations_; 34 | using pcl::Registration::max_iterations_; 35 | using pcl::Registration::final_transformation_; 36 | using pcl::Registration::transformation_epsilon_; 37 | using pcl::Registration::converged_; 38 | using pcl::Registration::corr_dist_threshold_; 39 | 40 | FastGICPMultiPoints(); 41 | virtual ~FastGICPMultiPoints() override; 42 | 43 | void setNumThreads(int n); 44 | 45 | void setRotationEpsilon(double eps); 46 | 47 | void setCorrespondenceRandomness(int k); 48 | 49 | void setRegularizationMethod(RegularizationMethod method); 50 | 51 | virtual void setInputSource(const PointCloudSourceConstPtr& cloud) override; 52 | 53 | virtual void setInputTarget(const PointCloudTargetConstPtr& cloud) override; 54 | 55 | protected: 56 | virtual void computeTransformation(PointCloudSource& output, const Matrix4& guess) override; 57 | 58 | private: 59 | bool is_converged(const Eigen::Matrix& delta) const; 60 | 61 | void update_correspondences(const Eigen::Matrix& x); 62 | 63 | Eigen::VectorXf loss_ls(const Eigen::Matrix& x, Eigen::MatrixXf* J) const; 64 | 65 | template 66 | bool calculate_covariances(const boost::shared_ptr>& cloud, pcl::search::KdTree& kdtree, std::vector>& covariances); 67 | 68 | private: 69 | int num_threads_; 70 | int k_correspondences_; 71 | double rotation_epsilon_; 72 | double neighbor_search_radius_; 73 | 74 | RegularizationMethod regularization_method_; 75 | 76 | pcl::search::KdTree source_kdtree; 77 | pcl::search::KdTree target_kdtree; 78 | 79 | std::vector> source_covs; 80 | std::vector> target_covs; 81 | 82 | std::vector> correspondences; 83 | std::vector> sq_distances; 84 | }; 85 | } // namespace fast_gicp 86 | 87 | #endif 88 | -------------------------------------------------------------------------------- /lidar_mapping/include/fast_gicp/gicp/fast_gicp.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_FAST_GICP_HPP 2 | #define FAST_GICP_FAST_GICP_HPP 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | namespace fast_gicp { 15 | 16 | /** 17 | * @brief Fast GICP algorithm optimized for multi threading with OpenMP 18 | */ 19 | template 20 | class FastGICP : public LsqRegistration { 21 | public: 22 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 23 | using Scalar = float; 24 | using Matrix4 = typename pcl::Registration::Matrix4; 25 | 26 | using PointCloudSource = typename pcl::Registration::PointCloudSource; 27 | using PointCloudSourcePtr = typename PointCloudSource::Ptr; 28 | using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr; 29 | 30 | using PointCloudTarget = typename pcl::Registration::PointCloudTarget; 31 | using PointCloudTargetPtr = typename PointCloudTarget::Ptr; 32 | using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr; 33 | 34 | protected: 35 | using pcl::Registration::reg_name_; 36 | using pcl::Registration::input_; 37 | using pcl::Registration::target_; 38 | using pcl::Registration::corr_dist_threshold_; 39 | 40 | public: 41 | FastGICP(); 42 | virtual ~FastGICP() override; 43 | 44 | void setNumThreads(int n); 45 | void setCorrespondenceRandomness(int k); 46 | void setRegularizationMethod(RegularizationMethod method); 47 | 48 | virtual void swapSourceAndTarget() override; 49 | virtual void clearSource() override; 50 | virtual void clearTarget() override; 51 | 52 | virtual void setInputSource(const PointCloudSourceConstPtr& cloud) override; 53 | virtual void setSourceCovariances(const std::vector>& covs); 54 | virtual void setInputTarget(const PointCloudTargetConstPtr& cloud) override; 55 | virtual void setTargetCovariances(const std::vector>& covs); 56 | 57 | const std::vector>& getSourceCovariances() const { 58 | return source_covs_; 59 | } 60 | 61 | const std::vector>& getTargetCovariances() const { 62 | return target_covs_; 63 | } 64 | 65 | protected: 66 | virtual void computeTransformation(PointCloudSource& output, const Matrix4& guess) override; 67 | 68 | virtual void update_correspondences(const Eigen::Isometry3d& trans); 69 | 70 | virtual double linearize(const Eigen::Isometry3d& trans, Eigen::Matrix* H, Eigen::Matrix* b) override; 71 | //virtual double linearize(const Eigen::Isometry3d& trans, Eigen::MatrixXd* H, Eigen::MatrixXd* b) override; 72 | 73 | virtual double compute_error(const Eigen::Isometry3d& trans) override; 74 | 75 | template 76 | bool calculate_covariances(const typename pcl::PointCloud::ConstPtr& cloud, pcl::search::KdTree& kdtree, std::vector>& covariances); 77 | 78 | protected: 79 | int num_threads_; 80 | int k_correspondences_; 81 | 82 | RegularizationMethod regularization_method_; 83 | 84 | std::shared_ptr> source_kdtree_; 85 | std::shared_ptr> target_kdtree_; 86 | 87 | std::vector> source_covs_; 88 | std::vector> target_covs_; 89 | 90 | std::vector> mahalanobis_; 91 | 92 | std::vector correspondences_; 93 | std::vector sq_distances_; 94 | }; 95 | } // namespace fast_gicp 96 | 97 | #endif 98 | -------------------------------------------------------------------------------- /lidar_mapping/include/fast_gicp/gicp/fast_gicp_st.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_FAST_GICP_ST_HPP 2 | #define FAST_GICP_FAST_GICP_ST_HPP 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | namespace fast_gicp { 15 | 16 | /** 17 | * @brief Fast GICP algorithm optimized for single threading 18 | */ 19 | template 20 | class FastGICPSingleThread : public FastGICP { 21 | public: 22 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 23 | using Scalar = float; 24 | using Matrix4 = typename pcl::Registration::Matrix4; 25 | 26 | using PointCloudSource = typename pcl::Registration::PointCloudSource; 27 | using PointCloudSourcePtr = typename PointCloudSource::Ptr; 28 | using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr; 29 | 30 | protected: 31 | using pcl::Registration::input_; 32 | using pcl::Registration::target_; 33 | 34 | using FastGICP::target_kdtree_; 35 | using FastGICP::correspondences_; 36 | using FastGICP::sq_distances_; 37 | using FastGICP::source_covs_; 38 | using FastGICP::target_covs_; 39 | using FastGICP::mahalanobis_; 40 | 41 | public: 42 | FastGICPSingleThread(); 43 | virtual ~FastGICPSingleThread() override; 44 | 45 | protected: 46 | virtual void computeTransformation(PointCloudSource& output, const Matrix4& guess) override; 47 | 48 | private: 49 | virtual void update_correspondences(const Eigen::Isometry3d& trans) override; 50 | 51 | virtual double linearize(const Eigen::Isometry3d& trans, Eigen::Matrix* H = nullptr, Eigen::Matrix* b = nullptr) override; 52 | 53 | virtual double compute_error(const Eigen::Isometry3d& trans) override; 54 | 55 | private: 56 | std::vector second_sq_distances_; 57 | std::vector> anchors_; 58 | }; 59 | } // namespace fast_gicp 60 | 61 | #endif 62 | -------------------------------------------------------------------------------- /lidar_mapping/include/fast_gicp/gicp/fast_vgicp.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_FAST_VGICP_HPP 2 | #define FAST_GICP_FAST_VGICP_HPP 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | #include 17 | 18 | namespace fast_gicp { 19 | 20 | /** 21 | * @brief Fast Voxelized GICP algorithm boosted with OpenMP 22 | */ 23 | template 24 | class FastVGICP : public FastGICP { 25 | public: 26 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 27 | using Scalar = float; 28 | using Matrix4 = typename pcl::Registration::Matrix4; 29 | 30 | using PointCloudSource = typename pcl::Registration::PointCloudSource; 31 | using PointCloudSourcePtr = typename PointCloudSource::Ptr; 32 | using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr; 33 | 34 | using PointCloudTarget = typename pcl::Registration::PointCloudTarget; 35 | using PointCloudTargetPtr = typename PointCloudTarget::Ptr; 36 | using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr; 37 | 38 | protected: 39 | using pcl::Registration::input_; 40 | using pcl::Registration::target_; 41 | 42 | using FastGICP::num_threads_; 43 | using FastGICP::source_kdtree_; 44 | using FastGICP::target_kdtree_; 45 | using FastGICP::source_covs_; 46 | using FastGICP::target_covs_; 47 | 48 | public: 49 | FastVGICP(); 50 | virtual ~FastVGICP() override; 51 | 52 | void setResolution(double resolution); 53 | void setVoxelAccumulationMode(VoxelAccumulationMode mode); 54 | void setNeighborSearchMethod(NeighborSearchMethod method); 55 | 56 | virtual void swapSourceAndTarget() override; 57 | virtual void setInputTarget(const PointCloudTargetConstPtr& cloud) override; 58 | 59 | protected: 60 | virtual void computeTransformation(PointCloudSource& output, const Matrix4& guess) override; 61 | virtual void update_correspondences(const Eigen::Isometry3d& trans) override; 62 | virtual double linearize(const Eigen::Isometry3d& trans, Eigen::Matrix* H = nullptr, Eigen::Matrix* b = nullptr) override; 63 | virtual double compute_error(const Eigen::Isometry3d& trans) override; 64 | 65 | protected: 66 | double voxel_resolution_; 67 | NeighborSearchMethod search_method_; 68 | VoxelAccumulationMode voxel_mode_; 69 | 70 | std::unique_ptr> voxelmap_; 71 | 72 | std::vector> voxel_correspondences_; 73 | std::vector> voxel_mahalanobis_; 74 | }; 75 | } // namespace fast_gicp 76 | 77 | #endif 78 | -------------------------------------------------------------------------------- /lidar_mapping/include/fast_gicp/gicp/fast_vgicp_cuda.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_FAST_VGICP_CUDA_HPP 2 | #define FAST_GICP_FAST_VGICP_CUDA_HPP 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | 15 | namespace fast_gicp { 16 | 17 | namespace cuda { 18 | class FastVGICPCudaCore; 19 | } 20 | 21 | enum class NearestNeighborMethod { CPU_PARALLEL_KDTREE, GPU_BRUTEFORCE, GPU_RBF_KERNEL }; 22 | 23 | /** 24 | * @brief Fast Voxelized GICP algorithm boosted with CUDA 25 | */ 26 | template 27 | class FastVGICPCuda : public LsqRegistration { 28 | public: 29 | using Scalar = float; 30 | using Matrix4 = typename pcl::Registration::Matrix4; 31 | 32 | using PointCloudSource = typename pcl::Registration::PointCloudSource; 33 | using PointCloudSourcePtr = typename PointCloudSource::Ptr; 34 | using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr; 35 | 36 | using PointCloudTarget = typename pcl::Registration::PointCloudTarget; 37 | using PointCloudTargetPtr = typename PointCloudTarget::Ptr; 38 | using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr; 39 | 40 | protected: 41 | using pcl::Registration::input_; 42 | using pcl::Registration::target_; 43 | 44 | public: 45 | FastVGICPCuda(); 46 | virtual ~FastVGICPCuda() override; 47 | 48 | void setCorrespondenceRandomness(int k); 49 | void setResolution(double resolution); 50 | void setKernelWidth(double kernel_width, double max_dist = -1.0); 51 | void setRegularizationMethod(RegularizationMethod method); 52 | void setNeighborSearchMethod(NeighborSearchMethod method, double radius = -1.0); 53 | void setNearestNeighborSearchMethod(NearestNeighborMethod method); 54 | 55 | virtual void swapSourceAndTarget() override; 56 | virtual void clearSource() override; 57 | virtual void clearTarget() override; 58 | 59 | virtual void setInputSource(const PointCloudSourceConstPtr& cloud) override; 60 | virtual void setInputTarget(const PointCloudTargetConstPtr& cloud) override; 61 | 62 | protected: 63 | virtual void computeTransformation(PointCloudSource& output, const Matrix4& guess) override; 64 | virtual double linearize(const Eigen::Isometry3d& trans, Eigen::Matrix* H = nullptr, Eigen::Matrix* b = nullptr) override; 65 | virtual double compute_error(const Eigen::Isometry3d& trans) override; 66 | 67 | template 68 | std::vector find_neighbors_parallel_kdtree(int k, typename pcl::PointCloud::ConstPtr cloud) const; 69 | 70 | private: 71 | int k_correspondences_; 72 | double voxel_resolution_; 73 | RegularizationMethod regularization_method_; 74 | NearestNeighborMethod neighbor_search_method_; 75 | 76 | std::unique_ptr vgicp_cuda_; 77 | }; 78 | 79 | } // namespace fast_gicp 80 | 81 | #endif 82 | -------------------------------------------------------------------------------- /lidar_mapping/include/fast_gicp/gicp/gicp_settings.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_GICP_SETTINGS_HPP 2 | #define FAST_GICP_GICP_SETTINGS_HPP 3 | 4 | namespace fast_gicp { 5 | 6 | enum class RegularizationMethod { NONE, MIN_EIG, NORMALIZED_MIN_EIG, PLANE, FROBENIUS }; 7 | 8 | enum class NeighborSearchMethod { DIRECT27, DIRECT7, DIRECT1, /* supported on only VGICP_CUDA */ DIRECT_RADIUS }; 9 | 10 | enum class VoxelAccumulationMode { ADDITIVE, ADDITIVE_WEIGHTED, MULTIPLICATIVE }; 11 | } 12 | 13 | #endif -------------------------------------------------------------------------------- /lidar_mapping/include/fast_gicp/gicp/impl/fast_gicp_st_impl.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_FAST_GICP_ST_IMPL_HPP 2 | #define FAST_GICP_FAST_GICP_ST_IMPL_HPP 3 | 4 | #include 5 | #include 6 | 7 | namespace fast_gicp { 8 | 9 | template 10 | FastGICPSingleThread::FastGICPSingleThread() : FastGICP() { 11 | this->reg_name_ = "FastGICPSingleThread"; 12 | this->num_threads_ = 1; 13 | } 14 | 15 | template 16 | FastGICPSingleThread::~FastGICPSingleThread() {} 17 | 18 | template 19 | void FastGICPSingleThread::computeTransformation(PointCloudSource& output, const Matrix4& guess) { 20 | anchors_.clear(); 21 | FastGICP::computeTransformation(output, guess); 22 | } 23 | 24 | template 25 | void FastGICPSingleThread::update_correspondences(const Eigen::Isometry3d& x) { 26 | assert(source_covs_.size() == input_->size()); 27 | assert(target_covs_.size() == target_->size()); 28 | 29 | Eigen::Isometry3f trans = x.template cast(); 30 | 31 | bool is_first = anchors_.empty(); 32 | 33 | correspondences_.resize(input_->size()); 34 | sq_distances_.resize(input_->size()); 35 | second_sq_distances_.resize(input_->size()); 36 | anchors_.resize(input_->size()); 37 | mahalanobis_.resize(input_->size()); 38 | 39 | std::vector k_indices; 40 | std::vector k_sq_dists; 41 | 42 | for (int i = 0; i < input_->size(); i++) { 43 | PointTarget pt; 44 | pt.getVector4fMap() = trans * input_->at(i).getVector4fMap(); 45 | 46 | if (!is_first) { 47 | double d = (pt.getVector4fMap() - anchors_[i]).norm(); 48 | double max_first = std::sqrt(sq_distances_[i]) + d; 49 | double min_second = std::sqrt(second_sq_distances_[i]) - d; 50 | 51 | if (max_first < min_second) { 52 | continue; 53 | } 54 | } 55 | 56 | target_kdtree_->nearestKSearch(pt, 2, k_indices, k_sq_dists); 57 | 58 | correspondences_[i] = k_sq_dists[0] < this->corr_dist_threshold_ * this->corr_dist_threshold_ ? k_indices[0] : -1; 59 | sq_distances_[i] = k_sq_dists[0]; 60 | second_sq_distances_[i] = k_sq_dists[1]; 61 | anchors_[i] = pt.getVector4fMap(); 62 | 63 | if (correspondences_[i] < 0) { 64 | continue; 65 | } 66 | 67 | const int target_index = correspondences_[i]; 68 | const auto& cov_A = source_covs_[i]; 69 | const auto& cov_B = target_covs_[target_index]; 70 | 71 | Eigen::Matrix4d RCR = cov_B + x.matrix() * cov_A * x.matrix().transpose(); 72 | RCR(3, 3) = 1.0; 73 | 74 | mahalanobis_[i] = RCR.inverse(); 75 | mahalanobis_[i](3, 3) = 0.0; 76 | } 77 | } 78 | 79 | template 80 | double FastGICPSingleThread::linearize(const Eigen::Isometry3d& trans, Eigen::Matrix* H, Eigen::Matrix* b) { 81 | if (H && b) { 82 | update_correspondences(trans); 83 | H->setZero(); 84 | b->setZero(); 85 | } 86 | 87 | double sum_errors = 0.0; 88 | for (int i = 0; i < input_->size(); i++) { 89 | int target_index = correspondences_[i]; 90 | if (target_index < 0) { 91 | continue; 92 | } 93 | 94 | const Eigen::Vector4d mean_A = input_->at(i).getVector4fMap().template cast(); 95 | const auto& cov_A = source_covs_[i]; 96 | 97 | const Eigen::Vector4d mean_B = target_->at(target_index).getVector4fMap().template cast(); 98 | const auto& cov_B = target_covs_[target_index]; 99 | 100 | const Eigen::Vector4d transed_mean_A = trans * mean_A; 101 | const Eigen::Vector4d error = mean_B - transed_mean_A; 102 | 103 | sum_errors += error.transpose() * mahalanobis_[i] * error; 104 | 105 | if (H == nullptr || b == nullptr) { 106 | continue; 107 | } 108 | 109 | Eigen::Matrix dtdx0 = Eigen::Matrix::Zero(); 110 | dtdx0.block<3, 3>(0, 0) = skewd(transed_mean_A.head<3>()); 111 | dtdx0.block<3, 3>(0, 3) = -Eigen::Matrix3d::Identity(); 112 | 113 | Eigen::Matrix jlossexp = dtdx0; 114 | 115 | (*H) += jlossexp.transpose() * mahalanobis_[i] * jlossexp; 116 | (*b) += jlossexp.transpose() * mahalanobis_[i] * error; 117 | } 118 | 119 | return sum_errors; 120 | } 121 | 122 | template 123 | double FastGICPSingleThread::compute_error(const Eigen::Isometry3d& trans) { 124 | return linearize(trans, nullptr, nullptr); 125 | } 126 | 127 | } // namespace fast_gicp 128 | 129 | #endif 130 | -------------------------------------------------------------------------------- /lidar_mapping/include/fast_gicp/gicp/lsq_registration.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_LSQ_REGISTRATION_HPP 2 | #define FAST_GICP_LSQ_REGISTRATION_HPP 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | namespace fast_gicp { 12 | 13 | enum class LSQ_OPTIMIZER_TYPE { GaussNewton, LevenbergMarquardt }; 14 | 15 | template 16 | class LsqRegistration : public pcl::Registration { 17 | public: 18 | using Scalar = float; 19 | using Matrix4 = typename pcl::Registration::Matrix4; 20 | 21 | using PointCloudSource = typename pcl::Registration::PointCloudSource; 22 | using PointCloudSourcePtr = typename PointCloudSource::Ptr; 23 | using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr; 24 | 25 | using PointCloudTarget = typename pcl::Registration::PointCloudTarget; 26 | using PointCloudTargetPtr = typename PointCloudTarget::Ptr; 27 | using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr; 28 | 29 | protected: 30 | using pcl::Registration::input_; 31 | using pcl::Registration::nr_iterations_; 32 | using pcl::Registration::max_iterations_; 33 | using pcl::Registration::final_transformation_; 34 | using pcl::Registration::transformation_epsilon_; 35 | using pcl::Registration::converged_; 36 | 37 | public: 38 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 39 | 40 | LsqRegistration(); 41 | virtual ~LsqRegistration(); 42 | 43 | void setRotationEpsilon(double eps); 44 | void setInitialLambdaFactor(double init_lambda_factor); 45 | void setDebugPrint(bool lm_debug_print); 46 | 47 | const Eigen::Matrix& getFinalHessian() const; 48 | 49 | virtual void swapSourceAndTarget() {} 50 | virtual void clearSource() {} 51 | virtual void clearTarget() {} 52 | 53 | protected: 54 | virtual void computeTransformation(PointCloudSource& output, const Matrix4& guess) override; 55 | 56 | bool is_converged(const Eigen::Isometry3d& delta) const; 57 | 58 | virtual double linearize(const Eigen::Isometry3d& trans, Eigen::Matrix* H = nullptr, Eigen::Matrix* b = nullptr) = 0; 59 | virtual double compute_error(const Eigen::Isometry3d& trans) = 0; 60 | 61 | bool step_optimize(Eigen::Isometry3d& x0, Eigen::Isometry3d& delta); 62 | bool step_gn(Eigen::Isometry3d& x0, Eigen::Isometry3d& delta); 63 | bool step_lm(Eigen::Isometry3d& x0, Eigen::Isometry3d& delta); 64 | 65 | protected: 66 | double rotation_epsilon_; 67 | 68 | LSQ_OPTIMIZER_TYPE lsq_optimizer_type_; 69 | int lm_max_iterations_; 70 | double lm_init_lambda_factor_; 71 | double lm_lambda_; 72 | bool lm_debug_print_; 73 | 74 | Eigen::Matrix final_hessian_; 75 | }; 76 | } // namespace fast_gicp 77 | 78 | #endif -------------------------------------------------------------------------------- /lidar_mapping/include/fast_gicp/ndt/impl/ndt_cuda_impl.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_NDT_CUDA_IMPL_HPP 2 | #define FAST_GICP_NDT_CUDA_IMPL_HPP 3 | 4 | #include 5 | 6 | #include 7 | 8 | namespace fast_gicp { 9 | 10 | template 11 | NDTCuda::NDTCuda() : LsqRegistration() { 12 | this->reg_name_ = "NDTCuda"; 13 | ndt_cuda_.reset(new cuda::NDTCudaCore()); 14 | } 15 | 16 | template 17 | NDTCuda::~NDTCuda() {} 18 | 19 | template 20 | void NDTCuda::setDistanceMode(NDTDistanceMode mode) { 21 | ndt_cuda_->set_distance_mode(mode); 22 | } 23 | 24 | template 25 | void NDTCuda::setResolution(double resolution) { 26 | ndt_cuda_->set_resolution(resolution); 27 | } 28 | 29 | template 30 | void NDTCuda::setNeighborSearchMethod(NeighborSearchMethod method, double radius) { 31 | ndt_cuda_->set_neighbor_search_method(method, radius); 32 | } 33 | 34 | template 35 | void NDTCuda::swapSourceAndTarget() { 36 | ndt_cuda_->swap_source_and_target(); 37 | input_.swap(target_); 38 | } 39 | 40 | template 41 | void NDTCuda::clearSource() { 42 | input_.reset(); 43 | } 44 | 45 | template 46 | void NDTCuda::clearTarget() { 47 | target_.reset(); 48 | } 49 | 50 | template 51 | void NDTCuda::setInputSource(const PointCloudSourceConstPtr& cloud) { 52 | if (cloud == input_) { 53 | return; 54 | } 55 | pcl::Registration::setInputSource(cloud); 56 | 57 | std::vector> points(cloud->size()); 58 | std::transform(cloud->begin(), cloud->end(), points.begin(), [=](const PointSource& pt) { return pt.getVector3fMap(); }); 59 | ndt_cuda_->set_source_cloud(points); 60 | } 61 | 62 | template 63 | void NDTCuda::setInputTarget(const PointCloudTargetConstPtr& cloud) { 64 | if (cloud == target_) { 65 | return; 66 | } 67 | 68 | pcl::Registration::setInputTarget(cloud); 69 | 70 | std::vector> points(cloud->size()); 71 | std::transform(cloud->begin(), cloud->end(), points.begin(), [=](const PointTarget& pt) { return pt.getVector3fMap(); }); 72 | ndt_cuda_->set_target_cloud(points); 73 | } 74 | 75 | template 76 | void NDTCuda::computeTransformation(PointCloudSource& output, const Matrix4& guess) { 77 | ndt_cuda_->create_voxelmaps(); 78 | LsqRegistration::computeTransformation(output, guess); 79 | } 80 | 81 | template 82 | double NDTCuda::linearize(const Eigen::Isometry3d& trans, Eigen::Matrix* H, Eigen::Matrix* b) { 83 | ndt_cuda_->update_correspondences(trans); 84 | return ndt_cuda_->compute_error(trans, H, b); 85 | } 86 | 87 | template 88 | double NDTCuda::compute_error(const Eigen::Isometry3d& trans) { 89 | return ndt_cuda_->compute_error(trans, nullptr, nullptr); 90 | } 91 | 92 | } // namespace fast_gicp 93 | 94 | #endif -------------------------------------------------------------------------------- /lidar_mapping/include/fast_gicp/ndt/ndt_cuda.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_NDT_CUDA_HPP 2 | #define FAST_GICP_NDT_CUDA_HPP 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | namespace fast_gicp { 16 | 17 | namespace cuda { 18 | class NDTCudaCore; 19 | } 20 | 21 | template 22 | class NDTCuda : public LsqRegistration { 23 | public: 24 | using Scalar = float; 25 | using Matrix4 = typename pcl::Registration::Matrix4; 26 | 27 | using PointCloudSource = typename pcl::Registration::PointCloudSource; 28 | using PointCloudSourcePtr = typename PointCloudSource::Ptr; 29 | using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr; 30 | 31 | using PointCloudTarget = typename pcl::Registration::PointCloudTarget; 32 | using PointCloudTargetPtr = typename PointCloudTarget::Ptr; 33 | using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr; 34 | 35 | protected: 36 | using pcl::Registration::reg_name_; 37 | using pcl::Registration::input_; 38 | using pcl::Registration::target_; 39 | using pcl::Registration::corr_dist_threshold_; 40 | 41 | public: 42 | NDTCuda(); 43 | virtual ~NDTCuda() override; 44 | 45 | void setDistanceMode(NDTDistanceMode mode); 46 | void setResolution(double resolution); 47 | void setNeighborSearchMethod(NeighborSearchMethod method, double radius = -1.0); 48 | 49 | virtual void swapSourceAndTarget() override; 50 | virtual void clearSource() override; 51 | virtual void clearTarget() override; 52 | 53 | virtual void setInputSource(const PointCloudSourceConstPtr& cloud) override; 54 | virtual void setInputTarget(const PointCloudTargetConstPtr& cloud) override; 55 | 56 | protected: 57 | virtual void computeTransformation(PointCloudSource& output, const Matrix4& guess) override; 58 | virtual double linearize(const Eigen::Isometry3d& trans, Eigen::Matrix* H, Eigen::Matrix* b) override; 59 | virtual double compute_error(const Eigen::Isometry3d& trans) override; 60 | 61 | protected: 62 | std::unique_ptr ndt_cuda_; 63 | }; 64 | } // namespace fast_gicp 65 | 66 | #endif -------------------------------------------------------------------------------- /lidar_mapping/include/fast_gicp/ndt/ndt_settings.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_NDT_SETTINGS_HPP 2 | #define FAST_GICP_NDT_SETTINGS_HPP 3 | 4 | namespace fast_gicp { 5 | 6 | enum class NDTDistanceMode { P2D, D2D }; 7 | 8 | } // namespace fast_gicp 9 | 10 | #endif -------------------------------------------------------------------------------- /lidar_mapping/include/fast_gicp/so3/so3.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_SO3_HPP 2 | #define FAST_GICP_SO3_HPP 3 | 4 | #include 5 | #include 6 | 7 | namespace fast_gicp { 8 | 9 | inline Eigen::Matrix3f skew(const Eigen::Vector3f& x) { 10 | Eigen::Matrix3f skew = Eigen::Matrix3f::Zero(); 11 | skew(0, 1) = -x[2]; 12 | skew(0, 2) = x[1]; 13 | skew(1, 0) = x[2]; 14 | skew(1, 2) = -x[0]; 15 | skew(2, 0) = -x[1]; 16 | skew(2, 1) = x[0]; 17 | 18 | return skew; 19 | } 20 | 21 | inline Eigen::Matrix3d skewd(const Eigen::Vector3d& x) { 22 | Eigen::Matrix3d skew = Eigen::Matrix3d::Zero(); 23 | skew(0, 1) = -x[2]; 24 | skew(0, 2) = x[1]; 25 | skew(1, 0) = x[2]; 26 | skew(1, 2) = -x[0]; 27 | skew(2, 0) = -x[1]; 28 | skew(2, 1) = x[0]; 29 | 30 | return skew; 31 | } 32 | 33 | /* 34 | * SO3 expmap code taken from Sophus 35 | * https://github.com/strasdat/Sophus/blob/593db47500ea1a2de5f0e6579c86147991509c59/sophus/so3.hpp#L585 36 | * 37 | * Copyright 2011-2017 Hauke Strasdat 38 | * 2012-2017 Steven Lovegrove 39 | * 40 | * Permission is hereby granted, free of charge, to any person obtaining a copy 41 | * of this software and associated documentation files (the "Software"), to 42 | * deal in the Software without restriction, including without limitation the 43 | * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 44 | * sell copies of the Software, and to permit persons to whom the Software is 45 | * furnished to do so, subject to the following conditions: 46 | * 47 | * The above copyright notice and this permission notice shall be included in 48 | * all copies or substantial portions of the Software. 49 | * 50 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 51 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 52 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 53 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 54 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 55 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 56 | * IN THE SOFTWARE. 57 | */ 58 | inline Eigen::Quaterniond so3_exp(const Eigen::Vector3d& omega) { 59 | double theta_sq = omega.dot(omega); 60 | 61 | double theta; 62 | double imag_factor; 63 | double real_factor; 64 | if(theta_sq < 1e-10) { 65 | theta = 0; 66 | double theta_quad = theta_sq * theta_sq; 67 | imag_factor = 0.5 - 1.0 / 48.0 * theta_sq + 1.0 / 3840.0 * theta_quad; 68 | real_factor = 1.0 - 1.0 / 8.0 * theta_sq + 1.0 / 384.0 * theta_quad; 69 | } else { 70 | theta = std::sqrt(theta_sq); 71 | double half_theta = 0.5 * theta; 72 | imag_factor = std::sin(half_theta) / theta; 73 | real_factor = std::cos(half_theta); 74 | } 75 | 76 | return Eigen::Quaterniond(real_factor, imag_factor * omega.x(), imag_factor * omega.y(), imag_factor * omega.z()); 77 | } 78 | 79 | } // namespace fast_gicp 80 | 81 | #endif -------------------------------------------------------------------------------- /lidar_mapping/include/global_mapping/util.h: -------------------------------------------------------------------------------- 1 | #ifndef _UTIL_H_ 2 | #define _UTIL_H_ 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | #include 27 | #ifndef PCL_NO_PRECOMPILE 28 | #include 29 | #include 30 | PCL_INSTANTIATE(Search, PCL_POINT_TYPES) 31 | #endif // PCL_NO_PRECOMPILE 32 | 33 | #include 34 | #include 35 | #include 36 | 37 | #include 38 | #include 39 | #include 40 | 41 | #include 42 | #include 43 | 44 | #include 45 | #include 46 | #include 47 | 48 | #include "global_mapping/Vector3.h" 49 | #include "global_mapping/Quaternion.h" 50 | #include "global_mapping/Pose6D.h" 51 | #include "scancontext/Scancontext.h" 52 | 53 | #include 54 | #include 55 | #include 56 | #include 57 | #include 58 | #include 59 | #include 60 | #include 61 | #include 62 | #include 63 | #include 64 | #include 65 | #include 66 | #include 67 | #include 68 | #include 69 | #include 70 | #include "time.h" 71 | 72 | using namespace std; 73 | using namespace alg; 74 | 75 | extern const string fix_frame_id = "/world"; 76 | extern const string odom_frame_id = "/global_odom"; 77 | extern const string base_frame_id = "/global_odom_high_freq"; 78 | 79 | extern const float map_octree_resolution = 0.05; 80 | extern const float map_octree_radius = 0.5; 81 | extern const float ref_octree_resolution = 0.3; 82 | 83 | extern const bool check_for_loop_closure = true; 84 | extern const float translation_threshold = 1.0; 85 | extern const int poses_before_reclosing = 10; 86 | float max_tolerable_fitness = 1.0; 87 | float proximity_threshold = 5; 88 | extern const int skip_recent_poses = 10; 89 | 90 | #endif 91 | -------------------------------------------------------------------------------- /lidar_mapping/launch/map.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /lidar_mapping/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | vils_mapping 4 | 0.0.0 5 | 6 | 7 | This is an advanced implentation of LOAM. 8 | LOAM is described in the following paper: 9 | J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. 10 | Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. 11 | 12 | 13 | qintong 14 | 15 | BSD 16 | 17 | Ji Zhang 18 | 19 | catkin 20 | geometry_msgs 21 | nav_msgs 22 | roscpp 23 | rospy 24 | std_msgs 25 | rosbag 26 | sensor_msgs 27 | tf 28 | image_transport 29 | 30 | geometry_msgs 31 | nav_msgs 32 | sensor_msgs 33 | roscpp 34 | rospy 35 | std_msgs 36 | rosbag 37 | tf 38 | image_transport 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /vils_estimator/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(vils_estimator) 3 | 4 | # add_definitions(-msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2) 5 | # set(CMAKE_C_FLAGS "-msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2") 6 | # set(CMAKE_CXX_FLAGS "-msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2") 7 | 8 | set(CMAKE_BUILD_TYPE "Release") 9 | set(CMAKE_CXX_FLAGS "-std=c++11") 10 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g -pthread ") 11 | 12 | find_package(catkin REQUIRED COMPONENTS 13 | roscpp 14 | std_msgs 15 | geometry_msgs 16 | nav_msgs 17 | tf 18 | cv_bridge 19 | ) 20 | 21 | find_package(OpenMP) 22 | if (OPENMP_FOUND) 23 | set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 24 | set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 25 | endif() 26 | 27 | find_package(OpenCV REQUIRED) 28 | 29 | # message(WARNING "OpenCV_VERSION: ${OpenCV_VERSION}") 30 | 31 | find_package(Ceres REQUIRED) 32 | 33 | find_package(PCL REQUIRED) 34 | 35 | 36 | include_directories(${catkin_INCLUDE_DIRS} ${CERES_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}) 37 | 38 | set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) 39 | find_package(Eigen3) 40 | include_directories( 41 | ${catkin_INCLUDE_DIRS} 42 | ${EIGEN3_INCLUDE_DIR} 43 | ) 44 | 45 | catkin_package() 46 | 47 | set(LIDAR_FUNCTIONS_DIR ${PROJECT_SOURCE_DIR}/src/lidar_functions) 48 | set(FAST_GICP_DIR ${LIDAR_FUNCTIONS_DIR}/fast_gicp) 49 | include_directories(${FAST_GICP_DIR}/include) 50 | 51 | 52 | add_executable(vils_estimator 53 | src/estimator_node.cpp 54 | src/parameters.cpp 55 | src/estimator.cpp 56 | src/feature_manager.cpp 57 | src/factor/pose_local_parameterization.cpp 58 | src/factor/projection_factor.cpp 59 | src/factor/projection_td_factor.cpp 60 | src/factor/marginalization_factor.cpp 61 | src/utility/utility.cpp 62 | src/utility/visualization.cpp 63 | src/utility/CameraPoseVisualization.cpp 64 | src/initial/solve_5pts.cpp 65 | src/initial/initial_aligment.cpp 66 | src/initial/initial_sfm.cpp 67 | src/initial/initial_ex_rotation.cpp 68 | src/lidar_frontend.cpp 69 | src/lidar_backend.cpp 70 | ) 71 | 72 | 73 | target_link_libraries(vils_estimator ${catkin_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES} ${PCL_LIBRARIES} 74 | ${FAST_GICP_DIR}/build/libfast_gicp.a) 75 | 76 | -------------------------------------------------------------------------------- /vils_estimator/launch/mynteye_leishen_indoor.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /vils_estimator/launch/mynteye_leishen_odom.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /vils_estimator/launch/mynteye_leishen_outdoor.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /vils_estimator/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | vils_estimator 4 | 0.0.0 5 | The vils_estimator package 6 | 7 | 8 | 9 | 10 | qintong 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | catkin 19 | roscpp 20 | pcl_conversions 21 | 22 | roscpp 23 | pcl_conversions 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /vils_estimator/src/estimator.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "parameters.h" 4 | #include "feature_manager.h" 5 | #include "utility/utility.h" 6 | #include "utility/tic_toc.h" 7 | #include "initial/solve_5pts.h" 8 | #include "initial/initial_sfm.h" 9 | #include "initial/initial_alignment.h" 10 | #include "initial/initial_ex_rotation.h" 11 | #include 12 | #include 13 | 14 | #include 15 | #include "factor/imu_factor.h" 16 | #include "factor/pose_local_parameterization.h" 17 | #include "factor/projection_factor.h" 18 | #include "factor/projection_td_factor.h" 19 | #include "factor/marginalization_factor.h" 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | #include "lidar_frontend.h" 26 | #include "lidar_backend.h" 27 | // 28 | class Estimator 29 | { 30 | public: 31 | // EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 32 | Estimator(); 33 | 34 | void setParameter(); 35 | 36 | // interface 37 | void processIMU(double t, const Vector3d &linear_acceleration, const Vector3d &angular_velocity); 38 | void processImage(const map > > > &image, const std_msgs::Header &header); 39 | void setReloFrame(double _frame_stamp, int _frame_index, vector &_match_points, Vector3d _relo_t, Matrix3d _relo_r); 40 | 41 | // internal 42 | void clearState(); 43 | bool initialStructure(); 44 | bool visualInitialAlign(); 45 | bool relativePose(Matrix3d &relative_R, Vector3d &relative_T, int &l); 46 | void slideWindow(); 47 | void solveOdometry(); 48 | void slideWindowNew(); 49 | void slideWindowOld(); 50 | void optimization(); 51 | void vector2double(); 52 | void double2vector(); 53 | bool failureDetection(); 54 | 55 | enum SolverFlag 56 | { 57 | INITIAL, 58 | NON_LINEAR 59 | }; 60 | 61 | enum MarginalizationFlag 62 | { 63 | MARGIN_OLD = 0, 64 | MARGIN_SECOND_NEW = 1 65 | }; 66 | 67 | SolverFlag solver_flag; 68 | MarginalizationFlag marginalization_flag; 69 | Vector3d g; 70 | MatrixXd Ap[2], backup_A; 71 | VectorXd bp[2], backup_b; 72 | 73 | Matrix3d ric[NUM_OF_CAM]; 74 | Vector3d tic[NUM_OF_CAM]; 75 | 76 | Vector3d Ps[(WINDOW_SIZE + 1)]; 77 | Vector3d Vs[(WINDOW_SIZE + 1)]; 78 | Matrix3d Rs[(WINDOW_SIZE + 1)]; 79 | Vector3d Bas[(WINDOW_SIZE + 1)]; 80 | Vector3d Bgs[(WINDOW_SIZE + 1)]; 81 | double td; 82 | 83 | Matrix3d back_R0, last_R, last_R0; 84 | Vector3d back_P0, last_P, last_P0; 85 | std_msgs::Header Headers[(WINDOW_SIZE + 1)]; 86 | 87 | IntegrationBase *pre_integrations[(WINDOW_SIZE + 1)]; 88 | Vector3d acc_0, gyr_0; 89 | 90 | vector dt_buf[(WINDOW_SIZE + 1)]; 91 | vector linear_acceleration_buf[(WINDOW_SIZE + 1)]; 92 | vector angular_velocity_buf[(WINDOW_SIZE + 1)]; 93 | 94 | int frame_count; 95 | int sum_of_outlier, sum_of_back, sum_of_front, sum_of_invalid; 96 | 97 | FeatureManager f_manager; 98 | MotionEstimator m_estimator; 99 | InitialEXRotation initial_ex_rotation; 100 | 101 | bool first_imu; 102 | bool is_valid, is_key; 103 | bool failure_occur; 104 | 105 | vector point_cloud; 106 | vector margin_cloud; 107 | vector key_poses; 108 | double initial_timestamp; 109 | 110 | double para_Pose[WINDOW_SIZE + 1][SIZE_POSE]; 111 | double para_SpeedBias[WINDOW_SIZE + 1][SIZE_SPEEDBIAS]; 112 | double para_Feature[NUM_OF_F][SIZE_FEATURE]; 113 | double para_Ex_Pose[NUM_OF_CAM][SIZE_POSE]; 114 | double para_Retrive_Pose[SIZE_POSE]; 115 | double para_Td[1][1]; 116 | double para_Tr[1][1]; 117 | 118 | int loop_window_index; 119 | 120 | MarginalizationInfo *last_marginalization_info; 121 | vector last_marginalization_parameter_blocks; 122 | 123 | map all_image_frame; 124 | IntegrationBase *tmp_pre_integration; 125 | 126 | //relocalization variable 127 | bool relocalization_info; 128 | double relo_frame_stamp; 129 | double relo_frame_index; 130 | int relo_frame_local_index; 131 | vector match_points; 132 | double relo_Pose[SIZE_POSE]; 133 | Matrix3d drift_correct_r; 134 | Vector3d drift_correct_t; 135 | Vector3d prev_relo_t; 136 | Matrix3d prev_relo_r; 137 | Vector3d relo_relative_t; 138 | Quaterniond relo_relative_q; 139 | double relo_relative_yaw; 140 | 141 | void processLidar(CloudData &cloud, const double time2); 142 | int lidar_count{0}; 143 | int lidar_count_{0}; 144 | LidarFrame current_lidar; 145 | map all_lidar_frame; 146 | Matrix3d RLB; 147 | Vector3d TLB, TBL; 148 | Matrix4d EX_LB{Matrix4d::Identity()}; 149 | LidarCalibration lidarCalibration; 150 | bool cov_estimate{false}; 151 | 152 | list LidarICPConstraints; 153 | list LidarLPSConstraints; 154 | 155 | double para_Ex_Pose_Lb[SIZE_POSE]; 156 | bool ESTIMATE_TLB; 157 | 158 | Vector3d LIDAR_INIT_t; 159 | Quaterniond LIDAR_INIT_R; 160 | 161 | Matrix3d tem_zv_r; 162 | Vector3d tem_zv_t; 163 | bool first_zv{true}; 164 | 165 | bool LPS_call{false}; 166 | Eigen::Quaterniond LPS_q; 167 | Eigen::Vector3d LPS_t; 168 | double LPS_time; 169 | }; -------------------------------------------------------------------------------- /vils_estimator/src/factor/marginalization_factor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include "../utility/utility.h" 11 | #include "../utility/tic_toc.h" 12 | 13 | const int NUM_THREADS = 4; 14 | 15 | struct ResidualBlockInfo 16 | { 17 | ResidualBlockInfo(ceres::CostFunction *_cost_function, ceres::LossFunction *_loss_function, std::vector _parameter_blocks, std::vector _drop_set) 18 | : cost_function(_cost_function), loss_function(_loss_function), parameter_blocks(_parameter_blocks), drop_set(_drop_set) {} 19 | 20 | void Evaluate(); 21 | 22 | ceres::CostFunction *cost_function; 23 | ceres::LossFunction *loss_function; 24 | std::vector parameter_blocks; 25 | std::vector drop_set; 26 | 27 | double **raw_jacobians; 28 | std::vector> jacobians; 29 | Eigen::VectorXd residuals; 30 | 31 | int localSize(int size) 32 | { 33 | return size == 7 ? 6 : size; 34 | } 35 | }; 36 | 37 | struct ThreadsStruct 38 | { 39 | std::vector sub_factors; 40 | Eigen::MatrixXd A; 41 | Eigen::VectorXd b; 42 | std::unordered_map parameter_block_size; //global size 43 | std::unordered_map parameter_block_idx; //local size 44 | }; 45 | 46 | class MarginalizationInfo 47 | { 48 | public: 49 | ~MarginalizationInfo(); 50 | int localSize(int size) const; 51 | int globalSize(int size) const; 52 | void addResidualBlockInfo(ResidualBlockInfo *residual_block_info); 53 | void preMarginalize(); 54 | void marginalize(); 55 | std::vector getParameterBlocks(std::unordered_map &addr_shift); 56 | 57 | std::vector factors; 58 | int m, n; 59 | std::unordered_map parameter_block_size; //global size 60 | int sum_block_size; 61 | std::unordered_map parameter_block_idx; //local size 62 | std::unordered_map parameter_block_data; 63 | 64 | std::vector keep_block_size; //global size 65 | std::vector keep_block_idx; //local size 66 | std::vector keep_block_data; 67 | 68 | Eigen::MatrixXd linearized_jacobians; 69 | Eigen::VectorXd linearized_residuals; 70 | const double eps = 1e-8; 71 | 72 | }; 73 | 74 | class MarginalizationFactor : public ceres::CostFunction 75 | { 76 | public: 77 | MarginalizationFactor(MarginalizationInfo* _marginalization_info); 78 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 79 | 80 | MarginalizationInfo* marginalization_info; 81 | }; 82 | -------------------------------------------------------------------------------- /vils_estimator/src/factor/pose_local_parameterization.cpp: -------------------------------------------------------------------------------- 1 | #include "pose_local_parameterization.h" 2 | 3 | bool PoseLocalParameterization::Plus(const double *x, const double *delta, double *x_plus_delta) const 4 | { 5 | Eigen::Map _p(x); 6 | Eigen::Map _q(x + 3); 7 | 8 | Eigen::Map dp(delta); 9 | 10 | Eigen::Quaterniond dq = Utility::deltaQ(Eigen::Map(delta + 3)); 11 | 12 | Eigen::Map p(x_plus_delta); 13 | Eigen::Map q(x_plus_delta + 3); 14 | 15 | p = _p + dp; 16 | q = (_q * dq).normalized(); 17 | 18 | return true; 19 | } 20 | bool PoseLocalParameterization::ComputeJacobian(const double *x, double *jacobian) const 21 | { 22 | Eigen::Map> j(jacobian); 23 | j.topRows<6>().setIdentity(); 24 | j.bottomRows<1>().setZero(); 25 | 26 | return true; 27 | } 28 | -------------------------------------------------------------------------------- /vils_estimator/src/factor/pose_local_parameterization.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include "../utility/utility.h" 6 | 7 | class PoseLocalParameterization : public ceres::LocalParameterization 8 | { 9 | virtual bool Plus(const double *x, const double *delta, double *x_plus_delta) const; 10 | virtual bool ComputeJacobian(const double *x, double *jacobian) const; 11 | virtual int GlobalSize() const { return 7; }; 12 | virtual int LocalSize() const { return 6; }; 13 | }; 14 | -------------------------------------------------------------------------------- /vils_estimator/src/factor/projection_factor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include "../utility/utility.h" 7 | #include "../utility/tic_toc.h" 8 | #include "../parameters.h" 9 | 10 | class ProjectionFactor : public ceres::SizedCostFunction<2, 7, 7, 7, 1> 11 | { 12 | public: 13 | ProjectionFactor(const Eigen::Vector3d &_pts_i, const Eigen::Vector3d &_pts_j); 14 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 15 | void check(double **parameters); 16 | 17 | Eigen::Vector3d pts_i, pts_j; 18 | Eigen::Matrix tangent_base; 19 | static Eigen::Matrix2d sqrt_info; 20 | static double sum_t; 21 | }; 22 | -------------------------------------------------------------------------------- /vils_estimator/src/factor/projection_td_factor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include "../utility/utility.h" 7 | #include "../utility/tic_toc.h" 8 | #include "../parameters.h" 9 | 10 | class ProjectionTdFactor : public ceres::SizedCostFunction<2, 7, 7, 7, 1, 1> 11 | { 12 | public: 13 | ProjectionTdFactor(const Eigen::Vector3d &_pts_i, const Eigen::Vector3d &_pts_j, 14 | const Eigen::Vector2d &_velocity_i, const Eigen::Vector2d &_velocity_j, 15 | const double _td_i, const double _td_j, const double _row_i, const double _row_j); 16 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 17 | void check(double **parameters); 18 | 19 | Eigen::Vector3d pts_i, pts_j; 20 | Eigen::Vector3d velocity_i, velocity_j; 21 | double td_i, td_j; 22 | Eigen::Matrix tangent_base; 23 | double row_i, row_j; 24 | static Eigen::Matrix2d sqrt_info; 25 | static double sum_t; 26 | }; 27 | -------------------------------------------------------------------------------- /vils_estimator/src/feature_manager.h: -------------------------------------------------------------------------------- 1 | #ifndef FEATURE_MANAGER_H 2 | #define FEATURE_MANAGER_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | using namespace std; 9 | 10 | #include 11 | using namespace Eigen; 12 | 13 | #include 14 | #include 15 | 16 | #include "parameters.h" 17 | 18 | class FeaturePerFrame 19 | { 20 | public: 21 | FeaturePerFrame(const Eigen::Matrix &_point, double td) 22 | { 23 | point.x() = _point(0); 24 | point.y() = _point(1); 25 | point.z() = _point(2); 26 | uv.x() = _point(3); 27 | uv.y() = _point(4); 28 | velocity.x() = _point(5); 29 | velocity.y() = _point(6); 30 | depth = _point(7); 31 | cur_td = td; 32 | } 33 | double cur_td; 34 | Vector3d point; 35 | Vector2d uv; 36 | Vector2d velocity; 37 | double z; 38 | bool is_used; 39 | double parallax; 40 | MatrixXd A; 41 | VectorXd b; 42 | double dep_gradient; 43 | double depth; // lidar depth, initialized with -1 from feature points in feature tracker node 44 | }; 45 | 46 | class FeaturePerId 47 | { 48 | public: 49 | const int feature_id; 50 | int start_frame; 51 | vector feature_per_frame; 52 | 53 | int used_num; 54 | bool is_outlier; 55 | bool is_margin; 56 | double estimated_depth; 57 | bool lidar_depth_flag; 58 | int solve_flag; // 0 haven't solve yet; 1 solve succ; 2 solve fail; 59 | 60 | Vector3d gt_p; 61 | 62 | FeaturePerId(int _feature_id, int _start_frame, double _measured_depth) 63 | : feature_id(_feature_id), start_frame(_start_frame), 64 | used_num(0), estimated_depth(-1.0), lidar_depth_flag(false), solve_flag(0) 65 | { 66 | if (_measured_depth > 0) 67 | { 68 | estimated_depth = _measured_depth; 69 | lidar_depth_flag = true; 70 | } 71 | else 72 | { 73 | estimated_depth = -1; 74 | lidar_depth_flag = false; 75 | } 76 | } 77 | 78 | int endFrame(); 79 | }; 80 | 81 | class FeatureManager 82 | { 83 | public: 84 | FeatureManager(Matrix3d _Rs[]); 85 | 86 | void setRic(Matrix3d _ric[]); 87 | 88 | void clearState(); 89 | 90 | int getFeatureCount(); 91 | 92 | bool addFeatureCheckParallax(int frame_count, const map>>> &image, double td); 93 | void debugShow(); 94 | vector> getCorresponding(int frame_count_l, int frame_count_r); 95 | 96 | //void updateDepth(const VectorXd &x); 97 | void setDepth(const VectorXd &x); 98 | void removeFailures(); 99 | void clearDepth(const VectorXd &x); 100 | VectorXd getDepthVector(); 101 | void triangulate(Vector3d Ps[], Vector3d tic[], Matrix3d ric[]); 102 | void removeBackShiftDepth(Eigen::Matrix3d marg_R, Eigen::Vector3d marg_P, Eigen::Matrix3d new_R, Eigen::Vector3d new_P); 103 | void removeBack(); 104 | void removeFront(int frame_count); 105 | void removeOutlier(); 106 | list feature; 107 | int last_track_num; 108 | 109 | private: 110 | double compensatedParallax2(const FeaturePerId &it_per_id, int frame_count); 111 | const Matrix3d *Rs; 112 | Matrix3d ric[NUM_OF_CAM]; 113 | }; 114 | 115 | #endif -------------------------------------------------------------------------------- /vils_estimator/src/initial/initial_ex_rotation.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include "../parameters.h" 5 | using namespace std; 6 | 7 | #include 8 | 9 | #include 10 | using namespace Eigen; 11 | #include 12 | 13 | /* This class help you to calibrate extrinsic rotation between imu and camera when your totally don't konw the extrinsic parameter */ 14 | class InitialEXRotation 15 | { 16 | public: 17 | InitialEXRotation(); 18 | void Reset(); 19 | bool CalibrationExRotation(vector > corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result); 20 | 21 | private: 22 | Matrix3d solveRelativeR(const vector > &corres); 23 | 24 | double testTriangulation(const vector &l, 25 | const vector &r, 26 | cv::Mat_ R, cv::Mat_ t); 27 | void decomposeE(cv::Mat E, 28 | cv::Mat_ &R1, cv::Mat_ &R2, 29 | cv::Mat_ &t1, cv::Mat_ &t2); 30 | int frame_count_; 31 | 32 | vector Rc; 33 | vector Rimu; 34 | vector Rc_g; 35 | Matrix3d ric; 36 | }; 37 | -------------------------------------------------------------------------------- /vils_estimator/src/initial/initial_sfm.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | using namespace Eigen; 12 | using namespace std; 13 | 14 | 15 | 16 | struct SFMFeature 17 | { 18 | bool state; 19 | int id; 20 | vector> observation; 21 | double position[3]; 22 | double depth; 23 | }; 24 | 25 | struct ReprojectionError3D 26 | { 27 | ReprojectionError3D(double observed_u, double observed_v) 28 | :observed_u(observed_u), observed_v(observed_v) 29 | {} 30 | 31 | template 32 | bool operator()(const T* const camera_R, const T* const camera_T, const T* point, T* residuals) const 33 | { 34 | T p[3]; 35 | ceres::QuaternionRotatePoint(camera_R, point, p); 36 | p[0] += camera_T[0]; p[1] += camera_T[1]; p[2] += camera_T[2]; 37 | T xp = p[0] / p[2]; 38 | T yp = p[1] / p[2]; 39 | residuals[0] = xp - T(observed_u); 40 | residuals[1] = yp - T(observed_v); 41 | return true; 42 | } 43 | 44 | static ceres::CostFunction* Create(const double observed_x, 45 | const double observed_y) 46 | { 47 | return (new ceres::AutoDiffCostFunction< 48 | ReprojectionError3D, 2, 4, 3, 3>( 49 | new ReprojectionError3D(observed_x,observed_y))); 50 | } 51 | 52 | double observed_u; 53 | double observed_v; 54 | }; 55 | 56 | class GlobalSFM 57 | { 58 | public: 59 | GlobalSFM(); 60 | bool construct(int frame_num, Quaterniond* q, Vector3d* T, int l, 61 | const Matrix3d relative_R, const Vector3d relative_T, 62 | vector &sfm_f, map &sfm_tracked_points); 63 | 64 | private: 65 | bool solveFrameByPnP(Matrix3d &R_initial, Vector3d &P_initial, int i, vector &sfm_f); 66 | 67 | void triangulatePoint(Eigen::Matrix &Pose0, Eigen::Matrix &Pose1, 68 | Vector2d &point0, Vector2d &point1, Vector3d &point_3d); 69 | void triangulateTwoFrames(int frame0, Eigen::Matrix &Pose0, 70 | int frame1, Eigen::Matrix &Pose1, 71 | vector &sfm_f); 72 | 73 | int feature_num; 74 | }; -------------------------------------------------------------------------------- /vils_estimator/src/initial/solve_5pts.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | using namespace std; 5 | 6 | #include 7 | //#include 8 | #include 9 | using namespace Eigen; 10 | 11 | #include 12 | 13 | class MotionEstimator 14 | { 15 | public: 16 | 17 | bool solveRelativeRT(const vector> &corres, Matrix3d &R, Vector3d &T); 18 | 19 | private: 20 | double testTriangulation(const vector &l, 21 | const vector &r, 22 | cv::Mat_ R, cv::Mat_ t); 23 | void decomposeE(cv::Mat E, 24 | cv::Mat_ &R1, cv::Mat_ &R2, 25 | cv::Mat_ &t1, cv::Mat_ &t2); 26 | }; 27 | 28 | 29 | -------------------------------------------------------------------------------- /vils_estimator/src/lidar_backend.cpp: -------------------------------------------------------------------------------- 1 | #include "lidar_backend.h" 2 | 3 | bool FindNearest2ID(const std_msgs::Header Headers[(WINDOW_SIZE + 1)], 4 | const double tl, 5 | int &id_a, int &id_b) 6 | { 7 | std::vector header; 8 | for (int i = 0; i < WINDOW_SIZE + 1; i++) 9 | { 10 | header.push_back(Headers[i].stamp.toSec()); 11 | } 12 | header.push_back(tl); 13 | std::sort(header.begin(), header.end(), less()); 14 | // std::vector::iterator it; 15 | auto it = std::find(header.begin(), header.end(), tl); 16 | if (it != header.end()) 17 | { 18 | // int index = std::distance(header.begin(),it); 19 | int index = it - header.begin(); 20 | id_a = index - 1; 21 | id_b = index; 22 | if (id_b > WINDOW_SIZE || id_a < 0) //|| (tl - header[id_a]) > 0.5 23 | { 24 | cout << "Not KEY LIDAR!!!" << endl; 25 | return false; 26 | } 27 | cout << "ID FIND!!! " << id_a << " " << id_b << endl; 28 | cout << "TIME CHECK!!! " << tl - header[id_a] << " " << header[id_b + 1] - tl << " " << endl; 29 | 30 | return true; 31 | } 32 | else 33 | { 34 | return false; 35 | } 36 | } 37 | 38 | bool FindWindowsID(const std_msgs::Header Headers[(WINDOW_SIZE + 1)], 39 | const double ta, const double tb, const double tc, const double td, 40 | int &id_a, int &id_b, int &id_c, int &id_d) 41 | { 42 | // cout << "Windows start from " << fixed << Headers[0].stamp.toSec() << " " 43 | // << "end at " << Headers[WINDOW_SIZE].stamp.toSec() << endl; 44 | // cout << "ta/b/c/d: " << fixed << ta << " " << tb << " " << tc << " " << td << endl; 45 | 46 | if ((Headers[0].stamp.toSec() > ta) || (Headers[WINDOW_SIZE].stamp.toSec() < td) || ((tb - ta) > 0.5)) 47 | { 48 | cout << "Not in windows!!" << endl; 49 | return false; 50 | } 51 | 52 | std::vector header; 53 | for (int i = 0; i < WINDOW_SIZE + 1; i++) 54 | { 55 | header.push_back(Headers[i].stamp.toSec()); 56 | } 57 | 58 | auto ita = std::find(header.begin(), header.end(), ta); 59 | auto itb = std::find(header.begin(), header.end(), tb); 60 | auto itc = std::find(header.begin(), header.end(), tc); 61 | auto itd = std::find(header.begin(), header.end(), td); 62 | 63 | if (ita != header.end()) 64 | { 65 | id_a = std::distance(header.begin(), ita); 66 | } 67 | if (itb != header.end()) 68 | { 69 | id_b = std::distance(header.begin(), itb); 70 | } 71 | if (itc != header.end()) 72 | { 73 | id_c = std::distance(header.begin(), itc); 74 | } 75 | if (itd != header.end()) 76 | { 77 | id_d = std::distance(header.begin(), itd); 78 | } 79 | if (id_b == id_c) 80 | { 81 | id_a--; 82 | id_b--; 83 | } 84 | 85 | if (id_b > id_a && id_d > id_c && id_a >= 0 && id_a != id_c) 86 | { 87 | return true; 88 | } 89 | else 90 | { 91 | return false; 92 | } 93 | } 94 | -------------------------------------------------------------------------------- /vils_estimator/src/lidar_functions/fast_gicp-master.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Stan994265/mVIL-Fusion/3dcb644e34f5c5908b46c78fb87505e9a800e803/vils_estimator/src/lidar_functions/fast_gicp-master.zip -------------------------------------------------------------------------------- /vils_estimator/src/lidar_functions/fast_gicp/.gitignore: -------------------------------------------------------------------------------- 1 | .vscode/* 2 | build/* 3 | README.html 4 | -------------------------------------------------------------------------------- /vils_estimator/src/lidar_functions/fast_gicp/.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "thidparty/Sophus"] 2 | path = thirdparty/Sophus 3 | url = https://github.com/strasdat/Sophus.git 4 | [submodule "thirdparty/Eigen"] 5 | path = thirdparty/Eigen 6 | url = https://github.com/eigenteam/eigen-git-mirror.git 7 | [submodule "thirdparty/nvbio"] 8 | path = thirdparty/nvbio 9 | url = https://github.com/NVlabs/nvbio.git 10 | -------------------------------------------------------------------------------- /vils_estimator/src/lidar_functions/fast_gicp/.travis.yml: -------------------------------------------------------------------------------- 1 | sudo: required 2 | language: generic 3 | dist: focal 4 | 5 | services: 6 | - docker 7 | 8 | matrix: 9 | include: 10 | - name: "Focal Fossa Nightly PCL" 11 | env: ROS_DISTRO=focal 12 | - name: "Focal Fossa Nightly PCL with CUDA" 13 | env: ROS_DISTRO=focal_cuda 14 | - name: "Focal Fossa noetic" 15 | env: ROS_DISTRO=noetic 16 | - name: "Focal Fossa noetic_llvm" 17 | env: ROS_DISTRO=noetic_llvm 18 | - name: "Bionic melodic_llvm" 19 | env: ROS_DISTRO=melodic_llvm 20 | - name: "Bionic melodic" 21 | env: ROS_DISTRO=melodic 22 | 23 | script: 24 | - echo "$DOCKER_PASSWORD" | docker login -u "$DOCKER_USERNAME" --password-stdin || true 25 | - docker build -f ./docker/$ROS_DISTRO/Dockerfile . 26 | 27 | env: 28 | global: 29 | - secure: "mVtiFoHyfQo+ty8T99uwADqBrU7Ex+Vn1U1a215RWEe5mZg1udvC7hl1lDkkGsDej9NW3vyZJqSyimq9OKqPb1Qe02bPP0FoOIA0IZzDAMEpWgQlUzwZGxrG4iTLv1O3gJJ+bgqBJyvy6RpH4KSXTuVRuuO38z4F4fkBR2gfqDiWh+cgx6xqBZnYewNW9ob3OqEw0tjxDtsyU4XLhK7u6Y4pIfkomH1jAQJxtNaMhMGqXTyasltqw3RP2LSbj7Lwv4IE//VDoi/rEQAqf3V8MIBzCsyX+je8zo30hy2fm1ioGqUDgdig45OTqD/G2UGuGFAV0SfVLFdCrDEdm7Wm3e2oWvFm1oB9dvq2zg4Re5xUwygi6ogqnEZnJJEOz3YYMGqjUnbTsysiyRfjwMm0PKDeQet+Fu02u09zdat3cy6QOsQz4CtPaz17jvSAyhqhnciG2TDpZb0CPuGHAlo7n+oe8KrcK68DDQbso3vNbSg+qxKaZXfJOR3jhAbT8nSVxX7yQ72FhWGPjJv4fjAXoSZiNx/pv2lI5GWUmL4fNkj70sh2KpC8mGjT8CpGVgSRhJGaYR5YJIDrEgkwe7VhOnYSg69WJ9ywK41ndOiHyyknS+K0yUqW4jelv2XdMJiTonfRZApGVHCv7PhS5NPKd4rkPItoZAOLk5DnsPapf+M=" 30 | - secure: "iKV5UtmQh1jxumupMe71mZ9uGlRNLgfj/V1h3SDKuMNbU2Wd6YUXX256VQmHSWGLCPJwP5z4ZsvUQTDO5pq9LnZ1Xa1EB02bDQzDjNvXNx5zWflDJz2Oq0PzRc7AY+PZv7Id85v1kO6zF0dFI6Xd9u/ztZK/HF6u+82+gRVgM/tdgEZ8lfhb7nK8Bz4G7jENr4buknY6iRU59/AAJH5dTBVDDopWCYP35dkycCSZXGYuascNh7Dwp8sKgIV/0932Qj+xL/+bI8d+4PnRaTBhvgp57oTqkp7GHRn36ZO82eigiv2AsAPWIe772b5H5l3FL9/qJjNE+SB6v67lNtSbMXcXsLQtSNqJHZXKAdMhVlZfW7uc1UiVFHq3KuZT4412oWb4Uw31SI5vj5cCGIQSHtH+TuYA9lVCzkZzQMB3pB0eBuMD+8Bh8jLosZ2OUM786KLBSAsmyuDTEJ6QBhx+3+s106vGWiCUKoCl55OAhN/YVCZ2ZrjFM4YdIYQLVEZ2Yq8shAqh1AlkX69KO3SOLE0vqd+MYisxs8JS0mSmSlfennFhw63A/B8qT6/deYgCbJ4qd5l7RHlaZyLgBBLmCDxrmtB1HSw3phIDxO1ezB/Noe4MXwwWD17hH24EiaJQ2c9RDhuMBYSrHxh1ZUFt2K71Ylbg2iv93G2ZnwBkJX8=" 31 | -------------------------------------------------------------------------------- /vils_estimator/src/lidar_functions/fast_gicp/include/fast_gicp/cuda/brute_force_knn.cuh: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_CUDA_BRUTE_FORCE_KNN_CUH 2 | #define FAST_GICP_CUDA_BRUTE_FORCE_KNN_CUH 3 | 4 | #include 5 | 6 | #include 7 | 8 | namespace fast_gicp { 9 | namespace cuda { 10 | 11 | void brute_force_knn_search(const thrust::device_vector& source, const thrust::device_vector& target, int k, thrust::device_vector>& k_neighbors, bool do_sort=false); 12 | 13 | } 14 | } // namespace fast_gicp 15 | 16 | 17 | #endif -------------------------------------------------------------------------------- /vils_estimator/src/lidar_functions/fast_gicp/include/fast_gicp/cuda/compute_derivatives.cuh: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_CUDA_COMPUTE_DERIVATIVES_CUH 2 | #define FAST_GICP_CUDA_COMPUTE_DERIVATIVES_CUH 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | namespace fast_gicp { 10 | namespace cuda { 11 | 12 | double compute_derivatives( 13 | const thrust::device_vector& src_points, 14 | const thrust::device_vector& src_covs, 15 | const GaussianVoxelMap& voxelmap, 16 | const thrust::device_vector>& voxel_correspondences, 17 | const thrust::device_ptr& linearized_x_ptr, 18 | const thrust::device_ptr& x_ptr, 19 | Eigen::Matrix* H, 20 | Eigen::Matrix* b); 21 | } 22 | } // namespace fast_gicp 23 | 24 | #endif -------------------------------------------------------------------------------- /vils_estimator/src/lidar_functions/fast_gicp/include/fast_gicp/cuda/compute_mahalanobis.cuh: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_CUDA_COMPUTE_MAHALANOBIS_CUH 2 | #define FAST_GICP_CUDA_COMPUTE_MAHALANOBIS_CUH 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | namespace fast_gicp { 11 | namespace cuda { 12 | 13 | void compute_mahalanobis( 14 | const thrust::device_vector& src_points, 15 | const thrust::device_vector& src_covs, 16 | const GaussianVoxelMap& voxelmap, 17 | const thrust::device_vector& voxel_correspondences, 18 | const Eigen::Isometry3f& linearized_x, 19 | thrust::device_vector& mahalanobis 20 | ); 21 | 22 | } 23 | } // namespace fast_gicp 24 | 25 | #endif -------------------------------------------------------------------------------- /vils_estimator/src/lidar_functions/fast_gicp/include/fast_gicp/cuda/covariance_estimation.cuh: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_CUDA_COVARIANCE_ESTIMATION_CUH 2 | #define FAST_GICP_CUDA_COVARIANCE_ESTIMATION_CUH 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace fast_gicp { 9 | namespace cuda { 10 | 11 | void covariance_estimation(const thrust::device_vector& points, int k, const thrust::device_vector& k_neighbors, thrust::device_vector& covariances); 12 | 13 | void covariance_estimation_rbf(const thrust::device_vector& points, double kernel_width, double max_dist, thrust::device_vector& covariances); 14 | } 15 | } // namespace fast_gicp 16 | 17 | #endif -------------------------------------------------------------------------------- /vils_estimator/src/lidar_functions/fast_gicp/include/fast_gicp/cuda/covariance_regularization.cuh: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_CUDA_COVARIANCE_REGULARIZATION_CUH 2 | #define FAST_GICP_CUDA_COVARIANCE_REGULARIZATION_CUH 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace fast_gicp { 9 | namespace cuda { 10 | 11 | void covariance_regularization(thrust::device_vector& means, thrust::device_vector& covs, RegularizationMethod method); 12 | 13 | } // namespace cuda 14 | } // namespace fast_gicp 15 | 16 | #endif -------------------------------------------------------------------------------- /vils_estimator/src/lidar_functions/fast_gicp/include/fast_gicp/cuda/fast_vgicp_cuda.cuh: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_FAST_VGICP_CUDA_CORE_CUH 2 | #define FAST_GICP_FAST_VGICP_CUDA_CORE_CUH 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | namespace thrust { 12 | 13 | template 14 | class pair; 15 | 16 | template 17 | class device_allocator; 18 | 19 | template 20 | class device_vector; 21 | } // namespace thrust 22 | 23 | namespace fast_gicp { 24 | namespace cuda { 25 | 26 | class GaussianVoxelMap; 27 | 28 | class FastVGICPCudaCore { 29 | public: 30 | using Points = thrust::device_vector>; 31 | using Indices = thrust::device_vector>; 32 | using Matrices = thrust::device_vector>; 33 | using Correspondences = thrust::device_vector, thrust::device_allocator>>; 34 | using VoxelCoordinates = thrust::device_vector>; 35 | 36 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 37 | FastVGICPCudaCore(); 38 | ~FastVGICPCudaCore(); 39 | 40 | void set_resolution(double resolution); 41 | void set_kernel_params(double kernel_width, double kernel_max_dist); 42 | void set_neighbor_search_method(fast_gicp::NeighborSearchMethod method, double radius); 43 | 44 | void swap_source_and_target(); 45 | void set_source_cloud(const std::vector>& cloud); 46 | void set_target_cloud(const std::vector>& cloud); 47 | 48 | void set_source_neighbors(int k, const std::vector& neighbors); 49 | void set_target_neighbors(int k, const std::vector& neighbors); 50 | void find_source_neighbors(int k); 51 | void find_target_neighbors(int k); 52 | 53 | void calculate_source_covariances(RegularizationMethod method); 54 | void calculate_target_covariances(RegularizationMethod method); 55 | 56 | void calculate_source_covariances_rbf(RegularizationMethod method); 57 | void calculate_target_covariances_rbf(RegularizationMethod method); 58 | 59 | void get_source_covariances(std::vector>& covs) const; 60 | void get_target_covariances(std::vector>& covs) const; 61 | 62 | void get_voxel_num_points(std::vector& num_points) const; 63 | void get_voxel_means(std::vector>& means) const; 64 | void get_voxel_covs(std::vector>& covs) const; 65 | void get_voxel_correspondences(std::vector>& correspondences) const; 66 | 67 | void create_target_voxelmap(); 68 | 69 | void update_correspondences(const Eigen::Isometry3d& trans); 70 | 71 | double compute_error(const Eigen::Isometry3d& trans, Eigen::Matrix* H, Eigen::Matrix* b) const; 72 | 73 | public: 74 | double resolution; 75 | double kernel_width; 76 | double kernel_max_dist; 77 | std::unique_ptr offsets; 78 | 79 | std::unique_ptr source_points; 80 | std::unique_ptr target_points; 81 | 82 | std::unique_ptr source_neighbors; 83 | std::unique_ptr target_neighbors; 84 | 85 | std::unique_ptr source_covariances; 86 | std::unique_ptr target_covariances; 87 | 88 | std::unique_ptr voxelmap; 89 | 90 | Eigen::Isometry3f linearized_x; 91 | std::unique_ptr voxel_correspondences; 92 | }; 93 | 94 | } // namespace cuda 95 | } // namespace fast_gicp 96 | 97 | #endif -------------------------------------------------------------------------------- /vils_estimator/src/lidar_functions/fast_gicp/include/fast_gicp/cuda/find_voxel_correspondences.cuh: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_CUDA_FIND_VOXEL_CORRESPONDENCES_CUH 2 | #define FAST_GICP_CUDA_FIND_VOXEL_CORRESPONDENCES_CUH 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | namespace fast_gicp { 12 | namespace cuda { 13 | 14 | void find_voxel_correspondences( 15 | const thrust::device_vector& src_points, 16 | const GaussianVoxelMap& voxelmap, 17 | const thrust::device_ptr& x_ptr, 18 | const thrust::device_vector& offsets, 19 | thrust::device_vector>& correspondences) ; 20 | 21 | } // namespace cuda 22 | } // namespace fast_gicp 23 | 24 | #endif -------------------------------------------------------------------------------- /vils_estimator/src/lidar_functions/fast_gicp/include/fast_gicp/cuda/gaussian_voxelmap.cuh: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_CUDA_GAUSSIAN_VOXELMAP_CUH 2 | #define FAST_GICP_CUDA_GAUSSIAN_VOXELMAP_CUH 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace fast_gicp { 9 | namespace cuda { 10 | 11 | struct VoxelMapInfo { 12 | int num_voxels; 13 | int num_buckets; 14 | int max_bucket_scan_count; 15 | float voxel_resolution; 16 | }; 17 | 18 | class GaussianVoxelMap { 19 | public: 20 | GaussianVoxelMap(float resolution, int init_num_buckets = 8192, int max_bucket_scan_count = 10); 21 | 22 | void create_voxelmap(const thrust::device_vector& points); 23 | void create_voxelmap(const thrust::device_vector& points, const thrust::device_vector& covariances); 24 | 25 | private: 26 | void create_bucket_table(cudaStream_t stream, const thrust::device_vector& points); 27 | 28 | public: 29 | const int init_num_buckets; 30 | VoxelMapInfo voxelmap_info; 31 | thrust::device_vector voxelmap_info_ptr; 32 | 33 | thrust::device_vector> buckets; 34 | 35 | // voxel data 36 | thrust::device_vector num_points; 37 | thrust::device_vector voxel_means; 38 | thrust::device_vector voxel_covs; 39 | }; 40 | 41 | } // namespace cuda 42 | } // namespace fast_gicp 43 | 44 | #endif -------------------------------------------------------------------------------- /vils_estimator/src/lidar_functions/fast_gicp/include/fast_gicp/cuda/ndt_compute_derivatives.cuh: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_CUDA_NDT_COMPUTE_DERIVATIVES_CUH 2 | #define FAST_GICP_CUDA_NDT_COMPUTE_DERIVATIVES_CUH 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | namespace fast_gicp { 11 | namespace cuda { 12 | 13 | double p2d_ndt_compute_derivatives( 14 | const GaussianVoxelMap& target_voxelmap, 15 | const thrust::device_vector& source_points, 16 | const thrust::device_vector>& correspondences, 17 | const thrust::device_ptr& linearized_x_ptr, 18 | const thrust::device_ptr& x_ptr, 19 | Eigen::Matrix* H, 20 | Eigen::Matrix* b); 21 | 22 | double d2d_ndt_compute_derivatives( 23 | const GaussianVoxelMap& target_voxelmap, 24 | const GaussianVoxelMap& source_voxelmap, 25 | const thrust::device_vector>& correspondences, 26 | const thrust::device_ptr& linearized_x_ptr, 27 | const thrust::device_ptr& x_ptr, 28 | Eigen::Matrix* H, 29 | Eigen::Matrix* b); 30 | 31 | } // namespace cuda 32 | } // namespace fast_gicp 33 | 34 | #endif -------------------------------------------------------------------------------- /vils_estimator/src/lidar_functions/fast_gicp/include/fast_gicp/cuda/ndt_cuda.cuh: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_NDT_CUDA_CUH 2 | #define FAST_GICP_NDT_CUDA_CUH 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | namespace thrust { 13 | template 14 | class pair; 15 | 16 | template 17 | class device_allocator; 18 | 19 | template 20 | class device_vector; 21 | } // namespace thrust 22 | 23 | namespace fast_gicp { 24 | namespace cuda { 25 | 26 | class GaussianVoxelMap; 27 | 28 | class NDTCudaCore { 29 | public: 30 | using Points = thrust::device_vector>; 31 | using Indices = thrust::device_vector>; 32 | using Matrices = thrust::device_vector>; 33 | using Correspondences = thrust::device_vector, thrust::device_allocator>>; 34 | using VoxelCoordinates = thrust::device_vector>; 35 | 36 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 37 | NDTCudaCore(); 38 | ~NDTCudaCore(); 39 | 40 | void set_distance_mode(fast_gicp::NDTDistanceMode mode); 41 | void set_resolution(double resolution); 42 | void set_neighbor_search_method(fast_gicp::NeighborSearchMethod method, double radius); 43 | 44 | void swap_source_and_target(); 45 | void set_source_cloud(const std::vector>& cloud); 46 | void set_target_cloud(const std::vector>& cloud); 47 | 48 | void create_voxelmaps(); 49 | void create_target_voxelmap(); 50 | void create_source_voxelmap(); 51 | 52 | void update_correspondences(const Eigen::Isometry3d& trans); 53 | double compute_error(const Eigen::Isometry3d& trans, Eigen::Matrix* H, Eigen::Matrix* b) const; 54 | 55 | public: 56 | fast_gicp::NDTDistanceMode distance_mode; 57 | double resolution; 58 | std::unique_ptr offsets; 59 | 60 | std::unique_ptr source_points; 61 | std::unique_ptr target_points; 62 | 63 | std::unique_ptr source_voxelmap; 64 | std::unique_ptr target_voxelmap; 65 | 66 | Eigen::Isometry3f linearized_x; 67 | std::unique_ptr correspondences; 68 | }; 69 | 70 | } // namespace cuda 71 | } // namespace fast_gicp 72 | 73 | #endif -------------------------------------------------------------------------------- /vils_estimator/src/lidar_functions/fast_gicp/include/fast_gicp/cuda/vector3_hash.cuh: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_CUDA_VECTOR3_HASH_CUH 2 | #define FAST_GICP_CUDA_VECTOR3_HASH_CUH 3 | 4 | namespace fast_gicp { 5 | namespace cuda { 6 | 7 | // taken from boost/hash.hpp 8 | __host__ __device__ inline void hash_combine(uint64_t& h, uint64_t k) { 9 | const uint64_t m = UINT64_C(0xc6a4a7935bd1e995); 10 | const int r = 47; 11 | 12 | k *= m; 13 | k ^= k >> r; 14 | k *= m; 15 | 16 | h ^= k; 17 | h *= m; 18 | 19 | h += 0xe6546b64; 20 | } 21 | 22 | inline __host__ __device__ bool equal(const Eigen::Vector3i& lhs, const Eigen::Vector3i& rhs) { 23 | return lhs[0] == rhs[0] && lhs[1] == rhs[1] && lhs[2] == rhs[2]; 24 | } 25 | 26 | // compute vector3i hash 27 | __host__ __device__ inline uint64_t vector3i_hash(const Eigen::Vector3i& x) { 28 | uint64_t seed = 0; 29 | hash_combine(seed, x[0]); 30 | hash_combine(seed, x[1]); 31 | hash_combine(seed, x[2]); 32 | return seed; 33 | } 34 | 35 | __host__ __device__ inline Eigen::Vector3i calc_voxel_coord(const Eigen::Vector3f& x, float resolution) { 36 | Eigen::Vector3i coord = (x.array() / resolution - 0.5).floor().cast(); 37 | return coord; 38 | } 39 | 40 | } 41 | } // namespace fast_gicp 42 | 43 | #endif -------------------------------------------------------------------------------- /vils_estimator/src/lidar_functions/fast_gicp/include/fast_gicp/gicp/experimental/fast_gicp_mp.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_FAST_GICP_MP_HPP 2 | #define FAST_GICP_FAST_GICP_MP_HPP 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | namespace fast_gicp { 14 | 15 | template 16 | class FastGICPMultiPoints : public pcl::Registration { 17 | public: 18 | using Scalar = float; 19 | using Matrix4 = typename pcl::Registration::Matrix4; 20 | 21 | using PointCloudSource = typename pcl::Registration::PointCloudSource; 22 | using PointCloudSourcePtr = typename PointCloudSource::Ptr; 23 | using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr; 24 | 25 | using PointCloudTarget = typename pcl::Registration::PointCloudTarget; 26 | using PointCloudTargetPtr = typename PointCloudTarget::Ptr; 27 | using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr; 28 | 29 | using pcl::Registration::reg_name_; 30 | using pcl::Registration::input_; 31 | using pcl::Registration::target_; 32 | 33 | using pcl::Registration::nr_iterations_; 34 | using pcl::Registration::max_iterations_; 35 | using pcl::Registration::final_transformation_; 36 | using pcl::Registration::transformation_epsilon_; 37 | using pcl::Registration::converged_; 38 | using pcl::Registration::corr_dist_threshold_; 39 | 40 | FastGICPMultiPoints(); 41 | virtual ~FastGICPMultiPoints() override; 42 | 43 | void setNumThreads(int n); 44 | 45 | void setRotationEpsilon(double eps); 46 | 47 | void setCorrespondenceRandomness(int k); 48 | 49 | void setRegularizationMethod(RegularizationMethod method); 50 | 51 | virtual void setInputSource(const PointCloudSourceConstPtr& cloud) override; 52 | 53 | virtual void setInputTarget(const PointCloudTargetConstPtr& cloud) override; 54 | 55 | protected: 56 | virtual void computeTransformation(PointCloudSource& output, const Matrix4& guess) override; 57 | 58 | private: 59 | bool is_converged(const Eigen::Matrix& delta) const; 60 | 61 | void update_correspondences(const Eigen::Matrix& x); 62 | 63 | Eigen::VectorXf loss_ls(const Eigen::Matrix& x, Eigen::MatrixXf* J) const; 64 | 65 | template 66 | bool calculate_covariances(const boost::shared_ptr>& cloud, pcl::search::KdTree& kdtree, std::vector>& covariances); 67 | 68 | private: 69 | int num_threads_; 70 | int k_correspondences_; 71 | double rotation_epsilon_; 72 | double neighbor_search_radius_; 73 | 74 | RegularizationMethod regularization_method_; 75 | 76 | pcl::search::KdTree source_kdtree; 77 | pcl::search::KdTree target_kdtree; 78 | 79 | std::vector> source_covs; 80 | std::vector> target_covs; 81 | 82 | std::vector> correspondences; 83 | std::vector> sq_distances; 84 | }; 85 | } // namespace fast_gicp 86 | 87 | #endif 88 | -------------------------------------------------------------------------------- /vils_estimator/src/lidar_functions/fast_gicp/include/fast_gicp/gicp/fast_gicp.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_FAST_GICP_HPP 2 | #define FAST_GICP_FAST_GICP_HPP 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | namespace fast_gicp { 15 | 16 | /** 17 | * @brief Fast GICP algorithm optimized for multi threading with OpenMP 18 | */ 19 | template 20 | class FastGICP : public LsqRegistration { 21 | public: 22 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 23 | using Scalar = float; 24 | using Matrix4 = typename pcl::Registration::Matrix4; 25 | 26 | using PointCloudSource = typename pcl::Registration::PointCloudSource; 27 | using PointCloudSourcePtr = typename PointCloudSource::Ptr; 28 | using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr; 29 | 30 | using PointCloudTarget = typename pcl::Registration::PointCloudTarget; 31 | using PointCloudTargetPtr = typename PointCloudTarget::Ptr; 32 | using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr; 33 | 34 | protected: 35 | using pcl::Registration::reg_name_; 36 | using pcl::Registration::input_; 37 | using pcl::Registration::target_; 38 | using pcl::Registration::corr_dist_threshold_; 39 | 40 | public: 41 | FastGICP(); 42 | virtual ~FastGICP() override; 43 | 44 | void setNumThreads(int n); 45 | void setCorrespondenceRandomness(int k); 46 | void setRegularizationMethod(RegularizationMethod method); 47 | 48 | virtual void swapSourceAndTarget() override; 49 | virtual void clearSource() override; 50 | virtual void clearTarget() override; 51 | 52 | virtual void setInputSource(const PointCloudSourceConstPtr& cloud) override; 53 | virtual void setSourceCovariances(const std::vector>& covs); 54 | virtual void setInputTarget(const PointCloudTargetConstPtr& cloud) override; 55 | virtual void setTargetCovariances(const std::vector>& covs); 56 | 57 | const std::vector>& getSourceCovariances() const { 58 | return source_covs_; 59 | } 60 | 61 | const std::vector>& getTargetCovariances() const { 62 | return target_covs_; 63 | } 64 | 65 | protected: 66 | virtual void computeTransformation(PointCloudSource& output, const Matrix4& guess) override; 67 | 68 | virtual void update_correspondences(const Eigen::Isometry3d& trans); 69 | 70 | virtual double linearize(const Eigen::Isometry3d& trans, Eigen::Matrix* H, Eigen::Matrix* b) override; 71 | //virtual double linearize(const Eigen::Isometry3d& trans, Eigen::MatrixXd* H, Eigen::MatrixXd* b) override; 72 | 73 | virtual double compute_error(const Eigen::Isometry3d& trans) override; 74 | 75 | template 76 | bool calculate_covariances(const typename pcl::PointCloud::ConstPtr& cloud, pcl::search::KdTree& kdtree, std::vector>& covariances); 77 | 78 | protected: 79 | int num_threads_; 80 | int k_correspondences_; 81 | 82 | RegularizationMethod regularization_method_; 83 | 84 | std::shared_ptr> source_kdtree_; 85 | std::shared_ptr> target_kdtree_; 86 | 87 | std::vector> source_covs_; 88 | std::vector> target_covs_; 89 | 90 | std::vector> mahalanobis_; 91 | 92 | std::vector correspondences_; 93 | std::vector sq_distances_; 94 | }; 95 | } // namespace fast_gicp 96 | 97 | #endif 98 | -------------------------------------------------------------------------------- /vils_estimator/src/lidar_functions/fast_gicp/include/fast_gicp/gicp/fast_gicp_st.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_FAST_GICP_ST_HPP 2 | #define FAST_GICP_FAST_GICP_ST_HPP 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | namespace fast_gicp { 15 | 16 | /** 17 | * @brief Fast GICP algorithm optimized for single threading 18 | */ 19 | template 20 | class FastGICPSingleThread : public FastGICP { 21 | public: 22 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 23 | using Scalar = float; 24 | using Matrix4 = typename pcl::Registration::Matrix4; 25 | 26 | using PointCloudSource = typename pcl::Registration::PointCloudSource; 27 | using PointCloudSourcePtr = typename PointCloudSource::Ptr; 28 | using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr; 29 | 30 | protected: 31 | using pcl::Registration::input_; 32 | using pcl::Registration::target_; 33 | 34 | using FastGICP::target_kdtree_; 35 | using FastGICP::correspondences_; 36 | using FastGICP::sq_distances_; 37 | using FastGICP::source_covs_; 38 | using FastGICP::target_covs_; 39 | using FastGICP::mahalanobis_; 40 | 41 | public: 42 | FastGICPSingleThread(); 43 | virtual ~FastGICPSingleThread() override; 44 | 45 | protected: 46 | virtual void computeTransformation(PointCloudSource& output, const Matrix4& guess) override; 47 | 48 | private: 49 | virtual void update_correspondences(const Eigen::Isometry3d& trans) override; 50 | 51 | virtual double linearize(const Eigen::Isometry3d& trans, Eigen::Matrix* H = nullptr, Eigen::Matrix* b = nullptr) override; 52 | 53 | virtual double compute_error(const Eigen::Isometry3d& trans) override; 54 | 55 | private: 56 | std::vector second_sq_distances_; 57 | std::vector> anchors_; 58 | }; 59 | } // namespace fast_gicp 60 | 61 | #endif 62 | -------------------------------------------------------------------------------- /vils_estimator/src/lidar_functions/fast_gicp/include/fast_gicp/gicp/fast_vgicp.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_FAST_VGICP_HPP 2 | #define FAST_GICP_FAST_VGICP_HPP 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | #include 17 | 18 | namespace fast_gicp { 19 | 20 | /** 21 | * @brief Fast Voxelized GICP algorithm boosted with OpenMP 22 | */ 23 | template 24 | class FastVGICP : public FastGICP { 25 | public: 26 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 27 | using Scalar = float; 28 | using Matrix4 = typename pcl::Registration::Matrix4; 29 | 30 | using PointCloudSource = typename pcl::Registration::PointCloudSource; 31 | using PointCloudSourcePtr = typename PointCloudSource::Ptr; 32 | using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr; 33 | 34 | using PointCloudTarget = typename pcl::Registration::PointCloudTarget; 35 | using PointCloudTargetPtr = typename PointCloudTarget::Ptr; 36 | using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr; 37 | 38 | protected: 39 | using pcl::Registration::input_; 40 | using pcl::Registration::target_; 41 | 42 | using FastGICP::num_threads_; 43 | using FastGICP::source_kdtree_; 44 | using FastGICP::target_kdtree_; 45 | using FastGICP::source_covs_; 46 | using FastGICP::target_covs_; 47 | 48 | public: 49 | FastVGICP(); 50 | virtual ~FastVGICP() override; 51 | 52 | void setResolution(double resolution); 53 | void setVoxelAccumulationMode(VoxelAccumulationMode mode); 54 | void setNeighborSearchMethod(NeighborSearchMethod method); 55 | 56 | virtual void swapSourceAndTarget() override; 57 | virtual void setInputTarget(const PointCloudTargetConstPtr& cloud) override; 58 | 59 | protected: 60 | virtual void computeTransformation(PointCloudSource& output, const Matrix4& guess) override; 61 | virtual void update_correspondences(const Eigen::Isometry3d& trans) override; 62 | virtual double linearize(const Eigen::Isometry3d& trans, Eigen::Matrix* H = nullptr, Eigen::Matrix* b = nullptr) override; 63 | virtual double compute_error(const Eigen::Isometry3d& trans) override; 64 | 65 | protected: 66 | double voxel_resolution_; 67 | NeighborSearchMethod search_method_; 68 | VoxelAccumulationMode voxel_mode_; 69 | 70 | std::unique_ptr> voxelmap_; 71 | 72 | std::vector> voxel_correspondences_; 73 | std::vector> voxel_mahalanobis_; 74 | }; 75 | } // namespace fast_gicp 76 | 77 | #endif 78 | -------------------------------------------------------------------------------- /vils_estimator/src/lidar_functions/fast_gicp/include/fast_gicp/gicp/fast_vgicp_cuda.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_FAST_VGICP_CUDA_HPP 2 | #define FAST_GICP_FAST_VGICP_CUDA_HPP 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | 15 | namespace fast_gicp { 16 | 17 | namespace cuda { 18 | class FastVGICPCudaCore; 19 | } 20 | 21 | enum class NearestNeighborMethod { CPU_PARALLEL_KDTREE, GPU_BRUTEFORCE, GPU_RBF_KERNEL }; 22 | 23 | /** 24 | * @brief Fast Voxelized GICP algorithm boosted with CUDA 25 | */ 26 | template 27 | class FastVGICPCuda : public LsqRegistration { 28 | public: 29 | using Scalar = float; 30 | using Matrix4 = typename pcl::Registration::Matrix4; 31 | 32 | using PointCloudSource = typename pcl::Registration::PointCloudSource; 33 | using PointCloudSourcePtr = typename PointCloudSource::Ptr; 34 | using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr; 35 | 36 | using PointCloudTarget = typename pcl::Registration::PointCloudTarget; 37 | using PointCloudTargetPtr = typename PointCloudTarget::Ptr; 38 | using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr; 39 | 40 | protected: 41 | using pcl::Registration::input_; 42 | using pcl::Registration::target_; 43 | 44 | public: 45 | FastVGICPCuda(); 46 | virtual ~FastVGICPCuda() override; 47 | 48 | void setCorrespondenceRandomness(int k); 49 | void setResolution(double resolution); 50 | void setKernelWidth(double kernel_width, double max_dist = -1.0); 51 | void setRegularizationMethod(RegularizationMethod method); 52 | void setNeighborSearchMethod(NeighborSearchMethod method, double radius = -1.0); 53 | void setNearestNeighborSearchMethod(NearestNeighborMethod method); 54 | 55 | virtual void swapSourceAndTarget() override; 56 | virtual void clearSource() override; 57 | virtual void clearTarget() override; 58 | 59 | virtual void setInputSource(const PointCloudSourceConstPtr& cloud) override; 60 | virtual void setInputTarget(const PointCloudTargetConstPtr& cloud) override; 61 | 62 | protected: 63 | virtual void computeTransformation(PointCloudSource& output, const Matrix4& guess) override; 64 | virtual double linearize(const Eigen::Isometry3d& trans, Eigen::Matrix* H = nullptr, Eigen::Matrix* b = nullptr) override; 65 | virtual double compute_error(const Eigen::Isometry3d& trans) override; 66 | 67 | template 68 | std::vector find_neighbors_parallel_kdtree(int k, typename pcl::PointCloud::ConstPtr cloud) const; 69 | 70 | private: 71 | int k_correspondences_; 72 | double voxel_resolution_; 73 | RegularizationMethod regularization_method_; 74 | NearestNeighborMethod neighbor_search_method_; 75 | 76 | std::unique_ptr vgicp_cuda_; 77 | }; 78 | 79 | } // namespace fast_gicp 80 | 81 | #endif 82 | -------------------------------------------------------------------------------- /vils_estimator/src/lidar_functions/fast_gicp/include/fast_gicp/gicp/gicp_settings.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_GICP_SETTINGS_HPP 2 | #define FAST_GICP_GICP_SETTINGS_HPP 3 | 4 | namespace fast_gicp { 5 | 6 | enum class RegularizationMethod { NONE, MIN_EIG, NORMALIZED_MIN_EIG, PLANE, FROBENIUS }; 7 | 8 | enum class NeighborSearchMethod { DIRECT27, DIRECT7, DIRECT1, /* supported on only VGICP_CUDA */ DIRECT_RADIUS }; 9 | 10 | enum class VoxelAccumulationMode { ADDITIVE, ADDITIVE_WEIGHTED, MULTIPLICATIVE }; 11 | } 12 | 13 | #endif -------------------------------------------------------------------------------- /vils_estimator/src/lidar_functions/fast_gicp/include/fast_gicp/gicp/impl/fast_gicp_st_impl.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_FAST_GICP_ST_IMPL_HPP 2 | #define FAST_GICP_FAST_GICP_ST_IMPL_HPP 3 | 4 | #include 5 | #include 6 | 7 | namespace fast_gicp { 8 | 9 | template 10 | FastGICPSingleThread::FastGICPSingleThread() : FastGICP() { 11 | this->reg_name_ = "FastGICPSingleThread"; 12 | this->num_threads_ = 1; 13 | } 14 | 15 | template 16 | FastGICPSingleThread::~FastGICPSingleThread() {} 17 | 18 | template 19 | void FastGICPSingleThread::computeTransformation(PointCloudSource& output, const Matrix4& guess) { 20 | anchors_.clear(); 21 | FastGICP::computeTransformation(output, guess); 22 | } 23 | 24 | template 25 | void FastGICPSingleThread::update_correspondences(const Eigen::Isometry3d& x) { 26 | assert(source_covs_.size() == input_->size()); 27 | assert(target_covs_.size() == target_->size()); 28 | 29 | Eigen::Isometry3f trans = x.template cast(); 30 | 31 | bool is_first = anchors_.empty(); 32 | 33 | correspondences_.resize(input_->size()); 34 | sq_distances_.resize(input_->size()); 35 | second_sq_distances_.resize(input_->size()); 36 | anchors_.resize(input_->size()); 37 | mahalanobis_.resize(input_->size()); 38 | 39 | std::vector k_indices; 40 | std::vector k_sq_dists; 41 | 42 | for (int i = 0; i < input_->size(); i++) { 43 | PointTarget pt; 44 | pt.getVector4fMap() = trans * input_->at(i).getVector4fMap(); 45 | 46 | if (!is_first) { 47 | double d = (pt.getVector4fMap() - anchors_[i]).norm(); 48 | double max_first = std::sqrt(sq_distances_[i]) + d; 49 | double min_second = std::sqrt(second_sq_distances_[i]) - d; 50 | 51 | if (max_first < min_second) { 52 | continue; 53 | } 54 | } 55 | 56 | target_kdtree_->nearestKSearch(pt, 2, k_indices, k_sq_dists); 57 | 58 | correspondences_[i] = k_sq_dists[0] < this->corr_dist_threshold_ * this->corr_dist_threshold_ ? k_indices[0] : -1; 59 | sq_distances_[i] = k_sq_dists[0]; 60 | second_sq_distances_[i] = k_sq_dists[1]; 61 | anchors_[i] = pt.getVector4fMap(); 62 | 63 | if (correspondences_[i] < 0) { 64 | continue; 65 | } 66 | 67 | const int target_index = correspondences_[i]; 68 | const auto& cov_A = source_covs_[i]; 69 | const auto& cov_B = target_covs_[target_index]; 70 | 71 | Eigen::Matrix4d RCR = cov_B + x.matrix() * cov_A * x.matrix().transpose(); 72 | RCR(3, 3) = 1.0; 73 | 74 | mahalanobis_[i] = RCR.inverse(); 75 | mahalanobis_[i](3, 3) = 0.0; 76 | } 77 | } 78 | 79 | template 80 | double FastGICPSingleThread::linearize(const Eigen::Isometry3d& trans, Eigen::Matrix* H, Eigen::Matrix* b) { 81 | if (H && b) { 82 | update_correspondences(trans); 83 | H->setZero(); 84 | b->setZero(); 85 | } 86 | 87 | double sum_errors = 0.0; 88 | for (int i = 0; i < input_->size(); i++) { 89 | int target_index = correspondences_[i]; 90 | if (target_index < 0) { 91 | continue; 92 | } 93 | 94 | const Eigen::Vector4d mean_A = input_->at(i).getVector4fMap().template cast(); 95 | const auto& cov_A = source_covs_[i]; 96 | 97 | const Eigen::Vector4d mean_B = target_->at(target_index).getVector4fMap().template cast(); 98 | const auto& cov_B = target_covs_[target_index]; 99 | 100 | const Eigen::Vector4d transed_mean_A = trans * mean_A; 101 | const Eigen::Vector4d error = mean_B - transed_mean_A; 102 | 103 | sum_errors += error.transpose() * mahalanobis_[i] * error; 104 | 105 | if (H == nullptr || b == nullptr) { 106 | continue; 107 | } 108 | 109 | Eigen::Matrix dtdx0 = Eigen::Matrix::Zero(); 110 | dtdx0.block<3, 3>(0, 0) = skewd(transed_mean_A.head<3>()); 111 | dtdx0.block<3, 3>(0, 3) = -Eigen::Matrix3d::Identity(); 112 | 113 | Eigen::Matrix jlossexp = dtdx0; 114 | 115 | (*H) += jlossexp.transpose() * mahalanobis_[i] * jlossexp; 116 | (*b) += jlossexp.transpose() * mahalanobis_[i] * error; 117 | } 118 | 119 | return sum_errors; 120 | } 121 | 122 | template 123 | double FastGICPSingleThread::compute_error(const Eigen::Isometry3d& trans) { 124 | return linearize(trans, nullptr, nullptr); 125 | } 126 | 127 | } // namespace fast_gicp 128 | 129 | #endif 130 | -------------------------------------------------------------------------------- /vils_estimator/src/lidar_functions/fast_gicp/include/fast_gicp/gicp/lsq_registration.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_LSQ_REGISTRATION_HPP 2 | #define FAST_GICP_LSQ_REGISTRATION_HPP 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | namespace fast_gicp { 12 | 13 | enum class LSQ_OPTIMIZER_TYPE { GaussNewton, LevenbergMarquardt }; 14 | 15 | template 16 | class LsqRegistration : public pcl::Registration { 17 | public: 18 | using Scalar = float; 19 | using Matrix4 = typename pcl::Registration::Matrix4; 20 | 21 | using PointCloudSource = typename pcl::Registration::PointCloudSource; 22 | using PointCloudSourcePtr = typename PointCloudSource::Ptr; 23 | using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr; 24 | 25 | using PointCloudTarget = typename pcl::Registration::PointCloudTarget; 26 | using PointCloudTargetPtr = typename PointCloudTarget::Ptr; 27 | using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr; 28 | 29 | protected: 30 | using pcl::Registration::input_; 31 | using pcl::Registration::nr_iterations_; 32 | using pcl::Registration::max_iterations_; 33 | using pcl::Registration::final_transformation_; 34 | using pcl::Registration::transformation_epsilon_; 35 | using pcl::Registration::converged_; 36 | 37 | public: 38 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 39 | 40 | LsqRegistration(); 41 | virtual ~LsqRegistration(); 42 | 43 | void setRotationEpsilon(double eps); 44 | void setInitialLambdaFactor(double init_lambda_factor); 45 | void setDebugPrint(bool lm_debug_print); 46 | 47 | const Eigen::Matrix& getFinalHessian() const; 48 | 49 | virtual void swapSourceAndTarget() {} 50 | virtual void clearSource() {} 51 | virtual void clearTarget() {} 52 | 53 | protected: 54 | virtual void computeTransformation(PointCloudSource& output, const Matrix4& guess) override; 55 | 56 | bool is_converged(const Eigen::Isometry3d& delta) const; 57 | 58 | virtual double linearize(const Eigen::Isometry3d& trans, Eigen::Matrix* H = nullptr, Eigen::Matrix* b = nullptr) = 0; 59 | virtual double compute_error(const Eigen::Isometry3d& trans) = 0; 60 | 61 | bool step_optimize(Eigen::Isometry3d& x0, Eigen::Isometry3d& delta); 62 | bool step_gn(Eigen::Isometry3d& x0, Eigen::Isometry3d& delta); 63 | bool step_lm(Eigen::Isometry3d& x0, Eigen::Isometry3d& delta); 64 | 65 | protected: 66 | double rotation_epsilon_; 67 | 68 | LSQ_OPTIMIZER_TYPE lsq_optimizer_type_; 69 | int lm_max_iterations_; 70 | double lm_init_lambda_factor_; 71 | double lm_lambda_; 72 | bool lm_debug_print_; 73 | 74 | Eigen::Matrix final_hessian_; 75 | }; 76 | } // namespace fast_gicp 77 | 78 | #endif -------------------------------------------------------------------------------- /vils_estimator/src/lidar_functions/fast_gicp/include/fast_gicp/ndt/impl/ndt_cuda_impl.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_NDT_CUDA_IMPL_HPP 2 | #define FAST_GICP_NDT_CUDA_IMPL_HPP 3 | 4 | #include 5 | 6 | #include 7 | 8 | namespace fast_gicp { 9 | 10 | template 11 | NDTCuda::NDTCuda() : LsqRegistration() { 12 | this->reg_name_ = "NDTCuda"; 13 | ndt_cuda_.reset(new cuda::NDTCudaCore()); 14 | } 15 | 16 | template 17 | NDTCuda::~NDTCuda() {} 18 | 19 | template 20 | void NDTCuda::setDistanceMode(NDTDistanceMode mode) { 21 | ndt_cuda_->set_distance_mode(mode); 22 | } 23 | 24 | template 25 | void NDTCuda::setResolution(double resolution) { 26 | ndt_cuda_->set_resolution(resolution); 27 | } 28 | 29 | template 30 | void NDTCuda::setNeighborSearchMethod(NeighborSearchMethod method, double radius) { 31 | ndt_cuda_->set_neighbor_search_method(method, radius); 32 | } 33 | 34 | template 35 | void NDTCuda::swapSourceAndTarget() { 36 | ndt_cuda_->swap_source_and_target(); 37 | input_.swap(target_); 38 | } 39 | 40 | template 41 | void NDTCuda::clearSource() { 42 | input_.reset(); 43 | } 44 | 45 | template 46 | void NDTCuda::clearTarget() { 47 | target_.reset(); 48 | } 49 | 50 | template 51 | void NDTCuda::setInputSource(const PointCloudSourceConstPtr& cloud) { 52 | if (cloud == input_) { 53 | return; 54 | } 55 | pcl::Registration::setInputSource(cloud); 56 | 57 | std::vector> points(cloud->size()); 58 | std::transform(cloud->begin(), cloud->end(), points.begin(), [=](const PointSource& pt) { return pt.getVector3fMap(); }); 59 | ndt_cuda_->set_source_cloud(points); 60 | } 61 | 62 | template 63 | void NDTCuda::setInputTarget(const PointCloudTargetConstPtr& cloud) { 64 | if (cloud == target_) { 65 | return; 66 | } 67 | 68 | pcl::Registration::setInputTarget(cloud); 69 | 70 | std::vector> points(cloud->size()); 71 | std::transform(cloud->begin(), cloud->end(), points.begin(), [=](const PointTarget& pt) { return pt.getVector3fMap(); }); 72 | ndt_cuda_->set_target_cloud(points); 73 | } 74 | 75 | template 76 | void NDTCuda::computeTransformation(PointCloudSource& output, const Matrix4& guess) { 77 | ndt_cuda_->create_voxelmaps(); 78 | LsqRegistration::computeTransformation(output, guess); 79 | } 80 | 81 | template 82 | double NDTCuda::linearize(const Eigen::Isometry3d& trans, Eigen::Matrix* H, Eigen::Matrix* b) { 83 | ndt_cuda_->update_correspondences(trans); 84 | return ndt_cuda_->compute_error(trans, H, b); 85 | } 86 | 87 | template 88 | double NDTCuda::compute_error(const Eigen::Isometry3d& trans) { 89 | return ndt_cuda_->compute_error(trans, nullptr, nullptr); 90 | } 91 | 92 | } // namespace fast_gicp 93 | 94 | #endif -------------------------------------------------------------------------------- /vils_estimator/src/lidar_functions/fast_gicp/include/fast_gicp/ndt/ndt_cuda.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_NDT_CUDA_HPP 2 | #define FAST_GICP_NDT_CUDA_HPP 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | namespace fast_gicp { 16 | 17 | namespace cuda { 18 | class NDTCudaCore; 19 | } 20 | 21 | template 22 | class NDTCuda : public LsqRegistration { 23 | public: 24 | using Scalar = float; 25 | using Matrix4 = typename pcl::Registration::Matrix4; 26 | 27 | using PointCloudSource = typename pcl::Registration::PointCloudSource; 28 | using PointCloudSourcePtr = typename PointCloudSource::Ptr; 29 | using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr; 30 | 31 | using PointCloudTarget = typename pcl::Registration::PointCloudTarget; 32 | using PointCloudTargetPtr = typename PointCloudTarget::Ptr; 33 | using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr; 34 | 35 | protected: 36 | using pcl::Registration::reg_name_; 37 | using pcl::Registration::input_; 38 | using pcl::Registration::target_; 39 | using pcl::Registration::corr_dist_threshold_; 40 | 41 | public: 42 | NDTCuda(); 43 | virtual ~NDTCuda() override; 44 | 45 | void setDistanceMode(NDTDistanceMode mode); 46 | void setResolution(double resolution); 47 | void setNeighborSearchMethod(NeighborSearchMethod method, double radius = -1.0); 48 | 49 | virtual void swapSourceAndTarget() override; 50 | virtual void clearSource() override; 51 | virtual void clearTarget() override; 52 | 53 | virtual void setInputSource(const PointCloudSourceConstPtr& cloud) override; 54 | virtual void setInputTarget(const PointCloudTargetConstPtr& cloud) override; 55 | 56 | protected: 57 | virtual void computeTransformation(PointCloudSource& output, const Matrix4& guess) override; 58 | virtual double linearize(const Eigen::Isometry3d& trans, Eigen::Matrix* H, Eigen::Matrix* b) override; 59 | virtual double compute_error(const Eigen::Isometry3d& trans) override; 60 | 61 | protected: 62 | std::unique_ptr ndt_cuda_; 63 | }; 64 | } // namespace fast_gicp 65 | 66 | #endif -------------------------------------------------------------------------------- /vils_estimator/src/lidar_functions/fast_gicp/include/fast_gicp/ndt/ndt_settings.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_NDT_SETTINGS_HPP 2 | #define FAST_GICP_NDT_SETTINGS_HPP 3 | 4 | namespace fast_gicp { 5 | 6 | enum class NDTDistanceMode { P2D, D2D }; 7 | 8 | } // namespace fast_gicp 9 | 10 | #endif -------------------------------------------------------------------------------- /vils_estimator/src/lidar_functions/fast_gicp/include/fast_gicp/so3/so3.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_SO3_HPP 2 | #define FAST_GICP_SO3_HPP 3 | 4 | #include 5 | #include 6 | 7 | namespace fast_gicp { 8 | 9 | inline Eigen::Matrix3f skew(const Eigen::Vector3f& x) { 10 | Eigen::Matrix3f skew = Eigen::Matrix3f::Zero(); 11 | skew(0, 1) = -x[2]; 12 | skew(0, 2) = x[1]; 13 | skew(1, 0) = x[2]; 14 | skew(1, 2) = -x[0]; 15 | skew(2, 0) = -x[1]; 16 | skew(2, 1) = x[0]; 17 | 18 | return skew; 19 | } 20 | 21 | inline Eigen::Matrix3d skewd(const Eigen::Vector3d& x) { 22 | Eigen::Matrix3d skew = Eigen::Matrix3d::Zero(); 23 | skew(0, 1) = -x[2]; 24 | skew(0, 2) = x[1]; 25 | skew(1, 0) = x[2]; 26 | skew(1, 2) = -x[0]; 27 | skew(2, 0) = -x[1]; 28 | skew(2, 1) = x[0]; 29 | 30 | return skew; 31 | } 32 | 33 | /* 34 | * SO3 expmap code taken from Sophus 35 | * https://github.com/strasdat/Sophus/blob/593db47500ea1a2de5f0e6579c86147991509c59/sophus/so3.hpp#L585 36 | * 37 | * Copyright 2011-2017 Hauke Strasdat 38 | * 2012-2017 Steven Lovegrove 39 | * 40 | * Permission is hereby granted, free of charge, to any person obtaining a copy 41 | * of this software and associated documentation files (the "Software"), to 42 | * deal in the Software without restriction, including without limitation the 43 | * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 44 | * sell copies of the Software, and to permit persons to whom the Software is 45 | * furnished to do so, subject to the following conditions: 46 | * 47 | * The above copyright notice and this permission notice shall be included in 48 | * all copies or substantial portions of the Software. 49 | * 50 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 51 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 52 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 53 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 54 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 55 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 56 | * IN THE SOFTWARE. 57 | */ 58 | inline Eigen::Quaterniond so3_exp(const Eigen::Vector3d& omega) { 59 | double theta_sq = omega.dot(omega); 60 | 61 | double theta; 62 | double imag_factor; 63 | double real_factor; 64 | if(theta_sq < 1e-10) { 65 | theta = 0; 66 | double theta_quad = theta_sq * theta_sq; 67 | imag_factor = 0.5 - 1.0 / 48.0 * theta_sq + 1.0 / 3840.0 * theta_quad; 68 | real_factor = 1.0 - 1.0 / 8.0 * theta_sq + 1.0 / 384.0 * theta_quad; 69 | } else { 70 | theta = std::sqrt(theta_sq); 71 | double half_theta = 0.5 * theta; 72 | imag_factor = std::sin(half_theta) / theta; 73 | real_factor = std::cos(half_theta); 74 | } 75 | 76 | return Eigen::Quaterniond(real_factor, imag_factor * omega.x(), imag_factor * omega.y(), imag_factor * omega.z()); 77 | } 78 | 79 | } // namespace fast_gicp 80 | 81 | #endif -------------------------------------------------------------------------------- /vils_estimator/src/parameters.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include "utility/utility.h" 7 | #include 8 | #include 9 | #include 10 | 11 | const double FOCAL_LENGTH = 460.0; //460 12 | const int WINDOW_SIZE = 6; //11 7 13 | const int NUM_OF_CAM = 1; 14 | const int NUM_OF_F = 1000; 15 | //#define UNIT_SPHERE_ERROR 16 | 17 | extern double INIT_DEPTH; 18 | extern double MIN_PARALLAX; 19 | extern int ESTIMATE_EXTRINSIC; 20 | 21 | extern double ACC_N, ACC_W; 22 | extern double GYR_N, GYR_W; 23 | 24 | extern std::vector RIC; 25 | extern std::vector TIC; 26 | 27 | extern Eigen::Matrix3d RLI; 28 | extern Eigen::Vector3d TLI; 29 | 30 | extern Eigen::Vector3d G; 31 | extern Eigen::Vector3d G_DIRECTION; 32 | 33 | extern double PBC_UX, PLB_UX; 34 | extern double PBC_LX, PLB_LX; 35 | extern double PBC_UY, PLB_UY; 36 | extern double PBC_LY, PLB_LY; 37 | extern double PBC_UZ, PLB_UZ; 38 | extern double PBC_LZ, PLB_LZ; 39 | 40 | extern int ANGLE_VI; 41 | 42 | extern double BIAS_ACC_THRESHOLD; 43 | extern double BIAS_GYR_THRESHOLD; 44 | extern double SOLVER_TIME; 45 | extern int NUM_ITERATIONS; 46 | extern std::string EX_CALIB_RESULT_PATH; 47 | extern std::string VINS_RESULT_PATH; 48 | extern std::string IMU_TOPIC; 49 | extern std::string LIDAR_TOPIC; 50 | extern std::string EX_RESULT_PATH; 51 | 52 | extern double TD; 53 | extern double TR; 54 | extern int ESTIMATE_TD; 55 | extern int ROLLING_SHUTTER; 56 | extern double ROW, COL; 57 | 58 | extern double LeafSize; 59 | extern int NumThreads; 60 | extern double TransformationEpsilon; 61 | extern double MaxCorrespondenceDistance; 62 | 63 | extern double MinDistance; 64 | extern double MaxDistance; 65 | 66 | extern double LidarTimeStep; 67 | extern int SHOW_LIDAR_CONSTRAINT; 68 | extern int ADD_LIDAR_ICP; 69 | extern int ADD_LPS; 70 | 71 | void readParameters(ros::NodeHandle &n); 72 | 73 | enum SIZE_PARAMETERIZATION 74 | { 75 | SIZE_POSE = 7, 76 | SIZE_SPEEDBIAS = 9, 77 | SIZE_FEATURE = 1 78 | }; 79 | 80 | enum StateOrder 81 | { 82 | O_P = 0, 83 | O_R = 3, 84 | O_V = 6, 85 | O_BA = 9, 86 | O_BG = 12 87 | }; 88 | 89 | enum NoiseOrder 90 | { 91 | O_AN = 0, 92 | O_GN = 3, 93 | O_AW = 6, 94 | O_GW = 9 95 | }; 96 | -------------------------------------------------------------------------------- /vils_estimator/src/utility/CameraPoseVisualization.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | class CameraPoseVisualization { 11 | public: 12 | std::string m_marker_ns; 13 | 14 | CameraPoseVisualization(float r, float g, float b, float a); 15 | 16 | void setImageBoundaryColor(float r, float g, float b, float a=1.0); 17 | void setOpticalCenterConnectorColor(float r, float g, float b, float a=1.0); 18 | void setScale(double s); 19 | void setLineWidth(double width); 20 | 21 | void add_pose(const Eigen::Vector3d& p, const Eigen::Quaterniond& q); 22 | void reset(); 23 | 24 | void publish_by(ros::Publisher& pub, const std_msgs::Header& header); 25 | void add_edge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1); 26 | void add_loopedge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1); 27 | private: 28 | std::vector m_markers; 29 | std_msgs::ColorRGBA m_image_boundary_color; 30 | std_msgs::ColorRGBA m_optical_center_connector_color; 31 | double m_scale; 32 | double m_line_width; 33 | 34 | static const Eigen::Vector3d imlt; 35 | static const Eigen::Vector3d imlb; 36 | static const Eigen::Vector3d imrt; 37 | static const Eigen::Vector3d imrb; 38 | static const Eigen::Vector3d oc ; 39 | static const Eigen::Vector3d lt0 ; 40 | static const Eigen::Vector3d lt1 ; 41 | static const Eigen::Vector3d lt2 ; 42 | }; 43 | -------------------------------------------------------------------------------- /vils_estimator/src/utility/tic_toc.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | class TicToc 8 | { 9 | public: 10 | TicToc() 11 | { 12 | tic(); 13 | } 14 | 15 | void tic() 16 | { 17 | start = std::chrono::system_clock::now(); 18 | } 19 | 20 | double toc() 21 | { 22 | end = std::chrono::system_clock::now(); 23 | std::chrono::duration elapsed_seconds = end - start; 24 | return elapsed_seconds.count() * 1000; 25 | } 26 | 27 | private: 28 | std::chrono::time_point start, end; 29 | }; 30 | -------------------------------------------------------------------------------- /vils_estimator/src/utility/utility.cpp: -------------------------------------------------------------------------------- 1 | #include "utility.h" 2 | 3 | Eigen::Matrix3d Utility::g2R(const Eigen::Vector3d &g) 4 | { 5 | Eigen::Matrix3d R0; 6 | Eigen::Vector3d ng1 = g.normalized(); 7 | Eigen::Vector3d ng2{0, 0, 1.0}; 8 | R0 = Eigen::Quaterniond::FromTwoVectors(ng1, ng2).toRotationMatrix(); 9 | double yaw = Utility::R2ypr(R0).x(); 10 | R0 = Utility::ypr2R(Eigen::Vector3d{-yaw, 0, 0}) * R0; 11 | // R0 = Utility::ypr2R(Eigen::Vector3d{-90, 0, 0}) * R0; 12 | return R0; 13 | } 14 | -------------------------------------------------------------------------------- /vils_estimator/src/utility/visualization.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include "CameraPoseVisualization.h" 18 | #include 19 | #include "../estimator.h" 20 | #include "../parameters.h" 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | extern ros::Publisher pub_odometry; 27 | extern ros::Publisher pub_path, pub_pose; 28 | extern ros::Publisher pub_cloud, pub_map; 29 | extern ros::Publisher pub_key_poses; 30 | extern ros::Publisher pub_ref_pose, pub_cur_pose; 31 | extern ros::Publisher pub_key; 32 | extern nav_msgs::Path path; 33 | extern nav_msgs::Path lidar_path; 34 | extern ros::Publisher pub_pose_graph; 35 | extern int IMAGE_ROW, IMAGE_COL; 36 | 37 | void registerPub(ros::NodeHandle &n); 38 | 39 | void pubLatestOdometry(const Eigen::Matrix3d &ex_bl_r, const Eigen::Vector3d &ex_bl_t, const Eigen::Vector3d &P, const Eigen::Quaterniond &Q, const Eigen::Vector3d &V, const std_msgs::Header &header); 40 | 41 | void printStatistics(const Estimator &estimator, double t); 42 | 43 | void pubOdometry(const Estimator &estimator, const std_msgs::Header &header); 44 | 45 | void pubInitialGuess(const Estimator &estimator, const std_msgs::Header &header); 46 | 47 | void pubKeyPoses(const Estimator &estimator, const std_msgs::Header &header); 48 | 49 | void pubCameraPose(const Estimator &estimator, const std_msgs::Header &header); 50 | 51 | void pubPointCloud(const Estimator &estimator, const std_msgs::Header &header); 52 | 53 | void pubLidarCloud(const CloudData cloud, const std_msgs::Header &header); 54 | 55 | void pubLidarCloud(const Estimator &estimator, const std_msgs::Header &header); 56 | 57 | void pubLidarPose(const Estimator &estimator, const std_msgs::Header &header); 58 | 59 | void pubTF(const Estimator &estimator, const std_msgs::Header &header); 60 | 61 | void pubKeyframe(const Estimator &estimator); 62 | 63 | void pubRelocalization(const Estimator &estimator); 64 | 65 | void pubLidarICPConstraintMarker(const Estimator &estimator, const std_msgs::Header &header); 66 | --------------------------------------------------------------------------------