logzhan
/
ORB-SLAM3
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
ORB-SLAM3/Thirdparty/Sophus/test/core/tests.hpp

563 lines
21 KiB
C++
Raw Blame History

#ifndef SOPUHS_TESTS_HPP
#define SOPUHS_TESTS_HPP
#include <array>
#include <Eigen/StdVector>
#include <unsupported/Eigen/MatrixFunctions>
#include <sophus/average.hpp>
#include <sophus/interpolate.hpp>
#include <sophus/num_diff.hpp>
#include <sophus/test_macros.hpp>
#ifdef SOPHUS_CERES
#include <ceres/jet.h>
#endif
namespace Sophus {
template <class LieGroup_>
class LieGroupTests {
public:
using LieGroup = LieGroup_;
using Scalar = typename LieGroup::Scalar;
using Transformation = typename LieGroup::Transformation;
using Tangent = typename LieGroup::Tangent;
using Point = typename LieGroup::Point;
using HomogeneousPoint = typename LieGroup::HomogeneousPoint;
using ConstPointMap = Eigen::Map<const Point>;
using Line = typename LieGroup::Line;
using Adjoint = typename LieGroup::Adjoint;
static int constexpr N = LieGroup::N;
static int constexpr DoF = LieGroup::DoF;
static int constexpr num_parameters = LieGroup::num_parameters;
LieGroupTests(
std::vector<LieGroup, Eigen::aligned_allocator<LieGroup>> const&
group_vec,
std::vector<Tangent, Eigen::aligned_allocator<Tangent>> const&
tangent_vec,
std::vector<Point, Eigen::aligned_allocator<Point>> const& point_vec)
: group_vec_(group_vec),
tangent_vec_(tangent_vec),
point_vec_(point_vec) {}
bool adjointTest() {
bool passed = true;
for (size_t i = 0; i < group_vec_.size(); ++i) {
Transformation T = group_vec_[i].matrix();
Adjoint Ad = group_vec_[i].Adj();
for (size_t j = 0; j < tangent_vec_.size(); ++j) {
Tangent x = tangent_vec_[j];
Transformation I;
I.setIdentity();
Tangent ad1 = Ad * x;
Tangent ad2 = LieGroup::vee(T * LieGroup::hat(x) *
group_vec_[i].inverse().matrix());
SOPHUS_TEST_APPROX(passed, ad1, ad2, Scalar(10) * kSmallEps,
"Adjoint case %, %", i, j);
}
}
return passed;
}
bool contructorAndAssignmentTest() {
bool passed = true;
for (LieGroup foo_T_bar : group_vec_) {
LieGroup foo_T2_bar = foo_T_bar;
SOPHUS_TEST_APPROX(passed, foo_T_bar.matrix(), foo_T2_bar.matrix(),
kSmallEps, "Copy constructor: %\nvs\n %",
transpose(foo_T_bar.matrix()),
transpose(foo_T2_bar.matrix()));
LieGroup foo_T3_bar;
foo_T3_bar = foo_T_bar;
SOPHUS_TEST_APPROX(passed, foo_T_bar.matrix(), foo_T3_bar.matrix(),
kSmallEps, "Copy assignment: %\nvs\n %",
transpose(foo_T_bar.matrix()),
transpose(foo_T3_bar.matrix()));
LieGroup foo_T4_bar(foo_T_bar.matrix());
SOPHUS_TEST_APPROX(
passed, foo_T_bar.matrix(), foo_T4_bar.matrix(), kSmallEps,
"Constructor from homogeneous matrix: %\nvs\n %",
transpose(foo_T_bar.matrix()), transpose(foo_T4_bar.matrix()));
Eigen::Map<LieGroup> foo_Tmap_bar(foo_T_bar.data());
LieGroup foo_T5_bar = foo_Tmap_bar;
SOPHUS_TEST_APPROX(
passed, foo_T_bar.matrix(), foo_T5_bar.matrix(), kSmallEps,
"Assignment from Eigen::Map type: %\nvs\n %",
transpose(foo_T_bar.matrix()), transpose(foo_T5_bar.matrix()));
Eigen::Map<LieGroup const> foo_Tcmap_bar(foo_T_bar.data());
LieGroup foo_T6_bar;
foo_T6_bar = foo_Tcmap_bar;
SOPHUS_TEST_APPROX(
passed, foo_T_bar.matrix(), foo_T5_bar.matrix(), kSmallEps,
"Assignment from Eigen::Map type: %\nvs\n %",
transpose(foo_T_bar.matrix()), transpose(foo_T5_bar.matrix()));
LieGroup I;
Eigen::Map<LieGroup> foo_Tmap2_bar(I.data());
foo_Tmap2_bar = foo_T_bar;
SOPHUS_TEST_APPROX(passed, foo_Tmap2_bar.matrix(), foo_T_bar.matrix(),
kSmallEps, "Assignment to Eigen::Map type: %\nvs\n %",
transpose(foo_Tmap2_bar.matrix()),
transpose(foo_T_bar.matrix()));
}
return passed;
}
bool derivativeTest() {
bool passed = true;
LieGroup g;
for (int i = 0; i < DoF; ++i) {
Transformation Gi = g.Dxi_exp_x_matrix_at_0(i);
Transformation Gi2 = curveNumDiff(
[i](Scalar xi) -> Transformation {
Tangent x;
setToZero(x);
setElementAt(x, xi, i);
return LieGroup::exp(x).matrix();
},
Scalar(0));
SOPHUS_TEST_APPROX(passed, Gi, Gi2, kSmallEpsSqrt,
"Dxi_exp_x_matrix_at_ case %", i);
}
return passed;
}
template <class G = LieGroup>
enable_if_t<std::is_same<G, Sophus::SO2<Scalar>>::value ||
std::is_same<G, Sophus::SO3<Scalar>>::value ||
std::is_same<G, Sophus::SE2<Scalar>>::value ||
std::is_same<G, Sophus::SE3<Scalar>>::value,
bool>
additionalDerivativeTest() {
bool passed = true;
for (size_t j = 0; j < tangent_vec_.size(); ++j) {
Tangent a = tangent_vec_[j];
Eigen::Matrix<Scalar, num_parameters, DoF> J = LieGroup::Dx_exp_x(a);
Eigen::Matrix<Scalar, num_parameters, DoF> J_num =
vectorFieldNumDiff<Scalar, num_parameters, DoF>(
[](Tangent const& x) -> Sophus::Vector<Scalar, num_parameters> {
return LieGroup::exp(x).params();
},
a);
SOPHUS_TEST_APPROX(passed, J, J_num, 3 * kSmallEpsSqrt,
"Dx_exp_x case: %", j);
}
Tangent o;
setToZero(o);
Eigen::Matrix<Scalar, num_parameters, DoF> J = LieGroup::Dx_exp_x_at_0();
Eigen::Matrix<Scalar, num_parameters, DoF> J_num =
vectorFieldNumDiff<Scalar, num_parameters, DoF>(
[](Tangent const& x) -> Sophus::Vector<Scalar, num_parameters> {
return LieGroup::exp(x).params();
},
o);
SOPHUS_TEST_APPROX(passed, J, J_num, kSmallEpsSqrt, "Dx_exp_x_at_0");
for (size_t i = 0; i < group_vec_.size(); ++i) {
LieGroup T = group_vec_[i];
Eigen::Matrix<Scalar, num_parameters, DoF> J = T.Dx_this_mul_exp_x_at_0();
Eigen::Matrix<Scalar, num_parameters, DoF> J_num =
vectorFieldNumDiff<Scalar, num_parameters, DoF>(
[T](Tangent const& x) -> Sophus::Vector<Scalar, num_parameters> {
return (T * LieGroup::exp(x)).params();
},
o);
SOPHUS_TEST_APPROX(passed, J, J_num, kSmallEpsSqrt,
"Dx_this_mul_exp_x_at_0 case: %", i);
}
return passed;
}
template <class G = LieGroup>
enable_if_t<!std::is_same<G, Sophus::SO2<Scalar>>::value &&
!std::is_same<G, Sophus::SO3<Scalar>>::value &&
!std::is_same<G, Sophus::SE2<Scalar>>::value &&
!std::is_same<G, Sophus::SE3<Scalar>>::value,
bool>
additionalDerivativeTest() {
return true;
}
bool productTest() {
bool passed = true;
for (size_t i = 0; i < group_vec_.size() - 1; ++i) {
LieGroup T1 = group_vec_[i];
LieGroup T2 = group_vec_[i + 1];
LieGroup mult = T1 * T2;
T1 *= T2;
SOPHUS_TEST_APPROX(passed, T1.matrix(), mult.matrix(), kSmallEps,
"Product case: %", i);
}
return passed;
}
bool expLogTest() {
bool passed = true;
for (size_t i = 0; i < group_vec_.size(); ++i) {
Transformation T1 = group_vec_[i].matrix();
Transformation T2 = LieGroup::exp(group_vec_[i].log()).matrix();
SOPHUS_TEST_APPROX(passed, T1, T2, kSmallEps, "G - exp(log(G)) case: %",
i);
}
return passed;
}
bool expMapTest() {
bool passed = true;
for (size_t i = 0; i < tangent_vec_.size(); ++i) {
Tangent omega = tangent_vec_[i];
Transformation exp_x = LieGroup::exp(omega).matrix();
Transformation expmap_hat_x = (LieGroup::hat(omega)).exp();
SOPHUS_TEST_APPROX(passed, exp_x, expmap_hat_x, Scalar(10) * kSmallEps,
"expmap(hat(x)) - exp(x) case: %", i);
}
return passed;
}
bool groupActionTest() {
bool passed = true;
for (size_t i = 0; i < group_vec_.size(); ++i) {
for (size_t j = 0; j < point_vec_.size(); ++j) {
Point const& p = point_vec_[j];
Point point1 = group_vec_[i] * p;
HomogeneousPoint hp = p.homogeneous();
HomogeneousPoint hpoint1 = group_vec_[i] * hp;
ConstPointMap p_map(p.data());
Point pointmap1 = group_vec_[i] * p_map;
Transformation T = group_vec_[i].matrix();
Point gt_point1 = map(T, p);
SOPHUS_TEST_APPROX(passed, point1, gt_point1, kSmallEps,
"Transform point case: %", i);
SOPHUS_TEST_APPROX(passed, hpoint1.hnormalized().eval(), gt_point1,
kSmallEps, "Transform homogeneous point case: %", i);
SOPHUS_TEST_APPROX(passed, pointmap1, gt_point1, kSmallEps,
"Transform map point case: %", i);
}
}
return passed;
}
bool lineActionTest() {
bool passed = point_vec_.size() > 1;
for (size_t i = 0; i < group_vec_.size(); ++i) {
for (size_t j = 0; j + 1 < point_vec_.size(); ++j) {
Point const& p1 = point_vec_[j];
Point const& p2 = point_vec_[j + 1];
Line l = Line::Through(p1, p2);
Point p1_t = group_vec_[i] * p1;
Point p2_t = group_vec_[i] * p2;
Line l_t = group_vec_[i] * l;
SOPHUS_TEST_APPROX(passed, l_t.squaredDistance(p1_t),
static_cast<Scalar>(0), kSmallEps,
"Transform line case (1st point) : %", i);
SOPHUS_TEST_APPROX(passed, l_t.squaredDistance(p2_t),
static_cast<Scalar>(0), kSmallEps,
"Transform line case (2nd point) : %", i);
SOPHUS_TEST_APPROX(passed, l_t.direction().squaredNorm(),
l.direction().squaredNorm(), kSmallEps,
"Transform line case (direction) : %", i);
}
}
return passed;
}
bool lieBracketTest() {
bool passed = true;
for (size_t i = 0; i < tangent_vec_.size(); ++i) {
for (size_t j = 0; j < tangent_vec_.size(); ++j) {
Tangent tangent1 =
LieGroup::lieBracket(tangent_vec_[i], tangent_vec_[j]);
Transformation hati = LieGroup::hat(tangent_vec_[i]);
Transformation hatj = LieGroup::hat(tangent_vec_[j]);
Tangent tangent2 = LieGroup::vee(hati * hatj - hatj * hati);
SOPHUS_TEST_APPROX(passed, tangent1, tangent2, kSmallEps,
"Lie Bracket case: %", i);
}
}
return passed;
}
bool veeHatTest() {
bool passed = true;
for (size_t i = 0; i < tangent_vec_.size(); ++i) {
SOPHUS_TEST_APPROX(passed, Tangent(tangent_vec_[i]),
LieGroup::vee(LieGroup::hat(tangent_vec_[i])),
kSmallEps, "Hat-vee case: %", i);
}
return passed;
}
bool newDeleteSmokeTest() {
bool passed = true;
LieGroup* raw_ptr = nullptr;
raw_ptr = new LieGroup();
SOPHUS_TEST_NEQ(passed, reinterpret_cast<std::uintptr_t>(raw_ptr), 0);
delete raw_ptr;
return passed;
}
bool interpolateAndMeanTest() {
bool passed = true;
using std::sqrt;
Scalar const eps = Constants<Scalar>::epsilon();
Scalar const sqrt_eps = sqrt(eps);
// TODO: Improve accuracy of ``interpolate`` (and hence ``exp`` and ``log``)
// so that we can use more accurate bounds in these tests, i.e.
// ``eps`` instead of ``sqrt_eps``.
for (LieGroup const& foo_T_bar : group_vec_) {
for (LieGroup const& foo_T_baz : group_vec_) {
// Test boundary conditions ``alpha=0`` and ``alpha=1``.
LieGroup foo_T_quiz = interpolate(foo_T_bar, foo_T_baz, Scalar(0));
SOPHUS_TEST_APPROX(passed, foo_T_quiz.matrix(), foo_T_bar.matrix(),
sqrt_eps);
foo_T_quiz = interpolate(foo_T_bar, foo_T_baz, Scalar(1));
SOPHUS_TEST_APPROX(passed, foo_T_quiz.matrix(), foo_T_baz.matrix(),
sqrt_eps);
}
}
for (Scalar alpha :
{Scalar(0.1), Scalar(0.5), Scalar(0.75), Scalar(0.99)}) {
for (LieGroup const& foo_T_bar : group_vec_) {
for (LieGroup const& foo_T_baz : group_vec_) {
LieGroup foo_T_quiz = interpolate(foo_T_bar, foo_T_baz, alpha);
// test left-invariance:
//
// dash_T_foo * interp(foo_T_bar, foo_T_baz)
// == interp(dash_T_foo * foo_T_bar, dash_T_foo * foo_T_baz)
if (interp_details::Traits<LieGroup>::hasShortestPathAmbiguity(
foo_T_bar.inverse() * foo_T_baz)) {
// skip check since there is a shortest path ambiguity
continue;
}
for (LieGroup const& dash_T_foo : group_vec_) {
LieGroup dash_T_quiz = interpolate(dash_T_foo * foo_T_bar,
dash_T_foo * foo_T_baz, alpha);
SOPHUS_TEST_APPROX(passed, dash_T_quiz.matrix(),
(dash_T_foo * foo_T_quiz).matrix(), sqrt_eps);
}
// test inverse-invariance:
//
// interp(foo_T_bar, foo_T_baz).inverse()
// == interp(foo_T_bar.inverse(), dash_T_foo.inverse())
LieGroup quiz_T_foo =
interpolate(foo_T_bar.inverse(), foo_T_baz.inverse(), alpha);
SOPHUS_TEST_APPROX(passed, quiz_T_foo.inverse().matrix(),
foo_T_quiz.matrix(), sqrt_eps);
}
}
for (LieGroup const& bar_T_foo : group_vec_) {
for (LieGroup const& baz_T_foo : group_vec_) {
LieGroup quiz_T_foo = interpolate(bar_T_foo, baz_T_foo, alpha);
// test right-invariance:
//
// interp(bar_T_foo, bar_T_foo) * foo_T_dash
// == interp(bar_T_foo * foo_T_dash, bar_T_foo * foo_T_dash)
if (interp_details::Traits<LieGroup>::hasShortestPathAmbiguity(
bar_T_foo * baz_T_foo.inverse())) {
// skip check since there is a shortest path ambiguity
continue;
}
for (LieGroup const& foo_T_dash : group_vec_) {
LieGroup quiz_T_dash = interpolate(bar_T_foo * foo_T_dash,
baz_T_foo * foo_T_dash, alpha);
SOPHUS_TEST_APPROX(passed, quiz_T_dash.matrix(),
(quiz_T_foo * foo_T_dash).matrix(), sqrt_eps);
}
}
}
}
for (LieGroup const& foo_T_bar : group_vec_) {
for (LieGroup const& foo_T_baz : group_vec_) {
if (interp_details::Traits<LieGroup>::hasShortestPathAmbiguity(
foo_T_bar.inverse() * foo_T_baz)) {
// skip check since there is a shortest path ambiguity
continue;
}
// test average({A, B}) == interp(A, B):
LieGroup foo_T_quiz = interpolate(foo_T_bar, foo_T_baz, 0.5);
optional<LieGroup> foo_T_iaverage = iterativeMean(
std::array<LieGroup, 2>({{foo_T_bar, foo_T_baz}}), 20);
optional<LieGroup> foo_T_average =
average(std::array<LieGroup, 2>({{foo_T_bar, foo_T_baz}}));
SOPHUS_TEST(passed, bool(foo_T_average),
"log(foo_T_bar): %\nlog(foo_T_baz): %",
transpose(foo_T_bar.log()), transpose(foo_T_baz.log()));
if (foo_T_average) {
SOPHUS_TEST_APPROX(
passed, foo_T_quiz.matrix(), foo_T_average->matrix(), sqrt_eps,
"log(foo_T_bar): %\nlog(foo_T_baz): %\n"
"log(interp): %\nlog(average): %",
transpose(foo_T_bar.log()), transpose(foo_T_baz.log()),
transpose(foo_T_quiz.log()), transpose(foo_T_average->log()));
}
SOPHUS_TEST(passed, bool(foo_T_iaverage),
"log(foo_T_bar): %\nlog(foo_T_baz): %\n"
"log(interp): %\nlog(iaverage): %",
transpose(foo_T_bar.log()), transpose(foo_T_baz.log()),
transpose(foo_T_quiz.log()),
transpose(foo_T_iaverage->log()));
if (foo_T_iaverage) {
SOPHUS_TEST_APPROX(
passed, foo_T_quiz.matrix(), foo_T_iaverage->matrix(), sqrt_eps,
"log(foo_T_bar): %\nlog(foo_T_baz): %",
transpose(foo_T_bar.log()), transpose(foo_T_baz.log()));
}
}
}
return passed;
}
bool testRandomSmoke() {
bool passed = true;
std::default_random_engine engine;
for (int i = 0; i < 100; ++i) {
LieGroup g = LieGroup::sampleUniform(engine);
SOPHUS_TEST_EQUAL(passed, g.params(), g.params());
}
return passed;
}
template <class S = Scalar>
enable_if_t<std::is_floating_point<S>::value, bool> doAllTestsPass() {
return doesLargeTestSetPass();
}
template <class S = Scalar>
enable_if_t<!std::is_floating_point<S>::value, bool> doAllTestsPass() {
return doesSmallTestSetPass();
}
private:
bool doesSmallTestSetPass() {
bool passed = true;
passed &= adjointTest();
passed &= contructorAndAssignmentTest();
passed &= productTest();
passed &= expLogTest();
passed &= groupActionTest();
passed &= lineActionTest();
passed &= lieBracketTest();
passed &= veeHatTest();
passed &= newDeleteSmokeTest();
return passed;
}
bool doesLargeTestSetPass() {
bool passed = true;
passed &= doesSmallTestSetPass();
passed &= additionalDerivativeTest();
passed &= derivativeTest();
passed &= expMapTest();
passed &= interpolateAndMeanTest();
passed &= testRandomSmoke();
return passed;
}
Scalar const kSmallEps = Constants<Scalar>::epsilon();
Scalar const kSmallEpsSqrt = Constants<Scalar>::epsilonSqrt();
Eigen::Matrix<Scalar, N - 1, 1> map(
Eigen::Matrix<Scalar, N, N> const& T,
Eigen::Matrix<Scalar, N - 1, 1> const& p) {
return T.template topLeftCorner<N - 1, N - 1>() * p +
T.template topRightCorner<N - 1, 1>();
}
Eigen::Matrix<Scalar, N, 1> map(Eigen::Matrix<Scalar, N, N> const& T,
Eigen::Matrix<Scalar, N, 1> const& p) {
return T * p;
}
std::vector<LieGroup, Eigen::aligned_allocator<LieGroup>> group_vec_;
std::vector<Tangent, Eigen::aligned_allocator<Tangent>> tangent_vec_;
std::vector<Point, Eigen::aligned_allocator<Point>> point_vec_;
};
template <class Scalar>
std::vector<SE3<Scalar>, Eigen::aligned_allocator<SE3<Scalar>>> getTestSE3s() {
Scalar const kPi = Constants<Scalar>::pi();
std::vector<SE3<Scalar>, Eigen::aligned_allocator<SE3<Scalar>>> se3_vec;
se3_vec.push_back(SE3<Scalar>(
SO3<Scalar>::exp(Vector3<Scalar>(Scalar(0.2), Scalar(0.5), Scalar(0.0))),
Vector3<Scalar>(Scalar(0), Scalar(0), Scalar(0))));
se3_vec.push_back(SE3<Scalar>(
SO3<Scalar>::exp(Vector3<Scalar>(Scalar(0.2), Scalar(0.5), Scalar(-1.0))),
Vector3<Scalar>(Scalar(10), Scalar(0), Scalar(0))));
se3_vec.push_back(
SE3<Scalar>::trans(Vector3<Scalar>(Scalar(0), Scalar(100), Scalar(5))));
se3_vec.push_back(SE3<Scalar>::rotZ(Scalar(0.00001)));
se3_vec.push_back(
SE3<Scalar>::trans(Scalar(0), Scalar(-0.00000001), Scalar(0.0000000001)) *
SE3<Scalar>::rotZ(Scalar(0.00001)));
se3_vec.push_back(SE3<Scalar>::transX(Scalar(0.01)) *
SE3<Scalar>::rotZ(Scalar(0.00001)));
se3_vec.push_back(SE3<Scalar>::trans(Scalar(4), Scalar(-5), Scalar(0)) *
SE3<Scalar>::rotX(kPi));
se3_vec.push_back(
SE3<Scalar>(SO3<Scalar>::exp(
Vector3<Scalar>(Scalar(0.2), Scalar(0.5), Scalar(0.0))),
Vector3<Scalar>(Scalar(0), Scalar(0), Scalar(0))) *
SE3<Scalar>::rotX(kPi) *
SE3<Scalar>(SO3<Scalar>::exp(Vector3<Scalar>(Scalar(-0.2), Scalar(-0.5),
Scalar(-0.0))),
Vector3<Scalar>(Scalar(0), Scalar(0), Scalar(0))));
se3_vec.push_back(
SE3<Scalar>(SO3<Scalar>::exp(
Vector3<Scalar>(Scalar(0.3), Scalar(0.5), Scalar(0.1))),
Vector3<Scalar>(Scalar(2), Scalar(0), Scalar(-7))) *
SE3<Scalar>::rotX(kPi) *
SE3<Scalar>(SO3<Scalar>::exp(Vector3<Scalar>(Scalar(-0.3), Scalar(-0.5),
Scalar(-0.1))),
Vector3<Scalar>(Scalar(0), Scalar(6), Scalar(0))));
return se3_vec;
}
template <class T>
std::vector<SE2<T>, Eigen::aligned_allocator<SE2<T>>> getTestSE2s() {
T const kPi = Constants<T>::pi();
std::vector<SE2<T>, Eigen::aligned_allocator<SE2<T>>> se2_vec;
se2_vec.push_back(SE2<T>());
se2_vec.push_back(SE2<T>(SO2<T>(0.2), Vector2<T>(10, 0)));
se2_vec.push_back(SE2<T>::transY(100));
se2_vec.push_back(SE2<T>::trans(Vector2<T>(1, 2)));
se2_vec.push_back(SE2<T>(SO2<T>(-1.), Vector2<T>(20, -1)));
se2_vec.push_back(
SE2<T>(SO2<T>(0.00001), Vector2<T>(-0.00000001, 0.0000000001)));
se2_vec.push_back(SE2<T>(SO2<T>(0.3), Vector2<T>(2, 0)) * SE2<T>::rot(kPi) *
SE2<T>(SO2<T>(-0.3), Vector2<T>(0, 6)));
return se2_vec;
}
} // namespace Sophus
#endif // TESTS_HPP
Reference in New Issue View Git Blame Copy Permalink