forked from logzhan/ORB-SLAM3-UESTC
75 lines
2.3 KiB
C++
75 lines
2.3 KiB
C++
|
#ifndef SOPHUS_VELOCITIES_HPP
|
||
|
|
#define SOPHUS_VELOCITIES_HPP
|
||
|
|
|
||
|
|
#include <functional>
|
||
|
|
|
||
|
|
#include "num_diff.hpp"
|
||
|
|
#include "se3.hpp"
|
||
|
|
|
||
|
|
namespace Sophus {
|
||
|
|
namespace experimental {
|
||
|
|
// Experimental since the API will certainly change drastically in the future.
|
||
|
|
|
||
|
|
// Transforms velocity vector by rotation ``foo_R_bar``.
|
||
|
|
//
|
||
|
|
// Note: vel_bar can be either a linear or a rotational velocity vector.
|
||
|
|
//
|
||
|
|
template <class Scalar>
|
||
|
|
Vector3<Scalar> transformVelocity(SO3<Scalar> const& foo_R_bar,
|
||
|
|
Vector3<Scalar> const& vel_bar) {
|
||
|
|
// For rotational velocities note that:
|
||
|
|
//
|
||
|
|
// vel_bar = vee(foo_R_bar * hat(vel_bar) * foo_R_bar^T)
|
||
|
|
// = vee(hat(Adj(foo_R_bar) * vel_bar))
|
||
|
|
// = Adj(foo_R_bar) * vel_bar
|
||
|
|
// = foo_R_bar * vel_bar.
|
||
|
|
//
|
||
|
|
return foo_R_bar * vel_bar;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Transforms velocity vector by pose ``foo_T_bar``.
|
||
|
|
//
|
||
|
|
// Note: vel_bar can be either a linear or a rotational velocity vector.
|
||
|
|
//
|
||
|
|
template <class Scalar>
|
||
|
|
Vector3<Scalar> transformVelocity(SE3<Scalar> const& foo_T_bar,
|
||
|
|
Vector3<Scalar> const& vel_bar) {
|
||
|
|
return transformVelocity(foo_T_bar.so3(), vel_bar);
|
||
|
|
}
|
||
|
|
|
||
|
|
// finite difference approximation of instantanious velocity in frame foo
|
||
|
|
//
|
||
|
|
template <class Scalar>
|
||
|
|
Vector3<Scalar> finiteDifferenceRotationalVelocity(
|
||
|
|
std::function<SO3<Scalar>(Scalar)> const& foo_R_bar, Scalar t,
|
||
|
|
Scalar h = Constants<Scalar>::epsilon()) {
|
||
|
|
// https://en.wikipedia.org/w/index.php?title=Angular_velocity&oldid=791867792#Angular_velocity_tensor
|
||
|
|
//
|
||
|
|
// W = dR(t)/dt * R^{-1}(t)
|
||
|
|
Matrix3<Scalar> dR_dt_in_frame_foo = curveNumDiff(
|
||
|
|
[&foo_R_bar](Scalar t0) -> Matrix3<Scalar> {
|
||
|
|
return foo_R_bar(t0).matrix();
|
||
|
|
},
|
||
|
|
t, h);
|
||
|
|
// velocity tensor
|
||
|
|
Matrix3<Scalar> W_in_frame_foo =
|
||
|
|
dR_dt_in_frame_foo * (foo_R_bar(t)).inverse().matrix();
|
||
|
|
return SO3<Scalar>::vee(W_in_frame_foo);
|
||
|
|
}
|
||
|
|
|
||
|
|
// finite difference approximation of instantanious velocity in frame foo
|
||
|
|
//
|
||
|
|
template <class Scalar>
|
||
|
|
Vector3<Scalar> finiteDifferenceRotationalVelocity(
|
||
|
|
std::function<SE3<Scalar>(Scalar)> const& foo_T_bar, Scalar t,
|
||
|
|
Scalar h = Constants<Scalar>::epsilon()) {
|
||
|
|
return finiteDifferenceRotationalVelocity<Scalar>(
|
||
|
|
[&foo_T_bar](Scalar t) -> SO3<Scalar> { return foo_T_bar(t).so3(); }, t,
|
||
|
|
h);
|
||
|
|
}
|
||
|
|
|
||
|
|
} // namespace experimental
|
||
|
|
} // namespace Sophus
|
||
|
|
|
||
|
|
#endif // SOPHUS_VELOCITIES_HPP
|