├── CMakeLists.txt ├── README.md ├── config ├── MH_05_cam0.txt ├── MH_05_imu0.txt └── euroc_config.yaml ├── doc ├── README-English-yellow.svg ├── results.png └── vins.gif ├── include ├── System.h ├── backend │ ├── backend.h │ ├── edge.h │ ├── edge_imu.h │ ├── edge_prior.h │ ├── edge_reprojection.h │ ├── eigen_types.h │ ├── imu_integration.h │ ├── loss_function.h │ ├── problem.h │ ├── vertex.h │ ├── vertex_inverse_depth.h │ ├── vertex_point_xyz.h │ ├── vertex_pose.h │ └── vertex_speedbias.h ├── camodocal │ ├── calib │ │ └── CameraCalibration.h │ ├── camera_models │ │ ├── Camera.h │ │ ├── CameraFactory.h │ │ ├── CataCamera.h │ │ ├── CostFunctionFactory.h │ │ ├── EquidistantCamera.h │ │ ├── PinholeCamera.h │ │ └── ScaramuzzaCamera.h │ ├── chessboard │ │ ├── Chessboard.h │ │ ├── ChessboardCorner.h │ │ ├── ChessboardQuad.h │ │ └── Spline.h │ ├── gpl │ │ ├── EigenQuaternionParameterization.h │ │ ├── EigenUtils.h │ │ └── gpl.h │ └── sparse_graph │ │ └── Transform.h ├── estimator.h ├── factor │ └── integration_base.h ├── feature_manager.h ├── feature_tracker.h ├── feature_tracker_parameters.h ├── initial │ ├── initial_alignment.h │ ├── initial_ex_rotation.h │ ├── initial_sfm.h │ └── solve_5pts.h ├── parameters.h └── utility │ ├── tic_toc.h │ └── utility.h ├── src ├── .DS_Store ├── System.cpp ├── backend │ ├── edge.cc │ ├── edge_imu.cc │ ├── edge_prior.cpp │ ├── edge_reprojection.cc │ ├── imu_integration.cc │ ├── loss_function.cc │ ├── problem.cc │ ├── vertex.cc │ └── vertex_pose.cc ├── camera_models │ ├── .DS_Store │ ├── calib │ │ └── CameraCalibration.cc │ ├── camera_models │ │ ├── Camera.cc │ │ ├── CameraFactory.cc │ │ ├── CataCamera.cc │ │ ├── CostFunctionFactory.cc │ │ ├── EquidistantCamera.cc │ │ ├── PinholeCamera.cc │ │ └── ScaramuzzaCamera.cc │ ├── chessboard │ │ └── Chessboard.cc │ ├── gpl │ │ ├── EigenQuaternionParameterization.cc │ │ └── gpl.cc │ ├── intrinsic_calib.cc │ └── sparse_graph │ │ └── Transform.cc ├── estimator.cpp ├── feature_manager.cpp ├── feature_tracker.cpp ├── feature_tracker_parameters.cpp ├── initial │ ├── initial_aligment.cpp │ ├── initial_ex_rotation.cpp │ ├── initial_sfm.cpp │ └── solve_5pts.cpp ├── parameters.cpp └── utility │ └── utility.cpp ├── test ├── CurveFitting.cpp └── run_euroc.cpp └── thirdparty └── Sophus └── sophus ├── se3.hpp ├── so3.hpp └── sophus.hpp /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(vins_estimator) 3 | 4 | set(CMAKE_BUILD_TYPE "Release") 5 | set(CMAKE_CXX_FLAGS "-std=c++11") 6 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g") 7 | set(CMAKE_CXX_FLAGS "-fopenmp") #for EIGEN 3.3 use -mavx & -mfma 8 | set(CMAKE_CXX_FLAGS "-msse4.1") 9 | 10 | set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) 11 | 12 | find_package(Eigen3 REQUIRED) 13 | find_package(Ceres REQUIRED) 14 | find_package(Pangolin REQUIRED) 15 | find_package(OpenCV REQUIRED) 16 | find_package(Boost REQUIRED COMPONENTS filesystem program_options system) 17 | 18 | 19 | message(WARNING "OpenCV_VERSION: ${OpenCV_VERSION}") 20 | 21 | include_directories( 22 | ${PROJECT_SOURCE_DIR}/include 23 | ${EIGEN3_INCLUDE_DIR} 24 | ${CERES_INCLUDE_DIRS} 25 | ${Pangolin_INCLUDE_DIRS} 26 | ${Boost_INCLUDE_DIRS} 27 | ) 28 | 29 | set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/bin) 30 | set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/bin) 31 | 32 | add_library(camera_model SHARED 33 | src/camera_models/chessboard/Chessboard.cc 34 | src/camera_models/calib/CameraCalibration.cc 35 | src/camera_models/camera_models/Camera.cc 36 | src/camera_models/camera_models/CameraFactory.cc 37 | src/camera_models/camera_models/CostFunctionFactory.cc 38 | src/camera_models/camera_models/PinholeCamera.cc 39 | src/camera_models/camera_models/CataCamera.cc 40 | src/camera_models/camera_models/EquidistantCamera.cc 41 | src/camera_models/camera_models/ScaramuzzaCamera.cc 42 | src/camera_models/sparse_graph/Transform.cc 43 | src/camera_models/gpl/gpl.cc 44 | src/camera_models/gpl/EigenQuaternionParameterization.cc) 45 | 46 | target_link_libraries(camera_model ${Boost_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES}) 47 | 48 | 49 | ADD_LIBRARY(MyVio SHARED 50 | src/System.cpp 51 | src/parameters.cpp 52 | src/estimator.cpp 53 | src/feature_manager.cpp 54 | src/feature_tracker.cpp 55 | 56 | src/utility/utility.cpp 57 | src/initial/solve_5pts.cpp 58 | src/initial/initial_aligment.cpp 59 | src/initial/initial_sfm.cpp 60 | src/initial/initial_ex_rotation.cpp 61 | 62 | src/backend/vertex.cc 63 | src/backend/edge.cc 64 | src/backend/problem.cc 65 | src/backend/vertex_pose.cc 66 | src/backend/edge_reprojection.cc 67 | src/backend/edge_imu.cc 68 | src/backend/edge_prior.cpp 69 | src/backend/loss_function.cc 70 | src/backend/imu_integration.cc 71 | ) 72 | 73 | target_link_libraries(MyVio 74 | ${OpenCV_LIBS} 75 | ${CERES_LIBRARIES} 76 | ${Pangolin_LIBRARIES} 77 | camera_model) 78 | 79 | add_executable(run_euroc test/run_euroc.cpp) 80 | target_link_libraries(run_euroc 81 | MyVio 82 | -lpthread) 83 | 84 | add_executable(testCurveFitting test/CurveFitting.cpp) 85 | target_link_libraries(testCurveFitting MyVio) 86 | 87 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # My VINS project 2 | [ ![Build Status](doc/README-English-yellow.svg) ](README_EN.md) 3 | 4 | **Project**： 5 | The work of this project based on the paper of VINS-Mono (https://github.com/HKUST-Aerial-Robotics/VINS-Mono). But we don't use ROS, Ceres, G2o. I implement the back end with Eigen, use the LM algorithmus，sliding window algorithmus，robust kernel function and so on. 6 | It can used in Ubuntu. 7 | 8 | ### Installation： 9 | 10 | 1. pangolin: 11 | 12 | 2. opencv 13 | 14 | 3. Eigen 15 | 16 | 4. Ceres: the initialization of the system use ceres to optimise the sfm，so we also need to installe ceres. 17 | 18 | ### Build 19 | 20 | ```c++ 21 | mkdir build 22 | cd build 23 | cmake .. 24 | make -j4 25 | ``` 26 | 27 | ### Process 28 | #### 1. CurveFitting Example to Verify Our Solver. 29 | ```c++ 30 | cd bin 31 | ./testCurveFitting 32 | ``` 33 | 34 | #### 2. VINs-Mono on Euroc Dataset 35 | ```c++ 36 | cd bin 37 | ./run_euroc /home/dataset/EuRoC/MH-05/mav0/ ../config/ 38 | ``` 39 | ![vins](doc/vins.gif) 40 | 41 | #### 3. VINs-Mono on Simulation Dataset (project homework) 42 | 43 | you can use this code to generate vio data. 44 | 45 | ```c++ 46 | https://github.com/chengKID/VIO_Data_Simulation_ROS.git 47 | ``` 48 | 49 | #### 4. Validation Results 50 | [evo package](https://github.com/MichaelGrupp/evo) 51 | ```c++ 52 | evo_ape euroc euroc_mh05_groundtruth.csv pose_output.txt -a -p 53 | ``` 54 | 55 | ![results](doc/results.png) 56 | 57 | ### Licence 58 | 59 | The source code is released under GPLv3 license. 60 | 61 | -------------------------------------------------------------------------------- /config/euroc_config.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #common parameters 4 | imu_topic: "/imu0" 5 | image_topic: "/cam0/image_raw" 6 | output_path: "/home/cheng_kid/FinalHomework" 7 | 8 | #camera calibration 9 | model_type: PINHOLE 10 | camera_name: camera 11 | image_width: 752 12 | image_height: 480 13 | distortion_parameters: 14 | k1: -2.917e-01 15 | k2: 8.228e-02 16 | p1: 5.333e-05 17 | p2: -1.578e-04 18 | projection_parameters: 19 | fx: 4.616e+02 20 | fy: 4.603e+02 21 | cx: 3.630e+02 22 | cy: 2.481e+02 23 | 24 | # Extrinsic parameter between IMU and Camera. 25 | estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it. 26 | # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess. 27 | # 2 Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning. 28 | #If you choose 0 or 1, you should write down the following matrix. 29 | #Rotation from camera frame to imu frame, imu^R_cam 30 | extrinsicRotation: !!opencv-matrix 31 | rows: 3 32 | cols: 3 33 | dt: d 34 | data: [0.0148655429818, -0.999880929698, 0.00414029679422, 35 | 0.999557249008, 0.0149672133247, 0.025715529948, 36 | -0.0257744366974, 0.00375618835797, 0.999660727178] 37 | #Translation from camera frame to imu frame, imu^T_cam 38 | extrinsicTranslation: !!opencv-matrix 39 | rows: 3 40 | cols: 1 41 | dt: d 42 | data: [-0.0216401454975,-0.064676986768, 0.00981073058949] 43 | 44 | #feature traker paprameters 45 | max_cnt: 150 # max feature number in feature tracking 46 | min_dist: 30 # min distance between two features 47 | freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image 48 | F_threshold: 1.0 # ransac threshold (pixel) 49 | show_track: 1 # publish tracking image as topic 50 | equalize: 1 # if image is too dark or light, trun on equalize to find enough features 51 | fisheye: 0 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points 52 | 53 | #optimization parameters 54 | max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time 55 | max_num_iterations: 8 # max solver itrations, to guarantee real time 56 | keyframe_parallax: 10.0 # keyframe selection threshold (pixel) 57 | 58 | #imu parameters The more accurate parameters you provide, the better performance 59 | acc_n: 0.08 # accelerometer measurement noise standard deviation. #0.2 0.04 60 | gyr_n: 0.004 # gyroscope measurement noise standard deviation. #0.05 0.004 61 | acc_w: 0.00004 # accelerometer bias random work noise standard deviation. #0.02 62 | gyr_w: 2.0e-6 # gyroscope bias random work noise standard deviation. #4.0e-5 63 | g_norm: 9.81007 # gravity magnitude 64 | 65 | #loop closure parameters 66 | loop_closure: 0 # start loop closure 67 | load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path' 68 | fast_relocalization: 0 # useful in real-time and large project 69 | pose_graph_save_path: "/home/hyj/slam_course_vins/" # save and load path 70 | 71 | #unsynchronization parameters 72 | estimate_td: 0 # online estimate time offset between camera and imu 73 | td: 0.0 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock) 74 | 75 | #rolling shutter parameters 76 | rolling_shutter: 0 # 0: global shutter camera, 1: rolling shutter camera 77 | rolling_shutter_tr: 0 # unit: s. rolling shutter read out time per frame (from data sheet). 78 | 79 | #visualization parameters 80 | save_image: 0 # save image in pose graph for visualization prupose; you can close this function by setting 0 81 | visualize_imu_forward: 0 # output imu forward propogation to achieve low latency and high frequence results 82 | visualize_camera_size: 0.4 # size of camera marker in RVIZ 83 | -------------------------------------------------------------------------------- /doc/README-English-yellow.svg: -------------------------------------------------------------------------------- 1 | READMEREADMEEnglishEnglish -------------------------------------------------------------------------------- /doc/results.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengKID/My_VINS/240d0a1f49371611c186fdce8bbaf4f5bbe282a1/doc/results.png -------------------------------------------------------------------------------- /doc/vins.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengKID/My_VINS/240d0a1f49371611c186fdce8bbaf4f5bbe282a1/doc/vins.gif -------------------------------------------------------------------------------- /include/System.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | // #include 13 | // #include 14 | #include 15 | 16 | #include "estimator.h" 17 | #include "parameters.h" 18 | #include "feature_tracker.h" 19 | 20 | 21 | //imu for vio 22 | struct IMU_MSG 23 | { 24 | double header; 25 | Eigen::Vector3d linear_acceleration; 26 | Eigen::Vector3d angular_velocity; 27 | }; 28 | typedef std::shared_ptr ImuConstPtr; 29 | 30 | //image for vio 31 | struct IMG_MSG { 32 | double header; 33 | vector points; 34 | vector id_of_point; 35 | vector u_of_point; 36 | vector v_of_point; 37 | vector velocity_x_of_point; 38 | vector velocity_y_of_point; 39 | }; 40 | typedef std::shared_ptr ImgConstPtr; 41 | 42 | class System 43 | { 44 | public: 45 | System(std::string sConfig_files); 46 | 47 | ~System(); 48 | 49 | void PubImageData(double dStampSec, cv::Mat &img); 50 | 51 | void PubImuData(double dStampSec, const Eigen::Vector3d &vGyr, 52 | const Eigen::Vector3d &vAcc); 53 | 54 | // thread: visual-inertial odometry 55 | void ProcessBackEnd(); 56 | void Draw(); 57 | 58 | pangolin::OpenGlRenderState s_cam; 59 | pangolin::View d_cam; 60 | 61 | #ifdef __APPLE__ 62 | void InitDrawGL(); 63 | void DrawGLFrame(); 64 | #endif 65 | 66 | private: 67 | 68 | //feature tracker 69 | std::vector r_status; 70 | std::vector r_err; 71 | // std::queue img_buf; 72 | 73 | // ros::Publisher pub_img, pub_match; 74 | // ros::Publisher pub_restart; 75 | 76 | FeatureTracker trackerData[NUM_OF_CAM]; 77 | double first_image_time; 78 | int pub_count = 1; 79 | bool first_image_flag = true; 80 | double last_image_time = 0; 81 | bool init_pub = 0; 82 | 83 | //estimator 84 | Estimator estimator; 85 | 86 | std::condition_variable con; 87 | double current_time = -1; 88 | std::queue imu_buf; 89 | std::queue feature_buf; 90 | // std::queue relo_buf; 91 | int sum_of_wait = 0; 92 | 93 | std::mutex m_buf; 94 | std::mutex m_state; 95 | std::mutex i_buf; 96 | std::mutex m_estimator; 97 | 98 | double latest_time; 99 | Eigen::Vector3d tmp_P; 100 | Eigen::Quaterniond tmp_Q; 101 | Eigen::Vector3d tmp_V; 102 | Eigen::Vector3d tmp_Ba; 103 | Eigen::Vector3d tmp_Bg; 104 | Eigen::Vector3d acc_0; 105 | Eigen::Vector3d gyr_0; 106 | bool init_feature = 0; 107 | bool init_imu = 1; 108 | double last_imu_t = 0; 109 | std::ofstream ofs_pose; 110 | std::vector vPath_to_draw; 111 | bool bStart_backend; 112 | std::vector, ImgConstPtr>> getMeasurements(); 113 | 114 | // TODO: HOMEWORK store the lm's computation cost in txt 115 | std::ofstream save_times; 116 | int times_count = 0; 117 | 118 | }; -------------------------------------------------------------------------------- /include/backend/backend.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by gaoxiang19 on 11/17/18. 3 | // 4 | 5 | #ifndef SLAM_COURSE_BACKEND_H 6 | #define SLAM_COURSE_BACKEND_H 7 | 8 | #include "problem.h" 9 | #include "vertex.h" 10 | #include "edge.h" 11 | #include "loss_function.h" 12 | 13 | #include "vertex_pose.h" 14 | #include "vertex_point_xyz.h" 15 | #include "vertex_inverse_depth.h" 16 | 17 | #include "edge_reprojection.h" 18 | 19 | 20 | #endif //SLAM_COURSE_BACKEND_H 21 | -------------------------------------------------------------------------------- /include/backend/edge.h: -------------------------------------------------------------------------------- 1 | #ifndef MYSLAM_BACKEND_EDGE_H 2 | #define MYSLAM_BACKEND_EDGE_H 3 | 4 | #include 5 | #include 6 | #include "eigen_types.h" 7 | #include 8 | #include "loss_function.h" 9 | 10 | namespace myslam { 11 | namespace backend { 12 | 13 | class Vertex; 14 | 15 | /** 16 | * With edge we can compute the residual, residual = prediction - measurement，the dimention is defined in function 17 | * Cost function is residual*information*residual，it is a number, which can be minimised at the back-end 18 | */ 19 | class Edge { 20 | public: 21 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 22 | 23 | /** 24 | * constructor, automaticaly allocate the space for jacobi 25 | * @param residual_dimension 残差维度 26 | * @param num_verticies 顶点数量 27 | * @param verticies_types 顶点类型名称, if not provide, we don't check the type 28 | */ 29 | explicit Edge(int residual_dimension, int num_verticies, 30 | const std::vector &verticies_types = std::vector()); 31 | 32 | virtual ~Edge(); 33 | 34 | /// Return id 35 | unsigned long Id() const { return id_; } 36 | 37 | /** 38 | * Set the vertex 39 | * @param vertex corresponding vertex object 40 | */ 41 | bool AddVertex(std::shared_ptr vertex) { 42 | verticies_.emplace_back(vertex); 43 | return true; 44 | } 45 | 46 | /** 47 | * Set some contexs 48 | * @param vertices set the sequence by reference oder 49 | * @return 50 | */ 51 | bool SetVertex(const std::vector> &vertices) { 52 | verticies_ = vertices; 53 | return true; 54 | } 55 | 56 | /// Return the first vertex 57 | std::shared_ptr GetVertex(int i) { 58 | return verticies_[i]; 59 | } 60 | 61 | /// Return all vertexs 62 | std::vector> Verticies() const { 63 | return verticies_; 64 | } 65 | 66 | /// Return the number of all vertexs 67 | size_t NumVertices() const { return verticies_.size(); } 68 | 69 | /// Return the type, this would be implemented in derived class 70 | virtual std::string TypeInfo() const = 0; 71 | 72 | /// Compute the residual, this would be implemented in derived class 73 | virtual void ComputeResidual() = 0; 74 | 75 | /// Compute jacobi, this would be implemented in derived class 76 | /// This back-end don't allow automatic compute the derivation, this jacobi must be implemented in derived class 77 | virtual void ComputeJacobians() = 0; 78 | 79 | // ///计算该edge对Hession矩阵的影响，由子类实现 80 | // virtual void ComputeHessionFactor() = 0; 81 | 82 | /// Compute the squared error, it should multiply informatin matrix 83 | double Chi2() const; 84 | double RobustChi2() const; 85 | 86 | /// Return residual 87 | VecX Residual() const { return residual_; } 88 | 89 | /// Return jacobi 90 | std::vector Jacobians() const { return jacobians_; } 91 | 92 | /// Set information matrix 93 | void SetInformation(const MatXX &information) { 94 | information_ = information; 95 | // sqrt information 96 | sqrt_information_ = Eigen::LLT(information_).matrixL().transpose(); 97 | } 98 | 99 | /// Return information matrix 100 | MatXX Information() const { 101 | return information_; 102 | } 103 | 104 | MatXX SqrtInformation() const { 105 | return sqrt_information_; 106 | } 107 | 108 | void SetLossFunction(LossFunction* ptr){ lossfunction_ = ptr; } 109 | LossFunction* GetLossFunction(){ return lossfunction_;} 110 | void RobustInfo(double& drho, MatXX& info) const; 111 | 112 | /// Set observation 113 | void SetObservation(const VecX &observation) { 114 | observation_ = observation; 115 | } 116 | 117 | /// Return observation 118 | VecX Observation() const { return observation_; } 119 | 120 | /// Check whether all edge information have been seted 121 | bool CheckValid(); 122 | 123 | int OrderingId() const { return ordering_id_; } 124 | 125 | void SetOrderingId(int id) { ordering_id_ = id; }; 126 | 127 | protected: 128 | unsigned long id_; // edge id 129 | int ordering_id_; //edge id in problem 130 | std::vector verticies_types_; // all vertexs info. used for debug 131 | std::vector> verticies_; // the corresponding vertex 132 | VecX residual_; // residual 133 | std::vector jacobians_; // jacobi，the dimention is residual x vertex[i] 134 | MatXX information_; // information matrix 135 | MatXX sqrt_information_; 136 | VecX observation_; // observations 137 | 138 | LossFunction *lossfunction_; 139 | }; 140 | 141 | } 142 | } 143 | 144 | #endif 145 | -------------------------------------------------------------------------------- /include/backend/edge_imu.h: -------------------------------------------------------------------------------- 1 | #ifndef MYSLAM_BACKEND_IMUEDGE_H 2 | #define MYSLAM_BACKEND_IMUEDGE_H 3 | 4 | #include 5 | #include 6 | #include "../thirdparty/Sophus/sophus/se3.hpp" 7 | 8 | #include "eigen_types.h" 9 | #include "edge.h" 10 | #include "../factor/integration_base.h" 11 | 12 | namespace myslam { 13 | namespace backend { 14 | 15 | /** 16 | * This edge is imu's error and each edge has four vertexs, the corresponding vertex are: Pi Mi Pj Mj 17 | */ 18 | class EdgeImu : public Edge { 19 | public: 20 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 21 | 22 | explicit EdgeImu(IntegrationBase* _pre_integration):pre_integration_(_pre_integration), 23 | Edge(15, 4, std::vector{"VertexPose", "VertexSpeedBias", "VertexPose", "VertexSpeedBias"}) { 24 | // if (pre_integration_) { 25 | // pre_integration_->GetJacobians(dr_dbg_, dv_dbg_, dv_dba_, dp_dbg_, dp_dba_); 26 | // Mat99 cov_meas = pre_integration_->GetCovarianceMeasurement(); 27 | // Mat66 cov_rand_walk = pre_integration_->GetCovarianceRandomWalk(); 28 | // Mat1515 cov = Mat1515::Zero(); 29 | // cov.block<9, 9>(0, 0) = cov_meas; 30 | // cov.block<6, 6>(9, 9) = cov_rand_walk; 31 | // SetInformation(cov.inverse()); 32 | // } 33 | } 34 | 35 | /// Return the type of the information 36 | virtual std::string TypeInfo() const override { return "EdgeImu"; } 37 | 38 | /// Compute the residual 39 | virtual void ComputeResidual() override; 40 | 41 | /// Compute the jacobi 42 | virtual void ComputeJacobians() override; 43 | 44 | // static void SetGravity(const Vec3 &g) { 45 | // gravity_ = g; 46 | // } 47 | 48 | private: 49 | enum StateOrder 50 | { 51 | O_P = 0, 52 | O_R = 3, 53 | O_V = 6, 54 | O_BA = 9, 55 | O_BG = 12 56 | }; 57 | IntegrationBase* pre_integration_; 58 | static Vec3 gravity_; 59 | 60 | Mat33 dp_dba_ = Mat33::Zero(); 61 | Mat33 dp_dbg_ = Mat33::Zero(); 62 | Mat33 dr_dbg_ = Mat33::Zero(); 63 | Mat33 dv_dba_ = Mat33::Zero(); 64 | Mat33 dv_dbg_ = Mat33::Zero(); 65 | }; 66 | 67 | } 68 | } 69 | #endif 70 | -------------------------------------------------------------------------------- /include/backend/edge_prior.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by heyijia on 19-1-30. 3 | // 4 | 5 | #ifndef SLAM_COURSE_EDGE_PRIOR_H 6 | #define SLAM_COURSE_EDGE_PRIOR_H 7 | 8 | #include 9 | #include 10 | 11 | #include 12 | 13 | #include "eigen_types.h" 14 | #include "edge.h" 15 | 16 | 17 | namespace myslam { 18 | namespace backend { 19 | 20 | /** 21 | * EdgeSE3Prior，this edge has only one contex, the contex is: Ti 22 | */ 23 | class EdgeSE3Prior : public Edge { 24 | public: 25 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 26 | 27 | EdgeSE3Prior(const Vec3 &p, const Qd &q) : 28 | Edge(6, 1, std::vector{"VertexPose"}), 29 | Pp_(p), Qp_(q) {} 30 | 31 | /// Return the type of the information 32 | virtual std::string TypeInfo() const override { return "EdgeSE3Prior"; } 33 | 34 | /// Compute the residual 35 | virtual void ComputeResidual() override; 36 | 37 | /// Compute the jacobi 38 | virtual void ComputeJacobians() override; 39 | 40 | 41 | private: 42 | Vec3 Pp_; // pose prior 43 | Qd Qp_; // Rotation prior 44 | }; 45 | 46 | } 47 | } 48 | 49 | 50 | #endif //SLAM_COURSE_EDGE_PRIOR_H 51 | -------------------------------------------------------------------------------- /include/backend/edge_reprojection.h: -------------------------------------------------------------------------------- 1 | #ifndef MYSLAM_BACKEND_VISUALEDGE_H 2 | #define MYSLAM_BACKEND_VISUALEDGE_H 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | #include "eigen_types.h" 10 | #include "edge.h" 11 | 12 | namespace myslam { 13 | namespace backend { 14 | 15 | /** 16 | * This edge is visual reprojection error and it has three contexs, the contexs are: 17 | * the InveseDepth of landmarks、the camera pose at the time that this landmark is observed first time: T_World_From_Body1， 18 | * and the corresponding mearsurement of Camera pose: T_World_From_Body2。 19 | * Note: the order of verticies_contex must be InveseDepth、T_World_From_Body1、T_World_From_Body2. 20 | */ 21 | class EdgeReprojection : public Edge { 22 | public: 23 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 24 | 25 | EdgeReprojection(const Vec3 &pts_i, const Vec3 &pts_j) 26 | : Edge(2, 4, std::vector{"VertexInverseDepth", "VertexPose", "VertexPose", "VertexPose"}) { 27 | pts_i_ = pts_i; 28 | pts_j_ = pts_j; 29 | } 30 | 31 | /// Return the type of the information 32 | virtual std::string TypeInfo() const override { return "EdgeReprojection"; } 33 | 34 | /// Compute the residual 35 | virtual void ComputeResidual() override; 36 | 37 | /// Compute the jacobi 38 | virtual void ComputeJacobians() override; 39 | 40 | // void SetTranslationImuFromCamera(Eigen::Quaterniond &qic_, Vec3 &tic_); 41 | 42 | private: 43 | //Translation imu from camera 44 | // Qd qic; 45 | // Vec3 tic; 46 | 47 | //measurements 48 | Vec3 pts_i_, pts_j_; 49 | }; 50 | 51 | /** 52 | * This edge is visual reprojection error and it has two contexs, the contexs are: 53 | * landmark's XYZ in world coordinante system、the camera pose which can observe the landmark: T_World_From_Body1 54 | * Note: the order of verticies_contex must be XYZ、T_World_From_Body1。 55 | */ 56 | class EdgeReprojectionXYZ : public Edge { 57 | public: 58 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 59 | 60 | EdgeReprojectionXYZ(const Vec3 &pts_i) 61 | : Edge(2, 2, std::vector{"VertexXYZ", "VertexPose"}) { 62 | obs_ = pts_i; 63 | } 64 | 65 | /// Return the type of the information 66 | virtual std::string TypeInfo() const override { return "EdgeReprojectionXYZ"; } 67 | 68 | /// Compute the residual 69 | virtual void ComputeResidual() override; 70 | 71 | /// Compute the jacobi 72 | virtual void ComputeJacobians() override; 73 | 74 | void SetTranslationImuFromCamera(Eigen::Quaterniond &qic_, Vec3 &tic_); 75 | 76 | private: 77 | //Translation imu from camera 78 | Qd qic; 79 | Vec3 tic; 80 | 81 | //measurements 82 | Vec3 obs_; 83 | }; 84 | 85 | /** 86 | * Just a example for computing the reprojects' pose 87 | */ 88 | class EdgeReprojectionPoseOnly : public Edge { 89 | public: 90 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 91 | 92 | EdgeReprojectionPoseOnly(const Vec3 &landmark_world, const Mat33 &K) : 93 | Edge(2, 1, std::vector{"VertexPose"}), 94 | landmark_world_(landmark_world), K_(K) {} 95 | 96 | /// Return the type of the information 97 | virtual std::string TypeInfo() const override { return "EdgeReprojectionPoseOnly"; } 98 | 99 | /// Compute the residual 100 | virtual void ComputeResidual() override; 101 | 102 | /// Compute the jacobi 103 | virtual void ComputeJacobians() override; 104 | 105 | private: 106 | Vec3 landmark_world_; 107 | Mat33 K_; 108 | }; 109 | 110 | } 111 | } 112 | 113 | #endif 114 | -------------------------------------------------------------------------------- /include/backend/eigen_types.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by gaoxiang19 on 11/3/18. 3 | // 4 | 5 | #ifndef MYSLAM_EIGEN_TYPES_H 6 | #define MYSLAM_EIGEN_TYPES_H 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | // double matricies 14 | typedef Eigen::Matrix MatXX; 15 | typedef Eigen::Matrix Mat1010; 16 | typedef Eigen::Matrix Mat1313; 17 | typedef Eigen::Matrix Mat810; 18 | typedef Eigen::Matrix Mat83; 19 | typedef Eigen::Matrix Mat66; 20 | typedef Eigen::Matrix Mat53; 21 | typedef Eigen::Matrix Mat43; 22 | typedef Eigen::Matrix Mat42; 23 | typedef Eigen::Matrix Mat33; 24 | typedef Eigen::Matrix Mat22; 25 | typedef Eigen::Matrix Mat23; 26 | typedef Eigen::Matrix Mat88; 27 | typedef Eigen::Matrix Mat77; 28 | typedef Eigen::Matrix Mat49; 29 | typedef Eigen::Matrix Mat89; 30 | typedef Eigen::Matrix Mat94; 31 | typedef Eigen::Matrix Mat98; 32 | typedef Eigen::Matrix Mat99; 33 | typedef Eigen::Matrix Mat66; 34 | typedef Eigen::Matrix Mat96; 35 | typedef Eigen::Matrix Mat81; 36 | typedef Eigen::Matrix Mat18; 37 | typedef Eigen::Matrix Mat91; 38 | typedef Eigen::Matrix Mat19; 39 | typedef Eigen::Matrix Mat84; 40 | typedef Eigen::Matrix Mat48; 41 | typedef Eigen::Matrix Mat44; 42 | typedef Eigen::Matrix Mat1414; 43 | typedef Eigen::Matrix Mat1515; 44 | 45 | // float matricies 46 | typedef Eigen::Matrix Mat33f; 47 | typedef Eigen::Matrix Mat103f; 48 | typedef Eigen::Matrix Mat22f; 49 | typedef Eigen::Matrix Vec3f; 50 | typedef Eigen::Matrix Vec2f; 51 | typedef Eigen::Matrix Vec6f; 52 | typedef Eigen::Matrix Mat18f; 53 | typedef Eigen::Matrix Mat66f; 54 | typedef Eigen::Matrix Mat88f; 55 | typedef Eigen::Matrix Mat84f; 56 | typedef Eigen::Matrix Mat66f; 57 | typedef Eigen::Matrix Mat44f; 58 | typedef Eigen::Matrix Mat1212f; 59 | typedef Eigen::Matrix Mat1313f; 60 | typedef Eigen::Matrix Mat1010f; 61 | typedef Eigen::Matrix Mat99f; 62 | typedef Eigen::Matrix Mat42f; 63 | typedef Eigen::Matrix Mat62f; 64 | typedef Eigen::Matrix Mat12f; 65 | typedef Eigen::Matrix MatXXf; 66 | typedef Eigen::Matrix Mat1414f; 67 | 68 | // double vectors 69 | typedef Eigen::Matrix Vec15; 70 | typedef Eigen::Matrix Vec14; 71 | typedef Eigen::Matrix Vec13; 72 | typedef Eigen::Matrix Vec10; 73 | typedef Eigen::Matrix Vec9; 74 | typedef Eigen::Matrix Vec8; 75 | typedef Eigen::Matrix Vec7; 76 | typedef Eigen::Matrix Vec6; 77 | typedef Eigen::Matrix Vec5; 78 | typedef Eigen::Matrix Vec4; 79 | typedef Eigen::Matrix Vec3; 80 | typedef Eigen::Matrix Vec2; 81 | typedef Eigen::Matrix Vec1; 82 | typedef Eigen::Matrix VecX; 83 | 84 | // float vectors 85 | typedef Eigen::Matrix Vec12f; 86 | typedef Eigen::Matrix Vec8f; 87 | typedef Eigen::Matrix Vec10f; 88 | typedef Eigen::Matrix Vec4f; 89 | typedef Eigen::Matrix Vec12f; 90 | typedef Eigen::Matrix Vec13f; 91 | typedef Eigen::Matrix Vec9f; 92 | typedef Eigen::Matrix VecXf; 93 | typedef Eigen::Matrix Vec14f; 94 | 95 | // Quaternions 96 | typedef Eigen::Quaterniond Qd; 97 | typedef Eigen::Quaternionf Qf; 98 | 99 | // Vector of Eigen vectors 100 | typedef std::vector> VecVec2; 101 | typedef std::vector> VecVec3; 102 | typedef std::vector> VecVec2f; 103 | typedef std::vector> VecVec3f; 104 | 105 | // Map of Eigen matrix 106 | typedef std::map, Eigen::aligned_allocator> MapMatXX; 107 | 108 | 109 | 110 | #endif -------------------------------------------------------------------------------- /include/backend/imu_integration.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "eigen_types.h" 4 | #include "../thirdparty/Sophus/sophus/se3.hpp" 5 | 6 | namespace myslam { 7 | namespace backend { 8 | 9 | class IMUIntegration { 10 | public: 11 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 12 | 13 | /** 14 | * constructor, with initial bias a and bias g 15 | * @param ba 16 | * @param bg 17 | */ 18 | explicit IMUIntegration(const Vec3 &ba, const Vec3 &bg) : ba_(ba), bg_(bg) { 19 | const Mat33 i3 = Mat33::Identity(); 20 | noise_measurement_.block<3, 3>(0, 0) = (acc_noise_ * acc_noise_) * i3; 21 | noise_measurement_.block<3, 3>(3, 3) = (gyr_noise_ * gyr_noise_) * i3; 22 | noise_random_walk_.block<3, 3>(0, 0) = (acc_random_walk_ * acc_random_walk_) * i3; 23 | noise_random_walk_.block<3, 3>(3, 3) = (gyr_random_walk_ * gyr_random_walk_) * i3; 24 | } 25 | 26 | ~IMUIntegration() {} 27 | 28 | /** 29 | * propage pre-integrated measurements using raw IMU data 30 | * @param dt 31 | * @param acc 32 | * @param gyr_1 33 | */ 34 | void Propagate(double dt, const Vec3 &acc, const Vec3 &gyr); 35 | 36 | /** 37 | * according to pre-integration, when bias is updated, pre-integration should also be updated using 38 | * first-order expansion of ba and bg 39 | * 40 | * @param delta_ba 41 | * @param delta_bg 42 | */ 43 | void Correct(const Vec3 &delta_ba, const Vec3 &delta_bg); 44 | 45 | void SetBiasG(const Vec3 &bg) { bg_ = bg; } 46 | 47 | void SetBiasA(const Vec3 &ba) { ba_ = ba; } 48 | 49 | /// if bias is update by a large value, redo the propagation 50 | void Repropagate(); 51 | 52 | /// reset measurements 53 | /// NOTE ba and bg will not be reset, only measurements and jacobians will be reset! 54 | void Reset() { 55 | sum_dt_ = 0; 56 | delta_r_ = Sophus::SO3d(); // dR 57 | delta_v_ = Vec3::Zero(); // dv 58 | delta_p_ = Vec3::Zero(); // dp 59 | 60 | // jacobian w.r.t bg and ba 61 | dr_dbg_ = Mat33::Zero(); 62 | dv_dbg_ = Mat33::Zero(); 63 | dv_dba_ = Mat33::Zero(); 64 | dp_dbg_ = Mat33::Zero(); 65 | dp_dba_ = Mat33::Zero(); 66 | 67 | // noise propagation 68 | covariance_measurement_ = Mat99::Zero(); 69 | covariance_random_walk_ = Mat66::Zero(); 70 | A_ = Mat99::Zero(); 71 | B_ = Mat96::Zero(); 72 | } 73 | 74 | /** 75 | * get the jacobians from r,v,p w.r.t. biases 76 | * @param _dr_dbg 77 | * @param _dv_dbg 78 | * @param _dv_dba 79 | * @param _dp_dbg 80 | * @param _dp_dba 81 | */ 82 | void GetJacobians(Mat33 &dr_dbg, Mat33 &dv_dbg, Mat33 &dv_dba, Mat33 &dp_dbg, Mat33 &dp_dba) const { 83 | dr_dbg = dr_dbg_; 84 | dv_dbg = dv_dbg_; 85 | dv_dba = dv_dba_; 86 | dp_dbg = dp_dbg_; 87 | dp_dba = dp_dba_; 88 | } 89 | 90 | Mat33 GetDrDbg() const { return dr_dbg_; } 91 | 92 | /// get propagated noise covariance 93 | Mat99 GetCovarianceMeasurement() const { 94 | return covariance_measurement_; 95 | } 96 | 97 | /// get random walk covariance 98 | Mat66 GetCovarianceRandomWalk() const { 99 | return noise_random_walk_ * sum_dt_; 100 | } 101 | 102 | /// get sum of time 103 | double GetSumDt() const { 104 | return sum_dt_; 105 | } 106 | 107 | /** 108 | * get the integrated measurements 109 | * @param delta_r 110 | * @param delta_v 111 | * @param delta_p 112 | */ 113 | void GetDeltaRVP(Sophus::SO3d &delta_r, Vec3 &delta_v, Vec3 &delta_p) const { 114 | delta_r = delta_r_; 115 | delta_v = delta_v_; 116 | delta_p = delta_p_; 117 | } 118 | 119 | Vec3 GetDv() const { return delta_v_; } 120 | 121 | Vec3 GetDp() const { return delta_p_; } 122 | 123 | Sophus::SO3d GetDr() const { return delta_r_; } 124 | 125 | private: 126 | // raw data from IMU 127 | std::vector dt_buf_; 128 | VecVec3 acc_buf_; 129 | VecVec3 gyr_buf_; 130 | 131 | // pre-integrated IMU measurements 132 | double sum_dt_ = 0; 133 | Sophus::SO3d delta_r_; // dR 134 | Vec3 delta_v_ = Vec3::Zero(); // dv 135 | Vec3 delta_p_ = Vec3::Zero(); // dp 136 | 137 | // gravity, biases 138 | static Vec3 gravity_; 139 | Vec3 bg_ = Vec3::Zero(); // initial bias of gyro 140 | Vec3 ba_ = Vec3::Zero(); // initial bias of accelerator 141 | 142 | // jacobian w.r.t bg and ba 143 | Mat33 dr_dbg_ = Mat33::Zero(); 144 | Mat33 dv_dbg_ = Mat33::Zero(); 145 | Mat33 dv_dba_ = Mat33::Zero(); 146 | Mat33 dp_dbg_ = Mat33::Zero(); 147 | Mat33 dp_dba_ = Mat33::Zero(); 148 | 149 | // noise propagation 150 | Mat99 covariance_measurement_ = Mat99::Zero(); 151 | Mat66 covariance_random_walk_ = Mat66::Zero(); 152 | Mat99 A_ = Mat99::Zero(); 153 | Mat96 B_ = Mat96::Zero(); 154 | 155 | // raw noise of imu measurement 156 | Mat66 noise_measurement_ = Mat66::Identity(); 157 | Mat66 noise_random_walk_ = Mat66::Identity(); 158 | 159 | /**@brief accelerometer measurement noise standard deviation*/ 160 | constexpr static double acc_noise_ = 0.2; 161 | /**@brief gyroscope measurement noise standard deviation*/ 162 | constexpr static double gyr_noise_ = 0.02; 163 | /**@brief accelerometer bias random walk noise standard deviation*/ 164 | constexpr static double acc_random_walk_ = 0.0002; 165 | /**@brief gyroscope bias random walk noise standard deviation*/ 166 | constexpr static double gyr_random_walk_ = 2.0e-5; 167 | }; 168 | 169 | } 170 | } 171 | 172 | -------------------------------------------------------------------------------- /include/backend/loss_function.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by gaoxiang19 on 11/10/18. 3 | // 4 | 5 | #ifndef MYSLAM_LOSS_FUNCTION_H 6 | #define MYSLAM_LOSS_FUNCTION_H 7 | 8 | #include "eigen_types.h" 9 | 10 | namespace myslam { 11 | namespace backend { 12 | 13 | /** 14 | * compute the scaling factor for a error: 15 | * The error is e^T Omega e 16 | * The output rho is 17 | * rho[0]: The actual scaled error value 18 | * rho[1]: First derivative of the scaling function 19 | * rho[2]: Second derivative of the scaling function 20 | * 21 | * LossFunction is a bese class, variaus other loss function could be derived from this base class 22 | */ 23 | class LossFunction { 24 | public: 25 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 26 | 27 | virtual ~LossFunction() {} 28 | 29 | // virtual double Compute(double error) const = 0; 30 | virtual void Compute(double err2, Eigen::Vector3d& rho) const = 0; 31 | }; 32 | 33 | /** 34 | * 平凡的Loss，with nothing to do 35 | * use nullptr as loss function has the same effect 36 | * 37 | * TrivalLoss(e) = e^2 38 | */ 39 | class TrivalLoss : public LossFunction { 40 | public: 41 | virtual void Compute(double err2, Eigen::Vector3d& rho) const override 42 | { 43 | // TODO:: whether multiply 1/2 44 | rho[0] = err2; 45 | rho[1] = 1; 46 | rho[2] = 0; 47 | } 48 | }; 49 | 50 | /** 51 | * Huber loss 52 | * 53 | * Huber(e) = e^2 if e <= delta 54 | * huber(e) = delta*(2*e - delta) if e > delta 55 | */ 56 | class HuberLoss : public LossFunction { 57 | public: 58 | explicit HuberLoss(double delta) : delta_(delta) {} 59 | 60 | virtual void Compute(double err2, Eigen::Vector3d& rho) const override; 61 | 62 | private: 63 | double delta_; 64 | 65 | }; 66 | 67 | /* 68 | * Cauchy loss 69 | * 70 | */ 71 | class CauchyLoss : public LossFunction 72 | { 73 | public: 74 | explicit CauchyLoss(double delta) : delta_(delta) {} 75 | 76 | virtual void Compute(double err2, Eigen::Vector3d& rho) const override; 77 | 78 | private: 79 | double delta_; 80 | }; 81 | 82 | class TukeyLoss : public LossFunction 83 | { 84 | public: 85 | explicit TukeyLoss(double delta) : delta_(delta) {} 86 | 87 | virtual void Compute(double err2, Eigen::Vector3d& rho) const override; 88 | 89 | private: 90 | double delta_; 91 | }; 92 | } 93 | } 94 | 95 | #endif //MYSLAM_LOSS_FUNCTION_H 96 | -------------------------------------------------------------------------------- /include/backend/problem.h: -------------------------------------------------------------------------------- 1 | #ifndef MYSLAM_BACKEND_PROBLEM_H 2 | #define MYSLAM_BACKEND_PROBLEM_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include "eigen_types.h" 9 | #include "edge.h" 10 | #include "vertex.h" 11 | 12 | #include 13 | #include 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | typedef unsigned long ulong; 21 | 22 | namespace myslam { 23 | namespace backend { 24 | 25 | typedef unsigned long ulong; 26 | // typedef std::unordered_map> HashVertex; 27 | typedef std::map> HashVertex; 28 | typedef std::unordered_map> HashEdge; 29 | typedef std::unordered_multimap> HashVertexIdToEdge; 30 | 31 | // TODO: HOMEWORKE Multi-thread && SSE 32 | struct ThreadsStruct { 33 | HashEdge sub_edges; 34 | MatXX sub_H; 35 | VecX sub_b; 36 | }; 37 | 38 | class Problem { 39 | public: 40 | 41 | /** 42 | * The type of problem 43 | * The problem like SLAM is still a general problem 44 | * 45 | * If it's a SLAM problem, then pose and landmark are different and Hessian Matrix is sparse 46 | * SLAM only allow sertain Vertex and Edge 47 | * If it's a general problem, then the hessian matrix is dense, if user don't define vertex as marginalized 48 | */ 49 | enum class ProblemType { 50 | SLAM_PROBLEM, 51 | GENERIC_PROBLEM 52 | }; 53 | 54 | /** 55 | * //TODO: HOMEWORK implement Dog-Leg algorithm 56 | * 57 | * \brief type of the step to take 58 | * */ 59 | enum { 60 | STEP_UNDEFINED, 61 | STEP_SD, STEP_GN, STEP_DL 62 | }; 63 | 64 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 65 | 66 | Problem(ProblemType problemType); 67 | 68 | ~Problem(); 69 | 70 | bool AddVertex(std::shared_ptr vertex); 71 | 72 | /** 73 | * remove a vertex 74 | * @param vertex_to_remove 75 | */ 76 | bool RemoveVertex(std::shared_ptr vertex); 77 | 78 | bool AddEdge(std::shared_ptr edge); 79 | 80 | bool RemoveEdge(std::shared_ptr edge); 81 | 82 | /** 83 | * Get the edges, which are defined as outlier during the optimisation, so we can remove those outliers 84 | * @param outlier_edges 85 | */ 86 | void GetOutlierEdges(std::vector> &outlier_edges); 87 | 88 | /** 89 | * Solve the problem 90 | * @param iterations 91 | * @return 92 | */ 93 | bool Solve(int iterations = 10); 94 | 95 | /// Marginalize a frame and landmark, whose host is exactly this frame 96 | bool Marginalize(std::shared_ptr frameVertex, 97 | const std::vector> &landmarkVerticies); 98 | 99 | bool Marginalize(const std::shared_ptr frameVertex); 100 | bool Marginalize(const std::vector > frameVertex,int pose_dim); 101 | 102 | MatXX GetHessianPrior(){ return H_prior_;} 103 | VecX GetbPrior(){ return b_prior_;} 104 | VecX GetErrPrior(){ return err_prior_;} 105 | MatXX GetJtPrior(){ return Jt_prior_inv_;} 106 | 107 | void SetHessianPrior(const MatXX& H){H_prior_ = H;} 108 | void SetbPrior(const VecX& b){b_prior_ = b;} 109 | void SetErrPrior(const VecX& b){err_prior_ = b;} 110 | void SetJtPrior(const MatXX& J){Jt_prior_inv_ = J;} 111 | 112 | void ExtendHessiansPriorSize(int dim); 113 | 114 | //test compute prior 115 | void TestComputePrior(); 116 | 117 | // TODO: HOMEWORK store the lm's computation cost in txt 118 | double comput_time = 0.0; 119 | private: 120 | 121 | /// Implement the Solve function for general problem 122 | bool SolveGenericProblem(int iterations); 123 | 124 | /// Implement the Solve function for SLAM problem 125 | bool SolveSLAMProblem(int iterations); 126 | 127 | /// Set the contex's ordering_index 128 | void SetOrdering(); 129 | 130 | /// set ordering for new vertex in slam problem 131 | void AddOrderingSLAM(std::shared_ptr v); 132 | 133 | /// Construct the big Hessian Matirx 134 | void MakeHessian(); 135 | 136 | /// Solve the SBA by using schur complement 137 | void SchurSBA(); 138 | 139 | /// Solve linear equation 140 | void SolveLinearSystem(); 141 | 142 | /// Update the states 143 | void UpdateStates(); 144 | 145 | void RollbackStates(); // When the residual increase after the update, redo 146 | 147 | /// Compute and update the Prior 148 | void ComputePrior(); 149 | 150 | /// Determine ob the contex is Pose contex 151 | bool IsPoseVertex(std::shared_ptr v); 152 | 153 | /// Determine ob the contex is landmark contex 154 | bool IsLandmarkVertex(std::shared_ptr v); 155 | 156 | /// After input contex, we must change the dimention of hessian matrix 157 | void ResizePoseHessiansWhenAddingPose(std::shared_ptr v); 158 | 159 | /// Check whether the ordering is right 160 | bool CheckOrdering(); 161 | 162 | void LogoutVectorSize(); 163 | 164 | /// Get the edge, which connects the contex 165 | std::vector> GetConnectedEdges(std::shared_ptr vertex); 166 | 167 | /// Levenberg 168 | /// Compute the initial Lambda 169 | void ComputeLambdaInitLM(); 170 | 171 | /// The diagonal of Hessian matrix add/subtract Lambda 172 | void AddLambdatoHessianLM(); 173 | 174 | void RemoveLambdaHessianLM(); 175 | 176 | /// LM 算法中用于判断 Lambda 在上次迭代中是否可以，以及Lambda怎么缩放 177 | bool IsGoodStepInLM(); 178 | 179 | /// PCG 迭代线性求解器 180 | VecX PCGSolver(const MatXX &A, const VecX &b, int maxIter); 181 | 182 | double currentLambda_; 183 | double currentChi_; 184 | double stopThresholdLM_; // LM 迭代退出阈值条件 185 | double ni_; //控制 Lambda 缩放大小 186 | 187 | ProblemType problemType_; 188 | 189 | /// Entire information matrix 190 | MatXX Hessian_; 191 | VecX b_; 192 | VecX delta_x_; 193 | 194 | /// The prior part 195 | MatXX H_prior_; 196 | VecX b_prior_; 197 | VecX b_prior_backup_; 198 | VecX err_prior_backup_; 199 | 200 | MatXX Jt_prior_inv_; 201 | VecX err_prior_; 202 | 203 | /// SBA's Pose 204 | MatXX H_pp_schur_; 205 | VecX b_pp_schur_; 206 | // Heesian's Landmark and pose part 207 | MatXX H_pp_; 208 | VecX b_pp_; 209 | MatXX H_ll_; 210 | VecX b_ll_; 211 | 212 | /// all vertices 213 | HashVertex verticies_; 214 | 215 | /// all edges 216 | HashEdge edges_; 217 | 218 | /// through vertex id search the edge 219 | HashVertexIdToEdge vertexToEdge_; 220 | 221 | /// Ordering related 222 | ulong ordering_poses_ = 0; 223 | ulong ordering_landmarks_ = 0; 224 | ulong ordering_generic_ = 0; 225 | std::map> idx_pose_vertices_; // based on ordering, pose顶点 226 | std::map> idx_landmark_vertices_; // based on ordering, landmark顶点 227 | 228 | // verticies need to marg. 229 | HashVertex verticies_marg_; 230 | 231 | bool bDebug = false; 232 | double t_hessian_cost_ = 0.0; 233 | double t_PCGsovle_cost_ = 0.0; 234 | 235 | // TODO: HOMEWORK Dog-Leg algorithm 236 | VecX _auxVector; ///< auxilary vector used to perform multiplications or other stuff 237 | double _normJG; 238 | double _normG; 239 | 240 | VecX hsd; ///< steepest decent step 241 | VecX hgn; ///< gaussian newton step 242 | VecX hdl; ///< final dogleg step 243 | 244 | double _delta = 1.0; //100 ///< trust region 245 | double alpha; 246 | double beta; 247 | int _lastStep; ///< type of the step taken by the algorithm 248 | bool stop; 249 | }; 250 | 251 | } 252 | } 253 | 254 | #endif 255 | -------------------------------------------------------------------------------- /include/backend/vertex.h: -------------------------------------------------------------------------------- 1 | #ifndef MYSLAM_BACKEND_VERTEX_H 2 | #define MYSLAM_BACKEND_VERTEX_H 3 | 4 | #include "eigen_types.h" 5 | 6 | namespace myslam { 7 | namespace backend { 8 | extern unsigned long global_vertex_id; 9 | /** 10 | * @brief vertex corresponds to parameter block 11 | * the variable is stored as VecX，the dimention should be defined with constructor 12 | */ 13 | class Vertex { 14 | public: 15 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 16 | 17 | /** 18 | * Constructor 19 | * @param num_dimension 20 | * @param local_dimension，if it's -1, then it has the same dimention as the local dimention 21 | */ 22 | explicit Vertex(int num_dimension, int local_dimension = -1); 23 | 24 | virtual ~Vertex(); 25 | 26 | /// Return the dimention of the variable 27 | int Dimension() const; 28 | 29 | /// Return the dimention of the local variable 30 | int LocalDimension() const; 31 | 32 | /// Return the id of this contex 33 | unsigned long Id() const { return id_; } 34 | 35 | /// Retrun the parameters 36 | VecX Parameters() const { return parameters_; } 37 | 38 | /// Return the reference of the parameters 39 | VecX &Parameters() { return parameters_; } 40 | 41 | /// Set the parameters 42 | void SetParameters(const VecX ¶ms) { parameters_ = params; } 43 | 44 | // Backup and reset the parameters, it's useful at the time when the estimation is bad 45 | void BackUpParameters() { parameters_backup_ = parameters_; } 46 | void RollBackParameters() { parameters_ = parameters_backup_; } 47 | 48 | /// Addition，can be overrided 49 | /// default is vedtor addition 50 | virtual void Plus(const VecX &delta); 51 | 52 | /// Return the derived class's name, it can be implement in the derived class 53 | virtual std::string TypeInfo() const = 0; 54 | 55 | int OrderingId() const { return ordering_id_; } 56 | 57 | void SetOrderingId(unsigned long id) { ordering_id_ = id; }; 58 | 59 | /// Fix the estimator 60 | void SetFixed(bool fixed = true) { 61 | fixed_ = fixed; 62 | } 63 | 64 | /// Check whether this point is fixed 65 | bool IsFixed() const { return fixed_; } 66 | 67 | protected: 68 | VecX parameters_; // current variable 69 | VecX parameters_backup_; // back up the parameters at every step of iteration，用于回滚 70 | int local_dimension_; // dimention of local parameters 71 | unsigned long id_; // contex's id 72 | 73 | /// ordering id是在problem中排序后的id，用于寻找雅可比对应块 74 | /// ordering id带有维度信息，例如ordering_id=6则对应Hessian中的第6列 75 | /// start from 0 76 | unsigned long ordering_id_ = 0; 77 | 78 | bool fixed_ = false; // ob it's fixed 79 | }; 80 | 81 | } 82 | } 83 | 84 | #endif 85 | -------------------------------------------------------------------------------- /include/backend/vertex_inverse_depth.h: -------------------------------------------------------------------------------- 1 | #ifndef MYSLAM_BACKEND_INVERSE_DEPTH_H 2 | #define MYSLAM_BACKEND_INVERSE_DEPTH_H 3 | 4 | #include "vertex.h" 5 | 6 | namespace myslam { 7 | namespace backend { 8 | 9 | /** 10 | * Use the inverse depth to store the contex 11 | */ 12 | class VertexInverseDepth : public Vertex { 13 | public: 14 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 15 | 16 | VertexInverseDepth() : Vertex(1) {} 17 | 18 | virtual std::string TypeInfo() const { return "VertexInverseDepth"; } 19 | }; 20 | 21 | } 22 | } 23 | 24 | #endif 25 | -------------------------------------------------------------------------------- /include/backend/vertex_point_xyz.h: -------------------------------------------------------------------------------- 1 | #ifndef MYSLAM_BACKEND_POINTVERTEX_H 2 | #define MYSLAM_BACKEND_POINTVERTEX_H 3 | 4 | #include "vertex.h" 5 | 6 | namespace myslam { 7 | namespace backend { 8 | 9 | /** 10 | * @brief Use the xyz format to paramiterise the contex 11 | */ 12 | class VertexPointXYZ : public Vertex { 13 | public: 14 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 15 | 16 | VertexPointXYZ() : Vertex(3) {} 17 | 18 | std::string TypeInfo() const { return "VertexPointXYZ"; } 19 | }; 20 | 21 | } 22 | } 23 | 24 | #endif 25 | -------------------------------------------------------------------------------- /include/backend/vertex_pose.h: -------------------------------------------------------------------------------- 1 | #ifndef MYSLAM_BACKEND_POSEVERTEX_H 2 | #define MYSLAM_BACKEND_POSEVERTEX_H 3 | 4 | #include 5 | #include "vertex.h" 6 | 7 | namespace myslam { 8 | namespace backend { 9 | 10 | /** 11 | * Pose vertex 12 | * parameters: tx, ty, tz, qx, qy, qz, qw, 7 DoF 13 | * optimization is perform on manifold, so update is 6 DoF, left multiplication 14 | * 15 | * pose is represented as Twb in VIO case 16 | */ 17 | class VertexPose : public Vertex { 18 | public: 19 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 20 | 21 | VertexPose() : Vertex(7, 6) {} 22 | 23 | /// Addition, can be overrided 24 | /// default is vector addition 25 | virtual void Plus(const VecX &delta) override; 26 | 27 | std::string TypeInfo() const { 28 | return "VertexPose"; 29 | } 30 | 31 | /** 32 | * need to maintain [H|b] matrix's type like following data structur 33 | * p: pose, m:mappoint 34 | * 35 | * Hp1_p2 36 | * Hp2_p2 Hp2_m1 Hp2_m2 Hp2_m3 | bp2 37 | * 38 | * Hm2_m2 | bm2 39 | * Hm2_m3 | bm3 40 | * 1. If this Camera is source camera，then maintain the vHessionSourceCamera； 41 | * 2. If this Camera is measurement camera, then maintain the vHessionMeasurementCamera； 42 | * 3. always maitain m_HessionDiagonal； 43 | */ 44 | }; 45 | 46 | } 47 | } 48 | 49 | #endif 50 | -------------------------------------------------------------------------------- /include/backend/vertex_speedbias.h: -------------------------------------------------------------------------------- 1 | #ifndef MYSLAM_BACKEND_SPEEDBIASVERTEX_H 2 | #define MYSLAM_BACKEND_SPEEDBIASVERTEX_H 3 | 4 | #include 5 | #include "vertex.h" 6 | 7 | namespace myslam { 8 | namespace backend { 9 | 10 | /** 11 | * SpeedBias vertex 12 | * parameters: v, ba, bg 9 DoF 13 | * 14 | */ 15 | class VertexSpeedBias : public Vertex { 16 | public: 17 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 18 | 19 | VertexSpeedBias() : Vertex(9) {} 20 | 21 | std::string TypeInfo() const { 22 | return "VertexSpeedBias"; 23 | } 24 | 25 | }; 26 | 27 | } 28 | } 29 | 30 | #endif 31 | -------------------------------------------------------------------------------- /include/camodocal/calib/CameraCalibration.h: -------------------------------------------------------------------------------- 1 | #ifndef CAMERACALIBRATION_H 2 | #define CAMERACALIBRATION_H 3 | 4 | #include 5 | 6 | #include "camodocal/camera_models/Camera.h" 7 | 8 | namespace camodocal 9 | { 10 | 11 | class CameraCalibration 12 | { 13 | public: 14 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 15 | CameraCalibration(); 16 | 17 | CameraCalibration(Camera::ModelType modelType, 18 | const std::string& cameraName, 19 | const cv::Size& imageSize, 20 | const cv::Size& boardSize, 21 | float squareSize); 22 | 23 | void clear(void); 24 | 25 | void addChessboardData(const std::vector& corners); 26 | 27 | bool calibrate(void); 28 | 29 | int sampleCount(void) const; 30 | std::vector >& imagePoints(void); 31 | const std::vector >& imagePoints(void) const; 32 | std::vector >& scenePoints(void); 33 | const std::vector >& scenePoints(void) const; 34 | CameraPtr& camera(void); 35 | const CameraConstPtr camera(void) const; 36 | 37 | Eigen::Matrix2d& measurementCovariance(void); 38 | const Eigen::Matrix2d& measurementCovariance(void) const; 39 | 40 | cv::Mat& cameraPoses(void); 41 | const cv::Mat& cameraPoses(void) const; 42 | 43 | void drawResults(std::vector& images) const; 44 | 45 | void writeParams(const std::string& filename) const; 46 | 47 | bool writeChessboardData(const std::string& filename) const; 48 | bool readChessboardData(const std::string& filename); 49 | 50 | void setVerbose(bool verbose); 51 | 52 | private: 53 | bool calibrateHelper(CameraPtr& camera, 54 | std::vector& rvecs, std::vector& tvecs) const; 55 | 56 | void optimize(CameraPtr& camera, 57 | std::vector& rvecs, std::vector& tvecs) const; 58 | 59 | template 60 | void readData(std::ifstream& ifs, T& data) const; 61 | 62 | template 63 | void writeData(std::ofstream& ofs, T data) const; 64 | 65 | cv::Size m_boardSize; 66 | float m_squareSize; 67 | 68 | CameraPtr m_camera; 69 | cv::Mat m_cameraPoses; 70 | 71 | std::vector > m_imagePoints; 72 | std::vector > m_scenePoints; 73 | 74 | Eigen::Matrix2d m_measurementCovariance; 75 | 76 | bool m_verbose; 77 | }; 78 | 79 | } 80 | 81 | #endif 82 | -------------------------------------------------------------------------------- /include/camodocal/camera_models/Camera.h: -------------------------------------------------------------------------------- 1 | #ifndef CAMERA_H 2 | #define CAMERA_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | namespace camodocal 10 | { 11 | 12 | class Camera 13 | { 14 | public: 15 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 16 | enum ModelType 17 | { 18 | KANNALA_BRANDT, 19 | MEI, 20 | PINHOLE, 21 | SCARAMUZZA 22 | }; 23 | 24 | class Parameters 25 | { 26 | public: 27 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 28 | Parameters(ModelType modelType); 29 | 30 | Parameters(ModelType modelType, const std::string& cameraName, 31 | int w, int h); 32 | 33 | ModelType& modelType(void); 34 | std::string& cameraName(void); 35 | int& imageWidth(void); 36 | int& imageHeight(void); 37 | 38 | ModelType modelType(void) const; 39 | const std::string& cameraName(void) const; 40 | int imageWidth(void) const; 41 | int imageHeight(void) const; 42 | 43 | int nIntrinsics(void) const; 44 | 45 | virtual bool readFromYamlFile(const std::string& filename) = 0; 46 | virtual void writeToYamlFile(const std::string& filename) const = 0; 47 | 48 | protected: 49 | ModelType m_modelType; 50 | int m_nIntrinsics; 51 | std::string m_cameraName; 52 | int m_imageWidth; 53 | int m_imageHeight; 54 | }; 55 | 56 | virtual ModelType modelType(void) const = 0; 57 | virtual const std::string& cameraName(void) const = 0; 58 | virtual int imageWidth(void) const = 0; 59 | virtual int imageHeight(void) const = 0; 60 | 61 | virtual cv::Mat& mask(void); 62 | virtual const cv::Mat& mask(void) const; 63 | 64 | virtual void estimateIntrinsics(const cv::Size& boardSize, 65 | const std::vector< std::vector >& objectPoints, 66 | const std::vector< std::vector >& imagePoints) = 0; 67 | virtual void estimateExtrinsics(const std::vector& objectPoints, 68 | const std::vector& imagePoints, 69 | cv::Mat& rvec, cv::Mat& tvec) const; 70 | 71 | // Lift points from the image plane to the sphere 72 | virtual void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const = 0; 73 | //%output P 74 | 75 | // Lift points from the image plane to the projective space 76 | virtual void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const = 0; 77 | //%output P 78 | 79 | // Projects 3D points to the image plane (Pi function) 80 | virtual void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const = 0; 81 | //%output p 82 | 83 | // Projects 3D points to the image plane (Pi function) 84 | // and calculates jacobian 85 | //virtual void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p, 86 | // Eigen::Matrix& J) const = 0; 87 | //%output p 88 | //%output J 89 | 90 | virtual void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const = 0; 91 | //%output p 92 | 93 | //virtual void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0) const = 0; 94 | virtual cv::Mat initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2, 95 | float fx = -1.0f, float fy = -1.0f, 96 | cv::Size imageSize = cv::Size(0, 0), 97 | float cx = -1.0f, float cy = -1.0f, 98 | cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const = 0; 99 | 100 | virtual int parameterCount(void) const = 0; 101 | 102 | virtual void readParameters(const std::vector& parameters) = 0; 103 | virtual void writeParameters(std::vector& parameters) const = 0; 104 | 105 | virtual void writeParametersToYamlFile(const std::string& filename) const = 0; 106 | 107 | virtual std::string parametersToString(void) const = 0; 108 | 109 | /** 110 | * \brief Calculates the reprojection distance between points 111 | * 112 | * \param P1 first 3D point coordinates 113 | * \param P2 second 3D point coordinates 114 | * \return euclidean distance in the plane 115 | */ 116 | double reprojectionDist(const Eigen::Vector3d& P1, const Eigen::Vector3d& P2) const; 117 | 118 | double reprojectionError(const std::vector< std::vector >& objectPoints, 119 | const std::vector< std::vector >& imagePoints, 120 | const std::vector& rvecs, 121 | const std::vector& tvecs, 122 | cv::OutputArray perViewErrors = cv::noArray()) const; 123 | 124 | double reprojectionError(const Eigen::Vector3d& P, 125 | const Eigen::Quaterniond& camera_q, 126 | const Eigen::Vector3d& camera_t, 127 | const Eigen::Vector2d& observed_p) const; 128 | 129 | void projectPoints(const std::vector& objectPoints, 130 | const cv::Mat& rvec, 131 | const cv::Mat& tvec, 132 | std::vector& imagePoints) const; 133 | protected: 134 | cv::Mat m_mask; 135 | }; 136 | 137 | typedef boost::shared_ptr CameraPtr; 138 | typedef boost::shared_ptr CameraConstPtr; 139 | 140 | } 141 | 142 | #endif 143 | -------------------------------------------------------------------------------- /include/camodocal/camera_models/CameraFactory.h: -------------------------------------------------------------------------------- 1 | #ifndef CAMERAFACTORY_H 2 | #define CAMERAFACTORY_H 3 | 4 | #include 5 | #include 6 | 7 | #include "camodocal/camera_models/Camera.h" 8 | 9 | namespace camodocal 10 | { 11 | 12 | class CameraFactory 13 | { 14 | public: 15 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 16 | CameraFactory(); 17 | 18 | static boost::shared_ptr instance(void); 19 | 20 | CameraPtr generateCamera(Camera::ModelType modelType, 21 | const std::string& cameraName, 22 | cv::Size imageSize) const; 23 | 24 | CameraPtr generateCameraFromYamlFile(const std::string& filename); 25 | 26 | private: 27 | static boost::shared_ptr m_instance; 28 | }; 29 | 30 | } 31 | 32 | #endif 33 | -------------------------------------------------------------------------------- /include/camodocal/camera_models/CataCamera.h: -------------------------------------------------------------------------------- 1 | #ifndef CATACAMERA_H 2 | #define CATACAMERA_H 3 | 4 | #include 5 | #include 6 | 7 | #include "ceres/rotation.h" 8 | #include "Camera.h" 9 | 10 | namespace camodocal 11 | { 12 | 13 | /** 14 | * C. Mei, and P. Rives, Single View Point Omnidirectional Camera Calibration 15 | * from Planar Grids, ICRA 2007 16 | */ 17 | 18 | class CataCamera: public Camera 19 | { 20 | public: 21 | class Parameters: public Camera::Parameters 22 | { 23 | public: 24 | Parameters(); 25 | Parameters(const std::string& cameraName, 26 | int w, int h, 27 | double xi, 28 | double k1, double k2, double p1, double p2, 29 | double gamma1, double gamma2, double u0, double v0); 30 | 31 | double& xi(void); 32 | double& k1(void); 33 | double& k2(void); 34 | double& p1(void); 35 | double& p2(void); 36 | double& gamma1(void); 37 | double& gamma2(void); 38 | double& u0(void); 39 | double& v0(void); 40 | 41 | double xi(void) const; 42 | double k1(void) const; 43 | double k2(void) const; 44 | double p1(void) const; 45 | double p2(void) const; 46 | double gamma1(void) const; 47 | double gamma2(void) const; 48 | double u0(void) const; 49 | double v0(void) const; 50 | 51 | bool readFromYamlFile(const std::string& filename); 52 | void writeToYamlFile(const std::string& filename) const; 53 | 54 | Parameters& operator=(const Parameters& other); 55 | friend std::ostream& operator<< (std::ostream& out, const Parameters& params); 56 | 57 | private: 58 | double m_xi; 59 | double m_k1; 60 | double m_k2; 61 | double m_p1; 62 | double m_p2; 63 | double m_gamma1; 64 | double m_gamma2; 65 | double m_u0; 66 | double m_v0; 67 | }; 68 | 69 | CataCamera(); 70 | 71 | /** 72 | * \brief Constructor from the projection model parameters 73 | */ 74 | CataCamera(const std::string& cameraName, 75 | int imageWidth, int imageHeight, 76 | double xi, double k1, double k2, double p1, double p2, 77 | double gamma1, double gamma2, double u0, double v0); 78 | /** 79 | * \brief Constructor from the projection model parameters 80 | */ 81 | CataCamera(const Parameters& params); 82 | 83 | Camera::ModelType modelType(void) const; 84 | const std::string& cameraName(void) const; 85 | int imageWidth(void) const; 86 | int imageHeight(void) const; 87 | 88 | void estimateIntrinsics(const cv::Size& boardSize, 89 | const std::vector< std::vector >& objectPoints, 90 | const std::vector< std::vector >& imagePoints); 91 | 92 | // Lift points from the image plane to the sphere 93 | void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const; 94 | //%output P 95 | 96 | // Lift points from the image plane to the projective space 97 | void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const; 98 | //%output P 99 | 100 | // Projects 3D points to the image plane (Pi function) 101 | void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const; 102 | //%output p 103 | 104 | // Projects 3D points to the image plane (Pi function) 105 | // and calculates jacobian 106 | void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p, 107 | Eigen::Matrix& J) const; 108 | //%output p 109 | //%output J 110 | 111 | void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const; 112 | //%output p 113 | 114 | template 115 | static void spaceToPlane(const T* const params, 116 | const T* const q, const T* const t, 117 | const Eigen::Matrix& P, 118 | Eigen::Matrix& p); 119 | 120 | void distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u) const; 121 | void distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u, 122 | Eigen::Matrix2d& J) const; 123 | 124 | void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0) const; 125 | cv::Mat initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2, 126 | float fx = -1.0f, float fy = -1.0f, 127 | cv::Size imageSize = cv::Size(0, 0), 128 | float cx = -1.0f, float cy = -1.0f, 129 | cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const; 130 | 131 | int parameterCount(void) const; 132 | 133 | const Parameters& getParameters(void) const; 134 | void setParameters(const Parameters& parameters); 135 | 136 | void readParameters(const std::vector& parameterVec); 137 | void writeParameters(std::vector& parameterVec) const; 138 | 139 | void writeParametersToYamlFile(const std::string& filename) const; 140 | 141 | std::string parametersToString(void) const; 142 | 143 | private: 144 | Parameters mParameters; 145 | 146 | double m_inv_K11, m_inv_K13, m_inv_K22, m_inv_K23; 147 | bool m_noDistortion; 148 | }; 149 | 150 | typedef boost::shared_ptr CataCameraPtr; 151 | typedef boost::shared_ptr CataCameraConstPtr; 152 | 153 | template 154 | void 155 | CataCamera::spaceToPlane(const T* const params, 156 | const T* const q, const T* const t, 157 | const Eigen::Matrix& P, 158 | Eigen::Matrix& p) 159 | { 160 | T P_w[3]; 161 | P_w[0] = T(P(0)); 162 | P_w[1] = T(P(1)); 163 | P_w[2] = T(P(2)); 164 | 165 | // Convert quaternion from Eigen convention (x, y, z, w) 166 | // to Ceres convention (w, x, y, z) 167 | T q_ceres[4] = {q[3], q[0], q[1], q[2]}; 168 | 169 | T P_c[3]; 170 | ceres::QuaternionRotatePoint(q_ceres, P_w, P_c); 171 | 172 | P_c[0] += t[0]; 173 | P_c[1] += t[1]; 174 | P_c[2] += t[2]; 175 | 176 | // project 3D object point to the image plane 177 | T xi = params[0]; 178 | T k1 = params[1]; 179 | T k2 = params[2]; 180 | T p1 = params[3]; 181 | T p2 = params[4]; 182 | T gamma1 = params[5]; 183 | T gamma2 = params[6]; 184 | T alpha = T(0); //cameraParams.alpha(); 185 | T u0 = params[7]; 186 | T v0 = params[8]; 187 | 188 | // Transform to model plane 189 | T len = sqrt(P_c[0] * P_c[0] + P_c[1] * P_c[1] + P_c[2] * P_c[2]); 190 | P_c[0] /= len; 191 | P_c[1] /= len; 192 | P_c[2] /= len; 193 | 194 | T u = P_c[0] / (P_c[2] + xi); 195 | T v = P_c[1] / (P_c[2] + xi); 196 | 197 | T rho_sqr = u * u + v * v; 198 | T L = T(1.0) + k1 * rho_sqr + k2 * rho_sqr * rho_sqr; 199 | T du = T(2.0) * p1 * u * v + p2 * (rho_sqr + T(2.0) * u * u); 200 | T dv = p1 * (rho_sqr + T(2.0) * v * v) + T(2.0) * p2 * u * v; 201 | 202 | u = L * u + du; 203 | v = L * v + dv; 204 | p(0) = gamma1 * (u + alpha * v) + u0; 205 | p(1) = gamma2 * v + v0; 206 | } 207 | 208 | } 209 | 210 | #endif 211 | -------------------------------------------------------------------------------- /include/camodocal/camera_models/CostFunctionFactory.h: -------------------------------------------------------------------------------- 1 | #ifndef COSTFUNCTIONFACTORY_H 2 | #define COSTFUNCTIONFACTORY_H 3 | 4 | #include 5 | #include 6 | 7 | #include "camodocal/camera_models/Camera.h" 8 | 9 | namespace ceres 10 | { 11 | class CostFunction; 12 | } 13 | 14 | namespace camodocal 15 | { 16 | 17 | enum 18 | { 19 | CAMERA_INTRINSICS = 1 << 0, 20 | CAMERA_POSE = 1 << 1, 21 | POINT_3D = 1 << 2, 22 | ODOMETRY_INTRINSICS = 1 << 3, 23 | ODOMETRY_3D_POSE = 1 << 4, 24 | ODOMETRY_6D_POSE = 1 << 5, 25 | CAMERA_ODOMETRY_TRANSFORM = 1 << 6 26 | }; 27 | 28 | class CostFunctionFactory 29 | { 30 | public: 31 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 32 | CostFunctionFactory(); 33 | 34 | static boost::shared_ptr instance(void); 35 | 36 | ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera, 37 | const Eigen::Vector3d& observed_P, 38 | const Eigen::Vector2d& observed_p, 39 | int flags) const; 40 | 41 | ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera, 42 | const Eigen::Vector3d& observed_P, 43 | const Eigen::Vector2d& observed_p, 44 | const Eigen::Matrix2d& sqrtPrecisionMat, 45 | int flags) const; 46 | 47 | ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera, 48 | const Eigen::Vector2d& observed_p, 49 | int flags, bool optimize_cam_odo_z = true) const; 50 | 51 | ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera, 52 | const Eigen::Vector2d& observed_p, 53 | const Eigen::Matrix2d& sqrtPrecisionMat, 54 | int flags, bool optimize_cam_odo_z = true) const; 55 | 56 | ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera, 57 | const Eigen::Vector3d& odo_pos, 58 | const Eigen::Vector3d& odo_att, 59 | const Eigen::Vector2d& observed_p, 60 | int flags, bool optimize_cam_odo_z = true) const; 61 | 62 | ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera, 63 | const Eigen::Quaterniond& cam_odo_q, 64 | const Eigen::Vector3d& cam_odo_t, 65 | const Eigen::Vector3d& odo_pos, 66 | const Eigen::Vector3d& odo_att, 67 | const Eigen::Vector2d& observed_p, 68 | int flags) const; 69 | 70 | ceres::CostFunction* generateCostFunction(const CameraConstPtr& cameraLeft, 71 | const CameraConstPtr& cameraRight, 72 | const Eigen::Vector3d& observed_P, 73 | const Eigen::Vector2d& observed_p_left, 74 | const Eigen::Vector2d& observed_p_right) const; 75 | 76 | private: 77 | static boost::shared_ptr m_instance; 78 | }; 79 | 80 | } 81 | 82 | #endif 83 | -------------------------------------------------------------------------------- /include/camodocal/camera_models/EquidistantCamera.h: -------------------------------------------------------------------------------- 1 | #ifndef EQUIDISTANTCAMERA_H 2 | #define EQUIDISTANTCAMERA_H 3 | 4 | #include 5 | #include 6 | 7 | #include "ceres/rotation.h" 8 | #include "Camera.h" 9 | 10 | namespace camodocal 11 | { 12 | 13 | /** 14 | * J. Kannala, and S. Brandt, A Generic Camera Model and Calibration Method 15 | * for Conventional, Wide-Angle, and Fish-Eye Lenses, PAMI 2006 16 | */ 17 | 18 | class EquidistantCamera: public Camera 19 | { 20 | public: 21 | class Parameters: public Camera::Parameters 22 | { 23 | public: 24 | Parameters(); 25 | Parameters(const std::string& cameraName, 26 | int w, int h, 27 | double k2, double k3, double k4, double k5, 28 | double mu, double mv, 29 | double u0, double v0); 30 | 31 | double& k2(void); 32 | double& k3(void); 33 | double& k4(void); 34 | double& k5(void); 35 | double& mu(void); 36 | double& mv(void); 37 | double& u0(void); 38 | double& v0(void); 39 | 40 | double k2(void) const; 41 | double k3(void) const; 42 | double k4(void) const; 43 | double k5(void) const; 44 | double mu(void) const; 45 | double mv(void) const; 46 | double u0(void) const; 47 | double v0(void) const; 48 | 49 | bool readFromYamlFile(const std::string& filename); 50 | void writeToYamlFile(const std::string& filename) const; 51 | 52 | Parameters& operator=(const Parameters& other); 53 | friend std::ostream& operator<< (std::ostream& out, const Parameters& params); 54 | 55 | private: 56 | // projection 57 | double m_k2; 58 | double m_k3; 59 | double m_k4; 60 | double m_k5; 61 | 62 | double m_mu; 63 | double m_mv; 64 | double m_u0; 65 | double m_v0; 66 | }; 67 | 68 | EquidistantCamera(); 69 | 70 | /** 71 | * \brief Constructor from the projection model parameters 72 | */ 73 | EquidistantCamera(const std::string& cameraName, 74 | int imageWidth, int imageHeight, 75 | double k2, double k3, double k4, double k5, 76 | double mu, double mv, 77 | double u0, double v0); 78 | /** 79 | * \brief Constructor from the projection model parameters 80 | */ 81 | EquidistantCamera(const Parameters& params); 82 | 83 | Camera::ModelType modelType(void) const; 84 | const std::string& cameraName(void) const; 85 | int imageWidth(void) const; 86 | int imageHeight(void) const; 87 | 88 | void estimateIntrinsics(const cv::Size& boardSize, 89 | const std::vector< std::vector >& objectPoints, 90 | const std::vector< std::vector >& imagePoints); 91 | 92 | // Lift points from the image plane to the sphere 93 | virtual void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const; 94 | //%output P 95 | 96 | // Lift points from the image plane to the projective space 97 | void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const; 98 | //%output P 99 | 100 | // Projects 3D points to the image plane (Pi function) 101 | void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const; 102 | //%output p 103 | 104 | // Projects 3D points to the image plane (Pi function) 105 | // and calculates jacobian 106 | void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p, 107 | Eigen::Matrix& J) const; 108 | //%output p 109 | //%output J 110 | 111 | void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const; 112 | //%output p 113 | 114 | template 115 | static void spaceToPlane(const T* const params, 116 | const T* const q, const T* const t, 117 | const Eigen::Matrix& P, 118 | Eigen::Matrix& p); 119 | 120 | void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0) const; 121 | cv::Mat initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2, 122 | float fx = -1.0f, float fy = -1.0f, 123 | cv::Size imageSize = cv::Size(0, 0), 124 | float cx = -1.0f, float cy = -1.0f, 125 | cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const; 126 | 127 | int parameterCount(void) const; 128 | 129 | const Parameters& getParameters(void) const; 130 | void setParameters(const Parameters& parameters); 131 | 132 | void readParameters(const std::vector& parameterVec); 133 | void writeParameters(std::vector& parameterVec) const; 134 | 135 | void writeParametersToYamlFile(const std::string& filename) const; 136 | 137 | std::string parametersToString(void) const; 138 | 139 | private: 140 | template 141 | static T r(T k2, T k3, T k4, T k5, T theta); 142 | 143 | 144 | void fitOddPoly(const std::vector& x, const std::vector& y, 145 | int n, std::vector& coeffs) const; 146 | 147 | void backprojectSymmetric(const Eigen::Vector2d& p_u, 148 | double& theta, double& phi) const; 149 | 150 | Parameters mParameters; 151 | 152 | double m_inv_K11, m_inv_K13, m_inv_K22, m_inv_K23; 153 | }; 154 | 155 | typedef boost::shared_ptr EquidistantCameraPtr; 156 | typedef boost::shared_ptr EquidistantCameraConstPtr; 157 | 158 | template 159 | T 160 | EquidistantCamera::r(T k2, T k3, T k4, T k5, T theta) 161 | { 162 | // k1 = 1 163 | return theta + 164 | k2 * theta * theta * theta + 165 | k3 * theta * theta * theta * theta * theta + 166 | k4 * theta * theta * theta * theta * theta * theta * theta + 167 | k5 * theta * theta * theta * theta * theta * theta * theta * theta * theta; 168 | } 169 | 170 | template 171 | void 172 | EquidistantCamera::spaceToPlane(const T* const params, 173 | const T* const q, const T* const t, 174 | const Eigen::Matrix& P, 175 | Eigen::Matrix& p) 176 | { 177 | T P_w[3]; 178 | P_w[0] = T(P(0)); 179 | P_w[1] = T(P(1)); 180 | P_w[2] = T(P(2)); 181 | 182 | // Convert quaternion from Eigen convention (x, y, z, w) 183 | // to Ceres convention (w, x, y, z) 184 | T q_ceres[4] = {q[3], q[0], q[1], q[2]}; 185 | 186 | T P_c[3]; 187 | ceres::QuaternionRotatePoint(q_ceres, P_w, P_c); 188 | 189 | P_c[0] += t[0]; 190 | P_c[1] += t[1]; 191 | P_c[2] += t[2]; 192 | 193 | // project 3D object point to the image plane; 194 | T k2 = params[0]; 195 | T k3 = params[1]; 196 | T k4 = params[2]; 197 | T k5 = params[3]; 198 | T mu = params[4]; 199 | T mv = params[5]; 200 | T u0 = params[6]; 201 | T v0 = params[7]; 202 | 203 | T len = sqrt(P_c[0] * P_c[0] + P_c[1] * P_c[1] + P_c[2] * P_c[2]); 204 | T theta = acos(P_c[2] / len); 205 | T phi = atan2(P_c[1], P_c[0]); 206 | 207 | Eigen::Matrix p_u = r(k2, k3, k4, k5, theta) * Eigen::Matrix(cos(phi), sin(phi)); 208 | 209 | p(0) = mu * p_u(0) + u0; 210 | p(1) = mv * p_u(1) + v0; 211 | } 212 | 213 | } 214 | 215 | #endif 216 | -------------------------------------------------------------------------------- /include/camodocal/camera_models/PinholeCamera.h: -------------------------------------------------------------------------------- 1 | #ifndef PINHOLECAMERA_H 2 | #define PINHOLECAMERA_H 3 | 4 | #include 5 | #include 6 | 7 | #include "ceres/rotation.h" 8 | #include "Camera.h" 9 | 10 | namespace camodocal 11 | { 12 | 13 | class PinholeCamera: public Camera 14 | { 15 | public: 16 | class Parameters: public Camera::Parameters 17 | { 18 | public: 19 | Parameters(); 20 | Parameters(const std::string& cameraName, 21 | int w, int h, 22 | double k1, double k2, double p1, double p2, 23 | double fx, double fy, double cx, double cy); 24 | 25 | double& k1(void); 26 | double& k2(void); 27 | double& p1(void); 28 | double& p2(void); 29 | double& fx(void); 30 | double& fy(void); 31 | double& cx(void); 32 | double& cy(void); 33 | 34 | double xi(void) const; 35 | double k1(void) const; 36 | double k2(void) const; 37 | double p1(void) const; 38 | double p2(void) const; 39 | double fx(void) const; 40 | double fy(void) const; 41 | double cx(void) const; 42 | double cy(void) const; 43 | 44 | bool readFromYamlFile(const std::string& filename); 45 | void writeToYamlFile(const std::string& filename) const; 46 | 47 | Parameters& operator=(const Parameters& other); 48 | friend std::ostream& operator<< (std::ostream& out, const Parameters& params); 49 | 50 | private: 51 | double m_k1; 52 | double m_k2; 53 | double m_p1; 54 | double m_p2; 55 | double m_fx; 56 | double m_fy; 57 | double m_cx; 58 | double m_cy; 59 | }; 60 | 61 | PinholeCamera(); 62 | 63 | /** 64 | * \brief Constructor from the projection model parameters 65 | */ 66 | PinholeCamera(const std::string& cameraName, 67 | int imageWidth, int imageHeight, 68 | double k1, double k2, double p1, double p2, 69 | double fx, double fy, double cx, double cy); 70 | /** 71 | * \brief Constructor from the projection model parameters 72 | */ 73 | PinholeCamera(const Parameters& params); 74 | 75 | Camera::ModelType modelType(void) const; 76 | const std::string& cameraName(void) const; 77 | int imageWidth(void) const; 78 | int imageHeight(void) const; 79 | 80 | void estimateIntrinsics(const cv::Size& boardSize, 81 | const std::vector< std::vector >& objectPoints, 82 | const std::vector< std::vector >& imagePoints); 83 | 84 | // Lift points from the image plane to the sphere 85 | virtual void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const; 86 | //%output P 87 | 88 | // Lift points from the image plane to the projective space 89 | void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const; 90 | //%output P 91 | 92 | // Projects 3D points to the image plane (Pi function) 93 | void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const; 94 | //%output p 95 | 96 | // Projects 3D points to the image plane (Pi function) 97 | // and calculates jacobian 98 | void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p, 99 | Eigen::Matrix& J) const; 100 | //%output p 101 | //%output J 102 | 103 | void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const; 104 | //%output p 105 | 106 | template 107 | static void spaceToPlane(const T* const params, 108 | const T* const q, const T* const t, 109 | const Eigen::Matrix& P, 110 | Eigen::Matrix& p); 111 | 112 | void distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u) const; 113 | void distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u, 114 | Eigen::Matrix2d& J) const; 115 | 116 | void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0) const; 117 | cv::Mat initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2, 118 | float fx = -1.0f, float fy = -1.0f, 119 | cv::Size imageSize = cv::Size(0, 0), 120 | float cx = -1.0f, float cy = -1.0f, 121 | cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const; 122 | 123 | int parameterCount(void) const; 124 | 125 | const Parameters& getParameters(void) const; 126 | void setParameters(const Parameters& parameters); 127 | 128 | void readParameters(const std::vector& parameterVec); 129 | void writeParameters(std::vector& parameterVec) const; 130 | 131 | void writeParametersToYamlFile(const std::string& filename) const; 132 | 133 | std::string parametersToString(void) const; 134 | 135 | private: 136 | Parameters mParameters; 137 | 138 | double m_inv_K11, m_inv_K13, m_inv_K22, m_inv_K23; 139 | bool m_noDistortion; 140 | }; 141 | 142 | typedef boost::shared_ptr PinholeCameraPtr; 143 | typedef boost::shared_ptr PinholeCameraConstPtr; 144 | 145 | template 146 | void 147 | PinholeCamera::spaceToPlane(const T* const params, 148 | const T* const q, const T* const t, 149 | const Eigen::Matrix& P, 150 | Eigen::Matrix& p) 151 | { 152 | T P_w[3]; 153 | P_w[0] = T(P(0)); 154 | P_w[1] = T(P(1)); 155 | P_w[2] = T(P(2)); 156 | 157 | // Convert quaternion from Eigen convention (x, y, z, w) 158 | // to Ceres convention (w, x, y, z) 159 | T q_ceres[4] = {q[3], q[0], q[1], q[2]}; 160 | 161 | T P_c[3]; 162 | ceres::QuaternionRotatePoint(q_ceres, P_w, P_c); 163 | 164 | P_c[0] += t[0]; 165 | P_c[1] += t[1]; 166 | P_c[2] += t[2]; 167 | 168 | // project 3D object point to the image plane 169 | T k1 = params[0]; 170 | T k2 = params[1]; 171 | T p1 = params[2]; 172 | T p2 = params[3]; 173 | T fx = params[4]; 174 | T fy = params[5]; 175 | T alpha = T(0); //cameraParams.alpha(); 176 | T cx = params[6]; 177 | T cy = params[7]; 178 | 179 | // Transform to model plane 180 | T u = P_c[0] / P_c[2]; 181 | T v = P_c[1] / P_c[2]; 182 | 183 | T rho_sqr = u * u + v * v; 184 | T L = T(1.0) + k1 * rho_sqr + k2 * rho_sqr * rho_sqr; 185 | T du = T(2.0) * p1 * u * v + p2 * (rho_sqr + T(2.0) * u * u); 186 | T dv = p1 * (rho_sqr + T(2.0) * v * v) + T(2.0) * p2 * u * v; 187 | 188 | u = L * u + du; 189 | v = L * v + dv; 190 | p(0) = fx * (u + alpha * v) + cx; 191 | p(1) = fy * v + cy; 192 | } 193 | 194 | } 195 | 196 | #endif 197 | -------------------------------------------------------------------------------- /include/camodocal/chessboard/Chessboard.h: -------------------------------------------------------------------------------- 1 | #ifndef CHESSBOARD_H 2 | #define CHESSBOARD_H 3 | 4 | #include 5 | #include 6 | 7 | namespace camodocal 8 | { 9 | 10 | // forward declarations 11 | class ChessboardCorner; 12 | typedef boost::shared_ptr ChessboardCornerPtr; 13 | class ChessboardQuad; 14 | typedef boost::shared_ptr ChessboardQuadPtr; 15 | 16 | class Chessboard 17 | { 18 | public: 19 | Chessboard(cv::Size boardSize, cv::Mat& image); 20 | 21 | void findCorners(bool useOpenCV = false); 22 | const std::vector& getCorners(void) const; 23 | bool cornersFound(void) const; 24 | 25 | const cv::Mat& getImage(void) const; 26 | const cv::Mat& getSketch(void) const; 27 | 28 | private: 29 | bool findChessboardCorners(const cv::Mat& image, 30 | const cv::Size& patternSize, 31 | std::vector& corners, 32 | int flags, bool useOpenCV); 33 | 34 | bool findChessboardCornersImproved(const cv::Mat& image, 35 | const cv::Size& patternSize, 36 | std::vector& corners, 37 | int flags); 38 | 39 | void cleanFoundConnectedQuads(std::vector& quadGroup, cv::Size patternSize); 40 | 41 | void findConnectedQuads(std::vector& quads, 42 | std::vector& group, 43 | int group_idx, int dilation); 44 | 45 | // int checkQuadGroup(std::vector& quadGroup, 46 | // std::vector& outCorners, 47 | // cv::Size patternSize); 48 | 49 | void labelQuadGroup(std::vector& quad_group, 50 | cv::Size patternSize, bool firstRun); 51 | 52 | void findQuadNeighbors(std::vector& quads, int dilation); 53 | 54 | int augmentBestRun(std::vector& candidateQuads, int candidateDilation, 55 | std::vector& existingQuads, int existingDilation); 56 | 57 | void generateQuads(std::vector& quads, 58 | cv::Mat& image, int flags, 59 | int dilation, bool firstRun); 60 | 61 | bool checkQuadGroup(std::vector& quads, 62 | std::vector& corners, 63 | cv::Size patternSize); 64 | 65 | void getQuadrangleHypotheses(const std::vector< std::vector >& contours, 66 | std::vector< std::pair >& quads, 67 | int classId) const; 68 | 69 | bool checkChessboard(const cv::Mat& image, cv::Size patternSize) const; 70 | 71 | bool checkBoardMonotony(std::vector& corners, 72 | cv::Size patternSize); 73 | 74 | bool matchCorners(ChessboardQuadPtr& quad1, int corner1, 75 | ChessboardQuadPtr& quad2, int corner2) const; 76 | 77 | cv::Mat mImage; 78 | cv::Mat mSketch; 79 | std::vector mCorners; 80 | cv::Size mBoardSize; 81 | bool mCornersFound; 82 | }; 83 | 84 | } 85 | 86 | #endif 87 | -------------------------------------------------------------------------------- /include/camodocal/chessboard/ChessboardCorner.h: -------------------------------------------------------------------------------- 1 | #ifndef CHESSBOARDCORNER_H 2 | #define CHESSBOARDCORNER_H 3 | 4 | #include 5 | #include 6 | 7 | namespace camodocal 8 | { 9 | 10 | class ChessboardCorner; 11 | typedef boost::shared_ptr ChessboardCornerPtr; 12 | 13 | class ChessboardCorner 14 | { 15 | public: 16 | ChessboardCorner() : row(0), column(0), needsNeighbor(true), count(0) {} 17 | 18 | float meanDist(int &n) const 19 | { 20 | float sum = 0; 21 | n = 0; 22 | for (int i = 0; i < 4; ++i) 23 | { 24 | if (neighbors[i].get()) 25 | { 26 | float dx = neighbors[i]->pt.x - pt.x; 27 | float dy = neighbors[i]->pt.y - pt.y; 28 | sum += sqrt(dx*dx + dy*dy); 29 | n++; 30 | } 31 | } 32 | return sum / std::max(n, 1); 33 | } 34 | 35 | cv::Point2f pt; // X and y coordinates 36 | int row; // Row and column of the corner 37 | int column; // in the found pattern 38 | bool needsNeighbor; // Does the corner require a neighbor? 39 | int count; // number of corner neighbors 40 | ChessboardCornerPtr neighbors[4]; // pointer to all corner neighbors 41 | }; 42 | 43 | } 44 | 45 | #endif 46 | -------------------------------------------------------------------------------- /include/camodocal/chessboard/ChessboardQuad.h: -------------------------------------------------------------------------------- 1 | #ifndef CHESSBOARDQUAD_H 2 | #define CHESSBOARDQUAD_H 3 | 4 | #include 5 | 6 | #include "camodocal/chessboard/ChessboardCorner.h" 7 | 8 | namespace camodocal 9 | { 10 | 11 | class ChessboardQuad; 12 | typedef boost::shared_ptr ChessboardQuadPtr; 13 | 14 | class ChessboardQuad 15 | { 16 | public: 17 | ChessboardQuad() : count(0), group_idx(-1), edge_len(FLT_MAX), labeled(false) {} 18 | 19 | int count; // Number of quad neighbors 20 | int group_idx; // Quad group ID 21 | float edge_len; // Smallest side length^2 22 | ChessboardCornerPtr corners[4]; // Coordinates of quad corners 23 | ChessboardQuadPtr neighbors[4]; // Pointers of quad neighbors 24 | bool labeled; // Has this corner been labeled? 25 | }; 26 | 27 | } 28 | 29 | #endif 30 | -------------------------------------------------------------------------------- /include/camodocal/gpl/EigenQuaternionParameterization.h: -------------------------------------------------------------------------------- 1 | #ifndef EIGENQUATERNIONPARAMETERIZATION_H 2 | #define EIGENQUATERNIONPARAMETERIZATION_H 3 | 4 | #include "ceres/local_parameterization.h" 5 | 6 | namespace camodocal 7 | { 8 | 9 | class EigenQuaternionParameterization : public ceres::LocalParameterization 10 | { 11 | public: 12 | virtual ~EigenQuaternionParameterization() {} 13 | virtual bool Plus(const double* x, 14 | const double* delta, 15 | double* x_plus_delta) const; 16 | virtual bool ComputeJacobian(const double* x, 17 | double* jacobian) const; 18 | virtual int GlobalSize() const { return 4; } 19 | virtual int LocalSize() const { return 3; } 20 | 21 | private: 22 | template 23 | void EigenQuaternionProduct(const T z[4], const T w[4], T zw[4]) const; 24 | }; 25 | 26 | 27 | template 28 | void 29 | EigenQuaternionParameterization::EigenQuaternionProduct(const T z[4], const T w[4], T zw[4]) const 30 | { 31 | zw[0] = z[3] * w[0] + z[0] * w[3] + z[1] * w[2] - z[2] * w[1]; 32 | zw[1] = z[3] * w[1] - z[0] * w[2] + z[1] * w[3] + z[2] * w[0]; 33 | zw[2] = z[3] * w[2] + z[0] * w[1] - z[1] * w[0] + z[2] * w[3]; 34 | zw[3] = z[3] * w[3] - z[0] * w[0] - z[1] * w[1] - z[2] * w[2]; 35 | } 36 | 37 | } 38 | 39 | #endif 40 | 41 | -------------------------------------------------------------------------------- /include/camodocal/gpl/gpl.h: -------------------------------------------------------------------------------- 1 | #ifndef GPL_H 2 | #define GPL_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace camodocal 9 | { 10 | 11 | template 12 | const T clamp(const T& v, const T& a, const T& b) 13 | { 14 | return std::min(b, std::max(a, v)); 15 | } 16 | 17 | double hypot3(double x, double y, double z); 18 | float hypot3f(float x, float y, float z); 19 | 20 | template 21 | const T normalizeTheta(const T& theta) 22 | { 23 | T normTheta = theta; 24 | 25 | while (normTheta < - M_PI) 26 | { 27 | normTheta += 2.0 * M_PI; 28 | } 29 | while (normTheta > M_PI) 30 | { 31 | normTheta -= 2.0 * M_PI; 32 | } 33 | 34 | return normTheta; 35 | } 36 | 37 | double d2r(double deg); 38 | float d2r(float deg); 39 | double r2d(double rad); 40 | float r2d(float rad); 41 | 42 | double sinc(double theta); 43 | 44 | template 45 | const T square(const T& x) 46 | { 47 | return x * x; 48 | } 49 | 50 | template 51 | const T cube(const T& x) 52 | { 53 | return x * x * x; 54 | } 55 | 56 | template 57 | const T random(const T& a, const T& b) 58 | { 59 | return static_cast(rand()) / RAND_MAX * (b - a) + a; 60 | } 61 | 62 | template 63 | const T randomNormal(const T& sigma) 64 | { 65 | T x1, x2, w; 66 | 67 | do 68 | { 69 | x1 = 2.0 * random(0.0, 1.0) - 1.0; 70 | x2 = 2.0 * random(0.0, 1.0) - 1.0; 71 | w = x1 * x1 + x2 * x2; 72 | } 73 | while (w >= 1.0 || w == 0.0); 74 | 75 | w = sqrt((-2.0 * log(w)) / w); 76 | 77 | return x1 * w * sigma; 78 | } 79 | 80 | unsigned long long timeInMicroseconds(void); 81 | 82 | double timeInSeconds(void); 83 | 84 | void colorDepthImage(cv::Mat& imgDepth, 85 | cv::Mat& imgColoredDepth, 86 | float minRange, float maxRange); 87 | 88 | bool colormap(const std::string& name, unsigned char idx, 89 | float& r, float& g, float& b); 90 | 91 | std::vector bresLine(int x0, int y0, int x1, int y1); 92 | std::vector bresCircle(int x0, int y0, int r); 93 | 94 | void fitCircle(const std::vector& points, 95 | double& centerX, double& centerY, double& radius); 96 | 97 | std::vector intersectCircles(double x1, double y1, double r1, 98 | double x2, double y2, double r2); 99 | 100 | void LLtoUTM(double latitude, double longitude, 101 | double& utmNorthing, double& utmEasting, 102 | std::string& utmZone); 103 | void UTMtoLL(double utmNorthing, double utmEasting, 104 | const std::string& utmZone, 105 | double& latitude, double& longitude); 106 | 107 | long int timestampDiff(uint64_t t1, uint64_t t2); 108 | 109 | } 110 | 111 | #endif 112 | -------------------------------------------------------------------------------- /include/camodocal/sparse_graph/Transform.h: -------------------------------------------------------------------------------- 1 | #ifndef TRANSFORM_H 2 | #define TRANSFORM_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace camodocal 9 | { 10 | 11 | class Transform 12 | { 13 | public: 14 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 15 | 16 | Transform(); 17 | Transform(const Eigen::Matrix4d& H); 18 | 19 | Eigen::Quaterniond& rotation(void); 20 | const Eigen::Quaterniond& rotation(void) const; 21 | double* rotationData(void); 22 | const double* const rotationData(void) const; 23 | 24 | Eigen::Vector3d& translation(void); 25 | const Eigen::Vector3d& translation(void) const; 26 | double* translationData(void); 27 | const double* const translationData(void) const; 28 | 29 | Eigen::Matrix4d toMatrix(void) const; 30 | 31 | private: 32 | Eigen::Quaterniond m_q; 33 | Eigen::Vector3d m_t; 34 | }; 35 | 36 | } 37 | 38 | #endif 39 | -------------------------------------------------------------------------------- /include/estimator.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "parameters.h" 4 | #include "feature_manager.h" 5 | #include "utility/utility.h" 6 | #include "utility/tic_toc.h" 7 | #include "initial/solve_5pts.h" 8 | #include "initial/initial_sfm.h" 9 | #include "initial/initial_alignment.h" 10 | #include "initial/initial_ex_rotation.h" 11 | 12 | #include "factor/integration_base.h" 13 | 14 | #include "backend/problem.h" 15 | 16 | #include 17 | #include 18 | #include 19 | 20 | class Estimator 21 | { 22 | public: 23 | Estimator(); 24 | 25 | void setParameter(); 26 | 27 | // interface 28 | void processIMU(double t, const Vector3d &linear_acceleration, const Vector3d &angular_velocity); 29 | 30 | void processImage(const map>>> &image, double header); 31 | void setReloFrame(double _frame_stamp, int _frame_index, vector &_match_points, Vector3d _relo_t, Matrix3d _relo_r); 32 | 33 | // internal 34 | void clearState(); 35 | bool initialStructure(); 36 | bool visualInitialAlign(); 37 | bool relativePose(Matrix3d &relative_R, Vector3d &relative_T, int &l); 38 | void slideWindow(); 39 | void solveOdometry(); 40 | void slideWindowNew(); 41 | void slideWindowOld(); 42 | void optimization(); 43 | void backendOptimization(); 44 | 45 | void problemSolve(); 46 | void MargOldFrame(); 47 | void MargNewFrame(); 48 | 49 | void vector2double(); 50 | void double2vector(); 51 | bool failureDetection(); 52 | 53 | 54 | enum SolverFlag 55 | { 56 | INITIAL, 57 | NON_LINEAR 58 | }; 59 | 60 | enum MarginalizationFlag 61 | { 62 | MARGIN_OLD = 0, 63 | MARGIN_SECOND_NEW = 1 64 | }; 65 | //////////////// OUR SOLVER /////////////////// 66 | MatXX Hprior_; 67 | VecX bprior_; 68 | VecX errprior_; 69 | MatXX Jprior_inv_; 70 | 71 | Eigen::Matrix2d project_sqrt_info_; 72 | //////////////// OUR SOLVER ////////////////// 73 | SolverFlag solver_flag; 74 | MarginalizationFlag marginalization_flag; 75 | Vector3d g; 76 | MatrixXd Ap[2], backup_A; 77 | VectorXd bp[2], backup_b; 78 | 79 | Matrix3d ric[NUM_OF_CAM]; 80 | Vector3d tic[NUM_OF_CAM]; 81 | 82 | Vector3d Ps[(WINDOW_SIZE + 1)]; 83 | Vector3d Vs[(WINDOW_SIZE + 1)]; 84 | Matrix3d Rs[(WINDOW_SIZE + 1)]; 85 | Vector3d Bas[(WINDOW_SIZE + 1)]; 86 | Vector3d Bgs[(WINDOW_SIZE + 1)]; 87 | double td; 88 | 89 | Matrix3d back_R0, last_R, last_R0; 90 | Vector3d back_P0, last_P, last_P0; 91 | double Headers[(WINDOW_SIZE + 1)]; 92 | 93 | IntegrationBase *pre_integrations[(WINDOW_SIZE + 1)]; 94 | Vector3d acc_0, gyr_0; 95 | 96 | vector dt_buf[(WINDOW_SIZE + 1)]; 97 | vector linear_acceleration_buf[(WINDOW_SIZE + 1)]; 98 | vector angular_velocity_buf[(WINDOW_SIZE + 1)]; 99 | 100 | int frame_count; 101 | int sum_of_outlier, sum_of_back, sum_of_front, sum_of_invalid; 102 | 103 | FeatureManager f_manager; 104 | MotionEstimator m_estimator; 105 | InitialEXRotation initial_ex_rotation; 106 | 107 | bool first_imu; 108 | bool is_valid, is_key; 109 | bool failure_occur; 110 | 111 | vector point_cloud; 112 | vector margin_cloud; 113 | vector key_poses; 114 | double initial_timestamp; 115 | 116 | 117 | double para_Pose[WINDOW_SIZE + 1][SIZE_POSE]; 118 | double para_SpeedBias[WINDOW_SIZE + 1][SIZE_SPEEDBIAS]; 119 | double para_Feature[NUM_OF_F][SIZE_FEATURE]; 120 | double para_Ex_Pose[NUM_OF_CAM][SIZE_POSE]; 121 | double para_Retrive_Pose[SIZE_POSE]; 122 | double para_Td[1][1]; 123 | double para_Tr[1][1]; 124 | 125 | int loop_window_index; 126 | 127 | // MarginalizationInfo *last_marginalization_info; 128 | vector last_marginalization_parameter_blocks; 129 | 130 | map all_image_frame; 131 | IntegrationBase *tmp_pre_integration; 132 | 133 | //relocalization variable 134 | bool relocalization_info; 135 | double relo_frame_stamp; 136 | double relo_frame_index; 137 | int relo_frame_local_index; 138 | vector match_points; 139 | double relo_Pose[SIZE_POSE]; 140 | Matrix3d drift_correct_r; 141 | Vector3d drift_correct_t; 142 | Vector3d prev_relo_t; 143 | Matrix3d prev_relo_r; 144 | Vector3d relo_relative_t; 145 | Quaterniond relo_relative_q; 146 | double relo_relative_yaw; 147 | 148 | // TODO: HOMEWORK store the lm's computation cost in txt 149 | double lm_times = 0.0; 150 | }; 151 | -------------------------------------------------------------------------------- /include/feature_manager.h: -------------------------------------------------------------------------------- 1 | #ifndef FEATURE_MANAGER_H 2 | #define FEATURE_MANAGER_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | using namespace std; 10 | 11 | #include 12 | using namespace Eigen; 13 | 14 | // #include 15 | // #include 16 | 17 | #include "parameters.h" 18 | 19 | class FeaturePerFrame 20 | { 21 | public: 22 | FeaturePerFrame(const Eigen::Matrix &_point, double td) 23 | { 24 | point.x() = _point(0); 25 | point.y() = _point(1); 26 | point.z() = _point(2); 27 | uv.x() = _point(3); 28 | uv.y() = _point(4); 29 | velocity.x() = _point(5); 30 | velocity.y() = _point(6); 31 | cur_td = td; 32 | } 33 | double cur_td; 34 | Vector3d point; 35 | Vector2d uv; 36 | Vector2d velocity; 37 | double z; 38 | bool is_used; 39 | double parallax; 40 | MatrixXd A; 41 | VectorXd b; 42 | double dep_gradient; 43 | }; 44 | 45 | class FeaturePerId 46 | { 47 | public: 48 | const int feature_id; 49 | int start_frame; 50 | vector feature_per_frame; 51 | 52 | int used_num; 53 | bool is_outlier; 54 | bool is_margin; 55 | double estimated_depth; 56 | int solve_flag; // 0 haven't solve yet; 1 solve succ; 2 solve fail; 57 | 58 | Vector3d gt_p; 59 | 60 | FeaturePerId(int _feature_id, int _start_frame) 61 | : feature_id(_feature_id), start_frame(_start_frame), 62 | used_num(0), estimated_depth(-1.0), solve_flag(0) 63 | { 64 | } 65 | 66 | int endFrame(); 67 | }; 68 | 69 | class FeatureManager 70 | { 71 | public: 72 | FeatureManager(Matrix3d _Rs[]); 73 | 74 | void setRic(Matrix3d _ric[]); 75 | 76 | void clearState(); 77 | 78 | int getFeatureCount(); 79 | 80 | bool addFeatureCheckParallax(int frame_count, const map>>> &image, double td); 81 | void debugShow(); 82 | vector> getCorresponding(int frame_count_l, int frame_count_r); 83 | 84 | //void updateDepth(const VectorXd &x); 85 | void setDepth(const VectorXd &x); 86 | void removeFailures(); 87 | void clearDepth(const VectorXd &x); 88 | VectorXd getDepthVector(); 89 | void triangulate(Vector3d Ps[], Vector3d tic[], Matrix3d ric[]); 90 | void removeBackShiftDepth(Eigen::Matrix3d marg_R, Eigen::Vector3d marg_P, Eigen::Matrix3d new_R, Eigen::Vector3d new_P); 91 | void removeBack(); 92 | void removeFront(int frame_count); 93 | void removeOutlier(); 94 | list feature; 95 | int last_track_num; 96 | 97 | private: 98 | double compensatedParallax2(const FeaturePerId &it_per_id, int frame_count); 99 | const Matrix3d *Rs; 100 | Matrix3d ric[NUM_OF_CAM]; 101 | }; 102 | 103 | #endif -------------------------------------------------------------------------------- /include/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 "utility/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 | 33 | void readImage(const cv::Mat &_img,double _cur_time); 34 | 35 | void setMask(); 36 | 37 | void addPoints(); 38 | 39 | bool updateID(unsigned int i); 40 | 41 | void readIntrinsicParameter(const string &calib_file); 42 | 43 | void showUndistortion(const string &name); 44 | 45 | void rejectWithF(); 46 | 47 | void undistortedPoints(); 48 | 49 | cv::Mat mask; 50 | cv::Mat fisheye_mask; 51 | cv::Mat prev_img, cur_img, forw_img; 52 | vector n_pts; 53 | vector prev_pts, cur_pts, forw_pts; 54 | vector prev_un_pts, cur_un_pts; 55 | vector pts_velocity; 56 | vector ids; 57 | vector track_cnt; 58 | map cur_un_pts_map; 59 | map prev_un_pts_map; 60 | camodocal::CameraPtr m_camera; 61 | double cur_time; 62 | double prev_time; 63 | 64 | static int n_id; 65 | }; 66 | -------------------------------------------------------------------------------- /include/feature_tracker_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 | extern std::string IMAGE_TOPIC; 11 | extern std::string IMU_TOPIC; 12 | extern std::string FISHEYE_MASK; 13 | extern std::vector CAM_NAMES; 14 | extern int MAX_CNT; 15 | extern int MIN_DIST; 16 | extern int WINDOW_SIZE; 17 | extern int FREQ; 18 | extern double F_THRESHOLD; 19 | extern int SHOW_TRACK; 20 | extern int STEREO_TRACK; 21 | extern int EQUALIZE; 22 | extern int FISHEYE; 23 | extern bool PUB_THIS_FRAME; 24 | 25 | void readParameters(); 26 | -------------------------------------------------------------------------------- /include/initial/initial_alignment.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | 6 | #include "../factor/integration_base.h" 7 | #include "../utility/utility.h" 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); -------------------------------------------------------------------------------- /include/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 | 20 | private: 21 | Matrix3d solveRelativeR(const vector> &corres); 22 | 23 | double testTriangulation(const vector &l, 24 | const vector &r, 25 | cv::Mat_ R, cv::Mat_ t); 26 | void decomposeE(cv::Mat E, 27 | cv::Mat_ &R1, cv::Mat_ &R2, 28 | cv::Mat_ &t1, cv::Mat_ &t2); 29 | int frame_count; 30 | 31 | vector Rc; 32 | vector Rimu; 33 | vector Rc_g; 34 | Matrix3d ric; 35 | }; 36 | -------------------------------------------------------------------------------- /include/initial/initial_sfm.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | using namespace Eigen; 12 | using namespace std; 13 | 14 | 15 | 16 | struct SFMFeature 17 | { 18 | bool state; 19 | int id; 20 | vector> observation; 21 | double position[3]; 22 | double depth; 23 | }; 24 | 25 | struct ReprojectionError3D 26 | { 27 | ReprojectionError3D(double observed_u, double observed_v) 28 | :observed_u(observed_u), observed_v(observed_v) 29 | {} 30 | 31 | template 32 | bool operator()(const T* const camera_R, const T* const camera_T, const T* point, T* residuals) const 33 | { 34 | T p[3]; 35 | ceres::QuaternionRotatePoint(camera_R, point, p); 36 | p[0] += camera_T[0]; p[1] += camera_T[1]; p[2] += camera_T[2]; 37 | T xp = p[0] / p[2]; 38 | T yp = p[1] / p[2]; 39 | residuals[0] = xp - T(observed_u); 40 | residuals[1] = yp - T(observed_v); 41 | return true; 42 | } 43 | 44 | static ceres::CostFunction* Create(const double observed_x, 45 | const double observed_y) 46 | { 47 | return (new ceres::AutoDiffCostFunction< 48 | ReprojectionError3D, 2, 4, 3, 3>( 49 | new ReprojectionError3D(observed_x,observed_y))); 50 | } 51 | 52 | double observed_u; 53 | double observed_v; 54 | }; 55 | 56 | class GlobalSFM 57 | { 58 | public: 59 | GlobalSFM(); 60 | bool construct(int frame_num, Quaterniond* q, Vector3d* T, int l, 61 | const Matrix3d relative_R, const Vector3d relative_T, 62 | vector &sfm_f, map &sfm_tracked_points); 63 | 64 | private: 65 | bool solveFrameByPnP(Matrix3d &R_initial, Vector3d &P_initial, int i, vector &sfm_f); 66 | 67 | void triangulatePoint(Eigen::Matrix &Pose0, Eigen::Matrix &Pose1, 68 | Vector2d &point0, Vector2d &point1, Vector3d &point_3d); 69 | void triangulateTwoFrames(int frame0, Eigen::Matrix &Pose0, 70 | int frame1, Eigen::Matrix &Pose1, 71 | vector &sfm_f); 72 | 73 | int feature_num; 74 | }; -------------------------------------------------------------------------------- /include/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 | bool solveRelativeRT(const vector> &corres, Matrix3d &R, Vector3d &T); 17 | 18 | private: 19 | double testTriangulation(const vector &l, 20 | const vector &r, 21 | cv::Mat_ R, cv::Mat_ t); 22 | void decomposeE(cv::Mat E, 23 | cv::Mat_ &R1, cv::Mat_ &R2, 24 | cv::Mat_ &t1, cv::Mat_ &t2); 25 | }; 26 | -------------------------------------------------------------------------------- /include/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 | //feature tracker 12 | // extern int ROW; 13 | // extern int COL; 14 | const int NUM_OF_CAM = 1; 15 | 16 | extern int FOCAL_LENGTH; 17 | extern std::string IMAGE_TOPIC; 18 | extern std::string IMU_TOPIC; 19 | extern std::string FISHEYE_MASK; 20 | extern std::vector CAM_NAMES; 21 | extern int MAX_CNT; 22 | extern int MIN_DIST; 23 | // extern int WINDOW_SIZE; 24 | extern int FREQ; 25 | extern double F_THRESHOLD; 26 | extern int SHOW_TRACK; 27 | extern bool STEREO_TRACK; 28 | extern int EQUALIZE; 29 | extern int FISHEYE; 30 | extern bool PUB_THIS_FRAME; 31 | 32 | //estimator 33 | 34 | // const double FOCAL_LENGTH = 460.0; 35 | const int WINDOW_SIZE = 10; 36 | // const int NUM_OF_CAM = 1; 37 | const int NUM_OF_F = 1000; 38 | //#define UNIT_SPHERE_ERROR 39 | 40 | extern double INIT_DEPTH; 41 | extern double MIN_PARALLAX; 42 | extern int ESTIMATE_EXTRINSIC; 43 | 44 | extern double ACC_N, ACC_W; 45 | extern double GYR_N, GYR_W; 46 | 47 | extern std::vector RIC; 48 | extern std::vector TIC; 49 | extern Eigen::Vector3d G; 50 | 51 | extern double BIAS_ACC_THRESHOLD; 52 | extern double BIAS_GYR_THRESHOLD; 53 | extern double SOLVER_TIME; 54 | extern int NUM_ITERATIONS; 55 | extern std::string EX_CALIB_RESULT_PATH; 56 | extern std::string VINS_RESULT_PATH; 57 | extern std::string IMU_TOPIC; 58 | extern double TD; 59 | extern double TR; 60 | extern int ESTIMATE_TD; 61 | extern int ROLLING_SHUTTER; 62 | extern double ROW, COL; 63 | 64 | // void readParameters(ros::NodeHandle &n); 65 | 66 | void readParameters(std::string config_file); 67 | 68 | enum SIZE_PARAMETERIZATION 69 | { 70 | SIZE_POSE = 7, 71 | SIZE_SPEEDBIAS = 9, 72 | SIZE_FEATURE = 1 73 | }; 74 | 75 | enum StateOrder 76 | { 77 | O_P = 0, 78 | O_R = 3, 79 | O_V = 6, 80 | O_BA = 9, 81 | O_BG = 12 82 | }; 83 | 84 | enum NoiseOrder 85 | { 86 | O_AN = 0, 87 | O_GN = 3, 88 | O_AW = 6, 89 | O_GW = 9 90 | }; 91 | -------------------------------------------------------------------------------- /include/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 | -------------------------------------------------------------------------------- /include/utility/utility.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | class Utility 9 | { 10 | public: 11 | template 12 | static Eigen::Quaternion deltaQ(const Eigen::MatrixBase &theta) 13 | { 14 | typedef typename Derived::Scalar Scalar_t; 15 | 16 | Eigen::Quaternion dq; 17 | Eigen::Matrix half_theta = theta; 18 | half_theta /= static_cast(2.0); 19 | dq.w() = static_cast(1.0); 20 | dq.x() = half_theta.x(); 21 | dq.y() = half_theta.y(); 22 | dq.z() = half_theta.z(); 23 | return dq; 24 | } 25 | 26 | template 27 | static Eigen::Matrix skewSymmetric(const Eigen::MatrixBase &q) 28 | { 29 | Eigen::Matrix ans; 30 | ans << typename Derived::Scalar(0), -q(2), q(1), 31 | q(2), typename Derived::Scalar(0), -q(0), 32 | -q(1), q(0), typename Derived::Scalar(0); 33 | return ans; 34 | } 35 | 36 | template 37 | static Eigen::Quaternion positify(const Eigen::QuaternionBase &q) 38 | { 39 | //printf("a: %f %f %f %f", q.w(), q.x(), q.y(), q.z()); 40 | //Eigen::Quaternion p(-q.w(), -q.x(), -q.y(), -q.z()); 41 | //printf("b: %f %f %f %f", p.w(), p.x(), p.y(), p.z()); 42 | //return q.template w() >= (typename Derived::Scalar)(0.0) ? q : Eigen::Quaternion(-q.w(), -q.x(), -q.y(), -q.z()); 43 | return q; 44 | } 45 | 46 | template 47 | static Eigen::Matrix Qleft(const Eigen::QuaternionBase &q) 48 | { 49 | Eigen::Quaternion qq = positify(q); 50 | Eigen::Matrix ans; 51 | ans(0, 0) = qq.w(), ans.template block<1, 3>(0, 1) = -qq.vec().transpose(); 52 | ans.template block<3, 1>(1, 0) = qq.vec(), ans.template block<3, 3>(1, 1) = qq.w() * Eigen::Matrix::Identity() + skewSymmetric(qq.vec()); 53 | return ans; 54 | } 55 | 56 | template 57 | static Eigen::Matrix Qright(const Eigen::QuaternionBase &p) 58 | { 59 | Eigen::Quaternion pp = positify(p); 60 | Eigen::Matrix ans; 61 | ans(0, 0) = pp.w(), ans.template block<1, 3>(0, 1) = -pp.vec().transpose(); 62 | ans.template block<3, 1>(1, 0) = pp.vec(), ans.template block<3, 3>(1, 1) = pp.w() * Eigen::Matrix::Identity() - skewSymmetric(pp.vec()); 63 | return ans; 64 | } 65 | 66 | static Eigen::Vector3d R2ypr(const Eigen::Matrix3d &R) 67 | { 68 | Eigen::Vector3d n = R.col(0); 69 | Eigen::Vector3d o = R.col(1); 70 | Eigen::Vector3d a = R.col(2); 71 | 72 | Eigen::Vector3d ypr(3); 73 | double y = atan2(n(1), n(0)); 74 | double p = atan2(-n(2), n(0) * cos(y) + n(1) * sin(y)); 75 | double r = atan2(a(0) * sin(y) - a(1) * cos(y), -o(0) * sin(y) + o(1) * cos(y)); 76 | ypr(0) = y; 77 | ypr(1) = p; 78 | ypr(2) = r; 79 | 80 | return ypr / M_PI * 180.0; 81 | } 82 | 83 | template 84 | static Eigen::Matrix ypr2R(const Eigen::MatrixBase &ypr) 85 | { 86 | typedef typename Derived::Scalar Scalar_t; 87 | 88 | Scalar_t y = ypr(0) / 180.0 * M_PI; 89 | Scalar_t p = ypr(1) / 180.0 * M_PI; 90 | Scalar_t r = ypr(2) / 180.0 * M_PI; 91 | 92 | Eigen::Matrix Rz; 93 | Rz << cos(y), -sin(y), 0, 94 | sin(y), cos(y), 0, 95 | 0, 0, 1; 96 | 97 | Eigen::Matrix Ry; 98 | Ry << cos(p), 0., sin(p), 99 | 0., 1., 0., 100 | -sin(p), 0., cos(p); 101 | 102 | Eigen::Matrix Rx; 103 | Rx << 1., 0., 0., 104 | 0., cos(r), -sin(r), 105 | 0., sin(r), cos(r); 106 | 107 | return Rz * Ry * Rx; 108 | } 109 | 110 | static Eigen::Matrix3d g2R(const Eigen::Vector3d &g); 111 | 112 | template 113 | struct uint_ 114 | { 115 | }; 116 | 117 | template 118 | void unroller(const Lambda &f, const IterT &iter, uint_) 119 | { 120 | unroller(f, iter, uint_()); 121 | f(iter + N); 122 | } 123 | 124 | template 125 | void unroller(const Lambda &f, const IterT &iter, uint_<0>) 126 | { 127 | f(iter); 128 | } 129 | 130 | template 131 | static T normalizeAngle(const T& angle_degrees) { 132 | T two_pi(2.0 * 180); 133 | if (angle_degrees > 0) 134 | return angle_degrees - 135 | two_pi * std::floor((angle_degrees + T(180)) / two_pi); 136 | else 137 | return angle_degrees + 138 | two_pi * std::floor((-angle_degrees + T(180)) / two_pi); 139 | }; 140 | }; 141 | -------------------------------------------------------------------------------- /src/.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengKID/My_VINS/240d0a1f49371611c186fdce8bbaf4f5bbe282a1/src/.DS_Store -------------------------------------------------------------------------------- /src/backend/edge.cc: -------------------------------------------------------------------------------- 1 | #include "backend/vertex.h" 2 | #include "backend/edge.h" 3 | //#include 4 | #include 5 | 6 | using namespace std; 7 | 8 | namespace myslam { 9 | namespace backend { 10 | 11 | unsigned long global_edge_id = 0; 12 | 13 | Edge::Edge(int residual_dimension, int num_verticies, 14 | const std::vector &verticies_types) { 15 | residual_.resize(residual_dimension, 1); 16 | // verticies_.resize(num_verticies); // TODO:: there could be problem，e.g. three null are resized here, following we use edge->addVertex, so before vertex it maybe a null element 17 | if (!verticies_types.empty()) 18 | verticies_types_ = verticies_types; 19 | jacobians_.resize(num_verticies); 20 | id_ = global_edge_id++; 21 | 22 | Eigen::MatrixXd information(residual_dimension, residual_dimension); 23 | information.setIdentity(); 24 | information_ = information; 25 | 26 | lossfunction_ = NULL; 27 | // cout<<"Edge construct residual_dimension="<TypeInfo() << endl; 84 | return false; 85 | } 86 | } 87 | } 88 | /* 89 | CHECK_EQ(information_.rows(), information_.cols()); 90 | CHECK_EQ(residual_.rows(), information_.rows()); 91 | CHECK_EQ(residual_.rows(), observation_.rows()); 92 | 93 | // check jacobians 94 | for (size_t i = 0; i < jacobians_.size(); ++i) { 95 | CHECK_EQ(jacobians_[i].rows(), residual_.rows()); 96 | CHECK_EQ(jacobians_[i].cols(), verticies_[i]->LocalDimension()); 97 | } 98 | */ 99 | return true; 100 | } 101 | 102 | } 103 | } -------------------------------------------------------------------------------- /src/backend/edge_imu.cc: -------------------------------------------------------------------------------- 1 | #include "backend/vertex_pose.h" 2 | #include "backend/vertex_speedbias.h" 3 | #include "backend/edge_imu.h" 4 | 5 | #include 6 | 7 | namespace myslam { 8 | namespace backend { 9 | using Sophus::SO3d; 10 | 11 | Vec3 EdgeImu::gravity_ = Vec3(0, 0, 9.8); 12 | 13 | void EdgeImu::ComputeResidual() { 14 | VecX param_0 = verticies_[0]->Parameters(); 15 | Qd Qi(param_0[6], param_0[3], param_0[4], param_0[5]); 16 | Vec3 Pi = param_0.head<3>(); 17 | 18 | VecX param_1 = verticies_[1]->Parameters(); 19 | Vec3 Vi = param_1.head<3>(); 20 | Vec3 Bai = param_1.segment(3, 3); 21 | Vec3 Bgi = param_1.tail<3>(); 22 | 23 | VecX param_2 = verticies_[2]->Parameters(); 24 | Qd Qj(param_2[6], param_2[3], param_2[4], param_2[5]); 25 | Vec3 Pj = param_2.head<3>(); 26 | 27 | VecX param_3 = verticies_[3]->Parameters(); 28 | Vec3 Vj = param_3.head<3>(); 29 | Vec3 Baj = param_3.segment(3, 3); 30 | Vec3 Bgj = param_3.tail<3>(); 31 | 32 | residual_ = pre_integration_->evaluate(Pi, Qi, Vi, Bai, Bgi, 33 | Pj, Qj, Vj, Baj, Bgj); 34 | // Mat1515 sqrt_info = Eigen::LLT< Mat1515 >(pre_integration_->covariance.inverse()).matrixL().transpose(); 35 | SetInformation(pre_integration_->covariance.inverse()); 36 | } 37 | 38 | void EdgeImu::ComputeJacobians() { 39 | 40 | VecX param_0 = verticies_[0]->Parameters(); 41 | Qd Qi(param_0[6], param_0[3], param_0[4], param_0[5]); 42 | Vec3 Pi = param_0.head<3>(); 43 | 44 | VecX param_1 = verticies_[1]->Parameters(); 45 | Vec3 Vi = param_1.head<3>(); 46 | Vec3 Bai = param_1.segment(3, 3); 47 | Vec3 Bgi = param_1.tail<3>(); 48 | 49 | VecX param_2 = verticies_[2]->Parameters(); 50 | Qd Qj(param_2[6], param_2[3], param_2[4], param_2[5]); 51 | Vec3 Pj = param_2.head<3>(); 52 | 53 | VecX param_3 = verticies_[3]->Parameters(); 54 | Vec3 Vj = param_3.head<3>(); 55 | Vec3 Baj = param_3.segment(3, 3); 56 | Vec3 Bgj = param_3.tail<3>(); 57 | 58 | double sum_dt = pre_integration_->sum_dt; 59 | Eigen::Matrix3d dp_dba = pre_integration_->jacobian.template block<3, 3>(O_P, O_BA); 60 | Eigen::Matrix3d dp_dbg = pre_integration_->jacobian.template block<3, 3>(O_P, O_BG); 61 | 62 | Eigen::Matrix3d dq_dbg = pre_integration_->jacobian.template block<3, 3>(O_R, O_BG); 63 | 64 | Eigen::Matrix3d dv_dba = pre_integration_->jacobian.template block<3, 3>(O_V, O_BA); 65 | Eigen::Matrix3d dv_dbg = pre_integration_->jacobian.template block<3, 3>(O_V, O_BG); 66 | 67 | if (pre_integration_->jacobian.maxCoeff() > 1e8 || pre_integration_->jacobian.minCoeff() < -1e8) 68 | { 69 | // ROS_WARN("numerical unstable in preintegration"); 70 | } 71 | 72 | // if (jacobians[0]) 73 | { 74 | Eigen::Matrix jacobian_pose_i; 75 | jacobian_pose_i.setZero(); 76 | 77 | jacobian_pose_i.block<3, 3>(O_P, O_P) = -Qi.inverse().toRotationMatrix(); 78 | jacobian_pose_i.block<3, 3>(O_P, O_R) = Utility::skewSymmetric(Qi.inverse() * (0.5 * G * sum_dt * sum_dt + Pj - Pi - Vi * sum_dt)); 79 | 80 | #if 0 81 | jacobian_pose_i.block<3, 3>(O_R, O_R) = -(Qj.inverse() * Qi).toRotationMatrix(); 82 | #else 83 | Eigen::Quaterniond corrected_delta_q = pre_integration_->delta_q * Utility::deltaQ(dq_dbg * (Bgi - pre_integration_->linearized_bg)); 84 | jacobian_pose_i.block<3, 3>(O_R, O_R) = -(Utility::Qleft(Qj.inverse() * Qi) * Utility::Qright(corrected_delta_q)).bottomRightCorner<3, 3>(); 85 | #endif 86 | 87 | jacobian_pose_i.block<3, 3>(O_V, O_R) = Utility::skewSymmetric(Qi.inverse() * (G * sum_dt + Vj - Vi)); 88 | // jacobian_pose_i = sqrt_info * jacobian_pose_i; 89 | 90 | if (jacobian_pose_i.maxCoeff() > 1e8 || jacobian_pose_i.minCoeff() < -1e8) 91 | { 92 | // ROS_WARN("numerical unstable in preintegration"); 93 | } 94 | jacobians_[0] = jacobian_pose_i; 95 | } 96 | // if (jacobians[1]) 97 | { 98 | Eigen::Matrix jacobian_speedbias_i; 99 | jacobian_speedbias_i.setZero(); 100 | jacobian_speedbias_i.block<3, 3>(O_P, O_V - O_V) = -Qi.inverse().toRotationMatrix() * sum_dt; 101 | jacobian_speedbias_i.block<3, 3>(O_P, O_BA - O_V) = -dp_dba; 102 | jacobian_speedbias_i.block<3, 3>(O_P, O_BG - O_V) = -dp_dbg; 103 | 104 | #if 0 105 | jacobian_speedbias_i.block<3, 3>(O_R, O_BG - O_V) = -dq_dbg; 106 | #else 107 | //Eigen::Quaterniond corrected_delta_q = pre_integration->delta_q * Utility::deltaQ(dq_dbg * (Bgi - pre_integration->linearized_bg)); 108 | //jacobian_speedbias_i.block<3, 3>(O_R, O_BG - O_V) = -Utility::Qleft(Qj.inverse() * Qi * corrected_delta_q).bottomRightCorner<3, 3>() * dq_dbg; 109 | jacobian_speedbias_i.block<3, 3>(O_R, O_BG - O_V) = -Utility::Qleft(Qj.inverse() * Qi * pre_integration_->delta_q).bottomRightCorner<3, 3>() * dq_dbg; 110 | #endif 111 | 112 | jacobian_speedbias_i.block<3, 3>(O_V, O_V - O_V) = -Qi.inverse().toRotationMatrix(); 113 | jacobian_speedbias_i.block<3, 3>(O_V, O_BA - O_V) = -dv_dba; 114 | jacobian_speedbias_i.block<3, 3>(O_V, O_BG - O_V) = -dv_dbg; 115 | 116 | jacobian_speedbias_i.block<3, 3>(O_BA, O_BA - O_V) = -Eigen::Matrix3d::Identity(); 117 | 118 | jacobian_speedbias_i.block<3, 3>(O_BG, O_BG - O_V) = -Eigen::Matrix3d::Identity(); 119 | 120 | // jacobian_speedbias_i = sqrt_info * jacobian_speedbias_i; 121 | jacobians_[1] = jacobian_speedbias_i; 122 | } 123 | // if (jacobians[2]) 124 | { 125 | Eigen::Matrix jacobian_pose_j; 126 | jacobian_pose_j.setZero(); 127 | 128 | jacobian_pose_j.block<3, 3>(O_P, O_P) = Qi.inverse().toRotationMatrix(); 129 | #if 0 130 | jacobian_pose_j.block<3, 3>(O_R, O_R) = Eigen::Matrix3d::Identity(); 131 | #else 132 | Eigen::Quaterniond corrected_delta_q = pre_integration_->delta_q * Utility::deltaQ(dq_dbg * (Bgi - pre_integration_->linearized_bg)); 133 | jacobian_pose_j.block<3, 3>(O_R, O_R) = Utility::Qleft(corrected_delta_q.inverse() * Qi.inverse() * Qj).bottomRightCorner<3, 3>(); 134 | #endif 135 | 136 | // jacobian_pose_j = sqrt_info * jacobian_pose_j; 137 | jacobians_[2] = jacobian_pose_j; 138 | 139 | } 140 | // if (jacobians[3]) 141 | { 142 | Eigen::Matrix jacobian_speedbias_j; 143 | jacobian_speedbias_j.setZero(); 144 | 145 | jacobian_speedbias_j.block<3, 3>(O_V, O_V - O_V) = Qi.inverse().toRotationMatrix(); 146 | 147 | jacobian_speedbias_j.block<3, 3>(O_BA, O_BA - O_V) = Eigen::Matrix3d::Identity(); 148 | 149 | jacobian_speedbias_j.block<3, 3>(O_BG, O_BG - O_V) = Eigen::Matrix3d::Identity(); 150 | 151 | // jacobian_speedbias_j = sqrt_info * jacobian_speedbias_j; 152 | jacobians_[3] = jacobian_speedbias_j; 153 | 154 | } 155 | 156 | 157 | } 158 | 159 | } 160 | } -------------------------------------------------------------------------------- /src/backend/edge_prior.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by heyijia on 19-1-30. 3 | // 4 | 5 | #include "../thirdparty/Sophus/sophus/se3.hpp" 6 | 7 | #include "backend/edge_prior.h" 8 | #include "backend/vertex_pose.h" 9 | 10 | #include 11 | 12 | #define USE_SO3_JACOBIAN 1 13 | 14 | namespace myslam { 15 | namespace backend { 16 | 17 | #ifndef USE_SO3_JACOBIAN 18 | template 19 | static Eigen::Matrix skewSymmetric(const Eigen::MatrixBase &q) 20 | { 21 | Eigen::Matrix ans; 22 | ans << typename Derived::Scalar(0), -q(2), q(1), 23 | q(2), typename Derived::Scalar(0), -q(0), 24 | -q(1), q(0), typename Derived::Scalar(0); 25 | return ans; 26 | } 27 | 28 | template 29 | static Eigen::Matrix Qleft(const Eigen::QuaternionBase &q) 30 | { 31 | Eigen::Quaternion qq = q; 32 | Eigen::Matrix ans; 33 | ans(0, 0) = qq.w(), ans.template block<1, 3>(0, 1) = -qq.vec().transpose(); 34 | ans.template block<3, 1>(1, 0) = qq.vec(), ans.template block<3, 3>(1, 1) = qq.w() * Eigen::Matrix::Identity() + skewSymmetric(qq.vec()); 35 | return ans; 36 | } 37 | #endif 38 | 39 | void EdgeSE3Prior::ComputeResidual() { 40 | VecX param_i = verticies_[0]->Parameters(); 41 | Qd Qi(param_i[6], param_i[3], param_i[4], param_i[5]); 42 | Vec3 Pi = param_i.head<3>(); 43 | 44 | // std::cout << Qi.vec().transpose() <<" "<< Qp_.vec().transpose()<(0,0) = Sophus::SO3d::log(res_r); 51 | #else 52 | residual_.block<3,1>(0,0) = 2 * (Qp_.inverse() * Qi).vec(); 53 | #endif 54 | // translation error 55 | residual_.block<3,1>(3,0) = Pi - Pp_; 56 | // std::cout << residual_.transpose() <Parameters(); 62 | Qd Qi(param_i[6], param_i[3], param_i[4], param_i[5]); 63 | 64 | // w.r.t. pose i 65 | Eigen::Matrix jacobian_pose_i = Eigen::Matrix::Zero(); 66 | 67 | #ifdef USE_SO3_JACOBIAN 68 | Sophus::SO3d ri(Qi); 69 | Sophus::SO3d rp(Qp_); 70 | Sophus::SO3d res_r = rp.inverse() * ri; 71 | // http://rpg.ifi.uzh.ch/docs/RSS15_Forster.pdf 公式A.32 72 | jacobian_pose_i.block<3,3>(0,3) = Sophus::SO3d::JacobianRInv(res_r.log()); 73 | #else 74 | jacobian_pose_i.block<3,3>(0,3) = Qleft(Qp_.inverse() * Qi).bottomRightCorner<3, 3>(); 75 | #endif 76 | jacobian_pose_i.block<3,3>(3,0) = Mat33::Identity(); 77 | 78 | jacobians_[0] = jacobian_pose_i; 79 | // std::cout << jacobian_pose_i << std::endl; 80 | } 81 | 82 | } 83 | } -------------------------------------------------------------------------------- /src/backend/edge_reprojection.cc: -------------------------------------------------------------------------------- 1 | #include "../thirdparty/Sophus/sophus/se3.hpp" 2 | #include "backend/vertex_pose.h" 3 | #include "backend/edge_reprojection.h" 4 | #include "utility/utility.h" 5 | 6 | #include 7 | 8 | namespace myslam { 9 | namespace backend { 10 | 11 | /* std::vector> verticies_; // the corresponding contex 12 | VecX residual_; // residual 13 | std::vector jacobians_; // jacobi, the dimention is residual x vertex[i] 14 | MatXX information_; // information matrix 15 | VecX observation_; // observation 16 | */ 17 | 18 | void EdgeReprojection::ComputeResidual() { 19 | // std::cout << pts_i_.transpose() <<" "<Parameters()[0]; 22 | 23 | VecX param_i = verticies_[1]->Parameters(); 24 | Qd Qi(param_i[6], param_i[3], param_i[4], param_i[5]); 25 | Vec3 Pi = param_i.head<3>(); 26 | 27 | VecX param_j = verticies_[2]->Parameters(); 28 | Qd Qj(param_j[6], param_j[3], param_j[4], param_j[5]); 29 | Vec3 Pj = param_j.head<3>(); 30 | 31 | VecX param_ext = verticies_[3]->Parameters(); 32 | Qd qic(param_ext[6], param_ext[3], param_ext[4], param_ext[5]); 33 | Vec3 tic = param_ext.head<3>(); 34 | 35 | Vec3 pts_camera_i = pts_i_ / inv_dep_i; 36 | Vec3 pts_imu_i = qic * pts_camera_i + tic; 37 | Vec3 pts_w = Qi * pts_imu_i + Pi; 38 | Vec3 pts_imu_j = Qj.inverse() * (pts_w - Pj); 39 | Vec3 pts_camera_j = qic.inverse() * (pts_imu_j - tic); 40 | 41 | double dep_j = pts_camera_j.z(); 42 | residual_ = (pts_camera_j / dep_j).head<2>() - pts_j_.head<2>(); /// J^t * J * delta_x = - J^t * r 43 | // residual_ = information_ * residual_; // remove information here, we multi information matrix in problem solver 44 | } 45 | 46 | //void EdgeReprojection::SetTranslationImuFromCamera(Eigen::Quaterniond &qic_, Vec3 &tic_) { 47 | // qic = qic_; 48 | // tic = tic_; 49 | //} 50 | 51 | void EdgeReprojection::ComputeJacobians() { 52 | double inv_dep_i = verticies_[0]->Parameters()[0]; 53 | 54 | VecX param_i = verticies_[1]->Parameters(); 55 | Qd Qi(param_i[6], param_i[3], param_i[4], param_i[5]); 56 | Vec3 Pi = param_i.head<3>(); 57 | 58 | VecX param_j = verticies_[2]->Parameters(); 59 | Qd Qj(param_j[6], param_j[3], param_j[4], param_j[5]); 60 | Vec3 Pj = param_j.head<3>(); 61 | 62 | VecX param_ext = verticies_[3]->Parameters(); 63 | Qd qic(param_ext[6], param_ext[3], param_ext[4], param_ext[5]); 64 | Vec3 tic = param_ext.head<3>(); 65 | 66 | Vec3 pts_camera_i = pts_i_ / inv_dep_i; 67 | Vec3 pts_imu_i = qic * pts_camera_i + tic; 68 | Vec3 pts_w = Qi * pts_imu_i + Pi; 69 | Vec3 pts_imu_j = Qj.inverse() * (pts_w - Pj); 70 | Vec3 pts_camera_j = qic.inverse() * (pts_imu_j - tic); 71 | 72 | double dep_j = pts_camera_j.z(); 73 | 74 | Mat33 Ri = Qi.toRotationMatrix(); 75 | Mat33 Rj = Qj.toRotationMatrix(); 76 | Mat33 ric = qic.toRotationMatrix(); 77 | Mat23 reduce(2, 3); 78 | reduce << 1. / dep_j, 0, -pts_camera_j(0) / (dep_j * dep_j), 79 | 0, 1. / dep_j, -pts_camera_j(1) / (dep_j * dep_j); 80 | // reduce = information_ * reduce; 81 | 82 | Eigen::Matrix jacobian_pose_i; 83 | Eigen::Matrix jaco_i; 84 | jaco_i.leftCols<3>() = ric.transpose() * Rj.transpose(); 85 | jaco_i.rightCols<3>() = ric.transpose() * Rj.transpose() * Ri * -Sophus::SO3d::hat(pts_imu_i); 86 | jacobian_pose_i.leftCols<6>() = reduce * jaco_i; 87 | 88 | Eigen::Matrix jacobian_pose_j; 89 | Eigen::Matrix jaco_j; 90 | jaco_j.leftCols<3>() = ric.transpose() * -Rj.transpose(); 91 | jaco_j.rightCols<3>() = ric.transpose() * Sophus::SO3d::hat(pts_imu_j); 92 | jacobian_pose_j.leftCols<6>() = reduce * jaco_j; 93 | 94 | Eigen::Vector2d jacobian_feature; 95 | jacobian_feature = reduce * ric.transpose() * Rj.transpose() * Ri * ric * pts_i_ * -1.0 / (inv_dep_i * inv_dep_i); 96 | 97 | Eigen::Matrix jacobian_ex_pose; 98 | Eigen::Matrix jaco_ex; 99 | jaco_ex.leftCols<3>() = ric.transpose() * (Rj.transpose() * Ri - Eigen::Matrix3d::Identity()); 100 | Eigen::Matrix3d tmp_r = ric.transpose() * Rj.transpose() * Ri * ric; 101 | jaco_ex.rightCols<3>() = -tmp_r * Utility::skewSymmetric(pts_camera_i) + Utility::skewSymmetric(tmp_r * pts_camera_i) + 102 | Utility::skewSymmetric(ric.transpose() * (Rj.transpose() * (Ri * tic + Pi - Pj) - tic)); 103 | jacobian_ex_pose.leftCols<6>() = reduce * jaco_ex; 104 | 105 | jacobians_[0] = jacobian_feature; 106 | jacobians_[1] = jacobian_pose_i; 107 | jacobians_[2] = jacobian_pose_j; 108 | jacobians_[3] = jacobian_ex_pose; 109 | 110 | ///------------- check jacobians ----------------- 111 | // { 112 | // std::cout << jacobians_[0] <() - pts_j_.head<2>(); 124 | // tmp_residual = information_ * tmp_residual; 125 | // std::cout <<"num jacobian: "<< (tmp_residual - residual_) / eps <Parameters(); 132 | 133 | VecX param_i = verticies_[1]->Parameters(); 134 | Qd Qi(param_i[6], param_i[3], param_i[4], param_i[5]); 135 | Vec3 Pi = param_i.head<3>(); 136 | 137 | Vec3 pts_imu_i = Qi.inverse() * (pts_w - Pi); 138 | Vec3 pts_camera_i = qic.inverse() * (pts_imu_i - tic); 139 | 140 | double dep_i = pts_camera_i.z(); 141 | residual_ = (pts_camera_i / dep_i).head<2>() - obs_.head<2>(); 142 | } 143 | 144 | void EdgeReprojectionXYZ::SetTranslationImuFromCamera(Eigen::Quaterniond &qic_, Vec3 &tic_) { 145 | qic = qic_; 146 | tic = tic_; 147 | } 148 | 149 | void EdgeReprojectionXYZ::ComputeJacobians() { 150 | 151 | Vec3 pts_w = verticies_[0]->Parameters(); 152 | 153 | VecX param_i = verticies_[1]->Parameters(); 154 | Qd Qi(param_i[6], param_i[3], param_i[4], param_i[5]); 155 | Vec3 Pi = param_i.head<3>(); 156 | 157 | Vec3 pts_imu_i = Qi.inverse() * (pts_w - Pi); 158 | Vec3 pts_camera_i = qic.inverse() * (pts_imu_i - tic); 159 | 160 | double dep_i = pts_camera_i.z(); 161 | 162 | Mat33 Ri = Qi.toRotationMatrix(); 163 | Mat33 ric = qic.toRotationMatrix(); 164 | Mat23 reduce(2, 3); 165 | reduce << 1. / dep_i, 0, -pts_camera_i(0) / (dep_i * dep_i), 166 | 0, 1. / dep_i, -pts_camera_i(1) / (dep_i * dep_i); 167 | 168 | Eigen::Matrix jacobian_pose_i; 169 | Eigen::Matrix jaco_i; 170 | jaco_i.leftCols<3>() = ric.transpose() * -Ri.transpose(); 171 | jaco_i.rightCols<3>() = ric.transpose() * Sophus::SO3d::hat(pts_imu_i); 172 | jacobian_pose_i.leftCols<6>() = reduce * jaco_i; 173 | 174 | Eigen::Matrix jacobian_feature; 175 | jacobian_feature = reduce * ric.transpose() * Ri.transpose(); 176 | 177 | jacobians_[0] = jacobian_feature; 178 | jacobians_[1] = jacobian_pose_i; 179 | 180 | } 181 | 182 | void EdgeReprojectionPoseOnly::ComputeResidual() { 183 | VecX pose_params = verticies_[0]->Parameters(); 184 | Sophus::SE3d pose( 185 | Qd(pose_params[6], pose_params[3], pose_params[4], pose_params[5]), 186 | pose_params.head<3>() 187 | ); 188 | 189 | Vec3 pc = pose * landmark_world_; 190 | pc = pc / pc[2]; 191 | Vec2 pixel = (K_ * pc).head<2>() - observation_; 192 | // TODO:: residual_ = ???? 193 | residual_ = pixel; 194 | } 195 | 196 | void EdgeReprojectionPoseOnly::ComputeJacobians() { 197 | // TODO implement jacobian here 198 | } 199 | 200 | } 201 | } -------------------------------------------------------------------------------- /src/backend/imu_integration.cc: -------------------------------------------------------------------------------- 1 | // 2 | // Created by gaoxiang19 on 19-1-7. 3 | // 4 | #include "backend/imu_integration.h" 5 | 6 | using Sophus::SO3d; 7 | 8 | namespace myslam { 9 | namespace backend { 10 | 11 | void IMUIntegration::Propagate(double dt, const Vec3 &acc, const Vec3 &gyr) { 12 | dt_buf_.emplace_back(dt); 13 | acc_buf_.emplace_back(acc); 14 | gyr_buf_.emplace_back(gyr); 15 | 16 | Sophus::SO3d dR = Sophus::SO3d::exp((gyr - bg_) * dt); 17 | delta_r_ = delta_r_ * dR; 18 | delta_v_ += delta_r_ * (acc - ba_) * dt; 19 | delta_p_ += delta_v_ * dt + 0.5 * (delta_r_ * (acc - ba_) * dt * dt); 20 | sum_dt_ += dt; 21 | 22 | // update jacobians w.r.t. bg and ba 23 | dr_dbg_ -= delta_r_.inverse().matrix() * SO3d::JacobianR(((gyr - bg_) * dt)) * dt; 24 | dv_dba_ -= delta_r_.matrix() * dt; 25 | dv_dbg_ -= delta_r_.matrix() * SO3d::hat(acc - ba_) * dr_dbg_ * dt; 26 | dp_dba_ += dv_dba_ * dt - 0.5 * delta_r_.matrix() * dt * dt; 27 | dp_dbg_ += dv_dbg_ * dt - 0.5 * delta_r_.matrix() * SO3d::hat(acc - ba_) * dr_dbg_ * dt * dt; 28 | 29 | // propagate noise 30 | A_.block<3, 3>(0, 0) = dR.inverse().matrix(); 31 | B_.block<3, 3>(0, 0) = SO3d::JacobianR(dR.log()); 32 | 33 | A_.block<3, 3>(3, 0) = -delta_r_.matrix() * SO3d::hat(acc - ba_) * dt; 34 | A_.block<3, 3>(3, 3) = Mat33::Identity(); 35 | B_.block<3, 3>(3, 3) = delta_r_.matrix() * dt; 36 | 37 | A_.block<3, 3>(6, 0) = -0.5 * delta_r_.matrix() * SO3d::hat(acc - ba_) * dt * dt; 38 | A_.block<3, 3>(6, 3) = Mat33::Identity() * dt; 39 | A_.block<3, 3>(6, 6) = Mat33::Identity(); 40 | B_.block<3, 3>(6, 3) = 0.5 * delta_r_.matrix() * dt * dt; 41 | 42 | covariance_measurement_ = A_ * covariance_measurement_ * A_.transpose() + B_ * noise_measurement_ * B_.transpose(); 43 | } 44 | 45 | void IMUIntegration::Repropagate() { 46 | // backup imu data 47 | auto dt = dt_buf_; 48 | auto acc_buf = acc_buf_; 49 | auto gyr_buf = gyr_buf_; 50 | Reset(); 51 | 52 | for (size_t i = 0; i < dt.size(); ++i) { 53 | Propagate(dt[i], acc_buf[i], gyr_buf[i]); 54 | } 55 | } 56 | 57 | void IMUIntegration::Correct(const Vec3 &delta_ba, const Vec3 &delta_bg) { 58 | delta_r_ = delta_r_ * SO3d::exp(dr_dbg_ * delta_bg); 59 | delta_v_ += dv_dba_ * delta_ba + dv_dbg_ * delta_bg; 60 | delta_p_ += dp_dba_ * delta_ba + dp_dbg_ * delta_bg; 61 | } 62 | 63 | } 64 | } -------------------------------------------------------------------------------- /src/backend/loss_function.cc: -------------------------------------------------------------------------------- 1 | // 2 | // Created by gaoxiang19 on 11/10/18. 3 | // 4 | 5 | #include "backend/loss_function.h" 6 | 7 | namespace myslam { 8 | namespace backend { 9 | 10 | void HuberLoss::Compute(double e, Eigen::Vector3d& rho) const { 11 | double dsqr = delta_ * delta_; 12 | if (e <= dsqr) { // inlier 13 | rho[0] = e; 14 | rho[1] = 1.; 15 | rho[2] = 0.; 16 | } else { // outlier 17 | double sqrte = sqrt(e); // absolut value of the error 18 | rho[0] = 2*sqrte*delta_ - dsqr; // rho(e) = 2 * delta * e^(1/2) - delta^2 19 | rho[1] = delta_ / sqrte; // rho'(e) = delta / sqrt(e) 20 | rho[2] = - 0.5 * rho[1] / e; // rho''(e) = -1 / (2*e^(3/2)) = -1/2 * (delta/e) / e 21 | } 22 | } 23 | 24 | void CauchyLoss::Compute(double err2, Eigen::Vector3d& rho) const { 25 | double dsqr = delta_ * delta_; // c^2 26 | double dsqrReci = 1. / dsqr; // 1/c^2 27 | double aux = dsqrReci * err2 + 1.0; // 1 + e^2/c^2 28 | rho[0] = dsqr * log(aux); // c^2 * log( 1 + e^2/c^2 ) 29 | rho[1] = 1. / aux; // rho' 30 | rho[2] = -dsqrReci * std::pow(rho[1], 2); // rho'' 31 | } 32 | 33 | void TukeyLoss::Compute(double e2, Eigen::Vector3d& rho) const 34 | { 35 | const double e = sqrt(e2); 36 | const double delta2 = delta_ * delta_; 37 | if (e <= delta_) { 38 | const double aux = e2 / delta2; 39 | rho[0] = delta2 * (1. - std::pow((1. - aux), 3)) / 3.; 40 | rho[1] = std::pow((1. - aux), 2); 41 | rho[2] = -2. * (1. - aux) / delta2; 42 | } else { 43 | rho[0] = delta2 / 3.; 44 | rho[1] = 0; 45 | rho[2] = 0; 46 | } 47 | } 48 | } 49 | } 50 | -------------------------------------------------------------------------------- /src/backend/vertex.cc: -------------------------------------------------------------------------------- 1 | #include "backend/vertex.h" 2 | #include 3 | 4 | namespace myslam { 5 | namespace backend { 6 | 7 | unsigned long global_vertex_id = 0; 8 | 9 | Vertex::Vertex(int num_dimension, int local_dimension) { 10 | parameters_.resize(num_dimension, 1); 11 | local_dimension_ = local_dimension > 0 ? local_dimension : num_dimension; 12 | id_ = global_vertex_id++; 13 | 14 | // std::cout << "Vertex construct num_dimension: " << num_dimension 15 | // << " local_dimension: " << local_dimension << " id_: " << id_ << std::endl; 16 | } 17 | 18 | Vertex::~Vertex() {} 19 | 20 | int Vertex::Dimension() const { 21 | return parameters_.rows(); 22 | } 23 | 24 | int Vertex::LocalDimension() const { 25 | return local_dimension_; 26 | } 27 | 28 | void Vertex::Plus(const VecX &delta) { 29 | parameters_ += delta; 30 | } 31 | 32 | } 33 | } -------------------------------------------------------------------------------- /src/backend/vertex_pose.cc: -------------------------------------------------------------------------------- 1 | #include "backend/vertex_pose.h" 2 | #include "../thirdparty/Sophus/sophus/se3.hpp" 3 | //#include 4 | namespace myslam { 5 | namespace backend { 6 | 7 | void VertexPose::Plus(const VecX &delta) { 8 | VecX ¶meters = Parameters(); 9 | parameters.head<3>() += delta.head<3>(); 10 | Qd q(parameters[6], parameters[3], parameters[4], parameters[5]); 11 | q = q * Sophus::SO3d::exp(Vec3(delta[3], delta[4], delta[5])).unit_quaternion(); // right multiplication with so3 12 | q.normalized(); 13 | parameters[3] = q.x(); 14 | parameters[4] = q.y(); 15 | parameters[5] = q.z(); 16 | parameters[6] = q.w(); 17 | // Qd test = Sophus::SO3d::exp(Vec3(0.2, 0.1, 0.1)).unit_quaternion() * Sophus::SO3d::exp(-Vec3(0.2, 0.1, 0.1)).unit_quaternion(); 18 | // std::cout << test.x()<<" "<< test.y()<<" "< 5 | 6 | namespace camodocal 7 | { 8 | 9 | Camera::Parameters::Parameters(ModelType modelType) 10 | : m_modelType(modelType) 11 | , m_imageWidth(0) 12 | , m_imageHeight(0) 13 | { 14 | switch (modelType) 15 | { 16 | case KANNALA_BRANDT: 17 | m_nIntrinsics = 8; 18 | break; 19 | case PINHOLE: 20 | m_nIntrinsics = 8; 21 | break; 22 | case SCARAMUZZA: 23 | m_nIntrinsics = SCARAMUZZA_CAMERA_NUM_PARAMS; 24 | break; 25 | case MEI: 26 | default: 27 | m_nIntrinsics = 9; 28 | } 29 | } 30 | 31 | Camera::Parameters::Parameters(ModelType modelType, 32 | const std::string& cameraName, 33 | int w, int h) 34 | : m_modelType(modelType) 35 | , m_cameraName(cameraName) 36 | , m_imageWidth(w) 37 | , m_imageHeight(h) 38 | { 39 | switch (modelType) 40 | { 41 | case KANNALA_BRANDT: 42 | m_nIntrinsics = 8; 43 | break; 44 | case PINHOLE: 45 | m_nIntrinsics = 8; 46 | break; 47 | case SCARAMUZZA: 48 | m_nIntrinsics = SCARAMUZZA_CAMERA_NUM_PARAMS; 49 | break; 50 | case MEI: 51 | default: 52 | m_nIntrinsics = 9; 53 | } 54 | } 55 | 56 | Camera::ModelType& 57 | Camera::Parameters::modelType(void) 58 | { 59 | return m_modelType; 60 | } 61 | 62 | std::string& 63 | Camera::Parameters::cameraName(void) 64 | { 65 | return m_cameraName; 66 | } 67 | 68 | int& 69 | Camera::Parameters::imageWidth(void) 70 | { 71 | return m_imageWidth; 72 | } 73 | 74 | int& 75 | Camera::Parameters::imageHeight(void) 76 | { 77 | return m_imageHeight; 78 | } 79 | 80 | Camera::ModelType 81 | Camera::Parameters::modelType(void) const 82 | { 83 | return m_modelType; 84 | } 85 | 86 | const std::string& 87 | Camera::Parameters::cameraName(void) const 88 | { 89 | return m_cameraName; 90 | } 91 | 92 | int 93 | Camera::Parameters::imageWidth(void) const 94 | { 95 | return m_imageWidth; 96 | } 97 | 98 | int 99 | Camera::Parameters::imageHeight(void) const 100 | { 101 | return m_imageHeight; 102 | } 103 | 104 | int 105 | Camera::Parameters::nIntrinsics(void) const 106 | { 107 | return m_nIntrinsics; 108 | } 109 | 110 | cv::Mat& 111 | Camera::mask(void) 112 | { 113 | return m_mask; 114 | } 115 | 116 | const cv::Mat& 117 | Camera::mask(void) const 118 | { 119 | return m_mask; 120 | } 121 | 122 | void 123 | Camera::estimateExtrinsics(const std::vector& objectPoints, 124 | const std::vector& imagePoints, 125 | cv::Mat& rvec, cv::Mat& tvec) const 126 | { 127 | std::vector Ms(imagePoints.size()); 128 | for (size_t i = 0; i < Ms.size(); ++i) 129 | { 130 | Eigen::Vector3d P; 131 | liftProjective(Eigen::Vector2d(imagePoints.at(i).x, imagePoints.at(i).y), P); 132 | 133 | P /= P(2); 134 | 135 | Ms.at(i).x = P(0); 136 | Ms.at(i).y = P(1); 137 | } 138 | 139 | // assume unit focal length, zero principal point, and zero distortion 140 | cv::solvePnP(objectPoints, Ms, cv::Mat::eye(3, 3, CV_64F), cv::noArray(), rvec, tvec); 141 | } 142 | 143 | double 144 | Camera::reprojectionDist(const Eigen::Vector3d& P1, const Eigen::Vector3d& P2) const 145 | { 146 | Eigen::Vector2d p1, p2; 147 | 148 | spaceToPlane(P1, p1); 149 | spaceToPlane(P2, p2); 150 | 151 | return (p1 - p2).norm(); 152 | } 153 | 154 | double 155 | Camera::reprojectionError(const std::vector< std::vector >& objectPoints, 156 | const std::vector< std::vector >& imagePoints, 157 | const std::vector& rvecs, 158 | const std::vector& tvecs, 159 | cv::OutputArray _perViewErrors) const 160 | { 161 | int imageCount = objectPoints.size(); 162 | size_t pointsSoFar = 0; 163 | double totalErr = 0.0; 164 | 165 | bool computePerViewErrors = _perViewErrors.needed(); 166 | cv::Mat perViewErrors; 167 | if (computePerViewErrors) 168 | { 169 | _perViewErrors.create(imageCount, 1, CV_64F); 170 | perViewErrors = _perViewErrors.getMat(); 171 | } 172 | 173 | for (int i = 0; i < imageCount; ++i) 174 | { 175 | size_t pointCount = imagePoints.at(i).size(); 176 | 177 | pointsSoFar += pointCount; 178 | 179 | std::vector estImagePoints; 180 | projectPoints(objectPoints.at(i), rvecs.at(i), tvecs.at(i), 181 | estImagePoints); 182 | 183 | double err = 0.0; 184 | for (size_t j = 0; j < imagePoints.at(i).size(); ++j) 185 | { 186 | err += cv::norm(imagePoints.at(i).at(j) - estImagePoints.at(j)); 187 | } 188 | 189 | if (computePerViewErrors) 190 | { 191 | perViewErrors.at(i) = err / pointCount; 192 | } 193 | 194 | totalErr += err; 195 | } 196 | 197 | return totalErr / pointsSoFar; 198 | } 199 | 200 | double 201 | Camera::reprojectionError(const Eigen::Vector3d& P, 202 | const Eigen::Quaterniond& camera_q, 203 | const Eigen::Vector3d& camera_t, 204 | const Eigen::Vector2d& observed_p) const 205 | { 206 | Eigen::Vector3d P_cam = camera_q.toRotationMatrix() * P + camera_t; 207 | 208 | Eigen::Vector2d p; 209 | spaceToPlane(P_cam, p); 210 | 211 | return (p - observed_p).norm(); 212 | } 213 | 214 | void 215 | Camera::projectPoints(const std::vector& objectPoints, 216 | const cv::Mat& rvec, 217 | const cv::Mat& tvec, 218 | std::vector& imagePoints) const 219 | { 220 | // project 3D object points to the image plane 221 | imagePoints.reserve(objectPoints.size()); 222 | 223 | //double 224 | cv::Mat R0; 225 | cv::Rodrigues(rvec, R0); 226 | 227 | Eigen::MatrixXd R(3,3); 228 | R << R0.at(0,0), R0.at(0,1), R0.at(0,2), 229 | R0.at(1,0), R0.at(1,1), R0.at(1,2), 230 | R0.at(2,0), R0.at(2,1), R0.at(2,2); 231 | 232 | Eigen::Vector3d t; 233 | t << tvec.at(0), tvec.at(1), tvec.at(2); 234 | 235 | for (size_t i = 0; i < objectPoints.size(); ++i) 236 | { 237 | const cv::Point3f& objectPoint = objectPoints.at(i); 238 | 239 | // Rotate and translate 240 | Eigen::Vector3d P; 241 | P << objectPoint.x, objectPoint.y, objectPoint.z; 242 | 243 | P = R * P + t; 244 | 245 | Eigen::Vector2d p; 246 | spaceToPlane(P, p); 247 | 248 | imagePoints.push_back(cv::Point2f(p(0), p(1))); 249 | } 250 | } 251 | 252 | } 253 | -------------------------------------------------------------------------------- /src/camera_models/camera_models/CameraFactory.cc: -------------------------------------------------------------------------------- 1 | #include "camodocal/camera_models/CameraFactory.h" 2 | 3 | #include 4 | 5 | 6 | #include "camodocal/camera_models/CataCamera.h" 7 | #include "camodocal/camera_models/EquidistantCamera.h" 8 | #include "camodocal/camera_models/PinholeCamera.h" 9 | #include "camodocal/camera_models/ScaramuzzaCamera.h" 10 | 11 | #include "ceres/ceres.h" 12 | 13 | namespace camodocal 14 | { 15 | 16 | boost::shared_ptr CameraFactory::m_instance; 17 | 18 | CameraFactory::CameraFactory() 19 | { 20 | 21 | } 22 | 23 | boost::shared_ptr 24 | CameraFactory::instance(void) 25 | { 26 | if (m_instance.get() == 0) 27 | { 28 | m_instance.reset(new CameraFactory); 29 | } 30 | 31 | return m_instance; 32 | } 33 | 34 | CameraPtr 35 | CameraFactory::generateCamera(Camera::ModelType modelType, 36 | const std::string& cameraName, 37 | cv::Size imageSize) const 38 | { 39 | switch (modelType) 40 | { 41 | case Camera::KANNALA_BRANDT: 42 | { 43 | EquidistantCameraPtr camera(new EquidistantCamera); 44 | 45 | EquidistantCamera::Parameters params = camera->getParameters(); 46 | params.cameraName() = cameraName; 47 | params.imageWidth() = imageSize.width; 48 | params.imageHeight() = imageSize.height; 49 | camera->setParameters(params); 50 | return camera; 51 | } 52 | case Camera::PINHOLE: 53 | { 54 | PinholeCameraPtr camera(new PinholeCamera); 55 | 56 | PinholeCamera::Parameters params = camera->getParameters(); 57 | params.cameraName() = cameraName; 58 | params.imageWidth() = imageSize.width; 59 | params.imageHeight() = imageSize.height; 60 | camera->setParameters(params); 61 | return camera; 62 | } 63 | case Camera::SCARAMUZZA: 64 | { 65 | OCAMCameraPtr camera(new OCAMCamera); 66 | 67 | OCAMCamera::Parameters params = camera->getParameters(); 68 | params.cameraName() = cameraName; 69 | params.imageWidth() = imageSize.width; 70 | params.imageHeight() = imageSize.height; 71 | camera->setParameters(params); 72 | return camera; 73 | } 74 | case Camera::MEI: 75 | default: 76 | { 77 | CataCameraPtr camera(new CataCamera); 78 | 79 | CataCamera::Parameters params = camera->getParameters(); 80 | params.cameraName() = cameraName; 81 | params.imageWidth() = imageSize.width; 82 | params.imageHeight() = imageSize.height; 83 | camera->setParameters(params); 84 | return camera; 85 | } 86 | } 87 | } 88 | 89 | CameraPtr 90 | CameraFactory::generateCameraFromYamlFile(const std::string& filename) 91 | { 92 | cv::FileStorage fs(filename, cv::FileStorage::READ); 93 | 94 | if (!fs.isOpened()) 95 | { 96 | return CameraPtr(); 97 | } 98 | 99 | Camera::ModelType modelType = Camera::MEI; 100 | if (!fs["model_type"].isNone()) 101 | { 102 | std::string sModelType; 103 | fs["model_type"] >> sModelType; 104 | 105 | if (boost::iequals(sModelType, "kannala_brandt")) 106 | { 107 | modelType = Camera::KANNALA_BRANDT; 108 | } 109 | else if (boost::iequals(sModelType, "mei")) 110 | { 111 | modelType = Camera::MEI; 112 | } 113 | else if (boost::iequals(sModelType, "scaramuzza")) 114 | { 115 | modelType = Camera::SCARAMUZZA; 116 | } 117 | else if (boost::iequals(sModelType, "pinhole")) 118 | { 119 | modelType = Camera::PINHOLE; 120 | } 121 | else 122 | { 123 | std::cerr << "# ERROR: Unknown camera model: " << sModelType << std::endl; 124 | return CameraPtr(); 125 | } 126 | } 127 | 128 | switch (modelType) 129 | { 130 | case Camera::KANNALA_BRANDT: 131 | { 132 | EquidistantCameraPtr camera(new EquidistantCamera); 133 | 134 | EquidistantCamera::Parameters params = camera->getParameters(); 135 | params.readFromYamlFile(filename); 136 | camera->setParameters(params); 137 | return camera; 138 | } 139 | case Camera::PINHOLE: 140 | { 141 | PinholeCameraPtr camera(new PinholeCamera); 142 | 143 | PinholeCamera::Parameters params = camera->getParameters(); 144 | params.readFromYamlFile(filename); 145 | camera->setParameters(params); 146 | return camera; 147 | } 148 | case Camera::SCARAMUZZA: 149 | { 150 | OCAMCameraPtr camera(new OCAMCamera); 151 | 152 | OCAMCamera::Parameters params = camera->getParameters(); 153 | params.readFromYamlFile(filename); 154 | camera->setParameters(params); 155 | return camera; 156 | } 157 | case Camera::MEI: 158 | default: 159 | { 160 | CataCameraPtr camera(new CataCamera); 161 | 162 | CataCamera::Parameters params = camera->getParameters(); 163 | params.readFromYamlFile(filename); 164 | camera->setParameters(params); 165 | return camera; 166 | } 167 | } 168 | 169 | return CameraPtr(); 170 | } 171 | 172 | } 173 | 174 | -------------------------------------------------------------------------------- /src/camera_models/gpl/EigenQuaternionParameterization.cc: -------------------------------------------------------------------------------- 1 | #include "camodocal/gpl/EigenQuaternionParameterization.h" 2 | 3 | #include 4 | 5 | namespace camodocal 6 | { 7 | 8 | bool 9 | EigenQuaternionParameterization::Plus(const double* x, 10 | const double* delta, 11 | double* x_plus_delta) const 12 | { 13 | const double norm_delta = 14 | sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]); 15 | if (norm_delta > 0.0) 16 | { 17 | const double sin_delta_by_delta = (sin(norm_delta) / norm_delta); 18 | double q_delta[4]; 19 | q_delta[0] = sin_delta_by_delta * delta[0]; 20 | q_delta[1] = sin_delta_by_delta * delta[1]; 21 | q_delta[2] = sin_delta_by_delta * delta[2]; 22 | q_delta[3] = cos(norm_delta); 23 | EigenQuaternionProduct(q_delta, x, x_plus_delta); 24 | } 25 | else 26 | { 27 | for (int i = 0; i < 4; ++i) 28 | { 29 | x_plus_delta[i] = x[i]; 30 | } 31 | } 32 | return true; 33 | } 34 | 35 | bool 36 | EigenQuaternionParameterization::ComputeJacobian(const double* x, 37 | double* jacobian) const 38 | { 39 | jacobian[0] = x[3]; jacobian[1] = x[2]; jacobian[2] = -x[1]; // NOLINT 40 | jacobian[3] = -x[2]; jacobian[4] = x[3]; jacobian[5] = x[0]; // NOLINT 41 | jacobian[6] = x[1]; jacobian[7] = -x[0]; jacobian[8] = x[3]; // NOLINT 42 | jacobian[9] = -x[0]; jacobian[10] = -x[1]; jacobian[11] = -x[2]; // NOLINT 43 | return true; 44 | } 45 | 46 | } 47 | -------------------------------------------------------------------------------- /src/camera_models/intrinsic_calib.cc: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include "camodocal/chessboard/Chessboard.h" 12 | #include "camodocal/calib/CameraCalibration.h" 13 | #include "camodocal/gpl/gpl.h" 14 | 15 | int main(int argc, char** argv) 16 | { 17 | cv::Size boardSize; 18 | float squareSize; 19 | std::string inputDir; 20 | std::string cameraModel; 21 | std::string cameraName; 22 | std::string prefix; 23 | std::string fileExtension; 24 | bool useOpenCV; 25 | bool viewResults; 26 | bool verbose; 27 | 28 | //========= Handling Program options ========= 29 | boost::program_options::options_description desc("Allowed options"); 30 | desc.add_options() 31 | ("help", "produce help message") 32 | ("width,w", boost::program_options::value(&boardSize.width)->default_value(8), "Number of inner corners on the chessboard pattern in x direction") 33 | ("height,h", boost::program_options::value(&boardSize.height)->default_value(12), "Number of inner corners on the chessboard pattern in y direction") 34 | ("size,s", boost::program_options::value(&squareSize)->default_value(7.f), "Size of one square in mm") 35 | ("input,i", boost::program_options::value(&inputDir)->default_value("calibrationdata"), "Input directory containing chessboard images") 36 | ("prefix,p", boost::program_options::value(&prefix)->default_value("left-"), "Prefix of images") 37 | ("file-extension,e", boost::program_options::value(&fileExtension)->default_value(".png"), "File extension of images") 38 | ("camera-model", boost::program_options::value(&cameraModel)->default_value("mei"), "Camera model: kannala-brandt | mei | pinhole") 39 | ("camera-name", boost::program_options::value(&cameraName)->default_value("camera"), "Name of camera") 40 | ("opencv", boost::program_options::bool_switch(&useOpenCV)->default_value(true), "Use OpenCV to detect corners") 41 | ("view-results", boost::program_options::bool_switch(&viewResults)->default_value(false), "View results") 42 | ("verbose,v", boost::program_options::bool_switch(&verbose)->default_value(true), "Verbose output") 43 | ; 44 | 45 | boost::program_options::positional_options_description pdesc; 46 | pdesc.add("input", 1); 47 | 48 | boost::program_options::variables_map vm; 49 | boost::program_options::store(boost::program_options::command_line_parser(argc, argv).options(desc).positional(pdesc).run(), vm); 50 | boost::program_options::notify(vm); 51 | 52 | if (vm.count("help")) 53 | { 54 | std::cout << desc << std::endl; 55 | return 1; 56 | } 57 | 58 | if (!boost::filesystem::exists(inputDir) && !boost::filesystem::is_directory(inputDir)) 59 | { 60 | std::cerr << "# ERROR: Cannot find input directory " << inputDir << "." << std::endl; 61 | return 1; 62 | } 63 | 64 | camodocal::Camera::ModelType modelType; 65 | if (boost::iequals(cameraModel, "kannala-brandt")) 66 | { 67 | modelType = camodocal::Camera::KANNALA_BRANDT; 68 | } 69 | else if (boost::iequals(cameraModel, "mei")) 70 | { 71 | modelType = camodocal::Camera::MEI; 72 | } 73 | else if (boost::iequals(cameraModel, "pinhole")) 74 | { 75 | modelType = camodocal::Camera::PINHOLE; 76 | } 77 | else if (boost::iequals(cameraModel, "scaramuzza")) 78 | { 79 | modelType = camodocal::Camera::SCARAMUZZA; 80 | } 81 | else 82 | { 83 | std::cerr << "# ERROR: Unknown camera model: " << cameraModel << std::endl; 84 | return 1; 85 | } 86 | 87 | switch (modelType) 88 | { 89 | case camodocal::Camera::KANNALA_BRANDT: 90 | std::cout << "# INFO: Camera model: Kannala-Brandt" << std::endl; 91 | break; 92 | case camodocal::Camera::MEI: 93 | std::cout << "# INFO: Camera model: Mei" << std::endl; 94 | break; 95 | case camodocal::Camera::PINHOLE: 96 | std::cout << "# INFO: Camera model: Pinhole" << std::endl; 97 | break; 98 | case camodocal::Camera::SCARAMUZZA: 99 | std::cout << "# INFO: Camera model: Scaramuzza-Omnidirect" << std::endl; 100 | break; 101 | } 102 | 103 | // look for images in input directory 104 | std::vector imageFilenames; 105 | boost::filesystem::directory_iterator itr; 106 | for (boost::filesystem::directory_iterator itr(inputDir); itr != boost::filesystem::directory_iterator(); ++itr) 107 | { 108 | if (!boost::filesystem::is_regular_file(itr->status())) 109 | { 110 | continue; 111 | } 112 | 113 | std::string filename = itr->path().filename().string(); 114 | 115 | // check if prefix matches 116 | if (!prefix.empty()) 117 | { 118 | if (filename.compare(0, prefix.length(), prefix) != 0) 119 | { 120 | continue; 121 | } 122 | } 123 | 124 | // check if file extension matches 125 | if (filename.compare(filename.length() - fileExtension.length(), fileExtension.length(), fileExtension) != 0) 126 | { 127 | continue; 128 | } 129 | 130 | imageFilenames.push_back(itr->path().string()); 131 | 132 | if (verbose) 133 | { 134 | std::cerr << "# INFO: Adding " << imageFilenames.back() << std::endl; 135 | } 136 | } 137 | 138 | if (imageFilenames.empty()) 139 | { 140 | std::cerr << "# ERROR: No chessboard images found." << std::endl; 141 | return 1; 142 | } 143 | 144 | if (verbose) 145 | { 146 | std::cerr << "# INFO: # images: " << imageFilenames.size() << std::endl; 147 | } 148 | 149 | std::sort(imageFilenames.begin(), imageFilenames.end()); 150 | 151 | cv::Mat image = cv::imread(imageFilenames.front(), -1); 152 | const cv::Size frameSize = image.size(); 153 | 154 | camodocal::CameraCalibration calibration(modelType, cameraName, frameSize, boardSize, squareSize); 155 | calibration.setVerbose(verbose); 156 | 157 | std::vector chessboardFound(imageFilenames.size(), false); 158 | for (size_t i = 0; i < imageFilenames.size(); ++i) 159 | { 160 | image = cv::imread(imageFilenames.at(i), -1); 161 | 162 | camodocal::Chessboard chessboard(boardSize, image); 163 | 164 | chessboard.findCorners(useOpenCV); 165 | if (chessboard.cornersFound()) 166 | { 167 | if (verbose) 168 | { 169 | std::cerr << "# INFO: Detected chessboard in image " << i + 1 << ", " << imageFilenames.at(i) << std::endl; 170 | } 171 | 172 | calibration.addChessboardData(chessboard.getCorners()); 173 | 174 | cv::Mat sketch; 175 | chessboard.getSketch().copyTo(sketch); 176 | 177 | cv::imshow("Image", sketch); 178 | cv::waitKey(50); 179 | } 180 | else if (verbose) 181 | { 182 | std::cerr << "# INFO: Did not detect chessboard in image " << i + 1 << std::endl; 183 | } 184 | chessboardFound.at(i) = chessboard.cornersFound(); 185 | } 186 | cv::destroyWindow("Image"); 187 | 188 | if (calibration.sampleCount() < 10) 189 | { 190 | std::cerr << "# ERROR: Insufficient number of detected chessboards." << std::endl; 191 | return 1; 192 | } 193 | 194 | if (verbose) 195 | { 196 | std::cerr << "# INFO: Calibrating..." << std::endl; 197 | } 198 | 199 | double startTime = camodocal::timeInSeconds(); 200 | 201 | calibration.calibrate(); 202 | calibration.writeParams(cameraName + "_camera_calib.yaml"); 203 | calibration.writeChessboardData(cameraName + "_chessboard_data.dat"); 204 | 205 | if (verbose) 206 | { 207 | std::cout << "# INFO: Calibration took a total time of " 208 | << std::fixed << std::setprecision(3) << camodocal::timeInSeconds() - startTime 209 | << " sec.\n"; 210 | } 211 | 212 | if (verbose) 213 | { 214 | std::cerr << "# INFO: Wrote calibration file to " << cameraName + "_camera_calib.yaml" << std::endl; 215 | } 216 | 217 | if (viewResults) 218 | { 219 | std::vector cbImages; 220 | std::vector cbImageFilenames; 221 | 222 | for (size_t i = 0; i < imageFilenames.size(); ++i) 223 | { 224 | if (!chessboardFound.at(i)) 225 | { 226 | continue; 227 | } 228 | 229 | cbImages.push_back(cv::imread(imageFilenames.at(i), -1)); 230 | cbImageFilenames.push_back(imageFilenames.at(i)); 231 | } 232 | 233 | // visualize observed and reprojected points 234 | calibration.drawResults(cbImages); 235 | 236 | for (size_t i = 0; i < cbImages.size(); ++i) 237 | { 238 | cv::putText(cbImages.at(i), cbImageFilenames.at(i), cv::Point(10,20), 239 | cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(255, 255, 255), 240 | 1, CV_AA); 241 | cv::imshow("Image", cbImages.at(i)); 242 | cv::waitKey(0); 243 | } 244 | } 245 | 246 | return 0; 247 | } 248 | -------------------------------------------------------------------------------- /src/camera_models/sparse_graph/Transform.cc: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace camodocal 4 | { 5 | 6 | Transform::Transform() 7 | { 8 | m_q.setIdentity(); 9 | m_t.setZero(); 10 | } 11 | 12 | Transform::Transform(const Eigen::Matrix4d& H) 13 | { 14 | m_q = Eigen::Quaterniond(H.block<3,3>(0,0)); 15 | m_t = H.block<3,1>(0,3); 16 | } 17 | 18 | Eigen::Quaterniond& 19 | Transform::rotation(void) 20 | { 21 | return m_q; 22 | } 23 | 24 | const Eigen::Quaterniond& 25 | Transform::rotation(void) const 26 | { 27 | return m_q; 28 | } 29 | 30 | double* 31 | Transform::rotationData(void) 32 | { 33 | return m_q.coeffs().data(); 34 | } 35 | 36 | const double* const 37 | Transform::rotationData(void) const 38 | { 39 | return m_q.coeffs().data(); 40 | } 41 | 42 | Eigen::Vector3d& 43 | Transform::translation(void) 44 | { 45 | return m_t; 46 | } 47 | 48 | const Eigen::Vector3d& 49 | Transform::translation(void) const 50 | { 51 | return m_t; 52 | } 53 | 54 | double* 55 | Transform::translationData(void) 56 | { 57 | return m_t.data(); 58 | } 59 | 60 | const double* const 61 | Transform::translationData(void) const 62 | { 63 | return m_t.data(); 64 | } 65 | 66 | Eigen::Matrix4d 67 | Transform::toMatrix(void) const 68 | { 69 | Eigen::Matrix4d H; 70 | H.setIdentity(); 71 | H.block<3,3>(0,0) = m_q.toRotationMatrix(); 72 | H.block<3,1>(0,3) = m_t; 73 | 74 | return H; 75 | } 76 | 77 | } 78 | -------------------------------------------------------------------------------- /src/feature_tracker_parameters.cpp: -------------------------------------------------------------------------------- 1 | #include "parameters.h" 2 | 3 | std::string IMAGE_TOPIC; 4 | std::string IMU_TOPIC; 5 | std::vector CAM_NAMES; 6 | std::string FISHEYE_MASK; 7 | int MAX_CNT; 8 | int MIN_DIST; 9 | int WINDOW_SIZE; 10 | int FREQ; 11 | double F_THRESHOLD; 12 | int SHOW_TRACK; 13 | int STEREO_TRACK; 14 | int EQUALIZE; 15 | int ROW; 16 | int COL; 17 | int FOCAL_LENGTH; 18 | int FISHEYE; 19 | bool PUB_THIS_FRAME; 20 | 21 | template 22 | T readParam(ros::NodeHandle &n, std::string name) 23 | { 24 | T ans; 25 | if (n.getParam(name, ans)) 26 | { 27 | // ROS_INFO_STREAM("Loaded " << name << ": " << ans); 28 | } 29 | else 30 | { 31 | // ROS_ERROR_STREAM("Failed to load " << name); 32 | n.shutdown(); 33 | } 34 | return ans; 35 | } 36 | 37 | void readParameters(ros::NodeHandle &n) 38 | { 39 | std::string config_file; 40 | config_file = readParam(n, "config_file"); 41 | cv::FileStorage fsSettings(config_file, cv::FileStorage::READ); 42 | if(!fsSettings.isOpened()) 43 | { 44 | std::cerr << "ERROR: Wrong path to settings" << std::endl; 45 | } 46 | std::string VINS_FOLDER_PATH = readParam(n, "vins_folder"); 47 | 48 | fsSettings["image_topic"] >> IMAGE_TOPIC; 49 | fsSettings["imu_topic"] >> IMU_TOPIC; 50 | MAX_CNT = fsSettings["max_cnt"]; 51 | MIN_DIST = fsSettings["min_dist"]; 52 | ROW = fsSettings["image_height"]; 53 | COL = fsSettings["image_width"]; 54 | FREQ = fsSettings["freq"]; 55 | F_THRESHOLD = fsSettings["F_threshold"]; 56 | SHOW_TRACK = fsSettings["show_track"]; 57 | EQUALIZE = fsSettings["equalize"]; 58 | FISHEYE = fsSettings["fisheye"]; 59 | if (FISHEYE == 1) 60 | FISHEYE_MASK = VINS_FOLDER_PATH + "config/fisheye_mask.jpg"; 61 | CAM_NAMES.push_back(config_file); 62 | 63 | WINDOW_SIZE = 20; 64 | STEREO_TRACK = false; 65 | FOCAL_LENGTH = 460; 66 | PUB_THIS_FRAME = false; 67 | 68 | if (FREQ == 0) 69 | FREQ = 100; 70 | 71 | fsSettings.release(); 72 | 73 | 74 | } 75 | -------------------------------------------------------------------------------- /src/initial/initial_aligment.cpp: -------------------------------------------------------------------------------- 1 | #include "initial/initial_alignment.h" 2 | 3 | void solveGyroscopeBias(map &all_image_frame, Vector3d* Bgs) 4 | { 5 | Matrix3d A; 6 | Vector3d b; 7 | Vector3d delta_bg; 8 | A.setZero(); 9 | b.setZero(); 10 | map::iterator frame_i; 11 | map::iterator frame_j; 12 | for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end(); frame_i++) 13 | { 14 | frame_j = next(frame_i); 15 | MatrixXd tmp_A(3, 3); 16 | tmp_A.setZero(); 17 | VectorXd tmp_b(3); 18 | tmp_b.setZero(); 19 | Eigen::Quaterniond q_ij(frame_i->second.R.transpose() * frame_j->second.R); 20 | tmp_A = frame_j->second.pre_integration->jacobian.template block<3, 3>(O_R, O_BG); 21 | tmp_b = 2 * (frame_j->second.pre_integration->delta_q.inverse() * q_ij).vec(); 22 | A += tmp_A.transpose() * tmp_A; 23 | b += tmp_A.transpose() * tmp_b; 24 | 25 | } 26 | delta_bg = A.ldlt().solve(b); 27 | // ROS_WARN_STREAM("gyroscope bias initial calibration " << delta_bg.transpose()); 28 | 29 | for (int i = 0; i <= WINDOW_SIZE; i++) 30 | Bgs[i] += delta_bg; 31 | 32 | for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end( ); frame_i++) 33 | { 34 | frame_j = next(frame_i); 35 | frame_j->second.pre_integration->repropagate(Vector3d::Zero(), Bgs[0]); 36 | } 37 | } 38 | 39 | 40 | MatrixXd TangentBasis(Vector3d &g0) 41 | { 42 | Vector3d b, c; 43 | Vector3d a = g0.normalized(); 44 | Vector3d tmp(0, 0, 1); 45 | if(a == tmp) 46 | tmp << 1, 0, 0; 47 | b = (tmp - a * (a.transpose() * tmp)).normalized(); 48 | c = a.cross(b); 49 | MatrixXd bc(3, 2); 50 | bc.block<3, 1>(0, 0) = b; 51 | bc.block<3, 1>(0, 1) = c; 52 | return bc; 53 | } 54 | 55 | void RefineGravity(map &all_image_frame, Vector3d &g, VectorXd &x) 56 | { 57 | Vector3d g0 = g.normalized() * G.norm(); 58 | Vector3d lx, ly; 59 | //VectorXd x; 60 | int all_frame_count = all_image_frame.size(); 61 | int n_state = all_frame_count * 3 + 2 + 1; 62 | 63 | MatrixXd A{n_state, n_state}; 64 | A.setZero(); 65 | VectorXd b{n_state}; 66 | b.setZero(); 67 | 68 | map::iterator frame_i; 69 | map::iterator frame_j; 70 | for(int k = 0; k < 4; k++) 71 | { 72 | MatrixXd lxly(3, 2); 73 | lxly = TangentBasis(g0); 74 | int i = 0; 75 | for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end(); frame_i++, i++) 76 | { 77 | frame_j = next(frame_i); 78 | 79 | MatrixXd tmp_A(6, 9); 80 | tmp_A.setZero(); 81 | VectorXd tmp_b(6); 82 | tmp_b.setZero(); 83 | 84 | double dt = frame_j->second.pre_integration->sum_dt; 85 | 86 | 87 | tmp_A.block<3, 3>(0, 0) = -dt * Matrix3d::Identity(); 88 | tmp_A.block<3, 2>(0, 6) = frame_i->second.R.transpose() * dt * dt / 2 * Matrix3d::Identity() * lxly; 89 | tmp_A.block<3, 1>(0, 8) = frame_i->second.R.transpose() * (frame_j->second.T - frame_i->second.T) / 100.0; 90 | tmp_b.block<3, 1>(0, 0) = frame_j->second.pre_integration->delta_p + frame_i->second.R.transpose() * frame_j->second.R * TIC[0] - TIC[0] - frame_i->second.R.transpose() * dt * dt / 2 * g0; 91 | 92 | tmp_A.block<3, 3>(3, 0) = -Matrix3d::Identity(); 93 | tmp_A.block<3, 3>(3, 3) = frame_i->second.R.transpose() * frame_j->second.R; 94 | tmp_A.block<3, 2>(3, 6) = frame_i->second.R.transpose() * dt * Matrix3d::Identity() * lxly; 95 | tmp_b.block<3, 1>(3, 0) = frame_j->second.pre_integration->delta_v - frame_i->second.R.transpose() * dt * Matrix3d::Identity() * g0; 96 | 97 | 98 | Matrix cov_inv = Matrix::Zero(); 99 | //cov.block<6, 6>(0, 0) = IMU_cov[i + 1]; 100 | //MatrixXd cov_inv = cov.inverse(); 101 | cov_inv.setIdentity(); 102 | 103 | MatrixXd r_A = tmp_A.transpose() * cov_inv * tmp_A; 104 | VectorXd r_b = tmp_A.transpose() * cov_inv * tmp_b; 105 | 106 | A.block<6, 6>(i * 3, i * 3) += r_A.topLeftCorner<6, 6>(); 107 | b.segment<6>(i * 3) += r_b.head<6>(); 108 | 109 | A.bottomRightCorner<3, 3>() += r_A.bottomRightCorner<3, 3>(); 110 | b.tail<3>() += r_b.tail<3>(); 111 | 112 | A.block<6, 3>(i * 3, n_state - 3) += r_A.topRightCorner<6, 3>(); 113 | A.block<3, 6>(n_state - 3, i * 3) += r_A.bottomLeftCorner<3, 6>(); 114 | } 115 | A = A * 1000.0; 116 | b = b * 1000.0; 117 | x = A.ldlt().solve(b); 118 | VectorXd dg = x.segment<2>(n_state - 3); 119 | g0 = (g0 + lxly * dg).normalized() * G.norm(); 120 | //double s = x(n_state - 1); 121 | } 122 | g = g0; 123 | } 124 | 125 | bool LinearAlignment(map &all_image_frame, Vector3d &g, VectorXd &x) 126 | { 127 | int all_frame_count = all_image_frame.size(); 128 | int n_state = all_frame_count * 3 + 3 + 1; 129 | 130 | MatrixXd A{n_state, n_state}; 131 | A.setZero(); 132 | VectorXd b{n_state}; 133 | b.setZero(); 134 | 135 | map::iterator frame_i; 136 | map::iterator frame_j; 137 | int i = 0; 138 | for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end(); frame_i++, i++) 139 | { 140 | frame_j = next(frame_i); 141 | 142 | MatrixXd tmp_A(6, 10); 143 | tmp_A.setZero(); 144 | VectorXd tmp_b(6); 145 | tmp_b.setZero(); 146 | 147 | double dt = frame_j->second.pre_integration->sum_dt; 148 | 149 | tmp_A.block<3, 3>(0, 0) = -dt * Matrix3d::Identity(); 150 | tmp_A.block<3, 3>(0, 6) = frame_i->second.R.transpose() * dt * dt / 2 * Matrix3d::Identity(); 151 | tmp_A.block<3, 1>(0, 9) = frame_i->second.R.transpose() * (frame_j->second.T - frame_i->second.T) / 100.0; 152 | tmp_b.block<3, 1>(0, 0) = frame_j->second.pre_integration->delta_p + frame_i->second.R.transpose() * frame_j->second.R * TIC[0] - TIC[0]; 153 | //cout << "delta_p " << frame_j->second.pre_integration->delta_p.transpose() << endl; 154 | tmp_A.block<3, 3>(3, 0) = -Matrix3d::Identity(); 155 | tmp_A.block<3, 3>(3, 3) = frame_i->second.R.transpose() * frame_j->second.R; 156 | tmp_A.block<3, 3>(3, 6) = frame_i->second.R.transpose() * dt * Matrix3d::Identity(); 157 | tmp_b.block<3, 1>(3, 0) = frame_j->second.pre_integration->delta_v; 158 | //cout << "delta_v " << frame_j->second.pre_integration->delta_v.transpose() << endl; 159 | 160 | Matrix cov_inv = Matrix::Zero(); 161 | //cov.block<6, 6>(0, 0) = IMU_cov[i + 1]; 162 | //MatrixXd cov_inv = cov.inverse(); 163 | cov_inv.setIdentity(); 164 | 165 | MatrixXd r_A = tmp_A.transpose() * cov_inv * tmp_A; 166 | VectorXd r_b = tmp_A.transpose() * cov_inv * tmp_b; 167 | 168 | A.block<6, 6>(i * 3, i * 3) += r_A.topLeftCorner<6, 6>(); 169 | b.segment<6>(i * 3) += r_b.head<6>(); 170 | 171 | A.bottomRightCorner<4, 4>() += r_A.bottomRightCorner<4, 4>(); 172 | b.tail<4>() += r_b.tail<4>(); 173 | 174 | A.block<6, 4>(i * 3, n_state - 4) += r_A.topRightCorner<6, 4>(); 175 | A.block<4, 6>(n_state - 4, i * 3) += r_A.bottomLeftCorner<4, 6>(); 176 | } 177 | A = A * 1000.0; 178 | b = b * 1000.0; 179 | x = A.ldlt().solve(b); 180 | double s = x(n_state - 1) / 100.0; 181 | // ROS_DEBUG("estimated scale: %f", s); 182 | g = x.segment<3>(n_state - 4); 183 | // ROS_DEBUG_STREAM(" result g " << g.norm() << " " << g.transpose()); 184 | if(fabs(g.norm() - G.norm()) > 1.0 || s < 0) 185 | { 186 | return false; 187 | } 188 | 189 | RefineGravity(all_image_frame, g, x); 190 | s = (x.tail<1>())(0) / 100.0; 191 | (x.tail<1>())(0) = s; 192 | // ROS_DEBUG_STREAM(" refine " << g.norm() << " " << g.transpose()); 193 | if(s < 0.0 ) 194 | return false; 195 | else 196 | return true; 197 | } 198 | 199 | bool VisualIMUAlignment(map &all_image_frame, Vector3d* Bgs, Vector3d &g, VectorXd &x) 200 | { 201 | solveGyroscopeBias(all_image_frame, Bgs); 202 | 203 | if(LinearAlignment(all_image_frame, g, x)) 204 | return true; 205 | else 206 | return false; 207 | } 208 | -------------------------------------------------------------------------------- /src/initial/initial_ex_rotation.cpp: -------------------------------------------------------------------------------- 1 | #include "initial/initial_ex_rotation.h" 2 | 3 | InitialEXRotation::InitialEXRotation(){ 4 | frame_count = 0; 5 | Rc.push_back(Matrix3d::Identity()); 6 | Rc_g.push_back(Matrix3d::Identity()); 7 | Rimu.push_back(Matrix3d::Identity()); 8 | ric = Matrix3d::Identity(); 9 | } 10 | 11 | bool InitialEXRotation::CalibrationExRotation(vector> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result) 12 | { 13 | frame_count++; 14 | Rc.push_back(solveRelativeR(corres)); 15 | Rimu.push_back(delta_q_imu.toRotationMatrix()); 16 | Rc_g.push_back(ric.inverse() * delta_q_imu * ric); 17 | 18 | Eigen::MatrixXd A(frame_count * 4, 4); 19 | A.setZero(); 20 | int sum_ok = 0; 21 | for (int i = 1; i <= frame_count; i++) 22 | { 23 | Quaterniond r1(Rc[i]); 24 | Quaterniond r2(Rc_g[i]); 25 | 26 | double angular_distance = 180 / M_PI * r1.angularDistance(r2); 27 | //ROS_DEBUG("%d %f", i, angular_distance); 28 | 29 | double huber = angular_distance > 5.0 ? 5.0 / angular_distance : 1.0; 30 | ++sum_ok; 31 | Matrix4d L, R; 32 | 33 | double w = Quaterniond(Rc[i]).w(); 34 | Vector3d q = Quaterniond(Rc[i]).vec(); 35 | L.block<3, 3>(0, 0) = w * Matrix3d::Identity() + Utility::skewSymmetric(q); 36 | L.block<3, 1>(0, 3) = q; 37 | L.block<1, 3>(3, 0) = -q.transpose(); 38 | L(3, 3) = w; 39 | 40 | Quaterniond R_ij(Rimu[i]); 41 | w = R_ij.w(); 42 | q = R_ij.vec(); 43 | R.block<3, 3>(0, 0) = w * Matrix3d::Identity() - Utility::skewSymmetric(q); 44 | R.block<3, 1>(0, 3) = q; 45 | R.block<1, 3>(3, 0) = -q.transpose(); 46 | R(3, 3) = w; 47 | 48 | A.block<4, 4>((i - 1) * 4, 0) = huber * (L - R); 49 | } 50 | 51 | JacobiSVD svd(A, ComputeFullU | ComputeFullV); 52 | Matrix x = svd.matrixV().col(3); 53 | Quaterniond estimated_R(x); 54 | ric = estimated_R.toRotationMatrix().inverse(); 55 | //cout << svd.singularValues().transpose() << endl; 56 | //cout << ric << endl; 57 | Vector3d ric_cov; 58 | ric_cov = svd.singularValues().tail<3>(); 59 | if (frame_count >= WINDOW_SIZE && ric_cov(1) > 0.25) 60 | { 61 | calib_ric_result = ric; 62 | return true; 63 | } 64 | else 65 | return false; 66 | } 67 | 68 | Matrix3d InitialEXRotation::solveRelativeR(const vector> &corres) 69 | { 70 | if (corres.size() >= 9) 71 | { 72 | vector ll, rr; 73 | for (int i = 0; i < int(corres.size()); i++) 74 | { 75 | ll.push_back(cv::Point2f(corres[i].first(0), corres[i].first(1))); 76 | rr.push_back(cv::Point2f(corres[i].second(0), corres[i].second(1))); 77 | } 78 | cv::Mat E = cv::findFundamentalMat(ll, rr); 79 | cv::Mat_ R1, R2, t1, t2; 80 | decomposeE(E, R1, R2, t1, t2); 81 | 82 | if (determinant(R1) + 1.0 < 1e-09) 83 | { 84 | E = -E; 85 | decomposeE(E, R1, R2, t1, t2); 86 | } 87 | double ratio1 = max(testTriangulation(ll, rr, R1, t1), testTriangulation(ll, rr, R1, t2)); 88 | double ratio2 = max(testTriangulation(ll, rr, R2, t1), testTriangulation(ll, rr, R2, t2)); 89 | cv::Mat_ ans_R_cv = ratio1 > ratio2 ? R1 : R2; 90 | 91 | Matrix3d ans_R_eigen; 92 | for (int i = 0; i < 3; i++) 93 | for (int j = 0; j < 3; j++) 94 | ans_R_eigen(j, i) = ans_R_cv(i, j); 95 | return ans_R_eigen; 96 | } 97 | return Matrix3d::Identity(); 98 | } 99 | 100 | double InitialEXRotation::testTriangulation(const vector &l, 101 | const vector &r, 102 | cv::Mat_ R, cv::Mat_ t) 103 | { 104 | cv::Mat pointcloud; 105 | cv::Matx34f P = cv::Matx34f(1, 0, 0, 0, 106 | 0, 1, 0, 0, 107 | 0, 0, 1, 0); 108 | cv::Matx34f P1 = cv::Matx34f(R(0, 0), R(0, 1), R(0, 2), t(0), 109 | R(1, 0), R(1, 1), R(1, 2), t(1), 110 | R(2, 0), R(2, 1), R(2, 2), t(2)); 111 | cv::triangulatePoints(P, P1, l, r, pointcloud); 112 | int front_count = 0; 113 | for (int i = 0; i < pointcloud.cols; i++) 114 | { 115 | double normal_factor = pointcloud.col(i).at(3); 116 | 117 | cv::Mat_ p_3d_l = cv::Mat(P) * (pointcloud.col(i) / normal_factor); 118 | cv::Mat_ p_3d_r = cv::Mat(P1) * (pointcloud.col(i) / normal_factor); 119 | if (p_3d_l(2) > 0 && p_3d_r(2) > 0) 120 | front_count++; 121 | } 122 | //ROS_DEBUG("MotionEstimator: %f", 1.0 * front_count / pointcloud.cols); 123 | return 1.0 * front_count / pointcloud.cols; 124 | } 125 | 126 | void InitialEXRotation::decomposeE(cv::Mat E, 127 | cv::Mat_ &R1, cv::Mat_ &R2, 128 | cv::Mat_ &t1, cv::Mat_ &t2) 129 | { 130 | cv::SVD svd(E, cv::SVD::MODIFY_A); 131 | cv::Matx33d W(0, -1, 0, 132 | 1, 0, 0, 133 | 0, 0, 1); 134 | cv::Matx33d Wt(0, 1, 0, 135 | -1, 0, 0, 136 | 0, 0, 1); 137 | R1 = svd.u * cv::Mat(W) * svd.vt; 138 | R2 = svd.u * cv::Mat(Wt) * svd.vt; 139 | t1 = svd.u.col(2); 140 | t2 = -svd.u.col(2); 141 | } 142 | -------------------------------------------------------------------------------- /src/initial/solve_5pts.cpp: -------------------------------------------------------------------------------- 1 | #include "initial/solve_5pts.h" 2 | 3 | 4 | namespace cv { 5 | void decomposeEssentialMat( InputArray _E, OutputArray _R1, OutputArray _R2, OutputArray _t ) 6 | { 7 | 8 | Mat E = _E.getMat().reshape(1, 3); 9 | CV_Assert(E.cols == 3 && E.rows == 3); 10 | 11 | Mat D, U, Vt; 12 | SVD::compute(E, D, U, Vt); 13 | 14 | if (determinant(U) < 0) U *= -1.; 15 | if (determinant(Vt) < 0) Vt *= -1.; 16 | 17 | Mat W = (Mat_(3, 3) << 0, 1, 0, -1, 0, 0, 0, 0, 1); 18 | W.convertTo(W, E.type()); 19 | 20 | Mat R1, R2, t; 21 | R1 = U * W * Vt; 22 | R2 = U * W.t() * Vt; 23 | t = U.col(2) * 1.0; 24 | 25 | R1.copyTo(_R1); 26 | R2.copyTo(_R2); 27 | t.copyTo(_t); 28 | } 29 | 30 | int recoverPose( InputArray E, InputArray _points1, InputArray _points2, InputArray _cameraMatrix, 31 | OutputArray _R, OutputArray _t, InputOutputArray _mask) 32 | { 33 | 34 | Mat points1, points2, cameraMatrix; 35 | _points1.getMat().convertTo(points1, CV_64F); 36 | _points2.getMat().convertTo(points2, CV_64F); 37 | _cameraMatrix.getMat().convertTo(cameraMatrix, CV_64F); 38 | 39 | int npoints = points1.checkVector(2); 40 | CV_Assert( npoints >= 0 && points2.checkVector(2) == npoints && 41 | points1.type() == points2.type()); 42 | 43 | CV_Assert(cameraMatrix.rows == 3 && cameraMatrix.cols == 3 && cameraMatrix.channels() == 1); 44 | 45 | if (points1.channels() > 1) 46 | { 47 | points1 = points1.reshape(1, npoints); 48 | points2 = points2.reshape(1, npoints); 49 | } 50 | 51 | double fx = cameraMatrix.at(0,0); 52 | double fy = cameraMatrix.at(1,1); 53 | double cx = cameraMatrix.at(0,2); 54 | double cy = cameraMatrix.at(1,2); 55 | 56 | points1.col(0) = (points1.col(0) - cx) / fx; 57 | points2.col(0) = (points2.col(0) - cx) / fx; 58 | points1.col(1) = (points1.col(1) - cy) / fy; 59 | points2.col(1) = (points2.col(1) - cy) / fy; 60 | 61 | points1 = points1.t(); 62 | points2 = points2.t(); 63 | 64 | Mat R1, R2, t; 65 | decomposeEssentialMat(E, R1, R2, t); 66 | Mat P0 = Mat::eye(3, 4, R1.type()); 67 | Mat P1(3, 4, R1.type()), P2(3, 4, R1.type()), P3(3, 4, R1.type()), P4(3, 4, R1.type()); 68 | P1(Range::all(), Range(0, 3)) = R1 * 1.0; P1.col(3) = t * 1.0; 69 | P2(Range::all(), Range(0, 3)) = R2 * 1.0; P2.col(3) = t * 1.0; 70 | P3(Range::all(), Range(0, 3)) = R1 * 1.0; P3.col(3) = -t * 1.0; 71 | P4(Range::all(), Range(0, 3)) = R2 * 1.0; P4.col(3) = -t * 1.0; 72 | 73 | // Do the cheirality check. 74 | // Notice here a threshold dist is used to filter 75 | // out far away points (i.e. infinite points) since 76 | // there depth may vary between postive and negtive. 77 | double dist = 50.0; 78 | Mat Q; 79 | triangulatePoints(P0, P1, points1, points2, Q); 80 | Mat mask1 = Q.row(2).mul(Q.row(3)) > 0; 81 | Q.row(0) /= Q.row(3); 82 | Q.row(1) /= Q.row(3); 83 | Q.row(2) /= Q.row(3); 84 | Q.row(3) /= Q.row(3); 85 | mask1 = (Q.row(2) < dist) & mask1; 86 | Q = P1 * Q; 87 | mask1 = (Q.row(2) > 0) & mask1; 88 | mask1 = (Q.row(2) < dist) & mask1; 89 | 90 | triangulatePoints(P0, P2, points1, points2, Q); 91 | Mat mask2 = Q.row(2).mul(Q.row(3)) > 0; 92 | Q.row(0) /= Q.row(3); 93 | Q.row(1) /= Q.row(3); 94 | Q.row(2) /= Q.row(3); 95 | Q.row(3) /= Q.row(3); 96 | mask2 = (Q.row(2) < dist) & mask2; 97 | Q = P2 * Q; 98 | mask2 = (Q.row(2) > 0) & mask2; 99 | mask2 = (Q.row(2) < dist) & mask2; 100 | 101 | triangulatePoints(P0, P3, points1, points2, Q); 102 | Mat mask3 = Q.row(2).mul(Q.row(3)) > 0; 103 | Q.row(0) /= Q.row(3); 104 | Q.row(1) /= Q.row(3); 105 | Q.row(2) /= Q.row(3); 106 | Q.row(3) /= Q.row(3); 107 | mask3 = (Q.row(2) < dist) & mask3; 108 | Q = P3 * Q; 109 | mask3 = (Q.row(2) > 0) & mask3; 110 | mask3 = (Q.row(2) < dist) & mask3; 111 | 112 | triangulatePoints(P0, P4, points1, points2, Q); 113 | Mat mask4 = Q.row(2).mul(Q.row(3)) > 0; 114 | Q.row(0) /= Q.row(3); 115 | Q.row(1) /= Q.row(3); 116 | Q.row(2) /= Q.row(3); 117 | Q.row(3) /= Q.row(3); 118 | mask4 = (Q.row(2) < dist) & mask4; 119 | Q = P4 * Q; 120 | mask4 = (Q.row(2) > 0) & mask4; 121 | mask4 = (Q.row(2) < dist) & mask4; 122 | 123 | mask1 = mask1.t(); 124 | mask2 = mask2.t(); 125 | mask3 = mask3.t(); 126 | mask4 = mask4.t(); 127 | 128 | // If _mask is given, then use it to filter outliers. 129 | if (!_mask.empty()) 130 | { 131 | Mat mask = _mask.getMat(); 132 | CV_Assert(mask.size() == mask1.size()); 133 | bitwise_and(mask, mask1, mask1); 134 | bitwise_and(mask, mask2, mask2); 135 | bitwise_and(mask, mask3, mask3); 136 | bitwise_and(mask, mask4, mask4); 137 | } 138 | if (_mask.empty() && _mask.needed()) 139 | { 140 | _mask.create(mask1.size(), CV_8U); 141 | } 142 | 143 | CV_Assert(_R.needed() && _t.needed()); 144 | _R.create(3, 3, R1.type()); 145 | _t.create(3, 1, t.type()); 146 | 147 | int good1 = countNonZero(mask1); 148 | int good2 = countNonZero(mask2); 149 | int good3 = countNonZero(mask3); 150 | int good4 = countNonZero(mask4); 151 | 152 | if (good1 >= good2 && good1 >= good3 && good1 >= good4) 153 | { 154 | R1.copyTo(_R); 155 | t.copyTo(_t); 156 | if (_mask.needed()) mask1.copyTo(_mask); 157 | return good1; 158 | } 159 | else if (good2 >= good1 && good2 >= good3 && good2 >= good4) 160 | { 161 | R2.copyTo(_R); 162 | t.copyTo(_t); 163 | if (_mask.needed()) mask2.copyTo(_mask); 164 | return good2; 165 | } 166 | else if (good3 >= good1 && good3 >= good2 && good3 >= good4) 167 | { 168 | t = -t; 169 | R1.copyTo(_R); 170 | t.copyTo(_t); 171 | if (_mask.needed()) mask3.copyTo(_mask); 172 | return good3; 173 | } 174 | else 175 | { 176 | t = -t; 177 | R2.copyTo(_R); 178 | t.copyTo(_t); 179 | if (_mask.needed()) mask4.copyTo(_mask); 180 | return good4; 181 | } 182 | } 183 | 184 | int recoverPose( InputArray E, InputArray _points1, InputArray _points2, OutputArray _R, 185 | OutputArray _t, double focal, Point2d pp, InputOutputArray _mask) 186 | { 187 | Mat cameraMatrix = (Mat_(3,3) << focal, 0, pp.x, 0, focal, pp.y, 0, 0, 1); 188 | return cv::recoverPose(E, _points1, _points2, cameraMatrix, _R, _t, _mask); 189 | } 190 | } 191 | 192 | 193 | bool MotionEstimator::solveRelativeRT(const vector> &corres, Matrix3d &Rotation, Vector3d &Translation) 194 | { 195 | if (corres.size() >= 15) 196 | { 197 | vector ll, rr; 198 | for (int i = 0; i < int(corres.size()); i++) 199 | { 200 | ll.push_back(cv::Point2f(corres[i].first(0), corres[i].first(1))); 201 | rr.push_back(cv::Point2f(corres[i].second(0), corres[i].second(1))); 202 | } 203 | cv::Mat mask; 204 | cv::Mat E = cv::findFundamentalMat(ll, rr, cv::FM_RANSAC, 0.3 / 460, 0.99, mask); 205 | cv::Mat cameraMatrix = (cv::Mat_(3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 1); 206 | cv::Mat rot, trans; 207 | int inlier_cnt = cv::recoverPose(E, ll, rr, cameraMatrix, rot, trans, mask); 208 | //cout << "inlier_cnt " << inlier_cnt << endl; 209 | 210 | Eigen::Matrix3d R; 211 | Eigen::Vector3d T; 212 | for (int i = 0; i < 3; i++) 213 | { 214 | T(i) = trans.at(i, 0); 215 | for (int j = 0; j < 3; j++) 216 | R(i, j) = rot.at(i, j); 217 | } 218 | 219 | Rotation = R.transpose(); 220 | Translation = -R.transpose() * T; 221 | if(inlier_cnt > 12) 222 | return true; 223 | else 224 | return false; 225 | } 226 | return false; 227 | } 228 | 229 | 230 | 231 | -------------------------------------------------------------------------------- /src/parameters.cpp: -------------------------------------------------------------------------------- 1 | #include "parameters.h" 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | using namespace std; 8 | using namespace cv; 9 | 10 | double INIT_DEPTH; 11 | double MIN_PARALLAX; 12 | double ACC_N, ACC_W; 13 | double GYR_N, GYR_W; 14 | 15 | vector RIC; 16 | vector TIC; 17 | 18 | Eigen::Vector3d G{0.0, 0.0, 9.8}; 19 | 20 | double BIAS_ACC_THRESHOLD; 21 | double BIAS_GYR_THRESHOLD; 22 | double SOLVER_TIME; 23 | int NUM_ITERATIONS; 24 | int ESTIMATE_EXTRINSIC; 25 | int ESTIMATE_TD; 26 | int ROLLING_SHUTTER; 27 | string EX_CALIB_RESULT_PATH; 28 | string VINS_RESULT_PATH; 29 | // string IMU_TOPIC; 30 | double ROW, COL; 31 | double TD, TR; 32 | 33 | 34 | 35 | int FOCAL_LENGTH; 36 | string IMAGE_TOPIC; 37 | string IMU_TOPIC; 38 | string FISHEYE_MASK; 39 | vector CAM_NAMES; 40 | int MAX_CNT; 41 | int MIN_DIST; 42 | // int WINDOW_SIZE; 43 | int FREQ; 44 | double F_THRESHOLD; 45 | int SHOW_TRACK; 46 | bool STEREO_TRACK; 47 | int EQUALIZE; 48 | int FISHEYE; 49 | bool PUB_THIS_FRAME; 50 | 51 | 52 | void readParameters(string config_file) 53 | { 54 | // string config_file; 55 | // config_file = readParam(n, "config_file"); 56 | // cv::FileStorage fsSettings(config_file, cv::FileStorage::READ); 57 | // if (!fsSettings.isOpened()) 58 | // { 59 | // cerr << "ERROR: Wrong path to settings" << endl; 60 | // } 61 | cv::FileStorage fsSettings(config_file, cv::FileStorage::READ); 62 | if (!fsSettings.isOpened()) 63 | { 64 | cerr << "1 readParameters ERROR: Wrong path to settings!" << endl; 65 | return; 66 | } 67 | 68 | fsSettings["imu_topic"] >> IMU_TOPIC; 69 | 70 | FOCAL_LENGTH = 460; 71 | SOLVER_TIME = fsSettings["max_solver_time"]; 72 | NUM_ITERATIONS = fsSettings["max_num_iterations"]; 73 | MIN_PARALLAX = fsSettings["keyframe_parallax"]; 74 | MIN_PARALLAX = MIN_PARALLAX / FOCAL_LENGTH; 75 | 76 | string OUTPUT_PATH; 77 | fsSettings["output_path"] >> OUTPUT_PATH; 78 | VINS_RESULT_PATH = OUTPUT_PATH + "/vins_result_no_loop.txt"; 79 | // cout << "result path " << VINS_RESULT_PATH << endl; 80 | // ofstream fout(VINS_RESULT_PATH, ios::out); 81 | // fout.close(); 82 | 83 | ACC_N = fsSettings["acc_n"]; 84 | ACC_W = fsSettings["acc_w"]; 85 | GYR_N = fsSettings["gyr_n"]; 86 | GYR_W = fsSettings["gyr_w"]; 87 | G.z() = fsSettings["g_norm"]; 88 | ROW = fsSettings["image_height"]; 89 | COL = fsSettings["image_width"]; 90 | // ROS_INFO("ROW: %f COL: %f ", ROW, COL); 91 | 92 | ESTIMATE_EXTRINSIC = fsSettings["estimate_extrinsic"]; 93 | if (ESTIMATE_EXTRINSIC == 2) 94 | { 95 | // ROS_WARN("have no prior about extrinsic param, calibrate extrinsic param"); 96 | RIC.push_back(Eigen::Matrix3d::Identity()); 97 | TIC.push_back(Eigen::Vector3d::Zero()); 98 | EX_CALIB_RESULT_PATH = OUTPUT_PATH + "/extrinsic_parameter.csv"; 99 | } 100 | else 101 | { 102 | if (ESTIMATE_EXTRINSIC == 1) 103 | { 104 | // ROS_WARN(" Optimize extrinsic param around initial guess!"); 105 | EX_CALIB_RESULT_PATH = OUTPUT_PATH + "/extrinsic_parameter.csv"; 106 | } 107 | if (ESTIMATE_EXTRINSIC == 0){ 108 | cout << " fix extrinsic param " << endl; 109 | } 110 | cv::Mat cv_R, cv_T; 111 | fsSettings["extrinsicRotation"] >> cv_R; 112 | fsSettings["extrinsicTranslation"] >> cv_T; 113 | Eigen::Matrix3d eigen_R; 114 | Eigen::Vector3d eigen_T; 115 | cv::cv2eigen(cv_R, eigen_R); 116 | cv::cv2eigen(cv_T, eigen_T); 117 | Eigen::Quaterniond Q(eigen_R); 118 | eigen_R = Q.normalized(); 119 | RIC.push_back(eigen_R); 120 | TIC.push_back(eigen_T); 121 | // ROS_INFO_STREAM("Extrinsic_R : " << endl 122 | // << RIC[0]); 123 | // ROS_INFO_STREAM("Extrinsic_T : " << endl 124 | // << TIC[0].transpose()); 125 | } 126 | 127 | INIT_DEPTH = 5.0; 128 | BIAS_ACC_THRESHOLD = 0.1; 129 | BIAS_GYR_THRESHOLD = 0.1; 130 | 131 | TD = fsSettings["td"]; 132 | ESTIMATE_TD = fsSettings["estimate_td"]; 133 | // if (ESTIMATE_TD) 134 | // ROS_INFO_STREAM("Unsynchronized sensors, online estimate time offset, initial td: " << TD); 135 | // else 136 | // ROS_INFO_STREAM("Synchronized sensors, fix time offset: " << TD); 137 | 138 | ROLLING_SHUTTER = fsSettings["rolling_shutter"]; 139 | if (ROLLING_SHUTTER) 140 | { 141 | TR = fsSettings["rolling_shutter_tr"]; 142 | // ROS_INFO_STREAM("rolling shutter camera, read out time per line: " << TR); 143 | } 144 | else 145 | { 146 | TR = 0; 147 | } 148 | 149 | //string VINS_FOLDER_PATH = readParam(n, "vins_folder"); 150 | 151 | fsSettings["image_topic"] >> IMAGE_TOPIC; 152 | fsSettings["imu_topic"] >> IMU_TOPIC; 153 | MAX_CNT = fsSettings["max_cnt"]; 154 | MIN_DIST = fsSettings["min_dist"]; 155 | ROW = fsSettings["image_height"]; 156 | COL = fsSettings["image_width"]; 157 | FREQ = fsSettings["freq"]; 158 | F_THRESHOLD = fsSettings["F_threshold"]; 159 | SHOW_TRACK = fsSettings["show_track"]; 160 | EQUALIZE = fsSettings["equalize"]; 161 | FISHEYE = fsSettings["fisheye"]; 162 | // if (FISHEYE == 1) 163 | // FISHEYE_MASK = VINS_FOLDER_PATH + "config/fisheye_mask.jpg"; 164 | CAM_NAMES.push_back(config_file); 165 | 166 | // WINDOW_SIZE = 20; 167 | STEREO_TRACK = false; 168 | PUB_THIS_FRAME = false; 169 | 170 | if (FREQ == 0){ 171 | FREQ = 10; 172 | } 173 | fsSettings.release(); 174 | 175 | cout << "1 readParameters: " 176 | << "\n INIT_DEPTH: " << INIT_DEPTH 177 | << "\n MIN_PARALLAX: " << MIN_PARALLAX 178 | << "\n ACC_N: " < 2 | #include 3 | #include "backend/problem.h" 4 | 5 | using namespace myslam::backend; 6 | using namespace std; 7 | 8 | // The contex of curve model, template parameters： the dimention of optimisers and data type 9 | class CurveFittingVertex: public Vertex 10 | { 11 | public: 12 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 13 | 14 | CurveFittingVertex(): Vertex(3) {} // abc: three paramters， Vertex is 3D 15 | virtual std::string TypeInfo() const { return "abc"; } 16 | }; 17 | 18 | // error model, template parameters： the dimention of observation，type and contex's type 19 | class CurveFittingEdge: public Edge 20 | { 21 | public: 22 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 23 | CurveFittingEdge( double x, double y ): Edge(1,1, std::vector{"abc"}) { 24 | x_ = x; 25 | y_ = y; 26 | } 27 | // compute the error of the curve model 28 | virtual void ComputeResidual() override 29 | { 30 | Vec3 abc = verticies_[0]->Parameters(); // estimated paramters 31 | residual_(0) = std::exp( abc(0)*x_*x_ + abc(1)*x_ + abc(2) ) - y_; // contruct the residual 32 | } 33 | 34 | // Compute the jacobi of residual to variables 35 | virtual void ComputeJacobians() override 36 | { 37 | Vec3 abc = verticies_[0]->Parameters(); 38 | double exp_y = std::exp( abc(0)*x_*x_ + abc(1)*x_ + abc(2) ); 39 | 40 | Eigen::Matrix jaco_abc; // the residual is 1D，states are 3，so it's a 1x3 jacobi matrix 41 | jaco_abc << x_ * x_ * exp_y, x_ * exp_y , 1 * exp_y; 42 | jacobians_[0] = jaco_abc; 43 | } 44 | /// Return the edge's type info. 45 | virtual std::string TypeInfo() const override { return "CurveFittingEdge"; } 46 | public: 47 | double x_,y_; // x 值， y 值为 _measurement 48 | }; 49 | 50 | int main() 51 | { 52 | double a=1.0, b=2.0, c=1.0; // true parameters 53 | int N = 100; // measurements 54 | double w_sigma= 1.; // Noise: Sigma 55 | 56 | std::default_random_engine generator; 57 | std::normal_distribution noise(0.,w_sigma); 58 | 59 | // Construct problem 60 | Problem problem(Problem::ProblemType::GENERIC_PROBLEM); 61 | shared_ptr< CurveFittingVertex > vertex(new CurveFittingVertex()); 62 | 63 | // Set the initial estimator: parameters a, b, c 64 | vertex->SetParameters(Eigen::Vector3d (0.,0.,0.)); 65 | // Insert the parameters into the least square problem 66 | problem.AddVertex(vertex); 67 | 68 | // construct N times observation 69 | for (int i = 0; i < N; ++i) { 70 | 71 | double x = i/100.; 72 | double n = noise(generator); 73 | // 观测 y 74 | double y = std::exp( a*x*x + b*x + c ) + n; 75 | // double y = std::exp( a*x*x + b*x + c ); 76 | 77 | // the correspoing residual functions 78 | shared_ptr< CurveFittingEdge > edge(new CurveFittingEdge(x,y)); 79 | std::vector> edge_vertex; 80 | edge_vertex.push_back(vertex); 81 | edge->SetVertex(edge_vertex); 82 | 83 | // insert the edge into the problem 84 | problem.AddEdge(edge); 85 | } 86 | 87 | std::cout<<"\nTest CurveFitting start..."<Parameters().transpose() << std::endl; 93 | std::cout << "-------ground truth: " << std::endl; 94 | std::cout << "1.0, 2.0, 1.0" << std::endl; 95 | 96 | // std 97 | return 0; 98 | } 99 | 100 | 101 | -------------------------------------------------------------------------------- /test/run_euroc.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include "System.h" 15 | 16 | using namespace std; 17 | using namespace cv; 18 | using namespace Eigen; 19 | 20 | const int nDelayTimes = 2; 21 | string sData_path = "/home/dataset/EuRoC/MH-05/mav0/"; 22 | string sConfig_path = "../config/"; 23 | 24 | std::shared_ptr pSystem; 25 | 26 | void PubImuData() 27 | { 28 | string sImu_data_file = sConfig_path + "MH_05_imu0.txt"; 29 | cout << "1 PubImuData start sImu_data_filea: " << sImu_data_file << endl; 30 | ifstream fsImu; 31 | fsImu.open(sImu_data_file.c_str()); 32 | if (!fsImu.is_open()) 33 | { 34 | cerr << "Failed to open imu file! " << sImu_data_file << endl; 35 | return; 36 | } 37 | 38 | std::string sImu_line; 39 | double dStampNSec = 0.0; 40 | Vector3d vAcc; 41 | Vector3d vGyr; 42 | while (std::getline(fsImu, sImu_line) && !sImu_line.empty()) // read imu data 43 | { 44 | std::istringstream ssImuData(sImu_line); 45 | ssImuData >> dStampNSec >> vGyr.x() >> vGyr.y() >> vGyr.z() >> vAcc.x() >> vAcc.y() >> vAcc.z(); 46 | // cout << "Imu t: " << fixed << dStampNSec << " gyr: " << vGyr.transpose() << " acc: " << vAcc.transpose() << endl; 47 | pSystem->PubImuData(dStampNSec / 1e9, vGyr, vAcc); 48 | usleep(5000*nDelayTimes); 49 | } 50 | fsImu.close(); 51 | } 52 | 53 | void PubImageData() 54 | { 55 | string sImage_file = sConfig_path + "MH_05_cam0.txt"; 56 | 57 | cout << "1 PubImageData start sImage_file: " << sImage_file << endl; 58 | 59 | ifstream fsImage; 60 | fsImage.open(sImage_file.c_str()); 61 | if (!fsImage.is_open()) 62 | { 63 | cerr << "Failed to open image file! " << sImage_file << endl; 64 | return; 65 | } 66 | 67 | std::string sImage_line; 68 | double dStampNSec; 69 | string sImgFileName; 70 | 71 | // cv::namedWindow("SOURCE IMAGE", CV_WINDOW_AUTOSIZE); 72 | while (std::getline(fsImage, sImage_line) && !sImage_line.empty()) 73 | { 74 | std::istringstream ssImuData(sImage_line); 75 | ssImuData >> dStampNSec >> sImgFileName; 76 | // cout << "Image t : " << fixed << dStampNSec << " Name: " << sImgFileName << endl; 77 | string imagePath = sData_path + "cam0/data/" + sImgFileName; 78 | 79 | Mat img = imread(imagePath.c_str(), 0); 80 | if (img.empty()) 81 | { 82 | cerr << "image is empty! path: " << imagePath << endl; 83 | return; 84 | } 85 | pSystem->PubImageData(dStampNSec / 1e9, img); 86 | // cv::imshow("SOURCE IMAGE", img); 87 | // cv::waitKey(0); 88 | usleep(50000*nDelayTimes); 89 | } 90 | fsImage.close(); 91 | } 92 | 93 | #ifdef __APPLE__ 94 | // support for MacOS 95 | void DrawIMGandGLinMainThrd(){ 96 | string sImage_file = sConfig_path + "MH_05_cam0.txt"; 97 | 98 | cout << "1 PubImageData start sImage_file: " << sImage_file << endl; 99 | 100 | ifstream fsImage; 101 | fsImage.open(sImage_file.c_str()); 102 | if (!fsImage.is_open()) 103 | { 104 | cerr << "Failed to open image file! " << sImage_file << endl; 105 | return; 106 | } 107 | 108 | std::string sImage_line; 109 | double dStampNSec; 110 | string sImgFileName; 111 | 112 | pSystem->InitDrawGL(); 113 | while (std::getline(fsImage, sImage_line) && !sImage_line.empty()) 114 | { 115 | std::istringstream ssImuData(sImage_line); 116 | ssImuData >> dStampNSec >> sImgFileName; 117 | // cout << "Image t : " << fixed << dStampNSec << " Name: " << sImgFileName << endl; 118 | string imagePath = sData_path + "cam0/data/" + sImgFileName; 119 | 120 | Mat img = imread(imagePath.c_str(), 0); 121 | if (img.empty()) 122 | { 123 | cerr << "image is empty! path: " << imagePath << endl; 124 | return; 125 | } 126 | //pSystem->PubImageData(dStampNSec / 1e9, img); 127 | cv::Mat show_img; 128 | cv::cvtColor(img, show_img, CV_GRAY2RGB); 129 | if (SHOW_TRACK) 130 | { 131 | for (unsigned int j = 0; j < pSystem->trackerData[0].cur_pts.size(); j++) 132 | { 133 | double len = min(1.0, 1.0 * pSystem->trackerData[0].track_cnt[j] / WINDOW_SIZE); 134 | cv::circle(show_img, pSystem->trackerData[0].cur_pts[j], 2, cv::Scalar(255 * (1 - len), 0, 255 * len), 2); 135 | } 136 | 137 | cv::namedWindow("IMAGE", CV_WINDOW_AUTOSIZE); 138 | cv::imshow("IMAGE", show_img); 139 | // cv::waitKey(1); 140 | } 141 | 142 | pSystem->DrawGLFrame(); 143 | usleep(50000*nDelayTimes); 144 | } 145 | fsImage.close(); 146 | 147 | } 148 | #endif 149 | 150 | int main(int argc, char **argv) 151 | { 152 | if(argc != 3) 153 | { 154 | cerr << "./run_euroc PATH_TO_FOLDER/MH-05/mav0 PATH_TO_CONFIG/config \n" 155 | << "For example: ./run_euroc /home/stevencui/dataset/EuRoC/MH-05/mav0/ ../config/"<< endl; 156 | return -1; 157 | } 158 | sData_path = argv[1]; 159 | sConfig_path = argv[2]; 160 | 161 | pSystem.reset(new System(sConfig_path)); 162 | 163 | std::thread thd_BackEnd(&System::ProcessBackEnd, pSystem); 164 | 165 | // sleep(5); 166 | std::thread thd_PubImuData(PubImuData); 167 | 168 | std::thread thd_PubImageData(PubImageData); 169 | 170 | #ifdef __linux__ 171 | std::thread thd_Draw(&System::Draw, pSystem); 172 | #elif __APPLE__ 173 | DrawIMGandGLinMainThrd(); 174 | #endif 175 | 176 | thd_PubImuData.join(); 177 | thd_PubImageData.join(); 178 | 179 | // thd_BackEnd.join(); 180 | #ifdef __linux__ 181 | thd_Draw.join(); 182 | #endif 183 | 184 | cout << "main end... see you ..." << endl; 185 | return 0; 186 | } 187 | -------------------------------------------------------------------------------- /thirdparty/Sophus/sophus/sophus.hpp: -------------------------------------------------------------------------------- 1 | // This file is part of Sophus. 2 | // 3 | // Copyright 2013 Hauke Strasdat 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to 7 | // deal in the Software without restriction, including without limitation the 8 | // rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 9 | // sell copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in 13 | // all copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 20 | // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 21 | // IN THE SOFTWARE. 22 | 23 | #ifndef SOPHUS_HPP 24 | #define SOPHUS_HPP 25 | 26 | #include 27 | 28 | // fix log1p not being found on Android in Eigen 29 | #if defined( ANDROID ) 30 | #include 31 | namespace std 32 | { 33 | using ::log1p; 34 | } 35 | #endif 36 | 37 | #include 38 | #include 39 | 40 | namespace Sophus { 41 | using namespace Eigen; 42 | 43 | template 44 | struct SophusConstants { 45 | EIGEN_ALWAYS_INLINE static 46 | const Scalar epsilon() { 47 | return static_cast ( 1e-10 ); 48 | } 49 | 50 | EIGEN_ALWAYS_INLINE static 51 | const Scalar pi() { 52 | return static_cast ( M_PI ); 53 | } 54 | }; 55 | 56 | template<> 57 | struct SophusConstants { 58 | EIGEN_ALWAYS_INLINE static 59 | float epsilon() { 60 | return static_cast ( 1e-5 ); 61 | } 62 | 63 | EIGEN_ALWAYS_INLINE static 64 | float pi() { 65 | return static_cast ( M_PI ); 66 | } 67 | }; 68 | 69 | class SophusException : public std::runtime_error { 70 | public: 71 | SophusException(const std::string &str) 72 | : runtime_error("Sophus exception: " + str) { 73 | } 74 | }; 75 | 76 | } 77 | 78 | #endif 79 | --------------------------------------------------------------------------------