├── aurova_odom ├── CMakeLists.txt └── package.xml ├── .project ├── dualquat_LOAM ├── include │ ├── dualquat │ │ ├── dualquat.h │ │ ├── quat_exponential.h │ │ ├── dualquat_query.h │ │ ├── quat_relational.h │ │ ├── dualquat_relational.h │ │ ├── relational.h │ │ ├── dualquat_exponential.h │ │ ├── dualquat_common.h │ │ ├── dualquat_helper.h │ │ ├── dualquat_transformation.h │ │ └── dualquat_base.h │ ├── KDTree_STD.h │ ├── laserMappingClass.h │ └── odomEstimationClass.h ├── src │ ├── KDTree_STD.cpp │ ├── laserMappingNode.cpp │ └── laserMappingClass.cpp ├── config │ ├── config_NTU.yaml │ ├── config_Blue.yaml │ ├── config_Heli.yaml │ ├── config_conSLAM.yaml │ ├── config_Kitti.yaml │ └── config_hesai_pandar.yaml ├── package.xml ├── launch │ ├── odomEstimation_HeLiPR_display_STD.launch │ ├── odomEstimation_HeLiPR_dataset.launch │ ├── odomEstimation_hensai.launch │ ├── odomEstimation_KITTI_dataset.launch │ ├── odomEstimation_NTU_dataset.launch │ ├── odomEstimation_KITTI_display_STD.launch │ ├── odomEstimation_conSLAM_dataset.launch │ ├── odomEstimation_ouster_BLUE_dataset.launch │ └── odomEstimation_ouster_BLUE_display_std.launch ├── CMakeLists.txt ├── Dockerfile ├── README.md └── rviz │ ├── kitti_fov.rviz │ ├── kitti_displayed_std.rviz │ └── heliPR_displayed_std.rviz ├── ackermann_to_odom ├── launch │ └── start_odometry.launch ├── cfg │ └── AckermannToOdom.cfg ├── package.xml ├── CMakeLists.txt ├── src │ ├── ackermann_to_odom_alg_node.cpp │ └── ackermann_to_odom_alg.cpp └── include │ ├── ackermann_to_odom_alg.h │ └── ackermann_to_odom_alg_node.h ├── gps_to_odom ├── src │ └── gps_to_odom_alg.cpp ├── cfg │ └── GpsToOdom.cfg ├── package.xml ├── CMakeLists.txt └── include │ ├── gps_to_odom_alg.h │ └── gps_to_odom_alg_node.h ├── odom_estimation_pc ├── src │ ├── lidar.cpp │ ├── lidarOptimization.cpp │ └── odomEstimationNode.cpp ├── include │ ├── lidar.h │ ├── lidarOptimization.h │ └── odomEstimationClass.h ├── package.xml ├── launch │ ├── odomEstimation_ouster.launch │ ├── odomEstimation_ouster_lite.launch │ ├── odomEstimation_carla.launch │ ├── odomEstimation_VLP16.launch │ └── odomEstimation_KITTI_dataset.launch ├── CMakeLists.txt └── rviz │ └── odomEstimation.rviz └── README.md /aurova_odom/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(aurova_odom) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /.project: -------------------------------------------------------------------------------- 1 | 2 | 3 | aurova_odom 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /dualquat_LOAM/include/dualquat/dualquat.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file dualquat/dualquat.h 3 | */ 4 | #pragma once 5 | 6 | #include "dualquat_base.h" 7 | #include "dualquat_relational.h" 8 | #include "dualquat_query.h" 9 | #include "dualquat_common.h" 10 | #include "dualquat_exponential.h" 11 | #include "dualquat_transformation.h" 12 | #include "dualquat_helper.h" 13 | -------------------------------------------------------------------------------- /ackermann_to_odom/launch/start_odometry.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /dualquat_LOAM/src/KDTree_STD.cpp: -------------------------------------------------------------------------------- 1 | #include "include/KDTree_STD.h" 2 | 3 | STDesc findClosestDescriptor(const STDesc& query, STDescKDTree& index, STDescCloud& cloud) { 4 | 5 | std::vector queryVec = query.toVector(); 6 | 7 | size_t ret_index; 8 | double out_dist_sqr; 9 | nanoflann::KNNResultSet resultSet(1); 10 | resultSet.init(&ret_index, &out_dist_sqr); 11 | 12 | index.findNeighbors(resultSet, queryVec.data(), nanoflann::SearchParameters(10)); 13 | 14 | return cloud.descriptors[ret_index]; 15 | } 16 | -------------------------------------------------------------------------------- /gps_to_odom/src/gps_to_odom_alg.cpp: -------------------------------------------------------------------------------- 1 | #include "gps_to_odom_alg.h" 2 | 3 | GpsToOdomAlgorithm::GpsToOdomAlgorithm(void) 4 | { 5 | pthread_mutex_init(&this->access_,NULL); 6 | } 7 | 8 | GpsToOdomAlgorithm::~GpsToOdomAlgorithm(void) 9 | { 10 | pthread_mutex_destroy(&this->access_); 11 | } 12 | 13 | void GpsToOdomAlgorithm::config_update(Config& config, uint32_t level) 14 | { 15 | this->lock(); 16 | 17 | // save the current configuration 18 | this->config_=config; 19 | 20 | this->unlock(); 21 | } 22 | 23 | // GpsToOdomAlgorithm Public API 24 | -------------------------------------------------------------------------------- /dualquat_LOAM/config/config_NTU.yaml: -------------------------------------------------------------------------------- 1 | # pre process 2 | ds_size: 0.2 3 | maximum_corner_num: 10 4 | 5 | # key points 6 | plane_detection_thre: 0.01 7 | plane_merge_normal_thre: 0.2 8 | voxel_size: 2.0 9 | voxel_init_num: 5 10 | proj_image_resolution: 0.5 11 | proj_dis_min: 0 12 | proj_dis_max: 5 13 | corner_thre: 5 14 | 15 | # std descriptor 16 | descriptor_near_num: 5 17 | descriptor_min_len: 1 18 | descriptor_max_len: 500 19 | non_max_suppression_radius: 3.0 20 | std_side_resolution: 0.1 21 | 22 | # candidate search 23 | skip_near_num: 2 24 | candidate_num: 2 25 | sub_frame_num: 1 26 | vertex_diff_threshold: 0.5 27 | rough_dis_threshold: 0.01 28 | normal_threshold: 0.2 29 | dis_threshold: 0.1 30 | icp_threshold: 0.4 31 | 32 | # window size 33 | max_window_size: 10.0 34 | # for Kitti max_window_size: 10.0 35 | # for BLUE max_window_size: 2.0 36 | kdtree_threshold: 10.0 37 | epsilon: 0.001 -------------------------------------------------------------------------------- /dualquat_LOAM/config/config_Blue.yaml: -------------------------------------------------------------------------------- 1 | # pre process 2 | ds_size: 0.5 3 | maximum_corner_num: 10 4 | 5 | # key points 6 | plane_detection_thre: 0.01 7 | plane_merge_normal_thre: 0.2 8 | voxel_size: 2.0 9 | voxel_init_num: 10 10 | proj_image_resolution: 0.5 11 | proj_dis_min: 0 12 | proj_dis_max: 5 13 | corner_thre: 10 14 | 15 | # std descriptor 16 | descriptor_near_num: 10 17 | descriptor_min_len: 1 18 | descriptor_max_len: 500 19 | non_max_suppression_radius: 3.0 20 | std_side_resolution: 0.5 21 | 22 | # candidate search 23 | skip_near_num: 5 24 | candidate_num: 5 25 | sub_frame_num: 1 26 | vertex_diff_threshold: 0.5 27 | rough_dis_threshold: 0.01 28 | normal_threshold: 0.2 29 | dis_threshold: 0.5 30 | icp_threshold: 0.4 31 | 32 | # window size 33 | max_window_size: 10.0 34 | # for Kitti max_window_size: 10.0 35 | # for BLUE max_window_size: 2.0 36 | kdtree_threshold: 10.0 37 | epsilon: 0.001 -------------------------------------------------------------------------------- /dualquat_LOAM/config/config_Heli.yaml: -------------------------------------------------------------------------------- 1 | # pre process 2 | ds_size: 0.2 3 | maximum_corner_num: 10 4 | 5 | # key points 6 | plane_detection_thre: 0.01 7 | plane_merge_normal_thre: 0.2 8 | voxel_size: 2.0 9 | voxel_init_num: 10 10 | proj_image_resolution: 0.5 11 | proj_dis_min: 0 12 | proj_dis_max: 5 13 | corner_thre: 10 14 | 15 | # std descriptor 16 | descriptor_near_num: 10 17 | descriptor_min_len: 1 18 | descriptor_max_len: 500 19 | non_max_suppression_radius: 3.0 20 | std_side_resolution: 0.5 21 | 22 | # candidate search 23 | skip_near_num: 5 24 | candidate_num: 5 25 | sub_frame_num: 1 26 | vertex_diff_threshold: 0.5 27 | rough_dis_threshold: 0.01 28 | normal_threshold: 0.2 29 | dis_threshold: 0.5 30 | icp_threshold: 0.4 31 | 32 | # window size 33 | max_window_size: 10.0 34 | # for Kitti max_window_size: 10.0 35 | # for BLUE max_window_size: 2.0 36 | kdtree_threshold: 10.0 37 | epsilon: 0.2 -------------------------------------------------------------------------------- /dualquat_LOAM/config/config_conSLAM.yaml: -------------------------------------------------------------------------------- 1 | # pre process 2 | ds_size: 0.1 3 | maximum_corner_num: 10 4 | 5 | # key points 6 | plane_detection_thre: 0.01 7 | plane_merge_normal_thre: 0.2 8 | voxel_size: 2.0 9 | voxel_init_num: 5 10 | proj_image_resolution: 0.5 11 | proj_dis_min: 0 12 | proj_dis_max: 5 13 | corner_thre: 5 14 | 15 | # std descriptor 16 | descriptor_near_num: 5 17 | descriptor_min_len: 1 18 | descriptor_max_len: 500 19 | non_max_suppression_radius: 3.0 20 | std_side_resolution: 0.1 21 | 22 | # candidate search 23 | skip_near_num: 2 24 | candidate_num: 2 25 | sub_frame_num: 1 26 | vertex_diff_threshold: 0.5 27 | rough_dis_threshold: 0.01 28 | normal_threshold: 0.2 29 | dis_threshold: 0.1 30 | icp_threshold: 0.4 31 | 32 | # window size 33 | max_window_size: 10.0 34 | # for Kitti max_window_size: 10.0 35 | # for BLUE max_window_size: 2.0 36 | kdtree_threshold: 10.0 37 | epsilon: 0.001 -------------------------------------------------------------------------------- /dualquat_LOAM/config/config_Kitti.yaml: -------------------------------------------------------------------------------- 1 | # pre process 2 | ds_size: 0.2 3 | maximum_corner_num: 10 4 | 5 | # key points 6 | plane_detection_thre: 0.01 7 | plane_merge_normal_thre: 0.2 8 | voxel_size: 2.0 9 | voxel_init_num: 10 10 | proj_image_resolution: 0.5 11 | proj_dis_min: 0 12 | proj_dis_max: 5 13 | corner_thre: 10 14 | 15 | # std descriptor 16 | descriptor_near_num: 10 17 | descriptor_min_len: 1 18 | descriptor_max_len: 500 19 | non_max_suppression_radius: 3.0 20 | std_side_resolution: 0.5 21 | 22 | # candidate search 23 | skip_near_num: 5 24 | candidate_num: 5 25 | sub_frame_num: 1 26 | vertex_diff_threshold: 0.5 27 | rough_dis_threshold: 0.01 28 | normal_threshold: 0.2 29 | dis_threshold: 0.5 30 | icp_threshold: 0.4 31 | 32 | # window size 33 | max_window_size: 10.0 34 | # for Kitti max_window_size: 10.0 35 | # for BLUE max_window_size: 2.0 36 | kdtree_threshold: 10.0 37 | epsilon: 0.001 38 | -------------------------------------------------------------------------------- /dualquat_LOAM/config/config_hesai_pandar.yaml: -------------------------------------------------------------------------------- 1 | # pre process 2 | ds_size: 0.1 3 | maximum_corner_num: 20 4 | 5 | # key points 6 | plane_detection_thre: 0.01 7 | plane_merge_normal_thre: 0.2 8 | voxel_size: 2.0 9 | voxel_init_num: 10 10 | proj_image_resolution: 0.5 11 | proj_dis_min: 0 12 | proj_dis_max: 5 13 | corner_thre: 10 14 | 15 | # std descriptor 16 | descriptor_near_num: 5 17 | descriptor_min_len: 1 18 | descriptor_max_len: 5000 19 | non_max_suppression_radius: 1.0 20 | std_side_resolution: 0.2 21 | 22 | # candidate search 23 | skip_near_num: 5 24 | candidate_num: 5 25 | sub_frame_num: 1 26 | vertex_diff_threshold: 0.5 27 | rough_dis_threshold: 0.01 28 | normal_threshold: 0.2 29 | dis_threshold: 0.5 30 | icp_threshold: 0.4 31 | 32 | # window size 33 | max_window_size: 10.0 34 | # for Kitti max_window_size: 10.0 35 | # for BLUE max_window_size: 2.0 36 | kdtree_threshold: 10.0 37 | epsilon: 0.001 -------------------------------------------------------------------------------- /dualquat_LOAM/include/KDTree_STD.h: -------------------------------------------------------------------------------- 1 | #ifndef KDTREEDESC_H 2 | #define KDTREEDESC_H 3 | 4 | #include 5 | #include 6 | #include "STDesc.h" 7 | 8 | struct STDescCloud { 9 | std::vector descriptors; 10 | 11 | inline size_t kdtree_get_point_count() const { return descriptors.size(); } 12 | 13 | inline double kdtree_get_pt(const size_t idx, const size_t dim) const { 14 | return descriptors[idx].toVector()[dim]; 15 | } 16 | 17 | template 18 | bool kdtree_get_bbox(BBOX&) const { return false; } 19 | 20 | }; 21 | 22 | typedef nanoflann::KDTreeSingleIndexAdaptor< 23 | nanoflann::L2_Simple_Adaptor, 24 | STDescCloud, 25 | 18 /* Dimensiones (3 side_length + 3 angle + 3 center + 3*3 vertices) */ 26 | > STDescKDTree; 27 | 28 | STDesc findClosestDescriptor(const STDesc& query, STDescKDTree& index, STDescCloud& cloud); 29 | 30 | #endif // KDTREEDESC_H 31 | -------------------------------------------------------------------------------- /odom_estimation_pc/src/lidar.cpp: -------------------------------------------------------------------------------- 1 | // Author of FLOAM: Wang Han 2 | // Email wh200720041@gmail.com 3 | // Homepage https://wanghan.pro 4 | 5 | // Author of Lilo: Edison Velasco 6 | // Email evs25@alu.ua.es 7 | 8 | #include "lidar.h" 9 | 10 | 11 | lidar::Lidar::Lidar(){ 12 | 13 | } 14 | 15 | 16 | void lidar::Lidar::setLines(double num_lines_in){ 17 | num_lines=num_lines_in; 18 | } 19 | 20 | void lidar::Lidar::setValidationAngle(bool validation_height_in){ 21 | validation_height = validation_height_in; 22 | } 23 | 24 | void lidar::Lidar::setVerticalAngle(double velodyne_height_in){ 25 | velodyne_height = velodyne_height_in; 26 | } 27 | 28 | 29 | void lidar::Lidar::setVerticalResolution(double vertical_angle_resolution_in){ 30 | vertical_angle_resolution = vertical_angle_resolution_in; 31 | } 32 | 33 | 34 | void lidar::Lidar::setScanPeriod(double scan_period_in){ 35 | scan_period = scan_period_in; 36 | } 37 | 38 | 39 | void lidar::Lidar::setMaxDistance(double max_distance_in){ 40 | max_distance = max_distance_in; 41 | } 42 | 43 | void lidar::Lidar::setMinDistance(double min_distance_in){ 44 | min_distance = min_distance_in; 45 | } 46 | -------------------------------------------------------------------------------- /odom_estimation_pc/include/lidar.h: -------------------------------------------------------------------------------- 1 | // Author of FLOAM: Wang Han 2 | // Email wh200720041@gmail.com 3 | // Homepage https://wanghan.pro 4 | 5 | // Author of Lilo: Edison Velasco 6 | // Email evs25@alu.ua.es 7 | 8 | #ifndef _LIDAR_H_ 9 | #define _LIDAR_H_ 10 | 11 | //define lidar parameter 12 | 13 | namespace lidar{ 14 | 15 | class Lidar 16 | { 17 | public: 18 | Lidar(); 19 | 20 | void setScanPeriod(double scan_period_in); 21 | void setLines(double num_lines_in); 22 | void setVerticalAngle(double velodyne_height_in); 23 | void setValidationAngle(bool validation_height_in); 24 | void setVerticalResolution(double vertical_angle_resolution_in); 25 | void setMaxDistance(double max_distance_in); 26 | void setMinDistance(double min_distance_in); 27 | 28 | double max_distance; 29 | double min_distance; 30 | int num_lines; 31 | double scan_period; 32 | int points_per_line; 33 | double horizontal_angle_resolution; 34 | double horizontal_angle; 35 | double vertical_angle_resolution; 36 | double velodyne_height; 37 | bool validation_height; 38 | }; 39 | 40 | 41 | } 42 | 43 | 44 | #endif // _LIDAR_H_ 45 | 46 | -------------------------------------------------------------------------------- /dualquat_LOAM/include/dualquat/quat_exponential.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file dualquat/quat_exponential.h 3 | * @brief This file provides exponential functions for quaternion types. 4 | */ 5 | #pragma once 6 | 7 | #include 8 | 9 | namespace dualquat 10 | { 11 | 12 | /** 13 | * Returns a exponential of a quaternion. 14 | */ 15 | template 16 | Quaternion 17 | exp(const Quaternion& q) 18 | { 19 | const auto vn = q.vec().norm(); 20 | const auto sinc = (vn > T(0)) ? std::sin(vn) / vn : T(1); 21 | 22 | Quaternion temp; 23 | temp.w() = std::cos(vn); 24 | temp.vec() = sinc * q.vec(); 25 | temp.coeffs() *= std::exp(q.w()); 26 | return temp; 27 | } 28 | 29 | /** 30 | * Returns a logarithm of a quaternion. 31 | */ 32 | template 33 | Quaternion 34 | log(const Quaternion& q) 35 | { 36 | const auto vn = q.vec().norm(); 37 | const auto phi = std::atan2(vn, q.w()); 38 | const auto qn = q.norm(); 39 | const auto phi_over_vn = (vn > T(0)) ? phi / vn : T(1) / qn; 40 | 41 | Quaternion temp; 42 | temp.w() = std::log(qn); 43 | temp.vec() = phi_over_vn * q.vec(); 44 | return temp; 45 | } 46 | 47 | } // namespace dualquat 48 | -------------------------------------------------------------------------------- /dualquat_LOAM/include/dualquat/dualquat_query.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file dualquat/dualquat_query.h 3 | * @brief This file provides query functions for dual quaternion types. 4 | */ 5 | #pragma once 6 | 7 | #include "dualquat_relational.h" 8 | 9 | namespace dualquat 10 | { 11 | 12 | template 13 | bool 14 | is_zero(const DualQuaternion& dq, T tol) 15 | { 16 | return almost_zero(dq, tol); 17 | } 18 | 19 | template 20 | bool 21 | is_real(const DualQuaternion& dq, T tol) 22 | { 23 | return almost_zero(dq.real().vec(), tol) 24 | && almost_zero(dq.dual().vec(), tol); 25 | } 26 | 27 | template 28 | bool 29 | is_pure(const DualQuaternion& dq, T tol) 30 | { 31 | using Matrix1 = Eigen::Matrix; 32 | 33 | return almost_zero(Matrix1(dq.real().w()), tol) 34 | && almost_zero(Matrix1(dq.dual().w()), tol); 35 | } 36 | 37 | template 38 | bool 39 | is_unit(const DualQuaternion& dq, T tol) 40 | { 41 | using Matrix1 = Eigen::Matrix; 42 | 43 | return almost_equal(Matrix1(dq.real().squaredNorm()), Matrix1(T(1)), tol) 44 | && almost_equal(Matrix1(dq.real().dot(dq.dual())), Matrix1(T(0)), tol); 45 | } 46 | 47 | } // namespace dualquat 48 | -------------------------------------------------------------------------------- /odom_estimation_pc/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | odom_estimation_pc 4 | 0.0.0 5 | 6 | 7 | This work is created based to Floam (Fast and Optimized Lidar Odometry And Mapping) 8 | 9 | 10 | EPVS 11 | 12 | BSD 13 | EPVelasco 14 | 15 | catkin 16 | geometry_msgs 17 | nav_msgs 18 | roscpp 19 | rospy 20 | std_msgs 21 | rosbag 22 | sensor_msgs 23 | tf 24 | eigen_conversions 25 | geometry_msgs 26 | nav_msgs 27 | sensor_msgs 28 | roscpp 29 | rospy 30 | std_msgs 31 | rosbag 32 | tf 33 | eigen_conversions 34 | 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /dualquat_LOAM/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | dualquat_loam 4 | 0.0.0 5 | 6 | 7 | This work is created based to Floam (Fast and Optimized Lidar Odometry And Mapping) and STD descriptors 8 | 9 | 10 | EPVS 11 | 12 | BSD 13 | EPVelasco 14 | 15 | catkin 16 | geometry_msgs 17 | nav_msgs 18 | roscpp 19 | rospy 20 | std_msgs 21 | rosbag 22 | sensor_msgs 23 | tf 24 | eigen_conversions 25 | geometry_msgs 26 | nav_msgs 27 | sensor_msgs 28 | roscpp 29 | rospy 30 | std_msgs 31 | rosbag 32 | tf 33 | eigen_conversions 34 | ceres-solver 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /odom_estimation_pc/launch/odomEstimation_ouster.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /odom_estimation_pc/launch/odomEstimation_ouster_lite.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /odom_estimation_pc/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(odom_estimation_pc) 3 | 4 | set(CMAKE_BUILD_TYPE "Release") 5 | set(CMAKE_CXX_FLAGS "-std=c++14") 6 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g") 7 | 8 | find_package(catkin REQUIRED COMPONENTS 9 | eigen_conversions 10 | geometry_msgs 11 | nav_msgs 12 | rosbag 13 | roscpp 14 | rospy 15 | sensor_msgs 16 | std_msgs 17 | tf 18 | ) 19 | 20 | 21 | find_package(Eigen3) 22 | if(NOT EIGEN3_FOUND) 23 | find_package(cmake_modules REQUIRED) 24 | find_package(Eigen REQUIRED) 25 | set(EIGEN3_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS}) 26 | set(EIGEN3_LIBRARIES ${EIGEN_LIBRARIES}) 27 | else() 28 | set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR}) 29 | endif() 30 | 31 | find_package(PCL REQUIRED) 32 | find_package(Ceres REQUIRED) 33 | 34 | include_directories( 35 | include 36 | ${catkin_INCLUDE_DIRS} 37 | ${PCL_INCLUDE_DIRS} 38 | ${CERES_INCLUDE_DIRS} 39 | ${EIGEN3_INCLUDE_DIRS} 40 | ) 41 | 42 | link_directories( 43 | include 44 | ${PCL_LIBRARY_DIRS} 45 | ${CERES_LIBRARY_DIRS} 46 | ) 47 | 48 | catkin_package( 49 | CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs 50 | DEPENDS EIGEN3 PCL Ceres 51 | INCLUDE_DIRS include 52 | ) 53 | 54 | add_executable(odom_estimation_node src/odomEstimationNode.cpp src/lidarOptimization.cpp src/lidar.cpp src/odomEstimationClass.cpp) 55 | target_link_libraries(odom_estimation_node ${EIGEN3_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES}) 56 | -------------------------------------------------------------------------------- /odom_estimation_pc/launch/odomEstimation_carla.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /dualquat_LOAM/include/dualquat/quat_relational.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file dualquat/quat_relational.h 3 | * @brief This file provides relational functions for quaternion types. 4 | */ 5 | #pragma once 6 | 7 | #include 8 | #include "relational.h" 9 | 10 | namespace dualquat 11 | { 12 | 13 | template 14 | bool almost_equal(const Quaternion& lhs, const Quaternion& rhs, T rtol, T atol) 15 | { 16 | return almost_equal(lhs.coeffs(), rhs.coeffs(), rtol, atol); 17 | } 18 | 19 | template 20 | bool almost_equal(const Quaternion& lhs, const Quaternion& rhs, T tol) 21 | { 22 | return almost_equal(lhs.coeffs(), rhs.coeffs(), tol); 23 | } 24 | 25 | template 26 | bool almost_zero(const Quaternion& q, T tol) 27 | { 28 | return almost_zero(q.coeffs(), tol); 29 | } 30 | 31 | /** 32 | * Returns true if the two quaternions represent the same rotation. 33 | * Note that every rotation is represented by two values, q and -q. 34 | */ 35 | template 36 | bool same_rotation(const Quaternion& lhs, const Quaternion& rhs, T tol) 37 | { 38 | #ifndef NDEBUG 39 | using Matrix1 = Eigen::Matrix; 40 | #endif 41 | assert(almost_equal(Matrix1(lhs.squaredNorm()), Matrix1(T(1)), tol)); 42 | assert(almost_equal(Matrix1(rhs.squaredNorm()), Matrix1(T(1)), tol)); 43 | 44 | using Coefficients = typename Quaternion::Coefficients; 45 | 46 | return almost_equal(lhs.coeffs(), rhs.coeffs(), tol) 47 | || almost_equal(lhs.coeffs(), Coefficients(-rhs.coeffs()), tol); 48 | } 49 | 50 | } // namespace dualquat 51 | -------------------------------------------------------------------------------- /dualquat_LOAM/include/dualquat/dualquat_relational.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file dualquat/dualquat_relational.h 3 | * @brief This file provides relational functions for dual quaternion types. 4 | */ 5 | #pragma once 6 | 7 | #include 8 | #include "quat_relational.h" 9 | #include "dualquat_query.h" 10 | 11 | namespace dualquat 12 | { 13 | 14 | template 15 | bool almost_equal(const DualQuaternion& lhs, const DualQuaternion& rhs, T rtol, T atol) 16 | { 17 | return almost_equal(lhs.real().coeffs(), rhs.real().coeffs(), rtol, atol) 18 | && almost_equal(lhs.dual().coeffs(), rhs.dual().coeffs(), rtol, atol); 19 | } 20 | 21 | template 22 | bool almost_equal(const DualQuaternion& lhs, const DualQuaternion& rhs, T tol) 23 | { 24 | return almost_equal(lhs.real().coeffs(), rhs.real().coeffs(), tol) 25 | && almost_equal(lhs.dual().coeffs(), rhs.dual().coeffs(), tol); 26 | } 27 | 28 | template 29 | bool almost_zero(const DualQuaternion& dq, T tol) 30 | { 31 | return almost_zero(dq.real().coeffs(), tol) 32 | && almost_zero(dq.dual().coeffs(), tol); 33 | } 34 | 35 | /** 36 | * Returns true if the two dual quaternions represent the same transformation. 37 | * Note that every transformation is represented by two values, dq and -dq. 38 | */ 39 | template 40 | bool same_transformation(const DualQuaternion& lhs, const DualQuaternion& rhs, T tol) 41 | { 42 | assert(is_unit(lhs, tol)); 43 | assert(is_unit(rhs, tol)); 44 | 45 | return almost_equal(lhs, rhs, tol) 46 | || almost_equal(lhs, -rhs, tol); 47 | } 48 | 49 | } // namespace dualquat 50 | -------------------------------------------------------------------------------- /dualquat_LOAM/include/dualquat/relational.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file dualquat/relational.h 3 | * @brief This file provides relational functions for DenseBase types. 4 | */ 5 | #pragma once 6 | 7 | #include 8 | 9 | namespace dualquat 10 | { 11 | 12 | /** 13 | * Returns true if two matrices are element-wise equal within tolerance. 14 | */ 15 | template 16 | bool almost_equal( 17 | const Eigen::MatrixBase& lhs, 18 | const Eigen::MatrixBase& rhs, 19 | const typename DerivedA::RealScalar& rel_tolerance, 20 | const typename DerivedA::RealScalar& abs_tolerance) 21 | { 22 | return ((lhs - rhs).array().abs() 23 | <= Eigen::MatrixBase::Constant(abs_tolerance).array().max(rel_tolerance * lhs.array().abs().max(rhs.array().abs()))).all(); 24 | } 25 | 26 | /** 27 | * Returns true if two matrices are element-wise equal within tolerance. 28 | */ 29 | template 30 | bool almost_equal( 31 | const Eigen::MatrixBase& lhs, 32 | const Eigen::MatrixBase& rhs, 33 | const typename DerivedA::RealScalar& tolerance) 34 | { 35 | return ((lhs - rhs).array().abs() 36 | <= tolerance * Eigen::MatrixBase::Ones().array().max(lhs.array().abs().max(rhs.array().abs()))).all(); 37 | } 38 | 39 | /** 40 | * Returns true if a matrix is element-wise equal to zero within tolerance. 41 | */ 42 | template 43 | bool almost_zero( 44 | const Eigen::MatrixBase& x, 45 | const typename Derived::RealScalar& tolerance) 46 | { 47 | return (x.array().abs() <= tolerance).all(); 48 | } 49 | 50 | } // namespace dualquat 51 | -------------------------------------------------------------------------------- /odom_estimation_pc/launch/odomEstimation_VLP16.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /odom_estimation_pc/launch/odomEstimation_KITTI_dataset.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /dualquat_LOAM/include/laserMappingClass.h: -------------------------------------------------------------------------------- 1 | // Author of FLOAM: Wang Han 2 | // Email wh200720041@gmail.com 3 | // Homepage https://wanghan.pro 4 | 5 | #ifndef _LASER_MAPPING_H_ 6 | #define _LASER_MAPPING_H_ 7 | 8 | //PCL lib 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | //eigen lib 17 | #include 18 | #include 19 | 20 | //c++ lib 21 | #include 22 | #include 23 | #include 24 | 25 | 26 | #define LASER_CELL_WIDTH 200.0 27 | #define LASER_CELL_HEIGHT 200.0 28 | #define LASER_CELL_DEPTH 200.0 29 | 30 | //separate map as many sub point clouds 31 | 32 | #define LASER_CELL_RANGE_HORIZONTAL 20 33 | #define LASER_CELL_RANGE_VERTICAL 20 34 | 35 | 36 | class LaserMappingClass 37 | { 38 | 39 | public: 40 | LaserMappingClass(); 41 | void init(double map_resolution); 42 | void updateCurrentPointsToMap(const pcl::PointCloud::Ptr& pc_in, const Eigen::Isometry3d& pose_current); 43 | pcl::PointCloud::Ptr getMap(void); 44 | 45 | private: 46 | int origin_in_map_x; 47 | int origin_in_map_y; 48 | int origin_in_map_z; 49 | int map_width; 50 | int map_height; 51 | int map_depth; 52 | std::vector::Ptr>>> map; 53 | pcl::VoxelGrid downSizeFilter; 54 | 55 | void addWidthCellNegative(void); 56 | void addWidthCellPositive(void); 57 | void addHeightCellNegative(void); 58 | void addHeightCellPositive(void); 59 | void addDepthCellNegative(void); 60 | void addDepthCellPositive(void); 61 | void checkPoints(int& x, int& y, int& z); 62 | 63 | }; 64 | 65 | 66 | #endif // _LASER_MAPPING_H_ 67 | 68 | -------------------------------------------------------------------------------- /odom_estimation_pc/include/lidarOptimization.h: -------------------------------------------------------------------------------- 1 | // Author of FLOAM: Wang Han 2 | // Email wh200720041@gmail.com 3 | // Homepage https://wanghan.pro 4 | 5 | // Author of Lilo: Edison Velasco 6 | // Email evs25@alu.ua.es 7 | 8 | #ifndef _LIDAR_OPTIMIZATION_ANALYTIC_H_ 9 | #define _LIDAR_OPTIMIZATION_ANALYTIC_H_ 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | void getTransformFromSe3(const Eigen::Matrix& se3, Eigen::Quaterniond& q, Eigen::Vector3d& t); 17 | 18 | Eigen::Matrix3d skew(Eigen::Vector3d& mat_in); 19 | 20 | class EdgeAnalyticCostFunction : public ceres::SizedCostFunction<1, 7> { 21 | public: 22 | 23 | EdgeAnalyticCostFunction(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_a_, Eigen::Vector3d last_point_b_); 24 | virtual ~EdgeAnalyticCostFunction() {} 25 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 26 | 27 | Eigen::Vector3d curr_point; 28 | Eigen::Vector3d last_point_a; 29 | Eigen::Vector3d last_point_b; 30 | }; 31 | 32 | class SurfNormAnalyticCostFunction : public ceres::SizedCostFunction<1, 7> { 33 | public: 34 | SurfNormAnalyticCostFunction(Eigen::Vector3d curr_point_, Eigen::Vector3d plane_unit_norm_, double negative_OA_dot_norm_); 35 | virtual ~SurfNormAnalyticCostFunction() {} 36 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 37 | 38 | Eigen::Vector3d curr_point; 39 | Eigen::Vector3d plane_unit_norm; 40 | double negative_OA_dot_norm; 41 | }; 42 | 43 | class PoseSE3Parameterization : public ceres::LocalParameterization { 44 | public: 45 | 46 | PoseSE3Parameterization() {} 47 | virtual ~PoseSE3Parameterization() {} 48 | virtual bool Plus(const double* x, const double* delta, double* x_plus_delta) const; 49 | virtual bool ComputeJacobian(const double* x, double* jacobian) const; 50 | virtual int GlobalSize() const { return 7; } 51 | virtual int LocalSize() const { return 6; } 52 | }; 53 | 54 | 55 | 56 | #endif // _LIDAR_OPTIMIZATION_ANALYTIC_H_ 57 | 58 | -------------------------------------------------------------------------------- /gps_to_odom/cfg/GpsToOdom.cfg: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | #* All rights reserved. 3 | #* 4 | #* Redistribution and use in source and binary forms, with or without 5 | #* modification, are permitted provided that the following conditions 6 | #* are met: 7 | #* 8 | #* * Redistributions of source code must retain the above copyright 9 | #* notice, this list of conditions and the following disclaimer. 10 | #* * Redistributions in binary form must reproduce the above 11 | #* copyright notice, this list of conditions and the following 12 | #* disclaimer in the documentation and/or other materials provided 13 | #* with the distribution. 14 | #* * Neither the name of the Willow Garage nor the names of its 15 | #* contributors may be used to endorse or promote products derived 16 | #* from this software without specific prior written permission. 17 | #* 18 | #* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 19 | #* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 20 | #* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 21 | #* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 22 | #* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 23 | #* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 24 | #* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | #* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | #* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 27 | #* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 28 | #* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | #* POSSIBILITY OF SUCH DAMAGE. 30 | #*********************************************************** 31 | 32 | # Author: 33 | 34 | PACKAGE='gps_to_odom' 35 | 36 | from dynamic_reconfigure.parameter_generator_catkin import * 37 | 38 | gen = ParameterGenerator() 39 | 40 | # Name Type Reconfiguration level Description Default Min Max 41 | #gen.add("velocity_scale_factor", double_t, 0, "Maximum velocity scale factor", 0.5, 0.0, 1.0) 42 | 43 | exit(gen.generate(PACKAGE, "GpsToOdomAlgorithm", "GpsToOdom")) 44 | -------------------------------------------------------------------------------- /ackermann_to_odom/cfg/AckermannToOdom.cfg: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | #* All rights reserved. 3 | #* 4 | #* Redistribution and use in source and binary forms, with or without 5 | #* modification, are permitted provided that the following conditions 6 | #* are met: 7 | #* 8 | #* * Redistributions of source code must retain the above copyright 9 | #* notice, this list of conditions and the following disclaimer. 10 | #* * Redistributions in binary form must reproduce the above 11 | #* copyright notice, this list of conditions and the following 12 | #* disclaimer in the documentation and/or other materials provided 13 | #* with the distribution. 14 | #* * Neither the name of the Willow Garage nor the names of its 15 | #* contributors may be used to endorse or promote products derived 16 | #* from this software without specific prior written permission. 17 | #* 18 | #* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 19 | #* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 20 | #* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 21 | #* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 22 | #* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 23 | #* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 24 | #* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | #* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | #* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 27 | #* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 28 | #* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | #* POSSIBILITY OF SUCH DAMAGE. 30 | #*********************************************************** 31 | 32 | # Author: 33 | 34 | PACKAGE='ackermann_to_odom' 35 | 36 | from dynamic_reconfigure.parameter_generator_catkin import * 37 | 38 | gen = ParameterGenerator() 39 | 40 | # Name Type Reconfiguration level Description Default Min Max 41 | #gen.add("velocity_scale_factor", double_t, 0, "Maximum velocity scale factor", 0.5, 0.0, 1.0) 42 | 43 | exit(gen.generate(PACKAGE, "AckermannToOdomAlgorithm", "AckermannToOdom")) 44 | -------------------------------------------------------------------------------- /dualquat_LOAM/include/dualquat/dualquat_exponential.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file dualquat/dualquat_exponential.h 3 | * @brief This file provides exponential functions for dual quaternion types. 4 | */ 5 | #pragma once 6 | 7 | #include 8 | #include "quat_exponential.h" 9 | 10 | namespace dualquat 11 | { 12 | 13 | /** 14 | * Returns a exponential of a dual quaternion. 15 | */ 16 | template 17 | DualQuaternion 18 | exp(const DualQuaternion& dq) 19 | { 20 | DualQuaternion temp; 21 | temp.real() = dualquat::exp(dq.real()); 22 | 23 | T sinc, alpha; 24 | const auto vn = dq.real().vec().norm(); 25 | if(vn > T(0)) 26 | { 27 | sinc = std::sin(vn) / vn; 28 | alpha = (std::cos(vn) - sinc) / (vn * vn); 29 | } 30 | else 31 | { 32 | sinc = T(1); 33 | alpha = - T(1) / T(3); 34 | } 35 | 36 | const auto vdot = dq.real().vec().dot(dq.dual().vec()); 37 | 38 | temp.dual().w() = - vdot * sinc; 39 | temp.dual().vec() = sinc * dq.dual().vec() + vdot * alpha * dq.real().vec(); 40 | temp.dual().coeffs() = std::exp(dq.real().w()) * temp.dual().coeffs() + dq.dual().w() * temp.real().coeffs(); 41 | 42 | return temp; 43 | } 44 | 45 | /** 46 | * Returns a logarithm of a dual quaternion. 47 | */ 48 | template 49 | DualQuaternion 50 | log(const DualQuaternion& dq) 51 | { 52 | DualQuaternion temp; 53 | temp.real() = dualquat::log(dq.real()); 54 | 55 | T phi_over_vn, alpha; 56 | const auto vn = dq.real().vec().norm(); 57 | const auto phi = std::atan2(vn, dq.real().w()); 58 | const auto qn = dq.real().norm(); 59 | if(vn > T(0)) 60 | { 61 | phi_over_vn = phi / vn; 62 | alpha = (dq.real().w() - phi_over_vn * qn * qn) / (vn * vn); 63 | } 64 | else 65 | { 66 | phi_over_vn = T(1) / qn; 67 | alpha = (- T(2) / T(3)) / qn; 68 | } 69 | 70 | temp.dual().w() = dq.real().dot(dq.dual()) / (qn * qn); 71 | temp.dual().vec() = phi_over_vn * dq.dual().vec() 72 | + ((dq.real().vec().dot(dq.dual().vec()) * alpha - dq.dual().w()) / (qn * qn)) * dq.real().vec(); 73 | 74 | return temp; 75 | } 76 | 77 | /** 78 | * Returns a dual quaternion raised to a power. 79 | */ 80 | template 81 | DualQuaternion 82 | pow(const DualQuaternion& dq, T t) 83 | { 84 | return exp(t * log(dq)); 85 | } 86 | 87 | } // namespace dualquat 88 | -------------------------------------------------------------------------------- /dualquat_LOAM/launch/odomEstimation_HeLiPR_display_STD.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | -------------------------------------------------------------------------------- /dualquat_LOAM/launch/odomEstimation_HeLiPR_dataset.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 24 | 25 | 26 | 27 | 28 | 31 | 32 | 33 | 34 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | -------------------------------------------------------------------------------- /dualquat_LOAM/launch/odomEstimation_hensai.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 24 | 25 | 26 | 27 | 28 | 31 | 32 | 33 | 34 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | -------------------------------------------------------------------------------- /dualquat_LOAM/launch/odomEstimation_KITTI_dataset.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 24 | 25 | 26 | 27 | 28 | 31 | 32 | 33 | 34 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | -------------------------------------------------------------------------------- /dualquat_LOAM/launch/odomEstimation_NTU_dataset.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 24 | 25 | 26 | 27 | 28 | 31 | 32 | 33 | 34 | 35 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /aurova_odom/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | aurova_odom 4 | 1.0.0 5 | The aurova_odom package 6 | 7 | 8 | 9 | 10 | mice85 11 | 12 | 13 | 14 | 15 | 16 | LGPL 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 | 52 | ackermann_to_odom 53 | gps_to_odom 54 | odom_estimation_pc 55 | 56 | 57 | catkin 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /dualquat_LOAM/launch/odomEstimation_KITTI_display_STD.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 24 | 25 | 26 | 27 | 28 | 31 | 32 | 33 | 34 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | -------------------------------------------------------------------------------- /dualquat_LOAM/launch/odomEstimation_conSLAM_dataset.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 24 | 25 | 26 | 27 | 28 | 31 | 32 | 33 | 34 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /dualquat_LOAM/include/dualquat/dualquat_common.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file dualquat/dualquat_common.h 3 | * @brief This file provides common functions for dual quaternion types. 4 | */ 5 | #pragma once 6 | 7 | #include 8 | 9 | namespace dualquat 10 | { 11 | 12 | /* Dual-valued functions */ 13 | 14 | template 15 | std::pair 16 | norm(const DualQuaternion& dq) 17 | { 18 | const auto n = dq.real().norm(); 19 | return std::make_pair(n, dq.real().dot(dq.dual()) / n); 20 | } 21 | 22 | template 23 | std::pair 24 | squared_norm(const DualQuaternion& dq) 25 | { 26 | return std::make_pair(dq.real().squaredNorm(), T(2) * dq.real().dot(dq.dual())); 27 | } 28 | 29 | /* DualQuaternion-valued functions */ 30 | 31 | template 32 | DualQuaternion 33 | identity() 34 | { 35 | return DualQuaternion( 36 | Quaternion::Identity(), 37 | Quaternion(Quaternion::Coefficients::Zero())); 38 | } 39 | 40 | template 41 | DualQuaternion 42 | inverse(const DualQuaternion& dq) 43 | { 44 | const auto inv_real = dq.real().inverse(); 45 | 46 | DualQuaternion temp; 47 | temp.real() = inv_real; 48 | temp.dual() = -(inv_real * dq.dual() * inv_real).coeffs(); 49 | return temp; 50 | } 51 | 52 | template 53 | DualQuaternion 54 | normalize(const DualQuaternion& dq) 55 | { 56 | const auto r = dq.real().normalized(); 57 | const auto d = dq.dual().coeffs() / dq.real().norm(); 58 | 59 | DualQuaternion temp; 60 | temp.real() = r; 61 | temp.dual() = d - r.coeffs().dot(d) * r.coeffs(); 62 | return temp; 63 | } 64 | 65 | /** 66 | * Dual-conjugate. 67 | */ 68 | template 69 | DualQuaternion 70 | dual_conjugate(const DualQuaternion& dq) 71 | { 72 | DualQuaternion temp(dq); 73 | temp.dual().coeffs() = -temp.dual().coeffs(); 74 | return temp; 75 | } 76 | 77 | /** 78 | * Quaternion-conjugate. 79 | */ 80 | template 81 | DualQuaternion 82 | quaternion_conjugate(const DualQuaternion& dq) 83 | { 84 | return DualQuaternion(dq.real().conjugate(), dq.dual().conjugate()); 85 | } 86 | 87 | /** 88 | * Total-conjugate. 89 | */ 90 | template 91 | DualQuaternion 92 | total_conjugate(const DualQuaternion& dq) 93 | { 94 | DualQuaternion temp(dq.real().conjugate(), dq.dual().conjugate()); 95 | temp.dual().coeffs() = -temp.dual().coeffs(); 96 | return temp; 97 | } 98 | 99 | /** 100 | * Returns the difference between the two dual quaternions. 101 | */ 102 | template 103 | DualQuaternion 104 | difference(const DualQuaternion& dq1, const DualQuaternion& dq2) 105 | { 106 | return inverse(dq1) * dq2; 107 | } 108 | 109 | } // namespace dualquat 110 | -------------------------------------------------------------------------------- /dualquat_LOAM/launch/odomEstimation_ouster_BLUE_dataset.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | -------------------------------------------------------------------------------- /dualquat_LOAM/launch/odomEstimation_ouster_BLUE_display_std.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | -------------------------------------------------------------------------------- /gps_to_odom/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | gps_to_odom 4 | 0.0.0 5 | The gps_to_odom package 6 | 7 | 8 | 9 | 10 | mice85 11 | 12 | 13 | 14 | 15 | 16 | LGPL 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 | 53 | iri_base_algorithm 54 | tf 55 | 56 | iri_base_algorithm 57 | tf 58 | 59 | iri_base_algorithm 60 | tf 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | -------------------------------------------------------------------------------- /ackermann_to_odom/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ackermann_to_odom 4 | 1.0.0 5 | The ackermann_to_odom package 6 | 7 | 8 | 9 | 10 | mice85 11 | 12 | 13 | 14 | 15 | 16 | LGPL 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 | 53 | iri_base_algorithm 54 | tf 55 | 56 | iri_base_algorithm 57 | tf 58 | 59 | iri_base_algorithm 60 | tf 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /dualquat_LOAM/include/dualquat/dualquat_helper.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file dualquat/dualquat_helper.h 3 | * @brief This file provides helper functions for dual quaternion types. 4 | */ 5 | #pragma once 6 | 7 | #include 8 | #include 9 | #include "dualquat_common.h" 10 | #include "dualquat_exponential.h" 11 | 12 | namespace dualquat 13 | { 14 | 15 | /** 16 | * Conversion from screw parameters to a dual quaternion. 17 | * @param [in] l The direction vector of the screw axis. 18 | * @param [in] m The moment vector of the screw axis. 19 | * @param [in] theta The angle of rotation in radians around the screw axis. 20 | * @param [in] d Displacement along the screw axis. 21 | */ 22 | template 23 | DualQuaternion 24 | convert_to_dualquat(const Vector3& l, const Vector3& m, T theta, T d) 25 | { 26 | const auto half_theta = T(0.5) * theta; 27 | const auto half_d = T(0.5) * d; 28 | const auto sine = std::sin(half_theta); 29 | 30 | DualQuaternion temp; 31 | temp.real() = Eigen::AngleAxis(theta, l); 32 | temp.dual().w() = -half_d * sine; 33 | temp.dual().vec() = sine * m + half_d * std::cos(half_theta) * l; 34 | return temp; 35 | } 36 | 37 | /** 38 | * Conversion from a dual quaternion to screw parameters. 39 | * @param [in] dq A dual quaternion representing a transformation. 40 | */ 41 | template 42 | std::tuple, Vector3, T, T> 43 | convert_to_screw(const DualQuaternion& dq) 44 | { 45 | const auto half_theta = std::acos(dq.real().w()); 46 | const auto theta = T(2) * half_theta; 47 | const Vector3 l = dq.real().vec() / std::sin(half_theta); 48 | const Vector3 t = T(2) * (dq.dual() * dq.real().conjugate()).vec(); 49 | const auto d = t.dot(l); 50 | const Vector3 m = T(0.5) * (t.cross(l) + (t - d * l) * std::tan(half_theta)); 51 | return std::make_tuple(l, m, theta, d); 52 | } 53 | 54 | /** 55 | * Screw linear interpolation. 56 | * @param [in] dq1 Unit Dual Quaternion as a start point for interpolation. 57 | * @param [in] dq2 Unit Dual Quaternion as an end point for interpolation. 58 | * @param [in] t Value varies between 0 and 1. 59 | */ 60 | template 61 | DualQuaternion 62 | sclerp(const DualQuaternion& dq1, const DualQuaternion& dq2, T t) 63 | { 64 | return dq1 * pow(quaternion_conjugate(dq1) * dq2, t); 65 | } 66 | 67 | /** 68 | * Screw linear interpolation. 69 | * @param [in] dq1 Unit Dual Quaternion as a start point for interpolation. 70 | * @param [in] dq2 Unit Dual Quaternion as an end point for interpolation. 71 | * @param [in] t Value varies between 0 and 1. 72 | */ 73 | template 74 | DualQuaternion 75 | sclerp_shortestpath(const DualQuaternion& dq1, const DualQuaternion& dq2, T t) 76 | { 77 | const auto cos_half_angle = dq1.real().dot(dq2.real()); 78 | const auto diff = quaternion_conjugate(dq1) * (cos_half_angle < T(0) ? -dq2 : dq2); 79 | return dq1 * pow(diff, t); 80 | } 81 | 82 | } // namespace dualquat 83 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ### Example of usage: 2 | 3 | You can run an example following the instructions in [applications](https://github.com/AUROVA-LAB/applications) (Examples). 4 | 5 | # aurova_odom 6 | This is a metapackage that contains different packages that perform different kind of odometry nodes. Compiling this metapackage into ROS will compile all the packages at once. This metapackage is grouped as a project for eclipse C++. Each package contains a "name_doxygen_config" configuration file for generate doxygen documentation. The packages contained in this metapackage are: 7 | 8 | **gps_to_odom** 9 | This package contains a node that, as input, reads the topics /fix, of type sensor_msgs::NavSatFix, and /rover/fix_velocity of type geometry_msgs::TwistWithCovariance (if from gazebo /fix_vel of type geometry_msgs::Vector3Stamped). This node transform gps location from /fix in utm frame to a frame defined in ~gps_to_odom/frame_id. Also calculates the orientation using the velocities from gps, and generate new odometry (message type nav_msgs::Odometry). This message is published in output topic called /odometry_gps. 10 | 11 | Parameters: 12 | * ~gps_to_odom/frame_id (default: ""): This is the frame to transform from utm (usually set as "map"). 13 | * ~gps_to_odom/min_speed (default: null): This is used as a threshold. Under this velocity, the orientation is not computed from the GPS velocity message. 14 | * ~gps_to_odom/max_speed (default: null): We add additive variance to the orientation message inversely proportional to this parameter. 15 | 16 | **ackermann_to_odom** 17 | This package contains a node that, as input, reads the topics /estimated_ackermann_state and /covariance_ackermann_state, of type ackermann_msgs::AckermannDriveStamped, and /virtual_imu_data of type sensor_msgs::Imu. This node parse this information as a new message type nav_msgs::Odometry using the 2D tricicle model. This message is published in an output topic called /odometry. 18 | 19 | Parameters: 20 | * ~odom_in_tf (default: false): If this parameter is set to true, the odometry is also published in /tf topic. 21 | * ~scan_in_tf (default: false): If this parameter is set to true, the laser transform read from the static robot transformation is published in /tf topic. 22 | * ~frame_id (default: ""): This parameter is the name of frame to transform if scan_in_tf is true. 23 | * ~child_id (default: ""): This parameter is the name of child frame to transform if scan_in_tf is true. 24 | 25 | **odom_estimation_pc** (**LiLO**->Lite LiDAR Odometry method based on SRI) 26 | 27 | This package estimates odometry from a point cloud segmented into edges and surface. The package works in conjunction with [pc2image](https://github.com/AUROVA-LAB/aurova_preprocessed/tree/master/pc2image), which converts a point cloud into an SRI range image and filters it to extract the desired features. 28 | As input topics are required: 29 | * /pcl_edge : Topic by which the edge point cloud is subscribed to. 30 | * /pcl_surf: Topic to subscribe to the surface point cloud. 31 | As output topic are: 32 | */odom: Topic that has the estimated odometry. 33 | 34 | Parameters: 35 | * ~/target_frame_name (default: "odom") 36 | * ~/source_frame_name (default: "base_link") 37 | * ~/clear_map (default: "false") : This paramater enable the clear map funtion. 38 | * ~/childframeID (default: ""): This parameter is the name of child frame to transform. 39 | -------------------------------------------------------------------------------- /dualquat_LOAM/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(dualquat_loam) 3 | 4 | ## Compile as C++14 5 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 6 | set(CMAKE_CXX_EXTENSIONS OFF) 7 | set(CMAKE_CXX_STANDARD 14) 8 | set(CMAKE_BUILD_TYPE "Release") 9 | set(CMAKE_CXX_FLAGS "-std=c++14") 10 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g") 11 | 12 | message("Current CPU archtecture: ${CMAKE_SYSTEM_PROCESSOR}") 13 | if(CMAKE_SYSTEM_PROCESSOR MATCHES "(x86)|(X86)|(amd64)|(AMD64)" ) 14 | include(ProcessorCount) 15 | ProcessorCount(N) 16 | message("Processer number: ${N}") 17 | if(N GREATER 5) 18 | add_definitions(-DMP_EN) 19 | add_definitions(-DMP_PROC_NUM=4) 20 | message("core for MP: 4") 21 | elseif(N GREATER 3) 22 | math(EXPR PROC_NUM "${N} - 2") 23 | add_definitions(-DMP_EN) 24 | add_definitions(-DMP_PROC_NUM="${PROC_NUM}") 25 | message("core for MP: ${PROC_NUM}") 26 | else() 27 | add_definitions(-DMP_PROC_NUM=1) 28 | endif() 29 | else() 30 | add_definitions(-DMP_PROC_NUM=1) 31 | endif() 32 | 33 | # Find nanoflannConfig.cmake: 34 | find_package(nanoflann) 35 | include_directories("/home/ws/src/nanoflann/include") 36 | 37 | find_package(catkin REQUIRED COMPONENTS 38 | eigen_conversions 39 | geometry_msgs 40 | nav_msgs 41 | rosbag 42 | roscpp 43 | rospy 44 | sensor_msgs 45 | std_msgs 46 | tf 47 | pcl_conversions 48 | pcl_ros 49 | tf_conversions 50 | 51 | ) 52 | 53 | include_directories( "/usr/include/dqrobotics" ) 54 | 55 | find_package(Eigen3) 56 | if(NOT EIGEN3_FOUND) 57 | find_package(cmake_modules REQUIRED) 58 | find_package(Eigen REQUIRED) 59 | set(EIGEN3_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS}) 60 | set(EIGEN3_LIBRARIES ${EIGEN_LIBRARIES}) 61 | else() 62 | set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR}) 63 | endif() 64 | 65 | ## System dependencies are found with CMake's conventions 66 | find_package(OpenMP QUIET) 67 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 68 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 69 | find_package(PCL REQUIRED) 70 | find_package(Ceres REQUIRED) 71 | find_package(GTSAM REQUIRED QUIET) 72 | 73 | set(CMAKE_AUTOMOC ON) 74 | set(CMAKE_AUTOUIC ON) 75 | set(CMAKE_AUTORCC ON) 76 | set(CMAKE_INCLUDE_CURRENT_DIR ON) 77 | 78 | 79 | include_directories( 80 | include 81 | ${catkin_INCLUDE_DIRS} 82 | ${PCL_INCLUDE_DIRS} 83 | ${CERES_INCLUDE_DIRS} 84 | ${EIGEN3_INCLUDE_DIRS} 85 | ) 86 | 87 | link_directories( 88 | include 89 | ${PCL_LIBRARY_DIRS} 90 | ${CERES_LIBRARY_DIRS} 91 | ) 92 | 93 | catkin_package( 94 | CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs 95 | DEPENDS EIGEN3 PCL Ceres 96 | INCLUDE_DIRS include 97 | ) 98 | 99 | add_executable(dq_pose_estimation_node src/odomEstimationNode.cpp src/odomEstimationClass.cpp src/STDesc.cpp src/KDTree_STD.cpp) 100 | target_link_libraries(dq_pose_estimation_node ${EIGEN3_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES} dqrobotics) 101 | 102 | add_executable(laser_mapping src/laserMappingNode.cpp src/laserMappingClass.cpp) 103 | target_link_libraries(laser_mapping ${EIGEN3_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES} dqrobotics) 104 | 105 | add_executable(std_extractor src/std_extractor.cpp src/STDesc.cpp src/KDTree_STD.cpp) 106 | target_link_libraries(std_extractor 107 | ${catkin_LIBRARIES} 108 | ${PCL_LIBRARIES} 109 | ${OpenCV_LIBS} 110 | ${CERES_LIBRARIES}) 111 | 112 | -------------------------------------------------------------------------------- /odom_estimation_pc/include/odomEstimationClass.h: -------------------------------------------------------------------------------- 1 | // Author of FLOAM: Wang Han 2 | // Email wh200720041@gmail.com 3 | // Homepage https://wanghan.pro 4 | 5 | // Author of Lilo: Edison Velasco 6 | // Email evs25@alu.ua.es 7 | 8 | #ifndef _ODOM_ESTIMATION_CLASS_H_ 9 | #define _ODOM_ESTIMATION_CLASS_H_ 10 | 11 | //std lib 12 | #include 13 | #include 14 | #include 15 | 16 | //PCL 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | //ceres 28 | #include 29 | #include 30 | 31 | //eigen 32 | #include 33 | #include 34 | 35 | //LOCAL LIB 36 | #include "lidar.h" 37 | #include "lidarOptimization.h" 38 | #include 39 | 40 | class OdomEstimationClass 41 | { 42 | 43 | public: 44 | OdomEstimationClass(); 45 | 46 | void init(lidar::Lidar lidar_param, double edge_resolution,double surf_resolution); 47 | void initMapWithPoints(const pcl::PointCloud::Ptr& edge_in, const pcl::PointCloud::Ptr& surf_in); 48 | void updatePointsToMap(const pcl::PointCloud::Ptr& edge_in, const pcl::PointCloud::Ptr& surf_in, bool clear_map ,double cropBox_len); 49 | void getMap(pcl::PointCloud::Ptr& laserCloudMap); 50 | 51 | Eigen::Isometry3d odom; 52 | pcl::PointCloud::Ptr laserCloudCornerMap; 53 | pcl::PointCloud::Ptr laserCloudSurfMap; 54 | private: 55 | //optimization variable 56 | double parameters[7] = {0, 0, 0, 1, 0, 0, 0}; 57 | Eigen::Map q_w_curr = Eigen::Map(parameters); 58 | Eigen::Map t_w_curr = Eigen::Map(parameters + 4); 59 | 60 | Eigen::Isometry3d last_odom; 61 | 62 | //kd-tree 63 | pcl::KdTreeFLANN::Ptr kdtreeEdgeMap; 64 | pcl::KdTreeFLANN::Ptr kdtreeSurfMap; 65 | 66 | //points downsampling before add to map 67 | pcl::VoxelGrid downSizeFilterEdge; 68 | pcl::VoxelGrid downSizeFilterSurf; 69 | 70 | //local map 71 | pcl::CropBox cropBoxFilter; 72 | 73 | //optimization count 74 | int optimization_count; 75 | 76 | //function 77 | void addEdgeCostFactor(const pcl::PointCloud::Ptr& pc_in, const pcl::PointCloud::Ptr& map_in, ceres::Problem& problem, ceres::LossFunction *loss_function); 78 | void addSurfCostFactor(const pcl::PointCloud::Ptr& pc_in, const pcl::PointCloud::Ptr& map_in, ceres::Problem& problem, ceres::LossFunction *loss_function); 79 | void addPointsToMap(const pcl::PointCloud::Ptr& downsampledEdgeCloud, const pcl::PointCloud::Ptr& downsampledSurfCloud, bool clear_map, double cropBox_len); 80 | void pointAssociateToMap(pcl::PointXYZ const *const pi, pcl::PointXYZ *const po); 81 | void downSamplingToMap(const pcl::PointCloud::Ptr& edge_pc_in, pcl::PointCloud::Ptr& edge_pc_out, const pcl::PointCloud::Ptr& surf_pc_in, pcl::PointCloud::Ptr& surf_pc_out); 82 | void occlude_pcd(pcl::PointCloud::Ptr & cld_ptr,int dim, double threshA, double threshB); 83 | }; 84 | 85 | #endif // _ODOM_ESTIMATION_CLASS_H_ 86 | 87 | -------------------------------------------------------------------------------- /dualquat_LOAM/include/dualquat/dualquat_transformation.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file dualquat/dualquat_transformation.h 3 | * @brief This file provides transformation functions for dual quaternion types. 4 | */ 5 | #pragma once 6 | 7 | #include "dualquat_common.h" 8 | 9 | namespace dualquat 10 | { 11 | 12 | /** 13 | * This transformation applies first the rotation and then the translation. 14 | */ 15 | template 16 | DualQuaternion 17 | transformation(const Quaternion& r, const Vector3& t) 18 | { 19 | auto dual = Quaternion(T(0), t.x(), t.y(), t.z()) * r; 20 | dual.coeffs() *= T(0.5); 21 | return DualQuaternion(r, dual); 22 | } 23 | 24 | /** 25 | * This transformation applies first the translation and then the rotation. 26 | */ 27 | template 28 | DualQuaternion 29 | transformation(const Vector3& t, const Quaternion& r) 30 | { 31 | auto dual = r * Quaternion(T(0), t.x(), t.y(), t.z()); 32 | dual.coeffs() *= T(0.5); 33 | return DualQuaternion(r, dual); 34 | } 35 | 36 | /** 37 | * This transformation applies pure rotation. 38 | */ 39 | template 40 | DualQuaternion 41 | transformation(const Quaternion& r) 42 | { 43 | return DualQuaternion( 44 | r, 45 | Quaternion(Quaternion::Coefficients::Zero())); 46 | } 47 | 48 | /** 49 | * This transformation applies pure translation. 50 | */ 51 | template 52 | DualQuaternion 53 | transformation(const Vector3& t) 54 | { 55 | auto dual = Quaternion(T(0), t.x(), t.y(), t.z()); 56 | dual.coeffs() *= T(0.5); 57 | return DualQuaternion(Quaternion::Identity(), dual); 58 | } 59 | 60 | /** 61 | * Returns the difference between the two dual quaternions representing the transformation. 62 | */ 63 | template 64 | DualQuaternion 65 | transformational_difference(const DualQuaternion& dq1, const DualQuaternion& dq2) 66 | { 67 | return quaternion_conjugate(dq1) * dq2; 68 | } 69 | 70 | /** 71 | * Transforms a point with a unit dual quaternion. 72 | * @param [in] dq A unit dual quaternion representing a transformation. 73 | * @param [in] p A point represented by a dual quaternion. 74 | */ 75 | template 76 | DualQuaternion 77 | transform_point(const DualQuaternion& dq, const DualQuaternion& p) 78 | { 79 | return dq * p * total_conjugate(dq); 80 | } 81 | 82 | /** 83 | * Transforms a point with a unit dual quaternion. 84 | * @see dualquat::transform_point() 85 | */ 86 | template 87 | DualQuaternion 88 | transform_point(const DualQuaternion& dq, const Vector3& p) 89 | { 90 | return transform_point(dq, DualQuaternion(p)); 91 | } 92 | 93 | /** 94 | * Transforms a line with a unit dual quaternion. 95 | * @param [in] dq A unit dual quaternion representing a transformation. 96 | * @param [in] l A line in the Plucker coordinates represented by a dual quaternion. 97 | */ 98 | template 99 | DualQuaternion 100 | transform_line(const DualQuaternion& dq, const DualQuaternion& l) 101 | { 102 | return dq * l * quaternion_conjugate(dq); 103 | } 104 | 105 | /** 106 | * Transforms a line with a unit dual quaternion. 107 | * @see dualquat::transform_line() 108 | */ 109 | template 110 | DualQuaternion 111 | transform_line(const DualQuaternion& dq, const Vector3& l, const Vector3& m) 112 | { 113 | return transform_line(dq, DualQuaternion(l, m)); 114 | } 115 | 116 | } // namespace dualquat 117 | -------------------------------------------------------------------------------- /dualquat_LOAM/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM osrf/ros:noetic-desktop-focal 2 | 3 | # install ros packages 4 | RUN apt-get update && apt-get install -y --no-install-recommends \ 5 | ros-noetic-desktop-full=1.5.0-1* \ 6 | && rm -rf /var/lib/apt/lists/* 7 | 8 | ENV DEBIAN_FRONTEND=noninteractive 9 | 10 | RUN apt-get update && apt-get install -y apt-utils curl wget git bash-completion build-essential sudo && rm -rf /var/lib/apt/lists/* 11 | 12 | # Now create the user 13 | ARG UID=1000 14 | ARG GID=1000 15 | RUN addgroup --gid ${GID} docker 16 | RUN adduser --gecos "ROS User" --disabled-password --uid ${UID} --gid ${GID} docker 17 | RUN usermod -a -G dialout docker 18 | RUN mkdir config && echo "ros ALL=(ALL) NOPASSWD: ALL" > config/99_aptget 19 | RUN cp config/99_aptget /etc/sudoers.d/99_aptget 20 | RUN chmod 0440 /etc/sudoers.d/99_aptget && chown root:root /etc/sudoers.d/99_aptget 21 | 22 | RUN apt-get update && apt-get install -y apt-utils curl wget git bash-completion build-essential sudo && rm -rf /var/lib/apt/lists/* 23 | 24 | ENV HOME /home/docker 25 | RUN mkdir -p ${HOME}/catkin_ws/library 26 | 27 | # Install Ceres 28 | RUN apt-get update && apt-get install cmake libgoogle-glog-dev libgflags-dev libatlas-base-dev libeigen3-dev libsuitesparse-dev -y 29 | RUN cd ${HOME}/catkin_ws/library && wget http://ceres-solver.org/ceres-solver-2.2.0.tar.gz 30 | RUN cd ${HOME}/catkin_ws/library && tar zxf ceres-solver-2.2.0.tar.gz && cd ceres-solver-2.2.0 && mkdir ceres-bin && cd ceres-bin && cmake .. && cmake .. && make -j$(nproc) && make install 31 | 32 | 33 | # ########### 34 | 35 | # Lidar Odometry dependeces 36 | RUN apt-get update && \ 37 | apt-get install -y --no-install-recommends \ 38 | ros-noetic-hector-trajectory-server \ 39 | vim # extras tools 40 | 41 | 42 | RUN . /opt/ros/noetic/setup.sh && \ 43 | mkdir -p ${HOME}/catkin_ws/src && \ 44 | cd ${HOME}/catkin_ws/src 45 | 46 | 47 | # Install DQ library 48 | RUN apt-get update && \ 49 | apt-get install -y software-properties-common && \ 50 | add-apt-repository ppa:dqrobotics-dev/release && \ 51 | apt-get update 52 | 53 | # Instalar libdqrobotics 54 | RUN apt-get install -y libdqrobotics 55 | 56 | # STD dependences 57 | # Add PPA 58 | RUN add-apt-repository ppa:borglab/gtsam-release-4.0 59 | RUN apt update && apt install -y libgtsam-dev libgtsam-unstable-dev 60 | 61 | # install nanoflann 62 | RUN cd ${HOME}/catkin_ws/library && git clone https://github.com/EPVelasco/nanoflann.git && cd nanoflann && mkdir build && cd build && cmake .. && sudo make install 63 | 64 | # catkin_make dualquat_loam 65 | RUN mkdir ${HOME}/catkin_ws/src/dualquat_LOAM/ 66 | COPY . ${HOME}/catkin_ws/src/dualquat_LOAM/ 67 | RUN . /opt/ros/noetic/setup.sh && \ 68 | cd ${HOME}/catkin_ws/ && catkin_make --only-pkg-with-deps dualquat_loam 69 | 70 | 71 | # Install Aurova preprocessing 72 | RUN cd ${HOME}/catkin_ws/src/ && git clone https://github.com/AUROVA-LAB/aurova_preprocessed.git 73 | RUN . /opt/ros/noetic/setup.sh && \ 74 | cd ${HOME}/catkin_ws/ && catkin_make --only-pkg-with-deps pc_feature_extraction 75 | 76 | 77 | # bash update 78 | RUN echo "TERM=xterm-256color" >> ~/.bashrc 79 | RUN echo "# COLOR Text" >> ~/.bashrc 80 | RUN echo "PS1='\[\033[01;33m\]\u\[\033[01;33m\]@\[\033[01;33m\]\h\[\033[01;34m\]:\[\033[00m\]\[\033[01;34m\]\w\[\033[00m\]\$ '" >> ~/.bashrc 81 | RUN echo "CLICOLOR=1" >> ~/.bashrc 82 | RUN echo "LSCOLORS=GxFxCxDxBxegedabagaced" >> ~/.bashrc 83 | RUN echo "" >> ~/.bashrc 84 | RUN echo "## ROS" >> ~/.bashrc 85 | RUN echo "source /opt/ros/noetic/setup.bash" >> ~/.bashrc 86 | 87 | ############ 88 | # Configura la variable de entorno DISPLAYWORKDIR 89 | USER docker 90 | ENV DISPLAY=:0 91 | ENV NVIDIA_DRIVER_CAPABILITIES graphics,compute,utility 92 | WORKDIR ${HOME}/catkin_ws 93 | -------------------------------------------------------------------------------- /ackermann_to_odom/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(ackermann_to_odom) 3 | 4 | ## Find catkin macros and libraries 5 | find_package(catkin REQUIRED) 6 | # ******************************************************************** 7 | # Add catkin additional components here 8 | # ******************************************************************** 9 | find_package(catkin REQUIRED COMPONENTS 10 | iri_base_algorithm 11 | tf 12 | ) 13 | 14 | ## System dependencies are found with CMake's conventions 15 | # find_package(Boost REQUIRED COMPONENTS system) 16 | 17 | # ******************************************************************** 18 | # Add system and labrobotica dependencies here 19 | # ******************************************************************** 20 | # find_package( REQUIRED) 21 | 22 | # ******************************************************************** 23 | # Add topic, service and action definition here 24 | # ******************************************************************** 25 | ## Generate messages in the 'msg' folder 26 | # add_message_files( 27 | # FILES 28 | # Message1.msg 29 | # Message2.msg 30 | # ) 31 | 32 | ## Generate services in the 'srv' folder 33 | # add_service_files( 34 | # FILES 35 | # Service1.srv 36 | # Service2.srv 37 | # ) 38 | 39 | ## Generate actions in the 'action' folder 40 | # add_action_files( 41 | # FILES 42 | # Action1.action 43 | # Action2.action 44 | # ) 45 | 46 | ## Generate added messages and services with any dependencies listed here 47 | # generate_messages( 48 | # DEPENDENCIES 49 | # std_msgs # Or other packages containing msgs 50 | # ) 51 | 52 | # ******************************************************************** 53 | # Add the dynamic reconfigure file 54 | # ******************************************************************** 55 | generate_dynamic_reconfigure_options(cfg/AckermannToOdom.cfg) 56 | 57 | # ******************************************************************** 58 | # Add run time dependencies here 59 | # ******************************************************************** 60 | catkin_package( 61 | # INCLUDE_DIRS 62 | # LIBRARIES 63 | # ******************************************************************** 64 | # Add ROS and IRI ROS run time dependencies 65 | # ******************************************************************** 66 | CATKIN_DEPENDS iri_base_algorithm 67 | # ******************************************************************** 68 | # Add system and labrobotica run time dependencies here 69 | # ******************************************************************** 70 | # DEPENDS 71 | ) 72 | 73 | ########### 74 | ## Build ## 75 | ########### 76 | 77 | # ******************************************************************** 78 | # Add the include directories 79 | # ******************************************************************** 80 | include_directories(include) 81 | include_directories(${catkin_INCLUDE_DIRS}) 82 | # include_directories(${_INCLUDE_DIR}) 83 | 84 | ## Declare a cpp library 85 | # add_library(${PROJECT_NAME} ) 86 | 87 | ## Declare a cpp executable 88 | add_executable(${PROJECT_NAME} src/ackermann_to_odom_alg.cpp src/ackermann_to_odom_alg_node.cpp) 89 | 90 | # ******************************************************************** 91 | # Add the libraries 92 | # ******************************************************************** 93 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) 94 | # target_link_libraries(${PROJECT_NAME} ${_LIBRARY}) 95 | 96 | # ******************************************************************** 97 | # Add message headers dependencies 98 | # ******************************************************************** 99 | # add_dependencies(${PROJECT_NAME} _generate_messages_cpp) 100 | # ******************************************************************** 101 | # Add dynamic reconfigure dependencies 102 | # ******************************************************************** 103 | add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS}) 104 | -------------------------------------------------------------------------------- /dualquat_LOAM/include/odomEstimationClass.h: -------------------------------------------------------------------------------- 1 | // Author of Lilo: Edison Velasco 2 | // Email evs25@alu.ua.es 3 | 4 | #ifndef _ODOM_ESTIMATION_CLASS_H_ 5 | #define _ODOM_ESTIMATION_CLASS_H_ 6 | 7 | //std lib 8 | #include 9 | #include 10 | #include 11 | 12 | //PCL 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | 29 | // ROS msgs 30 | #include 31 | 32 | //ceres 33 | #include 34 | #include 35 | 36 | //eigen 37 | #include 38 | #include 39 | 40 | //LOCAL LIB 41 | #include 42 | 43 | // STD LIB 44 | #include "STDesc.h" 45 | #include "KDTree_STD.h" 46 | 47 | class OdomEstimationClass 48 | { 49 | 50 | public: 51 | OdomEstimationClass(); 52 | 53 | void init(double edge_resolution,double surf_resolution); 54 | void initMapWithPoints(const pcl::PointCloud::Ptr& edge_in, const pcl::PointCloud::Ptr& surf_in); 55 | void updatePointsToMap(const pcl::PointCloud::Ptr& edge_in, const pcl::PointCloud::Ptr& surf_in, 56 | const std::vector& stdC_pair, const std::vector& stdM_pair, bool clear_map ,double cropBox_len, double cont_opti); 57 | void getMap(pcl::PointCloud::Ptr& laserCloudMap); 58 | 59 | Eigen::Isometry3d odom; 60 | pcl::PointCloud::Ptr laserCloudCornerMap; 61 | pcl::PointCloud::Ptr laserCloudSurfMap; 62 | geometry_msgs::PoseArrayConstPtr std_posesMap; 63 | 64 | pcl::PointCloud::Ptr edge_cloud; 65 | pcl::PointCloud::Ptr surf_cloud; 66 | 67 | private: 68 | 69 | double parameters[8] = {1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; 70 | Eigen::Map> dual_quat = Eigen::Map>(parameters); 71 | 72 | Eigen::Isometry3d last_odom; 73 | 74 | //kd-tree edge 75 | pcl::KdTreeFLANN::Ptr kdtreeEdgeMap; 76 | pcl::KdTreeFLANN::Ptr kdtreeLocalMap; 77 | 78 | //kd-tree surf 79 | pcl::KdTreeFLANN::Ptr kdtreeSurfMap; 80 | 81 | //points downsampling before add to map 82 | pcl::VoxelGrid downSizeFilterEdge; 83 | pcl::VoxelGrid downSizeFilterSurf; 84 | 85 | //local map 86 | pcl::CropBox cropBoxFilter; 87 | 88 | //optimization count 89 | int optimization_count; 90 | 91 | void addPointsToMap(const pcl::PointCloud::Ptr& downsampledEdgeCloud, const pcl::PointCloud::Ptr& downsampledSurfCloud, bool clear_map, double cropBox_len); 92 | void pointAssociateToMap(pcl::PointXYZ const *const pi, pcl::PointXYZ *const po); 93 | void pointLocalToMap(pcl::PointXYZ const *const pi, pcl::PointXYZ *const po); 94 | void downSamplingToMap(const pcl::PointCloud::Ptr& edge_pc_in, pcl::PointCloud::Ptr& edge_pc_out, const pcl::PointCloud::Ptr& surf_pc_in, pcl::PointCloud::Ptr& surf_pc_out); 95 | void occlude_pcd(pcl::PointCloud::Ptr & cld_ptr,int dim, double threshA, double threshB); 96 | void addEdgeDQCostFactor(const pcl::PointCloud::Ptr& pc_in, const pcl::PointCloud::Ptr& map_in, ceres::Problem& problem, ceres::LossFunction *loss_function, ceres::Manifold* dq_manifold, double cropBox_len); 97 | void addSurfDQCostFactor(const pcl::PointCloud::Ptr& pc_in, const pcl::PointCloud::Ptr& map_in, ceres::Problem& problem, ceres::LossFunction *loss_function, ceres::Manifold* dq_manifold, double cropBox_len); 98 | void addSTDCostFactor(std::vector stdC_pair, std::vector stdM_pair, ceres::Problem& problem, ceres::LossFunction *loss_function, ceres::Manifold* dq_manifold); 99 | 100 | }; 101 | 102 | #endif // _ODOM_ESTIMATION_CLASS_H_ -------------------------------------------------------------------------------- /gps_to_odom/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(gps_to_odom) 3 | 4 | ## Find catkin macros and libraries 5 | find_package(catkin REQUIRED) 6 | find_package(planning REQUIRED) 7 | find_package(PCL REQUIRED) 8 | FIND_PACKAGE(Eigen3 REQUIRED) 9 | # ******************************************************************** 10 | # Add catkin additional components here 11 | # ******************************************************************** 12 | find_package(catkin REQUIRED COMPONENTS iri_base_algorithm tf eigen_conversions tf_conversions) 13 | 14 | ## System dependencies are found with CMake's conventions 15 | # find_package(Boost REQUIRED COMPONENTS system) 16 | 17 | # ******************************************************************** 18 | # Add system and labrobotica dependencies here 19 | # ******************************************************************** 20 | # find_package( REQUIRED) 21 | 22 | # ******************************************************************** 23 | # Add topic, service and action definition here 24 | # ******************************************************************** 25 | ## Generate messages in the 'msg' folder 26 | # add_message_files( 27 | # FILES 28 | # Message1.msg 29 | # Message2.msg 30 | # ) 31 | 32 | ## Generate services in the 'srv' folder 33 | # add_service_files( 34 | # FILES 35 | # Service1.srv 36 | # Service2.srv 37 | # ) 38 | 39 | ## Generate actions in the 'action' folder 40 | # add_action_files( 41 | # FILES 42 | # Action1.action 43 | # Action2.action 44 | # ) 45 | 46 | ## Generate added messages and services with any dependencies listed here 47 | # generate_messages( 48 | # DEPENDENCIES 49 | # std_msgs # Or other packages containing msgs 50 | # ) 51 | 52 | # ******************************************************************** 53 | # Add the dynamic reconfigure file 54 | # ******************************************************************** 55 | generate_dynamic_reconfigure_options(cfg/GpsToOdom.cfg) 56 | 57 | # ******************************************************************** 58 | # Add run time dependencies here 59 | # ******************************************************************** 60 | catkin_package( 61 | # INCLUDE_DIRS 62 | # LIBRARIES 63 | # ******************************************************************** 64 | # Add ROS and IRI ROS run time dependencies 65 | # ******************************************************************** 66 | CATKIN_DEPENDS iri_base_algorithm 67 | # ******************************************************************** 68 | # Add system and labrobotica run time dependencies here 69 | # ******************************************************************** 70 | DEPENDS Eigen 71 | ) 72 | 73 | ########### 74 | ## Build ## 75 | ########### 76 | 77 | # ******************************************************************** 78 | # Add the include directories 79 | # ******************************************************************** 80 | include_directories(include) 81 | include_directories(${catkin_INCLUDE_DIRS} 82 | ${PCL_INCLUDE_DIRS} 83 | ${EIGEN_INCLUDE_DIRS}) 84 | # include_directories(${_INCLUDE_DIR}) 85 | 86 | ## Declare a cpp library 87 | # add_library(${PROJECT_NAME} ) 88 | 89 | ## Declare a cpp executable 90 | add_executable(${PROJECT_NAME} src/gps_to_odom_alg.cpp src/gps_to_odom_alg_node.cpp) 91 | 92 | # ******************************************************************** 93 | # Add the libraries 94 | # ******************************************************************** 95 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) 96 | TARGET_LINK_LIBRARIES(${PROJECT_NAME} planning) 97 | # target_link_libraries(${PROJECT_NAME} ${_LIBRARY}) 98 | 99 | # ******************************************************************** 100 | # Add message headers dependencies 101 | # ******************************************************************** 102 | # add_dependencies(${PROJECT_NAME} _generate_messages_cpp) 103 | # ******************************************************************** 104 | # Add dynamic reconfigure dependencies 105 | # ******************************************************************** 106 | add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS}) 107 | -------------------------------------------------------------------------------- /ackermann_to_odom/src/ackermann_to_odom_alg_node.cpp: -------------------------------------------------------------------------------- 1 | #include "ackermann_to_odom_alg_node.h" 2 | 3 | AckermannToOdomAlgNode::AckermannToOdomAlgNode(void) : 4 | algorithm_base::IriBaseAlgorithm() 5 | { 6 | //init class attributes if necessary 7 | //this->loop_rate_ = 10; //in [Hz] 8 | this->virtual_imu_msg_.orientation.x = 0.0; 9 | this->virtual_imu_msg_.orientation.y = 0.0; 10 | this->virtual_imu_msg_.orientation.z = 0.0; 11 | this->virtual_imu_msg_.orientation.w = 1.0; 12 | this->estimated_ackermann_state_.drive.speed = 0.0; 13 | this->estimated_ackermann_state_.drive.steering_angle = 0.0; 14 | 15 | // [init publishers] 16 | this->odometry_publisher_ = this->public_node_handle_.advertise < nav_msgs::Odometry > ("/odometry", 1); 17 | this->pose_publisher_ = this->public_node_handle_.advertise < geometry_msgs::PoseWithCovarianceStamped > ("/odometry_pose", 1); 18 | 19 | // [init subscribers] 20 | this->estimated_ackermann_subscriber_ = this->public_node_handle_.subscribe( 21 | "/estimated_ackermann_state", 1, &AckermannToOdomAlgNode::cb_ackermannState, this); 22 | this->virtual_imu_subscriber_ = this->public_node_handle_.subscribe("/virtual_imu_data", 1, 23 | &AckermannToOdomAlgNode::cb_imuData, this); 24 | 25 | // [init services] 26 | 27 | // [init clients] 28 | 29 | // [init action servers] 30 | 31 | // [init action clients] 32 | } 33 | 34 | AckermannToOdomAlgNode::~AckermannToOdomAlgNode(void) 35 | { 36 | // [free dynamic memory] 37 | } 38 | 39 | void AckermannToOdomAlgNode::mainNodeThread(void) 40 | { 41 | static bool first_exec = true; 42 | static bool odom_in_tf; 43 | static bool scan_in_tf; 44 | static std::string frame_id; 45 | static std::string child_id; 46 | 47 | // [read parameters] 48 | if (first_exec) 49 | { 50 | this->public_node_handle_.getParam("/odom_in_tf", odom_in_tf); 51 | this->public_node_handle_.getParam("/scan_in_tf", scan_in_tf); 52 | this->public_node_handle_.getParam("/frame_id", frame_id); 53 | this->public_node_handle_.getParam("/child_id", child_id); 54 | first_exec = false; 55 | } 56 | 57 | // [listen transform] 58 | try 59 | { 60 | this->listener_.lookupTransform(frame_id, child_id, ros::Time(0), this->scan_trans_); 61 | } 62 | catch (tf::TransformException ex) 63 | { 64 | ROS_ERROR("%s", ex.what()); 65 | ros::Duration(1.0).sleep(); 66 | } 67 | 68 | // [fill msg structures] 69 | this->alg_.generateNewOdometryMsg2D(this->estimated_ackermann_state_, this->virtual_imu_msg_, this->odometry_pose_, 70 | this->odometry_, this->odom_trans_); 71 | 72 | // [fill srv structure and make request to the server] 73 | 74 | // [fill action structure and make request to the action server] 75 | 76 | // [publish messages] 77 | if (odom_in_tf) 78 | { 79 | this->broadcaster_.sendTransform(this->odom_trans_); 80 | } 81 | if (scan_in_tf) 82 | { 83 | this->broadcaster_.sendTransform(this->scan_trans_); 84 | } 85 | 86 | this->odometry_publisher_.publish(this->odometry_); 87 | this->pose_publisher_.publish(this->odometry_pose_); 88 | } 89 | 90 | /* [subscriber callbacks] */ 91 | void AckermannToOdomAlgNode::cb_ackermannState( 92 | const ackermann_msgs::AckermannDriveStamped::ConstPtr& estimated_ackermann_state_msg) 93 | { 94 | this->alg_.lock(); 95 | 96 | this->estimated_ackermann_state_.drive.speed = estimated_ackermann_state_msg->drive.speed; 97 | this->estimated_ackermann_state_.drive.steering_angle = estimated_ackermann_state_msg->drive.steering_angle; 98 | 99 | this->alg_.unlock(); 100 | } 101 | 102 | void AckermannToOdomAlgNode::cb_imuData(const sensor_msgs::Imu::ConstPtr& Imu_msg) 103 | { 104 | this->alg_.lock(); 105 | 106 | this->virtual_imu_msg_.orientation.x = Imu_msg->orientation.x; 107 | this->virtual_imu_msg_.orientation.y = Imu_msg->orientation.y; 108 | this->virtual_imu_msg_.orientation.z = Imu_msg->orientation.z; 109 | this->virtual_imu_msg_.orientation.w = Imu_msg->orientation.w; 110 | 111 | this->alg_.unlock(); 112 | } 113 | 114 | /* [service callbacks] */ 115 | 116 | /* [action callbacks] */ 117 | 118 | /* [action requests] */ 119 | 120 | void AckermannToOdomAlgNode::node_config_update(Config &config, uint32_t level) 121 | { 122 | this->alg_.lock(); 123 | this->config_ = config; 124 | this->alg_.unlock(); 125 | } 126 | 127 | void AckermannToOdomAlgNode::addNodeDiagnostics(void) 128 | { 129 | } 130 | 131 | /* main function */ 132 | int main(int argc, char *argv[]) 133 | { 134 | return algorithm_base::main < AckermannToOdomAlgNode > (argc, argv, "ackermann_to_odom_alg_node"); 135 | } 136 | -------------------------------------------------------------------------------- /gps_to_odom/include/gps_to_odom_alg.h: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC. 2 | // Author 3 | // All rights reserved. 4 | // 5 | // This file is part of iri-ros-pkg 6 | // iri-ros-pkg is free software: you can redistribute it and/or modify 7 | // it under the terms of the GNU Lesser General Public License as published by 8 | // the Free Software Foundation, either version 3 of the License, or 9 | // at your option) any later version. 10 | // 11 | // This program is distributed in the hope that it will be useful, 12 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | // GNU Lesser General Public License for more details. 15 | // 16 | // You should have received a copy of the GNU Lesser General Public License 17 | // along with this program. If not, see . 18 | // 19 | // IMPORTANT NOTE: This code has been generated through a script from the 20 | // iri_ros_scripts. Please do NOT delete any comments to guarantee the correctness 21 | // of the scripts. ROS topics can be easly add by using those scripts. Please 22 | // refer to the IRI wiki page for more information: 23 | // http://wikiri.upc.es/index.php/Robotics_Lab 24 | /** 25 | * \file gps_to_odom_alg.h 26 | * 27 | * Created on: 04 Dec 2018 28 | * Author: m.a.munoz 29 | */ 30 | 31 | #ifndef _gps_to_odom_alg_h_ 32 | #define _gps_to_odom_alg_h_ 33 | 34 | #include 35 | #include 36 | #include 37 | #include "nav_msgs/Odometry.h" 38 | #include "sensor_msgs/NavSatFix.h" 39 | #include "geometry_msgs/TwistWithCovarianceStamped.h" 40 | #include "geometry_msgs/TwistStamped.h" 41 | #include "geometry_msgs/Vector3Stamped.h" 42 | #include "math.h" 43 | #include "planning/graph.h" 44 | 45 | //include gps_to_odom_alg main library 46 | 47 | /** 48 | * \brief IRI ROS Specific Driver Class 49 | * 50 | * 51 | */ 52 | class GpsToOdomAlgorithm 53 | { 54 | protected: 55 | /** 56 | * \brief define config type 57 | * 58 | * Define a Config type with the GpsToOdomConfig. All driver implementations 59 | * will then use the same variable type Config. 60 | */ 61 | pthread_mutex_t access_; 62 | 63 | // private attributes and methods 64 | 65 | public: 66 | /** 67 | * \brief define config type 68 | * 69 | * Define a Config type with the GpsToOdomConfig. All driver implementations 70 | * will then use the same variable type Config. 71 | */ 72 | typedef gps_to_odom::GpsToOdomConfig Config; 73 | 74 | /** 75 | * \brief config variable 76 | * 77 | * This variable has all the driver parameters defined in the cfg config file. 78 | * Is updated everytime function config_update() is called. 79 | */ 80 | Config config_; 81 | 82 | /** 83 | * \brief constructor 84 | * 85 | * In this constructor parameters related to the specific driver can be 86 | * initalized. Those parameters can be also set in the openDriver() function. 87 | * Attributes from the main node driver class IriBaseDriver such as loop_rate, 88 | * may be also overload here. 89 | */ 90 | GpsToOdomAlgorithm(void); 91 | 92 | /** 93 | * \brief Lock Algorithm 94 | * 95 | * Locks access to the Algorithm class 96 | */ 97 | void lock(void) { pthread_mutex_lock(&this->access_); }; 98 | 99 | /** 100 | * \brief Unlock Algorithm 101 | * 102 | * Unlocks access to the Algorithm class 103 | */ 104 | void unlock(void) { pthread_mutex_unlock(&this->access_); }; 105 | 106 | /** 107 | * \brief Tries Access to Algorithm 108 | * 109 | * Tries access to Algorithm 110 | * 111 | * \return true if the lock was adquired, false otherwise 112 | */ 113 | bool try_enter(void) 114 | { 115 | if(pthread_mutex_trylock(&this->access_)==0) 116 | return true; 117 | else 118 | return false; 119 | }; 120 | 121 | /** 122 | * \brief config update 123 | * 124 | * In this function the driver parameters must be updated with the input 125 | * config variable. Then the new configuration state will be stored in the 126 | * Config attribute. 127 | * 128 | * \param new_cfg the new driver configuration state 129 | * 130 | * \param level level in which the update is taken place 131 | */ 132 | void config_update(Config& config, uint32_t level=0); 133 | 134 | // here define all gps_to_odom_alg interface methods to retrieve and set 135 | // the driver parameters 136 | 137 | /** 138 | * \brief Destructor 139 | * 140 | * This destructor is called when the object is about to be destroyed. 141 | * 142 | */ 143 | ~GpsToOdomAlgorithm(void); 144 | }; 145 | 146 | #endif 147 | -------------------------------------------------------------------------------- /dualquat_LOAM/src/laserMappingNode.cpp: -------------------------------------------------------------------------------- 1 | // The mapping process is based on the FLOAM code. 2 | // Author of FLOAM: Wang Han 3 | // Email wh200720041@gmail.com 4 | // Author of LiDAR-Odometry_DQ: Edison Velasco 5 | // Email edison.velasco@ua.es 6 | 7 | //c++ lib 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | //ros lib 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | //pcl lib 23 | #include 24 | #include 25 | #include 26 | 27 | #include "laserMappingClass.h" 28 | 29 | 30 | LaserMappingClass laserMapping; 31 | std::mutex mutex_lock; 32 | std::queue odometryBuf; 33 | std::queue pointCloudBuf; 34 | float scan_period = 0.1; // time in seconds 35 | 36 | ros::Publisher map_pub; 37 | void odomCallback(const nav_msgs::Odometry::ConstPtr &msg) 38 | { 39 | mutex_lock.lock(); 40 | odometryBuf.push(msg); 41 | mutex_lock.unlock(); 42 | } 43 | 44 | void velodyneHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg) 45 | { 46 | mutex_lock.lock(); 47 | pointCloudBuf.push(laserCloudMsg); 48 | mutex_lock.unlock(); 49 | } 50 | 51 | 52 | void laser_mapping(){ 53 | while(1){ 54 | if(!odometryBuf.empty() && !pointCloudBuf.empty()){ 55 | 56 | //read data 57 | mutex_lock.lock(); 58 | if(!pointCloudBuf.empty() && pointCloudBuf.front()->header.stamp.toSec()header.stamp.toSec()-0.5*scan_period){ 59 | ROS_INFO("time stamp unaligned error and pointcloud discarded, pls check your data --> laser mapping node"); 60 | pointCloudBuf.pop(); 61 | mutex_lock.unlock(); 62 | continue; 63 | } 64 | 65 | if(!odometryBuf.empty() && odometryBuf.front()->header.stamp.toSec() < pointCloudBuf.front()->header.stamp.toSec()-0.5*scan_period){ 66 | odometryBuf.pop(); 67 | ROS_INFO("time stamp unaligned with path final, pls check your data --> laser mapping node"); 68 | mutex_lock.unlock(); 69 | continue; 70 | } 71 | 72 | //if time aligned 73 | pcl::PointCloud::Ptr pointcloud_in(new pcl::PointCloud()); 74 | pcl::fromROSMsg(*pointCloudBuf.front(), *pointcloud_in); 75 | ros::Time pointcloud_time = (pointCloudBuf.front())->header.stamp; 76 | 77 | Eigen::Isometry3d current_pose = Eigen::Isometry3d::Identity(); 78 | current_pose.rotate(Eigen::Quaterniond(odometryBuf.front()->pose.pose.orientation.w,odometryBuf.front()->pose.pose.orientation.x,odometryBuf.front()->pose.pose.orientation.y,odometryBuf.front()->pose.pose.orientation.z)); 79 | current_pose.pretranslate(Eigen::Vector3d(odometryBuf.front()->pose.pose.position.x,odometryBuf.front()->pose.pose.position.y,odometryBuf.front()->pose.pose.position.z)); 80 | pointCloudBuf.pop(); 81 | odometryBuf.pop(); 82 | mutex_lock.unlock(); 83 | 84 | laserMapping.updateCurrentPointsToMap(pointcloud_in,current_pose); 85 | 86 | pcl::PointCloud::Ptr pc_map = laserMapping.getMap(); 87 | sensor_msgs::PointCloud2 PointsMsg; 88 | pcl::toROSMsg(*pc_map, PointsMsg); 89 | PointsMsg.header.stamp = pointcloud_time; 90 | PointsMsg.header.frame_id = "map"; 91 | map_pub.publish(PointsMsg); 92 | 93 | } 94 | //sleep 2 ms every time 95 | std::chrono::milliseconds dura(2); 96 | std::this_thread::sleep_for(dura); 97 | } 98 | } 99 | 100 | int main(int argc, char **argv) 101 | { 102 | ros::init(argc, argv, "main"); 103 | ros::NodeHandle nh; 104 | 105 | double scan_period= 0.5; 106 | double max_dis = 100.0; 107 | double min_dis = 2.0; 108 | double map_resolution = 0.2; 109 | 110 | nh.getParam("/scan_period", scan_period); 111 | nh.getParam("/max_dis", max_dis); 112 | nh.getParam("/min_dis", min_dis); 113 | nh.getParam("/map_resolution", map_resolution); 114 | 115 | laserMapping.init(map_resolution); 116 | ros::Subscriber subLaserCloud = nh.subscribe("/ouster/points", 100, velodyneHandler); 117 | ros::Subscriber subOdometry = nh.subscribe("/odom_dq", 100, odomCallback); 118 | 119 | map_pub = nh.advertise("/map", 100); 120 | std::thread laser_mapping_process{laser_mapping}; 121 | 122 | ros::spin(); 123 | 124 | return 0; 125 | } 126 | -------------------------------------------------------------------------------- /dualquat_LOAM/include/dualquat/dualquat_base.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file dualquat/dualquat_base.h 3 | */ 4 | #pragma once 5 | 6 | #include 7 | #include 8 | 9 | namespace dualquat 10 | { 11 | 12 | template 13 | class DualQuaternion; 14 | 15 | using DualQuaternionf = DualQuaternion; 16 | using DualQuaterniond = DualQuaternion; 17 | 18 | template 19 | using Vector3 = Eigen::Matrix; 20 | 21 | template 22 | using Quaternion = Eigen::Quaternion; 23 | 24 | template 25 | class DualQuaternion 26 | { 27 | static_assert(std::is_floating_point::value, 28 | "Template parameter T must be floating_point type."); 29 | public: 30 | using value_type = T; 31 | 32 | /* Constructors */ 33 | DualQuaternion() 34 | {} 35 | 36 | DualQuaternion( 37 | const Quaternion& real, 38 | const Quaternion& dual) 39 | : real_(real), 40 | dual_(dual) 41 | {} 42 | 43 | /** 44 | * Creates a new dual quaternion with the given vector. 45 | */ 46 | explicit DualQuaternion(const Vector3& v) 47 | : DualQuaternion( 48 | Quaternion::Identity(), 49 | Quaternion(T(0), v.x(), v.y(), v.z())) 50 | {} 51 | 52 | /** 53 | * Creates a new dual quaternion with the given line (in Plucker coordinates). 54 | */ 55 | DualQuaternion(const Vector3& l, const Vector3& m) 56 | : DualQuaternion( 57 | Quaternion(T(0), l.x(), l.y(), l.z()), 58 | Quaternion(T(0), m.x(), m.y(), m.z())) 59 | {} 60 | 61 | /* Accessors */ 62 | const Quaternion& real() const noexcept { return real_; } 63 | const Quaternion& dual() const noexcept { return dual_; } 64 | 65 | Quaternion& real() noexcept { return real_; } 66 | Quaternion& dual() noexcept { return dual_; } 67 | 68 | /* Assignment operators */ 69 | DualQuaternion& operator += (const DualQuaternion&); 70 | DualQuaternion& operator -= (const DualQuaternion&); 71 | DualQuaternion& operator *= (const DualQuaternion&); 72 | DualQuaternion& operator *= (T); 73 | 74 | private: 75 | static constexpr bool needs_to_align = (sizeof(Quaternion) % 16) == 0; 76 | 77 | public: 78 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(needs_to_align) 79 | 80 | private: 81 | Quaternion real_; 82 | Quaternion dual_; 83 | }; 84 | 85 | /* Assignment operators */ 86 | 87 | template 88 | DualQuaternion& 89 | DualQuaternion::operator += (const DualQuaternion& rhs) 90 | { 91 | real_ = real_.coeffs() + rhs.real().coeffs(); 92 | dual_ = dual_.coeffs() + rhs.dual().coeffs(); 93 | return *this; 94 | } 95 | 96 | template 97 | DualQuaternion& 98 | DualQuaternion::operator -= (const DualQuaternion& rhs) 99 | { 100 | real_ = real_.coeffs() - rhs.real().coeffs(); 101 | dual_ = dual_.coeffs() - rhs.dual().coeffs(); 102 | return *this; 103 | } 104 | 105 | template 106 | DualQuaternion& 107 | DualQuaternion::operator *= (const DualQuaternion& rhs) 108 | { 109 | auto temp = real_; 110 | real_ = temp * rhs.real(); 111 | dual_ = (temp * rhs.dual()).coeffs() + (dual_ * rhs.real()).coeffs(); 112 | return *this; 113 | } 114 | 115 | template 116 | DualQuaternion& 117 | DualQuaternion::operator *= (T rhs) 118 | { 119 | real_.coeffs() *= rhs; 120 | dual_.coeffs() *= rhs; 121 | return *this; 122 | } 123 | 124 | /* Unary operators */ 125 | 126 | template 127 | DualQuaternion 128 | operator + (const DualQuaternion& rhs) 129 | { 130 | return rhs; 131 | } 132 | 133 | template 134 | DualQuaternion 135 | operator - (const DualQuaternion& rhs) 136 | { 137 | DualQuaternion temp; 138 | temp.real() = -rhs.real().coeffs(); 139 | temp.dual() = -rhs.dual().coeffs(); 140 | return temp; 141 | } 142 | 143 | /* Binary operators */ 144 | 145 | template 146 | DualQuaternion 147 | operator + (const DualQuaternion& lhs, const DualQuaternion& rhs) 148 | { 149 | DualQuaternion temp(lhs); 150 | return temp += rhs; 151 | } 152 | 153 | template 154 | DualQuaternion 155 | operator - (const DualQuaternion& lhs, const DualQuaternion& rhs) 156 | { 157 | DualQuaternion temp(lhs); 158 | return temp -= rhs; 159 | } 160 | 161 | template 162 | DualQuaternion 163 | operator * (const DualQuaternion& lhs, const DualQuaternion& rhs) 164 | { 165 | DualQuaternion temp(lhs); 166 | return temp *= rhs; 167 | } 168 | 169 | template