131 lines
4.5 KiB
C++
131 lines
4.5 KiB
C++
#include <iostream>
|
|
|
|
#include "tests.hpp"
|
|
|
|
#include <sophus/velocities.hpp>
|
|
|
|
namespace Sophus {
|
|
namespace experimental {
|
|
|
|
template <class Scalar>
|
|
bool tests_linear_velocities() {
|
|
bool passed = true;
|
|
std::vector<SE3<Scalar>, Eigen::aligned_allocator<SE3<Scalar>>> bar_Ts_baz;
|
|
|
|
for (size_t i = 0; i < 10; ++i) {
|
|
bar_Ts_baz.push_back(SE3<Scalar>::rotX(i * 0.001) *
|
|
SE3<Scalar>::rotY(i * 0.001) *
|
|
SE3<Scalar>::transX(0.01 * i));
|
|
}
|
|
|
|
SE3<Scalar> foo_T_bar =
|
|
SE3<Scalar>::rotX(0.5) * SE3<Scalar>::rotZ(0.2) * SE3<Scalar>::transY(2);
|
|
|
|
std::vector<SE3<Scalar>, Eigen::aligned_allocator<SE3<Scalar>>> foo_Ts_baz;
|
|
for (auto const& bar_T_baz : bar_Ts_baz) {
|
|
foo_Ts_baz.push_back(foo_T_bar * bar_T_baz);
|
|
}
|
|
|
|
auto gen_linear_vels =
|
|
[](std::vector<SE3<Scalar>, Eigen::aligned_allocator<SE3<Scalar>>> const&
|
|
a_Ts_b) {
|
|
std::vector<Vector3<Scalar>, Eigen::aligned_allocator<Vector3<Scalar>>>
|
|
linearVels_a;
|
|
for (size_t i = 0; i < a_Ts_b.size() - 1; ++i) {
|
|
linearVels_a.push_back(a_Ts_b[i + 1].translation() -
|
|
a_Ts_b[i].translation());
|
|
}
|
|
return linearVels_a;
|
|
};
|
|
|
|
// linear velocities in frame bar
|
|
std::vector<Vector3<Scalar>, Eigen::aligned_allocator<Vector3<Scalar>>>
|
|
linearVels_bar = gen_linear_vels(bar_Ts_baz);
|
|
// linear velocities in frame foo
|
|
std::vector<Vector3<Scalar>, Eigen::aligned_allocator<Vector3<Scalar>>>
|
|
linearVels_foo = gen_linear_vels(foo_Ts_baz);
|
|
|
|
for (size_t i = 0; i < linearVels_bar.size(); ++i) {
|
|
SOPHUS_TEST_APPROX(passed, linearVels_foo[i],
|
|
transformVelocity(foo_T_bar, linearVels_bar[i]),
|
|
sqrt(Constants<Scalar>::epsilon()));
|
|
}
|
|
return passed;
|
|
}
|
|
|
|
template <class Scalar>
|
|
bool tests_rotational_velocities() {
|
|
bool passed = true;
|
|
|
|
SE3<Scalar> foo_T_bar =
|
|
SE3<Scalar>::rotX(0.5) * SE3<Scalar>::rotZ(0.2) * SE3<Scalar>::transY(2);
|
|
|
|
// One parameter subgroup of SE3, motion through space given time t.
|
|
auto bar_T_baz = [](Scalar t) -> SE3<Scalar> {
|
|
return SE3<Scalar>::rotX(t * Scalar(0.01)) *
|
|
SE3<Scalar>::rotY(t * Scalar(0.0001)) *
|
|
SE3<Scalar>::transX(t * Scalar(0.0001));
|
|
};
|
|
|
|
std::vector<Scalar> ts = {Scalar(0), Scalar(0.3), Scalar(1)};
|
|
|
|
Scalar h = Constants<Scalar>::epsilon();
|
|
for (Scalar t : ts) {
|
|
// finite difference approximiation of instantanious velocity in frame bar
|
|
Vector3<Scalar> rotVel_in_frame_bar =
|
|
finiteDifferenceRotationalVelocity<Scalar>(bar_T_baz, t, h);
|
|
|
|
// finite difference approximiation of instantanious velocity in frame foo
|
|
Vector3<Scalar> rotVel_in_frame_foo =
|
|
finiteDifferenceRotationalVelocity<Scalar>(
|
|
[&foo_T_bar, bar_T_baz](Scalar t) -> SE3<Scalar> {
|
|
return foo_T_bar * bar_T_baz(t);
|
|
},
|
|
t, h);
|
|
|
|
Vector3<Scalar> rotVel_in_frame_bar2 =
|
|
transformVelocity(foo_T_bar.inverse(), rotVel_in_frame_foo);
|
|
SOPHUS_TEST_APPROX(
|
|
passed, rotVel_in_frame_bar, rotVel_in_frame_bar2,
|
|
// not too tight threshold, because of finit difference approximation
|
|
std::sqrt(Constants<Scalar>::epsilon()));
|
|
|
|
// The rotational velocities rotVel_in_frame_foo and rotVel_in_frame_bar
|
|
// should not be equal since they are in different frames (foo != bar).
|
|
SOPHUS_TEST_NOT_APPROX(passed, rotVel_in_frame_foo, rotVel_in_frame_bar,
|
|
Scalar(1e-3));
|
|
|
|
// Expect same result when using adjoint instead since:
|
|
// vee(bar_R_foo * hat(vel_foo) * bar_R_foo^T = bar_R_foo 8 vel_foo.
|
|
SOPHUS_TEST_APPROX(
|
|
passed, transformVelocity(foo_T_bar.inverse(), rotVel_in_frame_foo),
|
|
SO3<Scalar>::vee(foo_T_bar.so3().inverse().matrix() *
|
|
SO3<Scalar>::hat(rotVel_in_frame_foo) *
|
|
foo_T_bar.so3().matrix()),
|
|
Constants<Scalar>::epsilon());
|
|
}
|
|
return passed;
|
|
}
|
|
|
|
int test_velocities() {
|
|
using std::cerr;
|
|
using std::endl;
|
|
|
|
cerr << "Test Velocities" << endl << endl;
|
|
cerr << "Double tests: " << endl;
|
|
bool passed = tests_linear_velocities<double>();
|
|
passed &= tests_rotational_velocities<double>();
|
|
processTestResult(passed);
|
|
|
|
cerr << "Float tests: " << endl;
|
|
passed = tests_linear_velocities<float>();
|
|
passed &= tests_rotational_velocities<float>();
|
|
processTestResult(passed);
|
|
|
|
return 0;
|
|
}
|
|
} // namespace experimental
|
|
} // namespace Sophus
|
|
|
|
int main() { return Sophus::experimental::test_velocities(); }
|