[ { "path": ".gitignore", "content": "**/build\n**/devel\n**/.idea\n**/.vscode\n**/cmake-build-debug\n**/.cache\n\n!.gitignore\n" }, { "path": "README.md", "content": "# VINS-RGBD-FAST\n\n**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/).\n\n## Refinements\n* grid-based feature detection\n* extract FAST feature instead of Harris feature\n* added stationary initialization\n* added IMU-aided feature tracking\n* added extracted-feature area's quality judgement\n* solved feature clusttering problem result frome FAST feature\n* use \"sensor_msg::CompressedImage\" as image topic type\n \n## Related Paper:\n```\n@ARTICLE{9830851, \n author={Liu, Jianheng and Li, Xuanfu and Liu, Yueqian and Chen, Haoyao}, \n journal={IEEE Robotics and Automation Letters}, \n title={RGB-D Inertial Odometry for a Resource-Restricted Robot in Dynamic Environments}, \n year={2022}, \n volume={7}, \n number={4}, \n pages={9573-9580}, \n doi={10.1109/LRA.2022.3191193}}\n```\n\n## RGBD-Inertial Trajectory Estimation and Mapping for Small Ground Rescue Robot\n\nBased one open source SLAM framework [VINS-Mono](https://github.com/HKUST-Aerial-Robotics/VINS-Mono).\n\nThe approach contains\n+ Depth-integrated visual-inertial initialization process.\n+ Visual-inertial odometry by utilizing depth information while avoiding the limitation is working for 3D pose estimation.\n+ Noise elimination map which is suitable for path planning and navigation.\n\nHowever, the proposed approach can also be applied to other application like handheld and wheeled robot.\n\n## 1. Prerequisites\n1.1. **Ubuntu** 16.04 or 18.04.\n\n1.2. **ROS** version Kinetic or Melodic fully installation\n\n1.3. **Ceres Solver**\nFollow [Ceres Installation](http://ceres-solver.org/installation.html)\n\n1.4. **Sophus**\n```\n git clone http://github.com/strasdat/Sophus.git\n git checkout a621ff\n```\n\n1.3. **Atlas 200 DK环境配置**\n\n[https://blog.csdn.net/qq_42703283/article/details/110389270](https://blog.csdn.net/qq_42703283/article/details/110389270)\n\n1.4. **ROS多机通信**\n\n[https://blog.csdn.net/qq_42703283/article/details/110408848](\n\n## 2. Datasets\n\nRecording by RealSense D435i. Contain 9 bags in three different applicaions:\n+ [Handheld](https://star-center.shanghaitech.edu.cn/seafile/d/0ea45d1878914077ade5/)\n+ [Wheeled robot](https://star-center.shanghaitech.edu.cn/seafile/d/78c0375114854774b521/) ([Jackal](https://www.clearpathrobotics.com/jackal-small-unmanned-ground-vehicle/))\n+ [Tracked robot](https://star-center.shanghaitech.edu.cn/seafile/d/f611fc44df0c4b3d936d/)\n\nNote the rosbags are in compressed format. Use \"rosbag decompress\" to decompress.\n\nTopics:\n+ depth topic: /camera/aligned_depth_to_color/image_raw\n+ color topic: /camera/color/image_raw\n+ imu topic: /camera/imu\n\n我们使用的是压缩图像节点：\n\n+ depth topic: /camera/aligned_depth_to_color/image_raw\n+ color topic: /camera/color/image_raw/compressed\n+ imu topic: /camera/imu\n\n如何录制一个数据包\n\n1. 运行d435i\n2. `rosbag record /camera/imu /camera/color/image_raw /camera/aligned_depth_to_color/image_raw /camera/color/camera_info /camera/color/image_raw/compressed`\n\n\n## 3. Run with Docker\n\nmake Dockerfile like below\n```c\nFROM ros:melodic-ros-core-bionic\n\n# apt-get update\nRUN apt-get update\n\n# install essentials\nRUN apt install -y gcc\nRUN apt install -y g++\nRUN apt-get install -y cmake\nRUN apt-get install -y wget\nRUN apt install -y git\n\n# install ceres\nWORKDIR /home\nRUN apt-get install -y libgoogle-glog-dev libgflags-dev\nRUN apt-get install -y libatlas-base-dev\nRUN apt-get install -y libeigen3-dev\nRUN apt-get install -y libsuitesparse-dev\nRUN wget http://ceres-solver.org/ceres-solver-2.1.0.tar.gz\nRUN tar zxf ceres-solver-2.1.0.tar.gz\nWORKDIR /home/ceres-solver-2.1.0\nRUN mkdir build\nWORKDIR /home/ceres-solver-2.1.0/build\nRUN cmake ..\nRUN make\nRUN make install\n\n# install sophus\nWORKDIR /home\nRUN git clone https://github.com/demul/Sophus.git\nWORKDIR /home/Sophus\nRUN git checkout fix/unit_complex_eror\nRUN mkdir build\nWORKDIR /home/Sophus/build\nRUN cmake ..\nRUN make\nRUN make install\n\n# install ros dependencies\nWORKDIR /home\nRUN mkdir ros_ws\nWORKDIR /home/ros_ws\nRUN apt-get -y install ros-melodic-cv-bridge\nRUN apt-get -y install ros-melodic-nodelet\nRUN apt-get -y install ros-melodic-tf\nRUN apt-get -y install ros-melodic-image-transport\nRUN apt-get -y install ros-melodic-rviz\n\n# build vins-rgbd-fast\nRUN mkdir src\nWORKDIR /home/ros_ws/src\nRUN git clone https://github.com/jianhengLiu/VINS-RGBD-FAST\nWORKDIR /home/ros_ws\nRUN /bin/bash -c \". /opt/ros/melodic/setup.bash; cd /home/ros_ws; catkin_make\"\nRUN echo \"source /home/ros_ws/devel/setup.bash\" >> ~/.bashrc\n```\ndocker build .\n\n## 4. Licence\nThe source code is released under [GPLv3](http://www.gnu.org/licenses/) license.\n\n" }, { "path": "camera_model/CMakeLists.txt", "content": "cmake_minimum_required(VERSION 2.8.3)\nproject(camera_model)\n\nset(CMAKE_BUILD_TYPE \"Release\")\n# https://blog.csdn.net/qq_24624539/article/details/111056791\n#set(CMAKE_CXX_FLAGS \"-std=c++11\")\nset(CMAKE_CXX_FLAGS \"-std=c++14\")\nset(CMAKE_CXX_FLAGS_RELEASE \"-O3 -fPIC\")\n\nfind_package(catkin REQUIRED COMPONENTS\n roscpp\n std_msgs\n )\n\nfind_package(Boost REQUIRED COMPONENTS filesystem program_options system)\ninclude_directories(${Boost_INCLUDE_DIRS})\n\nfind_package(OpenCV REQUIRED)\n\n# set(EIGEN_INCLUDE_DIR \"/usr/local/include/eigen3\")\nfind_package(Ceres REQUIRED)\ninclude_directories(${CERES_INCLUDE_DIRS})\n\n\ncatkin_package(\n INCLUDE_DIRS include\n LIBRARIES camera_model\n CATKIN_DEPENDS roscpp std_msgs\n# DEPENDS system_lib\n )\n\ninclude_directories(\n ${catkin_INCLUDE_DIRS}\n )\n\ninclude_directories(\"include\")\n\n\nadd_executable(Calibration \n src/intrinsic_calib.cc\n src/chessboard/Chessboard.cc\n src/calib/CameraCalibration.cc\n src/camera_models/Camera.cc\n src/camera_models/CameraFactory.cc\n src/camera_models/CostFunctionFactory.cc\n src/camera_models/PinholeCamera.cc\n src/camera_models/CataCamera.cc\n src/camera_models/EquidistantCamera.cc\n src/camera_models/ScaramuzzaCamera.cc\n src/sparse_graph/Transform.cc\n src/gpl/gpl.cc\n src/gpl/EigenQuaternionParameterization.cc)\n\nadd_library(camera_model\n src/chessboard/Chessboard.cc\n src/calib/CameraCalibration.cc\n src/camera_models/Camera.cc\n src/camera_models/CameraFactory.cc\n src/camera_models/CostFunctionFactory.cc\n src/camera_models/PinholeCamera.cc\n src/camera_models/CataCamera.cc\n src/camera_models/EquidistantCamera.cc\n src/camera_models/ScaramuzzaCamera.cc\n src/sparse_graph/Transform.cc\n src/gpl/gpl.cc\n src/gpl/EigenQuaternionParameterization.cc)\n\ntarget_link_libraries(Calibration ${Boost_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES})\ntarget_link_libraries(camera_model ${Boost_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES})\n" }, { "path": "camera_model/cmake/FindEigen.cmake", "content": "# Ceres Solver - A fast non-linear least squares minimizer\n# Copyright 2015 Google Inc. All rights reserved.\n# http://ceres-solver.org/\n#\n# Redistribution and use in source and binary forms, with or without\n# modification, are permitted provided that the following conditions are met:\n#\n# * Redistributions of source code must retain the above copyright notice,\n# this list of conditions and the following disclaimer.\n# * Redistributions in binary form must reproduce the above copyright notice,\n# this list of conditions and the following disclaimer in the documentation\n# and/or other materials provided with the distribution.\n# * Neither the name of Google Inc. nor the names of its contributors may be\n# used to endorse or promote products derived from this software without\n# specific prior written permission.\n#\n# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS IS\"\n# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE\n# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE\n# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE\n# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR\n# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF\n# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS\n# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN\n# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)\n# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\n# POSSIBILITY OF SUCH DAMAGE.\n#\n# Author: alexs.mac@gmail.com (Alex Stewart)\n#\n\n# FindEigen.cmake - Find Eigen library, version >= 3.\n#\n# This module defines the following variables:\n#\n# EIGEN_FOUND: TRUE iff Eigen is found.\n# EIGEN_INCLUDE_DIRS: Include directories for Eigen.\n#\n# EIGEN_VERSION: Extracted from Eigen/src/Core/util/Macros.h\n# EIGEN_WORLD_VERSION: Equal to 3 if EIGEN_VERSION = 3.2.0\n# EIGEN_MAJOR_VERSION: Equal to 2 if EIGEN_VERSION = 3.2.0\n# EIGEN_MINOR_VERSION: Equal to 0 if EIGEN_VERSION = 3.2.0\n#\n# The following variables control the behaviour of this module:\n#\n# EIGEN_INCLUDE_DIR_HINTS: List of additional directories in which to\n# search for eigen includes, e.g: /timbuktu/eigen3.\n#\n# The following variables are also defined by this module, but in line with\n# CMake recommended FindPackage() module style should NOT be referenced directly\n# by callers (use the plural variables detailed above instead). These variables\n# do however affect the behaviour of the module via FIND_[PATH/LIBRARY]() which\n# are NOT re-called (i.e. search for library is not repeated) if these variables\n# are set with valid values _in the CMake cache_. This means that if these\n# variables are set directly in the cache, either by the user in the CMake GUI,\n# or by the user passing -DVAR=VALUE directives to CMake when called (which\n# explicitly defines a cache variable), then they will be used verbatim,\n# bypassing the HINTS variables and other hard-coded search locations.\n#\n# EIGEN_INCLUDE_DIR: Include directory for CXSparse, not including the\n# include directory of any dependencies.\n\n# Called if we failed to find Eigen or any of it's required dependencies,\n# unsets all public (designed to be used externally) variables and reports\n# error message at priority depending upon [REQUIRED/QUIET/] argument.\nmacro(EIGEN_REPORT_NOT_FOUND REASON_MSG)\n unset(EIGEN_FOUND)\n unset(EIGEN_INCLUDE_DIRS)\n # Make results of search visible in the CMake GUI if Eigen has not\n # been found so that user does not have to toggle to advanced view.\n mark_as_advanced(CLEAR EIGEN_INCLUDE_DIR)\n # Note _FIND_[REQUIRED/QUIETLY] variables defined by FindPackage()\n # use the camelcase library name, not uppercase.\n if (Eigen_FIND_QUIETLY)\n message(STATUS \"Failed to find Eigen - \" ${REASON_MSG} ${ARGN})\n elseif (Eigen_FIND_REQUIRED)\n message(FATAL_ERROR \"Failed to find Eigen - \" ${REASON_MSG} ${ARGN})\n else()\n # Neither QUIETLY nor REQUIRED, use no priority which emits a message\n # but continues configuration and allows generation.\n message(\"-- Failed to find Eigen - \" ${REASON_MSG} ${ARGN})\n endif ()\n return()\nendmacro(EIGEN_REPORT_NOT_FOUND)\n\n# Protect against any alternative find_package scripts for this library having\n# been called previously (in a client project) which set EIGEN_FOUND, but not\n# the other variables we require / set here which could cause the search logic\n# here to fail.\nunset(EIGEN_FOUND)\n\n# Search user-installed locations first, so that we prefer user installs\n# to system installs where both exist.\nlist(APPEND EIGEN_CHECK_INCLUDE_DIRS\n /usr/local/include\n /usr/local/homebrew/include # Mac OS X\n /opt/local/var/macports/software # Mac OS X.\n /opt/local/include\n /usr/include)\n# Additional suffixes to try appending to each search path.\nlist(APPEND EIGEN_CHECK_PATH_SUFFIXES\n eigen3 # Default root directory for Eigen.\n Eigen/include/eigen3 # Windows (for C:/Program Files prefix) < 3.3\n Eigen3/include/eigen3 ) # Windows (for C:/Program Files prefix) >= 3.3\n\n# Search supplied hint directories first if supplied.\nfind_path(EIGEN_INCLUDE_DIR\n NAMES Eigen/Core\n PATHS ${EIGEN_INCLUDE_DIR_HINTS}\n ${EIGEN_CHECK_INCLUDE_DIRS}\n PATH_SUFFIXES ${EIGEN_CHECK_PATH_SUFFIXES})\n\nif (NOT EIGEN_INCLUDE_DIR OR\n NOT EXISTS ${EIGEN_INCLUDE_DIR})\n eigen_report_not_found(\n \"Could not find eigen3 include directory, set EIGEN_INCLUDE_DIR to \"\n \"path to eigen3 include directory, e.g. /usr/local/include/eigen3.\")\nendif (NOT EIGEN_INCLUDE_DIR OR\n NOT EXISTS ${EIGEN_INCLUDE_DIR})\n\n# Mark internally as found, then verify. EIGEN_REPORT_NOT_FOUND() unsets\n# if called.\nset(EIGEN_FOUND TRUE)\n\n# Extract Eigen version from Eigen/src/Core/util/Macros.h\nif (EIGEN_INCLUDE_DIR)\n set(EIGEN_VERSION_FILE ${EIGEN_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h)\n if (NOT EXISTS ${EIGEN_VERSION_FILE})\n eigen_report_not_found(\n \"Could not find file: ${EIGEN_VERSION_FILE} \"\n \"containing version information in Eigen install located at: \"\n \"${EIGEN_INCLUDE_DIR}.\")\n else (NOT EXISTS ${EIGEN_VERSION_FILE})\n file(READ ${EIGEN_VERSION_FILE} EIGEN_VERSION_FILE_CONTENTS)\n\n string(REGEX MATCH \"#define EIGEN_WORLD_VERSION [0-9]+\"\n EIGEN_WORLD_VERSION \"${EIGEN_VERSION_FILE_CONTENTS}\")\n string(REGEX REPLACE \"#define EIGEN_WORLD_VERSION ([0-9]+)\" \"\\\\1\"\n EIGEN_WORLD_VERSION \"${EIGEN_WORLD_VERSION}\")\n\n string(REGEX MATCH \"#define EIGEN_MAJOR_VERSION [0-9]+\"\n EIGEN_MAJOR_VERSION \"${EIGEN_VERSION_FILE_CONTENTS}\")\n string(REGEX REPLACE \"#define EIGEN_MAJOR_VERSION ([0-9]+)\" \"\\\\1\"\n EIGEN_MAJOR_VERSION \"${EIGEN_MAJOR_VERSION}\")\n\n string(REGEX MATCH \"#define EIGEN_MINOR_VERSION [0-9]+\"\n EIGEN_MINOR_VERSION \"${EIGEN_VERSION_FILE_CONTENTS}\")\n string(REGEX REPLACE \"#define EIGEN_MINOR_VERSION ([0-9]+)\" \"\\\\1\"\n EIGEN_MINOR_VERSION \"${EIGEN_MINOR_VERSION}\")\n\n # This is on a single line s/t CMake does not interpret it as a list of\n # elements and insert ';' separators which would result in 3.;2.;0 nonsense.\n set(EIGEN_VERSION \"${EIGEN_WORLD_VERSION}.${EIGEN_MAJOR_VERSION}.${EIGEN_MINOR_VERSION}\")\n endif (NOT EXISTS ${EIGEN_VERSION_FILE})\nendif (EIGEN_INCLUDE_DIR)\n\n# Set standard CMake FindPackage variables if found.\nif (EIGEN_FOUND)\n set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR})\nendif (EIGEN_FOUND)\n\n# Handle REQUIRED / QUIET optional arguments and version.\ninclude(FindPackageHandleStandardArgs)\nfind_package_handle_standard_args(Eigen\n REQUIRED_VARS EIGEN_INCLUDE_DIRS\n VERSION_VAR EIGEN_VERSION)\n\n# Only mark internal variables as advanced if we found Eigen, otherwise\n# leave it visible in the standard GUI for the user to set manually.\nif (EIGEN_FOUND)\n mark_as_advanced(FORCE EIGEN_INCLUDE_DIR)\nendif (EIGEN_FOUND)\n" }, { "path": "camera_model/include/camodocal/calib/CameraCalibration.h", "content": "#ifndef CAMERACALIBRATION_H\n#define CAMERACALIBRATION_H\n\n#include \n\n#include \"camodocal/camera_models/Camera.h\"\n\nnamespace camodocal\n{\n\nclass CameraCalibration\n{\npublic:\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n CameraCalibration();\n\n CameraCalibration(Camera::ModelType modelType,\n const std::string& cameraName,\n const cv::Size& imageSize,\n const cv::Size& boardSize,\n float squareSize);\n\n void clear(void);\n\n void addChessboardData(const std::vector& corners);\n\n bool calibrate(void);\n\n int sampleCount(void) const;\n std::vector >& imagePoints(void);\n const std::vector >& imagePoints(void) const;\n std::vector >& scenePoints(void);\n const std::vector >& scenePoints(void) const;\n CameraPtr& camera(void);\n const CameraConstPtr camera(void) const;\n\n Eigen::Matrix2d& measurementCovariance(void);\n const Eigen::Matrix2d& measurementCovariance(void) const;\n\n cv::Mat& cameraPoses(void);\n const cv::Mat& cameraPoses(void) const;\n\n void drawResults(std::vector& images) const;\n\n void writeParams(const std::string& filename) const;\n\n bool writeChessboardData(const std::string& filename) const;\n bool readChessboardData(const std::string& filename);\n\n void setVerbose(bool verbose);\n\nprivate:\n bool calibrateHelper(CameraPtr& camera,\n std::vector& rvecs, std::vector& tvecs) const;\n\n void optimize(CameraPtr& camera,\n std::vector& rvecs, std::vector& tvecs) const;\n\n template\n void readData(std::ifstream& ifs, T& data) const;\n\n template\n void writeData(std::ofstream& ofs, T data) const;\n\n cv::Size m_boardSize;\n float m_squareSize;\n\n CameraPtr m_camera;\n cv::Mat m_cameraPoses;\n\n std::vector > m_imagePoints;\n std::vector > m_scenePoints;\n\n Eigen::Matrix2d m_measurementCovariance;\n\n bool m_verbose;\n};\n\n}\n\n#endif\n" }, { "path": "camera_model/include/camodocal/camera_models/Camera.h", "content": "#ifndef CAMERA_H\n#define CAMERA_H\n\n#include \n#include \n#include \n#include \n\nnamespace camodocal\n{\n\nclass Camera\n{\npublic:\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n enum ModelType\n {\n KANNALA_BRANDT,\n MEI,\n PINHOLE,\n SCARAMUZZA\n };\n\n class Parameters\n {\n public:\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n Parameters(ModelType modelType);\n\n Parameters(ModelType modelType, const std::string& cameraName,\n int w, int h);\n\n ModelType& modelType(void);\n std::string& cameraName(void);\n int& imageWidth(void);\n int& imageHeight(void);\n\n ModelType modelType(void) const;\n const std::string& cameraName(void) const;\n int imageWidth(void) const;\n int imageHeight(void) const;\n\n int nIntrinsics(void) const;\n\n virtual bool readFromYamlFile(const std::string& filename) = 0;\n virtual void writeToYamlFile(const std::string& filename) const = 0;\n\n protected:\n ModelType m_modelType;\n int m_nIntrinsics;\n std::string m_cameraName;\n int m_imageWidth;\n int m_imageHeight;\n };\n\n virtual ModelType modelType(void) const = 0;\n virtual const std::string& cameraName(void) const = 0;\n virtual int imageWidth(void) const = 0;\n virtual int imageHeight(void) const = 0;\n\n virtual cv::Mat& mask(void);\n virtual const cv::Mat& mask(void) const;\n\n virtual void estimateIntrinsics(const cv::Size& boardSize,\n const std::vector< std::vector >& objectPoints,\n const std::vector< std::vector >& imagePoints) = 0;\n virtual void estimateExtrinsics(const std::vector& objectPoints,\n const std::vector& imagePoints,\n cv::Mat& rvec, cv::Mat& tvec) const;\n\n // Lift points from the image plane to the sphere\n virtual void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const = 0;\n //%output P\n\n // Lift points from the image plane to the projective space\n virtual void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const = 0;\n //%output P\n\n // Projects 3D points to the image plane (Pi function)\n virtual void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const = 0;\n //%output p\n\n // Projects 3D points to the image plane (Pi function)\n // and calculates jacobian\n //virtual void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p,\n // Eigen::Matrix& J) const = 0;\n //%output p\n //%output J\n\n virtual void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const = 0;\n //%output p\n\n //virtual void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0) const = 0;\n virtual cv::Mat initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2,\n float fx = -1.0f, float fy = -1.0f,\n cv::Size imageSize = cv::Size(0, 0),\n float cx = -1.0f, float cy = -1.0f,\n cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const = 0;\n\n virtual int parameterCount(void) const = 0;\n\n virtual void readParameters(const std::vector& parameters) = 0;\n virtual void writeParameters(std::vector& parameters) const = 0;\n\n virtual void writeParametersToYamlFile(const std::string& filename) const = 0;\n\n virtual std::string parametersToString(void) const = 0;\n\n /**\n * \\brief Calculates the reprojection distance between points\n *\n * \\param P1 first 3D point coordinates\n * \\param P2 second 3D point coordinates\n * \\return euclidean distance in the plane\n */\n double reprojectionDist(const Eigen::Vector3d& P1, const Eigen::Vector3d& P2) const;\n\n double reprojectionError(const std::vector< std::vector >& objectPoints,\n const std::vector< std::vector >& imagePoints,\n const std::vector& rvecs,\n const std::vector& tvecs,\n cv::OutputArray perViewErrors = cv::noArray()) const;\n\n double reprojectionError(const Eigen::Vector3d& P,\n const Eigen::Quaterniond& camera_q,\n const Eigen::Vector3d& camera_t,\n const Eigen::Vector2d& observed_p) const;\n\n void projectPoints(const std::vector& objectPoints,\n const cv::Mat& rvec,\n const cv::Mat& tvec,\n std::vector& imagePoints) const;\nprotected:\n cv::Mat m_mask;\n};\n\ntypedef boost::shared_ptr CameraPtr;\ntypedef boost::shared_ptr CameraConstPtr;\n\n}\n\n#endif\n" }, { "path": "camera_model/include/camodocal/camera_models/CameraFactory.h", "content": "#ifndef CAMERAFACTORY_H\n#define CAMERAFACTORY_H\n\n#include \n#include \n\n#include \"camodocal/camera_models/Camera.h\"\n\nnamespace camodocal\n{\n\nclass CameraFactory\n{\npublic:\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n CameraFactory();\n\n static boost::shared_ptr instance(void);\n\n CameraPtr generateCamera(Camera::ModelType modelType,\n const std::string& cameraName,\n cv::Size imageSize) const;\n\n CameraPtr generateCameraFromYamlFile(const std::string& filename);\n\nprivate:\n static boost::shared_ptr m_instance;\n};\n\n}\n\n#endif\n" }, { "path": "camera_model/include/camodocal/camera_models/CataCamera.h", "content": "#ifndef CATACAMERA_H\r\n#define CATACAMERA_H\r\n\r\n#include \r\n#include \r\n\r\n#include \"ceres/rotation.h\"\r\n#include \"Camera.h\"\r\n\r\nnamespace camodocal\r\n{\r\n\r\n/**\r\n * C. Mei, and P. Rives, Single View Point Omnidirectional Camera Calibration\r\n * from Planar Grids, ICRA 2007\r\n */\r\n\r\nclass CataCamera: public Camera\r\n{\r\npublic:\r\n class Parameters: public Camera::Parameters\r\n {\r\n public:\r\n Parameters();\r\n Parameters(const std::string& cameraName,\r\n int w, int h,\r\n double xi,\r\n double k1, double k2, double p1, double p2,\r\n double gamma1, double gamma2, double u0, double v0);\r\n\r\n double& xi(void);\r\n double& k1(void);\r\n double& k2(void);\r\n double& p1(void);\r\n double& p2(void);\r\n double& gamma1(void);\r\n double& gamma2(void);\r\n double& u0(void);\r\n double& v0(void);\r\n\r\n double xi(void) const;\r\n double k1(void) const;\r\n double k2(void) const;\r\n double p1(void) const;\r\n double p2(void) const;\r\n double gamma1(void) const;\r\n double gamma2(void) const;\r\n double u0(void) const;\r\n double v0(void) const;\r\n\r\n bool readFromYamlFile(const std::string& filename);\r\n void writeToYamlFile(const std::string& filename) const;\r\n\r\n Parameters& operator=(const Parameters& other);\r\n friend std::ostream& operator<< (std::ostream& out, const Parameters& params);\r\n\r\n private:\r\n double m_xi;\r\n double m_k1;\r\n double m_k2;\r\n double m_p1;\r\n double m_p2;\r\n double m_gamma1;\r\n double m_gamma2;\r\n double m_u0;\r\n double m_v0;\r\n };\r\n\r\n CataCamera();\r\n\r\n /**\r\n * \\brief Constructor from the projection model parameters\r\n */\r\n CataCamera(const std::string& cameraName,\r\n int imageWidth, int imageHeight,\r\n double xi, double k1, double k2, double p1, double p2,\r\n double gamma1, double gamma2, double u0, double v0);\r\n /**\r\n * \\brief Constructor from the projection model parameters\r\n */\r\n CataCamera(const Parameters& params);\r\n\r\n Camera::ModelType modelType(void) const;\r\n const std::string& cameraName(void) const;\r\n int imageWidth(void) const;\r\n int imageHeight(void) const;\r\n\r\n void estimateIntrinsics(const cv::Size& boardSize,\r\n const std::vector< std::vector >& objectPoints,\r\n const std::vector< std::vector >& imagePoints);\r\n\r\n // Lift points from the image plane to the sphere\r\n void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const;\r\n //%output P\r\n\r\n // Lift points from the image plane to the projective space\r\n void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const;\r\n //%output P\r\n\r\n // Projects 3D points to the image plane (Pi function)\r\n void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const;\r\n //%output p\r\n\r\n // Projects 3D points to the image plane (Pi function)\r\n // and calculates jacobian\r\n void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p,\r\n Eigen::Matrix& J) const;\r\n //%output p\r\n //%output J\r\n\r\n void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const;\r\n //%output p\r\n\r\n template \r\n static void spaceToPlane(const T* const params,\r\n const T* const q, const T* const t,\r\n const Eigen::Matrix& P,\r\n Eigen::Matrix& p);\r\n\r\n void distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u) const;\r\n void distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u,\r\n Eigen::Matrix2d& J) const;\r\n\r\n void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0) const;\r\n cv::Mat initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2,\r\n float fx = -1.0f, float fy = -1.0f,\r\n cv::Size imageSize = cv::Size(0, 0),\r\n float cx = -1.0f, float cy = -1.0f,\r\n cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const;\r\n\r\n int parameterCount(void) const;\r\n\r\n const Parameters& getParameters(void) const;\r\n void setParameters(const Parameters& parameters);\r\n\r\n void readParameters(const std::vector& parameterVec);\r\n void writeParameters(std::vector& parameterVec) const;\r\n\r\n void writeParametersToYamlFile(const std::string& filename) const;\r\n\r\n std::string parametersToString(void) const;\r\n\r\nprivate:\r\n Parameters mParameters;\r\n\r\n double m_inv_K11, m_inv_K13, m_inv_K22, m_inv_K23;\r\n bool m_noDistortion;\r\n};\r\n\r\ntypedef boost::shared_ptr CataCameraPtr;\r\ntypedef boost::shared_ptr CataCameraConstPtr;\r\n\r\ntemplate \r\nvoid\r\nCataCamera::spaceToPlane(const T* const params,\r\n const T* const q, const T* const t,\r\n const Eigen::Matrix& P,\r\n Eigen::Matrix& p)\r\n{\r\n T P_w[3];\r\n P_w[0] = T(P(0));\r\n P_w[1] = T(P(1));\r\n P_w[2] = T(P(2));\r\n\r\n // Convert quaternion from Eigen convention (x, y, z, w)\r\n // to Ceres convention (w, x, y, z)\r\n T q_ceres[4] = {q[3], q[0], q[1], q[2]};\r\n\r\n T P_c[3];\r\n ceres::QuaternionRotatePoint(q_ceres, P_w, P_c);\r\n\r\n P_c[0] += t[0];\r\n P_c[1] += t[1];\r\n P_c[2] += t[2];\r\n\r\n // project 3D object point to the image plane\r\n T xi = params[0];\r\n T k1 = params[1];\r\n T k2 = params[2];\r\n T p1 = params[3];\r\n T p2 = params[4];\r\n T gamma1 = params[5];\r\n T gamma2 = params[6];\r\n T alpha = T(0); //cameraParams.alpha();\r\n T u0 = params[7];\r\n T v0 = params[8];\r\n\r\n // Transform to model plane\r\n T len = sqrt(P_c[0] * P_c[0] + P_c[1] * P_c[1] + P_c[2] * P_c[2]);\r\n P_c[0] /= len;\r\n P_c[1] /= len;\r\n P_c[2] /= len;\r\n\r\n T u = P_c[0] / (P_c[2] + xi);\r\n T v = P_c[1] / (P_c[2] + xi);\r\n\r\n T rho_sqr = u * u + v * v;\r\n T L = T(1.0) + k1 * rho_sqr + k2 * rho_sqr * rho_sqr;\r\n T du = T(2.0) * p1 * u * v + p2 * (rho_sqr + T(2.0) * u * u);\r\n T dv = p1 * (rho_sqr + T(2.0) * v * v) + T(2.0) * p2 * u * v;\r\n\r\n u = L * u + du;\r\n v = L * v + dv;\r\n p(0) = gamma1 * (u + alpha * v) + u0;\r\n p(1) = gamma2 * v + v0;\r\n}\r\n\r\n}\r\n\r\n#endif\r\n" }, { "path": "camera_model/include/camodocal/camera_models/CostFunctionFactory.h", "content": "#ifndef COSTFUNCTIONFACTORY_H\n#define COSTFUNCTIONFACTORY_H\n\n#include \n#include \n\n#include \"camodocal/camera_models/Camera.h\"\n\nnamespace ceres\n{\n class CostFunction;\n}\n\nnamespace camodocal\n{\n\nenum\n{\n CAMERA_INTRINSICS = 1 << 0,\n CAMERA_POSE = 1 << 1,\n POINT_3D = 1 << 2,\n ODOMETRY_INTRINSICS = 1 << 3,\n ODOMETRY_3D_POSE = 1 << 4,\n ODOMETRY_6D_POSE = 1 << 5,\n CAMERA_ODOMETRY_TRANSFORM = 1 << 6\n};\n\nclass CostFunctionFactory\n{\npublic:\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n CostFunctionFactory();\n\n static boost::shared_ptr instance(void);\n\n ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera,\n const Eigen::Vector3d& observed_P,\n const Eigen::Vector2d& observed_p,\n int flags) const;\n\n ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera,\n const Eigen::Vector3d& observed_P,\n const Eigen::Vector2d& observed_p,\n const Eigen::Matrix2d& sqrtPrecisionMat,\n int flags) const;\n\n ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera,\n const Eigen::Vector2d& observed_p,\n int flags, bool optimize_cam_odo_z = true) const;\n\n ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera,\n const Eigen::Vector2d& observed_p,\n const Eigen::Matrix2d& sqrtPrecisionMat,\n int flags, bool optimize_cam_odo_z = true) const;\n\n ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera,\n const Eigen::Vector3d& odo_pos,\n const Eigen::Vector3d& odo_att,\n const Eigen::Vector2d& observed_p,\n int flags, bool optimize_cam_odo_z = true) const;\n\n ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera,\n const Eigen::Quaterniond& cam_odo_q,\n const Eigen::Vector3d& cam_odo_t,\n const Eigen::Vector3d& odo_pos,\n const Eigen::Vector3d& odo_att,\n const Eigen::Vector2d& observed_p,\n int flags) const;\n\n ceres::CostFunction* generateCostFunction(const CameraConstPtr& cameraLeft,\n const CameraConstPtr& cameraRight,\n const Eigen::Vector3d& observed_P,\n const Eigen::Vector2d& observed_p_left,\n const Eigen::Vector2d& observed_p_right) const;\n\nprivate:\n static boost::shared_ptr m_instance;\n};\n\n}\n\n#endif\n" }, { "path": "camera_model/include/camodocal/camera_models/EquidistantCamera.h", "content": "#ifndef EQUIDISTANTCAMERA_H\r\n#define EQUIDISTANTCAMERA_H\r\n\r\n#include \r\n#include \r\n\r\n#include \"ceres/rotation.h\"\r\n#include \"Camera.h\"\r\n\r\nnamespace camodocal\r\n{\r\n\r\n/**\r\n * J. Kannala, and S. Brandt, A Generic Camera Model and Calibration Method\r\n * for Conventional, Wide-Angle, and Fish-Eye Lenses, PAMI 2006\r\n */\r\n\r\nclass EquidistantCamera: public Camera\r\n{\r\npublic:\r\n class Parameters: public Camera::Parameters\r\n {\r\n public:\r\n Parameters();\r\n Parameters(const std::string& cameraName,\r\n int w, int h,\r\n double k2, double k3, double k4, double k5,\r\n double mu, double mv,\r\n double u0, double v0);\r\n\r\n double& k2(void);\r\n double& k3(void);\r\n double& k4(void);\r\n double& k5(void);\r\n double& mu(void);\r\n double& mv(void);\r\n double& u0(void);\r\n double& v0(void);\r\n\r\n double k2(void) const;\r\n double k3(void) const;\r\n double k4(void) const;\r\n double k5(void) const;\r\n double mu(void) const;\r\n double mv(void) const;\r\n double u0(void) const;\r\n double v0(void) const;\r\n\r\n bool readFromYamlFile(const std::string& filename);\r\n void writeToYamlFile(const std::string& filename) const;\r\n\r\n Parameters& operator=(const Parameters& other);\r\n friend std::ostream& operator<< (std::ostream& out, const Parameters& params);\r\n\r\n private:\r\n // projection\r\n double m_k2;\r\n double m_k3;\r\n double m_k4;\r\n double m_k5;\r\n\r\n double m_mu;\r\n double m_mv;\r\n double m_u0;\r\n double m_v0;\r\n };\r\n\r\n EquidistantCamera();\r\n\r\n /**\r\n * \\brief Constructor from the projection model parameters\r\n */\r\n EquidistantCamera(const std::string& cameraName,\r\n int imageWidth, int imageHeight,\r\n double k2, double k3, double k4, double k5,\r\n double mu, double mv,\r\n double u0, double v0);\r\n /**\r\n * \\brief Constructor from the projection model parameters\r\n */\r\n EquidistantCamera(const Parameters& params);\r\n\r\n Camera::ModelType modelType(void) const;\r\n const std::string& cameraName(void) const;\r\n int imageWidth(void) const;\r\n int imageHeight(void) const;\r\n\r\n void estimateIntrinsics(const cv::Size& boardSize,\r\n const std::vector< std::vector >& objectPoints,\r\n const std::vector< std::vector >& imagePoints);\r\n\r\n // Lift points from the image plane to the sphere\r\n virtual void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const;\r\n //%output P\r\n\r\n // Lift points from the image plane to the projective space\r\n void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const;\r\n //%output P\r\n\r\n // Projects 3D points to the image plane (Pi function)\r\n void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const;\r\n //%output p\r\n\r\n // Projects 3D points to the image plane (Pi function)\r\n // and calculates jacobian\r\n void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p,\r\n Eigen::Matrix& J) const;\r\n //%output p\r\n //%output J\r\n\r\n void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const;\r\n //%output p\r\n\r\n template \r\n static void spaceToPlane(const T* const params,\r\n const T* const q, const T* const t,\r\n const Eigen::Matrix& P,\r\n Eigen::Matrix& p);\r\n\r\n void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0) const;\r\n cv::Mat initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2,\r\n float fx = -1.0f, float fy = -1.0f,\r\n cv::Size imageSize = cv::Size(0, 0),\r\n float cx = -1.0f, float cy = -1.0f,\r\n cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const;\r\n\r\n int parameterCount(void) const;\r\n\r\n const Parameters& getParameters(void) const;\r\n void setParameters(const Parameters& parameters);\r\n\r\n void readParameters(const std::vector& parameterVec);\r\n void writeParameters(std::vector& parameterVec) const;\r\n\r\n void writeParametersToYamlFile(const std::string& filename) const;\r\n\r\n std::string parametersToString(void) const;\r\n\r\nprivate:\r\n template\r\n static T r(T k2, T k3, T k4, T k5, T theta);\r\n\r\n\r\n void fitOddPoly(const std::vector& x, const std::vector& y,\r\n int n, std::vector& coeffs) const;\r\n\r\n void backprojectSymmetric(const Eigen::Vector2d& p_u,\r\n double& theta, double& phi) const;\r\n\r\n Parameters mParameters;\r\n\r\n double m_inv_K11, m_inv_K13, m_inv_K22, m_inv_K23;\r\n};\r\n\r\ntypedef boost::shared_ptr EquidistantCameraPtr;\r\ntypedef boost::shared_ptr EquidistantCameraConstPtr;\r\n\r\ntemplate\r\nT\r\nEquidistantCamera::r(T k2, T k3, T k4, T k5, T theta)\r\n{\r\n // k1 = 1\r\n return theta +\r\n k2 * theta * theta * theta +\r\n k3 * theta * theta * theta * theta * theta +\r\n k4 * theta * theta * theta * theta * theta * theta * theta +\r\n k5 * theta * theta * theta * theta * theta * theta * theta * theta * theta;\r\n}\r\n\r\ntemplate \r\nvoid\r\nEquidistantCamera::spaceToPlane(const T* const params,\r\n const T* const q, const T* const t,\r\n const Eigen::Matrix& P,\r\n Eigen::Matrix& p)\r\n{\r\n T P_w[3];\r\n P_w[0] = T(P(0));\r\n P_w[1] = T(P(1));\r\n P_w[2] = T(P(2));\r\n\r\n // Convert quaternion from Eigen convention (x, y, z, w)\r\n // to Ceres convention (w, x, y, z)\r\n T q_ceres[4] = {q[3], q[0], q[1], q[2]};\r\n\r\n T P_c[3];\r\n ceres::QuaternionRotatePoint(q_ceres, P_w, P_c);\r\n\r\n P_c[0] += t[0];\r\n P_c[1] += t[1];\r\n P_c[2] += t[2];\r\n\r\n // project 3D object point to the image plane;\r\n T k2 = params[0];\r\n T k3 = params[1];\r\n T k4 = params[2];\r\n T k5 = params[3];\r\n T mu = params[4];\r\n T mv = params[5];\r\n T u0 = params[6];\r\n T v0 = params[7];\r\n\r\n T len = sqrt(P_c[0] * P_c[0] + P_c[1] * P_c[1] + P_c[2] * P_c[2]);\r\n T theta = acos(P_c[2] / len);\r\n T phi = atan2(P_c[1], P_c[0]);\r\n\r\n Eigen::Matrix p_u = r(k2, k3, k4, k5, theta) * Eigen::Matrix(cos(phi), sin(phi));\r\n\r\n p(0) = mu * p_u(0) + u0;\r\n p(1) = mv * p_u(1) + v0;\r\n}\r\n\r\n}\r\n\r\n#endif\r\n" }, { "path": "camera_model/include/camodocal/camera_models/PinholeCamera.h", "content": "#ifndef PINHOLECAMERA_H\n#define PINHOLECAMERA_H\n\n#include \n#include \n\n#include \"ceres/rotation.h\"\n#include \"Camera.h\"\n\nnamespace camodocal\n{\n\nclass PinholeCamera: public Camera\n{\npublic:\n class Parameters: public Camera::Parameters\n {\n public:\n Parameters();\n Parameters(const std::string& cameraName,\n int w, int h,\n double k1, double k2, double p1, double p2,\n double fx, double fy, double cx, double cy);\n\n double& k1(void);\n double& k2(void);\n double& p1(void);\n double& p2(void);\n double& fx(void);\n double& fy(void);\n double& cx(void);\n double& cy(void);\n\n double xi(void) const;\n double k1(void) const;\n double k2(void) const;\n double p1(void) const;\n double p2(void) const;\n double fx(void) const;\n double fy(void) const;\n double cx(void) const;\n double cy(void) const;\n\n bool readFromYamlFile(const std::string& filename);\n void writeToYamlFile(const std::string& filename) const;\n\n Parameters& operator=(const Parameters& other);\n friend std::ostream& operator<< (std::ostream& out, const Parameters& params);\n\n private:\n double m_k1;\n double m_k2;\n double m_p1;\n double m_p2;\n double m_fx;\n double m_fy;\n double m_cx;\n double m_cy;\n };\n\n PinholeCamera();\n\n /**\n * \\brief Constructor from the projection model parameters\n */\n PinholeCamera(const std::string& cameraName,\n int imageWidth, int imageHeight,\n double k1, double k2, double p1, double p2,\n double fx, double fy, double cx, double cy);\n /**\n * \\brief Constructor from the projection model parameters\n */\n PinholeCamera(const Parameters& params);\n\n Camera::ModelType modelType(void) const;\n const std::string& cameraName(void) const;\n int imageWidth(void) const;\n int imageHeight(void) const;\n\n void estimateIntrinsics(const cv::Size& boardSize,\n const std::vector< std::vector >& objectPoints,\n const std::vector< std::vector >& imagePoints);\n\n // Lift points from the image plane to the sphere\n virtual void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const;\n //%output P\n\n // Lift points from the image plane to the projective space\n void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const;\n //%output P\n\n // Projects 3D points to the image plane (Pi function)\n void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const;\n //%output p\n\n // Projects 3D points to the image plane (Pi function)\n // and calculates jacobian\n void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p,\n Eigen::Matrix& J) const;\n //%output p\n //%output J\n\n void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const;\n //%output p\n\n template \n static void spaceToPlane(const T* const params,\n const T* const q, const T* const t,\n const Eigen::Matrix& P,\n Eigen::Matrix& p);\n\n void distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u) const;\n void distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u,\n Eigen::Matrix2d& J) const;\n\n void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0) const;\n cv::Mat initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2,\n float fx = -1.0f, float fy = -1.0f,\n cv::Size imageSize = cv::Size(0, 0),\n float cx = -1.0f, float cy = -1.0f,\n cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const;\n\n int parameterCount(void) const;\n\n const Parameters& getParameters(void) const;\n void setParameters(const Parameters& parameters);\n\n void readParameters(const std::vector& parameterVec);\n void writeParameters(std::vector& parameterVec) const;\n\n void writeParametersToYamlFile(const std::string& filename) const;\n\n std::string parametersToString(void) const;\n\nprivate:\n Parameters mParameters;\n\n double m_inv_K11, m_inv_K13, m_inv_K22, m_inv_K23;\n bool m_noDistortion;\n};\n\ntypedef boost::shared_ptr PinholeCameraPtr;\ntypedef boost::shared_ptr PinholeCameraConstPtr;\n\ntemplate \nvoid\nPinholeCamera::spaceToPlane(const T* const params,\n const T* const q, const T* const t,\n const Eigen::Matrix& P,\n Eigen::Matrix& p)\n{\n T P_w[3];\n P_w[0] = T(P(0));\n P_w[1] = T(P(1));\n P_w[2] = T(P(2));\n\n // Convert quaternion from Eigen convention (x, y, z, w)\n // to Ceres convention (w, x, y, z)\n T q_ceres[4] = {q[3], q[0], q[1], q[2]};\n\n T P_c[3];\n ceres::QuaternionRotatePoint(q_ceres, P_w, P_c);\n\n P_c[0] += t[0];\n P_c[1] += t[1];\n P_c[2] += t[2];\n\n // project 3D object point to the image plane\n T k1 = params[0];\n T k2 = params[1];\n T p1 = params[2];\n T p2 = params[3];\n T fx = params[4];\n T fy = params[5];\n T alpha = T(0); //cameraParams.alpha();\n T cx = params[6];\n T cy = params[7];\n\n // Transform to model plane\n T u = P_c[0] / P_c[2];\n T v = P_c[1] / P_c[2];\n\n T rho_sqr = u * u + v * v;\n T L = T(1.0) + k1 * rho_sqr + k2 * rho_sqr * rho_sqr;\n T du = T(2.0) * p1 * u * v + p2 * (rho_sqr + T(2.0) * u * u);\n T dv = p1 * (rho_sqr + T(2.0) * v * v) + T(2.0) * p2 * u * v;\n\n u = L * u + du;\n v = L * v + dv;\n p(0) = fx * (u + alpha * v) + cx;\n p(1) = fy * v + cy;\n}\n\n}\n\n#endif\n" }, { "path": "camera_model/include/camodocal/camera_models/ScaramuzzaCamera.h", "content": "#ifndef SCARAMUZZACAMERA_H\r\n#define SCARAMUZZACAMERA_H\r\n\r\n#include \r\n#include \r\n\r\n#include \"ceres/rotation.h\"\r\n#include \"Camera.h\"\r\n\r\nnamespace camodocal\r\n{\r\n\r\n#define SCARAMUZZA_POLY_SIZE 5\r\n#define SCARAMUZZA_INV_POLY_SIZE 20\r\n\r\n#define SCARAMUZZA_CAMERA_NUM_PARAMS (SCARAMUZZA_POLY_SIZE + SCARAMUZZA_INV_POLY_SIZE + 2 /*center*/ + 3 /*affine*/)\r\n\r\n/**\r\n * Scaramuzza Camera (Omnidirectional)\r\n * https://sites.google.com/site/scarabotix/ocamcalib-toolbox\r\n */\r\n\r\nclass OCAMCamera: public Camera\r\n{\r\npublic:\r\n class Parameters: public Camera::Parameters\r\n {\r\n public:\r\n Parameters();\r\n\r\n double& C(void) { return m_C; }\r\n double& D(void) { return m_D; }\r\n double& E(void) { return m_E; }\r\n\r\n double& center_x(void) { return m_center_x; }\r\n double& center_y(void) { return m_center_y; }\r\n\r\n double& poly(int idx) { return m_poly[idx]; }\r\n double& inv_poly(int idx) { return m_inv_poly[idx]; }\r\n\r\n double C(void) const { return m_C; }\r\n double D(void) const { return m_D; }\r\n double E(void) const { return m_E; }\r\n\r\n double center_x(void) const { return m_center_x; }\r\n double center_y(void) const { return m_center_y; }\r\n\r\n double poly(int idx) const { return m_poly[idx]; }\r\n double inv_poly(int idx) const { return m_inv_poly[idx]; }\r\n\r\n bool readFromYamlFile(const std::string& filename);\r\n void writeToYamlFile(const std::string& filename) const;\r\n\r\n Parameters& operator=(const Parameters& other);\r\n friend std::ostream& operator<< (std::ostream& out, const Parameters& params);\r\n\r\n private:\r\n double m_poly[SCARAMUZZA_POLY_SIZE];\r\n double m_inv_poly[SCARAMUZZA_INV_POLY_SIZE];\r\n double m_C;\r\n double m_D;\r\n double m_E;\r\n double m_center_x;\r\n double m_center_y;\r\n };\r\n\r\n OCAMCamera();\r\n\r\n /**\r\n * \\brief Constructor from the projection model parameters\r\n */\r\n OCAMCamera(const Parameters& params);\r\n\r\n Camera::ModelType modelType(void) const;\r\n const std::string& cameraName(void) const;\r\n int imageWidth(void) const;\r\n int imageHeight(void) const;\r\n\r\n void estimateIntrinsics(const cv::Size& boardSize,\r\n const std::vector< std::vector >& objectPoints,\r\n const std::vector< std::vector >& imagePoints);\r\n\r\n // Lift points from the image plane to the sphere\r\n void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const;\r\n //%output P\r\n\r\n // Lift points from the image plane to the projective space\r\n void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const;\r\n //%output P\r\n\r\n // Projects 3D points to the image plane (Pi function)\r\n void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const;\r\n //%output p\r\n\r\n // Projects 3D points to the image plane (Pi function)\r\n // and calculates jacobian\r\n //void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p,\r\n // Eigen::Matrix& J) const;\r\n //%output p\r\n //%output J\r\n\r\n void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const;\r\n //%output p\r\n\r\n template \r\n static void spaceToPlane(const T* const params,\r\n const T* const q, const T* const t,\r\n const Eigen::Matrix& P,\r\n Eigen::Matrix& p);\r\n template \r\n static void spaceToSphere(const T* const params,\r\n const T* const q, const T* const t,\r\n const Eigen::Matrix& P,\r\n Eigen::Matrix& P_s);\r\n template \r\n static void LiftToSphere(const T* const params,\r\n const Eigen::Matrix& p,\r\n Eigen::Matrix& P);\r\n\r\n template \r\n static void SphereToPlane(const T* const params, const Eigen::Matrix& P,\r\n Eigen::Matrix& p);\r\n\r\n\r\n void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0) const;\r\n cv::Mat initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2,\r\n float fx = -1.0f, float fy = -1.0f,\r\n cv::Size imageSize = cv::Size(0, 0),\r\n float cx = -1.0f, float cy = -1.0f,\r\n cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const;\r\n\r\n int parameterCount(void) const;\r\n\r\n const Parameters& getParameters(void) const;\r\n void setParameters(const Parameters& parameters);\r\n\r\n void readParameters(const std::vector& parameterVec);\r\n void writeParameters(std::vector& parameterVec) const;\r\n\r\n void writeParametersToYamlFile(const std::string& filename) const;\r\n\r\n std::string parametersToString(void) const;\r\n\r\nprivate:\r\n Parameters mParameters;\r\n\r\n double m_inv_scale;\r\n};\r\n\r\ntypedef boost::shared_ptr OCAMCameraPtr;\r\ntypedef boost::shared_ptr OCAMCameraConstPtr;\r\n\r\ntemplate \r\nvoid\r\nOCAMCamera::spaceToPlane(const T* const params,\r\n const T* const q, const T* const t,\r\n const Eigen::Matrix& P,\r\n Eigen::Matrix& p)\r\n{\r\n T P_c[3];\r\n {\r\n T P_w[3];\r\n P_w[0] = T(P(0));\r\n P_w[1] = T(P(1));\r\n P_w[2] = T(P(2));\r\n\r\n // Convert quaternion from Eigen convention (x, y, z, w)\r\n // to Ceres convention (w, x, y, z)\r\n T q_ceres[4] = {q[3], q[0], q[1], q[2]};\r\n\r\n ceres::QuaternionRotatePoint(q_ceres, P_w, P_c);\r\n\r\n P_c[0] += t[0];\r\n P_c[1] += t[1];\r\n P_c[2] += t[2];\r\n }\r\n\r\n T c = params[0];\r\n T d = params[1];\r\n T e = params[2];\r\n T xc[2] = { params[3], params[4] };\r\n\r\n //T poly[SCARAMUZZA_POLY_SIZE];\r\n //for (int i=0; i < SCARAMUZZA_POLY_SIZE; i++)\r\n // poly[i] = params[5+i];\r\n\r\n T inv_poly[SCARAMUZZA_INV_POLY_SIZE];\r\n for (int i=0; i < SCARAMUZZA_INV_POLY_SIZE; i++)\r\n inv_poly[i] = params[5 + SCARAMUZZA_POLY_SIZE + i];\r\n\r\n T norm_sqr = P_c[0] * P_c[0] + P_c[1] * P_c[1];\r\n T norm = T(0.0);\r\n if (norm_sqr > T(0.0))\r\n norm = sqrt(norm_sqr);\r\n\r\n T theta = atan2(-P_c[2], norm);\r\n T rho = T(0.0);\r\n T theta_i = T(1.0);\r\n\r\n for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++)\r\n {\r\n rho += theta_i * inv_poly[i];\r\n theta_i *= theta;\r\n }\r\n\r\n T invNorm = T(1.0) / norm;\r\n T xn[2] = {\r\n P_c[0] * invNorm * rho,\r\n P_c[1] * invNorm * rho\r\n };\r\n\r\n p(0) = xn[0] * c + xn[1] * d + xc[0];\r\n p(1) = xn[0] * e + xn[1] + xc[1];\r\n}\r\n\r\ntemplate \r\nvoid\r\nOCAMCamera::spaceToSphere(const T* const params,\r\n const T* const q, const T* const t,\r\n const Eigen::Matrix& P,\r\n Eigen::Matrix& P_s)\r\n{\r\n T P_c[3];\r\n {\r\n T P_w[3];\r\n P_w[0] = T(P(0));\r\n P_w[1] = T(P(1));\r\n P_w[2] = T(P(2));\r\n\r\n // Convert quaternion from Eigen convention (x, y, z, w)\r\n // to Ceres convention (w, x, y, z)\r\n T q_ceres[4] = {q[3], q[0], q[1], q[2]};\r\n\r\n ceres::QuaternionRotatePoint(q_ceres, P_w, P_c);\r\n\r\n P_c[0] += t[0];\r\n P_c[1] += t[1];\r\n P_c[2] += t[2];\r\n }\r\n\r\n //T poly[SCARAMUZZA_POLY_SIZE];\r\n //for (int i=0; i < SCARAMUZZA_POLY_SIZE; i++)\r\n // poly[i] = params[5+i];\r\n\r\n T norm_sqr = P_c[0] * P_c[0] + P_c[1] * P_c[1] + P_c[2] * P_c[2];\r\n T norm = T(0.0);\r\n if (norm_sqr > T(0.0))\r\n norm = sqrt(norm_sqr);\r\n\r\n P_s(0) = P_c[0] / norm;\r\n P_s(1) = P_c[1] / norm;\r\n P_s(2) = P_c[2] / norm;\r\n}\r\n\r\ntemplate \r\nvoid\r\nOCAMCamera::LiftToSphere(const T* const params,\r\n const Eigen::Matrix& p,\r\n Eigen::Matrix& P)\r\n{\r\n T c = params[0];\r\n T d = params[1];\r\n T e = params[2];\r\n T cc[2] = { params[3], params[4] };\r\n T poly[SCARAMUZZA_POLY_SIZE];\r\n for (int i=0; i < SCARAMUZZA_POLY_SIZE; i++)\r\n poly[i] = params[5+i];\r\n\r\n // Relative to Center\r\n T p_2d[2];\r\n p_2d[0] = T(p(0));\r\n p_2d[1] = T(p(1));\r\n\r\n T xc[2] = { p_2d[0] - cc[0], p_2d[1] - cc[1]};\r\n\r\n T inv_scale = T(1.0) / (c - d * e);\r\n\r\n // Affine Transformation\r\n T xc_a[2];\r\n\r\n xc_a[0] = inv_scale * (xc[0] - d * xc[1]);\r\n xc_a[1] = inv_scale * (-e * xc[0] + c * xc[1]);\r\n\r\n T norm_sqr = xc_a[0] * xc_a[0] + xc_a[1] * xc_a[1];\r\n T phi = sqrt(norm_sqr);\r\n T phi_i = T(1.0);\r\n T z = T(0.0);\r\n\r\n for (int i = 0; i < SCARAMUZZA_POLY_SIZE; i++)\r\n {\r\n if (i!=1) {\r\n z += phi_i * poly[i];\r\n }\r\n phi_i *= phi;\r\n }\r\n\r\n T p_3d[3];\r\n p_3d[0] = xc[0];\r\n p_3d[1] = xc[1];\r\n p_3d[2] = -z;\r\n\r\n T p_3d_norm_sqr = p_3d[0] * p_3d[0] + p_3d[1] * p_3d[1] + p_3d[2] * p_3d[2];\r\n T p_3d_norm = sqrt(p_3d_norm_sqr);\r\n\r\n P << p_3d[0] / p_3d_norm, p_3d[1] / p_3d_norm, p_3d[2] / p_3d_norm;\r\n}\r\n\r\ntemplate \r\nvoid OCAMCamera::SphereToPlane(const T* const params, const Eigen::Matrix& P,\r\n Eigen::Matrix& p) {\r\n T P_c[3];\r\n {\r\n P_c[0] = T(P(0));\r\n P_c[1] = T(P(1));\r\n P_c[2] = T(P(2));\r\n }\r\n\r\n T c = params[0];\r\n T d = params[1];\r\n T e = params[2];\r\n T xc[2] = {params[3], params[4]};\r\n\r\n T inv_poly[SCARAMUZZA_INV_POLY_SIZE];\r\n for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++)\r\n inv_poly[i] = params[5 + SCARAMUZZA_POLY_SIZE + i];\r\n\r\n T norm_sqr = P_c[0] * P_c[0] + P_c[1] * P_c[1];\r\n T norm = T(0.0);\r\n if (norm_sqr > T(0.0)) norm = sqrt(norm_sqr);\r\n\r\n T theta = atan2(-P_c[2], norm);\r\n T rho = T(0.0);\r\n T theta_i = T(1.0);\r\n\r\n for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++) {\r\n rho += theta_i * inv_poly[i];\r\n theta_i *= theta;\r\n }\r\n\r\n T invNorm = T(1.0) / norm;\r\n T xn[2] = {P_c[0] * invNorm * rho, P_c[1] * invNorm * rho};\r\n\r\n p(0) = xn[0] * c + xn[1] * d + xc[0];\r\n p(1) = xn[0] * e + xn[1] + xc[1];\r\n}\r\n}\r\n\r\n#endif\r\n" }, { "path": "camera_model/include/camodocal/chessboard/Chessboard.h", "content": "#ifndef CHESSBOARD_H\n#define CHESSBOARD_H\n\n#include \n#include \n\nnamespace camodocal\n{\n\n// forward declarations\nclass ChessboardCorner;\ntypedef boost::shared_ptr ChessboardCornerPtr;\nclass ChessboardQuad;\ntypedef boost::shared_ptr ChessboardQuadPtr;\n\nclass Chessboard\n{\npublic:\n Chessboard(cv::Size boardSize, cv::Mat& image);\n\n void findCorners(bool useOpenCV = false);\n const std::vector& getCorners(void) const;\n bool cornersFound(void) const;\n\n const cv::Mat& getImage(void) const;\n const cv::Mat& getSketch(void) const;\n\nprivate:\n bool findChessboardCorners(const cv::Mat& image,\n const cv::Size& patternSize,\n std::vector& corners,\n int flags, bool useOpenCV);\n\n bool findChessboardCornersImproved(const cv::Mat& image,\n const cv::Size& patternSize,\n std::vector& corners,\n int flags);\n\n void cleanFoundConnectedQuads(std::vector& quadGroup, cv::Size patternSize);\n\n void findConnectedQuads(std::vector& quads,\n std::vector& group,\n int group_idx, int dilation);\n\n// int checkQuadGroup(std::vector& quadGroup,\n// std::vector& outCorners,\n// cv::Size patternSize);\n\n void labelQuadGroup(std::vector& quad_group,\n cv::Size patternSize, bool firstRun);\n\n void findQuadNeighbors(std::vector& quads, int dilation);\n\n int augmentBestRun(std::vector& candidateQuads, int candidateDilation,\n std::vector& existingQuads, int existingDilation);\n\n void generateQuads(std::vector& quads,\n cv::Mat& image, int flags,\n int dilation, bool firstRun);\n\n bool checkQuadGroup(std::vector& quads,\n std::vector& corners,\n cv::Size patternSize);\n\n void getQuadrangleHypotheses(const std::vector< std::vector >& contours,\n std::vector< std::pair >& quads,\n int classId) const;\n\n bool checkChessboard(const cv::Mat& image, cv::Size patternSize) const;\n\n bool checkBoardMonotony(std::vector& corners,\n cv::Size patternSize);\n\n bool matchCorners(ChessboardQuadPtr& quad1, int corner1,\n ChessboardQuadPtr& quad2, int corner2) const;\n\n cv::Mat mImage;\n cv::Mat mSketch;\n std::vector mCorners;\n cv::Size mBoardSize;\n bool mCornersFound;\n};\n\n}\n\n#endif\n" }, { "path": "camera_model/include/camodocal/chessboard/ChessboardCorner.h", "content": "#ifndef CHESSBOARDCORNER_H\n#define CHESSBOARDCORNER_H\n\n#include \n#include \n\nnamespace camodocal\n{\n\nclass ChessboardCorner;\ntypedef boost::shared_ptr ChessboardCornerPtr;\n\nclass ChessboardCorner\n{\npublic:\n ChessboardCorner() : row(0), column(0), needsNeighbor(true), count(0) {}\n\n float meanDist(int &n) const\n {\n float sum = 0;\n n = 0;\n for (int i = 0; i < 4; ++i)\n {\n if (neighbors[i].get())\n {\n float dx = neighbors[i]->pt.x - pt.x;\n float dy = neighbors[i]->pt.y - pt.y;\n sum += sqrt(dx*dx + dy*dy);\n n++;\n }\n }\n return sum / std::max(n, 1);\n }\n\n cv::Point2f pt; // X and y coordinates\n int row; // Row and column of the corner\n int column; // in the found pattern\n bool needsNeighbor; // Does the corner require a neighbor?\n int count; // number of corner neighbors\n ChessboardCornerPtr neighbors[4]; // pointer to all corner neighbors\n};\n\n}\n\n#endif\n" }, { "path": "camera_model/include/camodocal/chessboard/ChessboardQuad.h", "content": "#ifndef CHESSBOARDQUAD_H\n#define CHESSBOARDQUAD_H\n\n#include \n\n#include \"camodocal/chessboard/ChessboardCorner.h\"\n\nnamespace camodocal\n{\n\nclass ChessboardQuad;\ntypedef boost::shared_ptr ChessboardQuadPtr;\n\nclass ChessboardQuad\n{\npublic:\n ChessboardQuad() : count(0), group_idx(-1), edge_len(FLT_MAX), labeled(false) {}\n\n int count; // Number of quad neighbors\n int group_idx; // Quad group ID\n float edge_len; // Smallest side length^2\n ChessboardCornerPtr corners[4]; // Coordinates of quad corners\n ChessboardQuadPtr neighbors[4]; // Pointers of quad neighbors\n bool labeled; // Has this corner been labeled?\n};\n\n}\n\n#endif\n" }, { "path": "camera_model/include/camodocal/chessboard/Spline.h", "content": "/* dynamo:- Event driven molecular dynamics simulator\n http://www.marcusbannerman.co.uk/dynamo\n Copyright (C) 2011 Marcus N Campbell Bannerman \n\n This program is free software: you can redistribute it and/or\n modify it under the terms of the GNU General Public License\n version 3 as published by the Free Software Foundation.\n\n This program is distributed in the hope that it will be useful,\n but WITHOUT ANY WARRANTY; without even the implied warranty of\n MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\n GNU General Public License for more details.\n\n You should have received a copy of the GNU General Public License\n along with this program. If not, see .\n*/\n\n#pragma once\n\n#include \n#include \n#include \n#include \n#include \n#include \n\nnamespace ublas = boost::numeric::ublas;\n\nclass Spline : private std::vector >\n{\npublic:\n //The boundary conditions available\n enum BC_type {\n\tFIXED_1ST_DERIV_BC,\n\tFIXED_2ND_DERIV_BC,\n\tPARABOLIC_RUNOUT_BC\n };\n\n enum Spline_type {\n\tLINEAR,\n\tCUBIC\n };\n\n //Constructor takes the boundary conditions as arguments, this\n //sets the first derivative (gradient) at the lower and upper\n //end points\n Spline():\n\t_valid(false),\n\t_BCLow(FIXED_2ND_DERIV_BC), _BCHigh(FIXED_2ND_DERIV_BC),\n\t_BCLowVal(0), _BCHighVal(0),\n\t_type(CUBIC)\n {}\n\n typedef std::vector > base;\n typedef base::const_iterator const_iterator;\n\n //Standard STL read-only container stuff\n const_iterator begin() const { return base::begin(); }\n const_iterator end() const { return base::end(); }\n void clear() { _valid = false; base::clear(); _data.clear(); }\n size_t size() const { return base::size(); }\n size_t max_size() const { return base::max_size(); }\n size_t capacity() const { return base::capacity(); }\n bool empty() const { return base::empty(); }\n\n //Add a point to the spline, and invalidate it so its\n //recalculated on the next access\n inline void addPoint(double x, double y)\n {\n\t_valid = false;\n\tbase::push_back(std::pair(x,y));\n }\n\n //Reset the boundary conditions\n inline void setLowBC(BC_type BC, double val = 0)\n { _BCLow = BC; _BCLowVal = val; _valid = false; }\n\n inline void setHighBC(BC_type BC, double val = 0)\n { _BCHigh = BC; _BCHighVal = val; _valid = false; }\n\n void setType(Spline_type type) { _type = type; _valid = false; }\n\n //Check if the spline has been calculated, then generate the\n //spline interpolated value\n double operator()(double xval)\n {\n\tif (!_valid) generate();\n\n\t//Special cases when we're outside the range of the spline points\n\tif (xval <= x(0)) return lowCalc(xval);\n\tif (xval >= x(size()-1)) return highCalc(xval);\n\n\t//Check all intervals except the last one\n\tfor (std::vector::const_iterator iPtr = _data.begin();\n\t\t iPtr != _data.end()-1; ++iPtr)\n\t\tif ((xval >= iPtr->x) && (xval <= (iPtr+1)->x))\n\t\t return splineCalc(iPtr, xval);\n\n\treturn splineCalc(_data.end() - 1, xval);\n }\n\nprivate:\n\n ///////PRIVATE DATA MEMBERS\n struct SplineData { double x,a,b,c,d; };\n //vector of calculated spline data\n std::vector _data;\n //Second derivative at each point\n ublas::vector _ddy;\n //Tracks whether the spline parameters have been calculated for\n //the current set of points\n bool _valid;\n //The boundary conditions\n BC_type _BCLow, _BCHigh;\n //The values of the boundary conditions\n double _BCLowVal, _BCHighVal;\n\n Spline_type _type;\n\n ///////PRIVATE FUNCTIONS\n //Function to calculate the value of a given spline at a point xval\n inline double splineCalc(std::vector::const_iterator i, double xval)\n {\n\tconst double lx = xval - i->x;\n\treturn ((i->a * lx + i->b) * lx + i->c) * lx + i->d;\n }\n\n inline double lowCalc(double xval)\n {\n\tconst double lx = xval - x(0);\n\n\tif (_type == LINEAR)\n\t return lx * _BCHighVal + y(0);\n\n\tconst double firstDeriv = (y(1) - y(0)) / h(0) - 2 * h(0) * (_data[0].b + 2 * _data[1].b) / 6;\n\n\tswitch(_BCLow)\n\t {\n\t case FIXED_1ST_DERIV_BC:\n\t\treturn lx * _BCLowVal + y(0);\n\t case FIXED_2ND_DERIV_BC:\n\t\t return lx * lx * _BCLowVal + firstDeriv * lx + y(0);\n\t case PARABOLIC_RUNOUT_BC:\n\t\treturn lx * lx * _ddy[0] + lx * firstDeriv + y(0);\n\t }\n\tthrow std::runtime_error(\"Unknown BC\");\n }\n\n inline double highCalc(double xval)\n {\n\tconst double lx = xval - x(size() - 1);\n\n\tif (_type == LINEAR)\n\t return lx * _BCHighVal + y(size() - 1);\n\n\tconst double firstDeriv = 2 * h(size() - 2) * (_ddy[size() - 2] + 2 * _ddy[size() - 1]) / 6 + (y(size() - 1) - y(size() - 2)) / h(size() - 2);\n\n\tswitch(_BCHigh)\n\t {\n\t case FIXED_1ST_DERIV_BC:\n\t\treturn lx * _BCHighVal + y(size() - 1);\n\t case FIXED_2ND_DERIV_BC:\n\t\treturn lx * lx * _BCHighVal + firstDeriv * lx + y(size() - 1);\n\t case PARABOLIC_RUNOUT_BC:\n\t\treturn lx * lx * _ddy[size()-1] + lx * firstDeriv + y(size() - 1);\n\t }\n\tthrow std::runtime_error(\"Unknown BC\");\n }\n\n //These just provide access to the point data in a clean way\n inline double x(size_t i) const { return operator[](i).first; }\n inline double y(size_t i) const { return operator[](i).second; }\n inline double h(size_t i) const { return x(i+1) - x(i); }\n\n //Invert a arbitrary matrix using the boost ublas library\n template\n bool InvertMatrix(ublas::matrix A,\n\t\tublas::matrix& inverse)\n {\n\tusing namespace ublas;\n\n\t// create a permutation matrix for the LU-factorization\n\tpermutation_matrix pm(A.size1());\n\n\t// perform LU-factorization\n\tint res = lu_factorize(A,pm);\n\t\tif( res != 0 ) return false;\n\n\t// create identity matrix of \"inverse\"\n\tinverse.assign(ublas::identity_matrix(A.size1()));\n\n\t// backsubstitute to get the inverse\n\tlu_substitute(A, pm, inverse);\n\n\treturn true;\n }\n\n //This function will recalculate the spline parameters and store\n //them in _data, ready for spline interpolation\n void generate()\n {\n\tif (size() < 2)\n\t throw std::runtime_error(\"Spline requires at least 2 points\");\n\n\t//If any spline points are at the same x location, we have to\n\t//just slightly seperate them\n\t{\n\t bool testPassed(false);\n\t while (!testPassed)\n\t\t{\n\t\t testPassed = true;\n\t\t std::sort(base::begin(), base::end());\n\n\t\t for (base::iterator iPtr = base::begin(); iPtr != base::end() - 1; ++iPtr)\n\t\tif (iPtr->first == (iPtr+1)->first)\n\t\t {\n\t\t\tif ((iPtr+1)->first != 0)\n\t\t\t (iPtr+1)->first += (iPtr+1)->first\n\t\t\t* std::numeric_limits::epsilon() * 10;\n\t\t\telse\n\t\t\t (iPtr+1)->first = std::numeric_limits::epsilon() * 10;\n\t\t\ttestPassed = false;\n\t\t\tbreak;\n\t\t }\n\t\t}\n\t}\n\n\tconst size_t e = size() - 1;\n\n\tswitch (_type)\n\t {\n\t case LINEAR:\n\t\t{\n\t\t _data.resize(e);\n\t\t for (size_t i(0); i < e; ++i)\n\t\t{\n\t\t _data[i].x = x(i);\n\t\t _data[i].a = 0;\n\t\t _data[i].b = 0;\n\t\t _data[i].c = (y(i+1) - y(i)) / (x(i+1) - x(i));\n\t\t _data[i].d = y(i);\n\t\t}\n\t\t break;\n\t\t}\n\t case CUBIC:\n\t\t{\n\t\t ublas::matrix A(size(), size());\n\t\t for (size_t yv(0); yv <= e; ++yv)\n\t\tfor (size_t xv(0); xv <= e; ++xv)\n\t\t A(xv,yv) = 0;\n\n\t\t for (size_t i(1); i < e; ++i)\n\t\t{\n\t\t A(i-1,i) = h(i-1);\n\t\t A(i,i) = 2 * (h(i-1) + h(i));\n\t\t A(i+1,i) = h(i);\n\t\t}\n\n\t\t ublas::vector C(size());\n\t\t for (size_t xv(0); xv <= e; ++xv)\n\t\tC(xv) = 0;\n\n\t\t for (size_t i(1); i < e; ++i)\n\t\tC(i) = 6 *\n\t\t ((y(i+1) - y(i)) / h(i)\n\t\t - (y(i) - y(i-1)) / h(i-1));\n\n\t\t //Boundary conditions\n\t\t switch(_BCLow)\n\t\t{\n\t\tcase FIXED_1ST_DERIV_BC:\n\t\t C(0) = 6 * ((y(1) - y(0)) / h(0) - _BCLowVal);\n\t\t A(0,0) = 2 * h(0);\n\t\t A(1,0) = h(0);\n\t\t break;\n\t\tcase FIXED_2ND_DERIV_BC:\n\t\t C(0) = _BCLowVal;\n\t\t A(0,0) = 1;\n\t\t break;\n\t\tcase PARABOLIC_RUNOUT_BC:\n\t\t C(0) = 0; A(0,0) = 1; A(1,0) = -1;\n\t\t break;\n\t\t}\n\n\t\t switch(_BCHigh)\n\t\t{\n\t\tcase FIXED_1ST_DERIV_BC:\n\t\t C(e) = 6 * (_BCHighVal - (y(e) - y(e-1)) / h(e-1));\n\t\t A(e,e) = 2 * h(e - 1);\n\t\t A(e-1,e) = h(e - 1);\n\t\t break;\n\t\tcase FIXED_2ND_DERIV_BC:\n\t\t C(e) = _BCHighVal;\n\t\t A(e,e) = 1;\n\t\t break;\n\t\tcase PARABOLIC_RUNOUT_BC:\n\t\t C(e) = 0; A(e,e) = 1; A(e-1,e) = -1;\n\t\t break;\n\t\t}\n\n\t\t ublas::matrix AInv(size(), size());\n\t\t InvertMatrix(A,AInv);\n\n\t\t _ddy = ublas::prod(C, AInv);\n\n\t\t _data.resize(size()-1);\n\t\t for (size_t i(0); i < e; ++i)\n\t\t{\n\t\t _data[i].x = x(i);\n\t\t _data[i].a = (_ddy(i+1) - _ddy(i)) / (6 * h(i));\n\t\t _data[i].b = _ddy(i) / 2;\n\t\t _data[i].c = (y(i+1) - y(i)) / h(i) - _ddy(i+1) * h(i) / 6 - _ddy(i) * h(i) / 3;\n\t\t _data[i].d = y(i);\n\t\t}\n\t\t}\n\t }\n\t_valid = true;\n }\n};\n" }, { "path": "camera_model/include/camodocal/gpl/EigenQuaternionParameterization.h", "content": "#ifndef EIGENQUATERNIONPARAMETERIZATION_H\n#define EIGENQUATERNIONPARAMETERIZATION_H\n\n#include \"ceres/local_parameterization.h\"\n\nnamespace camodocal\n{\n\nclass EigenQuaternionParameterization : public ceres::LocalParameterization\n{\npublic:\n virtual ~EigenQuaternionParameterization() {}\n virtual bool Plus(const double* x,\n const double* delta,\n double* x_plus_delta) const;\n virtual bool ComputeJacobian(const double* x,\n double* jacobian) const;\n virtual int GlobalSize() const { return 4; }\n virtual int LocalSize() const { return 3; }\n\nprivate:\n template\n void EigenQuaternionProduct(const T z[4], const T w[4], T zw[4]) const;\n};\n\n\ntemplate\nvoid\nEigenQuaternionParameterization::EigenQuaternionProduct(const T z[4], const T w[4], T zw[4]) const\n{\n zw[0] = z[3] * w[0] + z[0] * w[3] + z[1] * w[2] - z[2] * w[1];\n zw[1] = z[3] * w[1] - z[0] * w[2] + z[1] * w[3] + z[2] * w[0];\n zw[2] = z[3] * w[2] + z[0] * w[1] - z[1] * w[0] + z[2] * w[3];\n zw[3] = z[3] * w[3] - z[0] * w[0] - z[1] * w[1] - z[2] * w[2];\n}\n\n}\n\n#endif\n\n" }, { "path": "camera_model/include/camodocal/gpl/EigenUtils.h", "content": "#ifndef EIGENUTILS_H\n#define EIGENUTILS_H\n\n#include \n\n#include \"ceres/rotation.h\"\n#include \"camodocal/gpl/gpl.h\"\n\nnamespace camodocal\n{\n\n// Returns the 3D cross product skew symmetric matrix of a given 3D vector\ntemplate\nEigen::Matrix skew(const Eigen::Matrix& vec)\n{\n return (Eigen::Matrix() << T(0), -vec(2), vec(1),\n vec(2), T(0), -vec(0),\n -vec(1), vec(0), T(0)).finished();\n}\n\ntemplate\ntypename Eigen::MatrixBase::PlainObject sqrtm(const Eigen::MatrixBase& A)\n{\n Eigen::SelfAdjointEigenSolver es(A);\n\n return es.operatorSqrt();\n}\n\ntemplate\nEigen::Matrix AngleAxisToRotationMatrix(const Eigen::Matrix& rvec)\n{\n T angle = rvec.norm();\n if (angle == T(0))\n {\n return Eigen::Matrix::Identity();\n }\n\n Eigen::Matrix axis;\n axis = rvec.normalized();\n\n Eigen::Matrix rmat;\n rmat = Eigen::AngleAxis(angle, axis);\n\n return rmat;\n}\n\ntemplate\nEigen::Quaternion AngleAxisToQuaternion(const Eigen::Matrix& rvec)\n{\n Eigen::Matrix rmat = AngleAxisToRotationMatrix(rvec);\n\n return Eigen::Quaternion(rmat);\n}\n\ntemplate\nvoid AngleAxisToQuaternion(const Eigen::Matrix& rvec, T* q)\n{\n Eigen::Quaternion quat = AngleAxisToQuaternion(rvec);\n\n q[0] = quat.x();\n q[1] = quat.y();\n q[2] = quat.z();\n q[3] = quat.w();\n}\n\ntemplate\nEigen::Matrix RotationToAngleAxis(const Eigen::Matrix & rmat)\n{\n Eigen::AngleAxis angleaxis; \n angleaxis.fromRotationMatrix(rmat); \n return angleaxis.angle() * angleaxis.axis(); \n \n}\n\ntemplate\nvoid QuaternionToAngleAxis(const T* const q, Eigen::Matrix& rvec)\n{\n Eigen::Quaternion quat(q[3], q[0], q[1], q[2]);\n\n Eigen::Matrix rmat = quat.toRotationMatrix();\n\n Eigen::AngleAxis angleaxis;\n angleaxis.fromRotationMatrix(rmat);\n\n rvec = angleaxis.angle() * angleaxis.axis();\n}\n\ntemplate\nEigen::Matrix QuaternionToRotation(const T* const q)\n{\n T R[9];\n ceres::QuaternionToRotation(q, R);\n\n Eigen::Matrix rmat;\n for (int i = 0; i < 3; ++i)\n {\n for (int j = 0; j < 3; ++j)\n {\n rmat(i,j) = R[i * 3 + j];\n }\n }\n\n return rmat;\n}\n\ntemplate\nvoid QuaternionToRotation(const T* const q, T* rot)\n{\n ceres::QuaternionToRotation(q, rot);\n}\n\ntemplate\nEigen::Matrix QuaternionMultMatLeft(const Eigen::Quaternion& q)\n{\n return (Eigen::Matrix() << q.w(), -q.z(), q.y(), q.x(),\n q.z(), q.w(), -q.x(), q.y(),\n -q.y(), q.x(), q.w(), q.z(),\n -q.x(), -q.y(), -q.z(), q.w()).finished();\n}\n\ntemplate\nEigen::Matrix QuaternionMultMatRight(const Eigen::Quaternion& q)\n{\n return (Eigen::Matrix() << q.w(), q.z(), -q.y(), q.x(),\n -q.z(), q.w(), q.x(), q.y(),\n q.y(), -q.x(), q.w(), q.z(),\n -q.x(), -q.y(), -q.z(), q.w()).finished();\n}\n\n/// @param theta - rotation about screw axis\n/// @param d - projection of tvec on the rotation axis\n/// @param l - screw axis direction\n/// @param m - screw axis moment\ntemplate\nvoid AngleAxisAndTranslationToScrew(const Eigen::Matrix& rvec,\n const Eigen::Matrix& tvec,\n T& theta, T& d,\n Eigen::Matrix& l,\n Eigen::Matrix& m)\n{\n\n theta = rvec.norm();\n if (theta == 0)\n {\n l.setZero(); \n m.setZero(); \n std::cout << \"Warning: Undefined screw! Returned 0. \" << std::endl; \n }\n\n l = rvec.normalized();\n\n Eigen::Matrix t = tvec;\n\n d = t.transpose() * l;\n\n // point on screw axis - projection of origin on screw axis\n Eigen::Matrix c;\n c = 0.5 * (t - d * l + (1.0 / tan(theta / 2.0) * l).cross(t));\n\n // c and hence the screw axis is not defined if theta is either 0 or M_PI\n m = c.cross(l);\n}\n\ntemplate\nEigen::Matrix RPY2mat(T roll, T pitch, T yaw)\n{\n Eigen::Matrix m;\n\n T cr = cos(roll);\n T sr = sin(roll);\n T cp = cos(pitch);\n T sp = sin(pitch);\n T cy = cos(yaw);\n T sy = sin(yaw);\n\n m(0,0) = cy * cp;\n m(0,1) = cy * sp * sr - sy * cr;\n m(0,2) = cy * sp * cr + sy * sr;\n m(1,0) = sy * cp;\n m(1,1) = sy * sp * sr + cy * cr;\n m(1,2) = sy * sp * cr - cy * sr;\n m(2,0) = - sp;\n m(2,1) = cp * sr;\n m(2,2) = cp * cr;\n return m; \n}\n\ntemplate\nvoid mat2RPY(const Eigen::Matrix& m, T& roll, T& pitch, T& yaw)\n{\n roll = atan2(m(2,1), m(2,2));\n pitch = atan2(-m(2,0), sqrt(m(2,1) * m(2,1) + m(2,2) * m(2,2)));\n yaw = atan2(m(1,0), m(0,0));\n}\n\ntemplate\nEigen::Matrix homogeneousTransform(const Eigen::Matrix& R, const Eigen::Matrix& t)\n{\n Eigen::Matrix H;\n H.setIdentity();\n\n H.block(0,0,3,3) = R;\n H.block(0,3,3,1) = t;\n\n return H;\n}\n\ntemplate\nEigen::Matrix poseWithCartesianTranslation(const T* const q, const T* const p)\n{\n Eigen::Matrix pose = Eigen::Matrix::Identity();\n\n T rotation[9];\n ceres::QuaternionToRotation(q, rotation);\n for (int i = 0; i < 3; ++i)\n {\n for (int j = 0; j < 3; ++j)\n {\n pose(i,j) = rotation[i * 3 + j];\n }\n }\n\n pose(0,3) = p[0];\n pose(1,3) = p[1];\n pose(2,3) = p[2];\n\n return pose;\n}\n\ntemplate\nEigen::Matrix poseWithSphericalTranslation(const T* const q, const T* const p, const T scale = T(1.0))\n{\n Eigen::Matrix pose = Eigen::Matrix::Identity();\n\n T rotation[9];\n ceres::QuaternionToRotation(q, rotation);\n for (int i = 0; i < 3; ++i)\n {\n for (int j = 0; j < 3; ++j)\n {\n pose(i,j) = rotation[i * 3 + j];\n }\n }\n\n T theta = p[0];\n T phi = p[1];\n pose(0,3) = sin(theta) * cos(phi) * scale;\n pose(1,3) = sin(theta) * sin(phi) * scale;\n pose(2,3) = cos(theta) * scale;\n\n return pose;\n}\n\n// Returns the Sampson error of a given essential matrix and 2 image points\ntemplate\nT sampsonError(const Eigen::Matrix& E,\n const Eigen::Matrix& p1,\n const Eigen::Matrix& p2)\n{\n Eigen::Matrix Ex1 = E * p1;\n Eigen::Matrix Etx2 = E.transpose() * p2;\n\n T x2tEx1 = p2.dot(Ex1);\n\n // compute Sampson error\n T err = square(x2tEx1) / (square(Ex1(0,0)) + square(Ex1(1,0)) + square(Etx2(0,0)) + square(Etx2(1,0)));\n\n return err;\n}\n\n// Returns the Sampson error of a given rotation/translation and 2 image points\ntemplate\nT sampsonError(const Eigen::Matrix& R,\n const Eigen::Matrix& t,\n const Eigen::Matrix& p1,\n const Eigen::Matrix& p2)\n{\n // construct essential matrix\n Eigen::Matrix E = skew(t) * R;\n\n Eigen::Matrix Ex1 = E * p1;\n Eigen::Matrix Etx2 = E.transpose() * p2;\n\n T x2tEx1 = p2.dot(Ex1);\n\n // compute Sampson error\n T err = square(x2tEx1) / (square(Ex1(0,0)) + square(Ex1(1,0)) + square(Etx2(0,0)) + square(Etx2(1,0)));\n\n return err;\n}\n\n// Returns the Sampson error of a given rotation/translation and 2 image points\ntemplate\nT sampsonError(const Eigen::Matrix& H,\n const Eigen::Matrix& p1,\n const Eigen::Matrix& p2)\n{\n Eigen::Matrix R = H.block(0, 0, 3, 3);\n Eigen::Matrix t = H.block(0, 3, 3, 1);\n\n return sampsonError(R, t, p1, p2);\n}\n\ntemplate\nEigen::Matrix\ntransformPoint(const Eigen::Matrix& H, const Eigen::Matrix& P)\n{\n Eigen::Matrix P_trans = H.block(0, 0, 3, 3) * P + H.block(0, 3, 3, 1);\n\n return P_trans;\n}\n\ntemplate\nEigen::Matrix\nestimate3DRigidTransform(const std::vector, Eigen::aligned_allocator > >& points1,\n const std::vector, Eigen::aligned_allocator > >& points2)\n{\n // compute centroids\n Eigen::Matrix c1, c2;\n c1.setZero(); c2.setZero();\n\n for (size_t i = 0; i < points1.size(); ++i)\n {\n c1 += points1.at(i);\n c2 += points2.at(i);\n }\n\n c1 /= points1.size();\n c2 /= points1.size();\n\n Eigen::Matrix X(3, points1.size());\n Eigen::Matrix Y(3, points1.size());\n for (size_t i = 0; i < points1.size(); ++i)\n {\n X.col(i) = points1.at(i) - c1;\n Y.col(i) = points2.at(i) - c2;\n }\n\n Eigen::Matrix H = X * Y.transpose();\n\n Eigen::JacobiSVD< Eigen::Matrix > svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV);\n\n Eigen::Matrix U = svd.matrixU();\n Eigen::Matrix V = svd.matrixV();\n if (U.determinant() * V.determinant() < 0.0)\n {\n V.col(2) *= -1.0;\n }\n\n Eigen::Matrix R = V * U.transpose();\n Eigen::Matrix t = c2 - R * c1;\n\n return homogeneousTransform(R, t);\n}\n\ntemplate\nEigen::Matrix\nestimate3DRigidSimilarityTransform(const std::vector, Eigen::aligned_allocator > >& points1,\n const std::vector, Eigen::aligned_allocator > >& points2)\n{\n // compute centroids\n Eigen::Matrix c1, c2;\n c1.setZero(); c2.setZero();\n\n for (size_t i = 0; i < points1.size(); ++i)\n {\n c1 += points1.at(i);\n c2 += points2.at(i);\n }\n\n c1 /= points1.size();\n c2 /= points1.size();\n\n Eigen::Matrix X(3, points1.size());\n Eigen::Matrix Y(3, points1.size());\n for (size_t i = 0; i < points1.size(); ++i)\n {\n X.col(i) = points1.at(i) - c1;\n Y.col(i) = points2.at(i) - c2;\n }\n\n Eigen::Matrix H = X * Y.transpose();\n\n Eigen::JacobiSVD< Eigen::Matrix > svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV);\n\n Eigen::Matrix U = svd.matrixU();\n Eigen::Matrix V = svd.matrixV();\n if (U.determinant() * V.determinant() < 0.0)\n {\n V.col(2) *= -1.0;\n }\n\n Eigen::Matrix R = V * U.transpose();\n\n std::vector, Eigen::aligned_allocator > > rotatedPoints1(points1.size());\n for (size_t i = 0; i < points1.size(); ++i)\n {\n rotatedPoints1.at(i) = R * (points1.at(i) - c1);\n }\n\n double sum_ss = 0.0, sum_tt = 0.0;\n for (size_t i = 0; i < points1.size(); ++i)\n {\n sum_ss += (points1.at(i) - c1).squaredNorm();\n sum_tt += (points2.at(i) - c2).dot(rotatedPoints1.at(i));\n }\n\n double scale = sum_tt / sum_ss;\n\n Eigen::Matrix sR = scale * R;\n Eigen::Matrix t = c2 - sR * c1;\n\n return homogeneousTransform(sR, t);\n}\n\n}\n\n#endif\n" }, { "path": "camera_model/include/camodocal/gpl/gpl.h", "content": "#ifndef GPL_H\r\n#define GPL_H\r\n\r\n#include \r\n#include \r\n#include \r\n\r\nnamespace camodocal\r\n{\r\n\r\ntemplate\r\nconst T clamp(const T& v, const T& a, const T& b)\r\n{\r\n\treturn std::min(b, std::max(a, v));\r\n}\r\n\r\ndouble hypot3(double x, double y, double z);\r\nfloat hypot3f(float x, float y, float z);\r\n\r\ntemplate\r\nconst T normalizeTheta(const T& theta)\r\n{\r\n\tT normTheta = theta;\r\n\r\n\twhile (normTheta < - M_PI)\r\n\t{\r\n\t\tnormTheta += 2.0 * M_PI;\r\n\t}\r\n\twhile (normTheta > M_PI)\r\n\t{\r\n\t\tnormTheta -= 2.0 * M_PI;\r\n\t}\r\n\r\n\treturn normTheta;\r\n}\r\n\r\ndouble d2r(double deg);\r\nfloat d2r(float deg);\r\ndouble r2d(double rad);\r\nfloat r2d(float rad);\r\n\r\ndouble sinc(double theta);\r\n\r\ntemplate\r\nconst T square(const T& x)\r\n{\r\n\treturn x * x;\r\n}\r\n\r\ntemplate\r\nconst T cube(const T& x)\r\n{\r\n\treturn x * x * x;\r\n}\r\n\r\ntemplate\r\nconst T random(const T& a, const T& b)\r\n{\r\n\treturn static_cast(rand()) / RAND_MAX * (b - a) + a;\r\n}\r\n\r\ntemplate\r\nconst T randomNormal(const T& sigma)\r\n{\r\n T x1, x2, w;\r\n\r\n do\r\n {\r\n x1 = 2.0 * random(0.0, 1.0) - 1.0;\r\n x2 = 2.0 * random(0.0, 1.0) - 1.0;\r\n w = x1 * x1 + x2 * x2;\r\n }\r\n while (w >= 1.0 || w == 0.0);\r\n\r\n w = sqrt((-2.0 * log(w)) / w);\r\n\r\n return x1 * w * sigma;\r\n}\r\n\r\nunsigned long long timeInMicroseconds(void);\r\n\r\ndouble timeInSeconds(void);\r\n\r\nvoid colorDepthImage(cv::Mat& imgDepth,\r\n cv::Mat& imgColoredDepth,\r\n float minRange, float maxRange);\r\n\r\nbool colormap(const std::string& name, unsigned char idx,\r\n float& r, float& g, float& b);\r\n\r\nstd::vector bresLine(int x0, int y0, int x1, int y1);\r\nstd::vector bresCircle(int x0, int y0, int r);\r\n\r\nvoid fitCircle(const std::vector& points,\r\n double& centerX, double& centerY, double& radius);\r\n\r\nstd::vector intersectCircles(double x1, double y1, double r1,\r\n double x2, double y2, double r2);\r\n\r\nvoid LLtoUTM(double latitude, double longitude,\r\n double& utmNorthing, double& utmEasting,\r\n std::string& utmZone);\r\nvoid UTMtoLL(double utmNorthing, double utmEasting,\r\n const std::string& utmZone,\r\n double& latitude, double& longitude);\r\n\r\nlong int timestampDiff(uint64_t t1, uint64_t t2);\r\n\r\n}\r\n\r\n#endif\r\n" }, { "path": "camera_model/include/camodocal/sparse_graph/Transform.h", "content": "#ifndef TRANSFORM_H\n#define TRANSFORM_H\n\n#include \n#include \n#include \n\nnamespace camodocal\n{\n\nclass Transform\n{\npublic:\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n\n Transform();\n Transform(const Eigen::Matrix4d& H);\n\n Eigen::Quaterniond& rotation(void);\n const Eigen::Quaterniond& rotation(void) const;\n double* rotationData(void);\n const double* const rotationData(void) const;\n\n Eigen::Vector3d& translation(void);\n const Eigen::Vector3d& translation(void) const;\n double* translationData(void);\n const double* const translationData(void) const;\n\n Eigen::Matrix4d toMatrix(void) const;\n\nprivate:\n Eigen::Quaterniond m_q;\n Eigen::Vector3d m_t;\n};\n\n}\n\n#endif\n" }, { "path": "camera_model/instruction", "content": "rosrun camera_model Calibration -w 8 -h 11 -s 70 -i ~/bag/PX/calib/\n" }, { "path": "camera_model/package.xml", "content": "\n\n camera_model\n 0.0.0\n The camera_model package\n\n \n \n \n dvorak\n\n\n \n \n \n TODO\n\n\n \n \n \n \n\n\n \n \n \n \n\n\n \n \n \n \n \n \n \n \n \n \n \n catkin\n roscpp\n std_msgs\n roscpp\n std_msgs\n\n\n \n \n \n\n \n" }, { "path": "camera_model/readme.md", "content": "part of [camodocal](https://github.com/hengli/camodocal)\n\n[Google Ceres](http://ceres-solver.org) is needed.\n\n# Calibration:\n\nUse [intrinsic_calib.cc](https://github.com/dvorak0/camera_model/blob/master/src/intrinsic_calib.cc) to calibrate your camera.\n\n# Undistortion:\n\nSee [Camera.h](https://github.com/dvorak0/camera_model/blob/master/include/camodocal/camera_models/Camera.h) for general interface: \n\n - liftProjective: Lift points from the image plane to the projective space.\n - spaceToPlane: Projects 3D points to the image plane (Pi function)\n\n" }, { "path": "camera_model/src/calib/CameraCalibration.cc", "content": "#include \"camodocal/calib/CameraCalibration.h\"\n\n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n\n#include \"camodocal/camera_models/CameraFactory.h\"\n#include \"camodocal/sparse_graph/Transform.h\"\n#include \"camodocal/gpl/EigenQuaternionParameterization.h\"\n#include \"camodocal/gpl/EigenUtils.h\"\n#include \"camodocal/camera_models/CostFunctionFactory.h\"\n\n#include \"ceres/ceres.h\"\nnamespace camodocal\n{\n\nCameraCalibration::CameraCalibration()\n : m_boardSize(cv::Size(0,0))\n , m_squareSize(0.0f)\n , m_verbose(false)\n{\n\n}\n\nCameraCalibration::CameraCalibration(const Camera::ModelType modelType,\n const std::string& cameraName,\n const cv::Size& imageSize,\n const cv::Size& boardSize,\n float squareSize)\n : m_boardSize(boardSize)\n , m_squareSize(squareSize)\n , m_verbose(false)\n{\n m_camera = CameraFactory::instance()->generateCamera(modelType, cameraName, imageSize);\n}\n\nvoid\nCameraCalibration::clear(void)\n{\n m_imagePoints.clear();\n m_scenePoints.clear();\n}\n\nvoid\nCameraCalibration::addChessboardData(const std::vector& corners)\n{\n m_imagePoints.push_back(corners);\n\n std::vector scenePointsInView;\n for (int i = 0; i < m_boardSize.height; ++i)\n {\n for (int j = 0; j < m_boardSize.width; ++j)\n {\n scenePointsInView.push_back(cv::Point3f(i * m_squareSize, j * m_squareSize, 0.0));\n }\n }\n m_scenePoints.push_back(scenePointsInView);\n}\n\nbool\nCameraCalibration::calibrate(void)\n{\n int imageCount = m_imagePoints.size();\n\n // compute intrinsic camera parameters and extrinsic parameters for each of the views\n std::vector rvecs;\n std::vector tvecs;\n bool ret = calibrateHelper(m_camera, rvecs, tvecs);\n\n m_cameraPoses = cv::Mat(imageCount, 6, CV_64F);\n for (int i = 0; i < imageCount; ++i)\n {\n m_cameraPoses.at(i,0) = rvecs.at(i).at(0);\n m_cameraPoses.at(i,1) = rvecs.at(i).at(1);\n m_cameraPoses.at(i,2) = rvecs.at(i).at(2);\n m_cameraPoses.at(i,3) = tvecs.at(i).at(0);\n m_cameraPoses.at(i,4) = tvecs.at(i).at(1);\n m_cameraPoses.at(i,5) = tvecs.at(i).at(2);\n }\n\n // Compute measurement covariance.\n std::vector > errVec(m_imagePoints.size());\n Eigen::Vector2d errSum = Eigen::Vector2d::Zero();\n size_t errCount = 0;\n for (size_t i = 0; i < m_imagePoints.size(); ++i)\n {\n std::vector estImagePoints;\n m_camera->projectPoints(m_scenePoints.at(i), rvecs.at(i), tvecs.at(i),\n estImagePoints);\n\n for (size_t j = 0; j < m_imagePoints.at(i).size(); ++j)\n {\n cv::Point2f pObs = m_imagePoints.at(i).at(j);\n cv::Point2f pEst = estImagePoints.at(j);\n\n cv::Point2f err = pObs - pEst;\n\n errVec.at(i).push_back(err);\n\n errSum += Eigen::Vector2d(err.x, err.y);\n }\n\n errCount += m_imagePoints.at(i).size();\n }\n\n Eigen::Vector2d errMean = errSum / static_cast(errCount);\n\n Eigen::Matrix2d measurementCovariance = Eigen::Matrix2d::Zero();\n for (size_t i = 0; i < errVec.size(); ++i)\n {\n for (size_t j = 0; j < errVec.at(i).size(); ++j)\n {\n cv::Point2f err = errVec.at(i).at(j);\n double d0 = err.x - errMean(0);\n double d1 = err.y - errMean(1);\n\n measurementCovariance(0,0) += d0 * d0;\n measurementCovariance(0,1) += d0 * d1;\n measurementCovariance(1,1) += d1 * d1;\n }\n }\n measurementCovariance /= static_cast(errCount);\n measurementCovariance(1,0) = measurementCovariance(0,1);\n\n m_measurementCovariance = measurementCovariance;\n\n return ret;\n}\n\nint\nCameraCalibration::sampleCount(void) const\n{\n return m_imagePoints.size();\n}\n\nstd::vector >&\nCameraCalibration::imagePoints(void)\n{\n return m_imagePoints;\n}\n\nconst std::vector >&\nCameraCalibration::imagePoints(void) const\n{\n return m_imagePoints;\n}\n\nstd::vector >&\nCameraCalibration::scenePoints(void)\n{\n return m_scenePoints;\n}\n\nconst std::vector >&\nCameraCalibration::scenePoints(void) const\n{\n return m_scenePoints;\n}\n\nCameraPtr&\nCameraCalibration::camera(void)\n{\n return m_camera;\n}\n\nconst CameraConstPtr\nCameraCalibration::camera(void) const\n{\n return m_camera;\n}\n\nEigen::Matrix2d&\nCameraCalibration::measurementCovariance(void)\n{\n return m_measurementCovariance;\n}\n\nconst Eigen::Matrix2d&\nCameraCalibration::measurementCovariance(void) const\n{\n return m_measurementCovariance;\n}\n\ncv::Mat&\nCameraCalibration::cameraPoses(void)\n{\n return m_cameraPoses;\n}\n\nconst cv::Mat&\nCameraCalibration::cameraPoses(void) const\n{\n return m_cameraPoses;\n}\n\nvoid\nCameraCalibration::drawResults(std::vector& images) const\n{\n std::vector rvecs, tvecs;\n\n for (size_t i = 0; i < images.size(); ++i)\n {\n cv::Mat rvec(3, 1, CV_64F);\n rvec.at(0) = m_cameraPoses.at(i,0);\n rvec.at(1) = m_cameraPoses.at(i,1);\n rvec.at(2) = m_cameraPoses.at(i,2);\n\n cv::Mat tvec(3, 1, CV_64F);\n tvec.at(0) = m_cameraPoses.at(i,3);\n tvec.at(1) = m_cameraPoses.at(i,4);\n tvec.at(2) = m_cameraPoses.at(i,5);\n\n rvecs.push_back(rvec);\n tvecs.push_back(tvec);\n }\n\n int drawShiftBits = 4;\n int drawMultiplier = 1 << drawShiftBits;\n\n cv::Scalar green(0, 255, 0);\n cv::Scalar red(0, 0, 255);\n\n for (size_t i = 0; i < images.size(); ++i)\n {\n cv::Mat& image = images.at(i);\n if (image.channels() == 1)\n {\n cv::cvtColor(image, image, cv::COLOR_GRAY2RGB);\n }\n\n std::vector estImagePoints;\n m_camera->projectPoints(m_scenePoints.at(i), rvecs.at(i), tvecs.at(i),\n estImagePoints);\n\n float errorSum = 0.0f;\n float errorMax = std::numeric_limits::min();\n\n for (size_t j = 0; j < m_imagePoints.at(i).size(); ++j)\n {\n cv::Point2f pObs = m_imagePoints.at(i).at(j);\n cv::Point2f pEst = estImagePoints.at(j);\n\n cv::circle(image,\n cv::Point(cvRound(pObs.x * drawMultiplier),\n cvRound(pObs.y * drawMultiplier)),\n 5, green, 2, cv::LINE_AA, drawShiftBits);\n\n cv::circle(image,\n cv::Point(cvRound(pEst.x * drawMultiplier),\n cvRound(pEst.y * drawMultiplier)),\n 5, red, 2, cv::LINE_AA, drawShiftBits);\n\n float error = cv::norm(pObs - pEst);\n\n errorSum += error;\n if (error > errorMax)\n {\n errorMax = error;\n }\n }\n\n std::ostringstream oss;\n oss << \"Reprojection error: avg = \" << errorSum / m_imagePoints.at(i).size()\n << \" max = \" << errorMax;\n\n cv::putText(image, oss.str(), cv::Point(10, image.rows - 10),\n cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(255, 255, 255),\n 1, cv::LINE_AA);\n }\n}\n\nvoid\nCameraCalibration::writeParams(const std::string& filename) const\n{\n m_camera->writeParametersToYamlFile(filename);\n}\n\nbool\nCameraCalibration::writeChessboardData(const std::string& filename) const\n{\n std::ofstream ofs(filename.c_str(), std::ios::out | std::ios::binary);\n if (!ofs.is_open())\n {\n return false;\n }\n\n writeData(ofs, m_boardSize.width);\n writeData(ofs, m_boardSize.height);\n writeData(ofs, m_squareSize);\n\n writeData(ofs, m_measurementCovariance(0,0));\n writeData(ofs, m_measurementCovariance(0,1));\n writeData(ofs, m_measurementCovariance(1,0));\n writeData(ofs, m_measurementCovariance(1,1));\n\n writeData(ofs, m_cameraPoses.rows);\n writeData(ofs, m_cameraPoses.cols);\n writeData(ofs, m_cameraPoses.type());\n for (int i = 0; i < m_cameraPoses.rows; ++i)\n {\n for (int j = 0; j < m_cameraPoses.cols; ++j)\n {\n writeData(ofs, m_cameraPoses.at(i,j));\n }\n }\n\n writeData(ofs, m_imagePoints.size());\n for (size_t i = 0; i < m_imagePoints.size(); ++i)\n {\n writeData(ofs, m_imagePoints.at(i).size());\n for (size_t j = 0; j < m_imagePoints.at(i).size(); ++j)\n {\n const cv::Point2f& ipt = m_imagePoints.at(i).at(j);\n\n writeData(ofs, ipt.x);\n writeData(ofs, ipt.y);\n }\n }\n\n writeData(ofs, m_scenePoints.size());\n for (size_t i = 0; i < m_scenePoints.size(); ++i)\n {\n writeData(ofs, m_scenePoints.at(i).size());\n for (size_t j = 0; j < m_scenePoints.at(i).size(); ++j)\n {\n const cv::Point3f& spt = m_scenePoints.at(i).at(j);\n\n writeData(ofs, spt.x);\n writeData(ofs, spt.y);\n writeData(ofs, spt.z);\n }\n }\n\n return true;\n}\n\nbool\nCameraCalibration::readChessboardData(const std::string& filename)\n{\n std::ifstream ifs(filename.c_str(), std::ios::in | std::ios::binary);\n if (!ifs.is_open())\n {\n return false;\n }\n\n readData(ifs, m_boardSize.width);\n readData(ifs, m_boardSize.height);\n readData(ifs, m_squareSize);\n\n readData(ifs, m_measurementCovariance(0,0));\n readData(ifs, m_measurementCovariance(0,1));\n readData(ifs, m_measurementCovariance(1,0));\n readData(ifs, m_measurementCovariance(1,1));\n\n int rows, cols, type;\n readData(ifs, rows);\n readData(ifs, cols);\n readData(ifs, type);\n m_cameraPoses = cv::Mat(rows, cols, type);\n\n for (int i = 0; i < m_cameraPoses.rows; ++i)\n {\n for (int j = 0; j < m_cameraPoses.cols; ++j)\n {\n readData(ifs, m_cameraPoses.at(i,j));\n }\n }\n\n size_t nImagePointSets;\n readData(ifs, nImagePointSets);\n\n m_imagePoints.clear();\n m_imagePoints.resize(nImagePointSets);\n for (size_t i = 0; i < m_imagePoints.size(); ++i)\n {\n size_t nImagePoints;\n readData(ifs, nImagePoints);\n m_imagePoints.at(i).resize(nImagePoints);\n\n for (size_t j = 0; j < m_imagePoints.at(i).size(); ++j)\n {\n cv::Point2f& ipt = m_imagePoints.at(i).at(j);\n readData(ifs, ipt.x);\n readData(ifs, ipt.y);\n }\n }\n\n size_t nScenePointSets;\n readData(ifs, nScenePointSets);\n\n m_scenePoints.clear();\n m_scenePoints.resize(nScenePointSets);\n for (size_t i = 0; i < m_scenePoints.size(); ++i)\n {\n size_t nScenePoints;\n readData(ifs, nScenePoints);\n m_scenePoints.at(i).resize(nScenePoints);\n\n for (size_t j = 0; j < m_scenePoints.at(i).size(); ++j)\n {\n cv::Point3f& spt = m_scenePoints.at(i).at(j);\n readData(ifs, spt.x);\n readData(ifs, spt.y);\n readData(ifs, spt.z);\n }\n }\n\n return true;\n}\n\nvoid\nCameraCalibration::setVerbose(bool verbose)\n{\n m_verbose = verbose;\n}\n\nbool\nCameraCalibration::calibrateHelper(CameraPtr& camera,\n std::vector& rvecs, std::vector& tvecs) const\n{\n rvecs.assign(m_scenePoints.size(), cv::Mat());\n tvecs.assign(m_scenePoints.size(), cv::Mat());\n\n // STEP 1: Estimate intrinsics\n camera->estimateIntrinsics(m_boardSize, m_scenePoints, m_imagePoints);\n\n // STEP 2: Estimate extrinsics\n for (size_t i = 0; i < m_scenePoints.size(); ++i)\n {\n camera->estimateExtrinsics(m_scenePoints.at(i), m_imagePoints.at(i), rvecs.at(i), tvecs.at(i));\n }\n\n if (m_verbose)\n {\n std::cout << \"[\" << camera->cameraName() << \"] \"\n << \"# INFO: \" << \"Initial reprojection error: \"\n << std::fixed << std::setprecision(3)\n << camera->reprojectionError(m_scenePoints, m_imagePoints, rvecs, tvecs)\n << \" pixels\" << std::endl;\n }\n\n // STEP 3: optimization using ceres\n optimize(camera, rvecs, tvecs);\n\n if (m_verbose)\n {\n double err = camera->reprojectionError(m_scenePoints, m_imagePoints, rvecs, tvecs);\n std::cout << \"[\" << camera->cameraName() << \"] \" << \"# INFO: Final reprojection error: \"\n << err << \" pixels\" << std::endl;\n std::cout << \"[\" << camera->cameraName() << \"] \" << \"# INFO: \"\n << camera->parametersToString() << std::endl;\n }\n\n return true;\n}\n\nvoid\nCameraCalibration::optimize(CameraPtr& camera,\n std::vector& rvecs, std::vector& tvecs) const\n{\n // Use ceres to do optimization\n ceres::Problem problem;\n\n std::vector > transformVec(rvecs.size());\n for (size_t i = 0; i < rvecs.size(); ++i)\n {\n Eigen::Vector3d rvec;\n cv::cv2eigen(rvecs.at(i), rvec);\n\n transformVec.at(i).rotation() = Eigen::AngleAxisd(rvec.norm(), rvec.normalized());\n transformVec.at(i).translation() << tvecs[i].at(0),\n tvecs[i].at(1),\n tvecs[i].at(2);\n }\n\n std::vector intrinsicCameraParams;\n m_camera->writeParameters(intrinsicCameraParams);\n\n // create residuals for each observation\n for (size_t i = 0; i < m_imagePoints.size(); ++i)\n {\n for (size_t j = 0; j < m_imagePoints.at(i).size(); ++j)\n {\n const cv::Point3f& spt = m_scenePoints.at(i).at(j);\n const cv::Point2f& ipt = m_imagePoints.at(i).at(j);\n\n ceres::CostFunction* costFunction =\n CostFunctionFactory::instance()->generateCostFunction(camera,\n Eigen::Vector3d(spt.x, spt.y, spt.z),\n Eigen::Vector2d(ipt.x, ipt.y),\n CAMERA_INTRINSICS | CAMERA_POSE);\n\n ceres::LossFunction* lossFunction = new ceres::CauchyLoss(1.0);\n problem.AddResidualBlock(costFunction, lossFunction,\n intrinsicCameraParams.data(),\n transformVec.at(i).rotationData(),\n transformVec.at(i).translationData());\n }\n\n ceres::LocalParameterization* quaternionParameterization =\n new EigenQuaternionParameterization;\n\n problem.SetParameterization(transformVec.at(i).rotationData(),\n quaternionParameterization);\n }\n\n std::cout << \"begin ceres\" << std::endl;\n ceres::Solver::Options options;\n options.max_num_iterations = 1000;\n options.num_threads = 1;\n\n if (m_verbose)\n {\n options.minimizer_progress_to_stdout = true;\n }\n\n ceres::Solver::Summary summary;\n ceres::Solve(options, &problem, &summary);\n std::cout << \"end ceres\" << std::endl;\n\n if (m_verbose)\n {\n std::cout << summary.FullReport() << std::endl;\n }\n\n camera->readParameters(intrinsicCameraParams);\n\n for (size_t i = 0; i < rvecs.size(); ++i)\n {\n Eigen::AngleAxisd aa(transformVec.at(i).rotation());\n\n Eigen::Vector3d rvec = aa.angle() * aa.axis();\n cv::eigen2cv(rvec, rvecs.at(i));\n\n cv::Mat& tvec = tvecs.at(i);\n tvec.at(0) = transformVec.at(i).translation()(0);\n tvec.at(1) = transformVec.at(i).translation()(1);\n tvec.at(2) = transformVec.at(i).translation()(2);\n }\n}\n\ntemplate\nvoid\nCameraCalibration::readData(std::ifstream& ifs, T& data) const\n{\n char* buffer = new char[sizeof(T)];\n\n ifs.read(buffer, sizeof(T));\n\n data = *(reinterpret_cast(buffer));\n\n delete[] buffer;\n}\n\ntemplate\nvoid\nCameraCalibration::writeData(std::ofstream& ofs, T data) const\n{\n char* pData = reinterpret_cast(&data);\n\n ofs.write(pData, sizeof(T));\n}\n\n}\n" }, { "path": "camera_model/src/camera_models/Camera.cc", "content": "#include \"camodocal/camera_models/Camera.h\"\n#include \"camodocal/camera_models/ScaramuzzaCamera.h\"\n\n#include \n\nnamespace camodocal\n{\n\nCamera::Parameters::Parameters(ModelType modelType)\n : m_modelType(modelType)\n , m_imageWidth(0)\n , m_imageHeight(0)\n{\n switch (modelType)\n {\n case KANNALA_BRANDT:\n m_nIntrinsics = 8;\n break;\n case PINHOLE:\n m_nIntrinsics = 8;\n break;\n case SCARAMUZZA:\n m_nIntrinsics = SCARAMUZZA_CAMERA_NUM_PARAMS;\n break;\n case MEI:\n default:\n m_nIntrinsics = 9;\n }\n}\n\nCamera::Parameters::Parameters(ModelType modelType,\n const std::string& cameraName,\n int w, int h)\n : m_modelType(modelType)\n , m_cameraName(cameraName)\n , m_imageWidth(w)\n , m_imageHeight(h)\n{\n switch (modelType)\n {\n case KANNALA_BRANDT:\n m_nIntrinsics = 8;\n break;\n case PINHOLE:\n m_nIntrinsics = 8;\n break;\n case SCARAMUZZA:\n m_nIntrinsics = SCARAMUZZA_CAMERA_NUM_PARAMS;\n break;\n case MEI:\n default:\n m_nIntrinsics = 9;\n }\n}\n\nCamera::ModelType&\nCamera::Parameters::modelType(void)\n{\n return m_modelType;\n}\n\nstd::string&\nCamera::Parameters::cameraName(void)\n{\n return m_cameraName;\n}\n\nint&\nCamera::Parameters::imageWidth(void)\n{\n return m_imageWidth;\n}\n\nint&\nCamera::Parameters::imageHeight(void)\n{\n return m_imageHeight;\n}\n\nCamera::ModelType\nCamera::Parameters::modelType(void) const\n{\n return m_modelType;\n}\n\nconst std::string&\nCamera::Parameters::cameraName(void) const\n{\n return m_cameraName;\n}\n\nint\nCamera::Parameters::imageWidth(void) const\n{\n return m_imageWidth;\n}\n\nint\nCamera::Parameters::imageHeight(void) const\n{\n return m_imageHeight;\n}\n\nint\nCamera::Parameters::nIntrinsics(void) const\n{\n return m_nIntrinsics;\n}\n\ncv::Mat&\nCamera::mask(void)\n{\n return m_mask;\n}\n\nconst cv::Mat&\nCamera::mask(void) const\n{\n return m_mask;\n}\n\nvoid\nCamera::estimateExtrinsics(const std::vector& objectPoints,\n const std::vector& imagePoints,\n cv::Mat& rvec, cv::Mat& tvec) const\n{\n std::vector Ms(imagePoints.size());\n for (size_t i = 0; i < Ms.size(); ++i)\n {\n Eigen::Vector3d P;\n liftProjective(Eigen::Vector2d(imagePoints.at(i).x, imagePoints.at(i).y), P);\n\n P /= P(2);\n\n Ms.at(i).x = P(0);\n Ms.at(i).y = P(1);\n }\n\n // assume unit focal length, zero principal point, and zero distortion\n cv::solvePnP(objectPoints, Ms, cv::Mat::eye(3, 3, CV_64F), cv::noArray(), rvec, tvec);\n}\n\ndouble\nCamera::reprojectionDist(const Eigen::Vector3d& P1, const Eigen::Vector3d& P2) const\n{\n Eigen::Vector2d p1, p2;\n\n spaceToPlane(P1, p1);\n spaceToPlane(P2, p2);\n\n return (p1 - p2).norm();\n}\n\ndouble\nCamera::reprojectionError(const std::vector< std::vector >& objectPoints,\n const std::vector< std::vector >& imagePoints,\n const std::vector& rvecs,\n const std::vector& tvecs,\n cv::OutputArray _perViewErrors) const\n{\n int imageCount = objectPoints.size();\n size_t pointsSoFar = 0;\n double totalErr = 0.0;\n\n bool computePerViewErrors = _perViewErrors.needed();\n cv::Mat perViewErrors;\n if (computePerViewErrors)\n {\n _perViewErrors.create(imageCount, 1, CV_64F);\n perViewErrors = _perViewErrors.getMat();\n }\n\n for (int i = 0; i < imageCount; ++i)\n {\n size_t pointCount = imagePoints.at(i).size();\n\n pointsSoFar += pointCount;\n\n std::vector estImagePoints;\n projectPoints(objectPoints.at(i), rvecs.at(i), tvecs.at(i),\n estImagePoints);\n\n double err = 0.0;\n for (size_t j = 0; j < imagePoints.at(i).size(); ++j)\n {\n err += cv::norm(imagePoints.at(i).at(j) - estImagePoints.at(j));\n }\n\n if (computePerViewErrors)\n {\n perViewErrors.at(i) = err / pointCount;\n }\n\n totalErr += err;\n }\n\n return totalErr / pointsSoFar;\n}\n\ndouble\nCamera::reprojectionError(const Eigen::Vector3d& P,\n const Eigen::Quaterniond& camera_q,\n const Eigen::Vector3d& camera_t,\n const Eigen::Vector2d& observed_p) const\n{\n Eigen::Vector3d P_cam = camera_q.toRotationMatrix() * P + camera_t;\n\n Eigen::Vector2d p;\n spaceToPlane(P_cam, p);\n\n return (p - observed_p).norm();\n}\n\nvoid\nCamera::projectPoints(const std::vector& objectPoints,\n const cv::Mat& rvec,\n const cv::Mat& tvec,\n std::vector& imagePoints) const\n{\n // project 3D object points to the image plane\n imagePoints.reserve(objectPoints.size());\n\n //double\n cv::Mat R0;\n cv::Rodrigues(rvec, R0);\n\n Eigen::MatrixXd R(3,3);\n R << R0.at(0,0), R0.at(0,1), R0.at(0,2),\n R0.at(1,0), R0.at(1,1), R0.at(1,2),\n R0.at(2,0), R0.at(2,1), R0.at(2,2);\n\n Eigen::Vector3d t;\n t << tvec.at(0), tvec.at(1), tvec.at(2);\n\n for (size_t i = 0; i < objectPoints.size(); ++i)\n {\n const cv::Point3f& objectPoint = objectPoints.at(i);\n\n // Rotate and translate\n Eigen::Vector3d P;\n P << objectPoint.x, objectPoint.y, objectPoint.z;\n\n P = R * P + t;\n\n Eigen::Vector2d p;\n spaceToPlane(P, p);\n\n imagePoints.push_back(cv::Point2f(p(0), p(1)));\n }\n}\n\n}\n" }, { "path": "camera_model/src/camera_models/CameraFactory.cc", "content": "#include \"camodocal/camera_models/CameraFactory.h\"\n\n#include \n\n\n#include \"camodocal/camera_models/CataCamera.h\"\n#include \"camodocal/camera_models/EquidistantCamera.h\"\n#include \"camodocal/camera_models/PinholeCamera.h\"\n#include \"camodocal/camera_models/ScaramuzzaCamera.h\"\n\n#include \"ceres/ceres.h\"\n\nnamespace camodocal\n{\n\nboost::shared_ptr CameraFactory::m_instance;\n\nCameraFactory::CameraFactory()\n{\n\n}\n\nboost::shared_ptr\nCameraFactory::instance(void)\n{\n if (m_instance.get() == 0)\n {\n m_instance.reset(new CameraFactory);\n }\n\n return m_instance;\n}\n\nCameraPtr\nCameraFactory::generateCamera(Camera::ModelType modelType,\n const std::string& cameraName,\n cv::Size imageSize) const\n{\n switch (modelType)\n {\n case Camera::KANNALA_BRANDT:\n {\n EquidistantCameraPtr camera(new EquidistantCamera);\n\n EquidistantCamera::Parameters params = camera->getParameters();\n params.cameraName() = cameraName;\n params.imageWidth() = imageSize.width;\n params.imageHeight() = imageSize.height;\n camera->setParameters(params);\n return camera;\n }\n case Camera::PINHOLE:\n {\n PinholeCameraPtr camera(new PinholeCamera);\n\n PinholeCamera::Parameters params = camera->getParameters();\n params.cameraName() = cameraName;\n params.imageWidth() = imageSize.width;\n params.imageHeight() = imageSize.height;\n camera->setParameters(params);\n return camera;\n }\n case Camera::SCARAMUZZA:\n {\n OCAMCameraPtr camera(new OCAMCamera);\n\n OCAMCamera::Parameters params = camera->getParameters();\n params.cameraName() = cameraName;\n params.imageWidth() = imageSize.width;\n params.imageHeight() = imageSize.height;\n camera->setParameters(params);\n return camera;\n }\n case Camera::MEI:\n default:\n {\n CataCameraPtr camera(new CataCamera);\n\n CataCamera::Parameters params = camera->getParameters();\n params.cameraName() = cameraName;\n params.imageWidth() = imageSize.width;\n params.imageHeight() = imageSize.height;\n camera->setParameters(params);\n return camera;\n }\n }\n}\n\nCameraPtr\nCameraFactory::generateCameraFromYamlFile(const std::string& filename)\n{\n cv::FileStorage fs(filename, cv::FileStorage::READ);\n\n if (!fs.isOpened())\n {\n return CameraPtr();\n }\n\n Camera::ModelType modelType = Camera::MEI;\n if (!fs[\"model_type\"].isNone())\n {\n std::string sModelType;\n fs[\"model_type\"] >> sModelType;\n\n if (boost::iequals(sModelType, \"kannala_brandt\"))\n {\n modelType = Camera::KANNALA_BRANDT;\n }\n else if (boost::iequals(sModelType, \"mei\"))\n {\n modelType = Camera::MEI;\n }\n else if (boost::iequals(sModelType, \"scaramuzza\"))\n {\n modelType = Camera::SCARAMUZZA;\n }\n else if (boost::iequals(sModelType, \"pinhole\"))\n {\n modelType = Camera::PINHOLE;\n }\n else\n {\n std::cerr << \"# ERROR: Unknown camera model: \" << sModelType << std::endl;\n return CameraPtr();\n }\n }\n\n switch (modelType)\n {\n case Camera::KANNALA_BRANDT:\n {\n EquidistantCameraPtr camera(new EquidistantCamera);\n\n EquidistantCamera::Parameters params = camera->getParameters();\n params.readFromYamlFile(filename);\n camera->setParameters(params);\n return camera;\n }\n case Camera::PINHOLE:\n {\n PinholeCameraPtr camera(new PinholeCamera);\n\n PinholeCamera::Parameters params = camera->getParameters();\n params.readFromYamlFile(filename);\n camera->setParameters(params);\n return camera;\n }\n case Camera::SCARAMUZZA:\n {\n OCAMCameraPtr camera(new OCAMCamera);\n\n OCAMCamera::Parameters params = camera->getParameters();\n params.readFromYamlFile(filename);\n camera->setParameters(params);\n return camera;\n }\n case Camera::MEI:\n default:\n {\n CataCameraPtr camera(new CataCamera);\n\n CataCamera::Parameters params = camera->getParameters();\n params.readFromYamlFile(filename);\n camera->setParameters(params);\n return camera;\n }\n }\n\n return CameraPtr();\n}\n\n}\n\n" }, { "path": "camera_model/src/camera_models/CataCamera.cc", "content": "#include \"camodocal/camera_models/CataCamera.h\"\n\n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n\n#include \"camodocal/gpl/gpl.h\"\n\nnamespace camodocal\n{\n\nCataCamera::Parameters::Parameters()\n : Camera::Parameters(MEI)\n , m_xi(0.0)\n , m_k1(0.0)\n , m_k2(0.0)\n , m_p1(0.0)\n , m_p2(0.0)\n , m_gamma1(0.0)\n , m_gamma2(0.0)\n , m_u0(0.0)\n , m_v0(0.0)\n{\n\n}\n\nCataCamera::Parameters::Parameters(const std::string& cameraName,\n int w, int h,\n double xi,\n double k1, double k2,\n double p1, double p2,\n double gamma1, double gamma2,\n double u0, double v0)\n : Camera::Parameters(MEI, cameraName, w, h)\n , m_xi(xi)\n , m_k1(k1)\n , m_k2(k2)\n , m_p1(p1)\n , m_p2(p2)\n , m_gamma1(gamma1)\n , m_gamma2(gamma2)\n , m_u0(u0)\n , m_v0(v0)\n{\n}\n\ndouble&\nCataCamera::Parameters::xi(void)\n{\n return m_xi;\n}\n\ndouble&\nCataCamera::Parameters::k1(void)\n{\n return m_k1;\n}\n\ndouble&\nCataCamera::Parameters::k2(void)\n{\n return m_k2;\n}\n\ndouble&\nCataCamera::Parameters::p1(void)\n{\n return m_p1;\n}\n\ndouble&\nCataCamera::Parameters::p2(void)\n{\n return m_p2;\n}\n\ndouble&\nCataCamera::Parameters::gamma1(void)\n{\n return m_gamma1;\n}\n\ndouble&\nCataCamera::Parameters::gamma2(void)\n{\n return m_gamma2;\n}\n\ndouble&\nCataCamera::Parameters::u0(void)\n{\n return m_u0;\n}\n\ndouble&\nCataCamera::Parameters::v0(void)\n{\n return m_v0;\n}\n\ndouble\nCataCamera::Parameters::xi(void) const\n{\n return m_xi;\n}\n\ndouble\nCataCamera::Parameters::k1(void) const\n{\n return m_k1;\n}\n\ndouble\nCataCamera::Parameters::k2(void) const\n{\n return m_k2;\n}\n\ndouble\nCataCamera::Parameters::p1(void) const\n{\n return m_p1;\n}\n\ndouble\nCataCamera::Parameters::p2(void) const\n{\n return m_p2;\n}\n\ndouble\nCataCamera::Parameters::gamma1(void) const\n{\n return m_gamma1;\n}\n\ndouble\nCataCamera::Parameters::gamma2(void) const\n{\n return m_gamma2;\n}\n\ndouble\nCataCamera::Parameters::u0(void) const\n{\n return m_u0;\n}\n\ndouble\nCataCamera::Parameters::v0(void) const\n{\n return m_v0;\n}\n\nbool\nCataCamera::Parameters::readFromYamlFile(const std::string& filename)\n{\n cv::FileStorage fs(filename, cv::FileStorage::READ);\n\n if (!fs.isOpened())\n {\n return false;\n }\n\n if (!fs[\"model_type\"].isNone())\n {\n std::string sModelType;\n fs[\"model_type\"] >> sModelType;\n\n if (sModelType.compare(\"MEI\") != 0)\n {\n return false;\n }\n }\n\n m_modelType = MEI;\n fs[\"camera_name\"] >> m_cameraName;\n m_imageWidth = static_cast(fs[\"image_width\"]);\n m_imageHeight = static_cast(fs[\"image_height\"]);\n\n cv::FileNode n = fs[\"mirror_parameters\"];\n m_xi = static_cast(n[\"xi\"]);\n\n n = fs[\"distortion_parameters\"];\n m_k1 = static_cast(n[\"k1\"]);\n m_k2 = static_cast(n[\"k2\"]);\n m_p1 = static_cast(n[\"p1\"]);\n m_p2 = static_cast(n[\"p2\"]);\n\n n = fs[\"projection_parameters\"];\n m_gamma1 = static_cast(n[\"gamma1\"]);\n m_gamma2 = static_cast(n[\"gamma2\"]);\n m_u0 = static_cast(n[\"u0\"]);\n m_v0 = static_cast(n[\"v0\"]);\n\n return true;\n}\n\nvoid\nCataCamera::Parameters::writeToYamlFile(const std::string& filename) const\n{\n cv::FileStorage fs(filename, cv::FileStorage::WRITE);\n\n fs << \"model_type\" << \"MEI\";\n fs << \"camera_name\" << m_cameraName;\n fs << \"image_width\" << m_imageWidth;\n fs << \"image_height\" << m_imageHeight;\n\n // mirror: xi\n fs << \"mirror_parameters\";\n fs << \"{\" << \"xi\" << m_xi << \"}\";\n\n // radial distortion: k1, k2\n // tangential distortion: p1, p2\n fs << \"distortion_parameters\";\n fs << \"{\" << \"k1\" << m_k1\n << \"k2\" << m_k2\n << \"p1\" << m_p1\n << \"p2\" << m_p2 << \"}\";\n\n // projection: gamma1, gamma2, u0, v0\n fs << \"projection_parameters\";\n fs << \"{\" << \"gamma1\" << m_gamma1\n << \"gamma2\" << m_gamma2\n << \"u0\" << m_u0\n << \"v0\" << m_v0 << \"}\";\n\n fs.release();\n}\n\nCataCamera::Parameters&\nCataCamera::Parameters::operator=(const CataCamera::Parameters& other)\n{\n if (this != &other)\n {\n m_modelType = other.m_modelType;\n m_cameraName = other.m_cameraName;\n m_imageWidth = other.m_imageWidth;\n m_imageHeight = other.m_imageHeight;\n m_xi = other.m_xi;\n m_k1 = other.m_k1;\n m_k2 = other.m_k2;\n m_p1 = other.m_p1;\n m_p2 = other.m_p2;\n m_gamma1 = other.m_gamma1;\n m_gamma2 = other.m_gamma2;\n m_u0 = other.m_u0;\n m_v0 = other.m_v0;\n }\n\n return *this;\n}\n\nstd::ostream&\noperator<< (std::ostream& out, const CataCamera::Parameters& params)\n{\n out << \"Camera Parameters:\" << std::endl;\n out << \" model_type \" << \"MEI\" << std::endl;\n out << \" camera_name \" << params.m_cameraName << std::endl;\n out << \" image_width \" << params.m_imageWidth << std::endl;\n out << \" image_height \" << params.m_imageHeight << std::endl;\n\n out << \"Mirror Parameters\" << std::endl;\n out << std::fixed << std::setprecision(10);\n out << \" xi \" << params.m_xi << std::endl;\n\n // radial distortion: k1, k2\n // tangential distortion: p1, p2\n out << \"Distortion Parameters\" << std::endl;\n out << \" k1 \" << params.m_k1 << std::endl\n << \" k2 \" << params.m_k2 << std::endl\n << \" p1 \" << params.m_p1 << std::endl\n << \" p2 \" << params.m_p2 << std::endl;\n\n // projection: gamma1, gamma2, u0, v0\n out << \"Projection Parameters\" << std::endl;\n out << \" gamma1 \" << params.m_gamma1 << std::endl\n << \" gamma2 \" << params.m_gamma2 << std::endl\n << \" u0 \" << params.m_u0 << std::endl\n << \" v0 \" << params.m_v0 << std::endl;\n\n return out;\n}\n\nCataCamera::CataCamera()\n : m_inv_K11(1.0)\n , m_inv_K13(0.0)\n , m_inv_K22(1.0)\n , m_inv_K23(0.0)\n , m_noDistortion(true)\n{\n\n}\n\nCataCamera::CataCamera(const std::string& cameraName,\n int imageWidth, int imageHeight,\n double xi, double k1, double k2, double p1, double p2,\n double gamma1, double gamma2, double u0, double v0)\n : mParameters(cameraName, imageWidth, imageHeight,\n xi, k1, k2, p1, p2, gamma1, gamma2, u0, v0)\n{\n if ((mParameters.k1() == 0.0) &&\n (mParameters.k2() == 0.0) &&\n (mParameters.p1() == 0.0) &&\n (mParameters.p2() == 0.0))\n {\n m_noDistortion = true;\n }\n else\n {\n m_noDistortion = false;\n }\n\n // Inverse camera projection matrix parameters\n m_inv_K11 = 1.0 / mParameters.gamma1();\n m_inv_K13 = -mParameters.u0() / mParameters.gamma1();\n m_inv_K22 = 1.0 / mParameters.gamma2();\n m_inv_K23 = -mParameters.v0() / mParameters.gamma2();\n}\n\nCataCamera::CataCamera(const CataCamera::Parameters& params)\n : mParameters(params)\n{\n if ((mParameters.k1() == 0.0) &&\n (mParameters.k2() == 0.0) &&\n (mParameters.p1() == 0.0) &&\n (mParameters.p2() == 0.0))\n {\n m_noDistortion = true;\n }\n else\n {\n m_noDistortion = false;\n }\n\n // Inverse camera projection matrix parameters\n m_inv_K11 = 1.0 / mParameters.gamma1();\n m_inv_K13 = -mParameters.u0() / mParameters.gamma1();\n m_inv_K22 = 1.0 / mParameters.gamma2();\n m_inv_K23 = -mParameters.v0() / mParameters.gamma2();\n}\n\nCamera::ModelType\nCataCamera::modelType(void) const\n{\n return mParameters.modelType();\n}\n\nconst std::string&\nCataCamera::cameraName(void) const\n{\n return mParameters.cameraName();\n}\n\nint\nCataCamera::imageWidth(void) const\n{\n return mParameters.imageWidth();\n}\n\nint\nCataCamera::imageHeight(void) const\n{\n return mParameters.imageHeight();\n}\n\nvoid\nCataCamera::estimateIntrinsics(const cv::Size& boardSize,\n const std::vector< std::vector >& objectPoints,\n const std::vector< std::vector >& imagePoints)\n{\n Parameters params = getParameters();\n\n double u0 = params.imageWidth() / 2.0;\n double v0 = params.imageHeight() / 2.0;\n\n double gamma0 = 0.0;\n double minReprojErr = std::numeric_limits::max();\n\n std::vector rvecs, tvecs;\n rvecs.assign(objectPoints.size(), cv::Mat());\n tvecs.assign(objectPoints.size(), cv::Mat());\n\n params.xi() = 1.0;\n params.k1() = 0.0;\n params.k2() = 0.0;\n params.p1() = 0.0;\n params.p2() = 0.0;\n params.u0() = u0;\n params.v0() = v0;\n\n // Initialize gamma (focal length)\n // Use non-radial line image and xi = 1\n for (size_t i = 0; i < imagePoints.size(); ++i)\n {\n for (int r = 0; r < boardSize.height; ++r)\n {\n cv::Mat P(boardSize.width, 4, CV_64F);\n for (int c = 0; c < boardSize.width; ++c)\n {\n const cv::Point2f& imagePoint = imagePoints.at(i).at(r * boardSize.width + c);\n\n double u = imagePoint.x - u0;\n double v = imagePoint.y - v0;\n\n P.at(c, 0) = u;\n P.at(c, 1) = v;\n P.at(c, 2) = 0.5;\n P.at(c, 3) = -0.5 * (square(u) + square(v));\n }\n\n cv::Mat C;\n cv::SVD::solveZ(P, C);\n\n double t = square(C.at(0)) + square(C.at(1)) + C.at(2) * C.at(3);\n if (t < 0.0)\n {\n continue;\n }\n\n // check that line image is not radial\n double d = sqrt(1.0 / t);\n double nx = C.at(0) * d;\n double ny = C.at(1) * d;\n if (hypot(nx, ny) > 0.95)\n {\n continue;\n }\n\n double gamma = sqrt(C.at(2) / C.at(3));\n\n params.gamma1() = gamma;\n params.gamma2() = gamma;\n setParameters(params);\n\n for (size_t j = 0; j < objectPoints.size(); ++j)\n {\n estimateExtrinsics(objectPoints.at(j), imagePoints.at(j), rvecs.at(j), tvecs.at(j));\n }\n\n double reprojErr = reprojectionError(objectPoints, imagePoints, rvecs, tvecs, cv::noArray());\n\n if (reprojErr < minReprojErr)\n {\n minReprojErr = reprojErr;\n gamma0 = gamma;\n }\n }\n }\n\n if (gamma0 <= 0.0 && minReprojErr >= std::numeric_limits::max())\n {\n std::cout << \"[\" << params.cameraName() << \"] \"\n << \"# INFO: CataCamera model fails with given data. \" << std::endl;\n\n return;\n }\n\n params.gamma1() = gamma0;\n params.gamma2() = gamma0;\n setParameters(params);\n}\n\n/** \n * \\brief Lifts a point from the image plane to the unit sphere\n *\n * \\param p image coordinates\n * \\param P coordinates of the point on the sphere\n */\nvoid\nCataCamera::liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const\n{\n double mx_d, my_d,mx2_d, mxy_d, my2_d, mx_u, my_u;\n double rho2_d, rho4_d, radDist_d, Dx_d, Dy_d, inv_denom_d;\n double lambda;\n\n // Lift points to normalised plane\n mx_d = m_inv_K11 * p(0) + m_inv_K13;\n my_d = m_inv_K22 * p(1) + m_inv_K23;\n\n if (m_noDistortion)\n {\n mx_u = mx_d;\n my_u = my_d;\n }\n else\n {\n // Apply inverse distortion model\n if (0)\n {\n double k1 = mParameters.k1();\n double k2 = mParameters.k2();\n double p1 = mParameters.p1();\n double p2 = mParameters.p2();\n\n // Inverse distortion model\n // proposed by Heikkila\n mx2_d = mx_d*mx_d;\n my2_d = my_d*my_d;\n mxy_d = mx_d*my_d;\n rho2_d = mx2_d+my2_d;\n rho4_d = rho2_d*rho2_d;\n radDist_d = k1*rho2_d+k2*rho4_d;\n Dx_d = mx_d*radDist_d + p2*(rho2_d+2*mx2_d) + 2*p1*mxy_d;\n Dy_d = my_d*radDist_d + p1*(rho2_d+2*my2_d) + 2*p2*mxy_d;\n inv_denom_d = 1/(1+4*k1*rho2_d+6*k2*rho4_d+8*p1*my_d+8*p2*mx_d);\n\n mx_u = mx_d - inv_denom_d*Dx_d;\n my_u = my_d - inv_denom_d*Dy_d;\n }\n else\n {\n // Recursive distortion model\n int n = 6;\n Eigen::Vector2d d_u;\n distortion(Eigen::Vector2d(mx_d, my_d), d_u);\n // Approximate value\n mx_u = mx_d - d_u(0);\n my_u = my_d - d_u(1);\n\n for (int i = 1; i < n; ++i)\n {\n distortion(Eigen::Vector2d(mx_u, my_u), d_u);\n mx_u = mx_d - d_u(0);\n my_u = my_d - d_u(1);\n }\n }\n }\n\n // Lift normalised points to the sphere (inv_hslash)\n double xi = mParameters.xi();\n if (xi == 1.0)\n {\n lambda = 2.0 / (mx_u * mx_u + my_u * my_u + 1.0);\n P << lambda * mx_u, lambda * my_u, lambda - 1.0;\n }\n else\n {\n 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);\n P << lambda * mx_u, lambda * my_u, lambda - xi;\n }\n}\n\n/** \n * \\brief Lifts a point from the image plane to its projective ray\n *\n * \\param p image coordinates\n * \\param P coordinates of the projective ray\n */\nvoid\nCataCamera::liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const\n{\n double mx_d, my_d,mx2_d, mxy_d, my2_d, mx_u, my_u;\n double rho2_d, rho4_d, radDist_d, Dx_d, Dy_d, inv_denom_d;\n //double lambda;\n\n // Lift points to normalised plane\n mx_d = m_inv_K11 * p(0) + m_inv_K13;\n my_d = m_inv_K22 * p(1) + m_inv_K23;\n\n if (m_noDistortion)\n {\n mx_u = mx_d;\n my_u = my_d;\n }\n else\n {\n if (0)\n {\n double k1 = mParameters.k1();\n double k2 = mParameters.k2();\n double p1 = mParameters.p1();\n double p2 = mParameters.p2();\n\n // Apply inverse distortion model\n // proposed by Heikkila\n mx2_d = mx_d*mx_d;\n my2_d = my_d*my_d;\n mxy_d = mx_d*my_d;\n rho2_d = mx2_d+my2_d;\n rho4_d = rho2_d*rho2_d;\n radDist_d = k1*rho2_d+k2*rho4_d;\n Dx_d = mx_d*radDist_d + p2*(rho2_d+2*mx2_d) + 2*p1*mxy_d;\n Dy_d = my_d*radDist_d + p1*(rho2_d+2*my2_d) + 2*p2*mxy_d;\n inv_denom_d = 1/(1+4*k1*rho2_d+6*k2*rho4_d+8*p1*my_d+8*p2*mx_d);\n\n mx_u = mx_d - inv_denom_d*Dx_d;\n my_u = my_d - inv_denom_d*Dy_d;\n }\n else\n {\n // Recursive distortion model\n int n = 8;\n Eigen::Vector2d d_u;\n distortion(Eigen::Vector2d(mx_d, my_d), d_u);\n // Approximate value\n mx_u = mx_d - d_u(0);\n my_u = my_d - d_u(1);\n\n for (int i = 1; i < n; ++i)\n {\n distortion(Eigen::Vector2d(mx_u, my_u), d_u);\n mx_u = mx_d - d_u(0);\n my_u = my_d - d_u(1);\n }\n }\n }\n\n // Obtain a projective ray\n double xi = mParameters.xi();\n if (xi == 1.0)\n {\n P << mx_u, my_u, (1.0 - mx_u * mx_u - my_u * my_u) / 2.0;\n }\n else\n {\n // Reuse variable\n rho2_d = mx_u * mx_u + my_u * my_u;\n P << mx_u, my_u, 1.0 - xi * (rho2_d + 1.0) / (xi + sqrt(1.0 + (1.0 - xi * xi) * rho2_d));\n }\n}\n\n\n/** \n * \\brief Project a 3D point (\\a x,\\a y,\\a z) to the image plane in (\\a u,\\a v)\n *\n * \\param P 3D point coordinates\n * \\param p return value, contains the image point coordinates\n */\nvoid\nCataCamera::spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const\n{\n Eigen::Vector2d p_u, p_d;\n\n // Project points to the normalised plane\n double z = P(2) + mParameters.xi() * P.norm();\n p_u << P(0) / z, P(1) / z;\n\n if (m_noDistortion)\n {\n p_d = p_u;\n }\n else\n {\n // Apply distortion\n Eigen::Vector2d d_u;\n distortion(p_u, d_u);\n p_d = p_u + d_u;\n }\n\n // Apply generalised projection matrix\n p << mParameters.gamma1() * p_d(0) + mParameters.u0(),\n mParameters.gamma2() * p_d(1) + mParameters.v0();\n}\n\n#if 0\n/** \n * \\brief Project a 3D point to the image plane and calculate Jacobian\n *\n * \\param P 3D point coordinates\n * \\param p return value, contains the image point coordinates\n */\nvoid\nCataCamera::spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p,\n Eigen::Matrix& J) const\n{\n double xi = mParameters.xi();\n\n Eigen::Vector2d p_u, p_d;\n double norm, inv_denom;\n double dxdmx, dydmx, dxdmy, dydmy;\n\n norm = P.norm();\n // Project points to the normalised plane\n inv_denom = 1.0 / (P(2) + xi * norm);\n p_u << inv_denom * P(0), inv_denom * P(1);\n\n // Calculate jacobian\n inv_denom = inv_denom * inv_denom / norm;\n double dudx = inv_denom * (norm * P(2) + xi * (P(1) * P(1) + P(2) * P(2)));\n double dvdx = -inv_denom * xi * P(0) * P(1);\n double dudy = dvdx;\n double dvdy = inv_denom * (norm * P(2) + xi * (P(0) * P(0) + P(2) * P(2)));\n inv_denom = inv_denom * (-xi * P(2) - norm); // reuse variable\n double dudz = P(0) * inv_denom;\n double dvdz = P(1) * inv_denom;\n\n if (m_noDistortion)\n {\n p_d = p_u;\n }\n else\n {\n // Apply distortion\n Eigen::Vector2d d_u;\n distortion(p_u, d_u);\n p_d = p_u + d_u;\n }\n\n double gamma1 = mParameters.gamma1();\n double gamma2 = mParameters.gamma2();\n\n // Make the product of the jacobians\n // and add projection matrix jacobian\n inv_denom = gamma1 * (dudx * dxdmx + dvdx * dxdmy); // reuse\n dvdx = gamma2 * (dudx * dydmx + dvdx * dydmy);\n dudx = inv_denom;\n\n inv_denom = gamma1 * (dudy * dxdmx + dvdy * dxdmy); // reuse\n dvdy = gamma2 * (dudy * dydmx + dvdy * dydmy);\n dudy = inv_denom;\n\n inv_denom = gamma1 * (dudz * dxdmx + dvdz * dxdmy); // reuse\n dvdz = gamma2 * (dudz * dydmx + dvdz * dydmy);\n dudz = inv_denom;\n \n // Apply generalised projection matrix\n p << gamma1 * p_d(0) + mParameters.u0(),\n gamma2 * p_d(1) + mParameters.v0();\n\n J << dudx, dudy, dudz,\n dvdx, dvdy, dvdz;\n}\n#endif\n\n/** \n * \\brief Projects an undistorted 2D point p_u to the image plane\n *\n * \\param p_u 2D point coordinates\n * \\return image point coordinates\n */\nvoid\nCataCamera::undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const\n{\n Eigen::Vector2d p_d;\n\n if (m_noDistortion)\n {\n p_d = p_u;\n }\n else\n {\n // Apply distortion\n Eigen::Vector2d d_u;\n distortion(p_u, d_u);\n p_d = p_u + d_u;\n }\n\n // Apply generalised projection matrix\n p << mParameters.gamma1() * p_d(0) + mParameters.u0(),\n mParameters.gamma2() * p_d(1) + mParameters.v0();\n}\n\n/** \n * \\brief Apply distortion to input point (from the normalised plane)\n * \n * \\param p_u undistorted coordinates of point on the normalised plane\n * \\return to obtain the distorted point: p_d = p_u + d_u\n */\nvoid\nCataCamera::distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u) const\n{\n double k1 = mParameters.k1();\n double k2 = mParameters.k2();\n double p1 = mParameters.p1();\n double p2 = mParameters.p2();\n\n double mx2_u, my2_u, mxy_u, rho2_u, rad_dist_u;\n\n mx2_u = p_u(0) * p_u(0);\n my2_u = p_u(1) * p_u(1);\n mxy_u = p_u(0) * p_u(1);\n rho2_u = mx2_u + my2_u;\n rad_dist_u = k1 * rho2_u + k2 * rho2_u * rho2_u;\n d_u << p_u(0) * rad_dist_u + 2.0 * p1 * mxy_u + p2 * (rho2_u + 2.0 * mx2_u),\n p_u(1) * rad_dist_u + 2.0 * p2 * mxy_u + p1 * (rho2_u + 2.0 * my2_u);\n}\n\n/** \n * \\brief Apply distortion to input point (from the normalised plane)\n * and calculate Jacobian\n *\n * \\param p_u undistorted coordinates of point on the normalised plane\n * \\return to obtain the distorted point: p_d = p_u + d_u\n */\nvoid\nCataCamera::distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u,\n Eigen::Matrix2d& J) const\n{\n double k1 = mParameters.k1();\n double k2 = mParameters.k2();\n double p1 = mParameters.p1();\n double p2 = mParameters.p2();\n\n double mx2_u, my2_u, mxy_u, rho2_u, rad_dist_u;\n\n mx2_u = p_u(0) * p_u(0);\n my2_u = p_u(1) * p_u(1);\n mxy_u = p_u(0) * p_u(1);\n rho2_u = mx2_u + my2_u;\n rad_dist_u = k1 * rho2_u + k2 * rho2_u * rho2_u;\n d_u << p_u(0) * rad_dist_u + 2.0 * p1 * mxy_u + p2 * (rho2_u + 2.0 * mx2_u),\n p_u(1) * rad_dist_u + 2.0 * p2 * mxy_u + p1 * (rho2_u + 2.0 * my2_u);\n\n 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);\n 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);\n double dxdmy = dydmx;\n 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);\n\n J << dxdmx, dxdmy,\n dydmx, dydmy;\n}\n\nvoid\nCataCamera::initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale) const\n{\n cv::Size imageSize(mParameters.imageWidth(), mParameters.imageHeight());\n\n cv::Mat mapX = cv::Mat::zeros(imageSize, CV_32F);\n cv::Mat mapY = cv::Mat::zeros(imageSize, CV_32F);\n\n for (int v = 0; v < imageSize.height; ++v)\n {\n for (int u = 0; u < imageSize.width; ++u)\n {\n double mx_u = m_inv_K11 / fScale * u + m_inv_K13 / fScale;\n double my_u = m_inv_K22 / fScale * v + m_inv_K23 / fScale;\n\n double xi = mParameters.xi();\n double d2 = mx_u * mx_u + my_u * my_u;\n\n Eigen::Vector3d P;\n P << mx_u, my_u, 1.0 - xi * (d2 + 1.0) / (xi + sqrt(1.0 + (1.0 - xi * xi) * d2));\n\n Eigen::Vector2d p;\n spaceToPlane(P, p);\n\n mapX.at(v,u) = p(0);\n mapY.at(v,u) = p(1);\n }\n }\n\n cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false);\n}\n\ncv::Mat\nCataCamera::initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2,\n float fx, float fy,\n cv::Size imageSize,\n float cx, float cy,\n cv::Mat rmat) const\n{\n if (imageSize == cv::Size(0, 0))\n {\n imageSize = cv::Size(mParameters.imageWidth(), mParameters.imageHeight());\n }\n\n cv::Mat mapX = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F);\n cv::Mat mapY = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F);\n\n Eigen::Matrix3f K_rect;\n\n if (cx == -1.0f && cy == -1.0f)\n {\n K_rect << fx, 0, imageSize.width / 2,\n 0, fy, imageSize.height / 2,\n 0, 0, 1;\n }\n else\n {\n K_rect << fx, 0, cx,\n 0, fy, cy,\n 0, 0, 1;\n }\n\n if (fx == -1.0f || fy == -1.0f)\n {\n K_rect(0,0) = mParameters.gamma1();\n K_rect(1,1) = mParameters.gamma2();\n }\n\n Eigen::Matrix3f K_rect_inv = K_rect.inverse();\n\n Eigen::Matrix3f R, R_inv;\n cv::cv2eigen(rmat, R);\n R_inv = R.inverse();\n\n for (int v = 0; v < imageSize.height; ++v)\n {\n for (int u = 0; u < imageSize.width; ++u)\n {\n Eigen::Vector3f xo;\n xo << u, v, 1;\n\n Eigen::Vector3f uo = R_inv * K_rect_inv * xo;\n\n Eigen::Vector2d p;\n spaceToPlane(uo.cast(), p);\n\n mapX.at(v,u) = p(0);\n mapY.at(v,u) = p(1);\n }\n }\n\n cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false);\n\n cv::Mat K_rect_cv;\n cv::eigen2cv(K_rect, K_rect_cv);\n return K_rect_cv;\n}\n\nint\nCataCamera::parameterCount(void) const\n{\n return 9;\n}\n\nconst CataCamera::Parameters&\nCataCamera::getParameters(void) const\n{\n return mParameters;\n}\n\nvoid\nCataCamera::setParameters(const CataCamera::Parameters& parameters)\n{\n mParameters = parameters;\n\n if ((mParameters.k1() == 0.0) &&\n (mParameters.k2() == 0.0) &&\n (mParameters.p1() == 0.0) &&\n (mParameters.p2() == 0.0))\n {\n m_noDistortion = true;\n }\n else\n {\n m_noDistortion = false;\n }\n\n m_inv_K11 = 1.0 / mParameters.gamma1();\n m_inv_K13 = -mParameters.u0() / mParameters.gamma1();\n m_inv_K22 = 1.0 / mParameters.gamma2();\n m_inv_K23 = -mParameters.v0() / mParameters.gamma2();\n}\n\nvoid\nCataCamera::readParameters(const std::vector& parameterVec)\n{\n if ((int)parameterVec.size() != parameterCount())\n {\n return;\n }\n\n Parameters params = getParameters();\n\n params.xi() = parameterVec.at(0);\n params.k1() = parameterVec.at(1);\n params.k2() = parameterVec.at(2);\n params.p1() = parameterVec.at(3);\n params.p2() = parameterVec.at(4);\n params.gamma1() = parameterVec.at(5);\n params.gamma2() = parameterVec.at(6);\n params.u0() = parameterVec.at(7);\n params.v0() = parameterVec.at(8);\n\n setParameters(params);\n}\n\nvoid\nCataCamera::writeParameters(std::vector& parameterVec) const\n{\n parameterVec.resize(parameterCount());\n parameterVec.at(0) = mParameters.xi();\n parameterVec.at(1) = mParameters.k1();\n parameterVec.at(2) = mParameters.k2();\n parameterVec.at(3) = mParameters.p1();\n parameterVec.at(4) = mParameters.p2();\n parameterVec.at(5) = mParameters.gamma1();\n parameterVec.at(6) = mParameters.gamma2();\n parameterVec.at(7) = mParameters.u0();\n parameterVec.at(8) = mParameters.v0();\n}\n\nvoid\nCataCamera::writeParametersToYamlFile(const std::string& filename) const\n{\n mParameters.writeToYamlFile(filename);\n}\n\nstd::string\nCataCamera::parametersToString(void) const\n{\n std::ostringstream oss;\n oss << mParameters;\n\n return oss.str();\n}\n\n}\n" }, { "path": "camera_model/src/camera_models/CostFunctionFactory.cc", "content": "#include \"camodocal/camera_models/CostFunctionFactory.h\"\n\n#include \"ceres/ceres.h\"\n#include \"camodocal/camera_models/CataCamera.h\"\n#include \"camodocal/camera_models/EquidistantCamera.h\"\n#include \"camodocal/camera_models/PinholeCamera.h\"\n#include \"camodocal/camera_models/ScaramuzzaCamera.h\"\n\nnamespace camodocal\n{\n\ntemplate\nvoid\nworldToCameraTransform(const T* const q_cam_odo, const T* const t_cam_odo,\n const T* const p_odo, const T* const att_odo,\n T* q, T* t, bool optimize_cam_odo_z = true)\n{\n Eigen::Quaternion q_z_inv(cos(att_odo[0] / T(2)), T(0), T(0), -sin(att_odo[0] / T(2)));\n Eigen::Quaternion q_y_inv(cos(att_odo[1] / T(2)), T(0), -sin(att_odo[1] / T(2)), T(0));\n Eigen::Quaternion q_x_inv(cos(att_odo[2] / T(2)), -sin(att_odo[2] / T(2)), T(0), T(0));\n\n Eigen::Quaternion q_zyx_inv = q_x_inv * q_y_inv * q_z_inv;\n\n T q_odo[4] = {q_zyx_inv.w(), q_zyx_inv.x(), q_zyx_inv.y(), q_zyx_inv.z()};\n\n T q_odo_cam[4] = {q_cam_odo[3], -q_cam_odo[0], -q_cam_odo[1], -q_cam_odo[2]};\n\n T q0[4];\n ceres::QuaternionProduct(q_odo_cam, q_odo, q0);\n\n T t0[3];\n T t_odo[3] = {p_odo[0], p_odo[1], p_odo[2]};\n\n ceres::QuaternionRotatePoint(q_odo, t_odo, t0);\n\n t0[0] += t_cam_odo[0];\n t0[1] += t_cam_odo[1];\n\n if (optimize_cam_odo_z)\n {\n t0[2] += t_cam_odo[2];\n }\n\n ceres::QuaternionRotatePoint(q_odo_cam, t0, t);\n t[0] = -t[0];\n t[1] = -t[1];\n t[2] = -t[2];\n\n // Convert quaternion from Ceres convention (w, x, y, z)\n // to Eigen convention (x, y, z, w)\n q[0] = q0[1];\n q[1] = q0[2];\n q[2] = q0[3];\n q[3] = q0[0];\n}\n\ntemplate\nclass ReprojectionError1\n{\npublic:\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n\n ReprojectionError1(const Eigen::Vector3d& observed_P,\n const Eigen::Vector2d& observed_p)\n : m_observed_P(observed_P), m_observed_p(observed_p)\n , m_sqrtPrecisionMat(Eigen::Matrix2d::Identity()) {}\n\n ReprojectionError1(const Eigen::Vector3d& observed_P,\n const Eigen::Vector2d& observed_p,\n const Eigen::Matrix2d& sqrtPrecisionMat)\n : m_observed_P(observed_P), m_observed_p(observed_p)\n , m_sqrtPrecisionMat(sqrtPrecisionMat) {}\n\n ReprojectionError1(const std::vector& intrinsic_params,\n const Eigen::Vector3d& observed_P,\n const Eigen::Vector2d& observed_p)\n : m_intrinsic_params(intrinsic_params)\n , m_observed_P(observed_P), m_observed_p(observed_p) {}\n\n // variables: camera intrinsics and camera extrinsics\n template \n bool operator()(const T* const intrinsic_params,\n const T* const q,\n const T* const t,\n T* residuals) const\n {\n Eigen::Matrix P = m_observed_P.cast();\n\n Eigen::Matrix predicted_p;\n CameraT::spaceToPlane(intrinsic_params, q, t, P, predicted_p);\n\n Eigen::Matrix e = predicted_p - m_observed_p.cast();\n\n Eigen::Matrix e_weighted = m_sqrtPrecisionMat.cast() * e;\n\n residuals[0] = e_weighted(0);\n residuals[1] = e_weighted(1);\n\n return true;\n }\n\n // variables: camera-odometry transforms and odometry poses\n template \n bool operator()(const T* const q_cam_odo, const T* const t_cam_odo,\n const T* const p_odo, const T* const att_odo,\n T* residuals) const\n {\n T q[4], t[3];\n worldToCameraTransform(q_cam_odo, t_cam_odo, p_odo, att_odo, q, t);\n\n Eigen::Matrix P = m_observed_P.cast();\n\n std::vector intrinsic_params(m_intrinsic_params.begin(), m_intrinsic_params.end());\n\n // project 3D object point to the image plane\n Eigen::Matrix predicted_p;\n CameraT::spaceToPlane(intrinsic_params.data(), q, t, P, predicted_p);\n\n residuals[0] = predicted_p(0) - T(m_observed_p(0));\n residuals[1] = predicted_p(1) - T(m_observed_p(1));\n\n return true;\n }\n\n//private:\n // camera intrinsics\n std::vector m_intrinsic_params;\n\n // observed 3D point\n Eigen::Vector3d m_observed_P;\n\n // observed 2D point\n Eigen::Vector2d m_observed_p;\n\n // square root of precision matrix\n Eigen::Matrix2d m_sqrtPrecisionMat;\n};\n\n// variables: camera extrinsics, 3D point\ntemplate\nclass ReprojectionError2\n{\npublic:\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n\n ReprojectionError2(const std::vector& intrinsic_params,\n const Eigen::Vector2d& observed_p)\n : m_intrinsic_params(intrinsic_params), m_observed_p(observed_p) {}\n\n template \n bool operator()(const T* const q, const T* const t,\n const T* const point, T* residuals) const\n {\n Eigen::Matrix P;\n P(0) = T(point[0]);\n P(1) = T(point[1]);\n P(2) = T(point[2]);\n\n std::vector intrinsic_params(m_intrinsic_params.begin(), m_intrinsic_params.end());\n\n // project 3D object point to the image plane\n Eigen::Matrix predicted_p;\n CameraT::spaceToPlane(intrinsic_params.data(), q, t, P, predicted_p);\n\n residuals[0] = predicted_p(0) - T(m_observed_p(0));\n residuals[1] = predicted_p(1) - T(m_observed_p(1));\n\n return true;\n }\n\nprivate:\n // camera intrinsics\n std::vector m_intrinsic_params;\n\n // observed 2D point\n Eigen::Vector2d m_observed_p;\n};\n\ntemplate\nclass ReprojectionError3\n{\npublic:\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n\n ReprojectionError3(const Eigen::Vector2d& observed_p)\n : m_observed_p(observed_p)\n , m_sqrtPrecisionMat(Eigen::Matrix2d::Identity())\n , m_optimize_cam_odo_z(true) {}\n\n ReprojectionError3(const Eigen::Vector2d& observed_p,\n const Eigen::Matrix2d& sqrtPrecisionMat)\n : m_observed_p(observed_p)\n , m_sqrtPrecisionMat(sqrtPrecisionMat)\n , m_optimize_cam_odo_z(true) {}\n\n ReprojectionError3(const std::vector& intrinsic_params,\n const Eigen::Vector2d& observed_p)\n : m_intrinsic_params(intrinsic_params)\n , m_observed_p(observed_p)\n , m_sqrtPrecisionMat(Eigen::Matrix2d::Identity())\n , m_optimize_cam_odo_z(true) {}\n\n ReprojectionError3(const std::vector& intrinsic_params,\n const Eigen::Vector2d& observed_p,\n const Eigen::Matrix2d& sqrtPrecisionMat)\n : m_intrinsic_params(intrinsic_params)\n , m_observed_p(observed_p)\n , m_sqrtPrecisionMat(sqrtPrecisionMat)\n , m_optimize_cam_odo_z(true) {}\n\n\n ReprojectionError3(const std::vector& intrinsic_params,\n const Eigen::Vector3d& odo_pos,\n const Eigen::Vector3d& odo_att,\n const Eigen::Vector2d& observed_p,\n bool optimize_cam_odo_z)\n : m_intrinsic_params(intrinsic_params)\n , m_odo_pos(odo_pos), m_odo_att(odo_att)\n , m_observed_p(observed_p)\n , m_optimize_cam_odo_z(optimize_cam_odo_z) {}\n\n ReprojectionError3(const std::vector& intrinsic_params,\n const Eigen::Quaterniond& cam_odo_q,\n const Eigen::Vector3d& cam_odo_t,\n const Eigen::Vector3d& odo_pos,\n const Eigen::Vector3d& odo_att,\n const Eigen::Vector2d& observed_p)\n : m_intrinsic_params(intrinsic_params)\n , m_cam_odo_q(cam_odo_q), m_cam_odo_t(cam_odo_t)\n , m_odo_pos(odo_pos), m_odo_att(odo_att)\n , m_observed_p(observed_p)\n , m_optimize_cam_odo_z(true) {}\n\n // variables: camera intrinsics, camera-to-odometry transform,\n // odometry extrinsics, 3D point\n template \n bool operator()(const T* const intrinsic_params,\n const T* const q_cam_odo, const T* const t_cam_odo,\n const T* const p_odo, const T* const att_odo,\n const T* const point, T* residuals) const\n {\n T q[4], t[3];\n worldToCameraTransform(q_cam_odo, t_cam_odo, p_odo, att_odo, q, t, m_optimize_cam_odo_z);\n\n Eigen::Matrix P(point[0], point[1], point[2]);\n\n // project 3D object point to the image plane\n Eigen::Matrix predicted_p;\n CameraT::spaceToPlane(intrinsic_params, q, t, P, predicted_p);\n\n Eigen::Matrix err = predicted_p - m_observed_p.cast();\n Eigen::Matrix err_weighted = m_sqrtPrecisionMat.cast() * err;\n\n residuals[0] = err_weighted(0);\n residuals[1] = err_weighted(1);\n\n return true;\n }\n\n // variables: camera-to-odometry transform, 3D point\n template \n bool operator()(const T* const q_cam_odo, const T* const t_cam_odo,\n const T* const point, T* residuals) const\n {\n T p_odo[3] = {T(m_odo_pos(0)), T(m_odo_pos(1)), T(m_odo_pos(2))};\n T att_odo[3] = {T(m_odo_att(0)), T(m_odo_att(1)), T(m_odo_att(2))};\n T q[4], t[3];\n\n worldToCameraTransform(q_cam_odo, t_cam_odo, p_odo, att_odo, q, t, m_optimize_cam_odo_z);\n\n std::vector intrinsic_params(m_intrinsic_params.begin(), m_intrinsic_params.end());\n Eigen::Matrix P(point[0], point[1], point[2]);\n\n // project 3D object point to the image plane\n Eigen::Matrix predicted_p;\n CameraT::spaceToPlane(intrinsic_params.data(), q, t, P, predicted_p);\n\n residuals[0] = predicted_p(0) - T(m_observed_p(0));\n residuals[1] = predicted_p(1) - T(m_observed_p(1));\n\n return true;\n }\n\n // variables: camera-to-odometry transform, odometry extrinsics, 3D point\n template \n bool operator()(const T* const q_cam_odo, const T* const t_cam_odo,\n const T* const p_odo, const T* const att_odo,\n const T* const point, T* residuals) const\n {\n T q[4], t[3];\n worldToCameraTransform(q_cam_odo, t_cam_odo, p_odo, att_odo, q, t, m_optimize_cam_odo_z);\n\n std::vector intrinsic_params(m_intrinsic_params.begin(), m_intrinsic_params.end());\n Eigen::Matrix P(point[0], point[1], point[2]);\n\n // project 3D object point to the image plane\n Eigen::Matrix predicted_p;\n CameraT::spaceToPlane(intrinsic_params.data(), q, t, P, predicted_p);\n\n Eigen::Matrix err = predicted_p - m_observed_p.cast();\n Eigen::Matrix err_weighted = m_sqrtPrecisionMat.cast() * err;\n\n residuals[0] = err_weighted(0);\n residuals[1] = err_weighted(1);\n\n return true;\n }\n\n // variables: 3D point\n template \n bool operator()(const T* const point, T* residuals) const\n {\n 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))};\n T t_cam_odo[3] = {T(m_cam_odo_t(0)), T(m_cam_odo_t(1)), T(m_cam_odo_t(2))};\n T p_odo[3] = {T(m_odo_pos(0)), T(m_odo_pos(1)), T(m_odo_pos(2))};\n T att_odo[3] = {T(m_odo_att(0)), T(m_odo_att(1)), T(m_odo_att(2))};\n T q[4], t[3];\n\n worldToCameraTransform(q_cam_odo, t_cam_odo, p_odo, att_odo, q, t, m_optimize_cam_odo_z);\n\n std::vector intrinsic_params(m_intrinsic_params.begin(), m_intrinsic_params.end());\n Eigen::Matrix P(point[0], point[1], point[2]);\n\n // project 3D object point to the image plane\n Eigen::Matrix predicted_p;\n CameraT::spaceToPlane(intrinsic_params.data(), q, t, P, predicted_p);\n\n residuals[0] = predicted_p(0) - T(m_observed_p(0));\n residuals[1] = predicted_p(1) - T(m_observed_p(1));\n\n return true;\n }\n\nprivate:\n // camera intrinsics\n std::vector m_intrinsic_params;\n\n // observed camera-odometry transform\n Eigen::Quaterniond m_cam_odo_q;\n Eigen::Vector3d m_cam_odo_t;\n\n // observed odometry\n Eigen::Vector3d m_odo_pos;\n Eigen::Vector3d m_odo_att;\n\n // observed 2D point\n Eigen::Vector2d m_observed_p;\n\n Eigen::Matrix2d m_sqrtPrecisionMat;\n\n bool m_optimize_cam_odo_z;\n};\n\n// variables: camera intrinsics and camera extrinsics\ntemplate\nclass StereoReprojectionError\n{\npublic:\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n\n StereoReprojectionError(const Eigen::Vector3d& observed_P,\n const Eigen::Vector2d& observed_p_l,\n const Eigen::Vector2d& observed_p_r)\n : m_observed_P(observed_P)\n , m_observed_p_l(observed_p_l)\n , m_observed_p_r(observed_p_r)\n {\n\n }\n\n template \n bool operator()(const T* const intrinsic_params_l,\n const T* const intrinsic_params_r,\n const T* const q_l,\n const T* const t_l,\n const T* const q_l_r,\n const T* const t_l_r,\n T* residuals) const\n {\n Eigen::Matrix P;\n P(0) = T(m_observed_P(0));\n P(1) = T(m_observed_P(1));\n P(2) = T(m_observed_P(2));\n\n Eigen::Matrix predicted_p_l;\n CameraT::spaceToPlane(intrinsic_params_l, q_l, t_l, P, predicted_p_l);\n\n Eigen::Quaternion q_r = Eigen::Quaternion(q_l_r) * Eigen::Quaternion(q_l);\n\n Eigen::Matrix t_r;\n t_r(0) = t_l[0];\n t_r(1) = t_l[1];\n t_r(2) = t_l[2];\n\n t_r = Eigen::Quaternion(q_l_r) * t_r;\n t_r(0) += t_l_r[0];\n t_r(1) += t_l_r[1];\n t_r(2) += t_l_r[2];\n\n Eigen::Matrix predicted_p_r;\n CameraT::spaceToPlane(intrinsic_params_r, q_r.coeffs().data(), t_r.data(), P, predicted_p_r);\n\n residuals[0] = predicted_p_l(0) - T(m_observed_p_l(0));\n residuals[1] = predicted_p_l(1) - T(m_observed_p_l(1));\n residuals[2] = predicted_p_r(0) - T(m_observed_p_r(0));\n residuals[3] = predicted_p_r(1) - T(m_observed_p_r(1));\n\n return true;\n }\n\nprivate:\n // observed 3D point\n Eigen::Vector3d m_observed_P;\n\n // observed 2D point\n Eigen::Vector2d m_observed_p_l;\n Eigen::Vector2d m_observed_p_r;\n};\n\ntemplate \nclass ComprehensionError {\n public:\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n\n ComprehensionError(const Eigen::Vector3d& observed_P, const Eigen::Vector2d& observed_p)\n : m_observed_P(observed_P),\n m_observed_p(observed_p),\n m_sqrtPrecisionMat(Eigen::Matrix2d::Identity()) {\n }\n\n template \n bool operator()(const T* const intrinsic_params, const T* const q, const T* const t,\n T* residuals) const {\n {\n Eigen::Matrix p = m_observed_p.cast();\n \n Eigen::Matrix predicted_img_P;\n CameraT::LiftToSphere(intrinsic_params, p, predicted_img_P);\n \n Eigen::Matrix predicted_p;\n CameraT::SphereToPlane(intrinsic_params, predicted_img_P, predicted_p);\n\n Eigen::Matrix e = predicted_p - m_observed_p.cast();\n\n Eigen::Matrix e_weighted = m_sqrtPrecisionMat.cast() * e;\n\n residuals[0] = e_weighted(0);\n residuals[1] = e_weighted(1);\n }\n\n {\n Eigen::Matrix P = m_observed_P.cast();\n\n Eigen::Matrix predicted_p;\n CameraT::spaceToPlane(intrinsic_params, q, t, P, predicted_p);\n\n Eigen::Matrix e = predicted_p - m_observed_p.cast();\n\n Eigen::Matrix e_weighted = m_sqrtPrecisionMat.cast() * e;\n\n residuals[2] = e_weighted(0);\n residuals[3] = e_weighted(1);\n }\n\n return true;\n }\n\n // private:\n // camera intrinsics\n // std::vector m_intrinsic_params;\n\n // observed 3D point\n Eigen::Vector3d m_observed_P;\n\n // observed 2D point\n Eigen::Vector2d m_observed_p;\n\n // square root of precision matrix\n Eigen::Matrix2d m_sqrtPrecisionMat;\n};\n\nboost::shared_ptr CostFunctionFactory::m_instance;\n\nCostFunctionFactory::CostFunctionFactory()\n{\n\n}\n\nboost::shared_ptr\nCostFunctionFactory::instance(void)\n{\n if (m_instance.get() == 0)\n {\n m_instance.reset(new CostFunctionFactory);\n }\n\n return m_instance;\n}\n\nceres::CostFunction*\nCostFunctionFactory::generateCostFunction(const CameraConstPtr& camera,\n const Eigen::Vector3d& observed_P,\n const Eigen::Vector2d& observed_p,\n int flags) const\n{\n ceres::CostFunction* costFunction = 0;\n\n std::vector intrinsic_params;\n camera->writeParameters(intrinsic_params);\n\n switch (flags)\n {\n case CAMERA_INTRINSICS | CAMERA_POSE:\n switch (camera->modelType())\n {\n case Camera::KANNALA_BRANDT:\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 8, 4, 3>(\n new ReprojectionError1(observed_P, observed_p));\n break;\n case Camera::PINHOLE:\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 8, 4, 3>(\n new ReprojectionError1(observed_P, observed_p));\n break;\n case Camera::MEI:\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 9, 4, 3>(\n new ReprojectionError1(observed_P, observed_p));\n break;\n case Camera::SCARAMUZZA:\n costFunction =\n new ceres::AutoDiffCostFunction, 4, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3>(\n new ComprehensionError(observed_P, observed_p));\n // new ceres::AutoDiffCostFunction, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3>(\n // new ReprojectionError1(observed_P, observed_p));\n break;\n }\n break;\n case CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_6D_POSE:\n switch (camera->modelType())\n {\n case Camera::KANNALA_BRANDT:\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 3, 3, 3>(\n new ReprojectionError1(intrinsic_params, observed_P, observed_p));\n break;\n case Camera::PINHOLE:\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 3, 3, 3>(\n new ReprojectionError1(intrinsic_params, observed_P, observed_p));\n break;\n case Camera::MEI:\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 3, 3, 3>(\n new ReprojectionError1(intrinsic_params, observed_P, observed_p));\n break;\n case Camera::SCARAMUZZA:\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 3, 3, 3>(\n new ReprojectionError1(intrinsic_params, observed_P, observed_p));\n break;\n }\n break;\n }\n\n return costFunction;\n}\n\nceres::CostFunction*\nCostFunctionFactory::generateCostFunction(const CameraConstPtr& camera,\n const Eigen::Vector3d& observed_P,\n const Eigen::Vector2d& observed_p,\n const Eigen::Matrix2d& sqrtPrecisionMat,\n int flags) const\n{\n ceres::CostFunction* costFunction = 0;\n\n std::vector intrinsic_params;\n camera->writeParameters(intrinsic_params);\n\n switch (flags)\n {\n case CAMERA_INTRINSICS | CAMERA_POSE:\n switch (camera->modelType())\n {\n case Camera::KANNALA_BRANDT:\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 8, 4, 3>(\n new ReprojectionError1(observed_P, observed_p, sqrtPrecisionMat));\n break;\n case Camera::PINHOLE:\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 8, 4, 3>(\n new ReprojectionError1(observed_P, observed_p, sqrtPrecisionMat));\n break;\n case Camera::MEI:\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 9, 4, 3>(\n new ReprojectionError1(observed_P, observed_p, sqrtPrecisionMat));\n break;\n case Camera::SCARAMUZZA:\n costFunction =\n new ceres::AutoDiffCostFunction, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3>(\n new ReprojectionError1(observed_P, observed_p, sqrtPrecisionMat));\n break;\n }\n break;\n }\n\n return costFunction;\n}\n\nceres::CostFunction*\nCostFunctionFactory::generateCostFunction(const CameraConstPtr& camera,\n const Eigen::Vector2d& observed_p,\n int flags, bool optimize_cam_odo_z) const\n{\n ceres::CostFunction* costFunction = 0;\n\n std::vector intrinsic_params;\n camera->writeParameters(intrinsic_params);\n\n switch (flags)\n {\n case CAMERA_POSE | POINT_3D:\n switch (camera->modelType())\n {\n case Camera::KANNALA_BRANDT:\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 3, 3>(\n new ReprojectionError2(intrinsic_params, observed_p));\n break;\n case Camera::PINHOLE:\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 3, 3>(\n new ReprojectionError2(intrinsic_params, observed_p));\n break;\n case Camera::MEI:\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 3, 3>(\n new ReprojectionError2(intrinsic_params, observed_p));\n break;\n case Camera::SCARAMUZZA:\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 3, 3>(\n new ReprojectionError2(intrinsic_params, observed_p));\n break;\n }\n break;\n case CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_3D_POSE | POINT_3D:\n switch (camera->modelType())\n {\n case Camera::KANNALA_BRANDT:\n if (optimize_cam_odo_z)\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 3, 2, 1, 3>(\n new ReprojectionError3(intrinsic_params, observed_p));\n }\n else\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 2, 2, 1, 3>(\n new ReprojectionError3(intrinsic_params, observed_p));\n }\n break;\n case Camera::PINHOLE:\n if (optimize_cam_odo_z)\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 3, 2, 1, 3>(\n new ReprojectionError3(intrinsic_params, observed_p));\n }\n else\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 2, 2, 1, 3>(\n new ReprojectionError3(intrinsic_params, observed_p));\n }\n break;\n case Camera::MEI:\n if (optimize_cam_odo_z)\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 3, 2, 1, 3>(\n new ReprojectionError3(intrinsic_params, observed_p));\n }\n else\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 2, 2, 1, 3>(\n new ReprojectionError3(intrinsic_params, observed_p));\n }\n break;\n case Camera::SCARAMUZZA:\n if (optimize_cam_odo_z)\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 3, 2, 1, 3>(\n new ReprojectionError3(intrinsic_params, observed_p));\n }\n else\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 2, 2, 1, 3>(\n new ReprojectionError3(intrinsic_params, observed_p));\n }\n break;\n }\n break;\n case CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_6D_POSE | POINT_3D:\n switch (camera->modelType())\n {\n case Camera::KANNALA_BRANDT:\n if (optimize_cam_odo_z)\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 3, 3, 3, 3>(\n new ReprojectionError3(intrinsic_params, observed_p));\n }\n else\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 2, 3, 3, 3>(\n new ReprojectionError3(intrinsic_params, observed_p));\n }\n break;\n case Camera::PINHOLE:\n if (optimize_cam_odo_z)\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 3, 3, 3, 3>(\n new ReprojectionError3(intrinsic_params, observed_p));\n }\n else\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 2, 3, 3, 3>(\n new ReprojectionError3(intrinsic_params, observed_p));\n }\n break;\n case Camera::MEI:\n if (optimize_cam_odo_z)\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 3, 3, 3, 3>(\n new ReprojectionError3(intrinsic_params, observed_p));\n }\n else\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 2, 3, 3, 3>(\n new ReprojectionError3(intrinsic_params, observed_p));\n }\n break;\n case Camera::SCARAMUZZA:\n if (optimize_cam_odo_z)\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 3, 3, 3, 3>(\n new ReprojectionError3(intrinsic_params, observed_p));\n }\n else\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 2, 3, 3, 3>(\n new ReprojectionError3(intrinsic_params, observed_p));\n }\n break;\n }\n break;\n case CAMERA_INTRINSICS | CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_3D_POSE | POINT_3D:\n switch (camera->modelType())\n {\n case Camera::KANNALA_BRANDT:\n if (optimize_cam_odo_z)\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 8, 4, 3, 2, 1, 3>(\n new ReprojectionError3(observed_p));\n }\n else\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 8, 4, 2, 2, 1, 3>(\n new ReprojectionError3(observed_p));\n }\n break;\n case Camera::PINHOLE:\n if (optimize_cam_odo_z)\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 8, 4, 3, 2, 1, 3>(\n new ReprojectionError3(observed_p));\n }\n else\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 8, 4, 2, 2, 1, 3>(\n new ReprojectionError3(observed_p));\n }\n break;\n case Camera::MEI:\n if (optimize_cam_odo_z)\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 9, 4, 3, 2, 1, 3>(\n new ReprojectionError3(observed_p));\n }\n else\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 9, 4, 2, 2, 1, 3>(\n new ReprojectionError3(observed_p));\n }\n break;\n case Camera::SCARAMUZZA:\n if (optimize_cam_odo_z)\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3, 2, 1, 3>(\n new ReprojectionError3(observed_p));\n }\n else\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 2, 2, 1, 3>(\n new ReprojectionError3(observed_p));\n }\n break;\n }\n break;\n case CAMERA_INTRINSICS | CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_6D_POSE | POINT_3D:\n switch (camera->modelType())\n {\n case Camera::KANNALA_BRANDT:\n if (optimize_cam_odo_z)\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 8, 4, 3, 3, 3, 3>(\n new ReprojectionError3(observed_p));\n }\n else\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 8, 4, 2, 3, 3, 3>(\n new ReprojectionError3(observed_p));\n }\n break;\n case Camera::PINHOLE:\n if (optimize_cam_odo_z)\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 8, 4, 3, 3, 3, 3>(\n new ReprojectionError3(observed_p));\n }\n else\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 8, 4, 2, 3, 3, 3>(\n new ReprojectionError3(observed_p));\n }\n break;\n case Camera::MEI:\n if (optimize_cam_odo_z)\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 9, 4, 3, 3, 3, 3>(\n new ReprojectionError3(observed_p));\n }\n else\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 9, 4, 2, 3, 3, 3>(\n new ReprojectionError3(observed_p));\n }\n break;\n case Camera::SCARAMUZZA:\n if (optimize_cam_odo_z)\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3, 3, 3, 3>(\n new ReprojectionError3(observed_p));\n }\n else\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 2, 3, 3, 3>(\n new ReprojectionError3(observed_p));\n }\n break;\n }\n break;\n }\n\n return costFunction;\n}\n\nceres::CostFunction*\nCostFunctionFactory::generateCostFunction(const CameraConstPtr& camera,\n const Eigen::Vector2d& observed_p,\n const Eigen::Matrix2d& sqrtPrecisionMat,\n int flags, bool optimize_cam_odo_z) const\n{\n ceres::CostFunction* costFunction = 0;\n\n std::vector intrinsic_params;\n camera->writeParameters(intrinsic_params);\n\n switch (flags)\n {\n case CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_6D_POSE | POINT_3D:\n switch (camera->modelType())\n {\n case Camera::KANNALA_BRANDT:\n if (optimize_cam_odo_z)\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 3, 3, 3, 3>(\n new ReprojectionError3(intrinsic_params, observed_p, sqrtPrecisionMat));\n }\n else\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 2, 3, 3, 3>(\n new ReprojectionError3(intrinsic_params, observed_p, sqrtPrecisionMat));\n }\n break;\n case Camera::PINHOLE:\n if (optimize_cam_odo_z)\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 3, 3, 3, 3>(\n new ReprojectionError3(intrinsic_params, observed_p, sqrtPrecisionMat));\n }\n else\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 2, 3, 3, 3>(\n new ReprojectionError3(intrinsic_params, observed_p, sqrtPrecisionMat));\n }\n break;\n case Camera::MEI:\n if (optimize_cam_odo_z)\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 3, 3, 3, 3>(\n new ReprojectionError3(intrinsic_params, observed_p, sqrtPrecisionMat));\n }\n else\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 2, 3, 3, 3>(\n new ReprojectionError3(intrinsic_params, observed_p, sqrtPrecisionMat));\n }\n break;\n case Camera::SCARAMUZZA:\n if (optimize_cam_odo_z)\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 3, 3, 3, 3>(\n new ReprojectionError3(intrinsic_params, observed_p, sqrtPrecisionMat));\n }\n else\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 2, 3, 3, 3>(\n new ReprojectionError3(intrinsic_params, observed_p, sqrtPrecisionMat));\n }\n break;\n }\n break;\n case CAMERA_INTRINSICS | CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_6D_POSE | POINT_3D:\n switch (camera->modelType())\n {\n case Camera::KANNALA_BRANDT:\n if (optimize_cam_odo_z)\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 8, 4, 3, 3, 3, 3>(\n new ReprojectionError3(observed_p, sqrtPrecisionMat));\n }\n else\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 8, 4, 2, 3, 3, 3>(\n new ReprojectionError3(observed_p, sqrtPrecisionMat));\n }\n break;\n case Camera::PINHOLE:\n if (optimize_cam_odo_z)\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 8, 4, 3, 3, 3, 3>(\n new ReprojectionError3(observed_p, sqrtPrecisionMat));\n }\n else\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 8, 4, 2, 3, 3, 3>(\n new ReprojectionError3(observed_p, sqrtPrecisionMat));\n }\n break;\n case Camera::MEI:\n if (optimize_cam_odo_z)\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 9, 4, 3, 3, 3, 3>(\n new ReprojectionError3(observed_p, sqrtPrecisionMat));\n }\n else\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 9, 4, 2, 3, 3, 3>(\n new ReprojectionError3(observed_p, sqrtPrecisionMat));\n }\n break;\n case Camera::SCARAMUZZA:\n if (optimize_cam_odo_z)\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3, 3, 3, 3>(\n new ReprojectionError3(observed_p, sqrtPrecisionMat));\n }\n else\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 2, 3, 3, 3>(\n new ReprojectionError3(observed_p, sqrtPrecisionMat));\n }\n break;\n }\n break;\n }\n\n return costFunction;\n}\n\nceres::CostFunction*\nCostFunctionFactory::generateCostFunction(const CameraConstPtr& camera,\n const Eigen::Vector3d& odo_pos,\n const Eigen::Vector3d& odo_att,\n const Eigen::Vector2d& observed_p,\n int flags, bool optimize_cam_odo_z) const\n{\n ceres::CostFunction* costFunction = 0;\n\n std::vector intrinsic_params;\n camera->writeParameters(intrinsic_params);\n\n switch (flags)\n {\n case CAMERA_ODOMETRY_TRANSFORM | POINT_3D:\n switch (camera->modelType())\n {\n case Camera::KANNALA_BRANDT:\n if (optimize_cam_odo_z)\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 3, 3>(\n new ReprojectionError3(intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z));\n }\n else\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 2, 3>(\n new ReprojectionError3(intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z));\n }\n break;\n case Camera::PINHOLE:\n if (optimize_cam_odo_z)\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 3, 3>(\n new ReprojectionError3(intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z));\n }\n else\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 2, 3>(\n new ReprojectionError3(intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z));\n }\n break;\n case Camera::MEI:\n if (optimize_cam_odo_z)\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 3, 3>(\n new ReprojectionError3(intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z));\n }\n else\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 2, 3>(\n new ReprojectionError3(intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z));\n }\n break;\n case Camera::SCARAMUZZA:\n if (optimize_cam_odo_z)\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 3, 3>(\n new ReprojectionError3(intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z));\n }\n else\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 2, 3>(\n new ReprojectionError3(intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z));\n }\n break;\n }\n break;\n }\n\n return costFunction;\n}\n\nceres::CostFunction*\nCostFunctionFactory::generateCostFunction(const CameraConstPtr& camera,\n const Eigen::Quaterniond& cam_odo_q,\n const Eigen::Vector3d& cam_odo_t,\n const Eigen::Vector3d& odo_pos,\n const Eigen::Vector3d& odo_att,\n const Eigen::Vector2d& observed_p,\n int flags) const\n{\n ceres::CostFunction* costFunction = 0;\n\n std::vector intrinsic_params;\n camera->writeParameters(intrinsic_params);\n\n switch (flags)\n {\n case POINT_3D:\n switch (camera->modelType())\n {\n case Camera::KANNALA_BRANDT:\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 3>(\n new ReprojectionError3(intrinsic_params, cam_odo_q, cam_odo_t, odo_pos, odo_att, observed_p));\n break;\n case Camera::PINHOLE:\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 3>(\n new ReprojectionError3(intrinsic_params, cam_odo_q, cam_odo_t, odo_pos, odo_att, observed_p));\n break;\n case Camera::MEI:\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 3>(\n new ReprojectionError3(intrinsic_params, cam_odo_q, cam_odo_t, odo_pos, odo_att, observed_p));\n break;\n case Camera::SCARAMUZZA:\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 3>(\n new ReprojectionError3(intrinsic_params, cam_odo_q, cam_odo_t, odo_pos, odo_att, observed_p));\n break;\n }\n break;\n }\n\n return costFunction;\n}\n\nceres::CostFunction*\nCostFunctionFactory::generateCostFunction(const CameraConstPtr& cameraL,\n const CameraConstPtr& cameraR,\n const Eigen::Vector3d& observed_P,\n const Eigen::Vector2d& observed_p_l,\n const Eigen::Vector2d& observed_p_r) const\n{\n ceres::CostFunction* costFunction = 0;\n\n if (cameraL->modelType() != cameraR->modelType())\n {\n return costFunction;\n }\n\n switch (cameraL->modelType())\n {\n case Camera::KANNALA_BRANDT:\n costFunction =\n new ceres::AutoDiffCostFunction, 4, 8, 8, 4, 3, 4, 3>(\n new StereoReprojectionError(observed_P, observed_p_l, observed_p_r));\n break;\n case Camera::PINHOLE:\n costFunction =\n new ceres::AutoDiffCostFunction, 4, 8, 8, 4, 3, 4, 3>(\n new StereoReprojectionError(observed_P, observed_p_l, observed_p_r));\n break;\n case Camera::MEI:\n costFunction =\n new ceres::AutoDiffCostFunction, 4, 9, 9, 4, 3, 4, 3>(\n new StereoReprojectionError(observed_P, observed_p_l, observed_p_r));\n break;\n case Camera::SCARAMUZZA:\n costFunction =\n new ceres::AutoDiffCostFunction, 4, SCARAMUZZA_CAMERA_NUM_PARAMS, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3, 4, 3>(\n new StereoReprojectionError(observed_P, observed_p_l, observed_p_r));\n break;\n }\n\n return costFunction;\n}\n\n}\n\n" }, { "path": "camera_model/src/camera_models/EquidistantCamera.cc", "content": "#include \"camodocal/camera_models/EquidistantCamera.h\"\n\n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n\n#include \"camodocal/gpl/gpl.h\"\n\nnamespace camodocal\n{\n\nEquidistantCamera::Parameters::Parameters()\n : Camera::Parameters(KANNALA_BRANDT)\n , m_k2(0.0)\n , m_k3(0.0)\n , m_k4(0.0)\n , m_k5(0.0)\n , m_mu(0.0)\n , m_mv(0.0)\n , m_u0(0.0)\n , m_v0(0.0)\n{\n\n}\n\nEquidistantCamera::Parameters::Parameters(const std::string& cameraName,\n int w, int h,\n double k2, double k3, double k4, double k5,\n double mu, double mv,\n double u0, double v0)\n : Camera::Parameters(KANNALA_BRANDT, cameraName, w, h)\n , m_k2(k2)\n , m_k3(k3)\n , m_k4(k4)\n , m_k5(k5)\n , m_mu(mu)\n , m_mv(mv)\n , m_u0(u0)\n , m_v0(v0)\n{\n\n}\n\ndouble&\nEquidistantCamera::Parameters::k2(void)\n{\n return m_k2;\n}\n\ndouble&\nEquidistantCamera::Parameters::k3(void)\n{\n return m_k3;\n}\n\ndouble&\nEquidistantCamera::Parameters::k4(void)\n{\n return m_k4;\n}\n\ndouble&\nEquidistantCamera::Parameters::k5(void)\n{\n return m_k5;\n}\n\ndouble&\nEquidistantCamera::Parameters::mu(void)\n{\n return m_mu;\n}\n\ndouble&\nEquidistantCamera::Parameters::mv(void)\n{\n return m_mv;\n}\n\ndouble&\nEquidistantCamera::Parameters::u0(void)\n{\n return m_u0;\n}\n\ndouble&\nEquidistantCamera::Parameters::v0(void)\n{\n return m_v0;\n}\n\ndouble\nEquidistantCamera::Parameters::k2(void) const\n{\n return m_k2;\n}\n\ndouble\nEquidistantCamera::Parameters::k3(void) const\n{\n return m_k3;\n}\n\ndouble\nEquidistantCamera::Parameters::k4(void) const\n{\n return m_k4;\n}\n\ndouble\nEquidistantCamera::Parameters::k5(void) const\n{\n return m_k5;\n}\n\ndouble\nEquidistantCamera::Parameters::mu(void) const\n{\n return m_mu;\n}\n\ndouble\nEquidistantCamera::Parameters::mv(void) const\n{\n return m_mv;\n}\n\ndouble\nEquidistantCamera::Parameters::u0(void) const\n{\n return m_u0;\n}\n\ndouble\nEquidistantCamera::Parameters::v0(void) const\n{\n return m_v0;\n}\n\nbool\nEquidistantCamera::Parameters::readFromYamlFile(const std::string& filename)\n{\n cv::FileStorage fs(filename, cv::FileStorage::READ);\n\n if (!fs.isOpened())\n {\n return false;\n }\n\n if (!fs[\"model_type\"].isNone())\n {\n std::string sModelType;\n fs[\"model_type\"] >> sModelType;\n\n if (sModelType.compare(\"KANNALA_BRANDT\") != 0)\n {\n return false;\n }\n }\n\n m_modelType = KANNALA_BRANDT;\n fs[\"camera_name\"] >> m_cameraName;\n m_imageWidth = static_cast(fs[\"image_width\"]);\n m_imageHeight = static_cast(fs[\"image_height\"]);\n\n cv::FileNode n = fs[\"projection_parameters\"];\n m_k2 = static_cast(n[\"k2\"]);\n m_k3 = static_cast(n[\"k3\"]);\n m_k4 = static_cast(n[\"k4\"]);\n m_k5 = static_cast(n[\"k5\"]);\n m_mu = static_cast(n[\"mu\"]);\n m_mv = static_cast(n[\"mv\"]);\n m_u0 = static_cast(n[\"u0\"]);\n m_v0 = static_cast(n[\"v0\"]);\n\n return true;\n}\n\nvoid\nEquidistantCamera::Parameters::writeToYamlFile(const std::string& filename) const\n{\n cv::FileStorage fs(filename, cv::FileStorage::WRITE);\n\n fs << \"model_type\" << \"KANNALA_BRANDT\";\n fs << \"camera_name\" << m_cameraName;\n fs << \"image_width\" << m_imageWidth;\n fs << \"image_height\" << m_imageHeight;\n\n // projection: k2, k3, k4, k5, mu, mv, u0, v0\n fs << \"projection_parameters\";\n fs << \"{\" << \"k2\" << m_k2\n << \"k3\" << m_k3\n << \"k4\" << m_k4\n << \"k5\" << m_k5\n << \"mu\" << m_mu\n << \"mv\" << m_mv\n << \"u0\" << m_u0\n << \"v0\" << m_v0 << \"}\";\n\n fs.release();\n}\n\nEquidistantCamera::Parameters&\nEquidistantCamera::Parameters::operator=(const EquidistantCamera::Parameters& other)\n{\n if (this != &other)\n {\n m_modelType = other.m_modelType;\n m_cameraName = other.m_cameraName;\n m_imageWidth = other.m_imageWidth;\n m_imageHeight = other.m_imageHeight;\n m_k2 = other.m_k2;\n m_k3 = other.m_k3;\n m_k4 = other.m_k4;\n m_k5 = other.m_k5;\n m_mu = other.m_mu;\n m_mv = other.m_mv;\n m_u0 = other.m_u0;\n m_v0 = other.m_v0;\n }\n\n return *this;\n}\n\nstd::ostream&\noperator<< (std::ostream& out, const EquidistantCamera::Parameters& params)\n{\n out << \"Camera Parameters:\" << std::endl;\n out << \" model_type \" << \"KANNALA_BRANDT\" << std::endl;\n out << \" camera_name \" << params.m_cameraName << std::endl;\n out << \" image_width \" << params.m_imageWidth << std::endl;\n out << \" image_height \" << params.m_imageHeight << std::endl;\n\n // projection: k2, k3, k4, k5, mu, mv, u0, v0\n out << \"Projection Parameters\" << std::endl;\n out << \" k2 \" << params.m_k2 << std::endl\n << \" k3 \" << params.m_k3 << std::endl\n << \" k4 \" << params.m_k4 << std::endl\n << \" k5 \" << params.m_k5 << std::endl\n << \" mu \" << params.m_mu << std::endl\n << \" mv \" << params.m_mv << std::endl\n << \" u0 \" << params.m_u0 << std::endl\n << \" v0 \" << params.m_v0 << std::endl;\n\n return out;\n}\n\nEquidistantCamera::EquidistantCamera()\n : m_inv_K11(1.0)\n , m_inv_K13(0.0)\n , m_inv_K22(1.0)\n , m_inv_K23(0.0)\n{\n\n}\n\nEquidistantCamera::EquidistantCamera(const std::string& cameraName,\n int imageWidth, int imageHeight,\n double k2, double k3, double k4, double k5,\n double mu, double mv,\n double u0, double v0)\n : mParameters(cameraName, imageWidth, imageHeight,\n k2, k3, k4, k5, mu, mv, u0, v0)\n{\n // Inverse camera projection matrix parameters\n m_inv_K11 = 1.0 / mParameters.mu();\n m_inv_K13 = -mParameters.u0() / mParameters.mu();\n m_inv_K22 = 1.0 / mParameters.mv();\n m_inv_K23 = -mParameters.v0() / mParameters.mv();\n}\n\nEquidistantCamera::EquidistantCamera(const EquidistantCamera::Parameters& params)\n : mParameters(params)\n{\n // Inverse camera projection matrix parameters\n m_inv_K11 = 1.0 / mParameters.mu();\n m_inv_K13 = -mParameters.u0() / mParameters.mu();\n m_inv_K22 = 1.0 / mParameters.mv();\n m_inv_K23 = -mParameters.v0() / mParameters.mv();\n}\n\nCamera::ModelType\nEquidistantCamera::modelType(void) const\n{\n return mParameters.modelType();\n}\n\nconst std::string&\nEquidistantCamera::cameraName(void) const\n{\n return mParameters.cameraName();\n}\n\nint\nEquidistantCamera::imageWidth(void) const\n{\n return mParameters.imageWidth();\n}\n\nint\nEquidistantCamera::imageHeight(void) const\n{\n return mParameters.imageHeight();\n}\n\nvoid\nEquidistantCamera::estimateIntrinsics(const cv::Size& boardSize,\n const std::vector< std::vector >& objectPoints,\n const std::vector< std::vector >& imagePoints)\n{\n Parameters params = getParameters();\n\n double u0 = params.imageWidth() / 2.0;\n double v0 = params.imageHeight() / 2.0;\n\n double minReprojErr = std::numeric_limits::max();\n\n std::vector rvecs, tvecs;\n rvecs.assign(objectPoints.size(), cv::Mat());\n tvecs.assign(objectPoints.size(), cv::Mat());\n\n params.k2() = 0.0;\n params.k3() = 0.0;\n params.k4() = 0.0;\n params.k5() = 0.0;\n params.u0() = u0;\n params.v0() = v0;\n\n // Initialize focal length\n // C. Hughes, P. Denny, M. Glavin, and E. Jones,\n // Equidistant Fish-Eye Calibration and Rectification by Vanishing Point\n // Extraction, PAMI 2010\n // Find circles from rows of chessboard corners, and for each pair\n // of circles, find vanishing points: v1 and v2.\n // f = ||v1 - v2|| / PI;\n double f0 = 0.0;\n for (size_t i = 0; i < imagePoints.size(); ++i)\n {\n std::vector center(boardSize.height);\n double radius[boardSize.height];\n for (int r = 0; r < boardSize.height; ++r)\n {\n std::vector circle;\n for (int c = 0; c < boardSize.width; ++c)\n {\n circle.push_back(imagePoints.at(i).at(r * boardSize.width + c));\n }\n\n fitCircle(circle, center[r](0), center[r](1), radius[r]);\n }\n\n for (int j = 0; j < boardSize.height; ++j)\n {\n for (int k = j + 1; k < boardSize.height; ++k)\n {\n // find distance between pair of vanishing points which\n // correspond to intersection points of 2 circles\n std::vector ipts;\n ipts = intersectCircles(center[j](0), center[j](1), radius[j],\n center[k](0), center[k](1), radius[k]);\n\n if (ipts.size() < 2)\n {\n continue;\n }\n\n double f = cv::norm(ipts.at(0) - ipts.at(1)) / M_PI;\n\n params.mu() = f;\n params.mv() = f;\n\n setParameters(params);\n\n for (size_t l = 0; l < objectPoints.size(); ++l)\n {\n estimateExtrinsics(objectPoints.at(l), imagePoints.at(l), rvecs.at(l), tvecs.at(l));\n }\n\n double reprojErr = reprojectionError(objectPoints, imagePoints, rvecs, tvecs, cv::noArray());\n\n if (reprojErr < minReprojErr)\n {\n minReprojErr = reprojErr;\n f0 = f;\n }\n }\n }\n }\n\n if (f0 <= 0.0 && minReprojErr >= std::numeric_limits::max())\n {\n std::cout << \"[\" << params.cameraName() << \"] \"\n << \"# INFO: kannala-Brandt model fails with given data. \" << std::endl;\n\n return;\n }\n\n params.mu() = f0;\n params.mv() = f0;\n\n setParameters(params);\n}\n\n/**\n * \\brief Lifts a point from the image plane to the unit sphere\n *\n * \\param p image coordinates\n * \\param P coordinates of the point on the sphere\n */\nvoid\nEquidistantCamera::liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const\n{\n liftProjective(p, P);\n}\n\n/** \n * \\brief Lifts a point from the image plane to its projective ray\n *\n * \\param p image coordinates\n * \\param P coordinates of the projective ray\n */\nvoid\nEquidistantCamera::liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const\n{\n // Lift points to normalised plane\n Eigen::Vector2d p_u;\n p_u << m_inv_K11 * p(0) + m_inv_K13,\n m_inv_K22 * p(1) + m_inv_K23;\n\n // Obtain a projective ray\n double theta, phi;\n backprojectSymmetric(p_u, theta, phi);\n\n P(0) = sin(theta) * cos(phi);\n P(1) = sin(theta) * sin(phi);\n P(2) = cos(theta);\n}\n\n/** \n * \\brief Project a 3D point (\\a x,\\a y,\\a z) to the image plane in (\\a u,\\a v)\n *\n * \\param P 3D point coordinates\n * \\param p return value, contains the image point coordinates\n */\nvoid\nEquidistantCamera::spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const\n{\n double theta = acos(P(2) / P.norm());\n double phi = atan2(P(1), P(0));\n\n Eigen::Vector2d p_u = r(mParameters.k2(), mParameters.k3(), mParameters.k4(), mParameters.k5(), theta) * Eigen::Vector2d(cos(phi), sin(phi));\n\n // Apply generalised projection matrix\n p << mParameters.mu() * p_u(0) + mParameters.u0(),\n mParameters.mv() * p_u(1) + mParameters.v0();\n}\n\n\n/** \n * \\brief Project a 3D point to the image plane and calculate Jacobian\n *\n * \\param P 3D point coordinates\n * \\param p return value, contains the image point coordinates\n */\nvoid\nEquidistantCamera::spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p,\n Eigen::Matrix& J) const\n{\n double theta = acos(P(2) / P.norm());\n double phi = atan2(P(1), P(0));\n\n Eigen::Vector2d p_u = r(mParameters.k2(), mParameters.k3(), mParameters.k4(), mParameters.k5(), theta) * Eigen::Vector2d(cos(phi), sin(phi));\n\n // Apply generalised projection matrix\n p << mParameters.mu() * p_u(0) + mParameters.u0(),\n mParameters.mv() * p_u(1) + mParameters.v0();\n}\n\n/** \n * \\brief Projects an undistorted 2D point p_u to the image plane\n *\n * \\param p_u 2D point coordinates\n * \\return image point coordinates\n */\nvoid\nEquidistantCamera::undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const\n{\n// Eigen::Vector2d p_d;\n//\n// if (m_noDistortion)\n// {\n// p_d = p_u;\n// }\n// else\n// {\n// // Apply distortion\n// Eigen::Vector2d d_u;\n// distortion(p_u, d_u);\n// p_d = p_u + d_u;\n// }\n//\n// // Apply generalised projection matrix\n// p << mParameters.gamma1() * p_d(0) + mParameters.u0(),\n// mParameters.gamma2() * p_d(1) + mParameters.v0();\n}\n\nvoid\nEquidistantCamera::initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale) const\n{\n cv::Size imageSize(mParameters.imageWidth(), mParameters.imageHeight());\n\n cv::Mat mapX = cv::Mat::zeros(imageSize, CV_32F);\n cv::Mat mapY = cv::Mat::zeros(imageSize, CV_32F);\n\n for (int v = 0; v < imageSize.height; ++v)\n {\n for (int u = 0; u < imageSize.width; ++u)\n {\n double mx_u = m_inv_K11 / fScale * u + m_inv_K13 / fScale;\n double my_u = m_inv_K22 / fScale * v + m_inv_K23 / fScale;\n\n double theta, phi;\n backprojectSymmetric(Eigen::Vector2d(mx_u, my_u), theta, phi);\n\n Eigen::Vector3d P;\n P << sin(theta) * cos(phi), sin(theta) * sin(phi), cos(theta);\n\n Eigen::Vector2d p;\n spaceToPlane(P, p);\n\n mapX.at(v,u) = p(0);\n mapY.at(v,u) = p(1);\n }\n }\n\n cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false);\n}\n\ncv::Mat\nEquidistantCamera::initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2,\n float fx, float fy,\n cv::Size imageSize,\n float cx, float cy,\n cv::Mat rmat) const\n{\n if (imageSize == cv::Size(0, 0))\n {\n imageSize = cv::Size(mParameters.imageWidth(), mParameters.imageHeight());\n }\n\n cv::Mat mapX = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F);\n cv::Mat mapY = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F);\n\n Eigen::Matrix3f K_rect;\n\n if (cx == -1.0f && cy == -1.0f)\n {\n K_rect << fx, 0, imageSize.width / 2,\n 0, fy, imageSize.height / 2,\n 0, 0, 1;\n }\n else\n {\n K_rect << fx, 0, cx,\n 0, fy, cy,\n 0, 0, 1;\n }\n\n if (fx == -1.0f || fy == -1.0f)\n {\n K_rect(0,0) = mParameters.mu();\n K_rect(1,1) = mParameters.mv();\n }\n\n Eigen::Matrix3f K_rect_inv = K_rect.inverse();\n\n Eigen::Matrix3f R, R_inv;\n cv::cv2eigen(rmat, R);\n R_inv = R.inverse();\n\n for (int v = 0; v < imageSize.height; ++v)\n {\n for (int u = 0; u < imageSize.width; ++u)\n {\n Eigen::Vector3f xo;\n xo << u, v, 1;\n\n Eigen::Vector3f uo = R_inv * K_rect_inv * xo;\n\n Eigen::Vector2d p;\n spaceToPlane(uo.cast(), p);\n\n mapX.at(v,u) = p(0);\n mapY.at(v,u) = p(1);\n }\n }\n\n cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false);\n\n cv::Mat K_rect_cv;\n cv::eigen2cv(K_rect, K_rect_cv);\n return K_rect_cv;\n}\n\nint\nEquidistantCamera::parameterCount(void) const\n{\n return 8;\n}\n\nconst EquidistantCamera::Parameters&\nEquidistantCamera::getParameters(void) const\n{\n return mParameters;\n}\n\nvoid\nEquidistantCamera::setParameters(const EquidistantCamera::Parameters& parameters)\n{\n mParameters = parameters;\n\n // Inverse camera projection matrix parameters\n m_inv_K11 = 1.0 / mParameters.mu();\n m_inv_K13 = -mParameters.u0() / mParameters.mu();\n m_inv_K22 = 1.0 / mParameters.mv();\n m_inv_K23 = -mParameters.v0() / mParameters.mv();\n}\n\nvoid\nEquidistantCamera::readParameters(const std::vector& parameterVec)\n{\n if (parameterVec.size() != parameterCount())\n {\n return;\n }\n\n Parameters params = getParameters();\n\n params.k2() = parameterVec.at(0);\n params.k3() = parameterVec.at(1);\n params.k4() = parameterVec.at(2);\n params.k5() = parameterVec.at(3);\n params.mu() = parameterVec.at(4);\n params.mv() = parameterVec.at(5);\n params.u0() = parameterVec.at(6);\n params.v0() = parameterVec.at(7);\n\n setParameters(params);\n}\n\nvoid\nEquidistantCamera::writeParameters(std::vector& parameterVec) const\n{\n parameterVec.resize(parameterCount());\n parameterVec.at(0) = mParameters.k2();\n parameterVec.at(1) = mParameters.k3();\n parameterVec.at(2) = mParameters.k4();\n parameterVec.at(3) = mParameters.k5();\n parameterVec.at(4) = mParameters.mu();\n parameterVec.at(5) = mParameters.mv();\n parameterVec.at(6) = mParameters.u0();\n parameterVec.at(7) = mParameters.v0();\n}\n\nvoid\nEquidistantCamera::writeParametersToYamlFile(const std::string& filename) const\n{\n mParameters.writeToYamlFile(filename);\n}\n\nstd::string\nEquidistantCamera::parametersToString(void) const\n{\n std::ostringstream oss;\n oss << mParameters;\n\n return oss.str();\n}\n\nvoid\nEquidistantCamera::fitOddPoly(const std::vector& x, const std::vector& y,\n int n, std::vector& coeffs) const\n{\n std::vector pows;\n for (int i = 1; i <= n; i += 2)\n {\n pows.push_back(i);\n }\n\n Eigen::MatrixXd X(x.size(), pows.size());\n Eigen::MatrixXd Y(y.size(), 1);\n for (size_t i = 0; i < x.size(); ++i)\n {\n for (size_t j = 0; j < pows.size(); ++j)\n {\n X(i,j) = pow(x.at(i), pows.at(j));\n }\n Y(i,0) = y.at(i);\n }\n\n Eigen::MatrixXd A = (X.transpose() * X).inverse() * X.transpose() * Y;\n\n coeffs.resize(A.rows());\n for (int i = 0; i < A.rows(); ++i)\n {\n coeffs.at(i) = A(i,0);\n }\n}\n\nvoid\nEquidistantCamera::backprojectSymmetric(const Eigen::Vector2d& p_u,\n double& theta, double& phi) const\n{\n double tol = 1e-10;\n double p_u_norm = p_u.norm();\n\n if (p_u_norm < 1e-10)\n {\n phi = 0.0;\n }\n else\n {\n phi = atan2(p_u(1), p_u(0));\n }\n\n int npow = 9;\n if (mParameters.k5() == 0.0)\n {\n npow -= 2;\n }\n if (mParameters.k4() == 0.0)\n {\n npow -= 2;\n }\n if (mParameters.k3() == 0.0)\n {\n npow -= 2;\n }\n if (mParameters.k2() == 0.0)\n {\n npow -= 2;\n }\n\n Eigen::MatrixXd coeffs(npow + 1, 1);\n coeffs.setZero();\n coeffs(0) = -p_u_norm;\n coeffs(1) = 1.0;\n\n if (npow >= 3)\n {\n coeffs(3) = mParameters.k2();\n }\n if (npow >= 5)\n {\n coeffs(5) = mParameters.k3();\n }\n if (npow >= 7)\n {\n coeffs(7) = mParameters.k4();\n }\n if (npow >= 9)\n {\n coeffs(9) = mParameters.k5();\n }\n\n if (npow == 1)\n {\n theta = p_u_norm;\n }\n else\n {\n // Get eigenvalues of companion matrix corresponding to polynomial.\n // Eigenvalues correspond to roots of polynomial.\n Eigen::MatrixXd A(npow, npow);\n A.setZero();\n A.block(1, 0, npow - 1, npow - 1).setIdentity();\n A.col(npow - 1) = - coeffs.block(0, 0, npow, 1) / coeffs(npow);\n\n Eigen::EigenSolver es(A);\n Eigen::MatrixXcd eigval = es.eigenvalues();\n\n std::vector thetas;\n for (int i = 0; i < eigval.rows(); ++i)\n {\n if (fabs(eigval(i).imag()) > tol)\n {\n continue;\n }\n\n double t = eigval(i).real();\n\n if (t < -tol)\n {\n continue;\n }\n else if (t < 0.0)\n {\n t = 0.0;\n }\n\n thetas.push_back(t);\n }\n\n if (thetas.empty())\n {\n theta = p_u_norm;\n }\n else\n {\n theta = *std::min_element(thetas.begin(), thetas.end());\n }\n }\n}\n\n}\n" }, { "path": "camera_model/src/camera_models/PinholeCamera.cc", "content": "#include \"camodocal/camera_models/PinholeCamera.h\"\n\n#include \n#include \n#include \n#include \n#include \n#include \n#include \n\n#include \"camodocal/gpl/gpl.h\"\n\nnamespace camodocal\n{\n\nPinholeCamera::Parameters::Parameters()\n : Camera::Parameters(PINHOLE)\n , m_k1(0.0)\n , m_k2(0.0)\n , m_p1(0.0)\n , m_p2(0.0)\n , m_fx(0.0)\n , m_fy(0.0)\n , m_cx(0.0)\n , m_cy(0.0)\n{\n\n}\n\nPinholeCamera::Parameters::Parameters(const std::string& cameraName,\n int w, int h,\n double k1, double k2,\n double p1, double p2,\n double fx, double fy,\n double cx, double cy)\n : Camera::Parameters(PINHOLE, cameraName, w, h)\n , m_k1(k1)\n , m_k2(k2)\n , m_p1(p1)\n , m_p2(p2)\n , m_fx(fx)\n , m_fy(fy)\n , m_cx(cx)\n , m_cy(cy)\n{\n}\n\ndouble&\nPinholeCamera::Parameters::k1(void)\n{\n return m_k1;\n}\n\ndouble&\nPinholeCamera::Parameters::k2(void)\n{\n return m_k2;\n}\n\ndouble&\nPinholeCamera::Parameters::p1(void)\n{\n return m_p1;\n}\n\ndouble&\nPinholeCamera::Parameters::p2(void)\n{\n return m_p2;\n}\n\ndouble&\nPinholeCamera::Parameters::fx(void)\n{\n return m_fx;\n}\n\ndouble&\nPinholeCamera::Parameters::fy(void)\n{\n return m_fy;\n}\n\ndouble&\nPinholeCamera::Parameters::cx(void)\n{\n return m_cx;\n}\n\ndouble&\nPinholeCamera::Parameters::cy(void)\n{\n return m_cy;\n}\n\ndouble\nPinholeCamera::Parameters::k1(void) const\n{\n return m_k1;\n}\n\ndouble\nPinholeCamera::Parameters::k2(void) const\n{\n return m_k2;\n}\n\ndouble\nPinholeCamera::Parameters::p1(void) const\n{\n return m_p1;\n}\n\ndouble\nPinholeCamera::Parameters::p2(void) const\n{\n return m_p2;\n}\n\ndouble\nPinholeCamera::Parameters::fx(void) const\n{\n return m_fx;\n}\n\ndouble\nPinholeCamera::Parameters::fy(void) const\n{\n return m_fy;\n}\n\ndouble\nPinholeCamera::Parameters::cx(void) const\n{\n return m_cx;\n}\n\ndouble\nPinholeCamera::Parameters::cy(void) const\n{\n return m_cy;\n}\n\nbool\nPinholeCamera::Parameters::readFromYamlFile(const std::string& filename)\n{\n cv::FileStorage fs(filename, cv::FileStorage::READ);\n\n if (!fs.isOpened())\n {\n return false;\n }\n\n if (!fs[\"model_type\"].isNone())\n {\n std::string sModelType;\n fs[\"model_type\"] >> sModelType;\n\n if (sModelType.compare(\"PINHOLE\") != 0)\n {\n return false;\n }\n }\n\n m_modelType = PINHOLE;\n fs[\"camera_name\"] >> m_cameraName;\n m_imageWidth = static_cast(fs[\"image_width\"]);\n m_imageHeight = static_cast(fs[\"image_height\"]);\n\n cv::FileNode n = fs[\"distortion_parameters\"];\n m_k1 = static_cast(n[\"k1\"]);\n m_k2 = static_cast(n[\"k2\"]);\n m_p1 = static_cast(n[\"p1\"]);\n m_p2 = static_cast(n[\"p2\"]);\n\n n = fs[\"projection_parameters\"];\n m_fx = static_cast(n[\"fx\"]);\n m_fy = static_cast(n[\"fy\"]);\n m_cx = static_cast(n[\"cx\"]);\n m_cy = static_cast(n[\"cy\"]);\n\n return true;\n}\n\nvoid\nPinholeCamera::Parameters::writeToYamlFile(const std::string& filename) const\n{\n cv::FileStorage fs(filename, cv::FileStorage::WRITE);\n\n fs << \"model_type\" << \"PINHOLE\";\n fs << \"camera_name\" << m_cameraName;\n fs << \"image_width\" << m_imageWidth;\n fs << \"image_height\" << m_imageHeight;\n\n // radial distortion: k1, k2\n // tangential distortion: p1, p2\n fs << \"distortion_parameters\";\n fs << \"{\" << \"k1\" << m_k1\n << \"k2\" << m_k2\n << \"p1\" << m_p1\n << \"p2\" << m_p2 << \"}\";\n\n // projection: fx, fy, cx, cy\n fs << \"projection_parameters\";\n fs << \"{\" << \"fx\" << m_fx\n << \"fy\" << m_fy\n << \"cx\" << m_cx\n << \"cy\" << m_cy << \"}\";\n\n fs.release();\n}\n\nPinholeCamera::Parameters&\nPinholeCamera::Parameters::operator=(const PinholeCamera::Parameters& other)\n{\n if (this != &other)\n {\n m_modelType = other.m_modelType;\n m_cameraName = other.m_cameraName;\n m_imageWidth = other.m_imageWidth;\n m_imageHeight = other.m_imageHeight;\n m_k1 = other.m_k1;\n m_k2 = other.m_k2;\n m_p1 = other.m_p1;\n m_p2 = other.m_p2;\n m_fx = other.m_fx;\n m_fy = other.m_fy;\n m_cx = other.m_cx;\n m_cy = other.m_cy;\n }\n\n return *this;\n}\n\nstd::ostream&\noperator<< (std::ostream& out, const PinholeCamera::Parameters& params)\n{\n out << \"Camera Parameters:\" << std::endl;\n out << \" model_type \" << \"PINHOLE\" << std::endl;\n out << \" camera_name \" << params.m_cameraName << std::endl;\n out << \" image_width \" << params.m_imageWidth << std::endl;\n out << \" image_height \" << params.m_imageHeight << std::endl;\n\n // radial distortion: k1, k2\n // tangential distortion: p1, p2\n out << \"Distortion Parameters\" << std::endl;\n out << \" k1 \" << params.m_k1 << std::endl\n << \" k2 \" << params.m_k2 << std::endl\n << \" p1 \" << params.m_p1 << std::endl\n << \" p2 \" << params.m_p2 << std::endl;\n\n // projection: fx, fy, cx, cy\n out << \"Projection Parameters\" << std::endl;\n out << \" fx \" << params.m_fx << std::endl\n << \" fy \" << params.m_fy << std::endl\n << \" cx \" << params.m_cx << std::endl\n << \" cy \" << params.m_cy << std::endl;\n\n return out;\n}\n\nPinholeCamera::PinholeCamera()\n : m_inv_K11(1.0)\n , m_inv_K13(0.0)\n , m_inv_K22(1.0)\n , m_inv_K23(0.0)\n , m_noDistortion(true)\n{\n\n}\n\nPinholeCamera::PinholeCamera(const std::string& cameraName,\n int imageWidth, int imageHeight,\n double k1, double k2, double p1, double p2,\n double fx, double fy, double cx, double cy)\n : mParameters(cameraName, imageWidth, imageHeight,\n k1, k2, p1, p2, fx, fy, cx, cy)\n{\n if ((mParameters.k1() == 0.0) &&\n (mParameters.k2() == 0.0) &&\n (mParameters.p1() == 0.0) &&\n (mParameters.p2() == 0.0))\n {\n m_noDistortion = true;\n }\n else\n {\n m_noDistortion = false;\n }\n\n // Inverse camera projection matrix parameters\n m_inv_K11 = 1.0 / mParameters.fx();\n m_inv_K13 = -mParameters.cx() / mParameters.fx();\n m_inv_K22 = 1.0 / mParameters.fy();\n m_inv_K23 = -mParameters.cy() / mParameters.fy();\n}\n\nPinholeCamera::PinholeCamera(const PinholeCamera::Parameters& params)\n : mParameters(params)\n{\n if ((mParameters.k1() == 0.0) &&\n (mParameters.k2() == 0.0) &&\n (mParameters.p1() == 0.0) &&\n (mParameters.p2() == 0.0))\n {\n m_noDistortion = true;\n }\n else\n {\n m_noDistortion = false;\n }\n\n // Inverse camera projection matrix parameters\n m_inv_K11 = 1.0 / mParameters.fx();\n m_inv_K13 = -mParameters.cx() / mParameters.fx();\n m_inv_K22 = 1.0 / mParameters.fy();\n m_inv_K23 = -mParameters.cy() / mParameters.fy();\n}\n\nCamera::ModelType\nPinholeCamera::modelType(void) const\n{\n return mParameters.modelType();\n}\n\nconst std::string&\nPinholeCamera::cameraName(void) const\n{\n return mParameters.cameraName();\n}\n\nint\nPinholeCamera::imageWidth(void) const\n{\n return mParameters.imageWidth();\n}\n\nint\nPinholeCamera::imageHeight(void) const\n{\n return mParameters.imageHeight();\n}\n\nvoid\nPinholeCamera::estimateIntrinsics(const cv::Size& boardSize,\n const std::vector< std::vector >& objectPoints,\n const std::vector< std::vector >& imagePoints)\n{\n // Z. Zhang, A Flexible New Technique for Camera Calibration, PAMI 2000\n\n Parameters params = getParameters();\n\n params.k1() = 0.0;\n params.k2() = 0.0;\n params.p1() = 0.0;\n params.p2() = 0.0;\n\n double cx = params.imageWidth() / 2.0;\n double cy = params.imageHeight() / 2.0;\n params.cx() = cx;\n params.cy() = cy;\n\n size_t nImages = imagePoints.size();\n\n cv::Mat A(nImages * 2, 2, CV_64F);\n cv::Mat b(nImages * 2, 1, CV_64F);\n\n for (size_t i = 0; i < nImages; ++i)\n {\n const std::vector& oPoints = objectPoints.at(i);\n\n std::vector M(oPoints.size());\n for (size_t j = 0; j < M.size(); ++j)\n {\n M.at(j) = cv::Point2f(oPoints.at(j).x, oPoints.at(j).y);\n }\n\n cv::Mat H = cv::findHomography(M, imagePoints.at(i));\n\n H.at(0,0) -= H.at(2,0) * cx;\n H.at(0,1) -= H.at(2,1) * cx;\n H.at(0,2) -= H.at(2,2) * cx;\n H.at(1,0) -= H.at(2,0) * cy;\n H.at(1,1) -= H.at(2,1) * cy;\n H.at(1,2) -= H.at(2,2) * cy;\n\n double h[3], v[3], d1[3], d2[3];\n double n[4] = {0,0,0,0};\n\n for (int j = 0; j < 3; ++j)\n {\n double t0 = H.at(j,0);\n double t1 = H.at(j,1);\n h[j] = t0; v[j] = t1;\n d1[j] = (t0 + t1) * 0.5;\n d2[j] = (t0 - t1) * 0.5;\n n[0] += t0 * t0; n[1] += t1 * t1;\n n[2] += d1[j] * d1[j]; n[3] += d2[j] * d2[j];\n }\n\n for (int j = 0; j < 4; ++j)\n {\n n[j] = 1.0 / sqrt(n[j]);\n }\n\n for (int j = 0; j < 3; ++j)\n {\n h[j] *= n[0]; v[j] *= n[1];\n d1[j] *= n[2]; d2[j] *= n[3];\n }\n\n A.at(i * 2, 0) = h[0] * v[0];\n A.at(i * 2, 1) = h[1] * v[1];\n A.at(i * 2 + 1, 0) = d1[0] * d2[0];\n A.at(i * 2 + 1, 1) = d1[1] * d2[1];\n b.at(i * 2, 0) = -h[2] * v[2];\n b.at(i * 2 + 1, 0) = -d1[2] * d2[2];\n }\n\n cv::Mat f(2, 1, CV_64F);\n cv::solve(A, b, f, cv::DECOMP_NORMAL | cv::DECOMP_LU);\n\n params.fx() = sqrt(fabs(1.0 / f.at(0)));\n params.fy() = sqrt(fabs(1.0 / f.at(1)));\n\n setParameters(params);\n}\n\n/**\n * \\brief Lifts a point from the image plane to the unit sphere\n *\n * \\param p image coordinates\n * \\param P coordinates of the point on the sphere\n */\nvoid\nPinholeCamera::liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const\n{\n liftProjective(p, P);\n\n P.normalize();\n}\n\n/**\n * \\brief Lifts a point from the image plane to its projective ray\n *\n * \\param p image coordinates\n * \\param P coordinates of the projective ray\n */\nvoid\nPinholeCamera::liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const\n{\n double mx_d, my_d,mx2_d, mxy_d, my2_d, mx_u, my_u;\n double rho2_d, rho4_d, radDist_d, Dx_d, Dy_d, inv_denom_d;\n //double lambda;\n\n // Lift points to normalised plane\n mx_d = m_inv_K11 * p(0) + m_inv_K13;\n my_d = m_inv_K22 * p(1) + m_inv_K23;\n\n if (m_noDistortion)\n {\n mx_u = mx_d;\n my_u = my_d;\n }\n else\n {\n if (0)\n {\n double k1 = mParameters.k1();\n double k2 = mParameters.k2();\n double p1 = mParameters.p1();\n double p2 = mParameters.p2();\n\n // Apply inverse distortion model\n // proposed by Heikkila\n mx2_d = mx_d*mx_d;\n my2_d = my_d*my_d;\n mxy_d = mx_d*my_d;\n rho2_d = mx2_d+my2_d;\n rho4_d = rho2_d*rho2_d;\n radDist_d = k1*rho2_d+k2*rho4_d;\n Dx_d = mx_d*radDist_d + p2*(rho2_d+2*mx2_d) + 2*p1*mxy_d;\n Dy_d = my_d*radDist_d + p1*(rho2_d+2*my2_d) + 2*p2*mxy_d;\n inv_denom_d = 1/(1+4*k1*rho2_d+6*k2*rho4_d+8*p1*my_d+8*p2*mx_d);\n\n mx_u = mx_d - inv_denom_d*Dx_d;\n my_u = my_d - inv_denom_d*Dy_d;\n }\n else\n {\n // Recursive distortion model\n int n = 8;\n Eigen::Vector2d d_u;\n distortion(Eigen::Vector2d(mx_d, my_d), d_u);\n // Approximate value\n mx_u = mx_d - d_u(0);\n my_u = my_d - d_u(1);\n\n for (int i = 1; i < n; ++i)\n {\n distortion(Eigen::Vector2d(mx_u, my_u), d_u);\n mx_u = mx_d - d_u(0);\n my_u = my_d - d_u(1);\n }\n }\n }\n\n // Obtain a projective ray\n P << mx_u, my_u, 1.0;\n}\n\n\n/**\n * \\brief Project a 3D point (\\a x,\\a y,\\a z) to the image plane in (\\a u,\\a v)\n *\n * \\param P 3D point coordinates\n * \\param p return value, contains the image point coordinates\n */\nvoid\nPinholeCamera::spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const\n{\n Eigen::Vector2d p_u, p_d;\n\n // Project points to the normalised plane\n p_u << P(0) / P(2), P(1) / P(2);\n\n if (m_noDistortion)\n {\n p_d = p_u;\n }\n else\n {\n // Apply distortion\n Eigen::Vector2d d_u;\n distortion(p_u, d_u);\n p_d = p_u + d_u;\n }\n\n // Apply generalised projection matrix\n p << mParameters.fx() * p_d(0) + mParameters.cx(),\n mParameters.fy() * p_d(1) + mParameters.cy();\n}\n\n#if 0\n/**\n * \\brief Project a 3D point to the image plane and calculate Jacobian\n *\n * \\param P 3D point coordinates\n * \\param p return value, contains the image point coordinates\n */\nvoid\nPinholeCamera::spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p,\n Eigen::Matrix& J) const\n{\n Eigen::Vector2d p_u, p_d;\n double norm, inv_denom;\n double dxdmx, dydmx, dxdmy, dydmy;\n\n norm = P.norm();\n // Project points to the normalised plane\n inv_denom = 1.0 / P(2);\n p_u << inv_denom * P(0), inv_denom * P(1);\n\n // Calculate jacobian\n double dudx = inv_denom;\n double dvdx = 0.0;\n double dudy = 0.0;\n double dvdy = inv_denom;\n inv_denom = - inv_denom * inv_denom;\n double dudz = P(0) * inv_denom;\n double dvdz = P(1) * inv_denom;\n\n if (m_noDistortion)\n {\n p_d = p_u;\n }\n else\n {\n // Apply distortion\n Eigen::Vector2d d_u;\n distortion(p_u, d_u);\n p_d = p_u + d_u;\n }\n\n double fx = mParameters.fx();\n double fy = mParameters.fy();\n\n // Make the product of the jacobians\n // and add projection matrix jacobian\n inv_denom = fx * (dudx * dxdmx + dvdx * dxdmy); // reuse\n dvdx = fy * (dudx * dydmx + dvdx * dydmy);\n dudx = inv_denom;\n\n inv_denom = fx * (dudy * dxdmx + dvdy * dxdmy); // reuse\n dvdy = fy * (dudy * dydmx + dvdy * dydmy);\n dudy = inv_denom;\n\n inv_denom = fx * (dudz * dxdmx + dvdz * dxdmy); // reuse\n dvdz = fy * (dudz * dydmx + dvdz * dydmy);\n dudz = inv_denom;\n\n // Apply generalised projection matrix\n p << fx * p_d(0) + mParameters.cx(),\n fy * p_d(1) + mParameters.cy();\n\n J << dudx, dudy, dudz,\n dvdx, dvdy, dvdz;\n}\n#endif\n\n/**\n * \\brief Projects an undistorted 2D point p_u to the image plane\n *\n * \\param p_u 2D point coordinates\n * \\return image point coordinates\n */\nvoid\nPinholeCamera::undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const\n{\n Eigen::Vector2d p_d;\n\n if (m_noDistortion)\n {\n p_d = p_u;\n }\n else\n {\n // Apply distortion\n Eigen::Vector2d d_u;\n distortion(p_u, d_u);\n p_d = p_u + d_u;\n }\n\n // Apply generalised projection matrix\n p << mParameters.fx() * p_d(0) + mParameters.cx(),\n mParameters.fy() * p_d(1) + mParameters.cy();\n}\n\n/**\n * \\brief Apply distortion to input point (from the normalised plane)\n *\n * \\param p_u undistorted coordinates of point on the normalised plane\n * \\return to obtain the distorted point: p_d = p_u + d_u\n */\nvoid\nPinholeCamera::distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u) const\n{\n double k1 = mParameters.k1();\n double k2 = mParameters.k2();\n double p1 = mParameters.p1();\n double p2 = mParameters.p2();\n\n double mx2_u, my2_u, mxy_u, rho2_u, rad_dist_u;\n\n mx2_u = p_u(0) * p_u(0);\n my2_u = p_u(1) * p_u(1);\n mxy_u = p_u(0) * p_u(1);\n rho2_u = mx2_u + my2_u;\n rad_dist_u = k1 * rho2_u + k2 * rho2_u * rho2_u;\n d_u << p_u(0) * rad_dist_u + 2.0 * p1 * mxy_u + p2 * (rho2_u + 2.0 * mx2_u),\n p_u(1) * rad_dist_u + 2.0 * p2 * mxy_u + p1 * (rho2_u + 2.0 * my2_u);\n}\n\n/**\n * \\brief Apply distortion to input point (from the normalised plane)\n * and calculate Jacobian\n *\n * \\param p_u undistorted coordinates of point on the normalised plane\n * \\return to obtain the distorted point: p_d = p_u + d_u\n */\nvoid\nPinholeCamera::distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u,\n Eigen::Matrix2d& J) const\n{\n double k1 = mParameters.k1();\n double k2 = mParameters.k2();\n double p1 = mParameters.p1();\n double p2 = mParameters.p2();\n\n double mx2_u, my2_u, mxy_u, rho2_u, rad_dist_u;\n\n mx2_u = p_u(0) * p_u(0);\n my2_u = p_u(1) * p_u(1);\n mxy_u = p_u(0) * p_u(1);\n rho2_u = mx2_u + my2_u;\n rad_dist_u = k1 * rho2_u + k2 * rho2_u * rho2_u;\n d_u << p_u(0) * rad_dist_u + 2.0 * p1 * mxy_u + p2 * (rho2_u + 2.0 * mx2_u),\n p_u(1) * rad_dist_u + 2.0 * p2 * mxy_u + p1 * (rho2_u + 2.0 * my2_u);\n\n 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);\n 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);\n double dxdmy = dydmx;\n 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);\n\n J << dxdmx, dxdmy,\n dydmx, dydmy;\n}\n\nvoid\nPinholeCamera::initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale) const\n{\n cv::Size imageSize(mParameters.imageWidth(), mParameters.imageHeight());\n\n cv::Mat mapX = cv::Mat::zeros(imageSize, CV_32F);\n cv::Mat mapY = cv::Mat::zeros(imageSize, CV_32F);\n\n for (int v = 0; v < imageSize.height; ++v)\n {\n for (int u = 0; u < imageSize.width; ++u)\n {\n double mx_u = m_inv_K11 / fScale * u + m_inv_K13 / fScale;\n double my_u = m_inv_K22 / fScale * v + m_inv_K23 / fScale;\n\n Eigen::Vector3d P;\n P << mx_u, my_u, 1.0;\n\n Eigen::Vector2d p;\n spaceToPlane(P, p);\n\n mapX.at(v,u) = p(0);\n mapY.at(v,u) = p(1);\n }\n }\n\n cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false);\n}\n\ncv::Mat\nPinholeCamera::initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2,\n float fx, float fy,\n cv::Size imageSize,\n float cx, float cy,\n cv::Mat rmat) const\n{\n if (imageSize == cv::Size(0, 0))\n {\n imageSize = cv::Size(mParameters.imageWidth(), mParameters.imageHeight());\n }\n\n cv::Mat mapX = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F);\n cv::Mat mapY = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F);\n\n Eigen::Matrix3f R, R_inv;\n cv::cv2eigen(rmat, R);\n R_inv = R.inverse();\n\n // assume no skew\n Eigen::Matrix3f K_rect;\n\n if (cx == -1.0f || cy == -1.0f)\n {\n K_rect << fx, 0, imageSize.width / 2,\n 0, fy, imageSize.height / 2,\n 0, 0, 1;\n }\n else\n {\n K_rect << fx, 0, cx,\n 0, fy, cy,\n 0, 0, 1;\n }\n\n if (fx == -1.0f || fy == -1.0f)\n {\n K_rect(0,0) = mParameters.fx();\n K_rect(1,1) = mParameters.fy();\n }\n\n Eigen::Matrix3f K_rect_inv = K_rect.inverse();\n\n for (int v = 0; v < imageSize.height; ++v)\n {\n for (int u = 0; u < imageSize.width; ++u)\n {\n Eigen::Vector3f xo;\n xo << u, v, 1;\n\n Eigen::Vector3f uo = R_inv * K_rect_inv * xo;\n\n Eigen::Vector2d p;\n spaceToPlane(uo.cast(), p);\n\n mapX.at(v,u) = p(0);\n mapY.at(v,u) = p(1);\n }\n }\n\n cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false);\n\n cv::Mat K_rect_cv;\n cv::eigen2cv(K_rect, K_rect_cv);\n return K_rect_cv;\n}\n\nint\nPinholeCamera::parameterCount(void) const\n{\n return 8;\n}\n\nconst PinholeCamera::Parameters&\nPinholeCamera::getParameters(void) const\n{\n return mParameters;\n}\n\nvoid\nPinholeCamera::setParameters(const PinholeCamera::Parameters& parameters)\n{\n mParameters = parameters;\n\n if ((mParameters.k1() == 0.0) &&\n (mParameters.k2() == 0.0) &&\n (mParameters.p1() == 0.0) &&\n (mParameters.p2() == 0.0))\n {\n m_noDistortion = true;\n }\n else\n {\n m_noDistortion = false;\n }\n\n m_inv_K11 = 1.0 / mParameters.fx();\n m_inv_K13 = -mParameters.cx() / mParameters.fx();\n m_inv_K22 = 1.0 / mParameters.fy();\n m_inv_K23 = -mParameters.cy() / mParameters.fy();\n}\n\nvoid\nPinholeCamera::readParameters(const std::vector& parameterVec)\n{\n if ((int)parameterVec.size() != parameterCount())\n {\n return;\n }\n\n Parameters params = getParameters();\n\n params.k1() = parameterVec.at(0);\n params.k2() = parameterVec.at(1);\n params.p1() = parameterVec.at(2);\n params.p2() = parameterVec.at(3);\n params.fx() = parameterVec.at(4);\n params.fy() = parameterVec.at(5);\n params.cx() = parameterVec.at(6);\n params.cy() = parameterVec.at(7);\n\n setParameters(params);\n}\n\nvoid\nPinholeCamera::writeParameters(std::vector& parameterVec) const\n{\n parameterVec.resize(parameterCount());\n parameterVec.at(0) = mParameters.k1();\n parameterVec.at(1) = mParameters.k2();\n parameterVec.at(2) = mParameters.p1();\n parameterVec.at(3) = mParameters.p2();\n parameterVec.at(4) = mParameters.fx();\n parameterVec.at(5) = mParameters.fy();\n parameterVec.at(6) = mParameters.cx();\n parameterVec.at(7) = mParameters.cy();\n}\n\nvoid\nPinholeCamera::writeParametersToYamlFile(const std::string& filename) const\n{\n mParameters.writeToYamlFile(filename);\n}\n\nstd::string\nPinholeCamera::parametersToString(void) const\n{\n std::ostringstream oss;\n oss << mParameters;\n\n return oss.str();\n}\n\n}\n" }, { "path": "camera_model/src/camera_models/ScaramuzzaCamera.cc", "content": "#include \"camodocal/camera_models/ScaramuzzaCamera.h\"\n\n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n\n#include \"camodocal/gpl/gpl.h\"\n\n\nEigen::VectorXd polyfit(Eigen::VectorXd& xVec, Eigen::VectorXd& yVec, int poly_order) {\n assert(poly_order > 0);\n assert(xVec.size() > poly_order);\n assert(xVec.size() == yVec.size());\n\n Eigen::MatrixXd A(xVec.size(), poly_order+1);\n Eigen::VectorXd B(xVec.size());\n\n for(int i = 0; i < xVec.size(); ++i) {\n const double x = xVec(i);\n const double y = yVec(i);\n\n double x_pow_k = 1.0;\n\n for(int k=0; k<=poly_order; ++k) {\n A(i,k) = x_pow_k;\n x_pow_k *= x;\n }\n\n B(i) = y;\n }\n\n Eigen::JacobiSVD svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV);\n Eigen::VectorXd x = svd.solve(B);\n\n return x;\n}\n\nnamespace camodocal\n{\n\nOCAMCamera::Parameters::Parameters()\n : Camera::Parameters(SCARAMUZZA)\n , m_C(0.0)\n , m_D(0.0)\n , m_E(0.0)\n , m_center_x(0.0)\n , m_center_y(0.0)\n{\n memset(m_poly, 0, sizeof(double) * SCARAMUZZA_POLY_SIZE);\n memset(m_inv_poly, 0, sizeof(double) * SCARAMUZZA_INV_POLY_SIZE);\n}\n\n\n\nbool\nOCAMCamera::Parameters::readFromYamlFile(const std::string& filename)\n{\n cv::FileStorage fs(filename, cv::FileStorage::READ);\n\n if (!fs.isOpened())\n {\n return false;\n }\n\n if (!fs[\"model_type\"].isNone())\n {\n std::string sModelType;\n fs[\"model_type\"] >> sModelType;\n\n if (!boost::iequals(sModelType, \"scaramuzza\"))\n {\n return false;\n }\n }\n\n m_modelType = SCARAMUZZA;\n fs[\"camera_name\"] >> m_cameraName;\n m_imageWidth = static_cast(fs[\"image_width\"]);\n m_imageHeight = static_cast(fs[\"image_height\"]);\n\n cv::FileNode n = fs[\"poly_parameters\"];\n for(int i=0; i < SCARAMUZZA_POLY_SIZE; i++)\n m_poly[i] = static_cast(n[std::string(\"p\") + boost::lexical_cast(i)]);\n\n n = fs[\"inv_poly_parameters\"];\n for(int i=0; i < SCARAMUZZA_INV_POLY_SIZE; i++)\n m_inv_poly[i] = static_cast(n[std::string(\"p\") + boost::lexical_cast(i)]);\n\n n = fs[\"affine_parameters\"];\n m_C = static_cast(n[\"ac\"]);\n m_D = static_cast(n[\"ad\"]);\n m_E = static_cast(n[\"ae\"]);\n\n m_center_x = static_cast(n[\"cx\"]);\n m_center_y = static_cast(n[\"cy\"]);\n\n return true;\n}\n\nvoid\nOCAMCamera::Parameters::writeToYamlFile(const std::string& filename) const\n{\n cv::FileStorage fs(filename, cv::FileStorage::WRITE);\n\n fs << \"model_type\" << \"scaramuzza\";\n fs << \"camera_name\" << m_cameraName;\n fs << \"image_width\" << m_imageWidth;\n fs << \"image_height\" << m_imageHeight;\n\n fs << \"poly_parameters\";\n fs << \"{\";\n for(int i=0; i < SCARAMUZZA_POLY_SIZE; i++)\n fs << std::string(\"p\") + boost::lexical_cast(i) << m_poly[i];\n fs << \"}\";\n\n fs << \"inv_poly_parameters\";\n fs << \"{\";\n for(int i=0; i < SCARAMUZZA_INV_POLY_SIZE; i++)\n fs << std::string(\"p\") + boost::lexical_cast(i) << m_inv_poly[i];\n fs << \"}\";\n\n fs << \"affine_parameters\";\n fs << \"{\" << \"ac\" << m_C\n << \"ad\" << m_D\n << \"ae\" << m_E\n << \"cx\" << m_center_x\n << \"cy\" << m_center_y << \"}\";\n\n fs.release();\n}\n\nOCAMCamera::Parameters&\nOCAMCamera::Parameters::operator=(const OCAMCamera::Parameters& other)\n{\n if (this != &other)\n {\n m_modelType = other.m_modelType;\n m_cameraName = other.m_cameraName;\n m_imageWidth = other.m_imageWidth;\n m_imageHeight = other.m_imageHeight;\n m_C = other.m_C;\n m_D = other.m_D;\n m_E = other.m_E;\n m_center_x = other.m_center_x;\n m_center_y = other.m_center_y;\n\n memcpy(m_poly, other.m_poly, sizeof(double) * SCARAMUZZA_POLY_SIZE);\n memcpy(m_inv_poly, other.m_inv_poly, sizeof(double) * SCARAMUZZA_INV_POLY_SIZE);\n }\n\n return *this;\n}\n\nstd::ostream&\noperator<< (std::ostream& out, const OCAMCamera::Parameters& params)\n{\n out << \"Camera Parameters:\" << std::endl;\n out << \" model_type \" << \"scaramuzza\" << std::endl;\n out << \" camera_name \" << params.m_cameraName << std::endl;\n out << \" image_width \" << params.m_imageWidth << std::endl;\n out << \" image_height \" << params.m_imageHeight << std::endl;\n\n out << std::fixed << std::setprecision(10);\n\n out << \"Poly Parameters\" << std::endl;\n for(int i=0; i < SCARAMUZZA_POLY_SIZE; i++)\n out << std::string(\"p\") + boost::lexical_cast(i) << \": \" << params.m_poly[i] << std::endl;\n\n out << \"Inverse Poly Parameters\" << std::endl;\n for(int i=0; i < SCARAMUZZA_INV_POLY_SIZE; i++)\n out << std::string(\"p\") + boost::lexical_cast(i) << \": \" << params.m_inv_poly[i] << std::endl;\n\n out << \"Affine Parameters\" << std::endl;\n out << \" ac \" << params.m_C << std::endl\n << \" ad \" << params.m_D << std::endl\n << \" ae \" << params.m_E << std::endl;\n out << \" cx \" << params.m_center_x << std::endl\n << \" cy \" << params.m_center_y << std::endl;\n\n return out;\n}\n\nOCAMCamera::OCAMCamera()\n : m_inv_scale(0.0)\n{\n\n}\n\nOCAMCamera::OCAMCamera(const OCAMCamera::Parameters& params)\n : mParameters(params)\n{\n m_inv_scale = 1.0 / (params.C() - params.D() * params.E());\n}\n\nCamera::ModelType\nOCAMCamera::modelType(void) const\n{\n return mParameters.modelType();\n}\n\nconst std::string&\nOCAMCamera::cameraName(void) const\n{\n return mParameters.cameraName();\n}\n\nint\nOCAMCamera::imageWidth(void) const\n{\n return mParameters.imageWidth();\n}\n\nint\nOCAMCamera::imageHeight(void) const\n{\n return mParameters.imageHeight();\n}\n\nvoid\nOCAMCamera::estimateIntrinsics(const cv::Size& boardSize,\n const std::vector< std::vector >& objectPoints,\n const std::vector< std::vector >& imagePoints)\n{\n // std::cout << \"OCAMCamera::estimateIntrinsics - NOT IMPLEMENTED\" << std::endl;\n // throw std::string(\"OCAMCamera::estimateIntrinsics - NOT IMPLEMENTED\");\n\n // Reference: Page 30 of\n // \" Scaramuzza, D. Omnidirectional Vision: from Calibration to Robot Motion Estimation, ETH Zurich. Thesis no. 17635.\"\n // http://e-collection.library.ethz.ch/eserv/eth:30301/eth-30301-02.pdf\n // Matlab code: calibrate.m\n\n // First, estimate every image's extrinsics parameters\n std::vector< Eigen::Matrix3d > RList;\n std::vector< Eigen::Vector3d > TList;\n\n RList.reserve(imagePoints.size());\n TList.reserve(imagePoints.size());\n\n // i-th image\n for (size_t image_index = 0; image_index < imagePoints.size(); ++image_index)\n {\n const std::vector& objPts = objectPoints.at(image_index);\n const std::vector& imgPts = imagePoints.at(image_index);\n\n assert(objPts.size() == imgPts.size());\n assert(objPts.size() == static_cast(boardSize.width * boardSize.height));\n\n Eigen::MatrixXd M(objPts.size(), 6);\n\n for(size_t corner_index = 0; corner_index < objPts.size(); ++corner_index) {\n double X = objPts.at(corner_index).x;\n double Y = objPts.at(corner_index).y;\n assert(objPts.at(corner_index).z==0.0);\n \n double u = imgPts.at(corner_index).x;\n double v = imgPts.at(corner_index).y;\n\n M(corner_index, 0) = -v * X;\n M(corner_index, 1) = -v * Y;\n M(corner_index, 2) = u * X;\n M(corner_index, 3) = u * Y;\n M(corner_index, 4) = -v;\n M(corner_index, 5) = u;\n }\n\n Eigen::JacobiSVD svd(M, Eigen::ComputeFullU | Eigen::ComputeFullV);\n assert(svd.matrixV().cols() == 6);\n Eigen::VectorXd h = -svd.matrixV().col(5);\n\n // scaled version of R and T\n const double sr11 = h(0);\n const double sr12 = h(1);\n const double sr21 = h(2);\n const double sr22 = h(3);\n const double st1 = h(4);\n const double st2 = h(5);\n\n const double AA = square(sr11*sr12 + sr21*sr22);\n const double BB = square(sr11) + square(sr21);\n const double CC = square(sr12) + square(sr22);\n\n const double sr32_squared_1 = (- (CC-BB) + sqrt(square(CC-BB) + 4.0 * AA)) / 2.0;\n const double sr32_squared_2 = (- (CC-BB) - sqrt(square(CC-BB) + 4.0 * AA)) / 2.0;\n\n// printf(\"rst = %.12f\\n\", sr32_squared_1*sr32_squared_1 + (CC-BB)*sr32_squared_1 - AA);\n\n std::vector sr32_squared_values;\n if (sr32_squared_1 > 0) sr32_squared_values.push_back(sr32_squared_1);\n if (sr32_squared_2 > 0) sr32_squared_values.push_back(sr32_squared_2);\n assert(!sr32_squared_values.empty());\n\n std::vector sr32_values;\n std::vector sr31_values;\n for (auto sr32_squared : sr32_squared_values) {\n for(int sign = -1; sign <= 1; sign += 2) {\n const double sr32 = static_cast(sign) * std::sqrt(sr32_squared);\n sr32_values.push_back( sr32 );\n if (sr32_squared == 0.0) {\n // sr31 can be calculated through norm equality, \n // but it has positive and negative posibilities\n // positive one\n sr31_values.push_back(std::sqrt(CC-BB));\n // negative one\n sr32_values.push_back( sr32 );\n sr31_values.push_back(-std::sqrt(CC-BB));\n \n break; // skip the same situation\n } else {\n // sr31 can be calculated throught dot product == 0\n sr31_values.push_back(- (sr11*sr12 + sr21*sr22) / sr32);\n }\n }\n }\n\n // std::cout << \"h= \" << std::setprecision(12) << h.transpose() << std::endl;\n // std::cout << \"length: \" << sr32_values.size() << \" & \" << sr31_values.size() << std::endl;\n\n assert(!sr31_values.empty());\n assert(sr31_values.size() == sr32_values.size());\n \n std::vector H_values;\n for(size_t i=0;i H_candidates;\n\n for (auto& H : H_values)\n {\n Eigen::MatrixXd A_mat(2 * imagePoints.at(image_index).size(), 4);\n Eigen::VectorXd B_vec(2 * imagePoints.at(image_index).size());\n A_mat.setZero();\n B_vec.setZero();\n\n size_t line_index = 0;\n\n // iterate images\n const double& r11 = H(0,0);\n const double& r12 = H(0,1);\n // const double& r13 = H(0,2);\n const double& r21 = H(1,0);\n const double& r22 = H(1,1);\n // const double& r23 = H(1,2);\n const double& r31 = H(2,0);\n const double& r32 = H(2,1);\n // const double& r33 = H(2,2);\n const double& t1 = H(0);\n const double& t2 = H(1);\n \n // iterate chessboard corners in the image\n for(size_t j=0; j(A_mat.rows()));\n\n // pseudo-inverse for polynomial parameters and all t3s\n {\n Eigen::JacobiSVD svd(A_mat, Eigen::ComputeThinU | Eigen::ComputeThinV);\n\n Eigen::VectorXd x = svd.solve(B_vec);\n\n // std::cout << \"x(poly and t3) = \" << x << std::endl;\n\n if (x(2) > 0 && x(3) > 0) {\n H_candidates.push_back(H);\n }\n }\n }\n\n // printf(\"H_candidates.size()=%zu\\n\", H_candidates.size());\n assert(H_candidates.size()==1);\n\n Eigen::Matrix3d& H = H_candidates.front();\n\n Eigen::Matrix3d R; \n R.col(0) = H.col(0); \n R.col(1) = H.col(1); \n R.col(2) = H.col(0).cross(H.col(1)); \n\n Eigen::Vector3d T = H.col(2);\n RList.push_back(R);\n TList.push_back(T);\n\n // std::cout << \"#\" << image_index << \" frame\" << \" R =\" << R << \" \\nT = \" << T.transpose() << std::endl;\n }\n\n // Second, estimate camera intrinsic parameters and all t3\n Eigen::MatrixXd A_mat(2 * imagePoints.size() * imagePoints.at(0).size(), SCARAMUZZA_POLY_SIZE-1 + imagePoints.size());\n Eigen::VectorXd B_vec(2 * imagePoints.size() * imagePoints.at(0).size());\n A_mat.setZero();\n B_vec.setZero();\n\n size_t line_index = 0;\n\n // iterate images\n for(size_t i = 0; i < imagePoints.size(); ++i) {\n const double& r11 = RList.at(i)(0,0);\n const double& r12 = RList.at(i)(0,1);\n // const double& r13 = RList.at(i)(0,2);\n const double& r21 = RList.at(i)(1,0);\n const double& r22 = RList.at(i)(1,1);\n // const double& r23 = RList.at(i)(1,2);\n const double& r31 = RList.at(i)(2,0);\n const double& r32 = RList.at(i)(2,1);\n // const double& r33 = RList.at(i)(2,2);\n const double& t1 = TList.at(i)(0);\n const double& t2 = TList.at(i)(1);\n \n // iterate chessboard corners in the image\n for(size_t j=0; j(A_mat.rows()));\n\n Eigen::Matrix poly_coeff;\n // pseudo-inverse for polynomial parameters and all t3s\n {\n Eigen::JacobiSVD svd(A_mat, Eigen::ComputeThinU | Eigen::ComputeThinV);\n\n Eigen::VectorXd x = svd.solve(B_vec);\n\n poly_coeff[0] = x(0);\n poly_coeff[1] = 0.0;\n for(int i=1;i(SCARAMUZZA_POLY_SIZE-1+TList.size()));\n }\n\n Parameters params = getParameters();\n\n // Affine matrix A is constructed as [C D; E 1]\n params.C() = 1.0;\n params.D() = 0.0;\n params.E() = 0.0;\n\n params.center_x() = params.imageWidth() / 2.0;\n params.center_y() = params.imageHeight() / 2.0;\n \n for(size_t i=0; i rou_vec;\n std::vector z_vec;\n for(double rou = 0.0; rou <= (params.imageWidth() + params.imageHeight())/2; rou += 0.1) {\n double rou_pow_k = 1.0;\n double z = 0.0;\n\n for (int k = 0; k < SCARAMUZZA_POLY_SIZE; k++)\n {\n z += rou_pow_k * params.poly(k);\n rou_pow_k *= rou;\n }\n\n rou_vec.push_back(rou);\n z_vec.push_back(z);\n }\n\n assert(rou_vec.size() == z_vec.size());\n Eigen::VectorXd xVec(rou_vec.size());\n Eigen::VectorXd yVec(rou_vec.size());\n\n for(size_t i=0; i(v,u) = p(0);\n mapY.at(v,u) = p(1);\n }\n }\n\n cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false);\n}\n#endif\n\ncv::Mat\nOCAMCamera::initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2,\n float fx, float fy,\n cv::Size imageSize,\n float cx, float cy,\n cv::Mat rmat) const\n{\n if (imageSize == cv::Size(0, 0))\n {\n imageSize = cv::Size(mParameters.imageWidth(), mParameters.imageHeight());\n }\n\n cv::Mat mapX = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F);\n cv::Mat mapY = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F);\n\n Eigen::Matrix3f K_rect;\n\n K_rect << fx, 0, cx < 0 ? imageSize.width / 2 : cx,\n 0, fy, cy < 0 ? imageSize.height / 2 : cy,\n 0, 0, 1;\n\n if (fx < 0 || fy < 0)\n {\n throw std::string(std::string(__FUNCTION__) + \": Focal length must be specified\");\n }\n\n Eigen::Matrix3f K_rect_inv = K_rect.inverse();\n\n Eigen::Matrix3f R, R_inv;\n cv::cv2eigen(rmat, R);\n R_inv = R.inverse();\n\n for (int v = 0; v < imageSize.height; ++v)\n {\n for (int u = 0; u < imageSize.width; ++u)\n {\n Eigen::Vector3f xo;\n xo << u, v, 1;\n\n Eigen::Vector3f uo = R_inv * K_rect_inv * xo;\n\n Eigen::Vector2d p;\n spaceToPlane(uo.cast(), p);\n\n mapX.at(v,u) = p(0);\n mapY.at(v,u) = p(1);\n }\n }\n\n cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false);\n\n cv::Mat K_rect_cv;\n cv::eigen2cv(K_rect, K_rect_cv);\n return K_rect_cv;\n}\n\nint\nOCAMCamera::parameterCount(void) const\n{\n return SCARAMUZZA_CAMERA_NUM_PARAMS;\n}\n\nconst OCAMCamera::Parameters&\nOCAMCamera::getParameters(void) const\n{\n return mParameters;\n}\n\nvoid\nOCAMCamera::setParameters(const OCAMCamera::Parameters& parameters)\n{\n mParameters = parameters;\n\n m_inv_scale = 1.0 / (parameters.C() - parameters.D() * parameters.E());\n}\n\nvoid\nOCAMCamera::readParameters(const std::vector& parameterVec)\n{\n if ((int)parameterVec.size() != parameterCount())\n {\n return;\n }\n\n Parameters params = getParameters();\n\n params.C() = parameterVec.at(0);\n params.D() = parameterVec.at(1);\n params.E() = parameterVec.at(2);\n params.center_x() = parameterVec.at(3);\n params.center_y() = parameterVec.at(4);\n for (int i=0; i < SCARAMUZZA_POLY_SIZE; i++)\n params.poly(i) = parameterVec.at(5+i);\n for (int i=0; i < SCARAMUZZA_INV_POLY_SIZE; i++)\n params.inv_poly(i) = parameterVec.at(5 + SCARAMUZZA_POLY_SIZE + i);\n\n setParameters(params);\n}\n\nvoid\nOCAMCamera::writeParameters(std::vector& parameterVec) const\n{\n parameterVec.resize(parameterCount());\n parameterVec.at(0) = mParameters.C();\n parameterVec.at(1) = mParameters.D();\n parameterVec.at(2) = mParameters.E();\n parameterVec.at(3) = mParameters.center_x();\n parameterVec.at(4) = mParameters.center_y();\n for (int i=0; i < SCARAMUZZA_POLY_SIZE; i++)\n parameterVec.at(5+i) = mParameters.poly(i);\n for (int i=0; i < SCARAMUZZA_INV_POLY_SIZE; i++)\n parameterVec.at(5 + SCARAMUZZA_POLY_SIZE + i) = mParameters.inv_poly(i);\n}\n\nvoid\nOCAMCamera::writeParametersToYamlFile(const std::string& filename) const\n{\n mParameters.writeToYamlFile(filename);\n}\n\nstd::string\nOCAMCamera::parametersToString(void) const\n{\n std::ostringstream oss;\n oss << mParameters;\n\n return oss.str();\n}\n\n}\n" }, { "path": "camera_model/src/chessboard/Chessboard.cc", "content": "#include \"camodocal/chessboard/Chessboard.h\"\n\n#include \n#include \n\n#include \"camodocal/chessboard/ChessboardQuad.h\"\n#include \"camodocal/chessboard/Spline.h\"\n\n#define MAX_CONTOUR_APPROX 7\n\nnamespace camodocal\n{\n\nChessboard::Chessboard(cv::Size boardSize, cv::Mat& image)\n : mBoardSize(boardSize)\n , mCornersFound(false)\n{\n if (image.channels() == 1)\n {\n cv::cvtColor(image, mSketch, cv::COLOR_GRAY2BGR);\n image.copyTo(mImage);\n }\n else\n {\n image.copyTo(mSketch);\n cv::cvtColor(image, mImage, cv::COLOR_BGR2GRAY);\n }\n}\n\nvoid\nChessboard::findCorners(bool useOpenCV)\n{\n mCornersFound = findChessboardCorners(mImage, mBoardSize, mCorners,\n cv::CALIB_CB_ADAPTIVE_THRESH +\n cv::CALIB_CB_NORMALIZE_IMAGE +\n cv::CALIB_CB_FILTER_QUADS +\n cv::CALIB_CB_FAST_CHECK,\n useOpenCV);\n\n if (mCornersFound)\n {\n // draw chessboard corners\n cv::drawChessboardCorners(mSketch, mBoardSize, mCorners, mCornersFound);\n }\n}\n\nconst std::vector&\nChessboard::getCorners(void) const\n{\n return mCorners;\n}\n\nbool\nChessboard::cornersFound(void) const\n{\n return mCornersFound;\n}\n\nconst cv::Mat&\nChessboard::getImage(void) const\n{\n return mImage;\n}\n\nconst cv::Mat&\nChessboard::getSketch(void) const\n{\n return mSketch;\n}\n\nbool\nChessboard::findChessboardCorners(const cv::Mat& image,\n const cv::Size& patternSize,\n std::vector& corners,\n int flags, bool useOpenCV)\n{\n if (useOpenCV)\n {\n return cv::findChessboardCorners(image, patternSize, corners, flags);\n }\n else\n {\n return findChessboardCornersImproved(image, patternSize, corners, flags);\n }\n}\n\nbool\nChessboard::findChessboardCornersImproved(const cv::Mat& image,\n const cv::Size& patternSize,\n std::vector& corners,\n int flags)\n{\n /************************************************************************************\\\n This is improved variant of chessboard corner detection algorithm that\n uses a graph of connected quads. It is based on the code contributed\n by Vladimir Vezhnevets and Philip Gruebele.\n Here is the copyright notice from the original Vladimir's code:\n ===============================================================\n\n The algorithms developed and implemented by Vezhnevets Vldimir\n aka Dead Moroz (vvp@graphics.cs.msu.ru)\n See http://graphics.cs.msu.su/en/research/calibration/opencv.html\n for detailed information.\n\n Reliability additions and modifications made by Philip Gruebele.\n pgruebele@cox.net\n\n Some improvements were made by:\n 1) Martin Rufli - increased chance of correct corner matching\n Rufli, M., Scaramuzza, D., and Siegwart, R. (2008),\n Automatic Detection of Checkerboards on Blurred and Distorted Images,\n Proceedings of the IEEE/RSJ International Conference on\n Intelligent Robots and Systems (IROS 2008), Nice, France, September 2008.\n 2) Lionel Heng - post-detection checks and better thresholding\n\n \\************************************************************************************/\n\n //int bestDilation = -1;\n const int minDilations = 0;\n const int maxDilations = 7;\n\n std::vector outputQuadGroup;\n\n if (image.depth() != CV_8U || image.channels() == 2)\n {\n return false;\n }\n\n if (patternSize.width < 2 || patternSize.height < 2)\n {\n return false;\n }\n\n if (patternSize.width > 127 || patternSize.height > 127)\n {\n return false;\n }\n\n cv::Mat img = image;\n\n // Image histogram normalization and\n // BGR to Grayscale image conversion (if applicable)\n // MARTIN: Set to \"false\"\n if (image.channels() != 1 || (flags & cv::CALIB_CB_NORMALIZE_IMAGE))\n {\n cv::Mat norm_img(image.rows, image.cols, CV_8UC1);\n\n if (image.channels() != 1)\n {\n cv::cvtColor(image, norm_img, cv::COLOR_BGR2GRAY);\n img = norm_img;\n }\n\n if (flags & cv::CALIB_CB_NORMALIZE_IMAGE)\n {\n cv::equalizeHist(image, norm_img);\n img = norm_img;\n }\n }\n\n if (flags & cv::CALIB_CB_FAST_CHECK)\n {\n if (!checkChessboard(img, patternSize))\n {\n return false;\n }\n }\n\n // PART 1: FIND LARGEST PATTERN\n //-----------------------------------------------------------------------\n // Checker patterns are tried to be found by dilating the background and\n // then applying a canny edge finder on the closed contours (checkers).\n // Try one dilation run, but if the pattern is not found, repeat until\n // max_dilations is reached.\n\n int prevSqrSize = 0;\n bool found = false;\n std::vector outputCorners;\n\n for (int k = 0; k < 6; ++k)\n {\n for (int dilations = minDilations; dilations <= maxDilations; ++dilations)\n {\n if (found)\n {\n break;\n }\n\n cv::Mat thresh_img;\n\n // convert the input grayscale image to binary (black-n-white)\n if (flags & cv::CALIB_CB_ADAPTIVE_THRESH)\n {\n int blockSize = lround(prevSqrSize == 0 ?\n std::min(img.cols,img.rows)*(k%2 == 0 ? 0.2 : 0.1): prevSqrSize*2)|1;\n\n // convert to binary\n cv::adaptiveThreshold(img, thresh_img, 255, cv::ADAPTIVE_THRESH_MEAN_C, cv::THRESH_BINARY, blockSize, (k/2)*5);\n }\n else\n {\n // empiric threshold level\n double mean = (cv::mean(img))[0];\n int thresh_level = lround(mean - 10);\n thresh_level = std::max(thresh_level, 10);\n\n cv::threshold(img, thresh_img, thresh_level, 255, cv::THRESH_BINARY);\n }\n\n // MARTIN's Code\n // Use both a rectangular and a cross kernel. In this way, a more\n // homogeneous dilation is performed, which is crucial for small,\n // distorted checkers. Use the CROSS kernel first, since its action\n // on the image is more subtle\n cv::Mat kernel1 = cv::getStructuringElement(cv::MORPH_CROSS, cv::Size(3,3), cv::Point(1,1));\n cv::Mat kernel2 = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3,3), cv::Point(1,1));\n\n if (dilations >= 1)\n cv::dilate(thresh_img, thresh_img, kernel1);\n if (dilations >= 2)\n cv::dilate(thresh_img, thresh_img, kernel2);\n if (dilations >= 3)\n cv::dilate(thresh_img, thresh_img, kernel1);\n if (dilations >= 4)\n cv::dilate(thresh_img, thresh_img, kernel2);\n if (dilations >= 5)\n cv::dilate(thresh_img, thresh_img, kernel1);\n if (dilations >= 6)\n cv::dilate(thresh_img, thresh_img, kernel2);\n\n // In order to find rectangles that go to the edge, we draw a white\n // line around the image edge. Otherwise FindContours will miss those\n // clipped rectangle contours. The border color will be the image mean,\n // because otherwise we risk screwing up filters like cvSmooth()\n cv::rectangle(thresh_img, cv::Point(0,0),\n cv::Point(thresh_img.cols - 1, thresh_img.rows - 1),\n CV_RGB(255,255,255), 3, 8);\n\n // Generate quadrangles in the following function\n std::vector quads;\n\n generateQuads(quads, thresh_img, flags, dilations, true);\n if (quads.empty())\n {\n continue;\n }\n\n // The following function finds and assigns neighbor quads to every\n // quadrangle in the immediate vicinity fulfilling certain\n // prerequisites\n findQuadNeighbors(quads, dilations);\n\n // The connected quads will be organized in groups. The following loop\n // increases a \"group_idx\" identifier.\n // The function \"findConnectedQuads assigns all connected quads\n // a unique group ID.\n // If more quadrangles were assigned to a given group (i.e. connected)\n // than are expected by the input variable \"patternSize\", the\n // function \"cleanFoundConnectedQuads\" erases the surplus\n // quadrangles by minimizing the convex hull of the remaining pattern.\n\n for (int group_idx = 0; ; ++group_idx)\n {\n std::vector quadGroup;\n\n findConnectedQuads(quads, quadGroup, group_idx, dilations);\n\n if (quadGroup.empty())\n {\n break;\n }\n\n cleanFoundConnectedQuads(quadGroup, patternSize);\n\n // The following function labels all corners of every quad\n // with a row and column entry.\n // \"count\" specifies the number of found quads in \"quad_group\"\n // with group identifier \"group_idx\"\n // The last parameter is set to \"true\", because this is the\n // first function call and some initializations need to be\n // made.\n labelQuadGroup(quadGroup, patternSize, true);\n\n found = checkQuadGroup(quadGroup, outputCorners, patternSize);\n\n float sumDist = 0;\n int total = 0;\n\n for (int i = 0; i < (int)outputCorners.size(); ++i)\n {\n int ni = 0;\n float avgi = outputCorners.at(i)->meanDist(ni);\n sumDist += avgi * ni;\n total += ni;\n }\n prevSqrSize = lround(sumDist / std::max(total, 1));\n\n if (found && !checkBoardMonotony(outputCorners, patternSize))\n {\n found = false;\n }\n }\n }\n }\n\n if (!found)\n {\n return false;\n }\n else\n {\n corners.clear();\n corners.reserve(outputCorners.size());\n for (size_t i = 0; i < outputCorners.size(); ++i)\n {\n corners.push_back(outputCorners.at(i)->pt);\n }\n\n cv::cornerSubPix(image, corners, cv::Size(11, 11), cv::Size(-1,-1),\n cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::MAX_ITER, 30, 0.1));\n\n return true;\n }\n}\n\n//===========================================================================\n// ERASE OVERHEAD\n//===========================================================================\n// If we found too many connected quads, remove those which probably do not\n// belong.\nvoid\nChessboard::cleanFoundConnectedQuads(std::vector& quadGroup,\n cv::Size patternSize)\n{\n cv::Point2f center(0.0f, 0.0f);\n\n // Number of quads this pattern should contain\n int count = ((patternSize.width + 1)*(patternSize.height + 1) + 1)/2;\n\n // If we have more quadrangles than we should, try to eliminate duplicates\n // or ones which don't belong to the pattern rectangle. Else go to the end\n // of the function\n if ((int)quadGroup.size() <= count)\n {\n return;\n }\n\n // Create an array of quadrangle centers\n std::vector centers;\n centers.resize(quadGroup.size());\n\n for (size_t i = 0; i < quadGroup.size(); ++i)\n {\n cv::Point2f ci(0.0f, 0.0f);\n ChessboardQuadPtr& q = quadGroup[i];\n\n for (int j = 0; j < 4; ++j)\n {\n ci += q->corners[j]->pt;\n }\n\n ci *= 0.25f;\n\n // Centers(i), is the geometric center of quad(i)\n // Center, is the center of all found quads\n centers[i] = ci;\n\n center += ci;\n }\n\n center *= 1.0f / quadGroup.size();\n\n // If we have more quadrangles than we should, we try to eliminate bad\n // ones based on minimizing the bounding box. We iteratively remove the\n // point which reduces the size of the bounding box of the blobs the most\n // (since we want the rectangle to be as small as possible) remove the\n // quadrange that causes the biggest reduction in pattern size until we\n // have the correct number\n while ((int)quadGroup.size() > count)\n {\n double minBoxArea = DBL_MAX;\n int minBoxAreaIndex = -1;\n\n // For each point, calculate box area without that point\n for (size_t skip = 0; skip < quadGroup.size(); ++skip)\n {\n // get bounding rectangle\n cv::Point2f temp = centers[skip];\n centers[skip] = center;\n\n std::vector hull;\n cv::convexHull(centers, hull, true, true);\n centers[skip] = temp;\n\n double hull_area = fabs(cv::contourArea(hull));\n\n // remember smallest box area\n if (hull_area < minBoxArea)\n {\n minBoxArea = hull_area;\n minBoxAreaIndex = skip;\n }\n }\n\n ChessboardQuadPtr& q0 = quadGroup[minBoxAreaIndex];\n\n // remove any references to this quad as a neighbor\n for (size_t i = 0; i < quadGroup.size(); ++i)\n {\n ChessboardQuadPtr& q = quadGroup.at(i);\n for (int j = 0; j < 4; ++j)\n {\n if (q->neighbors[j] == q0)\n {\n q->neighbors[j].reset();\n q->count--;\n for (int k = 0; k < 4; ++k)\n {\n if (q0->neighbors[k] == q)\n {\n q0->neighbors[k].reset();\n q0->count--;\n break;\n }\n }\n break;\n }\n }\n }\n // remove the quad\n quadGroup.at(minBoxAreaIndex) = quadGroup.back();\n centers.at(minBoxAreaIndex) = centers.back();\n quadGroup.pop_back();\n centers.pop_back();\n }\n}\n\n//===========================================================================\n// FIND COONECTED QUADS\n//===========================================================================\nvoid\nChessboard::findConnectedQuads(std::vector& quads,\n std::vector& group,\n int group_idx, int dilation)\n{\n ChessboardQuadPtr q;\n\n // Scan the array for a first unlabeled quad\n for (size_t i = 0; i < quads.size(); ++i)\n {\n ChessboardQuadPtr& quad = quads.at(i);\n\n if (quad->count > 0 && quad->group_idx < 0)\n {\n q = quad;\n break;\n }\n }\n\n if (q.get() == 0)\n {\n return;\n }\n\n // Recursively find a group of connected quads starting from the seed quad\n\n std::vector stack;\n stack.push_back(q);\n\n group.push_back(q);\n q->group_idx = group_idx;\n\n while (!stack.empty())\n {\n q = stack.back();\n stack.pop_back();\n\n for (int i = 0; i < 4; ++i)\n {\n ChessboardQuadPtr& neighbor = q->neighbors[i];\n\n // If he neighbor exists and the neighbor has more than 0\n // neighbors and the neighbor has not been classified yet.\n if (neighbor.get() && neighbor->count > 0 && neighbor->group_idx < 0)\n {\n stack.push_back(neighbor);\n group.push_back(neighbor);\n neighbor->group_idx = group_idx;\n }\n }\n }\n}\n\nvoid\nChessboard::labelQuadGroup(std::vector& quadGroup,\n cv::Size patternSize, bool firstRun)\n{\n // If this is the first function call, a seed quad needs to be selected\n if (firstRun)\n {\n // Search for the (first) quad with the maximum number of neighbors\n // (usually 4). This will be our starting point.\n int mark = -1;\n int maxNeighborCount = 0;\n for (size_t i = 0; i < quadGroup.size(); ++i)\n {\n ChessboardQuadPtr& q = quadGroup.at(i);\n if (q->count > maxNeighborCount)\n {\n mark = i;\n maxNeighborCount = q->count;\n\n if (maxNeighborCount == 4)\n {\n break;\n }\n }\n }\n\n // Mark the starting quad's (per definition) upper left corner with\n //(0,0) and then proceed clockwise\n // The following labeling sequence enshures a \"right coordinate system\"\n ChessboardQuadPtr& q = quadGroup.at(mark);\n\n q->labeled = true;\n\n q->corners[0]->row = 0;\n q->corners[0]->column = 0;\n q->corners[1]->row = 0;\n q->corners[1]->column = 1;\n q->corners[2]->row = 1;\n q->corners[2]->column = 1;\n q->corners[3]->row = 1;\n q->corners[3]->column = 0;\n }\n\n\n // Mark all other corners with their respective row and column\n bool flagChanged = true;\n while (flagChanged)\n {\n // First reset the flag to \"false\"\n flagChanged = false;\n\n // Go through all quads top down is faster, since unlabeled quads will\n // be inserted at the end of the list\n for (int i = quadGroup.size() - 1; i >= 0; --i)\n {\n ChessboardQuadPtr& quad = quadGroup.at(i);\n\n // Check whether quad \"i\" has been labeled already\n if (!quad->labeled)\n {\n // Check its neighbors, whether some of them have been labeled\n // already\n for (int j = 0; j < 4; j++)\n {\n // Check whether the neighbor exists (i.e. is not the NULL\n // pointer)\n if (quad->neighbors[j])\n {\n ChessboardQuadPtr& quadNeighbor = quad->neighbors[j];\n\n // Only proceed, if neighbor \"j\" was labeled\n if (quadNeighbor->labeled)\n {\n // For every quad it could happen to pass here\n // multiple times. We therefore \"break\" later.\n // Check whitch of the neighbors corners is\n // connected to the current quad\n int connectedNeighborCornerId = -1;\n for (int k = 0; k < 4; ++k)\n {\n if (quadNeighbor->neighbors[k] == quad)\n {\n connectedNeighborCornerId = k;\n\n // there is only one, therefore\n break;\n }\n }\n\n\n // For the following calculations we need the row\n // and column of the connected neighbor corner and\n // all other corners of the connected quad \"j\",\n // clockwise (CW)\n ChessboardCornerPtr& conCorner = quadNeighbor->corners[connectedNeighborCornerId];\n ChessboardCornerPtr& conCornerCW1 = quadNeighbor->corners[(connectedNeighborCornerId+1)%4];\n ChessboardCornerPtr& conCornerCW2 = quadNeighbor->corners[(connectedNeighborCornerId+2)%4];\n ChessboardCornerPtr& conCornerCW3 = quadNeighbor->corners[(connectedNeighborCornerId+3)%4];\n\n quad->corners[j]->row = conCorner->row;\n quad->corners[j]->column = conCorner->column;\n quad->corners[(j+1)%4]->row = conCorner->row - conCornerCW2->row + conCornerCW3->row;\n quad->corners[(j+1)%4]->column = conCorner->column - conCornerCW2->column + conCornerCW3->column;\n quad->corners[(j+2)%4]->row = conCorner->row + conCorner->row - conCornerCW2->row;\n quad->corners[(j+2)%4]->column = conCorner->column + conCorner->column - conCornerCW2->column;\n quad->corners[(j+3)%4]->row = conCorner->row - conCornerCW2->row + conCornerCW1->row;\n quad->corners[(j+3)%4]->column = conCorner->column - conCornerCW2->column + conCornerCW1->column;\n\n // Mark this quad as labeled\n quad->labeled = true;\n\n // Changes have taken place, set the flag\n flagChanged = true;\n\n // once is enough!\n break;\n }\n }\n }\n }\n }\n }\n\n\n // All corners are marked with row and column\n // Record the minimal and maximal row and column indices\n // It is unlikely that more than 8bit checkers are used per dimension, if there are\n // an error would have been thrown at the beginning of \"cvFindChessboardCorners2\"\n int min_row = 127;\n int max_row = -127;\n int min_column = 127;\n int max_column = -127;\n\n for (int i = 0; i < (int)quadGroup.size(); ++i)\n {\n ChessboardQuadPtr& q = quadGroup.at(i);\n\n for (int j = 0; j < 4; ++j)\n {\n ChessboardCornerPtr& c = q->corners[j];\n\n if (c->row > max_row)\n {\n max_row = c->row;\n }\n if (c->row < min_row)\n {\n min_row = c->row;\n }\n if (c->column > max_column)\n {\n max_column = c->column;\n }\n if (c->column < min_column)\n {\n min_column = c->column;\n }\n }\n }\n\n // Label all internal corners with \"needsNeighbor\" = false\n // Label all external corners with \"needsNeighbor\" = true,\n // except if in a given dimension the pattern size is reached\n for (int i = min_row; i <= max_row; ++i)\n {\n for (int j = min_column; j <= max_column; ++j)\n {\n // A flag that indicates, whether a row/column combination is\n // executed multiple times\n bool flag = false;\n\n // Remember corner and quad\n int cornerID;\n int quadID;\n\n for (int k = 0; k < (int)quadGroup.size(); ++k)\n {\n ChessboardQuadPtr& q = quadGroup.at(k);\n\n for (int l = 0; l < 4; ++l)\n {\n if ((q->corners[l]->row == i) && (q->corners[l]->column == j))\n {\n if (flag)\n {\n // Passed at least twice through here\n q->corners[l]->needsNeighbor = false;\n quadGroup[quadID]->corners[cornerID]->needsNeighbor = false;\n }\n else\n {\n // Mark with needs a neighbor, but note the\n // address\n q->corners[l]->needsNeighbor = true;\n cornerID = l;\n quadID = k;\n }\n\n // set the flag to true\n flag = true;\n }\n }\n }\n }\n }\n\n // Complete Linking:\n // sometimes not all corners were properly linked in \"findQuadNeighbors\",\n // but after labeling each corner with its respective row and column, it is\n // possible to match them anyway.\n for (int i = min_row; i <= max_row; ++i)\n {\n for (int j = min_column; j <= max_column; ++j)\n {\n // the following \"number\" indicates the number of corners which\n // correspond to the given (i,j) value\n // 1 is a border corner or a conrer which still needs a neighbor\n // 2 is a fully connected internal corner\n // >2 something went wrong during labeling, report a warning\n int number = 1;\n\n // remember corner and quad\n int cornerID;\n int quadID;\n\n for (int k = 0; k < (int)quadGroup.size(); ++k)\n {\n ChessboardQuadPtr& q = quadGroup.at(k);\n\n for (int l = 0; l < 4; ++l)\n {\n if ((q->corners[l]->row == i) && (q->corners[l]->column == j))\n {\n\n if (number == 1)\n {\n // First corner, note its ID\n cornerID = l;\n quadID = k;\n }\n\n else if (number == 2)\n {\n // Second corner, check wheter this and the\n // first one have equal coordinates, else\n // interpolate\n cv::Point2f delta = q->corners[l]->pt - quadGroup[quadID]->corners[cornerID]->pt;\n\n if (delta.x != 0.0f || delta.y != 0.0f)\n {\n // Interpolate\n q->corners[l]->pt -= delta * 0.5f;\n\n quadGroup[quadID]->corners[cornerID]->pt += delta * 0.5f;\n }\n }\n else if (number > 2)\n {\n // Something went wrong during row/column labeling\n // Report a Warning\n // ->Implemented in the function \"mrWriteCorners\"\n }\n\n // increase the number by one\n ++number;\n }\n }\n }\n }\n }\n\n\n // Bordercorners don't need any neighbors, if the pattern size in the\n // respective direction is reached\n // The only time we can make shure that the target pattern size is reached in a given\n // dimension, is when the larger side has reached the target size in the maximal\n // direction, or if the larger side is larger than the smaller target size and the\n // smaller side equals the smaller target size\n int largerDimPattern = std::max(patternSize.height,patternSize.width);\n int smallerDimPattern = std::min(patternSize.height,patternSize.width);\n bool flagSmallerDim1 = false;\n bool flagSmallerDim2 = false;\n\n if ((largerDimPattern + 1) == max_column - min_column)\n {\n flagSmallerDim1 = true;\n // We found out that in the column direction the target pattern size is reached\n // Therefore border column corners do not need a neighbor anymore\n // Go through all corners\n for (int k = 0; k < (int)quadGroup.size(); ++k)\n {\n ChessboardQuadPtr& q = quadGroup.at(k);\n\n for (int l = 0; l < 4; ++l)\n {\n ChessboardCornerPtr& c = q->corners[l];\n\n if (c->column == min_column || c->column == max_column)\n {\n // Needs no neighbor anymore\n c->needsNeighbor = false;\n }\n }\n }\n }\n\n if ((largerDimPattern + 1) == max_row - min_row)\n {\n flagSmallerDim2 = true;\n // We found out that in the column direction the target pattern size is reached\n // Therefore border column corners do not need a neighbor anymore\n // Go through all corners\n for (int k = 0; k < (int)quadGroup.size(); ++k)\n {\n ChessboardQuadPtr& q = quadGroup.at(k);\n\n for (int l = 0; l < 4; ++l)\n {\n ChessboardCornerPtr& c = q->corners[l];\n\n if (c->row == min_row || c->row == max_row)\n {\n // Needs no neighbor anymore\n c->needsNeighbor = false;\n }\n }\n }\n }\n\n\n // Check the two flags:\n // - If one is true and the other false, then the pattern target\n // size was reached in in one direction -> We can check, whether the target\n // pattern size is also reached in the other direction\n // - If both are set to true, then we deal with a square board -> do nothing\n // - If both are set to false -> There is a possibility that the larger side is\n // larger than the smaller target size -> Check and if true, then check whether\n // the other side has the same size as the smaller target size\n if ((flagSmallerDim1 == false && flagSmallerDim2 == true))\n {\n // Larger target pattern size is in row direction, check wheter smaller target\n // pattern size is reached in column direction\n if ((smallerDimPattern + 1) == max_column - min_column)\n {\n for (int k = 0; k < (int)quadGroup.size(); ++k)\n {\n ChessboardQuadPtr& q = quadGroup.at(k);\n\n for (int l = 0; l < 4; ++l)\n {\n ChessboardCornerPtr& c = q->corners[l];\n\n if (c->column == min_column || c->column == max_column)\n {\n // Needs no neighbor anymore\n c->needsNeighbor = false;\n }\n }\n }\n }\n }\n\n if ((flagSmallerDim1 == true && flagSmallerDim2 == false))\n {\n // Larger target pattern size is in column direction, check wheter smaller target\n // pattern size is reached in row direction\n if ((smallerDimPattern + 1) == max_row - min_row)\n {\n for (int k = 0; k < (int)quadGroup.size(); ++k)\n {\n ChessboardQuadPtr& q = quadGroup.at(k);\n\n for (int l = 0; l < 4; ++l)\n {\n ChessboardCornerPtr& c = q->corners[l];\n\n if (c->row == min_row || c->row == max_row)\n {\n // Needs no neighbor anymore\n c->needsNeighbor = false;\n }\n }\n }\n }\n }\n\n if ((flagSmallerDim1 == false && flagSmallerDim2 == false) && smallerDimPattern + 1 < max_column - min_column)\n {\n // Larger target pattern size is in column direction, check wheter smaller target\n // pattern size is reached in row direction\n if ((smallerDimPattern + 1) == max_row - min_row)\n {\n for (int k = 0; k < (int)quadGroup.size(); ++k)\n {\n ChessboardQuadPtr& q = quadGroup.at(k);\n\n for (int l = 0; l < 4; ++l)\n {\n ChessboardCornerPtr& c = q->corners[l];\n\n if (c->row == min_row || c->row == max_row)\n {\n // Needs no neighbor anymore\n c->needsNeighbor = false;\n }\n }\n }\n }\n }\n\n if ((flagSmallerDim1 == false && flagSmallerDim2 == false) && smallerDimPattern + 1 < max_row - min_row)\n {\n // Larger target pattern size is in row direction, check wheter smaller target\n // pattern size is reached in column direction\n if ((smallerDimPattern + 1) == max_column - min_column)\n {\n for (int k = 0; k < (int)quadGroup.size(); ++k)\n {\n ChessboardQuadPtr& q = quadGroup.at(k);\n\n for (int l = 0; l < 4; ++l)\n {\n ChessboardCornerPtr& c = q->corners[l];\n\n if (c->column == min_column || c->column == max_column)\n {\n // Needs no neighbor anymore\n c->needsNeighbor = false;\n }\n }\n }\n }\n }\n}\n\n//===========================================================================\n// GIVE A GROUP IDX\n//===========================================================================\nvoid\nChessboard::findQuadNeighbors(std::vector& quads, int dilation)\n{\n // Thresh dilation is used to counter the effect of dilation on the\n // distance between 2 neighboring corners. Since the distance below is\n // computed as its square, we do here the same. Additionally, we take the\n // conservative assumption that dilation was performed using the 3x3 CROSS\n // kernel, which coresponds to the 4-neighborhood.\n const float thresh_dilation = (float)(2*dilation+3)*(2*dilation+3)*2; // the \"*2\" is for the x and y component\n // the \"3\" is for initial corner mismatch\n\n // Find quad neighbors\n for (size_t idx = 0; idx < quads.size(); ++idx)\n {\n ChessboardQuadPtr& curQuad = quads.at(idx);\n\n // Go through all quadrangles and label them in groups\n // For each corner of this quadrangle\n for (int i = 0; i < 4; ++i)\n {\n float minDist = FLT_MAX;\n int closestCornerIdx = -1;\n ChessboardQuadPtr closestQuad;\n\n if (curQuad->neighbors[i])\n {\n continue;\n }\n\n cv::Point2f pt = curQuad->corners[i]->pt;\n\n // Find the closest corner in all other quadrangles\n for (size_t k = 0; k < quads.size(); ++k)\n {\n if (k == idx)\n {\n continue;\n }\n\n ChessboardQuadPtr& quad = quads.at(k);\n\n for (int j = 0; j < 4; ++j)\n {\n // If it already has a neighbor\n if (quad->neighbors[j])\n {\n continue;\n }\n\n cv::Point2f dp = pt - quad->corners[j]->pt;\n float dist = dp.dot(dp);\n\n // The following \"if\" checks, whether \"dist\" is the\n // shortest so far and smaller than the smallest\n // edge length of the current and target quads\n if (dist < minDist &&\n dist <= (curQuad->edge_len + thresh_dilation) &&\n dist <= (quad->edge_len + thresh_dilation) )\n {\n // Check whether conditions are fulfilled\n if (matchCorners(curQuad, i, quad, j))\n {\n closestCornerIdx = j;\n closestQuad = quad;\n minDist = dist;\n }\n }\n }\n }\n\n // Have we found a matching corner point?\n if (closestCornerIdx >= 0 && minDist < FLT_MAX)\n {\n ChessboardCornerPtr closestCorner = closestQuad->corners[closestCornerIdx];\n\n // Make sure that the closest quad does not have the current\n // quad as neighbor already\n bool valid = true;\n for (int j = 0; j < 4; ++j)\n {\n if (closestQuad->neighbors[j] == curQuad)\n {\n valid = false;\n break;\n }\n }\n if (!valid)\n {\n continue;\n }\n\n // We've found one more corner - remember it\n closestCorner->pt = (pt + closestCorner->pt) * 0.5f;\n\n curQuad->count++;\n curQuad->neighbors[i] = closestQuad;\n curQuad->corners[i] = closestCorner;\n\n closestQuad->count++;\n closestQuad->neighbors[closestCornerIdx] = curQuad;\n closestQuad->corners[closestCornerIdx] = closestCorner;\n }\n }\n }\n}\n\n\n\n//===========================================================================\n// AUGMENT PATTERN WITH ADDITIONAL QUADS\n//===========================================================================\n// The first part of the function is basically a copy of\n// \"findQuadNeighbors\"\n// The comparisons between two points and two lines could be computed in their\n// own function\nint\nChessboard::augmentBestRun(std::vector& candidateQuads, int candidateDilation,\n std::vector& existingQuads, int existingDilation)\n{\n // thresh dilation is used to counter the effect of dilation on the\n // distance between 2 neighboring corners. Since the distance below is\n // computed as its square, we do here the same. Additionally, we take the\n // conservative assumption that dilation was performed using the 3x3 CROSS\n // kernel, which coresponds to the 4-neighborhood.\n const float thresh_dilation = (2*candidateDilation+3)*(2*existingDilation+3)*2; // the \"*2\" is for the x and y component\n\n // Search all old quads which have a neighbor that needs to be linked\n for (size_t idx = 0; idx < existingQuads.size(); ++idx)\n {\n ChessboardQuadPtr& curQuad = existingQuads.at(idx);\n\n // For each corner of this quadrangle\n for (int i = 0; i < 4; ++i)\n {\n float minDist = FLT_MAX;\n int closestCornerIdx = -1;\n ChessboardQuadPtr closestQuad;\n\n // If curQuad corner[i] is already linked, continue\n if (!curQuad->corners[i]->needsNeighbor)\n {\n continue;\n }\n\n cv::Point2f pt = curQuad->corners[i]->pt;\n\n // Look for a match in all candidateQuads' corners\n for (size_t k = 0; k < candidateQuads.size(); ++k)\n {\n ChessboardQuadPtr& candidateQuad = candidateQuads.at(k);\n\n // Only look at unlabeled new quads\n if (candidateQuad->labeled)\n {\n continue;\n }\n\n for (int j = 0; j < 4; ++j)\n {\n // Only proceed if they are less than dist away from each\n // other\n cv::Point2f dp = pt - candidateQuad->corners[j]->pt;\n float dist = dp.dot(dp);\n\n if ((dist < minDist) &&\n dist <= (curQuad->edge_len + thresh_dilation) &&\n dist <= (candidateQuad->edge_len + thresh_dilation))\n {\n if (matchCorners(curQuad, i, candidateQuad, j))\n {\n closestCornerIdx = j;\n closestQuad = candidateQuad;\n minDist = dist;\n }\n }\n }\n }\n\n // Have we found a matching corner point?\n if (closestCornerIdx >= 0 && minDist < FLT_MAX)\n {\n ChessboardCornerPtr closestCorner = closestQuad->corners[closestCornerIdx];\n closestCorner->pt = (pt + closestCorner->pt) * 0.5f;\n\n // We've found one more corner - remember it\n // ATTENTION: write the corner x and y coordinates separately,\n // else the crucial row/column entries will be overwritten !!!\n curQuad->corners[i]->pt = closestCorner->pt;\n curQuad->neighbors[i] = closestQuad;\n closestQuad->corners[closestCornerIdx]->pt = closestCorner->pt;\n\n // Label closest quad as labeled. In this way we exclude it\n // being considered again during the next loop iteration\n closestQuad->labeled = true;\n\n // We have a new member of the final pattern, copy it over\n ChessboardQuadPtr newQuad(new ChessboardQuad);\n newQuad->count = 1;\n newQuad->edge_len = closestQuad->edge_len;\n newQuad->group_idx = curQuad->group_idx; //the same as the current quad\n newQuad->labeled = false; //do it right afterwards\n\n curQuad->neighbors[i] = newQuad;\n\n // We only know one neighbor for sure\n newQuad->neighbors[closestCornerIdx] = curQuad;\n\n for (int j = 0; j < 4; j++)\n {\n newQuad->corners[j].reset(new ChessboardCorner);\n newQuad->corners[j]->pt = closestQuad->corners[j]->pt;\n }\n\n existingQuads.push_back(newQuad);\n\n // Start the function again\n return -1;\n }\n }\n }\n\n // Finished, don't start the function again\n return 1;\n}\n\n\n\n//===========================================================================\n// GENERATE QUADRANGLES\n//===========================================================================\nvoid\nChessboard::generateQuads(std::vector& quads,\n cv::Mat& image, int flags,\n int dilation, bool firstRun)\n{\n // Empirical lower bound for the size of allowable quadrangles.\n // MARTIN, modified: Added \"*0.1\" in order to find smaller quads.\n int minSize = lround(image.cols * image.rows * .03 * 0.01 * 0.92 * 0.1);\n\n std::vector< std::vector > contours;\n std::vector hierarchy;\n\n // Initialize contour retrieving routine\n cv::findContours(image, contours, hierarchy, cv::RETR_CCOMP, cv::CHAIN_APPROX_SIMPLE);\n\n std::vector< std::vector > quadContours;\n\n for (size_t i = 0; i < contours.size(); ++i)\n {\n // Reject contours with a too small perimeter and contours which are\n // completely surrounded by another contour\n // MARTIN: If this function is called during PART 1, then the parameter \"first run\"\n // is set to \"true\". This guarantees, that only \"nice\" squares are detected.\n // During PART 2, we allow the polygonal approximation function below to\n // approximate more freely, which can result in recognized \"squares\" that are in\n // reality multiple blurred and sticked together squares.\n std::vector& contour = contours.at(i);\n\n if (hierarchy[i][3] == -1 || cv::contourArea(contour) < minSize)\n {\n continue;\n }\n\n int min_approx_level = 1, max_approx_level;\n if (firstRun)\n {\n max_approx_level = 3;\n }\n else\n {\n max_approx_level = MAX_CONTOUR_APPROX;\n }\n\n std::vector approxContour;\n for (int approx_level = min_approx_level; approx_level <= max_approx_level; approx_level++)\n {\n cv::approxPolyDP(contour, approxContour, approx_level, true);\n\n if (approxContour.size() == 4)\n {\n break;\n }\n }\n\n // Reject non-quadrangles\n if (approxContour.size() == 4 && cv::isContourConvex(approxContour))\n {\n double p = cv::arcLength(approxContour, true);\n double area = cv::contourArea(approxContour);\n\n cv::Point pt[4];\n for (int i = 0; i < 4; i++)\n {\n pt[i] = approxContour[i];\n }\n\n cv::Point dp = pt[0] - pt[2];\n double d1 = sqrt(dp.dot(dp));\n\n dp = pt[1] - pt[3];\n double d2 = sqrt(dp.dot(dp));\n\n // PHILIPG: Only accept those quadrangles which are more\n // square than rectangular and which are big enough\n dp = pt[0] - pt[1];\n double d3 = sqrt(dp.dot(dp));\n dp = pt[1] - pt[2];\n double d4 = sqrt(dp.dot(dp));\n\n if (!(flags & cv::CALIB_CB_FILTER_QUADS) ||\n (d3*4 > d4 && d4*4 > d3 && d3*d4 < area*1.5 && area > minSize &&\n d1 >= 0.15 * p && d2 >= 0.15 * p))\n {\n quadContours.push_back(approxContour);\n }\n }\n }\n\n // Allocate quad & corner buffers\n quads.resize(quadContours.size());\n\n // Create array of quads structures\n for (size_t idx = 0; idx < quadContours.size(); ++idx)\n {\n ChessboardQuadPtr& q = quads.at(idx);\n std::vector& contour = quadContours.at(idx);\n\n // Reset group ID\n q.reset(new ChessboardQuad);\n assert(contour.size() == 4);\n\n for (int i = 0; i < 4; ++i)\n {\n cv::Point2f pt = contour.at(i);\n ChessboardCornerPtr corner(new ChessboardCorner);\n corner->pt = pt;\n q->corners[i] = corner;\n }\n\n for (int i = 0; i < 4; ++i)\n {\n cv::Point2f dp = q->corners[i]->pt - q->corners[(i+1)&3]->pt;\n float d = dp.dot(dp);\n if (q->edge_len > d)\n {\n q->edge_len = d;\n }\n }\n }\n}\n\nbool\nChessboard::checkQuadGroup(std::vector& quads,\n std::vector& corners,\n cv::Size patternSize)\n{\n // Initialize\n bool flagRow = false;\n bool flagColumn = false;\n int height = -1;\n int width = -1;\n\n // Compute the minimum and maximum row / column ID\n // (it is unlikely that more than 8bit checkers are used per dimension)\n int min_row = 127;\n int max_row = -127;\n int min_col = 127;\n int max_col = -127;\n\n for (size_t i = 0; i < quads.size(); ++i)\n {\n ChessboardQuadPtr& q = quads.at(i);\n\n for (int j = 0; j < 4; ++j)\n {\n ChessboardCornerPtr& c = q->corners[j];\n\n if (c->row > max_row)\n {\n max_row = c->row;\n }\n if (c->row < min_row)\n {\n min_row = c->row;\n }\n if (c->column > max_col)\n {\n max_col = c->column;\n }\n if (c->column < min_col)\n {\n min_col = c->column;\n }\n }\n }\n\n // If in a given direction the target pattern size is reached, we know exactly how\n // the checkerboard is oriented.\n // Else we need to prepare enough \"dummy\" corners for the worst case.\n for (size_t i = 0; i < quads.size(); ++i)\n {\n ChessboardQuadPtr& q = quads.at(i);\n\n for (int j = 0; j < 4; ++j)\n {\n ChessboardCornerPtr& c = q->corners[j];\n\n if (c->column == max_col && c->row != min_row && c->row != max_row && !c->needsNeighbor)\n {\n flagColumn = true;\n }\n if (c->row == max_row && c->column != min_col && c->column != max_col && !c->needsNeighbor)\n {\n flagRow = true;\n }\n }\n }\n\n if (flagColumn)\n {\n if (max_col - min_col == patternSize.width + 1)\n {\n width = patternSize.width;\n height = patternSize.height;\n }\n else\n {\n width = patternSize.height;\n height = patternSize.width;\n }\n }\n else if (flagRow)\n {\n if (max_row - min_row == patternSize.width + 1)\n {\n height = patternSize.width;\n width = patternSize.height;\n }\n else\n {\n height = patternSize.height;\n width = patternSize.width;\n }\n }\n else\n {\n // If target pattern size is not reached in at least one of the two\n // directions, then we do not know where the remaining corners are\n // located. Account for this.\n width = std::max(patternSize.width, patternSize.height);\n height = std::max(patternSize.width, patternSize.height);\n }\n\n ++min_row;\n ++min_col;\n max_row = min_row + height - 1;\n max_col = min_col + width - 1;\n\n corners.clear();\n\n int linkedBorderCorners = 0;\n\n // Write the corners in increasing order to the output file\n for (int i = min_row; i <= max_row; ++i)\n {\n for (int j = min_col; j <= max_col; ++j)\n {\n // Reset the iterator\n int iter = 1;\n\n for (int k = 0; k < (int)quads.size(); ++k)\n {\n ChessboardQuadPtr& quad = quads.at(k);\n\n for (int l = 0; l < 4; ++l)\n {\n ChessboardCornerPtr& c = quad->corners[l];\n\n if (c->row == i && c->column == j)\n {\n bool boardEdge = false;\n if (i == min_row || i == max_row ||\n j == min_col || j == max_col)\n {\n boardEdge = true;\n }\n\n if ((iter == 1 && boardEdge) || (iter == 2 && !boardEdge))\n {\n // The respective row and column have been found\n corners.push_back(quad->corners[l]);\n }\n\n if (iter == 2 && boardEdge)\n {\n ++linkedBorderCorners;\n }\n\n // If the iterator is larger than two, this means that more than\n // two corners have the same row / column entries. Then some\n // linking errors must have occured and we should not use the found\n // pattern\n if (iter > 2)\n {\n return false;\n }\n\n iter++;\n }\n }\n }\n }\n }\n\n if ((int)corners.size() != patternSize.width * patternSize.height ||\n linkedBorderCorners < (patternSize.width * 2 + patternSize.height * 2 - 2) * 0.75f)\n {\n return false;\n }\n\n // check that no corners lie at image boundary\n float border = 5.0f;\n for (int i = 0; i < (int)corners.size(); ++i)\n {\n ChessboardCornerPtr& c = corners.at(i);\n\n if (c->pt.x < border || c->pt.x > mImage.cols - border ||\n c->pt.y < border || c->pt.y > mImage.rows - border)\n {\n return false;\n }\n }\n\n // check if we need to transpose the board\n if (width != patternSize.width)\n {\n std::swap(width, height);\n\n std::vector outputCorners;\n outputCorners.resize(corners.size());\n\n for (int i = 0; i < height; ++i)\n {\n for (int j = 0; j < width; ++j)\n {\n outputCorners.at(i * width + j) = corners.at(j * height + i);\n }\n }\n\n corners = outputCorners;\n }\n\n // check if we need to revert the order in each row\n cv::Point2f p0 = corners.at(0)->pt;\n cv::Point2f p1 = corners.at(width-1)->pt;\n cv::Point2f p2 = corners.at(width)->pt;\n\n if ((p1 - p0).cross(p2 - p0) < 0.0f)\n {\n for (int i = 0; i < height; ++i)\n {\n for (int j = 0; j < width / 2; ++j)\n {\n std::swap(corners.at(i * width + j), corners.at(i * width + width - j - 1));\n }\n }\n }\n\n p0 = corners.at(0)->pt;\n p2 = corners.at(width)->pt;\n\n // check if we need to rotate the board\n if (p2.y < p0.y)\n {\n std::vector outputCorners;\n outputCorners.resize(corners.size());\n\n for (int i = 0; i < height; ++i)\n {\n for (int j = 0; j < width; ++j)\n {\n outputCorners.at(i * width + j) = corners.at((height - i - 1) * width + width - j - 1);\n }\n }\n\n corners = outputCorners;\n }\n\n return true;\n}\n\nvoid\nChessboard::getQuadrangleHypotheses(const std::vector< std::vector >& contours,\n std::vector< std::pair >& quads,\n int classId) const\n{\n const float minAspectRatio = 0.2f;\n const float maxAspectRatio = 5.0f;\n const float minBoxSize = 10.0f;\n\n for (size_t i = 0; i < contours.size(); ++i)\n {\n cv::RotatedRect box = cv::minAreaRect(contours.at(i));\n float boxSize = std::max(box.size.width, box.size.height);\n if (boxSize < minBoxSize)\n {\n continue;\n }\n\n float aspectRatio = box.size.width / std::max(box.size.height, 1.0f);\n\n if (aspectRatio < minAspectRatio || aspectRatio > maxAspectRatio)\n {\n continue;\n }\n\n quads.push_back(std::pair(boxSize, classId));\n }\n}\n\nbool less_pred(const std::pair& p1, const std::pair& p2)\n{\n return p1.first < p2.first;\n}\n\nvoid countClasses(const std::vector >& pairs, size_t idx1, size_t idx2, std::vector& counts)\n{\n counts.assign(2, 0);\n for (size_t i = idx1; i != idx2; ++i)\n {\n counts[pairs[i].second]++;\n }\n}\n\nbool\nChessboard::checkChessboard(const cv::Mat& image, cv::Size patternSize) const\n{\n const int erosionCount = 1;\n const float blackLevel = 20.f;\n const float whiteLevel = 130.f;\n const float blackWhiteGap = 70.f;\n\n cv::Mat white = image.clone();\n\n cv::Mat black = image.clone();\n\n cv::erode(white, white, cv::Mat(), cv::Point(-1,-1), erosionCount);\n cv::dilate(black, black, cv::Mat(), cv::Point(-1,-1), erosionCount);\n\n cv::Mat thresh(image.rows, image.cols, CV_8UC1);\n\n bool result = false;\n for (float threshLevel = blackLevel; threshLevel < whiteLevel && !result; threshLevel += 20.0f)\n {\n cv::threshold(white, thresh, threshLevel + blackWhiteGap, 255, cv::THRESH_BINARY);\n\n std::vector< std::vector > contours;\n std::vector hierarchy;\n\n // Initialize contour retrieving routine\n cv::findContours(thresh, contours, hierarchy, cv::RETR_CCOMP, cv::CHAIN_APPROX_SIMPLE);\n\n std::vector > quads;\n getQuadrangleHypotheses(contours, quads, 1);\n\n cv::threshold(black, thresh, threshLevel, 255, cv::THRESH_BINARY_INV);\n\n cv::findContours(thresh, contours, hierarchy, cv::RETR_CCOMP, cv::CHAIN_APPROX_SIMPLE);\n getQuadrangleHypotheses(contours, quads, 0);\n\n const size_t min_quads_count = patternSize.width * patternSize.height / 2;\n std::sort(quads.begin(), quads.end(), less_pred);\n\n // now check if there are many hypotheses with similar sizes\n // do this by floodfill-style algorithm\n const float sizeRelDev = 0.4f;\n\n for (size_t i = 0; i < quads.size(); ++i)\n {\n size_t j = i + 1;\n for (; j < quads.size(); ++j)\n {\n if (quads[j].first / quads[i].first > 1.0f + sizeRelDev)\n {\n break;\n }\n }\n\n if (j + 1 > min_quads_count + i)\n {\n // check the number of black and white squares\n std::vector counts;\n countClasses(quads, i, j, counts);\n const int blackCount = lroundf(ceilf(patternSize.width / 2.0f) * ceilf(patternSize.height / 2.0f));\n const int whiteCount = lroundf(floorf(patternSize.width / 2.0f) * floorf(patternSize.height / 2.0f));\n if (counts[0] < blackCount * 0.75f ||\n counts[1] < whiteCount * 0.75f)\n {\n continue;\n }\n result = true;\n break;\n }\n }\n }\n\n return result;\n}\n\nbool\nChessboard::checkBoardMonotony(std::vector& corners,\n cv::Size patternSize)\n{\n const float threshFactor = 0.2f;\n\n Spline splineXY, splineYX;\n splineXY.setLowBC(Spline::PARABOLIC_RUNOUT_BC);\n splineXY.setHighBC(Spline::PARABOLIC_RUNOUT_BC);\n splineYX.setLowBC(Spline::PARABOLIC_RUNOUT_BC);\n splineYX.setHighBC(Spline::PARABOLIC_RUNOUT_BC);\n\n // check if each row of corners approximates a cubic spline\n for (int i = 0; i < patternSize.height; ++i)\n {\n splineXY.clear();\n splineYX.clear();\n\n cv::Point2f p[3];\n p[0] = corners.at(i * patternSize.width)->pt;\n p[1] = corners.at(i * patternSize.width + patternSize.width / 2)->pt;\n p[2] = corners.at(i * patternSize.width + patternSize.width - 1)->pt;\n\n for (int j = 0; j < 3; ++j)\n {\n splineXY.addPoint(p[j].x, p[j].y);\n splineYX.addPoint(p[j].y, p[j].x);\n }\n\n for (int j = 1; j < patternSize.width - 1; ++j)\n {\n cv::Point2f& p_j = corners.at(i * patternSize.width + j)->pt;\n\n float thresh = std::numeric_limits::max();\n\n // up-neighbor\n if (i > 0)\n {\n cv::Point2f& neighbor = corners.at((i - 1) * patternSize.width + j)->pt;\n thresh = fminf(thresh, cv::norm(neighbor - p_j));\n }\n // down-neighbor\n if (i < patternSize.height - 1)\n {\n cv::Point2f& neighbor = corners.at((i + 1) * patternSize.width + j)->pt;\n thresh = fminf(thresh, cv::norm(neighbor - p_j));\n }\n // left-neighbor\n {\n cv::Point2f& neighbor = corners.at(i * patternSize.width + j - 1)->pt;\n thresh = fminf(thresh, cv::norm(neighbor - p_j));\n }\n // right-neighbor\n {\n cv::Point2f& neighbor = corners.at(i * patternSize.width + j + 1)->pt;\n thresh = fminf(thresh, cv::norm(neighbor - p_j));\n }\n\n thresh *= threshFactor;\n\n if (fminf(fabsf(splineXY(p_j.x) - p_j.y), fabsf(splineYX(p_j.y) - p_j.x)) > thresh)\n {\n return false;\n }\n }\n }\n\n // check if each column of corners approximates a cubic spline\n for (int j = 0; j < patternSize.width; ++j)\n {\n splineXY.clear();\n splineYX.clear();\n\n cv::Point2f p[3];\n p[0] = corners.at(j)->pt;\n p[1] = corners.at(patternSize.height / 2 * patternSize.width + j)->pt;\n p[2] = corners.at((patternSize.height - 1) * patternSize.width + j)->pt;\n\n for (int i = 0; i < 3; ++i)\n {\n splineXY.addPoint(p[i].x, p[i].y);\n splineYX.addPoint(p[i].y, p[i].x);\n }\n\n for (int i = 1; i < patternSize.height - 1; ++i)\n {\n cv::Point2f& p_i = corners.at(i * patternSize.width + j)->pt;\n\n float thresh = std::numeric_limits::max();\n\n // up-neighbor\n {\n cv::Point2f& neighbor = corners.at((i - 1) * patternSize.width + j)->pt;\n thresh = fminf(thresh, cv::norm(neighbor - p_i));\n }\n // down-neighbor\n {\n cv::Point2f& neighbor = corners.at((i + 1) * patternSize.width + j)->pt;\n thresh = fminf(thresh, cv::norm(neighbor - p_i));\n }\n // left-neighbor\n if (j > 0)\n {\n cv::Point2f& neighbor = corners.at(i * patternSize.width + j - 1)->pt;\n thresh = fminf(thresh, cv::norm(neighbor - p_i));\n }\n // right-neighbor\n if (j < patternSize.width - 1)\n {\n cv::Point2f& neighbor = corners.at(i * patternSize.width + j + 1)->pt;\n thresh = fminf(thresh, cv::norm(neighbor - p_i));\n }\n\n thresh *= threshFactor;\n\n if (fminf(fabsf(splineXY(p_i.x) - p_i.y), fabsf(splineYX(p_i.y) - p_i.x)) > thresh)\n {\n return false;\n }\n }\n }\n\n return true;\n}\n\nbool\nChessboard::matchCorners(ChessboardQuadPtr& quad1, int corner1,\n ChessboardQuadPtr& quad2, int corner2) const\n{\n // First Check everything from the viewpoint of the\n // current quad compute midpoints of \"parallel\" quad\n // sides 1\n float x1 = (quad1->corners[corner1]->pt.x + quad1->corners[(corner1+1)%4]->pt.x)/2;\n float y1 = (quad1->corners[corner1]->pt.y + quad1->corners[(corner1+1)%4]->pt.y)/2;\n float x2 = (quad1->corners[(corner1+2)%4]->pt.x + quad1->corners[(corner1+3)%4]->pt.x)/2;\n float y2 = (quad1->corners[(corner1+2)%4]->pt.y + quad1->corners[(corner1+3)%4]->pt.y)/2;\n // compute midpoints of \"parallel\" quad sides 2\n float x3 = (quad1->corners[corner1]->pt.x + quad1->corners[(corner1+3)%4]->pt.x)/2;\n float y3 = (quad1->corners[corner1]->pt.y + quad1->corners[(corner1+3)%4]->pt.y)/2;\n float x4 = (quad1->corners[(corner1+1)%4]->pt.x + quad1->corners[(corner1+2)%4]->pt.x)/2;\n float y4 = (quad1->corners[(corner1+1)%4]->pt.y + quad1->corners[(corner1+2)%4]->pt.y)/2;\n\n // MARTIN: Heuristic\n // For corner2 of quad2 to be considered,\n // it needs to be on the same side of the two lines as\n // corner1. This is given, if the cross product has\n // the same sign for both computations below:\n float a1 = x1 - x2;\n float b1 = y1 - y2;\n // the current corner\n float c11 = quad1->corners[corner1]->pt.x - x2;\n float d11 = quad1->corners[corner1]->pt.y - y2;\n // the candidate corner\n float c12 = quad2->corners[corner2]->pt.x - x2;\n float d12 = quad2->corners[corner2]->pt.y - y2;\n float sign11 = a1*d11 - c11*b1;\n float sign12 = a1*d12 - c12*b1;\n\n float a2 = x3 - x4;\n float b2 = y3 - y4;\n // the current corner\n float c21 = quad1->corners[corner1]->pt.x - x4;\n float d21 = quad1->corners[corner1]->pt.y - y4;\n // the candidate corner\n float c22 = quad2->corners[corner2]->pt.x - x4;\n float d22 = quad2->corners[corner2]->pt.y - y4;\n float sign21 = a2*d21 - c21*b2;\n float sign22 = a2*d22 - c22*b2;\n\n // Also make shure that two border quads of the same row or\n // column don't link. Check from the current corner's view,\n // whether the corner diagonal from the candidate corner\n // is also on the same side of the two lines as the current\n // corner and the candidate corner.\n float c13 = quad2->corners[(corner2+2)%4]->pt.x - x2;\n float d13 = quad2->corners[(corner2+2)%4]->pt.y - y2;\n float c23 = quad2->corners[(corner2+2)%4]->pt.x - x4;\n float d23 = quad2->corners[(corner2+2)%4]->pt.y - y4;\n float sign13 = a1*d13 - c13*b1;\n float sign23 = a2*d23 - c23*b2;\n\n\n // Second: Then check everything from the viewpoint of\n // the candidate quad. Compute midpoints of \"parallel\"\n // quad sides 1\n float u1 = (quad2->corners[corner2]->pt.x + quad2->corners[(corner2+1)%4]->pt.x)/2;\n float v1 = (quad2->corners[corner2]->pt.y + quad2->corners[(corner2+1)%4]->pt.y)/2;\n float u2 = (quad2->corners[(corner2+2)%4]->pt.x + quad2->corners[(corner2+3)%4]->pt.x)/2;\n float v2 = (quad2->corners[(corner2+2)%4]->pt.y + quad2->corners[(corner2+3)%4]->pt.y)/2;\n // compute midpoints of \"parallel\" quad sides 2\n float u3 = (quad2->corners[corner2]->pt.x + quad2->corners[(corner2+3)%4]->pt.x)/2;\n float v3 = (quad2->corners[corner2]->pt.y + quad2->corners[(corner2+3)%4]->pt.y)/2;\n float u4 = (quad2->corners[(corner2+1)%4]->pt.x + quad2->corners[(corner2+2)%4]->pt.x)/2;\n float v4 = (quad2->corners[(corner2+1)%4]->pt.y + quad2->corners[(corner2+2)%4]->pt.y)/2;\n\n // MARTIN: Heuristic\n // For corner2 of quad2 to be considered,\n // it needs to be on the same side of the two lines as\n // corner1. This is given, if the cross product has\n // the same sign for both computations below:\n float a3 = u1 - u2;\n float b3 = v1 - v2;\n // the current corner\n float c31 = quad1->corners[corner1]->pt.x - u2;\n float d31 = quad1->corners[corner1]->pt.y - v2;\n // the candidate corner\n float c32 = quad2->corners[corner2]->pt.x - u2;\n float d32 = quad2->corners[corner2]->pt.y - v2;\n float sign31 = a3*d31-c31*b3;\n float sign32 = a3*d32-c32*b3;\n\n float a4 = u3 - u4;\n float b4 = v3 - v4;\n // the current corner\n float c41 = quad1->corners[corner1]->pt.x - u4;\n float d41 = quad1->corners[corner1]->pt.y - v4;\n // the candidate corner\n float c42 = quad2->corners[corner2]->pt.x - u4;\n float d42 = quad2->corners[corner2]->pt.y - v4;\n float sign41 = a4*d41-c41*b4;\n float sign42 = a4*d42-c42*b4;\n\n // Also make sure that two border quads of the same row or\n // column don't link. Check from the candidate corner's view,\n // whether the corner diagonal from the current corner\n // is also on the same side of the two lines as the current\n // corner and the candidate corner.\n float c33 = quad1->corners[(corner1+2)%4]->pt.x - u2;\n float d33 = quad1->corners[(corner1+2)%4]->pt.y - v2;\n float c43 = quad1->corners[(corner1+2)%4]->pt.x - u4;\n float d43 = quad1->corners[(corner1+2)%4]->pt.y - v4;\n float sign33 = a3*d33-c33*b3;\n float sign43 = a4*d43-c43*b4;\n\n\n // This time we also need to make shure, that no quad\n // is linked to a quad of another dilation run which\n // may lie INSIDE it!!!\n // Third: Therefore check everything from the viewpoint\n // of the current quad compute midpoints of \"parallel\"\n // quad sides 1\n float x5 = quad1->corners[corner1]->pt.x;\n float y5 = quad1->corners[corner1]->pt.y;\n float x6 = quad1->corners[(corner1+1)%4]->pt.x;\n float y6 = quad1->corners[(corner1+1)%4]->pt.y;\n // compute midpoints of \"parallel\" quad sides 2\n float x7 = x5;\n float y7 = y5;\n float x8 = quad1->corners[(corner1+3)%4]->pt.x;\n float y8 = quad1->corners[(corner1+3)%4]->pt.y;\n\n // MARTIN: Heuristic\n // For corner2 of quad2 to be considered,\n // it needs to be on the other side of the two lines than\n // corner1. This is given, if the cross product has\n // a different sign for both computations below:\n float a5 = x6 - x5;\n float b5 = y6 - y5;\n // the current corner\n float c51 = quad1->corners[(corner1+2)%4]->pt.x - x5;\n float d51 = quad1->corners[(corner1+2)%4]->pt.y - y5;\n // the candidate corner\n float c52 = quad2->corners[corner2]->pt.x - x5;\n float d52 = quad2->corners[corner2]->pt.y - y5;\n float sign51 = a5*d51 - c51*b5;\n float sign52 = a5*d52 - c52*b5;\n\n float a6 = x8 - x7;\n float b6 = y8 - y7;\n // the current corner\n float c61 = quad1->corners[(corner1+2)%4]->pt.x - x7;\n float d61 = quad1->corners[(corner1+2)%4]->pt.y - y7;\n // the candidate corner\n float c62 = quad2->corners[corner2]->pt.x - x7;\n float d62 = quad2->corners[corner2]->pt.y - y7;\n float sign61 = a6*d61 - c61*b6;\n float sign62 = a6*d62 - c62*b6;\n\n\n // Fourth: Then check everything from the viewpoint of\n // the candidate quad compute midpoints of \"parallel\"\n // quad sides 1\n float u5 = quad2->corners[corner2]->pt.x;\n float v5 = quad2->corners[corner2]->pt.y;\n float u6 = quad2->corners[(corner2+1)%4]->pt.x;\n float v6 = quad2->corners[(corner2+1)%4]->pt.y;\n // compute midpoints of \"parallel\" quad sides 2\n float u7 = u5;\n float v7 = v5;\n float u8 = quad2->corners[(corner2+3)%4]->pt.x;\n float v8 = quad2->corners[(corner2+3)%4]->pt.y;\n\n // MARTIN: Heuristic\n // For corner2 of quad2 to be considered,\n // it needs to be on the other side of the two lines than\n // corner1. This is given, if the cross product has\n // a different sign for both computations below:\n float a7 = u6 - u5;\n float b7 = v6 - v5;\n // the current corner\n float c71 = quad1->corners[corner1]->pt.x - u5;\n float d71 = quad1->corners[corner1]->pt.y - v5;\n // the candidate corner\n float c72 = quad2->corners[(corner2+2)%4]->pt.x - u5;\n float d72 = quad2->corners[(corner2+2)%4]->pt.y - v5;\n float sign71 = a7*d71-c71*b7;\n float sign72 = a7*d72-c72*b7;\n\n float a8 = u8 - u7;\n float b8 = v8 - v7;\n // the current corner\n float c81 = quad1->corners[corner1]->pt.x - u7;\n float d81 = quad1->corners[corner1]->pt.y - v7;\n // the candidate corner\n float c82 = quad2->corners[(corner2+2)%4]->pt.x - u7;\n float d82 = quad2->corners[(corner2+2)%4]->pt.y - v7;\n float sign81 = a8*d81-c81*b8;\n float sign82 = a8*d82-c82*b8;\n\n // Check whether conditions are fulfilled\n if (((sign11 < 0 && sign12 < 0) || (sign11 > 0 && sign12 > 0)) &&\n ((sign21 < 0 && sign22 < 0) || (sign21 > 0 && sign22 > 0)) &&\n ((sign31 < 0 && sign32 < 0) || (sign31 > 0 && sign32 > 0)) &&\n ((sign41 < 0 && sign42 < 0) || (sign41 > 0 && sign42 > 0)) &&\n ((sign11 < 0 && sign13 < 0) || (sign11 > 0 && sign13 > 0)) &&\n ((sign21 < 0 && sign23 < 0) || (sign21 > 0 && sign23 > 0)) &&\n ((sign31 < 0 && sign33 < 0) || (sign31 > 0 && sign33 > 0)) &&\n ((sign41 < 0 && sign43 < 0) || (sign41 > 0 && sign43 > 0)) &&\n ((sign51 < 0 && sign52 > 0) || (sign51 > 0 && sign52 < 0)) &&\n ((sign61 < 0 && sign62 > 0) || (sign61 > 0 && sign62 < 0)) &&\n ((sign71 < 0 && sign72 > 0) || (sign71 > 0 && sign72 < 0)) &&\n ((sign81 < 0 && sign82 > 0) || (sign81 > 0 && sign82 < 0)))\n {\n return true;\n }\n else\n {\n return false;\n }\n}\n\n}\n" }, { "path": "camera_model/src/gpl/EigenQuaternionParameterization.cc", "content": "#include \"camodocal/gpl/EigenQuaternionParameterization.h\"\n\n#include \n\nnamespace camodocal\n{\n\nbool\nEigenQuaternionParameterization::Plus(const double* x,\n const double* delta,\n double* x_plus_delta) const\n{\n const double norm_delta =\n sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]);\n if (norm_delta > 0.0)\n {\n const double sin_delta_by_delta = (sin(norm_delta) / norm_delta);\n double q_delta[4];\n q_delta[0] = sin_delta_by_delta * delta[0];\n q_delta[1] = sin_delta_by_delta * delta[1];\n q_delta[2] = sin_delta_by_delta * delta[2];\n q_delta[3] = cos(norm_delta);\n EigenQuaternionProduct(q_delta, x, x_plus_delta);\n }\n else\n {\n for (int i = 0; i < 4; ++i)\n {\n x_plus_delta[i] = x[i];\n }\n }\n return true;\n}\n\nbool\nEigenQuaternionParameterization::ComputeJacobian(const double* x,\n double* jacobian) const\n{\n jacobian[0] = x[3]; jacobian[1] = x[2]; jacobian[2] = -x[1]; // NOLINT\n jacobian[3] = -x[2]; jacobian[4] = x[3]; jacobian[5] = x[0]; // NOLINT\n jacobian[6] = x[1]; jacobian[7] = -x[0]; jacobian[8] = x[3]; // NOLINT\n jacobian[9] = -x[0]; jacobian[10] = -x[1]; jacobian[11] = -x[2]; // NOLINT\n return true;\n}\n\n}\n" }, { "path": "camera_model/src/gpl/gpl.cc", "content": "#include \"camodocal/gpl/gpl.h\"\r\n\r\n#include \r\n#ifdef _WIN32\r\n#include \r\n#else\r\n#include \r\n#endif\r\n\r\n\r\n// source: https://stackoverflow.com/questions/5167269/clock-gettime-alternative-in-mac-os-x\r\n#ifdef __APPLE__\r\n#include \r\n#define ORWL_NANO (+1.0E-9)\r\n#define ORWL_GIGA UINT64_C(1000000000)\r\n\r\nstatic double orwl_timebase = 0.0;\r\nstatic uint64_t orwl_timestart = 0;\r\n\r\nstruct timespec orwl_gettime(void) {\r\n // be more careful in a multithreaded environement\r\n if (!orwl_timestart) {\r\n mach_timebase_info_data_t tb = { 0 };\r\n mach_timebase_info(&tb);\r\n orwl_timebase = tb.numer;\r\n orwl_timebase /= tb.denom;\r\n orwl_timestart = mach_absolute_time();\r\n }\r\n struct timespec t;\r\n double diff = (mach_absolute_time() - orwl_timestart) * orwl_timebase;\r\n t.tv_sec = diff * ORWL_NANO;\r\n t.tv_nsec = diff - (t.tv_sec * ORWL_GIGA);\r\n return t;\r\n}\r\n#endif // __APPLE__\r\n\r\n\r\nconst double WGS84_A = 6378137.0;\r\nconst double WGS84_ECCSQ = 0.00669437999013;\r\n\r\n// Windows lacks fminf\r\n#ifndef fminf\r\n#define fminf(x, y) (((x) < (y)) ? (x) : (y))\r\n#endif\r\n\r\nnamespace camodocal\r\n{\r\n\r\ndouble hypot3(double x, double y, double z)\r\n{\r\n\treturn sqrt(square(x) + square(y) + square(z));\r\n}\r\n\r\nfloat hypot3f(float x, float y, float z)\r\n{\r\n\treturn sqrtf(square(x) + square(y) + square(z));\r\n}\r\n\r\ndouble d2r(double deg)\r\n{\r\n\treturn deg / 180.0 * M_PI;\r\n}\r\n\r\nfloat d2r(float deg)\r\n{\r\n\treturn deg / 180.0f * M_PI;\r\n}\r\n\r\ndouble r2d(double rad)\r\n{\r\n\treturn rad / M_PI * 180.0;\r\n}\r\n\r\nfloat r2d(float rad)\r\n{\r\n\treturn rad / M_PI * 180.0f;\r\n}\r\n\r\ndouble sinc(double theta)\r\n{\r\n return sin(theta) / theta;\r\n}\r\n\r\n#ifdef _WIN32\r\n#include \r\n#include \r\n#include \r\nLARGE_INTEGER\r\ngetFILETIMEoffset()\r\n{\r\n SYSTEMTIME s;\r\n FILETIME f;\r\n LARGE_INTEGER t;\r\n\r\n s.wYear = 1970;\r\n s.wMonth = 1;\r\n s.wDay = 1;\r\n s.wHour = 0;\r\n s.wMinute = 0;\r\n s.wSecond = 0;\r\n s.wMilliseconds = 0;\r\n SystemTimeToFileTime(&s, &f);\r\n t.QuadPart = f.dwHighDateTime;\r\n t.QuadPart <<= 32;\r\n t.QuadPart |= f.dwLowDateTime;\r\n return (t);\r\n}\r\n\r\nint\r\nclock_gettime(int X, struct timespec *tp)\r\n{\r\n LARGE_INTEGER t;\r\n FILETIME f;\r\n double microseconds;\r\n static LARGE_INTEGER offset;\r\n static double frequencyToMicroseconds;\r\n static int initialized = 0;\r\n static BOOL usePerformanceCounter = 0;\r\n\r\n if (!initialized) {\r\n LARGE_INTEGER performanceFrequency;\r\n initialized = 1;\r\n usePerformanceCounter = QueryPerformanceFrequency(&performanceFrequency);\r\n if (usePerformanceCounter) {\r\n QueryPerformanceCounter(&offset);\r\n frequencyToMicroseconds = (double)performanceFrequency.QuadPart / 1000000.;\r\n } else {\r\n offset = getFILETIMEoffset();\r\n frequencyToMicroseconds = 10.;\r\n }\r\n }\r\n if (usePerformanceCounter) QueryPerformanceCounter(&t);\r\n else {\r\n GetSystemTimeAsFileTime(&f);\r\n t.QuadPart = f.dwHighDateTime;\r\n t.QuadPart <<= 32;\r\n t.QuadPart |= f.dwLowDateTime;\r\n }\r\n\r\n t.QuadPart -= offset.QuadPart;\r\n microseconds = (double)t.QuadPart / frequencyToMicroseconds;\r\n t.QuadPart = microseconds;\r\n tp->tv_sec = t.QuadPart / 1000000;\r\n tp->tv_nsec = (t.QuadPart % 1000000) * 1000;\r\n return (0);\r\n}\r\n#endif\r\n\r\nunsigned long long timeInMicroseconds(void)\r\n{\r\n\t struct timespec tp;\r\n#ifdef __APPLE__\r\n tp = orwl_gettime();\r\n#else\r\n\t clock_gettime(CLOCK_REALTIME, &tp);\r\n#endif\r\n\r\n\t return tp.tv_sec * 1000000 + tp.tv_nsec / 1000;\r\n}\r\n\r\ndouble timeInSeconds(void)\r\n{\r\n struct timespec tp;\r\n#ifdef __APPLE__\r\n tp = orwl_gettime();\r\n#else\r\n\t clock_gettime(CLOCK_REALTIME, &tp);\r\n#endif\r\n\r\n\t return static_cast(tp.tv_sec) +\r\n\t\t\t static_cast(tp.tv_nsec) / 1000000000.0;\r\n}\r\n\r\nfloat colormapAutumn[128][3] =\r\n{\r\n\t\t{1.0f,0.f,0.f},\r\n\t\t{1.0f,0.007874f,0.f},\r\n\t\t{1.0f,0.015748f,0.f},\r\n\t\t{1.0f,0.023622f,0.f},\r\n\t\t{1.0f,0.031496f,0.f},\r\n\t\t{1.0f,0.03937f,0.f},\r\n\t\t{1.0f,0.047244f,0.f},\r\n\t\t{1.0f,0.055118f,0.f},\r\n\t\t{1.0f,0.062992f,0.f},\r\n\t\t{1.0f,0.070866f,0.f},\r\n\t\t{1.0f,0.07874f,0.f},\r\n\t\t{1.0f,0.086614f,0.f},\r\n\t\t{1.0f,0.094488f,0.f},\r\n\t\t{1.0f,0.10236f,0.f},\r\n\t\t{1.0f,0.11024f,0.f},\r\n\t\t{1.0f,0.11811f,0.f},\r\n\t\t{1.0f,0.12598f,0.f},\r\n\t\t{1.0f,0.13386f,0.f},\r\n\t\t{1.0f,0.14173f,0.f},\r\n\t\t{1.0f,0.14961f,0.f},\r\n\t\t{1.0f,0.15748f,0.f},\r\n\t\t{1.0f,0.16535f,0.f},\r\n\t\t{1.0f,0.17323f,0.f},\r\n\t\t{1.0f,0.1811f,0.f},\r\n\t\t{1.0f,0.18898f,0.f},\r\n\t\t{1.0f,0.19685f,0.f},\r\n\t\t{1.0f,0.20472f,0.f},\r\n\t\t{1.0f,0.2126f,0.f},\r\n\t\t{1.0f,0.22047f,0.f},\r\n\t\t{1.0f,0.22835f,0.f},\r\n\t\t{1.0f,0.23622f,0.f},\r\n\t\t{1.0f,0.24409f,0.f},\r\n\t\t{1.0f,0.25197f,0.f},\r\n\t\t{1.0f,0.25984f,0.f},\r\n\t\t{1.0f,0.26772f,0.f},\r\n\t\t{1.0f,0.27559f,0.f},\r\n\t\t{1.0f,0.28346f,0.f},\r\n\t\t{1.0f,0.29134f,0.f},\r\n\t\t{1.0f,0.29921f,0.f},\r\n\t\t{1.0f,0.30709f,0.f},\r\n\t\t{1.0f,0.31496f,0.f},\r\n\t\t{1.0f,0.32283f,0.f},\r\n\t\t{1.0f,0.33071f,0.f},\r\n\t\t{1.0f,0.33858f,0.f},\r\n\t\t{1.0f,0.34646f,0.f},\r\n\t\t{1.0f,0.35433f,0.f},\r\n\t\t{1.0f,0.3622f,0.f},\r\n\t\t{1.0f,0.37008f,0.f},\r\n\t\t{1.0f,0.37795f,0.f},\r\n\t\t{1.0f,0.38583f,0.f},\r\n\t\t{1.0f,0.3937f,0.f},\r\n\t\t{1.0f,0.40157f,0.f},\r\n\t\t{1.0f,0.40945f,0.f},\r\n\t\t{1.0f,0.41732f,0.f},\r\n\t\t{1.0f,0.4252f,0.f},\r\n\t\t{1.0f,0.43307f,0.f},\r\n\t\t{1.0f,0.44094f,0.f},\r\n\t\t{1.0f,0.44882f,0.f},\r\n\t\t{1.0f,0.45669f,0.f},\r\n\t\t{1.0f,0.46457f,0.f},\r\n\t\t{1.0f,0.47244f,0.f},\r\n\t\t{1.0f,0.48031f,0.f},\r\n\t\t{1.0f,0.48819f,0.f},\r\n\t\t{1.0f,0.49606f,0.f},\r\n\t\t{1.0f,0.50394f,0.f},\r\n\t\t{1.0f,0.51181f,0.f},\r\n\t\t{1.0f,0.51969f,0.f},\r\n\t\t{1.0f,0.52756f,0.f},\r\n\t\t{1.0f,0.53543f,0.f},\r\n\t\t{1.0f,0.54331f,0.f},\r\n\t\t{1.0f,0.55118f,0.f},\r\n\t\t{1.0f,0.55906f,0.f},\r\n\t\t{1.0f,0.56693f,0.f},\r\n\t\t{1.0f,0.5748f,0.f},\r\n\t\t{1.0f,0.58268f,0.f},\r\n\t\t{1.0f,0.59055f,0.f},\r\n\t\t{1.0f,0.59843f,0.f},\r\n\t\t{1.0f,0.6063f,0.f},\r\n\t\t{1.0f,0.61417f,0.f},\r\n\t\t{1.0f,0.62205f,0.f},\r\n\t\t{1.0f,0.62992f,0.f},\r\n\t\t{1.0f,0.6378f,0.f},\r\n\t\t{1.0f,0.64567f,0.f},\r\n\t\t{1.0f,0.65354f,0.f},\r\n\t\t{1.0f,0.66142f,0.f},\r\n\t\t{1.0f,0.66929f,0.f},\r\n\t\t{1.0f,0.67717f,0.f},\r\n\t\t{1.0f,0.68504f,0.f},\r\n\t\t{1.0f,0.69291f,0.f},\r\n\t\t{1.0f,0.70079f,0.f},\r\n\t\t{1.0f,0.70866f,0.f},\r\n\t\t{1.0f,0.71654f,0.f},\r\n\t\t{1.0f,0.72441f,0.f},\r\n\t\t{1.0f,0.73228f,0.f},\r\n\t\t{1.0f,0.74016f,0.f},\r\n\t\t{1.0f,0.74803f,0.f},\r\n\t\t{1.0f,0.75591f,0.f},\r\n\t\t{1.0f,0.76378f,0.f},\r\n\t\t{1.0f,0.77165f,0.f},\r\n\t\t{1.0f,0.77953f,0.f},\r\n\t\t{1.0f,0.7874f,0.f},\r\n\t\t{1.0f,0.79528f,0.f},\r\n\t\t{1.0f,0.80315f,0.f},\r\n\t\t{1.0f,0.81102f,0.f},\r\n\t\t{1.0f,0.8189f,0.f},\r\n\t\t{1.0f,0.82677f,0.f},\r\n\t\t{1.0f,0.83465f,0.f},\r\n\t\t{1.0f,0.84252f,0.f},\r\n\t\t{1.0f,0.85039f,0.f},\r\n\t\t{1.0f,0.85827f,0.f},\r\n\t\t{1.0f,0.86614f,0.f},\r\n\t\t{1.0f,0.87402f,0.f},\r\n\t\t{1.0f,0.88189f,0.f},\r\n\t\t{1.0f,0.88976f,0.f},\r\n\t\t{1.0f,0.89764f,0.f},\r\n\t\t{1.0f,0.90551f,0.f},\r\n\t\t{1.0f,0.91339f,0.f},\r\n\t\t{1.0f,0.92126f,0.f},\r\n\t\t{1.0f,0.92913f,0.f},\r\n\t\t{1.0f,0.93701f,0.f},\r\n\t\t{1.0f,0.94488f,0.f},\r\n\t\t{1.0f,0.95276f,0.f},\r\n\t\t{1.0f,0.96063f,0.f},\r\n\t\t{1.0f,0.9685f,0.f},\r\n\t\t{1.0f,0.97638f,0.f},\r\n\t\t{1.0f,0.98425f,0.f},\r\n\t\t{1.0f,0.99213f,0.f},\r\n\t\t{1.0f,1.0f,0.0f}\r\n};\r\n\r\n\r\nfloat colormapJet[128][3] =\r\n{\r\n\t\t{0.0f,0.0f,0.53125f},\r\n\t\t{0.0f,0.0f,0.5625f},\r\n\t\t{0.0f,0.0f,0.59375f},\r\n\t\t{0.0f,0.0f,0.625f},\r\n\t\t{0.0f,0.0f,0.65625f},\r\n\t\t{0.0f,0.0f,0.6875f},\r\n\t\t{0.0f,0.0f,0.71875f},\r\n\t\t{0.0f,0.0f,0.75f},\r\n\t\t{0.0f,0.0f,0.78125f},\r\n\t\t{0.0f,0.0f,0.8125f},\r\n\t\t{0.0f,0.0f,0.84375f},\r\n\t\t{0.0f,0.0f,0.875f},\r\n\t\t{0.0f,0.0f,0.90625f},\r\n\t\t{0.0f,0.0f,0.9375f},\r\n\t\t{0.0f,0.0f,0.96875f},\r\n\t\t{0.0f,0.0f,1.0f},\r\n\t\t{0.0f,0.03125f,1.0f},\r\n\t\t{0.0f,0.0625f,1.0f},\r\n\t\t{0.0f,0.09375f,1.0f},\r\n\t\t{0.0f,0.125f,1.0f},\r\n\t\t{0.0f,0.15625f,1.0f},\r\n\t\t{0.0f,0.1875f,1.0f},\r\n\t\t{0.0f,0.21875f,1.0f},\r\n\t\t{0.0f,0.25f,1.0f},\r\n\t\t{0.0f,0.28125f,1.0f},\r\n\t\t{0.0f,0.3125f,1.0f},\r\n\t\t{0.0f,0.34375f,1.0f},\r\n\t\t{0.0f,0.375f,1.0f},\r\n\t\t{0.0f,0.40625f,1.0f},\r\n\t\t{0.0f,0.4375f,1.0f},\r\n\t\t{0.0f,0.46875f,1.0f},\r\n\t\t{0.0f,0.5f,1.0f},\r\n\t\t{0.0f,0.53125f,1.0f},\r\n\t\t{0.0f,0.5625f,1.0f},\r\n\t\t{0.0f,0.59375f,1.0f},\r\n\t\t{0.0f,0.625f,1.0f},\r\n\t\t{0.0f,0.65625f,1.0f},\r\n\t\t{0.0f,0.6875f,1.0f},\r\n\t\t{0.0f,0.71875f,1.0f},\r\n\t\t{0.0f,0.75f,1.0f},\r\n\t\t{0.0f,0.78125f,1.0f},\r\n\t\t{0.0f,0.8125f,1.0f},\r\n\t\t{0.0f,0.84375f,1.0f},\r\n\t\t{0.0f,0.875f,1.0f},\r\n\t\t{0.0f,0.90625f,1.0f},\r\n\t\t{0.0f,0.9375f,1.0f},\r\n\t\t{0.0f,0.96875f,1.0f},\r\n\t\t{0.0f,1.0f,1.0f},\r\n\t\t{0.03125f,1.0f,0.96875f},\r\n\t\t{0.0625f,1.0f,0.9375f},\r\n\t\t{0.09375f,1.0f,0.90625f},\r\n\t\t{0.125f,1.0f,0.875f},\r\n\t\t{0.15625f,1.0f,0.84375f},\r\n\t\t{0.1875f,1.0f,0.8125f},\r\n\t\t{0.21875f,1.0f,0.78125f},\r\n\t\t{0.25f,1.0f,0.75f},\r\n\t\t{0.28125f,1.0f,0.71875f},\r\n\t\t{0.3125f,1.0f,0.6875f},\r\n\t\t{0.34375f,1.0f,0.65625f},\r\n\t\t{0.375f,1.0f,0.625f},\r\n\t\t{0.40625f,1.0f,0.59375f},\r\n\t\t{0.4375f,1.0f,0.5625f},\r\n\t\t{0.46875f,1.0f,0.53125f},\r\n\t\t{0.5f,1.0f,0.5f},\r\n\t\t{0.53125f,1.0f,0.46875f},\r\n\t\t{0.5625f,1.0f,0.4375f},\r\n\t\t{0.59375f,1.0f,0.40625f},\r\n\t\t{0.625f,1.0f,0.375f},\r\n\t\t{0.65625f,1.0f,0.34375f},\r\n\t\t{0.6875f,1.0f,0.3125f},\r\n\t\t{0.71875f,1.0f,0.28125f},\r\n\t\t{0.75f,1.0f,0.25f},\r\n\t\t{0.78125f,1.0f,0.21875f},\r\n\t\t{0.8125f,1.0f,0.1875f},\r\n\t\t{0.84375f,1.0f,0.15625f},\r\n\t\t{0.875f,1.0f,0.125f},\r\n\t\t{0.90625f,1.0f,0.09375f},\r\n\t\t{0.9375f,1.0f,0.0625f},\r\n\t\t{0.96875f,1.0f,0.03125f},\r\n\t\t{1.0f,1.0f,0.0f},\r\n\t\t{1.0f,0.96875f,0.0f},\r\n\t\t{1.0f,0.9375f,0.0f},\r\n\t\t{1.0f,0.90625f,0.0f},\r\n\t\t{1.0f,0.875f,0.0f},\r\n\t\t{1.0f,0.84375f,0.0f},\r\n\t\t{1.0f,0.8125f,0.0f},\r\n\t\t{1.0f,0.78125f,0.0f},\r\n\t\t{1.0f,0.75f,0.0f},\r\n\t\t{1.0f,0.71875f,0.0f},\r\n\t\t{1.0f,0.6875f,0.0f},\r\n\t\t{1.0f,0.65625f,0.0f},\r\n\t\t{1.0f,0.625f,0.0f},\r\n\t\t{1.0f,0.59375f,0.0f},\r\n\t\t{1.0f,0.5625f,0.0f},\r\n\t\t{1.0f,0.53125f,0.0f},\r\n\t\t{1.0f,0.5f,0.0f},\r\n\t\t{1.0f,0.46875f,0.0f},\r\n\t\t{1.0f,0.4375f,0.0f},\r\n\t\t{1.0f,0.40625f,0.0f},\r\n\t\t{1.0f,0.375f,0.0f},\r\n\t\t{1.0f,0.34375f,0.0f},\r\n\t\t{1.0f,0.3125f,0.0f},\r\n\t\t{1.0f,0.28125f,0.0f},\r\n\t\t{1.0f,0.25f,0.0f},\r\n\t\t{1.0f,0.21875f,0.0f},\r\n\t\t{1.0f,0.1875f,0.0f},\r\n\t\t{1.0f,0.15625f,0.0f},\r\n\t\t{1.0f,0.125f,0.0f},\r\n\t\t{1.0f,0.09375f,0.0f},\r\n\t\t{1.0f,0.0625f,0.0f},\r\n\t\t{1.0f,0.03125f,0.0f},\r\n\t\t{1.0f,0.0f,0.0f},\r\n\t\t{0.96875f,0.0f,0.0f},\r\n\t\t{0.9375f,0.0f,0.0f},\r\n\t\t{0.90625f,0.0f,0.0f},\r\n\t\t{0.875f,0.0f,0.0f},\r\n\t\t{0.84375f,0.0f,0.0f},\r\n\t\t{0.8125f,0.0f,0.0f},\r\n\t\t{0.78125f,0.0f,0.0f},\r\n\t\t{0.75f,0.0f,0.0f},\r\n\t\t{0.71875f,0.0f,0.0f},\r\n\t\t{0.6875f,0.0f,0.0f},\r\n\t\t{0.65625f,0.0f,0.0f},\r\n\t\t{0.625f,0.0f,0.0f},\r\n\t\t{0.59375f,0.0f,0.0f},\r\n\t\t{0.5625f,0.0f,0.0f},\r\n\t\t{0.53125f,0.0f,0.0f},\r\n\t\t{0.5f,0.0f,0.0f}\r\n};\r\n\r\nvoid colorDepthImage(cv::Mat& imgDepth, cv::Mat& imgColoredDepth,\r\n\t\t\t\t\t float minRange, float maxRange)\r\n{\r\n\timgColoredDepth = cv::Mat::zeros(imgDepth.size(), CV_8UC3);\r\n\r\n\tfor (int i = 0; i < imgColoredDepth.rows; ++i)\r\n\t{\r\n\t\tconst float* depth = imgDepth.ptr(i);\r\n\t\tunsigned char* pixel = imgColoredDepth.ptr(i);\r\n\t\tfor (int j = 0; j < imgColoredDepth.cols; ++j)\r\n\t\t{\r\n\t\t\tif (depth[j] != 0)\r\n\t\t\t{\r\n\t\t\t\tint idx = fminf(depth[j] - minRange, maxRange - minRange) / (maxRange - minRange) * 127.0f;\r\n\t\t\t\tidx = 127 - idx;\r\n\r\n\t\t\t\tpixel[0] = colormapJet[idx][2] * 255.0f;\r\n\t\t\t\tpixel[1] = colormapJet[idx][1] * 255.0f;\r\n\t\t\t\tpixel[2] = colormapJet[idx][0] * 255.0f;\r\n\t\t\t}\r\n\r\n\t\t\tpixel += 3;\r\n\t\t}\r\n\t}\r\n}\r\n\r\nbool colormap(const std::string& name, unsigned char idx,\r\n\t\t\t float& r, float& g, float& b)\r\n{\r\n\tif (name.compare(\"jet\") == 0)\r\n\t{\r\n\t\tfloat* color = colormapJet[idx];\r\n\r\n\t\tr = color[0];\r\n\t\tg = color[1];\r\n\t\tb = color[2];\r\n\r\n\t\treturn true;\r\n\t}\r\n\telse if (name.compare(\"autumn\") == 0)\r\n\t{\r\n\t\tfloat* color = colormapAutumn[idx];\r\n\r\n\t\tr = color[0];\r\n\t\tg = color[1];\r\n\t\tb = color[2];\r\n\r\n\t\treturn true;\r\n\t}\r\n\r\n\treturn false;\r\n}\r\n\r\nstd::vector bresLine(int x0, int y0, int x1, int y1)\r\n{\r\n\t// Bresenham's line algorithm\r\n\t// Find cells intersected by line between (x0,y0) and (x1,y1)\r\n\r\n\tstd::vector cells;\r\n\r\n\tint dx = std::abs(x1 - x0);\r\n\tint dy = std::abs(y1 - y0);\r\n\r\n\tint sx = (x0 < x1) ? 1 : -1;\r\n\tint sy = (y0 < y1) ? 1 : -1;\r\n\r\n\tint err = dx - dy;\r\n\r\n\twhile (1)\r\n\t{\r\n\t\tcells.push_back(cv::Point2i(x0, y0));\r\n\r\n\t\tif (x0 == x1 && y0 == y1)\r\n\t\t{\r\n\t\t\tbreak;\r\n\t\t}\r\n\r\n\t\tint e2 = 2 * err;\r\n\t\tif (e2 > -dy)\r\n\t\t{\r\n\t\t\terr -= dy;\r\n\t\t\tx0 += sx;\r\n\t\t}\r\n\t\tif (e2 < dx)\r\n\t\t{\r\n\t\t\terr += dx;\r\n\t\t\ty0 += sy;\r\n\t\t}\r\n\t}\r\n\r\n\treturn cells;\r\n}\r\n\r\nstd::vector bresCircle(int x0, int y0, int r)\r\n{\r\n\t// Bresenham's circle algorithm\r\n\t// Find cells intersected by circle with center (x0,y0) and radius r\r\n\r\n\tstd::vector< std::vector > mask(2 * r + 1);\r\n\t\r\n\tfor (int i = 0; i < 2 * r + 1; ++i)\r\n\t{\r\n\t\tmask[i].resize(2 * r + 1);\r\n\t\tfor (int j = 0; j < 2 * r + 1; ++j)\r\n\t\t{\r\n\t\t\tmask[i][j] = false;\r\n\t\t}\r\n\t}\r\n\r\n\tint f = 1 - r;\r\n\tint ddF_x = 1;\r\n\tint ddF_y = -2 * r;\r\n\tint x = 0;\r\n\tint y = r;\r\n\r\n\tstd::vector line;\r\n\r\n\tline = bresLine(x0, y0 - r, x0, y0 + r);\r\n\tfor (std::vector::iterator it = line.begin(); it != line.end(); ++it)\r\n\t{\r\n\t\tmask[it->x - x0 + r][it->y - y0 + r] = true;\r\n\t}\r\n\r\n\tline = bresLine(x0 - r, y0, x0 + r, y0);\r\n\tfor (std::vector::iterator it = line.begin(); it != line.end(); ++it)\r\n\t{\r\n\t\tmask[it->x - x0 + r][it->y - y0 + r] = true;\r\n\t}\r\n\r\n\twhile (x < y)\r\n\t{\r\n\t\tif (f >= 0)\r\n\t\t{\r\n\t\t\ty--;\r\n\t\t\tddF_y += 2;\r\n\t\t\tf += ddF_y;\r\n\t\t}\r\n\r\n\t\tx++;\r\n\t\tddF_x += 2;\r\n\t\tf += ddF_x;\r\n\r\n\t\tline = bresLine(x0 - x, y0 + y, x0 + x, y0 + y);\r\n\t\tfor (std::vector::iterator it = line.begin(); it != line.end(); ++it)\r\n\t\t{\r\n\t\t\tmask[it->x - x0 + r][it->y - y0 + r] = true;\r\n\t\t}\r\n\r\n\t\tline = bresLine(x0 - x, y0 - y, x0 + x, y0 - y);\r\n\t\tfor (std::vector::iterator it = line.begin(); it != line.end(); ++it)\r\n\t\t{\r\n\t\t\tmask[it->x - x0 + r][it->y - y0 + r] = true;\r\n\t\t}\r\n\r\n\t\tline = bresLine(x0 - y, y0 + x, x0 + y, y0 + x);\r\n\t\tfor (std::vector::iterator it = line.begin(); it != line.end(); ++it)\r\n\t\t{\r\n\t\t\tmask[it->x - x0 + r][it->y - y0 + r] = true;\r\n\t\t}\r\n\r\n\t\tline = bresLine(x0 - y, y0 - x, x0 + y, y0 - x);\r\n\t\tfor (std::vector::iterator it = line.begin(); it != line.end(); ++it)\r\n\t\t{\r\n\t\t\tmask[it->x - x0 + r][it->y - y0 + r] = true;\r\n\t\t}\r\n\t}\r\n\r\n\tstd::vector cells;\r\n\tfor (int i = 0; i < 2 * r + 1; ++i)\r\n\t{\r\n\t\tfor (int j = 0; j < 2 * r + 1; ++j)\r\n\t\t{\r\n\t\t\tif (mask[i][j])\r\n\t\t\t{\r\n\t\t\t\tcells.push_back(cv::Point2i(i - r + x0, j - r + y0));\r\n\t\t\t}\r\n\t\t}\r\n\t}\r\n\r\n\treturn cells;\r\n}\r\n\r\nvoid\r\nfitCircle(const std::vector& points,\r\n double& centerX, double& centerY, double& radius)\r\n{\r\n // D. Umbach, and K. Jones, A Few Methods for Fitting Circles to Data,\r\n // IEEE Transactions on Instrumentation and Measurement, 2000\r\n // We use the modified least squares method.\r\n double sum_x = 0.0;\r\n double sum_y = 0.0;\r\n double sum_xx = 0.0;\r\n double sum_xy = 0.0;\r\n double sum_yy = 0.0;\r\n double sum_xxx = 0.0;\r\n double sum_xxy = 0.0;\r\n double sum_xyy = 0.0;\r\n double sum_yyy = 0.0;\r\n\r\n int n = points.size();\r\n for (int i = 0; i < n; ++i)\r\n {\r\n double x = points.at(i).x;\r\n double y = points.at(i).y;\r\n\r\n sum_x += x;\r\n sum_y += y;\r\n sum_xx += x * x;\r\n sum_xy += x * y;\r\n sum_yy += y * y;\r\n sum_xxx += x * x * x;\r\n sum_xxy += x * x * y;\r\n sum_xyy += x * y * y;\r\n sum_yyy += y * y * y;\r\n }\r\n\r\n double A = n * sum_xx - square(sum_x);\r\n double B = n * sum_xy - sum_x * sum_y;\r\n double C = n * sum_yy - square(sum_y);\r\n double D = 0.5 * (n * sum_xyy - sum_x * sum_yy + n * sum_xxx - sum_x * sum_xx);\r\n double E = 0.5 * (n * sum_xxy - sum_y * sum_xx + n * sum_yyy - sum_y * sum_yy);\r\n\r\n centerX = (D * C - B * E) / (A * C - square(B));\r\n centerY = (A * E - B * D) / (A * C - square(B));\r\n\r\n double sum_r = 0.0;\r\n for (int i = 0; i < n; ++i)\r\n {\r\n double x = points.at(i).x;\r\n double y = points.at(i).y;\r\n\r\n sum_r += hypot(x - centerX, y - centerY);\r\n }\r\n\r\n radius = sum_r / n;\r\n}\r\n\r\nstd::vector\r\nintersectCircles(double x1, double y1, double r1,\r\n double x2, double y2, double r2)\r\n{\r\n std::vector ipts;\r\n\r\n double d = hypot(x1 - x2, y1 - y2);\r\n if (d > r1 + r2)\r\n {\r\n // circles are separate\r\n return ipts;\r\n }\r\n if (d < fabs(r1 - r2))\r\n {\r\n // one circle is contained within the other\r\n return ipts;\r\n }\r\n\r\n double a = (square(r1) - square(r2) + square(d)) / (2.0 * d);\r\n double h = sqrt(square(r1) - square(a));\r\n\r\n double x3 = x1 + a * (x2 - x1) / d;\r\n double y3 = y1 + a * (y2 - y1) / d;\r\n\r\n if (h < 1e-10)\r\n {\r\n // two circles touch at one point\r\n ipts.push_back(cv::Point2d(x3, y3));\r\n return ipts;\r\n }\r\n\r\n ipts.push_back(cv::Point2d(x3 + h * (y2 - y1) / d,\r\n y3 - h * (x2 - x1) / d));\r\n ipts.push_back(cv::Point2d(x3 - h * (y2 - y1) / d,\r\n y3 + h * (x2 - x1) / d));\r\n return ipts;\r\n}\r\n\r\nchar\r\nUTMLetterDesignator(double latitude)\r\n{\r\n // This routine determines the correct UTM letter designator for the given latitude\r\n // returns 'Z' if latitude is outside the UTM limits of 84N to 80S\r\n // Written by Chuck Gantz- chuck.gantz@globalstar.com\r\n char letterDesignator;\r\n\r\n if ((84.0 >= latitude) && (latitude >= 72.0)) letterDesignator = 'X';\r\n else if ((72.0 > latitude) && (latitude >= 64.0)) letterDesignator = 'W';\r\n else if ((64.0 > latitude) && (latitude >= 56.0)) letterDesignator = 'V';\r\n else if ((56.0 > latitude) && (latitude >= 48.0)) letterDesignator = 'U';\r\n else if ((48.0 > latitude) && (latitude >= 40.0)) letterDesignator = 'T';\r\n else if ((40.0 > latitude) && (latitude >= 32.0)) letterDesignator = 'S';\r\n else if ((32.0 > latitude) && (latitude >= 24.0)) letterDesignator = 'R';\r\n else if ((24.0 > latitude) && (latitude >= 16.0)) letterDesignator = 'Q';\r\n else if ((16.0 > latitude) && (latitude >= 8.0)) letterDesignator = 'P';\r\n else if (( 8.0 > latitude) && (latitude >= 0.0)) letterDesignator = 'N';\r\n else if (( 0.0 > latitude) && (latitude >= -8.0)) letterDesignator = 'M';\r\n else if ((-8.0 > latitude) && (latitude >= -16.0)) letterDesignator = 'L';\r\n else if ((-16.0 > latitude) && (latitude >= -24.0)) letterDesignator = 'K';\r\n else if ((-24.0 > latitude) && (latitude >= -32.0)) letterDesignator = 'J';\r\n else if ((-32.0 > latitude) && (latitude >= -40.0)) letterDesignator = 'H';\r\n else if ((-40.0 > latitude) && (latitude >= -48.0)) letterDesignator = 'G';\r\n else if ((-48.0 > latitude) && (latitude >= -56.0)) letterDesignator = 'F';\r\n else if ((-56.0 > latitude) && (latitude >= -64.0)) letterDesignator = 'E';\r\n else if ((-64.0 > latitude) && (latitude >= -72.0)) letterDesignator = 'D';\r\n else if ((-72.0 > latitude) && (latitude >= -80.0)) letterDesignator = 'C';\r\n else letterDesignator = 'Z'; //This is here as an error flag to show that the Latitude is outside the UTM limits\r\n\r\n return letterDesignator;\r\n}\r\n\r\nvoid\r\nLLtoUTM(double latitude, double longitude,\r\n double& utmNorthing, double& utmEasting, std::string& utmZone)\r\n{\r\n // converts lat/long to UTM coords. Equations from USGS Bulletin 1532\r\n // East Longitudes are positive, West longitudes are negative.\r\n // North latitudes are positive, South latitudes are negative\r\n // Lat and Long are in decimal degrees\r\n // Written by Chuck Gantz- chuck.gantz@globalstar.com\r\n\r\n double k0 = 0.9996;\r\n\r\n double LongOrigin;\r\n double eccPrimeSquared;\r\n double N, T, C, A, M;\r\n\r\n double LatRad = latitude * M_PI / 180.0;\r\n double LongRad = longitude * M_PI / 180.0;\r\n double LongOriginRad;\r\n\r\n int ZoneNumber = static_cast((longitude + 180.0) / 6.0) + 1;\r\n\r\n if (latitude >= 56.0 && latitude < 64.0 &&\r\n longitude >= 3.0 && longitude < 12.0) {\r\n ZoneNumber = 32;\r\n }\r\n\r\n // Special zones for Svalbard\r\n if (latitude >= 72.0 && latitude < 84.0) {\r\n if ( longitude >= 0.0 && longitude < 9.0) ZoneNumber = 31;\r\n else if (longitude >= 9.0 && longitude < 21.0) ZoneNumber = 33;\r\n else if (longitude >= 21.0 && longitude < 33.0) ZoneNumber = 35;\r\n else if (longitude >= 33.0 && longitude < 42.0) ZoneNumber = 37;\r\n }\r\n LongOrigin = static_cast((ZoneNumber - 1) * 6 - 180 + 3); //+3 puts origin in middle of zone\r\n LongOriginRad = LongOrigin * M_PI / 180.0;\r\n\r\n // compute the UTM Zone from the latitude and longitude\r\n std::ostringstream oss;\r\n oss << ZoneNumber << UTMLetterDesignator(latitude);\r\n utmZone = oss.str();\r\n\r\n eccPrimeSquared = WGS84_ECCSQ / (1.0 - WGS84_ECCSQ);\r\n\r\n N = WGS84_A / sqrt(1.0 - WGS84_ECCSQ * sin(LatRad) * sin(LatRad));\r\n T = tan(LatRad) * tan(LatRad);\r\n C = eccPrimeSquared * cos(LatRad) * cos(LatRad);\r\n A = cos(LatRad) * (LongRad - LongOriginRad);\r\n\r\n M = WGS84_A * ((1.0 - WGS84_ECCSQ / 4.0\r\n - 3.0 * WGS84_ECCSQ * WGS84_ECCSQ / 64.0\r\n - 5.0 * WGS84_ECCSQ * WGS84_ECCSQ * WGS84_ECCSQ / 256.0)\r\n * LatRad\r\n - (3.0 * WGS84_ECCSQ / 8.0\r\n + 3.0 * WGS84_ECCSQ * WGS84_ECCSQ / 32.0\r\n + 45.0 * WGS84_ECCSQ * WGS84_ECCSQ * WGS84_ECCSQ / 1024.0)\r\n * sin(2.0 * LatRad)\r\n + (15.0 * WGS84_ECCSQ * WGS84_ECCSQ / 256.0\r\n + 45.0 * WGS84_ECCSQ * WGS84_ECCSQ * WGS84_ECCSQ / 1024.0)\r\n * sin(4.0 * LatRad)\r\n - (35.0 * WGS84_ECCSQ * WGS84_ECCSQ * WGS84_ECCSQ / 3072.0)\r\n * sin(6.0 * LatRad));\r\n\r\n utmEasting = k0 * N * (A + (1.0 - T + C) * A * A * A / 6.0\r\n + (5.0 - 18.0 * T + T * T + 72.0 * C\r\n - 58.0 * eccPrimeSquared)\r\n * A * A * A * A * A / 120.0)\r\n + 500000.0;\r\n\r\n utmNorthing = k0 * (M + N * tan(LatRad) *\r\n (A * A / 2.0 +\r\n (5.0 - T + 9.0 * C + 4.0 * C * C) * A * A * A * A / 24.0\r\n + (61.0 - 58.0 * T + T * T + 600.0 * C\r\n - 330.0 * eccPrimeSquared)\r\n * A * A * A * A * A * A / 720.0));\r\n if (latitude < 0.0) {\r\n utmNorthing += 10000000.0; //10000000 meter offset for southern hemisphere\r\n }\r\n}\r\n\r\nvoid\r\nUTMtoLL(double utmNorthing, double utmEasting, const std::string& utmZone,\r\n double& latitude, double& longitude)\r\n{\r\n // converts UTM coords to lat/long. Equations from USGS Bulletin 1532\r\n // East Longitudes are positive, West longitudes are negative.\r\n // North latitudes are positive, South latitudes are negative\r\n // Lat and Long are in decimal degrees.\r\n // Written by Chuck Gantz- chuck.gantz@globalstar.com\r\n\r\n double k0 = 0.9996;\r\n double eccPrimeSquared;\r\n double e1 = (1.0 - sqrt(1.0 - WGS84_ECCSQ)) / (1.0 + sqrt(1.0 - WGS84_ECCSQ));\r\n double N1, T1, C1, R1, D, M;\r\n double LongOrigin;\r\n double mu, phi1, phi1Rad;\r\n double x, y;\r\n int ZoneNumber;\r\n char ZoneLetter;\r\n bool NorthernHemisphere;\r\n\r\n x = utmEasting - 500000.0; //remove 500,000 meter offset for longitude\r\n y = utmNorthing;\r\n\r\n std::istringstream iss(utmZone);\r\n iss >> ZoneNumber >> ZoneLetter;\r\n if ((static_cast(ZoneLetter) - static_cast('N')) >= 0) {\r\n NorthernHemisphere = true;//point is in northern hemisphere\r\n } else {\r\n NorthernHemisphere = false;//point is in southern hemisphere\r\n y -= 10000000.0;//remove 10,000,000 meter offset used for southern hemisphere\r\n }\r\n\r\n LongOrigin = (ZoneNumber - 1.0) * 6.0 - 180.0 + 3.0; //+3 puts origin in middle of zone\r\n\r\n eccPrimeSquared = WGS84_ECCSQ / (1.0 - WGS84_ECCSQ);\r\n\r\n M = y / k0;\r\n mu = M / (WGS84_A * (1.0 - WGS84_ECCSQ / 4.0\r\n - 3.0 * WGS84_ECCSQ * WGS84_ECCSQ / 64.0\r\n - 5.0 * WGS84_ECCSQ * WGS84_ECCSQ * WGS84_ECCSQ / 256.0));\r\n\r\n phi1Rad = mu + (3.0 * e1 / 2.0 - 27.0 * e1 * e1 * e1 / 32.0) * sin(2.0 * mu)\r\n + (21.0 * e1 * e1 / 16.0 - 55.0 * e1 * e1 * e1 * e1 / 32.0)\r\n * sin(4.0 * mu)\r\n + (151.0 * e1 * e1 * e1 / 96.0) * sin(6.0 * mu);\r\n phi1 = phi1Rad / M_PI * 180.0;\r\n\r\n N1 = WGS84_A / sqrt(1.0 - WGS84_ECCSQ * sin(phi1Rad) * sin(phi1Rad));\r\n T1 = tan(phi1Rad) * tan(phi1Rad);\r\n C1 = eccPrimeSquared * cos(phi1Rad) * cos(phi1Rad);\r\n R1 = WGS84_A * (1.0 - WGS84_ECCSQ) /\r\n pow(1.0 - WGS84_ECCSQ * sin(phi1Rad) * sin(phi1Rad), 1.5);\r\n D = x / (N1 * k0);\r\n\r\n latitude = phi1Rad - (N1 * tan(phi1Rad) / R1)\r\n * (D * D / 2.0 - (5.0 + 3.0 * T1 + 10.0 * C1 - 4.0 * C1 * C1\r\n - 9.0 * eccPrimeSquared) * D * D * D * D / 24.0\r\n + (61.0 + 90.0 * T1 + 298.0 * C1 + 45.0 * T1 * T1\r\n - 252.0 * eccPrimeSquared - 3.0 * C1 * C1)\r\n * D * D * D * D * D * D / 720.0);\r\n latitude *= 180.0 / M_PI;\r\n\r\n longitude = (D - (1.0 + 2.0 * T1 + C1) * D * D * D / 6.0\r\n + (5.0 - 2.0 * C1 + 28.0 * T1 - 3.0 * C1 * C1\r\n + 8.0 * eccPrimeSquared + 24.0 * T1 * T1)\r\n * D * D * D * D * D / 120.0) / cos(phi1Rad);\r\n longitude = LongOrigin + longitude / M_PI * 180.0;\r\n}\r\n\r\nlong int\r\ntimestampDiff(uint64_t t1, uint64_t t2)\r\n{\r\n if (t2 > t1)\r\n {\r\n uint64_t d = t2 - t1;\r\n\r\n if (d > std::numeric_limits::max())\r\n {\r\n return std::numeric_limits::max();\r\n }\r\n else\r\n {\r\n return d;\r\n }\r\n }\r\n else\r\n {\r\n uint64_t d = t1 - t2;\r\n\r\n if (d > std::numeric_limits::max())\r\n {\r\n return std::numeric_limits::min();\r\n }\r\n else\r\n {\r\n return - static_cast(d);\r\n }\r\n }\r\n}\r\n\r\n}\r\n" }, { "path": "camera_model/src/intrinsic_calib.cc", "content": "#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n\n#include \"camodocal/chessboard/Chessboard.h\"\n#include \"camodocal/calib/CameraCalibration.h\"\n#include \"camodocal/gpl/gpl.h\"\n\nint main(int argc, char** argv)\n{\n cv::Size boardSize;\n float squareSize;\n std::string inputDir;\n std::string cameraModel;\n std::string cameraName;\n std::string prefix;\n std::string fileExtension;\n bool useOpenCV;\n bool viewResults;\n bool verbose;\n\n //========= Handling Program options =========\n boost::program_options::options_description desc(\"Allowed options\");\n desc.add_options()\n (\"help\", \"produce help message\")\n (\"width,w\", boost::program_options::value(&boardSize.width)->default_value(8), \"Number of inner corners on the chessboard pattern in x direction\")\n (\"height,h\", boost::program_options::value(&boardSize.height)->default_value(12), \"Number of inner corners on the chessboard pattern in y direction\")\n (\"size,s\", boost::program_options::value(&squareSize)->default_value(7.f), \"Size of one square in mm\")\n (\"input,i\", boost::program_options::value(&inputDir)->default_value(\"calibrationdata\"), \"Input directory containing chessboard images\")\n (\"prefix,p\", boost::program_options::value(&prefix)->default_value(\"left-\"), \"Prefix of images\")\n (\"file-extension,e\", boost::program_options::value(&fileExtension)->default_value(\".png\"), \"File extension of images\")\n (\"camera-model\", boost::program_options::value(&cameraModel)->default_value(\"mei\"), \"Camera model: kannala-brandt | mei | pinhole\")\n (\"camera-name\", boost::program_options::value(&cameraName)->default_value(\"camera\"), \"Name of camera\")\n (\"opencv\", boost::program_options::bool_switch(&useOpenCV)->default_value(true), \"Use OpenCV to detect corners\")\n (\"view-results\", boost::program_options::bool_switch(&viewResults)->default_value(false), \"View results\")\n (\"verbose,v\", boost::program_options::bool_switch(&verbose)->default_value(true), \"Verbose output\")\n ;\n\n boost::program_options::positional_options_description pdesc;\n pdesc.add(\"input\", 1);\n\n boost::program_options::variables_map vm;\n boost::program_options::store(boost::program_options::command_line_parser(argc, argv).options(desc).positional(pdesc).run(), vm);\n boost::program_options::notify(vm);\n\n if (vm.count(\"help\"))\n {\n std::cout << desc << std::endl;\n return 1;\n }\n\n if (!boost::filesystem::exists(inputDir) && !boost::filesystem::is_directory(inputDir))\n {\n std::cerr << \"# ERROR: Cannot find input directory \" << inputDir << \".\" << std::endl;\n return 1;\n }\n\n camodocal::Camera::ModelType modelType;\n if (boost::iequals(cameraModel, \"kannala-brandt\"))\n {\n modelType = camodocal::Camera::KANNALA_BRANDT;\n }\n else if (boost::iequals(cameraModel, \"mei\"))\n {\n modelType = camodocal::Camera::MEI;\n }\n else if (boost::iequals(cameraModel, \"pinhole\"))\n {\n modelType = camodocal::Camera::PINHOLE;\n }\n else if (boost::iequals(cameraModel, \"scaramuzza\"))\n {\n modelType = camodocal::Camera::SCARAMUZZA;\n }\n else\n {\n std::cerr << \"# ERROR: Unknown camera model: \" << cameraModel << std::endl;\n return 1;\n }\n\n switch (modelType)\n {\n case camodocal::Camera::KANNALA_BRANDT:\n std::cout << \"# INFO: Camera model: Kannala-Brandt\" << std::endl;\n break;\n case camodocal::Camera::MEI:\n std::cout << \"# INFO: Camera model: Mei\" << std::endl;\n break;\n case camodocal::Camera::PINHOLE:\n std::cout << \"# INFO: Camera model: Pinhole\" << std::endl;\n break;\n case camodocal::Camera::SCARAMUZZA:\n std::cout << \"# INFO: Camera model: Scaramuzza-Omnidirect\" << std::endl;\n break;\n }\n\n // look for images in input directory\n std::vector imageFilenames;\n boost::filesystem::directory_iterator itr;\n for (boost::filesystem::directory_iterator itr(inputDir); itr != boost::filesystem::directory_iterator(); ++itr)\n {\n if (!boost::filesystem::is_regular_file(itr->status()))\n {\n continue;\n }\n\n std::string filename = itr->path().filename().string();\n\n // check if prefix matches\n if (!prefix.empty())\n {\n if (filename.compare(0, prefix.length(), prefix) != 0)\n {\n continue;\n }\n }\n\n // check if file extension matches\n if (filename.compare(filename.length() - fileExtension.length(), fileExtension.length(), fileExtension) != 0)\n {\n continue;\n }\n\n imageFilenames.push_back(itr->path().string());\n\n if (verbose)\n {\n std::cerr << \"# INFO: Adding \" << imageFilenames.back() << std::endl;\n }\n }\n\n if (imageFilenames.empty())\n {\n std::cerr << \"# ERROR: No chessboard images found.\" << std::endl;\n return 1;\n }\n\n if (verbose)\n {\n std::cerr << \"# INFO: # images: \" << imageFilenames.size() << std::endl;\n }\n\n std::sort(imageFilenames.begin(), imageFilenames.end());\n\n cv::Mat image = cv::imread(imageFilenames.front(), -1);\n const cv::Size frameSize = image.size();\n\n camodocal::CameraCalibration calibration(modelType, cameraName, frameSize, boardSize, squareSize);\n calibration.setVerbose(verbose);\n\n std::vector chessboardFound(imageFilenames.size(), false);\n for (size_t i = 0; i < imageFilenames.size(); ++i)\n {\n image = cv::imread(imageFilenames.at(i), -1);\n\n camodocal::Chessboard chessboard(boardSize, image);\n\n chessboard.findCorners(useOpenCV);\n if (chessboard.cornersFound())\n {\n if (verbose)\n {\n std::cerr << \"# INFO: Detected chessboard in image \" << i + 1 << \", \" << imageFilenames.at(i) << std::endl;\n }\n\n calibration.addChessboardData(chessboard.getCorners());\n\n cv::Mat sketch;\n chessboard.getSketch().copyTo(sketch);\n\n cv::imshow(\"Image\", sketch);\n cv::waitKey(50);\n }\n else if (verbose)\n {\n std::cerr << \"# INFO: Did not detect chessboard in image \" << i + 1 << std::endl;\n }\n chessboardFound.at(i) = chessboard.cornersFound();\n }\n cv::destroyWindow(\"Image\");\n\n if (calibration.sampleCount() < 10)\n {\n std::cerr << \"# ERROR: Insufficient number of detected chessboards.\" << std::endl;\n return 1;\n }\n\n if (verbose)\n {\n std::cerr << \"# INFO: Calibrating...\" << std::endl;\n }\n\n double startTime = camodocal::timeInSeconds();\n\n calibration.calibrate();\n calibration.writeParams(cameraName + \"_camera_calib.yaml\");\n calibration.writeChessboardData(cameraName + \"_chessboard_data.dat\");\n\n if (verbose)\n {\n std::cout << \"# INFO: Calibration took a total time of \"\n << std::fixed << std::setprecision(3) << camodocal::timeInSeconds() - startTime\n << \" sec.\\n\";\n }\n\n if (verbose)\n {\n std::cerr << \"# INFO: Wrote calibration file to \" << cameraName + \"_camera_calib.yaml\" << std::endl;\n }\n\n if (viewResults)\n {\n std::vector cbImages;\n std::vector cbImageFilenames;\n\n for (size_t i = 0; i < imageFilenames.size(); ++i)\n {\n if (!chessboardFound.at(i))\n {\n continue;\n }\n\n cbImages.push_back(cv::imread(imageFilenames.at(i), -1));\n cbImageFilenames.push_back(imageFilenames.at(i));\n }\n\n // visualize observed and reprojected points\n calibration.drawResults(cbImages);\n\n for (size_t i = 0; i < cbImages.size(); ++i)\n {\n cv::putText(cbImages.at(i), cbImageFilenames.at(i), cv::Point(10,20),\n cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(255, 255, 255),\n 1, cv::LINE_AA);\n cv::imshow(\"Image\", cbImages.at(i));\n cv::waitKey(0);\n }\n }\n\n return 0;\n}\n" }, { "path": "camera_model/src/sparse_graph/Transform.cc", "content": "#include \n\nnamespace camodocal\n{\n\nTransform::Transform()\n{\n m_q.setIdentity();\n m_t.setZero();\n}\n\nTransform::Transform(const Eigen::Matrix4d& H)\n{\n m_q = Eigen::Quaterniond(H.block<3,3>(0,0));\n m_t = H.block<3,1>(0,3);\n}\n\nEigen::Quaterniond&\nTransform::rotation(void)\n{\n return m_q;\n}\n\nconst Eigen::Quaterniond&\nTransform::rotation(void) const\n{\n return m_q;\n}\n\ndouble*\nTransform::rotationData(void)\n{\n return m_q.coeffs().data();\n}\n\nconst double* const\nTransform::rotationData(void) const\n{\n return m_q.coeffs().data();\n}\n\nEigen::Vector3d&\nTransform::translation(void)\n{\n return m_t;\n}\n\nconst Eigen::Vector3d&\nTransform::translation(void) const\n{\n return m_t;\n}\n\ndouble*\nTransform::translationData(void)\n{\n return m_t.data();\n}\n\nconst double* const\nTransform::translationData(void) const\n{\n return m_t.data();\n}\n\nEigen::Matrix4d\nTransform::toMatrix(void) const\n{\n Eigen::Matrix4d H;\n H.setIdentity();\n H.block<3,3>(0,0) = m_q.toRotationMatrix();\n H.block<3,1>(0,3) = m_t;\n\n return H;\n}\n\n}\n" }, { "path": "config/campus.rviz", "content": "Panels:\n - Class: rviz/Displays\n Help Height: 0\n Name: Displays\n Property Tree Widget:\n Expanded:\n - /Global Options1\n - /Status1\n - /raw_image1\n - /VIO1\n - /VIO1/Path1\n - /pose_graph1/pose_graph_path1\n - /Odometry1\n - /Odometry2\n - /Odometry2/Shape1\n Splitter Ratio: 0.4651159942150116\n Tree Height: 441\n - Class: rviz/Selection\n Name: Selection\n - Class: rviz/Tool Properties\n Expanded:\n - /2D Pose Estimate1\n - /2D Nav Goal1\n - /Publish Point1\n Name: Tool Properties\n Splitter Ratio: 0.5886790156364441\n - Class: rviz/Views\n Expanded:\n - /Current View1\n Name: Views\n Splitter Ratio: 0.5\n - Class: rviz/Time\n Experimental: false\n Name: Time\n SyncMode: 0\n SyncSource: raw_image\n - Class: rviz/Displays\n Help Height: 78\n Name: Displays\n Property Tree Widget:\n Expanded:\n - /tracked image1\n - /raw_image1\n - /VIO1\n - /VIO1/Path1\n - /pose_graph1\n - /pose_graph1/pose_graph_path1\n - /pose_graph1/loop_match_image1\n - /pose_graph1/loop_match_image1/Status1\n - /Image1\n Splitter Ratio: 0.5\n Tree Height: 741\nPreferences:\n PromptSaveOnExit: true\nToolbars:\n toolButtonStyle: 2\nVisualization Manager:\n Class: \"\"\n Displays:\n - Alpha: 0.5\n Cell Size: 1\n Class: rviz/Grid\n Color: 130; 130; 130\n Enabled: true\n Line Style:\n Line Width: 0.029999999329447746\n Value: Lines\n Name: Grid\n Normal Cell Count: 0\n Offset:\n X: 0\n Y: 0\n Z: 0\n Plane: XY\n Plane Cell Count: 10\n Reference Frame: \n Value: true\n - Class: rviz/Axes\n Enabled: false\n Length: 1\n Name: Axes\n Radius: 0.10000000149011612\n Reference Frame: \n Value: false\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 255; 0; 0\n Enabled: true\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Lines\n Line Width: 0.5\n Name: ground_truth_path\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /benchmark_publisher/path\n Unreliable: false\n Value: true\n - Class: rviz/Image\n Enabled: true\n Image Topic: /vins_estimator/feature_img\n Max Value: 1\n Median window: 5\n Min Value: 0\n Name: tracked image\n Normalize Range: true\n Queue Size: 2\n Transport Hint: raw\n Unreliable: false\n Value: true\n - Class: rviz/Image\n Enabled: false\n Image Topic: /camera/color/image_raw\n Max Value: 1\n Median window: 5\n Min Value: 0\n Name: raw_image\n Normalize Range: true\n Queue Size: 2\n Transport Hint: raw\n Unreliable: false\n Value: false\n - Class: rviz/Group\n Displays:\n - Alpha: 10\n Buffer Length: 1\n Class: rviz/Path\n Color: 25; 255; 0\n Enabled: true\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Billboards\n Line Width: 1\n Name: Path\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /vins_estimator/path\n Unreliable: false\n Value: true\n - Class: rviz/MarkerArray\n Enabled: true\n Marker Topic: /vins_estimator/camera_pose_visual\n Name: camera_visual\n Namespaces:\n {}\n Queue Size: 100\n Value: true\n - Alpha: 1\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 10\n Min Value: -10\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/PointCloud\n Color: 0; 255; 0\n Color Transformer: FlatColor\n Decay Time: 1\n Enabled: false\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Min Color: 0; 0; 0\n Name: current_point\n Position Transformer: XYZ\n Queue Size: 1\n Selectable: true\n Size (Pixels): 1\n Size (m): 0.009999999776482582\n Style: Points\n Topic: /vins_estimator/point_cloud\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: false\n - Alpha: 1\n Autocompute Intensity Bounds: false\n Autocompute Value Bounds:\n Max Value: 10\n Min Value: -10\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/PointCloud\n Color: 255; 255; 255\n Color Transformer: FlatColor\n Decay Time: 3000\n Enabled: false\n Invert Rainbow: true\n Max Color: 0; 0; 0\n Max Intensity: 4096\n Min Color: 0; 0; 0\n Min Intensity: 0\n Name: history_point\n Position Transformer: XYZ\n Queue Size: 10\n Selectable: true\n Size (Pixels): 1\n Size (m): 0.5\n Style: Points\n Topic: /vins_estimator/history_cloud\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: false\n Value: false\n - Class: rviz/TF\n Enabled: false\n Frame Timeout: 15\n Frames:\n All Enabled: true\n Marker Scale: 1\n Name: TF\n Show Arrows: true\n Show Axes: true\n Show Names: true\n Tree:\n {}\n Update Interval: 0\n Value: false\n Enabled: true\n Name: VIO\n - Class: rviz/Group\n Displays:\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 252; 233; 79\n Enabled: true\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Billboards\n Line Width: 1\n Name: pose_graph_path\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /pose_graph/pose_graph_path\n Unreliable: false\n Value: true\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 255; 170; 0\n Enabled: false\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Lines\n Line Width: 0.029999999329447746\n Name: base_path\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /pose_graph/base_path\n Unreliable: false\n Value: false\n - Class: rviz/MarkerArray\n Enabled: false\n Marker Topic: /pose_graph/pose_graph\n Name: loop_visual\n Namespaces:\n {}\n Queue Size: 100\n Value: false\n - Class: rviz/MarkerArray\n Enabled: true\n Marker Topic: /pose_graph/camera_pose_visual\n Name: camera_visual\n Namespaces:\n {}\n Queue Size: 100\n Value: true\n - Class: rviz/Image\n Enabled: false\n Image Topic: /match_image\n Max Value: 1\n Median window: 5\n Min Value: 0\n Name: loop_match_image\n Normalize Range: true\n Queue Size: 2\n Transport Hint: raw\n Unreliable: false\n Value: false\n - Class: rviz/Marker\n Enabled: false\n Marker Topic: /pose_graph/key_odometrys\n Name: Marker\n Namespaces:\n {}\n Queue Size: 100\n Value: false\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 25; 255; 0\n Enabled: false\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Lines\n Line Width: 0.30000001192092896\n Name: Sequence1\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /pose_graph/path_1\n Unreliable: false\n Value: false\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 255; 0; 0\n Enabled: false\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Lines\n Line Width: 0.30000001192092896\n Name: Sequence2\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /pose_graph/path_2\n Unreliable: false\n Value: false\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 0; 85; 255\n Enabled: false\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Lines\n Line Width: 0.30000001192092896\n Name: Sequence3\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /pose_graph/path_3\n Unreliable: false\n Value: false\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 255; 170; 0\n Enabled: false\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Lines\n Line Width: 0.30000001192092896\n Name: Sequence4\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /pose_graph/path_4\n Unreliable: false\n Value: false\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 255; 170; 255\n Enabled: false\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Lines\n Line Width: 0.029999999329447746\n Name: Sequence5\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /pose_graph/path_5\n Unreliable: false\n Value: false\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 25; 255; 0\n Enabled: false\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Lines\n Line Width: 0.029999999329447746\n Name: no_loop_path\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /pose_graph/no_loop_path\n Unreliable: false\n Value: false\n Enabled: true\n Name: pose_graph\n - Angle Tolerance: 0.10000000149011612\n Class: rviz/Odometry\n Covariance:\n Orientation:\n Alpha: 0.5\n Color: 255; 255; 127\n Color Style: Unique\n Frame: Local\n Offset: 1\n Scale: 1\n Value: true\n Position:\n Alpha: 0.30000001192092896\n Color: 204; 51; 204\n Scale: 1\n Value: true\n Value: true\n Enabled: true\n Keep: 1\n Name: Odometry\n Position Tolerance: 0.10000000149011612\n Shape:\n Alpha: 1\n Axes Length: 1\n Axes Radius: 0.10000000149011612\n Color: 255; 25; 0\n Head Length: 0.30000001192092896\n Head Radius: 0.10000000149011612\n Shaft Length: 1\n Shaft Radius: 0.05000000074505806\n Value: Axes\n Topic: /vins_estimator/imu_propagate\n Unreliable: false\n Value: true\n - Angle Tolerance: 0.10000000149011612\n Class: rviz/Odometry\n Covariance:\n Orientation:\n Alpha: 0.5\n Color: 255; 255; 127\n Color Style: Unique\n Frame: Local\n Offset: 1\n Scale: 1\n Value: true\n Position:\n Alpha: 0.30000001192092896\n Color: 204; 51; 204\n Scale: 1\n Value: true\n Value: true\n Enabled: true\n Keep: 10000\n Name: Odometry\n Position Tolerance: 0.10000000149011612\n Shape:\n Alpha: 1\n Axes Length: 1\n Axes Radius: 0.10000000149011612\n Color: 255; 25; 0\n Head Length: 0.029999999329447746\n Head Radius: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Shaft Radius: 0.05000000074505806\n Value: Arrow\n Topic: /odom\n Unreliable: false\n Value: true\n - Class: rviz/Marker\n Enabled: true\n Marker Topic: /image_to_rviz/visualization_marker\n Name: Marker\n Namespaces:\n image: true\n Queue Size: 100\n Value: true\n - Class: rviz/Image\n Enabled: true\n Image Topic: /vins_estimator/semantic_mask\n Max Value: 1\n Median window: 5\n Min Value: 0\n Name: Image\n Normalize Range: true\n Queue Size: 2\n Transport Hint: raw\n Unreliable: false\n Value: true\n Enabled: true\n Global Options:\n Background Color: 0; 0; 0\n Default Light: true\n Fixed Frame: map\n Frame Rate: 30\n Name: root\n Tools:\n - Class: rviz/MoveCamera\n - Class: rviz/Select\n - Class: rviz/FocusCamera\n - Class: rviz/Measure\n - Class: rviz/SetInitialPose\n Theta std deviation: 0.2617993950843811\n Topic: /initialpose\n X std deviation: 0.5\n Y std deviation: 0.5\n - Class: rviz/SetGoal\n Topic: /move_base_simple/goal\n - Class: rviz/PublishPoint\n Single click: true\n Topic: /clicked_point\n Value: true\n Views:\n Current:\n Angle: -2.065000295639038\n Class: rviz/TopDownOrtho\n Enable Stereo Rendering:\n Stereo Eye Separation: 0.05999999865889549\n Stereo Focal Distance: 1\n Swap Stereo Eyes: false\n Value: false\n Invert Z Axis: false\n Name: Current View\n Near Clip Distance: 0.009999999776482582\n Scale: 2.8180553913116455\n Target Frame: \n Value: TopDownOrtho (rviz)\n X: 106.81703186035156\n Y: 118.53111267089844\n Saved:\n - Angle: -1.9099998474121094\n Class: rviz/TopDownOrtho\n Enable Stereo Rendering:\n Stereo Eye Separation: 0.05999999865889549\n Stereo Focal Distance: 1\n Swap Stereo Eyes: false\n Value: false\n Invert Z Axis: false\n Name: TopDownOrtho\n Near Clip Distance: 0.009999999776482582\n Scale: 2.8129937648773193\n Target Frame: \n Value: TopDownOrtho (rviz)\n X: 90.85389709472656\n Y: 126.16999053955078\nWindow Geometry:\n Displays:\n collapsed: false\n Height: 960\n Hide Left Dock: false\n Hide Right Dock: true\n Image:\n collapsed: false\n QMainWindow State: 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\n Selection:\n collapsed: false\n Time:\n collapsed: false\n Tool Properties:\n collapsed: false\n Views:\n collapsed: true\n Width: 1920\n X: 0\n Y: 27\n loop_match_image:\n collapsed: false\n raw_image:\n collapsed: false\n tracked image:\n collapsed: false\n" }, { "path": "config/indoor.rviz", "content": "Panels:\n - Class: rviz/Displays\n Help Height: 0\n Name: Displays\n Property Tree Widget:\n Expanded:\n - /Global Options1\n - /Status1\n - /raw_image1\n - /VIO1\n - /VIO1/Path1\n - /pose_graph1/pose_graph_path1\n Splitter Ratio: 0.4651159942150116\n Tree Height: 441\n - Class: rviz/Selection\n Name: Selection\n - Class: rviz/Tool Properties\n Expanded:\n - /2D Pose Estimate1\n - /2D Nav Goal1\n - /Publish Point1\n Name: Tool Properties\n Splitter Ratio: 0.5886790156364441\n - Class: rviz/Views\n Expanded:\n - /Current View1\n Name: Views\n Splitter Ratio: 0.5\n - Class: rviz/Time\n Experimental: false\n Name: Time\n SyncMode: 0\n SyncSource: raw_image\n - Class: rviz/Displays\n Help Height: 78\n Name: Displays\n Property Tree Widget:\n Expanded:\n - /tracked image1\n - /raw_image1\n - /VIO1\n - /VIO1/Path1\n - /pose_graph1\n - /pose_graph1/pose_graph_path1\n - /Odometry1\n Splitter Ratio: 0.5\n Tree Height: 731\nPreferences:\n PromptSaveOnExit: true\nToolbars:\n toolButtonStyle: 2\nVisualization Manager:\n Class: \"\"\n Displays:\n - Alpha: 0.5\n Cell Size: 1\n Class: rviz/Grid\n Color: 130; 130; 130\n Enabled: true\n Line Style:\n Line Width: 0.029999999329447746\n Value: Lines\n Name: Grid\n Normal Cell Count: 0\n Offset:\n X: 0\n Y: 0\n Z: 0\n Plane: XY\n Plane Cell Count: 10\n Reference Frame: \n Value: true\n - Class: rviz/Axes\n Enabled: false\n Length: 1\n Name: Axes\n Radius: 0.10000000149011612\n Reference Frame: \n Value: false\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 255; 0; 0\n Enabled: true\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Lines\n Line Width: 0.5\n Name: ground_truth_path\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /benchmark_publisher/path\n Unreliable: false\n Value: true\n - Class: rviz/Image\n Enabled: true\n Image Topic: /vins_estimator/feature_img\n Max Value: 1\n Median window: 5\n Min Value: 0\n Name: tracked image\n Normalize Range: true\n Queue Size: 2\n Transport Hint: raw\n Unreliable: false\n Value: true\n - Class: rviz/Image\n Enabled: true\n Image Topic: /camera/color/image_raw\n Max Value: 1\n Median window: 5\n Min Value: 0\n Name: raw_image\n Normalize Range: true\n Queue Size: 2\n Transport Hint: raw\n Unreliable: false\n Value: true\n - Class: rviz/Group\n Displays:\n - Alpha: 10\n Buffer Length: 1\n Class: rviz/Path\n Color: 25; 255; 0\n Enabled: true\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Lines\n Line Width: 1\n Name: Path\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /vins_estimator/path\n Unreliable: false\n Value: true\n - Class: rviz/MarkerArray\n Enabled: true\n Marker Topic: /vins_estimator/camera_pose_visual\n Name: camera_visual\n Namespaces:\n CameraPoseVisualization: true\n Queue Size: 100\n Value: true\n - Alpha: 1\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 10\n Min Value: -10\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/PointCloud\n Color: 0; 255; 0\n Color Transformer: FlatColor\n Decay Time: 1\n Enabled: false\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Min Color: 0; 0; 0\n Name: current_point\n Position Transformer: XYZ\n Queue Size: 1\n Selectable: true\n Size (Pixels): 1\n Size (m): 0.009999999776482582\n Style: Points\n Topic: /vins_estimator/point_cloud\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: false\n - Alpha: 1\n Autocompute Intensity Bounds: false\n Autocompute Value Bounds:\n Max Value: 10\n Min Value: -10\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/PointCloud\n Color: 255; 255; 255\n Color Transformer: FlatColor\n Decay Time: 3000\n Enabled: false\n Invert Rainbow: true\n Max Color: 0; 0; 0\n Max Intensity: 4096\n Min Color: 0; 0; 0\n Min Intensity: 0\n Name: history_point\n Position Transformer: XYZ\n Queue Size: 10\n Selectable: true\n Size (Pixels): 1\n Size (m): 0.5\n Style: Points\n Topic: /vins_estimator/history_cloud\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: false\n Value: false\n - Class: rviz/TF\n Enabled: false\n Frame Timeout: 15\n Frames:\n All Enabled: true\n Marker Scale: 1\n Name: TF\n Show Arrows: true\n Show Axes: true\n Show Names: true\n Tree:\n {}\n Update Interval: 0\n Value: false\n Enabled: true\n Name: VIO\n - Class: rviz/Group\n Displays:\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 239; 41; 41\n Enabled: true\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Lines\n Line Width: 1\n Name: pose_graph_path\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /pose_graph/pose_graph_path\n Unreliable: false\n Value: true\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 255; 170; 0\n Enabled: false\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Lines\n Line Width: 0.029999999329447746\n Name: base_path\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /pose_graph/base_path\n Unreliable: false\n Value: false\n - Class: rviz/MarkerArray\n Enabled: false\n Marker Topic: /pose_graph/pose_graph\n Name: loop_visual\n Namespaces:\n {}\n Queue Size: 100\n Value: false\n - Class: rviz/MarkerArray\n Enabled: true\n Marker Topic: /pose_graph/camera_pose_visual\n Name: camera_visual\n Namespaces:\n {}\n Queue Size: 100\n Value: true\n - Class: rviz/Image\n Enabled: true\n Image Topic: /pose_graph/match_image\n Max Value: 1\n Median window: 5\n Min Value: 0\n Name: loop_match_image\n Normalize Range: true\n Queue Size: 2\n Transport Hint: raw\n Unreliable: false\n Value: true\n - Class: rviz/Marker\n Enabled: false\n Marker Topic: /pose_graph/key_odometrys\n Name: Marker\n Namespaces:\n {}\n Queue Size: 100\n Value: false\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 25; 255; 0\n Enabled: false\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Lines\n Line Width: 0.30000001192092896\n Name: Sequence1\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /pose_graph/path_1\n Unreliable: false\n Value: false\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 255; 0; 0\n Enabled: false\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Lines\n Line Width: 0.30000001192092896\n Name: Sequence2\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /pose_graph/path_2\n Unreliable: false\n Value: false\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 0; 85; 255\n Enabled: false\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Lines\n Line Width: 0.30000001192092896\n Name: Sequence3\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /pose_graph/path_3\n Unreliable: false\n Value: false\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 255; 170; 0\n Enabled: false\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Lines\n Line Width: 0.30000001192092896\n Name: Sequence4\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /pose_graph/path_4\n Unreliable: false\n Value: false\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 255; 170; 255\n Enabled: false\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Lines\n Line Width: 0.029999999329447746\n Name: Sequence5\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /pose_graph/path_5\n Unreliable: false\n Value: false\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 25; 255; 0\n Enabled: false\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Lines\n Line Width: 0.029999999329447746\n Name: no_loop_path\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /pose_graph/no_loop_path\n Unreliable: false\n Value: false\n Enabled: true\n Name: pose_graph\n - Alpha: 1\n Axes Length: 1\n Axes Radius: 0.10000000149011612\n Class: rviz/Pose\n Color: 255; 25; 0\n Enabled: false\n Head Length: 0.30000001192092896\n Head Radius: 0.10000000149011612\n Name: Pose\n Shaft Length: 1\n Shaft Radius: 0.05000000074505806\n Shape: Arrow\n Topic: /mavros/vision_pose/remap_pose\n Unreliable: false\n Value: false\n - Angle Tolerance: 0.10000000149011612\n Class: rviz/Odometry\n Covariance:\n Orientation:\n Alpha: 0.5\n Color: 255; 255; 127\n Color Style: Unique\n Frame: Local\n Offset: 1\n Scale: 1\n Value: true\n Position:\n Alpha: 0.30000001192092896\n Color: 204; 51; 204\n Scale: 1\n Value: true\n Value: true\n Enabled: true\n Keep: 1\n Name: Odometry\n Position Tolerance: 0.10000000149011612\n Shape:\n Alpha: 1\n Axes Length: 1\n Axes Radius: 0.10000000149011612\n Color: 255; 25; 0\n Head Length: 0.30000001192092896\n Head Radius: 0.10000000149011612\n Shaft Length: 1\n Shaft Radius: 0.05000000074505806\n Value: Axes\n Topic: /vins_estimator/imu_propagate\n Unreliable: false\n Value: true\n Enabled: true\n Global Options:\n Background Color: 0; 0; 0\n Default Light: true\n Fixed Frame: map\n Frame Rate: 30\n Name: root\n Tools:\n - Class: rviz/MoveCamera\n - Class: rviz/Select\n - Class: rviz/FocusCamera\n - Class: rviz/Measure\n - Class: rviz/SetInitialPose\n Theta std deviation: 0.2617993950843811\n Topic: /initialpose\n X std deviation: 0.5\n Y std deviation: 0.5\n - Class: rviz/SetGoal\n Topic: /move_base_simple/goal\n - Class: rviz/PublishPoint\n Single click: true\n Topic: /clicked_point\n Value: true\n Views:\n Current:\n Class: rviz/XYOrbit\n Distance: 9.714073181152344\n Enable Stereo Rendering:\n Stereo Eye Separation: 0.05999999865889549\n Stereo Focal Distance: 1\n Swap Stereo Eyes: false\n Value: false\n Focal Point:\n X: -0.19246625900268555\n Y: 0.2890198230743408\n Z: 0\n Focal Shape Fixed Size: true\n Focal Shape Size: 0.05000000074505806\n Invert Z Axis: false\n Name: Current View\n Near Clip Distance: 0.009999999776482582\n Pitch: 0.4153980016708374\n Target Frame: \n Value: XYOrbit (rviz)\n Yaw: 3.9204025268554688\n Saved: ~\nWindow Geometry:\n Displays:\n collapsed: false\n Height: 960\n Hide Left Dock: false\n Hide Right Dock: true\n QMainWindow State: 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\n Selection:\n collapsed: false\n Time:\n collapsed: false\n Tool Properties:\n collapsed: false\n Views:\n collapsed: false\n Width: 1920\n X: 0\n Y: 27\n loop_match_image:\n collapsed: false\n raw_image:\n collapsed: false\n tracked image:\n collapsed: false\n" }, { "path": "config/openloris/openloris_vio.yaml", "content": "%YAML:1.0\n\nnum_threads: 0 # 0 Use the max number of threads of your device.\n # 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.)\n # x It is proper that 1 < x < MAX_THREAD.\n # For now, this parameter is relevant to grid-detector to run in parallel. \n\n#common parameters\nimu: 1\nstatic_init: 0\nimu_topic: \"/d400/imu0\"\nimage_topic: \"/d400/color/image_raw\"\ndepth_topic: \"/d400/aligned_depth_to_color/image_raw\"\noutput_path: \"/home/nrosliu/DynamicVINS-Atlas/src/Dynamic_VINS/output\"\n\n#RGBD camera Ideal Range\ndepth_min_dist: 0.3\ndepth_max_dist: 3\n\nfrontend_freq: 30 # It should be raised in VO mode(without IMU).\nkeyframe_parallax: 10.0 # keyframe selection threshold (pixel); if system fails frequently, please try to reduce the \"keyframe_parallax\" \nnum_grid_rows: 7\nnum_grid_cols: 8\n\n#unsynchronization parameters\nestimate_td: 1 ########## # online estimate time offset between camera and imu\ntd: 0.000 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)\n\n#camera calibration\nmodel_type: PINHOLE\ncamera_name: camera\nimage_width: 848\nimage_height: 480\n #TODO modify distortion\n\ndistortion_parameters:\n k1: 0.0\n k2: 0.0\n p1: 0.0\n p2: 0.0\nprojection_parameters:\n fx: 611.4509887695312\n fy: 611.4857177734375\n cx: 433.2039794921875\n cy: 249.4730224609375\n\n# Extrinsic parameter between IMU and Camera.\nestimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.\n # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.\n # 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.\n#If you choose 0 or 1, you should write down the following matrix.\n#Rotation from camera frame to imu frame, imu^R_cam\nextrinsicRotation: !!opencv-matrix\n rows: 3\n cols: 3\n dt: d\n data: [ 0.999975572679493, 0.003849141066713, 0.005854714944742,\n -0.003828680351062, 0.999986658473099, -0.003501944262433,\n -0.005868115609379, 0.003479442469180, 0.999976848846595]\n#Translation from camera frame to imu frame, imu^T_cam\nextrinsicTranslation: !!opencv-matrix\n rows: 3\n cols: 1\n dt: d\n data: [0.0203127935529, -0.00510325236246, -0.0112013882026]\n\n#feature traker paprameters\nmax_cnt: 130 # max feature number in feature tracking. It is suggested to be raised in VO mode.\nmin_dist: 30 # min distance between two features\nfreq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image\nF_threshold: 1.0 # ransac threshold (pixel)\nshow_track: 0 # publish tracking image as topic\nequalize: 0 # if image is too dark or light, trun on equalize to find enough features\nfisheye: 0 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points\n\n#optimization parameters\nmax_solver_time: 0.04 # max solver itration time (ms), to guarantee real time\nmax_num_iterations: 8 # max solver itrations, to guarantee real time\n\n#imu parameters The more accurate parameters you provide, the better performance\nacc_n: 0.1 # accelerometer measurement noise standard deviation. \ngyr_n: 0.01 # gyroscope measurement noise standard deviation. \nacc_w: 0.0002 # accelerometer bias random work noise standard deviation. #0.02\ngyr_w: 2.0e-5 # gyroscope bias random work noise standard deviation. #4.0e-5\n\ng_norm: 9.805 # gravity magnitude\n\n#rolling shutter parameters\nrolling_shutter: 1 # 0: global shutter camera, 1: rolling shutter camera\nrolling_shutter_tr: 0.033 # unit: s. rolling shutter read out time per frame (from data sheet)\n\n#loop closure parameters\nloop_closure: 0 # start loop closure\nfast_relocalization: 0 # useful in real-time and large project\nload_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path'\npose_graph_save_path: \"/home/nrosliu/DynamicVINS-Atlas/src/Dynamic_VINS/output/pose_graph\" # save and load path\n\n#visualization parameters\nsave_image: 0 # save image in pose graph for visualization prupose; you can close this function by setting 0\nvisualize_imu_forward: 0 # output imu forward propogation to achieve low latency and high frequence results\nvisualize_camera_size: 0.4 # size of camera marker in RVIZ\n\n#暂时仅考虑动物\ndynamic_label: [\"person\", \"cat\", \"dog\", \"bicycle\", \"car\",\"bus\"]\n\n" }, { "path": "config/openloris/openloris_vio_atlas.yaml", "content": "%YAML:1.0\n\nnum_threads: 6 # 0 Use the max number of threads of your device.\n # 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.)\n # x It is proper that 1 < x < MAX_THREAD.\n # For now, this parameter is relevant to grid-detector to run in parallel. \n\n\n#common parameters\nimu: 1\nstatic_init: 1\nimu_topic: \"/d400/imu0\"\nimage_topic: \"/decompressed_img\"\ndepth_topic: \"/d400/aligned_depth_to_color/image_raw\" # check your depth image bandwidth is lower than 30Mbs, otherwise compressedDepth is suggested\noutput_path: \"/home/nrosliu/DynamicVINS-Atlas/src/Dynamic_VINS/output\"\n\n# 0.3 \n Value: true\n - Alpha: 1\n Class: rviz/Axes\n Enabled: true\n Length: 1\n Name: Axes\n Radius: 0.10000000149011612\n Reference Frame: \n Show Trail: false\n Value: true\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 255; 0; 0\n Enabled: true\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Lines\n Line Width: 0.5\n Name: ground_truth_path\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Queue Size: 10\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /benchmark_publisher/path\n Unreliable: false\n Value: true\n - Class: rviz/Image\n Enabled: false\n Image Topic: /vins_estimator/feature_img\n Max Value: 1\n Median window: 5\n Min Value: 0\n Name: tracked image\n Normalize Range: true\n Queue Size: 2\n Transport Hint: raw\n Unreliable: false\n Value: false\n - Class: rviz/Image\n Enabled: false\n Image Topic: /camera/color/image_raw\n Max Value: 1\n Median window: 5\n Min Value: 0\n Name: raw_image\n Normalize Range: true\n Queue Size: 2\n Transport Hint: raw\n Unreliable: false\n Value: false\n - Class: rviz/Group\n Displays:\n - Alpha: 10\n Buffer Length: 1\n Class: rviz/Path\n Color: 255; 25; 0\n Enabled: true\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Billboards\n Line Width: 0.10000000149011612\n Name: Path\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Queue Size: 10\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /vins_estimator/path\n Unreliable: false\n Value: true\n - Class: rviz/MarkerArray\n Enabled: true\n Marker Topic: /vins_estimator/camera_pose_visual\n Name: camera_visual\n Namespaces:\n {}\n Queue Size: 100\n Value: true\n - Alpha: 1\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 10\n Min Value: -10\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/PointCloud\n Color: 0; 255; 0\n Color Transformer: FlatColor\n Decay Time: 10000\n Enabled: false\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Min Color: 0; 0; 0\n Name: current_point\n Position Transformer: XYZ\n Queue Size: 1\n Selectable: true\n Size (Pixels): 1\n Size (m): 0.009999999776482582\n Style: Points\n Topic: /vins_estimator/point_cloud\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: false\n - Alpha: 1\n Autocompute Intensity Bounds: false\n Autocompute Value Bounds:\n Max Value: 10\n Min Value: -10\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/PointCloud\n Color: 255; 255; 255\n Color Transformer: FlatColor\n Decay Time: 3000\n Enabled: false\n Invert Rainbow: true\n Max Color: 0; 0; 0\n Max Intensity: 4096\n Min Color: 0; 0; 0\n Min Intensity: 0\n Name: history_point\n Position Transformer: XYZ\n Queue Size: 10\n Selectable: true\n Size (Pixels): 1\n Size (m): 0.5\n Style: Points\n Topic: /vins_estimator/history_cloud\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: false\n Value: false\n - Class: rviz/TF\n Enabled: false\n Frame Timeout: 15\n Frames:\n All Enabled: true\n Marker Alpha: 1\n Marker Scale: 1\n Name: TF\n Show Arrows: true\n Show Axes: true\n Show Names: true\n Tree:\n {}\n Update Interval: 0\n Value: false\n Enabled: true\n Name: VIO\n - Class: rviz/Group\n Displays:\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 252; 233; 79\n Enabled: true\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Billboards\n Line Width: 1\n Name: pose_graph_path\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Queue Size: 10\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /pose_graph/pose_graph_path\n Unreliable: false\n Value: true\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 255; 170; 0\n Enabled: false\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Lines\n Line Width: 0.029999999329447746\n Name: base_path\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Queue Size: 10\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /pose_graph/base_path\n Unreliable: false\n Value: false\n - Class: rviz/MarkerArray\n Enabled: false\n Marker Topic: /pose_graph/pose_graph\n Name: loop_visual\n Namespaces:\n {}\n Queue Size: 100\n Value: false\n - Class: rviz/MarkerArray\n Enabled: true\n Marker Topic: /pose_graph/camera_pose_visual\n Name: camera_visual\n Namespaces:\n {}\n Queue Size: 100\n Value: true\n - Class: rviz/Image\n Enabled: false\n Image Topic: /pose_graph/match_image\n Max Value: 1\n Median window: 5\n Min Value: 0\n Name: loop_match_image\n Normalize Range: true\n Queue Size: 2\n Transport Hint: raw\n Unreliable: false\n Value: false\n - Class: rviz/Marker\n Enabled: false\n Marker Topic: /pose_graph/key_odometrys\n Name: Marker\n Namespaces:\n {}\n Queue Size: 100\n Value: false\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 25; 255; 0\n Enabled: false\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Lines\n Line Width: 0.30000001192092896\n Name: Sequence1\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Queue Size: 10\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /pose_graph/path_1\n Unreliable: false\n Value: false\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 255; 0; 0\n Enabled: false\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Lines\n Line Width: 0.30000001192092896\n Name: Sequence2\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Queue Size: 10\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /pose_graph/path_2\n Unreliable: false\n Value: false\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 0; 85; 255\n Enabled: false\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Lines\n Line Width: 0.30000001192092896\n Name: Sequence3\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Queue Size: 10\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /pose_graph/path_3\n Unreliable: false\n Value: false\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 255; 170; 0\n Enabled: false\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Lines\n Line Width: 0.30000001192092896\n Name: Sequence4\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Queue Size: 10\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /pose_graph/path_4\n Unreliable: false\n Value: false\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 255; 170; 255\n Enabled: false\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Lines\n Line Width: 0.029999999329447746\n Name: Sequence5\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Queue Size: 10\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /pose_graph/path_5\n Unreliable: false\n Value: false\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 25; 255; 0\n Enabled: false\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Lines\n Line Width: 0.029999999329447746\n Name: no_loop_path\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Queue Size: 10\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /pose_graph/no_loop_path\n Unreliable: false\n Value: false\n Enabled: true\n Name: pose_graph\n - Angle Tolerance: 0.10000000149011612\n Class: rviz/Odometry\n Covariance:\n Orientation:\n Alpha: 0.5\n Color: 255; 255; 127\n Color Style: Unique\n Frame: Local\n Offset: 1\n Scale: 1\n Value: true\n Position:\n Alpha: 0.30000001192092896\n Color: 204; 51; 204\n Scale: 1\n Value: true\n Value: true\n Enabled: true\n Keep: 1\n Name: Odometry\n Position Tolerance: 0.10000000149011612\n Queue Size: 10\n Shape:\n Alpha: 1\n Axes Length: 1\n Axes Radius: 0.10000000149011612\n Color: 239; 41; 41\n Head Length: 0.30000001192092896\n Head Radius: 0.10000000149011612\n Shaft Length: 1\n Shaft Radius: 0.05000000074505806\n Value: Arrow\n Topic: /vins_estimator/odometry\n Unreliable: false\n Value: true\n - Angle Tolerance: 0.10000000149011612\n Class: rviz/Odometry\n Covariance:\n Orientation:\n Alpha: 0.5\n Color: 255; 255; 127\n Color Style: Unique\n Frame: Local\n Offset: 1\n Scale: 1\n Value: true\n Position:\n Alpha: 0.30000001192092896\n Color: 204; 51; 204\n Scale: 1\n Value: true\n Value: true\n Enabled: false\n Keep: 1\n Name: Odometry\n Position Tolerance: 0.10000000149011612\n Queue Size: 10\n Shape:\n Alpha: 1\n Axes Length: 1\n Axes Radius: 0.10000000149011612\n Color: 114; 159; 207\n Head Length: 0.029999999329447746\n Head Radius: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Shaft Radius: 0.05000000074505806\n Value: Arrow\n Topic: /vins_estimator/odometry\n Unreliable: false\n Value: false\n - Class: rviz/Image\n Enabled: false\n Image Topic: /vins_estimator/semantic_mask\n Max Value: 1\n Median window: 5\n Min Value: 0\n Name: Image\n Normalize Range: true\n Queue Size: 2\n Transport Hint: raw\n Unreliable: false\n Value: false\n - Class: rviz/Marker\n Enabled: true\n Marker Topic: /image_to_rviz/visualization_marker\n Name: Marker\n Namespaces:\n {}\n Queue Size: 100\n Value: true\n - Class: rviz/Image\n Enabled: false\n Image Topic: /vins_estimator/semantic_mask\n Max Value: 1\n Median window: 5\n Min Value: 0\n Name: Image\n Normalize Range: true\n Queue Size: 2\n Transport Hint: raw\n Unreliable: false\n Value: false\n - Alpha: 10\n Buffer Length: 1\n Class: rviz/Path\n Color: 239; 41; 41\n Enabled: true\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Billboards\n Line Width: 1\n Name: Path\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Queue Size: 10\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /vins_estimator/path_rgbd\n Unreliable: false\n Value: true\n - Class: rviz/Marker\n Enabled: true\n Marker Topic: /vins_estimator/key_poses\n Name: Marker\n Namespaces:\n {}\n Queue Size: 100\n Value: true\n - Alpha: 1\n Axes Length: 1\n Axes Radius: 0.10000000149011612\n Class: rviz/Pose\n Color: 114; 159; 207\n Enabled: true\n Head Length: 0.30000001192092896\n Head Radius: 0.10000000149011612\n Name: Pose\n Queue Size: 10\n Shaft Length: 1\n Shaft Radius: 0.05000000074505806\n Shape: Arrow\n Topic: /mavros/vision_pose/remap_pose\n Unreliable: false\n Value: true\n - Angle Tolerance: 0.10000000149011612\n Class: rviz/Odometry\n Covariance:\n Orientation:\n Alpha: 0.5\n Color: 255; 255; 127\n Color Style: Unique\n Frame: Local\n Offset: 1\n Scale: 1\n Value: true\n Position:\n Alpha: 0.30000001192092896\n Color: 204; 51; 204\n Scale: 1\n Value: true\n Value: true\n Enabled: true\n Keep: 1\n Name: Odometry\n Position Tolerance: 0.10000000149011612\n Queue Size: 10\n Shape:\n Alpha: 1\n Axes Length: 1\n Axes Radius: 0.10000000149011612\n Color: 255; 25; 0\n Head Length: 0.30000001192092896\n Head Radius: 0.10000000149011612\n Shaft Length: 1\n Shaft Radius: 0.05000000074505806\n Value: Axes\n Topic: /odom\n Unreliable: false\n Value: true\n - Class: rviz/Image\n Enabled: false\n Image Topic: /usb_cam/image_raw\n Max Value: 1\n Median window: 5\n Min Value: 0\n Name: Image\n Normalize Range: true\n Queue Size: 2\n Transport Hint: raw\n Unreliable: false\n Value: false\n - Alpha: 1\n Axes Length: 1\n Axes Radius: 0.10000000149011612\n Class: rviz/Pose\n Color: 25; 255; 0\n Enabled: true\n Head Length: 0.30000001192092896\n Head Radius: 0.10000000149011612\n Name: Pose\n Queue Size: 10\n Shaft Length: 1\n Shaft Radius: 0.05000000074505806\n Shape: Axes\n Topic: /mavros/vision_pose/pose\n Unreliable: false\n Value: true\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 25; 255; 0\n Enabled: true\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Lines\n Line Width: 0.029999999329447746\n Name: Path\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Queue Size: 10\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /gt\n Unreliable: false\n Value: true\n - Alpha: 1\n Axes Length: 1\n Axes Radius: 0.10000000149011612\n Class: rviz/Pose\n Color: 255; 25; 0\n Enabled: true\n Head Length: 0.30000001192092896\n Head Radius: 0.10000000149011612\n Name: Pose\n Queue Size: 10\n Shaft Length: 1\n Shaft Radius: 0.05000000074505806\n Shape: Axes\n Topic: /tag_pose\n Unreliable: false\n Value: true\n - Alpha: 1\n Axes Length: 1\n Axes Radius: 0.10000000149011612\n Class: rviz/Pose\n Color: 255; 25; 0\n Enabled: true\n Head Length: 0.30000001192092896\n Head Radius: 0.10000000149011612\n Name: Pose\n Queue Size: 10\n Shaft Length: 1\n Shaft Radius: 0.05000000074505806\n Shape: Axes\n Topic: /mavros/setpoint_position/local\n Unreliable: false\n Value: true\n - Alpha: 1\n Axes Length: 1\n Axes Radius: 0.10000000149011612\n Class: rviz/Pose\n Color: 255; 25; 0\n Enabled: true\n Head Length: 0.30000001192092896\n Head Radius: 0.10000000149011612\n Name: Pose\n Queue Size: 10\n Shaft Length: 1\n Shaft Radius: 0.05000000074505806\n Shape: Axes\n Topic: /tag_pose\n Unreliable: false\n Value: true\n Enabled: true\n Global Options:\n Background Color: 0; 0; 0\n Default Light: true\n Fixed Frame: map\n Frame Rate: 30\n Name: root\n Tools:\n - Class: rviz/MoveCamera\n - Class: rviz/Select\n - Class: rviz/FocusCamera\n - Class: rviz/Measure\n - Class: rviz/SetInitialPose\n Theta std deviation: 0.2617993950843811\n Topic: /initialpose\n X std deviation: 0.5\n Y std deviation: 0.5\n - Class: rviz/SetGoal\n Topic: /move_base_simple/goal\n - Class: rviz/PublishPoint\n Single click: true\n Topic: /clicked_point\n Value: true\n Views:\n Current:\n Class: rviz/ThirdPersonFollower\n Distance: 8.819969177246094\n Enable Stereo Rendering:\n Stereo Eye Separation: 0.05999999865889549\n Stereo Focal Distance: 1\n Swap Stereo Eyes: false\n Value: false\n Field of View: 0.7853981852531433\n Focal Point:\n X: 1.5674259662628174\n Y: -1.2659180164337158\n Z: -4.0531158447265625e-06\n Focal Shape Fixed Size: true\n Focal Shape Size: 0.05000000074505806\n Invert Z Axis: false\n Name: Current View\n Near Clip Distance: 0.009999999776482582\n Pitch: 0.3497968316078186\n Target Frame: \n Yaw: 5.15224027633667\n Saved: ~\nWindow Geometry:\n Displays:\n collapsed: false\n Height: 1016\n Hide Left Dock: true\n Hide Right Dock: true\n Image:\n collapsed: false\n QMainWindow State: 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\n Selection:\n collapsed: false\n Time:\n collapsed: false\n Tool Properties:\n collapsed: false\n Views:\n collapsed: true\n Width: 924\n X: 72\n Y: 27\n loop_match_image:\n collapsed: false\n raw_image:\n collapsed: false\n tracked image:\n collapsed: false\n" }, { "path": "config/tsinghua.rviz", "content": "Panels:\n - Class: rviz/Displays\n Help Height: 0\n Name: Displays\n Property Tree Widget:\n Expanded:\n - /Global Options1\n - /Status1\n - /raw_image1\n - /VIO1\n - /VIO1/Path1\n - /pose_graph1/pose_graph_path1\n - /Odometry1\n - /Odometry2\n - /Odometry2/Shape1\n Splitter Ratio: 0.4651159942150116\n Tree Height: 441\n - Class: rviz/Selection\n Name: Selection\n - Class: rviz/Tool Properties\n Expanded:\n - /2D Pose Estimate1\n - /2D Nav Goal1\n - /Publish Point1\n Name: Tool Properties\n Splitter Ratio: 0.5886790156364441\n - Class: rviz/Views\n Expanded:\n - /Current View1\n Name: Views\n Splitter Ratio: 0.5\n - Class: rviz/Time\n Experimental: false\n Name: Time\n SyncMode: 0\n SyncSource: raw_image\n - Class: rviz/Displays\n Help Height: 78\n Name: Displays\n Property Tree Widget:\n Expanded:\n - /Global Options1\n - /tracked image1\n - /raw_image1\n - /VIO1\n - /VIO1/Path1\n - /pose_graph1\n - /pose_graph1/pose_graph_path1\n - /pose_graph1/loop_match_image1\n - /pose_graph1/loop_match_image1/Status1\n - /Image1\n - /Path1\n Splitter Ratio: 0.5\n Tree Height: 741\nPreferences:\n PromptSaveOnExit: true\nToolbars:\n toolButtonStyle: 2\nVisualization Manager:\n Class: \"\"\n Displays:\n - Alpha: 0.5\n Cell Size: 1\n Class: rviz/Grid\n Color: 130; 130; 130\n Enabled: true\n Line Style:\n Line Width: 0.029999999329447746\n Value: Lines\n Name: Grid\n Normal Cell Count: 0\n Offset:\n X: 0\n Y: 0\n Z: 0\n Plane: XY\n Plane Cell Count: 10\n Reference Frame: \n Value: true\n - Class: rviz/Axes\n Enabled: false\n Length: 1\n Name: Axes\n Radius: 0.10000000149011612\n Reference Frame: \n Value: false\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 255; 0; 0\n Enabled: true\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Lines\n Line Width: 0.5\n Name: ground_truth_path\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /benchmark_publisher/path\n Unreliable: false\n Value: true\n - Class: rviz/Image\n Enabled: true\n Image Topic: /feature_tracker/feature_img\n Max Value: 1\n Median window: 5\n Min Value: 0\n Name: tracked image\n Normalize Range: true\n Queue Size: 2\n Transport Hint: raw\n Unreliable: false\n Value: true\n - Class: rviz/Image\n Enabled: true\n Image Topic: /camera/color/image_raw\n Max Value: 1\n Median window: 5\n Min Value: 0\n Name: raw_image\n Normalize Range: true\n Queue Size: 2\n Transport Hint: raw\n Unreliable: false\n Value: true\n - Class: rviz/Group\n Displays:\n - Alpha: 10\n Buffer Length: 1\n Class: rviz/Path\n Color: 25; 255; 0\n Enabled: true\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Billboards\n Line Width: 1\n Name: Path\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /vins_estimator/path\n Unreliable: false\n Value: true\n - Class: rviz/MarkerArray\n Enabled: true\n Marker Topic: /vins_estimator/camera_pose_visual\n Name: camera_visual\n Namespaces:\n {}\n Queue Size: 100\n Value: true\n - Alpha: 1\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 10\n Min Value: -10\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/PointCloud\n Color: 0; 255; 0\n Color Transformer: FlatColor\n Decay Time: 1\n Enabled: false\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Min Color: 0; 0; 0\n Name: current_point\n Position Transformer: XYZ\n Queue Size: 1\n Selectable: true\n Size (Pixels): 1\n Size (m): 0.009999999776482582\n Style: Points\n Topic: /vins_estimator/point_cloud\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: false\n - Alpha: 1\n Autocompute Intensity Bounds: false\n Autocompute Value Bounds:\n Max Value: 10\n Min Value: -10\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/PointCloud\n Color: 255; 255; 255\n Color Transformer: FlatColor\n Decay Time: 3000\n Enabled: false\n Invert Rainbow: true\n Max Color: 0; 0; 0\n Max Intensity: 4096\n Min Color: 0; 0; 0\n Min Intensity: 0\n Name: history_point\n Position Transformer: XYZ\n Queue Size: 10\n Selectable: true\n Size (Pixels): 1\n Size (m): 0.5\n Style: Points\n Topic: /vins_estimator/history_cloud\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: false\n Value: false\n - Class: rviz/TF\n Enabled: false\n Frame Timeout: 15\n Frames:\n All Enabled: true\n Marker Scale: 1\n Name: TF\n Show Arrows: true\n Show Axes: true\n Show Names: true\n Tree:\n {}\n Update Interval: 0\n Value: false\n Enabled: true\n Name: VIO\n - Class: rviz/Group\n Displays:\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 252; 233; 79\n Enabled: true\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Billboards\n Line Width: 1\n Name: pose_graph_path\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /pose_graph/pose_graph_path\n Unreliable: false\n Value: true\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 255; 170; 0\n Enabled: false\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Lines\n Line Width: 0.029999999329447746\n Name: base_path\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /pose_graph/base_path\n Unreliable: false\n Value: false\n - Class: rviz/MarkerArray\n Enabled: false\n Marker Topic: /pose_graph/pose_graph\n Name: loop_visual\n Namespaces:\n {}\n Queue Size: 100\n Value: false\n - Class: rviz/MarkerArray\n Enabled: true\n Marker Topic: /pose_graph/camera_pose_visual\n Name: camera_visual\n Namespaces:\n {}\n Queue Size: 100\n Value: true\n - Class: rviz/Image\n Enabled: false\n Image Topic: /match_image\n Max Value: 1\n Median window: 5\n Min Value: 0\n Name: loop_match_image\n Normalize Range: true\n Queue Size: 2\n Transport Hint: raw\n Unreliable: false\n Value: false\n - Class: rviz/Marker\n Enabled: false\n Marker Topic: /pose_graph/key_odometrys\n Name: Marker\n Namespaces:\n {}\n Queue Size: 100\n Value: false\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 25; 255; 0\n Enabled: false\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Lines\n Line Width: 0.30000001192092896\n Name: Sequence1\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /pose_graph/path_1\n Unreliable: false\n Value: false\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 255; 0; 0\n Enabled: false\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Lines\n Line Width: 0.30000001192092896\n Name: Sequence2\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /pose_graph/path_2\n Unreliable: false\n Value: false\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 0; 85; 255\n Enabled: false\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Lines\n Line Width: 0.30000001192092896\n Name: Sequence3\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /pose_graph/path_3\n Unreliable: false\n Value: false\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 255; 170; 0\n Enabled: false\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Lines\n Line Width: 0.30000001192092896\n Name: Sequence4\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /pose_graph/path_4\n Unreliable: false\n Value: false\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 255; 170; 255\n Enabled: false\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Lines\n Line Width: 0.029999999329447746\n Name: Sequence5\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /pose_graph/path_5\n Unreliable: false\n Value: false\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 25; 255; 0\n Enabled: false\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Lines\n Line Width: 0.029999999329447746\n Name: no_loop_path\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /pose_graph/no_loop_path\n Unreliable: false\n Value: false\n Enabled: true\n Name: pose_graph\n - Angle Tolerance: 0.10000000149011612\n Class: rviz/Odometry\n Covariance:\n Orientation:\n Alpha: 0.5\n Color: 255; 255; 127\n Color Style: Unique\n Frame: Local\n Offset: 1\n Scale: 1\n Value: true\n Position:\n Alpha: 0.30000001192092896\n Color: 204; 51; 204\n Scale: 1\n Value: true\n Value: true\n Enabled: true\n Keep: 1\n Name: Odometry\n Position Tolerance: 0.10000000149011612\n Shape:\n Alpha: 1\n Axes Length: 1\n Axes Radius: 0.10000000149011612\n Color: 255; 25; 0\n Head Length: 0.30000001192092896\n Head Radius: 0.10000000149011612\n Shaft Length: 1\n Shaft Radius: 0.05000000074505806\n Value: Axes\n Topic: /vins_estimator/imu_propagate\n Unreliable: false\n Value: true\n - Angle Tolerance: 0.10000000149011612\n Class: rviz/Odometry\n Covariance:\n Orientation:\n Alpha: 0.5\n Color: 255; 255; 127\n Color Style: Unique\n Frame: Local\n Offset: 1\n Scale: 1\n Value: true\n Position:\n Alpha: 0.30000001192092896\n Color: 204; 51; 204\n Scale: 1\n Value: true\n Value: true\n Enabled: true\n Keep: 10000\n Name: Odometry\n Position Tolerance: 0.10000000149011612\n Shape:\n Alpha: 1\n Axes Length: 1\n Axes Radius: 0.10000000149011612\n Color: 255; 25; 0\n Head Length: 0.029999999329447746\n Head Radius: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Shaft Radius: 0.05000000074505806\n Value: Arrow\n Topic: /odom\n Unreliable: false\n Value: true\n - Class: rviz/Marker\n Enabled: true\n Marker Topic: /image_to_rviz/visualization_marker\n Name: Marker\n Namespaces:\n image: true\n Queue Size: 100\n Value: true\n - Class: rviz/Image\n Enabled: false\n Image Topic: /vins_estimator/semantic_mask\n Max Value: 1\n Median window: 5\n Min Value: 0\n Name: Image\n Normalize Range: true\n Queue Size: 2\n Transport Hint: raw\n Unreliable: false\n Value: false\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 25; 255; 0\n Enabled: true\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Billboards\n Line Width: 1\n Name: Path\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /vins_estimator/path\n Unreliable: false\n Value: true\n Enabled: true\n Global Options:\n Background Color: 0; 0; 0\n Default Light: true\n Fixed Frame: map\n Frame Rate: 30\n Name: root\n Tools:\n - Class: rviz/MoveCamera\n - Class: rviz/Select\n - Class: rviz/FocusCamera\n - Class: rviz/Measure\n - Class: rviz/SetInitialPose\n Theta std deviation: 0.2617993950843811\n Topic: /initialpose\n X std deviation: 0.5\n Y std deviation: 0.5\n - Class: rviz/SetGoal\n Topic: /move_base_simple/goal\n - Class: rviz/PublishPoint\n Single click: true\n Topic: /clicked_point\n Value: true\n Views:\n Current:\n Angle: -1.7600017786026\n Class: rviz/TopDownOrtho\n Enable Stereo Rendering:\n Stereo Eye Separation: 0.05999999865889549\n Stereo Focal Distance: 1\n Swap Stereo Eyes: false\n Value: false\n Invert Z Axis: false\n Name: Current View\n Near Clip Distance: 0.009999999776482582\n Scale: 1.4435186386108398\n Target Frame: \n Value: TopDownOrtho (rviz)\n X: 213.99978637695312\n Y: 93.23384857177734\n Saved:\n - Angle: -1.9099998474121094\n Class: rviz/TopDownOrtho\n Enable Stereo Rendering:\n Stereo Eye Separation: 0.05999999865889549\n Stereo Focal Distance: 1\n Swap Stereo Eyes: false\n Value: false\n Invert Z Axis: false\n Name: TopDownOrtho\n Near Clip Distance: 0.009999999776482582\n Scale: 2.8129937648773193\n Target Frame: \n Value: TopDownOrtho (rviz)\n X: 90.85389709472656\n Y: 126.16999053955078\nWindow Geometry:\n Displays:\n collapsed: false\n Height: 960\n Hide Left Dock: false\n Hide Right Dock: true\n Image:\n collapsed: false\n QMainWindow State: 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\n Selection:\n collapsed: false\n Time:\n collapsed: false\n Tool Properties:\n collapsed: false\n Views:\n collapsed: true\n Width: 1920\n X: 0\n Y: 27\n loop_match_image:\n collapsed: false\n raw_image:\n collapsed: false\n tracked image:\n collapsed: false\n" }, { "path": "config/tum_rgbd/tum_fr3.yaml", "content": "%YAML:1.0\n\nnum_threads: 0 # 0 Use the max number of threads of your device.\n # 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.)\n # x It is proper that 1 < x < MAX_THREAD.\n # For now, this parameter is relevant to grid-detector to run in parallel. \n\n#common parameters\nimu: 0\nimage_topic: \"/camera/rgb/image_color\"\ndepth_topic: \"/camera/depth/image\"\noutput_path: \"/home/nrosliu/DynamicVINS-Atlas/src/Dynamic_VINS/output\"\n\n#RGBD camera Ideal Range\ndepth_min_dist: 0.4\ndepth_max_dist: 10\nfix_depth: 1\n\nfrontend_freq: 30 # It should be raised in VO mode(without IMU).\nfreq: 30 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image\nkeyframe_parallax: 10.0 # keyframe selection threshold (pixel); if system fails frequently, please try to reduce the \"keyframe_parallax\" \nnum_grid_rows: 7\nnum_grid_cols: 8\n\n#camera calibration\nmodel_type: PINHOLE\ncamera_name: camera\nimage_width: 640\nimage_height: 480\n\ndistortion_parameters:\n k1: 0.0\n k2: 0.0\n p1: 0.0\n p2: 0.0\nprojection_parameters:\n fx: 535.4\n fy: 539.2\n cx: 320.1\n cy: 247.6\n\n# Extrinsic parameter between IMU and Camera.\nestimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.\n # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.\n # 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.\n#If you choose 0 or 1, you should write down the following matrix.\n#Rotation from camera frame to imu frame, imu^R_cam\nextrinsicRotation: !!opencv-matrix\n rows: 3\n cols: 3\n dt: d\n data: [ 1.0, 0.0, 0.0,\n 0.0, 1.0, 0.0,\n 0.0, 0.0, 1.0]\n#Translation from camera frame to imu frame, imu^T_cam\nextrinsicTranslation: !!opencv-matrix\n rows: 3\n cols: 1\n dt: d\n data: [0.0, 0.0, 0.0]\n\n#feature traker paprameters\nmax_cnt: 250 # max feature number in feature tracking. It is suggested to be raised in VO mode.\nmin_dist: 10 # min distance between two features\nF_threshold: 1.0 # ransac threshold (pixel)\nshow_track: 0 # publish tracking image as topic\nequalize: 0 # if image is too dark or light, trun on equalize to find enough features\nfisheye: 0 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points\n\n#optimization parameters\nmax_solver_time: 0.04 # max solver itration time (ms), to guarantee real time\nmax_num_iterations: 8 # max solver itrations, to guarantee real time\n\n#unsynchronization parameters\nestimate_td: 0 ########## # online estimate time offset between camera and imu\ntd: 0.000 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)\n\n#rolling shutter parameters\nrolling_shutter: 0 # 0: global shutter camera, 1: rolling shutter camera\nrolling_shutter_tr: 0.0 # unit: s. rolling shutter read out time per frame (from data sheet).\n\n#loop closure parameters\nloop_closure: 0 # start loop closure\nfast_relocalization: 0 # useful in real-time and large project\nload_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path'\npose_graph_save_path: \"/home/nrosliu/DynamicVINS-Atlas/src/Dynamic_VINS/output/pose_graph\" # save and load path\n\n#visualization parameters\nsave_image: 0 # save image in pose graph for visualization prupose; you can close this function by setting 0\nvisualize_imu_forward: 0 # output imu forward propogation to achieve low latency and high frequence results\nvisualize_camera_size: 0.4 # size of camera marker in RVIZ\n\n#暂时仅考虑动物\ndynamic_label: [\"person\", \"cat\", \"dog\", \"bicycle\", \"car\",\"bus\"]" }, { "path": "config/tum_rgbd/tum_fr3_atlas.yaml", "content": "%YAML:1.0\n\nnum_threads: 6 # 0 Use the max number of threads of your device.\n # 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.)\n # x It is proper that 1 < x < MAX_THREAD.\n # For now, this parameter is relevant to grid-detector to run in parallel. \n\n#common parameters\nimu: 0\nimage_topic: \"/decompressed_img\"\ndepth_topic: \"/camera/depth/image\"\noutput_path: \"/home/HwHiAiUser/Dynamic_VINS_ws/src/Dynamic_VINS/output\"\n\n#RGBD camera Ideal Range\ndepth_min_dist: 0.4\ndepth_max_dist: 10\n\nfrontend_freq: 30 # It should be raised in VO mode(without IMU).\nfreq: 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).\nkeyframe_parallax: 10.0 # keyframe selection threshold (pixel); if system fails frequently, please try to reduce the \"keyframe_parallax\" \nnum_grid_rows: 7\nnum_grid_cols: 8\n\n#camera calibration\nmodel_type: PINHOLE\ncamera_name: camera\nimage_width: 640\nimage_height: 480\n\ndistortion_parameters:\n k1: 0.0\n k2: 0.0\n p1: 0.0\n p2: 0.0\nprojection_parameters:\n fx: 535.4\n fy: 539.2\n cx: 320.1\n cy: 247.6\n\n# Extrinsic parameter between IMU and Camera.\nestimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.\n # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.\n # 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.\n#If you choose 0 or 1, you should write down the following matrix.\n#Rotation from camera frame to imu frame, imu^R_cam\nextrinsicRotation: !!opencv-matrix\n rows: 3\n cols: 3\n dt: d\n data: [ 0.999975572679493, 0.003849141066713, 0.005854714944742,\n -0.003828680351062, 0.999986658473099, -0.003501944262433,\n -0.005868115609379, 0.003479442469180, 0.999976848846595]\n#Translation from camera frame to imu frame, imu^T_cam\nextrinsicTranslation: !!opencv-matrix\n rows: 3\n cols: 1\n dt: d\n data: [0.0203127935529, -0.00510325236246, -0.0112013882026]\n\n#feature traker paprameters\nmax_cnt: 200 # max feature number in feature tracking. \nmin_dist: 20 # min distance between two features\nF_threshold: 1.0 # ransac threshold (pixel)\nshow_track: 0 # publish tracking image as topic\nequalize: 0 # if image is too dark or light, trun on equalize to find enough features\nfisheye: 0 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points\n\n#optimization parameters\nmax_solver_time: 0.04 # max solver itration time (ms), to guarantee real time\nmax_num_iterations: 8 # max solver itrations, to guarantee real time\n\ng_norm: 9.805 # gravity magnitude\n\n#unsynchronization parameters\nestimate_td: 0 ########## # online estimate time offset between camera and imu\ntd: 0.000 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)\n\n#rolling shutter parameters\nrolling_shutter: 0 # 0: global shutter camera, 1: rolling shutter camera\nrolling_shutter_tr: 0.0 # unit: s. rolling shutter read out time per frame (from data sheet).\n\n#loop closure parameters\nloop_closure: 0 # start loop closure\nfast_relocalization: 0 # useful in real-time and large project\nload_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path'\npose_graph_save_path: \"/home/nrosliu/DynamicVINS-Atlas/src/Dynamic_VINS/output/pose_graph\" # save and load path\n\n#visualization parameters\nsave_image: 0 # save image in pose graph for visualization prupose; you can close this function by setting 0\nvisualize_imu_forward: 0 # output imu forward propogation to achieve low latency and high frequence results\nvisualize_camera_size: 0.4 # size of camera marker in RVIZ\n\n#暂时仅考虑动物\ndynamic_label: [\"person\", \"cat\", \"dog\", \"bicycle\", \"car\",\"bus\"]" }, { "path": "config/video.rviz", "content": "Panels:\n - Class: rviz/Displays\n Help Height: 0\n Name: Displays\n Property Tree Widget:\n Expanded:\n - /Global Options1\n - /Status1\n - /VIO1\n - /VIO1/Path1\n - /pose_graph1/pose_graph_path1\n - /Odometry1\n - /Odometry1/Shape1\n - /Odometry2\n - /Odometry2/Shape1\n - /Marker1\n Splitter Ratio: 0.4651159942150116\n Tree Height: 311\n - Class: rviz/Selection\n Name: Selection\n - Class: rviz/Tool Properties\n Expanded:\n - /2D Pose Estimate1\n - /2D Nav Goal1\n - /Publish Point1\n Name: Tool Properties\n Splitter Ratio: 0.5886790156364441\n - Class: rviz/Views\n Expanded:\n - /Current View1\n Name: Views\n Splitter Ratio: 0.5\n - Class: rviz/Time\n Experimental: false\n Name: Time\n SyncMode: 0\n SyncSource: tracked image\n - Class: rviz/Displays\n Help Height: 78\n Name: Displays\n Property Tree Widget:\n Expanded:\n - /tracked image1\n - /raw_image1\n - /VIO1\n - /VIO1/Path1\n - /pose_graph1\n - /pose_graph1/pose_graph_path1\n Splitter Ratio: 0.5\n Tree Height: 363\nPreferences:\n PromptSaveOnExit: true\nToolbars:\n toolButtonStyle: 2\nVisualization Manager:\n Class: \"\"\n Displays:\n - Alpha: 0.5\n Cell Size: 1\n Class: rviz/Grid\n Color: 130; 130; 130\n Enabled: true\n Line Style:\n Line Width: 0.029999999329447746\n Value: Lines\n Name: Grid\n Normal Cell Count: 0\n Offset:\n X: 0\n Y: 0\n Z: 0\n Plane: XY\n Plane Cell Count: 10\n Reference Frame: \n Value: true\n - Alpha: 1\n Class: rviz/Axes\n Enabled: false\n Length: 1\n Name: Axes\n Radius: 0.10000000149011612\n Reference Frame: \n Show Trail: false\n Value: false\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 255; 0; 0\n Enabled: true\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Lines\n Line Width: 0.5\n Name: ground_truth_path\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Queue Size: 10\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /benchmark_publisher/path\n Unreliable: false\n Value: true\n - Class: rviz/Image\n Enabled: true\n Image Topic: /vins_estimator/feature_img\n Max Value: 1\n Median window: 5\n Min Value: 0\n Name: tracked image\n Normalize Range: true\n Queue Size: 2\n Transport Hint: raw\n Unreliable: false\n Value: true\n - Class: rviz/Image\n Enabled: false\n Image Topic: /camera/color/image_raw\n Max Value: 1\n Median window: 5\n Min Value: 0\n Name: raw_image\n Normalize Range: true\n Queue Size: 2\n Transport Hint: raw\n Unreliable: false\n Value: false\n - Class: rviz/Group\n Displays:\n - Alpha: 10\n Buffer Length: 1\n Class: rviz/Path\n Color: 25; 255; 0\n Enabled: true\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Billboards\n Line Width: 0.10000000149011612\n Name: Path\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Queue Size: 10\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /vins_estimator/path\n Unreliable: false\n Value: true\n - Class: rviz/MarkerArray\n Enabled: true\n Marker Topic: /vins_estimator/camera_pose_visual\n Name: camera_visual\n Namespaces:\n CameraPoseVisualization: true\n Queue Size: 100\n Value: true\n - Alpha: 1\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 10\n Min Value: -10\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/PointCloud\n Color: 0; 255; 0\n Color Transformer: FlatColor\n Decay Time: 10000\n Enabled: true\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Min Color: 0; 0; 0\n Name: current_point\n Position Transformer: XYZ\n Queue Size: 1\n Selectable: true\n Size (Pixels): 1\n Size (m): 0.009999999776482582\n Style: Points\n Topic: /vins_estimator/point_cloud\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: true\n - Alpha: 1\n Autocompute Intensity Bounds: false\n Autocompute Value Bounds:\n Max Value: 10\n Min Value: -10\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/PointCloud\n Color: 255; 255; 255\n Color Transformer: FlatColor\n Decay Time: 3000\n Enabled: true\n Invert Rainbow: true\n Max Color: 0; 0; 0\n Max Intensity: 4096\n Min Color: 0; 0; 0\n Min Intensity: 0\n Name: history_point\n Position Transformer: XYZ\n Queue Size: 10\n Selectable: true\n Size (Pixels): 1\n Size (m): 0.5\n Style: Points\n Topic: /vins_estimator/history_cloud\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: false\n Value: true\n - Class: rviz/TF\n Enabled: false\n Frame Timeout: 15\n Frames:\n All Enabled: true\n Marker Alpha: 1\n Marker Scale: 1\n Name: TF\n Show Arrows: true\n Show Axes: true\n Show Names: true\n Tree:\n {}\n Update Interval: 0\n Value: false\n Enabled: true\n Name: VIO\n - Class: rviz/Group\n Displays:\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 252; 233; 79\n Enabled: true\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Billboards\n Line Width: 1\n Name: pose_graph_path\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Queue Size: 10\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /pose_graph/pose_graph_path\n Unreliable: false\n Value: true\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 255; 170; 0\n Enabled: false\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Lines\n Line Width: 0.029999999329447746\n Name: base_path\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Queue Size: 10\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /pose_graph/base_path\n Unreliable: false\n Value: false\n - Class: rviz/MarkerArray\n Enabled: false\n Marker Topic: /pose_graph/pose_graph\n Name: loop_visual\n Namespaces:\n {}\n Queue Size: 100\n Value: false\n - Class: rviz/MarkerArray\n Enabled: true\n Marker Topic: /pose_graph/camera_pose_visual\n Name: camera_visual\n Namespaces:\n {}\n Queue Size: 100\n Value: true\n - Class: rviz/Image\n Enabled: false\n Image Topic: /pose_graph/match_image\n Max Value: 1\n Median window: 5\n Min Value: 0\n Name: loop_match_image\n Normalize Range: true\n Queue Size: 2\n Transport Hint: raw\n Unreliable: false\n Value: false\n - Class: rviz/Marker\n Enabled: false\n Marker Topic: /pose_graph/key_odometrys\n Name: Marker\n Namespaces:\n {}\n Queue Size: 100\n Value: false\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 25; 255; 0\n Enabled: false\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Lines\n Line Width: 0.30000001192092896\n Name: Sequence1\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Queue Size: 10\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /pose_graph/path_1\n Unreliable: false\n Value: false\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 255; 0; 0\n Enabled: false\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Lines\n Line Width: 0.30000001192092896\n Name: Sequence2\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Queue Size: 10\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /pose_graph/path_2\n Unreliable: false\n Value: false\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 0; 85; 255\n Enabled: false\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Lines\n Line Width: 0.30000001192092896\n Name: Sequence3\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Queue Size: 10\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /pose_graph/path_3\n Unreliable: false\n Value: false\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 255; 170; 0\n Enabled: false\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Lines\n Line Width: 0.30000001192092896\n Name: Sequence4\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Queue Size: 10\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /pose_graph/path_4\n Unreliable: false\n Value: false\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 255; 170; 255\n Enabled: false\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Lines\n Line Width: 0.029999999329447746\n Name: Sequence5\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Queue Size: 10\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /pose_graph/path_5\n Unreliable: false\n Value: false\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 25; 255; 0\n Enabled: false\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Lines\n Line Width: 0.029999999329447746\n Name: no_loop_path\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Queue Size: 10\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /pose_graph/no_loop_path\n Unreliable: false\n Value: false\n Enabled: true\n Name: pose_graph\n - Angle Tolerance: 0.10000000149011612\n Class: rviz/Odometry\n Covariance:\n Orientation:\n Alpha: 0.5\n Color: 255; 255; 127\n Color Style: Unique\n Frame: Local\n Offset: 1\n Scale: 1\n Value: true\n Position:\n Alpha: 0.30000001192092896\n Color: 204; 51; 204\n Scale: 1\n Value: true\n Value: true\n Enabled: true\n Keep: 1\n Name: Odometry\n Position Tolerance: 0.10000000149011612\n Queue Size: 10\n Shape:\n Alpha: 1\n Axes Length: 1\n Axes Radius: 0.10000000149011612\n Color: 239; 41; 41\n Head Length: 0.30000001192092896\n Head Radius: 0.10000000149011612\n Shaft Length: 1\n Shaft Radius: 0.05000000074505806\n Value: Arrow\n Topic: /vins_estimator/odometry\n Unreliable: false\n Value: true\n - Angle Tolerance: 0.10000000149011612\n Class: rviz/Odometry\n Covariance:\n Orientation:\n Alpha: 0.5\n Color: 255; 255; 127\n Color Style: Unique\n Frame: Local\n Offset: 1\n Scale: 1\n Value: true\n Position:\n Alpha: 0.30000001192092896\n Color: 204; 51; 204\n Scale: 1\n Value: true\n Value: true\n Enabled: false\n Keep: 1\n Name: Odometry\n Position Tolerance: 0.10000000149011612\n Queue Size: 10\n Shape:\n Alpha: 1\n Axes Length: 1\n Axes Radius: 0.10000000149011612\n Color: 114; 159; 207\n Head Length: 0.029999999329447746\n Head Radius: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Shaft Radius: 0.05000000074505806\n Value: Arrow\n Topic: /vins_estimator/odometry\n Unreliable: false\n Value: false\n - Class: rviz/Image\n Enabled: false\n Image Topic: /vins_estimator/semantic_mask\n Max Value: 1\n Median window: 5\n Min Value: 0\n Name: Image\n Normalize Range: true\n Queue Size: 2\n Transport Hint: raw\n Unreliable: false\n Value: false\n - Class: rviz/Marker\n Enabled: true\n Marker Topic: /image_to_rviz/visualization_marker\n Name: Marker\n Namespaces:\n {}\n Queue Size: 100\n Value: true\n - Class: rviz/Image\n Enabled: false\n Image Topic: /vins_estimator/semantic_mask\n Max Value: 1\n Median window: 5\n Min Value: 0\n Name: Image\n Normalize Range: true\n Queue Size: 2\n Transport Hint: raw\n Unreliable: false\n Value: false\n - Alpha: 10\n Buffer Length: 1\n Class: rviz/Path\n Color: 239; 41; 41\n Enabled: true\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Billboards\n Line Width: 1\n Name: Path\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Queue Size: 10\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /vins_estimator/path_rgbd\n Unreliable: false\n Value: true\n - Class: rviz/Marker\n Enabled: true\n Marker Topic: /vins_estimator/key_poses\n Name: Marker\n Namespaces:\n key_poses: true\n Queue Size: 100\n Value: true\n - Alpha: 1\n Axes Length: 1\n Axes Radius: 0.10000000149011612\n Class: rviz/Pose\n Color: 114; 159; 207\n Enabled: true\n Head Length: 0.30000001192092896\n Head Radius: 0.10000000149011612\n Name: Pose\n Queue Size: 10\n Shaft Length: 1\n Shaft Radius: 0.05000000074505806\n Shape: Arrow\n Topic: /mavros/vision_pose/remap_pose\n Unreliable: false\n Value: true\n Enabled: true\n Global Options:\n Background Color: 0; 0; 0\n Default Light: true\n Fixed Frame: map\n Frame Rate: 30\n Name: root\n Tools:\n - Class: rviz/MoveCamera\n - Class: rviz/Select\n - Class: rviz/FocusCamera\n - Class: rviz/Measure\n - Class: rviz/SetInitialPose\n Theta std deviation: 0.2617993950843811\n Topic: /initialpose\n X std deviation: 0.5\n Y std deviation: 0.5\n - Class: rviz/SetGoal\n Topic: /move_base_simple/goal\n - Class: rviz/PublishPoint\n Single click: true\n Topic: /clicked_point\n Value: true\n Views:\n Current:\n Class: rviz/ThirdPersonFollower\n Distance: 4.732760429382324\n Enable Stereo Rendering:\n Stereo Eye Separation: 0.05999999865889549\n Stereo Focal Distance: 1\n Swap Stereo Eyes: false\n Value: false\n Field of View: 0.7853981852531433\n Focal Point:\n X: 0.34211790561676025\n Y: 0.2611902952194214\n Z: -3.0994415283203125e-06\n Focal Shape Fixed Size: true\n Focal Shape Size: 0.05000000074505806\n Invert Z Axis: false\n Name: Current View\n Near Clip Distance: 0.009999999776482582\n Pitch: -0.19520258903503418\n Target Frame: \n Yaw: 4.882240295410156\n Saved: ~\nWindow Geometry:\n Displays:\n collapsed: false\n Height: 1043\n Hide Left Dock: false\n Hide Right Dock: true\n Image:\n collapsed: false\n QMainWindow State: 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\n Selection:\n collapsed: false\n Time:\n collapsed: false\n Tool Properties:\n collapsed: false\n Views:\n collapsed: true\n Width: 1848\n X: 72\n Y: 0\n loop_match_image:\n collapsed: false\n raw_image:\n collapsed: false\n tracked image:\n collapsed: false\n" }, { "path": "doc/INSTALL.md", "content": "# Dynamic-VINS\n\n## 1. Prerequisites\n\n**ROS**\n```\nsudo 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\n```\n\n**Ceres-Solver**\n```\n# CMake\nsudo apt-get install cmake\n# google-glog + gflags\nsudo apt-get install libgoogle-glog-dev libgflags-dev\n# BLAS & LAPACK\nsudo apt-get install libatlas-base-dev\n# Eigen3\nsudo apt-get install libeigen3-dev\n# SuiteSparse and CXSparse (optional)\nsudo apt-get install libsuitesparse-dev\n```\n```\ngit clone https://ceres-solver.googlesource.com/ceres-solver\ncd ceres-solver\ngit checkout 2.0.0\nmkdir ceres-bin\ncd ceres-bin\ncmake ..\nmake -j3\nsudo make install\n```\n\n**Sophus**\n```\ngit clone https://github.com/strasdat/Sophus.git\ncd Sophus\ngit checkout a621ff #版本回溯\n```\n`gedit sophus/so2.cpp` modify `sophus/so2.cpp` as\n```\nSO2::SO2()\n{\n unit_complex_.real(1.0);\n unit_complex_.imag(0.0);\n}\n```\nbuild\n```\nmkdir build && cd build && cmake .. && sudo make install\n```\n\n\n\n## 2. Prerequisites for object detection \n\nWe offer two kinds of device for tests, please follow the instruction for your match device.\n\n### 2.1. NVIDIA devices\n\nClone the repository and catkin_make:\n\n```\ncd {YOUR_WORKSPACE}/src\ngit clone https://github.com/jianhengLiu/Dynamic-VINS.git\n\n# build\ncd ..\ncatkin_make\n```\n\n\n\n### 2.2. HUAWEI Atlas200\n\n\n**!!!Note:**\n 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)\n\n0. prequisities\n\n```\n sudo apt install ros-melodic-image-transport-plugins\n```\n\n1. Clone the repository:\n\n```\n cd {YOUR_WORKSPACE}/src\n git clone https://github.com/jianhengLiu/Dynamic-VINS.git\n\n git clone https://github.com/jianhengLiu/compressedimg2img.git\n```\n\n1. allocate core \n```\n sudo npu-smi set -t aicpu-config -i 0 -c 0 -d 2\n```\n\n4. build\n``` \n cd ../..\n catkin_make\n```" }, { "path": "doc/RUNNING_PROCEDURE.md", "content": "# Dynamic-VINS testing procedure\n\n1. ```\n export ROS_HOSTNAME=192.168.2.223 #从机IP \n export ROS_MASTER_URI=http://192.168.2.223:11311\n roscore\n ```\n\n2. ```\n export ROS_HOSTNAME=192.168.2.223 #从机IP \n export ROS_MASTER_URI=http://192.168.2.223:11311\n rosrun image_transport republish raw in:=/d400/color/image_raw compressed out:=/d400/color/image_raw\n ```\n\n3. ```\n export ROS_HOSTNAME=192.168.2.223 #从机IP \n export ROS_MASTER_URI=http://192.168.2.223:11311\n rosrun image_transport republish raw in:=/d400/aligned_depth_to_color/image_raw compressedDepth out:=/d400/aligned_depth_to_color/image_raw\n ```\n\n5. Dynamic-VINS\n\n 1. run yolo, vins on atlas\n\n 1. ```\n ssh HwHiAiUser@192.168.2.2\n export ROS_HOSTNAME=192.168.2.2 #从机IP\n export ROS_MASTER_URI=http://192.168.2.223:11311\n roslaunch vins_estimator nodelet_openloris.launch\n ```\n\n 2. run yolo on atlas, and run vins on pc\n\n 1. ```\n (run vins on pc)\n export ROS_HOSTNAME=192.168.2.223 #从机IP \n export ROS_MASTER_URI=http://192.168.2.223:11311\n roslaunch vins_estimator nodelet_openloris_test.launch\n ```\n\n6. rviz visualiztion\n\n ```\n export ROS_HOSTNAME=192.168.2.223 #从机IP \n export ROS_MASTER_URI=http://192.168.2.223:11311\n roslaunch vins_estimator vins_rviz.launch \n ```" }, { "path": "output/.gitignore", "content": "*\n\n!.gitignore" }, { "path": "pose_graph/CMakeLists.txt", "content": "cmake_minimum_required(VERSION 2.8.3)\nproject(pose_graph)\n\nset(CMAKE_BUILD_TYPE \"Release\")\n# https://blog.csdn.net/qq_24624539/article/details/111056791\n#set(CMAKE_CXX_FLAGS \"-std=c++11\")\nset(CMAKE_CXX_FLAGS \"-std=c++14\")\n#-DEIGEN_USE_MKL_ALL\")\nset(CMAKE_CXX_FLAGS_RELEASE \"-O3 -Wall -g\")\n\nfind_package(catkin REQUIRED COMPONENTS\n roscpp\n std_msgs\n nav_msgs\n camera_model\n cv_bridge\n roslib\n message_filters\n\n nodelet\n )\n\nfind_package(OpenCV)\n\nfind_package(Ceres REQUIRED)\n\nset(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)\nfind_package(Eigen3)\n\ninclude_directories(${catkin_INCLUDE_DIRS} ${CERES_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR})\n\ninclude_directories(\n ${catkin_INCLUDE_DIRS}\n ${EIGEN3_INCLUDE_DIR}\n)\n\ncatkin_package()\n\nadd_library(pose_graph_lib\n src/pose_graph/pose_graph.cpp\n src/keyframe/keyframe.cpp\n src/utility/CameraPoseVisualization.cpp\n src/ThirdParty/DBoW/BowVector.cpp\n src/ThirdParty/DBoW/FBrief.cpp\n src/ThirdParty/DBoW/FeatureVector.cpp\n src/ThirdParty/DBoW/QueryResults.cpp\n src/ThirdParty/DBoW/ScoringObject.cpp\n src/ThirdParty/DUtils/Random.cpp\n src/ThirdParty/DUtils/Timestamp.cpp\n src/ThirdParty/DVision/BRIEF.cpp\n src/ThirdParty/VocabularyBinary.cpp\n )\ntarget_link_libraries(pose_graph_lib ${catkin_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES})\n#message(\"catkin_lib ${catkin_LIBRARIES}\")\n\nadd_library(pose_graph_nodelet src/pose_graph_nodelet.cpp)\ntarget_link_libraries(pose_graph_nodelet pose_graph_lib)" }, { "path": "pose_graph/cmake/FindEigen.cmake", "content": "# Ceres Solver - A fast non-linear least squares minimizer\n# Copyright 2015 Google Inc. All rights reserved.\n# http://ceres-solver.org/\n#\n# Redistribution and use in source and binary forms, with or without\n# modification, are permitted provided that the following conditions are met:\n#\n# * Redistributions of source code must retain the above copyright notice,\n# this list of conditions and the following disclaimer.\n# * Redistributions in binary form must reproduce the above copyright notice,\n# this list of conditions and the following disclaimer in the documentation\n# and/or other materials provided with the distribution.\n# * Neither the name of Google Inc. nor the names of its contributors may be\n# used to endorse or promote products derived from this software without\n# specific prior written permission.\n#\n# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS IS\"\n# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE\n# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE\n# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE\n# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR\n# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF\n# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS\n# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN\n# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)\n# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\n# POSSIBILITY OF SUCH DAMAGE.\n#\n# Author: alexs.mac@gmail.com (Alex Stewart)\n#\n\n# FindEigen.cmake - Find Eigen library, version >= 3.\n#\n# This module defines the following variables:\n#\n# EIGEN_FOUND: TRUE iff Eigen is found.\n# EIGEN_INCLUDE_DIRS: Include directories for Eigen.\n#\n# EIGEN_VERSION: Extracted from Eigen/src/Core/util/Macros.h\n# EIGEN_WORLD_VERSION: Equal to 3 if EIGEN_VERSION = 3.2.0\n# EIGEN_MAJOR_VERSION: Equal to 2 if EIGEN_VERSION = 3.2.0\n# EIGEN_MINOR_VERSION: Equal to 0 if EIGEN_VERSION = 3.2.0\n#\n# The following variables control the behaviour of this module:\n#\n# EIGEN_INCLUDE_DIR_HINTS: List of additional directories in which to\n# search for eigen includes, e.g: /timbuktu/eigen3.\n#\n# The following variables are also defined by this module, but in line with\n# CMake recommended FindPackage() module style should NOT be referenced directly\n# by callers (use the plural variables detailed above instead). These variables\n# do however affect the behaviour of the module via FIND_[PATH/LIBRARY]() which\n# are NOT re-called (i.e. search for library is not repeated) if these variables\n# are set with valid values _in the CMake cache_. This means that if these\n# variables are set directly in the cache, either by the user in the CMake GUI,\n# or by the user passing -DVAR=VALUE directives to CMake when called (which\n# explicitly defines a cache variable), then they will be used verbatim,\n# bypassing the HINTS variables and other hard-coded search locations.\n#\n# EIGEN_INCLUDE_DIR: Include directory for CXSparse, not including the\n# include directory of any dependencies.\n\n# Called if we failed to find Eigen or any of it's required dependencies,\n# unsets all public (designed to be used externally) variables and reports\n# error message at priority depending upon [REQUIRED/QUIET/] argument.\nmacro(EIGEN_REPORT_NOT_FOUND REASON_MSG)\n unset(EIGEN_FOUND)\n unset(EIGEN_INCLUDE_DIRS)\n # Make results of search visible in the CMake GUI if Eigen has not\n # been found so that user does not have to toggle to advanced view.\n mark_as_advanced(CLEAR EIGEN_INCLUDE_DIR)\n # Note _FIND_[REQUIRED/QUIETLY] variables defined by FindPackage()\n # use the camelcase library name, not uppercase.\n if (Eigen_FIND_QUIETLY)\n message(STATUS \"Failed to find Eigen - \" ${REASON_MSG} ${ARGN})\n elseif (Eigen_FIND_REQUIRED)\n message(FATAL_ERROR \"Failed to find Eigen - \" ${REASON_MSG} ${ARGN})\n else()\n # Neither QUIETLY nor REQUIRED, use no priority which emits a message\n # but continues configuration and allows generation.\n message(\"-- Failed to find Eigen - \" ${REASON_MSG} ${ARGN})\n endif ()\n return()\nendmacro(EIGEN_REPORT_NOT_FOUND)\n\n# Protect against any alternative find_package scripts for this library having\n# been called previously (in a client project) which set EIGEN_FOUND, but not\n# the other variables we require / set here which could cause the search logic\n# here to fail.\nunset(EIGEN_FOUND)\n\n# Search user-installed locations first, so that we prefer user installs\n# to system installs where both exist.\nlist(APPEND EIGEN_CHECK_INCLUDE_DIRS\n /usr/local/include\n /usr/local/homebrew/include # Mac OS X\n /opt/local/var/macports/software # Mac OS X.\n /opt/local/include\n /usr/include)\n# Additional suffixes to try appending to each search path.\nlist(APPEND EIGEN_CHECK_PATH_SUFFIXES\n eigen3 # Default root directory for Eigen.\n Eigen/include/eigen3 # Windows (for C:/Program Files prefix) < 3.3\n Eigen3/include/eigen3 ) # Windows (for C:/Program Files prefix) >= 3.3\n\n# Search supplied hint directories first if supplied.\nfind_path(EIGEN_INCLUDE_DIR\n NAMES Eigen/Core\n PATHS ${EIGEN_INCLUDE_DIR_HINTS}\n ${EIGEN_CHECK_INCLUDE_DIRS}\n PATH_SUFFIXES ${EIGEN_CHECK_PATH_SUFFIXES})\n\nif (NOT EIGEN_INCLUDE_DIR OR\n NOT EXISTS ${EIGEN_INCLUDE_DIR})\n eigen_report_not_found(\n \"Could not find eigen3 include directory, set EIGEN_INCLUDE_DIR to \"\n \"path to eigen3 include directory, e.g. /usr/local/include/eigen3.\")\nendif (NOT EIGEN_INCLUDE_DIR OR\n NOT EXISTS ${EIGEN_INCLUDE_DIR})\n\n# Mark internally as found, then verify. EIGEN_REPORT_NOT_FOUND() unsets\n# if called.\nset(EIGEN_FOUND TRUE)\n\n# Extract Eigen version from Eigen/src/Core/util/Macros.h\nif (EIGEN_INCLUDE_DIR)\n set(EIGEN_VERSION_FILE ${EIGEN_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h)\n if (NOT EXISTS ${EIGEN_VERSION_FILE})\n eigen_report_not_found(\n \"Could not find file: ${EIGEN_VERSION_FILE} \"\n \"containing version information in Eigen install located at: \"\n \"${EIGEN_INCLUDE_DIR}.\")\n else (NOT EXISTS ${EIGEN_VERSION_FILE})\n file(READ ${EIGEN_VERSION_FILE} EIGEN_VERSION_FILE_CONTENTS)\n\n string(REGEX MATCH \"#define EIGEN_WORLD_VERSION [0-9]+\"\n EIGEN_WORLD_VERSION \"${EIGEN_VERSION_FILE_CONTENTS}\")\n string(REGEX REPLACE \"#define EIGEN_WORLD_VERSION ([0-9]+)\" \"\\\\1\"\n EIGEN_WORLD_VERSION \"${EIGEN_WORLD_VERSION}\")\n\n string(REGEX MATCH \"#define EIGEN_MAJOR_VERSION [0-9]+\"\n EIGEN_MAJOR_VERSION \"${EIGEN_VERSION_FILE_CONTENTS}\")\n string(REGEX REPLACE \"#define EIGEN_MAJOR_VERSION ([0-9]+)\" \"\\\\1\"\n EIGEN_MAJOR_VERSION \"${EIGEN_MAJOR_VERSION}\")\n\n string(REGEX MATCH \"#define EIGEN_MINOR_VERSION [0-9]+\"\n EIGEN_MINOR_VERSION \"${EIGEN_VERSION_FILE_CONTENTS}\")\n string(REGEX REPLACE \"#define EIGEN_MINOR_VERSION ([0-9]+)\" \"\\\\1\"\n EIGEN_MINOR_VERSION \"${EIGEN_MINOR_VERSION}\")\n\n # This is on a single line s/t CMake does not interpret it as a list of\n # elements and insert ';' separators which would result in 3.;2.;0 nonsense.\n set(EIGEN_VERSION \"${EIGEN_WORLD_VERSION}.${EIGEN_MAJOR_VERSION}.${EIGEN_MINOR_VERSION}\")\n endif (NOT EXISTS ${EIGEN_VERSION_FILE})\nendif (EIGEN_INCLUDE_DIR)\n\n# Set standard CMake FindPackage variables if found.\nif (EIGEN_FOUND)\n set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR})\nendif (EIGEN_FOUND)\n\n# Handle REQUIRED / QUIET optional arguments and version.\ninclude(FindPackageHandleStandardArgs)\nfind_package_handle_standard_args(Eigen\n REQUIRED_VARS EIGEN_INCLUDE_DIRS\n VERSION_VAR EIGEN_VERSION)\n\n# Only mark internal variables as advanced if we found Eigen, otherwise\n# leave it visible in the standard GUI for the user to set manually.\nif (EIGEN_FOUND)\n mark_as_advanced(FORCE EIGEN_INCLUDE_DIR)\nendif (EIGEN_FOUND)\n" }, { "path": "pose_graph/nodelet_plugin.xml", "content": "\n \n \n PoseGraphNodelet\n \n \n" }, { "path": "pose_graph/package.xml", "content": "\n\n pose_graph\n 0.0.0\n pose_graph package\n\n \n \n \n tony-ws\n\n\n \n \n \n TODO\n\n\n \n \n \n \n\n\n \n \n \n \n\n\n \n \n \n \n \n \n \n \n \n \n \n catkin\n roscpp\n roscpp\n camera_model\n camera_model\n\n nodelet\n nodelet\n\n\n \n \n \n\n" }, { "path": "pose_graph/src/ThirdParty/DBoW/BowVector.cpp", "content": "/**\n * File: BowVector.cpp\n * Date: March 2011\n * Author: Dorian Galvez-Lopez\n * Description: bag of words vector\n * License: see the LICENSE.txt file\n *\n */\n\n#include \n#include \n#include \n#include \n#include \n\n#include \"BowVector.h\"\n\nnamespace DBoW2 {\n\n// --------------------------------------------------------------------------\n\nBowVector::BowVector(void)\n{\n}\n\n// --------------------------------------------------------------------------\n\nBowVector::~BowVector(void)\n{\n}\n\n// --------------------------------------------------------------------------\n\nvoid BowVector::addWeight(WordId id, WordValue v)\n{\n BowVector::iterator vit = this->lower_bound(id);\n \n if(vit != this->end() && !(this->key_comp()(id, vit->first)))\n {\n vit->second += v;\n }\n else\n {\n this->insert(vit, BowVector::value_type(id, v));\n }\n}\n\n// --------------------------------------------------------------------------\n\nvoid BowVector::addIfNotExist(WordId id, WordValue v)\n{\n BowVector::iterator vit = this->lower_bound(id);\n \n if(vit == this->end() || (this->key_comp()(id, vit->first)))\n {\n this->insert(vit, BowVector::value_type(id, v));\n }\n}\n\n// --------------------------------------------------------------------------\n\nvoid BowVector::normalize(LNorm norm_type)\n{\n double norm = 0.0; \n BowVector::iterator it;\n\n if(norm_type == DBoW2::L1)\n {\n for(it = begin(); it != end(); ++it)\n norm += fabs(it->second);\n }\n else\n {\n for(it = begin(); it != end(); ++it)\n norm += it->second * it->second;\n\t\tnorm = sqrt(norm); \n }\n\n if(norm > 0.0)\n {\n for(it = begin(); it != end(); ++it)\n it->second /= norm;\n }\n}\n\n// --------------------------------------------------------------------------\n\nstd::ostream& operator<< (std::ostream &out, const BowVector &v)\n{\n BowVector::const_iterator vit;\n std::vector::const_iterator iit;\n unsigned int i = 0; \n const unsigned int N = v.size();\n for(vit = v.begin(); vit != v.end(); ++vit, ++i)\n {\n out << \"<\" << vit->first << \", \" << vit->second << \">\";\n \n if(i < N-1) out << \", \";\n }\n return out;\n}\n\n// --------------------------------------------------------------------------\n\nvoid BowVector::saveM(const std::string &filename, size_t W) const\n{\n std::fstream f(filename.c_str(), std::ios::out);\n \n WordId last = 0;\n BowVector::const_iterator bit;\n for(bit = this->begin(); bit != this->end(); ++bit)\n {\n for(; last < bit->first; ++last)\n {\n f << \"0 \";\n }\n f << bit->second << \" \";\n \n last = bit->first + 1;\n }\n for(; last < (WordId)W; ++last)\n f << \"0 \";\n \n f.close();\n}\n\n// --------------------------------------------------------------------------\n\n} // namespace DBoW2\n\n" }, { "path": "pose_graph/src/ThirdParty/DBoW/BowVector.h", "content": "/**\n * File: BowVector.h\n * Date: March 2011\n * Author: Dorian Galvez-Lopez\n * Description: bag of words vector\n * License: see the LICENSE.txt file\n *\n */\n\n#ifndef __D_T_BOW_VECTOR__\n#define __D_T_BOW_VECTOR__\n\n#include \n#include \n#include \n\nnamespace DBoW2 {\n\n/// Id of words\ntypedef unsigned int WordId;\n\n/// Value of a word\ntypedef double WordValue;\n\n/// Id of nodes in the vocabulary treee\ntypedef unsigned int NodeId;\n\n/// L-norms for normalization\nenum LNorm\n{\n L1,\n L2\n};\n\n/// Weighting type\nenum WeightingType\n{\n TF_IDF,\n TF,\n IDF,\n BINARY\n};\n\n/// Scoring type\nenum ScoringType\n{\n L1_NORM,\n L2_NORM,\n CHI_SQUARE,\n KL,\n BHATTACHARYYA,\n DOT_PRODUCT\n};\n\n/// Vector of words to represent images\nclass BowVector: \n\tpublic std::map\n{\npublic:\n\n\t/** \n\t * Constructor\n\t */\n\tBowVector(void);\n\n\t/**\n\t * Destructor\n\t */\n\t~BowVector(void);\n\t\n\t/**\n\t * Adds a value to a word value existing in the vector, or creates a new\n\t * word with the given value\n\t * @param id word id to look for\n\t * @param v value to create the word with, or to add to existing word\n\t */\n\tvoid addWeight(WordId id, WordValue v);\n\t\n\t/**\n\t * Adds a word with a value to the vector only if this does not exist yet\n\t * @param id word id to look for\n\t * @param v value to give to the word if this does not exist\n\t */\n\tvoid addIfNotExist(WordId id, WordValue v);\n\n\t/**\n\t * L1-Normalizes the values in the vector \n\t * @param norm_type norm used\n\t */\n\tvoid normalize(LNorm norm_type);\n\t\n\t/**\n\t * Prints the content of the bow vector\n\t * @param out stream\n\t * @param v\n\t */\n\tfriend std::ostream& operator<<(std::ostream &out, const BowVector &v);\n\t\n\t/**\n\t * Saves the bow vector as a vector in a matlab file\n\t * @param filename\n\t * @param W number of words in the vocabulary\n\t */\n\tvoid saveM(const std::string &filename, size_t W) const;\n};\n\n} // namespace DBoW2\n\n#endif\n" }, { "path": "pose_graph/src/ThirdParty/DBoW/DBoW2.h", "content": "/*\n * File: DBoW2.h\n * Date: November 2011\n * Author: Dorian Galvez-Lopez\n * Description: Generic include file for the DBoW2 classes and\n * the specialized vocabularies and databases\n * License: see the LICENSE.txt file\n *\n */\n\n/*! \\mainpage DBoW2 Library\n *\n * DBoW2 library for C++:\n * Bag-of-word image database for image retrieval.\n *\n * Written by Dorian Galvez-Lopez,\n * University of Zaragoza\n * \n * Check my website to obtain updates: http://doriangalvez.com\n *\n * \\section requirements Requirements\n * This library requires the DUtils, DUtilsCV, DVision and OpenCV libraries,\n * as well as the boost::dynamic_bitset class.\n *\n * \\section citation Citation\n * If you use this software in academic works, please cite:\n 
\n   @@ARTICLE{GalvezTRO12,\n    author={Galvez-Lopez, Dorian and Tardos, J. D.}, \n    journal={IEEE Transactions on Robotics},\n    title={Bags of Binary Words for Fast Place Recognition in Image Sequences},\n    year={2012},\n    month={October},\n    volume={28},\n    number={5},\n    pages={1188--1197},\n    doi={10.1109/TRO.2012.2197158},\n    ISSN={1552-3098}\n  }\n
\n *\n * \\section license License\n * This file is licensed under a Creative Commons \n * Attribution-NonCommercial-ShareAlike 3.0 license. \n * This file can be freely used and users can use, download and edit this file \n * provided that credit is attributed to the original author. No users are \n * permitted to use this file for commercial purposes unless explicit permission\n * is given by the original author. Derivative works must be licensed using the\n * same or similar license.\n * Check http://creativecommons.org/licenses/by-nc-sa/3.0/ to obtain further\n * details.\n *\n */\n\n#ifndef __D_T_DBOW2__\n#define __D_T_DBOW2__\n\n/// Includes all the data structures to manage vocabularies and image databases\nnamespace DBoW2\n{\n}\n\n#include \"TemplatedVocabulary.h\"\n#include \"TemplatedDatabase.h\"\n#include \"BowVector.h\"\n#include \"FeatureVector.h\"\n#include \"QueryResults.h\"\n#include \"FBrief.h\"\n\n/// BRIEF Vocabulary\ntypedef DBoW2::TemplatedVocabulary \n BriefVocabulary;\n\n/// BRIEF Database\ntypedef DBoW2::TemplatedDatabase \n BriefDatabase;\n\n#endif\n\n" }, { "path": "pose_graph/src/ThirdParty/DBoW/FBrief.cpp", "content": "/**\r\n * File: FBrief.cpp\r\n * Date: November 2011\r\n * Author: Dorian Galvez-Lopez\r\n * Description: functions for BRIEF descriptors\r\n * License: see the LICENSE.txt file\r\n *\r\n */\r\n \r\n#include \r\n#include \r\n#include \r\n\r\n#include \"FBrief.h\"\r\n\r\nusing namespace std;\r\n\r\nnamespace DBoW2 {\r\n\r\n// --------------------------------------------------------------------------\r\n\r\nvoid FBrief::meanValue(const std::vector &descriptors, \r\n FBrief::TDescriptor &mean)\r\n{\r\n mean.reset();\r\n \r\n if(descriptors.empty()) return;\r\n \r\n const int N2 = descriptors.size() / 2;\r\n const int L = descriptors[0]->size();\r\n \r\n vector counters(L, 0);\r\n\r\n vector::const_iterator it;\r\n for(it = descriptors.begin(); it != descriptors.end(); ++it)\r\n {\r\n const FBrief::TDescriptor &desc = **it;\r\n for(int i = 0; i < L; ++i)\r\n {\r\n if(desc[i]) counters[i]++;\r\n }\r\n }\r\n \r\n for(int i = 0; i < L; ++i)\r\n {\r\n if(counters[i] > N2) mean.set(i);\r\n }\r\n \r\n}\r\n\r\n// --------------------------------------------------------------------------\r\n \r\ndouble FBrief::distance(const FBrief::TDescriptor &a, \r\n const FBrief::TDescriptor &b)\r\n{\r\n return (double)DVision::BRIEF::distance(a, b);\r\n}\r\n\r\n// --------------------------------------------------------------------------\r\n \r\nstd::string FBrief::toString(const FBrief::TDescriptor &a)\r\n{\r\n // from boost::bitset\r\n string s;\r\n to_string(a, s); // reversed\r\n return s;\r\n}\r\n\r\n// --------------------------------------------------------------------------\r\n \r\nvoid FBrief::fromString(FBrief::TDescriptor &a, const std::string &s)\r\n{\r\n // from boost::bitset\r\n stringstream ss(s);\r\n ss >> a;\r\n}\r\n\r\n// --------------------------------------------------------------------------\r\n\r\nvoid FBrief::toMat32F(const std::vector &descriptors, \r\n cv::Mat &mat)\r\n{\r\n if(descriptors.empty())\r\n {\r\n mat.release();\r\n return;\r\n }\r\n \r\n const int N = descriptors.size();\r\n const int L = descriptors[0].size();\r\n \r\n mat.create(N, L, CV_32F);\r\n \r\n for(int i = 0; i < N; ++i)\r\n {\r\n const TDescriptor& desc = descriptors[i];\r\n float *p = mat.ptr(i);\r\n for(int j = 0; j < L; ++j, ++p)\r\n {\r\n *p = (desc[j] ? 1 : 0);\r\n }\r\n } \r\n}\r\n\r\n// --------------------------------------------------------------------------\r\n\r\n} // namespace DBoW2\r\n\r\n" }, { "path": "pose_graph/src/ThirdParty/DBoW/FBrief.h", "content": "/**\r\n * File: FBrief.h\r\n * Date: November 2011\r\n * Author: Dorian Galvez-Lopez\r\n * Description: functions for BRIEF descriptors\r\n * License: see the LICENSE.txt file\r\n *\r\n */\r\n\r\n#ifndef __D_T_F_BRIEF__\r\n#define __D_T_F_BRIEF__\r\n\r\n#include \r\n#include \r\n#include \r\n\r\n#include \"FClass.h\"\r\n#include \"../DVision/DVision.h\"\r\n\r\nnamespace DBoW2 {\r\n\r\n/// Functions to manipulate BRIEF descriptors\r\nclass FBrief: protected FClass\r\n{\r\npublic:\r\n\r\n typedef DVision::BRIEF::bitset TDescriptor;\r\n typedef const TDescriptor *pDescriptor;\r\n\r\n /**\r\n * Calculates the mean value of a set of descriptors\r\n * @param descriptors\r\n * @param mean mean descriptor\r\n */\r\n static void meanValue(const std::vector &descriptors, \r\n TDescriptor &mean);\r\n \r\n /**\r\n * Calculates the distance between two descriptors\r\n * @param a\r\n * @param b\r\n * @return distance\r\n */\r\n static double distance(const TDescriptor &a, const TDescriptor &b);\r\n \r\n /**\r\n * Returns a string version of the descriptor\r\n * @param a descriptor\r\n * @return string version\r\n */\r\n static std::string toString(const TDescriptor &a);\r\n \r\n /**\r\n * Returns a descriptor from a string\r\n * @param a descriptor\r\n * @param s string version\r\n */\r\n static void fromString(TDescriptor &a, const std::string &s);\r\n \r\n /**\r\n * Returns a mat with the descriptors in float format\r\n * @param descriptors\r\n * @param mat (out) NxL 32F matrix\r\n */\r\n static void toMat32F(const std::vector &descriptors, \r\n cv::Mat &mat);\r\n\r\n};\r\n\r\n} // namespace DBoW2\r\n\r\n#endif\r\n\r\n" }, { "path": "pose_graph/src/ThirdParty/DBoW/FClass.h", "content": "/**\n * File: FClass.h\n * Date: November 2011\n * Author: Dorian Galvez-Lopez\n * Description: generic FClass to instantiate templated classes\n * License: see the LICENSE.txt file\n *\n */\n\n#ifndef __D_T_FCLASS__\n#define __D_T_FCLASS__\n\n#include \n#include \n#include \n\nnamespace DBoW2 {\n\n/// Generic class to encapsulate functions to manage descriptors.\n/**\n * This class must be inherited. Derived classes can be used as the\n * parameter F when creating Templated structures\n * (TemplatedVocabulary, TemplatedDatabase, ...)\n */\nclass FClass\n{\n class TDescriptor;\n typedef const TDescriptor *pDescriptor;\n \n /**\n * Calculates the mean value of a set of descriptors\n * @param descriptors\n * @param mean mean descriptor\n */\n virtual void meanValue(const std::vector &descriptors, \n TDescriptor &mean) = 0;\n \n /**\n * Calculates the distance between two descriptors\n * @param a\n * @param b\n * @return distance\n */\n static double distance(const TDescriptor &a, const TDescriptor &b);\n \n /**\n * Returns a string version of the descriptor\n * @param a descriptor\n * @return string version\n */\n static std::string toString(const TDescriptor &a);\n \n /**\n * Returns a descriptor from a string\n * @param a descriptor\n * @param s string version\n */\n static void fromString(TDescriptor &a, const std::string &s);\n\n /**\n * Returns a mat with the descriptors in float format\n * @param descriptors\n * @param mat (out) NxL 32F matrix\n */\n static void toMat32F(const std::vector &descriptors, \n cv::Mat &mat);\n};\n\n} // namespace DBoW2\n\n#endif\n" }, { "path": "pose_graph/src/ThirdParty/DBoW/FeatureVector.cpp", "content": "/**\n * File: FeatureVector.cpp\n * Date: November 2011\n * Author: Dorian Galvez-Lopez\n * Description: feature vector\n * License: see the LICENSE.txt file\n *\n */\n\n#include \"FeatureVector.h\"\n#include \n#include \n#include \n\nnamespace DBoW2 {\n\n// ---------------------------------------------------------------------------\n\nFeatureVector::FeatureVector(void)\n{\n}\n\n// ---------------------------------------------------------------------------\n\nFeatureVector::~FeatureVector(void)\n{\n}\n\n// ---------------------------------------------------------------------------\n\nvoid FeatureVector::addFeature(NodeId id, unsigned int i_feature)\n{\n FeatureVector::iterator vit = this->lower_bound(id);\n \n if(vit != this->end() && vit->first == id)\n {\n vit->second.push_back(i_feature);\n }\n else\n {\n vit = this->insert(vit, FeatureVector::value_type(id, \n std::vector() ));\n vit->second.push_back(i_feature);\n }\n}\n\n// ---------------------------------------------------------------------------\n\nstd::ostream& operator<<(std::ostream &out, \n const FeatureVector &v)\n{\n if(!v.empty())\n {\n FeatureVector::const_iterator vit = v.begin();\n \n const std::vector* f = &vit->second;\n\n out << \"<\" << vit->first << \": [\";\n if(!f->empty()) out << (*f)[0];\n for(unsigned int i = 1; i < f->size(); ++i)\n {\n out << \", \" << (*f)[i];\n }\n out << \"]>\";\n \n for(++vit; vit != v.end(); ++vit)\n {\n f = &vit->second;\n \n out << \", <\" << vit->first << \": [\";\n if(!f->empty()) out << (*f)[0];\n for(unsigned int i = 1; i < f->size(); ++i)\n {\n out << \", \" << (*f)[i];\n }\n out << \"]>\";\n }\n }\n \n return out; \n}\n\n// ---------------------------------------------------------------------------\n\n} // namespace DBoW2\n" }, { "path": "pose_graph/src/ThirdParty/DBoW/FeatureVector.h", "content": "/**\n * File: FeatureVector.h\n * Date: November 2011\n * Author: Dorian Galvez-Lopez\n * Description: feature vector\n * License: see the LICENSE.txt file\n *\n */\n\n#ifndef __D_T_FEATURE_VECTOR__\n#define __D_T_FEATURE_VECTOR__\n\n#include \"BowVector.h\"\n#include \n#include \n#include \n\nnamespace DBoW2 {\n\n/// Vector of nodes with indexes of local features\nclass FeatureVector: \n public std::map >\n{\npublic:\n\n /**\n * Constructor\n */\n FeatureVector(void);\n \n /**\n * Destructor\n */\n ~FeatureVector(void);\n \n /**\n * Adds a feature to an existing node, or adds a new node with an initial\n * feature\n * @param id node id to add or to modify\n * @param i_feature index of feature to add to the given node\n */\n void addFeature(NodeId id, unsigned int i_feature);\n\n /**\n * Sends a string versions of the feature vector through the stream\n * @param out stream\n * @param v feature vector\n */\n friend std::ostream& operator<<(std::ostream &out, const FeatureVector &v);\n \n};\n\n} // namespace DBoW2\n\n#endif\n\n" }, { "path": "pose_graph/src/ThirdParty/DBoW/QueryResults.cpp", "content": "/**\n * File: QueryResults.cpp\n * Date: March, November 2011\n * Author: Dorian Galvez-Lopez\n * Description: structure to store results of database queries\n * License: see the LICENSE.txt file\n *\n */\n\n#include \n#include \n#include \"QueryResults.h\"\n\nusing namespace std;\n\nnamespace DBoW2\n{\n\n// ---------------------------------------------------------------------------\n\nostream & operator<<(ostream& os, const Result& ret )\n{\n os << \"\";\n return os;\n}\n\n// ---------------------------------------------------------------------------\n\nostream & operator<<(ostream& os, const QueryResults& ret )\n{\n if(ret.size() == 1)\n os << \"1 result:\" << endl;\n else\n os << ret.size() << \" results:\" << endl;\n \n QueryResults::const_iterator rit;\n for(rit = ret.begin(); rit != ret.end(); ++rit)\n {\n os << *rit;\n if(rit + 1 != ret.end()) os << endl;\n }\n return os;\n}\n\n// ---------------------------------------------------------------------------\n\nvoid QueryResults::saveM(const std::string &filename) const\n{\n fstream f(filename.c_str(), ios::out);\n \n QueryResults::const_iterator qit;\n for(qit = begin(); qit != end(); ++qit)\n {\n f << qit->Id << \" \" << qit->Score << endl;\n }\n \n f.close();\n}\n\n// ---------------------------------------------------------------------------\n\n} // namespace DBoW2\n\n" }, { "path": "pose_graph/src/ThirdParty/DBoW/QueryResults.h", "content": "/**\n * File: QueryResults.h\n * Date: March, November 2011\n * Author: Dorian Galvez-Lopez\n * Description: structure to store results of database queries\n * License: see the LICENSE.txt file\n *\n */\n\n#ifndef __D_T_QUERY_RESULTS__\n#define __D_T_QUERY_RESULTS__\n\n#include \n\nnamespace DBoW2 {\n\n/// Id of entries of the database\ntypedef unsigned int EntryId;\n\n/// Single result of a query\nclass Result\n{\npublic:\n \n /// Entry id\n EntryId Id;\n \n /// Score obtained\n double Score;\n \n /// debug\n int nWords; // words in common\n // !!! this is filled only by Bhatt score!\n // (and for BCMatching, BCThresholding then)\n \n double bhatScore, chiScore;\n /// debug\n \n // only done by ChiSq and BCThresholding \n double sumCommonVi;\n double sumCommonWi;\n double expectedChiScore;\n /// debug\n\n /**\n * Empty constructors\n */\n inline Result(){}\n \n /**\n * Creates a result with the given data\n * @param _id entry id\n * @param _score score\n */\n inline Result(EntryId _id, double _score): Id(_id), Score(_score){}\n\n /**\n * Compares the scores of two results\n * @return true iff this.score < r.score\n */\n inline bool operator<(const Result &r) const\n {\n return this->Score < r.Score;\n }\n\n /**\n * Compares the scores of two results\n * @return true iff this.score > r.score\n */\n inline bool operator>(const Result &r) const\n {\n return this->Score > r.Score;\n }\n\n /**\n * Compares the entry id of the result\n * @return true iff this.id == id\n */\n inline bool operator==(EntryId id) const\n {\n return this->Id == id;\n }\n \n /**\n * Compares the score of this entry with a given one\n * @param s score to compare with\n * @return true iff this score < s\n */\n inline bool operator<(double s) const\n {\n return this->Score < s;\n }\n \n /**\n * Compares the score of this entry with a given one\n * @param s score to compare with\n * @return true iff this score > s\n */\n inline bool operator>(double s) const\n {\n return this->Score > s;\n }\n \n /**\n * Compares the score of two results\n * @param a\n * @param b\n * @return true iff a.Score > b.Score\n */\n static inline bool gt(const Result &a, const Result &b)\n {\n return a.Score > b.Score;\n }\n \n /**\n * Compares the scores of two results\n * @return true iff a.Score > b.Score\n */\n inline static bool ge(const Result &a, const Result &b)\n {\n return a.Score > b.Score;\n }\n \n /**\n * Returns true iff a.Score >= b.Score\n * @param a\n * @param b\n * @return true iff a.Score >= b.Score\n */\n static inline bool geq(const Result &a, const Result &b)\n {\n return a.Score >= b.Score;\n }\n \n /**\n * Returns true iff a.Score >= s\n * @param a\n * @param s\n * @return true iff a.Score >= s\n */\n static inline bool geqv(const Result &a, double s)\n {\n return a.Score >= s;\n }\n \n \n /**\n * Returns true iff a.Id < b.Id\n * @param a\n * @param b\n * @return true iff a.Id < b.Id\n */\n static inline bool ltId(const Result &a, const Result &b)\n {\n return a.Id < b.Id;\n }\n \n /**\n * Prints a string version of the result\n * @param os ostream\n * @param ret Result to print\n */\n friend std::ostream & operator<<(std::ostream& os, const Result& ret );\n};\n\n/// Multiple results from a query\nclass QueryResults: public std::vector\n{\npublic:\n\n /** \n * Multiplies all the scores in the vector by factor\n * @param factor\n */\n inline void scaleScores(double factor);\n \n /**\n * Prints a string version of the results\n * @param os ostream\n * @param ret QueryResults to print\n */\n friend std::ostream & operator<<(std::ostream& os, const QueryResults& ret );\n \n /**\n * Saves a matlab file with the results \n * @param filename \n */\n void saveM(const std::string &filename) const;\n \n};\n\n// --------------------------------------------------------------------------\n\ninline void QueryResults::scaleScores(double factor)\n{\n for(QueryResults::iterator qit = begin(); qit != end(); ++qit) \n qit->Score *= factor;\n}\n\n// --------------------------------------------------------------------------\n\n} // namespace TemplatedBoW\n \n#endif\n\n" }, { "path": "pose_graph/src/ThirdParty/DBoW/ScoringObject.cpp", "content": "/**\n * File: ScoringObject.cpp\n * Date: November 2011\n * Author: Dorian Galvez-Lopez\n * Description: functions to compute bow scores \n * License: see the LICENSE.txt file\n *\n */\n\n#include \n#include \"TemplatedVocabulary.h\"\n#include \"BowVector.h\"\n\nusing namespace DBoW2;\n\n// If you change the type of WordValue, make sure you change also the\n// epsilon value (this is needed by the KL method)\nconst double GeneralScoring::LOG_EPS = log(DBL_EPSILON); // FLT_EPSILON\n\n// ---------------------------------------------------------------------------\n// ---------------------------------------------------------------------------\n\ndouble L1Scoring::score(const BowVector &v1, const BowVector &v2) const\n{\n BowVector::const_iterator v1_it, v2_it;\n const BowVector::const_iterator v1_end = v1.end();\n const BowVector::const_iterator v2_end = v2.end();\n \n v1_it = v1.begin();\n v2_it = v2.begin();\n \n double score = 0;\n \n while(v1_it != v1_end && v2_it != v2_end)\n {\n const WordValue& vi = v1_it->second;\n const WordValue& wi = v2_it->second;\n \n if(v1_it->first == v2_it->first)\n {\n score += fabs(vi - wi) - fabs(vi) - fabs(wi);\n \n // move v1 and v2 forward\n ++v1_it;\n ++v2_it;\n }\n else if(v1_it->first < v2_it->first)\n {\n // move v1 forward\n v1_it = v1.lower_bound(v2_it->first);\n // v1_it = (first element >= v2_it.id)\n }\n else\n {\n // move v2 forward\n v2_it = v2.lower_bound(v1_it->first);\n // v2_it = (first element >= v1_it.id)\n }\n }\n \n // ||v - w||_{L1} = 2 + Sum(|v_i - w_i| - |v_i| - |w_i|) \n //\t\tfor all i | v_i != 0 and w_i != 0 \n // (Nister, 2006)\n // scaled_||v - w||_{L1} = 1 - 0.5 * ||v - w||_{L1}\n score = -score/2.0;\n\n return score; // [0..1]\n}\n\n// ---------------------------------------------------------------------------\n// ---------------------------------------------------------------------------\n\ndouble L2Scoring::score(const BowVector &v1, const BowVector &v2) const\n{\n BowVector::const_iterator v1_it, v2_it;\n const BowVector::const_iterator v1_end = v1.end();\n const BowVector::const_iterator v2_end = v2.end();\n \n v1_it = v1.begin();\n v2_it = v2.begin();\n \n double score = 0;\n \n while(v1_it != v1_end && v2_it != v2_end)\n {\n const WordValue& vi = v1_it->second;\n const WordValue& wi = v2_it->second;\n \n if(v1_it->first == v2_it->first)\n {\n score += vi * wi;\n \n // move v1 and v2 forward\n ++v1_it;\n ++v2_it;\n }\n else if(v1_it->first < v2_it->first)\n {\n // move v1 forward\n v1_it = v1.lower_bound(v2_it->first);\n // v1_it = (first element >= v2_it.id)\n }\n else\n {\n // move v2 forward\n v2_it = v2.lower_bound(v1_it->first);\n // v2_it = (first element >= v1_it.id)\n }\n }\n \n // ||v - w||_{L2} = sqrt( 2 - 2 * Sum(v_i * w_i) )\n\t//\t\tfor all i | v_i != 0 and w_i != 0 )\n\t// (Nister, 2006)\n\tif(score >= 1) // rounding errors\n\t score = 1.0;\n\telse\n score = 1.0 - sqrt(1.0 - score); // [0..1]\n\n return score;\n}\n\n// ---------------------------------------------------------------------------\n// ---------------------------------------------------------------------------\n\ndouble ChiSquareScoring::score(const BowVector &v1, const BowVector &v2) \n const\n{\n BowVector::const_iterator v1_it, v2_it;\n const BowVector::const_iterator v1_end = v1.end();\n const BowVector::const_iterator v2_end = v2.end();\n \n v1_it = v1.begin();\n v2_it = v2.begin();\n \n double score = 0;\n \n // all the items are taken into account\n \n while(v1_it != v1_end && v2_it != v2_end)\n {\n const WordValue& vi = v1_it->second;\n const WordValue& wi = v2_it->second;\n \n if(v1_it->first == v2_it->first)\n {\n // (v-w)^2/(v+w) - v - w = -4 vw/(v+w)\n // we move the -4 out\n if(vi + wi != 0.0) score += vi * wi / (vi + wi);\n \n // move v1 and v2 forward\n ++v1_it;\n ++v2_it;\n }\n else if(v1_it->first < v2_it->first)\n {\n // move v1 forward\n v1_it = v1.lower_bound(v2_it->first);\n }\n else\n {\n // move v2 forward\n v2_it = v2.lower_bound(v1_it->first);\n }\n }\n \n // this takes the -4 into account\n score = 2. * score; // [0..1]\n\n return score;\n}\n\n// ---------------------------------------------------------------------------\n// ---------------------------------------------------------------------------\n\ndouble KLScoring::score(const BowVector &v1, const BowVector &v2) const\n{ \n BowVector::const_iterator v1_it, v2_it;\n const BowVector::const_iterator v1_end = v1.end();\n const BowVector::const_iterator v2_end = v2.end();\n \n v1_it = v1.begin();\n v2_it = v2.begin();\n \n double score = 0;\n \n // all the items or v are taken into account\n \n while(v1_it != v1_end && v2_it != v2_end)\n {\n const WordValue& vi = v1_it->second;\n const WordValue& wi = v2_it->second;\n \n if(v1_it->first == v2_it->first)\n {\n if(vi != 0 && wi != 0) score += vi * log(vi/wi);\n \n // move v1 and v2 forward\n ++v1_it;\n ++v2_it;\n }\n else if(v1_it->first < v2_it->first)\n {\n // move v1 forward\n score += vi * (log(vi) - LOG_EPS);\n ++v1_it;\n }\n else\n {\n // move v2_it forward, do not add any score\n v2_it = v2.lower_bound(v1_it->first);\n // v2_it = (first element >= v1_it.id)\n }\n }\n \n // sum rest of items of v\n for(; v1_it != v1_end; ++v1_it) \n if(v1_it->second != 0)\n score += v1_it->second * (log(v1_it->second) - LOG_EPS);\n \n return score; // cannot be scaled\n}\n\n// ---------------------------------------------------------------------------\n// ---------------------------------------------------------------------------\n\ndouble BhattacharyyaScoring::score(const BowVector &v1, \n const BowVector &v2) const\n{\n BowVector::const_iterator v1_it, v2_it;\n const BowVector::const_iterator v1_end = v1.end();\n const BowVector::const_iterator v2_end = v2.end();\n \n v1_it = v1.begin();\n v2_it = v2.begin();\n \n double score = 0;\n \n while(v1_it != v1_end && v2_it != v2_end)\n {\n const WordValue& vi = v1_it->second;\n const WordValue& wi = v2_it->second;\n \n if(v1_it->first == v2_it->first)\n {\n score += sqrt(vi * wi);\n \n // move v1 and v2 forward\n ++v1_it;\n ++v2_it;\n }\n else if(v1_it->first < v2_it->first)\n {\n // move v1 forward\n v1_it = v1.lower_bound(v2_it->first);\n // v1_it = (first element >= v2_it.id)\n }\n else\n {\n // move v2 forward\n v2_it = v2.lower_bound(v1_it->first);\n // v2_it = (first element >= v1_it.id)\n }\n }\n\n return score; // already scaled\n}\n\n// ---------------------------------------------------------------------------\n// ---------------------------------------------------------------------------\n\ndouble DotProductScoring::score(const BowVector &v1, \n const BowVector &v2) const\n{\n BowVector::const_iterator v1_it, v2_it;\n const BowVector::const_iterator v1_end = v1.end();\n const BowVector::const_iterator v2_end = v2.end();\n \n v1_it = v1.begin();\n v2_it = v2.begin();\n \n double score = 0;\n \n while(v1_it != v1_end && v2_it != v2_end)\n {\n const WordValue& vi = v1_it->second;\n const WordValue& wi = v2_it->second;\n \n if(v1_it->first == v2_it->first)\n {\n score += vi * wi;\n \n // move v1 and v2 forward\n ++v1_it;\n ++v2_it;\n }\n else if(v1_it->first < v2_it->first)\n {\n // move v1 forward\n v1_it = v1.lower_bound(v2_it->first);\n // v1_it = (first element >= v2_it.id)\n }\n else\n {\n // move v2 forward\n v2_it = v2.lower_bound(v1_it->first);\n // v2_it = (first element >= v1_it.id)\n }\n }\n\n return score; // cannot scale\n}\n\n// ---------------------------------------------------------------------------\n// ---------------------------------------------------------------------------\n\n" }, { "path": "pose_graph/src/ThirdParty/DBoW/ScoringObject.h", "content": "/**\n * File: ScoringObject.h\n * Date: November 2011\n * Author: Dorian Galvez-Lopez\n * Description: functions to compute bow scores \n * License: see the LICENSE.txt file\n *\n */\n\n#ifndef __D_T_SCORING_OBJECT__\n#define __D_T_SCORING_OBJECT__\n\n#include \"BowVector.h\"\n\nnamespace DBoW2 {\n\n/// Base class of scoring functions\nclass GeneralScoring\n{\npublic:\n /**\n * Computes the score between two vectors. Vectors must be sorted and \n * normalized if necessary\n * @param v (in/out)\n * @param w (in/out)\n * @return score\n */\n virtual double score(const BowVector &v, const BowVector &w) const = 0;\n\n /**\n * Returns whether a vector must be normalized before scoring according\n * to the scoring scheme\n * @param norm norm to use\n * @return true iff must normalize\n */\n virtual bool mustNormalize(LNorm &norm) const = 0;\n\n /// Log of epsilon\n\tstatic const double LOG_EPS; \n // If you change the type of WordValue, make sure you change also the\n\t// epsilon value (this is needed by the KL method)\n\t\n virtual ~GeneralScoring() {} //!< Required for virtual base classes\t\n};\n\n/** \n * Macro for defining Scoring classes\n * @param NAME name of class\n * @param MUSTNORMALIZE if vectors must be normalized to compute the score\n * @param NORM type of norm to use when MUSTNORMALIZE\n */\n#define __SCORING_CLASS(NAME, MUSTNORMALIZE, NORM) \\\n NAME: public GeneralScoring \\\n { public: \\\n /** \\\n * Computes score between two vectors \\\n * @param v \\\n * @param w \\\n * @return score between v and w \\\n */ \\\n virtual double score(const BowVector &v, const BowVector &w) const; \\\n \\\n /** \\\n * Says if a vector must be normalized according to the scoring function \\\n * @param norm (out) if true, norm to use\n * @return true iff vectors must be normalized \\\n */ \\\n virtual inline bool mustNormalize(LNorm &norm) const \\\n { norm = NORM; return MUSTNORMALIZE; } \\\n }\n \n/// L1 Scoring object\nclass __SCORING_CLASS(L1Scoring, true, L1);\n\n/// L2 Scoring object\nclass __SCORING_CLASS(L2Scoring, true, L2);\n\n/// Chi square Scoring object\nclass __SCORING_CLASS(ChiSquareScoring, true, L1);\n\n/// KL divergence Scoring object\nclass __SCORING_CLASS(KLScoring, true, L1);\n\n/// Bhattacharyya Scoring object\nclass __SCORING_CLASS(BhattacharyyaScoring, true, L1);\n\n/// Dot product Scoring object\nclass __SCORING_CLASS(DotProductScoring, false, L1);\n\n#undef __SCORING_CLASS\n \n} // namespace DBoW2\n\n#endif\n\n" }, { "path": "pose_graph/src/ThirdParty/DBoW/TemplatedDatabase.h", "content": "/**\n * File: TemplatedDatabase.h\n * Date: March 2011\n * Author: Dorian Galvez-Lopez\n * Description: templated database of images\n * License: see the LICENSE.txt file\n *\n */\n \n#ifndef __D_T_TEMPLATED_DATABASE__\n#define __D_T_TEMPLATED_DATABASE__\n\n#include \n#include \n#include \n#include \n#include \n#include \n\n#include \"TemplatedVocabulary.h\"\n#include \"QueryResults.h\"\n#include \"ScoringObject.h\"\n#include \"BowVector.h\"\n#include \"FeatureVector.h\"\n\n#include \"../DUtils/DUtils.h\"\n\nnamespace DBoW2 {\n\n// For query functions\nstatic int MIN_COMMON_WORDS = 5;\n\n/// @param TDescriptor class of descriptor\n/// @param F class of descriptor functions\ntemplate\n/// Generic Database\nclass TemplatedDatabase\n{\npublic:\n\n /**\n * Creates an empty database without vocabulary\n * @param use_di a direct index is used to store feature indexes\n * @param di_levels levels to go up the vocabulary tree to select the \n * node id to store in the direct index when adding images\n */\n explicit TemplatedDatabase(bool use_di = true, int di_levels = 0);\n\n /**\n * Creates a database with the given vocabulary\n * @param T class inherited from TemplatedVocabulary\n * @param voc vocabulary\n * @param use_di a direct index is used to store feature indexes\n * @param di_levels levels to go up the vocabulary tree to select the \n * node id to store in the direct index when adding images\n */\n template\n explicit TemplatedDatabase(const T &voc, bool use_di = true, \n int di_levels = 0);\n\n /**\n * Copy constructor. Copies the vocabulary too\n * @param db object to copy\n */\n TemplatedDatabase(const TemplatedDatabase &db);\n\n /** \n * Creates the database from a file\n * @param filename\n */\n TemplatedDatabase(const std::string &filename);\n\n /** \n * Creates the database from a file\n * @param filename\n */\n TemplatedDatabase(const char *filename);\n\n /**\n * Destructor\n */\n virtual ~TemplatedDatabase(void);\n\n /**\n * Copies the given database and its vocabulary\n * @param db database to copy\n */\n TemplatedDatabase& operator=(\n const TemplatedDatabase &db);\n\n /**\n * Sets the vocabulary to use and clears the content of the database.\n * @param T class inherited from TemplatedVocabulary\n * @param voc vocabulary to copy\n */\n template\n inline void setVocabulary(const T &voc);\n \n /**\n * Sets the vocabulary to use and the direct index parameters, and clears\n * the content of the database\n * @param T class inherited from TemplatedVocabulary\n * @param voc vocabulary to copy\n * @param use_di a direct index is used to store feature indexes\n * @param di_levels levels to go up the vocabulary tree to select the \n * node id to store in the direct index when adding images\n */\n template\n void setVocabulary(const T& voc, bool use_di, int di_levels = 0);\n \n /**\n * Returns a pointer to the vocabulary used\n * @return vocabulary\n */\n inline const TemplatedVocabulary* getVocabulary() const;\n\n /** \n * Allocates some memory for the direct and inverted indexes\n * @param nd number of expected image entries in the database \n * @param ni number of expected words per image\n * @note Use 0 to ignore a parameter\n */\n void allocate(int nd = 0, int ni = 0);\n\n /**\n * Adds an entry to the database and returns its index\n * @param features features of the new entry\n * @param bowvec if given, the bow vector of these features is returned\n * @param fvec if given, the vector of nodes and feature indexes is returned\n * @return id of new entry\n */\n EntryId add(const std::vector &features,\n BowVector *bowvec = NULL, FeatureVector *fvec = NULL);\n\n /**\n * Adss an entry to the database and returns its index\n * @param vec bow vector\n * @param fec feature vector to add the entry. Only necessary if using the\n * direct index\n * @return id of new entry\n */\n EntryId add(const BowVector &vec, \n const FeatureVector &fec = FeatureVector() );\n\n void delete_entry(const EntryId entry_id);\n\n /**\n * Empties the database\n */\n inline void clear();\n\n /**\n * Returns the number of entries in the database \n * @return number of entries in the database\n */\n inline unsigned int size() const;\n \n /**\n * Checks if the direct index is being used\n * @return true iff using direct index\n */\n inline bool usingDirectIndex() const;\n \n /**\n * Returns the di levels when using direct index\n * @return di levels\n */\n inline int getDirectIndexLevels() const;\n \n /**\n * Queries the database with some features\n * @param features query features\n * @param ret (out) query results\n * @param max_results number of results to return. <= 0 means all\n * @param max_id only entries with id <= max_id are returned in ret. \n * < 0 means all\n */\n void query(const std::vector &features, QueryResults &ret,\n int max_results = 1, int max_id = -1) const;\n \n /**\n * Queries the database with a vector\n * @param vec bow vector already normalized\n * @param ret results\n * @param max_results number of results to return. <= 0 means all\n * @param max_id only entries with id <= max_id are returned in ret. \n * < 0 means all\n */\n void query(const BowVector &vec, QueryResults &ret, \n int max_results = 1, int max_id = -1) const;\n\n /**\n * Returns the a feature vector associated with a database entry\n * @param id entry id (must be < size())\n * @return const reference to map of nodes and their associated features in\n * the given entry\n */\n const FeatureVector& retrieveFeatures(EntryId id) const;\n\n /**\n * Stores the database in a file\n * @param filename\n */\n void save(const std::string &filename) const;\n \n /**\n * Loads the database from a file\n * @param filename\n */\n void load(const std::string &filename);\n \n /** \n * Stores the database in the given file storage structure\n * @param fs\n * @param name node name\n */\n virtual void save(cv::FileStorage &fs, \n const std::string &name = \"database\") const;\n \n /** \n * Loads the database from the given file storage structure\n * @param fs\n * @param name node name\n */\n virtual void load(const cv::FileStorage &fs, \n const std::string &name = \"database\");\n\nprotected:\n \n /// Query with L1 scoring\n void queryL1(const BowVector &vec, QueryResults &ret, \n int max_results, int max_id) const;\n \n /// Query with L2 scoring\n void queryL2(const BowVector &vec, QueryResults &ret, \n int max_results, int max_id) const;\n \n /// Query with Chi square scoring\n void queryChiSquare(const BowVector &vec, QueryResults &ret, \n int max_results, int max_id) const;\n \n /// Query with Bhattacharyya scoring\n void queryBhattacharyya(const BowVector &vec, QueryResults &ret, \n int max_results, int max_id) const;\n \n /// Query with KL divergence scoring \n void queryKL(const BowVector &vec, QueryResults &ret, \n int max_results, int max_id) const;\n \n /// Query with dot product scoring\n void queryDotProduct(const BowVector &vec, QueryResults &ret, \n int max_results, int max_id) const;\n\nprotected:\n\n /* Inverted file declaration */\n \n /// Item of IFRow\n struct IFPair\n {\n /// Entry id\n EntryId entry_id;\n \n /// Word weight in this entry\n WordValue word_weight;\n \n /**\n * Creates an empty pair\n */\n IFPair(){}\n \n /**\n * Creates an inverted file pair\n * @param eid entry id\n * @param wv word weight\n */\n IFPair(EntryId eid, WordValue wv): entry_id(eid), word_weight(wv) {}\n \n /**\n * Compares the entry ids\n * @param eid\n * @return true iff this entry id is the same as eid\n */\n inline bool operator==(EntryId eid) const { return entry_id == eid; }\n };\n \n /// Row of InvertedFile\n typedef std::list IFRow;\n // IFRows are sorted in ascending entry_id order\n \n /// Inverted index\n typedef std::vector InvertedFile; \n // InvertedFile[word_id] --> inverted file of that word\n \n /* Direct file declaration */\n\n /// Direct index\n typedef std::vector DirectFile;\n // DirectFile[entry_id] --> [ directentry, ... ]\n\nprotected:\n\n /// Associated vocabulary\n TemplatedVocabulary *m_voc;\n \n /// Flag to use direct index\n bool m_use_di;\n \n /// Levels to go up the vocabulary tree to select nodes to store\n /// in the direct index\n int m_dilevels;\n \n /// Inverted file (must have size() == |words|)\n InvertedFile m_ifile;\n\n \n /// Direct file (resized for allocation)\n DirectFile m_dfile;\n \n std::vector m_dBowfile;\n\n /// Number of valid entries in m_dfile\n int m_nentries;\n \n};\n\n// --------------------------------------------------------------------------\n\ntemplate\nTemplatedDatabase::TemplatedDatabase\n (bool use_di, int di_levels)\n : m_voc(NULL), m_use_di(use_di), m_dilevels(di_levels), m_nentries(0)\n{\n}\n\n// --------------------------------------------------------------------------\n\ntemplate\ntemplate\nTemplatedDatabase::TemplatedDatabase\n (const T &voc, bool use_di, int di_levels)\n : m_voc(NULL), m_use_di(use_di), m_dilevels(di_levels)\n{\n setVocabulary(voc);\n clear();\n}\n\n// --------------------------------------------------------------------------\n\ntemplate\nTemplatedDatabase::TemplatedDatabase\n (const TemplatedDatabase &db)\n : m_voc(NULL)\n{\n *this = db;\n}\n\n// --------------------------------------------------------------------------\n\ntemplate\nTemplatedDatabase::TemplatedDatabase\n (const std::string &filename)\n : m_voc(NULL)\n{\n load(filename);\n}\n\n// --------------------------------------------------------------------------\n\ntemplate\nTemplatedDatabase::TemplatedDatabase\n (const char *filename)\n : m_voc(NULL)\n{\n load(filename);\n}\n\n// --------------------------------------------------------------------------\n\ntemplate\nTemplatedDatabase::~TemplatedDatabase(void)\n{\n delete m_voc;\n}\n\n// --------------------------------------------------------------------------\n\ntemplate\nTemplatedDatabase& TemplatedDatabase::operator=\n (const TemplatedDatabase &db)\n{\n if(this != &db)\n {\n m_dfile = db.m_dfile;\n m_dBowfile = db.m_dBowfile;\n m_dilevels = db.m_dilevels;\n m_ifile = db.m_ifile;\n m_nentries = db.m_nentries;\n m_use_di = db.m_use_di;\n setVocabulary(*db.m_voc);\n }\n return *this;\n}\n\n// --------------------------------------------------------------------------\n\ntemplate\nEntryId TemplatedDatabase::add(\n const std::vector &features,\n BowVector *bowvec, FeatureVector *fvec)\n{\n BowVector aux;\n BowVector& v = (bowvec ? *bowvec : aux);\n \n if(m_use_di && fvec != NULL)\n {\n m_voc->transform(features, v, *fvec, m_dilevels); // with features\n return add(v, *fvec);\n }\n else if(m_use_di)\n {\n FeatureVector fv;\n m_voc->transform(features, v, fv, m_dilevels); // with features\n return add(v, fv);\n }\n else if(fvec != NULL)\n {\n m_voc->transform(features, v, *fvec, m_dilevels); // with features\n return add(v);\n }\n else\n {\n m_voc->transform(features, v); // with features\n return add(v);\n }\n}\n\n// ---------------------------------------------------------------------------\n\ntemplate\nEntryId TemplatedDatabase::add(const BowVector &v,\n const FeatureVector &fv)\n{\n EntryId entry_id = m_nentries++;\n\n BowVector::const_iterator vit;\n std::vector::const_iterator iit;\n\n if(m_use_di)\n {\n // update direct file\n if(entry_id == m_dfile.size())\n {\n m_dfile.push_back(fv);\n m_dBowfile.push_back(v);\n }\n else\n {\n m_dfile[entry_id] = fv;\n m_dBowfile[entry_id] = v;\n }\n }\n \n // update inverted file\n for(vit = v.begin(); vit != v.end(); ++vit)\n {\n const WordId& word_id = vit->first;\n const WordValue& word_weight = vit->second;\n \n IFRow& ifrow = m_ifile[word_id];\n ifrow.push_back(IFPair(entry_id, word_weight));\n }\n \n return entry_id;\n}\n\n// ---------------------------------------------------------------------------\n\ntemplate\nvoid TemplatedDatabase::delete_entry(const EntryId entry_id)\n{\n BowVector v = m_dBowfile[entry_id];\n\n BowVector::const_iterator vit;\n\n for (vit = v.begin(); vit != v.end(); ++vit)\n {\n const WordId& word_id = vit->first;\n IFRow& ifrow = m_ifile[word_id];\n typename IFRow::iterator rit;\n for (rit = ifrow.begin(); rit != ifrow.end(); ++rit)\n {\n if (rit->entry_id == entry_id)\n {\n ifrow.erase(rit);\n break;\n }\n }\n }\n m_dBowfile[entry_id].clear();\n m_dfile[entry_id].clear();\n}\n\n\n// --------------------------------------------------------------------------\n\ntemplate\ntemplate\ninline void TemplatedDatabase::setVocabulary\n (const T& voc)\n{\n delete m_voc;\n m_voc = new T(voc);\n clear();\n}\n\n// --------------------------------------------------------------------------\n\ntemplate\ntemplate\ninline void TemplatedDatabase::setVocabulary\n (const T& voc, bool use_di, int di_levels)\n{\n m_use_di = use_di;\n m_dilevels = di_levels;\n delete m_voc;\n m_voc = new T(voc);\n clear();\n}\n\n// --------------------------------------------------------------------------\n\ntemplate\ninline const TemplatedVocabulary* \nTemplatedDatabase::getVocabulary() const\n{\n return m_voc;\n}\n\n// --------------------------------------------------------------------------\n\ntemplate\ninline void TemplatedDatabase::clear()\n{\n // resize vectors\n m_ifile.resize(0);\n m_ifile.resize(m_voc->size());\n m_dfile.resize(0);\n m_dBowfile.resize(0);\n m_nentries = 0;\n}\n\n// --------------------------------------------------------------------------\n\ntemplate\nvoid TemplatedDatabase::allocate(int nd, int ni)\n{\n // m_ifile already contains |words| items\n if(ni > 0)\n {\n typename std::vector::iterator rit;\n for(rit = m_ifile.begin(); rit != m_ifile.end(); ++rit)\n {\n int n = (int)rit->size();\n if(ni > n)\n {\n rit->resize(ni);\n rit->resize(n);\n }\n }\n }\n \n if(m_use_di && (int)m_dfile.size() < nd)\n {\n m_dfile.resize(nd);\n m_dBowfile.resize(nd);\n }\n}\n\n// --------------------------------------------------------------------------\n\ntemplate\ninline unsigned int TemplatedDatabase::size() const\n{\n return m_nentries;\n}\n\n// --------------------------------------------------------------------------\n\ntemplate\ninline bool TemplatedDatabase::usingDirectIndex() const\n{\n return m_use_di;\n}\n\n// --------------------------------------------------------------------------\n\ntemplate\ninline int TemplatedDatabase::getDirectIndexLevels() const\n{\n return m_dilevels;\n}\n\n// --------------------------------------------------------------------------\n\ntemplate\nvoid TemplatedDatabase::query(\n const std::vector &features,\n QueryResults &ret, int max_results, int max_id) const\n{\n BowVector vec;\n m_voc->transform(features, vec);\n query(vec, ret, max_results, max_id);\n}\n\n// --------------------------------------------------------------------------\n\ntemplate\nvoid TemplatedDatabase::query(\n const BowVector &vec, \n QueryResults &ret, int max_results, int max_id) const\n{\n ret.resize(0);\n \n switch(m_voc->getScoringType())\n {\n case L1_NORM:\n queryL1(vec, ret, max_results, max_id);\n break;\n \n case L2_NORM:\n queryL2(vec, ret, max_results, max_id);\n break;\n \n case CHI_SQUARE:\n queryChiSquare(vec, ret, max_results, max_id);\n break;\n \n case KL:\n queryKL(vec, ret, max_results, max_id);\n break;\n \n case BHATTACHARYYA:\n queryBhattacharyya(vec, ret, max_results, max_id);\n break;\n \n case DOT_PRODUCT:\n queryDotProduct(vec, ret, max_results, max_id);\n break;\n }\n}\n\n// --------------------------------------------------------------------------\n\ntemplate\nvoid TemplatedDatabase::queryL1(const BowVector &vec, \n QueryResults &ret, int max_results, int max_id) const\n{\n BowVector::const_iterator vit;\n typename IFRow::const_iterator rit;\n \n std::map pairs;\n std::map::iterator pit;\n \n for(vit = vec.begin(); vit != vec.end(); ++vit)\n {\n const WordId word_id = vit->first;\n const WordValue& qvalue = vit->second;\n \n const IFRow& row = m_ifile[word_id];\n \n // IFRows are sorted in ascending entry_id order\n \n for(rit = row.begin(); rit != row.end(); ++rit)\n {\n const EntryId entry_id = rit->entry_id;\n const WordValue& dvalue = rit->word_weight;\n \n if((int)entry_id < max_id || max_id == -1 || (int)entry_id == m_nentries - 1)\n {\n double value = fabs(qvalue - dvalue) - fabs(qvalue) - fabs(dvalue);\n \n pit = pairs.lower_bound(entry_id);\n if(pit != pairs.end() && !(pairs.key_comp()(entry_id, pit->first)))\n {\n pit->second += value;\n }\n else\n {\n pairs.insert(pit, \n std::map::value_type(entry_id, value));\n }\n }\n \n } // for each inverted row\n } // for each query word\n\t\n // move to vector\n ret.reserve(pairs.size());\n for(pit = pairs.begin(); pit != pairs.end(); ++pit)\n {\n ret.push_back(Result(pit->first, pit->second));\n }\n\t\n // resulting \"scores\" are now in [-2 best .. 0 worst]\t\n \n // sort vector in ascending order of score\n std::sort(ret.begin(), ret.end());\n // (ret is inverted now --the lower the better--)\n\n // cut vector\n if(max_results > 0 && (int)ret.size() > max_results)\n ret.resize(max_results);\n \n // complete and scale score to [0 worst .. 1 best]\n // ||v - w||_{L1} = 2 + Sum(|v_i - w_i| - |v_i| - |w_i|) \n //\t\tfor all i | v_i != 0 and w_i != 0 \n // (Nister, 2006)\n // scaled_||v - w||_{L1} = 1 - 0.5 * ||v - w||_{L1}\n QueryResults::iterator qit;\n for(qit = ret.begin(); qit != ret.end(); qit++) \n qit->Score = -qit->Score/2.0;\n}\n\n// --------------------------------------------------------------------------\n\ntemplate\nvoid TemplatedDatabase::queryL2(const BowVector &vec, \n QueryResults &ret, int max_results, int max_id) const\n{\n BowVector::const_iterator vit;\n typename IFRow::const_iterator rit;\n \n std::map pairs;\n std::map::iterator pit;\n \n //map counters;\n //map::iterator cit;\n \n for(vit = vec.begin(); vit != vec.end(); ++vit)\n {\n const WordId word_id = vit->first;\n const WordValue& qvalue = vit->second;\n \n const IFRow& row = m_ifile[word_id];\n \n // IFRows are sorted in ascending entry_id order\n \n for(rit = row.begin(); rit != row.end(); ++rit)\n {\n const EntryId entry_id = rit->entry_id;\n const WordValue& dvalue = rit->word_weight;\n \n if((int)entry_id < max_id || max_id == -1)\n {\n double value = - qvalue * dvalue; // minus sign for sorting trick\n \n pit = pairs.lower_bound(entry_id);\n //cit = counters.lower_bound(entry_id);\n if(pit != pairs.end() && !(pairs.key_comp()(entry_id, pit->first)))\n {\n pit->second += value; \n //cit->second += 1;\n }\n else\n {\n pairs.insert(pit, \n std::map::value_type(entry_id, value));\n \n //counters.insert(cit, \n // map::value_type(entry_id, 1));\n }\n }\n \n } // for each inverted row\n } // for each query word\n\t\n // move to vector\n ret.reserve(pairs.size());\n //cit = counters.begin();\n for(pit = pairs.begin(); pit != pairs.end(); ++pit)//, ++cit)\n {\n ret.push_back(Result(pit->first, pit->second));// / cit->second));\n }\n\t\n // resulting \"scores\" are now in [-1 best .. 0 worst]\t\n \n // sort vector in ascending order of score\n std::sort(ret.begin(), ret.end());\n // (ret is inverted now --the lower the better--)\n\n // cut vector\n if(max_results > 0 && (int)ret.size() > max_results)\n ret.resize(max_results);\n\n // complete and scale score to [0 worst .. 1 best]\n // ||v - w||_{L2} = sqrt( 2 - 2 * Sum(v_i * w_i) \n\t//\t\tfor all i | v_i != 0 and w_i != 0 )\n\t// (Nister, 2006)\n\tQueryResults::iterator qit;\n for(qit = ret.begin(); qit != ret.end(); qit++) \n {\n if(qit->Score <= -1.0) // rounding error\n qit->Score = 1.0;\n else\n qit->Score = 1.0 - sqrt(1.0 + qit->Score); // [0..1]\n // the + sign is ok, it is due to - sign in \n // value = - qvalue * dvalue\n }\n \n}\n\n// --------------------------------------------------------------------------\n\ntemplate\nvoid TemplatedDatabase::queryChiSquare(const BowVector &vec, \n QueryResults &ret, int max_results, int max_id) const\n{\n BowVector::const_iterator vit;\n typename IFRow::const_iterator rit;\n \n std::map > pairs;\n std::map >::iterator pit;\n \n std::map > sums; // < sum vi, sum wi >\n std::map >::iterator sit;\n \n // In the current implementation, we suppose vec is not normalized\n \n //map expected;\n //map::iterator eit;\n \n for(vit = vec.begin(); vit != vec.end(); ++vit)\n {\n const WordId word_id = vit->first;\n const WordValue& qvalue = vit->second;\n \n const IFRow& row = m_ifile[word_id];\n \n // IFRows are sorted in ascending entry_id order\n \n for(rit = row.begin(); rit != row.end(); ++rit)\n {\n const EntryId entry_id = rit->entry_id;\n const WordValue& dvalue = rit->word_weight;\n \n if((int)entry_id < max_id || max_id == -1)\n {\n // (v-w)^2/(v+w) - v - w = -4 vw/(v+w)\n // we move the 4 out\n double value = 0;\n if(qvalue + dvalue != 0.0) // words may have weight zero\n value = - qvalue * dvalue / (qvalue + dvalue);\n \n pit = pairs.lower_bound(entry_id);\n sit = sums.lower_bound(entry_id);\n //eit = expected.lower_bound(entry_id);\n if(pit != pairs.end() && !(pairs.key_comp()(entry_id, pit->first)))\n {\n pit->second.first += value;\n pit->second.second += 1;\n //eit->second += dvalue;\n sit->second.first += qvalue;\n sit->second.second += dvalue;\n }\n else\n {\n pairs.insert(pit, \n std::map >::value_type(entry_id,\n std::make_pair(value, 1) ));\n //expected.insert(eit, \n // map::value_type(entry_id, dvalue));\n \n sums.insert(sit, \n std::map >::value_type(entry_id,\n std::make_pair(qvalue, dvalue) ));\n }\n }\n \n } // for each inverted row\n } // for each query word\n\t\n // move to vector\n ret.reserve(pairs.size());\n sit = sums.begin();\n for(pit = pairs.begin(); pit != pairs.end(); ++pit, ++sit)\n {\n if(pit->second.second >= MIN_COMMON_WORDS)\n {\n ret.push_back(Result(pit->first, pit->second.first));\n ret.back().nWords = pit->second.second;\n ret.back().sumCommonVi = sit->second.first;\n ret.back().sumCommonWi = sit->second.second;\n ret.back().expectedChiScore = \n 2 * sit->second.second / (1 + sit->second.second);\n }\n \n //ret.push_back(Result(pit->first, pit->second));\n }\n\t\n // resulting \"scores\" are now in [-2 best .. 0 worst]\t\n // we have to add +2 to the scores to obtain the chi square score\n \n // sort vector in ascending order of score\n std::sort(ret.begin(), ret.end());\n // (ret is inverted now --the lower the better--)\n\n // cut vector\n if(max_results > 0 && (int)ret.size() > max_results)\n ret.resize(max_results);\n\n // complete and scale score to [0 worst .. 1 best]\n QueryResults::iterator qit;\n for(qit = ret.begin(); qit != ret.end(); qit++)\n {\n // this takes the 4 into account\n qit->Score = - 2. * qit->Score; // [0..1]\n \n qit->chiScore = qit->Score;\n }\n \n}\n\n// --------------------------------------------------------------------------\n\ntemplate\nvoid TemplatedDatabase::queryKL(const BowVector &vec, \n QueryResults &ret, int max_results, int max_id) const\n{\n BowVector::const_iterator vit;\n typename IFRow::const_iterator rit;\n \n std::map pairs;\n std::map::iterator pit;\n \n for(vit = vec.begin(); vit != vec.end(); ++vit)\n {\n const WordId word_id = vit->first;\n const WordValue& vi = vit->second;\n \n const IFRow& row = m_ifile[word_id];\n \n // IFRows are sorted in ascending entry_id order\n \n for(rit = row.begin(); rit != row.end(); ++rit)\n { \n const EntryId entry_id = rit->entry_id;\n const WordValue& wi = rit->word_weight;\n \n if((int)entry_id < max_id || max_id == -1)\n {\n double value = 0;\n if(vi != 0 && wi != 0) value = vi * log(vi/wi);\n \n pit = pairs.lower_bound(entry_id);\n if(pit != pairs.end() && !(pairs.key_comp()(entry_id, pit->first)))\n {\n pit->second += value;\n }\n else\n {\n pairs.insert(pit, \n std::map::value_type(entry_id, value));\n }\n }\n \n } // for each inverted row\n } // for each query word\n\t\n // resulting \"scores\" are now in [-X worst .. 0 best .. X worst]\n // but we cannot make sure which ones are better without calculating\n // the complete score\n\n // complete scores and move to vector\n ret.reserve(pairs.size());\n for(pit = pairs.begin(); pit != pairs.end(); ++pit)\n {\n EntryId eid = pit->first;\n double value = 0.0;\n\n for(vit = vec.begin(); vit != vec.end(); ++vit)\n {\n const WordValue &vi = vit->second;\n const IFRow& row = m_ifile[vit->first];\n\n if(vi != 0)\n {\n if(row.end() == find(row.begin(), row.end(), eid ))\n {\n value += vi * (log(vi) - GeneralScoring::LOG_EPS);\n }\n }\n }\n \n pit->second += value;\n \n // to vector\n ret.push_back(Result(pit->first, pit->second));\n }\n \n // real scores are now in [0 best .. X worst]\n\n // sort vector in ascending order\n // (scores are inverted now --the lower the better--)\n std::sort(ret.begin(), ret.end());\n\n // cut vector\n if(max_results > 0 && (int)ret.size() > max_results)\n ret.resize(max_results);\n\n // cannot scale scores\n \n}\n\n// --------------------------------------------------------------------------\n\ntemplate\nvoid TemplatedDatabase::queryBhattacharyya(\n const BowVector &vec, QueryResults &ret, int max_results, int max_id) const\n{\n BowVector::const_iterator vit;\n typename IFRow::const_iterator rit;\n \n //map pairs;\n //map::iterator pit;\n \n std::map > pairs; // >\n std::map >::iterator pit;\n \n for(vit = vec.begin(); vit != vec.end(); ++vit)\n {\n const WordId word_id = vit->first;\n const WordValue& qvalue = vit->second;\n \n const IFRow& row = m_ifile[word_id];\n \n // IFRows are sorted in ascending entry_id order\n \n for(rit = row.begin(); rit != row.end(); ++rit)\n {\n const EntryId entry_id = rit->entry_id;\n const WordValue& dvalue = rit->word_weight;\n \n if((int)entry_id < max_id || max_id == -1)\n {\n double value = sqrt(qvalue * dvalue);\n \n pit = pairs.lower_bound(entry_id);\n if(pit != pairs.end() && !(pairs.key_comp()(entry_id, pit->first)))\n {\n pit->second.first += value;\n pit->second.second += 1;\n }\n else\n {\n pairs.insert(pit, \n std::map >::value_type(entry_id,\n std::make_pair(value, 1)));\n }\n }\n \n } // for each inverted row\n } // for each query word\n\t\n // move to vector\n ret.reserve(pairs.size());\n for(pit = pairs.begin(); pit != pairs.end(); ++pit)\n {\n if(pit->second.second >= MIN_COMMON_WORDS)\n {\n ret.push_back(Result(pit->first, pit->second.first));\n ret.back().nWords = pit->second.second;\n ret.back().bhatScore = pit->second.first;\n }\n }\n\t\n // scores are already in [0..1]\n\n // sort vector in descending order\n std::sort(ret.begin(), ret.end(), Result::gt);\n\n // cut vector\n if(max_results > 0 && (int)ret.size() > max_results)\n ret.resize(max_results);\n\n}\n\n// ---------------------------------------------------------------------------\n\ntemplate\nvoid TemplatedDatabase::queryDotProduct(\n const BowVector &vec, QueryResults &ret, int max_results, int max_id) const\n{\n BowVector::const_iterator vit;\n typename IFRow::const_iterator rit;\n \n std::map pairs;\n std::map::iterator pit;\n \n for(vit = vec.begin(); vit != vec.end(); ++vit)\n {\n const WordId word_id = vit->first;\n const WordValue& qvalue = vit->second;\n \n const IFRow& row = m_ifile[word_id];\n \n // IFRows are sorted in ascending entry_id order\n \n for(rit = row.begin(); rit != row.end(); ++rit)\n {\n const EntryId entry_id = rit->entry_id;\n const WordValue& dvalue = rit->word_weight;\n \n if((int)entry_id < max_id || max_id == -1)\n {\n double value; \n if(this->m_voc->getWeightingType() == BINARY)\n value = 1;\n else\n value = qvalue * dvalue;\n \n pit = pairs.lower_bound(entry_id);\n if(pit != pairs.end() && !(pairs.key_comp()(entry_id, pit->first)))\n {\n pit->second += value;\n }\n else\n {\n pairs.insert(pit, \n std::map::value_type(entry_id, value));\n }\n }\n \n } // for each inverted row\n } // for each query word\n\t\n // move to vector\n ret.reserve(pairs.size());\n for(pit = pairs.begin(); pit != pairs.end(); ++pit)\n {\n ret.push_back(Result(pit->first, pit->second));\n }\n\t\n // scores are the greater the better\n\n // sort vector in descending order\n std::sort(ret.begin(), ret.end(), Result::gt);\n\n // cut vector\n if(max_results > 0 && (int)ret.size() > max_results)\n ret.resize(max_results);\n\n // these scores cannot be scaled\n}\n\n// ---------------------------------------------------------------------------\n\ntemplate\nconst FeatureVector& TemplatedDatabase::retrieveFeatures\n (EntryId id) const\n{\n assert(id < size());\n return m_dfile[id];\n}\n\n// --------------------------------------------------------------------------\n\ntemplate\nvoid TemplatedDatabase::save(const std::string &filename) const\n{\n cv::FileStorage fs(filename.c_str(), cv::FileStorage::WRITE);\n if(!fs.isOpened()) throw std::string(\"Could not open file \") + filename;\n \n save(fs);\n}\n\n// --------------------------------------------------------------------------\n\ntemplate\nvoid TemplatedDatabase::save(cv::FileStorage &fs,\n const std::string &name) const\n{\n // Format YAML:\n // vocabulary { ... see TemplatedVocabulary::save }\n // database \n // {\n // nEntries: \n // usingDI: \n // diLevels: \n // invertedIndex\n // [\n // [\n // { \n // imageId: \n // weight: \n // }\n // ]\n // ]\n // directIndex\n // [\n // [\n // {\n // nodeId:\n // features: [ ]\n // }\n // ]\n // ]\n\n // invertedIndex[i] is for the i-th word\n // directIndex[i] is for the i-th entry\n // directIndex may be empty if not using direct index\n //\n // imageId's and nodeId's must be stored in ascending order\n // (according to the construction of the indexes)\n\n m_voc->save(fs);\n \n fs << name << \"{\";\n \n fs << \"nEntries\" << m_nentries;\n fs << \"usingDI\" << (m_use_di ? 1 : 0);\n fs << \"diLevels\" << m_dilevels;\n \n fs << \"invertedIndex\" << \"[\";\n \n typename InvertedFile::const_iterator iit;\n typename IFRow::const_iterator irit;\n for(iit = m_ifile.begin(); iit != m_ifile.end(); ++iit)\n {\n fs << \"[\"; // word of IF\n for(irit = iit->begin(); irit != iit->end(); ++irit)\n {\n fs << \"{:\" \n << \"imageId\" << (int)irit->entry_id\n << \"weight\" << irit->word_weight\n << \"}\";\n }\n fs << \"]\"; // word of IF\n }\n \n fs << \"]\"; // invertedIndex\n \n fs << \"directIndex\" << \"[\";\n \n typename DirectFile::const_iterator dit;\n typename FeatureVector::const_iterator drit;\n for(dit = m_dfile.begin(); dit != m_dfile.end(); ++dit)\n {\n fs << \"[\"; // entry of DF\n \n for(drit = dit->begin(); drit != dit->end(); ++drit)\n {\n NodeId nid = drit->first;\n const std::vector& features = drit->second;\n \n // save info of last_nid\n fs << \"{\";\n fs << \"nodeId\" << (int)nid;\n // msvc++ 2010 with opencv 2.3.1 does not allow FileStorage::operator<<\n // with vectors of unsigned int\n fs << \"features\" << \"[\" \n << *(const std::vector*)(&features) << \"]\";\n fs << \"}\";\n }\n \n fs << \"]\"; // entry of DF\n }\n \n fs << \"]\"; // directIndex\n \n fs << \"}\"; // database\n}\n\n// --------------------------------------------------------------------------\n\ntemplate\nvoid TemplatedDatabase::load(const std::string &filename)\n{\n cv::FileStorage fs(filename.c_str(), cv::FileStorage::READ);\n if(!fs.isOpened()) throw std::string(\"Could not open file \") + filename;\n \n load(fs);\n}\n\n// --------------------------------------------------------------------------\n\ntemplate\nvoid TemplatedDatabase::load(const cv::FileStorage &fs,\n const std::string &name)\n{ \n // load voc first\n // subclasses must instantiate m_voc before calling this ::load\n if(!m_voc) m_voc = new TemplatedVocabulary;\n \n m_voc->load(fs);\n\n // load database now\n clear(); // resizes inverted file \n \n cv::FileNode fdb = fs[name];\n \n m_nentries = (int)fdb[\"nEntries\"]; \n m_use_di = (int)fdb[\"usingDI\"] != 0;\n m_dilevels = (int)fdb[\"diLevels\"];\n \n cv::FileNode fn = fdb[\"invertedIndex\"];\n for(WordId wid = 0; wid < fn.size(); ++wid)\n {\n cv::FileNode fw = fn[wid];\n \n for(unsigned int i = 0; i < fw.size(); ++i)\n {\n EntryId eid = (int)fw[i][\"imageId\"];\n WordValue v = fw[i][\"weight\"];\n \n m_ifile[wid].push_back(IFPair(eid, v));\n }\n }\n \n if(m_use_di)\n {\n fn = fdb[\"directIndex\"];\n \n m_dfile.resize(fn.size());\n m_dBowfile.resize(fn.size());\n assert(m_nentries == (int)fn.size());\n \n FeatureVector::iterator dit;\n for(EntryId eid = 0; eid < fn.size(); ++eid)\n {\n cv::FileNode fe = fn[eid];\n \n m_dfile[eid].clear();\n m_dBowfile[eid].clear();\n for(unsigned int i = 0; i < fe.size(); ++i)\n {\n NodeId nid = (int)fe[i][\"nodeId\"];\n \n dit = m_dfile[eid].insert(m_dfile[eid].end(), \n make_pair(nid, std::vector() ));\n \n // this failed to compile with some opencv versions (2.3.1)\n //fe[i][\"features\"] >> dit->second;\n \n // this was ok until OpenCV 2.4.1\n //std::vector aux;\n //fe[i][\"features\"] >> aux; // OpenCV < 2.4.1\n //dit->second.resize(aux.size());\n //std::copy(aux.begin(), aux.end(), dit->second.begin());\n \n cv::FileNode ff = fe[i][\"features\"][0];\n dit->second.reserve(ff.size());\n \n cv::FileNodeIterator ffit;\n for(ffit = ff.begin(); ffit != ff.end(); ++ffit)\n {\n dit->second.push_back((int)*ffit); \n }\n }\n } // for each entry\n } // if use_id\n \n}\n\n// --------------------------------------------------------------------------\n\n/**\n * Writes printable information of the database\n * @param os stream to write to\n * @param db\n */\ntemplate\nstd::ostream& operator<<(std::ostream &os, \n const TemplatedDatabase &db)\n{\n os << \"Database: Entries = \" << db.size() << \", \"\n \"Using direct index = \" << (db.usingDirectIndex() ? \"yes\" : \"no\");\n \n if(db.usingDirectIndex())\n os << \", Direct index levels = \" << db.getDirectIndexLevels();\n \n os << \". \" << *db.getVocabulary();\n return os;\n}\n\n// --------------------------------------------------------------------------\n\n} // namespace DBoW2\n\n#endif\n" }, { "path": "pose_graph/src/ThirdParty/DBoW/TemplatedVocabulary.h", "content": "/**\n * File: TemplatedVocabulary.h\n * Date: February 2011\n * Author: Dorian Galvez-Lopez\n * Description: templated vocabulary \n * License: see the LICENSE.txt file\n *\n */\n\n#ifndef __D_T_TEMPLATED_VOCABULARY__\n#define __D_T_TEMPLATED_VOCABULARY__\n\n#include \n\n#include \n#include \n#include \n#include \n#include \n#include \n\n#include \"FeatureVector.h\"\n#include \"BowVector.h\"\n#include \"ScoringObject.h\"\n\n#include \"../DUtils/DUtils.h\"\n\n// Added by VINS [[[\n#include \"../VocabularyBinary.hpp\"\n#include \n// Added by VINS ]]]\n\nnamespace DBoW2 {\n\n/// @param TDescriptor class of descriptor\n/// @param F class of descriptor functions\ntemplate\n/// Generic Vocabulary\nclass TemplatedVocabulary\n{\t\t\npublic:\n \n /**\n * Initiates an empty vocabulary\n * @param k branching factor\n * @param L depth levels\n * @param weighting weighting type\n * @param scoring scoring type\n */\n TemplatedVocabulary(int k = 10, int L = 5, \n WeightingType weighting = TF_IDF, ScoringType scoring = L1_NORM);\n \n /**\n * Creates the vocabulary by loading a file\n * @param filename\n */\n TemplatedVocabulary(const std::string &filename);\n \n /**\n * Creates the vocabulary by loading a file\n * @param filename\n */\n TemplatedVocabulary(const char *filename);\n \n /** \n * Copy constructor\n * @param voc\n */\n TemplatedVocabulary(const TemplatedVocabulary &voc);\n \n /**\n * Destructor\n */\n virtual ~TemplatedVocabulary();\n \n /** \n * Assigns the given vocabulary to this by copying its data and removing\n * all the data contained by this vocabulary before\n * @param voc\n * @return reference to this vocabulary\n */\n TemplatedVocabulary& operator=(\n const TemplatedVocabulary &voc);\n \n /** \n * Creates a vocabulary from the training features with the already\n * defined parameters\n * @param training_features\n */\n virtual void create\n (const std::vector > &training_features);\n \n /**\n * Creates a vocabulary from the training features, setting the branching\n * factor and the depth levels of the tree\n * @param training_features\n * @param k branching factor\n * @param L depth levels\n */\n virtual void create\n (const std::vector > &training_features, \n int k, int L);\n\n /**\n * Creates a vocabulary from the training features, setting the branching\n * factor nad the depth levels of the tree, and the weighting and scoring\n * schemes\n */\n virtual void create\n (const std::vector > &training_features,\n int k, int L, WeightingType weighting, ScoringType scoring);\n\n /**\n * Returns the number of words in the vocabulary\n * @return number of words\n */\n virtual inline unsigned int size() const;\n \n /**\n * Returns whether the vocabulary is empty (i.e. it has not been trained)\n * @return true iff the vocabulary is empty\n */\n virtual inline bool empty() const;\n\n /**\n * Transforms a set of descriptores into a bow vector\n * @param features\n * @param v (out) bow vector of weighted words\n */\n virtual void transform(const std::vector& features, BowVector &v) \n const;\n \n /**\n * Transform a set of descriptors into a bow vector and a feature vector\n * @param features\n * @param v (out) bow vector\n * @param fv (out) feature vector of nodes and feature indexes\n * @param levelsup levels to go up the vocabulary tree to get the node index\n */\n virtual void transform(const std::vector& features,\n BowVector &v, FeatureVector &fv, int levelsup) const;\n\n /**\n * Transforms a single feature into a word (without weight)\n * @param feature\n * @return word id\n */\n virtual WordId transform(const TDescriptor& feature) const;\n \n /**\n * Returns the score of two vectors\n * @param a vector\n * @param b vector\n * @return score between vectors\n * @note the vectors must be already sorted and normalized if necessary\n */\n inline double score(const BowVector &a, const BowVector &b) const;\n \n /**\n * Returns the id of the node that is \"levelsup\" levels from the word given\n * @param wid word id\n * @param levelsup 0..L\n * @return node id. if levelsup is 0, returns the node id associated to the\n * word id\n */\n virtual NodeId getParentNode(WordId wid, int levelsup) const;\n \n /**\n * Returns the ids of all the words that are under the given node id,\n * by traversing any of the branches that goes down from the node\n * @param nid starting node id\n * @param words ids of words\n */\n void getWordsFromNode(NodeId nid, std::vector &words) const;\n \n /**\n * Returns the branching factor of the tree (k)\n * @return k\n */\n inline int getBranchingFactor() const { return m_k; }\n \n /** \n * Returns the depth levels of the tree (L)\n * @return L\n */\n inline int getDepthLevels() const { return m_L; }\n \n /**\n * Returns the real depth levels of the tree on average\n * @return average of depth levels of leaves\n */\n float getEffectiveLevels() const;\n \n /**\n * Returns the descriptor of a word\n * @param wid word id\n * @return descriptor\n */\n virtual inline TDescriptor getWord(WordId wid) const;\n \n /**\n * Returns the weight of a word\n * @param wid word id\n * @return weight\n */\n virtual inline WordValue getWordWeight(WordId wid) const;\n \n /** \n * Returns the weighting method\n * @return weighting method\n */\n inline WeightingType getWeightingType() const { return m_weighting; }\n \n /** \n * Returns the scoring method\n * @return scoring method\n */\n inline ScoringType getScoringType() const { return m_scoring; }\n \n /**\n * Changes the weighting method\n * @param type new weighting type\n */\n inline void setWeightingType(WeightingType type);\n \n /**\n * Changes the scoring method\n * @param type new scoring type\n */\n void setScoringType(ScoringType type);\n \n /**\n * Saves the vocabulary into a file\n * @param filename\n */\n void save(const std::string &filename) const;\n \n /**\n * Loads the vocabulary from a file\n * @param filename\n */\n void load(const std::string &filename);\n \n /** \n * Saves the vocabulary to a file storage structure\n * @param fn node in file storage\n */\n virtual void save(cv::FileStorage &fs, \n const std::string &name = \"vocabulary\") const;\n \n /**\n * Loads the vocabulary from a file storage node\n * @param fn first node\n * @param subname name of the child node of fn where the tree is stored.\n * If not given, the fn node is used instead\n */ \n virtual void load(const cv::FileStorage &fs, \n const std::string &name = \"vocabulary\");\n \n // Added by VINS [[[\n virtual void loadBin(const std::string &filename);\n // Added by VINS ]]]\n \n /** \n * Stops those words whose weight is below minWeight.\n * Words are stopped by setting their weight to 0. There are not returned\n * later when transforming image features into vectors.\n * Note that when using IDF or TF_IDF, the weight is the idf part, which\n * is equivalent to -log(f), where f is the frequency of the word\n * (f = Ni/N, Ni: number of training images where the word is present, \n * N: number of training images).\n * Note that the old weight is forgotten, and subsequent calls to this \n * function with a lower minWeight have no effect.\n * @return number of words stopped now\n */\n virtual int stopWords(double minWeight);\n\nprotected:\n\n /// Pointer to descriptor\n typedef const TDescriptor *pDescriptor;\n\n /// Tree node\n struct Node \n {\n /// Node id\n NodeId id;\n /// Weight if the node is a word\n WordValue weight;\n /// Children \n std::vector children;\n /// Parent node (undefined in case of root)\n NodeId parent;\n /// Node descriptor\n TDescriptor descriptor;\n\n /// Word id if the node is a word\n WordId word_id;\n\n /**\n * Empty constructor\n */\n Node(): id(0), weight(0), parent(0), word_id(0){}\n \n /**\n * Constructor\n * @param _id node id\n */\n Node(NodeId _id): id(_id), weight(0), parent(0), word_id(0){}\n\n /**\n * Returns whether the node is a leaf node\n * @return true iff the node is a leaf\n */\n inline bool isLeaf() const { return children.empty(); }\n };\n\nprotected:\n\n /**\n * Creates an instance of the scoring object accoring to m_scoring\n */\n void createScoringObject();\n\n /** \n * Returns a set of pointers to descriptores\n * @param training_features all the features\n * @param features (out) pointers to the training features\n */\n void getFeatures(\n const std::vector > &training_features,\n std::vector &features) const;\n\n /**\n * Returns the word id associated to a feature\n * @param feature\n * @param id (out) word id\n * @param weight (out) word weight\n * @param nid (out) if given, id of the node \"levelsup\" levels up\n * @param levelsup\n */\n virtual void transform(const TDescriptor &feature, \n WordId &id, WordValue &weight, NodeId* nid = NULL, int levelsup = 0) const;\n\n /**\n * Returns the word id associated to a feature\n * @param feature\n * @param id (out) word id\n */\n virtual void transform(const TDescriptor &feature, WordId &id) const;\n \n /**\n * Creates a level in the tree, under the parent, by running kmeans with\n * a descriptor set, and recursively creates the subsequent levels too\n * @param parent_id id of parent node\n * @param descriptors descriptors to run the kmeans on\n * @param current_level current level in the tree\n */\n void HKmeansStep(NodeId parent_id, const std::vector &descriptors,\n int current_level);\n\n /**\n * Creates k clusters from the given descriptors with some seeding algorithm.\n * @note In this class, kmeans++ is used, but this function should be\n * overriden by inherited classes.\n */\n virtual void initiateClusters(const std::vector &descriptors,\n std::vector &clusters) const;\n \n /**\n * Creates k clusters from the given descriptor sets by running the\n * initial step of kmeans++\n * @param descriptors \n * @param clusters resulting clusters\n */\n void initiateClustersKMpp(const std::vector &descriptors,\n std::vector &clusters) const;\n \n /**\n * Create the words of the vocabulary once the tree has been built\n */\n void createWords();\n \n /**\n * Sets the weights of the nodes of tree according to the given features.\n * Before calling this function, the nodes and the words must be already\n * created (by calling HKmeansStep and createWords)\n * @param features\n */\n void setNodeWeights(const std::vector > &features);\n \nprotected:\n\n /// Branching factor\n int m_k;\n \n /// Depth levels \n int m_L;\n \n /// Weighting method\n WeightingType m_weighting;\n \n /// Scoring method\n ScoringType m_scoring;\n \n /// Object for computing scores\n GeneralScoring* m_scoring_object;\n \n /// Tree nodes\n std::vector m_nodes;\n \n /// Words of the vocabulary (tree leaves)\n /// this condition holds: m_words[wid]->word_id == wid\n std::vector m_words;\n \n};\n\n// --------------------------------------------------------------------------\n\ntemplate\nTemplatedVocabulary::TemplatedVocabulary\n (int k, int L, WeightingType weighting, ScoringType scoring)\n : m_k(k), m_L(L), m_weighting(weighting), m_scoring(scoring),\n m_scoring_object(NULL)\n{\n //printf(\"loop start load bin\\n\");\n createScoringObject();\n}\n\n// --------------------------------------------------------------------------\n\ntemplate\nTemplatedVocabulary::TemplatedVocabulary\n (const std::string &filename): m_scoring_object(NULL)\n{\n //m_scoring = KL;\n // Changed by VINS [[[\n //printf(\"loop start load bin\\n\");\n loadBin(filename);\n // Changed by VINS ]]]\n}\n\n// --------------------------------------------------------------------------\n\ntemplate\nTemplatedVocabulary::TemplatedVocabulary\n (const char *filename): m_scoring_object(NULL)\n{\n //m_scoring = KL;\n // Changed by VINS [[[\n //printf(\"loop start load bin\\n\");\n loadBin(filename);\n // Changed by VINS ]]]\n}\n\n// --------------------------------------------------------------------------\n\ntemplate\nvoid TemplatedVocabulary::createScoringObject()\n{\n delete m_scoring_object;\n m_scoring_object = NULL;\n \n switch(m_scoring)\n {\n case L1_NORM: \n m_scoring_object = new L1Scoring;\n break;\n \n case L2_NORM:\n m_scoring_object = new L2Scoring;\n break;\n \n case CHI_SQUARE:\n m_scoring_object = new ChiSquareScoring;\n break;\n \n case KL:\n m_scoring_object = new KLScoring;\n break;\n \n case BHATTACHARYYA:\n m_scoring_object = new BhattacharyyaScoring;\n break;\n \n case DOT_PRODUCT:\n m_scoring_object = new DotProductScoring;\n break;\n \n }\n}\n\n// --------------------------------------------------------------------------\n\ntemplate\nvoid TemplatedVocabulary::setScoringType(ScoringType type)\n{\n m_scoring = type;\n createScoringObject();\n}\n\n// --------------------------------------------------------------------------\n\ntemplate\nvoid TemplatedVocabulary::setWeightingType(WeightingType type)\n{\n this->m_weighting = type;\n}\n\n// --------------------------------------------------------------------------\n\ntemplate\nTemplatedVocabulary::TemplatedVocabulary(\n const TemplatedVocabulary &voc)\n : m_scoring_object(NULL)\n{\n printf(\"loop start load vocabulary\\n\");\n *this = voc;\n}\n\n// --------------------------------------------------------------------------\n\ntemplate\nTemplatedVocabulary::~TemplatedVocabulary()\n{\n delete m_scoring_object;\n}\n\n// --------------------------------------------------------------------------\n\ntemplate\nTemplatedVocabulary& \nTemplatedVocabulary::operator=\n (const TemplatedVocabulary &voc)\n{ \n this->m_k = voc.m_k;\n this->m_L = voc.m_L;\n this->m_scoring = voc.m_scoring;\n this->m_weighting = voc.m_weighting;\n\n this->createScoringObject();\n \n this->m_nodes.clear();\n this->m_words.clear();\n \n this->m_nodes = voc.m_nodes;\n this->createWords();\n \n return *this;\n}\n\n// --------------------------------------------------------------------------\n\ntemplate\nvoid TemplatedVocabulary::create(\n const std::vector > &training_features)\n{\n m_nodes.clear();\n m_words.clear();\n \n // expected_nodes = Sum_{i=0..L} ( k^i )\n\tint expected_nodes = \n\t\t(int)((pow((double)m_k, (double)m_L + 1) - 1)/(m_k - 1));\n\n m_nodes.reserve(expected_nodes); // avoid allocations when creating the tree\n \n \n std::vector features;\n getFeatures(training_features, features);\n\n\n // create root \n m_nodes.push_back(Node(0)); // root\n \n // create the tree\n HKmeansStep(0, features, 1);\n\n // create the words\n createWords();\n\n // and set the weight of each node of the tree\n setNodeWeights(training_features);\n \n}\n\n// --------------------------------------------------------------------------\n\ntemplate\nvoid TemplatedVocabulary::create(\n const std::vector > &training_features,\n int k, int L)\n{\n m_k = k;\n m_L = L;\n \n create(training_features);\n}\n\n// --------------------------------------------------------------------------\n\ntemplate\nvoid TemplatedVocabulary::create(\n const std::vector > &training_features,\n int k, int L, WeightingType weighting, ScoringType scoring)\n{\n m_k = k;\n m_L = L;\n m_weighting = weighting;\n m_scoring = scoring;\n createScoringObject();\n \n create(training_features);\n}\n\n// --------------------------------------------------------------------------\n\ntemplate\nvoid TemplatedVocabulary::getFeatures(\n const std::vector > &training_features,\n std::vector &features) const\n{\n features.resize(0);\n \n typename std::vector >::const_iterator vvit;\n typename std::vector::const_iterator vit;\n for(vvit = training_features.begin(); vvit != training_features.end(); ++vvit)\n {\n features.reserve(features.size() + vvit->size());\n for(vit = vvit->begin(); vit != vvit->end(); ++vit)\n {\n features.push_back(&(*vit));\n }\n }\n}\n\n// --------------------------------------------------------------------------\n\ntemplate\nvoid TemplatedVocabulary::HKmeansStep(NodeId parent_id, \n const std::vector &descriptors, int current_level)\n{\n if(descriptors.empty()) return;\n \n // features associated to each cluster\n std::vector clusters;\n std::vector > groups; // groups[i] = [j1, j2, ...]\n\t// j1, j2, ... indices of descriptors associated to cluster i\n\n clusters.reserve(m_k);\n\tgroups.reserve(m_k);\n \n //const int msizes[] = { m_k, descriptors.size() };\n //cv::SparseMat assoc(2, msizes, CV_8U);\n //cv::SparseMat last_assoc(2, msizes, CV_8U); \n //// assoc.row(cluster_idx).col(descriptor_idx) = 1 iif associated\n \n if((int)descriptors.size() <= m_k)\n {\n // trivial case: one cluster per feature\n groups.resize(descriptors.size());\n\n for(unsigned int i = 0; i < descriptors.size(); i++)\n {\n groups[i].push_back(i);\n clusters.push_back(*descriptors[i]);\n }\n }\n else\n {\n // select clusters and groups with kmeans\n \n bool first_time = true;\n bool goon = true;\n \n // to check if clusters move after iterations\n std::vector last_association, current_association;\n\n while(goon)\n {\n // 1. Calculate clusters\n\n\t\t\tif(first_time)\n\t\t\t{\n // random sample \n initiateClusters(descriptors, clusters);\n }\n else\n {\n // calculate cluster centres\n\n for(unsigned int c = 0; c < clusters.size(); ++c)\n {\n std::vector cluster_descriptors;\n cluster_descriptors.reserve(groups[c].size());\n \n /*\n for(unsigned int d = 0; d < descriptors.size(); ++d)\n {\n if( assoc.find(c, d) )\n {\n cluster_descriptors.push_back(descriptors[d]);\n }\n }\n */\n \n std::vector::const_iterator vit;\n for(vit = groups[c].begin(); vit != groups[c].end(); ++vit)\n {\n cluster_descriptors.push_back(descriptors[*vit]);\n }\n \n \n F::meanValue(cluster_descriptors, clusters[c]);\n }\n \n } // if(!first_time)\n\n // 2. Associate features with clusters\n\n // calculate distances to cluster centers\n groups.clear();\n groups.resize(clusters.size(), std::vector());\n current_association.resize(descriptors.size());\n\n //assoc.clear();\n\n typename std::vector::const_iterator fit;\n //unsigned int d = 0;\n for(fit = descriptors.begin(); fit != descriptors.end(); ++fit)//, ++d)\n {\n double best_dist = F::distance(*(*fit), clusters[0]);\n unsigned int icluster = 0;\n \n for(unsigned int c = 1; c < clusters.size(); ++c)\n {\n double dist = F::distance(*(*fit), clusters[c]);\n if(dist < best_dist)\n {\n best_dist = dist;\n icluster = c;\n }\n }\n\n //assoc.ref(icluster, d) = 1;\n\n groups[icluster].push_back(fit - descriptors.begin());\n current_association[ fit - descriptors.begin() ] = icluster;\n }\n \n // kmeans++ ensures all the clusters has any feature associated with them\n\n // 3. check convergence\n if(first_time)\n {\n first_time = false;\n }\n else\n {\n //goon = !eqUChar(last_assoc, assoc);\n \n goon = false;\n for(unsigned int i = 0; i < current_association.size(); i++)\n {\n if(current_association[i] != last_association[i]){\n goon = true;\n break;\n }\n }\n }\n\n\t\t\tif(goon)\n\t\t\t{\n\t\t\t\t// copy last feature-cluster association\n\t\t\t\tlast_association = current_association;\n\t\t\t\t//last_assoc = assoc.clone();\n\t\t\t}\n\t\t\t\n\t\t} // while(goon)\n \n } // if must run kmeans\n \n // create nodes\n for(unsigned int i = 0; i < clusters.size(); ++i)\n {\n NodeId id = m_nodes.size();\n m_nodes.push_back(Node(id));\n m_nodes.back().descriptor = clusters[i];\n m_nodes.back().parent = parent_id;\n m_nodes[parent_id].children.push_back(id);\n }\n \n // go on with the next level\n if(current_level < m_L)\n {\n // iterate again with the resulting clusters\n const std::vector &children_ids = m_nodes[parent_id].children;\n for(unsigned int i = 0; i < clusters.size(); ++i)\n {\n NodeId id = children_ids[i];\n\n std::vector child_features;\n child_features.reserve(groups[i].size());\n\n std::vector::const_iterator vit;\n for(vit = groups[i].begin(); vit != groups[i].end(); ++vit)\n {\n child_features.push_back(descriptors[*vit]);\n }\n\n if(child_features.size() > 1)\n {\n HKmeansStep(id, child_features, current_level + 1);\n }\n }\n }\n}\n\n// --------------------------------------------------------------------------\n\ntemplate\nvoid TemplatedVocabulary::initiateClusters\n (const std::vector &descriptors,\n std::vector &clusters) const\n{\n initiateClustersKMpp(descriptors, clusters); \n}\n\n// --------------------------------------------------------------------------\n\ntemplate\nvoid TemplatedVocabulary::initiateClustersKMpp(\n const std::vector &pfeatures,\n std::vector &clusters) const\n{\n // Implements kmeans++ seeding algorithm\n // Algorithm:\n // 1. Choose one center uniformly at random from among the data points.\n // 2. For each data point x, compute D(x), the distance between x and the nearest \n // center that has already been chosen.\n // 3. Add one new data point as a center. Each point x is chosen with probability \n // proportional to D(x)^2.\n // 4. Repeat Steps 2 and 3 until k centers have been chosen.\n // 5. Now that the initial centers have been chosen, proceed using standard k-means \n // clustering.\n\n DUtils::Random::SeedRandOnce();\n\n clusters.resize(0);\n clusters.reserve(m_k);\n std::vector min_dists(pfeatures.size(), std::numeric_limits::max());\n \n // 1.\n \n int ifeature = DUtils::Random::RandomInt(0, pfeatures.size()-1);\n \n // create first cluster\n clusters.push_back(*pfeatures[ifeature]);\n\n // compute the initial distances\n typename std::vector::const_iterator fit;\n std::vector::iterator dit;\n dit = min_dists.begin();\n for(fit = pfeatures.begin(); fit != pfeatures.end(); ++fit, ++dit)\n {\n *dit = F::distance(*(*fit), clusters.back());\n } \n\n while((int)clusters.size() < m_k)\n {\n // 2.\n dit = min_dists.begin();\n for(fit = pfeatures.begin(); fit != pfeatures.end(); ++fit, ++dit)\n {\n if(*dit > 0)\n {\n double dist = F::distance(*(*fit), clusters.back());\n if(dist < *dit) *dit = dist;\n }\n }\n \n // 3.\n double dist_sum = std::accumulate(min_dists.begin(), min_dists.end(), 0.0);\n\n if(dist_sum > 0)\n {\n double cut_d;\n do\n {\n cut_d = DUtils::Random::RandomValue(0, dist_sum);\n } while(cut_d == 0.0);\n\n double d_up_now = 0;\n for(dit = min_dists.begin(); dit != min_dists.end(); ++dit)\n {\n d_up_now += *dit;\n if(d_up_now >= cut_d) break;\n }\n \n if(dit == min_dists.end()) \n ifeature = pfeatures.size()-1;\n else\n ifeature = dit - min_dists.begin();\n \n clusters.push_back(*pfeatures[ifeature]);\n\n } // if dist_sum > 0\n else\n break;\n \n } // while(used_clusters < m_k)\n\n}\n\n// --------------------------------------------------------------------------\n\ntemplate\nvoid TemplatedVocabulary::createWords()\n{\n m_words.resize(0);\n \n if(!m_nodes.empty())\n {\n m_words.reserve( (int)pow((double)m_k, (double)m_L) );\n\n typename std::vector::iterator nit;\n \n nit = m_nodes.begin(); // ignore root\n for(++nit; nit != m_nodes.end(); ++nit)\n {\n if(nit->isLeaf())\n {\n nit->word_id = m_words.size();\n m_words.push_back( &(*nit) );\n }\n }\n }\n}\n\n// --------------------------------------------------------------------------\n\ntemplate\nvoid TemplatedVocabulary::setNodeWeights\n (const std::vector > &training_features)\n{\n const unsigned int NWords = m_words.size();\n const unsigned int NDocs = training_features.size();\n\n if(m_weighting == TF || m_weighting == BINARY)\n {\n // idf part must be 1 always\n for(unsigned int i = 0; i < NWords; i++)\n m_words[i]->weight = 1;\n }\n else if(m_weighting == IDF || m_weighting == TF_IDF)\n {\n // IDF and TF-IDF: we calculte the idf path now\n\n // Note: this actually calculates the idf part of the tf-idf score.\n // The complete tf-idf score is calculated in ::transform\n\n std::vector Ni(NWords, 0);\n std::vector counted(NWords, false);\n \n typename std::vector >::const_iterator mit;\n typename std::vector::const_iterator fit;\n\n for(mit = training_features.begin(); mit != training_features.end(); ++mit)\n {\n fill(counted.begin(), counted.end(), false);\n\n for(fit = mit->begin(); fit < mit->end(); ++fit)\n {\n WordId word_id;\n transform(*fit, word_id);\n\n if(!counted[word_id])\n {\n Ni[word_id]++;\n counted[word_id] = true;\n }\n }\n }\n\n // set ln(N/Ni)\n for(unsigned int i = 0; i < NWords; i++)\n {\n if(Ni[i] > 0)\n {\n m_words[i]->weight = log((double)NDocs / (double)Ni[i]);\n }// else // This cannot occur if using kmeans++\n }\n \n }\n\n}\n\n// --------------------------------------------------------------------------\n\ntemplate\ninline unsigned int TemplatedVocabulary::size() const\n{\n return m_words.size();\n}\n\n// --------------------------------------------------------------------------\n\ntemplate\ninline bool TemplatedVocabulary::empty() const\n{\n return m_words.empty();\n}\n\n// --------------------------------------------------------------------------\n\ntemplate\nfloat TemplatedVocabulary::getEffectiveLevels() const\n{\n long sum = 0;\n typename std::vector::const_iterator wit;\n for(wit = m_words.begin(); wit != m_words.end(); ++wit)\n {\n const Node *p = *wit;\n \n for(; p->id != 0; sum++) p = &m_nodes[p->parent];\n }\n \n return (float)((double)sum / (double)m_words.size());\n}\n\n// --------------------------------------------------------------------------\n\ntemplate\nTDescriptor TemplatedVocabulary::getWord(WordId wid) const\n{\n return m_words[wid]->descriptor;\n}\n\n// --------------------------------------------------------------------------\n\ntemplate\nWordValue TemplatedVocabulary::getWordWeight(WordId wid) const\n{\n return m_words[wid]->weight;\n}\n\n// --------------------------------------------------------------------------\n\ntemplate\nWordId TemplatedVocabulary::transform\n (const TDescriptor& feature) const\n{\n if(empty())\n {\n return 0;\n }\n \n WordId wid;\n transform(feature, wid);\n return wid;\n}\n\n// --------------------------------------------------------------------------\n\ntemplate\nvoid TemplatedVocabulary::transform(\n const std::vector& features, BowVector &v) const\n{\n v.clear();\n \n if(empty())\n {\n return;\n }\n\n // normalize \n LNorm norm;\n bool must = m_scoring_object->mustNormalize(norm);\n\n typename std::vector::const_iterator fit;\n\n if(m_weighting == TF || m_weighting == TF_IDF)\n {\n for(fit = features.begin(); fit < features.end(); ++fit)\n {\n WordId id;\n WordValue w; \n // w is the idf value if TF_IDF, 1 if TF\n \n transform(*fit, id, w);\n \n // not stopped\n if(w > 0) v.addWeight(id, w);\n }\n \n if(!v.empty() && !must)\n {\n // unnecessary when normalizing\n const double nd = v.size();\n for(BowVector::iterator vit = v.begin(); vit != v.end(); vit++) \n vit->second /= nd;\n }\n \n }\n else // IDF || BINARY\n {\n for(fit = features.begin(); fit < features.end(); ++fit)\n {\n WordId id;\n WordValue w;\n // w is idf if IDF, or 1 if BINARY\n \n transform(*fit, id, w);\n \n // not stopped\n if(w > 0) v.addIfNotExist(id, w);\n \n } // if add_features\n } // if m_weighting == ...\n \n if(must) v.normalize(norm);\n}\n\n// --------------------------------------------------------------------------\n\ntemplate \nvoid TemplatedVocabulary::transform(\n const std::vector& features,\n BowVector &v, FeatureVector &fv, int levelsup) const\n{\n v.clear();\n fv.clear();\n \n if(empty()) // safe for subclasses\n {\n return;\n }\n \n // normalize \n LNorm norm;\n bool must = m_scoring_object->mustNormalize(norm);\n \n typename std::vector::const_iterator fit;\n \n if(m_weighting == TF || m_weighting == TF_IDF)\n {\n unsigned int i_feature = 0;\n for(fit = features.begin(); fit < features.end(); ++fit, ++i_feature)\n {\n WordId id;\n NodeId nid;\n WordValue w; \n // w is the idf value if TF_IDF, 1 if TF\n \n transform(*fit, id, w, &nid, levelsup);\n \n if(w > 0) // not stopped\n { \n v.addWeight(id, w);\n fv.addFeature(nid, i_feature);\n }\n }\n \n if(!v.empty() && !must)\n {\n // unnecessary when normalizing\n const double nd = v.size();\n for(BowVector::iterator vit = v.begin(); vit != v.end(); vit++) \n vit->second /= nd;\n }\n \n }\n else // IDF || BINARY\n {\n unsigned int i_feature = 0;\n for(fit = features.begin(); fit < features.end(); ++fit, ++i_feature)\n {\n WordId id;\n NodeId nid;\n WordValue w;\n // w is idf if IDF, or 1 if BINARY\n \n transform(*fit, id, w, &nid, levelsup);\n \n if(w > 0) // not stopped\n {\n v.addIfNotExist(id, w);\n fv.addFeature(nid, i_feature);\n }\n }\n } // if m_weighting == ...\n \n if(must) v.normalize(norm);\n}\n\n// --------------------------------------------------------------------------\n\ntemplate \ninline double TemplatedVocabulary::score\n (const BowVector &v1, const BowVector &v2) const\n{\n return m_scoring_object->score(v1, v2);\n}\n\n// --------------------------------------------------------------------------\n\ntemplate\nvoid TemplatedVocabulary::transform\n (const TDescriptor &feature, WordId &id) const\n{\n WordValue weight;\n transform(feature, id, weight);\n}\n\n// --------------------------------------------------------------------------\n\ntemplate\nvoid TemplatedVocabulary::transform(const TDescriptor &feature, \n WordId &word_id, WordValue &weight, NodeId *nid, int levelsup) const\n{ \n // propagate the feature down the tree\n std::vector nodes;\n typename std::vector::const_iterator nit;\n\n // level at which the node must be stored in nid, if given\n const int nid_level = m_L - levelsup;\n if(nid_level <= 0 && nid != NULL) *nid = 0; // root\n\n NodeId final_id = 0; // root\n int current_level = 0;\n\n do\n {\n ++current_level;\n nodes = m_nodes[final_id].children;\n final_id = nodes[0];\n \n double best_d = F::distance(feature, m_nodes[final_id].descriptor);\n\n for(nit = nodes.begin() + 1; nit != nodes.end(); ++nit)\n {\n NodeId id = *nit;\n double d = F::distance(feature, m_nodes[id].descriptor);\n if(d < best_d)\n {\n best_d = d;\n final_id = id;\n }\n }\n \n if(nid != NULL && current_level == nid_level)\n *nid = final_id;\n \n } while( !m_nodes[final_id].isLeaf() );\n\n // turn node id into word id\n word_id = m_nodes[final_id].word_id;\n weight = m_nodes[final_id].weight;\n}\n\n// --------------------------------------------------------------------------\n\ntemplate\nNodeId TemplatedVocabulary::getParentNode\n (WordId wid, int levelsup) const\n{\n NodeId ret = m_words[wid]->id; // node id\n while(levelsup > 0 && ret != 0) // ret == 0 --> root\n {\n --levelsup;\n ret = m_nodes[ret].parent;\n }\n return ret;\n}\n\n// --------------------------------------------------------------------------\n\ntemplate\nvoid TemplatedVocabulary::getWordsFromNode\n (NodeId nid, std::vector &words) const\n{\n words.clear();\n \n if(m_nodes[nid].isLeaf())\n {\n words.push_back(m_nodes[nid].word_id);\n }\n else\n {\n words.reserve(m_k); // ^1, ^2, ...\n \n std::vector parents;\n parents.push_back(nid);\n \n while(!parents.empty())\n {\n NodeId parentid = parents.back();\n parents.pop_back();\n \n const std::vector &child_ids = m_nodes[parentid].children;\n std::vector::const_iterator cit;\n \n for(cit = child_ids.begin(); cit != child_ids.end(); ++cit)\n {\n const Node &child_node = m_nodes[*cit];\n \n if(child_node.isLeaf())\n words.push_back(child_node.word_id);\n else\n parents.push_back(*cit);\n \n } // for each child\n } // while !parents.empty\n }\n}\n\n// --------------------------------------------------------------------------\n\ntemplate\nint TemplatedVocabulary::stopWords(double minWeight)\n{\n int c = 0;\n typename std::vector::iterator wit;\n for(wit = m_words.begin(); wit != m_words.end(); ++wit)\n {\n if((*wit)->weight < minWeight)\n {\n ++c;\n (*wit)->weight = 0;\n }\n }\n return c;\n}\n\n// --------------------------------------------------------------------------\n\ntemplate\nvoid TemplatedVocabulary::save(const std::string &filename) const\n{\n cv::FileStorage fs(filename.c_str(), cv::FileStorage::WRITE);\n if(!fs.isOpened()) throw std::string(\"Could not open file \") + filename;\n \n save(fs);\n}\n\n// --------------------------------------------------------------------------\n\ntemplate\nvoid TemplatedVocabulary::load(const std::string &filename)\n{\n cv::FileStorage fs(filename.c_str(), cv::FileStorage::READ);\n if(!fs.isOpened()) throw std::string(\"Could not open file \") + filename;\n \n this->load(fs);\n}\n\n// --------------------------------------------------------------------------\n\ntemplate\nvoid TemplatedVocabulary::save(cv::FileStorage &f,\n const std::string &name) const\n{\n // Format YAML:\n // vocabulary \n // {\n // k:\n // L:\n // scoringType:\n // weightingType:\n // nodes \n // [\n // {\n // nodeId:\n // parentId:\n // weight:\n // descriptor: \n // }\n // ]\n // words\n // [\n // {\n // wordId:\n // nodeId:\n // }\n // ]\n // }\n //\n // The root node (index 0) is not included in the node vector\n //\n \n f << name << \"{\";\n \n f << \"k\" << m_k;\n f << \"L\" << m_L;\n f << \"scoringType\" << m_scoring;\n f << \"weightingType\" << m_weighting;\n \n // tree\n f << \"nodes\" << \"[\";\n std::vector parents, children;\n std::vector::const_iterator pit;\n\n parents.push_back(0); // root\n\n while(!parents.empty())\n {\n NodeId pid = parents.back();\n parents.pop_back();\n\n const Node& parent = m_nodes[pid];\n children = parent.children;\n\n for(pit = children.begin(); pit != children.end(); pit++)\n {\n const Node& child = m_nodes[*pit];\n\n // save node data\n f << \"{:\";\n f << \"nodeId\" << (int)child.id;\n f << \"parentId\" << (int)pid;\n f << \"weight\" << (double)child.weight;\n f << \"descriptor\" << F::toString(child.descriptor);\n f << \"}\";\n \n // add to parent list\n if(!child.isLeaf())\n {\n parents.push_back(*pit);\n }\n }\n }\n \n f << \"]\"; // nodes\n\n // words\n f << \"words\" << \"[\";\n \n typename std::vector::const_iterator wit;\n for(wit = m_words.begin(); wit != m_words.end(); wit++)\n {\n WordId id = wit - m_words.begin();\n f << \"{:\";\n f << \"wordId\" << (int)id;\n f << \"nodeId\" << (int)(*wit)->id;\n f << \"}\";\n }\n \n f << \"]\"; // words\n\n f << \"}\";\n\n}\n\n// --------------------------------------------------------------------------\n\ntemplate\nvoid TemplatedVocabulary::load(const cv::FileStorage &fs,\n const std::string &name)\n{\n m_words.clear();\n m_nodes.clear();\n \n cv::FileNode fvoc = fs[name];\n \n m_k = (int)fvoc[\"k\"];\n m_L = (int)fvoc[\"L\"];\n m_scoring = (ScoringType)((int)fvoc[\"scoringType\"]);\n m_weighting = (WeightingType)((int)fvoc[\"weightingType\"]);\n \n createScoringObject();\n\n // nodes\n cv::FileNode fn = fvoc[\"nodes\"];\n\n m_nodes.resize(fn.size() + 1); // +1 to include root\n m_nodes[0].id = 0;\n\n for(unsigned int i = 0; i < fn.size(); ++i)\n {\n NodeId nid = (int)fn[i][\"nodeId\"];\n NodeId pid = (int)fn[i][\"parentId\"];\n WordValue weight = (WordValue)fn[i][\"weight\"];\n std::string d = (std::string)fn[i][\"descriptor\"];\n \n m_nodes[nid].id = nid;\n m_nodes[nid].parent = pid;\n m_nodes[nid].weight = weight;\n m_nodes[pid].children.push_back(nid);\n \n F::fromString(m_nodes[nid].descriptor, d);\n }\n \n // words\n fn = fvoc[\"words\"];\n \n m_words.resize(fn.size());\n\n for(unsigned int i = 0; i < fn.size(); ++i)\n {\n NodeId wid = (int)fn[i][\"wordId\"];\n NodeId nid = (int)fn[i][\"nodeId\"];\n \n m_nodes[nid].word_id = wid;\n m_words[wid] = &m_nodes[nid];\n }\n}\n \n// Added by VINS [[[\ntemplate\nvoid TemplatedVocabulary::loadBin(const std::string &filename) {\n \n m_words.clear();\n m_nodes.clear();\n //printf(\"loop load bin\\n\");\n std::ifstream ifStream(filename);\n VINSLoop::Vocabulary voc;\n voc.deserialize(ifStream);\n ifStream.close();\n \n m_k = voc.k;\n m_L = voc.L;\n m_scoring = (ScoringType)voc.scoringType;\n m_weighting = (WeightingType)voc.weightingType;\n \n createScoringObject();\n\n m_nodes.resize(voc.nNodes + 1); // +1 to include root\n m_nodes[0].id = 0;\n\n for(int i = 0; i < voc.nNodes; ++i)\n {\n NodeId nid = voc.nodes[i].nodeId;\n NodeId pid = voc.nodes[i].parentId;\n WordValue weight = voc.nodes[i].weight;\n \n m_nodes[nid].id = nid;\n m_nodes[nid].parent = pid;\n m_nodes[nid].weight = weight;\n m_nodes[pid].children.push_back(nid);\n \n // Sorry to break template here\n m_nodes[nid].descriptor = boost::dynamic_bitset<>(voc.nodes[i].descriptor, voc.nodes[i].descriptor + 4);\n \n if (i < 5) {\n std::string test;\n boost::to_string(m_nodes[nid].descriptor, test);\n //cout << \"descriptor[\" << i << \"] = \" << test << endl;\n }\n }\n \n // words\n m_words.resize(voc.nWords);\n\n for(int i = 0; i < voc.nWords; ++i)\n {\n NodeId wid = (int)voc.words[i].wordId;\n NodeId nid = (int)voc.words[i].nodeId;\n \n m_nodes[nid].word_id = wid;\n m_words[wid] = &m_nodes[nid];\n }\n}\n \n// Added by VINS ]]]\n\n// --------------------------------------------------------------------------\n\n/**\n * Writes printable information of the vocabulary\n * @param os stream to write to\n * @param voc\n */\ntemplate\nstd::ostream& operator<<(std::ostream &os, \n const TemplatedVocabulary &voc)\n{\n os << \"Vocabulary: k = \" << voc.getBranchingFactor() \n << \", L = \" << voc.getDepthLevels()\n << \", Weighting = \";\n\n switch(voc.getWeightingType())\n {\n case TF_IDF: os << \"tf-idf\"; break;\n case TF: os << \"tf\"; break;\n case IDF: os << \"idf\"; break;\n case BINARY: os << \"binary\"; break;\n }\n\n os << \", Scoring = \";\n switch(voc.getScoringType())\n {\n case L1_NORM: os << \"L1-norm\"; break;\n case L2_NORM: os << \"L2-norm\"; break;\n case CHI_SQUARE: os << \"Chi square distance\"; break;\n case KL: os << \"KL-divergence\"; break;\n case BHATTACHARYYA: os << \"Bhattacharyya coefficient\"; break;\n case DOT_PRODUCT: os << \"Dot product\"; break;\n }\n \n os << \", Number of words = \" << voc.size();\n\n return os;\n}\n\n} // namespace DBoW2\n\n#endif\n" }, { "path": "pose_graph/src/ThirdParty/DUtils/DException.h", "content": "/*\t\n * File: DException.h\n * Project: DUtils library\n * Author: Dorian Galvez-Lopez\n * Date: October 6, 2009\n * Description: general exception of the library\n * License: see the LICENSE.txt file\n *\n */\n\n#pragma once\n\n#ifndef __D_EXCEPTION__\n#define __D_EXCEPTION__\n\n#include \n#include \nusing namespace std;\n\nnamespace DUtils {\n\n/// General exception\nclass DException :\n\tpublic exception\n{\npublic:\n\t/**\n\t * Creates an exception with a general error message\n\t */\n\tDException(void) throw(): m_message(\"DUtils exception\"){}\n\n\t/**\n\t * Creates an exception with a custom error message\n\t * @param msg: message\n\t */\n\tDException(const char *msg) throw(): m_message(msg){}\n\t\n\t/**\n\t * Creates an exception with a custom error message\n\t * @param msg: message\n\t */\n\tDException(const string &msg) throw(): m_message(msg){}\n\n /**\n\t * Destructor\n\t */\n\tvirtual ~DException(void) throw(){}\n\n\t/**\n\t * Returns the exception message\n\t */\n\tvirtual const char* what() const throw()\n\t{\n\t\treturn m_message.c_str();\n\t}\n\nprotected:\n /// Error message\n\tstring m_message;\n};\n\n}\n\n#endif\n\n" }, { "path": "pose_graph/src/ThirdParty/DUtils/DUtils.h", "content": "/*\r\n * File: DUtils.h\r\n * Project: DUtils library\r\n * Author: Dorian Galvez-Lopez\r\n * Date: October 6, 2009\r\n * Description: include file for including all the library functionalities\r\n * License: see the LICENSE.txt file\r\n *\r\n */\r\n\r\n/*! \\mainpage DUtils Library\r\n *\r\n * DUtils library for C++:\r\n * Collection of classes with general utilities for C++ applications.\r\n *\r\n * Written by Dorian Galvez-Lopez,\r\n * University of Zaragoza\r\n * \r\n * Check my website to obtain updates: http://webdiis.unizar.es/~dorian\r\n *\r\n * \\section license License\r\n * This program is free software: you can redistribute it and/or modify\r\n * it under the terms of the GNU Lesser General Public License (LGPL) as \r\n * published by the Free Software Foundation, either version 3 of the License, \r\n * or any later version.\r\n *\r\n */\r\n\r\n\r\n#pragma once\r\n\r\n#ifndef __D_UTILS__\r\n#define __D_UTILS__\r\n\r\n/// Several utilities for C++ programs\r\nnamespace DUtils\r\n{\r\n}\r\n\r\n// Exception\r\n#include \"DException.h\"\r\n\r\n#include \"Timestamp.h\"\r\n// Random numbers\r\n#include \"Random.h\"\r\n\r\n\r\n#endif\r\n" }, { "path": "pose_graph/src/ThirdParty/DUtils/Random.cpp", "content": "/*\t\r\n * File: Random.cpp\r\n * Project: DUtils library\r\n * Author: Dorian Galvez-Lopez\r\n * Date: April 2010\r\n * Description: manages pseudo-random numbers\r\n * License: see the LICENSE.txt file\r\n *\r\n */\r\n\r\n#include \"Random.h\"\r\n#include \"Timestamp.h\"\r\n#include \r\nusing namespace std;\r\n\r\nbool DUtils::Random::m_already_seeded = false;\r\n\r\nvoid DUtils::Random::SeedRand(){\r\n\tTimestamp time;\r\n\ttime.setToCurrentTime();\r\n\tsrand((unsigned)time.getFloatTime()); \r\n}\r\n\r\nvoid DUtils::Random::SeedRandOnce()\r\n{\r\n if(!m_already_seeded)\r\n {\r\n DUtils::Random::SeedRand();\r\n m_already_seeded = true;\r\n }\r\n}\r\n\r\nvoid DUtils::Random::SeedRand(int seed)\r\n{\r\n\tsrand(seed); \r\n}\r\n\r\nvoid DUtils::Random::SeedRandOnce(int seed)\r\n{\r\n if(!m_already_seeded)\r\n {\r\n DUtils::Random::SeedRand(seed);\r\n m_already_seeded = true;\r\n }\r\n}\r\n\r\nint DUtils::Random::RandomInt(int min, int max){\r\n\tint d = max - min + 1;\r\n\treturn int(((double)rand()/((double)RAND_MAX + 1.0)) * d) + min;\r\n}\r\n\r\n// ---------------------------------------------------------------------------\r\n// ---------------------------------------------------------------------------\r\n\r\nDUtils::Random::UnrepeatedRandomizer::UnrepeatedRandomizer(int min, int max)\r\n{\r\n if(min <= max)\r\n {\r\n m_min = min;\r\n m_max = max;\r\n }\r\n else\r\n {\r\n m_min = max;\r\n m_max = min;\r\n }\r\n\r\n createValues();\r\n}\r\n\r\n// ---------------------------------------------------------------------------\r\n\r\nDUtils::Random::UnrepeatedRandomizer::UnrepeatedRandomizer\r\n (const DUtils::Random::UnrepeatedRandomizer& rnd)\r\n{\r\n *this = rnd;\r\n}\r\n\r\n// ---------------------------------------------------------------------------\r\n\r\nint DUtils::Random::UnrepeatedRandomizer::get()\r\n{\r\n if(empty()) createValues();\r\n \r\n DUtils::Random::SeedRandOnce();\r\n \r\n int k = DUtils::Random::RandomInt(0, m_values.size()-1);\r\n int ret = m_values[k];\r\n m_values[k] = m_values.back();\r\n m_values.pop_back();\r\n \r\n return ret;\r\n}\r\n\r\n// ---------------------------------------------------------------------------\r\n\r\nvoid DUtils::Random::UnrepeatedRandomizer::createValues()\r\n{\r\n int n = m_max - m_min + 1;\r\n \r\n m_values.resize(n);\r\n for(int i = 0; i < n; ++i) m_values[i] = m_min + i;\r\n}\r\n\r\n// ---------------------------------------------------------------------------\r\n\r\nvoid DUtils::Random::UnrepeatedRandomizer::reset()\r\n{\r\n if((int)m_values.size() != m_max - m_min + 1) createValues();\r\n}\r\n\r\n// ---------------------------------------------------------------------------\r\n\r\nDUtils::Random::UnrepeatedRandomizer& \r\nDUtils::Random::UnrepeatedRandomizer::operator=\r\n (const DUtils::Random::UnrepeatedRandomizer& rnd)\r\n{\r\n if(this != &rnd)\r\n {\r\n this->m_min = rnd.m_min;\r\n this->m_max = rnd.m_max;\r\n this->m_values = rnd.m_values;\r\n }\r\n return *this;\r\n}\r\n\r\n// ---------------------------------------------------------------------------\r\n\r\n\r\n" }, { "path": "pose_graph/src/ThirdParty/DUtils/Random.h", "content": "/*\t\n * File: Random.h\n * Project: DUtils library\n * Author: Dorian Galvez-Lopez\n * Date: April 2010, November 2011\n * Description: manages pseudo-random numbers\n * License: see the LICENSE.txt file\n *\n */\n\n#pragma once\n#ifndef __D_RANDOM__\n#define __D_RANDOM__\n\n#include \n#include \n\nnamespace DUtils {\n\n/// Functions to generate pseudo-random numbers\nclass Random\n{\npublic:\n class UnrepeatedRandomizer;\n \npublic:\n\t/**\n\t * Sets the random number seed to the current time\n\t */\n\tstatic void SeedRand();\n\t\n\t/**\n\t * Sets the random number seed to the current time only the first\n\t * time this function is called\n\t */\n\tstatic void SeedRandOnce();\n\n\t/** \n\t * Sets the given random number seed\n\t * @param seed\n\t */\n\tstatic void SeedRand(int seed);\n\n\t/** \n\t * Sets the given random number seed only the first time this function \n\t * is called\n\t * @param seed\n\t */\n\tstatic void SeedRandOnce(int seed);\n\n\t/**\n\t * Returns a random number in the range [0..1]\n\t * @return random T number in [0..1]\n\t */\n\ttemplate \n\tstatic T RandomValue(){\n\t\treturn (T)rand()/(T)RAND_MAX;\n\t}\n\n\t/**\n\t * Returns a random number in the range [min..max]\n\t * @param min\n\t * @param max\n\t * @return random T number in [min..max]\n\t */\n\ttemplate \n\tstatic T RandomValue(T min, T max){\n\t\treturn Random::RandomValue() * (max - min) + min;\n\t}\n\n\t/**\n\t * Returns a random int in the range [min..max]\n\t * @param min\n\t * @param max\n\t * @return random int in [min..max]\n\t */\n\tstatic int RandomInt(int min, int max);\n\t\n\t/** \n\t * Returns a random number from a gaussian distribution\n\t * @param mean\n\t * @param sigma standard deviation\n\t */\n\ttemplate \n\tstatic T RandomGaussianValue(T mean, T sigma)\n\t{\n // Box-Muller transformation\n T x1, x2, w, y1;\n\n do {\n x1 = (T)2. * RandomValue() - (T)1.;\n x2 = (T)2. * RandomValue() - (T)1.;\n w = x1 * x1 + x2 * x2;\n } while ( w >= (T)1. || w == (T)0. );\n\n w = sqrt( ((T)-2.0 * log( w ) ) / w );\n y1 = x1 * w;\n\n return( mean + y1 * sigma );\n\t}\n\nprivate:\n\n /// If SeedRandOnce() or SeedRandOnce(int) have already been called\n static bool m_already_seeded;\n \n};\n\n// ---------------------------------------------------------------------------\n\n/// Provides pseudo-random numbers with no repetitions\nclass Random::UnrepeatedRandomizer\n{\npublic:\n\n /** \n * Creates a randomizer that returns numbers in the range [min, max]\n * @param min\n * @param max\n */\n UnrepeatedRandomizer(int min, int max);\n ~UnrepeatedRandomizer(){}\n \n /**\n * Copies a randomizer\n * @param rnd\n */\n UnrepeatedRandomizer(const UnrepeatedRandomizer& rnd);\n \n /**\n * Copies a randomizer\n * @param rnd\n */\n UnrepeatedRandomizer& operator=(const UnrepeatedRandomizer& rnd);\n \n /** \n * Returns a random number not given before. If all the possible values\n * were already given, the process starts again\n * @return unrepeated random number\n */\n int get();\n \n /**\n * Returns whether all the possible values between min and max were\n * already given. If get() is called when empty() is true, the behaviour\n * is the same than after creating the randomizer\n * @return true iff all the values were returned\n */\n inline bool empty() const { return m_values.empty(); }\n \n /**\n * Returns the number of values still to be returned\n * @return amount of values to return\n */\n inline unsigned int left() const { return m_values.size(); }\n \n /**\n * Resets the randomizer as it were just created\n */\n void reset();\n \nprotected:\n\n /** \n * Creates the vector with available values\n */\n void createValues();\n\nprotected:\n\n /// Min of range of values\n int m_min;\n /// Max of range of values\n int m_max;\n\n /// Available values\n std::vector m_values;\n\n};\n\n}\n\n#endif\n\n" }, { "path": "pose_graph/src/ThirdParty/DUtils/Timestamp.cpp", "content": "/*\r\n * File: Timestamp.cpp\r\n * Author: Dorian Galvez-Lopez\r\n * Date: March 2009\r\n * Description: timestamping functions\r\n * \r\n * Note: in windows, this class has a 1ms resolution\r\n *\r\n * License: see the LICENSE.txt file\r\n *\r\n */\r\n\r\n#include \r\n#include \r\n#include \r\n#include \r\n#include \r\n#include \r\n\r\n#ifdef _WIN32\r\n#ifndef WIN32\r\n#define WIN32\r\n#endif\r\n#endif\r\n\r\n#ifdef WIN32\r\n#include \r\n#define sprintf sprintf_s\r\n#else\r\n#include \r\n#endif\r\n\r\n#include \"Timestamp.h\"\r\n\r\nusing namespace std;\r\n\r\nusing namespace DUtils;\r\n\r\nTimestamp::Timestamp(Timestamp::tOptions option)\r\n{\r\n if(option & CURRENT_TIME)\r\n setToCurrentTime();\r\n else if(option & ZERO)\r\n setTime(0.);\r\n}\r\n\r\nTimestamp::~Timestamp(void)\r\n{\r\n}\r\n\r\nbool Timestamp::empty() const\r\n{\r\n return m_secs == 0 && m_usecs == 0;\r\n}\r\n\r\nvoid Timestamp::setToCurrentTime(){\r\n\t\r\n#ifdef WIN32\r\n\tstruct __timeb32 timebuffer;\r\n\t_ftime32_s( &timebuffer ); // C4996\r\n\t// Note: _ftime is deprecated; consider using _ftime_s instead\r\n\tm_secs = timebuffer.time;\r\n\tm_usecs = timebuffer.millitm * 1000;\r\n#else\r\n\tstruct timeval now;\r\n\tgettimeofday(&now, NULL);\r\n\tm_secs = now.tv_sec;\r\n\tm_usecs = now.tv_usec;\r\n#endif\r\n\r\n}\r\n\r\nvoid Timestamp::setTime(const string &stime){\r\n\tstring::size_type p = stime.find('.');\r\n\tif(p == string::npos){\r\n\t\tm_secs = atol(stime.c_str());\r\n\t\tm_usecs = 0;\r\n\t}else{\r\n\t\tm_secs = atol(stime.substr(0, p).c_str());\r\n\t\t\r\n\t\tstring s_usecs = stime.substr(p+1, 6);\r\n\t\tm_usecs = atol(stime.substr(p+1).c_str());\r\n\t\tm_usecs *= (unsigned long)pow(10.0, double(6 - s_usecs.length()));\r\n\t}\r\n}\r\n\r\nvoid Timestamp::setTime(double s)\r\n{\r\n m_secs = (unsigned long)s;\r\n m_usecs = (s - (double)m_secs) * 1e6;\r\n}\r\n\r\ndouble Timestamp::getFloatTime() const {\r\n\treturn double(m_secs) + double(m_usecs)/1000000.0;\r\n}\r\n\r\nstring Timestamp::getStringTime() const {\r\n\tchar buf[32];\r\n\tsprintf(buf, \"%.6lf\", this->getFloatTime());\r\n\treturn string(buf);\r\n}\r\n\r\ndouble Timestamp::operator- (const Timestamp &t) const {\r\n\treturn this->getFloatTime() - t.getFloatTime();\r\n}\r\n\r\nTimestamp& Timestamp::operator+= (double s)\r\n{\r\n *this = *this + s;\r\n return *this;\r\n}\r\n\r\nTimestamp& Timestamp::operator-= (double s)\r\n{\r\n *this = *this - s;\r\n return *this;\r\n}\r\n\r\nTimestamp Timestamp::operator+ (double s) const\r\n{\r\n\tunsigned long secs = (long)floor(s);\r\n\tunsigned long usecs = (long)((s - (double)secs) * 1e6);\r\n\r\n return this->plus(secs, usecs);\r\n}\r\n\r\nTimestamp Timestamp::plus(unsigned long secs, unsigned long usecs) const\r\n{\r\n Timestamp t;\r\n\r\n\tconst unsigned long max = 1000000ul;\r\n\r\n\tif(m_usecs + usecs >= max)\r\n\t\tt.setTime(m_secs + secs + 1, m_usecs + usecs - max);\r\n\telse\r\n\t\tt.setTime(m_secs + secs, m_usecs + usecs);\r\n\t\r\n\treturn t;\r\n}\r\n\r\nTimestamp Timestamp::operator- (double s) const\r\n{\r\n\tunsigned long secs = (long)floor(s);\r\n\tunsigned long usecs = (long)((s - (double)secs) * 1e6);\r\n\r\n\treturn this->minus(secs, usecs);\r\n}\r\n\r\nTimestamp Timestamp::minus(unsigned long secs, unsigned long usecs) const\r\n{\r\n Timestamp t;\r\n\r\n\tconst unsigned long max = 1000000ul;\r\n\r\n\tif(m_usecs < usecs)\r\n\t\tt.setTime(m_secs - secs - 1, max - (usecs - m_usecs));\r\n\telse\r\n\t\tt.setTime(m_secs - secs, m_usecs - usecs);\r\n\t\r\n\treturn t;\r\n}\r\n\r\nbool Timestamp::operator> (const Timestamp &t) const\r\n{\r\n\tif(m_secs > t.m_secs) return true;\r\n\telse if(m_secs == t.m_secs) return m_usecs > t.m_usecs;\r\n\telse return false;\r\n}\r\n\r\nbool Timestamp::operator>= (const Timestamp &t) const\r\n{\r\n\tif(m_secs > t.m_secs) return true;\r\n\telse if(m_secs == t.m_secs) return m_usecs >= t.m_usecs;\r\n\telse return false;\r\n}\r\n\r\nbool Timestamp::operator< (const Timestamp &t) const\r\n{\r\n\tif(m_secs < t.m_secs) return true;\r\n\telse if(m_secs == t.m_secs) return m_usecs < t.m_usecs;\r\n\telse return false;\r\n}\r\n\r\nbool Timestamp::operator<= (const Timestamp &t) const\r\n{\r\n\tif(m_secs < t.m_secs) return true;\r\n\telse if(m_secs == t.m_secs) return m_usecs <= t.m_usecs;\r\n\telse return false;\r\n}\r\n\r\nbool Timestamp::operator== (const Timestamp &t) const\r\n{\r\n\treturn(m_secs == t.m_secs && m_usecs == t.m_usecs);\r\n}\r\n\r\n\r\nstring Timestamp::Format(bool machine_friendly) const \r\n{\r\n struct tm tm_time;\r\n\r\n time_t t = (time_t)getFloatTime();\r\n\r\n#ifdef WIN32\r\n localtime_s(&tm_time, &t);\r\n#else\r\n localtime_r(&t, &tm_time);\r\n#endif\r\n \r\n char buffer[128];\r\n \r\n if(machine_friendly)\r\n {\r\n strftime(buffer, 128, \"%Y%m%d_%H%M%S\", &tm_time);\r\n }\r\n else\r\n {\r\n strftime(buffer, 128, \"%c\", &tm_time); // Thu Aug 23 14:55:02 2001\r\n }\r\n \r\n return string(buffer);\r\n}\r\n\r\nstring Timestamp::Format(double s) {\r\n\tint days = int(s / (24. * 3600.0));\r\n\ts -= days * (24. * 3600.0);\r\n\tint hours = int(s / 3600.0);\r\n\ts -= hours * 3600;\r\n\tint minutes = int(s / 60.0);\r\n\ts -= minutes * 60;\r\n\tint seconds = int(s);\r\n\tint ms = int((s - seconds)*1e6);\r\n\r\n\tstringstream ss;\r\n\tss.fill('0');\r\n\tbool b;\r\n\tif((b = (days > 0))) ss << days << \"d \";\r\n\tif((b = (b || hours > 0))) ss << setw(2) << hours << \":\";\r\n\tif((b = (b || minutes > 0))) ss << setw(2) << minutes << \":\";\r\n\tif(b) ss << setw(2);\r\n\tss << seconds;\r\n\tif(!b) ss << \".\" << setw(6) << ms;\r\n\r\n\treturn ss.str();\r\n}\r\n\r\n\r\n" }, { "path": "pose_graph/src/ThirdParty/DUtils/Timestamp.h", "content": "/*\n * File: Timestamp.h\n * Author: Dorian Galvez-Lopez\n * Date: March 2009\n * Description: timestamping functions\n * License: see the LICENSE.txt file\n *\n */\n\n#ifndef __D_TIMESTAMP__\n#define __D_TIMESTAMP__\n\n#include \nusing namespace std;\n\nnamespace DUtils {\n\n/// Timestamp\nclass Timestamp\n{\npublic:\n\n /// Options to initiate a timestamp\n enum tOptions\n {\n NONE = 0,\n CURRENT_TIME = 0x1,\n ZERO = 0x2\n };\n \npublic:\n \n /**\n * Creates a timestamp \n * @param option option to set the initial time stamp\n */\n\tTimestamp(Timestamp::tOptions option = NONE);\n\t\n\t/**\n\t * Destructor\n\t */\n\tvirtual ~Timestamp(void);\n\n /**\n * Says if the timestamp is \"empty\": seconds and usecs are both 0, as \n * when initiated with the ZERO flag\n * @return true iif secs == usecs == 0\n */\n bool empty() const;\n\n\t/**\n\t * Sets this instance to the current time\n\t */\n\tvoid setToCurrentTime();\n\n\t/**\n\t * Sets the timestamp from seconds and microseconds\n\t * @param secs: seconds\n\t * @param usecs: microseconds\n\t */\n\tinline void setTime(unsigned long secs, unsigned long usecs){\n\t\tm_secs = secs;\n\t\tm_usecs = usecs;\n\t}\n\t\n\t/**\n\t * Returns the timestamp in seconds and microseconds\n\t * @param secs seconds\n\t * @param usecs microseconds\n\t */\n\tinline void getTime(unsigned long &secs, unsigned long &usecs) const\n\t{\n\t secs = m_secs;\n\t usecs = m_usecs;\n\t}\n\n\t/**\n\t * Sets the timestamp from a string with the time in seconds\n\t * @param stime: string such as \"1235603336.036609\"\n\t */\n\tvoid setTime(const string &stime);\n\t\n\t/**\n\t * Sets the timestamp from a number of seconds from the epoch\n\t * @param s seconds from the epoch\n\t */\n\tvoid setTime(double s);\n\t\n\t/**\n\t * Returns this timestamp as the number of seconds in (long) float format\n\t */\n\tdouble getFloatTime() const;\n\n\t/**\n\t * Returns this timestamp as the number of seconds in fixed length string format\n\t */\n\tstring getStringTime() const;\n\n\t/**\n\t * Returns the difference in seconds between this timestamp (greater) and t (smaller)\n\t * If the order is swapped, a negative number is returned\n\t * @param t: timestamp to subtract from this timestamp\n\t * @return difference in seconds\n\t */\n\tdouble operator- (const Timestamp &t) const;\n\n\t/** \n\t * Returns a copy of this timestamp + s seconds + us microseconds\n\t * @param s seconds\n\t * @param us microseconds\n\t */\n\tTimestamp plus(unsigned long s, unsigned long us) const;\n\n /**\n * Returns a copy of this timestamp - s seconds - us microseconds\n * @param s seconds\n * @param us microseconds\n */\n Timestamp minus(unsigned long s, unsigned long us) const;\n\n /**\n * Adds s seconds to this timestamp and returns a reference to itself\n * @param s seconds\n * @return reference to this timestamp\n */\n Timestamp& operator+= (double s);\n \n /**\n * Substracts s seconds to this timestamp and returns a reference to itself\n * @param s seconds\n * @return reference to this timestamp\n */\n Timestamp& operator-= (double s);\n\n\t/**\n\t * Returns a copy of this timestamp + s seconds\n\t * @param s: seconds\n\t */\n\tTimestamp operator+ (double s) const;\n\n\t/**\n\t * Returns a copy of this timestamp - s seconds\n\t * @param s: seconds\n\t */\n\tTimestamp operator- (double s) const;\n\n\t/**\n\t * Returns whether this timestamp is at the future of t\n\t * @param t\n\t */\n\tbool operator> (const Timestamp &t) const;\n\n\t/**\n\t * Returns whether this timestamp is at the future of (or is the same as) t\n\t * @param t\n\t */\n\tbool operator>= (const Timestamp &t) const;\n\n\t/** \n\t * Returns whether this timestamp and t represent the same instant\n\t * @param t\n\t */\n\tbool operator== (const Timestamp &t) const;\n\n\t/**\n\t * Returns whether this timestamp is at the past of t\n\t * @param t\n\t */\n\tbool operator< (const Timestamp &t) const;\n\n\t/**\n\t * Returns whether this timestamp is at the past of (or is the same as) t\n\t * @param t\n\t */\n\tbool operator<= (const Timestamp &t) const;\n\n /**\n * Returns the timestamp in a human-readable string\n * @param machine_friendly if true, the returned string is formatted\n * to yyyymmdd_hhmmss, without weekday or spaces\n * @note This has not been tested under Windows\n * @note The timestamp is truncated to seconds\n */\n string Format(bool machine_friendly = false) const;\n\n\t/**\n\t * Returns a string version of the elapsed time in seconds, with the format\n\t * xd hh:mm:ss, hh:mm:ss, mm:ss or s.us\n\t * @param s: elapsed seconds (given by getFloatTime) to format\n\t */\n\tstatic string Format(double s);\n\t\n\nprotected:\n /// Seconds\n\tunsigned long m_secs;\t// seconds\n\t/// Microseconds\n\tunsigned long m_usecs;\t// microseconds\n};\n\n}\n\n#endif\n\n" }, { "path": "pose_graph/src/ThirdParty/DVision/BRIEF.cpp", "content": "/**\n * File: BRIEF.cpp\n * Author: Dorian Galvez\n * Date: September 2010\n * Description: implementation of BRIEF (Binary Robust Independent \n * Elementary Features) descriptor by \n * Michael Calonder, Vincent Lepetitand Pascal Fua\n * + close binary tests (by Dorian Galvez-Lopez)\n * License: see the LICENSE.txt file\n *\n */\n\n#include \"BRIEF.h\"\n#include \"../DUtils/DUtils.h\"\n#include \n#include \n#include \n\nusing namespace std;\nusing namespace DVision;\n\n// ----------------------------------------------------------------------------\n\nBRIEF::BRIEF(int nbits, int patch_size, Type type):\n m_bit_length(nbits), m_patch_size(patch_size), m_type(type)\n{\n assert(patch_size > 1);\n assert(nbits > 0);\n generateTestPoints();\n}\n\n// ----------------------------------------------------------------------------\n\nBRIEF::~BRIEF()\n{\n}\n\n// ---------------------------------------------------------------------------\n\nvoid BRIEF::compute(const cv::Mat &image, \n const std::vector &points,\n vector &descriptors,\n bool treat_image) const\n{\n const float sigma = 2.f;\n const cv::Size ksize(9, 9);\n \n cv::Mat im;\n if(treat_image)\n {\n cv::Mat aux;\n if(image.depth() == 3)\n {\n cv::cvtColor(image, aux, cv::COLOR_RGB2GRAY);\n }\n else\n {\n aux = image;\n }\n\n cv::GaussianBlur(aux, im, ksize, sigma, sigma);\n \n }\n else\n {\n im = image;\n }\n \n assert(im.type() == CV_8UC1);\n assert(im.isContinuous());\n \n // use im now\n const int W = im.cols;\n const int H = im.rows;\n \n descriptors.resize(points.size());\n std::vector::iterator dit;\n\n std::vector::const_iterator kit;\n \n int x1, y1, x2, y2;\n \n dit = descriptors.begin();\n for(kit = points.begin(); kit != points.end(); ++kit, ++dit)\n {\n dit->resize(m_bit_length);\n dit->reset();\n\n for(unsigned int i = 0; i < m_x1.size(); ++i)\n {\n x1 = (int)(kit->pt.x + m_x1[i]);\n y1 = (int)(kit->pt.y + m_y1[i]);\n x2 = (int)(kit->pt.x + m_x2[i]);\n y2 = (int)(kit->pt.y + m_y2[i]);\n \n if(x1 >= 0 && x1 < W && y1 >= 0 && y1 < H \n && x2 >= 0 && x2 < W && y2 >= 0 && y2 < H)\n {\n if( im.ptr(y1)[x1] < im.ptr(y2)[x2] )\n {\n dit->set(i);\n } \n } // if (x,y)_1 and (x,y)_2 are in the image\n \n } // for each (x,y)\n } // for each keypoint\n}\n\n// ---------------------------------------------------------------------------\n\nvoid BRIEF::generateTestPoints()\n{ \n m_x1.resize(m_bit_length);\n m_y1.resize(m_bit_length);\n m_x2.resize(m_bit_length);\n m_y2.resize(m_bit_length);\n\n const float g_mean = 0.f;\n const float g_sigma = 0.2f * (float)m_patch_size;\n const float c_sigma = 0.08f * (float)m_patch_size;\n \n float sigma2;\n if(m_type == RANDOM)\n sigma2 = g_sigma;\n else\n sigma2 = c_sigma;\n \n const int max_v = m_patch_size / 2;\n \n DUtils::Random::SeedRandOnce();\n \n for(int i = 0; i < m_bit_length; ++i)\n {\n int x1, y1, x2, y2;\n \n do\n {\n x1 = DUtils::Random::RandomGaussianValue(g_mean, g_sigma);\n } while( x1 > max_v || x1 < -max_v);\n \n do\n {\n y1 = DUtils::Random::RandomGaussianValue(g_mean, g_sigma);\n } while( y1 > max_v || y1 < -max_v);\n \n float meanx, meany;\n if(m_type == RANDOM)\n meanx = meany = g_mean;\n else\n {\n meanx = x1;\n meany = y1;\n }\n \n do\n {\n x2 = DUtils::Random::RandomGaussianValue(meanx, sigma2);\n } while( x2 > max_v || x2 < -max_v);\n \n do\n {\n y2 = DUtils::Random::RandomGaussianValue(meany, sigma2);\n } while( y2 > max_v || y2 < -max_v);\n \n m_x1[i] = x1;\n m_y1[i] = y1;\n m_x2[i] = x2;\n m_y2[i] = y2;\n }\n\n}\n\n// ----------------------------------------------------------------------------\n\n\n" }, { "path": "pose_graph/src/ThirdParty/DVision/BRIEF.h", "content": "/**\n * File: BRIEF.h\n * Author: Dorian Galvez-Lopez\n * Date: March 2011\n * Description: implementation of BRIEF (Binary Robust Independent \n * Elementary Features) descriptor by \n * Michael Calonder, Vincent Lepetit and Pascal Fua\n * + close binary tests (by Dorian Galvez-Lopez)\n *\n * If you use this code with the RANDOM_CLOSE descriptor version, please cite:\n @INPROCEEDINGS{GalvezIROS11,\n author={Galvez-Lopez, Dorian and Tardos, Juan D.},\n booktitle={Intelligent Robots and Systems (IROS), 2011 IEEE/RSJ International Conference on},\n title={Real-time loop detection with bags of binary words},\n year={2011},\n month={sept.},\n volume={},\n number={},\n pages={51 -58},\n keywords={},\n doi={10.1109/IROS.2011.6094885},\n ISSN={2153-0858}\n }\n *\n * License: see the LICENSE.txt file\n *\n */\n\n#ifndef __D_BRIEF__\n#define __D_BRIEF__\n\n#include \n#include \n#include \n\nnamespace DVision {\n\n/// BRIEF descriptor\nclass BRIEF\n{\npublic:\n\n /// Bitset type\n typedef boost::dynamic_bitset<> bitset;\n\n /// Type of pairs\n enum Type\n {\n RANDOM, // random pairs (Calonder's original version)\n RANDOM_CLOSE, // random but close pairs (used in GalvezIROS11)\n };\n \npublic:\n\n /**\n * Creates the BRIEF a priori data for descriptors of nbits length\n * @param nbits descriptor length in bits\n * @param patch_size \n * @param type type of pairs to generate\n */\n BRIEF(int nbits = 256, int patch_size = 48, Type type = RANDOM_CLOSE);\n virtual ~BRIEF();\n \n /**\n * Returns the descriptor length in bits\n * @return descriptor length in bits\n */\n inline int getDescriptorLengthInBits() const\n {\n return m_bit_length;\n }\n \n /**\n * Returns the type of classifier\n */\n inline Type getType() const\n {\n return m_type;\n }\n \n /**\n * Returns the size of the patch\n */\n inline int getPatchSize() const\n {\n return m_patch_size;\n }\n \n /**\n * Returns the BRIEF descriptors of the given keypoints in the given image\n * @param image\n * @param points\n * @param descriptors \n * @param treat_image (default: true) if true, the image is converted to \n * grayscale if needed and smoothed. If not, it is assumed the image has\n * been treated by the user\n * @note this function is similar to BRIEF::compute\n */\n inline void operator() (const cv::Mat &image, \n const std::vector &points,\n std::vector &descriptors,\n bool treat_image = true) const\n {\n compute(image, points, descriptors, treat_image);\n }\n \n /**\n * Returns the BRIEF descriptors of the given keypoints in the given image\n * @param image\n * @param points\n * @param descriptors \n * @param treat_image (default: true) if true, the image is converted to \n * grayscale if needed and smoothed. If not, it is assumed the image has\n * been treated by the user\n * @note this function is similar to BRIEF::operator()\n */ \n void compute(const cv::Mat &image,\n const std::vector &points,\n std::vector &descriptors,\n bool treat_image = true) const;\n \n /**\n * Exports the test pattern\n * @param x1 x1 coordinates of pairs\n * @param y1 y1 coordinates of pairs\n * @param x2 x2 coordinates of pairs\n * @param y2 y2 coordinates of pairs\n */\n inline void exportPairs(std::vector &x1, std::vector &y1,\n std::vector &x2, std::vector &y2) const\n {\n x1 = m_x1;\n y1 = m_y1;\n x2 = m_x2;\n y2 = m_y2;\n }\n \n /**\n * Sets the test pattern\n * @param x1 x1 coordinates of pairs\n * @param y1 y1 coordinates of pairs\n * @param x2 x2 coordinates of pairs\n * @param y2 y2 coordinates of pairs\n */\n inline void importPairs(const std::vector &x1, \n const std::vector &y1, const std::vector &x2, \n const std::vector &y2)\n {\n m_x1 = x1;\n m_y1 = y1;\n m_x2 = x2;\n m_y2 = y2;\n m_bit_length = x1.size();\n }\n \n /**\n * Returns the Hamming distance between two descriptors\n * @param a first descriptor vector\n * @param b second descriptor vector\n * @return hamming distance\n */\n inline static int distance(const bitset &a, const bitset &b)\n {\n return (a^b).count();\n }\n\nprotected:\n\n /**\n * Generates random points in the patch coordinates, according to \n * m_patch_size and m_bit_length\n */\n void generateTestPoints();\n \nprotected:\n\n /// Descriptor length in bits\n int m_bit_length;\n\n /// Patch size\n int m_patch_size;\n \n /// Type of pairs\n Type m_type;\n\n /// Coordinates of test points relative to the center of the patch\n std::vector m_x1, m_x2;\n std::vector m_y1, m_y2;\n\n};\n\n} // namespace DVision\n\n#endif\n\n\n" }, { "path": "pose_graph/src/ThirdParty/DVision/DVision.h", "content": "/*\n * File: DVision.h\n * Project: DVision library\n * Author: Dorian Galvez-Lopez\n * Date: October 4, 2010\n * Description: several functions for computer vision\n * License: see the LICENSE.txt file\n *\n */\n\n/*! \\mainpage DVision Library\n *\n * DVision library for C++ and OpenCV:\n * Collection of classes with computer vision functionality\n *\n * Written by Dorian Galvez-Lopez,\n * University of Zaragoza\n * \n * Check my website to obtain updates: http://webdiis.unizar.es/~dorian\n *\n * \\section requirements Requirements\n * This library requires the DUtils and DUtilsCV libraries and the OpenCV library.\n *\n * \\section license License\n * This program is free software: you can redistribute it and/or modify\n * it under the terms of the GNU Lesser General Public License (LGPL) as \n * published by the Free Software Foundation, either version 3 of the License, \n * or any later version.\n *\n */\n\n#ifndef __D_VISION__\n#define __D_VISION__\n\n\n/// Computer vision functions\nnamespace DVision\n{\n}\n\n#include \"BRIEF.h\"\n\n#endif\n" }, { "path": "pose_graph/src/ThirdParty/VocabularyBinary.cpp", "content": "#include \"VocabularyBinary.hpp\"\n#include \nusing namespace std;\n\nVINSLoop::Vocabulary::Vocabulary()\n: nNodes(0), nodes(nullptr), nWords(0), words(nullptr) {\n}\n\nVINSLoop::Vocabulary::~Vocabulary() {\n if (nodes != nullptr) {\n delete [] nodes;\n nodes = nullptr;\n }\n \n if (words != nullptr) {\n delete [] words;\n words = nullptr;\n }\n}\n \nvoid VINSLoop::Vocabulary::serialize(ofstream& stream) {\n stream.write((const char *)this, staticDataSize());\n stream.write((const char *)nodes, sizeof(Node) * nNodes);\n stream.write((const char *)words, sizeof(Word) * nWords);\n}\n \nvoid VINSLoop::Vocabulary::deserialize(ifstream& stream) {\n stream.read((char *)this, staticDataSize());\n \n nodes = new Node[nNodes];\n stream.read((char *)nodes, sizeof(Node) * nNodes);\n \n words = new Word[nWords];\n stream.read((char *)words, sizeof(Word) * nWords);\n}\n" }, { "path": "pose_graph/src/ThirdParty/VocabularyBinary.hpp", "content": "#ifndef VocabularyBinary_hpp\n#define VocabularyBinary_hpp\n\n#include \n#include \n#include \n\nnamespace VINSLoop {\n\nstruct Node {\n int32_t nodeId;\n int32_t parentId;\n double weight;\n uint64_t descriptor[4];\n};\n\nstruct Word {\n int32_t nodeId;\n int32_t wordId;\n};\n\nstruct Vocabulary {\n int32_t k;\n int32_t L;\n int32_t scoringType;\n int32_t weightingType;\n\n int32_t nNodes;\n int32_t nWords;\n\n Node *nodes;\n Word *words;\n \n Vocabulary();\n ~Vocabulary();\n\n void serialize(std::ofstream &stream);\n void deserialize(std::ifstream &stream);\n\n inline static size_t staticDataSize() {\n return sizeof(Vocabulary) - sizeof(Node *) - sizeof(Word *);\n }\n};\n\n} // namespace VINSLoop\n\n#endif /* VocabularyBinary_hpp */\n" }, { "path": "pose_graph/src/keyframe/keyframe.cpp", "content": "#include \"keyframe.h\"\n#include \n\ntemplate \nstatic void reduceVector(vector &v, vector status) {\n int j = 0;\n for (int i = 0; i < int(v.size()); i++)\n if (status[i])\n v[j++] = v[i];\n v.resize(j);\n}\n\n// create keyframe online\nKeyFrame::KeyFrame(double _time_stamp, int _index, Vector3d &_vio_T_w_i,\n Matrix3d &_vio_R_w_i, cv::Mat &_image,\n vector &_point_3d,\n vector &_point_2d_uv,\n vector &_point_2d_norm,\n vector &_point_id, int _sequence) {\n time_stamp = _time_stamp;\n index = _index;\n vio_T_w_i = _vio_T_w_i;\n vio_R_w_i = _vio_R_w_i;\n T_w_i = vio_T_w_i;\n R_w_i = vio_R_w_i;\n origin_vio_T = vio_T_w_i;\n origin_vio_R = vio_R_w_i;\n image = _image.clone();\n cv::resize(image, thumbnail, cv::Size(80, 60));\n point_3d = _point_3d;\n point_2d_uv = _point_2d_uv;\n point_2d_norm = _point_2d_norm;\n point_id = _point_id;\n has_loop = false;\n loop_index = -1;\n has_fast_point = false;\n loop_info << 0, 0, 0, 0, 0, 0, 0, 0;\n sequence = _sequence;\n // compute keypoints, brief_desc,\n computeWindowBRIEFPoint();\n computeBRIEFPoint();\n if (!DEBUG_IMAGE)\n image.release();\n}\n\n// load previous keyframe\nKeyFrame::KeyFrame(double _time_stamp, int _index, Vector3d &_vio_T_w_i,\n Matrix3d &_vio_R_w_i, Vector3d &_T_w_i, Matrix3d &_R_w_i,\n cv::Mat &_image, int _loop_index,\n Eigen::Matrix &_loop_info,\n vector &_keypoints,\n vector &_keypoints_norm,\n vector &_brief_descriptors) {\n time_stamp = _time_stamp;\n index = _index;\n // vio_T_w_i = _vio_T_w_i;\n // vio_R_w_i = _vio_R_w_i;\n vio_T_w_i = _T_w_i;\n vio_R_w_i = _R_w_i;\n T_w_i = _T_w_i;\n R_w_i = _R_w_i;\n if (DEBUG_IMAGE) {\n image = _image.clone();\n cv::resize(image, thumbnail, cv::Size(80, 60));\n }\n if (_loop_index != -1)\n has_loop = true;\n else\n has_loop = false;\n loop_index = _loop_index;\n loop_info = _loop_info;\n has_fast_point = false;\n sequence = 0;\n keypoints = _keypoints;\n keypoints_norm = _keypoints_norm;\n brief_descriptors = _brief_descriptors;\n}\n\n//计算窗口中所有特征点的描述子\nvoid KeyFrame::computeWindowBRIEFPoint() {\n BriefExtractor extractor(BRIEF_PATTERN_FILE.c_str());\n for (int i = 0; i < (int)point_2d_uv.size(); i++) {\n cv::KeyPoint key;\n // point_2d_uv: pixel (x,y) in image\n key.pt = point_2d_uv[i];\n window_keypoints.push_back(key);\n }\n extractor(image, window_keypoints, window_brief_descriptors);\n}\n\n//额外检测新特征点并计算所有特征点的描述子，为了回环检测\nvoid KeyFrame::computeBRIEFPoint() {\n BriefExtractor extractor(BRIEF_PATTERN_FILE.c_str());\n const int fast_th = 20; // corner detector response threshold\n if (1)\n cv::FAST(image, keypoints, fast_th, true);\n else {\n cv::FAST(image, keypoints, fast_th, true);\n ROS_ERROR(\"num of FAST: %d\", (int)keypoints.size());\n keypoints.clear();\n vector tmp_pts;\n cv::goodFeaturesToTrack(image, tmp_pts, 50, 0.1, 10);\n ROS_ERROR(\"num of good features: %d\", (int)tmp_pts.size());\n for (int i = 0; i < (int)tmp_pts.size(); i++) {\n cv::KeyPoint key;\n key.pt = tmp_pts[i];\n keypoints.push_back(key);\n }\n }\n extractor(image, keypoints, brief_descriptors);\n for (int i = 0; i < (int)keypoints.size(); i++) {\n Eigen::Vector3d tmp_p;\n m_camera->liftProjective(\n Eigen::Vector2d(keypoints[i].pt.x, keypoints[i].pt.y), tmp_p);\n cv::KeyPoint tmp_norm;\n tmp_norm.pt = cv::Point2f(tmp_p.x() / tmp_p.z(), tmp_p.y() / tmp_p.z());\n keypoints_norm.push_back(tmp_norm);\n }\n}\n\nvoid BriefExtractor::operator()(const cv::Mat &im, vector &keys,\n vector &descriptors) const {\n m_brief.compute(im, keys, descriptors);\n}\n\nbool KeyFrame::searchInAera(const BRIEF::bitset window_descriptor,\n const std::vector &descriptors_old,\n const std::vector &keypoints_old,\n const std::vector &keypoints_old_norm,\n cv::Point2f &best_match,\n cv::Point2f &best_match_norm) {\n cv::Point2f best_pt;\n int bestDist = 128;\n int bestIndex = -1;\n for (int i = 0; i < (int)descriptors_old.size(); i++) {\n\n int dis = HammingDis(window_descriptor, descriptors_old[i]);\n if (dis < bestDist) {\n bestDist = dis;\n bestIndex = i;\n }\n }\n // printf(\"best dist %d\", bestDist);\n if (bestIndex != -1 && bestDist < 80) {\n best_match = keypoints_old[bestIndex].pt;\n best_match_norm = keypoints_old_norm[bestIndex].pt;\n return true;\n } else\n return false;\n}\n\nvoid KeyFrame::searchByBRIEFDes(\n std::vector &matched_2d_old,\n std::vector &matched_2d_old_norm, std::vector &status,\n const std::vector &descriptors_old,\n const std::vector &keypoints_old,\n const std::vector &keypoints_old_norm) {\n for (int i = 0; i < (int)window_brief_descriptors.size(); i++) {\n cv::Point2f pt(0.f, 0.f);\n cv::Point2f pt_norm(0.f, 0.f);\n if (searchInAera(window_brief_descriptors[i], descriptors_old,\n keypoints_old, keypoints_old_norm, pt, pt_norm))\n status.push_back(1);\n else\n status.push_back(0);\n matched_2d_old.push_back(pt);\n matched_2d_old_norm.push_back(pt_norm);\n }\n}\n\nvoid KeyFrame::FundmantalMatrixRANSAC(\n const std::vector &matched_2d_cur_norm,\n const std::vector &matched_2d_old_norm,\n vector &status) {\n int n = (int)matched_2d_cur_norm.size();\n for (int i = 0; i < n; i++)\n status.push_back(0);\n if (n >= 8) {\n vector tmp_cur(n), tmp_old(n);\n for (int i = 0; i < (int)matched_2d_cur_norm.size(); i++) {\n double FOCAL_LENGTH = 460.0;\n double tmp_x, tmp_y;\n tmp_x = FOCAL_LENGTH * matched_2d_cur_norm[i].x + COL / 2.0;\n tmp_y = FOCAL_LENGTH * matched_2d_cur_norm[i].y + ROW / 2.0;\n tmp_cur[i] = cv::Point2f(tmp_x, tmp_y);\n\n tmp_x = FOCAL_LENGTH * matched_2d_old_norm[i].x + COL / 2.0;\n tmp_y = FOCAL_LENGTH * matched_2d_old_norm[i].y + ROW / 2.0;\n tmp_old[i] = cv::Point2f(tmp_x, tmp_y);\n }\n cv::findFundamentalMat(tmp_cur, tmp_old, cv::FM_RANSAC, 3.0, 0.9, status);\n }\n}\n\nvoid KeyFrame::PnPRANSAC(const vector &matched_2d_old_norm,\n const std::vector &matched_3d,\n std::vector &status, Eigen::Vector3d &PnP_T_old,\n Eigen::Matrix3d &PnP_R_old) {\n // for (int i = 0; i < matched_3d.size(); i++)\n //\tprintf(\"3d x: %f, y: %f, z: %f\\n\",matched_3d[i].x, matched_3d[i].y,\n //matched_3d[i].z ); printf(\"match size %d \\n\", matched_3d.size());\n cv::Mat r, rvec, t, D, tmp_r;\n cv::Mat K = (cv::Mat_(3, 3) << 1.0, 0, 0, 0, 1.0, 0, 0, 0, 1.0);\n Matrix3d R_inital;\n Vector3d P_inital;\n Matrix3d R_w_c = origin_vio_R * qic;\n Vector3d T_w_c = origin_vio_T + origin_vio_R * tic;\n\n R_inital = R_w_c.inverse();\n P_inital = -(R_inital * T_w_c);\n\n cv::eigen2cv(R_inital, tmp_r);\n cv::Rodrigues(tmp_r, rvec);\n cv::eigen2cv(P_inital, t);\n\n cv::Mat inliers;\n TicToc t_pnp_ransac;\n\n if (CV_MAJOR_VERSION < 3)\n solvePnPRansac(matched_3d, matched_2d_old_norm, K, D, rvec, t, true, 100,\n 10.0 / 460.0, 100, inliers);\n else {\n if (CV_MINOR_VERSION < 2)\n solvePnPRansac(matched_3d, matched_2d_old_norm, K, D, rvec, t, true, 100,\n sqrt(10.0 / 460.0), 0.99, inliers);\n else\n solvePnPRansac(matched_3d, matched_2d_old_norm, K, D, rvec, t, true, 100,\n 10.0 / 460.0, 0.99, inliers);\n }\n\n for (int i = 0; i < (int)matched_2d_old_norm.size(); i++)\n status.push_back(0);\n\n for (int i = 0; i < inliers.rows; i++) {\n int n = inliers.at(i);\n status[n] = 1;\n }\n\n cv::Rodrigues(rvec, r);\n Matrix3d R_pnp, R_w_c_old;\n cv::cv2eigen(r, R_pnp);\n R_w_c_old = R_pnp.transpose();\n Vector3d T_pnp, T_w_c_old;\n cv::cv2eigen(t, T_pnp);\n T_w_c_old = R_w_c_old * (-T_pnp);\n\n PnP_R_old = R_w_c_old * qic.transpose();\n PnP_T_old = T_w_c_old - PnP_R_old * tic;\n}\n\n//寻找并建立关键帧与回环帧之间的匹配关系\nbool KeyFrame::findConnection(KeyFrame *old_kf) {\n TicToc tmp_t;\n // printf(\"find Connection\\n\");\n vector matched_2d_cur, matched_2d_old;\n vector matched_2d_cur_norm, matched_2d_old_norm;\n vector matched_3d;\n vector matched_id;\n vector status;\n\n matched_3d = point_3d;\n matched_2d_cur = point_2d_uv;\n matched_2d_cur_norm = point_2d_norm;\n matched_id = point_id;\n\n TicToc t_match;\n#if 0\n\t\tif (DEBUG_IMAGE) \n\t {\n\t cv::Mat gray_img, loop_match_img;\n\t cv::Mat old_img = old_kf->image;\n\t cv::hconcat(image, old_img, gray_img);\n\t cvtColor(gray_img, loop_match_img, CV_GRAY2RGB);\n\t for(int i = 0; i< (int)point_2d_uv.size(); i++)\n\t {\n\t cv::Point2f cur_pt = point_2d_uv[i];\n\t cv::circle(loop_match_img, cur_pt, 5, cv::Scalar(0, 255, 0));\n\t }\n\t for(int i = 0; i< (int)old_kf->keypoints.size(); i++)\n\t {\n\t cv::Point2f old_pt = old_kf->keypoints[i].pt;\n\t old_pt.x += COL;\n\t cv::circle(loop_match_img, old_pt, 5, cv::Scalar(0, 255, 0));\n\t }\n\t ostringstream path;\n\t path << \"/home/tony-ws1/raw_data/loop_image/\"\n\t << index << \"-\"\n\t << old_kf->index << \"-\" << \"0raw_point.jpg\";\n\t cv::imwrite( path.str().c_str(), loop_match_img);\n\t }\n#endif\n // ROS_ERROR(\"points before: %d\", (int)matched_2d_cur.size());\n // printf(\"search by des\\n\");\n searchByBRIEFDes(matched_2d_old, matched_2d_old_norm, status,\n old_kf->brief_descriptors, old_kf->keypoints,\n old_kf->keypoints_norm);\n reduceVector(matched_2d_cur, status);\n reduceVector(matched_2d_old, status);\n reduceVector(matched_2d_cur_norm, status);\n reduceVector(matched_2d_old_norm, status);\n reduceVector(matched_3d, status);\n reduceVector(matched_id, status);\n // printf(\"search by des finish\\n\");\n\n#if 0 \n\t\tif (DEBUG_IMAGE)\n\t {\n\t\t\tint gap = 10;\n \tcv::Mat gap_image(ROW, gap, CV_8UC1, cv::Scalar(255, 255, 255));\n cv::Mat gray_img, loop_match_img;\n cv::Mat old_img = old_kf->image;\n cv::hconcat(image, gap_image, gap_image);\n cv::hconcat(gap_image, old_img, gray_img);\n cvtColor(gray_img, loop_match_img, CV_GRAY2RGB);\n\t for(int i = 0; i< (int)matched_2d_cur.size(); i++)\n\t {\n\t cv::Point2f cur_pt = matched_2d_cur[i];\n\t cv::circle(loop_match_img, cur_pt, 5, cv::Scalar(0, 255, 0));\n\t }\n\t for(int i = 0; i< (int)matched_2d_old.size(); i++)\n\t {\n\t cv::Point2f old_pt = matched_2d_old[i];\n\t old_pt.x += (COL + gap);\n\t cv::circle(loop_match_img, old_pt, 5, cv::Scalar(0, 255, 0));\n\t }\n\t for (int i = 0; i< (int)matched_2d_cur.size(); i++)\n\t {\n\t cv::Point2f old_pt = matched_2d_old[i];\n\t old_pt.x += (COL + gap);\n\t cv::line(loop_match_img, matched_2d_cur[i], old_pt, cv::Scalar(0, 255, 0), 1, 8, 0);\n\t }\n\n\t ostringstream path, path1, path2;\n\t path << \"/home/tony-ws1/raw_data/loop_image/\"\n\t << index << \"-\"\n\t << old_kf->index << \"-\" << \"1descriptor_match.jpg\";\n\t cv::imwrite( path.str().c_str(), loop_match_img);\n\t /*\n\t path1 << \"/home/tony-ws1/raw_data/loop_image/\"\n\t << index << \"-\"\n\t << old_kf->index << \"-\" << \"1descriptor_match_1.jpg\";\n\t cv::imwrite( path1.str().c_str(), image);\n\t path2 << \"/home/tony-ws1/raw_data/loop_image/\"\n\t << index << \"-\"\n\t << old_kf->index << \"-\" << \"1descriptor_match_2.jpg\";\n\t cv::imwrite( path2.str().c_str(), old_img);\t \n\t */\n\t \n\t }\n#endif\n status.clear();\n/*\nFundmantalMatrixRANSAC(matched_2d_cur_norm, matched_2d_old_norm, status);\nreduceVector(matched_2d_cur, status);\nreduceVector(matched_2d_old, status);\nreduceVector(matched_2d_cur_norm, status);\nreduceVector(matched_2d_old_norm, status);\nreduceVector(matched_3d, status);\nreduceVector(matched_id, status);\n*/\n#if 0\n\t\tif (DEBUG_IMAGE)\n\t {\n\t\t\tint gap = 10;\n \tcv::Mat gap_image(ROW, gap, CV_8UC1, cv::Scalar(255, 255, 255));\n cv::Mat gray_img, loop_match_img;\n cv::Mat old_img = old_kf->image;\n cv::hconcat(image, gap_image, gap_image);\n cv::hconcat(gap_image, old_img, gray_img);\n cvtColor(gray_img, loop_match_img, CV_GRAY2RGB);\n\t for(int i = 0; i< (int)matched_2d_cur.size(); i++)\n\t {\n\t cv::Point2f cur_pt = matched_2d_cur[i];\n\t cv::circle(loop_match_img, cur_pt, 5, cv::Scalar(0, 255, 0));\n\t }\n\t for(int i = 0; i< (int)matched_2d_old.size(); i++)\n\t {\n\t cv::Point2f old_pt = matched_2d_old[i];\n\t old_pt.x += (COL + gap);\n\t cv::circle(loop_match_img, old_pt, 5, cv::Scalar(0, 255, 0));\n\t }\n\t for (int i = 0; i< (int)matched_2d_cur.size(); i++)\n\t {\n\t cv::Point2f old_pt = matched_2d_old[i];\n\t old_pt.x += (COL + gap) ;\n\t cv::line(loop_match_img, matched_2d_cur[i], old_pt, cv::Scalar(0, 255, 0), 1, 8, 0);\n\t }\n\n\t ostringstream path;\n\t path << \"/home/tony-ws1/raw_data/loop_image/\"\n\t << index << \"-\"\n\t << old_kf->index << \"-\" << \"2fundamental_match.jpg\";\n\t cv::imwrite( path.str().c_str(), loop_match_img);\n\t }\n#endif\n Eigen::Vector3d PnP_T_old;\n Eigen::Matrix3d PnP_R_old;\n Eigen::Vector3d relative_t;\n Quaterniond relative_q;\n double relative_yaw;\n\n // ROS_ERROR(\"points after: %d\", (int)matched_2d_cur.size());\n\n if ((int)matched_2d_cur.size() > MIN_LOOP_NUM) {\n\n status.clear();\n PnPRANSAC(matched_2d_old_norm, matched_3d, status, PnP_T_old, PnP_R_old);\n reduceVector(matched_2d_cur, status);\n reduceVector(matched_2d_old, status);\n reduceVector(matched_2d_cur_norm, status);\n reduceVector(matched_2d_old_norm, status);\n reduceVector(matched_3d, status);\n reduceVector(matched_id, status);\n ROS_WARN(\"points left after RANSAC: %d\", (int)matched_2d_cur.size());\n if (DEBUG_IMAGE) {\n int gap = 10;\n cv::Mat gap_image(ROW, gap, CV_8UC1, cv::Scalar(255, 255, 255));\n cv::Mat gray_img, loop_match_img;\n cv::Mat old_img = old_kf->image;\n // cv::imshow(\"image\",image);\n // cv::waitKey(1);\n cv::hconcat(image, gap_image, gap_image);\n // cv::imshow(\"gap_image\",gap_image);\n // cv::waitKey(1);\n cv::hconcat(gap_image, old_img, gray_img);\n // cv::imshow(\"gray_img\",gray_img);\n // cv::waitKey(1);\n cvtColor(gray_img, loop_match_img, CV_GRAY2RGB);\n for (int i = 0; i < (int)matched_2d_cur.size(); i++) {\n cv::Point2f cur_pt = matched_2d_cur[i];\n cv::circle(loop_match_img, cur_pt, 5, cv::Scalar(0, 255, 0));\n }\n for (int i = 0; i < (int)matched_2d_old.size(); i++) {\n cv::Point2f old_pt = matched_2d_old[i];\n old_pt.x += (COL + gap);\n cv::circle(loop_match_img, old_pt, 5, cv::Scalar(0, 255, 0));\n }\n for (int i = 0; i < (int)matched_2d_cur.size(); i++) {\n cv::Point2f old_pt = matched_2d_old[i];\n old_pt.x += (COL + gap);\n cv::line(loop_match_img, matched_2d_cur[i], old_pt,\n cv::Scalar(0, 255, 0), 2, 8, 0);\n }\n cv::Mat notation(50, COL + gap + COL, CV_8UC3, cv::Scalar(255, 255, 255));\n putText(notation,\n \"current frame: \" + to_string(index) +\n \" sequence: \" + to_string(sequence),\n cv::Point2f(20, 30), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(255),\n 3);\n\n putText(notation,\n \"previous frame: \" + to_string(old_kf->index) +\n \" sequence: \" + to_string(old_kf->sequence),\n cv::Point2f(20 + COL + gap, 30), cv::FONT_HERSHEY_SIMPLEX, 1,\n cv::Scalar(255), 3);\n cv::vconcat(notation, loop_match_img, loop_match_img);\n\n /*\n ostringstream path;\n path << \"/home/tony-ws1/raw_data/loop_image/\"\n << index << \"-\"\n << old_kf->index << \"-\" << \"3pnp_match.jpg\";\n cv::imwrite( path.str().c_str(), loop_match_img);\n */\n if ((int)matched_2d_cur.size() > MIN_LOOP_NUM) {\n /*\n cv::imshow(\"loop connection\",loop_match_img);\n cv::waitKey(10);\n */\n cv::Mat thumbimage;\n cv::resize(loop_match_img, thumbimage,\n cv::Size(loop_match_img.cols / 2, loop_match_img.rows / 2));\n sensor_msgs::ImagePtr msg =\n cv_bridge::CvImage(std_msgs::Header(), \"bgr8\", thumbimage)\n .toImageMsg();\n msg->header.stamp = ros::Time(time_stamp);\n pub_match_img.publish(msg);\n }\n }\n }\n\n if ((int)matched_2d_cur.size() > MIN_LOOP_NUM) {\n relative_t = PnP_R_old.transpose() * (origin_vio_T - PnP_T_old);\n relative_q = PnP_R_old.transpose() * origin_vio_R;\n relative_yaw = Utility::normalizeAngle(Utility::R2ypr(origin_vio_R).x() -\n Utility::R2ypr(PnP_R_old).x());\n // printf(\"PNP relative\\n\");\n // cout << \"pnp relative_t \" << relative_t.transpose() << endl;\n // cout << \"pnp relative_yaw \" << relative_yaw << endl;\n if (abs(relative_yaw) < 30.0 && relative_t.norm() < 20.0) {\n\n has_loop = true;\n loop_index = old_kf->index;\n loop_info << relative_t.x(), relative_t.y(), relative_t.z(),\n relative_q.w(), relative_q.x(), relative_q.y(), relative_q.z(),\n relative_yaw;\n if (FAST_RELOCALIZATION) {\n sensor_msgs::PointCloud msg_match_points;\n msg_match_points.header.stamp = ros::Time(time_stamp);\n for (int i = 0; i < (int)matched_2d_old_norm.size(); i++) {\n geometry_msgs::Point32 p;\n p.x = matched_2d_old_norm[i].x;\n p.y = matched_2d_old_norm[i].y;\n p.z = matched_id[i];\n msg_match_points.points.push_back(p);\n }\n Eigen::Vector3d T = old_kf->T_w_i;\n Eigen::Matrix3d R = old_kf->R_w_i;\n Quaterniond Q(R);\n sensor_msgs::ChannelFloat32 t_q_index;\n t_q_index.values.push_back(T.x());\n t_q_index.values.push_back(T.y());\n t_q_index.values.push_back(T.z());\n t_q_index.values.push_back(Q.w());\n t_q_index.values.push_back(Q.x());\n t_q_index.values.push_back(Q.y());\n t_q_index.values.push_back(Q.z());\n t_q_index.values.push_back(index);\n msg_match_points.channels.push_back(t_q_index);\n pub_match_points.publish(msg_match_points);\n }\n return true;\n }\n }\n // printf(\"loop final use num %d %lf--------------- \\n\",\n // (int)matched_2d_cur.size(), t_match.toc());\n return false;\n}\n\nint KeyFrame::HammingDis(const BRIEF::bitset &a, const BRIEF::bitset &b) {\n BRIEF::bitset xor_of_bitset = a ^ b;\n int dis = xor_of_bitset.count();\n return dis;\n}\n\nvoid KeyFrame::getVioPose(Eigen::Vector3d &_T_w_i, Eigen::Matrix3d &_R_w_i) {\n _T_w_i = vio_T_w_i;\n _R_w_i = vio_R_w_i;\n}\n\nvoid KeyFrame::getPose(Eigen::Vector3d &_T_w_i, Eigen::Matrix3d &_R_w_i) {\n _T_w_i = T_w_i;\n _R_w_i = R_w_i;\n}\n\nvoid KeyFrame::updatePose(const Eigen::Vector3d &_T_w_i,\n const Eigen::Matrix3d &_R_w_i) {\n T_w_i = _T_w_i;\n R_w_i = _R_w_i;\n}\n\nvoid KeyFrame::updateVioPose(const Eigen::Vector3d &_T_w_i,\n const Eigen::Matrix3d &_R_w_i) {\n vio_T_w_i = _T_w_i;\n vio_R_w_i = _R_w_i;\n T_w_i = vio_T_w_i;\n R_w_i = vio_R_w_i;\n}\n\nEigen::Vector3d KeyFrame::getLoopRelativeT() {\n // loop_info is updated in KeyFrame::findConnection(KeyFrame* old_kf)\n return Eigen::Vector3d(loop_info(0), loop_info(1), loop_info(2));\n}\n\nEigen::Quaterniond KeyFrame::getLoopRelativeQ() {\n return Eigen::Quaterniond(loop_info(3), loop_info(4), loop_info(5),\n loop_info(6));\n}\n\ndouble KeyFrame::getLoopRelativeYaw() { return loop_info(7); }\n\nvoid KeyFrame::updateLoop(Eigen::Matrix &_loop_info) {\n if (abs(_loop_info(7)) < 30.0 &&\n Vector3d(_loop_info(0), _loop_info(1), _loop_info(2)).norm() < 20.0) {\n // printf(\"update loop info\\n\");\n loop_info = _loop_info;\n }\n}\n\nBriefExtractor::BriefExtractor(const std::string &pattern_file) {\n // The DVision::BRIEF extractor computes a random pattern by default when\n // the object is created.\n // We load the pattern that we used to build the vocabulary, to make\n // the descriptors compatible with the predefined vocabulary\n\n // loads the pattern\n cv::FileStorage fs(pattern_file.c_str(), cv::FileStorage::READ);\n if (!fs.isOpened())\n throw string(\"Could not open file \") + pattern_file;\n\n vector x1, y1, x2, y2;\n fs[\"x1\"] >> x1;\n fs[\"x2\"] >> x2;\n fs[\"y1\"] >> y1;\n fs[\"y2\"] >> y2;\n\n m_brief.importPairs(x1, y1, x2, y2);\n}\n" }, { "path": "pose_graph/src/keyframe/keyframe.h", "content": "#pragma once\n\n#include \"../ThirdParty/DBoW/DBoW2.h\"\n#include \"../ThirdParty/DVision/DVision.h\"\n#include \"../utility/parameters.h\"\n#include \"../utility/tic_toc.h\"\n#include \"../utility/utility.h\"\n#include \"camodocal/camera_models/CameraFactory.h\"\n#include \"camodocal/camera_models/CataCamera.h\"\n#include \"camodocal/camera_models/PinholeCamera.h\"\n#include \n#include \n#include \n#include \n\n#define MIN_LOOP_NUM 25\n\nusing namespace Eigen;\nusing namespace std;\nusing namespace DVision;\n\nclass BriefExtractor {\npublic:\n virtual void operator()(const cv::Mat &im, vector &keys,\n vector &descriptors) const;\n BriefExtractor(const std::string &pattern_file);\n\n DVision::BRIEF m_brief;\n};\n\nclass KeyFrame {\npublic:\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n KeyFrame(double _time_stamp, int _index, Vector3d &_vio_T_w_i,\n Matrix3d &_vio_R_w_i, cv::Mat &_image,\n vector &_point_3d, vector &_point_2d_uv,\n vector &_point_2d_normal, vector &_point_id,\n int _sequence);\n KeyFrame(double _time_stamp, int _index, Vector3d &_vio_T_w_i,\n Matrix3d &_vio_R_w_i, Vector3d &_T_w_i, Matrix3d &_R_w_i,\n cv::Mat &_image, int _loop_index,\n Eigen::Matrix &_loop_info,\n vector &_keypoints,\n vector &_keypoints_norm,\n vector &_brief_descriptors);\n bool findConnection(KeyFrame *old_kf);\n void computeWindowBRIEFPoint();\n void computeBRIEFPoint();\n // void extractBrief();\n int HammingDis(const BRIEF::bitset &a, const BRIEF::bitset &b);\n bool searchInAera(const BRIEF::bitset window_descriptor,\n const std::vector &descriptors_old,\n const std::vector &keypoints_old,\n const std::vector &keypoints_old_norm,\n cv::Point2f &best_match, cv::Point2f &best_match_norm);\n void searchByBRIEFDes(std::vector &matched_2d_old,\n std::vector &matched_2d_old_norm,\n std::vector &status,\n const std::vector &descriptors_old,\n const std::vector &keypoints_old,\n const std::vector &keypoints_old_norm);\n void\n FundmantalMatrixRANSAC(const std::vector &matched_2d_cur_norm,\n const std::vector &matched_2d_old_norm,\n vector &status);\n void PnPRANSAC(const vector &matched_2d_old_norm,\n const std::vector &matched_3d,\n std::vector &status, Eigen::Vector3d &PnP_T_old,\n Eigen::Matrix3d &PnP_R_old);\n void getVioPose(Eigen::Vector3d &_T_w_i, Eigen::Matrix3d &_R_w_i);\n void getPose(Eigen::Vector3d &_T_w_i, Eigen::Matrix3d &_R_w_i);\n void updatePose(const Eigen::Vector3d &_T_w_i, const Eigen::Matrix3d &_R_w_i);\n void updateVioPose(const Eigen::Vector3d &_T_w_i,\n const Eigen::Matrix3d &_R_w_i);\n void updateLoop(Eigen::Matrix &_loop_info);\n\n Eigen::Vector3d getLoopRelativeT();\n double getLoopRelativeYaw();\n Eigen::Quaterniond getLoopRelativeQ();\n\n double time_stamp;\n int index;\n int local_index;\n Eigen::Vector3d vio_T_w_i;\n Eigen::Matrix3d vio_R_w_i;\n Eigen::Vector3d T_w_i;\n Eigen::Matrix3d R_w_i;\n Eigen::Vector3d origin_vio_T;\n Eigen::Matrix3d origin_vio_R;\n cv::Mat image;\n cv::Mat thumbnail;\n vector point_3d;\n vector point_2d_uv;\n vector point_2d_norm;\n vector point_id;\n vector keypoints;\n vector keypoints_norm;\n vector window_keypoints;\n vector brief_descriptors;\n vector window_brief_descriptors;\n bool has_fast_point;\n int sequence;\n\n bool has_loop;\n int loop_index;\n Eigen::Matrix loop_info;\n};\n" }, { "path": "pose_graph/src/pose_graph/pose_graph.cpp", "content": "#include \"pose_graph.h\"\n#include \nPoseGraph::PoseGraph() {\n posegraph_visualization = new CameraPoseVisualization(1.0, 0.0, 1.0, 1.0);\n posegraph_visualization->setScale(0.1);\n posegraph_visualization->setLineWidth(0.01);\n earliest_loop_index = -1;\n t_drift = Eigen::Vector3d(0, 0, 0);\n yaw_drift = 0;\n r_drift = Eigen::Matrix3d::Identity();\n w_t_vio = Eigen::Vector3d(0, 0, 0);\n w_r_vio = Eigen::Matrix3d::Identity();\n global_index = 0;\n sequence_cnt = 0;\n sequence_loop.push_back(false);\n base_sequence = 1;\n use_imu = false;\n}\n\nPoseGraph::~PoseGraph() { t_optimization.join(); }\n\nvoid PoseGraph::setIMUFlag(bool _use_imu) {\n use_imu = _use_imu;\n if (use_imu) {\n printf(\"VIO input, perfrom 4 DoF (x, y, z, yaw) pose graph optimization\\n\");\n t_optimization = std::thread(&PoseGraph::optimize4DoF, this);\n } else {\n printf(\"VO input, perfrom 6 DoF pose graph optimization\\n\");\n t_optimization = std::thread(&PoseGraph::optimize6DoF, this);\n }\n}\n\nvoid PoseGraph::registerPub(ros::NodeHandle &n) {\n pub_pg_path =\n n.advertise(\"/pose_graph/pose_graph_path\", 1000);\n pub_base_path = n.advertise(\"/pose_graph/base_path\", 1000);\n pub_pose_graph = n.advertise(\n \"/pose_graph/pose_graph\", 1000);\n for (int i = 1; i < 10; i++)\n pub_path[i] =\n n.advertise(\"/pose_graph/path_\" + to_string(i), 1000);\n}\n\nvoid PoseGraph::loadVocabulary(const std::string& voc_path) {\n voc = new BriefVocabulary(voc_path);\n db.setVocabulary(*voc, false, 0);\n}\n\nvoid PoseGraph::addKeyFrame(KeyFrame *cur_kf, bool flag_detect_loop) {\n // shift to base frame (the base(first) frame defined as the map frame)\n Vector3d vio_P_cur;\n Matrix3d vio_R_cur;\n // sequence = 1, sequence_cnt = 0 at init, sequence_cnt++\n // then sequence_cnt remains 1 (no new sequence in my case)\n if (sequence_cnt != cur_kf->sequence) {\n // run once\n sequence_cnt++;\n sequence_loop.push_back(false);\n w_t_vio = Eigen::Vector3d(0, 0, 0);\n w_r_vio = Eigen::Matrix3d::Identity();\n m_drift.lock();\n t_drift = Eigen::Vector3d(0, 0, 0);\n r_drift = Eigen::Matrix3d::Identity();\n m_drift.unlock();\n }\n\n cur_kf->getVioPose(vio_P_cur, vio_R_cur);\n vio_P_cur = w_r_vio * vio_P_cur + w_t_vio;\n vio_R_cur = w_r_vio * vio_R_cur;\n cur_kf->updateVioPose(vio_P_cur, vio_R_cur);\n cur_kf->index = global_index;\n global_index++;\n int loop_index = -1;\n // always true\n if (flag_detect_loop) {\n TicToc tmp_t;\n // get loop_index(match frame index) here\n loop_index = detectLoop(cur_kf, cur_kf->index);\n } else {\n addKeyFrameIntoVoc(cur_kf);\n }\n if (loop_index != -1) {\n // printf(\" %d detect loop with %d \\n\", cur_kf->index, loop_index);\n KeyFrame *old_kf = getKeyFrame(loop_index);\n\n //当前帧与回环候选帧进行描述子匹配\n if (cur_kf->findConnection(old_kf)) {\n // earliest_loop_index为最早的回环候选帧\n if (earliest_loop_index > loop_index || earliest_loop_index == -1)\n earliest_loop_index = loop_index;\n\n Vector3d w_P_old, w_P_cur, vio_P_cur;\n Matrix3d w_R_old, w_R_cur, vio_R_cur;\n old_kf->getVioPose(w_P_old, w_R_old);\n cur_kf->getVioPose(vio_P_cur, vio_R_cur);\n\n //获取当前帧与回环帧的相对位姿relative_q、relative_t\n Vector3d relative_t;\n Quaterniond relative_q;\n relative_t = cur_kf->getLoopRelativeT();\n relative_q = (cur_kf->getLoopRelativeQ()).toRotationMatrix();\n\n //重新计算当前帧位姿w_P_cur、w_R_cur\n // 使用回环优化过的w_R_old，w_P_old与匹配的相对位姿relative_t，relative_q来更新当前帧位姿\n w_P_cur = w_R_old * relative_t + w_P_old;\n w_R_cur = w_R_old * relative_q;\n\n //回环得到的位姿和VIO位姿之间的偏移量shift_r、shift_t\n double shift_yaw;\n Matrix3d shift_r;\n Vector3d shift_t;\n if (use_imu) {\n shift_yaw = Utility::R2ypr(w_R_cur).x() - Utility::R2ypr(vio_R_cur).x();\n shift_r = Utility::ypr2R(Vector3d(shift_yaw, 0, 0));\n } else {\n shift_r = w_R_cur * vio_R_cur.transpose();\n }\n shift_t = w_P_cur - w_R_cur * vio_R_cur.transpose() * vio_P_cur;\n // shift vio pose of whole sequence to the map frame\n if (old_kf->sequence != cur_kf->sequence &&\n sequence_loop[cur_kf->sequence] == 0) {\n w_r_vio = shift_r;\n w_t_vio = shift_t;\n vio_P_cur = w_r_vio * vio_P_cur + w_t_vio;\n vio_R_cur = w_r_vio * vio_R_cur;\n cur_kf->updateVioPose(vio_P_cur, vio_R_cur);\n list::iterator it = keyframelist.begin();\n for (; it != keyframelist.end(); it++) {\n if ((*it)->sequence == cur_kf->sequence) {\n Vector3d vio_P_cur;\n Matrix3d vio_R_cur;\n (*it)->getVioPose(vio_P_cur, vio_R_cur);\n vio_P_cur = w_r_vio * vio_P_cur + w_t_vio;\n vio_R_cur = w_r_vio * vio_R_cur;\n (*it)->updateVioPose(vio_P_cur, vio_R_cur);\n }\n }\n sequence_loop[cur_kf->sequence] = true;\n }\n //将当前帧放入优化队列中\n m_optimize_buf.lock();\n optimize_buf.push(cur_kf->index);\n m_optimize_buf.unlock();\n }\n }\n m_keyframelist.lock();\n\n //获取VIO当前帧的位姿P、R，根据偏移量得到实际位姿\n Vector3d P;\n Matrix3d R;\n cur_kf->getVioPose(P, R);\n P = r_drift * P + t_drift;\n R = r_drift * R;\n cur_kf->updatePose(P, R);\n\n Quaterniond Q{R};\n geometry_msgs::PoseStamped pose_stamped;\n pose_stamped.header.stamp = ros::Time(cur_kf->time_stamp);\n pose_stamped.header.frame_id = \"map\";\n pose_stamped.pose.position.x = P.x() + VISUALIZATION_SHIFT_X;\n pose_stamped.pose.position.y = P.y() + VISUALIZATION_SHIFT_Y;\n pose_stamped.pose.position.z = P.z();\n pose_stamped.pose.orientation.x = Q.x();\n pose_stamped.pose.orientation.y = Q.y();\n pose_stamped.pose.orientation.z = Q.z();\n pose_stamped.pose.orientation.w = Q.w();\n\n path[sequence_cnt].poses.push_back(pose_stamped);\n path[sequence_cnt].header = pose_stamped.header;\n\n // not used\n if (SAVE_LOOP_PATH) {\n ofstream loop_path_file(VINS_RESULT_PATH, ios::app);\n loop_path_file.setf(ios::fixed, ios::floatfield);\n loop_path_file.precision(0);\n loop_path_file << cur_kf->time_stamp * 1e9 << \",\";\n loop_path_file.precision(5);\n loop_path_file << P.x() << \",\" << P.y() << \",\" << P.z() << \",\" << Q.w()\n << \",\" << Q.x() << \",\" << Q.y() << \",\" << Q.z() << \",\"\n << endl;\n loop_path_file.close();\n }\n // not used\n // draw local connection\n if (SHOW_S_EDGE) {\n list::reverse_iterator rit = keyframelist.rbegin();\n for (int i = 0; i < 4; i++) {\n if (rit == keyframelist.rend())\n break;\n Vector3d conncected_P;\n Matrix3d connected_R;\n if ((*rit)->sequence == cur_kf->sequence) {\n (*rit)->getPose(conncected_P, connected_R);\n posegraph_visualization->add_edge(P, conncected_P);\n }\n rit++;\n }\n }\n // show connections between loop frames, not needed\n if (SHOW_L_EDGE) {\n if (cur_kf->has_loop) {\n // printf(\"has loop \\n\");\n KeyFrame *connected_KF = getKeyFrame(cur_kf->loop_index);\n Vector3d connected_P, P0;\n Matrix3d connected_R, R0;\n connected_KF->getPose(connected_P, connected_R);\n // cur_kf->getVioPose(P0, R0);\n cur_kf->getPose(P0, R0);\n if (cur_kf->sequence > 0) {\n // printf(\"add loop into visual \\n\");\n posegraph_visualization->add_loopedge(\n P0, connected_P +\n Vector3d(VISUALIZATION_SHIFT_X, VISUALIZATION_SHIFT_Y, 0));\n }\n }\n }\n // posegraph_visualization->add_pose(P + Vector3d(VISUALIZATION_SHIFT_X,\n // VISUALIZATION_SHIFT_Y, 0), Q);\n\n // add frame to key frame list\n keyframelist.push_back(cur_kf);\n publish();\n m_keyframelist.unlock();\n}\n\nvoid PoseGraph::loadKeyFrame(KeyFrame *cur_kf, bool flag_detect_loop) {\n cur_kf->index = global_index;\n global_index++;\n int loop_index = -1;\n if (flag_detect_loop)\n loop_index = detectLoop(cur_kf, cur_kf->index);\n else {\n addKeyFrameIntoVoc(cur_kf);\n }\n if (loop_index != -1) {\n printf(\" %d detect loop with %d \\n\", cur_kf->index, loop_index);\n KeyFrame *old_kf = getKeyFrame(loop_index);\n if (cur_kf->findConnection(old_kf)) {\n if (earliest_loop_index > loop_index || earliest_loop_index == -1)\n earliest_loop_index = loop_index;\n m_optimize_buf.lock();\n optimize_buf.push(cur_kf->index);\n m_optimize_buf.unlock();\n }\n }\n m_keyframelist.lock();\n Vector3d P;\n Matrix3d R;\n cur_kf->getPose(P, R);\n Quaterniond Q{R};\n geometry_msgs::PoseStamped pose_stamped;\n pose_stamped.header.stamp = ros::Time(cur_kf->time_stamp);\n pose_stamped.header.frame_id = \"map\";\n pose_stamped.pose.position.x = P.x() + VISUALIZATION_SHIFT_X;\n pose_stamped.pose.position.y = P.y() + VISUALIZATION_SHIFT_Y;\n pose_stamped.pose.position.z = P.z();\n pose_stamped.pose.orientation.x = Q.x();\n pose_stamped.pose.orientation.y = Q.y();\n pose_stamped.pose.orientation.z = Q.z();\n pose_stamped.pose.orientation.w = Q.w();\n base_path.poses.push_back(pose_stamped);\n base_path.header = pose_stamped.header;\n\n // draw local connection\n if (SHOW_S_EDGE) {\n list::reverse_iterator rit = keyframelist.rbegin();\n for (int i = 0; i < 1; i++) {\n if (rit == keyframelist.rend())\n break;\n Vector3d conncected_P;\n Matrix3d connected_R;\n if ((*rit)->sequence == cur_kf->sequence) {\n (*rit)->getPose(conncected_P, connected_R);\n posegraph_visualization->add_edge(P, conncected_P);\n }\n rit++;\n }\n }\n /*\n if (cur_kf->has_loop)\n {\n KeyFrame* connected_KF = getKeyFrame(cur_kf->loop_index);\n Vector3d connected_P;\n Matrix3d connected_R;\n connected_KF->getPose(connected_P, connected_R);\n posegraph_visualization->add_loopedge(P, connected_P, SHIFT);\n }\n */\n\n keyframelist.push_back(cur_kf);\n // publish();\n m_keyframelist.unlock();\n}\n\nKeyFrame *PoseGraph::getKeyFrame(int index) {\n // unique_lock lock(m_keyframelist);\n list::iterator it = keyframelist.begin();\n for (; it != keyframelist.end(); it++) {\n if ((*it)->index == index)\n break;\n }\n if (it != keyframelist.end())\n return *it;\n else\n return NULL;\n}\n\nint PoseGraph::detectLoop(KeyFrame *keyframe, int frame_index) {\n // put image into image_pool; for visualization\n cv::Mat compressed_image;\n // set false, not used\n if (DEBUG_IMAGE) {\n int feature_num = keyframe->keypoints.size();\n cv::resize(keyframe->image, compressed_image, cv::Size(376, 240));\n putText(compressed_image, \"feature_num:\" + to_string(feature_num),\n cv::Point2f(10, 10), cv::FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(255));\n image_pool[frame_index] = compressed_image;\n }\n TicToc tmp_t;\n // first query; then add this frame into database!\n QueryResults ret;\n TicToc t_query;\n db.query(keyframe->brief_descriptors, ret, 4, frame_index - 50);\n // printf(\"query time: %f\", t_query.toc());\n // cout << \"Searching for Image \" << frame_index << \". \" << ret << endl;\n\n TicToc t_add;\n db.add(keyframe->brief_descriptors);\n // printf(\"add feature time: %f\", t_add.toc());\n //------------------------------------------------------------------------------\n // ret[0] is the nearest neighbour's score. threshold change with neighour\n // score\n //------------------------------------------------------------------------------\n bool find_loop = false;\n cv::Mat loop_result;\n if (DEBUG_IMAGE) {\n loop_result = compressed_image.clone();\n if (!ret.empty())\n putText(loop_result, \"neighbour score:\" + to_string(ret[0].Score),\n cv::Point2f(10, 50), cv::FONT_HERSHEY_SIMPLEX, 0.5,\n cv::Scalar(255));\n }\n // visual loop result\n if (DEBUG_IMAGE) {\n for (unsigned int i = 0; i < ret.size(); i++) {\n int tmp_index = ret[i].Id;\n auto it = image_pool.find(tmp_index);\n cv::Mat tmp_image = (it->second).clone();\n putText(tmp_image,\n \"index: \" + to_string(tmp_index) +\n \"loop score:\" + to_string(ret[i].Score),\n cv::Point2f(10, 50), cv::FONT_HERSHEY_SIMPLEX, 0.5,\n cv::Scalar(255));\n cv::hconcat(loop_result, tmp_image, loop_result);\n }\n }\n // a good match with its nerghbour\n if (ret.size() >= 1 && ret[0].Score > 0.05) {\n for (unsigned int i = 1; i < ret.size(); i++) {\n // if (ret[i].Score > ret[0].Score * 0.3)\n if (ret[i].Score > 0.015) {\n find_loop = true;\n int tmp_index = ret[i].Id;\n if (DEBUG_IMAGE && 0) {\n auto it = image_pool.find(tmp_index);\n cv::Mat tmp_image = (it->second).clone();\n putText(tmp_image, \"loop score:\" + to_string(ret[i].Score),\n cv::Point2f(10, 50), cv::FONT_HERSHEY_SIMPLEX, 0.4,\n cv::Scalar(255));\n cv::hconcat(loop_result, tmp_image, loop_result);\n }\n }\n }\n }\n /*\n\n\n if (DEBUG_IMAGE)\n {\n cv::imshow(\"loop_result\", loop_result);\n cv::waitKey(20);\n }\n */\n if (find_loop && frame_index > 50) {\n int min_index = -1;\n for (unsigned int i = 0; i < ret.size(); i++) {\n if (min_index == -1 || (ret[i].Id < min_index && ret[i].Score > 0.015))\n min_index = ret[i].Id;\n }\n return min_index;\n } else\n return -1;\n}\n\nvoid PoseGraph::addKeyFrameIntoVoc(KeyFrame *keyframe) {\n // put image into image_pool; for visualization\n cv::Mat compressed_image;\n if (DEBUG_IMAGE) {\n int feature_num = keyframe->keypoints.size();\n cv::resize(keyframe->image, compressed_image, cv::Size(376, 240));\n putText(compressed_image, \"feature_num:\" + to_string(feature_num),\n cv::Point2f(10, 10), cv::FONT_HERSHEY_SIMPLEX, 0.4,\n cv::Scalar(255));\n image_pool[keyframe->index] = compressed_image;\n }\n\n db.add(keyframe->brief_descriptors);\n}\n\nvoid PoseGraph::optimize4DoF() {\n while (true) {\n int cur_index = -1;\n int first_looped_index = -1;\n m_optimize_buf.lock();\n while (!optimize_buf.empty()) {\n cur_index = optimize_buf.front();\n first_looped_index = earliest_loop_index;\n optimize_buf.pop();\n }\n m_optimize_buf.unlock();\n if (cur_index != -1) {\n printf(\"optimize pose graph \\n\");\n TicToc tmp_t;\n m_keyframelist.lock();\n KeyFrame *cur_kf = getKeyFrame(cur_index);\n\n int max_length = cur_index + 1;\n\n // w^t_i w^q_i\n double t_array[max_length][3];\n Quaterniond q_array[max_length];\n double euler_array[max_length][3];\n double sequence_array[max_length];\n\n ceres::Problem problem;\n ceres::Solver::Options options;\n options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;\n // options.minimizer_progress_to_stdout = true;\n // options.max_solver_time_in_seconds = SOLVER_TIME * 3;\n options.max_num_iterations = 5;\n ceres::Solver::Summary summary;\n ceres::LossFunction *loss_function;\n loss_function = new ceres::HuberLoss(0.1);\n // loss_function = new ceres::CauchyLoss(1.0);\n ceres::LocalParameterization *angle_local_parameterization =\n AngleLocalParameterization::Create();\n\n list::iterator it;\n\n int i = 0;\n for (it = keyframelist.begin(); it != keyframelist.end(); it++) {\n if ((*it)->index < first_looped_index)\n continue;\n (*it)->local_index = i;\n Quaterniond tmp_q;\n Matrix3d tmp_r;\n Vector3d tmp_t;\n (*it)->getVioPose(tmp_t, tmp_r);\n tmp_q = tmp_r;\n t_array[i][0] = tmp_t(0);\n t_array[i][1] = tmp_t(1);\n t_array[i][2] = tmp_t(2);\n q_array[i] = tmp_q;\n\n Vector3d euler_angle = Utility::R2ypr(tmp_q.toRotationMatrix());\n euler_array[i][0] = euler_angle.x();\n euler_array[i][1] = euler_angle.y();\n euler_array[i][2] = euler_angle.z();\n\n sequence_array[i] = (*it)->sequence;\n\n problem.AddParameterBlock(euler_array[i], 1,\n angle_local_parameterization);\n problem.AddParameterBlock(t_array[i], 3);\n\n if ((*it)->index == first_looped_index || (*it)->sequence == 0) {\n problem.SetParameterBlockConstant(euler_array[i]);\n problem.SetParameterBlockConstant(t_array[i]);\n }\n\n // add edge\n for (int j = 1; j < 5; j++) {\n if (i - j >= 0 && sequence_array[i] == sequence_array[i - j]) {\n Vector3d euler_conncected =\n Utility::R2ypr(q_array[i - j].toRotationMatrix());\n Vector3d relative_t(t_array[i][0] - t_array[i - j][0],\n t_array[i][1] - t_array[i - j][1],\n t_array[i][2] - t_array[i - j][2]);\n relative_t = q_array[i - j].inverse() * relative_t;\n double relative_yaw = euler_array[i][0] - euler_array[i - j][0];\n ceres::CostFunction *cost_function = FourDOFError::Create(\n relative_t.x(), relative_t.y(), relative_t.z(), relative_yaw,\n euler_conncected.y(), euler_conncected.z());\n problem.AddResidualBlock(cost_function, NULL, euler_array[i - j],\n t_array[i - j], euler_array[i],\n t_array[i]);\n }\n }\n\n // add loop edge\n\n if ((*it)->has_loop) {\n assert((*it)->loop_index >= first_looped_index);\n int connected_index = getKeyFrame((*it)->loop_index)->local_index;\n Vector3d euler_conncected =\n Utility::R2ypr(q_array[connected_index].toRotationMatrix());\n Vector3d relative_t;\n relative_t = (*it)->getLoopRelativeT();\n double relative_yaw = (*it)->getLoopRelativeYaw();\n ceres::CostFunction *cost_function = FourDOFWeightError::Create(\n relative_t.x(), relative_t.y(), relative_t.z(), relative_yaw,\n euler_conncected.y(), euler_conncected.z());\n problem.AddResidualBlock(\n cost_function, loss_function, euler_array[connected_index],\n t_array[connected_index], euler_array[i], t_array[i]);\n }\n\n if ((*it)->index == cur_index)\n break;\n i++;\n }\n m_keyframelist.unlock();\n\n ceres::Solve(options, &problem, &summary);\n // std::cout << summary.BriefReport() << \"\\n\";\n\n // printf(\"pose optimization time: %f \\n\", tmp_t.toc());\n /*\n for (int j = 0 ; j < i; j++)\n {\n printf(\"optimize i: %d p: %f, %f, %f\\n\", j, t_array[j][0],\n t_array[j][1], t_array[j][2] );\n }\n */\n m_keyframelist.lock();\n i = 0;\n for (it = keyframelist.begin(); it != keyframelist.end(); it++) {\n if ((*it)->index < first_looped_index)\n continue;\n Quaterniond tmp_q;\n tmp_q = Utility::ypr2R(\n Vector3d(euler_array[i][0], euler_array[i][1], euler_array[i][2]));\n Vector3d tmp_t = Vector3d(t_array[i][0], t_array[i][1], t_array[i][2]);\n Matrix3d tmp_r = tmp_q.toRotationMatrix();\n (*it)->updatePose(tmp_t, tmp_r);\n\n if ((*it)->index == cur_index)\n break;\n i++;\n }\n\n Vector3d cur_t, vio_t;\n Matrix3d cur_r, vio_r;\n cur_kf->getPose(cur_t, cur_r);\n cur_kf->getVioPose(vio_t, vio_r);\n m_drift.lock();\n yaw_drift = Utility::R2ypr(cur_r).x() - Utility::R2ypr(vio_r).x();\n r_drift = Utility::ypr2R(Vector3d(yaw_drift, 0, 0));\n t_drift = cur_t - r_drift * vio_t;\n m_drift.unlock();\n // cout << \"t_drift \" << t_drift.transpose() << endl;\n // cout << \"r_drift \" << Utility::R2ypr(r_drift).transpose() << endl;\n // cout << \"yaw drift \" << yaw_drift << endl;\n\n it++;\n for (; it != keyframelist.end(); it++) {\n Vector3d P;\n Matrix3d R;\n (*it)->getVioPose(P, R);\n P = r_drift * P + t_drift;\n R = r_drift * R;\n (*it)->updatePose(P, R);\n }\n m_keyframelist.unlock();\n updatePath();\n }\n\n std::chrono::milliseconds dura(2000);\n std::this_thread::sleep_for(dura);\n }\n}\n\nvoid PoseGraph::optimize6DoF() {\n while (true) {\n int cur_index = -1;\n int first_looped_index = -1;\n m_optimize_buf.lock();\n while (!optimize_buf.empty()) {\n cur_index = optimize_buf.front();\n first_looped_index = earliest_loop_index;\n optimize_buf.pop();\n }\n m_optimize_buf.unlock();\n if (cur_index != -1) {\n printf(\"optimize pose graph \\n\");\n TicToc tmp_t;\n m_keyframelist.lock();\n KeyFrame *cur_kf = getKeyFrame(cur_index);\n\n int max_length = cur_index + 1;\n\n // w^t_i w^q_i\n double t_array[max_length][3];\n double q_array[max_length][4];\n double sequence_array[max_length];\n\n ceres::Problem problem;\n ceres::Solver::Options options;\n options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;\n // ptions.minimizer_progress_to_stdout = true;\n // options.max_solver_time_in_seconds = SOLVER_TIME * 3;\n options.max_num_iterations = 5;\n ceres::Solver::Summary summary;\n ceres::LossFunction *loss_function;\n loss_function = new ceres::HuberLoss(0.1);\n // loss_function = new ceres::CauchyLoss(1.0);\n ceres::LocalParameterization *local_parameterization =\n new ceres::QuaternionParameterization();\n\n list::iterator it;\n\n int i = 0;\n for (it = keyframelist.begin(); it != keyframelist.end(); it++) {\n if ((*it)->index < first_looped_index)\n continue;\n (*it)->local_index = i;\n Quaterniond tmp_q;\n Matrix3d tmp_r;\n Vector3d tmp_t;\n (*it)->getVioPose(tmp_t, tmp_r);\n tmp_q = tmp_r;\n t_array[i][0] = tmp_t(0);\n t_array[i][1] = tmp_t(1);\n t_array[i][2] = tmp_t(2);\n q_array[i][0] = tmp_q.w();\n q_array[i][1] = tmp_q.x();\n q_array[i][2] = tmp_q.y();\n q_array[i][3] = tmp_q.z();\n\n sequence_array[i] = (*it)->sequence;\n\n problem.AddParameterBlock(q_array[i], 4, local_parameterization);\n problem.AddParameterBlock(t_array[i], 3);\n\n if ((*it)->index == first_looped_index || (*it)->sequence == 0) {\n problem.SetParameterBlockConstant(q_array[i]);\n problem.SetParameterBlockConstant(t_array[i]);\n }\n\n // add edge\n for (int j = 1; j < 5; j++) {\n if (i - j >= 0 && sequence_array[i] == sequence_array[i - j]) {\n Vector3d relative_t(t_array[i][0] - t_array[i - j][0],\n t_array[i][1] - t_array[i - j][1],\n t_array[i][2] - t_array[i - j][2]);\n Quaterniond q_i_j =\n Quaterniond(q_array[i - j][0], q_array[i - j][1],\n q_array[i - j][2], q_array[i - j][3]);\n Quaterniond q_i = Quaterniond(q_array[i][0], q_array[i][1],\n q_array[i][2], q_array[i][3]);\n relative_t = q_i_j.inverse() * relative_t;\n Quaterniond relative_q = q_i_j.inverse() * q_i;\n ceres::CostFunction *vo_function = RelativeRTError::Create(\n relative_t.x(), relative_t.y(), relative_t.z(), relative_q.w(),\n relative_q.x(), relative_q.y(), relative_q.z(), 0.1, 0.01);\n problem.AddResidualBlock(vo_function, NULL, q_array[i - j],\n t_array[i - j], q_array[i], t_array[i]);\n }\n }\n\n // add loop edge\n\n if ((*it)->has_loop) {\n assert((*it)->loop_index >= first_looped_index);\n int connected_index = getKeyFrame((*it)->loop_index)->local_index;\n Vector3d relative_t;\n relative_t = (*it)->getLoopRelativeT();\n Quaterniond relative_q;\n relative_q = (*it)->getLoopRelativeQ();\n ceres::CostFunction *loop_function = RelativeRTError::Create(\n relative_t.x(), relative_t.y(), relative_t.z(), relative_q.w(),\n relative_q.x(), relative_q.y(), relative_q.z(), 0.1, 0.01);\n problem.AddResidualBlock(\n loop_function, loss_function, q_array[connected_index],\n t_array[connected_index], q_array[i], t_array[i]);\n }\n\n if ((*it)->index == cur_index)\n break;\n i++;\n }\n m_keyframelist.unlock();\n\n ceres::Solve(options, &problem, &summary);\n // std::cout << summary.BriefReport() << \"\\n\";\n\n // printf(\"pose optimization time: %f \\n\", tmp_t.toc());\n /*\n for (int j = 0 ; j < i; j++)\n {\n printf(\"optimize i: %d p: %f, %f, %f\\n\", j, t_array[j][0],\n t_array[j][1], t_array[j][2] );\n }\n */\n m_keyframelist.lock();\n i = 0;\n for (it = keyframelist.begin(); it != keyframelist.end(); it++) {\n if ((*it)->index < first_looped_index)\n continue;\n Quaterniond tmp_q(q_array[i][0], q_array[i][1], q_array[i][2],\n q_array[i][3]);\n Vector3d tmp_t = Vector3d(t_array[i][0], t_array[i][1], t_array[i][2]);\n Matrix3d tmp_r = tmp_q.toRotationMatrix();\n (*it)->updatePose(tmp_t, tmp_r);\n\n if ((*it)->index == cur_index)\n break;\n i++;\n }\n\n Vector3d cur_t, vio_t;\n Matrix3d cur_r, vio_r;\n cur_kf->getPose(cur_t, cur_r);\n cur_kf->getVioPose(vio_t, vio_r);\n m_drift.lock();\n r_drift = cur_r * vio_r.transpose();\n t_drift = cur_t - r_drift * vio_t;\n m_drift.unlock();\n // cout << \"t_drift \" << t_drift.transpose() << endl;\n // cout << \"r_drift \" << Utility::R2ypr(r_drift).transpose() << endl;\n\n it++;\n for (; it != keyframelist.end(); it++) {\n Vector3d P;\n Matrix3d R;\n (*it)->getVioPose(P, R);\n P = r_drift * P + t_drift;\n R = r_drift * R;\n (*it)->updatePose(P, R);\n }\n m_keyframelist.unlock();\n updatePath();\n }\n\n std::chrono::milliseconds dura(2000);\n std::this_thread::sleep_for(dura);\n }\n return;\n}\n\nvoid PoseGraph::updatePath() {\n m_keyframelist.lock();\n list::iterator it;\n for (int i = 1; i <= sequence_cnt; i++) {\n path[i].poses.clear();\n }\n base_path.poses.clear();\n posegraph_visualization->reset();\n // not used\n if (SAVE_LOOP_PATH) {\n ofstream loop_path_file_tmp(VINS_RESULT_PATH, ios::out);\n loop_path_file_tmp.close();\n }\n\n for (it = keyframelist.begin(); it != keyframelist.end(); it++) {\n Vector3d P;\n Matrix3d R;\n (*it)->getPose(P, R);\n\n Quaterniond Q;\n Q = R;\n // printf(\"path p: %f, %f, %f\\n\", P.x(), P.z(), P.y() );\n\n geometry_msgs::PoseStamped pose_stamped;\n pose_stamped.header.stamp = ros::Time((*it)->time_stamp);\n pose_stamped.header.frame_id = \"map\";\n pose_stamped.pose.position.x = P.x() + VISUALIZATION_SHIFT_X;\n pose_stamped.pose.position.y = P.y() + VISUALIZATION_SHIFT_Y;\n pose_stamped.pose.position.z = P.z();\n pose_stamped.pose.orientation.x = Q.x();\n pose_stamped.pose.orientation.y = Q.y();\n pose_stamped.pose.orientation.z = Q.z();\n pose_stamped.pose.orientation.w = Q.w();\n\n if ((*it)->sequence == 0) {\n base_path.poses.push_back(pose_stamped);\n base_path.header = pose_stamped.header;\n } else {\n path[(*it)->sequence].poses.push_back(pose_stamped);\n path[(*it)->sequence].header = pose_stamped.header;\n }\n // not used\n if (SAVE_LOOP_PATH) {\n ofstream loop_path_file(VINS_RESULT_PATH, ios::app);\n loop_path_file.setf(ios::fixed, ios::floatfield);\n loop_path_file.precision(0);\n loop_path_file << (*it)->time_stamp * 1e9 << \",\";\n loop_path_file.precision(5);\n loop_path_file << P.x() << \",\" << P.y() << \",\" << P.z() << \",\" << Q.w()\n << \",\" << Q.x() << \",\" << Q.y() << \",\" << Q.z() << \",\"\n << endl;\n loop_path_file.close();\n }\n // draw local connection\n // not used\n if (SHOW_S_EDGE) {\n list::reverse_iterator rit = keyframelist.rbegin();\n list::reverse_iterator lrit;\n for (; rit != keyframelist.rend(); rit++) {\n if ((*rit)->index == (*it)->index) {\n lrit = rit;\n lrit++;\n for (int i = 0; i < 4; i++) {\n if (lrit == keyframelist.rend())\n break;\n if ((*lrit)->sequence == (*it)->sequence) {\n Vector3d conncected_P;\n Matrix3d connected_R;\n (*lrit)->getPose(conncected_P, connected_R);\n posegraph_visualization->add_edge(P, conncected_P);\n }\n lrit++;\n }\n break;\n }\n }\n }\n if (SHOW_L_EDGE) {\n if ((*it)->has_loop && (*it)->sequence == sequence_cnt) {\n\n KeyFrame *connected_KF = getKeyFrame((*it)->loop_index);\n Vector3d connected_P;\n Matrix3d connected_R;\n connected_KF->getPose(connected_P, connected_R);\n //(*it)->getVioPose(P, R);\n (*it)->getPose(P, R);\n if ((*it)->sequence > 0) {\n posegraph_visualization->add_loopedge(\n P, connected_P +\n Vector3d(VISUALIZATION_SHIFT_X, VISUALIZATION_SHIFT_Y, 0));\n }\n }\n }\n }\n publish();\n m_keyframelist.unlock();\n}\n\nvoid PoseGraph::savePoseGraph() {\n m_keyframelist.lock();\n TicToc tmp_t;\n FILE *pFile, *pFile_shan_pg, *pFile_shan_vio;\n printf(\"pose graph path: %s\\n\", POSE_GRAPH_SAVE_PATH.c_str());\n printf(\"pose graph saving... \\n\");\n string file_path = POSE_GRAPH_SAVE_PATH + \"pose_graph.txt\";\n string file_path_shan_pg =\n POSE_GRAPH_SAVE_PATH + \"stamped_traj_estimate_mono_pg.txt\";\n string file_path_shan_vio =\n POSE_GRAPH_SAVE_PATH + \"stamped_traj_estimate_mono_vio.txt\";\n // string file_path_shan_pg =\n // \"/home/shanzy/rpg_trajectory_evaluation/results/laptop/vio_mono/laptop_vio_mono_MH_01/stamped_traj_estimate.txt\";\n // string file_path_shan_vio =\n // \"/home/shanzy/rpg_trajectory_evaluation/results/laptop/vio_mono/laptop_vio_mono_MH_01/vio_stamped_traj_estimate.txt\";\n pFile = fopen(file_path.c_str(), \"w\");\n pFile_shan_pg = fopen(file_path_shan_pg.c_str(), \"w\");\n pFile_shan_vio = fopen(file_path_shan_vio.c_str(), \"w\");\n // fprintf(pFile, \"index time_stamp Tx Ty Tz Qw Qx Qy Qz loop_index\n // loop_info\\n\");\n list::iterator it;\n for (it = keyframelist.begin(); it != keyframelist.end(); it++) {\n std::string image_path, descriptor_path, brief_path, keypoints_path;\n if (DEBUG_IMAGE) {\n image_path =\n POSE_GRAPH_SAVE_PATH + to_string((*it)->index) + \"_image.png\";\n imwrite(image_path.c_str(), (*it)->image);\n }\n Quaterniond VIO_tmp_Q{(*it)->vio_R_w_i};\n Quaterniond PG_tmp_Q{(*it)->R_w_i};\n Quaterniond rVIO_tmp_Q{(*it)->vio_R_w_i.transpose()};\n Quaterniond rPG_tmp_Q{(*it)->R_w_i.transpose()};\n Vector3d VIO_tmp_T = (*it)->vio_T_w_i;\n Vector3d PG_tmp_T = (*it)->T_w_i;\n\n fprintf(pFile,\n \" %d %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %d %f %f %f %f \"\n \"%f %f %f %f %d\\n\",\n (*it)->index, (*it)->time_stamp, VIO_tmp_T.x(), VIO_tmp_T.y(),\n VIO_tmp_T.z(), PG_tmp_T.x(), PG_tmp_T.y(), PG_tmp_T.z(),\n VIO_tmp_Q.w(), VIO_tmp_Q.x(), VIO_tmp_Q.y(), VIO_tmp_Q.z(),\n PG_tmp_Q.w(), PG_tmp_Q.x(), PG_tmp_Q.y(), PG_tmp_Q.z(),\n (*it)->loop_index, (*it)->loop_info(0), (*it)->loop_info(1),\n (*it)->loop_info(2), (*it)->loop_info(3), (*it)->loop_info(4),\n (*it)->loop_info(5), (*it)->loop_info(6), (*it)->loop_info(7),\n (int)(*it)->keypoints.size());\n fprintf(pFile_shan_pg, \"%f %f %f %f %f %f %f %f\\n\", (*it)->time_stamp,\n PG_tmp_T.x(), PG_tmp_T.y(), PG_tmp_T.z(), PG_tmp_Q.x(),\n PG_tmp_Q.y(), PG_tmp_Q.z(), PG_tmp_Q.w());\n fprintf(pFile_shan_vio, \"%f %f %f %f %f %f %f %f\\n\", (*it)->time_stamp,\n VIO_tmp_T.x(), VIO_tmp_T.y(), VIO_tmp_T.z(), VIO_tmp_Q.x(),\n VIO_tmp_Q.y(), VIO_tmp_Q.z(), VIO_tmp_Q.w());\n\n // write keypoints, brief_descriptors vector keypoints\n // vector brief_descriptors;\n assert((*it)->keypoints.size() == (*it)->brief_descriptors.size());\n brief_path =\n POSE_GRAPH_SAVE_PATH + to_string((*it)->index) + \"_briefdes.dat\";\n std::ofstream brief_file(brief_path, std::ios::binary);\n keypoints_path =\n POSE_GRAPH_SAVE_PATH + to_string((*it)->index) + \"_keypoints.txt\";\n FILE *keypoints_file;\n keypoints_file = fopen(keypoints_path.c_str(), \"w\");\n for (int i = 0; i < (int)(*it)->keypoints.size(); i++) {\n brief_file << (*it)->brief_descriptors[i] << endl;\n fprintf(keypoints_file, \"%f %f %f %f\\n\", (*it)->keypoints[i].pt.x,\n (*it)->keypoints[i].pt.y, (*it)->keypoints_norm[i].pt.x,\n (*it)->keypoints_norm[i].pt.y);\n }\n brief_file.close();\n fclose(keypoints_file);\n }\n fclose(pFile_shan_vio);\n fclose(pFile_shan_pg);\n fclose(pFile);\n\n printf(\"save pose graph time: %f s\\n\", tmp_t.toc() / 1000);\n m_keyframelist.unlock();\n}\n\nvoid PoseGraph::loadPoseGraph() {\n TicToc tmp_t;\n FILE *pFile;\n string file_path = POSE_GRAPH_SAVE_PATH + \"pose_graph.txt\";\n printf(\"lode pose graph from: %s \\n\", file_path.c_str());\n printf(\"pose graph loading...\\n\");\n pFile = fopen(file_path.c_str(), \"r\");\n if (pFile == NULL) {\n printf(\n \"lode previous pose graph error: wrong previous pose graph path or no \"\n \"previous pose graph \\n the system will start with new pose graph \\n\");\n return;\n }\n int index;\n double time_stamp;\n double VIO_Tx, VIO_Ty, VIO_Tz;\n double PG_Tx, PG_Ty, PG_Tz;\n double VIO_Qw, VIO_Qx, VIO_Qy, VIO_Qz;\n double PG_Qw, PG_Qx, PG_Qy, PG_Qz;\n double loop_info_0, loop_info_1, loop_info_2, loop_info_3;\n double loop_info_4, loop_info_5, loop_info_6, loop_info_7;\n int loop_index;\n int keypoints_num;\n Eigen::Matrix loop_info;\n int cnt = 0;\n while (fscanf(pFile,\n \"%d %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf \"\n \"%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,\n &PG_Tz, &VIO_Qw, &VIO_Qx, &VIO_Qy, &VIO_Qz, &PG_Qw, &PG_Qx,\n &PG_Qy, &PG_Qz, &loop_index, &loop_info_0, &loop_info_1,\n &loop_info_2, &loop_info_3, &loop_info_4, &loop_info_5,\n &loop_info_6, &loop_info_7, &keypoints_num) != EOF) {\n /*\n printf(\"I read: %d %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf\n %lf %d %lf %lf %lf %lf %lf %lf %lf %lf %d\\n\", index, time_stamp, VIO_Tx,\n VIO_Ty, VIO_Tz, PG_Tx, PG_Ty, PG_Tz, VIO_Qw, VIO_Qx, VIO_Qy, VIO_Qz, PG_Qw,\n PG_Qx, PG_Qy, PG_Qz, loop_index, loop_info_0, loop_info_1, loop_info_2,\n loop_info_3, loop_info_4, loop_info_5, loop_info_6, loop_info_7,\n keypoints_num);\n */\n cv::Mat image;\n std::string image_path, descriptor_path;\n if (DEBUG_IMAGE) {\n image_path = POSE_GRAPH_SAVE_PATH + to_string(index) + \"_image.png\";\n image = cv::imread(image_path.c_str(), 0);\n }\n\n Vector3d VIO_T(VIO_Tx, VIO_Ty, VIO_Tz);\n Vector3d PG_T(PG_Tx, PG_Ty, PG_Tz);\n Quaterniond VIO_Q;\n VIO_Q.w() = VIO_Qw;\n VIO_Q.x() = VIO_Qx;\n VIO_Q.y() = VIO_Qy;\n VIO_Q.z() = VIO_Qz;\n Quaterniond PG_Q;\n PG_Q.w() = PG_Qw;\n PG_Q.x() = PG_Qx;\n PG_Q.y() = PG_Qy;\n PG_Q.z() = PG_Qz;\n Matrix3d VIO_R, PG_R;\n VIO_R = VIO_Q.toRotationMatrix();\n PG_R = PG_Q.toRotationMatrix();\n Eigen::Matrix loop_info;\n loop_info << loop_info_0, loop_info_1, loop_info_2, loop_info_3,\n loop_info_4, loop_info_5, loop_info_6, loop_info_7;\n\n if (loop_index != -1)\n if (earliest_loop_index > loop_index || earliest_loop_index == -1) {\n earliest_loop_index = loop_index;\n }\n\n // load keypoints, brief_descriptors\n string brief_path =\n POSE_GRAPH_SAVE_PATH + to_string(index) + \"_briefdes.dat\";\n std::ifstream brief_file(brief_path, std::ios::binary);\n string keypoints_path =\n POSE_GRAPH_SAVE_PATH + to_string(index) + \"_keypoints.txt\";\n FILE *keypoints_file;\n keypoints_file = fopen(keypoints_path.c_str(), \"r\");\n vector keypoints;\n vector keypoints_norm;\n vector brief_descriptors;\n for (int i = 0; i < keypoints_num; i++) {\n BRIEF::bitset tmp_des;\n brief_file >> tmp_des;\n brief_descriptors.push_back(tmp_des);\n cv::KeyPoint tmp_keypoint;\n cv::KeyPoint tmp_keypoint_norm;\n double p_x, p_y, p_x_norm, p_y_norm;\n if (!fscanf(keypoints_file, \"%lf %lf %lf %lf\", &p_x, &p_y, &p_x_norm,\n &p_y_norm))\n printf(\" fail to load pose graph \\n\");\n tmp_keypoint.pt.x = p_x;\n tmp_keypoint.pt.y = p_y;\n tmp_keypoint_norm.pt.x = p_x_norm;\n tmp_keypoint_norm.pt.y = p_y_norm;\n keypoints.push_back(tmp_keypoint);\n keypoints_norm.push_back(tmp_keypoint_norm);\n }\n brief_file.close();\n fclose(keypoints_file);\n\n KeyFrame *keyframe = new KeyFrame(\n time_stamp, index, VIO_T, VIO_R, PG_T, PG_R, image, loop_index,\n loop_info, keypoints, keypoints_norm, brief_descriptors);\n loadKeyFrame(keyframe, 0);\n if (cnt % 20 == 0) {\n publish();\n }\n cnt++;\n }\n fclose(pFile);\n printf(\"load pose graph time: %f s\\n\", tmp_t.toc() / 1000);\n base_sequence = 0;\n}\n\nvoid PoseGraph::publish() {\n for (int i = 1; i <= sequence_cnt; i++) {\n // if (sequence_loop[i] == true || i == base_sequence)\n if (1 || i == base_sequence) {\n pub_pg_path.publish(path[i]);\n pub_path[i].publish(path[i]);\n // posegraph_visualization->publish_by(pub_pose_graph,\n // path[sequence_cnt].header);\n }\n }\n\n pub_base_path.publish(base_path);\n\n // posegraph_visualization->publish_by(pub_pose_graph,\n // path[sequence_cnt].header);\n}\n\nvoid PoseGraph::updateKeyFrameLoop(int index,\n Eigen::Matrix &_loop_info) {\n KeyFrame *kf = getKeyFrame(index);\n kf->updateLoop(_loop_info);\n if (abs(_loop_info(7)) < 30.0 &&\n Vector3d(_loop_info(0), _loop_info(1), _loop_info(2)).norm() < 20.0) {\n if (FAST_RELOCALIZATION) {\n KeyFrame *old_kf = getKeyFrame(kf->loop_index);\n Vector3d w_P_old, w_P_cur, vio_P_cur;\n Matrix3d w_R_old, w_R_cur, vio_R_cur;\n old_kf->getPose(w_P_old, w_R_old);\n kf->getVioPose(vio_P_cur, vio_R_cur);\n\n Vector3d relative_t;\n Quaterniond relative_q;\n relative_t = kf->getLoopRelativeT();\n relative_q = (kf->getLoopRelativeQ()).toRotationMatrix();\n w_P_cur = w_R_old * relative_t + w_P_old;\n w_R_cur = w_R_old * relative_q;\n double shift_yaw;\n Matrix3d shift_r;\n Vector3d shift_t;\n shift_yaw = Utility::R2ypr(w_R_cur).x() - Utility::R2ypr(vio_R_cur).x();\n shift_r = Utility::ypr2R(Vector3d(shift_yaw, 0, 0));\n shift_t = w_P_cur - w_R_cur * vio_R_cur.transpose() * vio_P_cur;\n\n m_drift.lock();\n yaw_drift = shift_yaw;\n r_drift = shift_r;\n t_drift = shift_t;\n m_drift.unlock();\n }\n }\n}\n" }, { "path": "pose_graph/src/pose_graph/pose_graph.h", "content": "#pragma once\n\n#include \"../ThirdParty/DBoW/DBoW2.h\"\n#include \"../ThirdParty/DBoW/TemplatedDatabase.h\"\n#include \"../ThirdParty/DBoW/TemplatedVocabulary.h\"\n#include \"../ThirdParty/DVision/DVision.h\"\n#include \"../keyframe/keyframe.h\"\n#include \"../utility/CameraPoseVisualization.h\"\n#include \"../utility/tic_toc.h\"\n#include \"../utility/utility.h\"\n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n\n#define SHOW_S_EDGE false\n#define SHOW_L_EDGE false\n#define SAVE_LOOP_PATH true\n\nusing namespace DVision;\nusing namespace DBoW2;\n\nclass PoseGraph {\npublic:\n PoseGraph();\n ~PoseGraph();\n void registerPub(ros::NodeHandle &n);\n void addKeyFrame(KeyFrame *cur_kf, bool flag_detect_loop);\n void loadKeyFrame(KeyFrame *cur_kf, bool flag_detect_loop);\n void loadVocabulary(const std::string& voc_path);\n void setIMUFlag(bool _use_imu);\n void updateKeyFrameLoop(int index, Eigen::Matrix &_loop_info);\n KeyFrame *getKeyFrame(int index);\n nav_msgs::Path path[10];\n nav_msgs::Path base_path;\n\n CameraPoseVisualization *posegraph_visualization;\n void savePoseGraph();\n void loadPoseGraph();\n void publish();\n\n // vio ----> cur(闭环后的)\n Vector3d t_drift;\n double yaw_drift;\n Matrix3d r_drift;\n // map frame( base sequence or first sequence)<----> cur sequence frame\n Vector3d w_t_vio;\n Matrix3d w_r_vio;\n\nprivate:\n int detectLoop(KeyFrame *keyframe, int frame_index);\n void addKeyFrameIntoVoc(KeyFrame *keyframe);\n void optimize4DoF();\n void optimize6DoF();\n void updatePath();\n list keyframelist;\n std::mutex m_keyframelist;\n std::mutex m_optimize_buf;\n std::mutex m_path;\n std::mutex m_drift;\n std::thread t_optimization;\n std::queue optimize_buf;\n\n int global_index;\n int sequence_cnt;\n vector sequence_loop;\n map image_pool;\n int earliest_loop_index;\n int base_sequence;\n bool use_imu;\n\n BriefDatabase db;\n BriefVocabulary *voc{};\n\n ros::Publisher pub_pg_path;\n ros::Publisher pub_base_path;\n ros::Publisher pub_pose_graph;\n ros::Publisher pub_path[10];\n};\n\ntemplate inline\nvoid QuaternionInverse(const T q[4], T q_inverse[4])\n{\n\tq_inverse[0] = q[0];\n\tq_inverse[1] = -q[1];\n\tq_inverse[2] = -q[2];\n\tq_inverse[3] = -q[3];\n};\n\ntemplate T NormalizeAngle(const T &angle_degrees) {\n if (angle_degrees > T(180.0))\n return angle_degrees - T(360.0);\n else if (angle_degrees < T(-180.0))\n return angle_degrees + T(360.0);\n else\n return angle_degrees;\n};\n\nclass AngleLocalParameterization {\npublic:\n template \n bool operator()(const T *theta_radians, const T *delta_theta_radians,\n T *theta_radians_plus_delta) const {\n *theta_radians_plus_delta =\n NormalizeAngle(*theta_radians + *delta_theta_radians);\n\n return true;\n }\n\n static ceres::LocalParameterization *Create() {\n return (new ceres::AutoDiffLocalParameterization);\n }\n};\n\ntemplate \nvoid YawPitchRollToRotationMatrix(const T yaw, const T pitch, const T roll,\n T R[9]) {\n\n T y = yaw / T(180.0) * T(M_PI);\n T p = pitch / T(180.0) * T(M_PI);\n T r = roll / T(180.0) * T(M_PI);\n\n R[0] = cos(y) * cos(p);\n R[1] = -sin(y) * cos(r) + cos(y) * sin(p) * sin(r);\n R[2] = sin(y) * sin(r) + cos(y) * sin(p) * cos(r);\n R[3] = sin(y) * cos(p);\n R[4] = cos(y) * cos(r) + sin(y) * sin(p) * sin(r);\n R[5] = -cos(y) * sin(r) + sin(y) * sin(p) * cos(r);\n R[6] = -sin(p);\n R[7] = cos(p) * sin(r);\n R[8] = cos(p) * cos(r);\n};\n\ntemplate void RotationMatrixTranspose(const T R[9], T inv_R[9]) {\n inv_R[0] = R[0];\n inv_R[1] = R[3];\n inv_R[2] = R[6];\n inv_R[3] = R[1];\n inv_R[4] = R[4];\n inv_R[5] = R[7];\n inv_R[6] = R[2];\n inv_R[7] = R[5];\n inv_R[8] = R[8];\n};\n\ntemplate \nvoid RotationMatrixRotatePoint(const T R[9], const T t[3], T r_t[3]) {\n r_t[0] = R[0] * t[0] + R[1] * t[1] + R[2] * t[2];\n r_t[1] = R[3] * t[0] + R[4] * t[1] + R[5] * t[2];\n r_t[2] = R[6] * t[0] + R[7] * t[1] + R[8] * t[2];\n};\n\nstruct FourDOFError {\n FourDOFError(double t_x, double t_y, double t_z, double relative_yaw,\n double pitch_i, double roll_i)\n : t_x(t_x), t_y(t_y), t_z(t_z), relative_yaw(relative_yaw),\n pitch_i(pitch_i), roll_i(roll_i) {}\n\n template \n bool operator()(const T *const yaw_i, const T *ti, const T *yaw_j,\n const T *tj, T *residuals) const {\n T t_w_ij[3];\n t_w_ij[0] = tj[0] - ti[0];\n t_w_ij[1] = tj[1] - ti[1];\n t_w_ij[2] = tj[2] - ti[2];\n\n // euler to rotation\n T w_R_i[9];\n YawPitchRollToRotationMatrix(yaw_i[0], T(pitch_i), T(roll_i), w_R_i);\n // rotation transpose\n T i_R_w[9];\n RotationMatrixTranspose(w_R_i, i_R_w);\n // rotation matrix rotate point\n T t_i_ij[3];\n RotationMatrixRotatePoint(i_R_w, t_w_ij, t_i_ij);\n\n residuals[0] = (t_i_ij[0] - T(t_x));\n residuals[1] = (t_i_ij[1] - T(t_y));\n residuals[2] = (t_i_ij[2] - T(t_z));\n residuals[3] = NormalizeAngle(yaw_j[0] - yaw_i[0] - T(relative_yaw));\n\n return true;\n }\n\n static ceres::CostFunction *\n Create(const double t_x, const double t_y, const double t_z,\n const double relative_yaw, const double pitch_i, const double roll_i) {\n return (new ceres::AutoDiffCostFunction(\n new FourDOFError(t_x, t_y, t_z, relative_yaw, pitch_i, roll_i)));\n }\n\n double t_x, t_y, t_z;\n double relative_yaw, pitch_i, roll_i;\n};\n\nstruct FourDOFWeightError {\n FourDOFWeightError(double t_x, double t_y, double t_z, double relative_yaw,\n double pitch_i, double roll_i)\n : t_x(t_x), t_y(t_y), t_z(t_z), relative_yaw(relative_yaw),\n pitch_i(pitch_i), roll_i(roll_i) {\n weight = 1;\n }\n\n template \n bool operator()(const T *const yaw_i, const T *ti, const T *yaw_j,\n const T *tj, T *residuals) const {\n T t_w_ij[3];\n t_w_ij[0] = tj[0] - ti[0];\n t_w_ij[1] = tj[1] - ti[1];\n t_w_ij[2] = tj[2] - ti[2];\n\n // euler to rotation\n T w_R_i[9];\n YawPitchRollToRotationMatrix(yaw_i[0], T(pitch_i), T(roll_i), w_R_i);\n // rotation transpose\n T i_R_w[9];\n RotationMatrixTranspose(w_R_i, i_R_w);\n // rotation matrix rotate point\n T t_i_ij[3];\n RotationMatrixRotatePoint(i_R_w, t_w_ij, t_i_ij);\n\n residuals[0] = (t_i_ij[0] - T(t_x)) * T(weight);\n residuals[1] = (t_i_ij[1] - T(t_y)) * T(weight);\n residuals[2] = (t_i_ij[2] - T(t_z)) * T(weight);\n residuals[3] = NormalizeAngle((yaw_j[0] - yaw_i[0] - T(relative_yaw))) *\n T(weight) / T(10.0);\n\n return true;\n }\n\n static ceres::CostFunction *\n Create(const double t_x, const double t_y, const double t_z,\n const double relative_yaw, const double pitch_i, const double roll_i) {\n return (new ceres::AutoDiffCostFunction(\n new FourDOFWeightError(t_x, t_y, t_z, relative_yaw, pitch_i, roll_i)));\n }\n\n double t_x, t_y, t_z;\n double relative_yaw, pitch_i, roll_i;\n double weight;\n};\n\nstruct RelativeRTError {\n RelativeRTError(double t_x, double t_y, double t_z, double q_w, double q_x,\n double q_y, double q_z, double t_var, double q_var)\n : 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),\n t_var(t_var), q_var(q_var) {}\n\n template \n bool operator()(const T *const w_q_i, const T *ti, const T *w_q_j,\n const T *tj, T *residuals) const {\n T t_w_ij[3];\n t_w_ij[0] = tj[0] - ti[0];\n t_w_ij[1] = tj[1] - ti[1];\n t_w_ij[2] = tj[2] - ti[2];\n\n T i_q_w[4];\n QuaternionInverse(w_q_i, i_q_w);\n\n T t_i_ij[3];\n ceres::QuaternionRotatePoint(i_q_w, t_w_ij, t_i_ij);\n\n residuals[0] = (t_i_ij[0] - T(t_x)) / T(t_var);\n residuals[1] = (t_i_ij[1] - T(t_y)) / T(t_var);\n residuals[2] = (t_i_ij[2] - T(t_z)) / T(t_var);\n\n T relative_q[4];\n relative_q[0] = T(q_w);\n relative_q[1] = T(q_x);\n relative_q[2] = T(q_y);\n relative_q[3] = T(q_z);\n\n T q_i_j[4];\n ceres::QuaternionProduct(i_q_w, w_q_j, q_i_j);\n\n T relative_q_inv[4];\n QuaternionInverse(relative_q, relative_q_inv);\n\n T error_q[4];\n ceres::QuaternionProduct(relative_q_inv, q_i_j, error_q);\n\n residuals[3] = T(2) * error_q[1] / T(q_var);\n residuals[4] = T(2) * error_q[2] / T(q_var);\n residuals[5] = T(2) * error_q[3] / T(q_var);\n\n return true;\n }\n\n static ceres::CostFunction *Create(const double t_x, const double t_y,\n const double t_z, const double q_w,\n const double q_x, const double q_y,\n const double q_z, const double t_var,\n const double q_var) {\n return (new ceres::AutoDiffCostFunction(\n new RelativeRTError(t_x, t_y, t_z, q_w, q_x, q_y, q_z, t_var, q_var)));\n }\n\n double t_x, t_y, t_z, t_norm;\n double q_w, q_x, q_y, q_z;\n double t_var, q_var;\n};" }, { "path": "pose_graph/src/pose_graph_nodelet.cpp", "content": "#include \n#include \n#include \n#include \n#include \n#include \n#include \n\n#include \n\n#include \"keyframe/keyframe.h\"\n#include \"pose_graph/pose_graph.h\"\n#include \"utility/CameraPoseVisualization.h\"\n#include \"utility/tic_toc.h\"\n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n\n#include // 基类Nodelet所在的头文件\n#include \n\n#define SKIP_FIRST_CNT 10\n\ncamodocal::CameraPtr m_camera;\nEigen::Vector3d tic;\nEigen::Matrix3d qic;\nEigen::Matrix ti_d;\nEigen::Matrix qi_d;\nros::Publisher pub_match_img;\nros::Publisher pub_match_points;\nint VISUALIZATION_SHIFT_X;\nint VISUALIZATION_SHIFT_Y;\nstd::string BRIEF_PATTERN_FILE;\nstd::string POSE_GRAPH_SAVE_PATH;\nint ROW;\nint COL;\nstd::string VINS_RESULT_PATH;\nint DEBUG_IMAGE;\nint FAST_RELOCALIZATION;\nnamespace pose_graph_nodelet_ns {\nclass PoseGraphNodelet\n : public nodelet::Nodelet //任何nodelet plugin都要继承Nodelet类。\n{\npublic:\n PoseGraphNodelet() {\n frame_index = 0;\n sequence = 1;\n skip_first_cnt = 0;\n skip_cnt = 0;\n load_flag = 0;\n start_flag = 0;\n SKIP_DIS = 0;\n\n last_image_time = -1;\n\n last_t = Eigen::Vector3d(-100, -100, -100);\n\n cameraposevisual = new CameraPoseVisualization(1, 0, 0, 1);\n }\n\nprivate:\n void onInit() override {\n\n ros::NodeHandle &pn = getPrivateNodeHandle();\n ros::NodeHandle &nh = getMTNodeHandle();\n posegraph.registerPub(nh);\n\n // read param\n pn.getParam(\"visualization_shift_x\", VISUALIZATION_SHIFT_X);\n pn.getParam(\"visualization_shift_y\", VISUALIZATION_SHIFT_Y);\n pn.getParam(\"skip_cnt\", SKIP_CNT);\n pn.getParam(\"skip_dis\", SKIP_DIS);\n std::string config_file;\n pn.getParam(\"config_file\", config_file);\n\n cv::FileStorage fsSettings(config_file, cv::FileStorage::READ);\n if (!fsSettings.isOpened()) {\n std::cerr << \"ERROR: Wrong path to settings\" << std::endl;\n }\n\n double camera_visual_size = fsSettings[\"visualize_camera_size\"];\n cameraposevisual->setScale(camera_visual_size);\n cameraposevisual->setLineWidth(camera_visual_size / 10.0);\n\n LOOP_CLOSURE = fsSettings[\"loop_closure\"];\n std::string IMAGE_TOPIC;\n int LOAD_PREVIOUS_POSE_GRAPH;\n // prepare for loop closure (load vocabulary, set topic, etc)\n if (LOOP_CLOSURE) {\n ROW = fsSettings[\"image_height\"];\n COL = fsSettings[\"image_width\"];\n\n std::string pkg_path = ros::package::getPath(\"pose_graph\");\n string vocabulary_file = pkg_path + \"/../support_files/brief_k10L6.bin\";\n cout << \"vocabulary_file\" << vocabulary_file << endl;\n posegraph.loadVocabulary(vocabulary_file);\n\n BRIEF_PATTERN_FILE = pkg_path + \"/../support_files/brief_pattern.yml\";\n cout << \"BRIEF_PATTERN_FILE\" << BRIEF_PATTERN_FILE << endl;\n m_camera =\n camodocal::CameraFactory::instance()->generateCameraFromYamlFile(\n config_file.c_str());\n\n fsSettings[\"image_topic\"] >> IMAGE_TOPIC;\n fsSettings[\"pose_graph_save_path\"] >> POSE_GRAPH_SAVE_PATH;\n fsSettings[\"output_path\"] >> VINS_RESULT_PATH;\n fsSettings[\"save_image\"] >> DEBUG_IMAGE;\n\n cv::Mat cv_qid, cv_tid;\n fsSettings[\"extrinsicRotation\"] >> cv_qid;\n fsSettings[\"extrinsicTranslation\"] >> cv_tid;\n cv::cv2eigen(cv_qid, qi_d);\n cv::cv2eigen(cv_tid, ti_d);\n\n int USE_IMU = fsSettings[\"imu\"];\n posegraph.setIMUFlag(USE_IMU);\n\n VISUALIZE_IMU_FORWARD = fsSettings[\"visualize_imu_forward\"];\n LOAD_PREVIOUS_POSE_GRAPH = fsSettings[\"load_previous_pose_graph\"];\n FAST_RELOCALIZATION = fsSettings[\"fast_relocalization\"];\n VINS_RESULT_PATH = VINS_RESULT_PATH + \"/vins_result_loop.csv\";\n std::ofstream fout(VINS_RESULT_PATH, std::ios::out);\n fout.close();\n fsSettings.release();\n // not used\n if (LOAD_PREVIOUS_POSE_GRAPH) {\n printf(\"load pose graph\\n\");\n m_process.lock();\n posegraph.loadPoseGraph();\n m_process.unlock();\n printf(\"load pose graph finish\\n\");\n load_flag = true;\n } else {\n printf(\"no previous pose graph\\n\");\n load_flag = true;\n }\n }\n\n fsSettings.release();\n // publish camera pose by imu propagate and odometry (Ps and Rs of curr\n // frame) not important\n\n sub_imu_forward =\n nh.subscribe(\"/vins_estimator/imu_propagate\", 100,\n &PoseGraphNodelet::imu_forward_callback, this);\n // odometry_buf\n sub_vio = nh.subscribe(\"/vins_estimator/odometry\", 100,\n &PoseGraphNodelet::vio_callback, this);\n\n sub_image =\n nh.subscribe(IMAGE_TOPIC, 100, &PoseGraphNodelet::image_callback, this);\n\n // get keyframe_pose(Ps and Rs), store in pose_buf (marginalization_flag ==\n // 0)\n sub_pose = nh.subscribe(\"/vins_estimator/keyframe_pose\", 100,\n &PoseGraphNodelet::pose_callback, this);\n // get extrinsic (ric qic odometry.pose.pose.position and\n // odometry.pose.pose.orientation) update tic and qic real-time\n sub_extrinsic = nh.subscribe(\"/vins_estimator/extrinsic\", 100,\n &PoseGraphNodelet::extrinsic_callback, this);\n // get keyframe_point(pointclude), store in point_buf (marginalization_flag\n // == 0)\n sub_point = nh.subscribe(\"/vins_estimator/keyframe_point\", 100,\n &PoseGraphNodelet::point_callback, this);\n\n // do relocalization here.\n // pose_graph publish match_points to vins_estimator, estimator then publish\n // relo_relative_pose\n sub_relo_relative_pose =\n nh.subscribe(\"/vins_estimator/relo_relative_pose\", 100,\n &PoseGraphNodelet::relo_relative_pose_callback, this);\n\n pub_match_img = nh.advertise(\"match_image\", 100);\n pub_camera_pose_visual = nh.advertise(\n \"/pose_graph/camera_pose_visual\", 100);\n pub_key_odometrys = nh.advertise(\n \"/pose_graph/key_odometrys\", 100);\n // not used\n pub_vio_path =\n nh.advertise(\"/pose_graph/no_loop_path\", 100);\n pub_match_points =\n nh.advertise(\"/pose_graph/match_points\", 100);\n\n // main thread\n measurement_process = std::thread(&PoseGraphNodelet::process, this);\n // not used\n keyboard_command_process = std::thread(&PoseGraphNodelet::command, this);\n }\n\n ros::Subscriber sub_image, sub_imu_forward, sub_vio;\n ros::Subscriber sub_pose, sub_extrinsic, sub_point, sub_relo_relative_pose;\n\n std::thread measurement_process, keyboard_command_process;\n\n queue image_buf;\n queue point_buf;\n queue pose_buf;\n queue odometry_buf;\n std::mutex m_buf;\n std::mutex m_process;\n int frame_index;\n int sequence;\n PoseGraph posegraph;\n int skip_first_cnt;\n int SKIP_CNT{};\n int skip_cnt;\n bool load_flag;\n bool start_flag;\n double SKIP_DIS;\n\n int VISUALIZE_IMU_FORWARD{};\n int LOOP_CLOSURE{};\n\n ros::Publisher pub_camera_pose_visual;\n ros::Publisher pub_key_odometrys;\n ros::Publisher pub_vio_path;\n nav_msgs::Path no_loop_path;\n\n CameraPoseVisualization *cameraposevisual;\n\n Eigen::Vector3d last_t; //(-100, -100, -100);\n double last_image_time;\n\n // not used in my case, just ignore sequence 1-5\n void new_sequence() {\n printf(\"new sequence\\n\");\n sequence++;\n printf(\"sequence cnt %d \\n\", sequence);\n if (sequence > 5) {\n ROS_WARN(\"only support 5 sequences since it's boring to copy code for \"\n \"more sequences.\");\n ROS_BREAK();\n }\n posegraph.posegraph_visualization->reset();\n posegraph.publish();\n m_buf.lock();\n while (!image_buf.empty())\n image_buf.pop();\n while (!point_buf.empty())\n point_buf.pop();\n while (!pose_buf.empty())\n pose_buf.pop();\n while (!odometry_buf.empty())\n odometry_buf.pop();\n m_buf.unlock();\n }\n\n void image_callback(const sensor_msgs::ImageConstPtr &image_msg) {\n // ROS_WARN(\"image_callback!\");\n if (!LOOP_CLOSURE)\n return;\n m_buf.lock();\n image_buf.push(image_msg);\n m_buf.unlock();\n // printf(\" image time %f \\n\", image_msg->header.stamp.toSec());\n\n // detect unstable camera stream\n if (last_image_time == -1)\n last_image_time = image_msg->header.stamp.toSec();\n else if (image_msg->header.stamp.toSec() - last_image_time > 1.0 ||\n image_msg->header.stamp.toSec() < last_image_time) {\n ROS_WARN(\"image discontinue! detect a new sequence!\");\n new_sequence();\n }\n last_image_time = image_msg->header.stamp.toSec();\n }\n\n void point_callback(const sensor_msgs::PointCloudConstPtr &point_msg) {\n // ROS_INFO(\"point_callback!\");\n if (!LOOP_CLOSURE)\n return;\n m_buf.lock();\n point_buf.push(point_msg);\n m_buf.unlock();\n /*\n for (unsigned int i = 0; i < point_msg->points.size(); i++)\n {\n printf(\"%d, 3D point: %f, %f, %f 2D point %f, %f \\n\",i ,\n point_msg->points[i].x, point_msg->points[i].y, point_msg->points[i].z,\n point_msg->channels[i].values[0],\n point_msg->channels[i].values[1]);\n }\n */\n }\n\n void pose_callback(const nav_msgs::Odometry::ConstPtr &pose_msg) {\n // ROS_INFO(\"pose_callback!\");\n if (!LOOP_CLOSURE)\n return;\n m_buf.lock();\n pose_buf.push(pose_msg);\n m_buf.unlock();\n /*\n printf(\"pose t: %f, %f, %f q: %f, %f, %f %f \\n\",\n pose_msg->pose.pose.position.x, pose_msg->pose.pose.position.y,\n pose_msg->pose.pose.position.z,\n pose_msg->pose.pose.orientation.w,\n pose_msg->pose.pose.orientation.x,\n pose_msg->pose.pose.orientation.y,\n pose_msg->pose.pose.orientation.z);\n */\n }\n\n // not used\n void imu_forward_callback(const nav_msgs::Odometry::ConstPtr &forward_msg) {\n if (VISUALIZE_IMU_FORWARD) {\n Vector3d vio_t(forward_msg->pose.pose.position.x,\n forward_msg->pose.pose.position.y,\n forward_msg->pose.pose.position.z);\n Quaterniond vio_q;\n vio_q.w() = forward_msg->pose.pose.orientation.w;\n vio_q.x() = forward_msg->pose.pose.orientation.x;\n vio_q.y() = forward_msg->pose.pose.orientation.y;\n vio_q.z() = forward_msg->pose.pose.orientation.z;\n\n vio_t = posegraph.w_r_vio * vio_t + posegraph.w_t_vio;\n vio_q = posegraph.w_r_vio * vio_q;\n\n vio_t = posegraph.r_drift * vio_t + posegraph.t_drift;\n vio_q = posegraph.r_drift * vio_q;\n\n Vector3d vio_t_cam;\n Quaterniond vio_q_cam;\n vio_t_cam = vio_t + vio_q * tic;\n vio_q_cam = vio_q * qic;\n\n cameraposevisual->reset();\n cameraposevisual->add_pose(vio_t_cam, vio_q_cam);\n cameraposevisual->publish_by(pub_camera_pose_visual, forward_msg->header);\n }\n }\n\n void\n relo_relative_pose_callback(const nav_msgs::Odometry::ConstPtr &pose_msg) {\n Vector3d relative_t =\n Vector3d(pose_msg->pose.pose.position.x, pose_msg->pose.pose.position.y,\n pose_msg->pose.pose.position.z);\n Quaterniond relative_q;\n relative_q.w() = pose_msg->pose.pose.orientation.w;\n relative_q.x() = pose_msg->pose.pose.orientation.x;\n relative_q.y() = pose_msg->pose.pose.orientation.y;\n relative_q.z() = pose_msg->pose.pose.orientation.z;\n double relative_yaw = pose_msg->twist.twist.linear.x;\n int index = pose_msg->twist.twist.linear.y;\n // printf(\"receive index %d \\n\", index );\n Eigen::Matrix loop_info;\n loop_info << relative_t.x(), relative_t.y(), relative_t.z(), relative_q.w(),\n relative_q.x(), relative_q.y(), relative_q.z(), relative_yaw;\n posegraph.updateKeyFrameLoop(index, loop_info);\n }\n\n void vio_callback(const nav_msgs::Odometry::ConstPtr &pose_msg) {\n // ROS_INFO(\"vio_callback!\");\n Vector3d vio_t(pose_msg->pose.pose.position.x,\n pose_msg->pose.pose.position.y,\n pose_msg->pose.pose.position.z);\n Quaterniond vio_q;\n vio_q.w() = pose_msg->pose.pose.orientation.w;\n vio_q.x() = pose_msg->pose.pose.orientation.x;\n vio_q.y() = pose_msg->pose.pose.orientation.y;\n vio_q.z() = pose_msg->pose.pose.orientation.z;\n\n vio_t = posegraph.w_r_vio * vio_t + posegraph.w_t_vio;\n vio_q = posegraph.w_r_vio * vio_q;\n\n vio_t = posegraph.r_drift * vio_t + posegraph.t_drift;\n vio_q = posegraph.r_drift * vio_q;\n\n Vector3d vio_t_cam;\n Quaterniond vio_q_cam;\n vio_t_cam = vio_t + vio_q * tic;\n vio_q_cam = vio_q * qic;\n\n if (!VISUALIZE_IMU_FORWARD) {\n cameraposevisual->reset();\n cameraposevisual->add_pose(vio_t_cam, vio_q_cam);\n cameraposevisual->publish_by(pub_camera_pose_visual, pose_msg->header);\n }\n\n odometry_buf.push(vio_t_cam);\n if (odometry_buf.size() > 10) {\n odometry_buf.pop();\n }\n\n visualization_msgs::Marker key_odometrys;\n key_odometrys.header = pose_msg->header;\n key_odometrys.header.frame_id = \"map\";\n key_odometrys.ns = \"key_odometrys\";\n key_odometrys.type = visualization_msgs::Marker::SPHERE_LIST;\n key_odometrys.action = visualization_msgs::Marker::ADD;\n key_odometrys.pose.orientation.w = 1.0;\n key_odometrys.lifetime = ros::Duration();\n\n // static int key_odometrys_id = 0;\n key_odometrys.id = 0; // key_odometrys_id++;\n key_odometrys.scale.x = 0.1;\n key_odometrys.scale.y = 0.1;\n key_odometrys.scale.z = 0.1;\n key_odometrys.color.r = 1.0;\n key_odometrys.color.a = 1.0;\n\n for (unsigned int i = 0; i < odometry_buf.size(); i++) {\n geometry_msgs::Point pose_marker;\n Vector3d vio_t;\n vio_t = odometry_buf.front();\n odometry_buf.pop();\n pose_marker.x = vio_t.x();\n pose_marker.y = vio_t.y();\n pose_marker.z = vio_t.z();\n key_odometrys.points.push_back(pose_marker);\n odometry_buf.push(vio_t);\n }\n pub_key_odometrys.publish(key_odometrys);\n\n // not used\n if (!LOOP_CLOSURE) {\n geometry_msgs::PoseStamped pose_stamped;\n pose_stamped.header = pose_msg->header;\n pose_stamped.header.frame_id = \"map\";\n pose_stamped.pose.position.x = vio_t.x();\n pose_stamped.pose.position.y = vio_t.y();\n pose_stamped.pose.position.z = vio_t.z();\n no_loop_path.header = pose_msg->header;\n no_loop_path.header.frame_id = \"map\";\n no_loop_path.poses.push_back(pose_stamped);\n pub_vio_path.publish(no_loop_path);\n }\n }\n\n void extrinsic_callback(const nav_msgs::Odometry::ConstPtr &pose_msg) {\n m_process.lock();\n tic =\n Vector3d(pose_msg->pose.pose.position.x, pose_msg->pose.pose.position.y,\n pose_msg->pose.pose.position.z);\n qic = Quaterniond(pose_msg->pose.pose.orientation.w,\n pose_msg->pose.pose.orientation.x,\n pose_msg->pose.pose.orientation.y,\n pose_msg->pose.pose.orientation.z)\n .toRotationMatrix();\n m_process.unlock();\n }\n\n void process() {\n if (!LOOP_CLOSURE)\n return;\n while (true) {\n sensor_msgs::ImageConstPtr image_msg = NULL;\n sensor_msgs::PointCloudConstPtr point_msg = NULL;\n nav_msgs::Odometry::ConstPtr pose_msg = NULL;\n // find out the messages with same time stamp\n m_buf.lock();\n // get image_msg, pose_msg and point_msg\n if (!image_buf.empty() && !point_buf.empty() && !pose_buf.empty()) {\n if (image_buf.front()->header.stamp.toSec() >\n pose_buf.front()->header.stamp.toSec()) {\n pose_buf.pop();\n printf(\"throw pose at beginning\\n\");\n } else if (image_buf.front()->header.stamp.toSec() >\n point_buf.front()->header.stamp.toSec()) {\n point_buf.pop();\n printf(\"throw point at beginning\\n\");\n } else if (image_buf.back()->header.stamp.toSec() >=\n pose_buf.front()->header.stamp.toSec() &&\n point_buf.back()->header.stamp.toSec() >=\n pose_buf.front()->header.stamp.toSec()) {\n pose_msg = pose_buf.front();\n pose_buf.pop();\n while (!pose_buf.empty())\n pose_buf.pop();\n while (image_buf.front()->header.stamp.toSec() <\n pose_msg->header.stamp.toSec()) {\n image_buf.pop();\n }\n image_msg = image_buf.front();\n image_buf.pop();\n\n while (point_buf.front()->header.stamp.toSec() <\n pose_msg->header.stamp.toSec())\n point_buf.pop();\n point_msg = point_buf.front();\n point_buf.pop();\n }\n }\n m_buf.unlock();\n if (pose_msg != NULL) {\n // printf(\" pose time %f \\n\", pose_msg->header.stamp.toSec());\n // printf(\" point time %f \\n\", point_msg->header.stamp.toSec());\n // printf(\" image time %f \\n\", image_msg->header.stamp.toSec());\n // skip fisrt few\n if (skip_first_cnt < SKIP_FIRST_CNT) {\n skip_first_cnt++;\n continue;\n }\n\n if (skip_cnt < SKIP_CNT) {\n skip_cnt++;\n continue;\n } else {\n skip_cnt = 0;\n }\n\n cv::Mat image =\n cv_bridge::toCvCopy(image_msg, sensor_msgs::image_encodings::MONO8)\n ->image;\n // build keyframe\n Vector3d T = Vector3d(pose_msg->pose.pose.position.x,\n pose_msg->pose.pose.position.y,\n pose_msg->pose.pose.position.z);\n Matrix3d R = Quaterniond(pose_msg->pose.pose.orientation.w,\n pose_msg->pose.pose.orientation.x,\n pose_msg->pose.pose.orientation.y,\n pose_msg->pose.pose.orientation.z)\n .toRotationMatrix();\n\n //将距上一关键帧距离（平移向量的模）超过SKIP_DIS的图像创建为关键帧\n if ((T - last_t).norm() > SKIP_DIS) {\n vector point_3d;\n vector point_2d_uv;\n vector point_2d_normal;\n vector point_id;\n\n for (unsigned int i = 0; i < point_msg->points.size(); i++) {\n cv::Point3f p_3d;\n p_3d.x = point_msg->points[i].x;\n p_3d.y = point_msg->points[i].y;\n p_3d.z = point_msg->points[i].z;\n point_3d.push_back(p_3d);\n\n cv::Point2f p_2d_uv, p_2d_normal;\n double p_id;\n p_2d_normal.x = point_msg->channels[i].values[0];\n p_2d_normal.y = point_msg->channels[i].values[1];\n p_2d_uv.x = point_msg->channels[i].values[2];\n p_2d_uv.y = point_msg->channels[i].values[3];\n p_id = point_msg->channels[i].values[4];\n point_2d_normal.push_back(p_2d_normal);\n point_2d_uv.push_back(p_2d_uv);\n point_id.push_back(p_id);\n\n // printf(\"u %f, v %f \\n\", p_2d_uv.x, p_2d_uv.y);\n }\n\n // 通过frame_index标记对应帧\n KeyFrame *keyframe = new KeyFrame(\n pose_msg->header.stamp.toSec(), frame_index, T, R, image,\n point_3d, point_2d_uv, point_2d_normal, point_id, sequence);\n m_process.lock();\n start_flag = 1;\n //在posegraph中添加关键帧，flag_detect_loop=1回环检测\n posegraph.addKeyFrame(keyframe, 1);\n m_process.unlock();\n frame_index++;\n last_t = T;\n }\n }\n\n std::chrono::milliseconds dura(5);\n std::this_thread::sleep_for(dura);\n }\n }\n\n void command() {\n if (!LOOP_CLOSURE)\n return;\n while (1) {\n char c = getchar();\n if (c == 's') {\n m_process.lock();\n posegraph.savePoseGraph();\n m_process.unlock();\n printf(\"save pose graph finish\\nyou can set 'load_previous_pose_graph' \"\n \"to 1 in the config file to reuse it next time\\n\");\n printf(\"program shutting down...\\n\");\n ros::shutdown();\n }\n if (c == 'n')\n new_sequence();\n\n std::chrono::milliseconds dura(5);\n std::this_thread::sleep_for(dura);\n }\n }\n};\nPLUGINLIB_EXPORT_CLASS(pose_graph_nodelet_ns::PoseGraphNodelet,\n nodelet::Nodelet)\n} // namespace pose_graph_nodelet_ns\n" }, { "path": "pose_graph/src/utility/CameraPoseVisualization.cpp", "content": "#include \"CameraPoseVisualization.h\"\n\nconst Eigen::Vector3d CameraPoseVisualization::imlt = Eigen::Vector3d(-1.0, -0.5, 1.0);\nconst Eigen::Vector3d CameraPoseVisualization::imrt = Eigen::Vector3d( 1.0, -0.5, 1.0);\nconst Eigen::Vector3d CameraPoseVisualization::imlb = Eigen::Vector3d(-1.0, 0.5, 1.0);\nconst Eigen::Vector3d CameraPoseVisualization::imrb = Eigen::Vector3d( 1.0, 0.5, 1.0);\nconst Eigen::Vector3d CameraPoseVisualization::lt0 = Eigen::Vector3d(-0.7, -0.5, 1.0);\nconst Eigen::Vector3d CameraPoseVisualization::lt1 = Eigen::Vector3d(-0.7, -0.2, 1.0);\nconst Eigen::Vector3d CameraPoseVisualization::lt2 = Eigen::Vector3d(-1.0, -0.2, 1.0);\nconst Eigen::Vector3d CameraPoseVisualization::oc = Eigen::Vector3d(0.0, 0.0, 0.0);\n\nvoid Eigen2Point(const Eigen::Vector3d& v, geometry_msgs::Point& p) {\n p.x = v.x();\n p.y = v.y();\n p.z = v.z();\n}\n\nCameraPoseVisualization::CameraPoseVisualization(float r, float g, float b, float a)\n : m_marker_ns(\"CameraPoseVisualization\"), m_scale(0.2), m_line_width(0.01) {\n m_image_boundary_color.r = r;\n m_image_boundary_color.g = g;\n m_image_boundary_color.b = b;\n m_image_boundary_color.a = a;\n m_optical_center_connector_color.r = r;\n m_optical_center_connector_color.g = g;\n m_optical_center_connector_color.b = b;\n m_optical_center_connector_color.a = a;\n}\n\nvoid CameraPoseVisualization::setImageBoundaryColor(float r, float g, float b, float a) {\n m_image_boundary_color.r = r;\n m_image_boundary_color.g = g;\n m_image_boundary_color.b = b;\n m_image_boundary_color.a = a;\n}\n\nvoid CameraPoseVisualization::setOpticalCenterConnectorColor(float r, float g, float b, float a) {\n m_optical_center_connector_color.r = r;\n m_optical_center_connector_color.g = g;\n m_optical_center_connector_color.b = b;\n m_optical_center_connector_color.a = a;\n}\n\nvoid CameraPoseVisualization::setScale(double s) {\n m_scale = s;\n}\nvoid CameraPoseVisualization::setLineWidth(double width) {\n m_line_width = width;\n}\nvoid CameraPoseVisualization::add_edge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1){\n visualization_msgs::Marker marker;\n\n marker.ns = m_marker_ns;\n marker.id = m_markers.size() + 1;\n marker.type = visualization_msgs::Marker::LINE_LIST;\n marker.action = visualization_msgs::Marker::ADD;\n marker.scale.x = 0.01;\n\n marker.color.b = 1.0f;\n marker.color.a = 1.0;\n\n geometry_msgs::Point point0, point1;\n\n Eigen2Point(p0, point0);\n Eigen2Point(p1, point1);\n\n marker.points.push_back(point0);\n marker.points.push_back(point1);\n\n m_markers.push_back(marker);\n}\n\nvoid CameraPoseVisualization::add_loopedge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1){\n //m_markers.clear();\n visualization_msgs::Marker marker;\n\n marker.ns = m_marker_ns;\n marker.id = m_markers.size() + 1;\n marker.type = visualization_msgs::Marker::LINE_STRIP;\n marker.action = visualization_msgs::Marker::ADD;\n marker.lifetime = ros::Duration();\n //marker.scale.x = 0.4;\n marker.scale.x = 0.02;\n marker.color.r = 1.0f;\n //marker.color.g = 1.0f;\n //marker.color.b = 1.0f;\n marker.color.a = 1.0;\n\n geometry_msgs::Point point0, point1;\n\n Eigen2Point(p0, point0);\n Eigen2Point(p1, point1);\n\n marker.points.push_back(point0);\n marker.points.push_back(point1);\n\n m_markers.push_back(marker);\n}\n\n\nvoid CameraPoseVisualization::add_pose(const Eigen::Vector3d& p, const Eigen::Quaterniond& q) {\n visualization_msgs::Marker marker;\n\n marker.ns = m_marker_ns;\n marker.id = 0;\n marker.type = visualization_msgs::Marker::LINE_STRIP;\n marker.action = visualization_msgs::Marker::ADD;\n marker.scale.x = m_line_width;\n\n marker.pose.position.x = 0.0;\n marker.pose.position.y = 0.0;\n marker.pose.position.z = 0.0;\n marker.pose.orientation.w = 1.0;\n marker.pose.orientation.x = 0.0;\n marker.pose.orientation.y = 0.0;\n marker.pose.orientation.z = 0.0;\n\n\n geometry_msgs::Point pt_lt, pt_lb, pt_rt, pt_rb, pt_oc, pt_lt0, pt_lt1, pt_lt2;\n\n Eigen2Point(q * (m_scale *imlt) + p, pt_lt);\n Eigen2Point(q * (m_scale *imlb) + p, pt_lb);\n Eigen2Point(q * (m_scale *imrt) + p, pt_rt);\n Eigen2Point(q * (m_scale *imrb) + p, pt_rb);\n Eigen2Point(q * (m_scale *lt0 ) + p, pt_lt0);\n Eigen2Point(q * (m_scale *lt1 ) + p, pt_lt1);\n Eigen2Point(q * (m_scale *lt2 ) + p, pt_lt2);\n Eigen2Point(q * (m_scale *oc ) + p, pt_oc);\n\n // image boundaries\n marker.points.push_back(pt_lt);\n marker.points.push_back(pt_lb);\n marker.colors.push_back(m_image_boundary_color);\n marker.colors.push_back(m_image_boundary_color);\n\n marker.points.push_back(pt_lb);\n marker.points.push_back(pt_rb);\n marker.colors.push_back(m_image_boundary_color);\n marker.colors.push_back(m_image_boundary_color);\n\n marker.points.push_back(pt_rb);\n marker.points.push_back(pt_rt);\n marker.colors.push_back(m_image_boundary_color);\n marker.colors.push_back(m_image_boundary_color);\n\n marker.points.push_back(pt_rt);\n marker.points.push_back(pt_lt);\n marker.colors.push_back(m_image_boundary_color);\n marker.colors.push_back(m_image_boundary_color);\n\n // top-left indicator\n marker.points.push_back(pt_lt0);\n marker.points.push_back(pt_lt1);\n marker.colors.push_back(m_image_boundary_color);\n marker.colors.push_back(m_image_boundary_color);\n\n marker.points.push_back(pt_lt1);\n marker.points.push_back(pt_lt2);\n marker.colors.push_back(m_image_boundary_color);\n marker.colors.push_back(m_image_boundary_color);\n\n // optical center connector\n marker.points.push_back(pt_lt);\n marker.points.push_back(pt_oc);\n marker.colors.push_back(m_optical_center_connector_color);\n marker.colors.push_back(m_optical_center_connector_color);\n\n\n marker.points.push_back(pt_lb);\n marker.points.push_back(pt_oc);\n marker.colors.push_back(m_optical_center_connector_color);\n marker.colors.push_back(m_optical_center_connector_color);\n\n marker.points.push_back(pt_rt);\n marker.points.push_back(pt_oc);\n marker.colors.push_back(m_optical_center_connector_color);\n marker.colors.push_back(m_optical_center_connector_color);\n\n marker.points.push_back(pt_rb);\n marker.points.push_back(pt_oc);\n marker.colors.push_back(m_optical_center_connector_color);\n marker.colors.push_back(m_optical_center_connector_color);\n\n m_markers.push_back(marker);\n}\n\nvoid CameraPoseVisualization::reset() {\n\tm_markers.clear();\n //image.points.clear();\n //image.colors.clear();\n}\n\nvoid CameraPoseVisualization::publish_by( ros::Publisher &pub, const std_msgs::Header &header ) {\n\tvisualization_msgs::MarkerArray markerArray_msg;\n\t//int k = (int)m_markers.size();\n /*\n for (int i = 0; i < 5 && k > 0; i++)\n {\n k--;\n m_markers[k].header = header;\n markerArray_msg.markers.push_back(m_markers[k]);\n }\n */\n\n \n\tfor(auto& marker : m_markers) {\n\t\tmarker.header = header;\n\t\tmarkerArray_msg.markers.push_back(marker);\n\t}\n \n\tpub.publish(markerArray_msg);\n}\n" }, { "path": "pose_graph/src/utility/CameraPoseVisualization.h", "content": "#pragma once\n\n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \"parameters.h\"\n\nclass CameraPoseVisualization {\npublic:\n\tstd::string m_marker_ns;\n\n\tCameraPoseVisualization(float r, float g, float b, float a);\n\t\n\tvoid setImageBoundaryColor(float r, float g, float b, float a=1.0);\n\tvoid setOpticalCenterConnectorColor(float r, float g, float b, float a=1.0);\n\tvoid setScale(double s);\n\tvoid setLineWidth(double width);\n\n\tvoid add_pose(const Eigen::Vector3d& p, const Eigen::Quaterniond& q);\n\tvoid reset();\n\n\tvoid publish_by(ros::Publisher& pub, const std_msgs::Header& header);\n\tvoid add_edge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1);\n\tvoid add_loopedge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1);\n\t//void add_image(const Eigen::Vector3d& T, const Eigen::Matrix3d& R, const cv::Mat &src);\n\tvoid publish_image_by( ros::Publisher &pub, const std_msgs::Header &header);\nprivate:\n\tstd::vector m_markers;\n\tstd_msgs::ColorRGBA m_image_boundary_color;\n\tstd_msgs::ColorRGBA m_optical_center_connector_color;\n\tdouble m_scale;\n\tdouble m_line_width;\n\n\tstatic const Eigen::Vector3d imlt;\n\tstatic const Eigen::Vector3d imlb;\n\tstatic const Eigen::Vector3d imrt;\n\tstatic const Eigen::Vector3d imrb;\n\tstatic const Eigen::Vector3d oc ;\n\tstatic const Eigen::Vector3d lt0 ;\n\tstatic const Eigen::Vector3d lt1 ;\n\tstatic const Eigen::Vector3d lt2 ;\n};\n" }, { "path": "pose_graph/src/utility/parameters.h", "content": "#pragma once\n\n#include \"camodocal/camera_models/CameraFactory.h\"\n#include \"camodocal/camera_models/CataCamera.h\"\n#include \"camodocal/camera_models/PinholeCamera.h\"\n#include \n#include \n#include \n#include \n#include \n#include \n\nextern camodocal::CameraPtr m_camera;\nextern Eigen::Vector3d tic;\nextern Eigen::Matrix3d qic;\nextern Eigen::Matrix ti_d;\nextern Eigen::Matrix qi_d;\nextern ros::Publisher pub_match_img;\nextern ros::Publisher pub_match_points;\nextern int VISUALIZATION_SHIFT_X;\nextern int VISUALIZATION_SHIFT_Y;\nextern std::string BRIEF_PATTERN_FILE;\nextern std::string POSE_GRAPH_SAVE_PATH;\nextern int ROW;\nextern int COL;\nextern std::string VINS_RESULT_PATH;\nextern int DEBUG_IMAGE;\nextern int FAST_RELOCALIZATION;\n\n\n" }, { "path": "pose_graph/src/utility/tic_toc.h", "content": "#pragma once\n\n#include \n#include \n#include \n\nclass TicToc\n{\n public:\n TicToc()\n {\n tic();\n }\n\n void tic()\n {\n start = std::chrono::system_clock::now();\n }\n\n double toc()\n {\n end = std::chrono::system_clock::now();\n std::chrono::duration elapsed_seconds = end - start;\n return elapsed_seconds.count() * 1000;\n }\n\n private:\n std::chrono::time_point start, end;\n};\n" }, { "path": "pose_graph/src/utility/utility.cpp", "content": "#include \"utility.h\"\n\nEigen::Matrix3d Utility::g2R(const Eigen::Vector3d &g)\n{\n Eigen::Matrix3d R0;\n Eigen::Vector3d ng1 = g.normalized();\n Eigen::Vector3d ng2{0, 0, 1.0};\n R0 = Eigen::Quaterniond::FromTwoVectors(ng1, ng2).toRotationMatrix();\n double yaw = Utility::R2ypr(R0).x();\n R0 = Utility::ypr2R(Eigen::Vector3d{-yaw, 0, 0}) * R0;\n // R0 = Utility::ypr2R(Eigen::Vector3d{-90, 0, 0}) * R0;\n return R0;\n}\n" }, { "path": "pose_graph/src/utility/utility.h", "content": "#pragma once\n\n#include \n#include \n#include \n#include \n\nclass Utility\n{\n public:\n template \n static Eigen::Quaternion deltaQ(const Eigen::MatrixBase &theta)\n {\n typedef typename Derived::Scalar Scalar_t;\n\n Eigen::Quaternion dq;\n Eigen::Matrix half_theta = theta;\n half_theta /= static_cast(2.0);\n dq.w() = static_cast(1.0);\n dq.x() = half_theta.x();\n dq.y() = half_theta.y();\n dq.z() = half_theta.z();\n return dq;\n }\n\n template \n static Eigen::Matrix skewSymmetric(const Eigen::MatrixBase &q)\n {\n Eigen::Matrix ans;\n ans << typename Derived::Scalar(0), -q(2), q(1),\n q(2), typename Derived::Scalar(0), -q(0),\n -q(1), q(0), typename Derived::Scalar(0);\n return ans;\n }\n\n template \n static Eigen::Quaternion positify(const Eigen::QuaternionBase &q)\n {\n //printf(\"a: %f %f %f %f\", q.w(), q.x(), q.y(), q.z());\n //Eigen::Quaternion p(-q.w(), -q.x(), -q.y(), -q.z());\n //printf(\"b: %f %f %f %f\", p.w(), p.x(), p.y(), p.z());\n //return q.template w() >= (typename Derived::Scalar)(0.0) ? q : Eigen::Quaternion(-q.w(), -q.x(), -q.y(), -q.z());\n return q;\n }\n\n template \n static Eigen::Matrix Qleft(const Eigen::QuaternionBase &q)\n {\n Eigen::Quaternion qq = positify(q);\n Eigen::Matrix ans;\n ans(0, 0) = qq.w(), ans.template block<1, 3>(0, 1) = -qq.vec().transpose();\n ans.template block<3, 1>(1, 0) = qq.vec(), ans.template block<3, 3>(1, 1) = qq.w() * Eigen::Matrix::Identity() + skewSymmetric(qq.vec());\n return ans;\n }\n\n template \n static Eigen::Matrix Qright(const Eigen::QuaternionBase &p)\n {\n Eigen::Quaternion pp = positify(p);\n Eigen::Matrix ans;\n ans(0, 0) = pp.w(), ans.template block<1, 3>(0, 1) = -pp.vec().transpose();\n ans.template block<3, 1>(1, 0) = pp.vec(), ans.template block<3, 3>(1, 1) = pp.w() * Eigen::Matrix::Identity() - skewSymmetric(pp.vec());\n return ans;\n }\n\n static Eigen::Vector3d R2ypr(const Eigen::Matrix3d &R)\n {\n Eigen::Vector3d n = R.col(0);\n Eigen::Vector3d o = R.col(1);\n Eigen::Vector3d a = R.col(2);\n\n Eigen::Vector3d ypr(3);\n double y = atan2(n(1), n(0));\n double p = atan2(-n(2), n(0) * cos(y) + n(1) * sin(y));\n double r = atan2(a(0) * sin(y) - a(1) * cos(y), -o(0) * sin(y) + o(1) * cos(y));\n ypr(0) = y;\n ypr(1) = p;\n ypr(2) = r;\n\n return ypr / M_PI * 180.0;\n }\n\n template \n static Eigen::Matrix ypr2R(const Eigen::MatrixBase &ypr)\n {\n typedef typename Derived::Scalar Scalar_t;\n\n Scalar_t y = ypr(0) / 180.0 * M_PI;\n Scalar_t p = ypr(1) / 180.0 * M_PI;\n Scalar_t r = ypr(2) / 180.0 * M_PI;\n\n Eigen::Matrix Rz;\n Rz << cos(y), -sin(y), 0,\n sin(y), cos(y), 0,\n 0, 0, 1;\n\n Eigen::Matrix Ry;\n Ry << cos(p), 0., sin(p),\n 0., 1., 0.,\n -sin(p), 0., cos(p);\n\n Eigen::Matrix Rx;\n Rx << 1., 0., 0.,\n 0., cos(r), -sin(r),\n 0., sin(r), cos(r);\n\n return Rz * Ry * Rx;\n }\n\n static Eigen::Matrix3d g2R(const Eigen::Vector3d &g);\n\n template \n struct uint_\n {\n };\n\n template \n void unroller(const Lambda &f, const IterT &iter, uint_)\n {\n unroller(f, iter, uint_());\n f(iter + N);\n }\n\n template \n void unroller(const Lambda &f, const IterT &iter, uint_<0>)\n {\n f(iter);\n }\n\n template \n static T normalizeAngle(const T& angle_degrees) {\n T two_pi(2.0 * 180);\n if (angle_degrees > 0)\n return angle_degrees -\n two_pi * std::floor((angle_degrees + T(180)) / two_pi);\n else\n return angle_degrees +\n two_pi * std::floor((-angle_degrees + T(180)) / two_pi);\n };\n};\n" }, { "path": "support_files/brief_pattern.yml", "content": "%YAML:1.0\nx1:\n - 0\n - 4\n - 11\n - -4\n - 24\n - -3\n - -4\n - -7\n - -5\n - -2\n - 8\n - 1\n - -2\n - -5\n - -5\n - -8\n - 2\n - -8\n - 6\n - 4\n - -1\n - -14\n - 12\n - -12\n - -20\n - -11\n - 7\n - -11\n - 18\n - -10\n - 10\n - -20\n - 7\n - -2\n - 3\n - 7\n - -20\n - -3\n - -4\n - -6\n - -4\n - 14\n - 1\n - 2\n - -9\n - 15\n - 13\n - 14\n - -21\n - -6\n - 3\n - -7\n - 23\n - -22\n - -12\n - 0\n - -22\n - 1\n - -11\n - -11\n - -8\n - -7\n - -16\n - -6\n - 4\n - -16\n - 4\n - 7\n - 0\n - 0\n - -7\n - -4\n - 16\n - -3\n - 6\n - 5\n - 0\n - 8\n - -4\n - -12\n - 0\n - -11\n - 11\n - 3\n - -13\n - 9\n - 11\n - -23\n - 0\n - 6\n - 6\n - 3\n - -7\n - 6\n - 2\n - -4\n - 24\n - -3\n - 5\n - -5\n - 0\n - 0\n - -4\n - -10\n - -1\n - 8\n - 6\n - 3\n - -7\n - 7\n - -4\n - 5\n - 8\n - 10\n - 8\n - -10\n - 2\n - 6\n - 0\n - 10\n - -2\n - 5\n - -2\n - -1\n - -12\n - -10\n - -15\n - 12\n - 1\n - -6\n - 12\n - -7\n - -7\n - -8\n - 7\n - -11\n - 2\n - -22\n - 7\n - 20\n - -8\n - 4\n - 3\n - 11\n - -9\n - 4\n - -4\n - -12\n - 12\n - -22\n - -6\n - 7\n - 5\n - -1\n - 9\n - 10\n - -15\n - -2\n - 1\n - 14\n - 21\n - 3\n - 6\n - -13\n - 5\n - -19\n - 10\n - 6\n - 2\n - -14\n - -16\n - -15\n - 6\n - -8\n - 18\n - 7\n - 16\n - -4\n - -2\n - -4\n - 0\n - -4\n - -14\n - 0\n - -4\n - -13\n - -1\n - -14\n - 11\n - -4\n - -15\n - -8\n - -6\n - 3\n - -1\n - -1\n - 4\n - 8\n - 3\n - 0\n - 24\n - -3\n - -11\n - -10\n - -2\n - 2\n - -1\n - 4\n - 2\n - -1\n - 11\n - 0\n - 10\n - -1\n - 6\n - -4\n - -19\n - 1\n - 9\n - -6\n - -1\n - 12\n - 0\n - -9\n - -1\n - 5\n - 0\n - -6\n - -5\n - 0\n - 7\n - -19\n - 11\n - -9\n - 11\n - 0\n - 8\n - -1\n - 5\n - 10\n - -2\n - -6\n - -6\n - 0\n - -3\n - -7\n - 4\n - -4\n - 2\n - -10\n - -7\n - 18\n - 0\n - 12\n - -2\n - 0\ny1:\n - -10\n - 12\n - 13\n - 8\n - -2\n - 2\n - 4\n - 4\n - -11\n - -4\n - 7\n - 15\n - 17\n - 0\n - 10\n - -2\n - -7\n - 10\n - 11\n - -3\n - -4\n - 11\n - -5\n - -15\n - 0\n - 3\n - -3\n - -11\n - 3\n - -4\n - 4\n - 22\n - 16\n - 3\n - -2\n - -3\n - 0\n - 5\n - 15\n - -8\n - 2\n - 2\n - -12\n - -10\n - 5\n - 4\n - 10\n - -4\n - 20\n - -7\n - 5\n - 0\n - -18\n - 2\n - -2\n - -23\n - 15\n - 4\n - -1\n - -2\n - 3\n - 0\n - -16\n - 5\n - 0\n - -16\n - -2\n - 5\n - 6\n - 4\n - 13\n - -1\n - -16\n - -13\n - -7\n - -3\n - 4\n - 0\n - -10\n - -10\n - -9\n - 4\n - -8\n - 0\n - -15\n - -2\n - -10\n - 16\n - 1\n - 0\n - 6\n - -11\n - -3\n - -7\n - -2\n - 10\n - -3\n - -2\n - 0\n - 10\n - -7\n - 0\n - 4\n - 22\n - -5\n - 0\n - -1\n - -8\n - -1\n - -4\n - 12\n - -2\n - 18\n - -2\n - -6\n - -4\n - -2\n - -16\n - -8\n - 1\n - -5\n - 2\n - 3\n - 2\n - 7\n - -8\n - 3\n - 8\n - 0\n - -10\n - -4\n - -18\n - 9\n - 12\n - 3\n - 17\n - -8\n - -13\n - 5\n - -1\n - 8\n - -10\n - -3\n - -9\n - -2\n - 7\n - 5\n - -5\n - -8\n - 15\n - -12\n - -5\n - 0\n - 2\n - 15\n - -5\n - -4\n - -1\n - -9\n - 0\n - -5\n - -11\n - -5\n - -5\n - -10\n - 2\n - 16\n - 6\n - 17\n - 0\n - -7\n - 15\n - 5\n - -14\n - -2\n - 6\n - 5\n - 2\n - 12\n - -3\n - 2\n - 4\n - 13\n - 1\n - -1\n - -3\n - -1\n - -11\n - 8\n - -3\n - 19\n - 8\n - 7\n - -3\n - 5\n - 19\n - 4\n - -6\n - 4\n - -14\n - 3\n - 10\n - -15\n - -4\n - 14\n - 1\n - 14\n - -1\n - -9\n - -5\n - -16\n - 10\n - 6\n - 3\n - -13\n - 19\n - -7\n - -7\n - -12\n - -7\n - 1\n - 7\n - -7\n - 15\n - -2\n - -15\n - 6\n - 6\n - 2\n - -6\n - -16\n - 0\n - -5\n - -9\n - 11\n - 21\n - 3\n - 8\n - 23\n - -5\n - -20\n - -7\n - 13\n - -7\n - -7\n - 0\n - 6\n - 10\n - -15\n - 1\n - 0\n - 0\n - 0\n - -7\n - 10\n - -3\nx2:\n - 0\n - 0\n - 13\n - -6\n - 22\n - -7\n - -3\n - -12\n - 1\n - -4\n - 10\n - 4\n - -6\n - 0\n - -8\n - -5\n - 7\n - -2\n - -1\n - 8\n - -2\n - -14\n - 14\n - -10\n - -14\n - -4\n - 6\n - -14\n - 15\n - -5\n - 9\n - -18\n - 4\n - 0\n - -4\n - 3\n - -19\n - -10\n - -8\n - 0\n - -4\n - 10\n - 1\n - 5\n - -12\n - 19\n - 2\n - 11\n - -18\n - -5\n - -3\n - -5\n - 21\n - -17\n - -14\n - 1\n - -17\n - 5\n - -8\n - -8\n - -8\n - -1\n - -16\n - 1\n - -2\n - -12\n - 0\n - 3\n - 7\n - -1\n - -10\n - -2\n - 14\n - -5\n - 0\n - 4\n - -2\n - 15\n - 2\n - -9\n - -9\n - -11\n - 9\n - 2\n - -15\n - 8\n - 9\n - -24\n - -5\n - 10\n - 7\n - 0\n - -7\n - 6\n - 2\n - 0\n - 24\n - 0\n - 2\n - 0\n - 2\n - -1\n - -5\n - -10\n - -4\n - 19\n - 2\n - -6\n - -8\n - 13\n - -1\n - 5\n - 3\n - 13\n - 8\n - -10\n - 0\n - 1\n - -5\n - 11\n - -6\n - 8\n - -7\n - 1\n - -14\n - -6\n - -10\n - 11\n - 2\n - -5\n - 12\n - -12\n - -3\n - -5\n - 7\n - -5\n - -3\n - -16\n - 9\n - 20\n - -9\n - 8\n - 3\n - 15\n - -12\n - 4\n - -6\n - -15\n - 10\n - -17\n - -13\n - 6\n - 4\n - -3\n - 9\n - 7\n - -18\n - -10\n - -3\n - 10\n - 23\n - -9\n - 10\n - -15\n - 7\n - -14\n - 13\n - 10\n - 0\n - -20\n - -14\n - -15\n - 3\n - -7\n - 21\n - 7\n - 17\n - 1\n - 0\n - -2\n - -8\n - -11\n - -17\n - 2\n - -6\n - -18\n - -5\n - -10\n - 5\n - 2\n - -13\n - -6\n - -4\n - 0\n - 3\n - 2\n - 10\n - 10\n - 4\n - 0\n - 21\n - -13\n - -16\n - -17\n - 0\n - 3\n - -1\n - 6\n - 5\n - 8\n - 14\n - 2\n - 10\n - -6\n - 1\n - 0\n - -23\n - 0\n - 6\n - -4\n - 3\n - 9\n - -5\n - -6\n - 1\n - 12\n - 2\n - -6\n - -4\n - 7\n - -1\n - -17\n - 9\n - -12\n - 13\n - -1\n - 11\n - 5\n - 7\n - 10\n - -2\n - -3\n - -8\n - 1\n - 0\n - -6\n - 5\n - -6\n - 5\n - -12\n - -5\n - 21\n - 2\n - 17\n - -7\n - -1\ny2:\n - -6\n - 6\n - 10\n - 9\n - 0\n - 4\n - 1\n - 1\n - -12\n - 3\n - 1\n - 9\n - 15\n - 0\n - 14\n - 3\n - -13\n - 11\n - 10\n - -4\n - -2\n - 7\n - -7\n - -17\n - -6\n - 7\n - -4\n - -16\n - -6\n - -2\n - 6\n - 19\n - 8\n - 4\n - -1\n - -2\n - 3\n - 3\n - 21\n - -3\n - 6\n - 5\n - -12\n - -5\n - 0\n - 5\n - 12\n - -5\n - 24\n - -11\n - 7\n - 0\n - -20\n - 0\n - -4\n - -21\n - 15\n - 2\n - -4\n - 0\n - -3\n - 0\n - -19\n - 5\n - 4\n - -19\n - -1\n - 5\n - 4\n - 0\n - 10\n - 3\n - -15\n - -15\n - -7\n - -1\n - 8\n - -6\n - -6\n - -14\n - -15\n - 1\n - -8\n - -1\n - -5\n - 2\n - -10\n - 17\n - 5\n - 0\n - 9\n - -8\n - -5\n - -2\n - -1\n - 9\n - 0\n - -5\n - -2\n - 13\n - -4\n - -1\n - 0\n - 16\n - -10\n - -1\n - 0\n - -6\n - 3\n - -6\n - 10\n - 1\n - 18\n - -2\n - -10\n - -4\n - -1\n - -13\n - -4\n - -1\n - -8\n - 7\n - 4\n - -3\n - 4\n - -14\n - -4\n - 11\n - -1\n - -12\n - -5\n - -14\n - 9\n - 16\n - 1\n - 20\n - -9\n - -10\n - 10\n - 2\n - 4\n - -2\n - -4\n - -6\n - -6\n - 5\n - 5\n - -4\n - 0\n - 18\n - -15\n - -5\n - 0\n - 1\n - 8\n - -4\n - -2\n - 0\n - -11\n - 0\n - -7\n - -5\n - 0\n - 0\n - -3\n - 5\n - 12\n - 3\n - 13\n - 1\n - -9\n - 19\n - 4\n - -9\n - 4\n - 2\n - 3\n - -1\n - 14\n - 0\n - 2\n - 4\n - 14\n - 8\n - 0\n - -4\n - 0\n - -12\n - 9\n - 0\n - 23\n - 7\n - 0\n - -9\n - 2\n - 22\n - 2\n - -14\n - 6\n - -16\n - 1\n - 4\n - -14\n - -10\n - 19\n - 6\n - 16\n - -4\n - -9\n - 0\n - -7\n - 12\n - 5\n - 5\n - -14\n - 21\n - -5\n - -9\n - -17\n - -5\n - 4\n - 14\n - -5\n - 9\n - -4\n - -21\n - 4\n - 1\n - 5\n - 0\n - -12\n - 2\n - 0\n - -1\n - 6\n - 24\n - 0\n - 15\n - 23\n - -10\n - -12\n - -2\n - 12\n - -4\n - -5\n - 0\n - 5\n - 13\n - -21\n - 0\n - -2\n - 1\n - 6\n - -4\n - 9\n - -2\n" }, { "path": "vins_estimator/CMakeLists.txt", "content": "cmake_minimum_required(VERSION 2.8.3)\nproject(vins_estimator)\n\nset(CMAKE_BUILD_TYPE \"Release\")\n# set(CMAKE_BUILD_TYPE \"RelWithDebInfo\")\n# set(CMAKE_BUILD_TYPE \"Debug\")\nset(CMAKE_CXX_FLAGS \"-std=c++14\")\nset(CMAKE_CXX_FLAGS_RELEASE \"-O3 -Wall -g\")\n\nfind_package(catkin REQUIRED COMPONENTS\n roscpp\n std_msgs\n geometry_msgs\n nav_msgs\n tf\n cv_bridge\n camera_model\n message_filters\n image_transport\n\n nodelet\n )\n \nfind_package(OpenCV REQUIRED)\n\n# message(WARNING \"OpenCV_VERSION: ${OpenCV_VERSION}\")\n\nfind_package(Ceres REQUIRED)\n\nset(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)\nfind_package(Eigen3)\ninclude_directories(\n ${catkin_INCLUDE_DIRS}\n ${CERES_INCLUDE_DIRS}\n ${EIGEN3_INCLUDE_DIR}\n)\n\ncatkin_package()\n\nadd_library(vins_lib\n src/utility/parameters.cpp\n src/estimator/estimator.cpp\n src/feature_manager/feature_manager.cpp\n src/factor/pose_local_parameterization.cpp\n src/factor/projection_factor.cpp\n src/factor/projection_td_factor.cpp\n src/factor/marginalization_factor.cpp\n src/utility/utility.cpp\n src/utility/visualization.cpp\n src/utility/CameraPoseVisualization.cpp\n src/initial/solve_5pts.cpp\n src/initial/initial_aligment.cpp\n src/initial/initial_sfm.cpp\n src/initial/initial_ex_rotation.cpp\n src/feature_tracker/feature_tracker.cpp)\ntarget_link_libraries(vins_lib ${catkin_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES} /usr/local/lib/libSophus.so\n)\n\nadd_library(estimator_nodelet src/estimator_nodelet.cpp)\ntarget_link_libraries(estimator_nodelet vins_lib)\n" }, { "path": "vins_estimator/cmake/FindEigen.cmake", "content": "# Ceres Solver - A fast non-linear least squares minimizer\n# Copyright 2015 Google Inc. All rights reserved.\n# http://ceres-solver.org/\n#\n# Redistribution and use in source and binary forms, with or without\n# modification, are permitted provided that the following conditions are met:\n#\n# * Redistributions of source code must retain the above copyright notice,\n# this list of conditions and the following disclaimer.\n# * Redistributions in binary form must reproduce the above copyright notice,\n# this list of conditions and the following disclaimer in the documentation\n# and/or other materials provided with the distribution.\n# * Neither the name of Google Inc. nor the names of its contributors may be\n# used to endorse or promote products derived from this software without\n# specific prior written permission.\n#\n# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS IS\"\n# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE\n# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE\n# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE\n# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR\n# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF\n# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS\n# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN\n# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)\n# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\n# POSSIBILITY OF SUCH DAMAGE.\n#\n# Author: alexs.mac@gmail.com (Alex Stewart)\n#\n\n# FindEigen.cmake - Find Eigen library, version >= 3.\n#\n# This module defines the following variables:\n#\n# EIGEN_FOUND: TRUE iff Eigen is found.\n# EIGEN_INCLUDE_DIRS: Include directories for Eigen.\n#\n# EIGEN_VERSION: Extracted from Eigen/src/Core/util/Macros.h\n# EIGEN_WORLD_VERSION: Equal to 3 if EIGEN_VERSION = 3.2.0\n# EIGEN_MAJOR_VERSION: Equal to 2 if EIGEN_VERSION = 3.2.0\n# EIGEN_MINOR_VERSION: Equal to 0 if EIGEN_VERSION = 3.2.0\n#\n# The following variables control the behaviour of this module:\n#\n# EIGEN_INCLUDE_DIR_HINTS: List of additional directories in which to\n# search for eigen includes, e.g: /timbuktu/eigen3.\n#\n# The following variables are also defined by this module, but in line with\n# CMake recommended FindPackage() module style should NOT be referenced directly\n# by callers (use the plural variables detailed above instead). These variables\n# do however affect the behaviour of the module via FIND_[PATH/LIBRARY]() which\n# are NOT re-called (i.e. search for library is not repeated) if these variables\n# are set with valid values _in the CMake cache_. This means that if these\n# variables are set directly in the cache, either by the user in the CMake GUI,\n# or by the user passing -DVAR=VALUE directives to CMake when called (which\n# explicitly defines a cache variable), then they will be used verbatim,\n# bypassing the HINTS variables and other hard-coded search locations.\n#\n# EIGEN_INCLUDE_DIR: Include directory for CXSparse, not including the\n# include directory of any dependencies.\n\n# Called if we failed to find Eigen or any of it's required dependencies,\n# unsets all public (designed to be used externally) variables and reports\n# error message at priority depending upon [REQUIRED/QUIET/] argument.\nmacro(EIGEN_REPORT_NOT_FOUND REASON_MSG)\n unset(EIGEN_FOUND)\n unset(EIGEN_INCLUDE_DIRS)\n # Make results of search visible in the CMake GUI if Eigen has not\n # been found so that user does not have to toggle to advanced view.\n mark_as_advanced(CLEAR EIGEN_INCLUDE_DIR)\n # Note _FIND_[REQUIRED/QUIETLY] variables defined by FindPackage()\n # use the camelcase library name, not uppercase.\n if (Eigen_FIND_QUIETLY)\n message(STATUS \"Failed to find Eigen - \" ${REASON_MSG} ${ARGN})\n elseif (Eigen_FIND_REQUIRED)\n message(FATAL_ERROR \"Failed to find Eigen - \" ${REASON_MSG} ${ARGN})\n else()\n # Neither QUIETLY nor REQUIRED, use no priority which emits a message\n # but continues configuration and allows generation.\n message(\"-- Failed to find Eigen - \" ${REASON_MSG} ${ARGN})\n endif ()\n return()\nendmacro(EIGEN_REPORT_NOT_FOUND)\n\n# Protect against any alternative find_package scripts for this library having\n# been called previously (in a client project) which set EIGEN_FOUND, but not\n# the other variables we require / set here which could cause the search logic\n# here to fail.\nunset(EIGEN_FOUND)\n\n# Search user-installed locations first, so that we prefer user installs\n# to system installs where both exist.\nlist(APPEND EIGEN_CHECK_INCLUDE_DIRS\n /usr/local/include\n /usr/local/homebrew/include # Mac OS X\n /opt/local/var/macports/software # Mac OS X.\n /opt/local/include\n /usr/include)\n# Additional suffixes to try appending to each search path.\nlist(APPEND EIGEN_CHECK_PATH_SUFFIXES\n eigen3 # Default root directory for Eigen.\n Eigen/include/eigen3 # Windows (for C:/Program Files prefix) < 3.3\n Eigen3/include/eigen3 ) # Windows (for C:/Program Files prefix) >= 3.3\n\n# Search supplied hint directories first if supplied.\nfind_path(EIGEN_INCLUDE_DIR\n NAMES Eigen/Core\n PATHS ${EIGEN_INCLUDE_DIR_HINTS}\n ${EIGEN_CHECK_INCLUDE_DIRS}\n PATH_SUFFIXES ${EIGEN_CHECK_PATH_SUFFIXES})\n\nif (NOT EIGEN_INCLUDE_DIR OR\n NOT EXISTS ${EIGEN_INCLUDE_DIR})\n eigen_report_not_found(\n \"Could not find eigen3 include directory, set EIGEN_INCLUDE_DIR to \"\n \"path to eigen3 include directory, e.g. /usr/local/include/eigen3.\")\nendif (NOT EIGEN_INCLUDE_DIR OR\n NOT EXISTS ${EIGEN_INCLUDE_DIR})\n\n# Mark internally as found, then verify. EIGEN_REPORT_NOT_FOUND() unsets\n# if called.\nset(EIGEN_FOUND TRUE)\n\n# Extract Eigen version from Eigen/src/Core/util/Macros.h\nif (EIGEN_INCLUDE_DIR)\n set(EIGEN_VERSION_FILE ${EIGEN_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h)\n if (NOT EXISTS ${EIGEN_VERSION_FILE})\n eigen_report_not_found(\n \"Could not find file: ${EIGEN_VERSION_FILE} \"\n \"containing version information in Eigen install located at: \"\n \"${EIGEN_INCLUDE_DIR}.\")\n else (NOT EXISTS ${EIGEN_VERSION_FILE})\n file(READ ${EIGEN_VERSION_FILE} EIGEN_VERSION_FILE_CONTENTS)\n\n string(REGEX MATCH \"#define EIGEN_WORLD_VERSION [0-9]+\"\n EIGEN_WORLD_VERSION \"${EIGEN_VERSION_FILE_CONTENTS}\")\n string(REGEX REPLACE \"#define EIGEN_WORLD_VERSION ([0-9]+)\" \"\\\\1\"\n EIGEN_WORLD_VERSION \"${EIGEN_WORLD_VERSION}\")\n\n string(REGEX MATCH \"#define EIGEN_MAJOR_VERSION [0-9]+\"\n EIGEN_MAJOR_VERSION \"${EIGEN_VERSION_FILE_CONTENTS}\")\n string(REGEX REPLACE \"#define EIGEN_MAJOR_VERSION ([0-9]+)\" \"\\\\1\"\n EIGEN_MAJOR_VERSION \"${EIGEN_MAJOR_VERSION}\")\n\n string(REGEX MATCH \"#define EIGEN_MINOR_VERSION [0-9]+\"\n EIGEN_MINOR_VERSION \"${EIGEN_VERSION_FILE_CONTENTS}\")\n string(REGEX REPLACE \"#define EIGEN_MINOR_VERSION ([0-9]+)\" \"\\\\1\"\n EIGEN_MINOR_VERSION \"${EIGEN_MINOR_VERSION}\")\n\n # This is on a single line s/t CMake does not interpret it as a list of\n # elements and insert ';' separators which would result in 3.;2.;0 nonsense.\n set(EIGEN_VERSION \"${EIGEN_WORLD_VERSION}.${EIGEN_MAJOR_VERSION}.${EIGEN_MINOR_VERSION}\")\n endif (NOT EXISTS ${EIGEN_VERSION_FILE})\nendif (EIGEN_INCLUDE_DIR)\n\n# Set standard CMake FindPackage variables if found.\nif (EIGEN_FOUND)\n set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR})\nendif (EIGEN_FOUND)\n\n# Handle REQUIRED / QUIET optional arguments and version.\ninclude(FindPackageHandleStandardArgs)\nfind_package_handle_standard_args(Eigen\n REQUIRED_VARS EIGEN_INCLUDE_DIRS\n VERSION_VAR EIGEN_VERSION)\n\n# Only mark internal variables as advanced if we found Eigen, otherwise\n# leave it visible in the standard GUI for the user to set manually.\nif (EIGEN_FOUND)\n mark_as_advanced(FORCE EIGEN_INCLUDE_DIR)\nendif (EIGEN_FOUND)\n" }, { "path": "vins_estimator/launch/atlas200/compressed2img.launch", "content": "\n \n \n\n \n \n \n \n \n" }, { "path": "vins_estimator/launch/atlas200/img2compressed.launch", "content": "\n \n \n \n \n\n \n \n \n\n \n\n" }, { "path": "vins_estimator/launch/openloris/openloris_vio_atlas.launch", "content": "\n \n \n\n \n \n\n \n \n \n \n\n \n \n\n \n \n \n \n\n \n\n" }, { "path": "vins_estimator/launch/openloris/openloris_vio_pytorch.launch", "content": "\n \n \n\n \n\n \n \n\n \n \n \n \n\n \n \n \n \n \n \n \n\n" }, { "path": "vins_estimator/launch/openloris/openloris_vio_tensorrt.launch", "content": "\n \n \n\n \n\n \n \n\n \n \n \n \n\n \n\n" }, { "path": "vins_estimator/launch/openloris/openloris_vo.launch", "content": "\n \n \n\n \n \n\n \n \n\n \n \n \n \n\n \n\n" }, { "path": "vins_estimator/launch/realsense/l515.launch", "content": "\n \n /camera/l500_depth_sensor/visual_preset: 5\n /camera/l500_depth_sensor/laser_power: 100\n /camera/l500_depth_sensor/confidence_threshold: 1\n /camera/l500_depth_sensor/min_distance: 100\n /camera/l500_depth_sensor/receiver_gain: 18\n /camera/l500_depth_sensor/noise_filtering: 4\n /camera/l500_depth_sensor/post_processing_sharpening: 1\n /camera/l500_depth_sensor/pre_processing_sharpening: 0\n /camera/l500_depth_sensor/digital_gain: 2\n /camera/l500_depth_sensor/error_polling_enabled: true\n \n\n \n \n \n \n \n \n \n \n \n \n\n \n \n \n\n \n \n \n\n \n \n \n \n\n \n \n \n \n \n \n\n \n \n \n\n \n \n \n \n \n \n \n \n \n\n \n \n \n \n \n\n \n \n\n \n \n\n \n \n \n \n \n \n \n \n \n \n\n \n \n \n \n\n\n \n \n \n \n \n \n \n \n \n \n \n\n \n \n \n \n \n\n \n \n \n\n \n \n \n\n \n \n \n \n\n \n \n \n\n \n \n \n \n \n \n\n \n \n \n \n \n \n \n \n\n \n \n\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n\n \n \n\n \n \n" }, { "path": "vins_estimator/launch/realsense/realsense_vio.launch", "content": "\n \n \n\n \n \n\n \n \n \n \n \n\n \n\n" }, { "path": "vins_estimator/launch/realsense/realsense_vio_atlas.launch", "content": "\n \n \n\n \n \n\n \n \n \n \n\n \n \n \n \n \n\n \n\n" }, { "path": "vins_estimator/launch/realsense/rs_camera.launch", "content": "\n \n /camera/aligned_depth_to_color/image_raw/compressedDepth/png_level: 2\n \n \n /camera/stereo_module/laser_power: 360.0\n \n\n \n \n \n \n \n \n \n \n \n \n\n \n \n \n\n \n \n \n\n \n \n \n \n\n \n \n \n \n \n \n\n \n \n \n\n \n \n \n \n \n \n \n \n \n\n \n \n \n \n \n\n \n \n\n \n \n\n \n \n \n \n \n \n \n \n \n \n\n \n \n \n \n\n\n \n \n \n \n \n \n \n \n \n \n \n\n \n \n \n \n \n\n \n \n \n\n \n \n \n\n \n \n \n \n\n \n \n \n\n \n \n \n \n \n \n\n \n \n \n \n \n \n \n \n\n \n \n\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n\n \n \n\n \n \n" }, { "path": "vins_estimator/launch/tum_rgbd/tum_rgbd_atlas.launch", "content": "\n \n \n\n \n \n\n \n\n \n \n \n \n\n \n\n" }, { "path": "vins_estimator/launch/tum_rgbd/tum_rgbd_pytorch.launch", "content": "\n \n \n\n \n\n \n \n\n \n \n \n \n\n \n \n \n \n \n \n \n\n" }, { "path": "vins_estimator/launch/tum_rgbd/tum_rgbd_tensorrt.launch", "content": "\n \n \n\n \n \n \n \n \n\n \n \n\n \n \n \n \n\n \n\n" }, { "path": "vins_estimator/launch/vins_rviz.launch", "content": "\n \n\n" }, { "path": "vins_estimator/launch/yolo/atlas.launch", "content": "\n \n \n\n \n \n \n \n\n \n \n" }, { "path": "vins_estimator/launch/yolo/pytorch.launch", "content": "\n \n \n \n" }, { "path": "vins_estimator/launch/yolo/tensorrt.launch", "content": "\n \n \n \n \n \n" }, { "path": "vins_estimator/nodelet_description.xml", "content": "\n \n EstimatorNodelet\n \n" }, { "path": "vins_estimator/package.xml", "content": "\n\n vins_estimator\n 0.0.0\n The vins_estimator package\n\n \n \n \n qintong\n\n\n \n \n \n TODO\n\n catkin\n roscpp\n camera_model\n roscpp\n camera_model\n\n nodelet\n nodelet\n\n \n \n \n \n\n \n \n \n\n" }, { "path": "vins_estimator/src/estimator/estimator.cpp", "content": "#include \"estimator.h\"\n#include \"../utility/visualization.h\"\n#include \n#include \n#include \n#include \n#include \n\nEstimator::Estimator() : f_manager{Rs}\n{\n ROS_INFO(\"init begins\");\n clearState();\n}\n\nvoid Estimator::setParameter()\n{\n for (int i = 0; i < NUM_OF_CAM; i++)\n {\n tic[i] = TIC[i];\n ric[i] = RIC[i];\n }\n f_manager.setRic(ric);\n ProjectionFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();\n ProjectionTdFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();\n td = TD;\n g = G;\n\n featureTracker.readIntrinsicParameter(CAM_NAMES);\n if (FISHEYE)\n {\n featureTracker.fisheye_mask = cv::imread(FISHEYE_MASK, 0);\n if (!featureTracker.fisheye_mask.data)\n {\n ROS_INFO(\"load mask fail\");\n ROS_BREAK();\n }\n else\n ROS_INFO(\"load mask success\");\n }\n featureTracker.initGridsDetector();\n}\n\nvoid Estimator::clearState()\n{\n m_imu.lock();\n while (!imu_buf.empty())\n imu_buf.pop();\n m_imu.unlock();\n\n for (int i = 0; i < WINDOW_SIZE + 1; i++)\n {\n Rs[i].setIdentity();\n Ps[i].setZero();\n Vs[i].setZero();\n Bas[i].setZero();\n Bgs[i].setZero();\n dt_buf[i].clear();\n linear_acceleration_buf[i].clear();\n angular_velocity_buf[i].clear();\n\n if (pre_integrations[i] != nullptr)\n delete pre_integrations[i];\n pre_integrations[i] = nullptr;\n\n // cl\n find_solved[i] = 0;\n // cl\n }\n\n for (int i = 0; i < NUM_OF_CAM; i++)\n {\n tic[i] = Vector3d::Zero();\n ric[i] = Matrix3d::Identity();\n }\n for (auto &it : all_image_frame)\n {\n if (it.second.pre_integration != nullptr)\n {\n delete it.second.pre_integration;\n it.second.pre_integration = nullptr;\n }\n }\n first_imu = false, sum_of_back = 0;\n sum_of_front = 0;\n frame_count = 0;\n solver_flag = INITIAL;\n initial_timestamp = 0;\n all_image_frame.clear();\n td = TD;\n\n openExEstimation = false;\n\n delete tmp_pre_integration;\n\n delete last_marginalization_info;\n\n tmp_pre_integration = nullptr;\n last_marginalization_info = nullptr;\n last_marginalization_parameter_blocks.clear();\n\n f_manager.clearState();\n\n failure_occur = false;\n relocalization_info = false;\n\n drift_correct_r = Matrix3d::Identity();\n drift_correct_t = Vector3d::Zero();\n\n latest_Q = Eigen::Quaterniond(1, 0, 0, 0);\n\n init_imu = true;\n\n prevTime = -1;\n\n initFirstPoseFlag = false;\n}\n\nvoid Estimator::processIMU(double dt, const Vector3d &linear_acceleration,\n const Vector3d &angular_velocity)\n{\n if (!first_imu)\n {\n first_imu = true;\n acc_0 = linear_acceleration;\n gyr_0 = angular_velocity;\n }\n\n if (!pre_integrations[frame_count])\n {\n pre_integrations[frame_count] =\n new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};\n }\n if (frame_count != 0)\n {\n pre_integrations[frame_count]->push_back(dt, linear_acceleration, angular_velocity);\n // if(solver_flag != NON_LINEAR)\n tmp_pre_integration->push_back(dt, linear_acceleration, angular_velocity);\n\n dt_buf[frame_count].push_back(dt);\n linear_acceleration_buf[frame_count].push_back(linear_acceleration);\n angular_velocity_buf[frame_count].push_back(angular_velocity);\n\n int j = frame_count;\n Vector3d un_acc_0 = Rs[j] * (acc_0 - Bas[j]) - g;\n Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - Bgs[j];\n Rs[j] *= Utility::deltaQ(un_gyr * dt).toRotationMatrix();\n Vector3d un_acc_1 = Rs[j] * (linear_acceleration - Bas[j]) - g;\n Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);\n Ps[j] += dt * Vs[j] + 0.5 * dt * dt * un_acc;\n Vs[j] += dt * un_acc;\n }\n acc_0 = linear_acceleration;\n gyr_0 = angular_velocity;\n}\n\nvoid Estimator::processImage(map> &image,\n const std_msgs::Header &header)\n{\n ROS_DEBUG(\"new image coming ------------------------------------------\");\n ROS_DEBUG(\"Adding feature points %lu\", image.size());\n // FeaturePerFrame\n // FeaturePerId\n // feature\n if (f_manager.addFeatureCheckParallax(frame_count, image, td))\n marginalization_flag = MARGIN_OLD;\n else\n marginalization_flag = MARGIN_SECOND_NEW;\n\n ROS_DEBUG(\"%s\", marginalization_flag ? \"Non-keyframe\" : \"Keyframe\");\n ROS_DEBUG(\"Solving %d\", frame_count);\n ROS_DEBUG(\"number of feature: %d\", f_manager.getFeatureCount());\n Headers[frame_count] = header.stamp.toSec();\n\n if (USE_IMU)\n {\n double curTime = header.stamp.toSec() + td;\n\n while (!IMUAvailable(curTime))\n {\n printf(\"waiting for imu ... \\r\");\n std::chrono::milliseconds dura(2);\n std::this_thread::sleep_for(dura);\n }\n\n std::vector>> imu_vector;\n getIMUInterval(prevTime, curTime, imu_vector);\n if (!initFirstPoseFlag)\n initFirstIMUPose(imu_vector);\n for (size_t i = 0; i < imu_vector.size(); i++)\n {\n double dt;\n if (i == 0)\n dt = imu_vector[i].first - prevTime;\n else if (i == imu_vector.size() - 1)\n dt = curTime - imu_vector[i - 1].first;\n else\n dt = imu_vector[i].first - imu_vector[i - 1].first;\n processIMU(dt, imu_vector[i].second.first, imu_vector[i].second.second);\n }\n prevTime = curTime;\n }\n\n ImageFrame imageframe(image, header.stamp.toSec());\n imageframe.pre_integration = tmp_pre_integration;\n all_image_frame.insert(make_pair(header.stamp.toSec(), imageframe));\n tmp_pre_integration = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};\n\n if (ESTIMATE_EXTRINSIC == 2)\n {\n ROS_INFO(\"calibrating extrinsic param, rotation movement is needed\");\n if (frame_count != 0)\n {\n vector> corres =\n f_manager.getCorresponding(frame_count - 1, frame_count);\n Matrix3d calib_ric;\n if (initial_ex_rotation.CalibrationExRotation(\n corres, pre_integrations[frame_count]->delta_q, calib_ric))\n {\n ROS_WARN(\"initial extrinsic rotation calib success\");\n ROS_WARN_STREAM(\"initial extrinsic rotation: \" << endl << calib_ric);\n ric[0] = calib_ric;\n RIC[0] = calib_ric;\n ESTIMATE_EXTRINSIC = 1;\n }\n }\n }\n\n TicToc opt_time;\n if (solver_flag == INITIAL)\n {\n if (USE_IMU && !STATIC_INIT)\n {\n if (frame_count == WINDOW_SIZE)\n {\n bool result = false;\n if (ESTIMATE_EXTRINSIC != 2 && (header.stamp.toSec() - initial_timestamp) > 0.1)\n {\n result = initialStructure();\n initial_timestamp = header.stamp.toSec();\n }\n // if init sfm success\n if (result)\n {\n solver_flag = NON_LINEAR;\n solveOdometry();\n slideWindow();\n f_manager.removeFailures();\n ROS_INFO(\"Initialization finish!\");\n last_R = Rs[WINDOW_SIZE];\n last_P = Ps[WINDOW_SIZE];\n last_R0 = Rs[0];\n last_P0 = Ps[0];\n }\n else\n slideWindow();\n }\n else\n frame_count++;\n }\n else\n {\n f_manager.triangulateWithDepth(Ps, tic, ric);\n\n if (USE_IMU)\n {\n if (frame_count == WINDOW_SIZE)\n {\n int i = 0;\n for (auto &frame_it : all_image_frame)\n {\n frame_it.second.R = Rs[i];\n frame_it.second.T = Ps[i];\n i++;\n }\n if (ESTIMATE_EXTRINSIC != 2)\n {\n solveGyroscopeBias(all_image_frame, Bgs);\n for (int j = 0; j <= WINDOW_SIZE; j++)\n {\n pre_integrations[j]->repropagate(Vector3d::Zero(), Bgs[j]);\n }\n optimization();\n updateLatestStates();\n solver_flag = NON_LINEAR;\n slideWindow();\n ROS_INFO(\"Initialization finish!\");\n last_R = Rs[WINDOW_SIZE];\n last_P = Ps[WINDOW_SIZE];\n last_R0 = Rs[0];\n last_P0 = Ps[0];\n }\n }\n }\n else\n {\n if (frame_count == WINDOW_SIZE)\n {\n optimization();\n updateLatestStates();\n solver_flag = NON_LINEAR;\n slideWindow();\n ROS_INFO(\"Initialization finish!\");\n }\n }\n\n if (frame_count < WINDOW_SIZE)\n {\n frame_count++;\n int prev_frame = frame_count - 1;\n Ps[frame_count] = Ps[prev_frame];\n Vs[frame_count] = Vs[prev_frame];\n Rs[frame_count] = Rs[prev_frame];\n Bas[frame_count] = Bas[prev_frame];\n Bgs[frame_count] = Bgs[prev_frame];\n }\n }\n }\n else\n {\n TicToc t_solve;\n if (!USE_IMU)\n f_manager.initFramePoseByPnP(frame_count, Ps, Rs, tic, ric);\n\n f_manager.triangulateWithDepth(Ps, tic, ric);\n optimization();\n ROS_DEBUG(\"solver costs: %fms\", t_solve.toc());\n\n set removeIndex;\n movingConsistencyCheck(removeIndex);\n if (SHOW_TRACK)\n {\n for (auto iter = image.begin(), iter_next = image.begin(); iter != image.end();\n iter = iter_next)\n {\n ++iter_next;\n auto it = removeIndex.find(iter->first);\n\n if (it != removeIndex.end())\n {\n image.erase(iter);\n }\n }\n }\n\n if (failureDetection())\n {\n ROS_WARN(\"failure detection!\");\n failure_occur = true;\n clearState();\n setParameter();\n ROS_WARN(\"system reboot!\");\n return;\n }\n\n slideWindow();\n f_manager.removeFailures();\n // prepare output of VINS\n key_poses.clear();\n for (int i = 0; i <= WINDOW_SIZE; i++)\n key_poses.emplace_back(Ps[i]);\n\n last_R = Rs[WINDOW_SIZE];\n last_P = Ps[WINDOW_SIZE];\n last_R0 = Rs[0];\n last_P0 = Ps[0];\n updateLatestStates();\n }\n\n static double whole_opt_time = 0;\n static size_t cnt_frame = 0;\n ++cnt_frame;\n whole_opt_time += opt_time.toc();\n ROS_DEBUG(\"average opt costs: %f\", whole_opt_time / cnt_frame);\n}\n\n/**\n * @brief 视觉的结构初始化\n * @Description 确保IMU有充分运动激励\n * relativePose()找到具有足够视差的两帧,由F矩阵恢复R、t作为初始值\n * sfm.construct() 全局纯视觉SFM 恢复滑动窗口帧的位姿\n * visualInitialAlign()视觉惯性联合初始化\n * @return bool true:初始化成功\n */\nbool Estimator::initialStructure()\n{\n // check imu observibility\n bool is_imu_excited = false;\n {\n map::iterator frame_it;\n Vector3d sum_g;\n for (frame_it = all_image_frame.begin(), frame_it++; frame_it != all_image_frame.end();\n frame_it++)\n {\n double dt = frame_it->second.pre_integration->sum_dt;\n Vector3d tmp_g = frame_it->second.pre_integration->delta_v / dt;\n sum_g += tmp_g;\n }\n Vector3d aver_g;\n aver_g = sum_g * 1.0 / ((int)all_image_frame.size() - 1);\n double var = 0;\n for (frame_it = all_image_frame.begin(), frame_it++; frame_it != all_image_frame.end();\n frame_it++)\n {\n double dt = frame_it->second.pre_integration->sum_dt;\n Vector3d tmp_g = frame_it->second.pre_integration->delta_v / dt;\n var += (tmp_g - aver_g).transpose() * (tmp_g - aver_g);\n // cout << \"frame g \" << tmp_g.transpose() << endl;\n }\n var = sqrt(var / ((int)all_image_frame.size() - 1)); // 标准差\n // ROS_WARN(\"IMU variation %f!\", var);\n if (var < 0.25)\n {\n ROS_INFO(\"IMU excitation not enouth!\");\n // return false;\n }\n else\n {\n is_imu_excited = true;\n }\n }\n\n TicToc t_sfm;\n // global sfm\n Quaterniond Q[frame_count + 1];\n Vector3d T[frame_count + 1];\n map sfm_tracked_points;\n vector sfm_f;\n for (auto &it_per_id : f_manager.feature)\n {\n int imu_j = it_per_id.start_frame - 1;\n SFMFeature tmp_feature;\n tmp_feature.state = false;\n tmp_feature.id = it_per_id.feature_id;\n for (auto &it_per_frame : it_per_id.feature_per_frame)\n {\n imu_j++;\n Vector3d pts_j = it_per_frame.point;\n tmp_feature.observation.emplace_back(imu_j, Eigen::Vector2d{pts_j.x(), pts_j.y()});\n tmp_feature.observation_depth.emplace_back(imu_j, it_per_frame.depth);\n }\n sfm_f.emplace_back(tmp_feature);\n }\n Matrix3d relative_R;\n Vector3d relative_T;\n int l;\n //保证具有足够的视差,由F矩阵恢复Rt\n //第l帧是从第一帧开始到滑动窗口中第一个满足与当前帧的平均视差足够大的帧，会作为参考帧到下面的全局sfm使用\n //此处的relative_R，relative_T为当前帧到参考帧（第l帧）的坐标系变换Rt\n if (!relativePose(relative_R, relative_T, l))\n {\n ROS_INFO(\"Not enough features or parallax; Move device around\");\n // ROS_INFO(\"Not enough features!\");\n return false;\n }\n\n //对窗口中每个图像帧求解sfm问题\n //得到所有图像帧相对于参考帧的姿态四元数Q、平移向量T和特征点坐标sfm_tracked_points。\n GlobalSFM sfm;\n if (!sfm.construct(frame_count + 1, Q, T, l, relative_R, relative_T, sfm_f, sfm_tracked_points))\n {\n ROS_DEBUG(\"global SFM failed!\");\n marginalization_flag = MARGIN_OLD;\n return false;\n }\n\n // solve pnp for all frame\n //对于所有的图像帧，包括不在滑动窗口中的，提供初始的RT估计，然后solvePnP进行优化,得到每一帧的姿态\n map::iterator frame_it;\n map::iterator it;\n frame_it = all_image_frame.begin();\n for (int i = 0; frame_it != all_image_frame.end(); frame_it++)\n {\n // provide initial guess\n cv::Mat r, rvec, t, D, tmp_r;\n if ((frame_it->first) == Headers[i])\n {\n frame_it->second.is_key_frame = true;\n frame_it->second.R = Q[i].toRotationMatrix() * RIC[0].transpose();\n frame_it->second.T = T[i];\n i++;\n continue;\n }\n if ((frame_it->first) > Headers[i])\n {\n i++;\n }\n Matrix3d R_inital = (Q[i].inverse()).toRotationMatrix();\n Vector3d P_inital = -R_inital * T[i];\n cv::eigen2cv(R_inital, tmp_r);\n cv::Rodrigues(tmp_r, rvec);\n cv::eigen2cv(P_inital, t);\n\n frame_it->second.is_key_frame = false;\n vector pts_3_vector;\n vector pts_2_vector;\n // points: map>>>\n for (auto &id_pts : frame_it->second.points)\n {\n int feature_id = id_pts.first;\n it = sfm_tracked_points.find(feature_id);\n if (it != sfm_tracked_points.end())\n {\n Vector3d world_pts = it->second;\n cv::Point3f pts_3(world_pts(0), world_pts(1), world_pts(2));\n pts_3_vector.push_back(pts_3);\n Vector2d img_pts = id_pts.second.head<2>();\n cv::Point2f pts_2(img_pts(0), img_pts(1));\n pts_2_vector.push_back(pts_2);\n }\n }\n cv::Mat K = (cv::Mat_(3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 1);\n if (pts_3_vector.size() < 6)\n {\n cout << \"pts_3_vector size \" << pts_3_vector.size() << endl;\n ROS_DEBUG(\"Not enough points for solve pnp !\");\n return false;\n }\n /**\n *bool cv::solvePnP( 求解pnp问题\n * InputArray objectPoints, 特征点的3D坐标数组\n * InputArray imagePoints, 特征点对应的图像坐标\n * InputArray cameraMatrix, 相机内参矩阵\n * InputArray distCoeffs, 失真系数的输入向量\n * OutputArray rvec, 旋转向量\n * OutputArray tvec, 平移向量\n * bool useExtrinsicGuess = false, 为真则使用提供的初始估计值\n * int flags = SOLVEPNP_ITERATIVE 采用LM优化\n *)\n */\n if (!cv::solvePnP(pts_3_vector, pts_2_vector, K, D, rvec, t, 1))\n {\n ROS_DEBUG(\"solve pnp fail!\");\n return false;\n }\n cv::Rodrigues(rvec, r);\n MatrixXd R_pnp, tmp_R_pnp;\n cv::cv2eigen(r, tmp_R_pnp);\n //这里也同样需要将坐标变换矩阵转变成图像帧位姿，并转换为IMU坐标系的位姿\n R_pnp = tmp_R_pnp.transpose();\n MatrixXd T_pnp;\n cv::cv2eigen(t, T_pnp);\n T_pnp = R_pnp * (-T_pnp);\n frame_it->second.R = R_pnp * RIC[0].transpose();\n frame_it->second.T = T_pnp;\n }\n\n // Rs Ps ric init\n //进行视觉惯性联合初始化\n if (visualInitialAlignWithDepth())\n {\n if (!is_imu_excited)\n {\n // 利用加速度平均值估计Bas\n Vector3d sum_a(0, 0, 0);\n for (frame_it = all_image_frame.begin(), frame_it++; frame_it != all_image_frame.end();\n frame_it++)\n {\n double dt = frame_it->second.pre_integration->sum_dt;\n Vector3d tmp_a = frame_it->second.pre_integration->delta_v / dt;\n sum_a += tmp_a;\n }\n Vector3d avg_a;\n avg_a = sum_a * 1.0 / ((int)all_image_frame.size() - 1);\n\n Vector3d tmp_Bas = avg_a - Utility::g2R(avg_a).inverse() * G;\n ROS_WARN_STREAM(\"accelerator bias initial calibration \" << tmp_Bas.transpose());\n for (int i = 0; i <= WINDOW_SIZE; i++)\n {\n Bas[i] = tmp_Bas;\n }\n }\n return true;\n }\n else\n {\n ROS_INFO(\"misalign visual structure with IMU\");\n return false;\n }\n}\n\nbool Estimator::initialStructureWithDepth()\n{\n // check imu observibility\n bool is_imu_excited = false;\n\n map::iterator frame_it;\n Vector3d sum_a;\n for (frame_it = all_image_frame.begin(), frame_it++; frame_it != all_image_frame.end();\n frame_it++)\n {\n double dt = frame_it->second.pre_integration->sum_dt;\n Vector3d tmp_g = frame_it->second.pre_integration->delta_v / dt;\n sum_a += tmp_g;\n }\n Vector3d aver_g;\n aver_g = sum_a * 1.0 / ((int)all_image_frame.size() - 1);\n double var = 0;\n for (frame_it = all_image_frame.begin(), frame_it++; frame_it != all_image_frame.end();\n frame_it++)\n {\n double dt = frame_it->second.pre_integration->sum_dt;\n Vector3d tmp_g = frame_it->second.pre_integration->delta_v / dt;\n var += (tmp_g - aver_g).transpose() * (tmp_g - aver_g);\n }\n var = sqrt(var / ((int)all_image_frame.size() - 1)); // 标准差\n // ROS_WARN(\"IMU variation %f!\", var);\n if (var < 0.25)\n {\n ROS_INFO(\"IMU excitation not enouth!\");\n // return false;\n }\n else\n {\n is_imu_excited = true;\n }\n\n if (visualInitialAlignWithDepth())\n {\n if (!is_imu_excited)\n {\n Vector3d tmp_Bas = aver_g - Utility::g2R(aver_g).inverse() * G;\n ROS_WARN_STREAM(\"accelerator bias initial calibration \" << tmp_Bas.transpose());\n for (int i = 0; i <= WINDOW_SIZE; i++)\n {\n Bas[i] = tmp_Bas;\n }\n }\n return true;\n }\n else\n {\n staticInitialAlignWithDepth();\n return true;\n }\n\n ROS_INFO(\"misalign visual structure with IMU\");\n return false;\n}\n\nbool Estimator::visualInitialAlign()\n{\n TicToc t_g;\n VectorXd x;\n // solve scale\n bool result = VisualIMUAlignment(all_image_frame, Bgs, g, x);\n if (!result)\n {\n ROS_ERROR(\"solve g failed!\");\n return false;\n }\n\n // change state\n for (int i = 0; i <= frame_count; i++)\n {\n Matrix3d Ri = all_image_frame[Headers[i]].R;\n Vector3d Pi = all_image_frame[Headers[i]].T;\n Ps[i] = Pi;\n Rs[i] = Ri;\n all_image_frame[Headers[i]].is_key_frame = true;\n }\n\n VectorXd dep = f_manager.getDepthVector();\n for (int i = 0; i < dep.size(); i++)\n dep[i] = -1;\n f_manager.clearDepth(dep);\n\n // triangulat on cam pose , no tic\n Vector3d TIC_TMP[NUM_OF_CAM];\n for (int i = 0; i < NUM_OF_CAM; i++)\n TIC_TMP[i].setZero();\n ric[0] = RIC[0];\n f_manager.setRic(ric);\n // f_manager.triangulate(Ps, &(TIC_TMP[0]), &(RIC[0]));\n f_manager.triangulateWithDepth(Ps, &(TIC_TMP[0]), &(RIC[0]));\n\n double s = (x.tail<1>())(0);\n // ROS_DEBUG(\"the scale is %f\\n\", s);\n // do repropagate here\n for (int i = 0; i <= WINDOW_SIZE; i++)\n {\n pre_integrations[i]->repropagate(Vector3d::Zero(), Bgs[i]);\n }\n for (int i = frame_count; i >= 0; i--)\n Ps[i] = s * Ps[i] - Rs[i] * TIC[0] - (s * Ps[0] - Rs[0] * TIC[0]);\n int kv = -1;\n map::iterator frame_i;\n for (frame_i = all_image_frame.begin(); frame_i != all_image_frame.end(); frame_i++)\n {\n if (frame_i->second.is_key_frame)\n {\n kv++;\n Vs[kv] = frame_i->second.R * x.segment<3>(kv * 3);\n }\n }\n for (auto &it_per_id : f_manager.feature)\n {\n it_per_id.used_num = it_per_id.feature_per_frame.size();\n // if (it_per_id.used_num < 4)\n // continue;\n if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))\n continue;\n it_per_id.estimated_depth *= s;\n }\n\n Matrix3d R0 = Utility::g2R(g);\n double yaw = Utility::R2ypr(R0 * Rs[0]).x();\n R0 = Utility::ypr2R(Eigen::Vector3d{-yaw, 0, 0}) * R0;\n g = R0 * g;\n // Matrix3d rot_diff = R0 * Rs[0].transpose();\n Matrix3d rot_diff = R0;\n for (int i = 0; i <= frame_count; i++)\n {\n Ps[i] = rot_diff * Ps[i];\n Rs[i] = rot_diff * Rs[i];\n Vs[i] = rot_diff * Vs[i];\n }\n ROS_DEBUG_STREAM(\"g0 \" << g.transpose());\n ROS_DEBUG_STREAM(\"my R0 \" << Utility::R2ypr(Rs[0]).transpose());\n\n return true;\n}\n\n/**\n * @brief 视觉惯性联合初始化\n * @Description 陀螺仪的偏置校准(加速度偏置没有处理) 计算速度V[0:n] 重力g 尺度s\n * 更新了Bgs后，IMU测量量需要repropagate\n * 得到尺度s和重力g的方向后，需更新所有图像帧在世界坐标系下的Ps、Rs、Vs\n * @return bool true：成功\n */\nbool Estimator::staticInitialAlignWithDepth()\n{\n // 利用加速度平均值估计Bgs, Bas, g\n map::iterator frame_it;\n Vector3d sum_a(0, 0, 0);\n Vector3d sum_w(0, 0, 0);\n for (frame_it = all_image_frame.begin(), frame_it++; frame_it != all_image_frame.end();\n frame_it++)\n {\n sum_a +=\n frame_it->second.pre_integration->delta_v / frame_it->second.pre_integration->sum_dt;\n Vector3d tmp_w;\n for (auto &gyr_msg : frame_it->second.pre_integration->gyr_buf)\n {\n tmp_w += gyr_msg;\n }\n sum_w += tmp_w / frame_it->second.pre_integration->gyr_buf.size();\n }\n Vector3d avg_a = sum_a * 1.0 / ((int)all_image_frame.size() - 1);\n Vector3d avg_w = sum_w * 1.0 / ((int)all_image_frame.size() - 1);\n g = avg_a.normalized() * G.z();\n Vector3d tmp_Bas = avg_a - g;\n\n // solveGyroscopeBias(all_image_frame, Bgs);\n ROS_WARN_STREAM(\"gyroscope bias initial calibration \" << avg_w.transpose());\n ROS_WARN_STREAM(\"accelerator bias initial calibration \" << tmp_Bas.transpose());\n for (int i = 0; i <= WINDOW_SIZE; i++)\n {\n Bgs[i] = avg_w;\n Bas[i] = tmp_Bas;\n }\n\n //陀螺仪的偏置bgs改变，重新计算预积分\n for (int i = 0; i <= WINDOW_SIZE; i++)\n {\n pre_integrations[i]->repropagate(Bas[i], Bgs[i]);\n }\n\n //通过将重力旋转到z轴上，得到世界坐标系与摄像机坐标系c0之间的旋转矩阵rot_diff\n Matrix3d R0 = Utility::g2R(g);\n double yaw = Utility::R2ypr(R0).x();\n R0 = Utility::ypr2R(Eigen::Vector3d{-yaw, 0, 0}) * R0;\n g = R0 * G;\n\n Matrix3d rot_diff = R0;\n //所有变量从参考坐标系c0旋转到世界坐标系w\n for (int i = 0; i <= frame_count; i++)\n {\n ROS_ERROR(\"%d farme's Ps is %f | %f | %f\\n\", i, Ps[i].x(), Ps[i].y(),\n Ps[i].z()); // shan add\n ROS_ERROR(\"%d farme's Vs is %f | %f | %f\\n\", i, Vs[i].x(), Vs[i].y(), Vs[i].z());\n Ps[i] = rot_diff * Ps[i];\n Rs[i] = rot_diff * Rs[i];\n Vs[i] = rot_diff * Vs[i];\n // Vs[i] = Vector3d(0, 0, 0);\n }\n ROS_DEBUG_STREAM(\"static g0 \" << g.transpose());\n ROS_DEBUG_STREAM(\"my R0 \" << Utility::R2ypr(Rs[0]).transpose());\n\n return true;\n}\n\n/**\n * @brief 视觉惯性联合初始化\n * @Description 陀螺仪的偏置校准(加速度偏置没有处理) 计算速度V[0:n] 重力g 尺度s\n * 更新了Bgs后，IMU测量量需要repropagate\n * 得到尺度s和重力g的方向后，需更新所有图像帧在世界坐标系下的Ps、Rs、Vs\n * @return bool true：成功\n */\nbool Estimator::visualInitialAlignWithDepth()\n{\n TicToc t_g;\n VectorXd x;\n\n // solve scale\n //计算陀螺仪偏置，尺度，重力加速度和速度\n solveGyroscopeBias(all_image_frame, Bgs);\n\n if (!LinearAlignmentWithDepth(all_image_frame, g, x))\n {\n ROS_ERROR(\"solve g failed!\");\n return false;\n }\n\n // do repropagate here\n //陀螺仪的偏置bgs改变，重新计算预积分\n for (int i = 0; i <= WINDOW_SIZE; i++)\n {\n pre_integrations[i]->repropagate(Vector3d::Zero(), Bgs[i]);\n }\n // change state\n // 得到所有图像帧的位姿Ps、Rs，并将其置为关键帧\n for (int i = 0; i <= frame_count; i++)\n {\n Matrix3d Ri = all_image_frame[Headers[i]].R;\n Vector3d Pi = all_image_frame[Headers[i]].T;\n Ps[i] = Pi;\n Rs[i] = Ri;\n all_image_frame[Headers[i]].is_key_frame = true;\n }\n\n // do repropagate here\n //陀螺仪的偏置bgs改变，重新计算预积分\n for (int i = 0; i <= WINDOW_SIZE; i++)\n {\n pre_integrations[i]->repropagate(Vector3d::Zero(), Bgs[i]);\n }\n // ROS_ERROR(\"before %f | %f | %f\\n\", Ps[1].x(), Ps[1].y(), Ps[1].z());//shan\n // add 将Ps、Vs、depth尺度s缩放\n for (int i = frame_count; i >= 0; i--)\n Ps[i] = Ps[i] - Rs[i] * TIC[0] - (Ps[0] - Rs[0] * TIC[0]);\n // ROS_ERROR(\"after %f | %f | %f\\n\", Ps[1].x(), Ps[1].y(), Ps[1].z());//shan\n // add\n int kv = -1;\n map::iterator frame_i;\n for (frame_i = all_image_frame.begin(); frame_i != all_image_frame.end(); frame_i++)\n {\n if (frame_i->second.is_key_frame)\n {\n kv++;\n Vs[kv] = frame_i->second.R * x.segment<3>(kv * 3);\n }\n }\n\n //通过将重力旋转到z轴上，得到世界坐标系与摄像机坐标系c0之间的旋转矩阵rot_diff\n Matrix3d R0 = Utility::g2R(g);\n double yaw = Utility::R2ypr(R0 * Rs[0]).x();\n R0 = Utility::ypr2R(Eigen::Vector3d{-yaw, 0, 0}) * R0;\n g = R0 * g;\n // Matrix3d rot_diff = R0 * Rs[0].transpose();\n Matrix3d rot_diff = R0;\n //所有变量从参考坐标系c0旋转到世界坐标系w\n for (int i = 0; i <= frame_count; i++)\n {\n Ps[i] = rot_diff * Ps[i];\n Rs[i] = rot_diff * Rs[i];\n Vs[i] = rot_diff * Vs[i];\n }\n ROS_DEBUG_STREAM(\"g0 \" << g.transpose());\n ROS_DEBUG_STREAM(\"my R0 \" << Utility::R2ypr(Rs[0]).transpose());\n\n return true;\n}\n\n/**\n * @brief 判断两帧有足够视差30且内点数目大于12则可进行初始化，同时得到R和T\n * @Description 判断每帧到窗口最后一帧对应特征点的平均视差是否大于30\n solveRelativeRT()通过基础矩阵计算当前帧与第l帧之间的R和T,并判断内点数目是否足够\n * @param[out] relative_R 当前帧到第l帧之间的旋转矩阵R\n * @param[out] relative_T 当前帧到第l帧之间的平移向量T\n * @param[out] L 保存滑动窗口中与当前帧满足初始化条件的那一帧\n * @return bool 1:可以进行初始化;0:不满足初始化条件\n*/\n\nbool Estimator::relativePose(Matrix3d &relative_R, Vector3d &relative_T, int &l)\n{\n // find previous frame which contians enough correspondance and parallex with\n // newest frame\n for (int i = 0; i < WINDOW_SIZE; i++)\n {\n vector> corres;\n // corres = f_manager.getCorresponding(i, WINDOW_SIZE);\n corres = f_manager.getCorrespondingWithDepth(i, WINDOW_SIZE);\n if (corres.size() > 20)\n {\n double sum_parallax = 0;\n double average_parallax;\n for (auto &corre : corres)\n {\n Vector2d pts_0(corre.first(0) / corre.first(2), corre.first(1) / corre.first(2));\n Vector2d pts_1(corre.second(0) / corre.second(2),\n corre.second(1) / corre.second(2));\n // Vector2d pts_0(corres[j].first(0), corres[j].first(1));\n // Vector2d pts_1(corres[j].second(0), corres[j].second(1));\n double parallax = (pts_0 - pts_1).norm();\n sum_parallax = sum_parallax + parallax;\n }\n average_parallax = 1.0 * sum_parallax / int(corres.size());\n if (average_parallax * 460 > 30 &&\n m_estimator.solveRelativeRT_PNP(corres, relative_R, relative_T))\n {\n l = i;\n ROS_DEBUG(\"average_parallax %f choose l %d and newest frame to \"\n \"triangulate the whole structure\",\n average_parallax * 460, l);\n return true;\n }\n }\n }\n return false;\n}\n\nvoid Estimator::solveOdometry()\n{\n if (frame_count < WINDOW_SIZE)\n return;\n if (solver_flag == NON_LINEAR)\n {\n TicToc t_tri;\n f_manager.triangulateWithDepth(Ps, tic, ric);\n // f_manager.triangulate(Ps, tic, ric);\n ROS_DEBUG(\"triangulation costs %f\", t_tri.toc());\n optimization();\n }\n}\n\nvoid Estimator::vector2double()\n{\n for (int i = 0; i <= WINDOW_SIZE; i++)\n {\n para_Pose[i][0] = Ps[i].x();\n para_Pose[i][1] = Ps[i].y();\n para_Pose[i][2] = Ps[i].z();\n Quaterniond q{Rs[i]};\n para_Pose[i][3] = q.x();\n para_Pose[i][4] = q.y();\n para_Pose[i][5] = q.z();\n para_Pose[i][6] = q.w();\n\n if (USE_IMU)\n {\n para_SpeedBias[i][0] = Vs[i].x();\n para_SpeedBias[i][1] = Vs[i].y();\n para_SpeedBias[i][2] = Vs[i].z();\n\n para_SpeedBias[i][3] = Bas[i].x();\n para_SpeedBias[i][4] = Bas[i].y();\n para_SpeedBias[i][5] = Bas[i].z();\n\n para_SpeedBias[i][6] = Bgs[i].x();\n para_SpeedBias[i][7] = Bgs[i].y();\n para_SpeedBias[i][8] = Bgs[i].z();\n }\n }\n for (int i = 0; i < NUM_OF_CAM; i++)\n {\n para_Ex_Pose[i][0] = tic[i].x();\n para_Ex_Pose[i][1] = tic[i].y();\n para_Ex_Pose[i][2] = tic[i].z();\n Quaterniond q{ric[i]};\n para_Ex_Pose[i][3] = q.x();\n para_Ex_Pose[i][4] = q.y();\n para_Ex_Pose[i][5] = q.z();\n para_Ex_Pose[i][6] = q.w();\n }\n\n VectorXd dep = f_manager.getDepthVector();\n for (int i = 0; i < f_manager.getFeatureCount(); i++)\n para_Feature[i][0] = dep(i);\n if (ESTIMATE_TD)\n para_Td[0][0] = td;\n}\n\n// 数据转换，vector2double的相反过程\n// 同时这里为防止优化结果往零空间变化，会根据优化前后第一帧的位姿差进行修正。\nvoid Estimator::double2vector()\n{\n Vector3d origin_R0 = Utility::R2ypr(Rs[0]);\n Vector3d origin_P0 = Ps[0];\n\n if (failure_occur)\n {\n origin_R0 = Utility::R2ypr(last_R0);\n origin_P0 = last_P0;\n failure_occur = false;\n }\n if (USE_IMU)\n {\n Vector3d origin_R00 = Utility::R2ypr(\n Quaterniond(para_Pose[0][6], para_Pose[0][3], para_Pose[0][4], para_Pose[0][5])\n .toRotationMatrix());\n double y_diff = origin_R0.x() - origin_R00.x();\n // TODO\n Matrix3d rot_diff = Utility::ypr2R(Vector3d(y_diff, 0, 0));\n if (abs(abs(origin_R0.y()) - 90) < 1.0 || abs(abs(origin_R00.y()) - 90) < 1.0)\n {\n ROS_DEBUG(\"euler singular point!\");\n rot_diff = Rs[0] * Quaterniond(para_Pose[0][6], para_Pose[0][3], para_Pose[0][4],\n para_Pose[0][5])\n .toRotationMatrix()\n .transpose();\n }\n\n for (int i = 0; i <= WINDOW_SIZE; i++)\n {\n Rs[i] = rot_diff *\n Quaterniond(para_Pose[i][6], para_Pose[i][3], para_Pose[i][4], para_Pose[i][5])\n .normalized()\n .toRotationMatrix();\n\n Ps[i] = rot_diff * Vector3d(para_Pose[i][0] - para_Pose[0][0],\n para_Pose[i][1] - para_Pose[0][1],\n para_Pose[i][2] - para_Pose[0][2]) +\n origin_P0;\n\n Vs[i] = rot_diff *\n Vector3d(para_SpeedBias[i][0], para_SpeedBias[i][1], para_SpeedBias[i][2]);\n\n Bas[i] = Vector3d(para_SpeedBias[i][3], para_SpeedBias[i][4], para_SpeedBias[i][5]);\n\n Bgs[i] = Vector3d(para_SpeedBias[i][6], para_SpeedBias[i][7], para_SpeedBias[i][8]);\n }\n\n // relative info between two loop frame\n if (relocalization_info)\n {\n Matrix3d relo_r;\n Vector3d relo_t;\n relo_r = rot_diff * Quaterniond(relo_Pose[6], relo_Pose[3], relo_Pose[4], relo_Pose[5])\n .normalized()\n .toRotationMatrix();\n relo_t =\n rot_diff * Vector3d(relo_Pose[0] - para_Pose[0][0], relo_Pose[1] - para_Pose[0][1],\n relo_Pose[2] - para_Pose[0][2]) +\n origin_P0;\n double drift_correct_yaw;\n drift_correct_yaw = Utility::R2ypr(prev_relo_r).x() - Utility::R2ypr(relo_r).x();\n drift_correct_r = Utility::ypr2R(Vector3d(drift_correct_yaw, 0, 0));\n drift_correct_t = prev_relo_t - drift_correct_r * relo_t;\n relo_relative_t = relo_r.transpose() * (Ps[relo_frame_local_index] - relo_t);\n relo_relative_q = relo_r.transpose() * Rs[relo_frame_local_index];\n relo_relative_yaw = Utility::normalizeAngle(\n Utility::R2ypr(Rs[relo_frame_local_index]).x() - Utility::R2ypr(relo_r).x());\n // cout << \"vins relo \" << endl;\n // cout << \"vins relative_t \" << relo_relative_t.transpose() << endl;\n // cout << \"vins relative_yaw \" < 2.5)\n {\n ROS_INFO(\" big IMU acc bias estimation %f\", Bas[WINDOW_SIZE].norm());\n return true;\n }\n if (Bgs[WINDOW_SIZE].norm() > 1.0)\n {\n ROS_INFO(\" big IMU gyr bias estimation %f\", Bgs[WINDOW_SIZE].norm());\n return true;\n }\n /*\n if (tic(0) > 1)\n {\n ROS_INFO(\" big extri param estimation %d\", tic(0) > 1);\n return true;\n }\n */\n Vector3d tmp_P = Ps[WINDOW_SIZE];\n if ((tmp_P - last_P).norm() > 5)\n {\n ROS_INFO(\" big translation\");\n return true;\n }\n if (abs(tmp_P.z() - last_P.z()) > 1)\n {\n ROS_INFO(\" big z translation\");\n return true;\n }\n Matrix3d tmp_R = Rs[WINDOW_SIZE];\n Matrix3d delta_R = tmp_R.transpose() * last_R;\n Quaterniond delta_Q(delta_R);\n double delta_angle;\n delta_angle = acos(delta_Q.w()) * 2.0 / 3.14 * 180.0;\n if (delta_angle > 50)\n {\n ROS_INFO(\" big delta_angle \");\n // return true;\n }\n return false;\n}\n\nvoid Estimator::optimization()\n{\n TicToc t_whole, t_prepare;\n vector2double();\n\n ceres::Problem problem;\n ceres::LossFunction *loss_function;\n // loss_function = new ceres::HuberLoss(1.0);\n loss_function = new ceres::CauchyLoss(1.0);\n /*######优化参数：q、p；v、Ba、Bg#######*/\n //添加ceres参数块\n //因为ceres用的是double数组，所以在下面用vector2double做类型装换\n // Ps、Rs转变成para_Pose，Vs、Bas、Bgs转变成para_SpeedBias\n for (int i = 0; i < frame_count + 1; i++)\n {\n ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();\n problem.AddParameterBlock(para_Pose[i], SIZE_POSE, local_parameterization);\n if (USE_IMU)\n problem.AddParameterBlock(para_SpeedBias[i],\n SIZE_SPEEDBIAS); // v、Ba、Bg参数\n }\n if (!USE_IMU)\n {\n problem.SetParameterBlockConstant(para_Pose[0]);\n }\n /*######优化参数：imu与camera外参#######*/\n for (auto &i : para_Ex_Pose)\n {\n ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();\n problem.AddParameterBlock(i, SIZE_POSE, local_parameterization);\n if ((ESTIMATE_EXTRINSIC && frame_count == WINDOW_SIZE && Vs[0].norm() > 0.2) ||\n openExEstimation)\n {\n // ROS_DEBUG(\"estimate extinsic param\");\n openExEstimation = true;\n }\n else\n {\n // ROS_DEBUG(\"fix extinsic param\");\n problem.SetParameterBlockConstant(i);\n }\n }\n /*######优化参数：imu与camera之间的time offset#######*/\n if (USE_IMU)\n {\n problem.AddParameterBlock(para_Td[0], 1);\n //速度过低时，不估计td\n if (!ESTIMATE_TD || Vs[0].norm() < 0.2)\n {\n problem.SetParameterBlockConstant(para_Td[0]);\n }\n }\n\n //构建残差\n /*******先验残差*******/\n if (last_marginalization_info)\n {\n // construct new marginlization_factor\n MarginalizationFactor *marginalization_factor =\n new MarginalizationFactor(last_marginalization_info);\n problem.AddResidualBlock(marginalization_factor, NULL,\n last_marginalization_parameter_blocks);\n }\n\n /*******预积分残差*******/\n if (USE_IMU)\n {\n for (int i = 0; i < frame_count; i++) //预积分残差，总数目为frame_count\n {\n int j = i + 1;\n if (pre_integrations[j]->sum_dt >\n 10.0) //两图像帧之间时间过长，不使用中间的预积分 tzhang\n continue;\n IMUFactor *imu_factor = new IMUFactor(pre_integrations[j]);\n //添加残差格式：残差因子，鲁棒核函数，优化变量（i时刻位姿，i时刻速度与偏置，i+1时刻位姿，i+1时刻速度与偏置）\n problem.AddResidualBlock(imu_factor, NULL, para_Pose[i], para_SpeedBias[i],\n para_Pose[j], para_SpeedBias[j]);\n }\n }\n\n /*******重投影残差*******/\n //重投影残差相关，此时使用了Huber损失核函数\n int f_m_cnt = 0;\n int feature_index = -1;\n for (auto &it_per_id : f_manager.feature)\n {\n if (it_per_id.is_dynamic)\n {\n continue;\n }\n it_per_id.used_num = it_per_id.feature_per_frame.size();\n if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))\n continue;\n // if (it_per_id.used_num < 4)\n // continue;\n ++feature_index;\n\n int imu_i = it_per_id.start_frame, imu_j = imu_i - 1;\n\n Vector3d pts_i = it_per_id.feature_per_frame[0].point;\n\n for (auto &it_per_frame : it_per_id.feature_per_frame) //遍历观测到路标点的图像帧\n {\n imu_j++;\n if (imu_i == imu_j)\n {\n continue;\n }\n Vector3d pts_j = it_per_frame.point; //测量值\n if (ESTIMATE_TD)\n {\n ProjectionTdFactor *f_td = new ProjectionTdFactor(\n pts_i, pts_j, it_per_id.feature_per_frame[0].velocity, it_per_frame.velocity,\n it_per_id.feature_per_frame[0].cur_td, it_per_frame.cur_td,\n it_per_id.feature_per_frame[0].uv.y(), it_per_frame.uv.y());\n problem.AddResidualBlock(f_td, loss_function, para_Pose[imu_i], para_Pose[imu_j],\n para_Ex_Pose[0], para_Feature[feature_index], para_Td[0]);\n if (it_per_id.estimate_flag == 1 && FIX_DEPTH)\n problem.SetParameterBlockConstant(para_Feature[feature_index]);\n else if (it_per_id.estimate_flag == 2)\n {\n problem.SetParameterUpperBound(para_Feature[feature_index], 0,\n 2 / DEPTH_MAX_DIST);\n }\n }\n else\n {\n ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j);\n problem.AddResidualBlock(f, loss_function, para_Pose[imu_i], para_Pose[imu_j],\n para_Ex_Pose[0], para_Feature[feature_index]);\n if (it_per_id.estimate_flag == 1 && FIX_DEPTH)\n problem.SetParameterBlockConstant(para_Feature[feature_index]);\n else if (it_per_id.estimate_flag == 2)\n {\n // 防止远处的点深度过小\n problem.SetParameterUpperBound(para_Feature[feature_index], 0,\n 2 / DEPTH_MAX_DIST);\n }\n }\n f_m_cnt++;\n }\n }\n ROS_DEBUG(\"visual measurement count: %d\", f_m_cnt);\n ROS_DEBUG(\"prepare for ceres: %f\", t_prepare.toc());\n\n //添加闭环检测残差，计算滑动窗口中与每一个闭环关键帧的相对位姿，这个相对位置是为后面的图优化准备\n if (relocalization_info)\n {\n // printf(\"set relocalization factor! \\n\");\n ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();\n problem.AddParameterBlock(relo_Pose, SIZE_POSE, local_parameterization);\n int retrive_feature_index = 0;\n int feature_index = -1;\n for (auto &it_per_id : f_manager.feature)\n {\n if (it_per_id.is_dynamic)\n {\n continue;\n }\n it_per_id.used_num = it_per_id.feature_per_frame.size();\n if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))\n continue;\n // if (it_per_id.used_num < 4)\n // continue;\n ++feature_index;\n int start = it_per_id.start_frame;\n if (start <= relo_frame_local_index)\n {\n while ((int)match_points[retrive_feature_index].z() < it_per_id.feature_id)\n {\n retrive_feature_index++;\n }\n if ((int)match_points[retrive_feature_index].z() == it_per_id.feature_id)\n {\n Vector3d pts_j = Vector3d(match_points[retrive_feature_index].x(),\n match_points[retrive_feature_index].y(), 1.0);\n Vector3d pts_i = it_per_id.feature_per_frame[0].point;\n\n ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j);\n problem.AddResidualBlock(f, loss_function, para_Pose[start], relo_Pose,\n para_Ex_Pose[0], para_Feature[feature_index]);\n retrive_feature_index++;\n }\n }\n }\n }\n\n ceres::Solver::Options options;\n\n options.linear_solver_type = ceres::DENSE_SCHUR;\n // options.num_threads = 4;\n options.trust_region_strategy_type = ceres::DOGLEG;\n options.max_num_iterations = NUM_ITERATIONS;\n // options.use_explicit_schur_complement = true;\n // options.minimizer_progress_to_stdout = false;\n // options.use_nonmonotonic_steps = true;\n if (marginalization_flag == MARGIN_OLD)\n options.max_solver_time_in_seconds = SOLVER_TIME * 4.0 / 5.0;\n else\n options.max_solver_time_in_seconds = SOLVER_TIME;\n TicToc t_solver;\n ceres::Solver::Summary summary;\n ceres::Solve(options, &problem, &summary);\n // cout << summary.BriefReport() << endl;\n ROS_DEBUG(\"Iterations : %d\", static_cast(summary.iterations.size()));\n\n // 防止优化结果在零空间变化，通过固定第一帧的位姿\n double2vector();\n\n if (frame_count < WINDOW_SIZE)\n return;\n\n TicToc t_whole_marginalization;\n //边缘化处理\n //如果次新帧是关键帧，将边缘化最老帧，及其看到的路标点和IMU数据，将其转化为先验：\n if (marginalization_flag == MARGIN_OLD)\n {\n MarginalizationInfo *marginalization_info = new MarginalizationInfo();\n vector2double();\n\n // 先验部分，基于先验残差，边缘化滑窗中第0帧时刻的状态向量\n if (last_marginalization_info)\n {\n vector drop_set;\n for (int i = 0; i < static_cast(last_marginalization_parameter_blocks.size()); i++)\n {\n if (last_marginalization_parameter_blocks[i] == para_Pose[0] ||\n last_marginalization_parameter_blocks[i] == para_SpeedBias[0])\n drop_set.push_back(i);\n }\n // construct new marginlization_factor\n MarginalizationFactor *marginalization_factor =\n new MarginalizationFactor(last_marginalization_info);\n ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(\n marginalization_factor, NULL, last_marginalization_parameter_blocks, drop_set);\n\n marginalization_info->addResidualBlockInfo(residual_block_info);\n }\n\n if (USE_IMU)\n {\n // imu\n // 预积分部分，基于第0帧与第1帧之间的预积分残差，边缘化第0帧状态向量\n if (pre_integrations[1]->sum_dt < 10.0)\n {\n IMUFactor *imu_factor = new IMUFactor(pre_integrations[1]);\n ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(\n imu_factor, NULL,\n vector{para_Pose[0], para_SpeedBias[0], para_Pose[1],\n para_SpeedBias[1]},\n vector{0, 1}); //边缘化 para_Pose[0], para_SpeedBias[0]\n marginalization_info->addResidualBlockInfo(residual_block_info);\n }\n }\n\n //图像部分，基于与第0帧相关的图像残差，边缘化第一次观测的图像帧为第0帧的路标点和第0帧\n {\n int feature_index = -1;\n for (auto &it_per_id : f_manager.feature)\n {\n if (it_per_id.is_dynamic)\n continue;\n it_per_id.used_num = it_per_id.feature_per_frame.size();\n // if (it_per_id.used_num < 4)\n // continue;\n if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))\n continue;\n\n ++feature_index;\n\n int imu_i = it_per_id.start_frame, imu_j = imu_i - 1;\n if (imu_i != 0) //仅处理第一次观测的图像帧为第0帧的情形\n continue;\n\n Vector3d pts_i = it_per_id.feature_per_frame[0].point;\n\n for (auto &it_per_frame :\n it_per_id.feature_per_frame) //对观测到路标点的图像帧的遍历\n {\n imu_j++;\n if (imu_i == imu_j)\n continue;\n\n Vector3d pts_j = it_per_frame.point;\n if (ESTIMATE_TD)\n {\n ProjectionTdFactor *f_td = new ProjectionTdFactor(\n pts_i, pts_j, it_per_id.feature_per_frame[0].velocity,\n it_per_frame.velocity, it_per_id.feature_per_frame[0].cur_td,\n it_per_frame.cur_td, it_per_id.feature_per_frame[0].uv.y(),\n it_per_frame.uv.y());\n ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(\n f_td, loss_function,\n vector{para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0],\n para_Feature[feature_index], para_Td[0]},\n vector{0, 3});\n marginalization_info->addResidualBlockInfo(residual_block_info);\n }\n else\n {\n ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j);\n ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(\n f, loss_function,\n vector{para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0],\n para_Feature[feature_index]},\n vector{0, 3});\n marginalization_info->addResidualBlockInfo(residual_block_info);\n }\n }\n }\n }\n\n TicToc t_pre_margin;\n marginalization_info->preMarginalize();\n ROS_DEBUG(\"pre marginalization %f ms\", t_pre_margin.toc());\n\n TicToc t_margin;\n marginalization_info->marginalize();\n ROS_DEBUG(\"marginalization %f ms\", t_margin.toc());\n\n //仅仅改变滑窗double部分地址映射，具体值的通过slideWindow和vector2double函数完成；记住边缘化仅仅改变A和b，不改变状态向量\n //由于第0帧观测到的路标点全被边缘化，即边缘化后保存的状态向量中没有路标点;因此addr_shift无需添加路标点\n std::unordered_map addr_shift;\n for (int i = 1; i <= WINDOW_SIZE; i++) //最老图像帧数据丢弃，从i=1开始遍历\n {\n addr_shift[reinterpret_cast(para_Pose[i])] =\n para_Pose[i - 1]; // i数据保存到1-1指向的地址，滑窗向前移动一格\n if (USE_IMU)\n addr_shift[reinterpret_cast(para_SpeedBias[i])] = para_SpeedBias[i - 1];\n }\n for (auto &i : para_Ex_Pose)\n addr_shift[reinterpret_cast(i)] = i;\n if (ESTIMATE_TD)\n {\n addr_shift[reinterpret_cast(para_Td[0])] = para_Td[0];\n }\n vector parameter_blocks = marginalization_info->getParameterBlocks(addr_shift);\n\n delete last_marginalization_info;\n last_marginalization_info = marginalization_info;\n last_marginalization_parameter_blocks = parameter_blocks;\n }\n else //将次新的图像帧数据边缘化； tzhang\n {\n if (last_marginalization_info &&\n std::count(std::begin(last_marginalization_parameter_blocks),\n std::end(last_marginalization_parameter_blocks), para_Pose[WINDOW_SIZE - 1]))\n {\n MarginalizationInfo *marginalization_info = new MarginalizationInfo();\n vector2double();\n if (last_marginalization_info)\n {\n vector\n drop_set; //记录需要丢弃的变量在last_marginalization_parameter_blocks中的索引\n for (int i = 0; i < static_cast(last_marginalization_parameter_blocks.size());\n i++)\n {\n ROS_ASSERT(last_marginalization_parameter_blocks[i] !=\n para_SpeedBias[WINDOW_SIZE - 1]);\n if (last_marginalization_parameter_blocks[i] == para_Pose[WINDOW_SIZE - 1])\n drop_set.push_back(i);\n }\n // construct new marginlization_factor\n MarginalizationFactor *marginalization_factor =\n new MarginalizationFactor(last_marginalization_info);\n ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(\n marginalization_factor, NULL, last_marginalization_parameter_blocks, drop_set);\n\n marginalization_info->addResidualBlockInfo(residual_block_info);\n }\n\n TicToc t_pre_margin;\n // ROS_DEBUG(\"begin marginalization\");\n marginalization_info->preMarginalize();\n ROS_DEBUG(\"end pre marginalization, %f ms\", t_pre_margin.toc());\n\n TicToc t_margin;\n // ROS_DEBUG(\"begin marginalization\");\n marginalization_info->marginalize();\n ROS_DEBUG(\"end marginalization, %f ms\", t_margin.toc());\n\n std::unordered_map addr_shift;\n for (int i = 0; i <= WINDOW_SIZE; i++)\n {\n if (i == WINDOW_SIZE - 1) // WINDOW_SIZE - 1会被边缘化，不保存\n continue;\n else if (i == WINDOW_SIZE) // WINDOW_SIZE数据保存到WINDOW_SIZE-1指向的地址\n {\n addr_shift[reinterpret_cast(para_Pose[i])] = para_Pose[i - 1];\n\n addr_shift[reinterpret_cast(para_SpeedBias[i])] = para_SpeedBias[i - 1];\n }\n else\n {\n addr_shift[reinterpret_cast(para_Pose[i])] = para_Pose[i];\n\n addr_shift[reinterpret_cast(para_SpeedBias[i])] = para_SpeedBias[i];\n }\n }\n for (auto &i : para_Ex_Pose)\n addr_shift[reinterpret_cast(i)] = i;\n if (ESTIMATE_TD)\n {\n addr_shift[reinterpret_cast(para_Td[0])] = para_Td[0];\n }\n\n vector parameter_blocks =\n marginalization_info->getParameterBlocks(addr_shift);\n\n delete last_marginalization_info;\n last_marginalization_info = marginalization_info;\n last_marginalization_parameter_blocks = parameter_blocks;\n }\n }\n ROS_DEBUG(\"whole marginalization costs: %f\", t_whole_marginalization.toc());\n\n ROS_DEBUG(\"whole time for ceres: %f\", t_whole.toc());\n}\n\nvoid Estimator::slideWindow()\n{\n TicToc t_margin;\n if (marginalization_flag == MARGIN_OLD) // 边缘化最老的图像帧，即次新的图像帧为关键帧\n {\n double t_0 = Headers[0];\n back_R0 = Rs[0];\n back_P0 = Ps[0];\n if (frame_count == WINDOW_SIZE)\n {\n // 1、滑窗中的数据往前移动一帧；运行结果就是WINDOW_SIZE位置的状态为之前0位置对应的状态\n // 0,1,2...WINDOW_SIZE——>1,2...WINDOW_SIZE,0\n for (int i = 0; i < WINDOW_SIZE; i++)\n {\n Headers[i] = Headers[i + 1];\n Ps[i].swap(Ps[i + 1]);\n Rs[i].swap(Rs[i + 1]);\n if (USE_IMU)\n {\n std::swap(pre_integrations[i], pre_integrations[i + 1]);\n\n dt_buf[i].swap(dt_buf[i + 1]);\n linear_acceleration_buf[i].swap(linear_acceleration_buf[i + 1]);\n angular_velocity_buf[i].swap(angular_velocity_buf[i + 1]);\n\n Vs[i].swap(Vs[i + 1]);\n Bas[i].swap(Bas[i + 1]);\n Bgs[i].swap(Bgs[i + 1]);\n }\n ++find_solved[i + 1];\n find_solved[i] = find_solved[i + 1];\n }\n // 2、处理前，WINDOW_SIZE位置的状态为之前0位置对应的状态；处理后，WINDOW_SIZE位置的状态为之前WINDOW_SIZE位置对应的状态;之前0位置对应的状态被剔除\n // 0,1,2...WINDOW_SIZE——>1,2...WINDOW_SIZE,WINDOW_SIZE\n Headers[WINDOW_SIZE] = Headers[WINDOW_SIZE - 1];\n Ps[WINDOW_SIZE] = Ps[WINDOW_SIZE - 1];\n Rs[WINDOW_SIZE] = Rs[WINDOW_SIZE - 1];\n if (USE_IMU)\n {\n Vs[WINDOW_SIZE] = Vs[WINDOW_SIZE - 1];\n Bas[WINDOW_SIZE] = Bas[WINDOW_SIZE - 1];\n Bgs[WINDOW_SIZE] = Bgs[WINDOW_SIZE - 1];\n\n delete pre_integrations[WINDOW_SIZE];\n pre_integrations[WINDOW_SIZE] =\n new IntegrationBase{acc_0, gyr_0, Bas[WINDOW_SIZE], Bgs[WINDOW_SIZE]};\n\n dt_buf[WINDOW_SIZE].clear();\n linear_acceleration_buf[WINDOW_SIZE].clear();\n angular_velocity_buf[WINDOW_SIZE].clear();\n }\n find_solved[WINDOW_SIZE] = 0;\n\n // 3、对时刻t_0(对应滑窗第0帧)之前的所有数据进行剔除；即all_image_frame中仅保留滑窗中图像帧0与图像帧WINDOW_SIZE之间的数据\n map::iterator it_0;\n it_0 = all_image_frame.find(t_0);\n delete it_0->second.pre_integration;\n it_0->second.pre_integration = nullptr;\n for (auto it = all_image_frame.begin(); it != it_0; ++it)\n {\n delete it->second.pre_integration;\n it->second.pre_integration = NULL;\n }\n all_image_frame.erase(all_image_frame.begin(), it_0);\n all_image_frame.erase(t_0);\n slideWindowOld();\n }\n }\n else //边缘化次新的图像帧，主要完成的工作是数据衔接 tzhang\n { // 0,1,2...WINDOW_SIZE-2, WINDOW_SIZE-1,\n // WINDOW_SIZE——>0,,1,2...WINDOW_SIZE-2,WINDOW_SIZE, WINDOW_SIZE\n if (frame_count == WINDOW_SIZE)\n {\n Headers[frame_count - 1] = Headers[frame_count];\n Ps[frame_count - 1] = Ps[frame_count];\n Rs[frame_count - 1] = Rs[frame_count];\n\n find_solved[WINDOW_SIZE] = 0;\n\n if (USE_IMU)\n {\n for (unsigned int i = 0; i < dt_buf[frame_count].size(); i++)\n {\n double tmp_dt = dt_buf[frame_count][i];\n Vector3d tmp_linear_acceleration = linear_acceleration_buf[frame_count][i];\n Vector3d tmp_angular_velocity = angular_velocity_buf[frame_count][i];\n\n pre_integrations[frame_count - 1]->push_back(tmp_dt, tmp_linear_acceleration,\n tmp_angular_velocity);\n\n dt_buf[frame_count - 1].push_back(tmp_dt);\n linear_acceleration_buf[frame_count - 1].push_back(tmp_linear_acceleration);\n angular_velocity_buf[frame_count - 1].push_back(tmp_angular_velocity);\n }\n Vs[frame_count - 1] = Vs[frame_count];\n Bas[frame_count - 1] = Bas[frame_count];\n Bgs[frame_count - 1] = Bgs[frame_count];\n\n delete pre_integrations[WINDOW_SIZE];\n pre_integrations[WINDOW_SIZE] =\n new IntegrationBase{acc_0, gyr_0, Bas[WINDOW_SIZE], Bgs[WINDOW_SIZE]};\n\n dt_buf[WINDOW_SIZE].clear();\n linear_acceleration_buf[WINDOW_SIZE].clear();\n angular_velocity_buf[WINDOW_SIZE].clear();\n }\n slideWindowNew();\n }\n }\n}\n\n// real marginalization is removed in solve_ceres()\nvoid Estimator::slideWindowNew()\n{\n sum_of_front++;\n f_manager.removeFront(frame_count);\n}\n\n// real marginalization is removed in solve_ceres()\nvoid Estimator::slideWindowOld()\n{\n sum_of_back++;\n\n bool shift_depth = solver_flag == NON_LINEAR ? true : false;\n if (shift_depth)\n {\n Matrix3d R0, R1;\n Vector3d P0, P1;\n R0 = back_R0 * ric[0];\n R1 = Rs[0] * ric[0];\n P0 = back_P0 + back_R0 * tic[0];\n P1 = Ps[0] + Rs[0] * tic[0];\n f_manager.removeBackShiftDepth(R0, P0, R1, P1);\n }\n else\n f_manager.removeBack();\n}\n\n/**\n * @brief 进行重定位\n * @optional\n * @param[in] _frame_stamp 重定位帧时间戳\n * @param[in] _frame_index 重定位帧索引值\n * @param[in] _match_points 重定位帧的所有匹配点\n * @param[in] _relo_t 重定位帧平移向量\n * @param[in] _relo_r 重定位帧旋转矩阵\n * @return void\n */\nvoid Estimator::setReloFrame(double _frame_stamp, int _frame_index, vector &_match_points,\n Vector3d _relo_t, Matrix3d _relo_r)\n{\n relo_frame_stamp = _frame_stamp;\n relo_frame_index = _frame_index;\n match_points.clear();\n match_points = _match_points;\n prev_relo_t = std::move(_relo_t);\n prev_relo_r = std::move(_relo_r);\n for (int i = 0; i < WINDOW_SIZE; i++)\n {\n if (relo_frame_stamp == Headers[i])\n {\n relo_frame_local_index = i;\n relocalization_info = true;\n for (int j = 0; j < SIZE_POSE; j++)\n relo_Pose[j] = para_Pose[i][j];\n }\n }\n}\n\nvoid Estimator::inputIMU(double t, const Vector3d &linearAcceleration,\n const Vector3d &angularVelocity)\n{\n m_imu.lock();\n imu_buf.push(make_pair(t, make_pair(linearAcceleration, angularVelocity)));\n // imu_predict_buf.push(\n // make_pair(t, make_pair(linearAcceleration, angularVelocity)));\n m_imu.unlock();\n\n if (solver_flag == Estimator::SolverFlag::NON_LINEAR)\n {\n // predict imu (no residual error)\n m_propagate.lock();\n predict(t, linearAcceleration, angularVelocity);\n m_propagate.unlock();\n pubLatestOdometry(latest_P, latest_Q, latest_V, t);\n }\n}\n\nvoid Estimator::updateLatestStates()\n{\n m_propagate.lock();\n latest_time = Headers[frame_count] + td;\n latest_P = Ps[frame_count];\n latest_Q = Rs[frame_count];\n latest_V = Vs[frame_count];\n latest_Ba = Bas[frame_count];\n latest_Bg = Bgs[frame_count];\n\n if (USE_IMU)\n {\n m_imu.lock();\n queue>> tmp_imu_buf = imu_buf;\n for (sensor_msgs::ImuConstPtr tmp_imu_msg; !tmp_imu_buf.empty(); tmp_imu_buf.pop())\n predict(tmp_imu_buf.front().first, imu_buf.front().second.first,\n imu_buf.front().second.second);\n m_imu.unlock();\n }\n m_propagate.unlock();\n}\n\nMatrix3d Estimator::predictMotion(double t0, double t1)\n{\n Matrix3d relative_R = Matrix3d::Identity();\n if (imu_buf.empty())\n return relative_R;\n\n bool first_imu = true;\n double prev_imu_time;\n Eigen::Vector3d prev_gyr;\n\n m_imu.lock();\n queue>> imu_predict_buf = imu_buf;\n m_imu.unlock();\n if (t1 <= imu_predict_buf.back().first)\n {\n while (imu_predict_buf.front().first <= t0)\n {\n imu_predict_buf.pop();\n }\n while (imu_predict_buf.front().first <= t1 && !imu_predict_buf.empty())\n {\n double t = imu_predict_buf.front().first;\n\n Eigen::Vector3d angular_velocity = imu_predict_buf.front().second.second;\n\n // Eigen::Vector3d linear_acceleration{\n // imu_predict_buf.front()->linear_acceleration.x,\n // imu_predict_buf.front()->linear_acceleration.y,\n // imu_predict_buf.front()->linear_acceleration.z};\n\n imu_predict_buf.pop();\n\n if (first_imu)\n {\n prev_imu_time = t;\n first_imu = false;\n prev_gyr = angular_velocity;\n continue;\n }\n double dt = t - prev_imu_time;\n prev_imu_time = t;\n\n // Eigen::Vector3d un_acc_0 = tmp_Q * (acc_0 - tmp_Ba) - estimator.g;\n\n Eigen::Vector3d un_gyr = 0.5 * (prev_gyr + angular_velocity) - latest_Bg;\n // tmp_Q = tmp_Q * Utility::deltaQ(un_gyr * dt);\n\n // Eigen::Vector3d un_acc_1 =\n // tmp_Q * (linear_acceleration - tmp_Ba) - estimator.g;\n\n // un_acc = 0.5 * (un_acc_0 + un_acc_1);\n\n // tmp_P = tmp_P + dt * tmp_V + 0.5 * dt * dt * un_acc;\n // tmp_V = tmp_V + dt * un_acc;\n\n // acc_0 = linear_acceleration;\n prev_gyr = angular_velocity;\n\n // cl\n // Transform the mean angular velocity from the IMU\n // frame to the cam0 frames.\n // Compute the relative rotation.\n Vector3d cam0_angle_axisd = RIC.back().transpose() * un_gyr * dt;\n relative_R *= AngleAxisd(cam0_angle_axisd.norm(), cam0_angle_axisd.normalized())\n .toRotationMatrix()\n .transpose();\n }\n }\n\n return relative_R;\n}\n\nvoid Estimator::predict(double t, const Vector3d &linearAcceleration,\n const Vector3d &angularVelocity)\n{\n if (init_imu)\n {\n latest_time = t;\n init_imu = false;\n return;\n }\n double dt = t - latest_time;\n latest_time = t;\n Eigen::Vector3d un_acc_0 = latest_Q * (acc_0 - latest_Ba) - g;\n Eigen::Vector3d un_gyr = 0.5 * (gyr_0 + angularVelocity) - latest_Bg;\n latest_Q = latest_Q * Utility::deltaQ(un_gyr * dt);\n Eigen::Vector3d un_acc_1 = latest_Q * (linearAcceleration - latest_Ba) - g;\n Eigen::Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);\n latest_P = latest_P + dt * latest_V + 0.5 * dt * dt * un_acc;\n latest_V = latest_V + dt * un_acc;\n}\n\nbool Estimator::IMUAvailable(double t)\n{\n if (!imu_buf.empty() && t <= imu_buf.back().first)\n return true;\n else\n return false;\n}\n\nvoid Estimator::initFirstIMUPose(\n std::vector>> &imu_vector)\n{\n printf(\"init first imu pose\\n\");\n initFirstPoseFlag = true;\n // return;\n Eigen::Vector3d averAcc(0, 0, 0);\n int n = (int)imu_vector.size();\n for (auto &i : imu_vector)\n {\n averAcc = averAcc + i.second.first;\n }\n averAcc = averAcc / n;\n printf(\"averge acc %f %f %f\\n\", averAcc.x(), averAcc.y(), averAcc.z());\n Matrix3d R0 = Utility::g2R(averAcc);\n double yaw = Utility::R2ypr(R0).x();\n R0 = Utility::ypr2R(Eigen::Vector3d{-yaw, 0, 0}) * R0;\n Rs[0] = R0;\n cout << \"init R0 \" << endl << Rs[0] << endl;\n}\nbool Estimator::getIMUInterval(\n double t0, double t1,\n std::vector>> &imu_vector)\n{\n m_imu.lock();\n if (imu_buf.empty())\n {\n printf(\"not receive imu\\n\");\n m_imu.unlock();\n return false;\n }\n if (t1 <= imu_buf.back().first)\n {\n while (imu_buf.front().first <= t0)\n {\n imu_buf.pop();\n }\n while (imu_buf.front().first < t1)\n {\n imu_vector.emplace_back(std::move(imu_buf.front()));\n imu_buf.pop();\n }\n imu_vector.emplace_back(imu_buf.front());\n m_imu.unlock();\n return true;\n }\n else\n {\n printf(\"wait for imu\\n\");\n m_imu.unlock();\n return false;\n }\n}\n\ndouble Estimator::reprojectionError(Matrix3d &Ri, Vector3d &Pi, Matrix3d &rici, Vector3d &tici,\n Matrix3d &Rj, Vector3d &Pj, Matrix3d &ricj, Vector3d &ticj,\n double depth, Vector3d &uvi, Vector3d &uvj)\n{\n Vector3d pts_w = Ri * (rici * (depth * uvi) + tici) + Pi;\n Vector3d pts_cj = ricj.transpose() * (Rj.transpose() * (pts_w - Pj) - ticj);\n Vector2d residual = (pts_cj / pts_cj.z()).head<2>() - uvj.head<2>();\n double rx = residual.x();\n double ry = residual.y();\n return sqrt(rx * rx + ry * ry);\n}\n\ndouble Estimator::reprojectionError3D(Matrix3d &Ri, Vector3d &Pi, Matrix3d &rici, Vector3d &tici,\n Matrix3d &Rj, Vector3d &Pj, Matrix3d &ricj, Vector3d &ticj,\n double depth, Vector3d &uvi, Vector3d &uvj)\n{\n Vector3d pts_w = Ri * (rici * (depth * uvi) + tici) + Pi;\n Vector3d pts_cj = ricj.transpose() * (Rj.transpose() * (pts_w - Pj) - ticj);\n return (pts_cj - uvj).norm() / depth;\n}\n\nvoid Estimator::movingConsistencyCheck(set &removeIndex)\n{\n for (auto &it_per_id : f_manager.feature)\n {\n it_per_id.used_num = it_per_id.feature_per_frame.size();\n if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))\n continue;\n\n double depth = it_per_id.estimated_depth;\n if (depth < 0)\n continue;\n\n double err = 0;\n double err3D = 0;\n int errCnt = 0;\n int imu_i = it_per_id.start_frame, imu_j = imu_i - 1;\n Vector3d pts_i = it_per_id.feature_per_frame[0].point;\n for (auto &it_per_frame : it_per_id.feature_per_frame)\n {\n imu_j++;\n if (imu_i != imu_j)\n {\n Vector3d pts_j = it_per_frame.point;\n err += reprojectionError(Rs[imu_i], Ps[imu_i], ric[0], tic[0], Rs[imu_j], Ps[imu_j],\n ric[0], tic[0], depth, pts_i, pts_j);\n // for bleeding points\n err3D += reprojectionError3D(Rs[imu_i], Ps[imu_i], ric[0], tic[0], Rs[imu_j],\n Ps[imu_j], ric[0], tic[0], depth, pts_i, pts_j);\n errCnt++;\n }\n }\n if (errCnt > 0)\n {\n if (FOCAL_LENGTH * err / errCnt > 10 || err3D / errCnt > 2.0)\n {\n removeIndex.insert(it_per_id.feature_id);\n it_per_id.is_dynamic = true;\n }\n else\n {\n it_per_id.is_dynamic = false;\n }\n }\n }\n}\n" }, { "path": "vins_estimator/src/estimator/estimator.h", "content": "#pragma once\n\n#include \n#include \n\n#include \"../feature_manager/feature_manager.h\"\n#include \"../initial/initial_alignment.h\"\n#include \"../initial/initial_ex_rotation.h\"\n#include \"../initial/initial_sfm.h\"\n#include \"../initial/solve_5pts.h\"\n#include \"../utility/parameters.h\"\n#include \"../utility/tic_toc.h\"\n#include \"../utility/utility.h\"\n\n#include \"../feature_tracker/feature_tracker.h\"\n\n#include \n#include \n#include \n\n#include \"../factor/imu_factor.h\"\n#include \"../factor/marginalization_factor.h\"\n#include \"../factor/pose_local_parameterization.h\"\n#include \"../factor/projection_factor.h\"\n#include \"../factor/projection_td_factor.h\"\n#include \n\n#include \n#include \n#include \n\n#include \n#include \n\nclass Estimator\n{\npublic:\n Estimator();\n\n void setParameter();\n\n // interface\n void processIMU(double t, const Vector3d &linear_acceleration,\n const Vector3d &angular_velocity);\n\n void processImage(map> &image, const std_msgs::Header &header);\n\n void setReloFrame(double _frame_stamp, int _frame_index, vector &_match_points,\n Vector3d _relo_t, Matrix3d _relo_r);\n\n // internal\n void clearState();\n\n bool initialStructure();\n\n bool visualInitialAlign();\n\n bool visualInitialAlignWithDepth();\n\n bool relativePose(Matrix3d &relative_R, Vector3d &relative_T, int &l);\n\n void slideWindow();\n\n void slideWindowNew();\n\n void slideWindowOld();\n\n void solveOdometry();\n\n void optimization();\n\n void vector2double();\n\n void double2vector();\n\n bool failureDetection();\n\n bool staticInitialAlignWithDepth();\n\n void updateLatestStates();\n\n Matrix3d predictMotion(double t0, double t1);\n\n void inputIMU(double t, const Vector3d &linearAcceleration, const Vector3d &angularVelocity);\n\n void predict(double t, const Vector3d &linearAcceleration, const Vector3d &angularVelocity);\n\n bool IMUAvailable(double t);\n\n bool initialStructureWithDepth();\n void movingConsistencyCheck(set &removeIndex);\n\n double reprojectionError(Matrix3d &Ri, Vector3d &Pi, Matrix3d &rici, Vector3d &tici,\n Matrix3d &Rj, Vector3d &Pj, Matrix3d &ricj, Vector3d &ticj,\n double depth, Vector3d &uvi, Vector3d &uvj);\n double reprojectionError3D(Matrix3d &Ri, Vector3d &Pi, Matrix3d &rici, Vector3d &tici,\n Matrix3d &Rj, Vector3d &Pj, Matrix3d &ricj, Vector3d &ticj,\n double depth, Vector3d &uvi, Vector3d &uvj);\n\n bool\n getIMUInterval(double t0, double t1,\n std::vector>> &imu_vector);\n void\n initFirstIMUPose(std::vector>> &imu_vector);\n enum SolverFlag\n {\n INITIAL,\n NON_LINEAR\n };\n\n enum MarginalizationFlag\n {\n MARGIN_OLD = 0,\n MARGIN_SECOND_NEW = 1\n };\n bool openExEstimation;\n FeatureTracker featureTracker;\n\n SolverFlag solver_flag;\n MarginalizationFlag marginalization_flag;\n Vector3d g;\n // extrinsic\n Matrix3d ric[NUM_OF_CAM];\n Vector3d tic[NUM_OF_CAM];\n\n // VIO state vector\n Vector3d Ps[(WINDOW_SIZE + 1)];\n Vector3d Vs[(WINDOW_SIZE + 1)];\n Matrix3d Rs[(WINDOW_SIZE + 1)];\n Vector3d Bas[(WINDOW_SIZE + 1)];\n Vector3d Bgs[(WINDOW_SIZE + 1)];\n double td{};\n\n Matrix3d back_R0, last_R, last_R0;\n Vector3d back_P0, last_P, last_P0;\n double Headers[(WINDOW_SIZE + 1)];\n\n IntegrationBase *pre_integrations[(WINDOW_SIZE + 1)]{};\n Vector3d acc_0, gyr_0;\n\n vector dt_buf[(WINDOW_SIZE + 1)];\n vector linear_acceleration_buf[(WINDOW_SIZE + 1)];\n vector angular_velocity_buf[(WINDOW_SIZE + 1)];\n\n int frame_count{}; // cl:滑动窗口中帧的数目,最大为滑窗大小\n int sum_of_back{}, sum_of_front{}, sum_of_invalid{};\n\n FeatureManager f_manager;\n MotionEstimator m_estimator;\n InitialEXRotation initial_ex_rotation;\n\n bool first_imu{};\n bool is_valid{};\n bool failure_occur{};\n\n vector key_poses;\n double initial_timestamp{};\n\n double para_Pose[WINDOW_SIZE + 1][SIZE_POSE]{};\n\n double para_SpeedBias[WINDOW_SIZE + 1][SIZE_SPEEDBIAS]{};\n double para_Feature[NUM_OF_F][SIZE_FEATURE]{};\n double para_Ex_Pose[NUM_OF_CAM][SIZE_POSE]{};\n double para_Td[1][1]{};\n int find_solved[WINDOW_SIZE + 1]{};\n\n MarginalizationInfo *last_marginalization_info{};\n vector last_marginalization_parameter_blocks;\n\n map all_image_frame;\n IntegrationBase *tmp_pre_integration{};\n\n // relocalization variable\n bool relocalization_info{};\n double relo_frame_stamp{};\n double relo_frame_index{};\n int relo_frame_local_index{};\n vector match_points;\n double relo_Pose[SIZE_POSE]{};\n Matrix3d drift_correct_r;\n Vector3d drift_correct_t;\n Vector3d prev_relo_t;\n Matrix3d prev_relo_r;\n Vector3d relo_relative_t;\n Quaterniond relo_relative_q;\n double relo_relative_yaw{};\n\n std::mutex m_imu, m_propagate;\n\n bool init_imu{};\n double prevTime = -1;\n\n queue>> imu_buf;\n\n double latest_time{};\n Eigen::Vector3d latest_P;\n Eigen::Quaterniond latest_Q;\n Eigen::Vector3d latest_V;\n Eigen::Vector3d latest_Ba;\n Eigen::Vector3d latest_Bg;\n bool initFirstPoseFlag{};\n};\n" }, { "path": "vins_estimator/src/estimator_nodelet.cpp", "content": "#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n\n#include \"estimator/estimator.h\"\n#include \"feature_tracker/feature_tracker.h\"\n#include \"ros/console_backend.h\"\n#include \"sensor_msgs/image_encodings.h\"\n#include \"utility/parameters.h\"\n#include \"utility/tic_toc.h\"\n#include \"utility/visualization.h\"\n\n#include // 基类Nodelet所在的头文件\n#include \n\nnamespace estimator_nodelet_ns\n{\nclass EstimatorNodelet : public nodelet::Nodelet //任何nodelet plugin都要继承Nodelet类。\n{\npublic:\n EstimatorNodelet() = default;\n\nprivate:\n void onInit() override\n {\n ros::NodeHandle &pn = getPrivateNodeHandle();\n ros::NodeHandle &nh = getMTNodeHandle();\n\n ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);\n\n readParameters(pn);\n\n estimator.setParameter();\n#ifdef EIGEN_DONT_PARALLELIZE\n ROS_DEBUG(\"EIGEN_DONT_PARALLELIZE\");\n#endif\n ROS_WARN(\"waiting for image, semantic and imu...\");\n\n registerPub(nh);\n\n sub_image = nh.subscribe(IMAGE_TOPIC, 1000, &EstimatorNodelet::image_callback, this);\n sub_depth = nh.subscribe(DEPTH_TOPIC, 1000, &EstimatorNodelet::depth_callback, this);\n\n if (USE_IMU)\n sub_imu = nh.subscribe(IMU_TOPIC, 1000, &EstimatorNodelet::imu_callback, this,\n ros::TransportHints().tcpNoDelay());\n // topic from pose_graph, notify if there's relocalization\n sub_relo_points = nh.subscribe(\"/pose_graph/match_points\", 10,\n &EstimatorNodelet::relocalization_callback, this);\n\n dura = std::chrono::milliseconds(2);\n\n trackThread = std::thread(&EstimatorNodelet::process_tracker, this);\n processThread = std::thread(&EstimatorNodelet::process, this);\n }\n\n Estimator estimator;\n\n // thread relevance\n std::thread trackThread, processThread;\n std::chrono::milliseconds dura;\n std::condition_variable con_tracker;\n std::condition_variable con_estimator;\n std::mutex m_feature;\n std::mutex m_backend;\n std::mutex m_buf;\n std::mutex m_vis;\n\n // ROS and data buf relevance\n ros::Subscriber sub_imu, sub_relo_points, sub_image, sub_depth;\n queue img_buf;\n queue depth_buf;\n queue,\n map>>>\n feature_buf;\n queue relo_buf;\n queue> vis_img_buf;\n\n bool init_feature = false;\n bool init_pub = false;\n\n // frequency control relevance\n bool first_image_flag = true;\n double first_image_time = 0;\n double last_image_time = 0;\n int pub_count = 1;\n int input_count = 0;\n\n double last_imu_t = 0;\n\n void imu_callback(const sensor_msgs::ImuConstPtr &imu_msg)\n {\n /**\n * @brief nullptr may cause crash\n * typename boost::detail::sp_member_access::type = const\n * sensor_msgs::Imu_ >*]: Assertion `px != 0' failed.\n * https://github.com/mavlink/mavros/issues/432\n * https://github.com/mavlink/mavros/pull/434\n */\n if (imu_msg)\n {\n if (imu_msg->header.stamp.toSec() <= last_imu_t)\n {\n ROS_WARN(\"imu message in disorder! %f\", imu_msg->header.stamp.toSec());\n return;\n }\n\n last_imu_t = imu_msg->header.stamp.toSec();\n Vector3d acc(imu_msg->linear_acceleration.x, imu_msg->linear_acceleration.y,\n imu_msg->linear_acceleration.z);\n Vector3d gyr(imu_msg->angular_velocity.x, imu_msg->angular_velocity.y,\n imu_msg->angular_velocity.z);\n estimator.inputIMU(last_imu_t, acc, gyr);\n }\n }\n\n void image_callback(const sensor_msgs::ImageConstPtr &color_msg)\n {\n m_buf.lock();\n img_buf.emplace(color_msg);\n m_buf.unlock();\n con_tracker.notify_one();\n }\n\n void depth_callback(const sensor_msgs::ImageConstPtr &depth_msg)\n {\n m_buf.lock();\n depth_buf.emplace(depth_msg);\n m_buf.unlock();\n con_tracker.notify_one();\n }\n\n void relocalization_callback(const sensor_msgs::PointCloudConstPtr &points_msg)\n {\n m_buf.lock();\n relo_buf.push(points_msg);\n m_buf.unlock();\n }\n\n void visualizeFeatureFilter(const map> &features,\n double feature_time)\n {\n cv::Mat vis_img;\n m_vis.lock();\n while (!vis_img_buf.empty())\n {\n if (vis_img_buf.front().first.stamp.toSec() == feature_time)\n {\n vis_img = vis_img_buf.front().second;\n vis_img_buf.pop();\n break;\n }\n else if (vis_img_buf.front().first.stamp.toSec() < feature_time)\n {\n vis_img_buf.pop();\n }\n else\n {\n m_vis.unlock();\n return;\n }\n }\n m_vis.unlock();\n\n // Show image with tracked points in rviz (by topic pub_match)\n for (auto &feature : features)\n {\n cv::circle(vis_img, cv::Point(feature.second[3], feature.second[4]), 5,\n cv::Scalar(0, 255, 255), 2);\n }\n pubTrackImg(vis_img);\n\n // cv::imshow(\"grids_detector_img\",\n // estimator.featureTracker.grids_detector_img);\n // cv::moveWindow(\"grids_detector_img\", 0, 0);\n // cv::waitKey(1);\n\n // cv::imshow(\"feature_img\", vis_img);\n // cv::moveWindow(\"feature_img\", 0, 0);\n // cv::waitKey(1);\n }\n\n // thread: feature tracker\n [[noreturn]] void process_tracker()\n {\n while (1)\n {\n {\n sensor_msgs::ImageConstPtr color_msg = nullptr;\n sensor_msgs::ImageConstPtr depth_msg = nullptr;\n\n std::unique_lock locker(m_buf);\n while (img_buf.empty() || depth_buf.empty())\n {\n con_tracker.wait(locker);\n }\n\n double time_color = img_buf.front()->header.stamp.toSec();\n double time_depth = depth_buf.front()->header.stamp.toSec();\n\n if (time_color < time_depth - 0.003)\n {\n img_buf.pop();\n ROS_DEBUG(\"throw color\\n\");\n }\n else if (time_color > time_depth + 0.003)\n {\n depth_buf.pop();\n ROS_DEBUG(\"throw depth\\n\");\n }\n else\n {\n color_msg = img_buf.front();\n img_buf.pop();\n depth_msg = depth_buf.front();\n depth_buf.pop();\n }\n locker.unlock();\n\n if (color_msg == nullptr || depth_msg == nullptr)\n {\n ROS_DEBUG(\"time_color = %f, time_depth = %f\\n\", time_color, time_depth);\n continue;\n }\n\n if (first_image_flag)\n {\n first_image_flag = false;\n first_image_time = time_color;\n last_image_time = time_color;\n continue;\n }\n\n // detect unstable camera stream\n if (time_color - last_image_time > 1.0 || time_color < last_image_time)\n {\n ROS_WARN(\"image discontinue! reset the feature tracker!\");\n first_image_flag = true;\n last_image_time = 0;\n pub_count = 1;\n\n ROS_WARN(\"restart the estimator!\");\n m_feature.lock();\n while (!feature_buf.empty())\n feature_buf.pop();\n m_feature.unlock();\n m_backend.lock();\n estimator.clearState();\n estimator.setParameter();\n m_backend.unlock();\n last_imu_t = 0;\n\n continue;\n }\n\n // frequency control\n if (round(1.0 * input_count / (time_color - first_image_time)) > FRONTEND_FREQ)\n {\n ROS_DEBUG(\"Skip this frame.%f\",\n 1.0 * input_count / (time_color - first_image_time));\n continue;\n }\n ++input_count;\n\n // frequency control\n if (round(1.0 * pub_count / (time_color - first_image_time)) <= FREQ)\n {\n PUB_THIS_FRAME = true;\n // reset the frequency control\n if (abs(1.0 * pub_count / (time_color - first_image_time) - FREQ) < 0.01 * FREQ)\n {\n first_image_time = time_color;\n pub_count = 0;\n input_count = 0;\n }\n }\n else\n PUB_THIS_FRAME = false;\n\n TicToc t_r;\n // encodings in ros:\n // http://docs.ros.org/diamondback/api/sensor_msgs/html/image__encodings_8cpp_source.html\n // color has encoding RGB8\n cv_bridge::CvImageConstPtr ptr;\n if (color_msg->encoding == \"8UC1\") // shan:why 8UC1 need this operation? Find\n // answer:https://github.com/ros-perception/vision_opencv/issues/175\n {\n sensor_msgs::Image img;\n img.header = color_msg->header;\n img.height = color_msg->height;\n img.width = color_msg->width;\n img.is_bigendian = color_msg->is_bigendian;\n img.step = color_msg->step;\n img.data = color_msg->data;\n img.encoding = \"mono8\";\n ptr = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::MONO8);\n }\n else\n ptr = cv_bridge::toCvCopy(color_msg, sensor_msgs::image_encodings::MONO8);\n\n if (USE_IMU)\n {\n Matrix3d &&relative_R =\n estimator.predictMotion(last_image_time, time_color + estimator.td);\n estimator.featureTracker.readImage(ptr->image, time_color, relative_R);\n }\n else\n estimator.featureTracker.readImage(ptr->image, time_color);\n\n last_image_time = time_color;\n // always 0\n\n // update all id in ids[]\n // If has ids[i] == -1 (newly added pts by cv::goodFeaturesToTrack),\n // substitute by gloabl id counter (n_id)\n for (unsigned int i = 0;; i++)\n {\n bool completed = false;\n completed |= estimator.featureTracker.updateID(i);\n if (!completed)\n break;\n }\n if (PUB_THIS_FRAME)\n {\n pub_count++;\n\n std_msgs::Header feature_header = color_msg->header;\n map> image;\n auto &un_pts = estimator.featureTracker.cur_un_pts;\n auto &cur_pts = estimator.featureTracker.cur_pts;\n auto &ids = estimator.featureTracker.ids;\n auto &pts_velocity = estimator.featureTracker.pts_velocity;\n for (unsigned int j = 0; j < ids.size(); j++)\n {\n if (estimator.featureTracker.track_cnt[j] > 1)\n {\n int p_id = ids[j];\n geometry_msgs::Point32 p;\n double x = un_pts[j].x;\n double y = un_pts[j].y;\n double z = 1;\n\n int v = p_id * NUM_OF_CAM + 0.5;\n int feature_id = v / NUM_OF_CAM;\n double p_u = cur_pts[j].x;\n double p_v = cur_pts[j].y;\n double velocity_x = pts_velocity[j].x;\n double velocity_y = pts_velocity[j].y;\n\n ROS_ASSERT(z == 1);\n Eigen::Matrix xyz_uv_velocity;\n xyz_uv_velocity << x, y, z, p_u, p_v, velocity_x, velocity_y;\n image[feature_id] = xyz_uv_velocity;\n }\n }\n\n if (!init_pub)\n {\n init_pub = true;\n }\n else\n {\n if (!init_feature)\n {\n // skip the first detected feature, which doesn't contain optical\n // flow speed\n init_feature = true;\n continue;\n }\n if (!image.empty())\n {\n m_feature.lock();\n feature_buf.push(\n make_pair(make_pair(feature_header, depth_msg), std::move(image)));\n m_feature.unlock();\n con_estimator.notify_one();\n }\n else\n {\n first_image_time = time_color;\n pub_count = 0;\n input_count = 0;\n continue;\n }\n }\n\n // Show image with tracked points in rviz (by topic pub_match)\n if (SHOW_TRACK)\n {\n cv::Mat show_img = ptr->image;\n ptr = cv_bridge::cvtColor(ptr, sensor_msgs::image_encodings::BGR8);\n cv::Mat stereo_img = ptr->image;\n cv::Mat tmp_img = stereo_img.rowRange(0, ROW);\n cv::cvtColor(show_img, tmp_img, CV_GRAY2RGB);\n\n for (unsigned int j = 0; j < estimator.featureTracker.cur_pts.size(); j++)\n {\n if (estimator.featureTracker.track_cnt[j] > 1)\n {\n double len = std::min(\n 1.0, 1.0 * estimator.featureTracker.track_cnt[j] / WINDOW_SIZE);\n cv::circle(tmp_img, estimator.featureTracker.cur_pts[j], 5,\n cv::Scalar(255 * (1 - len), 0, 255 * len), -1);\n // draw speed line\n // Vector2d tmp_cur_un_pts\n // (trackerData[i].cur_un_pts[j].x,\n // trackerData[i].cur_un_pts[j].y); Vector2d\n // tmp_pts_velocity\n // (trackerData[i].pts_velocity[j].x,\n // trackerData[i].pts_velocity[j].y); Vector3d\n // tmp_prev_un_pts; tmp_prev_un_pts.head(2) =\n // tmp_cur_un_pts - 0.10 * tmp_pts_velocity;\n // tmp_prev_un_pts.z() = 1;\n // Vector2d tmp_prev_uv;\n // trackerData[i].m_camera->spaceToPlane(tmp_prev_un_pts,\n // tmp_prev_uv); cv::line(tmp_img,\n // trackerData[i].cur_pts[j],\n // cv::Point2f(tmp_prev_uv.x(),\n // tmp_prev_uv.y()), cv::Scalar(255 , 0, 0), 1\n // , 8, 0);\n\n // char name[10];\n // sprintf(name, \"%d\", trackerData[i].ids[j]);\n // cv::putText(tmp_img, name, trackerData[i].cur_pts[j],\n // cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 0, 0));\n }\n }\n if (USE_IMU)\n {\n for (auto &predict_pt : estimator.featureTracker.predict_pts)\n {\n cv::circle(tmp_img, predict_pt, 2, cv::Scalar(0, 255, 0), -1);\n }\n }\n\n m_vis.lock();\n vis_img_buf.push(make_pair(feature_header, tmp_img));\n m_vis.unlock();\n }\n }\n static double whole_process_time = 0;\n static size_t cnt_frame = 0;\n ++cnt_frame;\n double per_process_time = t_r.toc();\n whole_process_time += per_process_time;\n ROS_DEBUG(\"average feature tracking costs: %f\", whole_process_time / cnt_frame);\n // ROS_DEBUG(\"feature tracking costs: %f\", per_process_time);\n }\n std::this_thread::sleep_for(dura);\n }\n }\n\n // thread: visual-inertial odometry\n [[noreturn]] void process()\n {\n while (true)\n {\n std::unique_lock locker(m_feature);\n while (feature_buf.empty())\n {\n con_estimator.wait(locker);\n }\n\n pair,\n map>>\n feature_msg(std::move(feature_buf.front()));\n feature_buf.pop();\n locker.unlock();\n\n TicToc t_backend;\n m_backend.lock();\n // set relocalization frame\n sensor_msgs::PointCloudConstPtr relo_msg = nullptr;\n while (!relo_buf.empty())\n {\n relo_msg = relo_buf.front();\n relo_buf.pop();\n }\n\n if (relo_msg != nullptr)\n {\n vector match_points;\n double frame_stamp = relo_msg->header.stamp.toSec();\n for (auto point : relo_msg->points)\n {\n Vector3d u_v_id;\n u_v_id.x() = point.x;\n u_v_id.y() = point.y;\n u_v_id.z() = point.z;\n match_points.push_back(u_v_id);\n }\n Vector3d relo_t(relo_msg->channels[0].values[0], relo_msg->channels[0].values[1],\n relo_msg->channels[0].values[2]);\n Quaterniond relo_q(relo_msg->channels[0].values[3], relo_msg->channels[0].values[4],\n relo_msg->channels[0].values[5],\n relo_msg->channels[0].values[6]);\n Matrix3d relo_r = relo_q.toRotationMatrix();\n int frame_index;\n frame_index = relo_msg->channels[0].values[7];\n estimator.setReloFrame(frame_stamp, frame_index, match_points, relo_t, relo_r);\n }\n\n // depth has encoding TYPE_16UC1\n cv::Mat depth_img;\n if (feature_msg.first.second == nullptr)\n {\n depth_img = cv::Mat(ROW, COL, CV_16UC1, cv::Scalar(0));\n }\n else\n {\n if (feature_msg.first.second->encoding == \"mono16\" ||\n feature_msg.first.second->encoding == \"16UC1\")\n {\n depth_img = cv_bridge::toCvShare(feature_msg.first.second)->image;\n }\n else if (feature_msg.first.second->encoding == \"32FC1\")\n {\n cv::Mat depth_32fc1 = cv_bridge::toCvShare(feature_msg.first.second)->image;\n depth_32fc1.convertTo(depth_img, CV_16UC1, 1000);\n }\n else\n {\n ROS_ASSERT_MSG(1, \"Unknown depth encoding!\");\n }\n }\n estimator.f_manager.inputDepth(depth_img);\n\n double feature_time = feature_msg.first.first.stamp.toSec();\n\n TicToc t_processImage;\n estimator.processImage(feature_msg.second, feature_msg.first.first);\n\n std_msgs::Header header = feature_msg.first.first;\n header.frame_id = \"map\";\n pubOdometry(estimator, header);\n pubTF(estimator, header);\n pubKeyframe(estimator);\n if (relo_msg != nullptr)\n pubRelocalization(estimator);\n\n m_backend.unlock();\n if (SHOW_TRACK)\n {\n pubKeyPoses(estimator, header);\n pubCameraPose(estimator, header);\n pubPointCloud(estimator, header);\n visualizeFeatureFilter(feature_msg.second, feature_time);\n }\n\n static int cnt_frame = 0;\n static double whole_process_time = 0;\n double per_process_time = t_backend.toc();\n cnt_frame++;\n whole_process_time += per_process_time;\n printStatistics(estimator, per_process_time);\n ROS_DEBUG(\"average backend costs: %f\", whole_process_time / cnt_frame);\n // ROS_DEBUG(\"backend costs: %f\", per_process_time);\n std::this_thread::sleep_for(dura);\n }\n }\n};\n\nPLUGINLIB_EXPORT_CLASS(estimator_nodelet_ns::EstimatorNodelet, nodelet::Nodelet)\n} // namespace estimator_nodelet_ns" }, { "path": "vins_estimator/src/factor/imu_factor.h", "content": "#pragma once\n\n#include \n#include \n#include \n\n#include \"../utility/utility.h\"\n#include \"../utility/parameters.h\"\n#include \"integration_base.h\"\n\n#include \n\nclass IMUFactor : public ceres::SizedCostFunction<15, 7, 9, 7, 9>\n{\npublic:\n IMUFactor() = delete;\n\n IMUFactor(IntegrationBase *_pre_integration) : pre_integration(_pre_integration) {}\n\n virtual bool Evaluate(double const *const *parameters, double *residuals,\n double **jacobians) const\n {\n Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]);\n Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4],\n parameters[0][5]);\n\n Eigen::Vector3d Vi(parameters[1][0], parameters[1][1], parameters[1][2]);\n Eigen::Vector3d Bai(parameters[1][3], parameters[1][4], parameters[1][5]);\n Eigen::Vector3d Bgi(parameters[1][6], parameters[1][7], parameters[1][8]);\n\n Eigen::Vector3d Pj(parameters[2][0], parameters[2][1], parameters[2][2]);\n Eigen::Quaterniond Qj(parameters[2][6], parameters[2][3], parameters[2][4],\n parameters[2][5]);\n\n Eigen::Vector3d Vj(parameters[3][0], parameters[3][1], parameters[3][2]);\n Eigen::Vector3d Baj(parameters[3][3], parameters[3][4], parameters[3][5]);\n Eigen::Vector3d Bgj(parameters[3][6], parameters[3][7], parameters[3][8]);\n\n // Eigen::Matrix Fd;\n // Eigen::Matrix Gd;\n\n // Eigen::Vector3d pPj = Pi + Vi * sum_t - 0.5 * g * sum_t * sum_t + corrected_delta_p;\n // Eigen::Quaterniond pQj = Qi * delta_q;\n // Eigen::Vector3d pVj = Vi - g * sum_t + corrected_delta_v;\n // Eigen::Vector3d pBaj = Bai;\n // Eigen::Vector3d pBgj = Bgi;\n\n // Vi + Qi * delta_v - g * sum_dt = Vj;\n // Qi * delta_q = Qj;\n\n // delta_p = Qi.inverse() * (0.5 * g * sum_dt * sum_dt + Pj - Pi);\n // delta_v = Qi.inverse() * (g * sum_dt + Vj - Vi);\n // delta_q = Qi.inverse() * Qj;\n\n#if 0\n if ((Bai - pre_integration->linearized_ba).norm() > 0.10 ||\n (Bgi - pre_integration->linearized_bg).norm() > 0.01)\n {\n pre_integration->repropagate(Bai, Bgi);\n }\n#endif\n\n Eigen::Map> residual(residuals);\n residual = pre_integration->evaluate(Pi, Qi, Vi, Bai, Bgi, Pj, Qj, Vj, Baj, Bgj);\n\n Eigen::Matrix sqrt_info =\n Eigen::LLT>(pre_integration->covariance.inverse())\n .matrixL()\n .transpose();\n // sqrt_info.setIdentity();\n residual = sqrt_info * residual;\n\n if (jacobians)\n {\n double sum_dt = pre_integration->sum_dt;\n Eigen::Matrix3d dp_dba = pre_integration->jacobian.template block<3, 3>(O_P, O_BA);\n Eigen::Matrix3d dp_dbg = pre_integration->jacobian.template block<3, 3>(O_P, O_BG);\n\n Eigen::Matrix3d dq_dbg = pre_integration->jacobian.template block<3, 3>(O_R, O_BG);\n\n Eigen::Matrix3d dv_dba = pre_integration->jacobian.template block<3, 3>(O_V, O_BA);\n Eigen::Matrix3d dv_dbg = pre_integration->jacobian.template block<3, 3>(O_V, O_BG);\n\n if (pre_integration->jacobian.maxCoeff() > 1e8 ||\n pre_integration->jacobian.minCoeff() < -1e8)\n {\n ROS_WARN(\"numerical unstable in preintegration\");\n // std::cout << pre_integration->jacobian << std::endl;\n /// ROS_BREAK();\n }\n\n if (jacobians[0])\n {\n Eigen::Map> jacobian_pose_i(\n jacobians[0]);\n jacobian_pose_i.setZero();\n\n jacobian_pose_i.block<3, 3>(O_P, O_P) = -Qi.inverse().toRotationMatrix();\n jacobian_pose_i.block<3, 3>(O_P, O_R) = Utility::skewSymmetric(\n Qi.inverse() * (0.5 * G * sum_dt * sum_dt + Pj - Pi - Vi * sum_dt));\n\n#if 0\n jacobian_pose_i.block<3, 3>(O_R, O_R) = -(Qj.inverse() * Qi).toRotationMatrix();\n#else\n Eigen::Quaterniond corrected_delta_q =\n pre_integration->delta_q *\n Utility::deltaQ(dq_dbg * (Bgi - pre_integration->linearized_bg));\n jacobian_pose_i.block<3, 3>(O_R, O_R) =\n -(Utility::Qleft(Qj.inverse() * Qi) * Utility::Qright(corrected_delta_q))\n .bottomRightCorner<3, 3>();\n#endif\n\n jacobian_pose_i.block<3, 3>(O_V, O_R) =\n Utility::skewSymmetric(Qi.inverse() * (G * sum_dt + Vj - Vi));\n\n jacobian_pose_i = sqrt_info * jacobian_pose_i;\n\n if (jacobian_pose_i.maxCoeff() > 1e8 || jacobian_pose_i.minCoeff() < -1e8)\n {\n ROS_WARN(\"numerical unstable in preintegration\");\n // std::cout << sqrt_info << std::endl;\n // ROS_BREAK();\n }\n }\n if (jacobians[1])\n {\n Eigen::Map> jacobian_speedbias_i(\n jacobians[1]);\n jacobian_speedbias_i.setZero();\n jacobian_speedbias_i.block<3, 3>(O_P, O_V - O_V) =\n -Qi.inverse().toRotationMatrix() * sum_dt;\n jacobian_speedbias_i.block<3, 3>(O_P, O_BA - O_V) = -dp_dba;\n jacobian_speedbias_i.block<3, 3>(O_P, O_BG - O_V) = -dp_dbg;\n\n#if 0\n jacobian_speedbias_i.block<3, 3>(O_R, O_BG - O_V) = -dq_dbg;\n#else\n // Eigen::Quaterniond corrected_delta_q = pre_integration->delta_q *\n // Utility::deltaQ(dq_dbg * (Bgi - pre_integration->linearized_bg));\n // jacobian_speedbias_i.block<3, 3>(O_R, O_BG - O_V) = -Utility::Qleft(Qj.inverse()\n // * Qi * corrected_delta_q).bottomRightCorner<3, 3>() * dq_dbg;\n jacobian_speedbias_i.block<3, 3>(O_R, O_BG - O_V) =\n -Utility::Qleft(Qj.inverse() * Qi * pre_integration->delta_q)\n .bottomRightCorner<3, 3>() *\n dq_dbg;\n#endif\n\n jacobian_speedbias_i.block<3, 3>(O_V, O_V - O_V) = -Qi.inverse().toRotationMatrix();\n jacobian_speedbias_i.block<3, 3>(O_V, O_BA - O_V) = -dv_dba;\n jacobian_speedbias_i.block<3, 3>(O_V, O_BG - O_V) = -dv_dbg;\n\n jacobian_speedbias_i.block<3, 3>(O_BA, O_BA - O_V) = -Eigen::Matrix3d::Identity();\n\n jacobian_speedbias_i.block<3, 3>(O_BG, O_BG - O_V) = -Eigen::Matrix3d::Identity();\n\n jacobian_speedbias_i = sqrt_info * jacobian_speedbias_i;\n\n // ROS_ASSERT(fabs(jacobian_speedbias_i.maxCoeff()) < 1e8);\n // ROS_ASSERT(fabs(jacobian_speedbias_i.minCoeff()) < 1e8);\n }\n if (jacobians[2])\n {\n Eigen::Map> jacobian_pose_j(\n jacobians[2]);\n jacobian_pose_j.setZero();\n\n jacobian_pose_j.block<3, 3>(O_P, O_P) = Qi.inverse().toRotationMatrix();\n\n#if 0\n jacobian_pose_j.block<3, 3>(O_R, O_R) = Eigen::Matrix3d::Identity();\n#else\n Eigen::Quaterniond corrected_delta_q =\n pre_integration->delta_q *\n Utility::deltaQ(dq_dbg * (Bgi - pre_integration->linearized_bg));\n jacobian_pose_j.block<3, 3>(O_R, O_R) =\n Utility::Qleft(corrected_delta_q.inverse() * Qi.inverse() * Qj)\n .bottomRightCorner<3, 3>();\n#endif\n\n jacobian_pose_j = sqrt_info * jacobian_pose_j;\n\n // ROS_ASSERT(fabs(jacobian_pose_j.maxCoeff()) < 1e8);\n // ROS_ASSERT(fabs(jacobian_pose_j.minCoeff()) < 1e8);\n }\n if (jacobians[3])\n {\n Eigen::Map> jacobian_speedbias_j(\n jacobians[3]);\n jacobian_speedbias_j.setZero();\n\n jacobian_speedbias_j.block<3, 3>(O_V, O_V - O_V) = Qi.inverse().toRotationMatrix();\n\n jacobian_speedbias_j.block<3, 3>(O_BA, O_BA - O_V) = Eigen::Matrix3d::Identity();\n\n jacobian_speedbias_j.block<3, 3>(O_BG, O_BG - O_V) = Eigen::Matrix3d::Identity();\n\n jacobian_speedbias_j = sqrt_info * jacobian_speedbias_j;\n\n // ROS_ASSERT(fabs(jacobian_speedbias_j.maxCoeff()) < 1e8);\n // ROS_ASSERT(fabs(jacobian_speedbias_j.minCoeff()) < 1e8);\n }\n }\n\n return true;\n }\n\n // bool Evaluate_Direct(double const *const *parameters, Eigen::Matrix\n // &residuals, Eigen::Matrix &jacobians);\n\n // void checkCorrection();\n // void checkTransition();\n // void checkJacobian(double **parameters);\n IntegrationBase *pre_integration;\n};\n" }, { "path": "vins_estimator/src/factor/imu_factor_modified.h", "content": "#pragma once\n\n#include \n#include \n#include \n\n#include \"../utility/utility.h\"\n#include \"../utility/parameters.h\"\n#include \"integration_base.h\"\n\n#include \n\n// class IMUFactor : public ceres::SizedCostFunction<15, 7, 9, 7, 9>\nclass IMUFactor : public ceres::SizedCostFunction<15, 7, 3, 6, 7, 3, 6>\n{\npublic:\n IMUFactor() = delete;\n\n IMUFactor(IntegrationBase *_pre_integration) : pre_integration(_pre_integration) {}\n\n virtual bool Evaluate(double const *const *parameters, double *residuals,\n double **jacobians) const\n {\n Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]);\n Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4],\n parameters[0][5]);\n\n Eigen::Vector3d Vi(parameters[1][0], parameters[1][1], parameters[1][2]);\n\n Eigen::Vector3d Bai(parameters[2][0], parameters[2][1], parameters[2][2]);\n Eigen::Vector3d Bgi(parameters[2][3], parameters[2][4], parameters[2][5]);\n\n Eigen::Vector3d Pj(parameters[3][0], parameters[3][1], parameters[3][2]);\n Eigen::Quaterniond Qj(parameters[3][6], parameters[3][3], parameters[3][4],\n parameters[3][5]);\n\n Eigen::Vector3d Vj(parameters[4][0], parameters[4][1], parameters[4][2]);\n\n Eigen::Vector3d Baj(parameters[5][0], parameters[5][1], parameters[5][2]);\n Eigen::Vector3d Bgj(parameters[5][3], parameters[5][4], parameters[5][5]);\n\n // Eigen::Matrix Fd;\n // Eigen::Matrix Gd;\n\n // Eigen::Vector3d pPj = Pi + Vi * sum_t - 0.5 * g * sum_t * sum_t + corrected_delta_p;\n // Eigen::Quaterniond pQj = Qi * delta_q;\n // Eigen::Vector3d pVj = Vi - g * sum_t + corrected_delta_v;\n // Eigen::Vector3d pBaj = Bai;\n // Eigen::Vector3d pBgj = Bgi;\n\n // Vi + Qi * delta_v - g * sum_dt = Vj;\n // Qi * delta_q = Qj;\n\n // delta_p = Qi.inverse() * (0.5 * g * sum_dt * sum_dt + Pj - Pi);\n // delta_v = Qi.inverse() * (g * sum_dt + Vj - Vi);\n // delta_q = Qi.inverse() * Qj;\n\n#if 0\n if ((Bai - pre_integration->linearized_ba).norm() > 0.10 ||\n (Bgi - pre_integration->linearized_bg).norm() > 0.01)\n {\n pre_integration->repropagate(Bai, Bgi);\n }\n#endif\n\n Eigen::Map> residual(residuals);\n residual = pre_integration->evaluate(Pi, Qi, Vi, Bai, Bgi, Pj, Qj, Vj, Baj, Bgj);\n\n Eigen::Matrix sqrt_info =\n Eigen::LLT>(pre_integration->covariance.inverse())\n .matrixL()\n .transpose();\n // sqrt_info.setIdentity();\n residual = sqrt_info * residual;\n\n if (jacobians)\n {\n double sum_dt = pre_integration->sum_dt;\n Eigen::Matrix3d dp_dba = pre_integration->jacobian.template block<3, 3>(O_P, O_BA);\n Eigen::Matrix3d dp_dbg = pre_integration->jacobian.template block<3, 3>(O_P, O_BG);\n\n Eigen::Matrix3d dq_dbg = pre_integration->jacobian.template block<3, 3>(O_R, O_BG);\n\n Eigen::Matrix3d dv_dba = pre_integration->jacobian.template block<3, 3>(O_V, O_BA);\n Eigen::Matrix3d dv_dbg = pre_integration->jacobian.template block<3, 3>(O_V, O_BG);\n\n if (pre_integration->jacobian.maxCoeff() > 1e8 ||\n pre_integration->jacobian.minCoeff() < -1e8)\n {\n ROS_WARN(\"numerical unstable in preintegration\");\n // std::cout << pre_integration->jacobian << std::endl;\n /// ROS_BREAK();\n }\n\n if (jacobians[0])\n {\n Eigen::Map> jacobian_pose_i(\n jacobians[0]);\n jacobian_pose_i.setZero();\n\n jacobian_pose_i.block<3, 3>(O_P, O_P) = -Qi.inverse().toRotationMatrix();\n jacobian_pose_i.block<3, 3>(O_P, O_R) = Utility::skewSymmetric(\n Qi.inverse() * (0.5 * G * sum_dt * sum_dt + Pj - Pi - Vi * sum_dt));\n\n#if 0\n jacobian_pose_i.block<3, 3>(O_R, O_R) = -(Qj.inverse() * Qi).toRotationMatrix();\n#else\n Eigen::Quaterniond corrected_delta_q =\n pre_integration->delta_q *\n Utility::deltaQ(dq_dbg * (Bgi - pre_integration->linearized_bg));\n jacobian_pose_i.block<3, 3>(O_R, O_R) =\n -(Utility::Qleft(Qj.inverse() * Qi) * Utility::Qright(corrected_delta_q))\n .bottomRightCorner<3, 3>();\n#endif\n\n jacobian_pose_i.block<3, 3>(O_V, O_R) =\n Utility::skewSymmetric(Qi.inverse() * (G * sum_dt + Vj - Vi));\n\n jacobian_pose_i = sqrt_info * jacobian_pose_i;\n\n if (jacobian_pose_i.maxCoeff() > 1e8 || jacobian_pose_i.minCoeff() < -1e8)\n {\n ROS_WARN(\"numerical unstable in preintegration\");\n // std::cout << sqrt_info << std::endl;\n // ROS_BREAK();\n }\n }\n if (jacobians[1])\n {\n Eigen::Map> jacobian_speedbias_i(\n jacobians[1]);\n jacobian_speedbias_i.setZero();\n jacobian_speedbias_i.block<3, 3>(O_P, O_V - O_V) =\n -Qi.inverse().toRotationMatrix() * sum_dt;\n jacobian_speedbias_i.block<3, 3>(O_P, O_BA - O_V) = -dp_dba;\n jacobian_speedbias_i.block<3, 3>(O_P, O_BG - O_V) = -dp_dbg;\n\n#if 0\n jacobian_speedbias_i.block<3, 3>(O_R, O_BG - O_V) = -dq_dbg;\n#else\n // Eigen::Quaterniond corrected_delta_q = pre_integration->delta_q *\n // Utility::deltaQ(dq_dbg * (Bgi - pre_integration->linearized_bg));\n // jacobian_speedbias_i.block<3, 3>(O_R, O_BG - O_V) = -Utility::Qleft(Qj.inverse()\n // * Qi * corrected_delta_q).bottomRightCorner<3, 3>() * dq_dbg;\n jacobian_speedbias_i.block<3, 3>(O_R, O_BG - O_V) =\n -Utility::Qleft(Qj.inverse() * Qi * pre_integration->delta_q)\n .bottomRightCorner<3, 3>() *\n dq_dbg;\n#endif\n\n jacobian_speedbias_i.block<3, 3>(O_V, O_V - O_V) = -Qi.inverse().toRotationMatrix();\n jacobian_speedbias_i.block<3, 3>(O_V, O_BA - O_V) = -dv_dba;\n jacobian_speedbias_i.block<3, 3>(O_V, O_BG - O_V) = -dv_dbg;\n\n jacobian_speedbias_i.block<3, 3>(O_BA, O_BA - O_V) = -Eigen::Matrix3d::Identity();\n\n jacobian_speedbias_i.block<3, 3>(O_BG, O_BG - O_V) = -Eigen::Matrix3d::Identity();\n\n jacobian_speedbias_i = sqrt_info * jacobian_speedbias_i;\n\n // ROS_ASSERT(fabs(jacobian_speedbias_i.maxCoeff()) < 1e8);\n // ROS_ASSERT(fabs(jacobian_speedbias_i.minCoeff()) < 1e8);\n }\n if (jacobians[2])\n {\n Eigen::Map> jacobian_pose_j(\n jacobians[2]);\n jacobian_pose_j.setZero();\n\n jacobian_pose_j.block<3, 3>(O_P, O_P) = Qi.inverse().toRotationMatrix();\n\n#if 0\n jacobian_pose_j.block<3, 3>(O_R, O_R) = Eigen::Matrix3d::Identity();\n#else\n Eigen::Quaterniond corrected_delta_q =\n pre_integration->delta_q *\n Utility::deltaQ(dq_dbg * (Bgi - pre_integration->linearized_bg));\n jacobian_pose_j.block<3, 3>(O_R, O_R) =\n Utility::Qleft(corrected_delta_q.inverse() * Qi.inverse() * Qj)\n .bottomRightCorner<3, 3>();\n#endif\n\n jacobian_pose_j = sqrt_info * jacobian_pose_j;\n\n // ROS_ASSERT(fabs(jacobian_pose_j.maxCoeff()) < 1e8);\n // ROS_ASSERT(fabs(jacobian_pose_j.minCoeff()) < 1e8);\n }\n if (jacobians[3])\n {\n Eigen::Map> jacobian_speedbias_j(\n jacobians[3]);\n jacobian_speedbias_j.setZero();\n\n jacobian_speedbias_j.block<3, 3>(O_V, O_V - O_V) = Qi.inverse().toRotationMatrix();\n\n jacobian_speedbias_j.block<3, 3>(O_BA, O_BA - O_V) = Eigen::Matrix3d::Identity();\n\n jacobian_speedbias_j.block<3, 3>(O_BG, O_BG - O_V) = Eigen::Matrix3d::Identity();\n\n jacobian_speedbias_j = sqrt_info * jacobian_speedbias_j;\n\n // ROS_ASSERT(fabs(jacobian_speedbias_j.maxCoeff()) < 1e8);\n // ROS_ASSERT(fabs(jacobian_speedbias_j.minCoeff()) < 1e8);\n }\n }\n\n return true;\n }\n\n // bool Evaluate_Direct(double const *const *parameters, Eigen::Matrix\n // &residuals, Eigen::Matrix &jacobians);\n\n // void checkCorrection();\n // void checkTransition();\n // void checkJacobian(double **parameters);\n IntegrationBase *pre_integration;\n};\n" }, { "path": "vins_estimator/src/factor/integration_base.h", "content": "#pragma once\n\n#include \"../utility/utility.h\"\n#include \"../utility/parameters.h\"\n\n#include \nusing namespace Eigen;\n\nclass IntegrationBase\n{\npublic:\n IntegrationBase() = delete;\n IntegrationBase(const Eigen::Vector3d &_acc_0, const Eigen::Vector3d &_gyr_0,\n const Eigen::Vector3d &_linearized_ba, const Eigen::Vector3d &_linearized_bg)\n : acc_0{_acc_0}, gyr_0{_gyr_0}, linearized_acc{_acc_0}, linearized_gyr{_gyr_0},\n linearized_ba{_linearized_ba},\n linearized_bg{_linearized_bg}, jacobian{Eigen::Matrix::Identity()},\n covariance{Eigen::Matrix::Zero()}, sum_dt{0.0},\n delta_p{Eigen::Vector3d::Zero()}, delta_q{Eigen::Quaterniond::Identity()},\n delta_v{Eigen::Vector3d::Zero()}\n\n {\n noise = Eigen::Matrix::Zero();\n noise.block<3, 3>(0, 0) = (ACC_N * ACC_N) * Eigen::Matrix3d::Identity();\n noise.block<3, 3>(3, 3) = (GYR_N * GYR_N) * Eigen::Matrix3d::Identity();\n noise.block<3, 3>(6, 6) = (ACC_N * ACC_N) * Eigen::Matrix3d::Identity();\n noise.block<3, 3>(9, 9) = (GYR_N * GYR_N) * Eigen::Matrix3d::Identity();\n noise.block<3, 3>(12, 12) = (ACC_W * ACC_W) * Eigen::Matrix3d::Identity();\n noise.block<3, 3>(15, 15) = (GYR_W * GYR_W) * Eigen::Matrix3d::Identity();\n }\n\n void push_back(double dt, const Eigen::Vector3d &acc, const Eigen::Vector3d &gyr)\n {\n dt_buf.push_back(dt);\n acc_buf.push_back(acc);\n gyr_buf.push_back(gyr);\n propagate(dt, acc, gyr);\n }\n\n void repropagate(const Eigen::Vector3d &_linearized_ba, const Eigen::Vector3d &_linearized_bg)\n {\n sum_dt = 0.0;\n acc_0 = linearized_acc;\n gyr_0 = linearized_gyr;\n delta_p.setZero();\n delta_q.setIdentity();\n delta_v.setZero();\n linearized_ba = _linearized_ba;\n linearized_bg = _linearized_bg;\n jacobian.setIdentity();\n covariance.setZero();\n for (int i = 0; i < static_cast(dt_buf.size()); i++)\n propagate(dt_buf[i], acc_buf[i], gyr_buf[i]);\n }\n\n void midPointIntegration(double _dt, const Eigen::Vector3d &_acc_0,\n const Eigen::Vector3d &_gyr_0, const Eigen::Vector3d &_acc_1,\n const Eigen::Vector3d &_gyr_1, const Eigen::Vector3d &delta_p,\n const Eigen::Quaterniond &delta_q, const Eigen::Vector3d &delta_v,\n const Eigen::Vector3d &linearized_ba,\n const Eigen::Vector3d &linearized_bg, Eigen::Vector3d &result_delta_p,\n Eigen::Quaterniond &result_delta_q, Eigen::Vector3d &result_delta_v,\n Eigen::Vector3d &result_linearized_ba,\n Eigen::Vector3d &result_linearized_bg, bool update_jacobian)\n {\n // ROS_INFO(\"midpoint integration\");\n Vector3d un_acc_0 = delta_q * (_acc_0 - linearized_ba);\n Vector3d un_gyr = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;\n result_delta_q =\n delta_q * Quaterniond(1, un_gyr(0) * _dt / 2, un_gyr(1) * _dt / 2, un_gyr(2) * _dt / 2);\n Vector3d un_acc_1 = result_delta_q * (_acc_1 - linearized_ba);\n Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);\n result_delta_p = delta_p + delta_v * _dt + 0.5 * un_acc * _dt * _dt;\n result_delta_v = delta_v + un_acc * _dt;\n result_linearized_ba = linearized_ba;\n result_linearized_bg = linearized_bg;\n\n if (update_jacobian)\n {\n Vector3d w_x = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;\n Vector3d a_0_x = _acc_0 - linearized_ba;\n Vector3d a_1_x = _acc_1 - linearized_ba;\n Matrix3d R_w_x, R_a_0_x, R_a_1_x;\n\n R_w_x << 0, -w_x(2), w_x(1), w_x(2), 0, -w_x(0), -w_x(1), w_x(0), 0;\n 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;\n 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;\n\n MatrixXd F = MatrixXd::Zero(15, 15);\n F.block<3, 3>(0, 0) = Matrix3d::Identity();\n F.block<3, 3>(0, 3) = -0.25 * delta_q.toRotationMatrix() * R_a_0_x * _dt * _dt +\n -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x *\n (Matrix3d::Identity() - R_w_x * _dt) * _dt * _dt;\n F.block<3, 3>(0, 6) = MatrixXd::Identity(3, 3) * _dt;\n F.block<3, 3>(0, 9) = -0.25 *\n (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) *\n _dt * _dt;\n F.block<3, 3>(0, 12) =\n -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * _dt * -_dt;\n F.block<3, 3>(3, 3) = Matrix3d::Identity() - R_w_x * _dt;\n F.block<3, 3>(3, 12) = -1.0 * MatrixXd::Identity(3, 3) * _dt;\n F.block<3, 3>(6, 3) = -0.5 * delta_q.toRotationMatrix() * R_a_0_x * _dt +\n -0.5 * result_delta_q.toRotationMatrix() * R_a_1_x *\n (Matrix3d::Identity() - R_w_x * _dt) * _dt;\n F.block<3, 3>(6, 6) = Matrix3d::Identity();\n F.block<3, 3>(6, 9) =\n -0.5 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt;\n F.block<3, 3>(6, 12) = -0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * -_dt;\n F.block<3, 3>(9, 9) = Matrix3d::Identity();\n F.block<3, 3>(12, 12) = Matrix3d::Identity();\n // cout<<\"A\"<(0, 0) = 0.25 * delta_q.toRotationMatrix() * _dt * _dt;\n V.block<3, 3>(0, 3) =\n 0.25 * -result_delta_q.toRotationMatrix() * R_a_1_x * _dt * _dt * 0.5 * _dt;\n V.block<3, 3>(0, 6) = 0.25 * result_delta_q.toRotationMatrix() * _dt * _dt;\n V.block<3, 3>(0, 9) = V.block<3, 3>(0, 3);\n V.block<3, 3>(3, 3) = 0.5 * MatrixXd::Identity(3, 3) * _dt;\n V.block<3, 3>(3, 9) = 0.5 * MatrixXd::Identity(3, 3) * _dt;\n V.block<3, 3>(6, 0) = 0.5 * delta_q.toRotationMatrix() * _dt;\n V.block<3, 3>(6, 3) =\n 0.5 * -result_delta_q.toRotationMatrix() * R_a_1_x * _dt * 0.5 * _dt;\n V.block<3, 3>(6, 6) = 0.5 * result_delta_q.toRotationMatrix() * _dt;\n V.block<3, 3>(6, 9) = V.block<3, 3>(6, 3);\n V.block<3, 3>(9, 12) = MatrixXd::Identity(3, 3) * _dt;\n V.block<3, 3>(12, 15) = MatrixXd::Identity(3, 3) * _dt;\n\n // step_jacobian = F;\n // step_V = V;\n jacobian = F * jacobian;\n covariance = F * covariance * F.transpose() + V * noise * V.transpose();\n }\n }\n\n void propagate(double _dt, const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1)\n {\n dt = _dt;\n acc_1 = _acc_1;\n gyr_1 = _gyr_1;\n Vector3d result_delta_p;\n Quaterniond result_delta_q;\n Vector3d result_delta_v;\n Vector3d result_linearized_ba;\n Vector3d result_linearized_bg;\n\n midPointIntegration(_dt, acc_0, gyr_0, _acc_1, _gyr_1, delta_p, delta_q, delta_v,\n linearized_ba, linearized_bg, result_delta_p, result_delta_q,\n result_delta_v, result_linearized_ba, result_linearized_bg, 1);\n\n // checkJacobian(_dt, acc_0, gyr_0, acc_1, gyr_1, delta_p, delta_q, delta_v,\n // linearized_ba, linearized_bg);\n delta_p = result_delta_p;\n delta_q = result_delta_q;\n delta_v = result_delta_v;\n linearized_ba = result_linearized_ba;\n linearized_bg = result_linearized_bg;\n delta_q.normalize();\n sum_dt += dt;\n acc_0 = acc_1;\n gyr_0 = gyr_1;\n }\n\n Eigen::Matrix evaluate(const Eigen::Vector3d &Pi, const Eigen::Quaterniond &Qi,\n const Eigen::Vector3d &Vi, const Eigen::Vector3d &Bai,\n const Eigen::Vector3d &Bgi, const Eigen::Vector3d &Pj,\n const Eigen::Quaterniond &Qj, const Eigen::Vector3d &Vj,\n const Eigen::Vector3d &Baj, const Eigen::Vector3d &Bgj)\n {\n Eigen::Matrix residuals;\n\n Eigen::Matrix3d dp_dba = jacobian.block<3, 3>(O_P, O_BA);\n Eigen::Matrix3d dp_dbg = jacobian.block<3, 3>(O_P, O_BG);\n\n Eigen::Matrix3d dq_dbg = jacobian.block<3, 3>(O_R, O_BG);\n\n Eigen::Matrix3d dv_dba = jacobian.block<3, 3>(O_V, O_BA);\n Eigen::Matrix3d dv_dbg = jacobian.block<3, 3>(O_V, O_BG);\n\n Eigen::Vector3d dba = Bai - linearized_ba;\n Eigen::Vector3d dbg = Bgi - linearized_bg;\n\n Eigen::Quaterniond corrected_delta_q = delta_q * Utility::deltaQ(dq_dbg * dbg);\n Eigen::Vector3d corrected_delta_v = delta_v + dv_dba * dba + dv_dbg * dbg;\n Eigen::Vector3d corrected_delta_p = delta_p + dp_dba * dba + dp_dbg * dbg;\n\n residuals.block<3, 1>(O_P, 0) =\n Qi.inverse() * (0.5 * G * sum_dt * sum_dt + Pj - Pi - Vi * sum_dt) - corrected_delta_p;\n residuals.block<3, 1>(O_R, 0) =\n 2 * (corrected_delta_q.inverse() * (Qi.inverse() * Qj)).vec();\n residuals.block<3, 1>(O_V, 0) = Qi.inverse() * (G * sum_dt + Vj - Vi) - corrected_delta_v;\n residuals.block<3, 1>(O_BA, 0) = Baj - Bai;\n residuals.block<3, 1>(O_BG, 0) = Bgj - Bgi;\n return residuals;\n }\n\n double dt;\n Eigen::Vector3d acc_0, gyr_0;\n Eigen::Vector3d acc_1, gyr_1;\n\n const Eigen::Vector3d linearized_acc, linearized_gyr;\n Eigen::Vector3d linearized_ba, linearized_bg;\n\n Eigen::Matrix jacobian, covariance;\n Eigen::Matrix step_jacobian;\n Eigen::Matrix step_V;\n Eigen::Matrix noise;\n\n double sum_dt;\n Eigen::Vector3d delta_p;\n Eigen::Quaterniond delta_q;\n Eigen::Vector3d delta_v;\n\n std::vector dt_buf;\n std::vector acc_buf;\n std::vector gyr_buf;\n};" }, { "path": "vins_estimator/src/factor/marginalization_factor.cpp", "content": "#include \"marginalization_factor.h\"\n\nvoid ResidualBlockInfo::Evaluate()\n{\n residuals.resize(cost_function->num_residuals());\n\n std::vector block_sizes = cost_function->parameter_block_sizes();\n raw_jacobians = new double *[block_sizes.size()];\n jacobians.resize(block_sizes.size());\n\n for (int i = 0; i < static_cast(block_sizes.size()); i++)\n {\n jacobians[i].resize(cost_function->num_residuals(), block_sizes[i]);\n raw_jacobians[i] = jacobians[i].data();\n // dim += block_sizes[i] == 7 ? 6 : block_sizes[i];\n }\n cost_function->Evaluate(parameter_blocks.data(), residuals.data(), raw_jacobians);\n\n // std::vector tmp_idx(block_sizes.size());\n // Eigen::MatrixXd tmp(dim, dim);\n // for (int i = 0; i < static_cast(parameter_blocks.size()); i++)\n //{\n // int size_i = localSize(block_sizes[i]);\n // Eigen::MatrixXd jacobian_i = jacobians[i].leftCols(size_i);\n // for (int j = 0, sub_idx = 0; j < static_cast(parameter_blocks.size()); sub_idx +=\n // block_sizes[j] == 7 ? 6 : block_sizes[j], j++)\n // {\n // int size_j = localSize(block_sizes[j]);\n // Eigen::MatrixXd jacobian_j = jacobians[j].leftCols(size_j);\n // tmp_idx[j] = sub_idx;\n // tmp.block(tmp_idx[i], tmp_idx[j], size_i, size_j) = jacobian_i.transpose() *\n // jacobian_j;\n // }\n // }\n // Eigen::SelfAdjointEigenSolver saes(tmp);\n // std::cout << saes.eigenvalues() << std::endl;\n // ROS_ASSERT(saes.eigenvalues().minCoeff() >= -1e-6);\n\n if (loss_function)\n {\n double residual_scaling_, alpha_sq_norm_;\n\n double sq_norm, rho[3];\n\n sq_norm = residuals.squaredNorm();\n loss_function->Evaluate(sq_norm, rho);\n // printf(\"sq_norm: %f, rho[0]: %f, rho[1]: %f, rho[2]: %f\\n\", sq_norm, rho[0], rho[1],\n // rho[2]);\n\n double sqrt_rho1_ = sqrt(rho[1]);\n\n if ((sq_norm == 0.0) || (rho[2] <= 0.0))\n {\n residual_scaling_ = sqrt_rho1_;\n alpha_sq_norm_ = 0.0;\n }\n else\n {\n const double D = 1.0 + 2.0 * sq_norm * rho[2] / rho[1];\n const double alpha = 1.0 - sqrt(D);\n residual_scaling_ = sqrt_rho1_ / (1 - alpha);\n alpha_sq_norm_ = alpha / sq_norm;\n }\n\n for (int i = 0; i < static_cast(parameter_blocks.size()); i++)\n {\n jacobians[i] = sqrt_rho1_ * (jacobians[i] - alpha_sq_norm_ * residuals *\n (residuals.transpose() * jacobians[i]));\n }\n\n residuals *= residual_scaling_;\n }\n}\n\nMarginalizationInfo::~MarginalizationInfo()\n{\n // ROS_WARN(\"release marginlizationinfo\");\n\n for (auto it = parameter_block_data.begin(); it != parameter_block_data.end(); ++it)\n delete[] it->second;\n\n for (int i = 0; i < (int)factors.size(); i++)\n {\n delete[] factors[i]->raw_jacobians;\n\n delete factors[i]->cost_function;\n\n delete factors[i];\n }\n}\n\nvoid MarginalizationInfo::addResidualBlockInfo(ResidualBlockInfo *residual_block_info)\n{\n factors.emplace_back(residual_block_info);\n\n std::vector ¶meter_blocks = residual_block_info->parameter_blocks;\n std::vector parameter_block_sizes =\n residual_block_info->cost_function->parameter_block_sizes();\n\n for (int i = 0; i < static_cast(residual_block_info->parameter_blocks.size()); i++)\n {\n double *addr = parameter_blocks[i];\n int size = parameter_block_sizes[i];\n parameter_block_size[reinterpret_cast(addr)] = size;\n }\n\n for (int i = 0; i < static_cast(residual_block_info->drop_set.size()); i++)\n {\n double *addr = parameter_blocks[residual_block_info->drop_set[i]];\n parameter_block_idx[reinterpret_cast(addr)] = 0;\n }\n}\n\nvoid MarginalizationInfo::preMarginalize() //构建parameter_block_data <变量的内存地址， 变量数据>\n{\n for (auto it : factors)\n {\n it->Evaluate(); //求解残差因子对应的残差与雅克比\n\n std::vector block_sizes =\n it->cost_function->parameter_block_sizes(); //代价函数对应的优化变量参数块的数目\n for (int i = 0; i < static_cast(block_sizes.size()); i++)\n {\n long addr = reinterpret_cast(it->parameter_blocks[i]);\n int size = block_sizes[i];\n if (parameter_block_data.find(addr) == parameter_block_data.end())\n {\n double *data = new double[size];\n memcpy(data, it->parameter_blocks[i], sizeof(double) * size);\n parameter_block_data[addr] = data;\n }\n }\n }\n}\n\nint MarginalizationInfo::localSize(int size) const\n{\n return size == 7 ? 6 : size;\n}\n\nint MarginalizationInfo::globalSize(int size) const\n{\n return size == 6 ? 7 : size;\n}\n\nvoid *ThreadsConstructA(void *threadsstruct)\n{\n ThreadsStruct *p = ((ThreadsStruct *)threadsstruct);\n for (auto it : p->sub_factors)\n {\n for (int i = 0; i < static_cast(it->parameter_blocks.size()); i++)\n {\n int idx_i = p->parameter_block_idx[reinterpret_cast(it->parameter_blocks[i])];\n int size_i = p->parameter_block_size[reinterpret_cast(it->parameter_blocks[i])];\n if (size_i == 7)\n size_i = 6;\n Eigen::MatrixXd jacobian_i = it->jacobians[i].leftCols(size_i);\n for (int j = i; j < static_cast(it->parameter_blocks.size()); j++)\n {\n int idx_j = p->parameter_block_idx[reinterpret_cast(it->parameter_blocks[j])];\n int size_j =\n p->parameter_block_size[reinterpret_cast(it->parameter_blocks[j])];\n if (size_j == 7)\n size_j = 6;\n Eigen::MatrixXd jacobian_j = it->jacobians[j].leftCols(size_j);\n if (i == j)\n p->A.block(idx_i, idx_j, size_i, size_j) += jacobian_i.transpose() * jacobian_j;\n else\n {\n p->A.block(idx_i, idx_j, size_i, size_j) += jacobian_i.transpose() * jacobian_j;\n p->A.block(idx_j, idx_i, size_j, size_i) =\n p->A.block(idx_i, idx_j, size_i, size_j).transpose();\n }\n }\n p->b.segment(idx_i, size_i) += jacobian_i.transpose() * it->residuals;\n }\n }\n return threadsstruct;\n}\n\nvoid MarginalizationInfo::marginalize()\n{\n int pos = 0;\n for (auto &it : parameter_block_idx)\n {\n it.second = pos;\n pos += localSize(parameter_block_size[it.first]);\n }\n\n m = pos;\n\n for (const auto &it : parameter_block_size)\n {\n if (parameter_block_idx.find(it.first) == parameter_block_idx.end())\n {\n parameter_block_idx[it.first] = pos;\n pos += localSize(it.second);\n }\n }\n\n n = pos - m;\n\n // ROS_DEBUG(\"marginalization, pos: %d, m: %d, n: %d, size: %d\", pos, m, n,\n // (int)parameter_block_idx.size());\n\n TicToc t_summing;\n Eigen::MatrixXd A(pos, pos);\n Eigen::VectorXd b(pos);\n A.setZero();\n b.setZero();\n /*\n for (auto it : factors)\n {\n for (int i = 0; i < static_cast(it->parameter_blocks.size()); i++)\n {\n int idx_i = parameter_block_idx[reinterpret_cast(it->parameter_blocks[i])];\n int size_i =\n localSize(parameter_block_size[reinterpret_cast(it->parameter_blocks[i])]);\n Eigen::MatrixXd jacobian_i = it->jacobians[i].leftCols(size_i);\n for (int j = i; j < static_cast(it->parameter_blocks.size()); j++)\n {\n int idx_j = parameter_block_idx[reinterpret_cast(it->parameter_blocks[j])];\n int size_j =\n localSize(parameter_block_size[reinterpret_cast(it->parameter_blocks[j])]);\n Eigen::MatrixXd jacobian_j = it->jacobians[j].leftCols(size_j);\n if (i == j)\n A.block(idx_i, idx_j, size_i, size_j) += jacobian_i.transpose() * jacobian_j;\n else\n {\n A.block(idx_i, idx_j, size_i, size_j) += jacobian_i.transpose() * jacobian_j;\n A.block(idx_j, idx_i, size_j, size_i) = A.block(idx_i, idx_j, size_i,\n size_j).transpose();\n }\n }\n b.segment(idx_i, size_i) += jacobian_i.transpose() * it->residuals;\n }\n }\n ROS_INFO(\"summing up costs %f ms\", t_summing.toc());\n */\n // multi thread\n\n TicToc t_thread_summing;\n pthread_t tids[NUM_MAR_THREADS];\n ThreadsStruct threadsstruct[NUM_MAR_THREADS];\n int i = 0;\n for (auto it : factors)\n {\n threadsstruct[i].sub_factors.push_back(it);\n i++;\n i = i % NUM_MAR_THREADS;\n }\n for (int i = 0; i < NUM_MAR_THREADS; i++)\n {\n TicToc zero_matrix;\n threadsstruct[i].A = Eigen::MatrixXd::Zero(pos, pos);\n threadsstruct[i].b = Eigen::VectorXd::Zero(pos);\n threadsstruct[i].parameter_block_size = parameter_block_size;\n threadsstruct[i].parameter_block_idx = parameter_block_idx;\n int ret = pthread_create(&tids[i], NULL, ThreadsConstructA, (void *)&(threadsstruct[i]));\n if (ret != 0)\n {\n ROS_WARN(\"pthread_create error\");\n ROS_BREAK();\n }\n }\n for (int i = NUM_MAR_THREADS - 1; i >= 0; i--)\n {\n pthread_join(tids[i], NULL);\n A += threadsstruct[i].A;\n b += threadsstruct[i].b;\n }\n // ROS_DEBUG(\"thread summing up costs %f ms\", t_thread_summing.toc());\n // ROS_INFO(\"A diff %f , b diff %f \", (A - tmp_A).sum(), (b - tmp_b).sum());\n\n // TODO\n Eigen::MatrixXd Amm = 0.5 * (A.block(0, 0, m, m) + A.block(0, 0, m, m).transpose());\n Eigen::SelfAdjointEigenSolver saes(Amm);\n\n // ROS_ASSERT_MSG(saes.eigenvalues().minCoeff() >= -1e-4, \"min eigenvalue %f\",\n // saes.eigenvalues().minCoeff());\n\n Eigen::MatrixXd Amm_inv =\n saes.eigenvectors() *\n Eigen::VectorXd(\n (saes.eigenvalues().array() > eps).select(saes.eigenvalues().array().inverse(), 0))\n .asDiagonal() *\n saes.eigenvectors().transpose();\n // printf(\"error1: %f\\n\", (Amm * Amm_inv - Eigen::MatrixXd::Identity(m, m)).sum());\n\n Eigen::VectorXd bmm = b.segment(0, m);\n Eigen::MatrixXd Amr = A.block(0, m, m, n);\n Eigen::MatrixXd Arm = A.block(m, 0, n, m);\n Eigen::MatrixXd Arr = A.block(m, m, n, n);\n Eigen::VectorXd brr = b.segment(m, n);\n A = Arr - Arm * Amm_inv * Amr;\n b = brr - Arm * Amm_inv * bmm;\n\n Eigen::SelfAdjointEigenSolver saes2(A);\n Eigen::VectorXd S =\n Eigen::VectorXd((saes2.eigenvalues().array() > eps).select(saes2.eigenvalues().array(), 0));\n Eigen::VectorXd S_inv = Eigen::VectorXd(\n (saes2.eigenvalues().array() > eps).select(saes2.eigenvalues().array().inverse(), 0));\n\n Eigen::VectorXd S_sqrt = S.cwiseSqrt();\n Eigen::VectorXd S_inv_sqrt = S_inv.cwiseSqrt();\n\n linearized_jacobians = S_sqrt.asDiagonal() * saes2.eigenvectors().transpose();\n linearized_residuals = S_inv_sqrt.asDiagonal() * saes2.eigenvectors().transpose() * b;\n // std::cout << A << std::endl\n // << std::endl;\n // std::cout << linearized_jacobians << std::endl;\n // printf(\"error2: %f %f\\n\", (linearized_jacobians.transpose() * linearized_jacobians -\n // A).sum(),\n // (linearized_jacobians.transpose() * linearized_residuals - b).sum());\n}\n\nstd::vector\nMarginalizationInfo::getParameterBlocks(std::unordered_map &addr_shift)\n{\n std::vector keep_block_addr;\n keep_block_size.clear();\n keep_block_idx.clear();\n keep_block_data.clear();\n\n for (const auto &it : parameter_block_idx)\n {\n if (it.second >= m)\n {\n keep_block_size.push_back(parameter_block_size[it.first]);\n keep_block_idx.push_back(parameter_block_idx[it.first]);\n keep_block_data.push_back(parameter_block_data[it.first]);\n keep_block_addr.push_back(addr_shift[it.first]);\n }\n }\n sum_block_size = std::accumulate(std::begin(keep_block_size), std::end(keep_block_size), 0);\n\n return keep_block_addr;\n}\n\nMarginalizationFactor::MarginalizationFactor(MarginalizationInfo *_marginalization_info)\n : marginalization_info(_marginalization_info)\n{\n int cnt = 0;\n for (auto it : marginalization_info->keep_block_size)\n {\n mutable_parameter_block_sizes()->push_back(it);\n cnt += it;\n }\n // printf(\"residual size: %d, %d\\n\", cnt, n);\n set_num_residuals(marginalization_info->n);\n};\n\nbool MarginalizationFactor::Evaluate(double const *const *parameters, double *residuals,\n double **jacobians) const\n{\n // printf(\"internal addr,%d, %d\\n\", (int)parameter_block_sizes().size(), num_residuals());\n // for (int i = 0; i < static_cast(keep_block_size.size()); i++)\n //{\n // //printf(\"unsigned %x\\n\", reinterpret_cast(parameters[i]));\n // //printf(\"signed %x\\n\", reinterpret_cast(parameters[i]));\n // printf(\"jacobian %x\\n\", reinterpret_cast(jacobians));\n // printf(\"residual %x\\n\", reinterpret_cast(residuals));\n // }\n int n = marginalization_info->n;\n int m = marginalization_info->m;\n Eigen::VectorXd dx(n);\n for (int i = 0; i < static_cast(marginalization_info->keep_block_size.size()); i++)\n {\n int size = marginalization_info->keep_block_size[i];\n int idx = marginalization_info->keep_block_idx[i] - m;\n Eigen::VectorXd x = Eigen::Map(parameters[i], size);\n Eigen::VectorXd x0 =\n Eigen::Map(marginalization_info->keep_block_data[i], size);\n if (size != 7)\n dx.segment(idx, size) = x - x0;\n else\n {\n dx.segment<3>(idx + 0) = x.head<3>() - x0.head<3>();\n dx.segment<3>(idx + 3) =\n 2.0 * Utility::positify(Eigen::Quaterniond(x0(6), x0(3), x0(4), x0(5)).inverse() *\n Eigen::Quaterniond(x(6), x(3), x(4), x(5)))\n .vec();\n if (!((Eigen::Quaterniond(x0(6), x0(3), x0(4), x0(5)).inverse() *\n Eigen::Quaterniond(x(6), x(3), x(4), x(5)))\n .w() >= 0))\n {\n dx.segment<3>(idx + 3) =\n 2.0 *\n -Utility::positify(Eigen::Quaterniond(x0(6), x0(3), x0(4), x0(5)).inverse() *\n Eigen::Quaterniond(x(6), x(3), x(4), x(5)))\n .vec();\n }\n }\n }\n Eigen::Map(residuals, n) = marginalization_info->linearized_residuals +\n marginalization_info->linearized_jacobians * dx;\n if (jacobians)\n {\n for (int i = 0; i < static_cast(marginalization_info->keep_block_size.size()); i++)\n {\n if (jacobians[i])\n {\n int size = marginalization_info->keep_block_size[i],\n local_size = marginalization_info->localSize(size);\n int idx = marginalization_info->keep_block_idx[i] - m;\n Eigen::Map>\n jacobian(jacobians[i], n, size);\n jacobian.setZero();\n jacobian.leftCols(local_size) =\n marginalization_info->linearized_jacobians.middleCols(idx, local_size);\n }\n }\n }\n return true;\n}\n" }, { "path": "vins_estimator/src/factor/marginalization_factor.h", "content": "#pragma once\n\n#include \n#include \n#include \n#include \n#include \n#include \n\n#include \"../utility/parameters.h\"\n#include \"../utility/tic_toc.h\"\n#include \"../utility/utility.h\"\n\nconst int NUM_MAR_THREADS = 4;\n\nstruct ResidualBlockInfo\n{\n ResidualBlockInfo(ceres::CostFunction *_cost_function, ceres::LossFunction *_loss_function,\n std::vector _parameter_blocks, std::vector _drop_set)\n : cost_function(_cost_function), loss_function(_loss_function),\n parameter_blocks(_parameter_blocks), drop_set(_drop_set)\n {\n }\n\n void Evaluate();\n\n ceres::CostFunction *cost_function;\n ceres::LossFunction *loss_function;\n std::vector parameter_blocks;\n std::vector drop_set;\n\n double **raw_jacobians;\n std::vector> jacobians;\n Eigen::VectorXd residuals;\n\n int localSize(int size)\n {\n return size == 7 ? 6 : size;\n }\n};\n\nstruct ThreadsStruct\n{\n std::vector sub_factors;\n Eigen::MatrixXd A;\n Eigen::VectorXd b;\n std::unordered_map parameter_block_size; // global size\n std::unordered_map parameter_block_idx; // local size\n};\n\nclass MarginalizationInfo\n{\npublic:\n ~MarginalizationInfo();\n int localSize(int size) const;\n int globalSize(int size) const;\n void addResidualBlockInfo(ResidualBlockInfo *residual_block_info);\n void preMarginalize();\n void marginalize();\n std::vector getParameterBlocks(std::unordered_map &addr_shift);\n\n std::vector factors;\n int m, n;\n std::unordered_map parameter_block_size; // global size\n int sum_block_size;\n std::unordered_map parameter_block_idx; // local size\n std::unordered_map parameter_block_data;\n\n std::vector keep_block_size; // global size\n std::vector keep_block_idx; // local size\n std::vector keep_block_data;\n\n Eigen::MatrixXd linearized_jacobians;\n Eigen::VectorXd linearized_residuals;\n const double eps = 1e-8;\n};\n\nclass MarginalizationFactor : public ceres::CostFunction\n{\npublic:\n MarginalizationFactor(MarginalizationInfo *_marginalization_info);\n virtual bool Evaluate(double const *const *parameters, double *residuals,\n double **jacobians) const;\n\n MarginalizationInfo *marginalization_info;\n};\n" }, { "path": "vins_estimator/src/factor/pose_local_parameterization.cpp", "content": "#include \"pose_local_parameterization.h\"\n\nbool PoseLocalParameterization::Plus(const double *x, const double *delta,\n double *x_plus_delta) const\n{\n Eigen::Map _p(x);\n Eigen::Map _q(x + 3);\n\n Eigen::Map dp(delta);\n\n Eigen::Quaterniond dq = Utility::deltaQ(Eigen::Map(delta + 3));\n\n Eigen::Map p(x_plus_delta);\n Eigen::Map q(x_plus_delta + 3);\n\n p = _p + dp;\n q = (_q * dq).normalized();\n\n return true;\n}\nbool PoseLocalParameterization::ComputeJacobian(const double *x, double *jacobian) const\n{\n Eigen::Map> j(jacobian);\n j.topRows<6>().setIdentity();\n j.bottomRows<1>().setZero();\n\n return true;\n}\n" }, { "path": "vins_estimator/src/factor/pose_local_parameterization.h", "content": "#pragma once\n\n#include \n#include \n#include \"../utility/utility.h\"\n\nclass PoseLocalParameterization : public ceres::LocalParameterization\n{\n virtual bool Plus(const double *x, const double *delta, double *x_plus_delta) const;\n virtual bool ComputeJacobian(const double *x, double *jacobian) const;\n virtual int GlobalSize() const\n {\n return 7;\n };\n virtual int LocalSize() const\n {\n return 6;\n };\n};\n" }, { "path": "vins_estimator/src/factor/projection_factor.cpp", "content": "#include \"projection_factor.h\"\n\nEigen::Matrix2d ProjectionFactor::sqrt_info;\ndouble ProjectionFactor::sum_t;\n\nProjectionFactor::ProjectionFactor(const Eigen::Vector3d &_pts_i, const Eigen::Vector3d &_pts_j)\n : pts_i(_pts_i), pts_j(_pts_j)\n{\n#ifdef UNIT_SPHERE_ERROR\n Eigen::Vector3d b1, b2;\n Eigen::Vector3d a = pts_j.normalized();\n Eigen::Vector3d tmp(0, 0, 1);\n if (a == tmp)\n tmp << 1, 0, 0;\n b1 = (tmp - a * (a.transpose() * tmp)).normalized();\n b2 = a.cross(b1);\n tangent_base.block<1, 3>(0, 0) = b1.transpose();\n tangent_base.block<1, 3>(1, 0) = b2.transpose();\n#endif\n};\n\nbool ProjectionFactor::Evaluate(double const *const *parameters, double *residuals,\n double **jacobians) const\n{\n TicToc tic_toc;\n Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]);\n Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]);\n\n Eigen::Vector3d Pj(parameters[1][0], parameters[1][1], parameters[1][2]);\n Eigen::Quaterniond Qj(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]);\n\n Eigen::Vector3d tic(parameters[2][0], parameters[2][1], parameters[2][2]);\n Eigen::Quaterniond qic(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]);\n\n double inv_dep_i = parameters[3][0];\n\n Eigen::Vector3d pts_camera_i = pts_i / inv_dep_i;\n Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic;\n Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi;\n Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj);\n Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic);\n Eigen::Map residual(residuals);\n\n#ifdef UNIT_SPHERE_ERROR\n residual = tangent_base * (pts_camera_j.normalized() - pts_j.normalized());\n#else\n double dep_j = pts_camera_j.z();\n residual = (pts_camera_j / dep_j).head<2>() - pts_j.head<2>();\n#endif\n\n residual = sqrt_info * residual;\n\n if (jacobians)\n {\n Eigen::Matrix3d Ri = Qi.toRotationMatrix();\n Eigen::Matrix3d Rj = Qj.toRotationMatrix();\n Eigen::Matrix3d ric = qic.toRotationMatrix();\n Eigen::Matrix reduce(2, 3);\n#ifdef UNIT_SPHERE_ERROR\n double norm = pts_camera_j.norm();\n Eigen::Matrix3d norm_jaco;\n double x1, x2, x3;\n x1 = pts_camera_j(0);\n x2 = pts_camera_j(1);\n x3 = pts_camera_j(2);\n norm_jaco << 1.0 / norm - x1 * x1 / pow(norm, 3), -x1 * x2 / pow(norm, 3),\n -x1 * x3 / pow(norm, 3), -x1 * x2 / pow(norm, 3), 1.0 / norm - x2 * x2 / pow(norm, 3),\n -x2 * x3 / pow(norm, 3), -x1 * x3 / pow(norm, 3), -x2 * x3 / pow(norm, 3),\n 1.0 / norm - x3 * x3 / pow(norm, 3);\n reduce = tangent_base * norm_jaco;\n#else\n reduce << 1. / dep_j, 0, -pts_camera_j(0) / (dep_j * dep_j), 0, 1. / dep_j,\n -pts_camera_j(1) / (dep_j * dep_j);\n#endif\n reduce = sqrt_info * reduce;\n\n if (jacobians[0])\n {\n Eigen::Map> jacobian_pose_i(jacobians[0]);\n\n Eigen::Matrix jaco_i;\n jaco_i.leftCols<3>() = ric.transpose() * Rj.transpose();\n jaco_i.rightCols<3>() =\n ric.transpose() * Rj.transpose() * Ri * -Utility::skewSymmetric(pts_imu_i);\n\n jacobian_pose_i.leftCols<6>() = reduce * jaco_i;\n jacobian_pose_i.rightCols<1>().setZero();\n }\n\n if (jacobians[1])\n {\n Eigen::Map> jacobian_pose_j(jacobians[1]);\n\n Eigen::Matrix jaco_j;\n jaco_j.leftCols<3>() = ric.transpose() * -Rj.transpose();\n jaco_j.rightCols<3>() = ric.transpose() * Utility::skewSymmetric(pts_imu_j);\n\n jacobian_pose_j.leftCols<6>() = reduce * jaco_j;\n jacobian_pose_j.rightCols<1>().setZero();\n }\n if (jacobians[2])\n {\n Eigen::Map> jacobian_ex_pose(jacobians[2]);\n Eigen::Matrix jaco_ex;\n jaco_ex.leftCols<3>() =\n ric.transpose() * (Rj.transpose() * Ri - Eigen::Matrix3d::Identity());\n Eigen::Matrix3d tmp_r = ric.transpose() * Rj.transpose() * Ri * ric;\n jaco_ex.rightCols<3>() =\n -tmp_r * Utility::skewSymmetric(pts_camera_i) +\n Utility::skewSymmetric(tmp_r * pts_camera_i) +\n Utility::skewSymmetric(ric.transpose() *\n (Rj.transpose() * (Ri * tic + Pi - Pj) - tic));\n jacobian_ex_pose.leftCols<6>() = reduce * jaco_ex;\n jacobian_ex_pose.rightCols<1>().setZero();\n }\n if (jacobians[3])\n {\n Eigen::Map jacobian_feature(jacobians[3]);\n#if 1\n jacobian_feature = reduce * ric.transpose() * Rj.transpose() * Ri * ric * pts_i * -1.0 /\n (inv_dep_i * inv_dep_i);\n#else\n jacobian_feature = reduce * ric.transpose() * Rj.transpose() * Ri * ric * pts_i;\n#endif\n }\n }\n sum_t += tic_toc.toc();\n\n return true;\n}\n\nvoid ProjectionFactor::check(double **parameters)\n{\n double *res = new double[15];\n double **jaco = new double *[4];\n jaco[0] = new double[2 * 7];\n jaco[1] = new double[2 * 7];\n jaco[2] = new double[2 * 7];\n jaco[3] = new double[2 * 1];\n Evaluate(parameters, res, jaco);\n puts(\"check begins\");\n\n puts(\"my\");\n\n std::cout << Eigen::Map>(res).transpose() << std::endl << std::endl;\n std::cout << Eigen::Map>(jaco[0]) << std::endl\n << std::endl;\n std::cout << Eigen::Map>(jaco[1]) << std::endl\n << std::endl;\n std::cout << Eigen::Map>(jaco[2]) << std::endl\n << std::endl;\n std::cout << Eigen::Map(jaco[3]) << std::endl << std::endl;\n\n Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]);\n Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]);\n\n Eigen::Vector3d Pj(parameters[1][0], parameters[1][1], parameters[1][2]);\n Eigen::Quaterniond Qj(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]);\n\n Eigen::Vector3d tic(parameters[2][0], parameters[2][1], parameters[2][2]);\n Eigen::Quaterniond qic(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]);\n double inv_dep_i = parameters[3][0];\n\n Eigen::Vector3d pts_camera_i = pts_i / inv_dep_i;\n Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic;\n Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi;\n Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj);\n Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic);\n\n Eigen::Vector2d residual;\n#ifdef UNIT_SPHERE_ERROR\n residual = tangent_base * (pts_camera_j.normalized() - pts_j.normalized());\n#else\n double dep_j = pts_camera_j.z();\n residual = (pts_camera_j / dep_j).head<2>() - pts_j.head<2>();\n#endif\n residual = sqrt_info * residual;\n\n puts(\"num\");\n std::cout << residual.transpose() << std::endl;\n\n const double eps = 1e-6;\n Eigen::Matrix num_jacobian;\n for (int k = 0; k < 19; k++)\n {\n Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]);\n Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4],\n parameters[0][5]);\n\n Eigen::Vector3d Pj(parameters[1][0], parameters[1][1], parameters[1][2]);\n Eigen::Quaterniond Qj(parameters[1][6], parameters[1][3], parameters[1][4],\n parameters[1][5]);\n\n Eigen::Vector3d tic(parameters[2][0], parameters[2][1], parameters[2][2]);\n Eigen::Quaterniond qic(parameters[2][6], parameters[2][3], parameters[2][4],\n parameters[2][5]);\n double inv_dep_i = parameters[3][0];\n\n int a = k / 3, b = k % 3;\n Eigen::Vector3d delta = Eigen::Vector3d(b == 0, b == 1, b == 2) * eps;\n\n if (a == 0)\n Pi += delta;\n else if (a == 1)\n Qi = Qi * Utility::deltaQ(delta);\n else if (a == 2)\n Pj += delta;\n else if (a == 3)\n Qj = Qj * Utility::deltaQ(delta);\n else if (a == 4)\n tic += delta;\n else if (a == 5)\n qic = qic * Utility::deltaQ(delta);\n else if (a == 6)\n inv_dep_i += delta.x();\n\n Eigen::Vector3d pts_camera_i = pts_i / inv_dep_i;\n Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic;\n Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi;\n Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj);\n Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic);\n\n Eigen::Vector2d tmp_residual;\n#ifdef UNIT_SPHERE_ERROR\n tmp_residual = tangent_base * (pts_camera_j.normalized() - pts_j.normalized());\n#else\n double dep_j = pts_camera_j.z();\n tmp_residual = (pts_camera_j / dep_j).head<2>() - pts_j.head<2>();\n#endif\n tmp_residual = sqrt_info * tmp_residual;\n num_jacobian.col(k) = (tmp_residual - residual) / eps;\n }\n std::cout << num_jacobian << std::endl;\n}\n" }, { "path": "vins_estimator/src/factor/projection_factor.h", "content": "#pragma once\n\n#include \n#include \n#include \n#include \"../utility/utility.h\"\n#include \"../utility/tic_toc.h\"\n#include \"../utility/parameters.h\"\n\nclass ProjectionFactor : public ceres::SizedCostFunction<2, 7, 7, 7, 1>\n{\npublic:\n ProjectionFactor(const Eigen::Vector3d &_pts_i, const Eigen::Vector3d &_pts_j);\n virtual bool Evaluate(double const *const *parameters, double *residuals,\n double **jacobians) const;\n void check(double **parameters);\n\n Eigen::Vector3d pts_i, pts_j;\n Eigen::Matrix tangent_base;\n static Eigen::Matrix2d sqrt_info;\n static double sum_t;\n};\n" }, { "path": "vins_estimator/src/factor/projection_td_factor.cpp", "content": "#include \"projection_td_factor.h\"\n\nEigen::Matrix2d ProjectionTdFactor::sqrt_info;\ndouble ProjectionTdFactor::sum_t;\n\nProjectionTdFactor::ProjectionTdFactor(const Eigen::Vector3d &_pts_i, const Eigen::Vector3d &_pts_j,\n const Eigen::Vector2d &_velocity_i,\n const Eigen::Vector2d &_velocity_j, const double _td_i,\n const double _td_j, const double _row_i, const double _row_j)\n : pts_i(_pts_i), pts_j(_pts_j), td_i(_td_i), td_j(_td_j)\n{\n velocity_i.x() = _velocity_i.x();\n velocity_i.y() = _velocity_i.y();\n velocity_i.z() = 0;\n velocity_j.x() = _velocity_j.x();\n velocity_j.y() = _velocity_j.y();\n velocity_j.z() = 0;\n row_i = _row_i - ROW / 2;\n row_j = _row_j - ROW / 2;\n\n#ifdef UNIT_SPHERE_ERROR\n Eigen::Vector3d b1, b2;\n Eigen::Vector3d a = pts_j.normalized();\n Eigen::Vector3d tmp(0, 0, 1);\n if (a == tmp)\n tmp << 1, 0, 0;\n b1 = (tmp - a * (a.transpose() * tmp)).normalized();\n b2 = a.cross(b1);\n tangent_base.block<1, 3>(0, 0) = b1.transpose();\n tangent_base.block<1, 3>(1, 0) = b2.transpose();\n#endif\n};\n\nbool ProjectionTdFactor::Evaluate(double const *const *parameters, double *residuals,\n double **jacobians) const\n{\n TicToc tic_toc;\n Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]);\n Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]);\n\n Eigen::Vector3d Pj(parameters[1][0], parameters[1][1], parameters[1][2]);\n Eigen::Quaterniond Qj(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]);\n\n Eigen::Vector3d tic(parameters[2][0], parameters[2][1], parameters[2][2]);\n Eigen::Quaterniond qic(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]);\n\n double inv_dep_i = parameters[3][0];\n\n double td = parameters[4][0];\n\n Eigen::Vector3d pts_i_td, pts_j_td;\n pts_i_td = pts_i - (td - td_i + TR / ROW * row_i) * velocity_i;\n pts_j_td = pts_j - (td - td_j + TR / ROW * row_j) * velocity_j;\n Eigen::Vector3d pts_camera_i = pts_i_td / inv_dep_i;\n Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic;\n Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi;\n Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj);\n Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic);\n Eigen::Map residual(residuals);\n\n#ifdef UNIT_SPHERE_ERROR\n residual = tangent_base * (pts_camera_j.normalized() - pts_j_td.normalized());\n#else\n double dep_j = pts_camera_j.z();\n residual = (pts_camera_j / dep_j).head<2>() - pts_j_td.head<2>();\n#endif\n\n residual = sqrt_info * residual;\n\n if (jacobians)\n {\n Eigen::Matrix3d Ri = Qi.toRotationMatrix();\n Eigen::Matrix3d Rj = Qj.toRotationMatrix();\n Eigen::Matrix3d ric = qic.toRotationMatrix();\n Eigen::Matrix reduce(2, 3);\n#ifdef UNIT_SPHERE_ERROR\n double norm = pts_camera_j.norm();\n Eigen::Matrix3d norm_jaco;\n double x1, x2, x3;\n x1 = pts_camera_j(0);\n x2 = pts_camera_j(1);\n x3 = pts_camera_j(2);\n norm_jaco << 1.0 / norm - x1 * x1 / pow(norm, 3), -x1 * x2 / pow(norm, 3),\n -x1 * x3 / pow(norm, 3), -x1 * x2 / pow(norm, 3), 1.0 / norm - x2 * x2 / pow(norm, 3),\n -x2 * x3 / pow(norm, 3), -x1 * x3 / pow(norm, 3), -x2 * x3 / pow(norm, 3),\n 1.0 / norm - x3 * x3 / pow(norm, 3);\n reduce = tangent_base * norm_jaco;\n#else\n reduce << 1. / dep_j, 0, -pts_camera_j(0) / (dep_j * dep_j), 0, 1. / dep_j,\n -pts_camera_j(1) / (dep_j * dep_j);\n#endif\n reduce = sqrt_info * reduce;\n\n if (jacobians[0])\n {\n Eigen::Map> jacobian_pose_i(jacobians[0]);\n\n Eigen::Matrix jaco_i;\n jaco_i.leftCols<3>() = ric.transpose() * Rj.transpose();\n jaco_i.rightCols<3>() =\n ric.transpose() * Rj.transpose() * Ri * -Utility::skewSymmetric(pts_imu_i);\n\n jacobian_pose_i.leftCols<6>() = reduce * jaco_i;\n jacobian_pose_i.rightCols<1>().setZero();\n }\n\n if (jacobians[1])\n {\n Eigen::Map> jacobian_pose_j(jacobians[1]);\n\n Eigen::Matrix jaco_j;\n jaco_j.leftCols<3>() = ric.transpose() * -Rj.transpose();\n jaco_j.rightCols<3>() = ric.transpose() * Utility::skewSymmetric(pts_imu_j);\n\n jacobian_pose_j.leftCols<6>() = reduce * jaco_j;\n jacobian_pose_j.rightCols<1>().setZero();\n }\n if (jacobians[2])\n {\n Eigen::Map> jacobian_ex_pose(jacobians[2]);\n Eigen::Matrix jaco_ex;\n jaco_ex.leftCols<3>() =\n ric.transpose() * (Rj.transpose() * Ri - Eigen::Matrix3d::Identity());\n Eigen::Matrix3d tmp_r = ric.transpose() * Rj.transpose() * Ri * ric;\n jaco_ex.rightCols<3>() =\n -tmp_r * Utility::skewSymmetric(pts_camera_i) +\n Utility::skewSymmetric(tmp_r * pts_camera_i) +\n Utility::skewSymmetric(ric.transpose() *\n (Rj.transpose() * (Ri * tic + Pi - Pj) - tic));\n jacobian_ex_pose.leftCols<6>() = reduce * jaco_ex;\n jacobian_ex_pose.rightCols<1>().setZero();\n }\n if (jacobians[3])\n {\n Eigen::Map jacobian_feature(jacobians[3]);\n jacobian_feature = reduce * ric.transpose() * Rj.transpose() * Ri * ric * pts_i_td *\n -1.0 / (inv_dep_i * inv_dep_i);\n }\n if (jacobians[4])\n {\n Eigen::Map jacobian_td(jacobians[4]);\n jacobian_td = reduce * ric.transpose() * Rj.transpose() * Ri * ric * velocity_i /\n inv_dep_i * -1.0 +\n sqrt_info * velocity_j.head(2);\n }\n }\n sum_t += tic_toc.toc();\n\n return true;\n}\n\nvoid ProjectionTdFactor::check(double **parameters)\n{\n double *res = new double[2];\n double **jaco = new double *[5];\n jaco[0] = new double[2 * 7];\n jaco[1] = new double[2 * 7];\n jaco[2] = new double[2 * 7];\n jaco[3] = new double[2 * 1];\n jaco[4] = new double[2 * 1];\n Evaluate(parameters, res, jaco);\n puts(\"check begins\");\n\n puts(\"my\");\n\n std::cout << Eigen::Map>(res).transpose() << std::endl << std::endl;\n std::cout << Eigen::Map>(jaco[0]) << std::endl\n << std::endl;\n std::cout << Eigen::Map>(jaco[1]) << std::endl\n << std::endl;\n std::cout << Eigen::Map>(jaco[2]) << std::endl\n << std::endl;\n std::cout << Eigen::Map(jaco[3]) << std::endl << std::endl;\n std::cout << Eigen::Map(jaco[4]) << std::endl << std::endl;\n\n Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]);\n Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]);\n\n Eigen::Vector3d Pj(parameters[1][0], parameters[1][1], parameters[1][2]);\n Eigen::Quaterniond Qj(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]);\n\n Eigen::Vector3d tic(parameters[2][0], parameters[2][1], parameters[2][2]);\n Eigen::Quaterniond qic(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]);\n double inv_dep_i = parameters[3][0];\n double td = parameters[4][0];\n\n Eigen::Vector3d pts_i_td, pts_j_td;\n pts_i_td = pts_i - (td - td_i + TR / ROW * row_i) * velocity_i;\n pts_j_td = pts_j - (td - td_j + TR / ROW * row_j) * velocity_j;\n Eigen::Vector3d pts_camera_i = pts_i_td / inv_dep_i;\n Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic;\n Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi;\n Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj);\n Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic);\n Eigen::Vector2d residual;\n\n#ifdef UNIT_SPHERE_ERROR\n residual = tangent_base * (pts_camera_j.normalized() - pts_j_td.normalized());\n#else\n double dep_j = pts_camera_j.z();\n residual = (pts_camera_j / dep_j).head<2>() - pts_j_td.head<2>();\n#endif\n residual = sqrt_info * residual;\n\n puts(\"num\");\n std::cout << residual.transpose() << std::endl;\n\n const double eps = 1e-6;\n Eigen::Matrix num_jacobian;\n for (int k = 0; k < 20; k++)\n {\n Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]);\n Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4],\n parameters[0][5]);\n\n Eigen::Vector3d Pj(parameters[1][0], parameters[1][1], parameters[1][2]);\n Eigen::Quaterniond Qj(parameters[1][6], parameters[1][3], parameters[1][4],\n parameters[1][5]);\n\n Eigen::Vector3d tic(parameters[2][0], parameters[2][1], parameters[2][2]);\n Eigen::Quaterniond qic(parameters[2][6], parameters[2][3], parameters[2][4],\n parameters[2][5]);\n double inv_dep_i = parameters[3][0];\n double td = parameters[4][0];\n\n int a = k / 3, b = k % 3;\n Eigen::Vector3d delta = Eigen::Vector3d(b == 0, b == 1, b == 2) * eps;\n\n if (a == 0)\n Pi += delta;\n else if (a == 1)\n Qi = Qi * Utility::deltaQ(delta);\n else if (a == 2)\n Pj += delta;\n else if (a == 3)\n Qj = Qj * Utility::deltaQ(delta);\n else if (a == 4)\n tic += delta;\n else if (a == 5)\n qic = qic * Utility::deltaQ(delta);\n else if (a == 6 && b == 0)\n inv_dep_i += delta.x();\n else if (a == 6 && b == 1)\n td += delta.y();\n\n Eigen::Vector3d pts_i_td, pts_j_td;\n pts_i_td = pts_i - (td - td_i + TR / ROW * row_i) * velocity_i;\n pts_j_td = pts_j - (td - td_j + TR / ROW * row_j) * velocity_j;\n Eigen::Vector3d pts_camera_i = pts_i_td / inv_dep_i;\n Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic;\n Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi;\n Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj);\n Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic);\n Eigen::Vector2d tmp_residual;\n\n#ifdef UNIT_SPHERE_ERROR\n tmp_residual = tangent_base * (pts_camera_j.normalized() - pts_j_td.normalized());\n#else\n double dep_j = pts_camera_j.z();\n tmp_residual = (pts_camera_j / dep_j).head<2>() - pts_j_td.head<2>();\n#endif\n tmp_residual = sqrt_info * tmp_residual;\n\n num_jacobian.col(k) = (tmp_residual - residual) / eps;\n }\n std::cout << num_jacobian << std::endl;\n}\n" }, { "path": "vins_estimator/src/factor/projection_td_factor.h", "content": "#pragma once\n\n#include \n#include \n#include \n#include \"../utility/utility.h\"\n#include \"../utility/tic_toc.h\"\n#include \"../utility/parameters.h\"\n\nclass ProjectionTdFactor : public ceres::SizedCostFunction<2, 7, 7, 7, 1, 1>\n{\npublic:\n ProjectionTdFactor(const Eigen::Vector3d &_pts_i, const Eigen::Vector3d &_pts_j,\n const Eigen::Vector2d &_velocity_i, const Eigen::Vector2d &_velocity_j,\n const double _td_i, const double _td_j, const double _row_i,\n const double _row_j);\n virtual bool Evaluate(double const *const *parameters, double *residuals,\n double **jacobians) const;\n void check(double **parameters);\n\n Eigen::Vector3d pts_i, pts_j;\n Eigen::Vector3d velocity_i, velocity_j;\n double td_i, td_j;\n Eigen::Matrix tangent_base;\n double row_i, row_j;\n static Eigen::Matrix2d sqrt_info;\n static double sum_t;\n};\n" }, { "path": "vins_estimator/src/feature_manager/feature_manager.cpp", "content": "#include \"feature_manager.h\"\n#include \n#include \n#include \n#include \n\nint FeaturePerId::endFrame()\n{\n return start_frame + feature_per_frame.size() - 1;\n}\n\nFeatureManager::FeatureManager(Matrix3d _Rs[]) : Rs(_Rs)\n{\n for (auto &i : ric)\n i.setIdentity();\n}\n\nvoid FeatureManager::setRic(Matrix3d _ric[])\n{\n for (int i = 0; i < NUM_OF_CAM; i++)\n {\n ric[i] = _ric[i];\n }\n}\n\nvoid FeatureManager::clearState()\n{\n feature.clear();\n}\n\nint FeatureManager::getFeatureCount()\n{\n int cnt = 0;\n for (auto &it : feature)\n {\n if (it.is_dynamic)\n {\n continue;\n }\n it.used_num = it.feature_per_frame.size();\n // if (it.used_num >= 4)\n // cnt++;\n if (it.used_num >= 2 && it.start_frame < WINDOW_SIZE - 2)\n {\n cnt++;\n }\n }\n return cnt;\n}\n\nvoid FeatureManager::inputDepth(const cv::Mat &_depth_img)\n{\n depth_img = _depth_img;\n}\n\nbool FeatureManager::addFeatureCheckParallax(int frame_count,\n map> &image,\n double td)\n{\n ROS_DEBUG(\"input feature: %d\", (int)image.size());\n ROS_DEBUG(\"num of feature: %d\", getFeatureCount());\n double parallax_sum = 0;\n int parallax_num = 0;\n last_track_num = 0;\n\n for (auto iter = image.begin(), iter_next = image.begin(); iter != image.end();\n iter = iter_next)\n {\n ++iter_next;\n\n unsigned short pt_depth_mm =\n depth_img.at((int)iter->second(4), (int)iter->second(3));\n\n double pt_depth_m = pt_depth_mm / 1000.0;\n\n if (0 < pt_depth_m && pt_depth_m < DEPTH_MIN_DIST)\n {\n image.erase(iter);\n continue;\n }\n\n // std::find_if: http://c.biancheng.net/view/571.html\n int feature_id = iter->first;\n auto it =\n find_if(feature.begin(), feature.end(),\n [feature_id](const FeaturePerId &it) { return it.feature_id == feature_id; });\n\n if (it == feature.end())\n {\n feature.emplace_back(FeaturePerId(feature_id, frame_count));\n feature.back().feature_per_frame.emplace_back(\n FeaturePerFrame(iter->second, td, pt_depth_m));\n }\n else if (it->feature_id == feature_id)\n {\n it->feature_per_frame.emplace_back(FeaturePerFrame(iter->second, td, pt_depth_m));\n last_track_num++;\n }\n }\n if (frame_count < 2 || last_track_num < 20)\n return true;\n\n for (auto &it_per_id : feature)\n {\n if (it_per_id.start_frame <= frame_count - 2 &&\n it_per_id.start_frame + int(it_per_id.feature_per_frame.size()) - 1 >= frame_count - 1)\n {\n parallax_sum += compensatedParallax2(it_per_id, frame_count);\n parallax_num++;\n }\n }\n\n if (parallax_num == 0)\n {\n return true;\n }\n else\n {\n ROS_DEBUG(\"parallax_sum: %lf, parallax_num: %d\", parallax_sum, parallax_num);\n ROS_DEBUG(\"current parallax: %lf\", parallax_sum / parallax_num * FOCAL_LENGTH);\n return parallax_sum / parallax_num >= MIN_PARALLAX;\n }\n}\n\nvoid FeatureManager::debugShow()\n{\n ROS_DEBUG(\"debug show\");\n for (auto &it : feature)\n {\n ROS_ASSERT(!it.feature_per_frame.empty());\n ROS_ASSERT(it.start_frame >= 0);\n ROS_ASSERT(it.used_num >= 0);\n\n ROS_DEBUG(\"%d,%d,%d \", it.feature_id, it.used_num, it.start_frame);\n int sum = 0;\n for (auto &j : it.feature_per_frame)\n {\n ROS_DEBUG(\"%d,\", int(j.is_used));\n sum += j.is_used;\n printf(\"(%lf,%lf) \", j.point(0), j.point(1));\n }\n ROS_ASSERT(it.used_num == sum);\n }\n}\n\nvector> FeatureManager::getCorresponding(int frame_count_l,\n int frame_count_r)\n{\n vector> corres;\n for (auto &it : feature)\n {\n if (it.start_frame <= frame_count_l && it.endFrame() >= frame_count_r)\n {\n Vector3d a = Vector3d::Zero(), b = Vector3d::Zero();\n int idx_l = frame_count_l - it.start_frame;\n int idx_r = frame_count_r - it.start_frame;\n\n a = it.feature_per_frame[idx_l].point;\n\n b = it.feature_per_frame[idx_r].point;\n\n corres.emplace_back(a, b);\n }\n }\n return corres;\n}\n\nvector> FeatureManager::getCorrespondingWithDepth(int frame_count_l,\n int frame_count_r)\n{\n vector> corres;\n for (auto &it : feature)\n {\n if (it.start_frame <= frame_count_l && it.endFrame() >= frame_count_r)\n {\n Vector3d a = Vector3d::Zero(), b = Vector3d::Zero();\n int idx_l = frame_count_l - it.start_frame;\n int idx_r = frame_count_r - it.start_frame;\n\n double depth_a = it.feature_per_frame[idx_l].depth;\n if (depth_a < 0.1 || depth_a > 10) // max and min measurement\n continue;\n double depth_b = it.feature_per_frame[idx_r].depth;\n if (depth_b < 0.1 || depth_b > 10) // max and min measurement\n continue;\n a = it.feature_per_frame[idx_l].point;\n b = it.feature_per_frame[idx_r].point;\n a = a * depth_a;\n b = b * depth_b;\n\n corres.emplace_back(a, b);\n }\n }\n return corres;\n}\n\nvoid FeatureManager::setDepth(const VectorXd &x)\n{\n int feature_index = -1;\n for (auto &it_per_id : feature)\n {\n if (it_per_id.is_dynamic)\n {\n continue;\n }\n it_per_id.used_num = it_per_id.feature_per_frame.size();\n // if (it_per_id.used_num < 4)\n // continue;\n if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))\n continue;\n\n it_per_id.estimated_depth = 1.0 / x(++feature_index);\n // ROS_INFO(\"feature id %d , start_frame %d, depth %f \",\n // it_per_id->feature_id, it_per_id-> start_frame,\n // it_per_id->estimated_depth);\n if (it_per_id.estimated_depth < 0)\n {\n it_per_id.solve_flag = 2;\n }\n else\n it_per_id.solve_flag = 1;\n }\n}\n\nvoid FeatureManager::removeFailures()\n{\n for (auto it = feature.begin(), it_next = feature.begin(); it != feature.end(); it = it_next)\n {\n it_next++;\n if (it->solve_flag == 2)\n feature.erase(it);\n }\n}\n\nvoid FeatureManager::removeFailuresOutliers(Vector3d Ps[], Matrix3d _Rs[], Vector3d tic[],\n Matrix3d _ric[])\n{\n for (auto it = feature.begin(), it_next = feature.begin(); it != feature.end(); it = it_next)\n {\n it_next++;\n if (it->solve_flag == 2)\n {\n feature.erase(it);\n }\n else\n {\n if (it->is_dynamic)\n {\n continue;\n }\n double err = 0;\n int errCnt = 0;\n it->used_num = it->feature_per_frame.size();\n if (it->used_num < 4)\n continue;\n int imu_i = it->start_frame, imu_j = imu_i - 1;\n Vector3d pts_i = it->feature_per_frame[0].point;\n double depth = it->estimated_depth;\n for (auto &it_per_frame : it->feature_per_frame)\n {\n imu_j++;\n if (imu_i != imu_j)\n {\n Vector3d pts_j = it_per_frame.point;\n Vector3d pts_w = _Rs[imu_i] * (_ric[0] * (depth * pts_i) + tic[0]) + Ps[imu_i];\n Vector3d pts_cj = _ric[0].transpose() *\n (_Rs[imu_j].transpose() * (pts_w - Ps[imu_j]) - tic[0]);\n Vector2d residual = (pts_cj / pts_cj.z()).head<2>() - pts_j.head<2>();\n double rx = residual.x();\n double ry = residual.y();\n double tmp_error = sqrt(rx * rx + ry * ry);\n err += tmp_error;\n errCnt++;\n // printf(\"tmp_error %f\\n\", FOCAL_LENGTH / 1.5 * tmp_error);\n }\n }\n double ave_err = err / errCnt;\n if (ave_err * FOCAL_LENGTH > 10)\n feature.erase(it);\n }\n }\n}\n\nvoid FeatureManager::clearDepth(const VectorXd &x)\n{\n int feature_index = -1;\n for (auto &it_per_id : feature)\n {\n if (it_per_id.is_dynamic)\n {\n continue;\n }\n it_per_id.used_num = it_per_id.feature_per_frame.size();\n // if (it_per_id.used_num < 4)\n // continue;\n if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))\n continue;\n it_per_id.estimated_depth = 1.0 / x(++feature_index);\n }\n}\n\nVectorXd FeatureManager::getDepthVector()\n{\n VectorXd dep_vec(getFeatureCount());\n int feature_index = -1;\n for (auto &it_per_id : feature)\n {\n if (it_per_id.is_dynamic)\n {\n continue;\n }\n it_per_id.used_num = it_per_id.feature_per_frame.size();\n // if (it_per_id.used_num < 4)\n // continue;\n if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))\n continue;\n#if 1\n dep_vec(++feature_index) = 1. / it_per_id.estimated_depth;\n#else\n dep_vec(++feature_index) = it_per_id->estimated_depth;\n#endif\n }\n return dep_vec;\n}\n\nvoid FeatureManager::triangulate(Vector3d Ps[], Vector3d tic[], Matrix3d _ric[])\n{\n for (auto &it_per_id : feature)\n {\n it_per_id.used_num = it_per_id.feature_per_frame.size();\n if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))\n continue;\n // if (it_per_id.used_num < 4)\n // continue;\n // if (!(it_per_id.used_num >depth_corner > 0)\n // continue;\n\n int imu_i = it_per_id.start_frame, imu_j = imu_i - 1;\n\n ROS_ASSERT(NUM_OF_CAM == 1);\n Eigen::MatrixXd svd_A(2 * it_per_id.feature_per_frame.size(), 4);\n int svd_idx = 0;\n\n Eigen::Matrix P0;\n Eigen::Vector3d t0 = Ps[imu_i] + Rs[imu_i] * tic[0];\n Eigen::Matrix3d R0 = Rs[imu_i] * _ric[0];\n P0.leftCols<3>() = Eigen::Matrix3d::Identity();\n P0.rightCols<1>() = Eigen::Vector3d::Zero();\n\n for (auto &it_per_frame : it_per_id.feature_per_frame)\n {\n imu_j++;\n\n Eigen::Vector3d t1 = Ps[imu_j] + Rs[imu_j] * tic[0];\n Eigen::Matrix3d R1 = Rs[imu_j] * _ric[0];\n Eigen::Vector3d t = R0.transpose() * (t1 - t0);\n Eigen::Matrix3d R = R0.transpose() * R1;\n Eigen::Matrix P;\n P.leftCols<3>() = R.transpose();\n P.rightCols<1>() = -R.transpose() * t;\n Eigen::Vector3d f = it_per_frame.point.normalized();\n svd_A.row(svd_idx++) = f[0] * P.row(2) - f[2] * P.row(0);\n svd_A.row(svd_idx++) = f[1] * P.row(2) - f[2] * P.row(1);\n\n if (imu_i == imu_j)\n continue;\n }\n ROS_ASSERT(svd_idx == svd_A.rows());\n Eigen::Vector4d svd_V =\n Eigen::JacobiSVD(svd_A, Eigen::ComputeThinV).matrixV().rightCols<1>();\n double svd_method = svd_V[2] / svd_V[3];\n // it_per_id->estimated_depth = -b / A;\n // it_per_id->estimated_depth = svd_V[2] / svd_V[3];\n\n it_per_id.estimated_depth = svd_method;\n // it_per_id->estimated_depth = INIT_DEPTH;\n it_per_id.estimate_flag = 2;\n if (it_per_id.estimated_depth < 0.1)\n {\n it_per_id.estimated_depth = INIT_DEPTH;\n it_per_id.estimate_flag = 0;\n }\n }\n}\n\nvoid FeatureManager::triangulateWithDepth(Vector3d _Ps[], Vector3d _tic[], Matrix3d _ric[])\n{\n for (auto &it_per_id : feature)\n {\n if (it_per_id.estimated_depth > 0)\n continue;\n if (it_per_id.is_dynamic)\n {\n continue;\n }\n it_per_id.used_num = it_per_id.feature_per_frame.size();\n if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))\n continue;\n\n int imu_i = it_per_id.start_frame;\n\n Eigen::Vector3d tr = _Ps[imu_i] + Rs[imu_i] * _tic[0];\n Eigen::Matrix3d Rr = Rs[imu_i] * _ric[0];\n\n vector verified_depths;\n int no_depth_num = 0;\n\n vector rough_depths;\n /**\n * @brief\n * 对同一id特征点进行深度交叉验证，并将重投影误差小于阈值的点的，在初始帧上的深度做平均作为估计的深度\n */\n for (int k = 0; k < (int)it_per_id.feature_per_frame.size(); k++)\n {\n if (it_per_id.feature_per_frame[k].depth == 0)\n {\n no_depth_num++;\n continue;\n }\n Eigen::Vector3d t0 = _Ps[imu_i + k] + Rs[imu_i + k] * _tic[0];\n Eigen::Matrix3d R0 = Rs[imu_i + k] * _ric[0];\n Eigen::Vector3d point0(it_per_id.feature_per_frame[k].point *\n it_per_id.feature_per_frame[k].depth);\n\n // transform to reference frame\n Eigen::Vector3d t2r = Rr.transpose() * (t0 - tr);\n Eigen::Matrix3d R2r = Rr.transpose() * R0;\n\n for (int j = 0; j < (int)it_per_id.feature_per_frame.size(); j++)\n {\n if (k == j)\n continue;\n Eigen::Vector3d t1 = _Ps[imu_i + j] + Rs[imu_i + j] * _tic[0];\n Eigen::Matrix3d R1 = Rs[imu_i + j] * _ric[0];\n Eigen::Vector3d t20 = R0.transpose() * (t1 - t0);\n Eigen::Matrix3d R20 = R0.transpose() * R1;\n\n Eigen::Vector3d point1_projected = R20.transpose() * point0 - R20.transpose() * t20;\n Eigen::Vector2d point1_2d(it_per_id.feature_per_frame[j].point.x(),\n it_per_id.feature_per_frame[j].point.y());\n Eigen::Vector2d residual =\n point1_2d - Vector2d(point1_projected.x() / point1_projected.z(),\n point1_projected.y() / point1_projected.z());\n if (residual.norm() < 10.0 / 460)\n { // this can also be adjust to improve performance\n Eigen::Vector3d point_r = R2r * point0 + t2r;\n\n if (it_per_id.feature_per_frame[k].depth > DEPTH_MAX_DIST)\n {\n rough_depths.push_back(point_r.z());\n }\n else\n {\n verified_depths.push_back(point_r.z());\n }\n }\n }\n }\n\n if (verified_depths.empty())\n {\n if (rough_depths.empty())\n {\n if (no_depth_num == it_per_id.feature_per_frame.size())\n {\n int imu_j = imu_i - 1;\n\n ROS_ASSERT(NUM_OF_CAM == 1);\n Eigen::MatrixXd svd_A(2 * it_per_id.feature_per_frame.size(), 4);\n int svd_idx = 0;\n\n Eigen::Matrix P0;\n Eigen::Vector3d t0 = _Ps[imu_i] + Rs[imu_i] * _tic[0];\n Eigen::Matrix3d R0 = Rs[imu_i] * ric[0];\n P0.leftCols<3>() = Eigen::Matrix3d::Identity();\n P0.rightCols<1>() = Eigen::Vector3d::Zero();\n\n for (auto &it_per_frame : it_per_id.feature_per_frame)\n {\n imu_j++;\n\n Eigen::Vector3d t1 = _Ps[imu_j] + Rs[imu_j] * _tic[0];\n Eigen::Matrix3d R1 = Rs[imu_j] * ric[0];\n Eigen::Vector3d t = R0.transpose() * (t1 - t0);\n Eigen::Matrix3d R = R0.transpose() * R1;\n Eigen::Matrix P;\n P.leftCols<3>() = R.transpose();\n P.rightCols<1>() = -R.transpose() * t;\n Eigen::Vector3d f = it_per_frame.point.normalized();\n svd_A.row(svd_idx++) = f[0] * P.row(2) - f[2] * P.row(0);\n svd_A.row(svd_idx++) = f[1] * P.row(2) - f[2] * P.row(1);\n\n if (imu_i == imu_j)\n continue;\n }\n ROS_ASSERT(svd_idx == svd_A.rows());\n Eigen::Vector4d svd_V =\n Eigen::JacobiSVD(svd_A, Eigen::ComputeThinV)\n .matrixV()\n .rightCols<1>();\n double svd_method = svd_V[2] / svd_V[3];\n\n if (svd_method < DEPTH_MIN_DIST)\n {\n it_per_id.estimated_depth = DEPTH_MAX_DIST;\n it_per_id.estimate_flag = 2;\n }\n else\n {\n it_per_id.estimated_depth = svd_method;\n it_per_id.estimate_flag = 2;\n }\n }\n else\n {\n continue;\n }\n }\n else\n {\n double depth_sum =\n std::accumulate(std::begin(rough_depths), std::end(rough_depths), 0.0);\n double depth_ave = depth_sum / rough_depths.size();\n it_per_id.estimated_depth = depth_ave;\n it_per_id.estimate_flag = 0;\n }\n }\n else\n {\n double depth_sum =\n std::accumulate(std::begin(verified_depths), std::end(verified_depths), 0.0);\n double depth_ave = depth_sum / verified_depths.size();\n it_per_id.estimated_depth = depth_ave;\n it_per_id.estimate_flag = 1;\n }\n\n if (it_per_id.estimated_depth < 0.1)\n {\n it_per_id.estimated_depth = INIT_DEPTH;\n it_per_id.estimate_flag = 0;\n }\n }\n}\n\nbool FeatureManager::solvePoseByPnP(Eigen::Matrix3d &R, Eigen::Vector3d &P,\n vector &pts2D, vector &pts3D)\n{\n Eigen::Matrix3d R_initial;\n Eigen::Vector3d P_initial;\n\n // w_T_cam ---> cam_T_w\n R_initial = R.inverse(); // R_c_w from world to cam tzhang\n P_initial = -(R_initial * P); // P_c_w\n\n // printf(\"pnp size %d \\n\",(int)pts2D.size() );\n if (int(pts2D.size()) < 4)\n {\n printf(\"feature tracking not enough, please slowly move you device! \\n\");\n return false;\n }\n cv::Mat r, rvec, t, D, tmp_r;\n cv::eigen2cv(R_initial, tmp_r);\n cv::Rodrigues(tmp_r, rvec); //旋转矩阵到旋转向量 tzhang\n cv::eigen2cv(P_initial, t);\n cv::Mat K = (cv::Mat_(3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 1);\n bool pnp_succ;\n pnp_succ = cv::solvePnP(pts3D, pts2D, K, D, rvec, t, 1);\n // pnp_succ = solvePnPRansac(pts3D, pts2D, K, D, rvec, t, true, 100, 8.0 /\n // focalLength, 0.99, inliers);\n\n if (!pnp_succ)\n {\n printf(\"pnp failed ! \\n\");\n return false;\n }\n cv::Rodrigues(rvec, r); //旋转向量到旋转矩阵 tzhang\n // cout << \"r \" << endl << r << endl;\n Eigen::MatrixXd R_pnp;\n cv::cv2eigen(r, R_pnp);\n Eigen::MatrixXd T_pnp;\n cv::cv2eigen(t, T_pnp);\n\n // cam_T_w ---> w_T_cam\n R = R_pnp.transpose(); // R_w_c\n P = R * (-T_pnp);\n\n return true;\n}\n\nvoid FeatureManager::initFramePoseByPnP(int frameCnt, Vector3d Ps[], Matrix3d Rs[], Vector3d tic[],\n Matrix3d ric[])\n{\n if (frameCnt >\n 0) //对第一帧图像不做处理；因为此时路标点还未三角化，需要利用第一帧双目图像，进行路标点三角化\n // tzhang\n {\n vector pts2D;\n vector pts3D;\n for (auto &it_per_id : feature) //遍历每个路标点 tzhang\n {\n if (it_per_id.estimated_depth >\n 0) //该路标点完成了初始化，使用初始化完成的路标点，获取3D-2D点对\n // tzhang\n {\n int index = frameCnt - it_per_id.start_frame;\n if ((int)it_per_id.feature_per_frame.size() >=\n index + 1) // tzhang\n // 该路标点从start_frame图像帧到frameCnt对应的图像帧都能被观测到\n {\n //路标点在IMU坐标系坐标，第一次看到该路标点的图像帧时刻\n // tzhang\n Vector3d ptsInCam = ric[0] * (it_per_id.feature_per_frame[0].point *\n it_per_id.estimated_depth) +\n tic[0];\n // 路标点在世界坐标系下的坐标\n Vector3d ptsInWorld =\n Rs[it_per_id.start_frame] * ptsInCam + Ps[it_per_id.start_frame];\n\n cv::Point3f point3d(ptsInWorld.x(), ptsInWorld.y(),\n ptsInWorld.z()); //世界坐标系下三维坐标 tzhang\n cv::Point2f point2d(it_per_id.feature_per_frame[index].point.x(),\n it_per_id.feature_per_frame[index].point.y());\n pts3D.push_back(point3d);\n pts2D.push_back(point2d);\n }\n }\n }\n Eigen::Matrix3d RCam;\n Eigen::Vector3d PCam;\n // trans to w_T_cam\n // 以上一帧图像与世界坐标系之间的位姿变换作为后续PnP求解的初值 tzhang\n RCam = Rs[frameCnt - 1] * ric[0]; // R_w_c\n PCam = Rs[frameCnt - 1] * tic[0] + Ps[frameCnt - 1];\n\n if (solvePoseByPnP(RCam, PCam, pts2D, pts3D))\n {\n // trans to w_T_imu\n Rs[frameCnt] = RCam * ric[0].transpose(); // R_w_i = R_w_c*R_c_i\n Ps[frameCnt] = -RCam * ric[0].transpose() * tic[0] + PCam;\n }\n }\n}\n\nvoid FeatureManager::removeOutlier(set &outlierIndex)\n{\n std::set::iterator itSet;\n for (auto it = feature.begin(), it_next = feature.begin(); it != feature.end(); it = it_next)\n {\n it_next++;\n int index = it->feature_id;\n itSet = outlierIndex.find(index);\n if (itSet != outlierIndex.end())\n {\n feature.erase(it);\n // printf(\"remove outlier %d \\n\", index);\n }\n }\n}\n\nvoid FeatureManager::removeBackShiftDepth(Eigen::Matrix3d marg_R, Eigen::Vector3d marg_P,\n Eigen::Matrix3d new_R, Eigen::Vector3d new_P)\n{\n for (auto it = feature.begin(), it_next = feature.begin(); it != feature.end(); it = it_next)\n {\n it_next++;\n\n if (it->start_frame != 0)\n it->start_frame--;\n else\n {\n Eigen::Vector3d uv_i = it->feature_per_frame[0].point;\n it->feature_per_frame.erase(it->feature_per_frame.begin());\n if (it->feature_per_frame.size() < 2)\n {\n feature.erase(it);\n continue;\n }\n else\n {\n Eigen::Vector3d pts_i = uv_i * it->estimated_depth;\n Eigen::Vector3d w_pts_i = marg_R * pts_i + marg_P;\n Eigen::Vector3d pts_j = new_R.transpose() * (w_pts_i - new_P);\n double dep_j = pts_j(2);\n if (dep_j > 0)\n it->estimated_depth = dep_j;\n else\n it->estimated_depth = INIT_DEPTH;\n }\n }\n }\n}\n\nvoid FeatureManager::removeBack()\n{\n for (auto it = feature.begin(), it_next = feature.begin(); it != feature.end(); it = it_next)\n {\n it_next++;\n\n if (it->start_frame != 0)\n it->start_frame--;\n else\n {\n it->feature_per_frame.erase(it->feature_per_frame.begin());\n if (it->feature_per_frame.size() == 0)\n feature.erase(it);\n }\n }\n}\n\nvoid FeatureManager::removeFront(int frame_count)\n{\n for (auto it = feature.begin(), it_next = feature.begin(); it != feature.end(); it = it_next)\n {\n it_next++;\n\n if (it->start_frame == frame_count)\n {\n it->start_frame--;\n }\n else\n {\n int j = WINDOW_SIZE - 1 - it->start_frame;\n if (it->endFrame() < frame_count - 1)\n continue;\n it->feature_per_frame.erase(it->feature_per_frame.begin() + j);\n if (it->feature_per_frame.size() == 0)\n feature.erase(it);\n }\n }\n}\n\ndouble FeatureManager::compensatedParallax2(const FeaturePerId &it_per_id, int frame_count)\n{\n // check the second last frame is keyframe or not\n // parallax betwwen seconde last frame and third last frame\n const FeaturePerFrame &frame_i =\n it_per_id.feature_per_frame[frame_count - 2 - it_per_id.start_frame];\n const FeaturePerFrame &frame_j =\n it_per_id.feature_per_frame[frame_count - 1 - it_per_id.start_frame];\n\n double ans = 0;\n Vector3d p_j = frame_j.point;\n\n double u_j = p_j(0);\n double v_j = p_j(1);\n\n Vector3d p_i = frame_i.point;\n Vector3d p_i_comp;\n\n // int r_i = frame_count - 2;\n // int r_j = frame_count - 1;\n // p_i_comp = ric[camera_id_j].transpose() * Rs[r_j].transpose() * Rs[r_i] *\n // ric[camera_id_i] * p_i;\n p_i_comp = p_i;\n double dep_i = p_i(2);\n double u_i = p_i(0) / dep_i;\n double v_i = p_i(1) / dep_i;\n double du = u_i - u_j, dv = v_i - v_j;\n\n double dep_i_comp = p_i_comp(2);\n double u_i_comp = p_i_comp(0) / dep_i_comp;\n double v_i_comp = p_i_comp(1) / dep_i_comp;\n double du_comp = u_i_comp - u_j, dv_comp = v_i_comp - v_j;\n\n ans = max(ans, sqrt(min(du * du + dv * dv, du_comp * du_comp + dv_comp * dv_comp)));\n\n return ans;\n}\n" }, { "path": "vins_estimator/src/feature_manager/feature_manager.h", "content": "#ifndef FEATURE_MANAGER_H\n#define FEATURE_MANAGER_H\n\n#include \n#include \n#include \n#include \n\nusing namespace std;\n\n#include \n\nusing namespace Eigen;\n\n#include \n#include \n\n#include \n\n#include \"../utility/parameters.h\"\n\n#include \"../utility/tic_toc.h\"\n\nstruct DynamicObject\n{\n int x_center;\n int y_center;\n int x_vel;\n int y_vel;\n int x_weight_vel;\n int y_weight_vel;\n int no_update_times;\n bool is_update;\n int x1;\n int y1;\n int x2;\n int y2;\n};\n\nclass FeaturePerFrame\n{\npublic:\n FeaturePerFrame(Eigen::Matrix _point, double td, double _depth)\n {\n point.x() = _point(0);\n point.y() = _point(1);\n point.z() = _point(2);\n uv.x() = _point(3);\n uv.y() = _point(4);\n velocity.x() = _point(5);\n velocity.y() = _point(6);\n depth = _depth;\n cur_td = td;\n }\n\n double cur_td;\n Vector3d point;\n Vector2d uv;\n Vector2d velocity;\n double z{};\n bool is_used{};\n MatrixXd A;\n VectorXd b;\n double depth;\n};\n\nclass FeaturePerId\n{\npublic:\n const int feature_id;\n int start_frame;\n vector feature_per_frame;\n\n int used_num;\n bool is_dynamic;\n double estimated_depth;\n int estimate_flag; // 0 initial; 1 by depth image; 2 by triangulate\n int solve_flag; // 0 haven't solve yet; 1 solve succ; 2 solve fail;\n\n FeaturePerId(int _feature_id, int _start_frame)\n : feature_id(_feature_id), start_frame(_start_frame), used_num(0), is_dynamic(false),\n estimated_depth(-1.0), estimate_flag(0), solve_flag(0)\n {\n }\n\n int endFrame();\n};\n\nclass FeatureManager\n{\npublic:\n explicit FeatureManager(Matrix3d Rs[]);\n\n void setRic(Matrix3d _ric[]);\n\n void clearState();\n\n int getFeatureCount();\n\n bool addFeatureCheckParallax(int frame_count, map> &image,\n double td);\n\n void debugShow();\n\n vector> getCorresponding(int frame_count_l, int frame_count_r);\n\n vector> getCorrespondingWithDepth(int frame_count_l,\n int frame_count_r);\n\n // void updateDepth(const VectorXd &x);\n void setDepth(const VectorXd &x);\n\n void removeFailures();\n void removeFailuresOutliers(Vector3d Ps[], Matrix3d Rs[], Vector3d tic[], Matrix3d _ric[]);\n\n void clearDepth(const VectorXd &x);\n\n VectorXd getDepthVector();\n\n void triangulate(Vector3d Ps[], Vector3d tic[], Matrix3d _ric[]);\n\n void triangulateWithDepth(Vector3d Ps[], Vector3d _tic[], Matrix3d _ric[]);\n\n void initFramePoseByPnP(int frameCnt, Vector3d Ps[], Matrix3d Rs[], Vector3d tic[],\n Matrix3d ric[]);\n\n bool solvePoseByPnP(Eigen::Matrix3d &R_initial, Eigen::Vector3d &P_initial,\n vector &pts2D, vector &pts3D);\n\n void removeBackShiftDepth(Eigen::Matrix3d marg_R, Eigen::Vector3d marg_P, Eigen::Matrix3d new_R,\n Eigen::Vector3d new_P);\n\n void removeBack();\n\n void removeFront(int frame_count);\n\n void removeOutlier(set &outlierIndex);\n\n void inputDepth(const cv::Mat &_depth_img);\n\n void seedFilling(int x, int y, unsigned short last_depth, unsigned short _object_id);\n\n list feature; // cl:Lists将元素按顺序储存在链表中. 与 向量(vectors)相比,\n // 它允许快速的插入和删除，但是随机访问却比较慢.\n int last_track_num{};\n\n cv::Mat depth_img;\n\nprivate:\n double compensatedParallax2(const FeaturePerId &it_per_id, int frame_count);\n\n const Matrix3d *Rs;\n Matrix3d ric[NUM_OF_CAM];\n};\n\n#endif\n" }, { "path": "vins_estimator/src/feature_tracker/ThreadPool.h", "content": "#ifndef THREAD_POOL_H\n#define THREAD_POOL_H\n\n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n\nclass ThreadPool\n{\npublic:\n ThreadPool(size_t);\n template \n auto enqueue(F &&f, Args &&...args) -> std::future::type>;\n ~ThreadPool();\n\n // void wait_workers();\n\nprivate:\n // need to keep track of threads so we can join them\n std::vector workers;\n // the task queue\n std::queue> tasks;\n\n // synchronization\n std::mutex queue_mutex;\n std::condition_variable condition;\n bool stop;\n};\n\n// the constructor just launches some amount of workers\ninline ThreadPool::ThreadPool(size_t threads) : stop(false)\n{\n for (size_t i = 0; i < threads; ++i)\n workers.emplace_back(\n [this]\n {\n for (;;)\n {\n std::function task;\n {\n std::unique_lock lock(this->queue_mutex);\n this->condition.wait(lock,\n [this] { return this->stop || !this->tasks.empty(); });\n if (this->stop && this->tasks.empty())\n return;\n task = std::move(this->tasks.front());\n this->tasks.pop();\n }\n\n task();\n }\n });\n}\n\n// add new work item to the pool\ntemplate \nauto ThreadPool::enqueue(F &&f, Args &&...args)\n -> std::future::type>\n{\n using return_type = typename std::result_of::type;\n\n auto task = std::make_shared>(\n std::bind(std::forward(f), std::forward(args)...));\n\n std::future res = task->get_future();\n {\n std::unique_lock lock(queue_mutex);\n\n // don't allow enqueueing after stopping the pool\n if (stop)\n throw std::runtime_error(\"enqueue on stopped ThreadPool\");\n\n tasks.emplace([task]() { (*task)(); });\n }\n condition.notify_one();\n return res;\n}\n\n// the destructor joins all threads\ninline ThreadPool::~ThreadPool()\n{\n {\n std::unique_lock lock(queue_mutex);\n stop = true;\n }\n condition.notify_all();\n for (std::thread &worker : workers)\n worker.join();\n}\n\n// inline void ThreadPool::wait_workers() {\n// for (std::thread &worker : workers)\n// worker.join();\n// }\n\n#endif\n" }, { "path": "vins_estimator/src/feature_tracker/feature_tracker.cpp", "content": "#include \"feature_tracker.h\"\n#include \n#include \n#include \n#include \n#include \n#include \n\nvoid reduceVector(vector &v, vector status)\n{\n int j = 0;\n for (int i = 0; i < int(v.size()); i++)\n if (status[i])\n v[j++] = v[i];\n v.resize(j);\n}\n\nvoid reduceVector(vector &v, vector status)\n{\n int j = 0;\n for (int i = 0; i < int(v.size()); i++)\n if (status[i])\n v[j++] = v[i];\n v.resize(j);\n}\n\nFeatureTracker::FeatureTracker()\n{\n p_fast_feature_detector = cv::FastFeatureDetector::create();\n n_id = 0;\n}\n\nvoid FeatureTracker::initGridsDetector()\n{\n pool = std::make_shared(NUM_THREADS);\n\n grids_detector_img = cv::Mat(ROW, COL, CV_8UC1, cv::Scalar(0));\n\n grid_height = (int)(ROW / NUM_GRID_ROWS);\n grid_width = (int)(COL / NUM_GRID_COLS);\n grid_res_height = ROW - (NUM_GRID_ROWS - 1) * grid_height;\n grid_res_width = COL - (NUM_GRID_COLS - 1) * grid_width;\n\n if (grids_rect.empty())\n {\n for (int i = 0; i < NUM_GRID_ROWS; i++)\n for (int j = 0; j < NUM_GRID_COLS; j++)\n {\n cv::Rect rect;\n if (i == 0)\n {\n if (j == 0)\n rect = cv::Rect(0, 0, grid_width + 3, grid_height + 3);\n else if (j > 0 && j < NUM_GRID_COLS - 1)\n rect = cv::Rect(j * grid_width - 3, 0, grid_width + 6, grid_height + 3);\n else\n rect = cv::Rect(j * grid_width - 3, 0, grid_res_width + 3, grid_height + 3);\n }\n else if (i > 0 && i < NUM_GRID_ROWS - 1)\n {\n if (j == 0)\n rect = cv::Rect(0, i * grid_height - 3, grid_width + 3, grid_height + 6);\n else if (j > 0 && j < NUM_GRID_COLS - 1)\n rect = cv::Rect(j * grid_width - 3, i * grid_height - 3, grid_width + 6,\n grid_height + 6);\n else\n rect = cv::Rect(j * grid_width - 3, i * grid_height - 3, grid_res_width + 3,\n grid_height + 6);\n }\n else\n {\n if (j == 0)\n rect =\n cv::Rect(0, i * grid_height - 3, grid_width + 3, grid_res_height + 3);\n else if (j > 0 && j < NUM_GRID_COLS - 1)\n rect = cv::Rect(j * grid_width - 3, i * grid_height - 3, grid_width + 6,\n grid_res_height + 3);\n else\n rect = cv::Rect(j * grid_width - 3, i * grid_height - 3, grid_res_width + 3,\n grid_res_height + 3);\n }\n grids_rect.emplace_back(rect);\n grids_track_num.emplace_back(0);\n grids_texture_status.emplace_back(true);\n }\n }\n\n grids_threshold = (int)(MAX_CNT / grids_rect.size());\n ROS_ASSERT_MSG(grids_threshold > 0,\n \"Too many grids! \\n'max_cnt'(%d) is supposed to be bigger than \"\n \"'num_grid_rows(%d)' x 'num_grid_cols(%d)' = %d!\\nPlease reduce the \"\n \"'num_grid_rows' or 'num_grid_cols'!\",\n MAX_CNT, NUM_GRID_ROWS, NUM_GRID_COLS, NUM_GRID_ROWS * NUM_GRID_COLS);\n}\n\nbool FeatureTracker::inBorder(const cv::Point2f &pt)\n{\n const int BORDER_SIZE = 1;\n int img_x = cvRound(pt.x);\n int img_y = cvRound(pt.y);\n return BORDER_SIZE <= img_x && img_x < COL - BORDER_SIZE && BORDER_SIZE <= img_y &&\n img_y < ROW - BORDER_SIZE;\n}\n\nstd::vector FeatureTracker::gridDetect(size_t grid_id)\n{\n // TicToc t_temp_ceil_fast;\n std::vector temp_keypts;\n p_fast_feature_detector->detect(forw_img(grids_rect[grid_id]), temp_keypts,\n mask(grids_rect[grid_id]));\n\n if (SHOW_TRACK)\n {\n forw_img(grids_rect[grid_id]).copyTo(grids_detector_img(grids_rect[grid_id]));\n cv::drawKeypoints(grids_detector_img(grids_rect[grid_id]), temp_keypts,\n grids_detector_img(grids_rect[grid_id]), cv::Scalar::all(255),\n cv::DrawMatchesFlags::DRAW_OVER_OUTIMG);\n }\n\n if (temp_keypts.empty())\n {\n grids_texture_status[grid_id] = false;\n return {};\n }\n else\n {\n size_t grid_num_to_add = grids_threshold - grids_track_num[grid_id] + 2;\n if (temp_keypts.size() <= grid_num_to_add)\n {\n for (auto &temp_keypt : temp_keypts)\n {\n temp_keypt.pt.x += grids_rect[grid_id].x;\n temp_keypt.pt.y += grids_rect[grid_id].y;\n }\n return temp_keypts;\n }\n std::vector keypts_to_add(grid_num_to_add);\n int min_response_id = 0;\n for (size_t j = 0; j < temp_keypts.size(); ++j)\n {\n if (grid_num_to_add > 0)\n {\n temp_keypts[j].pt.x += grids_rect[grid_id].x;\n temp_keypts[j].pt.y += grids_rect[grid_id].y;\n keypts_to_add[j] = temp_keypts[j];\n --grid_num_to_add;\n if (temp_keypts[j].response < keypts_to_add[min_response_id].response)\n {\n min_response_id = j;\n }\n }\n else if (temp_keypts[j].response > keypts_to_add[min_response_id].response)\n {\n temp_keypts[j].pt.x += grids_rect[grid_id].x;\n temp_keypts[j].pt.y += grids_rect[grid_id].y;\n keypts_to_add[min_response_id] = temp_keypts[j];\n for (size_t k = 0; k < keypts_to_add.size(); ++k)\n {\n if (keypts_to_add[k].response < keypts_to_add[min_response_id].response)\n {\n min_response_id = k;\n }\n }\n }\n }\n keypts_to_add.resize(keypts_to_add.size() - grid_num_to_add);\n return keypts_to_add;\n }\n // printf(\"detect grids_img feature costs: %fms\\n\",\n // t_temp_ceil_fast.toc());\n}\n\nvoid FeatureTracker::setMask()\n{\n if (FISHEYE)\n mask = fisheye_mask.clone();\n else\n mask = cv::Mat(ROW, COL, CV_8UC1, cv::Scalar(255));\n\n // prefer to keep features that are tracked for long time\n vector>> cnt_pts_id;\n\n for (unsigned int i = 0; i < forw_pts.size(); i++)\n cnt_pts_id.emplace_back(track_cnt[i], make_pair(forw_pts[i], ids[i]));\n\n sort(cnt_pts_id.begin(), cnt_pts_id.end(),\n [](const pair> &a, const pair> &b)\n { return a.first > b.first; });\n\n forw_pts.clear();\n ids.clear();\n track_cnt.clear();\n\n for (auto &it : cnt_pts_id)\n {\n if (mask.at(it.second.first) == 255)\n {\n forw_pts.push_back(it.second.first);\n ids.push_back(it.second.second);\n track_cnt.push_back(it.first);\n cv::circle(mask, it.second.first, MIN_DIST, 0, -1);\n }\n }\n for (auto &pt : unstable_pts)\n {\n cv::circle(mask, pt, MIN_DIST, 0, -1);\n }\n}\n\nvoid FeatureTracker::addPoints()\n{\n for (auto &p : n_pts)\n {\n forw_pts.push_back(p);\n ids.push_back(-1);\n track_cnt.push_back(1);\n }\n}\n\nvoid FeatureTracker::addPoints(vector &Keypts)\n{\n for (auto &Keypt : Keypts)\n {\n if (mask.at(Keypt.pt) == 255)\n {\n forw_pts.push_back(Keypt.pt);\n ids.push_back(-1);\n track_cnt.push_back(1);\n // cl: prevent close feature selected\n cv::circle(mask, Keypt.pt, MIN_DIST, 0, -1);\n }\n }\n}\n\nvoid FeatureTracker::addPoints(int n_max_cnt, vector &Keypts)\n{\n if (Keypts.empty())\n {\n return;\n }\n\n sort(Keypts.begin(), Keypts.end(),\n [](const cv::KeyPoint &a, const cv::KeyPoint &b) { return a.response > b.response; });\n\n int n_add = 0;\n for (auto &Keypt : Keypts)\n {\n if (mask.at(Keypt.pt) == 255)\n {\n forw_pts.push_back(Keypt.pt);\n ids.push_back(-1);\n track_cnt.push_back(1);\n cv::circle(mask, Keypt.pt, MIN_DIST, 0, -1); // cl: prevent close feature selected\n n_add++;\n if (n_add == n_max_cnt)\n {\n break;\n }\n }\n }\n}\n\nvoid FeatureTracker::readImage(const cv::Mat &_img, double _cur_time, const Matrix3d &_relative_R)\n{\n cv::Mat img;\n TicToc t_r;\n cur_time = _cur_time;\n // too dark or too bright: histogram\n if (EQUALIZE)\n {\n cv::Ptr clahe = cv::createCLAHE(3.0, cv::Size(8, 8));\n TicToc t_c;\n clahe->apply(_img, img);\n ROS_DEBUG(\"CLAHE costs: %fms\", t_c.toc());\n }\n else\n img = _img;\n\n if (forw_img.empty())\n {\n // curr_img<--->forw_img\n cur_img = forw_img = img;\n }\n else\n {\n forw_img = img;\n }\n\n forw_pts.clear();\n unstable_pts.clear();\n\n if (!cur_pts.empty())\n {\n TicToc t_o;\n vector status;\n vector err;\n\n if (USE_IMU)\n {\n predictPtsInNextFrame(_relative_R);\n forw_pts = predict_pts;\n cv::calcOpticalFlowPyrLK(\n cur_img, forw_img, cur_pts, forw_pts, status, err, cv::Size(21, 21), 1,\n cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 30, 0.01),\n cv::OPTFLOW_USE_INITIAL_FLOW);\n }\n else\n {\n cv::calcOpticalFlowPyrLK(cur_img, forw_img, cur_pts, forw_pts, status, err,\n cv::Size(21, 21), 3);\n }\n\n for (int i = 0; i < int(forw_pts.size()); i++)\n {\n if (!status[i] && inBorder(forw_pts[i]))\n {\n unstable_pts.push_back(forw_pts[i]);\n }\n else if (status[i] && !inBorder(forw_pts[i]))\n {\n status[i] = 0;\n }\n }\n\n reduceVector(cur_pts, status);\n reduceVector(forw_pts, status);\n reduceVector(ids, status);\n reduceVector(cur_un_pts, status);\n reduceVector(track_cnt, status);\n\n // ROS_DEBUG(\"temporal optical flow costs: %fms\", t_o.toc());\n // 光流准确率，时间输出\n static double OpticalFlow_time = 0;\n static int of_cnt_frame = 0;\n OpticalFlow_time += t_o.toc();\n of_cnt_frame++;\n ROS_DEBUG(\"average optical flow costs: %fms\\n\", OpticalFlow_time / (double)of_cnt_frame);\n // static double succ_num = 0;\n // static double total_num = 0;\n // for (size_t i = 0; i < status.size(); i++) {\n // if (status[i])\n // succ_num++;\n // }\n // total_num += status.size();\n // ROS_DEBUG(\"average success ratio: %f\\n\", succ_num / total_num);\n }\n\n for (auto &n : track_cnt)\n n++;\n\n if (PUB_THIS_FRAME)\n {\n // TicToc t_m;\n //对cur_pts和forw_pts做ransac剔除outlier.\n rejectWithF();\n setMask();\n // ROS_DEBUG(\"set mask costs %fms\", t_m.toc());\n\n TicToc t_t;\n int n_max_cnt = MAX_CNT - static_cast(forw_pts.size());\n if (n_max_cnt > 0)\n {\n if (mask.empty())\n cout << \"mask is empty \" << endl;\n if (mask.type() != CV_8UC1)\n cout << \"mask type wrong \" << endl;\n\n // TicToc t_grid_detect;\n\n for (auto &grid_track_num : grids_track_num)\n grid_track_num = 0;\n\n for (auto &forw_pt : forw_pts)\n {\n int col_id = (int)forw_pt.x / grid_width;\n int row_id = (int)forw_pt.y / grid_height;\n if (col_id == NUM_GRID_COLS)\n --col_id;\n if (row_id == NUM_GRID_ROWS)\n --row_id;\n ++grids_track_num[col_id + NUM_GRID_COLS * row_id];\n }\n\n std::vector grids_id;\n for (size_t i = 0; i < grids_rect.size(); ++i)\n {\n if (grids_track_num[i] < grids_threshold && grids_texture_status[i])\n {\n grids_id.emplace_back(i);\n }\n else\n {\n grids_texture_status[i] = true;\n }\n }\n\n std::vector>> grids_keypts;\n for (int &i : grids_id)\n {\n grids_keypts.emplace_back(\n pool->enqueue([this](int grid_id) { return gridDetect(grid_id); }, i));\n }\n\n // TicToc t_a;\n for (auto &&grid_keypts_asyn : grids_keypts)\n {\n std::vector &&grid_keypts = grid_keypts_asyn.get();\n addPoints(grid_keypts);\n }\n // std::vector Keypts;\n // p_fast_feature_detector->detect(forw_img, Keypts, mask);\n // addPoints(n_max_cnt, Keypts);\n\n // ROS_DEBUG(\"selectFeature costs: %fms\", t_a.toc());\n\n /* static double grid_detect_fast_time = 0;\n grid_detect_fast_time += t_grid_detect.toc();\n static int cnt_grid_frame = 0;\n cnt_grid_frame++;\n printf(\"average grids_img detect feature costs: %fms\\n\",\n grid_detect_fast_time / (double)cnt_grid_frame); */\n }\n\n static double detect_time = 0;\n static int detect_cnt_frame = 0;\n detect_time += t_t.toc();\n detect_cnt_frame++;\n ROS_DEBUG(\"average detect costs: %fms\\n\", detect_time / (double)detect_cnt_frame);\n // ROS_DEBUG(\"detect feature costs: %fms\", t_t.toc());\n }\n prev_un_pts = cur_un_pts;\n cur_img = forw_img;\n cur_pts = forw_pts;\n\n // 去畸变，投影至归一化平面，计算特征点速度(pixel/s)\n undistortedPoints();\n prev_time = cur_time;\n ROS_DEBUG(\"Process Image costs: %fms\", t_r.toc());\n}\n\nvoid FeatureTracker::rejectWithF()\n{\n if (forw_pts.size() >= 8)\n {\n TicToc t_f;\n vector un_cur_pts(cur_pts.size()), un_forw_pts(forw_pts.size());\n for (unsigned int i = 0; i < cur_pts.size(); i++)\n {\n Eigen::Vector3d tmp_p;\n m_camera->liftProjective(Eigen::Vector2d(cur_pts[i].x, cur_pts[i].y), tmp_p);\n tmp_p.x() = FOCAL_LENGTH * tmp_p.x() / tmp_p.z() + COL / 2.0;\n tmp_p.y() = FOCAL_LENGTH * tmp_p.y() / tmp_p.z() + ROW / 2.0;\n un_cur_pts[i] = cv::Point2f(tmp_p.x(), tmp_p.y());\n\n m_camera->liftProjective(Eigen::Vector2d(forw_pts[i].x, forw_pts[i].y), tmp_p);\n tmp_p.x() = FOCAL_LENGTH * tmp_p.x() / tmp_p.z() + COL / 2.0;\n tmp_p.y() = FOCAL_LENGTH * tmp_p.y() / tmp_p.z() + ROW / 2.0;\n un_forw_pts[i] = cv::Point2f(tmp_p.x(), tmp_p.y());\n }\n\n vector status;\n cv::findFundamentalMat(un_cur_pts, un_forw_pts, cv::FM_RANSAC, F_THRESHOLD, 0.99, status);\n int size_a = cur_pts.size();\n reduceVector(cur_pts, status);\n reduceVector(forw_pts, status);\n reduceVector(cur_un_pts, status);\n reduceVector(ids, status);\n reduceVector(track_cnt, status);\n ROS_DEBUG(\"FM ransac: %d -> %lu: %f\", size_a, forw_pts.size(),\n 1.0 * forw_pts.size() / size_a);\n ROS_DEBUG(\"FM ransac costs: %fms\", t_f.toc());\n }\n}\n\nEigen::Vector3d FeatureTracker::get3dPt(const cv::Mat &depth, const cv::Point2f &pt)\n{\n Eigen::Vector3d tmp_P;\n m_camera->liftProjective(Eigen::Vector2d(pt.x, pt.y), tmp_P);\n Eigen::Vector3d P =\n tmp_P.normalized() * (((int)depth.at(round(pt.y), round(pt.x))) / 1000.0);\n\n return P;\n}\n\nbool FeatureTracker::updateID(unsigned int i)\n{\n if (i < ids.size())\n {\n if (ids[i] == -1)\n ids[i] = n_id++;\n return true;\n }\n else\n return false;\n}\n\nvoid FeatureTracker::readIntrinsicParameter(const string &calib_file)\n{\n ROS_DEBUG(\"reading paramerter of camera %s\", calib_file.c_str());\n m_camera = CameraFactory::instance()->generateCameraFromYamlFile(calib_file);\n}\n\nvoid FeatureTracker::showUndistortion(const string &name)\n{\n cv::Mat undistortedImg(ROW + 600, COL + 600, CV_8UC1, cv::Scalar(0));\n vector distortedp, undistortedp;\n for (int i = 0; i < COL; i++)\n for (int j = 0; j < ROW; j++)\n {\n Eigen::Vector2d a(i, j);\n Eigen::Vector3d b;\n m_camera->liftProjective(a, b);\n distortedp.push_back(a);\n undistortedp.emplace_back(b.x() / b.z(), b.y() / b.z());\n // printf(\"%f,%f->%f,%f,%f\\n)\\n\", a.x(), a.y(), b.x(), b.y(), b.z());\n }\n for (int i = 0; i < int(undistortedp.size()); i++)\n {\n cv::Mat pp(3, 1, CV_32FC1);\n pp.at(0, 0) = undistortedp[i].x() * FOCAL_LENGTH + COL / 2;\n pp.at(1, 0) = undistortedp[i].y() * FOCAL_LENGTH + ROW / 2;\n pp.at(2, 0) = 1.0;\n // cout << trackerData[0].K << endl;\n // printf(\"%lf %lf\\n\", p.at(1, 0), p.at(0, 0));\n // printf(\"%lf %lf\\n\", pp.at(1, 0), pp.at(0, 0));\n if (pp.at(1, 0) + 300 >= 0 && pp.at(1, 0) + 300 < ROW + 600 &&\n pp.at(0, 0) + 300 >= 0 && pp.at(0, 0) + 300 < COL + 600)\n {\n undistortedImg.at(pp.at(1, 0) + 300, pp.at(0, 0) + 300) =\n cur_img.at(distortedp[i].y(), distortedp[i].x());\n }\n else\n {\n // ROS_ERROR(\"(%f %f) -> (%f %f)\", distortedp[i].y, distortedp[i].x,\n // pp.at(1, 0), pp.at(0, 0));\n }\n }\n cv::imshow(name, undistortedImg);\n cv::waitKey(0);\n}\n\nvoid FeatureTracker::undistortedPoints()\n{\n cur_un_pts.clear();\n cur_un_pts_map.clear();\n // cv::undistortPoints(cur_pts, un_pts, K, cv::Mat());\n for (unsigned int i = 0; i < cur_pts.size(); i++)\n {\n Eigen::Vector2d a(cur_pts[i].x, cur_pts[i].y);\n Eigen::Vector3d b;\n // https://github.com/HKUST-Aerial-Robotics/VINS-Mono/blob/0d280936e441ebb782bf8855d86e13999a22da63/camera_model/src/camera_models/PinholeCamera.cc\n // brief Lifts a point from the image plane to its projective ray\n m_camera->liftProjective(a, b);\n // 特征点在相机坐标系的归一化坐标\n cur_un_pts.emplace_back(b.x() / b.z(), b.y() / b.z());\n cur_un_pts_map.insert(make_pair(ids[i], cv::Point2f(b.x() / b.z(), b.y() / b.z())));\n // printf(\"cur pts id %d %f %f\", ids[i], cur_un_pts[i].x, cur_un_pts[i].y);\n }\n // caculate points velocity\n if (!prev_un_pts_map.empty())\n {\n double dt = cur_time - prev_time;\n pts_velocity.clear();\n for (unsigned int i = 0; i < cur_un_pts.size(); i++)\n {\n if (ids[i] != -1)\n {\n std::map::iterator it;\n it = prev_un_pts_map.find(ids[i]);\n if (it != prev_un_pts_map.end())\n {\n double v_x = (cur_un_pts[i].x - it->second.x) / dt;\n double v_y = (cur_un_pts[i].y - it->second.y) / dt;\n pts_velocity.emplace_back(v_x, v_y);\n }\n else\n pts_velocity.emplace_back(0, 0);\n }\n else\n {\n pts_velocity.emplace_back(0, 0);\n }\n }\n }\n else\n {\n for (unsigned int i = 0; i < cur_pts.size(); i++)\n {\n pts_velocity.emplace_back(0, 0);\n }\n }\n prev_un_pts_map = cur_un_pts_map;\n}\n\nvoid FeatureTracker::predictPtsInNextFrame(const Matrix3d &_relative_R)\n{\n predict_pts.resize(cur_pts.size());\n for (unsigned int i = 0; i < cur_pts.size(); ++i)\n {\n Eigen::Vector3d tmp_P;\n m_camera->liftProjective(Eigen::Vector2d(cur_pts[i].x, cur_pts[i].y), tmp_P);\n Eigen::Vector3d predict_P = _relative_R * tmp_P;\n Eigen::Vector2d tmp_p;\n m_camera->spaceToPlane(predict_P, tmp_p);\n predict_pts[i].x = tmp_p.x();\n predict_pts[i].y = tmp_p.y();\n }\n}" }, { "path": "vins_estimator/src/feature_tracker/feature_tracker.h", "content": "#pragma once\n\n#include \n#include \n#include \n#include \n#include \n\n#include \n#include \n\n#include \"camodocal/camera_models/CameraFactory.h\"\n#include \"camodocal/camera_models/CataCamera.h\"\n#include \"camodocal/camera_models/PinholeCamera.h\"\n\n#include \"../utility/parameters.h\"\n#include \"../utility/tic_toc.h\"\n\n#include \"ThreadPool.h\"\n\nusing namespace std;\nusing namespace camodocal;\nusing namespace Eigen;\n\nbool inBorder(const cv::Point2f &pt);\n\nvoid reduceVector(vector &v, vector status);\n\nvoid reduceVector(vector &v, vector status);\n\nclass FeatureTracker\n{\npublic:\n FeatureTracker();\n\n void readImage(const cv::Mat &_img, double _cur_time,\n const Matrix3d &_relative_R = Matrix3d::Identity());\n\n void setMask();\n\n void addPoints();\n\n void addPoints(vector &Keypts);\n \n void addPoints(int n_max_cnt, vector &Keypts);\n\n bool updateID(unsigned int i);\n\n void readIntrinsicParameter(const string &calib_file);\n\n void showUndistortion(const string &name);\n\n void rejectWithF();\n\n void undistortedPoints();\n\n void predictPtsInNextFrame(const Matrix3d &_relative_R);\n\n Eigen::Vector3d get3dPt(const cv::Mat &_depth, const cv::Point2f &pt);\n\n void initGridsDetector();\n\n static bool inBorder(const cv::Point2f &pt);\n\n std::vector gridDetect(size_t grid_id);\n\n cv::Mat mask;\n cv::Mat fisheye_mask;\n cv::Mat cur_img, forw_img;\n vector n_pts;\n vector cur_pts, forw_pts, predict_pts, unstable_pts;\n vector prev_un_pts, cur_un_pts;\n vector pts_velocity;\n vector ids;\n vector track_cnt;\n map cur_un_pts_map;\n map prev_un_pts_map;\n camodocal::CameraPtr m_camera;\n double cur_time{};\n double prev_time{};\n\n int n_id;\n cv::Ptr p_fast_feature_detector;\n\n std::shared_ptr pool;\n\n std::vector grids_rect;\n std::vector grids_track_num;\n std::vector grids_texture_status; // true: abundant texture grid; false:\n // textureless grid\n int grid_height{};\n int grid_width{};\n int grid_res_height{};\n int grid_res_width{};\n int grids_threshold{};\n cv::Mat grids_detector_img;\n};\n" }, { "path": "vins_estimator/src/initial/initial_aligment.cpp", "content": "#include \"initial_alignment.h\"\n\nvoid solveGyroscopeBias(map &all_image_frame, Vector3d *Bgs)\n{\n Matrix3d A;\n Vector3d b;\n Vector3d delta_bg;\n A.setZero();\n b.setZero();\n map::iterator frame_i;\n map::iterator frame_j;\n for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end(); frame_i++)\n {\n frame_j = next(frame_i);\n MatrixXd tmp_A(3, 3);\n tmp_A.setZero();\n VectorXd tmp_b(3);\n tmp_b.setZero();\n Eigen::Quaterniond q_ij(frame_i->second.R.transpose() * frame_j->second.R);\n tmp_A = frame_j->second.pre_integration->jacobian.template block<3, 3>(O_R, O_BG);\n tmp_b = 2 * (frame_j->second.pre_integration->delta_q.inverse() * q_ij).vec();\n A += tmp_A.transpose() * tmp_A;\n b += tmp_A.transpose() * tmp_b;\n }\n delta_bg = A.ldlt().solve(b);\n ROS_WARN_STREAM(\"gyroscope bias initial calibration \" << delta_bg.transpose());\n\n for (int i = 0; i <= WINDOW_SIZE; i++)\n Bgs[i] += delta_bg;\n\n for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end(); frame_i++)\n {\n frame_j = next(frame_i);\n frame_j->second.pre_integration->repropagate(Vector3d::Zero(), Bgs[0]);\n }\n}\n\n/*void solveGyroscopeBias(map &all_image_frame, Vector3d*\nBgs)\n{\n Matrix3d A;\n Vector3d b;\n Vector3d delta_bg;\n A.setZero();\n b.setZero();\n map::iterator frame_i;\n map::iterator frame_j;\n int cnt = 0;\n for (frame_i = all_image_frame.end(); cntsecond.R.transpose() *\nframe_j->second.R); tmp_A = frame_j->second.pre_integration->jacobian.template\nblock<3, 3>(O_R, O_BG); tmp_b = 2 *\n(frame_j->second.pre_integration->delta_q.inverse() * q_ij).vec(); A +=\ntmp_A.transpose() * tmp_A; b += tmp_A.transpose() * tmp_b;\n\n }\n delta_bg = A.ldlt().solve(b);\n ROS_WARN_STREAM(\"gyroscope bias initial calibration \" <<\ndelta_bg.transpose());\n\n for (int i = 0; i <= WINDOW_SIZE; i++)\n Bgs[i] += delta_bg;\n\n for (frame_i = all_image_frame.begin(); next(frame_i) !=\nall_image_frame.end( ); frame_i++)\n {\n frame_j = next(frame_i);\n frame_j->second.pre_integration->repropagate(Vector3d::Zero(), Bgs[0]);\n }\n}*/\n\nMatrixXd TangentBasis(Vector3d &g0)\n{\n Vector3d b, c;\n Vector3d a = g0.normalized();\n Vector3d tmp(0, 0, 1);\n if (a == tmp)\n tmp << 1, 0, 0;\n b = (tmp - a * (a.transpose() * tmp)).normalized();\n c = a.cross(b);\n MatrixXd bc(3, 2);\n bc.block<3, 1>(0, 0) = b;\n bc.block<3, 1>(0, 1) = c;\n return bc;\n}\n\nvoid RefineGravity(map &all_image_frame, Vector3d &g, VectorXd &x)\n{\n Vector3d g0 = g.normalized() * G.norm();\n Vector3d lx, ly;\n // VectorXd x;\n int all_frame_count = all_image_frame.size();\n int n_state = all_frame_count * 3 + 2 + 1;\n\n MatrixXd A{n_state, n_state};\n A.setZero();\n VectorXd b{n_state};\n b.setZero();\n\n map::iterator frame_i;\n map::iterator frame_j;\n for (int k = 0; k < 4; k++)\n {\n MatrixXd lxly(3, 2);\n lxly = TangentBasis(g0);\n int i = 0;\n for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end();\n frame_i++, i++)\n {\n frame_j = next(frame_i);\n\n MatrixXd tmp_A(6, 9);\n tmp_A.setZero();\n VectorXd tmp_b(6);\n tmp_b.setZero();\n\n double dt = frame_j->second.pre_integration->sum_dt;\n\n tmp_A.block<3, 3>(0, 0) = -dt * Matrix3d::Identity();\n tmp_A.block<3, 2>(0, 6) =\n frame_i->second.R.transpose() * dt * dt / 2 * Matrix3d::Identity() * lxly;\n tmp_A.block<3, 1>(0, 8) =\n frame_i->second.R.transpose() * (frame_j->second.T - frame_i->second.T) / 100.0;\n tmp_b.block<3, 1>(0, 0) = frame_j->second.pre_integration->delta_p +\n frame_i->second.R.transpose() * frame_j->second.R * TIC[0] -\n TIC[0] - frame_i->second.R.transpose() * dt * dt / 2 * g0;\n\n tmp_A.block<3, 3>(3, 0) = -Matrix3d::Identity();\n tmp_A.block<3, 3>(3, 3) = frame_i->second.R.transpose() * frame_j->second.R;\n tmp_A.block<3, 2>(3, 6) =\n frame_i->second.R.transpose() * dt * Matrix3d::Identity() * lxly;\n tmp_b.block<3, 1>(3, 0) =\n frame_j->second.pre_integration->delta_v -\n frame_i->second.R.transpose() * dt * Matrix3d::Identity() * g0;\n\n Matrix cov_inv = Matrix::Zero();\n // cov.block<6, 6>(0, 0) = IMU_cov[i + 1];\n // MatrixXd cov_inv = cov.inverse();\n cov_inv.setIdentity();\n\n MatrixXd r_A = tmp_A.transpose() * cov_inv * tmp_A;\n VectorXd r_b = tmp_A.transpose() * cov_inv * tmp_b;\n\n A.block<6, 6>(i * 3, i * 3) += r_A.topLeftCorner<6, 6>();\n b.segment<6>(i * 3) += r_b.head<6>();\n\n A.bottomRightCorner<3, 3>() += r_A.bottomRightCorner<3, 3>();\n b.tail<3>() += r_b.tail<3>();\n\n A.block<6, 3>(i * 3, n_state - 3) += r_A.topRightCorner<6, 3>();\n A.block<3, 6>(n_state - 3, i * 3) += r_A.bottomLeftCorner<3, 6>();\n }\n A = A * 1000.0;\n b = b * 1000.0;\n x = A.ldlt().solve(b);\n VectorXd dg = x.segment<2>(n_state - 3);\n g0 = (g0 + lxly * dg).normalized() * G.norm();\n // double s = x(n_state - 1);\n }\n g = g0;\n}\n\nvoid RefineGravityWithDepth(map &all_image_frame, Vector3d &g, VectorXd &x)\n{\n Vector3d g0 = g.normalized() * G.norm();\n Vector3d lx, ly;\n // VectorXd x;\n int all_frame_count = all_image_frame.size();\n int n_state = all_frame_count * 3 + 2;\n\n MatrixXd A{n_state, n_state};\n A.setZero();\n VectorXd b{n_state};\n b.setZero();\n\n map::iterator frame_i;\n map::iterator frame_j;\n for (int k = 0; k < 4; k++)\n {\n MatrixXd lxly(3, 2);\n lxly = TangentBasis(g0);\n int i = 0;\n for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end();\n frame_i++, i++)\n {\n frame_j = next(frame_i);\n\n MatrixXd tmp_A(6, 8);\n tmp_A.setZero();\n VectorXd tmp_b(6);\n tmp_b.setZero();\n\n double dt = frame_j->second.pre_integration->sum_dt;\n\n tmp_A.block<3, 3>(0, 0) = -dt * Matrix3d::Identity();\n tmp_A.block<3, 2>(0, 6) =\n frame_i->second.R.transpose() * dt * dt / 2 * Matrix3d::Identity() * lxly;\n tmp_b.block<3, 1>(0, 0) =\n frame_j->second.pre_integration->delta_p +\n frame_i->second.R.transpose() * frame_j->second.R * TIC[0] - TIC[0] -\n frame_i->second.R.transpose() * dt * dt / 2 * g0 -\n frame_i->second.R.transpose() * (frame_j->second.T - frame_i->second.T);\n\n tmp_A.block<3, 3>(3, 0) = -Matrix3d::Identity();\n tmp_A.block<3, 3>(3, 3) = frame_i->second.R.transpose() * frame_j->second.R;\n tmp_A.block<3, 2>(3, 6) =\n frame_i->second.R.transpose() * dt * Matrix3d::Identity() * lxly;\n tmp_b.block<3, 1>(3, 0) =\n frame_j->second.pre_integration->delta_v -\n frame_i->second.R.transpose() * dt * Matrix3d::Identity() * g0;\n\n Matrix cov_inv = Matrix::Zero();\n // cov.block<6, 6>(0, 0) = IMU_cov[i + 1];\n // MatrixXd cov_inv = cov.inverse();\n cov_inv.setIdentity();\n\n MatrixXd r_A = tmp_A.transpose() * cov_inv * tmp_A;\n VectorXd r_b = tmp_A.transpose() * cov_inv * tmp_b;\n\n A.block<6, 6>(i * 3, i * 3) += r_A.topLeftCorner<6, 6>();\n b.segment<6>(i * 3) += r_b.head<6>();\n\n A.bottomRightCorner<2, 2>() += r_A.bottomRightCorner<2, 2>();\n b.tail<2>() += r_b.tail<2>();\n\n A.block<6, 2>(i * 3, n_state - 2) += r_A.topRightCorner<6, 2>();\n A.block<2, 6>(n_state - 2, i * 3) += r_A.bottomLeftCorner<2, 6>();\n }\n A = A * 1000.0;\n b = b * 1000.0;\n x = A.ldlt().solve(b);\n VectorXd dg = x.segment<2>(n_state - 2);\n g0 = (g0 + lxly * dg).normalized() * G.norm();\n // double s = x(n_state - 1);\n }\n g = g0;\n}\n\nbool LinearAlignment(map &all_image_frame, Vector3d &g, VectorXd &x)\n{\n int all_frame_count = all_image_frame.size();\n int n_state = all_frame_count * 3 + 3 + 1;\n\n MatrixXd A{n_state, n_state};\n A.setZero();\n VectorXd b{n_state};\n b.setZero();\n\n map::iterator frame_i;\n map::iterator frame_j;\n int i = 0;\n for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end(); frame_i++, i++)\n {\n frame_j = next(frame_i);\n\n MatrixXd tmp_A(6, 10);\n tmp_A.setZero();\n VectorXd tmp_b(6);\n tmp_b.setZero();\n\n double dt = frame_j->second.pre_integration->sum_dt;\n\n tmp_A.block<3, 3>(0, 0) = -dt * Matrix3d::Identity();\n tmp_A.block<3, 3>(0, 6) =\n frame_i->second.R.transpose() * dt * dt / 2 * Matrix3d::Identity();\n tmp_A.block<3, 1>(0, 9) =\n frame_i->second.R.transpose() * (frame_j->second.T - frame_i->second.T) / 100.0;\n tmp_b.block<3, 1>(0, 0) = frame_j->second.pre_integration->delta_p +\n frame_i->second.R.transpose() * frame_j->second.R * TIC[0] -\n TIC[0];\n // cout << \"delta_p \" <<\n // frame_j->second.pre_integration->delta_p.transpose() << endl;\n tmp_A.block<3, 3>(3, 0) = -Matrix3d::Identity();\n tmp_A.block<3, 3>(3, 3) = frame_i->second.R.transpose() * frame_j->second.R;\n tmp_A.block<3, 3>(3, 6) = frame_i->second.R.transpose() * dt * Matrix3d::Identity();\n tmp_b.block<3, 1>(3, 0) = frame_j->second.pre_integration->delta_v;\n // cout << \"delta_v \" <<\n // frame_j->second.pre_integration->delta_v.transpose() << endl;\n\n Matrix cov_inv = Matrix::Zero();\n // cov.block<6, 6>(0, 0) = IMU_cov[i + 1];\n // MatrixXd cov_inv = cov.inverse();\n cov_inv.setIdentity();\n\n MatrixXd r_A = tmp_A.transpose() * cov_inv * tmp_A;\n VectorXd r_b = tmp_A.transpose() * cov_inv * tmp_b;\n\n A.block<6, 6>(i * 3, i * 3) += r_A.topLeftCorner<6, 6>();\n b.segment<6>(i * 3) += r_b.head<6>();\n\n A.bottomRightCorner<4, 4>() += r_A.bottomRightCorner<4, 4>();\n b.tail<4>() += r_b.tail<4>();\n\n A.block<6, 4>(i * 3, n_state - 4) += r_A.topRightCorner<6, 4>();\n A.block<4, 6>(n_state - 4, i * 3) += r_A.bottomLeftCorner<4, 6>();\n }\n A = A * 1000.0;\n b = b * 1000.0;\n x = A.ldlt().solve(b);\n double s = x(n_state - 1) / 100.0;\n ROS_DEBUG(\"estimated scale: %f\", s);\n g = x.segment<3>(n_state - 4);\n ROS_DEBUG_STREAM(\" result g \" << g.norm() << \" \" << g.transpose());\n if (fabs(g.norm() - G.norm()) > 1.0 || s < 0)\n {\n return false;\n }\n\n RefineGravity(all_image_frame, g, x);\n s = (x.tail<1>())(0) / 100.0;\n (x.tail<1>())(0) = s;\n ROS_DEBUG_STREAM(\" refine \" << g.norm() << \" \" << g.transpose());\n if (s < 0.0)\n return false;\n else\n return true;\n}\n\n/**\n * @brief 计算重力加速度和速度\n * @optional 速度、重力向量和尺度初始化Paper -> V-B-2\n * 相邻帧之间的位置和速度与IMU预积分出来的位置和速度对齐，求解最小二乘\n * 重力细化 -> Paper V-B-3\n * @param[in] all_image_frame\n * 所有图像帧构成的map,图像帧保存了位姿，预积分量和关于角点的信息\n * @param[out] g 重力加速度\n * @param[out] x 待优化变量，窗口中每帧的速度V[0:n]、重力g\n * @return void\n */\nbool LinearAlignmentWithDepth(map &all_image_frame, Vector3d &g, VectorXd &x)\n{\n int all_frame_count = all_image_frame.size();\n int n_state = all_frame_count * 3 + 3; // no scale now\n\n MatrixXd A{n_state, n_state};\n A.setZero();\n VectorXd b{n_state};\n b.setZero();\n\n map::iterator frame_i;\n map::iterator frame_j;\n int i = 0;\n for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end(); frame_i++, i++)\n {\n frame_j = next(frame_i);\n\n MatrixXd tmp_A(6, 9); // no scale now\n tmp_A.setZero();\n VectorXd tmp_b(6);\n tmp_b.setZero();\n\n double dt = frame_j->second.pre_integration->sum_dt;\n\n tmp_A.block<3, 3>(0, 0) = -dt * Matrix3d::Identity();\n tmp_A.block<3, 3>(0, 6) =\n frame_i->second.R.transpose() * dt * dt / 2 * Matrix3d::Identity();\n tmp_b.block<3, 1>(0, 0) =\n frame_j->second.pre_integration->delta_p +\n frame_i->second.R.transpose() * frame_j->second.R * TIC[0] - TIC[0] -\n frame_i->second.R.transpose() * (frame_j->second.T - frame_i->second.T);\n tmp_A.block<3, 3>(3, 0) = -Matrix3d::Identity();\n tmp_A.block<3, 3>(3, 3) = frame_i->second.R.transpose() * frame_j->second.R;\n tmp_A.block<3, 3>(3, 6) = frame_i->second.R.transpose() * dt * Matrix3d::Identity();\n tmp_b.block<3, 1>(3, 0) = frame_j->second.pre_integration->delta_v;\n\n Matrix cov_inv = Matrix::Zero();\n\n cov_inv.setIdentity();\n\n MatrixXd r_A = tmp_A.transpose() * cov_inv * tmp_A;\n VectorXd r_b = tmp_A.transpose() * cov_inv * tmp_b;\n\n A.block<6, 6>(i * 3, i * 3) += r_A.topLeftCorner<6, 6>();\n b.segment<6>(i * 3) += r_b.head<6>();\n\n A.bottomRightCorner<3, 3>() += r_A.bottomRightCorner<3, 3>();\n b.tail<3>() += r_b.tail<3>();\n\n A.block<6, 3>(i * 3, n_state - 3) += r_A.topRightCorner<6, 3>();\n A.block<3, 6>(n_state - 3, i * 3) += r_A.bottomLeftCorner<3, 6>();\n }\n A = A * 1000.0;\n b = b * 1000.0;\n x = A.ldlt().solve(b);\n\n g = x.segment<3>(n_state - 3);\n ROS_DEBUG_STREAM(\" result g \" << g.norm() << \" \" << g.transpose());\n if (fabs(g.norm() - G.norm()) > 1.0)\n {\n return false;\n }\n\n RefineGravityWithDepth(all_image_frame, g, x);\n\n ROS_DEBUG_STREAM(\" refine \" << g.norm() << \" \" << g.transpose());\n\n return true;\n}\n\nbool LinearAlignmentWithDepthGravity(map &all_image_frame, Vector3d &g,\n VectorXd &x)\n{\n int all_frame_count = all_image_frame.size();\n int n_state = all_frame_count * 3; // no scale now\n\n MatrixXd A{n_state, n_state};\n A.setZero();\n VectorXd b{n_state};\n b.setZero();\n\n map::iterator frame_i;\n map::iterator frame_j;\n int i = 0;\n for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end(); frame_i++, i++)\n {\n frame_j = next(frame_i);\n\n MatrixXd tmp_A(6, 6); // no scale now\n tmp_A.setZero();\n VectorXd tmp_b(6);\n tmp_b.setZero();\n\n double dt = frame_j->second.pre_integration->sum_dt;\n\n tmp_A.block<3, 3>(0, 0) = -dt * Matrix3d::Identity();\n // G\n // tmp_A.block<3, 3>(0, 6) =\n // frame_i->second.R.transpose() * dt * dt / 2 * Matrix3d::Identity();\n tmp_b.block<3, 1>(0, 0) =\n frame_j->second.pre_integration->delta_p +\n frame_i->second.R.transpose() * frame_j->second.R * TIC[0] - TIC[0] -\n frame_i->second.R.transpose() * (frame_j->second.T - frame_i->second.T) -\n frame_i->second.R.transpose() * dt * dt / 2 * g;\n tmp_A.block<3, 3>(3, 0) = -Matrix3d::Identity();\n tmp_A.block<3, 3>(3, 3) = frame_i->second.R.transpose() * frame_j->second.R;\n // G\n // tmp_A.block<3, 3>(3, 6) =\n // frame_i->second.R.transpose() * dt * Matrix3d::Identity();\n tmp_b.block<3, 1>(3, 0) =\n frame_j->second.pre_integration->delta_v - frame_i->second.R.transpose() * dt * g;\n\n Matrix cov_inv = Matrix::Zero();\n\n cov_inv.setIdentity();\n\n MatrixXd r_A = tmp_A.transpose() * cov_inv * tmp_A;\n VectorXd r_b = tmp_A.transpose() * cov_inv * tmp_b;\n\n A.block<6, 6>(i * 3, i * 3) += r_A.topLeftCorner<6, 6>();\n b.segment<6>(i * 3) += r_b.head<6>();\n\n // G\n // A.bottomRightCorner<3, 3>() += r_A.bottomRightCorner<3, 3>();\n // b.tail<3>() += r_b.tail<3>();\n\n // A.block<6, 3>(i * 3, n_state - 3) += r_A.topRightCorner<6, 3>();\n // A.block<3, 6>(n_state - 3, i * 3) += r_A.bottomLeftCorner<3, 6>();\n }\n A = A * 1000.0;\n b = b * 1000.0;\n x = A.ldlt().solve(b);\n\n // g = x.segment<3>(n_state - 3);\n // ROS_DEBUG_STREAM(\" result g \" << g.norm() << \" \" << g.transpose());\n // if (fabs(g.norm() - G.norm()) > 1.0) {\n // return false;\n // }\n\n // RefineGravityWithDepth(all_image_frame, g, x);\n\n // ROS_DEBUG_STREAM(\" refine \" << g.norm() << \" \" << g.transpose());\n\n return true;\n}\n\nbool VisualIMUAlignment(map &all_image_frame, Vector3d *Bgs, Vector3d &g,\n VectorXd &x)\n{\n solveGyroscopeBias(all_image_frame, Bgs);\n\n if (LinearAlignmentWithDepth(all_image_frame, g, x))\n return true;\n else\n return false;\n}\n" }, { "path": "vins_estimator/src/initial/initial_alignment.h", "content": "#pragma once\n#include \"../factor/imu_factor.h\"\n#include \"../feature_manager/feature_manager.h\"\n#include \"../utility/utility.h\"\n#include \n#include \n#include \n#include \n\nusing namespace Eigen;\nusing namespace std;\n\nclass ImageFrame\n{\npublic:\n ImageFrame(){};\n ImageFrame(const map> &_points, double _t)\n : t{_t}, is_key_frame{false}\n {\n points = _points;\n };\n map> points;\n double t;\n Matrix3d R;\n Vector3d T;\n IntegrationBase *pre_integration;\n bool is_key_frame;\n};\n\nbool VisualIMUAlignment(map &all_image_frame, Vector3d *Bgs, Vector3d &g,\n VectorXd &x);\nbool LinearAlignmentWithDepth(map &all_image_frame, Vector3d &g, VectorXd &x);\nbool LinearAlignmentWithDepthGravity(map &all_image_frame, Vector3d &g,\n VectorXd &x);\nvoid solveGyroscopeBias(map &all_image_frame, Vector3d *Bgs);\n" }, { "path": "vins_estimator/src/initial/initial_ex_rotation.cpp", "content": "#include \"initial_ex_rotation.h\"\n\nInitialEXRotation::InitialEXRotation()\n{\n frame_count = 0;\n Rc.push_back(Matrix3d::Identity());\n Rc_g.push_back(Matrix3d::Identity());\n Rimu.push_back(Matrix3d::Identity());\n ric = Matrix3d::Identity();\n}\n\nbool InitialEXRotation::CalibrationExRotation(vector> corres,\n Quaterniond delta_q_imu, Matrix3d &calib_ric_result)\n{\n frame_count++;\n Rc.push_back(solveRelativeR(corres));\n Rimu.push_back(delta_q_imu.toRotationMatrix());\n Rc_g.push_back(ric.inverse() * delta_q_imu * ric);\n\n Eigen::MatrixXd A(frame_count * 4, 4);\n A.setZero();\n int sum_ok = 0;\n for (int i = 1; i <= frame_count; i++)\n {\n Quaterniond r1(Rc[i]);\n Quaterniond r2(Rc_g[i]);\n\n double angular_distance = 180 / M_PI * r1.angularDistance(r2);\n ROS_DEBUG(\"%d %f\", i, angular_distance);\n\n double huber = angular_distance > 5.0 ? 5.0 / angular_distance : 1.0;\n ++sum_ok;\n Matrix4d L, R;\n\n double w = Quaterniond(Rc[i]).w();\n Vector3d q = Quaterniond(Rc[i]).vec();\n L.block<3, 3>(0, 0) = w * Matrix3d::Identity() + Utility::skewSymmetric(q);\n L.block<3, 1>(0, 3) = q;\n L.block<1, 3>(3, 0) = -q.transpose();\n L(3, 3) = w;\n\n Quaterniond R_ij(Rimu[i]);\n w = R_ij.w();\n q = R_ij.vec();\n R.block<3, 3>(0, 0) = w * Matrix3d::Identity() - Utility::skewSymmetric(q);\n R.block<3, 1>(0, 3) = q;\n R.block<1, 3>(3, 0) = -q.transpose();\n R(3, 3) = w;\n\n A.block<4, 4>((i - 1) * 4, 0) = huber * (L - R);\n }\n\n JacobiSVD svd(A, ComputeFullU | ComputeFullV);\n Matrix x = svd.matrixV().col(3);\n Quaterniond estimated_R(x);\n ric = estimated_R.toRotationMatrix().inverse();\n // cout << svd.singularValues().transpose() << endl;\n // cout << ric << endl;\n Vector3d ric_cov;\n ric_cov = svd.singularValues().tail<3>();\n if (frame_count >= WINDOW_SIZE && ric_cov(1) > 0.25)\n {\n calib_ric_result = ric;\n return true;\n }\n else\n return false;\n}\n\nMatrix3d InitialEXRotation::solveRelativeR(const vector> &corres)\n{\n if (corres.size() >= 9)\n {\n vector ll, rr;\n for (int i = 0; i < int(corres.size()); i++)\n {\n ll.push_back(cv::Point2f(corres[i].first(0), corres[i].first(1)));\n rr.push_back(cv::Point2f(corres[i].second(0), corres[i].second(1)));\n }\n cv::Mat E = cv::findFundamentalMat(ll, rr);\n cv::Mat_ R1, R2, t1, t2;\n decomposeE(E, R1, R2, t1, t2);\n\n if (determinant(R1) + 1.0 < 1e-09)\n {\n E = -E;\n decomposeE(E, R1, R2, t1, t2);\n }\n double ratio1 = max(testTriangulation(ll, rr, R1, t1), testTriangulation(ll, rr, R1, t2));\n double ratio2 = max(testTriangulation(ll, rr, R2, t1), testTriangulation(ll, rr, R2, t2));\n cv::Mat_ ans_R_cv = ratio1 > ratio2 ? R1 : R2;\n\n Matrix3d ans_R_eigen;\n for (int i = 0; i < 3; i++)\n for (int j = 0; j < 3; j++)\n ans_R_eigen(j, i) = ans_R_cv(i, j);\n return ans_R_eigen;\n }\n return Matrix3d::Identity();\n}\n\ndouble InitialEXRotation::testTriangulation(const vector &l,\n const vector &r, cv::Mat_ R,\n cv::Mat_ t)\n{\n cv::Mat pointcloud;\n cv::Matx34f P = cv::Matx34f(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0);\n 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),\n R(2, 0), R(2, 1), R(2, 2), t(2));\n cv::triangulatePoints(P, P1, l, r, pointcloud);\n int front_count = 0;\n for (int i = 0; i < pointcloud.cols; i++)\n {\n double normal_factor = pointcloud.col(i).at(3);\n\n cv::Mat_ p_3d_l = cv::Mat(P) * (pointcloud.col(i) / normal_factor);\n cv::Mat_ p_3d_r = cv::Mat(P1) * (pointcloud.col(i) / normal_factor);\n if (p_3d_l(2) > 0 && p_3d_r(2) > 0)\n front_count++;\n }\n ROS_DEBUG(\"MotionEstimator: %f\", 1.0 * front_count / pointcloud.cols);\n return 1.0 * front_count / pointcloud.cols;\n}\n\nvoid InitialEXRotation::decomposeE(cv::Mat E, cv::Mat_ &R1, cv::Mat_ &R2,\n cv::Mat_ &t1, cv::Mat_ &t2)\n{\n cv::SVD svd(E, cv::SVD::MODIFY_A);\n cv::Matx33d W(0, -1, 0, 1, 0, 0, 0, 0, 1);\n cv::Matx33d Wt(0, 1, 0, -1, 0, 0, 0, 0, 1);\n R1 = svd.u * cv::Mat(W) * svd.vt;\n R2 = svd.u * cv::Mat(Wt) * svd.vt;\n t1 = svd.u.col(2);\n t2 = -svd.u.col(2);\n}\n" }, { "path": "vins_estimator/src/initial/initial_ex_rotation.h", "content": "#pragma once\n\n#include \n#include \"../utility/parameters.h\"\nusing namespace std;\n\n#include \n\n#include \nusing namespace Eigen;\n#include \n\n/* This class help you to calibrate extrinsic rotation between imu and camera when your totally\n * don't konw the extrinsic parameter */\nclass InitialEXRotation\n{\npublic:\n InitialEXRotation();\n bool CalibrationExRotation(vector> corres, Quaterniond delta_q_imu,\n Matrix3d &calib_ric_result);\n\nprivate:\n Matrix3d solveRelativeR(const vector> &corres);\n\n double testTriangulation(const vector &l, const vector &r,\n cv::Mat_ R, cv::Mat_ t);\n void decomposeE(cv::Mat E, cv::Mat_ &R1, cv::Mat_ &R2, cv::Mat_ &t1,\n cv::Mat_ &t2);\n int frame_count;\n\n vector Rc;\n vector Rimu;\n vector Rc_g;\n Matrix3d ric;\n};\n" }, { "path": "vins_estimator/src/initial/initial_sfm.cpp", "content": "#include \"initial_sfm.h\"\n\nGlobalSFM::GlobalSFM() {}\n\nvoid GlobalSFM::triangulatePoint(Eigen::Matrix &Pose0,\n Eigen::Matrix &Pose1, Vector2d &point0,\n Vector2d &point1, Vector3d &point_3d)\n{\n Matrix4d design_matrix = Matrix4d::Zero();\n design_matrix.row(0) = point0[0] * Pose0.row(2) - Pose0.row(0);\n design_matrix.row(1) = point0[1] * Pose0.row(2) - Pose0.row(1);\n design_matrix.row(2) = point1[0] * Pose1.row(2) - Pose1.row(0);\n design_matrix.row(3) = point1[1] * Pose1.row(2) - Pose1.row(1);\n Vector4d triangulated_point;\n triangulated_point = design_matrix.jacobiSvd(Eigen::ComputeFullV).matrixV().rightCols<1>();\n point_3d(0) = triangulated_point(0) / triangulated_point(3);\n point_3d(1) = triangulated_point(1) / triangulated_point(3);\n point_3d(2) = triangulated_point(2) / triangulated_point(3);\n}\n\nbool GlobalSFM::solveFrameByPnP(Matrix3d &R_initial, Vector3d &P_initial, int i,\n vector &sfm_f)\n{\n vector pts_2_vector;\n vector pts_3_vector;\n for (int j = 0; j < feature_num; j++)\n {\n if (sfm_f[j].state != true)\n continue;\n Vector2d point2d;\n for (int k = 0; k < (int)sfm_f[j].observation.size(); k++)\n {\n if (sfm_f[j].observation[k].first == i)\n {\n Vector2d img_pts = sfm_f[j].observation[k].second;\n cv::Point2f pts_2(img_pts(0), img_pts(1));\n pts_2_vector.push_back(pts_2);\n cv::Point3f pts_3(sfm_f[j].position[0], sfm_f[j].position[1], sfm_f[j].position[2]);\n pts_3_vector.push_back(pts_3);\n break;\n }\n }\n }\n if (int(pts_2_vector.size()) < 15)\n {\n printf(\"unstable features tracking, please slowly move you device!(%zu)\\n\",\n pts_2_vector.size());\n if (int(pts_2_vector.size()) < 10)\n return false;\n }\n cv::Mat r, rvec, t, D, tmp_r;\n cv::eigen2cv(R_initial, tmp_r);\n cv::Rodrigues(tmp_r, rvec);\n cv::eigen2cv(P_initial, t);\n cv::Mat K = (cv::Mat_(3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 1);\n bool pnp_succ;\n pnp_succ = cv::solvePnP(pts_3_vector, pts_2_vector, K, D, rvec, t, 1);\n if (!pnp_succ)\n {\n return false;\n }\n cv::Rodrigues(rvec, r);\n // cout << \"r \" << endl << r << endl;\n MatrixXd R_pnp;\n cv::cv2eigen(r, R_pnp);\n MatrixXd T_pnp;\n cv::cv2eigen(t, T_pnp);\n R_initial = R_pnp;\n P_initial = T_pnp;\n return true;\n}\n\nvoid GlobalSFM::triangulateTwoFrames(int frame0, Eigen::Matrix &Pose0, int frame1,\n Eigen::Matrix &Pose1, vector &sfm_f)\n{\n assert(frame0 != frame1);\n for (int j = 0; j < feature_num; j++)\n {\n if (sfm_f[j].state == true)\n continue;\n bool has_0 = false, has_1 = false;\n Vector2d point0;\n Vector2d point1;\n for (int k = 0; k < (int)sfm_f[j].observation.size(); k++)\n {\n if (sfm_f[j].observation[k].first == frame0)\n {\n point0 = sfm_f[j].observation[k].second;\n has_0 = true;\n }\n if (sfm_f[j].observation[k].first == frame1)\n {\n point1 = sfm_f[j].observation[k].second;\n has_1 = true;\n }\n }\n if (has_0 && has_1)\n {\n Vector3d point_3d;\n triangulatePoint(Pose0, Pose1, point0, point1, point_3d);\n sfm_f[j].state = true;\n sfm_f[j].position[0] = point_3d(0);\n sfm_f[j].position[1] = point_3d(1);\n sfm_f[j].position[2] = point_3d(2);\n // cout << \"trangulated : \" << frame1 << \" 3d point : \" << j << \" \" <<\n // point_3d.transpose() << endl;\n }\n }\n}\n\nvoid GlobalSFM::triangulateTwoFramesWithDepth(int frame0, Eigen::Matrix &Pose0,\n int frame1, Eigen::Matrix &Pose1,\n vector &sfm_f)\n{\n assert(frame0 != frame1);\n Matrix3d Pose0_R = Pose0.block<3, 3>(0, 0);\n Matrix3d Pose1_R = Pose1.block<3, 3>(0, 0);\n Vector3d Pose0_t = Pose0.block<3, 1>(0, 3);\n Vector3d Pose1_t = Pose1.block<3, 1>(0, 3);\n for (int j = 0; j < feature_num; j++)\n {\n if (sfm_f[j].state == true)\n continue;\n bool has_0 = false, has_1 = false;\n Vector3d point0;\n Vector2d point1;\n for (int k = 0; k < (int)sfm_f[j].observation.size(); k++)\n {\n if (sfm_f[j].observation_depth[k].second < 0.1 ||\n sfm_f[j].observation_depth[k].second > 10) // max and min measurement\n continue;\n if (sfm_f[j].observation[k].first == frame0)\n {\n point0 = Vector3d(\n sfm_f[j].observation[k].second.x() * sfm_f[j].observation_depth[k].second,\n sfm_f[j].observation[k].second.y() * sfm_f[j].observation_depth[k].second,\n sfm_f[j].observation_depth[k].second);\n has_0 = true;\n }\n if (sfm_f[j].observation[k].first == frame1)\n {\n point1 = sfm_f[j].observation[k].second;\n has_1 = true;\n }\n }\n if (has_0 && has_1)\n {\n Vector2d residual;\n Vector3d point_3d, point1_reprojected;\n // triangulatePoint(Pose0, Pose1, point0, point1, point_3d);\n point_3d = Pose0_R.transpose() * point0 -\n Pose0_R.transpose() * Pose0_t; // shan add:this is point in world;\n point1_reprojected = Pose1_R * point_3d + Pose1_t;\n\n residual = point1 - Vector2d(point1_reprojected.x() / point1_reprojected.z(),\n point1_reprojected.y() / point1_reprojected.z());\n\n // std::cout << residual.transpose()<<\"norm\"< &sfm_f, map &sfm_tracked_points)\n{\n feature_num = sfm_f.size();\n // cout << \"set 0 and \" << l << \" as known \" << endl;\n //假设第l帧为原点，根据当前帧到第l帧的relative_R，relative_T，得到当前帧位姿\n q[l].w() = 1;\n q[l].x() = 0;\n q[l].y() = 0;\n q[l].z() = 0;\n T[l].setZero();\n q[frame_num - 1] = q[l] * Quaterniond(relative_R);\n T[frame_num - 1] = relative_T;\n // cout << \"init q_l \" << q[l].w() << \" \" << q[l].vec().transpose() << endl;\n // cout << \"init t_l \" << T[l].transpose() << endl;\n\n // rotate to cam frame\n Matrix3d c_Rotation[frame_num];\n Vector3d c_Translation[frame_num];\n Quaterniond c_Quat[frame_num];\n double c_rotation[frame_num][4];\n double c_translation[frame_num][3];\n //这里的pose表示的是第l帧到每一帧的变换矩阵\n Eigen::Matrix Pose[frame_num];\n\n c_Quat[l] = q[l].inverse();\n c_Rotation[l] = c_Quat[l].toRotationMatrix();\n c_Translation[l] = -1 * (c_Rotation[l] * T[l]);\n Pose[l].block<3, 3>(0, 0) = c_Rotation[l];\n Pose[l].block<3, 1>(0, 3) = c_Translation[l];\n\n c_Quat[frame_num - 1] = q[frame_num - 1].inverse();\n c_Rotation[frame_num - 1] = c_Quat[frame_num - 1].toRotationMatrix();\n c_Translation[frame_num - 1] = -1 * (c_Rotation[frame_num - 1] * T[frame_num - 1]);\n Pose[frame_num - 1].block<3, 3>(0, 0) = c_Rotation[frame_num - 1];\n Pose[frame_num - 1].block<3, 1>(0, 3) = c_Translation[frame_num - 1];\n\n // 1: trangulate between l ----- frame_num - 1\n // 2: solve pnp l + 1; trangulate l + 1 ------- frame_num - 1;\n for (int i = l; i < frame_num - 1; i++)\n {\n // solve pnp\n if (i > l)\n {\n Matrix3d R_initial = c_Rotation[i - 1];\n Vector3d P_initial = c_Translation[i - 1];\n\n if (!solveFrameByPnP(R_initial, P_initial, i, sfm_f))\n return false;\n c_Rotation[i] = R_initial;\n c_Translation[i] = P_initial;\n c_Quat[i] = c_Rotation[i];\n Pose[i].block<3, 3>(0, 0) = c_Rotation[i];\n Pose[i].block<3, 1>(0, 3) = c_Translation[i];\n }\n\n // triangulate point based on the solve pnp result\n triangulateTwoFramesWithDepth(i, Pose[i], frame_num - 1, Pose[frame_num - 1], sfm_f);\n }\n // 3: triangulate l-----l+1 l+2 ... frame_num -2\n for (int i = l + 1; i < frame_num - 1; i++)\n triangulateTwoFramesWithDepth(l, Pose[l], i, Pose[i], sfm_f);\n // 4: solve pnp l-1; triangulate l-1 ----- l\n // l-2 l-2 ----- l\n for (int i = l - 1; i >= 0; i--)\n {\n // solve pnp\n Matrix3d R_initial = c_Rotation[i + 1];\n Vector3d P_initial = c_Translation[i + 1];\n if (!solveFrameByPnP(R_initial, P_initial, i, sfm_f))\n return false;\n c_Rotation[i] = R_initial;\n c_Translation[i] = P_initial;\n c_Quat[i] = c_Rotation[i];\n Pose[i].block<3, 3>(0, 0) = c_Rotation[i];\n Pose[i].block<3, 1>(0, 3) = c_Translation[i];\n // triangulate\n triangulateTwoFramesWithDepth(i, Pose[i], l, Pose[l], sfm_f);\n }\n // 5: triangulate all other points\n for (int j = 0; j < feature_num; j++)\n {\n if (sfm_f[j].state == true)\n continue;\n if ((int)sfm_f[j].observation.size() >= 2)\n {\n Vector3d point0;\n Vector2d point1;\n int frame_0 = sfm_f[j].observation[0].first;\n if (sfm_f[j].observation_depth[0].second < 0.1 ||\n sfm_f[j].observation_depth[0].second > 10) // max and min measurement\n continue;\n point0 =\n Vector3d(sfm_f[j].observation[0].second.x() * sfm_f[j].observation_depth[0].second,\n sfm_f[j].observation[0].second.y() * sfm_f[j].observation_depth[0].second,\n sfm_f[j].observation_depth[0].second);\n int frame_1 = sfm_f[j].observation.back().first;\n point1 = sfm_f[j].observation.back().second;\n Vector3d point_3d;\n // triangulatePoint(Pose[frame_0], Pose[frame_1], point0, point1,\n // point_3d);\n\n Matrix3d Pose0_R = Pose[frame_0].block<3, 3>(0, 0);\n Matrix3d Pose1_R = Pose[frame_1].block<3, 3>(0, 0);\n Vector3d Pose0_t = Pose[frame_0].block<3, 1>(0, 3);\n Vector3d Pose1_t = Pose[frame_1].block<3, 1>(0, 3);\n\n Vector2d residual;\n Vector3d point1_reprojected;\n // triangulatePoint(Pose0, Pose1, point0, point1, point_3d);\n point_3d =\n Pose0_R.transpose() * point0 - Pose0_R.transpose() * Pose0_t; // point in world;\n point1_reprojected = Pose1_R * point_3d + Pose1_t;\n\n residual = point1 - Vector2d(point1_reprojected.x() / point1_reprojected.z(),\n point1_reprojected.y() / point1_reprojected.z());\n\n if (residual.norm() < 1.0 / 460)\n { // reprojection error\n sfm_f[j].state = true;\n sfm_f[j].position[0] = point_3d(0);\n sfm_f[j].position[1] = point_3d(1);\n sfm_f[j].position[2] = point_3d(2);\n }\n // cout << \"trangulated : \" << frame_0 << \" \" << frame_1 << \" 3d point :\n // \" << j << \" \" << point_3d.transpose() << endl;\n }\n }\n\n /*\n for (int i = 0; i < frame_num; i++)\n {\n q[i] = c_Rotation[i].transpose();\n cout << \"solvePnP q\" << \" i \" << i <<\" \" < &sfm_f,\n map &sfm_tracked_points)\n{\n feature_num = sfm_f.size();\n // cout << \"set 0 and \" << l << \" as known \" << endl;\n //假设第l帧为原点，根据当前帧到第l帧的relative_R，relative_T，得到当前帧位姿\n q[l].w() = 1;\n q[l].x() = 0;\n q[l].y() = 0;\n q[l].z() = 0;\n T[l].setZero();\n q[frame_num - 1] = q[l] * Quaterniond(relative_R);\n T[frame_num - 1] = relative_T;\n // cout << \"init q_l \" << q[l].w() << \" \" << q[l].vec().transpose() << endl;\n // cout << \"init t_l \" << T[l].transpose() << endl;\n\n // rotate to cam frame\n Matrix3d c_Rotation[frame_num];\n Vector3d c_Translation[frame_num];\n Quaterniond c_Quat[frame_num];\n double c_rotation[frame_num][4];\n double c_translation[frame_num][3];\n //这里的pose表示的是第l帧到每一帧的变换矩阵\n Eigen::Matrix Pose[frame_num];\n\n c_Quat[l] = q[l].inverse();\n c_Rotation[l] = c_Quat[l].toRotationMatrix();\n c_Translation[l] = -1 * (c_Rotation[l] * T[l]);\n Pose[l].block<3, 3>(0, 0) = c_Rotation[l];\n Pose[l].block<3, 1>(0, 3) = c_Translation[l];\n\n c_Quat[frame_num - 1] = q[frame_num - 1].inverse();\n c_Rotation[frame_num - 1] = c_Quat[frame_num - 1].toRotationMatrix();\n c_Translation[frame_num - 1] = -1 * (c_Rotation[frame_num - 1] * T[frame_num - 1]);\n Pose[frame_num - 1].block<3, 3>(0, 0) = c_Rotation[frame_num - 1];\n Pose[frame_num - 1].block<3, 1>(0, 3) = c_Translation[frame_num - 1];\n\n // 1: trangulate between l ----- frame_num - 1\n // 2: solve pnp l + 1; trangulate l + 1 ------- frame_num - 1;\n for (int i = l; i < frame_num - 1; i++)\n {\n // solve pnp\n if (i > l)\n {\n Matrix3d R_initial = c_Rotation[i - 1];\n Vector3d P_initial = c_Translation[i - 1];\n\n cout << \"i0 = \" << i << endl;\n if (!solveFrameByPnP(R_initial, P_initial, i, sfm_f))\n return false;\n c_Rotation[i] = R_initial;\n c_Translation[i] = P_initial;\n c_Quat[i] = c_Rotation[i];\n Pose[i].block<3, 3>(0, 0) = c_Rotation[i];\n Pose[i].block<3, 1>(0, 3) = c_Translation[i];\n }\n\n // triangulate point based on the solve pnp result\n triangulateTwoFramesWithDepth(i, Pose[i], frame_num - 1, Pose[frame_num - 1], sfm_f);\n }\n // 3: triangulate l-----l+1 l+2 ... frame_num -2\n for (int i = l + 1; i < frame_num - 1; i++)\n triangulateTwoFramesWithDepth(l, Pose[l], i, Pose[i], sfm_f);\n // 4: solve pnp l-1; triangulate l-1 ----- l\n // l-2 l-2 ----- l\n for (int i = l - 1; i >= 0; i--)\n {\n // solve pnp\n Matrix3d R_initial = c_Rotation[i + 1];\n Vector3d P_initial = c_Translation[i + 1];\n cout << \"i1 = \" << i << endl;\n if (!solveFrameByPnP(R_initial, P_initial, i, sfm_f))\n return false;\n c_Rotation[i] = R_initial;\n c_Translation[i] = P_initial;\n c_Quat[i] = c_Rotation[i];\n Pose[i].block<3, 3>(0, 0) = c_Rotation[i];\n Pose[i].block<3, 1>(0, 3) = c_Translation[i];\n // triangulate\n triangulateTwoFramesWithDepth(i, Pose[i], l, Pose[l], sfm_f);\n }\n // 5: triangulate all other points\n for (int j = 0; j < feature_num; j++)\n {\n if (sfm_f[j].state == true)\n continue;\n if ((int)sfm_f[j].observation.size() >= 2)\n {\n Vector3d point0;\n Vector2d point1;\n int frame_0 = sfm_f[j].observation[0].first;\n if (sfm_f[j].observation_depth[0].second < 0.1 ||\n sfm_f[j].observation_depth[0].second > 10) // max and min measurement\n continue;\n point0 =\n Vector3d(sfm_f[j].observation[0].second.x() * sfm_f[j].observation_depth[0].second,\n sfm_f[j].observation[0].second.y() * sfm_f[j].observation_depth[0].second,\n sfm_f[j].observation_depth[0].second);\n int frame_1 = sfm_f[j].observation.back().first;\n point1 = sfm_f[j].observation.back().second;\n Vector3d point_3d;\n // triangulatePoint(Pose[frame_0], Pose[frame_1], point0, point1,\n // point_3d);\n\n Matrix3d Pose0_R = Pose[frame_0].block<3, 3>(0, 0);\n Matrix3d Pose1_R = Pose[frame_1].block<3, 3>(0, 0);\n Vector3d Pose0_t = Pose[frame_0].block<3, 1>(0, 3);\n Vector3d Pose1_t = Pose[frame_1].block<3, 1>(0, 3);\n\n Vector2d residual;\n Vector3d point1_reprojected;\n // triangulatePoint(Pose0, Pose1, point0, point1, point_3d);\n point_3d =\n Pose0_R.transpose() * point0 - Pose0_R.transpose() * Pose0_t; // point in world;\n point1_reprojected = Pose1_R * point_3d + Pose1_t;\n\n residual = point1 - Vector2d(point1_reprojected.x() / point1_reprojected.z(),\n point1_reprojected.y() / point1_reprojected.z());\n\n if (residual.norm() < 1.0 / 460)\n { // reprojection error\n sfm_f[j].state = true;\n sfm_f[j].position[0] = point_3d(0);\n sfm_f[j].position[1] = point_3d(1);\n sfm_f[j].position[2] = point_3d(2);\n }\n // cout << \"trangulated : \" << frame_0 << \" \" << frame_1 << \" 3d point :\n // \" << j << \" \" << point_3d.transpose() << endl;\n }\n }\n // full BA\n ceres::Problem problem;\n ceres::LocalParameterization *local_parameterization = new ceres::QuaternionParameterization();\n // cout << \" begin full BA \" << endl;\n for (int i = 0; i < frame_num; i++)\n {\n // double array for ceres\n c_translation[i][0] = c_Translation[i].x();\n c_translation[i][1] = c_Translation[i].y();\n c_translation[i][2] = c_Translation[i].z();\n c_rotation[i][0] = c_Quat[i].w();\n c_rotation[i][1] = c_Quat[i].x();\n c_rotation[i][2] = c_Quat[i].y();\n c_rotation[i][3] = c_Quat[i].z();\n // https://blog.csdn.net/weixin_43991178/article/details/100532618\n problem.AddParameterBlock(c_rotation[i], 4, local_parameterization);\n problem.AddParameterBlock(c_translation[i], 3);\n // 固定（不作优化变量）选定参考帧（第l帧）的旋转\n if (i == l)\n {\n problem.SetParameterBlockConstant(c_rotation[i]);\n }\n // 固定（不作优化变量）选定参考帧（第l帧）和当前帧的平移\n if (i == l || i == frame_num - 1)\n {\n problem.SetParameterBlockConstant(c_translation[i]);\n }\n }\n\n for (int i = 0; i < feature_num; i++)\n {\n if (sfm_f[i].state != true)\n continue;\n for (int j = 0; j < int(sfm_f[i].observation.size()); j++)\n {\n int l = sfm_f[i].observation[j].first;\n ceres::CostFunction *cost_function = ReprojectionError3D::Create(\n sfm_f[i].observation[j].second.x(), sfm_f[i].observation[j].second.y());\n\n problem.AddResidualBlock(cost_function, NULL, c_rotation[l], c_translation[l],\n sfm_f[i].position);\n }\n }\n ceres::Solver::Options options;\n options.linear_solver_type = ceres::DENSE_SCHUR;\n // options.minimizer_progress_to_stdout = true;\n options.max_solver_time_in_seconds = 0.2;\n ceres::Solver::Summary summary;\n ceres::Solve(options, &problem, &summary);\n // std::cout << summary.BriefReport() << \"\\n\";\n if (summary.termination_type == ceres::CONVERGENCE || summary.final_cost < 5e-03)\n {\n // cout << \"vision only BA converge\" << endl;\n }\n else\n {\n // cout << \"vision only BA not converge \" << endl;\n return false;\n }\n for (int i = 0; i < frame_num; i++)\n {\n q[i].w() = c_rotation[i][0];\n q[i].x() = c_rotation[i][1];\n q[i].y() = c_rotation[i][2];\n q[i].z() = c_rotation[i][3];\n q[i] = q[i].inverse();\n // cout << \"final q\" << \" i \" << i <<\" \" <\n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \nusing namespace Eigen;\nusing namespace std;\n\nstruct SFMFeature\n{\n bool state;\n int id;\n vector> observation;\n double position[3];\n double depth;\n vector> observation_depth;\n};\n\nstruct ReprojectionError3D\n{\n ReprojectionError3D(double observed_u, double observed_v)\n : observed_u(observed_u), observed_v(observed_v)\n {\n }\n\n template \n bool operator()(const T *const camera_R, const T *const camera_T, const T *point,\n T *residuals) const\n {\n T p[3];\n ceres::QuaternionRotatePoint(camera_R, point, p);\n p[0] += camera_T[0];\n p[1] += camera_T[1];\n p[2] += camera_T[2];\n T xp = p[0] / p[2];\n T yp = p[1] / p[2];\n residuals[0] = xp - T(observed_u);\n residuals[1] = yp - T(observed_v);\n return true;\n }\n\n static ceres::CostFunction *Create(const double observed_x, const double observed_y)\n {\n return (new ceres::AutoDiffCostFunction(\n new ReprojectionError3D(observed_x, observed_y)));\n }\n\n double observed_u;\n double observed_v;\n};\n\nclass GlobalSFM\n{\npublic:\n GlobalSFM();\n bool construct(int frame_num, Quaterniond *q, Vector3d *T, int l, const Matrix3d relative_R,\n const Vector3d relative_T, vector &sfm_f,\n map &sfm_tracked_points);\n bool constructWithDepth(int frame_num, Quaterniond *q, Vector3d *T, int l,\n const Matrix3d relative_R, const Vector3d relative_T,\n vector &sfm_f, map &sfm_tracked_points);\n\nprivate:\n bool solveFrameByPnP(Matrix3d &R_initial, Vector3d &P_initial, int i,\n vector &sfm_f);\n\n void triangulatePoint(Eigen::Matrix &Pose0, Eigen::Matrix &Pose1,\n Vector2d &point0, Vector2d &point1, Vector3d &point_3d);\n void triangulateTwoFrames(int frame0, Eigen::Matrix &Pose0, int frame1,\n Eigen::Matrix &Pose1, vector &sfm_f);\n void triangulateTwoFramesWithDepth(int frame0, Eigen::Matrix &Pose0, int frame1,\n Eigen::Matrix &Pose1,\n vector &sfm_f);\n\n int feature_num;\n};" }, { "path": "vins_estimator/src/initial/solve_5pts.cpp", "content": "#include \"solve_5pts.h\"\n#include \n#include \nusing Sophus::SE3;\nusing Sophus::SO3;\n\nnamespace cv\n{\nvoid decomposeEssentialMat(InputArray _E, OutputArray _R1, OutputArray _R2, OutputArray _t)\n{\n Mat E = _E.getMat().reshape(1, 3);\n CV_Assert(E.cols == 3 && E.rows == 3);\n\n Mat D, U, Vt;\n SVD::compute(E, D, U, Vt);\n\n if (determinant(U) < 0)\n U *= -1.;\n if (determinant(Vt) < 0)\n Vt *= -1.;\n\n Mat W = (Mat_(3, 3) << 0, 1, 0, -1, 0, 0, 0, 0, 1);\n W.convertTo(W, E.type());\n\n Mat R1, R2, t;\n R1 = U * W * Vt;\n R2 = U * W.t() * Vt;\n t = U.col(2) * 1.0;\n\n R1.copyTo(_R1);\n R2.copyTo(_R2);\n t.copyTo(_t);\n}\n\nint recoverPose(InputArray E, InputArray _points1, InputArray _points2, InputArray _cameraMatrix,\n OutputArray _R, OutputArray _t, InputOutputArray _mask)\n{\n Mat points1, points2, cameraMatrix;\n _points1.getMat().convertTo(points1, CV_64F);\n _points2.getMat().convertTo(points2, CV_64F);\n _cameraMatrix.getMat().convertTo(cameraMatrix, CV_64F);\n\n int npoints = points1.checkVector(2);\n CV_Assert(npoints >= 0 && points2.checkVector(2) == npoints &&\n points1.type() == points2.type());\n\n CV_Assert(cameraMatrix.rows == 3 && cameraMatrix.cols == 3 && cameraMatrix.channels() == 1);\n\n if (points1.channels() > 1)\n {\n points1 = points1.reshape(1, npoints);\n points2 = points2.reshape(1, npoints);\n }\n\n double fx = cameraMatrix.at(0, 0);\n double fy = cameraMatrix.at(1, 1);\n double cx = cameraMatrix.at(0, 2);\n double cy = cameraMatrix.at(1, 2);\n\n points1.col(0) = (points1.col(0) - cx) / fx;\n points2.col(0) = (points2.col(0) - cx) / fx;\n points1.col(1) = (points1.col(1) - cy) / fy;\n points2.col(1) = (points2.col(1) - cy) / fy;\n\n points1 = points1.t();\n points2 = points2.t();\n\n Mat R1, R2, t;\n decomposeEssentialMat(E, R1, R2, t);\n Mat P0 = Mat::eye(3, 4, R1.type());\n Mat P1(3, 4, R1.type()), P2(3, 4, R1.type()), P3(3, 4, R1.type()), P4(3, 4, R1.type());\n P1(Range::all(), Range(0, 3)) = R1 * 1.0;\n P1.col(3) = t * 1.0;\n P2(Range::all(), Range(0, 3)) = R2 * 1.0;\n P2.col(3) = t * 1.0;\n P3(Range::all(), Range(0, 3)) = R1 * 1.0;\n P3.col(3) = -t * 1.0;\n P4(Range::all(), Range(0, 3)) = R2 * 1.0;\n P4.col(3) = -t * 1.0;\n\n // Do the cheirality check.\n // Notice here a threshold dist is used to filter\n // out far away points (i.e. infinite points) since\n // there depth may vary between postive and negtive.\n double dist = 50.0;\n Mat Q;\n triangulatePoints(P0, P1, points1, points2, Q);\n Mat mask1 = Q.row(2).mul(Q.row(3)) > 0;\n Q.row(0) /= Q.row(3);\n Q.row(1) /= Q.row(3);\n Q.row(2) /= Q.row(3);\n Q.row(3) /= Q.row(3);\n mask1 = (Q.row(2) < dist) & mask1;\n Q = P1 * Q;\n mask1 = (Q.row(2) > 0) & mask1;\n mask1 = (Q.row(2) < dist) & mask1;\n\n triangulatePoints(P0, P2, points1, points2, Q);\n Mat mask2 = Q.row(2).mul(Q.row(3)) > 0;\n Q.row(0) /= Q.row(3);\n Q.row(1) /= Q.row(3);\n Q.row(2) /= Q.row(3);\n Q.row(3) /= Q.row(3);\n mask2 = (Q.row(2) < dist) & mask2;\n Q = P2 * Q;\n mask2 = (Q.row(2) > 0) & mask2;\n mask2 = (Q.row(2) < dist) & mask2;\n\n triangulatePoints(P0, P3, points1, points2, Q);\n Mat mask3 = Q.row(2).mul(Q.row(3)) > 0;\n Q.row(0) /= Q.row(3);\n Q.row(1) /= Q.row(3);\n Q.row(2) /= Q.row(3);\n Q.row(3) /= Q.row(3);\n mask3 = (Q.row(2) < dist) & mask3;\n Q = P3 * Q;\n mask3 = (Q.row(2) > 0) & mask3;\n mask3 = (Q.row(2) < dist) & mask3;\n\n triangulatePoints(P0, P4, points1, points2, Q);\n Mat mask4 = Q.row(2).mul(Q.row(3)) > 0;\n Q.row(0) /= Q.row(3);\n Q.row(1) /= Q.row(3);\n Q.row(2) /= Q.row(3);\n Q.row(3) /= Q.row(3);\n mask4 = (Q.row(2) < dist) & mask4;\n Q = P4 * Q;\n mask4 = (Q.row(2) > 0) & mask4;\n mask4 = (Q.row(2) < dist) & mask4;\n\n mask1 = mask1.t();\n mask2 = mask2.t();\n mask3 = mask3.t();\n mask4 = mask4.t();\n\n // If _mask is given, then use it to filter outliers.\n if (!_mask.empty())\n {\n Mat mask = _mask.getMat();\n CV_Assert(mask.size() == mask1.size());\n bitwise_and(mask, mask1, mask1);\n bitwise_and(mask, mask2, mask2);\n bitwise_and(mask, mask3, mask3);\n bitwise_and(mask, mask4, mask4);\n }\n if (_mask.empty() && _mask.needed())\n {\n _mask.create(mask1.size(), CV_8U);\n }\n\n CV_Assert(_R.needed() && _t.needed());\n _R.create(3, 3, R1.type());\n _t.create(3, 1, t.type());\n\n int good1 = countNonZero(mask1);\n int good2 = countNonZero(mask2);\n int good3 = countNonZero(mask3);\n int good4 = countNonZero(mask4);\n\n if (good1 >= good2 && good1 >= good3 && good1 >= good4)\n {\n R1.copyTo(_R);\n t.copyTo(_t);\n if (_mask.needed())\n mask1.copyTo(_mask);\n return good1;\n }\n else if (good2 >= good1 && good2 >= good3 && good2 >= good4)\n {\n R2.copyTo(_R);\n t.copyTo(_t);\n if (_mask.needed())\n mask2.copyTo(_mask);\n return good2;\n }\n else if (good3 >= good1 && good3 >= good2 && good3 >= good4)\n {\n t = -t;\n R1.copyTo(_R);\n t.copyTo(_t);\n if (_mask.needed())\n mask3.copyTo(_mask);\n return good3;\n }\n else\n {\n t = -t;\n R2.copyTo(_R);\n t.copyTo(_t);\n if (_mask.needed())\n mask4.copyTo(_mask);\n return good4;\n }\n}\n\nint recoverPose(InputArray E, InputArray _points1, InputArray _points2, OutputArray _R,\n OutputArray _t, double focal, Point2d pp, InputOutputArray _mask)\n{\n Mat cameraMatrix = (Mat_(3, 3) << focal, 0, pp.x, 0, focal, pp.y, 0, 0, 1);\n return cv::recoverPose(E, _points1, _points2, cameraMatrix, _R, _t, _mask);\n}\n} // namespace cv\n\nbool MotionEstimator::solveRelativeRT(const vector> &corres,\n Matrix3d &Rotation, Vector3d &Translation)\n{\n if (corres.size() >= 15)\n {\n vector ll, rr;\n for (int i = 0; i < int(corres.size()); i++)\n {\n ll.push_back(cv::Point2f(corres[i].first(0), corres[i].first(1)));\n rr.push_back(cv::Point2f(corres[i].second(0), corres[i].second(1)));\n }\n cv::Mat mask;\n cv::Mat E = cv::findFundamentalMat(ll, rr, cv::FM_RANSAC, 0.3 / 460, 0.99, mask);\n cv::Mat cameraMatrix = (cv::Mat_(3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 1);\n cv::Mat rot, trans;\n int inlier_cnt = cv::recoverPose(E, ll, rr, cameraMatrix, rot, trans, mask);\n // cout << \"inlier_cnt \" << inlier_cnt << endl;\n\n Eigen::Matrix3d R;\n Eigen::Vector3d T;\n for (int i = 0; i < 3; i++)\n {\n T(i) = trans.at(i, 0);\n for (int j = 0; j < 3; j++)\n R(i, j) = rot.at(i, j);\n }\n\n Rotation = R.transpose();\n Translation = -R.transpose() * T;\n if (inlier_cnt > 12)\n {\n ROS_ERROR_STREAM(\"----------5points-----------\");\n ROS_ERROR_STREAM(ll.size());\n ROS_ERROR_STREAM(inlier_cnt);\n ROS_ERROR_STREAM(Rotation);\n ROS_ERROR_STREAM(Translation);\n return true;\n }\n else\n return false;\n }\n return false;\n}\n\nbool MotionEstimator::solveRelativeRT_PNP(const vector> &corres,\n Matrix3d &Rotation, Vector3d &Translation)\n{\n vector lll;\n vector rr;\n for (int i = 0; i < int(corres.size()); i++)\n {\n if (corres[i].first(2) > 0 && corres[i].second(2) > 0)\n {\n lll.push_back(cv::Point3f(corres[i].first(0), corres[i].first(1), corres[i].first(2)));\n rr.push_back(cv::Point2f(corres[i].second(0) / corres[i].second(2),\n corres[i].second(1) / corres[i].second(2)));\n }\n }\n cv::Mat rvec, tvec, inliersArr;\n cv::Mat cameraMatrix = (cv::Mat_(3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 1);\n\n cv::solvePnPRansac(lll, rr, cameraMatrix, cv::Mat(), rvec, tvec, false, 100, 1.0 / 460, 0.99,\n inliersArr, cv::SOLVEPNP_EPNP); // maybe don't need 100times\n\n Vector3d tran(tvec.at(0, 0), tvec.at(1, 0), tvec.at(2, 0));\n Matrix3d rota =\n SO3(rvec.at(0, 0), rvec.at(1, 0), rvec.at(2, 0)).matrix();\n\n //\tVector2d tp1,residualV;\n //\tVector3d tp23d;\n // for (int i = 0; i < int(lll.size()); i++) {\n // tp1 = Vector2d(rr[i].x,rr[i].y);\n // tp23d = Vector3d(lll[i].x,lll[i].y,lll[i].z);\n //\n // tp23d = rota * tp23d + tran;\n // Vector2d tp2(tp23d.x()/tp23d.z(),tp23d.y()/tp23d.z());\n //\n // residualV = (tp2 - tp1);\n // ROS_ERROR_STREAM(residualV.transpose());\n // }\n ROS_DEBUG_STREAM(\"-----------pnp-----------\");\n ROS_DEBUG_STREAM(lll.size());\n ROS_DEBUG_STREAM(inliersArr.rows);\n\n Rotation = rota.transpose();\n Translation = -rota.transpose() * tran;\n\n ROS_DEBUG_STREAM(Rotation);\n ROS_DEBUG_STREAM(Translation);\n return true;\n}\n" }, { "path": "vins_estimator/src/initial/solve_5pts.h", "content": "#pragma once\n\n#include \nusing namespace std;\n\n#include \n//#include \n#include \nusing namespace Eigen;\n\n#include \n\nclass MotionEstimator\n{\npublic:\n bool solveRelativeRT(const vector> &corres, Matrix3d &R, Vector3d &T);\n bool solveRelativeRT_ICP(vector> &corres, Matrix3d &R, Vector3d &T);\n bool solveRelativeRT_PNP(const vector> &corres, Matrix3d &Rotation,\n Vector3d &Translation);\n\nprivate:\n double testTriangulation(const vector &l, const vector &r,\n cv::Mat_ R, cv::Mat_ t);\n void decomposeE(cv::Mat E, cv::Mat_ &R1, cv::Mat_ &R2, cv::Mat_ &t1,\n cv::Mat_ &t2);\n};\n" }, { "path": "vins_estimator/src/utility/CameraPoseVisualization.cpp", "content": "#include \"CameraPoseVisualization.h\"\n\nconst Eigen::Vector3d CameraPoseVisualization::imlt = Eigen::Vector3d(-1.0, -0.5, 1.0);\nconst Eigen::Vector3d CameraPoseVisualization::imrt = Eigen::Vector3d(1.0, -0.5, 1.0);\nconst Eigen::Vector3d CameraPoseVisualization::imlb = Eigen::Vector3d(-1.0, 0.5, 1.0);\nconst Eigen::Vector3d CameraPoseVisualization::imrb = Eigen::Vector3d(1.0, 0.5, 1.0);\nconst Eigen::Vector3d CameraPoseVisualization::lt0 = Eigen::Vector3d(-0.7, -0.5, 1.0);\nconst Eigen::Vector3d CameraPoseVisualization::lt1 = Eigen::Vector3d(-0.7, -0.2, 1.0);\nconst Eigen::Vector3d CameraPoseVisualization::lt2 = Eigen::Vector3d(-1.0, -0.2, 1.0);\nconst Eigen::Vector3d CameraPoseVisualization::oc = Eigen::Vector3d(0.0, 0.0, 0.0);\n\nvoid Eigen2Point(const Eigen::Vector3d &v, geometry_msgs::Point &p)\n{\n p.x = v.x();\n p.y = v.y();\n p.z = v.z();\n}\n\nCameraPoseVisualization::CameraPoseVisualization(float r, float g, float b, float a)\n : m_marker_ns(\"CameraPoseVisualization\"), m_scale(0.2), m_line_width(0.01)\n{\n m_image_boundary_color.r = r;\n m_image_boundary_color.g = g;\n m_image_boundary_color.b = b;\n m_image_boundary_color.a = a;\n m_optical_center_connector_color.r = r;\n m_optical_center_connector_color.g = g;\n m_optical_center_connector_color.b = b;\n m_optical_center_connector_color.a = a;\n}\n\nvoid CameraPoseVisualization::setImageBoundaryColor(float r, float g, float b, float a)\n{\n m_image_boundary_color.r = r;\n m_image_boundary_color.g = g;\n m_image_boundary_color.b = b;\n m_image_boundary_color.a = a;\n}\n\nvoid CameraPoseVisualization::setOpticalCenterConnectorColor(float r, float g, float b, float a)\n{\n m_optical_center_connector_color.r = r;\n m_optical_center_connector_color.g = g;\n m_optical_center_connector_color.b = b;\n m_optical_center_connector_color.a = a;\n}\n\nvoid CameraPoseVisualization::setScale(double s)\n{\n m_scale = s;\n}\nvoid CameraPoseVisualization::setLineWidth(double width)\n{\n m_line_width = width;\n}\nvoid CameraPoseVisualization::add_edge(const Eigen::Vector3d &p0, const Eigen::Vector3d &p1)\n{\n visualization_msgs::Marker marker;\n\n marker.ns = m_marker_ns;\n marker.id = m_markers.size() + 1;\n marker.type = visualization_msgs::Marker::LINE_LIST;\n marker.action = visualization_msgs::Marker::ADD;\n marker.scale.x = 0.005;\n\n marker.color.g = 1.0f;\n marker.color.a = 1.0;\n\n geometry_msgs::Point point0, point1;\n\n Eigen2Point(p0, point0);\n Eigen2Point(p1, point1);\n\n marker.points.push_back(point0);\n marker.points.push_back(point1);\n\n m_markers.push_back(marker);\n}\n\nvoid CameraPoseVisualization::add_loopedge(const Eigen::Vector3d &p0, const Eigen::Vector3d &p1)\n{\n visualization_msgs::Marker marker;\n\n marker.ns = m_marker_ns;\n marker.id = m_markers.size() + 1;\n marker.type = visualization_msgs::Marker::LINE_LIST;\n marker.action = visualization_msgs::Marker::ADD;\n marker.scale.x = 0.04;\n // marker.scale.x = 0.3;\n\n marker.color.r = 1.0f;\n marker.color.b = 1.0f;\n marker.color.a = 1.0;\n\n geometry_msgs::Point point0, point1;\n\n Eigen2Point(p0, point0);\n Eigen2Point(p1, point1);\n\n marker.points.push_back(point0);\n marker.points.push_back(point1);\n\n m_markers.push_back(marker);\n}\n\nvoid CameraPoseVisualization::add_pose(const Eigen::Vector3d &p, const Eigen::Quaterniond &q)\n{\n visualization_msgs::Marker marker;\n\n marker.ns = m_marker_ns;\n marker.id = m_markers.size() + 1;\n marker.type = visualization_msgs::Marker::LINE_STRIP;\n marker.action = visualization_msgs::Marker::ADD;\n marker.scale.x = m_line_width;\n\n marker.pose.position.x = 0.0;\n marker.pose.position.y = 0.0;\n marker.pose.position.z = 0.0;\n marker.pose.orientation.w = 1.0;\n marker.pose.orientation.x = 0.0;\n marker.pose.orientation.y = 0.0;\n marker.pose.orientation.z = 0.0;\n\n geometry_msgs::Point pt_lt, pt_lb, pt_rt, pt_rb, pt_oc, pt_lt0, pt_lt1, pt_lt2;\n\n Eigen2Point(q * (m_scale * imlt) + p, pt_lt);\n Eigen2Point(q * (m_scale * imlb) + p, pt_lb);\n Eigen2Point(q * (m_scale * imrt) + p, pt_rt);\n Eigen2Point(q * (m_scale * imrb) + p, pt_rb);\n Eigen2Point(q * (m_scale * lt0) + p, pt_lt0);\n Eigen2Point(q * (m_scale * lt1) + p, pt_lt1);\n Eigen2Point(q * (m_scale * lt2) + p, pt_lt2);\n Eigen2Point(q * (m_scale * oc) + p, pt_oc);\n\n // image boundaries\n marker.points.push_back(pt_lt);\n marker.points.push_back(pt_lb);\n marker.colors.push_back(m_image_boundary_color);\n marker.colors.push_back(m_image_boundary_color);\n\n marker.points.push_back(pt_lb);\n marker.points.push_back(pt_rb);\n marker.colors.push_back(m_image_boundary_color);\n marker.colors.push_back(m_image_boundary_color);\n\n marker.points.push_back(pt_rb);\n marker.points.push_back(pt_rt);\n marker.colors.push_back(m_image_boundary_color);\n marker.colors.push_back(m_image_boundary_color);\n\n marker.points.push_back(pt_rt);\n marker.points.push_back(pt_lt);\n marker.colors.push_back(m_image_boundary_color);\n marker.colors.push_back(m_image_boundary_color);\n\n // top-left indicator\n marker.points.push_back(pt_lt0);\n marker.points.push_back(pt_lt1);\n marker.colors.push_back(m_image_boundary_color);\n marker.colors.push_back(m_image_boundary_color);\n\n marker.points.push_back(pt_lt1);\n marker.points.push_back(pt_lt2);\n marker.colors.push_back(m_image_boundary_color);\n marker.colors.push_back(m_image_boundary_color);\n\n // optical center connector\n marker.points.push_back(pt_lt);\n marker.points.push_back(pt_oc);\n marker.colors.push_back(m_optical_center_connector_color);\n marker.colors.push_back(m_optical_center_connector_color);\n\n marker.points.push_back(pt_lb);\n marker.points.push_back(pt_oc);\n marker.colors.push_back(m_optical_center_connector_color);\n marker.colors.push_back(m_optical_center_connector_color);\n\n marker.points.push_back(pt_rt);\n marker.points.push_back(pt_oc);\n marker.colors.push_back(m_optical_center_connector_color);\n marker.colors.push_back(m_optical_center_connector_color);\n\n marker.points.push_back(pt_rb);\n marker.points.push_back(pt_oc);\n marker.colors.push_back(m_optical_center_connector_color);\n marker.colors.push_back(m_optical_center_connector_color);\n\n m_markers.push_back(marker);\n}\n\nvoid CameraPoseVisualization::reset()\n{\n m_markers.clear();\n}\n\nvoid CameraPoseVisualization::publish_by(ros::Publisher &pub, const std_msgs::Header &header)\n{\n visualization_msgs::MarkerArray markerArray_msg;\n\n for (auto &marker : m_markers)\n {\n marker.header = header;\n markerArray_msg.markers.push_back(marker);\n }\n\n pub.publish(markerArray_msg);\n}" }, { "path": "vins_estimator/src/utility/CameraPoseVisualization.h", "content": "#pragma once\n\n#include \n#include \n#include \n#include \n#include \n#include \n\nclass CameraPoseVisualization\n{\npublic:\n std::string m_marker_ns;\n\n CameraPoseVisualization(float r, float g, float b, float a);\n\n void setImageBoundaryColor(float r, float g, float b, float a = 1.0);\n void setOpticalCenterConnectorColor(float r, float g, float b, float a = 1.0);\n void setScale(double s);\n void setLineWidth(double width);\n\n void add_pose(const Eigen::Vector3d &p, const Eigen::Quaterniond &q);\n void reset();\n\n void publish_by(ros::Publisher &pub, const std_msgs::Header &header);\n void add_edge(const Eigen::Vector3d &p0, const Eigen::Vector3d &p1);\n void add_loopedge(const Eigen::Vector3d &p0, const Eigen::Vector3d &p1);\n\nprivate:\n std::vector m_markers;\n std_msgs::ColorRGBA m_image_boundary_color;\n std_msgs::ColorRGBA m_optical_center_connector_color;\n double m_scale;\n double m_line_width;\n\n static const Eigen::Vector3d imlt;\n static const Eigen::Vector3d imlb;\n static const Eigen::Vector3d imrt;\n static const Eigen::Vector3d imrb;\n static const Eigen::Vector3d oc;\n static const Eigen::Vector3d lt0;\n static const Eigen::Vector3d lt1;\n static const Eigen::Vector3d lt2;\n};\n" }, { "path": "vins_estimator/src/utility/parameters.cpp", "content": "#include \"parameters.h\"\n\n#include \n\ndouble INIT_DEPTH;\ndouble MIN_PARALLAX;\ndouble ACC_N, ACC_W;\ndouble GYR_N, GYR_W;\n\nstd::vector RIC;\nstd::vector TIC;\n\nEigen::Vector3d G{0.0, 0.0, 9.8};\n\ndouble BIAS_ACC_THRESHOLD;\ndouble BIAS_GYR_THRESHOLD;\ndouble SOLVER_TIME;\nint NUM_ITERATIONS;\nint ESTIMATE_EXTRINSIC;\nint ESTIMATE_TD;\nint ROLLING_SHUTTER;\nstd::string EX_CALIB_RESULT_PATH;\nstd::string VINS_RESULT_PATH;\nstd::string IMAGE_TOPIC;\nstd::string DEPTH_TOPIC;\nstd::string IMU_TOPIC;\n\nint MAX_CNT;\nint MAX_CNT_SET;\nint MIN_DIST;\nint FREQ;\ndouble F_THRESHOLD;\nint SHOW_TRACK;\nint EQUALIZE;\nint FISHEYE;\nstd::string FISHEYE_MASK;\nstd::string CAM_NAMES;\nint STEREO_TRACK;\nbool PUB_THIS_FRAME;\nEigen::Matrix3d Ric;\n\ndouble ROW, COL;\nint IMAGE_SIZE;\ndouble TD, TR;\n\ndouble DEPTH_MIN_DIST;\ndouble DEPTH_MAX_DIST;\nunsigned short DEPTH_MIN_DIST_MM;\nunsigned short DEPTH_MAX_DIST_MM;\n\nstd::vector SEMANTIC_LABEL;\nstd::vector STATIC_LABEL;\nstd::vector DYNAMIC_LABEL;\n\nint NUM_GRID_ROWS;\nint NUM_GRID_COLS;\n\nint FRONTEND_FREQ;\nint USE_IMU;\nint NUM_THREADS;\nint STATIC_INIT;\n\nint FIX_DEPTH;\n\ntemplate T readParam(ros::NodeHandle &n, std::string name)\n{\n T ans;\n if (n.getParam(name, ans))\n {\n ROS_INFO_STREAM(\"Loaded \" << name << \": \" << ans);\n }\n else\n {\n ROS_ERROR_STREAM(\"Failed to load \" << name);\n n.shutdown();\n }\n return ans;\n}\n\n// https://blog.csdn.net/qq_16952303/article/details/80259660\nvoid readParameters(ros::NodeHandle &n)\n{\n std::string config_file;\n config_file = readParam(n, \"config_file\");\n cv::FileStorage fsSettings;\n fsSettings.open(config_file, cv::FileStorage::READ);\n if (!fsSettings.isOpened())\n {\n std::cerr << \"ERROR: Wrong path to settings\" << std::endl;\n }\n\n NUM_THREADS = fsSettings[\"num_threads\"];\n if (NUM_THREADS <= 1)\n {\n NUM_THREADS = std::thread::hardware_concurrency();\n }\n\n fsSettings[\"image_topic\"] >> IMAGE_TOPIC;\n fsSettings[\"depth_topic\"] >> DEPTH_TOPIC;\n\n MAX_CNT = fsSettings[\"max_cnt\"];\n\n MAX_CNT_SET = MAX_CNT;\n\n MIN_DIST = fsSettings[\"min_dist\"];\n\n FREQ = fsSettings[\"freq\"];\n F_THRESHOLD = fsSettings[\"F_threshold\"];\n SHOW_TRACK = fsSettings[\"show_track\"];\n EQUALIZE = fsSettings[\"equalize\"];\n FISHEYE = fsSettings[\"fisheye\"];\n std::string VINS_FOLDER_PATH = readParam(n, \"vins_folder\");\n if (FISHEYE == 1)\n FISHEYE_MASK = VINS_FOLDER_PATH + \"config/fisheye_mask.jpg\";\n CAM_NAMES = config_file;\n\n DEPTH_MIN_DIST = fsSettings[\"depth_min_dist\"];\n DEPTH_MAX_DIST = fsSettings[\"depth_max_dist\"];\n\n DEPTH_MIN_DIST_MM = DEPTH_MIN_DIST * 1000;\n DEPTH_MAX_DIST_MM = DEPTH_MAX_DIST * 1000;\n\n NUM_GRID_ROWS = fsSettings[\"num_grid_rows\"];\n NUM_GRID_COLS = fsSettings[\"num_grid_cols\"];\n ROS_INFO(\"NUM_GRID_ROWS: %d, NUM_GRID_COLS: %d\", NUM_GRID_ROWS, NUM_GRID_COLS);\n\n FRONTEND_FREQ = fsSettings[\"frontend_freq\"];\n ROS_INFO(\"FRONTEND_FREQ: %d\", FRONTEND_FREQ);\n\n STEREO_TRACK = false;\n PUB_THIS_FRAME = false;\n\n if (FREQ == 0)\n FREQ = 100;\n\n SOLVER_TIME = fsSettings[\"max_solver_time\"];\n NUM_ITERATIONS = fsSettings[\"max_num_iterations\"];\n MIN_PARALLAX = fsSettings[\"keyframe_parallax\"];\n ROS_INFO(\"keyframe_parallax: %f\", MIN_PARALLAX);\n MIN_PARALLAX = MIN_PARALLAX / FOCAL_LENGTH;\n\n std::string OUTPUT_PATH;\n fsSettings[\"output_path\"] >> OUTPUT_PATH;\n VINS_RESULT_PATH = OUTPUT_PATH + \"/vins_result_no_loop.csv\";\n std::ofstream fout(VINS_RESULT_PATH, std::ios::out);\n fout.close();\n\n USE_IMU = fsSettings[\"imu\"];\n ROS_INFO(\"USE_IMU: %d\\n\", USE_IMU);\n if (USE_IMU)\n {\n fsSettings[\"imu_topic\"] >> IMU_TOPIC;\n printf(\"IMU_TOPIC: %s\\n\", IMU_TOPIC.c_str());\n ACC_N = fsSettings[\"acc_n\"];\n ACC_W = fsSettings[\"acc_w\"];\n GYR_N = fsSettings[\"gyr_n\"];\n GYR_W = fsSettings[\"gyr_w\"];\n G.z() = fsSettings[\"g_norm\"];\n }\n\n ROW = fsSettings[\"image_height\"];\n COL = fsSettings[\"image_width\"];\n IMAGE_SIZE = ROW * COL;\n ROS_INFO(\"ROW: %f COL: %f \", ROW, COL);\n\n for (auto iter : fsSettings[\"semantic_label\"])\n {\n SEMANTIC_LABEL.emplace_back(iter.string());\n }\n\n for (auto iter : fsSettings[\"static_label\"])\n {\n STATIC_LABEL.emplace_back(iter.string());\n }\n\n for (auto iter : fsSettings[\"dynamic_label\"])\n {\n DYNAMIC_LABEL.emplace_back(iter.string());\n }\n\n ESTIMATE_EXTRINSIC = fsSettings[\"estimate_extrinsic\"];\n if (ESTIMATE_EXTRINSIC == 2)\n {\n ROS_WARN(\"have no prior about extrinsic param, calibrate extrinsic param\");\n RIC.emplace_back(Eigen::Matrix3d::Identity());\n TIC.emplace_back(Eigen::Vector3d::Zero());\n EX_CALIB_RESULT_PATH = OUTPUT_PATH + \"/extrinsic_parameter.txt\";\n }\n else\n {\n if (ESTIMATE_EXTRINSIC == 1)\n {\n ROS_WARN(\" Optimize extrinsic param around initial guess!\");\n EX_CALIB_RESULT_PATH = OUTPUT_PATH + \"/extrinsic_parameter.txt\";\n }\n if (ESTIMATE_EXTRINSIC == 0)\n ROS_WARN(\" fix extrinsic param \");\n\n cv::Mat cv_R, cv_T;\n fsSettings[\"extrinsicRotation\"] >> cv_R;\n fsSettings[\"extrinsicTranslation\"] >> cv_T;\n Eigen::Matrix3d eigen_R;\n Eigen::Vector3d eigen_T;\n cv::cv2eigen(cv_R, eigen_R);\n cv::cv2eigen(cv_T, eigen_T);\n Eigen::Quaterniond Q(eigen_R);\n eigen_R = Q.normalized();\n Ric = eigen_R;\n RIC.push_back(eigen_R);\n TIC.push_back(eigen_T);\n ROS_INFO_STREAM(\"Extrinsic_R : \" << std::endl << RIC[0]);\n ROS_INFO_STREAM(\"Extrinsic_T : \" << std::endl << TIC[0].transpose());\n }\n\n INIT_DEPTH = 5.0;\n BIAS_ACC_THRESHOLD = 0.1;\n BIAS_GYR_THRESHOLD = 0.1;\n\n TD = fsSettings[\"td\"];\n ESTIMATE_TD = fsSettings[\"estimate_td\"];\n if (ESTIMATE_TD)\n ROS_INFO_STREAM(\"Unsynchronized sensors, online estimate time offset, initial td: \" << TD);\n else\n ROS_INFO_STREAM(\"Synchronized sensors, fix time offset: \" << TD);\n\n ROLLING_SHUTTER = fsSettings[\"rolling_shutter\"];\n if (ROLLING_SHUTTER)\n {\n TR = fsSettings[\"rolling_shutter_tr\"];\n ROS_INFO_STREAM(\"rolling shutter camera, read out time per line: \" << TR);\n }\n else\n {\n TR = 0;\n }\n\n STATIC_INIT = fsSettings[\"static_init\"];\n if (!fsSettings[\"fix_depth\"].empty())\n FIX_DEPTH = fsSettings[\"fix_depth\"];\n else\n FIX_DEPTH = 1;\n fsSettings.release();\n}\n" }, { "path": "vins_estimator/src/utility/parameters.h", "content": "#pragma once\n\n#include \"utility.h\"\n#include \n#include \n#include \n#include \n#include \n#include \n\nconst double FOCAL_LENGTH = 460.0;\nconst int WINDOW_SIZE = 10;\nconst int NUM_OF_CAM = 1;\nconst int NUM_OF_F = 1000;\n//#define UNIT_SPHERE_ERROR\n\nextern double INIT_DEPTH;\nextern double MIN_PARALLAX;\nextern int ESTIMATE_EXTRINSIC;\n\nextern double ACC_N, ACC_W;\nextern double GYR_N, GYR_W;\n\nextern std::vector RIC;\nextern std::vector TIC;\nextern Eigen::Vector3d G;\n\nextern double BIAS_ACC_THRESHOLD;\nextern double BIAS_GYR_THRESHOLD;\nextern double SOLVER_TIME;\nextern int NUM_ITERATIONS;\nextern std::string EX_CALIB_RESULT_PATH;\nextern std::string VINS_RESULT_PATH;\nextern std::string IMAGE_TOPIC;\nextern std::string DEPTH_TOPIC;\nextern std::string IMU_TOPIC;\nextern double TD;\nextern double TR;\nextern int ESTIMATE_TD;\nextern int ROLLING_SHUTTER;\nextern double ROW, COL;\n\nextern int IMAGE_SIZE;\n\nextern double DEPTH_MIN_DIST;\nextern double DEPTH_MAX_DIST;\nextern unsigned short DEPTH_MIN_DIST_MM;\nextern unsigned short DEPTH_MAX_DIST_MM;\nextern int MAX_CNT;\nextern int MAX_CNT_SET;\nextern int MIN_DIST;\nextern int FREQ;\nextern double F_THRESHOLD;\nextern int SHOW_TRACK;\nextern int EQUALIZE;\nextern int FISHEYE;\nextern std::string FISHEYE_MASK;\nextern std::string CAM_NAMES;\nextern int STEREO_TRACK;\nextern bool PUB_THIS_FRAME;\nextern Eigen::Matrix3d Ric;\n\nextern std::vector SEMANTIC_LABEL;\nextern std::vector STATIC_LABEL;\nextern std::vector DYNAMIC_LABEL;\n\nextern int NUM_GRID_ROWS;\nextern int NUM_GRID_COLS;\n\nextern int FRONTEND_FREQ;\n\nextern int USE_IMU;\nextern int NUM_THREADS;\n\nextern int STATIC_INIT;\n\nextern int FIX_DEPTH;\n\nvoid readParameters(ros::NodeHandle &n);\n\nenum SIZE_PARAMETERIZATION\n{\n SIZE_POSE = 7,\n SIZE_SPEEDBIAS = 9,\n // SIZE_SPEED = 3,\n // SIZE_BIAS = 6,\n SIZE_FEATURE = 1\n};\n\nenum StateOrder\n{\n O_P = 0,\n O_R = 3,\n O_V = 6,\n O_BA = 9,\n O_BG = 12\n};\n\nenum NoiseOrder\n{\n O_AN = 0,\n O_GN = 3,\n O_AW = 6,\n O_GW = 9\n};\n" }, { "path": "vins_estimator/src/utility/tic_toc.h", "content": "#pragma once\n\n#include \n#include \n#include \n\nclass TicToc\n{\npublic:\n TicToc()\n {\n tic();\n }\n\n void tic()\n {\n start = std::chrono::system_clock::now();\n }\n\n double toc()\n {\n end = std::chrono::system_clock::now();\n std::chrono::duration elapsed_seconds = end - start;\n return elapsed_seconds.count() * 1000;\n }\n\nprivate:\n std::chrono::time_point start, end;\n};\n" }, { "path": "vins_estimator/src/utility/utility.cpp", "content": "#include \"utility.h\"\n\n// https://blog.csdn.net/huanghaihui_123/article/details/103075107#commentBox\n// R_W_I\nEigen::Matrix3d Utility::g2R(const Eigen::Vector3d &g)\n{\n Eigen::Matrix3d R0;\n Eigen::Vector3d ng1 = g.normalized();\n Eigen::Vector3d ng2{0, 0, 1.0};\n R0 = Eigen::Quaterniond::FromTwoVectors(ng1, ng2).toRotationMatrix();\n double yaw = Utility::R2ypr(R0).x();\n R0 = Utility::ypr2R(Eigen::Vector3d{-yaw, 0, 0}) * R0;\n // R0 = Utility::ypr2R(Eigen::Vector3d{-90, 0, 0}) * R0;\n return R0;\n}\n" }, { "path": "vins_estimator/src/utility/utility.h", "content": "#pragma once\n\n#include \n#include \n#include \n#include \n\nclass Utility\n{\n public:\n template \n static Eigen::Quaternion deltaQ(const Eigen::MatrixBase &theta)\n {\n typedef typename Derived::Scalar Scalar_t;\n\n Eigen::Quaternion dq;\n Eigen::Matrix half_theta = theta;\n half_theta /= static_cast(2.0);\n dq.w() = static_cast(1.0);\n dq.x() = half_theta.x();\n dq.y() = half_theta.y();\n dq.z() = half_theta.z();\n return dq;\n }\n\n template \n static Eigen::Matrix skewSymmetric(const Eigen::MatrixBase &q)\n {\n Eigen::Matrix ans;\n ans << typename Derived::Scalar(0), -q(2), q(1),\n q(2), typename Derived::Scalar(0), -q(0),\n -q(1), q(0), typename Derived::Scalar(0);\n return ans;\n }\n\n template \n static Eigen::Quaternion positify(const Eigen::QuaternionBase &q)\n {\n //printf(\"a: %f %f %f %f\", q.w(), q.x(), q.y(), q.z());\n //Eigen::Quaternion p(-q.w(), -q.x(), -q.y(), -q.z());\n //printf(\"b: %f %f %f %f\", p.w(), p.x(), p.y(), p.z());\n //return q.template w() >= (typename Derived::Scalar)(0.0) ? q : Eigen::Quaternion(-q.w(), -q.x(), -q.y(), -q.z());\n return q;\n }\n\n template \n static Eigen::Matrix Qleft(const Eigen::QuaternionBase &q)\n {\n Eigen::Quaternion qq = positify(q);\n Eigen::Matrix ans;\n ans(0, 0) = qq.w(), ans.template block<1, 3>(0, 1) = -qq.vec().transpose();\n ans.template block<3, 1>(1, 0) = qq.vec(), ans.template block<3, 3>(1, 1) = qq.w() * Eigen::Matrix::Identity() + skewSymmetric(qq.vec());\n return ans;\n }\n\n template \n static Eigen::Matrix Qright(const Eigen::QuaternionBase &p)\n {\n Eigen::Quaternion pp = positify(p);\n Eigen::Matrix ans;\n ans(0, 0) = pp.w(), ans.template block<1, 3>(0, 1) = -pp.vec().transpose();\n ans.template block<3, 1>(1, 0) = pp.vec(), ans.template block<3, 3>(1, 1) = pp.w() * Eigen::Matrix::Identity() - skewSymmetric(pp.vec());\n return ans;\n }\n\n static Eigen::Vector3d R2ypr(const Eigen::Matrix3d &R)\n {\n Eigen::Vector3d n = R.col(0);\n Eigen::Vector3d o = R.col(1);\n Eigen::Vector3d a = R.col(2);\n\n Eigen::Vector3d ypr(3);\n double y = atan2(n(1), n(0));\n double p = atan2(-n(2), n(0) * cos(y) + n(1) * sin(y));\n double r = atan2(a(0) * sin(y) - a(1) * cos(y), -o(0) * sin(y) + o(1) * cos(y));\n ypr(0) = y;\n ypr(1) = p;\n ypr(2) = r;\n\n return ypr / M_PI * 180.0;\n }\n\n template \n static Eigen::Matrix ypr2R(const Eigen::MatrixBase &ypr)\n {\n typedef typename Derived::Scalar Scalar_t;\n\n Scalar_t y = ypr(0) / 180.0 * M_PI;\n Scalar_t p = ypr(1) / 180.0 * M_PI;\n Scalar_t r = ypr(2) / 180.0 * M_PI;\n\n Eigen::Matrix Rz;\n Rz << cos(y), -sin(y), 0,\n sin(y), cos(y), 0,\n 0, 0, 1;\n\n Eigen::Matrix Ry;\n Ry << cos(p), 0., sin(p),\n 0., 1., 0.,\n -sin(p), 0., cos(p);\n\n Eigen::Matrix Rx;\n Rx << 1., 0., 0.,\n 0., cos(r), -sin(r),\n 0., sin(r), cos(r);\n\n return Rz * Ry * Rx;\n }\n\n static Eigen::Matrix3d g2R(const Eigen::Vector3d &g);\n\n template \n struct uint_\n {\n };\n\n template \n void unroller(const Lambda &f, const IterT &iter, uint_)\n {\n unroller(f, iter, uint_());\n f(iter + N);\n }\n\n template \n void unroller(const Lambda &f, const IterT &iter, uint_<0>)\n {\n f(iter);\n }\n\n template \n static T normalizeAngle(const T& angle_degrees) {\n T two_pi(2.0 * 180);\n if (angle_degrees > 0)\n return angle_degrees -\n two_pi * std::floor((angle_degrees + T(180)) / two_pi);\n else\n return angle_degrees +\n two_pi * std::floor((-angle_degrees + T(180)) / two_pi);\n };\n};\n" }, { "path": "vins_estimator/src/utility/visualization.cpp", "content": "#include \"visualization.h\"\n#include \"parameters.h\"\n\nusing namespace ros;\nusing namespace Eigen;\nros::Publisher pub_odometry, pub_latest_odometry;\nros::Publisher pub_path;\nros::Publisher pub_relo_path;\nros::Publisher pub_point_cloud, pub_margin_cloud;\nros::Publisher pub_key_poses;\nros::Publisher pub_relo_relative_pose;\nros::Publisher pub_camera_pose;\nros::Publisher pub_imu_pose;\nros::Publisher pub_camera_pose_visual;\nnav_msgs::Path path, relo_path;\n\nros::Publisher pub_keyframe_pose;\nros::Publisher pub_keyframe_point;\nros::Publisher pub_extrinsic;\n\nros::Publisher pub_delta_v;\nros::Publisher pub_delta_p;\nros::Publisher pub_delta_t;\n\nros::Publisher pub_match, pub_semantic;\n\nCameraPoseVisualization cameraposevisual(0, 1, 0, 1);\nCameraPoseVisualization keyframebasevisual(0.0, 0.0, 1.0, 1.0);\nstatic double sum_of_path = 0;\nstatic Vector3d last_path(0.0, 0.0, 0.0);\nvector prev_pcd_id;\ndouble pubOdometry_current_time = -1;\n\nvoid registerPub(ros::NodeHandle &n)\n{\n pub_latest_odometry = n.advertise(\"/vins_estimator/imu_propagate\", 5);\n pub_path = n.advertise(\"/vins_estimator/path\", 1);\n pub_odometry = n.advertise(\"/vins_estimator/odometry\", 5);\n pub_key_poses = n.advertise(\"/vins_estimator/key_poses\", 5);\n pub_camera_pose = n.advertise(\"/vins_estimator/camera_pose\", 5);\n pub_camera_pose_visual =\n n.advertise(\"/vins_estimator/camera_pose_visual\", 5);\n pub_extrinsic = n.advertise(\"/vins_estimator/extrinsic\", 5);\n pub_relo_relative_pose =\n n.advertise(\"/vins_estimator/relo_relative_pose\", 5);\n\n pub_match = n.advertise(\"/vins_estimator/feature_img\", 1);\n pub_semantic = n.advertise(\"/vins_estimator/semantic_mask\", 1);\n\n pub_relo_path = n.advertise(\"/vins_estimator/relocalization_path\", 1);\n pub_delta_p = n.advertise(\"/vins_estimator/delta_p\", 1);\n pub_delta_v = n.advertise(\"/vins_estimator/delta_v\", 1);\n pub_delta_t = n.advertise(\"/vins_estimator/delta_t\", 1);\n pub_keyframe_pose = n.advertise(\"/vins_estimator/keyframe_pose\", 1);\n pub_keyframe_point = n.advertise(\"/vins_estimator/keyframe_point\", 1);\n pub_point_cloud = n.advertise(\"/vins_estimator/point_cloud\", 1);\n pub_margin_cloud = n.advertise(\"/vins_estimator/history_cloud\", 1);\n pub_imu_pose = n.advertise(\"/vins_estimator/imu_pose\", 5);\n\n cameraposevisual.setScale(1);\n cameraposevisual.setLineWidth(0.05);\n keyframebasevisual.setScale(0.1);\n keyframebasevisual.setLineWidth(0.01);\n}\n\nvoid pubLatestOdometry(const Eigen::Vector3d &P, const Eigen::Quaterniond &Q,\n const Eigen::Vector3d &V, const double &t)\n{\n nav_msgs::Odometry odometry;\n odometry.header.stamp = ros::Time(t);\n odometry.header.frame_id = \"map\";\n odometry.pose.pose.position.x = P.x();\n odometry.pose.pose.position.y = P.y();\n odometry.pose.pose.position.z = P.z();\n odometry.pose.pose.orientation.x = Q.x();\n odometry.pose.pose.orientation.y = Q.y();\n odometry.pose.pose.orientation.z = Q.z();\n odometry.pose.pose.orientation.w = Q.w();\n\n odometry.twist.twist.linear.x = V.x();\n odometry.twist.twist.linear.y = V.y();\n odometry.twist.twist.linear.z = V.z();\n\n // Eigen::Vector3d a_center = A - omega.cross(V);\n // odometry.twist.twist.angular.x = a_center.x();\n // odometry.twist.twist.angular.y = a_center.y();\n // odometry.twist.twist.angular.z = a_center.z();\n\n pub_latest_odometry.publish(odometry);\n\n geometry_msgs::PoseStamped pose;\n pose.header = odometry.header;\n pose.pose = odometry.pose.pose;\n pub_imu_pose.publish(pose);\n}\n\nvoid printStatistics(const Estimator &estimator, double t)\n{\n if (estimator.solver_flag != Estimator::SolverFlag::NON_LINEAR)\n return;\n printf(\"position: %f, %f, %f\\r\", estimator.Ps[WINDOW_SIZE].x(), estimator.Ps[WINDOW_SIZE].y(),\n estimator.Ps[WINDOW_SIZE].z());\n\n if (SHOW_TRACK)\n {\n // ROS_DEBUG_STREAM(\"position: \" << estimator.Ps[WINDOW_SIZE].transpose());\n // ROS_DEBUG_STREAM(\"orientation: \" << estimator.Vs[WINDOW_SIZE].transpose());\n for (int i = 0; i < NUM_OF_CAM; i++)\n {\n // ROS_DEBUG(\"calibration result for camera %d\", i);\n ROS_DEBUG_STREAM(\"extirnsic tic: \" << estimator.tic[i].transpose());\n ROS_DEBUG_STREAM(\"extrinsic ric: \" << Utility::R2ypr(estimator.ric[i]).transpose());\n if (ESTIMATE_EXTRINSIC)\n {\n cv::FileStorage fs(EX_CALIB_RESULT_PATH, cv::FileStorage::WRITE);\n Eigen::Matrix3d eigen_R;\n Eigen::Vector3d eigen_T;\n eigen_R = estimator.ric[i];\n eigen_T = estimator.tic[i];\n cv::Mat cv_R, cv_T;\n cv::eigen2cv(eigen_R, cv_R);\n cv::eigen2cv(eigen_T, cv_T);\n fs << \"extrinsicRotation\" << cv_R << \"extrinsicTranslation\" << cv_T;\n fs.release();\n }\n }\n }\n\n ROS_DEBUG(\"vo solver costs: %f ms\", t);\n\n sum_of_path += (estimator.Ps[WINDOW_SIZE] - last_path).norm();\n last_path = estimator.Ps[WINDOW_SIZE];\n ROS_DEBUG(\"sum of path %f\", sum_of_path);\n if (ESTIMATE_TD)\n ROS_DEBUG(\"td %f\", estimator.td);\n}\n\nvoid pubOdometry(const Estimator &estimator, const std_msgs::Header &header)\n{\n if (pubOdometry_current_time < 0)\n {\n pubOdometry_current_time = header.stamp.toSec();\n }\n double dt = header.stamp.toSec() - pubOdometry_current_time;\n pubOdometry_current_time = header.stamp.toSec();\n\n if (estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR)\n {\n nav_msgs::Odometry odometry;\n odometry.header = header;\n odometry.header.frame_id = \"map\";\n odometry.child_frame_id = \"map\";\n Quaterniond tmp_Q;\n tmp_Q = Quaterniond(estimator.Rs[WINDOW_SIZE]);\n odometry.pose.pose.position.x = estimator.Ps[WINDOW_SIZE].x();\n odometry.pose.pose.position.y = estimator.Ps[WINDOW_SIZE].y();\n odometry.pose.pose.position.z = estimator.Ps[WINDOW_SIZE].z();\n odometry.pose.pose.orientation.x = tmp_Q.x();\n odometry.pose.pose.orientation.y = tmp_Q.y();\n odometry.pose.pose.orientation.z = tmp_Q.z();\n odometry.pose.pose.orientation.w = tmp_Q.w();\n odometry.twist.twist.linear.x = estimator.Vs[WINDOW_SIZE].x();\n odometry.twist.twist.linear.y = estimator.Vs[WINDOW_SIZE].y();\n odometry.twist.twist.linear.z = estimator.Vs[WINDOW_SIZE].z();\n //\"odometry\"\n pub_odometry.publish(odometry);\n\n Vector3d delta_p = estimator.Ps[WINDOW_SIZE] - estimator.Ps[WINDOW_SIZE - 2];\n Vector3d delta_v = estimator.Vs[WINDOW_SIZE] - estimator.Vs[WINDOW_SIZE - 2];\n // 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());\n // ROS_ERROR(\"norm :%f|%f\",delta_p.norm(),delta_v.norm());\n // ROS_ERROR(\"delta_t :%f\",dt);\n std_msgs::Float64 rosp;\n std_msgs::Float64 rosv;\n std_msgs::Float64 rost;\n rosp.data = delta_p.norm();\n rosv.data = delta_v.norm();\n rost.data = dt;\n pub_delta_p.publish(rosp);\n pub_delta_v.publish(rosv);\n pub_delta_t.publish(rost);\n\n geometry_msgs::PoseStamped pose_stamped;\n pose_stamped.header = header;\n pose_stamped.header.frame_id = \"map\";\n pose_stamped.pose = odometry.pose.pose;\n path.header = header;\n path.header.frame_id = \"map\";\n path.poses.push_back(pose_stamped);\n //\"path\"\n pub_path.publish(path);\n\n Vector3d correct_t;\n Vector3d correct_v;\n Quaterniond correct_q;\n correct_t =\n estimator.drift_correct_r * estimator.Ps[WINDOW_SIZE] + estimator.drift_correct_t;\n correct_q = estimator.drift_correct_r * estimator.Rs[WINDOW_SIZE];\n odometry.pose.pose.position.x = correct_t.x();\n odometry.pose.pose.position.y = correct_t.y();\n odometry.pose.pose.position.z = correct_t.z();\n odometry.pose.pose.orientation.x = correct_q.x();\n odometry.pose.pose.orientation.y = correct_q.y();\n odometry.pose.pose.orientation.z = correct_q.z();\n odometry.pose.pose.orientation.w = correct_q.w();\n\n pose_stamped.pose = odometry.pose.pose;\n relo_path.header = header;\n relo_path.header.frame_id = \"map\";\n relo_path.poses.push_back(pose_stamped);\n //\"relocalization_path\"\n pub_relo_path.publish(relo_path);\n\n // write result to file\n ofstream foutC(VINS_RESULT_PATH, ios::app);\n foutC.setf(ios::fixed, ios::floatfield);\n foutC.precision(0);\n foutC << header.stamp.toSec() * 1e9 << \",\";\n foutC.precision(5);\n foutC << estimator.Ps[WINDOW_SIZE].x() << \",\" << estimator.Ps[WINDOW_SIZE].y() << \",\"\n << estimator.Ps[WINDOW_SIZE].z() << \",\" << tmp_Q.w() << \",\" << tmp_Q.x() << \",\"\n << tmp_Q.y() << \",\" << tmp_Q.z() << \",\" << estimator.Vs[WINDOW_SIZE].x() << \",\"\n << estimator.Vs[WINDOW_SIZE].y() << \",\" << estimator.Vs[WINDOW_SIZE].z() << \",\"\n << endl;\n foutC.close();\n }\n}\n\nvoid pubKeyPoses(const Estimator &estimator, const std_msgs::Header &header)\n{\n if (estimator.key_poses.empty())\n return;\n visualization_msgs::Marker key_poses;\n key_poses.header = header;\n key_poses.header.frame_id = \"map\";\n key_poses.ns = \"key_poses\";\n key_poses.type = visualization_msgs::Marker::SPHERE_LIST;\n key_poses.action = visualization_msgs::Marker::ADD;\n key_poses.pose.orientation.w = 1.0;\n key_poses.lifetime = ros::Duration();\n\n // static int key_poses_id = 0;\n key_poses.id = 0; // key_poses_id++;\n key_poses.scale.x = 0.05;\n key_poses.scale.y = 0.05;\n key_poses.scale.z = 0.05;\n key_poses.color.r = 1.0;\n key_poses.color.a = 1.0;\n\n for (int i = 0; i <= WINDOW_SIZE; i++)\n {\n geometry_msgs::Point pose_marker;\n Vector3d correct_pose;\n correct_pose = estimator.key_poses[i];\n pose_marker.x = correct_pose.x();\n pose_marker.y = correct_pose.y();\n pose_marker.z = correct_pose.z();\n key_poses.points.push_back(pose_marker);\n }\n //\"key_poses\"\n pub_key_poses.publish(key_poses);\n}\n\nvoid pubCameraPose(const Estimator &estimator, const std_msgs::Header &header)\n{\n int idx2 = WINDOW_SIZE - 1;\n\n if (estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR)\n {\n int i = idx2;\n Vector3d P = estimator.Ps[i] + estimator.Rs[i] * estimator.tic[0];\n Quaterniond R = Quaterniond(estimator.Rs[i] * estimator.ric[0]);\n\n geometry_msgs::PoseStamped pose_stamped;\n pose_stamped.header = header;\n pose_stamped.header.frame_id = \"map\";\n pose_stamped.pose.position.x = P.x();\n pose_stamped.pose.position.y = P.y();\n pose_stamped.pose.position.z = P.z();\n\n pose_stamped.pose.orientation.x = R.x();\n pose_stamped.pose.orientation.y = R.y();\n pose_stamped.pose.orientation.z = R.z();\n pose_stamped.pose.orientation.w = R.w();\n\n //\"camera_pose\"\n pub_camera_pose.publish(pose_stamped);\n\n cameraposevisual.reset();\n cameraposevisual.add_pose(P, R);\n //\"camera_pose_visual\"\n cameraposevisual.publish_by(pub_camera_pose_visual, pose_stamped.header);\n\n // \"sliding window pose visual\"\n // cameraposevisual.reset();\n // for (; idx2 >= 0; idx2--)\n // {\n // Vector3d P = estimator.Ps[idx2] + estimator.Rs[idx2] * estimator.tic[0];\n // Quaterniond R = Quaterniond(estimator.Rs[idx2] * estimator.ric[0]);\n // cameraposevisual.add_pose(P, R);\n // }\n // cameraposevisual.publish_by(pub_camera_pose_visual, pose_stamped.header);\n }\n}\n\nvoid pubIMUPose(const Estimator &estimator, const std_msgs::Header &header)\n{\n if (estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR)\n {\n Vector3d P = estimator.Ps[WINDOW_SIZE];\n Quaterniond R = Quaterniond(estimator.Rs[WINDOW_SIZE]);\n\n geometry_msgs::PoseStamped pose_stamped;\n pose_stamped.header = header;\n pose_stamped.header.frame_id = \"map\";\n pose_stamped.pose.position.x = P.x();\n pose_stamped.pose.position.y = P.y();\n pose_stamped.pose.position.z = P.z();\n pose_stamped.pose.orientation.x = R.x();\n pose_stamped.pose.orientation.y = R.y();\n pose_stamped.pose.orientation.z = R.z();\n pose_stamped.pose.orientation.w = R.w();\n\n pub_imu_pose.publish(pose_stamped);\n }\n}\n\nvoid pubPointCloud(const Estimator &estimator, const std_msgs::Header &header)\n{\n sensor_msgs::PointCloudPtr point_cloud_ptr(new sensor_msgs::PointCloud);\n point_cloud_ptr->header = header;\n\n for (auto &it_per_id : estimator.f_manager.feature)\n {\n if (it_per_id.is_dynamic)\n continue;\n int used_num;\n used_num = it_per_id.feature_per_frame.size();\n // if (used_num < 4)\n // continue;\n if (!(used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))\n continue;\n if (it_per_id.start_frame > WINDOW_SIZE * 3.0 / 4.0 || it_per_id.solve_flag != 1)\n continue;\n\n int imu_i = it_per_id.start_frame;\n double depth = it_per_id.feature_per_frame[0].depth;\n // camera coordinate\n Vector3d pts_i = depth == 0 ?\n it_per_id.feature_per_frame[0].point * it_per_id.estimated_depth :\n it_per_id.feature_per_frame[0].point * depth;\n // C_imu = extrinsicRotation * C_cam + extrinsicTranslation\n // C_world = extrinsicRotation * C_imu + extrinsicTranslation\n Vector3d w_pts_i = estimator.Rs[imu_i] * (estimator.ric[0] * pts_i + estimator.tic[0]) +\n estimator.Ps[imu_i];\n // ROS_ERROR(\"x: %f y: %f z: %f depth: %f\",\n // pts_i(0),pts_i(1),pts_i(2),depth);\n geometry_msgs::Point32 p;\n p.x = w_pts_i(0);\n p.y = w_pts_i(1);\n p.z = w_pts_i(2);\n point_cloud_ptr->points.push_back(p);\n }\n //\"point_cloud_ptr\"\n pub_point_cloud.publish(point_cloud_ptr);\n\n // pub margined potin\n sensor_msgs::PointCloudPtr margin_cloud_ptr(new sensor_msgs::PointCloud);\n margin_cloud_ptr->header = header;\n\n for (auto &it_per_id : estimator.f_manager.feature)\n {\n if (it_per_id.is_dynamic)\n continue;\n int used_num;\n used_num = it_per_id.feature_per_frame.size();\n // if (used_num < 4)\n // continue;\n if (!(used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))\n continue;\n // if (it_per_id->start_frame > WINDOW_SIZE * 3.0 / 4.0 ||\n // it_per_id->solve_flag != 1)\n // continue;\n\n if (it_per_id.start_frame == 0 && it_per_id.feature_per_frame.size() <= 2 &&\n it_per_id.solve_flag == 1)\n {\n int imu_i = it_per_id.start_frame;\n double depth = it_per_id.feature_per_frame[0].depth;\n Vector3d pts_i = depth == 0 ?\n it_per_id.feature_per_frame[0].point * it_per_id.estimated_depth :\n it_per_id.feature_per_frame[0].point * depth;\n // std::cout<<\"pts_i(0):\"<points.push_back(p);\n }\n }\n //\"history_cloud\"\n pub_margin_cloud.publish(margin_cloud_ptr);\n}\n\nvoid pubTF(const Estimator &estimator, const std_msgs::Header &header)\n{\n if (estimator.solver_flag != Estimator::SolverFlag::NON_LINEAR)\n return;\n static tf::TransformBroadcaster br;\n tf::Transform transform;\n tf::Quaternion q;\n // body frame\n Vector3d correct_t;\n Quaterniond correct_q;\n correct_t = estimator.Ps[WINDOW_SIZE];\n correct_q = estimator.Rs[WINDOW_SIZE];\n\n transform.setOrigin(tf::Vector3(correct_t(0), correct_t(1), correct_t(2)));\n q.setW(correct_q.w());\n q.setX(correct_q.x());\n q.setY(correct_q.y());\n q.setZ(correct_q.z());\n transform.setRotation(q);\n br.sendTransform(tf::StampedTransform(transform, header.stamp, \"map\", \"body\"));\n\n // camera frame\n transform.setOrigin(\n tf::Vector3(estimator.tic[0].x(), estimator.tic[0].y(), estimator.tic[0].z()));\n q.setW(Quaterniond(estimator.ric[0]).w());\n q.setX(Quaterniond(estimator.ric[0]).x());\n q.setY(Quaterniond(estimator.ric[0]).y());\n q.setZ(Quaterniond(estimator.ric[0]).z());\n transform.setRotation(q);\n br.sendTransform(tf::StampedTransform(transform, header.stamp, \"body\", \"camera\"));\n\n nav_msgs::Odometry odometry;\n odometry.header = header;\n odometry.header.frame_id = \"map\";\n odometry.pose.pose.position.x = estimator.tic[0].x();\n odometry.pose.pose.position.y = estimator.tic[0].y();\n odometry.pose.pose.position.z = estimator.tic[0].z();\n Quaterniond tmp_q{estimator.ric[0]};\n odometry.pose.pose.orientation.x = tmp_q.x();\n odometry.pose.pose.orientation.y = tmp_q.y();\n odometry.pose.pose.orientation.z = tmp_q.z();\n odometry.pose.pose.orientation.w = tmp_q.w();\n pub_extrinsic.publish(odometry);\n}\n\nvoid pubKeyframe(const Estimator &estimator)\n{\n // pub camera pose, 2D-3D points of keyframe\n if (estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR &&\n estimator.marginalization_flag == 0)\n {\n int i = WINDOW_SIZE - 2;\n // Vector3d P = estimator.Ps[i] + estimator.Rs[i] * estimator.tic[0];\n Vector3d P = estimator.Ps[i];\n Quaterniond R = Quaterniond(estimator.Rs[i]);\n\n nav_msgs::Odometry odometry;\n odometry.header.stamp = ros::Time(estimator.Headers[WINDOW_SIZE - 2]);\n odometry.header.frame_id = \"map\";\n odometry.pose.pose.position.x = P.x();\n odometry.pose.pose.position.y = P.y();\n odometry.pose.pose.position.z = P.z();\n odometry.pose.pose.orientation.x = R.x();\n odometry.pose.pose.orientation.y = R.y();\n odometry.pose.pose.orientation.z = R.z();\n odometry.pose.pose.orientation.w = R.w();\n // printf(\"time: %f t: %f %f %f r: %f %f %f %f\\n\",\n // odometry.header.stamp.toSec(), P.x(), P.y(), P.z(), R.w(), R.x(), R.y(),\n // R.z()); \"keyframe_pose\"\n pub_keyframe_pose.publish(odometry);\n\n sensor_msgs::PointCloudPtr point_cloud_ptr(new sensor_msgs::PointCloud);\n point_cloud_ptr->header.stamp = ros::Time(estimator.Headers[WINDOW_SIZE - 2]);\n for (auto &it_per_id : estimator.f_manager.feature)\n {\n int frame_size = it_per_id.feature_per_frame.size();\n if (it_per_id.start_frame < WINDOW_SIZE - 2 &&\n it_per_id.start_frame + frame_size - 1 >= WINDOW_SIZE - 2 &&\n it_per_id.solve_flag == 1)\n {\n int imu_i = it_per_id.start_frame;\n double depth = it_per_id.feature_per_frame[0].depth;\n depth = depth == 0 ? it_per_id.estimated_depth : depth;\n // a limit on distance, avoid large drifts\n // if (depth < DEPTH_MAX_DIST)\n {\n Vector3d pts_i = it_per_id.feature_per_frame[0].point * depth;\n Vector3d w_pts_i =\n estimator.Rs[imu_i] * (estimator.ric[0] * pts_i + estimator.tic[0]) +\n estimator.Ps[imu_i];\n geometry_msgs::Point32 p;\n p.x = w_pts_i(0);\n p.y = w_pts_i(1);\n p.z = w_pts_i(2);\n point_cloud_ptr->points.push_back(p);\n\n int imu_j = WINDOW_SIZE - 2 - it_per_id.start_frame;\n sensor_msgs::ChannelFloat32 p_2d;\n p_2d.values.push_back(it_per_id.feature_per_frame[imu_j].point.x());\n p_2d.values.push_back(it_per_id.feature_per_frame[imu_j].point.y());\n p_2d.values.push_back(it_per_id.feature_per_frame[imu_j].uv.x());\n p_2d.values.push_back(it_per_id.feature_per_frame[imu_j].uv.y());\n p_2d.values.push_back(it_per_id.feature_id);\n point_cloud_ptr->channels.push_back(p_2d);\n }\n }\n }\n //\"keyframe_point\"\n // now have more points than origin\n pub_keyframe_point.publish(point_cloud_ptr);\n }\n}\n\nvoid pubRelocalization(const Estimator &estimator)\n{\n nav_msgs::Odometry odometry;\n odometry.header.stamp = ros::Time(estimator.relo_frame_stamp);\n odometry.header.frame_id = \"map\";\n odometry.pose.pose.position.x = estimator.relo_relative_t.x();\n odometry.pose.pose.position.y = estimator.relo_relative_t.y();\n odometry.pose.pose.position.z = estimator.relo_relative_t.z();\n odometry.pose.pose.orientation.x = estimator.relo_relative_q.x();\n odometry.pose.pose.orientation.y = estimator.relo_relative_q.y();\n odometry.pose.pose.orientation.z = estimator.relo_relative_q.z();\n odometry.pose.pose.orientation.w = estimator.relo_relative_q.w();\n odometry.twist.twist.linear.x = estimator.relo_relative_yaw;\n odometry.twist.twist.linear.y = estimator.relo_frame_index;\n //\"relo_relative_pose\"\n pub_relo_relative_pose.publish(odometry);\n}\n\nvoid pubTrackImg(const cv_bridge::CvImageConstPtr &img_ptr)\n{\n sensor_msgs::CompressedImage image_up;\n image_up.format = \"jpeg\";\n std::vector encodeing;\n cv::imencode(\".jpg\", img_ptr->image, encodeing);\n encodeing.swap(image_up.data);\n image_up.header = std_msgs::Header();\n\n pub_match.publish(image_up);\n}\n\nvoid pubTrackImg(const sensor_msgs::ImagePtr &img_msg)\n{\n sensor_msgs::CompressedImage image_up;\n image_up.format = \"jpeg\";\n std::vector encodeing;\n cv::imencode(\".jpg\", img_msg->data, encodeing);\n encodeing.swap(image_up.data);\n image_up.header = std_msgs::Header();\n\n pub_match.publish(image_up);\n}\n\nvoid pubTrackImg(const cv::Mat &img)\n{\n // sensor_msgs::CompressedImage image_up;\n // image_up.format = \"jpeg\";\n // std::vector encodeing;\n // cv::imencode(\".jpg\", img, encodeing);\n // encodeing.swap(image_up.data);\n // image_up.header = std_msgs::Header();\n //\n // pub_match.publish(image_up);\n if (!img.empty())\n pub_match.publish(cv_bridge::CvImage(std_msgs::Header(), \"bgr8\", img).toImageMsg());\n}\n" }, { "path": "vins_estimator/src/utility/visualization.h", "content": "#pragma once\n\n#include \"../estimator/estimator.h\"\n#include \"CameraPoseVisualization.h\"\n#include \"parameters.h\"\n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n\n#include \n#include \n\nextern ros::Publisher pub_odometry;\nextern ros::Publisher pub_path, pub_pose;\nextern ros::Publisher pub_cloud, pub_map;\nextern ros::Publisher pub_key_poses;\nextern ros::Publisher pub_ref_pose, pub_cur_pose;\nextern ros::Publisher pub_key;\nextern nav_msgs::Path path;\nextern ros::Publisher pub_pose_graph;\nextern int IMAGE_ROW, IMAGE_COL;\n\nvoid registerPub(ros::NodeHandle &n);\n\nvoid pubLatestOdometry(const Eigen::Vector3d &P, const Eigen::Quaterniond &Q,\n const Eigen::Vector3d &V, const double &t);\n\n// void pubLatestOdometry(const Eigen::Vector3d &P, const Eigen::Quaterniond &Q,\n// const Eigen::Vector3d &V,\n// // const Eigen::Vector3d &A,\n// // const Eigen::Vector3d &omega,\n// const std_msgs::Header &header);\n\nvoid printStatistics(const Estimator &estimator, double t);\n\nvoid pubOdometry(const Estimator &estimator, const std_msgs::Header &header);\n\nvoid pubInitialGuess(const Estimator &estimator, const std_msgs::Header &header);\n\nvoid pubKeyPoses(const Estimator &estimator, const std_msgs::Header &header);\n\nvoid pubCameraPose(const Estimator &estimator, const std_msgs::Header &header);\n\nvoid pubIMUPose(const Estimator &estimator, const std_msgs::Header &header);\n\nvoid pubPointCloud(const Estimator &estimator, const std_msgs::Header &header);\n\nvoid pubTF(const Estimator &estimator, const std_msgs::Header &header);\n\nvoid pubKeyframe(const Estimator &estimator);\n\nvoid pubRelocalization(const Estimator &estimator);\n\nvoid pubTrackImg(const cv_bridge::CvImageConstPtr &img_ptr);\n\nvoid pubTrackImg(const sensor_msgs::ImagePtr &img_msg);\n\nvoid pubTrackImg(const cv::Mat &img);\n" } ]