├── 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 ├── advio_12_config.yaml ├── ol_market1_config.yaml ├── rpvio_rviz_config.rviz └── rpvio_sim_config.yaml ├── plane_segmentation ├── RecoverPlane_perpendicular.py ├── crf_inference.py ├── data_loader_new.py ├── inference.py ├── net.py ├── openloris.txt ├── pretrained_model │ ├── model.data-00000-of-00001 │ ├── model.index │ └── model.meta ├── requirements.txt ├── train.py └── utils.py ├── rpvio.patch ├── rpvio_estimator ├── CMakeLists.txt ├── cmake │ └── FindEigen.cmake ├── launch │ ├── advio_12.launch │ ├── ol_market1.launch │ ├── rpvio_rviz.launch │ └── rpvio_sim.launch ├── package.xml └── src │ ├── estimator.cpp │ ├── estimator.h │ ├── estimator_node.cpp │ ├── factor │ ├── homography_factor.h │ ├── imu_factor.h │ ├── integration_base.h │ ├── marginalization_factor.cpp │ ├── marginalization_factor.h │ ├── pose_local_parameterization.cpp │ ├── pose_local_parameterization.h │ ├── projection_factor.cpp │ ├── projection_factor.h │ ├── projection_td_factor.cpp │ └── projection_td_factor.h │ ├── feature_manager.cpp │ ├── feature_manager.h │ ├── initial │ ├── initial_aligment.cpp │ ├── initial_alignment.h │ ├── initial_ex_rotation.cpp │ ├── initial_ex_rotation.h │ ├── initial_sfm.cpp │ ├── initial_sfm.h │ ├── solve_5pts.cpp │ └── solve_5pts.h │ ├── parameters.cpp │ ├── parameters.h │ └── utility │ ├── CameraPoseVisualization.cpp │ ├── CameraPoseVisualization.h │ ├── tic_toc.h │ ├── utility.cpp │ ├── utility.h │ ├── visualization.cpp │ └── visualization.h ├── rpvio_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 └── scripts ├── convert_vins_to_tum.py ├── run_advio_12.sh ├── run_ol_market1.sh └── run_rpvio_sim.sh /camera_model/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(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 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(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(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(Calibration ${Boost_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES}) 67 | target_link_libraries(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 | camera_model 4 | 0.0.0 5 | The camera_model package 6 | 7 | 8 | 9 | 10 | lionel 11 | 12 | 13 | 14 | 15 | 16 | GPLv3 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 | 55 | -------------------------------------------------------------------------------- /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/advio_12_config.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #common parameters 4 | imu_topic: "/imu0" 5 | image_topic: "/cam0/image_raw" 6 | mask_topic: "/cam0/mask" 7 | output_path: "~/output/" 8 | 9 | #camera calibration 10 | model_type: PINHOLE 11 | camera_name: camera 12 | image_width: 720 13 | image_height: 1280 14 | distortion_parameters: 15 | k1: 0.0478 16 | k2: 0.0339 17 | p1: -0.00033 18 | p2: -0.00091 19 | projection_parameters: 20 | fx: 1077.2 21 | fy: 1079.3 22 | cx: 362.14 23 | cy: 636.39 24 | 25 | estimate_extrinsic: 0 26 | #Rotation from camera frame to imu frame, imu^R_cam 27 | extrinsicRotation: !!opencv-matrix 28 | rows: 3 29 | cols: 3 30 | dt: d 31 | data: [0.9999763379093255, -0.004079205042965442, -0.005539287650170447, -0.004066386342107199, -0.9999890330121858, 0.0023234365646622014, -0.00554870467502187, -0.0023008567036498766, -0.9999819588046867] 32 | 33 | #Translation from camera frame to imu frame, imu^T_cam 34 | extrinsicTranslation: !!opencv-matrix 35 | rows: 3 36 | cols: 1 37 | dt: d 38 | data: [-0.008977668364731128, 0.07557012320238939, -0.005545773942541918] 39 | 40 | #feature traker parameters 41 | max_cnt: 250 # max feature number in feature tracking 42 | min_dist: 30 # min distance between two features 43 | 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 44 | H_threshold: 1.0 # ransac threshold (pixel) 45 | show_track: 1 # publish tracking image as topic 46 | equalize: 1 # if image is too dark or light, trun on equalize to find enough features 47 | fisheye: 0 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points 48 | 49 | #optimization parameters 50 | max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time 51 | max_num_iterations: 8 # max solver itrations, to guarantee real time 52 | keyframe_parallax: 10.0 # keyframe selection threshold (pixel) 53 | 54 | #discrete-time imu noise parameters (as provided in dataset) 55 | acc_n: 0.1517 # accelerometer measurement noise standard deviation. 56 | gyr_n: 0.0758 # gyroscope measurement noise standard deviation. 57 | acc_w: 6.6407e-6 # accelerometer bias random work noise standard deviation. 58 | gyr_w: 1.6127e-6 # gyroscope bias random work noise standard deviation. 59 | g_norm: 9.8067 # gravity magnitude 60 | 61 | #unsynchronization parameters 62 | estimate_td: 0 63 | td: 0.0 # initial value of time offset. unit s. readed image clock + td = real image clock (IMU clock) 64 | 65 | #rolling shutter parameters 66 | rolling_shutter: 0 # 0: global shutter camera, 1: rolling shutter camera 67 | rolling_shutter_tr: 0 # unit: s. rolling shutter read out time per frame (from data sheet). 68 | 69 | #visualization parameters 70 | save_image: 1 # save image in pose graph for visualization prupose; you can close this function by setting 0 71 | visualize_imu_forward: 0 # output imu forward propogation to achieve low latency and high frequence results 72 | visualize_camera_size: 0.4 # size of camera marker in RVIZ 73 | -------------------------------------------------------------------------------- /config/ol_market1_config.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #common parameters 4 | imu_topic: "/d400/imu0" 5 | image_topic: "/d400/color/image_raw" 6 | mask_topic: "/planes/segments_crf" 7 | output_path: "~/market1" 8 | 9 | #camera calibration 10 | model_type: PINHOLE 11 | camera_name: camera 12 | image_width: 848 13 | image_height: 480 14 | distortion_parameters: 15 | k1: 0.0 16 | k2: 0.0 17 | p1: 0.0 18 | p2: 0.0 19 | projection_parameters: 20 | fx: 616.802734375 21 | fy: 616.7510375976562 22 | cx: 435.0341796875 23 | cy: 242.90113830566406 24 | 25 | estimate_extrinsic: 0 26 | #Rotation from camera frame to imu frame, imu^R_cam 27 | extrinsicRotation: !!opencv-matrix 28 | rows: 3 29 | cols: 3 30 | dt: d 31 | data: [9.99946615e-01, 5.27582295e-03, 8.88440156e-03, 32 | -5.28478975e-03, 9.99985549e-01, 9.86100143e-04, 33 | -8.87907068e-03, -1.03299969e-03, 9.99960047e-01] 34 | #Translation from camera frame to imu frame, imu^T_cam 35 | extrinsicTranslation: !!opencv-matrix 36 | rows: 3 37 | cols: 1 38 | dt: d 39 | data: [ 0.020096886903,-0.00507855368778,-0.0115051260218] 40 | 41 | #feature traker paprameters 42 | max_cnt: 250 # max feature number in feature tracking 43 | min_dist: 30 # min distance between two features 44 | 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 45 | H_threshold: 1.0 # ransac threshold (pixel) 46 | show_track: 1 # publish tracking image as topic 47 | equalize: 1 # if image is too dark or light, trun on equalize to find enough features 48 | fisheye: 0 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points 49 | 50 | #optimization parameters 51 | max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time 52 | max_num_iterations: 8 # max solver itrations, to guarantee real time 53 | keyframe_parallax: 10.0 # keyframe selection threshold (pixel) 54 | 55 | #discrete-time imu noise parameters (as provided in dataset) 56 | acc_n: 0.0163 # accelerometer measurement noise standard deviation. 57 | gyr_n: 0.0032 # gyroscope measurement noise standard deviation. 58 | acc_w: 0.0049 # accelerometer bias random work noise standard deviation. 59 | gyr_w: 0.0004 # gyroscope bias random work noise standard deviation. 60 | g_norm: 9.81007 # gravity magnitude 61 | 62 | #unsynchronization parameters 63 | estimate_td: 0 64 | td: 0.0 # initial value of time offset. unit s. readed image clock + td = real image clock (IMU clock) 65 | 66 | #rolling shutter parameters 67 | rolling_shutter: 0 # 0: global shutter camera, 1: rolling shutter camera 68 | rolling_shutter_tr: 0 # unit: s. rolling shutter read out time per frame (from data sheet). 69 | 70 | #visualization parameters 71 | save_image: 1 # save image in pose graph for visualization prupose; you can close this function by setting 0 72 | visualize_imu_forward: 0 # output imu forward propogation to achieve low latency and high frequence results 73 | visualize_camera_size: 0.4 # size of camera marker in RVIZ 74 | -------------------------------------------------------------------------------- /config/rpvio_sim_config.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #common parameters 4 | imu_topic: "/imu_throttled" 5 | image_topic: "/image" 6 | mask_topic: "/mask" 7 | output_path: "~/output/" 8 | 9 | #camera calibration 10 | model_type: PINHOLE 11 | camera_name: camera 12 | image_width: 640 13 | image_height: 480 14 | distortion_parameters: 15 | k1: 0 16 | k2: 0 17 | p1: 0 18 | p2: 0 19 | projection_parameters: 20 | fx: 320 21 | fy: 320 22 | cx: 320 23 | cy: 240 24 | 25 | estimate_extrinsic: 0 26 | #Rotation from camera frame to imu frame, imu^R_cam 27 | extrinsicRotation: !!opencv-matrix 28 | rows: 3 29 | cols: 3 30 | dt: d 31 | data: [0,0,1,1,0,0,0,1,0] 32 | 33 | #Translation from camera frame to imu frame, imu^T_cam 34 | extrinsicTranslation: !!opencv-matrix 35 | rows: 3 36 | cols: 1 37 | dt: d 38 | data: [0.50, 0, 0] 39 | 40 | #feature traker parameters 41 | max_cnt: 250 # max feature number in feature tracking 42 | min_dist: 30 # min distance between two features 43 | 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 44 | H_threshold: 1.0 # ransac threshold (pixel) 45 | show_track: 1 # publish tracking image as topic 46 | equalize: 1 # if image is too dark or light, trun on equalize to find enough features 47 | fisheye: 0 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points 48 | 49 | #optimization parameters 50 | max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time 51 | max_num_iterations: 8 # max solver itrations, to guarantee real time 52 | keyframe_parallax: 10.0 # keyframe selection threshold (pixel) 53 | 54 | #discrete-time imu noise parameters 55 | acc_n: 0.074427240 # accelerometer measurement noise standard deviation. 56 | gyr_n: 0.002759607 # gyroscope measurement noise standard deviation. 57 | acc_w: 3.9471004e-7 # accelerometer bias random work noise standard deviation. 58 | gyr_w: 3.1538983e-8 # gyroscope bias random work noise standard deviation. 59 | g_norm: 9.80665 # gravity magnitude 60 | 61 | #unsynchronization parameters 62 | estimate_td: 0 63 | td: -0.03079132514112524 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock) 64 | 65 | #rolling shutter parameters 66 | rolling_shutter: 0 # 0: global shutter camera, 1: rolling shutter camera 67 | rolling_shutter_tr: 0 # unit: s. rolling shutter read out time per frame (from data sheet). 68 | 69 | #visualization parameters 70 | save_image: 1 # save image in pose graph for visualization prupose; you can close this function by setting 0 71 | visualize_imu_forward: 0 # output imu forward propogation to achieve low latency and high frequence results 72 | visualize_camera_size: 0.4 # size of camera marker in RVIZ 73 | -------------------------------------------------------------------------------- /plane_segmentation/crf_inference.py: -------------------------------------------------------------------------------- 1 | # Author: Sudarshan 2 | 3 | import sys 4 | import numpy as np 5 | import pydensecrf.densecrf as dcrf 6 | import matplotlib.pyplot as plt 7 | import matplotlib.image as mpimg 8 | import cv2 9 | 10 | import os 11 | import argparse 12 | 13 | try: 14 | from cv2 import imread, imwrite 15 | except ImportError: 16 | 17 | 18 | from skimage.io import imread, imsave 19 | imwrite = imsave 20 | 21 | 22 | from pydensecrf.utils import unary_from_labels, create_pairwise_bilateral, create_pairwise_gaussian 23 | 24 | 25 | def CRF_act( fn_im ,fn_anno , fn_output , fn_output2 ): 26 | 27 | anno_rgb = imread(fn_anno).astype(np.uint8) 28 | anno_lbl = anno_rgb[:,:,0] + (anno_rgb[:,:,1] << 8) + (anno_rgb[:,:,2] << 16) 29 | 30 | colors_not_used, labels = np.unique(anno_lbl, return_inverse=True) 31 | 32 | img = imread(fn_im) 33 | img = cv2.resize(img , (320,192), interpolation =cv2.INTER_AREA) 34 | 35 | n_labels = 4 36 | 37 | colorize = np.empty((len(colors_not_used), 3), np.uint8) 38 | colorize[:,0] = (colors_not_used & 0x0000FF) 39 | colorize[:,1] = (colors_not_used & 0x0000FF) >> 8 40 | colorize[:,2] = (colors_not_used & 0x0000FF) >> 8 41 | 42 | 43 | d = dcrf.DenseCRF2D(img.shape[1], img.shape[0], n_labels) 44 | 45 | 46 | U = unary_from_labels(labels, n_labels, gt_prob=0.8, zero_unsure=False) 47 | 48 | d.setUnaryEnergy(U) 49 | 50 | 51 | feats = create_pairwise_gaussian(sdims=(5, 5), shape=img.shape[:2]) 52 | d.addPairwiseEnergy(feats, compat=2, 53 | kernel=dcrf.DIAG_KERNEL, 54 | normalization=dcrf.NORMALIZE_SYMMETRIC) 55 | 56 | feats = create_pairwise_bilateral(sdims=(100, 100), schan=(13, 13, 13), 57 | img=img, chdim=2) 58 | 59 | d.addPairwiseEnergy(feats, compat=7, 60 | kernel=dcrf.DIAG_KERNEL, 61 | normalization=dcrf.NORMALIZE_SYMMETRIC) 62 | 63 | 64 | Q, tmp1, tmp2 = d.startInference() 65 | for i in range(6): 66 | print("KL-divergence at {}: {}".format(i, d.klDivergence(Q))) 67 | d.stepInference(Q, tmp1, tmp2) 68 | MAP = np.argmax(Q, axis=0) 69 | MAP = colorize[MAP,:] 70 | 71 | MAP = MAP.reshape(img.shape) 72 | MAP = cv2.morphologyEx(MAP, cv2.MORPH_CLOSE, kernel) 73 | 74 | print(np.shape(MAP)) 75 | imwrite(fn_output, MAP) 76 | 77 | for rows in range(np.shape(MAP)[0]): 78 | for cols in range(np.shape(MAP)[1]): 79 | 80 | if (MAP[rows][cols][0] > 0): 81 | MAP[rows][cols] =255 82 | 83 | if (MAP[rows][cols][0] == 0): 84 | MAP[rows][cols] =0 85 | 86 | imwrite(fn_output2, MAP) 87 | #imwrite(fn_output, MAP.reshape(img.shape)) 88 | 89 | 90 | kernel = (10,10) 91 | 92 | parser = argparse.ArgumentParser() 93 | parser.add_argument('rgb_image_dir', help='path to original rgb images') 94 | parser.add_argument('labels_dir', help='path to plane_sgmts_modified output dir from planerecover') 95 | parser.add_argument('output_dir', help='path to output refined masks') 96 | 97 | args = parser.parse_args() 98 | 99 | RGB_image_dir = args.rgb_image_dir 100 | labels_dir = args.labels_dir 101 | output_dir = args.output_dir 102 | output_dir2= "/home/sudarshan/planerecover/CRF/Results_advio_single" 103 | 104 | images = sorted(os.listdir(RGB_image_dir)) 105 | labels = sorted(os.listdir(labels_dir)) 106 | 107 | k = 0 108 | for i, j in zip(images , labels): 109 | k +=1 110 | 111 | if ( k >8948): 112 | 113 | image_name = i #"img" + str(i) +".jpg" 114 | image_path = os.path.join(RGB_image_dir, image_name) 115 | 116 | labels_name = j #"OpenLORIS_images_img" + str(i) +".png" 117 | labels_path = os.path.join(labels_dir, labels_name) 118 | 119 | output_name = "img" + str(i) + "_crf_res" +".png" 120 | output_path = os.path.join(output_dir, output_name) 121 | output_path2 = os.path.join(output_dir2, output_name) 122 | 123 | 124 | CRF_act(image_path , labels_path, output_path, output_path2) 125 | 126 | 127 | print( "***********" + str(image_name) +"/" + "****** " + str(labels_name) + "*****" ) 128 | -------------------------------------------------------------------------------- /plane_segmentation/data_loader_new.py: -------------------------------------------------------------------------------- 1 | from __future__ import division 2 | import os 3 | import random 4 | import tensorflow as tf 5 | 6 | class DataLoader(object): 7 | def __init__(self, 8 | dataset_dir=None, 9 | batch_size=None, 10 | img_height=None, 11 | img_width=None, 12 | num_scales=None): 13 | self.dataset_dir = dataset_dir 14 | self.batch_size = batch_size 15 | self.img_height = img_height 16 | self.img_width = img_width 17 | self.num_scales = num_scales 18 | 19 | def load_train_batch(self): 20 | """Load a batch of training instances. 21 | """ 22 | seed = random.randint(0, 2**31 - 1) 23 | 24 | # Load the list of training files into queues and shuffle it 25 | file_list = self.format_file_list(self.dataset_dir, 'train_8000_recent_working') 26 | 27 | print("**************" + "LOADED NEW DATA train_8000_recent_working" + "***********") 28 | image_paths_queue = tf.train.string_input_producer( 29 | file_list['image_file_list'], 30 | seed=seed, 31 | shuffle=True) 32 | 33 | cam_paths_queue = tf.train.string_input_producer( 34 | file_list['cam_file_list'], 35 | seed=seed, 36 | shuffle=True) 37 | 38 | depth_paths_queue = tf.train.string_input_producer( 39 | file_list['depth_file_list'], 40 | seed=seed, 41 | shuffle=True) 42 | 43 | label_paths_queue = tf.train.string_input_producer( 44 | file_list['label_file_list'], 45 | seed=seed, 46 | shuffle=True) 47 | 48 | self.steps_per_epoch = int( 49 | len(file_list['image_file_list'])//self.batch_size) 50 | 51 | # Load images 52 | img_reader = tf.WholeFileReader() 53 | _, image_contents = img_reader.read(image_paths_queue) 54 | image_seq = tf.image.decode_image(image_contents, channels=3) 55 | # 56 | tgt_image = tf.reshape(image_seq, [self.img_height, self.img_width, 3]) 57 | #tgt_image= tf.cast(tgt_image, tf.float32) 58 | #print( " **** images loaded ******************") 59 | 60 | # Load labels 61 | label_reader = tf.WholeFileReader() 62 | _, label_contents = label_reader.read(label_paths_queue) 63 | label_seq = tf.image.decode_png(label_contents) 64 | tgt_label = tf.reshape(label_seq, [self.img_height, self.img_width, 1]) 65 | 66 | print(" CHECKS PASSED ") 67 | 68 | # Load depths 69 | depth_reader = tf.WholeFileReader() 70 | _, depth_contents = depth_reader.read(depth_paths_queue) 71 | # image_seq = tf.image.decode_jpeg(image_contents) 72 | tgt_image_detph = tf.image.decode_png(depth_contents,dtype=tf.uint16 ,channels=1)[:,:, 0] 73 | tgt_detph = tf.cast(tgt_image_detph, dtype=tf.float32) 74 | tgt_detph = tf.reshape(tgt_detph, [ self.img_height, self.img_width, 1]) \ 75 | / tf.constant(100., dtype=tf.float32,shape=[self.img_height, self.img_width, 1]) 76 | 77 | # Load camera intrinsics 78 | print("------------------- intrininsics inncluded ----------" ) 79 | cam_reader = tf.TextLineReader() 80 | _, raw_cam_contents = cam_reader.read(cam_paths_queue) 81 | rec_def = [] 82 | for i in range(9): 83 | rec_def.append([1.]) 84 | raw_cam_vec = tf.decode_csv(raw_cam_contents,record_defaults=rec_def) 85 | raw_cam_vec = tf.stack(raw_cam_vec) 86 | intrinsics = tf.reshape(raw_cam_vec, [3, 3]) 87 | #intrinsics = [ [1169.621094 , 0.000000, 646.295044] , [ 0.000000, 1167.105103 ,489.927032 ] , [ 0.000000 ,0.000000, 1.000000] ] 88 | 89 | # Form training batches 90 | tgt_image, tgt_detph, tgt_label, intrinsics = \ 91 | tf.train.batch([tgt_image, tgt_detph,tgt_label, intrinsics], 92 | batch_size=self.batch_size) #it will upload 4 batch from the dataset 93 | 94 | # Data augmentation 95 | tgt_image, tgt_detph, tgt_label, intrinsics = self.data_augmentation( 96 | tgt_image, tgt_detph, tgt_label,intrinsics, self.img_height, self.img_width) 97 | 98 | intrinsics = self.get_multi_scale_intrinsics( 99 | intrinsics, self.num_scales) 100 | 101 | return tgt_image, tgt_detph, tgt_label, intrinsics 102 | 103 | def make_intrinsics_matrix(self, fx, fy, cx, cy): 104 | # Assumes batch input 105 | batch_size = fx.get_shape().as_list()[0] 106 | zeros = tf.zeros_like(fx) 107 | r1 = tf.stack([fx, zeros, cx], axis=1) 108 | r2 = tf.stack([zeros, fy, cy], axis=1) 109 | r3 = tf.constant([0.,0.,1.], shape=[1, 3]) 110 | r3 = tf.tile(r3, [batch_size, 1]) 111 | intrinsics = tf.stack([r1, r2, r3], axis=1) 112 | return intrinsics 113 | 114 | def data_augmentation(self, im, depth, label, intrinsics, out_h, out_w): 115 | # Random scaling 116 | def random_scaling(im, depth, label, intrinsics): 117 | batch_size, in_h, in_w, _ = im.get_shape().as_list() 118 | scaling = tf.random_uniform([2], 1, 1.15) 119 | x_scaling = scaling[0] 120 | y_scaling = scaling[1] 121 | out_h = tf.cast(in_h * y_scaling, dtype=tf.int32) 122 | out_w = tf.cast(in_w * x_scaling, dtype=tf.int32) 123 | im = tf.image.resize_area(im, [out_h, out_w]) 124 | depth = tf.image.resize_area(depth, [out_h, out_w]) 125 | label = tf.image.resize_area(label, [out_h, out_w]) 126 | fx = intrinsics[:,0,0] * x_scaling 127 | fy = intrinsics[:,1,1] * y_scaling 128 | cx = intrinsics[:,0,2] * x_scaling 129 | cy = intrinsics[:,1,2] * y_scaling 130 | intrinsics = self.make_intrinsics_matrix(fx, fy, cx, cy) 131 | return im, depth, label, intrinsics 132 | 133 | # Random cropping 134 | def random_cropping(im, depth, label, intrinsics, out_h, out_w): 135 | # batch_size, in_h, in_w, _ = im.get_shape().as_list() 136 | batch_size, in_h, in_w, _ = tf.unstack(tf.shape(im)) 137 | offset_y = tf.random_uniform([1], 0, in_h - out_h + 1, dtype=tf.int32)[0] #the scale of in_h and out_h can be different 138 | offset_x = tf.random_uniform([1], 0, in_w - out_w + 1, dtype=tf.int32)[0] # because of the scaling process runs before it 139 | im = tf.image.crop_to_bounding_box(im, offset_y, offset_x, out_h, out_w) 140 | depth = tf.image.crop_to_bounding_box(depth, offset_y, offset_x, out_h, out_w) 141 | label = tf.image.crop_to_bounding_box(label, offset_y, offset_x, out_h, out_w) 142 | fx = intrinsics[:,0,0] 143 | fy = intrinsics[:,1,1] 144 | cx = intrinsics[:,0,2] - tf.cast(offset_x, dtype=tf.float32) 145 | cy = intrinsics[:,1,2] - tf.cast(offset_y, dtype=tf.float32) 146 | intrinsics = self.make_intrinsics_matrix(fx, fy, cx, cy) 147 | return im, depth, label, intrinsics 148 | im, depth, label, intrinsics = random_scaling(im, depth,label, intrinsics) 149 | im, depth, label, intrinsics = random_cropping(im, depth, label, intrinsics, out_h, out_w) 150 | im = tf.cast(im, dtype=tf.uint8) 151 | return im,depth,label, intrinsics 152 | 153 | def format_file_list(self, data_root, split): 154 | with open(data_root + '/%s.txt' % split, 'r') as f: 155 | frames = f.readlines() 156 | subfolders = [x.split(' ')[0] for x in frames] 157 | frame_ids = [x.split(' ')[1][:-1] for x in frames] 158 | image_file_list = [os.path.join(data_root, subfolders[i], 159 | frame_ids[i] + '.jpg') for i in range(len(frames))] 160 | #print(image_file_list) 161 | 162 | cam_file_list = [os.path.join(data_root, subfolders[i], 163 | frame_ids[i] + '_cam.txt') for i in range(len(frames))] 164 | 165 | depth_file_list = [os.path.join(data_root, subfolders[i], 166 | frame_ids[i] + '_depth.png') for i in range(len(frames))] 167 | 168 | label_file_list = [os.path.join(data_root, subfolders[i], 169 | frame_ids[i] + '_label.png') for i in range(len(frames))] 170 | 171 | all_list = {} 172 | all_list['image_file_list'] = image_file_list 173 | all_list['cam_file_list'] = cam_file_list 174 | all_list['depth_file_list'] = depth_file_list 175 | all_list['label_file_list']= label_file_list 176 | return all_list 177 | 178 | 179 | def get_multi_scale_intrinsics(self, intrinsics, num_scales): 180 | intrinsics_mscale = [] 181 | # Scale the intrinsics accordingly for each scale 182 | for s in range(num_scales): 183 | fx = intrinsics[:,0,0]/(2 ** s) 184 | fy = intrinsics[:,1,1]/(2 ** s) 185 | cx = intrinsics[:,0,2]/(2 ** s) 186 | cy = intrinsics[:,1,2]/(2 ** s) 187 | intrinsics_mscale.append( 188 | self.make_intrinsics_matrix(fx, fy, cx, cy)) 189 | intrinsics_mscale = tf.stack(intrinsics_mscale, axis=1) 190 | return intrinsics_mscale 191 | -------------------------------------------------------------------------------- /plane_segmentation/net.py: -------------------------------------------------------------------------------- 1 | from __future__ import division 2 | import tensorflow as tf 3 | import tensorflow.contrib.slim as slim 4 | from tensorflow.contrib.layers.python.layers import utils 5 | import numpy as np 6 | SCALING = 1.0 7 | BIAS = 0. 8 | 9 | def resize_like(inputs, ref): 10 | iH, iW = inputs.get_shape()[1], inputs.get_shape()[2] 11 | rH, rW = ref.get_shape()[1], ref.get_shape()[2] 12 | if iH == rH and iW == rW: 13 | return inputs 14 | return tf.image.resize_nearest_neighbor(inputs, [rH.value, rW.value]) 15 | 16 | 17 | def plane_pred_net(tgt_image, num_plane, is_training=True): 18 | H = tgt_image.get_shape()[1].value 19 | W = tgt_image.get_shape()[2].value 20 | with tf.variable_scope('depth_net') as sc: # design the nn architecture for the depth network 21 | end_points_collection = sc.original_name_scope + '_end_points' 22 | with slim.arg_scope([slim.conv2d, slim.conv2d_transpose], #define a conv2d operator with fixed params shown below 23 | normalizer_fn=None, 24 | weights_regularizer=slim.l2_regularizer(0.05), # using l2 regularizer with 0.05 weight 25 | activation_fn=tf.nn.relu, 26 | outputs_collections=end_points_collection): 27 | 28 | #for slim.conv2d the default padding mode = 'same' 29 | cnv1 = slim.conv2d(tgt_image, 32, [7, 7], stride=2, scope='cnv1') #4*96*160*32 30 | cnv1b = slim.conv2d(cnv1, 32, [7, 7], stride=1, scope='cnv1b') 31 | cnv2 = slim.conv2d(cnv1b, 64, [5, 5], stride=2, scope='cnv2') #4*48*80*64 32 | cnv2b = slim.conv2d(cnv2, 64, [5, 5], stride=1, scope='cnv2b') 33 | cnv3 = slim.conv2d(cnv2b, 128, [3, 3], stride=2, scope='cnv3') #4*24*40*128 34 | cnv3b = slim.conv2d(cnv3, 128, [3, 3], stride=1, scope='cnv3b') 35 | cnv4 = slim.conv2d(cnv3b, 256, [3, 3], stride=2, scope='cnv4') #4*12*20*256 36 | cnv4b = slim.conv2d(cnv4, 256, [3, 3], stride=1, scope='cnv4b') 37 | cnv5 = slim.conv2d(cnv4b, 512, [3, 3], stride=2, scope='cnv5') 38 | cnv5b = slim.conv2d(cnv5, 512, [3, 3], stride=1, scope='cnv5b') # 4*6*10*256 39 | 40 | with tf.variable_scope('param'): 41 | cnv6_plane = slim.conv2d(cnv5b, 512, [3, 3], stride=2, scope='cnv6_plane') # 4*3*5*256 42 | cnv7_plane = slim.conv2d(cnv6_plane, 512, [3, 3], stride=2, scope='cnv7_plane') # 4*2*3*256 43 | param_pred = slim.conv2d(cnv7_plane, 3*(num_plane), [1, 1], scope='param', # 4*2*3*3n 44 | stride=1, normalizer_fn=None, activation_fn=None) 45 | param_avg = tf.reduce_mean(param_pred, [1, 2]) #4*3n 46 | # Empirically we found that scaling by a small constant facilitates training. 47 | param_final = 0.01 * tf.reshape(param_avg, [-1, (num_plane), 3]) #4*n*3, 2 for n planes in tgt, B*n*num_param 48 | 49 | with tf.variable_scope('mask'): 50 | upcnv5 = slim.conv2d_transpose(cnv5b, 256, [3, 3], stride=2, scope='upcnv5') 51 | i5_in = tf.concat([upcnv5, cnv4b], axis=3) 52 | icnv5 = slim.conv2d(i5_in, 256, [3, 3], stride=1, scope='icnv5') # 4*12*20*256 53 | 54 | upcnv4 = slim.conv2d_transpose(icnv5, 128, [3, 3], stride=2, scope='upcnv4') # 4*24*40*128 55 | i4_in = tf.concat([upcnv4, cnv3b], axis=3) 56 | icnv4 = slim.conv2d(i4_in, 128, [3, 3], stride=1, scope='icnv4') 57 | segm4 = SCALING * slim.conv2d(icnv4, num_plane + 1, [3, 3], stride=1, 58 | activation_fn=None, normalizer_fn=None, scope='disp4') + BIAS # 4*24*40*(1+n) 59 | segm4_up = tf.image.resize_bilinear(segm4, [np.int(H/4), np.int(W/4)]) 60 | 61 | upcnv3 = slim.conv2d_transpose(icnv4, 64, [3, 3], stride=2, scope='upcnv3') # 4*48*80*64 62 | i3_in = tf.concat([upcnv3, cnv2b, segm4_up], axis=3) 63 | icnv3 = slim.conv2d(i3_in, 64, [3, 3], stride=1, scope='icnv3') 64 | segm3 = SCALING * slim.conv2d(icnv3, num_plane + 1, [3, 3], stride=1, #4*48*80*(1+n) 65 | activation_fn=None, normalizer_fn=None, scope='disp3') + BIAS 66 | segm3_up = tf.image.resize_bilinear(segm3, [np.int(H/2), np.int(W/2)]) 67 | 68 | upcnv2 = slim.conv2d_transpose(icnv3, 32, [3, 3], stride=2, scope='upcnv2') # 4*96*160*32 69 | i2_in = tf.concat([upcnv2, cnv1b, segm3_up], axis=3) 70 | icnv2 = slim.conv2d(i2_in, 32, [3, 3], stride=1, scope='icnv2') 71 | segm2 = SCALING * slim.conv2d(icnv2, num_plane + 1, [3, 3], stride=1, #4*96*160*(n+1) 72 | activation_fn=None, normalizer_fn=None, scope='disp2') + BIAS 73 | segm2_up = tf.image.resize_bilinear(segm2, [H, W]) 74 | 75 | upcnv1 = slim.conv2d_transpose(icnv2, 16, [3, 3], stride=2, scope='upcnv1') #4*192*320*16 76 | i1_in = tf.concat([upcnv1, segm2_up], axis=3) 77 | icnv1 = slim.conv2d(i1_in, 16, [3, 3], stride=1, scope='icnv1') #4*192*320*(n+1) 78 | segm1 = SCALING * slim.conv2d(icnv1, num_plane + 1, [3, 3], stride=1, 79 | activation_fn=None, normalizer_fn=None, scope='disp1') + BIAS 80 | 81 | end_points = utils.convert_collection_to_dict(end_points_collection) 82 | return param_final, [segm1, segm2, segm3, segm4], end_points 83 | -------------------------------------------------------------------------------- /plane_segmentation/pretrained_model/model.data-00000-of-00001: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/karnikram/rp-vio/42a0ac1c65417c7d6c778d6a262c03dbc1b52316/plane_segmentation/pretrained_model/model.data-00000-of-00001 -------------------------------------------------------------------------------- /plane_segmentation/pretrained_model/model.index: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/karnikram/rp-vio/42a0ac1c65417c7d6c778d6a262c03dbc1b52316/plane_segmentation/pretrained_model/model.index -------------------------------------------------------------------------------- /plane_segmentation/pretrained_model/model.meta: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/karnikram/rp-vio/42a0ac1c65417c7d6c778d6a262c03dbc1b52316/plane_segmentation/pretrained_model/model.meta -------------------------------------------------------------------------------- /plane_segmentation/requirements.txt: -------------------------------------------------------------------------------- 1 | # This file may be used to create an environment using: 2 | # $ conda create --name --file 3 | # platform: linux-64 4 | _libgcc_mutex=0.1=conda_forge 5 | _openmp_mutex=4.5=1_gnu 6 | addict=2.3.0=py36h9f0ad1d_2 7 | async_generator=1.10=py_0 8 | attrs=20.3.0=pyhd3eb1b0_0 9 | backcall=0.2.0=py_0 10 | blas=2.14=openblas 11 | bleach=1.5.0=py36_0 12 | bzip2=1.0.8=h7f98852_4 13 | c-ares=1.17.1=h36c2ea0_0 14 | ca-certificates=2020.10.14=0 15 | cairo=1.16.0=h9f066cc_1006 16 | certifi=2020.6.20=py36_0 17 | cycler=0.10.0=py_2 18 | cython=0.29.21=py36he6710b0_0 19 | dbus=1.13.6=hfdff14a_1 20 | decorator=4.4.2=py_0 21 | defusedxml=0.6.0=py_0 22 | entrypoints=0.3=py36_0 23 | expat=2.2.9=he1b5a44_2 24 | ffmpeg=4.3.1=h3215721_1 25 | fontconfig=2.13.1=h7e3eb15_1002 26 | freetype=2.10.4=h7ca028e_0 27 | gettext=0.19.8.1=hf34092f_1004 28 | glib=2.66.3=h58526e2_0 29 | gmp=6.2.1=h58526e2_0 30 | gnutls=3.6.13=h85f3911_1 31 | graphite2=1.3.13=h58526e2_1001 32 | gst-plugins-base=1.14.5=h0935bb2_2 33 | gstreamer=1.14.5=h36ae1b5_2 34 | harfbuzz=2.7.2=ha5b49bf_1 35 | hdf5=1.10.6=nompi_h3c11f04_101 36 | html5lib=0.9999999=py36_0 37 | icu=67.1=he1b5a44_0 38 | importlib-metadata=3.3.0=py36h5fab9bb_2 39 | intel-openmp=2020.2=254 40 | ipykernel=5.3.4=py36h5ca1d4c_0 41 | ipython=7.16.1=py36h5ca1d4c_0 42 | ipython_genutils=0.2.0=pyhd3eb1b0_1 43 | ipywidgets=7.6.0=pyhd3eb1b0_1 44 | jasper=1.900.1=h07fcdf6_1006 45 | jedi=0.18.0=py36h06a4308_0 46 | jinja2=2.11.2=py_0 47 | joblib=1.0.0=pyhd8ed1ab_0 48 | jpeg=9d=h36c2ea0_0 49 | jsonschema=3.0.2=py36_0 50 | jupyter_client=6.1.7=py_0 51 | jupyter_core=4.7.0=py36h06a4308_0 52 | jupyterlab_pygments=0.1.2=py_0 53 | kiwisolver=1.3.1=py36h51d7077_0 54 | krb5=1.17.2=h926e7f8_0 55 | lame=3.100=h14c3975_1001 56 | lcms2=2.11=hcbb858e_1 57 | ld_impl_linux-64=2.33.1=h53a641e_7 58 | libblas=3.8.0=14_openblas 59 | libcblas=3.8.0=14_openblas 60 | libclang=10.0.1=default_hde54327_1 61 | libcurl=7.71.1=hcdd3856_8 62 | libedit=3.1.20191231=h14c3975_1 63 | libev=4.33=h516909a_1 64 | libevent=2.1.10=hcdb4288_3 65 | libffi=3.2.1=he1b5a44_1007 66 | libgcc-ng=9.3.0=h5dbcf3e_17 67 | libgfortran-ng=7.3.0=hdf63c60_0 68 | libglib=2.66.3=hbe7bbb4_0 69 | libgomp=9.3.0=h5dbcf3e_17 70 | libiconv=1.16=h516909a_0 71 | liblapack=3.8.0=14_openblas 72 | liblapacke=3.8.0=14_openblas 73 | libllvm10=10.0.1=he513fc3_3 74 | libnghttp2=1.41.0=h8cfc5f6_2 75 | libopenblas=0.3.7=h5ec1e0e_6 76 | libopencv=4.5.0=py36_4 77 | libpng=1.6.37=h21135ba_2 78 | libpq=12.3=h255efa7_3 79 | libprotobuf=3.13.0.1=h8b12597_0 80 | libsodium=1.0.18=h7b6447c_0 81 | libssh2=1.9.0=hab1572f_5 82 | libstdcxx-ng=9.3.0=h2ae2ef3_17 83 | libtiff=4.1.0=h4f3a223_6 84 | libuuid=2.32.1=h7f98852_1000 85 | libwebp-base=1.1.0=h36c2ea0_3 86 | libxcb=1.13=h14c3975_1002 87 | libxkbcommon=0.10.0=he1b5a44_0 88 | libxml2=2.9.10=h68273f3_2 89 | llvmlite=0.34.0=py36h269e1b5_4 90 | lz4-c=1.9.2=he1b5a44_3 91 | markdown=3.3.3=pyh9f0ad1d_0 92 | markupsafe=1.1.1=py36h7b6447c_0 93 | matplotlib=3.3.3=py36h5fab9bb_0 94 | matplotlib-base=3.3.3=py36he12231b_0 95 | mistune=0.8.4=py36h7b6447c_0 96 | mkl=2019.4=243 97 | mkl-service=2.3.0=py36he8ac12f_0 98 | mkl_fft=1.1.0=py36hc1659b7_1 99 | mkl_random=1.1.0=py36hb3f55d8_0 100 | mysql-common=8.0.21=2 101 | mysql-libs=8.0.21=hf3661c5_2 102 | nbclient=0.5.1=py_0 103 | nbconvert=6.0.7=py36_0 104 | nbformat=5.0.8=py_0 105 | ncurses=6.2=he6710b0_1 106 | nest-asyncio=1.4.3=pyhd3eb1b0_0 107 | nettle=3.6=he412f7d_0 108 | notebook=6.0.3=py36_0 109 | nspr=4.29=he1b5a44_1 110 | nss=3.59=h2c00c37_0 111 | numba=0.51.2=py36h0573a6f_1 112 | numpy=1.17.0=py36h95a1406_0 113 | numpy-base=1.18.5=py36h2f8d375_0 114 | olefile=0.46=pyh9f0ad1d_1 115 | open3d=0.11.2=py36_0 116 | opencv=4.5.0=py36_4 117 | openh264=2.1.1=h8b12597_0 118 | openssl=1.1.1h=h7b6447c_0 119 | pandas=1.1.3=py36he6710b0_0 120 | pandoc=2.11=hb0f4dca_0 121 | pandocfilters=1.4.3=py36h06a4308_1 122 | parso=0.7.0=py_0 123 | pcre=8.44=he1b5a44_0 124 | pexpect=4.8.0=pyhd3eb1b0_3 125 | pickleshare=0.7.5=pyhd3eb1b0_1003 126 | pillow=8.0.1=py36h10ecd5c_0 127 | pip=20.3.3=py36h06a4308_0 128 | pixman=0.40.0=h36c2ea0_0 129 | plyfile=0.7.2=pyh9f0ad1d_0 130 | prometheus_client=0.9.0=pyhd3eb1b0_0 131 | prompt-toolkit=3.0.8=py_0 132 | protobuf=3.13.0.1=py36h201607c_1 133 | pthread-stubs=0.4=h36c2ea0_1001 134 | ptyprocess=0.6.0=pyhd3eb1b0_2 135 | py-opencv=4.5.0=py36he448a4c_4 136 | pydensecrf=1.0rc2=pypi_0 137 | pygments=2.7.3=pyhd3eb1b0_0 138 | pyparsing=2.4.7=pyh9f0ad1d_0 139 | pyqt=5.12.3=py36h5fab9bb_6 140 | pyqt-impl=5.12.3=py36h7ec31b9_6 141 | pyqt5-sip=4.19.18=py36hc4f0c31_6 142 | pyqtchart=5.12=py36h7ec31b9_6 143 | pyqtwebengine=5.12.1=py36h7ec31b9_6 144 | pyrsistent=0.17.3=py36h7b6447c_0 145 | python=3.6.11=h425cb1d_0_cpython 146 | python-dateutil=2.8.1=py_0 147 | python_abi=3.6=1_cp36m 148 | pytz=2020.1=py_0 149 | pyyaml=5.3.1=py36he6145b8_1 150 | pyzmq=20.0.0=py36h2531618_1 151 | qt=5.12.9=h1f2b2cb_0 152 | readline=8.0=h7b6447c_0 153 | scikit-learn=0.23.2=py36h0573a6f_0 154 | scipy=0.19.1=py36h9976243_3 155 | send2trash=1.5.0=pyhd3eb1b0_1 156 | setuptools=51.0.0=py36h06a4308_2 157 | six=1.15.0=pyh9f0ad1d_0 158 | sqlite=3.33.0=h62c20be_0 159 | tensorflow=1.4.1=0 160 | tensorflow-base=1.4.1=py36hd00c003_2 161 | tensorflow-gpu=1.4.1=0 162 | tensorflow-gpu-base=1.4.1=py36h01caf0a_0 163 | tensorflow-tensorboard=1.5.1=py36hf484d3e_1 164 | terminado=0.9.1=py36_0 165 | testpath=0.4.4=py_0 166 | threadpoolctl=2.1.0=pyh5ca1d4c_0 167 | tk=8.6.10=hbc83047_0 168 | tornado=6.1=py36h1d69622_0 169 | tqdm=4.55.0=pyhd8ed1ab_0 170 | traitlets=4.3.3=py36_0 171 | typing_extensions=3.7.4.3=py_0 172 | wcwidth=0.2.5=py_0 173 | webencodings=0.5.1=py_1 174 | werkzeug=1.0.1=pyh9f0ad1d_0 175 | wheel=0.36.2=pyhd3eb1b0_0 176 | widgetsnbextension=3.5.1=py36_0 177 | x264=1!152.20180806=h14c3975_0 178 | xorg-kbproto=1.0.7=h14c3975_1002 179 | xorg-libice=1.0.10=h516909a_0 180 | xorg-libsm=1.2.3=h84519dc_1000 181 | xorg-libx11=1.6.12=h516909a_0 182 | xorg-libxau=1.0.9=h14c3975_0 183 | xorg-libxdmcp=1.1.3=h516909a_0 184 | xorg-libxext=1.3.4=h516909a_0 185 | xorg-libxrender=0.9.10=h516909a_1002 186 | xorg-renderproto=0.11.1=h14c3975_1002 187 | xorg-xextproto=7.3.0=h14c3975_1002 188 | xorg-xproto=7.0.31=h7f98852_1007 189 | xz=5.2.5=h7b6447c_0 190 | yaml=0.2.5=h7b6447c_0 191 | zeromq=4.3.3=he6710b0_3 192 | zipp=3.4.0=py_0 193 | zlib=1.2.11=h7b6447c_3 194 | zstd=1.4.5=h6597ccf_2 195 | -------------------------------------------------------------------------------- /plane_segmentation/train.py: -------------------------------------------------------------------------------- 1 | from __future__ import division 2 | import tensorflow as tf 3 | import pprint 4 | import random 5 | import numpy as np 6 | 7 | from RecoverPlane_perpendicular import RecoverPlane 8 | 9 | import os 10 | 11 | flags = tf.app.flags 12 | flags.DEFINE_string("dataset_dir", "", "Dataset directory") 13 | flags.DEFINE_string("log_dir2", "", "log directory") 14 | flags.DEFINE_string("init_checkpoint_file",'', "Specific checkpoint file to initialize from") 15 | flags.DEFINE_float("learning_rate", 0.0001, "Learning rate of for adam") 16 | flags.DEFINE_float("beta1", 0.99, "Momentum term of adam") 17 | flags.DEFINE_float("beta2", 0.9999, "Momentum term of adam") 18 | flags.DEFINE_float("plane_weight",0.1, "Weight for plane regularization") 19 | flags.DEFINE_integer("batch_size", 8, "The size of of a sample batch") 20 | flags.DEFINE_integer("img_height", 192, "Image height") 21 | flags.DEFINE_integer("img_width", 320, "Image width") 22 | flags.DEFINE_integer("max_steps",800000 , "Maximum number of training iterations") 23 | flags.DEFINE_integer("summary_freq", 1000, "Logging every log_freq iterations") 24 | flags.DEFINE_integer("num_plane",3, "The estimated number of planes in the scenario") 25 | flags.DEFINE_integer("save_latest_freq", 5000, \ 26 | "Save the latest model every save_latest_freq iterations (overwrites the previous latest model)") 27 | flags.DEFINE_boolean("continue_train", False, "Continue training from previous checkpoint") 28 | flags.DEFINE_boolean("debug", True, "debug mode?") 29 | flags.DEFINE_string("gpu", "1", "GPU ID") 30 | 31 | 32 | check_file = flags.FLAGS.log_dir2 + '/' + flags.FLAGS.dataset_dir.split("/")[-1] +\ 33 | '_lr=' + str(flags.FLAGS.learning_rate) +\ 34 | '_b1=' + str(flags.FLAGS.beta1) + '_b2=' + str(flags.FLAGS.beta2) +\ 35 | '_weight=' + str(flags.FLAGS.plane_weight) +\ 36 | '_n_plane=' + str(flags.FLAGS.num_plane) 37 | 38 | 39 | flags.DEFINE_string("checkpoint_dir", check_file, "Directory name to save the checkpoints") 40 | 41 | FLAGS = flags.FLAGS # this is used to transfer all the params need during the app.run 42 | 43 | def main(_): 44 | seed = 8964 45 | tf.set_random_seed(seed) 46 | np.random.seed(seed) 47 | random.seed(seed) 48 | 49 | pp = pprint.PrettyPrinter() 50 | pp.pprint(flags.FLAGS.__flags) #change this to pp.pprint(tf.app.flags.FLAGS.flag_values_dict()) for tensorflow 1.5 or higher 51 | 52 | if not os.path.exists(FLAGS.checkpoint_dir): 53 | os.mkdir(FLAGS.checkpoint_dir) 54 | 55 | planeRecover = RecoverPlane() 56 | planeRecover.train(FLAGS) 57 | 58 | if __name__ == '__main__': 59 | tf.app.run() 60 | 61 | -------------------------------------------------------------------------------- /plane_segmentation/utils.py: -------------------------------------------------------------------------------- 1 | from __future__ import division 2 | import matplotlib.pyplot as plt 3 | import numpy as np 4 | import tensorflow as tf 5 | import random 6 | import colorsys 7 | 8 | 9 | # ****************************colorful mask part************************************* 10 | # Usage: apply different color to each plane 11 | # the plane determination is based on the plane_threshold = 0.5 now 12 | # and the area without additional color are belong to non-plane 13 | # 14 | def random_colors(N, bright=True): 15 | """ 16 | Generate random colors. 17 | To get visually distinct colors, generate them in HSV space then 18 | convert to RGB. 19 | """ 20 | brightness = 1.0 if bright else 0.7 21 | hsv = [(float(i) / N, 1, brightness) for i in range(N)] 22 | colors = list(map(lambda c: colorsys.hsv_to_rgb(*c), hsv)) 23 | random.shuffle(colors) 24 | return colors 25 | 26 | def apply_mask(image, mask, max_mask, color, alpha=0.4): 27 | """Apply the given mask to the image. 28 | alpha here means the alpha channel value 29 | """ 30 | b,h,w,_ = image.get_shape().as_list() 31 | res_mask = tf.concat([mask, mask, mask], axis=-1) # be consistent with color channel number 32 | ref_max_mask = tf.concat([max_mask,max_mask,max_mask],axis=-1) 33 | b_equal = tf.equal(res_mask, ref_max_mask) 34 | 35 | alpha_img = tf.where(b_equal, tf.cast(tf.scalar_mul(1 - alpha, tf.cast(image, tf.float32)), 36 | tf.uint8), image) 37 | 38 | alpha_color = alpha * np.array(color) 39 | ref_color = np.tile(alpha_color.astype(int), (b, h, w, 1)) 40 | ref_color = tf.constant(ref_color, dtype=tf.uint8) 41 | 42 | res_image = tf.where(b_equal, ref_color + alpha_img, alpha_img) 43 | 44 | return res_image 45 | 46 | 47 | def color_mask(image, pred_mask_s, colors, alpha=0.4 ): 48 | '''do iteration to assign the color to the corrosponding mask 49 | Based on experiment, the first plane will be red, and the second one will be green-blue, when num=2 50 | and drak blue ,red ,green for num=3 51 | ''' 52 | N = len(colors) 53 | masked_image = image 54 | for i in range(N): 55 | color = colors[i] 56 | 57 | mask = tf.expand_dims(pred_mask_s[:, :, :, i],axis=-1) 58 | max_mask = tf.reduce_max(pred_mask_s, axis=-1,keep_dims=True) 59 | masked_image = apply_mask(masked_image, mask, max_mask, color, alpha) 60 | 61 | return masked_image 62 | 63 | #********************************************************************************************* 64 | 65 | 66 | def meshgrid(batch, height, width, is_homogeneous=True): 67 | """Construct a 2D meshgrid. 68 | 69 | Args: 70 | batch: batch size 71 | height: height of the grid 72 | width: width of the grid 73 | is_homogeneous: whether to return in homogeneous coordinates 74 | Returns: 75 | x,y grid coordinates [batch, 2 (3 if homogeneous), height, width] 76 | """ 77 | x_t = tf.matmul(tf.ones(shape=tf.stack([height, 1])), 78 | tf.transpose(tf.expand_dims(tf.linspace(-1.0, 1.0, width), 1), [1, 0])) #tf.linspace(start,stop,num) 79 | y_t = tf.matmul(tf.expand_dims(tf.linspace(-1.0, 1.0, height), 1), 80 | tf.ones(shape=tf.stack([1, width]))) 81 | x_t = (x_t + 1.0) * 0.5 * tf.cast(width - 1, tf.float32) 82 | y_t = (y_t + 1.0) * 0.5 * tf.cast(height - 1, tf.float32) 83 | if is_homogeneous: 84 | ones = tf.ones_like(x_t) 85 | coords = tf.stack([x_t, y_t, ones], axis=0) 86 | else: 87 | coords = tf.stack([x_t, y_t], axis=0) 88 | coords = tf.tile(tf.expand_dims(coords, 0), [batch, 1, 1, 1]) 89 | return coords 90 | 91 | 92 | def compute_depth(img, pred_param, num_plane, intrinsics): 93 | batch, height, width, _ = img.get_shape().as_list() 94 | # Construct pixel grid coordinates 95 | pixel_coords = meshgrid(batch, height, width) # 3*128*416 96 | 97 | cam_coords = tf.reshape(pixel_coords, [batch, 3, -1]) 98 | 99 | unscaled_ray_Q = tf.matmul(tf.matrix_inverse(intrinsics), cam_coords) 100 | 101 | for k in range(num_plane): 102 | n_div_d = tf.expand_dims(pred_param[:, k, :], axis=1) 103 | scale = tf.matmul(n_div_d, tf.matmul(tf.matrix_inverse(intrinsics), cam_coords)) 104 | 105 | plane_based_Q = scale * 1./ (unscaled_ray_Q + 1e-10) 106 | plane_based_Q = tf.reshape(plane_based_Q, [batch, 3, height, width]) 107 | plane_based_Q = tf.transpose(plane_based_Q, perm=[0, 2, 3, 1]) 108 | 109 | if k == 0: 110 | plane_inv_depth_stack = plane_based_Q[:,:,:,-1:] 111 | else: 112 | plane_inv_depth_stack = tf.concat([plane_inv_depth_stack, 113 | plane_based_Q[:, :,:, -1:]], axis=-1) 114 | 115 | return plane_inv_depth_stack 116 | 117 | def compute_unscaled_ray(img, intrinsics): 118 | batch, height, width, _ = img.get_shape().as_list() 119 | # Construct pixel grid coordinates 120 | pixel_coords = meshgrid(batch, height, width) # 3*128*416 121 | 122 | cam_coords = tf.reshape(pixel_coords, [batch, 3, -1]) 123 | 124 | unscaled_ray_Q = tf.matmul(tf.matrix_inverse(intrinsics), cam_coords) 125 | 126 | return unscaled_ray_Q 127 | 128 | def compute_plane_equation(img, pred_param, ray, depth): 129 | 130 | batch, height, width, _ = img.get_shape().as_list() 131 | n_time_ray = tf.matmul(pred_param,ray) 132 | n_time_ray = tf.reshape(n_time_ray, [batch, 1, height, width]) 133 | n_time_ray = tf.transpose(n_time_ray, perm=[0, 2, 3, 1]) 134 | 135 | left_eq = n_time_ray * depth 136 | 137 | return left_eq 138 | 139 | def val2uint8(mat,maxVal): 140 | maxVal_mat = tf.constant(maxVal,tf.float32,mat.get_shape()) 141 | mat_vis = tf.where(tf.greater(mat,maxVal_mat), maxVal_mat, mat) 142 | return tf.cast(mat_vis * 255. / maxVal, tf.uint8) 143 | -------------------------------------------------------------------------------- /rpvio_estimator/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(rpvio_estimator) 3 | 4 | set(CMAKE_BUILD_TYPE "Release") 5 | set(CMAKE_CXX_FLAGS "-std=c++11") 6 | #-DEIGEN_USE_MKL_ALL") 7 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g") 8 | 9 | find_package(catkin REQUIRED COMPONENTS 10 | roscpp 11 | std_msgs 12 | geometry_msgs 13 | nav_msgs 14 | tf 15 | cv_bridge 16 | ) 17 | 18 | find_package(OpenCV REQUIRED) 19 | 20 | # message(WARNING "OpenCV_VERSION: ${OpenCV_VERSION}") 21 | 22 | find_package(Ceres REQUIRED) 23 | 24 | include_directories(${catkin_INCLUDE_DIRS} ${CERES_INCLUDE_DIRS}) 25 | 26 | set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) 27 | find_package(Eigen3) 28 | include_directories( 29 | ${catkin_INCLUDE_DIRS} 30 | ${EIGEN3_INCLUDE_DIR} 31 | ) 32 | 33 | catkin_package() 34 | 35 | add_executable(rpvio_estimator 36 | src/estimator_node.cpp 37 | src/parameters.cpp 38 | src/estimator.cpp 39 | src/feature_manager.cpp 40 | src/factor/pose_local_parameterization.cpp 41 | src/factor/projection_factor.cpp 42 | src/factor/projection_td_factor.cpp 43 | src/factor/marginalization_factor.cpp 44 | src/utility/utility.cpp 45 | src/utility/visualization.cpp 46 | src/utility/CameraPoseVisualization.cpp 47 | src/initial/solve_5pts.cpp 48 | src/initial/initial_aligment.cpp 49 | src/initial/initial_sfm.cpp 50 | src/initial/initial_ex_rotation.cpp 51 | ) 52 | 53 | 54 | target_link_libraries(rpvio_estimator ${catkin_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES}) 55 | 56 | 57 | -------------------------------------------------------------------------------- /rpvio_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 | -------------------------------------------------------------------------------- /rpvio_estimator/launch/advio_12.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /rpvio_estimator/launch/ol_market1.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /rpvio_estimator/launch/rpvio_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /rpvio_estimator/launch/rpvio_sim.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /rpvio_estimator/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rpvio_estimator 4 | 0.0.0 5 | The vins_estimator package 6 | 7 | karnikram 8 | GPLv3 9 | 10 | catkin 11 | roscpp 12 | roscpp 13 | 14 | 15 | -------------------------------------------------------------------------------- /rpvio_estimator/src/estimator.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "parameters.h" 4 | #include "feature_manager.h" 5 | #include "utility/utility.h" 6 | #include "utility/tic_toc.h" 7 | #include "initial/solve_5pts.h" 8 | #include "initial/initial_sfm.h" 9 | #include "initial/initial_alignment.h" 10 | #include "initial/initial_ex_rotation.h" 11 | #include 12 | #include 13 | 14 | #include 15 | #include "factor/imu_factor.h" 16 | #include "factor/pose_local_parameterization.h" 17 | #include "factor/projection_factor.h" 18 | #include "factor/projection_td_factor.h" 19 | #include "factor/marginalization_factor.h" 20 | #include "factor/homography_factor.h" 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | 27 | class Estimator 28 | { 29 | public: 30 | Estimator(); 31 | 32 | void setParameter(); 33 | 34 | // interface 35 | void processIMU(double t, const Vector3d &linear_acceleration, const Vector3d &angular_velocity); 36 | void processImage(const map>>> &image, const std_msgs::Header &header); 37 | void setReloFrame(double _frame_stamp, int _frame_index, vector &_match_points, Vector3d _relo_t, Matrix3d _relo_r); 38 | 39 | // internal 40 | void clearState(); 41 | bool initialStructure(); 42 | bool visualInitialAlign(map &relative_T, Eigen::Vector3d <); 43 | bool relativePose(Matrix3d &relative_R, Vector3d &relative_T, int &l); 44 | bool relativeHPose(map &relative_R, map &relative_T, map &n, int &l, int &lplane_id); 45 | void initializeNewPlanes(); 46 | void slideWindow(); 47 | void solveOdometry(); 48 | void slideWindowNew(); 49 | void slideWindowOld(); 50 | void optimization(); 51 | void vector2double(); 52 | void double2vector(); 53 | bool failureDetection(); 54 | 55 | 56 | enum SolverFlag 57 | { 58 | INITIAL, 59 | NON_LINEAR 60 | }; 61 | 62 | enum MarginalizationFlag 63 | { 64 | MARGIN_OLD = 0, 65 | MARGIN_SECOND_NEW = 1 66 | }; 67 | 68 | SolverFlag solver_flag; 69 | MarginalizationFlag marginalization_flag; 70 | Vector3d g; 71 | MatrixXd Ap[2], backup_A; 72 | VectorXd bp[2], backup_b; 73 | 74 | Matrix3d ric[NUM_OF_CAM]; 75 | Vector3d tic[NUM_OF_CAM]; 76 | 77 | Vector3d Ps[(WINDOW_SIZE + 1)]; 78 | Vector3d Vs[(WINDOW_SIZE + 1)]; 79 | Matrix3d Rs[(WINDOW_SIZE + 1)]; 80 | Vector3d Bas[(WINDOW_SIZE + 1)]; 81 | Vector3d Bgs[(WINDOW_SIZE + 1)]; 82 | double td; 83 | double s; 84 | int lplane_id; 85 | 86 | Matrix3d back_R0, last_R, last_R0; 87 | Vector3d back_P0, last_P, last_P0; 88 | std_msgs::Header Headers[(WINDOW_SIZE + 1)]; 89 | 90 | IntegrationBase *pre_integrations[(WINDOW_SIZE + 1)]; 91 | Vector3d acc_0, gyr_0; 92 | 93 | vector dt_buf[(WINDOW_SIZE + 1)]; 94 | vector linear_acceleration_buf[(WINDOW_SIZE + 1)]; 95 | vector angular_velocity_buf[(WINDOW_SIZE + 1)]; 96 | 97 | int frame_count; 98 | int sum_of_outlier, sum_of_back, sum_of_front, sum_of_invalid; 99 | 100 | FeatureManager f_manager; 101 | MotionEstimator m_estimator; 102 | InitialEXRotation initial_ex_rotation; 103 | 104 | bool first_imu; 105 | bool is_valid, is_key; 106 | bool failure_occur; 107 | 108 | vector point_cloud; 109 | vector margin_cloud; 110 | vector key_poses; 111 | double initial_timestamp; 112 | 113 | double para_Pose[WINDOW_SIZE + 1][SIZE_POSE]; 114 | double para_SpeedBias[WINDOW_SIZE + 1][SIZE_SPEEDBIAS]; 115 | double para_Feature[NUM_OF_F][SIZE_FEATURE]; 116 | double para_Ex_Pose[NUM_OF_CAM][SIZE_POSE]; 117 | double para_Retrive_Pose[SIZE_POSE]; 118 | double para_Td[1][1]; 119 | double para_Tr[1][1]; 120 | 121 | // In global camera frame (c_0) 122 | map> para_N; 123 | map> para_d; 124 | 125 | vector init_pids; 126 | 127 | int loop_window_index; 128 | 129 | MarginalizationInfo *last_marginalization_info; 130 | vector last_marginalization_parameter_blocks; 131 | 132 | map all_image_frame; 133 | IntegrationBase *tmp_pre_integration; 134 | 135 | //relocalization variables 136 | bool relocalization_info; 137 | double relo_frame_stamp; 138 | double relo_frame_index; 139 | int relo_frame_local_index; 140 | vector match_points; 141 | double relo_Pose[SIZE_POSE]; 142 | Matrix3d drift_correct_r; 143 | Vector3d drift_correct_t; 144 | Vector3d prev_relo_t; 145 | Matrix3d prev_relo_r; 146 | Vector3d relo_relative_t; 147 | Quaterniond relo_relative_q; 148 | double relo_relative_yaw; 149 | }; 150 | -------------------------------------------------------------------------------- /rpvio_estimator/src/factor/homography_factor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | 5 | struct HomographyFactor 6 | { 7 | HomographyFactor(const Eigen::Vector3d &_pts_i, const Eigen::Vector3d &_pts_j) : pts_i(_pts_i), pts_j(_pts_j) {} 8 | 9 | template 10 | bool operator()(const T* const pose_i, const T* const pose_j, const T* const para_n, const T* const para_depth, const T* const ex_pose, 11 | T* residuals) const 12 | { 13 | Eigen::Map> pi(pose_i); 14 | Eigen::Quaternion qi; 15 | qi.coeffs() << pose_i[3], pose_i[4], pose_i[5], pose_i[6]; 16 | 17 | Eigen::Map> pj(pose_j); 18 | Eigen::Quaternion qj; 19 | qj.coeffs() << pose_j[3], pose_j[4], pose_j[5], pose_j[6]; 20 | 21 | Eigen::Map> tic(ex_pose); 22 | Eigen::Quaternion qic; 23 | qic.coeffs() << ex_pose[3], ex_pose[4], ex_pose[5], ex_pose[6]; 24 | 25 | Eigen::Matrix n; 26 | n << para_n[0], para_n[1], para_n[2]; 27 | 28 | // transform camera normal to imu normal 29 | Eigen::Matrix n_imu_0 = qic*n;// + tic; 30 | 31 | // transform imu 0 normal to imu i normal 32 | Eigen::Matrix n_imu_i = qi.inverse()*n_imu_0;// - qi.inverse()*pi; 33 | 34 | Eigen::Map> depth(para_depth); 35 | 36 | Eigen::Quaternion qji = qj.inverse() * qi; 37 | Eigen::Matrix tji = qj.inverse() * (pi - pj); 38 | Eigen::Matrix di, di0; 39 | 40 | // convert camera depth to imu frame 41 | di0(0,0) = depth(0,0) + tic.dot(n_imu_0); 42 | // convert imu 0 depth to imu i depth 43 | di(0,0) = di0(0,0) - pi.dot(n_imu_0); 44 | 45 | Eigen::Matrix pts_imu_i = qic * pts_i.cast() + tic; 46 | 47 | // homography mapping 48 | Eigen::Matrix pts_imu_j = qji * pts_imu_i + (tji*(1.0/di(0,0)) * n_imu_i.transpose()) * pts_imu_i; 49 | 50 | Eigen::Matrix pts_cam_j = qic.inverse() * (pts_imu_j - tic); 51 | 52 | pts_cam_j = (pts_cam_j / pts_cam_j[2]); 53 | residuals[0] = pts_cam_j[0] - pts_j[0]; 54 | residuals[1] = pts_cam_j[1] - pts_j[1]; 55 | 56 | return true; 57 | } 58 | 59 | static ceres::CostFunction* Create(const Eigen::Vector3d &_pts_i, const Eigen::Vector3d &_pts_j) 60 | { 61 | return (new ceres::AutoDiffCostFunction 62 | (new HomographyFactor(_pts_i, _pts_j))); 63 | } 64 | 65 | Eigen::Vector3d pts_i, pts_j; 66 | }; 67 | -------------------------------------------------------------------------------- /rpvio_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 | -------------------------------------------------------------------------------- /rpvio_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 | -------------------------------------------------------------------------------- /rpvio_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 | -------------------------------------------------------------------------------- /rpvio_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 | -------------------------------------------------------------------------------- /rpvio_estimator/src/factor/projection_td_factor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include "../utility/utility.h" 7 | #include "../utility/tic_toc.h" 8 | #include "../parameters.h" 9 | 10 | class ProjectionTdFactor : public ceres::SizedCostFunction<2, 7, 7, 7, 1, 1> 11 | { 12 | public: 13 | ProjectionTdFactor(const Eigen::Vector3d &_pts_i, const Eigen::Vector3d &_pts_j, 14 | const Eigen::Vector2d &_velocity_i, const Eigen::Vector2d &_velocity_j, 15 | const double _td_i, const double _td_j, const double _row_i, const double _row_j); 16 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 17 | void check(double **parameters); 18 | 19 | Eigen::Vector3d pts_i, pts_j; 20 | Eigen::Vector3d velocity_i, velocity_j; 21 | double td_i, td_j; 22 | Eigen::Matrix tangent_base; 23 | double row_i, row_j; 24 | static Eigen::Matrix2d sqrt_info; 25 | static double sum_t; 26 | }; 27 | -------------------------------------------------------------------------------- /rpvio_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, plane_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 _plane_id, int _start_frame) 60 | : feature_id(_feature_id), plane_id(_plane_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 | int getLargestPlaneId(std::vector &init_pids); 79 | 80 | bool addFeatureCheckParallax(int frame_count, const map>>> &image, double td); 81 | void debugShow(); 82 | map>> getCorresponding(int frame_count_l, int frame_count_r); 83 | vector> getCorrespondingByPlane(int frame_count_l, int frame_count_r, int plane_id); 84 | set plane_ids; 85 | 86 | //void updateDepth(const VectorXd &x); 87 | void setDepth(const VectorXd &x); 88 | void removeFailures(); 89 | void clearDepth(const VectorXd &x); 90 | VectorXd getDepthVector(); 91 | void triangulate(Vector3d Ps[], Vector3d tic[], Matrix3d ric[]); 92 | void removeBackShiftDepth(Eigen::Matrix3d marg_R, Eigen::Vector3d marg_P, Eigen::Matrix3d new_R, Eigen::Vector3d new_P); 93 | void removeBack(); 94 | void removeFront(int frame_count); 95 | void removeOutlier(); 96 | list feature; 97 | int last_track_num; 98 | 99 | private: 100 | double compensatedParallax2(const FeaturePerId &it_per_id, int frame_count); 101 | const Matrix3d *Rs; 102 | Matrix3d ric[NUM_OF_CAM]; 103 | }; 104 | 105 | #endif -------------------------------------------------------------------------------- /rpvio_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 | -------------------------------------------------------------------------------- /rpvio_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); -------------------------------------------------------------------------------- /rpvio_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 | -------------------------------------------------------------------------------- /rpvio_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 | -------------------------------------------------------------------------------- /rpvio_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 | struct SFMFeature 15 | { 16 | bool state; 17 | int id; 18 | int plane_id; 19 | vector> observation; 20 | double position[3]; 21 | double image[3]; // u v 1 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 | 57 | struct ReprojectionErrorH 58 | { 59 | ReprojectionErrorH(double observed_u, double observed_v) 60 | :observed_u(observed_u), observed_v(observed_v) 61 | {} 62 | 63 | template 64 | bool operator()(const T* const R, const T* const t, const T* const n, const T* point, T* residuals) const 65 | { 66 | T rp[3]; 67 | T norm_point[3] = {point[0]/point[2], point[1]/point[2], point[2]/point[2]}; 68 | ceres::QuaternionRotatePoint(R, norm_point, rp); 69 | T np = n[0] * norm_point[0] + n[1] * norm_point[1] + n[2] * norm_point[2]; 70 | rp[0] += t[0]*np; rp[1] += t[1]*np; rp[2] += t[2]*np; 71 | T xp = rp[0] / rp[2]; 72 | T yp = rp[1] / rp[2]; 73 | residuals[0] = xp - T(observed_u); 74 | residuals[1] = yp - T(observed_v); 75 | return true; 76 | } 77 | 78 | static ceres::CostFunction* Create(const double observed_x, 79 | const double observed_y) 80 | { 81 | return (new ceres::AutoDiffCostFunction< 82 | ReprojectionErrorH, 2, 4, 3, 3, 3>( 83 | new ReprojectionErrorH(observed_x,observed_y))); 84 | } 85 | 86 | double observed_u; 87 | double observed_v; 88 | }; 89 | 90 | 91 | class GlobalSFM 92 | { 93 | public: 94 | GlobalSFM(); 95 | bool construct(int frame_num, Quaterniond* q, Vector3d* T, int l, 96 | const Matrix3d relative_R, const Vector3d relative_T, 97 | vector &sfm_f, map &sfm_tracked_points); 98 | 99 | bool constructH(int frame_num, Quaterniond* q, Vector3d* T, int l, 100 | const Matrix3d relative_R, const Vector3d relative_T, Vector3d &n, 101 | vector &sfm_f, map &sfm_tracked_points); 102 | 103 | private: 104 | bool solveFrameByPnP(Matrix3d &R_initial, Vector3d &P_initial, int i, vector &sfm_f); 105 | 106 | void triangulatePoint(Eigen::Matrix &Pose0, Eigen::Matrix &Pose1, 107 | Vector2d &point0, Vector2d &point1, Vector3d &point_3d); 108 | void triangulateTwoFrames(int frame0, Eigen::Matrix &Pose0, 109 | int frame1, Eigen::Matrix &Pose1, 110 | vector &sfm_f); 111 | 112 | int feature_num; 113 | }; 114 | -------------------------------------------------------------------------------- /rpvio_estimator/src/initial/solve_5pts.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | using namespace std; 5 | 6 | #include 7 | #include 8 | #include 9 | using namespace Eigen; 10 | 11 | #include 12 | 13 | class MotionEstimator 14 | { 15 | public: 16 | 17 | bool solveRelativeRT(const vector> &corres, Matrix3d &R, Vector3d &T); 18 | bool solveRelativeHRT(const vector> &corres, const Matrix3d &R_imu, const Matrix4d &TrIC, Matrix3d &R, Vector3d &T, Vector3d &n); 19 | void decomposeH(const cv::Mat &H, const cv::Mat &K, const Matrix3d &R_imu, const Matrix4d &TrIC, const Vector3d &mean_l, Matrix4d &est_Tr, Vector3d &est_n); 20 | 21 | private: 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 | }; 29 | 30 | 31 | -------------------------------------------------------------------------------- /rpvio_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 | int ROLLING_SHUTTER; 20 | std::string EX_CALIB_RESULT_PATH; 21 | std::string RPVIO_RESULT_PATH; 22 | std::string IMU_TOPIC; 23 | double ROW, COL; 24 | double TD, TR; 25 | 26 | template 27 | T readParam(ros::NodeHandle &n, std::string name) 28 | { 29 | T ans; 30 | if (n.getParam(name, ans)) 31 | { 32 | ROS_INFO_STREAM("Loaded " << name << ": " << ans); 33 | } 34 | else 35 | { 36 | ROS_ERROR_STREAM("Failed to load " << name); 37 | n.shutdown(); 38 | } 39 | return ans; 40 | } 41 | 42 | void readParameters(ros::NodeHandle &n) 43 | { 44 | std::string config_file; 45 | config_file = readParam(n, "config_file"); 46 | cv::FileStorage fsSettings(config_file, cv::FileStorage::READ); 47 | if(!fsSettings.isOpened()) 48 | { 49 | std::cerr << "ERROR: Wrong path to settings" << std::endl; 50 | } 51 | 52 | fsSettings["imu_topic"] >> IMU_TOPIC; 53 | 54 | SOLVER_TIME = fsSettings["max_solver_time"]; 55 | NUM_ITERATIONS = fsSettings["max_num_iterations"]; 56 | MIN_PARALLAX = fsSettings["keyframe_parallax"]; 57 | MIN_PARALLAX = MIN_PARALLAX / FOCAL_LENGTH; 58 | 59 | std::string OUTPUT_PATH; 60 | fsSettings["output_path"] >> OUTPUT_PATH; 61 | RPVIO_RESULT_PATH = OUTPUT_PATH + "/rpvio_result_no_loop.csv"; 62 | std::cout << "result path " << RPVIO_RESULT_PATH << std::endl; 63 | 64 | // create folder if not exists 65 | FileSystemHelper::createDirectoryIfNotExists(OUTPUT_PATH.c_str()); 66 | 67 | std::ofstream fout(RPVIO_RESULT_PATH, std::ios::out); 68 | fout.close(); 69 | 70 | ACC_N = fsSettings["acc_n"]; 71 | ACC_W = fsSettings["acc_w"]; 72 | GYR_N = fsSettings["gyr_n"]; 73 | GYR_W = fsSettings["gyr_w"]; 74 | G.z() = fsSettings["g_norm"]; 75 | ROW = fsSettings["image_height"]; 76 | COL = fsSettings["image_width"]; 77 | ROS_INFO("ROW: %f COL: %f ", ROW, COL); 78 | 79 | ESTIMATE_EXTRINSIC = fsSettings["estimate_extrinsic"]; 80 | if (ESTIMATE_EXTRINSIC == 2) 81 | { 82 | ROS_WARN("have no prior about extrinsic param, calibrate extrinsic param"); 83 | RIC.push_back(Eigen::Matrix3d::Identity()); 84 | TIC.push_back(Eigen::Vector3d::Zero()); 85 | EX_CALIB_RESULT_PATH = OUTPUT_PATH + "/extrinsic_parameter.csv"; 86 | 87 | } 88 | else 89 | { 90 | if ( ESTIMATE_EXTRINSIC == 1) 91 | { 92 | ROS_WARN(" Optimize extrinsic param around initial guess!"); 93 | EX_CALIB_RESULT_PATH = OUTPUT_PATH + "/extrinsic_parameter.csv"; 94 | } 95 | if (ESTIMATE_EXTRINSIC == 0) 96 | ROS_WARN(" fix extrinsic param "); 97 | 98 | cv::Mat cv_R, cv_T; 99 | fsSettings["extrinsicRotation"] >> cv_R; 100 | fsSettings["extrinsicTranslation"] >> cv_T; 101 | Eigen::Matrix3d eigen_R; 102 | Eigen::Vector3d eigen_T; 103 | cv::cv2eigen(cv_R, eigen_R); 104 | cv::cv2eigen(cv_T, eigen_T); 105 | Eigen::Quaterniond Q(eigen_R); 106 | eigen_R = Q.normalized(); 107 | RIC.push_back(eigen_R); 108 | TIC.push_back(eigen_T); 109 | ROS_INFO_STREAM("Extrinsic_R : " << std::endl << RIC[0]); 110 | ROS_INFO_STREAM("Extrinsic_T : " << std::endl << TIC[0].transpose()); 111 | 112 | } 113 | 114 | INIT_DEPTH = 5.0; 115 | BIAS_ACC_THRESHOLD = 0.1; 116 | BIAS_GYR_THRESHOLD = 0.1; 117 | 118 | TD = fsSettings["td"]; 119 | ESTIMATE_TD = fsSettings["estimate_td"]; 120 | if (ESTIMATE_TD) 121 | ROS_INFO_STREAM("Unsynchronized sensors, online estimate time offset, initial td: " << TD); 122 | else 123 | ROS_INFO_STREAM("Synchronized sensors, fix time offset: " << TD); 124 | 125 | ROLLING_SHUTTER = fsSettings["rolling_shutter"]; 126 | if (ROLLING_SHUTTER) 127 | { 128 | TR = fsSettings["rolling_shutter_tr"]; 129 | ROS_INFO_STREAM("rolling shutter camera, read out time per line: " << TR); 130 | } 131 | else 132 | { 133 | TR = 0; 134 | } 135 | 136 | fsSettings.release(); 137 | } 138 | -------------------------------------------------------------------------------- /rpvio_estimator/src/parameters.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include "utility/utility.h" 7 | #include 8 | #include 9 | #include 10 | 11 | const double FOCAL_LENGTH = 460.0; 12 | const int WINDOW_SIZE = 10; 13 | const int NUM_OF_CAM = 1; 14 | const int NUM_OF_F = 1000; 15 | //#define UNIT_SPHERE_ERROR 16 | 17 | extern double INIT_DEPTH; 18 | extern double MIN_PARALLAX; 19 | extern int ESTIMATE_EXTRINSIC; 20 | 21 | extern double ACC_N, ACC_W; 22 | extern double GYR_N, GYR_W; 23 | 24 | extern std::vector RIC; 25 | extern std::vector TIC; 26 | extern Eigen::Vector3d G; 27 | 28 | extern double BIAS_ACC_THRESHOLD; 29 | extern double BIAS_GYR_THRESHOLD; 30 | extern double SOLVER_TIME; 31 | extern int NUM_ITERATIONS; 32 | extern std::string EX_CALIB_RESULT_PATH; 33 | extern std::string RPVIO_RESULT_PATH; 34 | extern std::string IMU_TOPIC; 35 | extern double TD; 36 | extern double TR; 37 | extern int ESTIMATE_TD; 38 | extern int ROLLING_SHUTTER; 39 | extern double ROW, COL; 40 | 41 | 42 | void readParameters(ros::NodeHandle &n); 43 | 44 | enum SIZE_PARAMETERIZATION 45 | { 46 | SIZE_POSE = 7, 47 | SIZE_SPEEDBIAS = 9, 48 | SIZE_FEATURE = 1 49 | }; 50 | 51 | enum StateOrder 52 | { 53 | O_P = 0, 54 | O_R = 3, 55 | O_V = 6, 56 | O_BA = 9, 57 | O_BG = 12 58 | }; 59 | 60 | enum NoiseOrder 61 | { 62 | O_AN = 0, 63 | O_GN = 3, 64 | O_AW = 6, 65 | O_GW = 9 66 | }; 67 | -------------------------------------------------------------------------------- /rpvio_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 | } -------------------------------------------------------------------------------- /rpvio_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 | -------------------------------------------------------------------------------- /rpvio_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 | -------------------------------------------------------------------------------- /rpvio_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 | -------------------------------------------------------------------------------- /rpvio_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 | -------------------------------------------------------------------------------- /rpvio_estimator/src/utility/visualization.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include "CameraPoseVisualization.h" 17 | #include 18 | #include "../estimator.h" 19 | #include "../parameters.h" 20 | #include 21 | 22 | extern ros::Publisher pub_odometry; 23 | extern ros::Publisher pub_path, pub_pose; 24 | extern ros::Publisher pub_cloud, pub_map; 25 | extern ros::Publisher pub_key_poses; 26 | extern ros::Publisher pub_ref_pose, pub_cur_pose; 27 | extern ros::Publisher pub_key; 28 | extern nav_msgs::Path path; 29 | extern ros::Publisher pub_pose_graph; 30 | extern int IMAGE_ROW, IMAGE_COL; 31 | 32 | void registerPub(ros::NodeHandle &n); 33 | 34 | void pubLatestOdometry(const Eigen::Vector3d &P, const Eigen::Quaterniond &Q, const Eigen::Vector3d &V, const std_msgs::Header &header); 35 | 36 | void printStatistics(const Estimator &estimator, double t); 37 | 38 | void pubOdometry(const Estimator &estimator, const std_msgs::Header &header); 39 | 40 | void pubInitialGuess(const Estimator &estimator, const std_msgs::Header &header); 41 | 42 | void pubKeyPoses(const Estimator &estimator, const std_msgs::Header &header); 43 | 44 | void pubCameraPose(const Estimator &estimator, const std_msgs::Header &header); 45 | 46 | void pubPointCloud(const Estimator &estimator, const std_msgs::Header &header); 47 | 48 | void pubTF(const Estimator &estimator, const std_msgs::Header &header); 49 | 50 | void pubKeyframe(const Estimator &estimator); 51 | 52 | void pubRelocalization(const Estimator &estimator); -------------------------------------------------------------------------------- /rpvio_feature_tracker/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(rpvio_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 | camera_model 14 | message_filters 15 | ) 16 | 17 | find_package(OpenCV REQUIRED) 18 | 19 | catkin_package() 20 | 21 | include_directories( 22 | ${catkin_INCLUDE_DIRS} 23 | ) 24 | 25 | set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) 26 | find_package(Eigen3) 27 | include_directories( 28 | ${catkin_INCLUDE_DIRS} 29 | ${EIGEN3_INCLUDE_DIR} 30 | ) 31 | 32 | add_executable(rpvio_feature_tracker 33 | src/feature_tracker_node.cpp 34 | src/parameters.cpp 35 | src/feature_tracker.cpp 36 | ) 37 | 38 | target_link_libraries(rpvio_feature_tracker ${catkin_LIBRARIES} ${OpenCV_LIBS}) 39 | -------------------------------------------------------------------------------- /rpvio_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 | -------------------------------------------------------------------------------- /rpvio_feature_tracker/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rpvio_feature_tracker 4 | 0.0.0 5 | The rpvio_feature_tracker package 6 | 7 | karnikram 8 | GPLv3 9 | 10 | catkin 11 | roscpp 12 | camera_model 13 | message_generation 14 | roscpp 15 | camera_model 16 | message_runtime 17 | 18 | 19 | -------------------------------------------------------------------------------- /rpvio_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, const cv::Mat &_mask, double _cur_time); 34 | 35 | void setMask(const cv::Mat &_mask); 36 | 37 | void addPoints(const cv::Mat &_mask); 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 rejectWithH(); 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 plane_ids; 58 | vector track_cnt; 59 | map cur_un_pts_map; 60 | map prev_un_pts_map; 61 | camodocal::CameraPtr m_camera; 62 | double cur_time; 63 | double prev_time; 64 | 65 | static int n_id; 66 | }; 67 | -------------------------------------------------------------------------------- /rpvio_feature_tracker/src/parameters.cpp: -------------------------------------------------------------------------------- 1 | #include "parameters.h" 2 | 3 | std::string IMAGE_TOPIC; 4 | std::string MASK_TOPIC; 5 | std::string IMU_TOPIC; 6 | std::vector CAM_NAMES; 7 | std::string FISHEYE_MASK; 8 | int MAX_CNT; 9 | int MIN_DIST; 10 | int WINDOW_SIZE; 11 | int FREQ; 12 | double H_THRESHOLD; 13 | int SHOW_TRACK; 14 | int STEREO_TRACK; 15 | int EQUALIZE; 16 | int ROW; 17 | int COL; 18 | int FOCAL_LENGTH; 19 | int FISHEYE; 20 | bool PUB_THIS_FRAME; 21 | 22 | template 23 | T readParam(ros::NodeHandle &n, std::string name) 24 | { 25 | T ans; 26 | if (n.getParam(name, ans)) 27 | { 28 | ROS_INFO_STREAM("Loaded " << name << ": " << ans); 29 | } 30 | else 31 | { 32 | ROS_ERROR_STREAM("Failed to load " << name); 33 | n.shutdown(); 34 | } 35 | return ans; 36 | } 37 | 38 | void readParameters(ros::NodeHandle &n) 39 | { 40 | std::string config_file; 41 | config_file = readParam(n, "config_file"); 42 | cv::FileStorage fsSettings(config_file, cv::FileStorage::READ); 43 | if(!fsSettings.isOpened()) 44 | { 45 | std::cerr << "ERROR: Wrong path to settings" << std::endl; 46 | } 47 | std::string RPVIO_FOLDER_PATH = readParam(n, "rpvio_folder"); 48 | 49 | fsSettings["image_topic"] >> IMAGE_TOPIC; 50 | fsSettings["mask_topic"] >> MASK_TOPIC; 51 | fsSettings["imu_topic"] >> IMU_TOPIC; 52 | MAX_CNT = fsSettings["max_cnt"]; 53 | MIN_DIST = fsSettings["min_dist"]; 54 | ROW = fsSettings["image_height"]; 55 | COL = fsSettings["image_width"]; 56 | FREQ = fsSettings["freq"]; 57 | H_THRESHOLD = fsSettings["H_threshold"]; 58 | SHOW_TRACK = fsSettings["show_track"]; 59 | EQUALIZE = fsSettings["equalize"]; 60 | FISHEYE = fsSettings["fisheye"]; 61 | if (FISHEYE == 1) 62 | FISHEYE_MASK = RPVIO_FOLDER_PATH + "config/fisheye_mask.jpg"; 63 | CAM_NAMES.push_back(config_file); 64 | 65 | WINDOW_SIZE = 20; 66 | STEREO_TRACK = false; 67 | FOCAL_LENGTH = 460; 68 | PUB_THIS_FRAME = false; 69 | 70 | if (FREQ == 0) 71 | FREQ = 100; 72 | 73 | fsSettings.release(); 74 | 75 | 76 | } 77 | -------------------------------------------------------------------------------- /rpvio_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 MASK_TOPIC; 13 | extern std::string IMU_TOPIC; 14 | extern std::string FISHEYE_MASK; 15 | extern std::vector CAM_NAMES; 16 | extern int MAX_CNT; 17 | extern int MIN_DIST; 18 | extern int WINDOW_SIZE; 19 | extern int FREQ; 20 | extern double H_THRESHOLD; 21 | extern int SHOW_TRACK; 22 | extern int STEREO_TRACK; 23 | extern int EQUALIZE; 24 | extern int FISHEYE; 25 | extern bool PUB_THIS_FRAME; 26 | 27 | void readParameters(ros::NodeHandle &n); 28 | -------------------------------------------------------------------------------- /rpvio_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 | -------------------------------------------------------------------------------- /scripts/convert_vins_to_tum.py: -------------------------------------------------------------------------------- 1 | import pandas as pd 2 | import argparse 3 | 4 | def main(): 5 | parser = argparse.ArgumentParser(description='Converts the csv output of VINS-Mono to TUM RGB-D format txt') 6 | parser.add_argument('input_file_path', help='Path to the csv file') 7 | parser.add_argument('output_file_path', help='Path to the output txt file') 8 | args = parser.parse_args() 9 | 10 | data = pd.read_csv(args.input_file_path, usecols=range(8), names=list('txyzwijk')) 11 | 12 | with open(args.output_file_path,'w') as f: 13 | for i in range(len(data['t'])): 14 | f.write(f"{data['t'][i]*1e-9} {data['x'][i]} {data['y'][i]} {data['z'][i]} {data['i'][i]} {data['j'][i]} {data['k'][i]} {data['w'][i]}\n") 15 | 16 | if __name__ == '__main__': 17 | main() 18 | -------------------------------------------------------------------------------- /scripts/run_advio_12.sh: -------------------------------------------------------------------------------- 1 | #! /bin/bash 2 | 3 | dataset=$1 4 | 5 | rm -r $dataset/results/advio 6 | mkdir -p $dataset/results/advio 7 | cd $dataset/results/advio 8 | sed -i "s@~@$HOME@g" ~/catkin_ws/src/rp-vio/config/advio_12_config.yaml 9 | 10 | echo -e "######advio-12######\n" > report.txt 11 | 12 | for((i = 1; i <=1; i++)) 13 | do 14 | roslaunch rpvio_estimator advio_12.launch bagfile_path:=$dataset/12-mask.bag |& tee -a run_log.txt 15 | cp ~/output/rpvio_result_no_loop.csv ./rpvio_est.csv 16 | python ~/catkin_ws/src/rp-vio/scripts/convert_vins_to_tum.py rpvio_est.csv est_traj_$i.txt 17 | rm rpvio_est.csv 18 | evo_ape tum $dataset/groundtruth.txt est_traj_$i.txt --align --save_plot est_traj_$i.pdf |& tee -a report.txt 19 | done 20 | -------------------------------------------------------------------------------- /scripts/run_ol_market1.sh: -------------------------------------------------------------------------------- 1 | #! /bin/bash 2 | 3 | dataset=$1 4 | 5 | rm -r $dataset/results/ol 6 | mkdir -p $dataset/results/ol 7 | cd $dataset/results/ol 8 | sed -i "s@~@$HOME@g" ~/catkin_ws/src/rp-vio/config/ol_market1_config.yaml 9 | 10 | echo -e "######ol-market1######\n" > report.txt 11 | 12 | for((i = 1; i <=1; i++)) 13 | do 14 | roslaunch rpvio_estimator ol_market1.launch bagfile_path:=$dataset/market1-mask.bag 15 | cp ~/output/rpvio_result_no_loop.csv ./rpvio_est.csv 16 | python ~/catkin_ws/src/rp-vio/scripts/convert_vins_to_tum.py rpvio_est.csv est_traj_$i.txt 17 | rm rpvio_est.csv 18 | evo_ape tum $dataset/groundtruth.txt est_traj_$i.txt --align --save_plot est_traj_$i.pdf |& tee -a report.txt 19 | done 20 | -------------------------------------------------------------------------------- /scripts/run_rpvio_sim.sh: -------------------------------------------------------------------------------- 1 | dataset=$1 2 | 3 | rm -r $dataset/results/rpvio-sim/ 4 | mkdir -p $dataset/results/rpvio-sim/ 5 | cd $dataset/results/rpvio-sim/ 6 | sed -i "s@~@$HOME@g" ~/catkin_ws/src/rp-vio/config/rpvio_sim_config.yaml 7 | 8 | ########## static run ############# 9 | 10 | mkdir static 11 | echo -e "######static######\n" > report.txt 12 | 13 | for((i = 1; i <=1; i++)) 14 | do 15 | roslaunch rpvio_estimator rpvio_sim.launch bagfile_path:=$dataset/static/static.bag 16 | cp ~/output/rpvio_result_no_loop.csv ./rpvio_est.csv 17 | python ~/catkin_ws/src/rp-vio/scripts/convert_vins_to_tum.py rpvio_est.csv static/est_traj_$i.txt 18 | rm rpvio_est.csv 19 | evo_ape tum $dataset/static/groundtruth.txt static/est_traj_$i.txt --align --save_plot static/est_traj_$i.pdf |& tee -a report.txt 20 | done 21 | 22 | ########## c1 run ############# 23 | 24 | mkdir c1 25 | echo -e "######c1######\n" > report.txt 26 | 27 | for((i = 1; i <=1; i++)) 28 | do 29 | roslaunch rpvio_estimator rpvio_sim.launch bagfile_path:=$dataset/c1/c1.bag 30 | cp ~/output/rpvio_result_no_loop.csv ./rpvio_est.csv 31 | python ~/catkin_ws/src/rp-vio/scripts/convert_vins_to_tum.py rpvio_est.csv c1/est_traj_$i.txt 32 | rm rpvio_est.csv 33 | evo_ape tum $dataset/c1/groundtruth.txt c1/est_traj_$i.txt --align --save_plot c1/est_traj_$i.pdf |& tee -a report.txt 34 | done 35 | 36 | ########### c2 run ############# 37 | 38 | mkdir c2 39 | echo -e "######c2######\n" > report.txt 40 | 41 | for((i = 1; i <=1; i++)) 42 | do 43 | roslaunch rpvio_estimator rpvio_sim.launch bagfile_path:=$dataset/c2/c2.bag 44 | cp ~/output/rpvio_result_no_loop.csv ./rpvio_est.csv 45 | python ~/catkin_ws/src/rp-vio/scripts/convert_vins_to_tum.py rpvio_est.csv c2/est_traj_$i.txt 46 | rm rpvio_est.csv 47 | evo_ape tum $dataset/c2/groundtruth.txt c2/est_traj_$i.txt --align --save_plot c2/est_traj_$i.pdf |& tee -a report.txt 48 | done 49 | 50 | ########### c4 run ############# 51 | 52 | mkdir c4 53 | echo -e "######c4######\n" > report.txt 54 | 55 | for((i = 1; i <=1; i++)) 56 | do 57 | roslaunch rpvio_estimator rpvio_sim.launch bagfile_path:=$dataset/c4/c4.bag 58 | cp ~/output/rpvio_result_no_loop.csv ./rpvio_est.csv 59 | python ~/catkin_ws/src/rp-vio/scripts/convert_vins_to_tum.py rpvio_est.csv c4/est_traj_$i.txt 60 | rm rpvio_est.csv 61 | evo_ape tum $dataset/c4/groundtruth.txt c4/est_traj_$i.txt --align --save_plot c4/est_traj_$i.pdf |& tee -a report.txt 62 | done 63 | 64 | ############ c6 run ############# 65 | 66 | mkdir c6 67 | echo -e "######c6######\n" > report.txt 68 | 69 | for((i = 1; i <=1; i++)) 70 | do 71 | roslaunch rpvio_estimator rpvio_sim.launch bagfile_path:=$dataset/c6/c6.bag 72 | cp ~/output/rpvio_result_no_loop.csv ./rpvio_est.csv 73 | python ~/catkin_ws/src/rp-vio/scripts/convert_vins_to_tum.py rpvio_est.csv c6/est_traj_$i.txt 74 | rm rpvio_est.csv 75 | evo_ape tum $dataset/c6/groundtruth.txt c6/est_traj_$i.txt --align --save_plot c6/est_traj_$i.pdf |& tee -a report.txt 76 | done 77 | 78 | ############ c8 run ############# 79 | 80 | mkdir c8 81 | echo -e "######c8######\n" > report.txt 82 | 83 | for((i = 1; i <=1; i++)) 84 | do 85 | roslaunch rpvio_estimator rpvio_sim.launch bagfile_path:=$dataset/c8/c8.bag 86 | cp ~/output/rpvio_result_no_loop.csv ./rpvio_est.csv 87 | python ~/catkin_ws/src/rp-vio/scripts/convert_vins_to_tum.py rpvio_est.csv c8/est_traj_$i.txt 88 | rm rpvio_est.csv 89 | evo_ape tum $dataset/c8/groundtruth.txt c8/est_traj_$i.txt --align --save_plot c8/est_traj_$i.pdf |& tee -a report.txt 90 | done 91 | --------------------------------------------------------------------------------