├── LICENCE ├── README.md ├── camera_model ├── CMakeLists.txt ├── cmake │ └── FindEigen.cmake ├── include │ └── 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 ├── instruction ├── package.xml ├── readme.md └── src │ ├── 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 ├── config ├── gvins_rviz_config.rviz ├── simulator │ └── simulator_config.yaml └── visensor_f9p │ └── visensor_left_f9p_config.yaml ├── docker ├── Dockerfile ├── Makefile └── run.sh ├── estimator ├── CMakeLists.txt ├── cmake │ └── FindEigen.cmake ├── launch │ ├── simulator.launch │ └── visensor_f9p.launch ├── msg │ └── LocalSensorExternalTrigger.msg ├── package.xml └── src │ ├── estimator.cpp │ ├── estimator.h │ ├── estimator_node.cpp │ ├── factor │ ├── gnss_ddt_smooth_factor.cpp │ ├── gnss_ddt_smooth_factor.hpp │ ├── gnss_dt_anchor_factor.cpp │ ├── gnss_dt_anchor_factor.hpp │ ├── gnss_dt_ddt_factor.cpp │ ├── gnss_dt_ddt_factor.hpp │ ├── gnss_psr_dopp_factor.cpp │ ├── gnss_psr_dopp_factor.hpp │ ├── imu_factor.h │ ├── integration_base.h │ ├── marginalization_factor.cpp │ ├── marginalization_factor.h │ ├── pos_vel_factor.cpp │ ├── pos_vel_factor.hpp │ ├── pose_anchor_factor.cpp │ ├── pose_anchor_factor.h │ ├── pose_local_parameterization.cpp │ ├── pose_local_parameterization.h │ ├── projection_factor.cpp │ ├── projection_factor.h │ ├── projection_td_factor.cpp │ └── projection_td_factor.h │ ├── feature_manager.cpp │ ├── feature_manager.h │ ├── initial │ ├── gnss_vi_initializer.cpp │ ├── gnss_vi_initializer.h │ ├── initial_aligment.cpp │ ├── initial_alignment.h │ ├── initial_ex_rotation.cpp │ ├── initial_ex_rotation.h │ ├── initial_sfm.cpp │ ├── initial_sfm.h │ ├── solve_5pts.cpp │ └── solve_5pts.h │ ├── parameters.cpp │ ├── parameters.h │ └── utility │ ├── CameraPoseVisualization.cpp │ ├── CameraPoseVisualization.h │ ├── tic_toc.h │ ├── utility.cpp │ ├── utility.h │ ├── visualization.cpp │ └── visualization.h ├── feature_tracker ├── CMakeLists.txt ├── cmake │ └── FindEigen.cmake ├── package.xml └── src │ ├── feature_tracker.cpp │ ├── feature_tracker.h │ ├── feature_tracker_node.cpp │ ├── parameters.cpp │ ├── parameters.h │ └── tic_toc.h └── figures └── system_snapshot.png /README.md: -------------------------------------------------------------------------------- 1 | 相关原理推导，详见知乎：https://zhuanlan.zhihu.com/p/566273634 2 | 3 | # GVINS 4 | 5 | roslaunch gvins visensor_f9p.launch 6 | rviz -d /home/SLAM/GVINS/catkin_ws/src/GVINS/config/gvins_rviz_config.rviz 7 | rosbag play sports_field.bag 8 | 9 | GVINS: Tightly Coupled GNSS-Visual-Inertial Fusion for Smooth and Consistent State Estimation. [paper link](https://arxiv.org/pdf/2103.07899.pdf) 10 | 11 | Authors: Shaozu CAO, Xiuyuan LU and Shaojie SHEN 12 | 13 | ![](./figures/system_snapshot.png) 14 | 15 | **GVINS** is a non-linear optimization based system that tightly fuses GNSS raw measurements with visual and inertial information for real-time and drift-free state estimation. By incorporating GNSS pseudorange and Doppler shift measurements, GVINS is capable to provide smooth and consistent 6-DoF global localization in complex environment. The system framework and VIO part are adapted from [VINS-Mono](https://github.com/HKUST-Aerial-Robotics/VINS-Mono). Our system contains the following features: 16 | 17 | - global 6-DoF estimation in ECEF frame; 18 | - multi-constellation support (GPS, GLONASS, Galileo, BeiDou); 19 | - online local-ENU frame alignment; 20 | - global pose recovery in GNSS-unfriendly or even GNSS-denied area. 21 | 22 | **Video:** 23 | 24 | [![GVINS Video](http://img.youtube.com/vi/tEQN70AX68U/0.jpg)](https://www.youtube.com/watch?v=TebAal1ARnk "GVINS Video") 25 | 26 | ## 1. Prerequisites 27 | ### 1.1 C++11 Compiler 28 | This package requires some features of C++11. 29 | 30 | ### 1.2 ROS 31 | This package is developed under [ROS Kinetic](http://wiki.ros.org/kinetic) environment. 32 | 33 | ### 1.3 Eigen 34 | Our code uses [Eigen 3.3.3](https://gitlab.com/libeigen/eigen/-/archive/3.3.3/eigen-3.3.3.zip) for matrix manipulation. 35 | 36 | ### 1.4 Ceres 37 | We use [ceres](https://ceres-solver.googlesource.com/ceres-solver) 1.12.0 to solve the non-linear optimization problem. 38 | 39 | ### 1.5 gnss_comm 40 | This package also requires [gnss_comm](https://github.com/HKUST-Aerial-Robotics/gnss_comm) for ROS message definitions and some utility functions. Follow [those instructions](https://github.com/HKUST-Aerial-Robotics/gnss_comm#2-build-gnss_comm-library) to build the *gnss_comm* package. 41 | 42 | ## 2. Build GVINS 43 | Clone the repository to your catkin workspace (for example `~/catkin_ws/`): 44 | ``` 45 | cd ~/catkin_ws/src/ 46 | git clone https://github.com/HKUST-Aerial-Robotics/GVINS.git 47 | ``` 48 | Then build the package with: 49 | ``` 50 | cd ~/catkin_ws/ 51 | catkin_make 52 | source ~/catkin_ws/devel/setup.bash 53 | ``` 54 | If you encounter any problem during the building of GVINS, we recommend you to [try docker](#docker_section) first. 55 | 56 | ## 3. Docker Support 57 | To simplify the building process, we add docker in our code. Docker is like a sandbox so it can isolate our code from your local environment. To run with docker, first make sure [ros](http://wiki.ros.org/ROS/Installation) and [docker](https://docs.docker.com/get-docker/) are installed on your machine. Then add your account to `docker` group by `sudo usermod -aG docker $USER`. **Logout and re-login to avoid the *Permission denied* error**, then type: 58 | ``` 59 | cd ~/catkin_ws/src/GVINS/docker 60 | make build 61 | ``` 62 | The docker image `gvins:latest` should be successfully built after a while. Then you can run GVINS with: 63 | ``` 64 | ./run.sh LAUNCH_FILE 65 | ``` 66 | (for example `./run.sh visensor_f9p.launch`). Open another terminal and play your rosbag file, then you should be able to see the result. If you modify the code, simply re-run `./run.sh LAUNCH_FILE` to update. 67 | 68 | 69 | ## 4. Run GVINS with our dataset 70 | Download our [GVINS-Dataset](https://github.com/HKUST-Aerial-Robotics/GVINS-Dataset) and launch GVINS via: 71 | ``` 72 | roslaunch gvins visensor_f9p.launch 73 | ``` 74 | Open another terminal and launch the rviz by: 75 | ``` 76 | rviz -d ~/catkin_ws/src/GVINS/config/gvins_rviz_config.rviz 77 | ``` 78 | Then play the bag: 79 | ``` 80 | rosbag play sports_field.bag 81 | ``` 82 | 83 | ## 5. Run GVINS with your device 84 | 85 | 86 | 1. Setup the visual-inertial sensor suit according to [these instructions](https://github.com/HKUST-Aerial-Robotics/VINS-Mono#5-run-with-your-device); 87 | 2. Configure your GNSS receiver to output raw measurement and ephemeris and convert them as ros messages. If you are using u-blox receiver, please checkout our [ublox_driver](https://github.com/HKUST-Aerial-Robotics/ublox_driver) package; 88 | 3. Deal with the synchronization between visual-inertial sensor and GNSS receiver. A coarse synchronization can be done via [ublox_driver](https://github.com/HKUST-Aerial-Robotics/ublox_driver) but the accuracy is not guaranteed. For better performance, we recommend hardware synchronization via receiver's PPS signal; 89 | 4. Change the topic name in the config file and create a launch file pointing to the corresponding config file. Launch the GVINS with `roslaunch gvins YOUR_LAUNCH_FILE.launch`. 90 | 91 | 92 | ## 6. Acknowledgements 93 | The system framework and VIO part are adapted from [VINS-Mono](https://github.com/HKUST-Aerial-Robotics/VINS-Mono). We use [camodocal](https://github.com/hengli/camodocal) for camera modelling and [ceres](http://ceres-solver.org/) to solve the optimization problem. 94 | 95 | ## 7. Licence 96 | The source code is released under [GPLv3](https://www.gnu.org/licenses/gpl-3.0.html) license. 97 | # GVINS_NotedByLSY 98 | # GVINS_Noted_ByLSY 99 | # GVINS_Noted_ByLSY 100 | -------------------------------------------------------------------------------- /camera_model/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(gvins_camera_model) 3 | 4 | set(CMAKE_BUILD_TYPE "Release") 5 | set(CMAKE_CXX_FLAGS "-std=c++11") 6 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -fPIC") 7 | 8 | find_package(catkin REQUIRED COMPONENTS 9 | roscpp 10 | std_msgs 11 | ) 12 | 13 | find_package(Boost REQUIRED COMPONENTS filesystem program_options system) 14 | include_directories(${Boost_INCLUDE_DIRS}) 15 | 16 | find_package(OpenCV REQUIRED) 17 | 18 | # set(EIGEN_INCLUDE_DIR "/usr/local/include/eigen3") 19 | find_package(Ceres REQUIRED) 20 | include_directories(${CERES_INCLUDE_DIRS}) 21 | 22 | 23 | catkin_package( 24 | INCLUDE_DIRS include 25 | LIBRARIES gvins_camera_model 26 | CATKIN_DEPENDS roscpp std_msgs 27 | # DEPENDS system_lib 28 | ) 29 | 30 | include_directories( 31 | ${catkin_INCLUDE_DIRS} 32 | ) 33 | 34 | include_directories("include") 35 | 36 | 37 | add_executable(gvins_Calibration 38 | src/intrinsic_calib.cc 39 | src/chessboard/Chessboard.cc 40 | src/calib/CameraCalibration.cc 41 | src/camera_models/Camera.cc 42 | src/camera_models/CameraFactory.cc 43 | src/camera_models/CostFunctionFactory.cc 44 | src/camera_models/PinholeCamera.cc 45 | src/camera_models/CataCamera.cc 46 | src/camera_models/EquidistantCamera.cc 47 | src/camera_models/ScaramuzzaCamera.cc 48 | src/sparse_graph/Transform.cc 49 | src/gpl/gpl.cc 50 | src/gpl/EigenQuaternionParameterization.cc) 51 | 52 | add_library(gvins_camera_model 53 | src/chessboard/Chessboard.cc 54 | src/calib/CameraCalibration.cc 55 | src/camera_models/Camera.cc 56 | src/camera_models/CameraFactory.cc 57 | src/camera_models/CostFunctionFactory.cc 58 | src/camera_models/PinholeCamera.cc 59 | src/camera_models/CataCamera.cc 60 | src/camera_models/EquidistantCamera.cc 61 | src/camera_models/ScaramuzzaCamera.cc 62 | src/sparse_graph/Transform.cc 63 | src/gpl/gpl.cc 64 | src/gpl/EigenQuaternionParameterization.cc) 65 | 66 | target_link_libraries(gvins_Calibration ${Boost_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES}) 67 | target_link_libraries(gvins_camera_model ${Boost_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES}) 68 | -------------------------------------------------------------------------------- /camera_model/cmake/FindEigen.cmake: -------------------------------------------------------------------------------- 1 | # Ceres Solver - A fast non-linear least squares minimizer 2 | # Copyright 2015 Google Inc. All rights reserved. 3 | # http://ceres-solver.org/ 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions are met: 7 | # 8 | # * Redistributions of source code must retain the above copyright notice, 9 | # this list of conditions and the following disclaimer. 10 | # * Redistributions in binary form must reproduce the above copyright notice, 11 | # this list of conditions and the following disclaimer in the documentation 12 | # and/or other materials provided with the distribution. 13 | # * Neither the name of Google Inc. nor the names of its contributors may be 14 | # used to endorse or promote products derived from this software without 15 | # specific prior written permission. 16 | # 17 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | # POSSIBILITY OF SUCH DAMAGE. 28 | # 29 | # Author: alexs.mac@gmail.com (Alex Stewart) 30 | # 31 | 32 | # FindEigen.cmake - Find Eigen library, version >= 3. 33 | # 34 | # This module defines the following variables: 35 | # 36 | # EIGEN_FOUND: TRUE iff Eigen is found. 37 | # EIGEN_INCLUDE_DIRS: Include directories for Eigen. 38 | # 39 | # EIGEN_VERSION: Extracted from Eigen/src/Core/util/Macros.h 40 | # EIGEN_WORLD_VERSION: Equal to 3 if EIGEN_VERSION = 3.2.0 41 | # EIGEN_MAJOR_VERSION: Equal to 2 if EIGEN_VERSION = 3.2.0 42 | # EIGEN_MINOR_VERSION: Equal to 0 if EIGEN_VERSION = 3.2.0 43 | # 44 | # The following variables control the behaviour of this module: 45 | # 46 | # EIGEN_INCLUDE_DIR_HINTS: List of additional directories in which to 47 | # search for eigen includes, e.g: /timbuktu/eigen3. 48 | # 49 | # The following variables are also defined by this module, but in line with 50 | # CMake recommended FindPackage() module style should NOT be referenced directly 51 | # by callers (use the plural variables detailed above instead). These variables 52 | # do however affect the behaviour of the module via FIND_[PATH/LIBRARY]() which 53 | # are NOT re-called (i.e. search for library is not repeated) if these variables 54 | # are set with valid values _in the CMake cache_. This means that if these 55 | # variables are set directly in the cache, either by the user in the CMake GUI, 56 | # or by the user passing -DVAR=VALUE directives to CMake when called (which 57 | # explicitly defines a cache variable), then they will be used verbatim, 58 | # bypassing the HINTS variables and other hard-coded search locations. 59 | # 60 | # EIGEN_INCLUDE_DIR: Include directory for CXSparse, not including the 61 | # include directory of any dependencies. 62 | 63 | # Called if we failed to find Eigen or any of it's required dependencies, 64 | # unsets all public (designed to be used externally) variables and reports 65 | # error message at priority depending upon [REQUIRED/QUIET/] argument. 66 | macro(EIGEN_REPORT_NOT_FOUND REASON_MSG) 67 | unset(EIGEN_FOUND) 68 | unset(EIGEN_INCLUDE_DIRS) 69 | # Make results of search visible in the CMake GUI if Eigen has not 70 | # been found so that user does not have to toggle to advanced view. 71 | mark_as_advanced(CLEAR EIGEN_INCLUDE_DIR) 72 | # Note _FIND_[REQUIRED/QUIETLY] variables defined by FindPackage() 73 | # use the camelcase library name, not uppercase. 74 | if (Eigen_FIND_QUIETLY) 75 | message(STATUS "Failed to find Eigen - " ${REASON_MSG} ${ARGN}) 76 | elseif (Eigen_FIND_REQUIRED) 77 | message(FATAL_ERROR "Failed to find Eigen - " ${REASON_MSG} ${ARGN}) 78 | else() 79 | # Neither QUIETLY nor REQUIRED, use no priority which emits a message 80 | # but continues configuration and allows generation. 81 | message("-- Failed to find Eigen - " ${REASON_MSG} ${ARGN}) 82 | endif () 83 | return() 84 | endmacro(EIGEN_REPORT_NOT_FOUND) 85 | 86 | # Protect against any alternative find_package scripts for this library having 87 | # been called previously (in a client project) which set EIGEN_FOUND, but not 88 | # the other variables we require / set here which could cause the search logic 89 | # here to fail. 90 | unset(EIGEN_FOUND) 91 | 92 | # Search user-installed locations first, so that we prefer user installs 93 | # to system installs where both exist. 94 | list(APPEND EIGEN_CHECK_INCLUDE_DIRS 95 | /usr/local/include 96 | /usr/local/homebrew/include # Mac OS X 97 | /opt/local/var/macports/software # Mac OS X. 98 | /opt/local/include 99 | /usr/include) 100 | # Additional suffixes to try appending to each search path. 101 | list(APPEND EIGEN_CHECK_PATH_SUFFIXES 102 | eigen3 # Default root directory for Eigen. 103 | Eigen/include/eigen3 # Windows (for C:/Program Files prefix) < 3.3 104 | Eigen3/include/eigen3 ) # Windows (for C:/Program Files prefix) >= 3.3 105 | 106 | # Search supplied hint directories first if supplied. 107 | find_path(EIGEN_INCLUDE_DIR 108 | NAMES Eigen/Core 109 | PATHS ${EIGEN_INCLUDE_DIR_HINTS} 110 | ${EIGEN_CHECK_INCLUDE_DIRS} 111 | PATH_SUFFIXES ${EIGEN_CHECK_PATH_SUFFIXES}) 112 | 113 | if (NOT EIGEN_INCLUDE_DIR OR 114 | NOT EXISTS ${EIGEN_INCLUDE_DIR}) 115 | eigen_report_not_found( 116 | "Could not find eigen3 include directory, set EIGEN_INCLUDE_DIR to " 117 | "path to eigen3 include directory, e.g. /usr/local/include/eigen3.") 118 | endif (NOT EIGEN_INCLUDE_DIR OR 119 | NOT EXISTS ${EIGEN_INCLUDE_DIR}) 120 | 121 | # Mark internally as found, then verify. EIGEN_REPORT_NOT_FOUND() unsets 122 | # if called. 123 | set(EIGEN_FOUND TRUE) 124 | 125 | # Extract Eigen version from Eigen/src/Core/util/Macros.h 126 | if (EIGEN_INCLUDE_DIR) 127 | set(EIGEN_VERSION_FILE ${EIGEN_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h) 128 | if (NOT EXISTS ${EIGEN_VERSION_FILE}) 129 | eigen_report_not_found( 130 | "Could not find file: ${EIGEN_VERSION_FILE} " 131 | "containing version information in Eigen install located at: " 132 | "${EIGEN_INCLUDE_DIR}.") 133 | else (NOT EXISTS ${EIGEN_VERSION_FILE}) 134 | file(READ ${EIGEN_VERSION_FILE} EIGEN_VERSION_FILE_CONTENTS) 135 | 136 | string(REGEX MATCH "#define EIGEN_WORLD_VERSION [0-9]+" 137 | EIGEN_WORLD_VERSION "${EIGEN_VERSION_FILE_CONTENTS}") 138 | string(REGEX REPLACE "#define EIGEN_WORLD_VERSION ([0-9]+)" "\\1" 139 | EIGEN_WORLD_VERSION "${EIGEN_WORLD_VERSION}") 140 | 141 | string(REGEX MATCH "#define EIGEN_MAJOR_VERSION [0-9]+" 142 | EIGEN_MAJOR_VERSION "${EIGEN_VERSION_FILE_CONTENTS}") 143 | string(REGEX REPLACE "#define EIGEN_MAJOR_VERSION ([0-9]+)" "\\1" 144 | EIGEN_MAJOR_VERSION "${EIGEN_MAJOR_VERSION}") 145 | 146 | string(REGEX MATCH "#define EIGEN_MINOR_VERSION [0-9]+" 147 | EIGEN_MINOR_VERSION "${EIGEN_VERSION_FILE_CONTENTS}") 148 | string(REGEX REPLACE "#define EIGEN_MINOR_VERSION ([0-9]+)" "\\1" 149 | EIGEN_MINOR_VERSION "${EIGEN_MINOR_VERSION}") 150 | 151 | # This is on a single line s/t CMake does not interpret it as a list of 152 | # elements and insert ';' separators which would result in 3.;2.;0 nonsense. 153 | set(EIGEN_VERSION "${EIGEN_WORLD_VERSION}.${EIGEN_MAJOR_VERSION}.${EIGEN_MINOR_VERSION}") 154 | endif (NOT EXISTS ${EIGEN_VERSION_FILE}) 155 | endif (EIGEN_INCLUDE_DIR) 156 | 157 | # Set standard CMake FindPackage variables if found. 158 | if (EIGEN_FOUND) 159 | set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR}) 160 | endif (EIGEN_FOUND) 161 | 162 | # Handle REQUIRED / QUIET optional arguments and version. 163 | include(FindPackageHandleStandardArgs) 164 | find_package_handle_standard_args(Eigen 165 | REQUIRED_VARS EIGEN_INCLUDE_DIRS 166 | VERSION_VAR EIGEN_VERSION) 167 | 168 | # Only mark internal variables as advanced if we found Eigen, otherwise 169 | # leave it visible in the standard GUI for the user to set manually. 170 | if (EIGEN_FOUND) 171 | mark_as_advanced(FORCE EIGEN_INCLUDE_DIR) 172 | endif (EIGEN_FOUND) 173 | -------------------------------------------------------------------------------- /camera_model/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 | -------------------------------------------------------------------------------- /camera_model/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 | -------------------------------------------------------------------------------- /camera_model/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 | -------------------------------------------------------------------------------- /camera_model/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 | -------------------------------------------------------------------------------- /camera_model/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 | -------------------------------------------------------------------------------- /camera_model/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 | -------------------------------------------------------------------------------- /camera_model/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 | -------------------------------------------------------------------------------- /camera_model/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 | -------------------------------------------------------------------------------- /camera_model/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 | -------------------------------------------------------------------------------- /camera_model/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 | -------------------------------------------------------------------------------- /camera_model/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 | -------------------------------------------------------------------------------- /camera_model/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 | -------------------------------------------------------------------------------- /camera_model/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 | -------------------------------------------------------------------------------- /camera_model/instruction: -------------------------------------------------------------------------------- 1 | rosrun camera_model Calibration -w 8 -h 11 -s 70 -i ~/bag/PX/calib/ 2 | -------------------------------------------------------------------------------- /camera_model/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | gvins_camera_model 4 | 0.0.0 5 | The camera_model package 6 | 7 | 8 | 9 | 10 | dvorak 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | roscpp 44 | std_msgs 45 | roscpp 46 | std_msgs 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | -------------------------------------------------------------------------------- /camera_model/readme.md: -------------------------------------------------------------------------------- 1 | part of [camodocal](https://github.com/hengli/camodocal) 2 | 3 | [Google Ceres](http://ceres-solver.org) is needed. 4 | 5 | # Calibration: 6 | 7 | Use [intrinsic_calib.cc](https://github.com/dvorak0/camera_model/blob/master/src/intrinsic_calib.cc) to calibrate your camera. 8 | 9 | # Undistortion: 10 | 11 | See [Camera.h](https://github.com/dvorak0/camera_model/blob/master/include/camodocal/camera_models/Camera.h) for general interface: 12 | 13 | - liftProjective: Lift points from the image plane to the projective space. 14 | - spaceToPlane: Projects 3D points to the image plane (Pi function) 15 | 16 | -------------------------------------------------------------------------------- /camera_model/src/camera_models/Camera.cc: -------------------------------------------------------------------------------- 1 | #include "camodocal/camera_models/Camera.h" 2 | #include "camodocal/camera_models/ScaramuzzaCamera.h" 3 | 4 | #include 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 | -------------------------------------------------------------------------------- /camera_model/src/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 | -------------------------------------------------------------------------------- /camera_model/src/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 | -------------------------------------------------------------------------------- /camera_model/src/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 | -------------------------------------------------------------------------------- /config/gvins_rviz_config.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: ~ 7 | Splitter Ratio: 0.5 8 | Tree Height: 236 9 | - Class: rviz/Selection 10 | Name: Selection 11 | - Class: rviz/Tool Properties 12 | Expanded: 13 | - /2D Pose Estimate1 14 | - /2D Nav Goal1 15 | - /Publish Point1 16 | Name: Tool Properties 17 | Splitter Ratio: 0.588679016 18 | - Class: rviz/Views 19 | Expanded: 20 | - /Current View1 21 | Name: Views 22 | Splitter Ratio: 0.5 23 | - Class: rviz/Time 24 | Experimental: false 25 | Name: Time 26 | SyncMode: 0 27 | SyncSource: Image 28 | Toolbars: 29 | toolButtonStyle: 2 30 | Visualization Manager: 31 | Class: "" 32 | Displays: 33 | - Alpha: 0.5 34 | Cell Size: 10 35 | Class: rviz/Grid 36 | Color: 160; 160; 164 37 | Enabled: true 38 | Line Style: 39 | Line Width: 0.0299999993 40 | Value: Lines 41 | Name: Grid 42 | Normal Cell Count: 0 43 | Offset: 44 | X: 0 45 | Y: 0 46 | Z: 0 47 | Plane: XY 48 | Plane Cell Count: 50 49 | Reference Frame: 50 | Value: true 51 | - Class: rviz/Axes 52 | Enabled: true 53 | Length: 5 54 | Name: Axes 55 | Radius: 0.300000012 56 | Reference Frame: 57 | Value: true 58 | - Class: rviz/Image 59 | Enabled: true 60 | Image Topic: /cam1/image_raw 61 | Max Value: 1 62 | Median window: 5 63 | Min Value: 0 64 | Name: Image 65 | Normalize Range: true 66 | Queue Size: 2 67 | Transport Hint: raw 68 | Unreliable: false 69 | Value: true 70 | - Class: rviz/Image 71 | Enabled: true 72 | Image Topic: /gvins_feature_tracker/feature_img 73 | Max Value: 1 74 | Median window: 5 75 | Min Value: 0 76 | Name: Image 77 | Normalize Range: true 78 | Queue Size: 2 79 | Transport Hint: raw 80 | Unreliable: false 81 | Value: true 82 | - Alpha: 1 83 | Buffer Length: 1 84 | Class: rviz/Path 85 | Color: 0; 85; 255 86 | Enabled: true 87 | Head Diameter: 0.300000012 88 | Head Length: 0.200000003 89 | Length: 0.300000012 90 | Line Style: Lines 91 | Line Width: 0.0299999993 92 | Name: Path 93 | Offset: 94 | X: 0 95 | Y: 0 96 | Z: 0 97 | Pose Color: 255; 85; 255 98 | Pose Style: None 99 | Radius: 0.0299999993 100 | Shaft Diameter: 0.100000001 101 | Shaft Length: 0.100000001 102 | Topic: /gvins/gnss_enu_path 103 | Unreliable: false 104 | Value: true 105 | - Alpha: 1 106 | Buffer Length: 1 107 | Class: rviz/Path 108 | Color: 25; 255; 0 109 | Enabled: true 110 | Head Diameter: 0.300000012 111 | Head Length: 0.200000003 112 | Length: 0.300000012 113 | Line Style: Lines 114 | Line Width: 0.0299999993 115 | Name: Path 116 | Offset: 117 | X: 0 118 | Y: 0 119 | Z: 0 120 | Pose Color: 255; 85; 255 121 | Pose Style: None 122 | Radius: 0.0299999993 123 | Shaft Diameter: 0.100000001 124 | Shaft Length: 0.100000001 125 | Topic: /gvins/path 126 | Unreliable: false 127 | Value: true 128 | Enabled: true 129 | Global Options: 130 | Background Color: 48; 48; 48 131 | Default Light: true 132 | Fixed Frame: world 133 | Frame Rate: 30 134 | Name: root 135 | Tools: 136 | - Class: rviz/Interact 137 | Hide Inactive Objects: true 138 | - Class: rviz/MoveCamera 139 | - Class: rviz/Select 140 | - Class: rviz/FocusCamera 141 | - Class: rviz/Measure 142 | - Class: rviz/SetInitialPose 143 | Topic: /initialpose 144 | - Class: rviz/SetGoal 145 | Topic: /move_base_simple/goal 146 | - Class: rviz/PublishPoint 147 | Single click: true 148 | Topic: /clicked_point 149 | Value: true 150 | Views: 151 | Current: 152 | Class: rviz/Orbit 153 | Distance: 305.921631 154 | Enable Stereo Rendering: 155 | Stereo Eye Separation: 0.0599999987 156 | Stereo Focal Distance: 1 157 | Swap Stereo Eyes: false 158 | Value: false 159 | Focal Point: 160 | X: 16.1296978 161 | Y: 25.7230911 162 | Z: -1.10709679 163 | Focal Shape Fixed Size: true 164 | Focal Shape Size: 0.0500000007 165 | Invert Z Axis: false 166 | Name: Current View 167 | Near Clip Distance: 0.00999999978 168 | Pitch: 1.56979632 169 | Target Frame: 170 | Value: Orbit (rviz) 171 | Yaw: 5.08356237 172 | Saved: ~ 173 | Window Geometry: 174 | Displays: 175 | collapsed: false 176 | Height: 1056 177 | Hide Left Dock: false 178 | Hide Right Dock: true 179 | Image: 180 | collapsed: false 181 | QMainWindow State: 000000ff00000000fd0000000400000000000001d200000391fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006200fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000000a0049006d006100670065010000002b000000f60000001700fffffffb0000000a0049006d0061006700650100000127000001110000001700fffffffb000000100044006900730070006c006100790073010000023e0000017e000000da00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000391fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002b00000391000000b200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000035200fffffffb0000000800540069006d00650100000000000004500000000000000000000005a80000039100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 182 | Selection: 183 | collapsed: false 184 | Time: 185 | collapsed: false 186 | Tool Properties: 187 | collapsed: false 188 | Views: 189 | collapsed: true 190 | Width: 1920 191 | X: 0 192 | Y: 464 193 | -------------------------------------------------------------------------------- /config/simulator/simulator_config.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #common parameters 4 | imu_topic: "/simulator/imu0" 5 | image_topic: "/cam0/image_raw" 6 | output_path: "/home/shaozu/output/" 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 | gnss_enable: 1 25 | gnss_meas_topic: "/simulator/gnss0_meas" 26 | gnss_ephem_topic: "/simulator/gnss0_ephem" 27 | gnss_glo_ephem_topic: "/simulator/gnss0_gloephem" 28 | 29 | # Extrinsic parameter between IMU and Camera. 30 | estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it. 31 | # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess. 32 | # 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. 33 | #If you choose 0 or 1, you should write down the following matrix. 34 | #Rotation from camera frame to imu frame, imu^R_cam 35 | extrinsicRotation: !!opencv-matrix 36 | rows: 3 37 | cols: 3 38 | dt: d 39 | data: [0.0148655429818, -0.999880929698, 0.00414029679422, 40 | 0.999557249008, 0.0149672133247, 0.025715529948, 41 | -0.0257744366974, 0.00375618835797, 0.999660727178] 42 | #Translation from camera frame to imu frame, imu^T_cam 43 | extrinsicTranslation: !!opencv-matrix 44 | rows: 3 45 | cols: 1 46 | dt: d 47 | data: [0.03,-0.05, 0] 48 | 49 | #feature traker paprameters 50 | max_cnt: 150 # max feature number in feature tracking 51 | min_dist: 30 # min distance between two features 52 | 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 53 | F_threshold: 1.0 # ransac threshold (pixel) 54 | show_track: 1 # publish tracking image as topic 55 | equalize: 1 # if image is too dark or light, trun on equalize to find enough features 56 | fisheye: 0 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points 57 | 58 | #optimization parameters 59 | max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time 60 | max_num_iterations: 8 # max solver itrations, to guarantee real time 61 | keyframe_parallax: 10.0 # keyframe selection threshold (pixel) 62 | 63 | #imu parameters The more accurate parameters you provide, the better performance 64 | acc_n: 0.1 # accelerometer measurement noise standard deviation. #0.2 0.04 65 | gyr_n: 0.01 # gyroscope measurement noise standard deviation. #0.05 0.004 66 | acc_w: 0.001 # accelerometer bias random work noise standard deviation. #0.02 67 | gyr_w: 0.0001 # gyroscope bias random work noise standard deviation. #4.0e-5 68 | g_norm: 9.805 # gravity magnitude 69 | 70 | #unsynchronization parameters 71 | estimate_td: 0 # online estimate time offset between camera and imu 72 | td: 0.0 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock) 73 | -------------------------------------------------------------------------------- /config/visensor_f9p/visensor_left_f9p_config.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #common parameters 4 | imu_topic: "/imu0" 5 | image_topic: "/cam1/image_raw" 6 | output_dir: "/home/SLAM/GVINS/output/" 7 | 8 | #camera calibration 9 | model_type: MEI 10 | camera_name: camera 11 | image_width: 752 12 | image_height: 480 13 | mirror_parameters: 14 | xi: 1.8476540167437447 15 | distortion_parameters: 16 | k1: -0.06597811223735722 17 | k2: 0.8559479340704287 18 | p1: -0.0006445829733139821 19 | p2: 0.0015137487236065916 20 | projection_parameters: 21 | gamma1: 1338.1845333957547 22 | gamma2: 1340.1190112672946 23 | u0: 378.7909740462579 24 | v0: 217.69105287172025 25 | 26 | gnss_enable: 1 27 | gnss_meas_topic: "/ublox_driver/range_meas" # GNSS raw measurement topic 28 | gnss_ephem_topic: "/ublox_driver/ephem" # GPS, Galileo, BeiDou ephemeris 29 | gnss_glo_ephem_topic: "/ublox_driver/glo_ephem" # GLONASS ephemeris 30 | gnss_iono_params_topic: "/ublox_driver/iono_params" # GNSS broadcast ionospheric parameters 31 | gnss_tp_info_topic: "/ublox_driver/time_pulse_info" # PPS time info 32 | gnss_elevation_thres: 30 # satellite elevation threshold (degree) 33 | gnss_psr_std_thres: 2.0 # pseudo-range std threshold 34 | gnss_dopp_std_thres: 2.0 # doppler std threshold 35 | gnss_track_num_thres: 20 # number of satellite tracking epochs before entering estimator 36 | gnss_ddt_sigma: 0.1 37 | 38 | gnss_local_online_sync: 1 # if perform online synchronization betwen GNSS and local time 39 | local_trigger_info_topic: "/external_trigger" # external trigger info of the local sensor, if `gnss_local_online_sync` is 1 40 | gnss_local_time_diff: 18.0 # difference between GNSS and local time (s), if `gnss_local_online_sync` is 0 41 | 42 | gnss_iono_default_parameters: !!opencv-matrix 43 | rows: 1 44 | cols: 8 45 | dt: d 46 | data: [0.1118E-07, 0.2235E-07, -0.4172E-06, 0.6557E-06, 47 | 0.1249E+06, -0.4424E+06, 0.1507E+07, -0.2621E+06] 48 | 49 | # Extrinsic parameter between IMU and Camera. 50 | estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it. 51 | # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess. 52 | # 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. 53 | #If you choose 0 or 1, you should write down the following matrix. 54 | #Rotation from camera frame to imu frame, imu^R_cam 55 | extrinsicRotation: !!opencv-matrix 56 | rows: 3 57 | cols: 3 58 | dt: d 59 | data: [0.9999890386957373, -0.0043227774403168, 0.0017989117755288, 60 | 0.0043276579084841, 0.9999869417854389, -0.0027180205355500, 61 | -0.0017871388870994, 0.0027257758172719, 0.9999946881262878] 62 | #Translation from camera frame to imu frame, imu^T_cam 63 | extrinsicTranslation: !!opencv-matrix 64 | rows: 3 65 | cols: 1 66 | dt: d 67 | data: [-0.0759472920952561, -0.0039320527565750, -0.0016395029500217] 68 | 69 | #feature traker paprameters 70 | max_cnt: 150 # max feature number in feature tracking 71 | min_dist: 30 # min distance between two features 72 | freq: 0 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image 73 | F_threshold: 1.0 # ransac threshold (pixel) 74 | show_track: 1 # publish tracking image as topic 75 | equalize: 1 # if image is too dark or light, trun on equalize to find enough features 76 | fisheye: 0 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points 77 | 78 | #optimization parameters 79 | max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time 80 | max_num_iterations: 8 # max solver itrations, to guarantee real time 81 | keyframe_parallax: 10.0 # keyframe selection threshold (pixel) 82 | 83 | #imu parameters The more accurate parameters you provide, the better performance 84 | acc_n: 0.08 # accelerometer measurement noise standard deviation. #0.2 0.04 85 | gyr_n: 0.004 # gyroscope measurement noise standard deviation. #0.05 0.004 86 | acc_w: 0.00004 # accelerometer bias random work noise standard deviation. #0.02 87 | gyr_w: 2.0e-6 # gyroscope bias random work noise standard deviation. #4.0e-5 88 | g_norm: 9.787561 # gravity magnitude 89 | 90 | #unsynchronization parameters 91 | estimate_td: 0 # online estimate time offset between camera and imu 92 | td: 0.0 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock) 93 | -------------------------------------------------------------------------------- /docker/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ros:kinetic-perception 2 | 3 | ENV EIGEN_VERSION="3.3.3" 4 | ENV CERES_VERSION="1.12.0" 5 | ENV CATKIN_WS=/root/catkin_ws 6 | 7 | # set up thread number for building 8 | RUN if [ "x$(nproc)" = "x1" ] ; then export USE_PROC=1 ; \ 9 | else export USE_PROC=$(($(nproc)/2)) ; fi && \ 10 | apt-get update && apt-get install -y \ 11 | git \ 12 | cmake \ 13 | libatlas-base-dev \ 14 | libgoogle-glog-dev \ 15 | libsuitesparse-dev \ 16 | python-catkin-tools \ 17 | ros-${ROS_DISTRO}-cv-bridge \ 18 | ros-${ROS_DISTRO}-image-transport \ 19 | ros-${ROS_DISTRO}-message-filters \ 20 | ros-${ROS_DISTRO}-tf && \ 21 | rm -rf /var/lib/apt/lists/* && \ 22 | # install eigen 23 | git clone https://gitlab.com/libeigen/eigen.git && \ 24 | cd eigen && \ 25 | git checkout tags/${EIGEN_VERSION} && \ 26 | mkdir build && cd build && \ 27 | cmake .. && make install && \ 28 | cd ../.. && rm -rf eigen && \ 29 | # Build and install Ceres 30 | git clone https://ceres-solver.googlesource.com/ceres-solver && \ 31 | cd ceres-solver && \ 32 | git checkout tags/${CERES_VERSION} && \ 33 | mkdir build && cd build && \ 34 | cmake .. && \ 35 | make -j$(USE_PROC) install && \ 36 | cd ../.. && rm -rf ceres-solver && \ 37 | # create GVINS directory 38 | mkdir -p $CATKIN_WS/src/GVINS/ && \ 39 | cd $CATKIN_WS/src && \ 40 | # clone gnss_comm repo 41 | git clone https://github.com/HKUST-Aerial-Robotics/gnss_comm.git 42 | 43 | # Copy the local replica of GVINS 44 | COPY ./ $CATKIN_WS/src/GVINS/ 45 | # comment the above line and use the following command if you only have this dockerfile 46 | # RUN git clone https://github.com/HKUST-Aerial-Robotics/GVINS.git 47 | 48 | # Build GVINS 49 | WORKDIR $CATKIN_WS 50 | ENV TERM xterm 51 | ENV PYTHONIOENCODING UTF-8 52 | RUN catkin config \ 53 | --extend /opt/ros/$ROS_DISTRO \ 54 | --cmake-args \ 55 | -DCMAKE_BUILD_TYPE=Release && \ 56 | catkin build && \ 57 | sed -i '/exec "$@"/i \ 58 | source "/root/catkin_ws/devel/setup.bash"' /ros_entrypoint.sh 59 | -------------------------------------------------------------------------------- /docker/Makefile: -------------------------------------------------------------------------------- 1 | all: help 2 | 3 | help: 4 | @echo "" 5 | @echo "-- Help Menu" 6 | @echo "" 7 | @echo " 1. make build - build all images" 8 | @echo " 1. make clean - remove all images" 9 | @echo "" 10 | 11 | build: 12 | @docker build --tag gvins -f ./Dockerfile .. 13 | 14 | clean: 15 | @docker rmi -f gvins 16 | -------------------------------------------------------------------------------- /docker/run.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | trap : SIGTERM SIGINT 3 | 4 | function abspath() { 5 | # generate absolute path from relative path 6 | # $1 : relative filename 7 | # return : absolute path 8 | if [ -d "$1" ]; then 9 | # dir 10 | (cd "$1"; pwd) 11 | elif [ -f "$1" ]; then 12 | # file 13 | if [[ $1 = /* ]]; then 14 | echo "$1" 15 | elif [[ $1 == */* ]]; then 16 | echo "$(cd "${1%/*}"; pwd)/${1##*/}" 17 | else 18 | echo "$(pwd)/$1" 19 | fi 20 | fi 21 | } 22 | 23 | if [ "$#" -ne 1 ]; then 24 | echo "Usage: $0 LAUNCH_FILE" >&2 25 | exit 1 26 | fi 27 | 28 | roscore & 29 | ROSCORE_PID=$! 30 | sleep 1 31 | 32 | rviz -d ../config/gvins_rviz_config.rviz & 33 | RVIZ_PID=$! 34 | 35 | GVINS_DIR=$(abspath "..") 36 | 37 | docker run \ 38 | -it \ 39 | --rm \ 40 | --net=host \ 41 | -v ${GVINS_DIR}:/root/catkin_ws/src/GVINS/ \ 42 | gvins:latest \ 43 | /bin/bash -c \ 44 | "cd /root/catkin_ws/; \ 45 | catkin config \ 46 | --env-cache \ 47 | --extend /opt/ros/$ROS_DISTRO \ 48 | --cmake-args \ 49 | -DCMAKE_BUILD_TYPE=Release; \ 50 | catkin build; \ 51 | source devel/setup.bash; \ 52 | roslaunch gvins ${1}" 53 | 54 | wait $ROSCORE_PID 55 | wait $RVIZ_PID 56 | 57 | if [[ $? -gt 128 ]] 58 | then 59 | kill $ROSCORE_PID 60 | kill $RVIZ_PID 61 | fi 62 | -------------------------------------------------------------------------------- /estimator/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(gvins) 3 | 4 | set(CMAKE_BUILD_TYPE "Release") 5 | set(CMAKE_CXX_FLAGS "-std=c++11") 6 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g") 7 | 8 | find_package(catkin REQUIRED COMPONENTS 9 | roscpp 10 | std_msgs 11 | geometry_msgs 12 | nav_msgs 13 | tf 14 | cv_bridge 15 | rosbag 16 | message_generation 17 | gnss_comm 18 | ) 19 | 20 | add_message_files( 21 | DIRECTORY msg 22 | FILES LocalSensorExternalTrigger.msg 23 | ) 24 | generate_messages(DEPENDENCIES std_msgs) 25 | 26 | find_package(OpenCV REQUIRED) 27 | 28 | find_package(Ceres REQUIRED) 29 | 30 | include_directories(${catkin_INCLUDE_DIRS} ${CERES_INCLUDE_DIRS}) 31 | 32 | set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) 33 | find_package(Eigen3) 34 | include_directories( 35 | ${catkin_INCLUDE_DIRS} 36 | ${EIGEN3_INCLUDE_DIR} 37 | ) 38 | 39 | catkin_package() 40 | 41 | add_executable(${PROJECT_NAME} 42 | src/estimator_node.cpp 43 | src/parameters.cpp 44 | src/estimator.cpp 45 | src/feature_manager.cpp 46 | src/factor/pose_local_parameterization.cpp 47 | src/factor/projection_factor.cpp 48 | src/factor/projection_td_factor.cpp 49 | src/factor/marginalization_factor.cpp 50 | src/factor/gnss_psr_dopp_factor.cpp 51 | src/factor/gnss_dt_ddt_factor.cpp 52 | src/factor/gnss_dt_anchor_factor.cpp 53 | src/factor/gnss_ddt_smooth_factor.cpp 54 | src/factor/pos_vel_factor.cpp 55 | src/factor/pose_anchor_factor.cpp 56 | src/utility/utility.cpp 57 | src/utility/visualization.cpp 58 | src/utility/CameraPoseVisualization.cpp 59 | src/initial/solve_5pts.cpp 60 | src/initial/initial_aligment.cpp 61 | src/initial/initial_sfm.cpp 62 | src/initial/initial_ex_rotation.cpp 63 | src/initial/gnss_vi_initializer.cpp 64 | ) 65 | 66 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES}) 67 | add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_generate_messages_cpp) 68 | # add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) 69 | -------------------------------------------------------------------------------- /estimator/cmake/FindEigen.cmake: -------------------------------------------------------------------------------- 1 | # Ceres Solver - A fast non-linear least squares minimizer 2 | # Copyright 2015 Google Inc. All rights reserved. 3 | # http://ceres-solver.org/ 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions are met: 7 | # 8 | # * Redistributions of source code must retain the above copyright notice, 9 | # this list of conditions and the following disclaimer. 10 | # * Redistributions in binary form must reproduce the above copyright notice, 11 | # this list of conditions and the following disclaimer in the documentation 12 | # and/or other materials provided with the distribution. 13 | # * Neither the name of Google Inc. nor the names of its contributors may be 14 | # used to endorse or promote products derived from this software without 15 | # specific prior written permission. 16 | # 17 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | # POSSIBILITY OF SUCH DAMAGE. 28 | # 29 | # Author: alexs.mac@gmail.com (Alex Stewart) 30 | # 31 | 32 | # FindEigen.cmake - Find Eigen library, version >= 3. 33 | # 34 | # This module defines the following variables: 35 | # 36 | # EIGEN_FOUND: TRUE iff Eigen is found. 37 | # EIGEN_INCLUDE_DIRS: Include directories for Eigen. 38 | # 39 | # EIGEN_VERSION: Extracted from Eigen/src/Core/util/Macros.h 40 | # EIGEN_WORLD_VERSION: Equal to 3 if EIGEN_VERSION = 3.2.0 41 | # EIGEN_MAJOR_VERSION: Equal to 2 if EIGEN_VERSION = 3.2.0 42 | # EIGEN_MINOR_VERSION: Equal to 0 if EIGEN_VERSION = 3.2.0 43 | # 44 | # The following variables control the behaviour of this module: 45 | # 46 | # EIGEN_INCLUDE_DIR_HINTS: List of additional directories in which to 47 | # search for eigen includes, e.g: /timbuktu/eigen3. 48 | # 49 | # The following variables are also defined by this module, but in line with 50 | # CMake recommended FindPackage() module style should NOT be referenced directly 51 | # by callers (use the plural variables detailed above instead). These variables 52 | # do however affect the behaviour of the module via FIND_[PATH/LIBRARY]() which 53 | # are NOT re-called (i.e. search for library is not repeated) if these variables 54 | # are set with valid values _in the CMake cache_. This means that if these 55 | # variables are set directly in the cache, either by the user in the CMake GUI, 56 | # or by the user passing -DVAR=VALUE directives to CMake when called (which 57 | # explicitly defines a cache variable), then they will be used verbatim, 58 | # bypassing the HINTS variables and other hard-coded search locations. 59 | # 60 | # EIGEN_INCLUDE_DIR: Include directory for CXSparse, not including the 61 | # include directory of any dependencies. 62 | 63 | # Called if we failed to find Eigen or any of it's required dependencies, 64 | # unsets all public (designed to be used externally) variables and reports 65 | # error message at priority depending upon [REQUIRED/QUIET/] argument. 66 | macro(EIGEN_REPORT_NOT_FOUND REASON_MSG) 67 | unset(EIGEN_FOUND) 68 | unset(EIGEN_INCLUDE_DIRS) 69 | # Make results of search visible in the CMake GUI if Eigen has not 70 | # been found so that user does not have to toggle to advanced view. 71 | mark_as_advanced(CLEAR EIGEN_INCLUDE_DIR) 72 | # Note _FIND_[REQUIRED/QUIETLY] variables defined by FindPackage() 73 | # use the camelcase library name, not uppercase. 74 | if (Eigen_FIND_QUIETLY) 75 | message(STATUS "Failed to find Eigen - " ${REASON_MSG} ${ARGN}) 76 | elseif (Eigen_FIND_REQUIRED) 77 | message(FATAL_ERROR "Failed to find Eigen - " ${REASON_MSG} ${ARGN}) 78 | else() 79 | # Neither QUIETLY nor REQUIRED, use no priority which emits a message 80 | # but continues configuration and allows generation. 81 | message("-- Failed to find Eigen - " ${REASON_MSG} ${ARGN}) 82 | endif () 83 | return() 84 | endmacro(EIGEN_REPORT_NOT_FOUND) 85 | 86 | # Protect against any alternative find_package scripts for this library having 87 | # been called previously (in a client project) which set EIGEN_FOUND, but not 88 | # the other variables we require / set here which could cause the search logic 89 | # here to fail. 90 | unset(EIGEN_FOUND) 91 | 92 | # Search user-installed locations first, so that we prefer user installs 93 | # to system installs where both exist. 94 | list(APPEND EIGEN_CHECK_INCLUDE_DIRS 95 | /usr/local/include 96 | /usr/local/homebrew/include # Mac OS X 97 | /opt/local/var/macports/software # Mac OS X. 98 | /opt/local/include 99 | /usr/include) 100 | # Additional suffixes to try appending to each search path. 101 | list(APPEND EIGEN_CHECK_PATH_SUFFIXES 102 | eigen3 # Default root directory for Eigen. 103 | Eigen/include/eigen3 # Windows (for C:/Program Files prefix) < 3.3 104 | Eigen3/include/eigen3 ) # Windows (for C:/Program Files prefix) >= 3.3 105 | 106 | # Search supplied hint directories first if supplied. 107 | find_path(EIGEN_INCLUDE_DIR 108 | NAMES Eigen/Core 109 | PATHS ${EIGEN_INCLUDE_DIR_HINTS} 110 | ${EIGEN_CHECK_INCLUDE_DIRS} 111 | PATH_SUFFIXES ${EIGEN_CHECK_PATH_SUFFIXES}) 112 | 113 | if (NOT EIGEN_INCLUDE_DIR OR 114 | NOT EXISTS ${EIGEN_INCLUDE_DIR}) 115 | eigen_report_not_found( 116 | "Could not find eigen3 include directory, set EIGEN_INCLUDE_DIR to " 117 | "path to eigen3 include directory, e.g. /usr/local/include/eigen3.") 118 | endif (NOT EIGEN_INCLUDE_DIR OR 119 | NOT EXISTS ${EIGEN_INCLUDE_DIR}) 120 | 121 | # Mark internally as found, then verify. EIGEN_REPORT_NOT_FOUND() unsets 122 | # if called. 123 | set(EIGEN_FOUND TRUE) 124 | 125 | # Extract Eigen version from Eigen/src/Core/util/Macros.h 126 | if (EIGEN_INCLUDE_DIR) 127 | set(EIGEN_VERSION_FILE ${EIGEN_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h) 128 | if (NOT EXISTS ${EIGEN_VERSION_FILE}) 129 | eigen_report_not_found( 130 | "Could not find file: ${EIGEN_VERSION_FILE} " 131 | "containing version information in Eigen install located at: " 132 | "${EIGEN_INCLUDE_DIR}.") 133 | else (NOT EXISTS ${EIGEN_VERSION_FILE}) 134 | file(READ ${EIGEN_VERSION_FILE} EIGEN_VERSION_FILE_CONTENTS) 135 | 136 | string(REGEX MATCH "#define EIGEN_WORLD_VERSION [0-9]+" 137 | EIGEN_WORLD_VERSION "${EIGEN_VERSION_FILE_CONTENTS}") 138 | string(REGEX REPLACE "#define EIGEN_WORLD_VERSION ([0-9]+)" "\\1" 139 | EIGEN_WORLD_VERSION "${EIGEN_WORLD_VERSION}") 140 | 141 | string(REGEX MATCH "#define EIGEN_MAJOR_VERSION [0-9]+" 142 | EIGEN_MAJOR_VERSION "${EIGEN_VERSION_FILE_CONTENTS}") 143 | string(REGEX REPLACE "#define EIGEN_MAJOR_VERSION ([0-9]+)" "\\1" 144 | EIGEN_MAJOR_VERSION "${EIGEN_MAJOR_VERSION}") 145 | 146 | string(REGEX MATCH "#define EIGEN_MINOR_VERSION [0-9]+" 147 | EIGEN_MINOR_VERSION "${EIGEN_VERSION_FILE_CONTENTS}") 148 | string(REGEX REPLACE "#define EIGEN_MINOR_VERSION ([0-9]+)" "\\1" 149 | EIGEN_MINOR_VERSION "${EIGEN_MINOR_VERSION}") 150 | 151 | # This is on a single line s/t CMake does not interpret it as a list of 152 | # elements and insert ';' separators which would result in 3.;2.;0 nonsense. 153 | set(EIGEN_VERSION "${EIGEN_WORLD_VERSION}.${EIGEN_MAJOR_VERSION}.${EIGEN_MINOR_VERSION}") 154 | endif (NOT EXISTS ${EIGEN_VERSION_FILE}) 155 | endif (EIGEN_INCLUDE_DIR) 156 | 157 | # Set standard CMake FindPackage variables if found. 158 | if (EIGEN_FOUND) 159 | set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR}) 160 | endif (EIGEN_FOUND) 161 | 162 | # Handle REQUIRED / QUIET optional arguments and version. 163 | include(FindPackageHandleStandardArgs) 164 | find_package_handle_standard_args(Eigen 165 | REQUIRED_VARS EIGEN_INCLUDE_DIRS 166 | VERSION_VAR EIGEN_VERSION) 167 | 168 | # Only mark internal variables as advanced if we found Eigen, otherwise 169 | # leave it visible in the standard GUI for the user to set manually. 170 | if (EIGEN_FOUND) 171 | mark_as_advanced(FORCE EIGEN_INCLUDE_DIR) 172 | endif (EIGEN_FOUND) 173 | -------------------------------------------------------------------------------- /estimator/launch/simulator.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /estimator/launch/visensor_f9p.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /estimator/msg/LocalSensorExternalTrigger.msg: -------------------------------------------------------------------------------- 1 | # This message contains essential time information of the 2 | # local sensor (visual/inertial sensor) when get triggered 3 | Header header # local timestamp of the trigger 4 | uint32 trigger_id 5 | uint32 event_id 6 | time timestamp_host # corresponding timestamp of the Host PC, not used in GVINS -------------------------------------------------------------------------------- /estimator/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | gvins 4 | 1.0.0 5 | The GVINS package 6 | 7 | CAO Shaozu 8 | 9 | GPLv3 10 | 11 | catkin 12 | roscpp 13 | message_generation 14 | gnss_comm 15 | 16 | roscpp 17 | message_generation 18 | gnss_comm 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /estimator/src/estimator.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include "parameters.h" 5 | #include "feature_manager.h" 6 | #include "utility/utility.h" 7 | #include "utility/tic_toc.h" 8 | #include "initial/solve_5pts.h" 9 | #include "initial/initial_sfm.h" 10 | #include "initial/initial_alignment.h" 11 | #include "initial/initial_ex_rotation.h" 12 | #include "initial/gnss_vi_initializer.h" 13 | #include 14 | 15 | #include 16 | #include "factor/imu_factor.h" 17 | #include "factor/pose_local_parameterization.h" 18 | #include "factor/projection_factor.h" 19 | #include "factor/projection_td_factor.h" 20 | #include "factor/marginalization_factor.h" 21 | #include "factor/gnss_psr_dopp_factor.hpp" 22 | #include "factor/gnss_dt_ddt_factor.hpp" 23 | #include "factor/gnss_dt_anchor_factor.hpp" 24 | #include "factor/gnss_ddt_smooth_factor.hpp" 25 | #include "factor/pos_vel_factor.hpp" 26 | #include "factor/pose_anchor_factor.h" 27 | 28 | #include 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | using namespace gnss_comm; 35 | 36 | class Estimator 37 | { 38 | public: 39 | Estimator(); 40 | 41 | void setParameter(); 42 | 43 | // interface 44 | void processIMU(double t, const Vector3d &linear_acceleration, const Vector3d &angular_velocity); 45 | void processGNSS(const std::vector &gnss_mea); 46 | void inputEphem(EphemBasePtr ephem_ptr); 47 | void inputIonoParams(double ts, const std::vector &iono_params); 48 | void inputGNSSTimeDiff(const double t_diff); 49 | 50 | void processImage(const map>>> &image, const std_msgs::Header &header); 51 | 52 | // internal 53 | void clearState(); 54 | bool initialStructure(); 55 | bool visualInitialAlign(); 56 | // GNSS related 57 | bool GNSSVIAlign(); 58 | 59 | void updateGNSSStatistics(); 60 | 61 | bool relativePose(Matrix3d &relative_R, Vector3d &relative_T, int &l); 62 | void slideWindow(); 63 | void solveOdometry(); 64 | void slideWindowNew(); 65 | void slideWindowOld(); 66 | void optimization(); 67 | void vector2double(); 68 | void double2vector(); 69 | bool failureDetection(); 70 | 71 | enum SolverFlag 72 | { 73 | INITIAL, 74 | NON_LINEAR 75 | }; 76 | 77 | enum MarginalizationFlag 78 | { 79 | MARGIN_OLD = 0, 80 | MARGIN_SECOND_NEW = 1 81 | }; 82 | 83 | SolverFlag solver_flag; 84 | MarginalizationFlag marginalization_flag; 85 | Vector3d g; 86 | // MatrixXd Ap[2]; 87 | // VectorXd bp[2]; 88 | 89 | Matrix3d ric[NUM_OF_CAM]; 90 | Vector3d tic[NUM_OF_CAM]; 91 | 92 | Vector3d Ps[(WINDOW_SIZE + 1)]; 93 | Vector3d Vs[(WINDOW_SIZE + 1)]; 94 | Matrix3d Rs[(WINDOW_SIZE + 1)]; 95 | Vector3d Bas[(WINDOW_SIZE + 1)]; 96 | Vector3d Bgs[(WINDOW_SIZE + 1)]; 97 | double td; 98 | 99 | Matrix3d back_R0, last_R, last_R0; 100 | Vector3d back_P0, last_P, last_P0; 101 | std_msgs::Header Headers[(WINDOW_SIZE + 1)]; 102 | 103 | IntegrationBase *pre_integrations[(WINDOW_SIZE + 1)]; 104 | Vector3d acc_0, gyr_0; 105 | 106 | vector dt_buf[(WINDOW_SIZE + 1)]; 107 | vector linear_acceleration_buf[(WINDOW_SIZE + 1)]; 108 | vector angular_velocity_buf[(WINDOW_SIZE + 1)]; 109 | 110 | // GNSS related 111 | bool gnss_ready; 112 | Eigen::Vector3d anc_ecef; 113 | Eigen::Matrix3d R_ecef_enu; 114 | double yaw_enu_local; 115 | std::vector gnss_meas_buf[(WINDOW_SIZE+1)]; 116 | std::vector gnss_ephem_buf[(WINDOW_SIZE+1)]; 117 | std::vector latest_gnss_iono_params; 118 | std::map> sat2ephem; 119 | std::map> sat2time_index; 120 | std::map sat_track_status; 121 | double para_anc_ecef[3]; 122 | double para_yaw_enu_local[1]; 123 | double para_rcv_dt[(WINDOW_SIZE+1)*4]; 124 | double para_rcv_ddt[WINDOW_SIZE+1]; 125 | // GNSS statistics 126 | double diff_t_gnss_local; 127 | Eigen::Matrix3d R_enu_local; 128 | Eigen::Vector3d ecef_pos, enu_pos, enu_vel, enu_ypr; 129 | 130 | int frame_count; 131 | int sum_of_outlier, sum_of_back, sum_of_front, sum_of_invalid; 132 | 133 | FeatureManager f_manager; 134 | MotionEstimator m_estimator; 135 | InitialEXRotation initial_ex_rotation; 136 | 137 | bool first_imu; 138 | bool is_valid, is_key; 139 | bool failure_occur; 140 | 141 | vector point_cloud; 142 | vector margin_cloud; 143 | vector key_poses; 144 | double initial_timestamp; 145 | 146 | double para_Pose[WINDOW_SIZE + 1][SIZE_POSE]; 147 | double para_SpeedBias[WINDOW_SIZE + 1][SIZE_SPEEDBIAS]; 148 | double para_Feature[NUM_OF_F][SIZE_FEATURE]; 149 | double para_Ex_Pose[NUM_OF_CAM][SIZE_POSE]; 150 | double para_Td[1][1]; 151 | 152 | MarginalizationInfo *last_marginalization_info; 153 | vector last_marginalization_parameter_blocks; 154 | 155 | map all_image_frame; 156 | IntegrationBase *tmp_pre_integration; 157 | 158 | bool first_optimization; 159 | }; 160 | -------------------------------------------------------------------------------- /estimator/src/factor/gnss_ddt_smooth_factor.cpp: -------------------------------------------------------------------------------- 1 | #include "gnss_ddt_smooth_factor.hpp" 2 | 3 | bool DdtSmoothFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const 4 | { 5 | double rcv_ddt_i = parameters[0][0]; 6 | double rcv_ddt_j = parameters[1][0]; 7 | 8 | residuals[0] = (rcv_ddt_i - rcv_ddt_j) * weight_; 9 | 10 | if (jacobians) 11 | { 12 | if (jacobians[0]) 13 | { 14 | jacobians[0][0] = weight_; 15 | } 16 | if (jacobians[1]) 17 | { 18 | jacobians[1][0] = -weight_; 19 | } 20 | } 21 | return true; 22 | } -------------------------------------------------------------------------------- /estimator/src/factor/gnss_ddt_smooth_factor.hpp: -------------------------------------------------------------------------------- 1 | #ifndef DDT_SMOOTH_FACTOR_H_ 2 | #define DDT_SMOOTH_FACTOR_H_ 3 | 4 | #include 5 | #include 6 | 7 | /* 8 | ** parameters[0]: rev_ddt (t) in light travelling distance (m) 9 | ** parameters[1]: rev_ddt (t+1) in light travelling distance (m) 10 | */ 11 | class DdtSmoothFactor : public ceres::SizedCostFunction<1, 1, 1> 12 | { 13 | public: 14 | DdtSmoothFactor(const double weight=1) : weight_(weight) {} 15 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 16 | private: 17 | double weight_; 18 | }; 19 | 20 | #endif -------------------------------------------------------------------------------- /estimator/src/factor/gnss_dt_anchor_factor.cpp: -------------------------------------------------------------------------------- 1 | #include "gnss_dt_anchor_factor.hpp" 2 | 3 | bool DtAnchorFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const 4 | { 5 | double rev_dt_i = parameters[0][0]; 6 | 7 | residuals[0] = (rev_dt_i - 0.0) * dt_anchor_coeff_; // anchor to 0 8 | 9 | if (jacobians) 10 | { 11 | if (jacobians[0]) 12 | { 13 | jacobians[0][0] = dt_anchor_coeff_; 14 | } 15 | } 16 | 17 | return true; 18 | } -------------------------------------------------------------------------------- /estimator/src/factor/gnss_dt_anchor_factor.hpp: -------------------------------------------------------------------------------- 1 | #ifndef DT_ANCHOR_FACTOR_H_ 2 | #define DT_ANCHOR_FACTOR_H_ 3 | 4 | #include 5 | 6 | /* 7 | ** parameters[0]: rev_dt (t) in light travelling distance (m) 8 | */ 9 | class DtAnchorFactor : public ceres::SizedCostFunction<1, 1> 10 | { 11 | public: 12 | DtAnchorFactor(const double dt_anchor_coeff=1000) : dt_anchor_coeff_(dt_anchor_coeff) {} 13 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 14 | private: 15 | double dt_anchor_coeff_; 16 | }; 17 | 18 | #endif -------------------------------------------------------------------------------- /estimator/src/factor/gnss_dt_ddt_factor.cpp: -------------------------------------------------------------------------------- 1 | #include "gnss_dt_ddt_factor.hpp" 2 | 3 | DtDdtFactor::DtDdtFactor(const double delta_t_) : delta_t(delta_t_), dt_info_coeff(50) {} 4 | 5 | bool DtDdtFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const 6 | { 7 | double rev_dt_i = parameters[0][0]; 8 | double rev_dt_j = parameters[1][0]; 9 | double rev_ddt_i = parameters[2][0]; 10 | double rev_ddt_j = parameters[3][0]; 11 | 12 | double average_ddt = 0.5 * (rev_ddt_i + rev_ddt_j); 13 | residuals[0] = (rev_dt_j - rev_dt_i - average_ddt * delta_t) * dt_info_coeff; 14 | 15 | if (jacobians) 16 | { 17 | if (jacobians[0]) 18 | { 19 | jacobians[0][0] = -dt_info_coeff; 20 | } 21 | if (jacobians[1]) 22 | { 23 | jacobians[1][0] = dt_info_coeff; 24 | } 25 | if (jacobians[2]) 26 | { 27 | jacobians[2][0] = -0.5 * delta_t * dt_info_coeff; 28 | } 29 | if (jacobians[3]) 30 | { 31 | jacobians[3][0] = -0.5 * delta_t * dt_info_coeff; 32 | } 33 | } 34 | 35 | return true; 36 | } -------------------------------------------------------------------------------- /estimator/src/factor/gnss_dt_ddt_factor.hpp: -------------------------------------------------------------------------------- 1 | 2 | #ifndef DT_DDT_FACTOR_H_ 3 | #define DT_DDT_FACTOR_H_ 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | /* 10 | ** parameters[0]: rev_dt (t) in light travelling distance (m) 11 | ** parameters[1]: rev_dt (t+1) in light travelling distance (m) 12 | ** parameters[2]: rev_ddt(t) in light travelling distance per second (m/s) 13 | ** parameters[3]: rev_ddt(t+1) in light travelling distance per second (m/s) 14 | */ 15 | class DtDdtFactor : public ceres::SizedCostFunction<1, 1, 1, 1, 1> 16 | { 17 | public: 18 | DtDdtFactor() = delete; 19 | DtDdtFactor(const double delta_t_); 20 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 21 | private: 22 | double delta_t; 23 | double dt_info_coeff; 24 | }; 25 | 26 | #endif -------------------------------------------------------------------------------- /estimator/src/factor/gnss_psr_dopp_factor.hpp: -------------------------------------------------------------------------------- 1 | #ifndef GNSS_PSR_DOPP_FACTOR_H_ 2 | #define GNSS_PSR_DOPP_FACTOR_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | 11 | #define PSR_TO_DOPP_RATIO 5 12 | 13 | using namespace gnss_comm; 14 | 15 | /* 16 | ** parameters[0]: position and orientation at time k 17 | ** parameters[1]: velocity and acc/gyro bias at time k 18 | ** parameters[2]: position and orientation at time k+1 19 | ** parameters[3]: velocity and acc/gyro bias at time k+1 20 | ** parameters[4]: receiver clock bias in light travelling distance (m) 21 | ** parameters[5]: receiver clock bias change rate in clock bias light travelling distance per second (m/s) 22 | ** parameters[6]: yaw difference between ENU and local coordinate (rad) 23 | ** parameters[7]: anchor point's ECEF coordinate 24 | ** 25 | */ 26 | /** 27 | * 2：残差是2维，包括伪距和多普勒测量残差 28 | * 7：k时刻位置p和旋转q，一共7维 29 | * 9：k时刻速度、加速度计和陀螺仪的9维偏置 30 | * 7：k+1时刻位置p和旋转q，一共7维 31 | * 9：k+1时刻速度、加速度计和陀螺仪的9维偏置 32 | * 1：接收机时钟钟差 33 | * 1：接收机时钟钟差变化率 34 | * 1：ENU系和local world系之间的偏航角 35 | * 3：anchor point在ECEF下的3D位置 36 | * **/ 37 | class GnssPsrDoppFactor : public ceres::SizedCostFunction<2, 7, 9, 7, 9, 1, 1, 1, 3> 38 | { 39 | public: 40 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 41 | GnssPsrDoppFactor() = delete; 42 | GnssPsrDoppFactor(const ObsPtr &_obs, const EphemBasePtr &_ephem, std::vector &_iono_paras, 43 | const double _ratio); 44 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 45 | bool check_gradients(const std::vector ¶meters) const; 46 | private: 47 | const ObsPtr obs; 48 | const EphemBasePtr ephem; 49 | const std::vector &iono_paras; 50 | double ratio; 51 | int freq_idx; 52 | double freq; 53 | Eigen::Vector3d sv_pos; 54 | Eigen::Vector3d sv_vel; 55 | double svdt, svddt, tgd; 56 | double pr_uura, dp_uura; 57 | double relative_sqrt_info; 58 | }; 59 | 60 | #endif -------------------------------------------------------------------------------- /estimator/src/factor/marginalization_factor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include "../utility/utility.h" 11 | #include "../utility/tic_toc.h" 12 | 13 | const int NUM_THREADS = 4; 14 | 15 | struct ResidualBlockInfo 16 | { 17 | ResidualBlockInfo(ceres::CostFunction *_cost_function, ceres::LossFunction *_loss_function, std::vector _parameter_blocks, std::vector _drop_set) 18 | : cost_function(_cost_function), loss_function(_loss_function), parameter_blocks(_parameter_blocks), drop_set(_drop_set) {} 19 | 20 | void Evaluate(); 21 | 22 | ceres::CostFunction *cost_function; 23 | ceres::LossFunction *loss_function; 24 | std::vector parameter_blocks; 25 | std::vector drop_set; 26 | 27 | double **raw_jacobians; 28 | std::vector> jacobians; 29 | Eigen::VectorXd residuals; 30 | 31 | int localSize(int size) 32 | { 33 | return size == 7 ? 6 : size; 34 | } 35 | }; 36 | 37 | struct ThreadsStruct 38 | { 39 | std::vector sub_factors; 40 | Eigen::MatrixXd A; 41 | Eigen::VectorXd b; 42 | std::unordered_map parameter_block_size; //global size 43 | std::unordered_map parameter_block_idx; //local size 44 | }; 45 | 46 | class MarginalizationInfo 47 | { 48 | public: 49 | ~MarginalizationInfo(); 50 | int localSize(int size) const; 51 | int globalSize(int size) const; 52 | void addResidualBlockInfo(ResidualBlockInfo *residual_block_info); 53 | void preMarginalize(); 54 | void marginalize(); 55 | std::vector getParameterBlocks(std::unordered_map &addr_shift); 56 | 57 | std::vector factors; 58 | int m, n; 59 | std::unordered_map parameter_block_size; //global size 60 | int sum_block_size; 61 | std::unordered_map parameter_block_idx; //local size 62 | std::unordered_map parameter_block_data; 63 | 64 | std::vector keep_block_size; //global size 65 | std::vector keep_block_idx; //local size 66 | std::vector keep_block_data; 67 | 68 | Eigen::MatrixXd linearized_jacobians; 69 | Eigen::VectorXd linearized_residuals; 70 | const double eps = 1e-8; 71 | 72 | }; 73 | 74 | class MarginalizationFactor : public ceres::CostFunction 75 | { 76 | public: 77 | MarginalizationFactor(MarginalizationInfo* _marginalization_info); 78 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 79 | 80 | MarginalizationInfo* marginalization_info; 81 | }; 82 | -------------------------------------------------------------------------------- /estimator/src/factor/pos_vel_factor.cpp: -------------------------------------------------------------------------------- 1 | #include "pos_vel_factor.hpp" 2 | 3 | PosVelFactor::PosVelFactor(const double delta_t_) : delta_t(delta_t_), info_coeff(50) {} 4 | 5 | bool PosVelFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const 6 | { 7 | Eigen::Map pos_i(parameters[0]); 8 | Eigen::Map pos_j(parameters[1]); 9 | Eigen::Map vel_i(parameters[2]); 10 | Eigen::Map vel_j(parameters[3]); 11 | Eigen::Map res(residuals); 12 | 13 | Eigen::Vector3d average_vel = 0.5 * (vel_i + vel_j); 14 | res = (pos_j - pos_i - average_vel * delta_t) * info_coeff; 15 | 16 | if (jacobians) 17 | { 18 | if (jacobians[0]) 19 | { 20 | Eigen::Map> J_Pi(jacobians[0]); 21 | J_Pi.setZero(); 22 | J_Pi.topLeftCorner<3, 3>() = -Eigen::Matrix3d::Identity() * info_coeff; 23 | } 24 | if (jacobians[1]) 25 | { 26 | Eigen::Map> J_Pj(jacobians[1]); 27 | J_Pj.setZero(); 28 | J_Pj.topLeftCorner<3, 3>() = Eigen::Matrix3d::Identity() * info_coeff; 29 | } 30 | if (jacobians[2]) 31 | { 32 | Eigen::Map> J_Vi(jacobians[2]); 33 | J_Vi.setZero(); 34 | J_Vi.topLeftCorner<3, 3>() = -0.5 * Eigen::Matrix3d::Identity() * delta_t * info_coeff; 35 | } 36 | if (jacobians[3]) 37 | { 38 | Eigen::Map> J_Vj(jacobians[3]); 39 | J_Vj.setZero(); 40 | J_Vj.topLeftCorner<3, 3>() = -0.5 * Eigen::Matrix3d::Identity() * delta_t * info_coeff; 41 | } 42 | 43 | } 44 | 45 | return true; 46 | } -------------------------------------------------------------------------------- /estimator/src/factor/pos_vel_factor.hpp: -------------------------------------------------------------------------------- 1 | #ifndef POS_VEL_FACTOR_H_ 2 | #define POS_VEL_FACTOR_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | /* 9 | ** parameters[0]: position(k) in ECEF 10 | ** parameters[1]: position(k+1) in ECEF 11 | ** parameters[2]: velocity(k) in ECEF 12 | ** parameters[3]: velocity(k+1) in ECEF 13 | */ 14 | class PosVelFactor : public ceres::SizedCostFunction<3, 7, 7, 9, 9> 15 | { 16 | public: 17 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 18 | PosVelFactor() = delete; 19 | PosVelFactor(const double delta_t_); 20 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 21 | private: 22 | double delta_t; 23 | double info_coeff; 24 | }; 25 | 26 | #endif -------------------------------------------------------------------------------- /estimator/src/factor/pose_anchor_factor.cpp: -------------------------------------------------------------------------------- 1 | #include "pose_anchor_factor.h" 2 | 3 | PoseAnchorFactor::PoseAnchorFactor(const std::vector anchor_value) 4 | { 5 | for (int i = 0; i < 7; ++i) _anchor_point(i) = anchor_value[i]; 6 | } 7 | 8 | bool PoseAnchorFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const 9 | { 10 | Eigen::Map> pose(parameters[0]); 11 | Eigen::Map> res(residuals); 12 | res.head<3>() = pose.head<3>() - _anchor_point.head<3>(); 13 | const Eigen::Quaterniond curr_q(pose.tail<4>()); 14 | const Eigen::Quaterniond anchor_q(_anchor_point.tail<4>()); 15 | res.tail<3>() = 2.0 * (curr_q*anchor_q.inverse()).vec(); 16 | res *= sqrt_info; 17 | if (jacobians && jacobians[0]) 18 | { 19 | Eigen::Map> J(jacobians[0]); 20 | J.setZero(); 21 | J.topLeftCorner<3, 3>().setIdentity(); 22 | 23 | Eigen::Quaterniond anchor_q_inv = anchor_q.inverse(); 24 | Eigen::Matrix3d J_q; 25 | J_q << anchor_q_inv.w(), anchor_q_inv.z(), -anchor_q_inv.y(), 26 | -anchor_q_inv.z(), anchor_q_inv.w(), anchor_q_inv.x(), 27 | anchor_q_inv.y(), -anchor_q_inv.x(), anchor_q_inv.w(); 28 | J.block<3, 3>(3, 3) = J_q; 29 | J *= 2.0*sqrt_info; 30 | } 31 | return true; 32 | } -------------------------------------------------------------------------------- /estimator/src/factor/pose_anchor_factor.h: -------------------------------------------------------------------------------- 1 | #ifndef POSE_ANCHOR_FACTOR_H_ 2 | #define POSE_ANCHOR_FACTOR_H_ 3 | 4 | #include 5 | #include 6 | 7 | /* 8 | ** parameters[0]: pose which needs to be anchored to a constant value 9 | */ 10 | class PoseAnchorFactor : public ceres::SizedCostFunction<6, 7> 11 | { 12 | public: 13 | PoseAnchorFactor() = delete; 14 | PoseAnchorFactor(const std::vector anchor_value); 15 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 16 | private: 17 | Eigen::Matrix _anchor_point; 18 | constexpr static double sqrt_info = 120; 19 | }; 20 | 21 | #endif -------------------------------------------------------------------------------- /estimator/src/factor/pose_local_parameterization.cpp: -------------------------------------------------------------------------------- 1 | #include "pose_local_parameterization.h" 2 | 3 | bool PoseLocalParameterization::Plus(const double *x, const double *delta, double *x_plus_delta) const 4 | { 5 | Eigen::Map _p(x); 6 | Eigen::Map _q(x + 3); 7 | 8 | Eigen::Map dp(delta); 9 | 10 | Eigen::Quaterniond dq = Utility::deltaQ(Eigen::Map(delta + 3)); 11 | 12 | Eigen::Map p(x_plus_delta); 13 | Eigen::Map q(x_plus_delta + 3); 14 | 15 | p = _p + dp; 16 | q = (_q * dq).normalized(); 17 | 18 | return true; 19 | } 20 | bool PoseLocalParameterization::ComputeJacobian(const double *x, double *jacobian) const 21 | { 22 | Eigen::Map> j(jacobian); 23 | j.topRows<6>().setIdentity(); 24 | j.bottomRows<1>().setZero(); 25 | 26 | return true; 27 | } 28 | -------------------------------------------------------------------------------- /estimator/src/factor/pose_local_parameterization.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include "../utility/utility.h" 6 | 7 | class PoseLocalParameterization : public ceres::LocalParameterization 8 | { 9 | virtual bool Plus(const double *x, const double *delta, double *x_plus_delta) const; 10 | virtual bool ComputeJacobian(const double *x, double *jacobian) const; 11 | virtual int GlobalSize() const { return 7; }; 12 | virtual int LocalSize() const { return 6; }; 13 | }; 14 | -------------------------------------------------------------------------------- /estimator/src/factor/projection_factor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include "../utility/utility.h" 7 | #include "../utility/tic_toc.h" 8 | #include "../parameters.h" 9 | 10 | class ProjectionFactor : public ceres::SizedCostFunction<2, 7, 7, 7, 1> 11 | { 12 | public: 13 | ProjectionFactor(const Eigen::Vector3d &_pts_i, const Eigen::Vector3d &_pts_j); 14 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 15 | void check(double **parameters); 16 | 17 | Eigen::Vector3d pts_i, pts_j; 18 | Eigen::Matrix tangent_base; 19 | static Eigen::Matrix2d sqrt_info; 20 | static double sum_t; 21 | }; 22 | -------------------------------------------------------------------------------- /estimator/src/factor/projection_td_factor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include "../utility/utility.h" 7 | #include "../utility/tic_toc.h" 8 | #include "../parameters.h" 9 | 10 | class ProjectionTdFactor : public ceres::SizedCostFunction<2, 7, 7, 7, 1, 1> 11 | { 12 | public: 13 | ProjectionTdFactor(const Eigen::Vector3d &_pts_i, const Eigen::Vector3d &_pts_j, 14 | const Eigen::Vector2d &_velocity_i, const Eigen::Vector2d &_velocity_j, 15 | const double _td_i, const double _td_j); 16 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 17 | void check(double **parameters); 18 | 19 | Eigen::Vector3d pts_i, pts_j; 20 | Eigen::Vector3d velocity_i, velocity_j; 21 | double td_i, td_j; 22 | Eigen::Matrix tangent_base; 23 | static Eigen::Matrix2d sqrt_info; 24 | static double sum_t; 25 | }; 26 | -------------------------------------------------------------------------------- /estimator/src/feature_manager.h: -------------------------------------------------------------------------------- 1 | #ifndef FEATURE_MANAGER_H 2 | #define FEATURE_MANAGER_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | using namespace std; 9 | 10 | #include 11 | using namespace Eigen; 12 | 13 | #include 14 | #include 15 | 16 | #include "parameters.h" 17 | 18 | class FeaturePerFrame 19 | { 20 | public: 21 | FeaturePerFrame(const Eigen::Matrix &_point, double td) 22 | { 23 | point.x() = _point(0); 24 | point.y() = _point(1); 25 | point.z() = _point(2); 26 | uv.x() = _point(3); 27 | uv.y() = _point(4); 28 | velocity.x() = _point(5); 29 | velocity.y() = _point(6); 30 | cur_td = td; 31 | } 32 | double cur_td; 33 | Vector3d point; 34 | Vector2d uv; 35 | Vector2d velocity; 36 | double z; 37 | bool is_used; 38 | double parallax; 39 | MatrixXd A; 40 | VectorXd b; 41 | double dep_gradient; 42 | }; 43 | 44 | class FeaturePerId 45 | { 46 | public: 47 | const int feature_id; 48 | int start_frame; 49 | vector feature_per_frame; 50 | 51 | int used_num; 52 | bool is_outlier; 53 | bool is_margin; 54 | double estimated_depth; 55 | int solve_flag; // 0 haven't solve yet; 1 solve succ; 2 solve fail; 56 | 57 | Vector3d gt_p; 58 | 59 | FeaturePerId(int _feature_id, int _start_frame) 60 | : feature_id(_feature_id), start_frame(_start_frame), 61 | used_num(0), estimated_depth(-1.0), solve_flag(0) 62 | { 63 | } 64 | 65 | int endFrame(); 66 | }; 67 | 68 | class FeatureManager 69 | { 70 | public: 71 | FeatureManager(Matrix3d _Rs[]); 72 | 73 | void setRic(Matrix3d _ric[]); 74 | 75 | void clearState(); 76 | 77 | int getFeatureCount(); 78 | 79 | bool addFeatureCheckParallax(int frame_count, const map>>> &image, double td); 80 | void debugShow(); 81 | vector> getCorresponding(int frame_count_l, int frame_count_r); 82 | 83 | //void updateDepth(const VectorXd &x); 84 | void setDepth(const VectorXd &x); 85 | void removeFailures(); 86 | void clearDepth(const VectorXd &x); 87 | VectorXd getDepthVector(); 88 | void triangulate(Vector3d Ps[], Vector3d tic[], Matrix3d ric[]); 89 | void removeBackShiftDepth(Eigen::Matrix3d marg_R, Eigen::Vector3d marg_P, Eigen::Matrix3d new_R, Eigen::Vector3d new_P); 90 | void removeBack(); 91 | void removeFront(int frame_count); 92 | void removeOutlier(); 93 | list feature; 94 | int last_track_num; 95 | 96 | private: 97 | double compensatedParallax2(const FeaturePerId &it_per_id, int frame_count); 98 | const Matrix3d *Rs; 99 | Matrix3d ric[NUM_OF_CAM]; 100 | }; 101 | 102 | #endif -------------------------------------------------------------------------------- /estimator/src/initial/gnss_vi_initializer.cpp: -------------------------------------------------------------------------------- 1 | #include "gnss_vi_initializer.h" 2 | 3 | GNSSVIInitializer::GNSSVIInitializer(const std::vector> &gnss_meas_buf_, 4 | const std::vector> &gnss_ephem_buf_, const std::vector &iono_params_) 5 | : gnss_meas_buf(gnss_meas_buf_), gnss_ephem_buf(gnss_ephem_buf_), iono_params(iono_params_) 6 | { 7 | num_all_meas = 0; 8 | all_sat_states.clear(); 9 | for (uint32_t i = 0; i < gnss_meas_buf.size(); ++i) 10 | { 11 | num_all_meas += gnss_meas_buf[i].size(); 12 | all_sat_states.push_back(sat_states(gnss_meas_buf[i], gnss_ephem_buf[i])); 13 | } 14 | } 15 | 16 | /** 17 | * @brief 粗糙定位 18 | * / 19 | * 论文里的描述：在没有任何先验信息的情况下，将近期是有伪距测量值作为GNSS SPP算法的输入，得到接收器的一个粗糙ECEF位置 20 | * 以及四个导航系统中卫星的时钟偏差（钟差） 21 | * / 22 | * **/ 23 | bool GNSSVIInitializer::coarse_localization(Eigen::Matrix &result) 24 | { 25 | result.setZero(); 26 | std::vector accum_obs; 27 | std::vector accum_ephems; 28 | // 将BUF中所有的卫星观测数据和星历信息分别复制到accum_obs、accum_ephems中 29 | for (uint32_t i = 0; i < gnss_meas_buf.size(); ++i) 30 | { 31 | std::copy(gnss_meas_buf[i].begin(), gnss_meas_buf[i].end(), std::back_inserter(accum_obs)); 32 | std::copy(gnss_ephem_buf[i].begin(), gnss_ephem_buf[i].end(), std::back_inserter(accum_ephems)); 33 | } 34 | // 伪距定位得到接收机在ECEF系上的位置坐标以及四个卫星时钟的时钟偏差 35 | Eigen::Matrix xyzt = psr_pos(accum_obs, accum_ephems, iono_params); 36 | if (xyzt.topLeftCorner<3, 1>().norm() == 0) 37 | { 38 | std::cerr << "Failed to obtain a rough reference location.\n"; 39 | return false; 40 | } 41 | 42 | // 如果相应卫星时钟偏差<1，认为没有观测到该导航系统的卫星，那么将该卫星的时钟偏差置为0 43 | for (uint32_t k = 0; k < 4; ++k) 44 | { 45 | if (fabs(xyzt(k+3)) < 1) 46 | xyzt(k+3) = 0; // not observed yet 47 | } 48 | 49 | result = xyzt; 50 | return true; 51 | } 52 | 53 | /** 54 | * GNSS和VIO的偏航角对齐：即ENU系和Local world系的Z轴是完全重合的，仅需要知道两系之间的偏航角，就可以完成两系的对齐 55 | * */ 56 | bool GNSSVIInitializer::yaw_alignment(const std::vector &local_vs, 57 | const Eigen::Vector3d &rough_anchor_ecef, double &aligned_yaw, double &rcv_ddt) 58 | { 59 | aligned_yaw = 0; 60 | rcv_ddt = 0; 61 | 62 | // 估计值 63 | double est_yaw = 0; 64 | double est_rcv_ddt = 0; 65 | 66 | Eigen::Matrix3d rough_R_ecef_enu = ecef2rotation(rough_anchor_ecef); // ENU ——> ECEF的转换 67 | uint32_t align_iter = 0; 68 | double align_dx_norm = 1.0; 69 | // 最小二乘求解接收机时钟钟差变化率和偏航角 70 | while (align_iter < MAX_ITERATION && align_dx_norm > CONVERGENCE_EPSILON) 71 | { 72 | Eigen::MatrixXd align_G(num_all_meas, 2); // num_all_meas：数据总数量，2：2个优化量 73 | align_G.setZero(); 74 | align_G.col(1).setOnes(); // 这样设置的话，将接收机钟差变化率作为第二个优化量 75 | Eigen::VectorXd align_b(num_all_meas); 76 | align_b.setZero(); 77 | Eigen::Matrix3d align_R_enu_local(Eigen::AngleAxisd(est_yaw, Eigen::Vector3d::UnitZ())); // local——>ENU的转换 78 | // 绕Z轴旋转est_yaw角度的旋转矩阵的导数 79 | Eigen::Matrix3d align_tmp_M; 80 | align_tmp_M << -sin(est_yaw), -cos(est_yaw), 0, 81 | cos(est_yaw), -sin(est_yaw), 0, 82 | 0 , 0 , 0; 83 | 84 | // 构建多普勒测量的残差和雅可比矩阵，用于最小二乘求导 85 | uint32_t align_counter = 0; 86 | for (uint32_t i = 0; i < gnss_meas_buf.size(); ++i) 87 | { 88 | Eigen::Matrix ecef_vel_ddt; // 记录VIO速度转到ECEF下的数值和接收机钟差变化率 89 | ecef_vel_ddt.head<3>() = rough_R_ecef_enu * align_R_enu_local * local_vs[i]; // 将VIO速度从local 转到 ECEF下 90 | ecef_vel_ddt(3) = est_rcv_ddt; // 接收机时钟钟差变化率 91 | Eigen::VectorXd epoch_res; // 多普勒测量残差 92 | Eigen::MatrixXd epoch_J; // 多普勒测量雅可比 93 | dopp_res(ecef_vel_ddt, rough_anchor_ecef, gnss_meas_buf[i], all_sat_states[i], epoch_res, epoch_J); 94 | align_b.segment(align_counter, gnss_meas_buf[i].size()) = epoch_res; 95 | align_G.block(align_counter, 0, gnss_meas_buf[i].size(), 1) = 96 | epoch_J.leftCols(3)*rough_R_ecef_enu*align_tmp_M*local_vs[i]; // 因为dopp_res中仅仅对速度求到，所以需要乘上速度到偏航角的导数 97 | align_counter += gnss_meas_buf[i].size(); 98 | } 99 | // 最小二乘求解 100 | Eigen::VectorXd dx = -(align_G.transpose()*align_G).inverse() * align_G.transpose() * align_b; 101 | est_yaw += dx(0); 102 | est_rcv_ddt += dx(1); 103 | align_dx_norm = dx.norm(); 104 | ++ align_iter; 105 | } 106 | 107 | if (align_iter > MAX_ITERATION) 108 | { 109 | std::cerr << "Fail to initialize yaw offset.\n"; 110 | return false; 111 | } 112 | 113 | // 规范偏航角，使得偏航角的范围在0～180度之间 114 | aligned_yaw = est_yaw; 115 | if (aligned_yaw > M_PI) 116 | aligned_yaw -= floor(est_yaw/(2.0*M_PI) + 0.5) * (2.0*M_PI); 117 | else if (aligned_yaw < -M_PI) 118 | aligned_yaw -= ceil(est_yaw/(2.0*M_PI) - 0.5) * (2.0*M_PI); 119 | 120 | rcv_ddt = est_rcv_ddt; 121 | 122 | return true; 123 | } 124 | 125 | /*** 126 | * @brief 对anchor point位置精细化，并求出接收机的时钟钟差dt 127 | * @param[in] local_ps VIO求解出相机在local world下的位置（已求解） 128 | * @param[in] aligned_yaw ENU和local world之间的偏航角（已求解） 129 | * @param[in] aligned_ddt 接收机的时钟种差变化率（已求解） 130 | * @param[in] rough_ecef_dt 接收机在ECEF下的粗糙位置以及四个导航系统的卫星时钟钟差（已求解） 131 | * @param[out] refined_ecef_dt 再优化后的接收机的ECEF位置和接收机在四个导航系统下不同的钟差 132 | */ 133 | bool GNSSVIInitializer::anchor_refinement(const std::vector &local_ps, 134 | const double aligned_yaw, const double aligned_ddt, 135 | const Eigen::Matrix &rough_ecef_dt, Eigen::Matrix &refined_ecef_dt) 136 | { 137 | refined_ecef_dt.setZero(); // 状态清空，置零 138 | 139 | // 轴角构建local——ENU的旋转矩阵 140 | Eigen::Matrix3d aligned_R_enu_local(Eigen::AngleAxisd(aligned_yaw, Eigen::Vector3d::UnitZ())); 141 | 142 | // refine anchor point and receiver clock bias 143 | Eigen::Vector3d refine_anchor = rough_ecef_dt.head<3>(); // 用第一步得到的伪距位置作为初值 144 | Eigen::Vector4d refine_dt = rough_ecef_dt.tail<4>(); 145 | uint32_t refine_iter = 0; 146 | double refine_dx_norm = 1.0; 147 | std::vector unobserved_sys; // 记录没有被观测到的导航系统 148 | for (uint32_t k = 0; k < 4; ++k) 149 | { 150 | if (rough_ecef_dt(3+k) == 0) 151 | unobserved_sys.push_back(k); 152 | } 153 | 154 | // 构建相应残差和雅可比矩阵，进行变量的最小二乘求解 155 | while (refine_iter < MAX_ITERATION && refine_dx_norm > CONVERGENCE_EPSILON) 156 | { 157 | Eigen::MatrixXd refine_G(num_all_meas+unobserved_sys.size(), 7); 158 | Eigen::VectorXd refine_b(num_all_meas+unobserved_sys.size()); 159 | refine_G.setZero(); 160 | refine_b.setZero(); 161 | uint32_t refine_counter = 0; 162 | Eigen::Matrix3d refine_R_ecef_enu = ecef2rotation(refine_anchor); 163 | Eigen::Matrix3d refine_R_ecef_local = refine_R_ecef_enu * aligned_R_enu_local; 164 | for (uint32_t i = 0; i < gnss_meas_buf.size(); ++i) 165 | { 166 | Eigen::Matrix ecef_xyz_dt; 167 | ecef_xyz_dt.head<3>() = refine_R_ecef_local * local_ps[i] + refine_anchor; // 论文中公式（14） 168 | ecef_xyz_dt.tail<4>() = refine_dt + aligned_ddt * i * Eigen::Vector4d::Ones(); // 论文中公式（23） 169 | 170 | Eigen::VectorXd epoch_res; 171 | Eigen::MatrixXd epoch_J; 172 | std::vector tmp_atmos_delay, tmp_sv_azel; 173 | psr_res(ecef_xyz_dt, gnss_meas_buf[i], all_sat_states[i], iono_params, 174 | epoch_res, epoch_J, tmp_atmos_delay, tmp_sv_azel); // 对Panc构建残差和雅可比矩阵 175 | refine_b.segment(refine_counter, gnss_meas_buf[i].size()) = epoch_res; 176 | refine_G.middleRows(refine_counter, gnss_meas_buf[i].size()) = epoch_J; 177 | refine_counter += gnss_meas_buf[i].size(); 178 | } 179 | // 对接收机钟差构建残差和雅可比矩阵 180 | for (uint32_t k : unobserved_sys) 181 | { 182 | refine_b(refine_counter) = 0; 183 | refine_G(refine_counter, k+3) = 1.0; 184 | ++ refine_counter; 185 | } 186 | 187 | // 最小二乘求解 188 | Eigen::VectorXd dx = -(refine_G.transpose()*refine_G).inverse() * refine_G.transpose() * refine_b; 189 | refine_anchor += dx.head<3>(); 190 | refine_dt += dx.tail<4>(); 191 | refine_dx_norm = dx.norm(); 192 | ++ refine_iter; 193 | } 194 | 195 | if (refine_iter > MAX_ITERATION) 196 | { 197 | std::cerr << "Fail to perform anchor refinement.\n"; 198 | return false; 199 | } 200 | 201 | refined_ecef_dt.head<3>() = refine_anchor; 202 | refined_ecef_dt.tail<4>() = refine_dt; 203 | 204 | return true; 205 | } -------------------------------------------------------------------------------- /estimator/src/initial/gnss_vi_initializer.h: -------------------------------------------------------------------------------- 1 | #ifndef GNSS_VI_INITIALIZER 2 | #define GNSS_VI_INITIALIZER 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | #include "../feature_manager.h" 11 | 12 | using namespace gnss_comm; 13 | 14 | class GNSSVIInitializer 15 | { 16 | public: 17 | GNSSVIInitializer(const std::vector> &gnss_meas_buf_, 18 | const std::vector> &gnss_ephem_buf_, 19 | const std::vector &iono_params_); 20 | GNSSVIInitializer(const GNSSVIInitializer&) = delete; 21 | GNSSVIInitializer& operator=(const GNSSVIInitializer&) = delete; 22 | ~GNSSVIInitializer() {}; 23 | 24 | // get a rough location by SPP 25 | bool coarse_localization(Eigen::Matrix &result); 26 | bool yaw_alignment(const std::vector &local_vs, const Eigen::Vector3d &rough_anchor_ecef, 27 | double &aligned_yaw, double &rcv_ddt); 28 | bool anchor_refinement(const std::vector &local_ps, 29 | const double aligned_yaw, const double aligned_ddt, 30 | const Eigen::Matrix &rough_ecef_dt, Eigen::Matrix &refined_ecef_dt); 31 | 32 | private: 33 | const std::vector> &gnss_meas_buf; 34 | const std::vector> &gnss_ephem_buf; 35 | const std::vector &iono_params; 36 | 37 | uint32_t num_all_meas; 38 | std::vector> all_sat_states; 39 | 40 | static constexpr uint32_t MAX_ITERATION = 10; 41 | static constexpr double CONVERGENCE_EPSILON = 1e-5; 42 | }; 43 | 44 | 45 | #endif -------------------------------------------------------------------------------- /estimator/src/initial/initial_aligment.cpp: -------------------------------------------------------------------------------- 1 | #include "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 | -------------------------------------------------------------------------------- /estimator/src/initial/initial_alignment.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include "../factor/imu_factor.h" 5 | #include "../utility/utility.h" 6 | #include 7 | #include 8 | #include "../feature_manager.h" 9 | 10 | using namespace Eigen; 11 | using namespace std; 12 | 13 | class ImageFrame 14 | { 15 | public: 16 | ImageFrame(){}; 17 | ImageFrame(const map>>>& _points, double _t):t{_t},is_key_frame{false} 18 | { 19 | points = _points; 20 | }; 21 | map> > > points; 22 | double t; 23 | Matrix3d R; 24 | Vector3d T; 25 | IntegrationBase *pre_integration; 26 | bool is_key_frame; 27 | }; 28 | 29 | bool VisualIMUAlignment(map &all_image_frame, Vector3d* Bgs, Vector3d &g, VectorXd &x); -------------------------------------------------------------------------------- /estimator/src/initial/initial_ex_rotation.cpp: -------------------------------------------------------------------------------- 1 | #include "initial_ex_rotation.h" 2 | 3 | InitialEXRotation::InitialEXRotation(){ 4 | frame_count = 0; 5 | Rc.push_back(Matrix3d::Identity()); 6 | Rc_g.push_back(Matrix3d::Identity()); 7 | Rimu.push_back(Matrix3d::Identity()); 8 | ric = Matrix3d::Identity(); 9 | } 10 | 11 | bool InitialEXRotation::CalibrationExRotation(vector> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result) 12 | { 13 | frame_count++; 14 | Rc.push_back(solveRelativeR(corres)); 15 | Rimu.push_back(delta_q_imu.toRotationMatrix()); 16 | Rc_g.push_back(ric.inverse() * delta_q_imu * ric); 17 | 18 | Eigen::MatrixXd A(frame_count * 4, 4); 19 | A.setZero(); 20 | int sum_ok = 0; 21 | for (int i = 1; i <= frame_count; i++) 22 | { 23 | Quaterniond r1(Rc[i]); 24 | Quaterniond r2(Rc_g[i]); 25 | 26 | double angular_distance = 180 / M_PI * r1.angularDistance(r2); 27 | ROS_DEBUG( 28 | "%d %f", i, angular_distance); 29 | 30 | double huber = angular_distance > 5.0 ? 5.0 / angular_distance : 1.0; 31 | ++sum_ok; 32 | Matrix4d L, R; 33 | 34 | double w = Quaterniond(Rc[i]).w(); 35 | Vector3d q = Quaterniond(Rc[i]).vec(); 36 | L.block<3, 3>(0, 0) = w * Matrix3d::Identity() + Utility::skewSymmetric(q); 37 | L.block<3, 1>(0, 3) = q; 38 | L.block<1, 3>(3, 0) = -q.transpose(); 39 | L(3, 3) = w; 40 | 41 | Quaterniond R_ij(Rimu[i]); 42 | w = R_ij.w(); 43 | q = R_ij.vec(); 44 | R.block<3, 3>(0, 0) = w * Matrix3d::Identity() - Utility::skewSymmetric(q); 45 | R.block<3, 1>(0, 3) = q; 46 | R.block<1, 3>(3, 0) = -q.transpose(); 47 | R(3, 3) = w; 48 | 49 | A.block<4, 4>((i - 1) * 4, 0) = huber * (L - R); 50 | } 51 | 52 | JacobiSVD svd(A, ComputeFullU | ComputeFullV); 53 | Matrix x = svd.matrixV().col(3); 54 | Quaterniond estimated_R(x); 55 | ric = estimated_R.toRotationMatrix().inverse(); 56 | //cout << svd.singularValues().transpose() << endl; 57 | //cout << ric << endl; 58 | Vector3d ric_cov; 59 | ric_cov = svd.singularValues().tail<3>(); 60 | if (frame_count >= WINDOW_SIZE && ric_cov(1) > 0.25) 61 | { 62 | calib_ric_result = ric; 63 | return true; 64 | } 65 | else 66 | return false; 67 | } 68 | 69 | Matrix3d InitialEXRotation::solveRelativeR(const vector> &corres) 70 | { 71 | if (corres.size() >= 9) 72 | { 73 | vector ll, rr; 74 | for (int i = 0; i < int(corres.size()); i++) 75 | { 76 | ll.push_back(cv::Point2f(corres[i].first(0), corres[i].first(1))); 77 | rr.push_back(cv::Point2f(corres[i].second(0), corres[i].second(1))); 78 | } 79 | cv::Mat E = cv::findFundamentalMat(ll, rr); 80 | cv::Mat_ R1, R2, t1, t2; 81 | decomposeE(E, R1, R2, t1, t2); 82 | 83 | if (determinant(R1) + 1.0 < 1e-09) 84 | { 85 | E = -E; 86 | decomposeE(E, R1, R2, t1, t2); 87 | } 88 | double ratio1 = max(testTriangulation(ll, rr, R1, t1), testTriangulation(ll, rr, R1, t2)); 89 | double ratio2 = max(testTriangulation(ll, rr, R2, t1), testTriangulation(ll, rr, R2, t2)); 90 | cv::Mat_ ans_R_cv = ratio1 > ratio2 ? R1 : R2; 91 | 92 | Matrix3d ans_R_eigen; 93 | for (int i = 0; i < 3; i++) 94 | for (int j = 0; j < 3; j++) 95 | ans_R_eigen(j, i) = ans_R_cv(i, j); 96 | return ans_R_eigen; 97 | } 98 | return Matrix3d::Identity(); 99 | } 100 | 101 | double InitialEXRotation::testTriangulation(const vector &l, 102 | const vector &r, 103 | cv::Mat_ R, cv::Mat_ t) 104 | { 105 | cv::Mat pointcloud; 106 | cv::Matx34f P = cv::Matx34f(1, 0, 0, 0, 107 | 0, 1, 0, 0, 108 | 0, 0, 1, 0); 109 | cv::Matx34f P1 = cv::Matx34f(R(0, 0), R(0, 1), R(0, 2), t(0), 110 | R(1, 0), R(1, 1), R(1, 2), t(1), 111 | R(2, 0), R(2, 1), R(2, 2), t(2)); 112 | cv::triangulatePoints(P, P1, l, r, pointcloud); 113 | int front_count = 0; 114 | for (int i = 0; i < pointcloud.cols; i++) 115 | { 116 | double normal_factor = pointcloud.col(i).at(3); 117 | 118 | cv::Mat_ p_3d_l = cv::Mat(P) * (pointcloud.col(i) / normal_factor); 119 | cv::Mat_ p_3d_r = cv::Mat(P1) * (pointcloud.col(i) / normal_factor); 120 | if (p_3d_l(2) > 0 && p_3d_r(2) > 0) 121 | front_count++; 122 | } 123 | ROS_DEBUG("MotionEstimator: %f", 1.0 * front_count / pointcloud.cols); 124 | return 1.0 * front_count / pointcloud.cols; 125 | } 126 | 127 | void InitialEXRotation::decomposeE(cv::Mat E, 128 | cv::Mat_ &R1, cv::Mat_ &R2, 129 | cv::Mat_ &t1, cv::Mat_ &t2) 130 | { 131 | cv::SVD svd(E, cv::SVD::MODIFY_A); 132 | cv::Matx33d W(0, -1, 0, 133 | 1, 0, 0, 134 | 0, 0, 1); 135 | cv::Matx33d Wt(0, 1, 0, 136 | -1, 0, 0, 137 | 0, 0, 1); 138 | R1 = svd.u * cv::Mat(W) * svd.vt; 139 | R2 = svd.u * cv::Mat(Wt) * svd.vt; 140 | t1 = svd.u.col(2); 141 | t2 = -svd.u.col(2); 142 | } 143 | -------------------------------------------------------------------------------- /estimator/src/initial/initial_ex_rotation.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include "../parameters.h" 5 | using namespace std; 6 | 7 | #include 8 | 9 | #include 10 | using namespace Eigen; 11 | #include 12 | 13 | /* This class help you to calibrate extrinsic rotation between imu and camera when your totally don't konw the extrinsic parameter */ 14 | class InitialEXRotation 15 | { 16 | public: 17 | InitialEXRotation(); 18 | bool CalibrationExRotation(vector> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result); 19 | private: 20 | Matrix3d solveRelativeR(const vector> &corres); 21 | 22 | double testTriangulation(const vector &l, 23 | const vector &r, 24 | cv::Mat_ R, cv::Mat_ t); 25 | void decomposeE(cv::Mat E, 26 | cv::Mat_ &R1, cv::Mat_ &R2, 27 | cv::Mat_ &t1, cv::Mat_ &t2); 28 | int frame_count; 29 | 30 | vector< Matrix3d > Rc; 31 | vector< Matrix3d > Rimu; 32 | vector< Matrix3d > Rc_g; 33 | Matrix3d ric; 34 | }; 35 | 36 | 37 | -------------------------------------------------------------------------------- /estimator/src/initial/initial_sfm.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | using namespace Eigen; 12 | using namespace std; 13 | 14 | 15 | 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 | }; -------------------------------------------------------------------------------- /estimator/src/initial/solve_5pts.cpp: -------------------------------------------------------------------------------- 1 | #include "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 | -------------------------------------------------------------------------------- /estimator/src/initial/solve_5pts.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | using namespace std; 5 | 6 | #include 7 | //#include 8 | #include 9 | using namespace Eigen; 10 | 11 | #include 12 | 13 | class MotionEstimator 14 | { 15 | public: 16 | 17 | bool solveRelativeRT(const vector> &corres, Matrix3d &R, Vector3d &T); 18 | 19 | private: 20 | double testTriangulation(const vector &l, 21 | const vector &r, 22 | cv::Mat_ R, cv::Mat_ t); 23 | void decomposeE(cv::Mat E, 24 | cv::Mat_ &R1, cv::Mat_ &R2, 25 | cv::Mat_ &t1, cv::Mat_ &t2); 26 | }; 27 | 28 | 29 | -------------------------------------------------------------------------------- /estimator/src/parameters.cpp: -------------------------------------------------------------------------------- 1 | #include "parameters.h" 2 | 3 | double INIT_DEPTH; 4 | double MIN_PARALLAX; 5 | double ACC_N, ACC_W; 6 | double GYR_N, GYR_W; 7 | 8 | std::vector RIC; 9 | std::vector TIC; 10 | 11 | Eigen::Vector3d G{0.0, 0.0, 9.8}; 12 | 13 | double BIAS_ACC_THRESHOLD; 14 | double BIAS_GYR_THRESHOLD; 15 | double SOLVER_TIME; 16 | int NUM_ITERATIONS; 17 | int ESTIMATE_EXTRINSIC; 18 | int ESTIMATE_TD; 19 | std::string EX_CALIB_RESULT_PATH; 20 | std::string VINS_RESULT_PATH; 21 | std::string FACTOR_GRAPH_RESULT_PATH; 22 | std::string IMU_TOPIC; 23 | double ROW, COL; 24 | double TD; 25 | 26 | bool GNSS_ENABLE; 27 | std::string GNSS_EPHEM_TOPIC; 28 | std::string GNSS_GLO_EPHEM_TOPIC; 29 | std::string GNSS_MEAS_TOPIC; 30 | std::string GNSS_IONO_PARAMS_TOPIC; 31 | std::string GNSS_TP_INFO_TOPIC; 32 | std::vector GNSS_IONO_DEFAULT_PARAMS; 33 | bool GNSS_LOCAL_ONLINE_SYNC; 34 | std::string LOCAL_TRIGGER_INFO_TOPIC; 35 | double GNSS_LOCAL_TIME_DIFF; 36 | double GNSS_ELEVATION_THRES; 37 | double GNSS_PSR_STD_THRES; 38 | double GNSS_DOPP_STD_THRES; 39 | uint32_t GNSS_TRACK_NUM_THRES; 40 | double GNSS_DDT_WEIGHT; 41 | std::string GNSS_RESULT_PATH; 42 | 43 | template 44 | T readParam(ros::NodeHandle &n, std::string name) 45 | { 46 | T ans; 47 | if (n.getParam(name, ans)) 48 | { 49 | ROS_INFO_STREAM("Loaded " << name << ": " << ans); 50 | } 51 | else 52 | { 53 | ROS_ERROR_STREAM("Failed to load " << name); 54 | n.shutdown(); 55 | } 56 | return ans; 57 | } 58 | 59 | void readParameters(ros::NodeHandle &n) 60 | { 61 | std::string config_file; 62 | config_file = readParam(n, "config_file"); 63 | cv::FileStorage fsSettings(config_file, cv::FileStorage::READ); 64 | if(!fsSettings.isOpened()) 65 | { 66 | std::cerr << "ERROR: Wrong path to settings" << std::endl; 67 | } 68 | 69 | fsSettings["imu_topic"] >> IMU_TOPIC; 70 | 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 | std::string tmp_output_dir; 77 | fsSettings["output_dir"] >> tmp_output_dir; 78 | assert(!tmp_output_dir.empty() && "Output directory cannot be empty.\n"); 79 | if (tmp_output_dir[0] == '~') 80 | tmp_output_dir.replace(0, 1, getenv("HOME")); 81 | char actual_output_dir[PATH_MAX+1]; 82 | if(!realpath(tmp_output_dir.c_str(), actual_output_dir)) 83 | std::cerr << "ERROR: Failed to obtain the real path of " << tmp_output_dir << '\n'; 84 | std::string OUTPUT_DIR(actual_output_dir); 85 | FileSystemHelper::createDirectoryIfNotExists(OUTPUT_DIR.c_str()); 86 | 87 | VINS_RESULT_PATH = OUTPUT_DIR + "/vins_result_no_loop.csv"; 88 | std::ofstream fout1(VINS_RESULT_PATH, std::ios::out); 89 | fout1.close(); 90 | std::cout << "result path " << VINS_RESULT_PATH << std::endl; 91 | 92 | FACTOR_GRAPH_RESULT_PATH = OUTPUT_DIR + "/factor_graph_result.txt"; 93 | std::ofstream fout2(FACTOR_GRAPH_RESULT_PATH, std::ios::out); 94 | fout2.close(); 95 | 96 | ACC_N = fsSettings["acc_n"]; 97 | ACC_W = fsSettings["acc_w"]; 98 | GYR_N = fsSettings["gyr_n"]; 99 | GYR_W = fsSettings["gyr_w"]; 100 | G.z() = fsSettings["g_norm"]; 101 | ROW = fsSettings["image_height"]; 102 | COL = fsSettings["image_width"]; 103 | ROS_INFO("ROW: %f COL: %f ", ROW, COL); 104 | 105 | ESTIMATE_EXTRINSIC = fsSettings["estimate_extrinsic"]; 106 | if (ESTIMATE_EXTRINSIC == 2) 107 | { 108 | ROS_WARN("have no prior about extrinsic param, calibrate extrinsic param"); 109 | RIC.push_back(Eigen::Matrix3d::Identity()); 110 | TIC.push_back(Eigen::Vector3d::Zero()); 111 | EX_CALIB_RESULT_PATH = OUTPUT_DIR + "/extrinsic_parameter.csv"; 112 | 113 | } 114 | else 115 | { 116 | if ( ESTIMATE_EXTRINSIC == 1) 117 | { 118 | ROS_WARN(" Optimize extrinsic param around initial guess!"); 119 | EX_CALIB_RESULT_PATH = OUTPUT_DIR + "/extrinsic_parameter.csv"; 120 | } 121 | if (ESTIMATE_EXTRINSIC == 0) 122 | ROS_WARN(" fix extrinsic param "); 123 | 124 | cv::Mat cv_R, cv_T; 125 | fsSettings["extrinsicRotation"] >> cv_R; 126 | fsSettings["extrinsicTranslation"] >> cv_T; 127 | Eigen::Matrix3d eigen_R; 128 | Eigen::Vector3d eigen_T; 129 | cv::cv2eigen(cv_R, eigen_R); 130 | cv::cv2eigen(cv_T, eigen_T); 131 | Eigen::Quaterniond Q(eigen_R); 132 | eigen_R = Q.normalized(); 133 | RIC.push_back(eigen_R); 134 | TIC.push_back(eigen_T); 135 | ROS_INFO_STREAM("Extrinsic_R : " << std::endl << RIC[0]); 136 | ROS_INFO_STREAM("Extrinsic_T : " << std::endl << TIC[0].transpose()); 137 | 138 | } 139 | 140 | INIT_DEPTH = 5.0; 141 | BIAS_ACC_THRESHOLD = 0.1; 142 | BIAS_GYR_THRESHOLD = 0.1; 143 | 144 | TD = fsSettings["td"]; 145 | ESTIMATE_TD = fsSettings["estimate_td"]; 146 | if (ESTIMATE_TD) 147 | ROS_INFO_STREAM("Unsynchronized sensors, online estimate time offset, initial td: " << TD); 148 | else 149 | ROS_INFO_STREAM("Synchronized sensors, fix time offset: " << TD); 150 | 151 | int gnss_enable_value = fsSettings["gnss_enable"]; 152 | GNSS_ENABLE = (gnss_enable_value == 0 ? false : true); 153 | 154 | if (GNSS_ENABLE) 155 | { 156 | fsSettings["gnss_ephem_topic"] >> GNSS_EPHEM_TOPIC; 157 | fsSettings["gnss_glo_ephem_topic"] >> GNSS_GLO_EPHEM_TOPIC; 158 | fsSettings["gnss_meas_topic"] >> GNSS_MEAS_TOPIC; 159 | fsSettings["gnss_iono_params_topic"] >> GNSS_IONO_PARAMS_TOPIC; 160 | cv::Mat cv_iono; 161 | fsSettings["gnss_iono_default_parameters"] >> cv_iono; 162 | Eigen::Matrix eigen_iono; 163 | cv::cv2eigen(cv_iono, eigen_iono); 164 | for (uint32_t i = 0; i < 8; ++i) 165 | GNSS_IONO_DEFAULT_PARAMS.push_back(eigen_iono(0, i)); 166 | 167 | fsSettings["gnss_tp_info_topic"] >> GNSS_TP_INFO_TOPIC; 168 | int gnss_local_online_sync_value = fsSettings["gnss_local_online_sync"]; 169 | GNSS_LOCAL_ONLINE_SYNC = (gnss_local_online_sync_value == 0 ? false : true); 170 | if (GNSS_LOCAL_ONLINE_SYNC) 171 | fsSettings["local_trigger_info_topic"] >> LOCAL_TRIGGER_INFO_TOPIC; 172 | else 173 | GNSS_LOCAL_TIME_DIFF = fsSettings["gnss_local_time_diff"]; 174 | 175 | GNSS_ELEVATION_THRES = fsSettings["gnss_elevation_thres"]; 176 | const double gnss_ddt_sigma = fsSettings["gnss_ddt_sigma"]; 177 | GNSS_PSR_STD_THRES = fsSettings["gnss_psr_std_thres"]; 178 | GNSS_DOPP_STD_THRES = fsSettings["gnss_dopp_std_thres"]; 179 | const double track_thres = fsSettings["gnss_track_num_thres"]; 180 | GNSS_TRACK_NUM_THRES = static_cast(track_thres); 181 | GNSS_DDT_WEIGHT = 1.0 / gnss_ddt_sigma; 182 | GNSS_RESULT_PATH = OUTPUT_DIR + "/gnss_result.csv"; 183 | // clear output file 184 | std::ofstream gnss_output(GNSS_RESULT_PATH, std::ios::out); 185 | gnss_output.close(); 186 | ROS_INFO_STREAM("GNSS enabled"); 187 | } 188 | 189 | fsSettings.release(); 190 | } 191 | -------------------------------------------------------------------------------- /estimator/src/parameters.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include "utility/utility.h" 8 | #include 9 | #include 10 | #include 11 | 12 | const double FOCAL_LENGTH = 460.0; 13 | const int WINDOW_SIZE = 10; 14 | const int NUM_OF_CAM = 1; 15 | const int NUM_OF_F = 1000; 16 | //#define UNIT_SPHERE_ERROR 17 | 18 | extern double INIT_DEPTH; 19 | extern double MIN_PARALLAX; 20 | extern int ESTIMATE_EXTRINSIC; 21 | 22 | extern double ACC_N, ACC_W; 23 | extern double GYR_N, GYR_W; 24 | 25 | extern std::vector RIC; 26 | extern std::vector TIC; 27 | extern Eigen::Vector3d G; 28 | 29 | extern double BIAS_ACC_THRESHOLD; 30 | extern double BIAS_GYR_THRESHOLD; 31 | extern double SOLVER_TIME; 32 | extern int NUM_ITERATIONS; 33 | extern std::string EX_CALIB_RESULT_PATH; 34 | extern std::string VINS_RESULT_PATH; 35 | extern std::string FACTOR_GRAPH_RESULT_PATH; 36 | extern std::string IMU_TOPIC; 37 | extern double TD; 38 | extern int ESTIMATE_TD; 39 | extern double ROW, COL; 40 | 41 | extern bool GNSS_ENABLE; 42 | extern std::string GNSS_EPHEM_TOPIC; 43 | extern std::string GNSS_GLO_EPHEM_TOPIC; 44 | extern std::string GNSS_MEAS_TOPIC; 45 | extern std::string GNSS_IONO_PARAMS_TOPIC; 46 | extern std::string GNSS_TP_INFO_TOPIC; 47 | extern std::vector GNSS_IONO_DEFAULT_PARAMS; 48 | extern bool GNSS_LOCAL_ONLINE_SYNC; 49 | extern std::string LOCAL_TRIGGER_INFO_TOPIC; 50 | extern double GNSS_LOCAL_TIME_DIFF; 51 | extern double GNSS_ELEVATION_THRES; 52 | extern double GNSS_PSR_STD_THRES; 53 | extern double GNSS_DOPP_STD_THRES; 54 | extern uint32_t GNSS_TRACK_NUM_THRES; 55 | extern double GNSS_DDT_WEIGHT; 56 | extern std::string GNSS_RESULT_PATH; 57 | 58 | void readParameters(ros::NodeHandle &n); 59 | 60 | enum SIZE_PARAMETERIZATION 61 | { 62 | SIZE_POSE = 7, 63 | SIZE_SPEEDBIAS = 9, 64 | SIZE_FEATURE = 1 65 | }; 66 | 67 | enum StateOrder 68 | { 69 | O_P = 0, 70 | O_R = 3, 71 | O_V = 6, 72 | O_BA = 9, 73 | O_BG = 12 74 | }; 75 | 76 | enum NoiseOrder 77 | { 78 | O_AN = 0, 79 | O_GN = 3, 80 | O_AW = 6, 81 | O_GW = 9 82 | }; 83 | -------------------------------------------------------------------------------- /estimator/src/utility/CameraPoseVisualization.cpp: -------------------------------------------------------------------------------- 1 | #include "CameraPoseVisualization.h" 2 | 3 | const Eigen::Vector3d CameraPoseVisualization::imlt = Eigen::Vector3d(-1.0, -0.5, 1.0); 4 | const Eigen::Vector3d CameraPoseVisualization::imrt = Eigen::Vector3d( 1.0, -0.5, 1.0); 5 | const Eigen::Vector3d CameraPoseVisualization::imlb = Eigen::Vector3d(-1.0, 0.5, 1.0); 6 | const Eigen::Vector3d CameraPoseVisualization::imrb = Eigen::Vector3d( 1.0, 0.5, 1.0); 7 | const Eigen::Vector3d CameraPoseVisualization::lt0 = Eigen::Vector3d(-0.7, -0.5, 1.0); 8 | const Eigen::Vector3d CameraPoseVisualization::lt1 = Eigen::Vector3d(-0.7, -0.2, 1.0); 9 | const Eigen::Vector3d CameraPoseVisualization::lt2 = Eigen::Vector3d(-1.0, -0.2, 1.0); 10 | const Eigen::Vector3d CameraPoseVisualization::oc = Eigen::Vector3d(0.0, 0.0, 0.0); 11 | 12 | void Eigen2Point(const Eigen::Vector3d& v, geometry_msgs::Point& p) { 13 | p.x = v.x(); 14 | p.y = v.y(); 15 | p.z = v.z(); 16 | } 17 | 18 | CameraPoseVisualization::CameraPoseVisualization(float r, float g, float b, float a) 19 | : m_marker_ns("CameraPoseVisualization"), m_scale(0.2), m_line_width(0.01) { 20 | m_image_boundary_color.r = r; 21 | m_image_boundary_color.g = g; 22 | m_image_boundary_color.b = b; 23 | m_image_boundary_color.a = a; 24 | m_optical_center_connector_color.r = r; 25 | m_optical_center_connector_color.g = g; 26 | m_optical_center_connector_color.b = b; 27 | m_optical_center_connector_color.a = a; 28 | } 29 | 30 | void CameraPoseVisualization::setImageBoundaryColor(float r, float g, float b, float a) { 31 | m_image_boundary_color.r = r; 32 | m_image_boundary_color.g = g; 33 | m_image_boundary_color.b = b; 34 | m_image_boundary_color.a = a; 35 | } 36 | 37 | void CameraPoseVisualization::setOpticalCenterConnectorColor(float r, float g, float b, float a) { 38 | m_optical_center_connector_color.r = r; 39 | m_optical_center_connector_color.g = g; 40 | m_optical_center_connector_color.b = b; 41 | m_optical_center_connector_color.a = a; 42 | } 43 | 44 | void CameraPoseVisualization::setScale(double s) { 45 | m_scale = s; 46 | } 47 | void CameraPoseVisualization::setLineWidth(double width) { 48 | m_line_width = width; 49 | } 50 | void CameraPoseVisualization::add_edge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1){ 51 | visualization_msgs::Marker marker; 52 | 53 | marker.ns = m_marker_ns; 54 | marker.id = m_markers.size() + 1; 55 | marker.type = visualization_msgs::Marker::LINE_LIST; 56 | marker.action = visualization_msgs::Marker::ADD; 57 | marker.scale.x = 0.005; 58 | 59 | marker.color.g = 1.0f; 60 | marker.color.a = 1.0; 61 | 62 | geometry_msgs::Point point0, point1; 63 | 64 | Eigen2Point(p0, point0); 65 | Eigen2Point(p1, point1); 66 | 67 | marker.points.push_back(point0); 68 | marker.points.push_back(point1); 69 | 70 | m_markers.push_back(marker); 71 | } 72 | 73 | void CameraPoseVisualization::add_loopedge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1){ 74 | visualization_msgs::Marker marker; 75 | 76 | marker.ns = m_marker_ns; 77 | marker.id = m_markers.size() + 1; 78 | marker.type = visualization_msgs::Marker::LINE_LIST; 79 | marker.action = visualization_msgs::Marker::ADD; 80 | marker.scale.x = 0.04; 81 | //marker.scale.x = 0.3; 82 | 83 | marker.color.r = 1.0f; 84 | marker.color.b = 1.0f; 85 | marker.color.a = 1.0; 86 | 87 | geometry_msgs::Point point0, point1; 88 | 89 | Eigen2Point(p0, point0); 90 | Eigen2Point(p1, point1); 91 | 92 | marker.points.push_back(point0); 93 | marker.points.push_back(point1); 94 | 95 | m_markers.push_back(marker); 96 | } 97 | 98 | 99 | void CameraPoseVisualization::add_pose(const Eigen::Vector3d& p, const Eigen::Quaterniond& q) { 100 | visualization_msgs::Marker marker; 101 | 102 | marker.ns = m_marker_ns; 103 | marker.id = m_markers.size() + 1; 104 | marker.type = visualization_msgs::Marker::LINE_STRIP; 105 | marker.action = visualization_msgs::Marker::ADD; 106 | marker.scale.x = m_line_width; 107 | 108 | marker.pose.position.x = 0.0; 109 | marker.pose.position.y = 0.0; 110 | marker.pose.position.z = 0.0; 111 | marker.pose.orientation.w = 1.0; 112 | marker.pose.orientation.x = 0.0; 113 | marker.pose.orientation.y = 0.0; 114 | marker.pose.orientation.z = 0.0; 115 | 116 | 117 | geometry_msgs::Point pt_lt, pt_lb, pt_rt, pt_rb, pt_oc, pt_lt0, pt_lt1, pt_lt2; 118 | 119 | Eigen2Point(q * (m_scale *imlt) + p, pt_lt); 120 | Eigen2Point(q * (m_scale *imlb) + p, pt_lb); 121 | Eigen2Point(q * (m_scale *imrt) + p, pt_rt); 122 | Eigen2Point(q * (m_scale *imrb) + p, pt_rb); 123 | Eigen2Point(q * (m_scale *lt0 ) + p, pt_lt0); 124 | Eigen2Point(q * (m_scale *lt1 ) + p, pt_lt1); 125 | Eigen2Point(q * (m_scale *lt2 ) + p, pt_lt2); 126 | Eigen2Point(q * (m_scale *oc ) + p, pt_oc); 127 | 128 | // image boundaries 129 | marker.points.push_back(pt_lt); 130 | marker.points.push_back(pt_lb); 131 | marker.colors.push_back(m_image_boundary_color); 132 | marker.colors.push_back(m_image_boundary_color); 133 | 134 | marker.points.push_back(pt_lb); 135 | marker.points.push_back(pt_rb); 136 | marker.colors.push_back(m_image_boundary_color); 137 | marker.colors.push_back(m_image_boundary_color); 138 | 139 | marker.points.push_back(pt_rb); 140 | marker.points.push_back(pt_rt); 141 | marker.colors.push_back(m_image_boundary_color); 142 | marker.colors.push_back(m_image_boundary_color); 143 | 144 | marker.points.push_back(pt_rt); 145 | marker.points.push_back(pt_lt); 146 | marker.colors.push_back(m_image_boundary_color); 147 | marker.colors.push_back(m_image_boundary_color); 148 | 149 | // top-left indicator 150 | marker.points.push_back(pt_lt0); 151 | marker.points.push_back(pt_lt1); 152 | marker.colors.push_back(m_image_boundary_color); 153 | marker.colors.push_back(m_image_boundary_color); 154 | 155 | marker.points.push_back(pt_lt1); 156 | marker.points.push_back(pt_lt2); 157 | marker.colors.push_back(m_image_boundary_color); 158 | marker.colors.push_back(m_image_boundary_color); 159 | 160 | // optical center connector 161 | marker.points.push_back(pt_lt); 162 | marker.points.push_back(pt_oc); 163 | marker.colors.push_back(m_optical_center_connector_color); 164 | marker.colors.push_back(m_optical_center_connector_color); 165 | 166 | 167 | marker.points.push_back(pt_lb); 168 | marker.points.push_back(pt_oc); 169 | marker.colors.push_back(m_optical_center_connector_color); 170 | marker.colors.push_back(m_optical_center_connector_color); 171 | 172 | marker.points.push_back(pt_rt); 173 | marker.points.push_back(pt_oc); 174 | marker.colors.push_back(m_optical_center_connector_color); 175 | marker.colors.push_back(m_optical_center_connector_color); 176 | 177 | marker.points.push_back(pt_rb); 178 | marker.points.push_back(pt_oc); 179 | marker.colors.push_back(m_optical_center_connector_color); 180 | marker.colors.push_back(m_optical_center_connector_color); 181 | 182 | m_markers.push_back(marker); 183 | } 184 | 185 | void CameraPoseVisualization::reset() { 186 | m_markers.clear(); 187 | } 188 | 189 | void CameraPoseVisualization::publish_by( ros::Publisher &pub, const std_msgs::Header &header ) { 190 | visualization_msgs::MarkerArray markerArray_msg; 191 | 192 | for(auto& marker : m_markers) { 193 | marker.header = header; 194 | markerArray_msg.markers.push_back(marker); 195 | } 196 | 197 | pub.publish(markerArray_msg); 198 | } -------------------------------------------------------------------------------- /estimator/src/utility/CameraPoseVisualization.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | class CameraPoseVisualization { 11 | public: 12 | std::string m_marker_ns; 13 | 14 | CameraPoseVisualization(float r, float g, float b, float a); 15 | 16 | void setImageBoundaryColor(float r, float g, float b, float a=1.0); 17 | void setOpticalCenterConnectorColor(float r, float g, float b, float a=1.0); 18 | void setScale(double s); 19 | void setLineWidth(double width); 20 | 21 | void add_pose(const Eigen::Vector3d& p, const Eigen::Quaterniond& q); 22 | void reset(); 23 | 24 | void publish_by(ros::Publisher& pub, const std_msgs::Header& header); 25 | void add_edge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1); 26 | void add_loopedge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1); 27 | private: 28 | std::vector m_markers; 29 | std_msgs::ColorRGBA m_image_boundary_color; 30 | std_msgs::ColorRGBA m_optical_center_connector_color; 31 | double m_scale; 32 | double m_line_width; 33 | 34 | static const Eigen::Vector3d imlt; 35 | static const Eigen::Vector3d imlb; 36 | static const Eigen::Vector3d imrt; 37 | static const Eigen::Vector3d imrb; 38 | static const Eigen::Vector3d oc ; 39 | static const Eigen::Vector3d lt0 ; 40 | static const Eigen::Vector3d lt1 ; 41 | static const Eigen::Vector3d lt2 ; 42 | }; 43 | -------------------------------------------------------------------------------- /estimator/src/utility/tic_toc.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | class TicToc 8 | { 9 | public: 10 | TicToc() 11 | { 12 | tic(); 13 | } 14 | 15 | void tic() 16 | { 17 | start = std::chrono::system_clock::now(); 18 | } 19 | 20 | double toc() 21 | { 22 | end = std::chrono::system_clock::now(); 23 | std::chrono::duration elapsed_seconds = end - start; 24 | return elapsed_seconds.count() * 1000; 25 | } 26 | 27 | private: 28 | std::chrono::time_point start, end; 29 | }; 30 | -------------------------------------------------------------------------------- /estimator/src/utility/utility.cpp: -------------------------------------------------------------------------------- 1 | #include "utility.h" 2 | 3 | Eigen::Matrix3d Utility::g2R(const Eigen::Vector3d &g) 4 | { 5 | Eigen::Matrix3d R0; 6 | Eigen::Vector3d ng1 = g.normalized(); 7 | Eigen::Vector3d ng2{0, 0, 1.0}; 8 | R0 = Eigen::Quaterniond::FromTwoVectors(ng1, ng2).toRotationMatrix(); 9 | double yaw = Utility::R2ypr(R0).x(); 10 | R0 = Utility::ypr2R(Eigen::Vector3d{-yaw, 0, 0}) * R0; 11 | // R0 = Utility::ypr2R(Eigen::Vector3d{-90, 0, 0}) * R0; 12 | return R0; 13 | } 14 | -------------------------------------------------------------------------------- /estimator/src/utility/utility.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | class Utility 13 | { 14 | public: 15 | template 16 | static Eigen::Quaternion deltaQ(const Eigen::MatrixBase &theta) 17 | { 18 | typedef typename Derived::Scalar Scalar_t; 19 | 20 | Eigen::Quaternion dq; 21 | Eigen::Matrix half_theta = theta; 22 | half_theta /= static_cast(2.0); 23 | dq.w() = static_cast(1.0); 24 | dq.x() = half_theta.x(); 25 | dq.y() = half_theta.y(); 26 | dq.z() = half_theta.z(); 27 | return dq; 28 | } 29 | 30 | template 31 | static Eigen::Matrix skewSymmetric(const Eigen::MatrixBase &q) 32 | { 33 | Eigen::Matrix ans; 34 | ans << typename Derived::Scalar(0), -q(2), q(1), 35 | q(2), typename Derived::Scalar(0), -q(0), 36 | -q(1), q(0), typename Derived::Scalar(0); 37 | return ans; 38 | } 39 | 40 | template 41 | static Eigen::Quaternion positify(const Eigen::QuaternionBase &q) 42 | { 43 | //printf("a: %f %f %f %f", q.w(), q.x(), q.y(), q.z()); 44 | //Eigen::Quaternion p(-q.w(), -q.x(), -q.y(), -q.z()); 45 | //printf("b: %f %f %f %f", p.w(), p.x(), p.y(), p.z()); 46 | //return q.template w() >= (typename Derived::Scalar)(0.0) ? q : Eigen::Quaternion(-q.w(), -q.x(), -q.y(), -q.z()); 47 | return q; 48 | } 49 | 50 | template 51 | static Eigen::Matrix Qleft(const Eigen::QuaternionBase &q) 52 | { 53 | Eigen::Quaternion qq = positify(q); 54 | Eigen::Matrix ans; 55 | ans(0, 0) = qq.w(), ans.template block<1, 3>(0, 1) = -qq.vec().transpose(); 56 | ans.template block<3, 1>(1, 0) = qq.vec(), ans.template block<3, 3>(1, 1) = qq.w() * Eigen::Matrix::Identity() + skewSymmetric(qq.vec()); 57 | return ans; 58 | } 59 | 60 | template 61 | static Eigen::Matrix Qright(const Eigen::QuaternionBase &p) 62 | { 63 | Eigen::Quaternion pp = positify(p); 64 | Eigen::Matrix ans; 65 | ans(0, 0) = pp.w(), ans.template block<1, 3>(0, 1) = -pp.vec().transpose(); 66 | ans.template block<3, 1>(1, 0) = pp.vec(), ans.template block<3, 3>(1, 1) = pp.w() * Eigen::Matrix::Identity() - skewSymmetric(pp.vec()); 67 | return ans; 68 | } 69 | 70 | static Eigen::Vector3d R2ypr(const Eigen::Matrix3d &R) 71 | { 72 | Eigen::Vector3d n = R.col(0); 73 | Eigen::Vector3d o = R.col(1); 74 | Eigen::Vector3d a = R.col(2); 75 | 76 | Eigen::Vector3d ypr(3); 77 | double y = atan2(n(1), n(0)); 78 | double p = atan2(-n(2), n(0) * cos(y) + n(1) * sin(y)); 79 | double r = atan2(a(0) * sin(y) - a(1) * cos(y), -o(0) * sin(y) + o(1) * cos(y)); 80 | ypr(0) = y; 81 | ypr(1) = p; 82 | ypr(2) = r; 83 | 84 | return ypr / M_PI * 180.0; 85 | } 86 | 87 | template 88 | static Eigen::Matrix ypr2R(const Eigen::MatrixBase &ypr) 89 | { 90 | typedef typename Derived::Scalar Scalar_t; 91 | 92 | Scalar_t y = ypr(0) / 180.0 * M_PI; 93 | Scalar_t p = ypr(1) / 180.0 * M_PI; 94 | Scalar_t r = ypr(2) / 180.0 * M_PI; 95 | 96 | Eigen::Matrix Rz; 97 | Rz << cos(y), -sin(y), 0, 98 | sin(y), cos(y), 0, 99 | 0, 0, 1; 100 | 101 | Eigen::Matrix Ry; 102 | Ry << cos(p), 0., sin(p), 103 | 0., 1., 0., 104 | -sin(p), 0., cos(p); 105 | 106 | Eigen::Matrix Rx; 107 | Rx << 1., 0., 0., 108 | 0., cos(r), -sin(r), 109 | 0., sin(r), cos(r); 110 | 111 | return Rz * Ry * Rx; 112 | } 113 | 114 | static Eigen::Matrix3d g2R(const Eigen::Vector3d &g); 115 | 116 | template 117 | struct uint_ 118 | { 119 | }; 120 | 121 | template 122 | void unroller(const Lambda &f, const IterT &iter, uint_) 123 | { 124 | unroller(f, iter, uint_()); 125 | f(iter + N); 126 | } 127 | 128 | template 129 | void unroller(const Lambda &f, const IterT &iter, uint_<0>) 130 | { 131 | f(iter); 132 | } 133 | 134 | template 135 | static T normalizeAngle(const T& angle_degrees) { 136 | T two_pi(2.0 * 180); 137 | if (angle_degrees > 0) 138 | return angle_degrees - 139 | two_pi * std::floor((angle_degrees + T(180)) / two_pi); 140 | else 141 | return angle_degrees + 142 | two_pi * std::floor((-angle_degrees + T(180)) / two_pi); 143 | }; 144 | }; 145 | 146 | class FileSystemHelper 147 | { 148 | public: 149 | 150 | /****************************************************************************** 151 | * Recursively create directory if `path` not exists. 152 | * Return 0 if success. 153 | *****************************************************************************/ 154 | static int createDirectoryIfNotExists(const char *path) 155 | { 156 | struct stat info; 157 | int statRC = stat(path, &info); 158 | if( statRC != 0 ) 159 | { 160 | if (errno == ENOENT) 161 | { 162 | printf("%s not exists, trying to create it \n", path); 163 | if (! createDirectoryIfNotExists(dirname(strdupa(path)))) 164 | { 165 | if (mkdir(path, S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH) != 0) 166 | { 167 | fprintf(stderr, "Failed to create folder %s \n", path); 168 | return 1; 169 | } 170 | else 171 | return 0; 172 | } 173 | else 174 | return 1; 175 | } // directory not exists 176 | if (errno == ENOTDIR) 177 | { 178 | fprintf(stderr, "%s is not a directory path \n", path); 179 | return 1; 180 | } // something in path prefix is not a dir 181 | return 1; 182 | } 183 | return ( info.st_mode & S_IFDIR ) ? 0 : 1; 184 | } 185 | }; 186 | -------------------------------------------------------------------------------- /estimator/src/utility/visualization.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include "CameraPoseVisualization.h" 19 | #include 20 | #include 21 | 22 | #include "../estimator.h" 23 | #include "../parameters.h" 24 | 25 | extern ros::Publisher pub_odometry; 26 | extern ros::Publisher pub_path, pub_pose; 27 | extern ros::Publisher pub_cloud, pub_map; 28 | extern ros::Publisher pub_key_poses; 29 | extern ros::Publisher pub_ref_pose, pub_cur_pose; 30 | extern ros::Publisher pub_key; 31 | extern nav_msgs::Path path; 32 | extern ros::Publisher pub_pose_graph; 33 | extern int IMAGE_ROW, IMAGE_COL; 34 | 35 | void registerPub(ros::NodeHandle &n); 36 | 37 | void pubLatestOdometry(const Eigen::Vector3d &P, const Eigen::Quaterniond &Q, const Eigen::Vector3d &V, const std_msgs::Header &header); 38 | 39 | void printStatistics(const Estimator &estimator, double t); 40 | 41 | void pubOdometry(const Estimator &estimator, const std_msgs::Header &header); 42 | 43 | void pubGnssResult(const Estimator &estimator, const std_msgs::Header &header); 44 | 45 | void pubInitialGuess(const Estimator &estimator, const std_msgs::Header &header); 46 | 47 | void pubKeyPoses(const Estimator &estimator, const std_msgs::Header &header); 48 | 49 | void pubCameraPose(const Estimator &estimator, const std_msgs::Header &header); 50 | 51 | void pubPointCloud(const Estimator &estimator, const std_msgs::Header &header); 52 | 53 | void pubTF(const Estimator &estimator, const std_msgs::Header &header); 54 | 55 | void pubKeyframe(const Estimator &estimator); -------------------------------------------------------------------------------- /feature_tracker/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(gvins_feature_tracker) 3 | 4 | set(CMAKE_BUILD_TYPE "Release") 5 | set(CMAKE_CXX_FLAGS "-std=c++11") 6 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g") 7 | 8 | find_package(catkin REQUIRED COMPONENTS 9 | roscpp 10 | std_msgs 11 | sensor_msgs 12 | cv_bridge 13 | gvins_camera_model 14 | ) 15 | 16 | find_package(OpenCV REQUIRED) 17 | 18 | catkin_package() 19 | 20 | include_directories( 21 | ${catkin_INCLUDE_DIRS} 22 | ) 23 | 24 | set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) 25 | find_package(Eigen3) 26 | include_directories( 27 | ${catkin_INCLUDE_DIRS} 28 | ${EIGEN3_INCLUDE_DIR} 29 | ) 30 | 31 | add_executable(gvins_feature_tracker 32 | src/feature_tracker_node.cpp 33 | src/parameters.cpp 34 | src/feature_tracker.cpp 35 | ) 36 | 37 | target_link_libraries(gvins_feature_tracker ${catkin_LIBRARIES} ${OpenCV_LIBS}) 38 | -------------------------------------------------------------------------------- /feature_tracker/cmake/FindEigen.cmake: -------------------------------------------------------------------------------- 1 | # Ceres Solver - A fast non-linear least squares minimizer 2 | # Copyright 2015 Google Inc. All rights reserved. 3 | # http://ceres-solver.org/ 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions are met: 7 | # 8 | # * Redistributions of source code must retain the above copyright notice, 9 | # this list of conditions and the following disclaimer. 10 | # * Redistributions in binary form must reproduce the above copyright notice, 11 | # this list of conditions and the following disclaimer in the documentation 12 | # and/or other materials provided with the distribution. 13 | # * Neither the name of Google Inc. nor the names of its contributors may be 14 | # used to endorse or promote products derived from this software without 15 | # specific prior written permission. 16 | # 17 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | # POSSIBILITY OF SUCH DAMAGE. 28 | # 29 | # Author: alexs.mac@gmail.com (Alex Stewart) 30 | # 31 | 32 | # FindEigen.cmake - Find Eigen library, version >= 3. 33 | # 34 | # This module defines the following variables: 35 | # 36 | # EIGEN_FOUND: TRUE iff Eigen is found. 37 | # EIGEN_INCLUDE_DIRS: Include directories for Eigen. 38 | # 39 | # EIGEN_VERSION: Extracted from Eigen/src/Core/util/Macros.h 40 | # EIGEN_WORLD_VERSION: Equal to 3 if EIGEN_VERSION = 3.2.0 41 | # EIGEN_MAJOR_VERSION: Equal to 2 if EIGEN_VERSION = 3.2.0 42 | # EIGEN_MINOR_VERSION: Equal to 0 if EIGEN_VERSION = 3.2.0 43 | # 44 | # The following variables control the behaviour of this module: 45 | # 46 | # EIGEN_INCLUDE_DIR_HINTS: List of additional directories in which to 47 | # search for eigen includes, e.g: /timbuktu/eigen3. 48 | # 49 | # The following variables are also defined by this module, but in line with 50 | # CMake recommended FindPackage() module style should NOT be referenced directly 51 | # by callers (use the plural variables detailed above instead). These variables 52 | # do however affect the behaviour of the module via FIND_[PATH/LIBRARY]() which 53 | # are NOT re-called (i.e. search for library is not repeated) if these variables 54 | # are set with valid values _in the CMake cache_. This means that if these 55 | # variables are set directly in the cache, either by the user in the CMake GUI, 56 | # or by the user passing -DVAR=VALUE directives to CMake when called (which 57 | # explicitly defines a cache variable), then they will be used verbatim, 58 | # bypassing the HINTS variables and other hard-coded search locations. 59 | # 60 | # EIGEN_INCLUDE_DIR: Include directory for CXSparse, not including the 61 | # include directory of any dependencies. 62 | 63 | # Called if we failed to find Eigen or any of it's required dependencies, 64 | # unsets all public (designed to be used externally) variables and reports 65 | # error message at priority depending upon [REQUIRED/QUIET/] argument. 66 | macro(EIGEN_REPORT_NOT_FOUND REASON_MSG) 67 | unset(EIGEN_FOUND) 68 | unset(EIGEN_INCLUDE_DIRS) 69 | # Make results of search visible in the CMake GUI if Eigen has not 70 | # been found so that user does not have to toggle to advanced view. 71 | mark_as_advanced(CLEAR EIGEN_INCLUDE_DIR) 72 | # Note _FIND_[REQUIRED/QUIETLY] variables defined by FindPackage() 73 | # use the camelcase library name, not uppercase. 74 | if (Eigen_FIND_QUIETLY) 75 | message(STATUS "Failed to find Eigen - " ${REASON_MSG} ${ARGN}) 76 | elseif (Eigen_FIND_REQUIRED) 77 | message(FATAL_ERROR "Failed to find Eigen - " ${REASON_MSG} ${ARGN}) 78 | else() 79 | # Neither QUIETLY nor REQUIRED, use no priority which emits a message 80 | # but continues configuration and allows generation. 81 | message("-- Failed to find Eigen - " ${REASON_MSG} ${ARGN}) 82 | endif () 83 | return() 84 | endmacro(EIGEN_REPORT_NOT_FOUND) 85 | 86 | # Protect against any alternative find_package scripts for this library having 87 | # been called previously (in a client project) which set EIGEN_FOUND, but not 88 | # the other variables we require / set here which could cause the search logic 89 | # here to fail. 90 | unset(EIGEN_FOUND) 91 | 92 | # Search user-installed locations first, so that we prefer user installs 93 | # to system installs where both exist. 94 | list(APPEND EIGEN_CHECK_INCLUDE_DIRS 95 | /usr/local/include 96 | /usr/local/homebrew/include # Mac OS X 97 | /opt/local/var/macports/software # Mac OS X. 98 | /opt/local/include 99 | /usr/include) 100 | # Additional suffixes to try appending to each search path. 101 | list(APPEND EIGEN_CHECK_PATH_SUFFIXES 102 | eigen3 # Default root directory for Eigen. 103 | Eigen/include/eigen3 # Windows (for C:/Program Files prefix) < 3.3 104 | Eigen3/include/eigen3 ) # Windows (for C:/Program Files prefix) >= 3.3 105 | 106 | # Search supplied hint directories first if supplied. 107 | find_path(EIGEN_INCLUDE_DIR 108 | NAMES Eigen/Core 109 | PATHS ${EIGEN_INCLUDE_DIR_HINTS} 110 | ${EIGEN_CHECK_INCLUDE_DIRS} 111 | PATH_SUFFIXES ${EIGEN_CHECK_PATH_SUFFIXES}) 112 | 113 | if (NOT EIGEN_INCLUDE_DIR OR 114 | NOT EXISTS ${EIGEN_INCLUDE_DIR}) 115 | eigen_report_not_found( 116 | "Could not find eigen3 include directory, set EIGEN_INCLUDE_DIR to " 117 | "path to eigen3 include directory, e.g. /usr/local/include/eigen3.") 118 | endif (NOT EIGEN_INCLUDE_DIR OR 119 | NOT EXISTS ${EIGEN_INCLUDE_DIR}) 120 | 121 | # Mark internally as found, then verify. EIGEN_REPORT_NOT_FOUND() unsets 122 | # if called. 123 | set(EIGEN_FOUND TRUE) 124 | 125 | # Extract Eigen version from Eigen/src/Core/util/Macros.h 126 | if (EIGEN_INCLUDE_DIR) 127 | set(EIGEN_VERSION_FILE ${EIGEN_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h) 128 | if (NOT EXISTS ${EIGEN_VERSION_FILE}) 129 | eigen_report_not_found( 130 | "Could not find file: ${EIGEN_VERSION_FILE} " 131 | "containing version information in Eigen install located at: " 132 | "${EIGEN_INCLUDE_DIR}.") 133 | else (NOT EXISTS ${EIGEN_VERSION_FILE}) 134 | file(READ ${EIGEN_VERSION_FILE} EIGEN_VERSION_FILE_CONTENTS) 135 | 136 | string(REGEX MATCH "#define EIGEN_WORLD_VERSION [0-9]+" 137 | EIGEN_WORLD_VERSION "${EIGEN_VERSION_FILE_CONTENTS}") 138 | string(REGEX REPLACE "#define EIGEN_WORLD_VERSION ([0-9]+)" "\\1" 139 | EIGEN_WORLD_VERSION "${EIGEN_WORLD_VERSION}") 140 | 141 | string(REGEX MATCH "#define EIGEN_MAJOR_VERSION [0-9]+" 142 | EIGEN_MAJOR_VERSION "${EIGEN_VERSION_FILE_CONTENTS}") 143 | string(REGEX REPLACE "#define EIGEN_MAJOR_VERSION ([0-9]+)" "\\1" 144 | EIGEN_MAJOR_VERSION "${EIGEN_MAJOR_VERSION}") 145 | 146 | string(REGEX MATCH "#define EIGEN_MINOR_VERSION [0-9]+" 147 | EIGEN_MINOR_VERSION "${EIGEN_VERSION_FILE_CONTENTS}") 148 | string(REGEX REPLACE "#define EIGEN_MINOR_VERSION ([0-9]+)" "\\1" 149 | EIGEN_MINOR_VERSION "${EIGEN_MINOR_VERSION}") 150 | 151 | # This is on a single line s/t CMake does not interpret it as a list of 152 | # elements and insert ';' separators which would result in 3.;2.;0 nonsense. 153 | set(EIGEN_VERSION "${EIGEN_WORLD_VERSION}.${EIGEN_MAJOR_VERSION}.${EIGEN_MINOR_VERSION}") 154 | endif (NOT EXISTS ${EIGEN_VERSION_FILE}) 155 | endif (EIGEN_INCLUDE_DIR) 156 | 157 | # Set standard CMake FindPackage variables if found. 158 | if (EIGEN_FOUND) 159 | set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR}) 160 | endif (EIGEN_FOUND) 161 | 162 | # Handle REQUIRED / QUIET optional arguments and version. 163 | include(FindPackageHandleStandardArgs) 164 | find_package_handle_standard_args(Eigen 165 | REQUIRED_VARS EIGEN_INCLUDE_DIRS 166 | VERSION_VAR EIGEN_VERSION) 167 | 168 | # Only mark internal variables as advanced if we found Eigen, otherwise 169 | # leave it visible in the standard GUI for the user to set manually. 170 | if (EIGEN_FOUND) 171 | mark_as_advanced(FORCE EIGEN_INCLUDE_DIR) 172 | endif (EIGEN_FOUND) 173 | -------------------------------------------------------------------------------- /feature_tracker/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | gvins_feature_tracker 4 | 0.0.0 5 | The feature_tracker package 6 | 7 | 8 | 9 | 10 | dvorak 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | roscpp 44 | gvins_camera_model 45 | message_generation 46 | roscpp 47 | gvins_camera_model 48 | message_runtime 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /feature_tracker/src/feature_tracker.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | #include "camodocal/camera_models/CameraFactory.h" 13 | #include "camodocal/camera_models/CataCamera.h" 14 | #include "camodocal/camera_models/PinholeCamera.h" 15 | 16 | #include "parameters.h" 17 | #include "tic_toc.h" 18 | 19 | using namespace std; 20 | using namespace camodocal; 21 | using namespace Eigen; 22 | 23 | bool inBorder(const cv::Point2f &pt); 24 | 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 | -------------------------------------------------------------------------------- /feature_tracker/src/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 GVINS_FOLDER_PATH = readParam(n, "gvins_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 = GVINS_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 | -------------------------------------------------------------------------------- /feature_tracker/src/parameters.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | 5 | extern int ROW; 6 | extern int COL; 7 | extern int FOCAL_LENGTH; 8 | const int NUM_OF_CAM = 1; 9 | 10 | 11 | extern std::string IMAGE_TOPIC; 12 | extern std::string IMU_TOPIC; 13 | extern std::string FISHEYE_MASK; 14 | extern std::vector CAM_NAMES; 15 | extern int MAX_CNT; 16 | extern int MIN_DIST; 17 | extern int WINDOW_SIZE; 18 | extern int FREQ; 19 | extern double F_THRESHOLD; 20 | extern int SHOW_TRACK; 21 | extern int STEREO_TRACK; 22 | extern int EQUALIZE; 23 | extern int FISHEYE; 24 | extern bool PUB_THIS_FRAME; 25 | 26 | void readParameters(ros::NodeHandle &n); 27 | -------------------------------------------------------------------------------- /feature_tracker/src/tic_toc.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | class TicToc 8 | { 9 | public: 10 | TicToc() 11 | { 12 | tic(); 13 | } 14 | 15 | void tic() 16 | { 17 | start = std::chrono::system_clock::now(); 18 | } 19 | 20 | double toc() 21 | { 22 | end = std::chrono::system_clock::now(); 23 | std::chrono::duration elapsed_seconds = end - start; 24 | return elapsed_seconds.count() * 1000; 25 | } 26 | 27 | private: 28 | std::chrono::time_point start, end; 29 | }; 30 | -------------------------------------------------------------------------------- /figures/system_snapshot.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/syllinlin/GVINS_Noted_ByLSY/34b5e91c2b4d12325d6c82d51bba5dd7bb024236/figures/system_snapshot.png --------------------------------------------------------------------------------