├── LICENSE ├── README.md └── src ├── dof6_estimator ├── Screenshot from 2021-12-16 20-24-51.png ├── config │ └── config.yaml ├── include │ ├── callbacks.hpp │ ├── database.hpp │ ├── numerics.hpp │ └── system.hpp ├── launch │ └── estimation.launch ├── package.xml └── src │ ├── callbacks.cpp │ ├── database.cpp │ ├── dof6_estimator_node.cpp │ ├── numerics.cpp │ ├── system.cpp │ ├── system_estimate.cpp │ └── system_estimate_ransac_doublewarp_ceres.cpp ├── event_publisher ├── CMakeLists.txt ├── config │ ├── config.yaml │ └── config_depth.yaml ├── include │ └── event_reader.hpp ├── launch │ ├── event_publish.launch │ └── event_publisher_depth.launch ├── package.xml ├── readme.md └── src │ ├── event_publisher_node.cpp │ ├── event_reader.cpp │ └── main_depth.cpp └── rotation_estimator ├── CMakeLists.txt ├── config └── config.yaml ├── include ├── callbacks.hpp ├── database.hpp ├── numerics.hpp └── system.hpp ├── launch └── estimation.launch ├── package.xml └── src ├── callbacks.cpp ├── database.cpp ├── numerics.cpp ├── rotation_estimator_node.cpp ├── system.cpp ├── system_estimate.cpp └── system_estimate_ransac_doublewarp_ceres.cpp /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2023 huangxueyan 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 | # [CVPR2023] **P**rogessive Spatio-temporal Alignment for Efficient **E**vent-based **M**otion **E**stimation (PEME) 2 | 3 | Xueyan Huang, Yueyi Zhang*, Zhiwei Xiong 4 | 5 | *Corresponding Author 6 | 7 | University of Science and Technology of China (USTC), Hefei, China 8 | 9 | 10 | ## How to run 11 | 1. clone and build: 12 | 13 | git clone https://github.com/huangxueyan/PEME.git 14 | catkin_make 15 | 16 | 2. edit config file 17 | edit src/rotation_estimator/config/config.yaml when you want to test our model with different datasets. 18 | 3. roslaunch src/rotation_estimator/launch/estimation.launch 19 | 20 | ## Evaluation 21 | We evaluate our model using the same metrics as used in the https://github.com/pbideau/Event-ST-PPP repository 22 | 23 | python src/eval.py -p /home/your_output_dir -e results.txt 24 | 25 | ## Citation 26 | If our work inspires your research or some part of the codes are useful for your work, please cite our paper: 27 | ``` 28 | @inproceedings{huang2023progressive, 29 | title={Progressive Spatio-Temporal Alignment for Efficient Event-Based Motion Estimation}, 30 | author={Huang, Xueyan and Zhang, Yueyi and Xiong, Zhiwei}, 31 | booktitle={Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition}, 32 | pages={1537--1546}, 33 | year={2023} 34 | } 35 | ``` 36 | 37 | ## Acknowledgement 38 | This code is built upon the codebase from the original repository at https://github.com/Haram-kim/Globally_Aligned_Events. We would like to express our sincere gratitude to the original author, Haram Kim, for their contributions to the field and for making their code publicly available. We greatly appreciate their efforts in advancing research in this area. 39 | Our code is released under the MIT License. 40 | 41 | ## Contact 42 | If you have any questions or opinions, feel free to raise them by creating an 'issue' in this repository, or contact us via hxy2020@mail.ustc.edu.cn 43 | -------------------------------------------------------------------------------- /src/dof6_estimator/Screenshot from 2021-12-16 20-24-51.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/huangxueyan/PEME/d76593942d5c64433c99182ca819109c90896357/src/dof6_estimator/Screenshot from 2021-12-16 20-24-51.png -------------------------------------------------------------------------------- /src/dof6_estimator/config/config.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | Version: 1.0 4 | 5 | #-------------------------------------------------------------------------------------------- 6 | # General Parameters 7 | #-------------------------------------------------------------------------------------------- 8 | 9 | 10 | #-------------------------------------------------------------------------------------------- 11 | # General Parameters (fixed). 12 | #-------------------------------------------------------------------------------------------- 13 | # original image size for camera 14 | # width: 240 15 | # height: 180 16 | 17 | #-------------------------------------------------------------------------------------------- 18 | # Input Parameters 19 | #-------------------------------------------------------------------------------------------- 20 | groundtruth_dir: "/home/hxy/Documents/rosbag/Zhu-RAL/indoor_flying1/event_augdepth.txt" 21 | calib_dir: "/home/hxy/Documents/rosbag/Zhu-RAL/indoor_flying1/calib.txt" 22 | # groundtruth_dir: "/home/hxy/Documents/rosbag/Zhu-RAL/outdoor_driving1/event_augdepth.txt" 23 | # calib_dir: "/home/hxy/Documents/rosbag/Zhu-RAL/outdoor_driving1/calib.txt" 24 | 25 | output_dir: "/home/hxy/Desktop/data/6dof_estimation/indoor_flying1/images/30k_double_warp_" 26 | 27 | yaml_sample_count: 1000 28 | yaml_iter_num: 3 29 | yaml_ceres_iter_num: 3 30 | yaml_ceres_iter_thread: 16 31 | yaml_ts_start: 0.8 32 | yaml_ts_end: 0.8 33 | yaml_denoise_num: 2 34 | yaml_gaussian_size: 5 35 | yaml_gaussian_size_sigma: 1 36 | yaml_default_value_factor: 1 37 | 38 | 39 | # 2.7e7~3.1e7 for dynamic 40 | # 5.7e6~5.9e6 for shapes 41 | # you can use 42 | read_start_lines: 0e4 43 | read_max_lines: 25000e4 44 | # read_start_lines: 100e4 45 | # read_max_lines: 160e4 46 | 47 | 48 | #-------------------------------------------------------------------------------------------- 49 | # Event Parameters 50 | #-------------------------------------------------------------------------------------------- 51 | # Event.delta_time: 0.05 52 | using_fixed_time: 0 # 1 means true 53 | 54 | fixed_interval: 0.05 55 | Event.bundle_size: 3e4 -------------------------------------------------------------------------------- /src/dof6_estimator/include/callbacks.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | 4 | // ros 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | 15 | // std 16 | #include 17 | #include 18 | 19 | 20 | // self 21 | #include "system.hpp" 22 | 23 | using namespace std; 24 | 25 | class ImageGrabber 26 | { 27 | public: 28 | ImageGrabber(System* sys): system(sys){} 29 | void GrabImage(const sensor_msgs::ImageConstPtr& msg); 30 | System* system; 31 | // ros::Time begin_time; 32 | }; 33 | 34 | 35 | class EventGrabber 36 | { 37 | public: 38 | EventGrabber(System* sys): system(sys){} 39 | 40 | void GrabEvent(const dvs_msgs::EventDepthArrayConstPtr& msg); 41 | System* system; 42 | }; 43 | 44 | class PoseGrabber 45 | { 46 | public: 47 | PoseGrabber(System* sys): system(sys) { 48 | // que_valid = false; 49 | gt_velocity_file = fstream("/home/hxt/Desktop/data/gt_theta_velocity.txt", ios::out); 50 | // gt_velocity_file_quat = fstream("/home/hxt/Desktop/data/evo_data/gt_theta_velocity.txt", ios::out); 51 | } 52 | void GrabPose(const geometry_msgs::PoseStampedConstPtr& msg); 53 | System* system; 54 | 55 | 56 | fstream gt_velocity_file; 57 | // fstream gt_velocity_file_quat; //evo esti 58 | 59 | // bool que_valid; 60 | queue que_last_poseData; 61 | // ros::Time begin_time; 62 | }; 63 | 64 | -------------------------------------------------------------------------------- /src/dof6_estimator/include/database.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // std 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | // ros 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | using namespace std; 22 | 23 | 24 | struct EventData 25 | { 26 | double time_stamp; 27 | vector event; 28 | }; 29 | 30 | struct ImageData 31 | { 32 | double time_stamp; 33 | cv::Mat image; 34 | uint32_t seq; 35 | }; 36 | 37 | struct PoseData 38 | { 39 | double time_stamp; // starts from 0s 40 | ros::Time time_stamp_ros; // ros time 41 | uint32_t seq; 42 | Eigen::Quaterniond quat; // quanterion 43 | Eigen::Vector3d pose; // xyz 44 | }; 45 | 46 | struct CameraPara 47 | { 48 | 49 | CameraPara(); 50 | CameraPara(string filename); 51 | double fx; 52 | double fy; 53 | double cx; 54 | double cy; 55 | double k1, k2, p1, p2, k3; 56 | // double rd1; 57 | // double rd2; 58 | 59 | int width, height, height_map, width_map; 60 | 61 | cv::Mat cameraMatrix, distCoeffs ; 62 | 63 | Eigen::Matrix3d eg_cameraMatrix, eg_MapMatrix; 64 | 65 | 66 | }; 67 | 68 | enum PlotOption 69 | { 70 | U16C3_EVNET_IMAGE_COLOR, 71 | U16C1_EVNET_IMAGE, 72 | U8C1_EVNET_IMAGE, 73 | S16C1_EVNET_IMAGE, 74 | TIME_SURFACE, 75 | F32C1_EVENT_COUNT, 76 | }; 77 | 78 | 79 | /** 80 | * \brief a set of local events, stores as Eigen::Matrix2xf. 81 | */ 82 | struct EventBundle{ 83 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 84 | 85 | EventBundle(); 86 | EventBundle(const EventBundle& eb); 87 | ~EventBundle(); 88 | 89 | // core opearte 90 | void Append(std::vector& vec_eventData); 91 | void Append(std::vector& vec_eventData, std::vector& vec_eventDepth); 92 | void CopySize(const EventBundle& eb); 93 | 94 | void Clear(); 95 | void DiscriminateInner(int widht, int height); 96 | 97 | // image process 98 | // GetEventImage() // options with signed or color 99 | void InverseProjection(Eigen::Matrix3d& K); 100 | void InverseProjection(Eigen::Matrix3d& K, Eigen::Matrix3Xd& raw_coord_3d); // given depth 101 | void Projection(Eigen::Matrix3d& K); 102 | 103 | 104 | // events in eigen form used as 3d porjection 105 | Eigen::Matrix2Xd coord; // row, col = [2,pixels], used for Eigen rotation 106 | Eigen::Matrix3Xd coord_3d; // row, col = [3,pixels], used for Eigen rotation 107 | 108 | // relative time of event 109 | Eigen::VectorXd time_delta; // later is positive 110 | Eigen::VectorXd time_delta_rev; // currently not used 111 | 112 | // the estimate para of local events 113 | Eigen::Vector3d angular_velocity, // angleAxis current velocity, used to sharp local events 114 | angular_position; // angleAxis current pos, warp cur camera to world coord. 115 | 116 | // Eigen::Matrix3d rotation_cur2ref; // from camera to world s 117 | 118 | // events in vector form, used as data storage 119 | // double abs_tstamp; // receiving time at ROS system 120 | 121 | ros::Time first_tstamp, last_tstamp; // event time in ros system. 122 | // vector x, y; // original event coor used for opencv 123 | Eigen::VectorXf polar; // 0, 1 event polar 124 | Eigen::VectorXf isInner; // 0, 1 indicating the event is inner 125 | size_t size; 126 | 127 | }; 128 | -------------------------------------------------------------------------------- /src/dof6_estimator/include/numerics.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | #include 9 | 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | Eigen::Matrix3d hat(const Eigen::Vector3d &x); 18 | Eigen::Vector3d unhat(const Eigen::Matrix3d &x); 19 | Eigen::Matrix3d SO3(const Eigen::Vector3d &x); 20 | Eigen::Vector3d InvSO3(const Eigen::Matrix3d &x_hat); 21 | Eigen::Vector3d SO3add(const Eigen::Vector3d x, const Eigen::Vector3d y, bool cycle); 22 | 23 | 24 | struct MyEulerAngles { 25 | double roll, pitch, yaw; }; 26 | 27 | Eigen::Vector3d toEulerAngles(Eigen::Quaterniond q); 28 | 29 | Eigen::Quaterniond ToQuaternion(double yaw, double pitch, double roll); 30 | 31 | 32 | double getVar(cv::Mat& image, int& nonZero, int type); -------------------------------------------------------------------------------- /src/dof6_estimator/include/system.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | 4 | // ros 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | 13 | // third party 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | // std 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | 31 | // self 32 | #include "database.hpp" 33 | #include "numerics.hpp" 34 | 35 | 36 | using namespace std; 37 | 38 | /** 39 | * \brief receive ros_msg + imgproc + optimize + visualize 40 | */ 41 | class System 42 | { 43 | public: 44 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 45 | System(const string& yaml); 46 | ~System(); 47 | 48 | // ros msg 49 | void pushEventData(const std::vector& ros_vec_event); 50 | void pushEventData(const std::vector& ros_vec_event, const std::vector& vec_depth); 51 | void pushimageData(const ImageData& imageData); 52 | void pushPoseData(const PoseData& poseData); 53 | void updateEventBundle(); // use less 54 | void setBeginTime(ros::Time); 55 | 56 | void save_velocity(); // save velocity date to txt 57 | 58 | Eigen::Matrix3d get_local_rotation_b2f(bool inverse = false); 59 | Eigen::Matrix3d get_global_rotation_b2f(size_t idx_t1, size_t idx_t2); 60 | 61 | 62 | // imgproc 63 | void Run(); 64 | void undistortEvents(); 65 | cv::Mat getWarpedEventImage(const Eigen::Vector3d & temp_ang_vel, const Eigen::Vector3d& cur_trans_vel, EventBundle& event_out, 66 | const PlotOption& option = PlotOption::U16C3_EVNET_IMAGE_COLOR, bool ref_t1 = false); 67 | 68 | void getWarpedEventPoints(const EventBundle& eventIn, EventBundle& eventOut, 69 | const Eigen::Vector3d& cur_ang_vel,const Eigen::Vector3d& cur_trans_vel, double delta_time=-1, bool ref_t1=false); 70 | cv::Mat getImageFromBundle(EventBundle& eventBundle, 71 | const PlotOption option = PlotOption::U16C3_EVNET_IMAGE_COLOR, bool is_mapping=false); 72 | 73 | // void getMapImage(); 74 | 75 | // optimize 76 | void localCM(); 77 | 78 | void EstimateMotion_kim(); 79 | void EstimateMotion_CM_ceres(); 80 | void EstimateMotion_ransca_ceres(double ts_start, double ts_end, int sample_num, int total_iter_num); 81 | void EstimateMotion_ransca_samples_ceres(double sample_start, double sample_end); 82 | void EstimateMotion_KS_ceres(); 83 | // void EstimateMotion_ransca_once(double sample_ratio, double warp_time_ratio, double opti_steps); 84 | // void EstimateMotion_ransca_warp2bottom(double sample_start, double sample_end, double opti_steps); 85 | 86 | Eigen::Vector3d DeriveErrAnalytic(const Eigen::Vector3d &vel_angleAxis, const Eigen::Vector3d &pos_angleAxis); 87 | 88 | 89 | Eigen::Vector3d DeriveTimeErrAnalyticRansacBottom(const Eigen::Vector3d &vel_angleAxis, 90 | const std::vector& vec_sampled_idx, double& residuals); 91 | Eigen::Vector3d GetGlobalTimeResidual(); 92 | 93 | // random warp time version 94 | Eigen::Vector3d DeriveTimeErrAnalyticRansac(const Eigen::Vector3d &vel_angleAxis, 95 | const std::vector& vec_sampled_idx, double warp_time, double& residuals); 96 | void getTimeResidual(int sampled_x, int sampled_y, double sampled_time, double warp_time, 97 | double& residual, double& grad_x, double& grad_y); 98 | // Eigen::Vector3d DeriveTimeErrAnalyticLayer(const Eigen::Vector3d &vel_angleAxis, 99 | // const std::vector& vec_sampled_idx, double warp_time, double& residuals); 100 | 101 | void getSampledVec(vector& vec_sampled_idx, int samples_count, double sample_start, double sample_end); 102 | 103 | // visualize 104 | void visualize(); 105 | 106 | // bool inline checkEmpty(){return que_vec_eventData.empty();} 107 | 108 | // file 109 | bool inline file_opened() {return est_velocity_file.is_open();}; 110 | 111 | // thread 112 | // thread* thread_run; 113 | // thread* thread_view; 114 | // std::mutex que_vec_eventData_mutex; 115 | double total_evaluate_time; 116 | 117 | 118 | private: 119 | 120 | // configration 121 | string yaml; 122 | int yaml_iter_num; 123 | float yaml_ts_start; 124 | float yaml_ts_end; 125 | int yaml_sample_count; 126 | int yaml_ceres_iter_num; 127 | int yaml_gaussian_size; 128 | float yaml_gaussian_size_sigma; 129 | int yaml_denoise_num; 130 | float yaml_default_value_factor; 131 | int yaml_ceres_iter_thread; 132 | // motion 133 | vector vec_curr_time; 134 | vector vec_angular_velocity; 135 | vector vec_angular_position; 136 | 137 | 138 | // optimization 139 | cv::Mat cv_3D_surface_index, cv_3D_surface_index_count ; 140 | // camera param 141 | CameraPara camera; 142 | 143 | // image data 144 | ImageData curr_imageData; 145 | 146 | // image output 147 | cv::Mat curr_raw_image, // grey image from ros 148 | curr_undis_image, // undistort grey image 149 | curr_event_image, // current blur event image 150 | curr_undis_event_image, // current undistorted event image 151 | curr_warpped_event_image, // current sharp local event image using est 152 | curr_warpped_event_image_gt, // current sharp local event image using gt 153 | curr_map_image, // global image at t_curr view 154 | hot_image_C1, 155 | hot_image_C3; // time surface with applycolormap 156 | // undistor parameter 157 | cv::Mat undist_mesh_x, undist_mesh_y; 158 | 159 | // event data 160 | ros::Time beginTS; // begin time stamp of ros Time. 161 | 162 | EventBundle eventBundle; // current blur local events 163 | EventBundle event_undis_Bundle; // current undistort local events 164 | EventBundle event_warpped_Bundle; // current sharp local events 165 | EventBundle event_warpped_Bundle_gt; // current sharp local events 166 | EventBundle event_Map_Bundle; // current sharp local events the that warp to t0. 167 | 168 | // std::queue> que_vec_eventData; // saved eventData inorder to save 169 | std::vector> vec_vec_eventData; // saved eventData inorder to save 170 | std::vector> vec_vec_eventDepth; // saved eventData inorder to save 171 | int vec_vec_eventData_iter; // used to indicating the position of unpushed events. 172 | 173 | // map 3d 174 | vector vec_Bundle_Maps; // all the eventbundles that warpped to t0. 175 | 176 | 177 | // event bundle, how many msgs to build a bundle 178 | double delta_time = 0.01; // 0.01 seconds 179 | int max_store_count = int(1e5); // max local event nums 180 | 181 | 182 | // pose 183 | bool using_gt; 184 | vector vec_gt_poseData; // stored rosmsg 185 | 186 | // 逻辑是t2-t1->t0. 如eq(3)所示 187 | Eigen::Vector3d gt_angleAxis ; // gt anglar anxis from t1->t2. = theta / delta_time 188 | Eigen::Vector3d est_angleAxis; // estimated anglar anxis from t1->t2. = theta / delta_time 189 | Eigen::Vector3d est_trans_velocity; // estimated anglar anxis from t1->t2, translation velocity, need mul by delta_time 190 | 191 | Eigen::Matrix last_est_var; // 正则项， 控制est_N_norm的大小 192 | 193 | 194 | // output 195 | string output_dir; 196 | size_t seq_count; 197 | fstream est_theta_file, est_velocity_file; 198 | // fstream est_velocity_file_quat; // evo 199 | 200 | }; 201 | 202 | 203 | 204 | 205 | 206 | -------------------------------------------------------------------------------- /src/dof6_estimator/launch/estimation.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /src/dof6_estimator/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | dof6_estimator 4 | 0.0.0 5 | The dof6_estimator package 6 | 7 | 8 | 9 | 10 | hxt 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 | eigen_conversions 54 | nav_msgs 55 | roscpp 56 | sensor_msgs 57 | std_msgs 58 | tf 59 | cv_bridge 60 | eigen_conversions 61 | nav_msgs 62 | roscpp 63 | sensor_msgs 64 | std_msgs 65 | tf 66 | cv_bridge 67 | eigen_conversions 68 | nav_msgs 69 | roscpp 70 | sensor_msgs 71 | std_msgs 72 | tf 73 | 74 | event_publisher 75 | event_publisher 76 | 77 | dvs_msgs 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | -------------------------------------------------------------------------------- /src/dof6_estimator/src/callbacks.cpp: -------------------------------------------------------------------------------- 1 | #include "callbacks.hpp" 2 | #include "database.hpp" 3 | 4 | ros::Time begin_time = ros::Time(0); 5 | 6 | void ImageGrabber::GrabImage(const sensor_msgs::ImageConstPtr& msg) 7 | { 8 | if(begin_time == ros::Time(0)) 9 | { 10 | begin_time = msg->header.stamp; 11 | system->setBeginTime(msg->header.stamp); 12 | } 13 | 14 | static int last_img_seq = msg->header.seq; 15 | 16 | cv_bridge::CvImageConstPtr cv_ptr; 17 | try 18 | { 19 | cv_ptr = cv_bridge::toCvShare(msg,"mono8"); // gray 8char 20 | } 21 | catch(cv_bridge::Exception& e) 22 | { 23 | ROS_ERROR("receive image error: %s", e.what()); 24 | return ; 25 | } 26 | 27 | // fixme not need to constru a new imageData obj 28 | ImageData ImageData; 29 | ImageData.image = cv_ptr->image.clone(); 30 | ImageData.seq = msg->header.seq; 31 | ImageData.time_stamp = (cv_ptr->header.stamp - begin_time).toSec(); 32 | cout << "receiving image t: " << ImageData.time_stamp<< endl; 33 | 34 | system->pushimageData(ImageData); 35 | } 36 | 37 | 38 | void EventGrabber::GrabEvent(const dvs_msgs::EventDepthArrayConstPtr& msg) 39 | { 40 | if(msg->events.empty() || msg->events.size()<1000) return; 41 | 42 | if(begin_time == ros::Time(0)) 43 | { 44 | begin_time = msg->events[0].ts; 45 | system->setBeginTime(msg->events[0].ts); 46 | cout << "begin time " << msg->events[0].ts.sec << "." <events[0].ts.nsec << endl; 47 | } 48 | 49 | 50 | // not need to copy eventdata obj 51 | // EventData eventdata; 52 | // eventdata.event = msg->events; // vector 53 | double time_stamp = (msg->events[0].ts - begin_time).toSec(); 54 | double delta_time = (msg->events.back().ts-msg->events.front().ts).toSec(); 55 | cout<<"receiving events " << msg->events.size() <<", time: " << msg->events[0].ts.toSec()<<", delta time " << delta_time <que_vec_eventData_mutex.lock(); 58 | 59 | std::vector vec_depth(msg->depths.begin(), msg->depths.end()); 60 | system->pushEventData(msg->events, vec_depth); 61 | // system->que_vec_eventData_mutex.unlock(); 62 | 63 | } 64 | 65 | 66 | void PoseGrabber::GrabPose(const geometry_msgs::PoseStampedConstPtr& msg) 67 | { 68 | 69 | if(begin_time == ros::Time(0)) 70 | { 71 | begin_time = msg->header.stamp; 72 | system->setBeginTime(msg->header.stamp); 73 | } 74 | 75 | // not need to copy eventdata obj 76 | PoseData poseData; 77 | poseData.time_stamp = (msg->header.stamp-begin_time).toSec(); 78 | poseData.time_stamp_ros = msg->header.stamp; 79 | 80 | // vector 81 | poseData.pose << msg->pose.position.x, msg->pose.position.y, msg->pose.position.z; 82 | 83 | // input w,x,y,z. output and store: x,y,z,w 84 | poseData.quat = Eigen::Quaterniond(msg->pose.orientation.w, msg->pose.orientation.x, 85 | msg->pose.orientation.y, msg->pose.orientation.z); 86 | 87 | cout<<"receiving poses t: " << poseData.time_stamp << "， ros: " << std::to_string(poseData.time_stamp_ros.toSec()) << endl; 88 | cout << "----(xyz)(wxyz)" << poseData.pose.transpose() << poseData.quat.coeffs().transpose() << endl; 89 | 90 | 91 | if(que_last_poseData.size() < 10) 92 | { 93 | que_last_poseData.push(poseData); 94 | } 95 | else 96 | { 97 | PoseData last_poseData = que_last_poseData.front(); 98 | que_last_poseData.pop(); 99 | 100 | Eigen::Quaterniond t1_t2 = last_poseData.quat.inverse() * poseData.quat; 101 | // last_poseData.quat.angularDistance(poseData.quat); 102 | 103 | Eigen::Vector3d velocity = toEulerAngles(t1_t2) * 1 / (poseData.time_stamp_ros - last_poseData.time_stamp_ros).toSec(); 104 | 105 | // get middle timestamp of event bundle 106 | uint32_t second = last_poseData.time_stamp_ros.sec; 107 | uint32_t nsecond; 108 | if(last_poseData.time_stamp_ros.nsec > poseData.time_stamp_ros.nsec) 109 | { 110 | // example: first 1.12, last 1.15; 111 | nsecond = last_poseData.time_stamp_ros.nsec + (poseData.time_stamp_ros.nsec-last_poseData.time_stamp_ros.nsec)/2; 112 | } 113 | else 114 | { 115 | uint32_t delta_to_second = uint32_t(1000000000) - last_poseData.time_stamp_ros.nsec; 116 | // example: first 1.96, last 1.01; 117 | if(delta_to_second > poseData.time_stamp_ros.nsec) 118 | { 119 | nsecond = last_poseData.time_stamp_ros.nsec + (delta_to_second+poseData.time_stamp_ros.nsec) / 2; 120 | } 121 | // example: first 1.99, last 1.02; 122 | else 123 | { 124 | nsecond = (poseData.time_stamp_ros.nsec - delta_to_second) / 2; 125 | second++; 126 | } 127 | } 128 | 129 | gt_velocity_file << poseData.time_stamp_ros.sec << " " << poseData.time_stamp_ros.nsec <<" " << velocity.transpose() << endl; 130 | 131 | // evo part 132 | // // todo from quat to agnles axis and divide by time, get velocity 133 | // Eigen::AngleAxisd angle_axis = Eigen::AngleAxisd(t1_t2); 134 | // // cout << "angle axis " << angle_axis.axis() << "," << angle_axis.angle() << endl; 135 | // angle_axis.angle() = angle_axis.angle() / (poseData.time_stamp_ros - last_poseData.time_stamp_ros).toSec(); 136 | // // cout << "angle axis " << angle_axis.axis() << "," << angle_axis.angle() << endl; 137 | // Eigen::Quaterniond q = Eigen::Quaterniond(angle_axis); 138 | 139 | // gt_velocity_file_quat << poseData.time_stamp_ros << " 0 0 0 " << q.x() << " " 140 | // << q.y() << " " << q.z() << " " << q.w() << endl; 141 | 142 | last_poseData = poseData; 143 | que_last_poseData.push(poseData); 144 | } 145 | 146 | 147 | // system->pushPoseData(poseData); 148 | } -------------------------------------------------------------------------------- /src/dof6_estimator/src/database.cpp: -------------------------------------------------------------------------------- 1 | #include "database.hpp" 2 | 3 | 4 | using namespace std; 5 | 6 | 7 | CameraPara::CameraPara(string filename) 8 | { 9 | std::ifstream fin(filename.c_str()); 10 | if (!fin.is_open()) 11 | { 12 | cout << "failed opening calib file " << endl; 13 | return ; 14 | } 15 | 16 | fin >> fx >> fy >> cx >> cy >> k1 >> k2 >> p1 >> p2 >> k3 >> width >> height; 17 | 18 | cameraMatrix = (cv::Mat_(3,3) << fx, 0, cx , 0, fy, cy, 0, 0, 1); 19 | distCoeffs = (cv::Mat_(1,5) << k1, k2, p1, p2, k3); 20 | 21 | // cout << "camera param:" << endl; 22 | // cout << cameraMatrix << endl; 23 | 24 | cv::cv2eigen(cameraMatrix, eg_cameraMatrix); 25 | // cout << eg_cameraMatrix << endl; 26 | 27 | // the map展示的虚拟相机k内参可以自定义大小。 28 | height_map = 2 * height; 29 | width_map = 2 * width; 30 | eg_MapMatrix = eg_cameraMatrix; // deep copy 31 | 32 | eg_MapMatrix(0,2) = width_map/2; 33 | eg_MapMatrix(1,2) = height_map/2; 34 | 35 | cout << "camera matrix:: \n" << cameraMatrix << endl; 36 | cout << "camera matrix eigen: \n" << eg_cameraMatrix << endl; 37 | cout << "map matrix eigen: \n" << eg_MapMatrix << endl; 38 | } 39 | 40 | CameraPara::CameraPara() 41 | { 42 | width = 346; 43 | height = 260; 44 | 45 | // indoor_driving1 46 | // fx = 226.38018519795807; 47 | // fy = 226.15002947047415; 48 | // cx = 173.6470807871759; 49 | // cy = 133.73271487507847; 50 | // k1 = -0.048031442223833355; 51 | // k2 = 0.011330957517194437; 52 | // p1 = -0.055378166304281135; 53 | // p2 = 0.021500973881459395; 54 | // k3 = 0.0; 55 | 56 | 57 | // outdoor_driving1 58 | // fx = 223.1117516327428; 59 | // fy = 222.9723608800784; 60 | // cx = 174.7696749980204; 61 | // cy = 128.89224876369192; 62 | // k1 = -0.023142330649874532; 63 | // k2 = -0.03852833693290194; 64 | // p1 = 0.007851617759475266; 65 | // p2 = 0.008707820879327087; 66 | // k3 = 0.0; 67 | 68 | 69 | // cameraMatrix = (cv::Mat_(3,3) << fx, 0, cx , 0, fy, cy, 0, 0, 1); 70 | // distCoeffs = (cv::Mat_(1,5) << k1, k2, p1, p2, k3); 71 | 72 | // // cout << "camera param:" << endl; 73 | // // cout << cameraMatrix << endl; 74 | 75 | // cv::cv2eigen(cameraMatrix, eg_cameraMatrix); 76 | // // cout << eg_cameraMatrix << endl; 77 | 78 | // // the map展示的虚拟相机k内参可以自定义大小。 79 | // height_map = 2 * height; 80 | // width_map = 2 * width; 81 | // eg_MapMatrix = eg_cameraMatrix; // deep copy 82 | 83 | // eg_MapMatrix(0,2) = width_map/2; 84 | // eg_MapMatrix(1,2) = height_map/2; 85 | 86 | // cout << "camera matrix:: \n" << cameraMatrix << endl; 87 | // cout << "camera matrix eigen: \n" << eg_cameraMatrix << endl; 88 | // cout << "map matrix eigen: \n" << eg_MapMatrix << endl; 89 | 90 | } 91 | 92 | /** 93 | * \brief make a reference, TODO it a more efficient way?. 94 | */ 95 | EventBundle::EventBundle() 96 | { 97 | size = 0; 98 | coord.resize(2, size); 99 | coord_3d.resize(3, size); 100 | } 101 | 102 | 103 | /** 104 | * \brief make a reference, TODO it a more efficient way?. 105 | */ 106 | EventBundle::EventBundle(const EventBundle& eb) 107 | { 108 | // deep copy 109 | coord = eb.coord; 110 | coord_3d = eb.coord_3d; 111 | isInner = eb.isInner; 112 | 113 | 114 | angular_position = eb.angular_position; 115 | angular_velocity = eb.angular_velocity; 116 | 117 | time_delta = eb.time_delta; 118 | 119 | size = eb.size; 120 | 121 | // x = eb.x; 122 | // y = eb.y; 123 | polar = eb.polar; 124 | 125 | } 126 | 127 | 128 | /** 129 | * \brief reset all. 130 | */ 131 | void EventBundle::Clear() 132 | { 133 | // cout << "event bundle clearing, size " << size << endl; 134 | 135 | size = 0; 136 | 137 | // eigen reconstruct 138 | coord.resize(2,size); 139 | coord_3d.resize(3,size); 140 | isInner.resize(size); 141 | 142 | time_delta.resize(size); 143 | time_delta_rev.resize(size); 144 | 145 | angular_position = Eigen::Vector3d::Zero(); 146 | angular_velocity = Eigen::Vector3d::Zero(); 147 | 148 | // vector 149 | // time_stamps.clear(); 150 | // x.clear(); 151 | // y.clear(); 152 | // isInner.clear(); 153 | 154 | // cout << " event bundle clearing sucess" << endl; 155 | } 156 | 157 | 158 | /** 159 | * \brief strong DiscriminateInner, for the convinient of gradient . 160 | * \param width given boundary, may camera boundary or map view boundary 161 | * \param height 162 | */ 163 | void EventBundle::DiscriminateInner(int width, int height) 164 | { 165 | // cout << "DiscriminateInner "<< size << endl; 166 | isInner.resize(size); 167 | // if(x.size() != isInner.size()) cout << "inner wrong size" << endl; 168 | 169 | for(uint32_t i = 0; i < size; ++i) 170 | { 171 | if(coord(0,i)<3 || coord(0,i)>=(width-3) || coord(1,i)<3 || coord(1,i)>=(height-3)) 172 | isInner[i] = 0; 173 | else isInner[i] = 1; 174 | } 175 | } 176 | 177 | EventBundle::~EventBundle(){ 178 | 179 | } 180 | 181 | void EventBundle::Append(std::vector& vec_eventData, std::vector& vec_eventDepth) 182 | { 183 | if(size == 0) 184 | { 185 | // cout << "first appending events, "<< vec_eventData.size() << endl; 186 | first_tstamp = vec_eventData.front().ts; 187 | // abs_tstamp = eventData.time_stamp; 188 | } 189 | else 190 | { 191 | cout << "appending events" << endl; 192 | } 193 | 194 | last_tstamp = vec_eventData.back().ts; 195 | 196 | size_t old_size = size; 197 | size += vec_eventData.size(); 198 | 199 | coord.conservativeResize(2,size); 200 | coord_3d.conservativeResize(3,size); 201 | polar.conservativeResize(size); 202 | 203 | time_delta.conservativeResize(size); 204 | time_delta_rev.conservativeResize(size); // not used 205 | 206 | int counter = 0; 207 | for(int i=0; i& vec_eventData) 233 | { 234 | if(size == 0) 235 | { 236 | // cout << "first appending events, "<< vec_eventData.size() << endl; 237 | first_tstamp = vec_eventData.front().ts; 238 | // abs_tstamp = eventData.time_stamp; 239 | } 240 | else 241 | { 242 | cout << "appending events" << endl; 243 | } 244 | 245 | last_tstamp = vec_eventData.back().ts; 246 | 247 | size_t old_size = size; 248 | size += vec_eventData.size(); 249 | 250 | coord.conservativeResize(2,size); 251 | coord_3d.conservativeResize(3,size); 252 | polar.conservativeResize(size); 253 | 254 | time_delta.conservativeResize(size); 255 | time_delta_rev.conservativeResize(size); // not used 256 | 257 | int counter = 0; 258 | for(const auto& i: vec_eventData) 259 | { 260 | // TODO sampler 261 | 262 | // x.push_back(i.x); 263 | // y.push_back(i.y); 264 | polar(old_size+counter) = i.polarity==0; 265 | 266 | coord(0, old_size+counter) = i.x; 267 | coord(1, old_size+counter) = i.y; 268 | coord_3d(2,old_size+counter) = 1; 269 | time_delta(old_size+counter) = (i.ts - first_tstamp).toSec(); 270 | counter++; 271 | } 272 | 273 | // deep copy 274 | // coord_3d.topRightCorner(2,size-old_size) = coord.topRightCorner(2,size-old_size); 275 | 276 | // cout << "coord example" << endl; 277 | // for(int i=0; i< 5; i++) 278 | // cout << eventData.event[i].x << "," << eventData.event[i].y << endl; 279 | // cout << coord.topLeftCorner(2,5) << endl; 280 | // cout << coord_3d.topLeftCorner(3,5) << endl; 281 | } 282 | 283 | 284 | /** 285 | * \brief 6dof version, since 3d->2D points should keep the same with given x,y, we multi depth to x, y. 286 | */ 287 | void EventBundle::InverseProjection(Eigen::Matrix3d& K, Eigen::Matrix3Xd& raw_coord_3d) 288 | { 289 | if(coord_3d.cols() != size) 290 | { 291 | coord_3d.resize(3, size); 292 | cout << "resizing coord_3d" << endl; 293 | } 294 | 295 | // cout << coord.topLeftCorner(2,5) << endl; 296 | // cout << coord_3d.topLeftCorner(3,5) << endl; 297 | // cout << raw_coord_3d.topLeftCorner(3,5) << endl; 298 | coord_3d.row(0) = (coord.row(0).array()-K(0,2)) * raw_coord_3d.row(2).array() / K(0,0) ; 299 | coord_3d.row(1) = (coord.row(1).array()-K(1,2)) * raw_coord_3d.row(2).array() / K(1,1) ; 300 | coord_3d.row(2) = raw_coord_3d.row(2).eval(); 301 | // cout << coord_3d.topLeftCorner(3,5) << endl; 302 | 303 | } 304 | 305 | /** 306 | * \brief project camera to unisophere 307 | * \param K, camera proj matrix. from coor -> coor_3d 308 | */ 309 | void EventBundle::InverseProjection(Eigen::Matrix3d& K) 310 | { 311 | // eigen array pixel-wise operates 312 | 313 | // cout << "inverse project \n " << endl; 314 | // cout << "coord size " << coord.size() << "," << coord_3d.size() << endl; 315 | 316 | if(coord_3d.cols() != size) 317 | { 318 | coord_3d.resize(3, size); 319 | cout << "resizing coord_3d" << endl; 320 | } 321 | 322 | // cout << coord_3d.topLeftCorner(3,5) << endl; 323 | 324 | 325 | coord_3d.row(0) = (coord.row(0).array()-K(0,2)) / K(0,0) ; 326 | coord_3d.row(1) = (coord.row(1).array()-K(1,2)) / K(1,1) ; 327 | 328 | // 6dof dose not need to change depth 329 | // coord_3d.row(2) = Eigen::MatrixXd::Ones(1, size); 330 | // coord_3d.colwise().normalize(); 331 | 332 | // cout << coord_3d.topLeftCorner(3,5) << endl; 333 | 334 | // cout << coord_3d.bottomRightCorner(3,5) << endl; 335 | 336 | } 337 | 338 | 339 | /** 340 | * \brief project camera to unisophere 341 | * \param K, camera proj matrix. from coor_3d -> coor 342 | */ 343 | void EventBundle::Projection(Eigen::Matrix3d& K) 344 | { 345 | // eigen array pixel-wise operates 346 | if(coord.cols() != size) 347 | { 348 | coord.resize(3, size); 349 | cout << "---------wronging----------" << endl; 350 | cout << "resizing coord 2d" << endl; 351 | } 352 | 353 | coord.row(0) = coord_3d.row(0).array() / coord_3d.row(2).array() * K(0,0) + K(0,2); 354 | coord.row(1) = coord_3d.row(1).array() / coord_3d.row(2).array() * K(1,1) + K(1,2); 355 | 356 | // TODO add gaussian 357 | // coord = coord.array().round(); // pixel wise 358 | 359 | 360 | // cout << " projecting sucess " << endl; 361 | } 362 | 363 | 364 | /** 365 | * \brief copy the size paramter of another eventbundle. 366 | */ 367 | void EventBundle::CopySize(const EventBundle& ref) 368 | { 369 | // time_delta = ref.time_delta; 370 | 371 | size = ref.size; 372 | 373 | coord.resize(2,size); 374 | coord_3d.resize(3,size); 375 | isInner.resize(size); 376 | 377 | 378 | } -------------------------------------------------------------------------------- /src/dof6_estimator/src/dof6_estimator_node.cpp: -------------------------------------------------------------------------------- 1 | // std 2 | #include 3 | #include 4 | 5 | // third party 6 | 7 | // ros 8 | #include 9 | #include "callbacks.hpp" 10 | #include "system.hpp" 11 | #include "event_reader.hpp" 12 | // #include 13 | 14 | 15 | using namespace std; 16 | 17 | 18 | // rosbag play -r 0.1 ~/Documents/rosbag/Mono-rosbag/slider_depth.bag 19 | 20 | int main(int argc, char** argv) 21 | { 22 | 23 | ros::init(argc, argv, "rotation_estimator"); 24 | ros::start(); 25 | 26 | ros::NodeHandle nh("~"); 27 | 28 | string yaml; // system configration 29 | nh.param("yaml", yaml, ""); 30 | 31 | System* sys = new System(yaml); 32 | if(!sys->file_opened()) 33 | { 34 | cout << "failed opening file " << endl; 35 | return 0; 36 | } 37 | 38 | /* Event ROS version */ 39 | // ImageGrabber imageGrabber(&sys); 40 | // ros::Subscriber image_sub = nh.subscribe("/dvs/image_raw", 10, &ImageGrabber::GrabImage, &imageGrabber); 41 | // // PoseGrabber poseGrabber(sys); 42 | // // ros::Subscriber pose_sub = nh.subscribe("/optitrack/davis", 10, &PoseGrabber::GrabPose, &poseGrabber); 43 | // EventGrabber eventGrabber(sys); 44 | // ros::Subscriber event_sub = nh.subscribe("/events_depth", 10, &EventGrabber::GrabEvent, &eventGrabber); 45 | // ros::spin(); 46 | 47 | /** Event TXT version */ 48 | // ros::Time t1 = ros::Time::now(); 49 | Event_reader event_reader(yaml); 50 | while (true) 51 | { 52 | // read data 53 | dvs_msgs::EventArrayPtr msg_ptr = dvs_msgs::EventArrayPtr(new dvs_msgs::EventArray()); 54 | std::vector vec_depth; 55 | event_reader.acquire(msg_ptr, vec_depth); 56 | 57 | if(msg_ptr==nullptr || msg_ptr->events.empty() ) 58 | { 59 | cout << "wrong reading events, msgptr==null" << int(msg_ptr==nullptr) << "empty events " << int(msg_ptr->events.empty()) << endl; 60 | break; 61 | } 62 | 63 | sys->pushEventData(msg_ptr->events, vec_depth); 64 | // cout << "success reveive" << endl; 65 | // break; 66 | } 67 | // ros::Time t2 = ros::Time::now(); 68 | cout << "total optimize time " << sys->total_evaluate_time << endl; 69 | 70 | cout << "shutdown rotation estimator" << endl; 71 | return 0; 72 | } 73 | 74 | -------------------------------------------------------------------------------- /src/dof6_estimator/src/numerics.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "numerics.hpp" 3 | 4 | #include 5 | using namespace std; 6 | /** 7 | * \brief convert x[1x3] to asym matrix [3x3]. 8 | * \param[in] x . 9 | */ 10 | Eigen::Matrix3d hat(const Eigen::Vector3d &x) 11 | { 12 | Eigen::Matrix3d x_hat; 13 | x_hat << 0, -x(2), x(1), 14 | x(2), 0, -x(0), 15 | -x(1), x(0), 0; 16 | return x_hat; 17 | } 18 | 19 | 20 | /** 21 | * \brief from asy matrix [3x3] to vector [1x3]. 22 | */ 23 | Eigen::Vector3d unhat(const Eigen::Matrix3d &x_hat) 24 | { 25 | Eigen::Vector3d x; 26 | x << x_hat(2,1), x_hat(0,2), x_hat(1,0); 27 | return x; 28 | } 29 | 30 | /** 31 | * \brief from 3 AngleAxis theta to rotation matrix. 32 | * \param rpy rotation angle in rad format. . 33 | */ 34 | Eigen::Matrix3d SO3(const Eigen::Vector3d &x) 35 | { 36 | // cout << " hat(x) \n" << hat(x).transpose() << endl; 37 | // cout << " exp " << hat(x).exp().transpose() << endl; 38 | // cout << " matrix \n " << Eigen::Matrix3d(hat(x).exp()) << endl; 39 | 40 | return Eigen::Matrix3d(hat(x).exp()); 41 | } 42 | 43 | /** 44 | * \brief Constructor. 45 | * \param x_hat. 46 | */ 47 | Eigen::Vector3d InvSO3(const Eigen::Matrix3d &R) 48 | { 49 | 50 | Eigen::Matrix3d temp = R.log(); 51 | 52 | // cout << " R.log() \n" << R.log() << endl; 53 | // cout << " unhat(R.log()) \n" << unhat(R.log()) << endl; 54 | // cout << " unhat(()) \n" << unhat((R.log())) << endl; 55 | // cout << "R.log xyz" << temp(2,1)<< "," << temp(0,2)<<"," << temp(1,0) << endl; 56 | // return unhat(R.log()); 57 | return unhat(temp); 58 | 59 | } 60 | 61 | /** 62 | * \brief from 3 theta to rotation matrix. 63 | * \param rpy rotation angle in rad format. . 64 | */ 65 | Eigen::Vector3d SO3add(const Eigen::Vector3d x1, const Eigen::Vector3d x2, bool circle) 66 | { 67 | if (circle && (x1 + x2).norm() > M_PI) 68 | { 69 | return x1 + x2; 70 | } 71 | else 72 | { 73 | // cout << " SO3(x1) \n" << SO3(x1) << endl; 74 | // cout << " SO3(x1) * 2\n " << SO3(x1) * SO3(x2) << endl; 75 | // cout << " return \n " << InvSO3(SO3(x1) * SO3(x2)) << endl; 76 | return InvSO3(SO3(x1) * SO3(x2)); 77 | } 78 | } 79 | 80 | 81 | /** 82 | * \brief from quaternion to euler anglers， return rpy(xyz) theta. 83 | */ 84 | Eigen::Vector3d toEulerAngles(Eigen::Quaterniond q){ 85 | MyEulerAngles angles; 86 | 87 | // roll (x-axis rotation) 88 | double sinr_cosp = 2 * (q.w() * q.x() + q.y() * q.z()); 89 | double cosr_cosp = 1 - 2 * (q.x() * q.x() + q.y() * q.y()); 90 | angles.roll = std::atan2(sinr_cosp, cosr_cosp); 91 | 92 | // pitch (y-axis rotation) 93 | double sinp = 2 * (q.w() * q.y() - q.z() * q.x()); 94 | if (std::abs(sinp) >= 1) 95 | angles.pitch = std::copysign(M_PI / 2, sinp); // use 90 degrees if out of range 96 | else 97 | angles.pitch = std::asin(sinp); 98 | 99 | // yaw (z-axis rotation) 100 | double siny_cosp = 2 * (q.w() * q.z() + q.x() * q.y()); 101 | double cosy_cosp = 1 - 2 * (q.y() * q.y() + q.z() * q.z()); 102 | angles.yaw = std::atan2(siny_cosp, cosy_cosp); 103 | 104 | Eigen::Vector3d v3d(angles.roll, angles.pitch, angles.yaw); 105 | return v3d; 106 | } 107 | 108 | 109 | Eigen::Quaterniond ToQuaternion(double yaw, double pitch, double roll) // yaw (Z), pitch (Y), roll (X) 110 | { 111 | // Abbreviations for the various angular functions 112 | double cy = cos(yaw * 0.5); 113 | double sy = sin(yaw * 0.5); 114 | double cp = cos(pitch * 0.5); 115 | double sp = sin(pitch * 0.5); 116 | double cr = cos(roll * 0.5); 117 | double sr = sin(roll * 0.5); 118 | 119 | double w = cr * cp * cy + sr * sp * sy; 120 | double x = sr * cp * cy - cr * sp * sy; 121 | double y = cr * sp * cy + sr * cp * sy; 122 | double z = cr * cp * sy - sr * sp * cy; 123 | 124 | 125 | Eigen::Quaterniond q(w,x,y,z); 126 | // std::cout <<"Quaterniond: "<< q.coeffs().transpose() << ", norm: "<< q.coeffs().norm() << std::endl; 127 | return q; 128 | } 129 | 130 | 131 | /** 132 | * \brief . 133 | * \param type is opencv type CV_float or 134 | */ 135 | double getVar(cv::Mat& image, int& count_nozero, int type) 136 | { 137 | // std::cout << image.channels() << std::endl; 138 | 139 | // cv::Mat grayimage; 140 | // // cv::normalize(image, grayimage,0,255,cv::NORM_MINMAX, CV_8UC3); 141 | // image.convertTo(grayimage,CV_8UC3); 142 | // cv::cvtColor(grayimage, grayimage, cv::COLOR_BGR2GRAY, 1); 143 | // cv::imshow("image", image) ; 144 | // cv::imshow("normalize", grayimage) ; 145 | // cv::imshow("grayimage", grayimage) ; 146 | // cv::waitKey(0); 147 | 148 | assert(image.channels() == 1); 149 | // assert(image.type() == CV_16UC1); 150 | 151 | double mean = 0; 152 | count_nozero = 1; 153 | for(int x = 0; x < image.cols; x++) 154 | for(int y = 0; y < image.rows; y++) 155 | { 156 | double value = 1000; 157 | if(type == CV_16U) 158 | value = image.at(y,x); 159 | if(type == CV_32F) 160 | value = image.at(y,x); 161 | 162 | if(value>0) 163 | { 164 | mean += value; 165 | count_nozero++; 166 | } 167 | } 168 | 169 | mean /= count_nozero; 170 | 171 | double var = 0; 172 | for(int x = 0; x < image.cols; x++) 173 | for(int y = 0; y < image.rows; y++) 174 | { 175 | 176 | double value = 1000; 177 | if(type == CV_16U) 178 | value = image.at(y,x); 179 | if(type == CV_32F) 180 | value = image.at(y,x); 181 | if(value>0) 182 | { 183 | var += std::pow(value-mean,2); 184 | } 185 | } 186 | var /= count_nozero; 187 | return var; 188 | } -------------------------------------------------------------------------------- /src/dof6_estimator/src/system.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "system.hpp" 3 | 4 | 5 | using namespace std; 6 | 7 | System::System(const string& yaml) 8 | { 9 | cout << "init system" << endl; 10 | 11 | cv::FileStorage fSettings(yaml, cv::FileStorage::READ); 12 | if(!fSettings.isOpened()) 13 | { 14 | ROS_ERROR("counld not open file %s", yaml.c_str()); 15 | } 16 | 17 | string calib_dir = fSettings["calib_dir"]; 18 | camera = CameraPara(calib_dir); 19 | 20 | yaml_iter_num = fSettings["yaml_iter_num"]; 21 | yaml_ts_start = fSettings["yaml_ts_start"]; 22 | yaml_ts_end = fSettings["yaml_ts_end"]; 23 | yaml_sample_count = fSettings["yaml_sample_count"]; 24 | yaml_ceres_iter_thread = fSettings["yaml_ceres_iter_thread"]; 25 | yaml_ceres_iter_num = fSettings["yaml_ceres_iter_num"]; 26 | yaml_gaussian_size = fSettings["yaml_gaussian_size"]; 27 | yaml_gaussian_size_sigma = fSettings["yaml_gaussian_size_sigma"]; 28 | yaml_denoise_num = fSettings["yaml_denoise_num"]; 29 | yaml_default_value_factor = fSettings["yaml_default_value_factor"]; 30 | 31 | 32 | // undistore data 33 | undist_mesh_x, undist_mesh_y; 34 | cv::initUndistortRectifyMap(camera.cameraMatrix, camera.distCoeffs, 35 | cv::Mat::eye(3,3,CV_32FC1), camera.cameraMatrix, cv::Size(camera.width, camera.height), 36 | CV_32FC1, undist_mesh_x, undist_mesh_y); 37 | 38 | using_gt = false; 39 | vec_vec_eventData_iter = 0; 40 | seq_count = 1; 41 | 42 | // ros msg 43 | // vec_last_event_idx = 0; 44 | 45 | // visualize 46 | // before processing 47 | // cv::namedWindow("curr_raw_image", cv::WINDOW_NORMAL); 48 | // cv::namedWindow("curr_undis_image", cv::WINDOW_NORMAL); 49 | // cv::namedWindow("curr_event_image", cv::WINDOW_NORMAL); 50 | 51 | // after processing 52 | cv::namedWindow("curr_undis_event_image", cv::WINDOW_NORMAL); 53 | cv::namedWindow("curr_warpped_event_image", cv::WINDOW_NORMAL); 54 | // cv::namedWindow("curr_warpped_event_image_gt", cv::WINDOW_NORMAL); 55 | // cv::namedWindow("curr_map_image", cv::WINDOW_NORMAL); 56 | // cv::namedWindow("hot_image_C3", cv::WINDOW_NORMAL); 57 | // cv::namedWindow("timesurface_early", cv::WINDOW_NORMAL); 58 | // cv::namedWindow("timesurface_later", cv::WINDOW_NORMAL); 59 | // cv::namedWindow("opti", cv::WINDOW_NORMAL); 60 | 61 | // before processing 62 | curr_undis_image = cv::Mat(camera.height,camera.width, CV_8U); 63 | curr_raw_image = cv::Mat(camera.height,camera.width, CV_8U); 64 | curr_event_image = cv::Mat(camera.height,camera.width, CV_32FC3); 65 | hot_image_C1 = cv::Mat(camera.height,camera.width, CV_8UC1); 66 | hot_image_C3 = cv::Mat(camera.height,camera.width, CV_8UC3); 67 | 68 | // optimizeing 69 | est_angleAxis = Eigen::Vector3d(0,0,0); // set to 0. 70 | // int dims[] = {180,240,20}; // row, col, channels // useless 71 | // cv_3D_surface_index = cv::Mat(3, dims, CV_32S); // useless 72 | // cv_3D_surface_index_count = cv::Mat(180, 240, CV_32S); // useless 73 | 74 | // after processing 75 | curr_undis_event_image = cv::Mat(camera.height,camera.width, CV_32F); 76 | curr_map_image = cv::Mat(camera.height_map,camera.width_map, CV_32F); 77 | curr_warpped_event_image = cv::Mat(camera.height,camera.width, CV_32F); 78 | curr_warpped_event_image_gt = cv::Mat(camera.height,camera.width, CV_32F); 79 | 80 | // output file 81 | output_dir = string(fSettings["output_dir"]); 82 | string output_path = output_dir + std::to_string(yaml_sample_count) + 83 | "_timerange(0." + std::to_string(int(yaml_ts_start*10)) +"-0." +std::to_string(int(yaml_ts_end*10)) + ")"+ 84 | "_iter"+ std::to_string(yaml_iter_num) + "_ceres" + std::to_string(yaml_ceres_iter_num)+ 85 | "_gaussan" +std::to_string(yaml_gaussian_size) +"_sigma"+std::to_string(int(yaml_gaussian_size_sigma)) +"." +std::to_string(int(yaml_gaussian_size_sigma*10)%10)+ 86 | "_denoise" + std::to_string(yaml_denoise_num) + ".txt"; 87 | // "_defaultval" +std::to_string(int(yaml_default_value_factor)) +"." +std::to_string(int(yaml_default_value_factor*10)%10)+ ".txt"; 88 | cout << "open file " << output_path << endl; 89 | 90 | if(!fstream(output_path, ios::in).is_open()) 91 | { 92 | cout << "creating file " << endl; 93 | est_velocity_file = fstream(output_path, ios::out); 94 | } 95 | 96 | // est_velocity_file_quat = fstream("/home/hxy/Desktop/data/evo_data/ransac_velocity.txt", ios::out); 97 | 98 | 99 | // thread in background 100 | // thread_view = new thread(&System::visualize, this); 101 | // thread_run = new thread(&System::Run, this); 102 | 103 | 104 | // init 105 | total_evaluate_time = 0; 106 | 107 | est_angleAxis = Eigen::Vector3d(0.01,0.01,0.01); // estimated anglar anxis from t2->t1. = theta / delta_time 108 | est_trans_velocity = Eigen::Vector3d(0.01,0.01,0.01); // estimated anglar anxis from t2->t1, translation velocity, need mul by delta_time 109 | last_est_var << 0.01,0.01,0.01,0.01,0.01,0.01; 110 | 111 | } 112 | 113 | System::~System() 114 | { 115 | cout << "saving files " << endl; 116 | 117 | // delete thread_run; 118 | cv::destroyAllWindows(); 119 | 120 | est_velocity_file.close(); 121 | // est_velocity_file_quat.close(); 122 | 123 | } 124 | 125 | 126 | /** 127 | * \brief undistr events, and save the data to event_undis_Bundle (2d and 3d). 128 | */ 129 | void System::undistortEvents() 130 | { 131 | int point_size = eventBundle.size; 132 | // cout << "------unditort events num:" << point_size << endl; 133 | // cout << "------undisotrt eventBundle cols " << eventBundle.coord.rows() << "," << eventBundle.coord.cols() << endl; 134 | 135 | // vector raw_event_points(point_size), undis_event_points(point_size); 136 | 137 | // for(size_t i=0; i(1,4) << 0, 0, 0 ,0 ); 149 | // cv::undistortPoints(raw_event_points, undis_event_points, 150 | // camera.cameraMatrix,undis , cv::noArray(), camera.cameraMatrix); 151 | // } 152 | 153 | // convert points to cv_mat 154 | // cv::Mat temp_mat = cv::Mat(undis_event_points); 155 | // temp_mat = temp_mat.reshape(1,point_size); // channel 1, row = 2 156 | // cv::transpose(temp_mat, temp_mat); 157 | 158 | // // convert cv2eigen 159 | // event_undis_Bundle.CopySize(eventBundle); 160 | // cv::cv2eigen(temp_mat, event_undis_Bundle.coord); 161 | 162 | 163 | event_undis_Bundle.CopySize(eventBundle); 164 | event_undis_Bundle.coord = eventBundle.coord; 165 | // store 3d data 166 | // cout << event_undis_Bundle.coord.topLeftCorner(2,5) << endl; 167 | 168 | event_undis_Bundle.InverseProjection(camera.eg_cameraMatrix, eventBundle.coord_3d); 169 | // event_undis_Bundle.coord_3d = eventBundle.coord_3d; 170 | 171 | // store inner 172 | event_undis_Bundle.DiscriminateInner(camera.width, camera.height); 173 | 174 | getImageFromBundle(event_undis_Bundle, PlotOption::U16C3_EVNET_IMAGE_COLOR, false).convertTo(curr_undis_event_image, CV_32F); 175 | // cout << "success undistort events " << endl; 176 | } 177 | 178 | 179 | /** 180 | * \brief Constructor. 181 | * \param is_mapping means using K_map image size. 182 | * \param cv_3D_surface_index store index (height, width, channel) 183 | * \param cv_3D_surface_index_count store index count (height, width, count) 184 | */ 185 | cv::Mat System::getImageFromBundle(EventBundle& cur_event_bundle, const PlotOption option, bool is_mapping /*=false*/) 186 | { 187 | 188 | // cout << "getImageFromBundle " << cur_event_bundle.coord.cols() << ", is_mapping "<< is_mapping << endl; 189 | // cout << "enter for interval " << "cols " << cur_event_bundle.isInner.rows()<< endl; 190 | 191 | cv::Mat image; 192 | // cv::Mat img_surface_index; // store time index 193 | 194 | int max_count = 0; 195 | 196 | 197 | int width = camera.width, height = camera.height; 198 | 199 | if(is_mapping) 200 | { 201 | width = camera.width_map; 202 | height = camera.height_map; 203 | } 204 | // cout << " image size (h,w) = " << height << "," << width << endl; 205 | 206 | switch (option) 207 | { 208 | case PlotOption::U16C3_EVNET_IMAGE_COLOR: 209 | // cout << " choosing U16C3_EVNET_IMAGE_COLOR" << endl; 210 | image = cv::Mat(height,width, CV_16UC3); 211 | image = cv::Scalar(0,0,0); // clear first 212 | 213 | for(int i = cur_event_bundle.coord.cols()-1; i>0; i--) 214 | // for(int i=0; i= width || x < 0 || y >= height|| y < 0 ) 226 | cout << "x, y" << x << "," << y << endl; 227 | 228 | // cout << "x, y" << x << "," << y << endl; 229 | 230 | cv::Point2i point_temp(x,y); 231 | 232 | image.at(point_temp) += eventBundle.polar[i] > 0 ? cv::Vec3w(0, 0, 1) : cv::Vec3w(1, 0, 0); 233 | } 234 | 235 | // cout << "image size" << image.size() <= width || x < 0 || y >= height || y < 0 ) 251 | cout << "x, y" << x << "," << y << endl; 252 | 253 | cv::Point2i point_temp(x,y); 254 | image.at(point_temp) += 1; 255 | } 256 | break; 257 | case PlotOption::U8C1_EVNET_IMAGE: 258 | // cout << "enter case U16C1_EVNET_IMAGE" << endl; 259 | image = cv::Mat(height, width, CV_8UC1); 260 | image = cv::Scalar(0); 261 | 262 | for(int i=0; i= width || x < 0 || y >= height || y < 0 ) 270 | // cout << "x, y" << x << "," << y << endl; 271 | 272 | cv::Point2i point_temp(x,y); 273 | image.at(point_temp) += 1; 274 | } 275 | break; 276 | case PlotOption::TIME_SURFACE: 277 | // cout << "build time surface " << endl; 278 | // cv_3D_surface_index.setTo(0); cv_3D_surface_index_count.setTo(0); 279 | image = cv::Mat(height, width, CV_32FC1); 280 | image = cv::Scalar(0); 281 | 282 | for(int i=0; i= width || x < 0 || y >= height || y < 0 ) 290 | cout << "x, y" << x << "," << y << endl; 291 | 292 | image.at(y,x) = 0.1 - eventBundle.time_delta(i); // only for visualization 293 | 294 | // cv_3D_surface_index.at(y,x,cv_3D_surface_index_count.at(y,x)) = i; 295 | // cv_3D_surface_index_count.at(y,x) += 1; 296 | // max_count = std::max(max_count, cv_3D_surface_index_count.at(y,x)); 297 | 298 | // cout << eventBundle.time_delta(i)<< endl; 299 | // img_surface_index.at(y,x) = i; 300 | // cout << eventBundle.time_delta(i) << endl; 301 | } 302 | 303 | // cout << "max_count channels " << max_count << endl; 304 | // cout << "size " << eventBundle.time_delta.size() << endl; 305 | // cout << "size " << cur_event_bundle.coord.cols() << endl; 306 | break; 307 | case PlotOption::F32C1_EVENT_COUNT: 308 | image = cv::Mat(height, width, CV_32FC1); 309 | image = cv::Scalar(0); 310 | 311 | for(int i=0; i= width-1 || x < 0 || y >= height-1 || y < 0 ) 321 | // cout << "x, y" << x << "," << y << endl; 322 | 323 | image.at(y,x) += (1-dx)*(1-dy); 324 | image.at(y,x+1) += (dx)*(1-dy); 325 | image.at(y+1,x) += (1-dx)*(dy); 326 | image.at(y+1,x+1) += (dx)*(dy); 327 | } 328 | 329 | break; 330 | default: 331 | cout << "default choice " << endl; 332 | break; 333 | } 334 | 335 | // cout << " success get image " << endl; 336 | return image; 337 | } 338 | 339 | 340 | 341 | /** 342 | * \brief get rotatino from last (sharp) bundle to first bundle rotation. 343 | * \param idx_t1 front idx 344 | */ 345 | Eigen::Matrix3d System::get_global_rotation_b2f(size_t idx_t1, size_t idx_t2) 346 | { 347 | // from frist bundle to world coord 348 | Eigen::Matrix3d R1 = vec_gt_poseData[idx_t1].quat.toRotationMatrix(); 349 | Eigen::Matrix3d R2 = vec_gt_poseData[idx_t2].quat.toRotationMatrix(); 350 | 351 | return R1.transpose()*R2; 352 | } 353 | 354 | 355 | /** 356 | * \brief get_local_rotation_b2f using current eventbundle, return the rotation matrix from t2(end) to t1(start). 357 | * \param reverse from t1->t2. as intuision. 358 | */ 359 | Eigen::Matrix3d System::get_local_rotation_b2f(bool inverse) 360 | { 361 | int target1_pos, target2_pos; 362 | size_t start_pos = vec_gt_poseData.size() > 50 ? vec_gt_poseData.size()-50 : 0; 363 | 364 | double interval_1 = 1e5, interval_2 = 1e5; 365 | 366 | for(size_t i = start_pos; i< vec_gt_poseData.size(); i++) 367 | { 368 | // cout << "ros time " << std::to_string(vec_gt_poseData[i].time_stamp_ros.toSec()) << endl; 369 | if(abs((vec_gt_poseData[i].time_stamp_ros - eventBundle.first_tstamp).toSec()) < interval_1) 370 | { 371 | target1_pos = i; 372 | interval_1 = abs((vec_gt_poseData[i].time_stamp_ros - eventBundle.first_tstamp).toSec()); 373 | } 374 | 375 | if(abs((vec_gt_poseData[i].time_stamp_ros - eventBundle.last_tstamp).toSec()) < interval_2) 376 | { 377 | target2_pos = i; 378 | interval_2 = abs((vec_gt_poseData[i].time_stamp_ros - eventBundle.last_tstamp).toSec()); 379 | } 380 | } 381 | 382 | // TODO NSECONDS, and check the event timestamp and pose time stamp match. 383 | // cout << "event first time " << std::to_string(eventBundle.first_tstamp.toSec()) << 384 | // ", pose time: "<< std::to_string(vec_gt_poseData[target1_pos].time_stamp_ros.toSec())<t2 392 | if(inverse) 393 | return R2.transpose()*R1; 394 | 395 | // from t2->t1 396 | return R1.transpose()*R2; 397 | } 398 | 399 | 400 | /** 401 | * \brief run in back ground, avoid to affect the ros call back function. 402 | */ 403 | void System::Run() 404 | { 405 | 406 | /* update eventBundle */ 407 | eventBundle.Append(vec_vec_eventData[vec_vec_eventData_iter], vec_vec_eventDepth[vec_vec_eventData_iter]); 408 | vec_vec_eventData_iter++; 409 | 410 | // check current eventsize or event interval 411 | double time_interval = (eventBundle.last_tstamp-eventBundle.first_tstamp).toSec(); 412 | if(time_interval < 0.002 || eventBundle.size < 3000) 413 | { 414 | cout << "no enough interval or num: " <t1), add minus to convert it to t1->t2 . 484 | */ 485 | void System::save_velocity() 486 | { 487 | // for velocity 488 | double delta_time = (eventBundle.last_tstamp - eventBundle.first_tstamp).toSec(); 489 | 490 | // minus means from t1->t2. 491 | // double angle = (est_angleAxis * delta_time).norm(); 492 | // Eigen::AngleAxisd ag_pos = Eigen::AngleAxisd(angle, (est_angleAxis * delta_time) / angle); 493 | // Eigen::Quaterniond q = Eigen::Quaterniond(ag_pos); 494 | // Eigen::Vector3d euler_position = toEulerAngles(q) / delta_time; // back to velocity 495 | 496 | // WARNING, you should use ros timestamps not double (cout for double is 6 valid numbers) 497 | // est_velocity_file << seq_count++ <<" " << eventBundle.first_tstamp << " " << eventBundle.last_tstamp << " " << euler_position.transpose() << endl; 498 | 499 | est_velocity_file << seq_count++ <<" " << eventBundle.first_tstamp << " " << 500 | eventBundle.last_tstamp << " " << est_angleAxis.transpose() << " " << 501 | est_trans_velocity.transpose() << endl; 502 | 503 | } 504 | 505 | /** 506 | * \brief input evene vector from ros msg, according to time interval. 507 | */ 508 | void System::pushEventData(const std::vector& ros_vec_event) 509 | { 510 | // que_vec_eventData.push(ros_vec_event); 511 | vec_vec_eventData.push_back(ros_vec_event); 512 | // cout << " to vec_vec_eventData " << endl; 513 | 514 | Run(); 515 | } 516 | 517 | void System::pushEventData(const std::vector& ros_vec_event, const std::vector& vec_depth) 518 | { 519 | // que_vec_eventData.push(ros_vec_event); 520 | vec_vec_eventData.push_back(ros_vec_event); 521 | // cout << " to vec_vec_eventData " << endl; 522 | vec_vec_eventDepth.push_back(vec_depth); 523 | Run(); 524 | } 525 | 526 | void System::setBeginTime(ros::Time begin) 527 | { 528 | beginTS = begin; 529 | } 530 | 531 | 532 | /** 533 | * \brief input evene vector from ros msg. 534 | * \param[in] ImageData self defined imagedata. 535 | */ 536 | void System::pushimageData(const ImageData& imageData) 537 | { 538 | 539 | // can be save in vector 540 | 541 | // update current image 542 | curr_imageData = imageData; // copy construct 543 | curr_raw_image = imageData.image.clone(); 544 | 545 | // undistort image 546 | cv::remap(curr_raw_image, curr_undis_image, undist_mesh_x, undist_mesh_y, cv::INTER_LINEAR ); 547 | } 548 | 549 | 550 | 551 | /** useless 552 | * \brief average 6 pose data from euler anglers to compute angular velocity. 553 | */ 554 | void System::pushPoseData(const PoseData& poseData) 555 | { 556 | 557 | // Eigen::Vector3d v_2 = poseData.quat.toRotationMatrix().eulerAngles(2,1,0); 558 | Eigen::Vector3d curr_pos = toEulerAngles(poseData.quat); 559 | 560 | int loop = 6; 561 | if(vec_gt_poseData.size() > 12) 562 | { 563 | // [* * * target * * *] to get target velocity. 564 | vector::iterator it = vec_gt_poseData.end() - loop - 1; 565 | 566 | Eigen::Vector3d velocity(0,0,0); 567 | 568 | for(int k = 1; k<=loop; ++k) 569 | { 570 | // FIXME rpy: zyx, so v_1=(theta_z,y,x) 571 | // Eigen::Vector3d v_1 = (*it).quat.toRotationMatrix().eulerAngles(2,1,0); 572 | // Eigen::Vector3d v_2 = (*(it-loop)).quat.toRotationMatrix().eulerAngles(2,1,0); 573 | 574 | Eigen::Vector3d v_1 = toEulerAngles((*(it-loop-1+k)).quat); 575 | Eigen::Vector3d v_2 = toEulerAngles((*(it+k)).quat); 576 | 577 | double delta_time = (*(it+k)).time_stamp - (*(it-loop-1+k)).time_stamp ; 578 | 579 | Eigen::Vector3d delta_theta = v_2 - v_1; 580 | velocity += delta_theta / delta_time; 581 | 582 | // cout<< "loop " << k << " delta_t: " < 5 | 6 | using namespace std; 7 | 8 | 9 | /** 10 | * \brief given angular veloity(t1->t2), warp local event bundle become shaper 11 | */ 12 | cv::Mat System::getWarpedEventImage(const Eigen::Vector3d & cur_ang_vel, const Eigen::Vector3d& cur_trans_vel, EventBundle& event_out, const PlotOption& option, bool ref_t1) 13 | { 14 | // cout << "get warpped event image " << endl; 15 | // cout << "eventUndistorted.coord.cols() " << event_undis_Bundle.coord.cols() << endl; 16 | /* warp local events become sharper */ 17 | event_out.CopySize(event_undis_Bundle); 18 | getWarpedEventPoints(event_undis_Bundle, event_out, cur_ang_vel,cur_trans_vel, -1, ref_t1); 19 | event_out.Projection(camera.eg_cameraMatrix); 20 | event_out.DiscriminateInner(camera.width, camera.height); 21 | // getImageFromBundle(event_out, option, false).convertTo(curr_warpped_event_image, CV_32F); 22 | 23 | return getImageFromBundle(event_out, option, false); 24 | 25 | // testing 26 | 27 | // getWarpedEventPoints(event_undis_Bundle, event_out, Eigen::Vector3d(0,10,0)); 28 | // event_out.Projection(camera.eg_cameraMatrix); 29 | // event_out.DiscriminateInner(camera.width, camera.height); 30 | // getImageFromBundle(event_out, option, false).convertTo(y_img, CV_32F); 31 | // getWarpedEventPoints(event_undis_Bundle, event_out, Eigen::Vector3d(0,5,0)); 32 | // event_out.Projection(camera.eg_cameraMatrix); 33 | // event_out.DiscriminateInner(camera.width, camera.height); 34 | // getImageFromBundle(event_out, option, false).convertTo(y5_img, CV_32F); 35 | 36 | // cv::imshow("x ", x_img); 37 | // cv::imshow("y ", y_img); 38 | // cv::imshow("z ", z_img); 39 | // cv::imshow("x 5", x5_img); 40 | // cv::imshow("y 5", y5_img); 41 | // cv::imshow("z 5", z5_img); 42 | // cv::waitKey(0); 43 | 44 | // cout << " success get warpped event image " << endl; 45 | } 46 | 47 | /** 48 | * \brief given angular veloity, warp local event bundle(t2) to the reference time(t1) 49 | using kim RAL21, eqation(11), since the ratation angle is ratively small 50 | * \param cur_ang_vel angleAxis/delta_t from t1->t2, if multiply minus time, it becomes t2->t1. (AngleAxis inverse only add minus) 51 | * \param cur_ang_pos from t2->t0. default set (0,0,0) default, so the output is not rotated. 52 | * \param delta_time all events warp this time, if delta_time<0, warp to t1. 53 | */ 54 | void System::getWarpedEventPoints(const EventBundle& eventIn, EventBundle& eventOut, 55 | const Eigen::Vector3d& cur_ang_vel, const Eigen::Vector3d& cur_trans_vel, double delta_time, bool ref_t1 ) 56 | { 57 | // cout << "projecting " << endl; 58 | // the theta of rotation axis 59 | float ang_vel_norm = cur_ang_vel.norm(); 60 | 61 | Eigen::Matrix3Xd ang_vel_hat_mul_x, ang_vel_hat_sqr_mul_x; 62 | 63 | 64 | /** equation 11 of kim */ 65 | // ang_vel_hat_mul_x.resize(3,eventIn.size); // row, col 66 | // ang_vel_hat_sqr_mul_x.resize(3,eventIn.size); 67 | // ang_vel_hat_mul_x.row(0) = -cur_ang_vel(2)*eventIn.coord_3d.row(1) + cur_ang_vel(1)*eventIn.coord_3d.row(2); 68 | // ang_vel_hat_mul_x.row(1) = cur_ang_vel(2)*eventIn.coord_3d.row(0) - cur_ang_vel(0)*eventIn.coord_3d.row(2); 69 | // ang_vel_hat_mul_x.row(2) = -cur_ang_vel(1)*eventIn.coord_3d.row(0) + cur_ang_vel(0)*eventIn.coord_3d.row(1); 70 | 71 | 72 | // ang_vel_hat_sqr_mul_x.row(0) = -cur_ang_vel(2)*ang_vel_hat_mul_x.row(1) + cur_ang_vel(1)*ang_vel_hat_mul_x.row(2); 73 | // ang_vel_hat_sqr_mul_x.row(1) = cur_ang_vel(2)*ang_vel_hat_mul_x.row(0) - cur_ang_vel(0)*ang_vel_hat_mul_x.row(2); 74 | // ang_vel_hat_sqr_mul_x.row(2) = -cur_ang_vel(1)*ang_vel_hat_mul_x.row(0) + cur_ang_vel(0)*ang_vel_hat_mul_x.row(1); 75 | 76 | 77 | eventOut.CopySize(eventIn); 78 | if(false && ang_vel_norm < 1e-8) // TODO always computes 79 | { 80 | // cout << " small angle vec " << ang_vel_norm/3.14 * 180 << " degree /s" << endl; 81 | eventOut.coord_3d = eventIn.coord_3d ; 82 | } 83 | else 84 | { 85 | Eigen::VectorXd vec_delta_time = eventBundle.time_delta; 86 | if(ref_t1) vec_delta_time = eventBundle.time_delta.array() - eventBundle.time_delta(eventBundle.size-1); 87 | 88 | // kim second order version; 89 | { 90 | // eventOut.coord_3d = eventIn.coord_3d 91 | // + Eigen::MatrixXd( 92 | // ang_vel_hat_mul_x.array().rowwise() 93 | // * (-vec_delta_time.transpose().array())); 94 | // + ang_vel_hat_sqr_mul_x.array().rowwise() 95 | // * (0.5f * vec_delta_time.transpose().array().square()) ); 96 | // cout << "usingg est" << cur_ang_vel.transpose() << endl; 97 | // cout << "original " << eventIn.coord_3d.topLeftCorner(3,5)<< endl; 98 | // cout << "ang_vel_hat_mul_x " << ang_vel_hat_mul_x.topLeftCorner(3,5)<< endl; 99 | // cout << "delta time " << vec_delta_time.topRows(5).transpose() << endl; 100 | // cout << "final \n" << eventOut.coord_3d.topLeftCorner(3,5) << endl; 101 | } 102 | 103 | // exactly 104 | if(ref_t1 == false) // warp to t1 105 | { 106 | // TODO: projection, for unit rotation and zero translation remain the same points 107 | Eigen::Matrix axis = cur_ang_vel.normalized(); 108 | Eigen::Matrix skew_m; 109 | skew_m << 0.0, -axis(2), axis(1), axis(2), 0.0, -axis(0), -axis(1), axis(0), 0.0; 110 | 111 | Eigen::Matrix rotation; 112 | for(int i=0; i::Identity() + sin(theta)*skew_m + (1.0-cos(theta))*skew_m*skew_m; 116 | // eventOut.coord_3d.col(i) = rotation.transpose() * (eventIn.coord_3d.col(i).array() - (cur_trans_vel*vec_delta_time(i)).array()).matrix(); 117 | eventOut.coord_3d.col(i) = rotation * eventIn.coord_3d.col(i) + cur_trans_vel*vec_delta_time(i); 118 | } 119 | } 120 | else // warp to t2, p2 = R12^T * p1 - R12^T * Trans12 121 | { 122 | Eigen::Matrix axis = cur_ang_vel.normalized(); 123 | Eigen::Matrix skew_m; 124 | skew_m << 0.0, -axis(2), axis(1), axis(2), 0.0, -axis(0), -axis(1), axis(0), 0.0; 125 | 126 | Eigen::Matrix rotation; 127 | for(int i=0; i::Identity() + sin(theta)*skew_m + (1.0-cos(theta))*skew_m*skew_m; 131 | // eventOut.coord_3d.col(i) = rotation.transpose() * (eventIn.coord_3d.col(i).array() - (cur_trans_vel*vec_delta_time(i)).array()).matrix(); 132 | eventOut.coord_3d.col(i) = rotation * eventIn.coord_3d.col(i) + rotation * cur_trans_vel*vec_delta_time(i); 133 | } 134 | } 135 | 136 | // cout << "last \n " << eventOut.coord_3d.bottomRightCorner(3,5) << endl; 137 | 138 | } 139 | 140 | // cout << "sucess getWarpedEventPoints" << endl; 141 | } 142 | 143 | 144 | /** 145 | * \brief given start and end ratio (0~1), and samples_count, return noise free sampled events. 146 | * \param vec_sampled_idx 147 | * \param samples_count 148 | */ 149 | void System::getSampledVec(vector& vec_sampled_idx, int samples_count, double sample_start, double sample_end) 150 | { 151 | cv::RNG rng(int(ros::Time::now().nsec)); 152 | // get samples, filtered out noise 153 | for(int i=0; i< samples_count;) 154 | { 155 | int sample_idx = rng.uniform(int(event_undis_Bundle.size*sample_start), int(event_undis_Bundle.size*sample_end)); 156 | 157 | // check valid 8 neighbor hood existed 158 | int sampled_x = event_undis_Bundle.coord.col(sample_idx)[0]; 159 | int sampled_y = event_undis_Bundle.coord.col(sample_idx)[1]; 160 | if(sampled_x >= camera.width-2 || sampled_x < 1 || sampled_y >= camera.height-2 || sampled_y < 1 ) 161 | { 162 | // cout << "x, y" << sampled_x << "," << sampled_y << endl; 163 | continue; 164 | } 165 | 166 | int count = 0; 167 | for(int j=-1; j<2; j++) 168 | for(int k=-1; k<2; k++) 169 | { 170 | count += ( curr_undis_event_image.at(sampled_y+j,sampled_x+k)[0] + 171 | curr_undis_event_image.at(sampled_y+j,sampled_x+k)[1] + 172 | curr_undis_event_image.at(sampled_y+j,sampled_x+k)[2] ) > 0; 173 | // count += ( curr_undis_event_image.at(sampled_y+j,sampled_x+k) != default_value) 174 | } 175 | 176 | // valid denoised 177 | if(count > yaml_denoise_num) // TODO 178 | { 179 | vec_sampled_idx.push_back(sample_idx); 180 | i++; 181 | } 182 | } 183 | 184 | } 185 | 186 | 187 | 188 | -------------------------------------------------------------------------------- /src/dof6_estimator/src/system_estimate_ransac_doublewarp_ceres.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "system.hpp" 3 | #include "numerics.hpp" 4 | #include 5 | #include 6 | #include 7 | using namespace std; 8 | 9 | 10 | 11 | /** 12 | * \brief used for ceres to implement CM methods, automatie version. . 13 | */ 14 | struct ResidualCostFunction 15 | { 16 | 17 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 18 | 19 | ResidualCostFunction( 20 | const Eigen::Vector3d& points, 21 | const double delta_time_early, 22 | const double delta_time_later, 23 | const Eigen::Matrix3d& K, 24 | ceres::BiCubicInterpolator>* interpolator_early_ptr, 25 | ceres::BiCubicInterpolator>* interpolator_later_ptr, 26 | Eigen::Matrix& last_est_var): 27 | points_(points), delta_time_early_(delta_time_early), delta_time_later_(delta_time_later), 28 | intrisic_(K), interpolator_early_ptr_(interpolator_early_ptr), interpolator_later_ptr_(interpolator_later_ptr), 29 | last_est_var_(last_est_var) 30 | { 31 | 32 | } 33 | 34 | // operator 35 | template 36 | bool operator()(const T* const ag_v_ang, T* residual) const // ag_v_ang 8 vars of homography 37 | { 38 | // 6dof: r + v 39 | Eigen::Matrix points_T; 40 | points_T(0) = T(points_(0)); 41 | points_T(1) = T(points_(1)); 42 | points_T(2) = T(points_(2)); 43 | 44 | Eigen::Matrix delta_points_T, delta_second_points_T; 45 | Eigen::Matrix points_early_T, points_later_T; 46 | Eigen::Matrix points_2D_early_T, points_2D_later_T; 47 | 48 | // rotation part 49 | // { 50 | Eigen::Matrix ag_early = {ag_v_ang[0], ag_v_ang[1], ag_v_ang[2]}; 51 | ag_early = ag_early * T(delta_time_early_); 52 | Eigen::Matrix ag_later = {ag_v_ang[0], ag_v_ang[1], ag_v_ang[2]}; 53 | ag_later = ag_later * T(delta_time_later_); // negative time 54 | 55 | Eigen::Matrix Rotation_early; 56 | Eigen::Matrix Rotation_later; 57 | ceres::AngleAxisToRotationMatrix(&ag_early(0), &Rotation_early(0)); 58 | ceres::AngleAxisToRotationMatrix(&ag_later(0), &Rotation_later(0)); 59 | // } 60 | 61 | Eigen::Matrix vel; // translation velocity 62 | vel(0) = ag_v_ang[3]; 63 | vel(1) = ag_v_ang[4]; 64 | vel(2) = ag_v_ang[5]; 65 | 66 | // translation part new using matrix 67 | { 68 | points_early_T = Rotation_early * points_T + T(delta_time_early_) * vel; 69 | points_later_T = Rotation_later * points_T + T(delta_time_later_) * Rotation_later * vel; 70 | } 71 | 72 | points_2D_early_T(0) = points_early_T(0)/points_early_T(2)*T(intrisic_(0,0)) + T(intrisic_(0,2)); 73 | points_2D_early_T(1) = points_early_T(1)/points_early_T(2)*T(intrisic_(1,1)) + T(intrisic_(1,2)); 74 | 75 | points_2D_later_T(0) = points_later_T(0)/points_later_T(2)*T(intrisic_(0,0)) + T(intrisic_(0,2)); 76 | points_2D_later_T(1) = points_later_T(1)/points_later_T(2)*T(intrisic_(1,1)) + T(intrisic_(1,2)); 77 | 78 | // cout << "points "<< points(0) << ", "<< points(1) << endl; 79 | 80 | /* ceres interpolate version */ 81 | { 82 | // TODO double warp 83 | T early_loss = T(0), later_loss = T(0); 84 | interpolator_early_ptr_->Evaluate(points_2D_early_T(1), points_2D_early_T(0), &early_loss); 85 | interpolator_later_ptr_->Evaluate(points_2D_later_T(1), points_2D_later_T(0), &later_loss); 86 | 87 | // cout << "intered " << early_loss << ", later " << later_loss << endl; 88 | // residual[0] = early_loss; 89 | T norm_residual = T(0); 90 | for(int i=0;i<6;i++) 91 | { 92 | norm_residual += ceres::pow(T(last_est_var_(i,0)) - ag_v_ang[i], 2); 93 | } 94 | residual[0] = early_loss + later_loss + T(0.01) * norm_residual; 95 | } 96 | 97 | return true; 98 | } 99 | 100 | // make ceres costfunction 101 | static ceres::CostFunction* Create( 102 | const Eigen::Vector3d& points, 103 | const double delta_time_early, 104 | const double delta_time_later, 105 | const Eigen::Matrix3d& K, 106 | ceres::BiCubicInterpolator>* interpolator_early_ptr, 107 | ceres::BiCubicInterpolator>* interpolator_later_ptr, 108 | Eigen::Matrix& last_est_var) 109 | { 110 | return new ceres::AutoDiffCostFunction( 111 | new ResidualCostFunction(points, delta_time_early, delta_time_later, K, 112 | interpolator_early_ptr, interpolator_later_ptr, last_est_var)); 113 | } 114 | 115 | // inputs 116 | Eigen::Vector3d points_; 117 | double delta_time_early_, delta_time_later_; 118 | Eigen::Matrix3d intrisic_; 119 | 120 | ceres::BiCubicInterpolator> *interpolator_early_ptr_; 121 | ceres::BiCubicInterpolator> *interpolator_later_ptr_; 122 | Eigen::Matrix last_est_var_; 123 | // Eigen::Matrix3Xd ang_vel_hat_mul_x, ang_vel_hat_sqr_mul_x; 124 | }; 125 | 126 | 127 | 128 | 129 | /** 130 | * \brief using time as distance, self boosting, using ceres as optimizer, const gradient for each optimizing. 131 | \param [ts_start, ts_end]: time_range to form timesurface template 132 | \param sample_num: the sample count beyond the time_range 133 | */ 134 | void System::EstimateMotion_ransca_ceres(double ts_start, double ts_end, int sample_num, int total_iter_num) 135 | { 136 | cout <=0; i--) 196 | { 197 | 198 | int sampled_x = std::round(event_warpped_Bundle.coord.col(i)[0]), sampled_y = std::round(event_warpped_Bundle.coord.col(i)[1]); 199 | 200 | if(event_warpped_Bundle.isInner[i] < 1) continue; // outlier 201 | // linear add TODO improve to module 202 | cv_earlier_timesurface.at(sampled_y, sampled_x) = eventBundle.time_delta(i); 203 | } 204 | t2 = ros::Time::now(); 205 | if(show_time_info) 206 | cout << "cv_earlier_timesurface time " << (t2-t1).toSec() * 2 << endl; // 0.000106088 207 | 208 | // get t1 time surface of warpped image 209 | getWarpedEventImage(eg_angleAxis, eg_trans_vel, event_warpped_Bundle, PlotOption::U16C1_EVNET_IMAGE, true); 210 | // get later timesurface 211 | for(int i= event_warpped_Bundle.size*(1-timesurface_range); i< event_warpped_Bundle.size; i++) 212 | { 213 | 214 | int sampled_x = std::round(event_warpped_Bundle.coord.col(i)[0]), sampled_y = std::round(event_warpped_Bundle.coord.col(i)[1]); 215 | 216 | if(event_warpped_Bundle.isInner[i] < 1) continue; // outlier 217 | // linear add TODO improve to module 218 | cv_later_timesurface.at(sampled_y, sampled_x) = eventBundle.time_delta(event_warpped_Bundle.size-1) - eventBundle.time_delta(i); 219 | } 220 | 221 | // /* visualize timesurface */ 222 | // { 223 | 224 | // cv::Mat cv_earlier_timesurface_8U, cv_earlier_timesurface_color; 225 | // cv::normalize(cv_earlier_timesurface, cv_earlier_timesurface_8U, 255, 0, cv::NORM_MINMAX , CV_8UC1 ); 226 | // // cv_earlier_timesurface.convertTo(cv_earlier_timesurface_8U, CV_8UC1); 227 | // cv::applyColorMap(cv_earlier_timesurface_8U, cv_earlier_timesurface_color, cv::COLORMAP_JET); 228 | // cv::imshow("timesurface_early", cv_earlier_timesurface_color); 229 | // cv::waitKey(10); 230 | // } 231 | // { 232 | // // visualize timesurface 233 | // cv::Mat cv_later_timesurface_8U, cv_later_timesurface_color; 234 | // cv::normalize(cv_later_timesurface, cv_later_timesurface_8U, 255, 0, cv::NORM_MINMAX , CV_8UC1 ); 235 | // // cv_earlier_timesurface.convertTo(cv_earlier_timesurface_8U, CV_8UC1); 236 | // cv::applyColorMap(cv_later_timesurface_8U, cv_later_timesurface_color, cv::COLORMAP_JET); 237 | // cv::imshow("timesurface_later", cv_later_timesurface_color); 238 | // cv::waitKey(10); 239 | // } 240 | 241 | // add gaussian on cv_earlier_timesurface 242 | cv::Mat cv_earlier_timesurface_blur, cv_later_timesurface_blur; 243 | int gaussian_size = yaml_gaussian_size; 244 | float sigma = yaml_gaussian_size_sigma; 245 | cv::GaussianBlur(cv_earlier_timesurface, cv_earlier_timesurface_blur, cv::Size(gaussian_size, gaussian_size), sigma); 246 | cv::GaussianBlur(cv_later_timesurface, cv_later_timesurface_blur, cv::Size(gaussian_size, gaussian_size), sigma); 247 | 248 | // get timesurface in ceres 249 | t1 = ros::Time::now(); 250 | // vector line_grid_early; line_grid_early.assign((float*)cv_earlier_timesurface.data, (float*)cv_earlier_timesurface.data + 180*240); 251 | // vector line_grid_later; line_grid_later.assign((float*)cv_later_timesurface.data, (float*)cv_later_timesurface.data + 180*240); 252 | vector line_grid_early; line_grid_early.assign((float*)cv_earlier_timesurface_blur.data, (float*)cv_earlier_timesurface_blur.data + camera.width*camera.height); 253 | vector line_grid_later; line_grid_later.assign((float*)cv_later_timesurface_blur.data, (float*)cv_later_timesurface_blur.data + camera.width*camera.height); 254 | t2 = ros::Time::now(); 255 | if(show_time_info) 256 | cout << "convert inter time " << (t2-t1).toSec() << endl; // 0.000256303, using pointer reduce to 2.6943e-05 257 | 258 | ceres::Grid2D grid_early(line_grid_early.data(), 0, camera.height, 0, camera.width); 259 | ceres::Grid2D grid_later(line_grid_later.data(), 0, camera.height, 0, camera.width); 260 | auto* interpolator_early_ptr = new ceres::BiCubicInterpolator>(grid_early); 261 | auto* interpolator_later_ptr = new ceres::BiCubicInterpolator>(grid_later); 262 | 263 | // sample events 264 | // select 100 random points, and warp delta_t < min(t_point_delta_t). 265 | // accumulate all time difference before and after warpped points. 266 | t1 = ros::Time::now(); 267 | std::vector vec_sampled_idx; 268 | int samples_count = std::min(sample_num, int(eventBundle.size)); 269 | getSampledVec(vec_sampled_idx, samples_count, 0, 1); 270 | t2 = ros::Time::now(); 271 | if(show_time_info) 272 | cout << "getSampledVec time " << (t2-t1).toSec() << endl; // 0.000473817 273 | 274 | t1 = ros::Time::now(); 275 | // init problem 276 | ceres::Problem problem; 277 | // add residual 278 | for(int loop_temp =0; loop_temp < vec_sampled_idx.size(); loop_temp++) 279 | { 280 | size_t sample_idx = vec_sampled_idx[loop_temp]; 281 | double early_time = eventBundle.time_delta(sample_idx); 282 | double later_time = eventBundle.time_delta(sample_idx) - eventBundle.time_delta(event_warpped_Bundle.size-1); 283 | 284 | ceres::CostFunction* cost_function = ResidualCostFunction::Create( 285 | event_undis_Bundle.coord_3d.col(sample_idx), 286 | early_time, later_time, 287 | camera.eg_cameraMatrix, 288 | interpolator_early_ptr, interpolator_later_ptr, 289 | last_est_var); 290 | 291 | problem.AddResidualBlock(cost_function, nullptr, &ag_v_ang[0]); 292 | } 293 | t2 = ros::Time::now(); 294 | if(show_time_info) 295 | cout << "add residual time " << (t2-t1).toSec() << endl; // 0.00168042 296 | 297 | ceres::Solver::Options options; 298 | options.minimizer_progress_to_stdout = false; 299 | options.num_threads = yaml_ceres_iter_thread; 300 | // options.logging_type = ceres::SILENT; 301 | options.linear_solver_type = ceres::SPARSE_SCHUR; 302 | options.use_nonmonotonic_steps = true; 303 | options.max_num_iterations = yaml_ceres_iter_num; 304 | // options.initial_trust_region_radius = 1; 305 | problem.SetParameterLowerBound(&ag_v_ang[0],0,-0.2); 306 | problem.SetParameterUpperBound(&ag_v_ang[0],0, 0.2); 307 | problem.SetParameterLowerBound(&ag_v_ang[0],1,-0.6); 308 | problem.SetParameterUpperBound(&ag_v_ang[0],1, 0.6); 309 | problem.SetParameterLowerBound(&ag_v_ang[0],2,-0.2); 310 | problem.SetParameterUpperBound(&ag_v_ang[0],2, 0.2); 311 | 312 | problem.SetParameterLowerBound(&ag_v_ang[0],3,-2); 313 | problem.SetParameterUpperBound(&ag_v_ang[0],3, 2); 314 | problem.SetParameterLowerBound(&ag_v_ang[0],4,-2); 315 | problem.SetParameterUpperBound(&ag_v_ang[0],4, 2); 316 | problem.SetParameterLowerBound(&ag_v_ang[0],5,-1); // outdoor(0-8) 317 | problem.SetParameterUpperBound(&ag_v_ang[0],5, 8); // outdoor(0-8) 318 | 319 | ceres::Solver::Summary summary; 320 | 321 | // evaluate: choose init velocity, test whether using last_est or {0,0,0}, 322 | if(false && iter_ == 1) 323 | { 324 | double cost = 0; 325 | vector residual_vec; 326 | // previous old velocity 327 | ag_v_ang[0] = est_angleAxis(0); 328 | ag_v_ang[1] = est_angleAxis(1); 329 | ag_v_ang[2] = est_angleAxis(2); 330 | ag_v_ang[3] = est_trans_velocity(0); 331 | ag_v_ang[4] = est_trans_velocity(1); 332 | ag_v_ang[5] = est_trans_velocity(2); 333 | 334 | problem.Evaluate(ceres::Problem::EvaluateOptions(), &cost, &residual_vec, nullptr, nullptr); 335 | double residual_sum_old = std::accumulate(residual_vec.begin(), residual_vec.end(), 0.0); 336 | // 0 init 337 | memset(ag_v_ang, 0.1, sizeof(double)*6); // skip the norm 338 | 339 | problem.Evaluate(ceres::Problem::EvaluateOptions(), &cost, &residual_vec, nullptr, nullptr); 340 | double residual_sum_0 = std::accumulate(residual_vec.begin(), residual_vec.end(), 0.0); 341 | 342 | if(residual_sum_old < residual_sum_0) 343 | { 344 | ag_v_ang[0] = est_angleAxis(0); 345 | ag_v_ang[1] = est_angleAxis(1); 346 | ag_v_ang[2] = est_angleAxis(2); 347 | ag_v_ang[3] = est_trans_velocity(0); 348 | ag_v_ang[4] = est_trans_velocity(1); 349 | ag_v_ang[5] = est_trans_velocity(2); 350 | } 351 | 352 | // cout << "using ang" << ag_v_ang[0] << "," << ag_v_ang[1] << "," << ag_v_ang[2] << 353 | // " trans " << ag_v_ang[3] << "," << ag_v_ang[4] << "," << ag_v_ang[5] << 354 | // ", residual size " << residual_vec.size() << ", sum_0: "<< residual_sum_0 << ", sum_old: " <239 || y>179 || x<0 ||y<0) continue; 397 | // hot_image_C3.at(y,x) = cv::Vec3b(0,255,0); // green of events before warp 398 | // } 399 | 400 | // // compare with gt 401 | // // cout << "estimated angleAxis " << est_angleAxis.transpose() << endl; 402 | 403 | // // plot earlier timestamps oringe 22，G：07，B：201 404 | // for(int i=0; i239 || y>179 || x<0 ||y<0) continue; 412 | // hot_image_C3.at(y,x) = cv::Vec3b(0, 165, 255); // bottom events earlier 413 | // } 414 | 415 | // // visualize est 416 | // int sample_red = 1; 417 | // for(int i=0; i239 || y>179 || x<0 ||y<0) continue; 424 | // hot_image_C3.at(y,x) = cv::Vec3b(0,0,255); // red after warp 425 | // // cout << "all inlier red" << endl; 426 | // } 427 | // } 428 | 429 | } 430 | 431 | 432 | // output data 433 | est_angleAxis = Eigen::Vector3d(ag_v_ang[0],ag_v_ang[1],ag_v_ang[2]); 434 | est_trans_velocity = Eigen::Vector3d(ag_v_ang[3],ag_v_ang[4],ag_v_ang[5]); 435 | 436 | cout << " Loss: " << 0 << ", est_angleAxis " << est_angleAxis.transpose() << 437 | ", trans " << est_trans_velocity.transpose() << endl; 438 | 439 | last_est_var << est_angleAxis, est_trans_velocity; 440 | } 441 | 442 | -------------------------------------------------------------------------------- /src/event_publisher/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(event_publisher) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | cv_bridge 12 | image_transport 13 | roscpp 14 | sensor_msgs 15 | std_msgs 16 | dvs_msgs 17 | ) 18 | 19 | find_package(OpenCV 4 REQUIRED) 20 | find_package(Eigen3 REQUIRED) 21 | 22 | ## System dependencies are found with CMake's conventions 23 | # find_package(Boost REQUIRED COMPONENTS system) 24 | 25 | 26 | ## Uncomment this if the package has a setup.py. This macro ensures 27 | ## modules and global scripts declared therein get installed 28 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 29 | # catkin_python_setup() 30 | 31 | ################################################ 32 | ## Declare ROS messages, services and actions ## 33 | ################################################ 34 | 35 | ## To declare and build messages, services or actions from within this 36 | ## package, follow these steps: 37 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 38 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 39 | ## * In the file package.xml: 40 | ## * add a build_depend tag for "message_generation" 41 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 42 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 43 | ## but can be declared for certainty nonetheless: 44 | ## * add a exec_depend tag for "message_runtime" 45 | ## * In this file (CMakeLists.txt): 46 | ## * add "message_generation" and every package in MSG_DEP_SET to 47 | ## find_package(catkin REQUIRED COMPONENTS ...) 48 | ## * add "message_runtime" and every package in MSG_DEP_SET to 49 | ## catkin_package(CATKIN_DEPENDS ...) 50 | ## * uncomment the add_*_files sections below as needed 51 | ## and list every .msg/.srv/.action file to be processed 52 | ## * uncomment the generate_messages entry below 53 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 54 | 55 | ## Generate messages in the 'msg' folder 56 | # add_message_files( 57 | # FILES 58 | # Message1.msg 59 | # Message2.msg 60 | # ) 61 | 62 | ## Generate services in the 'srv' folder 63 | # add_service_files( 64 | # FILES 65 | # Service1.srv 66 | # Service2.srv 67 | # ) 68 | 69 | ## Generate actions in the 'action' folder 70 | # add_action_files( 71 | # FILES 72 | # Action1.action 73 | # Action2.action 74 | # ) 75 | 76 | ## Generate added messages and services with any dependencies listed here 77 | # generate_messages( 78 | # DEPENDENCIES 79 | # std_msgs # Or other packages containing msgs 80 | # ) 81 | 82 | ################################################ 83 | ## Declare ROS dynamic reconfigure parameters ## 84 | ################################################ 85 | 86 | ## To declare and build dynamic reconfigure parameters within this 87 | ## package, follow these steps: 88 | ## * In the file package.xml: 89 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 90 | ## * In this file (CMakeLists.txt): 91 | ## * add "dynamic_reconfigure" to 92 | ## find_package(catkin REQUIRED COMPONENTS ...) 93 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 94 | ## and list every .cfg file to be processed 95 | 96 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 97 | # generate_dynamic_reconfigure_options( 98 | # cfg/DynReconf1.cfg 99 | # cfg/DynReconf2.cfg 100 | # ) 101 | 102 | ################################### 103 | ## catkin specific configuration ## 104 | ################################### 105 | ## The catkin_package macro generates cmake config files for your package 106 | ## Declare things to be passed to dependent projects 107 | ## INCLUDE_DIRS: uncomment this if your package contains header files 108 | ## LIBRARIES: libraries you create in this project that dependent projects also need 109 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 110 | ## DEPENDS: system dependencies of this project that dependent projects also need 111 | catkin_package( 112 | INCLUDE_DIRS include 113 | LIBRARIES event_publisher 114 | # CATKIN_DEPENDS cv_bridge image_transport roscpp sensor_msg std_msg 115 | # DEPENDS 116 | ) 117 | 118 | ########### 119 | ## Build ## 120 | ########### 121 | 122 | ## Specify additional locations of header files 123 | ## Your package locations should be listed before other locations 124 | include_directories( 125 | include 126 | ${catkin_INCLUDE_DIRS} 127 | ${OpenCV_INCLUDE} 128 | ${Eigen3_INCLUDE_DIR} 129 | ) 130 | 131 | ## Declare a C++ library 132 | add_library(${PROJECT_NAME} 133 | src/event_reader.cpp 134 | ) 135 | 136 | ## Add cmake target dependencies of the library 137 | ## as an example, code may need to be generated before libraries 138 | ## either from message generation or dynamic reconfigure 139 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 140 | 141 | ## Declare a C++ executable 142 | ## With catkin_make all packages are built within a single CMake context 143 | ## The recommended prefix ensures that target names across packages don't collide 144 | add_executable(${PROJECT_NAME}_node src/event_publisher_node.cpp src/event_reader.cpp) 145 | 146 | ## Rename C++ executable without prefix 147 | ## The above recommended prefix causes long target names, the following renames the 148 | ## target back to the shorter version for ease of user use 149 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 150 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 151 | 152 | ## Add cmake target dependencies of the executable 153 | ## same as for the library above 154 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 155 | 156 | ## Specify libraries to link a library or executable target against 157 | target_link_libraries(${PROJECT_NAME}_node 158 | ${catkin_LIBRARIES} 159 | ${OpenCV_LIBS} 160 | ${Eigen3_LIBS} 161 | ) 162 | 163 | ############# 164 | ## Install ## 165 | ############# 166 | 167 | # all install targets should use catkin DESTINATION variables 168 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 169 | 170 | ## Mark executable scripts (Python etc.) for installation 171 | ## in contrast to setup.py, you can choose the destination 172 | # catkin_install_python(PROGRAMS 173 | # scripts/my_python_script 174 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 175 | # ) 176 | 177 | ## Mark executables for installation 178 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 179 | # install(TARGETS ${PROJECT_NAME}_node 180 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 181 | # ) 182 | 183 | ## Mark libraries for installation 184 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 185 | # install(TARGETS ${PROJECT_NAME} 186 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 187 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 188 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 189 | # ) 190 | 191 | ## Mark cpp header files for installation 192 | # install(DIRECTORY include/${PROJECT_NAME}/ 193 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 194 | # FILES_MATCHING PATTERN "*.h" 195 | # PATTERN ".svn" EXCLUDE 196 | # ) 197 | 198 | ## Mark other files for installation (e.g. launch and bag files, etc.) 199 | # install(FILES 200 | # # myfile1 201 | # # myfile2 202 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 203 | # ) 204 | 205 | ############# 206 | ## Testing ## 207 | ############# 208 | 209 | ## Add gtest based cpp test target and link libraries 210 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_event_publisher.cpp) 211 | # if(TARGET ${PROJECT_NAME}-test) 212 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 213 | # endif() 214 | 215 | ## Add folders to be run by python nosetests 216 | # catkin_add_nosetests(test) 217 | -------------------------------------------------------------------------------- /src/event_publisher/config/config.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | Version: 1.0 4 | 5 | #-------------------------------------------------------------------------------------------- 6 | # General Parameters (fixed). 7 | #-------------------------------------------------------------------------------------------- 8 | # original image size for camera 9 | width: 240 10 | height: 180 11 | 12 | 13 | # groundtruth 14 | groundtruth_dir: "/home/hxy/Documents/rosbag/Mono-unzip/poster_rotation/events.txt" 15 | # groundtruth_dir: "/home/hxy/Documents/rosbag/Mono-unzip/shapes_rotation/events.txt" 16 | # groundtruth_dir: "/home/hxy/Documents/rosbag/Mono-unzip/boxes_rotation/events.txt" 17 | # groundtruth_dir: "/home/hxy/Documents/rosbag/Mono-unzip/dynamic_rotation/events.txt" 18 | 19 | # groundtruth_dir: "/home/hxy/Desktop/Event-ST-PPP/dataset/dynamic_rotation/events.txt" 20 | 21 | # 2.7e7~3.1e7 for dynamic 22 | # 5.7e6~5.9e6 for shapes 23 | read_start_lines: 0e4 24 | read_max_lines: 3e7 25 | #-------------------------------------------------------------------------------------------- 26 | # Event Parameters (tunable). 27 | #-------------------------------------------------------------------------------------------- 28 | # size of real targets 29 | # Event.delta_time: 0.01 30 | using_fixed_time: 1 # 1 means true 31 | store2txt: 0 # 1 means true 32 | 33 | fixed_interval: 0.02 34 | Event.bundle_size: 3e4 35 | sleep_rate: 0.5 # 10Hz 36 | 37 | -------------------------------------------------------------------------------- /src/event_publisher/config/config_depth.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | Version: 1.0 4 | 5 | #-------------------------------------------------------------------------------------------- 6 | # General Parameters (fixed). 7 | #-------------------------------------------------------------------------------------------- 8 | # original image size for camera 9 | width: 346 10 | height: 260 11 | 12 | #-------------------------------------------------------------------------------------------- 13 | # Event Parameters (tunable). 14 | #-------------------------------------------------------------------------------------------- 15 | # size of real targets 16 | Event.delta_time: 0.1 17 | Event.max_event: 5e4 18 | Event.min_event: 20000 19 | Event.start_line: 0e4 20 | Event.end_line: 25000e4 21 | # Event.end_line: 250e4 -------------------------------------------------------------------------------- /src/event_publisher/include/event_reader.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // std 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | // ros 10 | #include 11 | #include 12 | #include 13 | 14 | 15 | // third party 16 | #include 17 | #include 18 | 19 | 20 | using namespace std; 21 | 22 | 23 | class Event_reader 24 | { 25 | 26 | public: 27 | Event_reader(std::string yaml="", 28 | ros::Publisher* event_array_pub = nullptr, 29 | ros::Publisher* event_image_pub = nullptr); 30 | 31 | bool read(int event_size = 0, double event_interval = 0); 32 | void publish(); 33 | void render(); 34 | double sleep_rate; 35 | 36 | 37 | // for txt 38 | bool acquire(dvs_msgs::EventArrayPtr ptr); 39 | bool acquire(dvs_msgs::EventArrayPtr ptr, std::vector& vec_depth); 40 | 41 | 42 | private: 43 | 44 | ros::Publisher* event_array_pub_; 45 | ros::Publisher* event_image_pub_; 46 | 47 | std::vector eventData; // single event 48 | int eventData_counter; 49 | 50 | int height, width; // event image size 51 | 52 | int event_bundle_size; // maximum size of event vector 53 | 54 | int read_max_lines; 55 | int read_start_lines; 56 | 57 | double delta_time; // 58 | double curr_time; 59 | 60 | int store2txt; 61 | int using_fixed_time; 62 | double fixed_interval; 63 | 64 | size_t count_pos; // start pos of each publish 65 | 66 | dvs_msgs::EventArrayPtr msg_ptr; // for publish 67 | 68 | cv_bridge::CvImage event_image; 69 | 70 | std::ifstream openFile; // read file 71 | int count_liens; 72 | }; 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | -------------------------------------------------------------------------------- /src/event_publisher/launch/event_publish.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 18 | 19 | -------------------------------------------------------------------------------- /src/event_publisher/launch/event_publisher_depth.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 18 | 19 | -------------------------------------------------------------------------------- /src/event_publisher/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | event_publisher 4 | 0.0.0 5 | The event_publisher package 6 | 7 | 8 | 9 | 10 | hxt 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 | roscpp 55 | sensor_msgs 56 | std_msgs 57 | cv_bridge 58 | image_transport 59 | roscpp 60 | sensor_msgs 61 | std_msgs 62 | cv_bridge 63 | image_transport 64 | roscpp 65 | sensor_msgs 66 | std_msgs 67 | 68 | 69 | dvs_msgs 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | -------------------------------------------------------------------------------- /src/event_publisher/readme.md: -------------------------------------------------------------------------------- 1 | not real-time version 2 | 3 | setting it fixed count number or fixed time interval. -------------------------------------------------------------------------------- /src/event_publisher/src/event_publisher_node.cpp: -------------------------------------------------------------------------------- 1 | // std 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | // ros 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | // third party 18 | #include 19 | #include 20 | 21 | // self 22 | #include "event_reader.hpp" 23 | 24 | 25 | using namespace std; 26 | using namespace Eigen; 27 | 28 | 29 | 30 | int main(int argc, char** argv) 31 | { 32 | 33 | cout << " hello event publisher " << endl; 34 | 35 | ros::init(argc, argv, "event_publisher"); 36 | 37 | ros::NodeHandle nh_private("~"); 38 | 39 | string yaml, event_dir; 40 | bool loop = false; // mease loop the events 41 | nh_private.param("yaml", yaml,""); 42 | nh_private.param("loop", loop, false); 43 | 44 | ros::Publisher event_array_pub = nh_private.advertise("/events",10); 45 | ros::Publisher raw_image = nh_private.advertise("/raw_image",10); 46 | ros::Publisher event_image = nh_private.advertise("/event_image",10); 47 | 48 | Event_reader event_reader(yaml, &event_array_pub, &event_image); 49 | 50 | double sleep_rate = event_reader.sleep_rate; 51 | ros::Rate ros_sleep_rate(sleep_rate); // 1s 10 times 52 | while(ros::ok()) 53 | { 54 | // ros::spinOnce(); // 55 | 56 | // cv_bridge::CvImage cv_image; 57 | // cv_image.encoding = "bgr8"; 58 | // cv_image.image = cv::imread("/home/hxt/Pictures/test.jpg",cv::IMREAD_COLOR); 59 | // sensor_msgs::ImagePtr msg = cv_image.toImageMsg(); 60 | // raw_image.publish(msg); 61 | 62 | 63 | // dvs_msgs::EventArrayPtr evmsg = dvs_msgs::EventArrayPtr(new dvs_msgs::EventArray());; 64 | // dvs_msgs::Event temp_ev; 65 | // evmsg->events.push_back(temp_ev); 66 | // event_array_pub.publish(evmsg); 67 | 68 | event_reader.publish(); 69 | ros_sleep_rate.sleep(); 70 | } 71 | 72 | ROS_INFO("Finish publishing date;"); 73 | 74 | 75 | return 0; 76 | } -------------------------------------------------------------------------------- /src/event_publisher/src/event_reader.cpp: -------------------------------------------------------------------------------- 1 | #include "event_reader.hpp" 2 | 3 | 4 | Event_reader::Event_reader(std::string yaml, 5 | ros::Publisher* event_array_pub, ros::Publisher* event_image_pub) 6 | :event_array_pub_(event_array_pub), event_image_pub_(event_image_pub) 7 | { 8 | 9 | 10 | cv::FileStorage fSettings(yaml, cv::FileStorage::READ); 11 | if (!fSettings.isOpened()) 12 | { 13 | throw std::runtime_error(std::string("Could not open file: ") + yaml); 14 | } 15 | 16 | 17 | count_pos = 0; 18 | read_max_lines = fSettings["read_max_lines"]; 19 | read_start_lines = fSettings["read_start_lines"]; 20 | event_bundle_size = fSettings["Event.bundle_size"]; 21 | using_fixed_time = fSettings["using_fixed_time"]; 22 | fixed_interval = fSettings["fixed_interval"]; 23 | sleep_rate = fSettings["sleep_rate"]; 24 | 25 | store2txt = fSettings["store2txt"]; 26 | 27 | // read(fSettings["groundtruth_dir"]); 28 | 29 | string dir = fSettings["groundtruth_dir"]; 30 | cout << "opening " << dir << endl; 31 | openFile = std::ifstream(dir.c_str(),std::ios_base::in); 32 | if(!openFile.is_open()) 33 | { 34 | std::cout << "file not opened " << std::endl; 35 | return; 36 | } 37 | 38 | count_liens = 0; 39 | 40 | }; 41 | 42 | 43 | /** 44 | * \brief read file. 45 | * \param return bool indicating read success. 46 | */ 47 | bool Event_reader::read(int target_size, double target_interval) 48 | { 49 | // std::vector vec_events; 50 | // std::ifstream openFile(dir.c_str(),std::ios_base::in); 51 | // if(!openFile.is_open()) 52 | // { 53 | // std::cout << "file not opened " << std::endl; 54 | // return; 55 | // } 56 | 57 | string line, token; 58 | std::vector vToken; 59 | 60 | int start_line = count_liens; 61 | double start_time = eventData.empty() ? 0: eventData.back().ts.toSec(); 62 | 63 | dvs_msgs::Event msg; 64 | while(getline(openFile, line)) // already opened in initial function 65 | { 66 | if(count_liens++ < read_start_lines) continue; 67 | if(count_liens > read_max_lines) 68 | return false; 69 | 70 | std::stringstream ss(line); 71 | while (getline(ss, token, ' ')) 72 | vToken.push_back(token); 73 | 74 | if(vToken.size() == 4) 75 | { 76 | char* temp; 77 | msg.ts = ros::Time(std::strtod(vToken[0].c_str(), &temp)); 78 | msg.x = uint16_t(std::strtod(vToken[1].c_str(),&temp)); 79 | msg.y = uint16_t(std::strtod(vToken[2].c_str(), &temp)); 80 | msg.polarity = uint8_t(std::strtod(vToken[3].c_str(), &temp)); 81 | eventData.emplace_back(msg); 82 | } 83 | vToken.clear(); 84 | 85 | 86 | if(target_size > 0 && count_liens-start_line > target_size) break; 87 | if(target_interval > 0 && eventData.back().ts.toSec()-start_time > target_interval) break; 88 | } 89 | 90 | return true; 91 | 92 | } 93 | 94 | 95 | /** \brief 96 | \param ptr given ptr, target_size>0 means fixed size reader, target_interval>0 means fixed time reader 97 | */ 98 | bool Event_reader::acquire(dvs_msgs::EventArrayPtr ptr) 99 | { 100 | // cout <<"reading events" << endl; 101 | // ptr->events.clear(); 102 | // ptr->events.reserve(event_bundle_size); 103 | 104 | 105 | int target_size = 0; 106 | double target_interval = 0; 107 | if(using_fixed_time==0) 108 | { 109 | target_size = event_bundle_size; 110 | // cout << "using fixed size " << event_bundle_size << endl; 111 | } 112 | else 113 | { 114 | target_interval = fixed_interval; 115 | cout << "using fixed time " << fixed_interval << endl; 116 | } 117 | 118 | dvs_msgs::Event msg; 119 | 120 | // reading 121 | string line, token; 122 | std::vector vToken; 123 | 124 | int start_line = max(count_liens, read_start_lines); 125 | // double start_time = eventData.empty() ? 0: eventData.back().ts.toSec(); 126 | 127 | while(getline(openFile, line)) // already opened in initial function 128 | { 129 | if(count_liens++ < read_start_lines) continue; 130 | if(count_liens > read_max_lines) break; 131 | 132 | std::stringstream ss(line); 133 | while (getline(ss, token, ' ')) 134 | vToken.push_back(token); 135 | 136 | if(vToken.size() == 4) 137 | { 138 | char* temp; 139 | msg.ts = ros::Time(std::strtod(vToken[0].c_str(), &temp)); 140 | msg.x = uint16_t(std::strtod(vToken[1].c_str(),&temp)); 141 | msg.y = uint16_t(std::strtod(vToken[2].c_str(), &temp)); 142 | msg.polarity = uint8_t(std::strtod(vToken[3].c_str(), &temp)); 143 | // eventData.emplace_back(msg); 144 | } 145 | vToken.clear(); 146 | 147 | ptr->events.push_back(msg); 148 | 149 | if(target_size > 0 && count_liens-start_line >= target_size) break; 150 | if(target_interval > 0 && (ptr->events.back().ts-ptr->events.front().ts).toSec() > target_interval) break; 151 | } 152 | 153 | // cout <<"sucesss events " << endl; 154 | 155 | return true; 156 | 157 | } 158 | 159 | /** \brief 160 | \param ptr given ptr, target_size>0 means fixed size reader, target_interval>0 means fixed time reader 161 | */ 162 | bool Event_reader::acquire(dvs_msgs::EventArrayPtr ptr, std::vector& vec_depth) 163 | { 164 | // cout <<"reading events" << endl; 165 | // ptr->events.clear(); 166 | // ptr->events.reserve(event_bundle_size); 167 | 168 | // cout << "new acquire " << endl; 169 | 170 | int target_size = 0; 171 | double target_interval = 0; 172 | if(using_fixed_time==0) 173 | { 174 | target_size = event_bundle_size; 175 | // cout << "using fixed size " << event_bundle_size << endl; 176 | } 177 | else 178 | { 179 | target_interval = fixed_interval; 180 | cout << "using fixed time " << fixed_interval << endl; 181 | } 182 | 183 | dvs_msgs::Event msg; 184 | 185 | // reading 186 | string line, token; 187 | std::vector vToken; 188 | 189 | int start_line = max(count_liens, read_start_lines); 190 | // double start_time = eventData.empty() ? 0: eventData.back().ts.toSec(); 191 | 192 | while(getline(openFile, line)) // already opened in initial function 193 | { 194 | if(count_liens++ < read_start_lines) continue; 195 | if(count_liens > read_max_lines) break; 196 | 197 | std::stringstream ss(line); 198 | while (getline(ss, token, ' ')) 199 | vToken.push_back(token); 200 | 201 | if(vToken.size() == 5) 202 | { 203 | char* temp; 204 | msg.ts = ros::Time(std::strtod(vToken[0].c_str(), &temp)); 205 | msg.x = uint16_t(std::strtod(vToken[1].c_str(),&temp)); 206 | msg.y = uint16_t(std::strtod(vToken[2].c_str(), &temp)); 207 | msg.polarity = uint8_t(std::strtod(vToken[4].c_str(), &temp)); 208 | // eventData.emplace_back(msg); 209 | vec_depth.push_back(std::strtod(vToken[3].c_str(), &temp)); 210 | // cout << msg.ts <<" " << msg.x <<" " << msg.y <<" " << msg.polarity << endl; 211 | } 212 | vToken.clear(); 213 | 214 | ptr->events.push_back(msg); 215 | 216 | if(target_size > 0 && count_liens-start_line >= target_size) break; 217 | if(target_interval > 0 && (ptr->events.back().ts-ptr->events.front().ts).toSec() > target_interval) break; 218 | } 219 | 220 | // cout <<"sucesss events " << endl; 221 | 222 | return true; 223 | 224 | } 225 | 226 | void Event_reader::publish() 227 | { 228 | 229 | if (!msg_ptr) 230 | { 231 | msg_ptr = dvs_msgs::EventArrayPtr(new dvs_msgs::EventArray()); 232 | } 233 | 234 | // fixed number 235 | if(!using_fixed_time) 236 | { 237 | read(event_bundle_size, 0); // read fixed size 238 | int current_size = 0; 239 | 240 | fstream est_velocity_file; 241 | 242 | if(store2txt) 243 | { 244 | est_velocity_file = fstream("/home/hxy/Desktop/data/seg_events/seg_start_" + 245 | std::to_string(count_pos) + ".txt", ios::out); 246 | } 247 | 248 | while(count_pos < eventData.size()) 249 | { 250 | msg_ptr->events.push_back(eventData[count_pos]); 251 | msg_ptr->header.seq = count_pos; 252 | msg_ptr->header.stamp = eventData[count_pos].ts; 253 | 254 | if(store2txt) 255 | { 256 | est_velocity_file << eventData[count_pos].ts 257 | << " " <0) << endl; 260 | } 261 | 262 | current_size++; 263 | count_pos++; 264 | } 265 | 266 | est_velocity_file.close(); 267 | 268 | } 269 | else // fixed interval 270 | { 271 | read(0, fixed_interval); // read fixed time 272 | double interval = 0; 273 | while(count_pos < eventData.size()) 274 | { 275 | msg_ptr->events.push_back(eventData[count_pos]); 276 | msg_ptr->header.seq = count_pos; 277 | msg_ptr->header.stamp = eventData[count_pos].ts; 278 | 279 | interval = (msg_ptr->events.back().ts - msg_ptr->events.front().ts).toSec(); 280 | count_pos++; 281 | } 282 | 283 | } 284 | 285 | if(!msg_ptr->events.empty()) 286 | { 287 | msg_ptr->height = 180; 288 | msg_ptr->width = 240; 289 | event_array_pub_->publish(msg_ptr); 290 | cout << "current time " << msg_ptr->events[0].ts << 291 | ", msg.size " << msg_ptr->events.size() << ", interval " <<(msg_ptr->events.back().ts - msg_ptr->events.front().ts).toSec() << endl; 292 | } 293 | msg_ptr.reset(); 294 | } 295 | 296 | 297 | 298 | 299 | /** 300 | * \brief accumulate events to event frame. 301 | */ 302 | void Event_reader::render() 303 | { 304 | 305 | } -------------------------------------------------------------------------------- /src/event_publisher/src/main_depth.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | // opencv 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | 13 | // messages 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "Event.h" 21 | #include "IMU.h" 22 | #include "Image.h" 23 | 24 | // file manager 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | 31 | int main(int argc, char **argv) 32 | { 33 | 34 | ros::init(argc, argv, "event_publisher"); 35 | 36 | ros::NodeHandle nh_private("~"); 37 | 38 | std::string filename = "filename"; 39 | std::string yaml = "yaml"; 40 | bool loop = false; 41 | nh_private.param("filename", filename, ""); 42 | nh_private.param("yaml", yaml, ""); 43 | nh_private.param("loop", loop, ""); 44 | 45 | // indoor flying 46 | ros::Publisher event_depth_array_pub = nh_private.advertise("/events_depth", 10); 47 | // ros::Publisher imu_pub = nh_private.advertise("/imu", 10); 48 | // ros::Publisher image_pub = nh_private.advertise("/image", 10); 49 | // ros::Publisher dvs_image_pub = nh_private.advertise("/renderer", 10); 50 | 51 | ros::Rate loop_rate(10); 52 | 53 | Event event(yaml); 54 | // IMU imu(yaml); 55 | // Image image(yaml); 56 | 57 | ROS_INFO("Reading event data"); 58 | bool isEventEmpty = event.eventDepthReader(filename); 59 | // ROS_INFO("Reading imu data"); 60 | // bool isImuEmpty = imu.imuReader(filename); 61 | // ROS_INFO("Reading image data"); 62 | // bool isImageEmpty = image.imageReader(filename); 63 | 64 | // event.setEventPublisher(&event_array_pub); 65 | event.setEventDepthPublisher(&event_depth_array_pub); 66 | // event.setDvsPublisher(&dvs_image_pub); 67 | // imu.setIMUPublisher(&imu_pub); 68 | // image.setImagePublisher(&image_pub); 69 | 70 | // initialize 71 | double time_begin = double(ros::Time::now().toSec()); 72 | double time_curr, time; 73 | 74 | ROS_INFO("Start publishing data"); 75 | while (ros::ok()) 76 | { 77 | time = double(ros::Time::now().toSec()); 78 | time_curr = time - time_begin; 79 | 80 | event.setCurrTime(time_curr); 81 | // imu.setCurrTime(time_curr); 82 | // image.setCurrTime(time_curr); 83 | // event.setImage(image.last_image); 84 | 85 | event.publishDepth(); 86 | // imu.publish(); 87 | // image.publish(); 88 | 89 | ros::spinOnce(); 90 | 91 | // termination condition 92 | if(!event.isRunning){ 93 | if(loop){ 94 | time_begin = time; 95 | event.reset(); 96 | // image.reset(); 97 | // imu.reset(); 98 | } 99 | else{ 100 | break; 101 | } 102 | } 103 | } 104 | ROS_INFO("Finish publishing data"); 105 | 106 | return 0; 107 | } -------------------------------------------------------------------------------- /src/rotation_estimator/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(rotation_estimator) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++14) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | cv_bridge 12 | eigen_conversions 13 | nav_msgs 14 | roscpp 15 | sensor_msgs 16 | std_msgs 17 | tf 18 | dvs_msgs 19 | event_publisher 20 | ) 21 | 22 | find_package(OpenCV 4 REQUIRED) 23 | find_package(Sophus REQUIRED ) 24 | find_package(Ceres REQUIRED ) 25 | find_package(Eigen3 3.3 REQUIRED PATH /usr/local ) 26 | 27 | 28 | find_package(OpenMP) 29 | 30 | if(OPENMP_FOUND) 31 | message("OpenMP found") 32 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 33 | set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}") 34 | else() 35 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-unknown-pragmas") 36 | endif() 37 | 38 | message("hxy found openMP " ${OPENMP_FOUND}) 39 | 40 | ## System dependencies are found with CMake's conventions 41 | # find_package(Boost REQUIRED COMPONENTS system) 42 | 43 | 44 | ## Uncomment this if the package has a setup.py. This macro ensures 45 | ## modules and global scripts declared therein get installed 46 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 47 | # catkin_python_setup() 48 | 49 | ################################################ 50 | ## Declare ROS messages, services and actions ## 51 | ################################################ 52 | 53 | ## To declare and build messages, services or actions from within this 54 | ## package, follow these steps: 55 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 56 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 57 | ## * In the file package.xml: 58 | ## * add a build_depend tag for "message_generation" 59 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 60 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 61 | ## but can be declared for certainty nonetheless: 62 | ## * add a exec_depend tag for "message_runtime" 63 | ## * In this file (CMakeLists.txt): 64 | ## * add "message_generation" and every package in MSG_DEP_SET to 65 | ## find_package(catkin REQUIRED COMPONENTS ...) 66 | ## * add "message_runtime" and every package in MSG_DEP_SET to 67 | ## catkin_package(CATKIN_DEPENDS ...) 68 | ## * uncomment the add_*_files sections below as needed 69 | ## and list every .msg/.srv/.action file to be processed 70 | ## * uncomment the generate_messages entry below 71 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 72 | 73 | ## Generate messages in the 'msg' folder 74 | # add_message_files( 75 | # FILES 76 | # Message1.msg 77 | # Message2.msg 78 | # ) 79 | 80 | ## Generate services in the 'srv' folder 81 | # add_service_files( 82 | # FILES 83 | # Service1.srv 84 | # Service2.srv 85 | # ) 86 | 87 | ## Generate actions in the 'action' folder 88 | # add_action_files( 89 | # FILES 90 | # Action1.action 91 | # Action2.action 92 | # ) 93 | 94 | ## Generate added messages and services with any dependencies listed here 95 | # generate_messages( 96 | # DEPENDENCIES 97 | # nav_msgs# sensor_msgs# std_msgs 98 | # ) 99 | 100 | ################################################ 101 | ## Declare ROS dynamic reconfigure parameters ## 102 | ################################################ 103 | 104 | ## To declare and build dynamic reconfigure parameters within this 105 | ## package, follow these steps: 106 | ## * In the file package.xml: 107 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 108 | ## * In this file (CMakeLists.txt): 109 | ## * add "dynamic_reconfigure" to 110 | ## find_package(catkin REQUIRED COMPONENTS ...) 111 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 112 | ## and list every .cfg file to be processed 113 | 114 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 115 | # generate_dynamic_reconfigure_options( 116 | # cfg/DynReconf1.cfg 117 | # cfg/DynReconf2.cfg 118 | # ) 119 | 120 | ################################### 121 | ## catkin specific configuration ## 122 | ################################### 123 | ## The catkin_package macro generates cmake config files for your package 124 | ## Declare things to be passed to dependent projects 125 | ## INCLUDE_DIRS: uncomment this if your package contains header files 126 | ## LIBRARIES: libraries you create in this project that dependent projects also need 127 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 128 | ## DEPENDS: system dependencies of this project that dependent projects also need 129 | catkin_package( 130 | INCLUDE_DIRS include 131 | LIBRARIES rotation_estimator 132 | CATKIN_DEPENDS cv_bridge eigen_conversions roscpp sensor_msgs std_msgs event_publisher 133 | DEPENDS 134 | ) 135 | 136 | ########### 137 | ## Build ## 138 | ########### 139 | 140 | ## Specify additional locations of header files 141 | ## Your package locations should be listed before other locations 142 | include_directories( 143 | include 144 | "/home/hxy/Desktop/src/event_publisher/include" 145 | ${catkin_INCLUDE_DIRS} 146 | ${OpenCV_INCLUDE_DIRS} 147 | ${Sophus_INCLUDE_DIRS} 148 | ${CERES_INCLUDE_DIRS} 149 | ) 150 | 151 | ## Declare a C++ library 152 | # add_library(${PROJECT_NAME} 153 | # src/${PROJECT_NAME}/rotation_estimator.cpp 154 | # ) 155 | 156 | ## Add cmake target dependencies of the library 157 | ## as an example, code may need to be generated before libraries 158 | ## either from message generation or dynamic reconfigure 159 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 160 | 161 | ## Declare a C++ executable 162 | ## With catkin_make all packages are built within a single CMake context 163 | ## The recommended prefix ensures that target names across packages don't collide 164 | add_executable(${PROJECT_NAME}_node 165 | src/rotation_estimator_node.cpp 166 | # src/callbacks.cpp 167 | src/database.cpp 168 | src/system.cpp 169 | src/numerics.cpp 170 | src/system_estimate.cpp # store utils for motion estimation 171 | src/system_estimate_ransac_doublewarp_ceres.cpp # timesurface method with double warp using ceres 172 | ) # using kim method with self defined gradient function, R 173 | 174 | ## Rename C++ executable without prefix 175 | ## The above recommended prefix causes long target names, the following renames the 176 | ## target back to the shorter version for ease of user use 177 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 178 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 179 | 180 | ## Add cmake target dependencies of the executable 181 | ## same as for the library above 182 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 183 | 184 | ## Specify libraries to link a library or executable target against 185 | target_link_libraries(${PROJECT_NAME}_node 186 | ${catkin_LIBRARIES} 187 | ${OpenCV_LIBS} 188 | ${eigen3_LIBS} 189 | ${Sophus_LIBRARIES} 190 | ${CERES_LIBRARIES} 191 | ) 192 | 193 | ############# 194 | ## Install ## 195 | ############# 196 | 197 | # all install targets should use catkin DESTINATION variables 198 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 199 | 200 | ## Mark executable scripts (Python etc.) for installation 201 | ## in contrast to setup.py, you can choose the destination 202 | # catkin_install_python(PROGRAMS 203 | # scripts/my_python_script 204 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 205 | # ) 206 | 207 | ## Mark executables for installation 208 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 209 | # install(TARGETS ${PROJECT_NAME}_node 210 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 211 | # ) 212 | 213 | ## Mark libraries for installation 214 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 215 | # install(TARGETS ${PROJECT_NAME} 216 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 217 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 218 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 219 | # ) 220 | 221 | ## Mark cpp header files for installation 222 | # install(DIRECTORY include/${PROJECT_NAME}/ 223 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 224 | # FILES_MATCHING PATTERN "*.h" 225 | # PATTERN ".svn" EXCLUDE 226 | # ) 227 | 228 | ## Mark other files for installation (e.g. launch and bag files, etc.) 229 | # install(FILES 230 | # # myfile1 231 | # # myfile2 232 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 233 | # ) 234 | 235 | ############# 236 | ## Testing ## 237 | ############# 238 | 239 | ## Add gtest based cpp test target and link libraries 240 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_rotation_estimator.cpp) 241 | # if(TARGET ${PROJECT_NAME}-test) 242 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 243 | # endif() 244 | 245 | ## Add folders to be run by python nosetests 246 | # catkin_add_nosetests(test) 247 | -------------------------------------------------------------------------------- /src/rotation_estimator/config/config.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | Version: 1.0 4 | 5 | #-------------------------------------------------------------------------------------------- 6 | # General Parameters 7 | #-------------------------------------------------------------------------------------------- 8 | 9 | 10 | #-------------------------------------------------------------------------------------------- 11 | # General Parameters (fixed). 12 | #-------------------------------------------------------------------------------------------- 13 | # original image size for camera 14 | # width: 240 15 | # height: 180 16 | 17 | #-------------------------------------------------------------------------------------------- 18 | # Input Parameters 19 | #-------------------------------------------------------------------------------------------- 20 | # groundtruth_dir: "/home/hxy/Documents/rosbag/Mono-unzip/poster_rotation/events.txt" 21 | # groundtruth_dir: "/home/hxy/Documents/rosbag/Mono-unzip/dynamic_rotation/events.txt" 22 | groundtruth_dir: "/home/hxy/Documents/rosbag/Mono-unzip/boxes_rotation/events.txt" 23 | # groundtruth_dir: "/home/hxy/Documents/rosbag/Mono-unzip/shapes_rotation/events.txt" 24 | output_dir: "/home/hxy/Desktop/data/rotation_estimation/poster_rotation/test_second_order_size30k_double_warp_" 25 | 26 | yaml_sample_count: 30000 27 | yaml_iter_num: 10 28 | yaml_ceres_iter_num: 10 29 | yaml_ceres_iter_thread: 8 30 | yaml_ts_start: 0.8 31 | yaml_ts_end: 0.8 32 | yaml_denoise_num: 4 33 | yaml_gaussian_size: 5 34 | yaml_gaussian_size_sigma: 1 35 | yaml_default_value_factor: 1 36 | yaml_ros_starttime: 0 # poster 1468940145.2311351, boxes 1468940843.8233166, dynamic 1473347265.9092634 37 | # 2.7e7~3.1e7 for dynamic 38 | # 5.7e6~5.9e6 for shapes 39 | # you can use 40 | read_start_lines: 0e4 41 | read_max_lines: 25000e4 42 | # read_start_lines: 1000e4 43 | # read_max_lines: 1090e4 44 | 45 | 46 | #-------------------------------------------------------------------------------------------- 47 | # Event Parameters 48 | #-------------------------------------------------------------------------------------------- 49 | Event.delta_time: 0.01 50 | using_fixed_time: 0 # 1 means true 51 | 52 | fixed_interval: 0.02 53 | Event.bundle_size: 3e4 -------------------------------------------------------------------------------- /src/rotation_estimator/include/callbacks.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | 4 | // ros 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | 14 | // std 15 | #include 16 | #include 17 | 18 | 19 | // self 20 | #include "system.hpp" 21 | 22 | using namespace std; 23 | 24 | class ImageGrabber 25 | { 26 | public: 27 | ImageGrabber(System* sys): system(sys){} 28 | void GrabImage(const sensor_msgs::ImageConstPtr& msg); 29 | System* system; 30 | // ros::Time begin_time; 31 | }; 32 | 33 | 34 | class EventGrabber 35 | { 36 | public: 37 | EventGrabber(System* sys): system(sys){} 38 | 39 | void GrabEvent(const dvs_msgs::EventArrayConstPtr& msg); 40 | System* system; 41 | }; 42 | 43 | class PoseGrabber 44 | { 45 | public: 46 | PoseGrabber(System* sys): system(sys) { 47 | // que_valid = false; 48 | gt_velocity_file = fstream("/home/hxt/Desktop/data/gt_theta_velocity.txt", ios::out); 49 | // gt_velocity_file_quat = fstream("/home/hxt/Desktop/data/evo_data/gt_theta_velocity.txt", ios::out); 50 | } 51 | void GrabPose(const geometry_msgs::PoseStampedConstPtr& msg); 52 | System* system; 53 | 54 | 55 | fstream gt_velocity_file; 56 | // fstream gt_velocity_file_quat; //evo esti 57 | 58 | // bool que_valid; 59 | queue que_last_poseData; 60 | // ros::Time begin_time; 61 | }; 62 | 63 | -------------------------------------------------------------------------------- /src/rotation_estimator/include/database.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // std 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | // ros 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | using namespace std; 22 | 23 | 24 | struct EventData 25 | { 26 | double time_stamp; 27 | vector event; 28 | }; 29 | 30 | struct ImageData 31 | { 32 | double time_stamp; 33 | cv::Mat image; 34 | uint32_t seq; 35 | }; 36 | 37 | struct PoseData 38 | { 39 | double time_stamp; // starts from 0s 40 | ros::Time time_stamp_ros; // ros time 41 | uint32_t seq; 42 | Eigen::Quaterniond quat; // quanterion 43 | Eigen::Vector3d pose; // xyz 44 | }; 45 | 46 | struct CameraPara 47 | { 48 | 49 | CameraPara(); 50 | CameraPara(string filename); 51 | double fx; 52 | double fy; 53 | double cx; 54 | double cy; 55 | double k1, k2, p1, p2, k3; 56 | // double rd1; 57 | // double rd2; 58 | 59 | int width, height, height_map, width_map; 60 | 61 | cv::Mat cameraMatrix, distCoeffs ; 62 | 63 | Eigen::Matrix3d eg_cameraMatrix, eg_MapMatrix; 64 | 65 | 66 | }; 67 | 68 | enum PlotOption 69 | { 70 | U16C3_EVNET_IMAGE_COLOR, 71 | U16C1_EVNET_IMAGE, 72 | U8C1_EVNET_IMAGE, 73 | S16C1_EVNET_IMAGE, 74 | TIME_SURFACE, 75 | F32C1_EVENT_COUNT, 76 | }; 77 | 78 | 79 | /** 80 | * \brief a set of local events, stores as Eigen::Matrix2xf. 81 | */ 82 | struct EventBundle{ 83 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 84 | 85 | EventBundle(); 86 | EventBundle(const EventBundle& eb); 87 | ~EventBundle(); 88 | 89 | // core opearte 90 | void Append(std::vector& vec_eventData); 91 | void CopySize(const EventBundle& eb); 92 | 93 | void Clear(); 94 | void DiscriminateInner(int widht, int height); 95 | 96 | // image process 97 | // GetEventImage() // options with signed or color 98 | void InverseProjection(Eigen::Matrix3d& K); 99 | void Projection(Eigen::Matrix3d& K); 100 | 101 | 102 | // events in eigen form used as 3d porjection 103 | Eigen::Matrix2Xd coord; // row, col = [2,pixels], used for Eigen rotation 104 | Eigen::Matrix3Xd coord_3d; // row, col = [3,pixels], used for Eigen rotation 105 | 106 | // relative time of event 107 | Eigen::VectorXd time_delta; // later is positive 108 | Eigen::VectorXd time_delta_rev; // currently not used 109 | 110 | // the estimate para of local events 111 | Eigen::Vector3d angular_velocity, // angleAxis current velocity, used to sharp local events 112 | angular_position; // angleAxis current pos, warp cur camera to world coord. 113 | 114 | // Eigen::Matrix3d rotation_cur2ref; // from camera to world s 115 | 116 | // events in vector form, used as data storage 117 | // double abs_tstamp; // receiving time at ROS system 118 | 119 | ros::Time first_tstamp, last_tstamp; // event time in ros system. 120 | // vector x, y; // original event coor used for opencv 121 | Eigen::VectorXf polar; // 0, 1 event polar 122 | Eigen::VectorXf isInner; // 0, 1 indicating the event is inner 123 | size_t size; 124 | 125 | }; 126 | -------------------------------------------------------------------------------- /src/rotation_estimator/include/numerics.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | #include 9 | 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | Eigen::Matrix3d hat(const Eigen::Vector3d &x); 18 | Eigen::Vector3d unhat(const Eigen::Matrix3d &x); 19 | Eigen::Matrix3d SO3(const Eigen::Vector3d &x); 20 | Eigen::Vector3d InvSO3(const Eigen::Matrix3d &x_hat); 21 | Eigen::Vector3d SO3add(const Eigen::Vector3d x, const Eigen::Vector3d y, bool cycle); 22 | 23 | 24 | struct MyEulerAngles { 25 | double roll, pitch, yaw; }; 26 | 27 | Eigen::Vector3d toEulerAngles(Eigen::Quaterniond q); 28 | 29 | Eigen::Quaterniond ToQuaternion(double yaw, double pitch, double roll); 30 | 31 | 32 | double getVar(cv::Mat& image, int& nonZero, int type); -------------------------------------------------------------------------------- /src/rotation_estimator/include/system.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | 4 | // ros 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | 13 | // third party 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | // std 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | 31 | // self 32 | #include "database.hpp" 33 | #include "numerics.hpp" 34 | 35 | 36 | using namespace std; 37 | 38 | /** 39 | * \brief receive ros_msg + imgproc + optimize + visualize 40 | */ 41 | class System 42 | { 43 | public: 44 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 45 | System(const string& yaml); 46 | ~System(); 47 | 48 | // ros msg 49 | void pushEventData(const std::vector& ros_vec_event); 50 | void pushimageData(const ImageData& imageData); 51 | void pushPoseData(const PoseData& poseData); 52 | void updateEventBundle(); // use less 53 | void setBeginTime(ros::Time); 54 | 55 | void save_velocity(); // save velocity date to txt 56 | 57 | Eigen::Matrix3d get_local_rotation_b2f(bool inverse = false); 58 | Eigen::Matrix3d get_global_rotation_b2f(size_t idx_t1, size_t idx_t2); 59 | 60 | 61 | // imgproc 62 | void Run(); 63 | void undistortEvents(); 64 | cv::Mat getWarpedEventImage(const Eigen::Vector3d & temp_ang_vel,EventBundle& event_out, 65 | const PlotOption& option = PlotOption::U16C3_EVNET_IMAGE_COLOR, bool ref_t1 = false, float range=1); 66 | 67 | void getWarpedEventPoints(const EventBundle& eventIn, EventBundle& eventOut, 68 | const Eigen::Vector3d& cur_ang_vel, bool ref_t1=false, float range=1); 69 | cv::Mat getImageFromBundle(EventBundle& eventBundle, 70 | const PlotOption option = PlotOption::U16C3_EVNET_IMAGE_COLOR, float range=1); 71 | 72 | void getMapImage(); 73 | 74 | // optimize 75 | void localCM(); 76 | 77 | void EstimateMotion_kim(); 78 | void EstimateMotion_CM_ceres(); 79 | void EstimateMotion_PPP_ceres(); 80 | void EstimateMotion_ransca_ceres(); 81 | void EstimateMotion_ransca_ceres_RT(); 82 | void EstimateMotion_KS_ceres(); 83 | 84 | void EstimateRunTime_CM(); 85 | void EstimateRunTime_PPP(); 86 | void EstimateRunTime_Single(); 87 | void EstimateRunTime_Double(); 88 | 89 | // void EstimateMotion_ransca_once(double sample_ratio, double warp_time_ratio, double opti_steps); 90 | // void EstimateMotion_ransca_warp2bottom(double sample_start, double sample_end, double opti_steps); 91 | 92 | Eigen::Vector3d DeriveErrAnalytic(const Eigen::Vector3d &vel_angleAxis, const Eigen::Vector3d &pos_angleAxis); 93 | 94 | 95 | Eigen::Vector3d DeriveTimeErrAnalyticRansacBottom(const Eigen::Vector3d &vel_angleAxis, 96 | const std::vector& vec_sampled_idx, double& residuals); 97 | Eigen::Vector3d GetGlobalTimeResidual(); 98 | 99 | // random warp time version 100 | Eigen::Vector3d DeriveTimeErrAnalyticRansac(const Eigen::Vector3d &vel_angleAxis, 101 | const std::vector& vec_sampled_idx, double warp_time, double& residuals); 102 | void getTimeResidual(int sampled_x, int sampled_y, double sampled_time, double warp_time, 103 | double& residual, double& grad_x, double& grad_y); 104 | // Eigen::Vector3d DeriveTimeErrAnalyticLayer(const Eigen::Vector3d &vel_angleAxis, 105 | // const std::vector& vec_sampled_idx, double warp_time, double& residuals); 106 | 107 | void getSampledVec(vector& vec_sampled_idx, int samples_count, double sample_start, double sample_end); 108 | 109 | // visualize 110 | void visualize(); 111 | 112 | // file 113 | bool inline file_opened() {return est_velocity_file.is_open();}; 114 | 115 | double total_evaluate_time, 116 | total_undistort_time, 117 | total_visual_time, 118 | total_timesurface_time, 119 | total_ceres_time, 120 | total_readevents_time, 121 | total_eventbundle_time, 122 | total_warpevents_time; 123 | 124 | 125 | // bool inline checkEmpty(){return que_vec_eventData.empty();} 126 | 127 | // thread 128 | // thread* thread_run; 129 | // thread* thread_view; 130 | // std::mutex que_vec_eventData_mutex; 131 | 132 | private: 133 | 134 | // configration 135 | string yaml; 136 | int yaml_iter_num; 137 | float yaml_ts_start; 138 | float yaml_ts_end; 139 | int yaml_sample_count; 140 | int yaml_ceres_iter_num; 141 | int yaml_gaussian_size; 142 | float yaml_gaussian_size_sigma; 143 | int yaml_denoise_num; 144 | float yaml_default_value_factor; 145 | int yaml_ceres_iter_thread; 146 | double yaml_ros_starttime; 147 | // motion 148 | vector vec_curr_time; 149 | vector vec_angular_velocity; 150 | vector vec_angular_position; 151 | 152 | 153 | // optimization 154 | cv::Mat cv_3D_surface_index, cv_3D_surface_index_count ; 155 | 156 | // camera param 157 | CameraPara camera; 158 | 159 | // image data 160 | ImageData curr_imageData; 161 | 162 | // image output 163 | cv::Mat curr_raw_image, // grey image from ros 164 | curr_undis_image, // undistort grey image 165 | curr_event_image, // current blur event image 166 | curr_undis_event_image, // current undistorted event image 167 | curr_warpped_event_image, // current sharp local event image using est 168 | curr_warpped_event_image_gt, // current sharp local event image using gt 169 | curr_map_image, // global image at t_curr view 170 | hot_image_C1, 171 | hot_image_C3; // time surface with applycolormap 172 | // undistor parameter 173 | cv::Mat undist_mesh_x, undist_mesh_y; 174 | 175 | // event data 176 | ros::Time beginTS; // begin time stamp of ros Time. 177 | 178 | EventBundle eventBundle; // current blur local events 179 | EventBundle event_undis_Bundle; // current undistort local events 180 | EventBundle event_warpped_Bundle; // current sharp local events 181 | EventBundle event_warpped_Bundle_gt; // current sharp local events 182 | EventBundle event_Map_Bundle; // current sharp local events the that warp to t0. 183 | 184 | // std::queue> que_vec_eventData; // saved eventData inorder to save 185 | std::vector> vec_vec_eventData; // saved eventData inorder to save 186 | std::vector> vec_vec_eventdepth; 187 | int vec_vec_eventData_iter; // used to indicating the position of unpushed events. 188 | 189 | // map 3d 190 | vector vec_Bundle_Maps; // all the eventbundles that warpped to t0. 191 | 192 | 193 | // event bundle, how many msgs to build a bundle 194 | double delta_time = 0.01; // 0.01 seconds 195 | int max_store_count = int(1e5); // max local event nums 196 | 197 | 198 | // pose 199 | bool using_gt; 200 | vector vec_gt_poseData; // stored rosmsg 201 | 202 | // 逻辑是t2-t1->t0. 如eq(3)所示 203 | Eigen::Vector3d gt_angleAxis ; // gt anglar anxis from t2->t1,. = theta / delta_time 204 | Eigen::Vector3d est_angleAxis; // estimated anglar anxis from t2->t1. = theta / delta_time 205 | 206 | Eigen::Vector3d last_est_var; // 正则项， 控制est_N_norm的大小 207 | 208 | // output 209 | size_t seq_count; 210 | fstream est_theta_file, est_velocity_file; 211 | // fstream est_velocity_file_quat; // evo 212 | 213 | }; 214 | 215 | 216 | 217 | 218 | 219 | -------------------------------------------------------------------------------- /src/rotation_estimator/launch/estimation.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /src/rotation_estimator/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rotation_estimator 4 | 0.0.0 5 | The rotation_estimator package 6 | 7 | 8 | 9 | 10 | hxt 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 | eigen_conversions 54 | nav_msgs 55 | roscpp 56 | sensor_msgs 57 | std_msgs 58 | tf 59 | cv_bridge 60 | eigen_conversions 61 | nav_msgs 62 | roscpp 63 | sensor_msgs 64 | std_msgs 65 | tf 66 | cv_bridge 67 | eigen_conversions 68 | nav_msgs 69 | roscpp 70 | sensor_msgs 71 | std_msgs 72 | tf 73 | 74 | event_publisher 75 | event_publisher 76 | 77 | dvs_msgs 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | -------------------------------------------------------------------------------- /src/rotation_estimator/src/callbacks.cpp: -------------------------------------------------------------------------------- 1 | #include "callbacks.hpp" 2 | #include "database.hpp" 3 | 4 | ros::Time begin_time = ros::Time(0); 5 | 6 | void ImageGrabber::GrabImage(const sensor_msgs::ImageConstPtr& msg) 7 | { 8 | if(begin_time == ros::Time(0)) 9 | { 10 | begin_time = msg->header.stamp; 11 | system->setBeginTime(msg->header.stamp); 12 | } 13 | 14 | static int last_img_seq = msg->header.seq; 15 | 16 | cv_bridge::CvImageConstPtr cv_ptr; 17 | try 18 | { 19 | cv_ptr = cv_bridge::toCvShare(msg,"mono8"); // gray 8char 20 | } 21 | catch(cv_bridge::Exception& e) 22 | { 23 | ROS_ERROR("receive image error: %s", e.what()); 24 | return ; 25 | } 26 | 27 | // fixme not need to constru a new imageData obj 28 | ImageData ImageData; 29 | ImageData.image = cv_ptr->image.clone(); 30 | ImageData.seq = msg->header.seq; 31 | ImageData.time_stamp = (cv_ptr->header.stamp - begin_time).toSec(); 32 | cout << "receiving image t: " << ImageData.time_stamp<< endl; 33 | 34 | system->pushimageData(ImageData); 35 | } 36 | 37 | 38 | void EventGrabber::GrabEvent(const dvs_msgs::EventArrayConstPtr& msg) 39 | { 40 | if(msg->events.empty() || msg->events.size()<1000) return; 41 | 42 | if(begin_time == ros::Time(0)) 43 | { 44 | begin_time = msg->events[0].ts; 45 | system->setBeginTime(msg->events[0].ts); 46 | cout << "begin time " << msg->events[0].ts.sec << "." <events[0].ts.nsec << endl; 47 | } 48 | 49 | 50 | // not need to copy eventdata obj 51 | // EventData eventdata; 52 | // eventdata.event = msg->events; // vector 53 | double time_stamp = (msg->events[0].ts - begin_time).toSec(); 54 | double delta_time = (msg->events.back().ts-msg->events.front().ts).toSec(); 55 | cout<<"receiving events " << msg->events.size() <<", time: " << msg->events[0].ts.toSec()<<", delta time " << delta_time <que_vec_eventData_mutex.lock(); 58 | system->pushEventData(msg->events); 59 | // system->que_vec_eventData_mutex.unlock(); 60 | 61 | } 62 | 63 | 64 | void PoseGrabber::GrabPose(const geometry_msgs::PoseStampedConstPtr& msg) 65 | { 66 | 67 | if(begin_time == ros::Time(0)) 68 | { 69 | begin_time = msg->header.stamp; 70 | system->setBeginTime(msg->header.stamp); 71 | } 72 | 73 | // not need to copy eventdata obj 74 | PoseData poseData; 75 | poseData.time_stamp = (msg->header.stamp-begin_time).toSec(); 76 | poseData.time_stamp_ros = msg->header.stamp; 77 | 78 | // vector 79 | poseData.pose << msg->pose.position.x, msg->pose.position.y, msg->pose.position.z; 80 | 81 | // input w,x,y,z. output and store: x,y,z,w 82 | poseData.quat = Eigen::Quaterniond(msg->pose.orientation.w, msg->pose.orientation.x, 83 | msg->pose.orientation.y, msg->pose.orientation.z); 84 | 85 | cout<<"receiving poses t: " << poseData.time_stamp << "， ros: " << std::to_string(poseData.time_stamp_ros.toSec()) << endl; 86 | cout << "----(xyz)(wxyz)" << poseData.pose.transpose() << poseData.quat.coeffs().transpose() << endl; 87 | 88 | 89 | if(que_last_poseData.size() < 10) 90 | { 91 | que_last_poseData.push(poseData); 92 | } 93 | else 94 | { 95 | PoseData last_poseData = que_last_poseData.front(); 96 | que_last_poseData.pop(); 97 | 98 | Eigen::Quaterniond t1_t2 = last_poseData.quat.inverse() * poseData.quat; 99 | // last_poseData.quat.angularDistance(poseData.quat); 100 | 101 | Eigen::Vector3d velocity = toEulerAngles(t1_t2) * 1 / (poseData.time_stamp_ros - last_poseData.time_stamp_ros).toSec(); 102 | 103 | // get middle timestamp of event bundle 104 | uint32_t second = last_poseData.time_stamp_ros.sec; 105 | uint32_t nsecond; 106 | if(last_poseData.time_stamp_ros.nsec > poseData.time_stamp_ros.nsec) 107 | { 108 | // example: first 1.12, last 1.15; 109 | nsecond = last_poseData.time_stamp_ros.nsec + (poseData.time_stamp_ros.nsec-last_poseData.time_stamp_ros.nsec)/2; 110 | } 111 | else 112 | { 113 | uint32_t delta_to_second = uint32_t(1000000000) - last_poseData.time_stamp_ros.nsec; 114 | // example: first 1.96, last 1.01; 115 | if(delta_to_second > poseData.time_stamp_ros.nsec) 116 | { 117 | nsecond = last_poseData.time_stamp_ros.nsec + (delta_to_second+poseData.time_stamp_ros.nsec) / 2; 118 | } 119 | // example: first 1.99, last 1.02; 120 | else 121 | { 122 | nsecond = (poseData.time_stamp_ros.nsec - delta_to_second) / 2; 123 | second++; 124 | } 125 | } 126 | 127 | gt_velocity_file << poseData.time_stamp_ros.sec << " " << poseData.time_stamp_ros.nsec <<" " << velocity.transpose() << endl; 128 | 129 | // evo part 130 | // // todo from quat to agnles axis and divide by time, get velocity 131 | // Eigen::AngleAxisd angle_axis = Eigen::AngleAxisd(t1_t2); 132 | // // cout << "angle axis " << angle_axis.axis() << "," << angle_axis.angle() << endl; 133 | // angle_axis.angle() = angle_axis.angle() / (poseData.time_stamp_ros - last_poseData.time_stamp_ros).toSec(); 134 | // // cout << "angle axis " << angle_axis.axis() << "," << angle_axis.angle() << endl; 135 | // Eigen::Quaterniond q = Eigen::Quaterniond(angle_axis); 136 | 137 | // gt_velocity_file_quat << poseData.time_stamp_ros << " 0 0 0 " << q.x() << " " 138 | // << q.y() << " " << q.z() << " " << q.w() << endl; 139 | 140 | last_poseData = poseData; 141 | que_last_poseData.push(poseData); 142 | } 143 | 144 | 145 | // system->pushPoseData(poseData); 146 | } -------------------------------------------------------------------------------- /src/rotation_estimator/src/database.cpp: -------------------------------------------------------------------------------- 1 | #include "database.hpp" 2 | 3 | 4 | using namespace std; 5 | 6 | 7 | CameraPara::CameraPara() 8 | { 9 | width = 240; 10 | height = 180; 11 | 12 | // fx = 230.2097; 13 | // fy = 231.1228; 14 | // cx = 121.6862; 15 | // cy = 86.8208; 16 | // rd1 = -0.4136; 17 | // rd2 = 0.2042; 18 | 19 | fx = 199.092366542; 20 | fy = 198.82882047; 21 | cx = 132.192071378; 22 | cy = 110.712660011; 23 | k1 = -0.368436311798; 24 | k2 = 0.150947243557; 25 | p1 = -0.000296130534385; 26 | p2 = -0.000759431726241; 27 | k3 = 0.0; 28 | 29 | 30 | 31 | 32 | cameraMatrix = (cv::Mat_(3,3) << 33 | fx, 0, cx , 0, fy, cy, 0, 0, 1); 34 | distCoeffs = (cv::Mat_(1,5) << k1, k2, p1, p2, k3); 35 | 36 | // cout << "camera param:" << endl; 37 | // cout << cameraMatrix << endl; 38 | 39 | cv::cv2eigen(cameraMatrix, eg_cameraMatrix); 40 | // cout << eg_cameraMatrix << endl; 41 | 42 | // the map展示的虚拟相机k内参可以自定义大小。 43 | height_map = 2 * height; 44 | width_map = 2 * width; 45 | eg_MapMatrix = eg_cameraMatrix; // deep copy 46 | 47 | eg_MapMatrix(0,2) = width_map/2; 48 | eg_MapMatrix(1,2) = height_map/2; 49 | 50 | cout << "camera matrix:: \n" << cameraMatrix << endl; 51 | cout << "camera matrix eigen: \n" << eg_cameraMatrix << endl; 52 | cout << "map matrix eigen: \n" << eg_MapMatrix << endl; 53 | 54 | } 55 | 56 | /** 57 | * \brief make a reference, TODO it a more efficient way?. 58 | */ 59 | EventBundle::EventBundle() 60 | { 61 | size = 0; 62 | coord.resize(2, size); 63 | coord_3d.resize(3, size); 64 | } 65 | 66 | 67 | /** 68 | * \brief make a reference, TODO it a more efficient way?. 69 | */ 70 | EventBundle::EventBundle(const EventBundle& eb) 71 | { 72 | // deep copy 73 | coord = eb.coord; 74 | coord_3d = eb.coord_3d; 75 | isInner = eb.isInner; 76 | 77 | 78 | angular_position = eb.angular_position; 79 | angular_velocity = eb.angular_velocity; 80 | 81 | time_delta = eb.time_delta; 82 | 83 | size = eb.size; 84 | 85 | // x = eb.x; 86 | // y = eb.y; 87 | polar = eb.polar; 88 | 89 | } 90 | 91 | 92 | /** 93 | * \brief reset all. 94 | */ 95 | void EventBundle::Clear() 96 | { 97 | // cout << "event bundle clearing, size " << size << endl; 98 | 99 | size = 0; 100 | 101 | // eigen reconstruct 102 | coord.resize(2,size); 103 | coord_3d.resize(3,size); 104 | isInner.resize(size); 105 | 106 | time_delta.resize(size); 107 | time_delta_rev.resize(size); 108 | 109 | angular_position = Eigen::Vector3d::Zero(); 110 | angular_velocity = Eigen::Vector3d::Zero(); 111 | 112 | // vector 113 | // time_stamps.clear(); 114 | // x.clear(); 115 | // y.clear(); 116 | // isInner.clear(); 117 | 118 | // cout << " event bundle clearing sucess" << endl; 119 | } 120 | 121 | 122 | /** 123 | * \brief strong DiscriminateInner, for the convinient of gradient . 124 | * \param width given boundary, may camera boundary or map view boundary 125 | * \param height 126 | */ 127 | void EventBundle::DiscriminateInner(int width, int height) 128 | { 129 | // cout << "DiscriminateInner "<< size << endl; 130 | isInner.resize(size); 131 | // if(x.size() != isInner.size()) cout << "inner wrong size" << endl; 132 | 133 | // #pragma omp parallel for 134 | for(uint32_t i = 0; i < size; ++i) 135 | { 136 | if(coord(0,i)<3 || coord(0,i)>=(width-3) || coord(1,i)<3 || coord(1,i)>=(height-3)) 137 | isInner[i] = 0; 138 | else isInner[i] = 1; 139 | } 140 | } 141 | 142 | EventBundle::~EventBundle(){ 143 | 144 | } 145 | 146 | 147 | /** 148 | * \brief append eventData to event bundle, resize bundle. 149 | */ 150 | void EventBundle::Append(std::vector& vec_eventData) 151 | { 152 | if(size == 0) 153 | { 154 | // cout << "first appending events, "<< vec_eventData.size() << endl; 155 | first_tstamp = vec_eventData.front().ts; 156 | // abs_tstamp = eventData.time_stamp; 157 | } 158 | else 159 | { 160 | cout << "appending events" << endl; 161 | } 162 | 163 | last_tstamp = vec_eventData.back().ts; 164 | 165 | size_t old_size = size; 166 | size += vec_eventData.size(); 167 | 168 | coord.conservativeResize(2,size); 169 | coord_3d.conservativeResize(3,size); 170 | polar.conservativeResize(size); 171 | 172 | time_delta.conservativeResize(size); 173 | time_delta_rev.conservativeResize(size); // not used 174 | 175 | int counter = 0; 176 | for(const auto& i: vec_eventData) 177 | { 178 | // TODO sampler 179 | 180 | // x.push_back(i.x); 181 | // y.push_back(i.y); 182 | polar(old_size+counter) = i.polarity==0; 183 | 184 | coord(0, old_size+counter) = i.x; 185 | coord(1, old_size+counter) = i.y; 186 | coord_3d(2,old_size+counter) = 1; 187 | time_delta(old_size+counter) = (i.ts - first_tstamp).toSec(); 188 | counter++; 189 | } 190 | 191 | // deep copy 192 | // coord_3d.topRightCorner(2,size-old_size) = coord.topRightCorner(2,size-old_size); 193 | 194 | // cout << "coord example" << endl; 195 | // for(int i=0; i< 5; i++) 196 | // cout << eventData.event[i].x << "," << eventData.event[i].y << endl; 197 | // cout << coord.topLeftCorner(2,5) << endl; 198 | // cout << coord_3d.topLeftCorner(3,5) << endl; 199 | } 200 | 201 | 202 | /** 203 | * \brief project camera to unisophere 204 | * \param K, camera proj matrix. from coor -> coor_3d 205 | */ 206 | void EventBundle::InverseProjection(Eigen::Matrix3d& K) 207 | { 208 | // eigen array pixel-wise operates 209 | 210 | // cout << "inverse project \n " << endl; 211 | // cout << "coord size " << coord.size() << "," << coord_3d.size() << endl; 212 | 213 | if(coord_3d.cols() != size) 214 | { 215 | coord_3d.resize(3, size); 216 | cout << "resizing coord_3d" << endl; 217 | } 218 | 219 | coord_3d.row(0) = (coord.row(0).array()-K(0,2)) / K(0,0); 220 | coord_3d.row(1) = (coord.row(1).array()-K(1,2)) / K(1,1); 221 | 222 | coord_3d.row(2) = Eigen::MatrixXd::Ones(1, size); 223 | 224 | // cout << coord_3d.topLeftCorner(3,5) << endl; 225 | coord_3d.colwise().normalize(); 226 | 227 | // cout << coord_3d.topLeftCorner(3,5) << endl; 228 | 229 | // cout << coord_3d.bottomRightCorner(3,5) << endl; 230 | 231 | } 232 | 233 | 234 | /** 235 | * \brief project camera to unisophere 236 | * \param K, camera proj matrix. from coor_3d -> coor 237 | */ 238 | void EventBundle::Projection(Eigen::Matrix3d& K) 239 | { 240 | // eigen array pixel-wise operates 241 | if(coord.cols() != size) 242 | { 243 | coord.resize(3, size); 244 | cout << "---------wronging----------" << endl; 245 | cout << "resizing coord 2d" << endl; 246 | } 247 | 248 | coord.row(0) = coord_3d.row(0).array() / coord_3d.row(2).array() * K(0,0) + K(0,2); 249 | coord.row(1) = coord_3d.row(1).array() / coord_3d.row(2).array() * K(1,1) + K(1,2); 250 | 251 | // TODO add gaussian 252 | // coord = coord.array().round(); // pixel wise 253 | 254 | 255 | // cout << " projecting sucess " << endl; 256 | } 257 | 258 | 259 | /** 260 | * \brief copy the size paramter of another eventbundle. 261 | */ 262 | void EventBundle::CopySize(const EventBundle& ref) 263 | { 264 | // time_delta = ref.time_delta; 265 | 266 | size = ref.size; 267 | 268 | coord.resize(2,size); 269 | coord_3d.resize(3,size); 270 | isInner.resize(size); 271 | 272 | 273 | } -------------------------------------------------------------------------------- /src/rotation_estimator/src/numerics.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "numerics.hpp" 3 | 4 | #include 5 | using namespace std; 6 | /** 7 | * \brief convert x[1x3] to asym matrix [3x3]. 8 | * \param[in] x . 9 | */ 10 | Eigen::Matrix3d hat(const Eigen::Vector3d &x) 11 | { 12 | Eigen::Matrix3d x_hat; 13 | x_hat << 0, -x(2), x(1), 14 | x(2), 0, -x(0), 15 | -x(1), x(0), 0; 16 | return x_hat; 17 | } 18 | 19 | 20 | /** 21 | * \brief from asy matrix [3x3] to vector [1x3]. 22 | */ 23 | Eigen::Vector3d unhat(const Eigen::Matrix3d &x_hat) 24 | { 25 | Eigen::Vector3d x; 26 | x << x_hat(2,1), x_hat(0,2), x_hat(1,0); 27 | return x; 28 | } 29 | 30 | /** 31 | * \brief from 3 AngleAxis theta to rotation matrix. 32 | * \param rpy rotation angle in rad format. . 33 | */ 34 | Eigen::Matrix3d SO3(const Eigen::Vector3d &x) 35 | { 36 | // cout << " hat(x) \n" << hat(x).transpose() << endl; 37 | // cout << " exp " << hat(x).exp().transpose() << endl; 38 | // cout << " matrix \n " << Eigen::Matrix3d(hat(x).exp()) << endl; 39 | 40 | return Eigen::Matrix3d(hat(x).exp()); 41 | } 42 | 43 | /** 44 | * \brief Constructor. 45 | * \param x_hat. 46 | */ 47 | Eigen::Vector3d InvSO3(const Eigen::Matrix3d &R) 48 | { 49 | 50 | Eigen::Matrix3d temp = R.log(); 51 | 52 | // cout << " R.log() \n" << R.log() << endl; 53 | // cout << " unhat(R.log()) \n" << unhat(R.log()) << endl; 54 | // cout << " unhat(()) \n" << unhat((R.log())) << endl; 55 | // cout << "R.log xyz" << temp(2,1)<< "," << temp(0,2)<<"," << temp(1,0) << endl; 56 | // return unhat(R.log()); 57 | return unhat(temp); 58 | 59 | } 60 | 61 | /** 62 | * \brief from 3 theta to rotation matrix. 63 | * \param rpy rotation angle in rad format. . 64 | */ 65 | Eigen::Vector3d SO3add(const Eigen::Vector3d x1, const Eigen::Vector3d x2, bool circle) 66 | { 67 | if (circle && (x1 + x2).norm() > M_PI) 68 | { 69 | return x1 + x2; 70 | } 71 | else 72 | { 73 | // cout << " SO3(x1) \n" << SO3(x1) << endl; 74 | // cout << " SO3(x1) * 2\n " << SO3(x1) * SO3(x2) << endl; 75 | // cout << " return \n " << InvSO3(SO3(x1) * SO3(x2)) << endl; 76 | return InvSO3(SO3(x1) * SO3(x2)); 77 | } 78 | } 79 | 80 | 81 | /** 82 | * \brief from quaternion to euler anglers， return rpy(xyz) theta. 83 | */ 84 | Eigen::Vector3d toEulerAngles(Eigen::Quaterniond q){ 85 | MyEulerAngles angles; 86 | 87 | // roll (x-axis rotation) 88 | double sinr_cosp = 2 * (q.w() * q.x() + q.y() * q.z()); 89 | double cosr_cosp = 1 - 2 * (q.x() * q.x() + q.y() * q.y()); 90 | angles.roll = std::atan2(sinr_cosp, cosr_cosp); 91 | 92 | // pitch (y-axis rotation) 93 | double sinp = 2 * (q.w() * q.y() - q.z() * q.x()); 94 | if (std::abs(sinp) >= 1) 95 | angles.pitch = std::copysign(M_PI / 2, sinp); // use 90 degrees if out of range 96 | else 97 | angles.pitch = std::asin(sinp); 98 | 99 | // yaw (z-axis rotation) 100 | double siny_cosp = 2 * (q.w() * q.z() + q.x() * q.y()); 101 | double cosy_cosp = 1 - 2 * (q.y() * q.y() + q.z() * q.z()); 102 | angles.yaw = std::atan2(siny_cosp, cosy_cosp); 103 | 104 | Eigen::Vector3d v3d(angles.roll, angles.pitch, angles.yaw); 105 | return v3d; 106 | } 107 | 108 | 109 | Eigen::Quaterniond ToQuaternion(double yaw, double pitch, double roll) // yaw (Z), pitch (Y), roll (X) 110 | { 111 | // Abbreviations for the various angular functions 112 | double cy = cos(yaw * 0.5); 113 | double sy = sin(yaw * 0.5); 114 | double cp = cos(pitch * 0.5); 115 | double sp = sin(pitch * 0.5); 116 | double cr = cos(roll * 0.5); 117 | double sr = sin(roll * 0.5); 118 | 119 | double w = cr * cp * cy + sr * sp * sy; 120 | double x = sr * cp * cy - cr * sp * sy; 121 | double y = cr * sp * cy + sr * cp * sy; 122 | double z = cr * cp * sy - sr * sp * cy; 123 | 124 | 125 | Eigen::Quaterniond q(w,x,y,z); 126 | // std::cout <<"Quaterniond: "<< q.coeffs().transpose() << ", norm: "<< q.coeffs().norm() << std::endl; 127 | return q; 128 | } 129 | 130 | 131 | /** 132 | * \brief . 133 | * \param type is opencv type CV_float or 134 | */ 135 | double getVar(cv::Mat& image, int& count_nozero, int type) 136 | { 137 | // std::cout << image.channels() << std::endl; 138 | 139 | // cv::Mat grayimage; 140 | // // cv::normalize(image, grayimage,0,255,cv::NORM_MINMAX, CV_8UC3); 141 | // image.convertTo(grayimage,CV_8UC3); 142 | // cv::cvtColor(grayimage, grayimage, cv::COLOR_BGR2GRAY, 1); 143 | // cv::imshow("image", image) ; 144 | // cv::imshow("normalize", grayimage) ; 145 | // cv::imshow("grayimage", grayimage) ; 146 | // cv::waitKey(0); 147 | 148 | assert(image.channels() == 1); 149 | // assert(image.type() == CV_16UC1); 150 | 151 | double mean = 0; 152 | count_nozero = 1; 153 | for(int x = 0; x < image.cols; x++) 154 | for(int y = 0; y < image.rows; y++) 155 | { 156 | double value = 1000; 157 | if(type == CV_16U) 158 | value = image.at(y,x); 159 | if(type == CV_32F) 160 | value = image.at(y,x); 161 | 162 | if(value>0) 163 | { 164 | mean += value; 165 | count_nozero++; 166 | } 167 | } 168 | 169 | mean /= count_nozero; 170 | 171 | double var = 0; 172 | for(int x = 0; x < image.cols; x++) 173 | for(int y = 0; y < image.rows; y++) 174 | { 175 | 176 | double value = 1000; 177 | if(type == CV_16U) 178 | value = image.at(y,x); 179 | if(type == CV_32F) 180 | value = image.at(y,x); 181 | if(value>0) 182 | { 183 | var += std::pow(value-mean,2); 184 | } 185 | } 186 | var /= count_nozero; 187 | return var; 188 | } -------------------------------------------------------------------------------- /src/rotation_estimator/src/rotation_estimator_node.cpp: -------------------------------------------------------------------------------- 1 | // std 2 | #include 3 | #include 4 | 5 | // third party 6 | 7 | // ros 8 | #include 9 | // #include "callbacks.hpp" 10 | #include "system.hpp" 11 | #include "event_reader.hpp" 12 | // #include 13 | 14 | 15 | using namespace std; 16 | 17 | class EventGrabber 18 | { 19 | public: 20 | EventGrabber(System* sys) : system(sys) {} 21 | 22 | void GrabEvent(const dvs_msgs::EventArrayConstPtr& msg); 23 | 24 | System* system; 25 | }; 26 | 27 | void EventGrabber::GrabEvent(const dvs_msgs::EventArrayConstPtr& msg) 28 | { 29 | system->pushEventData(msg->events); 30 | } 31 | 32 | // rosbag play -r 0.1 ~/Documents/rosbag/Mono-rosbag/slider_depth.bag 33 | 34 | int main(int argc, char** argv) 35 | { 36 | 37 | ros::init(argc, argv, "rotation_estimator"); 38 | ros::start(); 39 | 40 | ros::NodeHandle nh("~"); 41 | 42 | string yaml; // system configration 43 | nh.param("yaml", yaml, ""); 44 | 45 | System* sys = new System(yaml); 46 | 47 | if(!sys->file_opened()) 48 | { 49 | cout << "failed opening file " << endl; 50 | // return 0; 51 | } 52 | 53 | ros::Time t1, t2; 54 | t1 = ros::Time::now(); 55 | /* Event ROS version */ 56 | // EventGrabber eventGrabber(sys); 57 | // ros::Subscriber event_sub = nh.subscribe("/dvs/events", 10, &EventGrabber::GrabEvent, &eventGrabber); 58 | // ros::spin(); 59 | 60 | /** Event TXT version */ 61 | double total_event_acquire_time = 0; 62 | Event_reader event_reader(yaml); 63 | while (true) 64 | { 65 | // read data 66 | // ros::Time t1, t2; 67 | ros::Time t3 = ros::Time::now(); 68 | dvs_msgs::EventArrayPtr msg_ptr = dvs_msgs::EventArrayPtr(new dvs_msgs::EventArray()); 69 | event_reader.acquire(msg_ptr); 70 | // t2 = ros::Time::now(); 71 | total_event_acquire_time += (ros::Time::now() - t3).toSec(); 72 | // cout << "event_reader.acquire time " << (t2-t1).toSec() << endl; // 0.50643 73 | 74 | 75 | if(msg_ptr==nullptr || msg_ptr->events.empty() ) 76 | { 77 | cout << "wrong reading events, msgptr==null" << int(msg_ptr==nullptr) << "empty events " << int(msg_ptr->events.empty()) << endl; 78 | break; 79 | } 80 | 81 | 82 | // t1 = ros::Time::now(); 83 | sys->pushEventData(msg_ptr->events); 84 | // t2 = ros::Time::now(); 85 | // cout << "sys pushEventData" << (t2-t1).toSec() << endl; // 0.00691187 s 86 | 87 | // cout << "success reveive" << endl; 88 | // break; 89 | } 90 | 91 | 92 | double total_program_runtime = (ros::Time::now() - t1).toSec(); 93 | cout << " total program time "<< total_program_runtime << endl; 94 | cout << " total total_event_acquire_time "<< total_event_acquire_time << endl; 95 | // cout << " total read events time "<< sys->total_readevents_time << endl; 96 | cout << " total create event bundle time "<< sys->total_eventbundle_time << endl; 97 | cout << " total evaluate time "<< sys->total_evaluate_time << endl; 98 | cout << " total warpevents time "<< sys->total_warpevents_time << endl; 99 | cout << " total timesurface time "<< sys->total_timesurface_time << endl; 100 | cout << " total ceres time "<< sys->total_ceres_time << endl; 101 | cout << " total undistort time "<< sys->total_undistort_time << endl; 102 | cout << " total visual time "<< sys->total_visual_time << endl; 103 | 104 | cout << "shutdown rotation estimator" << endl; 105 | return 0; 106 | } 107 | 108 | -------------------------------------------------------------------------------- /src/rotation_estimator/src/system_estimate.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "system.hpp" 3 | #include "numerics.hpp" 4 | #include 5 | 6 | using namespace std; 7 | 8 | 9 | /** 10 | * \brief given angular veloity(t1->t2), warp local event bundle become shaper 11 | */ 12 | cv::Mat System::getWarpedEventImage(const Eigen::Vector3d & cur_ang_vel, EventBundle& event_out, const PlotOption& option, bool ref_t1, float timerange) 13 | { 14 | // cout << "get warpped event image " << endl; 15 | // cout << "eventUndistorted.coord.cols() " << event_undis_Bundle.coord.cols() << endl; 16 | /* warp local events become sharper */ 17 | ros::Time t1, t2, t3; 18 | 19 | event_out.CopySize(event_undis_Bundle); 20 | // t1 = ros::Time::now(); 21 | 22 | getWarpedEventPoints(event_undis_Bundle, event_out, cur_ang_vel, ref_t1, timerange); 23 | // t2= ros::Time::now(); 24 | 25 | event_out.Projection(camera.eg_cameraMatrix); 26 | 27 | event_out.DiscriminateInner(camera.width, camera.height); 28 | // t3 = ros::Time::now(); 29 | 30 | 31 | // cout << " getWarpedEventPoints time " << (t2-t1).toSec() << endl; 32 | // cout << " DiscriminateInner time " << (t3-t2).toSec() << endl; 33 | return getImageFromBundle(event_out, option, timerange); 34 | } 35 | 36 | /** 37 | * \brief given angular veloity, warp local event bundle(t2) to the reference time(t1) 38 | using kim RAL21, eqation(11), since the ratation angle is ratively small 39 | * \param cur_ang_vel angleAxis/delta_t from t1->t2, if multiply minus time, it becomes t2->t1. (AngleAxis inverse only add minus) 40 | * \param cur_ang_pos from t2->t0. default set (0,0,0) default, so the output is not rotated. 41 | * \param delta_time all events warp this time, if delta_time<0, warp to t1. 42 | */ 43 | void System::getWarpedEventPoints(const EventBundle& eventIn, EventBundle& eventOut, 44 | const Eigen::Vector3d& cur_ang_vel, bool ref_t1, float timerange) 45 | { 46 | // cout << "projecting " << endl; 47 | // the theta of rotation axis 48 | float ang_vel_norm = cur_ang_vel.norm(); 49 | 50 | eventOut.CopySize(eventIn); 51 | if(ang_vel_norm < 1e-8) 52 | { 53 | // cout << " small angle vec " << ang_vel_norm/3.14 * 180 << " degree /s" << endl; 54 | eventOut.coord_3d = eventIn.coord_3d ; 55 | } 56 | else if (timerange > 0.9) 57 | { 58 | // cout << "using whole warp " < axis = cur_ang_vel.normalized(); 101 | // Eigen::VectorXd angle_vec = vec_delta_time * ang_vel_norm ; 102 | 103 | // Eigen::VectorXd cos_angle_vec = angle_vec.array().cos(); 104 | // Eigen::VectorXd sin_angle_vec = angle_vec.array().sin(); 105 | 106 | // Eigen::Matrix3Xd first = eventIn.coord_3d.array().rowwise() * cos_angle_vec.transpose().array(); 107 | // Eigen::Matrix3Xd second = (-eventIn.coord_3d.array().colwise().cross(axis)).array().rowwise() * sin_angle_vec.transpose().array(); 108 | // Eigen::VectorXd third1 = axis.transpose() * eventIn.coord_3d; 109 | // Eigen::VectorXd third2 = third1.array() * (1-cos_angle_vec.array()).array();; 110 | // Eigen::Matrix3Xd third = axis * third2.transpose(); 111 | // eventOut.coord_3d = first + second + third; 112 | 113 | // cout << "last \n " << eventOut.coord_3d.bottomRightCorner(3,5) << endl; 114 | 115 | } 116 | else // warp events within given timerange 117 | { 118 | int n_num = int(eventIn.size * timerange); 119 | Eigen::VectorXd vec_delta_time = eventBundle.time_delta.topRows(n_num); // positive 120 | if(ref_t1) vec_delta_time = eventBundle.time_delta.topLeftCorner(1,n_num).array() - eventBundle.time_delta(eventBundle.size-1); // negative 121 | 122 | 123 | // cout << "warp num " << n_num << ", size " << eventOut.coord_3d.topLeftCorner(3,n_num).size() << endl; 124 | 125 | // taylor 126 | Eigen::Matrix temp_eventIn = eventIn.coord_3d.topLeftCorner(3,n_num); 127 | Eigen::Matrix3Xd ang_vel_hat_mul_x, ang_vel_hat_sqr_mul_x; /** equation 11 of kim */ 128 | ang_vel_hat_mul_x.resize(3, n_num); // row, col 129 | ang_vel_hat_sqr_mul_x.resize(3, n_num); 130 | ang_vel_hat_mul_x.row(0) = -cur_ang_vel(2)*temp_eventIn.row(1) + cur_ang_vel(1)*temp_eventIn.row(2); 131 | ang_vel_hat_mul_x.row(1) = cur_ang_vel(2)*temp_eventIn.row(0) - cur_ang_vel(0)*temp_eventIn.row(2); 132 | ang_vel_hat_mul_x.row(2) = -cur_ang_vel(1)*temp_eventIn.row(0) + cur_ang_vel(0)*temp_eventIn.row(1); 133 | 134 | 135 | // cout << "vec_delta_time " << vec_delta_time.rows() << endl; 136 | // cout << "temp_eventIn " << temp_eventIn.cols() << endl; 137 | // cout << "ang_vel_hat_mul_x " << ang_vel_hat_mul_x.cols() << endl; 138 | 139 | // first order version 140 | { 141 | eventOut.coord_3d.topLeftCorner(3,n_num) = temp_eventIn 142 | + Eigen::MatrixXd( 143 | ang_vel_hat_mul_x.array().rowwise() 144 | * (vec_delta_time.transpose().array())); 145 | 146 | eventOut.coord_3d.bottomRightCorner(1,eventIn.size-n_num).array() = 1; // set z axis to 1 147 | // cout << "eventOut \n " << eventOut.coord_3d.block(0,1000,3,5) << endl; 148 | 149 | 150 | } 151 | 152 | 153 | // kim second order version; 154 | // { 155 | // ang_vel_hat_sqr_mul_x.row(0) = -cur_ang_vel(2)*ang_vel_hat_mul_x.row(1) + cur_ang_vel(1)*ang_vel_hat_mul_x.row(2); 156 | // ang_vel_hat_sqr_mul_x.row(1) = cur_ang_vel(2)*ang_vel_hat_mul_x.row(0) - cur_ang_vel(0)*ang_vel_hat_mul_x.row(2); 157 | // ang_vel_hat_sqr_mul_x.row(2) = -cur_ang_vel(1)*ang_vel_hat_mul_x.row(0) + cur_ang_vel(0)*ang_vel_hat_mul_x.row(1); 158 | // eventOut.coord_3d = eventIn.coord_3d 159 | // + Eigen::MatrixXd( 160 | // ang_vel_hat_mul_x.array().rowwise() 161 | // * (vec_delta_time.transpose().array()) 162 | // + ang_vel_hat_sqr_mul_x.array().rowwise() 163 | // * (0.5f * vec_delta_time.transpose().array().square()) ); 164 | // } 165 | 166 | } 167 | 168 | // cout << "sucess getWarpedEventPoints" << endl; 169 | } 170 | 171 | 172 | /** 173 | * \brief given start and end ratio (0~1), and samples_count, return noise free sampled events. 174 | * \param vec_sampled_idx 175 | * \param samples_count 176 | */ 177 | void System::getSampledVec(vector& vec_sampled_idx, int samples_count, double sample_start, double sample_end) 178 | { 179 | cv::RNG rng(int(ros::Time::now().nsec)); 180 | // get samples, filtered out noise 181 | for(int i=0; i< samples_count;) 182 | { 183 | int sample_idx = rng.uniform(int(event_undis_Bundle.size*sample_start), int(event_undis_Bundle.size*sample_end)); 184 | 185 | // check valid 8 neighbor hood existed 186 | int sampled_x = event_undis_Bundle.coord.col(sample_idx)[0]; 187 | int sampled_y = event_undis_Bundle.coord.col(sample_idx)[1]; 188 | if(sampled_x >= 239 || sampled_x < 1 || sampled_y >= 179 || sampled_y < 1 ) 189 | { 190 | // cout << "x, y" << sampled_x << "," << sampled_y << endl; 191 | continue; 192 | } 193 | 194 | int count = 0; 195 | for(int j=-1; j<2; j++) 196 | for(int k=-1; k<2; k++) 197 | { 198 | count += ( curr_undis_event_image.at(sampled_y+j,sampled_x+k)[0] + 199 | curr_undis_event_image.at(sampled_y+j,sampled_x+k)[1] + 200 | curr_undis_event_image.at(sampled_y+j,sampled_x+k)[2] ) > 0; 201 | // count += ( curr_undis_event_image.at(sampled_y+j,sampled_x+k) != default_value) 202 | } 203 | 204 | // valid denoised 205 | if(count > yaml_denoise_num) // TODO 206 | { 207 | vec_sampled_idx.push_back(sample_idx); 208 | i++; 209 | } 210 | } 211 | 212 | } 213 | 214 | 215 | 216 | /** 217 | * \brief given event_warpped_Bundle and rotation matrix, 218 | * \param vec_Bundle_Maps, 219 | * \param curr_map_image, output 220 | */ 221 | void System::getMapImage() 222 | { 223 | cout << "mapping global" << endl; 224 | 225 | /* warp current event to t0 using gt */ 226 | Eigen::Matrix3d R_b2f = get_global_rotation_b2f(0,vec_gt_poseData.size()-1); 227 | Eigen::AngleAxisd angAxis_b2f(R_b2f); 228 | 229 | /* warp current event to t0 using estimated data */ 230 | 231 | 232 | event_Map_Bundle.CopySize(event_warpped_Bundle); 233 | getWarpedEventPoints(event_warpped_Bundle,event_Map_Bundle, angAxis_b2f.axis()*angAxis_b2f.angle()); 234 | event_Map_Bundle.Projection(camera.eg_MapMatrix); 235 | event_Map_Bundle.DiscriminateInner(camera.width_map, camera.height_map); 236 | event_Map_Bundle.angular_position = angAxis_b2f.axis() * angAxis_b2f.angle(); 237 | vec_Bundle_Maps.push_back(event_Map_Bundle); 238 | 239 | // cout << "test " << vec_Bundle_Maps[0].coord.topLeftCorner(2,5) << endl; 240 | 241 | 242 | 243 | /* get map from all events to t0 */ 244 | cv::Mat temp_img; 245 | curr_map_image.setTo(0); 246 | 247 | int start_idx = (vec_Bundle_Maps.size()-3) > 0 ? vec_Bundle_Maps.size()-3 : 0; 248 | for(size_t i=start_idx; i