Showing preview only (1,208K chars total). Download the full file or copy to clipboard to get everything.
Repository: jianhengLiu/VINS-RGBD-FAST
Branch: main
Commit: 757c256c747a
Files: 152
Total size: 1.1 MB
Directory structure:
gitextract_ongpbb8j/
├── .gitignore
├── README.md
├── camera_model/
│ ├── CMakeLists.txt
│ ├── cmake/
│ │ └── FindEigen.cmake
│ ├── include/
│ │ └── camodocal/
│ │ ├── calib/
│ │ │ └── CameraCalibration.h
│ │ ├── camera_models/
│ │ │ ├── Camera.h
│ │ │ ├── CameraFactory.h
│ │ │ ├── CataCamera.h
│ │ │ ├── CostFunctionFactory.h
│ │ │ ├── EquidistantCamera.h
│ │ │ ├── PinholeCamera.h
│ │ │ └── ScaramuzzaCamera.h
│ │ ├── chessboard/
│ │ │ ├── Chessboard.h
│ │ │ ├── ChessboardCorner.h
│ │ │ ├── ChessboardQuad.h
│ │ │ └── Spline.h
│ │ ├── gpl/
│ │ │ ├── EigenQuaternionParameterization.h
│ │ │ ├── EigenUtils.h
│ │ │ └── gpl.h
│ │ └── sparse_graph/
│ │ └── Transform.h
│ ├── instruction
│ ├── package.xml
│ ├── readme.md
│ └── src/
│ ├── calib/
│ │ └── CameraCalibration.cc
│ ├── camera_models/
│ │ ├── Camera.cc
│ │ ├── CameraFactory.cc
│ │ ├── CataCamera.cc
│ │ ├── CostFunctionFactory.cc
│ │ ├── EquidistantCamera.cc
│ │ ├── PinholeCamera.cc
│ │ └── ScaramuzzaCamera.cc
│ ├── chessboard/
│ │ └── Chessboard.cc
│ ├── gpl/
│ │ ├── EigenQuaternionParameterization.cc
│ │ └── gpl.cc
│ ├── intrinsic_calib.cc
│ └── sparse_graph/
│ └── Transform.cc
├── config/
│ ├── campus.rviz
│ ├── indoor.rviz
│ ├── openloris/
│ │ ├── openloris_vio.yaml
│ │ └── openloris_vio_atlas.yaml
│ ├── realsense/
│ │ ├── vio d455.yaml
│ │ ├── vio.yaml
│ │ ├── vio_atlas copy.yaml
│ │ ├── vio_atlas.yaml
│ │ ├── vio_campus.yaml
│ │ └── vio_indoor.yaml
│ ├── tagaided.rviz
│ ├── tsinghua.rviz
│ ├── tum_rgbd/
│ │ ├── tum_fr3.yaml
│ │ └── tum_fr3_atlas.yaml
│ └── video.rviz
├── doc/
│ ├── INSTALL.md
│ └── RUNNING_PROCEDURE.md
├── output/
│ └── .gitignore
├── pose_graph/
│ ├── CMakeLists.txt
│ ├── cmake/
│ │ └── FindEigen.cmake
│ ├── nodelet_plugin.xml
│ ├── package.xml
│ └── src/
│ ├── ThirdParty/
│ │ ├── DBoW/
│ │ │ ├── BowVector.cpp
│ │ │ ├── BowVector.h
│ │ │ ├── DBoW2.h
│ │ │ ├── FBrief.cpp
│ │ │ ├── FBrief.h
│ │ │ ├── FClass.h
│ │ │ ├── FeatureVector.cpp
│ │ │ ├── FeatureVector.h
│ │ │ ├── QueryResults.cpp
│ │ │ ├── QueryResults.h
│ │ │ ├── ScoringObject.cpp
│ │ │ ├── ScoringObject.h
│ │ │ ├── TemplatedDatabase.h
│ │ │ └── TemplatedVocabulary.h
│ │ ├── DUtils/
│ │ │ ├── DException.h
│ │ │ ├── DUtils.h
│ │ │ ├── Random.cpp
│ │ │ ├── Random.h
│ │ │ ├── Timestamp.cpp
│ │ │ └── Timestamp.h
│ │ ├── DVision/
│ │ │ ├── BRIEF.cpp
│ │ │ ├── BRIEF.h
│ │ │ └── DVision.h
│ │ ├── VocabularyBinary.cpp
│ │ └── VocabularyBinary.hpp
│ ├── keyframe/
│ │ ├── keyframe.cpp
│ │ └── keyframe.h
│ ├── pose_graph/
│ │ ├── pose_graph.cpp
│ │ └── pose_graph.h
│ ├── pose_graph_nodelet.cpp
│ └── utility/
│ ├── CameraPoseVisualization.cpp
│ ├── CameraPoseVisualization.h
│ ├── parameters.h
│ ├── tic_toc.h
│ ├── utility.cpp
│ └── utility.h
├── support_files/
│ └── brief_pattern.yml
└── vins_estimator/
├── CMakeLists.txt
├── cmake/
│ └── FindEigen.cmake
├── launch/
│ ├── atlas200/
│ │ ├── compressed2img.launch
│ │ └── img2compressed.launch
│ ├── openloris/
│ │ ├── openloris_vio_atlas.launch
│ │ ├── openloris_vio_pytorch.launch
│ │ ├── openloris_vio_tensorrt.launch
│ │ └── openloris_vo.launch
│ ├── realsense/
│ │ ├── l515.launch
│ │ ├── realsense_vio.launch
│ │ ├── realsense_vio_atlas.launch
│ │ └── rs_camera.launch
│ ├── tum_rgbd/
│ │ ├── tum_rgbd_atlas.launch
│ │ ├── tum_rgbd_pytorch.launch
│ │ └── tum_rgbd_tensorrt.launch
│ ├── vins_rviz.launch
│ └── yolo/
│ ├── atlas.launch
│ ├── pytorch.launch
│ └── tensorrt.launch
├── nodelet_description.xml
├── package.xml
└── src/
├── estimator/
│ ├── estimator.cpp
│ └── estimator.h
├── estimator_nodelet.cpp
├── factor/
│ ├── imu_factor.h
│ ├── imu_factor_modified.h
│ ├── integration_base.h
│ ├── marginalization_factor.cpp
│ ├── marginalization_factor.h
│ ├── pose_local_parameterization.cpp
│ ├── pose_local_parameterization.h
│ ├── projection_factor.cpp
│ ├── projection_factor.h
│ ├── projection_td_factor.cpp
│ └── projection_td_factor.h
├── feature_manager/
│ ├── feature_manager.cpp
│ └── feature_manager.h
├── feature_tracker/
│ ├── ThreadPool.h
│ ├── feature_tracker.cpp
│ └── feature_tracker.h
├── initial/
│ ├── initial_aligment.cpp
│ ├── initial_alignment.h
│ ├── initial_ex_rotation.cpp
│ ├── initial_ex_rotation.h
│ ├── initial_sfm.cpp
│ ├── initial_sfm.h
│ ├── solve_5pts.cpp
│ └── solve_5pts.h
└── utility/
├── CameraPoseVisualization.cpp
├── CameraPoseVisualization.h
├── parameters.cpp
├── parameters.h
├── tic_toc.h
├── utility.cpp
├── utility.h
├── visualization.cpp
└── visualization.h
================================================
FILE CONTENTS
================================================
================================================
FILE: .gitignore
================================================
**/build
**/devel
**/.idea
**/.vscode
**/cmake-build-debug
**/.cache
!.gitignore
================================================
FILE: README.md
================================================
# VINS-RGBD-FAST
**VINS-RGBD-FAST** is a SLAM system based on VINS-RGBD. We do some refinements to accelerate the system's performance in resource-constrained embedded paltform, like [HUAWEI Atlas200 DK](https://e.huawei.com/en/products/cloud-computing-dc/atlas/atlas-200), [NVIDIA Jetson AGX Xavier](https://www.nvidia.com/en-us/autonomous-machines/embedded-systems/jetson-agx-xavier/).
## Refinements
* grid-based feature detection
* extract FAST feature instead of Harris feature
* added stationary initialization
* added IMU-aided feature tracking
* added extracted-feature area's quality judgement
* solved feature clusttering problem result frome FAST feature
* use "sensor_msg::CompressedImage" as image topic type
## Related Paper:
```
@ARTICLE{9830851,
author={Liu, Jianheng and Li, Xuanfu and Liu, Yueqian and Chen, Haoyao},
journal={IEEE Robotics and Automation Letters},
title={RGB-D Inertial Odometry for a Resource-Restricted Robot in Dynamic Environments},
year={2022},
volume={7},
number={4},
pages={9573-9580},
doi={10.1109/LRA.2022.3191193}}
```
## RGBD-Inertial Trajectory Estimation and Mapping for Small Ground Rescue Robot
Based one open source SLAM framework [VINS-Mono](https://github.com/HKUST-Aerial-Robotics/VINS-Mono).
The approach contains
+ Depth-integrated visual-inertial initialization process.
+ Visual-inertial odometry by utilizing depth information while avoiding the limitation is working for 3D pose estimation.
+ Noise elimination map which is suitable for path planning and navigation.
However, the proposed approach can also be applied to other application like handheld and wheeled robot.
## 1. Prerequisites
1.1. **Ubuntu** 16.04 or 18.04.
1.2. **ROS** version Kinetic or Melodic fully installation
1.3. **Ceres Solver**
Follow [Ceres Installation](http://ceres-solver.org/installation.html)
1.4. **Sophus**
```
git clone http://github.com/strasdat/Sophus.git
git checkout a621ff
```
1.3. **Atlas 200 DK环境配置**
[https://blog.csdn.net/qq_42703283/article/details/110389270](https://blog.csdn.net/qq_42703283/article/details/110389270)
1.4. **ROS多机通信**
[https://blog.csdn.net/qq_42703283/article/details/110408848](
## 2. Datasets
Recording by RealSense D435i. Contain 9 bags in three different applicaions:
+ [Handheld](https://star-center.shanghaitech.edu.cn/seafile/d/0ea45d1878914077ade5/)
+ [Wheeled robot](https://star-center.shanghaitech.edu.cn/seafile/d/78c0375114854774b521/) ([Jackal](https://www.clearpathrobotics.com/jackal-small-unmanned-ground-vehicle/))
+ [Tracked robot](https://star-center.shanghaitech.edu.cn/seafile/d/f611fc44df0c4b3d936d/)
Note the rosbags are in compressed format. Use "rosbag decompress" to decompress.
Topics:
+ depth topic: /camera/aligned_depth_to_color/image_raw
+ color topic: /camera/color/image_raw
+ imu topic: /camera/imu
我们使用的是压缩图像节点：
+ depth topic: /camera/aligned_depth_to_color/image_raw
+ color topic: /camera/color/image_raw/compressed
+ imu topic: /camera/imu
如何录制一个数据包
1. 运行d435i
2. `rosbag record /camera/imu /camera/color/image_raw /camera/aligned_depth_to_color/image_raw /camera/color/camera_info /camera/color/image_raw/compressed`
## 3. Run with Docker
make Dockerfile like below
```c
FROM ros:melodic-ros-core-bionic
# apt-get update
RUN apt-get update
# install essentials
RUN apt install -y gcc
RUN apt install -y g++
RUN apt-get install -y cmake
RUN apt-get install -y wget
RUN apt install -y git
# install ceres
WORKDIR /home
RUN apt-get install -y libgoogle-glog-dev libgflags-dev
RUN apt-get install -y libatlas-base-dev
RUN apt-get install -y libeigen3-dev
RUN apt-get install -y libsuitesparse-dev
RUN wget http://ceres-solver.org/ceres-solver-2.1.0.tar.gz
RUN tar zxf ceres-solver-2.1.0.tar.gz
WORKDIR /home/ceres-solver-2.1.0
RUN mkdir build
WORKDIR /home/ceres-solver-2.1.0/build
RUN cmake ..
RUN make
RUN make install
# install sophus
WORKDIR /home
RUN git clone https://github.com/demul/Sophus.git
WORKDIR /home/Sophus
RUN git checkout fix/unit_complex_eror
RUN mkdir build
WORKDIR /home/Sophus/build
RUN cmake ..
RUN make
RUN make install
# install ros dependencies
WORKDIR /home
RUN mkdir ros_ws
WORKDIR /home/ros_ws
RUN apt-get -y install ros-melodic-cv-bridge
RUN apt-get -y install ros-melodic-nodelet
RUN apt-get -y install ros-melodic-tf
RUN apt-get -y install ros-melodic-image-transport
RUN apt-get -y install ros-melodic-rviz
# build vins-rgbd-fast
RUN mkdir src
WORKDIR /home/ros_ws/src
RUN git clone https://github.com/jianhengLiu/VINS-RGBD-FAST
WORKDIR /home/ros_ws
RUN /bin/bash -c ". /opt/ros/melodic/setup.bash; cd /home/ros_ws; catkin_make"
RUN echo "source /home/ros_ws/devel/setup.bash" >> ~/.bashrc
```
docker build .
## 4. Licence
The source code is released under [GPLv3](http://www.gnu.org/licenses/) license.
================================================
FILE: camera_model/CMakeLists.txt
================================================
cmake_minimum_required(VERSION 2.8.3)
project(camera_model)
set(CMAKE_BUILD_TYPE "Release")
# https://blog.csdn.net/qq_24624539/article/details/111056791
#set(CMAKE_CXX_FLAGS "-std=c++11")
set(CMAKE_CXX_FLAGS "-std=c++14")
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -fPIC")
find_package(catkin REQUIRED COMPONENTS
roscpp
std_msgs
)
find_package(Boost REQUIRED COMPONENTS filesystem program_options system)
include_directories(${Boost_INCLUDE_DIRS})
find_package(OpenCV REQUIRED)
# set(EIGEN_INCLUDE_DIR "/usr/local/include/eigen3")
find_package(Ceres REQUIRED)
include_directories(${CERES_INCLUDE_DIRS})
catkin_package(
INCLUDE_DIRS include
LIBRARIES camera_model
CATKIN_DEPENDS roscpp std_msgs
# DEPENDS system_lib
)
include_directories(
${catkin_INCLUDE_DIRS}
)
include_directories("include")
add_executable(Calibration
src/intrinsic_calib.cc
src/chessboard/Chessboard.cc
src/calib/CameraCalibration.cc
src/camera_models/Camera.cc
src/camera_models/CameraFactory.cc
src/camera_models/CostFunctionFactory.cc
src/camera_models/PinholeCamera.cc
src/camera_models/CataCamera.cc
src/camera_models/EquidistantCamera.cc
src/camera_models/ScaramuzzaCamera.cc
src/sparse_graph/Transform.cc
src/gpl/gpl.cc
src/gpl/EigenQuaternionParameterization.cc)
add_library(camera_model
src/chessboard/Chessboard.cc
src/calib/CameraCalibration.cc
src/camera_models/Camera.cc
src/camera_models/CameraFactory.cc
src/camera_models/CostFunctionFactory.cc
src/camera_models/PinholeCamera.cc
src/camera_models/CataCamera.cc
src/camera_models/EquidistantCamera.cc
src/camera_models/ScaramuzzaCamera.cc
src/sparse_graph/Transform.cc
src/gpl/gpl.cc
src/gpl/EigenQuaternionParameterization.cc)
target_link_libraries(Calibration ${Boost_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES})
target_link_libraries(camera_model ${Boost_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES})
================================================
FILE: camera_model/cmake/FindEigen.cmake
================================================
# Ceres Solver - A fast non-linear least squares minimizer
# Copyright 2015 Google Inc. All rights reserved.
# http://ceres-solver.org/
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
# * Redistributions of source code must retain the above copyright notice,
# this list of conditions and the following disclaimer.
# * Redistributions in binary form must reproduce the above copyright notice,
# this list of conditions and the following disclaimer in the documentation
# and/or other materials provided with the distribution.
# * Neither the name of Google Inc. nor the names of its contributors may be
# used to endorse or promote products derived from this software without
# specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
# Author: alexs.mac@gmail.com (Alex Stewart)
#
# FindEigen.cmake - Find Eigen library, version >= 3.
#
# This module defines the following variables:
#
# EIGEN_FOUND: TRUE iff Eigen is found.
# EIGEN_INCLUDE_DIRS: Include directories for Eigen.
#
# EIGEN_VERSION: Extracted from Eigen/src/Core/util/Macros.h
# EIGEN_WORLD_VERSION: Equal to 3 if EIGEN_VERSION = 3.2.0
# EIGEN_MAJOR_VERSION: Equal to 2 if EIGEN_VERSION = 3.2.0
# EIGEN_MINOR_VERSION: Equal to 0 if EIGEN_VERSION = 3.2.0
#
# The following variables control the behaviour of this module:
#
# EIGEN_INCLUDE_DIR_HINTS: List of additional directories in which to
# search for eigen includes, e.g: /timbuktu/eigen3.
#
# The following variables are also defined by this module, but in line with
# CMake recommended FindPackage() module style should NOT be referenced directly
# by callers (use the plural variables detailed above instead). These variables
# do however affect the behaviour of the module via FIND_[PATH/LIBRARY]() which
# are NOT re-called (i.e. search for library is not repeated) if these variables
# are set with valid values _in the CMake cache_. This means that if these
# variables are set directly in the cache, either by the user in the CMake GUI,
# or by the user passing -DVAR=VALUE directives to CMake when called (which
# explicitly defines a cache variable), then they will be used verbatim,
# bypassing the HINTS variables and other hard-coded search locations.
#
# EIGEN_INCLUDE_DIR: Include directory for CXSparse, not including the
# include directory of any dependencies.
# Called if we failed to find Eigen or any of it's required dependencies,
# unsets all public (designed to be used externally) variables and reports
# error message at priority depending upon [REQUIRED/QUIET/<NONE>] argument.
macro(EIGEN_REPORT_NOT_FOUND REASON_MSG)
unset(EIGEN_FOUND)
unset(EIGEN_INCLUDE_DIRS)
# Make results of search visible in the CMake GUI if Eigen has not
# been found so that user does not have to toggle to advanced view.
mark_as_advanced(CLEAR EIGEN_INCLUDE_DIR)
# Note <package>_FIND_[REQUIRED/QUIETLY] variables defined by FindPackage()
# use the camelcase library name, not uppercase.
if (Eigen_FIND_QUIETLY)
message(STATUS "Failed to find Eigen - " ${REASON_MSG} ${ARGN})
elseif (Eigen_FIND_REQUIRED)
message(FATAL_ERROR "Failed to find Eigen - " ${REASON_MSG} ${ARGN})
else()
# Neither QUIETLY nor REQUIRED, use no priority which emits a message
# but continues configuration and allows generation.
message("-- Failed to find Eigen - " ${REASON_MSG} ${ARGN})
endif ()
return()
endmacro(EIGEN_REPORT_NOT_FOUND)
# Protect against any alternative find_package scripts for this library having
# been called previously (in a client project) which set EIGEN_FOUND, but not
# the other variables we require / set here which could cause the search logic
# here to fail.
unset(EIGEN_FOUND)
# Search user-installed locations first, so that we prefer user installs
# to system installs where both exist.
list(APPEND EIGEN_CHECK_INCLUDE_DIRS
/usr/local/include
/usr/local/homebrew/include # Mac OS X
/opt/local/var/macports/software # Mac OS X.
/opt/local/include
/usr/include)
# Additional suffixes to try appending to each search path.
list(APPEND EIGEN_CHECK_PATH_SUFFIXES
eigen3 # Default root directory for Eigen.
Eigen/include/eigen3 # Windows (for C:/Program Files prefix) < 3.3
Eigen3/include/eigen3 ) # Windows (for C:/Program Files prefix) >= 3.3
# Search supplied hint directories first if supplied.
find_path(EIGEN_INCLUDE_DIR
NAMES Eigen/Core
PATHS ${EIGEN_INCLUDE_DIR_HINTS}
${EIGEN_CHECK_INCLUDE_DIRS}
PATH_SUFFIXES ${EIGEN_CHECK_PATH_SUFFIXES})
if (NOT EIGEN_INCLUDE_DIR OR
NOT EXISTS ${EIGEN_INCLUDE_DIR})
eigen_report_not_found(
"Could not find eigen3 include directory, set EIGEN_INCLUDE_DIR to "
"path to eigen3 include directory, e.g. /usr/local/include/eigen3.")
endif (NOT EIGEN_INCLUDE_DIR OR
NOT EXISTS ${EIGEN_INCLUDE_DIR})
# Mark internally as found, then verify. EIGEN_REPORT_NOT_FOUND() unsets
# if called.
set(EIGEN_FOUND TRUE)
# Extract Eigen version from Eigen/src/Core/util/Macros.h
if (EIGEN_INCLUDE_DIR)
set(EIGEN_VERSION_FILE ${EIGEN_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h)
if (NOT EXISTS ${EIGEN_VERSION_FILE})
eigen_report_not_found(
"Could not find file: ${EIGEN_VERSION_FILE} "
"containing version information in Eigen install located at: "
"${EIGEN_INCLUDE_DIR}.")
else (NOT EXISTS ${EIGEN_VERSION_FILE})
file(READ ${EIGEN_VERSION_FILE} EIGEN_VERSION_FILE_CONTENTS)
string(REGEX MATCH "#define EIGEN_WORLD_VERSION [0-9]+"
EIGEN_WORLD_VERSION "${EIGEN_VERSION_FILE_CONTENTS}")
string(REGEX REPLACE "#define EIGEN_WORLD_VERSION ([0-9]+)" "\\1"
EIGEN_WORLD_VERSION "${EIGEN_WORLD_VERSION}")
string(REGEX MATCH "#define EIGEN_MAJOR_VERSION [0-9]+"
EIGEN_MAJOR_VERSION "${EIGEN_VERSION_FILE_CONTENTS}")
string(REGEX REPLACE "#define EIGEN_MAJOR_VERSION ([0-9]+)" "\\1"
EIGEN_MAJOR_VERSION "${EIGEN_MAJOR_VERSION}")
string(REGEX MATCH "#define EIGEN_MINOR_VERSION [0-9]+"
EIGEN_MINOR_VERSION "${EIGEN_VERSION_FILE_CONTENTS}")
string(REGEX REPLACE "#define EIGEN_MINOR_VERSION ([0-9]+)" "\\1"
EIGEN_MINOR_VERSION "${EIGEN_MINOR_VERSION}")
# This is on a single line s/t CMake does not interpret it as a list of
# elements and insert ';' separators which would result in 3.;2.;0 nonsense.
set(EIGEN_VERSION "${EIGEN_WORLD_VERSION}.${EIGEN_MAJOR_VERSION}.${EIGEN_MINOR_VERSION}")
endif (NOT EXISTS ${EIGEN_VERSION_FILE})
endif (EIGEN_INCLUDE_DIR)
# Set standard CMake FindPackage variables if found.
if (EIGEN_FOUND)
set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR})
endif (EIGEN_FOUND)
# Handle REQUIRED / QUIET optional arguments and version.
include(FindPackageHandleStandardArgs)
find_package_handle_standard_args(Eigen
REQUIRED_VARS EIGEN_INCLUDE_DIRS
VERSION_VAR EIGEN_VERSION)
# Only mark internal variables as advanced if we found Eigen, otherwise
# leave it visible in the standard GUI for the user to set manually.
if (EIGEN_FOUND)
mark_as_advanced(FORCE EIGEN_INCLUDE_DIR)
endif (EIGEN_FOUND)
================================================
FILE: camera_model/include/camodocal/calib/CameraCalibration.h
================================================
#ifndef CAMERACALIBRATION_H
#define CAMERACALIBRATION_H
#include <opencv2/core/core.hpp>
#include "camodocal/camera_models/Camera.h"
namespace camodocal
{
class CameraCalibration
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
CameraCalibration();
CameraCalibration(Camera::ModelType modelType,
const std::string& cameraName,
const cv::Size& imageSize,
const cv::Size& boardSize,
float squareSize);
void clear(void);
void addChessboardData(const std::vector<cv::Point2f>& corners);
bool calibrate(void);
int sampleCount(void) const;
std::vector<std::vector<cv::Point2f> >& imagePoints(void);
const std::vector<std::vector<cv::Point2f> >& imagePoints(void) const;
std::vector<std::vector<cv::Point3f> >& scenePoints(void);
const std::vector<std::vector<cv::Point3f> >& scenePoints(void) const;
CameraPtr& camera(void);
const CameraConstPtr camera(void) const;
Eigen::Matrix2d& measurementCovariance(void);
const Eigen::Matrix2d& measurementCovariance(void) const;
cv::Mat& cameraPoses(void);
const cv::Mat& cameraPoses(void) const;
void drawResults(std::vector<cv::Mat>& images) const;
void writeParams(const std::string& filename) const;
bool writeChessboardData(const std::string& filename) const;
bool readChessboardData(const std::string& filename);
void setVerbose(bool verbose);
private:
bool calibrateHelper(CameraPtr& camera,
std::vector<cv::Mat>& rvecs, std::vector<cv::Mat>& tvecs) const;
void optimize(CameraPtr& camera,
std::vector<cv::Mat>& rvecs, std::vector<cv::Mat>& tvecs) const;
template<typename T>
void readData(std::ifstream& ifs, T& data) const;
template<typename T>
void writeData(std::ofstream& ofs, T data) const;
cv::Size m_boardSize;
float m_squareSize;
CameraPtr m_camera;
cv::Mat m_cameraPoses;
std::vector<std::vector<cv::Point2f> > m_imagePoints;
std::vector<std::vector<cv::Point3f> > m_scenePoints;
Eigen::Matrix2d m_measurementCovariance;
bool m_verbose;
};
}
#endif
================================================
FILE: camera_model/include/camodocal/camera_models/Camera.h
================================================
#ifndef CAMERA_H
#define CAMERA_H
#include <boost/shared_ptr.hpp>
#include <eigen3/Eigen/Dense>
#include <opencv2/core/core.hpp>
#include <vector>
namespace camodocal
{
class Camera
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
enum ModelType
{
KANNALA_BRANDT,
MEI,
PINHOLE,
SCARAMUZZA
};
class Parameters
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Parameters(ModelType modelType);
Parameters(ModelType modelType, const std::string& cameraName,
int w, int h);
ModelType& modelType(void);
std::string& cameraName(void);
int& imageWidth(void);
int& imageHeight(void);
ModelType modelType(void) const;
const std::string& cameraName(void) const;
int imageWidth(void) const;
int imageHeight(void) const;
int nIntrinsics(void) const;
virtual bool readFromYamlFile(const std::string& filename) = 0;
virtual void writeToYamlFile(const std::string& filename) const = 0;
protected:
ModelType m_modelType;
int m_nIntrinsics;
std::string m_cameraName;
int m_imageWidth;
int m_imageHeight;
};
virtual ModelType modelType(void) const = 0;
virtual const std::string& cameraName(void) const = 0;
virtual int imageWidth(void) const = 0;
virtual int imageHeight(void) const = 0;
virtual cv::Mat& mask(void);
virtual const cv::Mat& mask(void) const;
virtual void estimateIntrinsics(const cv::Size& boardSize,
const std::vector< std::vector<cv::Point3f> >& objectPoints,
const std::vector< std::vector<cv::Point2f> >& imagePoints) = 0;
virtual void estimateExtrinsics(const std::vector<cv::Point3f>& objectPoints,
const std::vector<cv::Point2f>& imagePoints,
cv::Mat& rvec, cv::Mat& tvec) const;
// Lift points from the image plane to the sphere
virtual void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const = 0;
//%output P
// Lift points from the image plane to the projective space
virtual void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const = 0;
//%output P
// Projects 3D points to the image plane (Pi function)
virtual void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const = 0;
//%output p
// Projects 3D points to the image plane (Pi function)
// and calculates jacobian
//virtual void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p,
// Eigen::Matrix<double,2,3>& J) const = 0;
//%output p
//%output J
virtual void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const = 0;
//%output p
//virtual void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0) const = 0;
virtual cv::Mat initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2,
float fx = -1.0f, float fy = -1.0f,
cv::Size imageSize = cv::Size(0, 0),
float cx = -1.0f, float cy = -1.0f,
cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const = 0;
virtual int parameterCount(void) const = 0;
virtual void readParameters(const std::vector<double>& parameters) = 0;
virtual void writeParameters(std::vector<double>& parameters) const = 0;
virtual void writeParametersToYamlFile(const std::string& filename) const = 0;
virtual std::string parametersToString(void) const = 0;
/**
* \brief Calculates the reprojection distance between points
*
* \param P1 first 3D point coordinates
* \param P2 second 3D point coordinates
* \return euclidean distance in the plane
*/
double reprojectionDist(const Eigen::Vector3d& P1, const Eigen::Vector3d& P2) const;
double reprojectionError(const std::vector< std::vector<cv::Point3f> >& objectPoints,
const std::vector< std::vector<cv::Point2f> >& imagePoints,
const std::vector<cv::Mat>& rvecs,
const std::vector<cv::Mat>& tvecs,
cv::OutputArray perViewErrors = cv::noArray()) const;
double reprojectionError(const Eigen::Vector3d& P,
const Eigen::Quaterniond& camera_q,
const Eigen::Vector3d& camera_t,
const Eigen::Vector2d& observed_p) const;
void projectPoints(const std::vector<cv::Point3f>& objectPoints,
const cv::Mat& rvec,
const cv::Mat& tvec,
std::vector<cv::Point2f>& imagePoints) const;
protected:
cv::Mat m_mask;
};
typedef boost::shared_ptr<Camera> CameraPtr;
typedef boost::shared_ptr<const Camera> CameraConstPtr;
}
#endif
================================================
FILE: camera_model/include/camodocal/camera_models/CameraFactory.h
================================================
#ifndef CAMERAFACTORY_H
#define CAMERAFACTORY_H
#include <boost/shared_ptr.hpp>
#include <opencv2/core/core.hpp>
#include "camodocal/camera_models/Camera.h"
namespace camodocal
{
class CameraFactory
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
CameraFactory();
static boost::shared_ptr<CameraFactory> instance(void);
CameraPtr generateCamera(Camera::ModelType modelType,
const std::string& cameraName,
cv::Size imageSize) const;
CameraPtr generateCameraFromYamlFile(const std::string& filename);
private:
static boost::shared_ptr<CameraFactory> m_instance;
};
}
#endif
================================================
FILE: camera_model/include/camodocal/camera_models/CataCamera.h
================================================
#ifndef CATACAMERA_H
#define CATACAMERA_H
#include <opencv2/core/core.hpp>
#include <string>
#include "ceres/rotation.h"
#include "Camera.h"
namespace camodocal
{
/**
* C. Mei, and P. Rives, Single View Point Omnidirectional Camera Calibration
* from Planar Grids, ICRA 2007
*/
class CataCamera: public Camera
{
public:
class Parameters: public Camera::Parameters
{
public:
Parameters();
Parameters(const std::string& cameraName,
int w, int h,
double xi,
double k1, double k2, double p1, double p2,
double gamma1, double gamma2, double u0, double v0);
double& xi(void);
double& k1(void);
double& k2(void);
double& p1(void);
double& p2(void);
double& gamma1(void);
double& gamma2(void);
double& u0(void);
double& v0(void);
double xi(void) const;
double k1(void) const;
double k2(void) const;
double p1(void) const;
double p2(void) const;
double gamma1(void) const;
double gamma2(void) const;
double u0(void) const;
double v0(void) const;
bool readFromYamlFile(const std::string& filename);
void writeToYamlFile(const std::string& filename) const;
Parameters& operator=(const Parameters& other);
friend std::ostream& operator<< (std::ostream& out, const Parameters& params);
private:
double m_xi;
double m_k1;
double m_k2;
double m_p1;
double m_p2;
double m_gamma1;
double m_gamma2;
double m_u0;
double m_v0;
};
CataCamera();
/**
* \brief Constructor from the projection model parameters
*/
CataCamera(const std::string& cameraName,
int imageWidth, int imageHeight,
double xi, double k1, double k2, double p1, double p2,
double gamma1, double gamma2, double u0, double v0);
/**
* \brief Constructor from the projection model parameters
*/
CataCamera(const Parameters& params);
Camera::ModelType modelType(void) const;
const std::string& cameraName(void) const;
int imageWidth(void) const;
int imageHeight(void) const;
void estimateIntrinsics(const cv::Size& boardSize,
const std::vector< std::vector<cv::Point3f> >& objectPoints,
const std::vector< std::vector<cv::Point2f> >& imagePoints);
// Lift points from the image plane to the sphere
void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const;
//%output P
// Lift points from the image plane to the projective space
void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const;
//%output P
// Projects 3D points to the image plane (Pi function)
void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const;
//%output p
// Projects 3D points to the image plane (Pi function)
// and calculates jacobian
void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p,
Eigen::Matrix<double,2,3>& J) const;
//%output p
//%output J
void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const;
//%output p
template <typename T>
static void spaceToPlane(const T* const params,
const T* const q, const T* const t,
const Eigen::Matrix<T, 3, 1>& P,
Eigen::Matrix<T, 2, 1>& p);
void distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u) const;
void distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u,
Eigen::Matrix2d& J) const;
void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0) const;
cv::Mat initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2,
float fx = -1.0f, float fy = -1.0f,
cv::Size imageSize = cv::Size(0, 0),
float cx = -1.0f, float cy = -1.0f,
cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const;
int parameterCount(void) const;
const Parameters& getParameters(void) const;
void setParameters(const Parameters& parameters);
void readParameters(const std::vector<double>& parameterVec);
void writeParameters(std::vector<double>& parameterVec) const;
void writeParametersToYamlFile(const std::string& filename) const;
std::string parametersToString(void) const;
private:
Parameters mParameters;
double m_inv_K11, m_inv_K13, m_inv_K22, m_inv_K23;
bool m_noDistortion;
};
typedef boost::shared_ptr<CataCamera> CataCameraPtr;
typedef boost::shared_ptr<const CataCamera> CataCameraConstPtr;
template <typename T>
void
CataCamera::spaceToPlane(const T* const params,
const T* const q, const T* const t,
const Eigen::Matrix<T, 3, 1>& P,
Eigen::Matrix<T, 2, 1>& p)
{
T P_w[3];
P_w[0] = T(P(0));
P_w[1] = T(P(1));
P_w[2] = T(P(2));
// Convert quaternion from Eigen convention (x, y, z, w)
// to Ceres convention (w, x, y, z)
T q_ceres[4] = {q[3], q[0], q[1], q[2]};
T P_c[3];
ceres::QuaternionRotatePoint(q_ceres, P_w, P_c);
P_c[0] += t[0];
P_c[1] += t[1];
P_c[2] += t[2];
// project 3D object point to the image plane
T xi = params[0];
T k1 = params[1];
T k2 = params[2];
T p1 = params[3];
T p2 = params[4];
T gamma1 = params[5];
T gamma2 = params[6];
T alpha = T(0); //cameraParams.alpha();
T u0 = params[7];
T v0 = params[8];
// Transform to model plane
T len = sqrt(P_c[0] * P_c[0] + P_c[1] * P_c[1] + P_c[2] * P_c[2]);
P_c[0] /= len;
P_c[1] /= len;
P_c[2] /= len;
T u = P_c[0] / (P_c[2] + xi);
T v = P_c[1] / (P_c[2] + xi);
T rho_sqr = u * u + v * v;
T L = T(1.0) + k1 * rho_sqr + k2 * rho_sqr * rho_sqr;
T du = T(2.0) * p1 * u * v + p2 * (rho_sqr + T(2.0) * u * u);
T dv = p1 * (rho_sqr + T(2.0) * v * v) + T(2.0) * p2 * u * v;
u = L * u + du;
v = L * v + dv;
p(0) = gamma1 * (u + alpha * v) + u0;
p(1) = gamma2 * v + v0;
}
}
#endif
================================================
FILE: camera_model/include/camodocal/camera_models/CostFunctionFactory.h
================================================
#ifndef COSTFUNCTIONFACTORY_H
#define COSTFUNCTIONFACTORY_H
#include <boost/shared_ptr.hpp>
#include <opencv2/core/core.hpp>
#include "camodocal/camera_models/Camera.h"
namespace ceres
{
class CostFunction;
}
namespace camodocal
{
enum
{
CAMERA_INTRINSICS = 1 << 0,
CAMERA_POSE = 1 << 1,
POINT_3D = 1 << 2,
ODOMETRY_INTRINSICS = 1 << 3,
ODOMETRY_3D_POSE = 1 << 4,
ODOMETRY_6D_POSE = 1 << 5,
CAMERA_ODOMETRY_TRANSFORM = 1 << 6
};
class CostFunctionFactory
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
CostFunctionFactory();
static boost::shared_ptr<CostFunctionFactory> instance(void);
ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera,
const Eigen::Vector3d& observed_P,
const Eigen::Vector2d& observed_p,
int flags) const;
ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera,
const Eigen::Vector3d& observed_P,
const Eigen::Vector2d& observed_p,
const Eigen::Matrix2d& sqrtPrecisionMat,
int flags) const;
ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera,
const Eigen::Vector2d& observed_p,
int flags, bool optimize_cam_odo_z = true) const;
ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera,
const Eigen::Vector2d& observed_p,
const Eigen::Matrix2d& sqrtPrecisionMat,
int flags, bool optimize_cam_odo_z = true) const;
ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera,
const Eigen::Vector3d& odo_pos,
const Eigen::Vector3d& odo_att,
const Eigen::Vector2d& observed_p,
int flags, bool optimize_cam_odo_z = true) const;
ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera,
const Eigen::Quaterniond& cam_odo_q,
const Eigen::Vector3d& cam_odo_t,
const Eigen::Vector3d& odo_pos,
const Eigen::Vector3d& odo_att,
const Eigen::Vector2d& observed_p,
int flags) const;
ceres::CostFunction* generateCostFunction(const CameraConstPtr& cameraLeft,
const CameraConstPtr& cameraRight,
const Eigen::Vector3d& observed_P,
const Eigen::Vector2d& observed_p_left,
const Eigen::Vector2d& observed_p_right) const;
private:
static boost::shared_ptr<CostFunctionFactory> m_instance;
};
}
#endif
================================================
FILE: camera_model/include/camodocal/camera_models/EquidistantCamera.h
================================================
#ifndef EQUIDISTANTCAMERA_H
#define EQUIDISTANTCAMERA_H
#include <opencv2/core/core.hpp>
#include <string>
#include "ceres/rotation.h"
#include "Camera.h"
namespace camodocal
{
/**
* J. Kannala, and S. Brandt, A Generic Camera Model and Calibration Method
* for Conventional, Wide-Angle, and Fish-Eye Lenses, PAMI 2006
*/
class EquidistantCamera: public Camera
{
public:
class Parameters: public Camera::Parameters
{
public:
Parameters();
Parameters(const std::string& cameraName,
int w, int h,
double k2, double k3, double k4, double k5,
double mu, double mv,
double u0, double v0);
double& k2(void);
double& k3(void);
double& k4(void);
double& k5(void);
double& mu(void);
double& mv(void);
double& u0(void);
double& v0(void);
double k2(void) const;
double k3(void) const;
double k4(void) const;
double k5(void) const;
double mu(void) const;
double mv(void) const;
double u0(void) const;
double v0(void) const;
bool readFromYamlFile(const std::string& filename);
void writeToYamlFile(const std::string& filename) const;
Parameters& operator=(const Parameters& other);
friend std::ostream& operator<< (std::ostream& out, const Parameters& params);
private:
// projection
double m_k2;
double m_k3;
double m_k4;
double m_k5;
double m_mu;
double m_mv;
double m_u0;
double m_v0;
};
EquidistantCamera();
/**
* \brief Constructor from the projection model parameters
*/
EquidistantCamera(const std::string& cameraName,
int imageWidth, int imageHeight,
double k2, double k3, double k4, double k5,
double mu, double mv,
double u0, double v0);
/**
* \brief Constructor from the projection model parameters
*/
EquidistantCamera(const Parameters& params);
Camera::ModelType modelType(void) const;
const std::string& cameraName(void) const;
int imageWidth(void) const;
int imageHeight(void) const;
void estimateIntrinsics(const cv::Size& boardSize,
const std::vector< std::vector<cv::Point3f> >& objectPoints,
const std::vector< std::vector<cv::Point2f> >& imagePoints);
// Lift points from the image plane to the sphere
virtual void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const;
//%output P
// Lift points from the image plane to the projective space
void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const;
//%output P
// Projects 3D points to the image plane (Pi function)
void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const;
//%output p
// Projects 3D points to the image plane (Pi function)
// and calculates jacobian
void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p,
Eigen::Matrix<double,2,3>& J) const;
//%output p
//%output J
void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const;
//%output p
template <typename T>
static void spaceToPlane(const T* const params,
const T* const q, const T* const t,
const Eigen::Matrix<T, 3, 1>& P,
Eigen::Matrix<T, 2, 1>& p);
void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0) const;
cv::Mat initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2,
float fx = -1.0f, float fy = -1.0f,
cv::Size imageSize = cv::Size(0, 0),
float cx = -1.0f, float cy = -1.0f,
cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const;
int parameterCount(void) const;
const Parameters& getParameters(void) const;
void setParameters(const Parameters& parameters);
void readParameters(const std::vector<double>& parameterVec);
void writeParameters(std::vector<double>& parameterVec) const;
void writeParametersToYamlFile(const std::string& filename) const;
std::string parametersToString(void) const;
private:
template<typename T>
static T r(T k2, T k3, T k4, T k5, T theta);
void fitOddPoly(const std::vector<double>& x, const std::vector<double>& y,
int n, std::vector<double>& coeffs) const;
void backprojectSymmetric(const Eigen::Vector2d& p_u,
double& theta, double& phi) const;
Parameters mParameters;
double m_inv_K11, m_inv_K13, m_inv_K22, m_inv_K23;
};
typedef boost::shared_ptr<EquidistantCamera> EquidistantCameraPtr;
typedef boost::shared_ptr<const EquidistantCamera> EquidistantCameraConstPtr;
template<typename T>
T
EquidistantCamera::r(T k2, T k3, T k4, T k5, T theta)
{
// k1 = 1
return theta +
k2 * theta * theta * theta +
k3 * theta * theta * theta * theta * theta +
k4 * theta * theta * theta * theta * theta * theta * theta +
k5 * theta * theta * theta * theta * theta * theta * theta * theta * theta;
}
template <typename T>
void
EquidistantCamera::spaceToPlane(const T* const params,
const T* const q, const T* const t,
const Eigen::Matrix<T, 3, 1>& P,
Eigen::Matrix<T, 2, 1>& p)
{
T P_w[3];
P_w[0] = T(P(0));
P_w[1] = T(P(1));
P_w[2] = T(P(2));
// Convert quaternion from Eigen convention (x, y, z, w)
// to Ceres convention (w, x, y, z)
T q_ceres[4] = {q[3], q[0], q[1], q[2]};
T P_c[3];
ceres::QuaternionRotatePoint(q_ceres, P_w, P_c);
P_c[0] += t[0];
P_c[1] += t[1];
P_c[2] += t[2];
// project 3D object point to the image plane;
T k2 = params[0];
T k3 = params[1];
T k4 = params[2];
T k5 = params[3];
T mu = params[4];
T mv = params[5];
T u0 = params[6];
T v0 = params[7];
T len = sqrt(P_c[0] * P_c[0] + P_c[1] * P_c[1] + P_c[2] * P_c[2]);
T theta = acos(P_c[2] / len);
T phi = atan2(P_c[1], P_c[0]);
Eigen::Matrix<T,2,1> p_u = r(k2, k3, k4, k5, theta) * Eigen::Matrix<T,2,1>(cos(phi), sin(phi));
p(0) = mu * p_u(0) + u0;
p(1) = mv * p_u(1) + v0;
}
}
#endif
================================================
FILE: camera_model/include/camodocal/camera_models/PinholeCamera.h
================================================
#ifndef PINHOLECAMERA_H
#define PINHOLECAMERA_H
#include <opencv2/core/core.hpp>
#include <string>
#include "ceres/rotation.h"
#include "Camera.h"
namespace camodocal
{
class PinholeCamera: public Camera
{
public:
class Parameters: public Camera::Parameters
{
public:
Parameters();
Parameters(const std::string& cameraName,
int w, int h,
double k1, double k2, double p1, double p2,
double fx, double fy, double cx, double cy);
double& k1(void);
double& k2(void);
double& p1(void);
double& p2(void);
double& fx(void);
double& fy(void);
double& cx(void);
double& cy(void);
double xi(void) const;
double k1(void) const;
double k2(void) const;
double p1(void) const;
double p2(void) const;
double fx(void) const;
double fy(void) const;
double cx(void) const;
double cy(void) const;
bool readFromYamlFile(const std::string& filename);
void writeToYamlFile(const std::string& filename) const;
Parameters& operator=(const Parameters& other);
friend std::ostream& operator<< (std::ostream& out, const Parameters& params);
private:
double m_k1;
double m_k2;
double m_p1;
double m_p2;
double m_fx;
double m_fy;
double m_cx;
double m_cy;
};
PinholeCamera();
/**
* \brief Constructor from the projection model parameters
*/
PinholeCamera(const std::string& cameraName,
int imageWidth, int imageHeight,
double k1, double k2, double p1, double p2,
double fx, double fy, double cx, double cy);
/**
* \brief Constructor from the projection model parameters
*/
PinholeCamera(const Parameters& params);
Camera::ModelType modelType(void) const;
const std::string& cameraName(void) const;
int imageWidth(void) const;
int imageHeight(void) const;
void estimateIntrinsics(const cv::Size& boardSize,
const std::vector< std::vector<cv::Point3f> >& objectPoints,
const std::vector< std::vector<cv::Point2f> >& imagePoints);
// Lift points from the image plane to the sphere
virtual void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const;
//%output P
// Lift points from the image plane to the projective space
void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const;
//%output P
// Projects 3D points to the image plane (Pi function)
void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const;
//%output p
// Projects 3D points to the image plane (Pi function)
// and calculates jacobian
void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p,
Eigen::Matrix<double,2,3>& J) const;
//%output p
//%output J
void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const;
//%output p
template <typename T>
static void spaceToPlane(const T* const params,
const T* const q, const T* const t,
const Eigen::Matrix<T, 3, 1>& P,
Eigen::Matrix<T, 2, 1>& p);
void distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u) const;
void distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u,
Eigen::Matrix2d& J) const;
void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0) const;
cv::Mat initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2,
float fx = -1.0f, float fy = -1.0f,
cv::Size imageSize = cv::Size(0, 0),
float cx = -1.0f, float cy = -1.0f,
cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const;
int parameterCount(void) const;
const Parameters& getParameters(void) const;
void setParameters(const Parameters& parameters);
void readParameters(const std::vector<double>& parameterVec);
void writeParameters(std::vector<double>& parameterVec) const;
void writeParametersToYamlFile(const std::string& filename) const;
std::string parametersToString(void) const;
private:
Parameters mParameters;
double m_inv_K11, m_inv_K13, m_inv_K22, m_inv_K23;
bool m_noDistortion;
};
typedef boost::shared_ptr<PinholeCamera> PinholeCameraPtr;
typedef boost::shared_ptr<const PinholeCamera> PinholeCameraConstPtr;
template <typename T>
void
PinholeCamera::spaceToPlane(const T* const params,
const T* const q, const T* const t,
const Eigen::Matrix<T, 3, 1>& P,
Eigen::Matrix<T, 2, 1>& p)
{
T P_w[3];
P_w[0] = T(P(0));
P_w[1] = T(P(1));
P_w[2] = T(P(2));
// Convert quaternion from Eigen convention (x, y, z, w)
// to Ceres convention (w, x, y, z)
T q_ceres[4] = {q[3], q[0], q[1], q[2]};
T P_c[3];
ceres::QuaternionRotatePoint(q_ceres, P_w, P_c);
P_c[0] += t[0];
P_c[1] += t[1];
P_c[2] += t[2];
// project 3D object point to the image plane
T k1 = params[0];
T k2 = params[1];
T p1 = params[2];
T p2 = params[3];
T fx = params[4];
T fy = params[5];
T alpha = T(0); //cameraParams.alpha();
T cx = params[6];
T cy = params[7];
// Transform to model plane
T u = P_c[0] / P_c[2];
T v = P_c[1] / P_c[2];
T rho_sqr = u * u + v * v;
T L = T(1.0) + k1 * rho_sqr + k2 * rho_sqr * rho_sqr;
T du = T(2.0) * p1 * u * v + p2 * (rho_sqr + T(2.0) * u * u);
T dv = p1 * (rho_sqr + T(2.0) * v * v) + T(2.0) * p2 * u * v;
u = L * u + du;
v = L * v + dv;
p(0) = fx * (u + alpha * v) + cx;
p(1) = fy * v + cy;
}
}
#endif
================================================
FILE: camera_model/include/camodocal/camera_models/ScaramuzzaCamera.h
================================================
#ifndef SCARAMUZZACAMERA_H
#define SCARAMUZZACAMERA_H
#include <opencv2/core/core.hpp>
#include <string>
#include "ceres/rotation.h"
#include "Camera.h"
namespace camodocal
{
#define SCARAMUZZA_POLY_SIZE 5
#define SCARAMUZZA_INV_POLY_SIZE 20
#define SCARAMUZZA_CAMERA_NUM_PARAMS (SCARAMUZZA_POLY_SIZE + SCARAMUZZA_INV_POLY_SIZE + 2 /*center*/ + 3 /*affine*/)
/**
* Scaramuzza Camera (Omnidirectional)
* https://sites.google.com/site/scarabotix/ocamcalib-toolbox
*/
class OCAMCamera: public Camera
{
public:
class Parameters: public Camera::Parameters
{
public:
Parameters();
double& C(void) { return m_C; }
double& D(void) { return m_D; }
double& E(void) { return m_E; }
double& center_x(void) { return m_center_x; }
double& center_y(void) { return m_center_y; }
double& poly(int idx) { return m_poly[idx]; }
double& inv_poly(int idx) { return m_inv_poly[idx]; }
double C(void) const { return m_C; }
double D(void) const { return m_D; }
double E(void) const { return m_E; }
double center_x(void) const { return m_center_x; }
double center_y(void) const { return m_center_y; }
double poly(int idx) const { return m_poly[idx]; }
double inv_poly(int idx) const { return m_inv_poly[idx]; }
bool readFromYamlFile(const std::string& filename);
void writeToYamlFile(const std::string& filename) const;
Parameters& operator=(const Parameters& other);
friend std::ostream& operator<< (std::ostream& out, const Parameters& params);
private:
double m_poly[SCARAMUZZA_POLY_SIZE];
double m_inv_poly[SCARAMUZZA_INV_POLY_SIZE];
double m_C;
double m_D;
double m_E;
double m_center_x;
double m_center_y;
};
OCAMCamera();
/**
* \brief Constructor from the projection model parameters
*/
OCAMCamera(const Parameters& params);
Camera::ModelType modelType(void) const;
const std::string& cameraName(void) const;
int imageWidth(void) const;
int imageHeight(void) const;
void estimateIntrinsics(const cv::Size& boardSize,
const std::vector< std::vector<cv::Point3f> >& objectPoints,
const std::vector< std::vector<cv::Point2f> >& imagePoints);
// Lift points from the image plane to the sphere
void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const;
//%output P
// Lift points from the image plane to the projective space
void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const;
//%output P
// Projects 3D points to the image plane (Pi function)
void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const;
//%output p
// Projects 3D points to the image plane (Pi function)
// and calculates jacobian
//void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p,
// Eigen::Matrix<double,2,3>& J) const;
//%output p
//%output J
void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const;
//%output p
template <typename T>
static void spaceToPlane(const T* const params,
const T* const q, const T* const t,
const Eigen::Matrix<T, 3, 1>& P,
Eigen::Matrix<T, 2, 1>& p);
template <typename T>
static void spaceToSphere(const T* const params,
const T* const q, const T* const t,
const Eigen::Matrix<T, 3, 1>& P,
Eigen::Matrix<T, 3, 1>& P_s);
template <typename T>
static void LiftToSphere(const T* const params,
const Eigen::Matrix<T, 2, 1>& p,
Eigen::Matrix<T, 3, 1>& P);
template <typename T>
static void SphereToPlane(const T* const params, const Eigen::Matrix<T, 3, 1>& P,
Eigen::Matrix<T, 2, 1>& p);
void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0) const;
cv::Mat initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2,
float fx = -1.0f, float fy = -1.0f,
cv::Size imageSize = cv::Size(0, 0),
float cx = -1.0f, float cy = -1.0f,
cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const;
int parameterCount(void) const;
const Parameters& getParameters(void) const;
void setParameters(const Parameters& parameters);
void readParameters(const std::vector<double>& parameterVec);
void writeParameters(std::vector<double>& parameterVec) const;
void writeParametersToYamlFile(const std::string& filename) const;
std::string parametersToString(void) const;
private:
Parameters mParameters;
double m_inv_scale;
};
typedef boost::shared_ptr<OCAMCamera> OCAMCameraPtr;
typedef boost::shared_ptr<const OCAMCamera> OCAMCameraConstPtr;
template <typename T>
void
OCAMCamera::spaceToPlane(const T* const params,
const T* const q, const T* const t,
const Eigen::Matrix<T, 3, 1>& P,
Eigen::Matrix<T, 2, 1>& p)
{
T P_c[3];
{
T P_w[3];
P_w[0] = T(P(0));
P_w[1] = T(P(1));
P_w[2] = T(P(2));
// Convert quaternion from Eigen convention (x, y, z, w)
// to Ceres convention (w, x, y, z)
T q_ceres[4] = {q[3], q[0], q[1], q[2]};
ceres::QuaternionRotatePoint(q_ceres, P_w, P_c);
P_c[0] += t[0];
P_c[1] += t[1];
P_c[2] += t[2];
}
T c = params[0];
T d = params[1];
T e = params[2];
T xc[2] = { params[3], params[4] };
//T poly[SCARAMUZZA_POLY_SIZE];
//for (int i=0; i < SCARAMUZZA_POLY_SIZE; i++)
// poly[i] = params[5+i];
T inv_poly[SCARAMUZZA_INV_POLY_SIZE];
for (int i=0; i < SCARAMUZZA_INV_POLY_SIZE; i++)
inv_poly[i] = params[5 + SCARAMUZZA_POLY_SIZE + i];
T norm_sqr = P_c[0] * P_c[0] + P_c[1] * P_c[1];
T norm = T(0.0);
if (norm_sqr > T(0.0))
norm = sqrt(norm_sqr);
T theta = atan2(-P_c[2], norm);
T rho = T(0.0);
T theta_i = T(1.0);
for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++)
{
rho += theta_i * inv_poly[i];
theta_i *= theta;
}
T invNorm = T(1.0) / norm;
T xn[2] = {
P_c[0] * invNorm * rho,
P_c[1] * invNorm * rho
};
p(0) = xn[0] * c + xn[1] * d + xc[0];
p(1) = xn[0] * e + xn[1] + xc[1];
}
template <typename T>
void
OCAMCamera::spaceToSphere(const T* const params,
const T* const q, const T* const t,
const Eigen::Matrix<T, 3, 1>& P,
Eigen::Matrix<T, 3, 1>& P_s)
{
T P_c[3];
{
T P_w[3];
P_w[0] = T(P(0));
P_w[1] = T(P(1));
P_w[2] = T(P(2));
// Convert quaternion from Eigen convention (x, y, z, w)
// to Ceres convention (w, x, y, z)
T q_ceres[4] = {q[3], q[0], q[1], q[2]};
ceres::QuaternionRotatePoint(q_ceres, P_w, P_c);
P_c[0] += t[0];
P_c[1] += t[1];
P_c[2] += t[2];
}
//T poly[SCARAMUZZA_POLY_SIZE];
//for (int i=0; i < SCARAMUZZA_POLY_SIZE; i++)
// poly[i] = params[5+i];
T norm_sqr = P_c[0] * P_c[0] + P_c[1] * P_c[1] + P_c[2] * P_c[2];
T norm = T(0.0);
if (norm_sqr > T(0.0))
norm = sqrt(norm_sqr);
P_s(0) = P_c[0] / norm;
P_s(1) = P_c[1] / norm;
P_s(2) = P_c[2] / norm;
}
template <typename T>
void
OCAMCamera::LiftToSphere(const T* const params,
const Eigen::Matrix<T, 2, 1>& p,
Eigen::Matrix<T, 3, 1>& P)
{
T c = params[0];
T d = params[1];
T e = params[2];
T cc[2] = { params[3], params[4] };
T poly[SCARAMUZZA_POLY_SIZE];
for (int i=0; i < SCARAMUZZA_POLY_SIZE; i++)
poly[i] = params[5+i];
// Relative to Center
T p_2d[2];
p_2d[0] = T(p(0));
p_2d[1] = T(p(1));
T xc[2] = { p_2d[0] - cc[0], p_2d[1] - cc[1]};
T inv_scale = T(1.0) / (c - d * e);
// Affine Transformation
T xc_a[2];
xc_a[0] = inv_scale * (xc[0] - d * xc[1]);
xc_a[1] = inv_scale * (-e * xc[0] + c * xc[1]);
T norm_sqr = xc_a[0] * xc_a[0] + xc_a[1] * xc_a[1];
T phi = sqrt(norm_sqr);
T phi_i = T(1.0);
T z = T(0.0);
for (int i = 0; i < SCARAMUZZA_POLY_SIZE; i++)
{
if (i!=1) {
z += phi_i * poly[i];
}
phi_i *= phi;
}
T p_3d[3];
p_3d[0] = xc[0];
p_3d[1] = xc[1];
p_3d[2] = -z;
T p_3d_norm_sqr = p_3d[0] * p_3d[0] + p_3d[1] * p_3d[1] + p_3d[2] * p_3d[2];
T p_3d_norm = sqrt(p_3d_norm_sqr);
P << p_3d[0] / p_3d_norm, p_3d[1] / p_3d_norm, p_3d[2] / p_3d_norm;
}
template <typename T>
void OCAMCamera::SphereToPlane(const T* const params, const Eigen::Matrix<T, 3, 1>& P,
Eigen::Matrix<T, 2, 1>& p) {
T P_c[3];
{
P_c[0] = T(P(0));
P_c[1] = T(P(1));
P_c[2] = T(P(2));
}
T c = params[0];
T d = params[1];
T e = params[2];
T xc[2] = {params[3], params[4]};
T inv_poly[SCARAMUZZA_INV_POLY_SIZE];
for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++)
inv_poly[i] = params[5 + SCARAMUZZA_POLY_SIZE + i];
T norm_sqr = P_c[0] * P_c[0] + P_c[1] * P_c[1];
T norm = T(0.0);
if (norm_sqr > T(0.0)) norm = sqrt(norm_sqr);
T theta = atan2(-P_c[2], norm);
T rho = T(0.0);
T theta_i = T(1.0);
for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++) {
rho += theta_i * inv_poly[i];
theta_i *= theta;
}
T invNorm = T(1.0) / norm;
T xn[2] = {P_c[0] * invNorm * rho, P_c[1] * invNorm * rho};
p(0) = xn[0] * c + xn[1] * d + xc[0];
p(1) = xn[0] * e + xn[1] + xc[1];
}
}
#endif
================================================
FILE: camera_model/include/camodocal/chessboard/Chessboard.h
================================================
#ifndef CHESSBOARD_H
#define CHESSBOARD_H
#include <boost/shared_ptr.hpp>
#include <opencv2/core/core.hpp>
namespace camodocal
{
// forward declarations
class ChessboardCorner;
typedef boost::shared_ptr<ChessboardCorner> ChessboardCornerPtr;
class ChessboardQuad;
typedef boost::shared_ptr<ChessboardQuad> ChessboardQuadPtr;
class Chessboard
{
public:
Chessboard(cv::Size boardSize, cv::Mat& image);
void findCorners(bool useOpenCV = false);
const std::vector<cv::Point2f>& getCorners(void) const;
bool cornersFound(void) const;
const cv::Mat& getImage(void) const;
const cv::Mat& getSketch(void) const;
private:
bool findChessboardCorners(const cv::Mat& image,
const cv::Size& patternSize,
std::vector<cv::Point2f>& corners,
int flags, bool useOpenCV);
bool findChessboardCornersImproved(const cv::Mat& image,
const cv::Size& patternSize,
std::vector<cv::Point2f>& corners,
int flags);
void cleanFoundConnectedQuads(std::vector<ChessboardQuadPtr>& quadGroup, cv::Size patternSize);
void findConnectedQuads(std::vector<ChessboardQuadPtr>& quads,
std::vector<ChessboardQuadPtr>& group,
int group_idx, int dilation);
// int checkQuadGroup(std::vector<ChessboardQuadPtr>& quadGroup,
// std::vector<ChessboardCornerPtr>& outCorners,
// cv::Size patternSize);
void labelQuadGroup(std::vector<ChessboardQuadPtr>& quad_group,
cv::Size patternSize, bool firstRun);
void findQuadNeighbors(std::vector<ChessboardQuadPtr>& quads, int dilation);
int augmentBestRun(std::vector<ChessboardQuadPtr>& candidateQuads, int candidateDilation,
std::vector<ChessboardQuadPtr>& existingQuads, int existingDilation);
void generateQuads(std::vector<ChessboardQuadPtr>& quads,
cv::Mat& image, int flags,
int dilation, bool firstRun);
bool checkQuadGroup(std::vector<ChessboardQuadPtr>& quads,
std::vector<ChessboardCornerPtr>& corners,
cv::Size patternSize);
void getQuadrangleHypotheses(const std::vector< std::vector<cv::Point> >& contours,
std::vector< std::pair<float, int> >& quads,
int classId) const;
bool checkChessboard(const cv::Mat& image, cv::Size patternSize) const;
bool checkBoardMonotony(std::vector<ChessboardCornerPtr>& corners,
cv::Size patternSize);
bool matchCorners(ChessboardQuadPtr& quad1, int corner1,
ChessboardQuadPtr& quad2, int corner2) const;
cv::Mat mImage;
cv::Mat mSketch;
std::vector<cv::Point2f> mCorners;
cv::Size mBoardSize;
bool mCornersFound;
};
}
#endif
================================================
FILE: camera_model/include/camodocal/chessboard/ChessboardCorner.h
================================================
#ifndef CHESSBOARDCORNER_H
#define CHESSBOARDCORNER_H
#include <boost/shared_ptr.hpp>
#include <opencv2/core/core.hpp>
namespace camodocal
{
class ChessboardCorner;
typedef boost::shared_ptr<ChessboardCorner> ChessboardCornerPtr;
class ChessboardCorner
{
public:
ChessboardCorner() : row(0), column(0), needsNeighbor(true), count(0) {}
float meanDist(int &n) const
{
float sum = 0;
n = 0;
for (int i = 0; i < 4; ++i)
{
if (neighbors[i].get())
{
float dx = neighbors[i]->pt.x - pt.x;
float dy = neighbors[i]->pt.y - pt.y;
sum += sqrt(dx*dx + dy*dy);
n++;
}
}
return sum / std::max(n, 1);
}
cv::Point2f pt; // X and y coordinates
int row; // Row and column of the corner
int column; // in the found pattern
bool needsNeighbor; // Does the corner require a neighbor?
int count; // number of corner neighbors
ChessboardCornerPtr neighbors[4]; // pointer to all corner neighbors
};
}
#endif
================================================
FILE: camera_model/include/camodocal/chessboard/ChessboardQuad.h
================================================
#ifndef CHESSBOARDQUAD_H
#define CHESSBOARDQUAD_H
#include <boost/shared_ptr.hpp>
#include "camodocal/chessboard/ChessboardCorner.h"
namespace camodocal
{
class ChessboardQuad;
typedef boost::shared_ptr<ChessboardQuad> ChessboardQuadPtr;
class ChessboardQuad
{
public:
ChessboardQuad() : count(0), group_idx(-1), edge_len(FLT_MAX), labeled(false) {}
int count; // Number of quad neighbors
int group_idx; // Quad group ID
float edge_len; // Smallest side length^2
ChessboardCornerPtr corners[4]; // Coordinates of quad corners
ChessboardQuadPtr neighbors[4]; // Pointers of quad neighbors
bool labeled; // Has this corner been labeled?
};
}
#endif
================================================
FILE: camera_model/include/camodocal/chessboard/Spline.h
================================================
/* dynamo:- Event driven molecular dynamics simulator
http://www.marcusbannerman.co.uk/dynamo
Copyright (C) 2011 Marcus N Campbell Bannerman <m.bannerman@gmail.com>
This program is free software: you can redistribute it and/or
modify it under the terms of the GNU General Public License
version 3 as published by the Free Software Foundation.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <exception>
namespace ublas = boost::numeric::ublas;
class Spline : private std::vector<std::pair<double, double> >
{
public:
//The boundary conditions available
enum BC_type {
FIXED_1ST_DERIV_BC,
FIXED_2ND_DERIV_BC,
PARABOLIC_RUNOUT_BC
};
enum Spline_type {
LINEAR,
CUBIC
};
//Constructor takes the boundary conditions as arguments, this
//sets the first derivative (gradient) at the lower and upper
//end points
Spline():
_valid(false),
_BCLow(FIXED_2ND_DERIV_BC), _BCHigh(FIXED_2ND_DERIV_BC),
_BCLowVal(0), _BCHighVal(0),
_type(CUBIC)
{}
typedef std::vector<std::pair<double, double> > base;
typedef base::const_iterator const_iterator;
//Standard STL read-only container stuff
const_iterator begin() const { return base::begin(); }
const_iterator end() const { return base::end(); }
void clear() { _valid = false; base::clear(); _data.clear(); }
size_t size() const { return base::size(); }
size_t max_size() const { return base::max_size(); }
size_t capacity() const { return base::capacity(); }
bool empty() const { return base::empty(); }
//Add a point to the spline, and invalidate it so its
//recalculated on the next access
inline void addPoint(double x, double y)
{
_valid = false;
base::push_back(std::pair<double, double>(x,y));
}
//Reset the boundary conditions
inline void setLowBC(BC_type BC, double val = 0)
{ _BCLow = BC; _BCLowVal = val; _valid = false; }
inline void setHighBC(BC_type BC, double val = 0)
{ _BCHigh = BC; _BCHighVal = val; _valid = false; }
void setType(Spline_type type) { _type = type; _valid = false; }
//Check if the spline has been calculated, then generate the
//spline interpolated value
double operator()(double xval)
{
if (!_valid) generate();
//Special cases when we're outside the range of the spline points
if (xval <= x(0)) return lowCalc(xval);
if (xval >= x(size()-1)) return highCalc(xval);
//Check all intervals except the last one
for (std::vector<SplineData>::const_iterator iPtr = _data.begin();
iPtr != _data.end()-1; ++iPtr)
if ((xval >= iPtr->x) && (xval <= (iPtr+1)->x))
return splineCalc(iPtr, xval);
return splineCalc(_data.end() - 1, xval);
}
private:
///////PRIVATE DATA MEMBERS
struct SplineData { double x,a,b,c,d; };
//vector of calculated spline data
std::vector<SplineData> _data;
//Second derivative at each point
ublas::vector<double> _ddy;
//Tracks whether the spline parameters have been calculated for
//the current set of points
bool _valid;
//The boundary conditions
BC_type _BCLow, _BCHigh;
//The values of the boundary conditions
double _BCLowVal, _BCHighVal;
Spline_type _type;
///////PRIVATE FUNCTIONS
//Function to calculate the value of a given spline at a point xval
inline double splineCalc(std::vector<SplineData>::const_iterator i, double xval)
{
const double lx = xval - i->x;
return ((i->a * lx + i->b) * lx + i->c) * lx + i->d;
}
inline double lowCalc(double xval)
{
const double lx = xval - x(0);
if (_type == LINEAR)
return lx * _BCHighVal + y(0);
const double firstDeriv = (y(1) - y(0)) / h(0) - 2 * h(0) * (_data[0].b + 2 * _data[1].b) / 6;
switch(_BCLow)
{
case FIXED_1ST_DERIV_BC:
return lx * _BCLowVal + y(0);
case FIXED_2ND_DERIV_BC:
return lx * lx * _BCLowVal + firstDeriv * lx + y(0);
case PARABOLIC_RUNOUT_BC:
return lx * lx * _ddy[0] + lx * firstDeriv + y(0);
}
throw std::runtime_error("Unknown BC");
}
inline double highCalc(double xval)
{
const double lx = xval - x(size() - 1);
if (_type == LINEAR)
return lx * _BCHighVal + y(size() - 1);
const double firstDeriv = 2 * h(size() - 2) * (_ddy[size() - 2] + 2 * _ddy[size() - 1]) / 6 + (y(size() - 1) - y(size() - 2)) / h(size() - 2);
switch(_BCHigh)
{
case FIXED_1ST_DERIV_BC:
return lx * _BCHighVal + y(size() - 1);
case FIXED_2ND_DERIV_BC:
return lx * lx * _BCHighVal + firstDeriv * lx + y(size() - 1);
case PARABOLIC_RUNOUT_BC:
return lx * lx * _ddy[size()-1] + lx * firstDeriv + y(size() - 1);
}
throw std::runtime_error("Unknown BC");
}
//These just provide access to the point data in a clean way
inline double x(size_t i) const { return operator[](i).first; }
inline double y(size_t i) const { return operator[](i).second; }
inline double h(size_t i) const { return x(i+1) - x(i); }
//Invert a arbitrary matrix using the boost ublas library
template<class T>
bool InvertMatrix(ublas::matrix<T> A,
ublas::matrix<T>& inverse)
{
using namespace ublas;
// create a permutation matrix for the LU-factorization
permutation_matrix<std::size_t> pm(A.size1());
// perform LU-factorization
int res = lu_factorize(A,pm);
if( res != 0 ) return false;
// create identity matrix of "inverse"
inverse.assign(ublas::identity_matrix<T>(A.size1()));
// backsubstitute to get the inverse
lu_substitute(A, pm, inverse);
return true;
}
//This function will recalculate the spline parameters and store
//them in _data, ready for spline interpolation
void generate()
{
if (size() < 2)
throw std::runtime_error("Spline requires at least 2 points");
//If any spline points are at the same x location, we have to
//just slightly seperate them
{
bool testPassed(false);
while (!testPassed)
{
testPassed = true;
std::sort(base::begin(), base::end());
for (base::iterator iPtr = base::begin(); iPtr != base::end() - 1; ++iPtr)
if (iPtr->first == (iPtr+1)->first)
{
if ((iPtr+1)->first != 0)
(iPtr+1)->first += (iPtr+1)->first
* std::numeric_limits<double>::epsilon() * 10;
else
(iPtr+1)->first = std::numeric_limits<double>::epsilon() * 10;
testPassed = false;
break;
}
}
}
const size_t e = size() - 1;
switch (_type)
{
case LINEAR:
{
_data.resize(e);
for (size_t i(0); i < e; ++i)
{
_data[i].x = x(i);
_data[i].a = 0;
_data[i].b = 0;
_data[i].c = (y(i+1) - y(i)) / (x(i+1) - x(i));
_data[i].d = y(i);
}
break;
}
case CUBIC:
{
ublas::matrix<double> A(size(), size());
for (size_t yv(0); yv <= e; ++yv)
for (size_t xv(0); xv <= e; ++xv)
A(xv,yv) = 0;
for (size_t i(1); i < e; ++i)
{
A(i-1,i) = h(i-1);
A(i,i) = 2 * (h(i-1) + h(i));
A(i+1,i) = h(i);
}
ublas::vector<double> C(size());
for (size_t xv(0); xv <= e; ++xv)
C(xv) = 0;
for (size_t i(1); i < e; ++i)
C(i) = 6 *
((y(i+1) - y(i)) / h(i)
- (y(i) - y(i-1)) / h(i-1));
//Boundary conditions
switch(_BCLow)
{
case FIXED_1ST_DERIV_BC:
C(0) = 6 * ((y(1) - y(0)) / h(0) - _BCLowVal);
A(0,0) = 2 * h(0);
A(1,0) = h(0);
break;
case FIXED_2ND_DERIV_BC:
C(0) = _BCLowVal;
A(0,0) = 1;
break;
case PARABOLIC_RUNOUT_BC:
C(0) = 0; A(0,0) = 1; A(1,0) = -1;
break;
}
switch(_BCHigh)
{
case FIXED_1ST_DERIV_BC:
C(e) = 6 * (_BCHighVal - (y(e) - y(e-1)) / h(e-1));
A(e,e) = 2 * h(e - 1);
A(e-1,e) = h(e - 1);
break;
case FIXED_2ND_DERIV_BC:
C(e) = _BCHighVal;
A(e,e) = 1;
break;
case PARABOLIC_RUNOUT_BC:
C(e) = 0; A(e,e) = 1; A(e-1,e) = -1;
break;
}
ublas::matrix<double> AInv(size(), size());
InvertMatrix(A,AInv);
_ddy = ublas::prod(C, AInv);
_data.resize(size()-1);
for (size_t i(0); i < e; ++i)
{
_data[i].x = x(i);
_data[i].a = (_ddy(i+1) - _ddy(i)) / (6 * h(i));
_data[i].b = _ddy(i) / 2;
_data[i].c = (y(i+1) - y(i)) / h(i) - _ddy(i+1) * h(i) / 6 - _ddy(i) * h(i) / 3;
_data[i].d = y(i);
}
}
}
_valid = true;
}
};
================================================
FILE: camera_model/include/camodocal/gpl/EigenQuaternionParameterization.h
================================================
#ifndef EIGENQUATERNIONPARAMETERIZATION_H
#define EIGENQUATERNIONPARAMETERIZATION_H
#include "ceres/local_parameterization.h"
namespace camodocal
{
class EigenQuaternionParameterization : public ceres::LocalParameterization
{
public:
virtual ~EigenQuaternionParameterization() {}
virtual bool Plus(const double* x,
const double* delta,
double* x_plus_delta) const;
virtual bool ComputeJacobian(const double* x,
double* jacobian) const;
virtual int GlobalSize() const { return 4; }
virtual int LocalSize() const { return 3; }
private:
template<typename T>
void EigenQuaternionProduct(const T z[4], const T w[4], T zw[4]) const;
};
template<typename T>
void
EigenQuaternionParameterization::EigenQuaternionProduct(const T z[4], const T w[4], T zw[4]) const
{
zw[0] = z[3] * w[0] + z[0] * w[3] + z[1] * w[2] - z[2] * w[1];
zw[1] = z[3] * w[1] - z[0] * w[2] + z[1] * w[3] + z[2] * w[0];
zw[2] = z[3] * w[2] + z[0] * w[1] - z[1] * w[0] + z[2] * w[3];
zw[3] = z[3] * w[3] - z[0] * w[0] - z[1] * w[1] - z[2] * w[2];
}
}
#endif
================================================
FILE: camera_model/include/camodocal/gpl/EigenUtils.h
================================================
#ifndef EIGENUTILS_H
#define EIGENUTILS_H
#include <eigen3/Eigen/Dense>
#include "ceres/rotation.h"
#include "camodocal/gpl/gpl.h"
namespace camodocal
{
// Returns the 3D cross product skew symmetric matrix of a given 3D vector
template<typename T>
Eigen::Matrix<T, 3, 3> skew(const Eigen::Matrix<T, 3, 1>& vec)
{
return (Eigen::Matrix<T, 3, 3>() << T(0), -vec(2), vec(1),
vec(2), T(0), -vec(0),
-vec(1), vec(0), T(0)).finished();
}
template<typename Derived>
typename Eigen::MatrixBase<Derived>::PlainObject sqrtm(const Eigen::MatrixBase<Derived>& A)
{
Eigen::SelfAdjointEigenSolver<typename Derived::PlainObject> es(A);
return es.operatorSqrt();
}
template<typename T>
Eigen::Matrix<T, 3, 3> AngleAxisToRotationMatrix(const Eigen::Matrix<T, 3, 1>& rvec)
{
T angle = rvec.norm();
if (angle == T(0))
{
return Eigen::Matrix<T, 3, 3>::Identity();
}
Eigen::Matrix<T, 3, 1> axis;
axis = rvec.normalized();
Eigen::Matrix<T, 3, 3> rmat;
rmat = Eigen::AngleAxis<T>(angle, axis);
return rmat;
}
template<typename T>
Eigen::Quaternion<T> AngleAxisToQuaternion(const Eigen::Matrix<T, 3, 1>& rvec)
{
Eigen::Matrix<T, 3, 3> rmat = AngleAxisToRotationMatrix<T>(rvec);
return Eigen::Quaternion<T>(rmat);
}
template<typename T>
void AngleAxisToQuaternion(const Eigen::Matrix<T, 3, 1>& rvec, T* q)
{
Eigen::Quaternion<T> quat = AngleAxisToQuaternion<T>(rvec);
q[0] = quat.x();
q[1] = quat.y();
q[2] = quat.z();
q[3] = quat.w();
}
template<typename T>
Eigen::Matrix<T, 3, 1> RotationToAngleAxis(const Eigen::Matrix<T, 3, 3> & rmat)
{
Eigen::AngleAxis<T> angleaxis;
angleaxis.fromRotationMatrix(rmat);
return angleaxis.angle() * angleaxis.axis();
}
template<typename T>
void QuaternionToAngleAxis(const T* const q, Eigen::Matrix<T, 3, 1>& rvec)
{
Eigen::Quaternion<T> quat(q[3], q[0], q[1], q[2]);
Eigen::Matrix<T, 3, 3> rmat = quat.toRotationMatrix();
Eigen::AngleAxis<T> angleaxis;
angleaxis.fromRotationMatrix(rmat);
rvec = angleaxis.angle() * angleaxis.axis();
}
template<typename T>
Eigen::Matrix<T, 3, 3> QuaternionToRotation(const T* const q)
{
T R[9];
ceres::QuaternionToRotation(q, R);
Eigen::Matrix<T, 3, 3> rmat;
for (int i = 0; i < 3; ++i)
{
for (int j = 0; j < 3; ++j)
{
rmat(i,j) = R[i * 3 + j];
}
}
return rmat;
}
template<typename T>
void QuaternionToRotation(const T* const q, T* rot)
{
ceres::QuaternionToRotation(q, rot);
}
template<typename T>
Eigen::Matrix<T,4,4> QuaternionMultMatLeft(const Eigen::Quaternion<T>& q)
{
return (Eigen::Matrix<T,4,4>() << q.w(), -q.z(), q.y(), q.x(),
q.z(), q.w(), -q.x(), q.y(),
-q.y(), q.x(), q.w(), q.z(),
-q.x(), -q.y(), -q.z(), q.w()).finished();
}
template<typename T>
Eigen::Matrix<T,4,4> QuaternionMultMatRight(const Eigen::Quaternion<T>& q)
{
return (Eigen::Matrix<T,4,4>() << q.w(), q.z(), -q.y(), q.x(),
-q.z(), q.w(), q.x(), q.y(),
q.y(), -q.x(), q.w(), q.z(),
-q.x(), -q.y(), -q.z(), q.w()).finished();
}
/// @param theta - rotation about screw axis
/// @param d - projection of tvec on the rotation axis
/// @param l - screw axis direction
/// @param m - screw axis moment
template<typename T>
void AngleAxisAndTranslationToScrew(const Eigen::Matrix<T, 3, 1>& rvec,
const Eigen::Matrix<T, 3, 1>& tvec,
T& theta, T& d,
Eigen::Matrix<T, 3, 1>& l,
Eigen::Matrix<T, 3, 1>& m)
{
theta = rvec.norm();
if (theta == 0)
{
l.setZero();
m.setZero();
std::cout << "Warning: Undefined screw! Returned 0. " << std::endl;
}
l = rvec.normalized();
Eigen::Matrix<T, 3, 1> t = tvec;
d = t.transpose() * l;
// point on screw axis - projection of origin on screw axis
Eigen::Matrix<T, 3, 1> c;
c = 0.5 * (t - d * l + (1.0 / tan(theta / 2.0) * l).cross(t));
// c and hence the screw axis is not defined if theta is either 0 or M_PI
m = c.cross(l);
}
template<typename T>
Eigen::Matrix<T, 3, 3> RPY2mat(T roll, T pitch, T yaw)
{
Eigen::Matrix<T, 3, 3> m;
T cr = cos(roll);
T sr = sin(roll);
T cp = cos(pitch);
T sp = sin(pitch);
T cy = cos(yaw);
T sy = sin(yaw);
m(0,0) = cy * cp;
m(0,1) = cy * sp * sr - sy * cr;
m(0,2) = cy * sp * cr + sy * sr;
m(1,0) = sy * cp;
m(1,1) = sy * sp * sr + cy * cr;
m(1,2) = sy * sp * cr - cy * sr;
m(2,0) = - sp;
m(2,1) = cp * sr;
m(2,2) = cp * cr;
return m;
}
template<typename T>
void mat2RPY(const Eigen::Matrix<T, 3, 3>& m, T& roll, T& pitch, T& yaw)
{
roll = atan2(m(2,1), m(2,2));
pitch = atan2(-m(2,0), sqrt(m(2,1) * m(2,1) + m(2,2) * m(2,2)));
yaw = atan2(m(1,0), m(0,0));
}
template<typename T>
Eigen::Matrix<T, 4, 4> homogeneousTransform(const Eigen::Matrix<T, 3, 3>& R, const Eigen::Matrix<T, 3, 1>& t)
{
Eigen::Matrix<T, 4, 4> H;
H.setIdentity();
H.block(0,0,3,3) = R;
H.block(0,3,3,1) = t;
return H;
}
template<typename T>
Eigen::Matrix<T, 4, 4> poseWithCartesianTranslation(const T* const q, const T* const p)
{
Eigen::Matrix<T, 4, 4> pose = Eigen::Matrix<T, 4, 4>::Identity();
T rotation[9];
ceres::QuaternionToRotation(q, rotation);
for (int i = 0; i < 3; ++i)
{
for (int j = 0; j < 3; ++j)
{
pose(i,j) = rotation[i * 3 + j];
}
}
pose(0,3) = p[0];
pose(1,3) = p[1];
pose(2,3) = p[2];
return pose;
}
template<typename T>
Eigen::Matrix<T, 4, 4> poseWithSphericalTranslation(const T* const q, const T* const p, const T scale = T(1.0))
{
Eigen::Matrix<T, 4, 4> pose = Eigen::Matrix<T, 4, 4>::Identity();
T rotation[9];
ceres::QuaternionToRotation(q, rotation);
for (int i = 0; i < 3; ++i)
{
for (int j = 0; j < 3; ++j)
{
pose(i,j) = rotation[i * 3 + j];
}
}
T theta = p[0];
T phi = p[1];
pose(0,3) = sin(theta) * cos(phi) * scale;
pose(1,3) = sin(theta) * sin(phi) * scale;
pose(2,3) = cos(theta) * scale;
return pose;
}
// Returns the Sampson error of a given essential matrix and 2 image points
template<typename T>
T sampsonError(const Eigen::Matrix<T, 3, 3>& E,
const Eigen::Matrix<T, 3, 1>& p1,
const Eigen::Matrix<T, 3, 1>& p2)
{
Eigen::Matrix<T, 3, 1> Ex1 = E * p1;
Eigen::Matrix<T, 3, 1> Etx2 = E.transpose() * p2;
T x2tEx1 = p2.dot(Ex1);
// compute Sampson error
T err = square(x2tEx1) / (square(Ex1(0,0)) + square(Ex1(1,0)) + square(Etx2(0,0)) + square(Etx2(1,0)));
return err;
}
// Returns the Sampson error of a given rotation/translation and 2 image points
template<typename T>
T sampsonError(const Eigen::Matrix<T, 3, 3>& R,
const Eigen::Matrix<T, 3, 1>& t,
const Eigen::Matrix<T, 3, 1>& p1,
const Eigen::Matrix<T, 3, 1>& p2)
{
// construct essential matrix
Eigen::Matrix<T, 3, 3> E = skew(t) * R;
Eigen::Matrix<T, 3, 1> Ex1 = E * p1;
Eigen::Matrix<T, 3, 1> Etx2 = E.transpose() * p2;
T x2tEx1 = p2.dot(Ex1);
// compute Sampson error
T err = square(x2tEx1) / (square(Ex1(0,0)) + square(Ex1(1,0)) + square(Etx2(0,0)) + square(Etx2(1,0)));
return err;
}
// Returns the Sampson error of a given rotation/translation and 2 image points
template<typename T>
T sampsonError(const Eigen::Matrix<T, 4, 4>& H,
const Eigen::Matrix<T, 3, 1>& p1,
const Eigen::Matrix<T, 3, 1>& p2)
{
Eigen::Matrix<T, 3, 3> R = H.block(0, 0, 3, 3);
Eigen::Matrix<T, 3, 1> t = H.block(0, 3, 3, 1);
return sampsonError(R, t, p1, p2);
}
template<typename T>
Eigen::Matrix<T, 3, 1>
transformPoint(const Eigen::Matrix<T, 4, 4>& H, const Eigen::Matrix<T, 3, 1>& P)
{
Eigen::Matrix<T, 3, 1> P_trans = H.block(0, 0, 3, 3) * P + H.block(0, 3, 3, 1);
return P_trans;
}
template<typename T>
Eigen::Matrix<T, 4, 4>
estimate3DRigidTransform(const std::vector<Eigen::Matrix<T, 3, 1>, Eigen::aligned_allocator<Eigen::Matrix<T, 3, 1> > >& points1,
const std::vector<Eigen::Matrix<T, 3, 1>, Eigen::aligned_allocator<Eigen::Matrix<T, 3, 1> > >& points2)
{
// compute centroids
Eigen::Matrix<T, 3, 1> c1, c2;
c1.setZero(); c2.setZero();
for (size_t i = 0; i < points1.size(); ++i)
{
c1 += points1.at(i);
c2 += points2.at(i);
}
c1 /= points1.size();
c2 /= points1.size();
Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> X(3, points1.size());
Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> Y(3, points1.size());
for (size_t i = 0; i < points1.size(); ++i)
{
X.col(i) = points1.at(i) - c1;
Y.col(i) = points2.at(i) - c2;
}
Eigen::Matrix<T, 3, 3> H = X * Y.transpose();
Eigen::JacobiSVD< Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> > svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::Matrix<T, 3, 3> U = svd.matrixU();
Eigen::Matrix<T, 3, 3> V = svd.matrixV();
if (U.determinant() * V.determinant() < 0.0)
{
V.col(2) *= -1.0;
}
Eigen::Matrix<T, 3, 3> R = V * U.transpose();
Eigen::Matrix<T, 3, 1> t = c2 - R * c1;
return homogeneousTransform(R, t);
}
template<typename T>
Eigen::Matrix<T, 4, 4>
estimate3DRigidSimilarityTransform(const std::vector<Eigen::Matrix<T, 3, 1>, Eigen::aligned_allocator<Eigen::Matrix<T, 3, 1> > >& points1,
const std::vector<Eigen::Matrix<T, 3, 1>, Eigen::aligned_allocator<Eigen::Matrix<T, 3, 1> > >& points2)
{
// compute centroids
Eigen::Matrix<T, 3, 1> c1, c2;
c1.setZero(); c2.setZero();
for (size_t i = 0; i < points1.size(); ++i)
{
c1 += points1.at(i);
c2 += points2.at(i);
}
c1 /= points1.size();
c2 /= points1.size();
Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> X(3, points1.size());
Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> Y(3, points1.size());
for (size_t i = 0; i < points1.size(); ++i)
{
X.col(i) = points1.at(i) - c1;
Y.col(i) = points2.at(i) - c2;
}
Eigen::Matrix<T, 3, 3> H = X * Y.transpose();
Eigen::JacobiSVD< Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> > svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::Matrix<T, 3, 3> U = svd.matrixU();
Eigen::Matrix<T, 3, 3> V = svd.matrixV();
if (U.determinant() * V.determinant() < 0.0)
{
V.col(2) *= -1.0;
}
Eigen::Matrix<T, 3, 3> R = V * U.transpose();
std::vector<Eigen::Matrix<T, 3, 1>, Eigen::aligned_allocator<Eigen::Matrix<T, 3, 1> > > rotatedPoints1(points1.size());
for (size_t i = 0; i < points1.size(); ++i)
{
rotatedPoints1.at(i) = R * (points1.at(i) - c1);
}
double sum_ss = 0.0, sum_tt = 0.0;
for (size_t i = 0; i < points1.size(); ++i)
{
sum_ss += (points1.at(i) - c1).squaredNorm();
sum_tt += (points2.at(i) - c2).dot(rotatedPoints1.at(i));
}
double scale = sum_tt / sum_ss;
Eigen::Matrix<T, 3, 3> sR = scale * R;
Eigen::Matrix<T, 3, 1> t = c2 - sR * c1;
return homogeneousTransform(sR, t);
}
}
#endif
================================================
FILE: camera_model/include/camodocal/gpl/gpl.h
================================================
#ifndef GPL_H
#define GPL_H
#include <algorithm>
#include <cmath>
#include <opencv2/core/core.hpp>
namespace camodocal
{
template<class T>
const T clamp(const T& v, const T& a, const T& b)
{
return std::min(b, std::max(a, v));
}
double hypot3(double x, double y, double z);
float hypot3f(float x, float y, float z);
template<class T>
const T normalizeTheta(const T& theta)
{
T normTheta = theta;
while (normTheta < - M_PI)
{
normTheta += 2.0 * M_PI;
}
while (normTheta > M_PI)
{
normTheta -= 2.0 * M_PI;
}
return normTheta;
}
double d2r(double deg);
float d2r(float deg);
double r2d(double rad);
float r2d(float rad);
double sinc(double theta);
template<class T>
const T square(const T& x)
{
return x * x;
}
template<class T>
const T cube(const T& x)
{
return x * x * x;
}
template<class T>
const T random(const T& a, const T& b)
{
return static_cast<double>(rand()) / RAND_MAX * (b - a) + a;
}
template<class T>
const T randomNormal(const T& sigma)
{
T x1, x2, w;
do
{
x1 = 2.0 * random(0.0, 1.0) - 1.0;
x2 = 2.0 * random(0.0, 1.0) - 1.0;
w = x1 * x1 + x2 * x2;
}
while (w >= 1.0 || w == 0.0);
w = sqrt((-2.0 * log(w)) / w);
return x1 * w * sigma;
}
unsigned long long timeInMicroseconds(void);
double timeInSeconds(void);
void colorDepthImage(cv::Mat& imgDepth,
cv::Mat& imgColoredDepth,
float minRange, float maxRange);
bool colormap(const std::string& name, unsigned char idx,
float& r, float& g, float& b);
std::vector<cv::Point2i> bresLine(int x0, int y0, int x1, int y1);
std::vector<cv::Point2i> bresCircle(int x0, int y0, int r);
void fitCircle(const std::vector<cv::Point2d>& points,
double& centerX, double& centerY, double& radius);
std::vector<cv::Point2d> intersectCircles(double x1, double y1, double r1,
double x2, double y2, double r2);
void LLtoUTM(double latitude, double longitude,
double& utmNorthing, double& utmEasting,
std::string& utmZone);
void UTMtoLL(double utmNorthing, double utmEasting,
const std::string& utmZone,
double& latitude, double& longitude);
long int timestampDiff(uint64_t t1, uint64_t t2);
}
#endif
================================================
FILE: camera_model/include/camodocal/sparse_graph/Transform.h
================================================
#ifndef TRANSFORM_H
#define TRANSFORM_H
#include <boost/shared_ptr.hpp>
#include <eigen3/Eigen/Dense>
#include <stdint.h>
namespace camodocal
{
class Transform
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Transform();
Transform(const Eigen::Matrix4d& H);
Eigen::Quaterniond& rotation(void);
const Eigen::Quaterniond& rotation(void) const;
double* rotationData(void);
const double* const rotationData(void) const;
Eigen::Vector3d& translation(void);
const Eigen::Vector3d& translation(void) const;
double* translationData(void);
const double* const translationData(void) const;
Eigen::Matrix4d toMatrix(void) const;
private:
Eigen::Quaterniond m_q;
Eigen::Vector3d m_t;
};
}
#endif
================================================
FILE: camera_model/instruction
================================================
rosrun camera_model Calibration -w 8 -h 11 -s 70 -i ~/bag/PX/calib/
================================================
FILE: camera_model/package.xml
================================================
<?xml version="1.0"?>
<package>
<name>camera_model</name>
<version>0.0.0</version>
<description>The camera_model package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="dvorak@todo.todo">dvorak</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but mutiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/camera_model</url> -->
<!-- Author tags are optional, mutiple are allowed, one per tag -->
<!-- Authors do not have to be maintianers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *_depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use run_depend for packages you need at runtime: -->
<!-- <run_depend>message_runtime</run_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>std_msgs</build_depend>
<run_depend>roscpp</run_depend>
<run_depend>std_msgs</run_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>
================================================
FILE: camera_model/readme.md
================================================
part of [camodocal](https://github.com/hengli/camodocal)
[Google Ceres](http://ceres-solver.org) is needed.
# Calibration:
Use [intrinsic_calib.cc](https://github.com/dvorak0/camera_model/blob/master/src/intrinsic_calib.cc) to calibrate your camera.
# Undistortion:
See [Camera.h](https://github.com/dvorak0/camera_model/blob/master/include/camodocal/camera_models/Camera.h) for general interface:
- liftProjective: Lift points from the image plane to the projective space.
- spaceToPlane: Projects 3D points to the image plane (Pi function)
================================================
FILE: camera_model/src/calib/CameraCalibration.cc
================================================
#include "camodocal/calib/CameraCalibration.h"
#include <cstdio>
#include <eigen3/Eigen/Dense>
#include <iomanip>
#include <iostream>
#include <algorithm>
#include <fstream>
#include <opencv2/core/core.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include "camodocal/camera_models/CameraFactory.h"
#include "camodocal/sparse_graph/Transform.h"
#include "camodocal/gpl/EigenQuaternionParameterization.h"
#include "camodocal/gpl/EigenUtils.h"
#include "camodocal/camera_models/CostFunctionFactory.h"
#include "ceres/ceres.h"
namespace camodocal
{
CameraCalibration::CameraCalibration()
: m_boardSize(cv::Size(0,0))
, m_squareSize(0.0f)
, m_verbose(false)
{
}
CameraCalibration::CameraCalibration(const Camera::ModelType modelType,
const std::string& cameraName,
const cv::Size& imageSize,
const cv::Size& boardSize,
float squareSize)
: m_boardSize(boardSize)
, m_squareSize(squareSize)
, m_verbose(false)
{
m_camera = CameraFactory::instance()->generateCamera(modelType, cameraName, imageSize);
}
void
CameraCalibration::clear(void)
{
m_imagePoints.clear();
m_scenePoints.clear();
}
void
CameraCalibration::addChessboardData(const std::vector<cv::Point2f>& corners)
{
m_imagePoints.push_back(corners);
std::vector<cv::Point3f> scenePointsInView;
for (int i = 0; i < m_boardSize.height; ++i)
{
for (int j = 0; j < m_boardSize.width; ++j)
{
scenePointsInView.push_back(cv::Point3f(i * m_squareSize, j * m_squareSize, 0.0));
}
}
m_scenePoints.push_back(scenePointsInView);
}
bool
CameraCalibration::calibrate(void)
{
int imageCount = m_imagePoints.size();
// compute intrinsic camera parameters and extrinsic parameters for each of the views
std::vector<cv::Mat> rvecs;
std::vector<cv::Mat> tvecs;
bool ret = calibrateHelper(m_camera, rvecs, tvecs);
m_cameraPoses = cv::Mat(imageCount, 6, CV_64F);
for (int i = 0; i < imageCount; ++i)
{
m_cameraPoses.at<double>(i,0) = rvecs.at(i).at<double>(0);
m_cameraPoses.at<double>(i,1) = rvecs.at(i).at<double>(1);
m_cameraPoses.at<double>(i,2) = rvecs.at(i).at<double>(2);
m_cameraPoses.at<double>(i,3) = tvecs.at(i).at<double>(0);
m_cameraPoses.at<double>(i,4) = tvecs.at(i).at<double>(1);
m_cameraPoses.at<double>(i,5) = tvecs.at(i).at<double>(2);
}
// Compute measurement covariance.
std::vector<std::vector<cv::Point2f> > errVec(m_imagePoints.size());
Eigen::Vector2d errSum = Eigen::Vector2d::Zero();
size_t errCount = 0;
for (size_t i = 0; i < m_imagePoints.size(); ++i)
{
std::vector<cv::Point2f> estImagePoints;
m_camera->projectPoints(m_scenePoints.at(i), rvecs.at(i), tvecs.at(i),
estImagePoints);
for (size_t j = 0; j < m_imagePoints.at(i).size(); ++j)
{
cv::Point2f pObs = m_imagePoints.at(i).at(j);
cv::Point2f pEst = estImagePoints.at(j);
cv::Point2f err = pObs - pEst;
errVec.at(i).push_back(err);
errSum += Eigen::Vector2d(err.x, err.y);
}
errCount += m_imagePoints.at(i).size();
}
Eigen::Vector2d errMean = errSum / static_cast<double>(errCount);
Eigen::Matrix2d measurementCovariance = Eigen::Matrix2d::Zero();
for (size_t i = 0; i < errVec.size(); ++i)
{
for (size_t j = 0; j < errVec.at(i).size(); ++j)
{
cv::Point2f err = errVec.at(i).at(j);
double d0 = err.x - errMean(0);
double d1 = err.y - errMean(1);
measurementCovariance(0,0) += d0 * d0;
measurementCovariance(0,1) += d0 * d1;
measurementCovariance(1,1) += d1 * d1;
}
}
measurementCovariance /= static_cast<double>(errCount);
measurementCovariance(1,0) = measurementCovariance(0,1);
m_measurementCovariance = measurementCovariance;
return ret;
}
int
CameraCalibration::sampleCount(void) const
{
return m_imagePoints.size();
}
std::vector<std::vector<cv::Point2f> >&
CameraCalibration::imagePoints(void)
{
return m_imagePoints;
}
const std::vector<std::vector<cv::Point2f> >&
CameraCalibration::imagePoints(void) const
{
return m_imagePoints;
}
std::vector<std::vector<cv::Point3f> >&
CameraCalibration::scenePoints(void)
{
return m_scenePoints;
}
const std::vector<std::vector<cv::Point3f> >&
CameraCalibration::scenePoints(void) const
{
return m_scenePoints;
}
CameraPtr&
CameraCalibration::camera(void)
{
return m_camera;
}
const CameraConstPtr
CameraCalibration::camera(void) const
{
return m_camera;
}
Eigen::Matrix2d&
CameraCalibration::measurementCovariance(void)
{
return m_measurementCovariance;
}
const Eigen::Matrix2d&
CameraCalibration::measurementCovariance(void) const
{
return m_measurementCovariance;
}
cv::Mat&
CameraCalibration::cameraPoses(void)
{
return m_cameraPoses;
}
const cv::Mat&
CameraCalibration::cameraPoses(void) const
{
return m_cameraPoses;
}
void
CameraCalibration::drawResults(std::vector<cv::Mat>& images) const
{
std::vector<cv::Mat> rvecs, tvecs;
for (size_t i = 0; i < images.size(); ++i)
{
cv::Mat rvec(3, 1, CV_64F);
rvec.at<double>(0) = m_cameraPoses.at<double>(i,0);
rvec.at<double>(1) = m_cameraPoses.at<double>(i,1);
rvec.at<double>(2) = m_cameraPoses.at<double>(i,2);
cv::Mat tvec(3, 1, CV_64F);
tvec.at<double>(0) = m_cameraPoses.at<double>(i,3);
tvec.at<double>(1) = m_cameraPoses.at<double>(i,4);
tvec.at<double>(2) = m_cameraPoses.at<double>(i,5);
rvecs.push_back(rvec);
tvecs.push_back(tvec);
}
int drawShiftBits = 4;
int drawMultiplier = 1 << drawShiftBits;
cv::Scalar green(0, 255, 0);
cv::Scalar red(0, 0, 255);
for (size_t i = 0; i < images.size(); ++i)
{
cv::Mat& image = images.at(i);
if (image.channels() == 1)
{
cv::cvtColor(image, image, cv::COLOR_GRAY2RGB);
}
std::vector<cv::Point2f> estImagePoints;
m_camera->projectPoints(m_scenePoints.at(i), rvecs.at(i), tvecs.at(i),
estImagePoints);
float errorSum = 0.0f;
float errorMax = std::numeric_limits<float>::min();
for (size_t j = 0; j < m_imagePoints.at(i).size(); ++j)
{
cv::Point2f pObs = m_imagePoints.at(i).at(j);
cv::Point2f pEst = estImagePoints.at(j);
cv::circle(image,
cv::Point(cvRound(pObs.x * drawMultiplier),
cvRound(pObs.y * drawMultiplier)),
5, green, 2, cv::LINE_AA, drawShiftBits);
cv::circle(image,
cv::Point(cvRound(pEst.x * drawMultiplier),
cvRound(pEst.y * drawMultiplier)),
5, red, 2, cv::LINE_AA, drawShiftBits);
float error = cv::norm(pObs - pEst);
errorSum += error;
if (error > errorMax)
{
errorMax = error;
}
}
std::ostringstream oss;
oss << "Reprojection error: avg = " << errorSum / m_imagePoints.at(i).size()
<< " max = " << errorMax;
cv::putText(image, oss.str(), cv::Point(10, image.rows - 10),
cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(255, 255, 255),
1, cv::LINE_AA);
}
}
void
CameraCalibration::writeParams(const std::string& filename) const
{
m_camera->writeParametersToYamlFile(filename);
}
bool
CameraCalibration::writeChessboardData(const std::string& filename) const
{
std::ofstream ofs(filename.c_str(), std::ios::out | std::ios::binary);
if (!ofs.is_open())
{
return false;
}
writeData(ofs, m_boardSize.width);
writeData(ofs, m_boardSize.height);
writeData(ofs, m_squareSize);
writeData(ofs, m_measurementCovariance(0,0));
writeData(ofs, m_measurementCovariance(0,1));
writeData(ofs, m_measurementCovariance(1,0));
writeData(ofs, m_measurementCovariance(1,1));
writeData(ofs, m_cameraPoses.rows);
writeData(ofs, m_cameraPoses.cols);
writeData(ofs, m_cameraPoses.type());
for (int i = 0; i < m_cameraPoses.rows; ++i)
{
for (int j = 0; j < m_cameraPoses.cols; ++j)
{
writeData(ofs, m_cameraPoses.at<double>(i,j));
}
}
writeData(ofs, m_imagePoints.size());
for (size_t i = 0; i < m_imagePoints.size(); ++i)
{
writeData(ofs, m_imagePoints.at(i).size());
for (size_t j = 0; j < m_imagePoints.at(i).size(); ++j)
{
const cv::Point2f& ipt = m_imagePoints.at(i).at(j);
writeData(ofs, ipt.x);
writeData(ofs, ipt.y);
}
}
writeData(ofs, m_scenePoints.size());
for (size_t i = 0; i < m_scenePoints.size(); ++i)
{
writeData(ofs, m_scenePoints.at(i).size());
for (size_t j = 0; j < m_scenePoints.at(i).size(); ++j)
{
const cv::Point3f& spt = m_scenePoints.at(i).at(j);
writeData(ofs, spt.x);
writeData(ofs, spt.y);
writeData(ofs, spt.z);
}
}
return true;
}
bool
CameraCalibration::readChessboardData(const std::string& filename)
{
std::ifstream ifs(filename.c_str(), std::ios::in | std::ios::binary);
if (!ifs.is_open())
{
return false;
}
readData(ifs, m_boardSize.width);
readData(ifs, m_boardSize.height);
readData(ifs, m_squareSize);
readData(ifs, m_measurementCovariance(0,0));
readData(ifs, m_measurementCovariance(0,1));
readData(ifs, m_measurementCovariance(1,0));
readData(ifs, m_measurementCovariance(1,1));
int rows, cols, type;
readData(ifs, rows);
readData(ifs, cols);
readData(ifs, type);
m_cameraPoses = cv::Mat(rows, cols, type);
for (int i = 0; i < m_cameraPoses.rows; ++i)
{
for (int j = 0; j < m_cameraPoses.cols; ++j)
{
readData(ifs, m_cameraPoses.at<double>(i,j));
}
}
size_t nImagePointSets;
readData(ifs, nImagePointSets);
m_imagePoints.clear();
m_imagePoints.resize(nImagePointSets);
for (size_t i = 0; i < m_imagePoints.size(); ++i)
{
size_t nImagePoints;
readData(ifs, nImagePoints);
m_imagePoints.at(i).resize(nImagePoints);
for (size_t j = 0; j < m_imagePoints.at(i).size(); ++j)
{
cv::Point2f& ipt = m_imagePoints.at(i).at(j);
readData(ifs, ipt.x);
readData(ifs, ipt.y);
}
}
size_t nScenePointSets;
readData(ifs, nScenePointSets);
m_scenePoints.clear();
m_scenePoints.resize(nScenePointSets);
for (size_t i = 0; i < m_scenePoints.size(); ++i)
{
size_t nScenePoints;
readData(ifs, nScenePoints);
m_scenePoints.at(i).resize(nScenePoints);
for (size_t j = 0; j < m_scenePoints.at(i).size(); ++j)
{
cv::Point3f& spt = m_scenePoints.at(i).at(j);
readData(ifs, spt.x);
readData(ifs, spt.y);
readData(ifs, spt.z);
}
}
return true;
}
void
CameraCalibration::setVerbose(bool verbose)
{
m_verbose = verbose;
}
bool
CameraCalibration::calibrateHelper(CameraPtr& camera,
std::vector<cv::Mat>& rvecs, std::vector<cv::Mat>& tvecs) const
{
rvecs.assign(m_scenePoints.size(), cv::Mat());
tvecs.assign(m_scenePoints.size(), cv::Mat());
// STEP 1: Estimate intrinsics
camera->estimateIntrinsics(m_boardSize, m_scenePoints, m_imagePoints);
// STEP 2: Estimate extrinsics
for (size_t i = 0; i < m_scenePoints.size(); ++i)
{
camera->estimateExtrinsics(m_scenePoints.at(i), m_imagePoints.at(i), rvecs.at(i), tvecs.at(i));
}
if (m_verbose)
{
std::cout << "[" << camera->cameraName() << "] "
<< "# INFO: " << "Initial reprojection error: "
<< std::fixed << std::setprecision(3)
<< camera->reprojectionError(m_scenePoints, m_imagePoints, rvecs, tvecs)
<< " pixels" << std::endl;
}
// STEP 3: optimization using ceres
optimize(camera, rvecs, tvecs);
if (m_verbose)
{
double err = camera->reprojectionError(m_scenePoints, m_imagePoints, rvecs, tvecs);
std::cout << "[" << camera->cameraName() << "] " << "# INFO: Final reprojection error: "
<< err << " pixels" << std::endl;
std::cout << "[" << camera->cameraName() << "] " << "# INFO: "
<< camera->parametersToString() << std::endl;
}
return true;
}
void
CameraCalibration::optimize(CameraPtr& camera,
std::vector<cv::Mat>& rvecs, std::vector<cv::Mat>& tvecs) const
{
// Use ceres to do optimization
ceres::Problem problem;
std::vector<Transform, Eigen::aligned_allocator<Transform> > transformVec(rvecs.size());
for (size_t i = 0; i < rvecs.size(); ++i)
{
Eigen::Vector3d rvec;
cv::cv2eigen(rvecs.at(i), rvec);
transformVec.at(i).rotation() = Eigen::AngleAxisd(rvec.norm(), rvec.normalized());
transformVec.at(i).translation() << tvecs[i].at<double>(0),
tvecs[i].at<double>(1),
tvecs[i].at<double>(2);
}
std::vector<double> intrinsicCameraParams;
m_camera->writeParameters(intrinsicCameraParams);
// create residuals for each observation
for (size_t i = 0; i < m_imagePoints.size(); ++i)
{
for (size_t j = 0; j < m_imagePoints.at(i).size(); ++j)
{
const cv::Point3f& spt = m_scenePoints.at(i).at(j);
const cv::Point2f& ipt = m_imagePoints.at(i).at(j);
ceres::CostFunction* costFunction =
CostFunctionFactory::instance()->generateCostFunction(camera,
Eigen::Vector3d(spt.x, spt.y, spt.z),
Eigen::Vector2d(ipt.x, ipt.y),
CAMERA_INTRINSICS | CAMERA_POSE);
ceres::LossFunction* lossFunction = new ceres::CauchyLoss(1.0);
problem.AddResidualBlock(costFunction, lossFunction,
intrinsicCameraParams.data(),
transformVec.at(i).rotationData(),
transformVec.at(i).translationData());
}
ceres::LocalParameterization* quaternionParameterization =
new EigenQuaternionParameterization;
problem.SetParameterization(transformVec.at(i).rotationData(),
quaternionParameterization);
}
std::cout << "begin ceres" << std::endl;
ceres::Solver::Options options;
options.max_num_iterations = 1000;
options.num_threads = 1;
if (m_verbose)
{
options.minimizer_progress_to_stdout = true;
}
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
std::cout << "end ceres" << std::endl;
if (m_verbose)
{
std::cout << summary.FullReport() << std::endl;
}
camera->readParameters(intrinsicCameraParams);
for (size_t i = 0; i < rvecs.size(); ++i)
{
Eigen::AngleAxisd aa(transformVec.at(i).rotation());
Eigen::Vector3d rvec = aa.angle() * aa.axis();
cv::eigen2cv(rvec, rvecs.at(i));
cv::Mat& tvec = tvecs.at(i);
tvec.at<double>(0) = transformVec.at(i).translation()(0);
tvec.at<double>(1) = transformVec.at(i).translation()(1);
tvec.at<double>(2) = transformVec.at(i).translation()(2);
}
}
template<typename T>
void
CameraCalibration::readData(std::ifstream& ifs, T& data) const
{
char* buffer = new char[sizeof(T)];
ifs.read(buffer, sizeof(T));
data = *(reinterpret_cast<T*>(buffer));
delete[] buffer;
}
template<typename T>
void
CameraCalibration::writeData(std::ofstream& ofs, T data) const
{
char* pData = reinterpret_cast<char*>(&data);
ofs.write(pData, sizeof(T));
}
}
================================================
FILE: camera_model/src/camera_models/Camera.cc
================================================
#include "camodocal/camera_models/Camera.h"
#include "camodocal/camera_models/ScaramuzzaCamera.h"
#include <opencv2/calib3d/calib3d.hpp>
namespace camodocal
{
Camera::Parameters::Parameters(ModelType modelType)
: m_modelType(modelType)
, m_imageWidth(0)
, m_imageHeight(0)
{
switch (modelType)
{
case KANNALA_BRANDT:
m_nIntrinsics = 8;
break;
case PINHOLE:
m_nIntrinsics = 8;
break;
case SCARAMUZZA:
m_nIntrinsics = SCARAMUZZA_CAMERA_NUM_PARAMS;
break;
case MEI:
default:
m_nIntrinsics = 9;
}
}
Camera::Parameters::Parameters(ModelType modelType,
const std::string& cameraName,
int w, int h)
: m_modelType(modelType)
, m_cameraName(cameraName)
, m_imageWidth(w)
, m_imageHeight(h)
{
switch (modelType)
{
case KANNALA_BRANDT:
m_nIntrinsics = 8;
break;
case PINHOLE:
m_nIntrinsics = 8;
break;
case SCARAMUZZA:
m_nIntrinsics = SCARAMUZZA_CAMERA_NUM_PARAMS;
break;
case MEI:
default:
m_nIntrinsics = 9;
}
}
Camera::ModelType&
Camera::Parameters::modelType(void)
{
return m_modelType;
}
std::string&
Camera::Parameters::cameraName(void)
{
return m_cameraName;
}
int&
Camera::Parameters::imageWidth(void)
{
return m_imageWidth;
}
int&
Camera::Parameters::imageHeight(void)
{
return m_imageHeight;
}
Camera::ModelType
Camera::Parameters::modelType(void) const
{
return m_modelType;
}
const std::string&
Camera::Parameters::cameraName(void) const
{
return m_cameraName;
}
int
Camera::Parameters::imageWidth(void) const
{
return m_imageWidth;
}
int
Camera::Parameters::imageHeight(void) const
{
return m_imageHeight;
}
int
Camera::Parameters::nIntrinsics(void) const
{
return m_nIntrinsics;
}
cv::Mat&
Camera::mask(void)
{
return m_mask;
}
const cv::Mat&
Camera::mask(void) const
{
return m_mask;
}
void
Camera::estimateExtrinsics(const std::vector<cv::Point3f>& objectPoints,
const std::vector<cv::Point2f>& imagePoints,
cv::Mat& rvec, cv::Mat& tvec) const
{
std::vector<cv::Point2f> Ms(imagePoints.size());
for (size_t i = 0; i < Ms.size(); ++i)
{
Eigen::Vector3d P;
liftProjective(Eigen::Vector2d(imagePoints.at(i).x, imagePoints.at(i).y), P);
P /= P(2);
Ms.at(i).x = P(0);
Ms.at(i).y = P(1);
}
// assume unit focal length, zero principal point, and zero distortion
cv::solvePnP(objectPoints, Ms, cv::Mat::eye(3, 3, CV_64F), cv::noArray(), rvec, tvec);
}
double
Camera::reprojectionDist(const Eigen::Vector3d& P1, const Eigen::Vector3d& P2) const
{
Eigen::Vector2d p1, p2;
spaceToPlane(P1, p1);
spaceToPlane(P2, p2);
return (p1 - p2).norm();
}
double
Camera::reprojectionError(const std::vector< std::vector<cv::Point3f> >& objectPoints,
const std::vector< std::vector<cv::Point2f> >& imagePoints,
const std::vector<cv::Mat>& rvecs,
const std::vector<cv::Mat>& tvecs,
cv::OutputArray _perViewErrors) const
{
int imageCount = objectPoints.size();
size_t pointsSoFar = 0;
double totalErr = 0.0;
bool computePerViewErrors = _perViewErrors.needed();
cv::Mat perViewErrors;
if (computePerViewErrors)
{
_perViewErrors.create(imageCount, 1, CV_64F);
perViewErrors = _perViewErrors.getMat();
}
for (int i = 0; i < imageCount; ++i)
{
size_t pointCount = imagePoints.at(i).size();
pointsSoFar += pointCount;
std::vector<cv::Point2f> estImagePoints;
projectPoints(objectPoints.at(i), rvecs.at(i), tvecs.at(i),
estImagePoints);
double err = 0.0;
for (size_t j = 0; j < imagePoints.at(i).size(); ++j)
{
err += cv::norm(imagePoints.at(i).at(j) - estImagePoints.at(j));
}
if (computePerViewErrors)
{
perViewErrors.at<double>(i) = err / pointCount;
}
totalErr += err;
}
return totalErr / pointsSoFar;
}
double
Camera::reprojectionError(const Eigen::Vector3d& P,
const Eigen::Quaterniond& camera_q,
const Eigen::Vector3d& camera_t,
const Eigen::Vector2d& observed_p) const
{
Eigen::Vector3d P_cam = camera_q.toRotationMatrix() * P + camera_t;
Eigen::Vector2d p;
spaceToPlane(P_cam, p);
return (p - observed_p).norm();
}
void
Camera::projectPoints(const std::vector<cv::Point3f>& objectPoints,
const cv::Mat& rvec,
const cv::Mat& tvec,
std::vector<cv::Point2f>& imagePoints) const
{
// project 3D object points to the image plane
imagePoints.reserve(objectPoints.size());
//double
cv::Mat R0;
cv::Rodrigues(rvec, R0);
Eigen::MatrixXd R(3,3);
R << R0.at<double>(0,0), R0.at<double>(0,1), R0.at<double>(0,2),
R0.at<double>(1,0), R0.at<double>(1,1), R0.at<double>(1,2),
R0.at<double>(2,0), R0.at<double>(2,1), R0.at<double>(2,2);
Eigen::Vector3d t;
t << tvec.at<double>(0), tvec.at<double>(1), tvec.at<double>(2);
for (size_t i = 0; i < objectPoints.size(); ++i)
{
const cv::Point3f& objectPoint = objectPoints.at(i);
// Rotate and translate
Eigen::Vector3d P;
P << objectPoint.x, objectPoint.y, objectPoint.z;
P = R * P + t;
Eigen::Vector2d p;
spaceToPlane(P, p);
imagePoints.push_back(cv::Point2f(p(0), p(1)));
}
}
}
================================================
FILE: camera_model/src/camera_models/CameraFactory.cc
================================================
#include "camodocal/camera_models/CameraFactory.h"
#include <boost/algorithm/string.hpp>
#include "camodocal/camera_models/CataCamera.h"
#include "camodocal/camera_models/EquidistantCamera.h"
#include "camodocal/camera_models/PinholeCamera.h"
#include "camodocal/camera_models/ScaramuzzaCamera.h"
#include "ceres/ceres.h"
namespace camodocal
{
boost::shared_ptr<CameraFactory> CameraFactory::m_instance;
CameraFactory::CameraFactory()
{
}
boost::shared_ptr<CameraFactory>
CameraFactory::instance(void)
{
if (m_instance.get() == 0)
{
m_instance.reset(new CameraFactory);
}
return m_instance;
}
CameraPtr
CameraFactory::generateCamera(Camera::ModelType modelType,
const std::string& cameraName,
cv::Size imageSize) const
{
switch (modelType)
{
case Camera::KANNALA_BRANDT:
{
EquidistantCameraPtr camera(new EquidistantCamera);
EquidistantCamera::Parameters params = camera->getParameters();
params.cameraName() = cameraName;
params.imageWidth() = imageSize.width;
params.imageHeight() = imageSize.height;
camera->setParameters(params);
return camera;
}
case Camera::PINHOLE:
{
PinholeCameraPtr camera(new PinholeCamera);
PinholeCamera::Parameters params = camera->getParameters();
params.cameraName() = cameraName;
params.imageWidth() = imageSize.width;
params.imageHeight() = imageSize.height;
camera->setParameters(params);
return camera;
}
case Camera::SCARAMUZZA:
{
OCAMCameraPtr camera(new OCAMCamera);
OCAMCamera::Parameters params = camera->getParameters();
params.cameraName() = cameraName;
params.imageWidth() = imageSize.width;
params.imageHeight() = imageSize.height;
camera->setParameters(params);
return camera;
}
case Camera::MEI:
default:
{
CataCameraPtr camera(new CataCamera);
CataCamera::Parameters params = camera->getParameters();
params.cameraName() = cameraName;
params.imageWidth() = imageSize.width;
params.imageHeight() = imageSize.height;
camera->setParameters(params);
return camera;
}
}
}
CameraPtr
CameraFactory::generateCameraFromYamlFile(const std::string& filename)
{
cv::FileStorage fs(filename, cv::FileStorage::READ);
if (!fs.isOpened())
{
return CameraPtr();
}
Camera::ModelType modelType = Camera::MEI;
if (!fs["model_type"].isNone())
{
std::string sModelType;
fs["model_type"] >> sModelType;
if (boost::iequals(sModelType, "kannala_brandt"))
{
modelType = Camera::KANNALA_BRANDT;
}
else if (boost::iequals(sModelType, "mei"))
{
modelType = Camera::MEI;
}
else if (boost::iequals(sModelType, "scaramuzza"))
{
modelType = Camera::SCARAMUZZA;
}
else if (boost::iequals(sModelType, "pinhole"))
{
modelType = Camera::PINHOLE;
}
else
{
std::cerr << "# ERROR: Unknown camera model: " << sModelType << std::endl;
return CameraPtr();
}
}
switch (modelType)
{
case Camera::KANNALA_BRANDT:
{
EquidistantCameraPtr camera(new EquidistantCamera);
EquidistantCamera::Parameters params = camera->getParameters();
params.readFromYamlFile(filename);
camera->setParameters(params);
return camera;
}
case Camera::PINHOLE:
{
PinholeCameraPtr camera(new PinholeCamera);
PinholeCamera::Parameters params = camera->getParameters();
params.readFromYamlFile(filename);
camera->setParameters(params);
return camera;
}
case Camera::SCARAMUZZA:
{
OCAMCameraPtr camera(new OCAMCamera);
OCAMCamera::Parameters params = camera->getParameters();
params.readFromYamlFile(filename);
camera->setParameters(params);
return camera;
}
case Camera::MEI:
default:
{
CataCameraPtr camera(new CataCamera);
CataCamera::Parameters params = camera->getParameters();
params.readFromYamlFile(filename);
camera->setParameters(params);
return camera;
}
}
return CameraPtr();
}
}
================================================
FILE: camera_model/src/camera_models/CataCamera.cc
================================================
#include "camodocal/camera_models/CataCamera.h"
#include <cmath>
#include <cstdio>
#include <eigen3/Eigen/Dense>
#include <iomanip>
#include <iostream>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include "camodocal/gpl/gpl.h"
namespace camodocal
{
CataCamera::Parameters::Parameters()
: Camera::Parameters(MEI)
, m_xi(0.0)
, m_k1(0.0)
, m_k2(0.0)
, m_p1(0.0)
, m_p2(0.0)
, m_gamma1(0.0)
, m_gamma2(0.0)
, m_u0(0.0)
, m_v0(0.0)
{
}
CataCamera::Parameters::Parameters(const std::string& cameraName,
int w, int h,
double xi,
double k1, double k2,
double p1, double p2,
double gamma1, double gamma2,
double u0, double v0)
: Camera::Parameters(MEI, cameraName, w, h)
, m_xi(xi)
, m_k1(k1)
, m_k2(k2)
, m_p1(p1)
, m_p2(p2)
, m_gamma1(gamma1)
, m_gamma2(gamma2)
, m_u0(u0)
, m_v0(v0)
{
}
double&
CataCamera::Parameters::xi(void)
{
return m_xi;
}
double&
CataCamera::Parameters::k1(void)
{
return m_k1;
}
double&
CataCamera::Parameters::k2(void)
{
return m_k2;
}
double&
CataCamera::Parameters::p1(void)
{
return m_p1;
}
double&
CataCamera::Parameters::p2(void)
{
return m_p2;
}
double&
CataCamera::Parameters::gamma1(void)
{
return m_gamma1;
}
double&
CataCamera::Parameters::gamma2(void)
{
return m_gamma2;
}
double&
CataCamera::Parameters::u0(void)
{
return m_u0;
}
double&
CataCamera::Parameters::v0(void)
{
return m_v0;
}
double
CataCamera::Parameters::xi(void) const
{
return m_xi;
}
double
CataCamera::Parameters::k1(void) const
{
return m_k1;
}
double
CataCamera::Parameters::k2(void) const
{
return m_k2;
}
double
CataCamera::Parameters::p1(void) const
{
return m_p1;
}
double
CataCamera::Parameters::p2(void) const
{
return m_p2;
}
double
CataCamera::Parameters::gamma1(void) const
{
return m_gamma1;
}
double
CataCamera::Parameters::gamma2(void) const
{
return m_gamma2;
}
double
CataCamera::Parameters::u0(void) const
{
return m_u0;
}
double
CataCamera::Parameters::v0(void) const
{
return m_v0;
}
bool
CataCamera::Parameters::readFromYamlFile(const std::string& filename)
{
cv::FileStorage fs(filename, cv::FileStorage::READ);
if (!fs.isOpened())
{
return false;
}
if (!fs["model_type"].isNone())
{
std::string sModelType;
fs["model_type"] >> sModelType;
if (sModelType.compare("MEI") != 0)
{
return false;
}
}
m_modelType = MEI;
fs["camera_name"] >> m_cameraName;
m_imageWidth = static_cast<int>(fs["image_width"]);
m_imageHeight = static_cast<int>(fs["image_height"]);
cv::FileNode n = fs["mirror_parameters"];
m_xi = static_cast<double>(n["xi"]);
n = fs["distortion_parameters"];
m_k1 = static_cast<double>(n["k1"]);
m_k2 = static_cast<double>(n["k2"]);
m_p1 = static_cast<double>(n["p1"]);
m_p2 = static_cast<double>(n["p2"]);
n = fs["projection_parameters"];
m_gamma1 = static_cast<double>(n["gamma1"]);
m_gamma2 = static_cast<double>(n["gamma2"]);
m_u0 = static_cast<double>(n["u0"]);
m_v0 = static_cast<double>(n["v0"]);
return true;
}
void
CataCamera::Parameters::writeToYamlFile(const std::string& filename) const
{
cv::FileStorage fs(filename, cv::FileStorage::WRITE);
fs << "model_type" << "MEI";
fs << "camera_name" << m_cameraName;
fs << "image_width" << m_imageWidth;
fs << "image_height" << m_imageHeight;
// mirror: xi
fs << "mirror_parameters";
fs << "{" << "xi" << m_xi << "}";
// radial distortion: k1, k2
// tangential distortion: p1, p2
fs << "distortion_parameters";
fs << "{" << "k1" << m_k1
<< "k2" << m_k2
<< "p1" << m_p1
<< "p2" << m_p2 << "}";
// projection: gamma1, gamma2, u0, v0
fs << "projection_parameters";
fs << "{" << "gamma1" << m_gamma1
<< "gamma2" << m_gamma2
<< "u0" << m_u0
<< "v0" << m_v0 << "}";
fs.release();
}
CataCamera::Parameters&
CataCamera::Parameters::operator=(const CataCamera::Parameters& other)
{
if (this != &other)
{
m_modelType = other.m_modelType;
m_cameraName = other.m_cameraName;
m_imageWidth = other.m_imageWidth;
m_imageHeight = other.m_imageHeight;
m_xi = other.m_xi;
m_k1 = other.m_k1;
m_k2 = other.m_k2;
m_p1 = other.m_p1;
m_p2 = other.m_p2;
m_gamma1 = other.m_gamma1;
m_gamma2 = other.m_gamma2;
m_u0 = other.m_u0;
m_v0 = other.m_v0;
}
return *this;
}
std::ostream&
operator<< (std::ostream& out, const CataCamera::Parameters& params)
{
out << "Camera Parameters:" << std::endl;
out << " model_type " << "MEI" << std::endl;
out << " camera_name " << params.m_cameraName << std::endl;
out << " image_width " << params.m_imageWidth << std::endl;
out << " image_height " << params.m_imageHeight << std::endl;
out << "Mirror Parameters" << std::endl;
out << std::fixed << std::setprecision(10);
out << " xi " << params.m_xi << std::endl;
// radial distortion: k1, k2
// tangential distortion: p1, p2
out << "Distortion Parameters" << std::endl;
out << " k1 " << params.m_k1 << std::endl
<< " k2 " << params.m_k2 << std::endl
<< " p1 " << params.m_p1 << std::endl
<< " p2 " << params.m_p2 << std::endl;
// projection: gamma1, gamma2, u0, v0
out << "Projection Parameters" << std::endl;
out << " gamma1 " << params.m_gamma1 << std::endl
<< " gamma2 " << params.m_gamma2 << std::endl
<< " u0 " << params.m_u0 << std::endl
<< " v0 " << params.m_v0 << std::endl;
return out;
}
CataCamera::CataCamera()
: m_inv_K11(1.0)
, m_inv_K13(0.0)
, m_inv_K22(1.0)
, m_inv_K23(0.0)
, m_noDistortion(true)
{
}
CataCamera::CataCamera(const std::string& cameraName,
int imageWidth, int imageHeight,
double xi, double k1, double k2, double p1, double p2,
double gamma1, double gamma2, double u0, double v0)
: mParameters(cameraName, imageWidth, imageHeight,
xi, k1, k2, p1, p2, gamma1, gamma2, u0, v0)
{
if ((mParameters.k1() == 0.0) &&
(mParameters.k2() == 0.0) &&
(mParameters.p1() == 0.0) &&
(mParameters.p2() == 0.0))
{
m_noDistortion = true;
}
else
{
m_noDistortion = false;
}
// Inverse camera projection matrix parameters
m_inv_K11 = 1.0 / mParameters.gamma1();
m_inv_K13 = -mParameters.u0() / mParameters.gamma1();
m_inv_K22 = 1.0 / mParameters.gamma2();
m_inv_K23 = -mParameters.v0() / mParameters.gamma2();
}
CataCamera::CataCamera(const CataCamera::Parameters& params)
: mParameters(params)
{
if ((mParameters.k1() == 0.0) &&
(mParameters.k2() == 0.0) &&
(mParameters.p1() == 0.0) &&
(mParameters.p2() == 0.0))
{
m_noDistortion = true;
}
else
{
m_noDistortion = false;
}
// Inverse camera projection matrix parameters
m_inv_K11 = 1.0 / mParameters.gamma1();
m_inv_K13 = -mParameters.u0() / mParameters.gamma1();
m_inv_K22 = 1.0 / mParameters.gamma2();
m_inv_K23 = -mParameters.v0() / mParameters.gamma2();
}
Camera::ModelType
CataCamera::modelType(void) const
{
return mParameters.modelType();
}
const std::string&
CataCamera::cameraName(void) const
{
return mParameters.cameraName();
}
int
CataCamera::imageWidth(void) const
{
return mParameters.imageWidth();
}
int
CataCamera::imageHeight(void) const
{
return mParameters.imageHeight();
}
void
CataCamera::estimateIntrinsics(const cv::Size& boardSize,
const std::vector< std::vector<cv::Point3f> >& objectPoints,
const std::vector< std::vector<cv::Point2f> >& imagePoints)
{
Parameters params = getParameters();
double u0 = params.imageWidth() / 2.0;
double v0 = params.imageHeight() / 2.0;
double gamma0 = 0.0;
double minReprojErr = std::numeric_limits<double>::max();
std::vector<cv::Mat> rvecs, tvecs;
rvecs.assign(objectPoints.size(), cv::Mat());
tvecs.assign(objectPoints.size(), cv::Mat());
params.xi() = 1.0;
params.k1() = 0.0;
params.k2() = 0.0;
params.p1() = 0.0;
params.p2() = 0.0;
params.u0() = u0;
params.v0() = v0;
// Initialize gamma (focal length)
// Use non-radial line image and xi = 1
for (size_t i = 0; i < imagePoints.size(); ++i)
{
for (int r = 0; r < boardSize.height; ++r)
{
cv::Mat P(boardSize.width, 4, CV_64F);
for (int c = 0; c < boardSize.width; ++c)
{
const cv::Point2f& imagePoint = imagePoints.at(i).at(r * boardSize.width + c);
double u = imagePoint.x - u0;
double v = imagePoint.y - v0;
P.at<double>(c, 0) = u;
P.at<double>(c, 1) = v;
P.at<double>(c, 2) = 0.5;
P.at<double>(c, 3) = -0.5 * (square(u) + square(v));
}
cv::Mat C;
cv::SVD::solveZ(P, C);
double t = square(C.at<double>(0)) + square(C.at<double>(1)) + C.at<double>(2) * C.at<double>(3);
if (t < 0.0)
{
continue;
}
// check that line image is not radial
double d = sqrt(1.0 / t);
double nx = C.at<double>(0) * d;
double ny = C.at<double>(1) * d;
if (hypot(nx, ny) > 0.95)
{
continue;
}
double gamma = sqrt(C.at<double>(2) / C.at<double>(3));
params.gamma1() = gamma;
params.gamma2() = gamma;
setParameters(params);
for (size_t j = 0; j < objectPoints.size(); ++j)
{
estimateExtrinsics(objectPoints.at(j), imagePoints.at(j), rvecs.at(j), tvecs.at(j));
}
double reprojErr = reprojectionError(objectPoints, imagePoints, rvecs, tvecs, cv::noArray());
if (reprojErr < minReprojErr)
{
minReprojErr = reprojErr;
gamma0 = gamma;
}
}
}
if (gamma0 <= 0.0 && minReprojErr >= std::numeric_limits<double>::max())
{
std::cout << "[" << params.cameraName() << "] "
<< "# INFO: CataCamera model fails with given data. " << std::endl;
return;
}
params.gamma1() = gamma0;
params.gamma2() = gamma0;
setParameters(params);
}
/**
* \brief Lifts a point from the image plane to the unit sphere
*
* \param p image coordinates
* \param P coordinates of the point on the sphere
*/
void
CataCamera::liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const
{
double mx_d, my_d,mx2_d, mxy_d, my2_d, mx_u, my_u;
double rho2_d, rho4_d, radDist_d, Dx_d, Dy_d, inv_denom_d;
double lambda;
// Lift points to normalised plane
mx_d = m_inv_K11 * p(0) + m_inv_K13;
my_d = m_inv_K22 * p(1) + m_inv_K23;
if (m_noDistortion)
{
mx_u = mx_d;
my_u = my_d;
}
else
{
// Apply inverse distortion model
if (0)
{
double k1 = mParameters.k1();
double k2 = mParameters.k2();
double p1 = mParameters.p1();
double p2 = mParameters.p2();
// Inverse distortion model
// proposed by Heikkila
mx2_d = mx_d*mx_d;
my2_d = my_d*my_d;
mxy_d = mx_d*my_d;
rho2_d = mx2_d+my2_d;
rho4_d = rho2_d*rho2_d;
radDist_d = k1*rho2_d+k2*rho4_d;
Dx_d = mx_d*radDist_d + p2*(rho2_d+2*mx2_d) + 2*p1*mxy_d;
Dy_d = my_d*radDist_d + p1*(rho2_d+2*my2_d) + 2*p2*mxy_d;
inv_denom_d = 1/(1+4*k1*rho2_d+6*k2*rho4_d+8*p1*my_d+8*p2*mx_d);
mx_u = mx_d - inv_denom_d*Dx_d;
my_u = my_d - inv_denom_d*Dy_d;
}
else
{
// Recursive distortion model
int n = 6;
Eigen::Vector2d d_u;
distortion(Eigen::Vector2d(mx_d, my_d), d_u);
// Approximate value
mx_u = mx_d - d_u(0);
my_u = my_d - d_u(1);
for (int i = 1; i < n; ++i)
{
distortion(Eigen::Vector2d(mx_u, my_u), d_u);
mx_u = mx_d - d_u(0);
my_u = my_d - d_u(1);
}
}
}
// Lift normalised points to the sphere (inv_hslash)
double xi = mParameters.xi();
if (xi == 1.0)
{
lambda = 2.0 / (mx_u * mx_u + my_u * my_u + 1.0);
P << lambda * mx_u, lambda * my_u, lambda - 1.0;
}
else
{
lambda = (xi + sqrt(1.0 + (1.0 - xi * xi) * (mx_u * mx_u + my_u * my_u))) / (1.0 + mx_u * mx_u + my_u * my_u);
P << lambda * mx_u, lambda * my_u, lambda - xi;
}
}
/**
* \brief Lifts a point from the image plane to its projective ray
*
* \param p image coordinates
* \param P coordinates of the projective ray
*/
void
CataCamera::liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const
{
double mx_d, my_d,mx2_d, mxy_d, my2_d, mx_u, my_u;
double rho2_d, rho4_d, radDist_d, Dx_d, Dy_d, inv_denom_d;
//double lambda;
// Lift points to normalised plane
mx_d = m_inv_K11 * p(0) + m_inv_K13;
my_d = m_inv_K22 * p(1) + m_inv_K23;
if (m_noDistortion)
{
mx_u = mx_d;
my_u = my_d;
}
else
{
if (0)
{
double k1 = mParameters.k1();
double k2 = mParameters.k2();
double p1 = mParameters.p1();
double p2 = mParameters.p2();
// Apply inverse distortion model
// proposed by Heikkila
mx2_d = mx_d*mx_d;
my2_d = my_d*my_d;
mxy_d = mx_d*my_d;
rho2_d = mx2_d+my2_d;
rho4_d = rho2_d*rho2_d;
radDist_d = k1*rho2_d+k2*rho4_d;
Dx_d = mx_d*radDist_d + p2*(rho2_d+2*mx2_d) + 2*p1*mxy_d;
Dy_d = my_d*radDist_d + p1*(rho2_d+2*my2_d) + 2*p2*mxy_d;
inv_denom_d = 1/(1+4*k1*rho2_d+6*k2*rho4_d+8*p1*my_d+8*p2*mx_d);
mx_u = mx_d - inv_denom_d*Dx_d;
my_u = my_d - inv_denom_d*Dy_d;
}
else
{
// Recursive distortion model
int n = 8;
Eigen::Vector2d d_u;
distortion(Eigen::Vector2d(mx_d, my_d), d_u);
// Approximate value
mx_u = mx_d - d_u(0);
my_u = my_d - d_u(1);
for (int i = 1; i < n; ++i)
{
distortion(Eigen::Vector2d(mx_u, my_u), d_u);
mx_u = mx_d - d_u(0);
my_u = my_d - d_u(1);
}
}
}
// Obtain a projective ray
double xi = mParameters.xi();
if (xi == 1.0)
{
P << mx_u, my_u, (1.0 - mx_u * mx_u - my_u * my_u) / 2.0;
}
else
{
// Reuse variable
rho2_d = mx_u * mx_u + my_u * my_u;
P << mx_u, my_u, 1.0 - xi * (rho2_d + 1.0) / (xi + sqrt(1.0 + (1.0 - xi * xi) * rho2_d));
}
}
/**
* \brief Project a 3D point (\a x,\a y,\a z) to the image plane in (\a u,\a v)
*
* \param P 3D point coordinates
* \param p return value, contains the image point coordinates
*/
void
CataCamera::spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const
{
Eigen::Vector2d p_u, p_d;
// Project points to the normalised plane
double z = P(2) + mParameters.xi() * P.norm();
p_u << P(0) / z, P(1) / z;
if (m_noDistortion)
{
p_d = p_u;
}
else
{
// Apply distortion
Eigen::Vector2d d_u;
distortion(p_u, d_u);
p_d = p_u + d_u;
}
// Apply generalised projection matrix
p << mParameters.gamma1() * p_d(0) + mParameters.u0(),
mParameters.gamma2() * p_d(1) + mParameters.v0();
}
#if 0
/**
* \brief Project a 3D point to the image plane and calculate Jacobian
*
* \param P 3D point coordinates
* \param p return value, contains the image point coordinates
*/
void
CataCamera::spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p,
Eigen::Matrix<double,2,3>& J) const
{
double xi = mParameters.xi();
Eigen::Vector2d p_u, p_d;
double norm, inv_denom;
double dxdmx, dydmx, dxdmy, dydmy;
norm = P.norm();
// Project points to the normalised plane
inv_denom = 1.0 / (P(2) + xi * norm);
p_u << inv_denom * P(0), inv_denom * P(1);
// Calculate jacobian
inv_denom = inv_denom * inv_denom / norm;
double dudx = inv_denom * (norm * P(2) + xi * (P(1) * P(1) + P(2) * P(2)));
double dvdx = -inv_denom * xi * P(0) * P(1);
double dudy = dvdx;
double dvdy = inv_denom * (norm * P(2) + xi * (P(0) * P(0) + P(2) * P(2)));
inv_denom = inv_denom * (-xi * P(2) - norm); // reuse variable
double dudz = P(0) * inv_denom;
double dvdz = P(1) * inv_denom;
if (m_noDistortion)
{
p_d = p_u;
}
else
{
// Apply distortion
Eigen::Vector2d d_u;
distortion(p_u, d_u);
p_d = p_u + d_u;
}
double gamma1 = mParameters.gamma1();
double gamma2 = mParameters.gamma2();
// Make the product of the jacobians
// and add projection matrix jacobian
inv_denom = gamma1 * (dudx * dxdmx + dvdx * dxdmy); // reuse
dvdx = gamma2 * (dudx * dydmx + dvdx * dydmy);
dudx = inv_denom;
inv_denom = gamma1 * (dudy * dxdmx + dvdy * dxdmy); // reuse
dvdy = gamma2 * (dudy * dydmx + dvdy * dydmy);
dudy = inv_denom;
inv_denom = gamma1 * (dudz * dxdmx + dvdz * dxdmy); // reuse
dvdz = gamma2 * (dudz * dydmx + dvdz * dydmy);
dudz = inv_denom;
// Apply generalised projection matrix
p << gamma1 * p_d(0) + mParameters.u0(),
gamma2 * p_d(1) + mParameters.v0();
J << dudx, dudy, dudz,
dvdx, dvdy, dvdz;
}
#endif
/**
* \brief Projects an undistorted 2D point p_u to the image plane
*
* \param p_u 2D point coordinates
* \return image point coordinates
*/
void
CataCamera::undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const
{
Eigen::Vector2d p_d;
if (m_noDistortion)
{
p_d = p_u;
}
else
{
// Apply distortion
Eigen::Vector2d d_u;
distortion(p_u, d_u);
p_d = p_u + d_u;
}
// Apply generalised projection matrix
p << mParameters.gamma1() * p_d(0) + mParameters.u0(),
mParameters.gamma2() * p_d(1) + mParameters.v0();
}
/**
* \brief Apply distortion to input point (from the normalised plane)
*
* \param p_u undistorted coordinates of point on the normalised plane
* \return to obtain the distorted point: p_d = p_u + d_u
*/
void
CataCamera::distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u) const
{
double k1 = mParameters.k1();
double k2 = mParameters.k2();
double p1 = mParameters.p1();
double p2 = mParameters.p2();
double mx2_u, my2_u, mxy_u, rho2_u, rad_dist_u;
mx2_u = p_u(0) * p_u(0);
my2_u = p_u(1) * p_u(1);
mxy_u = p_u(0) * p_u(1);
rho2_u = mx2_u + my2_u;
rad_dist_u = k1 * rho2_u + k2 * rho2_u * rho2_u;
d_u << p_u(0) * rad_dist_u + 2.0 * p1 * mxy_u + p2 * (rho2_u + 2.0 * mx2_u),
p_u(1) * rad_dist_u + 2.0 * p2 * mxy_u + p1 * (rho2_u + 2.0 * my2_u);
}
/**
* \brief Apply distortion to input point (from the normalised plane)
* and calculate Jacobian
*
* \param p_u undistorted coordinates of point on the normalised plane
* \return to obtain the distorted point: p_d = p_u + d_u
*/
void
CataCamera::distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u,
Eigen::Matrix2d& J) const
{
double k1 = mParameters.k1();
double k2 = mParameters.k2();
double p1 = mParameters.p1();
double p2 = mParameters.p2();
double mx2_u, my2_u, mxy_u, rho2_u, rad_dist_u;
mx2_u = p_u(0) * p_u(0);
my2_u = p_u(1) * p_u(1);
mxy_u = p_u(0) * p_u(1);
rho2_u = mx2_u + my2_u;
rad_dist_u = k1 * rho2_u + k2 * rho2_u * rho2_u;
d_u << p_u(0) * rad_dist_u + 2.0 * p1 * mxy_u + p2 * (rho2_u + 2.0 * mx2_u),
p_u(1) * rad_dist_u + 2.0 * p2 * mxy_u + p1 * (rho2_u + 2.0 * my2_u);
double dxdmx = 1.0 + rad_dist_u + k1 * 2.0 * mx2_u + k2 * rho2_u * 4.0 * mx2_u + 2.0 * p1 * p_u(1) + 6.0 * p2 * p_u(0);
double dydmx = k1 * 2.0 * p_u(0) * p_u(1) + k2 * 4.0 * rho2_u * p_u(0) * p_u(1) + p1 * 2.0 * p_u(0) + 2.0 * p2 * p_u(1);
double dxdmy = dydmx;
double dydmy = 1.0 + rad_dist_u + k1 * 2.0 * my2_u + k2 * rho2_u * 4.0 * my2_u + 6.0 * p1 * p_u(1) + 2.0 * p2 * p_u(0);
J << dxdmx, dxdmy,
dydmx, dydmy;
}
void
CataCamera::initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale) const
{
cv::Size imageSize(mParameters.imageWidth(), mParameters.imageHeight());
cv::Mat mapX = cv::Mat::zeros(imageSize, CV_32F);
cv::Mat mapY = cv::Mat::zeros(imageSize, CV_32F);
for (int v = 0; v < imageSize.height; ++v)
{
for (int u = 0; u < imageSize.width; ++u)
{
double mx_u = m_inv_K11 / fScale * u + m_inv_K13 / fScale;
double my_u = m_inv_K22 / fScale * v + m_inv_K23 / fScale;
double xi = mParameters.xi();
double d2 = mx_u * mx_u + my_u * my_u;
Eigen::Vector3d P;
P << mx_u, my_u, 1.0 - xi * (d2 + 1.0) / (xi + sqrt(1.0 + (1.0 - xi * xi) * d2));
Eigen::Vector2d p;
spaceToPlane(P, p);
mapX.at<float>(v,u) = p(0);
mapY.at<float>(v,u) = p(1);
}
}
cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false);
}
cv::Mat
CataCamera::initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2,
float fx, float fy,
cv::Size imageSize,
float cx, float cy,
cv::Mat rmat) const
{
if (imageSize == cv::Size(0, 0))
{
imageSize = cv::Size(mParameters.imageWidth(), mParameters.imageHeight());
}
cv::Mat mapX = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F);
cv::Mat mapY = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F);
Eigen::Matrix3f K_rect;
if (cx == -1.0f && cy == -1.0f)
{
K_rect << fx, 0, imageSize.width / 2,
0, fy, imageSize.height / 2,
0, 0, 1;
}
else
{
K_rect << fx, 0, cx,
0, fy, cy,
0, 0, 1;
}
if (fx == -1.0f || fy == -1.0f)
{
K_rect(0,0) = mParameters.gamma1();
K_rect(1,1) = mParameters.gamma2();
}
Eigen::Matrix3f K_rect_inv = K_rect.inverse();
Eigen::Matrix3f R, R_inv;
cv::cv2eigen(rmat, R);
R_inv = R.inverse();
for (int v = 0; v < imageSize.height; ++v)
{
for (int u = 0; u < imageSize.width; ++u)
{
Eigen::Vector3f xo;
xo << u, v, 1;
Eigen::Vector3f uo = R_inv * K_rect_inv * xo;
Eigen::Vector2d p;
spaceToPlane(uo.cast<double>(), p);
mapX.at<float>(v,u) = p(0);
mapY.at<float>(v,u) = p(1);
}
}
cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false);
cv::Mat K_rect_cv;
cv::eigen2cv(K_rect, K_rect_cv);
return K_rect_cv;
}
int
CataCamera::parameterCount(void) const
{
return 9;
}
const CataCamera::Parameters&
CataCamera::getParameters(void) const
{
return mParameters;
}
void
CataCamera::setParameters(const CataCamera::Parameters& parameters)
{
mParameters = parameters;
if ((mParameters.k1() == 0.0) &&
(mParameters.k2() == 0.0) &&
(mParameters.p1() == 0.0) &&
(mParameters.p2() == 0.0))
{
m_noDistortion = true;
}
else
{
m_noDistortion = false;
}
m_inv_K11 = 1.0 / mParameters.gamma1();
m_inv_K13 = -mParameters.u0() / mParameters.gamma1();
m_inv_K22 = 1.0 / mParameters.gamma2();
m_inv_K23 = -mParameters.v0() / mParameters.gamma2();
}
void
CataCamera::readParameters(const std::vector<double>& parameterVec)
{
if ((int)parameterVec.size() != parameterCount())
{
return;
}
Parameters params = getParameters();
params.xi() = parameterVec.at(0);
params.k1() = parameterVec.at(1);
params.k2() = parameterVec.at(2);
params.p1() = parameterVec.at(3);
params.p2() = parameterVec.at(4);
params.gamma1() = parameterVec.at(5);
params.gamma2() = parameterVec.at(6);
params.u0() = parameterVec.at(7);
params.v0() = parameterVec.at(8);
setParameters(params);
}
void
CataCamera::writeParameters(std::vector<double>& parameterVec) const
{
parameterVec.resize(parameterCount());
parameterVec.at(0) = mParameters.xi();
parameterVec.at(1) = mParameters.k1();
parameterVec.at(2) = mParameters.k2();
parameterVec.at(3) = mParameters.p1();
parameterVec.at(4) = mParameters.p2();
parameterVec.at(5) = mParameters.gamma1();
parameterVec.at(6) = mParameters.gamma2();
parameterVec.at(7) = mParameters.u0();
parameterVec.at(8) = mParameters.v0();
}
void
CataCamera::writeParametersToYamlFile(const std::string& filename) const
{
mParameters.writeToYamlFile(filename);
}
std::string
CataCamera::parametersToString(void) const
{
std::ostringstream oss;
oss << mParameters;
return oss.str();
}
}
================================================
FILE: camera_model/src/camera_models/CostFunctionFactory.cc
================================================
#include "camodocal/camera_models/CostFunctionFactory.h"
#include "ceres/ceres.h"
#include "camodocal/camera_models/CataCamera.h"
#include "camodocal/camera_models/EquidistantCamera.h"
#include "camodocal/camera_models/PinholeCamera.h"
#include "camodocal/camera_models/ScaramuzzaCamera.h"
namespace camodocal
{
template<typename T>
void
worldToCameraTransform(const T* const q_cam_odo, const T* const t_cam_odo,
const T* const p_odo, const T* const att_odo,
T* q, T* t, bool optimize_cam_odo_z = true)
{
Eigen::Quaternion<T> q_z_inv(cos(att_odo[0] / T(2)), T(0), T(0), -sin(att_odo[0] / T(2)));
Eigen::Quaternion<T> q_y_inv(cos(att_odo[1] / T(2)), T(0), -sin(att_odo[1] / T(2)), T(0));
Eigen::Quaternion<T> q_x_inv(cos(att_odo[2] / T(2)), -sin(att_odo[2] / T(2)), T(0), T(0));
Eigen::Quaternion<T> q_zyx_inv = q_x_inv * q_y_inv * q_z_inv;
T q_odo[4] = {q_zyx_inv.w(), q_zyx_inv.x(), q_zyx_inv.y(), q_zyx_inv.z()};
T q_odo_cam[4] = {q_cam_odo[3], -q_cam_odo[0], -q_cam_odo[1], -q_cam_odo[2]};
T q0[4];
ceres::QuaternionProduct(q_odo_cam, q_odo, q0);
T t0[3];
T t_odo[3] = {p_odo[0], p_odo[1], p_odo[2]};
ceres::QuaternionRotatePoint(q_odo, t_odo, t0);
t0[0] += t_cam_odo[0];
t0[1] += t_cam_odo[1];
if (optimize_cam_odo_z)
{
t0[2] += t_cam_odo[2];
}
ceres::QuaternionRotatePoint(q_odo_cam, t0, t);
t[0] = -t[0];
t[1] = -t[1];
t[2] = -t[2];
// Convert quaternion from Ceres convention (w, x, y, z)
// to Eigen convention (x, y, z, w)
q[0] = q0[1];
q[1] = q0[2];
q[2] = q0[3];
q[3] = q0[0];
}
template<class CameraT>
class ReprojectionError1
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
ReprojectionError1(const Eigen::Vector3d& observed_P,
const Eigen::Vector2d& observed_p)
: m_observed_P(observed_P), m_observed_p(observed_p)
, m_sqrtPrecisionMat(Eigen::Matrix2d::Identity()) {}
ReprojectionError1(const Eigen::Vector3d& observed_P,
const Eigen::Vector2d& observed_p,
const Eigen::Matrix2d& sqrtPrecisionMat)
: m_observed_P(observed_P), m_observed_p(observed_p)
, m_sqrtPrecisionMat(sqrtPrecisionMat) {}
ReprojectionError1(const std::vector<double>& intrinsic_params,
const Eigen::Vector3d& observed_P,
const Eigen::Vector2d& observed_p)
: m_intrinsic_params(intrinsic_params)
, m_observed_P(observed_P), m_observed_p(observed_p) {}
// variables: camera intrinsics and camera extrinsics
template <typename T>
bool operator()(const T* const intrinsic_params,
const T* const q,
const T* const t,
T* residuals) const
{
Eigen::Matrix<T, 3, 1> P = m_observed_P.cast<T>();
Eigen::Matrix<T, 2, 1> predicted_p;
CameraT::spaceToPlane(intrinsic_params, q, t, P, predicted_p);
Eigen::Matrix<T, 2, 1> e = predicted_p - m_observed_p.cast<T>();
Eigen::Matrix<T, 2, 1> e_weighted = m_sqrtPrecisionMat.cast<T>() * e;
residuals[0] = e_weighted(0);
residuals[1] = e_weighted(1);
return true;
}
// variables: camera-odometry transforms and odometry poses
template <typename T>
bool operator()(const T* const q_cam_odo, const T* const t_cam_odo,
const T* const p_odo, const T* const att_odo,
T* residuals) const
{
T q[4], t[3];
worldToCameraTransform(q_cam_odo, t_cam_odo, p_odo, att_odo, q, t);
Eigen::Matrix<T, 3, 1> P = m_observed_P.cast<T>();
std::vector<T> intrinsic_params(m_intrinsic_params.begin(), m_intrinsic_params.end());
// project 3D object point to the image plane
Eigen::Matrix<T, 2, 1> predicted_p;
CameraT::spaceToPlane(intrinsic_params.data(), q, t, P, predicted_p);
residuals[0] = predicted_p(0) - T(m_observed_p(0));
residuals[1] = predicted_p(1) - T(m_observed_p(1));
return true;
}
//private:
// camera intrinsics
std::vector<double> m_intrinsic_params;
// observed 3D point
Eigen::Vector3d m_observed_P;
// observed 2D point
Eigen::Vector2d m_observed_p;
// square root of precision matrix
Eigen::Matrix2d m_sqrtPrecisionMat;
};
// variables: camera extrinsics, 3D point
template<class CameraT>
class ReprojectionError2
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
ReprojectionError2(const std::vector<double>& intrinsic_params,
const Eigen::Vector2d& observed_p)
: m_intrinsic_params(intrinsic_params), m_observed_p(observed_p) {}
template <typename T>
bool operator()(const T* const q, const T* const t,
const T* const point, T* residuals) const
{
Eigen::Matrix<T, 3, 1> P;
P(0) = T(point[0]);
P(1) = T(point[1]);
P(2) = T(point[2]);
std::vector<T> intrinsic_params(m_intrinsic_params.begin(), m_intrinsic_params.end());
// project 3D object point to the image plane
Eigen::Matrix<T, 2, 1> predicted_p;
CameraT::spaceToPlane(intrinsic_params.data(), q, t, P, predicted_p);
residuals[0] = predicted_p(0) - T(m_observed_p(0));
residuals[1] = predicted_p(1) - T(m_observed_p(1));
return true;
}
private:
// camera intrinsics
std::vector<double> m_intrinsic_params;
// observed 2D point
Eigen::Vector2d m_observed_p;
};
template<class CameraT>
class ReprojectionError3
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
ReprojectionError3(const Eigen::Vector2d& observed_p)
: m_observed_p(observed_p)
, m_sqrtPrecisionMat(Eigen::Matrix2d::Identity())
, m_optimize_cam_odo_z(true) {}
ReprojectionError3(const Eigen::Vector2d& observed_p,
const Eigen::Matrix2d& sqrtPrecisionMat)
: m_observed_p(observed_p)
, m_sqrtPrecisionMat(sqrtPrecisionMat)
, m_optimize_cam_odo_z(true) {}
ReprojectionError3(const std::vector<double>& intrinsic_params,
const Eigen::Vector2d& observed_p)
: m_intrinsic_params(intrinsic_params)
, m_observed_p(observed_p)
, m_sqrtPrecisionMat(Eigen::Matrix2d::Identity())
, m_optimize_cam_odo_z(true) {}
ReprojectionError3(const std::vector<double>& intrinsic_params,
const Eigen::Vector2d& observed_p,
const Eigen::Matrix2d& sqrtPrecisionMat)
: m_intrinsic_params(intrinsic_params)
, m_observed_p(observed_p)
, m_sqrtPrecisionMat(sqrtPrecisionMat)
, m_optimize_cam_odo_z(true) {}
ReprojectionError3(const std::vector<double>& intrinsic_params,
const Eigen::Vector3d& odo_pos,
const Eigen::Vector3d& odo_att,
const Eigen::Vector2d& observed_p,
bool optimize_cam_odo_z)
: m_intrinsic_params(intrinsic_params)
, m_odo_pos(odo_pos), m_odo_att(odo_att)
, m_observed_p(observed_p)
, m_optimize_cam_odo_z(optimize_cam_odo_z) {}
ReprojectionError3(const std::vector<double>& intrinsic_params,
const Eigen::Quaterniond& cam_odo_q,
const Eigen::Vector3d& cam_odo_t,
const Eigen::Vector3d& odo_pos,
const Eigen::Vector3d& odo_att,
const Eigen::Vector2d& observed_p)
: m_intrinsic_params(intrinsic_params)
, m_cam_odo_q(cam_odo_q), m_cam_odo_t(cam_odo_t)
, m_odo_pos(odo_pos), m_odo_att(odo_att)
, m_observed_p(observed_p)
, m_optimize_cam_odo_z(true) {}
// variables: camera intrinsics, camera-to-odometry transform,
// odometry extrinsics, 3D point
template <typename T>
bool operator()(const T* const intrinsic_params,
const T* const q_cam_odo, const T* const t_cam_odo,
const T* const p_odo, const T* const att_odo,
const T* const point, T* residuals) const
{
T q[4], t[3];
worldToCameraTransform(q_cam_odo, t_cam_odo, p_odo, att_odo, q, t, m_optimize_cam_odo_z);
Eigen::Matrix<T, 3, 1> P(point[0], point[1], point[2]);
// project 3D object point to the image plane
Eigen::Matrix<T, 2, 1> predicted_p;
CameraT::spaceToPlane(intrinsic_params, q, t, P, predicted_p);
Eigen::Matrix<T, 2, 1> err = predicted_p - m_observed_p.cast<T>();
Eigen::Matrix<T, 2, 1> err_weighted = m_sqrtPrecisionMat.cast<T>() * err;
residuals[0] = err_weighted(0);
residuals[1] = err_weighted(1);
return true;
}
// variables: camera-to-odometry transform, 3D point
template <typename T>
bool operator()(const T* const q_cam_odo, const T* const t_cam_odo,
const T* const point, T* residuals) const
{
T p_odo[3] = {T(m_odo_pos(0)), T(m_odo_pos(1)), T(m_odo_pos(2))};
T att_odo[3] = {T(m_odo_att(0)), T(m_odo_att(1)), T(m_odo_att(2))};
T q[4], t[3];
worldToCameraTransform(q_cam_odo, t_cam_odo, p_odo, att_odo, q, t, m_optimize_cam_odo_z);
std::vector<T> intrinsic_params(m_intrinsic_params.begin(), m_intrinsic_params.end());
Eigen::Matrix<T, 3, 1> P(point[0], point[1], point[2]);
// project 3D object point to the image plane
Eigen::Matrix<T, 2, 1> predicted_p;
CameraT::spaceToPlane(intrinsic_params.data(), q, t, P, predicted_p);
residuals[0] = predicted_p(0) - T(m_observed_p(0));
residuals[1] = predicted_p(1) - T(m_observed_p(1));
return true;
}
// variables: camera-to-odometry transform, odometry extrinsics, 3D point
template <typename T>
bool operator()(const T* const q_cam_odo, const T* const t_cam_odo,
const T* const p_odo, const T* const att_odo,
const T* const point, T* residuals) const
{
T q[4], t[3];
worldToCameraTransform(q_cam_odo, t_cam_odo, p_odo, att_odo, q, t, m_optimize_cam_odo_z);
std::vector<T> intrinsic_params(m_intrinsic_params.begin(), m_intrinsic_params.end());
Eigen::Matrix<T, 3, 1> P(point[0], point[1], point[2]);
// project 3D object point to the image plane
Eigen::Matrix<T, 2, 1> predicted_p;
CameraT::spaceToPlane(intrinsic_params.data(), q, t, P, predicted_p);
Eigen::Matrix<T, 2, 1> err = predicted_p - m_observed_p.cast<T>();
Eigen::Matrix<T, 2, 1> err_weighted = m_sqrtPrecisionMat.cast<T>() * err;
residuals[0] = err_weighted(0);
residuals[1] = err_weighted(1);
return true;
}
// variables: 3D point
template <typename T>
bool operator()(const T* const point, T* residuals) const
{
T q_cam_odo[4] = {T(m_cam_odo_q.coeffs()(0)), T(m_cam_odo_q.coeffs()(1)), T(m_cam_odo_q.coeffs()(2)), T(m_cam_odo_q.coeffs()(3))};
T t_cam_odo[3] = {T(m_cam_odo_t(0)), T(m_cam_odo_t(1)), T(m_cam_odo_t(2))};
T p_odo[3] = {T(m_odo_pos(0)), T(m_odo_pos(1)), T(m_odo_pos(2))};
T att_odo[3] = {T(m_odo_att(0)), T(m_odo_att(1)), T(m_odo_att(2))};
T q[4], t[3];
worldToCameraTransform(q_cam_odo, t_cam_odo, p_odo, att_odo, q, t, m_optimize_cam_odo_z);
std::vector<T> intrinsic_params(m_intrinsic_params.begin(), m_intrinsic_params.end());
Eigen::Matrix<T, 3, 1> P(point[0], point[1], point[2]);
// project 3D object point to the image plane
Eigen::Matrix<T, 2, 1> predicted_p;
CameraT::spaceToPlane(intrinsic_params.data(), q, t, P, predicted_p);
residuals[0] = predicted_p(0) - T(m_observed_p(0));
residuals[1] = predicted_p(1) - T(m_observed_p(1));
return true;
}
private:
// camera intrinsics
std::vector<double> m_intrinsic_params;
// observed camera-odometry transform
Eigen::Quaterniond m_cam_odo_q;
Eigen::Vector3d m_cam_odo_t;
// observed odometry
Eigen::Vector3d m_odo_pos;
Eigen::Vector3d m_odo_att;
// observed 2D point
Eigen::Vector2d m_observed_p;
Eigen::Matrix2d m_sqrtPrecisionMat;
bool m_optimize_cam_odo_z;
};
// variables: camera intrinsics and camera extrinsics
template<class CameraT>
class StereoReprojectionError
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
StereoReprojectionError(const Eigen::Vector3d& observed_P,
const Eigen::Vector2d& observed_p_l,
const Eigen::Vector2d& observed_p_r)
: m_observed_P(observed_P)
, m_observed_p_l(observed_p_l)
, m_observed_p_r(observed_p_r)
{
}
template <typename T>
bool operator()(const T* const intrinsic_params_l,
const T* const intrinsic_params_r,
const T* const q_l,
const T* const t_l,
const T* const q_l_r,
const T* const t_l_r,
T* residuals) const
{
Eigen::Matrix<T, 3, 1> P;
P(0) = T(m_observed_P(0));
P(1) = T(m_observed_P(1));
P(2) = T(m_observed_P(2));
Eigen::Matrix<T, 2, 1> predicted_p_l;
CameraT::spaceToPlane(intrinsic_params_l, q_l, t_l, P, predicted_p_l);
Eigen::Quaternion<T> q_r = Eigen::Quaternion<T>(q_l_r) * Eigen::Quaternion<T>(q_l);
Eigen::Matrix<T, 3, 1> t_r;
t_r(0) = t_l[0];
t_r(1) = t_l[1];
t_r(2) = t_l[2];
t_r = Eigen::Quaternion<T>(q_l_r) * t_r;
t_r(0) += t_l_r[0];
t_r(1) += t_l_r[1];
t_r(2) += t_l_r[2];
Eigen::Matrix<T, 2, 1> predicted_p_r;
CameraT::spaceToPlane(intrinsic_params_r, q_r.coeffs().data(), t_r.data(), P, predicted_p_r);
residuals[0] = predicted_p_l(0) - T(m_observed_p_l(0));
residuals[1] = predicted_p_l(1) - T(m_observed_p_l(1));
residuals[2] = predicted_p_r(0) - T(m_observed_p_r(0));
residuals[3] = predicted_p_r(1) - T(m_observed_p_r(1));
return true;
}
private:
// observed 3D point
Eigen::Vector3d m_observed_P;
// observed 2D point
Eigen::Vector2d m_observed_p_l;
Eigen::Vector2d m_observed_p_r;
};
template <class CameraT>
class ComprehensionError {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
ComprehensionError(const Eigen::Vector3d& observed_P, const Eigen::Vector2d& observed_p)
: m_observed_P(observed_P),
m_observed_p(observed_p),
m_sqrtPrecisionMat(Eigen::Matrix2d::Identity()) {
}
template <typename T>
bool operator()(const T* const intrinsic_params, const T* const q, const T* const t,
T* residuals) const {
{
Eigen::Matrix<T, 2, 1> p = m_observed_p.cast<T>();
Eigen::Matrix<T, 3, 1> predicted_img_P;
CameraT::LiftToSphere(intrinsic_params, p, predicted_img_P);
Eigen::Matrix<T, 2, 1> predicted_p;
CameraT::SphereToPlane(intrinsic_params, predicted_img_P, predicted_p);
Eigen::Matrix<T, 2, 1> e = predicted_p - m_observed_p.cast<T>();
Eigen::Matrix<T, 2, 1> e_weighted = m_sqrtPrecisionMat.cast<T>() * e;
residuals[0] = e_weighted(0);
residuals[1] = e_weighted(1);
}
{
Eigen::Matrix<T, 3, 1> P = m_observed_P.cast<T>();
Eigen::Matrix<T, 2, 1> predicted_p;
CameraT::spaceToPlane(intrinsic_params, q, t, P, predicted_p);
Eigen::Matrix<T, 2, 1> e = predicted_p - m_observed_p.cast<T>();
Eigen::Matrix<T, 2, 1> e_weighted = m_sqrtPrecisionMat.cast<T>() * e;
residuals[2] = e_weighted(0);
residuals[3] = e_weighted(1);
}
return true;
}
// private:
// camera intrinsics
// std::vector<double> m_intrinsic_params;
// observed 3D point
Eigen::Vector3d m_observed_P;
// observed 2D point
Eigen::Vector2d m_observed_p;
// square root of precision matrix
Eigen::Matrix2d m_sqrtPrecisionMat;
};
boost::shared_ptr<CostFunctionFactory> CostFunctionFactory::m_instance;
CostFunctionFactory::CostFunctionFactory()
{
}
boost::shared_ptr<CostFunctionFactory>
CostFunctionFactory::instance(void)
{
if (m_instance.get() == 0)
{
m_instance.reset(new CostFunctionFactory);
}
return m_instance;
}
ceres::CostFunction*
CostFunctionFactory::generateCostFunction(const CameraConstPtr& camera,
const Eigen::Vector3d& observed_P,
const Eigen::Vector2d& observed_p,
int flags) const
{
ceres::CostFunction* costFunction = 0;
std::vector<double> intrinsic_params;
camera->writeParameters(intrinsic_params);
switch (flags)
{
case CAMERA_INTRINSICS | CAMERA_POSE:
switch (camera->modelType())
{
case Camera::KANNALA_BRANDT:
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError1<EquidistantCamera>, 2, 8, 4, 3>(
new ReprojectionError1<EquidistantCamera>(observed_P, observed_p));
break;
case Camera::PINHOLE:
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError1<PinholeCamera>, 2, 8, 4, 3>(
new ReprojectionError1<PinholeCamera>(observed_P, observed_p));
break;
case Camera::MEI:
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError1<CataCamera>, 2, 9, 4, 3>(
new ReprojectionError1<CataCamera>(observed_P, observed_p));
break;
case Camera::SCARAMUZZA:
costFunction =
new ceres::AutoDiffCostFunction<ComprehensionError<OCAMCamera>, 4, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3>(
new ComprehensionError<OCAMCamera>(observed_P, observed_p));
// new ceres::AutoDiffCostFunction<ReprojectionError1<OCAMCamera>, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3>(
// new ReprojectionError1<OCAMCamera>(observed_P, observed_p));
break;
}
break;
case CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_6D_POSE:
switch (camera->modelType())
{
case Camera::KANNALA_BRANDT:
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError1<EquidistantCamera>, 2, 4, 3, 3, 3>(
new ReprojectionError1<EquidistantCamera>(intrinsic_params, observed_P, observed_p));
break;
case Camera::PINHOLE:
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError1<PinholeCamera>, 2, 4, 3, 3, 3>(
new ReprojectionError1<PinholeCamera>(intrinsic_params, observed_P, observed_p));
break;
case Camera::MEI:
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError1<CataCamera>, 2, 4, 3, 3, 3>(
new ReprojectionError1<CataCamera>(intrinsic_params, observed_P, observed_p));
break;
case Camera::SCARAMUZZA:
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError1<OCAMCamera>, 2, 4, 3, 3, 3>(
new ReprojectionError1<OCAMCamera>(intrinsic_params, observed_P, observed_p));
break;
}
break;
}
return costFunction;
}
ceres::CostFunction*
CostFunctionFactory::generateCostFunction(const CameraConstPtr& camera,
const Eigen::Vector3d& observed_P,
const Eigen::Vector2d& observed_p,
const Eigen::Matrix2d& sqrtPrecisionMat,
int flags) const
{
ceres::CostFunction* costFunction = 0;
std::vector<double> intrinsic_params;
camera->writeParameters(intrinsic_params);
switch (flags)
{
case CAMERA_INTRINSICS | CAMERA_POSE:
switch (camera->modelType())
{
case Camera::KANNALA_BRANDT:
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError1<EquidistantCamera>, 2, 8, 4, 3>(
new ReprojectionError1<EquidistantCamera>(observed_P, observed_p, sqrtPrecisionMat));
break;
case Camera::PINHOLE:
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError1<PinholeCamera>, 2, 8, 4, 3>(
new ReprojectionError1<PinholeCamera>(observed_P, observed_p, sqrtPrecisionMat));
break;
case Camera::MEI:
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError1<CataCamera>, 2, 9, 4, 3>(
new ReprojectionError1<CataCamera>(observed_P, observed_p, sqrtPrecisionMat));
break;
case Camera::SCARAMUZZA:
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError1<OCAMCamera>, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3>(
new ReprojectionError1<OCAMCamera>(observed_P, observed_p, sqrtPrecisionMat));
break;
}
break;
}
return costFunction;
}
ceres::CostFunction*
CostFunctionFactory::generateCostFunction(const CameraConstPtr& camera,
const Eigen::Vector2d& observed_p,
int flags, bool optimize_cam_odo_z) const
{
ceres::CostFunction* costFunction = 0;
std::vector<double> intrinsic_params;
camera->writeParameters(intrinsic_params);
switch (flags)
{
case CAMERA_POSE | POINT_3D:
switch (camera->modelType())
{
case Camera::KANNALA_BRANDT:
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError2<EquidistantCamera>, 2, 4, 3, 3>(
new ReprojectionError2<EquidistantCamera>(intrinsic_params, observed_p));
break;
case Camera::PINHOLE:
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError2<PinholeCamera>, 2, 4, 3, 3>(
new ReprojectionError2<PinholeCamera>(intrinsic_params, observed_p));
break;
case Camera::MEI:
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError2<CataCamera>, 2, 4, 3, 3>(
new ReprojectionError2<CataCamera>(intrinsic_params, observed_p));
break;
case Camera::SCARAMUZZA:
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError2<OCAMCamera>, 2, 4, 3, 3>(
new ReprojectionError2<OCAMCamera>(intrinsic_params, observed_p));
break;
}
break;
case CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_3D_POSE | POINT_3D:
switch (camera->modelType())
{
case Camera::KANNALA_BRANDT:
if (optimize_cam_odo_z)
{
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError3<EquidistantCamera>, 2, 4, 3, 2, 1, 3>(
new ReprojectionError3<EquidistantCamera>(intrinsic_params, observed_p));
}
else
{
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError3<PinholeCamera>, 2, 4, 2, 2, 1, 3>(
new ReprojectionError3<PinholeCamera>(intrinsic_params, observed_p));
}
break;
case Camera::PINHOLE:
if (optimize_cam_odo_z)
{
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError3<PinholeCamera>, 2, 4, 3, 2, 1, 3>(
new ReprojectionError3<PinholeCamera>(intrinsic_params, observed_p));
}
else
{
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError3<PinholeCamera>, 2, 4, 2, 2, 1, 3>(
new ReprojectionError3<PinholeCamera>(intrinsic_params, observed_p));
}
break;
case Camera::MEI:
if (optimize_cam_odo_z)
{
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError3<CataCamera>, 2, 4, 3, 2, 1, 3>(
new ReprojectionError3<CataCamera>(intrinsic_params, observed_p));
}
else
{
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError3<CataCamera>, 2, 4, 2, 2, 1, 3>(
new ReprojectionError3<CataCamera>(intrinsic_params, observed_p));
}
break;
case Camera::SCARAMUZZA:
if (optimize_cam_odo_z)
{
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError3<OCAMCamera>, 2, 4, 3, 2, 1, 3>(
new ReprojectionError3<OCAMCamera>(intrinsic_params, observed_p));
}
else
{
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError3<OCAMCamera>, 2, 4, 2, 2, 1, 3>(
new ReprojectionError3<OCAMCamera>(intrinsic_params, observed_p));
}
break;
}
break;
case CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_6D_POSE | POINT_3D:
switch (camera->modelType())
{
case Camera::KANNALA_BRANDT:
if (optimize_cam_odo_z)
{
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError3<EquidistantCamera>, 2, 4, 3, 3, 3, 3>(
new ReprojectionError3<EquidistantCamera>(intrinsic_params, observed_p));
}
else
{
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError3<PinholeCamera>, 2, 4, 2, 3, 3, 3>(
new ReprojectionError3<PinholeCamera>(intrinsic_params, observed_p));
}
break;
case Camera::PINHOLE:
if (optimize_cam_odo_z)
{
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError3<PinholeCamera>, 2, 4, 3, 3, 3, 3>(
new ReprojectionError3<PinholeCamera>(intrinsic_params, observed_p));
}
else
{
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError3<PinholeCamera>, 2, 4, 2, 3, 3, 3>(
new ReprojectionError3<PinholeCamera>(intrinsic_params, observed_p));
}
break;
case Camera::MEI:
if (optimize_cam_odo_z)
{
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError3<CataCamera>, 2, 4, 3, 3, 3, 3>(
new ReprojectionError3<CataCamera>(intrinsic_params, observed_p));
}
else
{
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError3<CataCamera>, 2, 4, 2, 3, 3, 3>(
new ReprojectionError3<CataCamera>(intrinsic_params, observed_p));
}
break;
case Camera::SCARAMUZZA:
if (optimize_cam_odo_z)
{
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError3<OCAMCamera>, 2, 4, 3, 3, 3, 3>(
new ReprojectionError3<OCAMCamera>(intrinsic_params, observed_p));
}
else
{
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError3<OCAMCamera>, 2, 4, 2, 3, 3, 3>(
new ReprojectionError3<OCAMCamera>(intrinsic_params, observed_p));
}
break;
}
break;
case CAMERA_INTRINSICS | CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_3D_POSE | POINT_3D:
switch (camera->modelType())
{
case Camera::KANNALA_BRANDT:
if (optimize_cam_odo_z)
{
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError3<EquidistantCamera>, 2, 8, 4, 3, 2, 1, 3>(
new ReprojectionError3<EquidistantCamera>(observed_p));
}
else
{
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError3<PinholeCamera>, 2, 8, 4, 2, 2, 1, 3>(
new ReprojectionError3<PinholeCamera>(observed_p));
}
break;
case Camera::PINHOLE:
if (optimize_cam_odo_z)
{
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError3<PinholeCamera>, 2, 8, 4, 3, 2, 1, 3>(
new ReprojectionError3<PinholeCamera>(observed_p));
}
else
{
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError3<PinholeCamera>, 2, 8, 4, 2, 2, 1, 3>(
new ReprojectionError3<PinholeCamera>(observed_p));
}
break;
case Camera::MEI:
if (optimize_cam_odo_z)
{
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError3<CataCamera>, 2, 9, 4, 3, 2, 1, 3>(
new ReprojectionError3<CataCamera>(observed_p));
}
else
{
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError3<CataCamera>, 2, 9, 4, 2, 2, 1, 3>(
new ReprojectionError3<CataCamera>(observed_p));
}
break;
case Camera::SCARAMUZZA:
if (optimize_cam_odo_z)
{
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError3<OCAMCamera>, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3, 2, 1, 3>(
new ReprojectionError3<OCAMCamera>(observed_p));
}
else
{
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError3<OCAMCamera>, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 2, 2, 1, 3>(
new ReprojectionError3<OCAMCamera>(observed_p));
}
break;
}
break;
case CAMERA_INTRINSICS | CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_6D_POSE | POINT_3D:
switch (camera->modelType())
{
case Camera::KANNALA_BRANDT:
if (optimize_cam_odo_z)
{
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError3<EquidistantCamera>, 2, 8, 4, 3, 3, 3, 3>(
new ReprojectionError3<EquidistantCamera>(observed_p));
}
else
{
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError3<PinholeCamera>, 2, 8, 4, 2, 3, 3, 3>(
new ReprojectionError3<PinholeCamera>(observed_p));
}
break;
case Camera::PINHOLE:
if (optimize_cam_odo_z)
{
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError3<PinholeCamera>, 2, 8, 4, 3, 3, 3, 3>(
new ReprojectionError3<PinholeCamera>(observed_p));
}
else
{
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError3<PinholeCamera>, 2, 8, 4, 2, 3, 3, 3>(
new ReprojectionError3<PinholeCamera>(observed_p));
}
break;
case Camera::MEI:
if (optimize_cam_odo_z)
{
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError3<CataCamera>, 2, 9, 4, 3, 3, 3, 3>(
new ReprojectionError3<CataCamera>(observed_p));
}
else
{
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError3<CataCamera>, 2, 9, 4, 2, 3, 3, 3>(
new ReprojectionError3<CataCamera>(observed_p));
}
break;
case Camera::SCARAMUZZA:
if (optimize_cam_odo_z)
{
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError3<OCAMCamera>, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3, 3, 3, 3>(
new ReprojectionError3<OCAMCamera>(observed_p));
}
else
{
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError3<OCAMCamera>, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 2, 3, 3, 3>(
new ReprojectionError3<OCAMCamera>(observed_p));
}
break;
}
break;
}
return costFunction;
}
ceres::CostFunction*
CostFunctionFactory::generateCostFunction(const CameraConstPtr& camera,
const Eigen::Vector2d& observed_p,
const Eigen::Matrix2d& sqrtPrecisionMat,
int flags, bool optimize_cam_odo_z) const
{
ceres::CostFunction* costFunction = 0;
std::vector<double> intrinsic_params;
camera->writeParameters(intrinsic_params);
switch (flags)
{
case CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_6D_POSE | POINT_3D:
switch (camera->modelType())
{
case Camera::KANNALA_BRANDT:
if (optimize_cam_odo_z)
{
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError3<EquidistantCamera>, 2, 4, 3, 3, 3, 3>(
new ReprojectionError3<EquidistantCamera>(intrinsic_params, observed_p, sqrtPrecisionMat));
}
else
{
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError3<PinholeCamera>, 2, 4, 2, 3, 3, 3>(
new ReprojectionError3<PinholeCamera>(intrinsic_params, observed_p, sqrtPrecisionMat));
}
break;
case Camera::PINHOLE:
if (optimize_cam_odo_z)
{
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError3<PinholeCamera>, 2, 4, 3, 3, 3, 3>(
new ReprojectionError3<PinholeCamera>(intrinsic_params, observed_p, sqrtPrecisionMat));
}
else
{
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError3<PinholeCamera>, 2, 4, 2, 3, 3, 3>(
new ReprojectionError3<PinholeCamera>(intrinsic_params, observed_p, sqrtPrecisionMat));
}
break;
case Camera::MEI:
if (optimize_cam_odo_z)
{
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError3<CataCamera>, 2, 4, 3, 3, 3, 3>(
new ReprojectionError3<CataCamera>(intrinsic_params, observed_p, sqrtPrecisionMat));
}
else
{
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError3<CataCamera>, 2, 4, 2, 3, 3, 3>(
new ReprojectionError3<CataCamera>(intrinsic_params, observed_p, sqrtPrecisionMat));
}
break;
case Camera::SCARAMUZZA:
if (optimize_cam_odo_z)
{
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError3<OCAMCamera>, 2, 4, 3, 3, 3, 3>(
new ReprojectionError3<OCAMCamera>(intrinsic_params, observed_p, sqrtPrecisionMat));
}
else
{
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError3<OCAMCamera>, 2, 4, 2, 3, 3, 3>(
new ReprojectionError3<OCAMCamera>(intrinsic_params, observed_p, sqrtPrecisionMat));
}
break;
}
break;
case CAMERA_INTRINSICS | CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_6D_POSE | POINT_3D:
switch (camera->modelType())
{
case Camera::KANNALA_BRANDT:
if (optimize_cam_odo_z)
{
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError3<EquidistantCamera>, 2, 8, 4, 3, 3, 3, 3>(
new ReprojectionError3<EquidistantCamera>(observed_p, sqrtPrecisionMat));
}
else
{
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError3<PinholeCamera>, 2, 8, 4, 2, 3, 3, 3>(
new ReprojectionError3<PinholeCamera>(observed_p, sqrtPrecisionMat));
}
break;
case Camera::PINHOLE:
if (optimize_cam_odo_z)
{
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError3<PinholeCamera>, 2, 8, 4, 3, 3, 3, 3>(
new ReprojectionError3<PinholeCamera>(observed_p, sqrtPrecisionMat));
}
else
{
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError3<PinholeCamera>, 2, 8, 4, 2, 3, 3, 3>(
new ReprojectionError3<PinholeCamera>(observed_p, sqrtPrecisionMat));
}
break;
case Camera::MEI:
if (optimize_cam_odo_z)
{
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError3<CataCamera>, 2, 9, 4, 3, 3, 3, 3>(
new ReprojectionError3<CataCamera>(observed_p, sqrtPrecisionMat));
}
else
{
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError3<CataCamera>, 2, 9, 4, 2, 3, 3, 3>(
new ReprojectionError3<CataCamera>(observed_p, sqrtPrecisionMat));
}
break;
case Camera::SCARAMUZZA:
if (optimize_cam_odo_z)
{
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError3<OCAMCamera>, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3, 3, 3, 3>(
new ReprojectionError3<OCAMCamera>(observed_p, sqrtPrecisionMat));
}
else
{
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError3<OCAMCamera>, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 2, 3, 3, 3>(
new ReprojectionError3<OCAMCamera>(observed_p, sqrtPrecisionMat));
}
break;
}
break;
}
return costFunction;
}
ceres::CostFunction*
CostFunctionFactory::generateCostFunction(const CameraConstPtr& camera,
const Eigen::Vector3d& odo_pos,
const Eigen::Vector3d& odo_att,
const Eigen::Vector2d& observed_p,
int flags, bool optimize_cam_odo_z) const
{
ceres::CostFunction* costFunction = 0;
std::vector<double> intrinsic_params;
camera->writeParameters(intrinsic_params);
switch (flags)
{
case CAMERA_ODOMETRY_TRANSFORM | POINT_3D:
switch (camera->modelType())
{
case Camera::KANNALA_BRANDT:
if (optimize_cam_odo_z)
{
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError3<EquidistantCamera>, 2, 4, 3, 3>(
new ReprojectionError3<EquidistantCamera>(intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z));
}
else
{
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError3<EquidistantCamera>, 2, 4, 2, 3>(
new ReprojectionError3<EquidistantCamera>(intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z));
}
break;
case Camera::PINHOLE:
if (optimize_cam_odo_z)
{
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError3<PinholeCamera>, 2, 4, 3, 3>(
new ReprojectionError3<PinholeCamera>(intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z));
}
else
{
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError3<PinholeCamera>, 2, 4, 2, 3>(
new ReprojectionError3<PinholeCamera>(intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z));
}
break;
case Camera::MEI:
if (optimize_cam_odo_z)
{
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError3<CataCamera>, 2, 4, 3, 3>(
new ReprojectionError3<CataCamera>(intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z));
}
else
{
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError3<CataCamera>, 2, 4, 2, 3>(
new ReprojectionError3<CataCamera>(intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z));
}
break;
case Camera::SCARAMUZZA:
if (optimize_cam_odo_z)
{
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError3<OCAMCamera>, 2, 4, 3, 3>(
new ReprojectionError3<OCAMCamera>(intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z));
}
else
{
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError3<OCAMCamera>, 2, 4, 2, 3>(
new ReprojectionError3<OCAMCamera>(intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z));
}
break;
}
break;
}
return costFunction;
}
ceres::CostFunction*
CostFunctionFactory::generateCostFunction(const CameraConstPtr& camera,
const Eigen::Quaterniond& cam_odo_q,
const Eigen::Vector3d& cam_odo_t,
const Eigen::Vector3d& odo_pos,
const Eigen::Vector3d& odo_att,
const Eigen::Vector2d& observed_p,
int flags) const
{
ceres::CostFunction* costFunction = 0;
std::vector<double> intrinsic_params;
camera->writeParameters(intrinsic_params);
switch (flags)
{
case POINT_3D:
switch (camera->modelType())
{
case Camera::KANNALA_BRANDT:
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError3<EquidistantCamera>, 2, 3>(
new ReprojectionError3<EquidistantCamera>(intrinsic_params, cam_odo_q, cam_odo_t, odo_pos, odo_att, observed_p));
break;
case Camera::PINHOLE:
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError3<PinholeCamera>, 2, 3>(
new ReprojectionError3<PinholeCamera>(intrinsic_params, cam_odo_q, cam_odo_t, odo_pos, odo_att, observed_p));
break;
case Camera::MEI:
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError3<CataCamera>, 2, 3>(
new ReprojectionError3<CataCamera>(intrinsic_params, cam_odo_q, cam_odo_t, odo_pos, odo_att, observed_p));
break;
case Camera::SCARAMUZZA:
costFunction =
new ceres::AutoDiffCostFunction<ReprojectionError3<OCAMCamera>, 2, 3>(
new ReprojectionError3<OCAMCamera>(intrinsic_params, cam_odo_q, cam_odo_t, odo_pos, odo_att, observed_p));
break;
}
break;
}
return costFunction;
}
ceres::CostFunction*
CostFunctionFactory::generateCostFunction(const CameraConstPtr& cameraL,
const CameraConstPtr& cameraR,
const Eigen::Vector3d& observed_P,
const Eigen::Vector2d& observed_p_l,
const Eigen::Vector2d& observed_p_r) const
{
ceres::CostFunction* costFunction = 0;
if (cameraL->modelType() != cameraR->modelType())
{
return costFunction;
}
switch (cameraL->modelType())
{
case Camera::KANNALA_BRANDT:
costFunction =
new ceres::AutoDiffCostFunction<StereoReprojectionError<EquidistantCamera>, 4, 8, 8, 4, 3, 4, 3>(
new StereoReprojectionError<EquidistantCamera>(observed_P, observed_p_l, observed_p_r));
break;
case Camera::PINHOLE:
costFunction =
new ceres::AutoDiffCostFunction<StereoReprojectionError<PinholeCamera>, 4, 8, 8, 4, 3, 4, 3>(
new StereoReprojectionError<PinholeCamera>(observed_P, observed_p_l, observed_p_r));
break;
case Camera::MEI:
costFunction =
new ceres::AutoDiffCostFunction<StereoReprojectionError<CataCamera>, 4, 9, 9, 4, 3, 4, 3>(
new StereoReprojectionError<CataCamera>(observed_P, observed_p_l, observed_p_r));
break;
case Camera::SCARAMUZZA:
costFunction =
new ceres::AutoDiffCostFunction<StereoReprojectionError<OCAMCamera>, 4, SCARAMUZZA_CAMERA_NUM_PARAMS, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3, 4, 3>(
new StereoReprojectionError<OCAMCamera>(observed_P, observed_p_l, observed_p_r));
break;
}
return costFunction;
}
}
================================================
FILE: camera_model/src/camera_models/EquidistantCamera.cc
================================================
#include "camodocal/camera_models/EquidistantCamera.h"
#include <cmath>
#include <cstdio>
#include <eigen3/Eigen/Dense>
#include <iomanip>
#include <iostream>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include "camodocal/gpl/gpl.h"
namespace camodocal
{
EquidistantCamera::Parameters::Parameters()
: Camera::Parameters(KANNALA_BRANDT)
, m_k2(0.0)
, m_k3(0.0)
, m_k4(0.0)
, m_k5(0.0)
, m_mu(0.0)
, m_mv(0.0)
, m_u0(0.0)
, m_v0(0.0)
{
}
EquidistantCamera::Parameters::Parameters(const std::string& cameraName,
int w, int h,
double k2, double k3, double k4, double k5,
double mu, double mv,
double u0, double v0)
: Camera::Parameters(KANNALA_BRANDT, cameraName, w, h)
, m_k2(k2)
, m_k3(k3)
, m_k4(k4)
, m_k5(k5)
, m_mu(mu)
, m_mv(mv)
, m_u0(u0)
, m_v0(v0)
{
}
double&
EquidistantCamera::Parameters::k2(void)
{
return m_k2;
}
double&
EquidistantCamera::Parameters::k3(void)
{
return m_k3;
}
double&
EquidistantCamera::Parameters::k4(void)
{
return m_k4;
}
double&
EquidistantCamera::Parameters::k5(void)
{
return m_k5;
}
double&
EquidistantCamera::Parameters::mu(void)
{
return m_mu;
}
double&
EquidistantCamera::Parameters::mv(void)
{
return m_mv;
}
double&
EquidistantCamera::Parameters::u0(void)
{
return m_u0;
}
double&
EquidistantCamera::Parameters::v0(void)
{
return m_v0;
}
double
EquidistantCamera::Parameters::k2(void) const
{
return m_k2;
}
double
EquidistantCamera::Parameters::k3(void) const
{
return m_k3;
}
double
EquidistantCamera::Parameters::k4(void) const
{
return m_k4;
}
double
EquidistantCamera::Parameters::k5(void) const
{
return m_k5;
}
double
EquidistantCamera::Parameters::mu(void) const
{
return m_mu;
}
double
EquidistantCamera::Parameters::mv(void) const
{
return m_mv;
}
double
EquidistantCamera::Parameters::u0(void) const
{
return m_u0;
}
double
EquidistantCamera::Parameters::v0(void) const
{
return m_v0;
}
bool
EquidistantCamera::Parameters::readFromYamlFile(const std::string& filename)
{
cv::FileStorage fs(filename, cv::FileStorage::READ);
if (!fs.isOpened())
{
return false;
}
if (!fs["model_type"].isNone())
{
std::string sModelType;
fs["model_type"] >> sModelType;
if (sModelType.compare("KANNALA_BRANDT") != 0)
{
return false;
}
}
m_modelType = KANNALA_BRANDT;
fs["camera_name"] >> m_cameraName;
m_imageWidth = static_cast<int>(fs["image_width"]);
m_imageHeight = static_cast<int>(fs["image_height"]);
cv::FileNode n = fs["projection_parameters"];
m_k2 = static_cast<double>(n["k2"]);
m_k3 = static_cast<double>(n["k3"]);
m_k4 = static_cast<double>(n["k4"]);
m_k5 = static_cast<double>(n["k5"]);
m_mu = static_cast<double>(n["mu"]);
m_mv = static_cast<double>(n["mv"]);
m_u0 = static_cast<double>(n["u0"]);
m_v0 = static_cast<double>(n["v0"]);
return true;
}
void
EquidistantCamera::Parameters::writeToYamlFile(const std::string& filename) const
{
cv::FileStorage fs(filename, cv::FileStorage::WRITE);
fs << "model_type" << "KANNALA_BRANDT";
fs << "camera_name" << m_cameraName;
fs << "image_width" << m_imageWidth;
fs << "image_height" << m_imageHeight;
// projection: k2, k3, k4, k5, mu, mv, u0, v0
fs << "projection_parameters";
fs << "{" << "k2" << m_k2
<< "k3" << m_k3
<< "k4" << m_k4
<< "k5" << m_k5
<< "mu" << m_mu
<< "mv" << m_mv
<< "u0" << m_u0
<< "v0" << m_v0 << "}";
fs.release();
}
EquidistantCamera::Parameters&
EquidistantCamera::Parameters::operator=(const EquidistantCamera::Parameters& other)
{
if (this != &other)
{
m_modelType = other.m_modelType;
m_cameraName = other.m_cameraName;
m_imageWidth = other.m_imageWidth;
m_imageHeight = other.m_imageHeight;
m_k2 = other.m_k2;
m_k3 = other.m_k3;
m_k4 = other.m_k4;
m_k5 = other.m_k5;
m_mu = other.m_mu;
m_mv = other.m_mv;
m_u0 = other.m_u0;
m_v0 = other.m_v0;
}
return *this;
}
std::ostream&
operator<< (std::ostream& out, const EquidistantCamera::Parameters& params)
{
out << "Camera Parameters:" << std::endl;
out << " model_type " << "KANNALA_BRANDT" << std::endl;
out << " camera_name " << params.m_cameraName << std::endl;
out << " image_width " << params.m_imageWidth << std::endl;
out << " image_height " << params.m_imageHeight << std::endl;
// projection: k2, k3, k4, k5, mu, mv, u0, v0
out << "Projection Parameters" << std::endl;
out << " k2 " << params.m_k2 << std::endl
<< " k3 " << params.m_k3 << std::endl
<< " k4 " << params.m_k4 << std::endl
<< " k5 " << params.m_k5 << std::endl
<< " mu " << params.m_mu << std::endl
<< " mv " << params.m_mv << std::endl
<< " u0 " << params.m_u0 << std::endl
<< " v0 " << params.m_v0 << std::endl;
return out;
}
EquidistantCamera::EquidistantCamera()
: m_inv_K11(1.0)
, m_inv_K13(0.0)
, m_inv_K22(1.0)
, m_inv_K23(0.0)
{
}
EquidistantCamera::EquidistantCamera(const std::string& cameraName,
int imageWidth, int imageHeight,
double k2, double k3, double k4, double k5,
double mu, double mv,
double u0, double v0)
: mParameters(cameraName, imageWidth, imageHeight,
k2, k3, k4, k5, mu, mv, u0, v0)
{
// Inverse camera projection matrix parameters
m_inv_K11 = 1.0 / mParameters.mu();
m_inv_K13 = -mParameters.u0() / mParameters.mu();
m_inv_K22 = 1.0 / mParameters.mv();
m_inv_K23 = -mParameters.v0() / mParameters.mv();
}
EquidistantCamera::EquidistantCamera(const EquidistantCamera::Parameters& params)
: mParameters(params)
{
// Inverse camera projection matrix parameters
m_inv_K11 = 1.0 / mParameters.mu();
m_inv_K13 = -mParameters.u0() / mParameters.mu();
m_inv_K22 = 1.0 / mParameters.mv();
m_inv_K23 = -mParameters.v0() / mParameters.mv();
}
Camera::ModelType
EquidistantCamera::modelType(void) const
{
return mParameters.modelType();
}
const std::string&
EquidistantCamera::cameraName(void) const
{
return mParameters.cameraName();
}
int
EquidistantCamera::imageWidth(void) const
{
return mParameters.imageWidth();
}
int
EquidistantCamera::imageHeight(void) const
{
return mParameters.imageHeight();
}
void
EquidistantCamera::estimateIntrinsics(const cv::Size& boardSize,
const std::vector< std::vector<cv::Point3f> >& objectPoints,
const std::vector< std::vector<cv::Point2f> >& imagePoints)
{
Parameters params = getParameters();
double u0 = params.imageWidth() / 2.0;
double v0 = params.imageHeight() / 2.0;
double minReprojErr = std::numeric_limits<double>::max();
std::vector<cv::Mat> rvecs, tvecs;
rvecs.assign(objectPoints.size(), cv::Mat());
tvecs.assign(objectPoints.size(), cv::Mat());
params.k2() = 0.0;
params.k3() = 0.0;
params.k4() = 0.0;
params.k5() = 0.0;
params.u0() = u0;
params.v0() = v0;
// Initialize focal length
// C. Hughes, P. Denny, M. Glavin, and E. Jones,
// Equidistant Fish-Eye Calibration and Rectification by Vanishing Point
// Extraction, PAMI 2010
// Find circles from rows of chessboard corners, and for each pair
// of circles, find vanishing points: v1 and v2.
// f = ||v1 - v2|| / PI;
double f0 = 0.0;
for (size_t i = 0; i < imagePoints.size(); ++i)
{
std::vector<Eigen::Vector2d> center(boardSize.height);
double radius[boardSize.height];
for (int r = 0; r < boardSize.height; ++r)
{
std::vector<cv::Point2d> circle;
for (int c = 0; c < boardSize.width; ++c)
{
circle.push_back(imagePoints.at(i).at(r * boardSize.width + c));
}
fitCircle(circle, center[r](0), center[r](1), radius[r]);
}
for (int j = 0; j < boardSize.height; ++j)
{
for (int k = j + 1; k < boardSize.height; ++k)
{
// find distance between pair of vanishing points which
// correspond to intersection points of 2 circles
std::vector<cv::Point2d> ipts;
ipts = intersectCircles(center[j](0), center[j](1), radius[j],
center[k](0), center[k](1), radius[k]);
if (ipts.size() < 2)
{
continue;
}
gitextract_ongpbb8j/
├── .gitignore
├── README.md
├── camera_model/
│ ├── CMakeLists.txt
│ ├── cmake/
│ │ └── FindEigen.cmake
│ ├── include/
│ │ └── camodocal/
│ │ ├── calib/
│ │ │ └── CameraCalibration.h
│ │ ├── camera_models/
│ │ │ ├── Camera.h
│ │ │ ├── CameraFactory.h
│ │ │ ├── CataCamera.h
│ │ │ ├── CostFunctionFactory.h
│ │ │ ├── EquidistantCamera.h
│ │ │ ├── PinholeCamera.h
│ │ │ └── ScaramuzzaCamera.h
│ │ ├── chessboard/
│ │ │ ├── Chessboard.h
│ │ │ ├── ChessboardCorner.h
│ │ │ ├── ChessboardQuad.h
│ │ │ └── Spline.h
│ │ ├── gpl/
│ │ │ ├── EigenQuaternionParameterization.h
│ │ │ ├── EigenUtils.h
│ │ │ └── gpl.h
│ │ └── sparse_graph/
│ │ └── Transform.h
│ ├── instruction
│ ├── package.xml
│ ├── readme.md
│ └── src/
│ ├── calib/
│ │ └── CameraCalibration.cc
│ ├── camera_models/
│ │ ├── Camera.cc
│ │ ├── CameraFactory.cc
│ │ ├── CataCamera.cc
│ │ ├── CostFunctionFactory.cc
│ │ ├── EquidistantCamera.cc
│ │ ├── PinholeCamera.cc
│ │ └── ScaramuzzaCamera.cc
│ ├── chessboard/
│ │ └── Chessboard.cc
│ ├── gpl/
│ │ ├── EigenQuaternionParameterization.cc
│ │ └── gpl.cc
│ ├── intrinsic_calib.cc
│ └── sparse_graph/
│ └── Transform.cc
├── config/
│ ├── campus.rviz
│ ├── indoor.rviz
│ ├── openloris/
│ │ ├── openloris_vio.yaml
│ │ └── openloris_vio_atlas.yaml
│ ├── realsense/
│ │ ├── vio d455.yaml
│ │ ├── vio.yaml
│ │ ├── vio_atlas copy.yaml
│ │ ├── vio_atlas.yaml
│ │ ├── vio_campus.yaml
│ │ └── vio_indoor.yaml
│ ├── tagaided.rviz
│ ├── tsinghua.rviz
│ ├── tum_rgbd/
│ │ ├── tum_fr3.yaml
│ │ └── tum_fr3_atlas.yaml
│ └── video.rviz
├── doc/
│ ├── INSTALL.md
│ └── RUNNING_PROCEDURE.md
├── output/
│ └── .gitignore
├── pose_graph/
│ ├── CMakeLists.txt
│ ├── cmake/
│ │ └── FindEigen.cmake
│ ├── nodelet_plugin.xml
│ ├── package.xml
│ └── src/
│ ├── ThirdParty/
│ │ ├── DBoW/
│ │ │ ├── BowVector.cpp
│ │ │ ├── BowVector.h
│ │ │ ├── DBoW2.h
│ │ │ ├── FBrief.cpp
│ │ │ ├── FBrief.h
│ │ │ ├── FClass.h
│ │ │ ├── FeatureVector.cpp
│ │ │ ├── FeatureVector.h
│ │ │ ├── QueryResults.cpp
│ │ │ ├── QueryResults.h
│ │ │ ├── ScoringObject.cpp
│ │ │ ├── ScoringObject.h
│ │ │ ├── TemplatedDatabase.h
│ │ │ └── TemplatedVocabulary.h
│ │ ├── DUtils/
│ │ │ ├── DException.h
│ │ │ ├── DUtils.h
│ │ │ ├── Random.cpp
│ │ │ ├── Random.h
│ │ │ ├── Timestamp.cpp
│ │ │ └── Timestamp.h
│ │ ├── DVision/
│ │ │ ├── BRIEF.cpp
│ │ │ ├── BRIEF.h
│ │ │ └── DVision.h
│ │ ├── VocabularyBinary.cpp
│ │ └── VocabularyBinary.hpp
│ ├── keyframe/
│ │ ├── keyframe.cpp
│ │ └── keyframe.h
│ ├── pose_graph/
│ │ ├── pose_graph.cpp
│ │ └── pose_graph.h
│ ├── pose_graph_nodelet.cpp
│ └── utility/
│ ├── CameraPoseVisualization.cpp
│ ├── CameraPoseVisualization.h
│ ├── parameters.h
│ ├── tic_toc.h
│ ├── utility.cpp
│ └── utility.h
├── support_files/
│ └── brief_pattern.yml
└── vins_estimator/
├── CMakeLists.txt
├── cmake/
│ └── FindEigen.cmake
├── launch/
│ ├── atlas200/
│ │ ├── compressed2img.launch
│ │ └── img2compressed.launch
│ ├── openloris/
│ │ ├── openloris_vio_atlas.launch
│ │ ├── openloris_vio_pytorch.launch
│ │ ├── openloris_vio_tensorrt.launch
│ │ └── openloris_vo.launch
│ ├── realsense/
│ │ ├── l515.launch
│ │ ├── realsense_vio.launch
│ │ ├── realsense_vio_atlas.launch
│ │ └── rs_camera.launch
│ ├── tum_rgbd/
│ │ ├── tum_rgbd_atlas.launch
│ │ ├── tum_rgbd_pytorch.launch
│ │ └── tum_rgbd_tensorrt.launch
│ ├── vins_rviz.launch
│ └── yolo/
│ ├── atlas.launch
│ ├── pytorch.launch
│ └── tensorrt.launch
├── nodelet_description.xml
├── package.xml
└── src/
├── estimator/
│ ├── estimator.cpp
│ └── estimator.h
├── estimator_nodelet.cpp
├── factor/
│ ├── imu_factor.h
│ ├── imu_factor_modified.h
│ ├── integration_base.h
│ ├── marginalization_factor.cpp
│ ├── marginalization_factor.h
│ ├── pose_local_parameterization.cpp
│ ├── pose_local_parameterization.h
│ ├── projection_factor.cpp
│ ├── projection_factor.h
│ ├── projection_td_factor.cpp
│ └── projection_td_factor.h
├── feature_manager/
│ ├── feature_manager.cpp
│ └── feature_manager.h
├── feature_tracker/
│ ├── ThreadPool.h
│ ├── feature_tracker.cpp
│ └── feature_tracker.h
├── initial/
│ ├── initial_aligment.cpp
│ ├── initial_alignment.h
│ ├── initial_ex_rotation.cpp
│ ├── initial_ex_rotation.h
│ ├── initial_sfm.cpp
│ ├── initial_sfm.h
│ ├── solve_5pts.cpp
│ └── solve_5pts.h
└── utility/
├── CameraPoseVisualization.cpp
├── CameraPoseVisualization.h
├── parameters.cpp
├── parameters.h
├── tic_toc.h
├── utility.cpp
├── utility.h
├── visualization.cpp
└── visualization.h
SYMBOL INDEX (253 symbols across 86 files)
FILE: camera_model/include/camodocal/calib/CameraCalibration.h
function namespace (line 8) | namespace camodocal
FILE: camera_model/include/camodocal/camera_models/Camera.h
function namespace (line 9) | namespace camodocal
FILE: camera_model/include/camodocal/camera_models/CameraFactory.h
function namespace (line 9) | namespace camodocal
FILE: camera_model/include/camodocal/camera_models/CataCamera.h
function namespace (line 10) | namespace camodocal
FILE: camera_model/include/camodocal/camera_models/CostFunctionFactory.h
function namespace (line 9) | namespace ceres
function namespace (line 14) | namespace camodocal
FILE: camera_model/include/camodocal/camera_models/EquidistantCamera.h
function namespace (line 10) | namespace camodocal
FILE: camera_model/include/camodocal/camera_models/PinholeCamera.h
function namespace (line 10) | namespace camodocal
FILE: camera_model/include/camodocal/camera_models/ScaramuzzaCamera.h
function namespace (line 10) | namespace camodocal
FILE: camera_model/include/camodocal/chessboard/Chessboard.h
function namespace (line 7) | namespace camodocal
FILE: camera_model/include/camodocal/chessboard/ChessboardCorner.h
function namespace (line 7) | namespace camodocal
FILE: camera_model/include/camodocal/chessboard/ChessboardQuad.h
function namespace (line 8) | namespace camodocal
FILE: camera_model/include/camodocal/chessboard/Spline.h
type BC_type (line 33) | enum BC_type {
type Spline_type (line 39) | enum Spline_type {
type std (line 54) | typedef std::vector<std::pair<double, double> > base;
type base (line 55) | typedef base::const_iterator const_iterator;
function const_iterator (line 58) | const_iterator begin() const { return base::begin(); }
function clear (line 60) | void clear() { _valid = false; base::clear(); _data.clear(); }
function size (line 61) | size_t size() const { return base::size(); }
function capacity (line 63) | size_t capacity() const { return base::capacity(); }
function addPoint (line 68) | inline void addPoint(double x, double y)
function setType (line 81) | void setType(Spline_type type) { _type = type; _valid = false; }
type SplineData (line 105) | struct SplineData { double x,a,b,c,d; }
function splineCalc (line 122) | inline double splineCalc(std::vector<SplineData>::const_iterator i, doub...
function lowCalc (line 128) | inline double lowCalc(double xval)
function highCalc (line 149) | inline double highCalc(double xval)
function x (line 171) | inline double x(size_t i) const { return operator[](i).first; }
function y (line 172) | inline double y(size_t i) const { return operator[](i).second; }
function h (line 173) | inline double h(size_t i) const { return x(i+1) - x(i); }
function generate (line 200) | void generate()
FILE: camera_model/include/camodocal/gpl/EigenQuaternionParameterization.h
function namespace (line 6) | namespace camodocal
FILE: camera_model/include/camodocal/gpl/EigenUtils.h
function namespace (line 9) | namespace camodocal
FILE: camera_model/include/camodocal/gpl/gpl.h
function namespace (line 8) | namespace camodocal
FILE: camera_model/include/camodocal/sparse_graph/Transform.h
function namespace (line 8) | namespace camodocal
FILE: camera_model/src/calib/CameraCalibration.cc
type camodocal (line 21) | namespace camodocal
function CameraPtr (line 167) | CameraPtr&
function CameraConstPtr (line 173) | const CameraConstPtr
FILE: camera_model/src/camera_models/Camera.cc
type camodocal (line 6) | namespace camodocal
FILE: camera_model/src/camera_models/CameraFactory.cc
type camodocal (line 13) | namespace camodocal
function CameraPtr (line 34) | CameraPtr
function CameraPtr (line 89) | CameraPtr
FILE: camera_model/src/camera_models/CataCamera.cc
type camodocal (line 14) | namespace camodocal
FILE: camera_model/src/camera_models/CostFunctionFactory.cc
type camodocal (line 9) | namespace camodocal
function worldToCameraTransform (line 13) | void
class ReprojectionError1 (line 58) | class ReprojectionError1
method ReprojectionError1 (line 68) | ReprojectionError1(const Eigen::Vector3d& observed_P,
method ReprojectionError1 (line 74) | ReprojectionError1(const std::vector<double>& intrinsic_params,
class ReprojectionError2 (line 141) | class ReprojectionError2
class ReprojectionError3 (line 180) | class ReprojectionError3
method ReprojectionError3 (line 190) | ReprojectionError3(const Eigen::Vector2d& observed_p,
method ReprojectionError3 (line 196) | ReprojectionError3(const std::vector<double>& intrinsic_params,
method ReprojectionError3 (line 203) | ReprojectionError3(const std::vector<double>& intrinsic_params,
method ReprojectionError3 (line 212) | ReprojectionError3(const std::vector<double>& intrinsic_params,
method ReprojectionError3 (line 222) | ReprojectionError3(const std::vector<double>& intrinsic_params,
class StereoReprojectionError (line 356) | class StereoReprojectionError
class ComprehensionError (line 421) | class ComprehensionError {
FILE: camera_model/src/camera_models/EquidistantCamera.cc
type camodocal (line 14) | namespace camodocal
FILE: camera_model/src/camera_models/PinholeCamera.cc
type camodocal (line 13) | namespace camodocal
FILE: camera_model/src/camera_models/ScaramuzzaCamera.cc
function polyfit (line 18) | Eigen::VectorXd polyfit(Eigen::VectorXd& xVec, Eigen::VectorXd& yVec, in...
type camodocal (line 46) | namespace camodocal
FILE: camera_model/src/chessboard/Chessboard.cc
type camodocal (line 11) | namespace camodocal
function less_pred (line 1552) | bool less_pred(const std::pair<float, int>& p1, const std::pair<float,...
function countClasses (line 1557) | void countClasses(const std::vector<std::pair<float, int> >& pairs, si...
FILE: camera_model/src/gpl/EigenQuaternionParameterization.cc
type camodocal (line 5) | namespace camodocal
FILE: camera_model/src/gpl/gpl.cc
function orwl_gettime (line 20) | struct timespec orwl_gettime(void) {
type camodocal (line 46) | namespace camodocal
function hypot3 (line 49) | double hypot3(double x, double y, double z)
function hypot3f (line 54) | float hypot3f(float x, float y, float z)
function d2r (line 59) | double d2r(double deg)
function d2r (line 64) | float d2r(float deg)
function r2d (line 69) | double r2d(double rad)
function r2d (line 74) | float r2d(float rad)
function sinc (line 79) | double sinc(double theta)
function LARGE_INTEGER (line 88) | LARGE_INTEGER
function clock_gettime (line 109) | int
function timeInMicroseconds (line 149) | unsigned long long timeInMicroseconds(void)
function timeInSeconds (line 161) | double timeInSeconds(void)
function colorDepthImage (line 439) | void colorDepthImage(cv::Mat& imgDepth, cv::Mat& imgColoredDepth,
function colormap (line 465) | bool colormap(const std::string& name, unsigned char idx,
function bresLine (line 492) | std::vector<cv::Point2i> bresLine(int x0, int y0, int x1, int y1)
function bresCircle (line 532) | std::vector<cv::Point2i> bresCircle(int x0, int y0, int r)
function fitCircle (line 621) | void
function intersectCircles (line 676) | std::vector<cv::Point2d>
function UTMLetterDesignator (line 714) | char
function LLtoUTM (line 747) | void
function UTMtoLL (line 827) | void
function timestampDiff (line 897) | long int
FILE: camera_model/src/intrinsic_calib.cc
function main (line 15) | int main(int argc, char** argv)
FILE: camera_model/src/sparse_graph/Transform.cc
type camodocal (line 3) | namespace camodocal
FILE: pose_graph/src/ThirdParty/DBoW/BowVector.cpp
type DBoW2 (line 18) | namespace DBoW2 {
FILE: pose_graph/src/ThirdParty/DBoW/BowVector.h
function namespace (line 17) | namespace DBoW2 {
FILE: pose_graph/src/ThirdParty/DBoW/DBoW2.h
function namespace (line 59) | namespace DBoW2
type DBoW2 (line 71) | typedef DBoW2::TemplatedVocabulary<DBoW2::FBrief::TDescriptor, DBoW2::FB...
type DBoW2 (line 75) | typedef DBoW2::TemplatedDatabase<DBoW2::FBrief::TDescriptor, DBoW2::FBrief>
FILE: pose_graph/src/ThirdParty/DBoW/FBrief.cpp
type DBoW2 (line 18) | namespace DBoW2 {
FILE: pose_graph/src/ThirdParty/DBoW/FBrief.h
function namespace (line 20) | namespace DBoW2 {
FILE: pose_graph/src/ThirdParty/DBoW/FClass.h
function namespace (line 17) | namespace DBoW2 {
FILE: pose_graph/src/ThirdParty/DBoW/FeatureVector.cpp
type DBoW2 (line 15) | namespace DBoW2 {
FILE: pose_graph/src/ThirdParty/DBoW/FeatureVector.h
function namespace (line 18) | namespace DBoW2 {
FILE: pose_graph/src/ThirdParty/DBoW/QueryResults.cpp
type DBoW2 (line 16) | namespace DBoW2
function ostream (line 21) | ostream & operator<<(ostream& os, const Result& ret )
function ostream (line 29) | ostream & operator<<(ostream& os, const QueryResults& ret )
FILE: pose_graph/src/ThirdParty/DBoW/QueryResults.h
function namespace (line 15) | namespace DBoW2 {
FILE: pose_graph/src/ThirdParty/DBoW/ScoringObject.h
function namespace (line 15) | namespace DBoW2 {
FILE: pose_graph/src/ThirdParty/DBoW/TemplatedDatabase.h
function namespace (line 28) | namespace DBoW2 {
FILE: pose_graph/src/ThirdParty/DBoW/TemplatedVocabulary.h
function namespace (line 33) | namespace DBoW2 {
function m_scoring_object (line 515) | m_scoring_object(NULL)
FILE: pose_graph/src/ThirdParty/DUtils/DException.h
function namespace (line 20) | namespace DUtils {
FILE: pose_graph/src/ThirdParty/DUtils/DUtils.h
function namespace (line 36) | namespace DUtils
FILE: pose_graph/src/ThirdParty/DUtils/Random.h
function namespace (line 18) | namespace DUtils {
FILE: pose_graph/src/ThirdParty/DUtils/Timestamp.cpp
type __timeb32 (line 59) | struct __timeb32
type timeval (line 65) | struct timeval
function string (line 97) | string Timestamp::getStringTime() const {
function Timestamp (line 107) | Timestamp& Timestamp::operator+= (double s)
function Timestamp (line 113) | Timestamp& Timestamp::operator-= (double s)
function Timestamp (line 119) | Timestamp Timestamp::operator+ (double s) const
function Timestamp (line 127) | Timestamp Timestamp::plus(unsigned long secs, unsigned long usecs) const
function Timestamp (line 141) | Timestamp Timestamp::operator- (double s) const
function Timestamp (line 149) | Timestamp Timestamp::minus(unsigned long secs, unsigned long usecs) const
function string (line 197) | string Timestamp::Format(bool machine_friendly) const
function string (line 223) | string Timestamp::Format(double s) {
FILE: pose_graph/src/ThirdParty/DUtils/Timestamp.h
function namespace (line 16) | namespace DUtils {
FILE: pose_graph/src/ThirdParty/DVision/BRIEF.h
function namespace (line 36) | namespace DVision {
FILE: pose_graph/src/ThirdParty/DVision/DVision.h
function namespace (line 37) | namespace DVision
FILE: pose_graph/src/ThirdParty/VocabularyBinary.hpp
type VINSLoop (line 8) | namespace VINSLoop {
type Node (line 10) | struct Node {
type Word (line 17) | struct Word {
type Vocabulary (line 22) | struct Vocabulary {
method staticDataSize (line 40) | inline static size_t staticDataSize() {
FILE: pose_graph/src/keyframe/keyframe.cpp
function reduceVector (line 5) | static void reduceVector(vector<Derived> &v, vector<uchar> status) {
FILE: pose_graph/src/keyframe/keyframe.h
function class (line 22) | class BriefExtractor {
function class (line 31) | class KeyFrame {
FILE: pose_graph/src/pose_graph/pose_graph.cpp
function KeyFrame (line 295) | KeyFrame *PoseGraph::getKeyFrame(int index) {
FILE: pose_graph/src/pose_graph/pose_graph.h
function class (line 35) | class PoseGraph {
function class (line 111) | class AngleLocalParameterization {
function ceres (line 122) | static ceres::LocalParameterization *Create() {
type FourDOFError (line 166) | struct FourDOFError {
function const (line 209) | struct FourDOFWeightError {
function ceres (line 244) | static ceres::CostFunction *
type RelativeRTError (line 256) | struct RelativeRTError {
FILE: pose_graph/src/pose_graph_nodelet.cpp
type pose_graph_nodelet_ns (line 46) | namespace pose_graph_nodelet_ns {
class PoseGraphNodelet (line 47) | class PoseGraphNodelet
method PoseGraphNodelet (line 51) | PoseGraphNodelet() {
method onInit (line 68) | void onInit() override {
method new_sequence (line 231) | void new_sequence() {
method image_callback (line 254) | void image_callback(const sensor_msgs::ImageConstPtr &image_msg) {
method point_callback (line 274) | void point_callback(const sensor_msgs::PointCloudConstPtr &point_msg) {
method pose_callback (line 292) | void pose_callback(const nav_msgs::Odometry::ConstPtr &pose_msg) {
method imu_forward_callback (line 311) | void imu_forward_callback(const nav_msgs::Odometry::ConstPtr &forwar...
method relo_relative_pose_callback (line 339) | void
method vio_callback (line 358) | void vio_callback(const nav_msgs::Odometry::ConstPtr &pose_msg) {
method extrinsic_callback (line 436) | void extrinsic_callback(const nav_msgs::Odometry::ConstPtr &pose_msg) {
method process (line 449) | void process() {
method command (line 568) | void command() {
FILE: pose_graph/src/utility/CameraPoseVisualization.cpp
function Eigen2Point (line 12) | void Eigen2Point(const Eigen::Vector3d& v, geometry_msgs::Point& p) {
FILE: pose_graph/src/utility/CameraPoseVisualization.h
function class (line 12) | class CameraPoseVisualization {
FILE: pose_graph/src/utility/tic_toc.h
function class (line 7) | class TicToc
FILE: pose_graph/src/utility/utility.h
function class (line 8) | class Utility
FILE: vins_estimator/src/estimator/estimator.cpp
function Matrix3d (line 1790) | Matrix3d Estimator::predictMotion(double t0, double t1)
FILE: vins_estimator/src/estimator/estimator.h
function class (line 35) | class Estimator
function first_imu (line 152) | bool first_imu{}
function is_valid (line 153) | bool is_valid{}
function failure_occur (line 154) | bool failure_occur{}
function initial_timestamp (line 157) | double initial_timestamp{}
function MarginalizationInfo (line 167) | MarginalizationInfo *last_marginalization_info{}
function IntegrationBase (line 171) | IntegrationBase *tmp_pre_integration{}
function relocalization_info (line 174) | bool relocalization_info{}
function relo_frame_stamp (line 175) | double relo_frame_stamp{}
function relo_frame_index (line 176) | double relo_frame_index{}
function relo_frame_local_index (line 177) | int relo_frame_local_index{}
function relo_relative_yaw (line 186) | double relo_relative_yaw{}
function init_imu (line 190) | bool init_imu{}
function latest_time (line 195) | double latest_time{}
function initFirstPoseFlag (line 201) | bool initFirstPoseFlag{}
FILE: vins_estimator/src/estimator_nodelet.cpp
type estimator_nodelet_ns (line 24) | namespace estimator_nodelet_ns
class EstimatorNodelet (line 26) | class EstimatorNodelet : public nodelet::Nodelet //任何nodelet plugin都要...
method EstimatorNodelet (line 29) | EstimatorNodelet() = default;
method onInit (line 32) | void onInit() override
method imu_callback (line 99) | void imu_callback(const sensor_msgs::ImuConstPtr &imu_msg)
method image_callback (line 125) | void image_callback(const sensor_msgs::ImageConstPtr &color_msg)
method depth_callback (line 133) | void depth_callback(const sensor_msgs::ImageConstPtr &depth_msg)
method relocalization_callback (line 141) | void relocalization_callback(const sensor_msgs::PointCloudConstPtr &...
method visualizeFeatureFilter (line 148) | void visualizeFeatureFilter(const map<int, Eigen::Matrix<double, 7, ...
method process_tracker (line 192) | [[noreturn]] void process_tracker()
method process (line 462) | [[noreturn]] void process()
FILE: vins_estimator/src/factor/imu_factor.h
function pre_integration (line 18) | pre_integration(_pre_integration) {}
function virtual (line 20) | virtual bool Evaluate(double const *const *parameters, double *residuals,
FILE: vins_estimator/src/factor/imu_factor_modified.h
function pre_integration (line 19) | pre_integration(_pre_integration) {}
function virtual (line 21) | virtual bool Evaluate(double const *const *parameters, double *residuals,
FILE: vins_estimator/src/factor/integration_base.h
function class (line 9) | class IntegrationBase
function push_back (line 32) | void push_back(double dt, const Eigen::Vector3d &acc, const Eigen::Vecto...
function repropagate (line 40) | void repropagate(const Eigen::Vector3d &_linearized_ba, const Eigen::Vec...
function midPointIntegration (line 56) | void midPointIntegration(double _dt, const Eigen::Vector3d &_acc_0,
function propagate (line 136) | void propagate(double _dt, const Eigen::Vector3d &_acc_1, const Eigen::V...
FILE: vins_estimator/src/factor/marginalization_factor.h
type ResidualBlockInfo (line 16) | struct ResidualBlockInfo
type ThreadsStruct (line 42) | struct ThreadsStruct
function class (line 51) | class MarginalizationInfo
function class (line 78) | class MarginalizationFactor : public ceres::CostFunction
FILE: vins_estimator/src/factor/pose_local_parameterization.h
function class (line 7) | class PoseLocalParameterization : public ceres::LocalParameterization
FILE: vins_estimator/src/feature_manager/feature_manager.cpp
function VectorXd (line 302) | VectorXd FeatureManager::getDepthVector()
FILE: vins_estimator/src/feature_manager/feature_manager.h
type DynamicObject (line 24) | struct DynamicObject
function class (line 40) | class FeaturePerFrame
function class (line 67) | class FeaturePerId
function class (line 89) | class FeatureManager
FILE: vins_estimator/src/feature_tracker/ThreadPool.h
function class (line 14) | class ThreadPool
function ThreadPool (line 37) | inline ThreadPool::ThreadPool(size_t threads) : stop(false)
function task (line 68) | auto task = std::make_shared<std::packaged_task<return_type()>>(
function ThreadPool (line 86) | inline ThreadPool::~ThreadPool()
FILE: vins_estimator/src/feature_tracker/feature_tracker.cpp
function reduceVector (line 9) | void reduceVector(vector<cv::Point2f> &v, vector<uchar> status)
function reduceVector (line 18) | void reduceVector(vector<int> &v, vector<uchar> status)
FILE: vins_estimator/src/feature_tracker/feature_tracker.h
function class (line 31) | class FeatureTracker
FILE: vins_estimator/src/initial/initial_aligment.cpp
function solveGyroscopeBias (line 3) | void solveGyroscopeBias(map<double, ImageFrame> &all_image_frame, Vector...
function MatrixXd (line 79) | MatrixXd TangentBasis(Vector3d &g0)
function RefineGravity (line 94) | void RefineGravity(map<double, ImageFrame> &all_image_frame, Vector3d &g...
function RefineGravityWithDepth (line 170) | void RefineGravityWithDepth(map<double, ImageFrame> &all_image_frame, Ve...
function LinearAlignment (line 246) | bool LinearAlignment(map<double, ImageFrame> &all_image_frame, Vector3d ...
function LinearAlignmentWithDepth (line 337) | bool LinearAlignmentWithDepth(map<double, ImageFrame> &all_image_frame, ...
function LinearAlignmentWithDepthGravity (line 407) | bool LinearAlignmentWithDepthGravity(map<double, ImageFrame> &all_image_...
function VisualIMUAlignment (line 483) | bool VisualIMUAlignment(map<double, ImageFrame> &all_image_frame, Vector...
FILE: vins_estimator/src/initial/initial_alignment.h
function class (line 13) | class ImageFrame
FILE: vins_estimator/src/initial/initial_ex_rotation.cpp
function Matrix3d (line 70) | Matrix3d InitialEXRotation::solveRelativeR(const vector<pair<Vector3d, V...
FILE: vins_estimator/src/initial/initial_ex_rotation.h
function class (line 15) | class InitialEXRotation
FILE: vins_estimator/src/initial/initial_sfm.h
type SFMFeature (line 14) | struct SFMFeature
type ReprojectionError3D (line 24) | struct ReprojectionError3D
function class (line 57) | class GlobalSFM
FILE: vins_estimator/src/initial/solve_5pts.cpp
type cv (line 7) | namespace cv
function decomposeEssentialMat (line 9) | void decomposeEssentialMat(InputArray _E, OutputArray _R1, OutputArray...
function recoverPose (line 35) | int recoverPose(InputArray E, InputArray _points1, InputArray _points2...
function recoverPose (line 196) | int recoverPose(InputArray E, InputArray _points1, InputArray _points2...
FILE: vins_estimator/src/initial/solve_5pts.h
function class (line 13) | class MotionEstimator
FILE: vins_estimator/src/utility/CameraPoseVisualization.cpp
function Eigen2Point (line 12) | void Eigen2Point(const Eigen::Vector3d &v, geometry_msgs::Point &p)
FILE: vins_estimator/src/utility/CameraPoseVisualization.h
function class (line 10) | class CameraPoseVisualization
FILE: vins_estimator/src/utility/parameters.cpp
function T (line 65) | T readParam(ros::NodeHandle &n, std::string name)
function readParameters (line 81) | void readParameters(ros::NodeHandle &n)
FILE: vins_estimator/src/utility/parameters.h
type SIZE_PARAMETERIZATION (line 81) | enum SIZE_PARAMETERIZATION
type StateOrder (line 90) | enum StateOrder
type NoiseOrder (line 99) | enum NoiseOrder
FILE: vins_estimator/src/utility/tic_toc.h
function class (line 7) | class TicToc
FILE: vins_estimator/src/utility/utility.h
function class (line 8) | class Utility
FILE: vins_estimator/src/utility/visualization.cpp
function registerPub (line 34) | void registerPub(ros::NodeHandle &n)
function pubLatestOdometry (line 66) | void pubLatestOdometry(const Eigen::Vector3d &P, const Eigen::Quaternion...
function printStatistics (line 97) | void printStatistics(const Estimator &estimator, double t)
function pubOdometry (line 138) | void pubOdometry(const Estimator &estimator, const std_msgs::Header &hea...
function pubKeyPoses (line 229) | void pubKeyPoses(const Estimator &estimator, const std_msgs::Header &hea...
function pubCameraPose (line 264) | void pubCameraPose(const Estimator &estimator, const std_msgs::Header &h...
function pubIMUPose (line 306) | void pubIMUPose(const Estimator &estimator, const std_msgs::Header &header)
function pubPointCloud (line 328) | void pubPointCloud(const Estimator &estimator, const std_msgs::Header &h...
function pubTF (line 409) | void pubTF(const Estimator &estimator, const std_msgs::Header &header)
function pubKeyframe (line 454) | void pubKeyframe(const Estimator &estimator)
function pubRelocalization (line 522) | void pubRelocalization(const Estimator &estimator)
function pubTrackImg (line 540) | void pubTrackImg(const cv_bridge::CvImageConstPtr &img_ptr)
function pubTrackImg (line 552) | void pubTrackImg(const sensor_msgs::ImagePtr &img_msg)
function pubTrackImg (line 564) | void pubTrackImg(const cv::Mat &img)
Condensed preview — 152 files, each showing path, character count, and a content snippet. Download the .json file or copy for the full structured content (1,234K chars).
[
{
"path": ".gitignore",
"chars": 82,
"preview": "**/build\n**/devel\n**/.idea\n**/.vscode\n**/cmake-build-debug\n**/.cache\n\n!.gitignore\n"
},
{
"path": "README.md",
"chars": 4823,
"preview": "# VINS-RGBD-FAST\n\n**VINS-RGBD-FAST** is a SLAM system based on VINS-RGBD. We do some refinements to accelerate the syste"
},
{
"path": "camera_model/CMakeLists.txt",
"chars": 1984,
"preview": "cmake_minimum_required(VERSION 2.8.3)\nproject(camera_model)\n\nset(CMAKE_BUILD_TYPE \"Release\")\n# https://blog.csdn.net/qq_"
},
{
"path": "camera_model/cmake/FindEigen.cmake",
"chars": 7887,
"preview": "# Ceres Solver - A fast non-linear least squares minimizer\n# Copyright 2015 Google Inc. All rights reserved.\n# http://ce"
},
{
"path": "camera_model/include/camodocal/calib/CameraCalibration.h",
"chars": 2195,
"preview": "#ifndef CAMERACALIBRATION_H\n#define CAMERACALIBRATION_H\n\n#include <opencv2/core/core.hpp>\n\n#include \"camodocal/camera_mo"
},
{
"path": "camera_model/include/camodocal/camera_models/Camera.h",
"chars": 5063,
"preview": "#ifndef CAMERA_H\n#define CAMERA_H\n\n#include <boost/shared_ptr.hpp>\n#include <eigen3/Eigen/Dense>\n#include <opencv2/core/"
},
{
"path": "camera_model/include/camodocal/camera_models/CameraFactory.h",
"chars": 658,
"preview": "#ifndef CAMERAFACTORY_H\n#define CAMERAFACTORY_H\n\n#include <boost/shared_ptr.hpp>\n#include <opencv2/core/core.hpp>\n\n#incl"
},
{
"path": "camera_model/include/camodocal/camera_models/CataCamera.h",
"chars": 6561,
"preview": "#ifndef CATACAMERA_H\r\n#define CATACAMERA_H\r\n\r\n#include <opencv2/core/core.hpp>\r\n#include <string>\r\n\r\n#include \"ceres/rot"
},
{
"path": "camera_model/include/camodocal/camera_models/CostFunctionFactory.h",
"chars": 3444,
"preview": "#ifndef COSTFUNCTIONFACTORY_H\n#define COSTFUNCTIONFACTORY_H\n\n#include <boost/shared_ptr.hpp>\n#include <opencv2/core/core"
},
{
"path": "camera_model/include/camodocal/camera_models/EquidistantCamera.h",
"chars": 6787,
"preview": "#ifndef EQUIDISTANTCAMERA_H\r\n#define EQUIDISTANTCAMERA_H\r\n\r\n#include <opencv2/core/core.hpp>\r\n#include <string>\r\n\r\n#incl"
},
{
"path": "camera_model/include/camodocal/camera_models/PinholeCamera.h",
"chars": 5982,
"preview": "#ifndef PINHOLECAMERA_H\n#define PINHOLECAMERA_H\n\n#include <opencv2/core/core.hpp>\n#include <string>\n\n#include \"ceres/rot"
},
{
"path": "camera_model/include/camodocal/camera_models/ScaramuzzaCamera.h",
"chars": 10407,
"preview": "#ifndef SCARAMUZZACAMERA_H\r\n#define SCARAMUZZACAMERA_H\r\n\r\n#include <opencv2/core/core.hpp>\r\n#include <string>\r\n\r\n#includ"
},
{
"path": "camera_model/include/camodocal/chessboard/Chessboard.h",
"chars": 3058,
"preview": "#ifndef CHESSBOARD_H\n#define CHESSBOARD_H\n\n#include <boost/shared_ptr.hpp>\n#include <opencv2/core/core.hpp>\n\nnamespace c"
},
{
"path": "camera_model/include/camodocal/chessboard/ChessboardCorner.h",
"chars": 1196,
"preview": "#ifndef CHESSBOARDCORNER_H\n#define CHESSBOARDCORNER_H\n\n#include <boost/shared_ptr.hpp>\n#include <opencv2/core/core.hpp>\n"
},
{
"path": "camera_model/include/camodocal/chessboard/ChessboardQuad.h",
"chars": 773,
"preview": "#ifndef CHESSBOARDQUAD_H\n#define CHESSBOARDQUAD_H\n\n#include <boost/shared_ptr.hpp>\n\n#include \"camodocal/chessboard/Chess"
},
{
"path": "camera_model/include/camodocal/chessboard/Spline.h",
"chars": 8626,
"preview": "/* dynamo:- Event driven molecular dynamics simulator\n http://www.marcusbannerman.co.uk/dynamo\n Copyright (C) 201"
},
{
"path": "camera_model/include/camodocal/gpl/EigenQuaternionParameterization.h",
"chars": 1150,
"preview": "#ifndef EIGENQUATERNIONPARAMETERIZATION_H\n#define EIGENQUATERNIONPARAMETERIZATION_H\n\n#include \"ceres/local_parameterizat"
},
{
"path": "camera_model/include/camodocal/gpl/EigenUtils.h",
"chars": 11621,
"preview": "#ifndef EIGENUTILS_H\n#define EIGENUTILS_H\n\n#include <eigen3/Eigen/Dense>\n\n#include \"ceres/rotation.h\"\n#include \"camodoca"
},
{
"path": "camera_model/include/camodocal/gpl/gpl.h",
"chars": 2414,
"preview": "#ifndef GPL_H\r\n#define GPL_H\r\n\r\n#include <algorithm>\r\n#include <cmath>\r\n#include <opencv2/core/core.hpp>\r\n\r\nnamespace ca"
},
{
"path": "camera_model/include/camodocal/sparse_graph/Transform.h",
"chars": 744,
"preview": "#ifndef TRANSFORM_H\n#define TRANSFORM_H\n\n#include <boost/shared_ptr.hpp>\n#include <eigen3/Eigen/Dense>\n#include <stdint."
},
{
"path": "camera_model/instruction",
"chars": 68,
"preview": "rosrun camera_model Calibration -w 8 -h 11 -s 70 -i ~/bag/PX/calib/\n"
},
{
"path": "camera_model/package.xml",
"chars": 2048,
"preview": "<?xml version=\"1.0\"?>\n<package>\n <name>camera_model</name>\n <version>0.0.0</version>\n <description>The camera_model p"
},
{
"path": "camera_model/readme.md",
"chars": 552,
"preview": "part of [camodocal](https://github.com/hengli/camodocal)\n\n[Google Ceres](http://ceres-solver.org) is needed.\n\n# Calibrat"
},
{
"path": "camera_model/src/calib/CameraCalibration.cc",
"chars": 16568,
"preview": "#include \"camodocal/calib/CameraCalibration.h\"\n\n#include <cstdio>\n#include <eigen3/Eigen/Dense>\n#include <iomanip>\n#incl"
},
{
"path": "camera_model/src/camera_models/Camera.cc",
"chars": 5759,
"preview": "#include \"camodocal/camera_models/Camera.h\"\n#include \"camodocal/camera_models/ScaramuzzaCamera.h\"\n\n#include <opencv2/cal"
},
{
"path": "camera_model/src/camera_models/CameraFactory.cc",
"chars": 4440,
"preview": "#include \"camodocal/camera_models/CameraFactory.h\"\n\n#include <boost/algorithm/string.hpp>\n\n\n#include \"camodocal/camera_m"
},
{
"path": "camera_model/src/camera_models/CataCamera.cc",
"chars": 26012,
"preview": "#include \"camodocal/camera_models/CataCamera.h\"\n\n#include <cmath>\n#include <cstdio>\n#include <eigen3/Eigen/Dense>\n#inclu"
},
{
"path": "camera_model/src/camera_models/CostFunctionFactory.cc",
"chars": 45514,
"preview": "#include \"camodocal/camera_models/CostFunctionFactory.h\"\n\n#include \"ceres/ceres.h\"\n#include \"camodocal/camera_models/Cat"
},
{
"path": "camera_model/src/camera_models/EquidistantCamera.cc",
"chars": 20449,
"preview": "#include \"camodocal/camera_models/EquidistantCamera.h\"\n\n#include <cmath>\n#include <cstdio>\n#include <eigen3/Eigen/Dense>"
},
{
"path": "camera_model/src/camera_models/PinholeCamera.cc",
"chars": 22110,
"preview": "#include \"camodocal/camera_models/PinholeCamera.h\"\n\n#include <cmath>\n#include <cstdio>\n#include <eigen3/Eigen/Dense>\n#in"
},
{
"path": "camera_model/src/camera_models/ScaramuzzaCamera.cc",
"chars": 25873,
"preview": "#include \"camodocal/camera_models/ScaramuzzaCamera.h\"\n\n#include <cmath>\n#include <cstdio>\n#include <eigen3/Eigen/Dense>\n"
},
{
"path": "camera_model/src/chessboard/Chessboard.cc",
"chars": 70200,
"preview": "#include \"camodocal/chessboard/Chessboard.h\"\n\n#include <opencv2/calib3d/calib3d.hpp>\n#include <opencv2/imgproc/imgproc.h"
},
{
"path": "camera_model/src/gpl/EigenQuaternionParameterization.cc",
"chars": 1409,
"preview": "#include \"camodocal/gpl/EigenQuaternionParameterization.h\"\n\n#include <cmath>\n\nnamespace camodocal\n{\n\nbool\nEigenQuaternio"
},
{
"path": "camera_model/src/gpl/gpl.cc",
"chars": 25657,
"preview": "#include \"camodocal/gpl/gpl.h\"\r\n\r\n#include <set>\r\n#ifdef _WIN32\r\n#include <winsock.h>\r\n#else\r\n#include <time.h>\r\n#endif\r"
},
{
"path": "camera_model/src/intrinsic_calib.cc",
"chars": 8450,
"preview": "#include <boost/algorithm/string.hpp>\n#include <boost/filesystem.hpp>\n#include <boost/program_options.hpp>\n#include <iom"
},
{
"path": "camera_model/src/sparse_graph/Transform.cc",
"chars": 1075,
"preview": "#include <camodocal/sparse_graph/Transform.h>\n\nnamespace camodocal\n{\n\nTransform::Transform()\n{\n m_q.setIdentity();\n "
},
{
"path": "config/campus.rviz",
"chars": 19748,
"preview": "Panels:\n - Class: rviz/Displays\n Help Height: 0\n Name: Displays\n Property Tree Widget:\n Expanded:\n "
},
{
"path": "config/indoor.rviz",
"chars": 18296,
"preview": "Panels:\n - Class: rviz/Displays\n Help Height: 0\n Name: Displays\n Property Tree Widget:\n Expanded:\n "
},
{
"path": "config/openloris/openloris_vio.yaml",
"chars": 4939,
"preview": "%YAML:1.0\n\nnum_threads: 0 # 0 Use the max number of threads of your device.\n # For some devices, lik"
},
{
"path": "config/openloris/openloris_vio_atlas.yaml",
"chars": 5067,
"preview": "%YAML:1.0\n\nnum_threads: 6 # 0 Use the max number of threads of your device.\n # For some devices, lik"
},
{
"path": "config/realsense/vio d455.yaml",
"chars": 5068,
"preview": "%YAML:1.0\n\nnum_threads: 0 # 0 Use the max number of threads of your device.\n # For some devices, lik"
},
{
"path": "config/realsense/vio.yaml",
"chars": 4926,
"preview": "%YAML:1.0\n\nnum_threads: 0 # 0 Use the max number of threads of your device.\n # For some devices, lik"
},
{
"path": "config/realsense/vio_atlas copy.yaml",
"chars": 4853,
"preview": "%YAML:1.0\n\nnum_threads: 6 # 0 Use the max number of threads of your device.\n # For some devices, lik"
},
{
"path": "config/realsense/vio_atlas.yaml",
"chars": 4904,
"preview": "%YAML:1.0\n\nnum_threads: 6 # 0 Use the max number of threads of your device.\n # For some devices, lik"
},
{
"path": "config/realsense/vio_campus.yaml",
"chars": 4967,
"preview": "%YAML:1.0\n\nnum_threads: 0 # 0 Use the max number of threads of your device.\n # For some devices, lik"
},
{
"path": "config/realsense/vio_indoor.yaml",
"chars": 4956,
"preview": "%YAML:1.0\n\nnum_threads: 0 # 0 Use the max number of threads of your device.\n # For some devices, lik"
},
{
"path": "config/tagaided.rviz",
"chars": 25000,
"preview": "Panels:\n - Class: rviz/Displays\n Help Height: 0\n Name: Displays\n Property Tree Widget:\n Expanded:\n "
},
{
"path": "config/tsinghua.rviz",
"chars": 20563,
"preview": "Panels:\n - Class: rviz/Displays\n Help Height: 0\n Name: Displays\n Property Tree Widget:\n Expanded:\n "
},
{
"path": "config/tum_rgbd/tum_fr3.yaml",
"chars": 4187,
"preview": "%YAML:1.0\n\nnum_threads: 0 # 0 Use the max number of threads of your device.\n # For some devices, lik"
},
{
"path": "config/tum_rgbd/tum_fr3_atlas.yaml",
"chars": 4386,
"preview": "%YAML:1.0\n\nnum_threads: 6 # 0 Use the max number of threads of your device.\n # For some devices, lik"
},
{
"path": "config/video.rviz",
"chars": 21284,
"preview": "Panels:\n - Class: rviz/Displays\n Help Height: 0\n Name: Displays\n Property Tree Widget:\n Expanded:\n "
},
{
"path": "doc/INSTALL.md",
"chars": 1940,
"preview": "# Dynamic-VINS\n\n## 1. Prerequisites\n\n**ROS**\n```\nsudo apt-get install ros-melodic-cv-bridge ros-melodic-tf ros-melodic-m"
},
{
"path": "doc/RUNNING_PROCEDURE.md",
"chars": 1387,
"preview": "# Dynamic-VINS testing procedure\n\n1. ```\n export ROS_HOSTNAME=192.168.2.223 #从机IP \n export ROS_MASTER_URI=http:/"
},
{
"path": "output/.gitignore",
"chars": 14,
"preview": "*\n\n!.gitignore"
},
{
"path": "pose_graph/CMakeLists.txt",
"chars": 1599,
"preview": "cmake_minimum_required(VERSION 2.8.3)\nproject(pose_graph)\n\nset(CMAKE_BUILD_TYPE \"Release\")\n# https://blog.csdn.net/qq_24"
},
{
"path": "pose_graph/cmake/FindEigen.cmake",
"chars": 7887,
"preview": "# Ceres Solver - A fast non-linear least squares minimizer\n# Copyright 2015 Google Inc. All rights reserved.\n# http://ce"
},
{
"path": "pose_graph/nodelet_plugin.xml",
"chars": 280,
"preview": "<library path=\"lib/libpose_graph_nodelet\">\n <class name=\"pose_graph/PoseGraphNodelet\" type=\"pose_graph_nodelet_ns::Po"
},
{
"path": "pose_graph/package.xml",
"chars": 2048,
"preview": "<?xml version=\"1.0\"?>\n<package>\n <name>pose_graph</name>\n <version>0.0.0</version>\n <description>pose_graph package</"
},
{
"path": "pose_graph/src/ThirdParty/DBoW/BowVector.cpp",
"chars": 2752,
"preview": "/**\n * File: BowVector.cpp\n * Date: March 2011\n * Author: Dorian Galvez-Lopez\n * Description: bag of words vector\n * Lic"
},
{
"path": "pose_graph/src/ThirdParty/DBoW/BowVector.h",
"chars": 1885,
"preview": "/**\n * File: BowVector.h\n * Date: March 2011\n * Author: Dorian Galvez-Lopez\n * Description: bag of words vector\n * Licen"
},
{
"path": "pose_graph/src/ThirdParty/DBoW/DBoW2.h",
"chars": 2230,
"preview": "/*\n * File: DBoW2.h\n * Date: November 2011\n * Author: Dorian Galvez-Lopez\n * Description: Generic include file for the D"
},
{
"path": "pose_graph/src/ThirdParty/DBoW/FBrief.cpp",
"chars": 2382,
"preview": "/**\r\n * File: FBrief.cpp\r\n * Date: November 2011\r\n * Author: Dorian Galvez-Lopez\r\n * Description: functions for BRIEF de"
},
{
"path": "pose_graph/src/ThirdParty/DBoW/FBrief.h",
"chars": 1610,
"preview": "/**\r\n * File: FBrief.h\r\n * Date: November 2011\r\n * Author: Dorian Galvez-Lopez\r\n * Description: functions for BRIEF desc"
},
{
"path": "pose_graph/src/ThirdParty/DBoW/FClass.h",
"chars": 1644,
"preview": "/**\n * File: FClass.h\n * Date: November 2011\n * Author: Dorian Galvez-Lopez\n * Description: generic FClass to instantiat"
},
{
"path": "pose_graph/src/ThirdParty/DBoW/FeatureVector.cpp",
"chars": 1831,
"preview": "/**\n * File: FeatureVector.cpp\n * Date: November 2011\n * Author: Dorian Galvez-Lopez\n * Description: feature vector\n * L"
},
{
"path": "pose_graph/src/ThirdParty/DBoW/FeatureVector.h",
"chars": 1080,
"preview": "/**\n * File: FeatureVector.h\n * Date: November 2011\n * Author: Dorian Galvez-Lopez\n * Description: feature vector\n * Lic"
},
{
"path": "pose_graph/src/ThirdParty/DBoW/QueryResults.cpp",
"chars": 1384,
"preview": "/**\n * File: QueryResults.cpp\n * Date: March, November 2011\n * Author: Dorian Galvez-Lopez\n * Description: structure to "
},
{
"path": "pose_graph/src/ThirdParty/DBoW/QueryResults.h",
"chars": 4142,
"preview": "/**\n * File: QueryResults.h\n * Date: March, November 2011\n * Author: Dorian Galvez-Lopez\n * Description: structure to st"
},
{
"path": "pose_graph/src/ThirdParty/DBoW/ScoringObject.cpp",
"chars": 7890,
"preview": "/**\n * File: ScoringObject.cpp\n * Date: November 2011\n * Author: Dorian Galvez-Lopez\n * Description: functions to comput"
},
{
"path": "pose_graph/src/ThirdParty/DBoW/ScoringObject.h",
"chars": 2449,
"preview": "/**\n * File: ScoringObject.h\n * Date: November 2011\n * Author: Dorian Galvez-Lopez\n * Description: functions to compute "
},
{
"path": "pose_graph/src/ThirdParty/DBoW/TemplatedDatabase.h",
"chars": 37615,
"preview": "/**\n * File: TemplatedDatabase.h\n * Date: March 2011\n * Author: Dorian Galvez-Lopez\n * Description: templated database o"
},
{
"path": "pose_graph/src/ThirdParty/DBoW/TemplatedVocabulary.h",
"chars": 41589,
"preview": "/**\n * File: TemplatedVocabulary.h\n * Date: February 2011\n * Author: Dorian Galvez-Lopez\n * Description: templated vocab"
},
{
"path": "pose_graph/src/ThirdParty/DUtils/DException.h",
"chars": 1079,
"preview": "/*\t\n * File: DException.h\n * Project: DUtils library\n * Author: Dorian Galvez-Lopez\n * Date: October 6, 2009\n * Descript"
},
{
"path": "pose_graph/src/ThirdParty/DUtils/DUtils.h",
"chars": 1056,
"preview": "/*\r\n * File: DUtils.h\r\n * Project: DUtils library\r\n * Author: Dorian Galvez-Lopez\r\n * Date: October 6, 2009\r\n * Descript"
},
{
"path": "pose_graph/src/ThirdParty/DUtils/Random.cpp",
"chars": 2886,
"preview": "/*\t\r\n * File: Random.cpp\r\n * Project: DUtils library\r\n * Author: Dorian Galvez-Lopez\r\n * Date: April 2010\r\n * Descriptio"
},
{
"path": "pose_graph/src/ThirdParty/DUtils/Random.h",
"chars": 3774,
"preview": "/*\t\n * File: Random.h\n * Project: DUtils library\n * Author: Dorian Galvez-Lopez\n * Date: April 2010, November 2011\n * De"
},
{
"path": "pose_graph/src/ThirdParty/DUtils/Timestamp.cpp",
"chars": 5117,
"preview": "/*\r\n * File: Timestamp.cpp\r\n * Author: Dorian Galvez-Lopez\r\n * Date: March 2009\r\n * Description: timestamping functions\r"
},
{
"path": "pose_graph/src/ThirdParty/DUtils/Timestamp.h",
"chars": 4567,
"preview": "/*\n * File: Timestamp.h\n * Author: Dorian Galvez-Lopez\n * Date: March 2009\n * Description: timestamping functions\n * Lic"
},
{
"path": "pose_graph/src/ThirdParty/DVision/BRIEF.cpp",
"chars": 3866,
"preview": "/**\n * File: BRIEF.cpp\n * Author: Dorian Galvez\n * Date: September 2010\n * Description: implementation of BRIEF (Binary "
},
{
"path": "pose_graph/src/ThirdParty/DVision/BRIEF.h",
"chars": 4706,
"preview": "/**\n * File: BRIEF.h\n * Author: Dorian Galvez-Lopez\n * Date: March 2011\n * Description: implementation of BRIEF (Binary "
},
{
"path": "pose_graph/src/ThirdParty/DVision/DVision.h",
"chars": 1015,
"preview": "/*\n * File: DVision.h\n * Project: DVision library\n * Author: Dorian Galvez-Lopez\n * Date: October 4, 2010\n * Description"
},
{
"path": "pose_graph/src/ThirdParty/VocabularyBinary.cpp",
"chars": 934,
"preview": "#include \"VocabularyBinary.hpp\"\n#include <opencv2/core/core.hpp>\nusing namespace std;\n\nVINSLoop::Vocabulary::Vocabulary("
},
{
"path": "pose_graph/src/ThirdParty/VocabularyBinary.hpp",
"chars": 749,
"preview": "#ifndef VocabularyBinary_hpp\n#define VocabularyBinary_hpp\n\n#include <cstdint>\n#include <fstream>\n#include <string>\n\nname"
},
{
"path": "pose_graph/src/keyframe/keyframe.cpp",
"chars": 21279,
"preview": "#include \"keyframe.h\"\n#include <opencv2/highgui.hpp>\n\ntemplate <typename Derived>\nstatic void reduceVector(vector<Derive"
},
{
"path": "pose_graph/src/keyframe/keyframe.h",
"chars": 4170,
"preview": "#pragma once\n\n#include \"../ThirdParty/DBoW/DBoW2.h\"\n#include \"../ThirdParty/DVision/DVision.h\"\n#include \"../utility/para"
},
{
"path": "pose_graph/src/pose_graph/pose_graph.cpp",
"chars": 39048,
"preview": "#include \"pose_graph.h\"\n#include <opencv2/core.hpp>\nPoseGraph::PoseGraph() {\n posegraph_visualization = new CameraPoseV"
},
{
"path": "pose_graph/src/pose_graph/pose_graph.h",
"chars": 9421,
"preview": "#pragma once\n\n#include \"../ThirdParty/DBoW/DBoW2.h\"\n#include \"../ThirdParty/DBoW/TemplatedDatabase.h\"\n#include \"../Third"
},
{
"path": "pose_graph/src/pose_graph_nodelet.cpp",
"chars": 20806,
"preview": "#include <nav_msgs/Odometry.h>\n#include <nav_msgs/Path.h>\n#include <ros/ros.h>\n#include <sensor_msgs/Image.h>\n#include <"
},
{
"path": "pose_graph/src/utility/CameraPoseVisualization.cpp",
"chars": 7028,
"preview": "#include \"CameraPoseVisualization.h\"\n\nconst Eigen::Vector3d CameraPoseVisualization::imlt = Eigen::Vector3d(-1.0, -0.5, "
},
{
"path": "pose_graph/src/utility/CameraPoseVisualization.h",
"chars": 1556,
"preview": "#pragma once\n\n#include <ros/ros.h>\n#include <std_msgs/ColorRGBA.h>\n#include <visualization_msgs/Marker.h>\n#include <visu"
},
{
"path": "pose_graph/src/utility/parameters.h",
"chars": 885,
"preview": "#pragma once\n\n#include \"camodocal/camera_models/CameraFactory.h\"\n#include \"camodocal/camera_models/CataCamera.h\"\n#includ"
},
{
"path": "pose_graph/src/utility/tic_toc.h",
"chars": 488,
"preview": "#pragma once\n\n#include <ctime>\n#include <cstdlib>\n#include <chrono>\n\nclass TicToc\n{\n public:\n TicToc()\n {\n "
},
{
"path": "pose_graph/src/utility/utility.cpp",
"chars": 433,
"preview": "#include \"utility.h\"\n\nEigen::Matrix3d Utility::g2R(const Eigen::Vector3d &g)\n{\n Eigen::Matrix3d R0;\n Eigen::Vector"
},
{
"path": "pose_graph/src/utility/utility.h",
"chars": 4759,
"preview": "#pragma once\n\n#include <cmath>\n#include <cassert>\n#include <cstring>\n#include <eigen3/Eigen/Dense>\n\nclass Utility\n{\n pu"
},
{
"path": "support_files/brief_pattern.yml",
"chars": 6977,
"preview": "%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 - -"
},
{
"path": "vins_estimator/CMakeLists.txt",
"chars": 1682,
"preview": "cmake_minimum_required(VERSION 2.8.3)\nproject(vins_estimator)\n\nset(CMAKE_BUILD_TYPE \"Release\")\n# set(CMAKE_BUILD_TYPE \"R"
},
{
"path": "vins_estimator/cmake/FindEigen.cmake",
"chars": 7887,
"preview": "# Ceres Solver - A fast non-linear least squares minimizer\n# Copyright 2015 Google Inc. All rights reserved.\n# http://ce"
},
{
"path": "vins_estimator/launch/atlas200/compressed2img.launch",
"chars": 822,
"preview": "<launch>\n <arg name=\"manager_name\" default=\"nodelet_manager\" />\n <node pkg=\"nodelet\" type=\"nodelet\" name=\"$(arg ma"
},
{
"path": "vins_estimator/launch/atlas200/img2compressed.launch",
"chars": 763,
"preview": "<launch>\n <arg name=\"color_topic\" default=\"/d400/color/image_raw\" />\n <!-- <arg name=\"depth_topic\" default=\"/d400/"
},
{
"path": "vins_estimator/launch/openloris/openloris_vio_atlas.launch",
"chars": 1680,
"preview": "<launch>\n <arg name=\"config_path\" default=\"$(find vins_estimator)/../config/openloris/openloris_vio_atlas.yaml\" />\n "
},
{
"path": "vins_estimator/launch/openloris/openloris_vio_pytorch.launch",
"chars": 1308,
"preview": "<launch>\n <arg name=\"config_path\" default=\"$(find vins_estimator)/../config/openloris/openloris_vio.yaml\" />\n <arg"
},
{
"path": "vins_estimator/launch/openloris/openloris_vio_tensorrt.launch",
"chars": 1428,
"preview": "<launch>\n <arg name=\"config_path\" default=\"$(find vins_estimator)/../config/openloris/openloris_vio.yaml\" />\n <arg"
},
{
"path": "vins_estimator/launch/openloris/openloris_vo.launch",
"chars": 1307,
"preview": "<launch>\n <arg name=\"config_path\" default=\"$(find vins_estimator)/../config/openloris/openloris_vo.yaml\" />\n <arg "
},
{
"path": "vins_estimator/launch/realsense/l515.launch",
"chars": 7917,
"preview": "<launch>\n <rosparam>\n /camera/l500_depth_sensor/visual_preset: 5\n /camera/l500_depth_sensor/laser_power"
},
{
"path": "vins_estimator/launch/realsense/realsense_vio.launch",
"chars": 1234,
"preview": "<launch>\n <arg name=\"config_path\" default=\"$(find vins_estimator)/../config/realsense/vio.yaml\" />\n <arg name=\"vin"
},
{
"path": "vins_estimator/launch/realsense/realsense_vio_atlas.launch",
"chars": 1601,
"preview": "<launch>\n <arg name=\"config_path\" default=\"$(find vins_estimator)/../config/realsense/vio_atlas.yaml\" />\n <arg nam"
},
{
"path": "vins_estimator/launch/realsense/rs_camera.launch",
"chars": 7500,
"preview": "<launch>\n <rosparam>\n /camera/aligned_depth_to_color/image_raw/compressedDepth/png_level: 2\n </rosparam>\n "
},
{
"path": "vins_estimator/launch/tum_rgbd/tum_rgbd_atlas.launch",
"chars": 1683,
"preview": "<launch>\n <arg name=\"config_path\" default=\"$(find vins_estimator)/../config/tum_rgbd/tum_fr3_atlas.yaml\" />\n <arg "
},
{
"path": "vins_estimator/launch/tum_rgbd/tum_rgbd_pytorch.launch",
"chars": 1303,
"preview": "<launch>\n <arg name=\"config_path\" default=\"$(find vins_estimator)/../config/tum_rgbd/tum_fr3.yaml\" />\n <arg name=\""
},
{
"path": "vins_estimator/launch/tum_rgbd/tum_rgbd_tensorrt.launch",
"chars": 1414,
"preview": "<launch>\n <arg name=\"config_path\" default=\"$(find vins_estimator)/../config/tum_rgbd/tum_fr3.yaml\" />\n <arg name=\""
},
{
"path": "vins_estimator/launch/vins_rviz.launch",
"chars": 147,
"preview": "<launch>\n <node name=\"rvizvisualisation\" pkg=\"rviz\" type=\"rviz\" output=\"log\" args=\"-d $(find vins_estimator)/../confi"
},
{
"path": "vins_estimator/launch/yolo/atlas.launch",
"chars": 698,
"preview": "<launch>\n <arg name=\"manager_name\" default=\"nodelet_manager\" />\n <node pkg=\"nodelet\" type=\"nodelet\" name=\"$(arg ma"
},
{
"path": "vins_estimator/launch/yolo/pytorch.launch",
"chars": 257,
"preview": "<launch>\n <!-- <remap from=\"/camera/color/image_raw\" to=\"/d400/color/image_raw\" /> -->\n <!-- <remap from=\"/camera/"
},
{
"path": "vins_estimator/launch/yolo/tensorrt.launch",
"chars": 284,
"preview": "<launch>\n <node name=\"yolo_trt_ros\" pkg=\"yolo_ros\" type=\"yolo_trt_ros.py\" output=\"screen\">\n <remap from=\"~imag"
},
{
"path": "vins_estimator/nodelet_description.xml",
"chars": 251,
"preview": "<library path=\"lib/libestimator_nodelet\">\n <class name=\"vins_estimator/EstimatorNodelet\" type=\"estimator_nodelet_ns:"
},
{
"path": "vins_estimator/package.xml",
"chars": 1268,
"preview": "<?xml version=\"1.0\"?>\n<package>\n <name>vins_estimator</name>\n <version>0.0.0</version>\n <description>The vins_e"
},
{
"path": "vins_estimator/src/estimator/estimator.cpp",
"chars": 71544,
"preview": "#include \"estimator.h\"\n#include \"../utility/visualization.h\"\n#include <Eigen/src/Core/Matrix.h>\n#include <algorithm>\n#in"
},
{
"path": "vins_estimator/src/estimator/estimator.h",
"chars": 5860,
"preview": "#pragma once\n\n#include <mutex>\n#include <thread>\n\n#include \"../feature_manager/feature_manager.h\"\n#include \"../initial/i"
},
{
"path": "vins_estimator/src/estimator_nodelet.cpp",
"chars": 23089,
"preview": "#include <condition_variable>\n#include <cv_bridge/cv_bridge.h>\n#include <map>\n#include <mutex>\n#include <opencv2/core/ha"
},
{
"path": "vins_estimator/src/factor/imu_factor.h",
"chars": 9184,
"preview": "#pragma once\n\n#include <ros/assert.h>\n#include <iostream>\n#include <eigen3/Eigen/Dense>\n\n#include \"../utility/utility.h\""
},
{
"path": "vins_estimator/src/factor/imu_factor_modified.h",
"chars": 9261,
"preview": "#pragma once\n\n#include <ros/assert.h>\n#include <iostream>\n#include <eigen3/Eigen/Dense>\n\n#include \"../utility/utility.h\""
},
{
"path": "vins_estimator/src/factor/integration_base.h",
"chars": 10461,
"preview": "#pragma once\n\n#include \"../utility/utility.h\"\n#include \"../utility/parameters.h\"\n\n#include <ceres/ceres.h>\nusing namespa"
},
{
"path": "vins_estimator/src/factor/marginalization_factor.cpp",
"chars": 16087,
"preview": "#include \"marginalization_factor.h\"\n\nvoid ResidualBlockInfo::Evaluate()\n{\n residuals.resize(cost_function->num_residu"
},
{
"path": "vins_estimator/src/factor/marginalization_factor.h",
"chars": 2850,
"preview": "#pragma once\n\n#include <ceres/ceres.h>\n#include <cstdlib>\n#include <pthread.h>\n#include <ros/console.h>\n#include <ros/ro"
},
{
"path": "vins_estimator/src/factor/pose_local_parameterization.cpp",
"chars": 857,
"preview": "#include \"pose_local_parameterization.h\"\n\nbool PoseLocalParameterization::Plus(const double *x, const double *delta,\n "
},
{
"path": "vins_estimator/src/factor/pose_local_parameterization.h",
"chars": 473,
"preview": "#pragma once\n\n#include <eigen3/Eigen/Dense>\n#include <ceres/ceres.h>\n#include \"../utility/utility.h\"\n\nclass PoseLocalPar"
},
{
"path": "vins_estimator/src/factor/projection_factor.cpp",
"chars": 9814,
"preview": "#include \"projection_factor.h\"\n\nEigen::Matrix2d ProjectionFactor::sqrt_info;\ndouble ProjectionFactor::sum_t;\n\nP"
},
{
"path": "vins_estimator/src/factor/projection_factor.h",
"chars": 706,
"preview": "#pragma once\n\n#include <ros/assert.h>\n#include <ceres/ceres.h>\n#include <Eigen/Dense>\n#include \"../utility/utility.h\"\n#i"
},
{
"path": "vins_estimator/src/factor/projection_td_factor.cpp",
"chars": 11655,
"preview": "#include \"projection_td_factor.h\"\n\nEigen::Matrix2d ProjectionTdFactor::sqrt_info;\ndouble ProjectionTdFactor::su"
},
{
"path": "vins_estimator/src/factor/projection_td_factor.h",
"chars": 1082,
"preview": "#pragma once\n\n#include <ros/assert.h>\n#include <ceres/ceres.h>\n#include <Eigen/Dense>\n#include \"../utility/utility.h\"\n#i"
},
{
"path": "vins_estimator/src/feature_manager/feature_manager.cpp",
"chars": 26278,
"preview": "#include \"feature_manager.h\"\n#include <complex>\n#include <opencv2/core/hal/interface.h>\n#include <opencv2/highgui.hpp>\n#"
},
{
"path": "vins_estimator/src/feature_manager/feature_manager.h",
"chars": 3907,
"preview": "#ifndef FEATURE_MANAGER_H\n#define FEATURE_MANAGER_H\n\n#include <algorithm>\n#include <list>\n#include <numeric>\n#include <v"
},
{
"path": "vins_estimator/src/feature_tracker/ThreadPool.h",
"chars": 2760,
"preview": "#ifndef THREAD_POOL_H\n#define THREAD_POOL_H\n\n#include <condition_variable>\n#include <functional>\n#include <future>\n#incl"
},
{
"path": "vins_estimator/src/feature_tracker/feature_tracker.cpp",
"chars": 20711,
"preview": "#include \"feature_tracker.h\"\n#include <cstddef>\n#include <future>\n#include <memory>\n#include <opencv2/core/types.hpp>\n#i"
},
{
"path": "vins_estimator/src/feature_tracker/feature_tracker.h",
"chars": 2367,
"preview": "#pragma once\n\n#include <csignal>\n#include <cstdio>\n#include <execinfo.h>\n#include <iostream>\n#include <queue>\n\n#include "
},
{
"path": "vins_estimator/src/initial/initial_aligment.cpp",
"chars": 17883,
"preview": "#include \"initial_alignment.h\"\n\nvoid solveGyroscopeBias(map<double, ImageFrame> &all_image_frame, Vector3d *Bgs)\n{\n M"
},
{
"path": "vins_estimator/src/initial/initial_alignment.h",
"chars": 1222,
"preview": "#pragma once\n#include \"../factor/imu_factor.h\"\n#include \"../feature_manager/feature_manager.h\"\n#include \"../utility/util"
},
{
"path": "vins_estimator/src/initial/initial_ex_rotation.cpp",
"chars": 4899,
"preview": "#include \"initial_ex_rotation.h\"\n\nInitialEXRotation::InitialEXRotation()\n{\n frame_count = 0;\n Rc.push_back(Matrix3"
},
{
"path": "vins_estimator/src/initial/initial_ex_rotation.h",
"chars": 1076,
"preview": "#pragma once\n\n#include <vector>\n#include \"../utility/parameters.h\"\nusing namespace std;\n\n#include <opencv2/opencv.hpp>\n\n"
},
{
"path": "vins_estimator/src/initial/initial_sfm.cpp",
"chars": 25489,
"preview": "#include \"initial_sfm.h\"\n\nGlobalSFM::GlobalSFM() {}\n\nvoid GlobalSFM::triangulatePoint(Eigen::Matrix<double, 3, 4> &Pose0"
},
{
"path": "vins_estimator/src/initial/initial_sfm.h",
"chars": 2825,
"preview": "#pragma once\n#include <ceres/ceres.h>\n#include <ceres/rotation.h>\n#include <eigen3/Eigen/Dense>\n#include <iostream>\n#inc"
},
{
"path": "vins_estimator/src/initial/solve_5pts.cpp",
"chars": 9411,
"preview": "#include \"solve_5pts.h\"\n#include <sophus/se3.h>\n#include <sophus/so3.h>\nusing Sophus::SE3;\nusing Sophus::SO3;\n\nnamespace"
},
{
"path": "vins_estimator/src/initial/solve_5pts.h",
"chars": 896,
"preview": "#pragma once\n\n#include <vector>\nusing namespace std;\n\n#include <opencv2/opencv.hpp>\n//#include <opencv2/core/eigen.hpp>\n"
},
{
"path": "vins_estimator/src/utility/CameraPoseVisualization.cpp",
"chars": 6829,
"preview": "#include \"CameraPoseVisualization.h\"\n\nconst Eigen::Vector3d CameraPoseVisualization::imlt = Eigen::Vector3d(-1.0, -0.5, "
},
{
"path": "vins_estimator/src/utility/CameraPoseVisualization.h",
"chars": 1510,
"preview": "#pragma once\n\n#include <ros/ros.h>\n#include <std_msgs/ColorRGBA.h>\n#include <visualization_msgs/Marker.h>\n#include <visu"
},
{
"path": "vins_estimator/src/utility/parameters.cpp",
"chars": 6583,
"preview": "#include \"parameters.h\"\n\n#include <thread>\n\ndouble INIT_DEPTH;\ndouble MIN_PARALLAX;\ndouble ACC_N, ACC_W;\ndouble GYR_N, G"
},
{
"path": "vins_estimator/src/utility/parameters.h",
"chars": 2384,
"preview": "#pragma once\n\n#include \"utility.h\"\n#include <eigen3/Eigen/Dense>\n#include <fstream>\n#include <opencv2/core/eigen.hpp>\n#i"
},
{
"path": "vins_estimator/src/utility/tic_toc.h",
"chars": 526,
"preview": "#pragma once\n\n#include <ctime>\n#include <cstdlib>\n#include <chrono>\n\nclass TicToc\n{\npublic:\n TicToc()\n {\n t"
},
{
"path": "vins_estimator/src/utility/utility.cpp",
"chars": 536,
"preview": "#include \"utility.h\"\n\n// https://blog.csdn.net/huanghaihui_123/article/details/103075107#commentBox\n// R_W_I\nEigen::Matr"
},
{
"path": "vins_estimator/src/utility/utility.h",
"chars": 4759,
"preview": "#pragma once\n\n#include <cmath>\n#include <cassert>\n#include <cstring>\n#include <eigen3/Eigen/Dense>\n\nclass Utility\n{\n pu"
},
{
"path": "vins_estimator/src/utility/visualization.cpp",
"chars": 23805,
"preview": "#include \"visualization.h\"\n#include \"parameters.h\"\n\nusing namespace ros;\nusing namespace Eigen;\nros::Publisher pub_odome"
},
{
"path": "vins_estimator/src/utility/visualization.h",
"chars": 2402,
"preview": "#pragma once\n\n#include \"../estimator/estimator.h\"\n#include \"CameraPoseVisualization.h\"\n#include \"parameters.h\"\n#include "
}
]
About this extraction
This page contains the full source code of the jianhengLiu/VINS-RGBD-FAST GitHub repository, extracted and formatted as plain text for AI agents and large language models (LLMs). The extraction includes 152 files (1.1 MB), approximately 338.5k tokens, and a symbol index with 253 extracted functions, classes, methods, constants, and types. Use this with OpenClaw, Claude, ChatGPT, Cursor, Windsurf, or any other AI tool that accepts text input. You can copy the full output to your clipboard or download it as a .txt file.
Extracted by GitExtract — free GitHub repo to text converter for AI. Built by Nikandr Surkov.