Skip to content

Commit

Permalink
Dx_exp_x_times_point_at_0 (#371)
Browse files Browse the repository at this point in the history
  • Loading branch information
@strasdat
strasdat committed Aug 14, 2022
1 parent 8530978 commit 0422cad
Show file tree
Hide file tree
Showing 10 changed files with 125 additions and 4 deletions.
14 changes: 13 additions & 1 deletion sophus/cartesian.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -61,7 +61,7 @@ namespace Sophus {
/// with identity rotation, and hence represents pure translation.
///
/// The purpose of this class is two-fold:
/// - for educational purpose, to highlight how Lie groups genereliza over
/// - for educational purpose, to highlight how Lie groups generalize over
/// Euclidean vector spaces.
/// - to be used in templated/generic algorithms (such as Sophus::Spline)
/// which are implemented against the Lie group interface.
Expand All @@ -84,6 +84,8 @@ class CartesianBase {
static int constexpr num_parameters = M;
/// Group transformations are (M+1)x(M+1) matrices.
static int constexpr N = M + 1;
static int constexpr Dim = M;

using Transformation = Sophus::Matrix<Scalar, N, N>;
using Point = Sophus::Vector<Scalar, M>;
using HomogeneousPoint = Sophus::Vector<Scalar, N>;
Expand Down Expand Up @@ -256,6 +258,7 @@ class Cartesian : public CartesianBase<Cartesian<Scalar_, M, Options>, M> {
static int constexpr DoF = Base::DoF;
static int constexpr num_parameters = Base::num_parameters;
static int constexpr N = Base::N;
static int constexpr Dim = Base::Dim;

using Scalar = Scalar_;
using Transformation = typename Base::Transformation;
Expand Down Expand Up @@ -330,6 +333,15 @@ class Cartesian : public CartesianBase<Cartesian<Scalar_, M, Options>, M> {
return Dx_exp_x_at_0();
}

/// Returns derivative of exp(x) * p wrt. x_i at x=0.
///
SOPHUS_FUNC static Sophus::Matrix<Scalar, Dim, DoF> Dx_exp_x_times_point_at_0(
Point const&) {
Sophus::Matrix<Scalar, Dim, DoF> J;
J.setIdentity();
return J;
}

/// Returns derivative of exp(x).matrix() wrt. ``x_i at x=0``.
///
SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int i) {
Expand Down
12 changes: 12 additions & 0 deletions sophus/rxso2.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -90,6 +90,8 @@ class RxSO2Base {
static int constexpr num_parameters = 2;
/// Group transformations are 2x2 matrices.
static int constexpr N = 2;
/// Points are 2-dimensional
static int constexpr Dim = 2;
using Transformation = Matrix<Scalar, N, N>;
using Point = Vector2<Scalar>;
using HomogeneousPoint = Vector3<Scalar>;
Expand Down Expand Up @@ -543,11 +545,21 @@ class RxSO2 : public RxSO2Base<RxSO2<Scalar_, Options>> {
return J;
}

/// Returns derivative of exp(x) * p wrt. x_i at x=0.
///
SOPHUS_FUNC static Sophus::Matrix<Scalar, 2, DoF> Dx_exp_x_times_point_at_0(
Point const& point) {
Sophus::Matrix<Scalar, 2, DoF> j;
j << Sophus::SO2<Scalar>::Dx_exp_x_times_point_at_0(point), point;
return j;
}

/// Returns derivative of exp(x).matrix() wrt. ``x_i at x=0``.
///
SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int i) {
return generator(i);
}

/// Group exponential
///
/// This functions takes in an element of tangent space (= rotation angle
Expand Down
11 changes: 11 additions & 0 deletions sophus/rxso3.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -82,6 +82,8 @@ class RxSO3Base {
static int constexpr num_parameters = 4;
/// Group transformations are 3x3 matrices.
static int constexpr N = 3;
/// Points are 3-dimensional
static int constexpr Dim = 3;
using Transformation = Matrix<Scalar, N, N>;
using Point = Vector3<Scalar>;
using HomogeneousPoint = Vector4<Scalar>;
Expand Down Expand Up @@ -621,6 +623,15 @@ class RxSO3 : public RxSO3Base<RxSO3<Scalar_, Options>> {
return J;
}

/// Returns derivative of exp(x) * p wrt. x_i at x=0.
///
SOPHUS_FUNC static Sophus::Matrix<Scalar, 3, DoF> Dx_exp_x_times_point_at_0(
Point const& point) {
Sophus::Matrix<Scalar, 3, DoF> j;
j << Sophus::SO3<Scalar>::hat(-point), point;
return j;
}

/// Returns derivative of exp(x).matrix() wrt. ``x_i at x=0``.
///
SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int i) {
Expand Down
12 changes: 12 additions & 0 deletions sophus/se2.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -69,6 +69,8 @@ class SE2Base {
static int constexpr num_parameters = 4;
/// Group transformations are 3x3 matrices.
static int constexpr N = 3;
/// Points are 2-dimensional
static int constexpr Dim = 2;
using Transformation = Matrix<Scalar, N, N>;
using Point = Vector2<Scalar>;
using HomogeneousPoint = Vector3<Scalar>;
Expand Down Expand Up @@ -576,6 +578,16 @@ class SE2 : public SE2Base<SE2<Scalar_, Options>> {
return J;
}

/// Returns derivative of exp(x) * p wrt. x_i at x=0.
///
SOPHUS_FUNC static Sophus::Matrix<Scalar, 2, DoF> Dx_exp_x_times_point_at_0(
Point const& point) {
Sophus::Matrix<Scalar, 2, DoF> J;
J << Sophus::Matrix2<Scalar>::Identity(),
Sophus::SO2<Scalar>::Dx_exp_x_times_point_at_0(point);
return J;
}

/// Returns derivative of exp(x).matrix() wrt. ``x_i at x=0``.
///
SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int i) {
Expand Down
12 changes: 12 additions & 0 deletions sophus/se3.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -69,6 +69,8 @@ class SE3Base {
static int constexpr num_parameters = 7;
/// Group transformations are 4x4 matrices.
static int constexpr N = 4;
/// Points are 3-dimensional
static int constexpr Dim = 3;
using Transformation = Matrix<Scalar, N, N>;
using Point = Vector3<Scalar>;
using HomogeneousPoint = Vector4<Scalar>;
Expand Down Expand Up @@ -823,6 +825,16 @@ class SE3 : public SE3Base<SE3<Scalar_, Options>> {
return generator(i);
}

/// Returns derivative of exp(x) * p wrt. x_i at x=0.
///
SOPHUS_FUNC static Sophus::Matrix<Scalar, 3, DoF> Dx_exp_x_times_point_at_0(
Point const& point) {
Sophus::Matrix<Scalar, 3, DoF> J;
J << Sophus::Matrix3<Scalar>::Identity(),
Sophus::SO3<Scalar>::Dx_exp_x_times_point_at_0(point);
return J;
}

/// Group exponential
///
/// This functions takes in an element of tangent space (= twist ``a``) and
Expand Down
12 changes: 12 additions & 0 deletions sophus/sim2.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -70,6 +70,8 @@ class Sim2Base {
static int constexpr num_parameters = 4;
/// Group transformations are 3x3 matrices.
static int constexpr N = 3;
/// Points are 2-dimensional
static int constexpr Dim = 2;
using Transformation = Matrix<Scalar, N, N>;
using Point = Vector2<Scalar>;
using HomogeneousPoint = Vector3<Scalar>;
Expand Down Expand Up @@ -548,6 +550,16 @@ class Sim2 : public Sim2Base<Sim2<Scalar_, Options>> {
return J;
}

/// Returns derivative of exp(x) * p wrt. x_i at x=0.
///
SOPHUS_FUNC static Sophus::Matrix<Scalar, 2, DoF> Dx_exp_x_times_point_at_0(
Point const& point) {
Sophus::Matrix<Scalar, 2, DoF> J;
J << Sophus::Matrix2<Scalar>::Identity(),
Sophus::RxSO2<Scalar>::Dx_exp_x_times_point_at_0(point);
return J;
}

/// Returns derivative of exp(x).matrix() wrt. ``x_i at x=0``.
///
SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int i) {
Expand Down
12 changes: 12 additions & 0 deletions sophus/sim3.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -71,6 +71,8 @@ class Sim3Base {
static int constexpr num_parameters = 7;
/// Group transformations are 4x4 matrices.
static int constexpr N = 4;
/// Points are 3-dimensional
static int constexpr Dim = 3;
using Transformation = Matrix<Scalar, N, N>;
using Point = Vector3<Scalar>;
using HomogeneousPoint = Vector4<Scalar>;
Expand Down Expand Up @@ -576,6 +578,16 @@ class Sim3 : public Sim3Base<Sim3<Scalar_, Options>> {
return J;
}

/// Returns derivative of exp(x) * p wrt. x_i at x=0.
///
SOPHUS_FUNC static Sophus::Matrix<Scalar, 3, DoF> Dx_exp_x_times_point_at_0(
Point const& point) {
Sophus::Matrix<Scalar, 3, DoF> J;
J << Sophus::Matrix3<Scalar>::Identity(),
Sophus::RxSO3<Scalar>::Dx_exp_x_times_point_at_0(point);
return J;
}

/// Returns derivative of exp(x).matrix() wrt. ``x_i at x=0``.
///
SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int i) {
Expand Down
9 changes: 9 additions & 0 deletions sophus/so2.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -89,6 +89,8 @@ class SO2Base {
static int constexpr num_parameters = 2;
/// Group transformations are 2x2 matrices.
static int constexpr N = 2;
/// Points are 3-dimensional
static int constexpr Dim = 2;
using Transformation = Matrix<Scalar, N, N>;
using Point = Vector2<Scalar>;
using HomogeneousPoint = Vector3<Scalar>;
Expand Down Expand Up @@ -469,6 +471,13 @@ class SO2 : public SO2Base<SO2<Scalar_, Options>> {
return Sophus::Matrix<Scalar, num_parameters, DoF>(Scalar(0), Scalar(1));
}

/// Returns derivative of exp(x) * p wrt. x_i at x=0.
///
SOPHUS_FUNC static Sophus::Matrix<Scalar, 2, DoF> Dx_exp_x_times_point_at_0(
Point const& point) {
return Point(-point.y(), point.x());
}

/// Returns derivative of exp(x).matrix() wrt. ``x_i at x=0``.
///
SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int) {
Expand Down
10 changes: 10 additions & 0 deletions sophus/so3.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -87,6 +87,9 @@ class SO3Base {
static int constexpr num_parameters = 4;
/// Group transformations are 3x3 matrices.
static int constexpr N = 3;
/// Points are 3-dimensional
static int constexpr Dim = 3;

using Transformation = Matrix<Scalar, N, N>;
using Point = Vector3<Scalar>;
using HomogeneousPoint = Vector4<Scalar>;
Expand Down Expand Up @@ -656,6 +659,13 @@ class SO3 : public SO3Base<SO3<Scalar_, Options>> {
return J;
}

/// Returns derivative of exp(x) * p wrt. x_i at x=0.
///
SOPHUS_FUNC static Sophus::Matrix<Scalar, 3, DoF> Dx_exp_x_times_point_at_0(
Point const& point) {
return hat(-point);
}

/// Returns derivative of exp(x).matrix() wrt. ``x_i at x=0``.
///
SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int i) {
Expand Down
25 changes: 22 additions & 3 deletions test/core/tests.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,6 +48,7 @@ class LieGroupTests {
using Line = typename LieGroup::Line;
using Hyperplane = typename LieGroup::Hyperplane;
using Adjoint = typename LieGroup::Adjoint;
static int constexpr Dim = LieGroup::Dim;
static int constexpr N = LieGroup::N;
static int constexpr DoF = LieGroup::DoF;
static int constexpr num_parameters = LieGroup::num_parameters;
Expand Down Expand Up @@ -88,7 +89,7 @@ class LieGroupTests {
enable_if_t<std::is_same<G, SO3<Scalar>>::value ||
std::is_same<G, SE3<Scalar>>::value,
bool>
jacobianTest() {
leftJacobianTest() {
bool passed = true;
for (const auto& x : tangent_vec_) {
LieGroup const inv_exp_x = LieGroup::exp(x).inverse();
Expand Down Expand Up @@ -121,10 +122,27 @@ class LieGroupTests {
enable_if_t<!(std::is_same<G, SO3<Scalar>>::value ||
std::is_same<G, SE3<Scalar>>::value),
bool>
jacobianTest() {
leftJacobianTest() {
return true;
}

bool moreJacobiansTest() {
bool passed = true;
for (auto const& point : point_vec_) {
Matrix<Scalar, Dim, DoF> J = LieGroup::Dx_exp_x_times_point_at_0(point);
Tangent t;
setToZero(t);
Matrix<Scalar, Dim, DoF> const J_num =
vectorFieldNumDiff<Scalar, Dim, DoF>(
[point](Tangent const& x) { return LieGroup::exp(x) * point; },
t);

SOPHUS_TEST_APPROX(passed, J, J_num, kSmallEpsSqrt,
"Dx_exp_x_times_point_at_0");
}
return passed;
}

bool contructorAndAssignmentTest() {
bool passed = true;
for (LieGroup foo_T_bar : group_vec_) {
Expand Down Expand Up @@ -648,7 +666,8 @@ class LieGroupTests {
passed &= interpolateAndMeanTest();
passed &= testRandomSmoke();
passed &= testSpline();
passed &= jacobianTest();
passed &= leftJacobianTest();
passed &= moreJacobiansTest();
return passed;
}

Expand Down

0 comments on commit 0422cad

Please sign in to comment.