Repository: jianhengLiu/VINS-RGBD-FAST Branch: main Commit: 757c256c747a Files: 152 Total size: 1.1 MB Directory structure: gitextract_ongpbb8j/ ├── .gitignore ├── 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/ │ ├── campus.rviz │ ├── indoor.rviz │ ├── openloris/ │ │ ├── openloris_vio.yaml │ │ └── openloris_vio_atlas.yaml │ ├── realsense/ │ │ ├── vio d455.yaml │ │ ├── vio.yaml │ │ ├── vio_atlas copy.yaml │ │ ├── vio_atlas.yaml │ │ ├── vio_campus.yaml │ │ └── vio_indoor.yaml │ ├── tagaided.rviz │ ├── tsinghua.rviz │ ├── tum_rgbd/ │ │ ├── tum_fr3.yaml │ │ └── tum_fr3_atlas.yaml │ └── video.rviz ├── doc/ │ ├── INSTALL.md │ └── RUNNING_PROCEDURE.md ├── output/ │ └── .gitignore ├── pose_graph/ │ ├── CMakeLists.txt │ ├── cmake/ │ │ └── FindEigen.cmake │ ├── nodelet_plugin.xml │ ├── package.xml │ └── src/ │ ├── ThirdParty/ │ │ ├── DBoW/ │ │ │ ├── BowVector.cpp │ │ │ ├── BowVector.h │ │ │ ├── DBoW2.h │ │ │ ├── FBrief.cpp │ │ │ ├── FBrief.h │ │ │ ├── FClass.h │ │ │ ├── FeatureVector.cpp │ │ │ ├── FeatureVector.h │ │ │ ├── QueryResults.cpp │ │ │ ├── QueryResults.h │ │ │ ├── ScoringObject.cpp │ │ │ ├── ScoringObject.h │ │ │ ├── TemplatedDatabase.h │ │ │ └── TemplatedVocabulary.h │ │ ├── DUtils/ │ │ │ ├── DException.h │ │ │ ├── DUtils.h │ │ │ ├── Random.cpp │ │ │ ├── Random.h │ │ │ ├── Timestamp.cpp │ │ │ └── Timestamp.h │ │ ├── DVision/ │ │ │ ├── BRIEF.cpp │ │ │ ├── BRIEF.h │ │ │ └── DVision.h │ │ ├── VocabularyBinary.cpp │ │ └── VocabularyBinary.hpp │ ├── keyframe/ │ │ ├── keyframe.cpp │ │ └── keyframe.h │ ├── pose_graph/ │ │ ├── pose_graph.cpp │ │ └── pose_graph.h │ ├── pose_graph_nodelet.cpp │ └── utility/ │ ├── CameraPoseVisualization.cpp │ ├── CameraPoseVisualization.h │ ├── parameters.h │ ├── tic_toc.h │ ├── utility.cpp │ └── utility.h ├── support_files/ │ └── brief_pattern.yml └── vins_estimator/ ├── CMakeLists.txt ├── cmake/ │ └── FindEigen.cmake ├── launch/ │ ├── atlas200/ │ │ ├── compressed2img.launch │ │ └── img2compressed.launch │ ├── openloris/ │ │ ├── openloris_vio_atlas.launch │ │ ├── openloris_vio_pytorch.launch │ │ ├── openloris_vio_tensorrt.launch │ │ └── openloris_vo.launch │ ├── realsense/ │ │ ├── l515.launch │ │ ├── realsense_vio.launch │ │ ├── realsense_vio_atlas.launch │ │ └── rs_camera.launch │ ├── tum_rgbd/ │ │ ├── tum_rgbd_atlas.launch │ │ ├── tum_rgbd_pytorch.launch │ │ └── tum_rgbd_tensorrt.launch │ ├── vins_rviz.launch │ └── yolo/ │ ├── atlas.launch │ ├── pytorch.launch │ └── tensorrt.launch ├── nodelet_description.xml ├── package.xml └── src/ ├── estimator/ │ ├── estimator.cpp │ └── estimator.h ├── estimator_nodelet.cpp ├── factor/ │ ├── imu_factor.h │ ├── imu_factor_modified.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/ │ ├── feature_manager.cpp │ └── feature_manager.h ├── feature_tracker/ │ ├── ThreadPool.h │ ├── feature_tracker.cpp │ └── feature_tracker.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 └── utility/ ├── CameraPoseVisualization.cpp ├── CameraPoseVisualization.h ├── parameters.cpp ├── parameters.h ├── tic_toc.h ├── utility.cpp ├── utility.h ├── visualization.cpp └── visualization.h ================================================ FILE CONTENTS ================================================ ================================================ FILE: .gitignore ================================================ **/build **/devel **/.idea **/.vscode **/cmake-build-debug **/.cache !.gitignore ================================================ FILE: README.md ================================================ # VINS-RGBD-FAST **VINS-RGBD-FAST** is a SLAM system based on VINS-RGBD. We do some refinements to accelerate the system's performance in resource-constrained embedded paltform, like [HUAWEI Atlas200 DK](https://e.huawei.com/en/products/cloud-computing-dc/atlas/atlas-200), [NVIDIA Jetson AGX Xavier](https://www.nvidia.com/en-us/autonomous-machines/embedded-systems/jetson-agx-xavier/). ## Refinements * grid-based feature detection * extract FAST feature instead of Harris feature * added stationary initialization * added IMU-aided feature tracking * added extracted-feature area's quality judgement * solved feature clusttering problem result frome FAST feature * use "sensor_msg::CompressedImage" as image topic type ## Related Paper: ``` @ARTICLE{9830851, author={Liu, Jianheng and Li, Xuanfu and Liu, Yueqian and Chen, Haoyao}, journal={IEEE Robotics and Automation Letters}, title={RGB-D Inertial Odometry for a Resource-Restricted Robot in Dynamic Environments}, year={2022}, volume={7}, number={4}, pages={9573-9580}, doi={10.1109/LRA.2022.3191193}} ``` ## RGBD-Inertial Trajectory Estimation and Mapping for Small Ground Rescue Robot Based one open source SLAM framework [VINS-Mono](https://github.com/HKUST-Aerial-Robotics/VINS-Mono). The approach contains + Depth-integrated visual-inertial initialization process. + Visual-inertial odometry by utilizing depth information while avoiding the limitation is working for 3D pose estimation. + Noise elimination map which is suitable for path planning and navigation. However, the proposed approach can also be applied to other application like handheld and wheeled robot. ## 1. Prerequisites 1.1. **Ubuntu** 16.04 or 18.04. 1.2. **ROS** version Kinetic or Melodic fully installation 1.3. **Ceres Solver** Follow [Ceres Installation](http://ceres-solver.org/installation.html) 1.4. **Sophus** ``` git clone http://github.com/strasdat/Sophus.git git checkout a621ff ``` 1.3. **Atlas 200 DK环境配置** [https://blog.csdn.net/qq_42703283/article/details/110389270](https://blog.csdn.net/qq_42703283/article/details/110389270) 1.4. **ROS多机通信** [https://blog.csdn.net/qq_42703283/article/details/110408848]( ## 2. Datasets Recording by RealSense D435i. Contain 9 bags in three different applicaions: + [Handheld](https://star-center.shanghaitech.edu.cn/seafile/d/0ea45d1878914077ade5/) + [Wheeled robot](https://star-center.shanghaitech.edu.cn/seafile/d/78c0375114854774b521/) ([Jackal](https://www.clearpathrobotics.com/jackal-small-unmanned-ground-vehicle/)) + [Tracked robot](https://star-center.shanghaitech.edu.cn/seafile/d/f611fc44df0c4b3d936d/) Note the rosbags are in compressed format. Use "rosbag decompress" to decompress. Topics: + depth topic: /camera/aligned_depth_to_color/image_raw + color topic: /camera/color/image_raw + imu topic: /camera/imu 我们使用的是压缩图像节点： + depth topic: /camera/aligned_depth_to_color/image_raw + color topic: /camera/color/image_raw/compressed + imu topic: /camera/imu 如何录制一个数据包 1. 运行d435i 2. `rosbag record /camera/imu /camera/color/image_raw /camera/aligned_depth_to_color/image_raw /camera/color/camera_info /camera/color/image_raw/compressed` ## 3. Run with Docker make Dockerfile like below ```c FROM ros:melodic-ros-core-bionic # apt-get update RUN apt-get update # install essentials RUN apt install -y gcc RUN apt install -y g++ RUN apt-get install -y cmake RUN apt-get install -y wget RUN apt install -y git # install ceres WORKDIR /home RUN apt-get install -y libgoogle-glog-dev libgflags-dev RUN apt-get install -y libatlas-base-dev RUN apt-get install -y libeigen3-dev RUN apt-get install -y libsuitesparse-dev RUN wget http://ceres-solver.org/ceres-solver-2.1.0.tar.gz RUN tar zxf ceres-solver-2.1.0.tar.gz WORKDIR /home/ceres-solver-2.1.0 RUN mkdir build WORKDIR /home/ceres-solver-2.1.0/build RUN cmake .. RUN make RUN make install # install sophus WORKDIR /home RUN git clone https://github.com/demul/Sophus.git WORKDIR /home/Sophus RUN git checkout fix/unit_complex_eror RUN mkdir build WORKDIR /home/Sophus/build RUN cmake .. RUN make RUN make install # install ros dependencies WORKDIR /home RUN mkdir ros_ws WORKDIR /home/ros_ws RUN apt-get -y install ros-melodic-cv-bridge RUN apt-get -y install ros-melodic-nodelet RUN apt-get -y install ros-melodic-tf RUN apt-get -y install ros-melodic-image-transport RUN apt-get -y install ros-melodic-rviz # build vins-rgbd-fast RUN mkdir src WORKDIR /home/ros_ws/src RUN git clone https://github.com/jianhengLiu/VINS-RGBD-FAST WORKDIR /home/ros_ws RUN /bin/bash -c ". /opt/ros/melodic/setup.bash; cd /home/ros_ws; catkin_make" RUN echo "source /home/ros_ws/devel/setup.bash" >> ~/.bashrc ``` docker build . ## 4. Licence The source code is released under [GPLv3](http://www.gnu.org/licenses/) license. ================================================ FILE: camera_model/CMakeLists.txt ================================================ cmake_minimum_required(VERSION 2.8.3) project(camera_model) set(CMAKE_BUILD_TYPE "Release") # https://blog.csdn.net/qq_24624539/article/details/111056791 #set(CMAKE_CXX_FLAGS "-std=c++11") set(CMAKE_CXX_FLAGS "-std=c++14") set(CMAKE_CXX_FLAGS_RELEASE "-O3 -fPIC") find_package(catkin REQUIRED COMPONENTS roscpp std_msgs ) find_package(Boost REQUIRED COMPONENTS filesystem program_options system) include_directories(${Boost_INCLUDE_DIRS}) find_package(OpenCV REQUIRED) # set(EIGEN_INCLUDE_DIR "/usr/local/include/eigen3") find_package(Ceres REQUIRED) include_directories(${CERES_INCLUDE_DIRS}) catkin_package( INCLUDE_DIRS include LIBRARIES camera_model CATKIN_DEPENDS roscpp std_msgs # DEPENDS system_lib ) include_directories( ${catkin_INCLUDE_DIRS} ) include_directories("include") add_executable(Calibration src/intrinsic_calib.cc src/chessboard/Chessboard.cc src/calib/CameraCalibration.cc src/camera_models/Camera.cc src/camera_models/CameraFactory.cc src/camera_models/CostFunctionFactory.cc src/camera_models/PinholeCamera.cc src/camera_models/CataCamera.cc src/camera_models/EquidistantCamera.cc src/camera_models/ScaramuzzaCamera.cc src/sparse_graph/Transform.cc src/gpl/gpl.cc src/gpl/EigenQuaternionParameterization.cc) add_library(camera_model src/chessboard/Chessboard.cc src/calib/CameraCalibration.cc src/camera_models/Camera.cc src/camera_models/CameraFactory.cc src/camera_models/CostFunctionFactory.cc src/camera_models/PinholeCamera.cc src/camera_models/CataCamera.cc src/camera_models/EquidistantCamera.cc src/camera_models/ScaramuzzaCamera.cc src/sparse_graph/Transform.cc src/gpl/gpl.cc src/gpl/EigenQuaternionParameterization.cc) target_link_libraries(Calibration ${Boost_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES}) target_link_libraries(camera_model ${Boost_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES}) ================================================ FILE: camera_model/cmake/FindEigen.cmake ================================================ # Ceres Solver - A fast non-linear least squares minimizer # Copyright 2015 Google Inc. All rights reserved. # http://ceres-solver.org/ # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: # # * Redistributions of source code must retain the above copyright notice, # this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above copyright notice, # this list of conditions and the following disclaimer in the documentation # and/or other materials provided with the distribution. # * Neither the name of Google Inc. nor the names of its contributors may be # used to endorse or promote products derived from this software without # specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. # # Author: alexs.mac@gmail.com (Alex Stewart) # # FindEigen.cmake - Find Eigen library, version >= 3. # # This module defines the following variables: # # EIGEN_FOUND: TRUE iff Eigen is found. # EIGEN_INCLUDE_DIRS: Include directories for Eigen. # # EIGEN_VERSION: Extracted from Eigen/src/Core/util/Macros.h # EIGEN_WORLD_VERSION: Equal to 3 if EIGEN_VERSION = 3.2.0 # EIGEN_MAJOR_VERSION: Equal to 2 if EIGEN_VERSION = 3.2.0 # EIGEN_MINOR_VERSION: Equal to 0 if EIGEN_VERSION = 3.2.0 # # The following variables control the behaviour of this module: # # EIGEN_INCLUDE_DIR_HINTS: List of additional directories in which to # search for eigen includes, e.g: /timbuktu/eigen3. # # The following variables are also defined by this module, but in line with # CMake recommended FindPackage() module style should NOT be referenced directly # by callers (use the plural variables detailed above instead). These variables # do however affect the behaviour of the module via FIND_[PATH/LIBRARY]() which # are NOT re-called (i.e. search for library is not repeated) if these variables # are set with valid values _in the CMake cache_. This means that if these # variables are set directly in the cache, either by the user in the CMake GUI, # or by the user passing -DVAR=VALUE directives to CMake when called (which # explicitly defines a cache variable), then they will be used verbatim, # bypassing the HINTS variables and other hard-coded search locations. # # EIGEN_INCLUDE_DIR: Include directory for CXSparse, not including the # include directory of any dependencies. # Called if we failed to find Eigen or any of it's required dependencies, # unsets all public (designed to be used externally) variables and reports # error message at priority depending upon [REQUIRED/QUIET/] argument. macro(EIGEN_REPORT_NOT_FOUND REASON_MSG) unset(EIGEN_FOUND) unset(EIGEN_INCLUDE_DIRS) # Make results of search visible in the CMake GUI if Eigen has not # been found so that user does not have to toggle to advanced view. mark_as_advanced(CLEAR EIGEN_INCLUDE_DIR) # Note _FIND_[REQUIRED/QUIETLY] variables defined by FindPackage() # use the camelcase library name, not uppercase. if (Eigen_FIND_QUIETLY) message(STATUS "Failed to find Eigen - " ${REASON_MSG} ${ARGN}) elseif (Eigen_FIND_REQUIRED) message(FATAL_ERROR "Failed to find Eigen - " ${REASON_MSG} ${ARGN}) else() # Neither QUIETLY nor REQUIRED, use no priority which emits a message # but continues configuration and allows generation. message("-- Failed to find Eigen - " ${REASON_MSG} ${ARGN}) endif () return() endmacro(EIGEN_REPORT_NOT_FOUND) # Protect against any alternative find_package scripts for this library having # been called previously (in a client project) which set EIGEN_FOUND, but not # the other variables we require / set here which could cause the search logic # here to fail. unset(EIGEN_FOUND) # Search user-installed locations first, so that we prefer user installs # to system installs where both exist. list(APPEND EIGEN_CHECK_INCLUDE_DIRS /usr/local/include /usr/local/homebrew/include # Mac OS X /opt/local/var/macports/software # Mac OS X. /opt/local/include /usr/include) # Additional suffixes to try appending to each search path. list(APPEND EIGEN_CHECK_PATH_SUFFIXES eigen3 # Default root directory for Eigen. Eigen/include/eigen3 # Windows (for C:/Program Files prefix) < 3.3 Eigen3/include/eigen3 ) # Windows (for C:/Program Files prefix) >= 3.3 # Search supplied hint directories first if supplied. find_path(EIGEN_INCLUDE_DIR NAMES Eigen/Core PATHS ${EIGEN_INCLUDE_DIR_HINTS} ${EIGEN_CHECK_INCLUDE_DIRS} PATH_SUFFIXES ${EIGEN_CHECK_PATH_SUFFIXES}) if (NOT EIGEN_INCLUDE_DIR OR NOT EXISTS ${EIGEN_INCLUDE_DIR}) eigen_report_not_found( "Could not find eigen3 include directory, set EIGEN_INCLUDE_DIR to " "path to eigen3 include directory, e.g. /usr/local/include/eigen3.") endif (NOT EIGEN_INCLUDE_DIR OR NOT EXISTS ${EIGEN_INCLUDE_DIR}) # Mark internally as found, then verify. EIGEN_REPORT_NOT_FOUND() unsets # if called. set(EIGEN_FOUND TRUE) # Extract Eigen version from Eigen/src/Core/util/Macros.h if (EIGEN_INCLUDE_DIR) set(EIGEN_VERSION_FILE ${EIGEN_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h) if (NOT EXISTS ${EIGEN_VERSION_FILE}) eigen_report_not_found( "Could not find file: ${EIGEN_VERSION_FILE} " "containing version information in Eigen install located at: " "${EIGEN_INCLUDE_DIR}.") else (NOT EXISTS ${EIGEN_VERSION_FILE}) file(READ ${EIGEN_VERSION_FILE} EIGEN_VERSION_FILE_CONTENTS) string(REGEX MATCH "#define EIGEN_WORLD_VERSION [0-9]+" EIGEN_WORLD_VERSION "${EIGEN_VERSION_FILE_CONTENTS}") string(REGEX REPLACE "#define EIGEN_WORLD_VERSION ([0-9]+)" "\\1" EIGEN_WORLD_VERSION "${EIGEN_WORLD_VERSION}") string(REGEX MATCH "#define EIGEN_MAJOR_VERSION [0-9]+" EIGEN_MAJOR_VERSION "${EIGEN_VERSION_FILE_CONTENTS}") string(REGEX REPLACE "#define EIGEN_MAJOR_VERSION ([0-9]+)" "\\1" EIGEN_MAJOR_VERSION "${EIGEN_MAJOR_VERSION}") string(REGEX MATCH "#define EIGEN_MINOR_VERSION [0-9]+" EIGEN_MINOR_VERSION "${EIGEN_VERSION_FILE_CONTENTS}") string(REGEX REPLACE "#define EIGEN_MINOR_VERSION ([0-9]+)" "\\1" EIGEN_MINOR_VERSION "${EIGEN_MINOR_VERSION}") # This is on a single line s/t CMake does not interpret it as a list of # elements and insert ';' separators which would result in 3.;2.;0 nonsense. set(EIGEN_VERSION "${EIGEN_WORLD_VERSION}.${EIGEN_MAJOR_VERSION}.${EIGEN_MINOR_VERSION}") endif (NOT EXISTS ${EIGEN_VERSION_FILE}) endif (EIGEN_INCLUDE_DIR) # Set standard CMake FindPackage variables if found. if (EIGEN_FOUND) set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR}) endif (EIGEN_FOUND) # Handle REQUIRED / QUIET optional arguments and version. include(FindPackageHandleStandardArgs) find_package_handle_standard_args(Eigen REQUIRED_VARS EIGEN_INCLUDE_DIRS VERSION_VAR EIGEN_VERSION) # Only mark internal variables as advanced if we found Eigen, otherwise # leave it visible in the standard GUI for the user to set manually. if (EIGEN_FOUND) mark_as_advanced(FORCE EIGEN_INCLUDE_DIR) endif (EIGEN_FOUND) ================================================ FILE: camera_model/include/camodocal/calib/CameraCalibration.h ================================================ #ifndef CAMERACALIBRATION_H #define CAMERACALIBRATION_H #include #include "camodocal/camera_models/Camera.h" namespace camodocal { class CameraCalibration { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW CameraCalibration(); CameraCalibration(Camera::ModelType modelType, const std::string& cameraName, const cv::Size& imageSize, const cv::Size& boardSize, float squareSize); void clear(void); void addChessboardData(const std::vector& corners); bool calibrate(void); int sampleCount(void) const; std::vector >& imagePoints(void); const std::vector >& imagePoints(void) const; std::vector >& scenePoints(void); const std::vector >& scenePoints(void) const; CameraPtr& camera(void); const CameraConstPtr camera(void) const; Eigen::Matrix2d& measurementCovariance(void); const Eigen::Matrix2d& measurementCovariance(void) const; cv::Mat& cameraPoses(void); const cv::Mat& cameraPoses(void) const; void drawResults(std::vector& images) const; void writeParams(const std::string& filename) const; bool writeChessboardData(const std::string& filename) const; bool readChessboardData(const std::string& filename); void setVerbose(bool verbose); private: bool calibrateHelper(CameraPtr& camera, std::vector& rvecs, std::vector& tvecs) const; void optimize(CameraPtr& camera, std::vector& rvecs, std::vector& tvecs) const; template void readData(std::ifstream& ifs, T& data) const; template void writeData(std::ofstream& ofs, T data) const; cv::Size m_boardSize; float m_squareSize; CameraPtr m_camera; cv::Mat m_cameraPoses; std::vector > m_imagePoints; std::vector > m_scenePoints; Eigen::Matrix2d m_measurementCovariance; bool m_verbose; }; } #endif ================================================ FILE: camera_model/include/camodocal/camera_models/Camera.h ================================================ #ifndef CAMERA_H #define CAMERA_H #include #include #include #include namespace camodocal { class Camera { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW enum ModelType { KANNALA_BRANDT, MEI, PINHOLE, SCARAMUZZA }; class Parameters { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW Parameters(ModelType modelType); Parameters(ModelType modelType, const std::string& cameraName, int w, int h); ModelType& modelType(void); std::string& cameraName(void); int& imageWidth(void); int& imageHeight(void); ModelType modelType(void) const; const std::string& cameraName(void) const; int imageWidth(void) const; int imageHeight(void) const; int nIntrinsics(void) const; virtual bool readFromYamlFile(const std::string& filename) = 0; virtual void writeToYamlFile(const std::string& filename) const = 0; protected: ModelType m_modelType; int m_nIntrinsics; std::string m_cameraName; int m_imageWidth; int m_imageHeight; }; virtual ModelType modelType(void) const = 0; virtual const std::string& cameraName(void) const = 0; virtual int imageWidth(void) const = 0; virtual int imageHeight(void) const = 0; virtual cv::Mat& mask(void); virtual const cv::Mat& mask(void) const; virtual void estimateIntrinsics(const cv::Size& boardSize, const std::vector< std::vector >& objectPoints, const std::vector< std::vector >& imagePoints) = 0; virtual void estimateExtrinsics(const std::vector& objectPoints, const std::vector& imagePoints, cv::Mat& rvec, cv::Mat& tvec) const; // Lift points from the image plane to the sphere virtual void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const = 0; //%output P // Lift points from the image plane to the projective space virtual void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const = 0; //%output P // Projects 3D points to the image plane (Pi function) virtual void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const = 0; //%output p // Projects 3D points to the image plane (Pi function) // and calculates jacobian //virtual void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p, // Eigen::Matrix& J) const = 0; //%output p //%output J virtual void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const = 0; //%output p //virtual void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0) const = 0; virtual cv::Mat initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2, float fx = -1.0f, float fy = -1.0f, cv::Size imageSize = cv::Size(0, 0), float cx = -1.0f, float cy = -1.0f, cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const = 0; virtual int parameterCount(void) const = 0; virtual void readParameters(const std::vector& parameters) = 0; virtual void writeParameters(std::vector& parameters) const = 0; virtual void writeParametersToYamlFile(const std::string& filename) const = 0; virtual std::string parametersToString(void) const = 0; /** * \brief Calculates the reprojection distance between points * * \param P1 first 3D point coordinates * \param P2 second 3D point coordinates * \return euclidean distance in the plane */ double reprojectionDist(const Eigen::Vector3d& P1, const Eigen::Vector3d& P2) const; double reprojectionError(const std::vector< std::vector >& objectPoints, const std::vector< std::vector >& imagePoints, const std::vector& rvecs, const std::vector& tvecs, cv::OutputArray perViewErrors = cv::noArray()) const; double reprojectionError(const Eigen::Vector3d& P, const Eigen::Quaterniond& camera_q, const Eigen::Vector3d& camera_t, const Eigen::Vector2d& observed_p) const; void projectPoints(const std::vector& objectPoints, const cv::Mat& rvec, const cv::Mat& tvec, std::vector& imagePoints) const; protected: cv::Mat m_mask; }; typedef boost::shared_ptr CameraPtr; typedef boost::shared_ptr CameraConstPtr; } #endif ================================================ FILE: camera_model/include/camodocal/camera_models/CameraFactory.h ================================================ #ifndef CAMERAFACTORY_H #define CAMERAFACTORY_H #include #include #include "camodocal/camera_models/Camera.h" namespace camodocal { class CameraFactory { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW CameraFactory(); static boost::shared_ptr instance(void); CameraPtr generateCamera(Camera::ModelType modelType, const std::string& cameraName, cv::Size imageSize) const; CameraPtr generateCameraFromYamlFile(const std::string& filename); private: static boost::shared_ptr m_instance; }; } #endif ================================================ FILE: camera_model/include/camodocal/camera_models/CataCamera.h ================================================ #ifndef CATACAMERA_H #define CATACAMERA_H #include #include #include "ceres/rotation.h" #include "Camera.h" namespace camodocal { /** * C. Mei, and P. Rives, Single View Point Omnidirectional Camera Calibration * from Planar Grids, ICRA 2007 */ class CataCamera: public Camera { public: class Parameters: public Camera::Parameters { public: Parameters(); Parameters(const std::string& cameraName, int w, int h, double xi, double k1, double k2, double p1, double p2, double gamma1, double gamma2, double u0, double v0); double& xi(void); double& k1(void); double& k2(void); double& p1(void); double& p2(void); double& gamma1(void); double& gamma2(void); double& u0(void); double& v0(void); double xi(void) const; double k1(void) const; double k2(void) const; double p1(void) const; double p2(void) const; double gamma1(void) const; double gamma2(void) const; double u0(void) const; double v0(void) const; bool readFromYamlFile(const std::string& filename); void writeToYamlFile(const std::string& filename) const; Parameters& operator=(const Parameters& other); friend std::ostream& operator<< (std::ostream& out, const Parameters& params); private: double m_xi; double m_k1; double m_k2; double m_p1; double m_p2; double m_gamma1; double m_gamma2; double m_u0; double m_v0; }; CataCamera(); /** * \brief Constructor from the projection model parameters */ CataCamera(const std::string& cameraName, int imageWidth, int imageHeight, double xi, double k1, double k2, double p1, double p2, double gamma1, double gamma2, double u0, double v0); /** * \brief Constructor from the projection model parameters */ CataCamera(const Parameters& params); Camera::ModelType modelType(void) const; const std::string& cameraName(void) const; int imageWidth(void) const; int imageHeight(void) const; void estimateIntrinsics(const cv::Size& boardSize, const std::vector< std::vector >& objectPoints, const std::vector< std::vector >& imagePoints); // Lift points from the image plane to the sphere void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const; //%output P // Lift points from the image plane to the projective space void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const; //%output P // Projects 3D points to the image plane (Pi function) void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const; //%output p // Projects 3D points to the image plane (Pi function) // and calculates jacobian void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p, Eigen::Matrix& J) const; //%output p //%output J void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const; //%output p template static void spaceToPlane(const T* const params, const T* const q, const T* const t, const Eigen::Matrix& P, Eigen::Matrix& p); void distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u) const; void distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u, Eigen::Matrix2d& J) const; void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0) const; cv::Mat initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2, float fx = -1.0f, float fy = -1.0f, cv::Size imageSize = cv::Size(0, 0), float cx = -1.0f, float cy = -1.0f, cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const; int parameterCount(void) const; const Parameters& getParameters(void) const; void setParameters(const Parameters& parameters); void readParameters(const std::vector& parameterVec); void writeParameters(std::vector& parameterVec) const; void writeParametersToYamlFile(const std::string& filename) const; std::string parametersToString(void) const; private: Parameters mParameters; double m_inv_K11, m_inv_K13, m_inv_K22, m_inv_K23; bool m_noDistortion; }; typedef boost::shared_ptr CataCameraPtr; typedef boost::shared_ptr CataCameraConstPtr; template void CataCamera::spaceToPlane(const T* const params, const T* const q, const T* const t, const Eigen::Matrix& P, Eigen::Matrix& p) { T P_w[3]; P_w[0] = T(P(0)); P_w[1] = T(P(1)); P_w[2] = T(P(2)); // Convert quaternion from Eigen convention (x, y, z, w) // to Ceres convention (w, x, y, z) T q_ceres[4] = {q[3], q[0], q[1], q[2]}; T P_c[3]; ceres::QuaternionRotatePoint(q_ceres, P_w, P_c); P_c[0] += t[0]; P_c[1] += t[1]; P_c[2] += t[2]; // project 3D object point to the image plane T xi = params[0]; T k1 = params[1]; T k2 = params[2]; T p1 = params[3]; T p2 = params[4]; T gamma1 = params[5]; T gamma2 = params[6]; T alpha = T(0); //cameraParams.alpha(); T u0 = params[7]; T v0 = params[8]; // Transform to model plane T len = sqrt(P_c[0] * P_c[0] + P_c[1] * P_c[1] + P_c[2] * P_c[2]); P_c[0] /= len; P_c[1] /= len; P_c[2] /= len; T u = P_c[0] / (P_c[2] + xi); T v = P_c[1] / (P_c[2] + xi); T rho_sqr = u * u + v * v; T L = T(1.0) + k1 * rho_sqr + k2 * rho_sqr * rho_sqr; T du = T(2.0) * p1 * u * v + p2 * (rho_sqr + T(2.0) * u * u); T dv = p1 * (rho_sqr + T(2.0) * v * v) + T(2.0) * p2 * u * v; u = L * u + du; v = L * v + dv; p(0) = gamma1 * (u + alpha * v) + u0; p(1) = gamma2 * v + v0; } } #endif ================================================ FILE: camera_model/include/camodocal/camera_models/CostFunctionFactory.h ================================================ #ifndef COSTFUNCTIONFACTORY_H #define COSTFUNCTIONFACTORY_H #include #include #include "camodocal/camera_models/Camera.h" namespace ceres { class CostFunction; } namespace camodocal { enum { CAMERA_INTRINSICS = 1 << 0, CAMERA_POSE = 1 << 1, POINT_3D = 1 << 2, ODOMETRY_INTRINSICS = 1 << 3, ODOMETRY_3D_POSE = 1 << 4, ODOMETRY_6D_POSE = 1 << 5, CAMERA_ODOMETRY_TRANSFORM = 1 << 6 }; class CostFunctionFactory { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW CostFunctionFactory(); static boost::shared_ptr instance(void); ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera, const Eigen::Vector3d& observed_P, const Eigen::Vector2d& observed_p, int flags) const; ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera, const Eigen::Vector3d& observed_P, const Eigen::Vector2d& observed_p, const Eigen::Matrix2d& sqrtPrecisionMat, int flags) const; ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera, const Eigen::Vector2d& observed_p, int flags, bool optimize_cam_odo_z = true) const; ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera, const Eigen::Vector2d& observed_p, const Eigen::Matrix2d& sqrtPrecisionMat, int flags, bool optimize_cam_odo_z = true) const; ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera, const Eigen::Vector3d& odo_pos, const Eigen::Vector3d& odo_att, const Eigen::Vector2d& observed_p, int flags, bool optimize_cam_odo_z = true) const; ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera, const Eigen::Quaterniond& cam_odo_q, const Eigen::Vector3d& cam_odo_t, const Eigen::Vector3d& odo_pos, const Eigen::Vector3d& odo_att, const Eigen::Vector2d& observed_p, int flags) const; ceres::CostFunction* generateCostFunction(const CameraConstPtr& cameraLeft, const CameraConstPtr& cameraRight, const Eigen::Vector3d& observed_P, const Eigen::Vector2d& observed_p_left, const Eigen::Vector2d& observed_p_right) const; private: static boost::shared_ptr m_instance; }; } #endif ================================================ FILE: camera_model/include/camodocal/camera_models/EquidistantCamera.h ================================================ #ifndef EQUIDISTANTCAMERA_H #define EQUIDISTANTCAMERA_H #include #include #include "ceres/rotation.h" #include "Camera.h" namespace camodocal { /** * J. Kannala, and S. Brandt, A Generic Camera Model and Calibration Method * for Conventional, Wide-Angle, and Fish-Eye Lenses, PAMI 2006 */ class EquidistantCamera: public Camera { public: class Parameters: public Camera::Parameters { public: Parameters(); Parameters(const std::string& cameraName, int w, int h, double k2, double k3, double k4, double k5, double mu, double mv, double u0, double v0); double& k2(void); double& k3(void); double& k4(void); double& k5(void); double& mu(void); double& mv(void); double& u0(void); double& v0(void); double k2(void) const; double k3(void) const; double k4(void) const; double k5(void) const; double mu(void) const; double mv(void) const; double u0(void) const; double v0(void) const; bool readFromYamlFile(const std::string& filename); void writeToYamlFile(const std::string& filename) const; Parameters& operator=(const Parameters& other); friend std::ostream& operator<< (std::ostream& out, const Parameters& params); private: // projection double m_k2; double m_k3; double m_k4; double m_k5; double m_mu; double m_mv; double m_u0; double m_v0; }; EquidistantCamera(); /** * \brief Constructor from the projection model parameters */ EquidistantCamera(const std::string& cameraName, int imageWidth, int imageHeight, double k2, double k3, double k4, double k5, double mu, double mv, double u0, double v0); /** * \brief Constructor from the projection model parameters */ EquidistantCamera(const Parameters& params); Camera::ModelType modelType(void) const; const std::string& cameraName(void) const; int imageWidth(void) const; int imageHeight(void) const; void estimateIntrinsics(const cv::Size& boardSize, const std::vector< std::vector >& objectPoints, const std::vector< std::vector >& imagePoints); // Lift points from the image plane to the sphere virtual void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const; //%output P // Lift points from the image plane to the projective space void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const; //%output P // Projects 3D points to the image plane (Pi function) void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const; //%output p // Projects 3D points to the image plane (Pi function) // and calculates jacobian void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p, Eigen::Matrix& J) const; //%output p //%output J void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const; //%output p template static void spaceToPlane(const T* const params, const T* const q, const T* const t, const Eigen::Matrix& P, Eigen::Matrix& p); void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0) const; cv::Mat initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2, float fx = -1.0f, float fy = -1.0f, cv::Size imageSize = cv::Size(0, 0), float cx = -1.0f, float cy = -1.0f, cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const; int parameterCount(void) const; const Parameters& getParameters(void) const; void setParameters(const Parameters& parameters); void readParameters(const std::vector& parameterVec); void writeParameters(std::vector& parameterVec) const; void writeParametersToYamlFile(const std::string& filename) const; std::string parametersToString(void) const; private: template static T r(T k2, T k3, T k4, T k5, T theta); void fitOddPoly(const std::vector& x, const std::vector& y, int n, std::vector& coeffs) const; void backprojectSymmetric(const Eigen::Vector2d& p_u, double& theta, double& phi) const; Parameters mParameters; double m_inv_K11, m_inv_K13, m_inv_K22, m_inv_K23; }; typedef boost::shared_ptr EquidistantCameraPtr; typedef boost::shared_ptr EquidistantCameraConstPtr; template T EquidistantCamera::r(T k2, T k3, T k4, T k5, T theta) { // k1 = 1 return theta + k2 * theta * theta * theta + k3 * theta * theta * theta * theta * theta + k4 * theta * theta * theta * theta * theta * theta * theta + k5 * theta * theta * theta * theta * theta * theta * theta * theta * theta; } template void EquidistantCamera::spaceToPlane(const T* const params, const T* const q, const T* const t, const Eigen::Matrix& P, Eigen::Matrix& p) { T P_w[3]; P_w[0] = T(P(0)); P_w[1] = T(P(1)); P_w[2] = T(P(2)); // Convert quaternion from Eigen convention (x, y, z, w) // to Ceres convention (w, x, y, z) T q_ceres[4] = {q[3], q[0], q[1], q[2]}; T P_c[3]; ceres::QuaternionRotatePoint(q_ceres, P_w, P_c); P_c[0] += t[0]; P_c[1] += t[1]; P_c[2] += t[2]; // project 3D object point to the image plane; T k2 = params[0]; T k3 = params[1]; T k4 = params[2]; T k5 = params[3]; T mu = params[4]; T mv = params[5]; T u0 = params[6]; T v0 = params[7]; T len = sqrt(P_c[0] * P_c[0] + P_c[1] * P_c[1] + P_c[2] * P_c[2]); T theta = acos(P_c[2] / len); T phi = atan2(P_c[1], P_c[0]); Eigen::Matrix p_u = r(k2, k3, k4, k5, theta) * Eigen::Matrix(cos(phi), sin(phi)); p(0) = mu * p_u(0) + u0; p(1) = mv * p_u(1) + v0; } } #endif ================================================ FILE: camera_model/include/camodocal/camera_models/PinholeCamera.h ================================================ #ifndef PINHOLECAMERA_H #define PINHOLECAMERA_H #include #include #include "ceres/rotation.h" #include "Camera.h" namespace camodocal { class PinholeCamera: public Camera { public: class Parameters: public Camera::Parameters { public: Parameters(); Parameters(const std::string& cameraName, int w, int h, double k1, double k2, double p1, double p2, double fx, double fy, double cx, double cy); double& k1(void); double& k2(void); double& p1(void); double& p2(void); double& fx(void); double& fy(void); double& cx(void); double& cy(void); double xi(void) const; double k1(void) const; double k2(void) const; double p1(void) const; double p2(void) const; double fx(void) const; double fy(void) const; double cx(void) const; double cy(void) const; bool readFromYamlFile(const std::string& filename); void writeToYamlFile(const std::string& filename) const; Parameters& operator=(const Parameters& other); friend std::ostream& operator<< (std::ostream& out, const Parameters& params); private: double m_k1; double m_k2; double m_p1; double m_p2; double m_fx; double m_fy; double m_cx; double m_cy; }; PinholeCamera(); /** * \brief Constructor from the projection model parameters */ PinholeCamera(const std::string& cameraName, int imageWidth, int imageHeight, double k1, double k2, double p1, double p2, double fx, double fy, double cx, double cy); /** * \brief Constructor from the projection model parameters */ PinholeCamera(const Parameters& params); Camera::ModelType modelType(void) const; const std::string& cameraName(void) const; int imageWidth(void) const; int imageHeight(void) const; void estimateIntrinsics(const cv::Size& boardSize, const std::vector< std::vector >& objectPoints, const std::vector< std::vector >& imagePoints); // Lift points from the image plane to the sphere virtual void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const; //%output P // Lift points from the image plane to the projective space void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const; //%output P // Projects 3D points to the image plane (Pi function) void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const; //%output p // Projects 3D points to the image plane (Pi function) // and calculates jacobian void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p, Eigen::Matrix& J) const; //%output p //%output J void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const; //%output p template static void spaceToPlane(const T* const params, const T* const q, const T* const t, const Eigen::Matrix& P, Eigen::Matrix& p); void distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u) const; void distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u, Eigen::Matrix2d& J) const; void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0) const; cv::Mat initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2, float fx = -1.0f, float fy = -1.0f, cv::Size imageSize = cv::Size(0, 0), float cx = -1.0f, float cy = -1.0f, cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const; int parameterCount(void) const; const Parameters& getParameters(void) const; void setParameters(const Parameters& parameters); void readParameters(const std::vector& parameterVec); void writeParameters(std::vector& parameterVec) const; void writeParametersToYamlFile(const std::string& filename) const; std::string parametersToString(void) const; private: Parameters mParameters; double m_inv_K11, m_inv_K13, m_inv_K22, m_inv_K23; bool m_noDistortion; }; typedef boost::shared_ptr PinholeCameraPtr; typedef boost::shared_ptr PinholeCameraConstPtr; template void PinholeCamera::spaceToPlane(const T* const params, const T* const q, const T* const t, const Eigen::Matrix& P, Eigen::Matrix& p) { T P_w[3]; P_w[0] = T(P(0)); P_w[1] = T(P(1)); P_w[2] = T(P(2)); // Convert quaternion from Eigen convention (x, y, z, w) // to Ceres convention (w, x, y, z) T q_ceres[4] = {q[3], q[0], q[1], q[2]}; T P_c[3]; ceres::QuaternionRotatePoint(q_ceres, P_w, P_c); P_c[0] += t[0]; P_c[1] += t[1]; P_c[2] += t[2]; // project 3D object point to the image plane T k1 = params[0]; T k2 = params[1]; T p1 = params[2]; T p2 = params[3]; T fx = params[4]; T fy = params[5]; T alpha = T(0); //cameraParams.alpha(); T cx = params[6]; T cy = params[7]; // Transform to model plane T u = P_c[0] / P_c[2]; T v = P_c[1] / P_c[2]; T rho_sqr = u * u + v * v; T L = T(1.0) + k1 * rho_sqr + k2 * rho_sqr * rho_sqr; T du = T(2.0) * p1 * u * v + p2 * (rho_sqr + T(2.0) * u * u); T dv = p1 * (rho_sqr + T(2.0) * v * v) + T(2.0) * p2 * u * v; u = L * u + du; v = L * v + dv; p(0) = fx * (u + alpha * v) + cx; p(1) = fy * v + cy; } } #endif ================================================ FILE: camera_model/include/camodocal/camera_models/ScaramuzzaCamera.h ================================================ #ifndef SCARAMUZZACAMERA_H #define SCARAMUZZACAMERA_H #include #include #include "ceres/rotation.h" #include "Camera.h" namespace camodocal { #define SCARAMUZZA_POLY_SIZE 5 #define SCARAMUZZA_INV_POLY_SIZE 20 #define SCARAMUZZA_CAMERA_NUM_PARAMS (SCARAMUZZA_POLY_SIZE + SCARAMUZZA_INV_POLY_SIZE + 2 /*center*/ + 3 /*affine*/) /** * Scaramuzza Camera (Omnidirectional) * https://sites.google.com/site/scarabotix/ocamcalib-toolbox */ class OCAMCamera: public Camera { public: class Parameters: public Camera::Parameters { public: Parameters(); double& C(void) { return m_C; } double& D(void) { return m_D; } double& E(void) { return m_E; } double& center_x(void) { return m_center_x; } double& center_y(void) { return m_center_y; } double& poly(int idx) { return m_poly[idx]; } double& inv_poly(int idx) { return m_inv_poly[idx]; } double C(void) const { return m_C; } double D(void) const { return m_D; } double E(void) const { return m_E; } double center_x(void) const { return m_center_x; } double center_y(void) const { return m_center_y; } double poly(int idx) const { return m_poly[idx]; } double inv_poly(int idx) const { return m_inv_poly[idx]; } bool readFromYamlFile(const std::string& filename); void writeToYamlFile(const std::string& filename) const; Parameters& operator=(const Parameters& other); friend std::ostream& operator<< (std::ostream& out, const Parameters& params); private: double m_poly[SCARAMUZZA_POLY_SIZE]; double m_inv_poly[SCARAMUZZA_INV_POLY_SIZE]; double m_C; double m_D; double m_E; double m_center_x; double m_center_y; }; OCAMCamera(); /** * \brief Constructor from the projection model parameters */ OCAMCamera(const Parameters& params); Camera::ModelType modelType(void) const; const std::string& cameraName(void) const; int imageWidth(void) const; int imageHeight(void) const; void estimateIntrinsics(const cv::Size& boardSize, const std::vector< std::vector >& objectPoints, const std::vector< std::vector >& imagePoints); // Lift points from the image plane to the sphere void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const; //%output P // Lift points from the image plane to the projective space void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const; //%output P // Projects 3D points to the image plane (Pi function) void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const; //%output p // Projects 3D points to the image plane (Pi function) // and calculates jacobian //void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p, // Eigen::Matrix& J) const; //%output p //%output J void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const; //%output p template static void spaceToPlane(const T* const params, const T* const q, const T* const t, const Eigen::Matrix& P, Eigen::Matrix& p); template static void spaceToSphere(const T* const params, const T* const q, const T* const t, const Eigen::Matrix& P, Eigen::Matrix& P_s); template static void LiftToSphere(const T* const params, const Eigen::Matrix& p, Eigen::Matrix& P); template static void SphereToPlane(const T* const params, const Eigen::Matrix& P, Eigen::Matrix& p); void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0) const; cv::Mat initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2, float fx = -1.0f, float fy = -1.0f, cv::Size imageSize = cv::Size(0, 0), float cx = -1.0f, float cy = -1.0f, cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const; int parameterCount(void) const; const Parameters& getParameters(void) const; void setParameters(const Parameters& parameters); void readParameters(const std::vector& parameterVec); void writeParameters(std::vector& parameterVec) const; void writeParametersToYamlFile(const std::string& filename) const; std::string parametersToString(void) const; private: Parameters mParameters; double m_inv_scale; }; typedef boost::shared_ptr OCAMCameraPtr; typedef boost::shared_ptr OCAMCameraConstPtr; template void OCAMCamera::spaceToPlane(const T* const params, const T* const q, const T* const t, const Eigen::Matrix& P, Eigen::Matrix& p) { T P_c[3]; { T P_w[3]; P_w[0] = T(P(0)); P_w[1] = T(P(1)); P_w[2] = T(P(2)); // Convert quaternion from Eigen convention (x, y, z, w) // to Ceres convention (w, x, y, z) T q_ceres[4] = {q[3], q[0], q[1], q[2]}; ceres::QuaternionRotatePoint(q_ceres, P_w, P_c); P_c[0] += t[0]; P_c[1] += t[1]; P_c[2] += t[2]; } T c = params[0]; T d = params[1]; T e = params[2]; T xc[2] = { params[3], params[4] }; //T poly[SCARAMUZZA_POLY_SIZE]; //for (int i=0; i < SCARAMUZZA_POLY_SIZE; i++) // poly[i] = params[5+i]; T inv_poly[SCARAMUZZA_INV_POLY_SIZE]; for (int i=0; i < SCARAMUZZA_INV_POLY_SIZE; i++) inv_poly[i] = params[5 + SCARAMUZZA_POLY_SIZE + i]; T norm_sqr = P_c[0] * P_c[0] + P_c[1] * P_c[1]; T norm = T(0.0); if (norm_sqr > T(0.0)) norm = sqrt(norm_sqr); T theta = atan2(-P_c[2], norm); T rho = T(0.0); T theta_i = T(1.0); for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++) { rho += theta_i * inv_poly[i]; theta_i *= theta; } T invNorm = T(1.0) / norm; T xn[2] = { P_c[0] * invNorm * rho, P_c[1] * invNorm * rho }; p(0) = xn[0] * c + xn[1] * d + xc[0]; p(1) = xn[0] * e + xn[1] + xc[1]; } template void OCAMCamera::spaceToSphere(const T* const params, const T* const q, const T* const t, const Eigen::Matrix& P, Eigen::Matrix& P_s) { T P_c[3]; { T P_w[3]; P_w[0] = T(P(0)); P_w[1] = T(P(1)); P_w[2] = T(P(2)); // Convert quaternion from Eigen convention (x, y, z, w) // to Ceres convention (w, x, y, z) T q_ceres[4] = {q[3], q[0], q[1], q[2]}; ceres::QuaternionRotatePoint(q_ceres, P_w, P_c); P_c[0] += t[0]; P_c[1] += t[1]; P_c[2] += t[2]; } //T poly[SCARAMUZZA_POLY_SIZE]; //for (int i=0; i < SCARAMUZZA_POLY_SIZE; i++) // poly[i] = params[5+i]; T norm_sqr = P_c[0] * P_c[0] + P_c[1] * P_c[1] + P_c[2] * P_c[2]; T norm = T(0.0); if (norm_sqr > T(0.0)) norm = sqrt(norm_sqr); P_s(0) = P_c[0] / norm; P_s(1) = P_c[1] / norm; P_s(2) = P_c[2] / norm; } template void OCAMCamera::LiftToSphere(const T* const params, const Eigen::Matrix& p, Eigen::Matrix& P) { T c = params[0]; T d = params[1]; T e = params[2]; T cc[2] = { params[3], params[4] }; T poly[SCARAMUZZA_POLY_SIZE]; for (int i=0; i < SCARAMUZZA_POLY_SIZE; i++) poly[i] = params[5+i]; // Relative to Center T p_2d[2]; p_2d[0] = T(p(0)); p_2d[1] = T(p(1)); T xc[2] = { p_2d[0] - cc[0], p_2d[1] - cc[1]}; T inv_scale = T(1.0) / (c - d * e); // Affine Transformation T xc_a[2]; xc_a[0] = inv_scale * (xc[0] - d * xc[1]); xc_a[1] = inv_scale * (-e * xc[0] + c * xc[1]); T norm_sqr = xc_a[0] * xc_a[0] + xc_a[1] * xc_a[1]; T phi = sqrt(norm_sqr); T phi_i = T(1.0); T z = T(0.0); for (int i = 0; i < SCARAMUZZA_POLY_SIZE; i++) { if (i!=1) { z += phi_i * poly[i]; } phi_i *= phi; } T p_3d[3]; p_3d[0] = xc[0]; p_3d[1] = xc[1]; p_3d[2] = -z; T p_3d_norm_sqr = p_3d[0] * p_3d[0] + p_3d[1] * p_3d[1] + p_3d[2] * p_3d[2]; T p_3d_norm = sqrt(p_3d_norm_sqr); P << p_3d[0] / p_3d_norm, p_3d[1] / p_3d_norm, p_3d[2] / p_3d_norm; } template void OCAMCamera::SphereToPlane(const T* const params, const Eigen::Matrix& P, Eigen::Matrix& p) { T P_c[3]; { P_c[0] = T(P(0)); P_c[1] = T(P(1)); P_c[2] = T(P(2)); } T c = params[0]; T d = params[1]; T e = params[2]; T xc[2] = {params[3], params[4]}; T inv_poly[SCARAMUZZA_INV_POLY_SIZE]; for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++) inv_poly[i] = params[5 + SCARAMUZZA_POLY_SIZE + i]; T norm_sqr = P_c[0] * P_c[0] + P_c[1] * P_c[1]; T norm = T(0.0); if (norm_sqr > T(0.0)) norm = sqrt(norm_sqr); T theta = atan2(-P_c[2], norm); T rho = T(0.0); T theta_i = T(1.0); for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++) { rho += theta_i * inv_poly[i]; theta_i *= theta; } T invNorm = T(1.0) / norm; T xn[2] = {P_c[0] * invNorm * rho, P_c[1] * invNorm * rho}; p(0) = xn[0] * c + xn[1] * d + xc[0]; p(1) = xn[0] * e + xn[1] + xc[1]; } } #endif ================================================ FILE: camera_model/include/camodocal/chessboard/Chessboard.h ================================================ #ifndef CHESSBOARD_H #define CHESSBOARD_H #include #include namespace camodocal { // forward declarations class ChessboardCorner; typedef boost::shared_ptr ChessboardCornerPtr; class ChessboardQuad; typedef boost::shared_ptr ChessboardQuadPtr; class Chessboard { public: Chessboard(cv::Size boardSize, cv::Mat& image); void findCorners(bool useOpenCV = false); const std::vector& getCorners(void) const; bool cornersFound(void) const; const cv::Mat& getImage(void) const; const cv::Mat& getSketch(void) const; private: bool findChessboardCorners(const cv::Mat& image, const cv::Size& patternSize, std::vector& corners, int flags, bool useOpenCV); bool findChessboardCornersImproved(const cv::Mat& image, const cv::Size& patternSize, std::vector& corners, int flags); void cleanFoundConnectedQuads(std::vector& quadGroup, cv::Size patternSize); void findConnectedQuads(std::vector& quads, std::vector& group, int group_idx, int dilation); // int checkQuadGroup(std::vector& quadGroup, // std::vector& outCorners, // cv::Size patternSize); void labelQuadGroup(std::vector& quad_group, cv::Size patternSize, bool firstRun); void findQuadNeighbors(std::vector& quads, int dilation); int augmentBestRun(std::vector& candidateQuads, int candidateDilation, std::vector& existingQuads, int existingDilation); void generateQuads(std::vector& quads, cv::Mat& image, int flags, int dilation, bool firstRun); bool checkQuadGroup(std::vector& quads, std::vector& corners, cv::Size patternSize); void getQuadrangleHypotheses(const std::vector< std::vector >& contours, std::vector< std::pair >& quads, int classId) const; bool checkChessboard(const cv::Mat& image, cv::Size patternSize) const; bool checkBoardMonotony(std::vector& corners, cv::Size patternSize); bool matchCorners(ChessboardQuadPtr& quad1, int corner1, ChessboardQuadPtr& quad2, int corner2) const; cv::Mat mImage; cv::Mat mSketch; std::vector mCorners; cv::Size mBoardSize; bool mCornersFound; }; } #endif ================================================ FILE: camera_model/include/camodocal/chessboard/ChessboardCorner.h ================================================ #ifndef CHESSBOARDCORNER_H #define CHESSBOARDCORNER_H #include #include namespace camodocal { class ChessboardCorner; typedef boost::shared_ptr ChessboardCornerPtr; class ChessboardCorner { public: ChessboardCorner() : row(0), column(0), needsNeighbor(true), count(0) {} float meanDist(int &n) const { float sum = 0; n = 0; for (int i = 0; i < 4; ++i) { if (neighbors[i].get()) { float dx = neighbors[i]->pt.x - pt.x; float dy = neighbors[i]->pt.y - pt.y; sum += sqrt(dx*dx + dy*dy); n++; } } return sum / std::max(n, 1); } cv::Point2f pt; // X and y coordinates int row; // Row and column of the corner int column; // in the found pattern bool needsNeighbor; // Does the corner require a neighbor? int count; // number of corner neighbors ChessboardCornerPtr neighbors[4]; // pointer to all corner neighbors }; } #endif ================================================ FILE: camera_model/include/camodocal/chessboard/ChessboardQuad.h ================================================ #ifndef CHESSBOARDQUAD_H #define CHESSBOARDQUAD_H #include #include "camodocal/chessboard/ChessboardCorner.h" namespace camodocal { class ChessboardQuad; typedef boost::shared_ptr ChessboardQuadPtr; class ChessboardQuad { public: ChessboardQuad() : count(0), group_idx(-1), edge_len(FLT_MAX), labeled(false) {} int count; // Number of quad neighbors int group_idx; // Quad group ID float edge_len; // Smallest side length^2 ChessboardCornerPtr corners[4]; // Coordinates of quad corners ChessboardQuadPtr neighbors[4]; // Pointers of quad neighbors bool labeled; // Has this corner been labeled? }; } #endif ================================================ FILE: camera_model/include/camodocal/chessboard/Spline.h ================================================ /* dynamo:- Event driven molecular dynamics simulator http://www.marcusbannerman.co.uk/dynamo Copyright (C) 2011 Marcus N Campbell Bannerman This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License version 3 as published by the Free Software Foundation. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program. If not, see . */ #pragma once #include #include #include #include #include #include namespace ublas = boost::numeric::ublas; class Spline : private std::vector > { public: //The boundary conditions available enum BC_type { FIXED_1ST_DERIV_BC, FIXED_2ND_DERIV_BC, PARABOLIC_RUNOUT_BC }; enum Spline_type { LINEAR, CUBIC }; //Constructor takes the boundary conditions as arguments, this //sets the first derivative (gradient) at the lower and upper //end points Spline(): _valid(false), _BCLow(FIXED_2ND_DERIV_BC), _BCHigh(FIXED_2ND_DERIV_BC), _BCLowVal(0), _BCHighVal(0), _type(CUBIC) {} typedef std::vector > base; typedef base::const_iterator const_iterator; //Standard STL read-only container stuff const_iterator begin() const { return base::begin(); } const_iterator end() const { return base::end(); } void clear() { _valid = false; base::clear(); _data.clear(); } size_t size() const { return base::size(); } size_t max_size() const { return base::max_size(); } size_t capacity() const { return base::capacity(); } bool empty() const { return base::empty(); } //Add a point to the spline, and invalidate it so its //recalculated on the next access inline void addPoint(double x, double y) { _valid = false; base::push_back(std::pair(x,y)); } //Reset the boundary conditions inline void setLowBC(BC_type BC, double val = 0) { _BCLow = BC; _BCLowVal = val; _valid = false; } inline void setHighBC(BC_type BC, double val = 0) { _BCHigh = BC; _BCHighVal = val; _valid = false; } void setType(Spline_type type) { _type = type; _valid = false; } //Check if the spline has been calculated, then generate the //spline interpolated value double operator()(double xval) { if (!_valid) generate(); //Special cases when we're outside the range of the spline points if (xval <= x(0)) return lowCalc(xval); if (xval >= x(size()-1)) return highCalc(xval); //Check all intervals except the last one for (std::vector::const_iterator iPtr = _data.begin(); iPtr != _data.end()-1; ++iPtr) if ((xval >= iPtr->x) && (xval <= (iPtr+1)->x)) return splineCalc(iPtr, xval); return splineCalc(_data.end() - 1, xval); } private: ///////PRIVATE DATA MEMBERS struct SplineData { double x,a,b,c,d; }; //vector of calculated spline data std::vector _data; //Second derivative at each point ublas::vector _ddy; //Tracks whether the spline parameters have been calculated for //the current set of points bool _valid; //The boundary conditions BC_type _BCLow, _BCHigh; //The values of the boundary conditions double _BCLowVal, _BCHighVal; Spline_type _type; ///////PRIVATE FUNCTIONS //Function to calculate the value of a given spline at a point xval inline double splineCalc(std::vector::const_iterator i, double xval) { const double lx = xval - i->x; return ((i->a * lx + i->b) * lx + i->c) * lx + i->d; } inline double lowCalc(double xval) { const double lx = xval - x(0); if (_type == LINEAR) return lx * _BCHighVal + y(0); const double firstDeriv = (y(1) - y(0)) / h(0) - 2 * h(0) * (_data[0].b + 2 * _data[1].b) / 6; switch(_BCLow) { case FIXED_1ST_DERIV_BC: return lx * _BCLowVal + y(0); case FIXED_2ND_DERIV_BC: return lx * lx * _BCLowVal + firstDeriv * lx + y(0); case PARABOLIC_RUNOUT_BC: return lx * lx * _ddy[0] + lx * firstDeriv + y(0); } throw std::runtime_error("Unknown BC"); } inline double highCalc(double xval) { const double lx = xval - x(size() - 1); if (_type == LINEAR) return lx * _BCHighVal + y(size() - 1); const double firstDeriv = 2 * h(size() - 2) * (_ddy[size() - 2] + 2 * _ddy[size() - 1]) / 6 + (y(size() - 1) - y(size() - 2)) / h(size() - 2); switch(_BCHigh) { case FIXED_1ST_DERIV_BC: return lx * _BCHighVal + y(size() - 1); case FIXED_2ND_DERIV_BC: return lx * lx * _BCHighVal + firstDeriv * lx + y(size() - 1); case PARABOLIC_RUNOUT_BC: return lx * lx * _ddy[size()-1] + lx * firstDeriv + y(size() - 1); } throw std::runtime_error("Unknown BC"); } //These just provide access to the point data in a clean way inline double x(size_t i) const { return operator[](i).first; } inline double y(size_t i) const { return operator[](i).second; } inline double h(size_t i) const { return x(i+1) - x(i); } //Invert a arbitrary matrix using the boost ublas library template bool InvertMatrix(ublas::matrix A, ublas::matrix& inverse) { using namespace ublas; // create a permutation matrix for the LU-factorization permutation_matrix pm(A.size1()); // perform LU-factorization int res = lu_factorize(A,pm); if( res != 0 ) return false; // create identity matrix of "inverse" inverse.assign(ublas::identity_matrix(A.size1())); // backsubstitute to get the inverse lu_substitute(A, pm, inverse); return true; } //This function will recalculate the spline parameters and store //them in _data, ready for spline interpolation void generate() { if (size() < 2) throw std::runtime_error("Spline requires at least 2 points"); //If any spline points are at the same x location, we have to //just slightly seperate them { bool testPassed(false); while (!testPassed) { testPassed = true; std::sort(base::begin(), base::end()); for (base::iterator iPtr = base::begin(); iPtr != base::end() - 1; ++iPtr) if (iPtr->first == (iPtr+1)->first) { if ((iPtr+1)->first != 0) (iPtr+1)->first += (iPtr+1)->first * std::numeric_limits::epsilon() * 10; else (iPtr+1)->first = std::numeric_limits::epsilon() * 10; testPassed = false; break; } } } const size_t e = size() - 1; switch (_type) { case LINEAR: { _data.resize(e); for (size_t i(0); i < e; ++i) { _data[i].x = x(i); _data[i].a = 0; _data[i].b = 0; _data[i].c = (y(i+1) - y(i)) / (x(i+1) - x(i)); _data[i].d = y(i); } break; } case CUBIC: { ublas::matrix A(size(), size()); for (size_t yv(0); yv <= e; ++yv) for (size_t xv(0); xv <= e; ++xv) A(xv,yv) = 0; for (size_t i(1); i < e; ++i) { A(i-1,i) = h(i-1); A(i,i) = 2 * (h(i-1) + h(i)); A(i+1,i) = h(i); } ublas::vector C(size()); for (size_t xv(0); xv <= e; ++xv) C(xv) = 0; for (size_t i(1); i < e; ++i) C(i) = 6 * ((y(i+1) - y(i)) / h(i) - (y(i) - y(i-1)) / h(i-1)); //Boundary conditions switch(_BCLow) { case FIXED_1ST_DERIV_BC: C(0) = 6 * ((y(1) - y(0)) / h(0) - _BCLowVal); A(0,0) = 2 * h(0); A(1,0) = h(0); break; case FIXED_2ND_DERIV_BC: C(0) = _BCLowVal; A(0,0) = 1; break; case PARABOLIC_RUNOUT_BC: C(0) = 0; A(0,0) = 1; A(1,0) = -1; break; } switch(_BCHigh) { case FIXED_1ST_DERIV_BC: C(e) = 6 * (_BCHighVal - (y(e) - y(e-1)) / h(e-1)); A(e,e) = 2 * h(e - 1); A(e-1,e) = h(e - 1); break; case FIXED_2ND_DERIV_BC: C(e) = _BCHighVal; A(e,e) = 1; break; case PARABOLIC_RUNOUT_BC: C(e) = 0; A(e,e) = 1; A(e-1,e) = -1; break; } ublas::matrix AInv(size(), size()); InvertMatrix(A,AInv); _ddy = ublas::prod(C, AInv); _data.resize(size()-1); for (size_t i(0); i < e; ++i) { _data[i].x = x(i); _data[i].a = (_ddy(i+1) - _ddy(i)) / (6 * h(i)); _data[i].b = _ddy(i) / 2; _data[i].c = (y(i+1) - y(i)) / h(i) - _ddy(i+1) * h(i) / 6 - _ddy(i) * h(i) / 3; _data[i].d = y(i); } } } _valid = true; } }; ================================================ FILE: camera_model/include/camodocal/gpl/EigenQuaternionParameterization.h ================================================ #ifndef EIGENQUATERNIONPARAMETERIZATION_H #define EIGENQUATERNIONPARAMETERIZATION_H #include "ceres/local_parameterization.h" namespace camodocal { class EigenQuaternionParameterization : public ceres::LocalParameterization { public: virtual ~EigenQuaternionParameterization() {} virtual bool Plus(const double* x, const double* delta, double* x_plus_delta) const; virtual bool ComputeJacobian(const double* x, double* jacobian) const; virtual int GlobalSize() const { return 4; } virtual int LocalSize() const { return 3; } private: template void EigenQuaternionProduct(const T z[4], const T w[4], T zw[4]) const; }; template void EigenQuaternionParameterization::EigenQuaternionProduct(const T z[4], const T w[4], T zw[4]) const { zw[0] = z[3] * w[0] + z[0] * w[3] + z[1] * w[2] - z[2] * w[1]; zw[1] = z[3] * w[1] - z[0] * w[2] + z[1] * w[3] + z[2] * w[0]; zw[2] = z[3] * w[2] + z[0] * w[1] - z[1] * w[0] + z[2] * w[3]; zw[3] = z[3] * w[3] - z[0] * w[0] - z[1] * w[1] - z[2] * w[2]; } } #endif ================================================ FILE: camera_model/include/camodocal/gpl/EigenUtils.h ================================================ #ifndef EIGENUTILS_H #define EIGENUTILS_H #include #include "ceres/rotation.h" #include "camodocal/gpl/gpl.h" namespace camodocal { // Returns the 3D cross product skew symmetric matrix of a given 3D vector template Eigen::Matrix skew(const Eigen::Matrix& vec) { return (Eigen::Matrix() << T(0), -vec(2), vec(1), vec(2), T(0), -vec(0), -vec(1), vec(0), T(0)).finished(); } template typename Eigen::MatrixBase::PlainObject sqrtm(const Eigen::MatrixBase& A) { Eigen::SelfAdjointEigenSolver es(A); return es.operatorSqrt(); } template Eigen::Matrix AngleAxisToRotationMatrix(const Eigen::Matrix& rvec) { T angle = rvec.norm(); if (angle == T(0)) { return Eigen::Matrix::Identity(); } Eigen::Matrix axis; axis = rvec.normalized(); Eigen::Matrix rmat; rmat = Eigen::AngleAxis(angle, axis); return rmat; } template Eigen::Quaternion AngleAxisToQuaternion(const Eigen::Matrix& rvec) { Eigen::Matrix rmat = AngleAxisToRotationMatrix(rvec); return Eigen::Quaternion(rmat); } template void AngleAxisToQuaternion(const Eigen::Matrix& rvec, T* q) { Eigen::Quaternion quat = AngleAxisToQuaternion(rvec); q[0] = quat.x(); q[1] = quat.y(); q[2] = quat.z(); q[3] = quat.w(); } template Eigen::Matrix RotationToAngleAxis(const Eigen::Matrix & rmat) { Eigen::AngleAxis angleaxis; angleaxis.fromRotationMatrix(rmat); return angleaxis.angle() * angleaxis.axis(); } template void QuaternionToAngleAxis(const T* const q, Eigen::Matrix& rvec) { Eigen::Quaternion quat(q[3], q[0], q[1], q[2]); Eigen::Matrix rmat = quat.toRotationMatrix(); Eigen::AngleAxis angleaxis; angleaxis.fromRotationMatrix(rmat); rvec = angleaxis.angle() * angleaxis.axis(); } template Eigen::Matrix QuaternionToRotation(const T* const q) { T R[9]; ceres::QuaternionToRotation(q, R); Eigen::Matrix rmat; for (int i = 0; i < 3; ++i) { for (int j = 0; j < 3; ++j) { rmat(i,j) = R[i * 3 + j]; } } return rmat; } template void QuaternionToRotation(const T* const q, T* rot) { ceres::QuaternionToRotation(q, rot); } template Eigen::Matrix QuaternionMultMatLeft(const Eigen::Quaternion& q) { return (Eigen::Matrix() << q.w(), -q.z(), q.y(), q.x(), q.z(), q.w(), -q.x(), q.y(), -q.y(), q.x(), q.w(), q.z(), -q.x(), -q.y(), -q.z(), q.w()).finished(); } template Eigen::Matrix QuaternionMultMatRight(const Eigen::Quaternion& q) { return (Eigen::Matrix() << q.w(), q.z(), -q.y(), q.x(), -q.z(), q.w(), q.x(), q.y(), q.y(), -q.x(), q.w(), q.z(), -q.x(), -q.y(), -q.z(), q.w()).finished(); } /// @param theta - rotation about screw axis /// @param d - projection of tvec on the rotation axis /// @param l - screw axis direction /// @param m - screw axis moment template void AngleAxisAndTranslationToScrew(const Eigen::Matrix& rvec, const Eigen::Matrix& tvec, T& theta, T& d, Eigen::Matrix& l, Eigen::Matrix& m) { theta = rvec.norm(); if (theta == 0) { l.setZero(); m.setZero(); std::cout << "Warning: Undefined screw! Returned 0. " << std::endl; } l = rvec.normalized(); Eigen::Matrix t = tvec; d = t.transpose() * l; // point on screw axis - projection of origin on screw axis Eigen::Matrix c; c = 0.5 * (t - d * l + (1.0 / tan(theta / 2.0) * l).cross(t)); // c and hence the screw axis is not defined if theta is either 0 or M_PI m = c.cross(l); } template Eigen::Matrix RPY2mat(T roll, T pitch, T yaw) { Eigen::Matrix m; T cr = cos(roll); T sr = sin(roll); T cp = cos(pitch); T sp = sin(pitch); T cy = cos(yaw); T sy = sin(yaw); m(0,0) = cy * cp; m(0,1) = cy * sp * sr - sy * cr; m(0,2) = cy * sp * cr + sy * sr; m(1,0) = sy * cp; m(1,1) = sy * sp * sr + cy * cr; m(1,2) = sy * sp * cr - cy * sr; m(2,0) = - sp; m(2,1) = cp * sr; m(2,2) = cp * cr; return m; } template void mat2RPY(const Eigen::Matrix& m, T& roll, T& pitch, T& yaw) { roll = atan2(m(2,1), m(2,2)); pitch = atan2(-m(2,0), sqrt(m(2,1) * m(2,1) + m(2,2) * m(2,2))); yaw = atan2(m(1,0), m(0,0)); } template Eigen::Matrix homogeneousTransform(const Eigen::Matrix& R, const Eigen::Matrix& t) { Eigen::Matrix H; H.setIdentity(); H.block(0,0,3,3) = R; H.block(0,3,3,1) = t; return H; } template Eigen::Matrix poseWithCartesianTranslation(const T* const q, const T* const p) { Eigen::Matrix pose = Eigen::Matrix::Identity(); T rotation[9]; ceres::QuaternionToRotation(q, rotation); for (int i = 0; i < 3; ++i) { for (int j = 0; j < 3; ++j) { pose(i,j) = rotation[i * 3 + j]; } } pose(0,3) = p[0]; pose(1,3) = p[1]; pose(2,3) = p[2]; return pose; } template Eigen::Matrix poseWithSphericalTranslation(const T* const q, const T* const p, const T scale = T(1.0)) { Eigen::Matrix pose = Eigen::Matrix::Identity(); T rotation[9]; ceres::QuaternionToRotation(q, rotation); for (int i = 0; i < 3; ++i) { for (int j = 0; j < 3; ++j) { pose(i,j) = rotation[i * 3 + j]; } } T theta = p[0]; T phi = p[1]; pose(0,3) = sin(theta) * cos(phi) * scale; pose(1,3) = sin(theta) * sin(phi) * scale; pose(2,3) = cos(theta) * scale; return pose; } // Returns the Sampson error of a given essential matrix and 2 image points template T sampsonError(const Eigen::Matrix& E, const Eigen::Matrix& p1, const Eigen::Matrix& p2) { Eigen::Matrix Ex1 = E * p1; Eigen::Matrix Etx2 = E.transpose() * p2; T x2tEx1 = p2.dot(Ex1); // compute Sampson error T err = square(x2tEx1) / (square(Ex1(0,0)) + square(Ex1(1,0)) + square(Etx2(0,0)) + square(Etx2(1,0))); return err; } // Returns the Sampson error of a given rotation/translation and 2 image points template T sampsonError(const Eigen::Matrix& R, const Eigen::Matrix& t, const Eigen::Matrix& p1, const Eigen::Matrix& p2) { // construct essential matrix Eigen::Matrix E = skew(t) * R; Eigen::Matrix Ex1 = E * p1; Eigen::Matrix Etx2 = E.transpose() * p2; T x2tEx1 = p2.dot(Ex1); // compute Sampson error T err = square(x2tEx1) / (square(Ex1(0,0)) + square(Ex1(1,0)) + square(Etx2(0,0)) + square(Etx2(1,0))); return err; } // Returns the Sampson error of a given rotation/translation and 2 image points template T sampsonError(const Eigen::Matrix& H, const Eigen::Matrix& p1, const Eigen::Matrix& p2) { Eigen::Matrix R = H.block(0, 0, 3, 3); Eigen::Matrix t = H.block(0, 3, 3, 1); return sampsonError(R, t, p1, p2); } template Eigen::Matrix transformPoint(const Eigen::Matrix& H, const Eigen::Matrix& P) { Eigen::Matrix P_trans = H.block(0, 0, 3, 3) * P + H.block(0, 3, 3, 1); return P_trans; } template Eigen::Matrix estimate3DRigidTransform(const std::vector, Eigen::aligned_allocator > >& points1, const std::vector, Eigen::aligned_allocator > >& points2) { // compute centroids Eigen::Matrix c1, c2; c1.setZero(); c2.setZero(); for (size_t i = 0; i < points1.size(); ++i) { c1 += points1.at(i); c2 += points2.at(i); } c1 /= points1.size(); c2 /= points1.size(); Eigen::Matrix X(3, points1.size()); Eigen::Matrix Y(3, points1.size()); for (size_t i = 0; i < points1.size(); ++i) { X.col(i) = points1.at(i) - c1; Y.col(i) = points2.at(i) - c2; } Eigen::Matrix H = X * Y.transpose(); Eigen::JacobiSVD< Eigen::Matrix > svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV); Eigen::Matrix U = svd.matrixU(); Eigen::Matrix V = svd.matrixV(); if (U.determinant() * V.determinant() < 0.0) { V.col(2) *= -1.0; } Eigen::Matrix R = V * U.transpose(); Eigen::Matrix t = c2 - R * c1; return homogeneousTransform(R, t); } template Eigen::Matrix estimate3DRigidSimilarityTransform(const std::vector, Eigen::aligned_allocator > >& points1, const std::vector, Eigen::aligned_allocator > >& points2) { // compute centroids Eigen::Matrix c1, c2; c1.setZero(); c2.setZero(); for (size_t i = 0; i < points1.size(); ++i) { c1 += points1.at(i); c2 += points2.at(i); } c1 /= points1.size(); c2 /= points1.size(); Eigen::Matrix X(3, points1.size()); Eigen::Matrix Y(3, points1.size()); for (size_t i = 0; i < points1.size(); ++i) { X.col(i) = points1.at(i) - c1; Y.col(i) = points2.at(i) - c2; } Eigen::Matrix H = X * Y.transpose(); Eigen::JacobiSVD< Eigen::Matrix > svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV); Eigen::Matrix U = svd.matrixU(); Eigen::Matrix V = svd.matrixV(); if (U.determinant() * V.determinant() < 0.0) { V.col(2) *= -1.0; } Eigen::Matrix R = V * U.transpose(); std::vector, Eigen::aligned_allocator > > rotatedPoints1(points1.size()); for (size_t i = 0; i < points1.size(); ++i) { rotatedPoints1.at(i) = R * (points1.at(i) - c1); } double sum_ss = 0.0, sum_tt = 0.0; for (size_t i = 0; i < points1.size(); ++i) { sum_ss += (points1.at(i) - c1).squaredNorm(); sum_tt += (points2.at(i) - c2).dot(rotatedPoints1.at(i)); } double scale = sum_tt / sum_ss; Eigen::Matrix sR = scale * R; Eigen::Matrix t = c2 - sR * c1; return homogeneousTransform(sR, t); } } #endif ================================================ FILE: camera_model/include/camodocal/gpl/gpl.h ================================================ #ifndef GPL_H #define GPL_H #include #include #include namespace camodocal { template const T clamp(const T& v, const T& a, const T& b) { return std::min(b, std::max(a, v)); } double hypot3(double x, double y, double z); float hypot3f(float x, float y, float z); template const T normalizeTheta(const T& theta) { T normTheta = theta; while (normTheta < - M_PI) { normTheta += 2.0 * M_PI; } while (normTheta > M_PI) { normTheta -= 2.0 * M_PI; } return normTheta; } double d2r(double deg); float d2r(float deg); double r2d(double rad); float r2d(float rad); double sinc(double theta); template const T square(const T& x) { return x * x; } template const T cube(const T& x) { return x * x * x; } template const T random(const T& a, const T& b) { return static_cast(rand()) / RAND_MAX * (b - a) + a; } template const T randomNormal(const T& sigma) { T x1, x2, w; do { x1 = 2.0 * random(0.0, 1.0) - 1.0; x2 = 2.0 * random(0.0, 1.0) - 1.0; w = x1 * x1 + x2 * x2; } while (w >= 1.0 || w == 0.0); w = sqrt((-2.0 * log(w)) / w); return x1 * w * sigma; } unsigned long long timeInMicroseconds(void); double timeInSeconds(void); void colorDepthImage(cv::Mat& imgDepth, cv::Mat& imgColoredDepth, float minRange, float maxRange); bool colormap(const std::string& name, unsigned char idx, float& r, float& g, float& b); std::vector bresLine(int x0, int y0, int x1, int y1); std::vector bresCircle(int x0, int y0, int r); void fitCircle(const std::vector& points, double& centerX, double& centerY, double& radius); std::vector intersectCircles(double x1, double y1, double r1, double x2, double y2, double r2); void LLtoUTM(double latitude, double longitude, double& utmNorthing, double& utmEasting, std::string& utmZone); void UTMtoLL(double utmNorthing, double utmEasting, const std::string& utmZone, double& latitude, double& longitude); long int timestampDiff(uint64_t t1, uint64_t t2); } #endif ================================================ FILE: camera_model/include/camodocal/sparse_graph/Transform.h ================================================ #ifndef TRANSFORM_H #define TRANSFORM_H #include #include #include namespace camodocal { class Transform { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW Transform(); Transform(const Eigen::Matrix4d& H); Eigen::Quaterniond& rotation(void); const Eigen::Quaterniond& rotation(void) const; double* rotationData(void); const double* const rotationData(void) const; Eigen::Vector3d& translation(void); const Eigen::Vector3d& translation(void) const; double* translationData(void); const double* const translationData(void) const; Eigen::Matrix4d toMatrix(void) const; private: Eigen::Quaterniond m_q; Eigen::Vector3d m_t; }; } #endif ================================================ FILE: camera_model/instruction ================================================ rosrun camera_model Calibration -w 8 -h 11 -s 70 -i ~/bag/PX/calib/ ================================================ FILE: camera_model/package.xml ================================================ camera_model 0.0.0 The camera_model package dvorak TODO catkin roscpp std_msgs roscpp std_msgs ================================================ FILE: camera_model/readme.md ================================================ part of [camodocal](https://github.com/hengli/camodocal) [Google Ceres](http://ceres-solver.org) is needed. # Calibration: Use [intrinsic_calib.cc](https://github.com/dvorak0/camera_model/blob/master/src/intrinsic_calib.cc) to calibrate your camera. # Undistortion: See [Camera.h](https://github.com/dvorak0/camera_model/blob/master/include/camodocal/camera_models/Camera.h) for general interface: - liftProjective: Lift points from the image plane to the projective space. - spaceToPlane: Projects 3D points to the image plane (Pi function) ================================================ FILE: camera_model/src/calib/CameraCalibration.cc ================================================ #include "camodocal/calib/CameraCalibration.h" #include #include #include #include #include #include #include #include #include #include #include "camodocal/camera_models/CameraFactory.h" #include "camodocal/sparse_graph/Transform.h" #include "camodocal/gpl/EigenQuaternionParameterization.h" #include "camodocal/gpl/EigenUtils.h" #include "camodocal/camera_models/CostFunctionFactory.h" #include "ceres/ceres.h" namespace camodocal { CameraCalibration::CameraCalibration() : m_boardSize(cv::Size(0,0)) , m_squareSize(0.0f) , m_verbose(false) { } CameraCalibration::CameraCalibration(const Camera::ModelType modelType, const std::string& cameraName, const cv::Size& imageSize, const cv::Size& boardSize, float squareSize) : m_boardSize(boardSize) , m_squareSize(squareSize) , m_verbose(false) { m_camera = CameraFactory::instance()->generateCamera(modelType, cameraName, imageSize); } void CameraCalibration::clear(void) { m_imagePoints.clear(); m_scenePoints.clear(); } void CameraCalibration::addChessboardData(const std::vector& corners) { m_imagePoints.push_back(corners); std::vector scenePointsInView; for (int i = 0; i < m_boardSize.height; ++i) { for (int j = 0; j < m_boardSize.width; ++j) { scenePointsInView.push_back(cv::Point3f(i * m_squareSize, j * m_squareSize, 0.0)); } } m_scenePoints.push_back(scenePointsInView); } bool CameraCalibration::calibrate(void) { int imageCount = m_imagePoints.size(); // compute intrinsic camera parameters and extrinsic parameters for each of the views std::vector rvecs; std::vector tvecs; bool ret = calibrateHelper(m_camera, rvecs, tvecs); m_cameraPoses = cv::Mat(imageCount, 6, CV_64F); for (int i = 0; i < imageCount; ++i) { m_cameraPoses.at(i,0) = rvecs.at(i).at(0); m_cameraPoses.at(i,1) = rvecs.at(i).at(1); m_cameraPoses.at(i,2) = rvecs.at(i).at(2); m_cameraPoses.at(i,3) = tvecs.at(i).at(0); m_cameraPoses.at(i,4) = tvecs.at(i).at(1); m_cameraPoses.at(i,5) = tvecs.at(i).at(2); } // Compute measurement covariance. std::vector > errVec(m_imagePoints.size()); Eigen::Vector2d errSum = Eigen::Vector2d::Zero(); size_t errCount = 0; for (size_t i = 0; i < m_imagePoints.size(); ++i) { std::vector estImagePoints; m_camera->projectPoints(m_scenePoints.at(i), rvecs.at(i), tvecs.at(i), estImagePoints); for (size_t j = 0; j < m_imagePoints.at(i).size(); ++j) { cv::Point2f pObs = m_imagePoints.at(i).at(j); cv::Point2f pEst = estImagePoints.at(j); cv::Point2f err = pObs - pEst; errVec.at(i).push_back(err); errSum += Eigen::Vector2d(err.x, err.y); } errCount += m_imagePoints.at(i).size(); } Eigen::Vector2d errMean = errSum / static_cast(errCount); Eigen::Matrix2d measurementCovariance = Eigen::Matrix2d::Zero(); for (size_t i = 0; i < errVec.size(); ++i) { for (size_t j = 0; j < errVec.at(i).size(); ++j) { cv::Point2f err = errVec.at(i).at(j); double d0 = err.x - errMean(0); double d1 = err.y - errMean(1); measurementCovariance(0,0) += d0 * d0; measurementCovariance(0,1) += d0 * d1; measurementCovariance(1,1) += d1 * d1; } } measurementCovariance /= static_cast(errCount); measurementCovariance(1,0) = measurementCovariance(0,1); m_measurementCovariance = measurementCovariance; return ret; } int CameraCalibration::sampleCount(void) const { return m_imagePoints.size(); } std::vector >& CameraCalibration::imagePoints(void) { return m_imagePoints; } const std::vector >& CameraCalibration::imagePoints(void) const { return m_imagePoints; } std::vector >& CameraCalibration::scenePoints(void) { return m_scenePoints; } const std::vector >& CameraCalibration::scenePoints(void) const { return m_scenePoints; } CameraPtr& CameraCalibration::camera(void) { return m_camera; } const CameraConstPtr CameraCalibration::camera(void) const { return m_camera; } Eigen::Matrix2d& CameraCalibration::measurementCovariance(void) { return m_measurementCovariance; } const Eigen::Matrix2d& CameraCalibration::measurementCovariance(void) const { return m_measurementCovariance; } cv::Mat& CameraCalibration::cameraPoses(void) { return m_cameraPoses; } const cv::Mat& CameraCalibration::cameraPoses(void) const { return m_cameraPoses; } void CameraCalibration::drawResults(std::vector& images) const { std::vector rvecs, tvecs; for (size_t i = 0; i < images.size(); ++i) { cv::Mat rvec(3, 1, CV_64F); rvec.at(0) = m_cameraPoses.at(i,0); rvec.at(1) = m_cameraPoses.at(i,1); rvec.at(2) = m_cameraPoses.at(i,2); cv::Mat tvec(3, 1, CV_64F); tvec.at(0) = m_cameraPoses.at(i,3); tvec.at(1) = m_cameraPoses.at(i,4); tvec.at(2) = m_cameraPoses.at(i,5); rvecs.push_back(rvec); tvecs.push_back(tvec); } int drawShiftBits = 4; int drawMultiplier = 1 << drawShiftBits; cv::Scalar green(0, 255, 0); cv::Scalar red(0, 0, 255); for (size_t i = 0; i < images.size(); ++i) { cv::Mat& image = images.at(i); if (image.channels() == 1) { cv::cvtColor(image, image, cv::COLOR_GRAY2RGB); } std::vector estImagePoints; m_camera->projectPoints(m_scenePoints.at(i), rvecs.at(i), tvecs.at(i), estImagePoints); float errorSum = 0.0f; float errorMax = std::numeric_limits::min(); for (size_t j = 0; j < m_imagePoints.at(i).size(); ++j) { cv::Point2f pObs = m_imagePoints.at(i).at(j); cv::Point2f pEst = estImagePoints.at(j); cv::circle(image, cv::Point(cvRound(pObs.x * drawMultiplier), cvRound(pObs.y * drawMultiplier)), 5, green, 2, cv::LINE_AA, drawShiftBits); cv::circle(image, cv::Point(cvRound(pEst.x * drawMultiplier), cvRound(pEst.y * drawMultiplier)), 5, red, 2, cv::LINE_AA, drawShiftBits); float error = cv::norm(pObs - pEst); errorSum += error; if (error > errorMax) { errorMax = error; } } std::ostringstream oss; oss << "Reprojection error: avg = " << errorSum / m_imagePoints.at(i).size() << " max = " << errorMax; cv::putText(image, oss.str(), cv::Point(10, image.rows - 10), cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(255, 255, 255), 1, cv::LINE_AA); } } void CameraCalibration::writeParams(const std::string& filename) const { m_camera->writeParametersToYamlFile(filename); } bool CameraCalibration::writeChessboardData(const std::string& filename) const { std::ofstream ofs(filename.c_str(), std::ios::out | std::ios::binary); if (!ofs.is_open()) { return false; } writeData(ofs, m_boardSize.width); writeData(ofs, m_boardSize.height); writeData(ofs, m_squareSize); writeData(ofs, m_measurementCovariance(0,0)); writeData(ofs, m_measurementCovariance(0,1)); writeData(ofs, m_measurementCovariance(1,0)); writeData(ofs, m_measurementCovariance(1,1)); writeData(ofs, m_cameraPoses.rows); writeData(ofs, m_cameraPoses.cols); writeData(ofs, m_cameraPoses.type()); for (int i = 0; i < m_cameraPoses.rows; ++i) { for (int j = 0; j < m_cameraPoses.cols; ++j) { writeData(ofs, m_cameraPoses.at(i,j)); } } writeData(ofs, m_imagePoints.size()); for (size_t i = 0; i < m_imagePoints.size(); ++i) { writeData(ofs, m_imagePoints.at(i).size()); for (size_t j = 0; j < m_imagePoints.at(i).size(); ++j) { const cv::Point2f& ipt = m_imagePoints.at(i).at(j); writeData(ofs, ipt.x); writeData(ofs, ipt.y); } } writeData(ofs, m_scenePoints.size()); for (size_t i = 0; i < m_scenePoints.size(); ++i) { writeData(ofs, m_scenePoints.at(i).size()); for (size_t j = 0; j < m_scenePoints.at(i).size(); ++j) { const cv::Point3f& spt = m_scenePoints.at(i).at(j); writeData(ofs, spt.x); writeData(ofs, spt.y); writeData(ofs, spt.z); } } return true; } bool CameraCalibration::readChessboardData(const std::string& filename) { std::ifstream ifs(filename.c_str(), std::ios::in | std::ios::binary); if (!ifs.is_open()) { return false; } readData(ifs, m_boardSize.width); readData(ifs, m_boardSize.height); readData(ifs, m_squareSize); readData(ifs, m_measurementCovariance(0,0)); readData(ifs, m_measurementCovariance(0,1)); readData(ifs, m_measurementCovariance(1,0)); readData(ifs, m_measurementCovariance(1,1)); int rows, cols, type; readData(ifs, rows); readData(ifs, cols); readData(ifs, type); m_cameraPoses = cv::Mat(rows, cols, type); for (int i = 0; i < m_cameraPoses.rows; ++i) { for (int j = 0; j < m_cameraPoses.cols; ++j) { readData(ifs, m_cameraPoses.at(i,j)); } } size_t nImagePointSets; readData(ifs, nImagePointSets); m_imagePoints.clear(); m_imagePoints.resize(nImagePointSets); for (size_t i = 0; i < m_imagePoints.size(); ++i) { size_t nImagePoints; readData(ifs, nImagePoints); m_imagePoints.at(i).resize(nImagePoints); for (size_t j = 0; j < m_imagePoints.at(i).size(); ++j) { cv::Point2f& ipt = m_imagePoints.at(i).at(j); readData(ifs, ipt.x); readData(ifs, ipt.y); } } size_t nScenePointSets; readData(ifs, nScenePointSets); m_scenePoints.clear(); m_scenePoints.resize(nScenePointSets); for (size_t i = 0; i < m_scenePoints.size(); ++i) { size_t nScenePoints; readData(ifs, nScenePoints); m_scenePoints.at(i).resize(nScenePoints); for (size_t j = 0; j < m_scenePoints.at(i).size(); ++j) { cv::Point3f& spt = m_scenePoints.at(i).at(j); readData(ifs, spt.x); readData(ifs, spt.y); readData(ifs, spt.z); } } return true; } void CameraCalibration::setVerbose(bool verbose) { m_verbose = verbose; } bool CameraCalibration::calibrateHelper(CameraPtr& camera, std::vector& rvecs, std::vector& tvecs) const { rvecs.assign(m_scenePoints.size(), cv::Mat()); tvecs.assign(m_scenePoints.size(), cv::Mat()); // STEP 1: Estimate intrinsics camera->estimateIntrinsics(m_boardSize, m_scenePoints, m_imagePoints); // STEP 2: Estimate extrinsics for (size_t i = 0; i < m_scenePoints.size(); ++i) { camera->estimateExtrinsics(m_scenePoints.at(i), m_imagePoints.at(i), rvecs.at(i), tvecs.at(i)); } if (m_verbose) { std::cout << "[" << camera->cameraName() << "] " << "# INFO: " << "Initial reprojection error: " << std::fixed << std::setprecision(3) << camera->reprojectionError(m_scenePoints, m_imagePoints, rvecs, tvecs) << " pixels" << std::endl; } // STEP 3: optimization using ceres optimize(camera, rvecs, tvecs); if (m_verbose) { double err = camera->reprojectionError(m_scenePoints, m_imagePoints, rvecs, tvecs); std::cout << "[" << camera->cameraName() << "] " << "# INFO: Final reprojection error: " << err << " pixels" << std::endl; std::cout << "[" << camera->cameraName() << "] " << "# INFO: " << camera->parametersToString() << std::endl; } return true; } void CameraCalibration::optimize(CameraPtr& camera, std::vector& rvecs, std::vector& tvecs) const { // Use ceres to do optimization ceres::Problem problem; std::vector > transformVec(rvecs.size()); for (size_t i = 0; i < rvecs.size(); ++i) { Eigen::Vector3d rvec; cv::cv2eigen(rvecs.at(i), rvec); transformVec.at(i).rotation() = Eigen::AngleAxisd(rvec.norm(), rvec.normalized()); transformVec.at(i).translation() << tvecs[i].at(0), tvecs[i].at(1), tvecs[i].at(2); } std::vector intrinsicCameraParams; m_camera->writeParameters(intrinsicCameraParams); // create residuals for each observation for (size_t i = 0; i < m_imagePoints.size(); ++i) { for (size_t j = 0; j < m_imagePoints.at(i).size(); ++j) { const cv::Point3f& spt = m_scenePoints.at(i).at(j); const cv::Point2f& ipt = m_imagePoints.at(i).at(j); ceres::CostFunction* costFunction = CostFunctionFactory::instance()->generateCostFunction(camera, Eigen::Vector3d(spt.x, spt.y, spt.z), Eigen::Vector2d(ipt.x, ipt.y), CAMERA_INTRINSICS | CAMERA_POSE); ceres::LossFunction* lossFunction = new ceres::CauchyLoss(1.0); problem.AddResidualBlock(costFunction, lossFunction, intrinsicCameraParams.data(), transformVec.at(i).rotationData(), transformVec.at(i).translationData()); } ceres::LocalParameterization* quaternionParameterization = new EigenQuaternionParameterization; problem.SetParameterization(transformVec.at(i).rotationData(), quaternionParameterization); } std::cout << "begin ceres" << std::endl; ceres::Solver::Options options; options.max_num_iterations = 1000; options.num_threads = 1; if (m_verbose) { options.minimizer_progress_to_stdout = true; } ceres::Solver::Summary summary; ceres::Solve(options, &problem, &summary); std::cout << "end ceres" << std::endl; if (m_verbose) { std::cout << summary.FullReport() << std::endl; } camera->readParameters(intrinsicCameraParams); for (size_t i = 0; i < rvecs.size(); ++i) { Eigen::AngleAxisd aa(transformVec.at(i).rotation()); Eigen::Vector3d rvec = aa.angle() * aa.axis(); cv::eigen2cv(rvec, rvecs.at(i)); cv::Mat& tvec = tvecs.at(i); tvec.at(0) = transformVec.at(i).translation()(0); tvec.at(1) = transformVec.at(i).translation()(1); tvec.at(2) = transformVec.at(i).translation()(2); } } template void CameraCalibration::readData(std::ifstream& ifs, T& data) const { char* buffer = new char[sizeof(T)]; ifs.read(buffer, sizeof(T)); data = *(reinterpret_cast(buffer)); delete[] buffer; } template void CameraCalibration::writeData(std::ofstream& ofs, T data) const { char* pData = reinterpret_cast(&data); ofs.write(pData, sizeof(T)); } } ================================================ FILE: camera_model/src/camera_models/Camera.cc ================================================ #include "camodocal/camera_models/Camera.h" #include "camodocal/camera_models/ScaramuzzaCamera.h" #include namespace camodocal { Camera::Parameters::Parameters(ModelType modelType) : m_modelType(modelType) , m_imageWidth(0) , m_imageHeight(0) { switch (modelType) { case KANNALA_BRANDT: m_nIntrinsics = 8; break; case PINHOLE: m_nIntrinsics = 8; break; case SCARAMUZZA: m_nIntrinsics = SCARAMUZZA_CAMERA_NUM_PARAMS; break; case MEI: default: m_nIntrinsics = 9; } } Camera::Parameters::Parameters(ModelType modelType, const std::string& cameraName, int w, int h) : m_modelType(modelType) , m_cameraName(cameraName) , m_imageWidth(w) , m_imageHeight(h) { switch (modelType) { case KANNALA_BRANDT: m_nIntrinsics = 8; break; case PINHOLE: m_nIntrinsics = 8; break; case SCARAMUZZA: m_nIntrinsics = SCARAMUZZA_CAMERA_NUM_PARAMS; break; case MEI: default: m_nIntrinsics = 9; } } Camera::ModelType& Camera::Parameters::modelType(void) { return m_modelType; } std::string& Camera::Parameters::cameraName(void) { return m_cameraName; } int& Camera::Parameters::imageWidth(void) { return m_imageWidth; } int& Camera::Parameters::imageHeight(void) { return m_imageHeight; } Camera::ModelType Camera::Parameters::modelType(void) const { return m_modelType; } const std::string& Camera::Parameters::cameraName(void) const { return m_cameraName; } int Camera::Parameters::imageWidth(void) const { return m_imageWidth; } int Camera::Parameters::imageHeight(void) const { return m_imageHeight; } int Camera::Parameters::nIntrinsics(void) const { return m_nIntrinsics; } cv::Mat& Camera::mask(void) { return m_mask; } const cv::Mat& Camera::mask(void) const { return m_mask; } void Camera::estimateExtrinsics(const std::vector& objectPoints, const std::vector& imagePoints, cv::Mat& rvec, cv::Mat& tvec) const { std::vector Ms(imagePoints.size()); for (size_t i = 0; i < Ms.size(); ++i) { Eigen::Vector3d P; liftProjective(Eigen::Vector2d(imagePoints.at(i).x, imagePoints.at(i).y), P); P /= P(2); Ms.at(i).x = P(0); Ms.at(i).y = P(1); } // assume unit focal length, zero principal point, and zero distortion cv::solvePnP(objectPoints, Ms, cv::Mat::eye(3, 3, CV_64F), cv::noArray(), rvec, tvec); } double Camera::reprojectionDist(const Eigen::Vector3d& P1, const Eigen::Vector3d& P2) const { Eigen::Vector2d p1, p2; spaceToPlane(P1, p1); spaceToPlane(P2, p2); return (p1 - p2).norm(); } double Camera::reprojectionError(const std::vector< std::vector >& objectPoints, const std::vector< std::vector >& imagePoints, const std::vector& rvecs, const std::vector& tvecs, cv::OutputArray _perViewErrors) const { int imageCount = objectPoints.size(); size_t pointsSoFar = 0; double totalErr = 0.0; bool computePerViewErrors = _perViewErrors.needed(); cv::Mat perViewErrors; if (computePerViewErrors) { _perViewErrors.create(imageCount, 1, CV_64F); perViewErrors = _perViewErrors.getMat(); } for (int i = 0; i < imageCount; ++i) { size_t pointCount = imagePoints.at(i).size(); pointsSoFar += pointCount; std::vector estImagePoints; projectPoints(objectPoints.at(i), rvecs.at(i), tvecs.at(i), estImagePoints); double err = 0.0; for (size_t j = 0; j < imagePoints.at(i).size(); ++j) { err += cv::norm(imagePoints.at(i).at(j) - estImagePoints.at(j)); } if (computePerViewErrors) { perViewErrors.at(i) = err / pointCount; } totalErr += err; } return totalErr / pointsSoFar; } double Camera::reprojectionError(const Eigen::Vector3d& P, const Eigen::Quaterniond& camera_q, const Eigen::Vector3d& camera_t, const Eigen::Vector2d& observed_p) const { Eigen::Vector3d P_cam = camera_q.toRotationMatrix() * P + camera_t; Eigen::Vector2d p; spaceToPlane(P_cam, p); return (p - observed_p).norm(); } void Camera::projectPoints(const std::vector& objectPoints, const cv::Mat& rvec, const cv::Mat& tvec, std::vector& imagePoints) const { // project 3D object points to the image plane imagePoints.reserve(objectPoints.size()); //double cv::Mat R0; cv::Rodrigues(rvec, R0); Eigen::MatrixXd R(3,3); R << R0.at(0,0), R0.at(0,1), R0.at(0,2), R0.at(1,0), R0.at(1,1), R0.at(1,2), R0.at(2,0), R0.at(2,1), R0.at(2,2); Eigen::Vector3d t; t << tvec.at(0), tvec.at(1), tvec.at(2); for (size_t i = 0; i < objectPoints.size(); ++i) { const cv::Point3f& objectPoint = objectPoints.at(i); // Rotate and translate Eigen::Vector3d P; P << objectPoint.x, objectPoint.y, objectPoint.z; P = R * P + t; Eigen::Vector2d p; spaceToPlane(P, p); imagePoints.push_back(cv::Point2f(p(0), p(1))); } } } ================================================ FILE: camera_model/src/camera_models/CameraFactory.cc ================================================ #include "camodocal/camera_models/CameraFactory.h" #include #include "camodocal/camera_models/CataCamera.h" #include "camodocal/camera_models/EquidistantCamera.h" #include "camodocal/camera_models/PinholeCamera.h" #include "camodocal/camera_models/ScaramuzzaCamera.h" #include "ceres/ceres.h" namespace camodocal { boost::shared_ptr CameraFactory::m_instance; CameraFactory::CameraFactory() { } boost::shared_ptr CameraFactory::instance(void) { if (m_instance.get() == 0) { m_instance.reset(new CameraFactory); } return m_instance; } CameraPtr CameraFactory::generateCamera(Camera::ModelType modelType, const std::string& cameraName, cv::Size imageSize) const { switch (modelType) { case Camera::KANNALA_BRANDT: { EquidistantCameraPtr camera(new EquidistantCamera); EquidistantCamera::Parameters params = camera->getParameters(); params.cameraName() = cameraName; params.imageWidth() = imageSize.width; params.imageHeight() = imageSize.height; camera->setParameters(params); return camera; } case Camera::PINHOLE: { PinholeCameraPtr camera(new PinholeCamera); PinholeCamera::Parameters params = camera->getParameters(); params.cameraName() = cameraName; params.imageWidth() = imageSize.width; params.imageHeight() = imageSize.height; camera->setParameters(params); return camera; } case Camera::SCARAMUZZA: { OCAMCameraPtr camera(new OCAMCamera); OCAMCamera::Parameters params = camera->getParameters(); params.cameraName() = cameraName; params.imageWidth() = imageSize.width; params.imageHeight() = imageSize.height; camera->setParameters(params); return camera; } case Camera::MEI: default: { CataCameraPtr camera(new CataCamera); CataCamera::Parameters params = camera->getParameters(); params.cameraName() = cameraName; params.imageWidth() = imageSize.width; params.imageHeight() = imageSize.height; camera->setParameters(params); return camera; } } } CameraPtr CameraFactory::generateCameraFromYamlFile(const std::string& filename) { cv::FileStorage fs(filename, cv::FileStorage::READ); if (!fs.isOpened()) { return CameraPtr(); } Camera::ModelType modelType = Camera::MEI; if (!fs["model_type"].isNone()) { std::string sModelType; fs["model_type"] >> sModelType; if (boost::iequals(sModelType, "kannala_brandt")) { modelType = Camera::KANNALA_BRANDT; } else if (boost::iequals(sModelType, "mei")) { modelType = Camera::MEI; } else if (boost::iequals(sModelType, "scaramuzza")) { modelType = Camera::SCARAMUZZA; } else if (boost::iequals(sModelType, "pinhole")) { modelType = Camera::PINHOLE; } else { std::cerr << "# ERROR: Unknown camera model: " << sModelType << std::endl; return CameraPtr(); } } switch (modelType) { case Camera::KANNALA_BRANDT: { EquidistantCameraPtr camera(new EquidistantCamera); EquidistantCamera::Parameters params = camera->getParameters(); params.readFromYamlFile(filename); camera->setParameters(params); return camera; } case Camera::PINHOLE: { PinholeCameraPtr camera(new PinholeCamera); PinholeCamera::Parameters params = camera->getParameters(); params.readFromYamlFile(filename); camera->setParameters(params); return camera; } case Camera::SCARAMUZZA: { OCAMCameraPtr camera(new OCAMCamera); OCAMCamera::Parameters params = camera->getParameters(); params.readFromYamlFile(filename); camera->setParameters(params); return camera; } case Camera::MEI: default: { CataCameraPtr camera(new CataCamera); CataCamera::Parameters params = camera->getParameters(); params.readFromYamlFile(filename); camera->setParameters(params); return camera; } } return CameraPtr(); } } ================================================ FILE: camera_model/src/camera_models/CataCamera.cc ================================================ #include "camodocal/camera_models/CataCamera.h" #include #include #include #include #include #include #include #include #include "camodocal/gpl/gpl.h" namespace camodocal { CataCamera::Parameters::Parameters() : Camera::Parameters(MEI) , m_xi(0.0) , m_k1(0.0) , m_k2(0.0) , m_p1(0.0) , m_p2(0.0) , m_gamma1(0.0) , m_gamma2(0.0) , m_u0(0.0) , m_v0(0.0) { } CataCamera::Parameters::Parameters(const std::string& cameraName, int w, int h, double xi, double k1, double k2, double p1, double p2, double gamma1, double gamma2, double u0, double v0) : Camera::Parameters(MEI, cameraName, w, h) , m_xi(xi) , m_k1(k1) , m_k2(k2) , m_p1(p1) , m_p2(p2) , m_gamma1(gamma1) , m_gamma2(gamma2) , m_u0(u0) , m_v0(v0) { } double& CataCamera::Parameters::xi(void) { return m_xi; } double& CataCamera::Parameters::k1(void) { return m_k1; } double& CataCamera::Parameters::k2(void) { return m_k2; } double& CataCamera::Parameters::p1(void) { return m_p1; } double& CataCamera::Parameters::p2(void) { return m_p2; } double& CataCamera::Parameters::gamma1(void) { return m_gamma1; } double& CataCamera::Parameters::gamma2(void) { return m_gamma2; } double& CataCamera::Parameters::u0(void) { return m_u0; } double& CataCamera::Parameters::v0(void) { return m_v0; } double CataCamera::Parameters::xi(void) const { return m_xi; } double CataCamera::Parameters::k1(void) const { return m_k1; } double CataCamera::Parameters::k2(void) const { return m_k2; } double CataCamera::Parameters::p1(void) const { return m_p1; } double CataCamera::Parameters::p2(void) const { return m_p2; } double CataCamera::Parameters::gamma1(void) const { return m_gamma1; } double CataCamera::Parameters::gamma2(void) const { return m_gamma2; } double CataCamera::Parameters::u0(void) const { return m_u0; } double CataCamera::Parameters::v0(void) const { return m_v0; } bool CataCamera::Parameters::readFromYamlFile(const std::string& filename) { cv::FileStorage fs(filename, cv::FileStorage::READ); if (!fs.isOpened()) { return false; } if (!fs["model_type"].isNone()) { std::string sModelType; fs["model_type"] >> sModelType; if (sModelType.compare("MEI") != 0) { return false; } } m_modelType = MEI; fs["camera_name"] >> m_cameraName; m_imageWidth = static_cast(fs["image_width"]); m_imageHeight = static_cast(fs["image_height"]); cv::FileNode n = fs["mirror_parameters"]; m_xi = static_cast(n["xi"]); n = fs["distortion_parameters"]; m_k1 = static_cast(n["k1"]); m_k2 = static_cast(n["k2"]); m_p1 = static_cast(n["p1"]); m_p2 = static_cast(n["p2"]); n = fs["projection_parameters"]; m_gamma1 = static_cast(n["gamma1"]); m_gamma2 = static_cast(n["gamma2"]); m_u0 = static_cast(n["u0"]); m_v0 = static_cast(n["v0"]); return true; } void CataCamera::Parameters::writeToYamlFile(const std::string& filename) const { cv::FileStorage fs(filename, cv::FileStorage::WRITE); fs << "model_type" << "MEI"; fs << "camera_name" << m_cameraName; fs << "image_width" << m_imageWidth; fs << "image_height" << m_imageHeight; // mirror: xi fs << "mirror_parameters"; fs << "{" << "xi" << m_xi << "}"; // radial distortion: k1, k2 // tangential distortion: p1, p2 fs << "distortion_parameters"; fs << "{" << "k1" << m_k1 << "k2" << m_k2 << "p1" << m_p1 << "p2" << m_p2 << "}"; // projection: gamma1, gamma2, u0, v0 fs << "projection_parameters"; fs << "{" << "gamma1" << m_gamma1 << "gamma2" << m_gamma2 << "u0" << m_u0 << "v0" << m_v0 << "}"; fs.release(); } CataCamera::Parameters& CataCamera::Parameters::operator=(const CataCamera::Parameters& other) { if (this != &other) { m_modelType = other.m_modelType; m_cameraName = other.m_cameraName; m_imageWidth = other.m_imageWidth; m_imageHeight = other.m_imageHeight; m_xi = other.m_xi; m_k1 = other.m_k1; m_k2 = other.m_k2; m_p1 = other.m_p1; m_p2 = other.m_p2; m_gamma1 = other.m_gamma1; m_gamma2 = other.m_gamma2; m_u0 = other.m_u0; m_v0 = other.m_v0; } return *this; } std::ostream& operator<< (std::ostream& out, const CataCamera::Parameters& params) { out << "Camera Parameters:" << std::endl; out << " model_type " << "MEI" << std::endl; out << " camera_name " << params.m_cameraName << std::endl; out << " image_width " << params.m_imageWidth << std::endl; out << " image_height " << params.m_imageHeight << std::endl; out << "Mirror Parameters" << std::endl; out << std::fixed << std::setprecision(10); out << " xi " << params.m_xi << std::endl; // radial distortion: k1, k2 // tangential distortion: p1, p2 out << "Distortion Parameters" << std::endl; out << " k1 " << params.m_k1 << std::endl << " k2 " << params.m_k2 << std::endl << " p1 " << params.m_p1 << std::endl << " p2 " << params.m_p2 << std::endl; // projection: gamma1, gamma2, u0, v0 out << "Projection Parameters" << std::endl; out << " gamma1 " << params.m_gamma1 << std::endl << " gamma2 " << params.m_gamma2 << std::endl << " u0 " << params.m_u0 << std::endl << " v0 " << params.m_v0 << std::endl; return out; } CataCamera::CataCamera() : m_inv_K11(1.0) , m_inv_K13(0.0) , m_inv_K22(1.0) , m_inv_K23(0.0) , m_noDistortion(true) { } CataCamera::CataCamera(const std::string& cameraName, int imageWidth, int imageHeight, double xi, double k1, double k2, double p1, double p2, double gamma1, double gamma2, double u0, double v0) : mParameters(cameraName, imageWidth, imageHeight, xi, k1, k2, p1, p2, gamma1, gamma2, u0, v0) { if ((mParameters.k1() == 0.0) && (mParameters.k2() == 0.0) && (mParameters.p1() == 0.0) && (mParameters.p2() == 0.0)) { m_noDistortion = true; } else { m_noDistortion = false; } // Inverse camera projection matrix parameters m_inv_K11 = 1.0 / mParameters.gamma1(); m_inv_K13 = -mParameters.u0() / mParameters.gamma1(); m_inv_K22 = 1.0 / mParameters.gamma2(); m_inv_K23 = -mParameters.v0() / mParameters.gamma2(); } CataCamera::CataCamera(const CataCamera::Parameters& params) : mParameters(params) { if ((mParameters.k1() == 0.0) && (mParameters.k2() == 0.0) && (mParameters.p1() == 0.0) && (mParameters.p2() == 0.0)) { m_noDistortion = true; } else { m_noDistortion = false; } // Inverse camera projection matrix parameters m_inv_K11 = 1.0 / mParameters.gamma1(); m_inv_K13 = -mParameters.u0() / mParameters.gamma1(); m_inv_K22 = 1.0 / mParameters.gamma2(); m_inv_K23 = -mParameters.v0() / mParameters.gamma2(); } Camera::ModelType CataCamera::modelType(void) const { return mParameters.modelType(); } const std::string& CataCamera::cameraName(void) const { return mParameters.cameraName(); } int CataCamera::imageWidth(void) const { return mParameters.imageWidth(); } int CataCamera::imageHeight(void) const { return mParameters.imageHeight(); } void CataCamera::estimateIntrinsics(const cv::Size& boardSize, const std::vector< std::vector >& objectPoints, const std::vector< std::vector >& imagePoints) { Parameters params = getParameters(); double u0 = params.imageWidth() / 2.0; double v0 = params.imageHeight() / 2.0; double gamma0 = 0.0; double minReprojErr = std::numeric_limits::max(); std::vector rvecs, tvecs; rvecs.assign(objectPoints.size(), cv::Mat()); tvecs.assign(objectPoints.size(), cv::Mat()); params.xi() = 1.0; params.k1() = 0.0; params.k2() = 0.0; params.p1() = 0.0; params.p2() = 0.0; params.u0() = u0; params.v0() = v0; // Initialize gamma (focal length) // Use non-radial line image and xi = 1 for (size_t i = 0; i < imagePoints.size(); ++i) { for (int r = 0; r < boardSize.height; ++r) { cv::Mat P(boardSize.width, 4, CV_64F); for (int c = 0; c < boardSize.width; ++c) { const cv::Point2f& imagePoint = imagePoints.at(i).at(r * boardSize.width + c); double u = imagePoint.x - u0; double v = imagePoint.y - v0; P.at(c, 0) = u; P.at(c, 1) = v; P.at(c, 2) = 0.5; P.at(c, 3) = -0.5 * (square(u) + square(v)); } cv::Mat C; cv::SVD::solveZ(P, C); double t = square(C.at(0)) + square(C.at(1)) + C.at(2) * C.at(3); if (t < 0.0) { continue; } // check that line image is not radial double d = sqrt(1.0 / t); double nx = C.at(0) * d; double ny = C.at(1) * d; if (hypot(nx, ny) > 0.95) { continue; } double gamma = sqrt(C.at(2) / C.at(3)); params.gamma1() = gamma; params.gamma2() = gamma; setParameters(params); for (size_t j = 0; j < objectPoints.size(); ++j) { estimateExtrinsics(objectPoints.at(j), imagePoints.at(j), rvecs.at(j), tvecs.at(j)); } double reprojErr = reprojectionError(objectPoints, imagePoints, rvecs, tvecs, cv::noArray()); if (reprojErr < minReprojErr) { minReprojErr = reprojErr; gamma0 = gamma; } } } if (gamma0 <= 0.0 && minReprojErr >= std::numeric_limits::max()) { std::cout << "[" << params.cameraName() << "] " << "# INFO: CataCamera model fails with given data. " << std::endl; return; } params.gamma1() = gamma0; params.gamma2() = gamma0; setParameters(params); } /** * \brief Lifts a point from the image plane to the unit sphere * * \param p image coordinates * \param P coordinates of the point on the sphere */ void CataCamera::liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const { double mx_d, my_d,mx2_d, mxy_d, my2_d, mx_u, my_u; double rho2_d, rho4_d, radDist_d, Dx_d, Dy_d, inv_denom_d; double lambda; // Lift points to normalised plane mx_d = m_inv_K11 * p(0) + m_inv_K13; my_d = m_inv_K22 * p(1) + m_inv_K23; if (m_noDistortion) { mx_u = mx_d; my_u = my_d; } else { // Apply inverse distortion model if (0) { double k1 = mParameters.k1(); double k2 = mParameters.k2(); double p1 = mParameters.p1(); double p2 = mParameters.p2(); // Inverse distortion model // proposed by Heikkila mx2_d = mx_d*mx_d; my2_d = my_d*my_d; mxy_d = mx_d*my_d; rho2_d = mx2_d+my2_d; rho4_d = rho2_d*rho2_d; radDist_d = k1*rho2_d+k2*rho4_d; Dx_d = mx_d*radDist_d + p2*(rho2_d+2*mx2_d) + 2*p1*mxy_d; Dy_d = my_d*radDist_d + p1*(rho2_d+2*my2_d) + 2*p2*mxy_d; inv_denom_d = 1/(1+4*k1*rho2_d+6*k2*rho4_d+8*p1*my_d+8*p2*mx_d); mx_u = mx_d - inv_denom_d*Dx_d; my_u = my_d - inv_denom_d*Dy_d; } else { // Recursive distortion model int n = 6; Eigen::Vector2d d_u; distortion(Eigen::Vector2d(mx_d, my_d), d_u); // Approximate value mx_u = mx_d - d_u(0); my_u = my_d - d_u(1); for (int i = 1; i < n; ++i) { distortion(Eigen::Vector2d(mx_u, my_u), d_u); mx_u = mx_d - d_u(0); my_u = my_d - d_u(1); } } } // Lift normalised points to the sphere (inv_hslash) double xi = mParameters.xi(); if (xi == 1.0) { lambda = 2.0 / (mx_u * mx_u + my_u * my_u + 1.0); P << lambda * mx_u, lambda * my_u, lambda - 1.0; } else { lambda = (xi + sqrt(1.0 + (1.0 - xi * xi) * (mx_u * mx_u + my_u * my_u))) / (1.0 + mx_u * mx_u + my_u * my_u); P << lambda * mx_u, lambda * my_u, lambda - xi; } } /** * \brief Lifts a point from the image plane to its projective ray * * \param p image coordinates * \param P coordinates of the projective ray */ void CataCamera::liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const { double mx_d, my_d,mx2_d, mxy_d, my2_d, mx_u, my_u; double rho2_d, rho4_d, radDist_d, Dx_d, Dy_d, inv_denom_d; //double lambda; // Lift points to normalised plane mx_d = m_inv_K11 * p(0) + m_inv_K13; my_d = m_inv_K22 * p(1) + m_inv_K23; if (m_noDistortion) { mx_u = mx_d; my_u = my_d; } else { if (0) { double k1 = mParameters.k1(); double k2 = mParameters.k2(); double p1 = mParameters.p1(); double p2 = mParameters.p2(); // Apply inverse distortion model // proposed by Heikkila mx2_d = mx_d*mx_d; my2_d = my_d*my_d; mxy_d = mx_d*my_d; rho2_d = mx2_d+my2_d; rho4_d = rho2_d*rho2_d; radDist_d = k1*rho2_d+k2*rho4_d; Dx_d = mx_d*radDist_d + p2*(rho2_d+2*mx2_d) + 2*p1*mxy_d; Dy_d = my_d*radDist_d + p1*(rho2_d+2*my2_d) + 2*p2*mxy_d; inv_denom_d = 1/(1+4*k1*rho2_d+6*k2*rho4_d+8*p1*my_d+8*p2*mx_d); mx_u = mx_d - inv_denom_d*Dx_d; my_u = my_d - inv_denom_d*Dy_d; } else { // Recursive distortion model int n = 8; Eigen::Vector2d d_u; distortion(Eigen::Vector2d(mx_d, my_d), d_u); // Approximate value mx_u = mx_d - d_u(0); my_u = my_d - d_u(1); for (int i = 1; i < n; ++i) { distortion(Eigen::Vector2d(mx_u, my_u), d_u); mx_u = mx_d - d_u(0); my_u = my_d - d_u(1); } } } // Obtain a projective ray double xi = mParameters.xi(); if (xi == 1.0) { P << mx_u, my_u, (1.0 - mx_u * mx_u - my_u * my_u) / 2.0; } else { // Reuse variable rho2_d = mx_u * mx_u + my_u * my_u; P << mx_u, my_u, 1.0 - xi * (rho2_d + 1.0) / (xi + sqrt(1.0 + (1.0 - xi * xi) * rho2_d)); } } /** * \brief Project a 3D point (\a x,\a y,\a z) to the image plane in (\a u,\a v) * * \param P 3D point coordinates * \param p return value, contains the image point coordinates */ void CataCamera::spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const { Eigen::Vector2d p_u, p_d; // Project points to the normalised plane double z = P(2) + mParameters.xi() * P.norm(); p_u << P(0) / z, P(1) / z; if (m_noDistortion) { p_d = p_u; } else { // Apply distortion Eigen::Vector2d d_u; distortion(p_u, d_u); p_d = p_u + d_u; } // Apply generalised projection matrix p << mParameters.gamma1() * p_d(0) + mParameters.u0(), mParameters.gamma2() * p_d(1) + mParameters.v0(); } #if 0 /** * \brief Project a 3D point to the image plane and calculate Jacobian * * \param P 3D point coordinates * \param p return value, contains the image point coordinates */ void CataCamera::spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p, Eigen::Matrix& J) const { double xi = mParameters.xi(); Eigen::Vector2d p_u, p_d; double norm, inv_denom; double dxdmx, dydmx, dxdmy, dydmy; norm = P.norm(); // Project points to the normalised plane inv_denom = 1.0 / (P(2) + xi * norm); p_u << inv_denom * P(0), inv_denom * P(1); // Calculate jacobian inv_denom = inv_denom * inv_denom / norm; double dudx = inv_denom * (norm * P(2) + xi * (P(1) * P(1) + P(2) * P(2))); double dvdx = -inv_denom * xi * P(0) * P(1); double dudy = dvdx; double dvdy = inv_denom * (norm * P(2) + xi * (P(0) * P(0) + P(2) * P(2))); inv_denom = inv_denom * (-xi * P(2) - norm); // reuse variable double dudz = P(0) * inv_denom; double dvdz = P(1) * inv_denom; if (m_noDistortion) { p_d = p_u; } else { // Apply distortion Eigen::Vector2d d_u; distortion(p_u, d_u); p_d = p_u + d_u; } double gamma1 = mParameters.gamma1(); double gamma2 = mParameters.gamma2(); // Make the product of the jacobians // and add projection matrix jacobian inv_denom = gamma1 * (dudx * dxdmx + dvdx * dxdmy); // reuse dvdx = gamma2 * (dudx * dydmx + dvdx * dydmy); dudx = inv_denom; inv_denom = gamma1 * (dudy * dxdmx + dvdy * dxdmy); // reuse dvdy = gamma2 * (dudy * dydmx + dvdy * dydmy); dudy = inv_denom; inv_denom = gamma1 * (dudz * dxdmx + dvdz * dxdmy); // reuse dvdz = gamma2 * (dudz * dydmx + dvdz * dydmy); dudz = inv_denom; // Apply generalised projection matrix p << gamma1 * p_d(0) + mParameters.u0(), gamma2 * p_d(1) + mParameters.v0(); J << dudx, dudy, dudz, dvdx, dvdy, dvdz; } #endif /** * \brief Projects an undistorted 2D point p_u to the image plane * * \param p_u 2D point coordinates * \return image point coordinates */ void CataCamera::undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const { Eigen::Vector2d p_d; if (m_noDistortion) { p_d = p_u; } else { // Apply distortion Eigen::Vector2d d_u; distortion(p_u, d_u); p_d = p_u + d_u; } // Apply generalised projection matrix p << mParameters.gamma1() * p_d(0) + mParameters.u0(), mParameters.gamma2() * p_d(1) + mParameters.v0(); } /** * \brief Apply distortion to input point (from the normalised plane) * * \param p_u undistorted coordinates of point on the normalised plane * \return to obtain the distorted point: p_d = p_u + d_u */ void CataCamera::distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u) const { double k1 = mParameters.k1(); double k2 = mParameters.k2(); double p1 = mParameters.p1(); double p2 = mParameters.p2(); double mx2_u, my2_u, mxy_u, rho2_u, rad_dist_u; mx2_u = p_u(0) * p_u(0); my2_u = p_u(1) * p_u(1); mxy_u = p_u(0) * p_u(1); rho2_u = mx2_u + my2_u; rad_dist_u = k1 * rho2_u + k2 * rho2_u * rho2_u; d_u << p_u(0) * rad_dist_u + 2.0 * p1 * mxy_u + p2 * (rho2_u + 2.0 * mx2_u), p_u(1) * rad_dist_u + 2.0 * p2 * mxy_u + p1 * (rho2_u + 2.0 * my2_u); } /** * \brief Apply distortion to input point (from the normalised plane) * and calculate Jacobian * * \param p_u undistorted coordinates of point on the normalised plane * \return to obtain the distorted point: p_d = p_u + d_u */ void CataCamera::distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u, Eigen::Matrix2d& J) const { double k1 = mParameters.k1(); double k2 = mParameters.k2(); double p1 = mParameters.p1(); double p2 = mParameters.p2(); double mx2_u, my2_u, mxy_u, rho2_u, rad_dist_u; mx2_u = p_u(0) * p_u(0); my2_u = p_u(1) * p_u(1); mxy_u = p_u(0) * p_u(1); rho2_u = mx2_u + my2_u; rad_dist_u = k1 * rho2_u + k2 * rho2_u * rho2_u; d_u << p_u(0) * rad_dist_u + 2.0 * p1 * mxy_u + p2 * (rho2_u + 2.0 * mx2_u), p_u(1) * rad_dist_u + 2.0 * p2 * mxy_u + p1 * (rho2_u + 2.0 * my2_u); double dxdmx = 1.0 + rad_dist_u + k1 * 2.0 * mx2_u + k2 * rho2_u * 4.0 * mx2_u + 2.0 * p1 * p_u(1) + 6.0 * p2 * p_u(0); double dydmx = k1 * 2.0 * p_u(0) * p_u(1) + k2 * 4.0 * rho2_u * p_u(0) * p_u(1) + p1 * 2.0 * p_u(0) + 2.0 * p2 * p_u(1); double dxdmy = dydmx; double dydmy = 1.0 + rad_dist_u + k1 * 2.0 * my2_u + k2 * rho2_u * 4.0 * my2_u + 6.0 * p1 * p_u(1) + 2.0 * p2 * p_u(0); J << dxdmx, dxdmy, dydmx, dydmy; } void CataCamera::initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale) const { cv::Size imageSize(mParameters.imageWidth(), mParameters.imageHeight()); cv::Mat mapX = cv::Mat::zeros(imageSize, CV_32F); cv::Mat mapY = cv::Mat::zeros(imageSize, CV_32F); for (int v = 0; v < imageSize.height; ++v) { for (int u = 0; u < imageSize.width; ++u) { double mx_u = m_inv_K11 / fScale * u + m_inv_K13 / fScale; double my_u = m_inv_K22 / fScale * v + m_inv_K23 / fScale; double xi = mParameters.xi(); double d2 = mx_u * mx_u + my_u * my_u; Eigen::Vector3d P; P << mx_u, my_u, 1.0 - xi * (d2 + 1.0) / (xi + sqrt(1.0 + (1.0 - xi * xi) * d2)); Eigen::Vector2d p; spaceToPlane(P, p); mapX.at(v,u) = p(0); mapY.at(v,u) = p(1); } } cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false); } cv::Mat CataCamera::initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2, float fx, float fy, cv::Size imageSize, float cx, float cy, cv::Mat rmat) const { if (imageSize == cv::Size(0, 0)) { imageSize = cv::Size(mParameters.imageWidth(), mParameters.imageHeight()); } cv::Mat mapX = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F); cv::Mat mapY = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F); Eigen::Matrix3f K_rect; if (cx == -1.0f && cy == -1.0f) { K_rect << fx, 0, imageSize.width / 2, 0, fy, imageSize.height / 2, 0, 0, 1; } else { K_rect << fx, 0, cx, 0, fy, cy, 0, 0, 1; } if (fx == -1.0f || fy == -1.0f) { K_rect(0,0) = mParameters.gamma1(); K_rect(1,1) = mParameters.gamma2(); } Eigen::Matrix3f K_rect_inv = K_rect.inverse(); Eigen::Matrix3f R, R_inv; cv::cv2eigen(rmat, R); R_inv = R.inverse(); for (int v = 0; v < imageSize.height; ++v) { for (int u = 0; u < imageSize.width; ++u) { Eigen::Vector3f xo; xo << u, v, 1; Eigen::Vector3f uo = R_inv * K_rect_inv * xo; Eigen::Vector2d p; spaceToPlane(uo.cast(), p); mapX.at(v,u) = p(0); mapY.at(v,u) = p(1); } } cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false); cv::Mat K_rect_cv; cv::eigen2cv(K_rect, K_rect_cv); return K_rect_cv; } int CataCamera::parameterCount(void) const { return 9; } const CataCamera::Parameters& CataCamera::getParameters(void) const { return mParameters; } void CataCamera::setParameters(const CataCamera::Parameters& parameters) { mParameters = parameters; if ((mParameters.k1() == 0.0) && (mParameters.k2() == 0.0) && (mParameters.p1() == 0.0) && (mParameters.p2() == 0.0)) { m_noDistortion = true; } else { m_noDistortion = false; } m_inv_K11 = 1.0 / mParameters.gamma1(); m_inv_K13 = -mParameters.u0() / mParameters.gamma1(); m_inv_K22 = 1.0 / mParameters.gamma2(); m_inv_K23 = -mParameters.v0() / mParameters.gamma2(); } void CataCamera::readParameters(const std::vector& parameterVec) { if ((int)parameterVec.size() != parameterCount()) { return; } Parameters params = getParameters(); params.xi() = parameterVec.at(0); params.k1() = parameterVec.at(1); params.k2() = parameterVec.at(2); params.p1() = parameterVec.at(3); params.p2() = parameterVec.at(4); params.gamma1() = parameterVec.at(5); params.gamma2() = parameterVec.at(6); params.u0() = parameterVec.at(7); params.v0() = parameterVec.at(8); setParameters(params); } void CataCamera::writeParameters(std::vector& parameterVec) const { parameterVec.resize(parameterCount()); parameterVec.at(0) = mParameters.xi(); parameterVec.at(1) = mParameters.k1(); parameterVec.at(2) = mParameters.k2(); parameterVec.at(3) = mParameters.p1(); parameterVec.at(4) = mParameters.p2(); parameterVec.at(5) = mParameters.gamma1(); parameterVec.at(6) = mParameters.gamma2(); parameterVec.at(7) = mParameters.u0(); parameterVec.at(8) = mParameters.v0(); } void CataCamera::writeParametersToYamlFile(const std::string& filename) const { mParameters.writeToYamlFile(filename); } std::string CataCamera::parametersToString(void) const { std::ostringstream oss; oss << mParameters; return oss.str(); } } ================================================ FILE: camera_model/src/camera_models/CostFunctionFactory.cc ================================================ #include "camodocal/camera_models/CostFunctionFactory.h" #include "ceres/ceres.h" #include "camodocal/camera_models/CataCamera.h" #include "camodocal/camera_models/EquidistantCamera.h" #include "camodocal/camera_models/PinholeCamera.h" #include "camodocal/camera_models/ScaramuzzaCamera.h" namespace camodocal { template void worldToCameraTransform(const T* const q_cam_odo, const T* const t_cam_odo, const T* const p_odo, const T* const att_odo, T* q, T* t, bool optimize_cam_odo_z = true) { Eigen::Quaternion q_z_inv(cos(att_odo[0] / T(2)), T(0), T(0), -sin(att_odo[0] / T(2))); Eigen::Quaternion q_y_inv(cos(att_odo[1] / T(2)), T(0), -sin(att_odo[1] / T(2)), T(0)); Eigen::Quaternion q_x_inv(cos(att_odo[2] / T(2)), -sin(att_odo[2] / T(2)), T(0), T(0)); Eigen::Quaternion q_zyx_inv = q_x_inv * q_y_inv * q_z_inv; T q_odo[4] = {q_zyx_inv.w(), q_zyx_inv.x(), q_zyx_inv.y(), q_zyx_inv.z()}; T q_odo_cam[4] = {q_cam_odo[3], -q_cam_odo[0], -q_cam_odo[1], -q_cam_odo[2]}; T q0[4]; ceres::QuaternionProduct(q_odo_cam, q_odo, q0); T t0[3]; T t_odo[3] = {p_odo[0], p_odo[1], p_odo[2]}; ceres::QuaternionRotatePoint(q_odo, t_odo, t0); t0[0] += t_cam_odo[0]; t0[1] += t_cam_odo[1]; if (optimize_cam_odo_z) { t0[2] += t_cam_odo[2]; } ceres::QuaternionRotatePoint(q_odo_cam, t0, t); t[0] = -t[0]; t[1] = -t[1]; t[2] = -t[2]; // Convert quaternion from Ceres convention (w, x, y, z) // to Eigen convention (x, y, z, w) q[0] = q0[1]; q[1] = q0[2]; q[2] = q0[3]; q[3] = q0[0]; } template class ReprojectionError1 { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW ReprojectionError1(const Eigen::Vector3d& observed_P, const Eigen::Vector2d& observed_p) : m_observed_P(observed_P), m_observed_p(observed_p) , m_sqrtPrecisionMat(Eigen::Matrix2d::Identity()) {} ReprojectionError1(const Eigen::Vector3d& observed_P, const Eigen::Vector2d& observed_p, const Eigen::Matrix2d& sqrtPrecisionMat) : m_observed_P(observed_P), m_observed_p(observed_p) , m_sqrtPrecisionMat(sqrtPrecisionMat) {} ReprojectionError1(const std::vector& intrinsic_params, const Eigen::Vector3d& observed_P, const Eigen::Vector2d& observed_p) : m_intrinsic_params(intrinsic_params) , m_observed_P(observed_P), m_observed_p(observed_p) {} // variables: camera intrinsics and camera extrinsics template bool operator()(const T* const intrinsic_params, const T* const q, const T* const t, T* residuals) const { Eigen::Matrix P = m_observed_P.cast(); Eigen::Matrix predicted_p; CameraT::spaceToPlane(intrinsic_params, q, t, P, predicted_p); Eigen::Matrix e = predicted_p - m_observed_p.cast(); Eigen::Matrix e_weighted = m_sqrtPrecisionMat.cast() * e; residuals[0] = e_weighted(0); residuals[1] = e_weighted(1); return true; } // variables: camera-odometry transforms and odometry poses template bool operator()(const T* const q_cam_odo, const T* const t_cam_odo, const T* const p_odo, const T* const att_odo, T* residuals) const { T q[4], t[3]; worldToCameraTransform(q_cam_odo, t_cam_odo, p_odo, att_odo, q, t); Eigen::Matrix P = m_observed_P.cast(); std::vector intrinsic_params(m_intrinsic_params.begin(), m_intrinsic_params.end()); // project 3D object point to the image plane Eigen::Matrix predicted_p; CameraT::spaceToPlane(intrinsic_params.data(), q, t, P, predicted_p); residuals[0] = predicted_p(0) - T(m_observed_p(0)); residuals[1] = predicted_p(1) - T(m_observed_p(1)); return true; } //private: // camera intrinsics std::vector m_intrinsic_params; // observed 3D point Eigen::Vector3d m_observed_P; // observed 2D point Eigen::Vector2d m_observed_p; // square root of precision matrix Eigen::Matrix2d m_sqrtPrecisionMat; }; // variables: camera extrinsics, 3D point template class ReprojectionError2 { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW ReprojectionError2(const std::vector& intrinsic_params, const Eigen::Vector2d& observed_p) : m_intrinsic_params(intrinsic_params), m_observed_p(observed_p) {} template bool operator()(const T* const q, const T* const t, const T* const point, T* residuals) const { Eigen::Matrix P; P(0) = T(point[0]); P(1) = T(point[1]); P(2) = T(point[2]); std::vector intrinsic_params(m_intrinsic_params.begin(), m_intrinsic_params.end()); // project 3D object point to the image plane Eigen::Matrix predicted_p; CameraT::spaceToPlane(intrinsic_params.data(), q, t, P, predicted_p); residuals[0] = predicted_p(0) - T(m_observed_p(0)); residuals[1] = predicted_p(1) - T(m_observed_p(1)); return true; } private: // camera intrinsics std::vector m_intrinsic_params; // observed 2D point Eigen::Vector2d m_observed_p; }; template class ReprojectionError3 { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW ReprojectionError3(const Eigen::Vector2d& observed_p) : m_observed_p(observed_p) , m_sqrtPrecisionMat(Eigen::Matrix2d::Identity()) , m_optimize_cam_odo_z(true) {} ReprojectionError3(const Eigen::Vector2d& observed_p, const Eigen::Matrix2d& sqrtPrecisionMat) : m_observed_p(observed_p) , m_sqrtPrecisionMat(sqrtPrecisionMat) , m_optimize_cam_odo_z(true) {} ReprojectionError3(const std::vector& intrinsic_params, const Eigen::Vector2d& observed_p) : m_intrinsic_params(intrinsic_params) , m_observed_p(observed_p) , m_sqrtPrecisionMat(Eigen::Matrix2d::Identity()) , m_optimize_cam_odo_z(true) {} ReprojectionError3(const std::vector& intrinsic_params, const Eigen::Vector2d& observed_p, const Eigen::Matrix2d& sqrtPrecisionMat) : m_intrinsic_params(intrinsic_params) , m_observed_p(observed_p) , m_sqrtPrecisionMat(sqrtPrecisionMat) , m_optimize_cam_odo_z(true) {} ReprojectionError3(const std::vector& intrinsic_params, const Eigen::Vector3d& odo_pos, const Eigen::Vector3d& odo_att, const Eigen::Vector2d& observed_p, bool optimize_cam_odo_z) : m_intrinsic_params(intrinsic_params) , m_odo_pos(odo_pos), m_odo_att(odo_att) , m_observed_p(observed_p) , m_optimize_cam_odo_z(optimize_cam_odo_z) {} ReprojectionError3(const std::vector& intrinsic_params, const Eigen::Quaterniond& cam_odo_q, const Eigen::Vector3d& cam_odo_t, const Eigen::Vector3d& odo_pos, const Eigen::Vector3d& odo_att, const Eigen::Vector2d& observed_p) : m_intrinsic_params(intrinsic_params) , m_cam_odo_q(cam_odo_q), m_cam_odo_t(cam_odo_t) , m_odo_pos(odo_pos), m_odo_att(odo_att) , m_observed_p(observed_p) , m_optimize_cam_odo_z(true) {} // variables: camera intrinsics, camera-to-odometry transform, // odometry extrinsics, 3D point template bool operator()(const T* const intrinsic_params, const T* const q_cam_odo, const T* const t_cam_odo, const T* const p_odo, const T* const att_odo, const T* const point, T* residuals) const { T q[4], t[3]; worldToCameraTransform(q_cam_odo, t_cam_odo, p_odo, att_odo, q, t, m_optimize_cam_odo_z); Eigen::Matrix P(point[0], point[1], point[2]); // project 3D object point to the image plane Eigen::Matrix predicted_p; CameraT::spaceToPlane(intrinsic_params, q, t, P, predicted_p); Eigen::Matrix err = predicted_p - m_observed_p.cast(); Eigen::Matrix err_weighted = m_sqrtPrecisionMat.cast() * err; residuals[0] = err_weighted(0); residuals[1] = err_weighted(1); return true; } // variables: camera-to-odometry transform, 3D point template bool operator()(const T* const q_cam_odo, const T* const t_cam_odo, const T* const point, T* residuals) const { T p_odo[3] = {T(m_odo_pos(0)), T(m_odo_pos(1)), T(m_odo_pos(2))}; T att_odo[3] = {T(m_odo_att(0)), T(m_odo_att(1)), T(m_odo_att(2))}; T q[4], t[3]; worldToCameraTransform(q_cam_odo, t_cam_odo, p_odo, att_odo, q, t, m_optimize_cam_odo_z); std::vector intrinsic_params(m_intrinsic_params.begin(), m_intrinsic_params.end()); Eigen::Matrix P(point[0], point[1], point[2]); // project 3D object point to the image plane Eigen::Matrix predicted_p; CameraT::spaceToPlane(intrinsic_params.data(), q, t, P, predicted_p); residuals[0] = predicted_p(0) - T(m_observed_p(0)); residuals[1] = predicted_p(1) - T(m_observed_p(1)); return true; } // variables: camera-to-odometry transform, odometry extrinsics, 3D point template bool operator()(const T* const q_cam_odo, const T* const t_cam_odo, const T* const p_odo, const T* const att_odo, const T* const point, T* residuals) const { T q[4], t[3]; worldToCameraTransform(q_cam_odo, t_cam_odo, p_odo, att_odo, q, t, m_optimize_cam_odo_z); std::vector intrinsic_params(m_intrinsic_params.begin(), m_intrinsic_params.end()); Eigen::Matrix P(point[0], point[1], point[2]); // project 3D object point to the image plane Eigen::Matrix predicted_p; CameraT::spaceToPlane(intrinsic_params.data(), q, t, P, predicted_p); Eigen::Matrix err = predicted_p - m_observed_p.cast(); Eigen::Matrix err_weighted = m_sqrtPrecisionMat.cast() * err; residuals[0] = err_weighted(0); residuals[1] = err_weighted(1); return true; } // variables: 3D point template bool operator()(const T* const point, T* residuals) const { T q_cam_odo[4] = {T(m_cam_odo_q.coeffs()(0)), T(m_cam_odo_q.coeffs()(1)), T(m_cam_odo_q.coeffs()(2)), T(m_cam_odo_q.coeffs()(3))}; T t_cam_odo[3] = {T(m_cam_odo_t(0)), T(m_cam_odo_t(1)), T(m_cam_odo_t(2))}; T p_odo[3] = {T(m_odo_pos(0)), T(m_odo_pos(1)), T(m_odo_pos(2))}; T att_odo[3] = {T(m_odo_att(0)), T(m_odo_att(1)), T(m_odo_att(2))}; T q[4], t[3]; worldToCameraTransform(q_cam_odo, t_cam_odo, p_odo, att_odo, q, t, m_optimize_cam_odo_z); std::vector intrinsic_params(m_intrinsic_params.begin(), m_intrinsic_params.end()); Eigen::Matrix P(point[0], point[1], point[2]); // project 3D object point to the image plane Eigen::Matrix predicted_p; CameraT::spaceToPlane(intrinsic_params.data(), q, t, P, predicted_p); residuals[0] = predicted_p(0) - T(m_observed_p(0)); residuals[1] = predicted_p(1) - T(m_observed_p(1)); return true; } private: // camera intrinsics std::vector m_intrinsic_params; // observed camera-odometry transform Eigen::Quaterniond m_cam_odo_q; Eigen::Vector3d m_cam_odo_t; // observed odometry Eigen::Vector3d m_odo_pos; Eigen::Vector3d m_odo_att; // observed 2D point Eigen::Vector2d m_observed_p; Eigen::Matrix2d m_sqrtPrecisionMat; bool m_optimize_cam_odo_z; }; // variables: camera intrinsics and camera extrinsics template class StereoReprojectionError { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW StereoReprojectionError(const Eigen::Vector3d& observed_P, const Eigen::Vector2d& observed_p_l, const Eigen::Vector2d& observed_p_r) : m_observed_P(observed_P) , m_observed_p_l(observed_p_l) , m_observed_p_r(observed_p_r) { } template bool operator()(const T* const intrinsic_params_l, const T* const intrinsic_params_r, const T* const q_l, const T* const t_l, const T* const q_l_r, const T* const t_l_r, T* residuals) const { Eigen::Matrix P; P(0) = T(m_observed_P(0)); P(1) = T(m_observed_P(1)); P(2) = T(m_observed_P(2)); Eigen::Matrix predicted_p_l; CameraT::spaceToPlane(intrinsic_params_l, q_l, t_l, P, predicted_p_l); Eigen::Quaternion q_r = Eigen::Quaternion(q_l_r) * Eigen::Quaternion(q_l); Eigen::Matrix t_r; t_r(0) = t_l[0]; t_r(1) = t_l[1]; t_r(2) = t_l[2]; t_r = Eigen::Quaternion(q_l_r) * t_r; t_r(0) += t_l_r[0]; t_r(1) += t_l_r[1]; t_r(2) += t_l_r[2]; Eigen::Matrix predicted_p_r; CameraT::spaceToPlane(intrinsic_params_r, q_r.coeffs().data(), t_r.data(), P, predicted_p_r); residuals[0] = predicted_p_l(0) - T(m_observed_p_l(0)); residuals[1] = predicted_p_l(1) - T(m_observed_p_l(1)); residuals[2] = predicted_p_r(0) - T(m_observed_p_r(0)); residuals[3] = predicted_p_r(1) - T(m_observed_p_r(1)); return true; } private: // observed 3D point Eigen::Vector3d m_observed_P; // observed 2D point Eigen::Vector2d m_observed_p_l; Eigen::Vector2d m_observed_p_r; }; template class ComprehensionError { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW ComprehensionError(const Eigen::Vector3d& observed_P, const Eigen::Vector2d& observed_p) : m_observed_P(observed_P), m_observed_p(observed_p), m_sqrtPrecisionMat(Eigen::Matrix2d::Identity()) { } template bool operator()(const T* const intrinsic_params, const T* const q, const T* const t, T* residuals) const { { Eigen::Matrix p = m_observed_p.cast(); Eigen::Matrix predicted_img_P; CameraT::LiftToSphere(intrinsic_params, p, predicted_img_P); Eigen::Matrix predicted_p; CameraT::SphereToPlane(intrinsic_params, predicted_img_P, predicted_p); Eigen::Matrix e = predicted_p - m_observed_p.cast(); Eigen::Matrix e_weighted = m_sqrtPrecisionMat.cast() * e; residuals[0] = e_weighted(0); residuals[1] = e_weighted(1); } { Eigen::Matrix P = m_observed_P.cast(); Eigen::Matrix predicted_p; CameraT::spaceToPlane(intrinsic_params, q, t, P, predicted_p); Eigen::Matrix e = predicted_p - m_observed_p.cast(); Eigen::Matrix e_weighted = m_sqrtPrecisionMat.cast() * e; residuals[2] = e_weighted(0); residuals[3] = e_weighted(1); } return true; } // private: // camera intrinsics // std::vector m_intrinsic_params; // observed 3D point Eigen::Vector3d m_observed_P; // observed 2D point Eigen::Vector2d m_observed_p; // square root of precision matrix Eigen::Matrix2d m_sqrtPrecisionMat; }; boost::shared_ptr CostFunctionFactory::m_instance; CostFunctionFactory::CostFunctionFactory() { } boost::shared_ptr CostFunctionFactory::instance(void) { if (m_instance.get() == 0) { m_instance.reset(new CostFunctionFactory); } return m_instance; } ceres::CostFunction* CostFunctionFactory::generateCostFunction(const CameraConstPtr& camera, const Eigen::Vector3d& observed_P, const Eigen::Vector2d& observed_p, int flags) const { ceres::CostFunction* costFunction = 0; std::vector intrinsic_params; camera->writeParameters(intrinsic_params); switch (flags) { case CAMERA_INTRINSICS | CAMERA_POSE: switch (camera->modelType()) { case Camera::KANNALA_BRANDT: costFunction = new ceres::AutoDiffCostFunction, 2, 8, 4, 3>( new ReprojectionError1(observed_P, observed_p)); break; case Camera::PINHOLE: costFunction = new ceres::AutoDiffCostFunction, 2, 8, 4, 3>( new ReprojectionError1(observed_P, observed_p)); break; case Camera::MEI: costFunction = new ceres::AutoDiffCostFunction, 2, 9, 4, 3>( new ReprojectionError1(observed_P, observed_p)); break; case Camera::SCARAMUZZA: costFunction = new ceres::AutoDiffCostFunction, 4, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3>( new ComprehensionError(observed_P, observed_p)); // new ceres::AutoDiffCostFunction, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3>( // new ReprojectionError1(observed_P, observed_p)); break; } break; case CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_6D_POSE: switch (camera->modelType()) { case Camera::KANNALA_BRANDT: costFunction = new ceres::AutoDiffCostFunction, 2, 4, 3, 3, 3>( new ReprojectionError1(intrinsic_params, observed_P, observed_p)); break; case Camera::PINHOLE: costFunction = new ceres::AutoDiffCostFunction, 2, 4, 3, 3, 3>( new ReprojectionError1(intrinsic_params, observed_P, observed_p)); break; case Camera::MEI: costFunction = new ceres::AutoDiffCostFunction, 2, 4, 3, 3, 3>( new ReprojectionError1(intrinsic_params, observed_P, observed_p)); break; case Camera::SCARAMUZZA: costFunction = new ceres::AutoDiffCostFunction, 2, 4, 3, 3, 3>( new ReprojectionError1(intrinsic_params, observed_P, observed_p)); break; } break; } return costFunction; } ceres::CostFunction* CostFunctionFactory::generateCostFunction(const CameraConstPtr& camera, const Eigen::Vector3d& observed_P, const Eigen::Vector2d& observed_p, const Eigen::Matrix2d& sqrtPrecisionMat, int flags) const { ceres::CostFunction* costFunction = 0; std::vector intrinsic_params; camera->writeParameters(intrinsic_params); switch (flags) { case CAMERA_INTRINSICS | CAMERA_POSE: switch (camera->modelType()) { case Camera::KANNALA_BRANDT: costFunction = new ceres::AutoDiffCostFunction, 2, 8, 4, 3>( new ReprojectionError1(observed_P, observed_p, sqrtPrecisionMat)); break; case Camera::PINHOLE: costFunction = new ceres::AutoDiffCostFunction, 2, 8, 4, 3>( new ReprojectionError1(observed_P, observed_p, sqrtPrecisionMat)); break; case Camera::MEI: costFunction = new ceres::AutoDiffCostFunction, 2, 9, 4, 3>( new ReprojectionError1(observed_P, observed_p, sqrtPrecisionMat)); break; case Camera::SCARAMUZZA: costFunction = new ceres::AutoDiffCostFunction, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3>( new ReprojectionError1(observed_P, observed_p, sqrtPrecisionMat)); break; } break; } return costFunction; } ceres::CostFunction* CostFunctionFactory::generateCostFunction(const CameraConstPtr& camera, const Eigen::Vector2d& observed_p, int flags, bool optimize_cam_odo_z) const { ceres::CostFunction* costFunction = 0; std::vector intrinsic_params; camera->writeParameters(intrinsic_params); switch (flags) { case CAMERA_POSE | POINT_3D: switch (camera->modelType()) { case Camera::KANNALA_BRANDT: costFunction = new ceres::AutoDiffCostFunction, 2, 4, 3, 3>( new ReprojectionError2(intrinsic_params, observed_p)); break; case Camera::PINHOLE: costFunction = new ceres::AutoDiffCostFunction, 2, 4, 3, 3>( new ReprojectionError2(intrinsic_params, observed_p)); break; case Camera::MEI: costFunction = new ceres::AutoDiffCostFunction, 2, 4, 3, 3>( new ReprojectionError2(intrinsic_params, observed_p)); break; case Camera::SCARAMUZZA: costFunction = new ceres::AutoDiffCostFunction, 2, 4, 3, 3>( new ReprojectionError2(intrinsic_params, observed_p)); break; } break; case CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_3D_POSE | POINT_3D: switch (camera->modelType()) { case Camera::KANNALA_BRANDT: if (optimize_cam_odo_z) { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 3, 2, 1, 3>( new ReprojectionError3(intrinsic_params, observed_p)); } else { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 2, 2, 1, 3>( new ReprojectionError3(intrinsic_params, observed_p)); } break; case Camera::PINHOLE: if (optimize_cam_odo_z) { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 3, 2, 1, 3>( new ReprojectionError3(intrinsic_params, observed_p)); } else { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 2, 2, 1, 3>( new ReprojectionError3(intrinsic_params, observed_p)); } break; case Camera::MEI: if (optimize_cam_odo_z) { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 3, 2, 1, 3>( new ReprojectionError3(intrinsic_params, observed_p)); } else { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 2, 2, 1, 3>( new ReprojectionError3(intrinsic_params, observed_p)); } break; case Camera::SCARAMUZZA: if (optimize_cam_odo_z) { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 3, 2, 1, 3>( new ReprojectionError3(intrinsic_params, observed_p)); } else { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 2, 2, 1, 3>( new ReprojectionError3(intrinsic_params, observed_p)); } break; } break; case CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_6D_POSE | POINT_3D: switch (camera->modelType()) { case Camera::KANNALA_BRANDT: if (optimize_cam_odo_z) { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 3, 3, 3, 3>( new ReprojectionError3(intrinsic_params, observed_p)); } else { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 2, 3, 3, 3>( new ReprojectionError3(intrinsic_params, observed_p)); } break; case Camera::PINHOLE: if (optimize_cam_odo_z) { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 3, 3, 3, 3>( new ReprojectionError3(intrinsic_params, observed_p)); } else { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 2, 3, 3, 3>( new ReprojectionError3(intrinsic_params, observed_p)); } break; case Camera::MEI: if (optimize_cam_odo_z) { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 3, 3, 3, 3>( new ReprojectionError3(intrinsic_params, observed_p)); } else { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 2, 3, 3, 3>( new ReprojectionError3(intrinsic_params, observed_p)); } break; case Camera::SCARAMUZZA: if (optimize_cam_odo_z) { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 3, 3, 3, 3>( new ReprojectionError3(intrinsic_params, observed_p)); } else { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 2, 3, 3, 3>( new ReprojectionError3(intrinsic_params, observed_p)); } break; } break; case CAMERA_INTRINSICS | CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_3D_POSE | POINT_3D: switch (camera->modelType()) { case Camera::KANNALA_BRANDT: if (optimize_cam_odo_z) { costFunction = new ceres::AutoDiffCostFunction, 2, 8, 4, 3, 2, 1, 3>( new ReprojectionError3(observed_p)); } else { costFunction = new ceres::AutoDiffCostFunction, 2, 8, 4, 2, 2, 1, 3>( new ReprojectionError3(observed_p)); } break; case Camera::PINHOLE: if (optimize_cam_odo_z) { costFunction = new ceres::AutoDiffCostFunction, 2, 8, 4, 3, 2, 1, 3>( new ReprojectionError3(observed_p)); } else { costFunction = new ceres::AutoDiffCostFunction, 2, 8, 4, 2, 2, 1, 3>( new ReprojectionError3(observed_p)); } break; case Camera::MEI: if (optimize_cam_odo_z) { costFunction = new ceres::AutoDiffCostFunction, 2, 9, 4, 3, 2, 1, 3>( new ReprojectionError3(observed_p)); } else { costFunction = new ceres::AutoDiffCostFunction, 2, 9, 4, 2, 2, 1, 3>( new ReprojectionError3(observed_p)); } break; case Camera::SCARAMUZZA: if (optimize_cam_odo_z) { costFunction = new ceres::AutoDiffCostFunction, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3, 2, 1, 3>( new ReprojectionError3(observed_p)); } else { costFunction = new ceres::AutoDiffCostFunction, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 2, 2, 1, 3>( new ReprojectionError3(observed_p)); } break; } break; case CAMERA_INTRINSICS | CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_6D_POSE | POINT_3D: switch (camera->modelType()) { case Camera::KANNALA_BRANDT: if (optimize_cam_odo_z) { costFunction = new ceres::AutoDiffCostFunction, 2, 8, 4, 3, 3, 3, 3>( new ReprojectionError3(observed_p)); } else { costFunction = new ceres::AutoDiffCostFunction, 2, 8, 4, 2, 3, 3, 3>( new ReprojectionError3(observed_p)); } break; case Camera::PINHOLE: if (optimize_cam_odo_z) { costFunction = new ceres::AutoDiffCostFunction, 2, 8, 4, 3, 3, 3, 3>( new ReprojectionError3(observed_p)); } else { costFunction = new ceres::AutoDiffCostFunction, 2, 8, 4, 2, 3, 3, 3>( new ReprojectionError3(observed_p)); } break; case Camera::MEI: if (optimize_cam_odo_z) { costFunction = new ceres::AutoDiffCostFunction, 2, 9, 4, 3, 3, 3, 3>( new ReprojectionError3(observed_p)); } else { costFunction = new ceres::AutoDiffCostFunction, 2, 9, 4, 2, 3, 3, 3>( new ReprojectionError3(observed_p)); } break; case Camera::SCARAMUZZA: if (optimize_cam_odo_z) { costFunction = new ceres::AutoDiffCostFunction, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3, 3, 3, 3>( new ReprojectionError3(observed_p)); } else { costFunction = new ceres::AutoDiffCostFunction, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 2, 3, 3, 3>( new ReprojectionError3(observed_p)); } break; } break; } return costFunction; } ceres::CostFunction* CostFunctionFactory::generateCostFunction(const CameraConstPtr& camera, const Eigen::Vector2d& observed_p, const Eigen::Matrix2d& sqrtPrecisionMat, int flags, bool optimize_cam_odo_z) const { ceres::CostFunction* costFunction = 0; std::vector intrinsic_params; camera->writeParameters(intrinsic_params); switch (flags) { case CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_6D_POSE | POINT_3D: switch (camera->modelType()) { case Camera::KANNALA_BRANDT: if (optimize_cam_odo_z) { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 3, 3, 3, 3>( new ReprojectionError3(intrinsic_params, observed_p, sqrtPrecisionMat)); } else { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 2, 3, 3, 3>( new ReprojectionError3(intrinsic_params, observed_p, sqrtPrecisionMat)); } break; case Camera::PINHOLE: if (optimize_cam_odo_z) { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 3, 3, 3, 3>( new ReprojectionError3(intrinsic_params, observed_p, sqrtPrecisionMat)); } else { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 2, 3, 3, 3>( new ReprojectionError3(intrinsic_params, observed_p, sqrtPrecisionMat)); } break; case Camera::MEI: if (optimize_cam_odo_z) { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 3, 3, 3, 3>( new ReprojectionError3(intrinsic_params, observed_p, sqrtPrecisionMat)); } else { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 2, 3, 3, 3>( new ReprojectionError3(intrinsic_params, observed_p, sqrtPrecisionMat)); } break; case Camera::SCARAMUZZA: if (optimize_cam_odo_z) { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 3, 3, 3, 3>( new ReprojectionError3(intrinsic_params, observed_p, sqrtPrecisionMat)); } else { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 2, 3, 3, 3>( new ReprojectionError3(intrinsic_params, observed_p, sqrtPrecisionMat)); } break; } break; case CAMERA_INTRINSICS | CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_6D_POSE | POINT_3D: switch (camera->modelType()) { case Camera::KANNALA_BRANDT: if (optimize_cam_odo_z) { costFunction = new ceres::AutoDiffCostFunction, 2, 8, 4, 3, 3, 3, 3>( new ReprojectionError3(observed_p, sqrtPrecisionMat)); } else { costFunction = new ceres::AutoDiffCostFunction, 2, 8, 4, 2, 3, 3, 3>( new ReprojectionError3(observed_p, sqrtPrecisionMat)); } break; case Camera::PINHOLE: if (optimize_cam_odo_z) { costFunction = new ceres::AutoDiffCostFunction, 2, 8, 4, 3, 3, 3, 3>( new ReprojectionError3(observed_p, sqrtPrecisionMat)); } else { costFunction = new ceres::AutoDiffCostFunction, 2, 8, 4, 2, 3, 3, 3>( new ReprojectionError3(observed_p, sqrtPrecisionMat)); } break; case Camera::MEI: if (optimize_cam_odo_z) { costFunction = new ceres::AutoDiffCostFunction, 2, 9, 4, 3, 3, 3, 3>( new ReprojectionError3(observed_p, sqrtPrecisionMat)); } else { costFunction = new ceres::AutoDiffCostFunction, 2, 9, 4, 2, 3, 3, 3>( new ReprojectionError3(observed_p, sqrtPrecisionMat)); } break; case Camera::SCARAMUZZA: if (optimize_cam_odo_z) { costFunction = new ceres::AutoDiffCostFunction, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3, 3, 3, 3>( new ReprojectionError3(observed_p, sqrtPrecisionMat)); } else { costFunction = new ceres::AutoDiffCostFunction, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 2, 3, 3, 3>( new ReprojectionError3(observed_p, sqrtPrecisionMat)); } break; } break; } return costFunction; } ceres::CostFunction* CostFunctionFactory::generateCostFunction(const CameraConstPtr& camera, const Eigen::Vector3d& odo_pos, const Eigen::Vector3d& odo_att, const Eigen::Vector2d& observed_p, int flags, bool optimize_cam_odo_z) const { ceres::CostFunction* costFunction = 0; std::vector intrinsic_params; camera->writeParameters(intrinsic_params); switch (flags) { case CAMERA_ODOMETRY_TRANSFORM | POINT_3D: switch (camera->modelType()) { case Camera::KANNALA_BRANDT: if (optimize_cam_odo_z) { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 3, 3>( new ReprojectionError3(intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z)); } else { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 2, 3>( new ReprojectionError3(intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z)); } break; case Camera::PINHOLE: if (optimize_cam_odo_z) { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 3, 3>( new ReprojectionError3(intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z)); } else { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 2, 3>( new ReprojectionError3(intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z)); } break; case Camera::MEI: if (optimize_cam_odo_z) { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 3, 3>( new ReprojectionError3(intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z)); } else { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 2, 3>( new ReprojectionError3(intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z)); } break; case Camera::SCARAMUZZA: if (optimize_cam_odo_z) { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 3, 3>( new ReprojectionError3(intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z)); } else { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 2, 3>( new ReprojectionError3(intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z)); } break; } break; } return costFunction; } ceres::CostFunction* CostFunctionFactory::generateCostFunction(const CameraConstPtr& camera, const Eigen::Quaterniond& cam_odo_q, const Eigen::Vector3d& cam_odo_t, const Eigen::Vector3d& odo_pos, const Eigen::Vector3d& odo_att, const Eigen::Vector2d& observed_p, int flags) const { ceres::CostFunction* costFunction = 0; std::vector intrinsic_params; camera->writeParameters(intrinsic_params); switch (flags) { case POINT_3D: switch (camera->modelType()) { case Camera::KANNALA_BRANDT: costFunction = new ceres::AutoDiffCostFunction, 2, 3>( new ReprojectionError3(intrinsic_params, cam_odo_q, cam_odo_t, odo_pos, odo_att, observed_p)); break; case Camera::PINHOLE: costFunction = new ceres::AutoDiffCostFunction, 2, 3>( new ReprojectionError3(intrinsic_params, cam_odo_q, cam_odo_t, odo_pos, odo_att, observed_p)); break; case Camera::MEI: costFunction = new ceres::AutoDiffCostFunction, 2, 3>( new ReprojectionError3(intrinsic_params, cam_odo_q, cam_odo_t, odo_pos, odo_att, observed_p)); break; case Camera::SCARAMUZZA: costFunction = new ceres::AutoDiffCostFunction, 2, 3>( new ReprojectionError3(intrinsic_params, cam_odo_q, cam_odo_t, odo_pos, odo_att, observed_p)); break; } break; } return costFunction; } ceres::CostFunction* CostFunctionFactory::generateCostFunction(const CameraConstPtr& cameraL, const CameraConstPtr& cameraR, const Eigen::Vector3d& observed_P, const Eigen::Vector2d& observed_p_l, const Eigen::Vector2d& observed_p_r) const { ceres::CostFunction* costFunction = 0; if (cameraL->modelType() != cameraR->modelType()) { return costFunction; } switch (cameraL->modelType()) { case Camera::KANNALA_BRANDT: costFunction = new ceres::AutoDiffCostFunction, 4, 8, 8, 4, 3, 4, 3>( new StereoReprojectionError(observed_P, observed_p_l, observed_p_r)); break; case Camera::PINHOLE: costFunction = new ceres::AutoDiffCostFunction, 4, 8, 8, 4, 3, 4, 3>( new StereoReprojectionError(observed_P, observed_p_l, observed_p_r)); break; case Camera::MEI: costFunction = new ceres::AutoDiffCostFunction, 4, 9, 9, 4, 3, 4, 3>( new StereoReprojectionError(observed_P, observed_p_l, observed_p_r)); break; case Camera::SCARAMUZZA: costFunction = new ceres::AutoDiffCostFunction, 4, SCARAMUZZA_CAMERA_NUM_PARAMS, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3, 4, 3>( new StereoReprojectionError(observed_P, observed_p_l, observed_p_r)); break; } return costFunction; } } ================================================ FILE: camera_model/src/camera_models/EquidistantCamera.cc ================================================ #include "camodocal/camera_models/EquidistantCamera.h" #include #include #include #include #include #include #include #include #include "camodocal/gpl/gpl.h" namespace camodocal { EquidistantCamera::Parameters::Parameters() : Camera::Parameters(KANNALA_BRANDT) , m_k2(0.0) , m_k3(0.0) , m_k4(0.0) , m_k5(0.0) , m_mu(0.0) , m_mv(0.0) , m_u0(0.0) , m_v0(0.0) { } EquidistantCamera::Parameters::Parameters(const std::string& cameraName, int w, int h, double k2, double k3, double k4, double k5, double mu, double mv, double u0, double v0) : Camera::Parameters(KANNALA_BRANDT, cameraName, w, h) , m_k2(k2) , m_k3(k3) , m_k4(k4) , m_k5(k5) , m_mu(mu) , m_mv(mv) , m_u0(u0) , m_v0(v0) { } double& EquidistantCamera::Parameters::k2(void) { return m_k2; } double& EquidistantCamera::Parameters::k3(void) { return m_k3; } double& EquidistantCamera::Parameters::k4(void) { return m_k4; } double& EquidistantCamera::Parameters::k5(void) { return m_k5; } double& EquidistantCamera::Parameters::mu(void) { return m_mu; } double& EquidistantCamera::Parameters::mv(void) { return m_mv; } double& EquidistantCamera::Parameters::u0(void) { return m_u0; } double& EquidistantCamera::Parameters::v0(void) { return m_v0; } double EquidistantCamera::Parameters::k2(void) const { return m_k2; } double EquidistantCamera::Parameters::k3(void) const { return m_k3; } double EquidistantCamera::Parameters::k4(void) const { return m_k4; } double EquidistantCamera::Parameters::k5(void) const { return m_k5; } double EquidistantCamera::Parameters::mu(void) const { return m_mu; } double EquidistantCamera::Parameters::mv(void) const { return m_mv; } double EquidistantCamera::Parameters::u0(void) const { return m_u0; } double EquidistantCamera::Parameters::v0(void) const { return m_v0; } bool EquidistantCamera::Parameters::readFromYamlFile(const std::string& filename) { cv::FileStorage fs(filename, cv::FileStorage::READ); if (!fs.isOpened()) { return false; } if (!fs["model_type"].isNone()) { std::string sModelType; fs["model_type"] >> sModelType; if (sModelType.compare("KANNALA_BRANDT") != 0) { return false; } } m_modelType = KANNALA_BRANDT; fs["camera_name"] >> m_cameraName; m_imageWidth = static_cast(fs["image_width"]); m_imageHeight = static_cast(fs["image_height"]); cv::FileNode n = fs["projection_parameters"]; m_k2 = static_cast(n["k2"]); m_k3 = static_cast(n["k3"]); m_k4 = static_cast(n["k4"]); m_k5 = static_cast(n["k5"]); m_mu = static_cast(n["mu"]); m_mv = static_cast(n["mv"]); m_u0 = static_cast(n["u0"]); m_v0 = static_cast(n["v0"]); return true; } void EquidistantCamera::Parameters::writeToYamlFile(const std::string& filename) const { cv::FileStorage fs(filename, cv::FileStorage::WRITE); fs << "model_type" << "KANNALA_BRANDT"; fs << "camera_name" << m_cameraName; fs << "image_width" << m_imageWidth; fs << "image_height" << m_imageHeight; // projection: k2, k3, k4, k5, mu, mv, u0, v0 fs << "projection_parameters"; fs << "{" << "k2" << m_k2 << "k3" << m_k3 << "k4" << m_k4 << "k5" << m_k5 << "mu" << m_mu << "mv" << m_mv << "u0" << m_u0 << "v0" << m_v0 << "}"; fs.release(); } EquidistantCamera::Parameters& EquidistantCamera::Parameters::operator=(const EquidistantCamera::Parameters& other) { if (this != &other) { m_modelType = other.m_modelType; m_cameraName = other.m_cameraName; m_imageWidth = other.m_imageWidth; m_imageHeight = other.m_imageHeight; m_k2 = other.m_k2; m_k3 = other.m_k3; m_k4 = other.m_k4; m_k5 = other.m_k5; m_mu = other.m_mu; m_mv = other.m_mv; m_u0 = other.m_u0; m_v0 = other.m_v0; } return *this; } std::ostream& operator<< (std::ostream& out, const EquidistantCamera::Parameters& params) { out << "Camera Parameters:" << std::endl; out << " model_type " << "KANNALA_BRANDT" << std::endl; out << " camera_name " << params.m_cameraName << std::endl; out << " image_width " << params.m_imageWidth << std::endl; out << " image_height " << params.m_imageHeight << std::endl; // projection: k2, k3, k4, k5, mu, mv, u0, v0 out << "Projection Parameters" << std::endl; out << " k2 " << params.m_k2 << std::endl << " k3 " << params.m_k3 << std::endl << " k4 " << params.m_k4 << std::endl << " k5 " << params.m_k5 << std::endl << " mu " << params.m_mu << std::endl << " mv " << params.m_mv << std::endl << " u0 " << params.m_u0 << std::endl << " v0 " << params.m_v0 << std::endl; return out; } EquidistantCamera::EquidistantCamera() : m_inv_K11(1.0) , m_inv_K13(0.0) , m_inv_K22(1.0) , m_inv_K23(0.0) { } EquidistantCamera::EquidistantCamera(const std::string& cameraName, int imageWidth, int imageHeight, double k2, double k3, double k4, double k5, double mu, double mv, double u0, double v0) : mParameters(cameraName, imageWidth, imageHeight, k2, k3, k4, k5, mu, mv, u0, v0) { // Inverse camera projection matrix parameters m_inv_K11 = 1.0 / mParameters.mu(); m_inv_K13 = -mParameters.u0() / mParameters.mu(); m_inv_K22 = 1.0 / mParameters.mv(); m_inv_K23 = -mParameters.v0() / mParameters.mv(); } EquidistantCamera::EquidistantCamera(const EquidistantCamera::Parameters& params) : mParameters(params) { // Inverse camera projection matrix parameters m_inv_K11 = 1.0 / mParameters.mu(); m_inv_K13 = -mParameters.u0() / mParameters.mu(); m_inv_K22 = 1.0 / mParameters.mv(); m_inv_K23 = -mParameters.v0() / mParameters.mv(); } Camera::ModelType EquidistantCamera::modelType(void) const { return mParameters.modelType(); } const std::string& EquidistantCamera::cameraName(void) const { return mParameters.cameraName(); } int EquidistantCamera::imageWidth(void) const { return mParameters.imageWidth(); } int EquidistantCamera::imageHeight(void) const { return mParameters.imageHeight(); } void EquidistantCamera::estimateIntrinsics(const cv::Size& boardSize, const std::vector< std::vector >& objectPoints, const std::vector< std::vector >& imagePoints) { Parameters params = getParameters(); double u0 = params.imageWidth() / 2.0; double v0 = params.imageHeight() / 2.0; double minReprojErr = std::numeric_limits::max(); std::vector rvecs, tvecs; rvecs.assign(objectPoints.size(), cv::Mat()); tvecs.assign(objectPoints.size(), cv::Mat()); params.k2() = 0.0; params.k3() = 0.0; params.k4() = 0.0; params.k5() = 0.0; params.u0() = u0; params.v0() = v0; // Initialize focal length // C. Hughes, P. Denny, M. Glavin, and E. Jones, // Equidistant Fish-Eye Calibration and Rectification by Vanishing Point // Extraction, PAMI 2010 // Find circles from rows of chessboard corners, and for each pair // of circles, find vanishing points: v1 and v2. // f = ||v1 - v2|| / PI; double f0 = 0.0; for (size_t i = 0; i < imagePoints.size(); ++i) { std::vector center(boardSize.height); double radius[boardSize.height]; for (int r = 0; r < boardSize.height; ++r) { std::vector circle; for (int c = 0; c < boardSize.width; ++c) { circle.push_back(imagePoints.at(i).at(r * boardSize.width + c)); } fitCircle(circle, center[r](0), center[r](1), radius[r]); } for (int j = 0; j < boardSize.height; ++j) { for (int k = j + 1; k < boardSize.height; ++k) { // find distance between pair of vanishing points which // correspond to intersection points of 2 circles std::vector ipts; ipts = intersectCircles(center[j](0), center[j](1), radius[j], center[k](0), center[k](1), radius[k]); if (ipts.size() < 2) { continue; } double f = cv::norm(ipts.at(0) - ipts.at(1)) / M_PI; params.mu() = f; params.mv() = f; setParameters(params); for (size_t l = 0; l < objectPoints.size(); ++l) { estimateExtrinsics(objectPoints.at(l), imagePoints.at(l), rvecs.at(l), tvecs.at(l)); } double reprojErr = reprojectionError(objectPoints, imagePoints, rvecs, tvecs, cv::noArray()); if (reprojErr < minReprojErr) { minReprojErr = reprojErr; f0 = f; } } } } if (f0 <= 0.0 && minReprojErr >= std::numeric_limits::max()) { std::cout << "[" << params.cameraName() << "] " << "# INFO: kannala-Brandt model fails with given data. " << std::endl; return; } params.mu() = f0; params.mv() = f0; setParameters(params); } /** * \brief Lifts a point from the image plane to the unit sphere * * \param p image coordinates * \param P coordinates of the point on the sphere */ void EquidistantCamera::liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const { liftProjective(p, P); } /** * \brief Lifts a point from the image plane to its projective ray * * \param p image coordinates * \param P coordinates of the projective ray */ void EquidistantCamera::liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const { // Lift points to normalised plane Eigen::Vector2d p_u; p_u << m_inv_K11 * p(0) + m_inv_K13, m_inv_K22 * p(1) + m_inv_K23; // Obtain a projective ray double theta, phi; backprojectSymmetric(p_u, theta, phi); P(0) = sin(theta) * cos(phi); P(1) = sin(theta) * sin(phi); P(2) = cos(theta); } /** * \brief Project a 3D point (\a x,\a y,\a z) to the image plane in (\a u,\a v) * * \param P 3D point coordinates * \param p return value, contains the image point coordinates */ void EquidistantCamera::spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const { double theta = acos(P(2) / P.norm()); double phi = atan2(P(1), P(0)); Eigen::Vector2d p_u = r(mParameters.k2(), mParameters.k3(), mParameters.k4(), mParameters.k5(), theta) * Eigen::Vector2d(cos(phi), sin(phi)); // Apply generalised projection matrix p << mParameters.mu() * p_u(0) + mParameters.u0(), mParameters.mv() * p_u(1) + mParameters.v0(); } /** * \brief Project a 3D point to the image plane and calculate Jacobian * * \param P 3D point coordinates * \param p return value, contains the image point coordinates */ void EquidistantCamera::spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p, Eigen::Matrix& J) const { double theta = acos(P(2) / P.norm()); double phi = atan2(P(1), P(0)); Eigen::Vector2d p_u = r(mParameters.k2(), mParameters.k3(), mParameters.k4(), mParameters.k5(), theta) * Eigen::Vector2d(cos(phi), sin(phi)); // Apply generalised projection matrix p << mParameters.mu() * p_u(0) + mParameters.u0(), mParameters.mv() * p_u(1) + mParameters.v0(); } /** * \brief Projects an undistorted 2D point p_u to the image plane * * \param p_u 2D point coordinates * \return image point coordinates */ void EquidistantCamera::undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const { // Eigen::Vector2d p_d; // // if (m_noDistortion) // { // p_d = p_u; // } // else // { // // Apply distortion // Eigen::Vector2d d_u; // distortion(p_u, d_u); // p_d = p_u + d_u; // } // // // Apply generalised projection matrix // p << mParameters.gamma1() * p_d(0) + mParameters.u0(), // mParameters.gamma2() * p_d(1) + mParameters.v0(); } void EquidistantCamera::initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale) const { cv::Size imageSize(mParameters.imageWidth(), mParameters.imageHeight()); cv::Mat mapX = cv::Mat::zeros(imageSize, CV_32F); cv::Mat mapY = cv::Mat::zeros(imageSize, CV_32F); for (int v = 0; v < imageSize.height; ++v) { for (int u = 0; u < imageSize.width; ++u) { double mx_u = m_inv_K11 / fScale * u + m_inv_K13 / fScale; double my_u = m_inv_K22 / fScale * v + m_inv_K23 / fScale; double theta, phi; backprojectSymmetric(Eigen::Vector2d(mx_u, my_u), theta, phi); Eigen::Vector3d P; P << sin(theta) * cos(phi), sin(theta) * sin(phi), cos(theta); Eigen::Vector2d p; spaceToPlane(P, p); mapX.at(v,u) = p(0); mapY.at(v,u) = p(1); } } cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false); } cv::Mat EquidistantCamera::initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2, float fx, float fy, cv::Size imageSize, float cx, float cy, cv::Mat rmat) const { if (imageSize == cv::Size(0, 0)) { imageSize = cv::Size(mParameters.imageWidth(), mParameters.imageHeight()); } cv::Mat mapX = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F); cv::Mat mapY = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F); Eigen::Matrix3f K_rect; if (cx == -1.0f && cy == -1.0f) { K_rect << fx, 0, imageSize.width / 2, 0, fy, imageSize.height / 2, 0, 0, 1; } else { K_rect << fx, 0, cx, 0, fy, cy, 0, 0, 1; } if (fx == -1.0f || fy == -1.0f) { K_rect(0,0) = mParameters.mu(); K_rect(1,1) = mParameters.mv(); } Eigen::Matrix3f K_rect_inv = K_rect.inverse(); Eigen::Matrix3f R, R_inv; cv::cv2eigen(rmat, R); R_inv = R.inverse(); for (int v = 0; v < imageSize.height; ++v) { for (int u = 0; u < imageSize.width; ++u) { Eigen::Vector3f xo; xo << u, v, 1; Eigen::Vector3f uo = R_inv * K_rect_inv * xo; Eigen::Vector2d p; spaceToPlane(uo.cast(), p); mapX.at(v,u) = p(0); mapY.at(v,u) = p(1); } } cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false); cv::Mat K_rect_cv; cv::eigen2cv(K_rect, K_rect_cv); return K_rect_cv; } int EquidistantCamera::parameterCount(void) const { return 8; } const EquidistantCamera::Parameters& EquidistantCamera::getParameters(void) const { return mParameters; } void EquidistantCamera::setParameters(const EquidistantCamera::Parameters& parameters) { mParameters = parameters; // Inverse camera projection matrix parameters m_inv_K11 = 1.0 / mParameters.mu(); m_inv_K13 = -mParameters.u0() / mParameters.mu(); m_inv_K22 = 1.0 / mParameters.mv(); m_inv_K23 = -mParameters.v0() / mParameters.mv(); } void EquidistantCamera::readParameters(const std::vector& parameterVec) { if (parameterVec.size() != parameterCount()) { return; } Parameters params = getParameters(); params.k2() = parameterVec.at(0); params.k3() = parameterVec.at(1); params.k4() = parameterVec.at(2); params.k5() = parameterVec.at(3); params.mu() = parameterVec.at(4); params.mv() = parameterVec.at(5); params.u0() = parameterVec.at(6); params.v0() = parameterVec.at(7); setParameters(params); } void EquidistantCamera::writeParameters(std::vector& parameterVec) const { parameterVec.resize(parameterCount()); parameterVec.at(0) = mParameters.k2(); parameterVec.at(1) = mParameters.k3(); parameterVec.at(2) = mParameters.k4(); parameterVec.at(3) = mParameters.k5(); parameterVec.at(4) = mParameters.mu(); parameterVec.at(5) = mParameters.mv(); parameterVec.at(6) = mParameters.u0(); parameterVec.at(7) = mParameters.v0(); } void EquidistantCamera::writeParametersToYamlFile(const std::string& filename) const { mParameters.writeToYamlFile(filename); } std::string EquidistantCamera::parametersToString(void) const { std::ostringstream oss; oss << mParameters; return oss.str(); } void EquidistantCamera::fitOddPoly(const std::vector& x, const std::vector& y, int n, std::vector& coeffs) const { std::vector pows; for (int i = 1; i <= n; i += 2) { pows.push_back(i); } Eigen::MatrixXd X(x.size(), pows.size()); Eigen::MatrixXd Y(y.size(), 1); for (size_t i = 0; i < x.size(); ++i) { for (size_t j = 0; j < pows.size(); ++j) { X(i,j) = pow(x.at(i), pows.at(j)); } Y(i,0) = y.at(i); } Eigen::MatrixXd A = (X.transpose() * X).inverse() * X.transpose() * Y; coeffs.resize(A.rows()); for (int i = 0; i < A.rows(); ++i) { coeffs.at(i) = A(i,0); } } void EquidistantCamera::backprojectSymmetric(const Eigen::Vector2d& p_u, double& theta, double& phi) const { double tol = 1e-10; double p_u_norm = p_u.norm(); if (p_u_norm < 1e-10) { phi = 0.0; } else { phi = atan2(p_u(1), p_u(0)); } int npow = 9; if (mParameters.k5() == 0.0) { npow -= 2; } if (mParameters.k4() == 0.0) { npow -= 2; } if (mParameters.k3() == 0.0) { npow -= 2; } if (mParameters.k2() == 0.0) { npow -= 2; } Eigen::MatrixXd coeffs(npow + 1, 1); coeffs.setZero(); coeffs(0) = -p_u_norm; coeffs(1) = 1.0; if (npow >= 3) { coeffs(3) = mParameters.k2(); } if (npow >= 5) { coeffs(5) = mParameters.k3(); } if (npow >= 7) { coeffs(7) = mParameters.k4(); } if (npow >= 9) { coeffs(9) = mParameters.k5(); } if (npow == 1) { theta = p_u_norm; } else { // Get eigenvalues of companion matrix corresponding to polynomial. // Eigenvalues correspond to roots of polynomial. Eigen::MatrixXd A(npow, npow); A.setZero(); A.block(1, 0, npow - 1, npow - 1).setIdentity(); A.col(npow - 1) = - coeffs.block(0, 0, npow, 1) / coeffs(npow); Eigen::EigenSolver es(A); Eigen::MatrixXcd eigval = es.eigenvalues(); std::vector thetas; for (int i = 0; i < eigval.rows(); ++i) { if (fabs(eigval(i).imag()) > tol) { continue; } double t = eigval(i).real(); if (t < -tol) { continue; } else if (t < 0.0) { t = 0.0; } thetas.push_back(t); } if (thetas.empty()) { theta = p_u_norm; } else { theta = *std::min_element(thetas.begin(), thetas.end()); } } } } ================================================ FILE: camera_model/src/camera_models/PinholeCamera.cc ================================================ #include "camodocal/camera_models/PinholeCamera.h" #include #include #include #include #include #include #include #include "camodocal/gpl/gpl.h" namespace camodocal { PinholeCamera::Parameters::Parameters() : Camera::Parameters(PINHOLE) , m_k1(0.0) , m_k2(0.0) , m_p1(0.0) , m_p2(0.0) , m_fx(0.0) , m_fy(0.0) , m_cx(0.0) , m_cy(0.0) { } PinholeCamera::Parameters::Parameters(const std::string& cameraName, int w, int h, double k1, double k2, double p1, double p2, double fx, double fy, double cx, double cy) : Camera::Parameters(PINHOLE, cameraName, w, h) , m_k1(k1) , m_k2(k2) , m_p1(p1) , m_p2(p2) , m_fx(fx) , m_fy(fy) , m_cx(cx) , m_cy(cy) { } double& PinholeCamera::Parameters::k1(void) { return m_k1; } double& PinholeCamera::Parameters::k2(void) { return m_k2; } double& PinholeCamera::Parameters::p1(void) { return m_p1; } double& PinholeCamera::Parameters::p2(void) { return m_p2; } double& PinholeCamera::Parameters::fx(void) { return m_fx; } double& PinholeCamera::Parameters::fy(void) { return m_fy; } double& PinholeCamera::Parameters::cx(void) { return m_cx; } double& PinholeCamera::Parameters::cy(void) { return m_cy; } double PinholeCamera::Parameters::k1(void) const { return m_k1; } double PinholeCamera::Parameters::k2(void) const { return m_k2; } double PinholeCamera::Parameters::p1(void) const { return m_p1; } double PinholeCamera::Parameters::p2(void) const { return m_p2; } double PinholeCamera::Parameters::fx(void) const { return m_fx; } double PinholeCamera::Parameters::fy(void) const { return m_fy; } double PinholeCamera::Parameters::cx(void) const { return m_cx; } double PinholeCamera::Parameters::cy(void) const { return m_cy; } bool PinholeCamera::Parameters::readFromYamlFile(const std::string& filename) { cv::FileStorage fs(filename, cv::FileStorage::READ); if (!fs.isOpened()) { return false; } if (!fs["model_type"].isNone()) { std::string sModelType; fs["model_type"] >> sModelType; if (sModelType.compare("PINHOLE") != 0) { return false; } } m_modelType = PINHOLE; fs["camera_name"] >> m_cameraName; m_imageWidth = static_cast(fs["image_width"]); m_imageHeight = static_cast(fs["image_height"]); cv::FileNode n = fs["distortion_parameters"]; m_k1 = static_cast(n["k1"]); m_k2 = static_cast(n["k2"]); m_p1 = static_cast(n["p1"]); m_p2 = static_cast(n["p2"]); n = fs["projection_parameters"]; m_fx = static_cast(n["fx"]); m_fy = static_cast(n["fy"]); m_cx = static_cast(n["cx"]); m_cy = static_cast(n["cy"]); return true; } void PinholeCamera::Parameters::writeToYamlFile(const std::string& filename) const { cv::FileStorage fs(filename, cv::FileStorage::WRITE); fs << "model_type" << "PINHOLE"; fs << "camera_name" << m_cameraName; fs << "image_width" << m_imageWidth; fs << "image_height" << m_imageHeight; // radial distortion: k1, k2 // tangential distortion: p1, p2 fs << "distortion_parameters"; fs << "{" << "k1" << m_k1 << "k2" << m_k2 << "p1" << m_p1 << "p2" << m_p2 << "}"; // projection: fx, fy, cx, cy fs << "projection_parameters"; fs << "{" << "fx" << m_fx << "fy" << m_fy << "cx" << m_cx << "cy" << m_cy << "}"; fs.release(); } PinholeCamera::Parameters& PinholeCamera::Parameters::operator=(const PinholeCamera::Parameters& other) { if (this != &other) { m_modelType = other.m_modelType; m_cameraName = other.m_cameraName; m_imageWidth = other.m_imageWidth; m_imageHeight = other.m_imageHeight; m_k1 = other.m_k1; m_k2 = other.m_k2; m_p1 = other.m_p1; m_p2 = other.m_p2; m_fx = other.m_fx; m_fy = other.m_fy; m_cx = other.m_cx; m_cy = other.m_cy; } return *this; } std::ostream& operator<< (std::ostream& out, const PinholeCamera::Parameters& params) { out << "Camera Parameters:" << std::endl; out << " model_type " << "PINHOLE" << std::endl; out << " camera_name " << params.m_cameraName << std::endl; out << " image_width " << params.m_imageWidth << std::endl; out << " image_height " << params.m_imageHeight << std::endl; // radial distortion: k1, k2 // tangential distortion: p1, p2 out << "Distortion Parameters" << std::endl; out << " k1 " << params.m_k1 << std::endl << " k2 " << params.m_k2 << std::endl << " p1 " << params.m_p1 << std::endl << " p2 " << params.m_p2 << std::endl; // projection: fx, fy, cx, cy out << "Projection Parameters" << std::endl; out << " fx " << params.m_fx << std::endl << " fy " << params.m_fy << std::endl << " cx " << params.m_cx << std::endl << " cy " << params.m_cy << std::endl; return out; } PinholeCamera::PinholeCamera() : m_inv_K11(1.0) , m_inv_K13(0.0) , m_inv_K22(1.0) , m_inv_K23(0.0) , m_noDistortion(true) { } PinholeCamera::PinholeCamera(const std::string& cameraName, int imageWidth, int imageHeight, double k1, double k2, double p1, double p2, double fx, double fy, double cx, double cy) : mParameters(cameraName, imageWidth, imageHeight, k1, k2, p1, p2, fx, fy, cx, cy) { if ((mParameters.k1() == 0.0) && (mParameters.k2() == 0.0) && (mParameters.p1() == 0.0) && (mParameters.p2() == 0.0)) { m_noDistortion = true; } else { m_noDistortion = false; } // Inverse camera projection matrix parameters m_inv_K11 = 1.0 / mParameters.fx(); m_inv_K13 = -mParameters.cx() / mParameters.fx(); m_inv_K22 = 1.0 / mParameters.fy(); m_inv_K23 = -mParameters.cy() / mParameters.fy(); } PinholeCamera::PinholeCamera(const PinholeCamera::Parameters& params) : mParameters(params) { if ((mParameters.k1() == 0.0) && (mParameters.k2() == 0.0) && (mParameters.p1() == 0.0) && (mParameters.p2() == 0.0)) { m_noDistortion = true; } else { m_noDistortion = false; } // Inverse camera projection matrix parameters m_inv_K11 = 1.0 / mParameters.fx(); m_inv_K13 = -mParameters.cx() / mParameters.fx(); m_inv_K22 = 1.0 / mParameters.fy(); m_inv_K23 = -mParameters.cy() / mParameters.fy(); } Camera::ModelType PinholeCamera::modelType(void) const { return mParameters.modelType(); } const std::string& PinholeCamera::cameraName(void) const { return mParameters.cameraName(); } int PinholeCamera::imageWidth(void) const { return mParameters.imageWidth(); } int PinholeCamera::imageHeight(void) const { return mParameters.imageHeight(); } void PinholeCamera::estimateIntrinsics(const cv::Size& boardSize, const std::vector< std::vector >& objectPoints, const std::vector< std::vector >& imagePoints) { // Z. Zhang, A Flexible New Technique for Camera Calibration, PAMI 2000 Parameters params = getParameters(); params.k1() = 0.0; params.k2() = 0.0; params.p1() = 0.0; params.p2() = 0.0; double cx = params.imageWidth() / 2.0; double cy = params.imageHeight() / 2.0; params.cx() = cx; params.cy() = cy; size_t nImages = imagePoints.size(); cv::Mat A(nImages * 2, 2, CV_64F); cv::Mat b(nImages * 2, 1, CV_64F); for (size_t i = 0; i < nImages; ++i) { const std::vector& oPoints = objectPoints.at(i); std::vector M(oPoints.size()); for (size_t j = 0; j < M.size(); ++j) { M.at(j) = cv::Point2f(oPoints.at(j).x, oPoints.at(j).y); } cv::Mat H = cv::findHomography(M, imagePoints.at(i)); H.at(0,0) -= H.at(2,0) * cx; H.at(0,1) -= H.at(2,1) * cx; H.at(0,2) -= H.at(2,2) * cx; H.at(1,0) -= H.at(2,0) * cy; H.at(1,1) -= H.at(2,1) * cy; H.at(1,2) -= H.at(2,2) * cy; double h[3], v[3], d1[3], d2[3]; double n[4] = {0,0,0,0}; for (int j = 0; j < 3; ++j) { double t0 = H.at(j,0); double t1 = H.at(j,1); h[j] = t0; v[j] = t1; d1[j] = (t0 + t1) * 0.5; d2[j] = (t0 - t1) * 0.5; n[0] += t0 * t0; n[1] += t1 * t1; n[2] += d1[j] * d1[j]; n[3] += d2[j] * d2[j]; } for (int j = 0; j < 4; ++j) { n[j] = 1.0 / sqrt(n[j]); } for (int j = 0; j < 3; ++j) { h[j] *= n[0]; v[j] *= n[1]; d1[j] *= n[2]; d2[j] *= n[3]; } A.at(i * 2, 0) = h[0] * v[0]; A.at(i * 2, 1) = h[1] * v[1]; A.at(i * 2 + 1, 0) = d1[0] * d2[0]; A.at(i * 2 + 1, 1) = d1[1] * d2[1]; b.at(i * 2, 0) = -h[2] * v[2]; b.at(i * 2 + 1, 0) = -d1[2] * d2[2]; } cv::Mat f(2, 1, CV_64F); cv::solve(A, b, f, cv::DECOMP_NORMAL | cv::DECOMP_LU); params.fx() = sqrt(fabs(1.0 / f.at(0))); params.fy() = sqrt(fabs(1.0 / f.at(1))); setParameters(params); } /** * \brief Lifts a point from the image plane to the unit sphere * * \param p image coordinates * \param P coordinates of the point on the sphere */ void PinholeCamera::liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const { liftProjective(p, P); P.normalize(); } /** * \brief Lifts a point from the image plane to its projective ray * * \param p image coordinates * \param P coordinates of the projective ray */ void PinholeCamera::liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const { double mx_d, my_d,mx2_d, mxy_d, my2_d, mx_u, my_u; double rho2_d, rho4_d, radDist_d, Dx_d, Dy_d, inv_denom_d; //double lambda; // Lift points to normalised plane mx_d = m_inv_K11 * p(0) + m_inv_K13; my_d = m_inv_K22 * p(1) + m_inv_K23; if (m_noDistortion) { mx_u = mx_d; my_u = my_d; } else { if (0) { double k1 = mParameters.k1(); double k2 = mParameters.k2(); double p1 = mParameters.p1(); double p2 = mParameters.p2(); // Apply inverse distortion model // proposed by Heikkila mx2_d = mx_d*mx_d; my2_d = my_d*my_d; mxy_d = mx_d*my_d; rho2_d = mx2_d+my2_d; rho4_d = rho2_d*rho2_d; radDist_d = k1*rho2_d+k2*rho4_d; Dx_d = mx_d*radDist_d + p2*(rho2_d+2*mx2_d) + 2*p1*mxy_d; Dy_d = my_d*radDist_d + p1*(rho2_d+2*my2_d) + 2*p2*mxy_d; inv_denom_d = 1/(1+4*k1*rho2_d+6*k2*rho4_d+8*p1*my_d+8*p2*mx_d); mx_u = mx_d - inv_denom_d*Dx_d; my_u = my_d - inv_denom_d*Dy_d; } else { // Recursive distortion model int n = 8; Eigen::Vector2d d_u; distortion(Eigen::Vector2d(mx_d, my_d), d_u); // Approximate value mx_u = mx_d - d_u(0); my_u = my_d - d_u(1); for (int i = 1; i < n; ++i) { distortion(Eigen::Vector2d(mx_u, my_u), d_u); mx_u = mx_d - d_u(0); my_u = my_d - d_u(1); } } } // Obtain a projective ray P << mx_u, my_u, 1.0; } /** * \brief Project a 3D point (\a x,\a y,\a z) to the image plane in (\a u,\a v) * * \param P 3D point coordinates * \param p return value, contains the image point coordinates */ void PinholeCamera::spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const { Eigen::Vector2d p_u, p_d; // Project points to the normalised plane p_u << P(0) / P(2), P(1) / P(2); if (m_noDistortion) { p_d = p_u; } else { // Apply distortion Eigen::Vector2d d_u; distortion(p_u, d_u); p_d = p_u + d_u; } // Apply generalised projection matrix p << mParameters.fx() * p_d(0) + mParameters.cx(), mParameters.fy() * p_d(1) + mParameters.cy(); } #if 0 /** * \brief Project a 3D point to the image plane and calculate Jacobian * * \param P 3D point coordinates * \param p return value, contains the image point coordinates */ void PinholeCamera::spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p, Eigen::Matrix& J) const { Eigen::Vector2d p_u, p_d; double norm, inv_denom; double dxdmx, dydmx, dxdmy, dydmy; norm = P.norm(); // Project points to the normalised plane inv_denom = 1.0 / P(2); p_u << inv_denom * P(0), inv_denom * P(1); // Calculate jacobian double dudx = inv_denom; double dvdx = 0.0; double dudy = 0.0; double dvdy = inv_denom; inv_denom = - inv_denom * inv_denom; double dudz = P(0) * inv_denom; double dvdz = P(1) * inv_denom; if (m_noDistortion) { p_d = p_u; } else { // Apply distortion Eigen::Vector2d d_u; distortion(p_u, d_u); p_d = p_u + d_u; } double fx = mParameters.fx(); double fy = mParameters.fy(); // Make the product of the jacobians // and add projection matrix jacobian inv_denom = fx * (dudx * dxdmx + dvdx * dxdmy); // reuse dvdx = fy * (dudx * dydmx + dvdx * dydmy); dudx = inv_denom; inv_denom = fx * (dudy * dxdmx + dvdy * dxdmy); // reuse dvdy = fy * (dudy * dydmx + dvdy * dydmy); dudy = inv_denom; inv_denom = fx * (dudz * dxdmx + dvdz * dxdmy); // reuse dvdz = fy * (dudz * dydmx + dvdz * dydmy); dudz = inv_denom; // Apply generalised projection matrix p << fx * p_d(0) + mParameters.cx(), fy * p_d(1) + mParameters.cy(); J << dudx, dudy, dudz, dvdx, dvdy, dvdz; } #endif /** * \brief Projects an undistorted 2D point p_u to the image plane * * \param p_u 2D point coordinates * \return image point coordinates */ void PinholeCamera::undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const { Eigen::Vector2d p_d; if (m_noDistortion) { p_d = p_u; } else { // Apply distortion Eigen::Vector2d d_u; distortion(p_u, d_u); p_d = p_u + d_u; } // Apply generalised projection matrix p << mParameters.fx() * p_d(0) + mParameters.cx(), mParameters.fy() * p_d(1) + mParameters.cy(); } /** * \brief Apply distortion to input point (from the normalised plane) * * \param p_u undistorted coordinates of point on the normalised plane * \return to obtain the distorted point: p_d = p_u + d_u */ void PinholeCamera::distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u) const { double k1 = mParameters.k1(); double k2 = mParameters.k2(); double p1 = mParameters.p1(); double p2 = mParameters.p2(); double mx2_u, my2_u, mxy_u, rho2_u, rad_dist_u; mx2_u = p_u(0) * p_u(0); my2_u = p_u(1) * p_u(1); mxy_u = p_u(0) * p_u(1); rho2_u = mx2_u + my2_u; rad_dist_u = k1 * rho2_u + k2 * rho2_u * rho2_u; d_u << p_u(0) * rad_dist_u + 2.0 * p1 * mxy_u + p2 * (rho2_u + 2.0 * mx2_u), p_u(1) * rad_dist_u + 2.0 * p2 * mxy_u + p1 * (rho2_u + 2.0 * my2_u); } /** * \brief Apply distortion to input point (from the normalised plane) * and calculate Jacobian * * \param p_u undistorted coordinates of point on the normalised plane * \return to obtain the distorted point: p_d = p_u + d_u */ void PinholeCamera::distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u, Eigen::Matrix2d& J) const { double k1 = mParameters.k1(); double k2 = mParameters.k2(); double p1 = mParameters.p1(); double p2 = mParameters.p2(); double mx2_u, my2_u, mxy_u, rho2_u, rad_dist_u; mx2_u = p_u(0) * p_u(0); my2_u = p_u(1) * p_u(1); mxy_u = p_u(0) * p_u(1); rho2_u = mx2_u + my2_u; rad_dist_u = k1 * rho2_u + k2 * rho2_u * rho2_u; d_u << p_u(0) * rad_dist_u + 2.0 * p1 * mxy_u + p2 * (rho2_u + 2.0 * mx2_u), p_u(1) * rad_dist_u + 2.0 * p2 * mxy_u + p1 * (rho2_u + 2.0 * my2_u); double dxdmx = 1.0 + rad_dist_u + k1 * 2.0 * mx2_u + k2 * rho2_u * 4.0 * mx2_u + 2.0 * p1 * p_u(1) + 6.0 * p2 * p_u(0); double dydmx = k1 * 2.0 * p_u(0) * p_u(1) + k2 * 4.0 * rho2_u * p_u(0) * p_u(1) + p1 * 2.0 * p_u(0) + 2.0 * p2 * p_u(1); double dxdmy = dydmx; double dydmy = 1.0 + rad_dist_u + k1 * 2.0 * my2_u + k2 * rho2_u * 4.0 * my2_u + 6.0 * p1 * p_u(1) + 2.0 * p2 * p_u(0); J << dxdmx, dxdmy, dydmx, dydmy; } void PinholeCamera::initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale) const { cv::Size imageSize(mParameters.imageWidth(), mParameters.imageHeight()); cv::Mat mapX = cv::Mat::zeros(imageSize, CV_32F); cv::Mat mapY = cv::Mat::zeros(imageSize, CV_32F); for (int v = 0; v < imageSize.height; ++v) { for (int u = 0; u < imageSize.width; ++u) { double mx_u = m_inv_K11 / fScale * u + m_inv_K13 / fScale; double my_u = m_inv_K22 / fScale * v + m_inv_K23 / fScale; Eigen::Vector3d P; P << mx_u, my_u, 1.0; Eigen::Vector2d p; spaceToPlane(P, p); mapX.at(v,u) = p(0); mapY.at(v,u) = p(1); } } cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false); } cv::Mat PinholeCamera::initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2, float fx, float fy, cv::Size imageSize, float cx, float cy, cv::Mat rmat) const { if (imageSize == cv::Size(0, 0)) { imageSize = cv::Size(mParameters.imageWidth(), mParameters.imageHeight()); } cv::Mat mapX = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F); cv::Mat mapY = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F); Eigen::Matrix3f R, R_inv; cv::cv2eigen(rmat, R); R_inv = R.inverse(); // assume no skew Eigen::Matrix3f K_rect; if (cx == -1.0f || cy == -1.0f) { K_rect << fx, 0, imageSize.width / 2, 0, fy, imageSize.height / 2, 0, 0, 1; } else { K_rect << fx, 0, cx, 0, fy, cy, 0, 0, 1; } if (fx == -1.0f || fy == -1.0f) { K_rect(0,0) = mParameters.fx(); K_rect(1,1) = mParameters.fy(); } Eigen::Matrix3f K_rect_inv = K_rect.inverse(); for (int v = 0; v < imageSize.height; ++v) { for (int u = 0; u < imageSize.width; ++u) { Eigen::Vector3f xo; xo << u, v, 1; Eigen::Vector3f uo = R_inv * K_rect_inv * xo; Eigen::Vector2d p; spaceToPlane(uo.cast(), p); mapX.at(v,u) = p(0); mapY.at(v,u) = p(1); } } cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false); cv::Mat K_rect_cv; cv::eigen2cv(K_rect, K_rect_cv); return K_rect_cv; } int PinholeCamera::parameterCount(void) const { return 8; } const PinholeCamera::Parameters& PinholeCamera::getParameters(void) const { return mParameters; } void PinholeCamera::setParameters(const PinholeCamera::Parameters& parameters) { mParameters = parameters; if ((mParameters.k1() == 0.0) && (mParameters.k2() == 0.0) && (mParameters.p1() == 0.0) && (mParameters.p2() == 0.0)) { m_noDistortion = true; } else { m_noDistortion = false; } m_inv_K11 = 1.0 / mParameters.fx(); m_inv_K13 = -mParameters.cx() / mParameters.fx(); m_inv_K22 = 1.0 / mParameters.fy(); m_inv_K23 = -mParameters.cy() / mParameters.fy(); } void PinholeCamera::readParameters(const std::vector& parameterVec) { if ((int)parameterVec.size() != parameterCount()) { return; } Parameters params = getParameters(); params.k1() = parameterVec.at(0); params.k2() = parameterVec.at(1); params.p1() = parameterVec.at(2); params.p2() = parameterVec.at(3); params.fx() = parameterVec.at(4); params.fy() = parameterVec.at(5); params.cx() = parameterVec.at(6); params.cy() = parameterVec.at(7); setParameters(params); } void PinholeCamera::writeParameters(std::vector& parameterVec) const { parameterVec.resize(parameterCount()); parameterVec.at(0) = mParameters.k1(); parameterVec.at(1) = mParameters.k2(); parameterVec.at(2) = mParameters.p1(); parameterVec.at(3) = mParameters.p2(); parameterVec.at(4) = mParameters.fx(); parameterVec.at(5) = mParameters.fy(); parameterVec.at(6) = mParameters.cx(); parameterVec.at(7) = mParameters.cy(); } void PinholeCamera::writeParametersToYamlFile(const std::string& filename) const { mParameters.writeToYamlFile(filename); } std::string PinholeCamera::parametersToString(void) const { std::ostringstream oss; oss << mParameters; return oss.str(); } } ================================================ FILE: camera_model/src/camera_models/ScaramuzzaCamera.cc ================================================ #include "camodocal/camera_models/ScaramuzzaCamera.h" #include #include #include #include #include #include #include #include #include #include #include #include "camodocal/gpl/gpl.h" Eigen::VectorXd polyfit(Eigen::VectorXd& xVec, Eigen::VectorXd& yVec, int poly_order) { assert(poly_order > 0); assert(xVec.size() > poly_order); assert(xVec.size() == yVec.size()); Eigen::MatrixXd A(xVec.size(), poly_order+1); Eigen::VectorXd B(xVec.size()); for(int i = 0; i < xVec.size(); ++i) { const double x = xVec(i); const double y = yVec(i); double x_pow_k = 1.0; for(int k=0; k<=poly_order; ++k) { A(i,k) = x_pow_k; x_pow_k *= x; } B(i) = y; } Eigen::JacobiSVD svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV); Eigen::VectorXd x = svd.solve(B); return x; } namespace camodocal { OCAMCamera::Parameters::Parameters() : Camera::Parameters(SCARAMUZZA) , m_C(0.0) , m_D(0.0) , m_E(0.0) , m_center_x(0.0) , m_center_y(0.0) { memset(m_poly, 0, sizeof(double) * SCARAMUZZA_POLY_SIZE); memset(m_inv_poly, 0, sizeof(double) * SCARAMUZZA_INV_POLY_SIZE); } bool OCAMCamera::Parameters::readFromYamlFile(const std::string& filename) { cv::FileStorage fs(filename, cv::FileStorage::READ); if (!fs.isOpened()) { return false; } if (!fs["model_type"].isNone()) { std::string sModelType; fs["model_type"] >> sModelType; if (!boost::iequals(sModelType, "scaramuzza")) { return false; } } m_modelType = SCARAMUZZA; fs["camera_name"] >> m_cameraName; m_imageWidth = static_cast(fs["image_width"]); m_imageHeight = static_cast(fs["image_height"]); cv::FileNode n = fs["poly_parameters"]; for(int i=0; i < SCARAMUZZA_POLY_SIZE; i++) m_poly[i] = static_cast(n[std::string("p") + boost::lexical_cast(i)]); n = fs["inv_poly_parameters"]; for(int i=0; i < SCARAMUZZA_INV_POLY_SIZE; i++) m_inv_poly[i] = static_cast(n[std::string("p") + boost::lexical_cast(i)]); n = fs["affine_parameters"]; m_C = static_cast(n["ac"]); m_D = static_cast(n["ad"]); m_E = static_cast(n["ae"]); m_center_x = static_cast(n["cx"]); m_center_y = static_cast(n["cy"]); return true; } void OCAMCamera::Parameters::writeToYamlFile(const std::string& filename) const { cv::FileStorage fs(filename, cv::FileStorage::WRITE); fs << "model_type" << "scaramuzza"; fs << "camera_name" << m_cameraName; fs << "image_width" << m_imageWidth; fs << "image_height" << m_imageHeight; fs << "poly_parameters"; fs << "{"; for(int i=0; i < SCARAMUZZA_POLY_SIZE; i++) fs << std::string("p") + boost::lexical_cast(i) << m_poly[i]; fs << "}"; fs << "inv_poly_parameters"; fs << "{"; for(int i=0; i < SCARAMUZZA_INV_POLY_SIZE; i++) fs << std::string("p") + boost::lexical_cast(i) << m_inv_poly[i]; fs << "}"; fs << "affine_parameters"; fs << "{" << "ac" << m_C << "ad" << m_D << "ae" << m_E << "cx" << m_center_x << "cy" << m_center_y << "}"; fs.release(); } OCAMCamera::Parameters& OCAMCamera::Parameters::operator=(const OCAMCamera::Parameters& other) { if (this != &other) { m_modelType = other.m_modelType; m_cameraName = other.m_cameraName; m_imageWidth = other.m_imageWidth; m_imageHeight = other.m_imageHeight; m_C = other.m_C; m_D = other.m_D; m_E = other.m_E; m_center_x = other.m_center_x; m_center_y = other.m_center_y; memcpy(m_poly, other.m_poly, sizeof(double) * SCARAMUZZA_POLY_SIZE); memcpy(m_inv_poly, other.m_inv_poly, sizeof(double) * SCARAMUZZA_INV_POLY_SIZE); } return *this; } std::ostream& operator<< (std::ostream& out, const OCAMCamera::Parameters& params) { out << "Camera Parameters:" << std::endl; out << " model_type " << "scaramuzza" << std::endl; out << " camera_name " << params.m_cameraName << std::endl; out << " image_width " << params.m_imageWidth << std::endl; out << " image_height " << params.m_imageHeight << std::endl; out << std::fixed << std::setprecision(10); out << "Poly Parameters" << std::endl; for(int i=0; i < SCARAMUZZA_POLY_SIZE; i++) out << std::string("p") + boost::lexical_cast(i) << ": " << params.m_poly[i] << std::endl; out << "Inverse Poly Parameters" << std::endl; for(int i=0; i < SCARAMUZZA_INV_POLY_SIZE; i++) out << std::string("p") + boost::lexical_cast(i) << ": " << params.m_inv_poly[i] << std::endl; out << "Affine Parameters" << std::endl; out << " ac " << params.m_C << std::endl << " ad " << params.m_D << std::endl << " ae " << params.m_E << std::endl; out << " cx " << params.m_center_x << std::endl << " cy " << params.m_center_y << std::endl; return out; } OCAMCamera::OCAMCamera() : m_inv_scale(0.0) { } OCAMCamera::OCAMCamera(const OCAMCamera::Parameters& params) : mParameters(params) { m_inv_scale = 1.0 / (params.C() - params.D() * params.E()); } Camera::ModelType OCAMCamera::modelType(void) const { return mParameters.modelType(); } const std::string& OCAMCamera::cameraName(void) const { return mParameters.cameraName(); } int OCAMCamera::imageWidth(void) const { return mParameters.imageWidth(); } int OCAMCamera::imageHeight(void) const { return mParameters.imageHeight(); } void OCAMCamera::estimateIntrinsics(const cv::Size& boardSize, const std::vector< std::vector >& objectPoints, const std::vector< std::vector >& imagePoints) { // std::cout << "OCAMCamera::estimateIntrinsics - NOT IMPLEMENTED" << std::endl; // throw std::string("OCAMCamera::estimateIntrinsics - NOT IMPLEMENTED"); // Reference: Page 30 of // " Scaramuzza, D. Omnidirectional Vision: from Calibration to Robot Motion Estimation, ETH Zurich. Thesis no. 17635." // http://e-collection.library.ethz.ch/eserv/eth:30301/eth-30301-02.pdf // Matlab code: calibrate.m // First, estimate every image's extrinsics parameters std::vector< Eigen::Matrix3d > RList; std::vector< Eigen::Vector3d > TList; RList.reserve(imagePoints.size()); TList.reserve(imagePoints.size()); // i-th image for (size_t image_index = 0; image_index < imagePoints.size(); ++image_index) { const std::vector& objPts = objectPoints.at(image_index); const std::vector& imgPts = imagePoints.at(image_index); assert(objPts.size() == imgPts.size()); assert(objPts.size() == static_cast(boardSize.width * boardSize.height)); Eigen::MatrixXd M(objPts.size(), 6); for(size_t corner_index = 0; corner_index < objPts.size(); ++corner_index) { double X = objPts.at(corner_index).x; double Y = objPts.at(corner_index).y; assert(objPts.at(corner_index).z==0.0); double u = imgPts.at(corner_index).x; double v = imgPts.at(corner_index).y; M(corner_index, 0) = -v * X; M(corner_index, 1) = -v * Y; M(corner_index, 2) = u * X; M(corner_index, 3) = u * Y; M(corner_index, 4) = -v; M(corner_index, 5) = u; } Eigen::JacobiSVD svd(M, Eigen::ComputeFullU | Eigen::ComputeFullV); assert(svd.matrixV().cols() == 6); Eigen::VectorXd h = -svd.matrixV().col(5); // scaled version of R and T const double sr11 = h(0); const double sr12 = h(1); const double sr21 = h(2); const double sr22 = h(3); const double st1 = h(4); const double st2 = h(5); const double AA = square(sr11*sr12 + sr21*sr22); const double BB = square(sr11) + square(sr21); const double CC = square(sr12) + square(sr22); const double sr32_squared_1 = (- (CC-BB) + sqrt(square(CC-BB) + 4.0 * AA)) / 2.0; const double sr32_squared_2 = (- (CC-BB) - sqrt(square(CC-BB) + 4.0 * AA)) / 2.0; // printf("rst = %.12f\n", sr32_squared_1*sr32_squared_1 + (CC-BB)*sr32_squared_1 - AA); std::vector sr32_squared_values; if (sr32_squared_1 > 0) sr32_squared_values.push_back(sr32_squared_1); if (sr32_squared_2 > 0) sr32_squared_values.push_back(sr32_squared_2); assert(!sr32_squared_values.empty()); std::vector sr32_values; std::vector sr31_values; for (auto sr32_squared : sr32_squared_values) { for(int sign = -1; sign <= 1; sign += 2) { const double sr32 = static_cast(sign) * std::sqrt(sr32_squared); sr32_values.push_back( sr32 ); if (sr32_squared == 0.0) { // sr31 can be calculated through norm equality, // but it has positive and negative posibilities // positive one sr31_values.push_back(std::sqrt(CC-BB)); // negative one sr32_values.push_back( sr32 ); sr31_values.push_back(-std::sqrt(CC-BB)); break; // skip the same situation } else { // sr31 can be calculated throught dot product == 0 sr31_values.push_back(- (sr11*sr12 + sr21*sr22) / sr32); } } } // std::cout << "h= " << std::setprecision(12) << h.transpose() << std::endl; // std::cout << "length: " << sr32_values.size() << " & " << sr31_values.size() << std::endl; assert(!sr31_values.empty()); assert(sr31_values.size() == sr32_values.size()); std::vector H_values; for(size_t i=0;i(v,u) = p(0); mapY.at(v,u) = p(1); } } cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false); } #endif cv::Mat OCAMCamera::initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2, float fx, float fy, cv::Size imageSize, float cx, float cy, cv::Mat rmat) const { if (imageSize == cv::Size(0, 0)) { imageSize = cv::Size(mParameters.imageWidth(), mParameters.imageHeight()); } cv::Mat mapX = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F); cv::Mat mapY = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F); Eigen::Matrix3f K_rect; K_rect << fx, 0, cx < 0 ? imageSize.width / 2 : cx, 0, fy, cy < 0 ? imageSize.height / 2 : cy, 0, 0, 1; if (fx < 0 || fy < 0) { throw std::string(std::string(__FUNCTION__) + ": Focal length must be specified"); } Eigen::Matrix3f K_rect_inv = K_rect.inverse(); Eigen::Matrix3f R, R_inv; cv::cv2eigen(rmat, R); R_inv = R.inverse(); for (int v = 0; v < imageSize.height; ++v) { for (int u = 0; u < imageSize.width; ++u) { Eigen::Vector3f xo; xo << u, v, 1; Eigen::Vector3f uo = R_inv * K_rect_inv * xo; Eigen::Vector2d p; spaceToPlane(uo.cast(), p); mapX.at(v,u) = p(0); mapY.at(v,u) = p(1); } } cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false); cv::Mat K_rect_cv; cv::eigen2cv(K_rect, K_rect_cv); return K_rect_cv; } int OCAMCamera::parameterCount(void) const { return SCARAMUZZA_CAMERA_NUM_PARAMS; } const OCAMCamera::Parameters& OCAMCamera::getParameters(void) const { return mParameters; } void OCAMCamera::setParameters(const OCAMCamera::Parameters& parameters) { mParameters = parameters; m_inv_scale = 1.0 / (parameters.C() - parameters.D() * parameters.E()); } void OCAMCamera::readParameters(const std::vector& parameterVec) { if ((int)parameterVec.size() != parameterCount()) { return; } Parameters params = getParameters(); params.C() = parameterVec.at(0); params.D() = parameterVec.at(1); params.E() = parameterVec.at(2); params.center_x() = parameterVec.at(3); params.center_y() = parameterVec.at(4); for (int i=0; i < SCARAMUZZA_POLY_SIZE; i++) params.poly(i) = parameterVec.at(5+i); for (int i=0; i < SCARAMUZZA_INV_POLY_SIZE; i++) params.inv_poly(i) = parameterVec.at(5 + SCARAMUZZA_POLY_SIZE + i); setParameters(params); } void OCAMCamera::writeParameters(std::vector& parameterVec) const { parameterVec.resize(parameterCount()); parameterVec.at(0) = mParameters.C(); parameterVec.at(1) = mParameters.D(); parameterVec.at(2) = mParameters.E(); parameterVec.at(3) = mParameters.center_x(); parameterVec.at(4) = mParameters.center_y(); for (int i=0; i < SCARAMUZZA_POLY_SIZE; i++) parameterVec.at(5+i) = mParameters.poly(i); for (int i=0; i < SCARAMUZZA_INV_POLY_SIZE; i++) parameterVec.at(5 + SCARAMUZZA_POLY_SIZE + i) = mParameters.inv_poly(i); } void OCAMCamera::writeParametersToYamlFile(const std::string& filename) const { mParameters.writeToYamlFile(filename); } std::string OCAMCamera::parametersToString(void) const { std::ostringstream oss; oss << mParameters; return oss.str(); } } ================================================ FILE: camera_model/src/chessboard/Chessboard.cc ================================================ #include "camodocal/chessboard/Chessboard.h" #include #include #include "camodocal/chessboard/ChessboardQuad.h" #include "camodocal/chessboard/Spline.h" #define MAX_CONTOUR_APPROX 7 namespace camodocal { Chessboard::Chessboard(cv::Size boardSize, cv::Mat& image) : mBoardSize(boardSize) , mCornersFound(false) { if (image.channels() == 1) { cv::cvtColor(image, mSketch, cv::COLOR_GRAY2BGR); image.copyTo(mImage); } else { image.copyTo(mSketch); cv::cvtColor(image, mImage, cv::COLOR_BGR2GRAY); } } void Chessboard::findCorners(bool useOpenCV) { mCornersFound = findChessboardCorners(mImage, mBoardSize, mCorners, cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_NORMALIZE_IMAGE + cv::CALIB_CB_FILTER_QUADS + cv::CALIB_CB_FAST_CHECK, useOpenCV); if (mCornersFound) { // draw chessboard corners cv::drawChessboardCorners(mSketch, mBoardSize, mCorners, mCornersFound); } } const std::vector& Chessboard::getCorners(void) const { return mCorners; } bool Chessboard::cornersFound(void) const { return mCornersFound; } const cv::Mat& Chessboard::getImage(void) const { return mImage; } const cv::Mat& Chessboard::getSketch(void) const { return mSketch; } bool Chessboard::findChessboardCorners(const cv::Mat& image, const cv::Size& patternSize, std::vector& corners, int flags, bool useOpenCV) { if (useOpenCV) { return cv::findChessboardCorners(image, patternSize, corners, flags); } else { return findChessboardCornersImproved(image, patternSize, corners, flags); } } bool Chessboard::findChessboardCornersImproved(const cv::Mat& image, const cv::Size& patternSize, std::vector& corners, int flags) { /************************************************************************************\ This is improved variant of chessboard corner detection algorithm that uses a graph of connected quads. It is based on the code contributed by Vladimir Vezhnevets and Philip Gruebele. Here is the copyright notice from the original Vladimir's code: =============================================================== The algorithms developed and implemented by Vezhnevets Vldimir aka Dead Moroz (vvp@graphics.cs.msu.ru) See http://graphics.cs.msu.su/en/research/calibration/opencv.html for detailed information. Reliability additions and modifications made by Philip Gruebele. pgruebele@cox.net Some improvements were made by: 1) Martin Rufli - increased chance of correct corner matching Rufli, M., Scaramuzza, D., and Siegwart, R. (2008), Automatic Detection of Checkerboards on Blurred and Distorted Images, Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS 2008), Nice, France, September 2008. 2) Lionel Heng - post-detection checks and better thresholding \************************************************************************************/ //int bestDilation = -1; const int minDilations = 0; const int maxDilations = 7; std::vector outputQuadGroup; if (image.depth() != CV_8U || image.channels() == 2) { return false; } if (patternSize.width < 2 || patternSize.height < 2) { return false; } if (patternSize.width > 127 || patternSize.height > 127) { return false; } cv::Mat img = image; // Image histogram normalization and // BGR to Grayscale image conversion (if applicable) // MARTIN: Set to "false" if (image.channels() != 1 || (flags & cv::CALIB_CB_NORMALIZE_IMAGE)) { cv::Mat norm_img(image.rows, image.cols, CV_8UC1); if (image.channels() != 1) { cv::cvtColor(image, norm_img, cv::COLOR_BGR2GRAY); img = norm_img; } if (flags & cv::CALIB_CB_NORMALIZE_IMAGE) { cv::equalizeHist(image, norm_img); img = norm_img; } } if (flags & cv::CALIB_CB_FAST_CHECK) { if (!checkChessboard(img, patternSize)) { return false; } } // PART 1: FIND LARGEST PATTERN //----------------------------------------------------------------------- // Checker patterns are tried to be found by dilating the background and // then applying a canny edge finder on the closed contours (checkers). // Try one dilation run, but if the pattern is not found, repeat until // max_dilations is reached. int prevSqrSize = 0; bool found = false; std::vector outputCorners; for (int k = 0; k < 6; ++k) { for (int dilations = minDilations; dilations <= maxDilations; ++dilations) { if (found) { break; } cv::Mat thresh_img; // convert the input grayscale image to binary (black-n-white) if (flags & cv::CALIB_CB_ADAPTIVE_THRESH) { int blockSize = lround(prevSqrSize == 0 ? std::min(img.cols,img.rows)*(k%2 == 0 ? 0.2 : 0.1): prevSqrSize*2)|1; // convert to binary cv::adaptiveThreshold(img, thresh_img, 255, cv::ADAPTIVE_THRESH_MEAN_C, cv::THRESH_BINARY, blockSize, (k/2)*5); } else { // empiric threshold level double mean = (cv::mean(img))[0]; int thresh_level = lround(mean - 10); thresh_level = std::max(thresh_level, 10); cv::threshold(img, thresh_img, thresh_level, 255, cv::THRESH_BINARY); } // MARTIN's Code // Use both a rectangular and a cross kernel. In this way, a more // homogeneous dilation is performed, which is crucial for small, // distorted checkers. Use the CROSS kernel first, since its action // on the image is more subtle cv::Mat kernel1 = cv::getStructuringElement(cv::MORPH_CROSS, cv::Size(3,3), cv::Point(1,1)); cv::Mat kernel2 = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3,3), cv::Point(1,1)); if (dilations >= 1) cv::dilate(thresh_img, thresh_img, kernel1); if (dilations >= 2) cv::dilate(thresh_img, thresh_img, kernel2); if (dilations >= 3) cv::dilate(thresh_img, thresh_img, kernel1); if (dilations >= 4) cv::dilate(thresh_img, thresh_img, kernel2); if (dilations >= 5) cv::dilate(thresh_img, thresh_img, kernel1); if (dilations >= 6) cv::dilate(thresh_img, thresh_img, kernel2); // In order to find rectangles that go to the edge, we draw a white // line around the image edge. Otherwise FindContours will miss those // clipped rectangle contours. The border color will be the image mean, // because otherwise we risk screwing up filters like cvSmooth() cv::rectangle(thresh_img, cv::Point(0,0), cv::Point(thresh_img.cols - 1, thresh_img.rows - 1), CV_RGB(255,255,255), 3, 8); // Generate quadrangles in the following function std::vector quads; generateQuads(quads, thresh_img, flags, dilations, true); if (quads.empty()) { continue; } // The following function finds and assigns neighbor quads to every // quadrangle in the immediate vicinity fulfilling certain // prerequisites findQuadNeighbors(quads, dilations); // The connected quads will be organized in groups. The following loop // increases a "group_idx" identifier. // The function "findConnectedQuads assigns all connected quads // a unique group ID. // If more quadrangles were assigned to a given group (i.e. connected) // than are expected by the input variable "patternSize", the // function "cleanFoundConnectedQuads" erases the surplus // quadrangles by minimizing the convex hull of the remaining pattern. for (int group_idx = 0; ; ++group_idx) { std::vector quadGroup; findConnectedQuads(quads, quadGroup, group_idx, dilations); if (quadGroup.empty()) { break; } cleanFoundConnectedQuads(quadGroup, patternSize); // The following function labels all corners of every quad // with a row and column entry. // "count" specifies the number of found quads in "quad_group" // with group identifier "group_idx" // The last parameter is set to "true", because this is the // first function call and some initializations need to be // made. labelQuadGroup(quadGroup, patternSize, true); found = checkQuadGroup(quadGroup, outputCorners, patternSize); float sumDist = 0; int total = 0; for (int i = 0; i < (int)outputCorners.size(); ++i) { int ni = 0; float avgi = outputCorners.at(i)->meanDist(ni); sumDist += avgi * ni; total += ni; } prevSqrSize = lround(sumDist / std::max(total, 1)); if (found && !checkBoardMonotony(outputCorners, patternSize)) { found = false; } } } } if (!found) { return false; } else { corners.clear(); corners.reserve(outputCorners.size()); for (size_t i = 0; i < outputCorners.size(); ++i) { corners.push_back(outputCorners.at(i)->pt); } cv::cornerSubPix(image, corners, cv::Size(11, 11), cv::Size(-1,-1), cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::MAX_ITER, 30, 0.1)); return true; } } //=========================================================================== // ERASE OVERHEAD //=========================================================================== // If we found too many connected quads, remove those which probably do not // belong. void Chessboard::cleanFoundConnectedQuads(std::vector& quadGroup, cv::Size patternSize) { cv::Point2f center(0.0f, 0.0f); // Number of quads this pattern should contain int count = ((patternSize.width + 1)*(patternSize.height + 1) + 1)/2; // If we have more quadrangles than we should, try to eliminate duplicates // or ones which don't belong to the pattern rectangle. Else go to the end // of the function if ((int)quadGroup.size() <= count) { return; } // Create an array of quadrangle centers std::vector centers; centers.resize(quadGroup.size()); for (size_t i = 0; i < quadGroup.size(); ++i) { cv::Point2f ci(0.0f, 0.0f); ChessboardQuadPtr& q = quadGroup[i]; for (int j = 0; j < 4; ++j) { ci += q->corners[j]->pt; } ci *= 0.25f; // Centers(i), is the geometric center of quad(i) // Center, is the center of all found quads centers[i] = ci; center += ci; } center *= 1.0f / quadGroup.size(); // If we have more quadrangles than we should, we try to eliminate bad // ones based on minimizing the bounding box. We iteratively remove the // point which reduces the size of the bounding box of the blobs the most // (since we want the rectangle to be as small as possible) remove the // quadrange that causes the biggest reduction in pattern size until we // have the correct number while ((int)quadGroup.size() > count) { double minBoxArea = DBL_MAX; int minBoxAreaIndex = -1; // For each point, calculate box area without that point for (size_t skip = 0; skip < quadGroup.size(); ++skip) { // get bounding rectangle cv::Point2f temp = centers[skip]; centers[skip] = center; std::vector hull; cv::convexHull(centers, hull, true, true); centers[skip] = temp; double hull_area = fabs(cv::contourArea(hull)); // remember smallest box area if (hull_area < minBoxArea) { minBoxArea = hull_area; minBoxAreaIndex = skip; } } ChessboardQuadPtr& q0 = quadGroup[minBoxAreaIndex]; // remove any references to this quad as a neighbor for (size_t i = 0; i < quadGroup.size(); ++i) { ChessboardQuadPtr& q = quadGroup.at(i); for (int j = 0; j < 4; ++j) { if (q->neighbors[j] == q0) { q->neighbors[j].reset(); q->count--; for (int k = 0; k < 4; ++k) { if (q0->neighbors[k] == q) { q0->neighbors[k].reset(); q0->count--; break; } } break; } } } // remove the quad quadGroup.at(minBoxAreaIndex) = quadGroup.back(); centers.at(minBoxAreaIndex) = centers.back(); quadGroup.pop_back(); centers.pop_back(); } } //=========================================================================== // FIND COONECTED QUADS //=========================================================================== void Chessboard::findConnectedQuads(std::vector& quads, std::vector& group, int group_idx, int dilation) { ChessboardQuadPtr q; // Scan the array for a first unlabeled quad for (size_t i = 0; i < quads.size(); ++i) { ChessboardQuadPtr& quad = quads.at(i); if (quad->count > 0 && quad->group_idx < 0) { q = quad; break; } } if (q.get() == 0) { return; } // Recursively find a group of connected quads starting from the seed quad std::vector stack; stack.push_back(q); group.push_back(q); q->group_idx = group_idx; while (!stack.empty()) { q = stack.back(); stack.pop_back(); for (int i = 0; i < 4; ++i) { ChessboardQuadPtr& neighbor = q->neighbors[i]; // If he neighbor exists and the neighbor has more than 0 // neighbors and the neighbor has not been classified yet. if (neighbor.get() && neighbor->count > 0 && neighbor->group_idx < 0) { stack.push_back(neighbor); group.push_back(neighbor); neighbor->group_idx = group_idx; } } } } void Chessboard::labelQuadGroup(std::vector& quadGroup, cv::Size patternSize, bool firstRun) { // If this is the first function call, a seed quad needs to be selected if (firstRun) { // Search for the (first) quad with the maximum number of neighbors // (usually 4). This will be our starting point. int mark = -1; int maxNeighborCount = 0; for (size_t i = 0; i < quadGroup.size(); ++i) { ChessboardQuadPtr& q = quadGroup.at(i); if (q->count > maxNeighborCount) { mark = i; maxNeighborCount = q->count; if (maxNeighborCount == 4) { break; } } } // Mark the starting quad's (per definition) upper left corner with //(0,0) and then proceed clockwise // The following labeling sequence enshures a "right coordinate system" ChessboardQuadPtr& q = quadGroup.at(mark); q->labeled = true; q->corners[0]->row = 0; q->corners[0]->column = 0; q->corners[1]->row = 0; q->corners[1]->column = 1; q->corners[2]->row = 1; q->corners[2]->column = 1; q->corners[3]->row = 1; q->corners[3]->column = 0; } // Mark all other corners with their respective row and column bool flagChanged = true; while (flagChanged) { // First reset the flag to "false" flagChanged = false; // Go through all quads top down is faster, since unlabeled quads will // be inserted at the end of the list for (int i = quadGroup.size() - 1; i >= 0; --i) { ChessboardQuadPtr& quad = quadGroup.at(i); // Check whether quad "i" has been labeled already if (!quad->labeled) { // Check its neighbors, whether some of them have been labeled // already for (int j = 0; j < 4; j++) { // Check whether the neighbor exists (i.e. is not the NULL // pointer) if (quad->neighbors[j]) { ChessboardQuadPtr& quadNeighbor = quad->neighbors[j]; // Only proceed, if neighbor "j" was labeled if (quadNeighbor->labeled) { // For every quad it could happen to pass here // multiple times. We therefore "break" later. // Check whitch of the neighbors corners is // connected to the current quad int connectedNeighborCornerId = -1; for (int k = 0; k < 4; ++k) { if (quadNeighbor->neighbors[k] == quad) { connectedNeighborCornerId = k; // there is only one, therefore break; } } // For the following calculations we need the row // and column of the connected neighbor corner and // all other corners of the connected quad "j", // clockwise (CW) ChessboardCornerPtr& conCorner = quadNeighbor->corners[connectedNeighborCornerId]; ChessboardCornerPtr& conCornerCW1 = quadNeighbor->corners[(connectedNeighborCornerId+1)%4]; ChessboardCornerPtr& conCornerCW2 = quadNeighbor->corners[(connectedNeighborCornerId+2)%4]; ChessboardCornerPtr& conCornerCW3 = quadNeighbor->corners[(connectedNeighborCornerId+3)%4]; quad->corners[j]->row = conCorner->row; quad->corners[j]->column = conCorner->column; quad->corners[(j+1)%4]->row = conCorner->row - conCornerCW2->row + conCornerCW3->row; quad->corners[(j+1)%4]->column = conCorner->column - conCornerCW2->column + conCornerCW3->column; quad->corners[(j+2)%4]->row = conCorner->row + conCorner->row - conCornerCW2->row; quad->corners[(j+2)%4]->column = conCorner->column + conCorner->column - conCornerCW2->column; quad->corners[(j+3)%4]->row = conCorner->row - conCornerCW2->row + conCornerCW1->row; quad->corners[(j+3)%4]->column = conCorner->column - conCornerCW2->column + conCornerCW1->column; // Mark this quad as labeled quad->labeled = true; // Changes have taken place, set the flag flagChanged = true; // once is enough! break; } } } } } } // All corners are marked with row and column // Record the minimal and maximal row and column indices // It is unlikely that more than 8bit checkers are used per dimension, if there are // an error would have been thrown at the beginning of "cvFindChessboardCorners2" int min_row = 127; int max_row = -127; int min_column = 127; int max_column = -127; for (int i = 0; i < (int)quadGroup.size(); ++i) { ChessboardQuadPtr& q = quadGroup.at(i); for (int j = 0; j < 4; ++j) { ChessboardCornerPtr& c = q->corners[j]; if (c->row > max_row) { max_row = c->row; } if (c->row < min_row) { min_row = c->row; } if (c->column > max_column) { max_column = c->column; } if (c->column < min_column) { min_column = c->column; } } } // Label all internal corners with "needsNeighbor" = false // Label all external corners with "needsNeighbor" = true, // except if in a given dimension the pattern size is reached for (int i = min_row; i <= max_row; ++i) { for (int j = min_column; j <= max_column; ++j) { // A flag that indicates, whether a row/column combination is // executed multiple times bool flag = false; // Remember corner and quad int cornerID; int quadID; for (int k = 0; k < (int)quadGroup.size(); ++k) { ChessboardQuadPtr& q = quadGroup.at(k); for (int l = 0; l < 4; ++l) { if ((q->corners[l]->row == i) && (q->corners[l]->column == j)) { if (flag) { // Passed at least twice through here q->corners[l]->needsNeighbor = false; quadGroup[quadID]->corners[cornerID]->needsNeighbor = false; } else { // Mark with needs a neighbor, but note the // address q->corners[l]->needsNeighbor = true; cornerID = l; quadID = k; } // set the flag to true flag = true; } } } } } // Complete Linking: // sometimes not all corners were properly linked in "findQuadNeighbors", // but after labeling each corner with its respective row and column, it is // possible to match them anyway. for (int i = min_row; i <= max_row; ++i) { for (int j = min_column; j <= max_column; ++j) { // the following "number" indicates the number of corners which // correspond to the given (i,j) value // 1 is a border corner or a conrer which still needs a neighbor // 2 is a fully connected internal corner // >2 something went wrong during labeling, report a warning int number = 1; // remember corner and quad int cornerID; int quadID; for (int k = 0; k < (int)quadGroup.size(); ++k) { ChessboardQuadPtr& q = quadGroup.at(k); for (int l = 0; l < 4; ++l) { if ((q->corners[l]->row == i) && (q->corners[l]->column == j)) { if (number == 1) { // First corner, note its ID cornerID = l; quadID = k; } else if (number == 2) { // Second corner, check wheter this and the // first one have equal coordinates, else // interpolate cv::Point2f delta = q->corners[l]->pt - quadGroup[quadID]->corners[cornerID]->pt; if (delta.x != 0.0f || delta.y != 0.0f) { // Interpolate q->corners[l]->pt -= delta * 0.5f; quadGroup[quadID]->corners[cornerID]->pt += delta * 0.5f; } } else if (number > 2) { // Something went wrong during row/column labeling // Report a Warning // ->Implemented in the function "mrWriteCorners" } // increase the number by one ++number; } } } } } // Bordercorners don't need any neighbors, if the pattern size in the // respective direction is reached // The only time we can make shure that the target pattern size is reached in a given // dimension, is when the larger side has reached the target size in the maximal // direction, or if the larger side is larger than the smaller target size and the // smaller side equals the smaller target size int largerDimPattern = std::max(patternSize.height,patternSize.width); int smallerDimPattern = std::min(patternSize.height,patternSize.width); bool flagSmallerDim1 = false; bool flagSmallerDim2 = false; if ((largerDimPattern + 1) == max_column - min_column) { flagSmallerDim1 = true; // We found out that in the column direction the target pattern size is reached // Therefore border column corners do not need a neighbor anymore // Go through all corners for (int k = 0; k < (int)quadGroup.size(); ++k) { ChessboardQuadPtr& q = quadGroup.at(k); for (int l = 0; l < 4; ++l) { ChessboardCornerPtr& c = q->corners[l]; if (c->column == min_column || c->column == max_column) { // Needs no neighbor anymore c->needsNeighbor = false; } } } } if ((largerDimPattern + 1) == max_row - min_row) { flagSmallerDim2 = true; // We found out that in the column direction the target pattern size is reached // Therefore border column corners do not need a neighbor anymore // Go through all corners for (int k = 0; k < (int)quadGroup.size(); ++k) { ChessboardQuadPtr& q = quadGroup.at(k); for (int l = 0; l < 4; ++l) { ChessboardCornerPtr& c = q->corners[l]; if (c->row == min_row || c->row == max_row) { // Needs no neighbor anymore c->needsNeighbor = false; } } } } // Check the two flags: // - If one is true and the other false, then the pattern target // size was reached in in one direction -> We can check, whether the target // pattern size is also reached in the other direction // - If both are set to true, then we deal with a square board -> do nothing // - If both are set to false -> There is a possibility that the larger side is // larger than the smaller target size -> Check and if true, then check whether // the other side has the same size as the smaller target size if ((flagSmallerDim1 == false && flagSmallerDim2 == true)) { // Larger target pattern size is in row direction, check wheter smaller target // pattern size is reached in column direction if ((smallerDimPattern + 1) == max_column - min_column) { for (int k = 0; k < (int)quadGroup.size(); ++k) { ChessboardQuadPtr& q = quadGroup.at(k); for (int l = 0; l < 4; ++l) { ChessboardCornerPtr& c = q->corners[l]; if (c->column == min_column || c->column == max_column) { // Needs no neighbor anymore c->needsNeighbor = false; } } } } } if ((flagSmallerDim1 == true && flagSmallerDim2 == false)) { // Larger target pattern size is in column direction, check wheter smaller target // pattern size is reached in row direction if ((smallerDimPattern + 1) == max_row - min_row) { for (int k = 0; k < (int)quadGroup.size(); ++k) { ChessboardQuadPtr& q = quadGroup.at(k); for (int l = 0; l < 4; ++l) { ChessboardCornerPtr& c = q->corners[l]; if (c->row == min_row || c->row == max_row) { // Needs no neighbor anymore c->needsNeighbor = false; } } } } } if ((flagSmallerDim1 == false && flagSmallerDim2 == false) && smallerDimPattern + 1 < max_column - min_column) { // Larger target pattern size is in column direction, check wheter smaller target // pattern size is reached in row direction if ((smallerDimPattern + 1) == max_row - min_row) { for (int k = 0; k < (int)quadGroup.size(); ++k) { ChessboardQuadPtr& q = quadGroup.at(k); for (int l = 0; l < 4; ++l) { ChessboardCornerPtr& c = q->corners[l]; if (c->row == min_row || c->row == max_row) { // Needs no neighbor anymore c->needsNeighbor = false; } } } } } if ((flagSmallerDim1 == false && flagSmallerDim2 == false) && smallerDimPattern + 1 < max_row - min_row) { // Larger target pattern size is in row direction, check wheter smaller target // pattern size is reached in column direction if ((smallerDimPattern + 1) == max_column - min_column) { for (int k = 0; k < (int)quadGroup.size(); ++k) { ChessboardQuadPtr& q = quadGroup.at(k); for (int l = 0; l < 4; ++l) { ChessboardCornerPtr& c = q->corners[l]; if (c->column == min_column || c->column == max_column) { // Needs no neighbor anymore c->needsNeighbor = false; } } } } } } //=========================================================================== // GIVE A GROUP IDX //=========================================================================== void Chessboard::findQuadNeighbors(std::vector& quads, int dilation) { // Thresh dilation is used to counter the effect of dilation on the // distance between 2 neighboring corners. Since the distance below is // computed as its square, we do here the same. Additionally, we take the // conservative assumption that dilation was performed using the 3x3 CROSS // kernel, which coresponds to the 4-neighborhood. const float thresh_dilation = (float)(2*dilation+3)*(2*dilation+3)*2; // the "*2" is for the x and y component // the "3" is for initial corner mismatch // Find quad neighbors for (size_t idx = 0; idx < quads.size(); ++idx) { ChessboardQuadPtr& curQuad = quads.at(idx); // Go through all quadrangles and label them in groups // For each corner of this quadrangle for (int i = 0; i < 4; ++i) { float minDist = FLT_MAX; int closestCornerIdx = -1; ChessboardQuadPtr closestQuad; if (curQuad->neighbors[i]) { continue; } cv::Point2f pt = curQuad->corners[i]->pt; // Find the closest corner in all other quadrangles for (size_t k = 0; k < quads.size(); ++k) { if (k == idx) { continue; } ChessboardQuadPtr& quad = quads.at(k); for (int j = 0; j < 4; ++j) { // If it already has a neighbor if (quad->neighbors[j]) { continue; } cv::Point2f dp = pt - quad->corners[j]->pt; float dist = dp.dot(dp); // The following "if" checks, whether "dist" is the // shortest so far and smaller than the smallest // edge length of the current and target quads if (dist < minDist && dist <= (curQuad->edge_len + thresh_dilation) && dist <= (quad->edge_len + thresh_dilation) ) { // Check whether conditions are fulfilled if (matchCorners(curQuad, i, quad, j)) { closestCornerIdx = j; closestQuad = quad; minDist = dist; } } } } // Have we found a matching corner point? if (closestCornerIdx >= 0 && minDist < FLT_MAX) { ChessboardCornerPtr closestCorner = closestQuad->corners[closestCornerIdx]; // Make sure that the closest quad does not have the current // quad as neighbor already bool valid = true; for (int j = 0; j < 4; ++j) { if (closestQuad->neighbors[j] == curQuad) { valid = false; break; } } if (!valid) { continue; } // We've found one more corner - remember it closestCorner->pt = (pt + closestCorner->pt) * 0.5f; curQuad->count++; curQuad->neighbors[i] = closestQuad; curQuad->corners[i] = closestCorner; closestQuad->count++; closestQuad->neighbors[closestCornerIdx] = curQuad; closestQuad->corners[closestCornerIdx] = closestCorner; } } } } //=========================================================================== // AUGMENT PATTERN WITH ADDITIONAL QUADS //=========================================================================== // The first part of the function is basically a copy of // "findQuadNeighbors" // The comparisons between two points and two lines could be computed in their // own function int Chessboard::augmentBestRun(std::vector& candidateQuads, int candidateDilation, std::vector& existingQuads, int existingDilation) { // thresh dilation is used to counter the effect of dilation on the // distance between 2 neighboring corners. Since the distance below is // computed as its square, we do here the same. Additionally, we take the // conservative assumption that dilation was performed using the 3x3 CROSS // kernel, which coresponds to the 4-neighborhood. const float thresh_dilation = (2*candidateDilation+3)*(2*existingDilation+3)*2; // the "*2" is for the x and y component // Search all old quads which have a neighbor that needs to be linked for (size_t idx = 0; idx < existingQuads.size(); ++idx) { ChessboardQuadPtr& curQuad = existingQuads.at(idx); // For each corner of this quadrangle for (int i = 0; i < 4; ++i) { float minDist = FLT_MAX; int closestCornerIdx = -1; ChessboardQuadPtr closestQuad; // If curQuad corner[i] is already linked, continue if (!curQuad->corners[i]->needsNeighbor) { continue; } cv::Point2f pt = curQuad->corners[i]->pt; // Look for a match in all candidateQuads' corners for (size_t k = 0; k < candidateQuads.size(); ++k) { ChessboardQuadPtr& candidateQuad = candidateQuads.at(k); // Only look at unlabeled new quads if (candidateQuad->labeled) { continue; } for (int j = 0; j < 4; ++j) { // Only proceed if they are less than dist away from each // other cv::Point2f dp = pt - candidateQuad->corners[j]->pt; float dist = dp.dot(dp); if ((dist < minDist) && dist <= (curQuad->edge_len + thresh_dilation) && dist <= (candidateQuad->edge_len + thresh_dilation)) { if (matchCorners(curQuad, i, candidateQuad, j)) { closestCornerIdx = j; closestQuad = candidateQuad; minDist = dist; } } } } // Have we found a matching corner point? if (closestCornerIdx >= 0 && minDist < FLT_MAX) { ChessboardCornerPtr closestCorner = closestQuad->corners[closestCornerIdx]; closestCorner->pt = (pt + closestCorner->pt) * 0.5f; // We've found one more corner - remember it // ATTENTION: write the corner x and y coordinates separately, // else the crucial row/column entries will be overwritten !!! curQuad->corners[i]->pt = closestCorner->pt; curQuad->neighbors[i] = closestQuad; closestQuad->corners[closestCornerIdx]->pt = closestCorner->pt; // Label closest quad as labeled. In this way we exclude it // being considered again during the next loop iteration closestQuad->labeled = true; // We have a new member of the final pattern, copy it over ChessboardQuadPtr newQuad(new ChessboardQuad); newQuad->count = 1; newQuad->edge_len = closestQuad->edge_len; newQuad->group_idx = curQuad->group_idx; //the same as the current quad newQuad->labeled = false; //do it right afterwards curQuad->neighbors[i] = newQuad; // We only know one neighbor for sure newQuad->neighbors[closestCornerIdx] = curQuad; for (int j = 0; j < 4; j++) { newQuad->corners[j].reset(new ChessboardCorner); newQuad->corners[j]->pt = closestQuad->corners[j]->pt; } existingQuads.push_back(newQuad); // Start the function again return -1; } } } // Finished, don't start the function again return 1; } //=========================================================================== // GENERATE QUADRANGLES //=========================================================================== void Chessboard::generateQuads(std::vector& quads, cv::Mat& image, int flags, int dilation, bool firstRun) { // Empirical lower bound for the size of allowable quadrangles. // MARTIN, modified: Added "*0.1" in order to find smaller quads. int minSize = lround(image.cols * image.rows * .03 * 0.01 * 0.92 * 0.1); std::vector< std::vector > contours; std::vector hierarchy; // Initialize contour retrieving routine cv::findContours(image, contours, hierarchy, cv::RETR_CCOMP, cv::CHAIN_APPROX_SIMPLE); std::vector< std::vector > quadContours; for (size_t i = 0; i < contours.size(); ++i) { // Reject contours with a too small perimeter and contours which are // completely surrounded by another contour // MARTIN: If this function is called during PART 1, then the parameter "first run" // is set to "true". This guarantees, that only "nice" squares are detected. // During PART 2, we allow the polygonal approximation function below to // approximate more freely, which can result in recognized "squares" that are in // reality multiple blurred and sticked together squares. std::vector& contour = contours.at(i); if (hierarchy[i][3] == -1 || cv::contourArea(contour) < minSize) { continue; } int min_approx_level = 1, max_approx_level; if (firstRun) { max_approx_level = 3; } else { max_approx_level = MAX_CONTOUR_APPROX; } std::vector approxContour; for (int approx_level = min_approx_level; approx_level <= max_approx_level; approx_level++) { cv::approxPolyDP(contour, approxContour, approx_level, true); if (approxContour.size() == 4) { break; } } // Reject non-quadrangles if (approxContour.size() == 4 && cv::isContourConvex(approxContour)) { double p = cv::arcLength(approxContour, true); double area = cv::contourArea(approxContour); cv::Point pt[4]; for (int i = 0; i < 4; i++) { pt[i] = approxContour[i]; } cv::Point dp = pt[0] - pt[2]; double d1 = sqrt(dp.dot(dp)); dp = pt[1] - pt[3]; double d2 = sqrt(dp.dot(dp)); // PHILIPG: Only accept those quadrangles which are more // square than rectangular and which are big enough dp = pt[0] - pt[1]; double d3 = sqrt(dp.dot(dp)); dp = pt[1] - pt[2]; double d4 = sqrt(dp.dot(dp)); if (!(flags & cv::CALIB_CB_FILTER_QUADS) || (d3*4 > d4 && d4*4 > d3 && d3*d4 < area*1.5 && area > minSize && d1 >= 0.15 * p && d2 >= 0.15 * p)) { quadContours.push_back(approxContour); } } } // Allocate quad & corner buffers quads.resize(quadContours.size()); // Create array of quads structures for (size_t idx = 0; idx < quadContours.size(); ++idx) { ChessboardQuadPtr& q = quads.at(idx); std::vector& contour = quadContours.at(idx); // Reset group ID q.reset(new ChessboardQuad); assert(contour.size() == 4); for (int i = 0; i < 4; ++i) { cv::Point2f pt = contour.at(i); ChessboardCornerPtr corner(new ChessboardCorner); corner->pt = pt; q->corners[i] = corner; } for (int i = 0; i < 4; ++i) { cv::Point2f dp = q->corners[i]->pt - q->corners[(i+1)&3]->pt; float d = dp.dot(dp); if (q->edge_len > d) { q->edge_len = d; } } } } bool Chessboard::checkQuadGroup(std::vector& quads, std::vector& corners, cv::Size patternSize) { // Initialize bool flagRow = false; bool flagColumn = false; int height = -1; int width = -1; // Compute the minimum and maximum row / column ID // (it is unlikely that more than 8bit checkers are used per dimension) int min_row = 127; int max_row = -127; int min_col = 127; int max_col = -127; for (size_t i = 0; i < quads.size(); ++i) { ChessboardQuadPtr& q = quads.at(i); for (int j = 0; j < 4; ++j) { ChessboardCornerPtr& c = q->corners[j]; if (c->row > max_row) { max_row = c->row; } if (c->row < min_row) { min_row = c->row; } if (c->column > max_col) { max_col = c->column; } if (c->column < min_col) { min_col = c->column; } } } // If in a given direction the target pattern size is reached, we know exactly how // the checkerboard is oriented. // Else we need to prepare enough "dummy" corners for the worst case. for (size_t i = 0; i < quads.size(); ++i) { ChessboardQuadPtr& q = quads.at(i); for (int j = 0; j < 4; ++j) { ChessboardCornerPtr& c = q->corners[j]; if (c->column == max_col && c->row != min_row && c->row != max_row && !c->needsNeighbor) { flagColumn = true; } if (c->row == max_row && c->column != min_col && c->column != max_col && !c->needsNeighbor) { flagRow = true; } } } if (flagColumn) { if (max_col - min_col == patternSize.width + 1) { width = patternSize.width; height = patternSize.height; } else { width = patternSize.height; height = patternSize.width; } } else if (flagRow) { if (max_row - min_row == patternSize.width + 1) { height = patternSize.width; width = patternSize.height; } else { height = patternSize.height; width = patternSize.width; } } else { // If target pattern size is not reached in at least one of the two // directions, then we do not know where the remaining corners are // located. Account for this. width = std::max(patternSize.width, patternSize.height); height = std::max(patternSize.width, patternSize.height); } ++min_row; ++min_col; max_row = min_row + height - 1; max_col = min_col + width - 1; corners.clear(); int linkedBorderCorners = 0; // Write the corners in increasing order to the output file for (int i = min_row; i <= max_row; ++i) { for (int j = min_col; j <= max_col; ++j) { // Reset the iterator int iter = 1; for (int k = 0; k < (int)quads.size(); ++k) { ChessboardQuadPtr& quad = quads.at(k); for (int l = 0; l < 4; ++l) { ChessboardCornerPtr& c = quad->corners[l]; if (c->row == i && c->column == j) { bool boardEdge = false; if (i == min_row || i == max_row || j == min_col || j == max_col) { boardEdge = true; } if ((iter == 1 && boardEdge) || (iter == 2 && !boardEdge)) { // The respective row and column have been found corners.push_back(quad->corners[l]); } if (iter == 2 && boardEdge) { ++linkedBorderCorners; } // If the iterator is larger than two, this means that more than // two corners have the same row / column entries. Then some // linking errors must have occured and we should not use the found // pattern if (iter > 2) { return false; } iter++; } } } } } if ((int)corners.size() != patternSize.width * patternSize.height || linkedBorderCorners < (patternSize.width * 2 + patternSize.height * 2 - 2) * 0.75f) { return false; } // check that no corners lie at image boundary float border = 5.0f; for (int i = 0; i < (int)corners.size(); ++i) { ChessboardCornerPtr& c = corners.at(i); if (c->pt.x < border || c->pt.x > mImage.cols - border || c->pt.y < border || c->pt.y > mImage.rows - border) { return false; } } // check if we need to transpose the board if (width != patternSize.width) { std::swap(width, height); std::vector outputCorners; outputCorners.resize(corners.size()); for (int i = 0; i < height; ++i) { for (int j = 0; j < width; ++j) { outputCorners.at(i * width + j) = corners.at(j * height + i); } } corners = outputCorners; } // check if we need to revert the order in each row cv::Point2f p0 = corners.at(0)->pt; cv::Point2f p1 = corners.at(width-1)->pt; cv::Point2f p2 = corners.at(width)->pt; if ((p1 - p0).cross(p2 - p0) < 0.0f) { for (int i = 0; i < height; ++i) { for (int j = 0; j < width / 2; ++j) { std::swap(corners.at(i * width + j), corners.at(i * width + width - j - 1)); } } } p0 = corners.at(0)->pt; p2 = corners.at(width)->pt; // check if we need to rotate the board if (p2.y < p0.y) { std::vector outputCorners; outputCorners.resize(corners.size()); for (int i = 0; i < height; ++i) { for (int j = 0; j < width; ++j) { outputCorners.at(i * width + j) = corners.at((height - i - 1) * width + width - j - 1); } } corners = outputCorners; } return true; } void Chessboard::getQuadrangleHypotheses(const std::vector< std::vector >& contours, std::vector< std::pair >& quads, int classId) const { const float minAspectRatio = 0.2f; const float maxAspectRatio = 5.0f; const float minBoxSize = 10.0f; for (size_t i = 0; i < contours.size(); ++i) { cv::RotatedRect box = cv::minAreaRect(contours.at(i)); float boxSize = std::max(box.size.width, box.size.height); if (boxSize < minBoxSize) { continue; } float aspectRatio = box.size.width / std::max(box.size.height, 1.0f); if (aspectRatio < minAspectRatio || aspectRatio > maxAspectRatio) { continue; } quads.push_back(std::pair(boxSize, classId)); } } bool less_pred(const std::pair& p1, const std::pair& p2) { return p1.first < p2.first; } void countClasses(const std::vector >& pairs, size_t idx1, size_t idx2, std::vector& counts) { counts.assign(2, 0); for (size_t i = idx1; i != idx2; ++i) { counts[pairs[i].second]++; } } bool Chessboard::checkChessboard(const cv::Mat& image, cv::Size patternSize) const { const int erosionCount = 1; const float blackLevel = 20.f; const float whiteLevel = 130.f; const float blackWhiteGap = 70.f; cv::Mat white = image.clone(); cv::Mat black = image.clone(); cv::erode(white, white, cv::Mat(), cv::Point(-1,-1), erosionCount); cv::dilate(black, black, cv::Mat(), cv::Point(-1,-1), erosionCount); cv::Mat thresh(image.rows, image.cols, CV_8UC1); bool result = false; for (float threshLevel = blackLevel; threshLevel < whiteLevel && !result; threshLevel += 20.0f) { cv::threshold(white, thresh, threshLevel + blackWhiteGap, 255, cv::THRESH_BINARY); std::vector< std::vector > contours; std::vector hierarchy; // Initialize contour retrieving routine cv::findContours(thresh, contours, hierarchy, cv::RETR_CCOMP, cv::CHAIN_APPROX_SIMPLE); std::vector > quads; getQuadrangleHypotheses(contours, quads, 1); cv::threshold(black, thresh, threshLevel, 255, cv::THRESH_BINARY_INV); cv::findContours(thresh, contours, hierarchy, cv::RETR_CCOMP, cv::CHAIN_APPROX_SIMPLE); getQuadrangleHypotheses(contours, quads, 0); const size_t min_quads_count = patternSize.width * patternSize.height / 2; std::sort(quads.begin(), quads.end(), less_pred); // now check if there are many hypotheses with similar sizes // do this by floodfill-style algorithm const float sizeRelDev = 0.4f; for (size_t i = 0; i < quads.size(); ++i) { size_t j = i + 1; for (; j < quads.size(); ++j) { if (quads[j].first / quads[i].first > 1.0f + sizeRelDev) { break; } } if (j + 1 > min_quads_count + i) { // check the number of black and white squares std::vector counts; countClasses(quads, i, j, counts); const int blackCount = lroundf(ceilf(patternSize.width / 2.0f) * ceilf(patternSize.height / 2.0f)); const int whiteCount = lroundf(floorf(patternSize.width / 2.0f) * floorf(patternSize.height / 2.0f)); if (counts[0] < blackCount * 0.75f || counts[1] < whiteCount * 0.75f) { continue; } result = true; break; } } } return result; } bool Chessboard::checkBoardMonotony(std::vector& corners, cv::Size patternSize) { const float threshFactor = 0.2f; Spline splineXY, splineYX; splineXY.setLowBC(Spline::PARABOLIC_RUNOUT_BC); splineXY.setHighBC(Spline::PARABOLIC_RUNOUT_BC); splineYX.setLowBC(Spline::PARABOLIC_RUNOUT_BC); splineYX.setHighBC(Spline::PARABOLIC_RUNOUT_BC); // check if each row of corners approximates a cubic spline for (int i = 0; i < patternSize.height; ++i) { splineXY.clear(); splineYX.clear(); cv::Point2f p[3]; p[0] = corners.at(i * patternSize.width)->pt; p[1] = corners.at(i * patternSize.width + patternSize.width / 2)->pt; p[2] = corners.at(i * patternSize.width + patternSize.width - 1)->pt; for (int j = 0; j < 3; ++j) { splineXY.addPoint(p[j].x, p[j].y); splineYX.addPoint(p[j].y, p[j].x); } for (int j = 1; j < patternSize.width - 1; ++j) { cv::Point2f& p_j = corners.at(i * patternSize.width + j)->pt; float thresh = std::numeric_limits::max(); // up-neighbor if (i > 0) { cv::Point2f& neighbor = corners.at((i - 1) * patternSize.width + j)->pt; thresh = fminf(thresh, cv::norm(neighbor - p_j)); } // down-neighbor if (i < patternSize.height - 1) { cv::Point2f& neighbor = corners.at((i + 1) * patternSize.width + j)->pt; thresh = fminf(thresh, cv::norm(neighbor - p_j)); } // left-neighbor { cv::Point2f& neighbor = corners.at(i * patternSize.width + j - 1)->pt; thresh = fminf(thresh, cv::norm(neighbor - p_j)); } // right-neighbor { cv::Point2f& neighbor = corners.at(i * patternSize.width + j + 1)->pt; thresh = fminf(thresh, cv::norm(neighbor - p_j)); } thresh *= threshFactor; if (fminf(fabsf(splineXY(p_j.x) - p_j.y), fabsf(splineYX(p_j.y) - p_j.x)) > thresh) { return false; } } } // check if each column of corners approximates a cubic spline for (int j = 0; j < patternSize.width; ++j) { splineXY.clear(); splineYX.clear(); cv::Point2f p[3]; p[0] = corners.at(j)->pt; p[1] = corners.at(patternSize.height / 2 * patternSize.width + j)->pt; p[2] = corners.at((patternSize.height - 1) * patternSize.width + j)->pt; for (int i = 0; i < 3; ++i) { splineXY.addPoint(p[i].x, p[i].y); splineYX.addPoint(p[i].y, p[i].x); } for (int i = 1; i < patternSize.height - 1; ++i) { cv::Point2f& p_i = corners.at(i * patternSize.width + j)->pt; float thresh = std::numeric_limits::max(); // up-neighbor { cv::Point2f& neighbor = corners.at((i - 1) * patternSize.width + j)->pt; thresh = fminf(thresh, cv::norm(neighbor - p_i)); } // down-neighbor { cv::Point2f& neighbor = corners.at((i + 1) * patternSize.width + j)->pt; thresh = fminf(thresh, cv::norm(neighbor - p_i)); } // left-neighbor if (j > 0) { cv::Point2f& neighbor = corners.at(i * patternSize.width + j - 1)->pt; thresh = fminf(thresh, cv::norm(neighbor - p_i)); } // right-neighbor if (j < patternSize.width - 1) { cv::Point2f& neighbor = corners.at(i * patternSize.width + j + 1)->pt; thresh = fminf(thresh, cv::norm(neighbor - p_i)); } thresh *= threshFactor; if (fminf(fabsf(splineXY(p_i.x) - p_i.y), fabsf(splineYX(p_i.y) - p_i.x)) > thresh) { return false; } } } return true; } bool Chessboard::matchCorners(ChessboardQuadPtr& quad1, int corner1, ChessboardQuadPtr& quad2, int corner2) const { // First Check everything from the viewpoint of the // current quad compute midpoints of "parallel" quad // sides 1 float x1 = (quad1->corners[corner1]->pt.x + quad1->corners[(corner1+1)%4]->pt.x)/2; float y1 = (quad1->corners[corner1]->pt.y + quad1->corners[(corner1+1)%4]->pt.y)/2; float x2 = (quad1->corners[(corner1+2)%4]->pt.x + quad1->corners[(corner1+3)%4]->pt.x)/2; float y2 = (quad1->corners[(corner1+2)%4]->pt.y + quad1->corners[(corner1+3)%4]->pt.y)/2; // compute midpoints of "parallel" quad sides 2 float x3 = (quad1->corners[corner1]->pt.x + quad1->corners[(corner1+3)%4]->pt.x)/2; float y3 = (quad1->corners[corner1]->pt.y + quad1->corners[(corner1+3)%4]->pt.y)/2; float x4 = (quad1->corners[(corner1+1)%4]->pt.x + quad1->corners[(corner1+2)%4]->pt.x)/2; float y4 = (quad1->corners[(corner1+1)%4]->pt.y + quad1->corners[(corner1+2)%4]->pt.y)/2; // MARTIN: Heuristic // For corner2 of quad2 to be considered, // it needs to be on the same side of the two lines as // corner1. This is given, if the cross product has // the same sign for both computations below: float a1 = x1 - x2; float b1 = y1 - y2; // the current corner float c11 = quad1->corners[corner1]->pt.x - x2; float d11 = quad1->corners[corner1]->pt.y - y2; // the candidate corner float c12 = quad2->corners[corner2]->pt.x - x2; float d12 = quad2->corners[corner2]->pt.y - y2; float sign11 = a1*d11 - c11*b1; float sign12 = a1*d12 - c12*b1; float a2 = x3 - x4; float b2 = y3 - y4; // the current corner float c21 = quad1->corners[corner1]->pt.x - x4; float d21 = quad1->corners[corner1]->pt.y - y4; // the candidate corner float c22 = quad2->corners[corner2]->pt.x - x4; float d22 = quad2->corners[corner2]->pt.y - y4; float sign21 = a2*d21 - c21*b2; float sign22 = a2*d22 - c22*b2; // Also make shure that two border quads of the same row or // column don't link. Check from the current corner's view, // whether the corner diagonal from the candidate corner // is also on the same side of the two lines as the current // corner and the candidate corner. float c13 = quad2->corners[(corner2+2)%4]->pt.x - x2; float d13 = quad2->corners[(corner2+2)%4]->pt.y - y2; float c23 = quad2->corners[(corner2+2)%4]->pt.x - x4; float d23 = quad2->corners[(corner2+2)%4]->pt.y - y4; float sign13 = a1*d13 - c13*b1; float sign23 = a2*d23 - c23*b2; // Second: Then check everything from the viewpoint of // the candidate quad. Compute midpoints of "parallel" // quad sides 1 float u1 = (quad2->corners[corner2]->pt.x + quad2->corners[(corner2+1)%4]->pt.x)/2; float v1 = (quad2->corners[corner2]->pt.y + quad2->corners[(corner2+1)%4]->pt.y)/2; float u2 = (quad2->corners[(corner2+2)%4]->pt.x + quad2->corners[(corner2+3)%4]->pt.x)/2; float v2 = (quad2->corners[(corner2+2)%4]->pt.y + quad2->corners[(corner2+3)%4]->pt.y)/2; // compute midpoints of "parallel" quad sides 2 float u3 = (quad2->corners[corner2]->pt.x + quad2->corners[(corner2+3)%4]->pt.x)/2; float v3 = (quad2->corners[corner2]->pt.y + quad2->corners[(corner2+3)%4]->pt.y)/2; float u4 = (quad2->corners[(corner2+1)%4]->pt.x + quad2->corners[(corner2+2)%4]->pt.x)/2; float v4 = (quad2->corners[(corner2+1)%4]->pt.y + quad2->corners[(corner2+2)%4]->pt.y)/2; // MARTIN: Heuristic // For corner2 of quad2 to be considered, // it needs to be on the same side of the two lines as // corner1. This is given, if the cross product has // the same sign for both computations below: float a3 = u1 - u2; float b3 = v1 - v2; // the current corner float c31 = quad1->corners[corner1]->pt.x - u2; float d31 = quad1->corners[corner1]->pt.y - v2; // the candidate corner float c32 = quad2->corners[corner2]->pt.x - u2; float d32 = quad2->corners[corner2]->pt.y - v2; float sign31 = a3*d31-c31*b3; float sign32 = a3*d32-c32*b3; float a4 = u3 - u4; float b4 = v3 - v4; // the current corner float c41 = quad1->corners[corner1]->pt.x - u4; float d41 = quad1->corners[corner1]->pt.y - v4; // the candidate corner float c42 = quad2->corners[corner2]->pt.x - u4; float d42 = quad2->corners[corner2]->pt.y - v4; float sign41 = a4*d41-c41*b4; float sign42 = a4*d42-c42*b4; // Also make sure that two border quads of the same row or // column don't link. Check from the candidate corner's view, // whether the corner diagonal from the current corner // is also on the same side of the two lines as the current // corner and the candidate corner. float c33 = quad1->corners[(corner1+2)%4]->pt.x - u2; float d33 = quad1->corners[(corner1+2)%4]->pt.y - v2; float c43 = quad1->corners[(corner1+2)%4]->pt.x - u4; float d43 = quad1->corners[(corner1+2)%4]->pt.y - v4; float sign33 = a3*d33-c33*b3; float sign43 = a4*d43-c43*b4; // This time we also need to make shure, that no quad // is linked to a quad of another dilation run which // may lie INSIDE it!!! // Third: Therefore check everything from the viewpoint // of the current quad compute midpoints of "parallel" // quad sides 1 float x5 = quad1->corners[corner1]->pt.x; float y5 = quad1->corners[corner1]->pt.y; float x6 = quad1->corners[(corner1+1)%4]->pt.x; float y6 = quad1->corners[(corner1+1)%4]->pt.y; // compute midpoints of "parallel" quad sides 2 float x7 = x5; float y7 = y5; float x8 = quad1->corners[(corner1+3)%4]->pt.x; float y8 = quad1->corners[(corner1+3)%4]->pt.y; // MARTIN: Heuristic // For corner2 of quad2 to be considered, // it needs to be on the other side of the two lines than // corner1. This is given, if the cross product has // a different sign for both computations below: float a5 = x6 - x5; float b5 = y6 - y5; // the current corner float c51 = quad1->corners[(corner1+2)%4]->pt.x - x5; float d51 = quad1->corners[(corner1+2)%4]->pt.y - y5; // the candidate corner float c52 = quad2->corners[corner2]->pt.x - x5; float d52 = quad2->corners[corner2]->pt.y - y5; float sign51 = a5*d51 - c51*b5; float sign52 = a5*d52 - c52*b5; float a6 = x8 - x7; float b6 = y8 - y7; // the current corner float c61 = quad1->corners[(corner1+2)%4]->pt.x - x7; float d61 = quad1->corners[(corner1+2)%4]->pt.y - y7; // the candidate corner float c62 = quad2->corners[corner2]->pt.x - x7; float d62 = quad2->corners[corner2]->pt.y - y7; float sign61 = a6*d61 - c61*b6; float sign62 = a6*d62 - c62*b6; // Fourth: Then check everything from the viewpoint of // the candidate quad compute midpoints of "parallel" // quad sides 1 float u5 = quad2->corners[corner2]->pt.x; float v5 = quad2->corners[corner2]->pt.y; float u6 = quad2->corners[(corner2+1)%4]->pt.x; float v6 = quad2->corners[(corner2+1)%4]->pt.y; // compute midpoints of "parallel" quad sides 2 float u7 = u5; float v7 = v5; float u8 = quad2->corners[(corner2+3)%4]->pt.x; float v8 = quad2->corners[(corner2+3)%4]->pt.y; // MARTIN: Heuristic // For corner2 of quad2 to be considered, // it needs to be on the other side of the two lines than // corner1. This is given, if the cross product has // a different sign for both computations below: float a7 = u6 - u5; float b7 = v6 - v5; // the current corner float c71 = quad1->corners[corner1]->pt.x - u5; float d71 = quad1->corners[corner1]->pt.y - v5; // the candidate corner float c72 = quad2->corners[(corner2+2)%4]->pt.x - u5; float d72 = quad2->corners[(corner2+2)%4]->pt.y - v5; float sign71 = a7*d71-c71*b7; float sign72 = a7*d72-c72*b7; float a8 = u8 - u7; float b8 = v8 - v7; // the current corner float c81 = quad1->corners[corner1]->pt.x - u7; float d81 = quad1->corners[corner1]->pt.y - v7; // the candidate corner float c82 = quad2->corners[(corner2+2)%4]->pt.x - u7; float d82 = quad2->corners[(corner2+2)%4]->pt.y - v7; float sign81 = a8*d81-c81*b8; float sign82 = a8*d82-c82*b8; // Check whether conditions are fulfilled if (((sign11 < 0 && sign12 < 0) || (sign11 > 0 && sign12 > 0)) && ((sign21 < 0 && sign22 < 0) || (sign21 > 0 && sign22 > 0)) && ((sign31 < 0 && sign32 < 0) || (sign31 > 0 && sign32 > 0)) && ((sign41 < 0 && sign42 < 0) || (sign41 > 0 && sign42 > 0)) && ((sign11 < 0 && sign13 < 0) || (sign11 > 0 && sign13 > 0)) && ((sign21 < 0 && sign23 < 0) || (sign21 > 0 && sign23 > 0)) && ((sign31 < 0 && sign33 < 0) || (sign31 > 0 && sign33 > 0)) && ((sign41 < 0 && sign43 < 0) || (sign41 > 0 && sign43 > 0)) && ((sign51 < 0 && sign52 > 0) || (sign51 > 0 && sign52 < 0)) && ((sign61 < 0 && sign62 > 0) || (sign61 > 0 && sign62 < 0)) && ((sign71 < 0 && sign72 > 0) || (sign71 > 0 && sign72 < 0)) && ((sign81 < 0 && sign82 > 0) || (sign81 > 0 && sign82 < 0))) { return true; } else { return false; } } } ================================================ FILE: camera_model/src/gpl/EigenQuaternionParameterization.cc ================================================ #include "camodocal/gpl/EigenQuaternionParameterization.h" #include namespace camodocal { bool EigenQuaternionParameterization::Plus(const double* x, const double* delta, double* x_plus_delta) const { const double norm_delta = sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]); if (norm_delta > 0.0) { const double sin_delta_by_delta = (sin(norm_delta) / norm_delta); double q_delta[4]; q_delta[0] = sin_delta_by_delta * delta[0]; q_delta[1] = sin_delta_by_delta * delta[1]; q_delta[2] = sin_delta_by_delta * delta[2]; q_delta[3] = cos(norm_delta); EigenQuaternionProduct(q_delta, x, x_plus_delta); } else { for (int i = 0; i < 4; ++i) { x_plus_delta[i] = x[i]; } } return true; } bool EigenQuaternionParameterization::ComputeJacobian(const double* x, double* jacobian) const { jacobian[0] = x[3]; jacobian[1] = x[2]; jacobian[2] = -x[1]; // NOLINT jacobian[3] = -x[2]; jacobian[4] = x[3]; jacobian[5] = x[0]; // NOLINT jacobian[6] = x[1]; jacobian[7] = -x[0]; jacobian[8] = x[3]; // NOLINT jacobian[9] = -x[0]; jacobian[10] = -x[1]; jacobian[11] = -x[2]; // NOLINT return true; } } ================================================ FILE: camera_model/src/gpl/gpl.cc ================================================ #include "camodocal/gpl/gpl.h" #include #ifdef _WIN32 #include #else #include #endif // source: https://stackoverflow.com/questions/5167269/clock-gettime-alternative-in-mac-os-x #ifdef __APPLE__ #include #define ORWL_NANO (+1.0E-9) #define ORWL_GIGA UINT64_C(1000000000) static double orwl_timebase = 0.0; static uint64_t orwl_timestart = 0; struct timespec orwl_gettime(void) { // be more careful in a multithreaded environement if (!orwl_timestart) { mach_timebase_info_data_t tb = { 0 }; mach_timebase_info(&tb); orwl_timebase = tb.numer; orwl_timebase /= tb.denom; orwl_timestart = mach_absolute_time(); } struct timespec t; double diff = (mach_absolute_time() - orwl_timestart) * orwl_timebase; t.tv_sec = diff * ORWL_NANO; t.tv_nsec = diff - (t.tv_sec * ORWL_GIGA); return t; } #endif // __APPLE__ const double WGS84_A = 6378137.0; const double WGS84_ECCSQ = 0.00669437999013; // Windows lacks fminf #ifndef fminf #define fminf(x, y) (((x) < (y)) ? (x) : (y)) #endif namespace camodocal { double hypot3(double x, double y, double z) { return sqrt(square(x) + square(y) + square(z)); } float hypot3f(float x, float y, float z) { return sqrtf(square(x) + square(y) + square(z)); } double d2r(double deg) { return deg / 180.0 * M_PI; } float d2r(float deg) { return deg / 180.0f * M_PI; } double r2d(double rad) { return rad / M_PI * 180.0; } float r2d(float rad) { return rad / M_PI * 180.0f; } double sinc(double theta) { return sin(theta) / theta; } #ifdef _WIN32 #include #include #include LARGE_INTEGER getFILETIMEoffset() { SYSTEMTIME s; FILETIME f; LARGE_INTEGER t; s.wYear = 1970; s.wMonth = 1; s.wDay = 1; s.wHour = 0; s.wMinute = 0; s.wSecond = 0; s.wMilliseconds = 0; SystemTimeToFileTime(&s, &f); t.QuadPart = f.dwHighDateTime; t.QuadPart <<= 32; t.QuadPart |= f.dwLowDateTime; return (t); } int clock_gettime(int X, struct timespec *tp) { LARGE_INTEGER t; FILETIME f; double microseconds; static LARGE_INTEGER offset; static double frequencyToMicroseconds; static int initialized = 0; static BOOL usePerformanceCounter = 0; if (!initialized) { LARGE_INTEGER performanceFrequency; initialized = 1; usePerformanceCounter = QueryPerformanceFrequency(&performanceFrequency); if (usePerformanceCounter) { QueryPerformanceCounter(&offset); frequencyToMicroseconds = (double)performanceFrequency.QuadPart / 1000000.; } else { offset = getFILETIMEoffset(); frequencyToMicroseconds = 10.; } } if (usePerformanceCounter) QueryPerformanceCounter(&t); else { GetSystemTimeAsFileTime(&f); t.QuadPart = f.dwHighDateTime; t.QuadPart <<= 32; t.QuadPart |= f.dwLowDateTime; } t.QuadPart -= offset.QuadPart; microseconds = (double)t.QuadPart / frequencyToMicroseconds; t.QuadPart = microseconds; tp->tv_sec = t.QuadPart / 1000000; tp->tv_nsec = (t.QuadPart % 1000000) * 1000; return (0); } #endif unsigned long long timeInMicroseconds(void) { struct timespec tp; #ifdef __APPLE__ tp = orwl_gettime(); #else clock_gettime(CLOCK_REALTIME, &tp); #endif return tp.tv_sec * 1000000 + tp.tv_nsec / 1000; } double timeInSeconds(void) { struct timespec tp; #ifdef __APPLE__ tp = orwl_gettime(); #else clock_gettime(CLOCK_REALTIME, &tp); #endif return static_cast(tp.tv_sec) + static_cast(tp.tv_nsec) / 1000000000.0; } float colormapAutumn[128][3] = { {1.0f,0.f,0.f}, {1.0f,0.007874f,0.f}, {1.0f,0.015748f,0.f}, {1.0f,0.023622f,0.f}, {1.0f,0.031496f,0.f}, {1.0f,0.03937f,0.f}, {1.0f,0.047244f,0.f}, {1.0f,0.055118f,0.f}, {1.0f,0.062992f,0.f}, {1.0f,0.070866f,0.f}, {1.0f,0.07874f,0.f}, {1.0f,0.086614f,0.f}, {1.0f,0.094488f,0.f}, {1.0f,0.10236f,0.f}, {1.0f,0.11024f,0.f}, {1.0f,0.11811f,0.f}, {1.0f,0.12598f,0.f}, {1.0f,0.13386f,0.f}, {1.0f,0.14173f,0.f}, {1.0f,0.14961f,0.f}, {1.0f,0.15748f,0.f}, {1.0f,0.16535f,0.f}, {1.0f,0.17323f,0.f}, {1.0f,0.1811f,0.f}, {1.0f,0.18898f,0.f}, {1.0f,0.19685f,0.f}, {1.0f,0.20472f,0.f}, {1.0f,0.2126f,0.f}, {1.0f,0.22047f,0.f}, {1.0f,0.22835f,0.f}, {1.0f,0.23622f,0.f}, {1.0f,0.24409f,0.f}, {1.0f,0.25197f,0.f}, {1.0f,0.25984f,0.f}, {1.0f,0.26772f,0.f}, {1.0f,0.27559f,0.f}, {1.0f,0.28346f,0.f}, {1.0f,0.29134f,0.f}, {1.0f,0.29921f,0.f}, {1.0f,0.30709f,0.f}, {1.0f,0.31496f,0.f}, {1.0f,0.32283f,0.f}, {1.0f,0.33071f,0.f}, {1.0f,0.33858f,0.f}, {1.0f,0.34646f,0.f}, {1.0f,0.35433f,0.f}, {1.0f,0.3622f,0.f}, {1.0f,0.37008f,0.f}, {1.0f,0.37795f,0.f}, {1.0f,0.38583f,0.f}, {1.0f,0.3937f,0.f}, {1.0f,0.40157f,0.f}, {1.0f,0.40945f,0.f}, {1.0f,0.41732f,0.f}, {1.0f,0.4252f,0.f}, {1.0f,0.43307f,0.f}, {1.0f,0.44094f,0.f}, {1.0f,0.44882f,0.f}, {1.0f,0.45669f,0.f}, {1.0f,0.46457f,0.f}, {1.0f,0.47244f,0.f}, {1.0f,0.48031f,0.f}, {1.0f,0.48819f,0.f}, {1.0f,0.49606f,0.f}, {1.0f,0.50394f,0.f}, {1.0f,0.51181f,0.f}, {1.0f,0.51969f,0.f}, {1.0f,0.52756f,0.f}, {1.0f,0.53543f,0.f}, {1.0f,0.54331f,0.f}, {1.0f,0.55118f,0.f}, {1.0f,0.55906f,0.f}, {1.0f,0.56693f,0.f}, {1.0f,0.5748f,0.f}, {1.0f,0.58268f,0.f}, {1.0f,0.59055f,0.f}, {1.0f,0.59843f,0.f}, {1.0f,0.6063f,0.f}, {1.0f,0.61417f,0.f}, {1.0f,0.62205f,0.f}, {1.0f,0.62992f,0.f}, {1.0f,0.6378f,0.f}, {1.0f,0.64567f,0.f}, {1.0f,0.65354f,0.f}, {1.0f,0.66142f,0.f}, {1.0f,0.66929f,0.f}, {1.0f,0.67717f,0.f}, {1.0f,0.68504f,0.f}, {1.0f,0.69291f,0.f}, {1.0f,0.70079f,0.f}, {1.0f,0.70866f,0.f}, {1.0f,0.71654f,0.f}, {1.0f,0.72441f,0.f}, {1.0f,0.73228f,0.f}, {1.0f,0.74016f,0.f}, {1.0f,0.74803f,0.f}, {1.0f,0.75591f,0.f}, {1.0f,0.76378f,0.f}, {1.0f,0.77165f,0.f}, {1.0f,0.77953f,0.f}, {1.0f,0.7874f,0.f}, {1.0f,0.79528f,0.f}, {1.0f,0.80315f,0.f}, {1.0f,0.81102f,0.f}, {1.0f,0.8189f,0.f}, {1.0f,0.82677f,0.f}, {1.0f,0.83465f,0.f}, {1.0f,0.84252f,0.f}, {1.0f,0.85039f,0.f}, {1.0f,0.85827f,0.f}, {1.0f,0.86614f,0.f}, {1.0f,0.87402f,0.f}, {1.0f,0.88189f,0.f}, {1.0f,0.88976f,0.f}, {1.0f,0.89764f,0.f}, {1.0f,0.90551f,0.f}, {1.0f,0.91339f,0.f}, {1.0f,0.92126f,0.f}, {1.0f,0.92913f,0.f}, {1.0f,0.93701f,0.f}, {1.0f,0.94488f,0.f}, {1.0f,0.95276f,0.f}, {1.0f,0.96063f,0.f}, {1.0f,0.9685f,0.f}, {1.0f,0.97638f,0.f}, {1.0f,0.98425f,0.f}, {1.0f,0.99213f,0.f}, {1.0f,1.0f,0.0f} }; float colormapJet[128][3] = { {0.0f,0.0f,0.53125f}, {0.0f,0.0f,0.5625f}, {0.0f,0.0f,0.59375f}, {0.0f,0.0f,0.625f}, {0.0f,0.0f,0.65625f}, {0.0f,0.0f,0.6875f}, {0.0f,0.0f,0.71875f}, {0.0f,0.0f,0.75f}, {0.0f,0.0f,0.78125f}, {0.0f,0.0f,0.8125f}, {0.0f,0.0f,0.84375f}, {0.0f,0.0f,0.875f}, {0.0f,0.0f,0.90625f}, {0.0f,0.0f,0.9375f}, {0.0f,0.0f,0.96875f}, {0.0f,0.0f,1.0f}, {0.0f,0.03125f,1.0f}, {0.0f,0.0625f,1.0f}, {0.0f,0.09375f,1.0f}, {0.0f,0.125f,1.0f}, {0.0f,0.15625f,1.0f}, {0.0f,0.1875f,1.0f}, {0.0f,0.21875f,1.0f}, {0.0f,0.25f,1.0f}, {0.0f,0.28125f,1.0f}, {0.0f,0.3125f,1.0f}, {0.0f,0.34375f,1.0f}, {0.0f,0.375f,1.0f}, {0.0f,0.40625f,1.0f}, {0.0f,0.4375f,1.0f}, {0.0f,0.46875f,1.0f}, {0.0f,0.5f,1.0f}, {0.0f,0.53125f,1.0f}, {0.0f,0.5625f,1.0f}, {0.0f,0.59375f,1.0f}, {0.0f,0.625f,1.0f}, {0.0f,0.65625f,1.0f}, {0.0f,0.6875f,1.0f}, {0.0f,0.71875f,1.0f}, {0.0f,0.75f,1.0f}, {0.0f,0.78125f,1.0f}, {0.0f,0.8125f,1.0f}, {0.0f,0.84375f,1.0f}, {0.0f,0.875f,1.0f}, {0.0f,0.90625f,1.0f}, {0.0f,0.9375f,1.0f}, {0.0f,0.96875f,1.0f}, {0.0f,1.0f,1.0f}, {0.03125f,1.0f,0.96875f}, {0.0625f,1.0f,0.9375f}, {0.09375f,1.0f,0.90625f}, {0.125f,1.0f,0.875f}, {0.15625f,1.0f,0.84375f}, {0.1875f,1.0f,0.8125f}, {0.21875f,1.0f,0.78125f}, {0.25f,1.0f,0.75f}, {0.28125f,1.0f,0.71875f}, {0.3125f,1.0f,0.6875f}, {0.34375f,1.0f,0.65625f}, {0.375f,1.0f,0.625f}, {0.40625f,1.0f,0.59375f}, {0.4375f,1.0f,0.5625f}, {0.46875f,1.0f,0.53125f}, {0.5f,1.0f,0.5f}, {0.53125f,1.0f,0.46875f}, {0.5625f,1.0f,0.4375f}, {0.59375f,1.0f,0.40625f}, {0.625f,1.0f,0.375f}, {0.65625f,1.0f,0.34375f}, {0.6875f,1.0f,0.3125f}, {0.71875f,1.0f,0.28125f}, {0.75f,1.0f,0.25f}, {0.78125f,1.0f,0.21875f}, {0.8125f,1.0f,0.1875f}, {0.84375f,1.0f,0.15625f}, {0.875f,1.0f,0.125f}, {0.90625f,1.0f,0.09375f}, {0.9375f,1.0f,0.0625f}, {0.96875f,1.0f,0.03125f}, {1.0f,1.0f,0.0f}, {1.0f,0.96875f,0.0f}, {1.0f,0.9375f,0.0f}, {1.0f,0.90625f,0.0f}, {1.0f,0.875f,0.0f}, {1.0f,0.84375f,0.0f}, {1.0f,0.8125f,0.0f}, {1.0f,0.78125f,0.0f}, {1.0f,0.75f,0.0f}, {1.0f,0.71875f,0.0f}, {1.0f,0.6875f,0.0f}, {1.0f,0.65625f,0.0f}, {1.0f,0.625f,0.0f}, {1.0f,0.59375f,0.0f}, {1.0f,0.5625f,0.0f}, {1.0f,0.53125f,0.0f}, {1.0f,0.5f,0.0f}, {1.0f,0.46875f,0.0f}, {1.0f,0.4375f,0.0f}, {1.0f,0.40625f,0.0f}, {1.0f,0.375f,0.0f}, {1.0f,0.34375f,0.0f}, {1.0f,0.3125f,0.0f}, {1.0f,0.28125f,0.0f}, {1.0f,0.25f,0.0f}, {1.0f,0.21875f,0.0f}, {1.0f,0.1875f,0.0f}, {1.0f,0.15625f,0.0f}, {1.0f,0.125f,0.0f}, {1.0f,0.09375f,0.0f}, {1.0f,0.0625f,0.0f}, {1.0f,0.03125f,0.0f}, {1.0f,0.0f,0.0f}, {0.96875f,0.0f,0.0f}, {0.9375f,0.0f,0.0f}, {0.90625f,0.0f,0.0f}, {0.875f,0.0f,0.0f}, {0.84375f,0.0f,0.0f}, {0.8125f,0.0f,0.0f}, {0.78125f,0.0f,0.0f}, {0.75f,0.0f,0.0f}, {0.71875f,0.0f,0.0f}, {0.6875f,0.0f,0.0f}, {0.65625f,0.0f,0.0f}, {0.625f,0.0f,0.0f}, {0.59375f,0.0f,0.0f}, {0.5625f,0.0f,0.0f}, {0.53125f,0.0f,0.0f}, {0.5f,0.0f,0.0f} }; void colorDepthImage(cv::Mat& imgDepth, cv::Mat& imgColoredDepth, float minRange, float maxRange) { imgColoredDepth = cv::Mat::zeros(imgDepth.size(), CV_8UC3); for (int i = 0; i < imgColoredDepth.rows; ++i) { const float* depth = imgDepth.ptr(i); unsigned char* pixel = imgColoredDepth.ptr(i); for (int j = 0; j < imgColoredDepth.cols; ++j) { if (depth[j] != 0) { int idx = fminf(depth[j] - minRange, maxRange - minRange) / (maxRange - minRange) * 127.0f; idx = 127 - idx; pixel[0] = colormapJet[idx][2] * 255.0f; pixel[1] = colormapJet[idx][1] * 255.0f; pixel[2] = colormapJet[idx][0] * 255.0f; } pixel += 3; } } } bool colormap(const std::string& name, unsigned char idx, float& r, float& g, float& b) { if (name.compare("jet") == 0) { float* color = colormapJet[idx]; r = color[0]; g = color[1]; b = color[2]; return true; } else if (name.compare("autumn") == 0) { float* color = colormapAutumn[idx]; r = color[0]; g = color[1]; b = color[2]; return true; } return false; } std::vector bresLine(int x0, int y0, int x1, int y1) { // Bresenham's line algorithm // Find cells intersected by line between (x0,y0) and (x1,y1) std::vector cells; int dx = std::abs(x1 - x0); int dy = std::abs(y1 - y0); int sx = (x0 < x1) ? 1 : -1; int sy = (y0 < y1) ? 1 : -1; int err = dx - dy; while (1) { cells.push_back(cv::Point2i(x0, y0)); if (x0 == x1 && y0 == y1) { break; } int e2 = 2 * err; if (e2 > -dy) { err -= dy; x0 += sx; } if (e2 < dx) { err += dx; y0 += sy; } } return cells; } std::vector bresCircle(int x0, int y0, int r) { // Bresenham's circle algorithm // Find cells intersected by circle with center (x0,y0) and radius r std::vector< std::vector > mask(2 * r + 1); for (int i = 0; i < 2 * r + 1; ++i) { mask[i].resize(2 * r + 1); for (int j = 0; j < 2 * r + 1; ++j) { mask[i][j] = false; } } int f = 1 - r; int ddF_x = 1; int ddF_y = -2 * r; int x = 0; int y = r; std::vector line; line = bresLine(x0, y0 - r, x0, y0 + r); for (std::vector::iterator it = line.begin(); it != line.end(); ++it) { mask[it->x - x0 + r][it->y - y0 + r] = true; } line = bresLine(x0 - r, y0, x0 + r, y0); for (std::vector::iterator it = line.begin(); it != line.end(); ++it) { mask[it->x - x0 + r][it->y - y0 + r] = true; } while (x < y) { if (f >= 0) { y--; ddF_y += 2; f += ddF_y; } x++; ddF_x += 2; f += ddF_x; line = bresLine(x0 - x, y0 + y, x0 + x, y0 + y); for (std::vector::iterator it = line.begin(); it != line.end(); ++it) { mask[it->x - x0 + r][it->y - y0 + r] = true; } line = bresLine(x0 - x, y0 - y, x0 + x, y0 - y); for (std::vector::iterator it = line.begin(); it != line.end(); ++it) { mask[it->x - x0 + r][it->y - y0 + r] = true; } line = bresLine(x0 - y, y0 + x, x0 + y, y0 + x); for (std::vector::iterator it = line.begin(); it != line.end(); ++it) { mask[it->x - x0 + r][it->y - y0 + r] = true; } line = bresLine(x0 - y, y0 - x, x0 + y, y0 - x); for (std::vector::iterator it = line.begin(); it != line.end(); ++it) { mask[it->x - x0 + r][it->y - y0 + r] = true; } } std::vector cells; for (int i = 0; i < 2 * r + 1; ++i) { for (int j = 0; j < 2 * r + 1; ++j) { if (mask[i][j]) { cells.push_back(cv::Point2i(i - r + x0, j - r + y0)); } } } return cells; } void fitCircle(const std::vector& points, double& centerX, double& centerY, double& radius) { // D. Umbach, and K. Jones, A Few Methods for Fitting Circles to Data, // IEEE Transactions on Instrumentation and Measurement, 2000 // We use the modified least squares method. double sum_x = 0.0; double sum_y = 0.0; double sum_xx = 0.0; double sum_xy = 0.0; double sum_yy = 0.0; double sum_xxx = 0.0; double sum_xxy = 0.0; double sum_xyy = 0.0; double sum_yyy = 0.0; int n = points.size(); for (int i = 0; i < n; ++i) { double x = points.at(i).x; double y = points.at(i).y; sum_x += x; sum_y += y; sum_xx += x * x; sum_xy += x * y; sum_yy += y * y; sum_xxx += x * x * x; sum_xxy += x * x * y; sum_xyy += x * y * y; sum_yyy += y * y * y; } double A = n * sum_xx - square(sum_x); double B = n * sum_xy - sum_x * sum_y; double C = n * sum_yy - square(sum_y); double D = 0.5 * (n * sum_xyy - sum_x * sum_yy + n * sum_xxx - sum_x * sum_xx); double E = 0.5 * (n * sum_xxy - sum_y * sum_xx + n * sum_yyy - sum_y * sum_yy); centerX = (D * C - B * E) / (A * C - square(B)); centerY = (A * E - B * D) / (A * C - square(B)); double sum_r = 0.0; for (int i = 0; i < n; ++i) { double x = points.at(i).x; double y = points.at(i).y; sum_r += hypot(x - centerX, y - centerY); } radius = sum_r / n; } std::vector intersectCircles(double x1, double y1, double r1, double x2, double y2, double r2) { std::vector ipts; double d = hypot(x1 - x2, y1 - y2); if (d > r1 + r2) { // circles are separate return ipts; } if (d < fabs(r1 - r2)) { // one circle is contained within the other return ipts; } double a = (square(r1) - square(r2) + square(d)) / (2.0 * d); double h = sqrt(square(r1) - square(a)); double x3 = x1 + a * (x2 - x1) / d; double y3 = y1 + a * (y2 - y1) / d; if (h < 1e-10) { // two circles touch at one point ipts.push_back(cv::Point2d(x3, y3)); return ipts; } ipts.push_back(cv::Point2d(x3 + h * (y2 - y1) / d, y3 - h * (x2 - x1) / d)); ipts.push_back(cv::Point2d(x3 - h * (y2 - y1) / d, y3 + h * (x2 - x1) / d)); return ipts; } char UTMLetterDesignator(double latitude) { // This routine determines the correct UTM letter designator for the given latitude // returns 'Z' if latitude is outside the UTM limits of 84N to 80S // Written by Chuck Gantz- chuck.gantz@globalstar.com char letterDesignator; if ((84.0 >= latitude) && (latitude >= 72.0)) letterDesignator = 'X'; else if ((72.0 > latitude) && (latitude >= 64.0)) letterDesignator = 'W'; else if ((64.0 > latitude) && (latitude >= 56.0)) letterDesignator = 'V'; else if ((56.0 > latitude) && (latitude >= 48.0)) letterDesignator = 'U'; else if ((48.0 > latitude) && (latitude >= 40.0)) letterDesignator = 'T'; else if ((40.0 > latitude) && (latitude >= 32.0)) letterDesignator = 'S'; else if ((32.0 > latitude) && (latitude >= 24.0)) letterDesignator = 'R'; else if ((24.0 > latitude) && (latitude >= 16.0)) letterDesignator = 'Q'; else if ((16.0 > latitude) && (latitude >= 8.0)) letterDesignator = 'P'; else if (( 8.0 > latitude) && (latitude >= 0.0)) letterDesignator = 'N'; else if (( 0.0 > latitude) && (latitude >= -8.0)) letterDesignator = 'M'; else if ((-8.0 > latitude) && (latitude >= -16.0)) letterDesignator = 'L'; else if ((-16.0 > latitude) && (latitude >= -24.0)) letterDesignator = 'K'; else if ((-24.0 > latitude) && (latitude >= -32.0)) letterDesignator = 'J'; else if ((-32.0 > latitude) && (latitude >= -40.0)) letterDesignator = 'H'; else if ((-40.0 > latitude) && (latitude >= -48.0)) letterDesignator = 'G'; else if ((-48.0 > latitude) && (latitude >= -56.0)) letterDesignator = 'F'; else if ((-56.0 > latitude) && (latitude >= -64.0)) letterDesignator = 'E'; else if ((-64.0 > latitude) && (latitude >= -72.0)) letterDesignator = 'D'; else if ((-72.0 > latitude) && (latitude >= -80.0)) letterDesignator = 'C'; else letterDesignator = 'Z'; //This is here as an error flag to show that the Latitude is outside the UTM limits return letterDesignator; } void LLtoUTM(double latitude, double longitude, double& utmNorthing, double& utmEasting, std::string& utmZone) { // converts lat/long to UTM coords. Equations from USGS Bulletin 1532 // East Longitudes are positive, West longitudes are negative. // North latitudes are positive, South latitudes are negative // Lat and Long are in decimal degrees // Written by Chuck Gantz- chuck.gantz@globalstar.com double k0 = 0.9996; double LongOrigin; double eccPrimeSquared; double N, T, C, A, M; double LatRad = latitude * M_PI / 180.0; double LongRad = longitude * M_PI / 180.0; double LongOriginRad; int ZoneNumber = static_cast((longitude + 180.0) / 6.0) + 1; if (latitude >= 56.0 && latitude < 64.0 && longitude >= 3.0 && longitude < 12.0) { ZoneNumber = 32; } // Special zones for Svalbard if (latitude >= 72.0 && latitude < 84.0) { if ( longitude >= 0.0 && longitude < 9.0) ZoneNumber = 31; else if (longitude >= 9.0 && longitude < 21.0) ZoneNumber = 33; else if (longitude >= 21.0 && longitude < 33.0) ZoneNumber = 35; else if (longitude >= 33.0 && longitude < 42.0) ZoneNumber = 37; } LongOrigin = static_cast((ZoneNumber - 1) * 6 - 180 + 3); //+3 puts origin in middle of zone LongOriginRad = LongOrigin * M_PI / 180.0; // compute the UTM Zone from the latitude and longitude std::ostringstream oss; oss << ZoneNumber << UTMLetterDesignator(latitude); utmZone = oss.str(); eccPrimeSquared = WGS84_ECCSQ / (1.0 - WGS84_ECCSQ); N = WGS84_A / sqrt(1.0 - WGS84_ECCSQ * sin(LatRad) * sin(LatRad)); T = tan(LatRad) * tan(LatRad); C = eccPrimeSquared * cos(LatRad) * cos(LatRad); A = cos(LatRad) * (LongRad - LongOriginRad); M = WGS84_A * ((1.0 - WGS84_ECCSQ / 4.0 - 3.0 * WGS84_ECCSQ * WGS84_ECCSQ / 64.0 - 5.0 * WGS84_ECCSQ * WGS84_ECCSQ * WGS84_ECCSQ / 256.0) * LatRad - (3.0 * WGS84_ECCSQ / 8.0 + 3.0 * WGS84_ECCSQ * WGS84_ECCSQ / 32.0 + 45.0 * WGS84_ECCSQ * WGS84_ECCSQ * WGS84_ECCSQ / 1024.0) * sin(2.0 * LatRad) + (15.0 * WGS84_ECCSQ * WGS84_ECCSQ / 256.0 + 45.0 * WGS84_ECCSQ * WGS84_ECCSQ * WGS84_ECCSQ / 1024.0) * sin(4.0 * LatRad) - (35.0 * WGS84_ECCSQ * WGS84_ECCSQ * WGS84_ECCSQ / 3072.0) * sin(6.0 * LatRad)); utmEasting = k0 * N * (A + (1.0 - T + C) * A * A * A / 6.0 + (5.0 - 18.0 * T + T * T + 72.0 * C - 58.0 * eccPrimeSquared) * A * A * A * A * A / 120.0) + 500000.0; utmNorthing = k0 * (M + N * tan(LatRad) * (A * A / 2.0 + (5.0 - T + 9.0 * C + 4.0 * C * C) * A * A * A * A / 24.0 + (61.0 - 58.0 * T + T * T + 600.0 * C - 330.0 * eccPrimeSquared) * A * A * A * A * A * A / 720.0)); if (latitude < 0.0) { utmNorthing += 10000000.0; //10000000 meter offset for southern hemisphere } } void UTMtoLL(double utmNorthing, double utmEasting, const std::string& utmZone, double& latitude, double& longitude) { // converts UTM coords to lat/long. Equations from USGS Bulletin 1532 // East Longitudes are positive, West longitudes are negative. // North latitudes are positive, South latitudes are negative // Lat and Long are in decimal degrees. // Written by Chuck Gantz- chuck.gantz@globalstar.com double k0 = 0.9996; double eccPrimeSquared; double e1 = (1.0 - sqrt(1.0 - WGS84_ECCSQ)) / (1.0 + sqrt(1.0 - WGS84_ECCSQ)); double N1, T1, C1, R1, D, M; double LongOrigin; double mu, phi1, phi1Rad; double x, y; int ZoneNumber; char ZoneLetter; bool NorthernHemisphere; x = utmEasting - 500000.0; //remove 500,000 meter offset for longitude y = utmNorthing; std::istringstream iss(utmZone); iss >> ZoneNumber >> ZoneLetter; if ((static_cast(ZoneLetter) - static_cast('N')) >= 0) { NorthernHemisphere = true;//point is in northern hemisphere } else { NorthernHemisphere = false;//point is in southern hemisphere y -= 10000000.0;//remove 10,000,000 meter offset used for southern hemisphere } LongOrigin = (ZoneNumber - 1.0) * 6.0 - 180.0 + 3.0; //+3 puts origin in middle of zone eccPrimeSquared = WGS84_ECCSQ / (1.0 - WGS84_ECCSQ); M = y / k0; mu = M / (WGS84_A * (1.0 - WGS84_ECCSQ / 4.0 - 3.0 * WGS84_ECCSQ * WGS84_ECCSQ / 64.0 - 5.0 * WGS84_ECCSQ * WGS84_ECCSQ * WGS84_ECCSQ / 256.0)); phi1Rad = mu + (3.0 * e1 / 2.0 - 27.0 * e1 * e1 * e1 / 32.0) * sin(2.0 * mu) + (21.0 * e1 * e1 / 16.0 - 55.0 * e1 * e1 * e1 * e1 / 32.0) * sin(4.0 * mu) + (151.0 * e1 * e1 * e1 / 96.0) * sin(6.0 * mu); phi1 = phi1Rad / M_PI * 180.0; N1 = WGS84_A / sqrt(1.0 - WGS84_ECCSQ * sin(phi1Rad) * sin(phi1Rad)); T1 = tan(phi1Rad) * tan(phi1Rad); C1 = eccPrimeSquared * cos(phi1Rad) * cos(phi1Rad); R1 = WGS84_A * (1.0 - WGS84_ECCSQ) / pow(1.0 - WGS84_ECCSQ * sin(phi1Rad) * sin(phi1Rad), 1.5); D = x / (N1 * k0); latitude = phi1Rad - (N1 * tan(phi1Rad) / R1) * (D * D / 2.0 - (5.0 + 3.0 * T1 + 10.0 * C1 - 4.0 * C1 * C1 - 9.0 * eccPrimeSquared) * D * D * D * D / 24.0 + (61.0 + 90.0 * T1 + 298.0 * C1 + 45.0 * T1 * T1 - 252.0 * eccPrimeSquared - 3.0 * C1 * C1) * D * D * D * D * D * D / 720.0); latitude *= 180.0 / M_PI; longitude = (D - (1.0 + 2.0 * T1 + C1) * D * D * D / 6.0 + (5.0 - 2.0 * C1 + 28.0 * T1 - 3.0 * C1 * C1 + 8.0 * eccPrimeSquared + 24.0 * T1 * T1) * D * D * D * D * D / 120.0) / cos(phi1Rad); longitude = LongOrigin + longitude / M_PI * 180.0; } long int timestampDiff(uint64_t t1, uint64_t t2) { if (t2 > t1) { uint64_t d = t2 - t1; if (d > std::numeric_limits::max()) { return std::numeric_limits::max(); } else { return d; } } else { uint64_t d = t1 - t2; if (d > std::numeric_limits::max()) { return std::numeric_limits::min(); } else { return - static_cast(d); } } } } ================================================ FILE: camera_model/src/intrinsic_calib.cc ================================================ #include #include #include #include #include #include #include #include #include #include "camodocal/chessboard/Chessboard.h" #include "camodocal/calib/CameraCalibration.h" #include "camodocal/gpl/gpl.h" int main(int argc, char** argv) { cv::Size boardSize; float squareSize; std::string inputDir; std::string cameraModel; std::string cameraName; std::string prefix; std::string fileExtension; bool useOpenCV; bool viewResults; bool verbose; //========= Handling Program options ========= boost::program_options::options_description desc("Allowed options"); desc.add_options() ("help", "produce help message") ("width,w", boost::program_options::value(&boardSize.width)->default_value(8), "Number of inner corners on the chessboard pattern in x direction") ("height,h", boost::program_options::value(&boardSize.height)->default_value(12), "Number of inner corners on the chessboard pattern in y direction") ("size,s", boost::program_options::value(&squareSize)->default_value(7.f), "Size of one square in mm") ("input,i", boost::program_options::value(&inputDir)->default_value("calibrationdata"), "Input directory containing chessboard images") ("prefix,p", boost::program_options::value(&prefix)->default_value("left-"), "Prefix of images") ("file-extension,e", boost::program_options::value(&fileExtension)->default_value(".png"), "File extension of images") ("camera-model", boost::program_options::value(&cameraModel)->default_value("mei"), "Camera model: kannala-brandt | mei | pinhole") ("camera-name", boost::program_options::value(&cameraName)->default_value("camera"), "Name of camera") ("opencv", boost::program_options::bool_switch(&useOpenCV)->default_value(true), "Use OpenCV to detect corners") ("view-results", boost::program_options::bool_switch(&viewResults)->default_value(false), "View results") ("verbose,v", boost::program_options::bool_switch(&verbose)->default_value(true), "Verbose output") ; boost::program_options::positional_options_description pdesc; pdesc.add("input", 1); boost::program_options::variables_map vm; boost::program_options::store(boost::program_options::command_line_parser(argc, argv).options(desc).positional(pdesc).run(), vm); boost::program_options::notify(vm); if (vm.count("help")) { std::cout << desc << std::endl; return 1; } if (!boost::filesystem::exists(inputDir) && !boost::filesystem::is_directory(inputDir)) { std::cerr << "# ERROR: Cannot find input directory " << inputDir << "." << std::endl; return 1; } camodocal::Camera::ModelType modelType; if (boost::iequals(cameraModel, "kannala-brandt")) { modelType = camodocal::Camera::KANNALA_BRANDT; } else if (boost::iequals(cameraModel, "mei")) { modelType = camodocal::Camera::MEI; } else if (boost::iequals(cameraModel, "pinhole")) { modelType = camodocal::Camera::PINHOLE; } else if (boost::iequals(cameraModel, "scaramuzza")) { modelType = camodocal::Camera::SCARAMUZZA; } else { std::cerr << "# ERROR: Unknown camera model: " << cameraModel << std::endl; return 1; } switch (modelType) { case camodocal::Camera::KANNALA_BRANDT: std::cout << "# INFO: Camera model: Kannala-Brandt" << std::endl; break; case camodocal::Camera::MEI: std::cout << "# INFO: Camera model: Mei" << std::endl; break; case camodocal::Camera::PINHOLE: std::cout << "# INFO: Camera model: Pinhole" << std::endl; break; case camodocal::Camera::SCARAMUZZA: std::cout << "# INFO: Camera model: Scaramuzza-Omnidirect" << std::endl; break; } // look for images in input directory std::vector imageFilenames; boost::filesystem::directory_iterator itr; for (boost::filesystem::directory_iterator itr(inputDir); itr != boost::filesystem::directory_iterator(); ++itr) { if (!boost::filesystem::is_regular_file(itr->status())) { continue; } std::string filename = itr->path().filename().string(); // check if prefix matches if (!prefix.empty()) { if (filename.compare(0, prefix.length(), prefix) != 0) { continue; } } // check if file extension matches if (filename.compare(filename.length() - fileExtension.length(), fileExtension.length(), fileExtension) != 0) { continue; } imageFilenames.push_back(itr->path().string()); if (verbose) { std::cerr << "# INFO: Adding " << imageFilenames.back() << std::endl; } } if (imageFilenames.empty()) { std::cerr << "# ERROR: No chessboard images found." << std::endl; return 1; } if (verbose) { std::cerr << "# INFO: # images: " << imageFilenames.size() << std::endl; } std::sort(imageFilenames.begin(), imageFilenames.end()); cv::Mat image = cv::imread(imageFilenames.front(), -1); const cv::Size frameSize = image.size(); camodocal::CameraCalibration calibration(modelType, cameraName, frameSize, boardSize, squareSize); calibration.setVerbose(verbose); std::vector chessboardFound(imageFilenames.size(), false); for (size_t i = 0; i < imageFilenames.size(); ++i) { image = cv::imread(imageFilenames.at(i), -1); camodocal::Chessboard chessboard(boardSize, image); chessboard.findCorners(useOpenCV); if (chessboard.cornersFound()) { if (verbose) { std::cerr << "# INFO: Detected chessboard in image " << i + 1 << ", " << imageFilenames.at(i) << std::endl; } calibration.addChessboardData(chessboard.getCorners()); cv::Mat sketch; chessboard.getSketch().copyTo(sketch); cv::imshow("Image", sketch); cv::waitKey(50); } else if (verbose) { std::cerr << "# INFO: Did not detect chessboard in image " << i + 1 << std::endl; } chessboardFound.at(i) = chessboard.cornersFound(); } cv::destroyWindow("Image"); if (calibration.sampleCount() < 10) { std::cerr << "# ERROR: Insufficient number of detected chessboards." << std::endl; return 1; } if (verbose) { std::cerr << "# INFO: Calibrating..." << std::endl; } double startTime = camodocal::timeInSeconds(); calibration.calibrate(); calibration.writeParams(cameraName + "_camera_calib.yaml"); calibration.writeChessboardData(cameraName + "_chessboard_data.dat"); if (verbose) { std::cout << "# INFO: Calibration took a total time of " << std::fixed << std::setprecision(3) << camodocal::timeInSeconds() - startTime << " sec.\n"; } if (verbose) { std::cerr << "# INFO: Wrote calibration file to " << cameraName + "_camera_calib.yaml" << std::endl; } if (viewResults) { std::vector cbImages; std::vector cbImageFilenames; for (size_t i = 0; i < imageFilenames.size(); ++i) { if (!chessboardFound.at(i)) { continue; } cbImages.push_back(cv::imread(imageFilenames.at(i), -1)); cbImageFilenames.push_back(imageFilenames.at(i)); } // visualize observed and reprojected points calibration.drawResults(cbImages); for (size_t i = 0; i < cbImages.size(); ++i) { cv::putText(cbImages.at(i), cbImageFilenames.at(i), cv::Point(10,20), cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(255, 255, 255), 1, cv::LINE_AA); cv::imshow("Image", cbImages.at(i)); cv::waitKey(0); } } return 0; } ================================================ FILE: camera_model/src/sparse_graph/Transform.cc ================================================ #include namespace camodocal { Transform::Transform() { m_q.setIdentity(); m_t.setZero(); } Transform::Transform(const Eigen::Matrix4d& H) { m_q = Eigen::Quaterniond(H.block<3,3>(0,0)); m_t = H.block<3,1>(0,3); } Eigen::Quaterniond& Transform::rotation(void) { return m_q; } const Eigen::Quaterniond& Transform::rotation(void) const { return m_q; } double* Transform::rotationData(void) { return m_q.coeffs().data(); } const double* const Transform::rotationData(void) const { return m_q.coeffs().data(); } Eigen::Vector3d& Transform::translation(void) { return m_t; } const Eigen::Vector3d& Transform::translation(void) const { return m_t; } double* Transform::translationData(void) { return m_t.data(); } const double* const Transform::translationData(void) const { return m_t.data(); } Eigen::Matrix4d Transform::toMatrix(void) const { Eigen::Matrix4d H; H.setIdentity(); H.block<3,3>(0,0) = m_q.toRotationMatrix(); H.block<3,1>(0,3) = m_t; return H; } } ================================================ FILE: config/campus.rviz ================================================ Panels: - Class: rviz/Displays Help Height: 0 Name: Displays Property Tree Widget: Expanded: - /Global Options1 - /Status1 - /raw_image1 - /VIO1 - /VIO1/Path1 - /pose_graph1/pose_graph_path1 - /Odometry1 - /Odometry2 - /Odometry2/Shape1 Splitter Ratio: 0.4651159942150116 Tree Height: 441 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties Expanded: - /2D Pose Estimate1 - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.5886790156364441 - Class: rviz/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - Class: rviz/Time Experimental: false Name: Time SyncMode: 0 SyncSource: raw_image - Class: rviz/Displays Help Height: 78 Name: Displays Property Tree Widget: Expanded: - /tracked image1 - /raw_image1 - /VIO1 - /VIO1/Path1 - /pose_graph1 - /pose_graph1/pose_graph_path1 - /pose_graph1/loop_match_image1 - /pose_graph1/loop_match_image1/Status1 - /Image1 Splitter Ratio: 0.5 Tree Height: 741 Preferences: PromptSaveOnExit: true Toolbars: toolButtonStyle: 2 Visualization Manager: Class: "" Displays: - Alpha: 0.5 Cell Size: 1 Class: rviz/Grid Color: 130; 130; 130 Enabled: true Line Style: Line Width: 0.029999999329447746 Value: Lines Name: Grid Normal Cell Count: 0 Offset: X: 0 Y: 0 Z: 0 Plane: XY Plane Cell Count: 10 Reference Frame: Value: true - Class: rviz/Axes Enabled: false Length: 1 Name: Axes Radius: 0.10000000149011612 Reference Frame: Value: false - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 255; 0; 0 Enabled: true Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Lines Line Width: 0.5 Name: ground_truth_path Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /benchmark_publisher/path Unreliable: false Value: true - Class: rviz/Image Enabled: true Image Topic: /vins_estimator/feature_img Max Value: 1 Median window: 5 Min Value: 0 Name: tracked image Normalize Range: true Queue Size: 2 Transport Hint: raw Unreliable: false Value: true - Class: rviz/Image Enabled: false Image Topic: /camera/color/image_raw Max Value: 1 Median window: 5 Min Value: 0 Name: raw_image Normalize Range: true Queue Size: 2 Transport Hint: raw Unreliable: false Value: false - Class: rviz/Group Displays: - Alpha: 10 Buffer Length: 1 Class: rviz/Path Color: 25; 255; 0 Enabled: true Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Billboards Line Width: 1 Name: Path Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /vins_estimator/path Unreliable: false Value: true - Class: rviz/MarkerArray Enabled: true Marker Topic: /vins_estimator/camera_pose_visual Name: camera_visual Namespaces: {} Queue Size: 100 Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 10 Min Value: -10 Value: true Axis: Z Channel Name: intensity Class: rviz/PointCloud Color: 0; 255; 0 Color Transformer: FlatColor Decay Time: 1 Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 Min Color: 0; 0; 0 Name: current_point Position Transformer: XYZ Queue Size: 1 Selectable: true Size (Pixels): 1 Size (m): 0.009999999776482582 Style: Points Topic: /vins_estimator/point_cloud Unreliable: false Use Fixed Frame: true Use rainbow: true Value: false - Alpha: 1 Autocompute Intensity Bounds: false Autocompute Value Bounds: Max Value: 10 Min Value: -10 Value: true Axis: Z Channel Name: intensity Class: rviz/PointCloud Color: 255; 255; 255 Color Transformer: FlatColor Decay Time: 3000 Enabled: false Invert Rainbow: true Max Color: 0; 0; 0 Max Intensity: 4096 Min Color: 0; 0; 0 Min Intensity: 0 Name: history_point Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 1 Size (m): 0.5 Style: Points Topic: /vins_estimator/history_cloud Unreliable: false Use Fixed Frame: true Use rainbow: false Value: false - Class: rviz/TF Enabled: false Frame Timeout: 15 Frames: All Enabled: true Marker Scale: 1 Name: TF Show Arrows: true Show Axes: true Show Names: true Tree: {} Update Interval: 0 Value: false Enabled: true Name: VIO - Class: rviz/Group Displays: - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 252; 233; 79 Enabled: true Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Billboards Line Width: 1 Name: pose_graph_path Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /pose_graph/pose_graph_path Unreliable: false Value: true - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 255; 170; 0 Enabled: false Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Lines Line Width: 0.029999999329447746 Name: base_path Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /pose_graph/base_path Unreliable: false Value: false - Class: rviz/MarkerArray Enabled: false Marker Topic: /pose_graph/pose_graph Name: loop_visual Namespaces: {} Queue Size: 100 Value: false - Class: rviz/MarkerArray Enabled: true Marker Topic: /pose_graph/camera_pose_visual Name: camera_visual Namespaces: {} Queue Size: 100 Value: true - Class: rviz/Image Enabled: false Image Topic: /match_image Max Value: 1 Median window: 5 Min Value: 0 Name: loop_match_image Normalize Range: true Queue Size: 2 Transport Hint: raw Unreliable: false Value: false - Class: rviz/Marker Enabled: false Marker Topic: /pose_graph/key_odometrys Name: Marker Namespaces: {} Queue Size: 100 Value: false - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 25; 255; 0 Enabled: false Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Lines Line Width: 0.30000001192092896 Name: Sequence1 Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /pose_graph/path_1 Unreliable: false Value: false - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 255; 0; 0 Enabled: false Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Lines Line Width: 0.30000001192092896 Name: Sequence2 Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /pose_graph/path_2 Unreliable: false Value: false - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 0; 85; 255 Enabled: false Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Lines Line Width: 0.30000001192092896 Name: Sequence3 Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /pose_graph/path_3 Unreliable: false Value: false - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 255; 170; 0 Enabled: false Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Lines Line Width: 0.30000001192092896 Name: Sequence4 Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /pose_graph/path_4 Unreliable: false Value: false - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 255; 170; 255 Enabled: false Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Lines Line Width: 0.029999999329447746 Name: Sequence5 Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /pose_graph/path_5 Unreliable: false Value: false - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 25; 255; 0 Enabled: false Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Lines Line Width: 0.029999999329447746 Name: no_loop_path Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /pose_graph/no_loop_path Unreliable: false Value: false Enabled: true Name: pose_graph - Angle Tolerance: 0.10000000149011612 Class: rviz/Odometry Covariance: Orientation: Alpha: 0.5 Color: 255; 255; 127 Color Style: Unique Frame: Local Offset: 1 Scale: 1 Value: true Position: Alpha: 0.30000001192092896 Color: 204; 51; 204 Scale: 1 Value: true Value: true Enabled: true Keep: 1 Name: Odometry Position Tolerance: 0.10000000149011612 Shape: Alpha: 1 Axes Length: 1 Axes Radius: 0.10000000149011612 Color: 255; 25; 0 Head Length: 0.30000001192092896 Head Radius: 0.10000000149011612 Shaft Length: 1 Shaft Radius: 0.05000000074505806 Value: Axes Topic: /vins_estimator/imu_propagate Unreliable: false Value: true - Angle Tolerance: 0.10000000149011612 Class: rviz/Odometry Covariance: Orientation: Alpha: 0.5 Color: 255; 255; 127 Color Style: Unique Frame: Local Offset: 1 Scale: 1 Value: true Position: Alpha: 0.30000001192092896 Color: 204; 51; 204 Scale: 1 Value: true Value: true Enabled: true Keep: 10000 Name: Odometry Position Tolerance: 0.10000000149011612 Shape: Alpha: 1 Axes Length: 1 Axes Radius: 0.10000000149011612 Color: 255; 25; 0 Head Length: 0.029999999329447746 Head Radius: 0.10000000149011612 Shaft Length: 0.10000000149011612 Shaft Radius: 0.05000000074505806 Value: Arrow Topic: /odom Unreliable: false Value: true - Class: rviz/Marker Enabled: true Marker Topic: /image_to_rviz/visualization_marker Name: Marker Namespaces: image: true Queue Size: 100 Value: true - Class: rviz/Image Enabled: true Image Topic: /vins_estimator/semantic_mask Max Value: 1 Median window: 5 Min Value: 0 Name: Image Normalize Range: true Queue Size: 2 Transport Hint: raw Unreliable: false Value: true Enabled: true Global Options: Background Color: 0; 0; 0 Default Light: true Fixed Frame: map Frame Rate: 30 Name: root Tools: - Class: rviz/MoveCamera - Class: rviz/Select - Class: rviz/FocusCamera - Class: rviz/Measure - Class: rviz/SetInitialPose Theta std deviation: 0.2617993950843811 Topic: /initialpose X std deviation: 0.5 Y std deviation: 0.5 - Class: rviz/SetGoal Topic: /move_base_simple/goal - Class: rviz/PublishPoint Single click: true Topic: /clicked_point Value: true Views: Current: Angle: -2.065000295639038 Class: rviz/TopDownOrtho Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 Scale: 2.8180553913116455 Target Frame: Value: TopDownOrtho (rviz) X: 106.81703186035156 Y: 118.53111267089844 Saved: - Angle: -1.9099998474121094 Class: rviz/TopDownOrtho Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Invert Z Axis: false Name: TopDownOrtho Near Clip Distance: 0.009999999776482582 Scale: 2.8129937648773193 Target Frame: Value: TopDownOrtho (rviz) X: 90.85389709472656 Y: 126.16999053955078 Window Geometry: Displays: collapsed: false Height: 960 Hide Left Dock: false Hide Right Dock: true Image: collapsed: false QMainWindow State: 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 Selection: collapsed: false Time: collapsed: false Tool Properties: collapsed: false Views: collapsed: true Width: 1920 X: 0 Y: 27 loop_match_image: collapsed: false raw_image: collapsed: false tracked image: collapsed: false ================================================ FILE: config/indoor.rviz ================================================ Panels: - Class: rviz/Displays Help Height: 0 Name: Displays Property Tree Widget: Expanded: - /Global Options1 - /Status1 - /raw_image1 - /VIO1 - /VIO1/Path1 - /pose_graph1/pose_graph_path1 Splitter Ratio: 0.4651159942150116 Tree Height: 441 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties Expanded: - /2D Pose Estimate1 - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.5886790156364441 - Class: rviz/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - Class: rviz/Time Experimental: false Name: Time SyncMode: 0 SyncSource: raw_image - Class: rviz/Displays Help Height: 78 Name: Displays Property Tree Widget: Expanded: - /tracked image1 - /raw_image1 - /VIO1 - /VIO1/Path1 - /pose_graph1 - /pose_graph1/pose_graph_path1 - /Odometry1 Splitter Ratio: 0.5 Tree Height: 731 Preferences: PromptSaveOnExit: true Toolbars: toolButtonStyle: 2 Visualization Manager: Class: "" Displays: - Alpha: 0.5 Cell Size: 1 Class: rviz/Grid Color: 130; 130; 130 Enabled: true Line Style: Line Width: 0.029999999329447746 Value: Lines Name: Grid Normal Cell Count: 0 Offset: X: 0 Y: 0 Z: 0 Plane: XY Plane Cell Count: 10 Reference Frame: Value: true - Class: rviz/Axes Enabled: false Length: 1 Name: Axes Radius: 0.10000000149011612 Reference Frame: Value: false - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 255; 0; 0 Enabled: true Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Lines Line Width: 0.5 Name: ground_truth_path Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /benchmark_publisher/path Unreliable: false Value: true - Class: rviz/Image Enabled: true Image Topic: /vins_estimator/feature_img Max Value: 1 Median window: 5 Min Value: 0 Name: tracked image Normalize Range: true Queue Size: 2 Transport Hint: raw Unreliable: false Value: true - Class: rviz/Image Enabled: true Image Topic: /camera/color/image_raw Max Value: 1 Median window: 5 Min Value: 0 Name: raw_image Normalize Range: true Queue Size: 2 Transport Hint: raw Unreliable: false Value: true - Class: rviz/Group Displays: - Alpha: 10 Buffer Length: 1 Class: rviz/Path Color: 25; 255; 0 Enabled: true Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Lines Line Width: 1 Name: Path Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /vins_estimator/path Unreliable: false Value: true - Class: rviz/MarkerArray Enabled: true Marker Topic: /vins_estimator/camera_pose_visual Name: camera_visual Namespaces: CameraPoseVisualization: true Queue Size: 100 Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 10 Min Value: -10 Value: true Axis: Z Channel Name: intensity Class: rviz/PointCloud Color: 0; 255; 0 Color Transformer: FlatColor Decay Time: 1 Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 Min Color: 0; 0; 0 Name: current_point Position Transformer: XYZ Queue Size: 1 Selectable: true Size (Pixels): 1 Size (m): 0.009999999776482582 Style: Points Topic: /vins_estimator/point_cloud Unreliable: false Use Fixed Frame: true Use rainbow: true Value: false - Alpha: 1 Autocompute Intensity Bounds: false Autocompute Value Bounds: Max Value: 10 Min Value: -10 Value: true Axis: Z Channel Name: intensity Class: rviz/PointCloud Color: 255; 255; 255 Color Transformer: FlatColor Decay Time: 3000 Enabled: false Invert Rainbow: true Max Color: 0; 0; 0 Max Intensity: 4096 Min Color: 0; 0; 0 Min Intensity: 0 Name: history_point Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 1 Size (m): 0.5 Style: Points Topic: /vins_estimator/history_cloud Unreliable: false Use Fixed Frame: true Use rainbow: false Value: false - Class: rviz/TF Enabled: false Frame Timeout: 15 Frames: All Enabled: true Marker Scale: 1 Name: TF Show Arrows: true Show Axes: true Show Names: true Tree: {} Update Interval: 0 Value: false Enabled: true Name: VIO - Class: rviz/Group Displays: - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 239; 41; 41 Enabled: true Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Lines Line Width: 1 Name: pose_graph_path Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /pose_graph/pose_graph_path Unreliable: false Value: true - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 255; 170; 0 Enabled: false Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Lines Line Width: 0.029999999329447746 Name: base_path Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /pose_graph/base_path Unreliable: false Value: false - Class: rviz/MarkerArray Enabled: false Marker Topic: /pose_graph/pose_graph Name: loop_visual Namespaces: {} Queue Size: 100 Value: false - Class: rviz/MarkerArray Enabled: true Marker Topic: /pose_graph/camera_pose_visual Name: camera_visual Namespaces: {} Queue Size: 100 Value: true - Class: rviz/Image Enabled: true Image Topic: /pose_graph/match_image Max Value: 1 Median window: 5 Min Value: 0 Name: loop_match_image Normalize Range: true Queue Size: 2 Transport Hint: raw Unreliable: false Value: true - Class: rviz/Marker Enabled: false Marker Topic: /pose_graph/key_odometrys Name: Marker Namespaces: {} Queue Size: 100 Value: false - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 25; 255; 0 Enabled: false Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Lines Line Width: 0.30000001192092896 Name: Sequence1 Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /pose_graph/path_1 Unreliable: false Value: false - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 255; 0; 0 Enabled: false Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Lines Line Width: 0.30000001192092896 Name: Sequence2 Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /pose_graph/path_2 Unreliable: false Value: false - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 0; 85; 255 Enabled: false Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Lines Line Width: 0.30000001192092896 Name: Sequence3 Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /pose_graph/path_3 Unreliable: false Value: false - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 255; 170; 0 Enabled: false Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Lines Line Width: 0.30000001192092896 Name: Sequence4 Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /pose_graph/path_4 Unreliable: false Value: false - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 255; 170; 255 Enabled: false Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Lines Line Width: 0.029999999329447746 Name: Sequence5 Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /pose_graph/path_5 Unreliable: false Value: false - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 25; 255; 0 Enabled: false Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Lines Line Width: 0.029999999329447746 Name: no_loop_path Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /pose_graph/no_loop_path Unreliable: false Value: false Enabled: true Name: pose_graph - Alpha: 1 Axes Length: 1 Axes Radius: 0.10000000149011612 Class: rviz/Pose Color: 255; 25; 0 Enabled: false Head Length: 0.30000001192092896 Head Radius: 0.10000000149011612 Name: Pose Shaft Length: 1 Shaft Radius: 0.05000000074505806 Shape: Arrow Topic: /mavros/vision_pose/remap_pose Unreliable: false Value: false - Angle Tolerance: 0.10000000149011612 Class: rviz/Odometry Covariance: Orientation: Alpha: 0.5 Color: 255; 255; 127 Color Style: Unique Frame: Local Offset: 1 Scale: 1 Value: true Position: Alpha: 0.30000001192092896 Color: 204; 51; 204 Scale: 1 Value: true Value: true Enabled: true Keep: 1 Name: Odometry Position Tolerance: 0.10000000149011612 Shape: Alpha: 1 Axes Length: 1 Axes Radius: 0.10000000149011612 Color: 255; 25; 0 Head Length: 0.30000001192092896 Head Radius: 0.10000000149011612 Shaft Length: 1 Shaft Radius: 0.05000000074505806 Value: Axes Topic: /vins_estimator/imu_propagate Unreliable: false Value: true Enabled: true Global Options: Background Color: 0; 0; 0 Default Light: true Fixed Frame: map Frame Rate: 30 Name: root Tools: - Class: rviz/MoveCamera - Class: rviz/Select - Class: rviz/FocusCamera - Class: rviz/Measure - Class: rviz/SetInitialPose Theta std deviation: 0.2617993950843811 Topic: /initialpose X std deviation: 0.5 Y std deviation: 0.5 - Class: rviz/SetGoal Topic: /move_base_simple/goal - Class: rviz/PublishPoint Single click: true Topic: /clicked_point Value: true Views: Current: Class: rviz/XYOrbit Distance: 9.714073181152344 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: X: -0.19246625900268555 Y: 0.2890198230743408 Z: 0 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 Pitch: 0.4153980016708374 Target Frame: Value: XYOrbit (rviz) Yaw: 3.9204025268554688 Saved: ~ Window Geometry: Displays: collapsed: false Height: 960 Hide Left Dock: false Hide Right Dock: true QMainWindow State: 000000ff00000000fd00000004000000000000031100000366fc0200000014fb0000000a0049006d006100670065000000003d0000021d0000000000000000fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001200720061007700200049006d0061006700650000000028000000300000000000000000fb00000012007200610077005f0069006d006100670065010000003d000001c60000000000000000fb0000000a0049006d006100670065010000003d000000c50000000000000000fb0000000a0049006d006100670065000000003d000001ba0000000000000000fb0000001a0074007200610063006b0065006400200069006d00610067006500000001f3000001b00000000000000000fb000000100044006900730070006c0061007900730000000226000000c9000000c900fffffffc0000003d0000010d0000001600fffffffa000000010100000002fb0000001200720061007700200049006d0061006700650000000000ffffffff0000000000000000fb0000001a0074007200610063006b0065006400200069006d0061006700650100000000000002370000009500fffffffb00000020006c006f006f0070005f006d0061007400630068005f0069006d00610067006500000001aa000001b80000000000000000fb0000001000410052005f0069006d0061006700650100000373000000160000000000000000fb0000001200720061007700200069006d006100670065010000038f000000160000000000000000fb00000012007200610077005f0069006d0061006700650100000150000001260000001600fffffffb00000020006c006f006f0070005f006d0061007400630068005f0069006d006100670065010000027c000001270000001600ffffff000000010000025100000366fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fc0000003d000003660000000000fffffffaffffffff0100000002fb0000000a00560069006500770073000000023f0000016a0000010000fffffffb000000100044006900730070006c0061007900730000000000ffffffff0000015600fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000064000000039fc0100000002fb0000000800540069006d0065000000000000000640000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004690000036600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: collapsed: false Tool Properties: collapsed: false Views: collapsed: false Width: 1920 X: 0 Y: 27 loop_match_image: collapsed: false raw_image: collapsed: false tracked image: collapsed: false ================================================ FILE: config/openloris/openloris_vio.yaml ================================================ %YAML:1.0 num_threads: 0 # 0 Use the max number of threads of your device. # For some devices, like HUAWEI Atlas200, the auto detected max number of threads might not equivalent to the usable numble of threads. (Some cores(threads) might be reserved for other usage(NPU) by system.) # x It is proper that 1 < x < MAX_THREAD. # For now, this parameter is relevant to grid-detector to run in parallel. #common parameters imu: 1 static_init: 0 imu_topic: "/d400/imu0" image_topic: "/d400/color/image_raw" depth_topic: "/d400/aligned_depth_to_color/image_raw" output_path: "/home/nrosliu/DynamicVINS-Atlas/src/Dynamic_VINS/output" #RGBD camera Ideal Range depth_min_dist: 0.3 depth_max_dist: 3 frontend_freq: 30 # It should be raised in VO mode(without IMU). keyframe_parallax: 10.0 # keyframe selection threshold (pixel); if system fails frequently, please try to reduce the "keyframe_parallax" num_grid_rows: 7 num_grid_cols: 8 #unsynchronization parameters estimate_td: 1 ########## # online estimate time offset between camera and imu td: 0.000 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock) #camera calibration model_type: PINHOLE camera_name: camera image_width: 848 image_height: 480 #TODO modify distortion distortion_parameters: k1: 0.0 k2: 0.0 p1: 0.0 p2: 0.0 projection_parameters: fx: 611.4509887695312 fy: 611.4857177734375 cx: 433.2039794921875 cy: 249.4730224609375 # Extrinsic parameter between IMU and Camera. estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it. # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess. # 2 Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning. #If you choose 0 or 1, you should write down the following matrix. #Rotation from camera frame to imu frame, imu^R_cam extrinsicRotation: !!opencv-matrix rows: 3 cols: 3 dt: d data: [ 0.999975572679493, 0.003849141066713, 0.005854714944742, -0.003828680351062, 0.999986658473099, -0.003501944262433, -0.005868115609379, 0.003479442469180, 0.999976848846595] #Translation from camera frame to imu frame, imu^T_cam extrinsicTranslation: !!opencv-matrix rows: 3 cols: 1 dt: d data: [0.0203127935529, -0.00510325236246, -0.0112013882026] #feature traker paprameters max_cnt: 130 # max feature number in feature tracking. It is suggested to be raised in VO mode. min_dist: 30 # min distance between two features 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 F_threshold: 1.0 # ransac threshold (pixel) show_track: 0 # publish tracking image as topic equalize: 0 # if image is too dark or light, trun on equalize to find enough features fisheye: 0 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points #optimization parameters max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time max_num_iterations: 8 # max solver itrations, to guarantee real time #imu parameters The more accurate parameters you provide, the better performance acc_n: 0.1 # accelerometer measurement noise standard deviation. gyr_n: 0.01 # gyroscope measurement noise standard deviation. acc_w: 0.0002 # accelerometer bias random work noise standard deviation. #0.02 gyr_w: 2.0e-5 # gyroscope bias random work noise standard deviation. #4.0e-5 g_norm: 9.805 # gravity magnitude #rolling shutter parameters rolling_shutter: 1 # 0: global shutter camera, 1: rolling shutter camera rolling_shutter_tr: 0.033 # unit: s. rolling shutter read out time per frame (from data sheet) #loop closure parameters loop_closure: 0 # start loop closure fast_relocalization: 0 # useful in real-time and large project load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path' pose_graph_save_path: "/home/nrosliu/DynamicVINS-Atlas/src/Dynamic_VINS/output/pose_graph" # save and load path #visualization parameters save_image: 0 # save image in pose graph for visualization prupose; you can close this function by setting 0 visualize_imu_forward: 0 # output imu forward propogation to achieve low latency and high frequence results visualize_camera_size: 0.4 # size of camera marker in RVIZ #暂时仅考虑动物 dynamic_label: ["person", "cat", "dog", "bicycle", "car","bus"] ================================================ FILE: config/openloris/openloris_vio_atlas.yaml ================================================ %YAML:1.0 num_threads: 6 # 0 Use the max number of threads of your device. # For some devices, like HUAWEI Atlas200, the auto detected max number of threads might not equivalent to the usable numble of threads. (Some cores(threads) might be reserved for other usage(NPU) by system.) # x It is proper that 1 < x < MAX_THREAD. # For now, this parameter is relevant to grid-detector to run in parallel. #common parameters imu: 1 static_init: 1 imu_topic: "/d400/imu0" image_topic: "/decompressed_img" depth_topic: "/d400/aligned_depth_to_color/image_raw" # check your depth image bandwidth is lower than 30Mbs, otherwise compressedDepth is suggested output_path: "/home/nrosliu/DynamicVINS-Atlas/src/Dynamic_VINS/output" # 0.3 Value: true - Alpha: 1 Class: rviz/Axes Enabled: true Length: 1 Name: Axes Radius: 0.10000000149011612 Reference Frame: Show Trail: false Value: true - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 255; 0; 0 Enabled: true Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Lines Line Width: 0.5 Name: ground_truth_path Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Queue Size: 10 Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /benchmark_publisher/path Unreliable: false Value: true - Class: rviz/Image Enabled: false Image Topic: /vins_estimator/feature_img Max Value: 1 Median window: 5 Min Value: 0 Name: tracked image Normalize Range: true Queue Size: 2 Transport Hint: raw Unreliable: false Value: false - Class: rviz/Image Enabled: false Image Topic: /camera/color/image_raw Max Value: 1 Median window: 5 Min Value: 0 Name: raw_image Normalize Range: true Queue Size: 2 Transport Hint: raw Unreliable: false Value: false - Class: rviz/Group Displays: - Alpha: 10 Buffer Length: 1 Class: rviz/Path Color: 255; 25; 0 Enabled: true Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Billboards Line Width: 0.10000000149011612 Name: Path Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Queue Size: 10 Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /vins_estimator/path Unreliable: false Value: true - Class: rviz/MarkerArray Enabled: true Marker Topic: /vins_estimator/camera_pose_visual Name: camera_visual Namespaces: {} Queue Size: 100 Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 10 Min Value: -10 Value: true Axis: Z Channel Name: intensity Class: rviz/PointCloud Color: 0; 255; 0 Color Transformer: FlatColor Decay Time: 10000 Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 Min Color: 0; 0; 0 Name: current_point Position Transformer: XYZ Queue Size: 1 Selectable: true Size (Pixels): 1 Size (m): 0.009999999776482582 Style: Points Topic: /vins_estimator/point_cloud Unreliable: false Use Fixed Frame: true Use rainbow: true Value: false - Alpha: 1 Autocompute Intensity Bounds: false Autocompute Value Bounds: Max Value: 10 Min Value: -10 Value: true Axis: Z Channel Name: intensity Class: rviz/PointCloud Color: 255; 255; 255 Color Transformer: FlatColor Decay Time: 3000 Enabled: false Invert Rainbow: true Max Color: 0; 0; 0 Max Intensity: 4096 Min Color: 0; 0; 0 Min Intensity: 0 Name: history_point Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 1 Size (m): 0.5 Style: Points Topic: /vins_estimator/history_cloud Unreliable: false Use Fixed Frame: true Use rainbow: false Value: false - Class: rviz/TF Enabled: false Frame Timeout: 15 Frames: All Enabled: true Marker Alpha: 1 Marker Scale: 1 Name: TF Show Arrows: true Show Axes: true Show Names: true Tree: {} Update Interval: 0 Value: false Enabled: true Name: VIO - Class: rviz/Group Displays: - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 252; 233; 79 Enabled: true Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Billboards Line Width: 1 Name: pose_graph_path Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Queue Size: 10 Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /pose_graph/pose_graph_path Unreliable: false Value: true - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 255; 170; 0 Enabled: false Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Lines Line Width: 0.029999999329447746 Name: base_path Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Queue Size: 10 Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /pose_graph/base_path Unreliable: false Value: false - Class: rviz/MarkerArray Enabled: false Marker Topic: /pose_graph/pose_graph Name: loop_visual Namespaces: {} Queue Size: 100 Value: false - Class: rviz/MarkerArray Enabled: true Marker Topic: /pose_graph/camera_pose_visual Name: camera_visual Namespaces: {} Queue Size: 100 Value: true - Class: rviz/Image Enabled: false Image Topic: /pose_graph/match_image Max Value: 1 Median window: 5 Min Value: 0 Name: loop_match_image Normalize Range: true Queue Size: 2 Transport Hint: raw Unreliable: false Value: false - Class: rviz/Marker Enabled: false Marker Topic: /pose_graph/key_odometrys Name: Marker Namespaces: {} Queue Size: 100 Value: false - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 25; 255; 0 Enabled: false Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Lines Line Width: 0.30000001192092896 Name: Sequence1 Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Queue Size: 10 Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /pose_graph/path_1 Unreliable: false Value: false - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 255; 0; 0 Enabled: false Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Lines Line Width: 0.30000001192092896 Name: Sequence2 Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Queue Size: 10 Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /pose_graph/path_2 Unreliable: false Value: false - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 0; 85; 255 Enabled: false Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Lines Line Width: 0.30000001192092896 Name: Sequence3 Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Queue Size: 10 Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /pose_graph/path_3 Unreliable: false Value: false - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 255; 170; 0 Enabled: false Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Lines Line Width: 0.30000001192092896 Name: Sequence4 Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Queue Size: 10 Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /pose_graph/path_4 Unreliable: false Value: false - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 255; 170; 255 Enabled: false Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Lines Line Width: 0.029999999329447746 Name: Sequence5 Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Queue Size: 10 Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /pose_graph/path_5 Unreliable: false Value: false - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 25; 255; 0 Enabled: false Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Lines Line Width: 0.029999999329447746 Name: no_loop_path Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Queue Size: 10 Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /pose_graph/no_loop_path Unreliable: false Value: false Enabled: true Name: pose_graph - Angle Tolerance: 0.10000000149011612 Class: rviz/Odometry Covariance: Orientation: Alpha: 0.5 Color: 255; 255; 127 Color Style: Unique Frame: Local Offset: 1 Scale: 1 Value: true Position: Alpha: 0.30000001192092896 Color: 204; 51; 204 Scale: 1 Value: true Value: true Enabled: true Keep: 1 Name: Odometry Position Tolerance: 0.10000000149011612 Queue Size: 10 Shape: Alpha: 1 Axes Length: 1 Axes Radius: 0.10000000149011612 Color: 239; 41; 41 Head Length: 0.30000001192092896 Head Radius: 0.10000000149011612 Shaft Length: 1 Shaft Radius: 0.05000000074505806 Value: Arrow Topic: /vins_estimator/odometry Unreliable: false Value: true - Angle Tolerance: 0.10000000149011612 Class: rviz/Odometry Covariance: Orientation: Alpha: 0.5 Color: 255; 255; 127 Color Style: Unique Frame: Local Offset: 1 Scale: 1 Value: true Position: Alpha: 0.30000001192092896 Color: 204; 51; 204 Scale: 1 Value: true Value: true Enabled: false Keep: 1 Name: Odometry Position Tolerance: 0.10000000149011612 Queue Size: 10 Shape: Alpha: 1 Axes Length: 1 Axes Radius: 0.10000000149011612 Color: 114; 159; 207 Head Length: 0.029999999329447746 Head Radius: 0.10000000149011612 Shaft Length: 0.10000000149011612 Shaft Radius: 0.05000000074505806 Value: Arrow Topic: /vins_estimator/odometry Unreliable: false Value: false - Class: rviz/Image Enabled: false Image Topic: /vins_estimator/semantic_mask Max Value: 1 Median window: 5 Min Value: 0 Name: Image Normalize Range: true Queue Size: 2 Transport Hint: raw Unreliable: false Value: false - Class: rviz/Marker Enabled: true Marker Topic: /image_to_rviz/visualization_marker Name: Marker Namespaces: {} Queue Size: 100 Value: true - Class: rviz/Image Enabled: false Image Topic: /vins_estimator/semantic_mask Max Value: 1 Median window: 5 Min Value: 0 Name: Image Normalize Range: true Queue Size: 2 Transport Hint: raw Unreliable: false Value: false - Alpha: 10 Buffer Length: 1 Class: rviz/Path Color: 239; 41; 41 Enabled: true Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Billboards Line Width: 1 Name: Path Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Queue Size: 10 Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /vins_estimator/path_rgbd Unreliable: false Value: true - Class: rviz/Marker Enabled: true Marker Topic: /vins_estimator/key_poses Name: Marker Namespaces: {} Queue Size: 100 Value: true - Alpha: 1 Axes Length: 1 Axes Radius: 0.10000000149011612 Class: rviz/Pose Color: 114; 159; 207 Enabled: true Head Length: 0.30000001192092896 Head Radius: 0.10000000149011612 Name: Pose Queue Size: 10 Shaft Length: 1 Shaft Radius: 0.05000000074505806 Shape: Arrow Topic: /mavros/vision_pose/remap_pose Unreliable: false Value: true - Angle Tolerance: 0.10000000149011612 Class: rviz/Odometry Covariance: Orientation: Alpha: 0.5 Color: 255; 255; 127 Color Style: Unique Frame: Local Offset: 1 Scale: 1 Value: true Position: Alpha: 0.30000001192092896 Color: 204; 51; 204 Scale: 1 Value: true Value: true Enabled: true Keep: 1 Name: Odometry Position Tolerance: 0.10000000149011612 Queue Size: 10 Shape: Alpha: 1 Axes Length: 1 Axes Radius: 0.10000000149011612 Color: 255; 25; 0 Head Length: 0.30000001192092896 Head Radius: 0.10000000149011612 Shaft Length: 1 Shaft Radius: 0.05000000074505806 Value: Axes Topic: /odom Unreliable: false Value: true - Class: rviz/Image Enabled: false Image Topic: /usb_cam/image_raw Max Value: 1 Median window: 5 Min Value: 0 Name: Image Normalize Range: true Queue Size: 2 Transport Hint: raw Unreliable: false Value: false - Alpha: 1 Axes Length: 1 Axes Radius: 0.10000000149011612 Class: rviz/Pose Color: 25; 255; 0 Enabled: true Head Length: 0.30000001192092896 Head Radius: 0.10000000149011612 Name: Pose Queue Size: 10 Shaft Length: 1 Shaft Radius: 0.05000000074505806 Shape: Axes Topic: /mavros/vision_pose/pose Unreliable: false Value: true - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 25; 255; 0 Enabled: true Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Lines Line Width: 0.029999999329447746 Name: Path Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Queue Size: 10 Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /gt Unreliable: false Value: true - Alpha: 1 Axes Length: 1 Axes Radius: 0.10000000149011612 Class: rviz/Pose Color: 255; 25; 0 Enabled: true Head Length: 0.30000001192092896 Head Radius: 0.10000000149011612 Name: Pose Queue Size: 10 Shaft Length: 1 Shaft Radius: 0.05000000074505806 Shape: Axes Topic: /tag_pose Unreliable: false Value: true - Alpha: 1 Axes Length: 1 Axes Radius: 0.10000000149011612 Class: rviz/Pose Color: 255; 25; 0 Enabled: true Head Length: 0.30000001192092896 Head Radius: 0.10000000149011612 Name: Pose Queue Size: 10 Shaft Length: 1 Shaft Radius: 0.05000000074505806 Shape: Axes Topic: /mavros/setpoint_position/local Unreliable: false Value: true - Alpha: 1 Axes Length: 1 Axes Radius: 0.10000000149011612 Class: rviz/Pose Color: 255; 25; 0 Enabled: true Head Length: 0.30000001192092896 Head Radius: 0.10000000149011612 Name: Pose Queue Size: 10 Shaft Length: 1 Shaft Radius: 0.05000000074505806 Shape: Axes Topic: /tag_pose Unreliable: false Value: true Enabled: true Global Options: Background Color: 0; 0; 0 Default Light: true Fixed Frame: map Frame Rate: 30 Name: root Tools: - Class: rviz/MoveCamera - Class: rviz/Select - Class: rviz/FocusCamera - Class: rviz/Measure - Class: rviz/SetInitialPose Theta std deviation: 0.2617993950843811 Topic: /initialpose X std deviation: 0.5 Y std deviation: 0.5 - Class: rviz/SetGoal Topic: /move_base_simple/goal - Class: rviz/PublishPoint Single click: true Topic: /clicked_point Value: true Views: Current: Class: rviz/ThirdPersonFollower Distance: 8.819969177246094 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Field of View: 0.7853981852531433 Focal Point: X: 1.5674259662628174 Y: -1.2659180164337158 Z: -4.0531158447265625e-06 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 Pitch: 0.3497968316078186 Target Frame: Yaw: 5.15224027633667 Saved: ~ Window Geometry: Displays: collapsed: false Height: 1016 Hide Left Dock: true Hide Right Dock: true Image: collapsed: false QMainWindow State: 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 Selection: collapsed: false Time: collapsed: false Tool Properties: collapsed: false Views: collapsed: true Width: 924 X: 72 Y: 27 loop_match_image: collapsed: false raw_image: collapsed: false tracked image: collapsed: false ================================================ FILE: config/tsinghua.rviz ================================================ Panels: - Class: rviz/Displays Help Height: 0 Name: Displays Property Tree Widget: Expanded: - /Global Options1 - /Status1 - /raw_image1 - /VIO1 - /VIO1/Path1 - /pose_graph1/pose_graph_path1 - /Odometry1 - /Odometry2 - /Odometry2/Shape1 Splitter Ratio: 0.4651159942150116 Tree Height: 441 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties Expanded: - /2D Pose Estimate1 - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.5886790156364441 - Class: rviz/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - Class: rviz/Time Experimental: false Name: Time SyncMode: 0 SyncSource: raw_image - Class: rviz/Displays Help Height: 78 Name: Displays Property Tree Widget: Expanded: - /Global Options1 - /tracked image1 - /raw_image1 - /VIO1 - /VIO1/Path1 - /pose_graph1 - /pose_graph1/pose_graph_path1 - /pose_graph1/loop_match_image1 - /pose_graph1/loop_match_image1/Status1 - /Image1 - /Path1 Splitter Ratio: 0.5 Tree Height: 741 Preferences: PromptSaveOnExit: true Toolbars: toolButtonStyle: 2 Visualization Manager: Class: "" Displays: - Alpha: 0.5 Cell Size: 1 Class: rviz/Grid Color: 130; 130; 130 Enabled: true Line Style: Line Width: 0.029999999329447746 Value: Lines Name: Grid Normal Cell Count: 0 Offset: X: 0 Y: 0 Z: 0 Plane: XY Plane Cell Count: 10 Reference Frame: Value: true - Class: rviz/Axes Enabled: false Length: 1 Name: Axes Radius: 0.10000000149011612 Reference Frame: Value: false - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 255; 0; 0 Enabled: true Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Lines Line Width: 0.5 Name: ground_truth_path Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /benchmark_publisher/path Unreliable: false Value: true - Class: rviz/Image Enabled: true Image Topic: /feature_tracker/feature_img Max Value: 1 Median window: 5 Min Value: 0 Name: tracked image Normalize Range: true Queue Size: 2 Transport Hint: raw Unreliable: false Value: true - Class: rviz/Image Enabled: true Image Topic: /camera/color/image_raw Max Value: 1 Median window: 5 Min Value: 0 Name: raw_image Normalize Range: true Queue Size: 2 Transport Hint: raw Unreliable: false Value: true - Class: rviz/Group Displays: - Alpha: 10 Buffer Length: 1 Class: rviz/Path Color: 25; 255; 0 Enabled: true Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Billboards Line Width: 1 Name: Path Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /vins_estimator/path Unreliable: false Value: true - Class: rviz/MarkerArray Enabled: true Marker Topic: /vins_estimator/camera_pose_visual Name: camera_visual Namespaces: {} Queue Size: 100 Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 10 Min Value: -10 Value: true Axis: Z Channel Name: intensity Class: rviz/PointCloud Color: 0; 255; 0 Color Transformer: FlatColor Decay Time: 1 Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 Min Color: 0; 0; 0 Name: current_point Position Transformer: XYZ Queue Size: 1 Selectable: true Size (Pixels): 1 Size (m): 0.009999999776482582 Style: Points Topic: /vins_estimator/point_cloud Unreliable: false Use Fixed Frame: true Use rainbow: true Value: false - Alpha: 1 Autocompute Intensity Bounds: false Autocompute Value Bounds: Max Value: 10 Min Value: -10 Value: true Axis: Z Channel Name: intensity Class: rviz/PointCloud Color: 255; 255; 255 Color Transformer: FlatColor Decay Time: 3000 Enabled: false Invert Rainbow: true Max Color: 0; 0; 0 Max Intensity: 4096 Min Color: 0; 0; 0 Min Intensity: 0 Name: history_point Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 1 Size (m): 0.5 Style: Points Topic: /vins_estimator/history_cloud Unreliable: false Use Fixed Frame: true Use rainbow: false Value: false - Class: rviz/TF Enabled: false Frame Timeout: 15 Frames: All Enabled: true Marker Scale: 1 Name: TF Show Arrows: true Show Axes: true Show Names: true Tree: {} Update Interval: 0 Value: false Enabled: true Name: VIO - Class: rviz/Group Displays: - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 252; 233; 79 Enabled: true Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Billboards Line Width: 1 Name: pose_graph_path Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /pose_graph/pose_graph_path Unreliable: false Value: true - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 255; 170; 0 Enabled: false Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Lines Line Width: 0.029999999329447746 Name: base_path Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /pose_graph/base_path Unreliable: false Value: false - Class: rviz/MarkerArray Enabled: false Marker Topic: /pose_graph/pose_graph Name: loop_visual Namespaces: {} Queue Size: 100 Value: false - Class: rviz/MarkerArray Enabled: true Marker Topic: /pose_graph/camera_pose_visual Name: camera_visual Namespaces: {} Queue Size: 100 Value: true - Class: rviz/Image Enabled: false Image Topic: /match_image Max Value: 1 Median window: 5 Min Value: 0 Name: loop_match_image Normalize Range: true Queue Size: 2 Transport Hint: raw Unreliable: false Value: false - Class: rviz/Marker Enabled: false Marker Topic: /pose_graph/key_odometrys Name: Marker Namespaces: {} Queue Size: 100 Value: false - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 25; 255; 0 Enabled: false Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Lines Line Width: 0.30000001192092896 Name: Sequence1 Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /pose_graph/path_1 Unreliable: false Value: false - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 255; 0; 0 Enabled: false Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Lines Line Width: 0.30000001192092896 Name: Sequence2 Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /pose_graph/path_2 Unreliable: false Value: false - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 0; 85; 255 Enabled: false Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Lines Line Width: 0.30000001192092896 Name: Sequence3 Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /pose_graph/path_3 Unreliable: false Value: false - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 255; 170; 0 Enabled: false Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Lines Line Width: 0.30000001192092896 Name: Sequence4 Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /pose_graph/path_4 Unreliable: false Value: false - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 255; 170; 255 Enabled: false Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Lines Line Width: 0.029999999329447746 Name: Sequence5 Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /pose_graph/path_5 Unreliable: false Value: false - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 25; 255; 0 Enabled: false Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Lines Line Width: 0.029999999329447746 Name: no_loop_path Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /pose_graph/no_loop_path Unreliable: false Value: false Enabled: true Name: pose_graph - Angle Tolerance: 0.10000000149011612 Class: rviz/Odometry Covariance: Orientation: Alpha: 0.5 Color: 255; 255; 127 Color Style: Unique Frame: Local Offset: 1 Scale: 1 Value: true Position: Alpha: 0.30000001192092896 Color: 204; 51; 204 Scale: 1 Value: true Value: true Enabled: true Keep: 1 Name: Odometry Position Tolerance: 0.10000000149011612 Shape: Alpha: 1 Axes Length: 1 Axes Radius: 0.10000000149011612 Color: 255; 25; 0 Head Length: 0.30000001192092896 Head Radius: 0.10000000149011612 Shaft Length: 1 Shaft Radius: 0.05000000074505806 Value: Axes Topic: /vins_estimator/imu_propagate Unreliable: false Value: true - Angle Tolerance: 0.10000000149011612 Class: rviz/Odometry Covariance: Orientation: Alpha: 0.5 Color: 255; 255; 127 Color Style: Unique Frame: Local Offset: 1 Scale: 1 Value: true Position: Alpha: 0.30000001192092896 Color: 204; 51; 204 Scale: 1 Value: true Value: true Enabled: true Keep: 10000 Name: Odometry Position Tolerance: 0.10000000149011612 Shape: Alpha: 1 Axes Length: 1 Axes Radius: 0.10000000149011612 Color: 255; 25; 0 Head Length: 0.029999999329447746 Head Radius: 0.10000000149011612 Shaft Length: 0.10000000149011612 Shaft Radius: 0.05000000074505806 Value: Arrow Topic: /odom Unreliable: false Value: true - Class: rviz/Marker Enabled: true Marker Topic: /image_to_rviz/visualization_marker Name: Marker Namespaces: image: true Queue Size: 100 Value: true - Class: rviz/Image Enabled: false Image Topic: /vins_estimator/semantic_mask Max Value: 1 Median window: 5 Min Value: 0 Name: Image Normalize Range: true Queue Size: 2 Transport Hint: raw Unreliable: false Value: false - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 25; 255; 0 Enabled: true Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Billboards Line Width: 1 Name: Path Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /vins_estimator/path Unreliable: false Value: true Enabled: true Global Options: Background Color: 0; 0; 0 Default Light: true Fixed Frame: map Frame Rate: 30 Name: root Tools: - Class: rviz/MoveCamera - Class: rviz/Select - Class: rviz/FocusCamera - Class: rviz/Measure - Class: rviz/SetInitialPose Theta std deviation: 0.2617993950843811 Topic: /initialpose X std deviation: 0.5 Y std deviation: 0.5 - Class: rviz/SetGoal Topic: /move_base_simple/goal - Class: rviz/PublishPoint Single click: true Topic: /clicked_point Value: true Views: Current: Angle: -1.7600017786026 Class: rviz/TopDownOrtho Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 Scale: 1.4435186386108398 Target Frame: Value: TopDownOrtho (rviz) X: 213.99978637695312 Y: 93.23384857177734 Saved: - Angle: -1.9099998474121094 Class: rviz/TopDownOrtho Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Invert Z Axis: false Name: TopDownOrtho Near Clip Distance: 0.009999999776482582 Scale: 2.8129937648773193 Target Frame: Value: TopDownOrtho (rviz) X: 90.85389709472656 Y: 126.16999053955078 Window Geometry: Displays: collapsed: false Height: 960 Hide Left Dock: false Hide Right Dock: true Image: collapsed: false QMainWindow State: 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 Selection: collapsed: false Time: collapsed: false Tool Properties: collapsed: false Views: collapsed: true Width: 1920 X: 0 Y: 27 loop_match_image: collapsed: false raw_image: collapsed: false tracked image: collapsed: false ================================================ FILE: config/tum_rgbd/tum_fr3.yaml ================================================ %YAML:1.0 num_threads: 0 # 0 Use the max number of threads of your device. # For some devices, like HUAWEI Atlas200, the auto detected max number of threads might not equivalent to the usable numble of threads. (Some cores(threads) might be reserved for other usage(NPU) by system.) # x It is proper that 1 < x < MAX_THREAD. # For now, this parameter is relevant to grid-detector to run in parallel. #common parameters imu: 0 image_topic: "/camera/rgb/image_color" depth_topic: "/camera/depth/image" output_path: "/home/nrosliu/DynamicVINS-Atlas/src/Dynamic_VINS/output" #RGBD camera Ideal Range depth_min_dist: 0.4 depth_max_dist: 10 fix_depth: 1 frontend_freq: 30 # It should be raised in VO mode(without IMU). freq: 30 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image keyframe_parallax: 10.0 # keyframe selection threshold (pixel); if system fails frequently, please try to reduce the "keyframe_parallax" num_grid_rows: 7 num_grid_cols: 8 #camera calibration model_type: PINHOLE camera_name: camera image_width: 640 image_height: 480 distortion_parameters: k1: 0.0 k2: 0.0 p1: 0.0 p2: 0.0 projection_parameters: fx: 535.4 fy: 539.2 cx: 320.1 cy: 247.6 # Extrinsic parameter between IMU and Camera. estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it. # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess. # 2 Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning. #If you choose 0 or 1, you should write down the following matrix. #Rotation from camera frame to imu frame, imu^R_cam extrinsicRotation: !!opencv-matrix rows: 3 cols: 3 dt: d data: [ 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] #Translation from camera frame to imu frame, imu^T_cam extrinsicTranslation: !!opencv-matrix rows: 3 cols: 1 dt: d data: [0.0, 0.0, 0.0] #feature traker paprameters max_cnt: 250 # max feature number in feature tracking. It is suggested to be raised in VO mode. min_dist: 10 # min distance between two features F_threshold: 1.0 # ransac threshold (pixel) show_track: 0 # publish tracking image as topic equalize: 0 # if image is too dark or light, trun on equalize to find enough features fisheye: 0 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points #optimization parameters max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time max_num_iterations: 8 # max solver itrations, to guarantee real time #unsynchronization parameters estimate_td: 0 ########## # online estimate time offset between camera and imu td: 0.000 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock) #rolling shutter parameters rolling_shutter: 0 # 0: global shutter camera, 1: rolling shutter camera rolling_shutter_tr: 0.0 # unit: s. rolling shutter read out time per frame (from data sheet). #loop closure parameters loop_closure: 0 # start loop closure fast_relocalization: 0 # useful in real-time and large project load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path' pose_graph_save_path: "/home/nrosliu/DynamicVINS-Atlas/src/Dynamic_VINS/output/pose_graph" # save and load path #visualization parameters save_image: 0 # save image in pose graph for visualization prupose; you can close this function by setting 0 visualize_imu_forward: 0 # output imu forward propogation to achieve low latency and high frequence results visualize_camera_size: 0.4 # size of camera marker in RVIZ #暂时仅考虑动物 dynamic_label: ["person", "cat", "dog", "bicycle", "car","bus"] ================================================ FILE: config/tum_rgbd/tum_fr3_atlas.yaml ================================================ %YAML:1.0 num_threads: 6 # 0 Use the max number of threads of your device. # For some devices, like HUAWEI Atlas200, the auto detected max number of threads might not equivalent to the usable numble of threads. (Some cores(threads) might be reserved for other usage(NPU) by system.) # x It is proper that 1 < x < MAX_THREAD. # For now, this parameter is relevant to grid-detector to run in parallel. #common parameters imu: 0 image_topic: "/decompressed_img" depth_topic: "/camera/depth/image" output_path: "/home/HwHiAiUser/Dynamic_VINS_ws/src/Dynamic_VINS/output" #RGBD camera Ideal Range depth_min_dist: 0.4 depth_max_dist: 10 frontend_freq: 30 # It should be raised in VO mode(without IMU). freq: 30 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image # It should be raised in VO mode(without IMU). keyframe_parallax: 10.0 # keyframe selection threshold (pixel); if system fails frequently, please try to reduce the "keyframe_parallax" num_grid_rows: 7 num_grid_cols: 8 #camera calibration model_type: PINHOLE camera_name: camera image_width: 640 image_height: 480 distortion_parameters: k1: 0.0 k2: 0.0 p1: 0.0 p2: 0.0 projection_parameters: fx: 535.4 fy: 539.2 cx: 320.1 cy: 247.6 # Extrinsic parameter between IMU and Camera. estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it. # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess. # 2 Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning. #If you choose 0 or 1, you should write down the following matrix. #Rotation from camera frame to imu frame, imu^R_cam extrinsicRotation: !!opencv-matrix rows: 3 cols: 3 dt: d data: [ 0.999975572679493, 0.003849141066713, 0.005854714944742, -0.003828680351062, 0.999986658473099, -0.003501944262433, -0.005868115609379, 0.003479442469180, 0.999976848846595] #Translation from camera frame to imu frame, imu^T_cam extrinsicTranslation: !!opencv-matrix rows: 3 cols: 1 dt: d data: [0.0203127935529, -0.00510325236246, -0.0112013882026] #feature traker paprameters max_cnt: 200 # max feature number in feature tracking. min_dist: 20 # min distance between two features F_threshold: 1.0 # ransac threshold (pixel) show_track: 0 # publish tracking image as topic equalize: 0 # if image is too dark or light, trun on equalize to find enough features fisheye: 0 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points #optimization parameters max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time max_num_iterations: 8 # max solver itrations, to guarantee real time g_norm: 9.805 # gravity magnitude #unsynchronization parameters estimate_td: 0 ########## # online estimate time offset between camera and imu td: 0.000 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock) #rolling shutter parameters rolling_shutter: 0 # 0: global shutter camera, 1: rolling shutter camera rolling_shutter_tr: 0.0 # unit: s. rolling shutter read out time per frame (from data sheet). #loop closure parameters loop_closure: 0 # start loop closure fast_relocalization: 0 # useful in real-time and large project load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path' pose_graph_save_path: "/home/nrosliu/DynamicVINS-Atlas/src/Dynamic_VINS/output/pose_graph" # save and load path #visualization parameters save_image: 0 # save image in pose graph for visualization prupose; you can close this function by setting 0 visualize_imu_forward: 0 # output imu forward propogation to achieve low latency and high frequence results visualize_camera_size: 0.4 # size of camera marker in RVIZ #暂时仅考虑动物 dynamic_label: ["person", "cat", "dog", "bicycle", "car","bus"] ================================================ FILE: config/video.rviz ================================================ Panels: - Class: rviz/Displays Help Height: 0 Name: Displays Property Tree Widget: Expanded: - /Global Options1 - /Status1 - /VIO1 - /VIO1/Path1 - /pose_graph1/pose_graph_path1 - /Odometry1 - /Odometry1/Shape1 - /Odometry2 - /Odometry2/Shape1 - /Marker1 Splitter Ratio: 0.4651159942150116 Tree Height: 311 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties Expanded: - /2D Pose Estimate1 - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.5886790156364441 - Class: rviz/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - Class: rviz/Time Experimental: false Name: Time SyncMode: 0 SyncSource: tracked image - Class: rviz/Displays Help Height: 78 Name: Displays Property Tree Widget: Expanded: - /tracked image1 - /raw_image1 - /VIO1 - /VIO1/Path1 - /pose_graph1 - /pose_graph1/pose_graph_path1 Splitter Ratio: 0.5 Tree Height: 363 Preferences: PromptSaveOnExit: true Toolbars: toolButtonStyle: 2 Visualization Manager: Class: "" Displays: - Alpha: 0.5 Cell Size: 1 Class: rviz/Grid Color: 130; 130; 130 Enabled: true Line Style: Line Width: 0.029999999329447746 Value: Lines Name: Grid Normal Cell Count: 0 Offset: X: 0 Y: 0 Z: 0 Plane: XY Plane Cell Count: 10 Reference Frame: Value: true - Alpha: 1 Class: rviz/Axes Enabled: false Length: 1 Name: Axes Radius: 0.10000000149011612 Reference Frame: Show Trail: false Value: false - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 255; 0; 0 Enabled: true Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Lines Line Width: 0.5 Name: ground_truth_path Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Queue Size: 10 Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /benchmark_publisher/path Unreliable: false Value: true - Class: rviz/Image Enabled: true Image Topic: /vins_estimator/feature_img Max Value: 1 Median window: 5 Min Value: 0 Name: tracked image Normalize Range: true Queue Size: 2 Transport Hint: raw Unreliable: false Value: true - Class: rviz/Image Enabled: false Image Topic: /camera/color/image_raw Max Value: 1 Median window: 5 Min Value: 0 Name: raw_image Normalize Range: true Queue Size: 2 Transport Hint: raw Unreliable: false Value: false - Class: rviz/Group Displays: - Alpha: 10 Buffer Length: 1 Class: rviz/Path Color: 25; 255; 0 Enabled: true Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Billboards Line Width: 0.10000000149011612 Name: Path Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Queue Size: 10 Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /vins_estimator/path Unreliable: false Value: true - Class: rviz/MarkerArray Enabled: true Marker Topic: /vins_estimator/camera_pose_visual Name: camera_visual Namespaces: CameraPoseVisualization: true Queue Size: 100 Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 10 Min Value: -10 Value: true Axis: Z Channel Name: intensity Class: rviz/PointCloud Color: 0; 255; 0 Color Transformer: FlatColor Decay Time: 10000 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Min Color: 0; 0; 0 Name: current_point Position Transformer: XYZ Queue Size: 1 Selectable: true Size (Pixels): 1 Size (m): 0.009999999776482582 Style: Points Topic: /vins_estimator/point_cloud Unreliable: false Use Fixed Frame: true Use rainbow: true Value: true - Alpha: 1 Autocompute Intensity Bounds: false Autocompute Value Bounds: Max Value: 10 Min Value: -10 Value: true Axis: Z Channel Name: intensity Class: rviz/PointCloud Color: 255; 255; 255 Color Transformer: FlatColor Decay Time: 3000 Enabled: true Invert Rainbow: true Max Color: 0; 0; 0 Max Intensity: 4096 Min Color: 0; 0; 0 Min Intensity: 0 Name: history_point Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 1 Size (m): 0.5 Style: Points Topic: /vins_estimator/history_cloud Unreliable: false Use Fixed Frame: true Use rainbow: false Value: true - Class: rviz/TF Enabled: false Frame Timeout: 15 Frames: All Enabled: true Marker Alpha: 1 Marker Scale: 1 Name: TF Show Arrows: true Show Axes: true Show Names: true Tree: {} Update Interval: 0 Value: false Enabled: true Name: VIO - Class: rviz/Group Displays: - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 252; 233; 79 Enabled: true Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Billboards Line Width: 1 Name: pose_graph_path Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Queue Size: 10 Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /pose_graph/pose_graph_path Unreliable: false Value: true - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 255; 170; 0 Enabled: false Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Lines Line Width: 0.029999999329447746 Name: base_path Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Queue Size: 10 Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /pose_graph/base_path Unreliable: false Value: false - Class: rviz/MarkerArray Enabled: false Marker Topic: /pose_graph/pose_graph Name: loop_visual Namespaces: {} Queue Size: 100 Value: false - Class: rviz/MarkerArray Enabled: true Marker Topic: /pose_graph/camera_pose_visual Name: camera_visual Namespaces: {} Queue Size: 100 Value: true - Class: rviz/Image Enabled: false Image Topic: /pose_graph/match_image Max Value: 1 Median window: 5 Min Value: 0 Name: loop_match_image Normalize Range: true Queue Size: 2 Transport Hint: raw Unreliable: false Value: false - Class: rviz/Marker Enabled: false Marker Topic: /pose_graph/key_odometrys Name: Marker Namespaces: {} Queue Size: 100 Value: false - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 25; 255; 0 Enabled: false Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Lines Line Width: 0.30000001192092896 Name: Sequence1 Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Queue Size: 10 Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /pose_graph/path_1 Unreliable: false Value: false - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 255; 0; 0 Enabled: false Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Lines Line Width: 0.30000001192092896 Name: Sequence2 Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Queue Size: 10 Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /pose_graph/path_2 Unreliable: false Value: false - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 0; 85; 255 Enabled: false Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Lines Line Width: 0.30000001192092896 Name: Sequence3 Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Queue Size: 10 Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /pose_graph/path_3 Unreliable: false Value: false - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 255; 170; 0 Enabled: false Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Lines Line Width: 0.30000001192092896 Name: Sequence4 Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Queue Size: 10 Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /pose_graph/path_4 Unreliable: false Value: false - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 255; 170; 255 Enabled: false Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Lines Line Width: 0.029999999329447746 Name: Sequence5 Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Queue Size: 10 Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /pose_graph/path_5 Unreliable: false Value: false - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 25; 255; 0 Enabled: false Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Lines Line Width: 0.029999999329447746 Name: no_loop_path Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Queue Size: 10 Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /pose_graph/no_loop_path Unreliable: false Value: false Enabled: true Name: pose_graph - Angle Tolerance: 0.10000000149011612 Class: rviz/Odometry Covariance: Orientation: Alpha: 0.5 Color: 255; 255; 127 Color Style: Unique Frame: Local Offset: 1 Scale: 1 Value: true Position: Alpha: 0.30000001192092896 Color: 204; 51; 204 Scale: 1 Value: true Value: true Enabled: true Keep: 1 Name: Odometry Position Tolerance: 0.10000000149011612 Queue Size: 10 Shape: Alpha: 1 Axes Length: 1 Axes Radius: 0.10000000149011612 Color: 239; 41; 41 Head Length: 0.30000001192092896 Head Radius: 0.10000000149011612 Shaft Length: 1 Shaft Radius: 0.05000000074505806 Value: Arrow Topic: /vins_estimator/odometry Unreliable: false Value: true - Angle Tolerance: 0.10000000149011612 Class: rviz/Odometry Covariance: Orientation: Alpha: 0.5 Color: 255; 255; 127 Color Style: Unique Frame: Local Offset: 1 Scale: 1 Value: true Position: Alpha: 0.30000001192092896 Color: 204; 51; 204 Scale: 1 Value: true Value: true Enabled: false Keep: 1 Name: Odometry Position Tolerance: 0.10000000149011612 Queue Size: 10 Shape: Alpha: 1 Axes Length: 1 Axes Radius: 0.10000000149011612 Color: 114; 159; 207 Head Length: 0.029999999329447746 Head Radius: 0.10000000149011612 Shaft Length: 0.10000000149011612 Shaft Radius: 0.05000000074505806 Value: Arrow Topic: /vins_estimator/odometry Unreliable: false Value: false - Class: rviz/Image Enabled: false Image Topic: /vins_estimator/semantic_mask Max Value: 1 Median window: 5 Min Value: 0 Name: Image Normalize Range: true Queue Size: 2 Transport Hint: raw Unreliable: false Value: false - Class: rviz/Marker Enabled: true Marker Topic: /image_to_rviz/visualization_marker Name: Marker Namespaces: {} Queue Size: 100 Value: true - Class: rviz/Image Enabled: false Image Topic: /vins_estimator/semantic_mask Max Value: 1 Median window: 5 Min Value: 0 Name: Image Normalize Range: true Queue Size: 2 Transport Hint: raw Unreliable: false Value: false - Alpha: 10 Buffer Length: 1 Class: rviz/Path Color: 239; 41; 41 Enabled: true Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Billboards Line Width: 1 Name: Path Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Queue Size: 10 Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /vins_estimator/path_rgbd Unreliable: false Value: true - Class: rviz/Marker Enabled: true Marker Topic: /vins_estimator/key_poses Name: Marker Namespaces: key_poses: true Queue Size: 100 Value: true - Alpha: 1 Axes Length: 1 Axes Radius: 0.10000000149011612 Class: rviz/Pose Color: 114; 159; 207 Enabled: true Head Length: 0.30000001192092896 Head Radius: 0.10000000149011612 Name: Pose Queue Size: 10 Shaft Length: 1 Shaft Radius: 0.05000000074505806 Shape: Arrow Topic: /mavros/vision_pose/remap_pose Unreliable: false Value: true Enabled: true Global Options: Background Color: 0; 0; 0 Default Light: true Fixed Frame: map Frame Rate: 30 Name: root Tools: - Class: rviz/MoveCamera - Class: rviz/Select - Class: rviz/FocusCamera - Class: rviz/Measure - Class: rviz/SetInitialPose Theta std deviation: 0.2617993950843811 Topic: /initialpose X std deviation: 0.5 Y std deviation: 0.5 - Class: rviz/SetGoal Topic: /move_base_simple/goal - Class: rviz/PublishPoint Single click: true Topic: /clicked_point Value: true Views: Current: Class: rviz/ThirdPersonFollower Distance: 4.732760429382324 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Field of View: 0.7853981852531433 Focal Point: X: 0.34211790561676025 Y: 0.2611902952194214 Z: -3.0994415283203125e-06 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 Pitch: -0.19520258903503418 Target Frame: Yaw: 4.882240295410156 Saved: ~ Window Geometry: Displays: collapsed: false Height: 1043 Hide Left Dock: false Hide Right Dock: true Image: collapsed: false QMainWindow State: 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 Selection: collapsed: false Time: collapsed: false Tool Properties: collapsed: false Views: collapsed: true Width: 1848 X: 72 Y: 0 loop_match_image: collapsed: false raw_image: collapsed: false tracked image: collapsed: false ================================================ FILE: doc/INSTALL.md ================================================ # Dynamic-VINS ## 1. Prerequisites **ROS** ``` sudo apt-get install ros-melodic-cv-bridge ros-melodic-tf ros-melodic-message-filters ros-melodic-image-transport ros-melodic-nav-msgs ros-melodic-visualization-msgs ``` **Ceres-Solver** ``` # CMake sudo apt-get install cmake # google-glog + gflags sudo apt-get install libgoogle-glog-dev libgflags-dev # BLAS & LAPACK sudo apt-get install libatlas-base-dev # Eigen3 sudo apt-get install libeigen3-dev # SuiteSparse and CXSparse (optional) sudo apt-get install libsuitesparse-dev ``` ``` git clone https://ceres-solver.googlesource.com/ceres-solver cd ceres-solver git checkout 2.0.0 mkdir ceres-bin cd ceres-bin cmake .. make -j3 sudo make install ``` **Sophus** ``` git clone https://github.com/strasdat/Sophus.git cd Sophus git checkout a621ff #版本回溯 ``` `gedit sophus/so2.cpp` modify `sophus/so2.cpp` as ``` SO2::SO2() { unit_complex_.real(1.0); unit_complex_.imag(0.0); } ``` build ``` mkdir build && cd build && cmake .. && sudo make install ``` ## 2. Prerequisites for object detection We offer two kinds of device for tests, please follow the instruction for your match device. ### 2.1. NVIDIA devices Clone the repository and catkin_make: ``` cd {YOUR_WORKSPACE}/src git clone https://github.com/jianhengLiu/Dynamic-VINS.git # build cd .. catkin_make ``` ### 2.2. HUAWEI Atlas200 **!!!Note:** It is recommended to use high write speeds's microSD card(TF card), and a low write speeds' microSD card may result in Dyanmic-VINS not perferming in real time.(at least 40Mbs) 0. prequisities ``` sudo apt install ros-melodic-image-transport-plugins ``` 1. Clone the repository: ``` cd {YOUR_WORKSPACE}/src git clone https://github.com/jianhengLiu/Dynamic-VINS.git git clone https://github.com/jianhengLiu/compressedimg2img.git ``` 1. allocate core ``` sudo npu-smi set -t aicpu-config -i 0 -c 0 -d 2 ``` 4. build ``` cd ../.. catkin_make ``` ================================================ FILE: doc/RUNNING_PROCEDURE.md ================================================ # Dynamic-VINS testing procedure 1. ``` export ROS_HOSTNAME=192.168.2.223 #从机IP export ROS_MASTER_URI=http://192.168.2.223:11311 roscore ``` 2. ``` export ROS_HOSTNAME=192.168.2.223 #从机IP export ROS_MASTER_URI=http://192.168.2.223:11311 rosrun image_transport republish raw in:=/d400/color/image_raw compressed out:=/d400/color/image_raw ``` 3. ``` export ROS_HOSTNAME=192.168.2.223 #从机IP export ROS_MASTER_URI=http://192.168.2.223:11311 rosrun image_transport republish raw in:=/d400/aligned_depth_to_color/image_raw compressedDepth out:=/d400/aligned_depth_to_color/image_raw ``` 5. Dynamic-VINS 1. run yolo, vins on atlas 1. ``` ssh HwHiAiUser@192.168.2.2 export ROS_HOSTNAME=192.168.2.2 #从机IP export ROS_MASTER_URI=http://192.168.2.223:11311 roslaunch vins_estimator nodelet_openloris.launch ``` 2. run yolo on atlas, and run vins on pc 1. ``` (run vins on pc) export ROS_HOSTNAME=192.168.2.223 #从机IP export ROS_MASTER_URI=http://192.168.2.223:11311 roslaunch vins_estimator nodelet_openloris_test.launch ``` 6. rviz visualiztion ``` export ROS_HOSTNAME=192.168.2.223 #从机IP export ROS_MASTER_URI=http://192.168.2.223:11311 roslaunch vins_estimator vins_rviz.launch ``` ================================================ FILE: output/.gitignore ================================================ * !.gitignore ================================================ FILE: pose_graph/CMakeLists.txt ================================================ cmake_minimum_required(VERSION 2.8.3) project(pose_graph) set(CMAKE_BUILD_TYPE "Release") # https://blog.csdn.net/qq_24624539/article/details/111056791 #set(CMAKE_CXX_FLAGS "-std=c++11") set(CMAKE_CXX_FLAGS "-std=c++14") #-DEIGEN_USE_MKL_ALL") set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g") find_package(catkin REQUIRED COMPONENTS roscpp std_msgs nav_msgs camera_model cv_bridge roslib message_filters nodelet ) find_package(OpenCV) find_package(Ceres REQUIRED) set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) find_package(Eigen3) include_directories(${catkin_INCLUDE_DIRS} ${CERES_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR}) include_directories( ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR} ) catkin_package() add_library(pose_graph_lib src/pose_graph/pose_graph.cpp src/keyframe/keyframe.cpp src/utility/CameraPoseVisualization.cpp src/ThirdParty/DBoW/BowVector.cpp src/ThirdParty/DBoW/FBrief.cpp src/ThirdParty/DBoW/FeatureVector.cpp src/ThirdParty/DBoW/QueryResults.cpp src/ThirdParty/DBoW/ScoringObject.cpp src/ThirdParty/DUtils/Random.cpp src/ThirdParty/DUtils/Timestamp.cpp src/ThirdParty/DVision/BRIEF.cpp src/ThirdParty/VocabularyBinary.cpp ) target_link_libraries(pose_graph_lib ${catkin_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES}) #message("catkin_lib ${catkin_LIBRARIES}") add_library(pose_graph_nodelet src/pose_graph_nodelet.cpp) target_link_libraries(pose_graph_nodelet pose_graph_lib) ================================================ FILE: pose_graph/cmake/FindEigen.cmake ================================================ # Ceres Solver - A fast non-linear least squares minimizer # Copyright 2015 Google Inc. All rights reserved. # http://ceres-solver.org/ # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: # # * Redistributions of source code must retain the above copyright notice, # this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above copyright notice, # this list of conditions and the following disclaimer in the documentation # and/or other materials provided with the distribution. # * Neither the name of Google Inc. nor the names of its contributors may be # used to endorse or promote products derived from this software without # specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. # # Author: alexs.mac@gmail.com (Alex Stewart) # # FindEigen.cmake - Find Eigen library, version >= 3. # # This module defines the following variables: # # EIGEN_FOUND: TRUE iff Eigen is found. # EIGEN_INCLUDE_DIRS: Include directories for Eigen. # # EIGEN_VERSION: Extracted from Eigen/src/Core/util/Macros.h # EIGEN_WORLD_VERSION: Equal to 3 if EIGEN_VERSION = 3.2.0 # EIGEN_MAJOR_VERSION: Equal to 2 if EIGEN_VERSION = 3.2.0 # EIGEN_MINOR_VERSION: Equal to 0 if EIGEN_VERSION = 3.2.0 # # The following variables control the behaviour of this module: # # EIGEN_INCLUDE_DIR_HINTS: List of additional directories in which to # search for eigen includes, e.g: /timbuktu/eigen3. # # The following variables are also defined by this module, but in line with # CMake recommended FindPackage() module style should NOT be referenced directly # by callers (use the plural variables detailed above instead). These variables # do however affect the behaviour of the module via FIND_[PATH/LIBRARY]() which # are NOT re-called (i.e. search for library is not repeated) if these variables # are set with valid values _in the CMake cache_. This means that if these # variables are set directly in the cache, either by the user in the CMake GUI, # or by the user passing -DVAR=VALUE directives to CMake when called (which # explicitly defines a cache variable), then they will be used verbatim, # bypassing the HINTS variables and other hard-coded search locations. # # EIGEN_INCLUDE_DIR: Include directory for CXSparse, not including the # include directory of any dependencies. # Called if we failed to find Eigen or any of it's required dependencies, # unsets all public (designed to be used externally) variables and reports # error message at priority depending upon [REQUIRED/QUIET/] argument. macro(EIGEN_REPORT_NOT_FOUND REASON_MSG) unset(EIGEN_FOUND) unset(EIGEN_INCLUDE_DIRS) # Make results of search visible in the CMake GUI if Eigen has not # been found so that user does not have to toggle to advanced view. mark_as_advanced(CLEAR EIGEN_INCLUDE_DIR) # Note _FIND_[REQUIRED/QUIETLY] variables defined by FindPackage() # use the camelcase library name, not uppercase. if (Eigen_FIND_QUIETLY) message(STATUS "Failed to find Eigen - " ${REASON_MSG} ${ARGN}) elseif (Eigen_FIND_REQUIRED) message(FATAL_ERROR "Failed to find Eigen - " ${REASON_MSG} ${ARGN}) else() # Neither QUIETLY nor REQUIRED, use no priority which emits a message # but continues configuration and allows generation. message("-- Failed to find Eigen - " ${REASON_MSG} ${ARGN}) endif () return() endmacro(EIGEN_REPORT_NOT_FOUND) # Protect against any alternative find_package scripts for this library having # been called previously (in a client project) which set EIGEN_FOUND, but not # the other variables we require / set here which could cause the search logic # here to fail. unset(EIGEN_FOUND) # Search user-installed locations first, so that we prefer user installs # to system installs where both exist. list(APPEND EIGEN_CHECK_INCLUDE_DIRS /usr/local/include /usr/local/homebrew/include # Mac OS X /opt/local/var/macports/software # Mac OS X. /opt/local/include /usr/include) # Additional suffixes to try appending to each search path. list(APPEND EIGEN_CHECK_PATH_SUFFIXES eigen3 # Default root directory for Eigen. Eigen/include/eigen3 # Windows (for C:/Program Files prefix) < 3.3 Eigen3/include/eigen3 ) # Windows (for C:/Program Files prefix) >= 3.3 # Search supplied hint directories first if supplied. find_path(EIGEN_INCLUDE_DIR NAMES Eigen/Core PATHS ${EIGEN_INCLUDE_DIR_HINTS} ${EIGEN_CHECK_INCLUDE_DIRS} PATH_SUFFIXES ${EIGEN_CHECK_PATH_SUFFIXES}) if (NOT EIGEN_INCLUDE_DIR OR NOT EXISTS ${EIGEN_INCLUDE_DIR}) eigen_report_not_found( "Could not find eigen3 include directory, set EIGEN_INCLUDE_DIR to " "path to eigen3 include directory, e.g. /usr/local/include/eigen3.") endif (NOT EIGEN_INCLUDE_DIR OR NOT EXISTS ${EIGEN_INCLUDE_DIR}) # Mark internally as found, then verify. EIGEN_REPORT_NOT_FOUND() unsets # if called. set(EIGEN_FOUND TRUE) # Extract Eigen version from Eigen/src/Core/util/Macros.h if (EIGEN_INCLUDE_DIR) set(EIGEN_VERSION_FILE ${EIGEN_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h) if (NOT EXISTS ${EIGEN_VERSION_FILE}) eigen_report_not_found( "Could not find file: ${EIGEN_VERSION_FILE} " "containing version information in Eigen install located at: " "${EIGEN_INCLUDE_DIR}.") else (NOT EXISTS ${EIGEN_VERSION_FILE}) file(READ ${EIGEN_VERSION_FILE} EIGEN_VERSION_FILE_CONTENTS) string(REGEX MATCH "#define EIGEN_WORLD_VERSION [0-9]+" EIGEN_WORLD_VERSION "${EIGEN_VERSION_FILE_CONTENTS}") string(REGEX REPLACE "#define EIGEN_WORLD_VERSION ([0-9]+)" "\\1" EIGEN_WORLD_VERSION "${EIGEN_WORLD_VERSION}") string(REGEX MATCH "#define EIGEN_MAJOR_VERSION [0-9]+" EIGEN_MAJOR_VERSION "${EIGEN_VERSION_FILE_CONTENTS}") string(REGEX REPLACE "#define EIGEN_MAJOR_VERSION ([0-9]+)" "\\1" EIGEN_MAJOR_VERSION "${EIGEN_MAJOR_VERSION}") string(REGEX MATCH "#define EIGEN_MINOR_VERSION [0-9]+" EIGEN_MINOR_VERSION "${EIGEN_VERSION_FILE_CONTENTS}") string(REGEX REPLACE "#define EIGEN_MINOR_VERSION ([0-9]+)" "\\1" EIGEN_MINOR_VERSION "${EIGEN_MINOR_VERSION}") # This is on a single line s/t CMake does not interpret it as a list of # elements and insert ';' separators which would result in 3.;2.;0 nonsense. set(EIGEN_VERSION "${EIGEN_WORLD_VERSION}.${EIGEN_MAJOR_VERSION}.${EIGEN_MINOR_VERSION}") endif (NOT EXISTS ${EIGEN_VERSION_FILE}) endif (EIGEN_INCLUDE_DIR) # Set standard CMake FindPackage variables if found. if (EIGEN_FOUND) set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR}) endif (EIGEN_FOUND) # Handle REQUIRED / QUIET optional arguments and version. include(FindPackageHandleStandardArgs) find_package_handle_standard_args(Eigen REQUIRED_VARS EIGEN_INCLUDE_DIRS VERSION_VAR EIGEN_VERSION) # Only mark internal variables as advanced if we found Eigen, otherwise # leave it visible in the standard GUI for the user to set manually. if (EIGEN_FOUND) mark_as_advanced(FORCE EIGEN_INCLUDE_DIR) endif (EIGEN_FOUND) ================================================ FILE: pose_graph/nodelet_plugin.xml ================================================ PoseGraphNodelet ================================================ FILE: pose_graph/package.xml ================================================ pose_graph 0.0.0 pose_graph package tony-ws TODO catkin roscpp roscpp camera_model camera_model nodelet nodelet ================================================ FILE: pose_graph/src/ThirdParty/DBoW/BowVector.cpp ================================================ /** * File: BowVector.cpp * Date: March 2011 * Author: Dorian Galvez-Lopez * Description: bag of words vector * License: see the LICENSE.txt file * */ #include #include #include #include #include #include "BowVector.h" namespace DBoW2 { // -------------------------------------------------------------------------- BowVector::BowVector(void) { } // -------------------------------------------------------------------------- BowVector::~BowVector(void) { } // -------------------------------------------------------------------------- void BowVector::addWeight(WordId id, WordValue v) { BowVector::iterator vit = this->lower_bound(id); if(vit != this->end() && !(this->key_comp()(id, vit->first))) { vit->second += v; } else { this->insert(vit, BowVector::value_type(id, v)); } } // -------------------------------------------------------------------------- void BowVector::addIfNotExist(WordId id, WordValue v) { BowVector::iterator vit = this->lower_bound(id); if(vit == this->end() || (this->key_comp()(id, vit->first))) { this->insert(vit, BowVector::value_type(id, v)); } } // -------------------------------------------------------------------------- void BowVector::normalize(LNorm norm_type) { double norm = 0.0; BowVector::iterator it; if(norm_type == DBoW2::L1) { for(it = begin(); it != end(); ++it) norm += fabs(it->second); } else { for(it = begin(); it != end(); ++it) norm += it->second * it->second; norm = sqrt(norm); } if(norm > 0.0) { for(it = begin(); it != end(); ++it) it->second /= norm; } } // -------------------------------------------------------------------------- std::ostream& operator<< (std::ostream &out, const BowVector &v) { BowVector::const_iterator vit; std::vector::const_iterator iit; unsigned int i = 0; const unsigned int N = v.size(); for(vit = v.begin(); vit != v.end(); ++vit, ++i) { out << "<" << vit->first << ", " << vit->second << ">"; if(i < N-1) out << ", "; } return out; } // -------------------------------------------------------------------------- void BowVector::saveM(const std::string &filename, size_t W) const { std::fstream f(filename.c_str(), std::ios::out); WordId last = 0; BowVector::const_iterator bit; for(bit = this->begin(); bit != this->end(); ++bit) { for(; last < bit->first; ++last) { f << "0 "; } f << bit->second << " "; last = bit->first + 1; } for(; last < (WordId)W; ++last) f << "0 "; f.close(); } // -------------------------------------------------------------------------- } // namespace DBoW2 ================================================ FILE: pose_graph/src/ThirdParty/DBoW/BowVector.h ================================================ /** * File: BowVector.h * Date: March 2011 * Author: Dorian Galvez-Lopez * Description: bag of words vector * License: see the LICENSE.txt file * */ #ifndef __D_T_BOW_VECTOR__ #define __D_T_BOW_VECTOR__ #include #include #include namespace DBoW2 { /// Id of words typedef unsigned int WordId; /// Value of a word typedef double WordValue; /// Id of nodes in the vocabulary treee typedef unsigned int NodeId; /// L-norms for normalization enum LNorm { L1, L2 }; /// Weighting type enum WeightingType { TF_IDF, TF, IDF, BINARY }; /// Scoring type enum ScoringType { L1_NORM, L2_NORM, CHI_SQUARE, KL, BHATTACHARYYA, DOT_PRODUCT }; /// Vector of words to represent images class BowVector: public std::map { public: /** * Constructor */ BowVector(void); /** * Destructor */ ~BowVector(void); /** * Adds a value to a word value existing in the vector, or creates a new * word with the given value * @param id word id to look for * @param v value to create the word with, or to add to existing word */ void addWeight(WordId id, WordValue v); /** * Adds a word with a value to the vector only if this does not exist yet * @param id word id to look for * @param v value to give to the word if this does not exist */ void addIfNotExist(WordId id, WordValue v); /** * L1-Normalizes the values in the vector * @param norm_type norm used */ void normalize(LNorm norm_type); /** * Prints the content of the bow vector * @param out stream * @param v */ friend std::ostream& operator<<(std::ostream &out, const BowVector &v); /** * Saves the bow vector as a vector in a matlab file * @param filename * @param W number of words in the vocabulary */ void saveM(const std::string &filename, size_t W) const; }; } // namespace DBoW2 #endif ================================================ FILE: pose_graph/src/ThirdParty/DBoW/DBoW2.h ================================================ /* * File: DBoW2.h * Date: November 2011 * Author: Dorian Galvez-Lopez * Description: Generic include file for the DBoW2 classes and * the specialized vocabularies and databases * License: see the LICENSE.txt file * */ /*! \mainpage DBoW2 Library * * DBoW2 library for C++: * Bag-of-word image database for image retrieval. * * Written by Dorian Galvez-Lopez, * University of Zaragoza * * Check my website to obtain updates: http://doriangalvez.com * * \section requirements Requirements * This library requires the DUtils, DUtilsCV, DVision and OpenCV libraries, * as well as the boost::dynamic_bitset class. * * \section citation Citation * If you use this software in academic works, please cite: 
   @@ARTICLE{GalvezTRO12,
    author={Galvez-Lopez, Dorian and Tardos, J. D.}, 
    journal={IEEE Transactions on Robotics},
    title={Bags of Binary Words for Fast Place Recognition in Image Sequences},
    year={2012},
    month={October},
    volume={28},
    number={5},
    pages={1188--1197},
    doi={10.1109/TRO.2012.2197158},
    ISSN={1552-3098}
  }
* * \section license License * This file is licensed under a Creative Commons * Attribution-NonCommercial-ShareAlike 3.0 license. * This file can be freely used and users can use, download and edit this file * provided that credit is attributed to the original author. No users are * permitted to use this file for commercial purposes unless explicit permission * is given by the original author. Derivative works must be licensed using the * same or similar license. * Check http://creativecommons.org/licenses/by-nc-sa/3.0/ to obtain further * details. * */ #ifndef __D_T_DBOW2__ #define __D_T_DBOW2__ /// Includes all the data structures to manage vocabularies and image databases namespace DBoW2 { } #include "TemplatedVocabulary.h" #include "TemplatedDatabase.h" #include "BowVector.h" #include "FeatureVector.h" #include "QueryResults.h" #include "FBrief.h" /// BRIEF Vocabulary typedef DBoW2::TemplatedVocabulary BriefVocabulary; /// BRIEF Database typedef DBoW2::TemplatedDatabase BriefDatabase; #endif ================================================ FILE: pose_graph/src/ThirdParty/DBoW/FBrief.cpp ================================================ /** * File: FBrief.cpp * Date: November 2011 * Author: Dorian Galvez-Lopez * Description: functions for BRIEF descriptors * License: see the LICENSE.txt file * */ #include #include #include #include "FBrief.h" using namespace std; namespace DBoW2 { // -------------------------------------------------------------------------- void FBrief::meanValue(const std::vector &descriptors, FBrief::TDescriptor &mean) { mean.reset(); if(descriptors.empty()) return; const int N2 = descriptors.size() / 2; const int L = descriptors[0]->size(); vector counters(L, 0); vector::const_iterator it; for(it = descriptors.begin(); it != descriptors.end(); ++it) { const FBrief::TDescriptor &desc = **it; for(int i = 0; i < L; ++i) { if(desc[i]) counters[i]++; } } for(int i = 0; i < L; ++i) { if(counters[i] > N2) mean.set(i); } } // -------------------------------------------------------------------------- double FBrief::distance(const FBrief::TDescriptor &a, const FBrief::TDescriptor &b) { return (double)DVision::BRIEF::distance(a, b); } // -------------------------------------------------------------------------- std::string FBrief::toString(const FBrief::TDescriptor &a) { // from boost::bitset string s; to_string(a, s); // reversed return s; } // -------------------------------------------------------------------------- void FBrief::fromString(FBrief::TDescriptor &a, const std::string &s) { // from boost::bitset stringstream ss(s); ss >> a; } // -------------------------------------------------------------------------- void FBrief::toMat32F(const std::vector &descriptors, cv::Mat &mat) { if(descriptors.empty()) { mat.release(); return; } const int N = descriptors.size(); const int L = descriptors[0].size(); mat.create(N, L, CV_32F); for(int i = 0; i < N; ++i) { const TDescriptor& desc = descriptors[i]; float *p = mat.ptr(i); for(int j = 0; j < L; ++j, ++p) { *p = (desc[j] ? 1 : 0); } } } // -------------------------------------------------------------------------- } // namespace DBoW2 ================================================ FILE: pose_graph/src/ThirdParty/DBoW/FBrief.h ================================================ /** * File: FBrief.h * Date: November 2011 * Author: Dorian Galvez-Lopez * Description: functions for BRIEF descriptors * License: see the LICENSE.txt file * */ #ifndef __D_T_F_BRIEF__ #define __D_T_F_BRIEF__ #include #include #include #include "FClass.h" #include "../DVision/DVision.h" namespace DBoW2 { /// Functions to manipulate BRIEF descriptors class FBrief: protected FClass { public: typedef DVision::BRIEF::bitset TDescriptor; typedef const TDescriptor *pDescriptor; /** * Calculates the mean value of a set of descriptors * @param descriptors * @param mean mean descriptor */ static void meanValue(const std::vector &descriptors, TDescriptor &mean); /** * Calculates the distance between two descriptors * @param a * @param b * @return distance */ static double distance(const TDescriptor &a, const TDescriptor &b); /** * Returns a string version of the descriptor * @param a descriptor * @return string version */ static std::string toString(const TDescriptor &a); /** * Returns a descriptor from a string * @param a descriptor * @param s string version */ static void fromString(TDescriptor &a, const std::string &s); /** * Returns a mat with the descriptors in float format * @param descriptors * @param mat (out) NxL 32F matrix */ static void toMat32F(const std::vector &descriptors, cv::Mat &mat); }; } // namespace DBoW2 #endif ================================================ FILE: pose_graph/src/ThirdParty/DBoW/FClass.h ================================================ /** * File: FClass.h * Date: November 2011 * Author: Dorian Galvez-Lopez * Description: generic FClass to instantiate templated classes * License: see the LICENSE.txt file * */ #ifndef __D_T_FCLASS__ #define __D_T_FCLASS__ #include #include #include namespace DBoW2 { /// Generic class to encapsulate functions to manage descriptors. /** * This class must be inherited. Derived classes can be used as the * parameter F when creating Templated structures * (TemplatedVocabulary, TemplatedDatabase, ...) */ class FClass { class TDescriptor; typedef const TDescriptor *pDescriptor; /** * Calculates the mean value of a set of descriptors * @param descriptors * @param mean mean descriptor */ virtual void meanValue(const std::vector &descriptors, TDescriptor &mean) = 0; /** * Calculates the distance between two descriptors * @param a * @param b * @return distance */ static double distance(const TDescriptor &a, const TDescriptor &b); /** * Returns a string version of the descriptor * @param a descriptor * @return string version */ static std::string toString(const TDescriptor &a); /** * Returns a descriptor from a string * @param a descriptor * @param s string version */ static void fromString(TDescriptor &a, const std::string &s); /** * Returns a mat with the descriptors in float format * @param descriptors * @param mat (out) NxL 32F matrix */ static void toMat32F(const std::vector &descriptors, cv::Mat &mat); }; } // namespace DBoW2 #endif ================================================ FILE: pose_graph/src/ThirdParty/DBoW/FeatureVector.cpp ================================================ /** * File: FeatureVector.cpp * Date: November 2011 * Author: Dorian Galvez-Lopez * Description: feature vector * License: see the LICENSE.txt file * */ #include "FeatureVector.h" #include #include #include namespace DBoW2 { // --------------------------------------------------------------------------- FeatureVector::FeatureVector(void) { } // --------------------------------------------------------------------------- FeatureVector::~FeatureVector(void) { } // --------------------------------------------------------------------------- void FeatureVector::addFeature(NodeId id, unsigned int i_feature) { FeatureVector::iterator vit = this->lower_bound(id); if(vit != this->end() && vit->first == id) { vit->second.push_back(i_feature); } else { vit = this->insert(vit, FeatureVector::value_type(id, std::vector() )); vit->second.push_back(i_feature); } } // --------------------------------------------------------------------------- std::ostream& operator<<(std::ostream &out, const FeatureVector &v) { if(!v.empty()) { FeatureVector::const_iterator vit = v.begin(); const std::vector* f = &vit->second; out << "<" << vit->first << ": ["; if(!f->empty()) out << (*f)[0]; for(unsigned int i = 1; i < f->size(); ++i) { out << ", " << (*f)[i]; } out << "]>"; for(++vit; vit != v.end(); ++vit) { f = &vit->second; out << ", <" << vit->first << ": ["; if(!f->empty()) out << (*f)[0]; for(unsigned int i = 1; i < f->size(); ++i) { out << ", " << (*f)[i]; } out << "]>"; } } return out; } // --------------------------------------------------------------------------- } // namespace DBoW2 ================================================ FILE: pose_graph/src/ThirdParty/DBoW/FeatureVector.h ================================================ /** * File: FeatureVector.h * Date: November 2011 * Author: Dorian Galvez-Lopez * Description: feature vector * License: see the LICENSE.txt file * */ #ifndef __D_T_FEATURE_VECTOR__ #define __D_T_FEATURE_VECTOR__ #include "BowVector.h" #include #include #include namespace DBoW2 { /// Vector of nodes with indexes of local features class FeatureVector: public std::map > { public: /** * Constructor */ FeatureVector(void); /** * Destructor */ ~FeatureVector(void); /** * Adds a feature to an existing node, or adds a new node with an initial * feature * @param id node id to add or to modify * @param i_feature index of feature to add to the given node */ void addFeature(NodeId id, unsigned int i_feature); /** * Sends a string versions of the feature vector through the stream * @param out stream * @param v feature vector */ friend std::ostream& operator<<(std::ostream &out, const FeatureVector &v); }; } // namespace DBoW2 #endif ================================================ FILE: pose_graph/src/ThirdParty/DBoW/QueryResults.cpp ================================================ /** * File: QueryResults.cpp * Date: March, November 2011 * Author: Dorian Galvez-Lopez * Description: structure to store results of database queries * License: see the LICENSE.txt file * */ #include #include #include "QueryResults.h" using namespace std; namespace DBoW2 { // --------------------------------------------------------------------------- ostream & operator<<(ostream& os, const Result& ret ) { os << ""; return os; } // --------------------------------------------------------------------------- ostream & operator<<(ostream& os, const QueryResults& ret ) { if(ret.size() == 1) os << "1 result:" << endl; else os << ret.size() << " results:" << endl; QueryResults::const_iterator rit; for(rit = ret.begin(); rit != ret.end(); ++rit) { os << *rit; if(rit + 1 != ret.end()) os << endl; } return os; } // --------------------------------------------------------------------------- void QueryResults::saveM(const std::string &filename) const { fstream f(filename.c_str(), ios::out); QueryResults::const_iterator qit; for(qit = begin(); qit != end(); ++qit) { f << qit->Id << " " << qit->Score << endl; } f.close(); } // --------------------------------------------------------------------------- } // namespace DBoW2 ================================================ FILE: pose_graph/src/ThirdParty/DBoW/QueryResults.h ================================================ /** * File: QueryResults.h * Date: March, November 2011 * Author: Dorian Galvez-Lopez * Description: structure to store results of database queries * License: see the LICENSE.txt file * */ #ifndef __D_T_QUERY_RESULTS__ #define __D_T_QUERY_RESULTS__ #include namespace DBoW2 { /// Id of entries of the database typedef unsigned int EntryId; /// Single result of a query class Result { public: /// Entry id EntryId Id; /// Score obtained double Score; /// debug int nWords; // words in common // !!! this is filled only by Bhatt score! // (and for BCMatching, BCThresholding then) double bhatScore, chiScore; /// debug // only done by ChiSq and BCThresholding double sumCommonVi; double sumCommonWi; double expectedChiScore; /// debug /** * Empty constructors */ inline Result(){} /** * Creates a result with the given data * @param _id entry id * @param _score score */ inline Result(EntryId _id, double _score): Id(_id), Score(_score){} /** * Compares the scores of two results * @return true iff this.score < r.score */ inline bool operator<(const Result &r) const { return this->Score < r.Score; } /** * Compares the scores of two results * @return true iff this.score > r.score */ inline bool operator>(const Result &r) const { return this->Score > r.Score; } /** * Compares the entry id of the result * @return true iff this.id == id */ inline bool operator==(EntryId id) const { return this->Id == id; } /** * Compares the score of this entry with a given one * @param s score to compare with * @return true iff this score < s */ inline bool operator<(double s) const { return this->Score < s; } /** * Compares the score of this entry with a given one * @param s score to compare with * @return true iff this score > s */ inline bool operator>(double s) const { return this->Score > s; } /** * Compares the score of two results * @param a * @param b * @return true iff a.Score > b.Score */ static inline bool gt(const Result &a, const Result &b) { return a.Score > b.Score; } /** * Compares the scores of two results * @return true iff a.Score > b.Score */ inline static bool ge(const Result &a, const Result &b) { return a.Score > b.Score; } /** * Returns true iff a.Score >= b.Score * @param a * @param b * @return true iff a.Score >= b.Score */ static inline bool geq(const Result &a, const Result &b) { return a.Score >= b.Score; } /** * Returns true iff a.Score >= s * @param a * @param s * @return true iff a.Score >= s */ static inline bool geqv(const Result &a, double s) { return a.Score >= s; } /** * Returns true iff a.Id < b.Id * @param a * @param b * @return true iff a.Id < b.Id */ static inline bool ltId(const Result &a, const Result &b) { return a.Id < b.Id; } /** * Prints a string version of the result * @param os ostream * @param ret Result to print */ friend std::ostream & operator<<(std::ostream& os, const Result& ret ); }; /// Multiple results from a query class QueryResults: public std::vector { public: /** * Multiplies all the scores in the vector by factor * @param factor */ inline void scaleScores(double factor); /** * Prints a string version of the results * @param os ostream * @param ret QueryResults to print */ friend std::ostream & operator<<(std::ostream& os, const QueryResults& ret ); /** * Saves a matlab file with the results * @param filename */ void saveM(const std::string &filename) const; }; // -------------------------------------------------------------------------- inline void QueryResults::scaleScores(double factor) { for(QueryResults::iterator qit = begin(); qit != end(); ++qit) qit->Score *= factor; } // -------------------------------------------------------------------------- } // namespace TemplatedBoW #endif ================================================ FILE: pose_graph/src/ThirdParty/DBoW/ScoringObject.cpp ================================================ /** * File: ScoringObject.cpp * Date: November 2011 * Author: Dorian Galvez-Lopez * Description: functions to compute bow scores * License: see the LICENSE.txt file * */ #include #include "TemplatedVocabulary.h" #include "BowVector.h" using namespace DBoW2; // If you change the type of WordValue, make sure you change also the // epsilon value (this is needed by the KL method) const double GeneralScoring::LOG_EPS = log(DBL_EPSILON); // FLT_EPSILON // --------------------------------------------------------------------------- // --------------------------------------------------------------------------- double L1Scoring::score(const BowVector &v1, const BowVector &v2) const { BowVector::const_iterator v1_it, v2_it; const BowVector::const_iterator v1_end = v1.end(); const BowVector::const_iterator v2_end = v2.end(); v1_it = v1.begin(); v2_it = v2.begin(); double score = 0; while(v1_it != v1_end && v2_it != v2_end) { const WordValue& vi = v1_it->second; const WordValue& wi = v2_it->second; if(v1_it->first == v2_it->first) { score += fabs(vi - wi) - fabs(vi) - fabs(wi); // move v1 and v2 forward ++v1_it; ++v2_it; } else if(v1_it->first < v2_it->first) { // move v1 forward v1_it = v1.lower_bound(v2_it->first); // v1_it = (first element >= v2_it.id) } else { // move v2 forward v2_it = v2.lower_bound(v1_it->first); // v2_it = (first element >= v1_it.id) } } // ||v - w||_{L1} = 2 + Sum(|v_i - w_i| - |v_i| - |w_i|) // for all i | v_i != 0 and w_i != 0 // (Nister, 2006) // scaled_||v - w||_{L1} = 1 - 0.5 * ||v - w||_{L1} score = -score/2.0; return score; // [0..1] } // --------------------------------------------------------------------------- // --------------------------------------------------------------------------- double L2Scoring::score(const BowVector &v1, const BowVector &v2) const { BowVector::const_iterator v1_it, v2_it; const BowVector::const_iterator v1_end = v1.end(); const BowVector::const_iterator v2_end = v2.end(); v1_it = v1.begin(); v2_it = v2.begin(); double score = 0; while(v1_it != v1_end && v2_it != v2_end) { const WordValue& vi = v1_it->second; const WordValue& wi = v2_it->second; if(v1_it->first == v2_it->first) { score += vi * wi; // move v1 and v2 forward ++v1_it; ++v2_it; } else if(v1_it->first < v2_it->first) { // move v1 forward v1_it = v1.lower_bound(v2_it->first); // v1_it = (first element >= v2_it.id) } else { // move v2 forward v2_it = v2.lower_bound(v1_it->first); // v2_it = (first element >= v1_it.id) } } // ||v - w||_{L2} = sqrt( 2 - 2 * Sum(v_i * w_i) ) // for all i | v_i != 0 and w_i != 0 ) // (Nister, 2006) if(score >= 1) // rounding errors score = 1.0; else score = 1.0 - sqrt(1.0 - score); // [0..1] return score; } // --------------------------------------------------------------------------- // --------------------------------------------------------------------------- double ChiSquareScoring::score(const BowVector &v1, const BowVector &v2) const { BowVector::const_iterator v1_it, v2_it; const BowVector::const_iterator v1_end = v1.end(); const BowVector::const_iterator v2_end = v2.end(); v1_it = v1.begin(); v2_it = v2.begin(); double score = 0; // all the items are taken into account while(v1_it != v1_end && v2_it != v2_end) { const WordValue& vi = v1_it->second; const WordValue& wi = v2_it->second; if(v1_it->first == v2_it->first) { // (v-w)^2/(v+w) - v - w = -4 vw/(v+w) // we move the -4 out if(vi + wi != 0.0) score += vi * wi / (vi + wi); // move v1 and v2 forward ++v1_it; ++v2_it; } else if(v1_it->first < v2_it->first) { // move v1 forward v1_it = v1.lower_bound(v2_it->first); } else { // move v2 forward v2_it = v2.lower_bound(v1_it->first); } } // this takes the -4 into account score = 2. * score; // [0..1] return score; } // --------------------------------------------------------------------------- // --------------------------------------------------------------------------- double KLScoring::score(const BowVector &v1, const BowVector &v2) const { BowVector::const_iterator v1_it, v2_it; const BowVector::const_iterator v1_end = v1.end(); const BowVector::const_iterator v2_end = v2.end(); v1_it = v1.begin(); v2_it = v2.begin(); double score = 0; // all the items or v are taken into account while(v1_it != v1_end && v2_it != v2_end) { const WordValue& vi = v1_it->second; const WordValue& wi = v2_it->second; if(v1_it->first == v2_it->first) { if(vi != 0 && wi != 0) score += vi * log(vi/wi); // move v1 and v2 forward ++v1_it; ++v2_it; } else if(v1_it->first < v2_it->first) { // move v1 forward score += vi * (log(vi) - LOG_EPS); ++v1_it; } else { // move v2_it forward, do not add any score v2_it = v2.lower_bound(v1_it->first); // v2_it = (first element >= v1_it.id) } } // sum rest of items of v for(; v1_it != v1_end; ++v1_it) if(v1_it->second != 0) score += v1_it->second * (log(v1_it->second) - LOG_EPS); return score; // cannot be scaled } // --------------------------------------------------------------------------- // --------------------------------------------------------------------------- double BhattacharyyaScoring::score(const BowVector &v1, const BowVector &v2) const { BowVector::const_iterator v1_it, v2_it; const BowVector::const_iterator v1_end = v1.end(); const BowVector::const_iterator v2_end = v2.end(); v1_it = v1.begin(); v2_it = v2.begin(); double score = 0; while(v1_it != v1_end && v2_it != v2_end) { const WordValue& vi = v1_it->second; const WordValue& wi = v2_it->second; if(v1_it->first == v2_it->first) { score += sqrt(vi * wi); // move v1 and v2 forward ++v1_it; ++v2_it; } else if(v1_it->first < v2_it->first) { // move v1 forward v1_it = v1.lower_bound(v2_it->first); // v1_it = (first element >= v2_it.id) } else { // move v2 forward v2_it = v2.lower_bound(v1_it->first); // v2_it = (first element >= v1_it.id) } } return score; // already scaled } // --------------------------------------------------------------------------- // --------------------------------------------------------------------------- double DotProductScoring::score(const BowVector &v1, const BowVector &v2) const { BowVector::const_iterator v1_it, v2_it; const BowVector::const_iterator v1_end = v1.end(); const BowVector::const_iterator v2_end = v2.end(); v1_it = v1.begin(); v2_it = v2.begin(); double score = 0; while(v1_it != v1_end && v2_it != v2_end) { const WordValue& vi = v1_it->second; const WordValue& wi = v2_it->second; if(v1_it->first == v2_it->first) { score += vi * wi; // move v1 and v2 forward ++v1_it; ++v2_it; } else if(v1_it->first < v2_it->first) { // move v1 forward v1_it = v1.lower_bound(v2_it->first); // v1_it = (first element >= v2_it.id) } else { // move v2 forward v2_it = v2.lower_bound(v1_it->first); // v2_it = (first element >= v1_it.id) } } return score; // cannot scale } // --------------------------------------------------------------------------- // --------------------------------------------------------------------------- ================================================ FILE: pose_graph/src/ThirdParty/DBoW/ScoringObject.h ================================================ /** * File: ScoringObject.h * Date: November 2011 * Author: Dorian Galvez-Lopez * Description: functions to compute bow scores * License: see the LICENSE.txt file * */ #ifndef __D_T_SCORING_OBJECT__ #define __D_T_SCORING_OBJECT__ #include "BowVector.h" namespace DBoW2 { /// Base class of scoring functions class GeneralScoring { public: /** * Computes the score between two vectors. Vectors must be sorted and * normalized if necessary * @param v (in/out) * @param w (in/out) * @return score */ virtual double score(const BowVector &v, const BowVector &w) const = 0; /** * Returns whether a vector must be normalized before scoring according * to the scoring scheme * @param norm norm to use * @return true iff must normalize */ virtual bool mustNormalize(LNorm &norm) const = 0; /// Log of epsilon static const double LOG_EPS; // If you change the type of WordValue, make sure you change also the // epsilon value (this is needed by the KL method) virtual ~GeneralScoring() {} //!< Required for virtual base classes }; /** * Macro for defining Scoring classes * @param NAME name of class * @param MUSTNORMALIZE if vectors must be normalized to compute the score * @param NORM type of norm to use when MUSTNORMALIZE */ #define __SCORING_CLASS(NAME, MUSTNORMALIZE, NORM) \ NAME: public GeneralScoring \ { public: \ /** \ * Computes score between two vectors \ * @param v \ * @param w \ * @return score between v and w \ */ \ virtual double score(const BowVector &v, const BowVector &w) const; \ \ /** \ * Says if a vector must be normalized according to the scoring function \ * @param norm (out) if true, norm to use * @return true iff vectors must be normalized \ */ \ virtual inline bool mustNormalize(LNorm &norm) const \ { norm = NORM; return MUSTNORMALIZE; } \ } /// L1 Scoring object class __SCORING_CLASS(L1Scoring, true, L1); /// L2 Scoring object class __SCORING_CLASS(L2Scoring, true, L2); /// Chi square Scoring object class __SCORING_CLASS(ChiSquareScoring, true, L1); /// KL divergence Scoring object class __SCORING_CLASS(KLScoring, true, L1); /// Bhattacharyya Scoring object class __SCORING_CLASS(BhattacharyyaScoring, true, L1); /// Dot product Scoring object class __SCORING_CLASS(DotProductScoring, false, L1); #undef __SCORING_CLASS } // namespace DBoW2 #endif ================================================ FILE: pose_graph/src/ThirdParty/DBoW/TemplatedDatabase.h ================================================ /** * File: TemplatedDatabase.h * Date: March 2011 * Author: Dorian Galvez-Lopez * Description: templated database of images * License: see the LICENSE.txt file * */ #ifndef __D_T_TEMPLATED_DATABASE__ #define __D_T_TEMPLATED_DATABASE__ #include #include #include #include #include #include #include "TemplatedVocabulary.h" #include "QueryResults.h" #include "ScoringObject.h" #include "BowVector.h" #include "FeatureVector.h" #include "../DUtils/DUtils.h" namespace DBoW2 { // For query functions static int MIN_COMMON_WORDS = 5; /// @param TDescriptor class of descriptor /// @param F class of descriptor functions template /// Generic Database class TemplatedDatabase { public: /** * Creates an empty database without vocabulary * @param use_di a direct index is used to store feature indexes * @param di_levels levels to go up the vocabulary tree to select the * node id to store in the direct index when adding images */ explicit TemplatedDatabase(bool use_di = true, int di_levels = 0); /** * Creates a database with the given vocabulary * @param T class inherited from TemplatedVocabulary * @param voc vocabulary * @param use_di a direct index is used to store feature indexes * @param di_levels levels to go up the vocabulary tree to select the * node id to store in the direct index when adding images */ template explicit TemplatedDatabase(const T &voc, bool use_di = true, int di_levels = 0); /** * Copy constructor. Copies the vocabulary too * @param db object to copy */ TemplatedDatabase(const TemplatedDatabase &db); /** * Creates the database from a file * @param filename */ TemplatedDatabase(const std::string &filename); /** * Creates the database from a file * @param filename */ TemplatedDatabase(const char *filename); /** * Destructor */ virtual ~TemplatedDatabase(void); /** * Copies the given database and its vocabulary * @param db database to copy */ TemplatedDatabase& operator=( const TemplatedDatabase &db); /** * Sets the vocabulary to use and clears the content of the database. * @param T class inherited from TemplatedVocabulary * @param voc vocabulary to copy */ template inline void setVocabulary(const T &voc); /** * Sets the vocabulary to use and the direct index parameters, and clears * the content of the database * @param T class inherited from TemplatedVocabulary * @param voc vocabulary to copy * @param use_di a direct index is used to store feature indexes * @param di_levels levels to go up the vocabulary tree to select the * node id to store in the direct index when adding images */ template void setVocabulary(const T& voc, bool use_di, int di_levels = 0); /** * Returns a pointer to the vocabulary used * @return vocabulary */ inline const TemplatedVocabulary* getVocabulary() const; /** * Allocates some memory for the direct and inverted indexes * @param nd number of expected image entries in the database * @param ni number of expected words per image * @note Use 0 to ignore a parameter */ void allocate(int nd = 0, int ni = 0); /** * Adds an entry to the database and returns its index * @param features features of the new entry * @param bowvec if given, the bow vector of these features is returned * @param fvec if given, the vector of nodes and feature indexes is returned * @return id of new entry */ EntryId add(const std::vector &features, BowVector *bowvec = NULL, FeatureVector *fvec = NULL); /** * Adss an entry to the database and returns its index * @param vec bow vector * @param fec feature vector to add the entry. Only necessary if using the * direct index * @return id of new entry */ EntryId add(const BowVector &vec, const FeatureVector &fec = FeatureVector() ); void delete_entry(const EntryId entry_id); /** * Empties the database */ inline void clear(); /** * Returns the number of entries in the database * @return number of entries in the database */ inline unsigned int size() const; /** * Checks if the direct index is being used * @return true iff using direct index */ inline bool usingDirectIndex() const; /** * Returns the di levels when using direct index * @return di levels */ inline int getDirectIndexLevels() const; /** * Queries the database with some features * @param features query features * @param ret (out) query results * @param max_results number of results to return. <= 0 means all * @param max_id only entries with id <= max_id are returned in ret. * < 0 means all */ void query(const std::vector &features, QueryResults &ret, int max_results = 1, int max_id = -1) const; /** * Queries the database with a vector * @param vec bow vector already normalized * @param ret results * @param max_results number of results to return. <= 0 means all * @param max_id only entries with id <= max_id are returned in ret. * < 0 means all */ void query(const BowVector &vec, QueryResults &ret, int max_results = 1, int max_id = -1) const; /** * Returns the a feature vector associated with a database entry * @param id entry id (must be < size()) * @return const reference to map of nodes and their associated features in * the given entry */ const FeatureVector& retrieveFeatures(EntryId id) const; /** * Stores the database in a file * @param filename */ void save(const std::string &filename) const; /** * Loads the database from a file * @param filename */ void load(const std::string &filename); /** * Stores the database in the given file storage structure * @param fs * @param name node name */ virtual void save(cv::FileStorage &fs, const std::string &name = "database") const; /** * Loads the database from the given file storage structure * @param fs * @param name node name */ virtual void load(const cv::FileStorage &fs, const std::string &name = "database"); protected: /// Query with L1 scoring void queryL1(const BowVector &vec, QueryResults &ret, int max_results, int max_id) const; /// Query with L2 scoring void queryL2(const BowVector &vec, QueryResults &ret, int max_results, int max_id) const; /// Query with Chi square scoring void queryChiSquare(const BowVector &vec, QueryResults &ret, int max_results, int max_id) const; /// Query with Bhattacharyya scoring void queryBhattacharyya(const BowVector &vec, QueryResults &ret, int max_results, int max_id) const; /// Query with KL divergence scoring void queryKL(const BowVector &vec, QueryResults &ret, int max_results, int max_id) const; /// Query with dot product scoring void queryDotProduct(const BowVector &vec, QueryResults &ret, int max_results, int max_id) const; protected: /* Inverted file declaration */ /// Item of IFRow struct IFPair { /// Entry id EntryId entry_id; /// Word weight in this entry WordValue word_weight; /** * Creates an empty pair */ IFPair(){} /** * Creates an inverted file pair * @param eid entry id * @param wv word weight */ IFPair(EntryId eid, WordValue wv): entry_id(eid), word_weight(wv) {} /** * Compares the entry ids * @param eid * @return true iff this entry id is the same as eid */ inline bool operator==(EntryId eid) const { return entry_id == eid; } }; /// Row of InvertedFile typedef std::list IFRow; // IFRows are sorted in ascending entry_id order /// Inverted index typedef std::vector InvertedFile; // InvertedFile[word_id] --> inverted file of that word /* Direct file declaration */ /// Direct index typedef std::vector DirectFile; // DirectFile[entry_id] --> [ directentry, ... ] protected: /// Associated vocabulary TemplatedVocabulary *m_voc; /// Flag to use direct index bool m_use_di; /// Levels to go up the vocabulary tree to select nodes to store /// in the direct index int m_dilevels; /// Inverted file (must have size() == |words|) InvertedFile m_ifile; /// Direct file (resized for allocation) DirectFile m_dfile; std::vector m_dBowfile; /// Number of valid entries in m_dfile int m_nentries; }; // -------------------------------------------------------------------------- template TemplatedDatabase::TemplatedDatabase (bool use_di, int di_levels) : m_voc(NULL), m_use_di(use_di), m_dilevels(di_levels), m_nentries(0) { } // -------------------------------------------------------------------------- template template TemplatedDatabase::TemplatedDatabase (const T &voc, bool use_di, int di_levels) : m_voc(NULL), m_use_di(use_di), m_dilevels(di_levels) { setVocabulary(voc); clear(); } // -------------------------------------------------------------------------- template TemplatedDatabase::TemplatedDatabase (const TemplatedDatabase &db) : m_voc(NULL) { *this = db; } // -------------------------------------------------------------------------- template TemplatedDatabase::TemplatedDatabase (const std::string &filename) : m_voc(NULL) { load(filename); } // -------------------------------------------------------------------------- template TemplatedDatabase::TemplatedDatabase (const char *filename) : m_voc(NULL) { load(filename); } // -------------------------------------------------------------------------- template TemplatedDatabase::~TemplatedDatabase(void) { delete m_voc; } // -------------------------------------------------------------------------- template TemplatedDatabase& TemplatedDatabase::operator= (const TemplatedDatabase &db) { if(this != &db) { m_dfile = db.m_dfile; m_dBowfile = db.m_dBowfile; m_dilevels = db.m_dilevels; m_ifile = db.m_ifile; m_nentries = db.m_nentries; m_use_di = db.m_use_di; setVocabulary(*db.m_voc); } return *this; } // -------------------------------------------------------------------------- template EntryId TemplatedDatabase::add( const std::vector &features, BowVector *bowvec, FeatureVector *fvec) { BowVector aux; BowVector& v = (bowvec ? *bowvec : aux); if(m_use_di && fvec != NULL) { m_voc->transform(features, v, *fvec, m_dilevels); // with features return add(v, *fvec); } else if(m_use_di) { FeatureVector fv; m_voc->transform(features, v, fv, m_dilevels); // with features return add(v, fv); } else if(fvec != NULL) { m_voc->transform(features, v, *fvec, m_dilevels); // with features return add(v); } else { m_voc->transform(features, v); // with features return add(v); } } // --------------------------------------------------------------------------- template EntryId TemplatedDatabase::add(const BowVector &v, const FeatureVector &fv) { EntryId entry_id = m_nentries++; BowVector::const_iterator vit; std::vector::const_iterator iit; if(m_use_di) { // update direct file if(entry_id == m_dfile.size()) { m_dfile.push_back(fv); m_dBowfile.push_back(v); } else { m_dfile[entry_id] = fv; m_dBowfile[entry_id] = v; } } // update inverted file for(vit = v.begin(); vit != v.end(); ++vit) { const WordId& word_id = vit->first; const WordValue& word_weight = vit->second; IFRow& ifrow = m_ifile[word_id]; ifrow.push_back(IFPair(entry_id, word_weight)); } return entry_id; } // --------------------------------------------------------------------------- template void TemplatedDatabase::delete_entry(const EntryId entry_id) { BowVector v = m_dBowfile[entry_id]; BowVector::const_iterator vit; for (vit = v.begin(); vit != v.end(); ++vit) { const WordId& word_id = vit->first; IFRow& ifrow = m_ifile[word_id]; typename IFRow::iterator rit; for (rit = ifrow.begin(); rit != ifrow.end(); ++rit) { if (rit->entry_id == entry_id) { ifrow.erase(rit); break; } } } m_dBowfile[entry_id].clear(); m_dfile[entry_id].clear(); } // -------------------------------------------------------------------------- template template inline void TemplatedDatabase::setVocabulary (const T& voc) { delete m_voc; m_voc = new T(voc); clear(); } // -------------------------------------------------------------------------- template template inline void TemplatedDatabase::setVocabulary (const T& voc, bool use_di, int di_levels) { m_use_di = use_di; m_dilevels = di_levels; delete m_voc; m_voc = new T(voc); clear(); } // -------------------------------------------------------------------------- template inline const TemplatedVocabulary* TemplatedDatabase::getVocabulary() const { return m_voc; } // -------------------------------------------------------------------------- template inline void TemplatedDatabase::clear() { // resize vectors m_ifile.resize(0); m_ifile.resize(m_voc->size()); m_dfile.resize(0); m_dBowfile.resize(0); m_nentries = 0; } // -------------------------------------------------------------------------- template void TemplatedDatabase::allocate(int nd, int ni) { // m_ifile already contains |words| items if(ni > 0) { typename std::vector::iterator rit; for(rit = m_ifile.begin(); rit != m_ifile.end(); ++rit) { int n = (int)rit->size(); if(ni > n) { rit->resize(ni); rit->resize(n); } } } if(m_use_di && (int)m_dfile.size() < nd) { m_dfile.resize(nd); m_dBowfile.resize(nd); } } // -------------------------------------------------------------------------- template inline unsigned int TemplatedDatabase::size() const { return m_nentries; } // -------------------------------------------------------------------------- template inline bool TemplatedDatabase::usingDirectIndex() const { return m_use_di; } // -------------------------------------------------------------------------- template inline int TemplatedDatabase::getDirectIndexLevels() const { return m_dilevels; } // -------------------------------------------------------------------------- template void TemplatedDatabase::query( const std::vector &features, QueryResults &ret, int max_results, int max_id) const { BowVector vec; m_voc->transform(features, vec); query(vec, ret, max_results, max_id); } // -------------------------------------------------------------------------- template void TemplatedDatabase::query( const BowVector &vec, QueryResults &ret, int max_results, int max_id) const { ret.resize(0); switch(m_voc->getScoringType()) { case L1_NORM: queryL1(vec, ret, max_results, max_id); break; case L2_NORM: queryL2(vec, ret, max_results, max_id); break; case CHI_SQUARE: queryChiSquare(vec, ret, max_results, max_id); break; case KL: queryKL(vec, ret, max_results, max_id); break; case BHATTACHARYYA: queryBhattacharyya(vec, ret, max_results, max_id); break; case DOT_PRODUCT: queryDotProduct(vec, ret, max_results, max_id); break; } } // -------------------------------------------------------------------------- template void TemplatedDatabase::queryL1(const BowVector &vec, QueryResults &ret, int max_results, int max_id) const { BowVector::const_iterator vit; typename IFRow::const_iterator rit; std::map pairs; std::map::iterator pit; for(vit = vec.begin(); vit != vec.end(); ++vit) { const WordId word_id = vit->first; const WordValue& qvalue = vit->second; const IFRow& row = m_ifile[word_id]; // IFRows are sorted in ascending entry_id order for(rit = row.begin(); rit != row.end(); ++rit) { const EntryId entry_id = rit->entry_id; const WordValue& dvalue = rit->word_weight; if((int)entry_id < max_id || max_id == -1 || (int)entry_id == m_nentries - 1) { double value = fabs(qvalue - dvalue) - fabs(qvalue) - fabs(dvalue); pit = pairs.lower_bound(entry_id); if(pit != pairs.end() && !(pairs.key_comp()(entry_id, pit->first))) { pit->second += value; } else { pairs.insert(pit, std::map::value_type(entry_id, value)); } } } // for each inverted row } // for each query word // move to vector ret.reserve(pairs.size()); for(pit = pairs.begin(); pit != pairs.end(); ++pit) { ret.push_back(Result(pit->first, pit->second)); } // resulting "scores" are now in [-2 best .. 0 worst] // sort vector in ascending order of score std::sort(ret.begin(), ret.end()); // (ret is inverted now --the lower the better--) // cut vector if(max_results > 0 && (int)ret.size() > max_results) ret.resize(max_results); // complete and scale score to [0 worst .. 1 best] // ||v - w||_{L1} = 2 + Sum(|v_i - w_i| - |v_i| - |w_i|) // for all i | v_i != 0 and w_i != 0 // (Nister, 2006) // scaled_||v - w||_{L1} = 1 - 0.5 * ||v - w||_{L1} QueryResults::iterator qit; for(qit = ret.begin(); qit != ret.end(); qit++) qit->Score = -qit->Score/2.0; } // -------------------------------------------------------------------------- template void TemplatedDatabase::queryL2(const BowVector &vec, QueryResults &ret, int max_results, int max_id) const { BowVector::const_iterator vit; typename IFRow::const_iterator rit; std::map pairs; std::map::iterator pit; //map counters; //map::iterator cit; for(vit = vec.begin(); vit != vec.end(); ++vit) { const WordId word_id = vit->first; const WordValue& qvalue = vit->second; const IFRow& row = m_ifile[word_id]; // IFRows are sorted in ascending entry_id order for(rit = row.begin(); rit != row.end(); ++rit) { const EntryId entry_id = rit->entry_id; const WordValue& dvalue = rit->word_weight; if((int)entry_id < max_id || max_id == -1) { double value = - qvalue * dvalue; // minus sign for sorting trick pit = pairs.lower_bound(entry_id); //cit = counters.lower_bound(entry_id); if(pit != pairs.end() && !(pairs.key_comp()(entry_id, pit->first))) { pit->second += value; //cit->second += 1; } else { pairs.insert(pit, std::map::value_type(entry_id, value)); //counters.insert(cit, // map::value_type(entry_id, 1)); } } } // for each inverted row } // for each query word // move to vector ret.reserve(pairs.size()); //cit = counters.begin(); for(pit = pairs.begin(); pit != pairs.end(); ++pit)//, ++cit) { ret.push_back(Result(pit->first, pit->second));// / cit->second)); } // resulting "scores" are now in [-1 best .. 0 worst] // sort vector in ascending order of score std::sort(ret.begin(), ret.end()); // (ret is inverted now --the lower the better--) // cut vector if(max_results > 0 && (int)ret.size() > max_results) ret.resize(max_results); // complete and scale score to [0 worst .. 1 best] // ||v - w||_{L2} = sqrt( 2 - 2 * Sum(v_i * w_i) // for all i | v_i != 0 and w_i != 0 ) // (Nister, 2006) QueryResults::iterator qit; for(qit = ret.begin(); qit != ret.end(); qit++) { if(qit->Score <= -1.0) // rounding error qit->Score = 1.0; else qit->Score = 1.0 - sqrt(1.0 + qit->Score); // [0..1] // the + sign is ok, it is due to - sign in // value = - qvalue * dvalue } } // -------------------------------------------------------------------------- template void TemplatedDatabase::queryChiSquare(const BowVector &vec, QueryResults &ret, int max_results, int max_id) const { BowVector::const_iterator vit; typename IFRow::const_iterator rit; std::map > pairs; std::map >::iterator pit; std::map > sums; // < sum vi, sum wi > std::map >::iterator sit; // In the current implementation, we suppose vec is not normalized //map expected; //map::iterator eit; for(vit = vec.begin(); vit != vec.end(); ++vit) { const WordId word_id = vit->first; const WordValue& qvalue = vit->second; const IFRow& row = m_ifile[word_id]; // IFRows are sorted in ascending entry_id order for(rit = row.begin(); rit != row.end(); ++rit) { const EntryId entry_id = rit->entry_id; const WordValue& dvalue = rit->word_weight; if((int)entry_id < max_id || max_id == -1) { // (v-w)^2/(v+w) - v - w = -4 vw/(v+w) // we move the 4 out double value = 0; if(qvalue + dvalue != 0.0) // words may have weight zero value = - qvalue * dvalue / (qvalue + dvalue); pit = pairs.lower_bound(entry_id); sit = sums.lower_bound(entry_id); //eit = expected.lower_bound(entry_id); if(pit != pairs.end() && !(pairs.key_comp()(entry_id, pit->first))) { pit->second.first += value; pit->second.second += 1; //eit->second += dvalue; sit->second.first += qvalue; sit->second.second += dvalue; } else { pairs.insert(pit, std::map >::value_type(entry_id, std::make_pair(value, 1) )); //expected.insert(eit, // map::value_type(entry_id, dvalue)); sums.insert(sit, std::map >::value_type(entry_id, std::make_pair(qvalue, dvalue) )); } } } // for each inverted row } // for each query word // move to vector ret.reserve(pairs.size()); sit = sums.begin(); for(pit = pairs.begin(); pit != pairs.end(); ++pit, ++sit) { if(pit->second.second >= MIN_COMMON_WORDS) { ret.push_back(Result(pit->first, pit->second.first)); ret.back().nWords = pit->second.second; ret.back().sumCommonVi = sit->second.first; ret.back().sumCommonWi = sit->second.second; ret.back().expectedChiScore = 2 * sit->second.second / (1 + sit->second.second); } //ret.push_back(Result(pit->first, pit->second)); } // resulting "scores" are now in [-2 best .. 0 worst] // we have to add +2 to the scores to obtain the chi square score // sort vector in ascending order of score std::sort(ret.begin(), ret.end()); // (ret is inverted now --the lower the better--) // cut vector if(max_results > 0 && (int)ret.size() > max_results) ret.resize(max_results); // complete and scale score to [0 worst .. 1 best] QueryResults::iterator qit; for(qit = ret.begin(); qit != ret.end(); qit++) { // this takes the 4 into account qit->Score = - 2. * qit->Score; // [0..1] qit->chiScore = qit->Score; } } // -------------------------------------------------------------------------- template void TemplatedDatabase::queryKL(const BowVector &vec, QueryResults &ret, int max_results, int max_id) const { BowVector::const_iterator vit; typename IFRow::const_iterator rit; std::map pairs; std::map::iterator pit; for(vit = vec.begin(); vit != vec.end(); ++vit) { const WordId word_id = vit->first; const WordValue& vi = vit->second; const IFRow& row = m_ifile[word_id]; // IFRows are sorted in ascending entry_id order for(rit = row.begin(); rit != row.end(); ++rit) { const EntryId entry_id = rit->entry_id; const WordValue& wi = rit->word_weight; if((int)entry_id < max_id || max_id == -1) { double value = 0; if(vi != 0 && wi != 0) value = vi * log(vi/wi); pit = pairs.lower_bound(entry_id); if(pit != pairs.end() && !(pairs.key_comp()(entry_id, pit->first))) { pit->second += value; } else { pairs.insert(pit, std::map::value_type(entry_id, value)); } } } // for each inverted row } // for each query word // resulting "scores" are now in [-X worst .. 0 best .. X worst] // but we cannot make sure which ones are better without calculating // the complete score // complete scores and move to vector ret.reserve(pairs.size()); for(pit = pairs.begin(); pit != pairs.end(); ++pit) { EntryId eid = pit->first; double value = 0.0; for(vit = vec.begin(); vit != vec.end(); ++vit) { const WordValue &vi = vit->second; const IFRow& row = m_ifile[vit->first]; if(vi != 0) { if(row.end() == find(row.begin(), row.end(), eid )) { value += vi * (log(vi) - GeneralScoring::LOG_EPS); } } } pit->second += value; // to vector ret.push_back(Result(pit->first, pit->second)); } // real scores are now in [0 best .. X worst] // sort vector in ascending order // (scores are inverted now --the lower the better--) std::sort(ret.begin(), ret.end()); // cut vector if(max_results > 0 && (int)ret.size() > max_results) ret.resize(max_results); // cannot scale scores } // -------------------------------------------------------------------------- template void TemplatedDatabase::queryBhattacharyya( const BowVector &vec, QueryResults &ret, int max_results, int max_id) const { BowVector::const_iterator vit; typename IFRow::const_iterator rit; //map pairs; //map::iterator pit; std::map > pairs; // > std::map >::iterator pit; for(vit = vec.begin(); vit != vec.end(); ++vit) { const WordId word_id = vit->first; const WordValue& qvalue = vit->second; const IFRow& row = m_ifile[word_id]; // IFRows are sorted in ascending entry_id order for(rit = row.begin(); rit != row.end(); ++rit) { const EntryId entry_id = rit->entry_id; const WordValue& dvalue = rit->word_weight; if((int)entry_id < max_id || max_id == -1) { double value = sqrt(qvalue * dvalue); pit = pairs.lower_bound(entry_id); if(pit != pairs.end() && !(pairs.key_comp()(entry_id, pit->first))) { pit->second.first += value; pit->second.second += 1; } else { pairs.insert(pit, std::map >::value_type(entry_id, std::make_pair(value, 1))); } } } // for each inverted row } // for each query word // move to vector ret.reserve(pairs.size()); for(pit = pairs.begin(); pit != pairs.end(); ++pit) { if(pit->second.second >= MIN_COMMON_WORDS) { ret.push_back(Result(pit->first, pit->second.first)); ret.back().nWords = pit->second.second; ret.back().bhatScore = pit->second.first; } } // scores are already in [0..1] // sort vector in descending order std::sort(ret.begin(), ret.end(), Result::gt); // cut vector if(max_results > 0 && (int)ret.size() > max_results) ret.resize(max_results); } // --------------------------------------------------------------------------- template void TemplatedDatabase::queryDotProduct( const BowVector &vec, QueryResults &ret, int max_results, int max_id) const { BowVector::const_iterator vit; typename IFRow::const_iterator rit; std::map pairs; std::map::iterator pit; for(vit = vec.begin(); vit != vec.end(); ++vit) { const WordId word_id = vit->first; const WordValue& qvalue = vit->second; const IFRow& row = m_ifile[word_id]; // IFRows are sorted in ascending entry_id order for(rit = row.begin(); rit != row.end(); ++rit) { const EntryId entry_id = rit->entry_id; const WordValue& dvalue = rit->word_weight; if((int)entry_id < max_id || max_id == -1) { double value; if(this->m_voc->getWeightingType() == BINARY) value = 1; else value = qvalue * dvalue; pit = pairs.lower_bound(entry_id); if(pit != pairs.end() && !(pairs.key_comp()(entry_id, pit->first))) { pit->second += value; } else { pairs.insert(pit, std::map::value_type(entry_id, value)); } } } // for each inverted row } // for each query word // move to vector ret.reserve(pairs.size()); for(pit = pairs.begin(); pit != pairs.end(); ++pit) { ret.push_back(Result(pit->first, pit->second)); } // scores are the greater the better // sort vector in descending order std::sort(ret.begin(), ret.end(), Result::gt); // cut vector if(max_results > 0 && (int)ret.size() > max_results) ret.resize(max_results); // these scores cannot be scaled } // --------------------------------------------------------------------------- template const FeatureVector& TemplatedDatabase::retrieveFeatures (EntryId id) const { assert(id < size()); return m_dfile[id]; } // -------------------------------------------------------------------------- template void TemplatedDatabase::save(const std::string &filename) const { cv::FileStorage fs(filename.c_str(), cv::FileStorage::WRITE); if(!fs.isOpened()) throw std::string("Could not open file ") + filename; save(fs); } // -------------------------------------------------------------------------- template void TemplatedDatabase::save(cv::FileStorage &fs, const std::string &name) const { // Format YAML: // vocabulary { ... see TemplatedVocabulary::save } // database // { // nEntries: // usingDI: // diLevels: // invertedIndex // [ // [ // { // imageId: // weight: // } // ] // ] // directIndex // [ // [ // { // nodeId: // features: [ ] // } // ] // ] // invertedIndex[i] is for the i-th word // directIndex[i] is for the i-th entry // directIndex may be empty if not using direct index // // imageId's and nodeId's must be stored in ascending order // (according to the construction of the indexes) m_voc->save(fs); fs << name << "{"; fs << "nEntries" << m_nentries; fs << "usingDI" << (m_use_di ? 1 : 0); fs << "diLevels" << m_dilevels; fs << "invertedIndex" << "["; typename InvertedFile::const_iterator iit; typename IFRow::const_iterator irit; for(iit = m_ifile.begin(); iit != m_ifile.end(); ++iit) { fs << "["; // word of IF for(irit = iit->begin(); irit != iit->end(); ++irit) { fs << "{:" << "imageId" << (int)irit->entry_id << "weight" << irit->word_weight << "}"; } fs << "]"; // word of IF } fs << "]"; // invertedIndex fs << "directIndex" << "["; typename DirectFile::const_iterator dit; typename FeatureVector::const_iterator drit; for(dit = m_dfile.begin(); dit != m_dfile.end(); ++dit) { fs << "["; // entry of DF for(drit = dit->begin(); drit != dit->end(); ++drit) { NodeId nid = drit->first; const std::vector& features = drit->second; // save info of last_nid fs << "{"; fs << "nodeId" << (int)nid; // msvc++ 2010 with opencv 2.3.1 does not allow FileStorage::operator<< // with vectors of unsigned int fs << "features" << "[" << *(const std::vector*)(&features) << "]"; fs << "}"; } fs << "]"; // entry of DF } fs << "]"; // directIndex fs << "}"; // database } // -------------------------------------------------------------------------- template void TemplatedDatabase::load(const std::string &filename) { cv::FileStorage fs(filename.c_str(), cv::FileStorage::READ); if(!fs.isOpened()) throw std::string("Could not open file ") + filename; load(fs); } // -------------------------------------------------------------------------- template void TemplatedDatabase::load(const cv::FileStorage &fs, const std::string &name) { // load voc first // subclasses must instantiate m_voc before calling this ::load if(!m_voc) m_voc = new TemplatedVocabulary; m_voc->load(fs); // load database now clear(); // resizes inverted file cv::FileNode fdb = fs[name]; m_nentries = (int)fdb["nEntries"]; m_use_di = (int)fdb["usingDI"] != 0; m_dilevels = (int)fdb["diLevels"]; cv::FileNode fn = fdb["invertedIndex"]; for(WordId wid = 0; wid < fn.size(); ++wid) { cv::FileNode fw = fn[wid]; for(unsigned int i = 0; i < fw.size(); ++i) { EntryId eid = (int)fw[i]["imageId"]; WordValue v = fw[i]["weight"]; m_ifile[wid].push_back(IFPair(eid, v)); } } if(m_use_di) { fn = fdb["directIndex"]; m_dfile.resize(fn.size()); m_dBowfile.resize(fn.size()); assert(m_nentries == (int)fn.size()); FeatureVector::iterator dit; for(EntryId eid = 0; eid < fn.size(); ++eid) { cv::FileNode fe = fn[eid]; m_dfile[eid].clear(); m_dBowfile[eid].clear(); for(unsigned int i = 0; i < fe.size(); ++i) { NodeId nid = (int)fe[i]["nodeId"]; dit = m_dfile[eid].insert(m_dfile[eid].end(), make_pair(nid, std::vector() )); // this failed to compile with some opencv versions (2.3.1) //fe[i]["features"] >> dit->second; // this was ok until OpenCV 2.4.1 //std::vector aux; //fe[i]["features"] >> aux; // OpenCV < 2.4.1 //dit->second.resize(aux.size()); //std::copy(aux.begin(), aux.end(), dit->second.begin()); cv::FileNode ff = fe[i]["features"][0]; dit->second.reserve(ff.size()); cv::FileNodeIterator ffit; for(ffit = ff.begin(); ffit != ff.end(); ++ffit) { dit->second.push_back((int)*ffit); } } } // for each entry } // if use_id } // -------------------------------------------------------------------------- /** * Writes printable information of the database * @param os stream to write to * @param db */ template std::ostream& operator<<(std::ostream &os, const TemplatedDatabase &db) { os << "Database: Entries = " << db.size() << ", " "Using direct index = " << (db.usingDirectIndex() ? "yes" : "no"); if(db.usingDirectIndex()) os << ", Direct index levels = " << db.getDirectIndexLevels(); os << ". " << *db.getVocabulary(); return os; } // -------------------------------------------------------------------------- } // namespace DBoW2 #endif ================================================ FILE: pose_graph/src/ThirdParty/DBoW/TemplatedVocabulary.h ================================================ /** * File: TemplatedVocabulary.h * Date: February 2011 * Author: Dorian Galvez-Lopez * Description: templated vocabulary * License: see the LICENSE.txt file * */ #ifndef __D_T_TEMPLATED_VOCABULARY__ #define __D_T_TEMPLATED_VOCABULARY__ #include #include #include #include #include #include #include #include "FeatureVector.h" #include "BowVector.h" #include "ScoringObject.h" #include "../DUtils/DUtils.h" // Added by VINS [[[ #include "../VocabularyBinary.hpp" #include // Added by VINS ]]] namespace DBoW2 { /// @param TDescriptor class of descriptor /// @param F class of descriptor functions template /// Generic Vocabulary class TemplatedVocabulary { public: /** * Initiates an empty vocabulary * @param k branching factor * @param L depth levels * @param weighting weighting type * @param scoring scoring type */ TemplatedVocabulary(int k = 10, int L = 5, WeightingType weighting = TF_IDF, ScoringType scoring = L1_NORM); /** * Creates the vocabulary by loading a file * @param filename */ TemplatedVocabulary(const std::string &filename); /** * Creates the vocabulary by loading a file * @param filename */ TemplatedVocabulary(const char *filename); /** * Copy constructor * @param voc */ TemplatedVocabulary(const TemplatedVocabulary &voc); /** * Destructor */ virtual ~TemplatedVocabulary(); /** * Assigns the given vocabulary to this by copying its data and removing * all the data contained by this vocabulary before * @param voc * @return reference to this vocabulary */ TemplatedVocabulary& operator=( const TemplatedVocabulary &voc); /** * Creates a vocabulary from the training features with the already * defined parameters * @param training_features */ virtual void create (const std::vector > &training_features); /** * Creates a vocabulary from the training features, setting the branching * factor and the depth levels of the tree * @param training_features * @param k branching factor * @param L depth levels */ virtual void create (const std::vector > &training_features, int k, int L); /** * Creates a vocabulary from the training features, setting the branching * factor nad the depth levels of the tree, and the weighting and scoring * schemes */ virtual void create (const std::vector > &training_features, int k, int L, WeightingType weighting, ScoringType scoring); /** * Returns the number of words in the vocabulary * @return number of words */ virtual inline unsigned int size() const; /** * Returns whether the vocabulary is empty (i.e. it has not been trained) * @return true iff the vocabulary is empty */ virtual inline bool empty() const; /** * Transforms a set of descriptores into a bow vector * @param features * @param v (out) bow vector of weighted words */ virtual void transform(const std::vector& features, BowVector &v) const; /** * Transform a set of descriptors into a bow vector and a feature vector * @param features * @param v (out) bow vector * @param fv (out) feature vector of nodes and feature indexes * @param levelsup levels to go up the vocabulary tree to get the node index */ virtual void transform(const std::vector& features, BowVector &v, FeatureVector &fv, int levelsup) const; /** * Transforms a single feature into a word (without weight) * @param feature * @return word id */ virtual WordId transform(const TDescriptor& feature) const; /** * Returns the score of two vectors * @param a vector * @param b vector * @return score between vectors * @note the vectors must be already sorted and normalized if necessary */ inline double score(const BowVector &a, const BowVector &b) const; /** * Returns the id of the node that is "levelsup" levels from the word given * @param wid word id * @param levelsup 0..L * @return node id. if levelsup is 0, returns the node id associated to the * word id */ virtual NodeId getParentNode(WordId wid, int levelsup) const; /** * Returns the ids of all the words that are under the given node id, * by traversing any of the branches that goes down from the node * @param nid starting node id * @param words ids of words */ void getWordsFromNode(NodeId nid, std::vector &words) const; /** * Returns the branching factor of the tree (k) * @return k */ inline int getBranchingFactor() const { return m_k; } /** * Returns the depth levels of the tree (L) * @return L */ inline int getDepthLevels() const { return m_L; } /** * Returns the real depth levels of the tree on average * @return average of depth levels of leaves */ float getEffectiveLevels() const; /** * Returns the descriptor of a word * @param wid word id * @return descriptor */ virtual inline TDescriptor getWord(WordId wid) const; /** * Returns the weight of a word * @param wid word id * @return weight */ virtual inline WordValue getWordWeight(WordId wid) const; /** * Returns the weighting method * @return weighting method */ inline WeightingType getWeightingType() const { return m_weighting; } /** * Returns the scoring method * @return scoring method */ inline ScoringType getScoringType() const { return m_scoring; } /** * Changes the weighting method * @param type new weighting type */ inline void setWeightingType(WeightingType type); /** * Changes the scoring method * @param type new scoring type */ void setScoringType(ScoringType type); /** * Saves the vocabulary into a file * @param filename */ void save(const std::string &filename) const; /** * Loads the vocabulary from a file * @param filename */ void load(const std::string &filename); /** * Saves the vocabulary to a file storage structure * @param fn node in file storage */ virtual void save(cv::FileStorage &fs, const std::string &name = "vocabulary") const; /** * Loads the vocabulary from a file storage node * @param fn first node * @param subname name of the child node of fn where the tree is stored. * If not given, the fn node is used instead */ virtual void load(const cv::FileStorage &fs, const std::string &name = "vocabulary"); // Added by VINS [[[ virtual void loadBin(const std::string &filename); // Added by VINS ]]] /** * Stops those words whose weight is below minWeight. * Words are stopped by setting their weight to 0. There are not returned * later when transforming image features into vectors. * Note that when using IDF or TF_IDF, the weight is the idf part, which * is equivalent to -log(f), where f is the frequency of the word * (f = Ni/N, Ni: number of training images where the word is present, * N: number of training images). * Note that the old weight is forgotten, and subsequent calls to this * function with a lower minWeight have no effect. * @return number of words stopped now */ virtual int stopWords(double minWeight); protected: /// Pointer to descriptor typedef const TDescriptor *pDescriptor; /// Tree node struct Node { /// Node id NodeId id; /// Weight if the node is a word WordValue weight; /// Children std::vector children; /// Parent node (undefined in case of root) NodeId parent; /// Node descriptor TDescriptor descriptor; /// Word id if the node is a word WordId word_id; /** * Empty constructor */ Node(): id(0), weight(0), parent(0), word_id(0){} /** * Constructor * @param _id node id */ Node(NodeId _id): id(_id), weight(0), parent(0), word_id(0){} /** * Returns whether the node is a leaf node * @return true iff the node is a leaf */ inline bool isLeaf() const { return children.empty(); } }; protected: /** * Creates an instance of the scoring object accoring to m_scoring */ void createScoringObject(); /** * Returns a set of pointers to descriptores * @param training_features all the features * @param features (out) pointers to the training features */ void getFeatures( const std::vector > &training_features, std::vector &features) const; /** * Returns the word id associated to a feature * @param feature * @param id (out) word id * @param weight (out) word weight * @param nid (out) if given, id of the node "levelsup" levels up * @param levelsup */ virtual void transform(const TDescriptor &feature, WordId &id, WordValue &weight, NodeId* nid = NULL, int levelsup = 0) const; /** * Returns the word id associated to a feature * @param feature * @param id (out) word id */ virtual void transform(const TDescriptor &feature, WordId &id) const; /** * Creates a level in the tree, under the parent, by running kmeans with * a descriptor set, and recursively creates the subsequent levels too * @param parent_id id of parent node * @param descriptors descriptors to run the kmeans on * @param current_level current level in the tree */ void HKmeansStep(NodeId parent_id, const std::vector &descriptors, int current_level); /** * Creates k clusters from the given descriptors with some seeding algorithm. * @note In this class, kmeans++ is used, but this function should be * overriden by inherited classes. */ virtual void initiateClusters(const std::vector &descriptors, std::vector &clusters) const; /** * Creates k clusters from the given descriptor sets by running the * initial step of kmeans++ * @param descriptors * @param clusters resulting clusters */ void initiateClustersKMpp(const std::vector &descriptors, std::vector &clusters) const; /** * Create the words of the vocabulary once the tree has been built */ void createWords(); /** * Sets the weights of the nodes of tree according to the given features. * Before calling this function, the nodes and the words must be already * created (by calling HKmeansStep and createWords) * @param features */ void setNodeWeights(const std::vector > &features); protected: /// Branching factor int m_k; /// Depth levels int m_L; /// Weighting method WeightingType m_weighting; /// Scoring method ScoringType m_scoring; /// Object for computing scores GeneralScoring* m_scoring_object; /// Tree nodes std::vector m_nodes; /// Words of the vocabulary (tree leaves) /// this condition holds: m_words[wid]->word_id == wid std::vector m_words; }; // -------------------------------------------------------------------------- template TemplatedVocabulary::TemplatedVocabulary (int k, int L, WeightingType weighting, ScoringType scoring) : m_k(k), m_L(L), m_weighting(weighting), m_scoring(scoring), m_scoring_object(NULL) { //printf("loop start load bin\n"); createScoringObject(); } // -------------------------------------------------------------------------- template TemplatedVocabulary::TemplatedVocabulary (const std::string &filename): m_scoring_object(NULL) { //m_scoring = KL; // Changed by VINS [[[ //printf("loop start load bin\n"); loadBin(filename); // Changed by VINS ]]] } // -------------------------------------------------------------------------- template TemplatedVocabulary::TemplatedVocabulary (const char *filename): m_scoring_object(NULL) { //m_scoring = KL; // Changed by VINS [[[ //printf("loop start load bin\n"); loadBin(filename); // Changed by VINS ]]] } // -------------------------------------------------------------------------- template void TemplatedVocabulary::createScoringObject() { delete m_scoring_object; m_scoring_object = NULL; switch(m_scoring) { case L1_NORM: m_scoring_object = new L1Scoring; break; case L2_NORM: m_scoring_object = new L2Scoring; break; case CHI_SQUARE: m_scoring_object = new ChiSquareScoring; break; case KL: m_scoring_object = new KLScoring; break; case BHATTACHARYYA: m_scoring_object = new BhattacharyyaScoring; break; case DOT_PRODUCT: m_scoring_object = new DotProductScoring; break; } } // -------------------------------------------------------------------------- template void TemplatedVocabulary::setScoringType(ScoringType type) { m_scoring = type; createScoringObject(); } // -------------------------------------------------------------------------- template void TemplatedVocabulary::setWeightingType(WeightingType type) { this->m_weighting = type; } // -------------------------------------------------------------------------- template TemplatedVocabulary::TemplatedVocabulary( const TemplatedVocabulary &voc) : m_scoring_object(NULL) { printf("loop start load vocabulary\n"); *this = voc; } // -------------------------------------------------------------------------- template TemplatedVocabulary::~TemplatedVocabulary() { delete m_scoring_object; } // -------------------------------------------------------------------------- template TemplatedVocabulary& TemplatedVocabulary::operator= (const TemplatedVocabulary &voc) { this->m_k = voc.m_k; this->m_L = voc.m_L; this->m_scoring = voc.m_scoring; this->m_weighting = voc.m_weighting; this->createScoringObject(); this->m_nodes.clear(); this->m_words.clear(); this->m_nodes = voc.m_nodes; this->createWords(); return *this; } // -------------------------------------------------------------------------- template void TemplatedVocabulary::create( const std::vector > &training_features) { m_nodes.clear(); m_words.clear(); // expected_nodes = Sum_{i=0..L} ( k^i ) int expected_nodes = (int)((pow((double)m_k, (double)m_L + 1) - 1)/(m_k - 1)); m_nodes.reserve(expected_nodes); // avoid allocations when creating the tree std::vector features; getFeatures(training_features, features); // create root m_nodes.push_back(Node(0)); // root // create the tree HKmeansStep(0, features, 1); // create the words createWords(); // and set the weight of each node of the tree setNodeWeights(training_features); } // -------------------------------------------------------------------------- template void TemplatedVocabulary::create( const std::vector > &training_features, int k, int L) { m_k = k; m_L = L; create(training_features); } // -------------------------------------------------------------------------- template void TemplatedVocabulary::create( const std::vector > &training_features, int k, int L, WeightingType weighting, ScoringType scoring) { m_k = k; m_L = L; m_weighting = weighting; m_scoring = scoring; createScoringObject(); create(training_features); } // -------------------------------------------------------------------------- template void TemplatedVocabulary::getFeatures( const std::vector > &training_features, std::vector &features) const { features.resize(0); typename std::vector >::const_iterator vvit; typename std::vector::const_iterator vit; for(vvit = training_features.begin(); vvit != training_features.end(); ++vvit) { features.reserve(features.size() + vvit->size()); for(vit = vvit->begin(); vit != vvit->end(); ++vit) { features.push_back(&(*vit)); } } } // -------------------------------------------------------------------------- template void TemplatedVocabulary::HKmeansStep(NodeId parent_id, const std::vector &descriptors, int current_level) { if(descriptors.empty()) return; // features associated to each cluster std::vector clusters; std::vector > groups; // groups[i] = [j1, j2, ...] // j1, j2, ... indices of descriptors associated to cluster i clusters.reserve(m_k); groups.reserve(m_k); //const int msizes[] = { m_k, descriptors.size() }; //cv::SparseMat assoc(2, msizes, CV_8U); //cv::SparseMat last_assoc(2, msizes, CV_8U); //// assoc.row(cluster_idx).col(descriptor_idx) = 1 iif associated if((int)descriptors.size() <= m_k) { // trivial case: one cluster per feature groups.resize(descriptors.size()); for(unsigned int i = 0; i < descriptors.size(); i++) { groups[i].push_back(i); clusters.push_back(*descriptors[i]); } } else { // select clusters and groups with kmeans bool first_time = true; bool goon = true; // to check if clusters move after iterations std::vector last_association, current_association; while(goon) { // 1. Calculate clusters if(first_time) { // random sample initiateClusters(descriptors, clusters); } else { // calculate cluster centres for(unsigned int c = 0; c < clusters.size(); ++c) { std::vector cluster_descriptors; cluster_descriptors.reserve(groups[c].size()); /* for(unsigned int d = 0; d < descriptors.size(); ++d) { if( assoc.find(c, d) ) { cluster_descriptors.push_back(descriptors[d]); } } */ std::vector::const_iterator vit; for(vit = groups[c].begin(); vit != groups[c].end(); ++vit) { cluster_descriptors.push_back(descriptors[*vit]); } F::meanValue(cluster_descriptors, clusters[c]); } } // if(!first_time) // 2. Associate features with clusters // calculate distances to cluster centers groups.clear(); groups.resize(clusters.size(), std::vector()); current_association.resize(descriptors.size()); //assoc.clear(); typename std::vector::const_iterator fit; //unsigned int d = 0; for(fit = descriptors.begin(); fit != descriptors.end(); ++fit)//, ++d) { double best_dist = F::distance(*(*fit), clusters[0]); unsigned int icluster = 0; for(unsigned int c = 1; c < clusters.size(); ++c) { double dist = F::distance(*(*fit), clusters[c]); if(dist < best_dist) { best_dist = dist; icluster = c; } } //assoc.ref(icluster, d) = 1; groups[icluster].push_back(fit - descriptors.begin()); current_association[ fit - descriptors.begin() ] = icluster; } // kmeans++ ensures all the clusters has any feature associated with them // 3. check convergence if(first_time) { first_time = false; } else { //goon = !eqUChar(last_assoc, assoc); goon = false; for(unsigned int i = 0; i < current_association.size(); i++) { if(current_association[i] != last_association[i]){ goon = true; break; } } } if(goon) { // copy last feature-cluster association last_association = current_association; //last_assoc = assoc.clone(); } } // while(goon) } // if must run kmeans // create nodes for(unsigned int i = 0; i < clusters.size(); ++i) { NodeId id = m_nodes.size(); m_nodes.push_back(Node(id)); m_nodes.back().descriptor = clusters[i]; m_nodes.back().parent = parent_id; m_nodes[parent_id].children.push_back(id); } // go on with the next level if(current_level < m_L) { // iterate again with the resulting clusters const std::vector &children_ids = m_nodes[parent_id].children; for(unsigned int i = 0; i < clusters.size(); ++i) { NodeId id = children_ids[i]; std::vector child_features; child_features.reserve(groups[i].size()); std::vector::const_iterator vit; for(vit = groups[i].begin(); vit != groups[i].end(); ++vit) { child_features.push_back(descriptors[*vit]); } if(child_features.size() > 1) { HKmeansStep(id, child_features, current_level + 1); } } } } // -------------------------------------------------------------------------- template void TemplatedVocabulary::initiateClusters (const std::vector &descriptors, std::vector &clusters) const { initiateClustersKMpp(descriptors, clusters); } // -------------------------------------------------------------------------- template void TemplatedVocabulary::initiateClustersKMpp( const std::vector &pfeatures, std::vector &clusters) const { // Implements kmeans++ seeding algorithm // Algorithm: // 1. Choose one center uniformly at random from among the data points. // 2. For each data point x, compute D(x), the distance between x and the nearest // center that has already been chosen. // 3. Add one new data point as a center. Each point x is chosen with probability // proportional to D(x)^2. // 4. Repeat Steps 2 and 3 until k centers have been chosen. // 5. Now that the initial centers have been chosen, proceed using standard k-means // clustering. DUtils::Random::SeedRandOnce(); clusters.resize(0); clusters.reserve(m_k); std::vector min_dists(pfeatures.size(), std::numeric_limits::max()); // 1. int ifeature = DUtils::Random::RandomInt(0, pfeatures.size()-1); // create first cluster clusters.push_back(*pfeatures[ifeature]); // compute the initial distances typename std::vector::const_iterator fit; std::vector::iterator dit; dit = min_dists.begin(); for(fit = pfeatures.begin(); fit != pfeatures.end(); ++fit, ++dit) { *dit = F::distance(*(*fit), clusters.back()); } while((int)clusters.size() < m_k) { // 2. dit = min_dists.begin(); for(fit = pfeatures.begin(); fit != pfeatures.end(); ++fit, ++dit) { if(*dit > 0) { double dist = F::distance(*(*fit), clusters.back()); if(dist < *dit) *dit = dist; } } // 3. double dist_sum = std::accumulate(min_dists.begin(), min_dists.end(), 0.0); if(dist_sum > 0) { double cut_d; do { cut_d = DUtils::Random::RandomValue(0, dist_sum); } while(cut_d == 0.0); double d_up_now = 0; for(dit = min_dists.begin(); dit != min_dists.end(); ++dit) { d_up_now += *dit; if(d_up_now >= cut_d) break; } if(dit == min_dists.end()) ifeature = pfeatures.size()-1; else ifeature = dit - min_dists.begin(); clusters.push_back(*pfeatures[ifeature]); } // if dist_sum > 0 else break; } // while(used_clusters < m_k) } // -------------------------------------------------------------------------- template void TemplatedVocabulary::createWords() { m_words.resize(0); if(!m_nodes.empty()) { m_words.reserve( (int)pow((double)m_k, (double)m_L) ); typename std::vector::iterator nit; nit = m_nodes.begin(); // ignore root for(++nit; nit != m_nodes.end(); ++nit) { if(nit->isLeaf()) { nit->word_id = m_words.size(); m_words.push_back( &(*nit) ); } } } } // -------------------------------------------------------------------------- template void TemplatedVocabulary::setNodeWeights (const std::vector > &training_features) { const unsigned int NWords = m_words.size(); const unsigned int NDocs = training_features.size(); if(m_weighting == TF || m_weighting == BINARY) { // idf part must be 1 always for(unsigned int i = 0; i < NWords; i++) m_words[i]->weight = 1; } else if(m_weighting == IDF || m_weighting == TF_IDF) { // IDF and TF-IDF: we calculte the idf path now // Note: this actually calculates the idf part of the tf-idf score. // The complete tf-idf score is calculated in ::transform std::vector Ni(NWords, 0); std::vector counted(NWords, false); typename std::vector >::const_iterator mit; typename std::vector::const_iterator fit; for(mit = training_features.begin(); mit != training_features.end(); ++mit) { fill(counted.begin(), counted.end(), false); for(fit = mit->begin(); fit < mit->end(); ++fit) { WordId word_id; transform(*fit, word_id); if(!counted[word_id]) { Ni[word_id]++; counted[word_id] = true; } } } // set ln(N/Ni) for(unsigned int i = 0; i < NWords; i++) { if(Ni[i] > 0) { m_words[i]->weight = log((double)NDocs / (double)Ni[i]); }// else // This cannot occur if using kmeans++ } } } // -------------------------------------------------------------------------- template inline unsigned int TemplatedVocabulary::size() const { return m_words.size(); } // -------------------------------------------------------------------------- template inline bool TemplatedVocabulary::empty() const { return m_words.empty(); } // -------------------------------------------------------------------------- template float TemplatedVocabulary::getEffectiveLevels() const { long sum = 0; typename std::vector::const_iterator wit; for(wit = m_words.begin(); wit != m_words.end(); ++wit) { const Node *p = *wit; for(; p->id != 0; sum++) p = &m_nodes[p->parent]; } return (float)((double)sum / (double)m_words.size()); } // -------------------------------------------------------------------------- template TDescriptor TemplatedVocabulary::getWord(WordId wid) const { return m_words[wid]->descriptor; } // -------------------------------------------------------------------------- template WordValue TemplatedVocabulary::getWordWeight(WordId wid) const { return m_words[wid]->weight; } // -------------------------------------------------------------------------- template WordId TemplatedVocabulary::transform (const TDescriptor& feature) const { if(empty()) { return 0; } WordId wid; transform(feature, wid); return wid; } // -------------------------------------------------------------------------- template void TemplatedVocabulary::transform( const std::vector& features, BowVector &v) const { v.clear(); if(empty()) { return; } // normalize LNorm norm; bool must = m_scoring_object->mustNormalize(norm); typename std::vector::const_iterator fit; if(m_weighting == TF || m_weighting == TF_IDF) { for(fit = features.begin(); fit < features.end(); ++fit) { WordId id; WordValue w; // w is the idf value if TF_IDF, 1 if TF transform(*fit, id, w); // not stopped if(w > 0) v.addWeight(id, w); } if(!v.empty() && !must) { // unnecessary when normalizing const double nd = v.size(); for(BowVector::iterator vit = v.begin(); vit != v.end(); vit++) vit->second /= nd; } } else // IDF || BINARY { for(fit = features.begin(); fit < features.end(); ++fit) { WordId id; WordValue w; // w is idf if IDF, or 1 if BINARY transform(*fit, id, w); // not stopped if(w > 0) v.addIfNotExist(id, w); } // if add_features } // if m_weighting == ... if(must) v.normalize(norm); } // -------------------------------------------------------------------------- template void TemplatedVocabulary::transform( const std::vector& features, BowVector &v, FeatureVector &fv, int levelsup) const { v.clear(); fv.clear(); if(empty()) // safe for subclasses { return; } // normalize LNorm norm; bool must = m_scoring_object->mustNormalize(norm); typename std::vector::const_iterator fit; if(m_weighting == TF || m_weighting == TF_IDF) { unsigned int i_feature = 0; for(fit = features.begin(); fit < features.end(); ++fit, ++i_feature) { WordId id; NodeId nid; WordValue w; // w is the idf value if TF_IDF, 1 if TF transform(*fit, id, w, &nid, levelsup); if(w > 0) // not stopped { v.addWeight(id, w); fv.addFeature(nid, i_feature); } } if(!v.empty() && !must) { // unnecessary when normalizing const double nd = v.size(); for(BowVector::iterator vit = v.begin(); vit != v.end(); vit++) vit->second /= nd; } } else // IDF || BINARY { unsigned int i_feature = 0; for(fit = features.begin(); fit < features.end(); ++fit, ++i_feature) { WordId id; NodeId nid; WordValue w; // w is idf if IDF, or 1 if BINARY transform(*fit, id, w, &nid, levelsup); if(w > 0) // not stopped { v.addIfNotExist(id, w); fv.addFeature(nid, i_feature); } } } // if m_weighting == ... if(must) v.normalize(norm); } // -------------------------------------------------------------------------- template inline double TemplatedVocabulary::score (const BowVector &v1, const BowVector &v2) const { return m_scoring_object->score(v1, v2); } // -------------------------------------------------------------------------- template void TemplatedVocabulary::transform (const TDescriptor &feature, WordId &id) const { WordValue weight; transform(feature, id, weight); } // -------------------------------------------------------------------------- template void TemplatedVocabulary::transform(const TDescriptor &feature, WordId &word_id, WordValue &weight, NodeId *nid, int levelsup) const { // propagate the feature down the tree std::vector nodes; typename std::vector::const_iterator nit; // level at which the node must be stored in nid, if given const int nid_level = m_L - levelsup; if(nid_level <= 0 && nid != NULL) *nid = 0; // root NodeId final_id = 0; // root int current_level = 0; do { ++current_level; nodes = m_nodes[final_id].children; final_id = nodes[0]; double best_d = F::distance(feature, m_nodes[final_id].descriptor); for(nit = nodes.begin() + 1; nit != nodes.end(); ++nit) { NodeId id = *nit; double d = F::distance(feature, m_nodes[id].descriptor); if(d < best_d) { best_d = d; final_id = id; } } if(nid != NULL && current_level == nid_level) *nid = final_id; } while( !m_nodes[final_id].isLeaf() ); // turn node id into word id word_id = m_nodes[final_id].word_id; weight = m_nodes[final_id].weight; } // -------------------------------------------------------------------------- template NodeId TemplatedVocabulary::getParentNode (WordId wid, int levelsup) const { NodeId ret = m_words[wid]->id; // node id while(levelsup > 0 && ret != 0) // ret == 0 --> root { --levelsup; ret = m_nodes[ret].parent; } return ret; } // -------------------------------------------------------------------------- template void TemplatedVocabulary::getWordsFromNode (NodeId nid, std::vector &words) const { words.clear(); if(m_nodes[nid].isLeaf()) { words.push_back(m_nodes[nid].word_id); } else { words.reserve(m_k); // ^1, ^2, ... std::vector parents; parents.push_back(nid); while(!parents.empty()) { NodeId parentid = parents.back(); parents.pop_back(); const std::vector &child_ids = m_nodes[parentid].children; std::vector::const_iterator cit; for(cit = child_ids.begin(); cit != child_ids.end(); ++cit) { const Node &child_node = m_nodes[*cit]; if(child_node.isLeaf()) words.push_back(child_node.word_id); else parents.push_back(*cit); } // for each child } // while !parents.empty } } // -------------------------------------------------------------------------- template int TemplatedVocabulary::stopWords(double minWeight) { int c = 0; typename std::vector::iterator wit; for(wit = m_words.begin(); wit != m_words.end(); ++wit) { if((*wit)->weight < minWeight) { ++c; (*wit)->weight = 0; } } return c; } // -------------------------------------------------------------------------- template void TemplatedVocabulary::save(const std::string &filename) const { cv::FileStorage fs(filename.c_str(), cv::FileStorage::WRITE); if(!fs.isOpened()) throw std::string("Could not open file ") + filename; save(fs); } // -------------------------------------------------------------------------- template void TemplatedVocabulary::load(const std::string &filename) { cv::FileStorage fs(filename.c_str(), cv::FileStorage::READ); if(!fs.isOpened()) throw std::string("Could not open file ") + filename; this->load(fs); } // -------------------------------------------------------------------------- template void TemplatedVocabulary::save(cv::FileStorage &f, const std::string &name) const { // Format YAML: // vocabulary // { // k: // L: // scoringType: // weightingType: // nodes // [ // { // nodeId: // parentId: // weight: // descriptor: // } // ] // words // [ // { // wordId: // nodeId: // } // ] // } // // The root node (index 0) is not included in the node vector // f << name << "{"; f << "k" << m_k; f << "L" << m_L; f << "scoringType" << m_scoring; f << "weightingType" << m_weighting; // tree f << "nodes" << "["; std::vector parents, children; std::vector::const_iterator pit; parents.push_back(0); // root while(!parents.empty()) { NodeId pid = parents.back(); parents.pop_back(); const Node& parent = m_nodes[pid]; children = parent.children; for(pit = children.begin(); pit != children.end(); pit++) { const Node& child = m_nodes[*pit]; // save node data f << "{:"; f << "nodeId" << (int)child.id; f << "parentId" << (int)pid; f << "weight" << (double)child.weight; f << "descriptor" << F::toString(child.descriptor); f << "}"; // add to parent list if(!child.isLeaf()) { parents.push_back(*pit); } } } f << "]"; // nodes // words f << "words" << "["; typename std::vector::const_iterator wit; for(wit = m_words.begin(); wit != m_words.end(); wit++) { WordId id = wit - m_words.begin(); f << "{:"; f << "wordId" << (int)id; f << "nodeId" << (int)(*wit)->id; f << "}"; } f << "]"; // words f << "}"; } // -------------------------------------------------------------------------- template void TemplatedVocabulary::load(const cv::FileStorage &fs, const std::string &name) { m_words.clear(); m_nodes.clear(); cv::FileNode fvoc = fs[name]; m_k = (int)fvoc["k"]; m_L = (int)fvoc["L"]; m_scoring = (ScoringType)((int)fvoc["scoringType"]); m_weighting = (WeightingType)((int)fvoc["weightingType"]); createScoringObject(); // nodes cv::FileNode fn = fvoc["nodes"]; m_nodes.resize(fn.size() + 1); // +1 to include root m_nodes[0].id = 0; for(unsigned int i = 0; i < fn.size(); ++i) { NodeId nid = (int)fn[i]["nodeId"]; NodeId pid = (int)fn[i]["parentId"]; WordValue weight = (WordValue)fn[i]["weight"]; std::string d = (std::string)fn[i]["descriptor"]; m_nodes[nid].id = nid; m_nodes[nid].parent = pid; m_nodes[nid].weight = weight; m_nodes[pid].children.push_back(nid); F::fromString(m_nodes[nid].descriptor, d); } // words fn = fvoc["words"]; m_words.resize(fn.size()); for(unsigned int i = 0; i < fn.size(); ++i) { NodeId wid = (int)fn[i]["wordId"]; NodeId nid = (int)fn[i]["nodeId"]; m_nodes[nid].word_id = wid; m_words[wid] = &m_nodes[nid]; } } // Added by VINS [[[ template void TemplatedVocabulary::loadBin(const std::string &filename) { m_words.clear(); m_nodes.clear(); //printf("loop load bin\n"); std::ifstream ifStream(filename); VINSLoop::Vocabulary voc; voc.deserialize(ifStream); ifStream.close(); m_k = voc.k; m_L = voc.L; m_scoring = (ScoringType)voc.scoringType; m_weighting = (WeightingType)voc.weightingType; createScoringObject(); m_nodes.resize(voc.nNodes + 1); // +1 to include root m_nodes[0].id = 0; for(int i = 0; i < voc.nNodes; ++i) { NodeId nid = voc.nodes[i].nodeId; NodeId pid = voc.nodes[i].parentId; WordValue weight = voc.nodes[i].weight; m_nodes[nid].id = nid; m_nodes[nid].parent = pid; m_nodes[nid].weight = weight; m_nodes[pid].children.push_back(nid); // Sorry to break template here m_nodes[nid].descriptor = boost::dynamic_bitset<>(voc.nodes[i].descriptor, voc.nodes[i].descriptor + 4); if (i < 5) { std::string test; boost::to_string(m_nodes[nid].descriptor, test); //cout << "descriptor[" << i << "] = " << test << endl; } } // words m_words.resize(voc.nWords); for(int i = 0; i < voc.nWords; ++i) { NodeId wid = (int)voc.words[i].wordId; NodeId nid = (int)voc.words[i].nodeId; m_nodes[nid].word_id = wid; m_words[wid] = &m_nodes[nid]; } } // Added by VINS ]]] // -------------------------------------------------------------------------- /** * Writes printable information of the vocabulary * @param os stream to write to * @param voc */ template std::ostream& operator<<(std::ostream &os, const TemplatedVocabulary &voc) { os << "Vocabulary: k = " << voc.getBranchingFactor() << ", L = " << voc.getDepthLevels() << ", Weighting = "; switch(voc.getWeightingType()) { case TF_IDF: os << "tf-idf"; break; case TF: os << "tf"; break; case IDF: os << "idf"; break; case BINARY: os << "binary"; break; } os << ", Scoring = "; switch(voc.getScoringType()) { case L1_NORM: os << "L1-norm"; break; case L2_NORM: os << "L2-norm"; break; case CHI_SQUARE: os << "Chi square distance"; break; case KL: os << "KL-divergence"; break; case BHATTACHARYYA: os << "Bhattacharyya coefficient"; break; case DOT_PRODUCT: os << "Dot product"; break; } os << ", Number of words = " << voc.size(); return os; } } // namespace DBoW2 #endif ================================================ FILE: pose_graph/src/ThirdParty/DUtils/DException.h ================================================ /* * File: DException.h * Project: DUtils library * Author: Dorian Galvez-Lopez * Date: October 6, 2009 * Description: general exception of the library * License: see the LICENSE.txt file * */ #pragma once #ifndef __D_EXCEPTION__ #define __D_EXCEPTION__ #include #include using namespace std; namespace DUtils { /// General exception class DException : public exception { public: /** * Creates an exception with a general error message */ DException(void) throw(): m_message("DUtils exception"){} /** * Creates an exception with a custom error message * @param msg: message */ DException(const char *msg) throw(): m_message(msg){} /** * Creates an exception with a custom error message * @param msg: message */ DException(const string &msg) throw(): m_message(msg){} /** * Destructor */ virtual ~DException(void) throw(){} /** * Returns the exception message */ virtual const char* what() const throw() { return m_message.c_str(); } protected: /// Error message string m_message; }; } #endif ================================================ FILE: pose_graph/src/ThirdParty/DUtils/DUtils.h ================================================ /* * File: DUtils.h * Project: DUtils library * Author: Dorian Galvez-Lopez * Date: October 6, 2009 * Description: include file for including all the library functionalities * License: see the LICENSE.txt file * */ /*! \mainpage DUtils Library * * DUtils library for C++: * Collection of classes with general utilities for C++ applications. * * Written by Dorian Galvez-Lopez, * University of Zaragoza * * Check my website to obtain updates: http://webdiis.unizar.es/~dorian * * \section license License * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU Lesser General Public License (LGPL) as * published by the Free Software Foundation, either version 3 of the License, * or any later version. * */ #pragma once #ifndef __D_UTILS__ #define __D_UTILS__ /// Several utilities for C++ programs namespace DUtils { } // Exception #include "DException.h" #include "Timestamp.h" // Random numbers #include "Random.h" #endif ================================================ FILE: pose_graph/src/ThirdParty/DUtils/Random.cpp ================================================ /* * File: Random.cpp * Project: DUtils library * Author: Dorian Galvez-Lopez * Date: April 2010 * Description: manages pseudo-random numbers * License: see the LICENSE.txt file * */ #include "Random.h" #include "Timestamp.h" #include using namespace std; bool DUtils::Random::m_already_seeded = false; void DUtils::Random::SeedRand(){ Timestamp time; time.setToCurrentTime(); srand((unsigned)time.getFloatTime()); } void DUtils::Random::SeedRandOnce() { if(!m_already_seeded) { DUtils::Random::SeedRand(); m_already_seeded = true; } } void DUtils::Random::SeedRand(int seed) { srand(seed); } void DUtils::Random::SeedRandOnce(int seed) { if(!m_already_seeded) { DUtils::Random::SeedRand(seed); m_already_seeded = true; } } int DUtils::Random::RandomInt(int min, int max){ int d = max - min + 1; return int(((double)rand()/((double)RAND_MAX + 1.0)) * d) + min; } // --------------------------------------------------------------------------- // --------------------------------------------------------------------------- DUtils::Random::UnrepeatedRandomizer::UnrepeatedRandomizer(int min, int max) { if(min <= max) { m_min = min; m_max = max; } else { m_min = max; m_max = min; } createValues(); } // --------------------------------------------------------------------------- DUtils::Random::UnrepeatedRandomizer::UnrepeatedRandomizer (const DUtils::Random::UnrepeatedRandomizer& rnd) { *this = rnd; } // --------------------------------------------------------------------------- int DUtils::Random::UnrepeatedRandomizer::get() { if(empty()) createValues(); DUtils::Random::SeedRandOnce(); int k = DUtils::Random::RandomInt(0, m_values.size()-1); int ret = m_values[k]; m_values[k] = m_values.back(); m_values.pop_back(); return ret; } // --------------------------------------------------------------------------- void DUtils::Random::UnrepeatedRandomizer::createValues() { int n = m_max - m_min + 1; m_values.resize(n); for(int i = 0; i < n; ++i) m_values[i] = m_min + i; } // --------------------------------------------------------------------------- void DUtils::Random::UnrepeatedRandomizer::reset() { if((int)m_values.size() != m_max - m_min + 1) createValues(); } // --------------------------------------------------------------------------- DUtils::Random::UnrepeatedRandomizer& DUtils::Random::UnrepeatedRandomizer::operator= (const DUtils::Random::UnrepeatedRandomizer& rnd) { if(this != &rnd) { this->m_min = rnd.m_min; this->m_max = rnd.m_max; this->m_values = rnd.m_values; } return *this; } // --------------------------------------------------------------------------- ================================================ FILE: pose_graph/src/ThirdParty/DUtils/Random.h ================================================ /* * File: Random.h * Project: DUtils library * Author: Dorian Galvez-Lopez * Date: April 2010, November 2011 * Description: manages pseudo-random numbers * License: see the LICENSE.txt file * */ #pragma once #ifndef __D_RANDOM__ #define __D_RANDOM__ #include #include namespace DUtils { /// Functions to generate pseudo-random numbers class Random { public: class UnrepeatedRandomizer; public: /** * Sets the random number seed to the current time */ static void SeedRand(); /** * Sets the random number seed to the current time only the first * time this function is called */ static void SeedRandOnce(); /** * Sets the given random number seed * @param seed */ static void SeedRand(int seed); /** * Sets the given random number seed only the first time this function * is called * @param seed */ static void SeedRandOnce(int seed); /** * Returns a random number in the range [0..1] * @return random T number in [0..1] */ template static T RandomValue(){ return (T)rand()/(T)RAND_MAX; } /** * Returns a random number in the range [min..max] * @param min * @param max * @return random T number in [min..max] */ template static T RandomValue(T min, T max){ return Random::RandomValue() * (max - min) + min; } /** * Returns a random int in the range [min..max] * @param min * @param max * @return random int in [min..max] */ static int RandomInt(int min, int max); /** * Returns a random number from a gaussian distribution * @param mean * @param sigma standard deviation */ template static T RandomGaussianValue(T mean, T sigma) { // Box-Muller transformation T x1, x2, w, y1; do { x1 = (T)2. * RandomValue() - (T)1.; x2 = (T)2. * RandomValue() - (T)1.; w = x1 * x1 + x2 * x2; } while ( w >= (T)1. || w == (T)0. ); w = sqrt( ((T)-2.0 * log( w ) ) / w ); y1 = x1 * w; return( mean + y1 * sigma ); } private: /// If SeedRandOnce() or SeedRandOnce(int) have already been called static bool m_already_seeded; }; // --------------------------------------------------------------------------- /// Provides pseudo-random numbers with no repetitions class Random::UnrepeatedRandomizer { public: /** * Creates a randomizer that returns numbers in the range [min, max] * @param min * @param max */ UnrepeatedRandomizer(int min, int max); ~UnrepeatedRandomizer(){} /** * Copies a randomizer * @param rnd */ UnrepeatedRandomizer(const UnrepeatedRandomizer& rnd); /** * Copies a randomizer * @param rnd */ UnrepeatedRandomizer& operator=(const UnrepeatedRandomizer& rnd); /** * Returns a random number not given before. If all the possible values * were already given, the process starts again * @return unrepeated random number */ int get(); /** * Returns whether all the possible values between min and max were * already given. If get() is called when empty() is true, the behaviour * is the same than after creating the randomizer * @return true iff all the values were returned */ inline bool empty() const { return m_values.empty(); } /** * Returns the number of values still to be returned * @return amount of values to return */ inline unsigned int left() const { return m_values.size(); } /** * Resets the randomizer as it were just created */ void reset(); protected: /** * Creates the vector with available values */ void createValues(); protected: /// Min of range of values int m_min; /// Max of range of values int m_max; /// Available values std::vector m_values; }; } #endif ================================================ FILE: pose_graph/src/ThirdParty/DUtils/Timestamp.cpp ================================================ /* * File: Timestamp.cpp * Author: Dorian Galvez-Lopez * Date: March 2009 * Description: timestamping functions * * Note: in windows, this class has a 1ms resolution * * License: see the LICENSE.txt file * */ #include #include #include #include #include #include #ifdef _WIN32 #ifndef WIN32 #define WIN32 #endif #endif #ifdef WIN32 #include #define sprintf sprintf_s #else #include #endif #include "Timestamp.h" using namespace std; using namespace DUtils; Timestamp::Timestamp(Timestamp::tOptions option) { if(option & CURRENT_TIME) setToCurrentTime(); else if(option & ZERO) setTime(0.); } Timestamp::~Timestamp(void) { } bool Timestamp::empty() const { return m_secs == 0 && m_usecs == 0; } void Timestamp::setToCurrentTime(){ #ifdef WIN32 struct __timeb32 timebuffer; _ftime32_s( &timebuffer ); // C4996 // Note: _ftime is deprecated; consider using _ftime_s instead m_secs = timebuffer.time; m_usecs = timebuffer.millitm * 1000; #else struct timeval now; gettimeofday(&now, NULL); m_secs = now.tv_sec; m_usecs = now.tv_usec; #endif } void Timestamp::setTime(const string &stime){ string::size_type p = stime.find('.'); if(p == string::npos){ m_secs = atol(stime.c_str()); m_usecs = 0; }else{ m_secs = atol(stime.substr(0, p).c_str()); string s_usecs = stime.substr(p+1, 6); m_usecs = atol(stime.substr(p+1).c_str()); m_usecs *= (unsigned long)pow(10.0, double(6 - s_usecs.length())); } } void Timestamp::setTime(double s) { m_secs = (unsigned long)s; m_usecs = (s - (double)m_secs) * 1e6; } double Timestamp::getFloatTime() const { return double(m_secs) + double(m_usecs)/1000000.0; } string Timestamp::getStringTime() const { char buf[32]; sprintf(buf, "%.6lf", this->getFloatTime()); return string(buf); } double Timestamp::operator- (const Timestamp &t) const { return this->getFloatTime() - t.getFloatTime(); } Timestamp& Timestamp::operator+= (double s) { *this = *this + s; return *this; } Timestamp& Timestamp::operator-= (double s) { *this = *this - s; return *this; } Timestamp Timestamp::operator+ (double s) const { unsigned long secs = (long)floor(s); unsigned long usecs = (long)((s - (double)secs) * 1e6); return this->plus(secs, usecs); } Timestamp Timestamp::plus(unsigned long secs, unsigned long usecs) const { Timestamp t; const unsigned long max = 1000000ul; if(m_usecs + usecs >= max) t.setTime(m_secs + secs + 1, m_usecs + usecs - max); else t.setTime(m_secs + secs, m_usecs + usecs); return t; } Timestamp Timestamp::operator- (double s) const { unsigned long secs = (long)floor(s); unsigned long usecs = (long)((s - (double)secs) * 1e6); return this->minus(secs, usecs); } Timestamp Timestamp::minus(unsigned long secs, unsigned long usecs) const { Timestamp t; const unsigned long max = 1000000ul; if(m_usecs < usecs) t.setTime(m_secs - secs - 1, max - (usecs - m_usecs)); else t.setTime(m_secs - secs, m_usecs - usecs); return t; } bool Timestamp::operator> (const Timestamp &t) const { if(m_secs > t.m_secs) return true; else if(m_secs == t.m_secs) return m_usecs > t.m_usecs; else return false; } bool Timestamp::operator>= (const Timestamp &t) const { if(m_secs > t.m_secs) return true; else if(m_secs == t.m_secs) return m_usecs >= t.m_usecs; else return false; } bool Timestamp::operator< (const Timestamp &t) const { if(m_secs < t.m_secs) return true; else if(m_secs == t.m_secs) return m_usecs < t.m_usecs; else return false; } bool Timestamp::operator<= (const Timestamp &t) const { if(m_secs < t.m_secs) return true; else if(m_secs == t.m_secs) return m_usecs <= t.m_usecs; else return false; } bool Timestamp::operator== (const Timestamp &t) const { return(m_secs == t.m_secs && m_usecs == t.m_usecs); } string Timestamp::Format(bool machine_friendly) const { struct tm tm_time; time_t t = (time_t)getFloatTime(); #ifdef WIN32 localtime_s(&tm_time, &t); #else localtime_r(&t, &tm_time); #endif char buffer[128]; if(machine_friendly) { strftime(buffer, 128, "%Y%m%d_%H%M%S", &tm_time); } else { strftime(buffer, 128, "%c", &tm_time); // Thu Aug 23 14:55:02 2001 } return string(buffer); } string Timestamp::Format(double s) { int days = int(s / (24. * 3600.0)); s -= days * (24. * 3600.0); int hours = int(s / 3600.0); s -= hours * 3600; int minutes = int(s / 60.0); s -= minutes * 60; int seconds = int(s); int ms = int((s - seconds)*1e6); stringstream ss; ss.fill('0'); bool b; if((b = (days > 0))) ss << days << "d "; if((b = (b || hours > 0))) ss << setw(2) << hours << ":"; if((b = (b || minutes > 0))) ss << setw(2) << minutes << ":"; if(b) ss << setw(2); ss << seconds; if(!b) ss << "." << setw(6) << ms; return ss.str(); } ================================================ FILE: pose_graph/src/ThirdParty/DUtils/Timestamp.h ================================================ /* * File: Timestamp.h * Author: Dorian Galvez-Lopez * Date: March 2009 * Description: timestamping functions * License: see the LICENSE.txt file * */ #ifndef __D_TIMESTAMP__ #define __D_TIMESTAMP__ #include using namespace std; namespace DUtils { /// Timestamp class Timestamp { public: /// Options to initiate a timestamp enum tOptions { NONE = 0, CURRENT_TIME = 0x1, ZERO = 0x2 }; public: /** * Creates a timestamp * @param option option to set the initial time stamp */ Timestamp(Timestamp::tOptions option = NONE); /** * Destructor */ virtual ~Timestamp(void); /** * Says if the timestamp is "empty": seconds and usecs are both 0, as * when initiated with the ZERO flag * @return true iif secs == usecs == 0 */ bool empty() const; /** * Sets this instance to the current time */ void setToCurrentTime(); /** * Sets the timestamp from seconds and microseconds * @param secs: seconds * @param usecs: microseconds */ inline void setTime(unsigned long secs, unsigned long usecs){ m_secs = secs; m_usecs = usecs; } /** * Returns the timestamp in seconds and microseconds * @param secs seconds * @param usecs microseconds */ inline void getTime(unsigned long &secs, unsigned long &usecs) const { secs = m_secs; usecs = m_usecs; } /** * Sets the timestamp from a string with the time in seconds * @param stime: string such as "1235603336.036609" */ void setTime(const string &stime); /** * Sets the timestamp from a number of seconds from the epoch * @param s seconds from the epoch */ void setTime(double s); /** * Returns this timestamp as the number of seconds in (long) float format */ double getFloatTime() const; /** * Returns this timestamp as the number of seconds in fixed length string format */ string getStringTime() const; /** * Returns the difference in seconds between this timestamp (greater) and t (smaller) * If the order is swapped, a negative number is returned * @param t: timestamp to subtract from this timestamp * @return difference in seconds */ double operator- (const Timestamp &t) const; /** * Returns a copy of this timestamp + s seconds + us microseconds * @param s seconds * @param us microseconds */ Timestamp plus(unsigned long s, unsigned long us) const; /** * Returns a copy of this timestamp - s seconds - us microseconds * @param s seconds * @param us microseconds */ Timestamp minus(unsigned long s, unsigned long us) const; /** * Adds s seconds to this timestamp and returns a reference to itself * @param s seconds * @return reference to this timestamp */ Timestamp& operator+= (double s); /** * Substracts s seconds to this timestamp and returns a reference to itself * @param s seconds * @return reference to this timestamp */ Timestamp& operator-= (double s); /** * Returns a copy of this timestamp + s seconds * @param s: seconds */ Timestamp operator+ (double s) const; /** * Returns a copy of this timestamp - s seconds * @param s: seconds */ Timestamp operator- (double s) const; /** * Returns whether this timestamp is at the future of t * @param t */ bool operator> (const Timestamp &t) const; /** * Returns whether this timestamp is at the future of (or is the same as) t * @param t */ bool operator>= (const Timestamp &t) const; /** * Returns whether this timestamp and t represent the same instant * @param t */ bool operator== (const Timestamp &t) const; /** * Returns whether this timestamp is at the past of t * @param t */ bool operator< (const Timestamp &t) const; /** * Returns whether this timestamp is at the past of (or is the same as) t * @param t */ bool operator<= (const Timestamp &t) const; /** * Returns the timestamp in a human-readable string * @param machine_friendly if true, the returned string is formatted * to yyyymmdd_hhmmss, without weekday or spaces * @note This has not been tested under Windows * @note The timestamp is truncated to seconds */ string Format(bool machine_friendly = false) const; /** * Returns a string version of the elapsed time in seconds, with the format * xd hh:mm:ss, hh:mm:ss, mm:ss or s.us * @param s: elapsed seconds (given by getFloatTime) to format */ static string Format(double s); protected: /// Seconds unsigned long m_secs; // seconds /// Microseconds unsigned long m_usecs; // microseconds }; } #endif ================================================ FILE: pose_graph/src/ThirdParty/DVision/BRIEF.cpp ================================================ /** * File: BRIEF.cpp * Author: Dorian Galvez * Date: September 2010 * Description: implementation of BRIEF (Binary Robust Independent * Elementary Features) descriptor by * Michael Calonder, Vincent Lepetitand Pascal Fua * + close binary tests (by Dorian Galvez-Lopez) * License: see the LICENSE.txt file * */ #include "BRIEF.h" #include "../DUtils/DUtils.h" #include #include #include using namespace std; using namespace DVision; // ---------------------------------------------------------------------------- BRIEF::BRIEF(int nbits, int patch_size, Type type): m_bit_length(nbits), m_patch_size(patch_size), m_type(type) { assert(patch_size > 1); assert(nbits > 0); generateTestPoints(); } // ---------------------------------------------------------------------------- BRIEF::~BRIEF() { } // --------------------------------------------------------------------------- void BRIEF::compute(const cv::Mat &image, const std::vector &points, vector &descriptors, bool treat_image) const { const float sigma = 2.f; const cv::Size ksize(9, 9); cv::Mat im; if(treat_image) { cv::Mat aux; if(image.depth() == 3) { cv::cvtColor(image, aux, cv::COLOR_RGB2GRAY); } else { aux = image; } cv::GaussianBlur(aux, im, ksize, sigma, sigma); } else { im = image; } assert(im.type() == CV_8UC1); assert(im.isContinuous()); // use im now const int W = im.cols; const int H = im.rows; descriptors.resize(points.size()); std::vector::iterator dit; std::vector::const_iterator kit; int x1, y1, x2, y2; dit = descriptors.begin(); for(kit = points.begin(); kit != points.end(); ++kit, ++dit) { dit->resize(m_bit_length); dit->reset(); for(unsigned int i = 0; i < m_x1.size(); ++i) { x1 = (int)(kit->pt.x + m_x1[i]); y1 = (int)(kit->pt.y + m_y1[i]); x2 = (int)(kit->pt.x + m_x2[i]); y2 = (int)(kit->pt.y + m_y2[i]); if(x1 >= 0 && x1 < W && y1 >= 0 && y1 < H && x2 >= 0 && x2 < W && y2 >= 0 && y2 < H) { if( im.ptr(y1)[x1] < im.ptr(y2)[x2] ) { dit->set(i); } } // if (x,y)_1 and (x,y)_2 are in the image } // for each (x,y) } // for each keypoint } // --------------------------------------------------------------------------- void BRIEF::generateTestPoints() { m_x1.resize(m_bit_length); m_y1.resize(m_bit_length); m_x2.resize(m_bit_length); m_y2.resize(m_bit_length); const float g_mean = 0.f; const float g_sigma = 0.2f * (float)m_patch_size; const float c_sigma = 0.08f * (float)m_patch_size; float sigma2; if(m_type == RANDOM) sigma2 = g_sigma; else sigma2 = c_sigma; const int max_v = m_patch_size / 2; DUtils::Random::SeedRandOnce(); for(int i = 0; i < m_bit_length; ++i) { int x1, y1, x2, y2; do { x1 = DUtils::Random::RandomGaussianValue(g_mean, g_sigma); } while( x1 > max_v || x1 < -max_v); do { y1 = DUtils::Random::RandomGaussianValue(g_mean, g_sigma); } while( y1 > max_v || y1 < -max_v); float meanx, meany; if(m_type == RANDOM) meanx = meany = g_mean; else { meanx = x1; meany = y1; } do { x2 = DUtils::Random::RandomGaussianValue(meanx, sigma2); } while( x2 > max_v || x2 < -max_v); do { y2 = DUtils::Random::RandomGaussianValue(meany, sigma2); } while( y2 > max_v || y2 < -max_v); m_x1[i] = x1; m_y1[i] = y1; m_x2[i] = x2; m_y2[i] = y2; } } // ---------------------------------------------------------------------------- ================================================ FILE: pose_graph/src/ThirdParty/DVision/BRIEF.h ================================================ /** * File: BRIEF.h * Author: Dorian Galvez-Lopez * Date: March 2011 * Description: implementation of BRIEF (Binary Robust Independent * Elementary Features) descriptor by * Michael Calonder, Vincent Lepetit and Pascal Fua * + close binary tests (by Dorian Galvez-Lopez) * * If you use this code with the RANDOM_CLOSE descriptor version, please cite: @INPROCEEDINGS{GalvezIROS11, author={Galvez-Lopez, Dorian and Tardos, Juan D.}, booktitle={Intelligent Robots and Systems (IROS), 2011 IEEE/RSJ International Conference on}, title={Real-time loop detection with bags of binary words}, year={2011}, month={sept.}, volume={}, number={}, pages={51 -58}, keywords={}, doi={10.1109/IROS.2011.6094885}, ISSN={2153-0858} } * * License: see the LICENSE.txt file * */ #ifndef __D_BRIEF__ #define __D_BRIEF__ #include #include #include namespace DVision { /// BRIEF descriptor class BRIEF { public: /// Bitset type typedef boost::dynamic_bitset<> bitset; /// Type of pairs enum Type { RANDOM, // random pairs (Calonder's original version) RANDOM_CLOSE, // random but close pairs (used in GalvezIROS11) }; public: /** * Creates the BRIEF a priori data for descriptors of nbits length * @param nbits descriptor length in bits * @param patch_size * @param type type of pairs to generate */ BRIEF(int nbits = 256, int patch_size = 48, Type type = RANDOM_CLOSE); virtual ~BRIEF(); /** * Returns the descriptor length in bits * @return descriptor length in bits */ inline int getDescriptorLengthInBits() const { return m_bit_length; } /** * Returns the type of classifier */ inline Type getType() const { return m_type; } /** * Returns the size of the patch */ inline int getPatchSize() const { return m_patch_size; } /** * Returns the BRIEF descriptors of the given keypoints in the given image * @param image * @param points * @param descriptors * @param treat_image (default: true) if true, the image is converted to * grayscale if needed and smoothed. If not, it is assumed the image has * been treated by the user * @note this function is similar to BRIEF::compute */ inline void operator() (const cv::Mat &image, const std::vector &points, std::vector &descriptors, bool treat_image = true) const { compute(image, points, descriptors, treat_image); } /** * Returns the BRIEF descriptors of the given keypoints in the given image * @param image * @param points * @param descriptors * @param treat_image (default: true) if true, the image is converted to * grayscale if needed and smoothed. If not, it is assumed the image has * been treated by the user * @note this function is similar to BRIEF::operator() */ void compute(const cv::Mat &image, const std::vector &points, std::vector &descriptors, bool treat_image = true) const; /** * Exports the test pattern * @param x1 x1 coordinates of pairs * @param y1 y1 coordinates of pairs * @param x2 x2 coordinates of pairs * @param y2 y2 coordinates of pairs */ inline void exportPairs(std::vector &x1, std::vector &y1, std::vector &x2, std::vector &y2) const { x1 = m_x1; y1 = m_y1; x2 = m_x2; y2 = m_y2; } /** * Sets the test pattern * @param x1 x1 coordinates of pairs * @param y1 y1 coordinates of pairs * @param x2 x2 coordinates of pairs * @param y2 y2 coordinates of pairs */ inline void importPairs(const std::vector &x1, const std::vector &y1, const std::vector &x2, const std::vector &y2) { m_x1 = x1; m_y1 = y1; m_x2 = x2; m_y2 = y2; m_bit_length = x1.size(); } /** * Returns the Hamming distance between two descriptors * @param a first descriptor vector * @param b second descriptor vector * @return hamming distance */ inline static int distance(const bitset &a, const bitset &b) { return (a^b).count(); } protected: /** * Generates random points in the patch coordinates, according to * m_patch_size and m_bit_length */ void generateTestPoints(); protected: /// Descriptor length in bits int m_bit_length; /// Patch size int m_patch_size; /// Type of pairs Type m_type; /// Coordinates of test points relative to the center of the patch std::vector m_x1, m_x2; std::vector m_y1, m_y2; }; } // namespace DVision #endif ================================================ FILE: pose_graph/src/ThirdParty/DVision/DVision.h ================================================ /* * File: DVision.h * Project: DVision library * Author: Dorian Galvez-Lopez * Date: October 4, 2010 * Description: several functions for computer vision * License: see the LICENSE.txt file * */ /*! \mainpage DVision Library * * DVision library for C++ and OpenCV: * Collection of classes with computer vision functionality * * Written by Dorian Galvez-Lopez, * University of Zaragoza * * Check my website to obtain updates: http://webdiis.unizar.es/~dorian * * \section requirements Requirements * This library requires the DUtils and DUtilsCV libraries and the OpenCV library. * * \section license License * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU Lesser General Public License (LGPL) as * published by the Free Software Foundation, either version 3 of the License, * or any later version. * */ #ifndef __D_VISION__ #define __D_VISION__ /// Computer vision functions namespace DVision { } #include "BRIEF.h" #endif ================================================ FILE: pose_graph/src/ThirdParty/VocabularyBinary.cpp ================================================ #include "VocabularyBinary.hpp" #include using namespace std; VINSLoop::Vocabulary::Vocabulary() : nNodes(0), nodes(nullptr), nWords(0), words(nullptr) { } VINSLoop::Vocabulary::~Vocabulary() { if (nodes != nullptr) { delete [] nodes; nodes = nullptr; } if (words != nullptr) { delete [] words; words = nullptr; } } void VINSLoop::Vocabulary::serialize(ofstream& stream) { stream.write((const char *)this, staticDataSize()); stream.write((const char *)nodes, sizeof(Node) * nNodes); stream.write((const char *)words, sizeof(Word) * nWords); } void VINSLoop::Vocabulary::deserialize(ifstream& stream) { stream.read((char *)this, staticDataSize()); nodes = new Node[nNodes]; stream.read((char *)nodes, sizeof(Node) * nNodes); words = new Word[nWords]; stream.read((char *)words, sizeof(Word) * nWords); } ================================================ FILE: pose_graph/src/ThirdParty/VocabularyBinary.hpp ================================================ #ifndef VocabularyBinary_hpp #define VocabularyBinary_hpp #include #include #include namespace VINSLoop { struct Node { int32_t nodeId; int32_t parentId; double weight; uint64_t descriptor[4]; }; struct Word { int32_t nodeId; int32_t wordId; }; struct Vocabulary { int32_t k; int32_t L; int32_t scoringType; int32_t weightingType; int32_t nNodes; int32_t nWords; Node *nodes; Word *words; Vocabulary(); ~Vocabulary(); void serialize(std::ofstream &stream); void deserialize(std::ifstream &stream); inline static size_t staticDataSize() { return sizeof(Vocabulary) - sizeof(Node *) - sizeof(Word *); } }; } // namespace VINSLoop #endif /* VocabularyBinary_hpp */ ================================================ FILE: pose_graph/src/keyframe/keyframe.cpp ================================================ #include "keyframe.h" #include template static void reduceVector(vector &v, vector status) { int j = 0; for (int i = 0; i < int(v.size()); i++) if (status[i]) v[j++] = v[i]; v.resize(j); } // create keyframe online KeyFrame::KeyFrame(double _time_stamp, int _index, Vector3d &_vio_T_w_i, Matrix3d &_vio_R_w_i, cv::Mat &_image, vector &_point_3d, vector &_point_2d_uv, vector &_point_2d_norm, vector &_point_id, int _sequence) { time_stamp = _time_stamp; index = _index; vio_T_w_i = _vio_T_w_i; vio_R_w_i = _vio_R_w_i; T_w_i = vio_T_w_i; R_w_i = vio_R_w_i; origin_vio_T = vio_T_w_i; origin_vio_R = vio_R_w_i; image = _image.clone(); cv::resize(image, thumbnail, cv::Size(80, 60)); point_3d = _point_3d; point_2d_uv = _point_2d_uv; point_2d_norm = _point_2d_norm; point_id = _point_id; has_loop = false; loop_index = -1; has_fast_point = false; loop_info << 0, 0, 0, 0, 0, 0, 0, 0; sequence = _sequence; // compute keypoints, brief_desc, computeWindowBRIEFPoint(); computeBRIEFPoint(); if (!DEBUG_IMAGE) image.release(); } // load previous keyframe KeyFrame::KeyFrame(double _time_stamp, int _index, Vector3d &_vio_T_w_i, Matrix3d &_vio_R_w_i, Vector3d &_T_w_i, Matrix3d &_R_w_i, cv::Mat &_image, int _loop_index, Eigen::Matrix &_loop_info, vector &_keypoints, vector &_keypoints_norm, vector &_brief_descriptors) { time_stamp = _time_stamp; index = _index; // vio_T_w_i = _vio_T_w_i; // vio_R_w_i = _vio_R_w_i; vio_T_w_i = _T_w_i; vio_R_w_i = _R_w_i; T_w_i = _T_w_i; R_w_i = _R_w_i; if (DEBUG_IMAGE) { image = _image.clone(); cv::resize(image, thumbnail, cv::Size(80, 60)); } if (_loop_index != -1) has_loop = true; else has_loop = false; loop_index = _loop_index; loop_info = _loop_info; has_fast_point = false; sequence = 0; keypoints = _keypoints; keypoints_norm = _keypoints_norm; brief_descriptors = _brief_descriptors; } //计算窗口中所有特征点的描述子 void KeyFrame::computeWindowBRIEFPoint() { BriefExtractor extractor(BRIEF_PATTERN_FILE.c_str()); for (int i = 0; i < (int)point_2d_uv.size(); i++) { cv::KeyPoint key; // point_2d_uv: pixel (x,y) in image key.pt = point_2d_uv[i]; window_keypoints.push_back(key); } extractor(image, window_keypoints, window_brief_descriptors); } //额外检测新特征点并计算所有特征点的描述子，为了回环检测 void KeyFrame::computeBRIEFPoint() { BriefExtractor extractor(BRIEF_PATTERN_FILE.c_str()); const int fast_th = 20; // corner detector response threshold if (1) cv::FAST(image, keypoints, fast_th, true); else { cv::FAST(image, keypoints, fast_th, true); ROS_ERROR("num of FAST: %d", (int)keypoints.size()); keypoints.clear(); vector tmp_pts; cv::goodFeaturesToTrack(image, tmp_pts, 50, 0.1, 10); ROS_ERROR("num of good features: %d", (int)tmp_pts.size()); for (int i = 0; i < (int)tmp_pts.size(); i++) { cv::KeyPoint key; key.pt = tmp_pts[i]; keypoints.push_back(key); } } extractor(image, keypoints, brief_descriptors); for (int i = 0; i < (int)keypoints.size(); i++) { Eigen::Vector3d tmp_p; m_camera->liftProjective( Eigen::Vector2d(keypoints[i].pt.x, keypoints[i].pt.y), tmp_p); cv::KeyPoint tmp_norm; tmp_norm.pt = cv::Point2f(tmp_p.x() / tmp_p.z(), tmp_p.y() / tmp_p.z()); keypoints_norm.push_back(tmp_norm); } } void BriefExtractor::operator()(const cv::Mat &im, vector &keys, vector &descriptors) const { m_brief.compute(im, keys, descriptors); } bool KeyFrame::searchInAera(const BRIEF::bitset window_descriptor, const std::vector &descriptors_old, const std::vector &keypoints_old, const std::vector &keypoints_old_norm, cv::Point2f &best_match, cv::Point2f &best_match_norm) { cv::Point2f best_pt; int bestDist = 128; int bestIndex = -1; for (int i = 0; i < (int)descriptors_old.size(); i++) { int dis = HammingDis(window_descriptor, descriptors_old[i]); if (dis < bestDist) { bestDist = dis; bestIndex = i; } } // printf("best dist %d", bestDist); if (bestIndex != -1 && bestDist < 80) { best_match = keypoints_old[bestIndex].pt; best_match_norm = keypoints_old_norm[bestIndex].pt; return true; } else return false; } void KeyFrame::searchByBRIEFDes( std::vector &matched_2d_old, std::vector &matched_2d_old_norm, std::vector &status, const std::vector &descriptors_old, const std::vector &keypoints_old, const std::vector &keypoints_old_norm) { for (int i = 0; i < (int)window_brief_descriptors.size(); i++) { cv::Point2f pt(0.f, 0.f); cv::Point2f pt_norm(0.f, 0.f); if (searchInAera(window_brief_descriptors[i], descriptors_old, keypoints_old, keypoints_old_norm, pt, pt_norm)) status.push_back(1); else status.push_back(0); matched_2d_old.push_back(pt); matched_2d_old_norm.push_back(pt_norm); } } void KeyFrame::FundmantalMatrixRANSAC( const std::vector &matched_2d_cur_norm, const std::vector &matched_2d_old_norm, vector &status) { int n = (int)matched_2d_cur_norm.size(); for (int i = 0; i < n; i++) status.push_back(0); if (n >= 8) { vector tmp_cur(n), tmp_old(n); for (int i = 0; i < (int)matched_2d_cur_norm.size(); i++) { double FOCAL_LENGTH = 460.0; double tmp_x, tmp_y; tmp_x = FOCAL_LENGTH * matched_2d_cur_norm[i].x + COL / 2.0; tmp_y = FOCAL_LENGTH * matched_2d_cur_norm[i].y + ROW / 2.0; tmp_cur[i] = cv::Point2f(tmp_x, tmp_y); tmp_x = FOCAL_LENGTH * matched_2d_old_norm[i].x + COL / 2.0; tmp_y = FOCAL_LENGTH * matched_2d_old_norm[i].y + ROW / 2.0; tmp_old[i] = cv::Point2f(tmp_x, tmp_y); } cv::findFundamentalMat(tmp_cur, tmp_old, cv::FM_RANSAC, 3.0, 0.9, status); } } void KeyFrame::PnPRANSAC(const vector &matched_2d_old_norm, const std::vector &matched_3d, std::vector &status, Eigen::Vector3d &PnP_T_old, Eigen::Matrix3d &PnP_R_old) { // for (int i = 0; i < matched_3d.size(); i++) // printf("3d x: %f, y: %f, z: %f\n",matched_3d[i].x, matched_3d[i].y, //matched_3d[i].z ); printf("match size %d \n", matched_3d.size()); cv::Mat r, rvec, t, D, tmp_r; cv::Mat K = (cv::Mat_(3, 3) << 1.0, 0, 0, 0, 1.0, 0, 0, 0, 1.0); Matrix3d R_inital; Vector3d P_inital; Matrix3d R_w_c = origin_vio_R * qic; Vector3d T_w_c = origin_vio_T + origin_vio_R * tic; R_inital = R_w_c.inverse(); P_inital = -(R_inital * T_w_c); cv::eigen2cv(R_inital, tmp_r); cv::Rodrigues(tmp_r, rvec); cv::eigen2cv(P_inital, t); cv::Mat inliers; TicToc t_pnp_ransac; if (CV_MAJOR_VERSION < 3) solvePnPRansac(matched_3d, matched_2d_old_norm, K, D, rvec, t, true, 100, 10.0 / 460.0, 100, inliers); else { if (CV_MINOR_VERSION < 2) solvePnPRansac(matched_3d, matched_2d_old_norm, K, D, rvec, t, true, 100, sqrt(10.0 / 460.0), 0.99, inliers); else solvePnPRansac(matched_3d, matched_2d_old_norm, K, D, rvec, t, true, 100, 10.0 / 460.0, 0.99, inliers); } for (int i = 0; i < (int)matched_2d_old_norm.size(); i++) status.push_back(0); for (int i = 0; i < inliers.rows; i++) { int n = inliers.at(i); status[n] = 1; } cv::Rodrigues(rvec, r); Matrix3d R_pnp, R_w_c_old; cv::cv2eigen(r, R_pnp); R_w_c_old = R_pnp.transpose(); Vector3d T_pnp, T_w_c_old; cv::cv2eigen(t, T_pnp); T_w_c_old = R_w_c_old * (-T_pnp); PnP_R_old = R_w_c_old * qic.transpose(); PnP_T_old = T_w_c_old - PnP_R_old * tic; } //寻找并建立关键帧与回环帧之间的匹配关系 bool KeyFrame::findConnection(KeyFrame *old_kf) { TicToc tmp_t; // printf("find Connection\n"); vector matched_2d_cur, matched_2d_old; vector matched_2d_cur_norm, matched_2d_old_norm; vector matched_3d; vector matched_id; vector status; matched_3d = point_3d; matched_2d_cur = point_2d_uv; matched_2d_cur_norm = point_2d_norm; matched_id = point_id; TicToc t_match; #if 0 if (DEBUG_IMAGE) { cv::Mat gray_img, loop_match_img; cv::Mat old_img = old_kf->image; cv::hconcat(image, old_img, gray_img); cvtColor(gray_img, loop_match_img, CV_GRAY2RGB); for(int i = 0; i< (int)point_2d_uv.size(); i++) { cv::Point2f cur_pt = point_2d_uv[i]; cv::circle(loop_match_img, cur_pt, 5, cv::Scalar(0, 255, 0)); } for(int i = 0; i< (int)old_kf->keypoints.size(); i++) { cv::Point2f old_pt = old_kf->keypoints[i].pt; old_pt.x += COL; cv::circle(loop_match_img, old_pt, 5, cv::Scalar(0, 255, 0)); } ostringstream path; path << "/home/tony-ws1/raw_data/loop_image/" << index << "-" << old_kf->index << "-" << "0raw_point.jpg"; cv::imwrite( path.str().c_str(), loop_match_img); } #endif // ROS_ERROR("points before: %d", (int)matched_2d_cur.size()); // printf("search by des\n"); searchByBRIEFDes(matched_2d_old, matched_2d_old_norm, status, old_kf->brief_descriptors, old_kf->keypoints, old_kf->keypoints_norm); reduceVector(matched_2d_cur, status); reduceVector(matched_2d_old, status); reduceVector(matched_2d_cur_norm, status); reduceVector(matched_2d_old_norm, status); reduceVector(matched_3d, status); reduceVector(matched_id, status); // printf("search by des finish\n"); #if 0 if (DEBUG_IMAGE) { int gap = 10; cv::Mat gap_image(ROW, gap, CV_8UC1, cv::Scalar(255, 255, 255)); cv::Mat gray_img, loop_match_img; cv::Mat old_img = old_kf->image; cv::hconcat(image, gap_image, gap_image); cv::hconcat(gap_image, old_img, gray_img); cvtColor(gray_img, loop_match_img, CV_GRAY2RGB); for(int i = 0; i< (int)matched_2d_cur.size(); i++) { cv::Point2f cur_pt = matched_2d_cur[i]; cv::circle(loop_match_img, cur_pt, 5, cv::Scalar(0, 255, 0)); } for(int i = 0; i< (int)matched_2d_old.size(); i++) { cv::Point2f old_pt = matched_2d_old[i]; old_pt.x += (COL + gap); cv::circle(loop_match_img, old_pt, 5, cv::Scalar(0, 255, 0)); } for (int i = 0; i< (int)matched_2d_cur.size(); i++) { cv::Point2f old_pt = matched_2d_old[i]; old_pt.x += (COL + gap); cv::line(loop_match_img, matched_2d_cur[i], old_pt, cv::Scalar(0, 255, 0), 1, 8, 0); } ostringstream path, path1, path2; path << "/home/tony-ws1/raw_data/loop_image/" << index << "-" << old_kf->index << "-" << "1descriptor_match.jpg"; cv::imwrite( path.str().c_str(), loop_match_img); /* path1 << "/home/tony-ws1/raw_data/loop_image/" << index << "-" << old_kf->index << "-" << "1descriptor_match_1.jpg"; cv::imwrite( path1.str().c_str(), image); path2 << "/home/tony-ws1/raw_data/loop_image/" << index << "-" << old_kf->index << "-" << "1descriptor_match_2.jpg"; cv::imwrite( path2.str().c_str(), old_img); */ } #endif status.clear(); /* FundmantalMatrixRANSAC(matched_2d_cur_norm, matched_2d_old_norm, status); reduceVector(matched_2d_cur, status); reduceVector(matched_2d_old, status); reduceVector(matched_2d_cur_norm, status); reduceVector(matched_2d_old_norm, status); reduceVector(matched_3d, status); reduceVector(matched_id, status); */ #if 0 if (DEBUG_IMAGE) { int gap = 10; cv::Mat gap_image(ROW, gap, CV_8UC1, cv::Scalar(255, 255, 255)); cv::Mat gray_img, loop_match_img; cv::Mat old_img = old_kf->image; cv::hconcat(image, gap_image, gap_image); cv::hconcat(gap_image, old_img, gray_img); cvtColor(gray_img, loop_match_img, CV_GRAY2RGB); for(int i = 0; i< (int)matched_2d_cur.size(); i++) { cv::Point2f cur_pt = matched_2d_cur[i]; cv::circle(loop_match_img, cur_pt, 5, cv::Scalar(0, 255, 0)); } for(int i = 0; i< (int)matched_2d_old.size(); i++) { cv::Point2f old_pt = matched_2d_old[i]; old_pt.x += (COL + gap); cv::circle(loop_match_img, old_pt, 5, cv::Scalar(0, 255, 0)); } for (int i = 0; i< (int)matched_2d_cur.size(); i++) { cv::Point2f old_pt = matched_2d_old[i]; old_pt.x += (COL + gap) ; cv::line(loop_match_img, matched_2d_cur[i], old_pt, cv::Scalar(0, 255, 0), 1, 8, 0); } ostringstream path; path << "/home/tony-ws1/raw_data/loop_image/" << index << "-" << old_kf->index << "-" << "2fundamental_match.jpg"; cv::imwrite( path.str().c_str(), loop_match_img); } #endif Eigen::Vector3d PnP_T_old; Eigen::Matrix3d PnP_R_old; Eigen::Vector3d relative_t; Quaterniond relative_q; double relative_yaw; // ROS_ERROR("points after: %d", (int)matched_2d_cur.size()); if ((int)matched_2d_cur.size() > MIN_LOOP_NUM) { status.clear(); PnPRANSAC(matched_2d_old_norm, matched_3d, status, PnP_T_old, PnP_R_old); reduceVector(matched_2d_cur, status); reduceVector(matched_2d_old, status); reduceVector(matched_2d_cur_norm, status); reduceVector(matched_2d_old_norm, status); reduceVector(matched_3d, status); reduceVector(matched_id, status); ROS_WARN("points left after RANSAC: %d", (int)matched_2d_cur.size()); if (DEBUG_IMAGE) { int gap = 10; cv::Mat gap_image(ROW, gap, CV_8UC1, cv::Scalar(255, 255, 255)); cv::Mat gray_img, loop_match_img; cv::Mat old_img = old_kf->image; // cv::imshow("image",image); // cv::waitKey(1); cv::hconcat(image, gap_image, gap_image); // cv::imshow("gap_image",gap_image); // cv::waitKey(1); cv::hconcat(gap_image, old_img, gray_img); // cv::imshow("gray_img",gray_img); // cv::waitKey(1); cvtColor(gray_img, loop_match_img, CV_GRAY2RGB); for (int i = 0; i < (int)matched_2d_cur.size(); i++) { cv::Point2f cur_pt = matched_2d_cur[i]; cv::circle(loop_match_img, cur_pt, 5, cv::Scalar(0, 255, 0)); } for (int i = 0; i < (int)matched_2d_old.size(); i++) { cv::Point2f old_pt = matched_2d_old[i]; old_pt.x += (COL + gap); cv::circle(loop_match_img, old_pt, 5, cv::Scalar(0, 255, 0)); } for (int i = 0; i < (int)matched_2d_cur.size(); i++) { cv::Point2f old_pt = matched_2d_old[i]; old_pt.x += (COL + gap); cv::line(loop_match_img, matched_2d_cur[i], old_pt, cv::Scalar(0, 255, 0), 2, 8, 0); } cv::Mat notation(50, COL + gap + COL, CV_8UC3, cv::Scalar(255, 255, 255)); putText(notation, "current frame: " + to_string(index) + " sequence: " + to_string(sequence), cv::Point2f(20, 30), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(255), 3); putText(notation, "previous frame: " + to_string(old_kf->index) + " sequence: " + to_string(old_kf->sequence), cv::Point2f(20 + COL + gap, 30), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(255), 3); cv::vconcat(notation, loop_match_img, loop_match_img); /* ostringstream path; path << "/home/tony-ws1/raw_data/loop_image/" << index << "-" << old_kf->index << "-" << "3pnp_match.jpg"; cv::imwrite( path.str().c_str(), loop_match_img); */ if ((int)matched_2d_cur.size() > MIN_LOOP_NUM) { /* cv::imshow("loop connection",loop_match_img); cv::waitKey(10); */ cv::Mat thumbimage; cv::resize(loop_match_img, thumbimage, cv::Size(loop_match_img.cols / 2, loop_match_img.rows / 2)); sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", thumbimage) .toImageMsg(); msg->header.stamp = ros::Time(time_stamp); pub_match_img.publish(msg); } } } if ((int)matched_2d_cur.size() > MIN_LOOP_NUM) { relative_t = PnP_R_old.transpose() * (origin_vio_T - PnP_T_old); relative_q = PnP_R_old.transpose() * origin_vio_R; relative_yaw = Utility::normalizeAngle(Utility::R2ypr(origin_vio_R).x() - Utility::R2ypr(PnP_R_old).x()); // printf("PNP relative\n"); // cout << "pnp relative_t " << relative_t.transpose() << endl; // cout << "pnp relative_yaw " << relative_yaw << endl; if (abs(relative_yaw) < 30.0 && relative_t.norm() < 20.0) { has_loop = true; loop_index = old_kf->index; loop_info << relative_t.x(), relative_t.y(), relative_t.z(), relative_q.w(), relative_q.x(), relative_q.y(), relative_q.z(), relative_yaw; if (FAST_RELOCALIZATION) { sensor_msgs::PointCloud msg_match_points; msg_match_points.header.stamp = ros::Time(time_stamp); for (int i = 0; i < (int)matched_2d_old_norm.size(); i++) { geometry_msgs::Point32 p; p.x = matched_2d_old_norm[i].x; p.y = matched_2d_old_norm[i].y; p.z = matched_id[i]; msg_match_points.points.push_back(p); } Eigen::Vector3d T = old_kf->T_w_i; Eigen::Matrix3d R = old_kf->R_w_i; Quaterniond Q(R); sensor_msgs::ChannelFloat32 t_q_index; t_q_index.values.push_back(T.x()); t_q_index.values.push_back(T.y()); t_q_index.values.push_back(T.z()); t_q_index.values.push_back(Q.w()); t_q_index.values.push_back(Q.x()); t_q_index.values.push_back(Q.y()); t_q_index.values.push_back(Q.z()); t_q_index.values.push_back(index); msg_match_points.channels.push_back(t_q_index); pub_match_points.publish(msg_match_points); } return true; } } // printf("loop final use num %d %lf--------------- \n", // (int)matched_2d_cur.size(), t_match.toc()); return false; } int KeyFrame::HammingDis(const BRIEF::bitset &a, const BRIEF::bitset &b) { BRIEF::bitset xor_of_bitset = a ^ b; int dis = xor_of_bitset.count(); return dis; } void KeyFrame::getVioPose(Eigen::Vector3d &_T_w_i, Eigen::Matrix3d &_R_w_i) { _T_w_i = vio_T_w_i; _R_w_i = vio_R_w_i; } void KeyFrame::getPose(Eigen::Vector3d &_T_w_i, Eigen::Matrix3d &_R_w_i) { _T_w_i = T_w_i; _R_w_i = R_w_i; } void KeyFrame::updatePose(const Eigen::Vector3d &_T_w_i, const Eigen::Matrix3d &_R_w_i) { T_w_i = _T_w_i; R_w_i = _R_w_i; } void KeyFrame::updateVioPose(const Eigen::Vector3d &_T_w_i, const Eigen::Matrix3d &_R_w_i) { vio_T_w_i = _T_w_i; vio_R_w_i = _R_w_i; T_w_i = vio_T_w_i; R_w_i = vio_R_w_i; } Eigen::Vector3d KeyFrame::getLoopRelativeT() { // loop_info is updated in KeyFrame::findConnection(KeyFrame* old_kf) return Eigen::Vector3d(loop_info(0), loop_info(1), loop_info(2)); } Eigen::Quaterniond KeyFrame::getLoopRelativeQ() { return Eigen::Quaterniond(loop_info(3), loop_info(4), loop_info(5), loop_info(6)); } double KeyFrame::getLoopRelativeYaw() { return loop_info(7); } void KeyFrame::updateLoop(Eigen::Matrix &_loop_info) { if (abs(_loop_info(7)) < 30.0 && Vector3d(_loop_info(0), _loop_info(1), _loop_info(2)).norm() < 20.0) { // printf("update loop info\n"); loop_info = _loop_info; } } BriefExtractor::BriefExtractor(const std::string &pattern_file) { // The DVision::BRIEF extractor computes a random pattern by default when // the object is created. // We load the pattern that we used to build the vocabulary, to make // the descriptors compatible with the predefined vocabulary // loads the pattern cv::FileStorage fs(pattern_file.c_str(), cv::FileStorage::READ); if (!fs.isOpened()) throw string("Could not open file ") + pattern_file; vector x1, y1, x2, y2; fs["x1"] >> x1; fs["x2"] >> x2; fs["y1"] >> y1; fs["y2"] >> y2; m_brief.importPairs(x1, y1, x2, y2); } ================================================ FILE: pose_graph/src/keyframe/keyframe.h ================================================ #pragma once #include "../ThirdParty/DBoW/DBoW2.h" #include "../ThirdParty/DVision/DVision.h" #include "../utility/parameters.h" #include "../utility/tic_toc.h" #include "../utility/utility.h" #include "camodocal/camera_models/CameraFactory.h" #include "camodocal/camera_models/CataCamera.h" #include "camodocal/camera_models/PinholeCamera.h" #include #include #include #include #define MIN_LOOP_NUM 25 using namespace Eigen; using namespace std; using namespace DVision; class BriefExtractor { public: virtual void operator()(const cv::Mat &im, vector &keys, vector &descriptors) const; BriefExtractor(const std::string &pattern_file); DVision::BRIEF m_brief; }; class KeyFrame { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW KeyFrame(double _time_stamp, int _index, Vector3d &_vio_T_w_i, Matrix3d &_vio_R_w_i, cv::Mat &_image, vector &_point_3d, vector &_point_2d_uv, vector &_point_2d_normal, vector &_point_id, int _sequence); KeyFrame(double _time_stamp, int _index, Vector3d &_vio_T_w_i, Matrix3d &_vio_R_w_i, Vector3d &_T_w_i, Matrix3d &_R_w_i, cv::Mat &_image, int _loop_index, Eigen::Matrix &_loop_info, vector &_keypoints, vector &_keypoints_norm, vector &_brief_descriptors); bool findConnection(KeyFrame *old_kf); void computeWindowBRIEFPoint(); void computeBRIEFPoint(); // void extractBrief(); int HammingDis(const BRIEF::bitset &a, const BRIEF::bitset &b); bool searchInAera(const BRIEF::bitset window_descriptor, const std::vector &descriptors_old, const std::vector &keypoints_old, const std::vector &keypoints_old_norm, cv::Point2f &best_match, cv::Point2f &best_match_norm); void searchByBRIEFDes(std::vector &matched_2d_old, std::vector &matched_2d_old_norm, std::vector &status, const std::vector &descriptors_old, const std::vector &keypoints_old, const std::vector &keypoints_old_norm); void FundmantalMatrixRANSAC(const std::vector &matched_2d_cur_norm, const std::vector &matched_2d_old_norm, vector &status); void PnPRANSAC(const vector &matched_2d_old_norm, const std::vector &matched_3d, std::vector &status, Eigen::Vector3d &PnP_T_old, Eigen::Matrix3d &PnP_R_old); void getVioPose(Eigen::Vector3d &_T_w_i, Eigen::Matrix3d &_R_w_i); void getPose(Eigen::Vector3d &_T_w_i, Eigen::Matrix3d &_R_w_i); void updatePose(const Eigen::Vector3d &_T_w_i, const Eigen::Matrix3d &_R_w_i); void updateVioPose(const Eigen::Vector3d &_T_w_i, const Eigen::Matrix3d &_R_w_i); void updateLoop(Eigen::Matrix &_loop_info); Eigen::Vector3d getLoopRelativeT(); double getLoopRelativeYaw(); Eigen::Quaterniond getLoopRelativeQ(); double time_stamp; int index; int local_index; Eigen::Vector3d vio_T_w_i; Eigen::Matrix3d vio_R_w_i; Eigen::Vector3d T_w_i; Eigen::Matrix3d R_w_i; Eigen::Vector3d origin_vio_T; Eigen::Matrix3d origin_vio_R; cv::Mat image; cv::Mat thumbnail; vector point_3d; vector point_2d_uv; vector point_2d_norm; vector point_id; vector keypoints; vector keypoints_norm; vector window_keypoints; vector brief_descriptors; vector window_brief_descriptors; bool has_fast_point; int sequence; bool has_loop; int loop_index; Eigen::Matrix loop_info; }; ================================================ FILE: pose_graph/src/pose_graph/pose_graph.cpp ================================================ #include "pose_graph.h" #include PoseGraph::PoseGraph() { posegraph_visualization = new CameraPoseVisualization(1.0, 0.0, 1.0, 1.0); posegraph_visualization->setScale(0.1); posegraph_visualization->setLineWidth(0.01); earliest_loop_index = -1; t_drift = Eigen::Vector3d(0, 0, 0); yaw_drift = 0; r_drift = Eigen::Matrix3d::Identity(); w_t_vio = Eigen::Vector3d(0, 0, 0); w_r_vio = Eigen::Matrix3d::Identity(); global_index = 0; sequence_cnt = 0; sequence_loop.push_back(false); base_sequence = 1; use_imu = false; } PoseGraph::~PoseGraph() { t_optimization.join(); } void PoseGraph::setIMUFlag(bool _use_imu) { use_imu = _use_imu; if (use_imu) { printf("VIO input, perfrom 4 DoF (x, y, z, yaw) pose graph optimization\n"); t_optimization = std::thread(&PoseGraph::optimize4DoF, this); } else { printf("VO input, perfrom 6 DoF pose graph optimization\n"); t_optimization = std::thread(&PoseGraph::optimize6DoF, this); } } void PoseGraph::registerPub(ros::NodeHandle &n) { pub_pg_path = n.advertise("/pose_graph/pose_graph_path", 1000); pub_base_path = n.advertise("/pose_graph/base_path", 1000); pub_pose_graph = n.advertise( "/pose_graph/pose_graph", 1000); for (int i = 1; i < 10; i++) pub_path[i] = n.advertise("/pose_graph/path_" + to_string(i), 1000); } void PoseGraph::loadVocabulary(const std::string& voc_path) { voc = new BriefVocabulary(voc_path); db.setVocabulary(*voc, false, 0); } void PoseGraph::addKeyFrame(KeyFrame *cur_kf, bool flag_detect_loop) { // shift to base frame (the base(first) frame defined as the map frame) Vector3d vio_P_cur; Matrix3d vio_R_cur; // sequence = 1, sequence_cnt = 0 at init, sequence_cnt++ // then sequence_cnt remains 1 (no new sequence in my case) if (sequence_cnt != cur_kf->sequence) { // run once sequence_cnt++; sequence_loop.push_back(false); w_t_vio = Eigen::Vector3d(0, 0, 0); w_r_vio = Eigen::Matrix3d::Identity(); m_drift.lock(); t_drift = Eigen::Vector3d(0, 0, 0); r_drift = Eigen::Matrix3d::Identity(); m_drift.unlock(); } cur_kf->getVioPose(vio_P_cur, vio_R_cur); vio_P_cur = w_r_vio * vio_P_cur + w_t_vio; vio_R_cur = w_r_vio * vio_R_cur; cur_kf->updateVioPose(vio_P_cur, vio_R_cur); cur_kf->index = global_index; global_index++; int loop_index = -1; // always true if (flag_detect_loop) { TicToc tmp_t; // get loop_index(match frame index) here loop_index = detectLoop(cur_kf, cur_kf->index); } else { addKeyFrameIntoVoc(cur_kf); } if (loop_index != -1) { // printf(" %d detect loop with %d \n", cur_kf->index, loop_index); KeyFrame *old_kf = getKeyFrame(loop_index); //当前帧与回环候选帧进行描述子匹配 if (cur_kf->findConnection(old_kf)) { // earliest_loop_index为最早的回环候选帧 if (earliest_loop_index > loop_index || earliest_loop_index == -1) earliest_loop_index = loop_index; Vector3d w_P_old, w_P_cur, vio_P_cur; Matrix3d w_R_old, w_R_cur, vio_R_cur; old_kf->getVioPose(w_P_old, w_R_old); cur_kf->getVioPose(vio_P_cur, vio_R_cur); //获取当前帧与回环帧的相对位姿relative_q、relative_t Vector3d relative_t; Quaterniond relative_q; relative_t = cur_kf->getLoopRelativeT(); relative_q = (cur_kf->getLoopRelativeQ()).toRotationMatrix(); //重新计算当前帧位姿w_P_cur、w_R_cur // 使用回环优化过的w_R_old，w_P_old与匹配的相对位姿relative_t，relative_q来更新当前帧位姿 w_P_cur = w_R_old * relative_t + w_P_old; w_R_cur = w_R_old * relative_q; //回环得到的位姿和VIO位姿之间的偏移量shift_r、shift_t double shift_yaw; Matrix3d shift_r; Vector3d shift_t; if (use_imu) { shift_yaw = Utility::R2ypr(w_R_cur).x() - Utility::R2ypr(vio_R_cur).x(); shift_r = Utility::ypr2R(Vector3d(shift_yaw, 0, 0)); } else { shift_r = w_R_cur * vio_R_cur.transpose(); } shift_t = w_P_cur - w_R_cur * vio_R_cur.transpose() * vio_P_cur; // shift vio pose of whole sequence to the map frame if (old_kf->sequence != cur_kf->sequence && sequence_loop[cur_kf->sequence] == 0) { w_r_vio = shift_r; w_t_vio = shift_t; vio_P_cur = w_r_vio * vio_P_cur + w_t_vio; vio_R_cur = w_r_vio * vio_R_cur; cur_kf->updateVioPose(vio_P_cur, vio_R_cur); list::iterator it = keyframelist.begin(); for (; it != keyframelist.end(); it++) { if ((*it)->sequence == cur_kf->sequence) { Vector3d vio_P_cur; Matrix3d vio_R_cur; (*it)->getVioPose(vio_P_cur, vio_R_cur); vio_P_cur = w_r_vio * vio_P_cur + w_t_vio; vio_R_cur = w_r_vio * vio_R_cur; (*it)->updateVioPose(vio_P_cur, vio_R_cur); } } sequence_loop[cur_kf->sequence] = true; } //将当前帧放入优化队列中 m_optimize_buf.lock(); optimize_buf.push(cur_kf->index); m_optimize_buf.unlock(); } } m_keyframelist.lock(); //获取VIO当前帧的位姿P、R，根据偏移量得到实际位姿 Vector3d P; Matrix3d R; cur_kf->getVioPose(P, R); P = r_drift * P + t_drift; R = r_drift * R; cur_kf->updatePose(P, R); Quaterniond Q{R}; geometry_msgs::PoseStamped pose_stamped; pose_stamped.header.stamp = ros::Time(cur_kf->time_stamp); pose_stamped.header.frame_id = "map"; pose_stamped.pose.position.x = P.x() + VISUALIZATION_SHIFT_X; pose_stamped.pose.position.y = P.y() + VISUALIZATION_SHIFT_Y; pose_stamped.pose.position.z = P.z(); pose_stamped.pose.orientation.x = Q.x(); pose_stamped.pose.orientation.y = Q.y(); pose_stamped.pose.orientation.z = Q.z(); pose_stamped.pose.orientation.w = Q.w(); path[sequence_cnt].poses.push_back(pose_stamped); path[sequence_cnt].header = pose_stamped.header; // not used if (SAVE_LOOP_PATH) { ofstream loop_path_file(VINS_RESULT_PATH, ios::app); loop_path_file.setf(ios::fixed, ios::floatfield); loop_path_file.precision(0); loop_path_file << cur_kf->time_stamp * 1e9 << ","; loop_path_file.precision(5); loop_path_file << P.x() << "," << P.y() << "," << P.z() << "," << Q.w() << "," << Q.x() << "," << Q.y() << "," << Q.z() << "," << endl; loop_path_file.close(); } // not used // draw local connection if (SHOW_S_EDGE) { list::reverse_iterator rit = keyframelist.rbegin(); for (int i = 0; i < 4; i++) { if (rit == keyframelist.rend()) break; Vector3d conncected_P; Matrix3d connected_R; if ((*rit)->sequence == cur_kf->sequence) { (*rit)->getPose(conncected_P, connected_R); posegraph_visualization->add_edge(P, conncected_P); } rit++; } } // show connections between loop frames, not needed if (SHOW_L_EDGE) { if (cur_kf->has_loop) { // printf("has loop \n"); KeyFrame *connected_KF = getKeyFrame(cur_kf->loop_index); Vector3d connected_P, P0; Matrix3d connected_R, R0; connected_KF->getPose(connected_P, connected_R); // cur_kf->getVioPose(P0, R0); cur_kf->getPose(P0, R0); if (cur_kf->sequence > 0) { // printf("add loop into visual \n"); posegraph_visualization->add_loopedge( P0, connected_P + Vector3d(VISUALIZATION_SHIFT_X, VISUALIZATION_SHIFT_Y, 0)); } } } // posegraph_visualization->add_pose(P + Vector3d(VISUALIZATION_SHIFT_X, // VISUALIZATION_SHIFT_Y, 0), Q); // add frame to key frame list keyframelist.push_back(cur_kf); publish(); m_keyframelist.unlock(); } void PoseGraph::loadKeyFrame(KeyFrame *cur_kf, bool flag_detect_loop) { cur_kf->index = global_index; global_index++; int loop_index = -1; if (flag_detect_loop) loop_index = detectLoop(cur_kf, cur_kf->index); else { addKeyFrameIntoVoc(cur_kf); } if (loop_index != -1) { printf(" %d detect loop with %d \n", cur_kf->index, loop_index); KeyFrame *old_kf = getKeyFrame(loop_index); if (cur_kf->findConnection(old_kf)) { if (earliest_loop_index > loop_index || earliest_loop_index == -1) earliest_loop_index = loop_index; m_optimize_buf.lock(); optimize_buf.push(cur_kf->index); m_optimize_buf.unlock(); } } m_keyframelist.lock(); Vector3d P; Matrix3d R; cur_kf->getPose(P, R); Quaterniond Q{R}; geometry_msgs::PoseStamped pose_stamped; pose_stamped.header.stamp = ros::Time(cur_kf->time_stamp); pose_stamped.header.frame_id = "map"; pose_stamped.pose.position.x = P.x() + VISUALIZATION_SHIFT_X; pose_stamped.pose.position.y = P.y() + VISUALIZATION_SHIFT_Y; pose_stamped.pose.position.z = P.z(); pose_stamped.pose.orientation.x = Q.x(); pose_stamped.pose.orientation.y = Q.y(); pose_stamped.pose.orientation.z = Q.z(); pose_stamped.pose.orientation.w = Q.w(); base_path.poses.push_back(pose_stamped); base_path.header = pose_stamped.header; // draw local connection if (SHOW_S_EDGE) { list::reverse_iterator rit = keyframelist.rbegin(); for (int i = 0; i < 1; i++) { if (rit == keyframelist.rend()) break; Vector3d conncected_P; Matrix3d connected_R; if ((*rit)->sequence == cur_kf->sequence) { (*rit)->getPose(conncected_P, connected_R); posegraph_visualization->add_edge(P, conncected_P); } rit++; } } /* if (cur_kf->has_loop) { KeyFrame* connected_KF = getKeyFrame(cur_kf->loop_index); Vector3d connected_P; Matrix3d connected_R; connected_KF->getPose(connected_P, connected_R); posegraph_visualization->add_loopedge(P, connected_P, SHIFT); } */ keyframelist.push_back(cur_kf); // publish(); m_keyframelist.unlock(); } KeyFrame *PoseGraph::getKeyFrame(int index) { // unique_lock lock(m_keyframelist); list::iterator it = keyframelist.begin(); for (; it != keyframelist.end(); it++) { if ((*it)->index == index) break; } if (it != keyframelist.end()) return *it; else return NULL; } int PoseGraph::detectLoop(KeyFrame *keyframe, int frame_index) { // put image into image_pool; for visualization cv::Mat compressed_image; // set false, not used if (DEBUG_IMAGE) { int feature_num = keyframe->keypoints.size(); cv::resize(keyframe->image, compressed_image, cv::Size(376, 240)); putText(compressed_image, "feature_num:" + to_string(feature_num), cv::Point2f(10, 10), cv::FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(255)); image_pool[frame_index] = compressed_image; } TicToc tmp_t; // first query; then add this frame into database! QueryResults ret; TicToc t_query; db.query(keyframe->brief_descriptors, ret, 4, frame_index - 50); // printf("query time: %f", t_query.toc()); // cout << "Searching for Image " << frame_index << ". " << ret << endl; TicToc t_add; db.add(keyframe->brief_descriptors); // printf("add feature time: %f", t_add.toc()); //------------------------------------------------------------------------------ // ret[0] is the nearest neighbour's score. threshold change with neighour // score //------------------------------------------------------------------------------ bool find_loop = false; cv::Mat loop_result; if (DEBUG_IMAGE) { loop_result = compressed_image.clone(); if (!ret.empty()) putText(loop_result, "neighbour score:" + to_string(ret[0].Score), cv::Point2f(10, 50), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255)); } // visual loop result if (DEBUG_IMAGE) { for (unsigned int i = 0; i < ret.size(); i++) { int tmp_index = ret[i].Id; auto it = image_pool.find(tmp_index); cv::Mat tmp_image = (it->second).clone(); putText(tmp_image, "index: " + to_string(tmp_index) + "loop score:" + to_string(ret[i].Score), cv::Point2f(10, 50), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255)); cv::hconcat(loop_result, tmp_image, loop_result); } } // a good match with its nerghbour if (ret.size() >= 1 && ret[0].Score > 0.05) { for (unsigned int i = 1; i < ret.size(); i++) { // if (ret[i].Score > ret[0].Score * 0.3) if (ret[i].Score > 0.015) { find_loop = true; int tmp_index = ret[i].Id; if (DEBUG_IMAGE && 0) { auto it = image_pool.find(tmp_index); cv::Mat tmp_image = (it->second).clone(); putText(tmp_image, "loop score:" + to_string(ret[i].Score), cv::Point2f(10, 50), cv::FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(255)); cv::hconcat(loop_result, tmp_image, loop_result); } } } } /* if (DEBUG_IMAGE) { cv::imshow("loop_result", loop_result); cv::waitKey(20); } */ if (find_loop && frame_index > 50) { int min_index = -1; for (unsigned int i = 0; i < ret.size(); i++) { if (min_index == -1 || (ret[i].Id < min_index && ret[i].Score > 0.015)) min_index = ret[i].Id; } return min_index; } else return -1; } void PoseGraph::addKeyFrameIntoVoc(KeyFrame *keyframe) { // put image into image_pool; for visualization cv::Mat compressed_image; if (DEBUG_IMAGE) { int feature_num = keyframe->keypoints.size(); cv::resize(keyframe->image, compressed_image, cv::Size(376, 240)); putText(compressed_image, "feature_num:" + to_string(feature_num), cv::Point2f(10, 10), cv::FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(255)); image_pool[keyframe->index] = compressed_image; } db.add(keyframe->brief_descriptors); } void PoseGraph::optimize4DoF() { while (true) { int cur_index = -1; int first_looped_index = -1; m_optimize_buf.lock(); while (!optimize_buf.empty()) { cur_index = optimize_buf.front(); first_looped_index = earliest_loop_index; optimize_buf.pop(); } m_optimize_buf.unlock(); if (cur_index != -1) { printf("optimize pose graph \n"); TicToc tmp_t; m_keyframelist.lock(); KeyFrame *cur_kf = getKeyFrame(cur_index); int max_length = cur_index + 1; // w^t_i w^q_i double t_array[max_length][3]; Quaterniond q_array[max_length]; double euler_array[max_length][3]; double sequence_array[max_length]; ceres::Problem problem; ceres::Solver::Options options; options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY; // options.minimizer_progress_to_stdout = true; // options.max_solver_time_in_seconds = SOLVER_TIME * 3; options.max_num_iterations = 5; ceres::Solver::Summary summary; ceres::LossFunction *loss_function; loss_function = new ceres::HuberLoss(0.1); // loss_function = new ceres::CauchyLoss(1.0); ceres::LocalParameterization *angle_local_parameterization = AngleLocalParameterization::Create(); list::iterator it; int i = 0; for (it = keyframelist.begin(); it != keyframelist.end(); it++) { if ((*it)->index < first_looped_index) continue; (*it)->local_index = i; Quaterniond tmp_q; Matrix3d tmp_r; Vector3d tmp_t; (*it)->getVioPose(tmp_t, tmp_r); tmp_q = tmp_r; t_array[i][0] = tmp_t(0); t_array[i][1] = tmp_t(1); t_array[i][2] = tmp_t(2); q_array[i] = tmp_q; Vector3d euler_angle = Utility::R2ypr(tmp_q.toRotationMatrix()); euler_array[i][0] = euler_angle.x(); euler_array[i][1] = euler_angle.y(); euler_array[i][2] = euler_angle.z(); sequence_array[i] = (*it)->sequence; problem.AddParameterBlock(euler_array[i], 1, angle_local_parameterization); problem.AddParameterBlock(t_array[i], 3); if ((*it)->index == first_looped_index || (*it)->sequence == 0) { problem.SetParameterBlockConstant(euler_array[i]); problem.SetParameterBlockConstant(t_array[i]); } // add edge for (int j = 1; j < 5; j++) { if (i - j >= 0 && sequence_array[i] == sequence_array[i - j]) { Vector3d euler_conncected = Utility::R2ypr(q_array[i - j].toRotationMatrix()); Vector3d relative_t(t_array[i][0] - t_array[i - j][0], t_array[i][1] - t_array[i - j][1], t_array[i][2] - t_array[i - j][2]); relative_t = q_array[i - j].inverse() * relative_t; double relative_yaw = euler_array[i][0] - euler_array[i - j][0]; ceres::CostFunction *cost_function = FourDOFError::Create( relative_t.x(), relative_t.y(), relative_t.z(), relative_yaw, euler_conncected.y(), euler_conncected.z()); problem.AddResidualBlock(cost_function, NULL, euler_array[i - j], t_array[i - j], euler_array[i], t_array[i]); } } // add loop edge if ((*it)->has_loop) { assert((*it)->loop_index >= first_looped_index); int connected_index = getKeyFrame((*it)->loop_index)->local_index; Vector3d euler_conncected = Utility::R2ypr(q_array[connected_index].toRotationMatrix()); Vector3d relative_t; relative_t = (*it)->getLoopRelativeT(); double relative_yaw = (*it)->getLoopRelativeYaw(); ceres::CostFunction *cost_function = FourDOFWeightError::Create( relative_t.x(), relative_t.y(), relative_t.z(), relative_yaw, euler_conncected.y(), euler_conncected.z()); problem.AddResidualBlock( cost_function, loss_function, euler_array[connected_index], t_array[connected_index], euler_array[i], t_array[i]); } if ((*it)->index == cur_index) break; i++; } m_keyframelist.unlock(); ceres::Solve(options, &problem, &summary); // std::cout << summary.BriefReport() << "\n"; // printf("pose optimization time: %f \n", tmp_t.toc()); /* for (int j = 0 ; j < i; j++) { printf("optimize i: %d p: %f, %f, %f\n", j, t_array[j][0], t_array[j][1], t_array[j][2] ); } */ m_keyframelist.lock(); i = 0; for (it = keyframelist.begin(); it != keyframelist.end(); it++) { if ((*it)->index < first_looped_index) continue; Quaterniond tmp_q; tmp_q = Utility::ypr2R( Vector3d(euler_array[i][0], euler_array[i][1], euler_array[i][2])); Vector3d tmp_t = Vector3d(t_array[i][0], t_array[i][1], t_array[i][2]); Matrix3d tmp_r = tmp_q.toRotationMatrix(); (*it)->updatePose(tmp_t, tmp_r); if ((*it)->index == cur_index) break; i++; } Vector3d cur_t, vio_t; Matrix3d cur_r, vio_r; cur_kf->getPose(cur_t, cur_r); cur_kf->getVioPose(vio_t, vio_r); m_drift.lock(); yaw_drift = Utility::R2ypr(cur_r).x() - Utility::R2ypr(vio_r).x(); r_drift = Utility::ypr2R(Vector3d(yaw_drift, 0, 0)); t_drift = cur_t - r_drift * vio_t; m_drift.unlock(); // cout << "t_drift " << t_drift.transpose() << endl; // cout << "r_drift " << Utility::R2ypr(r_drift).transpose() << endl; // cout << "yaw drift " << yaw_drift << endl; it++; for (; it != keyframelist.end(); it++) { Vector3d P; Matrix3d R; (*it)->getVioPose(P, R); P = r_drift * P + t_drift; R = r_drift * R; (*it)->updatePose(P, R); } m_keyframelist.unlock(); updatePath(); } std::chrono::milliseconds dura(2000); std::this_thread::sleep_for(dura); } } void PoseGraph::optimize6DoF() { while (true) { int cur_index = -1; int first_looped_index = -1; m_optimize_buf.lock(); while (!optimize_buf.empty()) { cur_index = optimize_buf.front(); first_looped_index = earliest_loop_index; optimize_buf.pop(); } m_optimize_buf.unlock(); if (cur_index != -1) { printf("optimize pose graph \n"); TicToc tmp_t; m_keyframelist.lock(); KeyFrame *cur_kf = getKeyFrame(cur_index); int max_length = cur_index + 1; // w^t_i w^q_i double t_array[max_length][3]; double q_array[max_length][4]; double sequence_array[max_length]; ceres::Problem problem; ceres::Solver::Options options; options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY; // ptions.minimizer_progress_to_stdout = true; // options.max_solver_time_in_seconds = SOLVER_TIME * 3; options.max_num_iterations = 5; ceres::Solver::Summary summary; ceres::LossFunction *loss_function; loss_function = new ceres::HuberLoss(0.1); // loss_function = new ceres::CauchyLoss(1.0); ceres::LocalParameterization *local_parameterization = new ceres::QuaternionParameterization(); list::iterator it; int i = 0; for (it = keyframelist.begin(); it != keyframelist.end(); it++) { if ((*it)->index < first_looped_index) continue; (*it)->local_index = i; Quaterniond tmp_q; Matrix3d tmp_r; Vector3d tmp_t; (*it)->getVioPose(tmp_t, tmp_r); tmp_q = tmp_r; t_array[i][0] = tmp_t(0); t_array[i][1] = tmp_t(1); t_array[i][2] = tmp_t(2); q_array[i][0] = tmp_q.w(); q_array[i][1] = tmp_q.x(); q_array[i][2] = tmp_q.y(); q_array[i][3] = tmp_q.z(); sequence_array[i] = (*it)->sequence; problem.AddParameterBlock(q_array[i], 4, local_parameterization); problem.AddParameterBlock(t_array[i], 3); if ((*it)->index == first_looped_index || (*it)->sequence == 0) { problem.SetParameterBlockConstant(q_array[i]); problem.SetParameterBlockConstant(t_array[i]); } // add edge for (int j = 1; j < 5; j++) { if (i - j >= 0 && sequence_array[i] == sequence_array[i - j]) { Vector3d relative_t(t_array[i][0] - t_array[i - j][0], t_array[i][1] - t_array[i - j][1], t_array[i][2] - t_array[i - j][2]); Quaterniond q_i_j = Quaterniond(q_array[i - j][0], q_array[i - j][1], q_array[i - j][2], q_array[i - j][3]); Quaterniond q_i = Quaterniond(q_array[i][0], q_array[i][1], q_array[i][2], q_array[i][3]); relative_t = q_i_j.inverse() * relative_t; Quaterniond relative_q = q_i_j.inverse() * q_i; ceres::CostFunction *vo_function = RelativeRTError::Create( relative_t.x(), relative_t.y(), relative_t.z(), relative_q.w(), relative_q.x(), relative_q.y(), relative_q.z(), 0.1, 0.01); problem.AddResidualBlock(vo_function, NULL, q_array[i - j], t_array[i - j], q_array[i], t_array[i]); } } // add loop edge if ((*it)->has_loop) { assert((*it)->loop_index >= first_looped_index); int connected_index = getKeyFrame((*it)->loop_index)->local_index; Vector3d relative_t; relative_t = (*it)->getLoopRelativeT(); Quaterniond relative_q; relative_q = (*it)->getLoopRelativeQ(); ceres::CostFunction *loop_function = RelativeRTError::Create( relative_t.x(), relative_t.y(), relative_t.z(), relative_q.w(), relative_q.x(), relative_q.y(), relative_q.z(), 0.1, 0.01); problem.AddResidualBlock( loop_function, loss_function, q_array[connected_index], t_array[connected_index], q_array[i], t_array[i]); } if ((*it)->index == cur_index) break; i++; } m_keyframelist.unlock(); ceres::Solve(options, &problem, &summary); // std::cout << summary.BriefReport() << "\n"; // printf("pose optimization time: %f \n", tmp_t.toc()); /* for (int j = 0 ; j < i; j++) { printf("optimize i: %d p: %f, %f, %f\n", j, t_array[j][0], t_array[j][1], t_array[j][2] ); } */ m_keyframelist.lock(); i = 0; for (it = keyframelist.begin(); it != keyframelist.end(); it++) { if ((*it)->index < first_looped_index) continue; Quaterniond tmp_q(q_array[i][0], q_array[i][1], q_array[i][2], q_array[i][3]); Vector3d tmp_t = Vector3d(t_array[i][0], t_array[i][1], t_array[i][2]); Matrix3d tmp_r = tmp_q.toRotationMatrix(); (*it)->updatePose(tmp_t, tmp_r); if ((*it)->index == cur_index) break; i++; } Vector3d cur_t, vio_t; Matrix3d cur_r, vio_r; cur_kf->getPose(cur_t, cur_r); cur_kf->getVioPose(vio_t, vio_r); m_drift.lock(); r_drift = cur_r * vio_r.transpose(); t_drift = cur_t - r_drift * vio_t; m_drift.unlock(); // cout << "t_drift " << t_drift.transpose() << endl; // cout << "r_drift " << Utility::R2ypr(r_drift).transpose() << endl; it++; for (; it != keyframelist.end(); it++) { Vector3d P; Matrix3d R; (*it)->getVioPose(P, R); P = r_drift * P + t_drift; R = r_drift * R; (*it)->updatePose(P, R); } m_keyframelist.unlock(); updatePath(); } std::chrono::milliseconds dura(2000); std::this_thread::sleep_for(dura); } return; } void PoseGraph::updatePath() { m_keyframelist.lock(); list::iterator it; for (int i = 1; i <= sequence_cnt; i++) { path[i].poses.clear(); } base_path.poses.clear(); posegraph_visualization->reset(); // not used if (SAVE_LOOP_PATH) { ofstream loop_path_file_tmp(VINS_RESULT_PATH, ios::out); loop_path_file_tmp.close(); } for (it = keyframelist.begin(); it != keyframelist.end(); it++) { Vector3d P; Matrix3d R; (*it)->getPose(P, R); Quaterniond Q; Q = R; // printf("path p: %f, %f, %f\n", P.x(), P.z(), P.y() ); geometry_msgs::PoseStamped pose_stamped; pose_stamped.header.stamp = ros::Time((*it)->time_stamp); pose_stamped.header.frame_id = "map"; pose_stamped.pose.position.x = P.x() + VISUALIZATION_SHIFT_X; pose_stamped.pose.position.y = P.y() + VISUALIZATION_SHIFT_Y; pose_stamped.pose.position.z = P.z(); pose_stamped.pose.orientation.x = Q.x(); pose_stamped.pose.orientation.y = Q.y(); pose_stamped.pose.orientation.z = Q.z(); pose_stamped.pose.orientation.w = Q.w(); if ((*it)->sequence == 0) { base_path.poses.push_back(pose_stamped); base_path.header = pose_stamped.header; } else { path[(*it)->sequence].poses.push_back(pose_stamped); path[(*it)->sequence].header = pose_stamped.header; } // not used if (SAVE_LOOP_PATH) { ofstream loop_path_file(VINS_RESULT_PATH, ios::app); loop_path_file.setf(ios::fixed, ios::floatfield); loop_path_file.precision(0); loop_path_file << (*it)->time_stamp * 1e9 << ","; loop_path_file.precision(5); loop_path_file << P.x() << "," << P.y() << "," << P.z() << "," << Q.w() << "," << Q.x() << "," << Q.y() << "," << Q.z() << "," << endl; loop_path_file.close(); } // draw local connection // not used if (SHOW_S_EDGE) { list::reverse_iterator rit = keyframelist.rbegin(); list::reverse_iterator lrit; for (; rit != keyframelist.rend(); rit++) { if ((*rit)->index == (*it)->index) { lrit = rit; lrit++; for (int i = 0; i < 4; i++) { if (lrit == keyframelist.rend()) break; if ((*lrit)->sequence == (*it)->sequence) { Vector3d conncected_P; Matrix3d connected_R; (*lrit)->getPose(conncected_P, connected_R); posegraph_visualization->add_edge(P, conncected_P); } lrit++; } break; } } } if (SHOW_L_EDGE) { if ((*it)->has_loop && (*it)->sequence == sequence_cnt) { KeyFrame *connected_KF = getKeyFrame((*it)->loop_index); Vector3d connected_P; Matrix3d connected_R; connected_KF->getPose(connected_P, connected_R); //(*it)->getVioPose(P, R); (*it)->getPose(P, R); if ((*it)->sequence > 0) { posegraph_visualization->add_loopedge( P, connected_P + Vector3d(VISUALIZATION_SHIFT_X, VISUALIZATION_SHIFT_Y, 0)); } } } } publish(); m_keyframelist.unlock(); } void PoseGraph::savePoseGraph() { m_keyframelist.lock(); TicToc tmp_t; FILE *pFile, *pFile_shan_pg, *pFile_shan_vio; printf("pose graph path: %s\n", POSE_GRAPH_SAVE_PATH.c_str()); printf("pose graph saving... \n"); string file_path = POSE_GRAPH_SAVE_PATH + "pose_graph.txt"; string file_path_shan_pg = POSE_GRAPH_SAVE_PATH + "stamped_traj_estimate_mono_pg.txt"; string file_path_shan_vio = POSE_GRAPH_SAVE_PATH + "stamped_traj_estimate_mono_vio.txt"; // string file_path_shan_pg = // "/home/shanzy/rpg_trajectory_evaluation/results/laptop/vio_mono/laptop_vio_mono_MH_01/stamped_traj_estimate.txt"; // string file_path_shan_vio = // "/home/shanzy/rpg_trajectory_evaluation/results/laptop/vio_mono/laptop_vio_mono_MH_01/vio_stamped_traj_estimate.txt"; pFile = fopen(file_path.c_str(), "w"); pFile_shan_pg = fopen(file_path_shan_pg.c_str(), "w"); pFile_shan_vio = fopen(file_path_shan_vio.c_str(), "w"); // fprintf(pFile, "index time_stamp Tx Ty Tz Qw Qx Qy Qz loop_index // loop_info\n"); list::iterator it; for (it = keyframelist.begin(); it != keyframelist.end(); it++) { std::string image_path, descriptor_path, brief_path, keypoints_path; if (DEBUG_IMAGE) { image_path = POSE_GRAPH_SAVE_PATH + to_string((*it)->index) + "_image.png"; imwrite(image_path.c_str(), (*it)->image); } Quaterniond VIO_tmp_Q{(*it)->vio_R_w_i}; Quaterniond PG_tmp_Q{(*it)->R_w_i}; Quaterniond rVIO_tmp_Q{(*it)->vio_R_w_i.transpose()}; Quaterniond rPG_tmp_Q{(*it)->R_w_i.transpose()}; Vector3d VIO_tmp_T = (*it)->vio_T_w_i; Vector3d PG_tmp_T = (*it)->T_w_i; fprintf(pFile, " %d %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %d %f %f %f %f " "%f %f %f %f %d\n", (*it)->index, (*it)->time_stamp, VIO_tmp_T.x(), VIO_tmp_T.y(), VIO_tmp_T.z(), PG_tmp_T.x(), PG_tmp_T.y(), PG_tmp_T.z(), VIO_tmp_Q.w(), VIO_tmp_Q.x(), VIO_tmp_Q.y(), VIO_tmp_Q.z(), PG_tmp_Q.w(), PG_tmp_Q.x(), PG_tmp_Q.y(), PG_tmp_Q.z(), (*it)->loop_index, (*it)->loop_info(0), (*it)->loop_info(1), (*it)->loop_info(2), (*it)->loop_info(3), (*it)->loop_info(4), (*it)->loop_info(5), (*it)->loop_info(6), (*it)->loop_info(7), (int)(*it)->keypoints.size()); fprintf(pFile_shan_pg, "%f %f %f %f %f %f %f %f\n", (*it)->time_stamp, PG_tmp_T.x(), PG_tmp_T.y(), PG_tmp_T.z(), PG_tmp_Q.x(), PG_tmp_Q.y(), PG_tmp_Q.z(), PG_tmp_Q.w()); fprintf(pFile_shan_vio, "%f %f %f %f %f %f %f %f\n", (*it)->time_stamp, VIO_tmp_T.x(), VIO_tmp_T.y(), VIO_tmp_T.z(), VIO_tmp_Q.x(), VIO_tmp_Q.y(), VIO_tmp_Q.z(), VIO_tmp_Q.w()); // write keypoints, brief_descriptors vector keypoints // vector brief_descriptors; assert((*it)->keypoints.size() == (*it)->brief_descriptors.size()); brief_path = POSE_GRAPH_SAVE_PATH + to_string((*it)->index) + "_briefdes.dat"; std::ofstream brief_file(brief_path, std::ios::binary); keypoints_path = POSE_GRAPH_SAVE_PATH + to_string((*it)->index) + "_keypoints.txt"; FILE *keypoints_file; keypoints_file = fopen(keypoints_path.c_str(), "w"); for (int i = 0; i < (int)(*it)->keypoints.size(); i++) { brief_file << (*it)->brief_descriptors[i] << endl; fprintf(keypoints_file, "%f %f %f %f\n", (*it)->keypoints[i].pt.x, (*it)->keypoints[i].pt.y, (*it)->keypoints_norm[i].pt.x, (*it)->keypoints_norm[i].pt.y); } brief_file.close(); fclose(keypoints_file); } fclose(pFile_shan_vio); fclose(pFile_shan_pg); fclose(pFile); printf("save pose graph time: %f s\n", tmp_t.toc() / 1000); m_keyframelist.unlock(); } void PoseGraph::loadPoseGraph() { TicToc tmp_t; FILE *pFile; string file_path = POSE_GRAPH_SAVE_PATH + "pose_graph.txt"; printf("lode pose graph from: %s \n", file_path.c_str()); printf("pose graph loading...\n"); pFile = fopen(file_path.c_str(), "r"); if (pFile == NULL) { printf( "lode previous pose graph error: wrong previous pose graph path or no " "previous pose graph \n the system will start with new pose graph \n"); return; } int index; double time_stamp; double VIO_Tx, VIO_Ty, VIO_Tz; double PG_Tx, PG_Ty, PG_Tz; double VIO_Qw, VIO_Qx, VIO_Qy, VIO_Qz; double PG_Qw, PG_Qx, PG_Qy, PG_Qz; double loop_info_0, loop_info_1, loop_info_2, loop_info_3; double loop_info_4, loop_info_5, loop_info_6, loop_info_7; int loop_index; int keypoints_num; Eigen::Matrix loop_info; int cnt = 0; while (fscanf(pFile, "%d %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf " "%lf %d %lf %lf %lf %lf %lf %lf %lf %lf %d", &index, &time_stamp, &VIO_Tx, &VIO_Ty, &VIO_Tz, &PG_Tx, &PG_Ty, &PG_Tz, &VIO_Qw, &VIO_Qx, &VIO_Qy, &VIO_Qz, &PG_Qw, &PG_Qx, &PG_Qy, &PG_Qz, &loop_index, &loop_info_0, &loop_info_1, &loop_info_2, &loop_info_3, &loop_info_4, &loop_info_5, &loop_info_6, &loop_info_7, &keypoints_num) != EOF) { /* printf("I read: %d %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %d %lf %lf %lf %lf %lf %lf %lf %lf %d\n", index, time_stamp, VIO_Tx, VIO_Ty, VIO_Tz, PG_Tx, PG_Ty, PG_Tz, VIO_Qw, VIO_Qx, VIO_Qy, VIO_Qz, PG_Qw, PG_Qx, PG_Qy, PG_Qz, loop_index, loop_info_0, loop_info_1, loop_info_2, loop_info_3, loop_info_4, loop_info_5, loop_info_6, loop_info_7, keypoints_num); */ cv::Mat image; std::string image_path, descriptor_path; if (DEBUG_IMAGE) { image_path = POSE_GRAPH_SAVE_PATH + to_string(index) + "_image.png"; image = cv::imread(image_path.c_str(), 0); } Vector3d VIO_T(VIO_Tx, VIO_Ty, VIO_Tz); Vector3d PG_T(PG_Tx, PG_Ty, PG_Tz); Quaterniond VIO_Q; VIO_Q.w() = VIO_Qw; VIO_Q.x() = VIO_Qx; VIO_Q.y() = VIO_Qy; VIO_Q.z() = VIO_Qz; Quaterniond PG_Q; PG_Q.w() = PG_Qw; PG_Q.x() = PG_Qx; PG_Q.y() = PG_Qy; PG_Q.z() = PG_Qz; Matrix3d VIO_R, PG_R; VIO_R = VIO_Q.toRotationMatrix(); PG_R = PG_Q.toRotationMatrix(); Eigen::Matrix loop_info; loop_info << loop_info_0, loop_info_1, loop_info_2, loop_info_3, loop_info_4, loop_info_5, loop_info_6, loop_info_7; if (loop_index != -1) if (earliest_loop_index > loop_index || earliest_loop_index == -1) { earliest_loop_index = loop_index; } // load keypoints, brief_descriptors string brief_path = POSE_GRAPH_SAVE_PATH + to_string(index) + "_briefdes.dat"; std::ifstream brief_file(brief_path, std::ios::binary); string keypoints_path = POSE_GRAPH_SAVE_PATH + to_string(index) + "_keypoints.txt"; FILE *keypoints_file; keypoints_file = fopen(keypoints_path.c_str(), "r"); vector keypoints; vector keypoints_norm; vector brief_descriptors; for (int i = 0; i < keypoints_num; i++) { BRIEF::bitset tmp_des; brief_file >> tmp_des; brief_descriptors.push_back(tmp_des); cv::KeyPoint tmp_keypoint; cv::KeyPoint tmp_keypoint_norm; double p_x, p_y, p_x_norm, p_y_norm; if (!fscanf(keypoints_file, "%lf %lf %lf %lf", &p_x, &p_y, &p_x_norm, &p_y_norm)) printf(" fail to load pose graph \n"); tmp_keypoint.pt.x = p_x; tmp_keypoint.pt.y = p_y; tmp_keypoint_norm.pt.x = p_x_norm; tmp_keypoint_norm.pt.y = p_y_norm; keypoints.push_back(tmp_keypoint); keypoints_norm.push_back(tmp_keypoint_norm); } brief_file.close(); fclose(keypoints_file); KeyFrame *keyframe = new KeyFrame( time_stamp, index, VIO_T, VIO_R, PG_T, PG_R, image, loop_index, loop_info, keypoints, keypoints_norm, brief_descriptors); loadKeyFrame(keyframe, 0); if (cnt % 20 == 0) { publish(); } cnt++; } fclose(pFile); printf("load pose graph time: %f s\n", tmp_t.toc() / 1000); base_sequence = 0; } void PoseGraph::publish() { for (int i = 1; i <= sequence_cnt; i++) { // if (sequence_loop[i] == true || i == base_sequence) if (1 || i == base_sequence) { pub_pg_path.publish(path[i]); pub_path[i].publish(path[i]); // posegraph_visualization->publish_by(pub_pose_graph, // path[sequence_cnt].header); } } pub_base_path.publish(base_path); // posegraph_visualization->publish_by(pub_pose_graph, // path[sequence_cnt].header); } void PoseGraph::updateKeyFrameLoop(int index, Eigen::Matrix &_loop_info) { KeyFrame *kf = getKeyFrame(index); kf->updateLoop(_loop_info); if (abs(_loop_info(7)) < 30.0 && Vector3d(_loop_info(0), _loop_info(1), _loop_info(2)).norm() < 20.0) { if (FAST_RELOCALIZATION) { KeyFrame *old_kf = getKeyFrame(kf->loop_index); Vector3d w_P_old, w_P_cur, vio_P_cur; Matrix3d w_R_old, w_R_cur, vio_R_cur; old_kf->getPose(w_P_old, w_R_old); kf->getVioPose(vio_P_cur, vio_R_cur); Vector3d relative_t; Quaterniond relative_q; relative_t = kf->getLoopRelativeT(); relative_q = (kf->getLoopRelativeQ()).toRotationMatrix(); w_P_cur = w_R_old * relative_t + w_P_old; w_R_cur = w_R_old * relative_q; double shift_yaw; Matrix3d shift_r; Vector3d shift_t; shift_yaw = Utility::R2ypr(w_R_cur).x() - Utility::R2ypr(vio_R_cur).x(); shift_r = Utility::ypr2R(Vector3d(shift_yaw, 0, 0)); shift_t = w_P_cur - w_R_cur * vio_R_cur.transpose() * vio_P_cur; m_drift.lock(); yaw_drift = shift_yaw; r_drift = shift_r; t_drift = shift_t; m_drift.unlock(); } } } ================================================ FILE: pose_graph/src/pose_graph/pose_graph.h ================================================ #pragma once #include "../ThirdParty/DBoW/DBoW2.h" #include "../ThirdParty/DBoW/TemplatedDatabase.h" #include "../ThirdParty/DBoW/TemplatedVocabulary.h" #include "../ThirdParty/DVision/DVision.h" #include "../keyframe/keyframe.h" #include "../utility/CameraPoseVisualization.h" #include "../utility/tic_toc.h" #include "../utility/utility.h" #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #define SHOW_S_EDGE false #define SHOW_L_EDGE false #define SAVE_LOOP_PATH true using namespace DVision; using namespace DBoW2; class PoseGraph { public: PoseGraph(); ~PoseGraph(); void registerPub(ros::NodeHandle &n); void addKeyFrame(KeyFrame *cur_kf, bool flag_detect_loop); void loadKeyFrame(KeyFrame *cur_kf, bool flag_detect_loop); void loadVocabulary(const std::string& voc_path); void setIMUFlag(bool _use_imu); void updateKeyFrameLoop(int index, Eigen::Matrix &_loop_info); KeyFrame *getKeyFrame(int index); nav_msgs::Path path[10]; nav_msgs::Path base_path; CameraPoseVisualization *posegraph_visualization; void savePoseGraph(); void loadPoseGraph(); void publish(); // vio ----> cur(闭环后的) Vector3d t_drift; double yaw_drift; Matrix3d r_drift; // map frame( base sequence or first sequence)<----> cur sequence frame Vector3d w_t_vio; Matrix3d w_r_vio; private: int detectLoop(KeyFrame *keyframe, int frame_index); void addKeyFrameIntoVoc(KeyFrame *keyframe); void optimize4DoF(); void optimize6DoF(); void updatePath(); list keyframelist; std::mutex m_keyframelist; std::mutex m_optimize_buf; std::mutex m_path; std::mutex m_drift; std::thread t_optimization; std::queue optimize_buf; int global_index; int sequence_cnt; vector sequence_loop; map image_pool; int earliest_loop_index; int base_sequence; bool use_imu; BriefDatabase db; BriefVocabulary *voc{}; ros::Publisher pub_pg_path; ros::Publisher pub_base_path; ros::Publisher pub_pose_graph; ros::Publisher pub_path[10]; }; template inline void QuaternionInverse(const T q[4], T q_inverse[4]) { q_inverse[0] = q[0]; q_inverse[1] = -q[1]; q_inverse[2] = -q[2]; q_inverse[3] = -q[3]; }; template T NormalizeAngle(const T &angle_degrees) { if (angle_degrees > T(180.0)) return angle_degrees - T(360.0); else if (angle_degrees < T(-180.0)) return angle_degrees + T(360.0); else return angle_degrees; }; class AngleLocalParameterization { public: template bool operator()(const T *theta_radians, const T *delta_theta_radians, T *theta_radians_plus_delta) const { *theta_radians_plus_delta = NormalizeAngle(*theta_radians + *delta_theta_radians); return true; } static ceres::LocalParameterization *Create() { return (new ceres::AutoDiffLocalParameterization); } }; template void YawPitchRollToRotationMatrix(const T yaw, const T pitch, const T roll, T R[9]) { T y = yaw / T(180.0) * T(M_PI); T p = pitch / T(180.0) * T(M_PI); T r = roll / T(180.0) * T(M_PI); R[0] = cos(y) * cos(p); R[1] = -sin(y) * cos(r) + cos(y) * sin(p) * sin(r); R[2] = sin(y) * sin(r) + cos(y) * sin(p) * cos(r); R[3] = sin(y) * cos(p); R[4] = cos(y) * cos(r) + sin(y) * sin(p) * sin(r); R[5] = -cos(y) * sin(r) + sin(y) * sin(p) * cos(r); R[6] = -sin(p); R[7] = cos(p) * sin(r); R[8] = cos(p) * cos(r); }; template void RotationMatrixTranspose(const T R[9], T inv_R[9]) { inv_R[0] = R[0]; inv_R[1] = R[3]; inv_R[2] = R[6]; inv_R[3] = R[1]; inv_R[4] = R[4]; inv_R[5] = R[7]; inv_R[6] = R[2]; inv_R[7] = R[5]; inv_R[8] = R[8]; }; template void RotationMatrixRotatePoint(const T R[9], const T t[3], T r_t[3]) { r_t[0] = R[0] * t[0] + R[1] * t[1] + R[2] * t[2]; r_t[1] = R[3] * t[0] + R[4] * t[1] + R[5] * t[2]; r_t[2] = R[6] * t[0] + R[7] * t[1] + R[8] * t[2]; }; struct FourDOFError { FourDOFError(double t_x, double t_y, double t_z, double relative_yaw, double pitch_i, double roll_i) : t_x(t_x), t_y(t_y), t_z(t_z), relative_yaw(relative_yaw), pitch_i(pitch_i), roll_i(roll_i) {} template bool operator()(const T *const yaw_i, const T *ti, const T *yaw_j, const T *tj, T *residuals) const { T t_w_ij[3]; t_w_ij[0] = tj[0] - ti[0]; t_w_ij[1] = tj[1] - ti[1]; t_w_ij[2] = tj[2] - ti[2]; // euler to rotation T w_R_i[9]; YawPitchRollToRotationMatrix(yaw_i[0], T(pitch_i), T(roll_i), w_R_i); // rotation transpose T i_R_w[9]; RotationMatrixTranspose(w_R_i, i_R_w); // rotation matrix rotate point T t_i_ij[3]; RotationMatrixRotatePoint(i_R_w, t_w_ij, t_i_ij); residuals[0] = (t_i_ij[0] - T(t_x)); residuals[1] = (t_i_ij[1] - T(t_y)); residuals[2] = (t_i_ij[2] - T(t_z)); residuals[3] = NormalizeAngle(yaw_j[0] - yaw_i[0] - T(relative_yaw)); return true; } static ceres::CostFunction * Create(const double t_x, const double t_y, const double t_z, const double relative_yaw, const double pitch_i, const double roll_i) { return (new ceres::AutoDiffCostFunction( new FourDOFError(t_x, t_y, t_z, relative_yaw, pitch_i, roll_i))); } double t_x, t_y, t_z; double relative_yaw, pitch_i, roll_i; }; struct FourDOFWeightError { FourDOFWeightError(double t_x, double t_y, double t_z, double relative_yaw, double pitch_i, double roll_i) : t_x(t_x), t_y(t_y), t_z(t_z), relative_yaw(relative_yaw), pitch_i(pitch_i), roll_i(roll_i) { weight = 1; } template bool operator()(const T *const yaw_i, const T *ti, const T *yaw_j, const T *tj, T *residuals) const { T t_w_ij[3]; t_w_ij[0] = tj[0] - ti[0]; t_w_ij[1] = tj[1] - ti[1]; t_w_ij[2] = tj[2] - ti[2]; // euler to rotation T w_R_i[9]; YawPitchRollToRotationMatrix(yaw_i[0], T(pitch_i), T(roll_i), w_R_i); // rotation transpose T i_R_w[9]; RotationMatrixTranspose(w_R_i, i_R_w); // rotation matrix rotate point T t_i_ij[3]; RotationMatrixRotatePoint(i_R_w, t_w_ij, t_i_ij); residuals[0] = (t_i_ij[0] - T(t_x)) * T(weight); residuals[1] = (t_i_ij[1] - T(t_y)) * T(weight); residuals[2] = (t_i_ij[2] - T(t_z)) * T(weight); residuals[3] = NormalizeAngle((yaw_j[0] - yaw_i[0] - T(relative_yaw))) * T(weight) / T(10.0); return true; } static ceres::CostFunction * Create(const double t_x, const double t_y, const double t_z, const double relative_yaw, const double pitch_i, const double roll_i) { return (new ceres::AutoDiffCostFunction( new FourDOFWeightError(t_x, t_y, t_z, relative_yaw, pitch_i, roll_i))); } double t_x, t_y, t_z; double relative_yaw, pitch_i, roll_i; double weight; }; struct RelativeRTError { RelativeRTError(double t_x, double t_y, double t_z, double q_w, double q_x, double q_y, double q_z, double t_var, double q_var) : t_x(t_x), t_y(t_y), t_z(t_z), q_w(q_w), q_x(q_x), q_y(q_y), q_z(q_z), t_var(t_var), q_var(q_var) {} template bool operator()(const T *const w_q_i, const T *ti, const T *w_q_j, const T *tj, T *residuals) const { T t_w_ij[3]; t_w_ij[0] = tj[0] - ti[0]; t_w_ij[1] = tj[1] - ti[1]; t_w_ij[2] = tj[2] - ti[2]; T i_q_w[4]; QuaternionInverse(w_q_i, i_q_w); T t_i_ij[3]; ceres::QuaternionRotatePoint(i_q_w, t_w_ij, t_i_ij); residuals[0] = (t_i_ij[0] - T(t_x)) / T(t_var); residuals[1] = (t_i_ij[1] - T(t_y)) / T(t_var); residuals[2] = (t_i_ij[2] - T(t_z)) / T(t_var); T relative_q[4]; relative_q[0] = T(q_w); relative_q[1] = T(q_x); relative_q[2] = T(q_y); relative_q[3] = T(q_z); T q_i_j[4]; ceres::QuaternionProduct(i_q_w, w_q_j, q_i_j); T relative_q_inv[4]; QuaternionInverse(relative_q, relative_q_inv); T error_q[4]; ceres::QuaternionProduct(relative_q_inv, q_i_j, error_q); residuals[3] = T(2) * error_q[1] / T(q_var); residuals[4] = T(2) * error_q[2] / T(q_var); residuals[5] = T(2) * error_q[3] / T(q_var); return true; } static ceres::CostFunction *Create(const double t_x, const double t_y, const double t_z, const double q_w, const double q_x, const double q_y, const double q_z, const double t_var, const double q_var) { return (new ceres::AutoDiffCostFunction( new RelativeRTError(t_x, t_y, t_z, q_w, q_x, q_y, q_z, t_var, q_var))); } double t_x, t_y, t_z, t_norm; double q_w, q_x, q_y, q_z; double t_var, q_var; }; ================================================ FILE: pose_graph/src/pose_graph_nodelet.cpp ================================================ #include #include #include #include #include #include #include #include #include "keyframe/keyframe.h" #include "pose_graph/pose_graph.h" #include "utility/CameraPoseVisualization.h" #include "utility/tic_toc.h" #include #include #include #include #include #include #include #include #include #include // 基类Nodelet所在的头文件 #include #define SKIP_FIRST_CNT 10 camodocal::CameraPtr m_camera; Eigen::Vector3d tic; Eigen::Matrix3d qic; Eigen::Matrix ti_d; Eigen::Matrix qi_d; ros::Publisher pub_match_img; ros::Publisher pub_match_points; int VISUALIZATION_SHIFT_X; int VISUALIZATION_SHIFT_Y; std::string BRIEF_PATTERN_FILE; std::string POSE_GRAPH_SAVE_PATH; int ROW; int COL; std::string VINS_RESULT_PATH; int DEBUG_IMAGE; int FAST_RELOCALIZATION; namespace pose_graph_nodelet_ns { class PoseGraphNodelet : public nodelet::Nodelet //任何nodelet plugin都要继承Nodelet类。 { public: PoseGraphNodelet() { frame_index = 0; sequence = 1; skip_first_cnt = 0; skip_cnt = 0; load_flag = 0; start_flag = 0; SKIP_DIS = 0; last_image_time = -1; last_t = Eigen::Vector3d(-100, -100, -100); cameraposevisual = new CameraPoseVisualization(1, 0, 0, 1); } private: void onInit() override { ros::NodeHandle &pn = getPrivateNodeHandle(); ros::NodeHandle &nh = getMTNodeHandle(); posegraph.registerPub(nh); // read param pn.getParam("visualization_shift_x", VISUALIZATION_SHIFT_X); pn.getParam("visualization_shift_y", VISUALIZATION_SHIFT_Y); pn.getParam("skip_cnt", SKIP_CNT); pn.getParam("skip_dis", SKIP_DIS); std::string config_file; pn.getParam("config_file", config_file); cv::FileStorage fsSettings(config_file, cv::FileStorage::READ); if (!fsSettings.isOpened()) { std::cerr << "ERROR: Wrong path to settings" << std::endl; } double camera_visual_size = fsSettings["visualize_camera_size"]; cameraposevisual->setScale(camera_visual_size); cameraposevisual->setLineWidth(camera_visual_size / 10.0); LOOP_CLOSURE = fsSettings["loop_closure"]; std::string IMAGE_TOPIC; int LOAD_PREVIOUS_POSE_GRAPH; // prepare for loop closure (load vocabulary, set topic, etc) if (LOOP_CLOSURE) { ROW = fsSettings["image_height"]; COL = fsSettings["image_width"]; std::string pkg_path = ros::package::getPath("pose_graph"); string vocabulary_file = pkg_path + "/../support_files/brief_k10L6.bin"; cout << "vocabulary_file" << vocabulary_file << endl; posegraph.loadVocabulary(vocabulary_file); BRIEF_PATTERN_FILE = pkg_path + "/../support_files/brief_pattern.yml"; cout << "BRIEF_PATTERN_FILE" << BRIEF_PATTERN_FILE << endl; m_camera = camodocal::CameraFactory::instance()->generateCameraFromYamlFile( config_file.c_str()); fsSettings["image_topic"] >> IMAGE_TOPIC; fsSettings["pose_graph_save_path"] >> POSE_GRAPH_SAVE_PATH; fsSettings["output_path"] >> VINS_RESULT_PATH; fsSettings["save_image"] >> DEBUG_IMAGE; cv::Mat cv_qid, cv_tid; fsSettings["extrinsicRotation"] >> cv_qid; fsSettings["extrinsicTranslation"] >> cv_tid; cv::cv2eigen(cv_qid, qi_d); cv::cv2eigen(cv_tid, ti_d); int USE_IMU = fsSettings["imu"]; posegraph.setIMUFlag(USE_IMU); VISUALIZE_IMU_FORWARD = fsSettings["visualize_imu_forward"]; LOAD_PREVIOUS_POSE_GRAPH = fsSettings["load_previous_pose_graph"]; FAST_RELOCALIZATION = fsSettings["fast_relocalization"]; VINS_RESULT_PATH = VINS_RESULT_PATH + "/vins_result_loop.csv"; std::ofstream fout(VINS_RESULT_PATH, std::ios::out); fout.close(); fsSettings.release(); // not used if (LOAD_PREVIOUS_POSE_GRAPH) { printf("load pose graph\n"); m_process.lock(); posegraph.loadPoseGraph(); m_process.unlock(); printf("load pose graph finish\n"); load_flag = true; } else { printf("no previous pose graph\n"); load_flag = true; } } fsSettings.release(); // publish camera pose by imu propagate and odometry (Ps and Rs of curr // frame) not important sub_imu_forward = nh.subscribe("/vins_estimator/imu_propagate", 100, &PoseGraphNodelet::imu_forward_callback, this); // odometry_buf sub_vio = nh.subscribe("/vins_estimator/odometry", 100, &PoseGraphNodelet::vio_callback, this); sub_image = nh.subscribe(IMAGE_TOPIC, 100, &PoseGraphNodelet::image_callback, this); // get keyframe_pose(Ps and Rs), store in pose_buf (marginalization_flag == // 0) sub_pose = nh.subscribe("/vins_estimator/keyframe_pose", 100, &PoseGraphNodelet::pose_callback, this); // get extrinsic (ric qic odometry.pose.pose.position and // odometry.pose.pose.orientation) update tic and qic real-time sub_extrinsic = nh.subscribe("/vins_estimator/extrinsic", 100, &PoseGraphNodelet::extrinsic_callback, this); // get keyframe_point(pointclude), store in point_buf (marginalization_flag // == 0) sub_point = nh.subscribe("/vins_estimator/keyframe_point", 100, &PoseGraphNodelet::point_callback, this); // do relocalization here. // pose_graph publish match_points to vins_estimator, estimator then publish // relo_relative_pose sub_relo_relative_pose = nh.subscribe("/vins_estimator/relo_relative_pose", 100, &PoseGraphNodelet::relo_relative_pose_callback, this); pub_match_img = nh.advertise("match_image", 100); pub_camera_pose_visual = nh.advertise( "/pose_graph/camera_pose_visual", 100); pub_key_odometrys = nh.advertise( "/pose_graph/key_odometrys", 100); // not used pub_vio_path = nh.advertise("/pose_graph/no_loop_path", 100); pub_match_points = nh.advertise("/pose_graph/match_points", 100); // main thread measurement_process = std::thread(&PoseGraphNodelet::process, this); // not used keyboard_command_process = std::thread(&PoseGraphNodelet::command, this); } ros::Subscriber sub_image, sub_imu_forward, sub_vio; ros::Subscriber sub_pose, sub_extrinsic, sub_point, sub_relo_relative_pose; std::thread measurement_process, keyboard_command_process; queue image_buf; queue point_buf; queue pose_buf; queue odometry_buf; std::mutex m_buf; std::mutex m_process; int frame_index; int sequence; PoseGraph posegraph; int skip_first_cnt; int SKIP_CNT{}; int skip_cnt; bool load_flag; bool start_flag; double SKIP_DIS; int VISUALIZE_IMU_FORWARD{}; int LOOP_CLOSURE{}; ros::Publisher pub_camera_pose_visual; ros::Publisher pub_key_odometrys; ros::Publisher pub_vio_path; nav_msgs::Path no_loop_path; CameraPoseVisualization *cameraposevisual; Eigen::Vector3d last_t; //(-100, -100, -100); double last_image_time; // not used in my case, just ignore sequence 1-5 void new_sequence() { printf("new sequence\n"); sequence++; printf("sequence cnt %d \n", sequence); if (sequence > 5) { ROS_WARN("only support 5 sequences since it's boring to copy code for " "more sequences."); ROS_BREAK(); } posegraph.posegraph_visualization->reset(); posegraph.publish(); m_buf.lock(); while (!image_buf.empty()) image_buf.pop(); while (!point_buf.empty()) point_buf.pop(); while (!pose_buf.empty()) pose_buf.pop(); while (!odometry_buf.empty()) odometry_buf.pop(); m_buf.unlock(); } void image_callback(const sensor_msgs::ImageConstPtr &image_msg) { // ROS_WARN("image_callback!"); if (!LOOP_CLOSURE) return; m_buf.lock(); image_buf.push(image_msg); m_buf.unlock(); // printf(" image time %f \n", image_msg->header.stamp.toSec()); // detect unstable camera stream if (last_image_time == -1) last_image_time = image_msg->header.stamp.toSec(); else if (image_msg->header.stamp.toSec() - last_image_time > 1.0 || image_msg->header.stamp.toSec() < last_image_time) { ROS_WARN("image discontinue! detect a new sequence!"); new_sequence(); } last_image_time = image_msg->header.stamp.toSec(); } void point_callback(const sensor_msgs::PointCloudConstPtr &point_msg) { // ROS_INFO("point_callback!"); if (!LOOP_CLOSURE) return; m_buf.lock(); point_buf.push(point_msg); m_buf.unlock(); /* for (unsigned int i = 0; i < point_msg->points.size(); i++) { printf("%d, 3D point: %f, %f, %f 2D point %f, %f \n",i , point_msg->points[i].x, point_msg->points[i].y, point_msg->points[i].z, point_msg->channels[i].values[0], point_msg->channels[i].values[1]); } */ } void pose_callback(const nav_msgs::Odometry::ConstPtr &pose_msg) { // ROS_INFO("pose_callback!"); if (!LOOP_CLOSURE) return; m_buf.lock(); pose_buf.push(pose_msg); m_buf.unlock(); /* printf("pose t: %f, %f, %f q: %f, %f, %f %f \n", pose_msg->pose.pose.position.x, pose_msg->pose.pose.position.y, pose_msg->pose.pose.position.z, pose_msg->pose.pose.orientation.w, pose_msg->pose.pose.orientation.x, pose_msg->pose.pose.orientation.y, pose_msg->pose.pose.orientation.z); */ } // not used void imu_forward_callback(const nav_msgs::Odometry::ConstPtr &forward_msg) { if (VISUALIZE_IMU_FORWARD) { Vector3d vio_t(forward_msg->pose.pose.position.x, forward_msg->pose.pose.position.y, forward_msg->pose.pose.position.z); Quaterniond vio_q; vio_q.w() = forward_msg->pose.pose.orientation.w; vio_q.x() = forward_msg->pose.pose.orientation.x; vio_q.y() = forward_msg->pose.pose.orientation.y; vio_q.z() = forward_msg->pose.pose.orientation.z; vio_t = posegraph.w_r_vio * vio_t + posegraph.w_t_vio; vio_q = posegraph.w_r_vio * vio_q; vio_t = posegraph.r_drift * vio_t + posegraph.t_drift; vio_q = posegraph.r_drift * vio_q; Vector3d vio_t_cam; Quaterniond vio_q_cam; vio_t_cam = vio_t + vio_q * tic; vio_q_cam = vio_q * qic; cameraposevisual->reset(); cameraposevisual->add_pose(vio_t_cam, vio_q_cam); cameraposevisual->publish_by(pub_camera_pose_visual, forward_msg->header); } } void relo_relative_pose_callback(const nav_msgs::Odometry::ConstPtr &pose_msg) { Vector3d relative_t = Vector3d(pose_msg->pose.pose.position.x, pose_msg->pose.pose.position.y, pose_msg->pose.pose.position.z); Quaterniond relative_q; relative_q.w() = pose_msg->pose.pose.orientation.w; relative_q.x() = pose_msg->pose.pose.orientation.x; relative_q.y() = pose_msg->pose.pose.orientation.y; relative_q.z() = pose_msg->pose.pose.orientation.z; double relative_yaw = pose_msg->twist.twist.linear.x; int index = pose_msg->twist.twist.linear.y; // printf("receive index %d \n", index ); Eigen::Matrix loop_info; loop_info << relative_t.x(), relative_t.y(), relative_t.z(), relative_q.w(), relative_q.x(), relative_q.y(), relative_q.z(), relative_yaw; posegraph.updateKeyFrameLoop(index, loop_info); } void vio_callback(const nav_msgs::Odometry::ConstPtr &pose_msg) { // ROS_INFO("vio_callback!"); Vector3d vio_t(pose_msg->pose.pose.position.x, pose_msg->pose.pose.position.y, pose_msg->pose.pose.position.z); Quaterniond vio_q; vio_q.w() = pose_msg->pose.pose.orientation.w; vio_q.x() = pose_msg->pose.pose.orientation.x; vio_q.y() = pose_msg->pose.pose.orientation.y; vio_q.z() = pose_msg->pose.pose.orientation.z; vio_t = posegraph.w_r_vio * vio_t + posegraph.w_t_vio; vio_q = posegraph.w_r_vio * vio_q; vio_t = posegraph.r_drift * vio_t + posegraph.t_drift; vio_q = posegraph.r_drift * vio_q; Vector3d vio_t_cam; Quaterniond vio_q_cam; vio_t_cam = vio_t + vio_q * tic; vio_q_cam = vio_q * qic; if (!VISUALIZE_IMU_FORWARD) { cameraposevisual->reset(); cameraposevisual->add_pose(vio_t_cam, vio_q_cam); cameraposevisual->publish_by(pub_camera_pose_visual, pose_msg->header); } odometry_buf.push(vio_t_cam); if (odometry_buf.size() > 10) { odometry_buf.pop(); } visualization_msgs::Marker key_odometrys; key_odometrys.header = pose_msg->header; key_odometrys.header.frame_id = "map"; key_odometrys.ns = "key_odometrys"; key_odometrys.type = visualization_msgs::Marker::SPHERE_LIST; key_odometrys.action = visualization_msgs::Marker::ADD; key_odometrys.pose.orientation.w = 1.0; key_odometrys.lifetime = ros::Duration(); // static int key_odometrys_id = 0; key_odometrys.id = 0; // key_odometrys_id++; key_odometrys.scale.x = 0.1; key_odometrys.scale.y = 0.1; key_odometrys.scale.z = 0.1; key_odometrys.color.r = 1.0; key_odometrys.color.a = 1.0; for (unsigned int i = 0; i < odometry_buf.size(); i++) { geometry_msgs::Point pose_marker; Vector3d vio_t; vio_t = odometry_buf.front(); odometry_buf.pop(); pose_marker.x = vio_t.x(); pose_marker.y = vio_t.y(); pose_marker.z = vio_t.z(); key_odometrys.points.push_back(pose_marker); odometry_buf.push(vio_t); } pub_key_odometrys.publish(key_odometrys); // not used if (!LOOP_CLOSURE) { geometry_msgs::PoseStamped pose_stamped; pose_stamped.header = pose_msg->header; pose_stamped.header.frame_id = "map"; pose_stamped.pose.position.x = vio_t.x(); pose_stamped.pose.position.y = vio_t.y(); pose_stamped.pose.position.z = vio_t.z(); no_loop_path.header = pose_msg->header; no_loop_path.header.frame_id = "map"; no_loop_path.poses.push_back(pose_stamped); pub_vio_path.publish(no_loop_path); } } void extrinsic_callback(const nav_msgs::Odometry::ConstPtr &pose_msg) { m_process.lock(); tic = Vector3d(pose_msg->pose.pose.position.x, pose_msg->pose.pose.position.y, pose_msg->pose.pose.position.z); qic = Quaterniond(pose_msg->pose.pose.orientation.w, pose_msg->pose.pose.orientation.x, pose_msg->pose.pose.orientation.y, pose_msg->pose.pose.orientation.z) .toRotationMatrix(); m_process.unlock(); } void process() { if (!LOOP_CLOSURE) return; while (true) { sensor_msgs::ImageConstPtr image_msg = NULL; sensor_msgs::PointCloudConstPtr point_msg = NULL; nav_msgs::Odometry::ConstPtr pose_msg = NULL; // find out the messages with same time stamp m_buf.lock(); // get image_msg, pose_msg and point_msg if (!image_buf.empty() && !point_buf.empty() && !pose_buf.empty()) { if (image_buf.front()->header.stamp.toSec() > pose_buf.front()->header.stamp.toSec()) { pose_buf.pop(); printf("throw pose at beginning\n"); } else if (image_buf.front()->header.stamp.toSec() > point_buf.front()->header.stamp.toSec()) { point_buf.pop(); printf("throw point at beginning\n"); } else if (image_buf.back()->header.stamp.toSec() >= pose_buf.front()->header.stamp.toSec() && point_buf.back()->header.stamp.toSec() >= pose_buf.front()->header.stamp.toSec()) { pose_msg = pose_buf.front(); pose_buf.pop(); while (!pose_buf.empty()) pose_buf.pop(); while (image_buf.front()->header.stamp.toSec() < pose_msg->header.stamp.toSec()) { image_buf.pop(); } image_msg = image_buf.front(); image_buf.pop(); while (point_buf.front()->header.stamp.toSec() < pose_msg->header.stamp.toSec()) point_buf.pop(); point_msg = point_buf.front(); point_buf.pop(); } } m_buf.unlock(); if (pose_msg != NULL) { // printf(" pose time %f \n", pose_msg->header.stamp.toSec()); // printf(" point time %f \n", point_msg->header.stamp.toSec()); // printf(" image time %f \n", image_msg->header.stamp.toSec()); // skip fisrt few if (skip_first_cnt < SKIP_FIRST_CNT) { skip_first_cnt++; continue; } if (skip_cnt < SKIP_CNT) { skip_cnt++; continue; } else { skip_cnt = 0; } cv::Mat image = cv_bridge::toCvCopy(image_msg, sensor_msgs::image_encodings::MONO8) ->image; // build keyframe Vector3d T = Vector3d(pose_msg->pose.pose.position.x, pose_msg->pose.pose.position.y, pose_msg->pose.pose.position.z); Matrix3d R = Quaterniond(pose_msg->pose.pose.orientation.w, pose_msg->pose.pose.orientation.x, pose_msg->pose.pose.orientation.y, pose_msg->pose.pose.orientation.z) .toRotationMatrix(); //将距上一关键帧距离（平移向量的模）超过SKIP_DIS的图像创建为关键帧 if ((T - last_t).norm() > SKIP_DIS) { vector point_3d; vector point_2d_uv; vector point_2d_normal; vector point_id; for (unsigned int i = 0; i < point_msg->points.size(); i++) { cv::Point3f p_3d; p_3d.x = point_msg->points[i].x; p_3d.y = point_msg->points[i].y; p_3d.z = point_msg->points[i].z; point_3d.push_back(p_3d); cv::Point2f p_2d_uv, p_2d_normal; double p_id; p_2d_normal.x = point_msg->channels[i].values[0]; p_2d_normal.y = point_msg->channels[i].values[1]; p_2d_uv.x = point_msg->channels[i].values[2]; p_2d_uv.y = point_msg->channels[i].values[3]; p_id = point_msg->channels[i].values[4]; point_2d_normal.push_back(p_2d_normal); point_2d_uv.push_back(p_2d_uv); point_id.push_back(p_id); // printf("u %f, v %f \n", p_2d_uv.x, p_2d_uv.y); } // 通过frame_index标记对应帧 KeyFrame *keyframe = new KeyFrame( pose_msg->header.stamp.toSec(), frame_index, T, R, image, point_3d, point_2d_uv, point_2d_normal, point_id, sequence); m_process.lock(); start_flag = 1; //在posegraph中添加关键帧，flag_detect_loop=1回环检测 posegraph.addKeyFrame(keyframe, 1); m_process.unlock(); frame_index++; last_t = T; } } std::chrono::milliseconds dura(5); std::this_thread::sleep_for(dura); } } void command() { if (!LOOP_CLOSURE) return; while (1) { char c = getchar(); if (c == 's') { m_process.lock(); posegraph.savePoseGraph(); m_process.unlock(); printf("save pose graph finish\nyou can set 'load_previous_pose_graph' " "to 1 in the config file to reuse it next time\n"); printf("program shutting down...\n"); ros::shutdown(); } if (c == 'n') new_sequence(); std::chrono::milliseconds dura(5); std::this_thread::sleep_for(dura); } } }; PLUGINLIB_EXPORT_CLASS(pose_graph_nodelet_ns::PoseGraphNodelet, nodelet::Nodelet) } // namespace pose_graph_nodelet_ns ================================================ FILE: pose_graph/src/utility/CameraPoseVisualization.cpp ================================================ #include "CameraPoseVisualization.h" const Eigen::Vector3d CameraPoseVisualization::imlt = Eigen::Vector3d(-1.0, -0.5, 1.0); const Eigen::Vector3d CameraPoseVisualization::imrt = Eigen::Vector3d( 1.0, -0.5, 1.0); const Eigen::Vector3d CameraPoseVisualization::imlb = Eigen::Vector3d(-1.0, 0.5, 1.0); const Eigen::Vector3d CameraPoseVisualization::imrb = Eigen::Vector3d( 1.0, 0.5, 1.0); const Eigen::Vector3d CameraPoseVisualization::lt0 = Eigen::Vector3d(-0.7, -0.5, 1.0); const Eigen::Vector3d CameraPoseVisualization::lt1 = Eigen::Vector3d(-0.7, -0.2, 1.0); const Eigen::Vector3d CameraPoseVisualization::lt2 = Eigen::Vector3d(-1.0, -0.2, 1.0); const Eigen::Vector3d CameraPoseVisualization::oc = Eigen::Vector3d(0.0, 0.0, 0.0); void Eigen2Point(const Eigen::Vector3d& v, geometry_msgs::Point& p) { p.x = v.x(); p.y = v.y(); p.z = v.z(); } CameraPoseVisualization::CameraPoseVisualization(float r, float g, float b, float a) : m_marker_ns("CameraPoseVisualization"), m_scale(0.2), m_line_width(0.01) { m_image_boundary_color.r = r; m_image_boundary_color.g = g; m_image_boundary_color.b = b; m_image_boundary_color.a = a; m_optical_center_connector_color.r = r; m_optical_center_connector_color.g = g; m_optical_center_connector_color.b = b; m_optical_center_connector_color.a = a; } void CameraPoseVisualization::setImageBoundaryColor(float r, float g, float b, float a) { m_image_boundary_color.r = r; m_image_boundary_color.g = g; m_image_boundary_color.b = b; m_image_boundary_color.a = a; } void CameraPoseVisualization::setOpticalCenterConnectorColor(float r, float g, float b, float a) { m_optical_center_connector_color.r = r; m_optical_center_connector_color.g = g; m_optical_center_connector_color.b = b; m_optical_center_connector_color.a = a; } void CameraPoseVisualization::setScale(double s) { m_scale = s; } void CameraPoseVisualization::setLineWidth(double width) { m_line_width = width; } void CameraPoseVisualization::add_edge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1){ visualization_msgs::Marker marker; marker.ns = m_marker_ns; marker.id = m_markers.size() + 1; marker.type = visualization_msgs::Marker::LINE_LIST; marker.action = visualization_msgs::Marker::ADD; marker.scale.x = 0.01; marker.color.b = 1.0f; marker.color.a = 1.0; geometry_msgs::Point point0, point1; Eigen2Point(p0, point0); Eigen2Point(p1, point1); marker.points.push_back(point0); marker.points.push_back(point1); m_markers.push_back(marker); } void CameraPoseVisualization::add_loopedge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1){ //m_markers.clear(); visualization_msgs::Marker marker; marker.ns = m_marker_ns; marker.id = m_markers.size() + 1; marker.type = visualization_msgs::Marker::LINE_STRIP; marker.action = visualization_msgs::Marker::ADD; marker.lifetime = ros::Duration(); //marker.scale.x = 0.4; marker.scale.x = 0.02; marker.color.r = 1.0f; //marker.color.g = 1.0f; //marker.color.b = 1.0f; marker.color.a = 1.0; geometry_msgs::Point point0, point1; Eigen2Point(p0, point0); Eigen2Point(p1, point1); marker.points.push_back(point0); marker.points.push_back(point1); m_markers.push_back(marker); } void CameraPoseVisualization::add_pose(const Eigen::Vector3d& p, const Eigen::Quaterniond& q) { visualization_msgs::Marker marker; marker.ns = m_marker_ns; marker.id = 0; marker.type = visualization_msgs::Marker::LINE_STRIP; marker.action = visualization_msgs::Marker::ADD; marker.scale.x = m_line_width; marker.pose.position.x = 0.0; marker.pose.position.y = 0.0; marker.pose.position.z = 0.0; marker.pose.orientation.w = 1.0; marker.pose.orientation.x = 0.0; marker.pose.orientation.y = 0.0; marker.pose.orientation.z = 0.0; geometry_msgs::Point pt_lt, pt_lb, pt_rt, pt_rb, pt_oc, pt_lt0, pt_lt1, pt_lt2; Eigen2Point(q * (m_scale *imlt) + p, pt_lt); Eigen2Point(q * (m_scale *imlb) + p, pt_lb); Eigen2Point(q * (m_scale *imrt) + p, pt_rt); Eigen2Point(q * (m_scale *imrb) + p, pt_rb); Eigen2Point(q * (m_scale *lt0 ) + p, pt_lt0); Eigen2Point(q * (m_scale *lt1 ) + p, pt_lt1); Eigen2Point(q * (m_scale *lt2 ) + p, pt_lt2); Eigen2Point(q * (m_scale *oc ) + p, pt_oc); // image boundaries marker.points.push_back(pt_lt); marker.points.push_back(pt_lb); marker.colors.push_back(m_image_boundary_color); marker.colors.push_back(m_image_boundary_color); marker.points.push_back(pt_lb); marker.points.push_back(pt_rb); marker.colors.push_back(m_image_boundary_color); marker.colors.push_back(m_image_boundary_color); marker.points.push_back(pt_rb); marker.points.push_back(pt_rt); marker.colors.push_back(m_image_boundary_color); marker.colors.push_back(m_image_boundary_color); marker.points.push_back(pt_rt); marker.points.push_back(pt_lt); marker.colors.push_back(m_image_boundary_color); marker.colors.push_back(m_image_boundary_color); // top-left indicator marker.points.push_back(pt_lt0); marker.points.push_back(pt_lt1); marker.colors.push_back(m_image_boundary_color); marker.colors.push_back(m_image_boundary_color); marker.points.push_back(pt_lt1); marker.points.push_back(pt_lt2); marker.colors.push_back(m_image_boundary_color); marker.colors.push_back(m_image_boundary_color); // optical center connector marker.points.push_back(pt_lt); marker.points.push_back(pt_oc); marker.colors.push_back(m_optical_center_connector_color); marker.colors.push_back(m_optical_center_connector_color); marker.points.push_back(pt_lb); marker.points.push_back(pt_oc); marker.colors.push_back(m_optical_center_connector_color); marker.colors.push_back(m_optical_center_connector_color); marker.points.push_back(pt_rt); marker.points.push_back(pt_oc); marker.colors.push_back(m_optical_center_connector_color); marker.colors.push_back(m_optical_center_connector_color); marker.points.push_back(pt_rb); marker.points.push_back(pt_oc); marker.colors.push_back(m_optical_center_connector_color); marker.colors.push_back(m_optical_center_connector_color); m_markers.push_back(marker); } void CameraPoseVisualization::reset() { m_markers.clear(); //image.points.clear(); //image.colors.clear(); } void CameraPoseVisualization::publish_by( ros::Publisher &pub, const std_msgs::Header &header ) { visualization_msgs::MarkerArray markerArray_msg; //int k = (int)m_markers.size(); /* for (int i = 0; i < 5 && k > 0; i++) { k--; m_markers[k].header = header; markerArray_msg.markers.push_back(m_markers[k]); } */ for(auto& marker : m_markers) { marker.header = header; markerArray_msg.markers.push_back(marker); } pub.publish(markerArray_msg); } ================================================ FILE: pose_graph/src/utility/CameraPoseVisualization.h ================================================ #pragma once #include #include #include #include #include #include #include #include "parameters.h" class CameraPoseVisualization { public: std::string m_marker_ns; CameraPoseVisualization(float r, float g, float b, float a); void setImageBoundaryColor(float r, float g, float b, float a=1.0); void setOpticalCenterConnectorColor(float r, float g, float b, float a=1.0); void setScale(double s); void setLineWidth(double width); void add_pose(const Eigen::Vector3d& p, const Eigen::Quaterniond& q); void reset(); void publish_by(ros::Publisher& pub, const std_msgs::Header& header); void add_edge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1); void add_loopedge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1); //void add_image(const Eigen::Vector3d& T, const Eigen::Matrix3d& R, const cv::Mat &src); void publish_image_by( ros::Publisher &pub, const std_msgs::Header &header); private: std::vector m_markers; std_msgs::ColorRGBA m_image_boundary_color; std_msgs::ColorRGBA m_optical_center_connector_color; double m_scale; double m_line_width; static const Eigen::Vector3d imlt; static const Eigen::Vector3d imlb; static const Eigen::Vector3d imrt; static const Eigen::Vector3d imrb; static const Eigen::Vector3d oc ; static const Eigen::Vector3d lt0 ; static const Eigen::Vector3d lt1 ; static const Eigen::Vector3d lt2 ; }; ================================================ FILE: pose_graph/src/utility/parameters.h ================================================ #pragma once #include "camodocal/camera_models/CameraFactory.h" #include "camodocal/camera_models/CataCamera.h" #include "camodocal/camera_models/PinholeCamera.h" #include #include #include #include #include #include extern camodocal::CameraPtr m_camera; extern Eigen::Vector3d tic; extern Eigen::Matrix3d qic; extern Eigen::Matrix ti_d; extern Eigen::Matrix qi_d; extern ros::Publisher pub_match_img; extern ros::Publisher pub_match_points; extern int VISUALIZATION_SHIFT_X; extern int VISUALIZATION_SHIFT_Y; extern std::string BRIEF_PATTERN_FILE; extern std::string POSE_GRAPH_SAVE_PATH; extern int ROW; extern int COL; extern std::string VINS_RESULT_PATH; extern int DEBUG_IMAGE; extern int FAST_RELOCALIZATION; ================================================ FILE: pose_graph/src/utility/tic_toc.h ================================================ #pragma once #include #include #include class TicToc { public: TicToc() { tic(); } void tic() { start = std::chrono::system_clock::now(); } double toc() { end = std::chrono::system_clock::now(); std::chrono::duration elapsed_seconds = end - start; return elapsed_seconds.count() * 1000; } private: std::chrono::time_point start, end; }; ================================================ FILE: pose_graph/src/utility/utility.cpp ================================================ #include "utility.h" Eigen::Matrix3d Utility::g2R(const Eigen::Vector3d &g) { Eigen::Matrix3d R0; Eigen::Vector3d ng1 = g.normalized(); Eigen::Vector3d ng2{0, 0, 1.0}; R0 = Eigen::Quaterniond::FromTwoVectors(ng1, ng2).toRotationMatrix(); double yaw = Utility::R2ypr(R0).x(); R0 = Utility::ypr2R(Eigen::Vector3d{-yaw, 0, 0}) * R0; // R0 = Utility::ypr2R(Eigen::Vector3d{-90, 0, 0}) * R0; return R0; } ================================================ FILE: pose_graph/src/utility/utility.h ================================================ #pragma once #include #include #include #include class Utility { public: template static Eigen::Quaternion deltaQ(const Eigen::MatrixBase &theta) { typedef typename Derived::Scalar Scalar_t; Eigen::Quaternion dq; Eigen::Matrix half_theta = theta; half_theta /= static_cast(2.0); dq.w() = static_cast(1.0); dq.x() = half_theta.x(); dq.y() = half_theta.y(); dq.z() = half_theta.z(); return dq; } template static Eigen::Matrix skewSymmetric(const Eigen::MatrixBase &q) { Eigen::Matrix ans; ans << typename Derived::Scalar(0), -q(2), q(1), q(2), typename Derived::Scalar(0), -q(0), -q(1), q(0), typename Derived::Scalar(0); return ans; } template static Eigen::Quaternion positify(const Eigen::QuaternionBase &q) { //printf("a: %f %f %f %f", q.w(), q.x(), q.y(), q.z()); //Eigen::Quaternion p(-q.w(), -q.x(), -q.y(), -q.z()); //printf("b: %f %f %f %f", p.w(), p.x(), p.y(), p.z()); //return q.template w() >= (typename Derived::Scalar)(0.0) ? q : Eigen::Quaternion(-q.w(), -q.x(), -q.y(), -q.z()); return q; } template static Eigen::Matrix Qleft(const Eigen::QuaternionBase &q) { Eigen::Quaternion qq = positify(q); Eigen::Matrix ans; ans(0, 0) = qq.w(), ans.template block<1, 3>(0, 1) = -qq.vec().transpose(); ans.template block<3, 1>(1, 0) = qq.vec(), ans.template block<3, 3>(1, 1) = qq.w() * Eigen::Matrix::Identity() + skewSymmetric(qq.vec()); return ans; } template static Eigen::Matrix Qright(const Eigen::QuaternionBase &p) { Eigen::Quaternion pp = positify(p); Eigen::Matrix ans; ans(0, 0) = pp.w(), ans.template block<1, 3>(0, 1) = -pp.vec().transpose(); ans.template block<3, 1>(1, 0) = pp.vec(), ans.template block<3, 3>(1, 1) = pp.w() * Eigen::Matrix::Identity() - skewSymmetric(pp.vec()); return ans; } static Eigen::Vector3d R2ypr(const Eigen::Matrix3d &R) { Eigen::Vector3d n = R.col(0); Eigen::Vector3d o = R.col(1); Eigen::Vector3d a = R.col(2); Eigen::Vector3d ypr(3); double y = atan2(n(1), n(0)); double p = atan2(-n(2), n(0) * cos(y) + n(1) * sin(y)); double r = atan2(a(0) * sin(y) - a(1) * cos(y), -o(0) * sin(y) + o(1) * cos(y)); ypr(0) = y; ypr(1) = p; ypr(2) = r; return ypr / M_PI * 180.0; } template static Eigen::Matrix ypr2R(const Eigen::MatrixBase &ypr) { typedef typename Derived::Scalar Scalar_t; Scalar_t y = ypr(0) / 180.0 * M_PI; Scalar_t p = ypr(1) / 180.0 * M_PI; Scalar_t r = ypr(2) / 180.0 * M_PI; Eigen::Matrix Rz; Rz << cos(y), -sin(y), 0, sin(y), cos(y), 0, 0, 0, 1; Eigen::Matrix Ry; Ry << cos(p), 0., sin(p), 0., 1., 0., -sin(p), 0., cos(p); Eigen::Matrix Rx; Rx << 1., 0., 0., 0., cos(r), -sin(r), 0., sin(r), cos(r); return Rz * Ry * Rx; } static Eigen::Matrix3d g2R(const Eigen::Vector3d &g); template struct uint_ { }; template void unroller(const Lambda &f, const IterT &iter, uint_) { unroller(f, iter, uint_()); f(iter + N); } template void unroller(const Lambda &f, const IterT &iter, uint_<0>) { f(iter); } template static T normalizeAngle(const T& angle_degrees) { T two_pi(2.0 * 180); if (angle_degrees > 0) return angle_degrees - two_pi * std::floor((angle_degrees + T(180)) / two_pi); else return angle_degrees + two_pi * std::floor((-angle_degrees + T(180)) / two_pi); }; }; ================================================ FILE: support_files/brief_pattern.yml ================================================ %YAML:1.0 x1: - 0 - 4 - 11 - -4 - 24 - -3 - -4 - -7 - -5 - -2 - 8 - 1 - -2 - -5 - -5 - -8 - 2 - -8 - 6 - 4 - -1 - -14 - 12 - -12 - -20 - -11 - 7 - -11 - 18 - -10 - 10 - -20 - 7 - -2 - 3 - 7 - -20 - -3 - -4 - -6 - -4 - 14 - 1 - 2 - -9 - 15 - 13 - 14 - -21 - -6 - 3 - -7 - 23 - -22 - -12 - 0 - -22 - 1 - -11 - -11 - -8 - -7 - -16 - -6 - 4 - -16 - 4 - 7 - 0 - 0 - -7 - -4 - 16 - -3 - 6 - 5 - 0 - 8 - -4 - -12 - 0 - -11 - 11 - 3 - -13 - 9 - 11 - -23 - 0 - 6 - 6 - 3 - -7 - 6 - 2 - -4 - 24 - -3 - 5 - -5 - 0 - 0 - -4 - -10 - -1 - 8 - 6 - 3 - -7 - 7 - -4 - 5 - 8 - 10 - 8 - -10 - 2 - 6 - 0 - 10 - -2 - 5 - -2 - -1 - -12 - -10 - -15 - 12 - 1 - -6 - 12 - -7 - -7 - -8 - 7 - -11 - 2 - -22 - 7 - 20 - -8 - 4 - 3 - 11 - -9 - 4 - -4 - -12 - 12 - -22 - -6 - 7 - 5 - -1 - 9 - 10 - -15 - -2 - 1 - 14 - 21 - 3 - 6 - -13 - 5 - -19 - 10 - 6 - 2 - -14 - -16 - -15 - 6 - -8 - 18 - 7 - 16 - -4 - -2 - -4 - 0 - -4 - -14 - 0 - -4 - -13 - -1 - -14 - 11 - -4 - -15 - -8 - -6 - 3 - -1 - -1 - 4 - 8 - 3 - 0 - 24 - -3 - -11 - -10 - -2 - 2 - -1 - 4 - 2 - -1 - 11 - 0 - 10 - -1 - 6 - -4 - -19 - 1 - 9 - -6 - -1 - 12 - 0 - -9 - -1 - 5 - 0 - -6 - -5 - 0 - 7 - -19 - 11 - -9 - 11 - 0 - 8 - -1 - 5 - 10 - -2 - -6 - -6 - 0 - -3 - -7 - 4 - -4 - 2 - -10 - -7 - 18 - 0 - 12 - -2 - 0 y1: - -10 - 12 - 13 - 8 - -2 - 2 - 4 - 4 - -11 - -4 - 7 - 15 - 17 - 0 - 10 - -2 - -7 - 10 - 11 - -3 - -4 - 11 - -5 - -15 - 0 - 3 - -3 - -11 - 3 - -4 - 4 - 22 - 16 - 3 - -2 - -3 - 0 - 5 - 15 - -8 - 2 - 2 - -12 - -10 - 5 - 4 - 10 - -4 - 20 - -7 - 5 - 0 - -18 - 2 - -2 - -23 - 15 - 4 - -1 - -2 - 3 - 0 - -16 - 5 - 0 - -16 - -2 - 5 - 6 - 4 - 13 - -1 - -16 - -13 - -7 - -3 - 4 - 0 - -10 - -10 - -9 - 4 - -8 - 0 - -15 - -2 - -10 - 16 - 1 - 0 - 6 - -11 - -3 - -7 - -2 - 10 - -3 - -2 - 0 - 10 - -7 - 0 - 4 - 22 - -5 - 0 - -1 - -8 - -1 - -4 - 12 - -2 - 18 - -2 - -6 - -4 - -2 - -16 - -8 - 1 - -5 - 2 - 3 - 2 - 7 - -8 - 3 - 8 - 0 - -10 - -4 - -18 - 9 - 12 - 3 - 17 - -8 - -13 - 5 - -1 - 8 - -10 - -3 - -9 - -2 - 7 - 5 - -5 - -8 - 15 - -12 - -5 - 0 - 2 - 15 - -5 - -4 - -1 - -9 - 0 - -5 - -11 - -5 - -5 - -10 - 2 - 16 - 6 - 17 - 0 - -7 - 15 - 5 - -14 - -2 - 6 - 5 - 2 - 12 - -3 - 2 - 4 - 13 - 1 - -1 - -3 - -1 - -11 - 8 - -3 - 19 - 8 - 7 - -3 - 5 - 19 - 4 - -6 - 4 - -14 - 3 - 10 - -15 - -4 - 14 - 1 - 14 - -1 - -9 - -5 - -16 - 10 - 6 - 3 - -13 - 19 - -7 - -7 - -12 - -7 - 1 - 7 - -7 - 15 - -2 - -15 - 6 - 6 - 2 - -6 - -16 - 0 - -5 - -9 - 11 - 21 - 3 - 8 - 23 - -5 - -20 - -7 - 13 - -7 - -7 - 0 - 6 - 10 - -15 - 1 - 0 - 0 - 0 - -7 - 10 - -3 x2: - 0 - 0 - 13 - -6 - 22 - -7 - -3 - -12 - 1 - -4 - 10 - 4 - -6 - 0 - -8 - -5 - 7 - -2 - -1 - 8 - -2 - -14 - 14 - -10 - -14 - -4 - 6 - -14 - 15 - -5 - 9 - -18 - 4 - 0 - -4 - 3 - -19 - -10 - -8 - 0 - -4 - 10 - 1 - 5 - -12 - 19 - 2 - 11 - -18 - -5 - -3 - -5 - 21 - -17 - -14 - 1 - -17 - 5 - -8 - -8 - -8 - -1 - -16 - 1 - -2 - -12 - 0 - 3 - 7 - -1 - -10 - -2 - 14 - -5 - 0 - 4 - -2 - 15 - 2 - -9 - -9 - -11 - 9 - 2 - -15 - 8 - 9 - -24 - -5 - 10 - 7 - 0 - -7 - 6 - 2 - 0 - 24 - 0 - 2 - 0 - 2 - -1 - -5 - -10 - -4 - 19 - 2 - -6 - -8 - 13 - -1 - 5 - 3 - 13 - 8 - -10 - 0 - 1 - -5 - 11 - -6 - 8 - -7 - 1 - -14 - -6 - -10 - 11 - 2 - -5 - 12 - -12 - -3 - -5 - 7 - -5 - -3 - -16 - 9 - 20 - -9 - 8 - 3 - 15 - -12 - 4 - -6 - -15 - 10 - -17 - -13 - 6 - 4 - -3 - 9 - 7 - -18 - -10 - -3 - 10 - 23 - -9 - 10 - -15 - 7 - -14 - 13 - 10 - 0 - -20 - -14 - -15 - 3 - -7 - 21 - 7 - 17 - 1 - 0 - -2 - -8 - -11 - -17 - 2 - -6 - -18 - -5 - -10 - 5 - 2 - -13 - -6 - -4 - 0 - 3 - 2 - 10 - 10 - 4 - 0 - 21 - -13 - -16 - -17 - 0 - 3 - -1 - 6 - 5 - 8 - 14 - 2 - 10 - -6 - 1 - 0 - -23 - 0 - 6 - -4 - 3 - 9 - -5 - -6 - 1 - 12 - 2 - -6 - -4 - 7 - -1 - -17 - 9 - -12 - 13 - -1 - 11 - 5 - 7 - 10 - -2 - -3 - -8 - 1 - 0 - -6 - 5 - -6 - 5 - -12 - -5 - 21 - 2 - 17 - -7 - -1 y2: - -6 - 6 - 10 - 9 - 0 - 4 - 1 - 1 - -12 - 3 - 1 - 9 - 15 - 0 - 14 - 3 - -13 - 11 - 10 - -4 - -2 - 7 - -7 - -17 - -6 - 7 - -4 - -16 - -6 - -2 - 6 - 19 - 8 - 4 - -1 - -2 - 3 - 3 - 21 - -3 - 6 - 5 - -12 - -5 - 0 - 5 - 12 - -5 - 24 - -11 - 7 - 0 - -20 - 0 - -4 - -21 - 15 - 2 - -4 - 0 - -3 - 0 - -19 - 5 - 4 - -19 - -1 - 5 - 4 - 0 - 10 - 3 - -15 - -15 - -7 - -1 - 8 - -6 - -6 - -14 - -15 - 1 - -8 - -1 - -5 - 2 - -10 - 17 - 5 - 0 - 9 - -8 - -5 - -2 - -1 - 9 - 0 - -5 - -2 - 13 - -4 - -1 - 0 - 16 - -10 - -1 - 0 - -6 - 3 - -6 - 10 - 1 - 18 - -2 - -10 - -4 - -1 - -13 - -4 - -1 - -8 - 7 - 4 - -3 - 4 - -14 - -4 - 11 - -1 - -12 - -5 - -14 - 9 - 16 - 1 - 20 - -9 - -10 - 10 - 2 - 4 - -2 - -4 - -6 - -6 - 5 - 5 - -4 - 0 - 18 - -15 - -5 - 0 - 1 - 8 - -4 - -2 - 0 - -11 - 0 - -7 - -5 - 0 - 0 - -3 - 5 - 12 - 3 - 13 - 1 - -9 - 19 - 4 - -9 - 4 - 2 - 3 - -1 - 14 - 0 - 2 - 4 - 14 - 8 - 0 - -4 - 0 - -12 - 9 - 0 - 23 - 7 - 0 - -9 - 2 - 22 - 2 - -14 - 6 - -16 - 1 - 4 - -14 - -10 - 19 - 6 - 16 - -4 - -9 - 0 - -7 - 12 - 5 - 5 - -14 - 21 - -5 - -9 - -17 - -5 - 4 - 14 - -5 - 9 - -4 - -21 - 4 - 1 - 5 - 0 - -12 - 2 - 0 - -1 - 6 - 24 - 0 - 15 - 23 - -10 - -12 - -2 - 12 - -4 - -5 - 0 - 5 - 13 - -21 - 0 - -2 - 1 - 6 - -4 - 9 - -2 ================================================ FILE: vins_estimator/CMakeLists.txt ================================================ cmake_minimum_required(VERSION 2.8.3) project(vins_estimator) set(CMAKE_BUILD_TYPE "Release") # set(CMAKE_BUILD_TYPE "RelWithDebInfo") # set(CMAKE_BUILD_TYPE "Debug") set(CMAKE_CXX_FLAGS "-std=c++14") set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g") find_package(catkin REQUIRED COMPONENTS roscpp std_msgs geometry_msgs nav_msgs tf cv_bridge camera_model message_filters image_transport nodelet ) find_package(OpenCV REQUIRED) # message(WARNING "OpenCV_VERSION: ${OpenCV_VERSION}") find_package(Ceres REQUIRED) set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) find_package(Eigen3) include_directories( ${catkin_INCLUDE_DIRS} ${CERES_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR} ) catkin_package() add_library(vins_lib src/utility/parameters.cpp src/estimator/estimator.cpp src/feature_manager/feature_manager.cpp src/factor/pose_local_parameterization.cpp src/factor/projection_factor.cpp src/factor/projection_td_factor.cpp src/factor/marginalization_factor.cpp src/utility/utility.cpp src/utility/visualization.cpp src/utility/CameraPoseVisualization.cpp src/initial/solve_5pts.cpp src/initial/initial_aligment.cpp src/initial/initial_sfm.cpp src/initial/initial_ex_rotation.cpp src/feature_tracker/feature_tracker.cpp) target_link_libraries(vins_lib ${catkin_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES} /usr/local/lib/libSophus.so ) add_library(estimator_nodelet src/estimator_nodelet.cpp) target_link_libraries(estimator_nodelet vins_lib) ================================================ FILE: vins_estimator/cmake/FindEigen.cmake ================================================ # Ceres Solver - A fast non-linear least squares minimizer # Copyright 2015 Google Inc. All rights reserved. # http://ceres-solver.org/ # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: # # * Redistributions of source code must retain the above copyright notice, # this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above copyright notice, # this list of conditions and the following disclaimer in the documentation # and/or other materials provided with the distribution. # * Neither the name of Google Inc. nor the names of its contributors may be # used to endorse or promote products derived from this software without # specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. # # Author: alexs.mac@gmail.com (Alex Stewart) # # FindEigen.cmake - Find Eigen library, version >= 3. # # This module defines the following variables: # # EIGEN_FOUND: TRUE iff Eigen is found. # EIGEN_INCLUDE_DIRS: Include directories for Eigen. # # EIGEN_VERSION: Extracted from Eigen/src/Core/util/Macros.h # EIGEN_WORLD_VERSION: Equal to 3 if EIGEN_VERSION = 3.2.0 # EIGEN_MAJOR_VERSION: Equal to 2 if EIGEN_VERSION = 3.2.0 # EIGEN_MINOR_VERSION: Equal to 0 if EIGEN_VERSION = 3.2.0 # # The following variables control the behaviour of this module: # # EIGEN_INCLUDE_DIR_HINTS: List of additional directories in which to # search for eigen includes, e.g: /timbuktu/eigen3. # # The following variables are also defined by this module, but in line with # CMake recommended FindPackage() module style should NOT be referenced directly # by callers (use the plural variables detailed above instead). These variables # do however affect the behaviour of the module via FIND_[PATH/LIBRARY]() which # are NOT re-called (i.e. search for library is not repeated) if these variables # are set with valid values _in the CMake cache_. This means that if these # variables are set directly in the cache, either by the user in the CMake GUI, # or by the user passing -DVAR=VALUE directives to CMake when called (which # explicitly defines a cache variable), then they will be used verbatim, # bypassing the HINTS variables and other hard-coded search locations. # # EIGEN_INCLUDE_DIR: Include directory for CXSparse, not including the # include directory of any dependencies. # Called if we failed to find Eigen or any of it's required dependencies, # unsets all public (designed to be used externally) variables and reports # error message at priority depending upon [REQUIRED/QUIET/] argument. macro(EIGEN_REPORT_NOT_FOUND REASON_MSG) unset(EIGEN_FOUND) unset(EIGEN_INCLUDE_DIRS) # Make results of search visible in the CMake GUI if Eigen has not # been found so that user does not have to toggle to advanced view. mark_as_advanced(CLEAR EIGEN_INCLUDE_DIR) # Note _FIND_[REQUIRED/QUIETLY] variables defined by FindPackage() # use the camelcase library name, not uppercase. if (Eigen_FIND_QUIETLY) message(STATUS "Failed to find Eigen - " ${REASON_MSG} ${ARGN}) elseif (Eigen_FIND_REQUIRED) message(FATAL_ERROR "Failed to find Eigen - " ${REASON_MSG} ${ARGN}) else() # Neither QUIETLY nor REQUIRED, use no priority which emits a message # but continues configuration and allows generation. message("-- Failed to find Eigen - " ${REASON_MSG} ${ARGN}) endif () return() endmacro(EIGEN_REPORT_NOT_FOUND) # Protect against any alternative find_package scripts for this library having # been called previously (in a client project) which set EIGEN_FOUND, but not # the other variables we require / set here which could cause the search logic # here to fail. unset(EIGEN_FOUND) # Search user-installed locations first, so that we prefer user installs # to system installs where both exist. list(APPEND EIGEN_CHECK_INCLUDE_DIRS /usr/local/include /usr/local/homebrew/include # Mac OS X /opt/local/var/macports/software # Mac OS X. /opt/local/include /usr/include) # Additional suffixes to try appending to each search path. list(APPEND EIGEN_CHECK_PATH_SUFFIXES eigen3 # Default root directory for Eigen. Eigen/include/eigen3 # Windows (for C:/Program Files prefix) < 3.3 Eigen3/include/eigen3 ) # Windows (for C:/Program Files prefix) >= 3.3 # Search supplied hint directories first if supplied. find_path(EIGEN_INCLUDE_DIR NAMES Eigen/Core PATHS ${EIGEN_INCLUDE_DIR_HINTS} ${EIGEN_CHECK_INCLUDE_DIRS} PATH_SUFFIXES ${EIGEN_CHECK_PATH_SUFFIXES}) if (NOT EIGEN_INCLUDE_DIR OR NOT EXISTS ${EIGEN_INCLUDE_DIR}) eigen_report_not_found( "Could not find eigen3 include directory, set EIGEN_INCLUDE_DIR to " "path to eigen3 include directory, e.g. /usr/local/include/eigen3.") endif (NOT EIGEN_INCLUDE_DIR OR NOT EXISTS ${EIGEN_INCLUDE_DIR}) # Mark internally as found, then verify. EIGEN_REPORT_NOT_FOUND() unsets # if called. set(EIGEN_FOUND TRUE) # Extract Eigen version from Eigen/src/Core/util/Macros.h if (EIGEN_INCLUDE_DIR) set(EIGEN_VERSION_FILE ${EIGEN_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h) if (NOT EXISTS ${EIGEN_VERSION_FILE}) eigen_report_not_found( "Could not find file: ${EIGEN_VERSION_FILE} " "containing version information in Eigen install located at: " "${EIGEN_INCLUDE_DIR}.") else (NOT EXISTS ${EIGEN_VERSION_FILE}) file(READ ${EIGEN_VERSION_FILE} EIGEN_VERSION_FILE_CONTENTS) string(REGEX MATCH "#define EIGEN_WORLD_VERSION [0-9]+" EIGEN_WORLD_VERSION "${EIGEN_VERSION_FILE_CONTENTS}") string(REGEX REPLACE "#define EIGEN_WORLD_VERSION ([0-9]+)" "\\1" EIGEN_WORLD_VERSION "${EIGEN_WORLD_VERSION}") string(REGEX MATCH "#define EIGEN_MAJOR_VERSION [0-9]+" EIGEN_MAJOR_VERSION "${EIGEN_VERSION_FILE_CONTENTS}") string(REGEX REPLACE "#define EIGEN_MAJOR_VERSION ([0-9]+)" "\\1" EIGEN_MAJOR_VERSION "${EIGEN_MAJOR_VERSION}") string(REGEX MATCH "#define EIGEN_MINOR_VERSION [0-9]+" EIGEN_MINOR_VERSION "${EIGEN_VERSION_FILE_CONTENTS}") string(REGEX REPLACE "#define EIGEN_MINOR_VERSION ([0-9]+)" "\\1" EIGEN_MINOR_VERSION "${EIGEN_MINOR_VERSION}") # This is on a single line s/t CMake does not interpret it as a list of # elements and insert ';' separators which would result in 3.;2.;0 nonsense. set(EIGEN_VERSION "${EIGEN_WORLD_VERSION}.${EIGEN_MAJOR_VERSION}.${EIGEN_MINOR_VERSION}") endif (NOT EXISTS ${EIGEN_VERSION_FILE}) endif (EIGEN_INCLUDE_DIR) # Set standard CMake FindPackage variables if found. if (EIGEN_FOUND) set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR}) endif (EIGEN_FOUND) # Handle REQUIRED / QUIET optional arguments and version. include(FindPackageHandleStandardArgs) find_package_handle_standard_args(Eigen REQUIRED_VARS EIGEN_INCLUDE_DIRS VERSION_VAR EIGEN_VERSION) # Only mark internal variables as advanced if we found Eigen, otherwise # leave it visible in the standard GUI for the user to set manually. if (EIGEN_FOUND) mark_as_advanced(FORCE EIGEN_INCLUDE_DIR) endif (EIGEN_FOUND) ================================================ FILE: vins_estimator/launch/atlas200/compressed2img.launch ================================================ ================================================ FILE: vins_estimator/launch/atlas200/img2compressed.launch ================================================ ================================================ FILE: vins_estimator/launch/openloris/openloris_vio_atlas.launch ================================================ ================================================ FILE: vins_estimator/launch/openloris/openloris_vio_pytorch.launch ================================================ ================================================ FILE: vins_estimator/launch/openloris/openloris_vio_tensorrt.launch ================================================ ================================================ FILE: vins_estimator/launch/openloris/openloris_vo.launch ================================================ ================================================ FILE: vins_estimator/launch/realsense/l515.launch ================================================ /camera/l500_depth_sensor/visual_preset: 5 /camera/l500_depth_sensor/laser_power: 100 /camera/l500_depth_sensor/confidence_threshold: 1 /camera/l500_depth_sensor/min_distance: 100 /camera/l500_depth_sensor/receiver_gain: 18 /camera/l500_depth_sensor/noise_filtering: 4 /camera/l500_depth_sensor/post_processing_sharpening: 1 /camera/l500_depth_sensor/pre_processing_sharpening: 0 /camera/l500_depth_sensor/digital_gain: 2 /camera/l500_depth_sensor/error_polling_enabled: true ================================================ FILE: vins_estimator/launch/realsense/realsense_vio.launch ================================================ ================================================ FILE: vins_estimator/launch/realsense/realsense_vio_atlas.launch ================================================ ================================================ FILE: vins_estimator/launch/realsense/rs_camera.launch ================================================ /camera/aligned_depth_to_color/image_raw/compressedDepth/png_level: 2 /camera/stereo_module/laser_power: 360.0 ================================================ FILE: vins_estimator/launch/tum_rgbd/tum_rgbd_atlas.launch ================================================ ================================================ FILE: vins_estimator/launch/tum_rgbd/tum_rgbd_pytorch.launch ================================================ ================================================ FILE: vins_estimator/launch/tum_rgbd/tum_rgbd_tensorrt.launch ================================================ ================================================ FILE: vins_estimator/launch/vins_rviz.launch ================================================ ================================================ FILE: vins_estimator/launch/yolo/atlas.launch ================================================ ================================================ FILE: vins_estimator/launch/yolo/pytorch.launch ================================================ ================================================ FILE: vins_estimator/launch/yolo/tensorrt.launch ================================================ ================================================ FILE: vins_estimator/nodelet_description.xml ================================================ EstimatorNodelet ================================================ FILE: vins_estimator/package.xml ================================================ vins_estimator 0.0.0 The vins_estimator package qintong TODO catkin roscpp camera_model roscpp camera_model nodelet nodelet ================================================ FILE: vins_estimator/src/estimator/estimator.cpp ================================================ #include "estimator.h" #include "../utility/visualization.h" #include #include #include #include #include Estimator::Estimator() : f_manager{Rs} { ROS_INFO("init begins"); clearState(); } void Estimator::setParameter() { for (int i = 0; i < NUM_OF_CAM; i++) { tic[i] = TIC[i]; ric[i] = RIC[i]; } f_manager.setRic(ric); ProjectionFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity(); ProjectionTdFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity(); td = TD; g = G; featureTracker.readIntrinsicParameter(CAM_NAMES); if (FISHEYE) { featureTracker.fisheye_mask = cv::imread(FISHEYE_MASK, 0); if (!featureTracker.fisheye_mask.data) { ROS_INFO("load mask fail"); ROS_BREAK(); } else ROS_INFO("load mask success"); } featureTracker.initGridsDetector(); } void Estimator::clearState() { m_imu.lock(); while (!imu_buf.empty()) imu_buf.pop(); m_imu.unlock(); for (int i = 0; i < WINDOW_SIZE + 1; i++) { Rs[i].setIdentity(); Ps[i].setZero(); Vs[i].setZero(); Bas[i].setZero(); Bgs[i].setZero(); dt_buf[i].clear(); linear_acceleration_buf[i].clear(); angular_velocity_buf[i].clear(); if (pre_integrations[i] != nullptr) delete pre_integrations[i]; pre_integrations[i] = nullptr; // cl find_solved[i] = 0; // cl } for (int i = 0; i < NUM_OF_CAM; i++) { tic[i] = Vector3d::Zero(); ric[i] = Matrix3d::Identity(); } for (auto &it : all_image_frame) { if (it.second.pre_integration != nullptr) { delete it.second.pre_integration; it.second.pre_integration = nullptr; } } first_imu = false, sum_of_back = 0; sum_of_front = 0; frame_count = 0; solver_flag = INITIAL; initial_timestamp = 0; all_image_frame.clear(); td = TD; openExEstimation = false; delete tmp_pre_integration; delete last_marginalization_info; tmp_pre_integration = nullptr; last_marginalization_info = nullptr; last_marginalization_parameter_blocks.clear(); f_manager.clearState(); failure_occur = false; relocalization_info = false; drift_correct_r = Matrix3d::Identity(); drift_correct_t = Vector3d::Zero(); latest_Q = Eigen::Quaterniond(1, 0, 0, 0); init_imu = true; prevTime = -1; initFirstPoseFlag = false; } void Estimator::processIMU(double dt, const Vector3d &linear_acceleration, const Vector3d &angular_velocity) { if (!first_imu) { first_imu = true; acc_0 = linear_acceleration; gyr_0 = angular_velocity; } if (!pre_integrations[frame_count]) { pre_integrations[frame_count] = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]}; } if (frame_count != 0) { pre_integrations[frame_count]->push_back(dt, linear_acceleration, angular_velocity); // if(solver_flag != NON_LINEAR) tmp_pre_integration->push_back(dt, linear_acceleration, angular_velocity); dt_buf[frame_count].push_back(dt); linear_acceleration_buf[frame_count].push_back(linear_acceleration); angular_velocity_buf[frame_count].push_back(angular_velocity); int j = frame_count; Vector3d un_acc_0 = Rs[j] * (acc_0 - Bas[j]) - g; Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - Bgs[j]; Rs[j] *= Utility::deltaQ(un_gyr * dt).toRotationMatrix(); Vector3d un_acc_1 = Rs[j] * (linear_acceleration - Bas[j]) - g; Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1); Ps[j] += dt * Vs[j] + 0.5 * dt * dt * un_acc; Vs[j] += dt * un_acc; } acc_0 = linear_acceleration; gyr_0 = angular_velocity; } void Estimator::processImage(map> &image, const std_msgs::Header &header) { ROS_DEBUG("new image coming ------------------------------------------"); ROS_DEBUG("Adding feature points %lu", image.size()); // FeaturePerFrame // FeaturePerId // feature if (f_manager.addFeatureCheckParallax(frame_count, image, td)) marginalization_flag = MARGIN_OLD; else marginalization_flag = MARGIN_SECOND_NEW; ROS_DEBUG("%s", marginalization_flag ? "Non-keyframe" : "Keyframe"); ROS_DEBUG("Solving %d", frame_count); ROS_DEBUG("number of feature: %d", f_manager.getFeatureCount()); Headers[frame_count] = header.stamp.toSec(); if (USE_IMU) { double curTime = header.stamp.toSec() + td; while (!IMUAvailable(curTime)) { printf("waiting for imu ... \r"); std::chrono::milliseconds dura(2); std::this_thread::sleep_for(dura); } std::vector>> imu_vector; getIMUInterval(prevTime, curTime, imu_vector); if (!initFirstPoseFlag) initFirstIMUPose(imu_vector); for (size_t i = 0; i < imu_vector.size(); i++) { double dt; if (i == 0) dt = imu_vector[i].first - prevTime; else if (i == imu_vector.size() - 1) dt = curTime - imu_vector[i - 1].first; else dt = imu_vector[i].first - imu_vector[i - 1].first; processIMU(dt, imu_vector[i].second.first, imu_vector[i].second.second); } prevTime = curTime; } ImageFrame imageframe(image, header.stamp.toSec()); imageframe.pre_integration = tmp_pre_integration; all_image_frame.insert(make_pair(header.stamp.toSec(), imageframe)); tmp_pre_integration = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]}; if (ESTIMATE_EXTRINSIC == 2) { ROS_INFO("calibrating extrinsic param, rotation movement is needed"); if (frame_count != 0) { vector> corres = f_manager.getCorresponding(frame_count - 1, frame_count); Matrix3d calib_ric; if (initial_ex_rotation.CalibrationExRotation( corres, pre_integrations[frame_count]->delta_q, calib_ric)) { ROS_WARN("initial extrinsic rotation calib success"); ROS_WARN_STREAM("initial extrinsic rotation: " << endl << calib_ric); ric[0] = calib_ric; RIC[0] = calib_ric; ESTIMATE_EXTRINSIC = 1; } } } TicToc opt_time; if (solver_flag == INITIAL) { if (USE_IMU && !STATIC_INIT) { if (frame_count == WINDOW_SIZE) { bool result = false; if (ESTIMATE_EXTRINSIC != 2 && (header.stamp.toSec() - initial_timestamp) > 0.1) { result = initialStructure(); initial_timestamp = header.stamp.toSec(); } // if init sfm success if (result) { solver_flag = NON_LINEAR; solveOdometry(); slideWindow(); f_manager.removeFailures(); ROS_INFO("Initialization finish!"); last_R = Rs[WINDOW_SIZE]; last_P = Ps[WINDOW_SIZE]; last_R0 = Rs[0]; last_P0 = Ps[0]; } else slideWindow(); } else frame_count++; } else { f_manager.triangulateWithDepth(Ps, tic, ric); if (USE_IMU) { if (frame_count == WINDOW_SIZE) { int i = 0; for (auto &frame_it : all_image_frame) { frame_it.second.R = Rs[i]; frame_it.second.T = Ps[i]; i++; } if (ESTIMATE_EXTRINSIC != 2) { solveGyroscopeBias(all_image_frame, Bgs); for (int j = 0; j <= WINDOW_SIZE; j++) { pre_integrations[j]->repropagate(Vector3d::Zero(), Bgs[j]); } optimization(); updateLatestStates(); solver_flag = NON_LINEAR; slideWindow(); ROS_INFO("Initialization finish!"); last_R = Rs[WINDOW_SIZE]; last_P = Ps[WINDOW_SIZE]; last_R0 = Rs[0]; last_P0 = Ps[0]; } } } else { if (frame_count == WINDOW_SIZE) { optimization(); updateLatestStates(); solver_flag = NON_LINEAR; slideWindow(); ROS_INFO("Initialization finish!"); } } if (frame_count < WINDOW_SIZE) { frame_count++; int prev_frame = frame_count - 1; Ps[frame_count] = Ps[prev_frame]; Vs[frame_count] = Vs[prev_frame]; Rs[frame_count] = Rs[prev_frame]; Bas[frame_count] = Bas[prev_frame]; Bgs[frame_count] = Bgs[prev_frame]; } } } else { TicToc t_solve; if (!USE_IMU) f_manager.initFramePoseByPnP(frame_count, Ps, Rs, tic, ric); f_manager.triangulateWithDepth(Ps, tic, ric); optimization(); ROS_DEBUG("solver costs: %fms", t_solve.toc()); set removeIndex; movingConsistencyCheck(removeIndex); if (SHOW_TRACK) { for (auto iter = image.begin(), iter_next = image.begin(); iter != image.end(); iter = iter_next) { ++iter_next; auto it = removeIndex.find(iter->first); if (it != removeIndex.end()) { image.erase(iter); } } } if (failureDetection()) { ROS_WARN("failure detection!"); failure_occur = true; clearState(); setParameter(); ROS_WARN("system reboot!"); return; } slideWindow(); f_manager.removeFailures(); // prepare output of VINS key_poses.clear(); for (int i = 0; i <= WINDOW_SIZE; i++) key_poses.emplace_back(Ps[i]); last_R = Rs[WINDOW_SIZE]; last_P = Ps[WINDOW_SIZE]; last_R0 = Rs[0]; last_P0 = Ps[0]; updateLatestStates(); } static double whole_opt_time = 0; static size_t cnt_frame = 0; ++cnt_frame; whole_opt_time += opt_time.toc(); ROS_DEBUG("average opt costs: %f", whole_opt_time / cnt_frame); } /** * @brief 视觉的结构初始化 * @Description 确保IMU有充分运动激励 * relativePose()找到具有足够视差的两帧,由F矩阵恢复R、t作为初始值 * sfm.construct() 全局纯视觉SFM 恢复滑动窗口帧的位姿 * visualInitialAlign()视觉惯性联合初始化 * @return bool true:初始化成功 */ bool Estimator::initialStructure() { // check imu observibility bool is_imu_excited = false; { map::iterator frame_it; Vector3d sum_g; for (frame_it = all_image_frame.begin(), frame_it++; frame_it != all_image_frame.end(); frame_it++) { double dt = frame_it->second.pre_integration->sum_dt; Vector3d tmp_g = frame_it->second.pre_integration->delta_v / dt; sum_g += tmp_g; } Vector3d aver_g; aver_g = sum_g * 1.0 / ((int)all_image_frame.size() - 1); double var = 0; for (frame_it = all_image_frame.begin(), frame_it++; frame_it != all_image_frame.end(); frame_it++) { double dt = frame_it->second.pre_integration->sum_dt; Vector3d tmp_g = frame_it->second.pre_integration->delta_v / dt; var += (tmp_g - aver_g).transpose() * (tmp_g - aver_g); // cout << "frame g " << tmp_g.transpose() << endl; } var = sqrt(var / ((int)all_image_frame.size() - 1)); // 标准差 // ROS_WARN("IMU variation %f!", var); if (var < 0.25) { ROS_INFO("IMU excitation not enouth!"); // return false; } else { is_imu_excited = true; } } TicToc t_sfm; // global sfm Quaterniond Q[frame_count + 1]; Vector3d T[frame_count + 1]; map sfm_tracked_points; vector sfm_f; for (auto &it_per_id : f_manager.feature) { int imu_j = it_per_id.start_frame - 1; SFMFeature tmp_feature; tmp_feature.state = false; tmp_feature.id = it_per_id.feature_id; for (auto &it_per_frame : it_per_id.feature_per_frame) { imu_j++; Vector3d pts_j = it_per_frame.point; tmp_feature.observation.emplace_back(imu_j, Eigen::Vector2d{pts_j.x(), pts_j.y()}); tmp_feature.observation_depth.emplace_back(imu_j, it_per_frame.depth); } sfm_f.emplace_back(tmp_feature); } Matrix3d relative_R; Vector3d relative_T; int l; //保证具有足够的视差,由F矩阵恢复Rt //第l帧是从第一帧开始到滑动窗口中第一个满足与当前帧的平均视差足够大的帧，会作为参考帧到下面的全局sfm使用 //此处的relative_R，relative_T为当前帧到参考帧（第l帧）的坐标系变换Rt if (!relativePose(relative_R, relative_T, l)) { ROS_INFO("Not enough features or parallax; Move device around"); // ROS_INFO("Not enough features!"); return false; } //对窗口中每个图像帧求解sfm问题 //得到所有图像帧相对于参考帧的姿态四元数Q、平移向量T和特征点坐标sfm_tracked_points。 GlobalSFM sfm; if (!sfm.construct(frame_count + 1, Q, T, l, relative_R, relative_T, sfm_f, sfm_tracked_points)) { ROS_DEBUG("global SFM failed!"); marginalization_flag = MARGIN_OLD; return false; } // solve pnp for all frame //对于所有的图像帧，包括不在滑动窗口中的，提供初始的RT估计，然后solvePnP进行优化,得到每一帧的姿态 map::iterator frame_it; map::iterator it; frame_it = all_image_frame.begin(); for (int i = 0; frame_it != all_image_frame.end(); frame_it++) { // provide initial guess cv::Mat r, rvec, t, D, tmp_r; if ((frame_it->first) == Headers[i]) { frame_it->second.is_key_frame = true; frame_it->second.R = Q[i].toRotationMatrix() * RIC[0].transpose(); frame_it->second.T = T[i]; i++; continue; } if ((frame_it->first) > Headers[i]) { i++; } Matrix3d R_inital = (Q[i].inverse()).toRotationMatrix(); Vector3d P_inital = -R_inital * T[i]; cv::eigen2cv(R_inital, tmp_r); cv::Rodrigues(tmp_r, rvec); cv::eigen2cv(P_inital, t); frame_it->second.is_key_frame = false; vector pts_3_vector; vector pts_2_vector; // points: map>>> for (auto &id_pts : frame_it->second.points) { int feature_id = id_pts.first; it = sfm_tracked_points.find(feature_id); if (it != sfm_tracked_points.end()) { Vector3d world_pts = it->second; cv::Point3f pts_3(world_pts(0), world_pts(1), world_pts(2)); pts_3_vector.push_back(pts_3); Vector2d img_pts = id_pts.second.head<2>(); cv::Point2f pts_2(img_pts(0), img_pts(1)); pts_2_vector.push_back(pts_2); } } cv::Mat K = (cv::Mat_(3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 1); if (pts_3_vector.size() < 6) { cout << "pts_3_vector size " << pts_3_vector.size() << endl; ROS_DEBUG("Not enough points for solve pnp !"); return false; } /** *bool cv::solvePnP( 求解pnp问题 * InputArray objectPoints, 特征点的3D坐标数组 * InputArray imagePoints, 特征点对应的图像坐标 * InputArray cameraMatrix, 相机内参矩阵 * InputArray distCoeffs, 失真系数的输入向量 * OutputArray rvec, 旋转向量 * OutputArray tvec, 平移向量 * bool useExtrinsicGuess = false, 为真则使用提供的初始估计值 * int flags = SOLVEPNP_ITERATIVE 采用LM优化 *) */ if (!cv::solvePnP(pts_3_vector, pts_2_vector, K, D, rvec, t, 1)) { ROS_DEBUG("solve pnp fail!"); return false; } cv::Rodrigues(rvec, r); MatrixXd R_pnp, tmp_R_pnp; cv::cv2eigen(r, tmp_R_pnp); //这里也同样需要将坐标变换矩阵转变成图像帧位姿，并转换为IMU坐标系的位姿 R_pnp = tmp_R_pnp.transpose(); MatrixXd T_pnp; cv::cv2eigen(t, T_pnp); T_pnp = R_pnp * (-T_pnp); frame_it->second.R = R_pnp * RIC[0].transpose(); frame_it->second.T = T_pnp; } // Rs Ps ric init //进行视觉惯性联合初始化 if (visualInitialAlignWithDepth()) { if (!is_imu_excited) { // 利用加速度平均值估计Bas Vector3d sum_a(0, 0, 0); for (frame_it = all_image_frame.begin(), frame_it++; frame_it != all_image_frame.end(); frame_it++) { double dt = frame_it->second.pre_integration->sum_dt; Vector3d tmp_a = frame_it->second.pre_integration->delta_v / dt; sum_a += tmp_a; } Vector3d avg_a; avg_a = sum_a * 1.0 / ((int)all_image_frame.size() - 1); Vector3d tmp_Bas = avg_a - Utility::g2R(avg_a).inverse() * G; ROS_WARN_STREAM("accelerator bias initial calibration " << tmp_Bas.transpose()); for (int i = 0; i <= WINDOW_SIZE; i++) { Bas[i] = tmp_Bas; } } return true; } else { ROS_INFO("misalign visual structure with IMU"); return false; } } bool Estimator::initialStructureWithDepth() { // check imu observibility bool is_imu_excited = false; map::iterator frame_it; Vector3d sum_a; for (frame_it = all_image_frame.begin(), frame_it++; frame_it != all_image_frame.end(); frame_it++) { double dt = frame_it->second.pre_integration->sum_dt; Vector3d tmp_g = frame_it->second.pre_integration->delta_v / dt; sum_a += tmp_g; } Vector3d aver_g; aver_g = sum_a * 1.0 / ((int)all_image_frame.size() - 1); double var = 0; for (frame_it = all_image_frame.begin(), frame_it++; frame_it != all_image_frame.end(); frame_it++) { double dt = frame_it->second.pre_integration->sum_dt; Vector3d tmp_g = frame_it->second.pre_integration->delta_v / dt; var += (tmp_g - aver_g).transpose() * (tmp_g - aver_g); } var = sqrt(var / ((int)all_image_frame.size() - 1)); // 标准差 // ROS_WARN("IMU variation %f!", var); if (var < 0.25) { ROS_INFO("IMU excitation not enouth!"); // return false; } else { is_imu_excited = true; } if (visualInitialAlignWithDepth()) { if (!is_imu_excited) { Vector3d tmp_Bas = aver_g - Utility::g2R(aver_g).inverse() * G; ROS_WARN_STREAM("accelerator bias initial calibration " << tmp_Bas.transpose()); for (int i = 0; i <= WINDOW_SIZE; i++) { Bas[i] = tmp_Bas; } } return true; } else { staticInitialAlignWithDepth(); return true; } ROS_INFO("misalign visual structure with IMU"); return false; } bool Estimator::visualInitialAlign() { TicToc t_g; VectorXd x; // solve scale bool result = VisualIMUAlignment(all_image_frame, Bgs, g, x); if (!result) { ROS_ERROR("solve g failed!"); return false; } // change state for (int i = 0; i <= frame_count; i++) { Matrix3d Ri = all_image_frame[Headers[i]].R; Vector3d Pi = all_image_frame[Headers[i]].T; Ps[i] = Pi; Rs[i] = Ri; all_image_frame[Headers[i]].is_key_frame = true; } VectorXd dep = f_manager.getDepthVector(); for (int i = 0; i < dep.size(); i++) dep[i] = -1; f_manager.clearDepth(dep); // triangulat on cam pose , no tic Vector3d TIC_TMP[NUM_OF_CAM]; for (int i = 0; i < NUM_OF_CAM; i++) TIC_TMP[i].setZero(); ric[0] = RIC[0]; f_manager.setRic(ric); // f_manager.triangulate(Ps, &(TIC_TMP[0]), &(RIC[0])); f_manager.triangulateWithDepth(Ps, &(TIC_TMP[0]), &(RIC[0])); double s = (x.tail<1>())(0); // ROS_DEBUG("the scale is %f\n", s); // do repropagate here for (int i = 0; i <= WINDOW_SIZE; i++) { pre_integrations[i]->repropagate(Vector3d::Zero(), Bgs[i]); } for (int i = frame_count; i >= 0; i--) Ps[i] = s * Ps[i] - Rs[i] * TIC[0] - (s * Ps[0] - Rs[0] * TIC[0]); int kv = -1; map::iterator frame_i; for (frame_i = all_image_frame.begin(); frame_i != all_image_frame.end(); frame_i++) { if (frame_i->second.is_key_frame) { kv++; Vs[kv] = frame_i->second.R * x.segment<3>(kv * 3); } } for (auto &it_per_id : f_manager.feature) { it_per_id.used_num = it_per_id.feature_per_frame.size(); // if (it_per_id.used_num < 4) // continue; if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2)) continue; it_per_id.estimated_depth *= s; } Matrix3d R0 = Utility::g2R(g); double yaw = Utility::R2ypr(R0 * Rs[0]).x(); R0 = Utility::ypr2R(Eigen::Vector3d{-yaw, 0, 0}) * R0; g = R0 * g; // Matrix3d rot_diff = R0 * Rs[0].transpose(); Matrix3d rot_diff = R0; for (int i = 0; i <= frame_count; i++) { Ps[i] = rot_diff * Ps[i]; Rs[i] = rot_diff * Rs[i]; Vs[i] = rot_diff * Vs[i]; } ROS_DEBUG_STREAM("g0 " << g.transpose()); ROS_DEBUG_STREAM("my R0 " << Utility::R2ypr(Rs[0]).transpose()); return true; } /** * @brief 视觉惯性联合初始化 * @Description 陀螺仪的偏置校准(加速度偏置没有处理) 计算速度V[0:n] 重力g 尺度s * 更新了Bgs后，IMU测量量需要repropagate * 得到尺度s和重力g的方向后，需更新所有图像帧在世界坐标系下的Ps、Rs、Vs * @return bool true：成功 */ bool Estimator::staticInitialAlignWithDepth() { // 利用加速度平均值估计Bgs, Bas, g map::iterator frame_it; Vector3d sum_a(0, 0, 0); Vector3d sum_w(0, 0, 0); for (frame_it = all_image_frame.begin(), frame_it++; frame_it != all_image_frame.end(); frame_it++) { sum_a += frame_it->second.pre_integration->delta_v / frame_it->second.pre_integration->sum_dt; Vector3d tmp_w; for (auto &gyr_msg : frame_it->second.pre_integration->gyr_buf) { tmp_w += gyr_msg; } sum_w += tmp_w / frame_it->second.pre_integration->gyr_buf.size(); } Vector3d avg_a = sum_a * 1.0 / ((int)all_image_frame.size() - 1); Vector3d avg_w = sum_w * 1.0 / ((int)all_image_frame.size() - 1); g = avg_a.normalized() * G.z(); Vector3d tmp_Bas = avg_a - g; // solveGyroscopeBias(all_image_frame, Bgs); ROS_WARN_STREAM("gyroscope bias initial calibration " << avg_w.transpose()); ROS_WARN_STREAM("accelerator bias initial calibration " << tmp_Bas.transpose()); for (int i = 0; i <= WINDOW_SIZE; i++) { Bgs[i] = avg_w; Bas[i] = tmp_Bas; } //陀螺仪的偏置bgs改变，重新计算预积分 for (int i = 0; i <= WINDOW_SIZE; i++) { pre_integrations[i]->repropagate(Bas[i], Bgs[i]); } //通过将重力旋转到z轴上，得到世界坐标系与摄像机坐标系c0之间的旋转矩阵rot_diff Matrix3d R0 = Utility::g2R(g); double yaw = Utility::R2ypr(R0).x(); R0 = Utility::ypr2R(Eigen::Vector3d{-yaw, 0, 0}) * R0; g = R0 * G; Matrix3d rot_diff = R0; //所有变量从参考坐标系c0旋转到世界坐标系w for (int i = 0; i <= frame_count; i++) { ROS_ERROR("%d farme's Ps is %f | %f | %f\n", i, Ps[i].x(), Ps[i].y(), Ps[i].z()); // shan add ROS_ERROR("%d farme's Vs is %f | %f | %f\n", i, Vs[i].x(), Vs[i].y(), Vs[i].z()); Ps[i] = rot_diff * Ps[i]; Rs[i] = rot_diff * Rs[i]; Vs[i] = rot_diff * Vs[i]; // Vs[i] = Vector3d(0, 0, 0); } ROS_DEBUG_STREAM("static g0 " << g.transpose()); ROS_DEBUG_STREAM("my R0 " << Utility::R2ypr(Rs[0]).transpose()); return true; } /** * @brief 视觉惯性联合初始化 * @Description 陀螺仪的偏置校准(加速度偏置没有处理) 计算速度V[0:n] 重力g 尺度s * 更新了Bgs后，IMU测量量需要repropagate * 得到尺度s和重力g的方向后，需更新所有图像帧在世界坐标系下的Ps、Rs、Vs * @return bool true：成功 */ bool Estimator::visualInitialAlignWithDepth() { TicToc t_g; VectorXd x; // solve scale //计算陀螺仪偏置，尺度，重力加速度和速度 solveGyroscopeBias(all_image_frame, Bgs); if (!LinearAlignmentWithDepth(all_image_frame, g, x)) { ROS_ERROR("solve g failed!"); return false; } // do repropagate here //陀螺仪的偏置bgs改变，重新计算预积分 for (int i = 0; i <= WINDOW_SIZE; i++) { pre_integrations[i]->repropagate(Vector3d::Zero(), Bgs[i]); } // change state // 得到所有图像帧的位姿Ps、Rs，并将其置为关键帧 for (int i = 0; i <= frame_count; i++) { Matrix3d Ri = all_image_frame[Headers[i]].R; Vector3d Pi = all_image_frame[Headers[i]].T; Ps[i] = Pi; Rs[i] = Ri; all_image_frame[Headers[i]].is_key_frame = true; } // do repropagate here //陀螺仪的偏置bgs改变，重新计算预积分 for (int i = 0; i <= WINDOW_SIZE; i++) { pre_integrations[i]->repropagate(Vector3d::Zero(), Bgs[i]); } // ROS_ERROR("before %f | %f | %f\n", Ps[1].x(), Ps[1].y(), Ps[1].z());//shan // add 将Ps、Vs、depth尺度s缩放 for (int i = frame_count; i >= 0; i--) Ps[i] = Ps[i] - Rs[i] * TIC[0] - (Ps[0] - Rs[0] * TIC[0]); // ROS_ERROR("after %f | %f | %f\n", Ps[1].x(), Ps[1].y(), Ps[1].z());//shan // add int kv = -1; map::iterator frame_i; for (frame_i = all_image_frame.begin(); frame_i != all_image_frame.end(); frame_i++) { if (frame_i->second.is_key_frame) { kv++; Vs[kv] = frame_i->second.R * x.segment<3>(kv * 3); } } //通过将重力旋转到z轴上，得到世界坐标系与摄像机坐标系c0之间的旋转矩阵rot_diff Matrix3d R0 = Utility::g2R(g); double yaw = Utility::R2ypr(R0 * Rs[0]).x(); R0 = Utility::ypr2R(Eigen::Vector3d{-yaw, 0, 0}) * R0; g = R0 * g; // Matrix3d rot_diff = R0 * Rs[0].transpose(); Matrix3d rot_diff = R0; //所有变量从参考坐标系c0旋转到世界坐标系w for (int i = 0; i <= frame_count; i++) { Ps[i] = rot_diff * Ps[i]; Rs[i] = rot_diff * Rs[i]; Vs[i] = rot_diff * Vs[i]; } ROS_DEBUG_STREAM("g0 " << g.transpose()); ROS_DEBUG_STREAM("my R0 " << Utility::R2ypr(Rs[0]).transpose()); return true; } /** * @brief 判断两帧有足够视差30且内点数目大于12则可进行初始化，同时得到R和T * @Description 判断每帧到窗口最后一帧对应特征点的平均视差是否大于30 solveRelativeRT()通过基础矩阵计算当前帧与第l帧之间的R和T,并判断内点数目是否足够 * @param[out] relative_R 当前帧到第l帧之间的旋转矩阵R * @param[out] relative_T 当前帧到第l帧之间的平移向量T * @param[out] L 保存滑动窗口中与当前帧满足初始化条件的那一帧 * @return bool 1:可以进行初始化;0:不满足初始化条件 */ bool Estimator::relativePose(Matrix3d &relative_R, Vector3d &relative_T, int &l) { // find previous frame which contians enough correspondance and parallex with // newest frame for (int i = 0; i < WINDOW_SIZE; i++) { vector> corres; // corres = f_manager.getCorresponding(i, WINDOW_SIZE); corres = f_manager.getCorrespondingWithDepth(i, WINDOW_SIZE); if (corres.size() > 20) { double sum_parallax = 0; double average_parallax; for (auto &corre : corres) { Vector2d pts_0(corre.first(0) / corre.first(2), corre.first(1) / corre.first(2)); Vector2d pts_1(corre.second(0) / corre.second(2), corre.second(1) / corre.second(2)); // Vector2d pts_0(corres[j].first(0), corres[j].first(1)); // Vector2d pts_1(corres[j].second(0), corres[j].second(1)); double parallax = (pts_0 - pts_1).norm(); sum_parallax = sum_parallax + parallax; } average_parallax = 1.0 * sum_parallax / int(corres.size()); if (average_parallax * 460 > 30 && m_estimator.solveRelativeRT_PNP(corres, relative_R, relative_T)) { l = i; ROS_DEBUG("average_parallax %f choose l %d and newest frame to " "triangulate the whole structure", average_parallax * 460, l); return true; } } } return false; } void Estimator::solveOdometry() { if (frame_count < WINDOW_SIZE) return; if (solver_flag == NON_LINEAR) { TicToc t_tri; f_manager.triangulateWithDepth(Ps, tic, ric); // f_manager.triangulate(Ps, tic, ric); ROS_DEBUG("triangulation costs %f", t_tri.toc()); optimization(); } } void Estimator::vector2double() { for (int i = 0; i <= WINDOW_SIZE; i++) { para_Pose[i][0] = Ps[i].x(); para_Pose[i][1] = Ps[i].y(); para_Pose[i][2] = Ps[i].z(); Quaterniond q{Rs[i]}; para_Pose[i][3] = q.x(); para_Pose[i][4] = q.y(); para_Pose[i][5] = q.z(); para_Pose[i][6] = q.w(); if (USE_IMU) { para_SpeedBias[i][0] = Vs[i].x(); para_SpeedBias[i][1] = Vs[i].y(); para_SpeedBias[i][2] = Vs[i].z(); para_SpeedBias[i][3] = Bas[i].x(); para_SpeedBias[i][4] = Bas[i].y(); para_SpeedBias[i][5] = Bas[i].z(); para_SpeedBias[i][6] = Bgs[i].x(); para_SpeedBias[i][7] = Bgs[i].y(); para_SpeedBias[i][8] = Bgs[i].z(); } } for (int i = 0; i < NUM_OF_CAM; i++) { para_Ex_Pose[i][0] = tic[i].x(); para_Ex_Pose[i][1] = tic[i].y(); para_Ex_Pose[i][2] = tic[i].z(); Quaterniond q{ric[i]}; para_Ex_Pose[i][3] = q.x(); para_Ex_Pose[i][4] = q.y(); para_Ex_Pose[i][5] = q.z(); para_Ex_Pose[i][6] = q.w(); } VectorXd dep = f_manager.getDepthVector(); for (int i = 0; i < f_manager.getFeatureCount(); i++) para_Feature[i][0] = dep(i); if (ESTIMATE_TD) para_Td[0][0] = td; } // 数据转换，vector2double的相反过程 // 同时这里为防止优化结果往零空间变化，会根据优化前后第一帧的位姿差进行修正。 void Estimator::double2vector() { Vector3d origin_R0 = Utility::R2ypr(Rs[0]); Vector3d origin_P0 = Ps[0]; if (failure_occur) { origin_R0 = Utility::R2ypr(last_R0); origin_P0 = last_P0; failure_occur = false; } if (USE_IMU) { Vector3d origin_R00 = Utility::R2ypr( Quaterniond(para_Pose[0][6], para_Pose[0][3], para_Pose[0][4], para_Pose[0][5]) .toRotationMatrix()); double y_diff = origin_R0.x() - origin_R00.x(); // TODO Matrix3d rot_diff = Utility::ypr2R(Vector3d(y_diff, 0, 0)); if (abs(abs(origin_R0.y()) - 90) < 1.0 || abs(abs(origin_R00.y()) - 90) < 1.0) { ROS_DEBUG("euler singular point!"); rot_diff = Rs[0] * Quaterniond(para_Pose[0][6], para_Pose[0][3], para_Pose[0][4], para_Pose[0][5]) .toRotationMatrix() .transpose(); } for (int i = 0; i <= WINDOW_SIZE; i++) { Rs[i] = rot_diff * Quaterniond(para_Pose[i][6], para_Pose[i][3], para_Pose[i][4], para_Pose[i][5]) .normalized() .toRotationMatrix(); Ps[i] = rot_diff * Vector3d(para_Pose[i][0] - para_Pose[0][0], para_Pose[i][1] - para_Pose[0][1], para_Pose[i][2] - para_Pose[0][2]) + origin_P0; Vs[i] = rot_diff * Vector3d(para_SpeedBias[i][0], para_SpeedBias[i][1], para_SpeedBias[i][2]); Bas[i] = Vector3d(para_SpeedBias[i][3], para_SpeedBias[i][4], para_SpeedBias[i][5]); Bgs[i] = Vector3d(para_SpeedBias[i][6], para_SpeedBias[i][7], para_SpeedBias[i][8]); } // relative info between two loop frame if (relocalization_info) { Matrix3d relo_r; Vector3d relo_t; relo_r = rot_diff * Quaterniond(relo_Pose[6], relo_Pose[3], relo_Pose[4], relo_Pose[5]) .normalized() .toRotationMatrix(); relo_t = rot_diff * Vector3d(relo_Pose[0] - para_Pose[0][0], relo_Pose[1] - para_Pose[0][1], relo_Pose[2] - para_Pose[0][2]) + origin_P0; double drift_correct_yaw; drift_correct_yaw = Utility::R2ypr(prev_relo_r).x() - Utility::R2ypr(relo_r).x(); drift_correct_r = Utility::ypr2R(Vector3d(drift_correct_yaw, 0, 0)); drift_correct_t = prev_relo_t - drift_correct_r * relo_t; relo_relative_t = relo_r.transpose() * (Ps[relo_frame_local_index] - relo_t); relo_relative_q = relo_r.transpose() * Rs[relo_frame_local_index]; relo_relative_yaw = Utility::normalizeAngle( Utility::R2ypr(Rs[relo_frame_local_index]).x() - Utility::R2ypr(relo_r).x()); // cout << "vins relo " << endl; // cout << "vins relative_t " << relo_relative_t.transpose() << endl; // cout << "vins relative_yaw " < 2.5) { ROS_INFO(" big IMU acc bias estimation %f", Bas[WINDOW_SIZE].norm()); return true; } if (Bgs[WINDOW_SIZE].norm() > 1.0) { ROS_INFO(" big IMU gyr bias estimation %f", Bgs[WINDOW_SIZE].norm()); return true; } /* if (tic(0) > 1) { ROS_INFO(" big extri param estimation %d", tic(0) > 1); return true; } */ Vector3d tmp_P = Ps[WINDOW_SIZE]; if ((tmp_P - last_P).norm() > 5) { ROS_INFO(" big translation"); return true; } if (abs(tmp_P.z() - last_P.z()) > 1) { ROS_INFO(" big z translation"); return true; } Matrix3d tmp_R = Rs[WINDOW_SIZE]; Matrix3d delta_R = tmp_R.transpose() * last_R; Quaterniond delta_Q(delta_R); double delta_angle; delta_angle = acos(delta_Q.w()) * 2.0 / 3.14 * 180.0; if (delta_angle > 50) { ROS_INFO(" big delta_angle "); // return true; } return false; } void Estimator::optimization() { TicToc t_whole, t_prepare; vector2double(); ceres::Problem problem; ceres::LossFunction *loss_function; // loss_function = new ceres::HuberLoss(1.0); loss_function = new ceres::CauchyLoss(1.0); /*######优化参数：q、p；v、Ba、Bg#######*/ //添加ceres参数块 //因为ceres用的是double数组，所以在下面用vector2double做类型装换 // Ps、Rs转变成para_Pose，Vs、Bas、Bgs转变成para_SpeedBias for (int i = 0; i < frame_count + 1; i++) { ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization(); problem.AddParameterBlock(para_Pose[i], SIZE_POSE, local_parameterization); if (USE_IMU) problem.AddParameterBlock(para_SpeedBias[i], SIZE_SPEEDBIAS); // v、Ba、Bg参数 } if (!USE_IMU) { problem.SetParameterBlockConstant(para_Pose[0]); } /*######优化参数：imu与camera外参#######*/ for (auto &i : para_Ex_Pose) { ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization(); problem.AddParameterBlock(i, SIZE_POSE, local_parameterization); if ((ESTIMATE_EXTRINSIC && frame_count == WINDOW_SIZE && Vs[0].norm() > 0.2) || openExEstimation) { // ROS_DEBUG("estimate extinsic param"); openExEstimation = true; } else { // ROS_DEBUG("fix extinsic param"); problem.SetParameterBlockConstant(i); } } /*######优化参数：imu与camera之间的time offset#######*/ if (USE_IMU) { problem.AddParameterBlock(para_Td[0], 1); //速度过低时，不估计td if (!ESTIMATE_TD || Vs[0].norm() < 0.2) { problem.SetParameterBlockConstant(para_Td[0]); } } //构建残差 /*******先验残差*******/ if (last_marginalization_info) { // construct new marginlization_factor MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info); problem.AddResidualBlock(marginalization_factor, NULL, last_marginalization_parameter_blocks); } /*******预积分残差*******/ if (USE_IMU) { for (int i = 0; i < frame_count; i++) //预积分残差，总数目为frame_count { int j = i + 1; if (pre_integrations[j]->sum_dt > 10.0) //两图像帧之间时间过长，不使用中间的预积分 tzhang continue; IMUFactor *imu_factor = new IMUFactor(pre_integrations[j]); //添加残差格式：残差因子，鲁棒核函数，优化变量（i时刻位姿，i时刻速度与偏置，i+1时刻位姿，i+1时刻速度与偏置） problem.AddResidualBlock(imu_factor, NULL, para_Pose[i], para_SpeedBias[i], para_Pose[j], para_SpeedBias[j]); } } /*******重投影残差*******/ //重投影残差相关，此时使用了Huber损失核函数 int f_m_cnt = 0; int feature_index = -1; for (auto &it_per_id : f_manager.feature) { if (it_per_id.is_dynamic) { continue; } it_per_id.used_num = it_per_id.feature_per_frame.size(); if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2)) continue; // if (it_per_id.used_num < 4) // continue; ++feature_index; int imu_i = it_per_id.start_frame, imu_j = imu_i - 1; Vector3d pts_i = it_per_id.feature_per_frame[0].point; for (auto &it_per_frame : it_per_id.feature_per_frame) //遍历观测到路标点的图像帧 { imu_j++; if (imu_i == imu_j) { continue; } Vector3d pts_j = it_per_frame.point; //测量值 if (ESTIMATE_TD) { ProjectionTdFactor *f_td = new ProjectionTdFactor( pts_i, pts_j, it_per_id.feature_per_frame[0].velocity, it_per_frame.velocity, it_per_id.feature_per_frame[0].cur_td, it_per_frame.cur_td, it_per_id.feature_per_frame[0].uv.y(), it_per_frame.uv.y()); problem.AddResidualBlock(f_td, loss_function, para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index], para_Td[0]); if (it_per_id.estimate_flag == 1 && FIX_DEPTH) problem.SetParameterBlockConstant(para_Feature[feature_index]); else if (it_per_id.estimate_flag == 2) { problem.SetParameterUpperBound(para_Feature[feature_index], 0, 2 / DEPTH_MAX_DIST); } } else { ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j); problem.AddResidualBlock(f, loss_function, para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index]); if (it_per_id.estimate_flag == 1 && FIX_DEPTH) problem.SetParameterBlockConstant(para_Feature[feature_index]); else if (it_per_id.estimate_flag == 2) { // 防止远处的点深度过小 problem.SetParameterUpperBound(para_Feature[feature_index], 0, 2 / DEPTH_MAX_DIST); } } f_m_cnt++; } } ROS_DEBUG("visual measurement count: %d", f_m_cnt); ROS_DEBUG("prepare for ceres: %f", t_prepare.toc()); //添加闭环检测残差，计算滑动窗口中与每一个闭环关键帧的相对位姿，这个相对位置是为后面的图优化准备 if (relocalization_info) { // printf("set relocalization factor! \n"); ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization(); problem.AddParameterBlock(relo_Pose, SIZE_POSE, local_parameterization); int retrive_feature_index = 0; int feature_index = -1; for (auto &it_per_id : f_manager.feature) { if (it_per_id.is_dynamic) { continue; } it_per_id.used_num = it_per_id.feature_per_frame.size(); if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2)) continue; // if (it_per_id.used_num < 4) // continue; ++feature_index; int start = it_per_id.start_frame; if (start <= relo_frame_local_index) { while ((int)match_points[retrive_feature_index].z() < it_per_id.feature_id) { retrive_feature_index++; } if ((int)match_points[retrive_feature_index].z() == it_per_id.feature_id) { Vector3d pts_j = Vector3d(match_points[retrive_feature_index].x(), match_points[retrive_feature_index].y(), 1.0); Vector3d pts_i = it_per_id.feature_per_frame[0].point; ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j); problem.AddResidualBlock(f, loss_function, para_Pose[start], relo_Pose, para_Ex_Pose[0], para_Feature[feature_index]); retrive_feature_index++; } } } } ceres::Solver::Options options; options.linear_solver_type = ceres::DENSE_SCHUR; // options.num_threads = 4; options.trust_region_strategy_type = ceres::DOGLEG; options.max_num_iterations = NUM_ITERATIONS; // options.use_explicit_schur_complement = true; // options.minimizer_progress_to_stdout = false; // options.use_nonmonotonic_steps = true; if (marginalization_flag == MARGIN_OLD) options.max_solver_time_in_seconds = SOLVER_TIME * 4.0 / 5.0; else options.max_solver_time_in_seconds = SOLVER_TIME; TicToc t_solver; ceres::Solver::Summary summary; ceres::Solve(options, &problem, &summary); // cout << summary.BriefReport() << endl; ROS_DEBUG("Iterations : %d", static_cast(summary.iterations.size())); // 防止优化结果在零空间变化，通过固定第一帧的位姿 double2vector(); if (frame_count < WINDOW_SIZE) return; TicToc t_whole_marginalization; //边缘化处理 //如果次新帧是关键帧，将边缘化最老帧，及其看到的路标点和IMU数据，将其转化为先验： if (marginalization_flag == MARGIN_OLD) { MarginalizationInfo *marginalization_info = new MarginalizationInfo(); vector2double(); // 先验部分，基于先验残差，边缘化滑窗中第0帧时刻的状态向量 if (last_marginalization_info) { vector drop_set; for (int i = 0; i < static_cast(last_marginalization_parameter_blocks.size()); i++) { if (last_marginalization_parameter_blocks[i] == para_Pose[0] || last_marginalization_parameter_blocks[i] == para_SpeedBias[0]) drop_set.push_back(i); } // construct new marginlization_factor MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info); ResidualBlockInfo *residual_block_info = new ResidualBlockInfo( marginalization_factor, NULL, last_marginalization_parameter_blocks, drop_set); marginalization_info->addResidualBlockInfo(residual_block_info); } if (USE_IMU) { // imu // 预积分部分，基于第0帧与第1帧之间的预积分残差，边缘化第0帧状态向量 if (pre_integrations[1]->sum_dt < 10.0) { IMUFactor *imu_factor = new IMUFactor(pre_integrations[1]); ResidualBlockInfo *residual_block_info = new ResidualBlockInfo( imu_factor, NULL, vector{para_Pose[0], para_SpeedBias[0], para_Pose[1], para_SpeedBias[1]}, vector{0, 1}); //边缘化 para_Pose[0], para_SpeedBias[0] marginalization_info->addResidualBlockInfo(residual_block_info); } } //图像部分，基于与第0帧相关的图像残差，边缘化第一次观测的图像帧为第0帧的路标点和第0帧 { int feature_index = -1; for (auto &it_per_id : f_manager.feature) { if (it_per_id.is_dynamic) continue; it_per_id.used_num = it_per_id.feature_per_frame.size(); // if (it_per_id.used_num < 4) // continue; if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2)) continue; ++feature_index; int imu_i = it_per_id.start_frame, imu_j = imu_i - 1; if (imu_i != 0) //仅处理第一次观测的图像帧为第0帧的情形 continue; Vector3d pts_i = it_per_id.feature_per_frame[0].point; for (auto &it_per_frame : it_per_id.feature_per_frame) //对观测到路标点的图像帧的遍历 { imu_j++; if (imu_i == imu_j) continue; Vector3d pts_j = it_per_frame.point; if (ESTIMATE_TD) { ProjectionTdFactor *f_td = new ProjectionTdFactor( pts_i, pts_j, it_per_id.feature_per_frame[0].velocity, it_per_frame.velocity, it_per_id.feature_per_frame[0].cur_td, it_per_frame.cur_td, it_per_id.feature_per_frame[0].uv.y(), it_per_frame.uv.y()); ResidualBlockInfo *residual_block_info = new ResidualBlockInfo( f_td, loss_function, vector{para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index], para_Td[0]}, vector{0, 3}); marginalization_info->addResidualBlockInfo(residual_block_info); } else { ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j); ResidualBlockInfo *residual_block_info = new ResidualBlockInfo( f, loss_function, vector{para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index]}, vector{0, 3}); marginalization_info->addResidualBlockInfo(residual_block_info); } } } } TicToc t_pre_margin; marginalization_info->preMarginalize(); ROS_DEBUG("pre marginalization %f ms", t_pre_margin.toc()); TicToc t_margin; marginalization_info->marginalize(); ROS_DEBUG("marginalization %f ms", t_margin.toc()); //仅仅改变滑窗double部分地址映射，具体值的通过slideWindow和vector2double函数完成；记住边缘化仅仅改变A和b，不改变状态向量 //由于第0帧观测到的路标点全被边缘化，即边缘化后保存的状态向量中没有路标点;因此addr_shift无需添加路标点 std::unordered_map addr_shift; for (int i = 1; i <= WINDOW_SIZE; i++) //最老图像帧数据丢弃，从i=1开始遍历 { addr_shift[reinterpret_cast(para_Pose[i])] = para_Pose[i - 1]; // i数据保存到1-1指向的地址，滑窗向前移动一格 if (USE_IMU) addr_shift[reinterpret_cast(para_SpeedBias[i])] = para_SpeedBias[i - 1]; } for (auto &i : para_Ex_Pose) addr_shift[reinterpret_cast(i)] = i; if (ESTIMATE_TD) { addr_shift[reinterpret_cast(para_Td[0])] = para_Td[0]; } vector parameter_blocks = marginalization_info->getParameterBlocks(addr_shift); delete last_marginalization_info; last_marginalization_info = marginalization_info; last_marginalization_parameter_blocks = parameter_blocks; } else //将次新的图像帧数据边缘化； tzhang { if (last_marginalization_info && std::count(std::begin(last_marginalization_parameter_blocks), std::end(last_marginalization_parameter_blocks), para_Pose[WINDOW_SIZE - 1])) { MarginalizationInfo *marginalization_info = new MarginalizationInfo(); vector2double(); if (last_marginalization_info) { vector drop_set; //记录需要丢弃的变量在last_marginalization_parameter_blocks中的索引 for (int i = 0; i < static_cast(last_marginalization_parameter_blocks.size()); i++) { ROS_ASSERT(last_marginalization_parameter_blocks[i] != para_SpeedBias[WINDOW_SIZE - 1]); if (last_marginalization_parameter_blocks[i] == para_Pose[WINDOW_SIZE - 1]) drop_set.push_back(i); } // construct new marginlization_factor MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info); ResidualBlockInfo *residual_block_info = new ResidualBlockInfo( marginalization_factor, NULL, last_marginalization_parameter_blocks, drop_set); marginalization_info->addResidualBlockInfo(residual_block_info); } TicToc t_pre_margin; // ROS_DEBUG("begin marginalization"); marginalization_info->preMarginalize(); ROS_DEBUG("end pre marginalization, %f ms", t_pre_margin.toc()); TicToc t_margin; // ROS_DEBUG("begin marginalization"); marginalization_info->marginalize(); ROS_DEBUG("end marginalization, %f ms", t_margin.toc()); std::unordered_map addr_shift; for (int i = 0; i <= WINDOW_SIZE; i++) { if (i == WINDOW_SIZE - 1) // WINDOW_SIZE - 1会被边缘化，不保存 continue; else if (i == WINDOW_SIZE) // WINDOW_SIZE数据保存到WINDOW_SIZE-1指向的地址 { addr_shift[reinterpret_cast(para_Pose[i])] = para_Pose[i - 1]; addr_shift[reinterpret_cast(para_SpeedBias[i])] = para_SpeedBias[i - 1]; } else { addr_shift[reinterpret_cast(para_Pose[i])] = para_Pose[i]; addr_shift[reinterpret_cast(para_SpeedBias[i])] = para_SpeedBias[i]; } } for (auto &i : para_Ex_Pose) addr_shift[reinterpret_cast(i)] = i; if (ESTIMATE_TD) { addr_shift[reinterpret_cast(para_Td[0])] = para_Td[0]; } vector parameter_blocks = marginalization_info->getParameterBlocks(addr_shift); delete last_marginalization_info; last_marginalization_info = marginalization_info; last_marginalization_parameter_blocks = parameter_blocks; } } ROS_DEBUG("whole marginalization costs: %f", t_whole_marginalization.toc()); ROS_DEBUG("whole time for ceres: %f", t_whole.toc()); } void Estimator::slideWindow() { TicToc t_margin; if (marginalization_flag == MARGIN_OLD) // 边缘化最老的图像帧，即次新的图像帧为关键帧 { double t_0 = Headers[0]; back_R0 = Rs[0]; back_P0 = Ps[0]; if (frame_count == WINDOW_SIZE) { // 1、滑窗中的数据往前移动一帧；运行结果就是WINDOW_SIZE位置的状态为之前0位置对应的状态 // 0,1,2...WINDOW_SIZE——>1,2...WINDOW_SIZE,0 for (int i = 0; i < WINDOW_SIZE; i++) { Headers[i] = Headers[i + 1]; Ps[i].swap(Ps[i + 1]); Rs[i].swap(Rs[i + 1]); if (USE_IMU) { std::swap(pre_integrations[i], pre_integrations[i + 1]); dt_buf[i].swap(dt_buf[i + 1]); linear_acceleration_buf[i].swap(linear_acceleration_buf[i + 1]); angular_velocity_buf[i].swap(angular_velocity_buf[i + 1]); Vs[i].swap(Vs[i + 1]); Bas[i].swap(Bas[i + 1]); Bgs[i].swap(Bgs[i + 1]); } ++find_solved[i + 1]; find_solved[i] = find_solved[i + 1]; } // 2、处理前，WINDOW_SIZE位置的状态为之前0位置对应的状态；处理后，WINDOW_SIZE位置的状态为之前WINDOW_SIZE位置对应的状态;之前0位置对应的状态被剔除 // 0,1,2...WINDOW_SIZE——>1,2...WINDOW_SIZE,WINDOW_SIZE Headers[WINDOW_SIZE] = Headers[WINDOW_SIZE - 1]; Ps[WINDOW_SIZE] = Ps[WINDOW_SIZE - 1]; Rs[WINDOW_SIZE] = Rs[WINDOW_SIZE - 1]; if (USE_IMU) { Vs[WINDOW_SIZE] = Vs[WINDOW_SIZE - 1]; Bas[WINDOW_SIZE] = Bas[WINDOW_SIZE - 1]; Bgs[WINDOW_SIZE] = Bgs[WINDOW_SIZE - 1]; delete pre_integrations[WINDOW_SIZE]; pre_integrations[WINDOW_SIZE] = new IntegrationBase{acc_0, gyr_0, Bas[WINDOW_SIZE], Bgs[WINDOW_SIZE]}; dt_buf[WINDOW_SIZE].clear(); linear_acceleration_buf[WINDOW_SIZE].clear(); angular_velocity_buf[WINDOW_SIZE].clear(); } find_solved[WINDOW_SIZE] = 0; // 3、对时刻t_0(对应滑窗第0帧)之前的所有数据进行剔除；即all_image_frame中仅保留滑窗中图像帧0与图像帧WINDOW_SIZE之间的数据 map::iterator it_0; it_0 = all_image_frame.find(t_0); delete it_0->second.pre_integration; it_0->second.pre_integration = nullptr; for (auto it = all_image_frame.begin(); it != it_0; ++it) { delete it->second.pre_integration; it->second.pre_integration = NULL; } all_image_frame.erase(all_image_frame.begin(), it_0); all_image_frame.erase(t_0); slideWindowOld(); } } else //边缘化次新的图像帧，主要完成的工作是数据衔接 tzhang { // 0,1,2...WINDOW_SIZE-2, WINDOW_SIZE-1, // WINDOW_SIZE——>0,,1,2...WINDOW_SIZE-2,WINDOW_SIZE, WINDOW_SIZE if (frame_count == WINDOW_SIZE) { Headers[frame_count - 1] = Headers[frame_count]; Ps[frame_count - 1] = Ps[frame_count]; Rs[frame_count - 1] = Rs[frame_count]; find_solved[WINDOW_SIZE] = 0; if (USE_IMU) { for (unsigned int i = 0; i < dt_buf[frame_count].size(); i++) { double tmp_dt = dt_buf[frame_count][i]; Vector3d tmp_linear_acceleration = linear_acceleration_buf[frame_count][i]; Vector3d tmp_angular_velocity = angular_velocity_buf[frame_count][i]; pre_integrations[frame_count - 1]->push_back(tmp_dt, tmp_linear_acceleration, tmp_angular_velocity); dt_buf[frame_count - 1].push_back(tmp_dt); linear_acceleration_buf[frame_count - 1].push_back(tmp_linear_acceleration); angular_velocity_buf[frame_count - 1].push_back(tmp_angular_velocity); } Vs[frame_count - 1] = Vs[frame_count]; Bas[frame_count - 1] = Bas[frame_count]; Bgs[frame_count - 1] = Bgs[frame_count]; delete pre_integrations[WINDOW_SIZE]; pre_integrations[WINDOW_SIZE] = new IntegrationBase{acc_0, gyr_0, Bas[WINDOW_SIZE], Bgs[WINDOW_SIZE]}; dt_buf[WINDOW_SIZE].clear(); linear_acceleration_buf[WINDOW_SIZE].clear(); angular_velocity_buf[WINDOW_SIZE].clear(); } slideWindowNew(); } } } // real marginalization is removed in solve_ceres() void Estimator::slideWindowNew() { sum_of_front++; f_manager.removeFront(frame_count); } // real marginalization is removed in solve_ceres() void Estimator::slideWindowOld() { sum_of_back++; bool shift_depth = solver_flag == NON_LINEAR ? true : false; if (shift_depth) { Matrix3d R0, R1; Vector3d P0, P1; R0 = back_R0 * ric[0]; R1 = Rs[0] * ric[0]; P0 = back_P0 + back_R0 * tic[0]; P1 = Ps[0] + Rs[0] * tic[0]; f_manager.removeBackShiftDepth(R0, P0, R1, P1); } else f_manager.removeBack(); } /** * @brief 进行重定位 * @optional * @param[in] _frame_stamp 重定位帧时间戳 * @param[in] _frame_index 重定位帧索引值 * @param[in] _match_points 重定位帧的所有匹配点 * @param[in] _relo_t 重定位帧平移向量 * @param[in] _relo_r 重定位帧旋转矩阵 * @return void */ void Estimator::setReloFrame(double _frame_stamp, int _frame_index, vector &_match_points, Vector3d _relo_t, Matrix3d _relo_r) { relo_frame_stamp = _frame_stamp; relo_frame_index = _frame_index; match_points.clear(); match_points = _match_points; prev_relo_t = std::move(_relo_t); prev_relo_r = std::move(_relo_r); for (int i = 0; i < WINDOW_SIZE; i++) { if (relo_frame_stamp == Headers[i]) { relo_frame_local_index = i; relocalization_info = true; for (int j = 0; j < SIZE_POSE; j++) relo_Pose[j] = para_Pose[i][j]; } } } void Estimator::inputIMU(double t, const Vector3d &linearAcceleration, const Vector3d &angularVelocity) { m_imu.lock(); imu_buf.push(make_pair(t, make_pair(linearAcceleration, angularVelocity))); // imu_predict_buf.push( // make_pair(t, make_pair(linearAcceleration, angularVelocity))); m_imu.unlock(); if (solver_flag == Estimator::SolverFlag::NON_LINEAR) { // predict imu (no residual error) m_propagate.lock(); predict(t, linearAcceleration, angularVelocity); m_propagate.unlock(); pubLatestOdometry(latest_P, latest_Q, latest_V, t); } } void Estimator::updateLatestStates() { m_propagate.lock(); latest_time = Headers[frame_count] + td; latest_P = Ps[frame_count]; latest_Q = Rs[frame_count]; latest_V = Vs[frame_count]; latest_Ba = Bas[frame_count]; latest_Bg = Bgs[frame_count]; if (USE_IMU) { m_imu.lock(); queue>> tmp_imu_buf = imu_buf; for (sensor_msgs::ImuConstPtr tmp_imu_msg; !tmp_imu_buf.empty(); tmp_imu_buf.pop()) predict(tmp_imu_buf.front().first, imu_buf.front().second.first, imu_buf.front().second.second); m_imu.unlock(); } m_propagate.unlock(); } Matrix3d Estimator::predictMotion(double t0, double t1) { Matrix3d relative_R = Matrix3d::Identity(); if (imu_buf.empty()) return relative_R; bool first_imu = true; double prev_imu_time; Eigen::Vector3d prev_gyr; m_imu.lock(); queue>> imu_predict_buf = imu_buf; m_imu.unlock(); if (t1 <= imu_predict_buf.back().first) { while (imu_predict_buf.front().first <= t0) { imu_predict_buf.pop(); } while (imu_predict_buf.front().first <= t1 && !imu_predict_buf.empty()) { double t = imu_predict_buf.front().first; Eigen::Vector3d angular_velocity = imu_predict_buf.front().second.second; // Eigen::Vector3d linear_acceleration{ // imu_predict_buf.front()->linear_acceleration.x, // imu_predict_buf.front()->linear_acceleration.y, // imu_predict_buf.front()->linear_acceleration.z}; imu_predict_buf.pop(); if (first_imu) { prev_imu_time = t; first_imu = false; prev_gyr = angular_velocity; continue; } double dt = t - prev_imu_time; prev_imu_time = t; // Eigen::Vector3d un_acc_0 = tmp_Q * (acc_0 - tmp_Ba) - estimator.g; Eigen::Vector3d un_gyr = 0.5 * (prev_gyr + angular_velocity) - latest_Bg; // tmp_Q = tmp_Q * Utility::deltaQ(un_gyr * dt); // Eigen::Vector3d un_acc_1 = // tmp_Q * (linear_acceleration - tmp_Ba) - estimator.g; // un_acc = 0.5 * (un_acc_0 + un_acc_1); // tmp_P = tmp_P + dt * tmp_V + 0.5 * dt * dt * un_acc; // tmp_V = tmp_V + dt * un_acc; // acc_0 = linear_acceleration; prev_gyr = angular_velocity; // cl // Transform the mean angular velocity from the IMU // frame to the cam0 frames. // Compute the relative rotation. Vector3d cam0_angle_axisd = RIC.back().transpose() * un_gyr * dt; relative_R *= AngleAxisd(cam0_angle_axisd.norm(), cam0_angle_axisd.normalized()) .toRotationMatrix() .transpose(); } } return relative_R; } void Estimator::predict(double t, const Vector3d &linearAcceleration, const Vector3d &angularVelocity) { if (init_imu) { latest_time = t; init_imu = false; return; } double dt = t - latest_time; latest_time = t; Eigen::Vector3d un_acc_0 = latest_Q * (acc_0 - latest_Ba) - g; Eigen::Vector3d un_gyr = 0.5 * (gyr_0 + angularVelocity) - latest_Bg; latest_Q = latest_Q * Utility::deltaQ(un_gyr * dt); Eigen::Vector3d un_acc_1 = latest_Q * (linearAcceleration - latest_Ba) - g; Eigen::Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1); latest_P = latest_P + dt * latest_V + 0.5 * dt * dt * un_acc; latest_V = latest_V + dt * un_acc; } bool Estimator::IMUAvailable(double t) { if (!imu_buf.empty() && t <= imu_buf.back().first) return true; else return false; } void Estimator::initFirstIMUPose( std::vector>> &imu_vector) { printf("init first imu pose\n"); initFirstPoseFlag = true; // return; Eigen::Vector3d averAcc(0, 0, 0); int n = (int)imu_vector.size(); for (auto &i : imu_vector) { averAcc = averAcc + i.second.first; } averAcc = averAcc / n; printf("averge acc %f %f %f\n", averAcc.x(), averAcc.y(), averAcc.z()); Matrix3d R0 = Utility::g2R(averAcc); double yaw = Utility::R2ypr(R0).x(); R0 = Utility::ypr2R(Eigen::Vector3d{-yaw, 0, 0}) * R0; Rs[0] = R0; cout << "init R0 " << endl << Rs[0] << endl; } bool Estimator::getIMUInterval( double t0, double t1, std::vector>> &imu_vector) { m_imu.lock(); if (imu_buf.empty()) { printf("not receive imu\n"); m_imu.unlock(); return false; } if (t1 <= imu_buf.back().first) { while (imu_buf.front().first <= t0) { imu_buf.pop(); } while (imu_buf.front().first < t1) { imu_vector.emplace_back(std::move(imu_buf.front())); imu_buf.pop(); } imu_vector.emplace_back(imu_buf.front()); m_imu.unlock(); return true; } else { printf("wait for imu\n"); m_imu.unlock(); return false; } } double Estimator::reprojectionError(Matrix3d &Ri, Vector3d &Pi, Matrix3d &rici, Vector3d &tici, Matrix3d &Rj, Vector3d &Pj, Matrix3d &ricj, Vector3d &ticj, double depth, Vector3d &uvi, Vector3d &uvj) { Vector3d pts_w = Ri * (rici * (depth * uvi) + tici) + Pi; Vector3d pts_cj = ricj.transpose() * (Rj.transpose() * (pts_w - Pj) - ticj); Vector2d residual = (pts_cj / pts_cj.z()).head<2>() - uvj.head<2>(); double rx = residual.x(); double ry = residual.y(); return sqrt(rx * rx + ry * ry); } double Estimator::reprojectionError3D(Matrix3d &Ri, Vector3d &Pi, Matrix3d &rici, Vector3d &tici, Matrix3d &Rj, Vector3d &Pj, Matrix3d &ricj, Vector3d &ticj, double depth, Vector3d &uvi, Vector3d &uvj) { Vector3d pts_w = Ri * (rici * (depth * uvi) + tici) + Pi; Vector3d pts_cj = ricj.transpose() * (Rj.transpose() * (pts_w - Pj) - ticj); return (pts_cj - uvj).norm() / depth; } void Estimator::movingConsistencyCheck(set &removeIndex) { for (auto &it_per_id : f_manager.feature) { it_per_id.used_num = it_per_id.feature_per_frame.size(); if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2)) continue; double depth = it_per_id.estimated_depth; if (depth < 0) continue; double err = 0; double err3D = 0; int errCnt = 0; int imu_i = it_per_id.start_frame, imu_j = imu_i - 1; Vector3d pts_i = it_per_id.feature_per_frame[0].point; for (auto &it_per_frame : it_per_id.feature_per_frame) { imu_j++; if (imu_i != imu_j) { Vector3d pts_j = it_per_frame.point; err += reprojectionError(Rs[imu_i], Ps[imu_i], ric[0], tic[0], Rs[imu_j], Ps[imu_j], ric[0], tic[0], depth, pts_i, pts_j); // for bleeding points err3D += reprojectionError3D(Rs[imu_i], Ps[imu_i], ric[0], tic[0], Rs[imu_j], Ps[imu_j], ric[0], tic[0], depth, pts_i, pts_j); errCnt++; } } if (errCnt > 0) { if (FOCAL_LENGTH * err / errCnt > 10 || err3D / errCnt > 2.0) { removeIndex.insert(it_per_id.feature_id); it_per_id.is_dynamic = true; } else { it_per_id.is_dynamic = false; } } } } ================================================ FILE: vins_estimator/src/estimator/estimator.h ================================================ #pragma once #include #include #include "../feature_manager/feature_manager.h" #include "../initial/initial_alignment.h" #include "../initial/initial_ex_rotation.h" #include "../initial/initial_sfm.h" #include "../initial/solve_5pts.h" #include "../utility/parameters.h" #include "../utility/tic_toc.h" #include "../utility/utility.h" #include "../feature_tracker/feature_tracker.h" #include #include #include #include "../factor/imu_factor.h" #include "../factor/marginalization_factor.h" #include "../factor/pose_local_parameterization.h" #include "../factor/projection_factor.h" #include "../factor/projection_td_factor.h" #include #include #include #include #include #include class Estimator { public: Estimator(); void setParameter(); // interface void processIMU(double t, const Vector3d &linear_acceleration, const Vector3d &angular_velocity); void processImage(map> &image, const std_msgs::Header &header); void setReloFrame(double _frame_stamp, int _frame_index, vector &_match_points, Vector3d _relo_t, Matrix3d _relo_r); // internal void clearState(); bool initialStructure(); bool visualInitialAlign(); bool visualInitialAlignWithDepth(); bool relativePose(Matrix3d &relative_R, Vector3d &relative_T, int &l); void slideWindow(); void slideWindowNew(); void slideWindowOld(); void solveOdometry(); void optimization(); void vector2double(); void double2vector(); bool failureDetection(); bool staticInitialAlignWithDepth(); void updateLatestStates(); Matrix3d predictMotion(double t0, double t1); void inputIMU(double t, const Vector3d &linearAcceleration, const Vector3d &angularVelocity); void predict(double t, const Vector3d &linearAcceleration, const Vector3d &angularVelocity); bool IMUAvailable(double t); bool initialStructureWithDepth(); void movingConsistencyCheck(set &removeIndex); double reprojectionError(Matrix3d &Ri, Vector3d &Pi, Matrix3d &rici, Vector3d &tici, Matrix3d &Rj, Vector3d &Pj, Matrix3d &ricj, Vector3d &ticj, double depth, Vector3d &uvi, Vector3d &uvj); double reprojectionError3D(Matrix3d &Ri, Vector3d &Pi, Matrix3d &rici, Vector3d &tici, Matrix3d &Rj, Vector3d &Pj, Matrix3d &ricj, Vector3d &ticj, double depth, Vector3d &uvi, Vector3d &uvj); bool getIMUInterval(double t0, double t1, std::vector>> &imu_vector); void initFirstIMUPose(std::vector>> &imu_vector); enum SolverFlag { INITIAL, NON_LINEAR }; enum MarginalizationFlag { MARGIN_OLD = 0, MARGIN_SECOND_NEW = 1 }; bool openExEstimation; FeatureTracker featureTracker; SolverFlag solver_flag; MarginalizationFlag marginalization_flag; Vector3d g; // extrinsic Matrix3d ric[NUM_OF_CAM]; Vector3d tic[NUM_OF_CAM]; // VIO state vector Vector3d Ps[(WINDOW_SIZE + 1)]; Vector3d Vs[(WINDOW_SIZE + 1)]; Matrix3d Rs[(WINDOW_SIZE + 1)]; Vector3d Bas[(WINDOW_SIZE + 1)]; Vector3d Bgs[(WINDOW_SIZE + 1)]; double td{}; Matrix3d back_R0, last_R, last_R0; Vector3d back_P0, last_P, last_P0; double Headers[(WINDOW_SIZE + 1)]; IntegrationBase *pre_integrations[(WINDOW_SIZE + 1)]{}; Vector3d acc_0, gyr_0; vector dt_buf[(WINDOW_SIZE + 1)]; vector linear_acceleration_buf[(WINDOW_SIZE + 1)]; vector angular_velocity_buf[(WINDOW_SIZE + 1)]; int frame_count{}; // cl:滑动窗口中帧的数目,最大为滑窗大小 int sum_of_back{}, sum_of_front{}, sum_of_invalid{}; FeatureManager f_manager; MotionEstimator m_estimator; InitialEXRotation initial_ex_rotation; bool first_imu{}; bool is_valid{}; bool failure_occur{}; vector key_poses; double initial_timestamp{}; double para_Pose[WINDOW_SIZE + 1][SIZE_POSE]{}; double para_SpeedBias[WINDOW_SIZE + 1][SIZE_SPEEDBIAS]{}; double para_Feature[NUM_OF_F][SIZE_FEATURE]{}; double para_Ex_Pose[NUM_OF_CAM][SIZE_POSE]{}; double para_Td[1][1]{}; int find_solved[WINDOW_SIZE + 1]{}; MarginalizationInfo *last_marginalization_info{}; vector last_marginalization_parameter_blocks; map all_image_frame; IntegrationBase *tmp_pre_integration{}; // relocalization variable bool relocalization_info{}; double relo_frame_stamp{}; double relo_frame_index{}; int relo_frame_local_index{}; vector match_points; double relo_Pose[SIZE_POSE]{}; Matrix3d drift_correct_r; Vector3d drift_correct_t; Vector3d prev_relo_t; Matrix3d prev_relo_r; Vector3d relo_relative_t; Quaterniond relo_relative_q; double relo_relative_yaw{}; std::mutex m_imu, m_propagate; bool init_imu{}; double prevTime = -1; queue>> imu_buf; double latest_time{}; Eigen::Vector3d latest_P; Eigen::Quaterniond latest_Q; Eigen::Vector3d latest_V; Eigen::Vector3d latest_Ba; Eigen::Vector3d latest_Bg; bool initFirstPoseFlag{}; }; ================================================ FILE: vins_estimator/src/estimator_nodelet.cpp ================================================ #include #include #include #include #include #include #include #include #include #include #include #include "estimator/estimator.h" #include "feature_tracker/feature_tracker.h" #include "ros/console_backend.h" #include "sensor_msgs/image_encodings.h" #include "utility/parameters.h" #include "utility/tic_toc.h" #include "utility/visualization.h" #include // 基类Nodelet所在的头文件 #include namespace estimator_nodelet_ns { class EstimatorNodelet : public nodelet::Nodelet //任何nodelet plugin都要继承Nodelet类。 { public: EstimatorNodelet() = default; private: void onInit() override { ros::NodeHandle &pn = getPrivateNodeHandle(); ros::NodeHandle &nh = getMTNodeHandle(); ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info); readParameters(pn); estimator.setParameter(); #ifdef EIGEN_DONT_PARALLELIZE ROS_DEBUG("EIGEN_DONT_PARALLELIZE"); #endif ROS_WARN("waiting for image, semantic and imu..."); registerPub(nh); sub_image = nh.subscribe(IMAGE_TOPIC, 1000, &EstimatorNodelet::image_callback, this); sub_depth = nh.subscribe(DEPTH_TOPIC, 1000, &EstimatorNodelet::depth_callback, this); if (USE_IMU) sub_imu = nh.subscribe(IMU_TOPIC, 1000, &EstimatorNodelet::imu_callback, this, ros::TransportHints().tcpNoDelay()); // topic from pose_graph, notify if there's relocalization sub_relo_points = nh.subscribe("/pose_graph/match_points", 10, &EstimatorNodelet::relocalization_callback, this); dura = std::chrono::milliseconds(2); trackThread = std::thread(&EstimatorNodelet::process_tracker, this); processThread = std::thread(&EstimatorNodelet::process, this); } Estimator estimator; // thread relevance std::thread trackThread, processThread; std::chrono::milliseconds dura; std::condition_variable con_tracker; std::condition_variable con_estimator; std::mutex m_feature; std::mutex m_backend; std::mutex m_buf; std::mutex m_vis; // ROS and data buf relevance ros::Subscriber sub_imu, sub_relo_points, sub_image, sub_depth; queue img_buf; queue depth_buf; queue, map>>> feature_buf; queue relo_buf; queue> vis_img_buf; bool init_feature = false; bool init_pub = false; // frequency control relevance bool first_image_flag = true; double first_image_time = 0; double last_image_time = 0; int pub_count = 1; int input_count = 0; double last_imu_t = 0; void imu_callback(const sensor_msgs::ImuConstPtr &imu_msg) { /** * @brief nullptr may cause crash * typename boost::detail::sp_member_access::type = const * sensor_msgs::Imu_ >*]: Assertion `px != 0' failed. * https://github.com/mavlink/mavros/issues/432 * https://github.com/mavlink/mavros/pull/434 */ if (imu_msg) { if (imu_msg->header.stamp.toSec() <= last_imu_t) { ROS_WARN("imu message in disorder! %f", imu_msg->header.stamp.toSec()); return; } last_imu_t = imu_msg->header.stamp.toSec(); Vector3d acc(imu_msg->linear_acceleration.x, imu_msg->linear_acceleration.y, imu_msg->linear_acceleration.z); Vector3d gyr(imu_msg->angular_velocity.x, imu_msg->angular_velocity.y, imu_msg->angular_velocity.z); estimator.inputIMU(last_imu_t, acc, gyr); } } void image_callback(const sensor_msgs::ImageConstPtr &color_msg) { m_buf.lock(); img_buf.emplace(color_msg); m_buf.unlock(); con_tracker.notify_one(); } void depth_callback(const sensor_msgs::ImageConstPtr &depth_msg) { m_buf.lock(); depth_buf.emplace(depth_msg); m_buf.unlock(); con_tracker.notify_one(); } void relocalization_callback(const sensor_msgs::PointCloudConstPtr &points_msg) { m_buf.lock(); relo_buf.push(points_msg); m_buf.unlock(); } void visualizeFeatureFilter(const map> &features, double feature_time) { cv::Mat vis_img; m_vis.lock(); while (!vis_img_buf.empty()) { if (vis_img_buf.front().first.stamp.toSec() == feature_time) { vis_img = vis_img_buf.front().second; vis_img_buf.pop(); break; } else if (vis_img_buf.front().first.stamp.toSec() < feature_time) { vis_img_buf.pop(); } else { m_vis.unlock(); return; } } m_vis.unlock(); // Show image with tracked points in rviz (by topic pub_match) for (auto &feature : features) { cv::circle(vis_img, cv::Point(feature.second[3], feature.second[4]), 5, cv::Scalar(0, 255, 255), 2); } pubTrackImg(vis_img); // cv::imshow("grids_detector_img", // estimator.featureTracker.grids_detector_img); // cv::moveWindow("grids_detector_img", 0, 0); // cv::waitKey(1); // cv::imshow("feature_img", vis_img); // cv::moveWindow("feature_img", 0, 0); // cv::waitKey(1); } // thread: feature tracker [[noreturn]] void process_tracker() { while (1) { { sensor_msgs::ImageConstPtr color_msg = nullptr; sensor_msgs::ImageConstPtr depth_msg = nullptr; std::unique_lock locker(m_buf); while (img_buf.empty() || depth_buf.empty()) { con_tracker.wait(locker); } double time_color = img_buf.front()->header.stamp.toSec(); double time_depth = depth_buf.front()->header.stamp.toSec(); if (time_color < time_depth - 0.003) { img_buf.pop(); ROS_DEBUG("throw color\n"); } else if (time_color > time_depth + 0.003) { depth_buf.pop(); ROS_DEBUG("throw depth\n"); } else { color_msg = img_buf.front(); img_buf.pop(); depth_msg = depth_buf.front(); depth_buf.pop(); } locker.unlock(); if (color_msg == nullptr || depth_msg == nullptr) { ROS_DEBUG("time_color = %f, time_depth = %f\n", time_color, time_depth); continue; } if (first_image_flag) { first_image_flag = false; first_image_time = time_color; last_image_time = time_color; continue; } // detect unstable camera stream if (time_color - last_image_time > 1.0 || time_color < last_image_time) { ROS_WARN("image discontinue! reset the feature tracker!"); first_image_flag = true; last_image_time = 0; pub_count = 1; ROS_WARN("restart the estimator!"); m_feature.lock(); while (!feature_buf.empty()) feature_buf.pop(); m_feature.unlock(); m_backend.lock(); estimator.clearState(); estimator.setParameter(); m_backend.unlock(); last_imu_t = 0; continue; } // frequency control if (round(1.0 * input_count / (time_color - first_image_time)) > FRONTEND_FREQ) { ROS_DEBUG("Skip this frame.%f", 1.0 * input_count / (time_color - first_image_time)); continue; } ++input_count; // frequency control if (round(1.0 * pub_count / (time_color - first_image_time)) <= FREQ) { PUB_THIS_FRAME = true; // reset the frequency control if (abs(1.0 * pub_count / (time_color - first_image_time) - FREQ) < 0.01 * FREQ) { first_image_time = time_color; pub_count = 0; input_count = 0; } } else PUB_THIS_FRAME = false; TicToc t_r; // encodings in ros: // http://docs.ros.org/diamondback/api/sensor_msgs/html/image__encodings_8cpp_source.html // color has encoding RGB8 cv_bridge::CvImageConstPtr ptr; if (color_msg->encoding == "8UC1") // shan:why 8UC1 need this operation? Find // answer:https://github.com/ros-perception/vision_opencv/issues/175 { sensor_msgs::Image img; img.header = color_msg->header; img.height = color_msg->height; img.width = color_msg->width; img.is_bigendian = color_msg->is_bigendian; img.step = color_msg->step; img.data = color_msg->data; img.encoding = "mono8"; ptr = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::MONO8); } else ptr = cv_bridge::toCvCopy(color_msg, sensor_msgs::image_encodings::MONO8); if (USE_IMU) { Matrix3d &&relative_R = estimator.predictMotion(last_image_time, time_color + estimator.td); estimator.featureTracker.readImage(ptr->image, time_color, relative_R); } else estimator.featureTracker.readImage(ptr->image, time_color); last_image_time = time_color; // always 0 // update all id in ids[] // If has ids[i] == -1 (newly added pts by cv::goodFeaturesToTrack), // substitute by gloabl id counter (n_id) for (unsigned int i = 0;; i++) { bool completed = false; completed |= estimator.featureTracker.updateID(i); if (!completed) break; } if (PUB_THIS_FRAME) { pub_count++; std_msgs::Header feature_header = color_msg->header; map> image; auto &un_pts = estimator.featureTracker.cur_un_pts; auto &cur_pts = estimator.featureTracker.cur_pts; auto &ids = estimator.featureTracker.ids; auto &pts_velocity = estimator.featureTracker.pts_velocity; for (unsigned int j = 0; j < ids.size(); j++) { if (estimator.featureTracker.track_cnt[j] > 1) { int p_id = ids[j]; geometry_msgs::Point32 p; double x = un_pts[j].x; double y = un_pts[j].y; double z = 1; int v = p_id * NUM_OF_CAM + 0.5; int feature_id = v / NUM_OF_CAM; double p_u = cur_pts[j].x; double p_v = cur_pts[j].y; double velocity_x = pts_velocity[j].x; double velocity_y = pts_velocity[j].y; ROS_ASSERT(z == 1); Eigen::Matrix xyz_uv_velocity; xyz_uv_velocity << x, y, z, p_u, p_v, velocity_x, velocity_y; image[feature_id] = xyz_uv_velocity; } } if (!init_pub) { init_pub = true; } else { if (!init_feature) { // skip the first detected feature, which doesn't contain optical // flow speed init_feature = true; continue; } if (!image.empty()) { m_feature.lock(); feature_buf.push( make_pair(make_pair(feature_header, depth_msg), std::move(image))); m_feature.unlock(); con_estimator.notify_one(); } else { first_image_time = time_color; pub_count = 0; input_count = 0; continue; } } // Show image with tracked points in rviz (by topic pub_match) if (SHOW_TRACK) { cv::Mat show_img = ptr->image; ptr = cv_bridge::cvtColor(ptr, sensor_msgs::image_encodings::BGR8); cv::Mat stereo_img = ptr->image; cv::Mat tmp_img = stereo_img.rowRange(0, ROW); cv::cvtColor(show_img, tmp_img, CV_GRAY2RGB); for (unsigned int j = 0; j < estimator.featureTracker.cur_pts.size(); j++) { if (estimator.featureTracker.track_cnt[j] > 1) { double len = std::min( 1.0, 1.0 * estimator.featureTracker.track_cnt[j] / WINDOW_SIZE); cv::circle(tmp_img, estimator.featureTracker.cur_pts[j], 5, cv::Scalar(255 * (1 - len), 0, 255 * len), -1); // draw speed line // Vector2d tmp_cur_un_pts // (trackerData[i].cur_un_pts[j].x, // trackerData[i].cur_un_pts[j].y); Vector2d // tmp_pts_velocity // (trackerData[i].pts_velocity[j].x, // trackerData[i].pts_velocity[j].y); Vector3d // tmp_prev_un_pts; tmp_prev_un_pts.head(2) = // tmp_cur_un_pts - 0.10 * tmp_pts_velocity; // tmp_prev_un_pts.z() = 1; // Vector2d tmp_prev_uv; // trackerData[i].m_camera->spaceToPlane(tmp_prev_un_pts, // tmp_prev_uv); cv::line(tmp_img, // trackerData[i].cur_pts[j], // cv::Point2f(tmp_prev_uv.x(), // tmp_prev_uv.y()), cv::Scalar(255 , 0, 0), 1 // , 8, 0); // char name[10]; // sprintf(name, "%d", trackerData[i].ids[j]); // cv::putText(tmp_img, name, trackerData[i].cur_pts[j], // cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 0, 0)); } } if (USE_IMU) { for (auto &predict_pt : estimator.featureTracker.predict_pts) { cv::circle(tmp_img, predict_pt, 2, cv::Scalar(0, 255, 0), -1); } } m_vis.lock(); vis_img_buf.push(make_pair(feature_header, tmp_img)); m_vis.unlock(); } } static double whole_process_time = 0; static size_t cnt_frame = 0; ++cnt_frame; double per_process_time = t_r.toc(); whole_process_time += per_process_time; ROS_DEBUG("average feature tracking costs: %f", whole_process_time / cnt_frame); // ROS_DEBUG("feature tracking costs: %f", per_process_time); } std::this_thread::sleep_for(dura); } } // thread: visual-inertial odometry [[noreturn]] void process() { while (true) { std::unique_lock locker(m_feature); while (feature_buf.empty()) { con_estimator.wait(locker); } pair, map>> feature_msg(std::move(feature_buf.front())); feature_buf.pop(); locker.unlock(); TicToc t_backend; m_backend.lock(); // set relocalization frame sensor_msgs::PointCloudConstPtr relo_msg = nullptr; while (!relo_buf.empty()) { relo_msg = relo_buf.front(); relo_buf.pop(); } if (relo_msg != nullptr) { vector match_points; double frame_stamp = relo_msg->header.stamp.toSec(); for (auto point : relo_msg->points) { Vector3d u_v_id; u_v_id.x() = point.x; u_v_id.y() = point.y; u_v_id.z() = point.z; match_points.push_back(u_v_id); } Vector3d relo_t(relo_msg->channels[0].values[0], relo_msg->channels[0].values[1], relo_msg->channels[0].values[2]); Quaterniond relo_q(relo_msg->channels[0].values[3], relo_msg->channels[0].values[4], relo_msg->channels[0].values[5], relo_msg->channels[0].values[6]); Matrix3d relo_r = relo_q.toRotationMatrix(); int frame_index; frame_index = relo_msg->channels[0].values[7]; estimator.setReloFrame(frame_stamp, frame_index, match_points, relo_t, relo_r); } // depth has encoding TYPE_16UC1 cv::Mat depth_img; if (feature_msg.first.second == nullptr) { depth_img = cv::Mat(ROW, COL, CV_16UC1, cv::Scalar(0)); } else { if (feature_msg.first.second->encoding == "mono16" || feature_msg.first.second->encoding == "16UC1") { depth_img = cv_bridge::toCvShare(feature_msg.first.second)->image; } else if (feature_msg.first.second->encoding == "32FC1") { cv::Mat depth_32fc1 = cv_bridge::toCvShare(feature_msg.first.second)->image; depth_32fc1.convertTo(depth_img, CV_16UC1, 1000); } else { ROS_ASSERT_MSG(1, "Unknown depth encoding!"); } } estimator.f_manager.inputDepth(depth_img); double feature_time = feature_msg.first.first.stamp.toSec(); TicToc t_processImage; estimator.processImage(feature_msg.second, feature_msg.first.first); std_msgs::Header header = feature_msg.first.first; header.frame_id = "map"; pubOdometry(estimator, header); pubTF(estimator, header); pubKeyframe(estimator); if (relo_msg != nullptr) pubRelocalization(estimator); m_backend.unlock(); if (SHOW_TRACK) { pubKeyPoses(estimator, header); pubCameraPose(estimator, header); pubPointCloud(estimator, header); visualizeFeatureFilter(feature_msg.second, feature_time); } static int cnt_frame = 0; static double whole_process_time = 0; double per_process_time = t_backend.toc(); cnt_frame++; whole_process_time += per_process_time; printStatistics(estimator, per_process_time); ROS_DEBUG("average backend costs: %f", whole_process_time / cnt_frame); // ROS_DEBUG("backend costs: %f", per_process_time); std::this_thread::sleep_for(dura); } } }; PLUGINLIB_EXPORT_CLASS(estimator_nodelet_ns::EstimatorNodelet, nodelet::Nodelet) } // namespace estimator_nodelet_ns ================================================ FILE: vins_estimator/src/factor/imu_factor.h ================================================ #pragma once #include #include #include #include "../utility/utility.h" #include "../utility/parameters.h" #include "integration_base.h" #include class IMUFactor : public ceres::SizedCostFunction<15, 7, 9, 7, 9> { public: IMUFactor() = delete; IMUFactor(IntegrationBase *_pre_integration) : pre_integration(_pre_integration) {} virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const { Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]); Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]); Eigen::Vector3d Vi(parameters[1][0], parameters[1][1], parameters[1][2]); Eigen::Vector3d Bai(parameters[1][3], parameters[1][4], parameters[1][5]); Eigen::Vector3d Bgi(parameters[1][6], parameters[1][7], parameters[1][8]); Eigen::Vector3d Pj(parameters[2][0], parameters[2][1], parameters[2][2]); Eigen::Quaterniond Qj(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]); Eigen::Vector3d Vj(parameters[3][0], parameters[3][1], parameters[3][2]); Eigen::Vector3d Baj(parameters[3][3], parameters[3][4], parameters[3][5]); Eigen::Vector3d Bgj(parameters[3][6], parameters[3][7], parameters[3][8]); // Eigen::Matrix Fd; // Eigen::Matrix Gd; // Eigen::Vector3d pPj = Pi + Vi * sum_t - 0.5 * g * sum_t * sum_t + corrected_delta_p; // Eigen::Quaterniond pQj = Qi * delta_q; // Eigen::Vector3d pVj = Vi - g * sum_t + corrected_delta_v; // Eigen::Vector3d pBaj = Bai; // Eigen::Vector3d pBgj = Bgi; // Vi + Qi * delta_v - g * sum_dt = Vj; // Qi * delta_q = Qj; // delta_p = Qi.inverse() * (0.5 * g * sum_dt * sum_dt + Pj - Pi); // delta_v = Qi.inverse() * (g * sum_dt + Vj - Vi); // delta_q = Qi.inverse() * Qj; #if 0 if ((Bai - pre_integration->linearized_ba).norm() > 0.10 || (Bgi - pre_integration->linearized_bg).norm() > 0.01) { pre_integration->repropagate(Bai, Bgi); } #endif Eigen::Map> residual(residuals); residual = pre_integration->evaluate(Pi, Qi, Vi, Bai, Bgi, Pj, Qj, Vj, Baj, Bgj); Eigen::Matrix sqrt_info = Eigen::LLT>(pre_integration->covariance.inverse()) .matrixL() .transpose(); // sqrt_info.setIdentity(); residual = sqrt_info * residual; if (jacobians) { double sum_dt = pre_integration->sum_dt; Eigen::Matrix3d dp_dba = pre_integration->jacobian.template block<3, 3>(O_P, O_BA); Eigen::Matrix3d dp_dbg = pre_integration->jacobian.template block<3, 3>(O_P, O_BG); Eigen::Matrix3d dq_dbg = pre_integration->jacobian.template block<3, 3>(O_R, O_BG); Eigen::Matrix3d dv_dba = pre_integration->jacobian.template block<3, 3>(O_V, O_BA); Eigen::Matrix3d dv_dbg = pre_integration->jacobian.template block<3, 3>(O_V, O_BG); if (pre_integration->jacobian.maxCoeff() > 1e8 || pre_integration->jacobian.minCoeff() < -1e8) { ROS_WARN("numerical unstable in preintegration"); // std::cout << pre_integration->jacobian << std::endl; /// ROS_BREAK(); } if (jacobians[0]) { Eigen::Map> jacobian_pose_i( jacobians[0]); jacobian_pose_i.setZero(); jacobian_pose_i.block<3, 3>(O_P, O_P) = -Qi.inverse().toRotationMatrix(); jacobian_pose_i.block<3, 3>(O_P, O_R) = Utility::skewSymmetric( Qi.inverse() * (0.5 * G * sum_dt * sum_dt + Pj - Pi - Vi * sum_dt)); #if 0 jacobian_pose_i.block<3, 3>(O_R, O_R) = -(Qj.inverse() * Qi).toRotationMatrix(); #else Eigen::Quaterniond corrected_delta_q = pre_integration->delta_q * Utility::deltaQ(dq_dbg * (Bgi - pre_integration->linearized_bg)); jacobian_pose_i.block<3, 3>(O_R, O_R) = -(Utility::Qleft(Qj.inverse() * Qi) * Utility::Qright(corrected_delta_q)) .bottomRightCorner<3, 3>(); #endif jacobian_pose_i.block<3, 3>(O_V, O_R) = Utility::skewSymmetric(Qi.inverse() * (G * sum_dt + Vj - Vi)); jacobian_pose_i = sqrt_info * jacobian_pose_i; if (jacobian_pose_i.maxCoeff() > 1e8 || jacobian_pose_i.minCoeff() < -1e8) { ROS_WARN("numerical unstable in preintegration"); // std::cout << sqrt_info << std::endl; // ROS_BREAK(); } } if (jacobians[1]) { Eigen::Map> jacobian_speedbias_i( jacobians[1]); jacobian_speedbias_i.setZero(); jacobian_speedbias_i.block<3, 3>(O_P, O_V - O_V) = -Qi.inverse().toRotationMatrix() * sum_dt; jacobian_speedbias_i.block<3, 3>(O_P, O_BA - O_V) = -dp_dba; jacobian_speedbias_i.block<3, 3>(O_P, O_BG - O_V) = -dp_dbg; #if 0 jacobian_speedbias_i.block<3, 3>(O_R, O_BG - O_V) = -dq_dbg; #else // Eigen::Quaterniond corrected_delta_q = pre_integration->delta_q * // Utility::deltaQ(dq_dbg * (Bgi - pre_integration->linearized_bg)); // jacobian_speedbias_i.block<3, 3>(O_R, O_BG - O_V) = -Utility::Qleft(Qj.inverse() // * Qi * corrected_delta_q).bottomRightCorner<3, 3>() * dq_dbg; jacobian_speedbias_i.block<3, 3>(O_R, O_BG - O_V) = -Utility::Qleft(Qj.inverse() * Qi * pre_integration->delta_q) .bottomRightCorner<3, 3>() * dq_dbg; #endif jacobian_speedbias_i.block<3, 3>(O_V, O_V - O_V) = -Qi.inverse().toRotationMatrix(); jacobian_speedbias_i.block<3, 3>(O_V, O_BA - O_V) = -dv_dba; jacobian_speedbias_i.block<3, 3>(O_V, O_BG - O_V) = -dv_dbg; jacobian_speedbias_i.block<3, 3>(O_BA, O_BA - O_V) = -Eigen::Matrix3d::Identity(); jacobian_speedbias_i.block<3, 3>(O_BG, O_BG - O_V) = -Eigen::Matrix3d::Identity(); jacobian_speedbias_i = sqrt_info * jacobian_speedbias_i; // ROS_ASSERT(fabs(jacobian_speedbias_i.maxCoeff()) < 1e8); // ROS_ASSERT(fabs(jacobian_speedbias_i.minCoeff()) < 1e8); } if (jacobians[2]) { Eigen::Map> jacobian_pose_j( jacobians[2]); jacobian_pose_j.setZero(); jacobian_pose_j.block<3, 3>(O_P, O_P) = Qi.inverse().toRotationMatrix(); #if 0 jacobian_pose_j.block<3, 3>(O_R, O_R) = Eigen::Matrix3d::Identity(); #else Eigen::Quaterniond corrected_delta_q = pre_integration->delta_q * Utility::deltaQ(dq_dbg * (Bgi - pre_integration->linearized_bg)); jacobian_pose_j.block<3, 3>(O_R, O_R) = Utility::Qleft(corrected_delta_q.inverse() * Qi.inverse() * Qj) .bottomRightCorner<3, 3>(); #endif jacobian_pose_j = sqrt_info * jacobian_pose_j; // ROS_ASSERT(fabs(jacobian_pose_j.maxCoeff()) < 1e8); // ROS_ASSERT(fabs(jacobian_pose_j.minCoeff()) < 1e8); } if (jacobians[3]) { Eigen::Map> jacobian_speedbias_j( jacobians[3]); jacobian_speedbias_j.setZero(); jacobian_speedbias_j.block<3, 3>(O_V, O_V - O_V) = Qi.inverse().toRotationMatrix(); jacobian_speedbias_j.block<3, 3>(O_BA, O_BA - O_V) = Eigen::Matrix3d::Identity(); jacobian_speedbias_j.block<3, 3>(O_BG, O_BG - O_V) = Eigen::Matrix3d::Identity(); jacobian_speedbias_j = sqrt_info * jacobian_speedbias_j; // ROS_ASSERT(fabs(jacobian_speedbias_j.maxCoeff()) < 1e8); // ROS_ASSERT(fabs(jacobian_speedbias_j.minCoeff()) < 1e8); } } return true; } // bool Evaluate_Direct(double const *const *parameters, Eigen::Matrix // &residuals, Eigen::Matrix &jacobians); // void checkCorrection(); // void checkTransition(); // void checkJacobian(double **parameters); IntegrationBase *pre_integration; }; ================================================ FILE: vins_estimator/src/factor/imu_factor_modified.h ================================================ #pragma once #include #include #include #include "../utility/utility.h" #include "../utility/parameters.h" #include "integration_base.h" #include // class IMUFactor : public ceres::SizedCostFunction<15, 7, 9, 7, 9> class IMUFactor : public ceres::SizedCostFunction<15, 7, 3, 6, 7, 3, 6> { public: IMUFactor() = delete; IMUFactor(IntegrationBase *_pre_integration) : pre_integration(_pre_integration) {} virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const { Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]); Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]); Eigen::Vector3d Vi(parameters[1][0], parameters[1][1], parameters[1][2]); Eigen::Vector3d Bai(parameters[2][0], parameters[2][1], parameters[2][2]); Eigen::Vector3d Bgi(parameters[2][3], parameters[2][4], parameters[2][5]); Eigen::Vector3d Pj(parameters[3][0], parameters[3][1], parameters[3][2]); Eigen::Quaterniond Qj(parameters[3][6], parameters[3][3], parameters[3][4], parameters[3][5]); Eigen::Vector3d Vj(parameters[4][0], parameters[4][1], parameters[4][2]); Eigen::Vector3d Baj(parameters[5][0], parameters[5][1], parameters[5][2]); Eigen::Vector3d Bgj(parameters[5][3], parameters[5][4], parameters[5][5]); // Eigen::Matrix Fd; // Eigen::Matrix Gd; // Eigen::Vector3d pPj = Pi + Vi * sum_t - 0.5 * g * sum_t * sum_t + corrected_delta_p; // Eigen::Quaterniond pQj = Qi * delta_q; // Eigen::Vector3d pVj = Vi - g * sum_t + corrected_delta_v; // Eigen::Vector3d pBaj = Bai; // Eigen::Vector3d pBgj = Bgi; // Vi + Qi * delta_v - g * sum_dt = Vj; // Qi * delta_q = Qj; // delta_p = Qi.inverse() * (0.5 * g * sum_dt * sum_dt + Pj - Pi); // delta_v = Qi.inverse() * (g * sum_dt + Vj - Vi); // delta_q = Qi.inverse() * Qj; #if 0 if ((Bai - pre_integration->linearized_ba).norm() > 0.10 || (Bgi - pre_integration->linearized_bg).norm() > 0.01) { pre_integration->repropagate(Bai, Bgi); } #endif Eigen::Map> residual(residuals); residual = pre_integration->evaluate(Pi, Qi, Vi, Bai, Bgi, Pj, Qj, Vj, Baj, Bgj); Eigen::Matrix sqrt_info = Eigen::LLT>(pre_integration->covariance.inverse()) .matrixL() .transpose(); // sqrt_info.setIdentity(); residual = sqrt_info * residual; if (jacobians) { double sum_dt = pre_integration->sum_dt; Eigen::Matrix3d dp_dba = pre_integration->jacobian.template block<3, 3>(O_P, O_BA); Eigen::Matrix3d dp_dbg = pre_integration->jacobian.template block<3, 3>(O_P, O_BG); Eigen::Matrix3d dq_dbg = pre_integration->jacobian.template block<3, 3>(O_R, O_BG); Eigen::Matrix3d dv_dba = pre_integration->jacobian.template block<3, 3>(O_V, O_BA); Eigen::Matrix3d dv_dbg = pre_integration->jacobian.template block<3, 3>(O_V, O_BG); if (pre_integration->jacobian.maxCoeff() > 1e8 || pre_integration->jacobian.minCoeff() < -1e8) { ROS_WARN("numerical unstable in preintegration"); // std::cout << pre_integration->jacobian << std::endl; /// ROS_BREAK(); } if (jacobians[0]) { Eigen::Map> jacobian_pose_i( jacobians[0]); jacobian_pose_i.setZero(); jacobian_pose_i.block<3, 3>(O_P, O_P) = -Qi.inverse().toRotationMatrix(); jacobian_pose_i.block<3, 3>(O_P, O_R) = Utility::skewSymmetric( Qi.inverse() * (0.5 * G * sum_dt * sum_dt + Pj - Pi - Vi * sum_dt)); #if 0 jacobian_pose_i.block<3, 3>(O_R, O_R) = -(Qj.inverse() * Qi).toRotationMatrix(); #else Eigen::Quaterniond corrected_delta_q = pre_integration->delta_q * Utility::deltaQ(dq_dbg * (Bgi - pre_integration->linearized_bg)); jacobian_pose_i.block<3, 3>(O_R, O_R) = -(Utility::Qleft(Qj.inverse() * Qi) * Utility::Qright(corrected_delta_q)) .bottomRightCorner<3, 3>(); #endif jacobian_pose_i.block<3, 3>(O_V, O_R) = Utility::skewSymmetric(Qi.inverse() * (G * sum_dt + Vj - Vi)); jacobian_pose_i = sqrt_info * jacobian_pose_i; if (jacobian_pose_i.maxCoeff() > 1e8 || jacobian_pose_i.minCoeff() < -1e8) { ROS_WARN("numerical unstable in preintegration"); // std::cout << sqrt_info << std::endl; // ROS_BREAK(); } } if (jacobians[1]) { Eigen::Map> jacobian_speedbias_i( jacobians[1]); jacobian_speedbias_i.setZero(); jacobian_speedbias_i.block<3, 3>(O_P, O_V - O_V) = -Qi.inverse().toRotationMatrix() * sum_dt; jacobian_speedbias_i.block<3, 3>(O_P, O_BA - O_V) = -dp_dba; jacobian_speedbias_i.block<3, 3>(O_P, O_BG - O_V) = -dp_dbg; #if 0 jacobian_speedbias_i.block<3, 3>(O_R, O_BG - O_V) = -dq_dbg; #else // Eigen::Quaterniond corrected_delta_q = pre_integration->delta_q * // Utility::deltaQ(dq_dbg * (Bgi - pre_integration->linearized_bg)); // jacobian_speedbias_i.block<3, 3>(O_R, O_BG - O_V) = -Utility::Qleft(Qj.inverse() // * Qi * corrected_delta_q).bottomRightCorner<3, 3>() * dq_dbg; jacobian_speedbias_i.block<3, 3>(O_R, O_BG - O_V) = -Utility::Qleft(Qj.inverse() * Qi * pre_integration->delta_q) .bottomRightCorner<3, 3>() * dq_dbg; #endif jacobian_speedbias_i.block<3, 3>(O_V, O_V - O_V) = -Qi.inverse().toRotationMatrix(); jacobian_speedbias_i.block<3, 3>(O_V, O_BA - O_V) = -dv_dba; jacobian_speedbias_i.block<3, 3>(O_V, O_BG - O_V) = -dv_dbg; jacobian_speedbias_i.block<3, 3>(O_BA, O_BA - O_V) = -Eigen::Matrix3d::Identity(); jacobian_speedbias_i.block<3, 3>(O_BG, O_BG - O_V) = -Eigen::Matrix3d::Identity(); jacobian_speedbias_i = sqrt_info * jacobian_speedbias_i; // ROS_ASSERT(fabs(jacobian_speedbias_i.maxCoeff()) < 1e8); // ROS_ASSERT(fabs(jacobian_speedbias_i.minCoeff()) < 1e8); } if (jacobians[2]) { Eigen::Map> jacobian_pose_j( jacobians[2]); jacobian_pose_j.setZero(); jacobian_pose_j.block<3, 3>(O_P, O_P) = Qi.inverse().toRotationMatrix(); #if 0 jacobian_pose_j.block<3, 3>(O_R, O_R) = Eigen::Matrix3d::Identity(); #else Eigen::Quaterniond corrected_delta_q = pre_integration->delta_q * Utility::deltaQ(dq_dbg * (Bgi - pre_integration->linearized_bg)); jacobian_pose_j.block<3, 3>(O_R, O_R) = Utility::Qleft(corrected_delta_q.inverse() * Qi.inverse() * Qj) .bottomRightCorner<3, 3>(); #endif jacobian_pose_j = sqrt_info * jacobian_pose_j; // ROS_ASSERT(fabs(jacobian_pose_j.maxCoeff()) < 1e8); // ROS_ASSERT(fabs(jacobian_pose_j.minCoeff()) < 1e8); } if (jacobians[3]) { Eigen::Map> jacobian_speedbias_j( jacobians[3]); jacobian_speedbias_j.setZero(); jacobian_speedbias_j.block<3, 3>(O_V, O_V - O_V) = Qi.inverse().toRotationMatrix(); jacobian_speedbias_j.block<3, 3>(O_BA, O_BA - O_V) = Eigen::Matrix3d::Identity(); jacobian_speedbias_j.block<3, 3>(O_BG, O_BG - O_V) = Eigen::Matrix3d::Identity(); jacobian_speedbias_j = sqrt_info * jacobian_speedbias_j; // ROS_ASSERT(fabs(jacobian_speedbias_j.maxCoeff()) < 1e8); // ROS_ASSERT(fabs(jacobian_speedbias_j.minCoeff()) < 1e8); } } return true; } // bool Evaluate_Direct(double const *const *parameters, Eigen::Matrix // &residuals, Eigen::Matrix &jacobians); // void checkCorrection(); // void checkTransition(); // void checkJacobian(double **parameters); IntegrationBase *pre_integration; }; ================================================ FILE: vins_estimator/src/factor/integration_base.h ================================================ #pragma once #include "../utility/utility.h" #include "../utility/parameters.h" #include using namespace Eigen; class IntegrationBase { public: IntegrationBase() = delete; IntegrationBase(const Eigen::Vector3d &_acc_0, const Eigen::Vector3d &_gyr_0, const Eigen::Vector3d &_linearized_ba, const Eigen::Vector3d &_linearized_bg) : acc_0{_acc_0}, gyr_0{_gyr_0}, linearized_acc{_acc_0}, linearized_gyr{_gyr_0}, linearized_ba{_linearized_ba}, linearized_bg{_linearized_bg}, jacobian{Eigen::Matrix::Identity()}, covariance{Eigen::Matrix::Zero()}, sum_dt{0.0}, delta_p{Eigen::Vector3d::Zero()}, delta_q{Eigen::Quaterniond::Identity()}, delta_v{Eigen::Vector3d::Zero()} { noise = Eigen::Matrix::Zero(); noise.block<3, 3>(0, 0) = (ACC_N * ACC_N) * Eigen::Matrix3d::Identity(); noise.block<3, 3>(3, 3) = (GYR_N * GYR_N) * Eigen::Matrix3d::Identity(); noise.block<3, 3>(6, 6) = (ACC_N * ACC_N) * Eigen::Matrix3d::Identity(); noise.block<3, 3>(9, 9) = (GYR_N * GYR_N) * Eigen::Matrix3d::Identity(); noise.block<3, 3>(12, 12) = (ACC_W * ACC_W) * Eigen::Matrix3d::Identity(); noise.block<3, 3>(15, 15) = (GYR_W * GYR_W) * Eigen::Matrix3d::Identity(); } void push_back(double dt, const Eigen::Vector3d &acc, const Eigen::Vector3d &gyr) { dt_buf.push_back(dt); acc_buf.push_back(acc); gyr_buf.push_back(gyr); propagate(dt, acc, gyr); } void repropagate(const Eigen::Vector3d &_linearized_ba, const Eigen::Vector3d &_linearized_bg) { sum_dt = 0.0; acc_0 = linearized_acc; gyr_0 = linearized_gyr; delta_p.setZero(); delta_q.setIdentity(); delta_v.setZero(); linearized_ba = _linearized_ba; linearized_bg = _linearized_bg; jacobian.setIdentity(); covariance.setZero(); for (int i = 0; i < static_cast(dt_buf.size()); i++) propagate(dt_buf[i], acc_buf[i], gyr_buf[i]); } void midPointIntegration(double _dt, const Eigen::Vector3d &_acc_0, const Eigen::Vector3d &_gyr_0, const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1, const Eigen::Vector3d &delta_p, const Eigen::Quaterniond &delta_q, const Eigen::Vector3d &delta_v, const Eigen::Vector3d &linearized_ba, const Eigen::Vector3d &linearized_bg, Eigen::Vector3d &result_delta_p, Eigen::Quaterniond &result_delta_q, Eigen::Vector3d &result_delta_v, Eigen::Vector3d &result_linearized_ba, Eigen::Vector3d &result_linearized_bg, bool update_jacobian) { // ROS_INFO("midpoint integration"); Vector3d un_acc_0 = delta_q * (_acc_0 - linearized_ba); Vector3d un_gyr = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg; result_delta_q = delta_q * Quaterniond(1, un_gyr(0) * _dt / 2, un_gyr(1) * _dt / 2, un_gyr(2) * _dt / 2); Vector3d un_acc_1 = result_delta_q * (_acc_1 - linearized_ba); Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1); result_delta_p = delta_p + delta_v * _dt + 0.5 * un_acc * _dt * _dt; result_delta_v = delta_v + un_acc * _dt; result_linearized_ba = linearized_ba; result_linearized_bg = linearized_bg; if (update_jacobian) { Vector3d w_x = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg; Vector3d a_0_x = _acc_0 - linearized_ba; Vector3d a_1_x = _acc_1 - linearized_ba; Matrix3d R_w_x, R_a_0_x, R_a_1_x; R_w_x << 0, -w_x(2), w_x(1), w_x(2), 0, -w_x(0), -w_x(1), w_x(0), 0; R_a_0_x << 0, -a_0_x(2), a_0_x(1), a_0_x(2), 0, -a_0_x(0), -a_0_x(1), a_0_x(0), 0; R_a_1_x << 0, -a_1_x(2), a_1_x(1), a_1_x(2), 0, -a_1_x(0), -a_1_x(1), a_1_x(0), 0; MatrixXd F = MatrixXd::Zero(15, 15); F.block<3, 3>(0, 0) = Matrix3d::Identity(); F.block<3, 3>(0, 3) = -0.25 * delta_q.toRotationMatrix() * R_a_0_x * _dt * _dt + -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt * _dt; F.block<3, 3>(0, 6) = MatrixXd::Identity(3, 3) * _dt; F.block<3, 3>(0, 9) = -0.25 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt * _dt; F.block<3, 3>(0, 12) = -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * _dt * -_dt; F.block<3, 3>(3, 3) = Matrix3d::Identity() - R_w_x * _dt; F.block<3, 3>(3, 12) = -1.0 * MatrixXd::Identity(3, 3) * _dt; F.block<3, 3>(6, 3) = -0.5 * delta_q.toRotationMatrix() * R_a_0_x * _dt + -0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt; F.block<3, 3>(6, 6) = Matrix3d::Identity(); F.block<3, 3>(6, 9) = -0.5 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt; F.block<3, 3>(6, 12) = -0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * -_dt; F.block<3, 3>(9, 9) = Matrix3d::Identity(); F.block<3, 3>(12, 12) = Matrix3d::Identity(); // cout<<"A"<(0, 0) = 0.25 * delta_q.toRotationMatrix() * _dt * _dt; V.block<3, 3>(0, 3) = 0.25 * -result_delta_q.toRotationMatrix() * R_a_1_x * _dt * _dt * 0.5 * _dt; V.block<3, 3>(0, 6) = 0.25 * result_delta_q.toRotationMatrix() * _dt * _dt; V.block<3, 3>(0, 9) = V.block<3, 3>(0, 3); V.block<3, 3>(3, 3) = 0.5 * MatrixXd::Identity(3, 3) * _dt; V.block<3, 3>(3, 9) = 0.5 * MatrixXd::Identity(3, 3) * _dt; V.block<3, 3>(6, 0) = 0.5 * delta_q.toRotationMatrix() * _dt; V.block<3, 3>(6, 3) = 0.5 * -result_delta_q.toRotationMatrix() * R_a_1_x * _dt * 0.5 * _dt; V.block<3, 3>(6, 6) = 0.5 * result_delta_q.toRotationMatrix() * _dt; V.block<3, 3>(6, 9) = V.block<3, 3>(6, 3); V.block<3, 3>(9, 12) = MatrixXd::Identity(3, 3) * _dt; V.block<3, 3>(12, 15) = MatrixXd::Identity(3, 3) * _dt; // step_jacobian = F; // step_V = V; jacobian = F * jacobian; covariance = F * covariance * F.transpose() + V * noise * V.transpose(); } } void propagate(double _dt, const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1) { dt = _dt; acc_1 = _acc_1; gyr_1 = _gyr_1; Vector3d result_delta_p; Quaterniond result_delta_q; Vector3d result_delta_v; Vector3d result_linearized_ba; Vector3d result_linearized_bg; midPointIntegration(_dt, acc_0, gyr_0, _acc_1, _gyr_1, delta_p, delta_q, delta_v, linearized_ba, linearized_bg, result_delta_p, result_delta_q, result_delta_v, result_linearized_ba, result_linearized_bg, 1); // checkJacobian(_dt, acc_0, gyr_0, acc_1, gyr_1, delta_p, delta_q, delta_v, // linearized_ba, linearized_bg); delta_p = result_delta_p; delta_q = result_delta_q; delta_v = result_delta_v; linearized_ba = result_linearized_ba; linearized_bg = result_linearized_bg; delta_q.normalize(); sum_dt += dt; acc_0 = acc_1; gyr_0 = gyr_1; } Eigen::Matrix evaluate(const Eigen::Vector3d &Pi, const Eigen::Quaterniond &Qi, const Eigen::Vector3d &Vi, const Eigen::Vector3d &Bai, const Eigen::Vector3d &Bgi, const Eigen::Vector3d &Pj, const Eigen::Quaterniond &Qj, const Eigen::Vector3d &Vj, const Eigen::Vector3d &Baj, const Eigen::Vector3d &Bgj) { Eigen::Matrix residuals; Eigen::Matrix3d dp_dba = jacobian.block<3, 3>(O_P, O_BA); Eigen::Matrix3d dp_dbg = jacobian.block<3, 3>(O_P, O_BG); Eigen::Matrix3d dq_dbg = jacobian.block<3, 3>(O_R, O_BG); Eigen::Matrix3d dv_dba = jacobian.block<3, 3>(O_V, O_BA); Eigen::Matrix3d dv_dbg = jacobian.block<3, 3>(O_V, O_BG); Eigen::Vector3d dba = Bai - linearized_ba; Eigen::Vector3d dbg = Bgi - linearized_bg; Eigen::Quaterniond corrected_delta_q = delta_q * Utility::deltaQ(dq_dbg * dbg); Eigen::Vector3d corrected_delta_v = delta_v + dv_dba * dba + dv_dbg * dbg; Eigen::Vector3d corrected_delta_p = delta_p + dp_dba * dba + dp_dbg * dbg; residuals.block<3, 1>(O_P, 0) = Qi.inverse() * (0.5 * G * sum_dt * sum_dt + Pj - Pi - Vi * sum_dt) - corrected_delta_p; residuals.block<3, 1>(O_R, 0) = 2 * (corrected_delta_q.inverse() * (Qi.inverse() * Qj)).vec(); residuals.block<3, 1>(O_V, 0) = Qi.inverse() * (G * sum_dt + Vj - Vi) - corrected_delta_v; residuals.block<3, 1>(O_BA, 0) = Baj - Bai; residuals.block<3, 1>(O_BG, 0) = Bgj - Bgi; return residuals; } double dt; Eigen::Vector3d acc_0, gyr_0; Eigen::Vector3d acc_1, gyr_1; const Eigen::Vector3d linearized_acc, linearized_gyr; Eigen::Vector3d linearized_ba, linearized_bg; Eigen::Matrix jacobian, covariance; Eigen::Matrix step_jacobian; Eigen::Matrix step_V; Eigen::Matrix noise; double sum_dt; Eigen::Vector3d delta_p; Eigen::Quaterniond delta_q; Eigen::Vector3d delta_v; std::vector dt_buf; std::vector acc_buf; std::vector gyr_buf; }; ================================================ FILE: vins_estimator/src/factor/marginalization_factor.cpp ================================================ #include "marginalization_factor.h" void ResidualBlockInfo::Evaluate() { residuals.resize(cost_function->num_residuals()); std::vector block_sizes = cost_function->parameter_block_sizes(); raw_jacobians = new double *[block_sizes.size()]; jacobians.resize(block_sizes.size()); for (int i = 0; i < static_cast(block_sizes.size()); i++) { jacobians[i].resize(cost_function->num_residuals(), block_sizes[i]); raw_jacobians[i] = jacobians[i].data(); // dim += block_sizes[i] == 7 ? 6 : block_sizes[i]; } cost_function->Evaluate(parameter_blocks.data(), residuals.data(), raw_jacobians); // std::vector tmp_idx(block_sizes.size()); // Eigen::MatrixXd tmp(dim, dim); // for (int i = 0; i < static_cast(parameter_blocks.size()); i++) //{ // int size_i = localSize(block_sizes[i]); // Eigen::MatrixXd jacobian_i = jacobians[i].leftCols(size_i); // for (int j = 0, sub_idx = 0; j < static_cast(parameter_blocks.size()); sub_idx += // block_sizes[j] == 7 ? 6 : block_sizes[j], j++) // { // int size_j = localSize(block_sizes[j]); // Eigen::MatrixXd jacobian_j = jacobians[j].leftCols(size_j); // tmp_idx[j] = sub_idx; // tmp.block(tmp_idx[i], tmp_idx[j], size_i, size_j) = jacobian_i.transpose() * // jacobian_j; // } // } // Eigen::SelfAdjointEigenSolver saes(tmp); // std::cout << saes.eigenvalues() << std::endl; // ROS_ASSERT(saes.eigenvalues().minCoeff() >= -1e-6); if (loss_function) { double residual_scaling_, alpha_sq_norm_; double sq_norm, rho[3]; sq_norm = residuals.squaredNorm(); loss_function->Evaluate(sq_norm, rho); // printf("sq_norm: %f, rho[0]: %f, rho[1]: %f, rho[2]: %f\n", sq_norm, rho[0], rho[1], // rho[2]); double sqrt_rho1_ = sqrt(rho[1]); if ((sq_norm == 0.0) || (rho[2] <= 0.0)) { residual_scaling_ = sqrt_rho1_; alpha_sq_norm_ = 0.0; } else { const double D = 1.0 + 2.0 * sq_norm * rho[2] / rho[1]; const double alpha = 1.0 - sqrt(D); residual_scaling_ = sqrt_rho1_ / (1 - alpha); alpha_sq_norm_ = alpha / sq_norm; } for (int i = 0; i < static_cast(parameter_blocks.size()); i++) { jacobians[i] = sqrt_rho1_ * (jacobians[i] - alpha_sq_norm_ * residuals * (residuals.transpose() * jacobians[i])); } residuals *= residual_scaling_; } } MarginalizationInfo::~MarginalizationInfo() { // ROS_WARN("release marginlizationinfo"); for (auto it = parameter_block_data.begin(); it != parameter_block_data.end(); ++it) delete[] it->second; for (int i = 0; i < (int)factors.size(); i++) { delete[] factors[i]->raw_jacobians; delete factors[i]->cost_function; delete factors[i]; } } void MarginalizationInfo::addResidualBlockInfo(ResidualBlockInfo *residual_block_info) { factors.emplace_back(residual_block_info); std::vector ¶meter_blocks = residual_block_info->parameter_blocks; std::vector parameter_block_sizes = residual_block_info->cost_function->parameter_block_sizes(); for (int i = 0; i < static_cast(residual_block_info->parameter_blocks.size()); i++) { double *addr = parameter_blocks[i]; int size = parameter_block_sizes[i]; parameter_block_size[reinterpret_cast(addr)] = size; } for (int i = 0; i < static_cast(residual_block_info->drop_set.size()); i++) { double *addr = parameter_blocks[residual_block_info->drop_set[i]]; parameter_block_idx[reinterpret_cast(addr)] = 0; } } void MarginalizationInfo::preMarginalize() //构建parameter_block_data <变量的内存地址， 变量数据> { for (auto it : factors) { it->Evaluate(); //求解残差因子对应的残差与雅克比 std::vector block_sizes = it->cost_function->parameter_block_sizes(); //代价函数对应的优化变量参数块的数目 for (int i = 0; i < static_cast(block_sizes.size()); i++) { long addr = reinterpret_cast(it->parameter_blocks[i]); int size = block_sizes[i]; if (parameter_block_data.find(addr) == parameter_block_data.end()) { double *data = new double[size]; memcpy(data, it->parameter_blocks[i], sizeof(double) * size); parameter_block_data[addr] = data; } } } } int MarginalizationInfo::localSize(int size) const { return size == 7 ? 6 : size; } int MarginalizationInfo::globalSize(int size) const { return size == 6 ? 7 : size; } void *ThreadsConstructA(void *threadsstruct) { ThreadsStruct *p = ((ThreadsStruct *)threadsstruct); for (auto it : p->sub_factors) { for (int i = 0; i < static_cast(it->parameter_blocks.size()); i++) { int idx_i = p->parameter_block_idx[reinterpret_cast(it->parameter_blocks[i])]; int size_i = p->parameter_block_size[reinterpret_cast(it->parameter_blocks[i])]; if (size_i == 7) size_i = 6; Eigen::MatrixXd jacobian_i = it->jacobians[i].leftCols(size_i); for (int j = i; j < static_cast(it->parameter_blocks.size()); j++) { int idx_j = p->parameter_block_idx[reinterpret_cast(it->parameter_blocks[j])]; int size_j = p->parameter_block_size[reinterpret_cast(it->parameter_blocks[j])]; if (size_j == 7) size_j = 6; Eigen::MatrixXd jacobian_j = it->jacobians[j].leftCols(size_j); if (i == j) p->A.block(idx_i, idx_j, size_i, size_j) += jacobian_i.transpose() * jacobian_j; else { p->A.block(idx_i, idx_j, size_i, size_j) += jacobian_i.transpose() * jacobian_j; p->A.block(idx_j, idx_i, size_j, size_i) = p->A.block(idx_i, idx_j, size_i, size_j).transpose(); } } p->b.segment(idx_i, size_i) += jacobian_i.transpose() * it->residuals; } } return threadsstruct; } void MarginalizationInfo::marginalize() { int pos = 0; for (auto &it : parameter_block_idx) { it.second = pos; pos += localSize(parameter_block_size[it.first]); } m = pos; for (const auto &it : parameter_block_size) { if (parameter_block_idx.find(it.first) == parameter_block_idx.end()) { parameter_block_idx[it.first] = pos; pos += localSize(it.second); } } n = pos - m; // ROS_DEBUG("marginalization, pos: %d, m: %d, n: %d, size: %d", pos, m, n, // (int)parameter_block_idx.size()); TicToc t_summing; Eigen::MatrixXd A(pos, pos); Eigen::VectorXd b(pos); A.setZero(); b.setZero(); /* for (auto it : factors) { for (int i = 0; i < static_cast(it->parameter_blocks.size()); i++) { int idx_i = parameter_block_idx[reinterpret_cast(it->parameter_blocks[i])]; int size_i = localSize(parameter_block_size[reinterpret_cast(it->parameter_blocks[i])]); Eigen::MatrixXd jacobian_i = it->jacobians[i].leftCols(size_i); for (int j = i; j < static_cast(it->parameter_blocks.size()); j++) { int idx_j = parameter_block_idx[reinterpret_cast(it->parameter_blocks[j])]; int size_j = localSize(parameter_block_size[reinterpret_cast(it->parameter_blocks[j])]); Eigen::MatrixXd jacobian_j = it->jacobians[j].leftCols(size_j); if (i == j) A.block(idx_i, idx_j, size_i, size_j) += jacobian_i.transpose() * jacobian_j; else { A.block(idx_i, idx_j, size_i, size_j) += jacobian_i.transpose() * jacobian_j; A.block(idx_j, idx_i, size_j, size_i) = A.block(idx_i, idx_j, size_i, size_j).transpose(); } } b.segment(idx_i, size_i) += jacobian_i.transpose() * it->residuals; } } ROS_INFO("summing up costs %f ms", t_summing.toc()); */ // multi thread TicToc t_thread_summing; pthread_t tids[NUM_MAR_THREADS]; ThreadsStruct threadsstruct[NUM_MAR_THREADS]; int i = 0; for (auto it : factors) { threadsstruct[i].sub_factors.push_back(it); i++; i = i % NUM_MAR_THREADS; } for (int i = 0; i < NUM_MAR_THREADS; i++) { TicToc zero_matrix; threadsstruct[i].A = Eigen::MatrixXd::Zero(pos, pos); threadsstruct[i].b = Eigen::VectorXd::Zero(pos); threadsstruct[i].parameter_block_size = parameter_block_size; threadsstruct[i].parameter_block_idx = parameter_block_idx; int ret = pthread_create(&tids[i], NULL, ThreadsConstructA, (void *)&(threadsstruct[i])); if (ret != 0) { ROS_WARN("pthread_create error"); ROS_BREAK(); } } for (int i = NUM_MAR_THREADS - 1; i >= 0; i--) { pthread_join(tids[i], NULL); A += threadsstruct[i].A; b += threadsstruct[i].b; } // ROS_DEBUG("thread summing up costs %f ms", t_thread_summing.toc()); // ROS_INFO("A diff %f , b diff %f ", (A - tmp_A).sum(), (b - tmp_b).sum()); // TODO Eigen::MatrixXd Amm = 0.5 * (A.block(0, 0, m, m) + A.block(0, 0, m, m).transpose()); Eigen::SelfAdjointEigenSolver saes(Amm); // ROS_ASSERT_MSG(saes.eigenvalues().minCoeff() >= -1e-4, "min eigenvalue %f", // saes.eigenvalues().minCoeff()); Eigen::MatrixXd Amm_inv = saes.eigenvectors() * Eigen::VectorXd( (saes.eigenvalues().array() > eps).select(saes.eigenvalues().array().inverse(), 0)) .asDiagonal() * saes.eigenvectors().transpose(); // printf("error1: %f\n", (Amm * Amm_inv - Eigen::MatrixXd::Identity(m, m)).sum()); Eigen::VectorXd bmm = b.segment(0, m); Eigen::MatrixXd Amr = A.block(0, m, m, n); Eigen::MatrixXd Arm = A.block(m, 0, n, m); Eigen::MatrixXd Arr = A.block(m, m, n, n); Eigen::VectorXd brr = b.segment(m, n); A = Arr - Arm * Amm_inv * Amr; b = brr - Arm * Amm_inv * bmm; Eigen::SelfAdjointEigenSolver saes2(A); Eigen::VectorXd S = Eigen::VectorXd((saes2.eigenvalues().array() > eps).select(saes2.eigenvalues().array(), 0)); Eigen::VectorXd S_inv = Eigen::VectorXd( (saes2.eigenvalues().array() > eps).select(saes2.eigenvalues().array().inverse(), 0)); Eigen::VectorXd S_sqrt = S.cwiseSqrt(); Eigen::VectorXd S_inv_sqrt = S_inv.cwiseSqrt(); linearized_jacobians = S_sqrt.asDiagonal() * saes2.eigenvectors().transpose(); linearized_residuals = S_inv_sqrt.asDiagonal() * saes2.eigenvectors().transpose() * b; // std::cout << A << std::endl // << std::endl; // std::cout << linearized_jacobians << std::endl; // printf("error2: %f %f\n", (linearized_jacobians.transpose() * linearized_jacobians - // A).sum(), // (linearized_jacobians.transpose() * linearized_residuals - b).sum()); } std::vector MarginalizationInfo::getParameterBlocks(std::unordered_map &addr_shift) { std::vector keep_block_addr; keep_block_size.clear(); keep_block_idx.clear(); keep_block_data.clear(); for (const auto &it : parameter_block_idx) { if (it.second >= m) { keep_block_size.push_back(parameter_block_size[it.first]); keep_block_idx.push_back(parameter_block_idx[it.first]); keep_block_data.push_back(parameter_block_data[it.first]); keep_block_addr.push_back(addr_shift[it.first]); } } sum_block_size = std::accumulate(std::begin(keep_block_size), std::end(keep_block_size), 0); return keep_block_addr; } MarginalizationFactor::MarginalizationFactor(MarginalizationInfo *_marginalization_info) : marginalization_info(_marginalization_info) { int cnt = 0; for (auto it : marginalization_info->keep_block_size) { mutable_parameter_block_sizes()->push_back(it); cnt += it; } // printf("residual size: %d, %d\n", cnt, n); set_num_residuals(marginalization_info->n); }; bool MarginalizationFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const { // printf("internal addr,%d, %d\n", (int)parameter_block_sizes().size(), num_residuals()); // for (int i = 0; i < static_cast(keep_block_size.size()); i++) //{ // //printf("unsigned %x\n", reinterpret_cast(parameters[i])); // //printf("signed %x\n", reinterpret_cast(parameters[i])); // printf("jacobian %x\n", reinterpret_cast(jacobians)); // printf("residual %x\n", reinterpret_cast(residuals)); // } int n = marginalization_info->n; int m = marginalization_info->m; Eigen::VectorXd dx(n); for (int i = 0; i < static_cast(marginalization_info->keep_block_size.size()); i++) { int size = marginalization_info->keep_block_size[i]; int idx = marginalization_info->keep_block_idx[i] - m; Eigen::VectorXd x = Eigen::Map(parameters[i], size); Eigen::VectorXd x0 = Eigen::Map(marginalization_info->keep_block_data[i], size); if (size != 7) dx.segment(idx, size) = x - x0; else { dx.segment<3>(idx + 0) = x.head<3>() - x0.head<3>(); dx.segment<3>(idx + 3) = 2.0 * Utility::positify(Eigen::Quaterniond(x0(6), x0(3), x0(4), x0(5)).inverse() * Eigen::Quaterniond(x(6), x(3), x(4), x(5))) .vec(); if (!((Eigen::Quaterniond(x0(6), x0(3), x0(4), x0(5)).inverse() * Eigen::Quaterniond(x(6), x(3), x(4), x(5))) .w() >= 0)) { dx.segment<3>(idx + 3) = 2.0 * -Utility::positify(Eigen::Quaterniond(x0(6), x0(3), x0(4), x0(5)).inverse() * Eigen::Quaterniond(x(6), x(3), x(4), x(5))) .vec(); } } } Eigen::Map(residuals, n) = marginalization_info->linearized_residuals + marginalization_info->linearized_jacobians * dx; if (jacobians) { for (int i = 0; i < static_cast(marginalization_info->keep_block_size.size()); i++) { if (jacobians[i]) { int size = marginalization_info->keep_block_size[i], local_size = marginalization_info->localSize(size); int idx = marginalization_info->keep_block_idx[i] - m; Eigen::Map> jacobian(jacobians[i], n, size); jacobian.setZero(); jacobian.leftCols(local_size) = marginalization_info->linearized_jacobians.middleCols(idx, local_size); } } } return true; } ================================================ FILE: vins_estimator/src/factor/marginalization_factor.h ================================================ #pragma once #include #include #include #include #include #include #include "../utility/parameters.h" #include "../utility/tic_toc.h" #include "../utility/utility.h" const int NUM_MAR_THREADS = 4; struct ResidualBlockInfo { ResidualBlockInfo(ceres::CostFunction *_cost_function, ceres::LossFunction *_loss_function, std::vector _parameter_blocks, std::vector _drop_set) : cost_function(_cost_function), loss_function(_loss_function), parameter_blocks(_parameter_blocks), drop_set(_drop_set) { } void Evaluate(); ceres::CostFunction *cost_function; ceres::LossFunction *loss_function; std::vector parameter_blocks; std::vector drop_set; double **raw_jacobians; std::vector> jacobians; Eigen::VectorXd residuals; int localSize(int size) { return size == 7 ? 6 : size; } }; struct ThreadsStruct { std::vector sub_factors; Eigen::MatrixXd A; Eigen::VectorXd b; std::unordered_map parameter_block_size; // global size std::unordered_map parameter_block_idx; // local size }; class MarginalizationInfo { public: ~MarginalizationInfo(); int localSize(int size) const; int globalSize(int size) const; void addResidualBlockInfo(ResidualBlockInfo *residual_block_info); void preMarginalize(); void marginalize(); std::vector getParameterBlocks(std::unordered_map &addr_shift); std::vector factors; int m, n; std::unordered_map parameter_block_size; // global size int sum_block_size; std::unordered_map parameter_block_idx; // local size std::unordered_map parameter_block_data; std::vector keep_block_size; // global size std::vector keep_block_idx; // local size std::vector keep_block_data; Eigen::MatrixXd linearized_jacobians; Eigen::VectorXd linearized_residuals; const double eps = 1e-8; }; class MarginalizationFactor : public ceres::CostFunction { public: MarginalizationFactor(MarginalizationInfo *_marginalization_info); virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; MarginalizationInfo *marginalization_info; }; ================================================ FILE: vins_estimator/src/factor/pose_local_parameterization.cpp ================================================ #include "pose_local_parameterization.h" bool PoseLocalParameterization::Plus(const double *x, const double *delta, double *x_plus_delta) const { Eigen::Map _p(x); Eigen::Map _q(x + 3); Eigen::Map dp(delta); Eigen::Quaterniond dq = Utility::deltaQ(Eigen::Map(delta + 3)); Eigen::Map p(x_plus_delta); Eigen::Map q(x_plus_delta + 3); p = _p + dp; q = (_q * dq).normalized(); return true; } bool PoseLocalParameterization::ComputeJacobian(const double *x, double *jacobian) const { Eigen::Map> j(jacobian); j.topRows<6>().setIdentity(); j.bottomRows<1>().setZero(); return true; } ================================================ FILE: vins_estimator/src/factor/pose_local_parameterization.h ================================================ #pragma once #include #include #include "../utility/utility.h" class PoseLocalParameterization : public ceres::LocalParameterization { virtual bool Plus(const double *x, const double *delta, double *x_plus_delta) const; virtual bool ComputeJacobian(const double *x, double *jacobian) const; virtual int GlobalSize() const { return 7; }; virtual int LocalSize() const { return 6; }; }; ================================================ FILE: vins_estimator/src/factor/projection_factor.cpp ================================================ #include "projection_factor.h" Eigen::Matrix2d ProjectionFactor::sqrt_info; double ProjectionFactor::sum_t; ProjectionFactor::ProjectionFactor(const Eigen::Vector3d &_pts_i, const Eigen::Vector3d &_pts_j) : pts_i(_pts_i), pts_j(_pts_j) { #ifdef UNIT_SPHERE_ERROR Eigen::Vector3d b1, b2; Eigen::Vector3d a = pts_j.normalized(); Eigen::Vector3d tmp(0, 0, 1); if (a == tmp) tmp << 1, 0, 0; b1 = (tmp - a * (a.transpose() * tmp)).normalized(); b2 = a.cross(b1); tangent_base.block<1, 3>(0, 0) = b1.transpose(); tangent_base.block<1, 3>(1, 0) = b2.transpose(); #endif }; bool ProjectionFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const { TicToc tic_toc; Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]); Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]); Eigen::Vector3d Pj(parameters[1][0], parameters[1][1], parameters[1][2]); Eigen::Quaterniond Qj(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]); Eigen::Vector3d tic(parameters[2][0], parameters[2][1], parameters[2][2]); Eigen::Quaterniond qic(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]); double inv_dep_i = parameters[3][0]; Eigen::Vector3d pts_camera_i = pts_i / inv_dep_i; Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic; Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi; Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj); Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic); Eigen::Map residual(residuals); #ifdef UNIT_SPHERE_ERROR residual = tangent_base * (pts_camera_j.normalized() - pts_j.normalized()); #else double dep_j = pts_camera_j.z(); residual = (pts_camera_j / dep_j).head<2>() - pts_j.head<2>(); #endif residual = sqrt_info * residual; if (jacobians) { Eigen::Matrix3d Ri = Qi.toRotationMatrix(); Eigen::Matrix3d Rj = Qj.toRotationMatrix(); Eigen::Matrix3d ric = qic.toRotationMatrix(); Eigen::Matrix reduce(2, 3); #ifdef UNIT_SPHERE_ERROR double norm = pts_camera_j.norm(); Eigen::Matrix3d norm_jaco; double x1, x2, x3; x1 = pts_camera_j(0); x2 = pts_camera_j(1); x3 = pts_camera_j(2); norm_jaco << 1.0 / norm - x1 * x1 / pow(norm, 3), -x1 * x2 / pow(norm, 3), -x1 * x3 / pow(norm, 3), -x1 * x2 / pow(norm, 3), 1.0 / norm - x2 * x2 / pow(norm, 3), -x2 * x3 / pow(norm, 3), -x1 * x3 / pow(norm, 3), -x2 * x3 / pow(norm, 3), 1.0 / norm - x3 * x3 / pow(norm, 3); reduce = tangent_base * norm_jaco; #else reduce << 1. / dep_j, 0, -pts_camera_j(0) / (dep_j * dep_j), 0, 1. / dep_j, -pts_camera_j(1) / (dep_j * dep_j); #endif reduce = sqrt_info * reduce; if (jacobians[0]) { Eigen::Map> jacobian_pose_i(jacobians[0]); Eigen::Matrix jaco_i; jaco_i.leftCols<3>() = ric.transpose() * Rj.transpose(); jaco_i.rightCols<3>() = ric.transpose() * Rj.transpose() * Ri * -Utility::skewSymmetric(pts_imu_i); jacobian_pose_i.leftCols<6>() = reduce * jaco_i; jacobian_pose_i.rightCols<1>().setZero(); } if (jacobians[1]) { Eigen::Map> jacobian_pose_j(jacobians[1]); Eigen::Matrix jaco_j; jaco_j.leftCols<3>() = ric.transpose() * -Rj.transpose(); jaco_j.rightCols<3>() = ric.transpose() * Utility::skewSymmetric(pts_imu_j); jacobian_pose_j.leftCols<6>() = reduce * jaco_j; jacobian_pose_j.rightCols<1>().setZero(); } if (jacobians[2]) { Eigen::Map> jacobian_ex_pose(jacobians[2]); Eigen::Matrix jaco_ex; jaco_ex.leftCols<3>() = ric.transpose() * (Rj.transpose() * Ri - Eigen::Matrix3d::Identity()); Eigen::Matrix3d tmp_r = ric.transpose() * Rj.transpose() * Ri * ric; jaco_ex.rightCols<3>() = -tmp_r * Utility::skewSymmetric(pts_camera_i) + Utility::skewSymmetric(tmp_r * pts_camera_i) + Utility::skewSymmetric(ric.transpose() * (Rj.transpose() * (Ri * tic + Pi - Pj) - tic)); jacobian_ex_pose.leftCols<6>() = reduce * jaco_ex; jacobian_ex_pose.rightCols<1>().setZero(); } if (jacobians[3]) { Eigen::Map jacobian_feature(jacobians[3]); #if 1 jacobian_feature = reduce * ric.transpose() * Rj.transpose() * Ri * ric * pts_i * -1.0 / (inv_dep_i * inv_dep_i); #else jacobian_feature = reduce * ric.transpose() * Rj.transpose() * Ri * ric * pts_i; #endif } } sum_t += tic_toc.toc(); return true; } void ProjectionFactor::check(double **parameters) { double *res = new double[15]; double **jaco = new double *[4]; jaco[0] = new double[2 * 7]; jaco[1] = new double[2 * 7]; jaco[2] = new double[2 * 7]; jaco[3] = new double[2 * 1]; Evaluate(parameters, res, jaco); puts("check begins"); puts("my"); std::cout << Eigen::Map>(res).transpose() << std::endl << std::endl; std::cout << Eigen::Map>(jaco[0]) << std::endl << std::endl; std::cout << Eigen::Map>(jaco[1]) << std::endl << std::endl; std::cout << Eigen::Map>(jaco[2]) << std::endl << std::endl; std::cout << Eigen::Map(jaco[3]) << std::endl << std::endl; Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]); Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]); Eigen::Vector3d Pj(parameters[1][0], parameters[1][1], parameters[1][2]); Eigen::Quaterniond Qj(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]); Eigen::Vector3d tic(parameters[2][0], parameters[2][1], parameters[2][2]); Eigen::Quaterniond qic(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]); double inv_dep_i = parameters[3][0]; Eigen::Vector3d pts_camera_i = pts_i / inv_dep_i; Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic; Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi; Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj); Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic); Eigen::Vector2d residual; #ifdef UNIT_SPHERE_ERROR residual = tangent_base * (pts_camera_j.normalized() - pts_j.normalized()); #else double dep_j = pts_camera_j.z(); residual = (pts_camera_j / dep_j).head<2>() - pts_j.head<2>(); #endif residual = sqrt_info * residual; puts("num"); std::cout << residual.transpose() << std::endl; const double eps = 1e-6; Eigen::Matrix num_jacobian; for (int k = 0; k < 19; k++) { Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]); Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]); Eigen::Vector3d Pj(parameters[1][0], parameters[1][1], parameters[1][2]); Eigen::Quaterniond Qj(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]); Eigen::Vector3d tic(parameters[2][0], parameters[2][1], parameters[2][2]); Eigen::Quaterniond qic(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]); double inv_dep_i = parameters[3][0]; int a = k / 3, b = k % 3; Eigen::Vector3d delta = Eigen::Vector3d(b == 0, b == 1, b == 2) * eps; if (a == 0) Pi += delta; else if (a == 1) Qi = Qi * Utility::deltaQ(delta); else if (a == 2) Pj += delta; else if (a == 3) Qj = Qj * Utility::deltaQ(delta); else if (a == 4) tic += delta; else if (a == 5) qic = qic * Utility::deltaQ(delta); else if (a == 6) inv_dep_i += delta.x(); Eigen::Vector3d pts_camera_i = pts_i / inv_dep_i; Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic; Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi; Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj); Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic); Eigen::Vector2d tmp_residual; #ifdef UNIT_SPHERE_ERROR tmp_residual = tangent_base * (pts_camera_j.normalized() - pts_j.normalized()); #else double dep_j = pts_camera_j.z(); tmp_residual = (pts_camera_j / dep_j).head<2>() - pts_j.head<2>(); #endif tmp_residual = sqrt_info * tmp_residual; num_jacobian.col(k) = (tmp_residual - residual) / eps; } std::cout << num_jacobian << std::endl; } ================================================ FILE: vins_estimator/src/factor/projection_factor.h ================================================ #pragma once #include #include #include #include "../utility/utility.h" #include "../utility/tic_toc.h" #include "../utility/parameters.h" class ProjectionFactor : public ceres::SizedCostFunction<2, 7, 7, 7, 1> { public: ProjectionFactor(const Eigen::Vector3d &_pts_i, const Eigen::Vector3d &_pts_j); virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; void check(double **parameters); Eigen::Vector3d pts_i, pts_j; Eigen::Matrix tangent_base; static Eigen::Matrix2d sqrt_info; static double sum_t; }; ================================================ FILE: vins_estimator/src/factor/projection_td_factor.cpp ================================================ #include "projection_td_factor.h" Eigen::Matrix2d ProjectionTdFactor::sqrt_info; double ProjectionTdFactor::sum_t; ProjectionTdFactor::ProjectionTdFactor(const Eigen::Vector3d &_pts_i, const Eigen::Vector3d &_pts_j, const Eigen::Vector2d &_velocity_i, const Eigen::Vector2d &_velocity_j, const double _td_i, const double _td_j, const double _row_i, const double _row_j) : pts_i(_pts_i), pts_j(_pts_j), td_i(_td_i), td_j(_td_j) { velocity_i.x() = _velocity_i.x(); velocity_i.y() = _velocity_i.y(); velocity_i.z() = 0; velocity_j.x() = _velocity_j.x(); velocity_j.y() = _velocity_j.y(); velocity_j.z() = 0; row_i = _row_i - ROW / 2; row_j = _row_j - ROW / 2; #ifdef UNIT_SPHERE_ERROR Eigen::Vector3d b1, b2; Eigen::Vector3d a = pts_j.normalized(); Eigen::Vector3d tmp(0, 0, 1); if (a == tmp) tmp << 1, 0, 0; b1 = (tmp - a * (a.transpose() * tmp)).normalized(); b2 = a.cross(b1); tangent_base.block<1, 3>(0, 0) = b1.transpose(); tangent_base.block<1, 3>(1, 0) = b2.transpose(); #endif }; bool ProjectionTdFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const { TicToc tic_toc; Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]); Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]); Eigen::Vector3d Pj(parameters[1][0], parameters[1][1], parameters[1][2]); Eigen::Quaterniond Qj(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]); Eigen::Vector3d tic(parameters[2][0], parameters[2][1], parameters[2][2]); Eigen::Quaterniond qic(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]); double inv_dep_i = parameters[3][0]; double td = parameters[4][0]; Eigen::Vector3d pts_i_td, pts_j_td; pts_i_td = pts_i - (td - td_i + TR / ROW * row_i) * velocity_i; pts_j_td = pts_j - (td - td_j + TR / ROW * row_j) * velocity_j; Eigen::Vector3d pts_camera_i = pts_i_td / inv_dep_i; Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic; Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi; Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj); Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic); Eigen::Map residual(residuals); #ifdef UNIT_SPHERE_ERROR residual = tangent_base * (pts_camera_j.normalized() - pts_j_td.normalized()); #else double dep_j = pts_camera_j.z(); residual = (pts_camera_j / dep_j).head<2>() - pts_j_td.head<2>(); #endif residual = sqrt_info * residual; if (jacobians) { Eigen::Matrix3d Ri = Qi.toRotationMatrix(); Eigen::Matrix3d Rj = Qj.toRotationMatrix(); Eigen::Matrix3d ric = qic.toRotationMatrix(); Eigen::Matrix reduce(2, 3); #ifdef UNIT_SPHERE_ERROR double norm = pts_camera_j.norm(); Eigen::Matrix3d norm_jaco; double x1, x2, x3; x1 = pts_camera_j(0); x2 = pts_camera_j(1); x3 = pts_camera_j(2); norm_jaco << 1.0 / norm - x1 * x1 / pow(norm, 3), -x1 * x2 / pow(norm, 3), -x1 * x3 / pow(norm, 3), -x1 * x2 / pow(norm, 3), 1.0 / norm - x2 * x2 / pow(norm, 3), -x2 * x3 / pow(norm, 3), -x1 * x3 / pow(norm, 3), -x2 * x3 / pow(norm, 3), 1.0 / norm - x3 * x3 / pow(norm, 3); reduce = tangent_base * norm_jaco; #else reduce << 1. / dep_j, 0, -pts_camera_j(0) / (dep_j * dep_j), 0, 1. / dep_j, -pts_camera_j(1) / (dep_j * dep_j); #endif reduce = sqrt_info * reduce; if (jacobians[0]) { Eigen::Map> jacobian_pose_i(jacobians[0]); Eigen::Matrix jaco_i; jaco_i.leftCols<3>() = ric.transpose() * Rj.transpose(); jaco_i.rightCols<3>() = ric.transpose() * Rj.transpose() * Ri * -Utility::skewSymmetric(pts_imu_i); jacobian_pose_i.leftCols<6>() = reduce * jaco_i; jacobian_pose_i.rightCols<1>().setZero(); } if (jacobians[1]) { Eigen::Map> jacobian_pose_j(jacobians[1]); Eigen::Matrix jaco_j; jaco_j.leftCols<3>() = ric.transpose() * -Rj.transpose(); jaco_j.rightCols<3>() = ric.transpose() * Utility::skewSymmetric(pts_imu_j); jacobian_pose_j.leftCols<6>() = reduce * jaco_j; jacobian_pose_j.rightCols<1>().setZero(); } if (jacobians[2]) { Eigen::Map> jacobian_ex_pose(jacobians[2]); Eigen::Matrix jaco_ex; jaco_ex.leftCols<3>() = ric.transpose() * (Rj.transpose() * Ri - Eigen::Matrix3d::Identity()); Eigen::Matrix3d tmp_r = ric.transpose() * Rj.transpose() * Ri * ric; jaco_ex.rightCols<3>() = -tmp_r * Utility::skewSymmetric(pts_camera_i) + Utility::skewSymmetric(tmp_r * pts_camera_i) + Utility::skewSymmetric(ric.transpose() * (Rj.transpose() * (Ri * tic + Pi - Pj) - tic)); jacobian_ex_pose.leftCols<6>() = reduce * jaco_ex; jacobian_ex_pose.rightCols<1>().setZero(); } if (jacobians[3]) { Eigen::Map jacobian_feature(jacobians[3]); jacobian_feature = reduce * ric.transpose() * Rj.transpose() * Ri * ric * pts_i_td * -1.0 / (inv_dep_i * inv_dep_i); } if (jacobians[4]) { Eigen::Map jacobian_td(jacobians[4]); jacobian_td = reduce * ric.transpose() * Rj.transpose() * Ri * ric * velocity_i / inv_dep_i * -1.0 + sqrt_info * velocity_j.head(2); } } sum_t += tic_toc.toc(); return true; } void ProjectionTdFactor::check(double **parameters) { double *res = new double[2]; double **jaco = new double *[5]; jaco[0] = new double[2 * 7]; jaco[1] = new double[2 * 7]; jaco[2] = new double[2 * 7]; jaco[3] = new double[2 * 1]; jaco[4] = new double[2 * 1]; Evaluate(parameters, res, jaco); puts("check begins"); puts("my"); std::cout << Eigen::Map>(res).transpose() << std::endl << std::endl; std::cout << Eigen::Map>(jaco[0]) << std::endl << std::endl; std::cout << Eigen::Map>(jaco[1]) << std::endl << std::endl; std::cout << Eigen::Map>(jaco[2]) << std::endl << std::endl; std::cout << Eigen::Map(jaco[3]) << std::endl << std::endl; std::cout << Eigen::Map(jaco[4]) << std::endl << std::endl; Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]); Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]); Eigen::Vector3d Pj(parameters[1][0], parameters[1][1], parameters[1][2]); Eigen::Quaterniond Qj(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]); Eigen::Vector3d tic(parameters[2][0], parameters[2][1], parameters[2][2]); Eigen::Quaterniond qic(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]); double inv_dep_i = parameters[3][0]; double td = parameters[4][0]; Eigen::Vector3d pts_i_td, pts_j_td; pts_i_td = pts_i - (td - td_i + TR / ROW * row_i) * velocity_i; pts_j_td = pts_j - (td - td_j + TR / ROW * row_j) * velocity_j; Eigen::Vector3d pts_camera_i = pts_i_td / inv_dep_i; Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic; Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi; Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj); Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic); Eigen::Vector2d residual; #ifdef UNIT_SPHERE_ERROR residual = tangent_base * (pts_camera_j.normalized() - pts_j_td.normalized()); #else double dep_j = pts_camera_j.z(); residual = (pts_camera_j / dep_j).head<2>() - pts_j_td.head<2>(); #endif residual = sqrt_info * residual; puts("num"); std::cout << residual.transpose() << std::endl; const double eps = 1e-6; Eigen::Matrix num_jacobian; for (int k = 0; k < 20; k++) { Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]); Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]); Eigen::Vector3d Pj(parameters[1][0], parameters[1][1], parameters[1][2]); Eigen::Quaterniond Qj(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]); Eigen::Vector3d tic(parameters[2][0], parameters[2][1], parameters[2][2]); Eigen::Quaterniond qic(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]); double inv_dep_i = parameters[3][0]; double td = parameters[4][0]; int a = k / 3, b = k % 3; Eigen::Vector3d delta = Eigen::Vector3d(b == 0, b == 1, b == 2) * eps; if (a == 0) Pi += delta; else if (a == 1) Qi = Qi * Utility::deltaQ(delta); else if (a == 2) Pj += delta; else if (a == 3) Qj = Qj * Utility::deltaQ(delta); else if (a == 4) tic += delta; else if (a == 5) qic = qic * Utility::deltaQ(delta); else if (a == 6 && b == 0) inv_dep_i += delta.x(); else if (a == 6 && b == 1) td += delta.y(); Eigen::Vector3d pts_i_td, pts_j_td; pts_i_td = pts_i - (td - td_i + TR / ROW * row_i) * velocity_i; pts_j_td = pts_j - (td - td_j + TR / ROW * row_j) * velocity_j; Eigen::Vector3d pts_camera_i = pts_i_td / inv_dep_i; Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic; Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi; Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj); Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic); Eigen::Vector2d tmp_residual; #ifdef UNIT_SPHERE_ERROR tmp_residual = tangent_base * (pts_camera_j.normalized() - pts_j_td.normalized()); #else double dep_j = pts_camera_j.z(); tmp_residual = (pts_camera_j / dep_j).head<2>() - pts_j_td.head<2>(); #endif tmp_residual = sqrt_info * tmp_residual; num_jacobian.col(k) = (tmp_residual - residual) / eps; } std::cout << num_jacobian << std::endl; } ================================================ FILE: vins_estimator/src/factor/projection_td_factor.h ================================================ #pragma once #include #include #include #include "../utility/utility.h" #include "../utility/tic_toc.h" #include "../utility/parameters.h" class ProjectionTdFactor : public ceres::SizedCostFunction<2, 7, 7, 7, 1, 1> { public: ProjectionTdFactor(const Eigen::Vector3d &_pts_i, const Eigen::Vector3d &_pts_j, const Eigen::Vector2d &_velocity_i, const Eigen::Vector2d &_velocity_j, const double _td_i, const double _td_j, const double _row_i, const double _row_j); virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; void check(double **parameters); Eigen::Vector3d pts_i, pts_j; Eigen::Vector3d velocity_i, velocity_j; double td_i, td_j; Eigen::Matrix tangent_base; double row_i, row_j; static Eigen::Matrix2d sqrt_info; static double sum_t; }; ================================================ FILE: vins_estimator/src/feature_manager/feature_manager.cpp ================================================ #include "feature_manager.h" #include #include #include #include int FeaturePerId::endFrame() { return start_frame + feature_per_frame.size() - 1; } FeatureManager::FeatureManager(Matrix3d _Rs[]) : Rs(_Rs) { for (auto &i : ric) i.setIdentity(); } void FeatureManager::setRic(Matrix3d _ric[]) { for (int i = 0; i < NUM_OF_CAM; i++) { ric[i] = _ric[i]; } } void FeatureManager::clearState() { feature.clear(); } int FeatureManager::getFeatureCount() { int cnt = 0; for (auto &it : feature) { if (it.is_dynamic) { continue; } it.used_num = it.feature_per_frame.size(); // if (it.used_num >= 4) // cnt++; if (it.used_num >= 2 && it.start_frame < WINDOW_SIZE - 2) { cnt++; } } return cnt; } void FeatureManager::inputDepth(const cv::Mat &_depth_img) { depth_img = _depth_img; } bool FeatureManager::addFeatureCheckParallax(int frame_count, map> &image, double td) { ROS_DEBUG("input feature: %d", (int)image.size()); ROS_DEBUG("num of feature: %d", getFeatureCount()); double parallax_sum = 0; int parallax_num = 0; last_track_num = 0; for (auto iter = image.begin(), iter_next = image.begin(); iter != image.end(); iter = iter_next) { ++iter_next; unsigned short pt_depth_mm = depth_img.at((int)iter->second(4), (int)iter->second(3)); double pt_depth_m = pt_depth_mm / 1000.0; if (0 < pt_depth_m && pt_depth_m < DEPTH_MIN_DIST) { image.erase(iter); continue; } // std::find_if: http://c.biancheng.net/view/571.html int feature_id = iter->first; auto it = find_if(feature.begin(), feature.end(), [feature_id](const FeaturePerId &it) { return it.feature_id == feature_id; }); if (it == feature.end()) { feature.emplace_back(FeaturePerId(feature_id, frame_count)); feature.back().feature_per_frame.emplace_back( FeaturePerFrame(iter->second, td, pt_depth_m)); } else if (it->feature_id == feature_id) { it->feature_per_frame.emplace_back(FeaturePerFrame(iter->second, td, pt_depth_m)); last_track_num++; } } if (frame_count < 2 || last_track_num < 20) return true; for (auto &it_per_id : feature) { if (it_per_id.start_frame <= frame_count - 2 && it_per_id.start_frame + int(it_per_id.feature_per_frame.size()) - 1 >= frame_count - 1) { parallax_sum += compensatedParallax2(it_per_id, frame_count); parallax_num++; } } if (parallax_num == 0) { return true; } else { ROS_DEBUG("parallax_sum: %lf, parallax_num: %d", parallax_sum, parallax_num); ROS_DEBUG("current parallax: %lf", parallax_sum / parallax_num * FOCAL_LENGTH); return parallax_sum / parallax_num >= MIN_PARALLAX; } } void FeatureManager::debugShow() { ROS_DEBUG("debug show"); for (auto &it : feature) { ROS_ASSERT(!it.feature_per_frame.empty()); ROS_ASSERT(it.start_frame >= 0); ROS_ASSERT(it.used_num >= 0); ROS_DEBUG("%d,%d,%d ", it.feature_id, it.used_num, it.start_frame); int sum = 0; for (auto &j : it.feature_per_frame) { ROS_DEBUG("%d,", int(j.is_used)); sum += j.is_used; printf("(%lf,%lf) ", j.point(0), j.point(1)); } ROS_ASSERT(it.used_num == sum); } } vector> FeatureManager::getCorresponding(int frame_count_l, int frame_count_r) { vector> corres; for (auto &it : feature) { if (it.start_frame <= frame_count_l && it.endFrame() >= frame_count_r) { Vector3d a = Vector3d::Zero(), b = Vector3d::Zero(); int idx_l = frame_count_l - it.start_frame; int idx_r = frame_count_r - it.start_frame; a = it.feature_per_frame[idx_l].point; b = it.feature_per_frame[idx_r].point; corres.emplace_back(a, b); } } return corres; } vector> FeatureManager::getCorrespondingWithDepth(int frame_count_l, int frame_count_r) { vector> corres; for (auto &it : feature) { if (it.start_frame <= frame_count_l && it.endFrame() >= frame_count_r) { Vector3d a = Vector3d::Zero(), b = Vector3d::Zero(); int idx_l = frame_count_l - it.start_frame; int idx_r = frame_count_r - it.start_frame; double depth_a = it.feature_per_frame[idx_l].depth; if (depth_a < 0.1 || depth_a > 10) // max and min measurement continue; double depth_b = it.feature_per_frame[idx_r].depth; if (depth_b < 0.1 || depth_b > 10) // max and min measurement continue; a = it.feature_per_frame[idx_l].point; b = it.feature_per_frame[idx_r].point; a = a * depth_a; b = b * depth_b; corres.emplace_back(a, b); } } return corres; } void FeatureManager::setDepth(const VectorXd &x) { int feature_index = -1; for (auto &it_per_id : feature) { if (it_per_id.is_dynamic) { continue; } it_per_id.used_num = it_per_id.feature_per_frame.size(); // if (it_per_id.used_num < 4) // continue; if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2)) continue; it_per_id.estimated_depth = 1.0 / x(++feature_index); // ROS_INFO("feature id %d , start_frame %d, depth %f ", // it_per_id->feature_id, it_per_id-> start_frame, // it_per_id->estimated_depth); if (it_per_id.estimated_depth < 0) { it_per_id.solve_flag = 2; } else it_per_id.solve_flag = 1; } } void FeatureManager::removeFailures() { for (auto it = feature.begin(), it_next = feature.begin(); it != feature.end(); it = it_next) { it_next++; if (it->solve_flag == 2) feature.erase(it); } } void FeatureManager::removeFailuresOutliers(Vector3d Ps[], Matrix3d _Rs[], Vector3d tic[], Matrix3d _ric[]) { for (auto it = feature.begin(), it_next = feature.begin(); it != feature.end(); it = it_next) { it_next++; if (it->solve_flag == 2) { feature.erase(it); } else { if (it->is_dynamic) { continue; } double err = 0; int errCnt = 0; it->used_num = it->feature_per_frame.size(); if (it->used_num < 4) continue; int imu_i = it->start_frame, imu_j = imu_i - 1; Vector3d pts_i = it->feature_per_frame[0].point; double depth = it->estimated_depth; for (auto &it_per_frame : it->feature_per_frame) { imu_j++; if (imu_i != imu_j) { Vector3d pts_j = it_per_frame.point; Vector3d pts_w = _Rs[imu_i] * (_ric[0] * (depth * pts_i) + tic[0]) + Ps[imu_i]; Vector3d pts_cj = _ric[0].transpose() * (_Rs[imu_j].transpose() * (pts_w - Ps[imu_j]) - tic[0]); Vector2d residual = (pts_cj / pts_cj.z()).head<2>() - pts_j.head<2>(); double rx = residual.x(); double ry = residual.y(); double tmp_error = sqrt(rx * rx + ry * ry); err += tmp_error; errCnt++; // printf("tmp_error %f\n", FOCAL_LENGTH / 1.5 * tmp_error); } } double ave_err = err / errCnt; if (ave_err * FOCAL_LENGTH > 10) feature.erase(it); } } } void FeatureManager::clearDepth(const VectorXd &x) { int feature_index = -1; for (auto &it_per_id : feature) { if (it_per_id.is_dynamic) { continue; } it_per_id.used_num = it_per_id.feature_per_frame.size(); // if (it_per_id.used_num < 4) // continue; if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2)) continue; it_per_id.estimated_depth = 1.0 / x(++feature_index); } } VectorXd FeatureManager::getDepthVector() { VectorXd dep_vec(getFeatureCount()); int feature_index = -1; for (auto &it_per_id : feature) { if (it_per_id.is_dynamic) { continue; } it_per_id.used_num = it_per_id.feature_per_frame.size(); // if (it_per_id.used_num < 4) // continue; if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2)) continue; #if 1 dep_vec(++feature_index) = 1. / it_per_id.estimated_depth; #else dep_vec(++feature_index) = it_per_id->estimated_depth; #endif } return dep_vec; } void FeatureManager::triangulate(Vector3d Ps[], Vector3d tic[], Matrix3d _ric[]) { for (auto &it_per_id : feature) { it_per_id.used_num = it_per_id.feature_per_frame.size(); if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2)) continue; // if (it_per_id.used_num < 4) // continue; // if (!(it_per_id.used_num >depth_corner > 0) // continue; int imu_i = it_per_id.start_frame, imu_j = imu_i - 1; ROS_ASSERT(NUM_OF_CAM == 1); Eigen::MatrixXd svd_A(2 * it_per_id.feature_per_frame.size(), 4); int svd_idx = 0; Eigen::Matrix P0; Eigen::Vector3d t0 = Ps[imu_i] + Rs[imu_i] * tic[0]; Eigen::Matrix3d R0 = Rs[imu_i] * _ric[0]; P0.leftCols<3>() = Eigen::Matrix3d::Identity(); P0.rightCols<1>() = Eigen::Vector3d::Zero(); for (auto &it_per_frame : it_per_id.feature_per_frame) { imu_j++; Eigen::Vector3d t1 = Ps[imu_j] + Rs[imu_j] * tic[0]; Eigen::Matrix3d R1 = Rs[imu_j] * _ric[0]; Eigen::Vector3d t = R0.transpose() * (t1 - t0); Eigen::Matrix3d R = R0.transpose() * R1; Eigen::Matrix P; P.leftCols<3>() = R.transpose(); P.rightCols<1>() = -R.transpose() * t; Eigen::Vector3d f = it_per_frame.point.normalized(); svd_A.row(svd_idx++) = f[0] * P.row(2) - f[2] * P.row(0); svd_A.row(svd_idx++) = f[1] * P.row(2) - f[2] * P.row(1); if (imu_i == imu_j) continue; } ROS_ASSERT(svd_idx == svd_A.rows()); Eigen::Vector4d svd_V = Eigen::JacobiSVD(svd_A, Eigen::ComputeThinV).matrixV().rightCols<1>(); double svd_method = svd_V[2] / svd_V[3]; // it_per_id->estimated_depth = -b / A; // it_per_id->estimated_depth = svd_V[2] / svd_V[3]; it_per_id.estimated_depth = svd_method; // it_per_id->estimated_depth = INIT_DEPTH; it_per_id.estimate_flag = 2; if (it_per_id.estimated_depth < 0.1) { it_per_id.estimated_depth = INIT_DEPTH; it_per_id.estimate_flag = 0; } } } void FeatureManager::triangulateWithDepth(Vector3d _Ps[], Vector3d _tic[], Matrix3d _ric[]) { for (auto &it_per_id : feature) { if (it_per_id.estimated_depth > 0) continue; if (it_per_id.is_dynamic) { continue; } it_per_id.used_num = it_per_id.feature_per_frame.size(); if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2)) continue; int imu_i = it_per_id.start_frame; Eigen::Vector3d tr = _Ps[imu_i] + Rs[imu_i] * _tic[0]; Eigen::Matrix3d Rr = Rs[imu_i] * _ric[0]; vector verified_depths; int no_depth_num = 0; vector rough_depths; /** * @brief * 对同一id特征点进行深度交叉验证，并将重投影误差小于阈值的点的，在初始帧上的深度做平均作为估计的深度 */ for (int k = 0; k < (int)it_per_id.feature_per_frame.size(); k++) { if (it_per_id.feature_per_frame[k].depth == 0) { no_depth_num++; continue; } Eigen::Vector3d t0 = _Ps[imu_i + k] + Rs[imu_i + k] * _tic[0]; Eigen::Matrix3d R0 = Rs[imu_i + k] * _ric[0]; Eigen::Vector3d point0(it_per_id.feature_per_frame[k].point * it_per_id.feature_per_frame[k].depth); // transform to reference frame Eigen::Vector3d t2r = Rr.transpose() * (t0 - tr); Eigen::Matrix3d R2r = Rr.transpose() * R0; for (int j = 0; j < (int)it_per_id.feature_per_frame.size(); j++) { if (k == j) continue; Eigen::Vector3d t1 = _Ps[imu_i + j] + Rs[imu_i + j] * _tic[0]; Eigen::Matrix3d R1 = Rs[imu_i + j] * _ric[0]; Eigen::Vector3d t20 = R0.transpose() * (t1 - t0); Eigen::Matrix3d R20 = R0.transpose() * R1; Eigen::Vector3d point1_projected = R20.transpose() * point0 - R20.transpose() * t20; Eigen::Vector2d point1_2d(it_per_id.feature_per_frame[j].point.x(), it_per_id.feature_per_frame[j].point.y()); Eigen::Vector2d residual = point1_2d - Vector2d(point1_projected.x() / point1_projected.z(), point1_projected.y() / point1_projected.z()); if (residual.norm() < 10.0 / 460) { // this can also be adjust to improve performance Eigen::Vector3d point_r = R2r * point0 + t2r; if (it_per_id.feature_per_frame[k].depth > DEPTH_MAX_DIST) { rough_depths.push_back(point_r.z()); } else { verified_depths.push_back(point_r.z()); } } } } if (verified_depths.empty()) { if (rough_depths.empty()) { if (no_depth_num == it_per_id.feature_per_frame.size()) { int imu_j = imu_i - 1; ROS_ASSERT(NUM_OF_CAM == 1); Eigen::MatrixXd svd_A(2 * it_per_id.feature_per_frame.size(), 4); int svd_idx = 0; Eigen::Matrix P0; Eigen::Vector3d t0 = _Ps[imu_i] + Rs[imu_i] * _tic[0]; Eigen::Matrix3d R0 = Rs[imu_i] * ric[0]; P0.leftCols<3>() = Eigen::Matrix3d::Identity(); P0.rightCols<1>() = Eigen::Vector3d::Zero(); for (auto &it_per_frame : it_per_id.feature_per_frame) { imu_j++; Eigen::Vector3d t1 = _Ps[imu_j] + Rs[imu_j] * _tic[0]; Eigen::Matrix3d R1 = Rs[imu_j] * ric[0]; Eigen::Vector3d t = R0.transpose() * (t1 - t0); Eigen::Matrix3d R = R0.transpose() * R1; Eigen::Matrix P; P.leftCols<3>() = R.transpose(); P.rightCols<1>() = -R.transpose() * t; Eigen::Vector3d f = it_per_frame.point.normalized(); svd_A.row(svd_idx++) = f[0] * P.row(2) - f[2] * P.row(0); svd_A.row(svd_idx++) = f[1] * P.row(2) - f[2] * P.row(1); if (imu_i == imu_j) continue; } ROS_ASSERT(svd_idx == svd_A.rows()); Eigen::Vector4d svd_V = Eigen::JacobiSVD(svd_A, Eigen::ComputeThinV) .matrixV() .rightCols<1>(); double svd_method = svd_V[2] / svd_V[3]; if (svd_method < DEPTH_MIN_DIST) { it_per_id.estimated_depth = DEPTH_MAX_DIST; it_per_id.estimate_flag = 2; } else { it_per_id.estimated_depth = svd_method; it_per_id.estimate_flag = 2; } } else { continue; } } else { double depth_sum = std::accumulate(std::begin(rough_depths), std::end(rough_depths), 0.0); double depth_ave = depth_sum / rough_depths.size(); it_per_id.estimated_depth = depth_ave; it_per_id.estimate_flag = 0; } } else { double depth_sum = std::accumulate(std::begin(verified_depths), std::end(verified_depths), 0.0); double depth_ave = depth_sum / verified_depths.size(); it_per_id.estimated_depth = depth_ave; it_per_id.estimate_flag = 1; } if (it_per_id.estimated_depth < 0.1) { it_per_id.estimated_depth = INIT_DEPTH; it_per_id.estimate_flag = 0; } } } bool FeatureManager::solvePoseByPnP(Eigen::Matrix3d &R, Eigen::Vector3d &P, vector &pts2D, vector &pts3D) { Eigen::Matrix3d R_initial; Eigen::Vector3d P_initial; // w_T_cam ---> cam_T_w R_initial = R.inverse(); // R_c_w from world to cam tzhang P_initial = -(R_initial * P); // P_c_w // printf("pnp size %d \n",(int)pts2D.size() ); if (int(pts2D.size()) < 4) { printf("feature tracking not enough, please slowly move you device! \n"); return false; } cv::Mat r, rvec, t, D, tmp_r; cv::eigen2cv(R_initial, tmp_r); cv::Rodrigues(tmp_r, rvec); //旋转矩阵到旋转向量 tzhang cv::eigen2cv(P_initial, t); cv::Mat K = (cv::Mat_(3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 1); bool pnp_succ; pnp_succ = cv::solvePnP(pts3D, pts2D, K, D, rvec, t, 1); // pnp_succ = solvePnPRansac(pts3D, pts2D, K, D, rvec, t, true, 100, 8.0 / // focalLength, 0.99, inliers); if (!pnp_succ) { printf("pnp failed ! \n"); return false; } cv::Rodrigues(rvec, r); //旋转向量到旋转矩阵 tzhang // cout << "r " << endl << r << endl; Eigen::MatrixXd R_pnp; cv::cv2eigen(r, R_pnp); Eigen::MatrixXd T_pnp; cv::cv2eigen(t, T_pnp); // cam_T_w ---> w_T_cam R = R_pnp.transpose(); // R_w_c P = R * (-T_pnp); return true; } void FeatureManager::initFramePoseByPnP(int frameCnt, Vector3d Ps[], Matrix3d Rs[], Vector3d tic[], Matrix3d ric[]) { if (frameCnt > 0) //对第一帧图像不做处理；因为此时路标点还未三角化，需要利用第一帧双目图像，进行路标点三角化 // tzhang { vector pts2D; vector pts3D; for (auto &it_per_id : feature) //遍历每个路标点 tzhang { if (it_per_id.estimated_depth > 0) //该路标点完成了初始化，使用初始化完成的路标点，获取3D-2D点对 // tzhang { int index = frameCnt - it_per_id.start_frame; if ((int)it_per_id.feature_per_frame.size() >= index + 1) // tzhang // 该路标点从start_frame图像帧到frameCnt对应的图像帧都能被观测到 { //路标点在IMU坐标系坐标，第一次看到该路标点的图像帧时刻 // tzhang Vector3d ptsInCam = ric[0] * (it_per_id.feature_per_frame[0].point * it_per_id.estimated_depth) + tic[0]; // 路标点在世界坐标系下的坐标 Vector3d ptsInWorld = Rs[it_per_id.start_frame] * ptsInCam + Ps[it_per_id.start_frame]; cv::Point3f point3d(ptsInWorld.x(), ptsInWorld.y(), ptsInWorld.z()); //世界坐标系下三维坐标 tzhang cv::Point2f point2d(it_per_id.feature_per_frame[index].point.x(), it_per_id.feature_per_frame[index].point.y()); pts3D.push_back(point3d); pts2D.push_back(point2d); } } } Eigen::Matrix3d RCam; Eigen::Vector3d PCam; // trans to w_T_cam // 以上一帧图像与世界坐标系之间的位姿变换作为后续PnP求解的初值 tzhang RCam = Rs[frameCnt - 1] * ric[0]; // R_w_c PCam = Rs[frameCnt - 1] * tic[0] + Ps[frameCnt - 1]; if (solvePoseByPnP(RCam, PCam, pts2D, pts3D)) { // trans to w_T_imu Rs[frameCnt] = RCam * ric[0].transpose(); // R_w_i = R_w_c*R_c_i Ps[frameCnt] = -RCam * ric[0].transpose() * tic[0] + PCam; } } } void FeatureManager::removeOutlier(set &outlierIndex) { std::set::iterator itSet; for (auto it = feature.begin(), it_next = feature.begin(); it != feature.end(); it = it_next) { it_next++; int index = it->feature_id; itSet = outlierIndex.find(index); if (itSet != outlierIndex.end()) { feature.erase(it); // printf("remove outlier %d \n", index); } } } void FeatureManager::removeBackShiftDepth(Eigen::Matrix3d marg_R, Eigen::Vector3d marg_P, Eigen::Matrix3d new_R, Eigen::Vector3d new_P) { for (auto it = feature.begin(), it_next = feature.begin(); it != feature.end(); it = it_next) { it_next++; if (it->start_frame != 0) it->start_frame--; else { Eigen::Vector3d uv_i = it->feature_per_frame[0].point; it->feature_per_frame.erase(it->feature_per_frame.begin()); if (it->feature_per_frame.size() < 2) { feature.erase(it); continue; } else { Eigen::Vector3d pts_i = uv_i * it->estimated_depth; Eigen::Vector3d w_pts_i = marg_R * pts_i + marg_P; Eigen::Vector3d pts_j = new_R.transpose() * (w_pts_i - new_P); double dep_j = pts_j(2); if (dep_j > 0) it->estimated_depth = dep_j; else it->estimated_depth = INIT_DEPTH; } } } } void FeatureManager::removeBack() { for (auto it = feature.begin(), it_next = feature.begin(); it != feature.end(); it = it_next) { it_next++; if (it->start_frame != 0) it->start_frame--; else { it->feature_per_frame.erase(it->feature_per_frame.begin()); if (it->feature_per_frame.size() == 0) feature.erase(it); } } } void FeatureManager::removeFront(int frame_count) { for (auto it = feature.begin(), it_next = feature.begin(); it != feature.end(); it = it_next) { it_next++; if (it->start_frame == frame_count) { it->start_frame--; } else { int j = WINDOW_SIZE - 1 - it->start_frame; if (it->endFrame() < frame_count - 1) continue; it->feature_per_frame.erase(it->feature_per_frame.begin() + j); if (it->feature_per_frame.size() == 0) feature.erase(it); } } } double FeatureManager::compensatedParallax2(const FeaturePerId &it_per_id, int frame_count) { // check the second last frame is keyframe or not // parallax betwwen seconde last frame and third last frame const FeaturePerFrame &frame_i = it_per_id.feature_per_frame[frame_count - 2 - it_per_id.start_frame]; const FeaturePerFrame &frame_j = it_per_id.feature_per_frame[frame_count - 1 - it_per_id.start_frame]; double ans = 0; Vector3d p_j = frame_j.point; double u_j = p_j(0); double v_j = p_j(1); Vector3d p_i = frame_i.point; Vector3d p_i_comp; // int r_i = frame_count - 2; // int r_j = frame_count - 1; // p_i_comp = ric[camera_id_j].transpose() * Rs[r_j].transpose() * Rs[r_i] * // ric[camera_id_i] * p_i; p_i_comp = p_i; double dep_i = p_i(2); double u_i = p_i(0) / dep_i; double v_i = p_i(1) / dep_i; double du = u_i - u_j, dv = v_i - v_j; double dep_i_comp = p_i_comp(2); double u_i_comp = p_i_comp(0) / dep_i_comp; double v_i_comp = p_i_comp(1) / dep_i_comp; double du_comp = u_i_comp - u_j, dv_comp = v_i_comp - v_j; ans = max(ans, sqrt(min(du * du + dv * dv, du_comp * du_comp + dv_comp * dv_comp))); return ans; } ================================================ FILE: vins_estimator/src/feature_manager/feature_manager.h ================================================ #ifndef FEATURE_MANAGER_H #define FEATURE_MANAGER_H #include #include #include #include using namespace std; #include using namespace Eigen; #include #include #include #include "../utility/parameters.h" #include "../utility/tic_toc.h" struct DynamicObject { int x_center; int y_center; int x_vel; int y_vel; int x_weight_vel; int y_weight_vel; int no_update_times; bool is_update; int x1; int y1; int x2; int y2; }; class FeaturePerFrame { public: FeaturePerFrame(Eigen::Matrix _point, double td, double _depth) { point.x() = _point(0); point.y() = _point(1); point.z() = _point(2); uv.x() = _point(3); uv.y() = _point(4); velocity.x() = _point(5); velocity.y() = _point(6); depth = _depth; cur_td = td; } double cur_td; Vector3d point; Vector2d uv; Vector2d velocity; double z{}; bool is_used{}; MatrixXd A; VectorXd b; double depth; }; class FeaturePerId { public: const int feature_id; int start_frame; vector feature_per_frame; int used_num; bool is_dynamic; double estimated_depth; int estimate_flag; // 0 initial; 1 by depth image; 2 by triangulate int solve_flag; // 0 haven't solve yet; 1 solve succ; 2 solve fail; FeaturePerId(int _feature_id, int _start_frame) : feature_id(_feature_id), start_frame(_start_frame), used_num(0), is_dynamic(false), estimated_depth(-1.0), estimate_flag(0), solve_flag(0) { } int endFrame(); }; class FeatureManager { public: explicit FeatureManager(Matrix3d Rs[]); void setRic(Matrix3d _ric[]); void clearState(); int getFeatureCount(); bool addFeatureCheckParallax(int frame_count, map> &image, double td); void debugShow(); vector> getCorresponding(int frame_count_l, int frame_count_r); vector> getCorrespondingWithDepth(int frame_count_l, int frame_count_r); // void updateDepth(const VectorXd &x); void setDepth(const VectorXd &x); void removeFailures(); void removeFailuresOutliers(Vector3d Ps[], Matrix3d Rs[], Vector3d tic[], Matrix3d _ric[]); void clearDepth(const VectorXd &x); VectorXd getDepthVector(); void triangulate(Vector3d Ps[], Vector3d tic[], Matrix3d _ric[]); void triangulateWithDepth(Vector3d Ps[], Vector3d _tic[], Matrix3d _ric[]); void initFramePoseByPnP(int frameCnt, Vector3d Ps[], Matrix3d Rs[], Vector3d tic[], Matrix3d ric[]); bool solvePoseByPnP(Eigen::Matrix3d &R_initial, Eigen::Vector3d &P_initial, vector &pts2D, vector &pts3D); void removeBackShiftDepth(Eigen::Matrix3d marg_R, Eigen::Vector3d marg_P, Eigen::Matrix3d new_R, Eigen::Vector3d new_P); void removeBack(); void removeFront(int frame_count); void removeOutlier(set &outlierIndex); void inputDepth(const cv::Mat &_depth_img); void seedFilling(int x, int y, unsigned short last_depth, unsigned short _object_id); list feature; // cl:Lists将元素按顺序储存在链表中. 与 向量(vectors)相比, // 它允许快速的插入和删除，但是随机访问却比较慢. int last_track_num{}; cv::Mat depth_img; private: double compensatedParallax2(const FeaturePerId &it_per_id, int frame_count); const Matrix3d *Rs; Matrix3d ric[NUM_OF_CAM]; }; #endif ================================================ FILE: vins_estimator/src/feature_tracker/ThreadPool.h ================================================ #ifndef THREAD_POOL_H #define THREAD_POOL_H #include #include #include #include #include #include #include #include #include class ThreadPool { public: ThreadPool(size_t); template auto enqueue(F &&f, Args &&...args) -> std::future::type>; ~ThreadPool(); // void wait_workers(); private: // need to keep track of threads so we can join them std::vector workers; // the task queue std::queue> tasks; // synchronization std::mutex queue_mutex; std::condition_variable condition; bool stop; }; // the constructor just launches some amount of workers inline ThreadPool::ThreadPool(size_t threads) : stop(false) { for (size_t i = 0; i < threads; ++i) workers.emplace_back( [this] { for (;;) { std::function task; { std::unique_lock lock(this->queue_mutex); this->condition.wait(lock, [this] { return this->stop || !this->tasks.empty(); }); if (this->stop && this->tasks.empty()) return; task = std::move(this->tasks.front()); this->tasks.pop(); } task(); } }); } // add new work item to the pool template auto ThreadPool::enqueue(F &&f, Args &&...args) -> std::future::type> { using return_type = typename std::result_of::type; auto task = std::make_shared>( std::bind(std::forward(f), std::forward(args)...)); std::future res = task->get_future(); { std::unique_lock lock(queue_mutex); // don't allow enqueueing after stopping the pool if (stop) throw std::runtime_error("enqueue on stopped ThreadPool"); tasks.emplace([task]() { (*task)(); }); } condition.notify_one(); return res; } // the destructor joins all threads inline ThreadPool::~ThreadPool() { { std::unique_lock lock(queue_mutex); stop = true; } condition.notify_all(); for (std::thread &worker : workers) worker.join(); } // inline void ThreadPool::wait_workers() { // for (std::thread &worker : workers) // worker.join(); // } #endif ================================================ FILE: vins_estimator/src/feature_tracker/feature_tracker.cpp ================================================ #include "feature_tracker.h" #include #include #include #include #include #include void reduceVector(vector &v, vector status) { int j = 0; for (int i = 0; i < int(v.size()); i++) if (status[i]) v[j++] = v[i]; v.resize(j); } void reduceVector(vector &v, vector status) { int j = 0; for (int i = 0; i < int(v.size()); i++) if (status[i]) v[j++] = v[i]; v.resize(j); } FeatureTracker::FeatureTracker() { p_fast_feature_detector = cv::FastFeatureDetector::create(); n_id = 0; } void FeatureTracker::initGridsDetector() { pool = std::make_shared(NUM_THREADS); grids_detector_img = cv::Mat(ROW, COL, CV_8UC1, cv::Scalar(0)); grid_height = (int)(ROW / NUM_GRID_ROWS); grid_width = (int)(COL / NUM_GRID_COLS); grid_res_height = ROW - (NUM_GRID_ROWS - 1) * grid_height; grid_res_width = COL - (NUM_GRID_COLS - 1) * grid_width; if (grids_rect.empty()) { for (int i = 0; i < NUM_GRID_ROWS; i++) for (int j = 0; j < NUM_GRID_COLS; j++) { cv::Rect rect; if (i == 0) { if (j == 0) rect = cv::Rect(0, 0, grid_width + 3, grid_height + 3); else if (j > 0 && j < NUM_GRID_COLS - 1) rect = cv::Rect(j * grid_width - 3, 0, grid_width + 6, grid_height + 3); else rect = cv::Rect(j * grid_width - 3, 0, grid_res_width + 3, grid_height + 3); } else if (i > 0 && i < NUM_GRID_ROWS - 1) { if (j == 0) rect = cv::Rect(0, i * grid_height - 3, grid_width + 3, grid_height + 6); else if (j > 0 && j < NUM_GRID_COLS - 1) rect = cv::Rect(j * grid_width - 3, i * grid_height - 3, grid_width + 6, grid_height + 6); else rect = cv::Rect(j * grid_width - 3, i * grid_height - 3, grid_res_width + 3, grid_height + 6); } else { if (j == 0) rect = cv::Rect(0, i * grid_height - 3, grid_width + 3, grid_res_height + 3); else if (j > 0 && j < NUM_GRID_COLS - 1) rect = cv::Rect(j * grid_width - 3, i * grid_height - 3, grid_width + 6, grid_res_height + 3); else rect = cv::Rect(j * grid_width - 3, i * grid_height - 3, grid_res_width + 3, grid_res_height + 3); } grids_rect.emplace_back(rect); grids_track_num.emplace_back(0); grids_texture_status.emplace_back(true); } } grids_threshold = (int)(MAX_CNT / grids_rect.size()); ROS_ASSERT_MSG(grids_threshold > 0, "Too many grids! \n'max_cnt'(%d) is supposed to be bigger than " "'num_grid_rows(%d)' x 'num_grid_cols(%d)' = %d!\nPlease reduce the " "'num_grid_rows' or 'num_grid_cols'!", MAX_CNT, NUM_GRID_ROWS, NUM_GRID_COLS, NUM_GRID_ROWS * NUM_GRID_COLS); } bool FeatureTracker::inBorder(const cv::Point2f &pt) { const int BORDER_SIZE = 1; int img_x = cvRound(pt.x); int img_y = cvRound(pt.y); return BORDER_SIZE <= img_x && img_x < COL - BORDER_SIZE && BORDER_SIZE <= img_y && img_y < ROW - BORDER_SIZE; } std::vector FeatureTracker::gridDetect(size_t grid_id) { // TicToc t_temp_ceil_fast; std::vector temp_keypts; p_fast_feature_detector->detect(forw_img(grids_rect[grid_id]), temp_keypts, mask(grids_rect[grid_id])); if (SHOW_TRACK) { forw_img(grids_rect[grid_id]).copyTo(grids_detector_img(grids_rect[grid_id])); cv::drawKeypoints(grids_detector_img(grids_rect[grid_id]), temp_keypts, grids_detector_img(grids_rect[grid_id]), cv::Scalar::all(255), cv::DrawMatchesFlags::DRAW_OVER_OUTIMG); } if (temp_keypts.empty()) { grids_texture_status[grid_id] = false; return {}; } else { size_t grid_num_to_add = grids_threshold - grids_track_num[grid_id] + 2; if (temp_keypts.size() <= grid_num_to_add) { for (auto &temp_keypt : temp_keypts) { temp_keypt.pt.x += grids_rect[grid_id].x; temp_keypt.pt.y += grids_rect[grid_id].y; } return temp_keypts; } std::vector keypts_to_add(grid_num_to_add); int min_response_id = 0; for (size_t j = 0; j < temp_keypts.size(); ++j) { if (grid_num_to_add > 0) { temp_keypts[j].pt.x += grids_rect[grid_id].x; temp_keypts[j].pt.y += grids_rect[grid_id].y; keypts_to_add[j] = temp_keypts[j]; --grid_num_to_add; if (temp_keypts[j].response < keypts_to_add[min_response_id].response) { min_response_id = j; } } else if (temp_keypts[j].response > keypts_to_add[min_response_id].response) { temp_keypts[j].pt.x += grids_rect[grid_id].x; temp_keypts[j].pt.y += grids_rect[grid_id].y; keypts_to_add[min_response_id] = temp_keypts[j]; for (size_t k = 0; k < keypts_to_add.size(); ++k) { if (keypts_to_add[k].response < keypts_to_add[min_response_id].response) { min_response_id = k; } } } } keypts_to_add.resize(keypts_to_add.size() - grid_num_to_add); return keypts_to_add; } // printf("detect grids_img feature costs: %fms\n", // t_temp_ceil_fast.toc()); } void FeatureTracker::setMask() { if (FISHEYE) mask = fisheye_mask.clone(); else mask = cv::Mat(ROW, COL, CV_8UC1, cv::Scalar(255)); // prefer to keep features that are tracked for long time vector>> cnt_pts_id; for (unsigned int i = 0; i < forw_pts.size(); i++) cnt_pts_id.emplace_back(track_cnt[i], make_pair(forw_pts[i], ids[i])); sort(cnt_pts_id.begin(), cnt_pts_id.end(), [](const pair> &a, const pair> &b) { return a.first > b.first; }); forw_pts.clear(); ids.clear(); track_cnt.clear(); for (auto &it : cnt_pts_id) { if (mask.at(it.second.first) == 255) { forw_pts.push_back(it.second.first); ids.push_back(it.second.second); track_cnt.push_back(it.first); cv::circle(mask, it.second.first, MIN_DIST, 0, -1); } } for (auto &pt : unstable_pts) { cv::circle(mask, pt, MIN_DIST, 0, -1); } } void FeatureTracker::addPoints() { for (auto &p : n_pts) { forw_pts.push_back(p); ids.push_back(-1); track_cnt.push_back(1); } } void FeatureTracker::addPoints(vector &Keypts) { for (auto &Keypt : Keypts) { if (mask.at(Keypt.pt) == 255) { forw_pts.push_back(Keypt.pt); ids.push_back(-1); track_cnt.push_back(1); // cl: prevent close feature selected cv::circle(mask, Keypt.pt, MIN_DIST, 0, -1); } } } void FeatureTracker::addPoints(int n_max_cnt, vector &Keypts) { if (Keypts.empty()) { return; } sort(Keypts.begin(), Keypts.end(), [](const cv::KeyPoint &a, const cv::KeyPoint &b) { return a.response > b.response; }); int n_add = 0; for (auto &Keypt : Keypts) { if (mask.at(Keypt.pt) == 255) { forw_pts.push_back(Keypt.pt); ids.push_back(-1); track_cnt.push_back(1); cv::circle(mask, Keypt.pt, MIN_DIST, 0, -1); // cl: prevent close feature selected n_add++; if (n_add == n_max_cnt) { break; } } } } void FeatureTracker::readImage(const cv::Mat &_img, double _cur_time, const Matrix3d &_relative_R) { cv::Mat img; TicToc t_r; cur_time = _cur_time; // too dark or too bright: histogram if (EQUALIZE) { cv::Ptr clahe = cv::createCLAHE(3.0, cv::Size(8, 8)); TicToc t_c; clahe->apply(_img, img); ROS_DEBUG("CLAHE costs: %fms", t_c.toc()); } else img = _img; if (forw_img.empty()) { // curr_img<--->forw_img cur_img = forw_img = img; } else { forw_img = img; } forw_pts.clear(); unstable_pts.clear(); if (!cur_pts.empty()) { TicToc t_o; vector status; vector err; if (USE_IMU) { predictPtsInNextFrame(_relative_R); forw_pts = predict_pts; cv::calcOpticalFlowPyrLK( cur_img, forw_img, cur_pts, forw_pts, status, err, cv::Size(21, 21), 1, cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 30, 0.01), cv::OPTFLOW_USE_INITIAL_FLOW); } else { cv::calcOpticalFlowPyrLK(cur_img, forw_img, cur_pts, forw_pts, status, err, cv::Size(21, 21), 3); } for (int i = 0; i < int(forw_pts.size()); i++) { if (!status[i] && inBorder(forw_pts[i])) { unstable_pts.push_back(forw_pts[i]); } else if (status[i] && !inBorder(forw_pts[i])) { status[i] = 0; } } reduceVector(cur_pts, status); reduceVector(forw_pts, status); reduceVector(ids, status); reduceVector(cur_un_pts, status); reduceVector(track_cnt, status); // ROS_DEBUG("temporal optical flow costs: %fms", t_o.toc()); // 光流准确率，时间输出 static double OpticalFlow_time = 0; static int of_cnt_frame = 0; OpticalFlow_time += t_o.toc(); of_cnt_frame++; ROS_DEBUG("average optical flow costs: %fms\n", OpticalFlow_time / (double)of_cnt_frame); // static double succ_num = 0; // static double total_num = 0; // for (size_t i = 0; i < status.size(); i++) { // if (status[i]) // succ_num++; // } // total_num += status.size(); // ROS_DEBUG("average success ratio: %f\n", succ_num / total_num); } for (auto &n : track_cnt) n++; if (PUB_THIS_FRAME) { // TicToc t_m; //对cur_pts和forw_pts做ransac剔除outlier. rejectWithF(); setMask(); // ROS_DEBUG("set mask costs %fms", t_m.toc()); TicToc t_t; int n_max_cnt = MAX_CNT - static_cast(forw_pts.size()); if (n_max_cnt > 0) { if (mask.empty()) cout << "mask is empty " << endl; if (mask.type() != CV_8UC1) cout << "mask type wrong " << endl; // TicToc t_grid_detect; for (auto &grid_track_num : grids_track_num) grid_track_num = 0; for (auto &forw_pt : forw_pts) { int col_id = (int)forw_pt.x / grid_width; int row_id = (int)forw_pt.y / grid_height; if (col_id == NUM_GRID_COLS) --col_id; if (row_id == NUM_GRID_ROWS) --row_id; ++grids_track_num[col_id + NUM_GRID_COLS * row_id]; } std::vector grids_id; for (size_t i = 0; i < grids_rect.size(); ++i) { if (grids_track_num[i] < grids_threshold && grids_texture_status[i]) { grids_id.emplace_back(i); } else { grids_texture_status[i] = true; } } std::vector>> grids_keypts; for (int &i : grids_id) { grids_keypts.emplace_back( pool->enqueue([this](int grid_id) { return gridDetect(grid_id); }, i)); } // TicToc t_a; for (auto &&grid_keypts_asyn : grids_keypts) { std::vector &&grid_keypts = grid_keypts_asyn.get(); addPoints(grid_keypts); } // std::vector Keypts; // p_fast_feature_detector->detect(forw_img, Keypts, mask); // addPoints(n_max_cnt, Keypts); // ROS_DEBUG("selectFeature costs: %fms", t_a.toc()); /* static double grid_detect_fast_time = 0; grid_detect_fast_time += t_grid_detect.toc(); static int cnt_grid_frame = 0; cnt_grid_frame++; printf("average grids_img detect feature costs: %fms\n", grid_detect_fast_time / (double)cnt_grid_frame); */ } static double detect_time = 0; static int detect_cnt_frame = 0; detect_time += t_t.toc(); detect_cnt_frame++; ROS_DEBUG("average detect costs: %fms\n", detect_time / (double)detect_cnt_frame); // ROS_DEBUG("detect feature costs: %fms", t_t.toc()); } prev_un_pts = cur_un_pts; cur_img = forw_img; cur_pts = forw_pts; // 去畸变，投影至归一化平面，计算特征点速度(pixel/s) undistortedPoints(); prev_time = cur_time; ROS_DEBUG("Process Image costs: %fms", t_r.toc()); } void FeatureTracker::rejectWithF() { if (forw_pts.size() >= 8) { TicToc t_f; vector un_cur_pts(cur_pts.size()), un_forw_pts(forw_pts.size()); for (unsigned int i = 0; i < cur_pts.size(); i++) { Eigen::Vector3d tmp_p; m_camera->liftProjective(Eigen::Vector2d(cur_pts[i].x, cur_pts[i].y), tmp_p); tmp_p.x() = FOCAL_LENGTH * tmp_p.x() / tmp_p.z() + COL / 2.0; tmp_p.y() = FOCAL_LENGTH * tmp_p.y() / tmp_p.z() + ROW / 2.0; un_cur_pts[i] = cv::Point2f(tmp_p.x(), tmp_p.y()); m_camera->liftProjective(Eigen::Vector2d(forw_pts[i].x, forw_pts[i].y), tmp_p); tmp_p.x() = FOCAL_LENGTH * tmp_p.x() / tmp_p.z() + COL / 2.0; tmp_p.y() = FOCAL_LENGTH * tmp_p.y() / tmp_p.z() + ROW / 2.0; un_forw_pts[i] = cv::Point2f(tmp_p.x(), tmp_p.y()); } vector status; cv::findFundamentalMat(un_cur_pts, un_forw_pts, cv::FM_RANSAC, F_THRESHOLD, 0.99, status); int size_a = cur_pts.size(); reduceVector(cur_pts, status); reduceVector(forw_pts, status); reduceVector(cur_un_pts, status); reduceVector(ids, status); reduceVector(track_cnt, status); ROS_DEBUG("FM ransac: %d -> %lu: %f", size_a, forw_pts.size(), 1.0 * forw_pts.size() / size_a); ROS_DEBUG("FM ransac costs: %fms", t_f.toc()); } } Eigen::Vector3d FeatureTracker::get3dPt(const cv::Mat &depth, const cv::Point2f &pt) { Eigen::Vector3d tmp_P; m_camera->liftProjective(Eigen::Vector2d(pt.x, pt.y), tmp_P); Eigen::Vector3d P = tmp_P.normalized() * (((int)depth.at(round(pt.y), round(pt.x))) / 1000.0); return P; } bool FeatureTracker::updateID(unsigned int i) { if (i < ids.size()) { if (ids[i] == -1) ids[i] = n_id++; return true; } else return false; } void FeatureTracker::readIntrinsicParameter(const string &calib_file) { ROS_DEBUG("reading paramerter of camera %s", calib_file.c_str()); m_camera = CameraFactory::instance()->generateCameraFromYamlFile(calib_file); } void FeatureTracker::showUndistortion(const string &name) { cv::Mat undistortedImg(ROW + 600, COL + 600, CV_8UC1, cv::Scalar(0)); vector distortedp, undistortedp; for (int i = 0; i < COL; i++) for (int j = 0; j < ROW; j++) { Eigen::Vector2d a(i, j); Eigen::Vector3d b; m_camera->liftProjective(a, b); distortedp.push_back(a); undistortedp.emplace_back(b.x() / b.z(), b.y() / b.z()); // printf("%f,%f->%f,%f,%f\n)\n", a.x(), a.y(), b.x(), b.y(), b.z()); } for (int i = 0; i < int(undistortedp.size()); i++) { cv::Mat pp(3, 1, CV_32FC1); pp.at(0, 0) = undistortedp[i].x() * FOCAL_LENGTH + COL / 2; pp.at(1, 0) = undistortedp[i].y() * FOCAL_LENGTH + ROW / 2; pp.at(2, 0) = 1.0; // cout << trackerData[0].K << endl; // printf("%lf %lf\n", p.at(1, 0), p.at(0, 0)); // printf("%lf %lf\n", pp.at(1, 0), pp.at(0, 0)); if (pp.at(1, 0) + 300 >= 0 && pp.at(1, 0) + 300 < ROW + 600 && pp.at(0, 0) + 300 >= 0 && pp.at(0, 0) + 300 < COL + 600) { undistortedImg.at(pp.at(1, 0) + 300, pp.at(0, 0) + 300) = cur_img.at(distortedp[i].y(), distortedp[i].x()); } else { // ROS_ERROR("(%f %f) -> (%f %f)", distortedp[i].y, distortedp[i].x, // pp.at(1, 0), pp.at(0, 0)); } } cv::imshow(name, undistortedImg); cv::waitKey(0); } void FeatureTracker::undistortedPoints() { cur_un_pts.clear(); cur_un_pts_map.clear(); // cv::undistortPoints(cur_pts, un_pts, K, cv::Mat()); for (unsigned int i = 0; i < cur_pts.size(); i++) { Eigen::Vector2d a(cur_pts[i].x, cur_pts[i].y); Eigen::Vector3d b; // https://github.com/HKUST-Aerial-Robotics/VINS-Mono/blob/0d280936e441ebb782bf8855d86e13999a22da63/camera_model/src/camera_models/PinholeCamera.cc // brief Lifts a point from the image plane to its projective ray m_camera->liftProjective(a, b); // 特征点在相机坐标系的归一化坐标 cur_un_pts.emplace_back(b.x() / b.z(), b.y() / b.z()); cur_un_pts_map.insert(make_pair(ids[i], cv::Point2f(b.x() / b.z(), b.y() / b.z()))); // printf("cur pts id %d %f %f", ids[i], cur_un_pts[i].x, cur_un_pts[i].y); } // caculate points velocity if (!prev_un_pts_map.empty()) { double dt = cur_time - prev_time; pts_velocity.clear(); for (unsigned int i = 0; i < cur_un_pts.size(); i++) { if (ids[i] != -1) { std::map::iterator it; it = prev_un_pts_map.find(ids[i]); if (it != prev_un_pts_map.end()) { double v_x = (cur_un_pts[i].x - it->second.x) / dt; double v_y = (cur_un_pts[i].y - it->second.y) / dt; pts_velocity.emplace_back(v_x, v_y); } else pts_velocity.emplace_back(0, 0); } else { pts_velocity.emplace_back(0, 0); } } } else { for (unsigned int i = 0; i < cur_pts.size(); i++) { pts_velocity.emplace_back(0, 0); } } prev_un_pts_map = cur_un_pts_map; } void FeatureTracker::predictPtsInNextFrame(const Matrix3d &_relative_R) { predict_pts.resize(cur_pts.size()); for (unsigned int i = 0; i < cur_pts.size(); ++i) { Eigen::Vector3d tmp_P; m_camera->liftProjective(Eigen::Vector2d(cur_pts[i].x, cur_pts[i].y), tmp_P); Eigen::Vector3d predict_P = _relative_R * tmp_P; Eigen::Vector2d tmp_p; m_camera->spaceToPlane(predict_P, tmp_p); predict_pts[i].x = tmp_p.x(); predict_pts[i].y = tmp_p.y(); } } ================================================ FILE: vins_estimator/src/feature_tracker/feature_tracker.h ================================================ #pragma once #include #include #include #include #include #include #include #include "camodocal/camera_models/CameraFactory.h" #include "camodocal/camera_models/CataCamera.h" #include "camodocal/camera_models/PinholeCamera.h" #include "../utility/parameters.h" #include "../utility/tic_toc.h" #include "ThreadPool.h" using namespace std; using namespace camodocal; using namespace Eigen; bool inBorder(const cv::Point2f &pt); void reduceVector(vector &v, vector status); void reduceVector(vector &v, vector status); class FeatureTracker { public: FeatureTracker(); void readImage(const cv::Mat &_img, double _cur_time, const Matrix3d &_relative_R = Matrix3d::Identity()); void setMask(); void addPoints(); void addPoints(vector &Keypts); void addPoints(int n_max_cnt, vector &Keypts); bool updateID(unsigned int i); void readIntrinsicParameter(const string &calib_file); void showUndistortion(const string &name); void rejectWithF(); void undistortedPoints(); void predictPtsInNextFrame(const Matrix3d &_relative_R); Eigen::Vector3d get3dPt(const cv::Mat &_depth, const cv::Point2f &pt); void initGridsDetector(); static bool inBorder(const cv::Point2f &pt); std::vector gridDetect(size_t grid_id); cv::Mat mask; cv::Mat fisheye_mask; cv::Mat cur_img, forw_img; vector n_pts; vector cur_pts, forw_pts, predict_pts, unstable_pts; vector prev_un_pts, cur_un_pts; vector pts_velocity; vector ids; vector track_cnt; map cur_un_pts_map; map prev_un_pts_map; camodocal::CameraPtr m_camera; double cur_time{}; double prev_time{}; int n_id; cv::Ptr p_fast_feature_detector; std::shared_ptr pool; std::vector grids_rect; std::vector grids_track_num; std::vector grids_texture_status; // true: abundant texture grid; false: // textureless grid int grid_height{}; int grid_width{}; int grid_res_height{}; int grid_res_width{}; int grids_threshold{}; cv::Mat grids_detector_img; }; ================================================ FILE: vins_estimator/src/initial/initial_aligment.cpp ================================================ #include "initial_alignment.h" void solveGyroscopeBias(map &all_image_frame, Vector3d *Bgs) { Matrix3d A; Vector3d b; Vector3d delta_bg; A.setZero(); b.setZero(); map::iterator frame_i; map::iterator frame_j; for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end(); frame_i++) { frame_j = next(frame_i); MatrixXd tmp_A(3, 3); tmp_A.setZero(); VectorXd tmp_b(3); tmp_b.setZero(); Eigen::Quaterniond q_ij(frame_i->second.R.transpose() * frame_j->second.R); tmp_A = frame_j->second.pre_integration->jacobian.template block<3, 3>(O_R, O_BG); tmp_b = 2 * (frame_j->second.pre_integration->delta_q.inverse() * q_ij).vec(); A += tmp_A.transpose() * tmp_A; b += tmp_A.transpose() * tmp_b; } delta_bg = A.ldlt().solve(b); ROS_WARN_STREAM("gyroscope bias initial calibration " << delta_bg.transpose()); for (int i = 0; i <= WINDOW_SIZE; i++) Bgs[i] += delta_bg; for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end(); frame_i++) { frame_j = next(frame_i); frame_j->second.pre_integration->repropagate(Vector3d::Zero(), Bgs[0]); } } /*void solveGyroscopeBias(map &all_image_frame, Vector3d* Bgs) { Matrix3d A; Vector3d b; Vector3d delta_bg; A.setZero(); b.setZero(); map::iterator frame_i; map::iterator frame_j; int cnt = 0; for (frame_i = all_image_frame.end(); cntsecond.R.transpose() * frame_j->second.R); tmp_A = frame_j->second.pre_integration->jacobian.template block<3, 3>(O_R, O_BG); tmp_b = 2 * (frame_j->second.pre_integration->delta_q.inverse() * q_ij).vec(); A += tmp_A.transpose() * tmp_A; b += tmp_A.transpose() * tmp_b; } delta_bg = A.ldlt().solve(b); ROS_WARN_STREAM("gyroscope bias initial calibration " << delta_bg.transpose()); for (int i = 0; i <= WINDOW_SIZE; i++) Bgs[i] += delta_bg; for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end( ); frame_i++) { frame_j = next(frame_i); frame_j->second.pre_integration->repropagate(Vector3d::Zero(), Bgs[0]); } }*/ MatrixXd TangentBasis(Vector3d &g0) { Vector3d b, c; Vector3d a = g0.normalized(); Vector3d tmp(0, 0, 1); if (a == tmp) tmp << 1, 0, 0; b = (tmp - a * (a.transpose() * tmp)).normalized(); c = a.cross(b); MatrixXd bc(3, 2); bc.block<3, 1>(0, 0) = b; bc.block<3, 1>(0, 1) = c; return bc; } void RefineGravity(map &all_image_frame, Vector3d &g, VectorXd &x) { Vector3d g0 = g.normalized() * G.norm(); Vector3d lx, ly; // VectorXd x; int all_frame_count = all_image_frame.size(); int n_state = all_frame_count * 3 + 2 + 1; MatrixXd A{n_state, n_state}; A.setZero(); VectorXd b{n_state}; b.setZero(); map::iterator frame_i; map::iterator frame_j; for (int k = 0; k < 4; k++) { MatrixXd lxly(3, 2); lxly = TangentBasis(g0); int i = 0; for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end(); frame_i++, i++) { frame_j = next(frame_i); MatrixXd tmp_A(6, 9); tmp_A.setZero(); VectorXd tmp_b(6); tmp_b.setZero(); double dt = frame_j->second.pre_integration->sum_dt; tmp_A.block<3, 3>(0, 0) = -dt * Matrix3d::Identity(); tmp_A.block<3, 2>(0, 6) = frame_i->second.R.transpose() * dt * dt / 2 * Matrix3d::Identity() * lxly; tmp_A.block<3, 1>(0, 8) = frame_i->second.R.transpose() * (frame_j->second.T - frame_i->second.T) / 100.0; 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; tmp_A.block<3, 3>(3, 0) = -Matrix3d::Identity(); tmp_A.block<3, 3>(3, 3) = frame_i->second.R.transpose() * frame_j->second.R; tmp_A.block<3, 2>(3, 6) = frame_i->second.R.transpose() * dt * Matrix3d::Identity() * lxly; tmp_b.block<3, 1>(3, 0) = frame_j->second.pre_integration->delta_v - frame_i->second.R.transpose() * dt * Matrix3d::Identity() * g0; Matrix cov_inv = Matrix::Zero(); // cov.block<6, 6>(0, 0) = IMU_cov[i + 1]; // MatrixXd cov_inv = cov.inverse(); cov_inv.setIdentity(); MatrixXd r_A = tmp_A.transpose() * cov_inv * tmp_A; VectorXd r_b = tmp_A.transpose() * cov_inv * tmp_b; A.block<6, 6>(i * 3, i * 3) += r_A.topLeftCorner<6, 6>(); b.segment<6>(i * 3) += r_b.head<6>(); A.bottomRightCorner<3, 3>() += r_A.bottomRightCorner<3, 3>(); b.tail<3>() += r_b.tail<3>(); A.block<6, 3>(i * 3, n_state - 3) += r_A.topRightCorner<6, 3>(); A.block<3, 6>(n_state - 3, i * 3) += r_A.bottomLeftCorner<3, 6>(); } A = A * 1000.0; b = b * 1000.0; x = A.ldlt().solve(b); VectorXd dg = x.segment<2>(n_state - 3); g0 = (g0 + lxly * dg).normalized() * G.norm(); // double s = x(n_state - 1); } g = g0; } void RefineGravityWithDepth(map &all_image_frame, Vector3d &g, VectorXd &x) { Vector3d g0 = g.normalized() * G.norm(); Vector3d lx, ly; // VectorXd x; int all_frame_count = all_image_frame.size(); int n_state = all_frame_count * 3 + 2; MatrixXd A{n_state, n_state}; A.setZero(); VectorXd b{n_state}; b.setZero(); map::iterator frame_i; map::iterator frame_j; for (int k = 0; k < 4; k++) { MatrixXd lxly(3, 2); lxly = TangentBasis(g0); int i = 0; for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end(); frame_i++, i++) { frame_j = next(frame_i); MatrixXd tmp_A(6, 8); tmp_A.setZero(); VectorXd tmp_b(6); tmp_b.setZero(); double dt = frame_j->second.pre_integration->sum_dt; tmp_A.block<3, 3>(0, 0) = -dt * Matrix3d::Identity(); tmp_A.block<3, 2>(0, 6) = frame_i->second.R.transpose() * dt * dt / 2 * Matrix3d::Identity() * lxly; 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 - frame_i->second.R.transpose() * (frame_j->second.T - frame_i->second.T); tmp_A.block<3, 3>(3, 0) = -Matrix3d::Identity(); tmp_A.block<3, 3>(3, 3) = frame_i->second.R.transpose() * frame_j->second.R; tmp_A.block<3, 2>(3, 6) = frame_i->second.R.transpose() * dt * Matrix3d::Identity() * lxly; tmp_b.block<3, 1>(3, 0) = frame_j->second.pre_integration->delta_v - frame_i->second.R.transpose() * dt * Matrix3d::Identity() * g0; Matrix cov_inv = Matrix::Zero(); // cov.block<6, 6>(0, 0) = IMU_cov[i + 1]; // MatrixXd cov_inv = cov.inverse(); cov_inv.setIdentity(); MatrixXd r_A = tmp_A.transpose() * cov_inv * tmp_A; VectorXd r_b = tmp_A.transpose() * cov_inv * tmp_b; A.block<6, 6>(i * 3, i * 3) += r_A.topLeftCorner<6, 6>(); b.segment<6>(i * 3) += r_b.head<6>(); A.bottomRightCorner<2, 2>() += r_A.bottomRightCorner<2, 2>(); b.tail<2>() += r_b.tail<2>(); A.block<6, 2>(i * 3, n_state - 2) += r_A.topRightCorner<6, 2>(); A.block<2, 6>(n_state - 2, i * 3) += r_A.bottomLeftCorner<2, 6>(); } A = A * 1000.0; b = b * 1000.0; x = A.ldlt().solve(b); VectorXd dg = x.segment<2>(n_state - 2); g0 = (g0 + lxly * dg).normalized() * G.norm(); // double s = x(n_state - 1); } g = g0; } bool LinearAlignment(map &all_image_frame, Vector3d &g, VectorXd &x) { int all_frame_count = all_image_frame.size(); int n_state = all_frame_count * 3 + 3 + 1; MatrixXd A{n_state, n_state}; A.setZero(); VectorXd b{n_state}; b.setZero(); map::iterator frame_i; map::iterator frame_j; int i = 0; for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end(); frame_i++, i++) { frame_j = next(frame_i); MatrixXd tmp_A(6, 10); tmp_A.setZero(); VectorXd tmp_b(6); tmp_b.setZero(); double dt = frame_j->second.pre_integration->sum_dt; tmp_A.block<3, 3>(0, 0) = -dt * Matrix3d::Identity(); tmp_A.block<3, 3>(0, 6) = frame_i->second.R.transpose() * dt * dt / 2 * Matrix3d::Identity(); tmp_A.block<3, 1>(0, 9) = frame_i->second.R.transpose() * (frame_j->second.T - frame_i->second.T) / 100.0; 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]; // cout << "delta_p " << // frame_j->second.pre_integration->delta_p.transpose() << endl; tmp_A.block<3, 3>(3, 0) = -Matrix3d::Identity(); tmp_A.block<3, 3>(3, 3) = frame_i->second.R.transpose() * frame_j->second.R; tmp_A.block<3, 3>(3, 6) = frame_i->second.R.transpose() * dt * Matrix3d::Identity(); tmp_b.block<3, 1>(3, 0) = frame_j->second.pre_integration->delta_v; // cout << "delta_v " << // frame_j->second.pre_integration->delta_v.transpose() << endl; Matrix cov_inv = Matrix::Zero(); // cov.block<6, 6>(0, 0) = IMU_cov[i + 1]; // MatrixXd cov_inv = cov.inverse(); cov_inv.setIdentity(); MatrixXd r_A = tmp_A.transpose() * cov_inv * tmp_A; VectorXd r_b = tmp_A.transpose() * cov_inv * tmp_b; A.block<6, 6>(i * 3, i * 3) += r_A.topLeftCorner<6, 6>(); b.segment<6>(i * 3) += r_b.head<6>(); A.bottomRightCorner<4, 4>() += r_A.bottomRightCorner<4, 4>(); b.tail<4>() += r_b.tail<4>(); A.block<6, 4>(i * 3, n_state - 4) += r_A.topRightCorner<6, 4>(); A.block<4, 6>(n_state - 4, i * 3) += r_A.bottomLeftCorner<4, 6>(); } A = A * 1000.0; b = b * 1000.0; x = A.ldlt().solve(b); double s = x(n_state - 1) / 100.0; ROS_DEBUG("estimated scale: %f", s); g = x.segment<3>(n_state - 4); ROS_DEBUG_STREAM(" result g " << g.norm() << " " << g.transpose()); if (fabs(g.norm() - G.norm()) > 1.0 || s < 0) { return false; } RefineGravity(all_image_frame, g, x); s = (x.tail<1>())(0) / 100.0; (x.tail<1>())(0) = s; ROS_DEBUG_STREAM(" refine " << g.norm() << " " << g.transpose()); if (s < 0.0) return false; else return true; } /** * @brief 计算重力加速度和速度 * @optional 速度、重力向量和尺度初始化Paper -> V-B-2 * 相邻帧之间的位置和速度与IMU预积分出来的位置和速度对齐，求解最小二乘 * 重力细化 -> Paper V-B-3 * @param[in] all_image_frame * 所有图像帧构成的map,图像帧保存了位姿，预积分量和关于角点的信息 * @param[out] g 重力加速度 * @param[out] x 待优化变量，窗口中每帧的速度V[0:n]、重力g * @return void */ bool LinearAlignmentWithDepth(map &all_image_frame, Vector3d &g, VectorXd &x) { int all_frame_count = all_image_frame.size(); int n_state = all_frame_count * 3 + 3; // no scale now MatrixXd A{n_state, n_state}; A.setZero(); VectorXd b{n_state}; b.setZero(); map::iterator frame_i; map::iterator frame_j; int i = 0; for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end(); frame_i++, i++) { frame_j = next(frame_i); MatrixXd tmp_A(6, 9); // no scale now tmp_A.setZero(); VectorXd tmp_b(6); tmp_b.setZero(); double dt = frame_j->second.pre_integration->sum_dt; tmp_A.block<3, 3>(0, 0) = -dt * Matrix3d::Identity(); tmp_A.block<3, 3>(0, 6) = frame_i->second.R.transpose() * dt * dt / 2 * Matrix3d::Identity(); 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() * (frame_j->second.T - frame_i->second.T); tmp_A.block<3, 3>(3, 0) = -Matrix3d::Identity(); tmp_A.block<3, 3>(3, 3) = frame_i->second.R.transpose() * frame_j->second.R; tmp_A.block<3, 3>(3, 6) = frame_i->second.R.transpose() * dt * Matrix3d::Identity(); tmp_b.block<3, 1>(3, 0) = frame_j->second.pre_integration->delta_v; Matrix cov_inv = Matrix::Zero(); cov_inv.setIdentity(); MatrixXd r_A = tmp_A.transpose() * cov_inv * tmp_A; VectorXd r_b = tmp_A.transpose() * cov_inv * tmp_b; A.block<6, 6>(i * 3, i * 3) += r_A.topLeftCorner<6, 6>(); b.segment<6>(i * 3) += r_b.head<6>(); A.bottomRightCorner<3, 3>() += r_A.bottomRightCorner<3, 3>(); b.tail<3>() += r_b.tail<3>(); A.block<6, 3>(i * 3, n_state - 3) += r_A.topRightCorner<6, 3>(); A.block<3, 6>(n_state - 3, i * 3) += r_A.bottomLeftCorner<3, 6>(); } A = A * 1000.0; b = b * 1000.0; x = A.ldlt().solve(b); g = x.segment<3>(n_state - 3); ROS_DEBUG_STREAM(" result g " << g.norm() << " " << g.transpose()); if (fabs(g.norm() - G.norm()) > 1.0) { return false; } RefineGravityWithDepth(all_image_frame, g, x); ROS_DEBUG_STREAM(" refine " << g.norm() << " " << g.transpose()); return true; } bool LinearAlignmentWithDepthGravity(map &all_image_frame, Vector3d &g, VectorXd &x) { int all_frame_count = all_image_frame.size(); int n_state = all_frame_count * 3; // no scale now MatrixXd A{n_state, n_state}; A.setZero(); VectorXd b{n_state}; b.setZero(); map::iterator frame_i; map::iterator frame_j; int i = 0; for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end(); frame_i++, i++) { frame_j = next(frame_i); MatrixXd tmp_A(6, 6); // no scale now tmp_A.setZero(); VectorXd tmp_b(6); tmp_b.setZero(); double dt = frame_j->second.pre_integration->sum_dt; tmp_A.block<3, 3>(0, 0) = -dt * Matrix3d::Identity(); // G // tmp_A.block<3, 3>(0, 6) = // frame_i->second.R.transpose() * dt * dt / 2 * Matrix3d::Identity(); 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() * (frame_j->second.T - frame_i->second.T) - frame_i->second.R.transpose() * dt * dt / 2 * g; tmp_A.block<3, 3>(3, 0) = -Matrix3d::Identity(); tmp_A.block<3, 3>(3, 3) = frame_i->second.R.transpose() * frame_j->second.R; // G // tmp_A.block<3, 3>(3, 6) = // frame_i->second.R.transpose() * dt * Matrix3d::Identity(); tmp_b.block<3, 1>(3, 0) = frame_j->second.pre_integration->delta_v - frame_i->second.R.transpose() * dt * g; Matrix cov_inv = Matrix::Zero(); cov_inv.setIdentity(); MatrixXd r_A = tmp_A.transpose() * cov_inv * tmp_A; VectorXd r_b = tmp_A.transpose() * cov_inv * tmp_b; A.block<6, 6>(i * 3, i * 3) += r_A.topLeftCorner<6, 6>(); b.segment<6>(i * 3) += r_b.head<6>(); // G // A.bottomRightCorner<3, 3>() += r_A.bottomRightCorner<3, 3>(); // b.tail<3>() += r_b.tail<3>(); // A.block<6, 3>(i * 3, n_state - 3) += r_A.topRightCorner<6, 3>(); // A.block<3, 6>(n_state - 3, i * 3) += r_A.bottomLeftCorner<3, 6>(); } A = A * 1000.0; b = b * 1000.0; x = A.ldlt().solve(b); // g = x.segment<3>(n_state - 3); // ROS_DEBUG_STREAM(" result g " << g.norm() << " " << g.transpose()); // if (fabs(g.norm() - G.norm()) > 1.0) { // return false; // } // RefineGravityWithDepth(all_image_frame, g, x); // ROS_DEBUG_STREAM(" refine " << g.norm() << " " << g.transpose()); return true; } bool VisualIMUAlignment(map &all_image_frame, Vector3d *Bgs, Vector3d &g, VectorXd &x) { solveGyroscopeBias(all_image_frame, Bgs); if (LinearAlignmentWithDepth(all_image_frame, g, x)) return true; else return false; } ================================================ FILE: vins_estimator/src/initial/initial_alignment.h ================================================ #pragma once #include "../factor/imu_factor.h" #include "../feature_manager/feature_manager.h" #include "../utility/utility.h" #include #include #include #include using namespace Eigen; using namespace std; class ImageFrame { public: ImageFrame(){}; ImageFrame(const map> &_points, double _t) : t{_t}, is_key_frame{false} { points = _points; }; map> points; double t; Matrix3d R; Vector3d T; IntegrationBase *pre_integration; bool is_key_frame; }; bool VisualIMUAlignment(map &all_image_frame, Vector3d *Bgs, Vector3d &g, VectorXd &x); bool LinearAlignmentWithDepth(map &all_image_frame, Vector3d &g, VectorXd &x); bool LinearAlignmentWithDepthGravity(map &all_image_frame, Vector3d &g, VectorXd &x); void solveGyroscopeBias(map &all_image_frame, Vector3d *Bgs); ================================================ FILE: vins_estimator/src/initial/initial_ex_rotation.cpp ================================================ #include "initial_ex_rotation.h" InitialEXRotation::InitialEXRotation() { frame_count = 0; Rc.push_back(Matrix3d::Identity()); Rc_g.push_back(Matrix3d::Identity()); Rimu.push_back(Matrix3d::Identity()); ric = Matrix3d::Identity(); } bool InitialEXRotation::CalibrationExRotation(vector> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result) { frame_count++; Rc.push_back(solveRelativeR(corres)); Rimu.push_back(delta_q_imu.toRotationMatrix()); Rc_g.push_back(ric.inverse() * delta_q_imu * ric); Eigen::MatrixXd A(frame_count * 4, 4); A.setZero(); int sum_ok = 0; for (int i = 1; i <= frame_count; i++) { Quaterniond r1(Rc[i]); Quaterniond r2(Rc_g[i]); double angular_distance = 180 / M_PI * r1.angularDistance(r2); ROS_DEBUG("%d %f", i, angular_distance); double huber = angular_distance > 5.0 ? 5.0 / angular_distance : 1.0; ++sum_ok; Matrix4d L, R; double w = Quaterniond(Rc[i]).w(); Vector3d q = Quaterniond(Rc[i]).vec(); L.block<3, 3>(0, 0) = w * Matrix3d::Identity() + Utility::skewSymmetric(q); L.block<3, 1>(0, 3) = q; L.block<1, 3>(3, 0) = -q.transpose(); L(3, 3) = w; Quaterniond R_ij(Rimu[i]); w = R_ij.w(); q = R_ij.vec(); R.block<3, 3>(0, 0) = w * Matrix3d::Identity() - Utility::skewSymmetric(q); R.block<3, 1>(0, 3) = q; R.block<1, 3>(3, 0) = -q.transpose(); R(3, 3) = w; A.block<4, 4>((i - 1) * 4, 0) = huber * (L - R); } JacobiSVD svd(A, ComputeFullU | ComputeFullV); Matrix x = svd.matrixV().col(3); Quaterniond estimated_R(x); ric = estimated_R.toRotationMatrix().inverse(); // cout << svd.singularValues().transpose() << endl; // cout << ric << endl; Vector3d ric_cov; ric_cov = svd.singularValues().tail<3>(); if (frame_count >= WINDOW_SIZE && ric_cov(1) > 0.25) { calib_ric_result = ric; return true; } else return false; } Matrix3d InitialEXRotation::solveRelativeR(const vector> &corres) { if (corres.size() >= 9) { vector ll, rr; for (int i = 0; i < int(corres.size()); i++) { ll.push_back(cv::Point2f(corres[i].first(0), corres[i].first(1))); rr.push_back(cv::Point2f(corres[i].second(0), corres[i].second(1))); } cv::Mat E = cv::findFundamentalMat(ll, rr); cv::Mat_ R1, R2, t1, t2; decomposeE(E, R1, R2, t1, t2); if (determinant(R1) + 1.0 < 1e-09) { E = -E; decomposeE(E, R1, R2, t1, t2); } double ratio1 = max(testTriangulation(ll, rr, R1, t1), testTriangulation(ll, rr, R1, t2)); double ratio2 = max(testTriangulation(ll, rr, R2, t1), testTriangulation(ll, rr, R2, t2)); cv::Mat_ ans_R_cv = ratio1 > ratio2 ? R1 : R2; Matrix3d ans_R_eigen; for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) ans_R_eigen(j, i) = ans_R_cv(i, j); return ans_R_eigen; } return Matrix3d::Identity(); } double InitialEXRotation::testTriangulation(const vector &l, const vector &r, cv::Mat_ R, cv::Mat_ t) { cv::Mat pointcloud; cv::Matx34f P = cv::Matx34f(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0); cv::Matx34f P1 = cv::Matx34f(R(0, 0), R(0, 1), R(0, 2), t(0), R(1, 0), R(1, 1), R(1, 2), t(1), R(2, 0), R(2, 1), R(2, 2), t(2)); cv::triangulatePoints(P, P1, l, r, pointcloud); int front_count = 0; for (int i = 0; i < pointcloud.cols; i++) { double normal_factor = pointcloud.col(i).at(3); cv::Mat_ p_3d_l = cv::Mat(P) * (pointcloud.col(i) / normal_factor); cv::Mat_ p_3d_r = cv::Mat(P1) * (pointcloud.col(i) / normal_factor); if (p_3d_l(2) > 0 && p_3d_r(2) > 0) front_count++; } ROS_DEBUG("MotionEstimator: %f", 1.0 * front_count / pointcloud.cols); return 1.0 * front_count / pointcloud.cols; } void InitialEXRotation::decomposeE(cv::Mat E, cv::Mat_ &R1, cv::Mat_ &R2, cv::Mat_ &t1, cv::Mat_ &t2) { cv::SVD svd(E, cv::SVD::MODIFY_A); cv::Matx33d W(0, -1, 0, 1, 0, 0, 0, 0, 1); cv::Matx33d Wt(0, 1, 0, -1, 0, 0, 0, 0, 1); R1 = svd.u * cv::Mat(W) * svd.vt; R2 = svd.u * cv::Mat(Wt) * svd.vt; t1 = svd.u.col(2); t2 = -svd.u.col(2); } ================================================ FILE: vins_estimator/src/initial/initial_ex_rotation.h ================================================ #pragma once #include #include "../utility/parameters.h" using namespace std; #include #include using namespace Eigen; #include /* This class help you to calibrate extrinsic rotation between imu and camera when your totally * don't konw the extrinsic parameter */ class InitialEXRotation { public: InitialEXRotation(); bool CalibrationExRotation(vector> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result); private: Matrix3d solveRelativeR(const vector> &corres); double testTriangulation(const vector &l, const vector &r, cv::Mat_ R, cv::Mat_ t); void decomposeE(cv::Mat E, cv::Mat_ &R1, cv::Mat_ &R2, cv::Mat_ &t1, cv::Mat_ &t2); int frame_count; vector Rc; vector Rimu; vector Rc_g; Matrix3d ric; }; ================================================ FILE: vins_estimator/src/initial/initial_sfm.cpp ================================================ #include "initial_sfm.h" GlobalSFM::GlobalSFM() {} void GlobalSFM::triangulatePoint(Eigen::Matrix &Pose0, Eigen::Matrix &Pose1, Vector2d &point0, Vector2d &point1, Vector3d &point_3d) { Matrix4d design_matrix = Matrix4d::Zero(); design_matrix.row(0) = point0[0] * Pose0.row(2) - Pose0.row(0); design_matrix.row(1) = point0[1] * Pose0.row(2) - Pose0.row(1); design_matrix.row(2) = point1[0] * Pose1.row(2) - Pose1.row(0); design_matrix.row(3) = point1[1] * Pose1.row(2) - Pose1.row(1); Vector4d triangulated_point; triangulated_point = design_matrix.jacobiSvd(Eigen::ComputeFullV).matrixV().rightCols<1>(); point_3d(0) = triangulated_point(0) / triangulated_point(3); point_3d(1) = triangulated_point(1) / triangulated_point(3); point_3d(2) = triangulated_point(2) / triangulated_point(3); } bool GlobalSFM::solveFrameByPnP(Matrix3d &R_initial, Vector3d &P_initial, int i, vector &sfm_f) { vector pts_2_vector; vector pts_3_vector; for (int j = 0; j < feature_num; j++) { if (sfm_f[j].state != true) continue; Vector2d point2d; for (int k = 0; k < (int)sfm_f[j].observation.size(); k++) { if (sfm_f[j].observation[k].first == i) { Vector2d img_pts = sfm_f[j].observation[k].second; cv::Point2f pts_2(img_pts(0), img_pts(1)); pts_2_vector.push_back(pts_2); cv::Point3f pts_3(sfm_f[j].position[0], sfm_f[j].position[1], sfm_f[j].position[2]); pts_3_vector.push_back(pts_3); break; } } } if (int(pts_2_vector.size()) < 15) { printf("unstable features tracking, please slowly move you device!(%zu)\n", pts_2_vector.size()); if (int(pts_2_vector.size()) < 10) return false; } cv::Mat r, rvec, t, D, tmp_r; cv::eigen2cv(R_initial, tmp_r); cv::Rodrigues(tmp_r, rvec); cv::eigen2cv(P_initial, t); cv::Mat K = (cv::Mat_(3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 1); bool pnp_succ; pnp_succ = cv::solvePnP(pts_3_vector, pts_2_vector, K, D, rvec, t, 1); if (!pnp_succ) { return false; } cv::Rodrigues(rvec, r); // cout << "r " << endl << r << endl; MatrixXd R_pnp; cv::cv2eigen(r, R_pnp); MatrixXd T_pnp; cv::cv2eigen(t, T_pnp); R_initial = R_pnp; P_initial = T_pnp; return true; } void GlobalSFM::triangulateTwoFrames(int frame0, Eigen::Matrix &Pose0, int frame1, Eigen::Matrix &Pose1, vector &sfm_f) { assert(frame0 != frame1); for (int j = 0; j < feature_num; j++) { if (sfm_f[j].state == true) continue; bool has_0 = false, has_1 = false; Vector2d point0; Vector2d point1; for (int k = 0; k < (int)sfm_f[j].observation.size(); k++) { if (sfm_f[j].observation[k].first == frame0) { point0 = sfm_f[j].observation[k].second; has_0 = true; } if (sfm_f[j].observation[k].first == frame1) { point1 = sfm_f[j].observation[k].second; has_1 = true; } } if (has_0 && has_1) { Vector3d point_3d; triangulatePoint(Pose0, Pose1, point0, point1, point_3d); sfm_f[j].state = true; sfm_f[j].position[0] = point_3d(0); sfm_f[j].position[1] = point_3d(1); sfm_f[j].position[2] = point_3d(2); // cout << "trangulated : " << frame1 << " 3d point : " << j << " " << // point_3d.transpose() << endl; } } } void GlobalSFM::triangulateTwoFramesWithDepth(int frame0, Eigen::Matrix &Pose0, int frame1, Eigen::Matrix &Pose1, vector &sfm_f) { assert(frame0 != frame1); Matrix3d Pose0_R = Pose0.block<3, 3>(0, 0); Matrix3d Pose1_R = Pose1.block<3, 3>(0, 0); Vector3d Pose0_t = Pose0.block<3, 1>(0, 3); Vector3d Pose1_t = Pose1.block<3, 1>(0, 3); for (int j = 0; j < feature_num; j++) { if (sfm_f[j].state == true) continue; bool has_0 = false, has_1 = false; Vector3d point0; Vector2d point1; for (int k = 0; k < (int)sfm_f[j].observation.size(); k++) { if (sfm_f[j].observation_depth[k].second < 0.1 || sfm_f[j].observation_depth[k].second > 10) // max and min measurement continue; if (sfm_f[j].observation[k].first == frame0) { point0 = Vector3d( sfm_f[j].observation[k].second.x() * sfm_f[j].observation_depth[k].second, sfm_f[j].observation[k].second.y() * sfm_f[j].observation_depth[k].second, sfm_f[j].observation_depth[k].second); has_0 = true; } if (sfm_f[j].observation[k].first == frame1) { point1 = sfm_f[j].observation[k].second; has_1 = true; } } if (has_0 && has_1) { Vector2d residual; Vector3d point_3d, point1_reprojected; // triangulatePoint(Pose0, Pose1, point0, point1, point_3d); point_3d = Pose0_R.transpose() * point0 - Pose0_R.transpose() * Pose0_t; // shan add:this is point in world; point1_reprojected = Pose1_R * point_3d + Pose1_t; residual = point1 - Vector2d(point1_reprojected.x() / point1_reprojected.z(), point1_reprojected.y() / point1_reprojected.z()); // std::cout << residual.transpose()<<"norm"< &sfm_f, map &sfm_tracked_points) { feature_num = sfm_f.size(); // cout << "set 0 and " << l << " as known " << endl; //假设第l帧为原点，根据当前帧到第l帧的relative_R，relative_T，得到当前帧位姿 q[l].w() = 1; q[l].x() = 0; q[l].y() = 0; q[l].z() = 0; T[l].setZero(); q[frame_num - 1] = q[l] * Quaterniond(relative_R); T[frame_num - 1] = relative_T; // cout << "init q_l " << q[l].w() << " " << q[l].vec().transpose() << endl; // cout << "init t_l " << T[l].transpose() << endl; // rotate to cam frame Matrix3d c_Rotation[frame_num]; Vector3d c_Translation[frame_num]; Quaterniond c_Quat[frame_num]; double c_rotation[frame_num][4]; double c_translation[frame_num][3]; //这里的pose表示的是第l帧到每一帧的变换矩阵 Eigen::Matrix Pose[frame_num]; c_Quat[l] = q[l].inverse(); c_Rotation[l] = c_Quat[l].toRotationMatrix(); c_Translation[l] = -1 * (c_Rotation[l] * T[l]); Pose[l].block<3, 3>(0, 0) = c_Rotation[l]; Pose[l].block<3, 1>(0, 3) = c_Translation[l]; c_Quat[frame_num - 1] = q[frame_num - 1].inverse(); c_Rotation[frame_num - 1] = c_Quat[frame_num - 1].toRotationMatrix(); c_Translation[frame_num - 1] = -1 * (c_Rotation[frame_num - 1] * T[frame_num - 1]); Pose[frame_num - 1].block<3, 3>(0, 0) = c_Rotation[frame_num - 1]; Pose[frame_num - 1].block<3, 1>(0, 3) = c_Translation[frame_num - 1]; // 1: trangulate between l ----- frame_num - 1 // 2: solve pnp l + 1; trangulate l + 1 ------- frame_num - 1; for (int i = l; i < frame_num - 1; i++) { // solve pnp if (i > l) { Matrix3d R_initial = c_Rotation[i - 1]; Vector3d P_initial = c_Translation[i - 1]; if (!solveFrameByPnP(R_initial, P_initial, i, sfm_f)) return false; c_Rotation[i] = R_initial; c_Translation[i] = P_initial; c_Quat[i] = c_Rotation[i]; Pose[i].block<3, 3>(0, 0) = c_Rotation[i]; Pose[i].block<3, 1>(0, 3) = c_Translation[i]; } // triangulate point based on the solve pnp result triangulateTwoFramesWithDepth(i, Pose[i], frame_num - 1, Pose[frame_num - 1], sfm_f); } // 3: triangulate l-----l+1 l+2 ... frame_num -2 for (int i = l + 1; i < frame_num - 1; i++) triangulateTwoFramesWithDepth(l, Pose[l], i, Pose[i], sfm_f); // 4: solve pnp l-1; triangulate l-1 ----- l // l-2 l-2 ----- l for (int i = l - 1; i >= 0; i--) { // solve pnp Matrix3d R_initial = c_Rotation[i + 1]; Vector3d P_initial = c_Translation[i + 1]; if (!solveFrameByPnP(R_initial, P_initial, i, sfm_f)) return false; c_Rotation[i] = R_initial; c_Translation[i] = P_initial; c_Quat[i] = c_Rotation[i]; Pose[i].block<3, 3>(0, 0) = c_Rotation[i]; Pose[i].block<3, 1>(0, 3) = c_Translation[i]; // triangulate triangulateTwoFramesWithDepth(i, Pose[i], l, Pose[l], sfm_f); } // 5: triangulate all other points for (int j = 0; j < feature_num; j++) { if (sfm_f[j].state == true) continue; if ((int)sfm_f[j].observation.size() >= 2) { Vector3d point0; Vector2d point1; int frame_0 = sfm_f[j].observation[0].first; if (sfm_f[j].observation_depth[0].second < 0.1 || sfm_f[j].observation_depth[0].second > 10) // max and min measurement continue; point0 = Vector3d(sfm_f[j].observation[0].second.x() * sfm_f[j].observation_depth[0].second, sfm_f[j].observation[0].second.y() * sfm_f[j].observation_depth[0].second, sfm_f[j].observation_depth[0].second); int frame_1 = sfm_f[j].observation.back().first; point1 = sfm_f[j].observation.back().second; Vector3d point_3d; // triangulatePoint(Pose[frame_0], Pose[frame_1], point0, point1, // point_3d); Matrix3d Pose0_R = Pose[frame_0].block<3, 3>(0, 0); Matrix3d Pose1_R = Pose[frame_1].block<3, 3>(0, 0); Vector3d Pose0_t = Pose[frame_0].block<3, 1>(0, 3); Vector3d Pose1_t = Pose[frame_1].block<3, 1>(0, 3); Vector2d residual; Vector3d point1_reprojected; // triangulatePoint(Pose0, Pose1, point0, point1, point_3d); point_3d = Pose0_R.transpose() * point0 - Pose0_R.transpose() * Pose0_t; // point in world; point1_reprojected = Pose1_R * point_3d + Pose1_t; residual = point1 - Vector2d(point1_reprojected.x() / point1_reprojected.z(), point1_reprojected.y() / point1_reprojected.z()); if (residual.norm() < 1.0 / 460) { // reprojection error sfm_f[j].state = true; sfm_f[j].position[0] = point_3d(0); sfm_f[j].position[1] = point_3d(1); sfm_f[j].position[2] = point_3d(2); } // cout << "trangulated : " << frame_0 << " " << frame_1 << " 3d point : // " << j << " " << point_3d.transpose() << endl; } } /* for (int i = 0; i < frame_num; i++) { q[i] = c_Rotation[i].transpose(); cout << "solvePnP q" << " i " << i <<" " < &sfm_f, map &sfm_tracked_points) { feature_num = sfm_f.size(); // cout << "set 0 and " << l << " as known " << endl; //假设第l帧为原点，根据当前帧到第l帧的relative_R，relative_T，得到当前帧位姿 q[l].w() = 1; q[l].x() = 0; q[l].y() = 0; q[l].z() = 0; T[l].setZero(); q[frame_num - 1] = q[l] * Quaterniond(relative_R); T[frame_num - 1] = relative_T; // cout << "init q_l " << q[l].w() << " " << q[l].vec().transpose() << endl; // cout << "init t_l " << T[l].transpose() << endl; // rotate to cam frame Matrix3d c_Rotation[frame_num]; Vector3d c_Translation[frame_num]; Quaterniond c_Quat[frame_num]; double c_rotation[frame_num][4]; double c_translation[frame_num][3]; //这里的pose表示的是第l帧到每一帧的变换矩阵 Eigen::Matrix Pose[frame_num]; c_Quat[l] = q[l].inverse(); c_Rotation[l] = c_Quat[l].toRotationMatrix(); c_Translation[l] = -1 * (c_Rotation[l] * T[l]); Pose[l].block<3, 3>(0, 0) = c_Rotation[l]; Pose[l].block<3, 1>(0, 3) = c_Translation[l]; c_Quat[frame_num - 1] = q[frame_num - 1].inverse(); c_Rotation[frame_num - 1] = c_Quat[frame_num - 1].toRotationMatrix(); c_Translation[frame_num - 1] = -1 * (c_Rotation[frame_num - 1] * T[frame_num - 1]); Pose[frame_num - 1].block<3, 3>(0, 0) = c_Rotation[frame_num - 1]; Pose[frame_num - 1].block<3, 1>(0, 3) = c_Translation[frame_num - 1]; // 1: trangulate between l ----- frame_num - 1 // 2: solve pnp l + 1; trangulate l + 1 ------- frame_num - 1; for (int i = l; i < frame_num - 1; i++) { // solve pnp if (i > l) { Matrix3d R_initial = c_Rotation[i - 1]; Vector3d P_initial = c_Translation[i - 1]; cout << "i0 = " << i << endl; if (!solveFrameByPnP(R_initial, P_initial, i, sfm_f)) return false; c_Rotation[i] = R_initial; c_Translation[i] = P_initial; c_Quat[i] = c_Rotation[i]; Pose[i].block<3, 3>(0, 0) = c_Rotation[i]; Pose[i].block<3, 1>(0, 3) = c_Translation[i]; } // triangulate point based on the solve pnp result triangulateTwoFramesWithDepth(i, Pose[i], frame_num - 1, Pose[frame_num - 1], sfm_f); } // 3: triangulate l-----l+1 l+2 ... frame_num -2 for (int i = l + 1; i < frame_num - 1; i++) triangulateTwoFramesWithDepth(l, Pose[l], i, Pose[i], sfm_f); // 4: solve pnp l-1; triangulate l-1 ----- l // l-2 l-2 ----- l for (int i = l - 1; i >= 0; i--) { // solve pnp Matrix3d R_initial = c_Rotation[i + 1]; Vector3d P_initial = c_Translation[i + 1]; cout << "i1 = " << i << endl; if (!solveFrameByPnP(R_initial, P_initial, i, sfm_f)) return false; c_Rotation[i] = R_initial; c_Translation[i] = P_initial; c_Quat[i] = c_Rotation[i]; Pose[i].block<3, 3>(0, 0) = c_Rotation[i]; Pose[i].block<3, 1>(0, 3) = c_Translation[i]; // triangulate triangulateTwoFramesWithDepth(i, Pose[i], l, Pose[l], sfm_f); } // 5: triangulate all other points for (int j = 0; j < feature_num; j++) { if (sfm_f[j].state == true) continue; if ((int)sfm_f[j].observation.size() >= 2) { Vector3d point0; Vector2d point1; int frame_0 = sfm_f[j].observation[0].first; if (sfm_f[j].observation_depth[0].second < 0.1 || sfm_f[j].observation_depth[0].second > 10) // max and min measurement continue; point0 = Vector3d(sfm_f[j].observation[0].second.x() * sfm_f[j].observation_depth[0].second, sfm_f[j].observation[0].second.y() * sfm_f[j].observation_depth[0].second, sfm_f[j].observation_depth[0].second); int frame_1 = sfm_f[j].observation.back().first; point1 = sfm_f[j].observation.back().second; Vector3d point_3d; // triangulatePoint(Pose[frame_0], Pose[frame_1], point0, point1, // point_3d); Matrix3d Pose0_R = Pose[frame_0].block<3, 3>(0, 0); Matrix3d Pose1_R = Pose[frame_1].block<3, 3>(0, 0); Vector3d Pose0_t = Pose[frame_0].block<3, 1>(0, 3); Vector3d Pose1_t = Pose[frame_1].block<3, 1>(0, 3); Vector2d residual; Vector3d point1_reprojected; // triangulatePoint(Pose0, Pose1, point0, point1, point_3d); point_3d = Pose0_R.transpose() * point0 - Pose0_R.transpose() * Pose0_t; // point in world; point1_reprojected = Pose1_R * point_3d + Pose1_t; residual = point1 - Vector2d(point1_reprojected.x() / point1_reprojected.z(), point1_reprojected.y() / point1_reprojected.z()); if (residual.norm() < 1.0 / 460) { // reprojection error sfm_f[j].state = true; sfm_f[j].position[0] = point_3d(0); sfm_f[j].position[1] = point_3d(1); sfm_f[j].position[2] = point_3d(2); } // cout << "trangulated : " << frame_0 << " " << frame_1 << " 3d point : // " << j << " " << point_3d.transpose() << endl; } } // full BA ceres::Problem problem; ceres::LocalParameterization *local_parameterization = new ceres::QuaternionParameterization(); // cout << " begin full BA " << endl; for (int i = 0; i < frame_num; i++) { // double array for ceres c_translation[i][0] = c_Translation[i].x(); c_translation[i][1] = c_Translation[i].y(); c_translation[i][2] = c_Translation[i].z(); c_rotation[i][0] = c_Quat[i].w(); c_rotation[i][1] = c_Quat[i].x(); c_rotation[i][2] = c_Quat[i].y(); c_rotation[i][3] = c_Quat[i].z(); // https://blog.csdn.net/weixin_43991178/article/details/100532618 problem.AddParameterBlock(c_rotation[i], 4, local_parameterization); problem.AddParameterBlock(c_translation[i], 3); // 固定（不作优化变量）选定参考帧（第l帧）的旋转 if (i == l) { problem.SetParameterBlockConstant(c_rotation[i]); } // 固定（不作优化变量）选定参考帧（第l帧）和当前帧的平移 if (i == l || i == frame_num - 1) { problem.SetParameterBlockConstant(c_translation[i]); } } for (int i = 0; i < feature_num; i++) { if (sfm_f[i].state != true) continue; for (int j = 0; j < int(sfm_f[i].observation.size()); j++) { int l = sfm_f[i].observation[j].first; ceres::CostFunction *cost_function = ReprojectionError3D::Create( sfm_f[i].observation[j].second.x(), sfm_f[i].observation[j].second.y()); problem.AddResidualBlock(cost_function, NULL, c_rotation[l], c_translation[l], sfm_f[i].position); } } ceres::Solver::Options options; options.linear_solver_type = ceres::DENSE_SCHUR; // options.minimizer_progress_to_stdout = true; options.max_solver_time_in_seconds = 0.2; ceres::Solver::Summary summary; ceres::Solve(options, &problem, &summary); // std::cout << summary.BriefReport() << "\n"; if (summary.termination_type == ceres::CONVERGENCE || summary.final_cost < 5e-03) { // cout << "vision only BA converge" << endl; } else { // cout << "vision only BA not converge " << endl; return false; } for (int i = 0; i < frame_num; i++) { q[i].w() = c_rotation[i][0]; q[i].x() = c_rotation[i][1]; q[i].y() = c_rotation[i][2]; q[i].z() = c_rotation[i][3]; q[i] = q[i].inverse(); // cout << "final q" << " i " << i <<" " < #include #include #include #include #include #include #include #include using namespace Eigen; using namespace std; struct SFMFeature { bool state; int id; vector> observation; double position[3]; double depth; vector> observation_depth; }; struct ReprojectionError3D { ReprojectionError3D(double observed_u, double observed_v) : observed_u(observed_u), observed_v(observed_v) { } template bool operator()(const T *const camera_R, const T *const camera_T, const T *point, T *residuals) const { T p[3]; ceres::QuaternionRotatePoint(camera_R, point, p); p[0] += camera_T[0]; p[1] += camera_T[1]; p[2] += camera_T[2]; T xp = p[0] / p[2]; T yp = p[1] / p[2]; residuals[0] = xp - T(observed_u); residuals[1] = yp - T(observed_v); return true; } static ceres::CostFunction *Create(const double observed_x, const double observed_y) { return (new ceres::AutoDiffCostFunction( new ReprojectionError3D(observed_x, observed_y))); } double observed_u; double observed_v; }; class GlobalSFM { public: GlobalSFM(); bool construct(int frame_num, Quaterniond *q, Vector3d *T, int l, const Matrix3d relative_R, const Vector3d relative_T, vector &sfm_f, map &sfm_tracked_points); bool constructWithDepth(int frame_num, Quaterniond *q, Vector3d *T, int l, const Matrix3d relative_R, const Vector3d relative_T, vector &sfm_f, map &sfm_tracked_points); private: bool solveFrameByPnP(Matrix3d &R_initial, Vector3d &P_initial, int i, vector &sfm_f); void triangulatePoint(Eigen::Matrix &Pose0, Eigen::Matrix &Pose1, Vector2d &point0, Vector2d &point1, Vector3d &point_3d); void triangulateTwoFrames(int frame0, Eigen::Matrix &Pose0, int frame1, Eigen::Matrix &Pose1, vector &sfm_f); void triangulateTwoFramesWithDepth(int frame0, Eigen::Matrix &Pose0, int frame1, Eigen::Matrix &Pose1, vector &sfm_f); int feature_num; }; ================================================ FILE: vins_estimator/src/initial/solve_5pts.cpp ================================================ #include "solve_5pts.h" #include #include using Sophus::SE3; using Sophus::SO3; namespace cv { void decomposeEssentialMat(InputArray _E, OutputArray _R1, OutputArray _R2, OutputArray _t) { Mat E = _E.getMat().reshape(1, 3); CV_Assert(E.cols == 3 && E.rows == 3); Mat D, U, Vt; SVD::compute(E, D, U, Vt); if (determinant(U) < 0) U *= -1.; if (determinant(Vt) < 0) Vt *= -1.; Mat W = (Mat_(3, 3) << 0, 1, 0, -1, 0, 0, 0, 0, 1); W.convertTo(W, E.type()); Mat R1, R2, t; R1 = U * W * Vt; R2 = U * W.t() * Vt; t = U.col(2) * 1.0; R1.copyTo(_R1); R2.copyTo(_R2); t.copyTo(_t); } int recoverPose(InputArray E, InputArray _points1, InputArray _points2, InputArray _cameraMatrix, OutputArray _R, OutputArray _t, InputOutputArray _mask) { Mat points1, points2, cameraMatrix; _points1.getMat().convertTo(points1, CV_64F); _points2.getMat().convertTo(points2, CV_64F); _cameraMatrix.getMat().convertTo(cameraMatrix, CV_64F); int npoints = points1.checkVector(2); CV_Assert(npoints >= 0 && points2.checkVector(2) == npoints && points1.type() == points2.type()); CV_Assert(cameraMatrix.rows == 3 && cameraMatrix.cols == 3 && cameraMatrix.channels() == 1); if (points1.channels() > 1) { points1 = points1.reshape(1, npoints); points2 = points2.reshape(1, npoints); } double fx = cameraMatrix.at(0, 0); double fy = cameraMatrix.at(1, 1); double cx = cameraMatrix.at(0, 2); double cy = cameraMatrix.at(1, 2); points1.col(0) = (points1.col(0) - cx) / fx; points2.col(0) = (points2.col(0) - cx) / fx; points1.col(1) = (points1.col(1) - cy) / fy; points2.col(1) = (points2.col(1) - cy) / fy; points1 = points1.t(); points2 = points2.t(); Mat R1, R2, t; decomposeEssentialMat(E, R1, R2, t); Mat P0 = Mat::eye(3, 4, R1.type()); Mat P1(3, 4, R1.type()), P2(3, 4, R1.type()), P3(3, 4, R1.type()), P4(3, 4, R1.type()); P1(Range::all(), Range(0, 3)) = R1 * 1.0; P1.col(3) = t * 1.0; P2(Range::all(), Range(0, 3)) = R2 * 1.0; P2.col(3) = t * 1.0; P3(Range::all(), Range(0, 3)) = R1 * 1.0; P3.col(3) = -t * 1.0; P4(Range::all(), Range(0, 3)) = R2 * 1.0; P4.col(3) = -t * 1.0; // Do the cheirality check. // Notice here a threshold dist is used to filter // out far away points (i.e. infinite points) since // there depth may vary between postive and negtive. double dist = 50.0; Mat Q; triangulatePoints(P0, P1, points1, points2, Q); Mat mask1 = Q.row(2).mul(Q.row(3)) > 0; Q.row(0) /= Q.row(3); Q.row(1) /= Q.row(3); Q.row(2) /= Q.row(3); Q.row(3) /= Q.row(3); mask1 = (Q.row(2) < dist) & mask1; Q = P1 * Q; mask1 = (Q.row(2) > 0) & mask1; mask1 = (Q.row(2) < dist) & mask1; triangulatePoints(P0, P2, points1, points2, Q); Mat mask2 = Q.row(2).mul(Q.row(3)) > 0; Q.row(0) /= Q.row(3); Q.row(1) /= Q.row(3); Q.row(2) /= Q.row(3); Q.row(3) /= Q.row(3); mask2 = (Q.row(2) < dist) & mask2; Q = P2 * Q; mask2 = (Q.row(2) > 0) & mask2; mask2 = (Q.row(2) < dist) & mask2; triangulatePoints(P0, P3, points1, points2, Q); Mat mask3 = Q.row(2).mul(Q.row(3)) > 0; Q.row(0) /= Q.row(3); Q.row(1) /= Q.row(3); Q.row(2) /= Q.row(3); Q.row(3) /= Q.row(3); mask3 = (Q.row(2) < dist) & mask3; Q = P3 * Q; mask3 = (Q.row(2) > 0) & mask3; mask3 = (Q.row(2) < dist) & mask3; triangulatePoints(P0, P4, points1, points2, Q); Mat mask4 = Q.row(2).mul(Q.row(3)) > 0; Q.row(0) /= Q.row(3); Q.row(1) /= Q.row(3); Q.row(2) /= Q.row(3); Q.row(3) /= Q.row(3); mask4 = (Q.row(2) < dist) & mask4; Q = P4 * Q; mask4 = (Q.row(2) > 0) & mask4; mask4 = (Q.row(2) < dist) & mask4; mask1 = mask1.t(); mask2 = mask2.t(); mask3 = mask3.t(); mask4 = mask4.t(); // If _mask is given, then use it to filter outliers. if (!_mask.empty()) { Mat mask = _mask.getMat(); CV_Assert(mask.size() == mask1.size()); bitwise_and(mask, mask1, mask1); bitwise_and(mask, mask2, mask2); bitwise_and(mask, mask3, mask3); bitwise_and(mask, mask4, mask4); } if (_mask.empty() && _mask.needed()) { _mask.create(mask1.size(), CV_8U); } CV_Assert(_R.needed() && _t.needed()); _R.create(3, 3, R1.type()); _t.create(3, 1, t.type()); int good1 = countNonZero(mask1); int good2 = countNonZero(mask2); int good3 = countNonZero(mask3); int good4 = countNonZero(mask4); if (good1 >= good2 && good1 >= good3 && good1 >= good4) { R1.copyTo(_R); t.copyTo(_t); if (_mask.needed()) mask1.copyTo(_mask); return good1; } else if (good2 >= good1 && good2 >= good3 && good2 >= good4) { R2.copyTo(_R); t.copyTo(_t); if (_mask.needed()) mask2.copyTo(_mask); return good2; } else if (good3 >= good1 && good3 >= good2 && good3 >= good4) { t = -t; R1.copyTo(_R); t.copyTo(_t); if (_mask.needed()) mask3.copyTo(_mask); return good3; } else { t = -t; R2.copyTo(_R); t.copyTo(_t); if (_mask.needed()) mask4.copyTo(_mask); return good4; } } int recoverPose(InputArray E, InputArray _points1, InputArray _points2, OutputArray _R, OutputArray _t, double focal, Point2d pp, InputOutputArray _mask) { Mat cameraMatrix = (Mat_(3, 3) << focal, 0, pp.x, 0, focal, pp.y, 0, 0, 1); return cv::recoverPose(E, _points1, _points2, cameraMatrix, _R, _t, _mask); } } // namespace cv bool MotionEstimator::solveRelativeRT(const vector> &corres, Matrix3d &Rotation, Vector3d &Translation) { if (corres.size() >= 15) { vector ll, rr; for (int i = 0; i < int(corres.size()); i++) { ll.push_back(cv::Point2f(corres[i].first(0), corres[i].first(1))); rr.push_back(cv::Point2f(corres[i].second(0), corres[i].second(1))); } cv::Mat mask; cv::Mat E = cv::findFundamentalMat(ll, rr, cv::FM_RANSAC, 0.3 / 460, 0.99, mask); cv::Mat cameraMatrix = (cv::Mat_(3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 1); cv::Mat rot, trans; int inlier_cnt = cv::recoverPose(E, ll, rr, cameraMatrix, rot, trans, mask); // cout << "inlier_cnt " << inlier_cnt << endl; Eigen::Matrix3d R; Eigen::Vector3d T; for (int i = 0; i < 3; i++) { T(i) = trans.at(i, 0); for (int j = 0; j < 3; j++) R(i, j) = rot.at(i, j); } Rotation = R.transpose(); Translation = -R.transpose() * T; if (inlier_cnt > 12) { ROS_ERROR_STREAM("----------5points-----------"); ROS_ERROR_STREAM(ll.size()); ROS_ERROR_STREAM(inlier_cnt); ROS_ERROR_STREAM(Rotation); ROS_ERROR_STREAM(Translation); return true; } else return false; } return false; } bool MotionEstimator::solveRelativeRT_PNP(const vector> &corres, Matrix3d &Rotation, Vector3d &Translation) { vector lll; vector rr; for (int i = 0; i < int(corres.size()); i++) { if (corres[i].first(2) > 0 && corres[i].second(2) > 0) { lll.push_back(cv::Point3f(corres[i].first(0), corres[i].first(1), corres[i].first(2))); rr.push_back(cv::Point2f(corres[i].second(0) / corres[i].second(2), corres[i].second(1) / corres[i].second(2))); } } cv::Mat rvec, tvec, inliersArr; cv::Mat cameraMatrix = (cv::Mat_(3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 1); cv::solvePnPRansac(lll, rr, cameraMatrix, cv::Mat(), rvec, tvec, false, 100, 1.0 / 460, 0.99, inliersArr, cv::SOLVEPNP_EPNP); // maybe don't need 100times Vector3d tran(tvec.at(0, 0), tvec.at(1, 0), tvec.at(2, 0)); Matrix3d rota = SO3(rvec.at(0, 0), rvec.at(1, 0), rvec.at(2, 0)).matrix(); // Vector2d tp1,residualV; // Vector3d tp23d; // for (int i = 0; i < int(lll.size()); i++) { // tp1 = Vector2d(rr[i].x,rr[i].y); // tp23d = Vector3d(lll[i].x,lll[i].y,lll[i].z); // // tp23d = rota * tp23d + tran; // Vector2d tp2(tp23d.x()/tp23d.z(),tp23d.y()/tp23d.z()); // // residualV = (tp2 - tp1); // ROS_ERROR_STREAM(residualV.transpose()); // } ROS_DEBUG_STREAM("-----------pnp-----------"); ROS_DEBUG_STREAM(lll.size()); ROS_DEBUG_STREAM(inliersArr.rows); Rotation = rota.transpose(); Translation = -rota.transpose() * tran; ROS_DEBUG_STREAM(Rotation); ROS_DEBUG_STREAM(Translation); return true; } ================================================ FILE: vins_estimator/src/initial/solve_5pts.h ================================================ #pragma once #include using namespace std; #include //#include #include using namespace Eigen; #include class MotionEstimator { public: bool solveRelativeRT(const vector> &corres, Matrix3d &R, Vector3d &T); bool solveRelativeRT_ICP(vector> &corres, Matrix3d &R, Vector3d &T); bool solveRelativeRT_PNP(const vector> &corres, Matrix3d &Rotation, Vector3d &Translation); private: double testTriangulation(const vector &l, const vector &r, cv::Mat_ R, cv::Mat_ t); void decomposeE(cv::Mat E, cv::Mat_ &R1, cv::Mat_ &R2, cv::Mat_ &t1, cv::Mat_ &t2); }; ================================================ FILE: vins_estimator/src/utility/CameraPoseVisualization.cpp ================================================ #include "CameraPoseVisualization.h" const Eigen::Vector3d CameraPoseVisualization::imlt = Eigen::Vector3d(-1.0, -0.5, 1.0); const Eigen::Vector3d CameraPoseVisualization::imrt = Eigen::Vector3d(1.0, -0.5, 1.0); const Eigen::Vector3d CameraPoseVisualization::imlb = Eigen::Vector3d(-1.0, 0.5, 1.0); const Eigen::Vector3d CameraPoseVisualization::imrb = Eigen::Vector3d(1.0, 0.5, 1.0); const Eigen::Vector3d CameraPoseVisualization::lt0 = Eigen::Vector3d(-0.7, -0.5, 1.0); const Eigen::Vector3d CameraPoseVisualization::lt1 = Eigen::Vector3d(-0.7, -0.2, 1.0); const Eigen::Vector3d CameraPoseVisualization::lt2 = Eigen::Vector3d(-1.0, -0.2, 1.0); const Eigen::Vector3d CameraPoseVisualization::oc = Eigen::Vector3d(0.0, 0.0, 0.0); void Eigen2Point(const Eigen::Vector3d &v, geometry_msgs::Point &p) { p.x = v.x(); p.y = v.y(); p.z = v.z(); } CameraPoseVisualization::CameraPoseVisualization(float r, float g, float b, float a) : m_marker_ns("CameraPoseVisualization"), m_scale(0.2), m_line_width(0.01) { m_image_boundary_color.r = r; m_image_boundary_color.g = g; m_image_boundary_color.b = b; m_image_boundary_color.a = a; m_optical_center_connector_color.r = r; m_optical_center_connector_color.g = g; m_optical_center_connector_color.b = b; m_optical_center_connector_color.a = a; } void CameraPoseVisualization::setImageBoundaryColor(float r, float g, float b, float a) { m_image_boundary_color.r = r; m_image_boundary_color.g = g; m_image_boundary_color.b = b; m_image_boundary_color.a = a; } void CameraPoseVisualization::setOpticalCenterConnectorColor(float r, float g, float b, float a) { m_optical_center_connector_color.r = r; m_optical_center_connector_color.g = g; m_optical_center_connector_color.b = b; m_optical_center_connector_color.a = a; } void CameraPoseVisualization::setScale(double s) { m_scale = s; } void CameraPoseVisualization::setLineWidth(double width) { m_line_width = width; } void CameraPoseVisualization::add_edge(const Eigen::Vector3d &p0, const Eigen::Vector3d &p1) { visualization_msgs::Marker marker; marker.ns = m_marker_ns; marker.id = m_markers.size() + 1; marker.type = visualization_msgs::Marker::LINE_LIST; marker.action = visualization_msgs::Marker::ADD; marker.scale.x = 0.005; marker.color.g = 1.0f; marker.color.a = 1.0; geometry_msgs::Point point0, point1; Eigen2Point(p0, point0); Eigen2Point(p1, point1); marker.points.push_back(point0); marker.points.push_back(point1); m_markers.push_back(marker); } void CameraPoseVisualization::add_loopedge(const Eigen::Vector3d &p0, const Eigen::Vector3d &p1) { visualization_msgs::Marker marker; marker.ns = m_marker_ns; marker.id = m_markers.size() + 1; marker.type = visualization_msgs::Marker::LINE_LIST; marker.action = visualization_msgs::Marker::ADD; marker.scale.x = 0.04; // marker.scale.x = 0.3; marker.color.r = 1.0f; marker.color.b = 1.0f; marker.color.a = 1.0; geometry_msgs::Point point0, point1; Eigen2Point(p0, point0); Eigen2Point(p1, point1); marker.points.push_back(point0); marker.points.push_back(point1); m_markers.push_back(marker); } void CameraPoseVisualization::add_pose(const Eigen::Vector3d &p, const Eigen::Quaterniond &q) { visualization_msgs::Marker marker; marker.ns = m_marker_ns; marker.id = m_markers.size() + 1; marker.type = visualization_msgs::Marker::LINE_STRIP; marker.action = visualization_msgs::Marker::ADD; marker.scale.x = m_line_width; marker.pose.position.x = 0.0; marker.pose.position.y = 0.0; marker.pose.position.z = 0.0; marker.pose.orientation.w = 1.0; marker.pose.orientation.x = 0.0; marker.pose.orientation.y = 0.0; marker.pose.orientation.z = 0.0; geometry_msgs::Point pt_lt, pt_lb, pt_rt, pt_rb, pt_oc, pt_lt0, pt_lt1, pt_lt2; Eigen2Point(q * (m_scale * imlt) + p, pt_lt); Eigen2Point(q * (m_scale * imlb) + p, pt_lb); Eigen2Point(q * (m_scale * imrt) + p, pt_rt); Eigen2Point(q * (m_scale * imrb) + p, pt_rb); Eigen2Point(q * (m_scale * lt0) + p, pt_lt0); Eigen2Point(q * (m_scale * lt1) + p, pt_lt1); Eigen2Point(q * (m_scale * lt2) + p, pt_lt2); Eigen2Point(q * (m_scale * oc) + p, pt_oc); // image boundaries marker.points.push_back(pt_lt); marker.points.push_back(pt_lb); marker.colors.push_back(m_image_boundary_color); marker.colors.push_back(m_image_boundary_color); marker.points.push_back(pt_lb); marker.points.push_back(pt_rb); marker.colors.push_back(m_image_boundary_color); marker.colors.push_back(m_image_boundary_color); marker.points.push_back(pt_rb); marker.points.push_back(pt_rt); marker.colors.push_back(m_image_boundary_color); marker.colors.push_back(m_image_boundary_color); marker.points.push_back(pt_rt); marker.points.push_back(pt_lt); marker.colors.push_back(m_image_boundary_color); marker.colors.push_back(m_image_boundary_color); // top-left indicator marker.points.push_back(pt_lt0); marker.points.push_back(pt_lt1); marker.colors.push_back(m_image_boundary_color); marker.colors.push_back(m_image_boundary_color); marker.points.push_back(pt_lt1); marker.points.push_back(pt_lt2); marker.colors.push_back(m_image_boundary_color); marker.colors.push_back(m_image_boundary_color); // optical center connector marker.points.push_back(pt_lt); marker.points.push_back(pt_oc); marker.colors.push_back(m_optical_center_connector_color); marker.colors.push_back(m_optical_center_connector_color); marker.points.push_back(pt_lb); marker.points.push_back(pt_oc); marker.colors.push_back(m_optical_center_connector_color); marker.colors.push_back(m_optical_center_connector_color); marker.points.push_back(pt_rt); marker.points.push_back(pt_oc); marker.colors.push_back(m_optical_center_connector_color); marker.colors.push_back(m_optical_center_connector_color); marker.points.push_back(pt_rb); marker.points.push_back(pt_oc); marker.colors.push_back(m_optical_center_connector_color); marker.colors.push_back(m_optical_center_connector_color); m_markers.push_back(marker); } void CameraPoseVisualization::reset() { m_markers.clear(); } void CameraPoseVisualization::publish_by(ros::Publisher &pub, const std_msgs::Header &header) { visualization_msgs::MarkerArray markerArray_msg; for (auto &marker : m_markers) { marker.header = header; markerArray_msg.markers.push_back(marker); } pub.publish(markerArray_msg); } ================================================ FILE: vins_estimator/src/utility/CameraPoseVisualization.h ================================================ #pragma once #include #include #include #include #include #include class CameraPoseVisualization { public: std::string m_marker_ns; CameraPoseVisualization(float r, float g, float b, float a); void setImageBoundaryColor(float r, float g, float b, float a = 1.0); void setOpticalCenterConnectorColor(float r, float g, float b, float a = 1.0); void setScale(double s); void setLineWidth(double width); void add_pose(const Eigen::Vector3d &p, const Eigen::Quaterniond &q); void reset(); void publish_by(ros::Publisher &pub, const std_msgs::Header &header); void add_edge(const Eigen::Vector3d &p0, const Eigen::Vector3d &p1); void add_loopedge(const Eigen::Vector3d &p0, const Eigen::Vector3d &p1); private: std::vector m_markers; std_msgs::ColorRGBA m_image_boundary_color; std_msgs::ColorRGBA m_optical_center_connector_color; double m_scale; double m_line_width; static const Eigen::Vector3d imlt; static const Eigen::Vector3d imlb; static const Eigen::Vector3d imrt; static const Eigen::Vector3d imrb; static const Eigen::Vector3d oc; static const Eigen::Vector3d lt0; static const Eigen::Vector3d lt1; static const Eigen::Vector3d lt2; }; ================================================ FILE: vins_estimator/src/utility/parameters.cpp ================================================ #include "parameters.h" #include double INIT_DEPTH; double MIN_PARALLAX; double ACC_N, ACC_W; double GYR_N, GYR_W; std::vector RIC; std::vector TIC; Eigen::Vector3d G{0.0, 0.0, 9.8}; double BIAS_ACC_THRESHOLD; double BIAS_GYR_THRESHOLD; double SOLVER_TIME; int NUM_ITERATIONS; int ESTIMATE_EXTRINSIC; int ESTIMATE_TD; int ROLLING_SHUTTER; std::string EX_CALIB_RESULT_PATH; std::string VINS_RESULT_PATH; std::string IMAGE_TOPIC; std::string DEPTH_TOPIC; std::string IMU_TOPIC; int MAX_CNT; int MAX_CNT_SET; int MIN_DIST; int FREQ; double F_THRESHOLD; int SHOW_TRACK; int EQUALIZE; int FISHEYE; std::string FISHEYE_MASK; std::string CAM_NAMES; int STEREO_TRACK; bool PUB_THIS_FRAME; Eigen::Matrix3d Ric; double ROW, COL; int IMAGE_SIZE; double TD, TR; double DEPTH_MIN_DIST; double DEPTH_MAX_DIST; unsigned short DEPTH_MIN_DIST_MM; unsigned short DEPTH_MAX_DIST_MM; std::vector SEMANTIC_LABEL; std::vector STATIC_LABEL; std::vector DYNAMIC_LABEL; int NUM_GRID_ROWS; int NUM_GRID_COLS; int FRONTEND_FREQ; int USE_IMU; int NUM_THREADS; int STATIC_INIT; int FIX_DEPTH; template T readParam(ros::NodeHandle &n, std::string name) { T ans; if (n.getParam(name, ans)) { ROS_INFO_STREAM("Loaded " << name << ": " << ans); } else { ROS_ERROR_STREAM("Failed to load " << name); n.shutdown(); } return ans; } // https://blog.csdn.net/qq_16952303/article/details/80259660 void readParameters(ros::NodeHandle &n) { std::string config_file; config_file = readParam(n, "config_file"); cv::FileStorage fsSettings; fsSettings.open(config_file, cv::FileStorage::READ); if (!fsSettings.isOpened()) { std::cerr << "ERROR: Wrong path to settings" << std::endl; } NUM_THREADS = fsSettings["num_threads"]; if (NUM_THREADS <= 1) { NUM_THREADS = std::thread::hardware_concurrency(); } fsSettings["image_topic"] >> IMAGE_TOPIC; fsSettings["depth_topic"] >> DEPTH_TOPIC; MAX_CNT = fsSettings["max_cnt"]; MAX_CNT_SET = MAX_CNT; MIN_DIST = fsSettings["min_dist"]; FREQ = fsSettings["freq"]; F_THRESHOLD = fsSettings["F_threshold"]; SHOW_TRACK = fsSettings["show_track"]; EQUALIZE = fsSettings["equalize"]; FISHEYE = fsSettings["fisheye"]; std::string VINS_FOLDER_PATH = readParam(n, "vins_folder"); if (FISHEYE == 1) FISHEYE_MASK = VINS_FOLDER_PATH + "config/fisheye_mask.jpg"; CAM_NAMES = config_file; DEPTH_MIN_DIST = fsSettings["depth_min_dist"]; DEPTH_MAX_DIST = fsSettings["depth_max_dist"]; DEPTH_MIN_DIST_MM = DEPTH_MIN_DIST * 1000; DEPTH_MAX_DIST_MM = DEPTH_MAX_DIST * 1000; NUM_GRID_ROWS = fsSettings["num_grid_rows"]; NUM_GRID_COLS = fsSettings["num_grid_cols"]; ROS_INFO("NUM_GRID_ROWS: %d, NUM_GRID_COLS: %d", NUM_GRID_ROWS, NUM_GRID_COLS); FRONTEND_FREQ = fsSettings["frontend_freq"]; ROS_INFO("FRONTEND_FREQ: %d", FRONTEND_FREQ); STEREO_TRACK = false; PUB_THIS_FRAME = false; if (FREQ == 0) FREQ = 100; SOLVER_TIME = fsSettings["max_solver_time"]; NUM_ITERATIONS = fsSettings["max_num_iterations"]; MIN_PARALLAX = fsSettings["keyframe_parallax"]; ROS_INFO("keyframe_parallax: %f", MIN_PARALLAX); MIN_PARALLAX = MIN_PARALLAX / FOCAL_LENGTH; std::string OUTPUT_PATH; fsSettings["output_path"] >> OUTPUT_PATH; VINS_RESULT_PATH = OUTPUT_PATH + "/vins_result_no_loop.csv"; std::ofstream fout(VINS_RESULT_PATH, std::ios::out); fout.close(); USE_IMU = fsSettings["imu"]; ROS_INFO("USE_IMU: %d\n", USE_IMU); if (USE_IMU) { fsSettings["imu_topic"] >> IMU_TOPIC; printf("IMU_TOPIC: %s\n", IMU_TOPIC.c_str()); ACC_N = fsSettings["acc_n"]; ACC_W = fsSettings["acc_w"]; GYR_N = fsSettings["gyr_n"]; GYR_W = fsSettings["gyr_w"]; G.z() = fsSettings["g_norm"]; } ROW = fsSettings["image_height"]; COL = fsSettings["image_width"]; IMAGE_SIZE = ROW * COL; ROS_INFO("ROW: %f COL: %f ", ROW, COL); for (auto iter : fsSettings["semantic_label"]) { SEMANTIC_LABEL.emplace_back(iter.string()); } for (auto iter : fsSettings["static_label"]) { STATIC_LABEL.emplace_back(iter.string()); } for (auto iter : fsSettings["dynamic_label"]) { DYNAMIC_LABEL.emplace_back(iter.string()); } ESTIMATE_EXTRINSIC = fsSettings["estimate_extrinsic"]; if (ESTIMATE_EXTRINSIC == 2) { ROS_WARN("have no prior about extrinsic param, calibrate extrinsic param"); RIC.emplace_back(Eigen::Matrix3d::Identity()); TIC.emplace_back(Eigen::Vector3d::Zero()); EX_CALIB_RESULT_PATH = OUTPUT_PATH + "/extrinsic_parameter.txt"; } else { if (ESTIMATE_EXTRINSIC == 1) { ROS_WARN(" Optimize extrinsic param around initial guess!"); EX_CALIB_RESULT_PATH = OUTPUT_PATH + "/extrinsic_parameter.txt"; } if (ESTIMATE_EXTRINSIC == 0) ROS_WARN(" fix extrinsic param "); cv::Mat cv_R, cv_T; fsSettings["extrinsicRotation"] >> cv_R; fsSettings["extrinsicTranslation"] >> cv_T; Eigen::Matrix3d eigen_R; Eigen::Vector3d eigen_T; cv::cv2eigen(cv_R, eigen_R); cv::cv2eigen(cv_T, eigen_T); Eigen::Quaterniond Q(eigen_R); eigen_R = Q.normalized(); Ric = eigen_R; RIC.push_back(eigen_R); TIC.push_back(eigen_T); ROS_INFO_STREAM("Extrinsic_R : " << std::endl << RIC[0]); ROS_INFO_STREAM("Extrinsic_T : " << std::endl << TIC[0].transpose()); } INIT_DEPTH = 5.0; BIAS_ACC_THRESHOLD = 0.1; BIAS_GYR_THRESHOLD = 0.1; TD = fsSettings["td"]; ESTIMATE_TD = fsSettings["estimate_td"]; if (ESTIMATE_TD) ROS_INFO_STREAM("Unsynchronized sensors, online estimate time offset, initial td: " << TD); else ROS_INFO_STREAM("Synchronized sensors, fix time offset: " << TD); ROLLING_SHUTTER = fsSettings["rolling_shutter"]; if (ROLLING_SHUTTER) { TR = fsSettings["rolling_shutter_tr"]; ROS_INFO_STREAM("rolling shutter camera, read out time per line: " << TR); } else { TR = 0; } STATIC_INIT = fsSettings["static_init"]; if (!fsSettings["fix_depth"].empty()) FIX_DEPTH = fsSettings["fix_depth"]; else FIX_DEPTH = 1; fsSettings.release(); } ================================================ FILE: vins_estimator/src/utility/parameters.h ================================================ #pragma once #include "utility.h" #include #include #include #include #include #include const double FOCAL_LENGTH = 460.0; const int WINDOW_SIZE = 10; const int NUM_OF_CAM = 1; const int NUM_OF_F = 1000; //#define UNIT_SPHERE_ERROR extern double INIT_DEPTH; extern double MIN_PARALLAX; extern int ESTIMATE_EXTRINSIC; extern double ACC_N, ACC_W; extern double GYR_N, GYR_W; extern std::vector RIC; extern std::vector TIC; extern Eigen::Vector3d G; extern double BIAS_ACC_THRESHOLD; extern double BIAS_GYR_THRESHOLD; extern double SOLVER_TIME; extern int NUM_ITERATIONS; extern std::string EX_CALIB_RESULT_PATH; extern std::string VINS_RESULT_PATH; extern std::string IMAGE_TOPIC; extern std::string DEPTH_TOPIC; extern std::string IMU_TOPIC; extern double TD; extern double TR; extern int ESTIMATE_TD; extern int ROLLING_SHUTTER; extern double ROW, COL; extern int IMAGE_SIZE; extern double DEPTH_MIN_DIST; extern double DEPTH_MAX_DIST; extern unsigned short DEPTH_MIN_DIST_MM; extern unsigned short DEPTH_MAX_DIST_MM; extern int MAX_CNT; extern int MAX_CNT_SET; extern int MIN_DIST; extern int FREQ; extern double F_THRESHOLD; extern int SHOW_TRACK; extern int EQUALIZE; extern int FISHEYE; extern std::string FISHEYE_MASK; extern std::string CAM_NAMES; extern int STEREO_TRACK; extern bool PUB_THIS_FRAME; extern Eigen::Matrix3d Ric; extern std::vector SEMANTIC_LABEL; extern std::vector STATIC_LABEL; extern std::vector DYNAMIC_LABEL; extern int NUM_GRID_ROWS; extern int NUM_GRID_COLS; extern int FRONTEND_FREQ; extern int USE_IMU; extern int NUM_THREADS; extern int STATIC_INIT; extern int FIX_DEPTH; void readParameters(ros::NodeHandle &n); enum SIZE_PARAMETERIZATION { SIZE_POSE = 7, SIZE_SPEEDBIAS = 9, // SIZE_SPEED = 3, // SIZE_BIAS = 6, SIZE_FEATURE = 1 }; enum StateOrder { O_P = 0, O_R = 3, O_V = 6, O_BA = 9, O_BG = 12 }; enum NoiseOrder { O_AN = 0, O_GN = 3, O_AW = 6, O_GW = 9 }; ================================================ FILE: vins_estimator/src/utility/tic_toc.h ================================================ #pragma once #include #include #include class TicToc { public: TicToc() { tic(); } void tic() { start = std::chrono::system_clock::now(); } double toc() { end = std::chrono::system_clock::now(); std::chrono::duration elapsed_seconds = end - start; return elapsed_seconds.count() * 1000; } private: std::chrono::time_point start, end; }; ================================================ FILE: vins_estimator/src/utility/utility.cpp ================================================ #include "utility.h" // https://blog.csdn.net/huanghaihui_123/article/details/103075107#commentBox // R_W_I Eigen::Matrix3d Utility::g2R(const Eigen::Vector3d &g) { Eigen::Matrix3d R0; Eigen::Vector3d ng1 = g.normalized(); Eigen::Vector3d ng2{0, 0, 1.0}; R0 = Eigen::Quaterniond::FromTwoVectors(ng1, ng2).toRotationMatrix(); double yaw = Utility::R2ypr(R0).x(); R0 = Utility::ypr2R(Eigen::Vector3d{-yaw, 0, 0}) * R0; // R0 = Utility::ypr2R(Eigen::Vector3d{-90, 0, 0}) * R0; return R0; } ================================================ FILE: vins_estimator/src/utility/utility.h ================================================ #pragma once #include #include #include #include class Utility { public: template static Eigen::Quaternion deltaQ(const Eigen::MatrixBase &theta) { typedef typename Derived::Scalar Scalar_t; Eigen::Quaternion dq; Eigen::Matrix half_theta = theta; half_theta /= static_cast(2.0); dq.w() = static_cast(1.0); dq.x() = half_theta.x(); dq.y() = half_theta.y(); dq.z() = half_theta.z(); return dq; } template static Eigen::Matrix skewSymmetric(const Eigen::MatrixBase &q) { Eigen::Matrix ans; ans << typename Derived::Scalar(0), -q(2), q(1), q(2), typename Derived::Scalar(0), -q(0), -q(1), q(0), typename Derived::Scalar(0); return ans; } template static Eigen::Quaternion positify(const Eigen::QuaternionBase &q) { //printf("a: %f %f %f %f", q.w(), q.x(), q.y(), q.z()); //Eigen::Quaternion p(-q.w(), -q.x(), -q.y(), -q.z()); //printf("b: %f %f %f %f", p.w(), p.x(), p.y(), p.z()); //return q.template w() >= (typename Derived::Scalar)(0.0) ? q : Eigen::Quaternion(-q.w(), -q.x(), -q.y(), -q.z()); return q; } template static Eigen::Matrix Qleft(const Eigen::QuaternionBase &q) { Eigen::Quaternion qq = positify(q); Eigen::Matrix ans; ans(0, 0) = qq.w(), ans.template block<1, 3>(0, 1) = -qq.vec().transpose(); ans.template block<3, 1>(1, 0) = qq.vec(), ans.template block<3, 3>(1, 1) = qq.w() * Eigen::Matrix::Identity() + skewSymmetric(qq.vec()); return ans; } template static Eigen::Matrix Qright(const Eigen::QuaternionBase &p) { Eigen::Quaternion pp = positify(p); Eigen::Matrix ans; ans(0, 0) = pp.w(), ans.template block<1, 3>(0, 1) = -pp.vec().transpose(); ans.template block<3, 1>(1, 0) = pp.vec(), ans.template block<3, 3>(1, 1) = pp.w() * Eigen::Matrix::Identity() - skewSymmetric(pp.vec()); return ans; } static Eigen::Vector3d R2ypr(const Eigen::Matrix3d &R) { Eigen::Vector3d n = R.col(0); Eigen::Vector3d o = R.col(1); Eigen::Vector3d a = R.col(2); Eigen::Vector3d ypr(3); double y = atan2(n(1), n(0)); double p = atan2(-n(2), n(0) * cos(y) + n(1) * sin(y)); double r = atan2(a(0) * sin(y) - a(1) * cos(y), -o(0) * sin(y) + o(1) * cos(y)); ypr(0) = y; ypr(1) = p; ypr(2) = r; return ypr / M_PI * 180.0; } template static Eigen::Matrix ypr2R(const Eigen::MatrixBase &ypr) { typedef typename Derived::Scalar Scalar_t; Scalar_t y = ypr(0) / 180.0 * M_PI; Scalar_t p = ypr(1) / 180.0 * M_PI; Scalar_t r = ypr(2) / 180.0 * M_PI; Eigen::Matrix Rz; Rz << cos(y), -sin(y), 0, sin(y), cos(y), 0, 0, 0, 1; Eigen::Matrix Ry; Ry << cos(p), 0., sin(p), 0., 1., 0., -sin(p), 0., cos(p); Eigen::Matrix Rx; Rx << 1., 0., 0., 0., cos(r), -sin(r), 0., sin(r), cos(r); return Rz * Ry * Rx; } static Eigen::Matrix3d g2R(const Eigen::Vector3d &g); template struct uint_ { }; template void unroller(const Lambda &f, const IterT &iter, uint_) { unroller(f, iter, uint_()); f(iter + N); } template void unroller(const Lambda &f, const IterT &iter, uint_<0>) { f(iter); } template static T normalizeAngle(const T& angle_degrees) { T two_pi(2.0 * 180); if (angle_degrees > 0) return angle_degrees - two_pi * std::floor((angle_degrees + T(180)) / two_pi); else return angle_degrees + two_pi * std::floor((-angle_degrees + T(180)) / two_pi); }; }; ================================================ FILE: vins_estimator/src/utility/visualization.cpp ================================================ #include "visualization.h" #include "parameters.h" using namespace ros; using namespace Eigen; ros::Publisher pub_odometry, pub_latest_odometry; ros::Publisher pub_path; ros::Publisher pub_relo_path; ros::Publisher pub_point_cloud, pub_margin_cloud; ros::Publisher pub_key_poses; ros::Publisher pub_relo_relative_pose; ros::Publisher pub_camera_pose; ros::Publisher pub_imu_pose; ros::Publisher pub_camera_pose_visual; nav_msgs::Path path, relo_path; ros::Publisher pub_keyframe_pose; ros::Publisher pub_keyframe_point; ros::Publisher pub_extrinsic; ros::Publisher pub_delta_v; ros::Publisher pub_delta_p; ros::Publisher pub_delta_t; ros::Publisher pub_match, pub_semantic; CameraPoseVisualization cameraposevisual(0, 1, 0, 1); CameraPoseVisualization keyframebasevisual(0.0, 0.0, 1.0, 1.0); static double sum_of_path = 0; static Vector3d last_path(0.0, 0.0, 0.0); vector prev_pcd_id; double pubOdometry_current_time = -1; void registerPub(ros::NodeHandle &n) { pub_latest_odometry = n.advertise("/vins_estimator/imu_propagate", 5); pub_path = n.advertise("/vins_estimator/path", 1); pub_odometry = n.advertise("/vins_estimator/odometry", 5); pub_key_poses = n.advertise("/vins_estimator/key_poses", 5); pub_camera_pose = n.advertise("/vins_estimator/camera_pose", 5); pub_camera_pose_visual = n.advertise("/vins_estimator/camera_pose_visual", 5); pub_extrinsic = n.advertise("/vins_estimator/extrinsic", 5); pub_relo_relative_pose = n.advertise("/vins_estimator/relo_relative_pose", 5); pub_match = n.advertise("/vins_estimator/feature_img", 1); pub_semantic = n.advertise("/vins_estimator/semantic_mask", 1); pub_relo_path = n.advertise("/vins_estimator/relocalization_path", 1); pub_delta_p = n.advertise("/vins_estimator/delta_p", 1); pub_delta_v = n.advertise("/vins_estimator/delta_v", 1); pub_delta_t = n.advertise("/vins_estimator/delta_t", 1); pub_keyframe_pose = n.advertise("/vins_estimator/keyframe_pose", 1); pub_keyframe_point = n.advertise("/vins_estimator/keyframe_point", 1); pub_point_cloud = n.advertise("/vins_estimator/point_cloud", 1); pub_margin_cloud = n.advertise("/vins_estimator/history_cloud", 1); pub_imu_pose = n.advertise("/vins_estimator/imu_pose", 5); cameraposevisual.setScale(1); cameraposevisual.setLineWidth(0.05); keyframebasevisual.setScale(0.1); keyframebasevisual.setLineWidth(0.01); } void pubLatestOdometry(const Eigen::Vector3d &P, const Eigen::Quaterniond &Q, const Eigen::Vector3d &V, const double &t) { nav_msgs::Odometry odometry; odometry.header.stamp = ros::Time(t); odometry.header.frame_id = "map"; odometry.pose.pose.position.x = P.x(); odometry.pose.pose.position.y = P.y(); odometry.pose.pose.position.z = P.z(); odometry.pose.pose.orientation.x = Q.x(); odometry.pose.pose.orientation.y = Q.y(); odometry.pose.pose.orientation.z = Q.z(); odometry.pose.pose.orientation.w = Q.w(); odometry.twist.twist.linear.x = V.x(); odometry.twist.twist.linear.y = V.y(); odometry.twist.twist.linear.z = V.z(); // Eigen::Vector3d a_center = A - omega.cross(V); // odometry.twist.twist.angular.x = a_center.x(); // odometry.twist.twist.angular.y = a_center.y(); // odometry.twist.twist.angular.z = a_center.z(); pub_latest_odometry.publish(odometry); geometry_msgs::PoseStamped pose; pose.header = odometry.header; pose.pose = odometry.pose.pose; pub_imu_pose.publish(pose); } void printStatistics(const Estimator &estimator, double t) { if (estimator.solver_flag != Estimator::SolverFlag::NON_LINEAR) return; printf("position: %f, %f, %f\r", estimator.Ps[WINDOW_SIZE].x(), estimator.Ps[WINDOW_SIZE].y(), estimator.Ps[WINDOW_SIZE].z()); if (SHOW_TRACK) { // ROS_DEBUG_STREAM("position: " << estimator.Ps[WINDOW_SIZE].transpose()); // ROS_DEBUG_STREAM("orientation: " << estimator.Vs[WINDOW_SIZE].transpose()); for (int i = 0; i < NUM_OF_CAM; i++) { // ROS_DEBUG("calibration result for camera %d", i); ROS_DEBUG_STREAM("extirnsic tic: " << estimator.tic[i].transpose()); ROS_DEBUG_STREAM("extrinsic ric: " << Utility::R2ypr(estimator.ric[i]).transpose()); if (ESTIMATE_EXTRINSIC) { cv::FileStorage fs(EX_CALIB_RESULT_PATH, cv::FileStorage::WRITE); Eigen::Matrix3d eigen_R; Eigen::Vector3d eigen_T; eigen_R = estimator.ric[i]; eigen_T = estimator.tic[i]; cv::Mat cv_R, cv_T; cv::eigen2cv(eigen_R, cv_R); cv::eigen2cv(eigen_T, cv_T); fs << "extrinsicRotation" << cv_R << "extrinsicTranslation" << cv_T; fs.release(); } } } ROS_DEBUG("vo solver costs: %f ms", t); sum_of_path += (estimator.Ps[WINDOW_SIZE] - last_path).norm(); last_path = estimator.Ps[WINDOW_SIZE]; ROS_DEBUG("sum of path %f", sum_of_path); if (ESTIMATE_TD) ROS_DEBUG("td %f", estimator.td); } void pubOdometry(const Estimator &estimator, const std_msgs::Header &header) { if (pubOdometry_current_time < 0) { pubOdometry_current_time = header.stamp.toSec(); } double dt = header.stamp.toSec() - pubOdometry_current_time; pubOdometry_current_time = header.stamp.toSec(); if (estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR) { nav_msgs::Odometry odometry; odometry.header = header; odometry.header.frame_id = "map"; odometry.child_frame_id = "map"; Quaterniond tmp_Q; tmp_Q = Quaterniond(estimator.Rs[WINDOW_SIZE]); odometry.pose.pose.position.x = estimator.Ps[WINDOW_SIZE].x(); odometry.pose.pose.position.y = estimator.Ps[WINDOW_SIZE].y(); odometry.pose.pose.position.z = estimator.Ps[WINDOW_SIZE].z(); odometry.pose.pose.orientation.x = tmp_Q.x(); odometry.pose.pose.orientation.y = tmp_Q.y(); odometry.pose.pose.orientation.z = tmp_Q.z(); odometry.pose.pose.orientation.w = tmp_Q.w(); odometry.twist.twist.linear.x = estimator.Vs[WINDOW_SIZE].x(); odometry.twist.twist.linear.y = estimator.Vs[WINDOW_SIZE].y(); odometry.twist.twist.linear.z = estimator.Vs[WINDOW_SIZE].z(); //"odometry" pub_odometry.publish(odometry); Vector3d delta_p = estimator.Ps[WINDOW_SIZE] - estimator.Ps[WINDOW_SIZE - 2]; Vector3d delta_v = estimator.Vs[WINDOW_SIZE] - estimator.Vs[WINDOW_SIZE - 2]; // ROS_ERROR("delta_p_v:%f,%f,%f|%f,%f,%f",delta_p.x(),delta_p.y(),delta_p.z(),delta_v.x(),delta_v.y(),delta_v.z()); // ROS_ERROR("norm :%f|%f",delta_p.norm(),delta_v.norm()); // ROS_ERROR("delta_t :%f",dt); std_msgs::Float64 rosp; std_msgs::Float64 rosv; std_msgs::Float64 rost; rosp.data = delta_p.norm(); rosv.data = delta_v.norm(); rost.data = dt; pub_delta_p.publish(rosp); pub_delta_v.publish(rosv); pub_delta_t.publish(rost); geometry_msgs::PoseStamped pose_stamped; pose_stamped.header = header; pose_stamped.header.frame_id = "map"; pose_stamped.pose = odometry.pose.pose; path.header = header; path.header.frame_id = "map"; path.poses.push_back(pose_stamped); //"path" pub_path.publish(path); Vector3d correct_t; Vector3d correct_v; Quaterniond correct_q; correct_t = estimator.drift_correct_r * estimator.Ps[WINDOW_SIZE] + estimator.drift_correct_t; correct_q = estimator.drift_correct_r * estimator.Rs[WINDOW_SIZE]; odometry.pose.pose.position.x = correct_t.x(); odometry.pose.pose.position.y = correct_t.y(); odometry.pose.pose.position.z = correct_t.z(); odometry.pose.pose.orientation.x = correct_q.x(); odometry.pose.pose.orientation.y = correct_q.y(); odometry.pose.pose.orientation.z = correct_q.z(); odometry.pose.pose.orientation.w = correct_q.w(); pose_stamped.pose = odometry.pose.pose; relo_path.header = header; relo_path.header.frame_id = "map"; relo_path.poses.push_back(pose_stamped); //"relocalization_path" pub_relo_path.publish(relo_path); // write result to file ofstream foutC(VINS_RESULT_PATH, ios::app); foutC.setf(ios::fixed, ios::floatfield); foutC.precision(0); foutC << header.stamp.toSec() * 1e9 << ","; foutC.precision(5); foutC << estimator.Ps[WINDOW_SIZE].x() << "," << estimator.Ps[WINDOW_SIZE].y() << "," << estimator.Ps[WINDOW_SIZE].z() << "," << tmp_Q.w() << "," << tmp_Q.x() << "," << tmp_Q.y() << "," << tmp_Q.z() << "," << estimator.Vs[WINDOW_SIZE].x() << "," << estimator.Vs[WINDOW_SIZE].y() << "," << estimator.Vs[WINDOW_SIZE].z() << "," << endl; foutC.close(); } } void pubKeyPoses(const Estimator &estimator, const std_msgs::Header &header) { if (estimator.key_poses.empty()) return; visualization_msgs::Marker key_poses; key_poses.header = header; key_poses.header.frame_id = "map"; key_poses.ns = "key_poses"; key_poses.type = visualization_msgs::Marker::SPHERE_LIST; key_poses.action = visualization_msgs::Marker::ADD; key_poses.pose.orientation.w = 1.0; key_poses.lifetime = ros::Duration(); // static int key_poses_id = 0; key_poses.id = 0; // key_poses_id++; key_poses.scale.x = 0.05; key_poses.scale.y = 0.05; key_poses.scale.z = 0.05; key_poses.color.r = 1.0; key_poses.color.a = 1.0; for (int i = 0; i <= WINDOW_SIZE; i++) { geometry_msgs::Point pose_marker; Vector3d correct_pose; correct_pose = estimator.key_poses[i]; pose_marker.x = correct_pose.x(); pose_marker.y = correct_pose.y(); pose_marker.z = correct_pose.z(); key_poses.points.push_back(pose_marker); } //"key_poses" pub_key_poses.publish(key_poses); } void pubCameraPose(const Estimator &estimator, const std_msgs::Header &header) { int idx2 = WINDOW_SIZE - 1; if (estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR) { int i = idx2; Vector3d P = estimator.Ps[i] + estimator.Rs[i] * estimator.tic[0]; Quaterniond R = Quaterniond(estimator.Rs[i] * estimator.ric[0]); geometry_msgs::PoseStamped pose_stamped; pose_stamped.header = header; pose_stamped.header.frame_id = "map"; pose_stamped.pose.position.x = P.x(); pose_stamped.pose.position.y = P.y(); pose_stamped.pose.position.z = P.z(); pose_stamped.pose.orientation.x = R.x(); pose_stamped.pose.orientation.y = R.y(); pose_stamped.pose.orientation.z = R.z(); pose_stamped.pose.orientation.w = R.w(); //"camera_pose" pub_camera_pose.publish(pose_stamped); cameraposevisual.reset(); cameraposevisual.add_pose(P, R); //"camera_pose_visual" cameraposevisual.publish_by(pub_camera_pose_visual, pose_stamped.header); // "sliding window pose visual" // cameraposevisual.reset(); // for (; idx2 >= 0; idx2--) // { // Vector3d P = estimator.Ps[idx2] + estimator.Rs[idx2] * estimator.tic[0]; // Quaterniond R = Quaterniond(estimator.Rs[idx2] * estimator.ric[0]); // cameraposevisual.add_pose(P, R); // } // cameraposevisual.publish_by(pub_camera_pose_visual, pose_stamped.header); } } void pubIMUPose(const Estimator &estimator, const std_msgs::Header &header) { if (estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR) { Vector3d P = estimator.Ps[WINDOW_SIZE]; Quaterniond R = Quaterniond(estimator.Rs[WINDOW_SIZE]); geometry_msgs::PoseStamped pose_stamped; pose_stamped.header = header; pose_stamped.header.frame_id = "map"; pose_stamped.pose.position.x = P.x(); pose_stamped.pose.position.y = P.y(); pose_stamped.pose.position.z = P.z(); pose_stamped.pose.orientation.x = R.x(); pose_stamped.pose.orientation.y = R.y(); pose_stamped.pose.orientation.z = R.z(); pose_stamped.pose.orientation.w = R.w(); pub_imu_pose.publish(pose_stamped); } } void pubPointCloud(const Estimator &estimator, const std_msgs::Header &header) { sensor_msgs::PointCloudPtr point_cloud_ptr(new sensor_msgs::PointCloud); point_cloud_ptr->header = header; for (auto &it_per_id : estimator.f_manager.feature) { if (it_per_id.is_dynamic) continue; int used_num; used_num = it_per_id.feature_per_frame.size(); // if (used_num < 4) // continue; if (!(used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2)) continue; if (it_per_id.start_frame > WINDOW_SIZE * 3.0 / 4.0 || it_per_id.solve_flag != 1) continue; int imu_i = it_per_id.start_frame; double depth = it_per_id.feature_per_frame[0].depth; // camera coordinate Vector3d pts_i = depth == 0 ? it_per_id.feature_per_frame[0].point * it_per_id.estimated_depth : it_per_id.feature_per_frame[0].point * depth; // C_imu = extrinsicRotation * C_cam + extrinsicTranslation // C_world = extrinsicRotation * C_imu + extrinsicTranslation Vector3d w_pts_i = estimator.Rs[imu_i] * (estimator.ric[0] * pts_i + estimator.tic[0]) + estimator.Ps[imu_i]; // ROS_ERROR("x: %f y: %f z: %f depth: %f", // pts_i(0),pts_i(1),pts_i(2),depth); geometry_msgs::Point32 p; p.x = w_pts_i(0); p.y = w_pts_i(1); p.z = w_pts_i(2); point_cloud_ptr->points.push_back(p); } //"point_cloud_ptr" pub_point_cloud.publish(point_cloud_ptr); // pub margined potin sensor_msgs::PointCloudPtr margin_cloud_ptr(new sensor_msgs::PointCloud); margin_cloud_ptr->header = header; for (auto &it_per_id : estimator.f_manager.feature) { if (it_per_id.is_dynamic) continue; int used_num; used_num = it_per_id.feature_per_frame.size(); // if (used_num < 4) // continue; if (!(used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2)) continue; // if (it_per_id->start_frame > WINDOW_SIZE * 3.0 / 4.0 || // it_per_id->solve_flag != 1) // continue; if (it_per_id.start_frame == 0 && it_per_id.feature_per_frame.size() <= 2 && it_per_id.solve_flag == 1) { int imu_i = it_per_id.start_frame; double depth = it_per_id.feature_per_frame[0].depth; Vector3d pts_i = depth == 0 ? it_per_id.feature_per_frame[0].point * it_per_id.estimated_depth : it_per_id.feature_per_frame[0].point * depth; // std::cout<<"pts_i(0):"<points.push_back(p); } } //"history_cloud" pub_margin_cloud.publish(margin_cloud_ptr); } void pubTF(const Estimator &estimator, const std_msgs::Header &header) { if (estimator.solver_flag != Estimator::SolverFlag::NON_LINEAR) return; static tf::TransformBroadcaster br; tf::Transform transform; tf::Quaternion q; // body frame Vector3d correct_t; Quaterniond correct_q; correct_t = estimator.Ps[WINDOW_SIZE]; correct_q = estimator.Rs[WINDOW_SIZE]; transform.setOrigin(tf::Vector3(correct_t(0), correct_t(1), correct_t(2))); q.setW(correct_q.w()); q.setX(correct_q.x()); q.setY(correct_q.y()); q.setZ(correct_q.z()); transform.setRotation(q); br.sendTransform(tf::StampedTransform(transform, header.stamp, "map", "body")); // camera frame transform.setOrigin( tf::Vector3(estimator.tic[0].x(), estimator.tic[0].y(), estimator.tic[0].z())); q.setW(Quaterniond(estimator.ric[0]).w()); q.setX(Quaterniond(estimator.ric[0]).x()); q.setY(Quaterniond(estimator.ric[0]).y()); q.setZ(Quaterniond(estimator.ric[0]).z()); transform.setRotation(q); br.sendTransform(tf::StampedTransform(transform, header.stamp, "body", "camera")); nav_msgs::Odometry odometry; odometry.header = header; odometry.header.frame_id = "map"; odometry.pose.pose.position.x = estimator.tic[0].x(); odometry.pose.pose.position.y = estimator.tic[0].y(); odometry.pose.pose.position.z = estimator.tic[0].z(); Quaterniond tmp_q{estimator.ric[0]}; odometry.pose.pose.orientation.x = tmp_q.x(); odometry.pose.pose.orientation.y = tmp_q.y(); odometry.pose.pose.orientation.z = tmp_q.z(); odometry.pose.pose.orientation.w = tmp_q.w(); pub_extrinsic.publish(odometry); } void pubKeyframe(const Estimator &estimator) { // pub camera pose, 2D-3D points of keyframe if (estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR && estimator.marginalization_flag == 0) { int i = WINDOW_SIZE - 2; // Vector3d P = estimator.Ps[i] + estimator.Rs[i] * estimator.tic[0]; Vector3d P = estimator.Ps[i]; Quaterniond R = Quaterniond(estimator.Rs[i]); nav_msgs::Odometry odometry; odometry.header.stamp = ros::Time(estimator.Headers[WINDOW_SIZE - 2]); odometry.header.frame_id = "map"; odometry.pose.pose.position.x = P.x(); odometry.pose.pose.position.y = P.y(); odometry.pose.pose.position.z = P.z(); odometry.pose.pose.orientation.x = R.x(); odometry.pose.pose.orientation.y = R.y(); odometry.pose.pose.orientation.z = R.z(); odometry.pose.pose.orientation.w = R.w(); // printf("time: %f t: %f %f %f r: %f %f %f %f\n", // odometry.header.stamp.toSec(), P.x(), P.y(), P.z(), R.w(), R.x(), R.y(), // R.z()); "keyframe_pose" pub_keyframe_pose.publish(odometry); sensor_msgs::PointCloudPtr point_cloud_ptr(new sensor_msgs::PointCloud); point_cloud_ptr->header.stamp = ros::Time(estimator.Headers[WINDOW_SIZE - 2]); for (auto &it_per_id : estimator.f_manager.feature) { int frame_size = it_per_id.feature_per_frame.size(); if (it_per_id.start_frame < WINDOW_SIZE - 2 && it_per_id.start_frame + frame_size - 1 >= WINDOW_SIZE - 2 && it_per_id.solve_flag == 1) { int imu_i = it_per_id.start_frame; double depth = it_per_id.feature_per_frame[0].depth; depth = depth == 0 ? it_per_id.estimated_depth : depth; // a limit on distance, avoid large drifts // if (depth < DEPTH_MAX_DIST) { Vector3d pts_i = it_per_id.feature_per_frame[0].point * depth; Vector3d w_pts_i = estimator.Rs[imu_i] * (estimator.ric[0] * pts_i + estimator.tic[0]) + estimator.Ps[imu_i]; geometry_msgs::Point32 p; p.x = w_pts_i(0); p.y = w_pts_i(1); p.z = w_pts_i(2); point_cloud_ptr->points.push_back(p); int imu_j = WINDOW_SIZE - 2 - it_per_id.start_frame; sensor_msgs::ChannelFloat32 p_2d; p_2d.values.push_back(it_per_id.feature_per_frame[imu_j].point.x()); p_2d.values.push_back(it_per_id.feature_per_frame[imu_j].point.y()); p_2d.values.push_back(it_per_id.feature_per_frame[imu_j].uv.x()); p_2d.values.push_back(it_per_id.feature_per_frame[imu_j].uv.y()); p_2d.values.push_back(it_per_id.feature_id); point_cloud_ptr->channels.push_back(p_2d); } } } //"keyframe_point" // now have more points than origin pub_keyframe_point.publish(point_cloud_ptr); } } void pubRelocalization(const Estimator &estimator) { nav_msgs::Odometry odometry; odometry.header.stamp = ros::Time(estimator.relo_frame_stamp); odometry.header.frame_id = "map"; odometry.pose.pose.position.x = estimator.relo_relative_t.x(); odometry.pose.pose.position.y = estimator.relo_relative_t.y(); odometry.pose.pose.position.z = estimator.relo_relative_t.z(); odometry.pose.pose.orientation.x = estimator.relo_relative_q.x(); odometry.pose.pose.orientation.y = estimator.relo_relative_q.y(); odometry.pose.pose.orientation.z = estimator.relo_relative_q.z(); odometry.pose.pose.orientation.w = estimator.relo_relative_q.w(); odometry.twist.twist.linear.x = estimator.relo_relative_yaw; odometry.twist.twist.linear.y = estimator.relo_frame_index; //"relo_relative_pose" pub_relo_relative_pose.publish(odometry); } void pubTrackImg(const cv_bridge::CvImageConstPtr &img_ptr) { sensor_msgs::CompressedImage image_up; image_up.format = "jpeg"; std::vector encodeing; cv::imencode(".jpg", img_ptr->image, encodeing); encodeing.swap(image_up.data); image_up.header = std_msgs::Header(); pub_match.publish(image_up); } void pubTrackImg(const sensor_msgs::ImagePtr &img_msg) { sensor_msgs::CompressedImage image_up; image_up.format = "jpeg"; std::vector encodeing; cv::imencode(".jpg", img_msg->data, encodeing); encodeing.swap(image_up.data); image_up.header = std_msgs::Header(); pub_match.publish(image_up); } void pubTrackImg(const cv::Mat &img) { // sensor_msgs::CompressedImage image_up; // image_up.format = "jpeg"; // std::vector encodeing; // cv::imencode(".jpg", img, encodeing); // encodeing.swap(image_up.data); // image_up.header = std_msgs::Header(); // // pub_match.publish(image_up); if (!img.empty()) pub_match.publish(cv_bridge::CvImage(std_msgs::Header(), "bgr8", img).toImageMsg()); } ================================================ FILE: vins_estimator/src/utility/visualization.h ================================================ #pragma once #include "../estimator/estimator.h" #include "CameraPoseVisualization.h" #include "parameters.h" #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include extern ros::Publisher pub_odometry; extern ros::Publisher pub_path, pub_pose; extern ros::Publisher pub_cloud, pub_map; extern ros::Publisher pub_key_poses; extern ros::Publisher pub_ref_pose, pub_cur_pose; extern ros::Publisher pub_key; extern nav_msgs::Path path; extern ros::Publisher pub_pose_graph; extern int IMAGE_ROW, IMAGE_COL; void registerPub(ros::NodeHandle &n); void pubLatestOdometry(const Eigen::Vector3d &P, const Eigen::Quaterniond &Q, const Eigen::Vector3d &V, const double &t); // void pubLatestOdometry(const Eigen::Vector3d &P, const Eigen::Quaterniond &Q, // const Eigen::Vector3d &V, // // const Eigen::Vector3d &A, // // const Eigen::Vector3d &omega, // const std_msgs::Header &header); void printStatistics(const Estimator &estimator, double t); void pubOdometry(const Estimator &estimator, const std_msgs::Header &header); void pubInitialGuess(const Estimator &estimator, const std_msgs::Header &header); void pubKeyPoses(const Estimator &estimator, const std_msgs::Header &header); void pubCameraPose(const Estimator &estimator, const std_msgs::Header &header); void pubIMUPose(const Estimator &estimator, const std_msgs::Header &header); void pubPointCloud(const Estimator &estimator, const std_msgs::Header &header); void pubTF(const Estimator &estimator, const std_msgs::Header &header); void pubKeyframe(const Estimator &estimator); void pubRelocalization(const Estimator &estimator); void pubTrackImg(const cv_bridge::CvImageConstPtr &img_ptr); void pubTrackImg(const sensor_msgs::ImagePtr &img_msg); void pubTrackImg(const cv::Mat &img);