├── LICENSE ├── README.md ├── config ├── euroc │ ├── cam0_fisheye.yaml │ ├── cam1_fisheye.yaml │ ├── euroc_stereo.yaml │ └── fisheye_mask.jpg ├── fpv │ ├── cam0_fisheye.yaml │ ├── cam1_fisheye.yaml │ ├── fisheye_mask.jpg │ └── fpv_forward.yaml ├── fpv_forward_45 │ ├── cam0_fisheye.yaml │ ├── cam1_fisheye.yaml │ ├── fisheye_mask.jpg │ └── fpv_forward_45_degree.yaml ├── fpv_out_forward │ ├── cam0_fisheye.yaml │ ├── cam1_fisheye.yaml │ ├── fisheye_mask.jpg │ ├── fisheye_mask3.jpg │ └── fpv_outdoor_forward.yaml ├── tum_vi │ ├── TUM_512.yaml │ ├── cam0_fisheye.yaml │ ├── cam1_fisheye.yaml │ └── fisheye_mask.jpg └── vins_rviz_config.rviz ├── feature_tracker ├── CMakeLists.txt ├── cmake │ └── FindEigen.cmake ├── package.xml └── src │ ├── feature_tracker.cpp │ ├── feature_tracker.h │ ├── feature_tracker_node.cpp │ ├── parameters.cpp │ ├── parameters.h │ └── tic_toc.h ├── launch ├── debug_feature_track.launch ├── euroc_stereo.launch ├── feature_tracker.launch ├── feature_tracker_45.launch ├── fpv.launch ├── fpv_forward_45.launch ├── fpv_outdoor.launch ├── tum_vio.launch └── vins_rviz.launch └── vins_estimator ├── CMakeLists.txt ├── cmake └── FindEigen.cmake ├── package.xml └── src ├── debug_optimize.cpp ├── estimator.cpp ├── estimator.h ├── estimator_node.cpp ├── estimator_node_ss.cpp ├── estimator_node_stereo.cpp ├── factor ├── imu_factor.h ├── integration_base.h ├── marginalization_factor.cpp ├── marginalization_factor.h ├── pose_local_parameterization.cpp ├── pose_local_parameterization.h ├── projectionOneFrameTwoCamFactor.cpp ├── projectionOneFrameTwoCamFactor.h ├── projectionTwoFrameOneCamFactor.cpp ├── projectionTwoFrameOneCamFactor.h ├── projectionTwoFrameTwoCamFactor.cpp ├── projectionTwoFrameTwoCamFactor.h ├── projection_factor.cpp ├── projection_factor.h ├── projection_td_factor.cpp ├── projection_td_factor.h ├── sampson_factor.cpp ├── sampson_factor.h └── sampson_jacobian.cpp ├── feature_manager.cpp ├── feature_manager.h ├── initial ├── initial_aligment.cpp ├── initial_alignment.h ├── initial_ex_rotation.cpp ├── initial_ex_rotation.h ├── initial_sfm.cpp ├── initial_sfm.h ├── solve_5pts.cpp ├── solve_5pts.h ├── solve_opt.cpp ├── solve_opt.h ├── translate_factor.cpp └── translate_factor.h ├── parameters.cpp ├── parameters.h └── utility ├── CameraPoseVisualization.cpp ├── CameraPoseVisualization.h ├── tic_toc.h ├── utility.cpp ├── utility.h ├── visualization.cpp └── visualization.h /README.md: -------------------------------------------------------------------------------- 1 | # VINS-Stereo 2 | A stereo camera based VIO method, which extends and builds on VINS-Mono https://github.com/HKUST-Aerial-Robotics/VINS-Mono 3 | 4 | # Compile 5 | Needs to build VINS-Mono first, since it depends on camera_model and pose_graph modules in VINS-Mono. 6 | 7 | # Run 8 | 9 | roslaunch fpv.launch (euroc_stereo.launch or tum_vio.launch) 10 | rosbag play [data_sequence].bag 11 | 12 | data_sequence can be downloaded at 13 | https://fpv.ifi.uzh.ch/?page_id=50 for FPV dataset 14 | https://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets for EuRoc MAV dataset 15 | https://vision.in.tum.de/data/datasets/visual-inertial-dataset for TUM VI dataset 16 | 17 | A demo using FPV data sequences is shown below: 18 | 19 | UTH-FPV Dataset 21 | 22 | 23 | More details can be found in the report https://fpv.ifi.uzh.ch/wp-content/uploads/sourcenova/uni-comp/2019-2020-uzh-fpv-benchmark/submissions/15/details.pdf 24 | 25 | # TODO: 26 | Need to reduce the feature tracking drift to obtain more accurate feature matches between the stereo image pairs, as suggested in "A Comparative Analysis of Tightly-coupled Monocular, Binocular, and Stereo VINS". 27 | One promising solution is to replace FAST+KLT frontend with LIBVISO, an accurate dense stereo tracking method "StereoScan: Dense 3D Reconstruction in Real-time". 28 | ... 29 | -------------------------------------------------------------------------------- /config/euroc/cam0_fisheye.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | model_type: MEI 4 | camera_name: camera 5 | image_width: 752 6 | image_height: 480 7 | mirror_parameters: 8 | xi: 3.6313355285286337e+00 9 | distortion_parameters: 10 | k1: 1.1757726639872075e+00 11 | k2: 1.5491281051140213e+01 12 | p1: -8.1237172954550494e-04 13 | p2: 6.7297684030310243e-04 14 | projection_parameters: 15 | gamma1: 2.1387619122017772e+03 16 | gamma2: 2.1315886210259278e+03 17 | u0: 3.6119856633263799e+02 18 | v0: 2.4827644773395667e+02 19 | 20 | #### for pose graph node 21 | image_topic: "/cam0/image_raw" 22 | output_path: "/home/davidz/work/result/euroc" 23 | #loop closure parameters 24 | loop_closure: 1 #1 # start loop closure 25 | load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path' 26 | fast_relocalization: 1 # useful in real-time and large project 27 | pose_graph_save_path: "/home/davidz/work/result/pose_graph/" # save and load path 28 | 29 | #visualization parameters 30 | save_image: 1 #1 # save image in pose graph for visualization prupose; you can close this function by setting 0 31 | visualize_imu_forward: 0 # output imu forward propogation to achieve low latency and high frequence results 32 | visualize_camera_size: 0.4 # size of camera marker in RVIZ 33 | 34 | -------------------------------------------------------------------------------- /config/euroc/cam1_fisheye.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | model_type: MEI 4 | camera_name: camera 5 | image_width: 752 6 | image_height: 480 7 | mirror_parameters: 8 | xi: 8.1261505894146113e-01 9 | distortion_parameters: 10 | k1: -3.4247886078397966e-01 11 | k2: 1.7108792755973357e-01 12 | p1: -6.6653144364906686e-04 13 | p2: 7.2311255846767091e-04 14 | projection_parameters: 15 | gamma1: 8.3406249735437791e+02 16 | gamma2: 8.3140606765916948e+02 17 | u0: 3.7432007355249738e+02 18 | v0: 2.5422391621480082e+02 -------------------------------------------------------------------------------- /config/euroc/euroc_stereo.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #common parameters 4 | imu_topic: "/imu0" 5 | image0_topic: "/cam0/image_raw" 6 | image1_topic: "/cam1/image_raw" 7 | 8 | output_path: "/home/davidz/work/result/euroc" 9 | 10 | #camera calibration 11 | cam0_calib: "cam0_fisheye.yaml" 12 | cam1_calib: "cam1_fisheye.yaml" 13 | # image_width: 640 14 | # image_height: 480 15 | 16 | ### for debug 17 | #camera calibration 18 | image_topic: "/cam0/image_raw" 19 | 20 | model_type: MEI 21 | camera_name: camera 22 | image_width: 752 23 | image_height: 480 24 | mirror_parameters: 25 | xi: 3.6313355285286337e+00 26 | distortion_parameters: 27 | k1: 1.1757726639872075e+00 28 | k2: 1.5491281051140213e+01 29 | p1: -8.1237172954550494e-04 30 | p2: 6.7297684030310243e-04 31 | projection_parameters: 32 | gamma1: 2.1387619122017772e+03 33 | gamma2: 2.1315886210259278e+03 34 | u0: 3.6119856633263799e+02 35 | v0: 2.4827644773395667e+02 36 | 37 | # Extrinsic parameter between IMU and Camera. 38 | estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it. 39 | # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess. 40 | # 2 Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning. 41 | #If you choose 0 or 1, you should write down the following matrix. 42 | #Rotation from camera frame to imu frame, imu^R_cam 43 | extrinsicRotation: !!opencv-matrix 44 | rows: 3 45 | cols: 3 46 | dt: d 47 | data: [ 0.0148655429818, -0.999880929698, 0.00414029679422, 48 | 0.999557249008, 0.0149672133247, 0.025715529948, 49 | -0.0257744366974, 0.00375618835797, 0.999660727178] 50 | 51 | extrinsicTranslation: !!opencv-matrix 52 | rows: 3 53 | cols: 1 54 | dt: d 55 | data: [ -0.0216401454975, -0.064676986768, 0.00981073058949] 56 | 57 | Rrl: !!opencv-matrix 58 | rows: 3 59 | cols: 3 60 | dt: d 61 | data: [ 0.999997, 0.00231714, 0.000343393, 62 | -0.00231207, 0.999898, 0.0140907, 63 | -0.000376008, -0.0140898, 0.999901 ] 64 | 65 | Trl: !!opencv-matrix 66 | rows: 3 67 | cols: 1 68 | dt: d 69 | data: [-0.110074, 0.000399122, -0.000853703] 70 | #data: [0.110074, -0.000156612, 0.000889383] 71 | 72 | body_T_cam1: !!opencv-matrix # Timu2c_2, Tc1_2_c2 is a virtual transformation [I,t] t = [0.1, 0, 0], note "mbf" in estimator_dpt.cpp 73 | rows: 4 74 | cols: 4 75 | dt: d 76 | data: [0.0125552670891, -0.999755099723, 0.0182237714554, -0.0198435579556, 77 | 0.999598781151, 0.0130119051815, 0.0251588363115, 0.0453689425024, 78 | -0.0253898008918, 0.0179005838253, 0.999517347078, 0.00786212447038, 79 | 0., 0. , 0. , 1.] 80 | 81 | #feature traker paprameters 82 | max_cnt: 150 # 300 #250 # max feature number in feature tracking 83 | min_dist: 25 # min distance between two features 84 | freq: 10 #10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image 85 | F_threshold: 1.0 # ransac threshold (pixel) 86 | show_track: 1 # publish tracking image as topic 87 | equalize: 0 # if image is too dark or light, trun on equalize to find enough features 88 | fisheye: 1 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points 89 | flow_back: 1 # if need double check by reverse optical flow 90 | 91 | #optimization parameters 92 | max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time 93 | max_num_iterations: 8 # max solver itrations, to guarantee real time 94 | keyframe_parallax: 10.0 # keyframe selection threshold (pixel) 95 | 96 | #imu parameters The more accurate parameters you provide, the better performance 97 | # seems 0.12 & 0.02 is a better choice compared to 0.1 & 0.05 98 | acc_n: 0.1 # 0.1 # 0.12 # 0.12 # 0.1 # 0.04 # accelerometer measurement noise standard deviation. #0.2 0.04 99 | gyr_n: 0.01 # 0.05 # 0.02 # 0.01 0.05 #0.004 # gyroscope measurement noise standard deviation. #0.05 0.004 100 | acc_w: 0.001 #0.0004 # accelerometer bias random work noise standard deviation. #0.02 101 | gyr_w: 0.0001 #2.0e-5 # gyroscope bias random work noise standard deviation. #4.0e-5 102 | g_norm: 9.80766 # gravity magnitude 103 | 104 | #unsynchronization parameters 105 | estimate_td: 0 #1 # online estimate time offset between camera and imu 106 | td: 0 #-0.0166845720919 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock) 107 | 108 | #rolling shutter parameters 109 | rolling_shutter: 0 # 0: global shutter camera, 1: rolling shutter camera 110 | rolling_shutter_tr: 0 # unit: s. rolling shutter read out time per frame (from data sheet). 111 | 112 | #loop closure parameters 113 | # loop_closure: 0 #1 # start loop closure 114 | # load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path' 115 | # fast_relocalization: 1 # useful in real-time and large project 116 | # pose_graph_save_path: "/home/tony-ws1/output/pose_graph/" # save and load path 117 | 118 | #visualization parameters 119 | # save_image: 1 #1 # save image in pose graph for visualization prupose; you can close this function by setting 0 120 | # visualize_imu_forward: 0 # output imu forward propogation to achieve low latency and high frequence results 121 | # visualize_camera_size: 0.4 # size of camera marker in RVIZ 122 | -------------------------------------------------------------------------------- /config/euroc/fisheye_mask.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rising-turtle/VINS-Stereo/51ce0442bb5b15bc8cfb85fd3a5cb76e8f2ce835/config/euroc/fisheye_mask.jpg -------------------------------------------------------------------------------- /config/fpv/cam0_fisheye.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | model_type: KANNALA_BRANDT 4 | camera_name: camera 5 | image_width: 640 6 | image_height: 480 7 | projection_parameters: 8 | # k2: 0.0034823894022493434 9 | # k3: 0.0007150348452162257 10 | # k4: -0.0020532361418706202 11 | # k5: 0.00020293673591811182 12 | k2: -0.013721808247486035 13 | k3: 0.020727425669427896 14 | k4: -0.012786476702685545 15 | k5: 0.0025242267320687625 16 | mu: 278.66723066149086 17 | mv: 278.48991409740296 18 | u0: 319.75221200593535 19 | v0: 241.96858910358173 20 | 21 | 22 | #### for pose graph node 23 | image_topic: "/snappy_cam/stereo_l" 24 | output_path: "/home/davidz/work/result/fpv_forward" 25 | #loop closure parameters 26 | loop_closure: 1 #1 # start loop closure 27 | load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path' 28 | fast_relocalization: 1 # useful in real-time and large project 29 | pose_graph_save_path: "/home/davidz/work/result/pose_graph/" # save and load path 30 | 31 | #visualization parameters 32 | save_image: 1 #1 # save image in pose graph for visualization prupose; you can close this function by setting 0 33 | visualize_imu_forward: 0 # output imu forward propogation to achieve low latency and high frequence results 34 | visualize_camera_size: 0.4 # size of camera marker in RVIZ 35 | 36 | -------------------------------------------------------------------------------- /config/fpv/cam1_fisheye.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | model_type: KANNALA_BRANDT 4 | camera_name: camera 5 | image_width: 640 6 | image_height: 480 7 | projection_parameters: 8 | k2: -0.008456929295619607 9 | k3: 0.011407590938612062 10 | k4: -0.006951788325762078 11 | k5: 0.0015368127092821786 12 | mu: 277.61640629770613 13 | mv: 277.63749695723294 14 | u0: 314.8944703346039 15 | v0: 236.04310050462587 16 | -------------------------------------------------------------------------------- /config/fpv/fisheye_mask.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rising-turtle/VINS-Stereo/51ce0442bb5b15bc8cfb85fd3a5cb76e8f2ce835/config/fpv/fisheye_mask.jpg -------------------------------------------------------------------------------- /config/fpv/fpv_forward.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #common parameters 4 | imu_topic: "/snappy_imu" 5 | image0_topic: "/snappy_cam/stereo_l" 6 | image1_topic: "/snappy_cam/stereo_r" 7 | 8 | output_path: "/home/davidz/work/result/fpv_forward" 9 | 10 | #camera calibration 11 | cam0_calib: "cam0_fisheye.yaml" 12 | cam1_calib: "cam1_fisheye.yaml" 13 | # image_width: 640 14 | # image_height: 480 15 | 16 | ### for debug 17 | #camera calibration 18 | image_topic: "/snappy_cam/stereo_l" 19 | 20 | model_type: KANNALA_BRANDT 21 | camera_name: camera 22 | image_width: 640 23 | image_height: 480 24 | projection_parameters: 25 | k2: -0.013721808247486035 26 | k3: 0.020727425669427896 27 | k4: -0.012786476702685545 28 | k5: 0.0025242267320687625 29 | mu: 278.66723066149086 30 | mv: 278.48991409740296 31 | u0: 319.75221200593535 32 | v0: 241.96858910358173 33 | 34 | # Extrinsic parameter between IMU and Camera. 35 | estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it. 36 | # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess. 37 | # 2 Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning. 38 | #If you choose 0 or 1, you should write down the following matrix. 39 | #Rotation from camera frame to imu frame, imu^R_cam 40 | extrinsicRotation: !!opencv-matrix 41 | rows: 3 42 | cols: 3 43 | dt: d 44 | data: [-0.02822879, 0.01440125, 0.99949774, 45 | -0.99960149, -0.00041887, -0.02822568, 46 | 0.00001218, -0.99989621, 0.01440734] 47 | extrinsicTranslation: !!opencv-matrix 48 | rows: 3 49 | cols: 1 50 | dt: d 51 | data: [0.0011, 0.0217, -0.0001 ] 52 | 53 | Rrl: !!opencv-matrix 54 | rows: 3 55 | cols: 3 56 | dt: d 57 | data: [ 0.9998053, 0.01119774, 0.01624713, 58 | -0.01114776, 0.99993286, -0.00316357, 59 | -0.01628147, 0.00298183, 0.999863] 60 | 61 | Trl: !!opencv-matrix 62 | rows: 3 63 | cols: 1 64 | dt: d 65 | data: [-0.07961594, 0.00074435, 0.00044255 ] 66 | 67 | body_T_cam1: !!opencv-matrix # Timu2c_2, Tc1_2_c2 is a virtual transformation [I,t] t = [0.1, 0, 0], note "mbf" in estimator_dpt.cpp 68 | rows: 4 69 | cols: 4 70 | dt: d 71 | data: [-0.01182306, 0.01155299, 0.99986336, -0.00029, 72 | -0.99987014, 0.01081377, -0.01194809, -0.0579069465, 73 | -0.01095033 , -0.99987479, 0.01142364, -0.0001919, 74 | 0, 0, 0, 1.0] 75 | 76 | #feature traker paprameters 77 | max_cnt: 300 # 300 #250 # max feature number in feature tracking 78 | min_dist: 25 # min distance between two features 79 | freq: 10 #10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image 80 | F_threshold: 1.0 # ransac threshold (pixel) 81 | show_track: 1 # publish tracking image as topic 82 | equalize: 0 # if image is too dark or light, trun on equalize to find enough features 83 | fisheye: 1 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points 84 | flow_back: 1 #1 # if need double check by reverse optical flow 85 | 86 | #optimization parameters 87 | max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time 88 | max_num_iterations: 8 # max solver itrations, to guarantee real time 89 | keyframe_parallax: 10.0 # keyframe selection threshold (pixel) 90 | 91 | #imu parameters The more accurate parameters you provide, the better performance 92 | # seems 0.12 & 0.02 is a better choice compared to 0.1 & 0.05 93 | acc_n: 0.12 #0.1 # 0.12 # 0.1 # 0.12 # 0.12 # 0.1 # 0.04 # accelerometer measurement noise standard deviation. #0.2 0.04 94 | gyr_n: 0.02 # 0.02 # 0.05 # 0.02 # 0.01 0.05 #0.004 # gyroscope measurement noise standard deviation. #0.05 0.004 95 | acc_w: 0.002 #0.002 #0.0004 # accelerometer bias random work noise standard deviation. #0.02 96 | gyr_w: 0.00004 #2.0e-5 # gyroscope bias random work noise standard deviation. #4.0e-5 97 | g_norm: 9.80766 # gravity magnitude 98 | 99 | #unsynchronization parameters 100 | estimate_td: 1 # online estimate time offset between camera and imu 101 | td: -0.0166845720919 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock) 102 | 103 | #rolling shutter parameters 104 | rolling_shutter: 0 # 0: global shutter camera, 1: rolling shutter camera 105 | rolling_shutter_tr: 0 # unit: s. rolling shutter read out time per frame (from data sheet). 106 | 107 | #loop closure parameters 108 | # loop_closure: 0 #1 # start loop closure 109 | # load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path' 110 | # fast_relocalization: 1 # useful in real-time and large project 111 | # pose_graph_save_path: "/home/tony-ws1/output/pose_graph/" # save and load path 112 | 113 | #visualization parameters 114 | # save_image: 1 #1 # save image in pose graph for visualization prupose; you can close this function by setting 0 115 | # visualize_imu_forward: 0 # output imu forward propogation to achieve low latency and high frequence results 116 | # visualize_camera_size: 0.4 # size of camera marker in RVIZ 117 | -------------------------------------------------------------------------------- /config/fpv_forward_45/cam0_fisheye.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | model_type: KANNALA_BRANDT 4 | camera_name: camera 5 | image_width: 640 6 | image_height: 480 7 | projection_parameters: 8 | # k2: 0.0034823894022493434 9 | # k3: 0.0007150348452162257 10 | # k4: -0.0020532361418706202 11 | # k5: 0.00020293673591811182 12 | k2: -6.545154718304953e-06 13 | k3: -0.010379525898159981 14 | k4: 0.014935312423953146 15 | k5: -0.005639061406567785 16 | mu: 275.46015578667294 17 | mv: 274.9948095922592 18 | u0: 315.958384100568 19 | v0: 242.7123497822731 20 | 21 | 22 | #### for pose graph node 23 | image_topic: "/snappy_cam/stereo_l" 24 | output_path: "/home/davidz/work/result/fpv_forward_45" 25 | #loop closure parameters 26 | loop_closure: 1 #1 # start loop closure 27 | load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path' 28 | fast_relocalization: 1 # useful in real-time and large project 29 | pose_graph_save_path: "/home/davidz/work/result/pose_graph/" # save and load path 30 | 31 | #visualization parameters 32 | save_image: 1 #1 # save image in pose graph for visualization prupose; you can close this function by setting 0 33 | visualize_imu_forward: 0 # output imu forward propogation to achieve low latency and high frequence results 34 | visualize_camera_size: 0.4 # size of camera marker in RVIZ 35 | 36 | -------------------------------------------------------------------------------- /config/fpv_forward_45/cam1_fisheye.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | model_type: KANNALA_BRANDT 4 | camera_name: camera 5 | image_width: 640 6 | image_height: 480 7 | projection_parameters: 8 | k2: -0.012138050918285051 9 | k3: 0.02244029339184358 10 | k4: -0.013753165428754275 11 | k5: 0.002725090438517269 12 | mu: 274.4628309070672 13 | mv: 273.9261674470783 14 | u0: 315.93654481793794 15 | v0: 235.779167375461 16 | -------------------------------------------------------------------------------- /config/fpv_forward_45/fisheye_mask.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rising-turtle/VINS-Stereo/51ce0442bb5b15bc8cfb85fd3a5cb76e8f2ce835/config/fpv_forward_45/fisheye_mask.jpg -------------------------------------------------------------------------------- /config/fpv_forward_45/fpv_forward_45_degree.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #common parameters 4 | imu_topic: "/snappy_imu" 5 | image0_topic: "/snappy_cam/stereo_l" 6 | image1_topic: "/snappy_cam/stereo_r" 7 | 8 | output_path: "/home/davidz/work/result/fpv_forward_45" 9 | 10 | #camera calibration 11 | cam0_calib: "cam0_fisheye.yaml" 12 | cam1_calib: "cam1_fisheye.yaml" 13 | image_width: 640 14 | image_height: 480 15 | 16 | # Extrinsic parameter between IMU and Camera. 17 | estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it. 18 | # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess. 19 | # 2 Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning. 20 | #If you choose 0 or 1, you should write down the following matrix. 21 | #Rotation from camera frame to imu frame, imu^R_cam 22 | extrinsicRotation: !!opencv-matrix 23 | rows: 3 24 | cols: 3 25 | dt: d 26 | data: [-0.027257, -0.713921, 0.699696, 27 | -0.999626, 0.017931, -0.020644, 28 | 0.002192, -0.699997, -0.714142] 29 | 30 | extrinsicTranslation: !!opencv-matrix 31 | rows: 3 32 | cols: 1 33 | dt: d 34 | data: [0.007515, 0.024045, 0.005773] 35 | 36 | Rrl: !!opencv-matrix 37 | rows: 3 38 | cols: 3 39 | dt: d 40 | data: [ 0.9998713028057842, 0.0019367839962957476, 0.015925661468614183, 41 | -0.0020450612045710957, 0.9999748874354272, 0.0067854420026733295, 42 | -0.01591211959893364, -0.006817137687752675, 0.9998501543149417] 43 | 44 | Trl: !!opencv-matrix 45 | rows: 3 46 | cols: 1 47 | dt: d 48 | data: [-0.07993259169905036, -7.205400304978925e-05, -0.0009141881692325364] 49 | 50 | body_T_cam1: !!opencv-matrix # Timu2c_2, Tc1_2_c2 is a virtual transformation [I,t] t = [0.1, 0, 0], note "mbf" in estimator_dpt.cpp 51 | rows: 4 52 | cols: 4 53 | dt: d 54 | data: [-0.017493, -0.709099, 0.704892, 0.006710, 55 | -0.999791, 0.019835, -0.004857, -0.055874, 56 | -0.010537, -0.704830, -0.709298, 0.004231, 57 | 0.000000, 0.000000, -0.000000, 1.000000] 58 | 59 | #feature traker paprameters 60 | max_cnt: 300 # max feature number in feature tracking 61 | min_dist: 25 # min distance between two features 62 | freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image 63 | F_threshold: 1.0 #1.0 # ransac threshold (pixel) 64 | show_track: 1 # publish tracking image as topic 65 | equalize: 0 # if image is too dark or light, trun on equalize to find enough features 66 | fisheye: 1 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points 67 | flow_back: 1 #1 # if need double check by reverse optical flow 68 | 69 | #optimization parameters 70 | max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time 71 | max_num_iterations: 8 # max solver itrations, to guarantee real time 72 | keyframe_parallax: 10.0 # keyframe selection threshold (pixel) 73 | 74 | #imu parameters The more accurate parameters you provide, the better performance 75 | acc_n: 0.12 # 0.1 #0.12 #0.1 # 0.04 # accelerometer measurement noise standard deviation. #0.2 0.04 76 | gyr_n: 0.02 # 0.05 # 0.02 #0.05 #0.004 # gyroscope measurement noise standard deviation. #0.05 0.004 77 | acc_w: 0.002 #0.0004 # accelerometer bias random work noise standard deviation. #0.02 78 | gyr_w: 0.00004 #2.0e-5 # gyroscope bias random work noise standard deviation. #4.0e-5 79 | g_norm: 9.80766 # gravity magnitude 80 | 81 | #unsynchronization parameters 82 | estimate_td: 1 # online estimate time offset between camera and imu 83 | td: -0.0149507360078 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock) 84 | 85 | #rolling shutter parameters 86 | rolling_shutter: 0 # 0: global shutter camera, 1: rolling shutter camera 87 | rolling_shutter_tr: 0 # unit: s. rolling shutter read out time per frame (from data sheet). 88 | 89 | #loop closure parameters 90 | # loop_closure: 0 #1 # start loop closure 91 | # load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path' 92 | # fast_relocalization: 1 # useful in real-time and large project 93 | # pose_graph_save_path: "/home/tony-ws1/output/pose_graph/" # save and load path 94 | 95 | #visualization parameters 96 | # save_image: 1 #1 # save image in pose graph for visualization prupose; you can close this function by setting 0 97 | # visualize_imu_forward: 0 # output imu forward propogation to achieve low latency and high frequence results 98 | # visualize_camera_size: 0.4 # size of camera marker in RVIZ 99 | -------------------------------------------------------------------------------- /config/fpv_out_forward/cam0_fisheye.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | model_type: KANNALA_BRANDT 4 | camera_name: camera 5 | image_width: 640 6 | image_height: 480 7 | projection_parameters: 8 | k2: -0.005719912631104124 9 | k3: 0.004742449009601135 10 | k4: 0.0012060658036136048 11 | k5: -0.001580292679344826 12 | mu: 277.4786896484645 13 | mv: 277.42548548840034 14 | u0: 320.1052053576385 15 | v0: 242.10083077857894 16 | 17 | 18 | #### for pose graph node 19 | image_topic: "/snappy_cam/stereo_l" 20 | output_path: "/home/davidz/work/result/fpv_out_forward" 21 | #loop closure parameters 22 | loop_closure: 1 #1 # start loop closure 23 | load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path' 24 | fast_relocalization: 1 # useful in real-time and large project 25 | pose_graph_save_path: "/home/davidz/work/result/pose_graph/" # save and load path 26 | 27 | #visualization parameters 28 | save_image: 1 #1 # save image in pose graph for visualization prupose; you can close this function by setting 0 29 | visualize_imu_forward: 0 # output imu forward propogation to achieve low latency and high frequence results 30 | visualize_camera_size: 0.4 # size of camera marker in RVIZ 31 | 32 | -------------------------------------------------------------------------------- /config/fpv_out_forward/cam1_fisheye.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | model_type: KANNALA_BRANDT 4 | camera_name: camera 5 | image_width: 640 6 | image_height: 480 7 | projection_parameters: 8 | k2: -0.009025009906076716 9 | k3: 0.009967427035376123 10 | k4: -0.0029538969814842117 11 | k5: -0.0003503551771748748 12 | mu: 276.78679780974477 13 | mv: 276.79332134030807 14 | u0: 314.2862327340746 15 | v0: 236.51313088043128 16 | -------------------------------------------------------------------------------- /config/fpv_out_forward/fisheye_mask.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rising-turtle/VINS-Stereo/51ce0442bb5b15bc8cfb85fd3a5cb76e8f2ce835/config/fpv_out_forward/fisheye_mask.jpg -------------------------------------------------------------------------------- /config/fpv_out_forward/fisheye_mask3.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rising-turtle/VINS-Stereo/51ce0442bb5b15bc8cfb85fd3a5cb76e8f2ce835/config/fpv_out_forward/fisheye_mask3.jpg -------------------------------------------------------------------------------- /config/fpv_out_forward/fpv_outdoor_forward.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #common parameters 4 | imu_topic: "/snappy_imu" 5 | image0_topic: "/snappy_cam/stereo_l" 6 | image1_topic: "/snappy_cam/stereo_r" 7 | 8 | output_path: "/home/davidz/work/result/fpv_outdoor_forward" 9 | 10 | #camera calibration 11 | cam0_calib: "cam0_fisheye.yaml" 12 | cam1_calib: "cam1_fisheye.yaml" 13 | image_width: 640 14 | image_height: 480 15 | 16 | # Extrinsic parameter between IMU and Camera. 17 | estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it. 18 | # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess. 19 | # 2 Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning. 20 | #If you choose 0 or 1, you should write down the following matrix. 21 | #Rotation from camera frame to imu frame, imu^R_cam 22 | extrinsicRotation: !!opencv-matrix 23 | rows: 3 24 | cols: 3 25 | dt: d 26 | data: [-0.031798, 0.012828, 0.999412, 27 | -0.999493, 0.000952, -0.031813, 28 | -0.001359, -0.999917, 0.012791] 29 | 30 | extrinsicTranslation: !!opencv-matrix 31 | rows: 3 32 | cols: 1 33 | dt: d 34 | data: [0.010168, 0.020803, -0.000749] 35 | 36 | Rrl: !!opencv-matrix 37 | rows: 3 38 | cols: 3 39 | dt: d 40 | data: [0.9997470623689986, 0.009836089265916417, 0.020225296846065624, 41 | -0.00975774768296796, 0.9999445171722606, -0.0039684930755682956, 42 | -0.020263209141547188, 0.0037701359508940783, 0.9997875716521978] 43 | 44 | Trl: !!opencv-matrix 45 | rows: 3 46 | cols: 1 47 | dt: d 48 | data: [-0.07919358086270675, 0.000831414953842084, 0.00044568632114983057] 49 | 50 | body_T_cam1: !!opencv-matrix # Timu2c_2, Tc1_2_c2 is a virtual transformation [I,t] t = [0.1, 0, 0], note "mbf" in estimator_dpt.cpp 51 | rows: 4 52 | cols: 4 53 | dt: d 54 | data: [-0.011450, 0.009171, 0.999892, 0.008808, 55 | -0.999875, 0.010831, -0.011549, -0.058384, 56 | -0.010935, -0.999899, 0.009046, -0.000788, 57 | 0.000000, 0.000000, -0.000000, 1.000000] 58 | 59 | #feature traker paprameters 60 | max_cnt: 300 #250 # max feature number in feature tracking 61 | min_dist: 25 # min distance between two features 62 | freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image 63 | F_threshold: 1.0 # ransac threshold (pixel) 64 | show_track: 1 # publish tracking image as topic 65 | equalize: 0 # if image is too dark or light, trun on equalize to find enough features 66 | fisheye: 1 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points 67 | flow_back: 1 #1 # if need double check by reverse optical flow 68 | 69 | #optimization parameters 70 | max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time 71 | max_num_iterations: 8 # max solver itrations, to guarantee real time 72 | keyframe_parallax: 10.0 # keyframe selection threshold (pixel) 73 | 74 | #imu parameters The more accurate parameters you provide, the better performance 75 | acc_n: 0.12 # 0.1 #0.12 # 0.12 #0.1 # 0.04 # accelerometer measurement noise standard deviation. #0.2 0.04 76 | gyr_n: 0.02 # 0.05 #0.02 #0.05 # 0.02 # 0.05 #0.004 # gyroscope measurement noise standard deviation. #0.05 0.004 77 | acc_w: 0.002 #0.0004 # accelerometer bias random work noise standard deviation. #0.02 78 | gyr_w: 0.00004 #2.0e-5 # gyroscope bias random work noise standard deviation. #4.0e-5 79 | g_norm: 9.80766 # gravity magnitude 80 | 81 | #unsynchronization parameters 82 | estimate_td: 1 # online estimate time offset between camera and imu 83 | td: -0.007983859928063504 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock) 84 | 85 | #rolling shutter parameters 86 | rolling_shutter: 0 # 0: global shutter camera, 1: rolling shutter camera 87 | rolling_shutter_tr: 0 # unit: s. rolling shutter read out time per frame (from data sheet). 88 | 89 | #loop closure parameters 90 | # loop_closure: 0 #1 # start loop closure 91 | # load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path' 92 | # fast_relocalization: 1 # useful in real-time and large project 93 | # pose_graph_save_path: "/home/tony-ws1/output/pose_graph/" # save and load path 94 | 95 | #visualization parameters 96 | # save_image: 1 #1 # save image in pose graph for visualization prupose; you can close this function by setting 0 97 | # visualize_imu_forward: 0 # output imu forward propogation to achieve low latency and high frequence results 98 | # visualize_camera_size: 0.4 # size of camera marker in RVIZ 99 | -------------------------------------------------------------------------------- /config/tum_vi/TUM_512.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #common parameters 4 | imu_topic: "/imu0" 5 | image0_topic: "/cam0/image_raw" 6 | image1_topic: "/cam1/image_raw" 7 | 8 | output_path: "/home/davidz/work/result/tum_vi" 9 | 10 | #camera calibration 11 | cam0_calib: "cam0_fisheye.yaml" 12 | cam1_calib: "cam1_fisheye.yaml" 13 | # image_width: 640 14 | # image_height: 480 15 | 16 | ### for debug 17 | #camera calibration 18 | image_topic: "/cam0/image_raw" 19 | 20 | model_type: KANNALA_BRANDT 21 | camera_name: camera 22 | image_width: 512 23 | image_height: 512 24 | projection_parameters: 25 | k2: 0.0034823894022493434 26 | k3: 0.0007150348452162257 27 | k4: -0.0020532361418706202 28 | k5: 0.00020293673591811182 29 | mu: 190.97847715128717 30 | mv: 190.9733070521226 31 | u0: 254.93170605935475 32 | v0: 256.8974428996504 33 | 34 | # Extrinsic parameter between IMU and Camera. 35 | estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it. 36 | # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess. 37 | # 2 Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning. 38 | #If you choose 0 or 1, you should write down the following matrix. 39 | #Rotation from camera frame to imu frame, imu^R_cam 40 | extrinsicRotation: !!opencv-matrix 41 | rows: 3 42 | cols: 3 43 | dt: d 44 | data: [ -9.9951465899298464e-01, 7.5842033363785165e-03, 45 | -3.0214670573904204e-02, 2.9940114644659861e-02, 46 | -3.4023430206013172e-02, -9.9897246995704592e-01, 47 | -8.6044170750674241e-03, -9.9939225835343004e-01, 48 | 3.3779845322755464e-02 ] 49 | extrinsicTranslation: !!opencv-matrix 50 | rows: 3 51 | cols: 1 52 | dt: d 53 | data: [ 4.4511917113940799e-02, -7.3197096234105752e-02, 54 | -4.7972907300764499e-02 ] 55 | 56 | Rrl: !!opencv-matrix 57 | rows: 3 58 | cols: 3 59 | dt: d 60 | data: [ 0.999999, -0.000823, -0.000656, 61 | 0.000792, 0.998899, -0.046896, 62 | 0.000694, 0.046895, 0.998900] 63 | 64 | Trl: !!opencv-matrix 65 | rows: 3 66 | cols: 1 67 | dt: d 68 | data: [-0.101061, -0.001976, -0.001176 ] 69 | 70 | body_T_cam1: !!opencv-matrix # Timu2c_2, Tc1_2_c2 is a virtual transformation [I,t] t = [0.1, 0, 0], note "mbf" in estimator_dpt.cpp 71 | rows: 4 72 | cols: 4 73 | dt: d 74 | data: [-0.999511, 0.00810408, -0.0301991, -0.0554563, 75 | 0.0302991, 0.0125116, -0.999463, -0.06925, 76 | -0.00772188, -0.999889, -0.0127511, -0.0474529, 77 | 0., 0. , 0. , 1.] 78 | 79 | #feature traker paprameters 80 | max_cnt: 150 # 300 #250 # max feature number in feature tracking 81 | min_dist: 25 # min distance between two features 82 | freq: 10 #10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image 83 | F_threshold: 1.0 # ransac threshold (pixel) 84 | show_track: 1 # publish tracking image as topic 85 | equalize: 0 # if image is too dark or light, trun on equalize to find enough features 86 | fisheye: 1 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points 87 | flow_back: 1 # if need double check by reverse optical flow 88 | 89 | #optimization parameters 90 | max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time 91 | max_num_iterations: 8 # max solver itrations, to guarantee real time 92 | keyframe_parallax: 10.0 # keyframe selection threshold (pixel) 93 | 94 | #imu parameters The more accurate parameters you provide, the better performance 95 | # seems 0.12 & 0.02 is a better choice compared to 0.1 & 0.05 96 | acc_n: 0.04 # 0.1 # 0.12 # 0.12 # 0.1 # 0.04 # accelerometer measurement noise standard deviation. #0.2 0.04 97 | gyr_n: 0.004 # 0.05 # 0.02 # 0.01 0.05 #0.004 # gyroscope measurement noise standard deviation. #0.05 0.004 98 | acc_w: 0.0004 #0.0004 # accelerometer bias random work noise standard deviation. #0.02 99 | gyr_w: 0.00002 #2.0e-5 # gyroscope bias random work noise standard deviation. #4.0e-5 100 | g_norm: 9.80766 # gravity magnitude 101 | 102 | #unsynchronization parameters 103 | estimate_td: 0 #1 # online estimate time offset between camera and imu 104 | td: 0 #-0.0166845720919 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock) 105 | 106 | #rolling shutter parameters 107 | rolling_shutter: 0 # 0: global shutter camera, 1: rolling shutter camera 108 | rolling_shutter_tr: 0 # unit: s. rolling shutter read out time per frame (from data sheet). 109 | 110 | #loop closure parameters 111 | # loop_closure: 0 #1 # start loop closure 112 | # load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path' 113 | # fast_relocalization: 1 # useful in real-time and large project 114 | # pose_graph_save_path: "/home/tony-ws1/output/pose_graph/" # save and load path 115 | 116 | #visualization parameters 117 | # save_image: 1 #1 # save image in pose graph for visualization prupose; you can close this function by setting 0 118 | # visualize_imu_forward: 0 # output imu forward propogation to achieve low latency and high frequence results 119 | # visualize_camera_size: 0.4 # size of camera marker in RVIZ 120 | -------------------------------------------------------------------------------- /config/tum_vi/cam0_fisheye.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | model_type: KANNALA_BRANDT 4 | camera_name: camera 5 | image_width: 512 6 | image_height: 512 7 | projection_parameters: 8 | k2: 0.0034823894022493434 9 | k3: 0.0007150348452162257 10 | k4: -0.0020532361418706202 11 | k5: 0.00020293673591811182 12 | mu: 190.97847715128717 13 | mv: 190.9733070521226 14 | u0: 254.93170605935475 15 | v0: 256.8974428996504 16 | 17 | #### for pose graph node 18 | image_topic: "/cam0/image_raw" 19 | output_path: "/home/davidz/work/result/tum_vi" 20 | #loop closure parameters 21 | loop_closure: 1 #1 # start loop closure 22 | load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path' 23 | fast_relocalization: 1 # useful in real-time and large project 24 | pose_graph_save_path: "/home/davidz/work/result/pose_graph/" # save and load path 25 | 26 | #visualization parameters 27 | save_image: 1 #1 # save image in pose graph for visualization prupose; you can close this function by setting 0 28 | visualize_imu_forward: 0 # output imu forward propogation to achieve low latency and high frequence results 29 | visualize_camera_size: 0.4 # size of camera marker in RVIZ 30 | 31 | -------------------------------------------------------------------------------- /config/tum_vi/cam1_fisheye.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | model_type: KANNALA_BRANDT 4 | camera_name: camera 5 | image_width: 512 6 | image_height: 512 7 | projection_parameters: 8 | k2: 0.0034003170790442797 9 | k3: 0.001766278153469831 10 | k4: -0.00266312569781606 11 | k5: 0.0003299517423931039 12 | mu: 190.44236969414825 13 | mv: 190.4344384721956 14 | u0: 252.59949716835982 15 | v0: 254.91723064636983 16 | -------------------------------------------------------------------------------- /config/tum_vi/fisheye_mask.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rising-turtle/VINS-Stereo/51ce0442bb5b15bc8cfb85fd3a5cb76e8f2ce835/config/tum_vi/fisheye_mask.jpg -------------------------------------------------------------------------------- /feature_tracker/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(feature_tracker_stereo) 3 | 4 | set(CMAKE_BUILD_TYPE "Release") 5 | set(CMAKE_CXX_FLAGS "-std=c++11") 6 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g") 7 | 8 | find_package(catkin REQUIRED COMPONENTS 9 | roscpp 10 | std_msgs 11 | sensor_msgs 12 | cv_bridge 13 | camera_model 14 | ) 15 | 16 | find_package(OpenCV REQUIRED) 17 | 18 | catkin_package() 19 | 20 | include_directories( 21 | ${catkin_INCLUDE_DIRS} 22 | ) 23 | 24 | set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) 25 | find_package(Eigen3) 26 | include_directories( 27 | ${catkin_INCLUDE_DIRS} 28 | ${EIGEN3_INCLUDE_DIR} 29 | ) 30 | 31 | add_executable(feature_tracker_stereo 32 | src/feature_tracker_node.cpp 33 | src/parameters.cpp 34 | src/feature_tracker.cpp 35 | ) 36 | 37 | target_link_libraries(feature_tracker_stereo ${catkin_LIBRARIES} ${OpenCV_LIBS}) 38 | -------------------------------------------------------------------------------- /feature_tracker/cmake/FindEigen.cmake: -------------------------------------------------------------------------------- 1 | # Ceres Solver - A fast non-linear least squares minimizer 2 | # Copyright 2015 Google Inc. All rights reserved. 3 | # http://ceres-solver.org/ 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions are met: 7 | # 8 | # * Redistributions of source code must retain the above copyright notice, 9 | # this list of conditions and the following disclaimer. 10 | # * Redistributions in binary form must reproduce the above copyright notice, 11 | # this list of conditions and the following disclaimer in the documentation 12 | # and/or other materials provided with the distribution. 13 | # * Neither the name of Google Inc. nor the names of its contributors may be 14 | # used to endorse or promote products derived from this software without 15 | # specific prior written permission. 16 | # 17 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | # POSSIBILITY OF SUCH DAMAGE. 28 | # 29 | # Author: alexs.mac@gmail.com (Alex Stewart) 30 | # 31 | 32 | # FindEigen.cmake - Find Eigen library, version >= 3. 33 | # 34 | # This module defines the following variables: 35 | # 36 | # EIGEN_FOUND: TRUE iff Eigen is found. 37 | # EIGEN_INCLUDE_DIRS: Include directories for Eigen. 38 | # 39 | # EIGEN_VERSION: Extracted from Eigen/src/Core/util/Macros.h 40 | # EIGEN_WORLD_VERSION: Equal to 3 if EIGEN_VERSION = 3.2.0 41 | # EIGEN_MAJOR_VERSION: Equal to 2 if EIGEN_VERSION = 3.2.0 42 | # EIGEN_MINOR_VERSION: Equal to 0 if EIGEN_VERSION = 3.2.0 43 | # 44 | # The following variables control the behaviour of this module: 45 | # 46 | # EIGEN_INCLUDE_DIR_HINTS: List of additional directories in which to 47 | # search for eigen includes, e.g: /timbuktu/eigen3. 48 | # 49 | # The following variables are also defined by this module, but in line with 50 | # CMake recommended FindPackage() module style should NOT be referenced directly 51 | # by callers (use the plural variables detailed above instead). These variables 52 | # do however affect the behaviour of the module via FIND_[PATH/LIBRARY]() which 53 | # are NOT re-called (i.e. search for library is not repeated) if these variables 54 | # are set with valid values _in the CMake cache_. This means that if these 55 | # variables are set directly in the cache, either by the user in the CMake GUI, 56 | # or by the user passing -DVAR=VALUE directives to CMake when called (which 57 | # explicitly defines a cache variable), then they will be used verbatim, 58 | # bypassing the HINTS variables and other hard-coded search locations. 59 | # 60 | # EIGEN_INCLUDE_DIR: Include directory for CXSparse, not including the 61 | # include directory of any dependencies. 62 | 63 | # Called if we failed to find Eigen or any of it's required dependencies, 64 | # unsets all public (designed to be used externally) variables and reports 65 | # error message at priority depending upon [REQUIRED/QUIET/] argument. 66 | macro(EIGEN_REPORT_NOT_FOUND REASON_MSG) 67 | unset(EIGEN_FOUND) 68 | unset(EIGEN_INCLUDE_DIRS) 69 | # Make results of search visible in the CMake GUI if Eigen has not 70 | # been found so that user does not have to toggle to advanced view. 71 | mark_as_advanced(CLEAR EIGEN_INCLUDE_DIR) 72 | # Note _FIND_[REQUIRED/QUIETLY] variables defined by FindPackage() 73 | # use the camelcase library name, not uppercase. 74 | if (Eigen_FIND_QUIETLY) 75 | message(STATUS "Failed to find Eigen - " ${REASON_MSG} ${ARGN}) 76 | elseif (Eigen_FIND_REQUIRED) 77 | message(FATAL_ERROR "Failed to find Eigen - " ${REASON_MSG} ${ARGN}) 78 | else() 79 | # Neither QUIETLY nor REQUIRED, use no priority which emits a message 80 | # but continues configuration and allows generation. 81 | message("-- Failed to find Eigen - " ${REASON_MSG} ${ARGN}) 82 | endif () 83 | return() 84 | endmacro(EIGEN_REPORT_NOT_FOUND) 85 | 86 | # Protect against any alternative find_package scripts for this library having 87 | # been called previously (in a client project) which set EIGEN_FOUND, but not 88 | # the other variables we require / set here which could cause the search logic 89 | # here to fail. 90 | unset(EIGEN_FOUND) 91 | 92 | # Search user-installed locations first, so that we prefer user installs 93 | # to system installs where both exist. 94 | list(APPEND EIGEN_CHECK_INCLUDE_DIRS 95 | /usr/local/include 96 | /usr/local/homebrew/include # Mac OS X 97 | /opt/local/var/macports/software # Mac OS X. 98 | /opt/local/include 99 | /usr/include) 100 | # Additional suffixes to try appending to each search path. 101 | list(APPEND EIGEN_CHECK_PATH_SUFFIXES 102 | eigen3 # Default root directory for Eigen. 103 | Eigen/include/eigen3 # Windows (for C:/Program Files prefix) < 3.3 104 | Eigen3/include/eigen3 ) # Windows (for C:/Program Files prefix) >= 3.3 105 | 106 | # Search supplied hint directories first if supplied. 107 | find_path(EIGEN_INCLUDE_DIR 108 | NAMES Eigen/Core 109 | PATHS ${EIGEN_INCLUDE_DIR_HINTS} 110 | ${EIGEN_CHECK_INCLUDE_DIRS} 111 | PATH_SUFFIXES ${EIGEN_CHECK_PATH_SUFFIXES}) 112 | 113 | if (NOT EIGEN_INCLUDE_DIR OR 114 | NOT EXISTS ${EIGEN_INCLUDE_DIR}) 115 | eigen_report_not_found( 116 | "Could not find eigen3 include directory, set EIGEN_INCLUDE_DIR to " 117 | "path to eigen3 include directory, e.g. /usr/local/include/eigen3.") 118 | endif (NOT EIGEN_INCLUDE_DIR OR 119 | NOT EXISTS ${EIGEN_INCLUDE_DIR}) 120 | 121 | # Mark internally as found, then verify. EIGEN_REPORT_NOT_FOUND() unsets 122 | # if called. 123 | set(EIGEN_FOUND TRUE) 124 | 125 | # Extract Eigen version from Eigen/src/Core/util/Macros.h 126 | if (EIGEN_INCLUDE_DIR) 127 | set(EIGEN_VERSION_FILE ${EIGEN_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h) 128 | if (NOT EXISTS ${EIGEN_VERSION_FILE}) 129 | eigen_report_not_found( 130 | "Could not find file: ${EIGEN_VERSION_FILE} " 131 | "containing version information in Eigen install located at: " 132 | "${EIGEN_INCLUDE_DIR}.") 133 | else (NOT EXISTS ${EIGEN_VERSION_FILE}) 134 | file(READ ${EIGEN_VERSION_FILE} EIGEN_VERSION_FILE_CONTENTS) 135 | 136 | string(REGEX MATCH "#define EIGEN_WORLD_VERSION [0-9]+" 137 | EIGEN_WORLD_VERSION "${EIGEN_VERSION_FILE_CONTENTS}") 138 | string(REGEX REPLACE "#define EIGEN_WORLD_VERSION ([0-9]+)" "\\1" 139 | EIGEN_WORLD_VERSION "${EIGEN_WORLD_VERSION}") 140 | 141 | string(REGEX MATCH "#define EIGEN_MAJOR_VERSION [0-9]+" 142 | EIGEN_MAJOR_VERSION "${EIGEN_VERSION_FILE_CONTENTS}") 143 | string(REGEX REPLACE "#define EIGEN_MAJOR_VERSION ([0-9]+)" "\\1" 144 | EIGEN_MAJOR_VERSION "${EIGEN_MAJOR_VERSION}") 145 | 146 | string(REGEX MATCH "#define EIGEN_MINOR_VERSION [0-9]+" 147 | EIGEN_MINOR_VERSION "${EIGEN_VERSION_FILE_CONTENTS}") 148 | string(REGEX REPLACE "#define EIGEN_MINOR_VERSION ([0-9]+)" "\\1" 149 | EIGEN_MINOR_VERSION "${EIGEN_MINOR_VERSION}") 150 | 151 | # This is on a single line s/t CMake does not interpret it as a list of 152 | # elements and insert ';' separators which would result in 3.;2.;0 nonsense. 153 | set(EIGEN_VERSION "${EIGEN_WORLD_VERSION}.${EIGEN_MAJOR_VERSION}.${EIGEN_MINOR_VERSION}") 154 | endif (NOT EXISTS ${EIGEN_VERSION_FILE}) 155 | endif (EIGEN_INCLUDE_DIR) 156 | 157 | # Set standard CMake FindPackage variables if found. 158 | if (EIGEN_FOUND) 159 | set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR}) 160 | endif (EIGEN_FOUND) 161 | 162 | # Handle REQUIRED / QUIET optional arguments and version. 163 | include(FindPackageHandleStandardArgs) 164 | find_package_handle_standard_args(Eigen 165 | REQUIRED_VARS EIGEN_INCLUDE_DIRS 166 | VERSION_VAR EIGEN_VERSION) 167 | 168 | # Only mark internal variables as advanced if we found Eigen, otherwise 169 | # leave it visible in the standard GUI for the user to set manually. 170 | if (EIGEN_FOUND) 171 | mark_as_advanced(FORCE EIGEN_INCLUDE_DIR) 172 | endif (EIGEN_FOUND) 173 | -------------------------------------------------------------------------------- /feature_tracker/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | feature_tracker_stereo 4 | 0.0.0 5 | The feature_tracker package for stereo input 6 | 7 | 8 | 9 | 10 | dvorak 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | roscpp 44 | camera_model 45 | message_generation 46 | roscpp 47 | camera_model 48 | message_runtime 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /feature_tracker/src/feature_tracker.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | #include "camodocal/camera_models/CameraFactory.h" 13 | #include "camodocal/camera_models/CataCamera.h" 14 | #include "camodocal/camera_models/PinholeCamera.h" 15 | 16 | #include "parameters.h" 17 | #include "tic_toc.h" 18 | 19 | using namespace std; 20 | using namespace camodocal; 21 | using namespace Eigen; 22 | 23 | bool inBorder(const cv::Point2f &pt); 24 | double distance(cv::Point2f pt1, cv::Point2f pt2); 25 | void reduceVector(vector &v, vector status); 26 | void reduceVector(vector &v, vector status); 27 | 28 | class FeatureTracker 29 | { 30 | public: 31 | FeatureTracker(); 32 | 33 | void readImage(const cv::Mat &_img,double _cur_time); 34 | 35 | // map>>> trackImage(double _cur_time, const cv::Mat &_img, const cv::Mat &_img1); 36 | void trackImage(double _cur_time, const cv::Mat &_img, const cv::Mat &_img1); 37 | 38 | void setMask(); 39 | 40 | void addPoints(); 41 | 42 | bool updateID(unsigned int i); 43 | 44 | void readIntrinsicParameter(vector &calib_file); 45 | 46 | void showUndistortion(const string &name); 47 | 48 | void rejectWithF(); 49 | 50 | void undistortedPoints(); 51 | 52 | vector ptsVelocity(vector &ids, vector &pts, 53 | map &cur_id_pts, map &prev_id_pts); 54 | 55 | vector undistortedPts(vector &pts, camodocal::CameraPtr cam); 56 | 57 | void drawTrack(const cv::Mat &imLeft, const cv::Mat &imRight, 58 | vector &curLeftIds, 59 | vector &curLeftPts, 60 | vector &curRightPts, 61 | map &prevLeftPtsMap); 62 | cv::Mat getTrackImage(); 63 | cv::Mat imTrack; 64 | cv::Mat mask; 65 | cv::Mat fisheye_mask; 66 | cv::Mat prev_img, cur_img, forw_img; 67 | vector n_pts; 68 | // vector prev_pts, cur_pts, forw_pts; 69 | // vector prev_un_pts, cur_un_pts; 70 | // vector pts_velocity; 71 | vector prev_pts, cur_pts, forw_pts, cur_right_pts; 72 | vector prev_un_pts, cur_un_pts, cur_un_right_pts; 73 | vector pts_velocity, right_pts_velocity; 74 | 75 | // vector ids; 76 | vector ids, ids_right; 77 | vector track_cnt; 78 | // map cur_un_pts_map; 79 | // map prev_un_pts_map; 80 | 81 | map cur_un_pts_map, prev_un_pts_map; 82 | map cur_un_right_pts_map, prev_un_right_pts_map; 83 | map prevLeftPtsMap; 84 | 85 | vector m_camera; 86 | double cur_time; 87 | double prev_time; 88 | 89 | static int n_id; 90 | int stereo_cam; 91 | bool hasPrediction; 92 | vector predict_pts; 93 | }; 94 | -------------------------------------------------------------------------------- /feature_tracker/src/parameters.cpp: -------------------------------------------------------------------------------- 1 | #include "parameters.h" 2 | #include 3 | 4 | std::string IMAGE1_TOPIC; 5 | std::string IMAGE2_TOPIC; 6 | std::string IMU_TOPIC; 7 | std::vector CAM_NAMES; 8 | std::string FISHEYE_MASK; 9 | Eigen::Matrix3d E; 10 | int MAX_CNT; 11 | int MIN_DIST; 12 | int WINDOW_SIZE; 13 | int FREQ; 14 | double F_THRESHOLD; 15 | int SHOW_TRACK; 16 | int STEREO_TRACK; 17 | int EQUALIZE; 18 | int ROW; 19 | int COL; 20 | int FOCAL_LENGTH; 21 | int FISHEYE; 22 | bool PUB_THIS_FRAME; 23 | int FLOW_BACK; 24 | 25 | template 26 | T readParam(ros::NodeHandle &n, std::string name) 27 | { 28 | T ans; 29 | if (n.getParam(name, ans)) 30 | { 31 | ROS_INFO_STREAM("Loaded " << name << ": " << ans); 32 | } 33 | else 34 | { 35 | ROS_ERROR_STREAM("Failed to load " << name); 36 | n.shutdown(); 37 | } 38 | return ans; 39 | } 40 | namespace { 41 | 42 | template 43 | static Eigen::Matrix skewSymmetric(const Eigen::MatrixBase &q) 44 | { 45 | Eigen::Matrix ans; 46 | ans << typename Derived::Scalar(0), -q(2), q(1), 47 | q(2), typename Derived::Scalar(0), -q(0), 48 | -q(1), q(0), typename Derived::Scalar(0); 49 | return ans; 50 | } 51 | } 52 | 53 | void readParameters(ros::NodeHandle &n) 54 | { 55 | std::string config_file; 56 | config_file = readParam(n, "config_file"); 57 | cv::FileStorage fsSettings(config_file, cv::FileStorage::READ); 58 | if(!fsSettings.isOpened()){ 59 | std::cerr << "ERROR: Wrong path to settings" << std::endl; 60 | } 61 | std::string VINS_FOLDER_PATH = readParam(n, "vins_folder"); 62 | 63 | fsSettings["image0_topic"] >> IMAGE1_TOPIC; 64 | fsSettings["image1_topic"] >> IMAGE2_TOPIC; 65 | // fsSettings["imu_topic"] >> IMU_TOPIC; 66 | MAX_CNT = fsSettings["max_cnt"]; 67 | MIN_DIST = fsSettings["min_dist"]; 68 | ROW = fsSettings["image_height"]; 69 | COL = fsSettings["image_width"]; 70 | FREQ = fsSettings["freq"]; 71 | F_THRESHOLD = fsSettings["F_threshold"]; 72 | SHOW_TRACK = fsSettings["show_track"]; 73 | EQUALIZE = fsSettings["equalize"]; 74 | FISHEYE = fsSettings["fisheye"]; 75 | FLOW_BACK = fsSettings["flow_back"]; 76 | if (FISHEYE == 1) 77 | FISHEYE_MASK = VINS_FOLDER_PATH + "/fisheye_mask.jpg"; 78 | 79 | // CAM_NAMES.push_back(config_file); 80 | std::string cam0Calibfile, cam1Calibfile; 81 | fsSettings["cam0_calib"] >> cam0Calibfile; 82 | fsSettings["cam1_calib"] >> cam1Calibfile; 83 | 84 | std::string cam0Path = VINS_FOLDER_PATH + "/" + cam0Calibfile; 85 | std::string cam1Path = VINS_FOLDER_PATH + "/" + cam1Calibfile; 86 | 87 | ROS_DEBUG("cam0Path: %s", cam0Path.c_str()); 88 | ROS_DEBUG("cam1Path: %s", cam1Path.c_str()); 89 | CAM_NAMES.push_back(cam0Path); 90 | CAM_NAMES.push_back(cam1Path); 91 | 92 | // transformation between stereo cams 93 | { 94 | Eigen::Matrix3d Rlr; 95 | Eigen::Vector3d Tlr; 96 | Eigen::Matrix3d Rrl; // Trl 97 | Eigen::Vector3d Trl; 98 | cv::Mat cv_R, cv_T; 99 | fsSettings["Rrl"] >> cv_R; 100 | fsSettings["Trl"] >> cv_T; 101 | cv::cv2eigen(cv_R, Rrl); 102 | cv::cv2eigen(cv_T, Trl); 103 | Eigen::Quaterniond qq(Rrl); 104 | Rrl = qq.normalized(); 105 | Rlr = Rrl.transpose(); 106 | Tlr = - Rlr * Trl; 107 | ROS_INFO_STREAM("Rrl: " << std::endl << Rrl); 108 | ROS_INFO_STREAM("Trl: " << std::endl << Trl.transpose()); 109 | 110 | E.setZero(); 111 | // const Eigen::Vector3d t_0_1 = T_0_1.translation(); 112 | // const Eigen::Matrix3d R_0_1 = T_0_1.rotationMatrix(); 113 | // E.topLeftCorner<3, 3>() = Sophus::SO3d::hat(t_0_1.normalized()) * R_0_1; 114 | E = skewSymmetric(Trl) * Rrl; 115 | } 116 | 117 | WINDOW_SIZE = 20; 118 | // STEREO_TRACK = false; 119 | FOCAL_LENGTH = 460; 120 | PUB_THIS_FRAME = false; 121 | 122 | if (FREQ == 0) 123 | FREQ = 100; 124 | 125 | fsSettings.release(); 126 | 127 | } 128 | -------------------------------------------------------------------------------- /feature_tracker/src/parameters.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | extern int ROW; 8 | extern int COL; 9 | extern int FOCAL_LENGTH; 10 | 11 | const int NUM_OF_CAM = 2; 12 | const int STEREO = 1; 13 | 14 | extern std::string IMAGE1_TOPIC; 15 | extern std::string IMAGE2_TOPIC; 16 | extern std::string IMU_TOPIC; 17 | extern std::string FISHEYE_MASK; 18 | extern std::vector CAM_NAMES; 19 | extern int MAX_CNT; 20 | extern int MIN_DIST; 21 | extern int WINDOW_SIZE; 22 | extern int FREQ; 23 | extern double F_THRESHOLD; 24 | extern int SHOW_TRACK; 25 | extern int STEREO_TRACK; 26 | extern int EQUALIZE; 27 | extern int FISHEYE; 28 | extern bool PUB_THIS_FRAME; 29 | extern int FLOW_BACK; 30 | extern Eigen::Matrix3d E; // essential matrix 31 | 32 | void readParameters(ros::NodeHandle &n); 33 | -------------------------------------------------------------------------------- /feature_tracker/src/tic_toc.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | class TicToc 8 | { 9 | public: 10 | TicToc() 11 | { 12 | tic(); 13 | } 14 | 15 | void tic() 16 | { 17 | start = std::chrono::system_clock::now(); 18 | } 19 | 20 | double toc() 21 | { 22 | end = std::chrono::system_clock::now(); 23 | std::chrono::duration elapsed_seconds = end - start; 24 | return elapsed_seconds.count() * 1000; 25 | } 26 | 27 | private: 28 | std::chrono::time_point start, end; 29 | }; 30 | -------------------------------------------------------------------------------- /launch/debug_feature_track.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /launch/euroc_stereo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /launch/feature_tracker.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /launch/feature_tracker_45.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /launch/fpv.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /launch/fpv_forward_45.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /launch/fpv_outdoor.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /launch/tum_vio.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /launch/vins_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /vins_estimator/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(vins_estimator_stereo) 3 | 4 | set(CMAKE_BUILD_TYPE "Release") 5 | set(CMAKE_CXX_FLAGS "-std=c++11") 6 | #-DEIGEN_USE_MKL_ALL") 7 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g") 8 | 9 | find_package(catkin REQUIRED COMPONENTS 10 | roscpp 11 | std_msgs 12 | geometry_msgs 13 | nav_msgs 14 | tf 15 | cv_bridge 16 | camera_model 17 | ) 18 | 19 | find_package(OpenCV REQUIRED) 20 | 21 | # message(WARNING "OpenCV_VERSION: ${OpenCV_VERSION}") 22 | 23 | find_package(Ceres REQUIRED) 24 | 25 | include_directories(${catkin_INCLUDE_DIRS} ${CERES_INCLUDE_DIRS}) 26 | 27 | set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) 28 | find_package(Eigen3) 29 | include_directories( 30 | ${catkin_INCLUDE_DIRS} 31 | ${EIGEN3_INCLUDE_DIR} 32 | ) 33 | 34 | catkin_package() 35 | 36 | add_executable(vins_estimator_stereo 37 | # src/estimator_node_ss.cpp 38 | # src/estimator_node.cpp 39 | src/estimator_node_stereo.cpp 40 | src/parameters.cpp 41 | src/estimator.cpp 42 | src/feature_manager.cpp 43 | src/factor/pose_local_parameterization.cpp 44 | src/factor/projection_factor.cpp 45 | src/factor/projection_td_factor.cpp 46 | src/factor/marginalization_factor.cpp 47 | src/factor/sampson_factor.cpp 48 | src/factor/sampson_jacobian.cpp 49 | src/factor/projectionOneFrameTwoCamFactor.cpp 50 | src/factor/projectionTwoFrameOneCamFactor.cpp 51 | src/factor/projectionTwoFrameTwoCamFactor.cpp 52 | src/utility/utility.cpp 53 | src/utility/visualization.cpp 54 | src/utility/CameraPoseVisualization.cpp 55 | src/initial/solve_5pts.cpp 56 | src/initial/solve_opt.cpp 57 | src/initial/translate_factor.cpp 58 | src/initial/initial_aligment.cpp 59 | src/initial/initial_sfm.cpp 60 | src/initial/initial_ex_rotation.cpp 61 | ) 62 | 63 | 64 | target_link_libraries(vins_estimator_stereo ${catkin_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES}) 65 | 66 | 67 | add_executable(debug_optimize 68 | src/debug_optimize.cpp 69 | src/parameters.cpp 70 | src/factor/pose_local_parameterization.cpp 71 | src/factor/projectionOneFrameTwoCamFactor.cpp 72 | src/factor/projectionTwoFrameOneCamFactor.cpp 73 | src/factor/projectionTwoFrameTwoCamFactor.cpp 74 | src/utility/utility.cpp 75 | ) 76 | 77 | target_link_libraries(debug_optimize ${catkin_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES}) -------------------------------------------------------------------------------- /vins_estimator/cmake/FindEigen.cmake: -------------------------------------------------------------------------------- 1 | # Ceres Solver - A fast non-linear least squares minimizer 2 | # Copyright 2015 Google Inc. All rights reserved. 3 | # http://ceres-solver.org/ 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions are met: 7 | # 8 | # * Redistributions of source code must retain the above copyright notice, 9 | # this list of conditions and the following disclaimer. 10 | # * Redistributions in binary form must reproduce the above copyright notice, 11 | # this list of conditions and the following disclaimer in the documentation 12 | # and/or other materials provided with the distribution. 13 | # * Neither the name of Google Inc. nor the names of its contributors may be 14 | # used to endorse or promote products derived from this software without 15 | # specific prior written permission. 16 | # 17 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | # POSSIBILITY OF SUCH DAMAGE. 28 | # 29 | # Author: alexs.mac@gmail.com (Alex Stewart) 30 | # 31 | 32 | # FindEigen.cmake - Find Eigen library, version >= 3. 33 | # 34 | # This module defines the following variables: 35 | # 36 | # EIGEN_FOUND: TRUE iff Eigen is found. 37 | # EIGEN_INCLUDE_DIRS: Include directories for Eigen. 38 | # 39 | # EIGEN_VERSION: Extracted from Eigen/src/Core/util/Macros.h 40 | # EIGEN_WORLD_VERSION: Equal to 3 if EIGEN_VERSION = 3.2.0 41 | # EIGEN_MAJOR_VERSION: Equal to 2 if EIGEN_VERSION = 3.2.0 42 | # EIGEN_MINOR_VERSION: Equal to 0 if EIGEN_VERSION = 3.2.0 43 | # 44 | # The following variables control the behaviour of this module: 45 | # 46 | # EIGEN_INCLUDE_DIR_HINTS: List of additional directories in which to 47 | # search for eigen includes, e.g: /timbuktu/eigen3. 48 | # 49 | # The following variables are also defined by this module, but in line with 50 | # CMake recommended FindPackage() module style should NOT be referenced directly 51 | # by callers (use the plural variables detailed above instead). These variables 52 | # do however affect the behaviour of the module via FIND_[PATH/LIBRARY]() which 53 | # are NOT re-called (i.e. search for library is not repeated) if these variables 54 | # are set with valid values _in the CMake cache_. This means that if these 55 | # variables are set directly in the cache, either by the user in the CMake GUI, 56 | # or by the user passing -DVAR=VALUE directives to CMake when called (which 57 | # explicitly defines a cache variable), then they will be used verbatim, 58 | # bypassing the HINTS variables and other hard-coded search locations. 59 | # 60 | # EIGEN_INCLUDE_DIR: Include directory for CXSparse, not including the 61 | # include directory of any dependencies. 62 | 63 | # Called if we failed to find Eigen or any of it's required dependencies, 64 | # unsets all public (designed to be used externally) variables and reports 65 | # error message at priority depending upon [REQUIRED/QUIET/] argument. 66 | macro(EIGEN_REPORT_NOT_FOUND REASON_MSG) 67 | unset(EIGEN_FOUND) 68 | unset(EIGEN_INCLUDE_DIRS) 69 | # Make results of search visible in the CMake GUI if Eigen has not 70 | # been found so that user does not have to toggle to advanced view. 71 | mark_as_advanced(CLEAR EIGEN_INCLUDE_DIR) 72 | # Note _FIND_[REQUIRED/QUIETLY] variables defined by FindPackage() 73 | # use the camelcase library name, not uppercase. 74 | if (Eigen_FIND_QUIETLY) 75 | message(STATUS "Failed to find Eigen - " ${REASON_MSG} ${ARGN}) 76 | elseif (Eigen_FIND_REQUIRED) 77 | message(FATAL_ERROR "Failed to find Eigen - " ${REASON_MSG} ${ARGN}) 78 | else() 79 | # Neither QUIETLY nor REQUIRED, use no priority which emits a message 80 | # but continues configuration and allows generation. 81 | message("-- Failed to find Eigen - " ${REASON_MSG} ${ARGN}) 82 | endif () 83 | return() 84 | endmacro(EIGEN_REPORT_NOT_FOUND) 85 | 86 | # Protect against any alternative find_package scripts for this library having 87 | # been called previously (in a client project) which set EIGEN_FOUND, but not 88 | # the other variables we require / set here which could cause the search logic 89 | # here to fail. 90 | unset(EIGEN_FOUND) 91 | 92 | # Search user-installed locations first, so that we prefer user installs 93 | # to system installs where both exist. 94 | list(APPEND EIGEN_CHECK_INCLUDE_DIRS 95 | /usr/local/include 96 | /usr/local/homebrew/include # Mac OS X 97 | /opt/local/var/macports/software # Mac OS X. 98 | /opt/local/include 99 | /usr/include) 100 | # Additional suffixes to try appending to each search path. 101 | list(APPEND EIGEN_CHECK_PATH_SUFFIXES 102 | eigen3 # Default root directory for Eigen. 103 | Eigen/include/eigen3 # Windows (for C:/Program Files prefix) < 3.3 104 | Eigen3/include/eigen3 ) # Windows (for C:/Program Files prefix) >= 3.3 105 | 106 | # Search supplied hint directories first if supplied. 107 | find_path(EIGEN_INCLUDE_DIR 108 | NAMES Eigen/Core 109 | PATHS ${EIGEN_INCLUDE_DIR_HINTS} 110 | ${EIGEN_CHECK_INCLUDE_DIRS} 111 | PATH_SUFFIXES ${EIGEN_CHECK_PATH_SUFFIXES}) 112 | 113 | if (NOT EIGEN_INCLUDE_DIR OR 114 | NOT EXISTS ${EIGEN_INCLUDE_DIR}) 115 | eigen_report_not_found( 116 | "Could not find eigen3 include directory, set EIGEN_INCLUDE_DIR to " 117 | "path to eigen3 include directory, e.g. /usr/local/include/eigen3.") 118 | endif (NOT EIGEN_INCLUDE_DIR OR 119 | NOT EXISTS ${EIGEN_INCLUDE_DIR}) 120 | 121 | # Mark internally as found, then verify. EIGEN_REPORT_NOT_FOUND() unsets 122 | # if called. 123 | set(EIGEN_FOUND TRUE) 124 | 125 | # Extract Eigen version from Eigen/src/Core/util/Macros.h 126 | if (EIGEN_INCLUDE_DIR) 127 | set(EIGEN_VERSION_FILE ${EIGEN_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h) 128 | if (NOT EXISTS ${EIGEN_VERSION_FILE}) 129 | eigen_report_not_found( 130 | "Could not find file: ${EIGEN_VERSION_FILE} " 131 | "containing version information in Eigen install located at: " 132 | "${EIGEN_INCLUDE_DIR}.") 133 | else (NOT EXISTS ${EIGEN_VERSION_FILE}) 134 | file(READ ${EIGEN_VERSION_FILE} EIGEN_VERSION_FILE_CONTENTS) 135 | 136 | string(REGEX MATCH "#define EIGEN_WORLD_VERSION [0-9]+" 137 | EIGEN_WORLD_VERSION "${EIGEN_VERSION_FILE_CONTENTS}") 138 | string(REGEX REPLACE "#define EIGEN_WORLD_VERSION ([0-9]+)" "\\1" 139 | EIGEN_WORLD_VERSION "${EIGEN_WORLD_VERSION}") 140 | 141 | string(REGEX MATCH "#define EIGEN_MAJOR_VERSION [0-9]+" 142 | EIGEN_MAJOR_VERSION "${EIGEN_VERSION_FILE_CONTENTS}") 143 | string(REGEX REPLACE "#define EIGEN_MAJOR_VERSION ([0-9]+)" "\\1" 144 | EIGEN_MAJOR_VERSION "${EIGEN_MAJOR_VERSION}") 145 | 146 | string(REGEX MATCH "#define EIGEN_MINOR_VERSION [0-9]+" 147 | EIGEN_MINOR_VERSION "${EIGEN_VERSION_FILE_CONTENTS}") 148 | string(REGEX REPLACE "#define EIGEN_MINOR_VERSION ([0-9]+)" "\\1" 149 | EIGEN_MINOR_VERSION "${EIGEN_MINOR_VERSION}") 150 | 151 | # This is on a single line s/t CMake does not interpret it as a list of 152 | # elements and insert ';' separators which would result in 3.;2.;0 nonsense. 153 | set(EIGEN_VERSION "${EIGEN_WORLD_VERSION}.${EIGEN_MAJOR_VERSION}.${EIGEN_MINOR_VERSION}") 154 | endif (NOT EXISTS ${EIGEN_VERSION_FILE}) 155 | endif (EIGEN_INCLUDE_DIR) 156 | 157 | # Set standard CMake FindPackage variables if found. 158 | if (EIGEN_FOUND) 159 | set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR}) 160 | endif (EIGEN_FOUND) 161 | 162 | # Handle REQUIRED / QUIET optional arguments and version. 163 | include(FindPackageHandleStandardArgs) 164 | find_package_handle_standard_args(Eigen 165 | REQUIRED_VARS EIGEN_INCLUDE_DIRS 166 | VERSION_VAR EIGEN_VERSION) 167 | 168 | # Only mark internal variables as advanced if we found Eigen, otherwise 169 | # leave it visible in the standard GUI for the user to set manually. 170 | if (EIGEN_FOUND) 171 | mark_as_advanced(FORCE EIGEN_INCLUDE_DIR) 172 | endif (EIGEN_FOUND) 173 | -------------------------------------------------------------------------------- /vins_estimator/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | vins_estimator_stereo 4 | 0.0.0 5 | The vins_estimator package 6 | 7 | 8 | 9 | 10 | qintong 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | roscpp 44 | roscpp 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | -------------------------------------------------------------------------------- /vins_estimator/src/estimator.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "parameters.h" 4 | #include "feature_manager.h" 5 | #include "utility/utility.h" 6 | #include "utility/tic_toc.h" 7 | #include "initial/solve_5pts.h" 8 | #include "initial/initial_sfm.h" 9 | #include "initial/initial_alignment.h" 10 | #include "initial/initial_ex_rotation.h" 11 | #include 12 | #include 13 | 14 | #include 15 | #include "factor/imu_factor.h" 16 | #include "factor/pose_local_parameterization.h" 17 | #include "factor/projection_factor.h" 18 | #include "factor/projection_td_factor.h" 19 | #include "factor/marginalization_factor.h" 20 | #include "factor/sampson_factor.h" 21 | #include "factor/projectionOneFrameTwoCamFactor.h" 22 | #include "factor/projectionTwoFrameOneCamFactor.h" 23 | #include "factor/projectionTwoFrameTwoCamFactor.h" 24 | 25 | #include "camodocal/camera_models/CameraFactory.h" 26 | #include "camodocal/camera_models/CataCamera.h" 27 | #include "camodocal/camera_models/PinholeCamera.h" 28 | 29 | #include 30 | #include 31 | #include 32 | 33 | 34 | class Estimator 35 | { 36 | public: 37 | Estimator(); 38 | 39 | void setParameter(); 40 | 41 | // check stereo visual input 42 | void readIntrinsicParameter(vector& calib_file); 43 | bool outlierCheck(int imu_i, int imu_j, Eigen::Vector3d &pts_i, Eigen::Vector3d &pts_j, double inv_dep_i); 44 | 45 | // interface 46 | void processIMU(double t, const Vector3d &linear_acceleration, const Vector3d &angular_velocity); 47 | void processImage(const map>>> &image, const std_msgs::Header &header); 48 | void setReloFrame(double _frame_stamp, int _frame_index, vector &_match_points, Vector3d _relo_t, Matrix3d _relo_r); 49 | 50 | // internal 51 | void clearState(); 52 | bool initialStructure(); 53 | bool initialStructureStereo(); 54 | bool visualInitialAlign(); 55 | bool visualInitialAlignWithDepth(); 56 | bool relativePose(Matrix3d &relative_R, Vector3d &relative_T, int &l); 57 | bool relativePoseHybrid(Matrix3d &relative_R, Vector3d &relative_T, int &l); 58 | void slideWindow(); 59 | void solveOdometry(); 60 | void slideWindowNew(); 61 | void slideWindowOld(); 62 | void optimization(); 63 | void optimizationStereo(); 64 | void optimizationStereoWithCorrection(); 65 | void vector2double(); 66 | void double2vector(); 67 | bool failureDetection(); 68 | 69 | void showStatus(); 70 | 71 | enum SolverFlag 72 | { 73 | INITIAL, 74 | NON_LINEAR 75 | }; 76 | 77 | enum MarginalizationFlag 78 | { 79 | MARGIN_OLD = 0, 80 | MARGIN_SECOND_NEW = 1 81 | }; 82 | 83 | SolverFlag solver_flag; 84 | MarginalizationFlag marginalization_flag; 85 | Vector3d g; 86 | MatrixXd Ap[2], backup_A; 87 | VectorXd bp[2], backup_b; 88 | 89 | Matrix3d ric[NUM_OF_CAM]; 90 | Vector3d tic[NUM_OF_CAM]; 91 | vector m_camera; 92 | 93 | Vector3d Ps[(WINDOW_SIZE + 1)]; 94 | Vector3d Vs[(WINDOW_SIZE + 1)]; 95 | Matrix3d Rs[(WINDOW_SIZE + 1)]; 96 | Vector3d Bas[(WINDOW_SIZE + 1)]; 97 | Vector3d Bgs[(WINDOW_SIZE + 1)]; 98 | double td; 99 | 100 | Matrix3d back_R0, last_R, last_R0; 101 | Vector3d back_P0, last_P, last_P0; 102 | std_msgs::Header Headers[(WINDOW_SIZE + 1)]; 103 | 104 | IntegrationBase *pre_integrations[(WINDOW_SIZE + 1)]; 105 | Vector3d acc_0, gyr_0; 106 | 107 | vector dt_buf[(WINDOW_SIZE + 1)]; 108 | vector linear_acceleration_buf[(WINDOW_SIZE + 1)]; 109 | vector angular_velocity_buf[(WINDOW_SIZE + 1)]; 110 | 111 | int frame_count; 112 | int sum_of_outlier, sum_of_back, sum_of_front, sum_of_invalid; 113 | 114 | FeatureManager f_manager; 115 | MotionEstimator m_estimator; 116 | InitialEXRotation initial_ex_rotation; 117 | 118 | bool first_imu; 119 | bool is_valid, is_key; 120 | bool failure_occur; 121 | 122 | vector point_cloud; 123 | vector margin_cloud; 124 | vector key_poses; 125 | double initial_timestamp; 126 | 127 | 128 | double para_Pose[WINDOW_SIZE + 1][SIZE_POSE]; 129 | double para_SpeedBias[WINDOW_SIZE + 1][SIZE_SPEEDBIAS]; 130 | double para_Feature[NUM_OF_F][SIZE_FEATURE]; 131 | double para_Ex_Pose[NUM_OF_CAM][SIZE_POSE]; 132 | double para_Retrive_Pose[SIZE_POSE]; 133 | double para_Td[1][1]; 134 | double para_Tr[1][1]; 135 | 136 | int loop_window_index; 137 | 138 | MarginalizationInfo *last_marginalization_info; 139 | vector last_marginalization_parameter_blocks; 140 | 141 | map all_image_frame; 142 | IntegrationBase *tmp_pre_integration; 143 | 144 | //relocalization variable 145 | bool relocalization_info; 146 | double relo_frame_stamp; 147 | double relo_frame_index; 148 | int relo_frame_local_index; 149 | vector match_points; 150 | double relo_Pose[SIZE_POSE]; 151 | Matrix3d drift_correct_r; 152 | Vector3d drift_correct_t; 153 | Vector3d prev_relo_t; 154 | Matrix3d prev_relo_r; 155 | Vector3d relo_relative_t; 156 | Quaterniond relo_relative_q; 157 | double relo_relative_yaw; 158 | }; 159 | -------------------------------------------------------------------------------- /vins_estimator/src/factor/imu_factor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | 6 | #include "../utility/utility.h" 7 | #include "../parameters.h" 8 | #include "integration_base.h" 9 | 10 | #include 11 | 12 | class IMUFactor : public ceres::SizedCostFunction<15, 7, 9, 7, 9> 13 | { 14 | public: 15 | IMUFactor() = delete; 16 | IMUFactor(IntegrationBase* _pre_integration):pre_integration(_pre_integration) 17 | { 18 | } 19 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const 20 | { 21 | 22 | Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]); 23 | Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]); 24 | 25 | Eigen::Vector3d Vi(parameters[1][0], parameters[1][1], parameters[1][2]); 26 | Eigen::Vector3d Bai(parameters[1][3], parameters[1][4], parameters[1][5]); 27 | Eigen::Vector3d Bgi(parameters[1][6], parameters[1][7], parameters[1][8]); 28 | 29 | Eigen::Vector3d Pj(parameters[2][0], parameters[2][1], parameters[2][2]); 30 | Eigen::Quaterniond Qj(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]); 31 | 32 | Eigen::Vector3d Vj(parameters[3][0], parameters[3][1], parameters[3][2]); 33 | Eigen::Vector3d Baj(parameters[3][3], parameters[3][4], parameters[3][5]); 34 | Eigen::Vector3d Bgj(parameters[3][6], parameters[3][7], parameters[3][8]); 35 | 36 | //Eigen::Matrix Fd; 37 | //Eigen::Matrix Gd; 38 | 39 | //Eigen::Vector3d pPj = Pi + Vi * sum_t - 0.5 * g * sum_t * sum_t + corrected_delta_p; 40 | //Eigen::Quaterniond pQj = Qi * delta_q; 41 | //Eigen::Vector3d pVj = Vi - g * sum_t + corrected_delta_v; 42 | //Eigen::Vector3d pBaj = Bai; 43 | //Eigen::Vector3d pBgj = Bgi; 44 | 45 | //Vi + Qi * delta_v - g * sum_dt = Vj; 46 | //Qi * delta_q = Qj; 47 | 48 | //delta_p = Qi.inverse() * (0.5 * g * sum_dt * sum_dt + Pj - Pi); 49 | //delta_v = Qi.inverse() * (g * sum_dt + Vj - Vi); 50 | //delta_q = Qi.inverse() * Qj; 51 | 52 | #if 0 53 | if ((Bai - pre_integration->linearized_ba).norm() > 0.10 || 54 | (Bgi - pre_integration->linearized_bg).norm() > 0.01) 55 | { 56 | pre_integration->repropagate(Bai, Bgi); 57 | } 58 | #endif 59 | 60 | Eigen::Map> residual(residuals); 61 | residual = pre_integration->evaluate(Pi, Qi, Vi, Bai, Bgi, 62 | Pj, Qj, Vj, Baj, Bgj); 63 | 64 | Eigen::Matrix sqrt_info = Eigen::LLT>(pre_integration->covariance.inverse()).matrixL().transpose(); 65 | //sqrt_info.setIdentity(); 66 | residual = sqrt_info * residual; 67 | 68 | if (jacobians) 69 | { 70 | double sum_dt = pre_integration->sum_dt; 71 | Eigen::Matrix3d dp_dba = pre_integration->jacobian.template block<3, 3>(O_P, O_BA); 72 | Eigen::Matrix3d dp_dbg = pre_integration->jacobian.template block<3, 3>(O_P, O_BG); 73 | 74 | Eigen::Matrix3d dq_dbg = pre_integration->jacobian.template block<3, 3>(O_R, O_BG); 75 | 76 | Eigen::Matrix3d dv_dba = pre_integration->jacobian.template block<3, 3>(O_V, O_BA); 77 | Eigen::Matrix3d dv_dbg = pre_integration->jacobian.template block<3, 3>(O_V, O_BG); 78 | 79 | if (pre_integration->jacobian.maxCoeff() > 1e8 || pre_integration->jacobian.minCoeff() < -1e8) 80 | { 81 | ROS_WARN("numerical unstable in preintegration"); 82 | //std::cout << pre_integration->jacobian << std::endl; 83 | /// ROS_BREAK(); 84 | } 85 | 86 | if (jacobians[0]) 87 | { 88 | Eigen::Map> jacobian_pose_i(jacobians[0]); 89 | jacobian_pose_i.setZero(); 90 | 91 | jacobian_pose_i.block<3, 3>(O_P, O_P) = -Qi.inverse().toRotationMatrix(); 92 | jacobian_pose_i.block<3, 3>(O_P, O_R) = Utility::skewSymmetric(Qi.inverse() * (0.5 * G * sum_dt * sum_dt + Pj - Pi - Vi * sum_dt)); 93 | 94 | #if 0 95 | jacobian_pose_i.block<3, 3>(O_R, O_R) = -(Qj.inverse() * Qi).toRotationMatrix(); 96 | #else 97 | Eigen::Quaterniond corrected_delta_q = pre_integration->delta_q * Utility::deltaQ(dq_dbg * (Bgi - pre_integration->linearized_bg)); 98 | jacobian_pose_i.block<3, 3>(O_R, O_R) = -(Utility::Qleft(Qj.inverse() * Qi) * Utility::Qright(corrected_delta_q)).bottomRightCorner<3, 3>(); 99 | #endif 100 | 101 | jacobian_pose_i.block<3, 3>(O_V, O_R) = Utility::skewSymmetric(Qi.inverse() * (G * sum_dt + Vj - Vi)); 102 | 103 | jacobian_pose_i = sqrt_info * jacobian_pose_i; 104 | 105 | if (jacobian_pose_i.maxCoeff() > 1e8 || jacobian_pose_i.minCoeff() < -1e8) 106 | { 107 | ROS_WARN("numerical unstable in preintegration"); 108 | //std::cout << sqrt_info << std::endl; 109 | //ROS_BREAK(); 110 | } 111 | } 112 | if (jacobians[1]) 113 | { 114 | Eigen::Map> jacobian_speedbias_i(jacobians[1]); 115 | jacobian_speedbias_i.setZero(); 116 | jacobian_speedbias_i.block<3, 3>(O_P, O_V - O_V) = -Qi.inverse().toRotationMatrix() * sum_dt; 117 | jacobian_speedbias_i.block<3, 3>(O_P, O_BA - O_V) = -dp_dba; 118 | jacobian_speedbias_i.block<3, 3>(O_P, O_BG - O_V) = -dp_dbg; 119 | 120 | #if 0 121 | jacobian_speedbias_i.block<3, 3>(O_R, O_BG - O_V) = -dq_dbg; 122 | #else 123 | //Eigen::Quaterniond corrected_delta_q = pre_integration->delta_q * Utility::deltaQ(dq_dbg * (Bgi - pre_integration->linearized_bg)); 124 | //jacobian_speedbias_i.block<3, 3>(O_R, O_BG - O_V) = -Utility::Qleft(Qj.inverse() * Qi * corrected_delta_q).bottomRightCorner<3, 3>() * dq_dbg; 125 | jacobian_speedbias_i.block<3, 3>(O_R, O_BG - O_V) = -Utility::Qleft(Qj.inverse() * Qi * pre_integration->delta_q).bottomRightCorner<3, 3>() * dq_dbg; 126 | #endif 127 | 128 | jacobian_speedbias_i.block<3, 3>(O_V, O_V - O_V) = -Qi.inverse().toRotationMatrix(); 129 | jacobian_speedbias_i.block<3, 3>(O_V, O_BA - O_V) = -dv_dba; 130 | jacobian_speedbias_i.block<3, 3>(O_V, O_BG - O_V) = -dv_dbg; 131 | 132 | jacobian_speedbias_i.block<3, 3>(O_BA, O_BA - O_V) = -Eigen::Matrix3d::Identity(); 133 | 134 | jacobian_speedbias_i.block<3, 3>(O_BG, O_BG - O_V) = -Eigen::Matrix3d::Identity(); 135 | 136 | jacobian_speedbias_i = sqrt_info * jacobian_speedbias_i; 137 | 138 | //ROS_ASSERT(fabs(jacobian_speedbias_i.maxCoeff()) < 1e8); 139 | //ROS_ASSERT(fabs(jacobian_speedbias_i.minCoeff()) < 1e8); 140 | } 141 | if (jacobians[2]) 142 | { 143 | Eigen::Map> jacobian_pose_j(jacobians[2]); 144 | jacobian_pose_j.setZero(); 145 | 146 | jacobian_pose_j.block<3, 3>(O_P, O_P) = Qi.inverse().toRotationMatrix(); 147 | 148 | #if 0 149 | jacobian_pose_j.block<3, 3>(O_R, O_R) = Eigen::Matrix3d::Identity(); 150 | #else 151 | Eigen::Quaterniond corrected_delta_q = pre_integration->delta_q * Utility::deltaQ(dq_dbg * (Bgi - pre_integration->linearized_bg)); 152 | jacobian_pose_j.block<3, 3>(O_R, O_R) = Utility::Qleft(corrected_delta_q.inverse() * Qi.inverse() * Qj).bottomRightCorner<3, 3>(); 153 | #endif 154 | 155 | jacobian_pose_j = sqrt_info * jacobian_pose_j; 156 | 157 | //ROS_ASSERT(fabs(jacobian_pose_j.maxCoeff()) < 1e8); 158 | //ROS_ASSERT(fabs(jacobian_pose_j.minCoeff()) < 1e8); 159 | } 160 | if (jacobians[3]) 161 | { 162 | Eigen::Map> jacobian_speedbias_j(jacobians[3]); 163 | jacobian_speedbias_j.setZero(); 164 | 165 | jacobian_speedbias_j.block<3, 3>(O_V, O_V - O_V) = Qi.inverse().toRotationMatrix(); 166 | 167 | jacobian_speedbias_j.block<3, 3>(O_BA, O_BA - O_V) = Eigen::Matrix3d::Identity(); 168 | 169 | jacobian_speedbias_j.block<3, 3>(O_BG, O_BG - O_V) = Eigen::Matrix3d::Identity(); 170 | 171 | jacobian_speedbias_j = sqrt_info * jacobian_speedbias_j; 172 | 173 | //ROS_ASSERT(fabs(jacobian_speedbias_j.maxCoeff()) < 1e8); 174 | //ROS_ASSERT(fabs(jacobian_speedbias_j.minCoeff()) < 1e8); 175 | } 176 | } 177 | 178 | return true; 179 | } 180 | 181 | //bool Evaluate_Direct(double const *const *parameters, Eigen::Matrix &residuals, Eigen::Matrix &jacobians); 182 | 183 | //void checkCorrection(); 184 | //void checkTransition(); 185 | //void checkJacobian(double **parameters); 186 | IntegrationBase* pre_integration; 187 | 188 | }; 189 | 190 | -------------------------------------------------------------------------------- /vins_estimator/src/factor/marginalization_factor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include "../utility/utility.h" 11 | #include "../utility/tic_toc.h" 12 | 13 | const int NUM_THREADS = 4; 14 | 15 | struct ResidualBlockInfo 16 | { 17 | ResidualBlockInfo(ceres::CostFunction *_cost_function, ceres::LossFunction *_loss_function, std::vector _parameter_blocks, std::vector _drop_set) 18 | : cost_function(_cost_function), loss_function(_loss_function), parameter_blocks(_parameter_blocks), drop_set(_drop_set) {} 19 | 20 | void Evaluate(); 21 | 22 | ceres::CostFunction *cost_function; 23 | ceres::LossFunction *loss_function; 24 | std::vector parameter_blocks; 25 | std::vector drop_set; 26 | 27 | double **raw_jacobians; 28 | std::vector> jacobians; 29 | Eigen::VectorXd residuals; 30 | 31 | int localSize(int size) 32 | { 33 | return size == 7 ? 6 : size; 34 | } 35 | }; 36 | 37 | struct ThreadsStruct 38 | { 39 | std::vector sub_factors; 40 | Eigen::MatrixXd A; 41 | Eigen::VectorXd b; 42 | std::unordered_map parameter_block_size; //global size 43 | std::unordered_map parameter_block_idx; //local size 44 | }; 45 | 46 | class MarginalizationInfo 47 | { 48 | public: 49 | ~MarginalizationInfo(); 50 | int localSize(int size) const; 51 | int globalSize(int size) const; 52 | void addResidualBlockInfo(ResidualBlockInfo *residual_block_info); 53 | void preMarginalize(); 54 | void marginalize(); 55 | std::vector getParameterBlocks(std::unordered_map &addr_shift); 56 | 57 | std::vector factors; 58 | int m, n; 59 | std::unordered_map parameter_block_size; //global size 60 | int sum_block_size; 61 | std::unordered_map parameter_block_idx; //local size 62 | std::unordered_map parameter_block_data; 63 | 64 | std::vector keep_block_size; //global size 65 | std::vector keep_block_idx; //local size 66 | std::vector keep_block_data; 67 | 68 | Eigen::MatrixXd linearized_jacobians; 69 | Eigen::VectorXd linearized_residuals; 70 | const double eps = 1e-8; 71 | 72 | }; 73 | 74 | class MarginalizationFactor : public ceres::CostFunction 75 | { 76 | public: 77 | MarginalizationFactor(MarginalizationInfo* _marginalization_info); 78 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 79 | 80 | MarginalizationInfo* marginalization_info; 81 | }; 82 | -------------------------------------------------------------------------------- /vins_estimator/src/factor/pose_local_parameterization.cpp: -------------------------------------------------------------------------------- 1 | #include "pose_local_parameterization.h" 2 | 3 | bool PoseLocalParameterization::Plus(const double *x, const double *delta, double *x_plus_delta) const 4 | { 5 | Eigen::Map _p(x); 6 | Eigen::Map _q(x + 3); 7 | 8 | Eigen::Map dp(delta); 9 | 10 | Eigen::Quaterniond dq = Utility::deltaQ(Eigen::Map(delta + 3)); 11 | 12 | Eigen::Map p(x_plus_delta); 13 | Eigen::Map q(x_plus_delta + 3); 14 | 15 | p = _p + dp; 16 | q = (_q * dq).normalized(); 17 | 18 | return true; 19 | } 20 | bool PoseLocalParameterization::ComputeJacobian(const double *x, double *jacobian) const 21 | { 22 | Eigen::Map> j(jacobian); 23 | j.topRows<6>().setIdentity(); 24 | j.bottomRows<1>().setZero(); 25 | 26 | return true; 27 | } 28 | -------------------------------------------------------------------------------- /vins_estimator/src/factor/pose_local_parameterization.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include "../utility/utility.h" 6 | 7 | class PoseLocalParameterization : public ceres::LocalParameterization 8 | { 9 | virtual bool Plus(const double *x, const double *delta, double *x_plus_delta) const; 10 | virtual bool ComputeJacobian(const double *x, double *jacobian) const; 11 | virtual int GlobalSize() const { return 7; }; 12 | virtual int LocalSize() const { return 6; }; 13 | }; 14 | -------------------------------------------------------------------------------- /vins_estimator/src/factor/projectionOneFrameTwoCamFactor.cpp: -------------------------------------------------------------------------------- 1 | /******************************************************* 2 | * Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science and Technology 3 | * 4 | * This file is part of VINS. 5 | * 6 | * Licensed under the GNU General Public License v3.0; 7 | * you may not use this file except in compliance with the License. 8 | * 9 | * Author: Qin Tong (qintonguav@gmail.com) 10 | *******************************************************/ 11 | 12 | #include "projectionOneFrameTwoCamFactor.h" 13 | 14 | // Eigen::Matrix2d ProjectionOneFrameTwoCamFactor::sqrt_info; 15 | double ProjectionOneFrameTwoCamFactor::sum_t; 16 | 17 | ProjectionOneFrameTwoCamFactor::ProjectionOneFrameTwoCamFactor(const Eigen::Vector3d &_pts_i, const Eigen::Vector3d &_pts_j, 18 | const Eigen::Vector2d &_velocity_i, const Eigen::Vector2d &_velocity_j, 19 | const double _td_i, const double _td_j) : 20 | pts_i(_pts_i), pts_j(_pts_j), 21 | td_i(_td_i), td_j(_td_j) 22 | { 23 | velocity_i.x() = _velocity_i.x(); 24 | velocity_i.y() = _velocity_i.y(); 25 | velocity_i.z() = 0; 26 | velocity_j.x() = _velocity_j.x(); 27 | velocity_j.y() = _velocity_j.y(); 28 | velocity_j.z() = 0; 29 | sqrt_info = FOCAL_LENGTH / 1.5 * Eigen::Matrix2d::Identity(); 30 | #ifdef UNIT_SPHERE_ERROR 31 | Eigen::Vector3d b1, b2; 32 | Eigen::Vector3d a = pts_j.normalized(); 33 | Eigen::Vector3d tmp(0, 0, 1); 34 | if(a == tmp) 35 | tmp << 1, 0, 0; 36 | b1 = (tmp - a * (a.transpose() * tmp)).normalized(); 37 | b2 = a.cross(b1); 38 | tangent_base.block<1, 3>(0, 0) = b1.transpose(); 39 | tangent_base.block<1, 3>(1, 0) = b2.transpose(); 40 | #endif 41 | }; 42 | 43 | 44 | void ProjectionOneFrameTwoCamFactor::setInfoMatrix(Eigen::Matrix2d& OMEGA){ 45 | sqrt_info = OMEGA ; 46 | } 47 | 48 | bool ProjectionOneFrameTwoCamFactor::debug(double const *const *parameters, double *residuals, double **jacobians) const 49 | { 50 | TicToc tic_toc; 51 | 52 | Eigen::Vector3d tic(parameters[0][0], parameters[0][1], parameters[0][2]); 53 | Eigen::Quaterniond qic(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]); 54 | 55 | Eigen::Vector3d tic2(parameters[1][0], parameters[1][1], parameters[1][2]); 56 | Eigen::Quaterniond qic2(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]); 57 | 58 | double inv_dep_i = parameters[2][0]; 59 | 60 | double td = parameters[3][0]; 61 | 62 | Eigen::Vector3d pts_i_td, pts_j_td; 63 | pts_i_td = pts_i - (td - td_i) * velocity_i; 64 | pts_j_td = pts_j - (td - td_j) * velocity_j; 65 | 66 | Eigen::Vector3d pts_camera_i = pts_i_td / inv_dep_i; 67 | Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic; 68 | Eigen::Vector3d pts_imu_j = pts_imu_i; 69 | Eigen::Vector3d pts_camera_j = qic2.inverse() * (pts_imu_j - tic2); 70 | Eigen::Map residual(residuals); 71 | 72 | #ifdef UNIT_SPHERE_ERROR 73 | residual = tangent_base * (pts_camera_j.normalized() - pts_j_td.normalized()); 74 | #else 75 | double dep_j = pts_camera_j.z(); 76 | residual = (pts_camera_j / dep_j).head<2>() - pts_j_td.head<2>(); 77 | 78 | std::cout<<"pts_camera_j: "< residual(residuals); 113 | 114 | #ifdef UNIT_SPHERE_ERROR 115 | residual = tangent_base * (pts_camera_j.normalized() - pts_j_td.normalized()); 116 | #else 117 | double dep_j = pts_camera_j.z(); 118 | residual = (pts_camera_j / dep_j).head<2>() - pts_j_td.head<2>(); 119 | #endif 120 | 121 | residual = sqrt_info * residual; 122 | 123 | if (jacobians) 124 | { 125 | Eigen::Matrix3d ric = qic.toRotationMatrix(); 126 | Eigen::Matrix3d ric2 = qic2.toRotationMatrix(); 127 | Eigen::Matrix reduce(2, 3); 128 | #ifdef UNIT_SPHERE_ERROR 129 | double norm = pts_camera_j.norm(); 130 | Eigen::Matrix3d norm_jaco; 131 | double x1, x2, x3; 132 | x1 = pts_camera_j(0); 133 | x2 = pts_camera_j(1); 134 | x3 = pts_camera_j(2); 135 | norm_jaco << 1.0 / norm - x1 * x1 / pow(norm, 3), - x1 * x2 / pow(norm, 3), - x1 * x3 / pow(norm, 3), 136 | - x1 * x2 / pow(norm, 3), 1.0 / norm - x2 * x2 / pow(norm, 3), - x2 * x3 / pow(norm, 3), 137 | - x1 * x3 / pow(norm, 3), - x2 * x3 / pow(norm, 3), 1.0 / norm - x3 * x3 / pow(norm, 3); 138 | reduce = tangent_base * norm_jaco; 139 | #else 140 | reduce << 1. / dep_j, 0, -pts_camera_j(0) / (dep_j * dep_j), 141 | 0, 1. / dep_j, -pts_camera_j(1) / (dep_j * dep_j); 142 | #endif 143 | reduce = sqrt_info * reduce; 144 | 145 | if (jacobians[0]) 146 | { 147 | Eigen::Map> jacobian_ex_pose(jacobians[0]); 148 | Eigen::Matrix jaco_ex; 149 | jaco_ex.leftCols<3>() = ric2.transpose(); 150 | jaco_ex.rightCols<3>() = ric2.transpose() * ric * -Utility::skewSymmetric(pts_camera_i); 151 | jacobian_ex_pose.leftCols<6>() = reduce * jaco_ex; 152 | jacobian_ex_pose.rightCols<1>().setZero(); 153 | } 154 | if (jacobians[1]) 155 | { 156 | Eigen::Map> jacobian_ex_pose1(jacobians[1]); 157 | Eigen::Matrix jaco_ex; 158 | jaco_ex.leftCols<3>() = - ric2.transpose(); 159 | jaco_ex.rightCols<3>() = Utility::skewSymmetric(pts_camera_j); 160 | jacobian_ex_pose1.leftCols<6>() = reduce * jaco_ex; 161 | jacobian_ex_pose1.rightCols<1>().setZero(); 162 | } 163 | if (jacobians[2]) 164 | { 165 | Eigen::Map jacobian_feature(jacobians[2]); 166 | #if 1 167 | jacobian_feature = reduce * ric2.transpose() * ric * pts_i * -1.0 / (inv_dep_i * inv_dep_i); 168 | #else 169 | jacobian_feature = reduce * ric.transpose() * Rj.transpose() * Ri * ric * pts_i; 170 | #endif 171 | } 172 | if (jacobians[3]) 173 | { 174 | Eigen::Map jacobian_td(jacobians[3]); 175 | jacobian_td = reduce * ric2.transpose() * ric * velocity_i / inv_dep_i * -1.0 + 176 | sqrt_info * velocity_j.head(2); 177 | } 178 | } 179 | sum_t += tic_toc.toc(); 180 | 181 | return true; 182 | } 183 | 184 | void ProjectionOneFrameTwoCamFactor::check(double **parameters) 185 | { 186 | double *res = new double[15]; 187 | double **jaco = new double *[4]; 188 | jaco[0] = new double[2 * 7]; 189 | jaco[1] = new double[2 * 7]; 190 | jaco[2] = new double[2 * 1]; 191 | jaco[3] = new double[2 * 1]; 192 | Evaluate(parameters, res, jaco); 193 | puts("check begins"); 194 | 195 | puts("my"); 196 | 197 | std::cout << Eigen::Map>(res).transpose() << std::endl 198 | << std::endl; 199 | std::cout << Eigen::Map>(jaco[0]) << std::endl 200 | << std::endl; 201 | std::cout << Eigen::Map>(jaco[1]) << std::endl 202 | << std::endl; 203 | std::cout << Eigen::Map(jaco[2]) << std::endl 204 | << std::endl; 205 | std::cout << Eigen::Map(jaco[3]) << std::endl 206 | << std::endl; 207 | 208 | Eigen::Vector3d tic(parameters[0][0], parameters[0][1], parameters[0][2]); 209 | Eigen::Quaterniond qic(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]); 210 | 211 | Eigen::Vector3d tic2(parameters[1][0], parameters[1][1], parameters[1][2]); 212 | Eigen::Quaterniond qic2(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]); 213 | 214 | double inv_dep_i = parameters[2][0]; 215 | 216 | double td = parameters[3][0]; 217 | 218 | Eigen::Vector3d pts_i_td, pts_j_td; 219 | pts_i_td = pts_i - (td - td_i) * velocity_i; 220 | pts_j_td = pts_j - (td - td_j) * velocity_j; 221 | 222 | Eigen::Vector3d pts_camera_i = pts_i_td / inv_dep_i; 223 | Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic; 224 | Eigen::Vector3d pts_imu_j = pts_imu_i; 225 | Eigen::Vector3d pts_camera_j = qic2.inverse() * (pts_imu_j - tic2); 226 | 227 | 228 | Eigen::Vector2d residual; 229 | #ifdef UNIT_SPHERE_ERROR 230 | residual = tangent_base * (pts_camera_j.normalized() - pts_j_td.normalized()); 231 | #else 232 | double dep_j = pts_camera_j.z(); 233 | residual = (pts_camera_j / dep_j).head<2>() - pts_j_td.head<2>(); 234 | #endif 235 | residual = sqrt_info * residual; 236 | 237 | puts("num"); 238 | std::cout << residual.transpose() << std::endl; 239 | 240 | const double eps = 1e-6; 241 | Eigen::Matrix num_jacobian; 242 | for (int k = 0; k < 14; k++) 243 | { 244 | Eigen::Vector3d tic(parameters[0][0], parameters[0][1], parameters[0][2]); 245 | Eigen::Quaterniond qic(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]); 246 | 247 | Eigen::Vector3d tic2(parameters[1][0], parameters[1][1], parameters[1][2]); 248 | Eigen::Quaterniond qic2(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]); 249 | 250 | double inv_dep_i = parameters[2][0]; 251 | 252 | double td = parameters[3][0]; 253 | 254 | int a = k / 3, b = k % 3; 255 | Eigen::Vector3d delta = Eigen::Vector3d(b == 0, b == 1, b == 2) * eps; 256 | 257 | if (a == 0) 258 | tic += delta; 259 | else if (a == 1) 260 | qic = qic * Utility::deltaQ(delta); 261 | else if (a == 2) 262 | tic2 += delta; 263 | else if (a == 3) 264 | qic2 = qic2 * Utility::deltaQ(delta); 265 | else if (a == 4) 266 | { 267 | if (b == 0) 268 | inv_dep_i += delta.x(); 269 | else 270 | td += delta.y(); 271 | } 272 | 273 | Eigen::Vector3d pts_i_td, pts_j_td; 274 | pts_i_td = pts_i - (td - td_i) * velocity_i; 275 | pts_j_td = pts_j - (td - td_j) * velocity_j; 276 | 277 | Eigen::Vector3d pts_camera_i = pts_i_td / inv_dep_i; 278 | Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic; 279 | Eigen::Vector3d pts_imu_j = pts_imu_i; 280 | Eigen::Vector3d pts_camera_j = qic2.inverse() * (pts_imu_j - tic2); 281 | 282 | Eigen::Vector2d tmp_residual; 283 | #ifdef UNIT_SPHERE_ERROR 284 | tmp_residual = tangent_base * (pts_camera_j.normalized() - pts_j_td.normalized()); 285 | #else 286 | double dep_j = pts_camera_j.z(); 287 | tmp_residual = (pts_camera_j / dep_j).head<2>() - pts_j_td.head<2>(); 288 | #endif 289 | tmp_residual = sqrt_info * tmp_residual; 290 | num_jacobian.col(k) = (tmp_residual - residual) / eps; 291 | } 292 | std::cout << num_jacobian.block<2, 6>(0, 0) << std::endl; 293 | std::cout << num_jacobian.block<2, 6>(0, 6) << std::endl; 294 | std::cout << num_jacobian.block<2, 1>(0, 12) << std::endl; 295 | std::cout << num_jacobian.block<2, 1>(0, 13) << std::endl; 296 | } 297 | -------------------------------------------------------------------------------- /vins_estimator/src/factor/projectionOneFrameTwoCamFactor.h: -------------------------------------------------------------------------------- 1 | /******************************************************* 2 | * Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science and Technology 3 | * 4 | * This file is part of VINS. 5 | * 6 | * Licensed under the GNU General Public License v3.0; 7 | * you may not use this file except in compliance with the License. 8 | * 9 | * Author: Qin Tong (qintonguav@gmail.com) 10 | *******************************************************/ 11 | 12 | #pragma once 13 | 14 | #include 15 | #include 16 | #include 17 | #include "../utility/utility.h" 18 | #include "../utility/tic_toc.h" 19 | #include "../parameters.h" 20 | 21 | class ProjectionOneFrameTwoCamFactor : public ceres::SizedCostFunction<2, 7, 7, 1, 1> 22 | { 23 | public: 24 | ProjectionOneFrameTwoCamFactor(const Eigen::Vector3d &_pts_i, const Eigen::Vector3d &_pts_j, 25 | const Eigen::Vector2d &_velocity_i, const Eigen::Vector2d &_velocity_j, 26 | const double _td_i, const double _td_j); 27 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 28 | bool debug(double const *const *parameters, double *residuals, double **jacobians) const; 29 | void check(double **parameters); 30 | void setInfoMatrix(Eigen::Matrix2d& OMEGA); 31 | 32 | Eigen::Vector3d pts_i, pts_j; 33 | Eigen::Vector3d velocity_i, velocity_j; 34 | double td_i, td_j; 35 | Eigen::Matrix tangent_base; 36 | // static Eigen::Matrix2d sqrt_info; 37 | Eigen::Matrix2d sqrt_info; 38 | static double sum_t; 39 | }; 40 | -------------------------------------------------------------------------------- /vins_estimator/src/factor/projectionTwoFrameOneCamFactor.h: -------------------------------------------------------------------------------- 1 | /******************************************************* 2 | * Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science and Technology 3 | * 4 | * This file is part of VINS. 5 | * 6 | * Licensed under the GNU General Public License v3.0; 7 | * you may not use this file except in compliance with the License. 8 | * 9 | * Author: Qin Tong (qintonguav@gmail.com) 10 | *******************************************************/ 11 | 12 | #pragma once 13 | 14 | #include 15 | #include 16 | #include 17 | #include "../utility/utility.h" 18 | #include "../utility/tic_toc.h" 19 | #include "../parameters.h" 20 | 21 | class ProjectionTwoFrameOneCamFactor : public ceres::SizedCostFunction<2, 7, 7, 7, 1, 1> 22 | { 23 | public: 24 | ProjectionTwoFrameOneCamFactor(const Eigen::Vector3d &_pts_i, const Eigen::Vector3d &_pts_j, 25 | const Eigen::Vector2d &_velocity_i, const Eigen::Vector2d &_velocity_j, 26 | const double _td_i, const double _td_j); 27 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 28 | void check(double **parameters); 29 | void setInfoMatrix(Eigen::Matrix2d& ); 30 | bool debug(double const *const *parameters, double *residuals, double **jacobians) const; 31 | 32 | Eigen::Vector3d pts_i, pts_j; 33 | Eigen::Vector3d velocity_i, velocity_j; 34 | double td_i, td_j; 35 | Eigen::Matrix tangent_base; 36 | // static Eigen::Matrix2d sqrt_info; 37 | Eigen::Matrix2d sqrt_info; 38 | 39 | static double sum_t; 40 | }; 41 | -------------------------------------------------------------------------------- /vins_estimator/src/factor/projectionTwoFrameTwoCamFactor.h: -------------------------------------------------------------------------------- 1 | /******************************************************* 2 | * Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science and Technology 3 | * 4 | * This file is part of VINS. 5 | * 6 | * Licensed under the GNU General Public License v3.0; 7 | * you may not use this file except in compliance with the License. 8 | * 9 | * Author: Qin Tong (qintonguav@gmail.com) 10 | *******************************************************/ 11 | 12 | #pragma once 13 | 14 | #include 15 | #include 16 | #include 17 | #include "../utility/utility.h" 18 | #include "../utility/tic_toc.h" 19 | #include "../parameters.h" 20 | 21 | class ProjectionTwoFrameTwoCamFactor : public ceres::SizedCostFunction<2, 7, 7, 7, 7, 1, 1> 22 | { 23 | public: 24 | ProjectionTwoFrameTwoCamFactor(const Eigen::Vector3d &_pts_i, const Eigen::Vector3d &_pts_j, 25 | const Eigen::Vector2d &_velocity_i, const Eigen::Vector2d &_velocity_j, 26 | const double _td_i, const double _td_j); 27 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 28 | bool debug(double const *const *parameters, double *residuals, double **jacobians) const; 29 | void check(double **parameters); 30 | void setInfoMatrix(Eigen::Matrix2d& OMEGA); 31 | 32 | Eigen::Vector3d pts_i, pts_j; 33 | Eigen::Vector3d velocity_i, velocity_j; 34 | double td_i, td_j; 35 | Eigen::Matrix tangent_base; 36 | // static Eigen::Matrix2d sqrt_info; 37 | Eigen::Matrix2d sqrt_info; 38 | static double sum_t; 39 | }; 40 | -------------------------------------------------------------------------------- /vins_estimator/src/factor/projection_factor.cpp: -------------------------------------------------------------------------------- 1 | /******************************************************* 2 | * Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science and Technology 3 | * 4 | * This file is part of VINS. 5 | * 6 | * Licensed under the GNU General Public License v3.0; 7 | * you may not use this file except in compliance with the License. 8 | *******************************************************/ 9 | 10 | #include "projection_factor.h" 11 | 12 | Eigen::Matrix2d ProjectionFactor::sqrt_info; 13 | double ProjectionFactor::sum_t; 14 | 15 | ProjectionFactor::ProjectionFactor(const Eigen::Vector3d &_pts_i, const Eigen::Vector3d &_pts_j) : pts_i(_pts_i), pts_j(_pts_j) 16 | { 17 | #ifdef UNIT_SPHERE_ERROR 18 | Eigen::Vector3d b1, b2; 19 | Eigen::Vector3d a = pts_j.normalized(); 20 | Eigen::Vector3d tmp(0, 0, 1); 21 | if(a == tmp) 22 | tmp << 1, 0, 0; 23 | b1 = (tmp - a * (a.transpose() * tmp)).normalized(); 24 | b2 = a.cross(b1); 25 | tangent_base.block<1, 3>(0, 0) = b1.transpose(); 26 | tangent_base.block<1, 3>(1, 0) = b2.transpose(); 27 | #endif 28 | }; 29 | 30 | bool ProjectionFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const 31 | { 32 | TicToc tic_toc; 33 | Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]); 34 | Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]); 35 | 36 | Eigen::Vector3d Pj(parameters[1][0], parameters[1][1], parameters[1][2]); 37 | Eigen::Quaterniond Qj(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]); 38 | 39 | Eigen::Vector3d tic(parameters[2][0], parameters[2][1], parameters[2][2]); 40 | Eigen::Quaterniond qic(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]); 41 | 42 | double inv_dep_i = parameters[3][0]; 43 | 44 | Eigen::Vector3d pts_camera_i = pts_i / inv_dep_i; 45 | Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic; 46 | Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi; 47 | Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj); 48 | Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic); 49 | Eigen::Map residual(residuals); 50 | 51 | #ifdef UNIT_SPHERE_ERROR 52 | residual = tangent_base * (pts_camera_j.normalized() - pts_j.normalized()); 53 | #else 54 | double dep_j = pts_camera_j.z(); 55 | residual = (pts_camera_j / dep_j).head<2>() - pts_j.head<2>(); 56 | #endif 57 | 58 | residual = sqrt_info * residual; 59 | 60 | if (jacobians) 61 | { 62 | Eigen::Matrix3d Ri = Qi.toRotationMatrix(); 63 | Eigen::Matrix3d Rj = Qj.toRotationMatrix(); 64 | Eigen::Matrix3d ric = qic.toRotationMatrix(); 65 | Eigen::Matrix reduce(2, 3); 66 | #ifdef UNIT_SPHERE_ERROR 67 | double norm = pts_camera_j.norm(); 68 | Eigen::Matrix3d norm_jaco; 69 | double x1, x2, x3; 70 | x1 = pts_camera_j(0); 71 | x2 = pts_camera_j(1); 72 | x3 = pts_camera_j(2); 73 | norm_jaco << 1.0 / norm - x1 * x1 / pow(norm, 3), - x1 * x2 / pow(norm, 3), - x1 * x3 / pow(norm, 3), 74 | - x1 * x2 / pow(norm, 3), 1.0 / norm - x2 * x2 / pow(norm, 3), - x2 * x3 / pow(norm, 3), 75 | - x1 * x3 / pow(norm, 3), - x2 * x3 / pow(norm, 3), 1.0 / norm - x3 * x3 / pow(norm, 3); 76 | reduce = tangent_base * norm_jaco; 77 | #else 78 | reduce << 1. / dep_j, 0, -pts_camera_j(0) / (dep_j * dep_j), 79 | 0, 1. / dep_j, -pts_camera_j(1) / (dep_j * dep_j); 80 | #endif 81 | reduce = sqrt_info * reduce; 82 | 83 | if (jacobians[0]) 84 | { 85 | Eigen::Map> jacobian_pose_i(jacobians[0]); 86 | 87 | Eigen::Matrix jaco_i; 88 | jaco_i.leftCols<3>() = ric.transpose() * Rj.transpose(); 89 | jaco_i.rightCols<3>() = ric.transpose() * Rj.transpose() * Ri * -Utility::skewSymmetric(pts_imu_i); 90 | 91 | jacobian_pose_i.leftCols<6>() = reduce * jaco_i; 92 | jacobian_pose_i.rightCols<1>().setZero(); 93 | } 94 | 95 | if (jacobians[1]) 96 | { 97 | Eigen::Map> jacobian_pose_j(jacobians[1]); 98 | 99 | Eigen::Matrix jaco_j; 100 | jaco_j.leftCols<3>() = ric.transpose() * -Rj.transpose(); 101 | jaco_j.rightCols<3>() = ric.transpose() * Utility::skewSymmetric(pts_imu_j); 102 | 103 | jacobian_pose_j.leftCols<6>() = reduce * jaco_j; 104 | jacobian_pose_j.rightCols<1>().setZero(); 105 | } 106 | if (jacobians[2]) 107 | { 108 | Eigen::Map> jacobian_ex_pose(jacobians[2]); 109 | Eigen::Matrix jaco_ex; 110 | jaco_ex.leftCols<3>() = ric.transpose() * (Rj.transpose() * Ri - Eigen::Matrix3d::Identity()); 111 | Eigen::Matrix3d tmp_r = ric.transpose() * Rj.transpose() * Ri * ric; 112 | jaco_ex.rightCols<3>() = -tmp_r * Utility::skewSymmetric(pts_camera_i) + Utility::skewSymmetric(tmp_r * pts_camera_i) + 113 | Utility::skewSymmetric(ric.transpose() * (Rj.transpose() * (Ri * tic + Pi - Pj) - tic)); 114 | jacobian_ex_pose.leftCols<6>() = reduce * jaco_ex; 115 | jacobian_ex_pose.rightCols<1>().setZero(); 116 | } 117 | if (jacobians[3]) 118 | { 119 | Eigen::Map jacobian_feature(jacobians[3]); 120 | #if 1 121 | jacobian_feature = reduce * ric.transpose() * Rj.transpose() * Ri * ric * pts_i * -1.0 / (inv_dep_i * inv_dep_i); 122 | #else 123 | jacobian_feature = reduce * ric.transpose() * Rj.transpose() * Ri * ric * pts_i; 124 | #endif 125 | } 126 | } 127 | sum_t += tic_toc.toc(); 128 | 129 | return true; 130 | } 131 | 132 | void ProjectionFactor::check(double **parameters) 133 | { 134 | double *res = new double[15]; 135 | double **jaco = new double *[4]; 136 | jaco[0] = new double[2 * 7]; 137 | jaco[1] = new double[2 * 7]; 138 | jaco[2] = new double[2 * 7]; 139 | jaco[3] = new double[2 * 1]; 140 | Evaluate(parameters, res, jaco); 141 | puts("check begins"); 142 | 143 | puts("my"); 144 | 145 | std::cout << Eigen::Map>(res).transpose() << std::endl 146 | << std::endl; 147 | std::cout << Eigen::Map>(jaco[0]) << std::endl 148 | << std::endl; 149 | std::cout << Eigen::Map>(jaco[1]) << std::endl 150 | << std::endl; 151 | std::cout << Eigen::Map>(jaco[2]) << std::endl 152 | << std::endl; 153 | std::cout << Eigen::Map(jaco[3]) << std::endl 154 | << std::endl; 155 | 156 | Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]); 157 | Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]); 158 | 159 | Eigen::Vector3d Pj(parameters[1][0], parameters[1][1], parameters[1][2]); 160 | Eigen::Quaterniond Qj(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]); 161 | 162 | Eigen::Vector3d tic(parameters[2][0], parameters[2][1], parameters[2][2]); 163 | Eigen::Quaterniond qic(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]); 164 | double inv_dep_i = parameters[3][0]; 165 | 166 | Eigen::Vector3d pts_camera_i = pts_i / inv_dep_i; 167 | Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic; 168 | Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi; 169 | Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj); 170 | Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic); 171 | 172 | 173 | Eigen::Vector2d residual; 174 | #ifdef UNIT_SPHERE_ERROR 175 | residual = tangent_base * (pts_camera_j.normalized() - pts_j.normalized()); 176 | #else 177 | double dep_j = pts_camera_j.z(); 178 | residual = (pts_camera_j / dep_j).head<2>() - pts_j.head<2>(); 179 | #endif 180 | residual = sqrt_info * residual; 181 | 182 | puts("num"); 183 | std::cout << residual.transpose() << std::endl; 184 | 185 | const double eps = 1e-6; 186 | Eigen::Matrix num_jacobian; 187 | for (int k = 0; k < 19; k++) 188 | { 189 | Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]); 190 | Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]); 191 | 192 | Eigen::Vector3d Pj(parameters[1][0], parameters[1][1], parameters[1][2]); 193 | Eigen::Quaterniond Qj(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]); 194 | 195 | Eigen::Vector3d tic(parameters[2][0], parameters[2][1], parameters[2][2]); 196 | Eigen::Quaterniond qic(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]); 197 | double inv_dep_i = parameters[3][0]; 198 | 199 | int a = k / 3, b = k % 3; 200 | Eigen::Vector3d delta = Eigen::Vector3d(b == 0, b == 1, b == 2) * eps; 201 | 202 | if (a == 0) 203 | Pi += delta; 204 | else if (a == 1) 205 | Qi = Qi * Utility::deltaQ(delta); 206 | else if (a == 2) 207 | Pj += delta; 208 | else if (a == 3) 209 | Qj = Qj * Utility::deltaQ(delta); 210 | else if (a == 4) 211 | tic += delta; 212 | else if (a == 5) 213 | qic = qic * Utility::deltaQ(delta); 214 | else if (a == 6) 215 | inv_dep_i += delta.x(); 216 | 217 | Eigen::Vector3d pts_camera_i = pts_i / inv_dep_i; 218 | Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic; 219 | Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi; 220 | Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj); 221 | Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic); 222 | 223 | Eigen::Vector2d tmp_residual; 224 | #ifdef UNIT_SPHERE_ERROR 225 | tmp_residual = tangent_base * (pts_camera_j.normalized() - pts_j.normalized()); 226 | #else 227 | double dep_j = pts_camera_j.z(); 228 | tmp_residual = (pts_camera_j / dep_j).head<2>() - pts_j.head<2>(); 229 | #endif 230 | tmp_residual = sqrt_info * tmp_residual; 231 | num_jacobian.col(k) = (tmp_residual - residual) / eps; 232 | } 233 | std::cout << num_jacobian << std::endl; 234 | } 235 | -------------------------------------------------------------------------------- /vins_estimator/src/factor/projection_factor.h: -------------------------------------------------------------------------------- 1 | /******************************************************* 2 | * Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science and Technology 3 | * 4 | * This file is part of VINS. 5 | * 6 | * Licensed under the GNU General Public License v3.0; 7 | * you may not use this file except in compliance with the License. 8 | *******************************************************/ 9 | 10 | #pragma once 11 | 12 | #include 13 | #include 14 | #include 15 | #include "../utility/utility.h" 16 | #include "../utility/tic_toc.h" 17 | #include "../parameters.h" 18 | 19 | class ProjectionFactor : public ceres::SizedCostFunction<2, 7, 7, 7, 1> 20 | { 21 | public: 22 | ProjectionFactor(const Eigen::Vector3d &_pts_i, const Eigen::Vector3d &_pts_j); 23 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 24 | void check(double **parameters); 25 | 26 | Eigen::Vector3d pts_i, pts_j; 27 | Eigen::Matrix tangent_base; 28 | static Eigen::Matrix2d sqrt_info; 29 | static double sum_t; 30 | }; 31 | -------------------------------------------------------------------------------- /vins_estimator/src/factor/projection_td_factor.cpp: -------------------------------------------------------------------------------- 1 | #include "projection_td_factor.h" 2 | 3 | Eigen::Matrix2d ProjectionTdFactor::sqrt_info; 4 | double ProjectionTdFactor::sum_t; 5 | 6 | ProjectionTdFactor::ProjectionTdFactor(const Eigen::Vector3d &_pts_i, const Eigen::Vector3d &_pts_j, 7 | const Eigen::Vector2d &_velocity_i, const Eigen::Vector2d &_velocity_j, 8 | const double _td_i, const double _td_j, const double _row_i, const double _row_j) : 9 | pts_i(_pts_i), pts_j(_pts_j), 10 | td_i(_td_i), td_j(_td_j) 11 | { 12 | velocity_i.x() = _velocity_i.x(); 13 | velocity_i.y() = _velocity_i.y(); 14 | velocity_i.z() = 0; 15 | velocity_j.x() = _velocity_j.x(); 16 | velocity_j.y() = _velocity_j.y(); 17 | velocity_j.z() = 0; 18 | row_i = _row_i - ROW / 2; 19 | row_j = _row_j - ROW / 2; 20 | 21 | #ifdef UNIT_SPHERE_ERROR 22 | Eigen::Vector3d b1, b2; 23 | Eigen::Vector3d a = pts_j.normalized(); 24 | Eigen::Vector3d tmp(0, 0, 1); 25 | if(a == tmp) 26 | tmp << 1, 0, 0; 27 | b1 = (tmp - a * (a.transpose() * tmp)).normalized(); 28 | b2 = a.cross(b1); 29 | tangent_base.block<1, 3>(0, 0) = b1.transpose(); 30 | tangent_base.block<1, 3>(1, 0) = b2.transpose(); 31 | #endif 32 | }; 33 | 34 | bool ProjectionTdFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const 35 | { 36 | TicToc tic_toc; 37 | Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]); 38 | Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]); 39 | 40 | Eigen::Vector3d Pj(parameters[1][0], parameters[1][1], parameters[1][2]); 41 | Eigen::Quaterniond Qj(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]); 42 | 43 | Eigen::Vector3d tic(parameters[2][0], parameters[2][1], parameters[2][2]); 44 | Eigen::Quaterniond qic(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]); 45 | 46 | double inv_dep_i = parameters[3][0]; 47 | 48 | double td = parameters[4][0]; 49 | 50 | Eigen::Vector3d pts_i_td, pts_j_td; 51 | pts_i_td = pts_i - (td - td_i + TR / ROW * row_i) * velocity_i; 52 | pts_j_td = pts_j - (td - td_j + TR / ROW * row_j) * velocity_j; 53 | Eigen::Vector3d pts_camera_i = pts_i_td / inv_dep_i; 54 | Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic; 55 | Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi; 56 | Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj); 57 | Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic); 58 | Eigen::Map residual(residuals); 59 | 60 | #ifdef UNIT_SPHERE_ERROR 61 | residual = tangent_base * (pts_camera_j.normalized() - pts_j_td.normalized()); 62 | #else 63 | double dep_j = pts_camera_j.z(); 64 | residual = (pts_camera_j / dep_j).head<2>() - pts_j_td.head<2>(); 65 | #endif 66 | 67 | residual = sqrt_info * residual; 68 | 69 | if (jacobians) 70 | { 71 | Eigen::Matrix3d Ri = Qi.toRotationMatrix(); 72 | Eigen::Matrix3d Rj = Qj.toRotationMatrix(); 73 | Eigen::Matrix3d ric = qic.toRotationMatrix(); 74 | Eigen::Matrix reduce(2, 3); 75 | #ifdef UNIT_SPHERE_ERROR 76 | double norm = pts_camera_j.norm(); 77 | Eigen::Matrix3d norm_jaco; 78 | double x1, x2, x3; 79 | x1 = pts_camera_j(0); 80 | x2 = pts_camera_j(1); 81 | x3 = pts_camera_j(2); 82 | norm_jaco << 1.0 / norm - x1 * x1 / pow(norm, 3), - x1 * x2 / pow(norm, 3), - x1 * x3 / pow(norm, 3), 83 | - x1 * x2 / pow(norm, 3), 1.0 / norm - x2 * x2 / pow(norm, 3), - x2 * x3 / pow(norm, 3), 84 | - x1 * x3 / pow(norm, 3), - x2 * x3 / pow(norm, 3), 1.0 / norm - x3 * x3 / pow(norm, 3); 85 | reduce = tangent_base * norm_jaco; 86 | #else 87 | reduce << 1. / dep_j, 0, -pts_camera_j(0) / (dep_j * dep_j), 88 | 0, 1. / dep_j, -pts_camera_j(1) / (dep_j * dep_j); 89 | #endif 90 | reduce = sqrt_info * reduce; 91 | 92 | if (jacobians[0]) 93 | { 94 | Eigen::Map> jacobian_pose_i(jacobians[0]); 95 | 96 | Eigen::Matrix jaco_i; 97 | jaco_i.leftCols<3>() = ric.transpose() * Rj.transpose(); 98 | jaco_i.rightCols<3>() = ric.transpose() * Rj.transpose() * Ri * -Utility::skewSymmetric(pts_imu_i); 99 | 100 | jacobian_pose_i.leftCols<6>() = reduce * jaco_i; 101 | jacobian_pose_i.rightCols<1>().setZero(); 102 | } 103 | 104 | if (jacobians[1]) 105 | { 106 | Eigen::Map> jacobian_pose_j(jacobians[1]); 107 | 108 | Eigen::Matrix jaco_j; 109 | jaco_j.leftCols<3>() = ric.transpose() * -Rj.transpose(); 110 | jaco_j.rightCols<3>() = ric.transpose() * Utility::skewSymmetric(pts_imu_j); 111 | 112 | jacobian_pose_j.leftCols<6>() = reduce * jaco_j; 113 | jacobian_pose_j.rightCols<1>().setZero(); 114 | } 115 | if (jacobians[2]) 116 | { 117 | Eigen::Map> jacobian_ex_pose(jacobians[2]); 118 | Eigen::Matrix jaco_ex; 119 | jaco_ex.leftCols<3>() = ric.transpose() * (Rj.transpose() * Ri - Eigen::Matrix3d::Identity()); 120 | Eigen::Matrix3d tmp_r = ric.transpose() * Rj.transpose() * Ri * ric; 121 | jaco_ex.rightCols<3>() = -tmp_r * Utility::skewSymmetric(pts_camera_i) + Utility::skewSymmetric(tmp_r * pts_camera_i) + 122 | Utility::skewSymmetric(ric.transpose() * (Rj.transpose() * (Ri * tic + Pi - Pj) - tic)); 123 | jacobian_ex_pose.leftCols<6>() = reduce * jaco_ex; 124 | jacobian_ex_pose.rightCols<1>().setZero(); 125 | } 126 | if (jacobians[3]) 127 | { 128 | Eigen::Map jacobian_feature(jacobians[3]); 129 | jacobian_feature = reduce * ric.transpose() * Rj.transpose() * Ri * ric * pts_i_td * -1.0 / (inv_dep_i * inv_dep_i); 130 | } 131 | if (jacobians[4]) 132 | { 133 | Eigen::Map jacobian_td(jacobians[4]); 134 | jacobian_td = reduce * ric.transpose() * Rj.transpose() * Ri * ric * velocity_i / inv_dep_i * -1.0 + 135 | sqrt_info * velocity_j.head(2); 136 | } 137 | } 138 | sum_t += tic_toc.toc(); 139 | 140 | return true; 141 | } 142 | 143 | void ProjectionTdFactor::check(double **parameters) 144 | { 145 | double *res = new double[2]; 146 | double **jaco = new double *[5]; 147 | jaco[0] = new double[2 * 7]; 148 | jaco[1] = new double[2 * 7]; 149 | jaco[2] = new double[2 * 7]; 150 | jaco[3] = new double[2 * 1]; 151 | jaco[4] = new double[2 * 1]; 152 | Evaluate(parameters, res, jaco); 153 | puts("check begins"); 154 | 155 | puts("my"); 156 | 157 | std::cout << Eigen::Map>(res).transpose() << std::endl 158 | << std::endl; 159 | std::cout << Eigen::Map>(jaco[0]) << std::endl 160 | << std::endl; 161 | std::cout << Eigen::Map>(jaco[1]) << std::endl 162 | << std::endl; 163 | std::cout << Eigen::Map>(jaco[2]) << std::endl 164 | << std::endl; 165 | std::cout << Eigen::Map(jaco[3]) << std::endl 166 | << std::endl; 167 | std::cout << Eigen::Map(jaco[4]) << std::endl 168 | << std::endl; 169 | 170 | Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]); 171 | Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]); 172 | 173 | Eigen::Vector3d Pj(parameters[1][0], parameters[1][1], parameters[1][2]); 174 | Eigen::Quaterniond Qj(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]); 175 | 176 | Eigen::Vector3d tic(parameters[2][0], parameters[2][1], parameters[2][2]); 177 | Eigen::Quaterniond qic(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]); 178 | double inv_dep_i = parameters[3][0]; 179 | double td = parameters[4][0]; 180 | 181 | Eigen::Vector3d pts_i_td, pts_j_td; 182 | pts_i_td = pts_i - (td - td_i + TR / ROW * row_i) * velocity_i; 183 | pts_j_td = pts_j - (td - td_j + TR / ROW * row_j) * velocity_j; 184 | Eigen::Vector3d pts_camera_i = pts_i_td / inv_dep_i; 185 | Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic; 186 | Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi; 187 | Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj); 188 | Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic); 189 | Eigen::Vector2d residual; 190 | 191 | #ifdef UNIT_SPHERE_ERROR 192 | residual = tangent_base * (pts_camera_j.normalized() - pts_j_td.normalized()); 193 | #else 194 | double dep_j = pts_camera_j.z(); 195 | residual = (pts_camera_j / dep_j).head<2>() - pts_j_td.head<2>(); 196 | #endif 197 | residual = sqrt_info * residual; 198 | 199 | puts("num"); 200 | std::cout << residual.transpose() << std::endl; 201 | 202 | const double eps = 1e-6; 203 | Eigen::Matrix num_jacobian; 204 | for (int k = 0; k < 20; k++) 205 | { 206 | Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]); 207 | Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]); 208 | 209 | Eigen::Vector3d Pj(parameters[1][0], parameters[1][1], parameters[1][2]); 210 | Eigen::Quaterniond Qj(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]); 211 | 212 | Eigen::Vector3d tic(parameters[2][0], parameters[2][1], parameters[2][2]); 213 | Eigen::Quaterniond qic(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]); 214 | double inv_dep_i = parameters[3][0]; 215 | double td = parameters[4][0]; 216 | 217 | 218 | int a = k / 3, b = k % 3; 219 | Eigen::Vector3d delta = Eigen::Vector3d(b == 0, b == 1, b == 2) * eps; 220 | 221 | if (a == 0) 222 | Pi += delta; 223 | else if (a == 1) 224 | Qi = Qi * Utility::deltaQ(delta); 225 | else if (a == 2) 226 | Pj += delta; 227 | else if (a == 3) 228 | Qj = Qj * Utility::deltaQ(delta); 229 | else if (a == 4) 230 | tic += delta; 231 | else if (a == 5) 232 | qic = qic * Utility::deltaQ(delta); 233 | else if (a == 6 && b == 0) 234 | inv_dep_i += delta.x(); 235 | else if (a == 6 && b == 1) 236 | td += delta.y(); 237 | 238 | Eigen::Vector3d pts_i_td, pts_j_td; 239 | pts_i_td = pts_i - (td - td_i + TR / ROW * row_i) * velocity_i; 240 | pts_j_td = pts_j - (td - td_j + TR / ROW * row_j) * velocity_j; 241 | Eigen::Vector3d pts_camera_i = pts_i_td / inv_dep_i; 242 | Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic; 243 | Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi; 244 | Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj); 245 | Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic); 246 | Eigen::Vector2d tmp_residual; 247 | 248 | #ifdef UNIT_SPHERE_ERROR 249 | tmp_residual = tangent_base * (pts_camera_j.normalized() - pts_j_td.normalized()); 250 | #else 251 | double dep_j = pts_camera_j.z(); 252 | tmp_residual = (pts_camera_j / dep_j).head<2>() - pts_j_td.head<2>(); 253 | #endif 254 | tmp_residual = sqrt_info * tmp_residual; 255 | 256 | num_jacobian.col(k) = (tmp_residual - residual) / eps; 257 | } 258 | std::cout << num_jacobian << std::endl; 259 | } 260 | -------------------------------------------------------------------------------- /vins_estimator/src/factor/projection_td_factor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include "../utility/utility.h" 7 | #include "../utility/tic_toc.h" 8 | #include "../parameters.h" 9 | 10 | class ProjectionTdFactor : public ceres::SizedCostFunction<2, 7, 7, 7, 1, 1> 11 | { 12 | public: 13 | ProjectionTdFactor(const Eigen::Vector3d &_pts_i, const Eigen::Vector3d &_pts_j, 14 | const Eigen::Vector2d &_velocity_i, const Eigen::Vector2d &_velocity_j, 15 | const double _td_i, const double _td_j, const double _row_i, const double _row_j); 16 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 17 | void check(double **parameters); 18 | 19 | Eigen::Vector3d pts_i, pts_j; 20 | Eigen::Vector3d velocity_i, velocity_j; 21 | double td_i, td_j; 22 | Eigen::Matrix tangent_base; 23 | double row_i, row_j; 24 | static Eigen::Matrix2d sqrt_info; 25 | static double sum_t; 26 | }; 27 | -------------------------------------------------------------------------------- /vins_estimator/src/factor/sampson_factor.h: -------------------------------------------------------------------------------- 1 | /* 2 | Aug. 21, 2019, He Zhang, hzhang8@vcu.edu 3 | 4 | sampson approximation to the geometric distance 5 | 6 | */ 7 | 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | #include "../utility/utility.h" 14 | #include "../utility/tic_toc.h" 15 | 16 | /* 17 | struct SampsonCostFunctor 18 | { 19 | public: 20 | SampsonCostFunctor(const Eigen::Vector3d &_pts_i, const Eigen::Vector3d &_pts_j, 21 | const Eigen::Matrix4d& _sqrt_info): 22 | pts_i_(_pts_i), pts_j_(_pts_j), sqrt_info(_sqrt_info){} 23 | 24 | template 25 | bool operator()( T const *const *parameters, T *residuals) const{ 26 | 27 | typename Eigen::Matrix Pi(parameters[0][0], parameters[0][1], parameters[0][2]); 28 | typename Eigen::Quaternion Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]); 29 | 30 | typename Eigen::Matrix Pj(parameters[1][0], parameters[1][1], parameters[1][2]); 31 | typename Eigen::Quaternion Qj(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]); 32 | 33 | typename Eigen::Matrix tic(parameters[2][0], parameters[2][1], parameters[2][2]); 34 | typename Eigen::Quaternion qic(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]); 35 | 36 | T inv_dep_i = parameters[3][0]; 37 | typename Eigen::Matrix pts_i = pts_i_.cast(); 38 | typename Eigen::Matrix pts_j = pts_j_.cast(); 39 | 40 | typename Eigen::Matrix pts_camera_i = pts_i / inv_dep_i; 41 | typename Eigen::Matrix pts_imu_i = qic * pts_camera_i + tic; 42 | typename Eigen::Matrix pts_w = Qi * pts_imu_i + Pi; 43 | typename Eigen::Matrix pts_imu_j = Qj.inverse() * (pts_w - Pj); 44 | typename Eigen::Matrix pts_camera_j = qic.inverse() * (pts_imu_j - tic); 45 | 46 | T dep_j = pts_camera_j.z(); 47 | typename Eigen::Matrix epsilon; 48 | epsilon(0) = (pts_camera_j(0) / dep_j) - pts_j(0); 49 | epsilon(1) = (pts_camera_j(1) / dep_j) - pts_j(1); 50 | 51 | // compute J = d_epsilon_d_X, [2x4], where X = (pi_x, pi_y, pj_x, pj_y) 52 | typename Eigen::Matrix d_epsilon_d_pts_cam_j; 53 | d_epsilon_d_pts_cam_j << 1./dep_j, 0, -pts_camera_j.x()/(dep_j*dep_j), 54 | 0, 1./dep_j, -pts_camera_j.y()/(dep_j*dep_j); 55 | typename Eigen::Matrix d_pts_cam_j_d_pts_cam_i; 56 | d_pts_cam_j_d_pts_cam_i = qic.inverse()*Qj.inverse()*Qi*qic; 57 | typename Eigen::Matrix d_pts_cam_i_d_pts_i; 58 | d_pts_cam_i_d_pts_i << 1./inv_dep_i, 0, 59 | 0, 1./inv_dep_i, 60 | 0, 0; 61 | 62 | typename Eigen::Matrix d_epsilon_d_pts_i = d_epsilon_d_pts_cam_j * d_pts_cam_j_d_pts_cam_i * d_pts_cam_i_d_pts_i; 63 | typename Eigen::Matrix d_epsilon_d_pts_j = Eigen::Matrix::Identity*(T(-1)); 64 | typename Eigen::Matrix d_epsilon_d_X; 65 | d_epsilon_d_X.block<2,2>(0, 0) = d_epsilon_d_pts_i; 66 | d_epsilon_d_X.block<2,2>(0, 2) = d_epsilon_d_pts_j; 67 | 68 | typename Eigen::Matrix J = d_epsilon_d_X; 69 | typename Eigen::Matrix JtJ = J*J.transpose(); 70 | 71 | typename Eigen::Map> residual(residuals); 72 | residual = -J.transpose()*JtJ.inverse()*epsilon; 73 | // residual = sqrt_info*residual; 74 | 75 | return true; 76 | } 77 | 78 | private: 79 | Eigen::Vector3d pts_i_, pts_j_; 80 | Eigen::Matrix4d sqrt_info; 81 | 82 | }; 83 | */ 84 | class SampsonFactor : public ceres::SizedCostFunction<4, 7, 7, 7, 1> 85 | { 86 | public: 87 | SampsonFactor(const Eigen::Vector3d& _pts_i, const Eigen::Vector3d& _pts_j); 88 | //const SampsonCostFunctor* functor); 89 | 90 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 91 | 92 | Eigen::Vector3d pts_i, pts_j; 93 | static Eigen::Matrix sqrt_info; 94 | 95 | void check(double **parameters); 96 | 97 | // private: 98 | // std::unique_ptr functor_; 99 | 100 | }; 101 | 102 | class SampsonFactorCross : public ceres::SizedCostFunction<4, 7, 7, 7, 1> 103 | { 104 | public: 105 | SampsonFactorCross(const Eigen::Vector3d& _pts_i, const Eigen::Vector3d& _pts_j); 106 | //const SampsonCostFunctor* functor); 107 | 108 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 109 | 110 | Eigen::Vector3d pts_i, pts_j; 111 | static Eigen::Matrix sqrt_info; 112 | 113 | void check(double **parameters); 114 | 115 | void compute_Jacobian_pose(double const *const *parameters, double** jacobians) const; 116 | // void compute_Jacobian_pose_j(double const *const *parameters, double** jacobians) const; 117 | 118 | 119 | // private: 120 | // std::unique_ptr functor_; 121 | 122 | }; 123 | 124 | class SampsonFactorEssential : public ceres::SizedCostFunction<4, 7, 7, 7> 125 | { 126 | public: 127 | SampsonFactorEssential(const Eigen::Vector3d& _pts_i, const Eigen::Vector3d& _pts_j); 128 | //const SampsonCostFunctor* functor); 129 | 130 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 131 | 132 | Eigen::Vector3d pts_i, pts_j; 133 | static Eigen::Matrix sqrt_info; 134 | 135 | void compute_Jacobian_pose(double const *const *parameters, double** jacobians) const; 136 | 137 | void check(double **parameters); 138 | 139 | }; 140 | 141 | class SampsonFactorWithLambda : public ceres::SizedCostFunction<5, 7, 7, 7, 1> 142 | { 143 | public: 144 | SampsonFactorWithLambda(const Eigen::Vector3d& _pts_i, const Eigen::Vector3d& _pts_j); 145 | 146 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 147 | 148 | Eigen::Vector3d pts_i, pts_j; 149 | static Eigen::Matrix sqrt_info; 150 | 151 | void check(double **parameters); 152 | 153 | }; 154 | 155 | 156 | class SampsonFactorCrossWithLambda : public ceres::SizedCostFunction<5, 7, 7, 7, 1> 157 | { 158 | public: 159 | SampsonFactorCrossWithLambda(const Eigen::Vector3d& _pts_i, const Eigen::Vector3d& _pts_j); 160 | 161 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 162 | 163 | Eigen::Vector3d pts_i, pts_j; 164 | static Eigen::Matrix sqrt_info; 165 | 166 | void check(double **parameters); 167 | 168 | }; -------------------------------------------------------------------------------- /vins_estimator/src/feature_manager.h: -------------------------------------------------------------------------------- 1 | #ifndef FEATURE_MANAGER_H 2 | #define FEATURE_MANAGER_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | using namespace std; 9 | 10 | #include 11 | using namespace Eigen; 12 | 13 | #include 14 | #include 15 | 16 | #include "parameters.h" 17 | 18 | enum DPT_TYPE 19 | { 20 | NO_DEPTH =0, DEPTH_MES, DEPTH_TRI, INVALID 21 | } ; 22 | 23 | class FeaturePerFrame 24 | { 25 | public: 26 | FeaturePerFrame(const Eigen::Matrix &_point, double td) 27 | { 28 | point.x() = _point(0); 29 | point.y() = _point(1); 30 | point.z() = _point(2); 31 | ori_point = point; 32 | uv.x() = _point(3); 33 | uv.y() = _point(4); 34 | velocity.x() = _point(5); 35 | velocity.y() = _point(6); 36 | cur_td = td; 37 | is_stereo = false; 38 | dpt = -1; 39 | gc_succeed =false; 40 | } 41 | void rightObservation(const Eigen::Matrix &_point) 42 | { 43 | pointRight.x() = _point(0); 44 | pointRight.y() = _point(1); 45 | pointRight.z() = _point(2); 46 | ori_pointRight = pointRight; 47 | uvRight.x() = _point(3); 48 | uvRight.y() = _point(4); 49 | velocityRight.x() = _point(5); 50 | velocityRight.y() = _point(6); 51 | is_stereo = true; 52 | 53 | // call getDepth() 54 | getDepth(); 55 | } 56 | 57 | // triangulation to compute depth 58 | double getDepth(); 59 | 60 | Eigen::Matrix2d getOmega(); // inverse of covariance matrix 61 | Eigen::Matrix2d getOmegaRight(); // weighted inverse of covariance matrix 62 | 63 | void print(){ 64 | printf("point: %f %f %f uv: %f %f \n", point.x(), point.y(), point.z(), uv.x(), uv.y()); 65 | } 66 | double cur_td; 67 | bool gc_succeed; 68 | Vector3d point, pointRight; 69 | Vector3d ori_point, ori_pointRight; 70 | Vector2d uv, uvRight; 71 | Vector2d velocity, velocityRight; 72 | double z; 73 | bool is_used; 74 | double parallax; 75 | MatrixXd A; 76 | VectorXd b; 77 | double dep_gradient; 78 | bool is_stereo; 79 | double dpt; 80 | int feat_id; 81 | }; 82 | 83 | class FeaturePerId 84 | { 85 | public: 86 | const int feature_id; 87 | int start_frame; 88 | vector feature_per_frame; 89 | 90 | int used_num; 91 | bool is_outlier; 92 | bool is_margin; 93 | double estimated_depth; 94 | int solve_flag; // 0 haven't solve yet; 1 solve succ; 2 solve fail; 95 | 96 | Vector3d gt_p; 97 | 98 | FeaturePerId(int _feature_id, int _start_frame) 99 | : feature_id(_feature_id), start_frame(_start_frame), 100 | used_num(0), estimated_depth(-1.0), solve_flag(0),dpt_type(NO_DEPTH) 101 | { 102 | } 103 | 104 | int endFrame(); 105 | DPT_TYPE dpt_type; 106 | }; 107 | 108 | class FeatureManager 109 | { 110 | public: 111 | FeatureManager(Matrix3d _Rs[]); 112 | 113 | void setRic(Matrix3d _ric[]); 114 | 115 | void clearState(); 116 | 117 | int getFeatureCount(); 118 | 119 | bool addFeatureCheckParallax(int frame_count, const map>>> &image, double td); 120 | void debugShow(); 121 | vector> getCorresponding(int frame_count_l, int frame_count_r); 122 | vector> getCorrespondingWithDepth(int frame_count_l, int frame_count_r); 123 | 124 | //void updateDepth(const VectorXd &x); 125 | void setDepth(const VectorXd &x); 126 | void removeFailures(); 127 | void clearDepth(const VectorXd &x); 128 | VectorXd getDepthVector(); 129 | void triangulate(Vector3d Ps[], Vector3d tic[], Matrix3d ric[]); 130 | void triangulatePoint(Eigen::Matrix &Pose0, Eigen::Matrix &Pose1, 131 | Eigen::Vector2d &point0, Eigen::Vector2d &point1, Eigen::Vector3d &point_3d); 132 | void triangulateStereo(); 133 | void triangulateWithDepth(Vector3d Ps[], Vector3d tic[], Matrix3d ric[]); 134 | void removeBackShiftDepth(Eigen::Matrix3d marg_R, Eigen::Vector3d marg_P, Eigen::Matrix3d new_R, Eigen::Vector3d new_P); 135 | void removeBack(); 136 | void removeFront(int frame_count); 137 | void removeOutlier(); 138 | list feature; 139 | int last_track_num; 140 | double last_average_parallax; 141 | int new_feature_num; 142 | int long_track_num; 143 | 144 | 145 | private: 146 | double compensatedParallax2(const FeaturePerId &it_per_id, int frame_count); 147 | const Matrix3d *Rs; 148 | Matrix3d ric[NUM_OF_CAM]; 149 | }; 150 | 151 | #endif -------------------------------------------------------------------------------- /vins_estimator/src/initial/initial_alignment.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include "../factor/imu_factor.h" 5 | #include "../utility/utility.h" 6 | #include 7 | #include 8 | #include "../feature_manager.h" 9 | 10 | using namespace Eigen; 11 | using namespace std; 12 | 13 | class ImageFrame 14 | { 15 | public: 16 | ImageFrame(){}; 17 | ImageFrame(const map>>>& _points, double _t):t{_t},is_key_frame{false} 18 | { 19 | points = _points; 20 | }; 21 | map> > > points; 22 | double t; 23 | Matrix3d R; 24 | Vector3d T; 25 | IntegrationBase *pre_integration; 26 | bool is_key_frame; 27 | }; 28 | 29 | bool VisualIMUAlignment(map &all_image_frame, Vector3d* Bgs, Vector3d &g, VectorXd &x); 30 | bool VisualIMUAlignmentWithDepth(map &all_image_frame, Vector3d* Bgs, Vector3d &g, VectorXd &x); -------------------------------------------------------------------------------- /vins_estimator/src/initial/initial_ex_rotation.cpp: -------------------------------------------------------------------------------- 1 | #include "initial_ex_rotation.h" 2 | 3 | InitialEXRotation::InitialEXRotation(){ 4 | frame_count = 0; 5 | Rc.push_back(Matrix3d::Identity()); 6 | Rc_g.push_back(Matrix3d::Identity()); 7 | Rimu.push_back(Matrix3d::Identity()); 8 | ric = Matrix3d::Identity(); 9 | } 10 | 11 | bool InitialEXRotation::CalibrationExRotation(vector> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result) 12 | { 13 | frame_count++; 14 | Rc.push_back(solveRelativeR(corres)); 15 | Rimu.push_back(delta_q_imu.toRotationMatrix()); 16 | Rc_g.push_back(ric.inverse() * delta_q_imu * ric); 17 | 18 | Eigen::MatrixXd A(frame_count * 4, 4); 19 | A.setZero(); 20 | int sum_ok = 0; 21 | for (int i = 1; i <= frame_count; i++) 22 | { 23 | Quaterniond r1(Rc[i]); 24 | Quaterniond r2(Rc_g[i]); 25 | 26 | double angular_distance = 180 / M_PI * r1.angularDistance(r2); 27 | ROS_DEBUG( 28 | "%d %f", i, angular_distance); 29 | 30 | double huber = angular_distance > 5.0 ? 5.0 / angular_distance : 1.0; 31 | ++sum_ok; 32 | Matrix4d L, R; 33 | 34 | double w = Quaterniond(Rc[i]).w(); 35 | Vector3d q = Quaterniond(Rc[i]).vec(); 36 | L.block<3, 3>(0, 0) = w * Matrix3d::Identity() + Utility::skewSymmetric(q); 37 | L.block<3, 1>(0, 3) = q; 38 | L.block<1, 3>(3, 0) = -q.transpose(); 39 | L(3, 3) = w; 40 | 41 | Quaterniond R_ij(Rimu[i]); 42 | w = R_ij.w(); 43 | q = R_ij.vec(); 44 | R.block<3, 3>(0, 0) = w * Matrix3d::Identity() - Utility::skewSymmetric(q); 45 | R.block<3, 1>(0, 3) = q; 46 | R.block<1, 3>(3, 0) = -q.transpose(); 47 | R(3, 3) = w; 48 | 49 | A.block<4, 4>((i - 1) * 4, 0) = huber * (L - R); 50 | } 51 | 52 | JacobiSVD svd(A, ComputeFullU | ComputeFullV); 53 | Matrix x = svd.matrixV().col(3); 54 | Quaterniond estimated_R(x); 55 | ric = estimated_R.toRotationMatrix().inverse(); 56 | //cout << svd.singularValues().transpose() << endl; 57 | //cout << ric << endl; 58 | Vector3d ric_cov; 59 | ric_cov = svd.singularValues().tail<3>(); 60 | if (frame_count >= WINDOW_SIZE && ric_cov(1) > 0.25) 61 | { 62 | calib_ric_result = ric; 63 | return true; 64 | } 65 | else 66 | return false; 67 | } 68 | 69 | Matrix3d InitialEXRotation::solveRelativeR(const vector> &corres) 70 | { 71 | if (corres.size() >= 9) 72 | { 73 | vector ll, rr; 74 | for (int i = 0; i < int(corres.size()); i++) 75 | { 76 | ll.push_back(cv::Point2f(corres[i].first(0), corres[i].first(1))); 77 | rr.push_back(cv::Point2f(corres[i].second(0), corres[i].second(1))); 78 | } 79 | cv::Mat E = cv::findFundamentalMat(ll, rr); 80 | cv::Mat_ R1, R2, t1, t2; 81 | decomposeE(E, R1, R2, t1, t2); 82 | 83 | if (determinant(R1) + 1.0 < 1e-09) 84 | { 85 | E = -E; 86 | decomposeE(E, R1, R2, t1, t2); 87 | } 88 | double ratio1 = max(testTriangulation(ll, rr, R1, t1), testTriangulation(ll, rr, R1, t2)); 89 | double ratio2 = max(testTriangulation(ll, rr, R2, t1), testTriangulation(ll, rr, R2, t2)); 90 | cv::Mat_ ans_R_cv = ratio1 > ratio2 ? R1 : R2; 91 | 92 | Matrix3d ans_R_eigen; 93 | for (int i = 0; i < 3; i++) 94 | for (int j = 0; j < 3; j++) 95 | ans_R_eigen(j, i) = ans_R_cv(i, j); 96 | return ans_R_eigen; 97 | } 98 | return Matrix3d::Identity(); 99 | } 100 | 101 | double InitialEXRotation::testTriangulation(const vector &l, 102 | const vector &r, 103 | cv::Mat_ R, cv::Mat_ t) 104 | { 105 | cv::Mat pointcloud; 106 | cv::Matx34f P = cv::Matx34f(1, 0, 0, 0, 107 | 0, 1, 0, 0, 108 | 0, 0, 1, 0); 109 | cv::Matx34f P1 = cv::Matx34f(R(0, 0), R(0, 1), R(0, 2), t(0), 110 | R(1, 0), R(1, 1), R(1, 2), t(1), 111 | R(2, 0), R(2, 1), R(2, 2), t(2)); 112 | cv::triangulatePoints(P, P1, l, r, pointcloud); 113 | int front_count = 0; 114 | for (int i = 0; i < pointcloud.cols; i++) 115 | { 116 | double normal_factor = pointcloud.col(i).at(3); 117 | 118 | cv::Mat_ p_3d_l = cv::Mat(P) * (pointcloud.col(i) / normal_factor); 119 | cv::Mat_ p_3d_r = cv::Mat(P1) * (pointcloud.col(i) / normal_factor); 120 | if (p_3d_l(2) > 0 && p_3d_r(2) > 0) 121 | front_count++; 122 | } 123 | ROS_DEBUG("MotionEstimator: %f", 1.0 * front_count / pointcloud.cols); 124 | return 1.0 * front_count / pointcloud.cols; 125 | } 126 | 127 | void InitialEXRotation::decomposeE(cv::Mat E, 128 | cv::Mat_ &R1, cv::Mat_ &R2, 129 | cv::Mat_ &t1, cv::Mat_ &t2) 130 | { 131 | cv::SVD svd(E, cv::SVD::MODIFY_A); 132 | cv::Matx33d W(0, -1, 0, 133 | 1, 0, 0, 134 | 0, 0, 1); 135 | cv::Matx33d Wt(0, 1, 0, 136 | -1, 0, 0, 137 | 0, 0, 1); 138 | R1 = svd.u * cv::Mat(W) * svd.vt; 139 | R2 = svd.u * cv::Mat(Wt) * svd.vt; 140 | t1 = svd.u.col(2); 141 | t2 = -svd.u.col(2); 142 | } 143 | -------------------------------------------------------------------------------- /vins_estimator/src/initial/initial_ex_rotation.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include "../parameters.h" 5 | using namespace std; 6 | 7 | #include 8 | 9 | #include 10 | using namespace Eigen; 11 | #include 12 | 13 | /* This class help you to calibrate extrinsic rotation between imu and camera when your totally don't konw the extrinsic parameter */ 14 | class InitialEXRotation 15 | { 16 | public: 17 | InitialEXRotation(); 18 | bool CalibrationExRotation(vector> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result); 19 | private: 20 | Matrix3d solveRelativeR(const vector> &corres); 21 | 22 | double testTriangulation(const vector &l, 23 | const vector &r, 24 | cv::Mat_ R, cv::Mat_ t); 25 | void decomposeE(cv::Mat E, 26 | cv::Mat_ &R1, cv::Mat_ &R2, 27 | cv::Mat_ &t1, cv::Mat_ &t2); 28 | int frame_count; 29 | 30 | vector< Matrix3d > Rc; 31 | vector< Matrix3d > Rimu; 32 | vector< Matrix3d > Rc_g; 33 | Matrix3d ric; 34 | }; 35 | 36 | 37 | -------------------------------------------------------------------------------- /vins_estimator/src/initial/initial_sfm.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | using namespace Eigen; 12 | using namespace std; 13 | 14 | 15 | struct SFMFeature 16 | { 17 | bool state; 18 | int id; 19 | vector> observation; 20 | double position[3]; 21 | double depth; 22 | 23 | vector> observation_depth; 24 | }; 25 | 26 | struct ReprojectionError3D 27 | { 28 | ReprojectionError3D(double observed_u, double observed_v) 29 | :observed_u(observed_u), observed_v(observed_v) 30 | {} 31 | 32 | template 33 | bool operator()(const T* const camera_R, const T* const camera_T, const T* point, T* residuals) const 34 | { 35 | T p[3]; 36 | ceres::QuaternionRotatePoint(camera_R, point, p); 37 | p[0] += camera_T[0]; p[1] += camera_T[1]; p[2] += camera_T[2]; 38 | T xp = p[0] / p[2]; 39 | T yp = p[1] / p[2]; 40 | residuals[0] = xp - T(observed_u); 41 | residuals[1] = yp - T(observed_v); 42 | return true; 43 | } 44 | 45 | static ceres::CostFunction* Create(const double observed_x, 46 | const double observed_y) 47 | { 48 | return (new ceres::AutoDiffCostFunction< 49 | ReprojectionError3D, 2, 4, 3, 3>( 50 | new ReprojectionError3D(observed_x,observed_y))); 51 | } 52 | 53 | double observed_u; 54 | double observed_v; 55 | }; 56 | 57 | class GlobalSFM 58 | { 59 | public: 60 | GlobalSFM(); 61 | bool construct(int frame_num, Quaterniond* q, Vector3d* T, int l, 62 | const Matrix3d relative_R, const Vector3d relative_T, 63 | vector &sfm_f, map &sfm_tracked_points); 64 | 65 | private: 66 | bool solveFrameByPnP(Matrix3d &R_initial, Vector3d &P_initial, int i, vector &sfm_f); 67 | 68 | void triangulatePoint(Eigen::Matrix &Pose0, Eigen::Matrix &Pose1, 69 | Vector2d &point0, Vector2d &point1, Vector3d &point_3d); 70 | void triangulateTwoFrames(int frame0, Eigen::Matrix &Pose0, 71 | int frame1, Eigen::Matrix &Pose1, 72 | vector &sfm_f); 73 | void triangulateTwoFramesWithDepth(int frame0, Eigen::Matrix &Pose0, 74 | int frame1, Eigen::Matrix &Pose1, 75 | vector &sfm_f); 76 | 77 | int feature_num; 78 | }; -------------------------------------------------------------------------------- /vins_estimator/src/initial/solve_5pts.cpp: -------------------------------------------------------------------------------- 1 | #include "solve_5pts.h" 2 | #include "solve_opt.h" 3 | 4 | namespace cv { 5 | void decomposeEssentialMat( InputArray _E, OutputArray _R1, OutputArray _R2, OutputArray _t ) 6 | { 7 | 8 | Mat E = _E.getMat().reshape(1, 3); 9 | CV_Assert(E.cols == 3 && E.rows == 3); 10 | 11 | Mat D, U, Vt; 12 | SVD::compute(E, D, U, Vt); 13 | 14 | if (determinant(U) < 0) U *= -1.; 15 | if (determinant(Vt) < 0) Vt *= -1.; 16 | 17 | Mat W = (Mat_(3, 3) << 0, 1, 0, -1, 0, 0, 0, 0, 1); 18 | W.convertTo(W, E.type()); 19 | 20 | Mat R1, R2, t; 21 | R1 = U * W * Vt; 22 | R2 = U * W.t() * Vt; 23 | t = U.col(2) * 1.0; 24 | 25 | R1.copyTo(_R1); 26 | R2.copyTo(_R2); 27 | t.copyTo(_t); 28 | } 29 | 30 | int recoverPose( InputArray E, InputArray _points1, InputArray _points2, InputArray _cameraMatrix, 31 | OutputArray _R, OutputArray _t, InputOutputArray _mask) 32 | { 33 | 34 | Mat points1, points2, cameraMatrix; 35 | _points1.getMat().convertTo(points1, CV_64F); 36 | _points2.getMat().convertTo(points2, CV_64F); 37 | _cameraMatrix.getMat().convertTo(cameraMatrix, CV_64F); 38 | 39 | int npoints = points1.checkVector(2); 40 | CV_Assert( npoints >= 0 && points2.checkVector(2) == npoints && 41 | points1.type() == points2.type()); 42 | 43 | CV_Assert(cameraMatrix.rows == 3 && cameraMatrix.cols == 3 && cameraMatrix.channels() == 1); 44 | 45 | if (points1.channels() > 1) 46 | { 47 | points1 = points1.reshape(1, npoints); 48 | points2 = points2.reshape(1, npoints); 49 | } 50 | 51 | double fx = cameraMatrix.at(0,0); 52 | double fy = cameraMatrix.at(1,1); 53 | double cx = cameraMatrix.at(0,2); 54 | double cy = cameraMatrix.at(1,2); 55 | 56 | points1.col(0) = (points1.col(0) - cx) / fx; 57 | points2.col(0) = (points2.col(0) - cx) / fx; 58 | points1.col(1) = (points1.col(1) - cy) / fy; 59 | points2.col(1) = (points2.col(1) - cy) / fy; 60 | 61 | points1 = points1.t(); 62 | points2 = points2.t(); 63 | 64 | Mat R1, R2, t; 65 | decomposeEssentialMat(E, R1, R2, t); 66 | Mat P0 = Mat::eye(3, 4, R1.type()); 67 | Mat P1(3, 4, R1.type()), P2(3, 4, R1.type()), P3(3, 4, R1.type()), P4(3, 4, R1.type()); 68 | P1(Range::all(), Range(0, 3)) = R1 * 1.0; P1.col(3) = t * 1.0; 69 | P2(Range::all(), Range(0, 3)) = R2 * 1.0; P2.col(3) = t * 1.0; 70 | P3(Range::all(), Range(0, 3)) = R1 * 1.0; P3.col(3) = -t * 1.0; 71 | P4(Range::all(), Range(0, 3)) = R2 * 1.0; P4.col(3) = -t * 1.0; 72 | 73 | // Do the cheirality check. 74 | // Notice here a threshold dist is used to filter 75 | // out far away points (i.e. infinite points) since 76 | // there depth may vary between postive and negtive. 77 | double dist = 50.0; 78 | Mat Q; 79 | triangulatePoints(P0, P1, points1, points2, Q); 80 | Mat mask1 = Q.row(2).mul(Q.row(3)) > 0; 81 | Q.row(0) /= Q.row(3); 82 | Q.row(1) /= Q.row(3); 83 | Q.row(2) /= Q.row(3); 84 | Q.row(3) /= Q.row(3); 85 | mask1 = (Q.row(2) < dist) & mask1; 86 | Q = P1 * Q; 87 | mask1 = (Q.row(2) > 0) & mask1; 88 | mask1 = (Q.row(2) < dist) & mask1; 89 | 90 | triangulatePoints(P0, P2, points1, points2, Q); 91 | Mat mask2 = Q.row(2).mul(Q.row(3)) > 0; 92 | Q.row(0) /= Q.row(3); 93 | Q.row(1) /= Q.row(3); 94 | Q.row(2) /= Q.row(3); 95 | Q.row(3) /= Q.row(3); 96 | mask2 = (Q.row(2) < dist) & mask2; 97 | Q = P2 * Q; 98 | mask2 = (Q.row(2) > 0) & mask2; 99 | mask2 = (Q.row(2) < dist) & mask2; 100 | 101 | triangulatePoints(P0, P3, points1, points2, Q); 102 | Mat mask3 = Q.row(2).mul(Q.row(3)) > 0; 103 | Q.row(0) /= Q.row(3); 104 | Q.row(1) /= Q.row(3); 105 | Q.row(2) /= Q.row(3); 106 | Q.row(3) /= Q.row(3); 107 | mask3 = (Q.row(2) < dist) & mask3; 108 | Q = P3 * Q; 109 | mask3 = (Q.row(2) > 0) & mask3; 110 | mask3 = (Q.row(2) < dist) & mask3; 111 | 112 | triangulatePoints(P0, P4, points1, points2, Q); 113 | Mat mask4 = Q.row(2).mul(Q.row(3)) > 0; 114 | Q.row(0) /= Q.row(3); 115 | Q.row(1) /= Q.row(3); 116 | Q.row(2) /= Q.row(3); 117 | Q.row(3) /= Q.row(3); 118 | mask4 = (Q.row(2) < dist) & mask4; 119 | Q = P4 * Q; 120 | mask4 = (Q.row(2) > 0) & mask4; 121 | mask4 = (Q.row(2) < dist) & mask4; 122 | 123 | mask1 = mask1.t(); 124 | mask2 = mask2.t(); 125 | mask3 = mask3.t(); 126 | mask4 = mask4.t(); 127 | 128 | // If _mask is given, then use it to filter outliers. 129 | if (!_mask.empty()) 130 | { 131 | Mat mask = _mask.getMat(); 132 | CV_Assert(mask.size() == mask1.size()); 133 | bitwise_and(mask, mask1, mask1); 134 | bitwise_and(mask, mask2, mask2); 135 | bitwise_and(mask, mask3, mask3); 136 | bitwise_and(mask, mask4, mask4); 137 | } 138 | if (_mask.empty() && _mask.needed()) 139 | { 140 | _mask.create(mask1.size(), CV_8U); 141 | } 142 | 143 | CV_Assert(_R.needed() && _t.needed()); 144 | _R.create(3, 3, R1.type()); 145 | _t.create(3, 1, t.type()); 146 | 147 | int good1 = countNonZero(mask1); 148 | int good2 = countNonZero(mask2); 149 | int good3 = countNonZero(mask3); 150 | int good4 = countNonZero(mask4); 151 | 152 | if (good1 >= good2 && good1 >= good3 && good1 >= good4) 153 | { 154 | R1.copyTo(_R); 155 | t.copyTo(_t); 156 | if (_mask.needed()) mask1.copyTo(_mask); 157 | return good1; 158 | } 159 | else if (good2 >= good1 && good2 >= good3 && good2 >= good4) 160 | { 161 | R2.copyTo(_R); 162 | t.copyTo(_t); 163 | if (_mask.needed()) mask2.copyTo(_mask); 164 | return good2; 165 | } 166 | else if (good3 >= good1 && good3 >= good2 && good3 >= good4) 167 | { 168 | t = -t; 169 | R1.copyTo(_R); 170 | t.copyTo(_t); 171 | if (_mask.needed()) mask3.copyTo(_mask); 172 | return good3; 173 | } 174 | else 175 | { 176 | t = -t; 177 | R2.copyTo(_R); 178 | t.copyTo(_t); 179 | if (_mask.needed()) mask4.copyTo(_mask); 180 | return good4; 181 | } 182 | } 183 | 184 | int recoverPose( InputArray E, InputArray _points1, InputArray _points2, OutputArray _R, 185 | OutputArray _t, double focal, Point2d pp, InputOutputArray _mask) 186 | { 187 | Mat cameraMatrix = (Mat_(3,3) << focal, 0, pp.x, 0, focal, pp.y, 0, 0, 1); 188 | return cv::recoverPose(E, _points1, _points2, cameraMatrix, _R, _t, _mask); 189 | } 190 | 191 | 192 | vector> getInliersWithValidDepth(const vector> &corres, cv::Mat& mask) 193 | { 194 | assert(mask.rows == corres.size()); 195 | 196 | vector> ret; 197 | for(int i=0; i(i,0) == 0) continue; 200 | 201 | if(corres[i].first.z() <= 0.1 ) 202 | continue; // invalid depth pair 203 | 204 | ret.push_back(make_pair(corres[i].first, corres[i].second)); 205 | } 206 | return ret; 207 | } 208 | 209 | } 210 | 211 | 212 | bool MotionEstimator::solveRelativeRT(const vector> &corres, Matrix3d &Rotation, Vector3d &Translation) 213 | { 214 | if (corres.size() >= 15) 215 | { 216 | vector ll, rr; 217 | for (int i = 0; i < int(corres.size()); i++) 218 | { 219 | ll.push_back(cv::Point2f(corres[i].first(0), corres[i].first(1))); 220 | rr.push_back(cv::Point2f(corres[i].second(0), corres[i].second(1))); 221 | } 222 | cv::Mat mask; 223 | cv::Mat E = cv::findFundamentalMat(ll, rr, cv::FM_RANSAC, 0.3 / 460, 0.99, mask); 224 | cv::Mat cameraMatrix = (cv::Mat_(3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 1); 225 | cv::Mat rot, trans; 226 | int inlier_cnt = cv::recoverPose(E, ll, rr, cameraMatrix, rot, trans, mask); 227 | cout << "solve_5pts.cpp: in solveRelativeRT(): inlier_cnt " << inlier_cnt << endl; 228 | 229 | Eigen::Matrix3d R; 230 | Eigen::Vector3d T; 231 | for (int i = 0; i < 3; i++) 232 | { 233 | T(i) = trans.at(i, 0); 234 | for (int j = 0; j < 3; j++) 235 | R(i, j) = rot.at(i, j); 236 | } 237 | 238 | Rotation = R.transpose(); 239 | Translation = -R.transpose() * T; 240 | if(inlier_cnt > 12){ 241 | 242 | // ROS_WARN("---------------5points----------------"); 243 | // ROS_WARN("input points %d inliers %d", ll.size(), inlier_cnt); 244 | 245 | return true; 246 | } 247 | else 248 | return false; 249 | } 250 | return false; 251 | } 252 | 253 | bool MotionEstimator::solveRelativeHybrid(const vector> &corres, Matrix3d &Rotation, Vector3d &Translation) 254 | { 255 | OptSolver sopt; 256 | if (corres.size() >= 15) 257 | { 258 | // solve [R,t] using 2D-2D match 259 | vector ll, rr; 260 | for (int i = 0; i < int(corres.size()); i++) 261 | { 262 | ll.push_back(cv::Point2f(corres[i].first(0), corres[i].first(1))); 263 | rr.push_back(cv::Point2f(corres[i].second(0), corres[i].second(1))); 264 | } 265 | cv::Mat mask; 266 | cv::Mat E = cv::findFundamentalMat(ll, rr, cv::FM_RANSAC, 0.3 / 460, 0.99, mask); 267 | cv::Mat cameraMatrix = (cv::Mat_(3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 1); 268 | cv::Mat rot, trans; 269 | int inlier_cnt = cv::recoverPose(E, ll, rr, cameraMatrix, rot, trans, mask); 270 | cout << "solve_5pts.cpp: in solveRelativeHybrid(): inlier_cnt " << inlier_cnt << endl; 271 | 272 | Eigen::Matrix3d R; 273 | Eigen::Vector3d T; 274 | for (int i = 0; i < 3; i++) 275 | { 276 | T(i) = trans.at(i, 0); 277 | for (int j = 0; j < 3; j++) 278 | R(i, j) = rot.at(i, j); 279 | } 280 | 281 | // with the speficied number of features 282 | vector> inliers; 283 | 284 | inliers = getInliersWithValidDepth(corres, mask); 285 | 286 | if(inlier_cnt > 12 && inliers.size() >= 5){ 287 | 288 | // optimization to solve R, T // R is Rji, T is tji 289 | sopt.solveHybrid(inliers, R, T); 290 | 291 | Rotation = R.transpose(); // Rotation is Rij 292 | Translation = -R.transpose() * T; // Translation is tij 293 | 294 | ROS_WARN("---------------5points----------------"); 295 | ROS_WARN("input points %d 2D inliers 3D inliers %d", ll.size(), inlier_cnt, inliers.size()); 296 | 297 | return true; 298 | } 299 | else{ 300 | ROS_DEBUG("2D inlier_cnt: %d 3D inliers: %d", inlier_cnt, inliers.size()); 301 | return false; 302 | } 303 | }else{ 304 | ROS_DEBUG("2D corresponds cnt: %d", corres.size()); 305 | } 306 | return false; 307 | } 308 | -------------------------------------------------------------------------------- /vins_estimator/src/initial/solve_5pts.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | using namespace std; 5 | 6 | #include 7 | //#include 8 | #include 9 | using namespace Eigen; 10 | 11 | #include 12 | 13 | class MotionEstimator 14 | { 15 | public: 16 | 17 | bool solveRelativeRT(const vector> &corres, Matrix3d &R, Vector3d &T); 18 | bool solveRelativeHybrid(const vector> &corres, Matrix3d &Rotation, Vector3d &Translation); 19 | 20 | private: 21 | double testTriangulation(const vector &l, 22 | const vector &r, 23 | cv::Mat_ R, cv::Mat_ t); 24 | void decomposeE(cv::Mat E, 25 | cv::Mat_ &R1, cv::Mat_ &R2, 26 | cv::Mat_ &t1, cv::Mat_ &t2); 27 | }; 28 | 29 | 30 | -------------------------------------------------------------------------------- /vins_estimator/src/initial/solve_opt.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Jan. 10, 2020, He Zhang, hzhang8@vcu.edu 3 | 4 | functions related to optimization 5 | 6 | */ 7 | 8 | #include "solve_opt.h" 9 | #include "translate_factor.h" 10 | #include "../feature_manager.h" 11 | 12 | OptSolver::OptSolver(){} 13 | 14 | // notice here corres must be inliers 15 | bool OptSolver::solveHybrid(const vector> &corres, Matrix3d &Rji, Vector3d &tji) 16 | { 17 | // first solve for initial translation 18 | solveTCeres(corres, Rji, tji); 19 | 20 | // TODO: remove outliers? 21 | 22 | // optimization the result 23 | solveCeres(corres, Rji, tji); 24 | return true; 25 | } 26 | 27 | bool OptSolver::solveTCeres(const vector> &corres, const Matrix3d &Rji, Vector3d &tji) 28 | { 29 | para_T[0][0] = tji(0); 30 | para_T[0][1] = tji(1); 31 | para_T[0][2] = tji(2); 32 | 33 | ceres::Problem problem; 34 | ceres::LossFunction *loss_function; 35 | // loss_function = new ceres::CauchyLoss(1.0); 36 | loss_function = new ceres::HuberLoss(1.0); 37 | 38 | problem.AddParameterBlock(para_T[0], 3); 39 | 40 | 41 | Matrix3d cov_pti = Matrix3d::Identity(); 42 | 43 | double ceres_sum_err = 0; 44 | for(int i=0; i> &corres, Matrix3d &Rji, Vector3d &tji) 82 | { 83 | Quaterniond qji(Rji); 84 | 85 | m_tji[0][0] = tji.x(); 86 | m_tji[0][1] = tji.y(); 87 | m_tji[0][2] = tji.z(); 88 | 89 | m_qji[0][0] = qji.w(); 90 | m_qji[0][1] = qji.x(); 91 | m_qji[0][2] = qji.y(); 92 | m_qji[0][3] = qji.z(); 93 | 94 | 95 | //full BA 96 | ceres::Problem problem; 97 | ceres::LocalParameterization* local_parameterization = new ceres::QuaternionParameterization(); 98 | problem.AddParameterBlock(m_qji[0], 4, local_parameterization); 99 | problem.AddParameterBlock(m_tji[0], 3); 100 | 101 | int N = corres.size(); 102 | for(int i=0; i 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include "initial_sfm.h" 20 | using namespace Eigen; 21 | using namespace std; 22 | 23 | class OptSolver 24 | { 25 | public: 26 | OptSolver(); 27 | 28 | // optimize to solve [R, t] 29 | bool solveCeres(const vector> &corres, Matrix3d &Rji, Vector3d &tv); 30 | 31 | // entrance function, estimate [R,t] given intial R 32 | bool solveHybrid(const vector> &corres, Matrix3d &Rji, Vector3d &tv); 33 | 34 | // only solve t in [R; t] 35 | bool solveTCeres(const vector> &corres, const Matrix3d &Rji, Vector3d &tji); 36 | 37 | // camera pose 38 | double m_tji[1][3]; 39 | double m_qji[1][4]; 40 | 41 | double para_T[1][3]; // for translation 42 | double para_s[1][0]; // for scale 43 | 44 | double para_pt[1000][3]; 45 | 46 | int feature_num; 47 | }; -------------------------------------------------------------------------------- /vins_estimator/src/initial/translate_factor.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | 3 | Jan. 7, 2020, He Zhang, hzhang8@vcu.edu 4 | 5 | factors to solve t in [R,t] given R using ceres 6 | 7 | */ 8 | 9 | #include "translate_factor.h" 10 | using namespace Eigen; 11 | 12 | SinglePtFactor::SinglePtFactor(const Eigen::Vector3d& pt, const Eigen::Matrix3d& C): 13 | pt_xi(pt) 14 | { 15 | Matrix3d INF = C.inverse(); 16 | LLT lltOfA(INF); // compute the Cholesky decomposition of A 17 | inf_pt_xi = lltOfA.matrixL(); // retrieve factor L in the decomposition 18 | // The previous two lines can also be written as "L = A.llt().matrixL()" 19 | } 20 | 21 | bool SinglePtFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const 22 | { 23 | Eigen::Vector3d pt(parameters[0][0], parameters[0][1], parameters[0][2]); 24 | Eigen::Map> residual(residuals); 25 | 26 | residual = pt - pt_xi; 27 | residual = inf_pt_xi * residual; 28 | 29 | if(jacobians){ 30 | if(jacobians[0]){ 31 | Eigen::Map> jacobian_t(jacobians[0]); 32 | jacobian_t = inf_pt_xi; 33 | } 34 | } 35 | return true; 36 | } 37 | 38 | 39 | TranslateWithPtFactor::TranslateWithPtFactor(const Eigen::Matrix3d& R, const Eigen::Vector2d& xnj, 40 | const Eigen::Matrix3d& cov_xi): 41 | pt_xnj(xnj), Rji(R), cov_pt_xi(cov_xi) 42 | {} 43 | 44 | bool TranslateWithPtFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const 45 | { 46 | Eigen::Vector3d T(parameters[0][0], parameters[0][1], parameters[0][2]); 47 | Eigen::Vector3d pt_xi(parameters[1][0], parameters[1][1], parameters[1][2]); 48 | Eigen::Map> residual(residuals); 49 | 50 | Eigen::Matrix R1 = Rji.row(0); 51 | Eigen::Matrix R2 = Rji.row(1); 52 | Eigen::Matrix R3 = Rji.row(2); 53 | 54 | Eigen::Matrix A; 55 | A << 1, 0, -pt_xnj(0), 56 | 0, 1, -pt_xnj(1); 57 | Eigen::Matrix b; 58 | Eigen::Matrix dr_dx = (R1 - pt_xnj(0)*R3); 59 | Eigen::Matrix dr_dy = (R2 - pt_xnj(1)*R3); 60 | b(0) = (pt_xnj(0)*R3 - R1)*pt_xi; 61 | b(1) = (pt_xnj(1)*R3 - R2)*pt_xi; 62 | 63 | // weight 64 | Eigen::Matrix2d W = Eigen::Matrix2d::Identity(); 65 | W(0,0) = 1./sqrt(dr_dx * cov_pt_xi * dr_dx.transpose()); 66 | W(1,1) = 1./sqrt(dr_dy * cov_pt_xi * dr_dy.transpose()); 67 | 68 | residual = A*T - b; 69 | residual = W * residual; 70 | 71 | if(jacobians){ 72 | if(jacobians[0]){ 73 | Eigen::Map> jacobian_t(jacobians[0]); 74 | 75 | jacobian_t = W*A; 76 | } 77 | if(jacobians[1]){ 78 | Eigen::Map> jacobian_t(jacobians[1]); 79 | 80 | jacobian_t.row(0) = dr_dx; 81 | jacobian_t.row(1) = dr_dy; 82 | jacobian_t = W * jacobian_t; 83 | } 84 | } 85 | 86 | return true; 87 | } 88 | 89 | TranslateFactor::TranslateFactor(const Eigen::Matrix3d& R, const Eigen::Vector3d& xi, const Eigen::Vector2d& xnj, 90 | const Eigen::Matrix3d& cov_xi): 91 | pt_xi(xi), pt_xnj(xnj), Rji(R), cov_pt_xi(cov_xi) 92 | {} 93 | 94 | bool TranslateFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const 95 | { 96 | Eigen::Vector3d T(parameters[0][0], parameters[0][1], parameters[0][2]); 97 | Eigen::Map> residual(residuals); 98 | 99 | Eigen::Matrix R1 = Rji.row(0); 100 | Eigen::Matrix R2 = Rji.row(1); 101 | Eigen::Matrix R3 = Rji.row(2); 102 | 103 | Eigen::Matrix A; 104 | A << 1, 0, -pt_xnj(0), 105 | 0, 1, -pt_xnj(1); 106 | Eigen::Matrix b; 107 | Eigen::Matrix dr_dx = (R1 - pt_xnj(0)*R3); 108 | Eigen::Matrix dr_dy = (R2 - pt_xnj(1)*R3); 109 | b(0) = (pt_xnj(0)*R3 - R1)*pt_xi; 110 | b(1) = (pt_xnj(1)*R3 - R2)*pt_xi; 111 | 112 | // weight 113 | Eigen::Matrix2d W = Eigen::Matrix2d::Identity(); 114 | W(0,0) = 1./sqrt(dr_dx * cov_pt_xi * dr_dx.transpose()); 115 | W(1,1) = 1./sqrt(dr_dy * cov_pt_xi * dr_dy.transpose()); 116 | 117 | residual = A*T - b; 118 | residual = W * residual; 119 | 120 | if(jacobians){ 121 | if(jacobians[0]){ 122 | Eigen::Map> jacobian_t(jacobians[0]); 123 | 124 | jacobian_t = W*A; 125 | 126 | } 127 | } 128 | 129 | return true; 130 | } 131 | 132 | 133 | TranslateScaleFactor::TranslateScaleFactor(const Eigen::Matrix3d& R, const Eigen::Vector3d& nt, const Eigen::Vector3d& xi, const Eigen::Vector2d& xnj, 134 | const Eigen::Matrix3d& cov_xi): 135 | pt_xi(xi), pt_xnj(xnj), Rji(R), ntji(nt), cov_pt_xi(cov_xi) 136 | {} 137 | 138 | bool TranslateScaleFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const 139 | { 140 | double scale = parameters[0][0]; 141 | Eigen::Vector3d T = scale * ntji; 142 | Eigen::Map> residual(residuals); 143 | 144 | Eigen::Matrix R1 = Rji.row(0); 145 | Eigen::Matrix R2 = Rji.row(1); 146 | Eigen::Matrix R3 = Rji.row(2); 147 | 148 | Eigen::Matrix A; 149 | A << 1, 0, -pt_xnj(0), 150 | 0, 1, -pt_xnj(1); 151 | Eigen::Matrix b; 152 | Eigen::Matrix dr_dx = (R1 - pt_xnj(0)*R3); 153 | Eigen::Matrix dr_dy = (R2 - pt_xnj(1)*R3); 154 | b(0) = (pt_xnj(0)*R3 - R1)*pt_xi; 155 | b(1) = (pt_xnj(1)*R3 - R2)*pt_xi; 156 | 157 | // weight 158 | Eigen::Matrix2d W = Eigen::Matrix2d::Identity(); 159 | W(0,0) = 1./sqrt(dr_dx * cov_pt_xi * dr_dx.transpose()); 160 | W(1,1) = 1./sqrt(dr_dy * cov_pt_xi * dr_dy.transpose()); 161 | 162 | residual = A*T - b; 163 | residual = W * residual; 164 | 165 | if(jacobians){ 166 | if(jacobians[0]){ 167 | Eigen::Map> jacobian_t(jacobians[0]); 168 | 169 | jacobian_t = W*A*ntji; 170 | 171 | } 172 | } 173 | 174 | return true; 175 | } -------------------------------------------------------------------------------- /vins_estimator/src/initial/translate_factor.h: -------------------------------------------------------------------------------- 1 | /* 2 | 3 | Jan. 7, 2020, He Zhang, hzhang8@vcu.edu 4 | 5 | factors to solve t in [R,t] given R using ceres 6 | 7 | */ 8 | 9 | #pragma once 10 | 11 | 12 | #include 13 | #include 14 | 15 | 16 | class TranslateFactor : public ceres::SizedCostFunction<2, 3> 17 | { 18 | public: 19 | TranslateFactor(const Eigen::Matrix3d& R, const Eigen::Vector3d& xi, const Eigen::Vector2d& xnj, 20 | const Eigen::Matrix3d& cov_xi); 21 | virtual bool Evaluate(double const* const *params, double *residuals, double**jacobians) const; 22 | 23 | Eigen::Vector3d pt_xi; 24 | Eigen::Vector2d pt_xnj; 25 | Eigen::Matrix3d Rji; 26 | Eigen::Matrix3d cov_pt_xi; 27 | // Eigen::Matrix2d cov_pt_xnj; 28 | 29 | }; 30 | 31 | class TranslateScaleFactor : public ceres::SizedCostFunction<2, 1> 32 | { 33 | public: 34 | TranslateScaleFactor(const Eigen::Matrix3d& R, const Eigen::Vector3d& nt, const Eigen::Vector3d& xi, const Eigen::Vector2d& xnj, 35 | const Eigen::Matrix3d& cov_xi); 36 | virtual bool Evaluate(double const* const *params, double *residuals, double**jacobians) const; 37 | 38 | Eigen::Vector3d pt_xi; 39 | Eigen::Vector2d pt_xnj; 40 | Eigen::Matrix3d Rji; 41 | Eigen::Vector3d ntji; // normalized tji 42 | Eigen::Matrix3d cov_pt_xi; 43 | }; 44 | 45 | class TranslateWithPtFactor : public ceres::SizedCostFunction<2, 3, 3> 46 | { 47 | public: 48 | TranslateWithPtFactor(const Eigen::Matrix3d& R, const Eigen::Vector2d& xnj, 49 | const Eigen::Matrix3d& cov_xi); 50 | virtual bool Evaluate(double const* const *params, double *residuals, double**jacobians) const; 51 | 52 | Eigen::Vector2d pt_xnj; 53 | Eigen::Matrix3d Rji; 54 | Eigen::Matrix3d cov_pt_xi; 55 | }; 56 | 57 | class SinglePtFactor : public ceres::SizedCostFunction<3, 3> 58 | { 59 | public: 60 | SinglePtFactor(const Eigen::Vector3d& pt, const Eigen::Matrix3d& cov); 61 | 62 | virtual bool Evaluate(double const* const *params, double *residuals, double**jacobians) const; 63 | 64 | Eigen::Vector3d pt_xi; 65 | Eigen::Matrix3d inf_pt_xi; 66 | }; -------------------------------------------------------------------------------- /vins_estimator/src/parameters.cpp: -------------------------------------------------------------------------------- 1 | #include "parameters.h" 2 | #include 3 | #include 4 | 5 | double INIT_DEPTH; 6 | double MIN_PARALLAX; 7 | double ACC_N, ACC_W; 8 | double GYR_N, GYR_W; 9 | 10 | 11 | using namespace std; 12 | std::vector RIC; 13 | std::vector TIC; 14 | std::vector CAM_NAMES; 15 | 16 | Eigen::Matrix3d Rrl; 17 | Eigen::Vector3d Trl; 18 | Eigen::Matrix3d Rlr; 19 | Eigen::Vector3d Tlr; 20 | 21 | Eigen::Vector3d G{0.0, 0.0, 9.8}; 22 | 23 | double BIAS_ACC_THRESHOLD; 24 | double BIAS_GYR_THRESHOLD; 25 | double SOLVER_TIME; 26 | int NUM_ITERATIONS; 27 | int ESTIMATE_EXTRINSIC; 28 | int ESTIMATE_TD; 29 | int ROLLING_SHUTTER; 30 | std::string EX_CALIB_RESULT_PATH; 31 | std::string VINS_RESULT_PATH; 32 | std::string IMU_TOPIC; 33 | double ROW, COL; 34 | double TD, TR; 35 | double nG = 1.; 36 | bool g_use_sampson_model = false; 37 | bool g_use_stereo_correction = true; // false; 38 | bool g_opt_verbose = false; 39 | 40 | template 41 | T readParam(ros::NodeHandle &n, std::string name) 42 | { 43 | T ans; 44 | if (n.getParam(name, ans)) 45 | { 46 | ROS_INFO_STREAM("Loaded " << name << ": " << ans); 47 | } 48 | else 49 | { 50 | ROS_ERROR_STREAM("Failed to load " << name); 51 | n.shutdown(); 52 | } 53 | return ans; 54 | } 55 | 56 | void readParameters(ros::NodeHandle &n) 57 | { 58 | std::string config_file; 59 | config_file = readParam(n, "config_file"); 60 | 61 | n.param("gravity_norm", nG, nG); 62 | n.param("use_sampson_model", g_use_sampson_model, g_use_sampson_model); 63 | n.param("use_stereo_correction", g_use_stereo_correction, g_use_stereo_correction); 64 | n.param("opt_verbose", g_opt_verbose, g_opt_verbose); 65 | ROS_INFO("parameters.cpp: gravity_norm: %lf", nG); 66 | cout <<"parameters.cpp: "<< (g_use_sampson_model?"Yes use sampson model":"Not use sampson model")<(n, "vins_folder"); 77 | 78 | fsSettings["imu_topic"] >> IMU_TOPIC; 79 | 80 | SOLVER_TIME = fsSettings["max_solver_time"]; 81 | NUM_ITERATIONS = fsSettings["max_num_iterations"]; 82 | MIN_PARALLAX = fsSettings["keyframe_parallax"]; 83 | MIN_PARALLAX = MIN_PARALLAX / FOCAL_LENGTH; 84 | 85 | std::string OUTPUT_PATH; 86 | fsSettings["output_path"] >> OUTPUT_PATH; 87 | VINS_RESULT_PATH = OUTPUT_PATH + "/vins_result_no_loop.csv"; 88 | std::cout << "result path " << VINS_RESULT_PATH << std::endl; 89 | 90 | // create folder if not exists 91 | FileSystemHelper::createDirectoryIfNotExists(OUTPUT_PATH.c_str()); 92 | 93 | std::ofstream fout(VINS_RESULT_PATH, std::ios::out); 94 | fout.close(); 95 | 96 | ACC_N = fsSettings["acc_n"]; 97 | ACC_W = fsSettings["acc_w"]; 98 | GYR_N = fsSettings["gyr_n"]; 99 | GYR_W = fsSettings["gyr_w"]; 100 | G.z() = fsSettings["g_norm"]; 101 | ROW = fsSettings["image_height"]; 102 | COL = fsSettings["image_width"]; 103 | ROS_INFO("ROW: %f COL: %f ", ROW, COL); 104 | 105 | ESTIMATE_EXTRINSIC = fsSettings["estimate_extrinsic"]; 106 | if (ESTIMATE_EXTRINSIC == 2) 107 | { 108 | ROS_WARN("have no prior about extrinsic param, calibrate extrinsic param"); 109 | RIC.push_back(Eigen::Matrix3d::Identity()); 110 | TIC.push_back(Eigen::Vector3d::Zero()); 111 | EX_CALIB_RESULT_PATH = OUTPUT_PATH + "/extrinsic_parameter.csv"; 112 | 113 | } 114 | else 115 | { 116 | if ( ESTIMATE_EXTRINSIC == 1) 117 | { 118 | ROS_WARN(" Optimize extrinsic param around initial guess!"); 119 | EX_CALIB_RESULT_PATH = OUTPUT_PATH + "/extrinsic_parameter.csv"; 120 | } 121 | if (ESTIMATE_EXTRINSIC == 0) 122 | ROS_WARN(" fix extrinsic param "); 123 | 124 | // T_u2c1 125 | cv::Mat cv_R, cv_T; 126 | fsSettings["extrinsicRotation"] >> cv_R; 127 | fsSettings["extrinsicTranslation"] >> cv_T; 128 | Eigen::Matrix3d eigen_R; 129 | Eigen::Vector3d eigen_T; 130 | cv::cv2eigen(cv_R, eigen_R); 131 | cv::cv2eigen(cv_T, eigen_T); 132 | Eigen::Quaterniond Q(eigen_R); 133 | eigen_R = Q.normalized(); 134 | RIC.push_back(eigen_R); 135 | TIC.push_back(eigen_T); 136 | ROS_INFO_STREAM("Extrinsic_R1 : " << std::endl << RIC[0]); 137 | ROS_INFO_STREAM("Extrinsic_T1 : " << std::endl << TIC[0].transpose()); 138 | 139 | // T_u2c2 140 | cv::Mat cv_TT; 141 | fsSettings["body_T_cam1"] >> cv_TT; 142 | Eigen::Matrix4d T; 143 | cv::cv2eigen(cv_TT, T); 144 | RIC.push_back(T.block<3, 3>(0, 0)); 145 | TIC.push_back(T.block<3, 1>(0, 3)); 146 | ROS_INFO_STREAM("Extrinsic_R2 : " << std::endl << RIC[1]); 147 | ROS_INFO_STREAM("Extrinsic_T2 : " << std::endl << TIC[1].transpose()); 148 | } 149 | 150 | // transformation between stereo cams 151 | { 152 | cv::Mat cv_R, cv_T; 153 | fsSettings["Rrl"] >> cv_R; 154 | fsSettings["Trl"] >> cv_T; 155 | cv::cv2eigen(cv_R, Rrl); 156 | cv::cv2eigen(cv_T, Trl); 157 | Eigen::Quaterniond qq(Rrl); 158 | Rrl = qq.normalized(); 159 | Rlr = Rrl.transpose(); 160 | Tlr = - Rlr * Trl; 161 | ROS_INFO_STREAM("Rrl: " << std::endl << Rrl); 162 | ROS_INFO_STREAM("Trl: " << std::endl << Trl.transpose()); 163 | } 164 | 165 | { 166 | // CAM_NAMES.push_back(config_file); 167 | std::string cam0Calibfile, cam1Calibfile; 168 | fsSettings["cam0_calib"] >> cam0Calibfile; 169 | fsSettings["cam1_calib"] >> cam1Calibfile; 170 | 171 | std::string cam0Path = VINS_FOLDER_PATH + "/" + cam0Calibfile; 172 | std::string cam1Path = VINS_FOLDER_PATH + "/" + cam1Calibfile; 173 | 174 | ROS_DEBUG("cam0Path: %s", cam0Path.c_str()); 175 | ROS_DEBUG("cam1Path: %s", cam1Path.c_str()); 176 | CAM_NAMES.push_back(cam0Path); 177 | CAM_NAMES.push_back(cam1Path); 178 | } 179 | 180 | 181 | INIT_DEPTH = 15.0; 182 | BIAS_ACC_THRESHOLD = 0.1; 183 | BIAS_GYR_THRESHOLD = 0.1; 184 | 185 | TD = fsSettings["td"]; 186 | ESTIMATE_TD = fsSettings["estimate_td"]; 187 | if (ESTIMATE_TD) 188 | ROS_INFO_STREAM("Unsynchronized sensors, online estimate time offset, initial td: " << TD); 189 | else 190 | ROS_INFO_STREAM("Synchronized sensors, fix time offset: " << TD); 191 | 192 | ROLLING_SHUTTER = fsSettings["rolling_shutter"]; 193 | if (ROLLING_SHUTTER) 194 | { 195 | TR = fsSettings["rolling_shutter_tr"]; 196 | ROS_INFO_STREAM("rolling shutter camera, read out time per line: " << TR); 197 | } 198 | else 199 | { 200 | TR = 0; 201 | } 202 | 203 | fsSettings.release(); 204 | } 205 | -------------------------------------------------------------------------------- /vins_estimator/src/parameters.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include "utility/utility.h" 7 | #include 8 | #include 9 | #include 10 | 11 | #define SQ(x) (((x)*(x))) 12 | 13 | const double FOCAL_LENGTH = 460.0; 14 | const int WINDOW_SIZE = 10; 15 | const int NUM_OF_CAM = 2; // this is two cams 16 | const int NUM_OF_F = 1000; 17 | //#define UNIT_SPHERE_ERROR 18 | 19 | extern double INIT_DEPTH; 20 | extern double MIN_PARALLAX; 21 | extern int ESTIMATE_EXTRINSIC; 22 | 23 | extern double ACC_N, ACC_W; 24 | extern double GYR_N, GYR_W; 25 | 26 | extern std::vector RIC; // Tic 27 | extern std::vector TIC; 28 | extern std::vector CAM_NAMES; 29 | 30 | extern Eigen::Matrix3d Rlr; // Tlr 31 | extern Eigen::Vector3d Tlr; 32 | extern Eigen::Matrix3d Rrl; // Trl 33 | extern Eigen::Vector3d Trl; 34 | extern Eigen::Vector3d G; 35 | 36 | extern double BIAS_ACC_THRESHOLD; 37 | extern double BIAS_GYR_THRESHOLD; 38 | extern double SOLVER_TIME; 39 | extern int NUM_ITERATIONS; 40 | extern std::string EX_CALIB_RESULT_PATH; 41 | extern std::string VINS_RESULT_PATH; 42 | extern std::string IMU_TOPIC; 43 | extern double TD; 44 | extern double TR; 45 | extern int ESTIMATE_TD; 46 | extern int ROLLING_SHUTTER; 47 | extern double ROW, COL; 48 | extern double nG; 49 | extern bool g_use_sampson_model; 50 | extern bool g_use_stereo_correction; 51 | extern bool g_opt_verbose; 52 | 53 | void readParameters(ros::NodeHandle &n); 54 | 55 | enum SIZE_PARAMETERIZATION 56 | { 57 | SIZE_POSE = 7, 58 | SIZE_SPEEDBIAS = 9, 59 | SIZE_FEATURE = 1 60 | }; 61 | 62 | enum StateOrder 63 | { 64 | O_P = 0, 65 | O_R = 3, 66 | O_V = 6, 67 | O_BA = 9, 68 | O_BG = 12 69 | }; 70 | 71 | enum NoiseOrder 72 | { 73 | O_AN = 0, 74 | O_GN = 3, 75 | O_AW = 6, 76 | O_GW = 9 77 | }; 78 | -------------------------------------------------------------------------------- /vins_estimator/src/utility/CameraPoseVisualization.cpp: -------------------------------------------------------------------------------- 1 | #include "CameraPoseVisualization.h" 2 | 3 | const Eigen::Vector3d CameraPoseVisualization::imlt = Eigen::Vector3d(-1.0, -0.5, 1.0); 4 | const Eigen::Vector3d CameraPoseVisualization::imrt = Eigen::Vector3d( 1.0, -0.5, 1.0); 5 | const Eigen::Vector3d CameraPoseVisualization::imlb = Eigen::Vector3d(-1.0, 0.5, 1.0); 6 | const Eigen::Vector3d CameraPoseVisualization::imrb = Eigen::Vector3d( 1.0, 0.5, 1.0); 7 | const Eigen::Vector3d CameraPoseVisualization::lt0 = Eigen::Vector3d(-0.7, -0.5, 1.0); 8 | const Eigen::Vector3d CameraPoseVisualization::lt1 = Eigen::Vector3d(-0.7, -0.2, 1.0); 9 | const Eigen::Vector3d CameraPoseVisualization::lt2 = Eigen::Vector3d(-1.0, -0.2, 1.0); 10 | const Eigen::Vector3d CameraPoseVisualization::oc = Eigen::Vector3d(0.0, 0.0, 0.0); 11 | 12 | void Eigen2Point(const Eigen::Vector3d& v, geometry_msgs::Point& p) { 13 | p.x = v.x(); 14 | p.y = v.y(); 15 | p.z = v.z(); 16 | } 17 | 18 | CameraPoseVisualization::CameraPoseVisualization(float r, float g, float b, float a) 19 | : m_marker_ns("CameraPoseVisualization"), m_scale(0.2), m_line_width(0.01) { 20 | m_image_boundary_color.r = r; 21 | m_image_boundary_color.g = g; 22 | m_image_boundary_color.b = b; 23 | m_image_boundary_color.a = a; 24 | m_optical_center_connector_color.r = r; 25 | m_optical_center_connector_color.g = g; 26 | m_optical_center_connector_color.b = b; 27 | m_optical_center_connector_color.a = a; 28 | } 29 | 30 | void CameraPoseVisualization::setImageBoundaryColor(float r, float g, float b, float a) { 31 | m_image_boundary_color.r = r; 32 | m_image_boundary_color.g = g; 33 | m_image_boundary_color.b = b; 34 | m_image_boundary_color.a = a; 35 | } 36 | 37 | void CameraPoseVisualization::setOpticalCenterConnectorColor(float r, float g, float b, float a) { 38 | m_optical_center_connector_color.r = r; 39 | m_optical_center_connector_color.g = g; 40 | m_optical_center_connector_color.b = b; 41 | m_optical_center_connector_color.a = a; 42 | } 43 | 44 | void CameraPoseVisualization::setScale(double s) { 45 | m_scale = s; 46 | } 47 | void CameraPoseVisualization::setLineWidth(double width) { 48 | m_line_width = width; 49 | } 50 | void CameraPoseVisualization::add_edge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1){ 51 | visualization_msgs::Marker marker; 52 | 53 | marker.ns = m_marker_ns; 54 | marker.id = m_markers.size() + 1; 55 | marker.type = visualization_msgs::Marker::LINE_LIST; 56 | marker.action = visualization_msgs::Marker::ADD; 57 | marker.scale.x = 0.005; 58 | 59 | marker.color.g = 1.0f; 60 | marker.color.a = 1.0; 61 | 62 | geometry_msgs::Point point0, point1; 63 | 64 | Eigen2Point(p0, point0); 65 | Eigen2Point(p1, point1); 66 | 67 | marker.points.push_back(point0); 68 | marker.points.push_back(point1); 69 | 70 | m_markers.push_back(marker); 71 | } 72 | 73 | void CameraPoseVisualization::add_loopedge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1){ 74 | visualization_msgs::Marker marker; 75 | 76 | marker.ns = m_marker_ns; 77 | marker.id = m_markers.size() + 1; 78 | marker.type = visualization_msgs::Marker::LINE_LIST; 79 | marker.action = visualization_msgs::Marker::ADD; 80 | marker.scale.x = 0.04; 81 | //marker.scale.x = 0.3; 82 | 83 | marker.color.r = 1.0f; 84 | marker.color.b = 1.0f; 85 | marker.color.a = 1.0; 86 | 87 | geometry_msgs::Point point0, point1; 88 | 89 | Eigen2Point(p0, point0); 90 | Eigen2Point(p1, point1); 91 | 92 | marker.points.push_back(point0); 93 | marker.points.push_back(point1); 94 | 95 | m_markers.push_back(marker); 96 | } 97 | 98 | 99 | void CameraPoseVisualization::add_pose(const Eigen::Vector3d& p, const Eigen::Quaterniond& q) { 100 | visualization_msgs::Marker marker; 101 | 102 | marker.ns = m_marker_ns; 103 | marker.id = m_markers.size() + 1; 104 | marker.type = visualization_msgs::Marker::LINE_STRIP; 105 | marker.action = visualization_msgs::Marker::ADD; 106 | marker.scale.x = m_line_width; 107 | 108 | marker.pose.position.x = 0.0; 109 | marker.pose.position.y = 0.0; 110 | marker.pose.position.z = 0.0; 111 | marker.pose.orientation.w = 1.0; 112 | marker.pose.orientation.x = 0.0; 113 | marker.pose.orientation.y = 0.0; 114 | marker.pose.orientation.z = 0.0; 115 | 116 | 117 | geometry_msgs::Point pt_lt, pt_lb, pt_rt, pt_rb, pt_oc, pt_lt0, pt_lt1, pt_lt2; 118 | 119 | Eigen2Point(q * (m_scale *imlt) + p, pt_lt); 120 | Eigen2Point(q * (m_scale *imlb) + p, pt_lb); 121 | Eigen2Point(q * (m_scale *imrt) + p, pt_rt); 122 | Eigen2Point(q * (m_scale *imrb) + p, pt_rb); 123 | Eigen2Point(q * (m_scale *lt0 ) + p, pt_lt0); 124 | Eigen2Point(q * (m_scale *lt1 ) + p, pt_lt1); 125 | Eigen2Point(q * (m_scale *lt2 ) + p, pt_lt2); 126 | Eigen2Point(q * (m_scale *oc ) + p, pt_oc); 127 | 128 | // image boundaries 129 | marker.points.push_back(pt_lt); 130 | marker.points.push_back(pt_lb); 131 | marker.colors.push_back(m_image_boundary_color); 132 | marker.colors.push_back(m_image_boundary_color); 133 | 134 | marker.points.push_back(pt_lb); 135 | marker.points.push_back(pt_rb); 136 | marker.colors.push_back(m_image_boundary_color); 137 | marker.colors.push_back(m_image_boundary_color); 138 | 139 | marker.points.push_back(pt_rb); 140 | marker.points.push_back(pt_rt); 141 | marker.colors.push_back(m_image_boundary_color); 142 | marker.colors.push_back(m_image_boundary_color); 143 | 144 | marker.points.push_back(pt_rt); 145 | marker.points.push_back(pt_lt); 146 | marker.colors.push_back(m_image_boundary_color); 147 | marker.colors.push_back(m_image_boundary_color); 148 | 149 | // top-left indicator 150 | marker.points.push_back(pt_lt0); 151 | marker.points.push_back(pt_lt1); 152 | marker.colors.push_back(m_image_boundary_color); 153 | marker.colors.push_back(m_image_boundary_color); 154 | 155 | marker.points.push_back(pt_lt1); 156 | marker.points.push_back(pt_lt2); 157 | marker.colors.push_back(m_image_boundary_color); 158 | marker.colors.push_back(m_image_boundary_color); 159 | 160 | // optical center connector 161 | marker.points.push_back(pt_lt); 162 | marker.points.push_back(pt_oc); 163 | marker.colors.push_back(m_optical_center_connector_color); 164 | marker.colors.push_back(m_optical_center_connector_color); 165 | 166 | 167 | marker.points.push_back(pt_lb); 168 | marker.points.push_back(pt_oc); 169 | marker.colors.push_back(m_optical_center_connector_color); 170 | marker.colors.push_back(m_optical_center_connector_color); 171 | 172 | marker.points.push_back(pt_rt); 173 | marker.points.push_back(pt_oc); 174 | marker.colors.push_back(m_optical_center_connector_color); 175 | marker.colors.push_back(m_optical_center_connector_color); 176 | 177 | marker.points.push_back(pt_rb); 178 | marker.points.push_back(pt_oc); 179 | marker.colors.push_back(m_optical_center_connector_color); 180 | marker.colors.push_back(m_optical_center_connector_color); 181 | 182 | m_markers.push_back(marker); 183 | } 184 | 185 | void CameraPoseVisualization::reset() { 186 | m_markers.clear(); 187 | } 188 | 189 | void CameraPoseVisualization::publish_by( ros::Publisher &pub, const std_msgs::Header &header ) { 190 | visualization_msgs::MarkerArray markerArray_msg; 191 | 192 | for(auto& marker : m_markers) { 193 | marker.header = header; 194 | markerArray_msg.markers.push_back(marker); 195 | } 196 | 197 | pub.publish(markerArray_msg); 198 | } -------------------------------------------------------------------------------- /vins_estimator/src/utility/CameraPoseVisualization.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | class CameraPoseVisualization { 11 | public: 12 | std::string m_marker_ns; 13 | 14 | CameraPoseVisualization(float r, float g, float b, float a); 15 | 16 | void setImageBoundaryColor(float r, float g, float b, float a=1.0); 17 | void setOpticalCenterConnectorColor(float r, float g, float b, float a=1.0); 18 | void setScale(double s); 19 | void setLineWidth(double width); 20 | 21 | void add_pose(const Eigen::Vector3d& p, const Eigen::Quaterniond& q); 22 | void reset(); 23 | 24 | void publish_by(ros::Publisher& pub, const std_msgs::Header& header); 25 | void add_edge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1); 26 | void add_loopedge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1); 27 | private: 28 | std::vector m_markers; 29 | std_msgs::ColorRGBA m_image_boundary_color; 30 | std_msgs::ColorRGBA m_optical_center_connector_color; 31 | double m_scale; 32 | double m_line_width; 33 | 34 | static const Eigen::Vector3d imlt; 35 | static const Eigen::Vector3d imlb; 36 | static const Eigen::Vector3d imrt; 37 | static const Eigen::Vector3d imrb; 38 | static const Eigen::Vector3d oc ; 39 | static const Eigen::Vector3d lt0 ; 40 | static const Eigen::Vector3d lt1 ; 41 | static const Eigen::Vector3d lt2 ; 42 | }; 43 | -------------------------------------------------------------------------------- /vins_estimator/src/utility/tic_toc.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | class TicToc 8 | { 9 | public: 10 | TicToc() 11 | { 12 | tic(); 13 | } 14 | 15 | void tic() 16 | { 17 | start = std::chrono::system_clock::now(); 18 | } 19 | 20 | double toc() 21 | { 22 | end = std::chrono::system_clock::now(); 23 | std::chrono::duration elapsed_seconds = end - start; 24 | return elapsed_seconds.count() * 1000; 25 | } 26 | 27 | private: 28 | std::chrono::time_point start, end; 29 | }; 30 | -------------------------------------------------------------------------------- /vins_estimator/src/utility/utility.cpp: -------------------------------------------------------------------------------- 1 | #include "utility.h" 2 | 3 | Eigen::Matrix3d Utility::g2R(const Eigen::Vector3d &g) 4 | { 5 | Eigen::Matrix3d R0; 6 | Eigen::Vector3d ng1 = g.normalized(); 7 | Eigen::Vector3d ng2{0, 0, 1.0}; 8 | R0 = Eigen::Quaterniond::FromTwoVectors(ng1, ng2).toRotationMatrix(); 9 | double yaw = Utility::R2ypr(R0).x(); 10 | R0 = Utility::ypr2R(Eigen::Vector3d{-yaw, 0, 0}) * R0; 11 | // R0 = Utility::ypr2R(Eigen::Vector3d{-90, 0, 0}) * R0; 12 | return R0; 13 | } 14 | -------------------------------------------------------------------------------- /vins_estimator/src/utility/utility.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | class Utility 13 | { 14 | public: 15 | template 16 | static Eigen::Quaternion deltaQ(const Eigen::MatrixBase &theta) 17 | { 18 | typedef typename Derived::Scalar Scalar_t; 19 | 20 | Eigen::Quaternion dq; 21 | Eigen::Matrix half_theta = theta; 22 | half_theta /= static_cast(2.0); 23 | dq.w() = static_cast(1.0); 24 | dq.x() = half_theta.x(); 25 | dq.y() = half_theta.y(); 26 | dq.z() = half_theta.z(); 27 | return dq; 28 | } 29 | 30 | template 31 | static Eigen::Matrix skewSymmetric(const Eigen::MatrixBase &q) 32 | { 33 | Eigen::Matrix ans; 34 | ans << typename Derived::Scalar(0), -q(2), q(1), 35 | q(2), typename Derived::Scalar(0), -q(0), 36 | -q(1), q(0), typename Derived::Scalar(0); 37 | return ans; 38 | } 39 | 40 | template 41 | static Eigen::Quaternion positify(const Eigen::QuaternionBase &q) 42 | { 43 | //printf("a: %f %f %f %f", q.w(), q.x(), q.y(), q.z()); 44 | //Eigen::Quaternion p(-q.w(), -q.x(), -q.y(), -q.z()); 45 | //printf("b: %f %f %f %f", p.w(), p.x(), p.y(), p.z()); 46 | //return q.template w() >= (typename Derived::Scalar)(0.0) ? q : Eigen::Quaternion(-q.w(), -q.x(), -q.y(), -q.z()); 47 | return q; 48 | } 49 | 50 | template 51 | static Eigen::Matrix Qleft(const Eigen::QuaternionBase &q) 52 | { 53 | Eigen::Quaternion qq = positify(q); 54 | Eigen::Matrix ans; 55 | ans(0, 0) = qq.w(), ans.template block<1, 3>(0, 1) = -qq.vec().transpose(); 56 | ans.template block<3, 1>(1, 0) = qq.vec(), ans.template block<3, 3>(1, 1) = qq.w() * Eigen::Matrix::Identity() + skewSymmetric(qq.vec()); 57 | return ans; 58 | } 59 | 60 | template 61 | static Eigen::Matrix Qright(const Eigen::QuaternionBase &p) 62 | { 63 | Eigen::Quaternion pp = positify(p); 64 | Eigen::Matrix ans; 65 | ans(0, 0) = pp.w(), ans.template block<1, 3>(0, 1) = -pp.vec().transpose(); 66 | ans.template block<3, 1>(1, 0) = pp.vec(), ans.template block<3, 3>(1, 1) = pp.w() * Eigen::Matrix::Identity() - skewSymmetric(pp.vec()); 67 | return ans; 68 | } 69 | 70 | static Eigen::Vector3d R2ypr(const Eigen::Matrix3d &R) 71 | { 72 | Eigen::Vector3d n = R.col(0); 73 | Eigen::Vector3d o = R.col(1); 74 | Eigen::Vector3d a = R.col(2); 75 | 76 | Eigen::Vector3d ypr(3); 77 | double y = atan2(n(1), n(0)); 78 | double p = atan2(-n(2), n(0) * cos(y) + n(1) * sin(y)); 79 | double r = atan2(a(0) * sin(y) - a(1) * cos(y), -o(0) * sin(y) + o(1) * cos(y)); 80 | ypr(0) = y; 81 | ypr(1) = p; 82 | ypr(2) = r; 83 | 84 | return ypr / M_PI * 180.0; 85 | } 86 | 87 | template 88 | static Eigen::Matrix ypr2R(const Eigen::MatrixBase &ypr) 89 | { 90 | typedef typename Derived::Scalar Scalar_t; 91 | 92 | Scalar_t y = ypr(0) / 180.0 * M_PI; 93 | Scalar_t p = ypr(1) / 180.0 * M_PI; 94 | Scalar_t r = ypr(2) / 180.0 * M_PI; 95 | 96 | Eigen::Matrix Rz; 97 | Rz << cos(y), -sin(y), 0, 98 | sin(y), cos(y), 0, 99 | 0, 0, 1; 100 | 101 | Eigen::Matrix Ry; 102 | Ry << cos(p), 0., sin(p), 103 | 0., 1., 0., 104 | -sin(p), 0., cos(p); 105 | 106 | Eigen::Matrix Rx; 107 | Rx << 1., 0., 0., 108 | 0., cos(r), -sin(r), 109 | 0., sin(r), cos(r); 110 | 111 | return Rz * Ry * Rx; 112 | } 113 | 114 | static Eigen::Matrix3d g2R(const Eigen::Vector3d &g); 115 | 116 | template 117 | struct uint_ 118 | { 119 | }; 120 | 121 | template 122 | void unroller(const Lambda &f, const IterT &iter, uint_) 123 | { 124 | unroller(f, iter, uint_()); 125 | f(iter + N); 126 | } 127 | 128 | template 129 | void unroller(const Lambda &f, const IterT &iter, uint_<0>) 130 | { 131 | f(iter); 132 | } 133 | 134 | template 135 | static T normalizeAngle(const T& angle_degrees) { 136 | T two_pi(2.0 * 180); 137 | if (angle_degrees > 0) 138 | return angle_degrees - 139 | two_pi * std::floor((angle_degrees + T(180)) / two_pi); 140 | else 141 | return angle_degrees + 142 | two_pi * std::floor((-angle_degrees + T(180)) / two_pi); 143 | }; 144 | }; 145 | 146 | class FileSystemHelper 147 | { 148 | public: 149 | 150 | /****************************************************************************** 151 | * Recursively create directory if `path` not exists. 152 | * Return 0 if success. 153 | *****************************************************************************/ 154 | static int createDirectoryIfNotExists(const char *path) 155 | { 156 | struct stat info; 157 | int statRC = stat(path, &info); 158 | if( statRC != 0 ) 159 | { 160 | if (errno == ENOENT) 161 | { 162 | printf("%s not exists, trying to create it \n", path); 163 | if (! createDirectoryIfNotExists(dirname(strdupa(path)))) 164 | { 165 | if (mkdir(path, S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH) != 0) 166 | { 167 | fprintf(stderr, "Failed to create folder %s \n", path); 168 | return 1; 169 | } 170 | else 171 | return 0; 172 | } 173 | else 174 | return 1; 175 | } // directory not exists 176 | if (errno == ENOTDIR) 177 | { 178 | fprintf(stderr, "%s is not a directory path \n", path); 179 | return 1; 180 | } // something in path prefix is not a dir 181 | return 1; 182 | } 183 | return ( info.st_mode & S_IFDIR ) ? 0 : 1; 184 | } 185 | }; 186 | -------------------------------------------------------------------------------- /vins_estimator/src/utility/visualization.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include "CameraPoseVisualization.h" 17 | #include 18 | #include "../estimator.h" 19 | #include "../parameters.h" 20 | #include 21 | 22 | extern ros::Publisher pub_odometry; 23 | extern ros::Publisher pub_path, pub_pose; 24 | extern ros::Publisher pub_cloud, pub_map; 25 | extern ros::Publisher pub_key_poses; 26 | extern ros::Publisher pub_ref_pose, pub_cur_pose; 27 | extern ros::Publisher pub_key; 28 | extern nav_msgs::Path path; 29 | extern ros::Publisher pub_pose_graph; 30 | extern int IMAGE_ROW, IMAGE_COL; 31 | 32 | void registerPub(ros::NodeHandle &n); 33 | 34 | void pubLatestOdometry(const Eigen::Vector3d &P, const Eigen::Quaterniond &Q, const Eigen::Vector3d &V, const std_msgs::Header &header); 35 | 36 | void printStatistics(const Estimator &estimator, double t); 37 | 38 | void pubOdometry(const Estimator &estimator, const std_msgs::Header &header); 39 | 40 | void pubInitialGuess(const Estimator &estimator, const std_msgs::Header &header); 41 | 42 | void pubKeyPoses(const Estimator &estimator, const std_msgs::Header &header); 43 | 44 | void pubCameraPose(const Estimator &estimator, const std_msgs::Header &header); 45 | 46 | void pubPointCloud(const Estimator &estimator, const std_msgs::Header &header); 47 | 48 | void pubTF(const Estimator &estimator, const std_msgs::Header &header); 49 | 50 | void pubKeyframe(const Estimator &estimator); 51 | 52 | void pubRelocalization(const Estimator &estimator); --------------------------------------------------------------------------------