template class sophus::PointTransformer

Overview

Functor to efficiently transform a number of point given a Se3 pose. More…

#include <point_transform.h>

template <class TT>
class PointTransformer {
public:
    // construction

    PointTransformer();
    PointTransformer(sophus::Se3<TT> const& foo_from_bar);

    // methods

    Eigen::Matrix<TT, 3, 1> transform(Eigen::Matrix<TT, 3, 1> const& point_in_bar) const;
    Eigen::Matrix<TT, 3, 1> scaledTransform(InverseDepthPoint3<TT> const& inverse_depth_point_in_bar) const;
    Eigen::Matrix<TT, 2, 1> projTransform(Eigen::Matrix<TT, 3, 1> const& point_in_bar) const;
    Eigen::Matrix<TT, 2, 1> projTransform(InverseDepthPoint3<TT> const& inverse_depth_point_in_bar) const;
    Eigen::Matrix<TT, 2, 6> dxProjExpXTransformPointAt0(InverseDepthPoint3<TT> const& inverse_depth_point_in_bar) const;
    Eigen::Matrix<TT, 2, 3> dxProjTransformX(InverseDepthPoint3<TT> const& inverse_depth_point_in_bar) const;
    sophus::Se3<TT> const& fooFromBar() const;
    Eigen::Matrix<TT, 3, 3> const& fooRotationBar() const;
    Eigen::Matrix<TT, 3, 1> const& barOriginInFoo() const;
};

Detailed Documentation

Functor to efficiently transform a number of point given a Se3 pose.

When transforming a point point_in_bar given a sophus::Se3 pose foo_from_bar, one can simply use

Eigen::Vector3d = foo_from_bar * point_in_bar;

Internally, this applies the (unit) quaternion to the left and the right of the point to rotate it and then adds the translation:

point_in_foo = q*point_in_bar*q' + bar_origin_in_foo.

If there are a lot of point to transform, there is a more efficient way using the rotation matrix R which can be precomputed from the quaternion q.

point_in_foo = R * point_in_bar + bar_origin_in_foo

This is what this functor is for.

Methods

Eigen::Matrix<TT, 3, 1> transform(Eigen::Matrix<TT, 3, 1> const& point_in_bar) const

Transforms a 3-point from frame bar to frame foo.

Eigen::Matrix<TT, 2, 1> projTransform(Eigen::Matrix<TT, 3, 1> const& point_in_bar) const

Transforms 3-point in frame bar to foo and projects it onto the Euclidean plane z=1 in foo.

Eigen::Matrix<TT, 2, 1> projTransform(InverseDepthPoint3<TT> const& inverse_depth_point_in_bar) const

Transforms and projects the 3d inverse depth point in frame bar to the Euclidean plane z=1 in foo.

Eigen::Matrix<TT, 2, 6> dxProjExpXTransformPointAt0(InverseDepthPoint3<TT> const& inverse_depth_point_in_bar) const

Returns pose derivative of inverse depth point projection at the identity:

Dx proj(exp(x) * foo_from_bar * foo_in_bar.toEuclideanPoint3()) at x=0

with foo_in_bar = (a,b,psi) being an inverse depth point.