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.