├── .gitignore ├── .vscode ├── c_cpp_properties.json └── settings.json ├── LICENSE ├── README.md ├── asset └── overview.png ├── calibration_pattern_creator ├── marker_3x6.bmp ├── marker_3x6.pdf └── paint.m ├── circle_detector ├── CMakeLists.txt ├── include │ ├── circle_detector.h │ └── cylindroid_factor.h ├── launch │ └── circle_detector.launch ├── package.xml └── src │ └── circle_detector.cpp ├── circle_msgs ├── CMakeLists.txt ├── msg │ ├── circle.msg │ └── circleArray.msg └── package.xml ├── config └── setup.yaml ├── corner_detector ├── CMakeLists.txt ├── include │ └── corner_detector.h ├── launch │ └── corner_detector.launch ├── package.xml └── src │ └── corner_detector.cpp ├── corner_msgs ├── CMakeLists.txt ├── msg │ ├── corner.msg │ └── cornerArray.msg └── package.xml ├── estimator ├── CMakeLists.txt ├── include │ ├── calibration_board.h │ ├── camera.h │ ├── estimator_manager.h │ ├── factor │ │ ├── analytic_diff │ │ │ ├── image_feature_factor.h │ │ │ ├── rd_spline_view.h │ │ │ ├── so3_spline_view.h │ │ │ ├── split_spline_view.h │ │ │ └── trajectory_value_factor.h │ │ └── ceres_local_param.h │ ├── initialization.h │ ├── trajectory_estimator.h │ ├── trajectory_manager.h │ └── trajectory_viewer.h ├── launch │ └── estimator.launch ├── package.xml ├── rviz │ └── estimator.rviz └── src │ ├── estimator │ ├── estimator_manager.cpp │ ├── initialization.cpp │ ├── trajectory_estimator.cpp │ └── trajectory_manager.cpp │ ├── estimator_node.cpp │ ├── sophus_lib │ ├── average.hpp │ ├── common.hpp │ ├── geometry.hpp │ ├── interpolate.hpp │ ├── interpolate_details.hpp │ ├── num_diff.hpp │ ├── rotation_matrix.hpp │ ├── rxso2.hpp │ ├── rxso3.hpp │ ├── se2.hpp │ ├── se3.hpp │ ├── sim2.hpp │ ├── sim3.hpp │ ├── sim_details.hpp │ ├── so2.hpp │ ├── so3.hpp │ ├── types.hpp │ └── velocities.hpp │ ├── spline │ ├── assert.h │ ├── ceres_spline_helper.h │ ├── ceres_spline_helper_jet.h │ ├── rd_spline.h │ ├── se3_spline.h │ ├── so3_spline.h │ ├── spline_common.h │ ├── spline_segment.h │ ├── trajectory.cpp │ └── trajectory.h │ └── utils │ ├── eigen_utils.hpp │ ├── math_utils.h │ ├── mypcl_cloud_type.h │ ├── parameter_struct.h │ ├── sophus_utils.hpp │ └── yaml_utils.h ├── eventmap_generator ├── CMakeLists.txt ├── include │ └── generator.h ├── launch │ └── generator.launch ├── msg │ └── eventmap.msg ├── package.xml └── src │ ├── generator.cpp │ └── generator_node.cpp ├── record_bag ├── circle_info.bag └── corner_info.bag └── run.sh /.gitignore: -------------------------------------------------------------------------------- 1 | devel/ 2 | logs/ 3 | build/ 4 | bin/ 5 | lib/ 6 | msg_gen/ 7 | srv_gen/ 8 | msg/*Action.msg 9 | msg/*ActionFeedback.msg 10 | msg/*ActionGoal.msg 11 | msg/*ActionResult.msg 12 | msg/*Feedback.msg 13 | msg/*Goal.msg 14 | msg/*Result.msg 15 | msg/_*.py 16 | build_isolated/ 17 | devel_isolated/ 18 | 19 | # Generated by dynamic reconfigure 20 | *.cfgc 21 | /cfg/cpp/ 22 | /cfg/*.py 23 | 24 | # Ignore generated docs 25 | *.dox 26 | *.wikidoc 27 | 28 | # eclipse stuff 29 | .project 30 | .cproject 31 | 32 | # qcreator stuff 33 | CMakeLists.txt.user 34 | 35 | srv/_*.py 36 | *.pcd 37 | *.pyc 38 | qtcreator-* 39 | *.user 40 | 41 | /planning/cfg 42 | /planning/docs 43 | /planning/src 44 | 45 | *~ 46 | 47 | # Emacs 48 | .#* 49 | 50 | # Catkin custom files 51 | CATKIN_IGNORE 52 | record_bag/trial_A_3.bag 53 | .vscode/ -------------------------------------------------------------------------------- /.vscode/c_cpp_properties.json: -------------------------------------------------------------------------------- 1 | { 2 | "configurations": [ 3 | { 4 | "browse": { 5 | "databaseFilename": "${default}", 6 | "limitSymbolsToIncludedHeaders": false 7 | }, 8 | "includePath": [ 9 | "/data/catkin_calib/devel/include/**", 10 | "/opt/ros/melodic/include/**", 11 | "/data/catkin_calib/src/EF-Calib/circle_detector/include/**", 12 | "/data/catkin_calib/src/EF-Calib/corner_detector/include/**", 13 | "/data/catkin_calib/src/EF-Calib/rpg_dvs_ros/davis_ros_driver/include/**", 14 | "/data/catkin_calib/src/EF-Calib/rpg_dvs_ros/dvs_calibration/include/**", 15 | "/data/catkin_calib/src/EF-Calib/rpg_dvs_ros/dvs_renderer/include/**", 16 | "/data/catkin_calib/src/EF-Calib/rpg_dvs_ros/dvs_ros_driver/include/**", 17 | "/data/catkin_calib/src/EF-Calib/rpg_dvs_ros/dvxplorer_ros_driver/include/**", 18 | "/data/catkin_calib/src/EF-Calib/estimator/include/**", 19 | "/data/catkin_calib/src/EF-Calib/eventmap_generator/include/**", 20 | "/usr/include/**" 21 | ], 22 | "name": "ROS", 23 | "intelliSenseMode": "gcc-x64", 24 | "compilerPath": "/usr/bin/gcc", 25 | "cStandard": "gnu11", 26 | "cppStandard": "c++14" 27 | } 28 | ], 29 | "version": 4 30 | } -------------------------------------------------------------------------------- /.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "python.autoComplete.extraPaths": [ 3 | "/data/catkin_calib/devel/lib/python2.7/dist-packages", 4 | "/opt/ros/melodic/lib/python2.7/dist-packages" 5 | ], 6 | "python.analysis.extraPaths": [ 7 | "/data/catkin_calib/devel/lib/python2.7/dist-packages", 8 | "/opt/ros/melodic/lib/python2.7/dist-packages" 9 | ], 10 | "files.associations": { 11 | "ostream": "cpp", 12 | "cctype": "cpp", 13 | "clocale": "cpp", 14 | "cmath": "cpp", 15 | "csignal": "cpp", 16 | "cstdarg": "cpp", 17 | "cstddef": "cpp", 18 | "cstdio": "cpp", 19 | "cstdlib": "cpp", 20 | "cstring": "cpp", 21 | "ctime": "cpp", 22 | "cwchar": "cpp", 23 | "cwctype": "cpp", 24 | "array": "cpp", 25 | "atomic": "cpp", 26 | "strstream": "cpp", 27 | "bit": "cpp", 28 | "*.tcc": "cpp", 29 | "bitset": "cpp", 30 | "chrono": "cpp", 31 | "cinttypes": "cpp", 32 | "compare": "cpp", 33 | "complex": "cpp", 34 | "concepts": "cpp", 35 | "condition_variable": "cpp", 36 | "cstdint": "cpp", 37 | "deque": "cpp", 38 | "list": "cpp", 39 | "map": "cpp", 40 | "set": "cpp", 41 | "unordered_map": "cpp", 42 | "unordered_set": "cpp", 43 | "vector": "cpp", 44 | "exception": "cpp", 45 | "algorithm": "cpp", 46 | "functional": "cpp", 47 | "iterator": "cpp", 48 | "memory": "cpp", 49 | "memory_resource": "cpp", 50 | "numeric": "cpp", 51 | "optional": "cpp", 52 | "random": "cpp", 53 | "ratio": "cpp", 54 | "string": "cpp", 55 | "string_view": "cpp", 56 | "system_error": "cpp", 57 | "tuple": "cpp", 58 | "type_traits": "cpp", 59 | "utility": "cpp", 60 | "hash_map": "cpp", 61 | "hash_set": "cpp", 62 | "fstream": "cpp", 63 | "initializer_list": "cpp", 64 | "iomanip": "cpp", 65 | "iosfwd": "cpp", 66 | "iostream": "cpp", 67 | "istream": "cpp", 68 | "limits": "cpp", 69 | "mutex": "cpp", 70 | "new": "cpp", 71 | "ranges": "cpp", 72 | "sstream": "cpp", 73 | "stdexcept": "cpp", 74 | "stop_token": "cpp", 75 | "streambuf": "cpp", 76 | "thread": "cpp", 77 | "cfenv": "cpp", 78 | "typeindex": "cpp", 79 | "typeinfo": "cpp", 80 | "valarray": "cpp" 81 | } 82 | } -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2024 Shaoan Wang 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # EF-Calib: Spatiotemporal Calibration of Event- and Frame-Based Cameras Using Continuous-Time Trajectories 2 | 3 |
4 | overview image 5 |
6 | 7 | # Installation 8 | 9 | We have tested EF-Calib on machines with the following configurations: 10 | * Ubuntu 20.04 LTS 11 | * ROS Noetic 12 | * OpenCV 4+ 13 | * Ceres 1.13.0 14 | 15 | ## Driver Installation 16 | 17 | To work with event cameras, especially for the Dynamic Vision Sensors (DVS/DAVIS), you need to install some drivers. Please follow the instructions at [rpg_dvs_ros](https://github.com/uzh-rpg/rpg_dvs_ros) before moving on to the next step. Note that you need to replace the name of the ROS distribution with the one installed on your computer. 18 | 19 | We use catkin tools to build the code. You should have it installed during the driver installation. 20 | 21 | ## Dependencies Installation 22 | 23 | ### Ceres-solver 24 | 25 | Please follow this [link](http://ceres-solver.org/installation.html) to install Ceres-solver. Note that versions above 2.0.0 might encounter issues due to some recent changes. 26 | 27 | ### OpenCV 4 28 | 29 | Please follow this [link](https://opencv.org/get-started/) to install OpenCV 4. 30 | 31 | ## EF-Calib Installation 32 | 33 | You should have created a catkin workspace in Driver Installation. If not, please go back and create one. 34 | 35 | Clone this repository into the `src` folder of your catkin workspace. 36 | 37 | $ cd ~/catkin_ws/src 38 | $ git clone https://github.com/wsakobe/EF-Calib.git 39 | 40 | Build all the packages of EF-Calib. 41 | 42 | $ catkin build 43 | 44 | 45 | # Usage 46 | 47 | First, record the image data and event streams synchronized according to the method described in the paper. Alternatively, you can download an example rosbag from this [link](https://drive.google.com/file/d/1UXtpTBYqeFjvbvVHNQ-meKYFzR9RP7e2/view?usp=sharing) 48 | 49 | Configure parameters in the `config/setup.yaml` file. 50 | 51 | - `BagName`: File path of the recorded rosbag 52 | - `square_size`: Adjacent feature spacing on the calibration board 53 | - `init_window_size`: Minimum number of frames required for initialization 54 | - `board_width`: Calibration board size (number of features in the width direction) 55 | - `board_height`: Calibration board size (number of features in the height direction) 56 | - `knot_distance`: B-spline knot spacing 57 | - `continue_number`: Minimum number of consecutive B-spline knots 58 | - `opt_time`: Total duration of the rosbag 59 | 60 | Next, run the following commands to extract features from frames and events into `corner_info.bag` and `circle_info.bag`, respectively. 61 | 62 | $ source ../devel/setup.bash 63 | $ cd EF-Calib 64 | $ ./run.sh 65 | 66 | Once valid `corner_info.bag` and `circle_info.bag` files are obtained, run the estimator node to complete the calibration process. If successful, the calibration results will be displayed in the Terminal. 67 | 68 | $ roslaunch estimator estimator.launch 69 | 70 | # Credits 71 | 72 | This code was developed by [Shaoan Wang](https://wsakobe.github.io/) from Peking University. 73 | 74 | For researchers that have leveraged or compared to this work, please cite the following: 75 | 76 | ```latex 77 | @ARTICLE{10705060, 78 | author={Wang, Shaoan and Xin, Zhanhua and Hu, Yaoqing and Li, Dongyue and Zhu, Mingzhu and Yu, Junzhi}, 79 | journal={IEEE Robotics and Automation Letters}, 80 | title={EF-Calib: Spatiotemporal Calibration of Event- and Frame-Based Cameras Using Continuous-Time Trajectories}, 81 | year={2024}, 82 | volume={9}, 83 | number={11}, 84 | pages={10280-10287}, 85 | keywords={Cameras;Calibration;Trajectory;Splines (mathematics);Robot vision systems;Accuracy;Lighting;Spatiotemporal phenomena;Synchronization;Pattern recognition;Calibration and identification;sensor fusion;event camera;spatiotemporal calibration}, 86 | doi={10.1109/LRA.2024.3474475} 87 | } 88 | ``` 89 | 90 | # Acknowledgement 91 | 92 | The B-spline module is adapted from [basalt](https://gitlab.com/VladyslavUsenko/basalt-headers). Thanks for the excellent job! 93 | -------------------------------------------------------------------------------- /asset/overview.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wsakobe/EF-Calib/0e218c9640a237769aebffc63c615c44b1c2e4e0/asset/overview.png -------------------------------------------------------------------------------- /calibration_pattern_creator/marker_3x6.bmp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wsakobe/EF-Calib/0e218c9640a237769aebffc63c615c44b1c2e4e0/calibration_pattern_creator/marker_3x6.bmp -------------------------------------------------------------------------------- /calibration_pattern_creator/marker_3x6.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wsakobe/EF-Calib/0e218c9640a237769aebffc63c615c44b1c2e4e0/calibration_pattern_creator/marker_3x6.pdf -------------------------------------------------------------------------------- /calibration_pattern_creator/paint.m: -------------------------------------------------------------------------------- 1 | marker_size = [3, 6]; % [row, col] 2 | desired_mm = 500; % distance between two crosspoints 3 | 4 | [temp_1, temp_2] = generateCellTemp(); 5 | 6 | marker_row1 = []; 7 | marker_row2 = []; 8 | for i=1:marker_size(2) 9 | if mod(i,2) 10 | marker_row1 = [marker_row1 temp_2]; 11 | marker_row2 = [marker_row2 temp_1]; 12 | else 13 | marker_row1 = [marker_row1 temp_1]; 14 | marker_row2 = [marker_row2 temp_2]; 15 | end 16 | end 17 | marker_img=[]; 18 | for i=1:marker_size(1) 19 | if mod(i,2) 20 | marker_img = [marker_img; marker_row1]; 21 | else 22 | marker_img = [marker_img; marker_row2]; 23 | end 24 | end 25 | 26 | desired_width_inches = desired_mm / 25.4; 27 | desired_height_inches = desired_mm / 25.4; 28 | 29 | imshow(marker_img); 30 | imwrite(marker_img,['marker_', num2str(marker_size(1)), 'x', num2str(marker_size(2)), '.bmp']); 31 | 32 | screen_dpi = 96; 33 | desired_width_pixels = desired_width_inches * screen_dpi; 34 | desired_height_pixels = desired_height_inches * screen_dpi; 35 | set(gcf, 'Position', [100, 100, desired_width_pixels, desired_height_pixels]); 36 | set(gcf, 'PaperUnits', 'inches'); 37 | set(gcf, 'PaperSize', [desired_width_inches, desired_height_inches]); 38 | exportgraphics(gcf,['marker_', num2str(marker_size(1)), 'x', num2str(marker_size(2)), '.pdf'],'ContentType','vector'); 39 | 40 | function [temp, temp_inv] = generateCellTemp() 41 | temp = ones(900,900); 42 | 43 | center_x = 450; 44 | center_y = 450; 45 | theta_circ = linspace(0, pi*2, 500); 46 | r_circ = ones(size(theta_circ)) * 300; 47 | x = r_circ .* cos(theta_circ) + center_x; 48 | y = r_circ .* sin(theta_circ) + center_y; 49 | temp = insertShape(temp,'FilledPolygon',[[center_x;x';center_x],[center_y;y';center_y]],'Color','black','Opacity',1,'SmoothEdges', false); 50 | 51 | % 定义扇形的参数 52 | theta = linspace(0, pi/2, 500); % 扇形的角度范围 53 | r = ones(size(theta)) * 150; % 扇形的半径 54 | x = r .* cos(theta) + center_x + 1; % 扇形在x轴的坐标 55 | y = r .* sin(theta) + center_y + 1; % 扇形在y轴的坐标 56 | temp = insertShape(temp,'FilledPolygon',[[center_x+1;x';center_x+1],[center_y+1;y';center_y+1]],'Color','white','Opacity',1,'SmoothEdges', false); 57 | 58 | theta = linspace(-pi, -pi/2, 500); % 扇形的角度范围 59 | r = ones(size(theta)) * 150; % 扇形的半径 60 | x = r .* cos(theta) + center_x; % 扇形在x轴的坐标 61 | y = r .* sin(theta) + center_y; % 扇形在y轴的坐标 62 | temp = insertShape(temp,'FilledPolygon',[[center_x;x';center_x],[center_y;y';center_y]],'Color','white','Opacity',1,'SmoothEdges', false); 63 | 64 | temp_inv = imrotate(temp, 90); 65 | end 66 | -------------------------------------------------------------------------------- /circle_detector/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(circle_detector) 3 | 4 | set(CMAKE_CXX_STANDARD 17) 5 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -g -pthread") 6 | 7 | find_package(catkin REQUIRED COMPONENTS 8 | circle_msgs 9 | cv_bridge 10 | image_transport 11 | roscpp 12 | rosbag 13 | std_msgs 14 | dvs_msgs 15 | eventmap_generator 16 | ) 17 | 18 | find_package(OpenCV REQUIRED) 19 | find_package(Ceres REQUIRED) 20 | 21 | ################################### 22 | ## catkin specific configuration ## 23 | ################################### 24 | ## The catkin_package macro generates cmake config files for your package 25 | ## Declare things to be passed to dependent projects 26 | ## INCLUDE_DIRS: uncomment this if your package contains header files 27 | ## LIBRARIES: libraries you create in this project that dependent projects also need 28 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 29 | ## DEPENDS: system dependencies of this project that dependent projects also need 30 | catkin_package( 31 | # INCLUDE_DIRS include 32 | # LIBRARIES circle_detector 33 | # CATKIN_DEPENDS circle_msgs cv_bridge image_transport opencv3 roscpp std_msgs 34 | # DEPENDS system_lib 35 | ) 36 | 37 | include_directories( 38 | include 39 | ${catkin_INCLUDE_DIRS} 40 | ${OpenCV_INCLUDE_DIRS} 41 | ${CERES_INCLUDE_DIRS} 42 | ) 43 | 44 | add_executable(circle_detector 45 | src/circle_detector.cpp) 46 | 47 | target_link_libraries(circle_detector 48 | ${catkin_LIBRARIES} 49 | ${OpenCV_LIBRARIES} 50 | ${CERES_LIBRARIES} 51 | -lglog 52 | ) 53 | 54 | add_dependencies(circle_detector circle_msgs_generate_messages_cpp) 55 | 56 | -------------------------------------------------------------------------------- /circle_detector/include/cylindroid_factor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "ceres/ceres.h" 4 | #include "glog/logging.h" 5 | #include 6 | #include 7 | #include 8 | 9 | namespace circle_detector{ 10 | 11 | struct CylindroidResidual{ 12 | 13 | CylindroidResidual(cv::Point3i point, int time) 14 | : point_(point), time_(time) {} 15 | 16 | template 17 | bool operator()(const T* const coeffs, T* residual) const { 18 | T x_rot = ceres::cos(coeffs[2] / 180.0 * 3.14) * ((T)point_.x - (coeffs[0] + coeffs[5] * (T)(point_.z - time_))) + ceres::sin(coeffs[2] / 180.0 * 3.14) * ((T)point_.y - (coeffs[1] + coeffs[6] * (T)(point_.z - time_))); 19 | T y_rot = ceres::sin(coeffs[2] / 180.0 * 3.14) * ((T)point_.x - (coeffs[0] + coeffs[5] * (T)(point_.z - time_))) - ceres::cos(coeffs[2] / 180.0 * 3.14) * ((T)point_.y - (coeffs[1] + coeffs[6] * (T)(point_.z - time_))); 20 | 21 | residual[0] = ceres::sqrt((x_rot * x_rot * 4.0) / (coeffs[3] * coeffs[3]) + (y_rot * y_rot * 4.0) / (coeffs[4] * coeffs[4])) - T(1); 22 | 23 | return true; 24 | } 25 | 26 | private: 27 | const cv::Point3i point_; 28 | const int time_; 29 | }; 30 | 31 | } // namespace circle_detector -------------------------------------------------------------------------------- /circle_detector/launch/circle_detector.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /circle_detector/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | circle_detector 4 | 0.0.0 5 | The circle_detector package 6 | 7 | 8 | 9 | 10 | root 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | circle_msgs 53 | cv_bridge 54 | image_transport 55 | opencv3 56 | roscpp 57 | rosbag 58 | std_msgs 59 | dvs_msgs 60 | eventmap_generator 61 | circle_msgs 62 | cv_bridge 63 | image_transport 64 | opencv3 65 | roscpp 66 | rosbag 67 | std_msgs 68 | dvs_msgs 69 | circle_msgs 70 | cv_bridge 71 | image_transport 72 | opencv3 73 | roscpp 74 | rosbag 75 | std_msgs 76 | dvs_msgs 77 | eventmap_generator 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | -------------------------------------------------------------------------------- /circle_detector/src/circle_detector.cpp: -------------------------------------------------------------------------------- 1 | #include "circle_detector.h" 2 | 3 | using namespace circle_detector; 4 | 5 | int main(int argc, char** argv) { 6 | google::InitGoogleLogging(argv[0]); 7 | 8 | ros::init(argc, argv, "circle_detector"); 9 | ros::NodeHandle nh; 10 | 11 | circle_detector::CircleDetector cd(nh); 12 | 13 | ros::spin(); 14 | 15 | ros::shutdown(); 16 | 17 | return 0; 18 | } 19 | -------------------------------------------------------------------------------- /circle_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(circle_msgs) 3 | 4 | # search for all msg files 5 | FILE(GLOB messages_to_build RELATIVE "${CMAKE_CURRENT_SOURCE_DIR}/msg" 6 | "${CMAKE_CURRENT_SOURCE_DIR}/msg/*.msg") 7 | 8 | find_package(catkin REQUIRED COMPONENTS 9 | roscpp 10 | std_msgs 11 | message_generation 12 | message_runtime 13 | ) 14 | 15 | add_message_files( 16 | FILES 17 | ${messages_to_build} 18 | ) 19 | 20 | generate_messages( 21 | DEPENDENCIES 22 | std_msgs 23 | ) 24 | 25 | catkin_package( 26 | # INCLUDE_DIRS include 27 | # LIBRARIES corner_detector 28 | # CATKIN_DEPENDS message_generation message_runtime cv_bridge image_transport opencv3 roscpp sensor_msgs std_msgs 29 | # DEPENDS system_lib 30 | CATKIN_DEPENDS message_generation message_runtime roscpp std_msgs 31 | ) 32 | 33 | include_directories( 34 | include 35 | ${catkin_INCLUDE_DIRS} 36 | ) 37 | -------------------------------------------------------------------------------- /circle_msgs/msg/circle.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float32 x 3 | float32 y 4 | int32 x_grid 5 | int32 y_grid 6 | float32 velo_x 7 | float32 velo_y 8 | time timestamp 9 | time start_ts 10 | time end_ts -------------------------------------------------------------------------------- /circle_msgs/msg/circleArray.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | circle[] circles -------------------------------------------------------------------------------- /circle_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | circle_msgs 4 | 1.0.0 5 | The circle_msgs package 6 | 7 | 8 | 9 | 10 | root 11 | 12 | 13 | 14 | 15 | 16 | GPLv3 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 | roscpp 53 | std_msgs 54 | roscpp 55 | std_msgs 56 | roscpp 57 | std_msgs 58 | message_generation 59 | message_generation 60 | message_runtime 61 | message_runtime 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | -------------------------------------------------------------------------------- /config/setup.yaml: -------------------------------------------------------------------------------- 1 | ## Board size 2 | BoardWidth: 6 3 | BoardHeight: 3 4 | square_size: 40 # Distance between two neighboring crosspoints (unit:mm) 5 | 6 | ## Bag config 7 | BagName: "/data/catkin_calib/src/EF-Calib/record_bag/trial_A_3.bag" 8 | StorePath: "/data/catkin_calib/src/EF-Calib/record_bag/" 9 | cornerBagPath: "/data/catkin_calib/src/EF-Calib/record_bag/corner_info.bag" 10 | circleBagPath: "/data/catkin_calib/src/EF-Calib/record_bag/circle_info.bag" 11 | 12 | ## Estimator config 13 | log_path: "/data/catkin_calib/logs" 14 | init_window_size: 8 15 | 16 | conv_cam_width: 1280 17 | conv_cam_height: 1024 18 | ev_cam_width: 346 19 | ev_cam_height: 260 20 | 21 | board_width: 6 22 | board_height: 3 23 | 24 | knot_distance: 0.035 25 | continue_number: 15 26 | 27 | opt_time: 40 -------------------------------------------------------------------------------- /corner_detector/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(corner_detector) 3 | 4 | # search for all msg files 5 | FILE(GLOB messages_to_build RELATIVE "${CMAKE_CURRENT_SOURCE_DIR}/msg" 6 | "${CMAKE_CURRENT_SOURCE_DIR}/msg/*.msg") 7 | 8 | find_package(OpenCV 3 REQUIRED) 9 | find_package(catkin REQUIRED COMPONENTS 10 | cv_bridge 11 | image_transport 12 | roscpp 13 | rosbag 14 | sensor_msgs 15 | std_msgs 16 | corner_msgs 17 | ) 18 | 19 | catkin_package( 20 | # INCLUDE_DIRS include 21 | # LIBRARIES corner_detector 22 | # CATKIN_DEPENDS message_generation message_runtime cv_bridge image_transport opencv3 roscpp sensor_msgs std_msgs 23 | # DEPENDS system_lib 24 | ) 25 | 26 | include_directories( 27 | include 28 | ${catkin_INCLUDE_DIRS} 29 | ${OpenCV_INCLUDE_DIRS} 30 | ) 31 | 32 | add_executable(corner_detector 33 | src/corner_detector.cpp) 34 | 35 | target_link_libraries(corner_detector 36 | ${catkin_LIBRARIES} 37 | ${OpenCV_LIBRARIES} 38 | ) 39 | 40 | add_dependencies(corner_detector corner_msgs_generate_messages_cpp) 41 | -------------------------------------------------------------------------------- /corner_detector/launch/corner_detector.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /corner_detector/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | corner_detector 4 | 1.0.0 5 | The corner_detector package 6 | 7 | 8 | 9 | 10 | root 11 | 12 | 13 | 14 | 15 | 16 | GPLv3 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 | cv_bridge 53 | image_transport 54 | opencv3 55 | roscpp 56 | rosbag 57 | sensor_msgs 58 | std_msgs 59 | cv_bridge 60 | image_transport 61 | opencv3 62 | roscpp 63 | rosbag 64 | sensor_msgs 65 | std_msgs 66 | cv_bridge 67 | image_transport 68 | opencv3 69 | roscpp 70 | rosbag 71 | sensor_msgs 72 | std_msgs 73 | corner_msgs 74 | corner_msgs 75 | corner_msgs 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | -------------------------------------------------------------------------------- /corner_detector/src/corner_detector.cpp: -------------------------------------------------------------------------------- 1 | #include "corner_detector.h" 2 | 3 | int main(int argc, char** argv) { 4 | ros::init(argc, argv, "image_processor"); 5 | ros::NodeHandle nh; 6 | corner_detector::ImageProcessor* ip = new corner_detector::ImageProcessor(nh); 7 | 8 | ros::spin(); 9 | 10 | return 0; 11 | } 12 | -------------------------------------------------------------------------------- /corner_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(corner_msgs) 3 | 4 | # search for all msg files 5 | FILE(GLOB messages_to_build RELATIVE "${CMAKE_CURRENT_SOURCE_DIR}/msg" 6 | "${CMAKE_CURRENT_SOURCE_DIR}/msg/*.msg") 7 | 8 | find_package(catkin REQUIRED COMPONENTS 9 | roscpp 10 | std_msgs 11 | message_generation 12 | message_runtime 13 | ) 14 | 15 | add_message_files( 16 | FILES 17 | ${messages_to_build} 18 | ) 19 | 20 | generate_messages( 21 | DEPENDENCIES 22 | std_msgs 23 | ) 24 | 25 | catkin_package( 26 | # INCLUDE_DIRS include 27 | # LIBRARIES corner_detector 28 | # CATKIN_DEPENDS message_generation message_runtime cv_bridge image_transport opencv3 roscpp sensor_msgs std_msgs 29 | # DEPENDS system_lib 30 | CATKIN_DEPENDS message_generation message_runtime roscpp std_msgs 31 | ) 32 | 33 | include_directories( 34 | include 35 | ${catkin_INCLUDE_DIRS} 36 | ) 37 | -------------------------------------------------------------------------------- /corner_msgs/msg/corner.msg: -------------------------------------------------------------------------------- 1 | float32 x 2 | float32 y 3 | int32 x_grid 4 | int32 y_grid -------------------------------------------------------------------------------- /corner_msgs/msg/cornerArray.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | corner[] corners 3 | time timestamp -------------------------------------------------------------------------------- /corner_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | corner_msgs 4 | 1.0.0 5 | The corner_msgs package 6 | 7 | 8 | 9 | 10 | root 11 | 12 | 13 | 14 | 15 | 16 | GPLv3 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 | roscpp 53 | std_msgs 54 | roscpp 55 | std_msgs 56 | roscpp 57 | std_msgs 58 | message_generation 59 | message_generation 60 | message_runtime 61 | message_runtime 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | -------------------------------------------------------------------------------- /estimator/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(estimator) 3 | 4 | set(CMAKE_CXX_STANDARD 17) 5 | set(CMAKE_CXX_FLAGS_RELEASE "-O3") 6 | 7 | find_package(catkin REQUIRED COMPONENTS 8 | roscpp 9 | rosbag 10 | circle_msgs 11 | corner_msgs 12 | std_msgs 13 | geometry_msgs 14 | nav_msgs 15 | pcl_conversions 16 | pcl_ros 17 | visualization_msgs 18 | eigen_conversions 19 | ) 20 | 21 | find_package(Eigen3 REQUIRED) 22 | find_package(Ceres REQUIRED) 23 | # find_package(Sophus REQUIRED) 24 | find_package(OpenCV 3 REQUIRED) 25 | 26 | #find_package(yaml-cpp REQUIRED) 27 | find_package(PkgConfig REQUIRED) 28 | pkg_check_modules(YAML_CPP REQUIRED yaml-cpp>=0.5) 29 | 30 | catkin_package( 31 | #INCLUDE_DIRS src 32 | ) 33 | 34 | ########### 35 | ## Build ## 36 | ########### 37 | include_directories( 38 | include 39 | src 40 | ${catkin_INCLUDE_DIRS} 41 | ${YAML_INCLUDE_DIRS} 42 | ${CERES_INCLUDE_DIRS} 43 | # ${Sophus_INCLUDE_DIRS} 44 | ${EIGEN3_INCLUDE_DIR} 45 | ) 46 | 47 | list(APPEND thirdparty_libraries 48 | ${YAML_CPP_LIBRARIES} 49 | ${Boost_LIBRARIES} 50 | ${catkin_LIBRARIES} 51 | ${OpenCV_LIBS} 52 | ${PCL_LIBRARIES} 53 | ${CERES_LIBRARIES} 54 | ) 55 | 56 | ###################### 57 | ### Estimator 58 | ###################### 59 | 60 | add_library(spline_lib src/spline/trajectory.cpp) 61 | target_link_libraries(spline_lib ${thirdparty_libraries}) 62 | 63 | add_executable(estimator 64 | src/estimator_node.cpp 65 | src/estimator/estimator_manager.cpp 66 | src/estimator/initialization.cpp 67 | src/estimator/trajectory_manager.cpp 68 | src/estimator/trajectory_estimator.cpp 69 | ) 70 | target_link_libraries(estimator 71 | spline_lib 72 | ${thirdparty_libraries} 73 | ) -------------------------------------------------------------------------------- /estimator/include/calibration_board.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "ros/ros.h" 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | namespace estimator{ 11 | class CalibBoard{ 12 | public: 13 | typedef std::shared_ptr Ptr; 14 | 15 | CalibBoard(cv::Size board_size, double squarelen) 16 | : board_size_(board_size), square_len(squarelen) 17 | { 18 | } 19 | 20 | Eigen::Vector3d getBoardPointInWorld(int x, int y){ 21 | Eigen::Vector3d point_world; 22 | point_world = Eigen::Vector3d(x * square_len, y * square_len, 0); 23 | 24 | return point_world; 25 | } 26 | 27 | double square_len; 28 | 29 | private: 30 | cv::Size board_size_; 31 | 32 | }; 33 | } //namespace estimator 34 | -------------------------------------------------------------------------------- /estimator/include/camera.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "ros/ros.h" 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | namespace estimator{ 11 | 12 | class Camera{ 13 | public: 14 | typedef std::shared_ptr Ptr; 15 | Camera(cv::Size image_size) 16 | : image_size_(image_size) 17 | { 18 | image_width_ = image_size_.width; 19 | image_height_ = image_size_.height; 20 | } 21 | 22 | double intrinsicParams[4]; //fx_, fy_, cx_, cy_ 23 | double distortParams[5]; //k1_, k2_, d1_, d2_, k3_ 24 | 25 | int image_width_, image_height_; 26 | cv::Size image_size_; 27 | 28 | Sophus::SO3 R_C2W; 29 | Eigen::Matrix t_C2W; 30 | 31 | double time_delay = 0; 32 | 33 | cv::Mat getIntrinsicMatrixOC(){ 34 | cv::Mat K = (cv::Mat_(3, 3) << intrinsicParams[0], 0, intrinsicParams[2], 0, intrinsicParams[1], intrinsicParams[3], 0, 0, 1); 35 | return K; 36 | } 37 | 38 | cv::Mat getDistortionMatrixOC(){ 39 | cv::Mat K = (cv::Mat_(5, 1) << distortParams[0], distortParams[1], distortParams[2], distortParams[3], distortParams[4]); 40 | return K; 41 | } 42 | 43 | Eigen::Matrix getIntrinsicMatrix(){ 44 | Eigen::Matrix K; 45 | K(0, 0) = intrinsicParams[0]; 46 | K(1, 1) = intrinsicParams[1]; 47 | K(0, 2) = intrinsicParams[2]; 48 | K(1, 2) = intrinsicParams[3]; 49 | K(2, 2) = 1; 50 | 51 | return K; 52 | } 53 | 54 | Eigen::Matrix getDistortionMatrix(){ 55 | Eigen::Matrix K; 56 | K(0, 0) = distortParams[0]; 57 | K(1, 0) = distortParams[1]; 58 | K(2, 0) = distortParams[2]; 59 | K(3, 0) = distortParams[3]; 60 | K(4, 0) = distortParams[4]; 61 | return K; 62 | } 63 | 64 | Eigen::Matrix getExtrinsicMatrix(){ 65 | Eigen::Matrix T; 66 | T.block<3, 3>(0, 0) = R_C2W.matrix(); 67 | T.block<3, 1>(0, 3) = t_C2W; 68 | T.block<1, 4>(3, 0) << 0, 0, 0, 1; 69 | return T; 70 | } 71 | 72 | void updateIntrinsic(cv::Mat& intrinsic, cv::Mat& distortion){ 73 | intrinsicParams[0] = intrinsic.at(0, 0); 74 | intrinsicParams[1] = intrinsic.at(1, 1); 75 | intrinsicParams[2] = intrinsic.at(0, 2); 76 | intrinsicParams[3] = intrinsic.at(1, 2); 77 | 78 | distortParams[0] = distortion.at(0, 0); 79 | distortParams[1] = distortion.at(0, 1); 80 | distortParams[2] = distortion.at(0, 2); 81 | distortParams[3] = distortion.at(0, 3); 82 | distortParams[4] = distortion.at(0, 4); 83 | } 84 | 85 | void updateExtrinsic(cv::Mat& Transformation){ 86 | Eigen::Matrix4d eigenT; 87 | cv::cv2eigen(Transformation, eigenT); 88 | SE3d se3(eigenT); 89 | R_C2W = se3.rotationMatrix(); 90 | t_C2W = se3.translation(); 91 | } 92 | 93 | Eigen::Vector2d projectIntoImage(Eigen::Vector3d input_point){ 94 | Eigen::Vector2d normalized_input_point(input_point.x() / input_point.z(), input_point.y() / input_point.z()); 95 | 96 | // Apply distortion correction 97 | double r2 = normalized_input_point.squaredNorm(); 98 | double radial_distortion = 1.0 + distortParams[0] * r2 + distortParams[1] * r2 * r2 + distortParams[4] * r2 * r2 * r2; 99 | 100 | double x_corr = normalized_input_point.x() * radial_distortion + 101 | 2.0 * distortParams[2] * normalized_input_point.x() * normalized_input_point.y() + 102 | distortParams[3] * (r2 + 2.0 * normalized_input_point.x() * normalized_input_point.x()); 103 | double y_corr = normalized_input_point.y() * radial_distortion + 104 | 2.0 * distortParams[3] * normalized_input_point.x() * normalized_input_point.y() + 105 | distortParams[2] * (r2 + 2.0 * normalized_input_point.y() * normalized_input_point.y()); 106 | 107 | // Construct undistorted coordinates 108 | Eigen::Vector3d undistorted_point(x_corr, y_corr, 1); 109 | //Eigen::Vector3d undistorted_point1(normalized_input_point.x(), normalized_input_point.y(), 1); 110 | 111 | Eigen::Vector3d projected_point; 112 | projected_point = getIntrinsicMatrix() * undistorted_point; 113 | 114 | // Normalize the projected point 115 | Eigen::Vector2d normalized_point(projected_point.x() / projected_point.z(), projected_point.y() / projected_point.z()); 116 | 117 | //std::cout << "input:\n" << input_point << "\nproject:\n" << undistorted_point << "\nnormalized:\n" << normalized_point << "x:" << x_corr << " y:" << y_corr << std::endl; 118 | 119 | return normalized_point; 120 | } 121 | 122 | private: 123 | }; 124 | 125 | } // namespace estimator 126 | -------------------------------------------------------------------------------- /estimator/include/estimator_manager.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include "initialization.h" 7 | #include "trajectory_manager.h" 8 | #include "trajectory_viewer.h" 9 | 10 | #include 11 | 12 | namespace estimator{ 13 | 14 | class EstimatorManager{ 15 | public: 16 | EstimatorManager(const YAML::Node& yaml_node, ros::NodeHandle& nh); 17 | ~EstimatorManager(); 18 | 19 | void cornerArrayCallback(const corner_msgs::cornerArray& msg); 20 | void circleArrayCallback(const circle_msgs::circleArray& msg); 21 | 22 | void initialization(); 23 | void optimization(); 24 | 25 | private: 26 | void readBags(std::string corner_bag_path, std::string circle_bag_path); 27 | void performEstimator(); 28 | void setInitialState(); 29 | void updateTrajectoryInitial(); 30 | void calculateRepError(int CAM_TYPE, int tag = 0); 31 | void clearPreStatus(); 32 | 33 | ros::NodeHandle nh_; 34 | ros::Subscriber corner_sub_; 35 | ros::Subscriber circle_sub_; 36 | rosbag::Bag read_bag_corner, read_bag_circle; 37 | 38 | Initializer::Ptr est_initializer; 39 | TrajectoryManager::Ptr trajectory_manager_; 40 | Trajectory::Ptr trajectory_; 41 | Camera::Ptr event_camera_, convent_camera_; 42 | CalibBoard::Ptr calib_board_; 43 | 44 | TrajectoryViewer traj_viewer; 45 | 46 | std::mutex init_lc; 47 | 48 | double knot_distance; 49 | double opt_time; 50 | std::vector rep_err_conv, rep_err_ev, rep_err_ev_st, rep_err_ev_ed; 51 | ceres::Solver::Summary summary; 52 | int num_disp_pose = 0, continue_num = 10; 53 | int64_t current_start_time, current_time, current_end_time, prev_time; 54 | cv::Mat T; 55 | Eigen::Matrix4d eigenT; 56 | SE3d se3; 57 | 58 | std::vector corner_used, circle_used; 59 | }; 60 | 61 | } // namespace estimator 62 | -------------------------------------------------------------------------------- /estimator/include/factor/analytic_diff/rd_spline_view.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | namespace estimator 12 | { 13 | 14 | class RdSplineView 15 | { 16 | public: 17 | static constexpr int N = SplineOrder; ///< Order of the spline. 18 | static constexpr int DIM = 3; ///< Dimension of euclidean vector space. 19 | 20 | using MatN = Eigen::Matrix; 21 | using VecN = Eigen::Matrix; 22 | using VecD = Eigen::Matrix; 23 | 24 | /// @brief Struct to store the Jacobian of the spline 25 | struct JacobianStruct 26 | { 27 | size_t 28 | start_idx; ///< Start index of the non-zero elements of the Jacobian. 29 | std::array d_val_d_knot; ///< Value of nonzero Jacobians. 30 | }; 31 | 32 | RdSplineView() {} 33 | 34 | /// @brief Evaluate value or derivative of the spline 35 | /// 36 | /// @param Derivative derivative to evaluate (0 for value) 37 | /// @param[in] time_ns time for evaluating of the spline 38 | /// @param[out] J if not nullptr, return the Jacobian of the value with 39 | /// respect to knots 40 | /// @return value of the spline or derivative. Euclidean vector of dimention 41 | /// DIM. 42 | template 43 | static VecD evaluate(const int64_t time_ns, 44 | const SplineSegmentMeta &splne_meta, 45 | double const *const *knots, 46 | JacobianStruct *J = nullptr) 47 | { 48 | std::pair ui = splne_meta.computeTIndexNs(time_ns); 49 | size_t s = ui.second + splne_meta.n; 50 | double u = ui.first; 51 | 52 | VecN p; 53 | baseCoeffsWithTimeR3(p, u); 54 | 55 | VecN coeff = splne_meta.pow_inv_dt[Derivative] * (blending_matrix_ * p); 56 | 57 | VecD res; 58 | res.setZero(); 59 | 60 | for (int i = 0; i < N; i++) 61 | { 62 | Eigen::Map p(knots[s + i]); 63 | res += coeff[i] * p; 64 | if (J) 65 | J->d_val_d_knot[i] = coeff[i]; 66 | } 67 | 68 | if (J) 69 | J->start_idx = s; 70 | 71 | return res; 72 | } 73 | 74 | /// @brief Alias for first derivative of spline. See \ref evaluate. 75 | static VecD velocity(const int64_t time_ns, 76 | const SplineSegmentMeta &splne_meta, 77 | double const *const *knots, 78 | JacobianStruct *J = nullptr) 79 | { 80 | return evaluate<1>(time_ns, splne_meta, knots, J); 81 | } 82 | 83 | /// @brief Alias for second derivative of spline. See \ref evaluate. 84 | static VecD acceleration(const int64_t time_ns, 85 | const SplineSegmentMeta &splne_meta, 86 | double const *const *knots, 87 | JacobianStruct *J = nullptr) 88 | { 89 | return evaluate<2>(time_ns, splne_meta, knots, J); 90 | } 91 | 92 | // protected: 93 | /// @brief Vector of derivatives of time polynomial. 94 | /// 95 | /// Computes a derivative of \f$ \begin{bmatrix}1 & t & t^2 & \dots & 96 | /// t^{N-1}\end{bmatrix} \f$ with repect to time. For example, the first 97 | /// derivative would be \f$ \begin{bmatrix}0 & 1 & 2 t & \dots & (N-1) 98 | /// t^{N-2}\end{bmatrix} \f$. 99 | /// @param Derivative derivative to evaluate 100 | /// @param[out] res_const vector to store the result 101 | /// @param[in] t 102 | template 103 | static void baseCoeffsWithTimeR3(const Eigen::MatrixBase &res_const, 104 | double t) 105 | { 106 | EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, N); 107 | Eigen::MatrixBase &res = 108 | const_cast &>(res_const); 109 | 110 | res.setZero(); 111 | 112 | if (Derivative < N) 113 | { 114 | res[Derivative] = base_coefficients_(Derivative, Derivative); 115 | 116 | double _t = t; 117 | for (int j = Derivative + 1; j < N; j++) 118 | { 119 | res[j] = base_coefficients_(Derivative, j) * _t; 120 | _t = _t * t; 121 | } 122 | } 123 | } 124 | 125 | ///< Base coefficients matrix. See \ref computeBaseCoefficients. 126 | static inline MatN base_coefficients_ = computeBaseCoefficients(); 127 | 128 | ///< Blending matrix. See \ref computeBlendingMatrix. 129 | static inline MatN blending_matrix_ = computeBlendingMatrix(); 130 | }; 131 | 132 | // typename RdSplineView::MatN RdSplineView::base_coefficients_ = 133 | // estimator::computeBaseCoefficients(); 134 | 135 | // typename RdSplineView::MatN RdSplineView::blending_matrix_ = 136 | // estimator::computeBlendingMatrix(); 137 | 138 | } // namespace estimator 139 | -------------------------------------------------------------------------------- /estimator/include/factor/analytic_diff/split_spline_view.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace estimator 7 | { 8 | struct SplitSpineView : public So3SplineView, public RdSplineView 9 | { 10 | static constexpr int N = SplineOrder; // Order of the spline. 11 | static constexpr int DEG = N - 1; // Degree of the spline. 12 | static constexpr int DIM = 3; // Dimension of euclidean vector space. 13 | 14 | using MatN = Eigen::Matrix; 15 | using VecN = Eigen::Matrix; 16 | using Vec3 = Eigen::Matrix; 17 | using Mat3 = Eigen::Matrix; 18 | 19 | using VecD = Eigen::Matrix; 20 | using MatD = Eigen::Matrix; 21 | 22 | using SO3 = Sophus::SO3; 23 | 24 | using SO3View = So3SplineView; 25 | using R3View = RdSplineView; 26 | 27 | // struct SplineIMUData 28 | // { 29 | // double timestamp; 30 | // Eigen::Vector3d gyro; 31 | // VecD accel; 32 | // SO3 R_inv; 33 | // }; 34 | struct SplineIMUData 35 | { 36 | int64_t time_ns; 37 | Eigen::Vector3d gyro; 38 | VecD accel; 39 | SO3 R_inv; 40 | size_t start_idx; 41 | }; 42 | 43 | SplitSpineView() {} 44 | 45 | static SplineIMUData Evaluate( 46 | const int64_t time_ns, const SplineSegmentMeta &splne_meta, 47 | double const *const *knots, const Vec3 &gravity, 48 | typename SO3View::JacobianStruct *J_rot_w = nullptr, 49 | typename SO3View::JacobianStruct *J_rot_a = nullptr, 50 | typename R3View::JacobianStruct *J_pos = nullptr) 51 | { 52 | SplineIMUData spline_imu_data; 53 | spline_imu_data.time_ns = time_ns; 54 | 55 | std::pair ui = splne_meta.computeTIndexNs(time_ns); 56 | size_t s = ui.second; 57 | double u = ui.first; 58 | 59 | size_t R_knot_offset = s; 60 | size_t P_knot_offset = s + splne_meta.NumParameters(); 61 | spline_imu_data.start_idx = s; 62 | 63 | VecN Up, lambda_a; 64 | R3View::template baseCoeffsWithTimeR3<2>(Up, u); 65 | lambda_a = splne_meta.pow_inv_dt[2] * R3View::blending_matrix_ * Up; 66 | 67 | VecN Ur, lambda_R; 68 | SO3View::template baseCoeffsWithTime<0>(Ur, u); 69 | lambda_R = SO3View::blending_matrix_ * Ur; 70 | 71 | VecN Uw, lambda_w; 72 | SO3View::template baseCoeffsWithTime<1>(Uw, u); 73 | lambda_w = splne_meta.pow_inv_dt[1] * SO3View::blending_matrix_ * Uw; 74 | 75 | VecD accelerate; 76 | accelerate.setZero(); 77 | 78 | if (J_pos) 79 | J_pos->start_idx = s; 80 | for (int i = 0; i < N; i++) 81 | { 82 | Eigen::Map p(knots[P_knot_offset + i]); 83 | accelerate += lambda_a[i] * p; 84 | 85 | if (J_pos) 86 | J_pos->d_val_d_knot[i] = lambda_a[i]; 87 | } 88 | 89 | Vec3 d_vec[DEG]; // d_1, d_2, d_3 90 | SO3 A_rot_inv[DEG]; // A_1_inv, A_2_inv, A_3_inv 91 | SO3 A_accum_inv; // A_3_inv * A2_inv * A1_inv 92 | Mat3 A_post_inv[N]; // A_3_inv*A2_inv*A1_inv, A_3_inv*A2_inv, A_3_inv, I 93 | Mat3 Jr_dvec_inv[DEG]; // Jr_inv(d1), Jr_inv(d2), Jr_inv(d3) 94 | Mat3 Jr_kdelta[DEG]; // Jr(-kd1), Jr(-kd2), Jr(-kd3) 95 | 96 | A_post_inv[N - 1] = A_accum_inv.matrix(); // Identity Matrix 97 | /// 2 1 0 98 | for (int i = DEG - 1; i >= 0; i--) 99 | { 100 | Eigen::Map R0(knots[R_knot_offset + i]); 101 | Eigen::Map R1(knots[R_knot_offset + i + 1]); 102 | 103 | d_vec[i] = (R0.inverse() * R1).log(); 104 | 105 | Vec3 k_delta = lambda_R[i + 1] * d_vec[i]; 106 | A_rot_inv[i] = Sophus::SO3d::exp(-k_delta); 107 | A_accum_inv *= A_rot_inv[i]; 108 | 109 | if (J_rot_w || J_rot_a) 110 | { 111 | A_post_inv[i] = A_accum_inv.matrix(); 112 | 113 | Sophus::rightJacobianInvSO3(d_vec[i], Jr_dvec_inv[i]); 114 | Sophus::rightJacobianSO3(-k_delta, Jr_kdelta[i]); 115 | } 116 | } 117 | 118 | /// Omega(j) 119 | Vec3 omega[N]; // w(1), w(2), w(3), w(4) 120 | { 121 | omega[0] = Vec3::Zero(); 122 | for (int i = 0; i < DEG; i++) 123 | { 124 | omega[i + 1] = A_rot_inv[i] * omega[i] + lambda_w[i + 1] * d_vec[i]; 125 | } 126 | spline_imu_data.gyro = omega[3]; 127 | 128 | Eigen::Map Ri(knots[R_knot_offset]); 129 | SO3 R_inv = A_accum_inv * Ri.inverse(); 130 | 131 | spline_imu_data.accel = R_inv * (accelerate + gravity); 132 | spline_imu_data.R_inv = R_inv; 133 | } 134 | 135 | if (J_rot_w) 136 | { 137 | J_rot_w->start_idx = s; 138 | for (int i = 0; i < N; i++) 139 | { 140 | J_rot_w->d_val_d_knot[i].setZero(); 141 | } 142 | 143 | // d(omega) / d(d_j) 144 | Mat3 d_omega_d_delta[DEG]; // w(4)/d1, w(4)/d2, w(4)/d3 145 | d_omega_d_delta[0] = lambda_w[1] * A_post_inv[1]; 146 | for (int i = 1; i < DEG; i++) 147 | { 148 | d_omega_d_delta[i] = lambda_R[i + 1] * A_post_inv[i] * 149 | SO3::hat(omega[i]) * Jr_kdelta[i] + 150 | lambda_w[i + 1] * A_post_inv[i + 1]; 151 | } 152 | 153 | for (int i = 0; i < DEG; i++) 154 | { 155 | J_rot_w->d_val_d_knot[i] -= 156 | d_omega_d_delta[i] * Jr_dvec_inv[i].transpose(); 157 | J_rot_w->d_val_d_knot[i + 1] += d_omega_d_delta[i] * Jr_dvec_inv[i]; 158 | } 159 | } 160 | 161 | if (J_rot_a) 162 | { 163 | // for accelerate jacobian 164 | Mat3 R_accum[DEG - 1]; // R_i, R_i*A_1, R_i*A_1*A_2 165 | Eigen::Map R0(knots[R_knot_offset]); 166 | R_accum[0] = R0.matrix(); 167 | /// 1 2 168 | for (int i = 1; i < DEG; i++) 169 | { 170 | R_accum[i] = R_accum[i - 1] * A_rot_inv[i - 1].matrix().transpose(); 171 | } 172 | 173 | J_rot_a->start_idx = s; 174 | for (int i = 0; i < N; i++) 175 | { 176 | J_rot_a->d_val_d_knot[i].setZero(); 177 | } 178 | 179 | Mat3 lhs = 180 | spline_imu_data.R_inv.matrix() * SO3::hat(accelerate + gravity); 181 | J_rot_a->d_val_d_knot[0] += lhs * R_accum[0]; 182 | for (int i = 0; i < DEG; i++) 183 | { 184 | Mat3 d_a_d_delta = lambda_R[i + 1] * lhs * R_accum[i] * Jr_kdelta[i]; 185 | 186 | J_rot_a->d_val_d_knot[i] -= d_a_d_delta * Jr_dvec_inv[i].transpose(); 187 | J_rot_a->d_val_d_knot[i + 1] += d_a_d_delta * Jr_dvec_inv[i]; 188 | } 189 | } 190 | 191 | return spline_imu_data; 192 | } 193 | }; 194 | 195 | } // namespace estimator -------------------------------------------------------------------------------- /estimator/include/factor/ceres_local_param.h: -------------------------------------------------------------------------------- 1 | /** 2 | BSD 3-Clause License 3 | 4 | This file is part of the Basalt project. 5 | https://gitlab.com/VladyslavUsenko/basalt-headers.git 6 | 7 | Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. 8 | All rights reserved. 9 | 10 | Redistribution and use in source and binary forms, with or without 11 | modification, are permitted provided that the following conditions are met: 12 | 13 | * Redistributions of source code must retain the above copyright notice, this 14 | list of conditions and the following disclaimer. 15 | 16 | * Redistributions in binary form must reproduce the above copyright notice, 17 | this list of conditions and the following disclaimer in the documentation 18 | and/or other materials provided with the distribution. 19 | 20 | * Neither the name of the copyright holder nor the names of its 21 | contributors may be used to endorse or promote products derived from 22 | this software without specific prior written permission. 23 | 24 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 26 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 27 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 28 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 29 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 30 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 32 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 33 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 34 | 35 | @file 36 | @brief Generic local parametrization for Sophus Lie group types to be used with 37 | ceres. 38 | */ 39 | 40 | /** 41 | File adapted from Sophus 42 | 43 | Copyright 2011-2017 Hauke Strasdat 44 | 2012-2017 Steven Lovegrove 45 | 46 | Permission is hereby granted, free of charge, to any person obtaining a copy 47 | of this software and associated documentation files (the "Software"), to 48 | deal in the Software without restriction, including without limitation the 49 | rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 50 | sell copies of the Software, and to permit persons to whom the Software is 51 | furnished to do so, subject to the following conditions: 52 | 53 | The above copyright notice and this permission notice shall be included in 54 | all copies or substantial portions of the Software. 55 | 56 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 57 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 58 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 59 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 60 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 61 | FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 62 | IN THE SOFTWARE. 63 | */ 64 | 65 | #pragma once 66 | 67 | #include 68 | #include 69 | 70 | namespace estimator 71 | { 72 | 73 | /// @brief Local parametrization for ceres that can be used with Sophus Lie 74 | /// group implementations. 75 | template 76 | class LieLocalParameterization : public ceres::LocalParameterization 77 | { 78 | public: 79 | virtual ~LieLocalParameterization() {} 80 | 81 | using Tangentd = typename Groupd::Tangent; 82 | 83 | /// @brief plus operation for Ceres 84 | /// 85 | /// T * exp(x) 86 | /// 87 | virtual bool Plus(double const *T_raw, double const *delta_raw, 88 | double *T_plus_delta_raw) const 89 | { 90 | Eigen::Map const T(T_raw); 91 | Eigen::Map const delta(delta_raw); 92 | Eigen::Map T_plus_delta(T_plus_delta_raw); 93 | T_plus_delta = T * Groupd::exp(delta); 94 | return true; 95 | } 96 | 97 | ///@brief Jacobian of plus operation for Ceres 98 | /// 99 | /// Dx T * exp(x) with x=0 100 | /// w -z y 101 | /// z w -x 102 | /// -y x w 103 | /// -x -y -z 104 | /// 4*3 105 | virtual bool ComputeJacobian(double const *T_raw, 106 | double *jacobian_raw) const 107 | { 108 | Eigen::Map T(T_raw); 109 | // TODO 110 | Eigen::Map> 112 | // Eigen::Map> 113 | jacobian(jacobian_raw); 114 | jacobian = T.Dx_this_mul_exp_x_at_0(); 115 | return true; 116 | } 117 | 118 | ///@brief Global size 119 | virtual int GlobalSize() const { return Groupd::num_parameters; } 120 | 121 | ///@brief Local size 122 | virtual int LocalSize() const { return Groupd::DoF; } 123 | }; 124 | 125 | template 126 | class LieAnalyticLocalParameterization : public ceres::LocalParameterization 127 | { 128 | public: 129 | virtual ~LieAnalyticLocalParameterization() {} 130 | 131 | using Tangentd = typename Groupd::Tangent; 132 | 133 | /// @brief plus operation for Ceres 134 | /// 135 | /// T * exp(x) 136 | /// 137 | virtual bool Plus(double const *T_raw, double const *delta_raw, 138 | double *T_plus_delta_raw) const 139 | { 140 | Eigen::Map const T(T_raw); 141 | Eigen::Map const delta(delta_raw); 142 | Eigen::Map T_plus_delta(T_plus_delta_raw); 143 | T_plus_delta = T * Groupd::exp(delta); 144 | return true; 145 | } 146 | 147 | virtual bool ComputeJacobian(double const *T_raw, 148 | double *jacobian_raw) const 149 | { 150 | Eigen::Map T(T_raw); 151 | Eigen::Map> 153 | jacobian(jacobian_raw); 154 | jacobian.setZero(); 155 | jacobian(0, 0) = 1; 156 | jacobian(1, 1) = 1; 157 | jacobian(2, 2) = 1; 158 | return true; 159 | } 160 | 161 | ///@brief Global size 162 | virtual int GlobalSize() const { return Groupd::num_parameters; } 163 | 164 | ///@brief Local size 165 | virtual int LocalSize() const { return Groupd::DoF; } 166 | }; 167 | 168 | } // namespace estimator 169 | -------------------------------------------------------------------------------- /estimator/include/initialization.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "ros/ros.h" 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include 21 | #include 22 | #include 23 | 24 | #define CONV_CAM 0 25 | #define EV_CAM 1 26 | #define BOTH 2 27 | 28 | namespace estimator{ 29 | 30 | class Initializer{ 31 | public: 32 | typedef std::shared_ptr Ptr; 33 | 34 | Initializer(Camera::Ptr event_camera, Camera::Ptr convent_camera, CalibBoard::Ptr calib_board) 35 | : event_camera_(event_camera), convent_camera_(convent_camera), calib_board_(calib_board) 36 | { 37 | b_conv_initialized = false; 38 | b_ev_initialized = false; 39 | b_both_initialized = false; 40 | }; 41 | 42 | bool convInitialSucc(){ return b_conv_initialized;} 43 | bool evInitialSucc(){ return b_ev_initialized;} 44 | bool judgeBufferStatus(int cam_type){ 45 | if (cam_type == 0){ 46 | if (getCornerBufferNum() < initial_window_size){ 47 | ROS_INFO("Conv cam no sufficient frames, now: %d, %d", getCornerBufferNum(), initial_window_size); 48 | return false; 49 | } 50 | return true; 51 | } 52 | else{ 53 | if (getCircleBufferNum() < initial_window_size){ 54 | ROS_INFO("EV cam no sufficient frames, now: %d", getCircleBufferNum()); 55 | return false; 56 | } 57 | return true; 58 | } 59 | } 60 | 61 | void processConv(); 62 | void processEv(); 63 | void estimateInitialExtrinsic(); 64 | 65 | std::vector corner_buffer_, corner_buffer_selected; 66 | std::deque circle_buffer_, circle_buffer_selected; 67 | int initial_window_size = 10; 68 | float square_size; 69 | cv::Size conv_cam_size, ev_cam_size; 70 | double epoch_time; 71 | 72 | private: 73 | double computeReprojectionError(const cv::Mat& H, const std::vector& point_1, const std::vector& point_2) { 74 | double total_error = 0.0; 75 | int point_count = static_cast(point_1.size()); 76 | 77 | for (int i = 0; i < point_count; ++i) { 78 | cv::Point2f p1 = point_1[i]; 79 | cv::Point3d p1_homogeneous((double)p1.x, (double)p1.y, 1.0); 80 | cv::Mat p1_transformed = H * cv::Mat(p1_homogeneous); 81 | p1_transformed /= p1_transformed.at(2); 82 | 83 | cv::Point2f p2 = point_2[i]; 84 | double error = cv::norm(cv::Point2f(p1_transformed.at(0), p1_transformed.at(1)) - p2); 85 | total_error += error; 86 | } 87 | 88 | return total_error / point_count; 89 | } 90 | int getCornerBufferNum(){ 91 | corner_buffer_selected.clear(); 92 | legal_corner_size = 0; 93 | if (corner_buffer_.empty()) return 0; 94 | cv::Mat w, u, vt, rotationMatrix, rvec; 95 | for (auto c:corner_buffer_){ 96 | if ((c.timestamp - corner_buffer_.front().timestamp).toSec() > epoch_time) break; 97 | static bool fisrt_judge = true; 98 | if (fisrt_judge){ 99 | fisrt_judge = false; 100 | last_corner = c; 101 | corner_buffer_selected.emplace_back(c); 102 | } 103 | float dist = 0; 104 | std::vector point_1, point_2; 105 | for (auto corner_last:last_corner.corners){ 106 | for (auto corner_now:c.corners){ 107 | if (corner_last.x_grid == corner_now.x_grid && corner_last.y_grid == corner_now.y_grid){ 108 | dist += std::sqrt((corner_last.x - corner_now.x) * (corner_last.x - corner_now.x) + (corner_last.y - corner_now.y) * (corner_last.y - corner_now.y)); 109 | point_1.emplace_back(cv::Point2f(corner_last.x, corner_last.y)); 110 | point_2.emplace_back(cv::Point2f(corner_now.x, corner_now.y)); 111 | } 112 | } 113 | } 114 | cv::Mat H = cv::findHomography(point_1, point_2); 115 | cv::Mat h1 = H.col(0); 116 | cv::Mat h2 = H.col(1); 117 | cv::Mat h3 = H.col(2); 118 | 119 | double lambda = 1.0 / cv::norm(h1); 120 | 121 | cv::Mat r1 = lambda * h1; 122 | cv::Mat r2 = lambda * h2; 123 | cv::Mat r3 = r1.cross(r2); 124 | 125 | rotationMatrix = (cv::Mat_(3, 3) << r1.at(0), r2.at(0), r3.at(0), 126 | r1.at(1), r2.at(1), r3.at(1), 127 | r1.at(2), r2.at(2), r3.at(2)); 128 | 129 | cv::SVDecomp(rotationMatrix, w, u, vt); 130 | rotationMatrix = u * vt; 131 | cv::Rodrigues(rotationMatrix, rvec); 132 | 133 | double reprojection_error = computeReprojectionError(H, point_1, point_2); 134 | dist /= last_circle.circles.size(); 135 | if (dist > convent_camera_->image_height_ * 0.1 && reprojection_error < 1 && cv::norm(rvec) > 0.03){ 136 | legal_corner_size++; 137 | last_corner = c; 138 | corner_buffer_selected.emplace_back(c); 139 | } 140 | } 141 | std::cout << "corner num:" << legal_corner_size << "\n"; 142 | return legal_corner_size; 143 | } 144 | int getCircleBufferNum(){ 145 | circle_buffer_selected.clear(); 146 | legal_circle_size = 0; 147 | if (circle_buffer_.empty()) return 0; 148 | cv::Mat w, u, vt, rotationMatrix, rvec; 149 | for (auto cir:circle_buffer_){ 150 | if ((cir.circles[0].timestamp - circle_buffer_.front().circles[0].timestamp).toSec() > epoch_time) break; 151 | static bool first_inside = true; 152 | if (first_inside){ 153 | first_inside = false; 154 | last_circle = cir; 155 | circle_buffer_selected.emplace_back(cir); 156 | } 157 | float dist = 0; 158 | std::vector point_1, point_2; 159 | for (auto circle_last:last_circle.circles){ 160 | for (auto circle_now:cir.circles){ 161 | if (circle_last.x_grid == circle_now.x_grid && circle_last.y_grid == circle_now.y_grid){ 162 | dist += std::sqrt((circle_last.x - circle_now.x) * (circle_last.x - circle_now.x) + (circle_last.y - circle_now.y) * (circle_last.y - circle_now.y)); 163 | point_1.emplace_back(cv::Point2f(circle_last.x, circle_last.y)); 164 | point_2.emplace_back(cv::Point2f(circle_now.x, circle_now.y)); 165 | } 166 | } 167 | } 168 | cv::Mat H = cv::findHomography(point_1, point_2); 169 | cv::Mat h1 = H.col(0); 170 | cv::Mat h2 = H.col(1); 171 | cv::Mat h3 = H.col(2); 172 | 173 | double lambda = 1.0 / cv::norm(h1); 174 | 175 | cv::Mat r1 = lambda * h1; 176 | cv::Mat r2 = lambda * h2; 177 | cv::Mat r3 = r1.cross(r2); 178 | 179 | rotationMatrix = (cv::Mat_(3, 3) << r1.at(0), r2.at(0), r3.at(0), 180 | r1.at(1), r2.at(1), r3.at(1), 181 | r1.at(2), r2.at(2), r3.at(2)); 182 | 183 | cv::SVDecomp(rotationMatrix, w, u, vt); 184 | rotationMatrix = u * vt; 185 | cv::Rodrigues(rotationMatrix, rvec); 186 | 187 | double reprojection_error = computeReprojectionError(H, point_1, point_2); 188 | dist /= last_circle.circles.size(); 189 | if (dist > event_camera_->image_height_ * 0.2 && reprojection_error < 0.5 && cv::norm(rvec) > 0.03){ 190 | legal_circle_size++; 191 | last_circle = cir; 192 | circle_buffer_selected.emplace_back(cir); 193 | } 194 | } 195 | std::cout << "circle num:" << legal_circle_size << "\n"; 196 | return legal_circle_size; 197 | } 198 | 199 | bool solveRelativePose(const corner_msgs::cornerArray& corners, Camera::Ptr cam, cv::Mat& Transformation); 200 | bool solveRelativePose(const circle_msgs::circleArray& circles, Camera::Ptr cam, cv::Mat& Transformation); 201 | void refinePose(const corner_msgs::cornerArray& corners, Camera::Ptr cam, cv::Mat& Transformation); 202 | void buildProblem(ceres::Problem* problem, std::vector imagePoints, std::vector worldPoints, Camera::Ptr cam, cv::Mat& Transformation); 203 | 204 | std::vector> corner_image_cluster, circle_image_cluster, circle_image_cluster_random; 205 | std::vector> corner_world_cluster, circle_world_cluster, circle_world_cluster_random; 206 | 207 | bool b_conv_initialized, b_ev_initialized, b_both_initialized; 208 | double rms_conv, rms_ev; 209 | corner_msgs::cornerArray last_corner; 210 | circle_msgs::circleArray last_circle; 211 | int legal_corner_size, legal_circle_size; 212 | 213 | Trajectory::Ptr trajectory_; 214 | Camera::Ptr event_camera_, convent_camera_; 215 | CalibBoard::Ptr calib_board_; 216 | 217 | cv::Mat convCameraMatrix, convDistCoeffs; 218 | std::vector convRvecs, convTvecs; 219 | cv::Mat evCameraMatrix, evDistCoeffs; 220 | std::vector evRvecs, evTvecs; 221 | 222 | struct SnavelyReprojectionError; 223 | ceres::Problem problem; 224 | double rot[3], trans[3]; 225 | 226 | friend class EstimatorManager; 227 | }; 228 | 229 | }; -------------------------------------------------------------------------------- /estimator/include/trajectory_estimator.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include "factor/ceres_local_param.h" 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | namespace estimator{ 16 | class TrajectoryEstimator{ 17 | static ceres::Problem::Options DefaultProblemOptions() 18 | { 19 | ceres::Problem::Options options; 20 | options.loss_function_ownership = ceres::TAKE_OWNERSHIP; 21 | options.local_parameterization_ownership = ceres::DO_NOT_TAKE_OWNERSHIP; 22 | return options; 23 | } 24 | 25 | public: 26 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 27 | 28 | typedef std::shared_ptr Ptr; 29 | 30 | TrajectoryEstimator(Trajectory::Ptr trajectory, Camera::Ptr event_camera, Camera::Ptr convent_camera, CalibBoard::Ptr calib_board); 31 | 32 | ~TrajectoryEstimator(){ 33 | delete analytic_local_parameterization_; 34 | } 35 | 36 | void addConvFeature(const corner_msgs::corner &corner, ros::Time timestamp); 37 | 38 | void addEventFeature(const circle_msgs::circle &circle); 39 | 40 | void addControlPoints(const SplineMeta &spline_meta, std::vector &vec, std::shared_ptr problem, bool addPosKnot = false, bool isEvent = false); 41 | 42 | ceres::Solver::Summary solve(int max_iterations, bool progress, int num_threads); 43 | ceres::Solver::Summary solve1(int max_iterations, bool progress, int num_threads); 44 | 45 | class UpdateTrajectoryCallback : public ceres::IterationCallback { 46 | public: 47 | UpdateTrajectoryCallback(TrajectoryEstimator* estimator) : estimator_(estimator) {} 48 | 49 | virtual ceres::CallbackReturnType operator()(const ceres::IterationSummary& summary) { 50 | return ceres::SOLVER_CONTINUE; 51 | } 52 | 53 | private: 54 | TrajectoryEstimator* estimator_; 55 | }; 56 | 57 | std::shared_ptr problem_, problem_1; 58 | std::vector> trajectory_info_; 59 | 60 | double max_time_delay = 0.05; // unit: second 61 | 62 | private: 63 | Trajectory::Ptr trajectory_; 64 | 65 | Camera::Ptr event_camera_, convent_camera_; 66 | CalibBoard::Ptr calib_board_; 67 | 68 | // std::shared_ptr problem_; 69 | ceres::LocalParameterization *analytic_local_parameterization_; 70 | 71 | friend class UpdateTrajectoryCallback; 72 | }; 73 | 74 | } //namespace estimator -------------------------------------------------------------------------------- /estimator/include/trajectory_manager.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "trajectory_estimator.h" 3 | #include 4 | 5 | namespace estimator 6 | { 7 | 8 | class TrajectoryManager 9 | { 10 | public: 11 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 12 | 13 | typedef std::shared_ptr Ptr; 14 | 15 | TrajectoryManager(Trajectory::Ptr trajectory); 16 | 17 | void extendTrajectory(int64_t max_time, SE3d now_knot); 18 | void setOriginalPose(); 19 | 20 | private: 21 | Trajectory::Ptr trajectory_; 22 | 23 | int64_t max_bef_ns; 24 | int max_bef_idx; 25 | int max_aft_idx; 26 | }; 27 | 28 | } //namespace estimator 29 | -------------------------------------------------------------------------------- /estimator/include/trajectory_viewer.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | 13 | #include 14 | #include 15 | 16 | namespace estimator 17 | { 18 | class TrajectoryViewer 19 | { 20 | private: 21 | ros::Publisher pub_cam_pose_; 22 | 23 | // PublishSplineTrajectory 24 | ros::Publisher pub_spline_trajectory_; 25 | ros::Publisher pub_spline_trajectory_before_opt_; 26 | 27 | public: 28 | void SetPublisher(ros::NodeHandle &nh) 29 | { 30 | pub_cam_pose_ = nh.advertise("/camera/pose", 10); 31 | 32 | /// spline trajectory 33 | pub_spline_trajectory_ = nh.advertise("/spline/trajectory", 10); 34 | pub_spline_trajectory_before_opt_ = nh.advertise("/spline/trajectory_bef_opt", 10); 35 | std::cout << "[SetPublisher] init done.\n"; 36 | } 37 | 38 | void PublishPose(SE3d pose, int cam_type, int id, bool refresh = false) 39 | { 40 | ros::Time time_now = ros::Time::now(); 41 | visualization_msgs::MarkerArray marker_array; 42 | 43 | if (refresh){ 44 | visualization_msgs::Marker marker; 45 | marker.action = visualization_msgs::Marker::DELETEALL; 46 | marker_array.markers.push_back(marker); 47 | 48 | // Publish the MarkerArray message 49 | pub_cam_pose_.publish(marker_array); 50 | 51 | return; 52 | } 53 | 54 | geometry_msgs::PoseStamped poseIinG; 55 | poseIinG.header.stamp = time_now; 56 | poseIinG.header.frame_id = "/map"; 57 | tf::pointEigenToMsg(pose.translation(), poseIinG.pose.position); 58 | tf::quaternionEigenToMsg(pose.unit_quaternion(), 59 | poseIinG.pose.orientation); 60 | 61 | visualization_msgs::Marker marker; 62 | marker.header.frame_id = "/map"; 63 | marker.header.stamp = time_now; 64 | marker.ns = "pose_markers"; 65 | marker.id = id; 66 | marker.type = visualization_msgs::Marker::SPHERE; 67 | marker.action = visualization_msgs::Marker::ADD; 68 | marker.pose = poseIinG.pose; 69 | 70 | if (cam_type == 1){ 71 | marker.scale.x = 2; 72 | marker.scale.y = 2; 73 | marker.scale.z = 2; 74 | marker.color.r = 1.0; 75 | marker.color.g = 0.0; 76 | marker.color.b = 0.0; 77 | marker.color.a = 0.8; 78 | }else if (cam_type == 0){ 79 | marker.scale.x = 5; 80 | marker.scale.y = 5; 81 | marker.scale.z = 5; 82 | marker.color.r = 0.3; 83 | marker.color.g = 0.3; 84 | marker.color.b = 0.9; 85 | marker.color.a = 0.9; 86 | }else if (cam_type == 2){ 87 | marker.scale.x = 2; 88 | marker.scale.y = 2; 89 | marker.scale.z = 2; 90 | marker.color.r = 0.0; 91 | marker.color.g = 1.0; 92 | marker.color.b = 0.0; 93 | marker.color.a = 0.9; 94 | }else if (cam_type == 3){ 95 | marker.scale.x = 5; 96 | marker.scale.y = 5; 97 | marker.scale.z = 5; 98 | marker.color.r = 0.5; 99 | marker.color.g = 0.5; 100 | marker.color.b = 0.5; 101 | marker.color.a = 0.9; 102 | } 103 | 104 | marker_array.markers.push_back(marker); 105 | pub_cam_pose_.publish(marker_array); 106 | } 107 | 108 | void PublishSplineTrajectory(Trajectory::Ptr trajectory, std::vector> trajectory_info, int64_t min_time, int continue_num, 109 | int64_t max_time, int64_t dt, bool stage) 110 | { 111 | ros::Time time_now = ros::Time::now(); 112 | ros::Time t_temp; 113 | 114 | if (stage){ 115 | if (pub_spline_trajectory_.getNumSubscribers() != 0) 116 | { 117 | std::vector poses_geo; 118 | int num = -1; 119 | for (auto traj:trajectory_info){ 120 | if ((traj.second - traj.first) >= continue_num){ 121 | PublishPose(trajectory->poseNs(std::max(0, (traj.first - trajectory->DEG + 1)) * trajectory->getDtNs()), 3, num--); 122 | for (int64_t t = std::max(0, (traj.first - trajectory->DEG + 1)) * trajectory->getDtNs(); t < std::min(max_time, (traj.second - 3) * trajectory->getDtNs()); t += dt){ 123 | SE3d pose = trajectory->poseNs(t); 124 | geometry_msgs::PoseStamped poseIinG; 125 | poseIinG.header.stamp = t_temp.fromSec(t * NS_TO_S); 126 | poseIinG.header.frame_id = "/map"; 127 | tf::pointEigenToMsg(pose.translation(), poseIinG.pose.position); 128 | tf::quaternionEigenToMsg(pose.unit_quaternion(), 129 | poseIinG.pose.orientation); 130 | poses_geo.push_back(poseIinG); 131 | } 132 | PublishPose(trajectory->poseNs(std::min(max_time, (traj.second - 3) * trajectory->getDtNs()) - 0.0001 * S_TO_NS), 0, num--); 133 | } 134 | } 135 | 136 | nav_msgs::Path traj_path; 137 | traj_path.header.stamp = time_now; 138 | traj_path.header.frame_id = "/map"; 139 | traj_path.poses = poses_geo; 140 | 141 | pub_spline_trajectory_.publish(traj_path); 142 | } 143 | } 144 | else{ 145 | if (pub_spline_trajectory_before_opt_.getNumSubscribers() != 0) 146 | { 147 | std::vector poses_geo; 148 | for (int64_t t = min_time; t < max_time; t += dt) 149 | { 150 | int start_cp_idx = (long long)(t) / trajectory->getDtNs(); 151 | for (auto traj:trajectory_info){ 152 | if ((traj.second - traj.first) >= continue_num && start_cp_idx >= traj.first && start_cp_idx <= traj.second){ 153 | SE3d pose = trajectory->poseNs(t); 154 | geometry_msgs::PoseStamped poseIinG; 155 | poseIinG.header.stamp = t_temp.fromSec(t * NS_TO_S); 156 | poseIinG.header.frame_id = "/map"; 157 | tf::pointEigenToMsg(pose.translation(), poseIinG.pose.position); 158 | tf::quaternionEigenToMsg(pose.unit_quaternion(), 159 | poseIinG.pose.orientation); 160 | poses_geo.push_back(poseIinG); 161 | } 162 | } 163 | } 164 | 165 | nav_msgs::Path traj_path; 166 | traj_path.header.stamp = time_now; 167 | traj_path.header.frame_id = "/map"; 168 | traj_path.poses = poses_geo; 169 | 170 | pub_spline_trajectory_before_opt_.publish(traj_path); 171 | } 172 | } 173 | } 174 | }; 175 | 176 | } // namespace estimator -------------------------------------------------------------------------------- /estimator/launch/estimator.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /estimator/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | estimator 4 | 0.0.0 5 | The estimator package 6 | 7 | 8 | 9 | 10 | root 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | circle_msgs 53 | corner_msgs 54 | opencv3 55 | roscpp 56 | rosbag 57 | circle_msgs 58 | corner_msgs 59 | opencv3 60 | roscpp 61 | rosbag 62 | circle_msgs 63 | corner_msgs 64 | opencv3 65 | roscpp 66 | rosbag 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | -------------------------------------------------------------------------------- /estimator/rviz/estimator.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 104 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Grid1 10 | - /Path1 11 | - /MarkerArray1 12 | - /Path2 13 | Splitter Ratio: 0.5 14 | Tree Height: 470 15 | - Class: rviz/Selection 16 | Name: Selection 17 | - Class: rviz/Tool Properties 18 | Expanded: 19 | - /2D Pose Estimate1 20 | - /2D Nav Goal1 21 | - /Publish Point1 22 | Name: Tool Properties 23 | Splitter Ratio: 0.5886790156364441 24 | - Class: rviz/Views 25 | Expanded: 26 | - /Current View1 27 | Name: Views 28 | Splitter Ratio: 0.5 29 | - Class: rviz/Time 30 | Experimental: false 31 | Name: Time 32 | SyncMode: 0 33 | SyncSource: "" 34 | Preferences: 35 | PromptSaveOnExit: true 36 | Toolbars: 37 | toolButtonStyle: 2 38 | Visualization Manager: 39 | Class: "" 40 | Displays: 41 | - Alpha: 0.5 42 | Cell Size: 50 43 | Class: rviz/Grid 44 | Color: 160; 160; 164 45 | Enabled: true 46 | Line Style: 47 | Line Width: 0.029999999329447746 48 | Value: Lines 49 | Name: Grid 50 | Normal Cell Count: 0 51 | Offset: 52 | X: 0 53 | Y: 0 54 | Z: 0 55 | Plane: XY 56 | Plane Cell Count: 10 57 | Reference Frame: 58 | Value: true 59 | - Alpha: 1 60 | Buffer Length: 1 61 | Class: rviz/Path 62 | Color: 25; 255; 0 63 | Enabled: true 64 | Head Diameter: 0.30000001192092896 65 | Head Length: 0.20000000298023224 66 | Length: 0.30000001192092896 67 | Line Style: Lines 68 | Line Width: 0.029999999329447746 69 | Name: Path 70 | Offset: 71 | X: 0 72 | Y: 0 73 | Z: 0 74 | Pose Color: 255; 85; 255 75 | Pose Style: None 76 | Radius: 0.029999999329447746 77 | Shaft Diameter: 0.10000000149011612 78 | Shaft Length: 0.10000000149011612 79 | Topic: /spline/trajectory 80 | Unreliable: false 81 | Value: true 82 | - Class: rviz/MarkerArray 83 | Enabled: true 84 | Marker Topic: /camera/pose 85 | Name: MarkerArray 86 | Namespaces: 87 | {} 88 | Queue Size: 100 89 | Value: true 90 | - Alpha: 1 91 | Buffer Length: 1 92 | Class: rviz/Path 93 | Color: 114; 159; 207 94 | Enabled: true 95 | Head Diameter: 0.30000001192092896 96 | Head Length: 0.20000000298023224 97 | Length: 0.30000001192092896 98 | Line Style: Lines 99 | Line Width: 0.029999999329447746 100 | Name: Path 101 | Offset: 102 | X: 0 103 | Y: 0 104 | Z: 0 105 | Pose Color: 255; 85; 255 106 | Pose Style: None 107 | Radius: 0.029999999329447746 108 | Shaft Diameter: 0.10000000149011612 109 | Shaft Length: 0.10000000149011612 110 | Topic: /spline/trajectory_bef_opt 111 | Unreliable: false 112 | Value: true 113 | Enabled: true 114 | Global Options: 115 | Background Color: 48; 48; 48 116 | Default Light: true 117 | Fixed Frame: map 118 | Frame Rate: 30 119 | Name: root 120 | Tools: 121 | - Class: rviz/Interact 122 | Hide Inactive Objects: true 123 | - Class: rviz/MoveCamera 124 | - Class: rviz/Select 125 | - Class: rviz/FocusCamera 126 | - Class: rviz/Measure 127 | - Class: rviz/SetInitialPose 128 | Theta std deviation: 0.2617993950843811 129 | Topic: /initialpose 130 | X std deviation: 0.5 131 | Y std deviation: 0.5 132 | - Class: rviz/SetGoal 133 | Topic: /move_base_simple/goal 134 | - Class: rviz/PublishPoint 135 | Single click: true 136 | Topic: /clicked_point 137 | Value: true 138 | Views: 139 | Current: 140 | Class: rviz/Orbit 141 | Distance: 758.266845703125 142 | Enable Stereo Rendering: 143 | Stereo Eye Separation: 0.05999999865889549 144 | Stereo Focal Distance: 1 145 | Swap Stereo Eyes: false 146 | Value: false 147 | Focal Point: 148 | X: -229.94384765625 149 | Y: -173.66639709472656 150 | Z: 72.33602905273438 151 | Focal Shape Fixed Size: true 152 | Focal Shape Size: 0.05000000074505806 153 | Invert Z Axis: false 154 | Name: Current View 155 | Near Clip Distance: 0.009999999776482582 156 | Pitch: 1.2997965812683105 157 | Target Frame: 158 | Value: Orbit (rviz) 159 | Yaw: 0.9103978872299194 160 | Saved: ~ 161 | Window Geometry: 162 | Displays: 163 | collapsed: false 164 | Height: 846 165 | Hide Left Dock: false 166 | Hide Right Dock: false 167 | QMainWindow State: 000000ff00000000fd00000004000000000000017b0000028cfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000008200fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000520000028c0000011e00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001110000028cfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007301000000520000028c000000e000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005da00000045fc0100000002fb0000000800540069006d00650100000000000005da0000034f00fffffffb0000000800540069006d006501000000000000045000000000000000000000033c0000028c00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 168 | Selection: 169 | collapsed: false 170 | Time: 171 | collapsed: false 172 | Tool Properties: 173 | collapsed: false 174 | Views: 175 | collapsed: false 176 | Width: 1498 177 | X: 3505 178 | Y: 647 179 | -------------------------------------------------------------------------------- /estimator/src/estimator/trajectory_estimator.cpp: -------------------------------------------------------------------------------- 1 | #include "trajectory_estimator.h" 2 | 3 | namespace estimator{ 4 | 5 | TrajectoryEstimator::TrajectoryEstimator(Trajectory::Ptr trajectory, Camera::Ptr event_camera, Camera::Ptr convent_camera, CalibBoard::Ptr calib_board) 6 | :trajectory_(trajectory), event_camera_(event_camera), convent_camera_(convent_camera), calib_board_(calib_board) 7 | { 8 | problem_ = std::make_shared(DefaultProblemOptions()); 9 | problem_1 = std::make_shared(DefaultProblemOptions()); 10 | analytic_local_parameterization_ = new LieAnalyticLocalParameterization(); 11 | } 12 | 13 | void TrajectoryEstimator::addControlPoints( 14 | const SplineMeta &spline_meta, std::vector &vec, std::shared_ptr problem, 15 | bool addPosKnot, bool fixed){ 16 | for (auto const &seg : spline_meta.segments) 17 | { 18 | size_t start_idx = trajectory_->computeTIndexNs(seg.t0_ns).second; 19 | for (size_t i = start_idx; i < (start_idx + seg.NumParameters()); ++i){ 20 | if (addPosKnot){ 21 | vec.emplace_back(trajectory_->getKnotPos(i).data()); 22 | problem->AddParameterBlock(vec.back(), 3); 23 | if (fixed){ 24 | problem->SetParameterBlockConstant(vec.back()); 25 | } 26 | } 27 | else{ 28 | vec.emplace_back(trajectory_->getKnotSO3(i).data()); 29 | problem->AddParameterBlock(vec.back(), 4, analytic_local_parameterization_); 30 | if (fixed){ 31 | problem->SetParameterBlockConstant(vec.back()); 32 | } 33 | } 34 | } 35 | } 36 | } 37 | 38 | void TrajectoryEstimator::addConvFeature(const corner_msgs::corner &corner, ros::Time timestamp){ 39 | SplineMeta spline_meta; 40 | int64_t data_start_time = trajectory_->getDataStartTime(); 41 | trajectory_->CaculateSplineMeta({{timestamp.toSec() * S_TO_NS - data_start_time - max_time_delay * S_TO_NS, timestamp.toSec() * S_TO_NS - data_start_time + max_time_delay * S_TO_NS}}, spline_meta); 42 | ceres::CostFunction *cost_function_conv = new ConventFactor(corner, timestamp, spline_meta.segments.at(0), convent_camera_, calib_board_, data_start_time); 43 | 44 | std::vector vec; 45 | addControlPoints(spline_meta, vec, problem_1, false, true); // 4 * 4 46 | addControlPoints(spline_meta, vec, problem_1, true, true); // 4 * 4 + 4 * 3 47 | 48 | // add intrinsic & distortion 49 | vec.emplace_back(convent_camera_->intrinsicParams); 50 | problem_1->AddParameterBlock(convent_camera_->intrinsicParams, 4); 51 | vec.emplace_back(convent_camera_->distortParams); 52 | problem_1->AddParameterBlock(convent_camera_->distortParams, 5); 53 | 54 | // add extrinsic 55 | vec.emplace_back(convent_camera_->R_C2W.data()); // 4 * 7 + 9 + 4 56 | problem_1->AddParameterBlock(vec.back(), 4, analytic_local_parameterization_); 57 | vec.emplace_back(convent_camera_->t_C2W.data()); // 4 * 7 + 9 + 7 58 | problem_1->AddParameterBlock(vec.back(), 3); 59 | 60 | // add time delay 61 | vec.emplace_back(&convent_camera_->time_delay); // 4 * 7 + 9 + 7 + 1 = 45 dims 62 | problem_1->AddParameterBlock(vec.back(), 1); 63 | problem_1->SetParameterLowerBound(&convent_camera_->time_delay, 0, -max_time_delay); 64 | problem_1->SetParameterUpperBound(&convent_camera_->time_delay, 0, max_time_delay); 65 | 66 | problem_1->AddResidualBlock(cost_function_conv, new ceres::HuberLoss(2.0), vec); 67 | } 68 | 69 | void TrajectoryEstimator::addEventFeature(const circle_msgs::circle &circle){ 70 | SplineMeta spline_meta; 71 | int64_t data_start_time = trajectory_->getDataStartTime(); 72 | 73 | trajectory_->CaculateSplineMeta({{circle.timestamp.toSec() * S_TO_NS - data_start_time, circle.timestamp.toSec() * S_TO_NS - data_start_time}}, spline_meta); 74 | ceres::CostFunction *cost_function_ev = new EventFactor(circle, spline_meta.segments.at(0), event_camera_, calib_board_, data_start_time); 75 | 76 | std::vector vec; 77 | addControlPoints(spline_meta, vec, problem_, false); // 4 * 4 78 | addControlPoints(spline_meta, vec, problem_, true); // 4 * 4 + 4 * 3 79 | 80 | // add intrinsic & distortion 81 | vec.emplace_back(event_camera_->intrinsicParams); // 4 * 7 + 4 82 | problem_->AddParameterBlock(vec.back(), 4); 83 | vec.emplace_back(event_camera_->distortParams); // 4 * 7 + 4 + 5 84 | problem_->AddParameterBlock(vec.back(), 5); 85 | 86 | problem_->AddResidualBlock(cost_function_ev, NULL, vec); 87 | 88 | for (float ratio_st = 0.3; ratio_st < 1.0; ratio_st += 0.5){ 89 | SplineMeta spline_meta1; 90 | trajectory_->CaculateSplineMeta({{(circle.start_ts.toSec() + (circle.timestamp - circle.start_ts).toSec() * ratio_st) * S_TO_NS - data_start_time, (circle.start_ts.toSec() + (circle.timestamp - circle.start_ts).toSec() * ratio_st) * S_TO_NS - data_start_time}}, spline_meta1); 91 | ceres::CostFunction *cost_function_ev1 = new EventFactor(circle, spline_meta1.segments.at(0), event_camera_, calib_board_, data_start_time, 1, ratio_st); 92 | 93 | std::vector vec1; 94 | addControlPoints(spline_meta1, vec1, problem_, false); // 4 * 4 95 | addControlPoints(spline_meta1, vec1, problem_, true); // 4 * 4 + 4 * 3 96 | 97 | // add intrinsic & distortion 98 | vec1.emplace_back(event_camera_->intrinsicParams); // 4 * 7 + 4 99 | problem_->AddParameterBlock(vec1.back(), 4); 100 | vec1.emplace_back(event_camera_->distortParams); // 4 * 7 + 4 + 5 101 | problem_->AddParameterBlock(vec1.back(), 5); 102 | 103 | problem_->AddResidualBlock(cost_function_ev1, NULL, vec1); 104 | } 105 | 106 | for (float ratio_end = 0.3; ratio_end < 1.0; ratio_end += 0.5){ 107 | SplineMeta spline_meta2; 108 | 109 | trajectory_->CaculateSplineMeta({{(circle.end_ts.toSec() + (circle.timestamp - circle.end_ts).toSec() * ratio_end) * S_TO_NS - data_start_time, (circle.end_ts.toSec() + (circle.timestamp - circle.end_ts).toSec() * ratio_end) * S_TO_NS - data_start_time}}, spline_meta2); 110 | ceres::CostFunction *cost_function_ev2 = new EventFactor(circle, spline_meta2.segments.at(0), event_camera_, calib_board_, data_start_time, 2, ratio_end); 111 | 112 | std::vector vec2; 113 | addControlPoints(spline_meta2, vec2, problem_, false); // 4 * 4 114 | addControlPoints(spline_meta2, vec2, problem_, true); // 4 * 4 + 4 * 3 115 | 116 | // add intrinsic & distortion 117 | vec2.emplace_back(event_camera_->intrinsicParams); // 4 * 7 + 4 118 | problem_->AddParameterBlock(vec2.back(), 4); 119 | vec2.emplace_back(event_camera_->distortParams); // 4 * 7 + 4 + 5 120 | problem_->AddParameterBlock(vec2.back(), 5); 121 | 122 | problem_->AddResidualBlock(cost_function_ev2, NULL, vec2); 123 | } 124 | } 125 | 126 | ceres::Solver::Summary TrajectoryEstimator::solve(int max_iterations = 50, bool progress = false, int num_threads = 1){ 127 | ceres::Solver::Options options; 128 | 129 | options.minimizer_type = ceres::TRUST_REGION; 130 | options.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT; 131 | options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY; 132 | options.minimizer_progress_to_stdout = progress; 133 | options.max_num_iterations = num_threads; 134 | options.logging_type = ceres::PER_MINIMIZER_ITERATION; 135 | options.update_state_every_iteration = true; 136 | UpdateTrajectoryCallback callback(this); 137 | options.callbacks.push_back(&callback); 138 | 139 | if (num_threads < 1){ 140 | num_threads = 1; // std::thread::hardware_concurrency(); // mine is 8 141 | } 142 | options.num_threads = num_threads; 143 | options.max_num_iterations = max_iterations; 144 | 145 | ceres::Solver::Summary summary; 146 | ceres::Solve(options, problem_.get(), &summary); 147 | 148 | std::cout << summary.BriefReport() << std::endl; 149 | 150 | return summary; 151 | } 152 | 153 | ceres::Solver::Summary TrajectoryEstimator::solve1(int max_iterations = 50, bool progress = false, int num_threads = 1){ 154 | ceres::Solver::Options options; 155 | 156 | options.minimizer_type = ceres::TRUST_REGION; 157 | options.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT; 158 | options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY; 159 | options.minimizer_progress_to_stdout = progress; 160 | options.max_num_iterations = num_threads; 161 | options.logging_type = ceres::PER_MINIMIZER_ITERATION; 162 | options.parameter_tolerance = 1e-12; 163 | 164 | if (num_threads < 1){ 165 | num_threads = 1; // std::thread::hardware_concurrency(); // mine is 8 166 | } 167 | options.num_threads = num_threads; 168 | options.max_num_iterations = max_iterations; 169 | 170 | ceres::Solver::Summary summary; 171 | ceres::Solve(options, problem_1.get(), &summary); //old 172 | 173 | std::cout << summary.BriefReport() << std::endl; 174 | 175 | return summary; 176 | } 177 | 178 | 179 | 180 | } // namespace estimator -------------------------------------------------------------------------------- /estimator/src/estimator/trajectory_manager.cpp: -------------------------------------------------------------------------------- 1 | #include "trajectory_manager.h" 2 | 3 | namespace estimator{ 4 | 5 | TrajectoryManager::TrajectoryManager(Trajectory::Ptr trajectory) 6 | : trajectory_(trajectory) 7 | { 8 | 9 | } 10 | 11 | void TrajectoryManager::extendTrajectory(int64_t max_time, SE3d now_knot){ 12 | double max_bef = trajectory_->maxTimeNs() * NS_TO_S; 13 | max_bef_ns = trajectory_->maxTimeNs(); 14 | max_bef_idx = trajectory_->cpnum() - 1; 15 | 16 | trajectory_->extendKnotsTo(max_time, now_knot); // maxTime>=max_time 17 | double max_aft = trajectory_->maxTimeNs() * NS_TO_S; 18 | max_aft_idx = trajectory_->cpnum() - 1; 19 | } 20 | 21 | } //namespace estimator -------------------------------------------------------------------------------- /estimator/src/estimator_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * EF-Calib 3 | * Copyright (C) 2023 Shaoan Wang 4 | * 5 | * This program is free software: you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation, either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License 16 | * along with this program. If not, see . 17 | */ 18 | 19 | #include "estimator_manager.h" 20 | 21 | #include 22 | #include 23 | 24 | using namespace estimator; 25 | 26 | int main(int argc, char **argv) 27 | { 28 | google::InitGoogleLogging(argv[0]); 29 | 30 | ros::init(argc, argv, "estimator"); 31 | ros::NodeHandle nh("~"); 32 | 33 | std::string config_path; 34 | nh.param("config_path", config_path, "../config/setup.yaml"); 35 | ROS_INFO("Estimator load %s.", config_path.c_str()); 36 | YAML::Node config_node = YAML::LoadFile(config_path); 37 | std::string log_path = config_node["log_path"].as(); 38 | FLAGS_log_dir = log_path; 39 | FLAGS_colorlogtostderr = true; 40 | LOG(INFO) << "Start Estimator"; 41 | 42 | EstimatorManager estimator_manager(config_node, nh); 43 | 44 | ros::spin(); 45 | 46 | return 0; 47 | } 48 | 49 | -------------------------------------------------------------------------------- /estimator/src/sophus_lib/average.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SOPHUS_AVERAGE_HPP 2 | #define SOPHUS_AVERAGE_HPP 3 | 4 | #include "common.hpp" 5 | #include "rxso2.hpp" 6 | #include "rxso3.hpp" 7 | #include "se2.hpp" 8 | #include "se3.hpp" 9 | #include "sim2.hpp" 10 | #include "sim3.hpp" 11 | #include "so2.hpp" 12 | #include "so3.hpp" 13 | 14 | namespace Sophus { 15 | 16 | template 17 | optional iterativeMean( 18 | SequenceContainer const& foo_Ts_bar, int max_num_iterations) { 19 | size_t N = foo_Ts_bar.size(); 20 | SOPHUS_ENSURE(N >= 1, "N must be >= 1."); 21 | 22 | using Group = typename SequenceContainer::value_type; 23 | using Scalar = typename Group::Scalar; 24 | using Tangent = typename Group::Tangent; 25 | 26 | // This implements the algorithm in the beginning of Sec. 4.2 in 27 | // ftp://ftp-sop.inria.fr/epidaure/Publications/Arsigny/arsigny_rr_biinvariant_average.pdf. 28 | Group foo_T_average = foo_Ts_bar.front(); 29 | Scalar w = Scalar(1. / N); 30 | for (int i = 0; i < max_num_iterations; ++i) { 31 | Tangent average; 32 | setToZero(average); 33 | for (Group const& foo_T_bar : foo_Ts_bar) { 34 | average += w * (foo_T_average.inverse() * foo_T_bar).log(); 35 | } 36 | Group foo_T_newaverage = foo_T_average * Group::exp(average); 37 | if (squaredNorm( 38 | (foo_T_newaverage.inverse() * foo_T_average).log()) < 39 | Constants::epsilon()) { 40 | return foo_T_newaverage; 41 | } 42 | 43 | foo_T_average = foo_T_newaverage; 44 | } 45 | // LCOV_EXCL_START 46 | return nullopt; 47 | // LCOV_EXCL_STOP 48 | } 49 | 50 | // Mean implementation for SO(2). 51 | template 53 | enable_if_t< 54 | std::is_same>::value, 55 | optional> 56 | average(SequenceContainer const& foo_Ts_bar) { 57 | // This implements rotational part of Proposition 12 from Sec. 6.2 of 58 | // ftp://ftp-sop.inria.fr/epidaure/Publications/Arsigny/arsigny_rr_biinvariant_average.pdf. 59 | size_t N = std::distance(std::begin(foo_Ts_bar), std::end(foo_Ts_bar)); 60 | SOPHUS_ENSURE(N >= 1, "N must be >= 1."); 61 | SO2 foo_T_average = foo_Ts_bar.front(); 62 | Scalar w = Scalar(1. / N); 63 | 64 | Scalar average(0); 65 | for (SO2 const& foo_T_bar : foo_Ts_bar) { 66 | average += w * (foo_T_average.inverse() * foo_T_bar).log(); 67 | } 68 | return foo_T_average * SO2::exp(average); 69 | } 70 | 71 | // Mean implementation for RxSO(2). 72 | template 74 | enable_if_t< 75 | std::is_same>::value, 76 | optional> 77 | average(SequenceContainer const& foo_Ts_bar) { 78 | size_t N = std::distance(std::begin(foo_Ts_bar), std::end(foo_Ts_bar)); 79 | SOPHUS_ENSURE(N >= 1, "N must be >= 1."); 80 | RxSO2 foo_T_average = foo_Ts_bar.front(); 81 | Scalar w = Scalar(1. / N); 82 | 83 | Vector2 average(Scalar(0), Scalar(0)); 84 | for (RxSO2 const& foo_T_bar : foo_Ts_bar) { 85 | average += w * (foo_T_average.inverse() * foo_T_bar).log(); 86 | } 87 | return foo_T_average * RxSO2::exp(average); 88 | } 89 | 90 | namespace details { 91 | template 92 | void getQuaternion(T const&); 93 | 94 | template 95 | Eigen::Quaternion getUnitQuaternion(SO3 const& R) { 96 | return R.unit_quaternion(); 97 | } 98 | 99 | template 100 | Eigen::Quaternion getUnitQuaternion(RxSO3 const& sR) { 101 | return sR.so3().unit_quaternion(); 102 | } 103 | 104 | template 106 | Eigen::Quaternion averageUnitQuaternion( 107 | SequenceContainer const& foo_Ts_bar) { 108 | // This: http://stackoverflow.com/a/27410865/1221742 109 | size_t N = std::distance(std::begin(foo_Ts_bar), std::end(foo_Ts_bar)); 110 | SOPHUS_ENSURE(N >= 1, "N must be >= 1."); 111 | Eigen::Matrix Q(4, N); 112 | int i = 0; 113 | Scalar w = Scalar(1. / N); 114 | for (auto const& foo_T_bar : foo_Ts_bar) { 115 | Q.col(i) = w * details::getUnitQuaternion(foo_T_bar).coeffs(); 116 | ++i; 117 | } 118 | 119 | Eigen::Matrix QQt = Q * Q.transpose(); 120 | // TODO: Figure out why we can't use SelfAdjointEigenSolver here. 121 | Eigen::EigenSolver> es(QQt); 122 | 123 | std::complex max_eigenvalue = es.eigenvalues()[0]; 124 | Eigen::Matrix, 4, 1> max_eigenvector = 125 | es.eigenvectors().col(0); 126 | 127 | for (int i = 1; i < 4; i++) { 128 | if (std::norm(es.eigenvalues()[i]) > std::norm(max_eigenvalue)) { 129 | max_eigenvalue = es.eigenvalues()[i]; 130 | max_eigenvector = es.eigenvectors().col(i); 131 | } 132 | } 133 | Eigen::Quaternion quat; 134 | quat.coeffs() << // 135 | max_eigenvector[0].real(), // 136 | max_eigenvector[1].real(), // 137 | max_eigenvector[2].real(), // 138 | max_eigenvector[3].real(); 139 | return quat; 140 | } 141 | } // namespace details 142 | 143 | // Mean implementation for SO(3). 144 | // 145 | // TODO: Detect degenerated cases and return nullopt. 146 | template 148 | enable_if_t< 149 | std::is_same>::value, 150 | optional> 151 | average(SequenceContainer const& foo_Ts_bar) { 152 | return SO3(details::averageUnitQuaternion(foo_Ts_bar)); 153 | } 154 | 155 | // Mean implementation for R x SO(3). 156 | template 158 | enable_if_t< 159 | std::is_same>::value, 160 | optional> 161 | average(SequenceContainer const& foo_Ts_bar) { 162 | size_t N = std::distance(std::begin(foo_Ts_bar), std::end(foo_Ts_bar)); 163 | 164 | SOPHUS_ENSURE(N >= 1, "N must be >= 1."); 165 | Scalar scale_sum = Scalar(0); 166 | using std::exp; 167 | using std::log; 168 | for (RxSO3 const& foo_T_bar : foo_Ts_bar) { 169 | scale_sum += log(foo_T_bar.scale()); 170 | } 171 | return RxSO3(exp(scale_sum / Scalar(N)), 172 | SO3(details::averageUnitQuaternion(foo_Ts_bar))); 173 | } 174 | 175 | template 177 | enable_if_t< 178 | std::is_same>::value, 179 | optional> 180 | average(SequenceContainer const& foo_Ts_bar, int max_num_iterations = 20) { 181 | // TODO: Implement Proposition 12 from Sec. 6.2 of 182 | // ftp://ftp-sop.inria.fr/epidaure/Publications/Arsigny/arsigny_rr_biinvariant_average.pdf. 183 | return iterativeMean(foo_Ts_bar, max_num_iterations); 184 | } 185 | 186 | template 188 | enable_if_t< 189 | std::is_same>::value, 190 | optional> 191 | average(SequenceContainer const& foo_Ts_bar, int max_num_iterations = 20) { 192 | return iterativeMean(foo_Ts_bar, max_num_iterations); 193 | } 194 | 195 | template 197 | enable_if_t< 198 | std::is_same>::value, 199 | optional> 200 | average(SequenceContainer const& foo_Ts_bar, int max_num_iterations = 20) { 201 | return iterativeMean(foo_Ts_bar, max_num_iterations); 202 | } 203 | 204 | template 206 | enable_if_t< 207 | std::is_same>::value, 208 | optional> 209 | average(SequenceContainer const& foo_Ts_bar, int max_num_iterations = 20) { 210 | return iterativeMean(foo_Ts_bar, max_num_iterations); 211 | } 212 | 213 | } // namespace Sophus 214 | 215 | #endif // SOPHUS_AVERAGE_HPP 216 | -------------------------------------------------------------------------------- /estimator/src/sophus_lib/common.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SOPHUS_COMMON_HPP 2 | #define SOPHUS_COMMON_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | 13 | // following boost's assert.hpp 14 | #undef SOPHUS_ENSURE 15 | 16 | // ENSURES are similar to ASSERTS, but they are always checked for (including in 17 | // release builds). At the moment there are no ASSERTS in Sophus which should 18 | // only be used for checks which are performance critical. 19 | 20 | #ifdef __GNUC__ 21 | #define SOPHUS_FUNCTION __PRETTY_FUNCTION__ 22 | #elif (_MSC_VER >= 1310) 23 | #define SOPHUS_FUNCTION __FUNCTION__ 24 | #else 25 | #define SOPHUS_FUNCTION "unknown" 26 | #endif 27 | 28 | // Make sure this compiles with older versions of Eigen which do not have 29 | // EIGEN_DEVICE_FUNC defined. 30 | #ifndef EIGEN_DEVICE_FUNC 31 | #define EIGEN_DEVICE_FUNC 32 | #endif 33 | 34 | #define SOPHUS_FUNC EIGEN_DEVICE_FUNC 35 | 36 | namespace Sophus { 37 | namespace details { 38 | 39 | // Following: http://stackoverflow.com/a/22759544 40 | template 41 | class IsStreamable { 42 | private: 43 | template 44 | static auto test(int) 45 | -> decltype(std::declval() << std::declval(), 46 | std::true_type()); 47 | 48 | template 49 | static auto test(...) -> std::false_type; 50 | 51 | public: 52 | static bool const value = decltype(test(0))::value; 53 | }; 54 | 55 | template 56 | class ArgToStream { 57 | public: 58 | static void impl(std::stringstream& stream, T&& arg) { 59 | stream << std::forward(arg); 60 | } 61 | }; 62 | 63 | inline void FormatStream(std::stringstream& stream, char const* text) { 64 | stream << text; 65 | return; 66 | } 67 | 68 | // Following: http://en.cppreference.com/w/cpp/language/parameter_pack 69 | template 70 | void FormatStream(std::stringstream& stream, char const* text, T&& arg, 71 | Args&&... args) { 72 | static_assert(IsStreamable::value, 73 | "One of the args has no ostream overload!"); 74 | for (; *text != '\0'; ++text) { 75 | if (*text == '%') { 76 | ArgToStream::impl(stream, std::forward(arg)); 77 | FormatStream(stream, text + 1, std::forward(args)...); 78 | return; 79 | } 80 | stream << *text; 81 | } 82 | stream << "\nFormat-Warning: There are " << sizeof...(Args) + 1 83 | << " args unused."; 84 | return; 85 | } 86 | 87 | template 88 | std::string FormatString(char const* text, Args&&... args) { 89 | std::stringstream stream; 90 | FormatStream(stream, text, std::forward(args)...); 91 | return stream.str(); 92 | } 93 | 94 | inline std::string FormatString() { return std::string(); } 95 | } // namespace details 96 | } // namespace Sophus 97 | 98 | #if defined(SOPHUS_DISABLE_ENSURES) 99 | 100 | #define SOPHUS_ENSURE(expr, ...) ((void)0) 101 | 102 | #elif defined(SOPHUS_ENABLE_ENSURE_HANDLER) 103 | 104 | namespace Sophus { 105 | void ensureFailed(char const* function, char const* file, int line, 106 | char const* description); 107 | } 108 | 109 | #define SOPHUS_ENSURE(expr, ...) \ 110 | ((expr) ? ((void)0) \ 111 | : ::Sophus::ensureFailed( \ 112 | SOPHUS_FUNCTION, __FILE__, __LINE__, \ 113 | Sophus::details::FormatString(##__VA_ARGS__).c_str())) 114 | #else 115 | // LCOV_EXCL_START 116 | 117 | namespace Sophus { 118 | template 119 | SOPHUS_FUNC void defaultEnsure(char const* function, char const* file, int line, 120 | char const* description, Args&&... args) { 121 | std::printf("Sophus ensure failed in function '%s', file '%s', line %d.\n", 122 | function, file, line); 123 | #ifdef __CUDACC__ 124 | std::printf("%s", description); 125 | #else 126 | std::cout << details::FormatString(description, std::forward(args)...) 127 | << std::endl; 128 | std::abort(); 129 | #endif 130 | } 131 | } // namespace Sophus 132 | 133 | // LCOV_EXCL_STOP 134 | #define SOPHUS_ENSURE(expr, ...) \ 135 | ((expr) ? ((void)0) \ 136 | : Sophus::defaultEnsure(SOPHUS_FUNCTION, __FILE__, __LINE__, \ 137 | ##__VA_ARGS__)) 138 | #endif 139 | 140 | namespace Sophus { 141 | 142 | template 143 | struct Constants { 144 | SOPHUS_FUNC static Scalar epsilon() { return Scalar(1e-10); } 145 | 146 | SOPHUS_FUNC static Scalar epsilonSqrt() { 147 | using std::sqrt; 148 | return sqrt(epsilon()); 149 | } 150 | 151 | SOPHUS_FUNC static Scalar pi() { return Scalar(M_PI); } 152 | }; 153 | 154 | template <> 155 | struct Constants { 156 | SOPHUS_FUNC static float constexpr epsilon() { 157 | return static_cast(1e-5); 158 | } 159 | 160 | SOPHUS_FUNC static float epsilonSqrt() { return std::sqrt(epsilon()); } 161 | 162 | SOPHUS_FUNC static float constexpr pi() { return static_cast(M_PI); } 163 | }; 164 | 165 | // Leightweight optional implementation which require ``T`` to have a 166 | // default constructor. 167 | // 168 | // TODO: Replace with std::optional once Sophus moves to c++17. 169 | // 170 | struct nullopt_t { 171 | explicit constexpr nullopt_t() {} 172 | }; 173 | 174 | constexpr nullopt_t nullopt{}; 175 | template 176 | 177 | class optional { 178 | public: 179 | optional() : is_valid_(false) {} 180 | 181 | optional(nullopt_t) : is_valid_(false) {} 182 | 183 | optional(T const& type) : type_(type), is_valid_(true) {} 184 | 185 | explicit operator bool() const { return is_valid_; } 186 | 187 | T const* operator->() const { 188 | SOPHUS_ENSURE(is_valid_, "must be valid"); 189 | return &type_; 190 | } 191 | 192 | T* operator->() { 193 | SOPHUS_ENSURE(is_valid_, "must be valid"); 194 | return &type_; 195 | } 196 | 197 | T const& operator*() const { 198 | SOPHUS_ENSURE(is_valid_, "must be valid"); 199 | return type_; 200 | } 201 | 202 | T& operator*() { 203 | SOPHUS_ENSURE(is_valid_, "must be valid"); 204 | return type_; 205 | } 206 | 207 | private: 208 | T type_; 209 | bool is_valid_; 210 | }; 211 | 212 | template 213 | using enable_if_t = typename std::enable_if::type; 214 | 215 | template 216 | struct IsUniformRandomBitGenerator { 217 | static const bool value = std::is_unsigned::value && 218 | std::is_unsigned::value && 219 | std::is_unsigned::value; 220 | }; 221 | } // namespace Sophus 222 | 223 | #endif // SOPHUS_COMMON_HPP 224 | -------------------------------------------------------------------------------- /estimator/src/sophus_lib/geometry.hpp: -------------------------------------------------------------------------------- 1 | #ifndef GEOMETRY_HPP 2 | #define GEOMETRY_HPP 3 | 4 | #include "se2.hpp" 5 | #include "se3.hpp" 6 | #include "so2.hpp" 7 | #include "so3.hpp" 8 | #include "types.hpp" 9 | 10 | namespace Sophus { 11 | 12 | // Takes in a rotation ``R_foo_plane`` and returns the corresponding line 13 | // normal along the y-axis (in reference frame ``foo``). 14 | // 15 | template 16 | Vector2 normalFromSO2(SO2 const& R_foo_line) { 17 | return R_foo_line.matrix().col(1); 18 | } 19 | 20 | // Takes in line normal in reference frame foo and constructs a corresponding 21 | // rotation matrix ``R_foo_line``. 22 | // 23 | // Precondition: ``normal_foo`` must not be close to zero. 24 | // 25 | template 26 | SO2 SO2FromNormal(Vector2 normal_foo) { 27 | SOPHUS_ENSURE(normal_foo.squaredNorm() > Constants::epsilon(), "%", 28 | normal_foo.transpose()); 29 | normal_foo.normalize(); 30 | return SO2(normal_foo.y(), -normal_foo.x()); 31 | } 32 | 33 | // Takes in a rotation ``R_foo_plane`` and returns the corresponding plane 34 | // normal along the z-axis 35 | // (in reference frame ``foo``). 36 | // 37 | template 38 | Vector3 normalFromSO3(SO3 const& R_foo_plane) { 39 | return R_foo_plane.matrix().col(2); 40 | } 41 | 42 | // Takes in plane normal in reference frame foo and constructs a corresponding 43 | // rotation matrix ``R_foo_plane``. 44 | // 45 | // Note: The ``plane`` frame is defined as such that the normal points along the 46 | // positive z-axis. One can specify hints for the x-axis and y-axis of the 47 | // ``plane`` frame. 48 | // 49 | // Preconditions: 50 | // - ``normal_foo``, ``xDirHint_foo``, ``yDirHint_foo`` must not be close to 51 | // zero. 52 | // - ``xDirHint_foo`` and ``yDirHint_foo`` must be approx. perpendicular. 53 | // 54 | template 55 | Matrix3 rotationFromNormal(Vector3 const& normal_foo, 56 | Vector3 xDirHint_foo = Vector3(T(1), T(0), 57 | T(0)), 58 | Vector3 yDirHint_foo = Vector3(T(0), T(1), 59 | T(0))) { 60 | SOPHUS_ENSURE(xDirHint_foo.dot(yDirHint_foo) < Constants::epsilon(), 61 | "xDirHint (%) and yDirHint (%) must be perpendicular.", 62 | xDirHint_foo.transpose(), yDirHint_foo.transpose()); 63 | using std::abs; 64 | using std::sqrt; 65 | T const xDirHint_foo_sqr_length = xDirHint_foo.squaredNorm(); 66 | T const yDirHint_foo_sqr_length = yDirHint_foo.squaredNorm(); 67 | T const normal_foo_sqr_length = normal_foo.squaredNorm(); 68 | SOPHUS_ENSURE(xDirHint_foo_sqr_length > Constants::epsilon(), "%", 69 | xDirHint_foo.transpose()); 70 | SOPHUS_ENSURE(yDirHint_foo_sqr_length > Constants::epsilon(), "%", 71 | yDirHint_foo.transpose()); 72 | SOPHUS_ENSURE(normal_foo_sqr_length > Constants::epsilon(), "%", 73 | normal_foo.transpose()); 74 | 75 | Matrix3 basis_foo; 76 | basis_foo.col(2) = normal_foo; 77 | 78 | if (abs(xDirHint_foo_sqr_length - T(1)) > Constants::epsilon()) { 79 | xDirHint_foo.normalize(); 80 | } 81 | if (abs(yDirHint_foo_sqr_length - T(1)) > Constants::epsilon()) { 82 | yDirHint_foo.normalize(); 83 | } 84 | if (abs(normal_foo_sqr_length - T(1)) > Constants::epsilon()) { 85 | basis_foo.col(2).normalize(); 86 | } 87 | 88 | T abs_x_dot_z = abs(basis_foo.col(2).dot(xDirHint_foo)); 89 | T abs_y_dot_z = abs(basis_foo.col(2).dot(yDirHint_foo)); 90 | if (abs_x_dot_z < abs_y_dot_z) { 91 | // basis_foo.z and xDirHint are far from parallel. 92 | basis_foo.col(1) = basis_foo.col(2).cross(xDirHint_foo).normalized(); 93 | basis_foo.col(0) = basis_foo.col(1).cross(basis_foo.col(2)); 94 | } else { 95 | // basis_foo.z and yDirHint are far from parallel. 96 | basis_foo.col(0) = yDirHint_foo.cross(basis_foo.col(2)).normalized(); 97 | basis_foo.col(1) = basis_foo.col(2).cross(basis_foo.col(0)); 98 | } 99 | T det = basis_foo.determinant(); 100 | // sanity check 101 | SOPHUS_ENSURE(abs(det - T(1)) < Constants::epsilon(), 102 | "Determinant of basis is not 1, but %. Basis is \n%\n", det, 103 | basis_foo); 104 | return basis_foo; 105 | } 106 | 107 | // Takes in plane normal in reference frame foo and constructs a corresponding 108 | // rotation matrix ``R_foo_plane``. 109 | // 110 | // See ``rotationFromNormal`` for details. 111 | // 112 | template 113 | SO3 SO3FromNormal(Vector3 const& normal_foo) { 114 | return SO3(rotationFromNormal(normal_foo)); 115 | } 116 | 117 | // Returns a line (wrt. to frame ``foo``), given a pose of the ``line`` in 118 | // reference frame ``foo``. 119 | // 120 | // Note: The plane is defined by X-axis of the ``line`` frame. 121 | // 122 | template 123 | Line2 lineFromSE2(SE2 const& T_foo_line) { 124 | return Line2(normalFromSO2(T_foo_line.so2()), T_foo_line.translation()); 125 | } 126 | 127 | // Returns the pose ``T_foo_line``, given a line in reference frame ``foo``. 128 | // 129 | // Note: The line is defined by X-axis of the frame ``line``. 130 | // 131 | template 132 | SE2 SE2FromLine(Line2 const& line_foo) { 133 | T const d = line_foo.offset(); 134 | Vector2 const n = line_foo.normal(); 135 | SO2 const R_foo_plane = SO2FromNormal(n); 136 | return SE2(R_foo_plane, -d * n); 137 | } 138 | 139 | // Returns a plane (wrt. to frame ``foo``), given a pose of the ``plane`` in 140 | // reference frame ``foo``. 141 | // 142 | // Note: The plane is defined by XY-plane of the frame ``plane``. 143 | // 144 | template 145 | Plane3 planeFromSE3(SE3 const& T_foo_plane) { 146 | return Plane3(normalFromSO3(T_foo_plane.so3()), T_foo_plane.translation()); 147 | } 148 | 149 | // Returns the pose ``T_foo_plane``, given a plane in reference frame ``foo``. 150 | // 151 | // Note: The plane is defined by XY-plane of the frame ``plane``. 152 | // 153 | template 154 | SE3 SE3FromPlane(Plane3 const& plane_foo) { 155 | T const d = plane_foo.offset(); 156 | Vector3 const n = plane_foo.normal(); 157 | SO3 const R_foo_plane = SO3FromNormal(n); 158 | return SE3(R_foo_plane, -d * n); 159 | } 160 | 161 | // Takes in a hyperplane and returns unique representation by ensuring that the 162 | // ``offset`` is not negative. 163 | // 164 | template 165 | Eigen::Hyperplane makeHyperplaneUnique( 166 | Eigen::Hyperplane const& plane) { 167 | if (plane.offset() >= 0) { 168 | return plane; 169 | } 170 | 171 | return Eigen::Hyperplane(-plane.normal(), -plane.offset()); 172 | } 173 | 174 | } // namespace Sophus 175 | 176 | #endif // GEOMETRY_HPP 177 | -------------------------------------------------------------------------------- /estimator/src/sophus_lib/interpolate.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SOPHUS_INTERPOLATE_HPP 2 | #define SOPHUS_INTERPOLATE_HPP 3 | 4 | #include 5 | 6 | #include "interpolate_details.hpp" 7 | 8 | namespace Sophus { 9 | 10 | // This function interpolates between two Lie group elements ``foo_T_bar`` 11 | // and ``foo_T_baz`` with an interpolation factor of ``alpha`` in [0, 1]. 12 | // 13 | // It returns a pose ``foo_T_quiz`` with ``quiz`` being a frame between ``bar`` 14 | // and ``baz``. If ``alpha=0`` it returns ``foo_T_bar``. If it is 1, it returns 15 | // ``foo_T_bar``. 16 | // 17 | // (Since interpolation on Lie groups is inverse-invariant, we can equivalently 18 | // think of the input arguments as being ``bar_T_foo``, ``baz_T_foo`` and the 19 | // return value being ``quiz_T_foo``.) 20 | // 21 | // Precondition: ``p`` must be in [0, 1]. 22 | // 23 | template 24 | enable_if_t::supported, G> interpolate( 25 | G const& foo_T_bar, G const& foo_T_baz, Scalar2 p = Scalar2(0.5f)) { 26 | using Scalar = typename G::Scalar; 27 | Scalar inter_p(p); 28 | SOPHUS_ENSURE(inter_p >= Scalar(0) && inter_p <= Scalar(1), 29 | "p (%) must in [0, 1]."); 30 | return foo_T_bar * G::exp(inter_p * (foo_T_bar.inverse() * foo_T_baz).log()); 31 | } 32 | 33 | } // namespace Sophus 34 | 35 | #endif // SOPHUS_INTERPOLATE_HPP 36 | -------------------------------------------------------------------------------- /estimator/src/sophus_lib/interpolate_details.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SOPHUS_INTERPOLATE_DETAILS_HPP 2 | #define SOPHUS_INTERPOLATE_DETAILS_HPP 3 | 4 | #include "rxso2.hpp" 5 | #include "rxso3.hpp" 6 | #include "se2.hpp" 7 | #include "se3.hpp" 8 | #include "sim2.hpp" 9 | #include "sim3.hpp" 10 | #include "so2.hpp" 11 | #include "so3.hpp" 12 | 13 | namespace Sophus { 14 | namespace interp_details { 15 | 16 | template 17 | struct Traits; 18 | 19 | template 20 | struct Traits> { 21 | static bool constexpr supported = true; 22 | 23 | static bool hasShortestPathAmbiguity(SO2 const& foo_T_bar) { 24 | using std::abs; 25 | Scalar angle = foo_T_bar.log(); 26 | return abs(abs(angle) - Constants::pi()) < 27 | Constants::epsilon(); 28 | } 29 | }; 30 | 31 | template 32 | struct Traits> { 33 | static bool constexpr supported = true; 34 | 35 | static bool hasShortestPathAmbiguity(RxSO2 const& foo_T_bar) { 36 | return Traits>::hasShortestPathAmbiguity(foo_T_bar.so2()); 37 | } 38 | }; 39 | 40 | template 41 | struct Traits> { 42 | static bool constexpr supported = true; 43 | 44 | static bool hasShortestPathAmbiguity(SO3 const& foo_T_bar) { 45 | using std::abs; 46 | Scalar angle = foo_T_bar.logAndTheta().theta; 47 | return abs(abs(angle) - Constants::pi()) < 48 | Constants::epsilon(); 49 | } 50 | }; 51 | 52 | template 53 | struct Traits> { 54 | static bool constexpr supported = true; 55 | 56 | static bool hasShortestPathAmbiguity(RxSO3 const& foo_T_bar) { 57 | return Traits>::hasShortestPathAmbiguity(foo_T_bar.so3()); 58 | } 59 | }; 60 | 61 | template 62 | struct Traits> { 63 | static bool constexpr supported = true; 64 | 65 | static bool hasShortestPathAmbiguity(SE2 const& foo_T_bar) { 66 | return Traits>::hasShortestPathAmbiguity(foo_T_bar.so2()); 67 | } 68 | }; 69 | 70 | template 71 | struct Traits> { 72 | static bool constexpr supported = true; 73 | 74 | static bool hasShortestPathAmbiguity(SE3 const& foo_T_bar) { 75 | return Traits>::hasShortestPathAmbiguity(foo_T_bar.so3()); 76 | } 77 | }; 78 | 79 | template 80 | struct Traits> { 81 | static bool constexpr supported = true; 82 | 83 | static bool hasShortestPathAmbiguity(Sim2 const& foo_T_bar) { 84 | return Traits>::hasShortestPathAmbiguity( 85 | foo_T_bar.rxso2().so2()); 86 | ; 87 | } 88 | }; 89 | 90 | template 91 | struct Traits> { 92 | static bool constexpr supported = true; 93 | 94 | static bool hasShortestPathAmbiguity(Sim3 const& foo_T_bar) { 95 | return Traits>::hasShortestPathAmbiguity( 96 | foo_T_bar.rxso3().so3()); 97 | ; 98 | } 99 | }; 100 | 101 | } // namespace details 102 | } // namespace Sophus 103 | 104 | #endif // SOPHUS_INTERPOLATE_DETAILS_HPP 105 | -------------------------------------------------------------------------------- /estimator/src/sophus_lib/num_diff.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SOPHUS_NUM_DIFF_HPP 2 | #define SOPHUS_NUM_DIFF_HPP 3 | // Numerical differentiation using finite differences 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include "types.hpp" 10 | 11 | namespace Sophus { 12 | 13 | namespace details { 14 | template 15 | class Curve { 16 | public: 17 | template 18 | static auto num_diff(Fn curve, Scalar t, Scalar h) -> decltype(curve(t)) { 19 | using ReturnType = decltype(curve(t)); 20 | static_assert(std::is_floating_point::value, 21 | "Scalar must be a floating point type."); 22 | static_assert(IsFloatingPoint::value, 23 | "ReturnType must be either a floating point scalar, " 24 | "vector or matrix."); 25 | 26 | return (curve(t + h) - curve(t - h)) / (Scalar(2) * h); 27 | } 28 | }; 29 | 30 | template 31 | class VectorField { 32 | public: 33 | static Eigen::Matrix num_diff( 34 | std::function(Sophus::Vector)> 35 | vector_field, 36 | Sophus::Vector const& a, Scalar eps) { 37 | static_assert(std::is_floating_point::value, 38 | "Scalar must be a floating point type."); 39 | Eigen::Matrix J; 40 | Sophus::Vector h; 41 | h.setZero(); 42 | for (int i = 0; i < M; ++i) { 43 | h[i] = eps; 44 | J.row(i) = 45 | (vector_field(a + h) - vector_field(a - h)) / (Scalar(2) * eps); 46 | h[i] = Scalar(0); 47 | } 48 | 49 | return J; 50 | } 51 | }; 52 | 53 | template 54 | class VectorField { 55 | public: 56 | static Eigen::Matrix num_diff( 57 | std::function(Scalar)> vector_field, 58 | Scalar const& a, Scalar eps) { 59 | return details::Curve::num_diff(std::move(vector_field), a, eps); 60 | } 61 | }; 62 | } // namespace details 63 | 64 | // Calculates the derivative of a curve at a point ``t``. 65 | // 66 | // Here, a curve is a function from a Scalar to a Euclidean space. Thus, it 67 | // returns either a Scalar, a vector or a matrix. 68 | // 69 | template 70 | auto curveNumDiff(Fn curve, Scalar t, 71 | Scalar h = Constants::epsilonSqrt()) 72 | -> decltype(details::Curve::num_diff(std::move(curve), t, h)) { 73 | return details::Curve::num_diff(std::move(curve), t, h); 74 | } 75 | 76 | // Calculates the derivative of a vector field at a point ``a``. 77 | // 78 | // Here, a vector field is a function from a vector space to another vector 79 | // space. 80 | // 81 | template 82 | Eigen::Matrix vectorFieldNumDiff( 83 | Fn vector_field, ScalarOrVector const& a, 84 | Scalar eps = Constants::epsilonSqrt()) { 85 | return details::VectorField::num_diff(std::move(vector_field), 86 | a, eps); 87 | } 88 | 89 | } // namespace Sophus 90 | 91 | #endif // SOPHUS_NUM_DIFF_HPP 92 | -------------------------------------------------------------------------------- /estimator/src/sophus_lib/rotation_matrix.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SOPHUS_ROTATION_MATRIX_HPP 2 | #define SOPHUS_ROTATION_MATRIX_HPP 3 | 4 | #include 5 | #include 6 | 7 | #include "types.hpp" 8 | 9 | namespace Sophus { 10 | 11 | // Takes in arbiray square matrix and returns true if it is 12 | // orthogonal. 13 | template 14 | SOPHUS_FUNC bool isOrthogonal(Eigen::MatrixBase const& R) { 15 | using Scalar = typename D::Scalar; 16 | static int const N = D::RowsAtCompileTime; 17 | static int const M = D::ColsAtCompileTime; 18 | 19 | static_assert(N == M, "must be a square matrix"); 20 | static_assert(N >= 2, "must have compile time dimension >= 2"); 21 | 22 | return (R * R.transpose() - Matrix::Identity()).norm() < 23 | Constants::epsilon(); 24 | } 25 | 26 | // Takes in arbiray square matrix and returns true if it is 27 | // "scaled-orthogonal" with positive determinant. 28 | // 29 | template 30 | SOPHUS_FUNC bool isScaledOrthogonalAndPositive(Eigen::MatrixBase const& sR) { 31 | using Scalar = typename D::Scalar; 32 | static int const N = D::RowsAtCompileTime; 33 | static int const M = D::ColsAtCompileTime; 34 | using std::pow; 35 | using std::sqrt; 36 | 37 | Scalar det = sR.determinant(); 38 | 39 | if (det <= Scalar(0)) { 40 | return false; 41 | } 42 | 43 | Scalar scale_sqr = pow(det, Scalar(2. / N)); 44 | 45 | static_assert(N == M, "must be a square matrix"); 46 | static_assert(N >= 2, "must have compile time dimension >= 2"); 47 | 48 | return (sR * sR.transpose() - scale_sqr * Matrix::Identity()) 49 | .template lpNorm() < 50 | sqrt(Constants::epsilon()); 51 | } 52 | 53 | // Takes in arbiray square matrix (2x2 or larger) and returns closest 54 | // orthogonal matrix with positive determinant. 55 | template 56 | SOPHUS_FUNC enable_if_t< 57 | std::is_floating_point::value, 58 | Matrix> 59 | makeRotationMatrix(Eigen::MatrixBase const& R) { 60 | using Scalar = typename D::Scalar; 61 | static int const N = D::RowsAtCompileTime; 62 | static int const M = D::ColsAtCompileTime; 63 | 64 | static_assert(N == M, "must be a square matrix"); 65 | static_assert(N >= 2, "must have compile time dimension >= 2"); 66 | 67 | Eigen::JacobiSVD> svd( 68 | R, Eigen::ComputeFullU | Eigen::ComputeFullV); 69 | 70 | // Determine determinant of orthogonal matrix U*V'. 71 | Scalar d = (svd.matrixU() * svd.matrixV().transpose()).determinant(); 72 | // Starting from the identity matrix D, set the last entry to d (+1 or 73 | // -1), so that det(U*D*V') = 1. 74 | Matrix Diag = Matrix::Identity(); 75 | Diag(N - 1, N - 1) = d; 76 | return svd.matrixU() * Diag * svd.matrixV().transpose(); 77 | } 78 | 79 | } // namespace Sophus 80 | 81 | #endif // SOPHUS_ROTATION_MATRIX_HPP 82 | -------------------------------------------------------------------------------- /estimator/src/sophus_lib/sim_details.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SOPHUS_SIM_DETAILS_HPP 2 | #define SOPHUS_SIM_DETAILS_HPP 3 | 4 | #include "types.hpp" 5 | 6 | namespace Sophus { 7 | namespace details { 8 | 9 | template 10 | Matrix calcW(Matrix const& Omega, 11 | Scalar const theta, Scalar const sigma) { 12 | using std::abs; 13 | using std::exp; 14 | using std::sin; 15 | using std::cos; 16 | static Matrix const I = Matrix::Identity(); 17 | static Scalar const one(1); 18 | static Scalar const half(0.5); 19 | Matrix const Omega2 = Omega * Omega; 20 | Scalar const scale = exp(sigma); 21 | Scalar A, B, C; 22 | if (abs(sigma) < Constants::epsilon()) { 23 | C = one; 24 | if (abs(theta) < Constants::epsilon()) { 25 | A = half; 26 | B = Scalar(1. / 6.); 27 | } else { 28 | Scalar theta_sq = theta * theta; 29 | A = (one - cos(theta)) / theta_sq; 30 | B = (theta - sin(theta)) / (theta_sq * theta); 31 | } 32 | } else { 33 | C = (scale - one) / sigma; 34 | if (abs(theta) < Constants::epsilon()) { 35 | Scalar sigma_sq = sigma * sigma; 36 | A = ((sigma - one) * scale + one) / sigma_sq; 37 | B = (scale * half * sigma_sq + scale - one - sigma * scale) / 38 | (sigma_sq * sigma); 39 | } else { 40 | Scalar theta_sq = theta * theta; 41 | Scalar a = scale * sin(theta); 42 | Scalar b = scale * cos(theta); 43 | Scalar c = theta_sq + sigma * sigma; 44 | A = (a * sigma + (one - b) * theta) / (theta * c); 45 | B = (C - ((b - one) * sigma + a * theta) / (c)) * one / (theta_sq); 46 | } 47 | } 48 | return A * Omega + B * Omega2 + C * I; 49 | } 50 | 51 | template 52 | Matrix calcWInv(Matrix const& Omega, 53 | Scalar const theta, Scalar const sigma, 54 | Scalar const scale) { 55 | using std::abs; 56 | using std::sin; 57 | using std::cos; 58 | static Matrix const I = Matrix::Identity(); 59 | static Scalar const half(0.5); 60 | static Scalar const one(1); 61 | static Scalar const two(2); 62 | Matrix const Omega2 = Omega * Omega; 63 | Scalar const scale_sq = scale * scale; 64 | Scalar const theta_sq = theta * theta; 65 | Scalar const sin_theta = sin(theta); 66 | Scalar const cos_theta = cos(theta); 67 | 68 | Scalar a, b, c; 69 | if (abs(sigma * sigma) < Constants::epsilon()) { 70 | c = one - half * sigma; 71 | a = -half; 72 | if (abs(theta_sq) < Constants::epsilon()) { 73 | b = Scalar(1. / 12.); 74 | } else { 75 | b = (theta * sin_theta + two * cos_theta - two) / 76 | (two * theta_sq * (cos_theta - one)); 77 | } 78 | } else { 79 | Scalar const scale_cu = scale_sq * scale; 80 | c = sigma / (scale - one); 81 | if (abs(theta_sq) < Constants::epsilon()) { 82 | a = (-sigma * scale + scale - one) / ((scale - one) * (scale - one)); 83 | b = (scale_sq * sigma - two * scale_sq + scale * sigma + two * scale) / 84 | (two * scale_cu - Scalar(6) * scale_sq + Scalar(6) * scale - two); 85 | } else { 86 | Scalar const s_sin_theta = scale * sin_theta; 87 | Scalar const s_cos_theta = scale * cos_theta; 88 | a = (theta * s_cos_theta - theta - sigma * s_sin_theta) / 89 | (theta * (scale_sq - two * s_cos_theta + one)); 90 | b = -scale * 91 | (theta * s_sin_theta - theta * sin_theta + sigma * s_cos_theta - 92 | scale * sigma + sigma * cos_theta - sigma) / 93 | (theta_sq * (scale_cu - two * scale * s_cos_theta - scale_sq + 94 | two * s_cos_theta + scale - one)); 95 | } 96 | } 97 | return a * Omega + b * Omega2 + c * I; 98 | } 99 | 100 | } // details 101 | } // Sophus 102 | 103 | #endif 104 | -------------------------------------------------------------------------------- /estimator/src/sophus_lib/types.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SOPHUS_TYEPES_HPP 2 | #define SOPHUS_TYEPES_HPP 3 | 4 | #include "common.hpp" 5 | 6 | namespace Sophus { 7 | 8 | template 9 | using Vector = Eigen::Matrix; 10 | 11 | template 12 | using Vector2 = Vector; 13 | using Vector2f = Vector2; 14 | using Vector2d = Vector2; 15 | 16 | template 17 | using Vector3 = Vector; 18 | using Vector3f = Vector3; 19 | using Vector3d = Vector3; 20 | 21 | template 22 | using Vector4 = Vector; 23 | using Vector4f = Vector4; 24 | using Vector4d = Vector4; 25 | 26 | template 27 | using Vector6 = Vector; 28 | using Vector6f = Vector6; 29 | using Vector6d = Vector6; 30 | 31 | template 32 | using Vector7 = Vector; 33 | using Vector7f = Vector7; 34 | using Vector7d = Vector7; 35 | 36 | template 37 | using Matrix = Eigen::Matrix; 38 | 39 | template 40 | using Matrix2 = Matrix; 41 | using Matrix2f = Matrix2; 42 | using Matrix2d = Matrix2; 43 | 44 | template 45 | using Matrix3 = Matrix; 46 | using Matrix3f = Matrix3; 47 | using Matrix3d = Matrix3; 48 | 49 | template 50 | using Matrix4 = Matrix; 51 | using Matrix4f = Matrix4; 52 | using Matrix4d = Matrix4; 53 | 54 | template 55 | using Matrix6 = Matrix; 56 | using Matrix6f = Matrix6; 57 | using Matrix6d = Matrix6; 58 | 59 | template 60 | using Matrix7 = Matrix; 61 | using Matrix7f = Matrix7; 62 | using Matrix7d = Matrix7; 63 | 64 | template 65 | using ParametrizedLine = Eigen::ParametrizedLine; 66 | 67 | template 68 | using ParametrizedLine3 = ParametrizedLine; 69 | using ParametrizedLine3f = ParametrizedLine3; 70 | using ParametrizedLine3d = ParametrizedLine3; 71 | 72 | template 73 | using ParametrizedLine2 = ParametrizedLine; 74 | using ParametrizedLine2f = ParametrizedLine2; 75 | using ParametrizedLine2d = ParametrizedLine2; 76 | 77 | namespace details { 78 | template 79 | class MaxMetric { 80 | public: 81 | static Scalar impl(Scalar s0, Scalar s1) { 82 | using std::abs; 83 | return abs(s0 - s1); 84 | } 85 | }; 86 | 87 | template 88 | class MaxMetric> { 89 | public: 90 | static Scalar impl(Matrix const& p0, 91 | Matrix const& p1) { 92 | return (p0 - p1).template lpNorm(); 93 | } 94 | }; 95 | 96 | template 97 | class SetToZero { 98 | public: 99 | static void impl(Scalar& s) { s = Scalar(0); } 100 | }; 101 | 102 | template 103 | class SetToZero> { 104 | public: 105 | static void impl(Matrix& v) { v.setZero(); } 106 | }; 107 | 108 | template 109 | class SetElementAt; 110 | 111 | template 112 | class SetElementAt { 113 | public: 114 | static void impl(Scalar& s, Scalar value, int at) { 115 | SOPHUS_ENSURE(at == 0, "is %", at); 116 | s = value; 117 | } 118 | }; 119 | 120 | template 121 | class SetElementAt, Scalar> { 122 | public: 123 | static void impl(Vector& v, Scalar value, int at) { 124 | SOPHUS_ENSURE(at >= 0 && at < N, "is %", at); 125 | v[at] = value; 126 | } 127 | }; 128 | 129 | template 130 | class SquaredNorm { 131 | public: 132 | static Scalar impl(Scalar const& s) { return s * s; } 133 | }; 134 | 135 | template 136 | class SquaredNorm> { 137 | public: 138 | static Scalar impl(Matrix const& s) { return s.squaredNorm(); } 139 | }; 140 | 141 | template 142 | class Transpose { 143 | public: 144 | static Scalar impl(Scalar const& s) { return s; } 145 | }; 146 | 147 | template 148 | class Transpose> { 149 | public: 150 | static Matrix impl(Matrix const& s) { 151 | return s.transpose(); 152 | } 153 | }; 154 | } // namespace details 155 | 156 | // Returns maximum metric between two points ``p0`` and ``p1``, with ``p`` 157 | // being a matrix or a scalar. 158 | // 159 | template 160 | auto maxMetric(T const& p0, T const& p1) 161 | -> decltype(details::MaxMetric::impl(p0, p1)) { 162 | return details::MaxMetric::impl(p0, p1); 163 | } 164 | 165 | // Sets point ``p`` to zero, with ``p`` being a matrix or a scalar. 166 | // 167 | template 168 | void setToZero(T& p) { 169 | return details::SetToZero::impl(p); 170 | } 171 | 172 | // Sets point ``p`` to zero, with ``p`` being a matrix or a scalar. 173 | // 174 | template 175 | void setElementAt(T& p, Scalar value, int at) { 176 | return details::SetElementAt::impl(p, value, at); 177 | } 178 | 179 | // Returns the squared 2-norm of ``p``, with ``p`` being a vector or a scalar. 180 | // 181 | template 182 | auto squaredNorm(T const& p) -> decltype(details::SquaredNorm::impl(p)) { 183 | return details::SquaredNorm::impl(p); 184 | } 185 | 186 | // Returns ``p.transpose()`` if ``p`` is a matrix, and simply ``p`` if m is a 187 | // scalar. 188 | // 189 | template 190 | auto transpose(T const& p) -> decltype(details::Transpose::impl(T())) { 191 | return details::Transpose::impl(p); 192 | } 193 | 194 | template 195 | struct IsFloatingPoint { 196 | static bool const value = std::is_floating_point::value; 197 | }; 198 | 199 | template 200 | struct IsFloatingPoint> { 201 | static bool const value = std::is_floating_point::value; 202 | }; 203 | 204 | template 205 | struct GetScalar { 206 | using Scalar = Scalar_; 207 | }; 208 | 209 | template 210 | struct GetScalar> { 211 | using Scalar = Scalar_; 212 | }; 213 | 214 | // Planes in 3d are hyperplanes. 215 | template 216 | using Plane3 = Eigen::Hyperplane; 217 | using Plane3d = Plane3; 218 | using Plane3f = Plane3; 219 | 220 | // Lines in 2d are hyperplanes. 221 | template 222 | using Line2 = Eigen::Hyperplane; 223 | using Line2d = Line2; 224 | using Line2f = Line2; 225 | 226 | } // namespace Sophus 227 | 228 | #endif // SOPHUS_TYEPES_HPP 229 | -------------------------------------------------------------------------------- /estimator/src/sophus_lib/velocities.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SOPHUS_VELOCITIES_HPP 2 | #define SOPHUS_VELOCITIES_HPP 3 | 4 | #include 5 | 6 | #include "num_diff.hpp" 7 | #include "se3.hpp" 8 | 9 | namespace Sophus { 10 | namespace experimental { 11 | // Experimental since the API will certainly change drastically in the future. 12 | 13 | // Transforms velocity vector by rotation ``foo_R_bar``. 14 | // 15 | // Note: vel_bar can be either a linear or a rotational velocity vector. 16 | // 17 | template 18 | Vector3 transformVelocity(SO3 const& foo_R_bar, 19 | Vector3 const& vel_bar) { 20 | // For rotational velocities note that: 21 | // 22 | // vel_bar = vee(foo_R_bar * hat(vel_bar) * foo_R_bar^T) 23 | // = vee(hat(Adj(foo_R_bar) * vel_bar)) 24 | // = Adj(foo_R_bar) * vel_bar 25 | // = foo_R_bar * vel_bar. 26 | // 27 | return foo_R_bar * vel_bar; 28 | } 29 | 30 | // Transforms velocity vector by pose ``foo_T_bar``. 31 | // 32 | // Note: vel_bar can be either a linear or a rotational velocity vector. 33 | // 34 | template 35 | Vector3 transformVelocity(SE3 const& foo_T_bar, 36 | Vector3 const& vel_bar) { 37 | return transformVelocity(foo_T_bar.so3(), vel_bar); 38 | } 39 | 40 | // finite difference approximation of instantanious velocity in frame foo 41 | // 42 | template 43 | Vector3 finiteDifferenceRotationalVelocity( 44 | std::function(Scalar)> const& foo_R_bar, Scalar t, 45 | Scalar h = Constants::epsilon()) { 46 | // https://en.wikipedia.org/w/index.php?title=Angular_velocity&oldid=791867792#Angular_velocity_tensor 47 | // 48 | // W = dR(t)/dt * R^{-1}(t) 49 | Matrix3 dR_dt_in_frame_foo = curveNumDiff( 50 | [&foo_R_bar](Scalar t0) -> Matrix3 { 51 | return foo_R_bar(t0).matrix(); 52 | }, 53 | t, h); 54 | // velocity tensor 55 | Matrix3 W_in_frame_foo = 56 | dR_dt_in_frame_foo * (foo_R_bar(t)).inverse().matrix(); 57 | return SO3::vee(W_in_frame_foo); 58 | } 59 | 60 | // finite difference approximation of instantanious velocity in frame foo 61 | // 62 | template 63 | Vector3 finiteDifferenceRotationalVelocity( 64 | std::function(Scalar)> const& foo_T_bar, Scalar t, 65 | Scalar h = Constants::epsilon()) { 66 | return finiteDifferenceRotationalVelocity( 67 | [&foo_T_bar](Scalar t) -> SO3 { return foo_T_bar(t).so3(); }, t, 68 | h); 69 | } 70 | 71 | } // namespace experimental 72 | } // namespace Sophus 73 | 74 | #endif // SOPHUS_VELOCITIES_HPP 75 | -------------------------------------------------------------------------------- /estimator/src/spline/assert.h: -------------------------------------------------------------------------------- 1 | /** 2 | BSD 3-Clause License 3 | 4 | This file is part of the Basalt project. 5 | https://gitlab.com/VladyslavUsenko/basalt-headers.git 6 | 7 | Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. 8 | All rights reserved. 9 | 10 | Redistribution and use in source and binary forms, with or without 11 | modification, are permitted provided that the following conditions are met: 12 | 13 | * Redistributions of source code must retain the above copyright notice, this 14 | list of conditions and the following disclaimer. 15 | 16 | * Redistributions in binary form must reproduce the above copyright notice, 17 | this list of conditions and the following disclaimer in the documentation 18 | and/or other materials provided with the distribution. 19 | 20 | * Neither the name of the copyright holder nor the names of its 21 | contributors may be used to endorse or promote products derived from 22 | this software without specific prior written permission. 23 | 24 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 26 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 27 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 28 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 29 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 30 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 32 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 33 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 34 | 35 | 36 | @file 37 | @brief Assertions used in the project 38 | */ 39 | 40 | #pragma once 41 | 42 | #include 43 | 44 | namespace basalt 45 | { 46 | 47 | #define UNUSED(x) (void)(x) 48 | 49 | inline void assertion_failed(char const *expr, char const *function, 50 | char const *file, long line) 51 | { 52 | std::cerr << "***** Assertion (" << expr << ") failed in " << function 53 | << ":\n" 54 | << file << ':' << line << ":" << std::endl; 55 | std::abort(); 56 | } 57 | 58 | inline void assertion_failed_msg(char const *expr, char const *msg, 59 | char const *function, char const *file, 60 | long line) 61 | { 62 | std::cerr << "***** Assertion (" << expr << ") failed in " << function 63 | << ":\n" 64 | << file << ':' << line << ": " << msg << std::endl; 65 | std::abort(); 66 | } 67 | } // namespace basalt 68 | 69 | #define BASALT_LIKELY(x) __builtin_expect(x, 1) 70 | 71 | #if defined(BASALT_DISABLE_ASSERTS) 72 | 73 | #define BASALT_ASSERT(expr) ((void)0) 74 | 75 | #define BASALT_ASSERT_MSG(expr, msg) ((void)0) 76 | 77 | #define BASALT_ASSERT_STREAM(expr, msg) ((void)0) 78 | 79 | #else 80 | 81 | #define BASALT_ASSERT(expr) \ 82 | (BASALT_LIKELY(!!(expr)) \ 83 | ? ((void)0) \ 84 | : ::basalt::assertion_failed(#expr, __PRETTY_FUNCTION__, __FILE__, \ 85 | __LINE__)) 86 | 87 | #define BASALT_ASSERT_MSG(expr, msg) \ 88 | (BASALT_LIKELY(!!(expr)) \ 89 | ? ((void)0) \ 90 | : ::basalt::assertion_failed_msg(#expr, msg, __PRETTY_FUNCTION__, \ 91 | __FILE__, __LINE__)) 92 | 93 | #define BASALT_ASSERT_STREAM(expr, msg) \ 94 | (BASALT_LIKELY(!!(expr)) \ 95 | ? ((void)0) \ 96 | : (std::cerr << msg << std::endl, \ 97 | ::basalt::assertion_failed(#expr, __PRETTY_FUNCTION__, __FILE__, \ 98 | __LINE__))) 99 | 100 | #endif 101 | -------------------------------------------------------------------------------- /estimator/src/spline/ceres_spline_helper.h: -------------------------------------------------------------------------------- 1 | /** 2 | BSD 3-Clause License 3 | 4 | This file is part of the Basalt project. 5 | https://gitlab.com/VladyslavUsenko/basalt-headers.git 6 | 7 | Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. 8 | All rights reserved. 9 | 10 | Redistribution and use in source and binary forms, with or without 11 | modification, are permitted provided that the following conditions are met: 12 | 13 | * Redistributions of source code must retain the above copyright notice, this 14 | list of conditions and the following disclaimer. 15 | 16 | * Redistributions in binary form must reproduce the above copyright notice, 17 | this list of conditions and the following disclaimer in the documentation 18 | and/or other materials provided with the distribution. 19 | 20 | * Neither the name of the copyright holder nor the names of its 21 | contributors may be used to endorse or promote products derived from 22 | this software without specific prior written permission. 23 | 24 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 26 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 27 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 28 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 29 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 30 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 32 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 33 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 34 | 35 | @file 36 | @brief Helper for implementing Lie group and Euclidean b-splines in ceres. 37 | */ 38 | 39 | #pragma once 40 | 41 | #include "spline_common.h" 42 | #include 43 | 44 | namespace estimator 45 | { 46 | 47 | /// @brief Helper for implementing Lie group and Euclidean b-splines in ceres of 48 | /// order N 49 | /// 50 | /// See [[arXiv:1911.08860]](https://arxiv.org/abs/1911.08860) for more details. 51 | template 52 | struct CeresSplineHelper 53 | { 54 | static constexpr int N = _N; // Order of the spline. 55 | static constexpr int DEG = _N - 1; // Degree of the spline. 56 | 57 | using MatN = Eigen::Matrix; 58 | using VecN = Eigen::Matrix; 59 | 60 | static const MatN blending_matrix_; 61 | static const MatN cumulative_blending_matrix_; 62 | static const MatN base_coefficients_; 63 | 64 | /// @brief Vector of derivatives of time polynomial. 65 | /// 66 | /// Computes a derivative of \f$ \begin{bmatrix}1 & t & t^2 & \dots & 67 | /// t^{N-1}\end{bmatrix} \f$ with repect to time. For example, the first 68 | /// derivative would be \f$ \begin{bmatrix}0 & 1 & 2 t & \dots & (N-1) 69 | /// t^{N-2}\end{bmatrix} \f$. 70 | /// @param Derivative derivative to evaluate 71 | /// @param[out] res_const vector to store the result 72 | /// @param[in] t 73 | template 74 | static inline void baseCoeffsWithTime( 75 | const Eigen::MatrixBase &res_const, double t) 76 | { 77 | EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, N); 78 | Eigen::MatrixBase &res = 79 | const_cast &>(res_const); 80 | 81 | res.setZero(); 82 | 83 | if (Derivative < N) 84 | { 85 | res[Derivative] = base_coefficients_(Derivative, Derivative); 86 | 87 | double _t = t; 88 | for (int j = Derivative + 1; j < N; j++) 89 | { 90 | res[j] = base_coefficients_(Derivative, j) * _t; 91 | _t = _t * t; 92 | } 93 | } 94 | } 95 | 96 | /// @brief Evaluate Lie group cummulative B-spline and time derivatives. 97 | /// 98 | /// @param[in] sKnots array of pointers of the spline knots. The size of each 99 | /// knot should be GroupT::num_parameters: 4 for SO(3) and 7 for SE(3). 100 | /// @param[in] u normalized time to compute value of the spline 101 | /// @param[in] inv_dt inverse of the time spacing in seconds between spline 102 | /// knots 103 | /// @param[out] transform_out if not nullptr return the value of the spline 104 | /// @param[out] vel_out if not nullptr velocity (first time derivative) in the 105 | /// body frame 106 | /// @param[out] accel_out if not nullptr acceleration (second time derivative) 107 | /// in the body frame 108 | template class GroupT> 109 | static inline void evaluate_lie( 110 | T const *const *sKnots, const double u, const double inv_dt, 111 | GroupT *transform_out = nullptr, 112 | typename GroupT::Tangent *vel_out = nullptr, 113 | typename GroupT::Tangent *accel_out = nullptr, 114 | typename GroupT::Tangent *jerk_out = nullptr) 115 | { 116 | using Group = GroupT; 117 | using Tangent = typename GroupT::Tangent; 118 | using Adjoint = typename GroupT::Adjoint; 119 | 120 | VecN p, coeff, dcoeff, ddcoeff, dddcoeff; 121 | 122 | CeresSplineHelper::template baseCoeffsWithTime<0>(p, u); // 0\1\2\3次导 p变u不变 p是大U 123 | coeff = CeresSplineHelper::cumulative_blending_matrix_ * p; 124 | 125 | if (vel_out || accel_out || jerk_out) 126 | { 127 | CeresSplineHelper::template baseCoeffsWithTime<1>(p, u); 128 | dcoeff = inv_dt * CeresSplineHelper::cumulative_blending_matrix_ * p; 129 | 130 | if (accel_out || jerk_out) 131 | { 132 | CeresSplineHelper::template baseCoeffsWithTime<2>(p, u); 133 | ddcoeff = inv_dt * inv_dt * 134 | CeresSplineHelper::cumulative_blending_matrix_ * p; 135 | 136 | if (jerk_out) 137 | { 138 | CeresSplineHelper::template baseCoeffsWithTime<3>(p, u); 139 | dddcoeff = inv_dt * inv_dt * inv_dt * 140 | CeresSplineHelper::cumulative_blending_matrix_ * p; 141 | } 142 | } 143 | } 144 | 145 | if (transform_out) 146 | { 147 | Eigen::Map const p00(sKnots[0]); 148 | *transform_out = p00; 149 | } 150 | 151 | Tangent rot_vel, rot_accel, rot_jerk; 152 | 153 | if (vel_out || accel_out || jerk_out) 154 | rot_vel.setZero(); 155 | if (accel_out || jerk_out) 156 | rot_accel.setZero(); 157 | if (jerk_out) 158 | rot_jerk.setZero(); 159 | 160 | for (int i = 0; i < DEG; i++) 161 | { 162 | Eigen::Map const p0(sKnots[i]); 163 | Eigen::Map const p1(sKnots[i + 1]); 164 | 165 | Group r01 = p0.inverse() * p1; 166 | Tangent delta = r01.log(); 167 | 168 | Group exp_kdelta = Group::exp(delta * coeff[i + 1]); 169 | 170 | if (transform_out) 171 | (*transform_out) *= exp_kdelta; 172 | 173 | if (vel_out || accel_out || jerk_out) 174 | { 175 | Adjoint A = exp_kdelta.inverse().Adj(); 176 | 177 | rot_vel = A * rot_vel; 178 | Tangent rot_vel_current = delta * dcoeff[i + 1]; 179 | rot_vel += rot_vel_current; 180 | 181 | if (accel_out || jerk_out) 182 | { 183 | rot_accel = A * rot_accel; 184 | Tangent accel_lie_bracket = 185 | Group::lieBracket(rot_vel, rot_vel_current); 186 | rot_accel += ddcoeff[i + 1] * delta + accel_lie_bracket; 187 | 188 | if (jerk_out) 189 | { 190 | rot_jerk = A * rot_jerk; 191 | rot_jerk += dddcoeff[i + 1] * delta + 192 | Group::lieBracket(ddcoeff[i + 1] * rot_vel + 193 | 2 * dcoeff[i + 1] * rot_accel - 194 | dcoeff[i + 1] * accel_lie_bracket, 195 | delta); 196 | } 197 | } 198 | } 199 | } 200 | 201 | if (vel_out) 202 | *vel_out = rot_vel; 203 | if (accel_out) 204 | *accel_out = rot_accel; 205 | if (jerk_out) 206 | *jerk_out = rot_jerk; 207 | } 208 | 209 | /// @brief Evaluate Euclidean B-spline or time derivatives. 210 | /// 211 | /// @param[in] sKnots array of pointers of the spline knots. The size of each 212 | /// knot should be DIM. 213 | /// @param[in] u normalized time to compute value of the spline 214 | /// @param[in] inv_dt inverse of the time spacing in seconds between spline 215 | /// knots 216 | /// @param[out] vec_out if DERIV=0 returns value of the spline, otherwise 217 | /// corresponding derivative. 218 | template 219 | static inline void evaluate(T const *const *sKnots, const double u, 220 | const double inv_dt, 221 | Eigen::Matrix *vec_out) 222 | { 223 | if (!vec_out) 224 | return; 225 | 226 | using VecD = Eigen::Matrix; 227 | 228 | VecN p, coeff; 229 | 230 | CeresSplineHelper::template baseCoeffsWithTime(p, u); 231 | coeff = 232 | std::pow(inv_dt, DERIV) * CeresSplineHelper::blending_matrix_ * p; 233 | 234 | vec_out->setZero(); 235 | 236 | for (int i = 0; i < N; i++) 237 | { 238 | Eigen::Map const p(sKnots[i]); 239 | 240 | (*vec_out) += coeff[i] * p; 241 | } 242 | } 243 | }; 244 | 245 | template 246 | const typename CeresSplineHelper<_N>::MatN 247 | CeresSplineHelper<_N>::base_coefficients_ = 248 | estimator::computeBaseCoefficients<_N, double>(); 249 | 250 | template 251 | const typename CeresSplineHelper<_N>::MatN 252 | CeresSplineHelper<_N>::blending_matrix_ = 253 | estimator::computeBlendingMatrix<_N, double, false>(); 254 | 255 | template 256 | const typename CeresSplineHelper<_N>::MatN 257 | CeresSplineHelper<_N>::cumulative_blending_matrix_ = 258 | estimator::computeBlendingMatrix<_N, double, true>(); 259 | 260 | } // namespace estimator 261 | -------------------------------------------------------------------------------- /estimator/src/spline/spline_common.h: -------------------------------------------------------------------------------- 1 | /** 2 | BSD 3-Clause License 3 | 4 | This file is part of the Basalt project. 5 | https://gitlab.com/VladyslavUsenko/basalt-headers.git 6 | 7 | Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. 8 | All rights reserved. 9 | 10 | Redistribution and use in source and binary forms, with or without 11 | modification, are permitted provided that the following conditions are met: 12 | 13 | * Redistributions of source code must retain the above copyright notice, this 14 | list of conditions and the following disclaimer. 15 | 16 | * Redistributions in binary form must reproduce the above copyright notice, 17 | this list of conditions and the following disclaimer in the documentation 18 | and/or other materials provided with the distribution. 19 | 20 | * Neither the name of the copyright holder nor the names of its 21 | contributors may be used to endorse or promote products derived from 22 | this software without specific prior written permission. 23 | 24 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 26 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 27 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 28 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 29 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 30 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 32 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 33 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 34 | 35 | @file 36 | @brief Common functions for B-spline evaluation 37 | */ 38 | 39 | #pragma once 40 | 41 | #include 42 | #include 43 | 44 | namespace estimator 45 | { 46 | 47 | inline const int SplineOrder = 4; 48 | inline const int BiasSplineOrder = 3; 49 | 50 | /// @brief Compute binomial coefficient. 51 | /// 52 | /// Computes number of combinations that include k objects out of n. 53 | /// @param[in] n 54 | /// @param[in] k 55 | /// @return binomial coefficient 56 | constexpr inline uint64_t C_n_k(uint64_t n, uint64_t k) 57 | { 58 | if (k > n) 59 | { 60 | return 0; 61 | } 62 | uint64_t r = 1; 63 | for (uint64_t d = 1; d <= k; ++d) 64 | { 65 | r *= n--; 66 | r /= d; 67 | } 68 | return r; 69 | } 70 | 71 | /// @brief Compute blending matrix for uniform B-spline evaluation. 72 | /// 73 | /// @param _N order of the spline 74 | /// @param _Scalar scalar type to use 75 | /// @param _Cumulative if the spline should be cumulative 76 | template 77 | Eigen::Matrix<_Scalar, _N, _N> computeBlendingMatrix() 78 | { 79 | Eigen::Matrix m; 80 | m.setZero(); 81 | 82 | for (int i = 0; i < _N; ++i) 83 | { 84 | for (int j = 0; j < _N; ++j) 85 | { 86 | double sum = 0; 87 | 88 | for (int s = j; s < _N; ++s) 89 | { 90 | sum += std::pow(-1.0, s - j) * C_n_k(_N, s - j) * 91 | std::pow(_N - s - 1.0, _N - 1.0 - i); 92 | } 93 | m(j, i) = C_n_k(_N - 1, _N - 1 - i) * sum; 94 | } 95 | } 96 | 97 | if (_Cumulative) 98 | { 99 | for (int i = 0; i < _N; i++) 100 | { 101 | for (int j = i + 1; j < _N; j++) 102 | { 103 | m.row(i) += m.row(j); 104 | } 105 | } 106 | } 107 | 108 | uint64_t factorial = 1; 109 | for (int i = 2; i < _N; ++i) 110 | { 111 | factorial *= i; 112 | } 113 | 114 | return (m / factorial).template cast<_Scalar>(); 115 | } 116 | 117 | /// @brief Compute base coefficient matrix for polynomials of size N. 118 | /// 119 | /// In each row starting from 0 contains the derivative coefficients of the 120 | /// polynomial. For _N=5 we get the following matrix: \f[ \begin{bmatrix} 121 | /// 1 & 1 & 1 & 1 & 1 122 | /// \\0 & 1 & 2 & 3 & 4 123 | /// \\0 & 0 & 2 & 6 & 12 124 | /// \\0 & 0 & 0 & 6 & 24 125 | /// \\0 & 0 & 0 & 0 & 24 126 | /// \\ \end{bmatrix} 127 | /// \f] 128 | /// Functions \ref RdSpline::baseCoeffsWithTime and \ref 129 | /// So3Spline::baseCoeffsWithTime use this matrix to compute derivatives of the 130 | /// time polynomial. 131 | /// 132 | /// @param _N order of the polynomial 133 | /// @param _Scalar scalar type to use 134 | template 135 | Eigen::Matrix<_Scalar, _N, _N> computeBaseCoefficients() 136 | { 137 | Eigen::Matrix base_coefficients; 138 | 139 | base_coefficients.setZero(); 140 | base_coefficients.row(0).setOnes(); 141 | 142 | const int DEG = _N - 1; 143 | int order = DEG; 144 | for (int n = 1; n < _N; n++) 145 | { 146 | for (int i = DEG - order; i < _N; i++) 147 | { 148 | base_coefficients(n, i) = (order - DEG + i) * base_coefficients(n - 1, i); 149 | } 150 | order--; 151 | } 152 | return base_coefficients.template cast<_Scalar>(); 153 | } 154 | 155 | } // namespace estimator 156 | -------------------------------------------------------------------------------- /estimator/src/spline/spline_segment.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | namespace estimator 9 | { 10 | 11 | // Define time types 12 | using time_span_t = std::pair; 13 | using time_init_t = std::initializer_list; 14 | 15 | static constexpr double NS_TO_S = 1e-9; ///< Nanosecond to second conversion 16 | static constexpr double S_TO_NS = 1e9; ///< Second to nanosecond conversion 17 | 18 | struct MetaData 19 | { 20 | virtual size_t NumParameters() const = 0; 21 | }; 22 | 23 | template 24 | struct SplineSegmentMeta : public MetaData 25 | { 26 | static constexpr int N = _N; // Order of the spline. 27 | static constexpr int DEG = _N - 1; // Degree of the spline. 28 | 29 | int64_t t0_ns; // First valid time 30 | int64_t dt_ns; // Knot spacing 31 | size_t n; // Number of knots 32 | 33 | double pow_inv_dt[N]; 34 | 35 | SplineSegmentMeta(int64_t _t0_ns, int64_t _dt_ns, size_t _n = 0) 36 | : t0_ns(_t0_ns), dt_ns(_dt_ns), n(_n) 37 | { 38 | pow_inv_dt[0] = 1.0; 39 | pow_inv_dt[1] = S_TO_NS / dt_ns; 40 | 41 | for (size_t i = 2; i < N; i++) 42 | { 43 | pow_inv_dt[i] = pow_inv_dt[i - 1] * pow_inv_dt[1]; 44 | } 45 | } 46 | 47 | size_t NumParameters() const override { return n; } 48 | 49 | int64_t MinTimeNs() const { return t0_ns; } 50 | 51 | int64_t MaxTimeNs() const { return t0_ns + (n - DEG) * dt_ns; } 52 | 53 | std::pair computeTIndexNs(int64_t time_ns) const 54 | { 55 | if (time_ns < MinTimeNs() || time_ns >= MaxTimeNs()) 56 | { 57 | std::cout << time_ns << " not in " << MinTimeNs() << ", " << MaxTimeNs() 58 | << "\n"; 59 | } 60 | 61 | assert(time_ns >= MinTimeNs() && time_ns < MaxTimeNs() && 62 | "computeTIndexNs from SplineSegmentMeta"); 63 | 64 | int64_t st_ns = (time_ns - t0_ns); 65 | int64_t s = st_ns / dt_ns; 66 | double u = double(st_ns % dt_ns) / double(dt_ns); 67 | 68 | return std::make_pair(u, s); 69 | } 70 | 71 | template 72 | size_t PotentiallyUnsafeFloor(T x) const 73 | { 74 | return static_cast(std::floor(x)); 75 | } 76 | 77 | // This way of treating Jets are potentially unsafe, hence the function name 78 | template 79 | size_t PotentiallyUnsafeFloor(const ceres::Jet &x) const 80 | { 81 | return static_cast(ceres::floor(x.a)); 82 | }; 83 | 84 | bool computeTIndexNs(const int64_t &time_ns, size_t &s, double &u) const 85 | { 86 | if (time_ns < MinTimeNs() || time_ns >= MaxTimeNs()) 87 | { 88 | return false; 89 | } 90 | 91 | int64_t st_ns = (time_ns - t0_ns); 92 | s = st_ns / dt_ns; 93 | u = double(st_ns % dt_ns) / double(dt_ns); 94 | return true; 95 | } 96 | 97 | template 98 | bool computeTIndexNs(const T &time_ns, size_t &s, T &u) const 99 | { 100 | if (time_ns < T(MinTimeNs()) || time_ns >= T(MaxTimeNs())) 101 | { 102 | return false; 103 | } 104 | 105 | T st = (time_ns - T(t0_ns)) / T(dt_ns); 106 | s = PotentiallyUnsafeFloor(st); 107 | u = st - T(s); 108 | return true; 109 | } 110 | }; 111 | 112 | template 113 | struct SplineMeta 114 | { 115 | std::vector> segments; 116 | 117 | size_t NumParameters() const 118 | { 119 | size_t n = 0; 120 | for (auto &segment_meta : segments) 121 | { 122 | n += segment_meta.NumParameters(); 123 | } 124 | return n; 125 | } 126 | 127 | bool ComputeSplineIndex(const int64_t &time_ns, size_t &idx, 128 | double &u) const 129 | { 130 | idx = 0; 131 | for (auto const &seg : segments) 132 | { 133 | size_t s = 0; 134 | if (seg.computeTIndexNs(time_ns, s, u)) 135 | { 136 | idx += s; 137 | return true; 138 | } 139 | else 140 | { 141 | idx += seg.NumParameters(); 142 | } 143 | } 144 | std::cout << std::fixed << std::setprecision(15) 145 | << "ComputeSplineIndex1 Problem :" << time_ns << " not in [" 146 | << segments[0].t0_ns << ", " << segments[0].MaxTimeNs() << "]\n"; 147 | return false; 148 | } 149 | 150 | template 151 | bool ComputeSplineIndex(const T &time_ns, size_t &idx, T &u) const 152 | { 153 | idx = 0; 154 | for (auto const &seg : segments) 155 | { 156 | size_t s = 0; 157 | if (seg.computeTIndexNs(time_ns, s, u)) 158 | { 159 | idx += s; 160 | return true; 161 | } 162 | else 163 | { 164 | idx += seg.NumParameters(); 165 | } 166 | } 167 | std::cout << std::fixed << std::setprecision(15) 168 | << "ComputeSplineIndex2 Problem :" << time_ns << " not in [" 169 | << segments[0].t0_ns << ", " << segments[0].MaxTimeNs() << "]\n"; 170 | return false; 171 | } 172 | }; 173 | 174 | } // namespace estimator 175 | -------------------------------------------------------------------------------- /estimator/src/spline/trajectory.cpp: -------------------------------------------------------------------------------- 1 | #include "trajectory.h" 2 | 3 | namespace estimator 4 | { 5 | 6 | 7 | } // namespace estimator 8 | -------------------------------------------------------------------------------- /estimator/src/spline/trajectory.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include "../utils/parameter_struct.h" 5 | #include 6 | #include "se3_spline.h" 7 | 8 | namespace estimator{ 9 | 10 | enum CameraType{ 11 | Event = 1, 12 | Conv = 0 13 | }; 14 | 15 | class TrajectoryManager; 16 | 17 | class Trajectory : public Se3Spline{ 18 | public: 19 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 20 | 21 | typedef std::shared_ptr Ptr; 22 | 23 | Trajectory(double time_interval, double start_time = 0) 24 | : Se3Spline(time_interval * S_TO_NS, start_time * S_TO_NS), 25 | data_start_time_(-1) 26 | { 27 | this->extendKnotsTo(start_time, SO3d(Eigen::Quaterniond::Identity()), Eigen::Vector3d(0, 0, 0)); 28 | std::cout << GREEN << "[init maxTime] " << this->maxTimeNs() * NS_TO_S << RESET << std::endl; 29 | } 30 | 31 | void setDataStartTime(int64_t time) { data_start_time_ = time; } 32 | void setExtrinsicMat(SE3d& T) { } 33 | int64_t getDataStartTime() { return data_start_time_; } 34 | 35 | bool fix_ld; 36 | double ld_lower; 37 | double ld_upper; 38 | 39 | 40 | private: 41 | int64_t data_start_time_; 42 | std::map EP_Ev2Conv_; 43 | 44 | friend TrajectoryManager; 45 | 46 | }; 47 | 48 | } // namespace estimator 49 | -------------------------------------------------------------------------------- /estimator/src/utils/eigen_utils.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | BSD 3-Clause License 3 | 4 | This file is part of the Basalt project. 5 | https://gitlab.com/VladyslavUsenko/basalt-headers.git 6 | 7 | Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. 8 | All rights reserved. 9 | 10 | Redistribution and use in source and binary forms, with or without 11 | modification, are permitted provided that the following conditions are met: 12 | 13 | * Redistributions of source code must retain the above copyright notice, this 14 | list of conditions and the following disclaimer. 15 | 16 | * Redistributions in binary form must reproduce the above copyright notice, 17 | this list of conditions and the following disclaimer in the documentation 18 | and/or other materials provided with the distribution. 19 | 20 | * Neither the name of the copyright holder nor the names of its 21 | contributors may be used to endorse or promote products derived from 22 | this software without specific prior written permission. 23 | 24 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 26 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 27 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 28 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 29 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 30 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 32 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 33 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 34 | 35 | @file 36 | @brief Definition of the standard containers with Eigen allocator. 37 | */ 38 | 39 | #pragma once 40 | 41 | #include 42 | #include 43 | #include 44 | #include 45 | 46 | #include 47 | 48 | namespace Eigen 49 | { 50 | 51 | template 52 | using aligned_vector = std::vector>; 53 | 54 | template 55 | using aligned_deque = std::deque>; 56 | 57 | template 58 | using aligned_map = std::map, 59 | Eigen::aligned_allocator>>; 60 | 61 | template 62 | using aligned_unordered_map = 63 | std::unordered_map, std::equal_to, 64 | Eigen::aligned_allocator>>; 65 | 66 | inline Eigen::Affine3d getTransBetween(Eigen::Vector3d trans_start, 67 | Eigen::Quaterniond rot_start, 68 | Eigen::Vector3d trans_end, 69 | Eigen::Quaterniond rot_end) 70 | { 71 | Eigen::Translation3d t_s(trans_start(0), trans_start(1), trans_start(2)); 72 | Eigen::Translation3d t_e(trans_end(0), trans_end(1), trans_end(2)); 73 | 74 | Eigen::Affine3d start = t_s * rot_start.toRotationMatrix(); 75 | Eigen::Affine3d end = t_e * rot_end.toRotationMatrix(); 76 | 77 | Eigen::Affine3d result = start.inverse() * end; 78 | return result; 79 | } 80 | 81 | template 82 | inline Eigen::Matrix SkewSymmetric(const Eigen::Matrix &w) 83 | { 84 | Eigen::Matrix w_x; 85 | w_x << T(0), -w(2), w(1), w(2), T(0), -w(0), -w(1), w(0), T(0); 86 | return w_x; 87 | } 88 | 89 | /** sorts vectors from large to small 90 | * vec: vector to be sorted 91 | * sorted_vec: sorted results 92 | * ind: the position of each element in the sort result in the original vector 93 | * https://www.programmersought.com/article/343692646/ 94 | */ 95 | inline void sort_vec(const Eigen::Vector3d &vec, Eigen::Vector3d &sorted_vec, 96 | Eigen::Vector3i &ind) 97 | { 98 | ind = Eigen::Vector3i::LinSpaced(vec.size(), 0, vec.size() - 1); //[0 1 2] 99 | auto rule = [vec](int i, int j) -> bool 100 | { 101 | return vec(i) > vec(j); 102 | }; // regular expression, as a predicate of sort 103 | 104 | std::sort(ind.data(), ind.data() + ind.size(), rule); 105 | 106 | // The data member function returns a pointer to the first element of 107 | // VectorXd, similar to begin() 108 | for (int i = 0; i < vec.size(); i++) 109 | { 110 | sorted_vec(i) = vec(ind(i)); 111 | } 112 | } 113 | 114 | inline Eigen::Vector3d R2ypr(const Eigen::Matrix3d &R) 115 | { 116 | Eigen::Vector3d n = R.col(0); 117 | Eigen::Vector3d o = R.col(1); 118 | Eigen::Vector3d a = R.col(2); 119 | 120 | Eigen::Vector3d ypr(3); 121 | double y = atan2(n(1), n(0)); 122 | double p = atan2(-n(2), n(0) * std::cos(y) + n(1) * std::sin(y)); 123 | double r = atan2(a(0) * std::sin(y) - a(1) * std::cos(y), 124 | -o(0) * std::sin(y) + o(1) * std::cos(y)); 125 | ypr(0) = y; 126 | ypr(1) = p; 127 | ypr(2) = r; 128 | 129 | return ypr / M_PI * 180.0; 130 | } 131 | 132 | template 133 | Eigen::Matrix ypr2R( 134 | const Eigen::MatrixBase &ypr) 135 | { 136 | typedef typename Derived::Scalar Scalar_t; 137 | 138 | Scalar_t y = ypr(0) / 180.0 * M_PI; 139 | Scalar_t p = ypr(1) / 180.0 * M_PI; 140 | Scalar_t r = ypr(2) / 180.0 * M_PI; 141 | 142 | Eigen::Matrix Rz; 143 | Rz << std::cos(y), -std::sin(y), 0, std::sin(y), std::cos(y), 0, 0, 0, 1; 144 | 145 | Eigen::Matrix Ry; 146 | Ry << std::cos(p), 0., std::sin(p), 0., 1., 0., -std::sin(p), 0., std::cos(p); 147 | 148 | Eigen::Matrix Rx; 149 | Rx << 1., 0., 0., 0., std::cos(r), -std::sin(r), 0., std::sin(r), std::cos(r); 150 | 151 | return Rz * Ry * Rx; 152 | } 153 | 154 | template 155 | static Eigen::Quaternion positify( 156 | const Eigen::QuaternionBase &q) 157 | { 158 | // printf("a: %f %f %f %f", q.w(), q.x(), q.y(), q.z()); 159 | // Eigen::Quaternion p(-q.w(), -q.x(), -q.y(), 160 | // -q.z()); printf("b: %f %f %f %f", p.w(), p.x(), p.y(), p.z()); return 161 | // q.template w() >= (typename Derived::Scalar)(0.0) ? q : 162 | // Eigen::Quaternion(-q.w(), -q.x(), -q.y(), -q.z()); 163 | return q; 164 | } 165 | 166 | } // namespace Eigen 167 | -------------------------------------------------------------------------------- /estimator/src/utils/math_utils.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // #include "sophus_utils.hpp" 4 | #include "parameter_struct.h" 5 | 6 | namespace estimator 7 | { 8 | 9 | inline PoseData XYThetaToPoseData(double x, double y, double theta, 10 | double timestamp = 0) 11 | { 12 | PoseData pose; 13 | Eigen::Vector3d p(x, y, 0); 14 | Eigen::AngleAxisd rotation_vector(theta, Eigen::Vector3d::UnitZ()); 15 | Eigen::Quaterniond q(rotation_vector); 16 | pose.timestamp = timestamp; 17 | pose.position = p; 18 | pose.orientation.setQuaternion(q); 19 | 20 | return pose; 21 | } 22 | 23 | inline PoseData SE3ToPoseData(SE3d se3_pose, double time = 0) 24 | { 25 | PoseData pose; 26 | pose.timestamp = time; 27 | pose.position = se3_pose.translation(); 28 | pose.orientation = se3_pose.so3(); 29 | return pose; 30 | } 31 | 32 | inline SE3d Matrix4fToSE3d(Eigen::Matrix4f matrix) 33 | { 34 | Eigen::Vector3d trans(matrix(0, 3), matrix(1, 3), matrix(2, 3)); 35 | Eigen::Quaterniond q(matrix.block<3, 3>(0, 0).cast()); 36 | q.normalize(); 37 | return SE3d(q, trans); 38 | } 39 | 40 | inline void SE3dToPositionEuler(SE3d se3_pose, Eigen::Vector3d &position, 41 | Eigen::Vector3d &euler) 42 | { 43 | position = se3_pose.translation(); 44 | Eigen::Quaterniond q = se3_pose.unit_quaternion(); 45 | euler = q.toRotationMatrix().eulerAngles(0, 1, 2); 46 | euler *= 180 / M_PI; 47 | } 48 | 49 | } // namespace estimator 50 | -------------------------------------------------------------------------------- /estimator/src/utils/mypcl_cloud_type.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace my_pcl 7 | { 8 | 9 | struct PointXYZIRT 10 | { 11 | PCL_ADD_POINT4D; // quad-word XYZ 12 | float intensity; ///< laser intensity reading 13 | uint16_t ring; ///< laser ring number 14 | float time; ///< laser time reading 15 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW // ensure proper alignment 16 | } EIGEN_ALIGN16; 17 | 18 | struct OusterPointXYZIRT 19 | { 20 | PCL_ADD_POINT4D; 21 | float intensity; 22 | uint32_t t; 23 | uint16_t reflectivity; 24 | uint8_t ring; 25 | // uint16_t ambient; 26 | uint32_t range; 27 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 28 | } EIGEN_ALIGN16; 29 | 30 | struct PointXYZT 31 | { 32 | PCL_ADD_POINT4D; /// quad-word XYZ 33 | float intensity; 34 | double timestamp; /// laser timestamp 35 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW // ensure proper alignment 36 | } EIGEN_ALIGN16; 37 | 38 | struct PointXYZIRPYT 39 | { 40 | PCL_ADD_POINT4D; 41 | PCL_ADD_INTENSITY; 42 | float roll; 43 | float pitch; 44 | float yaw; 45 | double time; 46 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 47 | } EIGEN_ALIGN16; 48 | 49 | } // namespace my_pcl 50 | 51 | // https://github.com/PointCloudLibrary/pcl/issues/3190 52 | POINT_CLOUD_REGISTER_POINT_STRUCT(my_pcl::PointXYZIRT, // 53 | (float, x, x) // 54 | (float, y, y) // 55 | (float, z, z) // 56 | (float, intensity, intensity) // 57 | (uint16_t, ring, ring) // 58 | (float, time, time) // 59 | ) 60 | 61 | POINT_CLOUD_REGISTER_POINT_STRUCT(my_pcl::OusterPointXYZIRT, // 62 | (float, x, x) // 63 | (float, y, y) // 64 | (float, z, z) // 65 | (float, intensity, intensity) // 66 | (uint32_t, t, t) // 67 | (uint16_t, reflectivity, reflectivity) // 68 | (uint8_t, ring, ring) // 69 | // (uint16_t, ambient, ambient) // 70 | (uint32_t, range, range)) // 71 | 72 | POINT_CLOUD_REGISTER_POINT_STRUCT(my_pcl::PointXYZT, // 73 | (float, x, x) // 74 | (float, y, y) // 75 | (float, z, z) // 76 | (float, intensity, intensity) // 77 | (double, timestamp, timestamp) // 78 | ) 79 | 80 | POINT_CLOUD_REGISTER_POINT_STRUCT(my_pcl::PointXYZIRPYT, // 81 | (float, x, x) // 82 | (float, y, y) // 83 | (float, z, z) // 84 | (float, intensity, intensity) // 85 | (float, roll, roll) // 86 | (float, pitch, pitch) // 87 | (float, yaw, yaw) // 88 | (double, time, time) // 89 | ) 90 | 91 | typedef pcl::PointXYZI VPoint; 92 | typedef pcl::PointCloud VPointCloud; 93 | 94 | typedef pcl::PointXYZ GPoint; 95 | typedef pcl::PointCloud GPointCloud; 96 | 97 | typedef my_pcl::PointXYZIRT RTPoint; 98 | typedef pcl::PointCloud RTPointCloud; 99 | 100 | typedef my_pcl::OusterPointXYZIRT OusterPoint; 101 | typedef pcl::PointCloud OusterPointCloud; 102 | 103 | typedef my_pcl::PointXYZT PosPoint; 104 | typedef pcl::PointCloud PosCloud; 105 | 106 | typedef my_pcl::PointXYZIRPYT PosePoint; 107 | typedef pcl::PointCloud PosePointCloud; 108 | 109 | typedef pcl::PointXYZRGB ColorPoint; 110 | typedef pcl::PointCloud ColorPointCloud; 111 | -------------------------------------------------------------------------------- /estimator/src/utils/parameter_struct.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // #include 4 | #include 5 | #include 6 | #include 7 | #include "camera.h" 8 | #include "calibration_board.h" 9 | 10 | namespace estimator 11 | { 12 | 13 | #define RESET "\033[0m" 14 | #define BLACK "\033[30m" /* Black */ 15 | #define RED "\033[31m" /* Red */ 16 | #define GREEN "\033[32m" /* Green */ 17 | #define YELLOW "\033[33m" /* Yellow */ 18 | #define BLUE "\033[34m" /* Blue */ 19 | #define MAGENTA "\033[35m" /* Magenta */ 20 | #define CYAN "\033[36m" /* Cyan */ 21 | #define WHITE "\033[37m" /* White */ 22 | 23 | enum MODE 24 | { 25 | Calibration = 0, 26 | Odometry_Offline, // 27 | Odometry_Online, // 28 | Location_Offline, 29 | Location_Online, 30 | Visualization 31 | }; 32 | 33 | struct VecData 34 | { 35 | double timestamp; 36 | Eigen::Vector3d p; 37 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 38 | }; 39 | 40 | struct IMUData 41 | { 42 | int64_t timestamp; 43 | Eigen::Vector3d gyro; 44 | Eigen::Vector3d accel; 45 | SO3d orientation; 46 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 47 | }; 48 | 49 | struct IMUBias 50 | { 51 | IMUBias() 52 | : gyro_bias(Eigen::Vector3d::Zero()), 53 | accel_bias(Eigen::Vector3d::Zero()) {} 54 | Eigen::Vector3d gyro_bias; 55 | Eigen::Vector3d accel_bias; 56 | }; 57 | 58 | struct IMUState 59 | { 60 | IMUState() 61 | : timestamp(0), 62 | p(Eigen::Vector3d::Zero()), 63 | v(Eigen::Vector3d::Zero()), 64 | g(Eigen::Vector3d(0, 0, -9.8)) {} 65 | double timestamp; 66 | Eigen::Vector3d p; 67 | Eigen::Vector3d v; 68 | Eigen::Quaterniond q; 69 | IMUBias bias; 70 | Eigen::Vector3d g; 71 | }; 72 | 73 | struct PoseData 74 | { 75 | PoseData() : timestamp(0), position(Eigen::Vector3d::Zero()) {} 76 | 77 | double timestamp; 78 | Eigen::Vector3d position; 79 | SO3d orientation; 80 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 81 | }; 82 | 83 | struct SystemState : public IMUState 84 | { 85 | SystemState() : name("") {} 86 | 87 | SystemState(const IMUState &imu) 88 | { 89 | timestamp = imu.timestamp; 90 | p = imu.p; 91 | v = imu.v; 92 | q = imu.q; 93 | bias = imu.bias; 94 | g = imu.g; 95 | 96 | name = ""; 97 | } 98 | 99 | std::string name; 100 | }; 101 | 102 | struct ExtrinsicParam 103 | { 104 | ExtrinsicParam() 105 | : p(Eigen::Vector3d::Zero()), 106 | q(Eigen::Quaterniond::Identity()), 107 | t_offset(0) {} 108 | 109 | // node["Extrinsics"] 110 | void Init(const YAML::Node &node) 111 | { 112 | if (!(node["time_offset"] && node["Trans"] && node["Rot"])) 113 | { 114 | LOG(WARNING) 115 | << "[ExtrinsicParam::Init] input yaml node has not parameters " 116 | "of Extrinsics struct. Return without Initialziation."; 117 | return; 118 | } 119 | 120 | t_offset = yaml::GetValue(node, "time_offset"); 121 | std::vector params_vec; 122 | yaml::GetValues(node, "Trans", 3, params_vec); 123 | p << params_vec[0], params_vec[1], params_vec[2]; 124 | 125 | yaml::GetValues(node, "Rot", 9, params_vec); 126 | Eigen::Matrix3d rot; 127 | rot << params_vec[0], params_vec[1], params_vec[2], params_vec[3], 128 | params_vec[4], params_vec[5], params_vec[6], params_vec[7], 129 | params_vec[8]; 130 | 131 | q = Eigen::Quaterniond(rot); 132 | q.normalized(); 133 | so3 = SO3d(q); 134 | se3 = SE3d(so3, p); 135 | } 136 | 137 | Eigen::Vector3d p; 138 | SO3d so3; 139 | Eigen::Quaterniond q; 140 | SE3d se3; 141 | double t_offset; 142 | }; 143 | 144 | } // namespace estimator 145 | -------------------------------------------------------------------------------- /estimator/src/utils/yaml_utils.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace yaml 7 | { 8 | 9 | template 10 | inline T GetValue(const YAML::Node &node, const std::string &descri, 11 | T intial = T(0)) 12 | { 13 | T value = intial; 14 | if (node[descri]) 15 | { 16 | value = node[descri].as(); 17 | } 18 | else 19 | { 20 | // std::cout << descri << " set as 0\n"; 21 | } 22 | return value; 23 | } 24 | 25 | template 26 | inline bool GetValues(const YAML::Node &node, const std::string &descri, 27 | size_t size, std::vector ¶ms_vec) 28 | { 29 | params_vec.resize(size); 30 | 31 | if (node[descri]) 32 | { 33 | for (size_t i = 0; i < size; i++) 34 | { 35 | params_vec.at(i) = node[descri][i].as(); 36 | } 37 | return true; 38 | } 39 | else 40 | { 41 | std::cout << "Note [" << descri << "] is not provided.\n"; 42 | return false; 43 | } 44 | } 45 | 46 | inline std::string GetString(const YAML::Node &node, const std::string &descri, 47 | std::string intial = "") 48 | { 49 | std::string str = intial; 50 | if (node[descri]) 51 | { 52 | str = node[descri].as(); 53 | } 54 | else 55 | { 56 | std::cout << "Note [" << descri << "] set as an empty string\n"; 57 | } 58 | return str; 59 | } 60 | 61 | inline bool GetBool(const YAML::Node &node, const std::string &descri, 62 | bool ret = false) 63 | { 64 | if (node[descri]) 65 | { 66 | ret = node[descri].as(); 67 | } 68 | return ret; 69 | } 70 | 71 | } // namespace yaml 72 | -------------------------------------------------------------------------------- /eventmap_generator/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(eventmap_generator) 3 | 4 | set(CMAKE_CXX_STANDARD 17) 5 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -g -pthread") 6 | set(cv_bridge_DIR /usr/local/share/cv_bridge/cmake) 7 | 8 | # search for all msg files 9 | FILE(GLOB messages_to_build RELATIVE "${CMAKE_CURRENT_SOURCE_DIR}/msg" 10 | "${CMAKE_CURRENT_SOURCE_DIR}/msg/*.msg") 11 | 12 | find_package(OpenCV REQUIRED) 13 | find_package(catkin REQUIRED COMPONENTS 14 | cv_bridge 15 | image_transport 16 | roscpp 17 | rosbag 18 | std_msgs 19 | dvs_msgs 20 | sensor_msgs 21 | message_generation 22 | ) 23 | 24 | add_message_files( 25 | FILES 26 | ${messages_to_build} 27 | ) 28 | 29 | generate_messages( 30 | DEPENDENCIES 31 | std_msgs 32 | sensor_msgs 33 | ) 34 | 35 | ################################### 36 | ## catkin specific configuration ## 37 | ################################### 38 | ## The catkin_package macro generates cmake config files for your package 39 | ## Declare things to be passed to dependent projects 40 | ## INCLUDE_DIRS: uncomment this if your package contains header files 41 | ## LIBRARIES: libraries you create in this project that dependent projects also need 42 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 43 | ## DEPENDS: system dependencies of this project that dependent projects also need 44 | catkin_package( 45 | # INCLUDE_DIRS include 46 | # LIBRARIES circle_detector 47 | CATKIN_DEPENDS message_generation message_runtime roscpp std_msgs sensor_msgs 48 | # DEPENDS system_lib 49 | ) 50 | 51 | include_directories( 52 | include 53 | ${catkin_INCLUDE_DIRS} 54 | ${OpenCV_INCLUDE_DIRS} 55 | ) 56 | 57 | add_executable(generator 58 | src/generator_node.cpp 59 | src/generator.cpp) 60 | 61 | target_link_libraries(generator 62 | ${catkin_LIBRARIES} 63 | ${OpenCV_LIBRARIES} 64 | ) 65 | 66 | -------------------------------------------------------------------------------- /eventmap_generator/include/generator.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "ros/ros.h" 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include 21 | #include 22 | #include 23 | 24 | namespace generator{ 25 | 26 | using EventQueue = std::deque; 27 | 28 | class EventQueueMat 29 | { 30 | public: 31 | EventQueueMat(int width, int height, int queueLen){ 32 | width_ = width; 33 | height_ = height; 34 | queueLen_ = queueLen; 35 | eqMat_ = std::vector(width_ * height_, EventQueue()); 36 | } 37 | 38 | void insertEvent(const dvs_msgs::Event& e){ 39 | if(!insideImage(e.x, e.y)) 40 | return; 41 | else{ 42 | EventQueue& eq = getEventQueue(e.x, e.y); 43 | eq.push_back(e); 44 | while(eq.size() > queueLen_) 45 | eq.pop_front(); 46 | } 47 | } 48 | 49 | void clear(){ 50 | eqMat_.clear(); 51 | } 52 | 53 | bool insideImage(const size_t x, const size_t y){ 54 | return !(x < 0 || x >= width_ || y < 0 || y >= height_); 55 | } 56 | 57 | inline EventQueue& getEventQueue(const size_t x, const size_t y){ 58 | return eqMat_[x + width_ * y]; 59 | } 60 | 61 | size_t width_; 62 | size_t height_; 63 | size_t queueLen_; 64 | std::vector eqMat_; 65 | 66 | private: 67 | 68 | }; 69 | 70 | class EventMap { 71 | public: 72 | EventMap(ros::NodeHandle& nh, ros::NodeHandle& nh_private); 73 | ~EventMap(){event_sub_.shutdown();}; 74 | 75 | private: 76 | struct Job{ 77 | size_t start_col_, end_col_; 78 | size_t start_row_, end_row_; 79 | size_t i_thread_; 80 | ros::Time trig_time; 81 | cv::Mat* event_no_polarity_; 82 | cv::Mat* event_positive_; 83 | cv::Mat* event_negative_; 84 | }; 85 | 86 | void eventsCallback(const dvs_msgs::EventArray::ConstPtr& msg); 87 | void init(int width, int height); 88 | void readBag(std::string bagName); 89 | void clearEventQueue(); 90 | void thread(Job &job); 91 | void generateEventMap(const ros::Time& triggered_time); 92 | void generateEventMap_hyperthread(const ros::Time& triggered_time); 93 | 94 | ros::NodeHandle nh_; 95 | ros::Subscriber event_sub_; 96 | ros::Publisher eventmap_pub_; 97 | 98 | rosbag::Bag read_bag, write_bag; 99 | std::string bagName, store_path; 100 | 101 | bool bSensorInitialized_ = false; 102 | int max_event_queue_length_ = 10; 103 | int output_event_num_; 104 | ros::Time trig_time_, last_time_; 105 | cv::Size sensor_size_; 106 | 107 | // containers 108 | EventQueue events_; 109 | std::shared_ptr pEventQueueMat_; 110 | 111 | std::vector timestamps; 112 | 113 | int count = 0; 114 | }; 115 | 116 | 117 | }; -------------------------------------------------------------------------------- /eventmap_generator/launch/generator.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /eventmap_generator/msg/eventmap.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | sensor_msgs/Image eventmap 3 | time[] timestamps -------------------------------------------------------------------------------- /eventmap_generator/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | eventmap_generator 4 | 0.0.0 5 | The eventmap_generator package 6 | 7 | 8 | 9 | 10 | root 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | cv_bridge 53 | image_transport 54 | opencv3 55 | roscpp 56 | rosbag 57 | std_msgs 58 | dvs_msgs 59 | cv_bridge 60 | image_transport 61 | opencv3 62 | roscpp 63 | rosbag 64 | std_msgs 65 | dvs_msgs 66 | cv_bridge 67 | image_transport 68 | opencv3 69 | roscpp 70 | rosbag 71 | std_msgs 72 | dvs_msgs 73 | message_generation 74 | message_generation 75 | message_runtime 76 | message_runtime 77 | sensor_msgs 78 | sensor_msgs 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | -------------------------------------------------------------------------------- /eventmap_generator/src/generator.cpp: -------------------------------------------------------------------------------- 1 | #include "generator.h" 2 | 3 | namespace generator{ 4 | 5 | EventMap::EventMap(ros::NodeHandle& nh, ros::NodeHandle& nh_private) : nh_(nh){ 6 | nh_.param("BagName", bagName, std::string("record.bag")); 7 | nh_.param("StorePath", store_path, std::string("intermediateBags")); 8 | 9 | event_sub_ = nh_.subscribe("/dvs/events", 10, &EventMap::eventsCallback, this); 10 | eventmap_pub_ = nh_.advertise("/eventmap", 1); 11 | 12 | // Judge if event queue is empty 13 | bSensorInitialized_ = false; 14 | if(pEventQueueMat_) 15 | pEventQueueMat_->clear(); 16 | sensor_size_ = cv::Size(0,0); 17 | 18 | readBag(bagName); 19 | } 20 | 21 | void EventMap::readBag(std::string bagName){ 22 | read_bag.open(bagName, rosbag::bagmode::Read); 23 | write_bag.open(store_path + "/eventmap.bag", rosbag::bagmode::Write); 24 | 25 | rosbag::View view(read_bag, rosbag::TopicQuery("/dvs/events")); 26 | 27 | for (const rosbag::MessageInstance& m : view) { 28 | dvs_msgs::EventArray::ConstPtr msg = m.instantiate(); 29 | 30 | if (!bSensorInitialized_) { 31 | init(msg->width, msg->height); 32 | } 33 | for(const dvs_msgs::Event& e : msg->events){ 34 | static bool first_input = true; 35 | if (first_input){ 36 | first_input = false; 37 | trig_time_ = e.ts; 38 | last_time_ = e.ts; 39 | } 40 | events_.push_back(e); 41 | pEventQueueMat_->insertEvent(e); 42 | 43 | ros::Duration duration = e.ts - last_time_; 44 | double time_diff_ms = duration.toSec() * 1000; 45 | if (time_diff_ms > 3){ 46 | ROS_ERROR("Missing event alert!"); 47 | trig_time_ = e.ts; 48 | last_time_ = e.ts; 49 | events_.clear(); 50 | continue; 51 | } 52 | last_time_ = e.ts; 53 | 54 | duration = e.ts - trig_time_; 55 | time_diff_ms = duration.toSec() * 1000; 56 | 57 | if (time_diff_ms > 30 && events_.size() > 10000){ 58 | trig_time_ = events_[events_.size() - 10000].ts; 59 | ROS_INFO("The timestamp is %d.%d:", trig_time_.sec, trig_time_.nsec); 60 | generateEventMap(trig_time_); 61 | 62 | trig_time_ = e.ts; 63 | events_.clear(); 64 | } 65 | } 66 | } 67 | 68 | read_bag.close(); 69 | write_bag.close(); 70 | 71 | ros::shutdown(); 72 | } 73 | 74 | void EventMap::eventsCallback(const dvs_msgs::EventArray::ConstPtr& msg){ 75 | if(!bSensorInitialized_){ 76 | init(msg->width, msg->height); 77 | } 78 | 79 | for(const dvs_msgs::Event& e : msg->events){ 80 | static bool first_input = true; 81 | if (first_input){ 82 | first_input = false; 83 | trig_time_ = e.ts; 84 | } 85 | events_.push_back(e); 86 | pEventQueueMat_->insertEvent(e); 87 | 88 | ros::Duration duration = e.ts - trig_time_; 89 | double time_diff_ms = duration.toSec() * 1000; 90 | 91 | if (time_diff_ms > 30){ 92 | //clearEventQueue(); 93 | if (events_.size() > 1000){ 94 | trig_time_ = events_[events_.size() - 1000].ts; 95 | } 96 | 97 | ROS_INFO("The timestamp is %d.%d:", trig_time_.sec, trig_time_.nsec); 98 | generateEventMap(trig_time_); 99 | 100 | trig_time_ = e.ts; 101 | events_.clear(); 102 | } 103 | } 104 | } 105 | 106 | void EventMap::init(int width, int height){ 107 | sensor_size_ = cv::Size(width, height); 108 | bSensorInitialized_ = true; 109 | pEventQueueMat_.reset(new EventQueueMat(width, height, max_event_queue_length_)); 110 | timestamps.resize(width * height); 111 | ROS_INFO("Sensor size: (%d x %d)", sensor_size_.width, sensor_size_.height); 112 | } 113 | 114 | void EventMap::clearEventQueue() 115 | { 116 | static constexpr size_t MAX_EVENT_QUEUE_LENGTH = 1000000; 117 | if (events_.size() > MAX_EVENT_QUEUE_LENGTH) 118 | { 119 | size_t remove_events = events_.size() - MAX_EVENT_QUEUE_LENGTH; 120 | events_.erase(events_.begin(), events_.begin() + remove_events); 121 | } 122 | } 123 | 124 | void EventMap::generateEventMap(const ros::Time& triggered_time){ 125 | cv::Mat event_map_no_polarity = cv::Mat::zeros(cv::Size(sensor_size_.width, sensor_size_.height), CV_32F); 126 | cv::Mat plot = cv::Mat::zeros(cv::Size(sensor_size_.width, sensor_size_.height), CV_32FC3); 127 | int count = 0; 128 | for(size_t x = 0; x < sensor_size_.width; ++x){ 129 | for(size_t y = 0; y < sensor_size_.height; ++y){ 130 | EventQueue& eq = pEventQueueMat_->getEventQueue(x, y); 131 | if (!eq.empty() && eq.back().ts >= triggered_time){ 132 | count++; 133 | if (eq.back().polarity){ 134 | event_map_no_polarity.at(y, x) = 1; 135 | cv::circle(plot, cvPoint(x, y), 0.5, cv::Scalar(1, 1, 1), -1); 136 | timestamps[y * sensor_size_.width + x] = eq.back().ts; 137 | }else{ 138 | event_map_no_polarity.at(y, x) = -1; 139 | cv::circle(plot, cvPoint(x, y), 0.5, cv::Scalar(1, 1, 1), -1); 140 | timestamps[y * sensor_size_.width + x] = eq.back().ts; 141 | } 142 | } 143 | } 144 | } 145 | event_map_no_polarity = (event_map_no_polarity + 1) / 2; 146 | ROS_INFO("Event map generated"); 147 | 148 | // Publish eventmap 149 | eventmap_generator::eventmap eventmap_msg; 150 | cv_bridge::CvImage cv_image; 151 | cv_image.encoding = sensor_msgs::image_encodings::TYPE_32FC1; 152 | cv_image.image = event_map_no_polarity; 153 | cv_image.header.stamp = triggered_time; 154 | static sensor_msgs::Image ros_image; 155 | cv_image.toImageMsg(ros_image); 156 | cv::imshow("event", plot); 157 | cv::waitKey(1); 158 | 159 | eventmap_msg.eventmap = ros_image; 160 | eventmap_msg.timestamps = timestamps; 161 | if(eventmap_pub_.getNumSubscribers() > 0) 162 | { 163 | eventmap_pub_.publish(eventmap_msg); 164 | } 165 | 166 | write_bag.write("/eventmap", ros::Time::now(), eventmap_msg); 167 | } 168 | 169 | void EventMap::generateEventMap_hyperthread(const ros::Time& triggered_time){ 170 | ROS_INFO("Event map begin"); 171 | cv::Mat event_map_no_polarity = cv::Mat::zeros(cv::Size(sensor_size_.width, sensor_size_.height), CV_32F); 172 | 173 | // distribute jobs 174 | int NUM_THREAD_TS = 8; 175 | std::vector jobs(NUM_THREAD_TS); 176 | size_t num_col_per_thread = sensor_size_.width / NUM_THREAD_TS; 177 | size_t res_col = sensor_size_.width % NUM_THREAD_TS; 178 | for(size_t i = 0; i < NUM_THREAD_TS; i++) 179 | { 180 | jobs[i].i_thread_ = i; 181 | jobs[i].start_col_ = num_col_per_thread * i; 182 | if(i == NUM_THREAD_TS - 1) 183 | jobs[i].end_col_ = jobs[i].start_col_ + num_col_per_thread - 1 + res_col; 184 | else 185 | jobs[i].end_col_ = jobs[i].start_col_ + num_col_per_thread - 1; 186 | jobs[i].start_row_ = 0; 187 | jobs[i].end_row_ = sensor_size_.height - 1; 188 | jobs[i].trig_time = triggered_time; 189 | jobs[i].event_no_polarity_ = &event_map_no_polarity; 190 | } 191 | 192 | // hyper thread processing 193 | std::vector threads; 194 | threads.reserve(NUM_THREAD_TS); 195 | for(size_t i = 0; i < NUM_THREAD_TS; i++) 196 | threads.emplace_back(std::bind(&EventMap::thread, this, jobs[i])); 197 | for(auto& thread:threads) 198 | if(thread.joinable()) 199 | thread.join(); 200 | 201 | event_map_no_polarity = (event_map_no_polarity + 1) / 2; 202 | ROS_INFO("Event map generated"); 203 | 204 | // Publish eventmap 205 | eventmap_generator::eventmap eventmap_msg; 206 | cv_bridge::CvImage cv_image; 207 | cv_image.encoding = sensor_msgs::image_encodings::TYPE_32FC1; 208 | cv_image.image = event_map_no_polarity; 209 | cv_image.header.stamp = triggered_time; 210 | static sensor_msgs::Image ros_image; 211 | cv_image.toImageMsg(ros_image); 212 | cv::imshow("event", event_map_no_polarity); 213 | cv::waitKey(1); 214 | 215 | eventmap_msg.eventmap = ros_image; 216 | eventmap_msg.timestamps = timestamps; 217 | if(eventmap_pub_.getNumSubscribers() > 0) 218 | { 219 | eventmap_pub_.publish(eventmap_msg); 220 | } 221 | } 222 | 223 | void EventMap::thread(Job &job){ 224 | size_t start_col = job.start_col_; 225 | size_t end_col = job.end_col_; 226 | size_t start_row = job.start_row_; 227 | size_t end_row = job.end_row_; 228 | size_t i_thread = job.i_thread_; 229 | ros::Time trig_time = job.trig_time; 230 | cv::Mat& event_map_no_polarity = *job.event_no_polarity_; 231 | 232 | for(size_t y = start_row; y <= end_row; y++){ 233 | for(size_t x = start_col; x <= end_col; x++){ 234 | EventQueue& eq = pEventQueueMat_->getEventQueue(x, y); 235 | if (!eq.empty() && eq.back().ts > trig_time){ 236 | if (eq.back().polarity){ 237 | event_map_no_polarity.at(y, x) = 1; 238 | }else{ 239 | event_map_no_polarity.at(y, x) = -1; 240 | } 241 | } 242 | } 243 | } 244 | } 245 | 246 | }; -------------------------------------------------------------------------------- /eventmap_generator/src/generator_node.cpp: -------------------------------------------------------------------------------- 1 | #include "generator.h" 2 | 3 | int main(int argc, char** argv) { 4 | ros::init(argc, argv, "eventmap_generator"); 5 | ros::NodeHandle nh; 6 | ros::NodeHandle nh_private("~"); 7 | 8 | generator::EventMap em(nh, nh_private); 9 | 10 | ros::spin(); 11 | 12 | return 0; 13 | } 14 | -------------------------------------------------------------------------------- /record_bag/circle_info.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wsakobe/EF-Calib/0e218c9640a237769aebffc63c615c44b1c2e4e0/record_bag/circle_info.bag -------------------------------------------------------------------------------- /record_bag/corner_info.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wsakobe/EF-Calib/0e218c9640a237769aebffc63c615c44b1c2e4e0/record_bag/corner_info.bag -------------------------------------------------------------------------------- /run.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | roslaunch corner_detector corner_detector.launch 4 | 5 | wait 6 | 7 | roslaunch eventmap_generator generator.launch 8 | 9 | wait 10 | 11 | roslaunch circle_detector circle_detector.launch 12 | --------------------------------------------------------------------------------