├── LICENCE ├── README.md ├── changed_files_vins_mono ├── Camera.cc ├── PinholeCamera.cc └── PinholeCamera.h ├── config ├── 3dm │ └── 3dm_config.yaml ├── A3 │ └── A3_config.yaml ├── AR_demo.rviz ├── cam_rantan.yaml ├── euroc │ ├── euroc_config.yaml │ ├── euroc_config_no_extrinsic.yaml │ ├── ex_calib_result.yaml │ └── vins_result.csv ├── extrinsic_parameter_example.pdf ├── fisheye_mask.jpg ├── fisheye_mask_752x480.jpg ├── robocane_config.yaml ├── robocane_config_320.yaml ├── tum_config.yaml ├── tum_vio_config.yaml └── vins_rviz_config.rviz ├── feature_tracker ├── CMakeLists.txt ├── freak │ ├── CMakeLists.txt │ ├── freak.cpp │ ├── freak.h │ ├── freak_demo.cpp │ └── hammingseg.h ├── package.xml └── src │ ├── feature_tracker.cpp │ ├── feature_tracker.h │ ├── feature_tracker_node.cpp │ ├── feature_tracker_node_bag.cpp │ ├── frontend.cpp │ ├── frontend.h │ ├── keyframe.cpp │ ├── keyframe.h │ ├── kf_node.cpp │ ├── kf_tracker.cpp │ ├── kf_tracker.h │ ├── parameters.cpp │ ├── parameters.h │ ├── test_cam_model.cpp │ ├── test_cam_model_2.cpp │ ├── test_cam_model_lm.cpp │ └── tic_toc.h ├── launch ├── feature_track.launch ├── kf_track.launch ├── mapping.launch ├── robocane_data.launch ├── robocane_data_320.launch ├── run_tum_vio │ ├── run_estimator.launch │ ├── run_feature_tracker.launch │ ├── run_together.launch │ ├── run_together_no_view.launch │ ├── run_vins-mono_ext.sh │ ├── test.launch │ └── vins_feature_ext.launch ├── tmp.launch ├── vins_feature_ext.launch ├── vins_local_kf.launch ├── vins_local_kf_bag.launch ├── vins_rviz.launch ├── vins_rviz_config.rviz ├── vins_rviz_ext.launch └── vins_rviz_ext_compare.launch ├── matlab_tools ├── distortion_model │ ├── 1.png │ ├── Equi2Pin.m │ ├── Equi2Pin4.m │ ├── Undistort.m │ ├── distortEquidist.m │ ├── equi-out.png │ ├── out.png │ ├── radtan-out.png │ ├── radtan-out_4.png │ └── test.m ├── libs_dir │ └── gtsam-toolbox-2.3.0-win64 │ │ └── toolbox │ │ ├── +gtsam │ │ ├── +noiseModel │ │ │ ├── +mEstimator │ │ │ │ ├── Base.m │ │ │ │ ├── Fair.m │ │ │ │ ├── Huber.m │ │ │ │ ├── Null.m │ │ │ │ └── Tukey.m │ │ │ ├── Base.m │ │ │ ├── Constrained.m │ │ │ ├── Diagonal.m │ │ │ ├── Gaussian.m │ │ │ ├── Isotropic.m │ │ │ ├── Robust.m │ │ │ └── Unit.m │ │ ├── +utilities │ │ │ ├── allPose3s.m │ │ │ ├── extractPoint2.m │ │ │ ├── extractPoint3.m │ │ │ ├── extractPose2.m │ │ │ ├── extractPose3.m │ │ │ ├── insertBackprojections.m │ │ │ ├── insertProjectionFactors.m │ │ │ ├── perturbPoint2.m │ │ │ ├── perturbPoint3.m │ │ │ └── reprojectionErrors.m │ │ ├── BearingFactor2D.m │ │ ├── BearingRangeFactor2D.m │ │ ├── BetweenFactorLieMatrix.m │ │ ├── BetweenFactorLieScalar.m │ │ ├── BetweenFactorLieVector.m │ │ ├── BetweenFactorPoint2.m │ │ ├── BetweenFactorPoint3.m │ │ ├── BetweenFactorPose2.m │ │ ├── BetweenFactorPose3.m │ │ ├── BetweenFactorRot2.m │ │ ├── BetweenFactorRot3.m │ │ ├── CHECK.m │ │ ├── Cal3DS2.m │ │ ├── Cal3_S2.m │ │ ├── Cal3_S2Stereo.m │ │ ├── CalibratedCamera.m │ │ ├── ConjugateGradientParameters.m │ │ ├── Contents.m │ │ ├── DoglegOptimizer.m │ │ ├── DoglegParams.m │ │ ├── EQUALITY.m │ │ ├── Errors.m │ │ ├── GaussNewtonOptimizer.m │ │ ├── GaussNewtonParams.m │ │ ├── GaussianBayesNet.m │ │ ├── GaussianBayesNetBase.m │ │ ├── GaussianBayesTree.m │ │ ├── GaussianBayesTreeBase.m │ │ ├── GaussianBayesTreeClique.m │ │ ├── GaussianConditional.m │ │ ├── GaussianDensity.m │ │ ├── GaussianFactor.m │ │ ├── GaussianFactorGraph.m │ │ ├── GaussianISAM.m │ │ ├── GaussianMultifrontalSolver.m │ │ ├── GaussianSequentialSolver.m │ │ ├── GeneralSFMFactor2Cal3_S2.m │ │ ├── GeneralSFMFactorCal3DS2.m │ │ ├── GeneralSFMFactorCal3_S2.m │ │ ├── GenericProjectionFactorCal3DS2.m │ │ ├── GenericProjectionFactorCal3_S2.m │ │ ├── GenericStereoFactor3D.m │ │ ├── HessianFactor.m │ │ ├── ISAM2.m │ │ ├── ISAM2BayesTree.m │ │ ├── ISAM2Clique.m │ │ ├── ISAM2DoglegParams.m │ │ ├── ISAM2GaussNewtonParams.m │ │ ├── ISAM2Params.m │ │ ├── ISAM2Result.m │ │ ├── ISAM2ThresholdMap.m │ │ ├── ISAM2ThresholdMapValue.m │ │ ├── IndexConditional.m │ │ ├── IndexFactor.m │ │ ├── IterativeOptimizationParameters.m │ │ ├── JacobianFactor.m │ │ ├── JointMarginal.m │ │ ├── KalmanFilter.m │ │ ├── KeyList.m │ │ ├── KeySet.m │ │ ├── KeyVector.m │ │ ├── LabeledSymbol.m │ │ ├── LevenbergMarquardtOptimizer.m │ │ ├── LevenbergMarquardtParams.m │ │ ├── LieMatrix.m │ │ ├── LieScalar.m │ │ ├── LieVector.m │ │ ├── LinearContainerFactor.m │ │ ├── Marginals.m │ │ ├── NonlinearEqualityCal3_S2.m │ │ ├── NonlinearEqualityCalibratedCamera.m │ │ ├── NonlinearEqualityLieMatrix.m │ │ ├── NonlinearEqualityLieScalar.m │ │ ├── NonlinearEqualityLieVector.m │ │ ├── NonlinearEqualityPoint2.m │ │ ├── NonlinearEqualityPoint3.m │ │ ├── NonlinearEqualityPose2.m │ │ ├── NonlinearEqualityPose3.m │ │ ├── NonlinearEqualityRot2.m │ │ ├── NonlinearEqualityRot3.m │ │ ├── NonlinearEqualitySimpleCamera.m │ │ ├── NonlinearEqualityStereoPoint2.m │ │ ├── NonlinearFactor.m │ │ ├── NonlinearFactorGraph.m │ │ ├── NonlinearISAM.m │ │ ├── NonlinearOptimizer.m │ │ ├── NonlinearOptimizerParams.m │ │ ├── Ordering.m │ │ ├── Permutation.m │ │ ├── PinholeCameraCal3DS2.m │ │ ├── Point2.m │ │ ├── Point3.m │ │ ├── Pose2.m │ │ ├── Pose3.m │ │ ├── PoseRotationPrior2D.m │ │ ├── PoseRotationPrior3D.m │ │ ├── PoseTranslationPrior2D.m │ │ ├── PoseTranslationPrior3D.m │ │ ├── PriorFactorCal3_S2.m │ │ ├── PriorFactorCalibratedCamera.m │ │ ├── PriorFactorLieMatrix.m │ │ ├── PriorFactorLieScalar.m │ │ ├── PriorFactorLieVector.m │ │ ├── PriorFactorPoint2.m │ │ ├── PriorFactorPoint3.m │ │ ├── PriorFactorPose2.m │ │ ├── PriorFactorPose3.m │ │ ├── PriorFactorRot2.m │ │ ├── PriorFactorRot3.m │ │ ├── PriorFactorSimpleCamera.m │ │ ├── PriorFactorStereoPoint2.m │ │ ├── RangeFactorCalibratedCamera.m │ │ ├── RangeFactorCalibratedCameraPoint.m │ │ ├── RangeFactorPose2.m │ │ ├── RangeFactorPose3.m │ │ ├── RangeFactorPosePoint2.m │ │ ├── RangeFactorPosePoint3.m │ │ ├── RangeFactorSimpleCamera.m │ │ ├── RangeFactorSimpleCameraPoint.m │ │ ├── Rot2.m │ │ ├── Rot3.m │ │ ├── Sampler.m │ │ ├── SimpleCamera.m │ │ ├── StereoPoint2.m │ │ ├── SubgraphSolver.m │ │ ├── SubgraphSolverParameters.m │ │ ├── SuccessiveLinearizationParams.m │ │ ├── SymbolicBayesNet.m │ │ ├── SymbolicBayesNetBase.m │ │ ├── SymbolicBayesTree.m │ │ ├── SymbolicBayesTreeBase.m │ │ ├── SymbolicBayesTreeClique.m │ │ ├── SymbolicFactorGraph.m │ │ ├── SymbolicMultifrontalSolver.m │ │ ├── SymbolicSequentialSolver.m │ │ ├── Value.m │ │ ├── Values.m │ │ ├── VariableIndex.m │ │ ├── VectorValues.m │ │ ├── VisualISAMGenerateData.m │ │ ├── VisualISAMInitialize.m │ │ ├── VisualISAMPlot.m │ │ ├── VisualISAMStep.m │ │ ├── allocateVectorValues.m │ │ ├── circlePose2.m │ │ ├── circlePose3.m │ │ ├── covarianceEllipse.m │ │ ├── covarianceEllipse3D.m │ │ ├── determinant.m │ │ ├── findExampleDataFile.m │ │ ├── gradient.m │ │ ├── hasKeyIntersection.m │ │ ├── keyDifference.m │ │ ├── keyIntersection.m │ │ ├── linear_independent.m │ │ ├── load2D.m │ │ ├── load3D.m │ │ ├── mrsymbol.m │ │ ├── mrsymbolChr.m │ │ ├── mrsymbolIndex.m │ │ ├── mrsymbolLabel.m │ │ ├── optimize.m │ │ ├── optimizeGradientSearch.m │ │ ├── plot2DPoints.m │ │ ├── plot2DTrajectory.m │ │ ├── plot3DPoints.m │ │ ├── plot3DTrajectory.m │ │ ├── plotBayesNet.m │ │ ├── plotBayesTree.m │ │ ├── plotCamera.m │ │ ├── plotPoint2.m │ │ ├── plotPoint3.m │ │ ├── plotPose2.m │ │ ├── plotPose3.m │ │ ├── printKeySet.m │ │ ├── summarize.m │ │ ├── summarizeAsNonlinearContainer.m │ │ ├── symbol.m │ │ ├── symbolChr.m │ │ └── symbolIndex.m │ │ ├── README-gtsam-toolbox.txt │ │ ├── gtsam_examples │ │ ├── Data │ │ │ ├── VO_calibration.txt │ │ │ ├── VO_camera_poses_large.txt │ │ │ ├── VO_stereo_factors_large.txt │ │ │ ├── example.graph │ │ │ ├── sphere2500.txt │ │ │ ├── sphere2500_groundtruth.txt │ │ │ ├── sphere_smallnoise.graph │ │ │ ├── w100-odom.graph │ │ │ └── w10000-odom.graph │ │ ├── LocalizationExample.m │ │ ├── OdometryExample.m │ │ ├── PlanarSLAMExample.m │ │ ├── PlanarSLAMExample_graph.m │ │ ├── PlanarSLAMExample_sampling.m │ │ ├── Pose2SLAMExample.m │ │ ├── Pose2SLAMExample_advanced.m │ │ ├── Pose2SLAMExample_circle.m │ │ ├── Pose2SLAMExample_graph.m │ │ ├── Pose2SLAMwSPCG.m │ │ ├── Pose3SLAMExample.m │ │ ├── Pose3SLAMExample_graph.m │ │ ├── SBAExample.m │ │ ├── SFMExample.m │ │ ├── StereoVOExample.m │ │ ├── StereoVOExample_large.m │ │ ├── VisualISAMDemo.m │ │ ├── VisualISAMExample.m │ │ ├── VisualISAM_gui.fig │ │ ├── VisualISAM_gui.m │ │ ├── gtsamExamples.fig │ │ └── gtsamExamples.m │ │ ├── gtsam_tests │ │ ├── testJacobianFactor.m │ │ ├── testKalmanFilter.m │ │ ├── testLocalizationExample.m │ │ ├── testOdometryExample.m │ │ ├── testPlanarSLAMExample.m │ │ ├── testPose2SLAMExample.m │ │ ├── testPose3SLAMExample.m │ │ ├── testSFMExample.m │ │ ├── testStereoVOExample.m │ │ ├── testThinBayesTree.m │ │ ├── testThinTree.m │ │ ├── testThinTreeBayesNet.m │ │ ├── testVisualISAMExample.m │ │ ├── test_gtsam.m │ │ ├── thinBayesTree.m │ │ ├── thinTree.m │ │ └── thinTreeBayesNet.m │ │ └── gtsam_wrapper.mexw64 ├── result_comp │ ├── VINS-Mono │ │ ├── ETAS_2F_640_30 │ │ │ ├── run_1.csv │ │ │ ├── run_10.csv │ │ │ ├── run_2.csv │ │ │ ├── run_3.csv │ │ │ ├── run_4.csv │ │ │ ├── run_5.csv │ │ │ ├── run_6.csv │ │ │ ├── run_7.csv │ │ │ ├── run_8.csv │ │ │ └── run_9.csv │ │ ├── ETAS_4F_640_30 │ │ │ ├── run_1.csv │ │ │ ├── run_10.csv │ │ │ ├── run_2.csv │ │ │ ├── run_3.csv │ │ │ ├── run_4.csv │ │ │ ├── run_5.csv │ │ │ ├── run_6.csv │ │ │ ├── run_7.csv │ │ │ ├── run_8.csv │ │ │ └── run_9.csv │ │ ├── ETAS_F2_640_30 │ │ │ ├── run_1.csv │ │ │ ├── run_10.csv │ │ │ ├── run_2.csv │ │ │ ├── run_3.csv │ │ │ ├── run_4.csv │ │ │ ├── run_5.csv │ │ │ ├── run_6.csv │ │ │ ├── run_7.csv │ │ │ ├── run_8.csv │ │ │ └── run_9.csv │ │ └── ETAS_F4_640_30 │ │ │ ├── run_1.csv │ │ │ ├── run_10.csv │ │ │ ├── run_2.csv │ │ │ ├── run_3.csv │ │ │ ├── run_4.csv │ │ │ ├── run_5.csv │ │ │ ├── run_6.csv │ │ │ ├── run_7.csv │ │ │ ├── run_8.csv │ │ │ └── run_9.csv │ ├── VINS-Mono_ext │ │ ├── ETAS_2F_640_30 │ │ │ ├── run_1.csv │ │ │ ├── run_10.csv │ │ │ ├── run_2.csv │ │ │ ├── run_3.csv │ │ │ ├── run_4.csv │ │ │ ├── run_5.csv │ │ │ ├── run_6.csv │ │ │ ├── run_7.csv │ │ │ ├── run_8.csv │ │ │ └── run_9.csv │ │ ├── ETAS_4F_640_30 │ │ │ ├── run_1.csv │ │ │ ├── run_10.csv │ │ │ ├── run_2.csv │ │ │ ├── run_3.csv │ │ │ ├── run_4.csv │ │ │ ├── run_5.csv │ │ │ ├── run_6.csv │ │ │ ├── run_7.csv │ │ │ ├── run_8.csv │ │ │ └── run_9.csv │ │ ├── ETAS_F2_640_30 │ │ │ ├── run_1.csv │ │ │ ├── run_10.csv │ │ │ ├── run_2.csv │ │ │ ├── run_3.csv │ │ │ ├── run_4.csv │ │ │ ├── run_5.csv │ │ │ ├── run_6.csv │ │ │ ├── run_7.csv │ │ │ ├── run_8.csv │ │ │ └── run_9.csv │ │ └── ETAS_F4_640_30 │ │ │ ├── run_1.csv │ │ │ ├── run_10.csv │ │ │ ├── run_2.csv │ │ │ ├── run_3.csv │ │ │ ├── run_4.csv │ │ │ ├── run_5.csv │ │ │ ├── run_6.csv │ │ │ ├── run_7.csv │ │ │ ├── run_8.csv │ │ │ └── run_9.csv │ ├── analyze_data.m │ └── result.docx └── simulation │ ├── SFM.m │ ├── add_path.m │ ├── computeMeanSigma.m │ ├── computeRMSE.m │ ├── computeRMSEArray.m │ ├── createFeatureHorizontal.m │ ├── createObservations.m │ ├── createObservations_VINS.m │ ├── createTrajectory.m │ ├── default_option.m │ ├── error_bar.fig │ ├── error_bar.png │ ├── extractTrajectory.m │ ├── get_rs_r200.m │ ├── in_cam_view.m │ ├── main_swing_simulation.m │ ├── main_swing_simulation_statistics.m │ ├── mu_sigma.mat │ ├── mu_sigma_100.mat │ ├── param_global.m │ ├── plotRMSE.m │ ├── plot_result_statistics.m │ ├── plot_trajectory_and_gt.m │ ├── pose2pt.m │ ├── swing_simulation_ISAM.m │ ├── swing_simulation_ISAM_Initialize.m │ ├── swing_simulation_ISAM_Plot.m │ ├── swing_simulation_ISAM_Step.m │ ├── swing_simulation_ISAM_VINS.m │ ├── swing_simulation_ISAM_data.m │ ├── swing_simulation_Plot.m │ ├── swing_simulation_Plot_surf.m │ ├── swing_simulation_SFM.m │ ├── swing_simulation_SFM_VINS.m │ ├── swing_simulation_data.m │ ├── swing_simulation_data_VINS.m │ ├── testMeanSigma.m │ ├── tiltR.m │ └── trajectory.png ├── support_files ├── brief_k10L6.bin ├── brief_pattern.yml ├── cmake │ └── FindEigen.cmake ├── image │ ├── vins.png │ └── vins_black.png └── paper │ ├── jfr2017yi.pdf │ └── tro_technical_report.pdf ├── sync_time ├── CMakeLists.txt ├── package.xml └── src │ ├── bundle_adjust.cpp │ ├── bundle_adjust.h │ ├── estimator_wrapper.cpp │ ├── estimator_wrapper.h │ ├── feature_manager.cpp │ ├── feature_manager.h │ ├── imu_structure.cpp │ ├── imu_structure.h │ ├── main_sync_time.cpp │ ├── main_sync_time2.cpp │ ├── read_file.cpp │ ├── rotation_error.cpp │ ├── rotation_error.h │ ├── shift_imu.cpp │ ├── shift_imu.h │ ├── shift_imu2.cpp │ ├── shift_imu2.h │ ├── track_feature.cpp │ ├── track_feature.h │ └── vision_sfm.cpp ├── vins_estimator ├── CMakeLists.txt ├── cmake │ └── FindEigen.cmake ├── launch │ ├── .tum_vio.launch.swp │ ├── 3dm.launch │ ├── black_box.launch │ ├── cla.launch │ ├── euroc.launch │ ├── euroc_no_extrinsic_param.launch │ ├── realsense_color.launch │ ├── realsense_fisheye.launch │ ├── tum.launch │ ├── tum_vio.launch │ └── vins_rviz.launch ├── package.xml └── src │ ├── estimator.cpp │ ├── estimator.cpp_back │ ├── estimator.cpp_back2 │ ├── estimator.cpp_before │ ├── estimator.h │ ├── estimator.h_back │ ├── estimator_node.cpp │ ├── factor │ ├── imu_factor.h │ ├── integration_base.h │ ├── marginalization_factor.cpp │ ├── marginalization_factor.h │ ├── pose_local_parameterization.cpp │ ├── pose_local_parameterization.h │ ├── projection_factor.cpp │ ├── projection_factor.h │ ├── projection_td_factor.cpp │ └── projection_td_factor.h │ ├── feature_manager.cpp │ ├── feature_manager.cpp_back │ ├── feature_manager.cpp_back2 │ ├── feature_manager.cpp_before │ ├── feature_manager.h │ ├── feature_manager.h_back │ ├── feature_manager.h_back2 │ ├── feature_manager.h_before │ ├── 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 │ ├── parameters.cpp │ ├── parameters.h │ └── utility │ ├── CameraPoseVisualization.cpp │ ├── CameraPoseVisualization.h │ ├── tic_toc.h │ ├── utility.cpp │ ├── utility.h │ ├── visualization.cpp │ └── visualization.h └── vins_estimator_back ├── .CMakeLists.txt.swp ├── CMakeLists.txt_back ├── launch ├── 3dm.launch ├── A3.launch ├── euroc.launch ├── euroc_no_extrinsic_param.launch └── vins_rviz.launch ├── package.xml_back └── src ├── check_project_factor.cpp ├── estimator.cpp ├── estimator.cpp_back2 ├── estimator.h ├── estimator.h_back2 ├── estimator_node.cpp ├── estimator_node.cpp_back2 ├── factor ├── .ipynb_checkpoints │ └── plot_w-checkpoint.ipynb ├── imu_factor.h ├── integration_base.h ├── marginalization_factor.cpp ├── marginalization_factor.h ├── plot_w.ipynb ├── pose_local_parameterization.cpp ├── pose_local_parameterization.h ├── projection_factor.cpp ├── projection_factor.h ├── weight_projection_factor.cpp └── weight_projection_factor.h ├── feature_manager.cpp ├── feature_manager.cpp_back2 ├── feature_manager.h ├── feature_manager.h_back2 ├── 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 ├── loop-closure ├── DLoopDetector.h ├── TemplatedLoopDetector.h ├── ThirdParty │ ├── DBoW │ │ ├── BowVector.cpp │ │ ├── BowVector.h │ │ ├── DBoW2.h │ │ ├── FBrief.cpp │ │ ├── FBrief.h │ │ ├── FClass.h │ │ ├── FeatureVector.cpp │ │ ├── FeatureVector.h │ │ ├── QueryResults.cpp │ │ ├── QueryResults.h │ │ ├── ScoringObject.cpp │ │ ├── ScoringObject.h │ │ ├── TemplatedDatabase.h │ │ └── TemplatedVocabulary.h │ ├── DUtils │ │ ├── DException.h │ │ ├── DUtils.h │ │ ├── Random.cpp │ │ ├── Random.h │ │ ├── Timestamp.cpp │ │ └── Timestamp.h │ ├── DVision │ │ ├── BRIEF.cpp │ │ ├── BRIEF.h │ │ └── DVision.h │ ├── VocabularyBinary.cpp │ └── VocabularyBinary.hpp ├── demoDetector.h ├── keyframe.cpp ├── keyframe.h ├── keyframe_database.cpp ├── keyframe_database.h ├── loop_closure.cpp └── loop_closure.h ├── parameters.cpp ├── parameters.h └── utility ├── CameraPoseVisualization.cpp ├── CameraPoseVisualization.h ├── tic_toc.h ├── utility.cpp ├── utility.h ├── visualization.cpp └── visualization.h /config/cam_rantan.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #camera calibration 4 | model_type: PINHOLE 5 | camera_name: camera 6 | image_width: 512 7 | image_height: 512 8 | distortion_parameters: 9 | k1: -0.2847798 # -0.239552 # -0.2847798 10 | k2: 0.08245052 # 0.037056 # 0.08245052 11 | p1: -1.0946156e-6 # 3.5763956e-6 # -1.0946156e-6 12 | p2: 4.78701072e-6 # -1.4032145e-5 # 4.78701072e-6 13 | k3: -0.0104085 14 | projection_parameters: 15 | # 615.426, 625.456, 318.837, 240.594 16 | fx: 190.9785 17 | fy: 190.9733 18 | cx: 254.9317 19 | cy: 256.8974 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /config/euroc/ex_calib_result.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | extrinsicRotation: !!opencv-matrix 3 | rows: 3 4 | cols: 3 5 | dt: d 6 | data: [ 1.5087081689421966e-02, -9.9987316451290043e-01, 7 | 5.1024359922114014e-03, 9.9977674414763396e-01, 8 | 1.5160733030654239e-02, 1.4717813544457878e-02, 9 | -1.4793303473291847e-02, 4.8792479882799021e-03, 10 | 9.9987866819500448e-01 ] 11 | extrinsicTranslation: !!opencv-matrix 12 | rows: 3 13 | cols: 1 14 | dt: d 15 | data: [ -1.5943309272181735e-02, -6.9280146859510200e-02, 16 | -3.2146183419019701e-03 ] 17 | -------------------------------------------------------------------------------- /config/extrinsic_parameter_example.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rising-turtle/VINS-Mono_Ext/1aa44f5ec4c4a1b64726e8fe2d0d28ca3b6ade23/config/extrinsic_parameter_example.pdf -------------------------------------------------------------------------------- /config/fisheye_mask.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rising-turtle/VINS-Mono_Ext/1aa44f5ec4c4a1b64726e8fe2d0d28ca3b6ade23/config/fisheye_mask.jpg -------------------------------------------------------------------------------- /config/fisheye_mask_752x480.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rising-turtle/VINS-Mono_Ext/1aa44f5ec4c4a1b64726e8fe2d0d28ca3b6ade23/config/fisheye_mask_752x480.jpg -------------------------------------------------------------------------------- /feature_tracker/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(feature_tracker_ext) 3 | 4 | set(CMAKE_BUILD_TYPE "Release") 5 | set(CMAKE_CXX_FLAGS "-std=c++11 -O3 -w -mssse3") 6 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -w -g -mssse3") 7 | 8 | find_package(catkin REQUIRED COMPONENTS 9 | roscpp 10 | std_msgs 11 | sensor_msgs 12 | cv_bridge 13 | camera_model 14 | rosbag 15 | ) 16 | 17 | find_package(OpenCV REQUIRED) 18 | 19 | catkin_package() 20 | 21 | include_directories( 22 | ${catkin_INCLUDE_DIRS} 23 | ) 24 | 25 | set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) 26 | # find_package(Eigen3 REQUIRED) 27 | include_directories("/usr/include/eigen3") 28 | include_directories( 29 | ${catkin_INCLUDE_DIRS} 30 | ${EIGEN3_INCLUDE_DIR} 31 | ) 32 | 33 | add_executable(feature_tracker_ext 34 | src/feature_tracker_node.cpp 35 | src/parameters.cpp 36 | src/feature_tracker.cpp 37 | ) 38 | 39 | target_link_libraries(feature_tracker_ext ${catkin_LIBRARIES} ${OpenCV_LIBS}) 40 | 41 | add_executable(feature_tracker_ext_bag 42 | src/feature_tracker_node_bag.cpp 43 | src/parameters.cpp 44 | src/feature_tracker.cpp) 45 | 46 | target_link_libraries(feature_tracker_ext_bag ${catkin_LIBRARIES} ${OpenCV_LIBS}) 47 | 48 | # New freak node 49 | # add_subdirectory(freak) 50 | 51 | add_executable(kf_tracker 52 | src/kf_node.cpp 53 | src/parameters.cpp 54 | src/feature_tracker.cpp 55 | src/kf_tracker.cpp 56 | src/frontend.cpp 57 | src/keyframe.cpp 58 | ) 59 | target_link_libraries(kf_tracker ${catkin_LIBRARIES} ${OpenCV_LIBS}) 60 | 61 | add_executable(test_cam_model src/test_cam_model.cpp) 62 | target_link_libraries(test_cam_model ${catkin_LIBRARIES}) 63 | 64 | add_executable(test_cam_model_2 src/test_cam_model_2.cpp) 65 | add_executable(test_cam_model_lm src/test_cam_model_lm.cpp) 66 | -------------------------------------------------------------------------------- /feature_tracker/freak/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | include_directories( ${CMAKE_CURRENT_SOURCE_DIR} ) 2 | 3 | 4 | # ADD all sources files 5 | set( freak_SRCS 6 | freak.cpp 7 | ) 8 | 9 | # Static version 10 | add_library( freak_p STATIC ${freak_SRCS} ) 11 | target_link_libraries( freak_p ${OpenCV_LIBS} ) 12 | 13 | ######### 14 | # Shared library 15 | 16 | # Set public header 17 | set( freak_HDRS 18 | freak.h 19 | hammingseg.h 20 | ) 21 | 22 | add_library( freak SHARED ${freak_SRCS} ${freak_HDRS} ) 23 | target_link_libraries( freak ${OpenCV_LIBS} ) 24 | 25 | # add_executable(freak_demo freak_demo.cpp) 26 | # target_link_libraries(freak_demo freak_p ${OpenCV_LIBS}) 27 | 28 | -------------------------------------------------------------------------------- /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 | 25 | void reduceVector(vector &v, vector status); 26 | void reduceVector(vector &v, vector status); 27 | 28 | class FeatureTracker 29 | { 30 | public: 31 | FeatureTracker(); 32 | virtual ~FeatureTracker(); 33 | 34 | virtual void readImage(const cv::Mat &_img); 35 | 36 | void setMask(); 37 | 38 | void addPoints(); 39 | 40 | bool updateID(unsigned int i); 41 | 42 | void readIntrinsicParameter(const string &calib_file); 43 | 44 | void showUndistortion(const string &name); 45 | 46 | void undistortedPoints_new(); 47 | 48 | void rejectWithF(); 49 | 50 | bool liftPixel(Eigen::Vector2d pt, Eigen::Vector3d& un_pt); 51 | 52 | vector undistortedPoints(); 53 | 54 | cv::Mat mask; 55 | cv::Mat fisheye_mask; 56 | cv::Mat prev_img, cur_img, forw_img; 57 | vector n_pts; 58 | vector prev_pts, cur_pts, forw_pts; 59 | vector ids; 60 | vector track_cnt; 61 | camodocal::CameraPtr m_camera; 62 | 63 | vector prev_un_pts, cur_un_pts; 64 | map cur_un_pts_map; 65 | map prev_un_pts_map; 66 | 67 | static int n_id; 68 | }; 69 | -------------------------------------------------------------------------------- /feature_tracker/src/frontend.h: -------------------------------------------------------------------------------- 1 | /* 2 | Dec. 1 2017, He Zhang, hxzhang1@ualr.edu 3 | 4 | frontend - detect, track and match Freak features 5 | 6 | */ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | using namespace std; 15 | 16 | // class CFrame; // frame contains feature points, ids, obs_times, 17 | class FeatureTracker; 18 | 19 | class CFrontend 20 | { 21 | public: 22 | CFrontend(FeatureTracker*); 23 | virtual ~CFrontend(); 24 | 25 | // image callback 26 | void imgCallback( const sensor_msgs::ImageConstPtr &img_msg); 27 | 28 | // for debug 29 | void imgCallback2( const sensor_msgs::ImageConstPtr &img_msg); 30 | 31 | // publish this frame 32 | void pubCurFrame(const sensor_msgs::ImageConstPtr &img_msg); 33 | // bool mbPubThisFrame; 34 | 35 | int mPubCnt; // count the number of published frames 36 | // double mFREQ; // frequency control for publish 37 | 38 | // queue mqKF; // keyframes 39 | 40 | FeatureTracker* mpFTracker; // feature tracker 41 | 42 | // parameters 43 | double mFirstImgTime ; 44 | bool mbFirstImgFlag; 45 | bool mbShowTrack; // whether to publish img with tracked feature for display 46 | // int mWinSize; // number of nodes in the window, outside nodes are marginalized 47 | 48 | bool init_pub; 49 | // publisher 50 | ros::Publisher mPubImg; 51 | ros::Publisher mPubMatch; 52 | }; 53 | 54 | -------------------------------------------------------------------------------- /feature_tracker/src/keyframe.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Dec. 2 2017, He Zhang, hxzhang1@ualr.edu 3 | 4 | keyframe structure 5 | 6 | */ 7 | 8 | #include "keyframe.h" 9 | // #include "../freak/freak.h" 10 | // #include "../freak/hammingseg.h" 11 | // #include 12 | #include 13 | #include 14 | 15 | using namespace std; 16 | 17 | CKeyFrame::CKeyFrame(): 18 | mbDiscribed(false) 19 | {} 20 | CKeyFrame::~CKeyFrame(){} 21 | /* 22 | void CKeyFrame::describe(cv::Mat& img, vector& kpts, cv::Mat& desc) 23 | { 24 | cv_ext::FREAK extractor; 25 | // extractor.compute(mImg, mvKPts, mDiscriptor); 26 | desc = cv::Mat(); 27 | extractor.compute(img, kpts, desc); 28 | return ; 29 | } 30 | 31 | void CKeyFrame::describe() 32 | { 33 | if(mbDiscribed) return; 34 | vector tmpPts = mvKPts; 35 | describe(mImg, tmpPts, mDiscriptor); 36 | mDes2Id.resize(mDiscriptor.rows); 37 | for(int i=0; i= mIds.size()) 52 | { 53 | cerr <<"i = "< mIds.size() = "< 11 | #include 12 | 13 | 14 | class CKeyFrame 15 | { 16 | public: 17 | CKeyFrame(); 18 | virtual ~CKeyFrame(); 19 | // void describe(); 20 | 21 | cv::Mat mImg; // camera image 22 | std::vector mvKPts; // key points in the mImg 23 | cv::Mat mDiscriptor; // store key points' discriptors 24 | std::vector mIds; // key point's id 25 | int mUnTracked; // number of untracked features 26 | std::vector mDes2Id; // map from descriptor to Id 27 | bool mbDiscribed; // whether the freak descriptor has been computed 28 | protected: 29 | // void describe(cv::Mat& img, std::vector& kpts, cv::Mat& desc); 30 | }; 31 | -------------------------------------------------------------------------------- /feature_tracker/src/parameters.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | 5 | extern int ROW; 6 | extern int COL; 7 | extern int FOCAL_LENGTH; 8 | const int NUM_OF_CAM = 1; 9 | 10 | 11 | extern std::string IMAGE_TOPIC; 12 | extern std::string IMU_TOPIC; 13 | extern std::string FISHEYE_MASK; 14 | extern std::vector CAM_NAMES; 15 | extern int MAX_CNT; 16 | extern int MIN_DIST; 17 | extern int WINDOW_SIZE; 18 | extern int FREQ; 19 | extern double F_THRESHOLD; 20 | extern int SHOW_TRACK; 21 | extern int STEREO_TRACK; 22 | extern int EQUALIZE; 23 | extern int FISHEYE; 24 | extern bool PUB_THIS_FRAME; 25 | 26 | // added 27 | extern double FEATURE_SIZE; // default feature size for calculating discriptor 28 | extern double NEW_KF_THRESHOLD; // threshold to create new KF 29 | extern int LEAST_NUM_FOR_PNP; // Least matches for start PnP 30 | extern int TRACKED_ENOUGH_FEATURE; // threhold to decide whether enough feature has been tracked 31 | extern int MAX_NUM_KFS; // max number of keyframes in the queue 32 | 33 | void readParameters(ros::NodeHandle &n); 34 | -------------------------------------------------------------------------------- /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/feature_track.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /launch/kf_track.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /launch/mapping.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /launch/robocane_data.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /launch/robocane_data_320.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /launch/run_tum_vio/run_estimator.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /launch/run_tum_vio/run_feature_tracker.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /launch/run_tum_vio/run_together.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 | -------------------------------------------------------------------------------- /launch/run_tum_vio/run_together_no_view.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /launch/run_tum_vio/test.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 | -------------------------------------------------------------------------------- /launch/run_tum_vio/vins_feature_ext.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /launch/tmp.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /launch/vins_feature_ext.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /launch/vins_local_kf.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /launch/vins_local_kf_bag.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /launch/vins_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /launch/vins_rviz_ext.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /launch/vins_rviz_ext_compare.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /matlab_tools/distortion_model/1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rising-turtle/VINS-Mono_Ext/1aa44f5ec4c4a1b64726e8fe2d0d28ca3b6ade23/matlab_tools/distortion_model/1.png -------------------------------------------------------------------------------- /matlab_tools/distortion_model/Equi2Pin.m: -------------------------------------------------------------------------------- 1 | function [ k1, k2, p1, p2, k3, err ] = Equi2Pin( imageSize, D, K ) 2 | 3 | if nargin == 0 4 | %% camera intrinsic parameters in TUM vio's dataset 5 | imageSize = [512 512]; 6 | D = [0.0034823894022493434, 0.0007150348452162257, -0.0020532361418706202, 0.00020293673591811182]; 7 | % pinhole-equi-512 8 | intri = [190.97847715128717, 190.9733070521226, 254.93170605935475, 256.8974428996504]; 9 | K = [intri(1) 0 intri(3) 10 | 0 intri(2) intri(4) 11 | 0 0 1]; 12 | end 13 | %split up K and D into parameters of interest 14 | fx = K(1,1); 15 | fy = K(2,2); 16 | cx = K(1,3); 17 | cy = K(2,3); 18 | k1 = D(1); 19 | k2 = D(2); 20 | k3 = D(3); 21 | k4 = D(4); 22 | 23 | image = zeros(imageSize); 24 | [i, j] = find(~isnan(image)); 25 | 26 | % Xp = the xyz vals of points on the z plane 27 | Xp = K\[j i ones(length(i),1)]'; 28 | 29 | % Now we calculate how those points distort i.e forward map them through the distortion 30 | x = Xp(1,:); 31 | y = Xp(2,:); 32 | 33 | %get distorted points 34 | % th = atan(sqrt(x.^2 + y.^2)); 35 | % theta = (theta + k1*theta + k2*theta.^3 + k3*theta.^5 + k4*theta.^7); 36 | % thetad = th*(1.0 + k1*th.^2 + k2*th.^4 + k3*th.^6 + k4*th.^8); 37 | % omega = atan2(y,x); 38 | % u = theta.*cos(omega); 39 | % v = theta.*sin(omega); 40 | 41 | % update equidist distort function 42 | [u, v] = distortEquidist(x, y, D); 43 | 44 | %optimize 45 | err_func = @(d) getPinErr(u,v,x,y,d); 46 | [D,err] = fminunc(err_func, [0,0,0,0,0]); 47 | 48 | k1 = D(1); 49 | k2 = D(2); 50 | k3 = D(5); 51 | p1 = D(3); 52 | p2 = D(4); 53 | 54 | end 55 | 56 | function [ err ] = getPinErr( u, v, x, y, D) 57 | 58 | k1 = D(1); 59 | k2 = D(2); 60 | k3 = D(5); 61 | p1 = D(3); 62 | p2 = D(4); 63 | 64 | r2 = x.^2 + y.^2; 65 | u_pin = x.*(1+k1*r2 + k2*r2.^2 + k3*r2.^3) + 2*p1.*x.*y + p2*(r2 + 2*x.^2); 66 | v_pin = y.*(1+k1*r2 + k2*r2.^2 + k3*r2.^3) + 2*p2.*x.*y + p1*(r2 + 2*y.^2); 67 | 68 | err = mean((u-u_pin).^2 + (v-v_pin).^2); 69 | 70 | end -------------------------------------------------------------------------------- /matlab_tools/distortion_model/Equi2Pin4.m: -------------------------------------------------------------------------------- 1 | function [ k1, k2, p1, p2, err ] = Equi2Pin4( imageSize, D, K ) 2 | 3 | if nargin == 0 4 | %% camera intrinsic parameters in TUM vio's dataset 5 | imageSize = [512 512]; 6 | D = [0.0034823894022493434, 0.0007150348452162257, -0.0020532361418706202, 0.00020293673591811182]; 7 | % pinhole-equi-512 8 | intri = [190.97847715128717, 190.9733070521226, 254.93170605935475, 256.8974428996504]; 9 | K = [intri(1) 0 intri(3) 10 | 0 intri(2) intri(4) 11 | 0 0 1]; 12 | end 13 | %split up K and D into parameters of interest 14 | fx = K(1,1); 15 | fy = K(2,2); 16 | cx = K(1,3); 17 | cy = K(2,3); 18 | k1 = D(1); 19 | k2 = D(2); 20 | k3 = D(3); 21 | k4 = D(4); 22 | 23 | image = zeros(imageSize); 24 | [i, j] = find(~isnan(image)); 25 | 26 | % Xp = the xyz vals of points on the z plane 27 | Xp = K\[j i ones(length(i),1)]'; 28 | 29 | % Now we calculate how those points distort i.e forward map them through the distortion 30 | x = Xp(1,:); 31 | y = Xp(2,:); 32 | 33 | %get distorted points 34 | % th = atan(sqrt(x.^2 + y.^2)); 35 | % theta = (theta + k1*theta + k2*theta.^3 + k3*theta.^5 + k4*theta.^7); 36 | % thetad = th*(1.0 + k1*th.^2 + k2*th.^4 + k3*th.^6 + k4*th.^8); 37 | % omega = atan2(y,x); 38 | % u = theta.*cos(omega); 39 | % v = theta.*sin(omega); 40 | 41 | % update equidist distort function 42 | [u, v] = distortEquidist(x, y, D); 43 | 44 | %optimize 45 | err_func = @(d) getPinErr(u,v,x,y,d); 46 | [D,err] = fminunc(err_func, [0,0,0,0,0]); 47 | 48 | k1 = D(1); 49 | k2 = D(2); 50 | % k3 = D(5); 51 | p1 = D(3); 52 | p2 = D(4); 53 | 54 | end 55 | 56 | function [ err ] = getPinErr( u, v, x, y, D) 57 | 58 | k1 = D(1); 59 | k2 = D(2); 60 | % k3 = D(5); 61 | p1 = D(3); 62 | p2 = D(4); 63 | 64 | r2 = x.^2 + y.^2; 65 | % u_pin = x.*(1+k1*r2 + k2*r2.^2 + k3*r2.^3) + 2*p1.*x.*y + p2*(r2 + 2*x.^2); 66 | % v_pin = y.*(1+k1*r2 + k2*r2.^2 + k3*r2.^3) + 2*p2.*x.*y + p1*(r2 + 2*y.^2); 67 | 68 | u_pin = x.*(1+k1*r2 + k2*r2.^2) + 2*p1.*x.*y + p2*(r2 + 2*x.^2); 69 | v_pin = y.*(1+k1*r2 + k2*r2.^2) + 2*p2.*x.*y + p1*(r2 + 2*y.^2); 70 | err = mean((u-u_pin).^2 + (v-v_pin).^2); 71 | 72 | end -------------------------------------------------------------------------------- /matlab_tools/distortion_model/distortEquidist.m: -------------------------------------------------------------------------------- 1 | function [u, v] = distortEquidist(x, y, D) 2 | k1 = D(1); 3 | k2 = D(2); 4 | k3 = D(3); 5 | k4 = D(4); 6 | u = repmat(x,1); 7 | v = repmat(y,1); 8 | for i = 1:size(u,2) 9 | xi = x(i); 10 | yi = y(i); 11 | r= sqrt(xi*xi + yi*yi); 12 | if r < 1e-8 13 | continue; 14 | end 15 | th = atan(r); 16 | thd = th*(1.0 + k1*th.^2 + k2*th.^4 + k3*th.^6 + k4*th.^8); 17 | s = thd/r; 18 | u(i) = xi * s; 19 | v(i) = yi * s; 20 | end 21 | end 22 | -------------------------------------------------------------------------------- /matlab_tools/distortion_model/equi-out.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rising-turtle/VINS-Mono_Ext/1aa44f5ec4c4a1b64726e8fe2d0d28ca3b6ade23/matlab_tools/distortion_model/equi-out.png -------------------------------------------------------------------------------- /matlab_tools/distortion_model/out.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rising-turtle/VINS-Mono_Ext/1aa44f5ec4c4a1b64726e8fe2d0d28ca3b6ade23/matlab_tools/distortion_model/out.png -------------------------------------------------------------------------------- /matlab_tools/distortion_model/radtan-out.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rising-turtle/VINS-Mono_Ext/1aa44f5ec4c4a1b64726e8fe2d0d28ca3b6ade23/matlab_tools/distortion_model/radtan-out.png -------------------------------------------------------------------------------- /matlab_tools/distortion_model/radtan-out_4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rising-turtle/VINS-Mono_Ext/1aa44f5ec4c4a1b64726e8fe2d0d28ca3b6ade23/matlab_tools/distortion_model/radtan-out_4.png -------------------------------------------------------------------------------- /matlab_tools/distortion_model/test.m: -------------------------------------------------------------------------------- 1 | imageD = imread('1.png'); 2 | % pinhole-equi-512 3 | intri = [190.97847715128717, 190.9733070521226, 254.93170605935475, 256.8974428996504]; 4 | D = [0.0034823894022493434, 0.0007150348452162257, -0.0020532361418706202, 0.00020293673591811182]; 5 | fu = intri(1); 6 | fv = intri(2); 7 | pu = intri(3); 8 | pv = intri(4); 9 | 10 | K = [fu 0 pu 11 | 0 fv pv 12 | 0 0 1]; 13 | unImg = Undistort(imageD, D, K, 'equi'); 14 | imshow(unImg); 15 | imwrite(unImg, 'equi-out.png'); 16 | 17 | 18 | % pinhole-radtan-512 19 | D = [-0.284779809132009,0.0824505247648799, -1.09461560717260e-06, ... 20 | 4.78701071508643e-06,-0.0104084647110111]; 21 | unImg = Undistort(imageD, D, K, 'radtan'); 22 | imshow(unImg); 23 | imwrite(unImg, 'radtan-out.png'); 24 | 25 | % pinhole-radtan-512 26 | D = [-0.239552085066865,0.037056076630330, 3.576395603190594e-06, ... 27 | -1.403214455830240e-05,0]; 28 | unImg = Undistort(imageD, D, K, 'radtan'); 29 | imshow(unImg); 30 | imwrite(unImg, 'radtan-out_4.png'); -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/+gtsam/+noiseModel/+mEstimator/Base.m: -------------------------------------------------------------------------------- 1 | %class Base, see Doxygen page for details 2 | %at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html 3 | % 4 | classdef Base < handle 5 | properties 6 | ptr_gtsamnoiseModelmEstimatorBase = 0 7 | end 8 | methods 9 | function obj = Base(varargin) 10 | if (nargin == 2 || (nargin == 3 && strcmp(varargin{3}, 'void'))) && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) 11 | if nargin == 2 12 | my_ptr = varargin{2}; 13 | else 14 | my_ptr = gtsam_wrapper(526, varargin{2}); 15 | end 16 | gtsam_wrapper(525, my_ptr); 17 | else 18 | error('Arguments do not match any overload of gtsam.noiseModel.mEstimator.Base constructor'); 19 | end 20 | obj.ptr_gtsamnoiseModelmEstimatorBase = my_ptr; 21 | end 22 | 23 | function delete(obj) 24 | gtsam_wrapper(527, obj.ptr_gtsamnoiseModelmEstimatorBase); 25 | end 26 | 27 | function display(obj), obj.print(''); end 28 | %DISPLAY Calls print on the object 29 | function disp(obj), obj.display; end 30 | %DISP Calls print on the object 31 | end 32 | 33 | methods(Static = true) 34 | end 35 | end 36 | -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/+gtsam/+noiseModel/Base.m: -------------------------------------------------------------------------------- 1 | %class Base, see Doxygen page for details 2 | %at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html 3 | % 4 | classdef Base < handle 5 | properties 6 | ptr_gtsamnoiseModelBase = 0 7 | end 8 | methods 9 | function obj = Base(varargin) 10 | if (nargin == 2 || (nargin == 3 && strcmp(varargin{3}, 'void'))) && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) 11 | if nargin == 2 12 | my_ptr = varargin{2}; 13 | else 14 | my_ptr = gtsam_wrapper(485, varargin{2}); 15 | end 16 | gtsam_wrapper(484, my_ptr); 17 | else 18 | error('Arguments do not match any overload of gtsam.noiseModel.Base constructor'); 19 | end 20 | obj.ptr_gtsamnoiseModelBase = my_ptr; 21 | end 22 | 23 | function delete(obj) 24 | gtsam_wrapper(486, obj.ptr_gtsamnoiseModelBase); 25 | end 26 | 27 | function display(obj), obj.print(''); end 28 | %DISPLAY Calls print on the object 29 | function disp(obj), obj.display; end 30 | %DISP Calls print on the object 31 | end 32 | 33 | methods(Static = true) 34 | end 35 | end 36 | -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/+gtsam/+utilities/allPose3s.m: -------------------------------------------------------------------------------- 1 | function varargout = allPose3s(varargin) 2 | if length(varargin) == 1 && isa(varargin{1},'gtsam.Values') 3 | varargout{1} = gtsam_wrapper(2123, varargin{:}); 4 | else 5 | error('Arguments do not match any overload of function gtsam.utilities.allPose3s'); 6 | end 7 | -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/+gtsam/+utilities/extractPoint2.m: -------------------------------------------------------------------------------- 1 | function varargout = extractPoint2(varargin) 2 | if length(varargin) == 1 && isa(varargin{1},'gtsam.Values') 3 | varargout{1} = gtsam_wrapper(2127, varargin{:}); 4 | else 5 | error('Arguments do not match any overload of function gtsam.utilities.extractPoint2'); 6 | end 7 | -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/+gtsam/+utilities/extractPoint3.m: -------------------------------------------------------------------------------- 1 | function varargout = extractPoint3(varargin) 2 | if length(varargin) == 1 && isa(varargin{1},'gtsam.Values') 3 | varargout{1} = gtsam_wrapper(2128, varargin{:}); 4 | else 5 | error('Arguments do not match any overload of function gtsam.utilities.extractPoint3'); 6 | end 7 | -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/+gtsam/+utilities/extractPose2.m: -------------------------------------------------------------------------------- 1 | function varargout = extractPose2(varargin) 2 | if length(varargin) == 1 && isa(varargin{1},'gtsam.Values') 3 | varargout{1} = gtsam_wrapper(2129, varargin{:}); 4 | else 5 | error('Arguments do not match any overload of function gtsam.utilities.extractPose2'); 6 | end 7 | -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/+gtsam/+utilities/extractPose3.m: -------------------------------------------------------------------------------- 1 | function varargout = extractPose3(varargin) 2 | if length(varargin) == 1 && isa(varargin{1},'gtsam.Values') 3 | varargout{1} = gtsam_wrapper(2130, varargin{:}); 4 | else 5 | error('Arguments do not match any overload of function gtsam.utilities.extractPose3'); 6 | end 7 | -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/+gtsam/+utilities/insertBackprojections.m: -------------------------------------------------------------------------------- 1 | function varargout = insertBackprojections(varargin) 2 | if length(varargin) == 5 && isa(varargin{1},'gtsam.Values') && isa(varargin{2},'gtsam.SimpleCamera') && isa(varargin{3},'double') && isa(varargin{4},'double') && isa(varargin{5},'double') 3 | gtsam_wrapper(2133, varargin{:}); 4 | else 5 | error('Arguments do not match any overload of function gtsam.utilities.insertBackprojections'); 6 | end 7 | -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/+gtsam/+utilities/insertProjectionFactors.m: -------------------------------------------------------------------------------- 1 | function varargout = insertProjectionFactors(varargin) 2 | if length(varargin) == 6 && isa(varargin{1},'gtsam.NonlinearFactorGraph') && isa(varargin{2},'numeric') && isa(varargin{3},'double') && isa(varargin{4},'double') && isa(varargin{5},'gtsam.noiseModel.Base') && isa(varargin{6},'gtsam.Cal3_S2') 3 | gtsam_wrapper(2134, varargin{:}); 4 | elseif length(varargin) == 7 && isa(varargin{1},'gtsam.NonlinearFactorGraph') && isa(varargin{2},'numeric') && isa(varargin{3},'double') && isa(varargin{4},'double') && isa(varargin{5},'gtsam.noiseModel.Base') && isa(varargin{6},'gtsam.Cal3_S2') && isa(varargin{7},'gtsam.Pose3') 5 | gtsam_wrapper(2135, varargin{:}); 6 | else 7 | error('Arguments do not match any overload of function gtsam.utilities.insertProjectionFactors'); 8 | end 9 | -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/+gtsam/+utilities/perturbPoint2.m: -------------------------------------------------------------------------------- 1 | function varargout = perturbPoint2(varargin) 2 | if length(varargin) == 3 && isa(varargin{1},'gtsam.Values') && isa(varargin{2},'double') && isa(varargin{3},'numeric') 3 | gtsam_wrapper(2149, varargin{:}); 4 | else 5 | error('Arguments do not match any overload of function gtsam.utilities.perturbPoint2'); 6 | end 7 | -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/+gtsam/+utilities/perturbPoint3.m: -------------------------------------------------------------------------------- 1 | function varargout = perturbPoint3(varargin) 2 | if length(varargin) == 3 && isa(varargin{1},'gtsam.Values') && isa(varargin{2},'double') && isa(varargin{3},'numeric') 3 | gtsam_wrapper(2150, varargin{:}); 4 | else 5 | error('Arguments do not match any overload of function gtsam.utilities.perturbPoint3'); 6 | end 7 | -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/+gtsam/+utilities/reprojectionErrors.m: -------------------------------------------------------------------------------- 1 | function varargout = reprojectionErrors(varargin) 2 | if length(varargin) == 2 && isa(varargin{1},'gtsam.NonlinearFactorGraph') && isa(varargin{2},'gtsam.Values') 3 | varargout{1} = gtsam_wrapper(2153, varargin{:}); 4 | else 5 | error('Arguments do not match any overload of function gtsam.utilities.reprojectionErrors'); 6 | end 7 | -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/+gtsam/CHECK.m: -------------------------------------------------------------------------------- 1 | function CHECK(name,assertion) 2 | % CHECK throw an error if an assertion fails 3 | % 4 | % CHECK(name,assertion) 5 | % - name of test 6 | % - assertion 7 | 8 | if (assertion~=1) 9 | error(['CHECK ' name ' fails']); 10 | end 11 | -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/+gtsam/EQUALITY.m: -------------------------------------------------------------------------------- 1 | function EQUALITY(name,expected,actual,tol) 2 | % test equality of two vectors/matrices up to tolerance 3 | 4 | if nargin<4,tol=1e-9;end 5 | 6 | sameSize = size(expected)==size(actual); 7 | if all(sameSize) 8 | equal = abs(expected-actual)xZ--> Y (z pointing towards viewer, Z pointing away from viewer) 12 | % Vehicle at p0 is looking towards y axis (X-axis points towards world y) 13 | 14 | if nargin<3,symbolChar=0;end 15 | if nargin<2,radius=1.0;end 16 | if nargin<1,numPoses=8;end 17 | 18 | % Force symbolChar to be a single character 19 | symbolChar = char(symbolChar); 20 | symbolChar = symbolChar(1); 21 | 22 | values = gtsam.Values; 23 | theta = 0.0; 24 | dtheta = 2*pi()/numPoses; 25 | for i = 1:numPoses 26 | key = gtsam.symbol(symbolChar, i-1); 27 | pose = gtsam.Pose2(radius*cos(theta), radius*sin(theta), pi()/2 + theta); 28 | values.insert(key, pose); 29 | theta = theta + dtheta; 30 | end 31 | -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/+gtsam/circlePose3.m: -------------------------------------------------------------------------------- 1 | function values = circlePose3(numPoses, radius, symbolChar) 2 | % circlePose3 generates a set of poses in a circle. This function 3 | % returns those poses inside a gtsam.Values object, with sequential 4 | % keys starting from 0. An optional character may be provided, which 5 | % will be stored in the msb of each key (i.e. gtsam.Symbol). 6 | % 7 | % We use aerospace/navlab convention, X forward, Y right, Z down 8 | % First pose will be at (R,0,0) 9 | % ^y ^ X 10 | % | | 11 | % z-->xZ--> Y (z pointing towards viewer, Z pointing away from viewer) 12 | % Vehicle at p0 is looking towards y axis (X-axis points towards world y) 13 | 14 | if nargin<3,symbolChar=0;end 15 | if nargin<2,radius=1.0;end 16 | if nargin<1,numPoses=8;end 17 | 18 | % Force symbolChar to be a single character 19 | symbolChar = char(symbolChar); 20 | symbolChar = symbolChar(1); 21 | 22 | values = gtsam.Values; 23 | theta = 0.0; 24 | dtheta = 2*pi()/numPoses; 25 | gRo = gtsam.Rot3([0, 1, 0 ; 1, 0, 0 ; 0, 0, -1]); 26 | for i = 1:numPoses 27 | key = gtsam.symbol(symbolChar, i-1); 28 | gti = gtsam.Point3(radius*cos(theta), radius*sin(theta), 0); 29 | oRi = gtsam.Rot3.Yaw(-theta); % negative yaw goes counterclockwise, with Z down ! 30 | gTi = gtsam.Pose3(gRo.compose(oRi), gti); 31 | values.insert(key, gTi); 32 | theta = theta + dtheta; 33 | end 34 | -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/+gtsam/covarianceEllipse.m: -------------------------------------------------------------------------------- 1 | function covarianceEllipse(x,P,color) 2 | % covarianceEllipse plots a Gaussian as an uncertainty ellipse 3 | % Based on Maybeck Vol 1, page 366 4 | % k=2.296 corresponds to 1 std, 68.26% of all probability 5 | % k=11.82 corresponds to 3 std, 99.74% of all probability 6 | % 7 | % covarianceEllipse(x,P,color) 8 | % it is assumed x and y are the first two components of state x 9 | 10 | [e,s] = eig(P(1:2,1:2)); 11 | s1 = s(1,1); 12 | s2 = s(2,2); 13 | k = 2.296; 14 | [ex,ey] = ellipse( sqrt(s1*k)*e(:,1), sqrt(s2*k)*e(:,2), x(1:2) ); 15 | line(ex,ey,'color',color); 16 | 17 | function [x,y] = ellipse(a,b,c); 18 | % ellipse: return the x and y coordinates for an ellipse 19 | % [x,y] = ellipse(a,b,c); 20 | % a, and b are the axes. c is the center 21 | 22 | global ellipse_x ellipse_y 23 | if ~exist('elipse_x') 24 | q =0:2*pi/25:2*pi; 25 | ellipse_x = cos(q); 26 | ellipse_y = sin(q); 27 | end 28 | 29 | points = a*ellipse_x + b*ellipse_y; 30 | x = c(1) + points(1,:); 31 | y = c(2) + points(2,:); 32 | end 33 | 34 | end -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/+gtsam/covarianceEllipse3D.m: -------------------------------------------------------------------------------- 1 | function covarianceEllipse3D(c,P) 2 | % covarianceEllipse3D plots a Gaussian as an uncertainty ellipse 3 | % Based on Maybeck Vol 1, page 366 4 | % k=2.296 corresponds to 1 std, 68.26% of all probability 5 | % k=11.82 corresponds to 3 std, 99.74% of all probability 6 | % 7 | % Modified from http://www.mathworks.com/matlabcentral/newsreader/view_thread/42966 8 | 9 | [e,s] = svd(P); 10 | k = 11.82; 11 | radii = k*sqrt(diag(s)); 12 | 13 | % generate data for "unrotated" ellipsoid 14 | [xc,yc,zc] = ellipsoid(0,0,0,radii(1),radii(2),radii(3),8); 15 | 16 | % rotate data with orientation matrix U and center M 17 | data = kron(e(:,1),xc) + kron(e(:,2),yc) + kron(e(:,3),zc); 18 | n = size(data,2); 19 | x = data(1:n,:)+c(1); 20 | y = data(n+1:2*n,:)+c(2); 21 | z = data(2*n+1:end,:)+c(3); 22 | 23 | % now plot the rotated ellipse 24 | sc = surf(x,y,z,abs(xc)); 25 | shading interp 26 | alpha(0.5) 27 | axis equal -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/+gtsam/determinant.m: -------------------------------------------------------------------------------- 1 | function varargout = determinant(varargin) 2 | if length(varargin) == 1 && isa(varargin{1},'gtsam.GaussianBayesNet') 3 | varargout{1} = gtsam_wrapper(2125, varargin{:}); 4 | elseif length(varargin) == 1 && isa(varargin{1},'gtsam.GaussianBayesTree') 5 | varargout{1} = gtsam_wrapper(2126, varargin{:}); 6 | else 7 | error('Arguments do not match any overload of function gtsam.determinant'); 8 | end 9 | -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/+gtsam/findExampleDataFile.m: -------------------------------------------------------------------------------- 1 | function datafile = findExampleDataFile(datasetName) 2 | %FINDEXAMPLEDATAFILE Find a dataset in the examples folder 3 | 4 | [ myPath, ~, ~ ] = fileparts(mfilename('fullpath')); 5 | searchPath = { ... 6 | fullfile(myPath, [ '../../examples/Data/' datasetName ]) ... 7 | fullfile(myPath, [ '../gtsam_examples/Data/' datasetName ]) }; 8 | datafile = []; 9 | for path = searchPath 10 | if exist(path{:}, 'file') 11 | datafile = path{:}; 12 | end 13 | end 14 | if isempty(datafile) 15 | error([ 'Could not find example data file ' datasetName ]); 16 | end 17 | 18 | end 19 | 20 | -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/+gtsam/gradient.m: -------------------------------------------------------------------------------- 1 | function varargout = gradient(varargin) 2 | if length(varargin) == 2 && isa(varargin{1},'gtsam.GaussianBayesTree') && isa(varargin{2},'gtsam.VectorValues') 3 | varargout{1} = gtsam_wrapper(2131, varargin{:}); 4 | else 5 | error('Arguments do not match any overload of function gtsam.gradient'); 6 | end 7 | -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/+gtsam/hasKeyIntersection.m: -------------------------------------------------------------------------------- 1 | function varargout = hasKeyIntersection(varargin) 2 | if length(varargin) == 2 && isa(varargin{1},'gtsam.KeySet') && isa(varargin{2},'gtsam.KeySet') 3 | varargout{1} = gtsam_wrapper(2132, varargin{:}); 4 | else 5 | error('Arguments do not match any overload of function gtsam.hasKeyIntersection'); 6 | end 7 | -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/+gtsam/keyDifference.m: -------------------------------------------------------------------------------- 1 | function varargout = keyDifference(varargin) 2 | if length(varargin) == 2 && isa(varargin{1},'gtsam.KeySet') && isa(varargin{2},'gtsam.KeySet') 3 | varargout{1} = gtsam_wrapper(2136, varargin{:}); 4 | else 5 | error('Arguments do not match any overload of function gtsam.keyDifference'); 6 | end 7 | -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/+gtsam/keyIntersection.m: -------------------------------------------------------------------------------- 1 | function varargout = keyIntersection(varargin) 2 | if length(varargin) == 2 && isa(varargin{1},'gtsam.KeySet') && isa(varargin{2},'gtsam.KeySet') 3 | varargout{1} = gtsam_wrapper(2137, varargin{:}); 4 | else 5 | error('Arguments do not match any overload of function gtsam.keyIntersection'); 6 | end 7 | -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/+gtsam/linear_independent.m: -------------------------------------------------------------------------------- 1 | function varargout = linear_independent(varargin) 2 | if length(varargin) == 3 && isa(varargin{1},'double') && isa(varargin{2},'double') && isa(varargin{3},'double') 3 | varargout{1} = gtsam_wrapper(2138, varargin{:}); 4 | else 5 | error('Arguments do not match any overload of function gtsam.linear_independent'); 6 | end 7 | -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/+gtsam/load2D.m: -------------------------------------------------------------------------------- 1 | function varargout = load2D(varargin) 2 | if length(varargin) == 5 && isa(varargin{1},'char') && isa(varargin{2},'gtsam.noiseModel.Diagonal') && isa(varargin{3},'numeric') && isa(varargin{4},'logical') && isa(varargin{5},'logical') 3 | [ varargout{1} varargout{2} ] = gtsam_wrapper(2139, varargin{:}); 4 | elseif length(varargin) == 4 && isa(varargin{1},'char') && isa(varargin{2},'gtsam.noiseModel.Diagonal') && isa(varargin{3},'numeric') && isa(varargin{4},'logical') 5 | [ varargout{1} varargout{2} ] = gtsam_wrapper(2140, varargin{:}); 6 | elseif length(varargin) == 3 && isa(varargin{1},'char') && isa(varargin{2},'gtsam.noiseModel.Diagonal') && isa(varargin{3},'numeric') 7 | [ varargout{1} varargout{2} ] = gtsam_wrapper(2141, varargin{:}); 8 | elseif length(varargin) == 2 && isa(varargin{1},'char') && isa(varargin{2},'gtsam.noiseModel.Diagonal') 9 | [ varargout{1} varargout{2} ] = gtsam_wrapper(2142, varargin{:}); 10 | else 11 | error('Arguments do not match any overload of function gtsam.load2D'); 12 | end 13 | -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/+gtsam/load3D.m: -------------------------------------------------------------------------------- 1 | function [graph,initial] = load3D(filename,model,successive,N) 2 | % load3D reads a TORO-style 3D pose graph 3 | % cannot read noise model from file yet, uses specified model 4 | % if [successive] is tru, constructs initial estimate from odometry 5 | 6 | import gtsam.* 7 | 8 | if nargin<3, successive=false; end 9 | fid = fopen(filename); 10 | if fid < 0 11 | error(['load2D: Cannot open file ' filename]); 12 | end 13 | 14 | % scan all lines into a cell array 15 | columns=textscan(fid,'%s','delimiter','\n'); 16 | fclose(fid); 17 | lines=columns{1}; 18 | 19 | % loop over lines and add vertices 20 | graph = NonlinearFactorGraph; 21 | initial = Values; 22 | origin=gtsam.Pose3; 23 | initial.insert(0,origin); 24 | n=size(lines,1); 25 | if nargin<4, N=n;end 26 | 27 | for i=1:n 28 | line_i=lines{i}; 29 | if strcmp('VERTEX3',line_i(1:7)) 30 | v = textscan(line_i,'%s %d %f %f %f %f %f %f',1); 31 | i1=v{2}; 32 | if (~successive && i1i1 49 | initial.insert(i2,initial.at(i1).compose(dpose)); 50 | else 51 | initial.insert(i1,initial.at(i2).compose(dpose.inverse)); 52 | end 53 | end 54 | end 55 | end 56 | end 57 | end 58 | 59 | -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/+gtsam/mrsymbol.m: -------------------------------------------------------------------------------- 1 | function varargout = mrsymbol(varargin) 2 | if length(varargin) == 3 && isa(varargin{1},'char') && isa(varargin{2},'char') && isa(varargin{3},'numeric') 3 | varargout{1} = gtsam_wrapper(2143, varargin{:}); 4 | else 5 | error('Arguments do not match any overload of function gtsam.mrsymbol'); 6 | end 7 | -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/+gtsam/mrsymbolChr.m: -------------------------------------------------------------------------------- 1 | function varargout = mrsymbolChr(varargin) 2 | if length(varargin) == 1 && isa(varargin{1},'numeric') 3 | varargout{1} = gtsam_wrapper(2144, varargin{:}); 4 | else 5 | error('Arguments do not match any overload of function gtsam.mrsymbolChr'); 6 | end 7 | -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/+gtsam/mrsymbolIndex.m: -------------------------------------------------------------------------------- 1 | function varargout = mrsymbolIndex(varargin) 2 | if length(varargin) == 1 && isa(varargin{1},'numeric') 3 | varargout{1} = gtsam_wrapper(2145, varargin{:}); 4 | else 5 | error('Arguments do not match any overload of function gtsam.mrsymbolIndex'); 6 | end 7 | -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/+gtsam/mrsymbolLabel.m: -------------------------------------------------------------------------------- 1 | function varargout = mrsymbolLabel(varargin) 2 | if length(varargin) == 1 && isa(varargin{1},'numeric') 3 | varargout{1} = gtsam_wrapper(2146, varargin{:}); 4 | else 5 | error('Arguments do not match any overload of function gtsam.mrsymbolLabel'); 6 | end 7 | -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/+gtsam/optimize.m: -------------------------------------------------------------------------------- 1 | function varargout = optimize(varargin) 2 | if length(varargin) == 1 && isa(varargin{1},'gtsam.GaussianBayesTree') 3 | varargout{1} = gtsam_wrapper(2147, varargin{:}); 4 | else 5 | error('Arguments do not match any overload of function gtsam.optimize'); 6 | end 7 | -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/+gtsam/optimizeGradientSearch.m: -------------------------------------------------------------------------------- 1 | function varargout = optimizeGradientSearch(varargin) 2 | if length(varargin) == 1 && isa(varargin{1},'gtsam.GaussianBayesTree') 3 | varargout{1} = gtsam_wrapper(2148, varargin{:}); 4 | else 5 | error('Arguments do not match any overload of function gtsam.optimizeGradientSearch'); 6 | end 7 | -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/+gtsam/plot2DPoints.m: -------------------------------------------------------------------------------- 1 | function plot2DPoints(values, linespec, marginals) 2 | %PLOT2DPOINTS Plots the Point2's in a values, with optional covariances 3 | % Finds all the Point2 objects in the given Values object and plots them. 4 | % If a Marginals object is given, this function will also plot marginal 5 | % covariance ellipses for each point. 6 | 7 | import gtsam.* 8 | 9 | if ~exist('linespec', 'var') || isempty(linespec) 10 | linespec = 'g'; 11 | end 12 | haveMarginals = exist('marginals', 'var'); 13 | keys = KeyVector(values.keys); 14 | 15 | holdstate = ishold; 16 | hold on 17 | 18 | % Plot points and covariance matrices 19 | for i = 0:keys.size-1 20 | key = keys.at(i); 21 | p = values.at(key); 22 | if isa(p, 'gtsam.Point2') 23 | if haveMarginals 24 | P = marginals.marginalCovariance(key); 25 | gtsam.plotPoint2(p, linespec, P); 26 | else 27 | gtsam.plotPoint2(p, linespec); 28 | end 29 | end 30 | end 31 | 32 | if ~holdstate 33 | hold off 34 | end 35 | 36 | end 37 | 38 | -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/+gtsam/plot2DTrajectory.m: -------------------------------------------------------------------------------- 1 | function plot2DTrajectory(values, linespec, marginals) 2 | %PLOT2DTRAJECTORY Plots the Pose2's in a values, with optional covariances 3 | % Finds all the Pose2 objects in the given Values object and plots them 4 | % in increasing key order, connecting consecutive poses with a line. If 5 | % a Marginals object is given, this function will also plot marginal 6 | % covariance ellipses for each pose. 7 | 8 | import gtsam.* 9 | 10 | if ~exist('linespec', 'var') || isempty(linespec) 11 | linespec = 'k*-'; 12 | end 13 | 14 | haveMarginals = exist('marginals', 'var'); 15 | keys = KeyVector(values.keys); 16 | 17 | holdstate = ishold; 18 | hold on 19 | 20 | % Plot poses and covariance matrices 21 | lastIndex = []; 22 | for i = 0:keys.size-1 23 | key = keys.at(i); 24 | x = values.at(key); 25 | if isa(x, 'gtsam.Pose2') 26 | if ~isempty(lastIndex) 27 | % Draw line from last pose then covariance ellipse on top of 28 | % last pose. 29 | lastKey = keys.at(lastIndex); 30 | lastPose = values.at(lastKey); 31 | plot([ x.x; lastPose.x ], [ x.y; lastPose.y ], linespec); 32 | if haveMarginals 33 | P = marginals.marginalCovariance(lastKey); 34 | gtsam.plotPose2(lastPose, 'g', P); 35 | else 36 | gtsam.plotPose2(lastPose, 'g', []); 37 | end 38 | 39 | end 40 | lastIndex = i; 41 | end 42 | end 43 | 44 | % Draw final covariance ellipse 45 | if ~isempty(lastIndex) 46 | lastKey = keys.at(lastIndex); 47 | lastPose = values.at(lastKey); 48 | if haveMarginals 49 | P = marginals.marginalCovariance(lastKey); 50 | gtsam.plotPose2(lastPose, 'g', P); 51 | else 52 | gtsam.plotPose2(lastPose, 'g', []); 53 | end 54 | end 55 | 56 | if ~holdstate 57 | hold off 58 | end 59 | 60 | end 61 | 62 | -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/+gtsam/plot3DPoints.m: -------------------------------------------------------------------------------- 1 | function plot3DPoints(values, linespec, marginals) 2 | %PLOT3DPOINTS Plots the Point3's in a values, with optional covariances 3 | % Finds all the Point3 objects in the given Values object and plots them. 4 | % If a Marginals object is given, this function will also plot marginal 5 | % covariance ellipses for each point. 6 | 7 | import gtsam.* 8 | 9 | if ~exist('linespec', 'var') || isempty(linespec) 10 | linespec = 'g'; 11 | end 12 | haveMarginals = exist('marginals', 'var'); 13 | keys = KeyVector(values.keys); 14 | 15 | holdstate = ishold; 16 | hold on 17 | 18 | % Plot points and covariance matrices 19 | for i = 0:keys.size-1 20 | key = keys.at(i); 21 | p = values.at(key); 22 | if isa(p, 'gtsam.Point3') 23 | if haveMarginals 24 | P = marginals.marginalCovariance(key); 25 | gtsam.plotPoint3(p, linespec, P); 26 | else 27 | gtsam.plotPoint3(p, linespec); 28 | end 29 | end 30 | end 31 | 32 | if ~holdstate 33 | hold off 34 | end 35 | 36 | end 37 | -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/+gtsam/plot3DTrajectory.m: -------------------------------------------------------------------------------- 1 | function plot3DTrajectory(values,linespec,frames,scale,marginals) 2 | % plot3DTrajectory plots a 3D trajectory 3 | % plot3DTrajectory(values,linespec,frames,scale,marginals) 4 | 5 | if ~exist('scale','var') || isempty(scale), scale=1; end 6 | if ~exist('frames','var'), scale=[]; end 7 | 8 | import gtsam.* 9 | 10 | haveMarginals = exist('marginals', 'var'); 11 | keys = KeyVector(values.keys); 12 | 13 | holdstate = ishold; 14 | hold on 15 | 16 | % Plot poses and covariance matrices 17 | lastIndex = []; 18 | for i = 0:keys.size-1 19 | key = keys.at(i); 20 | x = values.at(key); 21 | if isa(x, 'gtsam.Pose3') 22 | if ~isempty(lastIndex) 23 | % Draw line from last pose then covariance ellipse on top of 24 | % last pose. 25 | lastKey = keys.at(lastIndex); 26 | lastPose = values.at(lastKey); 27 | plot3([ x.x; lastPose.x ], [ x.y; lastPose.y ], [ x.z; lastPose.z ], linespec); 28 | if haveMarginals 29 | P = marginals.marginalCovariance(lastKey); 30 | else 31 | P = []; 32 | end 33 | gtsam.plotPose3(lastPose, P, scale); 34 | end 35 | lastIndex = i; 36 | end 37 | end 38 | 39 | % Draw final pose 40 | if ~isempty(lastIndex) 41 | lastKey = keys.at(lastIndex); 42 | lastPose = values.at(lastKey); 43 | if haveMarginals 44 | P = marginals.marginalCovariance(lastKey); 45 | else 46 | P = []; 47 | end 48 | gtsam.plotPose3(lastPose, P, scale); 49 | end 50 | 51 | if ~holdstate 52 | hold off 53 | end 54 | 55 | end -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/+gtsam/plotBayesNet.m: -------------------------------------------------------------------------------- 1 | function plotBayesNet(bayesNet) 2 | % plotBayesNet saves as dot file, renders, and shows as image 3 | % Needs dot installed 4 | 5 | bayesNet.saveGraph('/tmp/bayesNet.dot') 6 | !dot -Tpng -o /tmp/dotImage.png /tmp/bayesNet.dot 7 | dotImage=imread('/tmp/dotImage.png'); 8 | imshow(dotImage) -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/+gtsam/plotBayesTree.m: -------------------------------------------------------------------------------- 1 | function plotBayesTree(bayesTree) 2 | % plotBayesTree saves as dot file, renders, and shows as image 3 | % Needs dot installed 4 | 5 | bayesTree.saveGraph('/tmp/bayesTree.dot') 6 | !dot -Tpng -o /tmp/dotImage.png /tmp/bayesTree.dot 7 | dotImage=imread('/tmp/dotImage.png'); 8 | imshow(dotImage) -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/+gtsam/plotCamera.m: -------------------------------------------------------------------------------- 1 | function plotCamera(pose, axisLength) 2 | C = pose.translation().vector(); 3 | R = pose.rotation().matrix(); 4 | 5 | xAxis = C+R(:,1)*axisLength; 6 | L = [C xAxis]'; 7 | line(L(:,1),L(:,2),L(:,3),'Color','r'); 8 | 9 | yAxis = C+R(:,2)*axisLength; 10 | L = [C yAxis]'; 11 | line(L(:,1),L(:,2),L(:,3),'Color','g'); 12 | 13 | zAxis = C+R(:,3)*axisLength; 14 | L = [C zAxis]'; 15 | line(L(:,1),L(:,2),L(:,3),'Color','b'); 16 | 17 | axis equal 18 | end -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/+gtsam/plotPoint2.m: -------------------------------------------------------------------------------- 1 | function plotPoint2(p,color,P) 2 | % plotPoint2 shows a Point2, possibly with covariance matrix 3 | if size(color,2)==1 4 | plot(p.x,p.y,[color '*']); 5 | else 6 | plot(p.x,p.y,color); 7 | end 8 | if exist('P', 'var') && (~isempty(P)) 9 | gtsam.covarianceEllipse([p.x;p.y],P,color(1)); 10 | end -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/+gtsam/plotPoint3.m: -------------------------------------------------------------------------------- 1 | function plotPoint3(p, color, P) 2 | %PLOTPOINT3 Plot a Point3 with an optional covariance matrix 3 | if size(color,2)==1 4 | plot3(p.x,p.y,p.z,[color '*']); 5 | else 6 | plot3(p.x,p.y,p.z,color); 7 | end 8 | if exist('P', 'var') 9 | gtsam.covarianceEllipse3D([p.x;p.y;p.z],P); 10 | end 11 | 12 | end 13 | 14 | -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/+gtsam/plotPose2.m: -------------------------------------------------------------------------------- 1 | function plotPose2(pose,color,P,axisLength) 2 | % plotPose2 shows a Pose2, possibly with covariance matrix 3 | if nargin<4,axisLength=0.1;end 4 | 5 | plot(pose.x,pose.y,[color '*']); 6 | c = cos(pose.theta); 7 | s = sin(pose.theta); 8 | quiver(pose.x,pose.y,c,s,axisLength,color); 9 | if nargin>2 && (~isempty(P)) 10 | pPp = P(1:2,1:2); % covariance matrix in pose coordinate frame 11 | gRp = [c -s;s c]; % rotation from pose to global 12 | gtsam.covarianceEllipse([pose.x;pose.y],gRp*pPp*gRp',color); 13 | end -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/+gtsam/plotPose3.m: -------------------------------------------------------------------------------- 1 | function plotPose3(pose, P, axisLength) 2 | % plotPose3 shows a Pose, possibly with covariance matrix 3 | if nargin<3,axisLength=0.1;end 4 | 5 | % get rotation and translation (center) 6 | gRp = pose.rotation().matrix(); % rotation from pose to global 7 | C = pose.translation().vector(); 8 | 9 | if ~isempty(axisLength) 10 | % draw the camera axes 11 | xAxis = C+gRp(:,1)*axisLength; 12 | L = [C xAxis]'; 13 | line(L(:,1),L(:,2),L(:,3),'Color','r'); 14 | 15 | yAxis = C+gRp(:,2)*axisLength; 16 | L = [C yAxis]'; 17 | line(L(:,1),L(:,2),L(:,3),'Color','g'); 18 | 19 | zAxis = C+gRp(:,3)*axisLength; 20 | L = [C zAxis]'; 21 | line(L(:,1),L(:,2),L(:,3),'Color','b'); 22 | end 23 | 24 | % plot the covariance 25 | if (nargin>2) && (~isempty(P)) 26 | pPp = P(4:6,4:6); % covariance matrix in pose coordinate frame 27 | gPp = gRp*pPp*gRp'; % convert the covariance matrix to global coordinate frame 28 | gtsam.covarianceEllipse3D(C,gPp); 29 | end 30 | 31 | end 32 | -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/+gtsam/printKeySet.m: -------------------------------------------------------------------------------- 1 | function varargout = printKeySet(varargin) 2 | if length(varargin) == 1 && isa(varargin{1},'gtsam.KeySet') 3 | gtsam_wrapper(2151, varargin{:}); 4 | elseif length(varargin) == 2 && isa(varargin{1},'gtsam.KeySet') && isa(varargin{2},'char') 5 | gtsam_wrapper(2152, varargin{:}); 6 | else 7 | error('Arguments do not match any overload of function gtsam.printKeySet'); 8 | end 9 | -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/+gtsam/summarize.m: -------------------------------------------------------------------------------- 1 | function varargout = summarize(varargin) 2 | if length(varargin) == 3 && isa(varargin{1},'gtsam.NonlinearFactorGraph') && isa(varargin{2},'gtsam.Values') && isa(varargin{3},'gtsam.KeySet') 3 | [ varargout{1} varargout{2} ] = gtsam_wrapper(2154, varargin{:}); 4 | else 5 | error('Arguments do not match any overload of function gtsam.summarize'); 6 | end 7 | -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/+gtsam/summarizeAsNonlinearContainer.m: -------------------------------------------------------------------------------- 1 | function varargout = summarizeAsNonlinearContainer(varargin) 2 | if length(varargin) == 3 && isa(varargin{1},'gtsam.NonlinearFactorGraph') && isa(varargin{2},'gtsam.Values') && isa(varargin{3},'gtsam.KeySet') 3 | varargout{1} = gtsam_wrapper(2155, varargin{:}); 4 | else 5 | error('Arguments do not match any overload of function gtsam.summarizeAsNonlinearContainer'); 6 | end 7 | -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/+gtsam/symbol.m: -------------------------------------------------------------------------------- 1 | function varargout = symbol(varargin) 2 | if length(varargin) == 2 && isa(varargin{1},'char') && isa(varargin{2},'numeric') 3 | varargout{1} = gtsam_wrapper(2156, varargin{:}); 4 | else 5 | error('Arguments do not match any overload of function gtsam.symbol'); 6 | end 7 | -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/+gtsam/symbolChr.m: -------------------------------------------------------------------------------- 1 | function varargout = symbolChr(varargin) 2 | if length(varargin) == 1 && isa(varargin{1},'numeric') 3 | varargout{1} = gtsam_wrapper(2157, varargin{:}); 4 | else 5 | error('Arguments do not match any overload of function gtsam.symbolChr'); 6 | end 7 | -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/+gtsam/symbolIndex.m: -------------------------------------------------------------------------------- 1 | function varargout = symbolIndex(varargin) 2 | if length(varargin) == 1 && isa(varargin{1},'numeric') 3 | varargout{1} = gtsam_wrapper(2158, varargin{:}); 4 | else 5 | error('Arguments do not match any overload of function gtsam.symbolIndex'); 6 | end 7 | -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/gtsam_examples/Data/VO_calibration.txt: -------------------------------------------------------------------------------- 1 | 721.5377 721.5377 0.0 609.5593 172.854 0.537150588 -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/gtsam_examples/PlanarSLAMExample_graph.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % GTSAM Copyright 2010, Georgia Tech Research Corporation, 3 | % Atlanta, Georgia 30332-0415 4 | % All Rights Reserved 5 | % Authors: Frank Dellaert, et al. (see THANKS for the full author list) 6 | % 7 | % See LICENSE for the license information 8 | % 9 | % @brief Read graph from file and perform GraphSLAM 10 | % @author Frank Dellaert 11 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 12 | 13 | clear 14 | 15 | import gtsam.* 16 | 17 | %% Find data file 18 | datafile = findExampleDataFile('example.graph'); 19 | 20 | %% Initialize graph, initial estimate, and odometry noise 21 | model = noiseModel.Diagonal.Sigmas([0.05; 0.05; 2*pi/180]); 22 | [graph,initial] = load2D(datafile, model); 23 | 24 | %% Add a Gaussian prior on a pose in the middle 25 | priorMean = initial.at(40); 26 | priorNoise = noiseModel.Diagonal.Sigmas([0.1; 0.1; 2*pi/180]); 27 | graph.add(PriorFactorPose2(40, priorMean, priorNoise)); % add directly to graph 28 | 29 | %% Plot Initial Estimate 30 | cla 31 | plot2DTrajectory(initial, 'r-'); axis equal 32 | 33 | %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd 34 | optimizer = LevenbergMarquardtOptimizer(graph, initial); 35 | tic 36 | result = optimizer.optimizeSafely; 37 | toc 38 | 39 | %% Plot Covariance Ellipses 40 | cla;hold on 41 | marginals = Marginals(graph, result); 42 | plot2DTrajectory(result, 'g', marginals); 43 | plot2DPoints(result, 'b', marginals); 44 | axis tight 45 | axis equal 46 | view(2) 47 | 48 | -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/gtsam_examples/Pose2SLAMExample_circle.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % GTSAM Copyright 2010, Georgia Tech Research Corporation, 3 | % Atlanta, Georgia 30332-0415 4 | % All Rights Reserved 5 | % Authors: Frank Dellaert, et al. (see THANKS for the full author list) 6 | % 7 | % See LICENSE for the license information 8 | % 9 | % @brief Read graph from file and perform GraphSLAM 10 | % @author Frank Dellaert 11 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 12 | 13 | import gtsam.* 14 | 15 | %% Create a hexagon of poses 16 | hexagon = circlePose2(6,1.0); 17 | p0 = hexagon.at(0); 18 | p1 = hexagon.at(1); 19 | 20 | %% create a Pose graph with one equality constraint and one measurement 21 | fg = NonlinearFactorGraph; 22 | fg.add(NonlinearEqualityPose2(0, p0)); 23 | delta = p0.between(p1); 24 | covariance = noiseModel.Diagonal.Sigmas([0.05; 0.05; 5*pi/180]); 25 | fg.add(BetweenFactorPose2(0,1, delta, covariance)); 26 | fg.add(BetweenFactorPose2(1,2, delta, covariance)); 27 | fg.add(BetweenFactorPose2(2,3, delta, covariance)); 28 | fg.add(BetweenFactorPose2(3,4, delta, covariance)); 29 | fg.add(BetweenFactorPose2(4,5, delta, covariance)); 30 | fg.add(BetweenFactorPose2(5,0, delta, covariance)); 31 | 32 | %% Create initial config 33 | initial = Values; 34 | initial.insert(0, p0); 35 | initial.insert(1, hexagon.at(1).retract([-0.1, 0.1,-0.1]')); 36 | initial.insert(2, hexagon.at(2).retract([ 0.1,-0.1, 0.1]')); 37 | initial.insert(3, hexagon.at(3).retract([-0.1, 0.1,-0.1]')); 38 | initial.insert(4, hexagon.at(4).retract([ 0.1,-0.1, 0.1]')); 39 | initial.insert(5, hexagon.at(5).retract([-0.1, 0.1,-0.1]')); 40 | 41 | %% Plot Initial Estimate 42 | cla 43 | plot2DTrajectory(initial, 'g*-'); axis equal 44 | 45 | %% optimize 46 | optimizer = DoglegOptimizer(fg, initial); 47 | result = optimizer.optimizeSafely; 48 | 49 | %% Show Result 50 | hold on; plot2DTrajectory(result, 'b*-'); 51 | view(2); 52 | axis([-1.5 1.5 -1.5 1.5]); 53 | result.print(sprintf('\nFinal result:\n')); 54 | -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/gtsam_examples/Pose2SLAMExample_graph.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % GTSAM Copyright 2010, Georgia Tech Research Corporation, 3 | % Atlanta, Georgia 30332-0415 4 | % All Rights Reserved 5 | % Authors: Frank Dellaert, et al. (see THANKS for the full author list) 6 | % 7 | % See LICENSE for the license information 8 | % 9 | % @brief Read graph from file and perform GraphSLAM 10 | % @author Frank Dellaert 11 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 12 | 13 | import gtsam.* 14 | 15 | %% Find data file 16 | datafile = findExampleDataFile('w100-odom.graph'); 17 | 18 | %% Initialize graph, initial estimate, and odometry noise 19 | model = noiseModel.Diagonal.Sigmas([0.05; 0.05; 5*pi/180]); 20 | [graph,initial] = load2D(datafile, model); 21 | 22 | %% Add a Gaussian prior on pose x_1 23 | priorMean = Pose2(0, 0, 0); % prior mean is at origin 24 | priorNoise = noiseModel.Diagonal.Sigmas([0.01; 0.01; 0.01]); 25 | graph.add(PriorFactorPose2(0, priorMean, priorNoise)); % add directly to graph 26 | 27 | %% Plot Initial Estimate 28 | cla 29 | plot2DTrajectory(initial, 'g-*'); axis equal 30 | 31 | %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd 32 | optimizer = LevenbergMarquardtOptimizer(graph, initial); 33 | tic 34 | result = optimizer.optimizeSafely; 35 | toc 36 | hold on; plot2DTrajectory(result, 'b-*'); 37 | 38 | %% Plot Covariance Ellipses 39 | marginals = Marginals(graph, result); 40 | P={}; 41 | for i=1:result.size()-1 42 | pose_i = result.at(i); 43 | P{i}=marginals.marginalCovariance(i); 44 | plotPose2(pose_i,'b',P{i}) 45 | end 46 | view(2) 47 | axis tight; axis equal; 48 | % fprintf(1,'%.5f %.5f %.5f\n',P{99}) -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/gtsam_examples/Pose3SLAMExample.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % GTSAM Copyright 2010, Georgia Tech Research Corporation, 3 | % Atlanta, Georgia 30332-0415 4 | % All Rights Reserved 5 | % Authors: Frank Dellaert, et al. (see THANKS for the full author list) 6 | % 7 | % See LICENSE for the license information 8 | % 9 | % @brief Read graph from file and perform GraphSLAM 10 | % @author Frank Dellaert 11 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 12 | 13 | import gtsam.* 14 | 15 | %% Create a hexagon of poses 16 | hexagon = circlePose3(6,1.0); 17 | p0 = hexagon.at(0); 18 | p1 = hexagon.at(1); 19 | 20 | %% create a Pose graph with one equality constraint and one measurement 21 | fg = NonlinearFactorGraph; 22 | fg.add(NonlinearEqualityPose3(0, p0)); 23 | delta = p0.between(p1); 24 | covariance = noiseModel.Diagonal.Sigmas([5*pi/180; 5*pi/180; 5*pi/180; 0.05; 0.05; 0.05]); 25 | fg.add(BetweenFactorPose3(0,1, delta, covariance)); 26 | fg.add(BetweenFactorPose3(1,2, delta, covariance)); 27 | fg.add(BetweenFactorPose3(2,3, delta, covariance)); 28 | fg.add(BetweenFactorPose3(3,4, delta, covariance)); 29 | fg.add(BetweenFactorPose3(4,5, delta, covariance)); 30 | fg.add(BetweenFactorPose3(5,0, delta, covariance)); 31 | 32 | %% Create initial config 33 | initial = Values; 34 | s = 0.10; 35 | initial.insert(0, p0); 36 | initial.insert(1, hexagon.at(1).retract(s*randn(6,1))); 37 | initial.insert(2, hexagon.at(2).retract(s*randn(6,1))); 38 | initial.insert(3, hexagon.at(3).retract(s*randn(6,1))); 39 | initial.insert(4, hexagon.at(4).retract(s*randn(6,1))); 40 | initial.insert(5, hexagon.at(5).retract(s*randn(6,1))); 41 | 42 | %% Plot Initial Estimate 43 | cla 44 | plot3DTrajectory(initial, 'g-*'); 45 | 46 | %% optimize 47 | optimizer = DoglegOptimizer(fg, initial); 48 | result = optimizer.optimizeSafely(); 49 | 50 | %% Show Result 51 | hold on; plot3DTrajectory(result, 'b-*', true, 0.3); 52 | axis([-2 2 -2 2 -1 1]); 53 | axis equal 54 | view(-37,40) 55 | colormap hot 56 | result.print(sprintf('\nFinal result:\n')); 57 | -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/gtsam_examples/Pose3SLAMExample_graph.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % GTSAM Copyright 2010, Georgia Tech Research Corporation, 3 | % Atlanta, Georgia 30332-0415 4 | % All Rights Reserved 5 | % Authors: Frank Dellaert, et al. (see THANKS for the full author list) 6 | % 7 | % See LICENSE for the license information 8 | % 9 | % @brief Read graph from file and perform GraphSLAM 10 | % @author Frank Dellaert 11 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 12 | 13 | import gtsam.* 14 | 15 | %% Find data file 16 | N = 2500; 17 | % dataset = 'sphere_smallnoise.graph'; 18 | % dataset = 'sphere2500_groundtruth.txt'; 19 | dataset = 'sphere2500.txt'; 20 | 21 | datafile = findExampleDataFile(dataset); 22 | 23 | %% Initialize graph, initial estimate, and odometry noise 24 | model = noiseModel.Diagonal.Sigmas([5*pi/180; 5*pi/180; 5*pi/180; 0.05; 0.05; 0.05]); 25 | [graph,initial]=load3D(datafile,model,true,N); 26 | 27 | %% Plot Initial Estimate 28 | cla 29 | first = initial.at(0); 30 | plot3(first.x(),first.y(),first.z(),'r*'); hold on 31 | plot3DTrajectory(initial,'g-',false); 32 | drawnow; 33 | 34 | %% Read again, now with all constraints, and optimize 35 | graph = load3D(datafile, model, false, N); 36 | graph.add(NonlinearEqualityPose3(0, first)); 37 | optimizer = LevenbergMarquardtOptimizer(graph, initial); 38 | result = optimizer.optimizeSafely(); 39 | plot3DTrajectory(result, 'r-', false); axis equal; 40 | 41 | view(3); axis equal; -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/gtsam_examples/VisualISAMDemo.m: -------------------------------------------------------------------------------- 1 | % VisualISAMDemo: runs VisualSLAM iSAM demo in a GUI 2 | % Authors: Duy Nguyen Ta 3 | 4 | % Make sure global variables are visible on command prompt 5 | % so you can examine how they change as you step through 6 | global options truth data noiseModels isam result frame_i 7 | 8 | % Start GUI 9 | VisualISAM_gui -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/gtsam_examples/VisualISAMExample.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % GTSAM Copyright 2010, Georgia Tech Research Corporation, 3 | % Atlanta, Georgia 30332-0415 4 | % All Rights Reserved 5 | % Authors: Frank Dellaert, et al. (see THANKS for the full author list) 6 | % 7 | % See LICENSE for the license information 8 | % 9 | % @brief A simple visual SLAM example for structure from motion 10 | % @author Duy-Nguyen Ta 11 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 12 | 13 | import gtsam.* 14 | 15 | % Data Options 16 | options.triangle = false; 17 | options.nrCameras = 20; 18 | options.showImages = false; 19 | 20 | % iSAM Options 21 | options.hardConstraint = false; 22 | options.pointPriors = false; 23 | options.batchInitialization = true; 24 | options.reorderInterval = 10; 25 | options.alwaysRelinearize = false; 26 | 27 | % Display Options 28 | options.saveDotFile = false; 29 | options.printStats = false; 30 | options.drawInterval = 5; 31 | options.cameraInterval = 1; 32 | options.drawTruePoses = false; 33 | options.saveFigures = false; 34 | options.saveDotFiles = false; 35 | 36 | %% Generate data 37 | [data,truth] = VisualISAMGenerateData(options); 38 | 39 | %% Initialize iSAM with the first pose and points 40 | [noiseModels,isam,result,nextPose] = VisualISAMInitialize(data,truth,options); 41 | cla; 42 | VisualISAMPlot(truth, data, isam, result, options) 43 | 44 | %% Main loop for iSAM: stepping through all poses 45 | for frame_i=3:options.nrCameras 46 | [isam,result,nextPose] = VisualISAMStep(data,noiseModels,isam,result,truth,nextPose); 47 | if mod(frame_i,options.drawInterval)==0 48 | VisualISAMPlot(truth, data, isam, result, options) 49 | end 50 | end 51 | -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/gtsam_examples/VisualISAM_gui.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rising-turtle/VINS-Mono_Ext/1aa44f5ec4c4a1b64726e8fe2d0d28ca3b6ade23/matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/gtsam_examples/VisualISAM_gui.fig -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/gtsam_examples/gtsamExamples.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rising-turtle/VINS-Mono_Ext/1aa44f5ec4c4a1b64726e8fe2d0d28ca3b6ade23/matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/gtsam_examples/gtsamExamples.fig -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/gtsam_tests/testJacobianFactor.m: -------------------------------------------------------------------------------- 1 | %----------------------------------------------------------------------- 2 | % eliminate 3 | 4 | import gtsam.* 5 | 6 | % the combined linear factor 7 | Ax2 = [ 8 | -5., 0. 9 | +0.,-5. 10 | 10., 0. 11 | +0.,10. 12 | ]; 13 | 14 | Al1 = [ 15 | 5., 0. 16 | 0., 5. 17 | 0., 0. 18 | 0., 0. 19 | ]; 20 | 21 | Ax1 = [ 22 | 0.00, 0. % f4 23 | 0.00, 0. % f4 24 | -10., 0. % f2 25 | 0.00,-10. % f2 26 | ]; 27 | 28 | x2 = 1; 29 | l1 = 2; 30 | x1 = 3; 31 | 32 | % the RHS 33 | b2=[-1;1.5;2;-1]; 34 | sigmas = [1;1;1;1]; 35 | model4 = noiseModel.Diagonal.Sigmas(sigmas); 36 | combined = JacobianFactor(x2, Ax2, l1, Al1, x1, Ax1, b2, model4); 37 | 38 | % eliminate the first variable (x2) in the combined factor, destructive ! 39 | actualCG = combined.eliminateFirst(); 40 | 41 | % create expected Conditional Gaussian 42 | R11 = [ 43 | 11.1803, 0.00 44 | 0.00, 11.1803 45 | ]; 46 | S12 = [ 47 | -2.23607, 0.00 48 | +0.00,-2.23607 49 | ]; 50 | S13 = [ 51 | -8.94427, 0.00 52 | +0.00,-8.94427 53 | ]; 54 | d=[2.23607;-1.56525]; 55 | expectedCG = GaussianConditional(x2,d,R11,l1,S12,x1,S13,[1;1]); 56 | % check if the result matches 57 | CHECK('actualCG.equals(expectedCG,1e-5)',actualCG.equals(expectedCG,1e-4)); 58 | 59 | % the expected linear factor 60 | Bl1 = [ 61 | 4.47214, 0.00 62 | 0.00, 4.47214 63 | ]; 64 | 65 | Bx1 = [ 66 | % x1 67 | -4.47214, 0.00 68 | +0.00, -4.47214 69 | ]; 70 | 71 | % the RHS 72 | b1= [0.0;0.894427]; 73 | 74 | model2 = noiseModel.Diagonal.Sigmas([1;1]); 75 | expectedLF = JacobianFactor(l1, Bl1, x1, Bx1, b1, model2); 76 | 77 | % check if the result matches the combined (reduced) factor 78 | % FIXME: JacobianFactor/GaussianFactor mismatch 79 | %CHECK('combined.equals(expectedLF,1e-5)',combined.equals(expectedLF,1e-4)); 80 | -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/gtsam_tests/testLocalizationExample.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % GTSAM Copyright 2010, Georgia Tech Research Corporation, 3 | % Atlanta, Georgia 30332-0415 4 | % All Rights Reserved 5 | % Authors: Frank Dellaert, et al. (see THANKS for the full author list) 6 | % 7 | % See LICENSE for the license information 8 | % 9 | % @brief Example of a simple 2D localization example 10 | % @author Frank Dellaert 11 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 12 | 13 | import gtsam.* 14 | 15 | %% Create the graph (defined in pose2SLAM.h, derived from NonlinearFactorGraph) 16 | graph = NonlinearFactorGraph; 17 | 18 | %% Add two odometry factors 19 | odometry = Pose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case) 20 | odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); % 20cm std on x,y, 0.1 rad on theta 21 | graph.add(BetweenFactorPose2(1, 2, odometry, odometryNoise)); 22 | graph.add(BetweenFactorPose2(2, 3, odometry, odometryNoise)); 23 | 24 | %% Add three "GPS" measurements 25 | % We use Pose2 Priors here with high variance on theta 26 | groundTruth = Values; 27 | groundTruth.insert(1, Pose2(0.0, 0.0, 0.0)); 28 | groundTruth.insert(2, Pose2(2.0, 0.0, 0.0)); 29 | groundTruth.insert(3, Pose2(4.0, 0.0, 0.0)); 30 | model = noiseModel.Diagonal.Sigmas([0.1; 0.1; 10]); 31 | for i=1:3 32 | graph.add(PriorFactorPose2(i, groundTruth.at(i), model)); 33 | end 34 | 35 | %% Initialize to noisy points 36 | initialEstimate = Values; 37 | initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2)); 38 | initialEstimate.insert(2, Pose2(2.3, 0.1,-0.2)); 39 | initialEstimate.insert(3, Pose2(4.1, 0.1, 0.1)); 40 | 41 | %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd 42 | optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate); 43 | result = optimizer.optimizeSafely(); 44 | 45 | %% Plot Covariance Ellipses 46 | marginals = Marginals(graph, result); 47 | P={}; 48 | for i=1:result.size() 49 | pose_i = result.at(i); 50 | CHECK('pose_i.equals(groundTruth.pose(i)',pose_i.equals(groundTruth.at(i),1e-4)); 51 | P{i}=marginals.marginalCovariance(i); 52 | end 53 | -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/gtsam_tests/testOdometryExample.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % GTSAM Copyright 2010, Georgia Tech Research Corporation, 3 | % Atlanta, Georgia 30332-0415 4 | % All Rights Reserved 5 | % Authors: Frank Dellaert, et al. (see THANKS for the full author list) 6 | % 7 | % See LICENSE for the license information 8 | % 9 | % @brief Example of a simple 2D localization example 10 | % @author Frank Dellaert 11 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 12 | 13 | import gtsam.* 14 | 15 | %% Create the graph (defined in pose2SLAM.h, derived from NonlinearFactorGraph) 16 | graph = NonlinearFactorGraph; 17 | 18 | %% Add a Gaussian prior on pose x_1 19 | priorMean = Pose2(0.0, 0.0, 0.0); % prior mean is at origin 20 | priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]); % 30cm std on x,y, 0.1 rad on theta 21 | graph.add(PriorFactorPose2(1, priorMean, priorNoise)); % add directly to graph 22 | 23 | %% Add two odometry factors 24 | odometry = Pose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case) 25 | odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); % 20cm std on x,y, 0.1 rad on theta 26 | graph.add(BetweenFactorPose2(1, 2, odometry, odometryNoise)); 27 | graph.add(BetweenFactorPose2(2, 3, odometry, odometryNoise)); 28 | 29 | %% Initialize to noisy points 30 | initialEstimate = Values; 31 | initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2)); 32 | initialEstimate.insert(2, Pose2(2.3, 0.1,-0.2)); 33 | initialEstimate.insert(3, Pose2(4.1, 0.1, 0.1)); 34 | 35 | %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd 36 | optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate); 37 | result = optimizer.optimizeSafely(); 38 | marginals = Marginals(graph, result); 39 | marginals.marginalCovariance(1); 40 | 41 | %% Check first pose equality 42 | pose_1 = result.at(1); 43 | CHECK('pose_1.equals(Pose2,1e-4)',pose_1.equals(Pose2,1e-4)); 44 | -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/gtsam_tests/testPose3SLAMExample.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % GTSAM Copyright 2010, Georgia Tech Research Corporation, 3 | % Atlanta, Georgia 30332-0415 4 | % All Rights Reserved 5 | % Authors: Frank Dellaert, et al. (see THANKS for the full author list) 6 | % 7 | % See LICENSE for the license information 8 | % 9 | % @brief Read graph from file and perform GraphSLAM 10 | % @author Frank Dellaert 11 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 12 | 13 | import gtsam.* 14 | 15 | %% Create a hexagon of poses 16 | hexagon = circlePose3(6,1.0); 17 | p0 = hexagon.at(0); 18 | p1 = hexagon.at(1); 19 | 20 | %% create a Pose graph with one equality constraint and one measurement 21 | fg = NonlinearFactorGraph; 22 | fg.add(NonlinearEqualityPose3(0, p0)); 23 | delta = p0.between(p1); 24 | covariance = noiseModel.Diagonal.Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180]); 25 | fg.add(BetweenFactorPose3(0,1, delta, covariance)); 26 | fg.add(BetweenFactorPose3(1,2, delta, covariance)); 27 | fg.add(BetweenFactorPose3(2,3, delta, covariance)); 28 | fg.add(BetweenFactorPose3(3,4, delta, covariance)); 29 | fg.add(BetweenFactorPose3(4,5, delta, covariance)); 30 | fg.add(BetweenFactorPose3(5,0, delta, covariance)); 31 | 32 | %% Create initial config 33 | initial = Values; 34 | s = 0.10; 35 | initial.insert(0, p0); 36 | initial.insert(1, hexagon.at(1).retract(s*randn(6,1))); 37 | initial.insert(2, hexagon.at(2).retract(s*randn(6,1))); 38 | initial.insert(3, hexagon.at(3).retract(s*randn(6,1))); 39 | initial.insert(4, hexagon.at(4).retract(s*randn(6,1))); 40 | initial.insert(5, hexagon.at(5).retract(s*randn(6,1))); 41 | 42 | %% optimize 43 | optimizer = LevenbergMarquardtOptimizer(fg, initial); 44 | result = optimizer.optimizeSafely; 45 | 46 | pose_1 = result.at(1); 47 | CHECK('pose_1.equals(Pose3,1e-4)',pose_1.equals(p1,1e-4)); 48 | 49 | 50 | -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/gtsam_tests/testThinBayesTree.m: -------------------------------------------------------------------------------- 1 | % /* ---------------------------------------------------------------------------- 2 | % 3 | % * GTSAM Copyright 2010, Georgia Tech Research Corporation, 4 | % * Atlanta, Georgia 30332-0415 5 | % * All Rights Reserved 6 | % * Authors: Frank Dellaert, et al. (see THANKS for the full author list) 7 | % 8 | % * See LICENSE for the license information 9 | % 10 | % * -------------------------------------------------------------------------- */ 11 | % 12 | % /** 13 | % * @file testThinBayesTree.cpp 14 | % * @brief Test of binary tree 15 | % * @date Sep 14, 2012 16 | % * @author Frank Dellaert 17 | % * @author Jean-Guillaume Durand 18 | % */ 19 | 20 | %% Run the tests 21 | import gtsam.* 22 | bayesTree = thinBayesTree(3,2); 23 | EQUALITY('7 = bayesTree.size', 7, bayesTree.size); -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/gtsam_tests/testThinTree.m: -------------------------------------------------------------------------------- 1 | % /* ---------------------------------------------------------------------------- 2 | % 3 | % * GTSAM Copyright 2010, Georgia Tech Research Corporation, 4 | % * Atlanta, Georgia 30332-0415 5 | % * All Rights Reserved 6 | % * Authors: Frank Dellaert, et al. (see THANKS for the full author list) 7 | % 8 | % * See LICENSE for the license information 9 | % 10 | % * -------------------------------------------------------------------------- */ 11 | % 12 | % /** 13 | % * @file testThinTree.cpp 14 | % * @brief Test of binary tree 15 | % * @date Sep 13, 2012 16 | % * @author Frank Dellaert 17 | % * @author Jean-Guillaume Durand 18 | % */ 19 | 20 | %% Clear working space 21 | clc, close all, clear all; 22 | 23 | %% Create different trees for our example 24 | import gtsam.* 25 | t0 = thinTree(2,1); 26 | t1 = thinTree(3,2); 27 | % Add contents in it 28 | % TODO 29 | %% Create the set of expected output TestValues 30 | expectedNumberOfNodes0 = 3; 31 | expectedNumberOfNodes1 = 7; 32 | expectedParentsOf6in1 = [3 1]; 33 | expectedParentsOf7in1 = [3 1]; 34 | 35 | %% Run the tests 36 | % Tree depth 37 | %TODO 38 | % Number of parents for each node 39 | %TODO 40 | % Number of elements 41 | EQUALITY('expectedNumberOfNodes0,t0.getNumberOfElements', expectedNumberOfNodes0,t0.getNumberOfElements); 42 | EQUALITY('expectedNumberOfNodes1,t1.getNumberOfElements', expectedNumberOfNodes1,t1.getNumberOfElements); 43 | % Parents linking 44 | EQUALITY('expectedParentsOf6in1,t1.getParents(6)', expectedParentsOf6in1,t1.getParents(6)); 45 | EQUALITY('expectedParentsOf7in1,t1.getParents(7)', expectedParentsOf7in1,t1.getParents(7)); 46 | % Adding an element 47 | 48 | bn = thinTreeBayesNet(3,2); 49 | EQUALITY('7 = bn.size', 7, bn.size); -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/gtsam_tests/testThinTreeBayesNet.m: -------------------------------------------------------------------------------- 1 | % /* ---------------------------------------------------------------------------- 2 | % 3 | % * GTSAM Copyright 2010, Georgia Tech Research Corporation, 4 | % * Atlanta, Georgia 30332-0415 5 | % * All Rights Reserved 6 | % * Authors: Frank Dellaert, et al. (see THANKS for the full author list) 7 | % 8 | % * See LICENSE for the license information 9 | % 10 | % * -------------------------------------------------------------------------- */ 11 | % 12 | % /** 13 | % * @file testThinTreeBayesNet.cpp 14 | % * @brief Test of binary tree 15 | % * @date Sep 13, 2012 16 | % * @author Frank Dellaert 17 | % * @author Jean-Guillaume Durand 18 | % */ 19 | 20 | %% Run the tests 21 | import gtsam.* 22 | [bayesNet tree] = thinTreeBayesNet(4,2); 23 | EQUALITY('7 = bayesNet.size', 7, bayesNet.size); 24 | gtsam.plotBayesNet(bayesNet); -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/gtsam_tests/testVisualISAMExample.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % GTSAM Copyright 2010, Georgia Tech Research Corporation, 3 | % Atlanta, Georgia 30332-0415 4 | % All Rights Reserved 5 | % Authors: Frank Dellaert, et al. (see THANKS for the full author list) 6 | % 7 | % See LICENSE for the license information 8 | % 9 | % @brief A simple visual SLAM example for structure from motion 10 | % @author Duy-Nguyen Ta 11 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 12 | 13 | import gtsam.* 14 | 15 | % Data Options 16 | options.triangle = false; 17 | options.nrCameras = 20; 18 | options.showImages = false; 19 | 20 | % iSAM Options 21 | options.hardConstraint = false; 22 | options.pointPriors = false; 23 | options.batchInitialization = true; 24 | options.reorderInterval = 10; 25 | options.alwaysRelinearize = false; 26 | 27 | % Display Options 28 | options.saveDotFile = false; 29 | options.printStats = false; 30 | options.drawInterval = 5; 31 | options.cameraInterval = 1; 32 | options.drawTruePoses = false; 33 | options.saveFigures = false; 34 | options.saveDotFiles = false; 35 | 36 | %% Generate data 37 | [data,truth] = VisualISAMGenerateData(options); 38 | 39 | %% Initialize iSAM with the first pose and points 40 | [noiseModels,isam,result,nextPose] = VisualISAMInitialize(data,truth,options); 41 | 42 | %% Main loop for iSAM: stepping through all poses 43 | for frame_i=3:options.nrCameras 44 | [isam,result,nextPose] = VisualISAMStep(data,noiseModels,isam,result,truth,nextPose); 45 | end 46 | 47 | for i=1:size(truth.cameras,2) 48 | pose_i = result.at(symbol('x',i)); 49 | CHECK('pose_i.equals(truth.cameras{i}.pose,1e-5)',pose_i.equals(truth.cameras{i}.pose,1e-5)) 50 | end 51 | 52 | for j=1:size(truth.points,2) 53 | point_j = result.at(symbol('l',j)); 54 | CHECK('point_j.equals(truth.points{j},1e-5)',point_j.equals(truth.points{j},1e-5)) 55 | end 56 | -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/gtsam_tests/test_gtsam.m: -------------------------------------------------------------------------------- 1 | % Test runner script - runs each test 2 | 3 | display 'Starting: testJacobianFactor' 4 | testJacobianFactor 5 | 6 | display 'Starting: testKalmanFilter' 7 | testKalmanFilter 8 | 9 | display 'Starting: testLocalizationExample' 10 | testLocalizationExample 11 | 12 | display 'Starting: testOdometryExample' 13 | testOdometryExample 14 | 15 | display 'Starting: testPlanarSLAMExample' 16 | testPlanarSLAMExample 17 | 18 | display 'Starting: testPose2SLAMExample' 19 | testPose2SLAMExample 20 | 21 | display 'Starting: testPose3SLAMExample' 22 | testPose3SLAMExample 23 | 24 | display 'Starting: testSFMExample' 25 | testSFMExample 26 | 27 | display 'Starting: testStereoVOExample' 28 | testStereoVOExample 29 | 30 | display 'Starting: testVisualISAMExample' 31 | testVisualISAMExample 32 | 33 | % end of tests 34 | display 'Tests complete!' 35 | -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/gtsam_tests/thinBayesTree.m: -------------------------------------------------------------------------------- 1 | function bayesTree = thinBayesTree(depth, width) 2 | import gtsam.* 3 | bayesNet = thinTreeBayesNet(depth, width); 4 | bayesTree = GaussianBayesTree(bayesNet); 5 | end -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/gtsam_tests/thinTreeBayesNet.m: -------------------------------------------------------------------------------- 1 | function [bayesNet, tree] = thinTreeBayesNet(depth,width) 2 | % thinTreeBayesNet 3 | 4 | import gtsam.* 5 | bayesNet = GaussianBayesNet; 6 | tree = thinTree(depth,width); 7 | 8 | % Add root to the Bayes net 9 | gc = gtsam.GaussianConditional(1, 5*rand(1), 5*rand(1), 3*rand(1)); 10 | bayesNet.push_front(gc); 11 | 12 | n=tree.getNumberOfElements(); 13 | for i=2:n 14 | % Getting the parents of that node 15 | parents = tree.getParents(i); 16 | di = tree.getNodeDepth(i); 17 | % Create and link the corresponding GaussianConditionals 18 | if tree.getW == 1 || di == 2 19 | % Creation of single-parent GaussianConditional 20 | gc = gtsam.GaussianConditional(n-i, 5*rand(1), 5*rand(1), n-parents(1), 5*rand(1), 5*rand(1)); 21 | elseif tree.getW == 2 || di == 3 22 | % GaussianConditionalj associated with the second parent 23 | gc = gtsam.GaussianConditional(n-i, 5*rand(1), 5*rand(1), n-parents(1), 5*rand(1), n-parents(2), 5*rand(1), 5*rand(1)); 24 | end 25 | % Add conditional to the Bayes net 26 | bayesNet.push_front(gc); 27 | end 28 | 29 | end -------------------------------------------------------------------------------- /matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/gtsam_wrapper.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rising-turtle/VINS-Mono_Ext/1aa44f5ec4c4a1b64726e8fe2d0d28ca3b6ade23/matlab_tools/libs_dir/gtsam-toolbox-2.3.0-win64/toolbox/gtsam_wrapper.mexw64 -------------------------------------------------------------------------------- /matlab_tools/result_comp/analyze_data.m: -------------------------------------------------------------------------------- 1 | function analyze_data( fdir ) 2 | %UNTITLED Summary of this function goes here 3 | % Detailed explanation goes here 4 | 5 | if nargin == 0 6 | fdir = './VINS-Mono_ext/ETAS_2F_640_30'; 7 | fdir = './VINS-Mono_ext/ETAS_4F_640_30'; 8 | fdir = './VINS-Mono_ext/ETAS_F2_640_30/vins_kf'; 9 | fdir = './VINS-Mono_ext/ETAS_F4_640_30'; 10 | fdir = './VINS-Mono/ETAS_2F_640_30'; 11 | fdir = './VINS-Mono/ETAS_4F_640_30'; 12 | fdir = './VINS-Mono/ETAS_F2_640_30'; 13 | fdir = './VINS-Mono/ETAS_F4_640_30'; 14 | fdir = './vslam_okvis/ETAS_2F_640_30'; 15 | fdir = './vslam_okvis/ETAS_4F_640_30'; 16 | fdir = './vslam_okvis/ETAS_F2_640_30'; 17 | fdir = './vslam_okvis/ETAS_F4_640_30'; 18 | end 19 | 20 | ev = [] 21 | 22 | for i=1:10 23 | fname = strcat('/run_',int2str(i)); 24 | %fname = strcat(fname, '.csv'); 25 | fname = strcat(fname, '.log'); 26 | fname = strcat(fdir, fname); 27 | fid = fopen(fname); 28 | col = textscan(fid, '%s', 'delimiter', '\n'); 29 | lines = col{1}; 30 | N = size(lines,1); 31 | line = lines{N}; 32 | 33 | data = textscan(line, '%f %f %f %f\n', 'delimiter', ','); 34 | e = [data{2}, data{3}, data{4}]; 35 | ev = [ev; norm(e)]; 36 | end 37 | 38 | fprintf('%s has mean %f std %f min: %f max: %f\n', fdir, mean(ev), std(ev), ... 39 | min(ev), max(ev)); 40 | 41 | 42 | end 43 | 44 | -------------------------------------------------------------------------------- /matlab_tools/result_comp/result.docx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rising-turtle/VINS-Mono_Ext/1aa44f5ec4c4a1b64726e8fe2d0d28ca3b6ade23/matlab_tools/result_comp/result.docx -------------------------------------------------------------------------------- /matlab_tools/simulation/add_path.m: -------------------------------------------------------------------------------- 1 | 2 | 3 | global g_libs 4 | g_libs = '../libs_dir'; 5 | addpath(strcat(g_libs, '/gtsam-toolbox-2.3.0-win64/toolbox')); -------------------------------------------------------------------------------- /matlab_tools/simulation/computeMeanSigma.m: -------------------------------------------------------------------------------- 1 | %% 2 | % Feb. 9 2018, He Zhang, hxzhang1@ualr.edu 3 | % compute mean and sigma 4 | function [mu, sigma, err_sigma] = computeMeanSigma(T, g_pts) 5 | 6 | %% compute mean 7 | import gtsam.* 8 | M = size(T{1}, 1); 9 | mu = zeros(M, 3); 10 | err_sigma = zeros(M,1); 11 | for i = 1:M 12 | e_pts = []; 13 | err = zeros(length(T),1); 14 | for j = 1:length(T) 15 | pt = T{j}(i, :); 16 | e_pts = [e_pts; pt]; 17 | 18 | % compute the error of pose i 19 | g_pt = g_pts(i, :); 20 | dpt = pt - g_pt; 21 | err(j) = sqrt(dpt*dpt'); 22 | end 23 | mu(i, : ) = mean(e_pts); 24 | err_sigma(i) = std(err); 25 | end 26 | 27 | %% compute covarianze 28 | for i = 1:M 29 | e_pts = []; 30 | mu_i = mu(i, :); 31 | COV = zeros(3, 3); 32 | for j = 1:length(T) 33 | pt = T{j}(i, :); 34 | dpt = pt - mu_i; 35 | COV = COV + dpt' * dpt; 36 | end 37 | COV = COV./length(T); 38 | sigma{i} = COV; 39 | end 40 | 41 | end 42 | -------------------------------------------------------------------------------- /matlab_tools/simulation/computeRMSE.m: -------------------------------------------------------------------------------- 1 | %% 2 | % Feb. 9 2018, He Zhang, hxzhang1@ualr.edu 3 | % compute RMSE using the result from ISAM2 4 | function [rmse, g_pts] = computeRMSE(result, truth, options) 5 | 6 | %% 7 | import gtsam.* 8 | g_pts = []; 9 | e_pts = []; 10 | rmse = []; 11 | M = 1 ; 12 | while result.exists(symbol('x', M)) 13 | ii = symbol('x', M); 14 | pi = result.at(ii); 15 | epi = pose2pt(pi); 16 | gpi = pose2pt(truth.cameras{M}.pose); 17 | e_pts = [e_pts; epi]; 18 | g_pts = [g_pts; gpi]; 19 | rmse_i = computeRMSEArray(e_pts, g_pts); 20 | if options.rmse_ratio 21 | dis = computeDis(g_pts); 22 | rmse_i = rmse_i/dis; 23 | end 24 | rmse = [rmse; rmse_i]; 25 | M = M + options.cameraInterval; 26 | end 27 | 28 | end 29 | 30 | %% compute the distance of the traversed route 31 | function dis = computeDis(pts) 32 | dis = 0; 33 | for i=2:size(pts) 34 | dp = pts(i, :) - pts(i-1, :); 35 | dis = dis + sqrt(dp * dp'); 36 | end 37 | end 38 | 39 | -------------------------------------------------------------------------------- /matlab_tools/simulation/computeRMSEArray.m: -------------------------------------------------------------------------------- 1 | %% 2 | % Feb. 9 2018, He Zhang, hxzhang1@ualr.edu 3 | % compute rmse given the vector 4 | function r = computeRMSEArray(pts1, pts2) 5 | 6 | r2 = 0; 7 | for i=1:size(pts1,1) 8 | dp = pts1(i, :) - pts2(i, :); 9 | r2 = r2 + dp*dp'; 10 | end 11 | r = r2/size(pts1,1); 12 | r = sqrt(r); 13 | end 14 | 15 | 16 | -------------------------------------------------------------------------------- /matlab_tools/simulation/createFeatureHorizontal.m: -------------------------------------------------------------------------------- 1 | %% create a number of features by distributing around area [x, y] on plane z 2 | 3 | function vfeats = createFeatureHorizontal(x, y, z) 4 | 5 | vfeats = []; 6 | id = 1; 7 | for i=1:length(x) 8 | for j = 1:length(y) 9 | feature.x = x(i); 10 | feature.y = y(j); 11 | feature.z = z; 12 | feature.id = id; 13 | id = id + 1; 14 | vfeats = [vfeats; feature]; 15 | end 16 | end 17 | 18 | 19 | end -------------------------------------------------------------------------------- /matlab_tools/simulation/createObservations.m: -------------------------------------------------------------------------------- 1 | % given camera pose, and feature locations, generate observations 2 | function obs = createObservations(feats, pts, tilt) 3 | % compute camera's tilt rotation 4 | angle = -(pi/2+tilt); 5 | ca = cos(angle); 6 | sa = sin(angle); 7 | Ro2c = [1 0 0; 8 | 0 ca -sa; 9 | 0 sa ca;]; 10 | 11 | Rc2o = Ro2c'; 12 | 13 | % get feature points 14 | gpc = get_feature_pc(feats); 15 | 16 | % hold on; 17 | % ggpt = gpc'; 18 | % scatter3(ggpt(:,1), ggpt(:,2), ggpt(:,3), 'g'); 19 | 20 | N = size(gpc,2); 21 | % camera model 22 | cam = get_rs_r200(); 23 | 24 | obs = []; 25 | for i=1:size(pts,1) 26 | t = pts(i, :)'; 27 | t = -Rc2o * t; 28 | lpc = transform_pc(Rc2o, t, gpc); 29 | 30 | % lppt = lpc'; 31 | % figure; 32 | % grid on; 33 | % scatter3(lppt(:,1), lppt(:,2), lppt(:,3), 'g'); 34 | 35 | %% check out how many features are available 36 | s_obi.obs=[]; 37 | for j=1:N 38 | fpt = lpc(:, j); 39 | [good, px, py] = in_cam_view(cam, fpt); 40 | if good == 1 41 | obs_ij = generate_obs(feats(j), i, fpt, px, py); 42 | s_obi.obs = [s_obi.obs; obs_ij]; 43 | end 44 | end 45 | 46 | % add features into observation 47 | obs = [obs; s_obi]; 48 | end 49 | 50 | end 51 | 52 | %% generate observation for this point 53 | function obs = generate_obs(feat, pose_i, lpt, px, py) 54 | gpt = [feat.x feat.y feat.z]; 55 | obs.gpt = gpt; 56 | obs.lpt = lpt; 57 | obs.pose_id = pose_i; 58 | obs.feat_id = feat.id; 59 | obs.obs_x = px; 60 | obs.obs_y = py; 61 | end 62 | 63 | %% get point cloud from features 64 | function gpc = get_feature_pc(feats) 65 | n = length(feats); 66 | gpc = zeros(3, n); 67 | for i = 1:n 68 | ft = feats(i); 69 | gpc(1, i) = ft.x; 70 | gpc(2, i) = ft.y; 71 | gpc(3, i) = ft.z; 72 | end 73 | end 74 | 75 | %% transform point cloud from global coordinate into camera's coordinate 76 | function lpc = transform_pc(R, t, gpc) 77 | [m, n] = size(gpc); 78 | translation = repmat(t, 1, n); 79 | lpc = R * gpc + translation; 80 | end 81 | -------------------------------------------------------------------------------- /matlab_tools/simulation/default_option.m: -------------------------------------------------------------------------------- 1 | function options = default_option() 2 | % Data Options 3 | options.triangle = false; 4 | options.nrCameras = 20; 5 | options.showImages = false; 6 | 7 | % iSAM Options 8 | options.hardConstraint = false; 9 | options.pointPriors = true; 10 | options.batchInitialization = true; 11 | options.reorderInterval = 10; 12 | options.alwaysRelinearize = false; 13 | 14 | % Display Options 15 | options.saveDotFile = false; 16 | options.printStats = false; 17 | options.drawInterval = 10; 18 | options.cameraInterval = 1; 19 | options.drawTruePoses = false; 20 | options.saveFigures = false; 21 | options.saveDotFiles = false; 22 | 23 | % plot rmse 24 | options.rmse_ratio = false; 25 | end 26 | -------------------------------------------------------------------------------- /matlab_tools/simulation/error_bar.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rising-turtle/VINS-Mono_Ext/1aa44f5ec4c4a1b64726e8fe2d0d28ca3b6ade23/matlab_tools/simulation/error_bar.fig -------------------------------------------------------------------------------- /matlab_tools/simulation/error_bar.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rising-turtle/VINS-Mono_Ext/1aa44f5ec4c4a1b64726e8fe2d0d28ca3b6ade23/matlab_tools/simulation/error_bar.png -------------------------------------------------------------------------------- /matlab_tools/simulation/extractTrajectory.m: -------------------------------------------------------------------------------- 1 | %% 2 | % Feb. 9 2018, He Zhang, hxzhang1@ualr.edu 3 | % plot the result of isam2 4 | function [e_pts, g_pts] = extractTrajectory(result, options, truth) 5 | 6 | %% Plot cameras 7 | import gtsam.* 8 | M = 1; 9 | 10 | %% gt_pts, and estimate_pts 11 | g_pts = []; 12 | e_pts = []; 13 | 14 | while result.exists(symbol('x',M)) 15 | ii = symbol('x',M); 16 | pose_i = result.at(ii); 17 | 18 | ept = pose2pt(pose_i); 19 | e_pts = [e_pts; ept]; 20 | gpi = pose2pt(truth.cameras{M}.pose); 21 | g_pts = [g_pts; gpi]; 22 | 23 | M = M + options.cameraInterval; 24 | end 25 | 26 | end -------------------------------------------------------------------------------- /matlab_tools/simulation/get_rs_r200.m: -------------------------------------------------------------------------------- 1 | %% return rs 200 PINHOLE model 2 | function cam = get_rs_r200() 3 | cam.cx = 318.837; 4 | cam.cy = 240.594; 5 | cam.fx = 615.426; 6 | cam.fy = 625.456; 7 | cam.width = 640; 8 | cam.height = 480; 9 | end -------------------------------------------------------------------------------- /matlab_tools/simulation/in_cam_view.m: -------------------------------------------------------------------------------- 1 | %% check out whether this feature point is in camera's view 2 | function [good, px, py] = in_cam_view(cam, pt) 3 | good = 0; 4 | px = -1; 5 | py = -1; 6 | if pt(3) <= 0 %% z <= 0 7 | return; 8 | end 9 | 10 | dx = 20; 11 | dy = 5; 12 | px = cam.cx + cam.fx * pt(1)/pt(3); 13 | py = cam.cy + cam.fy * pt(2)/pt(3); 14 | if px > dx && px < cam.width - dx && py >dy && py < cam.height - dy 15 | good = 1; 16 | end 17 | end -------------------------------------------------------------------------------- /matlab_tools/simulation/main_swing_simulation.m: -------------------------------------------------------------------------------- 1 | %% 2 | % Feb. 9 2018, He Zhang, hxzhang1@ualr.edu 3 | % main swing simulation using ISAM2 4 | 5 | param_global; 6 | 7 | % [ data1, truth1, isam1, result1, options1 ] = swing_simulation_ISAM(); 8 | % swing_simulation_ISAM_Plot(truth, data, isam, result, options); 9 | % options1.drawTruePoses = true; 10 | % swing_simulation_Plot_surf(truth1, isam1, result1, options1); 11 | 12 | [ data1, truth1, result1, options1 ] = swing_simulation_SFM(); 13 | 14 | % options1.rmse_ratio = true; 15 | rmse1 = computeRMSE(result1, truth1, options1); 16 | 17 | plot(rmse1, 'g-*'); 18 | 19 | % [ data2, truth2, isam2, result2, options2 ] = swing_simulation_ISAM_VINS(); 20 | % options2.drawTruePoses = true; 21 | % swing_simulation_Plot(truth2, isam2, result2, options2); 22 | % options2.rmse_ratio = true; 23 | [ data2, truth2, result2, options2 ] = swing_simulation_SFM_VINS(); 24 | rmse2 = computeRMSE(result2, truth2, options2); 25 | 26 | plotRMSE(rmse1, rmse2); 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /matlab_tools/simulation/main_swing_simulation_statistics.m: -------------------------------------------------------------------------------- 1 | %% 2 | % Feb. 9 2018, He Zhang, hxzhang1@ualr.edu 3 | % main swing simulation using ISAM2 4 | 5 | add_path; 6 | 7 | M = 500; 8 | T1 = {}; 9 | g_pts = []; 10 | 11 | i = 1; 12 | while i<=M 13 | 14 | % [ data1, truth1, isam1, result1, options1 ] = swing_simulation_ISAM(); 15 | [ data1, truth1, result1, options1, converge ] = swing_simulation_SFM(); 16 | [ epts, g_pts] = extractTrajectory(result1, options1, truth1); 17 | if converge 18 | T1{i} = epts; 19 | fprintf('run i = %d for M = %d\n', i, M); 20 | i = i + 1; 21 | else 22 | % plot_trajectory_and_gt(epts, g_pts, 'g'); 23 | fprintf('fail to converge at i = %d\n', i); 24 | end 25 | end 26 | % 27 | [mu1, sigma1, sigma_err1] = computeMeanSigma(T1, g_pts); 28 | 29 | T2 = {}; 30 | i = 1; 31 | while i<=M 32 | 33 | % [ data1, truth1, isam1, result1, options1 ] = swing_simulation_ISAM(); 34 | [ data2, truth2, result2, options2, converge ] = swing_simulation_SFM_VINS(); 35 | [epts, g_pts1] = extractTrajectory(result2, options2, truth2); 36 | if converge 37 | T2{i} = epts; 38 | fprintf('run i = %d for M = %d \n', i, M); 39 | i = i + 1; 40 | else 41 | % plot_trajectory_and_gt(epts, g_pts1, 'r'); 42 | fprintf('fail to converge at i = %d\n', i); 43 | end 44 | end 45 | 46 | [mu2, sigma2, sigma_err2] = computeMeanSigma(T2, g_pts); 47 | 48 | fmat = 'mu_sigma.mat'; 49 | save(fmat, 'mu1', 'sigma1', 'sigma_err1', 'mu2', 'sigma2', 'sigma_err2', 'g_pts'); 50 | fprintf('succeed to compute and save mean, sigma and ground truth\n'); 51 | 52 | % plot_result_statistics(); 53 | 54 | 55 | 56 | 57 | 58 | 59 | -------------------------------------------------------------------------------- /matlab_tools/simulation/mu_sigma.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rising-turtle/VINS-Mono_Ext/1aa44f5ec4c4a1b64726e8fe2d0d28ca3b6ade23/matlab_tools/simulation/mu_sigma.mat -------------------------------------------------------------------------------- /matlab_tools/simulation/mu_sigma_100.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rising-turtle/VINS-Mono_Ext/1aa44f5ec4c4a1b64726e8fe2d0d28ca3b6ade23/matlab_tools/simulation/mu_sigma_100.mat -------------------------------------------------------------------------------- /matlab_tools/simulation/param_global.m: -------------------------------------------------------------------------------- 1 | %% 2 | % Feb. 10 2018, tune parameters 3 | 4 | global g_param; 5 | %% noise 6 | g_param.odo_t_std = 0.07; %0.07; % 0.05; % 0.15; 7 | g_param.odo_R_std = 0.02; % 0.02; 8 | g_param.pt_std = 0.01; % 0.02 % 0.05; 9 | g_param.pix_std = 1.; %1. 2.; 10 | 11 | g_param.H = 0.7; % 0.7 12 | g_param.feature_step = 0.4; 13 | g_param.pose_step = 0.3; % 0.3; 14 | g_param.swing_times = 5; % 7; 15 | g_param.num_pts = 70; %70; 16 | 17 | g_param.swing_angle = 15; % 30; % degree 18 | g_param.x_range = 1.0; 19 | 20 | g_param.iterate_num = 10; 21 | 22 | g_param.diverge_lambda = 100000; % if diverge, lambda = diverge_lambda 23 | g_param.diverge_error = 200000; % if diverge, err > diverge_error 24 | 25 | 26 | -------------------------------------------------------------------------------- /matlab_tools/simulation/plotRMSE.m: -------------------------------------------------------------------------------- 1 | %% 2 | % Feb. 9 2018, He Zhang, hxzhang1@ualr.edu 3 | % compute rmse given the vector 4 | 5 | function plotRMSE(rmse1, rmse2) 6 | 7 | plot(rmse1, 'g-*'); 8 | hold on; 9 | grid on; 10 | xlabel('Number of Poses'); 11 | ylabel('RMSE'); 12 | plot(rmse2, 'r-.s'); 13 | 14 | legend('proposed', 'VINS-Mono'); 15 | end -------------------------------------------------------------------------------- /matlab_tools/simulation/plot_trajectory_and_gt.m: -------------------------------------------------------------------------------- 1 | %% plot trajectory 2 | function plot_trajectory_and_gt(mu1, g_pts, color) 3 | figure; 4 | plot3(g_pts(:,1), g_pts(:,2), g_pts(:,3), 'Color', 'k', 'LineWidth', 2, 'Marker', '*'); % plot ground truth 5 | hold on; 6 | 7 | %import gtsam.* 8 | %% Generate data 9 | tilt = 30.*pi/180.; 10 | R = tiltR(tilt); 11 | 12 | %% plot mu1 sigma1 13 | % for i = 2:size(mu1,1) 14 | % P = sigma1{i}; 15 | % gPp = R*P*R'; 16 | % cov_ellipse_3d(mu1(i,:), gPp, 'g-'); 17 | % end 18 | plot3(mu1(:,1), mu1(:,2), mu1(:,3), 'Color', color, 'LineWidth', 2, 'Marker', 's'); 19 | 20 | 21 | axis([-2 2 -1 14 -1 4]);axis equal 22 | view(3) 23 | % colormap('hot'); 24 | drawnow 25 | grid on; 26 | xlabel('x') 27 | ylabel('y') 28 | zlabel('z') 29 | 30 | end 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /matlab_tools/simulation/pose2pt.m: -------------------------------------------------------------------------------- 1 | %% extract location from pose 2 | function pt = pose2pt(pose) 3 | import gtsam.* 4 | C = pose.translation().vector(); 5 | pt = [C(1), C(2), C(3)]; 6 | end -------------------------------------------------------------------------------- /matlab_tools/simulation/swing_simulation_ISAM.m: -------------------------------------------------------------------------------- 1 | %% 2 | % Feb. 7 2018, He Zhang, hxzhang1@ualr.edu 3 | % run ISAM using swing motion data, and then plot and save results 4 | function [ data, truth, isam, result, options ] = swing_simulation_ISAM(options) 5 | param_global; 6 | global g_param; 7 | if nargin == 0 8 | options = default_option(); 9 | end 10 | import gtsam.* 11 | %% Generate data 12 | tilt = 30.*pi/180.; 13 | H = g_param.H; % 1.2 14 | R = tiltR(tilt); 15 | [obs, pts, vfeats] = swing_simulation_data(tilt, H); 16 | [data,truth, options] = swing_simulation_ISAM_data(options, obs, pts, vfeats, R); 17 | 18 | %% Initialize iSAM with the first pose and points 19 | options.hardConstraint = true; 20 | [noiseModels,isam,result,nextPose, truth] = swing_simulation_ISAM_Initialize(data,truth,options); 21 | % cla; 22 | % VisualISAMPlot(truth, data, isam, result, options); 23 | 24 | %% Main loop for iSAM: stepping through all poses 25 | for frame_i=3:options.nrCameras 26 | [isam,result,nextPose, truth] = swing_simulation_ISAM_Step(data,noiseModels,isam,result,truth,nextPose); 27 | if mod(frame_i,options.drawInterval)==0 28 | % swing_simulation_ISAM_Plot(truth, data, isam, result, options) 29 | end 30 | end 31 | 32 | end 33 | -------------------------------------------------------------------------------- /matlab_tools/simulation/swing_simulation_ISAM_Plot.m: -------------------------------------------------------------------------------- 1 | %% 2 | % plot current state of isam2 3 | function swing_simulation_ISAM_Plot(truth, data, isam, result, options) 4 | 5 | h=gca; 6 | cla(h); 7 | hold on; 8 | 9 | %% Plot points 10 | % Can't use data because current frame might not see all points 11 | marginals = Marginals(isam.getFactorsUnsafe(), isam.calculateEstimate()); % TODO - this is slow 12 | gtsam.plot3DPoints(result, [], marginals); 13 | 14 | %% Plot cameras 15 | import gtsam.* 16 | M = 1; 17 | while result.exists(symbol('x',M)) 18 | ii = symbol('x',M); 19 | pose_i = result.at(ii); 20 | if options.hardConstraint && (M==1) 21 | gtsam.plotPose3(pose_i,[],1); % 10 22 | else 23 | P = marginals.marginalCovariance(ii); 24 | gtsam.plotPose3(pose_i,P,1); % 10 25 | end 26 | if options.drawTruePoses % show ground truth 27 | gtsam.plotPose3(truth.cameras{M}.pose,[],1); 28 | end 29 | 30 | M = M + options.cameraInterval; 31 | end 32 | 33 | %% draw 34 | % axis([-40 40 -40 40 -10 20]);axis equal 35 | axis([-2 2 -1 14 -1 4]);axis equal 36 | view(3) 37 | colormap('hot') 38 | drawnow 39 | grid on; 40 | xlabel('x') 41 | ylabel('y') 42 | zlabel('z') 43 | %% do various optional things 44 | 45 | if options.saveFigures 46 | figToSave = figure('visible','off'); 47 | newax = copyobj(h,figToSave); 48 | colormap(figToSave,'hot'); 49 | set(newax, 'units', 'normalized', 'position', [0.13 0.11 0.775 0.815]); 50 | print(figToSave,'-dpng',sprintf('VisualiSAM%03d.png',M)); 51 | axes(h); 52 | end 53 | 54 | if options.saveDotFiles 55 | isam.saveGraph(sprintf('VisualiSAM%03d.dot',M)); 56 | end 57 | 58 | if options.saveDotFile 59 | isam.saveGraph(sprintf('VisualiSAM.dot')); 60 | end 61 | 62 | if options.printStats 63 | isam.printStats(); 64 | end 65 | -------------------------------------------------------------------------------- /matlab_tools/simulation/swing_simulation_ISAM_VINS.m: -------------------------------------------------------------------------------- 1 | %% 2 | % Feb. 9 2018, He Zhang, hxzhang1@ualr.edu 3 | % run ISAM using swing motion data, following the feature pipeline of 4 | % VINS-Mono 5 | 6 | function [ data, truth, isam, result, options ] = swing_simulation_ISAM_VINS(options) 7 | param_global; 8 | global g_param; 9 | if nargin == 0 10 | options = default_option(); 11 | end 12 | import gtsam.* 13 | %% Generate data 14 | tilt = 30.*pi/180.; 15 | H = g_param.H; % 1.2 16 | R = tiltR(tilt); 17 | [obs, pts, vfeats] = swing_simulation_data_VINS(tilt, H); 18 | [data,truth, options] = swing_simulation_ISAM_data(options, obs, pts, vfeats, R); 19 | 20 | %% Initialize iSAM with the first pose and points 21 | options.hardConstraint = true; 22 | [noiseModels,isam,result,nextPose, truth] = swing_simulation_ISAM_Initialize(data,truth,options); 23 | % cla; 24 | % VisualISAMPlot(truth, data, isam, result, options); 25 | 26 | %% Main loop for iSAM: stepping through all poses 27 | for frame_i=3:options.nrCameras 28 | [isam,result,nextPose, truth] = swing_simulation_ISAM_Step(data,noiseModels,isam,result,truth,nextPose); 29 | if mod(frame_i,options.drawInterval)==0 30 | % swing_simulation_ISAM_Plot(truth, data, isam, result, options) 31 | end 32 | end 33 | 34 | end 35 | -------------------------------------------------------------------------------- /matlab_tools/simulation/swing_simulation_SFM.m: -------------------------------------------------------------------------------- 1 | %% 2 | % Feb. 10 2018, He Zhang, hxzhang1@ualr.edu 3 | % run SFM using swing motion data, and then plot and save results 4 | function [ data, truth, result, options, converge ] = swing_simulation_SFM(options) 5 | clear; 6 | param_global; 7 | global g_param; 8 | if nargin == 0 9 | options = default_option(); 10 | end 11 | import gtsam.* 12 | %% Generate data 13 | tilt = 30.*pi/180.; 14 | H = g_param.H; % 1.2 15 | R = tiltR(tilt); 16 | [obs, pts, vfeats] = swing_simulation_data(tilt, H); 17 | [data,truth, options] = swing_simulation_ISAM_data(options, obs, pts, vfeats, R); 18 | 19 | [ data, truth, result, options, converge ] = SFM(data,truth, options); 20 | 21 | end 22 | 23 | -------------------------------------------------------------------------------- /matlab_tools/simulation/swing_simulation_SFM_VINS.m: -------------------------------------------------------------------------------- 1 | %% 2 | % Feb. 10 2018, He Zhang, hxzhang1@ualr.edu 3 | % run SFM using swing motion data, and then plot and save results 4 | function [ data, truth, result, options, converge ] = swing_simulation_SFM_VINS(options) 5 | clear; 6 | param_global; 7 | global g_param; 8 | if nargin == 0 9 | options = default_option(); 10 | end 11 | import gtsam.* 12 | %% Generate data 13 | tilt = 30.*pi/180.; 14 | H = g_param.H; % 1.2 15 | R = tiltR(tilt); 16 | [obs, pts, vfeats] = swing_simulation_data_VINS(tilt, H); 17 | [data,truth, options] = swing_simulation_ISAM_data(options, obs, pts, vfeats, R); 18 | 19 | [ data, truth, result, options, converge ] = SFM(data,truth, options); 20 | 21 | end 22 | 23 | -------------------------------------------------------------------------------- /matlab_tools/simulation/swing_simulation_data.m: -------------------------------------------------------------------------------- 1 | %% 2 | % Feb. 5 2018, He Zhang, hxzhang1@ualr.edu 3 | % simulate swing motion 4 | function [obs, pts, vfeats] = swing_simulation_data(tilt, H) 5 | %% parameters 6 | param_global; 7 | global g_param; 8 | if nargin == 0 9 | H = g_param.H; % camera's height installed in robocane 10 | tilt = 30.*pi/180.; % camera's tilt angle 11 | end 12 | 13 | %% generate features 14 | % assume features 15 | step = g_param.feature_step; 16 | x = [-2.2:step:2.2]; 17 | y = [0:step:14]; 18 | z = 0; 19 | vfeats = createFeatureHorizontal(x, y, z); % create a number of features on a plane 20 | 21 | %% generate trajectory 22 | px = [-g_param.x_range:g_param.pose_step:g_param.x_range]; 23 | n_swing = g_param.swing_times; % 7; 24 | pts = createTrajectory(px, H, n_swing); % create trajectory swing motion 25 | 26 | %% generate observations 27 | % given camera pose, and feature locations, generate observations 28 | obs = createObservations(vfeats, pts, tilt); 29 | 30 | % show_obs(obs); 31 | 32 | end 33 | 34 | %% show it 35 | function show_obs(obs) 36 | for i =1:length(obs) 37 | obs_i = obs(i); 38 | for j = 1:length(obs_i.obs) 39 | obs_ij = obs_i.obs(j); 40 | fprintf('pose_id %d feat_id %d obs_x: %f obs_y %f\n', obs_ij.pose_id, ... 41 | obs_ij.feat_id, obs_ij.obs_x, obs_ij.obs_y); 42 | fprintf('gpt: %f %f %f, lpt: %f %f %f\n', obs_ij.gpt(1), obs_ij.gpt(2), ... 43 | obs_ij.gpt(3), obs_ij.lpt(1), obs_ij.lpt(2), obs_ij.lpt(3)); 44 | end 45 | end 46 | end 47 | 48 | 49 | 50 | 51 | 52 | -------------------------------------------------------------------------------- /matlab_tools/simulation/swing_simulation_data_VINS.m: -------------------------------------------------------------------------------- 1 | %% 2 | % Feb. 5 2018, He Zhang, hxzhang1@ualr.edu 3 | % simulate swing motion, create feature observation follow VINS pipeline 4 | function [ obs, pts, vfeats ] = swing_simulation_data_VINS( tilt, H ) 5 | %% parameters 6 | param_global; 7 | global g_param; 8 | if nargin == 0 9 | H = g_param.H; % camera's height installed in robocane 10 | tilt = 30.*pi/180.; % camera's tilt angle 11 | end 12 | 13 | %% generate features 14 | % assume features 15 | step = g_param.feature_step; 16 | x = [-2.2:step:2.2]; 17 | y = [0:step:14]; 18 | z = 0; 19 | vfeats = createFeatureHorizontal(x, y, z); % create a number of features on a plane 20 | 21 | %% generate trajectory 22 | px = [-g_param.x_range:g_param.pose_step:g_param.x_range]; 23 | n_swing = g_param.swing_times; % 7; 24 | pts = createTrajectory(px, H, n_swing); % create trajectory swing motion 25 | 26 | %% generate observations 27 | % given camera pose, and feature locations, generate observations 28 | [obs, vfeats] = createObservations_VINS(vfeats, pts, tilt); 29 | 30 | % show_obs(obs); 31 | 32 | end 33 | 34 | %% show it 35 | function show_obs(obs) 36 | for i =1:length(obs) 37 | obs_i = obs(i); 38 | for j = 1:length(obs_i.obs) 39 | obs_ij = obs_i.obs(j); 40 | fprintf('pose_id %d feat_id %d obs_x: %f obs_y %f\n', obs_ij.pose_id, ... 41 | obs_ij.feat_id, obs_ij.obs_x, obs_ij.obs_y); 42 | fprintf('gpt: %f %f %f, lpt: %f %f %f\n', obs_ij.gpt(1), obs_ij.gpt(2), ... 43 | obs_ij.gpt(3), obs_ij.lpt(1), obs_ij.lpt(2), obs_ij.lpt(3)); 44 | end 45 | end 46 | end 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | -------------------------------------------------------------------------------- /matlab_tools/simulation/testMeanSigma.m: -------------------------------------------------------------------------------- 1 | function testMeanSigma() 2 | 3 | T = {}; 4 | M = 1000; 5 | pts = randn(M,3); 6 | for i = 1:M 7 | T{i} = pts(i, :); 8 | end 9 | 10 | [mu, sigma] = computeMeanSigma(T); 11 | 12 | cov_ellipse_3d(mu, sigma{1}); 13 | hold on; 14 | scatter3(pts(:,1), pts(:,2), pts(:,3), 'k+'); 15 | 16 | 17 | end 18 | 19 | %% plot covariance Ellipse 20 | function cov_ellipse_3d(c,P) 21 | % covarianceEllipse3D plots a Gaussian as an uncertainty ellipse 22 | % Based on Maybeck Vol 1, page 366 23 | % k=2.296 corresponds to 1 std, 68.26% of all probability 24 | % k=11.82 corresponds to 3 std, 99.74% of all probability 25 | % 26 | % Modified from http://www.mathworks.com/matlabcentral/newsreader/view_thread/42966 27 | 28 | [e,s] = svd(P); 29 | %k = 11.82; 30 | k=2.296 31 | radii = k*sqrt(diag(s)); 32 | 33 | % generate data for "unrotated" ellipsoid 34 | [xc,yc,zc] = ellipsoid(0,0,0,radii(1),radii(2),radii(3),8); 35 | 36 | % rotate data with orientation matrix U and center M 37 | data = kron(e(:,1),xc) + kron(e(:,2),yc) + kron(e(:,3),zc); 38 | n = size(data,2); 39 | x = data(1:n,:)+c(1); 40 | y = data(n+1:2*n,:)+c(2); 41 | z = data(2*n+1:end,:)+c(3); 42 | 43 | % now plot the rotated ellipse 44 | % sc = surf(x,y,z,abs(xc)); 45 | % sc = mesh(x, y, z, abs(xc)); 46 | % sc = mesh(x, y, z, 'edgecolor', 'g'); 47 | sc = plot3(x, y, z, 'r-'); 48 | shading interp 49 | alpha(0.5) 50 | axis equal 51 | end 52 | -------------------------------------------------------------------------------- /matlab_tools/simulation/tiltR.m: -------------------------------------------------------------------------------- 1 | 2 | function R = tiltR(tilt) 3 | angle = -(pi/2+tilt); 4 | ca = cos(angle); sa = sin(angle); 5 | R = [1 0 0; 6 | 0 ca -sa; 7 | 0 sa ca;]; 8 | end -------------------------------------------------------------------------------- /matlab_tools/simulation/trajectory.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rising-turtle/VINS-Mono_Ext/1aa44f5ec4c4a1b64726e8fe2d0d28ca3b6ade23/matlab_tools/simulation/trajectory.png -------------------------------------------------------------------------------- /support_files/brief_k10L6.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rising-turtle/VINS-Mono_Ext/1aa44f5ec4c4a1b64726e8fe2d0d28ca3b6ade23/support_files/brief_k10L6.bin -------------------------------------------------------------------------------- /support_files/image/vins.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rising-turtle/VINS-Mono_Ext/1aa44f5ec4c4a1b64726e8fe2d0d28ca3b6ade23/support_files/image/vins.png -------------------------------------------------------------------------------- /support_files/image/vins_black.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rising-turtle/VINS-Mono_Ext/1aa44f5ec4c4a1b64726e8fe2d0d28ca3b6ade23/support_files/image/vins_black.png -------------------------------------------------------------------------------- /support_files/paper/jfr2017yi.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rising-turtle/VINS-Mono_Ext/1aa44f5ec4c4a1b64726e8fe2d0d28ca3b6ade23/support_files/paper/jfr2017yi.pdf -------------------------------------------------------------------------------- /support_files/paper/tro_technical_report.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rising-turtle/VINS-Mono_Ext/1aa44f5ec4c4a1b64726e8fe2d0d28ca3b6ade23/support_files/paper/tro_technical_report.pdf -------------------------------------------------------------------------------- /sync_time/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(sync_time) 3 | 4 | set(CMAKE_BUILD_TYPE "Release") 5 | set(CMAKE_CXX_FLAGS "-std=c++11 -O3 -w -mssse3") 6 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -w -g -mssse3") 7 | 8 | find_package(catkin REQUIRED COMPONENTS 9 | roscpp 10 | std_msgs 11 | sensor_msgs 12 | cv_bridge 13 | camera_model 14 | rosbag 15 | ) 16 | 17 | find_package(OpenCV REQUIRED) 18 | 19 | catkin_package() 20 | 21 | include_directories( 22 | ${catkin_INCLUDE_DIRS} 23 | ) 24 | 25 | set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) 26 | # find_package(Eigen3 REQUIRED) 27 | include_directories("/usr/include/eigen3") 28 | include_directories( 29 | ${catkin_INCLUDE_DIRS} 30 | ${EIGEN3_INCLUDE_DIR} 31 | ../vins_estimator/src 32 | ) 33 | 34 | ## vision only SFM 35 | add_executable(vision_sfm 36 | src/vision_sfm.cpp 37 | src/read_file.cpp 38 | src/track_feature.cpp 39 | src/feature_manager.cpp 40 | src/bundle_adjust.cpp 41 | ) 42 | 43 | target_link_libraries(vision_sfm vins_mono ${catkin_LIBRARIES} ${OpenCV_LIBS}) 44 | 45 | ## align IMU's integration to SFM's result 46 | add_executable(sync_time 47 | src/main_sync_time.cpp 48 | src/read_file.cpp 49 | src/shift_imu.cpp 50 | src/rotation_error.cpp 51 | src/imu_structure.cpp 52 | ) 53 | target_link_libraries(sync_time ${catkin_LIBRARIES}) 54 | 55 | ## align IMU's integration to minimize TIC error 56 | add_executable(sync_time2 57 | src/main_sync_time2.cpp 58 | src/read_file.cpp 59 | src/shift_imu2.cpp 60 | src/estimator_wrapper.cpp 61 | src/imu_structure.cpp 62 | src/track_feature.cpp) 63 | target_link_libraries(sync_time2 vins_mono ${catkin_LIBRARIES} ${OpenCV_LIBS}) 64 | 65 | 66 | 67 | 68 | -------------------------------------------------------------------------------- /sync_time/src/bundle_adjust.h: -------------------------------------------------------------------------------- 1 | /* 2 | Jan. 9 2018, He Zhang, hxzhang1@ualr.edu 3 | 4 | input: bunch of images 5 | output: relative rotation between these images 6 | 7 | */ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include "initial/solve_5pts.h" 13 | #include "initial/initial_sfm.h" 14 | #include "initial/initial_alignment.h" 15 | #include "initial/initial_ex_rotation.h" 16 | 17 | using namespace std; 18 | 19 | class CFeatManager; 20 | 21 | class CBundleAdjust 22 | { 23 | public: 24 | CBundleAdjust(int frame_cnt, bool record_result = true); 25 | virtual ~CBundleAdjust(); 26 | 27 | bool SFM(Eigen::Quaterniond Q[], Eigen::Vector3d T[], CFeatManager* ); 28 | bool SFM(CFeatManager*, vector timestamp ); 29 | bool relativePose( Eigen::Matrix3d &relative_R, Eigen::Vector3d &relative_T, int &l, CFeatManager* pFM); 30 | // map mmAllFrames; 31 | int mFrameCnt; 32 | bool mbRecordResult; 33 | MotionEstimator m_estimator; 34 | }; 35 | -------------------------------------------------------------------------------- /sync_time/src/estimator_wrapper.h: -------------------------------------------------------------------------------- 1 | /* 2 | Jan. 14 2018, He Zhang, hxzhang1@ualr.edu 3 | 4 | wrapper of estimator, make it output the result of initialization, 5 | then output an error 6 | 7 | */ 8 | 9 | #pragma once 10 | 11 | #include "estimator.h" 12 | #include "imu_structure.h" 13 | #include 14 | #include 15 | #include 16 | 17 | using namespace std; 18 | 19 | class Estimator; 20 | 21 | class CEstimator 22 | { 23 | public: 24 | CEstimator(); 25 | virtual ~CEstimator(); 26 | 27 | double errInitCalib(vector> >>& vfeat, vector vheader, 28 | vector>& imu_data); // use the drift from TIC as error 29 | 30 | Estimator mEstimator; 31 | double mIMUTime; 32 | void sendIMU(CIMUData& imu_msg); 33 | void reset(); 34 | }; 35 | 36 | -------------------------------------------------------------------------------- /sync_time/src/feature_manager.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Jan. 11 2018, He Zhang, hxzhang1@ualr.edu 3 | 4 | manage features 5 | 6 | */ 7 | #include "feature_manager.h" 8 | 9 | using namespace Eigen; 10 | 11 | CFeatManager::CFeatManager(){} 12 | CFeatManager::~CFeatManager(){} 13 | 14 | void CFeatManager::addFeature(int frame_cnt, vector idx, vector undist_pts) 15 | { 16 | assert(idx.size() == undist_pts.size()); 17 | for(int i=0; imvFeatPts.push_back(pt); 34 | } 35 | } 36 | return ; 37 | } 38 | 39 | vector> CFeatManager::getCorresponding(int frame_l, int frame_r) 40 | { 41 | vector > corres; 42 | for(auto & it: mlFeats) 43 | { 44 | if(it.mStartFrame <= frame_l && it.mStartFrame + it.endFrame() >= frame_r) 45 | { 46 | Vector3d a = Vector3d::Zero(); 47 | Vector3d b = Vector3d::Zero(); 48 | int idx_l = frame_l - it.mStartFrame; 49 | int idx_r = frame_r - it.mStartFrame; 50 | a = it.mvFeatPts[idx_l].m_pt; 51 | b = it.mvFeatPts[idx_r].m_pt; 52 | corres.push_back(make_pair(a,b)); 53 | } 54 | } 55 | return corres; 56 | } 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /sync_time/src/feature_manager.h: -------------------------------------------------------------------------------- 1 | /* 2 | Jan. 11 2018, He Zhang, hxzhang1@ualr.edu 3 | 4 | manage features 5 | 6 | */ 7 | #pragma once 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | using namespace std; 15 | 16 | class CFeatPt 17 | { 18 | public: 19 | CFeatPt(const Eigen::Vector3d& _pt){ 20 | mz = _pt(2); 21 | m_pt = _pt / mz; 22 | } 23 | Eigen::Vector3d m_pt; 24 | double mz; 25 | }; 26 | 27 | class CFeatId 28 | { 29 | public: 30 | const int mFId; 31 | int mStartFrame; 32 | vector mvFeatPts; 33 | double mEstimatedDepth; 34 | int mSolveFlag; // 0: hasn't been solved, 1: solve succeed, 2: solve failed 35 | CFeatId(int feat_id, int start_frame): 36 | mFId(feat_id), mStartFrame(start_frame), 37 | mEstimatedDepth(-1.), mSolveFlag(0) 38 | {} 39 | int endFrame(){ return mStartFrame + mvFeatPts.size() - 1; } 40 | }; 41 | 42 | class CFeatManager 43 | { 44 | public: 45 | CFeatManager(); 46 | virtual ~CFeatManager(); 47 | 48 | void addFeature(int frame_cnt, vector idx, vector undist_pts); 49 | list mlFeats; 50 | 51 | vector> getCorresponding(int frame_l, int frame_r); 52 | }; 53 | -------------------------------------------------------------------------------- /sync_time/src/imu_structure.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Jan. 12 2018, He Zhang, hxzhang1@ualr.edu 3 | 4 | IMU data structure 5 | 6 | */ 7 | 8 | #include "imu_structure.h" 9 | 10 | CIMUData::CIMUData(){} 11 | CIMUData::CIMUData(double timestamp, vector reading) 12 | { 13 | mTimestamp = timestamp; 14 | m_ax = reading[0]; 15 | m_ay = reading[1]; 16 | m_az = reading[2]; 17 | m_wx = reading[3]; 18 | m_wy = reading[4]; 19 | m_wz = reading[5]; 20 | } 21 | 22 | CIMUData::~CIMUData(){} 23 | -------------------------------------------------------------------------------- /sync_time/src/imu_structure.h: -------------------------------------------------------------------------------- 1 | /* 2 | Jan. 12 2018, He Zhang, hxzhang1@ualr.edu 3 | 4 | IMU data structure 5 | 6 | */ 7 | #pragma once 8 | 9 | #include 10 | #include 11 | 12 | using namespace std; 13 | 14 | class CIMUData 15 | { 16 | public: 17 | CIMUData(); 18 | CIMUData(double timestamp, vector data); 19 | ~CIMUData(); 20 | 21 | double mTimestamp; 22 | double m_ax; 23 | double m_ay; 24 | double m_az; 25 | double m_wx; 26 | double m_wy; 27 | double m_wz; 28 | }; 29 | -------------------------------------------------------------------------------- /sync_time/src/rotation_error.h: -------------------------------------------------------------------------------- 1 | /* 2 | Jan. 12 2018, He Zhang, hxzhang1@ualr.edu 3 | 4 | input: 5 | 1. imu readings 6 | 2. sfm rotation result 7 | output: 8 | 1. sum of rotation difference between imu integration and sfm result 9 | */ 10 | 11 | #pragma once 12 | 13 | #include "imu_structure.h" 14 | 15 | class CRotDiff 16 | { 17 | public: 18 | CRotDiff(); 19 | virtual ~CRotDiff(); 20 | 21 | double diffRot(vector> &sfm_rot, vector>& imu_data); 22 | 23 | void integrateIMU(vector> & imu_rot, vector>& imu_data); 24 | }; 25 | 26 | -------------------------------------------------------------------------------- /sync_time/src/shift_imu.h: -------------------------------------------------------------------------------- 1 | /* 2 | Jan. 12 2018, He Zhang, hxzhang1@ualr.edu 3 | 4 | shift imu's timestamp to minimize rotation_difference 5 | 6 | */ 7 | #pragma once 8 | #include 9 | #include 10 | #include "imu_structure.h" 11 | 12 | using namespace std; 13 | class CRotDiff; 14 | 15 | class CShiftIMU 16 | { 17 | public: 18 | CShiftIMU(); 19 | virtual ~CShiftIMU(); 20 | 21 | void prepareSFMData(vector& sfm_time, vector>& sfm_data); 22 | void prepareIMUData(vector& imu_time, vector>& imu_data); 23 | virtual double findIt(int range); // find best shift ms 24 | virtual double shiftIt(double sf); // shift IMU ms, and output rotation difference 25 | virtual bool findMatchByShift(double sf, vector>& ); // given sf ms, find out matched IMU collections 26 | int closeTo(double timestamp, int from, double sf); 27 | 28 | vector m_sfm_time; 29 | vector> m_sfm_qvs; 30 | vector m_imu_time; 31 | vector m_imu_data; 32 | 33 | CRotDiff* mpRotDiff; // given shift seconds, compute the angle difference between IMU's estimation and SFM's estimation 34 | 35 | }; 36 | -------------------------------------------------------------------------------- /sync_time/src/shift_imu2.h: -------------------------------------------------------------------------------- 1 | /* 2 | Jan. 15 2018, He Zhang, hxzhang1@ualr.edu 3 | 4 | shift imu's timestamp to minimize estimator's TIC difference 5 | 6 | */ 7 | 8 | #pragma once 9 | #include 10 | #include 11 | #include 12 | #include "imu_structure.h" 13 | #include "estimator_wrapper.h" 14 | 15 | using namespace std; 16 | 17 | class CShiftIMUByTIC 18 | { 19 | public: 20 | CShiftIMUByTIC(); 21 | virtual ~CShiftIMUByTIC(); 22 | 23 | void prepareFeatData( vector> >>& vfeat, vector & header); 24 | void prepareIMUData(vector& imu_time, vector>& imu_data); 25 | virtual double findIt(int range); // find best shift ms 26 | virtual double shiftIt(double sf); // shift IMU ms, and output rotation difference 27 | virtual bool findMatchByShift(double sf, vector>& ); // given sf ms, find out matched IMU collections 28 | int closeTo(double timestamp, int from, double sf); 29 | 30 | vector m_feats_header; 31 | vector> >> m_feats; 32 | vector m_imu_time; 33 | vector m_imu_data; 34 | 35 | CEstimator mCEstimator; 36 | }; 37 | -------------------------------------------------------------------------------- /sync_time/src/track_feature.h: -------------------------------------------------------------------------------- 1 | /* 2 | Jan. 10 2018, He Zhang, hxzhang1@ualr.edu 3 | continuously extract and track features 4 | input: 5 | a sequence of images, 6 | output: 7 | extracted feature and tracked information 8 | */ 9 | #pragma once 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #include 19 | #include 20 | 21 | #include "camodocal/camera_models/CameraFactory.h" 22 | #include "camodocal/camera_models/CataCamera.h" 23 | #include "camodocal/camera_models/PinholeCamera.h" 24 | 25 | using namespace std; 26 | 27 | class CTrackFeat 28 | { 29 | public: 30 | CTrackFeat(int win_size = 50); 31 | ~CTrackFeat(); 32 | 33 | void initParam(); 34 | void readImage(const cv::Mat& img); 35 | void getFeat(map> >& feat); 36 | 37 | void getListFeat(vector& vImgs, vector> >>& vfeat); 38 | 39 | void rejectWithF(); 40 | void setMask(); 41 | void addPoints(); 42 | cv::Mat showTrack(); 43 | 44 | cv::Mat mCurImg; 45 | cv::Mat mForImg; 46 | cv::Mat mPreImg; 47 | cv::Mat mMask; 48 | vector mvForPts; 49 | vector mvCurPts; 50 | vector mvPrePts; 51 | vector mvNewPts; 52 | vector mvIds; 53 | vector mvTrackCnt; 54 | 55 | static int sNId; 56 | 57 | vector undistortedPoints(); 58 | bool mbPubFrame; 59 | 60 | // camera related parameters 61 | int COL; 62 | int ROW; 63 | double FOCAL_LENGTH; 64 | camodocal::CameraPtr m_camera; 65 | void readCamIntrinsicParam(const string f); 66 | 67 | // threshold parameters 68 | double MIN_DIST; 69 | double F_THRESHOLD; 70 | int MAX_CNT; 71 | int WINDOW_SIZE; 72 | }; 73 | -------------------------------------------------------------------------------- /vins_estimator/launch/.tum_vio.launch.swp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rising-turtle/VINS-Mono_Ext/1aa44f5ec4c4a1b64726e8fe2d0d28ca3b6ade23/vins_estimator/launch/.tum_vio.launch.swp -------------------------------------------------------------------------------- /vins_estimator/launch/3dm.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /vins_estimator/launch/black_box.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 | -------------------------------------------------------------------------------- /vins_estimator/launch/cla.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 | -------------------------------------------------------------------------------- /vins_estimator/launch/euroc.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 | -------------------------------------------------------------------------------- /vins_estimator/launch/euroc_no_extrinsic_param.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 | -------------------------------------------------------------------------------- /vins_estimator/launch/realsense_color.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 | -------------------------------------------------------------------------------- /vins_estimator/launch/realsense_fisheye.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 | -------------------------------------------------------------------------------- /vins_estimator/launch/tum.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 | -------------------------------------------------------------------------------- /vins_estimator/launch/tum_vio.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /vins_estimator/launch/vins_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /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/projection_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 ProjectionFactor : public ceres::SizedCostFunction<2, 7, 7, 7, 1> 11 | { 12 | public: 13 | ProjectionFactor(const Eigen::Vector3d &_pts_i, const Eigen::Vector3d &_pts_j); 14 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 15 | void check(double **parameters); 16 | 17 | Eigen::Vector3d pts_i, pts_j; 18 | Eigen::Matrix tangent_base; 19 | static Eigen::Matrix2d sqrt_info; 20 | static double sum_t; 21 | }; 22 | -------------------------------------------------------------------------------- /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/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); -------------------------------------------------------------------------------- /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/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 | 19 | private: 20 | double testTriangulation(const vector &l, 21 | const vector &r, 22 | cv::Mat_ R, cv::Mat_ t); 23 | void decomposeE(cv::Mat E, 24 | cv::Mat_ &R1, cv::Mat_ &R2, 25 | cv::Mat_ &t1, cv::Mat_ &t2); 26 | }; 27 | 28 | 29 | -------------------------------------------------------------------------------- /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 | const double FOCAL_LENGTH = 460.0; 12 | const int WINDOW_SIZE = 10; 13 | const int NUM_OF_CAM = 1; 14 | const int NUM_OF_F = 1000; 15 | //#define UNIT_SPHERE_ERROR 16 | 17 | extern double INIT_DEPTH; 18 | extern double MIN_PARALLAX; 19 | extern int ESTIMATE_EXTRINSIC; 20 | 21 | extern double ACC_N, ACC_W; 22 | extern double GYR_N, GYR_W; 23 | 24 | extern std::vector RIC; 25 | extern std::vector TIC; 26 | extern Eigen::Vector3d G; 27 | 28 | extern double BIAS_ACC_THRESHOLD; 29 | extern double BIAS_GYR_THRESHOLD; 30 | extern double SOLVER_TIME; 31 | extern int NUM_ITERATIONS; 32 | extern std::string EX_CALIB_RESULT_PATH; 33 | extern std::string VINS_RESULT_PATH; 34 | extern std::string IMU_TOPIC; 35 | extern double TD; 36 | extern double TR; 37 | extern int ESTIMATE_TD; 38 | extern int ROLLING_SHUTTER; 39 | extern double ROW, COL; 40 | 41 | 42 | void readParameters(ros::NodeHandle &n); 43 | 44 | enum SIZE_PARAMETERIZATION 45 | { 46 | SIZE_POSE = 7, 47 | SIZE_SPEEDBIAS = 9, 48 | SIZE_FEATURE = 1 49 | }; 50 | 51 | enum StateOrder 52 | { 53 | O_P = 0, 54 | O_R = 3, 55 | O_V = 6, 56 | O_BA = 9, 57 | O_BG = 12 58 | }; 59 | 60 | enum NoiseOrder 61 | { 62 | O_AN = 0, 63 | O_GN = 3, 64 | O_AW = 6, 65 | O_GW = 9 66 | }; 67 | -------------------------------------------------------------------------------- /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/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); -------------------------------------------------------------------------------- /vins_estimator_back/.CMakeLists.txt.swp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rising-turtle/VINS-Mono_Ext/1aa44f5ec4c4a1b64726e8fe2d0d28ca3b6ade23/vins_estimator_back/.CMakeLists.txt.swp -------------------------------------------------------------------------------- /vins_estimator_back/launch/3dm.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /vins_estimator_back/launch/A3.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /vins_estimator_back/launch/euroc.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /vins_estimator_back/launch/euroc_no_extrinsic_param.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /vins_estimator_back/launch/vins_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /vins_estimator_back/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_back/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_back/src/factor/projection_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 ProjectionFactor : public ceres::SizedCostFunction<2, 7, 7, 7, 1> 11 | { 12 | public: 13 | ProjectionFactor(const Eigen::Vector3d &_pts_i, const Eigen::Vector3d &_pts_j); 14 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 15 | void check(double **parameters); 16 | 17 | Eigen::Vector3d pts_i, pts_j; 18 | Eigen::Matrix tangent_base; 19 | static Eigen::Matrix2d sqrt_info; 20 | static double sum_t; 21 | }; 22 | -------------------------------------------------------------------------------- /vins_estimator_back/src/factor/weight_projection_factor.h: -------------------------------------------------------------------------------- 1 | /* 2 | Jan. 7 2018, He Zhang, hxzhang1@ualr.edu 3 | add weight for projection factor 4 | */ 5 | 6 | #pragma once 7 | 8 | #include 9 | #include 10 | #include 11 | #include "../utility/utility.h" 12 | #include "../utility/tic_toc.h" 13 | #include "../parameters.h" 14 | 15 | class WeightProjectionFactor : public ceres::SizedCostFunction<2, 7, 7, 7, 1> 16 | { 17 | public: 18 | WeightProjectionFactor(const Eigen::Vector3d &_pts_i, const Eigen::Vector3d & _pts_j, double count); 19 | virtual bool Evaluate(double const * const* parameters, double * residuals, double ** jacobians) const; 20 | void check(double** parameters); 21 | 22 | double mcount; // number of times being observed 23 | 24 | Eigen::Vector3d pts_i, pts_j; 25 | Eigen::Matrix tangent_base; 26 | static Eigen::Matrix2d sqrt_info; 27 | static double sum_t; 28 | }; 29 | -------------------------------------------------------------------------------- /vins_estimator_back/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):points{_points},t{_t},is_key_frame{false} 18 | { 19 | }; 20 | map > > points; 21 | double t; 22 | Matrix3d R; 23 | Vector3d T; 24 | IntegrationBase *pre_integration; 25 | bool is_key_frame; 26 | }; 27 | 28 | bool VisualIMUAlignment(map &all_image_frame, Vector3d* Bgs, Vector3d &g, VectorXd &x); -------------------------------------------------------------------------------- /vins_estimator_back/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_back/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 | 19 | private: 20 | double testTriangulation(const vector &l, 21 | const vector &r, 22 | cv::Mat_ R, cv::Mat_ t); 23 | void decomposeE(cv::Mat E, 24 | cv::Mat_ &R1, cv::Mat_ &R2, 25 | cv::Mat_ &t1, cv::Mat_ &t2); 26 | }; 27 | 28 | 29 | -------------------------------------------------------------------------------- /vins_estimator_back/src/loop-closure/ThirdParty/DBoW/FBrief.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FBrief.h 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: functions for BRIEF descriptors 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_F_BRIEF__ 11 | #define __D_T_F_BRIEF__ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | #include "FClass.h" 18 | #include "../DVision/DVision.h" 19 | 20 | namespace DBoW2 { 21 | 22 | /// Functions to manipulate BRIEF descriptors 23 | class FBrief: protected FClass 24 | { 25 | public: 26 | 27 | typedef DVision::BRIEF::bitset TDescriptor; 28 | typedef const TDescriptor *pDescriptor; 29 | 30 | /** 31 | * Calculates the mean value of a set of descriptors 32 | * @param descriptors 33 | * @param mean mean descriptor 34 | */ 35 | static void meanValue(const std::vector &descriptors, 36 | TDescriptor &mean); 37 | 38 | /** 39 | * Calculates the distance between two descriptors 40 | * @param a 41 | * @param b 42 | * @return distance 43 | */ 44 | static double distance(const TDescriptor &a, const TDescriptor &b); 45 | 46 | /** 47 | * Returns a string version of the descriptor 48 | * @param a descriptor 49 | * @return string version 50 | */ 51 | static std::string toString(const TDescriptor &a); 52 | 53 | /** 54 | * Returns a descriptor from a string 55 | * @param a descriptor 56 | * @param s string version 57 | */ 58 | static void fromString(TDescriptor &a, const std::string &s); 59 | 60 | /** 61 | * Returns a mat with the descriptors in float format 62 | * @param descriptors 63 | * @param mat (out) NxL 32F matrix 64 | */ 65 | static void toMat32F(const std::vector &descriptors, 66 | cv::Mat &mat); 67 | 68 | }; 69 | 70 | } // namespace DBoW2 71 | 72 | #endif 73 | 74 | -------------------------------------------------------------------------------- /vins_estimator_back/src/loop-closure/ThirdParty/DBoW/FClass.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FClass.h 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: generic FClass to instantiate templated classes 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_FCLASS__ 11 | #define __D_T_FCLASS__ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | namespace DBoW2 { 18 | 19 | /// Generic class to encapsulate functions to manage descriptors. 20 | /** 21 | * This class must be inherited. Derived classes can be used as the 22 | * parameter F when creating Templated structures 23 | * (TemplatedVocabulary, TemplatedDatabase, ...) 24 | */ 25 | class FClass 26 | { 27 | class TDescriptor; 28 | typedef const TDescriptor *pDescriptor; 29 | 30 | /** 31 | * Calculates the mean value of a set of descriptors 32 | * @param descriptors 33 | * @param mean mean descriptor 34 | */ 35 | virtual void meanValue(const std::vector &descriptors, 36 | TDescriptor &mean) = 0; 37 | 38 | /** 39 | * Calculates the distance between two descriptors 40 | * @param a 41 | * @param b 42 | * @return distance 43 | */ 44 | static double distance(const TDescriptor &a, const TDescriptor &b); 45 | 46 | /** 47 | * Returns a string version of the descriptor 48 | * @param a descriptor 49 | * @return string version 50 | */ 51 | static std::string toString(const TDescriptor &a); 52 | 53 | /** 54 | * Returns a descriptor from a string 55 | * @param a descriptor 56 | * @param s string version 57 | */ 58 | static void fromString(TDescriptor &a, const std::string &s); 59 | 60 | /** 61 | * Returns a mat with the descriptors in float format 62 | * @param descriptors 63 | * @param mat (out) NxL 32F matrix 64 | */ 65 | static void toMat32F(const std::vector &descriptors, 66 | cv::Mat &mat); 67 | }; 68 | 69 | } // namespace DBoW2 70 | 71 | #endif 72 | -------------------------------------------------------------------------------- /vins_estimator_back/src/loop-closure/ThirdParty/DBoW/FeatureVector.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FeatureVector.cpp 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: feature vector 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #include "FeatureVector.h" 11 | #include 12 | #include 13 | #include 14 | 15 | namespace DBoW2 { 16 | 17 | // --------------------------------------------------------------------------- 18 | 19 | FeatureVector::FeatureVector(void) 20 | { 21 | } 22 | 23 | // --------------------------------------------------------------------------- 24 | 25 | FeatureVector::~FeatureVector(void) 26 | { 27 | } 28 | 29 | // --------------------------------------------------------------------------- 30 | 31 | void FeatureVector::addFeature(NodeId id, unsigned int i_feature) 32 | { 33 | FeatureVector::iterator vit = this->lower_bound(id); 34 | 35 | if(vit != this->end() && vit->first == id) 36 | { 37 | vit->second.push_back(i_feature); 38 | } 39 | else 40 | { 41 | vit = this->insert(vit, FeatureVector::value_type(id, 42 | std::vector() )); 43 | vit->second.push_back(i_feature); 44 | } 45 | } 46 | 47 | // --------------------------------------------------------------------------- 48 | 49 | std::ostream& operator<<(std::ostream &out, 50 | const FeatureVector &v) 51 | { 52 | if(!v.empty()) 53 | { 54 | FeatureVector::const_iterator vit = v.begin(); 55 | 56 | const std::vector* f = &vit->second; 57 | 58 | out << "<" << vit->first << ": ["; 59 | if(!f->empty()) out << (*f)[0]; 60 | for(unsigned int i = 1; i < f->size(); ++i) 61 | { 62 | out << ", " << (*f)[i]; 63 | } 64 | out << "]>"; 65 | 66 | for(++vit; vit != v.end(); ++vit) 67 | { 68 | f = &vit->second; 69 | 70 | out << ", <" << vit->first << ": ["; 71 | if(!f->empty()) out << (*f)[0]; 72 | for(unsigned int i = 1; i < f->size(); ++i) 73 | { 74 | out << ", " << (*f)[i]; 75 | } 76 | out << "]>"; 77 | } 78 | } 79 | 80 | return out; 81 | } 82 | 83 | // --------------------------------------------------------------------------- 84 | 85 | } // namespace DBoW2 86 | -------------------------------------------------------------------------------- /vins_estimator_back/src/loop-closure/ThirdParty/DBoW/FeatureVector.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FeatureVector.h 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: feature vector 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_FEATURE_VECTOR__ 11 | #define __D_T_FEATURE_VECTOR__ 12 | 13 | #include "BowVector.h" 14 | #include 15 | #include 16 | #include 17 | 18 | namespace DBoW2 { 19 | 20 | /// Vector of nodes with indexes of local features 21 | class FeatureVector: 22 | public std::map > 23 | { 24 | public: 25 | 26 | /** 27 | * Constructor 28 | */ 29 | FeatureVector(void); 30 | 31 | /** 32 | * Destructor 33 | */ 34 | ~FeatureVector(void); 35 | 36 | /** 37 | * Adds a feature to an existing node, or adds a new node with an initial 38 | * feature 39 | * @param id node id to add or to modify 40 | * @param i_feature index of feature to add to the given node 41 | */ 42 | void addFeature(NodeId id, unsigned int i_feature); 43 | 44 | /** 45 | * Sends a string versions of the feature vector through the stream 46 | * @param out stream 47 | * @param v feature vector 48 | */ 49 | friend std::ostream& operator<<(std::ostream &out, const FeatureVector &v); 50 | 51 | }; 52 | 53 | } // namespace DBoW2 54 | 55 | #endif 56 | 57 | -------------------------------------------------------------------------------- /vins_estimator_back/src/loop-closure/ThirdParty/DBoW/QueryResults.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * File: QueryResults.cpp 3 | * Date: March, November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: structure to store results of database queries 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #include 11 | #include 12 | #include "QueryResults.h" 13 | 14 | using namespace std; 15 | 16 | namespace DBoW2 17 | { 18 | 19 | // --------------------------------------------------------------------------- 20 | 21 | ostream & operator<<(ostream& os, const Result& ret ) 22 | { 23 | os << ""; 24 | return os; 25 | } 26 | 27 | // --------------------------------------------------------------------------- 28 | 29 | ostream & operator<<(ostream& os, const QueryResults& ret ) 30 | { 31 | if(ret.size() == 1) 32 | os << "1 result:" << endl; 33 | else 34 | os << ret.size() << " results:" << endl; 35 | 36 | QueryResults::const_iterator rit; 37 | for(rit = ret.begin(); rit != ret.end(); ++rit) 38 | { 39 | os << *rit; 40 | if(rit + 1 != ret.end()) os << endl; 41 | } 42 | return os; 43 | } 44 | 45 | // --------------------------------------------------------------------------- 46 | 47 | void QueryResults::saveM(const std::string &filename) const 48 | { 49 | fstream f(filename.c_str(), ios::out); 50 | 51 | QueryResults::const_iterator qit; 52 | for(qit = begin(); qit != end(); ++qit) 53 | { 54 | f << qit->Id << " " << qit->Score << endl; 55 | } 56 | 57 | f.close(); 58 | } 59 | 60 | // --------------------------------------------------------------------------- 61 | 62 | } // namespace DBoW2 63 | 64 | -------------------------------------------------------------------------------- /vins_estimator_back/src/loop-closure/ThirdParty/DUtils/DException.h: -------------------------------------------------------------------------------- 1 | /* 2 | * File: DException.h 3 | * Project: DUtils library 4 | * Author: Dorian Galvez-Lopez 5 | * Date: October 6, 2009 6 | * Description: general exception of the library 7 | * License: see the LICENSE.txt file 8 | * 9 | */ 10 | 11 | #pragma once 12 | 13 | #ifndef __D_EXCEPTION__ 14 | #define __D_EXCEPTION__ 15 | 16 | #include 17 | #include 18 | using namespace std; 19 | 20 | namespace DUtils { 21 | 22 | /// General exception 23 | class DException : 24 | public exception 25 | { 26 | public: 27 | /** 28 | * Creates an exception with a general error message 29 | */ 30 | DException(void) throw(): m_message("DUtils exception"){} 31 | 32 | /** 33 | * Creates an exception with a custom error message 34 | * @param msg: message 35 | */ 36 | DException(const char *msg) throw(): m_message(msg){} 37 | 38 | /** 39 | * Creates an exception with a custom error message 40 | * @param msg: message 41 | */ 42 | DException(const string &msg) throw(): m_message(msg){} 43 | 44 | /** 45 | * Destructor 46 | */ 47 | virtual ~DException(void) throw(){} 48 | 49 | /** 50 | * Returns the exception message 51 | */ 52 | virtual const char* what() const throw() 53 | { 54 | return m_message.c_str(); 55 | } 56 | 57 | protected: 58 | /// Error message 59 | string m_message; 60 | }; 61 | 62 | } 63 | 64 | #endif 65 | 66 | -------------------------------------------------------------------------------- /vins_estimator_back/src/loop-closure/ThirdParty/DUtils/DUtils.h: -------------------------------------------------------------------------------- 1 | /* 2 | * File: DUtils.h 3 | * Project: DUtils library 4 | * Author: Dorian Galvez-Lopez 5 | * Date: October 6, 2009 6 | * Description: include file for including all the library functionalities 7 | * License: see the LICENSE.txt file 8 | * 9 | */ 10 | 11 | /*! \mainpage DUtils Library 12 | * 13 | * DUtils library for C++: 14 | * Collection of classes with general utilities for C++ applications. 15 | * 16 | * Written by Dorian Galvez-Lopez, 17 | * University of Zaragoza 18 | * 19 | * Check my website to obtain updates: http://webdiis.unizar.es/~dorian 20 | * 21 | * \section license License 22 | * This program is free software: you can redistribute it and/or modify 23 | * it under the terms of the GNU Lesser General Public License (LGPL) as 24 | * published by the Free Software Foundation, either version 3 of the License, 25 | * or any later version. 26 | * 27 | */ 28 | 29 | 30 | #pragma once 31 | 32 | #ifndef __D_UTILS__ 33 | #define __D_UTILS__ 34 | 35 | /// Several utilities for C++ programs 36 | namespace DUtils 37 | { 38 | } 39 | 40 | // Exception 41 | #include "DException.h" 42 | 43 | #include "Timestamp.h" 44 | // Random numbers 45 | #include "Random.h" 46 | 47 | 48 | #endif 49 | -------------------------------------------------------------------------------- /vins_estimator_back/src/loop-closure/ThirdParty/DVision/DVision.h: -------------------------------------------------------------------------------- 1 | /* 2 | * File: DVision.h 3 | * Project: DVision library 4 | * Author: Dorian Galvez-Lopez 5 | * Date: October 4, 2010 6 | * Description: several functions for computer vision 7 | * License: see the LICENSE.txt file 8 | * 9 | */ 10 | 11 | /*! \mainpage DVision Library 12 | * 13 | * DVision library for C++ and OpenCV: 14 | * Collection of classes with computer vision functionality 15 | * 16 | * Written by Dorian Galvez-Lopez, 17 | * University of Zaragoza 18 | * 19 | * Check my website to obtain updates: http://webdiis.unizar.es/~dorian 20 | * 21 | * \section requirements Requirements 22 | * This library requires the DUtils and DUtilsCV libraries and the OpenCV library. 23 | * 24 | * \section license License 25 | * This program is free software: you can redistribute it and/or modify 26 | * it under the terms of the GNU Lesser General Public License (LGPL) as 27 | * published by the Free Software Foundation, either version 3 of the License, 28 | * or any later version. 29 | * 30 | */ 31 | 32 | #ifndef __D_VISION__ 33 | #define __D_VISION__ 34 | 35 | 36 | /// Computer vision functions 37 | namespace DVision 38 | { 39 | } 40 | 41 | #include "BRIEF.h" 42 | 43 | #endif 44 | -------------------------------------------------------------------------------- /vins_estimator_back/src/loop-closure/ThirdParty/VocabularyBinary.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // VocabularyBinary.cpp 3 | // VINS_ios 4 | // 5 | // Created by Yang Liu on 3/13/17. 6 | // Copyright © 2017 栗大人. All rights reserved. 7 | // 8 | 9 | #include "VocabularyBinary.hpp" 10 | #include 11 | using namespace std; 12 | 13 | VINSLoop::Vocabulary::Vocabulary() 14 | : nNodes(0), nodes(nullptr), nWords(0), words(nullptr) { 15 | } 16 | 17 | VINSLoop::Vocabulary::~Vocabulary() { 18 | if (nodes != nullptr) { 19 | delete [] nodes; 20 | nodes = nullptr; 21 | } 22 | 23 | if (words != nullptr) { 24 | delete [] words; 25 | words = nullptr; 26 | } 27 | } 28 | 29 | void VINSLoop::Vocabulary::serialize(ofstream& stream) { 30 | stream.write((const char *)this, staticDataSize()); 31 | stream.write((const char *)nodes, sizeof(Node) * nNodes); 32 | stream.write((const char *)words, sizeof(Word) * nWords); 33 | } 34 | 35 | void VINSLoop::Vocabulary::deserialize(ifstream& stream) { 36 | stream.read((char *)this, staticDataSize()); 37 | 38 | nodes = new Node[nNodes]; 39 | stream.read((char *)nodes, sizeof(Node) * nNodes); 40 | 41 | words = new Word[nWords]; 42 | stream.read((char *)words, sizeof(Word) * nWords); 43 | } 44 | -------------------------------------------------------------------------------- /vins_estimator_back/src/loop-closure/ThirdParty/VocabularyBinary.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // VocabularyBinary.hpp 3 | // VINS_ios 4 | // 5 | // Created by Yang Liu on 3/13/17. 6 | // Copyright © 2017 栗大人. All rights reserved. 7 | // 8 | 9 | #ifndef VocabularyBinary_hpp 10 | #define VocabularyBinary_hpp 11 | 12 | #include 13 | #include 14 | #include 15 | 16 | namespace VINSLoop { 17 | 18 | struct Node { 19 | int32_t nodeId; 20 | int32_t parentId; 21 | double weight; 22 | uint64_t descriptor[4]; 23 | }; 24 | 25 | struct Word { 26 | int32_t nodeId; 27 | int32_t wordId; 28 | }; 29 | 30 | struct Vocabulary { 31 | int32_t k; 32 | int32_t L; 33 | int32_t scoringType; 34 | int32_t weightingType; 35 | 36 | int32_t nNodes; 37 | int32_t nWords; 38 | 39 | Node* nodes; 40 | Word* words; 41 | 42 | Vocabulary(); 43 | ~Vocabulary(); 44 | 45 | void serialize(std::ofstream& stream); 46 | void deserialize(std::ifstream& stream); 47 | 48 | inline static size_t staticDataSize() { 49 | return sizeof(Vocabulary) - sizeof(Node*) - sizeof(Word*); 50 | } 51 | }; 52 | 53 | } 54 | 55 | #endif /* VocabularyBinary_hpp */ 56 | -------------------------------------------------------------------------------- /vins_estimator_back/src/loop-closure/loop_closure.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * File: demo_brief.cpp 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: demo application of DLoopDetector 6 | * License: see the LICENSE.txt file 7 | */ 8 | 9 | // ---------------------------------------------------------------------------- 10 | #include "loop_closure.h" 11 | 12 | LoopClosure::LoopClosure(const char *_voc_file, int _image_w, int _image_h) 13 | :demo(_voc_file,_image_w, _image_h), IMAGE_W(_image_w), IMAGE_H(_image_h) 14 | { 15 | printf(" loop closure init finish\n"); 16 | } 17 | 18 | void LoopClosure::initCameraModel(const std::string &calib_file) 19 | { 20 | demo.initCameraModel(calib_file); 21 | } 22 | 23 | bool LoopClosure::startLoopClosure(std::vector &keys, std::vector &descriptors, 24 | std::vector &cur_pts, 25 | std::vector &old_pts, 26 | int &old_index) 27 | { 28 | try 29 | { 30 | bool loop_succ = false; 31 | loop_succ = demo.run("BRIEF", keys, descriptors, cur_pts, old_pts, old_index); 32 | return loop_succ; 33 | } 34 | catch(const std::string &ex) 35 | { 36 | cout << "Error: " << ex << endl; 37 | return false; 38 | } 39 | } 40 | 41 | void LoopClosure::eraseIndex(std::vector &erase_index) 42 | { 43 | demo.eraseIndex(erase_index); 44 | } -------------------------------------------------------------------------------- /vins_estimator_back/src/loop-closure/loop_closure.h: -------------------------------------------------------------------------------- 1 | #ifndef __LOOP_CLOSURE__ 2 | #define __LOOP_CLOSURE__ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | // DLoopDetector and DBoW2 9 | #include "ThirdParty/DBoW/DBoW2.h" // defines BriefVocabulary 10 | #include "DLoopDetector.h" // defines BriefLoopDetector 11 | #include "ThirdParty/DVision/DVision.h" // Brief 12 | 13 | // OpenCV 14 | #include 15 | #include 16 | 17 | #include "demoDetector.h" 18 | //#include "brief_extractor.h" 19 | 20 | using namespace DLoopDetector; 21 | using namespace DBoW2; 22 | using namespace DVision; 23 | using namespace std; 24 | 25 | 26 | class LoopClosure 27 | { 28 | public: 29 | LoopClosure(const char *voc_file, int _image_w, int _image_h); 30 | 31 | bool startLoopClosure(std::vector &keys, std::vector &descriptors, 32 | std::vector &cur_pts, 33 | std::vector &old_pts, 34 | int &old_index); 35 | void initCameraModel(const std::string &calib_file); 36 | 37 | void eraseIndex(std::vector &erase_index); 38 | /* data */ 39 | demoDetector demo; 40 | int IMAGE_W; 41 | int IMAGE_H; 42 | }; 43 | 44 | #endif -------------------------------------------------------------------------------- /vins_estimator_back/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 | const double FOCAL_LENGTH = 460.0; 12 | const int WINDOW_SIZE = 10; 13 | const int NUM_OF_CAM = 1; 14 | const int NUM_OF_F = 1000; 15 | const double LOOP_INFO_VALUE = 50.0; 16 | //#define DEPTH_PRIOR 17 | //#define GT 18 | #define UNIT_SPHERE_ERROR 19 | 20 | extern double INIT_DEPTH; 21 | extern double MIN_PARALLAX; 22 | extern int ESTIMATE_EXTRINSIC; 23 | 24 | extern double ACC_N, ACC_W; 25 | extern double GYR_N, GYR_W; 26 | 27 | extern std::vector RIC; 28 | extern std::vector TIC; 29 | extern Eigen::Vector3d G; 30 | 31 | extern double BIAS_ACC_THRESHOLD; 32 | extern double BIAS_GYR_THRESHOLD; 33 | extern double SOLVER_TIME; 34 | extern int NUM_ITERATIONS; 35 | extern std::string EX_CALIB_RESULT_PATH; 36 | extern std::string VINS_RESULT_PATH; 37 | extern std::string VINS_FOLDER_PATH; 38 | extern double ROW, COL; 39 | 40 | extern int LOOP_CLOSURE; 41 | extern int MIN_LOOP_NUM; 42 | extern int MAX_KEYFRAME_NUM; 43 | extern std::string PATTERN_FILE; 44 | extern std::string VOC_FILE; 45 | extern std::string CAM_NAMES; 46 | extern std::string IMAGE_TOPIC; 47 | extern std::string IMU_TOPIC; 48 | 49 | void readParameters(ros::NodeHandle &n); 50 | 51 | enum SIZE_PARAMETERIZATION 52 | { 53 | SIZE_POSE = 7, 54 | SIZE_SPEEDBIAS = 9, 55 | SIZE_FEATURE = 1 56 | }; 57 | 58 | enum StateOrder 59 | { 60 | O_P = 0, 61 | O_R = 3, 62 | O_V = 6, 63 | O_BA = 9, 64 | O_BG = 12 65 | }; 66 | 67 | enum NoiseOrder 68 | { 69 | O_AN = 0, 70 | O_GN = 3, 71 | O_AW = 6, 72 | O_GW = 9 73 | }; 74 | -------------------------------------------------------------------------------- /vins_estimator_back/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_back/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_back/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 | --------------------------------------------------------------------------------