├── .dockerignore ├── .gitignore ├── docker ├── build_docker.sh ├── run_docker.sh ├── Dockerfile └── 1.Dockerfile ├── scripts ├── debug_plot.sh └── transform_monitor.py ├── launch ├── test_outdoor.launch ├── test_indoor.launch ├── test_outdoor_64.launch ├── 16_scans_test.launch ├── test_indoor_rs32.launch ├── rs32_scans_test.launch ├── 64_scans_test.launch ├── map_4D.launch └── map_4D_indoor.launch ├── include ├── 3rdparty │ └── sophus │ │ ├── example_ensure_handler.cpp │ │ ├── LICENSE.txt │ │ ├── interpolate.hpp │ │ ├── velocities.hpp │ │ ├── rotation_matrix.hpp │ │ ├── interpolate_details.hpp │ │ ├── num_diff.hpp │ │ ├── sim_details.hpp │ │ ├── geometry.hpp │ │ ├── common.hpp │ │ ├── types.hpp │ │ └── test_macros.hpp ├── factor │ ├── GravityLocalParameterization.h │ ├── PoseLocalParameterization.h │ ├── PriorFactor.h │ ├── PlaneProjectionFactor.h │ ├── PivotPointPlaneFactor.h │ ├── PointDistanceFactor.h │ ├── PlaneToPlaneFactor.h │ ├── MarginalizationFactor.h │ └── ImuFactor.h ├── utils │ ├── TicToc.h │ ├── common_ros.h │ ├── Twist.h │ ├── LoadVirtual.h │ ├── CircularBuffer.h │ └── math_utils.h ├── point_processor │ └── point_types.h ├── map_builder │ └── MapBuilder.h ├── imu_processor │ ├── ImuInitializer.h │ ├── ImuInitializer_viorb.h │ └── MeasurementManager.h └── feature_manager │ └── FeatureManager.h ├── config ├── outdoor_test_config.yaml ├── indoor_test_config.yaml └── outdoor_test_config_64.yaml ├── test ├── test_visualizaer.cc ├── test_point_processor │ ├── test_point_odometry.cc │ ├── test_point_mapping.cc │ └── test_point_processor.cc └── test_imu_processor │ ├── test_circular_buffer.cc │ └── test_measurement_manager.cc ├── src ├── factor │ ├── GravityLocalParameterization.cc │ ├── PoseLocalParameterization.cc │ ├── PriorFactor.cc │ └── PointDistanceFactor.cc ├── mapping_node.cc ├── odometry_node.cc ├── map_builder_node.cc ├── processor_node.cc ├── input_filters_node.cc ├── imu_processor │ └── MeasurementManager.cc ├── save_bag_to_pcd.cc └── estimator_node.cc ├── README.md └── package.xml /.dockerignore: -------------------------------------------------------------------------------- 1 | docker 2 | .git 3 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | .idea 2 | .vscode 3 | build 4 | cmake-build-* 5 | -------------------------------------------------------------------------------- /docker/build_docker.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )" 3 | docker build --build-arg your_name=${USER} -t hyye/lio -f ${DIR}/Dockerfile ${DIR}/.. -------------------------------------------------------------------------------- /scripts/debug_plot.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | python transform_monitor.py & 3 | rqt_plot /predict_odom/pose/pose/position:x:y:z & 4 | rqt_plot /extrinsic_lb/pose/position:x:y:z & 5 | rqt_plot /debug/imu_rot/vector/y /debug/laser_rot/vector/y 6 | -------------------------------------------------------------------------------- /launch/test_outdoor.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /include/3rdparty/sophus/example_ensure_handler.cpp: -------------------------------------------------------------------------------- 1 | #include "common.hpp" 2 | 3 | #include 4 | #include 5 | 6 | namespace Sophus { 7 | void ensureFailed(char const* function, char const* file, int line, 8 | char const* description) { 9 | std::printf("Sophus ensure failed in function '%s', file '%s', line %d.\n", 10 | file, function, line); 11 | std::printf("Description: %s\n", description); 12 | std::abort(); 13 | } 14 | } 15 | -------------------------------------------------------------------------------- /launch/test_indoor.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /launch/test_outdoor_64.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /launch/16_scans_test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /launch/test_indoor_rs32.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /launch/rs32_scans_test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /launch/64_scans_test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /launch/map_4D.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /launch/map_4D_indoor.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /include/3rdparty/sophus/LICENSE.txt: -------------------------------------------------------------------------------- 1 | Copyright 2011-2017 Hauke Strasdat 2 | 2012-2017 Steven Lovegrove 3 | 4 | Permission is hereby granted, free of charge, to any person obtaining a copy 5 | of this software and associated documentation files (the "Software"), to 6 | deal in the Software without restriction, including without limitation the 7 | rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 8 | sell copies of the Software, and to permit persons to whom the Software is 9 | furnished to do so, subject to the following conditions: 10 | 11 | The above copyright notice and this permission notice shall be included in 12 | all copies or substantial portions of the Software. 13 | 14 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 19 | FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 20 | IN THE SOFTWARE. -------------------------------------------------------------------------------- /include/3rdparty/sophus/interpolate.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SOPHUS_INTERPOLATE_HPP 2 | #define SOPHUS_INTERPOLATE_HPP 3 | 4 | #include 5 | 6 | #include "interpolate_details.hpp" 7 | 8 | namespace Sophus { 9 | 10 | // This function interpolates between two Lie group elements ``foo_T_bar`` 11 | // and ``foo_T_baz`` with an interpolation factor of ``alpha`` in [0, 1]. 12 | // 13 | // It returns a pose ``foo_T_quiz`` with ``quiz`` being a frame between ``bar`` 14 | // and ``baz``. If ``alpha=0`` it returns ``foo_T_bar``. If it is 1, it returns 15 | // ``foo_T_bar``. 16 | // 17 | // (Since interpolation on Lie groups is inverse-invariant, we can equivalently 18 | // think of the input arguments as being ``bar_T_foo``, ``baz_T_foo`` and the 19 | // return value being ``quiz_T_foo``.) 20 | // 21 | // Precondition: ``p`` must be in [0, 1]. 22 | // 23 | template 24 | enable_if_t::supported, G> interpolate( 25 | G const& foo_T_bar, G const& foo_T_baz, Scalar2 p = Scalar2(0.5f)) { 26 | using Scalar = typename G::Scalar; 27 | Scalar inter_p(p); 28 | SOPHUS_ENSURE(inter_p >= Scalar(0) && inter_p <= Scalar(1), 29 | "p (%) must in [0, 1]."); 30 | return foo_T_bar * G::exp(inter_p * (foo_T_bar.inverse() * foo_T_baz).log()); 31 | } 32 | 33 | } // namespace Sophus 34 | 35 | #endif // SOPHUS_INTERPOLATE_HPP 36 | -------------------------------------------------------------------------------- /docker/run_docker.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | until nvidia-docker ps 4 | do 5 | echo "Waiting for docker server" 6 | sleep 1 7 | done 8 | 9 | 10 | # Make sure processes in the container can connect to the x server 11 | # Necessary so rviz can create a context for OpenGL rendering 12 | XAUTH=/tmp/.docker.xauth 13 | if [ ! -f $XAUTH ] 14 | then 15 | xauth_list=$(xauth nlist :0 | sed -e 's/^..../ffff/') 16 | if [ ! -z "$xauth_list" ] 17 | then 18 | echo $xauth_list | xauth -f $XAUTH nmerge - 19 | else 20 | touch $XAUTH 21 | fi 22 | chmod a+r $XAUTH 23 | fi 24 | 25 | # nvidia-docker run -it \ 26 | # -e DISPLAY=$DISPLAY \ 27 | # -e QT_X11_NO_MITSHM=1 \ 28 | # -e XAUTHORITY=$XAUTH \ 29 | # -v "$XAUTH:$XAUTH" \ 30 | # -v "/tmp/.X11-unix:/tmp/.X11-unix" \ 31 | # -v "/etc/localtime:/etc/localtime:ro" \ 32 | # -v "/dev/input:/dev/input" \ 33 | # --privileged \ 34 | # --rm=true \ 35 | # --net=host \ 36 | # hyye/lio 37 | 38 | nvidia-docker run -it \ 39 | -e DISPLAY=$DISPLAY \ 40 | -e QT_X11_NO_MITSHM=1 \ 41 | -e XAUTHORITY=$XAUTH \ 42 | -v "$XAUTH:$XAUTH" \ 43 | --user=$USER \ 44 | --volume="/etc/group:/etc/group:ro" \ 45 | --volume="/etc/passwd:/etc/passwd:ro" \ 46 | --volume="/etc/shadow:/etc/shadow:ro" \ 47 | --volume="/etc/sudoers.d:/etc/sudoers.d:ro" \ 48 | --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \ 49 | --privileged \ 50 | --rm=true \ 51 | --net=host \ 52 | hyye/lio \ 53 | /bin/bash -c "source /workspace/devel/setup.bash; sleep 1; roslaunch lio test_indoor.launch & sleep 1; roslaunch lio map_4D_indoor.launch" 54 | -------------------------------------------------------------------------------- /config/outdoor_test_config.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | # lidar features 3 | min_plane_dis: 0.2 4 | 5 | min_match_sq_dis: 1.0 6 | 7 | corner_filter_size: 0.2 8 | surf_filter_size: 0.4 9 | map_filter_size: 0.6 10 | 11 | # window sizes 12 | window_size: 7 13 | opt_window_size: 5 14 | 15 | init_window_factor: 1 16 | 17 | # Extrinsic parameter between IMU and Camera (adapted from VINS-mono) 18 | estimate_extrinsic: 1 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it. 19 | # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess. 20 | # 2 Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning. 21 | opt_extrinsic: 1 22 | 23 | extrinsic_rotation: !!opencv-matrix 24 | rows: 3 25 | cols: 3 26 | dt: d 27 | data: [1, 0, 0, 28 | 0, 1, 0, 29 | 0, 0, 1] 30 | 31 | #Translation from imu frame to laser frame, laser^T_imu 32 | extrinsic_translation: !!opencv-matrix 33 | rows: 3 34 | cols: 1 35 | dt: d 36 | data: [-2.4, 0, 0.7] 37 | 38 | # optimization options 39 | run_optimization: 1 40 | 41 | update_laser_imu: 1 42 | gravity_fix: 1 43 | 44 | plane_projection_factor: 0 45 | 46 | imu_factor: 1 47 | point_distance_factor: 1 48 | 49 | prior_factor: 0 50 | 51 | marginalization_factor: 1 52 | 53 | odom_io: 3 54 | 55 | pcl_viewer: 0 56 | 57 | # IMU noises 58 | acc_n: 0.2 59 | gyr_n: 0.02 60 | acc_w: 0.0002 61 | gyr_w: 2.0e-5 62 | g_norm: 9.80 63 | 64 | # lidar deskew 65 | enable_deskew: 1 66 | cutoff_deskew: 0 67 | 68 | keep_features: 0 69 | 70 | msg_time_delay: 0.05 -------------------------------------------------------------------------------- /config/indoor_test_config.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | # lidar features 3 | min_plane_dis: 0.2 4 | 5 | min_match_sq_dis: 1.0 6 | 7 | corner_filter_size: 0.2 8 | surf_filter_size: 0.4 9 | map_filter_size: 0.6 10 | 11 | # window sizes 12 | window_size: 12 #7 13 | opt_window_size: 7 #5 14 | 15 | init_window_factor: 2 16 | 17 | # Extrinsic parameter between IMU and Camera (adapted from VINS-mono) 18 | estimate_extrinsic: 2 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it. 19 | # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess. 20 | # 2 Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning. 21 | opt_extrinsic: 1 22 | 23 | extrinsic_rotation: !!opencv-matrix 24 | rows: 3 25 | cols: 3 26 | dt: d 27 | data: [1, 0, 0, 28 | 0, 1, 0, 29 | 0, 0, 1] 30 | 31 | #Translation from imu frame to laser frame, laser^T_imu 32 | extrinsic_translation: !!opencv-matrix 33 | rows: 3 34 | cols: 1 35 | dt: d 36 | data: [0.0, 0.0, -0.081939] 37 | 38 | # optimization options 39 | run_optimization: 1 40 | 41 | update_laser_imu: 1 42 | gravity_fix: 1 43 | 44 | plane_projection_factor: 0 45 | 46 | imu_factor: 1 47 | point_distance_factor: 1 48 | 49 | prior_factor: 0 50 | 51 | marginalization_factor: 1 52 | 53 | odom_io: 2 54 | 55 | pcl_viewer: 0 56 | 57 | # IMU noises 58 | acc_n: 0.2 59 | gyr_n: 0.02 60 | acc_w: 0.0002 61 | gyr_w: 2.0e-5 62 | g_norm: 9.805 63 | 64 | # lidar deskew 65 | enable_deskew: 1 66 | cutoff_deskew: 0 67 | 68 | keep_features: 1 69 | 70 | msg_time_delay: 0.05 -------------------------------------------------------------------------------- /config/outdoor_test_config_64.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | # lidar features 3 | min_plane_dis: 0.2 4 | 5 | min_match_sq_dis: 1.0 6 | 7 | corner_filter_size: 0.2 8 | surf_filter_size: 0.4 9 | map_filter_size: 0.6 10 | 11 | # window sizes 12 | window_size: 7 13 | opt_window_size: 5 14 | 15 | init_window_factor: 1 16 | 17 | # Extrinsic parameter between IMU and Camera (adapted from VINS-mono) 18 | estimate_extrinsic: 1 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it. 19 | # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess. 20 | # 2 Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning. 21 | opt_extrinsic: 1 22 | 23 | # KITTI 00 24 | extrinsic_rotation: !!opencv-matrix 25 | rows: 3 26 | cols: 3 27 | dt: d 28 | data: [9.999976e-01,7.553071e-04,-2.035826e-03, 29 | -7.854027e-04,9.998898e-01,-1.482298e-02, 30 | 2.024406e-03,1.482454e-02,9.998881e-01] 31 | 32 | #Translation from imu frame to laser frame, laser^T_imu 33 | extrinsic_translation: !!opencv-matrix 34 | rows: 3 35 | cols: 1 36 | dt: d 37 | data: [-8.086759e-01,3.195559e-01,-7.997231e-01] 38 | 39 | # optimization options 40 | run_optimization: 1 41 | 42 | update_laser_imu: 1 43 | gravity_fix: 1 44 | 45 | plane_projection_factor: 0 46 | 47 | imu_factor: 1 48 | point_distance_factor: 1 49 | 50 | prior_factor: 1 51 | 52 | marginalization_factor: 1 53 | 54 | odom_io: 3 55 | 56 | pcl_viewer: 0 57 | 58 | # IMU noises 59 | acc_n: 0.2 60 | gyr_n: 0.02 61 | acc_w: 0.0002 62 | gyr_w: 2.0e-5 63 | g_norm: 9.80 64 | 65 | # lidar deskew 66 | enable_deskew: 1 67 | cutoff_deskew: 1 68 | 69 | keep_features: 0 70 | 71 | msg_time_delay: 0.00 72 | -------------------------------------------------------------------------------- /test/test_visualizaer.cc: -------------------------------------------------------------------------------- 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 5/7/18. 29 | // 30 | 31 | #include 32 | #include 33 | #include 34 | 35 | #include "lio/LineObject.h" 36 | #include "utils/TicToc.h" 37 | #include "visualizer/Visualizer.h" 38 | 39 | using namespace lio; 40 | using namespace std; 41 | 42 | TEST(LineObjectTest, LineObjectCreate) { 43 | 44 | } 45 | 46 | 47 | int main(int argc, char **argv) { 48 | testing::InitGoogleTest(&argc, argv); 49 | 50 | google::InitGoogleLogging(argv[0]); 51 | google::ParseCommandLineFlags(&argc, &argv, true); 52 | 53 | FLAGS_alsologtostderr = true; 54 | 55 | PlaneNormalVisualizer vis; 56 | 57 | vis.Spin(); 58 | 59 | return RUN_ALL_TESTS(); 60 | } -------------------------------------------------------------------------------- /include/factor/GravityLocalParameterization.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 4/11/18. 29 | // 30 | 31 | #ifndef LIO_GRAVITYLOCALPARAMETERIZATION_H_ 32 | #define LIO_GRAVITYLOCALPARAMETERIZATION_H_ 33 | 34 | #include 35 | #include 36 | 37 | #include "utils/math_utils.h" 38 | 39 | namespace lio { 40 | 41 | using namespace mathutils; 42 | 43 | class GravityLocalParameterization : public ceres::LocalParameterization { 44 | 45 | virtual bool Plus(const double *x, const double *delta, double *x_plus_delta) const; 46 | virtual bool ComputeJacobian(const double *x, double *jacobian) const; 47 | virtual int GlobalSize() const { return 4; }; 48 | virtual int LocalSize() const { return 2; }; 49 | 50 | }; 51 | 52 | } 53 | 54 | #endif //LIO_GRAVITYLOCALPARAMETERIZATION_H_ 55 | -------------------------------------------------------------------------------- /include/factor/PoseLocalParameterization.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 4/4/18. 29 | // 30 | 31 | #ifndef LIO_POSELOCALPARAMETERIZATION_H_ 32 | #define LIO_POSELOCALPARAMETERIZATION_H_ 33 | 34 | #include 35 | #include 36 | 37 | #include "utils/math_utils.h" 38 | 39 | namespace lio { 40 | 41 | using namespace mathutils; 42 | 43 | class PoseLocalParameterization : public ceres::LocalParameterization { 44 | 45 | virtual bool Plus(const double *x, const double *delta, double *x_plus_delta) const; 46 | virtual bool ComputeJacobian(const double *x, double *jacobian) const; 47 | virtual int GlobalSize() const { return 7; }; 48 | virtual int LocalSize() const { return 6; }; 49 | 50 | }; 51 | 52 | } // namespace lio 53 | 54 | #endif //LIO_POSELOCALPARAMETERIZATION_H_ 55 | -------------------------------------------------------------------------------- /include/utils/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 | -------------------------------------------------------------------------------- /include/factor/PriorFactor.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 4/13/18. 29 | // 30 | 31 | #ifndef LIO_PRIORFACTOR_H_ 32 | #define LIO_PRIORFACTOR_H_ 33 | 34 | #include 35 | #include "imu_processor/IntegrationBase.h" 36 | #include "utils/math_utils.h" 37 | 38 | namespace lio { 39 | 40 | using namespace mathutils; 41 | 42 | class PriorFactor : public ceres::SizedCostFunction<6, 7> { 43 | 44 | public: 45 | PriorFactor() = delete; 46 | PriorFactor(const Eigen::Vector3d &pos, const Eigen::Quaterniond &rot) : pos_{pos}, rot_{rot} {} 47 | 48 | bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 49 | 50 | void Check(double const *const *parameters); 51 | 52 | Eigen::Vector3d pos_; 53 | Eigen::Quaterniond rot_; 54 | 55 | }; 56 | 57 | } 58 | 59 | #endif //LIO_PRIORFACTOR_H_ 60 | -------------------------------------------------------------------------------- /include/utils/common_ros.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/16/18. 29 | // 30 | 31 | #ifndef LIO_COMMON_ROS_H_ 32 | #define LIO_COMMON_ROS_H_ 33 | 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | namespace lio { 40 | 41 | template 42 | inline void PublishCloudMsg(ros::Publisher& publisher, 43 | const pcl::PointCloud& cloud, 44 | const ros::Time& stamp, 45 | std::string frame_id) { 46 | sensor_msgs::PointCloud2 msg; 47 | pcl::toROSMsg(cloud, msg); 48 | msg.header.stamp = stamp; 49 | msg.header.frame_id = frame_id; 50 | publisher.publish(msg); 51 | } 52 | 53 | } // namespace lio 54 | 55 | #endif //LIO_COMMON_ROS_H_ 56 | -------------------------------------------------------------------------------- /src/factor/GravityLocalParameterization.cc: -------------------------------------------------------------------------------- 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 4/11/18. 29 | // 30 | 31 | #include "factor/GravityLocalParameterization.h" 32 | 33 | namespace lio { 34 | 35 | bool GravityLocalParameterization::Plus(const double *x, const double *delta, double *x_plus_delta) const 36 | { 37 | Eigen::Map q(x); 38 | 39 | Eigen::Map dq_xy(delta); 40 | 41 | Eigen::Quaterniond dq = DeltaQ(Eigen::Vector3d(dq_xy.x(), dq_xy.y(), 0.0)); 42 | 43 | Eigen::Map q_plus(x_plus_delta); 44 | 45 | q_plus = (q * dq).normalized(); 46 | 47 | return true; 48 | } 49 | bool GravityLocalParameterization::ComputeJacobian(const double *x, double *jacobian) const 50 | { 51 | Eigen::Map> j(jacobian); 52 | j.topRows<2>().setIdentity(); 53 | j.bottomRows<2>().setZero(); 54 | 55 | return true; 56 | } 57 | 58 | } -------------------------------------------------------------------------------- /include/point_processor/point_types.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 | // adapted from velodyne ros driver 28 | 29 | #ifndef LIO_POINT_TYPES_H_ 30 | #define LIO_POINT_TYPES_H_ 31 | 32 | #include 33 | 34 | namespace lio { 35 | 36 | /** Euclidean coordinate, including intensity and ring number. */ 37 | struct PointXYZIR 38 | { 39 | PCL_ADD_POINT4D; // quad-word XYZ 40 | float intensity; ///< laser intensity reading 41 | uint16_t ring; ///< laser ring number 42 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW // ensure proper alignment 43 | } 44 | EIGEN_ALIGN16; 45 | } // namespace lio 46 | 47 | POINT_CLOUD_REGISTER_POINT_STRUCT(lio::PointXYZIR, 48 | (float, x, x) 49 | (float, y, y) 50 | (float, z, z) 51 | (float, intensity, intensity) 52 | (uint16_t, ring, ring)) 53 | 54 | #endif // LIO_POINT_TYPES_H_ -------------------------------------------------------------------------------- /src/factor/PoseLocalParameterization.cc: -------------------------------------------------------------------------------- 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 4/4/18. 29 | // 30 | 31 | #include "factor/PoseLocalParameterization.h" 32 | 33 | namespace lio { 34 | 35 | bool PoseLocalParameterization::Plus(const double *x, const double *delta, double *x_plus_delta) const 36 | { 37 | Eigen::Map p(x); 38 | Eigen::Map q(x + 3); 39 | 40 | Eigen::Map dp(delta); 41 | 42 | Eigen::Quaterniond dq = DeltaQ(Eigen::Map(delta + 3)); 43 | 44 | Eigen::Map p_plus(x_plus_delta); 45 | Eigen::Map q_plus(x_plus_delta + 3); 46 | 47 | p_plus = p + dp; 48 | q_plus = (q * dq).normalized(); 49 | 50 | return true; 51 | } 52 | bool PoseLocalParameterization::ComputeJacobian(const double *x, double *jacobian) const 53 | { 54 | Eigen::Map> j(jacobian); 55 | j.topRows<6>().setIdentity(); 56 | j.bottomRows<1>().setZero(); 57 | 58 | return true; 59 | } 60 | 61 | } -------------------------------------------------------------------------------- /docker/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM osrf/ros:melodic-desktop-full 2 | 3 | ARG your_name 4 | 5 | # see: https://gitlab.com/nvidia/opengl/tree/ubuntu18.04 6 | 7 | RUN dpkg --add-architecture i386 && \ 8 | apt-get update && apt-get install -y --no-install-recommends \ 9 | libxau6 libxau6:i386 \ 10 | libxdmcp6 libxdmcp6:i386 \ 11 | libxcb1 libxcb1:i386 \ 12 | libxext6 libxext6:i386 \ 13 | libx11-6 libx11-6:i386 && \ 14 | rm -rf /var/lib/apt/lists/* 15 | 16 | # nvidia-container-runtime 17 | ENV NVIDIA_VISIBLE_DEVICES \ 18 | ${NVIDIA_VISIBLE_DEVICES:-all} 19 | ENV NVIDIA_DRIVER_CAPABILITIES \ 20 | ${NVIDIA_DRIVER_CAPABILITIES:+$NVIDIA_DRIVER_CAPABILITIES,}graphics,compat32,utility 21 | 22 | RUN echo "/usr/local/nvidia/lib" >> /etc/ld.so.conf.d/nvidia.conf && \ 23 | echo "/usr/local/nvidia/lib64" >> /etc/ld.so.conf.d/nvidia.conf 24 | 25 | # Required for non-glvnd setups. 26 | ENV LD_LIBRARY_PATH /usr/lib/x86_64-linux-gnu:/usr/lib/i386-linux-gnu${LD_LIBRARY_PATH:+:${LD_LIBRARY_PATH}}:/usr/local/nvidia/lib:/usr/local/nvidia/lib64 27 | 28 | RUN apt-get update && apt-get install -y --no-install-recommends \ 29 | libglvnd0 libglvnd0:i386 \ 30 | libgl1 libgl1:i386 \ 31 | libglx0 libglx0:i386 \ 32 | libegl1 libegl1:i386 \ 33 | libgles2 libgles2:i386 && \ 34 | rm -rf /var/lib/apt/lists/* 35 | 36 | COPY --from=nvidia/opengl:1.0-glvnd-runtime-ubuntu18.04 \ 37 | /usr/share/glvnd/egl_vendor.d/10_nvidia.json \ 38 | /usr/share/glvnd/egl_vendor.d/10_nvidia.json 39 | 40 | # Prerequisites 41 | # ENV USER=$your_name 42 | RUN adduser $your_name 43 | RUN curl http://ceres-solver.org/ceres-solver-1.14.0.tar.gz --output ceres-solver-1.14.0.tar.gz 44 | RUN apt-get update && apt-get install -y --no-install-recommends \ 45 | libgoogle-glog-dev libatlas-base-dev libeigen3-dev libsuitesparse-dev 46 | RUN tar zxf ceres-solver-1.14.0.tar.gz && mkdir ceres-bin && cd ceres-bin && cmake ../ceres-solver-1.14.0 && make -j3 && make install 47 | # USER $yourname 48 | # RUN mkdir -p /workspace && chmod -R 777 /workspace 49 | # COPY --chown=1000:1000 . /workspace/src/ 50 | COPY . /workspace/src/ 51 | RUN /bin/bash -c "source /opt/ros/melodic/setup.bash && cd /workspace && catkin_make -DCMAKE_BUILD_TYPE=Release" 52 | -------------------------------------------------------------------------------- /include/map_builder/MapBuilder.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 5/31/18. 29 | // 30 | 31 | #ifndef LIO_MAPBUILDER_H_ 32 | #define LIO_MAPBUILDER_H_ 33 | 34 | #include "point_processor/PointMapping.h" 35 | 36 | namespace lio { 37 | 38 | using namespace std; 39 | using namespace mathutils; 40 | 41 | struct MapBuilderConfig { 42 | float corner_filter_size = 0.2; 43 | float surf_filter_size = 0.4; 44 | float map_filter_size = 0.6; 45 | 46 | float min_match_sq_dis = 1.0; 47 | float min_plane_dis = 0.2; 48 | }; 49 | 50 | class MapBuilder : public PointMapping { 51 | public: 52 | MapBuilder() = delete; 53 | MapBuilder(MapBuilderConfig config); 54 | void SetupRos(ros::NodeHandle &nh); 55 | void ProcessMap(); 56 | void OptimizeMap(); 57 | void PublishMapBuilderResults(); 58 | void Transform4DAssociateToMap(); 59 | void Transform4DUpdate(); 60 | 61 | private: 62 | bool system_init_ = false; 63 | bool enable_4d_ = true; 64 | int skip_count_ = 2; 65 | MapBuilderConfig config_; 66 | int odom_count_ = 0; 67 | 68 | }; 69 | 70 | } 71 | 72 | #endif //LIO_MAPBUILDER_H_ 73 | -------------------------------------------------------------------------------- /src/mapping_node.cc: -------------------------------------------------------------------------------- 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/25/18. 29 | // 30 | 31 | // 32 | // Created by hyye on 3/15/18. 33 | // 34 | 35 | #include 36 | #include 37 | #include 38 | 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | 45 | #include 46 | 47 | #include "point_processor/PointMapping.h" 48 | #include "utils/TicToc.h" 49 | 50 | using namespace lio; 51 | using namespace std; 52 | using namespace mathutils; 53 | 54 | int main(int argc, char **argv) { 55 | 56 | google::InitGoogleLogging(argv[0]); 57 | FLAGS_alsologtostderr = true; 58 | 59 | ros::init(argc, argv, "point_mapping"); 60 | 61 | ros::NodeHandle nh("~");; 62 | 63 | PointMapping mapper; 64 | mapper.SetupRos(nh); 65 | mapper.Reset(); 66 | 67 | ros::Rate r(100); 68 | while (ros::ok()) { 69 | mapper.Process(); 70 | ros::spinOnce(); 71 | r.sleep(); 72 | } 73 | 74 | return 0; 75 | } -------------------------------------------------------------------------------- /include/factor/PlaneProjectionFactor.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 4/12/18. 29 | // 30 | 31 | #ifndef LIO_PLANEPROJECTIONFACTOR_H_ 32 | #define LIO_PLANEPROJECTIONFACTOR_H_ 33 | 34 | #include 35 | #include 36 | 37 | #include "utils/CircularBuffer.h" 38 | #include "utils/Twist.h" 39 | #include "utils/common_ros.h" 40 | #include "utils/TicToc.h" 41 | #include "utils/math_utils.h" 42 | #include "utils/geometry_utils.h" 43 | 44 | namespace lio { 45 | 46 | using namespace mathutils; 47 | 48 | class PlaneProjectionFactor : public ceres::SizedCostFunction<4, 7, 7, 7> { 49 | 50 | public: 51 | PlaneProjectionFactor(const Eigen::Vector4d &local_coeffi, const Eigen::Vector4d &local_coeffj, const double &score); 52 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 53 | 54 | void Check(double **parameters); 55 | 56 | Eigen::Vector4d local_coeffi_; 57 | Eigen::Vector4d local_coeffj_; 58 | double score_; 59 | 60 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 61 | 62 | }; 63 | 64 | } 65 | 66 | #endif //LIO_PLANEPROJECTIONFACTOR_H_ 67 | -------------------------------------------------------------------------------- /src/odometry_node.cc: -------------------------------------------------------------------------------- 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/15/18. 29 | // 30 | 31 | #include 32 | #include 33 | #include 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | 41 | #include 42 | 43 | #include "point_processor/PointOdometry.h" 44 | #include "utils/TicToc.h" 45 | 46 | using namespace lio; 47 | using namespace std; 48 | using namespace mathutils; 49 | 50 | DEFINE_int32(io_ratio, 2, "ratio of io"); 51 | 52 | int main(int argc, char **argv) { 53 | 54 | google::InitGoogleLogging(argv[0]); 55 | FLAGS_alsologtostderr = true; 56 | 57 | ros::init(argc, argv, "point_odometry"); 58 | 59 | ros::NodeHandle nh("~"); 60 | 61 | PointOdometry odometry(0.1, FLAGS_io_ratio); 62 | odometry.SetupRos(nh); 63 | odometry.Reset(); 64 | 65 | ros::Rate r(100); 66 | while (ros::ok()) { 67 | odometry.Process(); 68 | ros::spinOnce(); 69 | r.sleep(); 70 | } 71 | 72 | return 0; 73 | } -------------------------------------------------------------------------------- /include/factor/PivotPointPlaneFactor.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 5/17/18. 29 | // 30 | 31 | #ifndef LIO_PIVOTPOINTPLANEFACTOR_H_ 32 | #define LIO_PIVOTPOINTPLANEFACTOR_H_ 33 | 34 | #include 35 | #include 36 | 37 | #include "utils/CircularBuffer.h" 38 | #include "utils/Twist.h" 39 | #include "utils/common_ros.h" 40 | #include "utils/TicToc.h" 41 | #include "utils/math_utils.h" 42 | #include "utils/geometry_utils.h" 43 | 44 | namespace lio { 45 | 46 | using namespace mathutils; 47 | 48 | class PivotPointPlaneFactor : public ceres::SizedCostFunction<1, 7, 7, 7> { 49 | 50 | public: 51 | PivotPointPlaneFactor(const Eigen::Vector3d &point, 52 | const Eigen::Vector4d &coeff); 53 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 54 | void Check(double **parameters); 55 | 56 | Eigen::Vector3d point_; 57 | Eigen::Vector4d coeff_; 58 | 59 | // TODO: necessary? 60 | // static Eigen::Matrix3d sqrt_info; 61 | static double sum_t; 62 | 63 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 64 | 65 | }; 66 | 67 | } 68 | 69 | #endif //LIO_PIVOTPOINTPLANEFACTOR_H_ 70 | -------------------------------------------------------------------------------- /src/map_builder_node.cc: -------------------------------------------------------------------------------- 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 5/31/18. 29 | // 30 | 31 | #include 32 | #include 33 | 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | #include 40 | 41 | #include 42 | 43 | #include 44 | #include 45 | 46 | #include "map_builder/MapBuilder.h" 47 | #include "utils/TicToc.h" 48 | 49 | using namespace lio; 50 | using namespace std; 51 | using namespace mathutils; 52 | 53 | static ros::NodeHandlePtr nh_ptr; 54 | 55 | int main(int argc, char **argv) { 56 | 57 | google::InitGoogleLogging(argv[0]); 58 | FLAGS_alsologtostderr = true; 59 | 60 | ros::init(argc, argv, "map_builder"); 61 | 62 | ros::NodeHandle nh("~"); 63 | 64 | MapBuilderConfig config; 65 | config.map_filter_size = 0.2; 66 | 67 | MapBuilder mapper(config); 68 | mapper.SetupRos(nh); 69 | mapper.Reset(); 70 | 71 | ros::Rate r(100); 72 | while (ros::ok()) { 73 | mapper.ProcessMap(); 74 | ros::spinOnce(); 75 | r.sleep(); 76 | } 77 | 78 | return 0; 79 | } 80 | -------------------------------------------------------------------------------- /include/factor/PointDistanceFactor.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 4/3/18. 29 | // 30 | 31 | #ifndef LIO_POINTDISTANCEFACTOR_H_ 32 | #define LIO_POINTDISTANCEFACTOR_H_ 33 | 34 | #include 35 | #include 36 | 37 | #include "utils/CircularBuffer.h" 38 | #include "utils/Twist.h" 39 | #include "utils/common_ros.h" 40 | #include "utils/TicToc.h" 41 | #include "utils/math_utils.h" 42 | #include "utils/geometry_utils.h" 43 | 44 | namespace lio { 45 | 46 | using namespace mathutils; 47 | 48 | class PointDistanceFactor : public ceres::SizedCostFunction<1, 7, 7> { 49 | 50 | public: 51 | PointDistanceFactor(const Eigen::Vector3d &point, 52 | const Eigen::Vector4d &coeff, 53 | const Eigen::Matrix info_mat); 54 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 55 | void Check(double **parameters); 56 | 57 | Eigen::Vector3d point_; 58 | Eigen::Vector4d coeff_; 59 | Twist transform_lb_; 60 | Eigen::Matrix info_mat_; 61 | 62 | // TODO: necessary? 63 | static Eigen::Matrix3d sqrt_info; 64 | static double sum_t; 65 | 66 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 67 | 68 | }; 69 | 70 | } // namespace lio 71 | 72 | #endif //LIO_POINTDISTANCEFACTOR_H_ 73 | -------------------------------------------------------------------------------- /include/factor/PlaneToPlaneFactor.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 5/9/18. 29 | // 30 | 31 | #ifndef LIO_PLANETOPLANEFACTOR_H_ 32 | #define LIO_PLANETOPLANEFACTOR_H_ 33 | 34 | #include 35 | #include 36 | 37 | #include "utils/CircularBuffer.h" 38 | #include "utils/Twist.h" 39 | #include "utils/common_ros.h" 40 | #include "utils/TicToc.h" 41 | #include "utils/math_utils.h" 42 | #include "utils/geometry_utils.h" 43 | 44 | #include "feature_manager/FeatureManager.h" 45 | 46 | namespace lio { 47 | 48 | using namespace mathutils; 49 | 50 | class PlaneToPlaneFactor : public ceres::SizedCostFunction<3, 7, 7, 7> { 51 | public: 52 | PlaneToPlaneFactor(const Eigen::Vector3d &pi_local, const Eigen::Vector3d &ni_local, 53 | const Eigen::Vector3d &pj_local, const Eigen::Vector3d &nj_local); 54 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 55 | 56 | void Check(double **parameters); 57 | 58 | Eigen::Vector3d pi_local_; 59 | Eigen::Vector3d ni_local_; 60 | Eigen::Vector3d pj_local_; 61 | Eigen::Vector3d nj_local_; 62 | // Eigen::Matrix3d mahalanobis_; 63 | 64 | PointNormalFeature pfi_; 65 | PointNormalFeature pfj_; 66 | 67 | const double eps_ = 1e-8; 68 | 69 | static double sum_t_; 70 | 71 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 72 | }; 73 | 74 | } 75 | 76 | #endif //LIO_PLANETOPLANEFACTOR_H_ 77 | -------------------------------------------------------------------------------- /test/test_point_processor/test_point_odometry.cc: -------------------------------------------------------------------------------- 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/15/18. 29 | // 30 | 31 | #include 32 | #include 33 | #include 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | 41 | #include 42 | 43 | #include "point_processor/PointOdometry.h" 44 | #include "utils/TicToc.h" 45 | 46 | using namespace lio; 47 | using namespace std; 48 | using namespace mathutils; 49 | 50 | static ros::Publisher test_pub; 51 | static ros::NodeHandlePtr nh_ptr; 52 | 53 | TEST(PointProcessorTest, ObjectTest) { 54 | PointOdometry odometry; 55 | odometry.SetupRos(*nh_ptr); 56 | odometry.Reset(); 57 | 58 | ros::Rate r(100); 59 | while (ros::ok()) { 60 | odometry.Process(); 61 | ros::spinOnce(); 62 | r.sleep(); 63 | } 64 | 65 | } 66 | 67 | int main(int argc, char **argv) { 68 | testing::InitGoogleTest(&argc, argv); 69 | 70 | google::InitGoogleLogging(argv[0]); 71 | google::ParseCommandLineFlags(&argc, &argv, true); 72 | 73 | ros::init(argc, argv, "test_point_odometry"); 74 | { 75 | ros::NodeHandle nh("~"); 76 | nh_ptr = boost::make_shared(nh); 77 | } 78 | 79 | FLAGS_alsologtostderr = true; 80 | 81 | return RUN_ALL_TESTS(); 82 | } -------------------------------------------------------------------------------- /test/test_point_processor/test_point_mapping.cc: -------------------------------------------------------------------------------- 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/25/18. 29 | // 30 | 31 | // 32 | // Created by hyye on 3/15/18. 33 | // 34 | 35 | #include 36 | #include 37 | #include 38 | 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | 45 | #include 46 | 47 | #include "point_processor/PointMapping.h" 48 | #include "utils/TicToc.h" 49 | 50 | using namespace lio; 51 | using namespace std; 52 | using namespace mathutils; 53 | 54 | static ros::NodeHandlePtr nh_ptr; 55 | 56 | TEST(PointMappingTest, ObjectTest) { 57 | PointMapping mapper; 58 | mapper.SetupRos(*nh_ptr); 59 | mapper.Reset(); 60 | 61 | ros::Rate r(100); 62 | while (ros::ok()) { 63 | mapper.Process(); 64 | ros::spinOnce(); 65 | r.sleep(); 66 | } 67 | 68 | } 69 | 70 | int main(int argc, char **argv) { 71 | testing::InitGoogleTest(&argc, argv); 72 | 73 | google::InitGoogleLogging(argv[0]); 74 | google::ParseCommandLineFlags(&argc, &argv, true); 75 | 76 | ros::init(argc, argv, "test_point_mapping"); 77 | { 78 | ros::NodeHandle nh("~"); 79 | nh_ptr = boost::make_shared(nh); 80 | } 81 | 82 | FLAGS_alsologtostderr = true; 83 | 84 | return RUN_ALL_TESTS(); 85 | } -------------------------------------------------------------------------------- /docker/1.Dockerfile: -------------------------------------------------------------------------------- 1 | FROM osrf/ros:melodic-desktop-full 2 | 3 | ARG your_name 4 | LABEL com.nvidia.volumes.needed="nvidia_driver" 5 | ENV PATH /usr/local/nvidia/bin:${PATH} 6 | ENV LD_LIBRARY_PATH /usr/local/nvidia/lib:/usr/local/nvidia/lib64:${LD_LIBRARY_PATH} 7 | 8 | RUN apt-get update \ 9 | && apt-get install -y \ 10 | wget \ 11 | lsb-release \ 12 | sudo \ 13 | mesa-utils \ 14 | && apt-get clean 15 | # see: https://gitlab.com/nvidia/opengl/tree/ubuntu18.04 16 | # RUN dpkg --add-architecture i386 && \ 17 | # apt-get update && apt-get install -y --no-install-recommends \ 18 | # libxau6 libxau6:i386 \ 19 | # libxdmcp6 libxdmcp6:i386 \ 20 | # libxcb1 libxcb1:i386 \ 21 | # libxext6 libxext6:i386 \ 22 | # libx11-6 libx11-6:i386 && \ 23 | # rm -rf /var/lib/apt/lists/* 24 | # 25 | # # nvidia-container-runtime 26 | # ENV NVIDIA_VISIBLE_DEVICES \ 27 | # ${NVIDIA_VISIBLE_DEVICES:-all} 28 | # ENV NVIDIA_DRIVER_CAPABILITIES \ 29 | # ${NVIDIA_DRIVER_CAPABILITIES:+$NVIDIA_DRIVER_CAPABILITIES,}graphics,compat32,utility 30 | # 31 | # RUN echo "/usr/local/nvidia/lib" >> /etc/ld.so.conf.d/nvidia.conf && \ 32 | # echo "/usr/local/nvidia/lib64" >> /etc/ld.so.conf.d/nvidia.conf 33 | # 34 | # # Required for non-glvnd setups. 35 | # ENV LD_LIBRARY_PATH /usr/lib/x86_64-linux-gnu:/usr/lib/i386-linux-gnu${LD_LIBRARY_PATH:+:${LD_LIBRARY_PATH}}:/usr/local/nvidia/lib:/usr/local/nvidia/lib64 36 | # 37 | # RUN apt-get update && apt-get install -y --no-install-recommends \ 38 | # libglvnd0 libglvnd0:i386 \ 39 | # libgl1 libgl1:i386 \ 40 | # libglx0 libglx0:i386 \ 41 | # libegl1 libegl1:i386 \ 42 | # libgles2 libgles2:i386 && \ 43 | # rm -rf /var/lib/apt/lists/* 44 | # 45 | # COPY --from=nvidia/opengl:1.0-glvnd-runtime-ubuntu18.04 \ 46 | # /usr/share/glvnd/egl_vendor.d/10_nvidia.json \ 47 | # /usr/share/glvnd/egl_vendor.d/10_nvidia.json 48 | # 49 | # Prerequisites 50 | # ENV USER=$your_name 51 | RUN adduser $your_name 52 | RUN curl http://ceres-solver.org/ceres-solver-1.14.0.tar.gz --output ceres-solver-1.14.0.tar.gz 53 | RUN apt-get update && apt-get install -y --no-install-recommends \ 54 | libgoogle-glog-dev libatlas-base-dev libeigen3-dev libsuitesparse-dev 55 | RUN tar zxf ceres-solver-1.14.0.tar.gz && mkdir ceres-bin && cd ceres-bin && cmake ../ceres-solver-1.14.0 && make -j6 && make install 56 | # USER $yourname 57 | # RUN mkdir -p /workspace && chmod -R 777 /workspace 58 | # COPY --chown=1000:1000 . /workspace/src/ 59 | COPY . /workspace/src/ 60 | RUN /bin/bash -c "source /opt/ros/melodic/setup.bash && cd /workspace && catkin_make -DCMAKE_BUILD_TYPE=Release" 61 | -------------------------------------------------------------------------------- /include/3rdparty/sophus/velocities.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SOPHUS_VELOCITIES_HPP 2 | #define SOPHUS_VELOCITIES_HPP 3 | 4 | #include 5 | 6 | #include "num_diff.hpp" 7 | #include "se3.hpp" 8 | 9 | namespace Sophus { 10 | namespace experimental { 11 | // Experimental since the API will certainly change drastically in the future. 12 | 13 | // Transforms velocity vector by rotation ``foo_R_bar``. 14 | // 15 | // Note: vel_bar can be either a linear or a rotational velocity vector. 16 | // 17 | template 18 | Vector3 transformVelocity(SO3 const& foo_R_bar, 19 | Vector3 const& vel_bar) { 20 | // For rotational velocities note that: 21 | // 22 | // vel_bar = vee(foo_R_bar * hat(vel_bar) * foo_R_bar^T) 23 | // = vee(hat(Adj(foo_R_bar) * vel_bar)) 24 | // = Adj(foo_R_bar) * vel_bar 25 | // = foo_R_bar * vel_bar. 26 | // 27 | return foo_R_bar * vel_bar; 28 | } 29 | 30 | // Transforms velocity vector by pose ``foo_T_bar``. 31 | // 32 | // Note: vel_bar can be either a linear or a rotational velocity vector. 33 | // 34 | template 35 | Vector3 transformVelocity(SE3 const& foo_T_bar, 36 | Vector3 const& vel_bar) { 37 | return transformVelocity(foo_T_bar.so3(), vel_bar); 38 | } 39 | 40 | // finite difference approximation of instantanious velocity in frame foo 41 | // 42 | template 43 | Vector3 finiteDifferenceRotationalVelocity( 44 | std::function(Scalar)> const& foo_R_bar, Scalar t, 45 | Scalar h = Constants::epsilon()) { 46 | // https://en.wikipedia.org/w/index.php?title=Angular_velocity&oldid=791867792#Angular_velocity_tensor 47 | // 48 | // W = dR(t)/dt * R^{-1}(t) 49 | Matrix3 dR_dt_in_frame_foo = curveNumDiff( 50 | [&foo_R_bar](Scalar t0) -> Matrix3 { 51 | return foo_R_bar(t0).matrix(); 52 | }, 53 | t, h); 54 | // velocity tensor 55 | Matrix3 W_in_frame_foo = 56 | dR_dt_in_frame_foo * (foo_R_bar(t)).inverse().matrix(); 57 | return SO3::vee(W_in_frame_foo); 58 | } 59 | 60 | // finite difference approximation of instantanious velocity in frame foo 61 | // 62 | template 63 | Vector3 finiteDifferenceRotationalVelocity( 64 | std::function(Scalar)> const& foo_T_bar, Scalar t, 65 | Scalar h = Constants::epsilon()) { 66 | return finiteDifferenceRotationalVelocity( 67 | [&foo_T_bar](Scalar t) -> SO3 { return foo_T_bar(t).so3(); }, t, 68 | h); 69 | } 70 | 71 | } // namespace experimental 72 | } // namespace Sophus 73 | 74 | #endif // SOPHUS_VELOCITIES_HPP 75 | -------------------------------------------------------------------------------- /src/processor_node.cc: -------------------------------------------------------------------------------- 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/15/18. 29 | // 30 | 31 | #include 32 | #include 33 | #include 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | 41 | #include 42 | 43 | #include "point_processor/PointProcessor.h" 44 | #include "utils/TicToc.h" 45 | 46 | using namespace lio; 47 | using namespace std; 48 | using namespace mathutils; 49 | 50 | 51 | int main(int argc, char **argv) { 52 | 53 | google::InitGoogleLogging(argv[0]); 54 | FLAGS_alsologtostderr = true; 55 | ros::init(argc, argv, "point_processor"); 56 | 57 | ros::NodeHandle nh("~"); 58 | 59 | int sensor_type; 60 | double rad_diff; 61 | bool infer_start_ori; 62 | nh.param("sensor_type", sensor_type, 16); 63 | nh.param("rad_diff", rad_diff, 0.2); 64 | nh.param("infer_start_ori", infer_start_ori, false); 65 | 66 | PointProcessor processor; // Default sensor_type is 16 67 | 68 | if (sensor_type == 32) { 69 | processor = PointProcessor(-30.67f, 10.67f, 32); 70 | } else if (sensor_type == 64) { 71 | processor = PointProcessor(-24.9f, 2, 64); 72 | } else if (sensor_type == 320) { 73 | processor = PointProcessor(-25, 15, 32, true); 74 | } 75 | 76 | PointProcessorConfig config; 77 | config.rad_diff = rad_diff; 78 | config.infer_start_ori_ = infer_start_ori; 79 | processor.SetupConfig(config); 80 | 81 | LOG(INFO) << "Sensor type: " << processor.laser_scans.size(); 82 | 83 | processor.SetupRos(nh); 84 | 85 | ros::Rate r(100); 86 | while (ros::ok()) { 87 | ros::spinOnce(); 88 | r.sleep(); 89 | } 90 | 91 | return 0; 92 | } -------------------------------------------------------------------------------- /include/3rdparty/sophus/rotation_matrix.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SOPHUS_ROTATION_MATRIX_HPP 2 | #define SOPHUS_ROTATION_MATRIX_HPP 3 | 4 | #include 5 | #include 6 | 7 | #include "types.hpp" 8 | 9 | namespace Sophus { 10 | 11 | // Takes in arbiray square matrix and returns true if it is 12 | // orthogonal. 13 | template 14 | SOPHUS_FUNC bool isOrthogonal(Eigen::MatrixBase const& R) { 15 | using Scalar = typename D::Scalar; 16 | static int const N = D::RowsAtCompileTime; 17 | static int const M = D::ColsAtCompileTime; 18 | 19 | static_assert(N == M, "must be a square matrix"); 20 | static_assert(N >= 2, "must have compile time dimension >= 2"); 21 | 22 | return (R * R.transpose() - Matrix::Identity()).norm() < 23 | Constants::epsilon(); 24 | } 25 | 26 | // Takes in arbiray square matrix and returns true if it is 27 | // "scaled-orthogonal" with positive determinant. 28 | // 29 | template 30 | SOPHUS_FUNC bool isScaledOrthogonalAndPositive(Eigen::MatrixBase const& sR) { 31 | using Scalar = typename D::Scalar; 32 | static int const N = D::RowsAtCompileTime; 33 | static int const M = D::ColsAtCompileTime; 34 | using std::pow; 35 | using std::sqrt; 36 | 37 | Scalar det = sR.determinant(); 38 | 39 | if (det <= Scalar(0)) { 40 | return false; 41 | } 42 | 43 | Scalar scale_sqr = pow(det, Scalar(2. / N)); 44 | 45 | static_assert(N == M, "must be a square matrix"); 46 | static_assert(N >= 2, "must have compile time dimension >= 2"); 47 | 48 | return (sR * sR.transpose() - scale_sqr * Matrix::Identity()) 49 | .template lpNorm() < 50 | sqrt(Constants::epsilon()); 51 | } 52 | 53 | // Takes in arbiray square matrix (2x2 or larger) and returns closest 54 | // orthogonal matrix with positive determinant. 55 | template 56 | SOPHUS_FUNC enable_if_t< 57 | std::is_floating_point::value, 58 | Matrix> 59 | makeRotationMatrix(Eigen::MatrixBase const& R) { 60 | using Scalar = typename D::Scalar; 61 | static int const N = D::RowsAtCompileTime; 62 | static int const M = D::ColsAtCompileTime; 63 | 64 | static_assert(N == M, "must be a square matrix"); 65 | static_assert(N >= 2, "must have compile time dimension >= 2"); 66 | 67 | Eigen::JacobiSVD> svd( 68 | R, Eigen::ComputeFullU | Eigen::ComputeFullV); 69 | 70 | // Determine determinant of orthogonal matrix U*V'. 71 | Scalar d = (svd.matrixU() * svd.matrixV().transpose()).determinant(); 72 | // Starting from the identity matrix D, set the last entry to d (+1 or 73 | // -1), so that det(U*D*V') = 1. 74 | Matrix Diag = Matrix::Identity(); 75 | Diag(N - 1, N - 1) = d; 76 | return svd.matrixU() * Diag * svd.matrixV().transpose(); 77 | } 78 | 79 | } // namespace Sophus 80 | 81 | #endif // SOPHUS_ROTATION_MATRIX_HPP 82 | -------------------------------------------------------------------------------- /scripts/transform_monitor.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import numpy as np 3 | import rospy 4 | from nav_msgs.msg import Odometry 5 | from geometry_msgs.msg import Twist, Quaternion, Vector3, Vector3Stamped 6 | from tf.transformations import euler_from_quaternion, quaternion_matrix, quaternion_from_matrix, euler_from_quaternion, euler_from_matrix, euler_matrix 7 | 8 | 9 | class transform_monitor(object): 10 | def __init__(self): 11 | rospy.init_node('transform_monitor') 12 | self.laser_odom = rospy.get_param('~laser_odom', default='/local_laser_odom/') 13 | self.imu_odom = rospy.get_param('~imu_odom', default='/predict_odom') 14 | 15 | def get_odom(self, odom_msg): 16 | pos = odom_msg.pose.pose.position 17 | quat = odom_msg.pose.pose.orientation 18 | explicit_pos = [pos.x, pos.y, pos.z] 19 | explicit_quat = [quat.x, quat.y, quat.z, quat.w] 20 | 21 | transform_world_laser = quaternion_matrix(explicit_quat) 22 | transform_world_laser[0:3, 3] = explicit_pos 23 | euler_angles = euler_from_quaternion(explicit_quat, 'rzyx') 24 | z = euler_angles[0] 25 | y = euler_angles[1] 26 | x = euler_angles[2] 27 | 28 | v = Vector3Stamped() 29 | v.header = odom_msg.header 30 | v.vector.x = x 31 | v.vector.y = y 32 | v.vector.z = z 33 | 34 | self.imu_rot_pub.publish(v) 35 | 36 | # print('imu_rot_y', y) 37 | 38 | def get_laser_odom(self, odom_msg): 39 | pos = odom_msg.pose.pose.position 40 | quat = odom_msg.pose.pose.orientation 41 | explicit_pos = [pos.x, pos.y, pos.z] 42 | explicit_quat = [quat.x, quat.y, quat.z, quat.w] 43 | 44 | transform_world_laser = quaternion_matrix(explicit_quat) 45 | transform_world_laser[0:3, 3] = explicit_pos 46 | euler_angles = euler_from_quaternion(explicit_quat, 'rzyx') 47 | z = euler_angles[0] 48 | y = euler_angles[1] 49 | x = euler_angles[2] 50 | 51 | v = Vector3Stamped() 52 | v.header = odom_msg.header 53 | v.vector.x = x 54 | v.vector.y = y 55 | v.vector.z = z 56 | 57 | self.laser_rot_pub.publish(v) 58 | 59 | # print('laser_rot_y', y) 60 | 61 | def main(self): 62 | r = rospy.Rate(1000) 63 | self.odom_sub = rospy.Subscriber(self.imu_odom, Odometry, self.get_odom) 64 | self.laser_odom_sub = rospy.Subscriber(self.laser_odom, Odometry, self.get_laser_odom) 65 | print('self.imu_odom', self.imu_odom) 66 | print('self.laser_odom', self.laser_odom) 67 | self.imu_rot_pub = rospy.Publisher('/debug/imu_rot', Vector3Stamped, queue_size=10) 68 | self.laser_rot_pub = rospy.Publisher('/debug/laser_rot', Vector3Stamped, queue_size=10) 69 | while not rospy.is_shutdown(): 70 | r.sleep() 71 | 72 | 73 | if __name__ == '__main__': 74 | monitor = transform_monitor() 75 | try: 76 | monitor.main() 77 | except rospy.ROSException: 78 | print 'Interrupted' 79 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # LIO-mapping 2 | ## A Tightly Coupled 3D Lidar and Inertial Odometry and Mapping Approach 3 | #### Authors: Haoyang Ye, Yuying Chen, and Ming Liu from [RAM-LAB](https://ram-lab.com/). 4 | [[Paper]](https://arxiv.org/abs/1904.06993), [[Project]](https://sites.google.com/view/lio-mapping), [[Bib]](https://ram-lab.com/papers/2019/icra_2019_ye.bib). ICRA 2019. 5 | ## Table of Contents 6 | * [Demo Results](#demo-results) 7 | * [Prerequisites](#prerequisites) 8 | * [Build](#build) 9 | * [Examples](#examples) 10 | * [Docker](#docker) 11 | * [Credits](#credits) 12 | * [Licence](#licence) 13 | 14 | ## Demo Results 15 | lio-mapping-gt 16 | lio-mapping-indoor 17 | 18 | Video: [[More indoor and outdoor tests]](https://ram-lab.com/file/hyye/lio-mapping.mp4). 19 | 20 | ## Prerequisites 21 | See [Dockerfile](docker/Dockerfile) as a reference: 22 | 1. [ROS](http://wiki.ros.org/melodic/Installation) with Ubuntu 18.04 or Ubuntu 16.04. 23 | 2. [Ceres-solver](http://ceres-solver.org/installation.html#linux). 24 | 3. [PCL](http://www.pointclouds.org/downloads/), the default version accompanying by ROS. 25 | 4. [OpenCV](https://docs.opencv.org/master/d7/d9f/tutorial_linux_install.html), the default version accompanying by ROS. 26 | 27 | ## Build 28 | 1. `git clone git@github.com:hyye/lio-mapping.git` into the `src` folder of your catkin workspace. 29 | 2. `catkin build -DCMAKE_BUILD_TYPE=Release lio` or `catkin_make -DCMAKE_BUILD_TYPE=Release`. 30 | 31 | ## Examples 32 | Some [sample data](https://drive.google.com/drive/folders/1dPy667dAnJy9wgXmlnRgQZxQF_ESuve3). 33 | 1. `source devel/setup.zsh`, or `setup.bash` if your prefer `bash`. 34 | 2. `roslaunch lio test_indoor.launch &`. 35 | 3. `roslaunch lio map_4D_indoor.launch &`. 36 | 4. `rosbag play fast1.bag`. 37 | 38 | ## Docker 39 | Try it out using docker: 40 | 1. Run `docker/build_docker.sh`. 41 | 2. Run `docker/run_docker.sh`. 42 | 3. Run `rosbag play fast1.bag`, in your host machine or in the running container. 43 | 44 | Note: Visualization (rviz) can run in the running container with [nvidia-docker](https://github.com/NVIDIA/nvidia-docker). The [Dockerfile](docker/Dockerfile) is compatible with [nvidia-docker 2.0](https://github.com/nvidia/nvidia-docker/wiki/Installation-(version-2.0)); [1.Dockerfile](docker/1.Dockerfile) with [nvidia-docker 1.0](https://github.com/nvidia/nvidia-docker/wiki/Installation-(version-1.0)). 45 | 46 | ## Credits 47 | The feature extraction, lidar-only odometry and baseline implemented were heavily derived or taken from the original [LOAM](http://wiki.ros.org/loam_velodyne) and its [modified version](https://github.com/laboshinl/loam_velodyne) (the point_processor in our project), and one of the initialization methods and the optimization pipeline from [VINS-mono](https://github.com/HKUST-Aerial-Robotics/VINS-Mono). The copyright headers are retained for the relevant files. 48 | 49 | ## Licence 50 | The source code is released under [GPL-3.0](https://www.gnu.org/licenses/). 51 | -------------------------------------------------------------------------------- /include/3rdparty/sophus/interpolate_details.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SOPHUS_INTERPOLATE_DETAILS_HPP 2 | #define SOPHUS_INTERPOLATE_DETAILS_HPP 3 | 4 | #include "rxso2.hpp" 5 | #include "rxso3.hpp" 6 | #include "se2.hpp" 7 | #include "se3.hpp" 8 | #include "sim2.hpp" 9 | #include "sim3.hpp" 10 | #include "so2.hpp" 11 | #include "so3.hpp" 12 | 13 | namespace Sophus { 14 | namespace interp_details { 15 | 16 | template 17 | struct Traits; 18 | 19 | template 20 | struct Traits> { 21 | static bool constexpr supported = true; 22 | 23 | static bool hasShortestPathAmbiguity(SO2 const& foo_T_bar) { 24 | using std::abs; 25 | Scalar angle = foo_T_bar.log(); 26 | return abs(abs(angle) - Constants::pi()) < 27 | Constants::epsilon(); 28 | } 29 | }; 30 | 31 | template 32 | struct Traits> { 33 | static bool constexpr supported = true; 34 | 35 | static bool hasShortestPathAmbiguity(RxSO2 const& foo_T_bar) { 36 | return Traits>::hasShortestPathAmbiguity(foo_T_bar.so2()); 37 | } 38 | }; 39 | 40 | template 41 | struct Traits> { 42 | static bool constexpr supported = true; 43 | 44 | static bool hasShortestPathAmbiguity(SO3 const& foo_T_bar) { 45 | using std::abs; 46 | Scalar angle = foo_T_bar.logAndTheta().theta; 47 | return abs(abs(angle) - Constants::pi()) < 48 | Constants::epsilon(); 49 | } 50 | }; 51 | 52 | template 53 | struct Traits> { 54 | static bool constexpr supported = true; 55 | 56 | static bool hasShortestPathAmbiguity(RxSO3 const& foo_T_bar) { 57 | return Traits>::hasShortestPathAmbiguity(foo_T_bar.so3()); 58 | } 59 | }; 60 | 61 | template 62 | struct Traits> { 63 | static bool constexpr supported = true; 64 | 65 | static bool hasShortestPathAmbiguity(SE2 const& foo_T_bar) { 66 | return Traits>::hasShortestPathAmbiguity(foo_T_bar.so2()); 67 | } 68 | }; 69 | 70 | template 71 | struct Traits> { 72 | static bool constexpr supported = true; 73 | 74 | static bool hasShortestPathAmbiguity(SE3 const& foo_T_bar) { 75 | return Traits>::hasShortestPathAmbiguity(foo_T_bar.so3()); 76 | } 77 | }; 78 | 79 | template 80 | struct Traits> { 81 | static bool constexpr supported = true; 82 | 83 | static bool hasShortestPathAmbiguity(Sim2 const& foo_T_bar) { 84 | return Traits>::hasShortestPathAmbiguity( 85 | foo_T_bar.rxso2().so2()); 86 | ; 87 | } 88 | }; 89 | 90 | template 91 | struct Traits> { 92 | static bool constexpr supported = true; 93 | 94 | static bool hasShortestPathAmbiguity(Sim3 const& foo_T_bar) { 95 | return Traits>::hasShortestPathAmbiguity( 96 | foo_T_bar.rxso3().so3()); 97 | ; 98 | } 99 | }; 100 | 101 | } // namespace details 102 | } // namespace Sophus 103 | 104 | #endif // SOPHUS_INTERPOLATE_DETAILS_HPP 105 | -------------------------------------------------------------------------------- /include/3rdparty/sophus/num_diff.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SOPHUS_NUM_DIFF_HPP 2 | #define SOPHUS_NUM_DIFF_HPP 3 | // Numerical differentiation using finite differences 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include "types.hpp" 10 | 11 | namespace Sophus { 12 | 13 | namespace details { 14 | template 15 | class Curve { 16 | public: 17 | template 18 | static auto num_diff(Fn curve, Scalar t, Scalar h) -> decltype(curve(t)) { 19 | using ReturnType = decltype(curve(t)); 20 | static_assert(std::is_floating_point::value, 21 | "Scalar must be a floating point type."); 22 | static_assert(IsFloatingPoint::value, 23 | "ReturnType must be either a floating point scalar, " 24 | "vector or matrix."); 25 | 26 | return (curve(t + h) - curve(t - h)) / (Scalar(2) * h); 27 | } 28 | }; 29 | 30 | template 31 | class VectorField { 32 | public: 33 | static Eigen::Matrix num_diff( 34 | std::function(Sophus::Vector)> 35 | vector_field, 36 | Sophus::Vector const& a, Scalar eps) { 37 | static_assert(std::is_floating_point::value, 38 | "Scalar must be a floating point type."); 39 | Eigen::Matrix J; 40 | Sophus::Vector h; 41 | h.setZero(); 42 | for (int i = 0; i < M; ++i) { 43 | h[i] = eps; 44 | J.row(i) = 45 | (vector_field(a + h) - vector_field(a - h)) / (Scalar(2) * eps); 46 | h[i] = Scalar(0); 47 | } 48 | 49 | return J; 50 | } 51 | }; 52 | 53 | template 54 | class VectorField { 55 | public: 56 | static Eigen::Matrix num_diff( 57 | std::function(Scalar)> vector_field, 58 | Scalar const& a, Scalar eps) { 59 | return details::Curve::num_diff(std::move(vector_field), a, eps); 60 | } 61 | }; 62 | } // namespace details 63 | 64 | // Calculates the derivative of a curve at a point ``t``. 65 | // 66 | // Here, a curve is a function from a Scalar to a Euclidean space. Thus, it 67 | // returns either a Scalar, a vector or a matrix. 68 | // 69 | template 70 | auto curveNumDiff(Fn curve, Scalar t, 71 | Scalar h = Constants::epsilonSqrt()) 72 | -> decltype(details::Curve::num_diff(std::move(curve), t, h)) { 73 | return details::Curve::num_diff(std::move(curve), t, h); 74 | } 75 | 76 | // Calculates the derivative of a vector field at a point ``a``. 77 | // 78 | // Here, a vector field is a function from a vector space to another vector 79 | // space. 80 | // 81 | template 82 | Eigen::Matrix vectorFieldNumDiff( 83 | Fn vector_field, ScalarOrVector const& a, 84 | Scalar eps = Constants::epsilonSqrt()) { 85 | return details::VectorField::num_diff(std::move(vector_field), 86 | a, eps); 87 | } 88 | 89 | } // namespace Sophus 90 | 91 | #endif // SOPHUS_NUM_DIFF_HPP 92 | -------------------------------------------------------------------------------- /include/imu_processor/ImuInitializer.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 5/20/18. 29 | // 30 | 31 | #ifndef LIO_IMUINITIALIZER_H_ 32 | #define LIO_IMUINITIALIZER_H_ 33 | 34 | #include 35 | 36 | #include 37 | #include 38 | #include 39 | 40 | #include "imu_processor/IntegrationBase.h" 41 | 42 | #include "utils/Twist.h" 43 | #include "utils/common_ros.h" 44 | #include "utils/TicToc.h" 45 | #include "utils/math_utils.h" 46 | #include "utils/geometry_utils.h" 47 | #include "utils/CircularBuffer.h" 48 | #include "3rdparty/sophus/se3.hpp" 49 | 50 | namespace lio { 51 | 52 | using namespace std; 53 | using namespace mathutils; 54 | using namespace geometryutils; 55 | 56 | using Eigen::VectorXd; 57 | 58 | typedef Twist Transform; 59 | 60 | struct LaserTransform { 61 | LaserTransform() {}; 62 | LaserTransform(double laser_time, Transform laser_transform) : time{laser_time}, transform{laser_transform} {}; 63 | 64 | double time; 65 | Transform transform; 66 | shared_ptr pre_integration; 67 | 68 | public: 69 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 70 | 71 | }; 72 | 73 | typedef pair PairTimeLaserTransform; 74 | 75 | class ImuInitializer { 76 | 77 | public: 78 | 79 | ///< R_WI is the rotation from the inertial frame into Lidar's world frame 80 | static bool Initialization(CircularBuffer &all_laser_transforms, 81 | CircularBuffer &Vs, 82 | CircularBuffer &Bas, 83 | CircularBuffer &Bgs, 84 | Vector3d &g, 85 | Transform &transform_lb, 86 | Matrix3d &R_WI); 87 | 88 | static bool EstimateExtrinsicRotation(CircularBuffer &all_laser_transforms, 89 | Transform &transform_lb); 90 | 91 | }; 92 | 93 | } 94 | 95 | #endif //LIO_IMUINITIALIZER_H_ 96 | -------------------------------------------------------------------------------- /include/imu_processor/ImuInitializer_viorb.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/26/18. 29 | // 30 | 31 | #ifndef LIO_IMUINITIALIZER_H_ 32 | #define LIO_IMUINITIALIZER_H_ 33 | 34 | #include 35 | 36 | #include 37 | #include 38 | #include 39 | 40 | #include "imu_processor/IntegrationBase.h" 41 | 42 | #include "utils/Twist.h" 43 | #include "utils/common_ros.h" 44 | #include "utils/TicToc.h" 45 | #include "utils/math_utils.h" 46 | #include "utils/geometry_utils.h" 47 | #include "utils/CircularBuffer.h" 48 | #include "3rdparty/sophus/se3.hpp" 49 | 50 | namespace lio { 51 | 52 | using namespace std; 53 | using namespace mathutils; 54 | using namespace geometryutils; 55 | 56 | using Eigen::VectorXd; 57 | 58 | typedef Twist Transform; 59 | 60 | struct LaserTransform { 61 | LaserTransform() {}; 62 | LaserTransform(double laser_time, Transform laser_transform) : time{laser_time}, transform{laser_transform} {}; 63 | 64 | double time; 65 | Transform transform; 66 | shared_ptr pre_integration; 67 | 68 | public: 69 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 70 | 71 | }; 72 | 73 | typedef pair PairTimeLaserTransform; 74 | 75 | class ImuInitializer { 76 | 77 | public: 78 | 79 | ///< R_WI is the rotation from the inertial frame into Lidar's world frame 80 | static bool Initialization(CircularBuffer &all_laser_transforms, 81 | CircularBuffer &Vs, 82 | CircularBuffer &Bas, 83 | CircularBuffer &Bgs, 84 | Vector3d &g, 85 | Transform &transform_lb, 86 | Matrix3d &R_WI); 87 | 88 | static bool EstimateExtrinsicRotation(CircularBuffer &all_laser_transforms, 89 | Transform &transform_lb); 90 | 91 | }; 92 | 93 | } 94 | 95 | #endif //LIO_IMUINITIALIZER_H_ 96 | -------------------------------------------------------------------------------- /include/utils/Twist.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/23/18. 29 | // 30 | 31 | #ifndef LIO_TWIST_H_ 32 | #define LIO_TWIST_H_ 33 | 34 | #include 35 | #include 36 | 37 | namespace lio { 38 | 39 | template 40 | struct Twist { 41 | Eigen::Quaternion rot; 42 | Eigen::Matrix pos; 43 | 44 | static Twist Identity() { 45 | return Twist(); 46 | } 47 | 48 | Twist() { 49 | rot.setIdentity(); 50 | pos.setZero(); 51 | } 52 | 53 | Twist(Eigen::Quaternion rot_in, Eigen::Matrix pos_in) { 54 | this->rot = rot_in; 55 | this->pos = pos_in; 56 | } 57 | 58 | Twist(Eigen::Transform transform) { 59 | this->rot = Eigen::Quaternion{transform.linear()}.normalized(); 60 | this->pos = transform.translation(); 61 | } 62 | 63 | Eigen::Transform transform() const { 64 | Eigen::Transform transform; 65 | transform.linear() = rot.normalized().toRotationMatrix(); 66 | transform.translation() = pos; 67 | return transform; 68 | } 69 | 70 | Twist inverse() const { 71 | Eigen::Transform transform_inv = this->transform().inverse(); 72 | Twist twist_inv; 73 | twist_inv.rot = transform_inv.linear(); 74 | twist_inv.pos = transform_inv.translation(); 75 | return twist_inv; 76 | } 77 | 78 | Twist operator*(const Twist &other) const { 79 | Eigen::Transform transform_out = this->transform() * other.transform(); 80 | return Twist(transform_out); 81 | } 82 | 83 | template 84 | Twist cast() const { 85 | Twist twist_new{this->rot.template cast(), this->pos.template cast()}; 86 | return twist_new; 87 | } 88 | 89 | friend std::ostream &operator<<(std::ostream &os, const Twist &twist) { 90 | os << twist.pos.x() << " " << twist.pos.y() << " " << twist.pos.z() << " " << twist.rot.w() << " " << twist.rot.x() 91 | << " " << twist.rot.y() << " " << twist.rot.z(); 92 | return os; 93 | } 94 | 95 | public: 96 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 97 | }; // class Twist 98 | 99 | } // namespace lio 100 | 101 | #endif //LIO_TWIST_H_ 102 | -------------------------------------------------------------------------------- /include/utils/LoadVirtual.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 5/12/18. 29 | // 30 | 31 | #ifndef LIO_LOADVIRTUAL_H_ 32 | #define LIO_LOADVIRTUAL_H_ 33 | 34 | #include 35 | 36 | #include "utils/CircularBuffer.h" 37 | 38 | namespace lio_test { 39 | 40 | struct MotionData { 41 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 42 | double timestamp; 43 | Eigen::Matrix3d Rwb; 44 | Eigen::Vector3d twb; 45 | Eigen::Vector3d imu_acc; 46 | Eigen::Vector3d imu_gyro; 47 | 48 | Eigen::Vector3d imu_gyro_bias; 49 | Eigen::Vector3d imu_acc_bias; 50 | 51 | Eigen::Vector3d imu_velocity; 52 | }; 53 | 54 | void LoadPoseVel(std::string filename, std::vector &pose) { 55 | 56 | std::ifstream f; 57 | f.open(filename.c_str()); 58 | 59 | if (!f.is_open()) { 60 | std::cerr << " can't open LoadFeatures file " << std::endl; 61 | return; 62 | } 63 | 64 | while (!f.eof()) { 65 | 66 | std::string s; 67 | std::getline(f, s); 68 | 69 | if (!s.empty()) { 70 | std::stringstream ss; 71 | ss << s; 72 | 73 | MotionData data; 74 | double time; 75 | Eigen::Quaterniond q; 76 | Eigen::Vector3d t; 77 | Eigen::Vector3d gyro; 78 | Eigen::Vector3d acc; 79 | 80 | Eigen::Vector3d vel; 81 | Eigen::Vector3d acc_bias; 82 | Eigen::Vector3d gyro_bias; 83 | 84 | ss >> time; 85 | ss >> q.w(); 86 | ss >> q.x(); 87 | ss >> q.y(); 88 | ss >> q.z(); 89 | ss >> t(0); 90 | ss >> t(1); 91 | ss >> t(2); 92 | ss >> vel(0); 93 | ss >> vel(1); 94 | ss >> vel(2); 95 | ss >> gyro(0); 96 | ss >> gyro(1); 97 | ss >> gyro(2); 98 | ss >> acc(0); 99 | ss >> acc(1); 100 | ss >> acc(2); 101 | ss >> acc_bias(0); 102 | ss >> acc_bias(1); 103 | ss >> acc_bias(2); 104 | ss >> gyro_bias(0); 105 | ss >> gyro_bias(1); 106 | ss >> gyro_bias(2); 107 | 108 | data.timestamp = time; 109 | data.imu_gyro = gyro; 110 | data.imu_acc = acc; 111 | data.twb = t; 112 | data.Rwb = Eigen::Matrix3d(q); 113 | 114 | data.imu_velocity = vel; 115 | data.imu_acc_bias = acc_bias; 116 | data.imu_gyro_bias = gyro_bias; 117 | 118 | pose.push_back(data); 119 | 120 | } 121 | } 122 | } 123 | 124 | } 125 | 126 | #endif //LIO_LOADVIRTUAL_H_ 127 | -------------------------------------------------------------------------------- /test/test_imu_processor/test_circular_buffer.cc: -------------------------------------------------------------------------------- 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/27/18. 29 | // 30 | 31 | 32 | #include 33 | #include 34 | #include 35 | #include "imu_processor/Estimator.h" 36 | #include "utils/TicToc.h" 37 | 38 | #include "utils/CircularBuffer.h" 39 | 40 | using namespace lio; 41 | using namespace std; 42 | using namespace mathutils; 43 | 44 | TEST(CirclurBufferTest, ForLoopTest) { 45 | 46 | CircularBuffer > pre_integrations{5 + 1}; 47 | 48 | for (int i = 0; i < 10; ++i) { 49 | Eigen::Vector3d acc_last_(i, i, i); 50 | Eigen::Vector3d gyr_last_(i + 1, i + 1, i + 1); 51 | Eigen::Vector3d Bas_(i + 2, i + 2, i + 2); 52 | Eigen::Vector3d Bgs_(i + 3, i + 3, i + 3); 53 | pre_integrations.push(std::make_shared(IntegrationBase(acc_last_, gyr_last_, Bas_, Bgs_))); 54 | } 55 | 56 | for (int i = 0; i < pre_integrations.size(); ++i) { 57 | DLOG(INFO) << pre_integrations[i]->acc0_.transpose(); 58 | } 59 | 60 | } 61 | 62 | TEST(CirclurBufferTest, SharedPtrTest) { 63 | 64 | // CircularBuffer > pre_integrations = CircularBuffer >(5 + 1); 65 | CircularBuffer > pre_integrations{5 + 1}; 66 | 67 | for (int i = 0; i < 10; ++i) { 68 | Eigen::Vector3d acc_last_(i, i, i); 69 | Eigen::Vector3d gyr_last_(i + 1, i + 1, i + 1); 70 | Eigen::Vector3d Bas_(i + 2, i + 2, i + 2); 71 | Eigen::Vector3d Bgs_(i + 3, i + 3, i + 3); 72 | pre_integrations.push(std::make_shared(IntegrationBase(acc_last_, gyr_last_, Bas_, Bgs_))); 73 | } 74 | 75 | DLOG(INFO) << pre_integrations.size(); 76 | DLOG(INFO) << pre_integrations[0]->acc0_.transpose(); 77 | DLOG(INFO) << pre_integrations[pre_integrations.size() - 1]->acc0_.transpose(); 78 | 79 | for (int j = 0; j < 5; ++j) { 80 | pre_integrations[j].reset(); 81 | } 82 | 83 | for (int k = 0; k < pre_integrations.size() * 2; ++k) { 84 | DLOG(INFO) << (pre_integrations[k] != nullptr ? "not nullptr" : "nullptr"); 85 | } 86 | 87 | CircularBuffer > vec_buf{5 + 1}; 88 | 89 | for (int l = 0; l < 6; ++l) { 90 | vec_buf.push(vector()); 91 | } 92 | 93 | for (int l = 0; l < 6; ++l) { 94 | DLOG(INFO) << vec_buf[l].size(); 95 | } 96 | 97 | } 98 | 99 | int main(int argc, char **argv) { 100 | testing::InitGoogleTest(&argc, argv); 101 | 102 | google::InitGoogleLogging(argv[0]); 103 | google::ParseCommandLineFlags(&argc, &argv, true); 104 | 105 | FLAGS_alsologtostderr = true; 106 | 107 | return RUN_ALL_TESTS(); 108 | } -------------------------------------------------------------------------------- /include/3rdparty/sophus/sim_details.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SOPHUS_SIM_DETAILS_HPP 2 | #define SOPHUS_SIM_DETAILS_HPP 3 | 4 | #include "types.hpp" 5 | 6 | namespace Sophus { 7 | namespace details { 8 | 9 | template 10 | Matrix calcW(Matrix const& Omega, 11 | Scalar const theta, Scalar const sigma) { 12 | using std::abs; 13 | using std::exp; 14 | using std::sin; 15 | using std::cos; 16 | static Matrix const I = Matrix::Identity(); 17 | static Scalar const one(1); 18 | static Scalar const half(0.5); 19 | Matrix const Omega2 = Omega * Omega; 20 | Scalar const scale = exp(sigma); 21 | Scalar A, B, C; 22 | if (abs(sigma) < Constants::epsilon()) { 23 | C = one; 24 | if (abs(theta) < Constants::epsilon()) { 25 | A = half; 26 | B = Scalar(1. / 6.); 27 | } else { 28 | Scalar theta_sq = theta * theta; 29 | A = (one - cos(theta)) / theta_sq; 30 | B = (theta - sin(theta)) / (theta_sq * theta); 31 | } 32 | } else { 33 | C = (scale - one) / sigma; 34 | if (abs(theta) < Constants::epsilon()) { 35 | Scalar sigma_sq = sigma * sigma; 36 | A = ((sigma - one) * scale + one) / sigma_sq; 37 | B = (scale * half * sigma_sq + scale - one - sigma * scale) / 38 | (sigma_sq * sigma); 39 | } else { 40 | Scalar theta_sq = theta * theta; 41 | Scalar a = scale * sin(theta); 42 | Scalar b = scale * cos(theta); 43 | Scalar c = theta_sq + sigma * sigma; 44 | A = (a * sigma + (one - b) * theta) / (theta * c); 45 | B = (C - ((b - one) * sigma + a * theta) / (c)) * one / (theta_sq); 46 | } 47 | } 48 | return A * Omega + B * Omega2 + C * I; 49 | } 50 | 51 | template 52 | Matrix calcWInv(Matrix const& Omega, 53 | Scalar const theta, Scalar const sigma, 54 | Scalar const scale) { 55 | using std::abs; 56 | using std::sin; 57 | using std::cos; 58 | static Matrix const I = Matrix::Identity(); 59 | static Scalar const half(0.5); 60 | static Scalar const one(1); 61 | static Scalar const two(2); 62 | Matrix const Omega2 = Omega * Omega; 63 | Scalar const scale_sq = scale * scale; 64 | Scalar const theta_sq = theta * theta; 65 | Scalar const sin_theta = sin(theta); 66 | Scalar const cos_theta = cos(theta); 67 | 68 | Scalar a, b, c; 69 | if (abs(sigma * sigma) < Constants::epsilon()) { 70 | c = one - half * sigma; 71 | a = -half; 72 | if (abs(theta_sq) < Constants::epsilon()) { 73 | b = Scalar(1. / 12.); 74 | } else { 75 | b = (theta * sin_theta + two * cos_theta - two) / 76 | (two * theta_sq * (cos_theta - one)); 77 | } 78 | } else { 79 | Scalar const scale_cu = scale_sq * scale; 80 | c = sigma / (scale - one); 81 | if (abs(theta_sq) < Constants::epsilon()) { 82 | a = (-sigma * scale + scale - one) / ((scale - one) * (scale - one)); 83 | b = (scale_sq * sigma - two * scale_sq + scale * sigma + two * scale) / 84 | (two * scale_cu - Scalar(6) * scale_sq + Scalar(6) * scale - two); 85 | } else { 86 | Scalar const s_sin_theta = scale * sin_theta; 87 | Scalar const s_cos_theta = scale * cos_theta; 88 | a = (theta * s_cos_theta - theta - sigma * s_sin_theta) / 89 | (theta * (scale_sq - two * s_cos_theta + one)); 90 | b = -scale * 91 | (theta * s_sin_theta - theta * sin_theta + sigma * s_cos_theta - 92 | scale * sigma + sigma * cos_theta - sigma) / 93 | (theta_sq * (scale_cu - two * scale * s_cos_theta - scale_sq + 94 | two * s_cos_theta + scale - one)); 95 | } 96 | } 97 | return a * Omega + b * Omega2 + c * I; 98 | } 99 | 100 | } // details 101 | } // Sophus 102 | 103 | #endif 104 | -------------------------------------------------------------------------------- /src/input_filters_node.cc: -------------------------------------------------------------------------------- 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 18-6-13. 29 | // 30 | 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | using namespace lio; 37 | 38 | static ros::Publisher pub_filtered_cloud; 39 | static Eigen::Affine3f transform_to_world; 40 | static tf::TransformBroadcaster *br_ptr; 41 | static tf::Transform tf_transform; 42 | 43 | void PointCloudHandler(const sensor_msgs::PointCloud2ConstPtr &raw_points_msg) { 44 | 45 | // fetch new input cloud 46 | pcl::PointCloud laser_cloud_in, tmp_cloud; 47 | pcl::fromROSMsg(*raw_points_msg, laser_cloud_in); 48 | // pcl::PointCloud::ConstPtr laser_cloud_in_ptr(new pcl::PointCloud(laser_cloud_in)); 49 | pcl::PointCloud::Ptr transformed_ptr(new pcl::PointCloud()); 50 | pcl::PointCloud laser_cloud_out; 51 | 52 | pcl::transformPointCloud(laser_cloud_in, *transformed_ptr, transform_to_world); 53 | 54 | pcl::CropBox box_filter; 55 | box_filter.setMin(Eigen::Vector4f(-10, -5, -1.7, 1.0)); 56 | box_filter.setMax(Eigen::Vector4f(5, 7, 0.6, 1.0)); 57 | box_filter.setNegative(true); 58 | box_filter.setInputCloud(transformed_ptr); 59 | box_filter.filter(tmp_cloud); 60 | 61 | pcl::transformPointCloud(tmp_cloud, laser_cloud_out, transform_to_world.inverse()); 62 | 63 | PublishCloudMsg(pub_filtered_cloud, 64 | laser_cloud_out, 65 | raw_points_msg->header.stamp, 66 | raw_points_msg->header.frame_id); 67 | 68 | br_ptr->sendTransform(tf::StampedTransform(tf_transform, raw_points_msg->header.stamp, "imu_link_kaist", "right_velodyne")); 69 | } 70 | 71 | int main(int argc, char **argv) { 72 | 73 | ros::init(argc, argv, "input_filters"); 74 | ros::NodeHandle nh("~"); 75 | 76 | ros::Subscriber sub_raw_points = nh.subscribe 77 | ("/velodyne_points", 2, PointCloudHandler); 78 | 79 | br_ptr = new tf::TransformBroadcaster(); 80 | pub_filtered_cloud = nh.advertise("/filtered_points", 2); 81 | 82 | Eigen::Matrix3f R_inv; 83 | R_inv << -4.91913910e-01, -5.01145813e-01, -7.11950546e-01, 84 | 7.13989130e-01, -7.00156621e-01, -4.78439170e-04, 85 | -4.98237120e-01, -5.08560301e-01, 7.02229444e-01; 86 | transform_to_world.setIdentity(); 87 | transform_to_world.linear() = R_inv.transpose(); 88 | 89 | 90 | Eigen::Quaternionf q_eigen(R_inv.transpose()); 91 | tf::Quaternion q(tf::Quaternion{q_eigen.x(), q_eigen.y(), q_eigen.z(), q_eigen.w()}); 92 | tf_transform.setRotation(q); 93 | 94 | ros::Rate r(500); 95 | 96 | while (ros::ok()) { 97 | ros::spinOnce(); 98 | r.sleep(); 99 | } 100 | } -------------------------------------------------------------------------------- /include/imu_processor/MeasurementManager.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/27/18. 29 | // 30 | 31 | #ifndef LIO_MEASUREMENTMANAGER_H_ 32 | #define LIO_MEASUREMENTMANAGER_H_ 33 | 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | #include 43 | 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | 50 | #include "utils/CircularBuffer.h" 51 | #include "utils/Twist.h" 52 | #include "utils/common_ros.h" 53 | #include "utils/TicToc.h" 54 | #include "utils/math_utils.h" 55 | #include "utils/geometry_utils.h" 56 | #include "3rdparty/sophus/se3.hpp" 57 | 58 | namespace lio { 59 | 60 | using namespace std; 61 | using namespace mathutils; 62 | using namespace geometryutils; 63 | typedef Twist Transform; 64 | typedef Sophus::SO3f SO3f; 65 | typedef Sophus::SO3d SO3d; 66 | typedef nav_msgs::OdometryConstPtr OdomMsgConstPtr; 67 | typedef sensor_msgs::PointCloud2ConstPtr CompactDataConstPtr; 68 | typedef sensor_msgs::ImuConstPtr ImuMsgConstPtr; 69 | //typedef vector, OdomMsgConstPtr> > PairMeasurements; 70 | typedef pair, CompactDataConstPtr> PairMeasurement; 71 | typedef vector PairMeasurements; 72 | 73 | struct MeasurementManagerConfig { 74 | string imu_topic = "/imu/data"; 75 | string laser_topic = "/velodyne_points"; 76 | string laser_odom_topic = "/aft_mapped_to_init"; // NOTE: Check if the time is too long 77 | string compact_data_topic = "/compact_data"; // NOTE: Check if the time is too long 78 | double msg_time_delay = 0; 79 | bool enable_imu = true; 80 | }; 81 | 82 | class MeasurementManager { 83 | 84 | public: 85 | virtual void SetupRos(ros::NodeHandle &nh); 86 | void ImuHandler(const sensor_msgs::ImuConstPtr &raw_imu_msg); 87 | void LaserOdomHandler(const nav_msgs::OdometryConstPtr &laser_odom_msg); 88 | void CompactDataHandler(const sensor_msgs::PointCloud2ConstPtr &compact_data_msg); 89 | 90 | PairMeasurements GetMeasurements(); 91 | 92 | protected: 93 | ros::NodeHandle nh_; 94 | TicToc mm_tic_toc_; 95 | MeasurementManagerConfig mm_config_; 96 | 97 | mutex buf_mutex_; 98 | mutex state_mutex_; 99 | mutex thread_mutex_; 100 | std::condition_variable con_; 101 | queue imu_buf_; 102 | queue laser_odom_buf_; 103 | queue compact_data_buf_; 104 | 105 | double imu_last_time_ = -1; 106 | double curr_time_ = -1; 107 | 108 | ros::Subscriber sub_imu_; 109 | ros::Subscriber sub_laser_odom_; 110 | ros::Subscriber sub_compact_data_; 111 | 112 | bool is_ros_setup_ = false; 113 | 114 | }; 115 | 116 | } 117 | 118 | #endif //LIO_MEASUREMENTMANAGER_H_ 119 | -------------------------------------------------------------------------------- /include/factor/MarginalizationFactor.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 5/4/18. 29 | // 30 | 31 | /// adapted from VINS-mono 32 | 33 | #ifndef LIO_MARGINALIZATIONFACTOR_H_ 34 | #define LIO_MARGINALIZATIONFACTOR_H_ 35 | 36 | #include 37 | #include 38 | #include 39 | #include 40 | 41 | #include "utils/CircularBuffer.h" 42 | #include "utils/Twist.h" 43 | #include "utils/common_ros.h" 44 | #include "utils/TicToc.h" 45 | #include "utils/math_utils.h" 46 | #include "utils/geometry_utils.h" 47 | 48 | namespace lio { 49 | 50 | const int NUM_THREADS = 4; 51 | 52 | struct ResidualBlockInfo { 53 | ResidualBlockInfo(ceres::CostFunction *_cost_function, 54 | ceres::LossFunction *_loss_function, 55 | std::vector _parameter_blocks, 56 | std::vector _drop_set) 57 | : cost_function(_cost_function), 58 | loss_function(_loss_function), 59 | parameter_blocks(_parameter_blocks), 60 | drop_set(_drop_set) {} 61 | 62 | void Evaluate(); 63 | 64 | ceres::CostFunction *cost_function; 65 | ceres::LossFunction *loss_function; 66 | std::vector parameter_blocks; 67 | std::vector drop_set; 68 | 69 | double **raw_jacobians; 70 | std::vector> jacobians; 71 | Eigen::VectorXd residuals; 72 | 73 | int localSize(int size) { 74 | return size == 7 ? 6 : size; 75 | } 76 | }; 77 | 78 | struct ThreadsStruct { 79 | std::vector sub_factors; 80 | Eigen::MatrixXd A; 81 | Eigen::VectorXd b; 82 | std::unordered_map parameter_block_size; //global size 83 | std::unordered_map parameter_block_idx; //local size 84 | }; 85 | 86 | class MarginalizationInfo { 87 | public: 88 | ~MarginalizationInfo(); 89 | int LocalSize(int size) const; 90 | int GlobalSize(int size) const; 91 | void AddResidualBlockInfo(ResidualBlockInfo *residual_block_info); 92 | void PreMarginalize(); 93 | void Marginalize(); 94 | std::vector GetParameterBlocks(std::unordered_map &addr_shift); 95 | 96 | std::vector factors; 97 | int m, n; 98 | std::unordered_map parameter_block_size; //global size 99 | int sum_block_size; 100 | std::unordered_map parameter_block_idx; //local size 101 | std::unordered_map parameter_block_data; 102 | 103 | std::vector keep_block_size; //global size 104 | std::vector keep_block_idx; //local size 105 | std::vector keep_block_data; 106 | 107 | Eigen::MatrixXd linearized_jacobians; 108 | Eigen::VectorXd linearized_residuals; 109 | const double eps = 1e-8; 110 | }; 111 | 112 | class MarginalizationFactor : public ceres::CostFunction { 113 | public: 114 | MarginalizationFactor(MarginalizationInfo* _marginalization_info); 115 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 116 | 117 | MarginalizationInfo* marginalization_info; 118 | }; 119 | 120 | } 121 | 122 | #endif //LIO_MARGINALIZATIONFACTOR_H_ 123 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | lio 4 | 0.0.0 5 | The lio package 6 | 7 | 8 | 9 | 10 | hyye 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 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | dynamic_reconfigure 53 | geometry_msgs 54 | message_generation 55 | nav_msgs 56 | pcl_conversions 57 | pcl_ros 58 | rosbag 59 | roscpp 60 | rospy 61 | sensor_msgs 62 | std_msgs 63 | tf 64 | tf_conversions 65 | dynamic_reconfigure 66 | geometry_msgs 67 | nav_msgs 68 | pcl_conversions 69 | pcl_ros 70 | rosbag 71 | roscpp 72 | rospy 73 | sensor_msgs 74 | std_msgs 75 | tf 76 | tf_conversions 77 | dynamic_reconfigure 78 | geometry_msgs 79 | message_runtime 80 | nav_msgs 81 | pcl_conversions 82 | pcl_ros 83 | rosbag 84 | roscpp 85 | rospy 86 | sensor_msgs 87 | std_msgs 88 | tf 89 | tf_conversions 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | -------------------------------------------------------------------------------- /src/factor/PriorFactor.cc: -------------------------------------------------------------------------------- 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 18-6-2. 29 | // 30 | 31 | #include "factor/PriorFactor.h" 32 | 33 | namespace lio { 34 | 35 | bool PriorFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const { 36 | Eigen::Vector3d P{parameters[0][0], parameters[0][1], parameters[0][2]}; 37 | Eigen::Quaterniond Q(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]); 38 | 39 | Matrix sqrt_info; 40 | sqrt_info.setZero(); 41 | sqrt_info.topLeftCorner<3, 3>() = Matrix3d::Identity() * 1000; 42 | sqrt_info.bottomRightCorner<3, 3>() = Matrix3d::Identity() * 0.1; 43 | 44 | Eigen::Map> residual(residuals); 45 | residual.topRows<3>() = P - pos_; 46 | residual.bottomRows<3>() = 2 * (rot_.inverse() * Q).coeffs().head<3>(); 47 | 48 | // FIXME: info 49 | residual = sqrt_info * residual; 50 | DLOG(INFO) << "residual: " << residual.transpose(); 51 | 52 | if (jacobians) { 53 | if (jacobians[0]) { 54 | Eigen::Map > jacobian_prior(jacobians[0]); 55 | Eigen::Matrix jaco_prior; 56 | jaco_prior.setIdentity(); 57 | 58 | jaco_prior.bottomRightCorner<3, 3>() = LeftQuatMatrix(Q.inverse() * rot_).topLeftCorner<3, 3>(); 59 | 60 | // FIXME: info 61 | jacobian_prior.setZero(); 62 | jacobian_prior.leftCols<6>() = sqrt_info * jaco_prior; 63 | jacobian_prior.rightCols<1>().setZero(); 64 | } 65 | } 66 | return true; 67 | } 68 | 69 | void PriorFactor::Check(double const *const *parameters) { 70 | Eigen::Vector3d P{parameters[0][0], parameters[0][1], parameters[0][2]}; 71 | Eigen::Quaterniond Q(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]); 72 | 73 | double *res = new double[6]; 74 | double **jaco = new double *[1]; 75 | jaco[0] = new double[6 * 7]; 76 | Evaluate(parameters, res, jaco); 77 | DLOG(INFO) << "check begins"; 78 | DLOG(INFO) << "analytical"; 79 | 80 | DLOG(INFO) << Eigen::Map>(res).transpose(); 81 | DLOG(INFO) << std::endl << Eigen::Map>(jaco[0]); 82 | 83 | double info = 1; 84 | 85 | Eigen::Matrix residual; 86 | residual.topRows<3>() = P - pos_; 87 | residual.bottomRows<3>() = 2 * (rot_.inverse() * Q).coeffs().head<3>(); 88 | 89 | residual = info * residual; 90 | DLOG(INFO) << "num"; 91 | DLOG(INFO) << residual.transpose(); 92 | 93 | const double eps = 1e-6; 94 | Eigen::Matrix num_jacobian; 95 | for (int k = 0; k < 6; k++) { 96 | P = Eigen::Vector3d{parameters[0][0], parameters[0][1], parameters[0][2]}; 97 | Q = Eigen::Quaterniond(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]); 98 | 99 | int a = k / 3, b = k % 3; 100 | Eigen::Vector3d delta = Eigen::Vector3d(b == 0, b == 1, b == 2) * eps; 101 | 102 | if (a == 0) 103 | P += delta; 104 | else if (a == 1) 105 | Q = Q * DeltaQ(delta); 106 | 107 | Eigen::Matrix tmp_residual; 108 | 109 | tmp_residual.topRows<3>() = P - pos_; 110 | tmp_residual.bottomRows<3>() = 2 * (rot_.inverse() * Q).coeffs().head<3>(); 111 | 112 | // FIXME: info 113 | tmp_residual = info * tmp_residual; 114 | 115 | num_jacobian.col(k) = (tmp_residual - residual) / eps; 116 | } 117 | DLOG(INFO) << std::endl << num_jacobian.block<6, 6>(0, 0); 118 | } 119 | 120 | } -------------------------------------------------------------------------------- /test/test_point_processor/test_point_processor.cc: -------------------------------------------------------------------------------- 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/15/18. 29 | // 30 | 31 | #include 32 | #include 33 | #include 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | 41 | #include 42 | 43 | #include "point_processor/PointProcessor.h" 44 | #include "utils/TicToc.h" 45 | 46 | using namespace lio; 47 | using namespace std; 48 | using namespace mathutils; 49 | 50 | static ros::Publisher test_pub; 51 | static ros::NodeHandlePtr nh_ptr; 52 | 53 | DEFINE_int64(sensor_type, 16, "Sensor type - number of scans (default 16)"); 54 | 55 | TEST(PointProcessorTest, AngleTest) { 56 | 57 | EXPECT_DOUBLE_EQ(NormalizeRad(-3.4 - 2 * M_PI), -3.4 + 2 * M_PI); 58 | EXPECT_DOUBLE_EQ(NormalizeRad(3.4 + 2 * M_PI), 3.4 - 2 * M_PI); 59 | 60 | EXPECT_DOUBLE_EQ(NormalizeDeg(-190 - 360), -190 + 360); 61 | EXPECT_DOUBLE_EQ(NormalizeDeg(190 + 360), 190 - 360); 62 | 63 | } 64 | 65 | TEST(PointProcessorTest, SubscribeTest) { 66 | 67 | PointProcessor processor; 68 | 69 | if (FLAGS_sensor_type == 32) { 70 | processor = PointProcessor(-30.67f, 10.67f, 32); 71 | } else if (FLAGS_sensor_type == 64) { 72 | processor = PointProcessor(-24.9f, 2, 64); 73 | } 74 | 75 | DLOG(INFO) << "Sensor type: " << processor.laser_scans.size(); 76 | 77 | processor.SetupRos(*nh_ptr); 78 | 79 | ros::Rate r(100); 80 | while (ros::ok()) { 81 | ros::spinOnce(); 82 | r.sleep(); 83 | } 84 | 85 | 86 | } 87 | 88 | TEST(PointProcessorTest, ObjectTest) { 89 | 90 | pcl::PointXYZI p; 91 | // PointProcessor pp(-24.8f, 2.0f, 64); 92 | PointProcessor pp; 93 | 94 | pcl::PCDReader pcd_reader; 95 | 96 | PointCloud test_pc; 97 | // pcd_reader.read("/mnt/HDD/Datasets/Lidar/sz_tests/OneDrive-2018-03-07/output/1520430666.350994142.pcd", test_pc); 98 | pcd_reader.read("/mnt/HDD/Datasets/Lidar/Lidar_IMU/output/1512528800.091783000.pcd", test_pc); 99 | // pcd_reader.read("/home/hyye/Desktop/output/kitti_data/1317013369.682684898.pcd", test_pc); 100 | 101 | PointCloudConstPtr test_pc_ptr(new PointCloud(test_pc)); 102 | 103 | pp.SetDeskew(false); 104 | pp.SetInputCloud(test_pc_ptr); 105 | pp.PointToRing(); 106 | pp.ExtractFeaturePoints(); 107 | pp.SetupRos(*nh_ptr); 108 | 109 | PointCloud test_pc_out; 110 | for (const auto &ring_scan : pp.laser_scans) { 111 | for (const auto &p : *ring_scan) { 112 | // cout << p.intensity << " "; 113 | } 114 | // cout << endl; 115 | test_pc_out += (*ring_scan); 116 | } 117 | 118 | sensor_msgs::PointCloud2 pc_msg; 119 | pcl::toROSMsg(test_pc_out, pc_msg); 120 | pc_msg.header.frame_id = "map"; 121 | ros::Rate r(10); 122 | while (ros::ok()) { 123 | // test_pub.publish(pc_msg); 124 | PublishCloudMsg(test_pub, test_pc_out, ros::Time::now(), "map"); 125 | pp.PublishResults(); 126 | ros::spinOnce(); 127 | r.sleep(); 128 | } 129 | 130 | return; 131 | 132 | } 133 | 134 | int main(int argc, char **argv) { 135 | testing::InitGoogleTest(&argc, argv); 136 | 137 | google::InitGoogleLogging(argv[0]); 138 | google::ParseCommandLineFlags(&argc, &argv, true); 139 | 140 | ros::init(argc, argv, "test_point_processor"); 141 | { 142 | ros::NodeHandle nh("~"); 143 | nh_ptr = boost::make_shared(nh); 144 | test_pub = nh.advertise("test_points", 5); 145 | } 146 | 147 | FLAGS_alsologtostderr = true; 148 | 149 | return RUN_ALL_TESTS(); 150 | } -------------------------------------------------------------------------------- /include/utils/CircularBuffer.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 | #ifndef LIO_CIRCULARBUFFER_H_ 28 | #define LIO_CIRCULARBUFFER_H_ 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | namespace lio { 35 | 36 | /// Adapted from https://github.com/laboshinl/loam_velodyne/blob/master/include/loam_velodyne/CircularBuffer.h 37 | 38 | /** \brief Simple circular buffer implementation for storing data history. 39 | * 40 | * @tparam T The buffer element type. 41 | */ 42 | template 43 | class CircularBuffer { 44 | public: 45 | CircularBuffer(const size_t &capacity = 200) 46 | : capacity_(capacity), 47 | size_(0), 48 | start_idx_(0) { 49 | buffer_ = new T[capacity]; 50 | }; 51 | 52 | ~CircularBuffer() { 53 | delete[] buffer_; 54 | buffer_ = NULL; 55 | } 56 | 57 | void Reset(size_t capacity = 1) { 58 | delete[] buffer_; 59 | buffer_ = NULL; 60 | 61 | capacity_ = capacity; 62 | size_ = 0; 63 | start_idx_ = 0; 64 | buffer_ = new T[capacity]; 65 | } 66 | 67 | /** \brief Retrieve the buffer size. 68 | * 69 | * @return the buffer size 70 | */ 71 | const size_t &size() { 72 | return size_; 73 | } 74 | 75 | /** \brief Retrieve the buffer capacity. 76 | * 77 | * @return the buffer capacity 78 | */ 79 | const size_t &capacity() { 80 | return capacity_; 81 | } 82 | 83 | /** \brief Ensure that this buffer has at least the required capacity. 84 | * 85 | * @param req_apacity the minimum required capacity 86 | */ 87 | void EnsureCapacity(const int &req_apacity) { 88 | if (req_apacity > 0 && capacity_ < req_apacity) { 89 | // create new buffer and copy (valid) entries 90 | T *new_buffer = new T[req_apacity]; 91 | for (size_t i = 0; i < size_; i++) { 92 | new_buffer[i] = (*this)[i]; 93 | } 94 | 95 | // switch buffer pointers and delete old buffer 96 | T *old_buffer = buffer_; 97 | buffer_ = new_buffer; 98 | start_idx_ = 0; 99 | 100 | delete[] old_buffer; 101 | } 102 | } 103 | 104 | /** \brief Check if the buffer is empty. 105 | * 106 | * @return true if the buffer is empty, false otherwise 107 | */ 108 | bool empty() { 109 | return size_ == 0; 110 | } 111 | 112 | /** \brief Retrieve the i-th element of the buffer. 113 | * 114 | * 115 | * @param i the buffer index 116 | * @return the element at the i-th position as no-constant 117 | */ 118 | T &operator[](const size_t &i) { 119 | return buffer_[(start_idx_ + i) % capacity_]; 120 | } 121 | 122 | /** \brief Retrieve the i-th element of the buffer. 123 | * 124 | * 125 | * @param i the buffer index 126 | * @return the element at the i-th position 127 | */ 128 | const T &operator[](const size_t &i) const { 129 | return buffer_[(start_idx_ + i) % capacity_]; 130 | } 131 | 132 | /** \brief Retrieve the first (oldest) element of the buffer. 133 | * 134 | * @return the first element 135 | */ 136 | const T &first() const { 137 | return buffer_[start_idx_]; 138 | } 139 | 140 | T &first() { 141 | return buffer_[start_idx_]; 142 | } 143 | 144 | /** \brief Retrieve the last (latest) element of the buffer. 145 | * 146 | * @return the last element 147 | */ 148 | const T &last() const { 149 | size_t idx = size_ == 0 ? 0 : (start_idx_ + size_ - 1) % capacity_; 150 | return buffer_[idx]; 151 | } 152 | 153 | T &last() { 154 | size_t idx = size_ == 0 ? 0 : (start_idx_ + size_ - 1) % capacity_; 155 | return buffer_[idx]; 156 | } 157 | 158 | /** \brief Push a new element to the buffer. 159 | * 160 | * If the buffer reached its capacity, the oldest element is overwritten. 161 | * 162 | * @param element the element to push 163 | */ 164 | void push(const T &element) { 165 | if (size_ < capacity_) { 166 | buffer_[size_] = element; 167 | ++size_; 168 | } else { 169 | buffer_[start_idx_] = element; 170 | start_idx_ = (start_idx_ + 1) % capacity_; 171 | } 172 | } 173 | 174 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 175 | 176 | private: 177 | size_t capacity_; ///< buffer capacity 178 | size_t size_; ///< current buffer size 179 | size_t start_idx_; ///< current start index 180 | T *buffer_; ///< internal element buffer 181 | }; 182 | 183 | } // namespace lio 184 | 185 | #endif // LIO_CIRCULARBUFFER_H_ 186 | -------------------------------------------------------------------------------- /src/imu_processor/MeasurementManager.cc: -------------------------------------------------------------------------------- 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/27/18. 29 | // 30 | 31 | #include "imu_processor/MeasurementManager.h" 32 | 33 | namespace lio { 34 | 35 | void MeasurementManager::SetupRos(ros::NodeHandle &nh) { 36 | is_ros_setup_ = true; 37 | 38 | nh_ = nh; 39 | 40 | sub_imu_ = nh_.subscribe(mm_config_.imu_topic, 41 | 1000, 42 | &MeasurementManager::ImuHandler, 43 | this, 44 | ros::TransportHints().tcpNoDelay()); 45 | 46 | sub_compact_data_ = nh_.subscribe(mm_config_.compact_data_topic, 47 | 10, 48 | &MeasurementManager::CompactDataHandler, 49 | this); 50 | // sub_laser_odom_ = 51 | // nh_.subscribe(mm_config_.laser_odom_topic, 10, &MeasurementManager::LaserOdomHandler, this); 52 | } 53 | 54 | PairMeasurements MeasurementManager::GetMeasurements() { 55 | 56 | PairMeasurements measurements; 57 | 58 | while (true) { 59 | 60 | if (mm_config_.enable_imu) { 61 | 62 | if (imu_buf_.empty() || compact_data_buf_.empty()) { 63 | return measurements; 64 | } 65 | 66 | if (imu_buf_.back()->header.stamp.toSec() 67 | <= compact_data_buf_.front()->header.stamp.toSec() + mm_config_.msg_time_delay) { 68 | //ROS_DEBUG("wait for imu, only should happen at the beginning"); 69 | // Count for waiting time 70 | return measurements; 71 | } 72 | 73 | if (imu_buf_.front()->header.stamp.toSec() 74 | >= compact_data_buf_.front()->header.stamp.toSec() + mm_config_.msg_time_delay) { 75 | ROS_DEBUG("throw compact_data, only should happen at the beginning"); 76 | compact_data_buf_.pop(); 77 | continue; 78 | } 79 | CompactDataConstPtr compact_data_msg = compact_data_buf_.front(); 80 | compact_data_buf_.pop(); 81 | 82 | vector imu_measurements; 83 | while (imu_buf_.front()->header.stamp.toSec() 84 | < compact_data_msg->header.stamp.toSec() + mm_config_.msg_time_delay) { 85 | imu_measurements.emplace_back(imu_buf_.front()); 86 | imu_buf_.pop(); 87 | } 88 | 89 | // NOTE: one message after laser odom msg 90 | imu_measurements.emplace_back(imu_buf_.front()); 91 | 92 | if (imu_measurements.empty()) { 93 | ROS_DEBUG("no imu between two image"); 94 | } 95 | measurements.emplace_back(imu_measurements, compact_data_msg); 96 | } else { 97 | vector imu_measurements; 98 | if (compact_data_buf_.empty()) { 99 | return measurements; 100 | } 101 | CompactDataConstPtr compact_data_msg = compact_data_buf_.front(); 102 | compact_data_buf_.pop(); 103 | measurements.emplace_back(imu_measurements, compact_data_msg); 104 | } 105 | 106 | } 107 | 108 | } 109 | 110 | void MeasurementManager::ImuHandler(const sensor_msgs::ImuConstPtr &raw_imu_msg) { 111 | if (raw_imu_msg->header.stamp.toSec() <= imu_last_time_) { 112 | LOG(ERROR) << ("imu message in disorder!"); 113 | return; 114 | } 115 | 116 | imu_last_time_ = raw_imu_msg->header.stamp.toSec(); 117 | 118 | buf_mutex_.lock(); 119 | imu_buf_.push(raw_imu_msg); 120 | buf_mutex_.unlock(); 121 | 122 | con_.notify_one(); 123 | 124 | { 125 | lock_guard lg(state_mutex_); 126 | // TODO: is it necessary to do predict here? 127 | // Predict(raw_imu_msg); 128 | 129 | // std_msgs::Header header = imu_msg->header; 130 | // header.frame_id = "world"; 131 | // if (flag == INITIALIZED) 132 | // PubLatestOdometry(states, header); 133 | } 134 | } // ImuHandler 135 | 136 | void MeasurementManager::LaserOdomHandler(const nav_msgs::OdometryConstPtr &laser_odom_msg) { 137 | buf_mutex_.lock(); 138 | laser_odom_buf_.push(laser_odom_msg); 139 | buf_mutex_.unlock(); 140 | con_.notify_one(); 141 | } 142 | 143 | void MeasurementManager::CompactDataHandler(const sensor_msgs::PointCloud2ConstPtr &compact_data_msg) { 144 | buf_mutex_.lock(); 145 | compact_data_buf_.push(compact_data_msg); 146 | buf_mutex_.unlock(); 147 | con_.notify_one(); 148 | } 149 | 150 | } -------------------------------------------------------------------------------- /test/test_imu_processor/test_measurement_manager.cc: -------------------------------------------------------------------------------- 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/25/18. 29 | // 30 | 31 | #include 32 | #include 33 | #include 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | 40 | #include 41 | 42 | #include 43 | 44 | #include 45 | #include 46 | 47 | #include "imu_processor/Estimator.h" 48 | #include "point_processor/PointOdometry.h" 49 | #include "utils/TicToc.h" 50 | 51 | using namespace lio; 52 | using namespace std; 53 | using namespace mathutils; 54 | 55 | static ros::NodeHandlePtr nh_ptr; 56 | 57 | DEFINE_string(config_file, "/home/hyye/dev_ws/src/lio/config/test_config.yaml", "yaml config file"); 58 | 59 | TEST(MeasurementManager, ObjectTest) { 60 | 61 | EstimatorConfig estimator_config; 62 | string config_file = FLAGS_config_file; 63 | cv::FileStorage fs_settings(config_file, cv::FileStorage::READ); 64 | 65 | { 66 | int tmp_int; 67 | estimator_config.min_match_sq_dis = fs_settings["min_match_sq_dis"]; 68 | estimator_config.min_plane_dis = fs_settings["min_plane_dis"]; 69 | 70 | estimator_config.corner_filter_size = fs_settings["corner_filter_size"]; 71 | estimator_config.surf_filter_size = fs_settings["surf_filter_size"]; 72 | estimator_config.map_filter_size = fs_settings["map_filter_size"]; 73 | 74 | tmp_int = fs_settings["window_size"]; 75 | estimator_config.window_size = size_t(tmp_int); 76 | 77 | tmp_int = fs_settings["opt_window_size"]; 78 | estimator_config.opt_window_size = size_t(tmp_int); 79 | estimator_config.init_window_factor = fs_settings["init_window_factor"]; 80 | 81 | tmp_int = fs_settings["estimate_extrinsic"]; 82 | estimator_config.estimate_extrinsic = tmp_int; 83 | 84 | tmp_int = fs_settings["opt_extrinsic"]; 85 | estimator_config.opt_extrinsic = (tmp_int > 0); 86 | 87 | cv::Mat cv_R, cv_T; 88 | fs_settings["extrinsic_rotation"] >> cv_R; 89 | fs_settings["extrinsic_translation"] >> cv_T; 90 | Eigen::Matrix3d eigen_R; 91 | Eigen::Vector3d eigen_T; 92 | cv::cv2eigen(cv_R, eigen_R); 93 | cv::cv2eigen(cv_T, eigen_T); 94 | 95 | estimator_config.transform_lb = Transform{Eigen::Quaternionf(eigen_R.cast()), eigen_T.cast()}; 96 | 97 | tmp_int = fs_settings["run_optimization"]; 98 | estimator_config.run_optimization = (tmp_int > 0); 99 | tmp_int = fs_settings["update_laser_imu"]; 100 | estimator_config.update_laser_imu = (tmp_int > 0); 101 | tmp_int = fs_settings["gravity_fix"]; 102 | estimator_config.gravity_fix = (tmp_int > 0); 103 | tmp_int = fs_settings["plane_projection_factor"]; 104 | estimator_config.plane_projection_factor = (tmp_int > 0); 105 | tmp_int = fs_settings["imu_factor"]; 106 | estimator_config.imu_factor = (tmp_int > 0); 107 | tmp_int = fs_settings["point_distance_factor"]; 108 | estimator_config.point_distance_factor = (tmp_int > 0); 109 | tmp_int = fs_settings["prior_factor"]; 110 | estimator_config.prior_factor = (tmp_int > 0); 111 | tmp_int = fs_settings["marginalization_factor"]; 112 | estimator_config.marginalization_factor = (tmp_int > 0); 113 | 114 | tmp_int = fs_settings["pcl_viewer"]; 115 | estimator_config.pcl_viewer = (tmp_int > 0); 116 | } 117 | 118 | Estimator estimator(estimator_config); 119 | estimator.SetupRos(*nh_ptr); 120 | 121 | int odom_io = fs_settings["odom_io"]; 122 | 123 | PointOdometry odometry(0.1, odom_io); 124 | odometry.SetupRos(*nh_ptr); 125 | odometry.Reset(); 126 | 127 | thread odom(&PointOdometry::Spin, &odometry); 128 | 129 | thread measurement_manager(&Estimator::ProcessEstimation, &estimator); 130 | 131 | if (estimator_config.pcl_viewer) { 132 | boost::thread visualizer(boost::bind(&PlaneNormalVisualizer::Spin, &(estimator.normal_vis))); 133 | } 134 | 135 | // while (!estimator.normal_vis.viewer->wasStopped()) { 136 | // { 137 | // boost::mutex::scoped_lock lk(estimator.normal_vis.m); 138 | // DLOG(INFO) << ">>>>>>> spin <<<<<<<"; 139 | // estimator.normal_vis.viewer->spinOnce(100); 140 | // } 141 | // boost::this_thread::sleep(boost::posix_time::microseconds(100000)); 142 | // } 143 | 144 | while (ros::ok()) { 145 | ros::spinOnce(); 146 | } 147 | 148 | } 149 | 150 | int main(int argc, char **argv) { 151 | testing::InitGoogleTest(&argc, argv); 152 | 153 | google::InitGoogleLogging(argv[0]); 154 | google::ParseCommandLineFlags(&argc, &argv, true); 155 | 156 | ros::init(argc, argv, "test_measurement_manager"); 157 | { 158 | ros::NodeHandle nh("~"); 159 | nh_ptr = boost::make_shared(nh); 160 | } 161 | 162 | FLAGS_alsologtostderr = true; 163 | 164 | return RUN_ALL_TESTS(); 165 | } -------------------------------------------------------------------------------- /include/feature_manager/FeatureManager.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 5/9/18. 29 | // 30 | 31 | #ifndef LIO_FEATURE_MANAGER_H_ 32 | #define LIO_FEATURE_MANAGER_H_ 33 | 34 | #include 35 | #include 36 | #include 37 | 38 | namespace lio { 39 | 40 | using std::unique_ptr; 41 | 42 | struct Feature { 43 | std::string feature_name; 44 | virtual void GetFeature(Feature *feature) { 45 | DLOG(WARNING) << ">>>>>>> GetFeature not implemented <<<<<<<"; 46 | } 47 | }; 48 | 49 | struct PointNormalFeature : public Feature { 50 | public: 51 | PointNormalFeature() { 52 | feature_name = "PointNormalFeature"; 53 | } 54 | PointNormalFeature(const Eigen::Vector3d &point3d_in, const Eigen::Vector3d &normal3d_in) { 55 | feature_name = "PointNormalFeature"; 56 | point3d = point3d_in; 57 | normal3d = normal3d_in; 58 | diag_covariance = Eigen::Vector3d{gicp_epsilon, 1.0, 1.0}.asDiagonal(); 59 | UpdateCovariance(normal3d); 60 | } 61 | 62 | void UpdateCovariance(const Eigen::Vector3d &normal3d_in); 63 | 64 | PointNormalFeature &GetFeatureInner() { 65 | return *this; 66 | } 67 | 68 | void GetFeature(Feature *feature) { 69 | PointNormalFeature *derived_feature = static_cast(feature); 70 | *derived_feature = this->GetFeatureInner(); 71 | } 72 | 73 | double gicp_epsilon = 0.001; 74 | Eigen::Vector3d e1{1.0, 0.0, 0.0}; 75 | Eigen::Vector3d point3d; /// the point in the current frame 76 | Eigen::Vector3d normal3d; /// the normal vector in the current frame 77 | Eigen::Matrix3d diag_covariance; 78 | Eigen::Matrix3d covariance; /// the covariance in the current frame from the normal vector 79 | 80 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 81 | 82 | }; 83 | 84 | struct PointPlaneFeature : public Feature { 85 | PointPlaneFeature() { 86 | feature_name = "PointPlaneFeature"; 87 | } 88 | 89 | PointPlaneFeature(const Eigen::Vector3d &point_in, const Eigen::Vector4d &coeffs_in) { 90 | feature_name = "PointPlaneFeature"; 91 | point = point_in; 92 | coeffs = coeffs_in; 93 | } 94 | 95 | PointPlaneFeature &GetFeatureInner() { 96 | return *this; 97 | } 98 | 99 | void GetFeature(Feature *feature) { 100 | PointPlaneFeature *derived_feature = static_cast(feature); 101 | *derived_feature = this->GetFeatureInner(); 102 | } 103 | 104 | double score; 105 | Eigen::Vector3d point; 106 | Eigen::Vector4d coeffs; 107 | 108 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 109 | }; 110 | 111 | struct FeaturePerFrame { 112 | int id; 113 | std::vector> features; 114 | }; 115 | 116 | class FeatureManager { 117 | public: 118 | FeatureManager(); 119 | // static void CalculateFeatures(const pcl::KdTreeFLANN::Ptr &kdtree_surf_from_map, 120 | // const PointCloudPtr &local_surf_points_filtered_ptr, 121 | // const PointCloudPtr &surf_stack, 122 | // const Transform &local_transform, 123 | // vector &features); 124 | // 125 | // static void CalculateLaserOdom(const pcl::KdTreeFLANN::Ptr &kdtree_surf_from_map, 126 | // const PointCloudPtr &local_surf_points_filtered_ptr, 127 | // const PointCloudPtr &surf_stack, 128 | // const Transform &local_transform, 129 | // vector &features); 130 | // 131 | // static void CalculateFeatures(const pcl::KdTreeFLANN::Ptr &kdtree_surf_from_map, 132 | // const PointCloudPtr &local_surf_points_filtered_ptr, 133 | // const PointCloudPtr &surf_stack, 134 | // const pcl::KdTreeFLANN::Ptr &kdtree_corner_from_map, 135 | // const PointCloudPtr &local_corner_points_filtered_ptr, 136 | // const PointCloudPtr &corner_stack, 137 | // const Transform &local_transform, 138 | // vector &features); 139 | // 140 | // static void CalculateLaserOdom(const pcl::KdTreeFLANN::Ptr &kdtree_surf_from_map, 141 | // const PointCloudPtr &local_surf_points_filtered_ptr, 142 | // const PointCloudPtr &surf_stack, 143 | // const pcl::KdTreeFLANN::Ptr &kdtree_corner_from_map, 144 | // const PointCloudPtr &local_corner_points_filtered_ptr, 145 | // const PointCloudPtr &corner_stack, 146 | // const Transform &local_transform, 147 | // vector &features); 148 | }; 149 | 150 | } 151 | 152 | #endif //LIO_FEATURE_MANAGER_H_ 153 | -------------------------------------------------------------------------------- /src/save_bag_to_pcd.cc: -------------------------------------------------------------------------------- 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 5/31/18. 29 | // 30 | 31 | #include 32 | #include 33 | 34 | #include 35 | 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | 44 | #include 45 | #include 46 | 47 | #include 48 | #include 49 | 50 | #include 51 | #include 52 | 53 | #include 54 | #include 55 | 56 | #include "utils/TicToc.h" 57 | 58 | #include 59 | #define foreach BOOST_FOREACH 60 | 61 | using namespace std; 62 | 63 | DEFINE_string(input_filename, "", "the name of input bag file"); 64 | DEFINE_string(output_filename, "", "the name of output pcd file"); 65 | DEFINE_string(odom_name, "/aft_mapped_to_init", "the name of the odom"); 66 | DEFINE_string(laser_name, "/cloud_registered", "the name of the laser"); 67 | 68 | static bool laser_available = false; 69 | static bool odom_available = false; 70 | static sensor_msgs::PointCloud2 tmp_points_msg; 71 | static nav_msgs::Odometry tmp_odom_msg; 72 | static pcl::PointCloud map_points, tmp_points; 73 | static int frame_count = 0; 74 | 75 | void LaserHandler(const sensor_msgs::PointCloud2ConstPtr &laser_msg) { 76 | tmp_points_msg = *laser_msg; 77 | tmp_points.clear(); 78 | pcl::fromROSMsg(tmp_points_msg, tmp_points); 79 | laser_available = true; 80 | } 81 | 82 | void OdomHandler(const nav_msgs::Odometry::ConstPtr &odom_msg) { 83 | tmp_odom_msg = *odom_msg; 84 | odom_available = true; 85 | } 86 | 87 | void ProcessData() { 88 | if (laser_available && odom_available && (tmp_odom_msg.header.stamp - tmp_points_msg.header.stamp).toSec() < 0.005) { 89 | odom_available = false; 90 | laser_available = false; 91 | } else { 92 | return; 93 | } 94 | 95 | Eigen::Affine3f transform_to_init; 96 | transform_to_init.setIdentity(); 97 | 98 | // transform_to_init.translation() = 99 | // Eigen::Vector3f(tmp_odom_msg.pose.pose.position.x, 100 | // tmp_odom_msg.pose.pose.position.y, 101 | // tmp_odom_msg.pose.pose.position.z); 102 | // 103 | // Eigen::Quaternionf tmp_quat; 104 | // tmp_quat.x() = tmp_odom_msg.pose.pose.orientation.x; 105 | // tmp_quat.y() = tmp_odom_msg.pose.pose.orientation.y; 106 | // tmp_quat.z() = tmp_odom_msg.pose.pose.orientation.z; 107 | // tmp_quat.w() = tmp_odom_msg.pose.pose.orientation.w; 108 | // 109 | // transform_to_init.linear() = tmp_quat.normalized().toRotationMatrix(); 110 | 111 | pcl::PointCloud transformed_cloud; 112 | 113 | pcl::transformPointCloud(tmp_points, transformed_cloud, transform_to_init); 114 | 115 | map_points += transformed_cloud; 116 | 117 | DLOG(INFO) << "transform_to_init: " << std::endl << transform_to_init.matrix(); 118 | DLOG(INFO) << "merged: " << frame_count << "th frame"; 119 | ++frame_count; 120 | 121 | } 122 | 123 | // Load bag 124 | void LoadBag(const std::string &input_filename, const std::string &output_filename) { 125 | rosbag::Bag bag; 126 | bag.open(input_filename, rosbag::bagmode::Read); 127 | 128 | std::vector topics; 129 | topics.push_back(std::string(FLAGS_odom_name)); 130 | topics.push_back(std::string(FLAGS_laser_name)); 131 | 132 | rosbag::View view(bag, rosbag::TopicQuery(topics)); 133 | 134 | foreach(rosbag::MessageInstance const m, view) { 135 | sensor_msgs::PointCloud2ConstPtr ptc = m.instantiate(); 136 | nav_msgs::OdometryConstPtr odom = m.instantiate(); 137 | 138 | if (odom != NULL) { 139 | OdomHandler(odom); 140 | ProcessData(); 141 | } 142 | 143 | if (ptc != NULL) { 144 | LaserHandler(ptc); 145 | ProcessData(); 146 | } 147 | 148 | if (!ros::ok()) { 149 | break; 150 | } 151 | 152 | } 153 | 154 | bag.close(); 155 | 156 | if (map_points.size() > 0) { 157 | pcl::PCDWriter pcd_writer; 158 | DLOG(INFO) << "saving..."; 159 | pcd_writer.writeBinary(output_filename, map_points); 160 | DLOG(INFO) << "saved as " << output_filename; 161 | } else { 162 | LOG(FATAL) << "no points saved"; 163 | } 164 | } 165 | 166 | int main(int argc, char **argv) { 167 | 168 | google::InitGoogleLogging(argv[0]); 169 | google::ParseCommandLineFlags(&argc, &argv, true); 170 | FLAGS_alsologtostderr = true; 171 | 172 | ros::init(argc, argv, "map_builder"); 173 | 174 | ros::NodeHandle nh("~");; 175 | 176 | std::string input_filename = FLAGS_input_filename; 177 | std::string output_filename = FLAGS_output_filename; 178 | 179 | if (input_filename != "" && output_filename != "") { 180 | LoadBag(input_filename, output_filename); 181 | } else { 182 | LOG(FATAL) << "empty file_name"; 183 | } 184 | 185 | return 0; 186 | } 187 | 188 | -------------------------------------------------------------------------------- /include/3rdparty/sophus/geometry.hpp: -------------------------------------------------------------------------------- 1 | #ifndef GEOMETRY_HPP 2 | #define GEOMETRY_HPP 3 | 4 | #include "se2.hpp" 5 | #include "se3.hpp" 6 | #include "so2.hpp" 7 | #include "so3.hpp" 8 | #include "types.hpp" 9 | 10 | namespace Sophus { 11 | 12 | // Takes in a rotation ``R_foo_plane`` and returns the corresponding line 13 | // normal along the y-axis (in reference frame ``foo``). 14 | // 15 | template 16 | Vector2 normalFromSO2(SO2 const& R_foo_line) { 17 | return R_foo_line.matrix().col(1); 18 | } 19 | 20 | // Takes in line normal in reference frame foo and constructs a corresponding 21 | // rotation matrix ``R_foo_line``. 22 | // 23 | // Precondition: ``normal_foo`` must not be close to zero. 24 | // 25 | template 26 | SO2 SO2FromNormal(Vector2 normal_foo) { 27 | SOPHUS_ENSURE(normal_foo.squaredNorm() > Constants::epsilon(), "%", 28 | normal_foo.transpose()); 29 | normal_foo.normalize(); 30 | return SO2(normal_foo.y(), -normal_foo.x()); 31 | } 32 | 33 | // Takes in a rotation ``R_foo_plane`` and returns the corresponding plane 34 | // normal along the z-axis 35 | // (in reference frame ``foo``). 36 | // 37 | template 38 | Vector3 normalFromSO3(SO3 const& R_foo_plane) { 39 | return R_foo_plane.matrix().col(2); 40 | } 41 | 42 | // Takes in plane normal in reference frame foo and constructs a corresponding 43 | // rotation matrix ``R_foo_plane``. 44 | // 45 | // Note: The ``plane`` frame is defined as such that the normal points along the 46 | // positive z-axis. One can specify hints for the x-axis and y-axis of the 47 | // ``plane`` frame. 48 | // 49 | // Preconditions: 50 | // - ``normal_foo``, ``xDirHint_foo``, ``yDirHint_foo`` must not be close to 51 | // zero. 52 | // - ``xDirHint_foo`` and ``yDirHint_foo`` must be approx. perpendicular. 53 | // 54 | template 55 | Matrix3 rotationFromNormal(Vector3 const& normal_foo, 56 | Vector3 xDirHint_foo = Vector3(T(1), T(0), 57 | T(0)), 58 | Vector3 yDirHint_foo = Vector3(T(0), T(1), 59 | T(0))) { 60 | SOPHUS_ENSURE(xDirHint_foo.dot(yDirHint_foo) < Constants::epsilon(), 61 | "xDirHint (%) and yDirHint (%) must be perpendicular.", 62 | xDirHint_foo.transpose(), yDirHint_foo.transpose()); 63 | using std::abs; 64 | using std::sqrt; 65 | T const xDirHint_foo_sqr_length = xDirHint_foo.squaredNorm(); 66 | T const yDirHint_foo_sqr_length = yDirHint_foo.squaredNorm(); 67 | T const normal_foo_sqr_length = normal_foo.squaredNorm(); 68 | SOPHUS_ENSURE(xDirHint_foo_sqr_length > Constants::epsilon(), "%", 69 | xDirHint_foo.transpose()); 70 | SOPHUS_ENSURE(yDirHint_foo_sqr_length > Constants::epsilon(), "%", 71 | yDirHint_foo.transpose()); 72 | SOPHUS_ENSURE(normal_foo_sqr_length > Constants::epsilon(), "%", 73 | normal_foo.transpose()); 74 | 75 | Matrix3 basis_foo; 76 | basis_foo.col(2) = normal_foo; 77 | 78 | if (abs(xDirHint_foo_sqr_length - T(1)) > Constants::epsilon()) { 79 | xDirHint_foo.normalize(); 80 | } 81 | if (abs(yDirHint_foo_sqr_length - T(1)) > Constants::epsilon()) { 82 | yDirHint_foo.normalize(); 83 | } 84 | if (abs(normal_foo_sqr_length - T(1)) > Constants::epsilon()) { 85 | basis_foo.col(2).normalize(); 86 | } 87 | 88 | T abs_x_dot_z = abs(basis_foo.col(2).dot(xDirHint_foo)); 89 | T abs_y_dot_z = abs(basis_foo.col(2).dot(yDirHint_foo)); 90 | if (abs_x_dot_z < abs_y_dot_z) { 91 | // basis_foo.z and xDirHint are far from parallel. 92 | basis_foo.col(1) = basis_foo.col(2).cross(xDirHint_foo).normalized(); 93 | basis_foo.col(0) = basis_foo.col(1).cross(basis_foo.col(2)); 94 | } else { 95 | // basis_foo.z and yDirHint are far from parallel. 96 | basis_foo.col(0) = yDirHint_foo.cross(basis_foo.col(2)).normalized(); 97 | basis_foo.col(1) = basis_foo.col(2).cross(basis_foo.col(0)); 98 | } 99 | T det = basis_foo.determinant(); 100 | // sanity check 101 | SOPHUS_ENSURE(abs(det - T(1)) < Constants::epsilon(), 102 | "Determinant of basis is not 1, but %. Basis is \n%\n", det, 103 | basis_foo); 104 | return basis_foo; 105 | } 106 | 107 | // Takes in plane normal in reference frame foo and constructs a corresponding 108 | // rotation matrix ``R_foo_plane``. 109 | // 110 | // See ``rotationFromNormal`` for details. 111 | // 112 | template 113 | SO3 SO3FromNormal(Vector3 const& normal_foo) { 114 | return SO3(rotationFromNormal(normal_foo)); 115 | } 116 | 117 | // Returns a line (wrt. to frame ``foo``), given a pose of the ``line`` in 118 | // reference frame ``foo``. 119 | // 120 | // Note: The plane is defined by X-axis of the ``line`` frame. 121 | // 122 | template 123 | Line2 lineFromSE2(SE2 const& T_foo_line) { 124 | return Line2(normalFromSO2(T_foo_line.so2()), T_foo_line.translation()); 125 | } 126 | 127 | // Returns the pose ``T_foo_line``, given a line in reference frame ``foo``. 128 | // 129 | // Note: The line is defined by X-axis of the frame ``line``. 130 | // 131 | template 132 | SE2 SE2FromLine(Line2 const& line_foo) { 133 | T const d = line_foo.offset(); 134 | Vector2 const n = line_foo.normal(); 135 | SO2 const R_foo_plane = SO2FromNormal(n); 136 | return SE2(R_foo_plane, -d * n); 137 | } 138 | 139 | // Returns a plane (wrt. to frame ``foo``), given a pose of the ``plane`` in 140 | // reference frame ``foo``. 141 | // 142 | // Note: The plane is defined by XY-plane of the frame ``plane``. 143 | // 144 | template 145 | Plane3 planeFromSE3(SE3 const& T_foo_plane) { 146 | return Plane3(normalFromSO3(T_foo_plane.so3()), T_foo_plane.translation()); 147 | } 148 | 149 | // Returns the pose ``T_foo_plane``, given a plane in reference frame ``foo``. 150 | // 151 | // Note: The plane is defined by XY-plane of the frame ``plane``. 152 | // 153 | template 154 | SE3 SE3FromPlane(Plane3 const& plane_foo) { 155 | T const d = plane_foo.offset(); 156 | Vector3 const n = plane_foo.normal(); 157 | SO3 const R_foo_plane = SO3FromNormal(n); 158 | return SE3(R_foo_plane, -d * n); 159 | } 160 | 161 | // Takes in a hyperplane and returns unique representation by ensuring that the 162 | // ``offset`` is not negative. 163 | // 164 | template 165 | Eigen::Hyperplane makeHyperplaneUnique( 166 | Eigen::Hyperplane const& plane) { 167 | if (plane.offset() >= 0) { 168 | return plane; 169 | } 170 | 171 | return Eigen::Hyperplane(-plane.normal(), -plane.offset()); 172 | } 173 | 174 | } // namespace Sophus 175 | 176 | #endif // GEOMETRY_HPP 177 | -------------------------------------------------------------------------------- /include/3rdparty/sophus/common.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SOPHUS_COMMON_HPP 2 | #define SOPHUS_COMMON_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | 13 | // following boost's assert.hpp 14 | #undef SOPHUS_ENSURE 15 | 16 | // ENSURES are similar to ASSERTS, but they are always checked for (including in 17 | // release builds). At the moment there are no ASSERTS in Sophus which should 18 | // only be used for checks which are performance critical. 19 | 20 | #ifdef __GNUC__ 21 | #define SOPHUS_FUNCTION __PRETTY_FUNCTION__ 22 | #elif (_MSC_VER >= 1310) 23 | #define SOPHUS_FUNCTION __FUNCTION__ 24 | #else 25 | #define SOPHUS_FUNCTION "unknown" 26 | #endif 27 | 28 | // Make sure this compiles with older versions of Eigen which do not have 29 | // EIGEN_DEVICE_FUNC defined. 30 | #ifndef EIGEN_DEVICE_FUNC 31 | #define EIGEN_DEVICE_FUNC 32 | #endif 33 | 34 | #define SOPHUS_FUNC EIGEN_DEVICE_FUNC 35 | 36 | namespace Sophus { 37 | namespace details { 38 | 39 | // Following: http://stackoverflow.com/a/22759544 40 | template 41 | class IsStreamable { 42 | private: 43 | template 44 | static auto test(int) 45 | -> decltype(std::declval() << std::declval(), 46 | std::true_type()); 47 | 48 | template 49 | static auto test(...) -> std::false_type; 50 | 51 | public: 52 | static bool const value = decltype(test(0))::value; 53 | }; 54 | 55 | template 56 | class ArgToStream { 57 | public: 58 | static void impl(std::stringstream& stream, T&& arg) { 59 | stream << std::forward(arg); 60 | } 61 | }; 62 | 63 | inline void FormatStream(std::stringstream& stream, char const* text) { 64 | stream << text; 65 | return; 66 | } 67 | 68 | // Following: http://en.cppreference.com/w/cpp/language/parameter_pack 69 | template 70 | void FormatStream(std::stringstream& stream, char const* text, T&& arg, 71 | Args&&... args) { 72 | static_assert(IsStreamable::value, 73 | "One of the args has no ostream overload!"); 74 | for (; *text != '\0'; ++text) { 75 | if (*text == '%') { 76 | ArgToStream::impl(stream, std::forward(arg)); 77 | FormatStream(stream, text + 1, std::forward(args)...); 78 | return; 79 | } 80 | stream << *text; 81 | } 82 | stream << "\nFormat-Warning: There are " << sizeof...(Args) + 1 83 | << " args unused."; 84 | return; 85 | } 86 | 87 | template 88 | std::string FormatString(char const* text, Args&&... args) { 89 | std::stringstream stream; 90 | FormatStream(stream, text, std::forward(args)...); 91 | return stream.str(); 92 | } 93 | 94 | inline std::string FormatString() { return std::string(); } 95 | } // namespace details 96 | } // namespace Sophus 97 | 98 | #if defined(SOPHUS_DISABLE_ENSURES) 99 | 100 | #define SOPHUS_ENSURE(expr, ...) ((void)0) 101 | 102 | #elif defined(SOPHUS_ENABLE_ENSURE_HANDLER) 103 | 104 | namespace Sophus { 105 | void ensureFailed(char const* function, char const* file, int line, 106 | char const* description); 107 | } 108 | 109 | #define SOPHUS_ENSURE(expr, ...) \ 110 | ((expr) ? ((void)0) \ 111 | : ::Sophus::ensureFailed( \ 112 | SOPHUS_FUNCTION, __FILE__, __LINE__, \ 113 | Sophus::details::FormatString(##__VA_ARGS__).c_str())) 114 | #else 115 | // LCOV_EXCL_START 116 | 117 | namespace Sophus { 118 | template 119 | SOPHUS_FUNC void defaultEnsure(char const* function, char const* file, int line, 120 | char const* description, Args&&... args) { 121 | std::printf("Sophus ensure failed in function '%s', file '%s', line %d.\n", 122 | function, file, line); 123 | #ifdef __CUDACC__ 124 | std::printf("%s", description); 125 | #else 126 | std::cout << details::FormatString(description, std::forward(args)...) 127 | << std::endl; 128 | std::abort(); 129 | #endif 130 | } 131 | } // namespace Sophus 132 | 133 | // LCOV_EXCL_STOP 134 | #define SOPHUS_ENSURE(expr, ...) \ 135 | ((expr) ? ((void)0) \ 136 | : Sophus::defaultEnsure(SOPHUS_FUNCTION, __FILE__, __LINE__, \ 137 | ##__VA_ARGS__)) 138 | #endif 139 | 140 | namespace Sophus { 141 | 142 | template 143 | struct Constants { 144 | SOPHUS_FUNC static Scalar epsilon() { return Scalar(1e-10); } 145 | 146 | SOPHUS_FUNC static Scalar epsilonSqrt() { 147 | using std::sqrt; 148 | return sqrt(epsilon()); 149 | } 150 | 151 | SOPHUS_FUNC static Scalar pi() { return Scalar(M_PI); } 152 | }; 153 | 154 | template <> 155 | struct Constants { 156 | SOPHUS_FUNC static float constexpr epsilon() { 157 | return static_cast(1e-5); 158 | } 159 | 160 | SOPHUS_FUNC static float epsilonSqrt() { return std::sqrt(epsilon()); } 161 | 162 | SOPHUS_FUNC static float constexpr pi() { return static_cast(M_PI); } 163 | }; 164 | 165 | // Leightweight optional implementation which require ``T`` to have a 166 | // default constructor. 167 | // 168 | // TODO: Replace with std::optional once Sophus moves to c++17. 169 | // 170 | struct nullopt_t { 171 | explicit constexpr nullopt_t() {} 172 | }; 173 | 174 | constexpr nullopt_t nullopt{}; 175 | template 176 | 177 | class optional { 178 | public: 179 | optional() : is_valid_(false) {} 180 | 181 | optional(nullopt_t) : is_valid_(false) {} 182 | 183 | optional(T const& type) : type_(type), is_valid_(true) {} 184 | 185 | explicit operator bool() const { return is_valid_; } 186 | 187 | T const* operator->() const { 188 | SOPHUS_ENSURE(is_valid_, "must be valid"); 189 | return &type_; 190 | } 191 | 192 | T* operator->() { 193 | SOPHUS_ENSURE(is_valid_, "must be valid"); 194 | return &type_; 195 | } 196 | 197 | T const& operator*() const { 198 | SOPHUS_ENSURE(is_valid_, "must be valid"); 199 | return type_; 200 | } 201 | 202 | T& operator*() { 203 | SOPHUS_ENSURE(is_valid_, "must be valid"); 204 | return type_; 205 | } 206 | 207 | private: 208 | T type_; 209 | bool is_valid_; 210 | }; 211 | 212 | template 213 | using enable_if_t = typename std::enable_if::type; 214 | 215 | template 216 | struct IsUniformRandomBitGenerator { 217 | static const bool value = std::is_unsigned::value && 218 | std::is_unsigned::value && 219 | std::is_unsigned::value; 220 | }; 221 | } // namespace Sophus 222 | 223 | #endif // SOPHUS_COMMON_HPP 224 | -------------------------------------------------------------------------------- /include/3rdparty/sophus/types.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SOPHUS_TYEPES_HPP 2 | #define SOPHUS_TYEPES_HPP 3 | 4 | #include "common.hpp" 5 | 6 | namespace Sophus { 7 | 8 | template 9 | using Vector = Eigen::Matrix; 10 | 11 | template 12 | using Vector2 = Vector; 13 | using Vector2f = Vector2; 14 | using Vector2d = Vector2; 15 | 16 | template 17 | using Vector3 = Vector; 18 | using Vector3f = Vector3; 19 | using Vector3d = Vector3; 20 | 21 | template 22 | using Vector4 = Vector; 23 | using Vector4f = Vector4; 24 | using Vector4d = Vector4; 25 | 26 | template 27 | using Vector6 = Vector; 28 | using Vector6f = Vector6; 29 | using Vector6d = Vector6; 30 | 31 | template 32 | using Vector7 = Vector; 33 | using Vector7f = Vector7; 34 | using Vector7d = Vector7; 35 | 36 | template 37 | using Matrix = Eigen::Matrix; 38 | 39 | template 40 | using Matrix2 = Matrix; 41 | using Matrix2f = Matrix2; 42 | using Matrix2d = Matrix2; 43 | 44 | template 45 | using Matrix3 = Matrix; 46 | using Matrix3f = Matrix3; 47 | using Matrix3d = Matrix3; 48 | 49 | template 50 | using Matrix4 = Matrix; 51 | using Matrix4f = Matrix4; 52 | using Matrix4d = Matrix4; 53 | 54 | template 55 | using Matrix6 = Matrix; 56 | using Matrix6f = Matrix6; 57 | using Matrix6d = Matrix6; 58 | 59 | template 60 | using Matrix7 = Matrix; 61 | using Matrix7f = Matrix7; 62 | using Matrix7d = Matrix7; 63 | 64 | template 65 | using ParametrizedLine = Eigen::ParametrizedLine; 66 | 67 | template 68 | using ParametrizedLine3 = ParametrizedLine; 69 | using ParametrizedLine3f = ParametrizedLine3; 70 | using ParametrizedLine3d = ParametrizedLine3; 71 | 72 | template 73 | using ParametrizedLine2 = ParametrizedLine; 74 | using ParametrizedLine2f = ParametrizedLine2; 75 | using ParametrizedLine2d = ParametrizedLine2; 76 | 77 | namespace details { 78 | template 79 | class MaxMetric { 80 | public: 81 | static Scalar impl(Scalar s0, Scalar s1) { 82 | using std::abs; 83 | return abs(s0 - s1); 84 | } 85 | }; 86 | 87 | template 88 | class MaxMetric> { 89 | public: 90 | static Scalar impl(Matrix const& p0, 91 | Matrix const& p1) { 92 | return (p0 - p1).template lpNorm(); 93 | } 94 | }; 95 | 96 | template 97 | class SetToZero { 98 | public: 99 | static void impl(Scalar& s) { s = Scalar(0); } 100 | }; 101 | 102 | template 103 | class SetToZero> { 104 | public: 105 | static void impl(Matrix& v) { v.setZero(); } 106 | }; 107 | 108 | template 109 | class SetElementAt; 110 | 111 | template 112 | class SetElementAt { 113 | public: 114 | static void impl(Scalar& s, Scalar value, int at) { 115 | SOPHUS_ENSURE(at == 0, "is %", at); 116 | s = value; 117 | } 118 | }; 119 | 120 | template 121 | class SetElementAt, Scalar> { 122 | public: 123 | static void impl(Vector& v, Scalar value, int at) { 124 | SOPHUS_ENSURE(at >= 0 && at < N, "is %", at); 125 | v[at] = value; 126 | } 127 | }; 128 | 129 | template 130 | class SquaredNorm { 131 | public: 132 | static Scalar impl(Scalar const& s) { return s * s; } 133 | }; 134 | 135 | template 136 | class SquaredNorm> { 137 | public: 138 | static Scalar impl(Matrix const& s) { return s.squaredNorm(); } 139 | }; 140 | 141 | template 142 | class Transpose { 143 | public: 144 | static Scalar impl(Scalar const& s) { return s; } 145 | }; 146 | 147 | template 148 | class Transpose> { 149 | public: 150 | static Matrix impl(Matrix const& s) { 151 | return s.transpose(); 152 | } 153 | }; 154 | } // namespace details 155 | 156 | // Returns maximum metric between two points ``p0`` and ``p1``, with ``p`` 157 | // being a matrix or a scalar. 158 | // 159 | template 160 | auto maxMetric(T const& p0, T const& p1) 161 | -> decltype(details::MaxMetric::impl(p0, p1)) { 162 | return details::MaxMetric::impl(p0, p1); 163 | } 164 | 165 | // Sets point ``p`` to zero, with ``p`` being a matrix or a scalar. 166 | // 167 | template 168 | void setToZero(T& p) { 169 | return details::SetToZero::impl(p); 170 | } 171 | 172 | // Sets point ``p`` to zero, with ``p`` being a matrix or a scalar. 173 | // 174 | template 175 | void setElementAt(T& p, Scalar value, int at) { 176 | return details::SetElementAt::impl(p, value, at); 177 | } 178 | 179 | // Returns the squared 2-norm of ``p``, with ``p`` being a vector or a scalar. 180 | // 181 | template 182 | auto squaredNorm(T const& p) -> decltype(details::SquaredNorm::impl(p)) { 183 | return details::SquaredNorm::impl(p); 184 | } 185 | 186 | // Returns ``p.transpose()`` if ``p`` is a matrix, and simply ``p`` if m is a 187 | // scalar. 188 | // 189 | template 190 | auto transpose(T const& p) -> decltype(details::Transpose::impl(T())) { 191 | return details::Transpose::impl(p); 192 | } 193 | 194 | template 195 | struct IsFloatingPoint { 196 | static bool const value = std::is_floating_point::value; 197 | }; 198 | 199 | template 200 | struct IsFloatingPoint> { 201 | static bool const value = std::is_floating_point::value; 202 | }; 203 | 204 | template 205 | struct GetScalar { 206 | using Scalar = Scalar_; 207 | }; 208 | 209 | template 210 | struct GetScalar> { 211 | using Scalar = Scalar_; 212 | }; 213 | 214 | // Planes in 3d are hyperplanes. 215 | template 216 | using Plane3 = Eigen::Hyperplane; 217 | using Plane3d = Plane3; 218 | using Plane3f = Plane3; 219 | 220 | // Lines in 2d are hyperplanes. 221 | template 222 | using Line2 = Eigen::Hyperplane; 223 | using Line2d = Line2; 224 | using Line2f = Line2; 225 | 226 | } // namespace Sophus 227 | 228 | #endif // SOPHUS_TYEPES_HPP 229 | -------------------------------------------------------------------------------- /include/3rdparty/sophus/test_macros.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SOPUHS_TESTS_MACROS_HPP 2 | #define SOPUHS_TESTS_MACROS_HPP 3 | 4 | #include 5 | 6 | namespace Sophus { 7 | namespace details { 8 | 9 | template 10 | class Pretty { 11 | public: 12 | static std::string impl(Scalar s) { return FormatString("%", s); } 13 | }; 14 | 15 | template 16 | class Pretty> { 17 | public: 18 | static std::string impl(Matrix const& v) { 19 | return FormatString("\n%\n", v); 20 | } 21 | }; 22 | 23 | template 24 | std::string pretty(T const& v) { 25 | return Pretty::impl(v); 26 | } 27 | 28 | template 29 | void testFailed(bool& passed, char const* func, char const* file, int line, 30 | std::string const& msg) { 31 | std::cerr << FormatString("Test failed in function %, file %, line %\n", func, 32 | file, line); 33 | std::cerr << msg << "\n\n"; 34 | passed = false; 35 | } 36 | } // namespace details 37 | 38 | void processTestResult(bool passed) { 39 | if (!passed) { 40 | // LCOV_EXCL_START 41 | std::cerr << "failed!" << std::endl << std::endl; 42 | exit(-1); 43 | // LCOV_EXCL_STOP 44 | } 45 | std::cerr << "passed." << std::endl << std::endl; 46 | } 47 | } // namespace Sophus 48 | 49 | #define SOPHUS_STRINGIFY(x) #x 50 | 51 | // GenericTests whether condition is true. 52 | // The in-out parameter passed will be set to false if test fails. 53 | #define SOPHUS_TEST(passed, condition, ...) \ 54 | do { \ 55 | if (!(condition)) { \ 56 | std::string msg = Sophus::details::FormatString( \ 57 | "condition ``%`` is false\n", SOPHUS_STRINGIFY(condition)); \ 58 | msg += Sophus::details::FormatString(__VA_ARGS__); \ 59 | Sophus::details::testFailed(passed, SOPHUS_FUNCTION, __FILE__, __LINE__, \ 60 | msg); \ 61 | } \ 62 | } while (false) 63 | 64 | // GenericTests whether left is equal to right given a threshold. 65 | // The in-out parameter passed will be set to false if test fails. 66 | #define SOPHUS_TEST_EQUAL(passed, left, right, ...) \ 67 | do { \ 68 | if (left != right) { \ 69 | std::string msg = Sophus::details::FormatString( \ 70 | "% (=%) is not equal to % (=%)\n", SOPHUS_STRINGIFY(left), \ 71 | Sophus::details::pretty(left), SOPHUS_STRINGIFY(right), \ 72 | Sophus::details::pretty(right)); \ 73 | msg += Sophus::details::FormatString(__VA_ARGS__); \ 74 | Sophus::details::testFailed(passed, SOPHUS_FUNCTION, __FILE__, __LINE__, \ 75 | msg); \ 76 | } \ 77 | } while (false) 78 | 79 | // GenericTests whether left is equal to right given a threshold. 80 | // The in-out parameter passed will be set to false if test fails. 81 | #define SOPHUS_TEST_NEQ(passed, left, right, ...) \ 82 | do { \ 83 | if (left == right) { \ 84 | std::string msg = Sophus::details::FormatString( \ 85 | "% (=%) shoudl not be equal to % (=%)\n", SOPHUS_STRINGIFY(left), \ 86 | Sophus::details::pretty(left), SOPHUS_STRINGIFY(right), \ 87 | Sophus::details::pretty(right)); \ 88 | msg += Sophus::details::FormatString(__VA_ARGS__); \ 89 | Sophus::details::testFailed(passed, SOPHUS_FUNCTION, __FILE__, __LINE__, \ 90 | msg); \ 91 | } \ 92 | } while (false) 93 | 94 | // GenericTests whether left is approximatly equal to right given a threshold. 95 | // The in-out parameter passed will be set to false if test fails. 96 | #define SOPHUS_TEST_APPROX(passed, left, right, thr, ...) \ 97 | do { \ 98 | auto nrm = Sophus::maxMetric((left), (right)); \ 99 | if (!(nrm < (thr))) { \ 100 | std::string msg = Sophus::details::FormatString( \ 101 | "% (=%) is not approx % (=%); % is %; nrm is %\n", \ 102 | SOPHUS_STRINGIFY(left), Sophus::details::pretty(left), \ 103 | SOPHUS_STRINGIFY(right), Sophus::details::pretty(right), \ 104 | SOPHUS_STRINGIFY(thr), Sophus::details::pretty(thr), nrm); \ 105 | msg += Sophus::details::FormatString(__VA_ARGS__); \ 106 | Sophus::details::testFailed(passed, SOPHUS_FUNCTION, __FILE__, __LINE__, \ 107 | msg); \ 108 | } \ 109 | } while (false) 110 | 111 | // GenericTests whether left is NOT approximatly equal to right given a 112 | // threshold. 113 | // The in-out parameter passed will be set to false if test fails. 114 | #define SOPHUS_TEST_NOT_APPROX(passed, left, right, thr, ...) \ 115 | do { \ 116 | auto nrm = Sophus::maxMetric((left), (right)); \ 117 | if (nrm < (thr)) { \ 118 | std::string msg = Sophus::details::FormatString( \ 119 | "% (=%) is approx % (=%), but it should not!\n % is %; nrm is %\n", \ 120 | SOPHUS_STRINGIFY(left), Sophus::details::pretty(left), \ 121 | SOPHUS_STRINGIFY(right), Sophus::details::pretty(right), \ 122 | SOPHUS_STRINGIFY(thr), Sophus::details::pretty(thr), nrm); \ 123 | msg += Sophus::details::FormatString(__VA_ARGS__); \ 124 | Sophus::details::testFailed(passed, SOPHUS_FUNCTION, __FILE__, __LINE__, \ 125 | msg); \ 126 | } \ 127 | } while (false) 128 | 129 | #endif // SOPUHS_TESTS_MACROS_HPP 130 | -------------------------------------------------------------------------------- /src/estimator_node.cc: -------------------------------------------------------------------------------- 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 5/31/18. 29 | // 30 | 31 | #include 32 | #include 33 | 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | #include 40 | 41 | #include 42 | 43 | #include 44 | #include 45 | 46 | #include "imu_processor/Estimator.h" 47 | #include "point_processor/PointOdometry.h" 48 | #include "utils/TicToc.h" 49 | 50 | using namespace lio; 51 | using namespace std; 52 | using namespace mathutils; 53 | 54 | static ros::NodeHandlePtr nh_ptr; 55 | 56 | static std::string config_file = "/home/hyye/dev_ws/src/lio/config/test_config.yaml"; 57 | 58 | void Run() { 59 | 60 | DLOG(INFO) << "config_file: " << config_file; 61 | 62 | MeasurementManagerConfig mm_config; 63 | EstimatorConfig estimator_config; 64 | cv::FileStorage fs_settings(config_file, cv::FileStorage::READ); 65 | 66 | { 67 | int tmp_int; 68 | double tmp_double; 69 | estimator_config.min_match_sq_dis = fs_settings["min_match_sq_dis"]; 70 | estimator_config.min_plane_dis = fs_settings["min_plane_dis"]; 71 | 72 | estimator_config.corner_filter_size = fs_settings["corner_filter_size"]; 73 | estimator_config.surf_filter_size = fs_settings["surf_filter_size"]; 74 | estimator_config.map_filter_size = fs_settings["map_filter_size"]; 75 | 76 | tmp_int = fs_settings["window_size"]; 77 | estimator_config.window_size = size_t(tmp_int); 78 | 79 | tmp_int = fs_settings["opt_window_size"]; 80 | estimator_config.opt_window_size = size_t(tmp_int); 81 | estimator_config.init_window_factor = fs_settings["init_window_factor"]; 82 | 83 | tmp_int = fs_settings["estimate_extrinsic"]; 84 | estimator_config.estimate_extrinsic = tmp_int; 85 | 86 | tmp_int = fs_settings["opt_extrinsic"]; 87 | estimator_config.opt_extrinsic = (tmp_int > 0); 88 | 89 | cv::Mat cv_R, cv_T; 90 | fs_settings["extrinsic_rotation"] >> cv_R; 91 | fs_settings["extrinsic_translation"] >> cv_T; 92 | Eigen::Matrix3d eigen_R; 93 | Eigen::Vector3d eigen_T; 94 | cv::cv2eigen(cv_R, eigen_R); 95 | cv::cv2eigen(cv_T, eigen_T); 96 | 97 | estimator_config.transform_lb = Transform{Eigen::Quaternionf(eigen_R.cast()), eigen_T.cast()}; 98 | 99 | tmp_int = fs_settings["run_optimization"]; 100 | estimator_config.run_optimization = (tmp_int > 0); 101 | tmp_int = fs_settings["update_laser_imu"]; 102 | estimator_config.update_laser_imu = (tmp_int > 0); 103 | tmp_int = fs_settings["gravity_fix"]; 104 | estimator_config.gravity_fix = (tmp_int > 0); 105 | tmp_int = fs_settings["plane_projection_factor"]; 106 | estimator_config.plane_projection_factor = (tmp_int > 0); 107 | tmp_int = fs_settings["imu_factor"]; 108 | estimator_config.imu_factor = (tmp_int > 0); 109 | tmp_int = fs_settings["point_distance_factor"]; 110 | estimator_config.point_distance_factor = (tmp_int > 0); 111 | tmp_int = fs_settings["prior_factor"]; 112 | estimator_config.prior_factor = (tmp_int > 0); 113 | tmp_int = fs_settings["marginalization_factor"]; 114 | estimator_config.marginalization_factor = (tmp_int > 0); 115 | 116 | tmp_int = fs_settings["pcl_viewer"]; 117 | estimator_config.pcl_viewer = (tmp_int > 0); 118 | 119 | tmp_int = fs_settings["enable_deskew"]; 120 | estimator_config.enable_deskew = (tmp_int > 0); 121 | tmp_int = fs_settings["cutoff_deskew"]; 122 | estimator_config.cutoff_deskew = (tmp_int > 0); 123 | 124 | tmp_int = fs_settings["keep_features"]; 125 | estimator_config.keep_features = (tmp_int > 0); 126 | 127 | tmp_double = fs_settings["acc_n"]; 128 | estimator_config.pim_config.acc_n = tmp_double; 129 | tmp_double = fs_settings["gyr_n"]; 130 | estimator_config.pim_config.gyr_n = tmp_double; 131 | tmp_double = fs_settings["acc_w"]; 132 | estimator_config.pim_config.acc_w = tmp_double; 133 | tmp_double = fs_settings["gyr_w"]; 134 | estimator_config.pim_config.gyr_w = tmp_double; 135 | tmp_double = fs_settings["g_norm"]; 136 | estimator_config.pim_config.g_norm = tmp_double; 137 | 138 | tmp_double = fs_settings["msg_time_delay"]; 139 | mm_config.msg_time_delay = tmp_double; 140 | } 141 | 142 | Estimator estimator(estimator_config); 143 | estimator.SetupRos(*nh_ptr); 144 | 145 | int odom_io = fs_settings["odom_io"]; 146 | 147 | PointOdometry odometry(0.1, odom_io); 148 | odometry.SetupRos(*nh_ptr); 149 | odometry.Reset(); 150 | 151 | thread odom(&PointOdometry::Spin, &odometry); 152 | 153 | thread measurement_manager(&Estimator::ProcessEstimation, &estimator); 154 | 155 | if (estimator_config.pcl_viewer) { 156 | boost::thread visualizer(boost::bind(&PlaneNormalVisualizer::Spin, &(estimator.normal_vis))); 157 | } 158 | 159 | // while (!estimator.normal_vis.viewer->wasStopped()) { 160 | // { 161 | // boost::mutex::scoped_lock lk(estimator.normal_vis.m); 162 | // DLOG(INFO) << ">>>>>>> spin <<<<<<<"; 163 | // estimator.normal_vis.viewer->spinOnce(100); 164 | // } 165 | // boost::this_thread::sleep(boost::posix_time::microseconds(100000)); 166 | // } 167 | 168 | ros::Rate r(1000); 169 | 170 | while (ros::ok()) { 171 | ros::spinOnce(); 172 | r.sleep(); 173 | } 174 | 175 | } 176 | 177 | int main(int argc, char **argv) { 178 | 179 | google::InitGoogleLogging(argv[0]); 180 | google::ParseCommandLineFlags(&argc, &argv, true); 181 | 182 | ros::init(argc, argv, "test_measurement_manager"); 183 | { 184 | ros::NodeHandle nh("~"); 185 | nh_ptr = boost::make_shared(nh); 186 | } 187 | 188 | nh_ptr->param("config_file", config_file, std::string("/home/hyye/dev_ws/src/lio/config/test_config.yaml")); 189 | FLAGS_alsologtostderr = true; 190 | 191 | Run(); 192 | 193 | return 0; 194 | } 195 | -------------------------------------------------------------------------------- /src/factor/PointDistanceFactor.cc: -------------------------------------------------------------------------------- 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 4/3/18. 29 | // 30 | 31 | #include "factor/PointDistanceFactor.h" 32 | 33 | namespace lio { 34 | 35 | static double sqrt_info_static = 100; 36 | 37 | PointDistanceFactor::PointDistanceFactor(const Eigen::Vector3d &point, 38 | const Eigen::Vector4d &coeff, 39 | const Eigen::Matrix info_mat) : point_{point}, 40 | coeff_{coeff}, 41 | info_mat_{info_mat} { 42 | /// add new point and coeff 43 | } 44 | 45 | bool PointDistanceFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const { 46 | TicToc tic_toc; 47 | 48 | Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]); 49 | Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]); 50 | 51 | Eigen::Vector3d tlb(parameters[1][0], parameters[1][1], parameters[1][2]); 52 | Eigen::Quaterniond qlb(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]); 53 | 54 | // Eigen::Vector3d tlb = transform_lb_.pos; 55 | // Eigen::Quaterniond qlb = transform_lb_.rot; 56 | 57 | 58 | Eigen::Quaterniond Qli = Qi * qlb.conjugate(); 59 | Eigen::Vector3d Pli = Pi - Qli * tlb; 60 | 61 | Eigen::Vector3d w(coeff_.x(), coeff_.y(), coeff_.z()); 62 | double b = coeff_.w(); 63 | 64 | double residual = (w.transpose() * (Qli * point_ + Pli) + b); 65 | 66 | double sqrt_info = sqrt_info_static; 67 | // FIXME: 100 magic number, info_mat 68 | residuals[0] = sqrt_info * residual; 69 | 70 | if (jacobians) { 71 | Eigen::Matrix3d Ri = Qi.toRotationMatrix(); 72 | Eigen::Matrix3d rlb = qlb.toRotationMatrix(); 73 | 74 | if (jacobians[0]) { 75 | Eigen::Map > jacobian_pose_i(jacobians[0]); 76 | Eigen::Matrix jaco_i; 77 | 78 | jaco_i.leftCols<3>() = w.transpose(); 79 | jaco_i.rightCols<3>() = 80 | -w.transpose() * Ri * (SkewSymmetric(rlb.transpose() * point_) - SkewSymmetric(rlb.transpose() * tlb)); 81 | 82 | // FIXME: 100 magic number, info_mat 83 | jacobian_pose_i.setZero(); 84 | jacobian_pose_i.leftCols<6>() = sqrt_info * jaco_i /* * info_mat_*/; 85 | jacobian_pose_i.rightCols<1>().setZero(); 86 | } 87 | 88 | if (jacobians[1]) { 89 | Eigen::Map > jacobian_pose_ex(jacobians[1]); 90 | jacobian_pose_ex.setZero(); 91 | 92 | Eigen::Matrix jaco_ex; 93 | jaco_ex.leftCols<3>() = -w.transpose() * Ri * rlb.transpose(); 94 | jaco_ex.rightCols<3>() = 95 | w.transpose() * Ri * (SkewSymmetric(rlb.transpose() * point_) - SkewSymmetric(rlb.transpose() * tlb)); 96 | 97 | // FIXME: 100 magic number, info_mat 98 | jacobian_pose_ex.setZero(); 99 | jacobian_pose_ex.leftCols<6>() = sqrt_info * jaco_ex; 100 | jacobian_pose_ex.rightCols<1>().setZero(); 101 | } 102 | } 103 | 104 | return true; 105 | } 106 | 107 | void PointDistanceFactor::Check(double **parameters) { 108 | Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]); 109 | Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]); 110 | 111 | Eigen::Vector3d tlb(parameters[1][0], parameters[1][1], parameters[1][2]); 112 | Eigen::Quaterniond qlb(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]); 113 | 114 | // Eigen::Vector3d tlb = transform_lb_.pos; 115 | // Eigen::Quaterniond qlb = transform_lb_.rot; 116 | 117 | Eigen::Quaterniond Qli = Qi * qlb.conjugate(); 118 | Eigen::Vector3d Pli = Pi - Qli * tlb; 119 | 120 | double *res = new double[1]; 121 | // double **jaco = new double *[1]; 122 | double **jaco = new double *[2]; 123 | jaco[0] = new double[1 * 7]; 124 | jaco[1] = new double[1 * 7]; 125 | Evaluate(parameters, res, jaco); 126 | DLOG(INFO) << "check begins"; 127 | DLOG(INFO) << "analytical"; 128 | 129 | DLOG(INFO) << *res; 130 | DLOG(INFO) << std::endl << Eigen::Map>(jaco[0]); 131 | DLOG(INFO) << std::endl << Eigen::Map>(jaco[1]); 132 | 133 | double sqrt_info = sqrt_info_static; 134 | 135 | Eigen::Vector3d coeffw(coeff_.x(), coeff_.y(), coeff_.z()); 136 | double coeffb = coeff_.w(); 137 | 138 | double residual = (coeffw.transpose() * (Qli * point_ + Pli) + coeffb); 139 | 140 | residual = sqrt_info * residual; 141 | DLOG(INFO) << "num"; 142 | DLOG(INFO) << residual; 143 | 144 | const double eps = 1e-6; 145 | // Eigen::Matrix num_jacobian; 146 | Eigen::Matrix num_jacobian; 147 | // for (int k = 0; k < 6; k++) { 148 | for (int k = 0; k < 12; k++) { 149 | Pi = Eigen::Vector3d{parameters[0][0], parameters[0][1], parameters[0][2]}; 150 | Qi = Eigen::Quaterniond(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]); 151 | 152 | tlb = Eigen::Vector3d(parameters[1][0], parameters[1][1], parameters[1][2]); 153 | qlb = Eigen::Quaterniond(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]); 154 | 155 | int a = k / 3, b = k % 3; 156 | Eigen::Vector3d delta = Eigen::Vector3d(b == 0, b == 1, b == 2) * eps; 157 | 158 | if (a == 0) 159 | Pi += delta; 160 | else if (a == 1) 161 | Qi = Qi * DeltaQ(delta); 162 | else if (a == 2) 163 | tlb += delta; 164 | else if (a == 3) 165 | qlb = qlb * DeltaQ(delta); 166 | 167 | Qli = Qi * qlb.conjugate(); 168 | Pli = Pi - Qli * tlb; 169 | 170 | double tmp_residual = (coeffw.transpose() * (Qli * point_ + Pli) + coeffb); 171 | 172 | tmp_residual = sqrt_info * tmp_residual; 173 | 174 | num_jacobian(k) = (tmp_residual - residual) / eps; 175 | } 176 | DLOG(INFO) << std::endl << num_jacobian.block<1, 6>(0, 0); 177 | DLOG(INFO) << std::endl << num_jacobian.block<1, 6>(0, 6); 178 | } 179 | 180 | } // namespace lio -------------------------------------------------------------------------------- /include/utils/math_utils.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/16/18. 29 | // 30 | 31 | #ifndef LIO_MATH_UTILS_H_ 32 | #define LIO_MATH_UTILS_H_ 33 | 34 | #include 35 | #include 36 | 37 | namespace mathutils { 38 | 39 | template 40 | inline T RadToDeg(T rad) { 41 | return rad * 180.0 / M_PI; 42 | } 43 | 44 | template 45 | inline T NormalizeRad(T rad) { 46 | rad = fmod(rad + M_PI, 2 * M_PI); 47 | if (rad < 0) { 48 | rad += 2 * M_PI; 49 | } 50 | return rad - M_PI; 51 | } 52 | 53 | template 54 | inline T DegToRad(T deg) { 55 | return deg / 180.0 * M_PI; 56 | } 57 | 58 | template 59 | inline T NormalizeDeg(T deg) { 60 | deg = fmod(deg + 180.0, 360.0); 61 | if (deg < 0) { 62 | deg += 360.0; 63 | } 64 | return deg - 180.0; 65 | } 66 | 67 | inline bool RadLt(double a, double b) { 68 | return NormalizeRad(a - b) < 0; 69 | } 70 | 71 | inline bool RadGt(double a, double b) { 72 | return NormalizeRad(a - b) > 0; 73 | } 74 | 75 | template 76 | inline PointT ScalePoint(const PointT &p, float scale) { 77 | PointT p_o = p; 78 | p_o.x *= scale; 79 | p_o.y *= scale; 80 | p_o.z *= scale; 81 | return p_o; 82 | } 83 | 84 | template 85 | inline float CalcSquaredDiff(const PointT &a, const PointT &b) { 86 | float diff_x = a.x - b.x; 87 | float diff_y = a.y - b.y; 88 | float diff_z = a.z - b.z; 89 | 90 | return diff_x * diff_x + diff_y * diff_y + diff_z * diff_z; 91 | } 92 | 93 | template 94 | inline float CalcSquaredDiff(const PointT &a, const PointT &b, const float &wb) { 95 | float diff_x = a.x - b.x * wb; 96 | float diff_y = a.y - b.y * wb; 97 | float diff_z = a.z - b.z * wb; 98 | 99 | return diff_x * diff_x + diff_y * diff_y + diff_z * diff_z; 100 | } 101 | 102 | template 103 | inline float CalcPointDistance(const PointT &p) { 104 | return sqrt(p.x * p.x + p.y * p.y + p.z * p.z); 105 | } 106 | 107 | template 108 | inline float CalcSquaredPointDistance(const PointT &p) { 109 | return p.x * p.x + p.y * p.y + p.z * p.z; 110 | } 111 | 112 | ///< Matrix calculations 113 | 114 | static const Eigen::Matrix3d I3x3 = Eigen::Matrix3d::Identity(); 115 | 116 | template 117 | inline Eigen::Quaternion DeltaQ(const Eigen::MatrixBase &theta) { 118 | typedef typename Derived::Scalar Scalar_t; 119 | 120 | Eigen::Quaternion dq; 121 | Eigen::Matrix half_theta = theta; 122 | half_theta /= static_cast(2.0); 123 | dq.w() = static_cast(1.0); 124 | dq.x() = half_theta.x(); 125 | dq.y() = half_theta.y(); 126 | dq.z() = half_theta.z(); 127 | return dq; 128 | } 129 | 130 | template 131 | inline Eigen::Matrix SkewSymmetric(const Eigen::MatrixBase &v3d) { 132 | Eigen::Matrix m; 133 | m << typename Derived::Scalar(0), -v3d.z(), v3d.y(), 134 | v3d.z(), typename Derived::Scalar(0), -v3d.x(), 135 | -v3d.y(), v3d.x(), typename Derived::Scalar(0); 136 | return m; 137 | } 138 | 139 | template 140 | inline Eigen::Matrix LeftQuatMatrix(const Eigen::QuaternionBase &q) { 141 | Eigen::Matrix m; 142 | Eigen::Matrix vq = q.vec(); 143 | typename Derived::Scalar q4 = q.w(); 144 | m.block(0, 0, 3, 3) << q4 * I3x3 + SkewSymmetric(vq); 145 | m.block(3, 0, 1, 3) << -vq.transpose(); 146 | m.block(0, 3, 3, 1) << vq; 147 | m(3, 3) = q4; 148 | return m; 149 | } 150 | 151 | template 152 | inline Eigen::Matrix RightQuatMatrix(const Eigen::QuaternionBase &p) { 153 | Eigen::Matrix m; 154 | Eigen::Matrix vp = p.vec(); 155 | typename Derived::Scalar p4 = p.w(); 156 | m.block(0, 0, 3, 3) << p4 * I3x3 - SkewSymmetric(vp); 157 | m.block(3, 0, 1, 3) << -vp.transpose(); 158 | m.block(0, 3, 3, 1) << vp; 159 | m(3, 3) = p4; 160 | return m; 161 | } 162 | 163 | template 164 | inline Eigen::Matrix LeftQuatMatrix(const Eigen::Matrix &q) { 165 | Eigen::Matrix m; 166 | Eigen::Matrix vq{q.x(), q.y(), q.z()}; 167 | T q4 = q.w(); 168 | m.block(0, 0, 3, 3) << q4 * I3x3 + SkewSymmetric(vq); 169 | m.block(3, 0, 1, 3) << -vq.transpose(); 170 | m.block(0, 3, 3, 1) << vq; 171 | m(3, 3) = q4; 172 | return m; 173 | } 174 | 175 | template 176 | inline Eigen::Matrix RightQuatMatrix(const Eigen::Matrix &p) { 177 | Eigen::Matrix m; 178 | Eigen::Matrix vp{p.x(), p.y(), p.z()}; 179 | T p4 = p.w(); 180 | m.block(0, 0, 3, 3) << p4 * I3x3 - SkewSymmetric(vp); 181 | m.block(3, 0, 1, 3) << -vp.transpose(); 182 | m.block(0, 3, 3, 1) << vp; 183 | m(3, 3) = p4; 184 | return m; 185 | } 186 | 187 | // adapted from VINS-mono 188 | inline Eigen::Vector3d R2ypr(const Eigen::Matrix3d &R) 189 | { 190 | Eigen::Vector3d n = R.col(0); 191 | Eigen::Vector3d o = R.col(1); 192 | Eigen::Vector3d a = R.col(2); 193 | 194 | Eigen::Vector3d ypr(3); 195 | double y = atan2(n(1), n(0)); 196 | double p = atan2(-n(2), n(0) * cos(y) + n(1) * sin(y)); 197 | double r = atan2(a(0) * sin(y) - a(1) * cos(y), -o(0) * sin(y) + o(1) * cos(y)); 198 | ypr(0) = y; 199 | ypr(1) = p; 200 | ypr(2) = r; 201 | 202 | return ypr / M_PI * 180.0; 203 | } 204 | 205 | template 206 | inline Eigen::Matrix ypr2R(const Eigen::MatrixBase &ypr) 207 | { 208 | typedef typename Derived::Scalar Scalar_t; 209 | 210 | Scalar_t y = ypr(0) / 180.0 * M_PI; 211 | Scalar_t p = ypr(1) / 180.0 * M_PI; 212 | Scalar_t r = ypr(2) / 180.0 * M_PI; 213 | 214 | Eigen::Matrix Rz; 215 | Rz << cos(y), -sin(y), 0, 216 | sin(y), cos(y), 0, 217 | 0, 0, 1; 218 | 219 | Eigen::Matrix Ry; 220 | Ry << cos(p), 0., sin(p), 221 | 0., 1., 0., 222 | -sin(p), 0., cos(p); 223 | 224 | Eigen::Matrix Rx; 225 | Rx << 1., 0., 0., 226 | 0., cos(r), -sin(r), 227 | 0., sin(r), cos(r); 228 | 229 | return Rz * Ry * Rx; 230 | } 231 | 232 | } // namespance mathutils 233 | 234 | #endif //LIO_MATH_UTILS_H_ 235 | -------------------------------------------------------------------------------- /include/factor/ImuFactor.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 4/4/18. 29 | // 30 | 31 | #ifndef LIO_IMUFACTOR_H_ 32 | #define LIO_IMUFACTOR_H_ 33 | 34 | /// adapted from VINS-mono 35 | 36 | #include 37 | #include "imu_processor/IntegrationBase.h" 38 | #include "utils/math_utils.h" 39 | 40 | namespace lio { 41 | 42 | using namespace mathutils; 43 | 44 | class ImuFactor : public ceres::SizedCostFunction<15, 7, 9, 7, 9> { 45 | 46 | public: 47 | ImuFactor() = delete; 48 | ImuFactor(std::shared_ptr pre_integration) : pre_integration_{ 49 | pre_integration} { 50 | // NOTE: g_vec_ is the gravity in laser's original frame 51 | g_vec_ = pre_integration_->g_vec_; 52 | } 53 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const { 54 | 55 | Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]); 56 | Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]); 57 | 58 | Eigen::Vector3d Vi(parameters[1][0], parameters[1][1], parameters[1][2]); 59 | Eigen::Vector3d Bai(parameters[1][3], parameters[1][4], parameters[1][5]); 60 | Eigen::Vector3d Bgi(parameters[1][6], parameters[1][7], parameters[1][8]); 61 | 62 | Eigen::Vector3d Pj(parameters[2][0], parameters[2][1], parameters[2][2]); 63 | Eigen::Quaterniond Qj(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]); 64 | 65 | Eigen::Vector3d Vj(parameters[3][0], parameters[3][1], parameters[3][2]); 66 | Eigen::Vector3d Baj(parameters[3][3], parameters[3][4], parameters[3][5]); 67 | Eigen::Vector3d Bgj(parameters[3][6], parameters[3][7], parameters[3][8]); 68 | 69 | Eigen::Map> residual(residuals); 70 | residual = pre_integration_->Evaluate(Pi, Qi, Vi, Bai, Bgi, 71 | Pj, Qj, Vj, Baj, Bgj); 72 | 73 | 74 | Eigen::Matrix sqrt_info = 75 | Eigen::LLT>(pre_integration_->covariance_.inverse()).matrixL().transpose(); 76 | 77 | residual = sqrt_info * residual; 78 | 79 | if (jacobians) { 80 | double sum_dt = pre_integration_->sum_dt_; 81 | Eigen::Matrix3d dp_dba = pre_integration_->jacobian_.template block<3, 3>(O_P, O_BA); 82 | Eigen::Matrix3d dp_dbg = pre_integration_->jacobian_.template block<3, 3>(O_P, O_BG); 83 | 84 | Eigen::Matrix3d dq_dbg = pre_integration_->jacobian_.template block<3, 3>(O_R, O_BG); 85 | 86 | Eigen::Matrix3d dv_dba = pre_integration_->jacobian_.template block<3, 3>(O_V, O_BA); 87 | Eigen::Matrix3d dv_dbg = pre_integration_->jacobian_.template block<3, 3>(O_V, O_BG); 88 | 89 | if (pre_integration_->jacobian_.maxCoeff() > 1e8 || pre_integration_->jacobian_.minCoeff() < -1e8) { 90 | ROS_DEBUG("numerical unstable in preintegration"); 91 | } 92 | 93 | if (jacobians[0]) { 94 | Eigen::Map> jacobian_pose_i(jacobians[0]); 95 | jacobian_pose_i.setZero(); 96 | 97 | jacobian_pose_i.block<3, 3>(O_P, O_P) = -Qi.inverse().toRotationMatrix(); 98 | jacobian_pose_i.block<3, 3>(O_P, O_R) = 99 | SkewSymmetric(Qi.inverse() * (-0.5 * g_vec_ * sum_dt * sum_dt + Pj - Pi - Vi * sum_dt)); 100 | 101 | Eigen::Quaterniond corrected_delta_q = 102 | pre_integration_->delta_q_ * DeltaQ(dq_dbg * (Bgi - pre_integration_->linearized_bg_)); 103 | jacobian_pose_i.block<3, 3>(O_R, O_R) = 104 | -(LeftQuatMatrix(Qj.inverse() * Qi) * RightQuatMatrix(corrected_delta_q)).topLeftCorner<3, 3>(); 105 | 106 | jacobian_pose_i.block<3, 3>(O_V, O_R) = 107 | SkewSymmetric(Qi.inverse() * (-g_vec_ * sum_dt + Vj - Vi)); 108 | 109 | jacobian_pose_i = sqrt_info * jacobian_pose_i; 110 | 111 | if (jacobian_pose_i.maxCoeff() > 1e8 || jacobian_pose_i.minCoeff() < -1e8) { 112 | ROS_DEBUG("numerical unstable in preintegration"); 113 | } 114 | } 115 | if (jacobians[1]) { 116 | Eigen::Map> jacobian_speedbias_i(jacobians[1]); 117 | jacobian_speedbias_i.setZero(); 118 | jacobian_speedbias_i.block<3, 3>(O_P, O_V - O_V) = -Qi.inverse().toRotationMatrix() * sum_dt; 119 | jacobian_speedbias_i.block<3, 3>(O_P, O_BA - O_V) = -dp_dba; 120 | jacobian_speedbias_i.block<3, 3>(O_P, O_BG - O_V) = -dp_dbg; 121 | 122 | Eigen::Quaterniond corrected_delta_q = 123 | pre_integration_->delta_q_ * DeltaQ(dq_dbg * (Bgi - pre_integration_->linearized_bg_)); 124 | jacobian_speedbias_i.block<3, 3>(O_R, O_BG - O_V) = 125 | -LeftQuatMatrix(Qj.inverse() * Qi * corrected_delta_q).topLeftCorner<3, 3>() * dq_dbg; 126 | 127 | jacobian_speedbias_i.block<3, 3>(O_V, O_V - O_V) = -Qi.inverse().toRotationMatrix(); 128 | jacobian_speedbias_i.block<3, 3>(O_V, O_BA - O_V) = -dv_dba; 129 | jacobian_speedbias_i.block<3, 3>(O_V, O_BG - O_V) = -dv_dbg; 130 | 131 | jacobian_speedbias_i.block<3, 3>(O_BA, O_BA - O_V) = -Eigen::Matrix3d::Identity(); 132 | 133 | jacobian_speedbias_i.block<3, 3>(O_BG, O_BG - O_V) = -Eigen::Matrix3d::Identity(); 134 | 135 | jacobian_speedbias_i = sqrt_info * jacobian_speedbias_i; 136 | 137 | } 138 | if (jacobians[2]) { 139 | Eigen::Map> jacobian_pose_j(jacobians[2]); 140 | jacobian_pose_j.setZero(); 141 | 142 | jacobian_pose_j.block<3, 3>(O_P, O_P) = Qi.inverse().toRotationMatrix(); 143 | 144 | Eigen::Quaterniond corrected_delta_q = 145 | pre_integration_->delta_q_ * DeltaQ(dq_dbg * (Bgi - pre_integration_->linearized_bg_)); 146 | jacobian_pose_j.block<3, 3>(O_R, O_R) = 147 | LeftQuatMatrix(corrected_delta_q.inverse() * Qi.inverse() * Qj).topLeftCorner<3, 3>(); 148 | 149 | jacobian_pose_j = sqrt_info * jacobian_pose_j; 150 | 151 | } 152 | if (jacobians[3]) { 153 | Eigen::Map> jacobian_speedbias_j(jacobians[3]); 154 | jacobian_speedbias_j.setZero(); 155 | 156 | jacobian_speedbias_j.block<3, 3>(O_V, O_V - O_V) = Qi.inverse().toRotationMatrix(); 157 | 158 | jacobian_speedbias_j.block<3, 3>(O_BA, O_BA - O_V) = Eigen::Matrix3d::Identity(); 159 | 160 | jacobian_speedbias_j.block<3, 3>(O_BG, O_BG - O_V) = Eigen::Matrix3d::Identity(); 161 | 162 | jacobian_speedbias_j = sqrt_info * jacobian_speedbias_j; 163 | 164 | } 165 | } 166 | 167 | return true; 168 | } 169 | 170 | std::shared_ptr pre_integration_; 171 | Eigen::Vector3d g_vec_; 172 | 173 | const double eps_ = 10e-8; 174 | 175 | }; 176 | 177 | } // namespace lio 178 | 179 | #endif //LIO_IMUFACTOR_H_ 180 | --------------------------------------------------------------------------------