PoseLib

PoseLib

This library provides a collection of minimal solvers for camera pose estimation. The focus is on calibrated absolute pose estimation problems from different types of correspondences (e.g. point-point, point-line, line-point, line-line).

The goals of this project are to provide

• Fast and robust implementation of the current state-of-the-art solvers.
• Consistent calling interface between different solvers.
• Minimize dependencies, both external (currently only Eigen) and internal. Each solver is (mostly) stand-alone, making it easy to extract only a specific solver to integrate into other frameworks.
• Robust estimators (based on LO-RANSAC) that just works out-of-the-box for most cases.

Robust Estimation and Non-linear Refinement

We provide robust estimators for the most common problems

• Absolute pose from points (and lines)
• Relative pose from points (Essential / Fundamental matrix, unknown shared focal length)
• Relative pose from points and monocular depth estimates.
• Homography
• Generalized relative pose

It is fairly straight-forward to implement robust estimators for other problems. See for example absolute_pose.h. If you implement estimators for other problems, please consider submitting a pull-request.

In robust.h we provide interfaces which normalizes the data, calls the RANSAC and runs a post-RANSAC non-linear refinement. It is also possible to directly call the individual components as well (see e.g. ransac.h, bundle.h, etc.). The RANSAC is straight-forward implementation of LO-RANSAC which generate hypothesis with minimal solvers and relies on non-linear refinement for refitting.

The robust estimator takes the following options.

General RANSAC options:

struct RansacOptions {
    size_t max_iterations = 100000;
    size_t min_iterations = 1000;
    double dyn_num_trials_mult = 3.0;
    double success_prob = 0.9999;
    unsigned long seed = 0;
    // If we should use PROSAC sampling. Assumes data is sorted
    bool progressive_sampling = false;
    size_t max_prosac_iterations = 100000;
    // Whether to treat the input 'best_model' as an initial model and score it before running the main RANSAC loop
    bool score_initial_model = false;
};

the non-linear refinement:

struct BundleOptions {
    size_t max_iterations = 100;
    enum LossType {
        TRIVIAL, TRUNCATED, HUBER, CAUCHY, TRUNCATED_CAUCHY, TRUNCATED_LE_ZACH
    } loss_type = LossType::CAUCHY;
    double loss_scale = 1.0;
    double gradient_tol = 1e-8;
    double step_tol = 1e-8;
    double initial_lambda = 1e-3;
    double min_lambda = 1e-10;
    double max_lambda = 1e10;
    bool verbose = false;
};

and the estimator specific options which contain the error thresholds and other settings. For more details and additional option structs (like AbsolutePoseOptions or MonoDepthRelativePoseOptions), see types.h.

For example, RelativePoseOptions:

struct RelativePoseOptions {
    RansacOptions ransac;
    BundleOptions bundle;

    double max_error = 1.0;      // Inlier threshold (in pixels)
    bool tangent_sampson = false; // Whether to use tangent Sampson error in LO
    bool real_focal_check = false; // Whether to use real focal length checking
};

Note that in robust.h this is only used for the post-RANSAC refinement. For the relative pose estimators using monocular depth we recommend using TRUNCATED_CAUCHY.

In bundle.h we provide non-linear refinement for different problems. Mainly minimizing reprojection error and Sampson error as these performed best in our internal evaluations. These are used in the LO-RANSAC to perform non-linear refitting. Most estimators directly minimize the MSAC score (using loss_type = TRUNCATED and loss_scale = threshold) over all input correspondences. In practice we found that this works quite well and avoids recursive LO where inliers are added in steps.

Camera models

PoseLib use COLMAP-compatible camera models. These are defined in colmap_models.h. Currently we only support

• SIMPLE_PINHOLE
• PINHOLE
• SIMPLE_RADIAL
• RADIAL
• OPENCV
• OPENCV_FISHEYE

but it is relatively straight-forward to add other models. If you do so please consider opening a pull-request. In contrast to COLMAP, we require analytical jacobians for the distortion mappings which make it a bit more work to port them.

The Camera struct currently contains width/height fields, however these are not used anywhere in the code-base and are provided simply to be consistent with COLMAP. The Camera class also provides the helper function initialize_from_txt(str) which initializes the camera from a line given by the cameras.txt file of a COLMAP reconstruction.

The python bindings also expose the poselib.Camera class with focal(), focal_x(), focal_y(), model_name(), prinicipal_point() read-only methods and a read-write params property, but currently this is only used as a return type for some methods. To supply camera information to robust estimators you should use python dicts as shown below.

Python bindings

The python bindings can be installed by running pip install .. The python bindings expose all minimal solvers, e.g. poselib.p3p(x,X), as well as all robust estimators from robust.h.

Examples of how the robust estimators can be called are

camera = {'model': 'SIMPLE_PINHOLE', 'width': 1200, 'height': 800, 'params': [960, 600, 400]}

pose, info = poselib.estimate_absolute_pose(p2d, p3d, camera, {'max_error': 16.0}, {})

or

F, info = poselib.estimate_fundamental(p2d_1, p2d_2, {'max_error': 0.75, 'progressive_sampling': True}, {})
mask_inl = info['inliers']

The return value info is a dict containing information about the robust estimation (inliers, iterations, etc). The last two options are dicts which describe the RansacOptions and BundleOptions. Ommited values are set to their default (see above), except for the loss_scale used for the Cauchy loss which is set to half of the threshold used in RANSAC (which seems to be a good heuristic). Dicts with the default options can be obtained as opt = poselib.RansacOptions() or poselib.BundleOptions().

Some of the available estimators are listed below, check pyposelib.cpp and robust.h for more details. The table also shows which error threshold is used in the estimation (max_error or max_errors). All thresholds are given in pixels.

Storing poses and estimated camera parameters

To handle poses and cameras we provide the following classes:

• CameraPose: This class is the return type for the most of the methods. While the class internally represent the pose with q and t, it also exposes R (3x3) and Rt (3x4) which are read/write, i.e. you can do pose.R = Rnew and it will update the underlying quaternion q.
• MonoDepthTwoViewGeometry: This class is the return type for the calibrated monocular depth solvers and estimators. It holds CameraPose as pose, the relative scale of the two depths and their shifts shift1, shift2.
• Image: Following COLMAP, this class stores information about the camera (image.camera) and its pose (image.pose) used to take an image.
• ImagePair: This class holds information about two cameras (image_pair.camera1, image_pair.camera2) and their relative pose (image_pair.pose). This class is used as the return type for the estimate_shared_focal_relative_pose robust estimator.
• MonoDepthImagePair: Similar to ImagePair, but instead of holding pose directly it holds MonoDepthTwoViewGeometry as geometry. This class is used for the uncalibrated relative pose solvers/estimators using points and monocular depth estimates.
• ProjectiveImagePair: This class is used for F + radial distortion solvers. It stores information about the cameras (image.camera1, image.camera2) which usually contain only the distortion parameters. The class also stores the fundamental matrix F which contains information about the pose and other intrinsics.

All of these are also exposed via python bindings as: poselib.CameraPose, poselib.Image, poselib.ImagePair.

Benchmarking the robust estimators

To sanity-check the robust estimators we benchmark against the LO-RANSAC implementation from pycolmap.

For all of the metrics higher is better (except for runtime).

Minimal Solvers

Naming convention

For the solver names we use a slightly non-standard notation where we denote the solver as

pXpYplZlpWll

where the number of correspondences required is given by

• Xp - 2D point to 3D point,
• Ypl - 2D point to 3D line,
• Zlp - 2D line to 3D point,
• Wll - 2D line to 3D line.

The prefix with u is for upright solvers and g for generalized camera solvers. Solvers that estimate focal length have the postfix with f and similarly s for solvers that estimate scale.

Calling conventions

All solvers return their solutions as a vector of CameraPose structs, which defined as

struct CameraPose {
   Eigen::Vector4d q;
   Eigen::Vector3d t;
};

where the rotation is representation as a quaternion q and the convention is that [R t] maps from the world coordinate system into the camera coordinate system.

For 2D point to 3D point correspondences, the image points are represented as unit-length bearings vectors. The returned camera poses (R,t) then satisfies (for some lambda)

  lambda * x[i] = R * X[i] + t

where x[i] is the 2D point and X[i] is the 3D point. Note that only the P3P solver filters solutions with negative lambda.

Solvers that use point-to-point constraints take one vector with bearing vectors x and one vector with the corresponding 3D points X, e.g. for the P3P solver the function declaration is

int p3p(const std::vector &x,
        const std::vector &X,
        std::vector *output);

Each solver returns the number of real solutions found.

For constraints with 2D lines, the lines are represented in homogeneous coordinates. In the case of 2D line to 3D point constraints, the returned camera poses then satisfies

  l[i].transpose() * (R * X[i] + t) = 0

where l[i] is the line and X[i] is the 3D point.

For constraints with 3D lines, the lines are represented by