class sophus::CameraModel

Overview

Concrete camera model class. More…

#include <camera_model.h>

class CameraModel {
public:
    // construction

    CameraModel();

    template <class TTransformModelT>
    CameraModel(TTransformModelT model);

    CameraModel(
        ImageSize image_size,
        CameraDistortionType projection_type,
        Eigen::VectorXd const& params
        );

    // methods

    bool isEmpty() const;
    std::string_view distortionModelName() const;
    CameraDistortionVariant& modelVariant();
    CameraDistortionVariant const& modelVariant() const;
    CameraDistortionType distortionType() const;
    Eigen::Vector2d focalLength() const;
    void setFocalLength(Eigen::Vector2d const& focal_length);
    Eigen::Vector2d principalPoint() const;
    void setPrincipalPoint(Eigen::Vector2d const& principal_point);
    Eigen::VectorXd params() const;
    void setParams(Eigen::VectorXd const& params);
    Eigen::VectorXd distortionParams() const;
    Eigen::Vector2d camProj(Eigen::Vector3d const& point_camera) const;
    Eigen::Vector2d distort(Eigen::Vector2d const& point2_in_camera_lifted) const;
    Eigen::Matrix2d dxDistort(Eigen::Vector2d const& point2_in_camera_lifted) const;
    Eigen::Vector2d undistort(Eigen::Vector2d const& pixel_image) const;
    MutImage<Eigen::Vector2f> undistortTable() const;
    Eigen::Matrix<double, 2, 3> dxCamProjX(Eigen::Vector3d const& point_in_camera) const;
    Eigen::Matrix<double, 2, 6> dxCamProjExpXPointAt0(Eigen::Vector3d const& point_in_camera) const;
    Eigen::Vector3d camUnproj(Eigen::Vector2d const& pixel_image, double depth_z) const;
    CameraModel subsampleDown() const;
    CameraModel subsampleUp() const;
    CameraModel binDown() const;
    CameraModel binUp() const;
    ImageSize const& imageSize() const;
    CameraModel roi(Eigen::Vector2i const& top_left, ImageSize roi_size) const;
    bool contains(Eigen::Vector2i const& obs, int border = 0) const;
    bool contains(Eigen::Vector2d const& obs, double border = 0) const;
    CameraModel scale(ImageSize image_size) const;
    static CameraModel createDefaultPinholeModel(ImageSize image_size);
};

Detailed Documentation

Concrete camera model class.

Construction

template <class TTransformModelT>
CameraModel(TTransformModelT model)

Constructs camera model from frame_name and concrete projection model.

CameraModel(
    ImageSize image_size,
    CameraDistortionType projection_type,
    Eigen::VectorXd const& params
    )

Constructs camera model from frame_name, image_size, projection_type flag and params vector.

Precondition: params.size() must match the number of parameters of the specified projection_type (TransformModel::kNumParams).

Methods

bool isEmpty() const

Returns true if this camera remains default-initialized with zero image dimensions.

std::string_view distortionModelName() const

Returns name of the camera distortion model.

CameraDistortionVariant& modelVariant()

Distortion variant mutator.

CameraDistortionVariant const& modelVariant() const

Distortion variant accessor.

CameraDistortionType distortionType() const

Camera transform flag.

void setFocalLength(Eigen::Vector2d const& focal_length)

Focal length.

void setPrincipalPoint(Eigen::Vector2d const& principal_point)

Focal length.

Eigen::VectorXd params() const

Returns params vector by value.

void setParams(Eigen::VectorXd const& params)

Sets params vector.

Precontion: params.size() must match the number of parameters of the specivied projection_type (TransformModel::kNumParams).

Eigen::VectorXd distortionParams() const

Returns distortion parameters vector by value.

Eigen::Vector2d camProj(Eigen::Vector3d const& point_camera) const

Given a point in 3D space in the camera frame, compute the corresponding pixel coordinates in the image.

Eigen::Vector2d distort(Eigen::Vector2d const& point2_in_camera_lifted) const

Maps a 2-point in the z=1 plane of the camera to a (distorted) pixel in the image.

Eigen::Vector2d undistort(Eigen::Vector2d const& pixel_image) const

Maps a pixel in the image to a 2-point in the z=1 plane of the camera.

Eigen::Matrix<double, 2, 3> dxCamProjX(Eigen::Vector3d const& point_in_camera) const

Derivative of camProj(x) with respect to x=0.

Eigen::Matrix<double, 2, 6> dxCamProjExpXPointAt0(Eigen::Vector3d const& point_in_camera) const

Derivative of camProj(exp(x) * point) with respect to x=0.

Eigen::Vector3d camUnproj(Eigen::Vector2d const& pixel_image, double depth_z) const

Given pixel coordinates in the distorted image, and a corresponding depth, reproject to a 3d point in the camera’s reference frame.

CameraModel subsampleDown() const

Subsamples pixel down, factor of 0.5.

See for details: https://docs.google.com/document/d/1xmhCMWklP2UoQMGaMqFnsoPWoeMvBfXN7S8-ie9k0UA/edit#heading=h.97r8rr8owwpc

If the original width [height] is odd, the new width [height] will be: (width+1)/2 [height+1)/2].

CameraModel subsampleUp() const

Subsamples pixel up, factor of 2.0.

CameraModel binDown() const

Bins pixel down, factor of 0.5.

See for details: https://docs.google.com/document/d/1xmhCMWklP2UoQMGaMqFnsoPWoeMvBfXN7S8-ie9k0UA/edit#heading=h.elfm6123mecj

If the original width [height] is odd, the new width [height] will be: (width+1)/2 [height+1)/2].

CameraModel binUp() const

Bins pixel up, factor of 2.0.

See for details: https://docs.google.com/document/d/1xmhCMWklP2UoQMGaMqFnsoPWoeMvBfXN7S8-ie9k0UA/edit#heading=h.elfm6123mecj

ImageSize const& imageSize() const

Image size accessor.

CameraModel roi(Eigen::Vector2i const& top_left, ImageSize roi_size) const

Region of interest given top_left and ` roi_size.

bool contains(Eigen::Vector2i const& obs, int border = 0) const

Returns true if obs is within image.

Note: Postiive border makes the image frame smaller.

bool contains(Eigen::Vector2d const& obs, double border = 0) const

Returns true if obs is within image.

Postiive border makes the image frame smaller.

static CameraModel createDefaultPinholeModel(ImageSize image_size)

Creates default pinhole model from image_size.