diff --git a/README.md b/README.md index fd797d1e..bf849ade 100644 --- a/README.md +++ b/README.md @@ -26,6 +26,9 @@ At the moment, it provides the groups: introduced (to the best of knowledge) in this [paper][barrau15]. NOTE: The implementation here differs slightly from the developments in the [paper][barrau15]. +- Bundle<>: allows manipulating a manifold bundle as a single Lie group. + Referred to as a *composite manifold* in Section IV of the + [reference paper](http://arxiv.org/abs/1812.01537). Other Lie groups can and will be added, contributions are welcome. diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 5565867f..5a707e75 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -13,6 +13,8 @@ add_executable(se3_sam_selfcalib se3_sam_selfcalib.cpp) add_executable(se_2_3_localization se_2_3_localization.cpp) +add_executable(bundle_sam bundle_sam.cpp) + set(CXX_11_EXAMPLE_TARGETS # SO2 @@ -34,6 +36,9 @@ set(CXX_11_EXAMPLE_TARGETS # SE_2_3 se_2_3_localization + + # Bundle + bundle_sam ) # Link to manif diff --git a/examples/bundle_sam.cpp b/examples/bundle_sam.cpp new file mode 100644 index 00000000..84ea7f23 --- /dev/null +++ b/examples/bundle_sam.cpp @@ -0,0 +1,386 @@ +/** + * \file bundle_sam.cpp + * + * Created on: Feb 28, 2021 + * \author: pettni + * + * Adopted from se2_sam.cpp, see that file for explanations. + */ + + +// manif +#include "manif/Rn.h" +#include "manif/SE2.h" +#include "manif/Bundle.h" + +// Std +#include +#include +#include +#include + +// Debug +#include +#include + +// std shortcuts and namespaces +using std::cout; +using std::endl; +using std::vector; +using std::map; +using std::list; +using std::pair; + +// Eigen namespace +using namespace Eigen; + +// manif namespace and shortcuts +using manif::SE2d; +using manif::SE2Tangentd; + +static constexpr int DoF = SE2d::DoF; +static constexpr int Dim = SE2d::Dim; + +// Define many data types (Tangent refers to the tangent of SE2) +typedef Array ArrayT; // tangent-size array +typedef Matrix VectorT; // tangent-size vector +typedef Matrix MatrixT; // tangent-size square matrix +typedef Matrix VectorB; // landmark-size vector +typedef Array ArrayY; // measurement-size array +typedef Matrix VectorY; // measurement-size vector +typedef Matrix MatrixY; // measurement-size square matrix +typedef Matrix MatrixYT; // measurement x tangent size matrix +typedef Matrix MatrixYB; // measurement x landmark size matrix + +// some experiment constants +static const int NUM_POSES = 3; +static const int NUM_LMKS = 5; +static const int NUM_FACTORS = 9; +static const int NUM_MEAS = NUM_POSES * DoF + NUM_FACTORS * Dim; +static const int MAX_ITER = 20; // for the solver + + +// bundle state type to optimize over +using BundleT = manif::Bundle; + +// Insert a relative pose factor from pose XI to pose XJ +// into the residual-jacobian pair (r, J) +template +void add_pose_factor( + const BundleT & X, + const SE2Tangentd & control, + const MatrixT & W, + Eigen::Ref> r, + Eigen::Ref> J) +{ + // index start position and length in the DoF of BundleT + constexpr int BegI = manif::internal::intseq_element::value; + constexpr int LenI = manif::internal::intseq_element::value; + constexpr int BegJ = manif::internal::intseq_element::value; + constexpr int LenJ = manif::internal::intseq_element::value; + + MatrixT J_d_xi, J_d_xj; // Jacobian of motion wrt poses i and j + + auto d = X.element().rminus(X.element(), J_d_xj, J_d_xi); + r = W * (d - control).coeffs(); + J.setZero(); + J.block<3, LenI>(0, BegI) = W * J_d_xi; + J.block<3, LenJ>(0, BegJ) = W * J_d_xj; +} + + +// Insert a landmark measurement factor of landmark LK +// from pose XI into the residual-jacobian pair (r, J) +template +void add_beacon_factor( + const BundleT & X, + const VectorY & measurement, + const MatrixY & S, + Eigen::Ref> r, + Eigen::Ref> J) +{ + // index start position and length in the DoF of BundleT + constexpr int BegX = manif::internal::intseq_element::value; + constexpr int LenX = manif::internal::intseq_element::value; + constexpr int BegLMK = manif::internal::intseq_element::value; + constexpr int LenLMK = manif::internal::intseq_element::value; + + MatrixT J_ix_x; // Jacobian of inverse pose wrt pose + MatrixYT J_e_ix; // Jacobian of measurement expectation wrt inverse pose + MatrixYT J_e_x; // Jacobian of measurement expectation wrt pose + MatrixYB J_e_b; // Jacobian of measurement expectation wrt lmk + + auto e = X.element().inverse(J_ix_x).act(X.element().coeffs(), J_e_ix, J_e_b); + J_e_x = J_e_ix * J_ix_x; + r = S * (e - measurement); + J.setZero(); + J.block(0, BegX) = S * J_e_x; + J.block(0, BegLMK) = S * J_e_b; +} + + +int main() +{ + std::srand((unsigned int) time(0)); + + // DEBUG INFO + cout << endl; + cout << "2D Smoothing and Mapping. 3 poses, 5 landmarks." << endl; + cout << "-----------------------------------------------" << endl; + cout << std::fixed << std::setprecision(3) << std::showpos; + + // START CONFIGURATION + // + // + + // Define the robot pose elements + SE2d X_simu, // pose of the simulated robot + Xi, // robot pose at time i + Xj; // robot pose at time j + vector poses, // estimator poses + poses_simu;// simulator poses + Xi.setIdentity(); + X_simu.setIdentity(); + + + // Define a control vector and its noise and covariance in the tangent of SE2 + SE2Tangentd u; // control signal, generic + SE2Tangentd u_nom; // nominal control signal + ArrayT u_sigmas; // control noise std specification + VectorT u_noise; // control noise + MatrixT Q; // Covariance + MatrixT W; // sqrt Info + vector controls; // robot controls + + u_nom << 0.1, 0.0, 0.05; + u_sigmas << 0.01, 0.01, 0.01; + Q = (u_sigmas * u_sigmas).matrix().asDiagonal(); + W = u_sigmas.inverse() .matrix().asDiagonal(); // this is Q^(-T/2) + + // Landmarks in R^2 and map + VectorB b; // Landmark, generic + vector landmarks(NUM_LMKS), landmarks_simu; + { + // Define five landmarks (beacons) in R^2 + VectorB b0, b1, b2, b3, b4; + b0 << 3.0, 0.0; + b1 << 2.0, -1.0; + b2 << 2.0, 1.0; + b3 << 3.0, -1.0; + b4 << 3.0, 1.0; + landmarks_simu.push_back(b0); + landmarks_simu.push_back(b1); + landmarks_simu.push_back(b2); + landmarks_simu.push_back(b3); + landmarks_simu.push_back(b4); + } // destroy b0...b4 + + // Define the beacon's measurements in R^2 + VectorY y, y_noise; + ArrayY y_sigmas; + MatrixY R; // Covariance + MatrixY S; // sqrt Info + vector> measurements(NUM_POSES); // y = measurements[pose_id][lmk_id] + + y_sigmas << 0.001, 0.001; + R = (y_sigmas * y_sigmas).matrix().asDiagonal(); + S = y_sigmas.inverse() .matrix().asDiagonal(); // this is R^(-T/2) + + // Problem-size variables + + /* + * The factor graph of the SAM problem looks like this: + * + * ------- b1 + * b3 / | + * | / b4 | + * | / / \| + * X0 ---- X1 ---- X2 + * | \ / \ / + * | b0 b2 + * * + * + * where: + * - Xi are poses + * - bk are landmarks or beacons + * - * is a pose prior to anchor the map and make the problem observable + * + * Define pairs of nodes for all the landmark measurements + * There are 3 pose nodes [0..2] and 5 landmarks [0..4]. + * A pair pose -- lmk means that the lmk was measured from the pose + * Each pair declares a factor in the factor graph + * We declare 9 pairs, or 9 factors, as follows: + */ + vector> pairs(NUM_POSES); + pairs[0].push_back(0); // 0-0 + pairs[0].push_back(1); // 0-1 + pairs[0].push_back(3); // 0-3 + pairs[1].push_back(0); // 1-0 + pairs[1].push_back(2); // 1-2 + pairs[1].push_back(4); // 1-4 + pairs[2].push_back(1); // 2-1 + pairs[2].push_back(2); // 2-2 + pairs[2].push_back(4); // 2-4 + + // + // + // END CONFIGURATION + + + //// Simulator ################################################################### + poses_simu. push_back(X_simu); + poses. push_back(Xi + SE2Tangentd::Random()); // use very noisy priors + + // temporal loop + for (int i = 0; i < NUM_POSES; ++i) + { + // make measurements + for (const auto& k : pairs[i]) + { + // simulate measurement + b = landmarks_simu[k]; // lmk coordinates in world frame + y_noise = y_sigmas * ArrayY::Random(); // measurement noise + y = X_simu.inverse().act(b); // landmark measurement, before adding noise + + // add noise and compute prior lmk from prior pose + measurements[i][k] = y + y_noise; // store noisy measurements + b = Xi.act(y + y_noise); // mapped landmark with noise + landmarks[k] = b + VectorB::Random(); // use very noisy priors + } + + // make motions + if (i < NUM_POSES - 1) // do not make the last motion since we're done after 3rd pose + { + // move simulator, without noise + X_simu = X_simu + u_nom; + + // move prior, with noise + u_noise = u_sigmas * ArrayT::Random(); + Xi = Xi + (u_nom + u_noise); + + // store + poses_simu. push_back(X_simu); + poses. push_back(Xi + SE2Tangentd::Random()); // use very noisy priors + controls. push_back(u_nom + u_noise); + } + } + + //// Estimator ################################################################# + + // Insert priors into bundle state + BundleT X(poses[0], poses[1], poses[2], + manif::R2d(landmarks[0]), manif::R2d(landmarks[1]), + manif::R2d(landmarks[2]), manif::R2d(landmarks[3]), + manif::R2d(landmarks[4])); + + cout << "prior" << std::showpos << endl; + cout << "pose :" << X.element<0>().translation().transpose() << " " << X.element<0>().angle() << endl; + cout << "pose :" << X.element<1>().translation().transpose() << " " << X.element<1>().angle() << endl; + cout << "pose :" << X.element<2>().translation().transpose() << " " << X.element<2>().angle() << endl; + + cout << "lmk :" << X.element<3>() << endl; + cout << "lmk :" << X.element<4>() << endl; + cout << "lmk :" << X.element<5>() << endl; + cout << "lmk :" << X.element<6>() << endl; + cout << "lmk :" << X.element<7>() << endl; + cout << "-----------------------------------------------" << endl; + + // iterate + // DEBUG INFO + cout << "iterations" << std::noshowpos << endl; + for (int iteration = 0; iteration < MAX_ITER; ++iteration) + { + Matrix J; // full Jacobian + Matrix r; // full residual + + int row = 0; // keep track of row in J and r + + // first residual: prior + r.segment(row) = X.element<0>().lminus(SE2d::Identity(), J.block(row, 0)).coeffs(); + row += DoF; + + // motion residuals + add_pose_factor<0, 1>(X, controls[0], W, r.segment(row), J.block(row, 0)); + row += DoF; + + add_pose_factor<1, 2>(X, controls[1], W, r.segment(row), J.block(row, 0)); + row += DoF; + + // measurement residuals + add_beacon_factor<0, 0>(X, measurements[0][0], S, r.segment(row), J.block(row, 0)); + row += Dim; + add_beacon_factor<0, 1>(X, measurements[0][1], S, r.segment(row), J.block(row, 0)); + row += Dim; + add_beacon_factor<0, 3>(X, measurements[0][3], S, r.segment(row), J.block(row, 0)); + row += Dim; + + add_beacon_factor<1, 0>(X, measurements[1][0], S, r.segment(row), J.block(row, 0)); + row += Dim; + add_beacon_factor<1, 2>(X, measurements[1][2], S, r.segment(row), J.block(row, 0)); + row += Dim; + add_beacon_factor<1, 4>(X, measurements[1][4], S, r.segment(row), J.block(row, 0)); + row += Dim; + + add_beacon_factor<2, 1>(X, measurements[2][1], S, r.segment(row), J.block(row, 0)); + row += Dim; + add_beacon_factor<2, 2>(X, measurements[2][2], S, r.segment(row), J.block(row, 0)); + row += Dim; + add_beacon_factor<2, 4>(X, measurements[2][4], S, r.segment(row), J.block(row, 0)); + row += Dim; + + // 4. Solve ----------------------------------------------------------------- + + // compute optimal step + // ATTENTION: This is an expensive step!! + // ATTENTION: Use QR factorization and column reordering for larger problems!! + const auto dX = (-(J.transpose() * J).inverse() * J.transpose() * r).eval(); + + // update estimate + X += BundleT::Tangent(dX); + + // DEBUG INFO + cout << "residual norm: " << std::scientific << r.norm() << ", step norm: " << dX.norm() << endl; + + // conditional exit + if (dX.norm() < 1e-6) break; + } + cout << "-----------------------------------------------" << endl; + + + //// Print results #################################################################### + + cout << std::fixed; + + // solved problem + cout << "posterior" << std::showpos << endl; + cout << "pose :" << X.element<0>().translation().transpose() << " " << X.element<0>().angle() << endl; + cout << "pose :" << X.element<1>().translation().transpose() << " " << X.element<1>().angle() << endl; + cout << "pose :" << X.element<2>().translation().transpose() << " " << X.element<2>().angle() << endl; + + cout << "lmk :" << X.element<3>() << endl; + cout << "lmk :" << X.element<4>() << endl; + cout << "lmk :" << X.element<5>() << endl; + cout << "lmk :" << X.element<6>() << endl; + cout << "lmk :" << X.element<7>() << endl; + + cout << "-----------------------------------------------" << endl; + + // ground truth + cout << "ground truth" << std::showpos << endl; + for (const auto& X : poses_simu) + cout << "pose : " << X.translation().transpose() << " " << X.angle() << endl; + for (const auto& b : landmarks_simu) + cout << "lmk : " << b.transpose() << endl; + cout << "-----------------------------------------------" << endl; + + return 0; +} diff --git a/include/manif/Bundle.h b/include/manif/Bundle.h new file mode 100644 index 00000000..4d9a27bf --- /dev/null +++ b/include/manif/Bundle.h @@ -0,0 +1,16 @@ +#ifndef _MANIF_BUNDLE_H_ +#define _MANIF_BUNDLE_H_ + +#include "manif/impl/macro.h" +#include "manif/impl/utils.h" +#include "manif/impl/lie_group_base.h" +#include "manif/impl/tangent_base.h" + +#include "manif/impl/bundle/Bundle_base.h" +#include "manif/impl/bundle/Bundle_map.h" +#include "manif/impl/bundle/Bundle.h" +#include "manif/impl/bundle/BundleTangent_base.h" +#include "manif/impl/bundle/BundleTangent.h" +#include "manif/impl/bundle/BundleTangent_map.h" + +#endif // _MANIF_BUNDLE_H_ diff --git a/include/manif/Rn.h b/include/manif/Rn.h index 1affac10..b78fd8ca 100644 --- a/include/manif/Rn.h +++ b/include/manif/Rn.h @@ -6,7 +6,6 @@ #include "manif/impl/lie_group_base.h" #include "manif/impl/tangent_base.h" -#include "manif/impl/rn/Rn_properties.h" #include "manif/impl/rn/Rn_base.h" #include "manif/impl/rn/RnTangent_base.h" #include "manif/impl/rn/Rn.h" diff --git a/include/manif/impl/bundle/Bundle.h b/include/manif/impl/bundle/Bundle.h new file mode 100644 index 00000000..dc81d45e --- /dev/null +++ b/include/manif/impl/bundle/Bundle.h @@ -0,0 +1,190 @@ +#ifndef _MANIF_MANIF_BUNDLE_H_ +#define _MANIF_MANIF_BUNDLE_H_ + +#include "manif/impl/bundle/Bundle_base.h" +#include "manif/impl/traits.h" + +namespace manif { + +// Forward declare for type traits specialization + +template class ... _T> struct Bundle; +template class ... _T> struct BundleTangent; + +namespace internal { + +//! Traits specialization +template class ... _T> +struct traits> +{ + // Bundle-specific traits + using IdxList = typename make_intseq::type; + + using LenDim = intseq<_T<_Scalar>::Dim ...>; + using BegDim = intseq_psum_t; + + using LenDoF = intseq<_T<_Scalar>::DoF ...>; + using BegDoF = intseq_psum_t; + + using LenTra = intseq<_T<_Scalar>::Transformation::RowsAtCompileTime ...>; + using BegTra = intseq_psum_t; + + using LenRep = intseq<_T<_Scalar>::RepSize ...>; + using BegRep = intseq_psum_t; + + template + using ElementType = typename bundle_element<_Idx, _T<_Scalar>...>::type; + + // Regular traits + using Scalar = _Scalar; + + using LieGroup = Bundle<_Scalar, _T ...>; + using Tangent = BundleTangent<_Scalar, _T ...>; + + using Base = BundleBase>; + + static constexpr int Dim = intseq_sum::value; + static constexpr int DoF = intseq_sum::value; + static constexpr int RepSize = intseq_sum::value; + + using DataType = Eigen::Matrix<_Scalar, RepSize, 1>; + using Jacobian = Eigen::Matrix<_Scalar, DoF, DoF>; + using Transformation = Eigen::Matrix< + _Scalar, intseq_sum::value, intseq_sum::value + >; + using Vector = Eigen::Matrix<_Scalar, Dim, 1>; +}; + +} // namespace internal + +// +// Bundle LieGroup +// + +/** + * @brief Represents a Bundle (or Composite) element as + * described in Section IV of the reference paper + * (see also Example 7). + * + * A Bundle of Lie groups can be utilized as + * a single group with element-wise operations. This can be + * convenient when working with aggregate states that consist of + * multiple Lie group sub-states, like the example in Section VIIb + * of the reference paper. + * + * Example: create an element of the composite + * using double as the scalar type. + * + * > Bundle element; + */ +template class ... _T> +struct Bundle : BundleBase> +{ +private: + + static_assert(sizeof...(_T) > 0, "Must have at least one element in Bundle !"); + + using Base = BundleBase>; + using Type = Bundle<_Scalar, _T...>; + + using BegRep = typename internal::traits::BegRep; + using LenRep = typename internal::traits::LenRep; + +protected: + + using Base::derived; + +public: + + MANIF_MAKE_ALIGNED_OPERATOR_NEW_COND + + MANIF_COMPLETE_GROUP_TYPEDEF + MANIF_INHERIT_GROUP_API + + using Base::BundleSize; + + Bundle() = default; + ~Bundle() = default; + + MANIF_COPY_CONSTRUCTOR(Bundle); + MANIF_MOVE_CONSTRUCTOR(Bundle); + + // Copy constructor + template + Bundle(const LieGroupBase<_DerivedOther> & o); + + MANIF_GROUP_ASSIGN_OP(Bundle); + + // LieGroup common API + + /** + * @brief Get a reference to the underlying DataType. + * @param[out] a reference to the underlying Eigen vector + */ + DataType & coeffs(); + + /** + * @brief Get a const reference to the underlying DataType. + * @param[out] a const reference to the underlying Eigen vector + */ + const DataType & coeffs() const; + + + // Bundle specific API + + /** + * @brief Construct from Bundle elements + */ + Bundle(const _T<_Scalar> & ... elements); + +protected: + + // Helper for the elements constructor + template + Bundle(internal::intseq<_BegRep...>, internal::intseq<_LenRep...>, const _T<_Scalar> & ... elements); + +protected: + + //! Underlying data (Eigen) vector + DataType data_; +}; + + +template class ... _T> +template +Bundle<_Scalar, _T...>::Bundle(const LieGroupBase<_DerivedOther> & o) +: Bundle(o.coeffs()) +{} + +template class ... _T> +Bundle<_Scalar, _T...>::Bundle(const _T<_Scalar> & ... elements) +: Bundle(BegRep{}, LenRep{}, elements ...) +{} + +template class ... _T> +template +Bundle<_Scalar, _T...>::Bundle( + internal::intseq<_BegRep...>, internal::intseq<_LenRep...>, const _T<_Scalar> & ... elements) +{ + // c++11 "fold expression" + auto l = {((data_.template segment<_LenRep>(_BegRep) = elements.coeffs()), 0) ...}; + static_cast(l); // compiler warning +} + +template class ... _T> +typename Bundle<_Scalar, _T...>::DataType & +Bundle<_Scalar, _T...>::coeffs() +{ + return data_; +} + +template class ... _T> +const typename Bundle<_Scalar, _T...>::DataType & +Bundle<_Scalar, _T...>::coeffs() const +{ + return data_; +} + +} // namespace manif + +#endif // _MANIF_MANIF_BUNDLE_H_ diff --git a/include/manif/impl/bundle/BundleTangent.h b/include/manif/impl/bundle/BundleTangent.h new file mode 100644 index 00000000..58566dea --- /dev/null +++ b/include/manif/impl/bundle/BundleTangent.h @@ -0,0 +1,176 @@ +#ifndef _MANIF_MANIF_BUNDLETANGENT_H_ +#define _MANIF_MANIF_BUNDLETANGENT_H_ + +#include "manif/impl/bundle/BundleTangent_base.h" +#include "manif/impl/traits.h" + +namespace manif { + +// Forward declare for type traits specialization + +template class ... _T> struct Bundle; +template class ... _T> struct BundleTangent; + +namespace internal { + +//! Traits specialization +template class ... _T> +struct traits> +{ + // BundleTangent-specific traits + using IdxList = typename make_intseq::type; + + using LenDim = intseq<_T<_Scalar>::Tangent::Dim ...>; + using BegDim = intseq_psum_t; + + using LenDoF = intseq<_T<_Scalar>::Tangent::DoF ...>; + using BegDoF = intseq_psum_t; + + using LenRep = intseq<_T<_Scalar>::Tangent::RepSize ...>; + using BegRep = intseq_psum_t; + + using LenAlg = intseq<_T<_Scalar>::Tangent::LieAlg::RowsAtCompileTime ...>; + using BegAlg = intseq_psum_t; + + template + using ElementType = typename bundle_element<_Idx, _T<_Scalar>...>::type::Tangent; + + // Regular traits + using Scalar = _Scalar; + + using LieGroup = Bundle<_Scalar, _T...>; + using Tangent = BundleTangent<_Scalar, _T...>; + + using Base = BundleTangentBase; + + static constexpr int Dim = intseq_sum::value; + static constexpr int DoF = intseq_sum::value; + static constexpr int RepSize = intseq_sum::value; + + using DataType = Eigen::Matrix; + using Jacobian = Eigen::Matrix; + using LieAlg = Eigen::Matrix::value, + intseq_sum::value>; +}; + +} // namespace internal + +// +// BundleTangent +// + +/** + * @brief Represents a BundleTangent element. + */ +template class ... _T> +struct BundleTangent : BundleTangentBase> +{ +private: + + static_assert(sizeof...(_T) > 0, "Must have at least one element in BundleTangent !"); + + using Base = BundleTangentBase>; + using Type = BundleTangent<_Scalar, _T...>; + + using BegRep = typename internal::traits::BegRep; + using LenRep = typename internal::traits::LenRep; + +protected: + + using Base::derived; + +public: + + MANIF_MAKE_ALIGNED_OPERATOR_NEW_COND + + MANIF_TANGENT_TYPEDEF + MANIF_INHERIT_TANGENT_API + MANIF_INHERIT_TANGENT_OPERATOR + + using Base::BundleSize; + + BundleTangent() = default; + ~BundleTangent() = default; + + MANIF_COPY_CONSTRUCTOR(BundleTangent) + MANIF_MOVE_CONSTRUCTOR(BundleTangent) + + // Copy constructors given base + template + BundleTangent(const TangentBase<_DerivedOther> & o); + + MANIF_TANGENT_ASSIGN_OP(BundleTangent) + + // Tangent common API + + /** + * @brief Get a reference to the underlying DataType. + */ + DataType & coeffs(); + + /** + * @brief Get a const reference to the underlying DataType. + */ + const DataType & coeffs() const; + + + // BundleTangent specific API + + /** + * @brief Construct from BundleTangent elements + */ + BundleTangent(const typename _T<_Scalar>::Tangent & ... elements); + +protected: + + // Helper for the elements constructor + template + BundleTangent( + internal::intseq<_BegRep...>, internal::intseq<_LenRep...>, + const typename _T<_Scalar>::Tangent & ... elements); + +protected: + + DataType data_; +}; + + +template class ... _T> +template +BundleTangent<_Scalar, _T...>::BundleTangent(const TangentBase<_DerivedOther> & o) +: data_(o.coeffs()) +{} + +template class ... _T> +BundleTangent<_Scalar, _T...>::BundleTangent(const typename _T<_Scalar>::Tangent & ... elements) +: BundleTangent(BegRep{}, LenRep{}, elements ...) +{} + +template class ... _T> +template +BundleTangent<_Scalar, _T...>::BundleTangent( + internal::intseq<_BegRep...>, internal::intseq<_LenRep...>, + const typename _T<_Scalar>::Tangent & ... elements) +{ + // c++11 "fold expression" + auto l = {((data_.template segment<_LenRep>(_BegRep) = elements.coeffs()), 0) ...}; + static_cast(l); // compiler warning +} + +template class ... _T> +typename BundleTangent<_Scalar, _T...>::DataType & +BundleTangent<_Scalar, _T...>::coeffs() +{ + return data_; +} + +template class ... _T> +const typename BundleTangent<_Scalar, _T...>::DataType & +BundleTangent<_Scalar, _T...>::coeffs() const +{ + return data_; +} + +} // namespace manif + +#endif // _MANIF_MANIF_BUNDLETANGENT_H_ diff --git a/include/manif/impl/bundle/BundleTangent_base.h b/include/manif/impl/bundle/BundleTangent_base.h new file mode 100644 index 00000000..5df46468 --- /dev/null +++ b/include/manif/impl/bundle/BundleTangent_base.h @@ -0,0 +1,405 @@ +#ifndef _MANIF_MANIF_BUNDLETANGENT_BASE_H_ +#define _MANIF_MANIF_BUNDLETANGENT_BASE_H_ + +#include "manif/impl/tangent_base.h" +#include "manif/impl/traits.h" + +using manif::internal::intseq; + +namespace manif { + +/** + * @brief The base class of the Bundle tangent. + */ +template +struct BundleTangentBase : TangentBase<_Derived> +{ +private: + + using Base = TangentBase<_Derived>; + using Type = BundleTangentBase<_Derived>; + + using IdxList = typename internal::traits<_Derived>::IdxList; + + using BegAlg = typename internal::traits<_Derived>::BegAlg; + using LenAlg = typename internal::traits<_Derived>::LenAlg; + + template + using ElementType = typename internal::traits<_Derived>::template ElementType<_Idx>; + +public: + // start index of DoF for each element as intseq + using BegDoF = typename internal::traits<_Derived>::BegDoF; + // length of DoF for each element as intseq + using LenDoF = typename internal::traits<_Derived>::LenDoF; + + // start index of internal representation for each element as intseq + using BegRep = typename internal::traits<_Derived>::BegRep; + // lenth of internal representation for each element as intseq + using LenRep = typename internal::traits<_Derived>::LenRep; + + + MANIF_TANGENT_TYPEDEF + MANIF_INHERIT_TANGENT_API + MANIF_INHERIT_TANGENT_OPERATOR + + using Base::data; + using Base::coeffs; + +protected: + + using Base::derived; + + MANIF_DEFAULT_CONSTRUCTOR(BundleTangentBase) + +public: + + MANIF_TANGENT_ML_ASSIGN_OP(BundleTangentBase) + + // Tangent common API + + /** + * @brief Hat operator. + * @return An element of the Lie algebra. + */ + LieAlg hat() const; + + /** + * @brief Exponential operator. + * @return An element of the Lie Group. + */ + LieGroup exp(OptJacobianRef J_m_t = {}) const; + + /** + * @brief This function is deprecated. + * Please considere using + * @ref exp instead. + */ + MANIF_DEPRECATED + LieGroup retract(OptJacobianRef J_m_t = {}) const; + + /** + * @brief Get the right Jacobian. + */ + Jacobian rjac() const; + + /** + * @brief Get the left Jacobian. + */ + Jacobian ljac() const; + + /** + * @brief Get the inverse of the right Jacobian. + */ + Jacobian rjacinv() const; + + /** + * @brief Get the inverse of the left Jacobian. + */ + Jacobian ljacinv() const; + + /** + * @brief + */ + Jacobian smallAdj() const; + + + // BundleTangent specific API + + /** + * @brief Number of elements in the BundleTangent + */ + static constexpr std::size_t BundleSize = IdxList::size(); + + /** + * @brief Access BundleTangent element as Map + * @tparam _Idx element index + */ + template + Eigen::Map> element(); + + /** + * @brief Access BundleTangent element as Map to const + * @tparam _Idx element index + */ + template + Eigen::Map> element() const; + +protected: + + template + LieAlg + hat_impl(intseq<_Idx...>, intseq<_BegAlg...>, intseq<_LenAlg...>) const; + + template + LieGroup + exp_impl(OptJacobianRef J_m_t, intseq<_Idx...>, intseq<_BegDoF...>, intseq<_LenDoF...>) const; + + template + Jacobian + rjac_impl(intseq<_Idx...>, intseq<_BegDoF...>, intseq<_LenDoF...>) const; + + template + Jacobian + ljac_impl(intseq<_Idx...>, intseq<_BegDoF...>, intseq<_LenDoF...>) const; + + template + Jacobian + rjacinv_impl(intseq<_Idx...>, intseq<_BegDoF...>, intseq<_LenDoF...>) const; + + template + Jacobian + ljacinv_impl(intseq<_Idx...>, intseq<_BegDoF...>, intseq<_LenDoF...>) const; + + template + Jacobian + smallAdj_impl(intseq<_Idx...>, intseq<_BegDoF...>, intseq<_LenDoF...>) const; + + friend internal::GeneratorEvaluator>; + friend internal::RandomEvaluatorImpl>; +}; + + +template +typename BundleTangentBase<_Derived>::LieAlg +BundleTangentBase<_Derived>::hat() const +{ + return hat_impl(IdxList{}, BegAlg{}, LenAlg{}); +} + +template +template +typename BundleTangentBase<_Derived>::LieAlg +BundleTangentBase<_Derived>::hat_impl( + intseq<_Idx...>, intseq<_BegAlg...>, intseq<_LenAlg...>) const +{ + LieAlg ret = LieAlg::Zero(); + // c++11 "fold expression" + auto l = {((ret.template block<_LenAlg, _LenAlg>(_BegAlg, _BegAlg) = element<_Idx>().hat()), 0) ...}; + static_cast(l); // compiler warning + return ret; +} + +template +typename BundleTangentBase<_Derived>::LieGroup +BundleTangentBase<_Derived>::exp(OptJacobianRef J_m_t) const +{ + if (J_m_t) { + J_m_t->setZero(); + } + return exp_impl(J_m_t, IdxList{}, BegDoF{}, LenDoF{}); +} + +template +template +typename BundleTangentBase<_Derived>::LieGroup +BundleTangentBase<_Derived>::exp_impl( + OptJacobianRef J_m_t, intseq<_Idx...>, intseq<_BegDoF...>, intseq<_LenDoF...>) const +{ + if (J_m_t) { + return LieGroup(element<_Idx>().exp(J_m_t->template block<_LenDoF, _LenDoF>(_BegDoF, _BegDoF)) ...); + } + return LieGroup(element<_Idx>().exp() ...); +} + +template +typename BundleTangentBase<_Derived>::LieGroup +BundleTangentBase<_Derived>::retract(OptJacobianRef J_m_t) const +{ + return exp(J_m_t); +} + +template +typename BundleTangentBase<_Derived>::Jacobian +BundleTangentBase<_Derived>::rjac() const +{ + return rjac_impl(IdxList{}, BegDoF{}, LenDoF{}); +} + +template +typename BundleTangentBase<_Derived>::Jacobian +BundleTangentBase<_Derived>::ljac() const +{ + return ljac_impl(IdxList{}, BegDoF{}, LenDoF{}); +} + +template +typename BundleTangentBase<_Derived>::Jacobian +BundleTangentBase<_Derived>::rjacinv() const +{ + return rjacinv_impl(IdxList{}, BegDoF{}, LenDoF{}); +} + +template +typename BundleTangentBase<_Derived>::Jacobian +BundleTangentBase<_Derived>::ljacinv() const +{ + return ljacinv_impl(IdxList{}, BegDoF{}, LenDoF{}); +} + +template +typename BundleTangentBase<_Derived>::Jacobian +BundleTangentBase<_Derived>::smallAdj() const +{ + return smallAdj_impl(IdxList{}, BegDoF{}, LenDoF{}); +} + +template +template +typename BundleTangentBase<_Derived>::Jacobian +BundleTangentBase<_Derived>::rjac_impl( + intseq<_Idx...>, intseq<_BegDoF...>, intseq<_LenDoF...>) const +{ + Jacobian Jr = Jacobian::Zero(); + // c++11 "fold expression" + auto l = {((Jr.template block<_LenDoF, _LenDoF>(_BegDoF, _BegDoF) = element<_Idx>().rjac() ), 0) ...}; + static_cast(l); // compiler warning + return Jr; +} + +template +template +typename BundleTangentBase<_Derived>::Jacobian +BundleTangentBase<_Derived>::ljac_impl( + intseq<_Idx...>, intseq<_BegDoF...>, intseq<_LenDoF...>) const +{ + Jacobian Jr = Jacobian::Zero(); + // c++11 "fold expression" + auto l = {((Jr.template block<_LenDoF, _LenDoF>(_BegDoF, _BegDoF) = element<_Idx>().ljac()), 0) ...}; + static_cast(l); // compiler warning + return Jr; +} + +template +template +typename BundleTangentBase<_Derived>::Jacobian +BundleTangentBase<_Derived>::rjacinv_impl( + intseq<_Idx...>, intseq<_BegDoF...>, intseq<_LenDoF...>) const +{ + Jacobian Jr = Jacobian::Zero(); + // c++11 "fold expression" + auto l = { + ((Jr.template block<_LenDoF, _LenDoF>(_BegDoF, _BegDoF) = element<_Idx>().rjacinv()), 0) ... + }; + static_cast(l); // compiler warning + return Jr; +} + +template +template +typename BundleTangentBase<_Derived>::Jacobian +BundleTangentBase<_Derived>::ljacinv_impl( + intseq<_Idx...>, intseq<_BegDoF...>, intseq<_LenDoF...>) const +{ + Jacobian Jr = Jacobian::Zero(); + // c++11 "fold expression" + auto l = { + ((Jr.template block<_LenDoF, _LenDoF>(_BegDoF, _BegDoF) = element<_Idx>().ljacinv()), 0) ... + }; + static_cast(l); // compiler warning + return Jr; +} + +template +template +typename BundleTangentBase<_Derived>::Jacobian +BundleTangentBase<_Derived>::smallAdj_impl( + intseq<_Idx...>, intseq<_BegDoF...>, intseq<_LenDoF...>) const +{ + Jacobian Jr = Jacobian::Zero(); + // c++11 "fold expression" + auto l = { + ((Jr.template block<_LenDoF, _LenDoF>(_BegDoF, _BegDoF) = element<_Idx>().smallAdj()), 0) ... + }; + static_cast(l); // compiler warning + return Jr; +} + +template +template +Eigen::Map::template ElementType<_Idx>> +BundleTangentBase<_Derived>::element() +{ + return Eigen::Map>( + static_cast<_Derived &>(*this).coeffs().data() + + internal::intseq_element<_Idx, BegRep>::value); +} + +template +template +Eigen::Map::template ElementType<_Idx>> +BundleTangentBase<_Derived>::element() const +{ + return Eigen::Map>( + static_cast(*this).coeffs().data() + + internal::intseq_element<_Idx, BegRep>::value); +} + +namespace internal { + +/** + * @brief Generator specialization for BundleTangentBase objects. + */ +template +struct GeneratorEvaluator> +{ + static typename BundleTangentBase::LieAlg + run(const unsigned int i) + { + MANIF_CHECK( + i < BundleTangentBase::DoF, + "Index i must less than DoF!", + invalid_argument); + + return run( + i, + typename BundleTangentBase::IdxList{}, + typename BundleTangentBase::BegDoF{}, + typename BundleTangentBase::LenDoF{}, + typename BundleTangentBase::BegAlg{}, + typename BundleTangentBase::LenAlg{}); + } + + template + static typename BundleTangentBase::LieAlg + run( + const unsigned int i, intseq<_Idx...>, + intseq<_BegDoF...>, intseq<_LenDoF...>, + intseq<_BegAlg...>, intseq<_LenAlg...>) + { + using LieAlg = typename BundleTangentBase::LieAlg; + LieAlg Ei = LieAlg::Constant(0); + // c++11 "fold expression" + auto l = {((Ei.template block<_LenAlg, _LenAlg>(_BegAlg, _BegAlg) = + (i >= _BegDoF && i < _BegDoF + _LenDoF) ? + BundleTangentBase::template ElementType<_Idx>::Generator(i - _BegDoF) : + BundleTangentBase::template ElementType<_Idx>::LieAlg::Zero() + ), 0) ...}; + static_cast(l); // compiler warning + return Ei; + } +}; + +/** + * @brief Random specialization for BundleTangent objects. + */ +template +struct RandomEvaluatorImpl> +{ + static void run(BundleTangentBase & m) + { + run(m, typename BundleTangentBase::IdxList{}); + } + + template + static void run(BundleTangentBase & m, intseq<_Idx...>) + { + m = typename BundleTangentBase::Tangent( + BundleTangentBase::template ElementType<_Idx>::Random() ...); + } +}; + +} // namespace internal +} // namespace manif + +#endif // _MANIF_MANIF_BUNDLETANGENT_BASE_H_ diff --git a/include/manif/impl/bundle/BundleTangent_map.h b/include/manif/impl/bundle/BundleTangent_map.h new file mode 100644 index 00000000..0b91f46c --- /dev/null +++ b/include/manif/impl/bundle/BundleTangent_map.h @@ -0,0 +1,99 @@ +#ifndef _MANIF_MANIF_BUNDLETANGENT_MAP_H_ +#define _MANIF_MANIF_BUNDLETANGENT_MAP_H_ + +#include "manif/impl/bundle/BundleTangent.h" + +namespace manif { +namespace internal { + +/** + * @brief traits specialization for Eigen Map + */ +template class ... T> +struct traits, 0>> + : public traits> +{ + using typename traits>::Scalar; + using traits>::DoF; + using Base = BundleTangentBase, 0>>; + using DataType = Eigen::Map, 0>; +}; + +/** + * @brief traits specialization for Eigen const Map + */ +template class ... T> +struct traits, 0>> + : public traits> +{ + using typename traits>::Scalar; + using traits>::DoF; + using Base = BundleTangentBase, 0>>; + using DataType = Eigen::Map, 0>; +}; + +} // namespace internal +} // namespace manif + + +namespace Eigen { + +/** + * @brief Specialization of Map for manif::Bundle + */ +template class ... T> +class Map, 0> + : public manif::BundleTangentBase, 0>> +{ + using Base = manif::BundleTangentBase, 0>>; + +public: + + MANIF_TANGENT_TYPEDEF + MANIF_INHERIT_TANGENT_API + MANIF_INHERIT_TANGENT_OPERATOR + + using Base::BundleSize; + + Map(Scalar * coeffs) : data_(coeffs) { } + + MANIF_TANGENT_MAP_ASSIGN_OP(BundleTangent) + + DataType & coeffs() {return data_;} + + const DataType & coeffs() const {return data_;} + +protected: + + DataType data_; +}; + +/** + * @brief Specialization of Map for const manif::BundleTangent + */ +template class ... T> +class Map, 0> + : public manif::BundleTangentBase, 0>> +{ + using Base = manif::BundleTangentBase, 0>>; + +public: + + MANIF_TANGENT_TYPEDEF + MANIF_INHERIT_TANGENT_API + MANIF_INHERIT_TANGENT_OPERATOR + + using Base::BundleSize; + + Map(const Scalar * coeffs) : data_(coeffs) { } + + const DataType & coeffs() const {return data_;} + +protected: + + const DataType data_; +}; + +} // namespace Eigen + +#endif // _MANIF_MANIF_BUNDLETANGENT_MAP_H_ diff --git a/include/manif/impl/bundle/Bundle_base.h b/include/manif/impl/bundle/Bundle_base.h new file mode 100644 index 00000000..29c05cc8 --- /dev/null +++ b/include/manif/impl/bundle/Bundle_base.h @@ -0,0 +1,395 @@ +#ifndef _MANIF_MANIF_BUNDLE_BASE_H_ +#define _MANIF_MANIF_BUNDLE_BASE_H_ + +#include "manif/impl/lie_group_base.h" +#include "manif/impl/traits.h" + +using manif::internal::intseq; + +namespace manif { + +/** + * @brief The base class of the Bundle group. + */ +template +struct BundleBase : LieGroupBase<_Derived> +{ +private: + + using Base = LieGroupBase<_Derived>; + using Type = BundleBase<_Derived>; + + using IdxList = typename internal::traits<_Derived>::IdxList; + + using BegDim = typename internal::traits<_Derived>::BegDim; + using LenDim = typename internal::traits<_Derived>::LenDim; + + using BegTra = typename internal::traits<_Derived>::BegTra; + using LenTra = typename internal::traits<_Derived>::LenTra; + + template + using ElementType = typename internal::traits<_Derived>::template ElementType; + +public: + // start index of DoF for each element as intseq + using BegDoF = typename internal::traits<_Derived>::BegDoF; + // length of DoF for each element as intseq + using LenDoF = typename internal::traits<_Derived>::LenDoF; + + // start index of internal representation for each element as intseq + using BegRep = typename internal::traits<_Derived>::BegRep; + // lenth of internal representation for each element as intseq + using LenRep = typename internal::traits<_Derived>::LenRep; + + MANIF_GROUP_TYPEDEF + MANIF_INHERIT_GROUP_AUTO_API + MANIF_INHERIT_GROUP_OPERATOR + + using Base::coeffs; + + using Transformation = typename internal::traits<_Derived>::Transformation; + + // LieGroup common API + +protected: + + using Base::derived; + + MANIF_DEFAULT_CONSTRUCTOR(BundleBase) + +public: + + MANIF_GROUP_ML_ASSIGN_OP(BundleBase) + + /** + * @brief Get the inverse of this. + * @param[out] -optional- J_minv_m Jacobian of the inverse wrt this. + */ + LieGroup inverse(OptJacobianRef J_minv_m = {}) const; + + /** + * @brief Get the corresponding Lie algebra element. + * @param[out] -optional- J_t_m Jacobian of the tangent wrt to this. + * @return The tangent of this. + */ + Tangent log(OptJacobianRef J_t_m = {}) const; + + /** + * @brief This function is deprecated. + * Please consider using + * @ref log instead. + */ + MANIF_DEPRECATED + Tangent lift(OptJacobianRef J_t_m = {}) const; + + /** + * @brief Composition of this and another Bundle element. + * @param[in] m Another Bundle element. + * @param[out] -optional- J_mc_ma Jacobian of the composition wrt this. + * @param[out] -optional- J_mc_mb Jacobian of the composition wrt m. + * @return The composition of 'this . m'. + */ + template + LieGroup compose( + const LieGroupBase<_DerivedOther> & m, + OptJacobianRef J_mc_ma = {}, + OptJacobianRef J_mc_mb = {}) const; + + /** + * @brief Bundle group action + * @param v vector. + * @param[out] -optional- J_vout_m The Jacobian of the new object wrt this. + * @param[out] -optional- J_vout_v The Jacobian of the new object wrt input object. + * @return The translated vector. + */ + Vector act( + const Vector & v, + tl::optional>> J_vout_m = {}, + tl::optional>> J_vout_v = {}) const; + + /** + * @brief Get the adjoint matrix at this. + */ + Jacobian adj() const; + + + // Bundle-specific API + + /** + * @brief Number of elements in bundle + */ + static constexpr std::size_t BundleSize = IdxList::size(); + + /** + * @brief Get the element-diagonal transformation matrix + */ + Transformation transform() const; + + /** + * @brief Access Bundle element as Map + * @tparam _Idx element index + */ + template + Eigen::Map> element(); + + /** + * @brief Access Bundle element as Map to const + * @tparam _Idx element index + */ + template + Eigen::Map> element() const; + +protected: + + template + LieGroup + inverse_impl(OptJacobianRef, intseq<_Idx...>, intseq<_BegDoF...>, intseq<_LenDoF...>) const; + + template + Tangent + log_impl(OptJacobianRef, intseq<_Idx...>, intseq<_BegDoF...>, intseq<_LenDoF...>) const; + + template + LieGroup + compose_impl( + const LieGroupBase<_DerivedOther> & m, + OptJacobianRef J_mc_ma, OptJacobianRef J_mc_mb, + intseq<_Idx...>, intseq<_BegDoF...>, intseq<_LenDoF...>) const; + + template + Vector act_impl( + const Vector & v, + tl::optional>> J_vout_m, + tl::optional>> J_vout_v, + intseq<_Idx...>, intseq<_BegDim...>, intseq<_LenDim...>, + intseq<_BegDoF...>, intseq<_LenDoF...>) const; + + template + Jacobian adj_impl(intseq<_Idx...>, intseq<_BegDoF...>, intseq<_LenDoF...>) const; + + template + Transformation + transform_impl(intseq<_Idx...>, intseq<_BegTra...>, intseq<_LenTra...>) const; + + friend internal::RandomEvaluatorImpl>; +}; + + +template +typename BundleBase<_Derived>::Transformation +BundleBase<_Derived>::transform() const +{ + return transform_impl(IdxList{}, BegTra{}, LenTra{}); +} + +template +template +typename BundleBase<_Derived>::Transformation +BundleBase<_Derived>::transform_impl( + intseq<_Idx...>, intseq<_BegTra...>, intseq<_LenTra...>) const +{ + Transformation ret = Transformation::Zero(); + // cxx11 "fold expression" + auto l = + {((ret.template element<_LenTra, _LenTra>(_BegTra, _BegTra) = element<_Idx>().transform()), 0) ...}; + static_cast(l); // compiler warning + return ret; +} + +template +typename BundleBase<_Derived>::LieGroup +BundleBase<_Derived>::inverse(OptJacobianRef J_minv_m) const +{ + if (J_minv_m) { + J_minv_m->setZero(); + } + return inverse_impl(J_minv_m, IdxList{}, BegDoF{}, LenDoF{}); +} + +template +template +typename BundleBase<_Derived>::LieGroup +BundleBase<_Derived>::inverse_impl( + OptJacobianRef J_minv_m, intseq<_Idx...>, intseq<_BegDoF...>, intseq<_LenDoF...>) const +{ + if (J_minv_m) { + return LieGroup( + element<_Idx>().inverse(J_minv_m->template block<_LenDoF, _LenDoF>(_BegDoF, _BegDoF)) ... + ); + } + return LieGroup(element<_Idx>().inverse() ...); +} + +template +typename BundleBase<_Derived>::Tangent +BundleBase<_Derived>::log(OptJacobianRef J_t_m) const +{ + if (J_t_m) { + J_t_m->setZero(); + } + return log_impl(J_t_m, IdxList{}, BegDoF{}, LenDoF{}); +} + +template +template +typename BundleBase<_Derived>::Tangent +BundleBase<_Derived>::log_impl( + OptJacobianRef J_minv_m, intseq<_Idx...>, intseq<_BegDoF...>, intseq<_LenDoF...>) const +{ + if (J_minv_m) { + return Tangent( + element<_Idx>().log(J_minv_m->template block<_LenDoF, _LenDoF>(_BegDoF, _BegDoF))... + ); + } + return Tangent(element<_Idx>().log() ...); +} + +template +typename BundleBase<_Derived>::Tangent +BundleBase<_Derived>::lift(OptJacobianRef J_t_m) const +{ + return log(J_t_m); +} + +template +template +typename BundleBase<_Derived>::LieGroup +BundleBase<_Derived>::compose( + const LieGroupBase<_DerivedOther> & m, OptJacobianRef J_mc_ma, OptJacobianRef J_mc_mb) const +{ + if (J_mc_ma) { + J_mc_ma->setZero(); + } + if (J_mc_mb) { + J_mc_mb->setZero(); + } + return compose_impl(m, J_mc_ma, J_mc_mb, IdxList{}, BegDoF{}, LenDoF{}); +} + +template +template +typename BundleBase<_Derived>::LieGroup +BundleBase<_Derived>::compose_impl( + const LieGroupBase<_DerivedOther> & m, OptJacobianRef J_mc_ma, OptJacobianRef J_mc_mb, + intseq<_Idx...>, intseq<_BegDoF...>, intseq<_LenDoF...>) const +{ + return LieGroup( + element<_Idx>().compose( + static_cast(m).template element<_Idx>(), + J_mc_ma ? + J_mc_ma->template block<_LenDoF, _LenDoF>(_BegDoF, _BegDoF) : + tl::optional>>{}, + J_mc_mb ? + J_mc_mb->template block<_LenDoF, _LenDoF>(_BegDoF, _BegDoF) : + tl::optional>>{} + ) ... + ); +} + +template +typename BundleBase<_Derived>::Vector +BundleBase<_Derived>::act( + const typename BundleBase<_Derived>::Vector & v, + tl::optional>> J_vout_m, + tl::optional>> J_vout_v) const +{ + if (J_vout_m) { + J_vout_m->setZero(); + } + if (J_vout_v) { + J_vout_v->setZero(); + } + + return act_impl(v, J_vout_m, J_vout_v, IdxList{}, BegDim{}, LenDim{}, BegDoF{}, LenDoF{}); +} + +template +template +typename BundleBase<_Derived>::Vector +BundleBase<_Derived>::act_impl( + const typename BundleBase<_Derived>::Vector & v, + tl::optional>> J_vout_m, + tl::optional>> J_vout_v, + intseq<_Idx...>, intseq<_BegDim...>, intseq<_LenDim...>, + intseq<_BegDoF...>, intseq<_LenDoF...>) const +{ + Vector ret; + // cxx11 "fold expression" + auto l = {((ret.template segment<_LenDim>(_BegDim) = element<_Idx>().act( + v.template segment<_LenDim>(_BegDim), + J_vout_m ? + J_vout_m->template block<_LenDim, _LenDoF>(_BegDim, _BegDoF) : + tl::optional>>{}, + J_vout_v ? + J_vout_v->template block<_LenDim, _LenDim>(_BegDim, _BegDim) : + tl::optional>>{} + )), 0) ...}; + static_cast(l); // compiler warning + return ret; +} + +template +typename BundleBase<_Derived>::Jacobian +BundleBase<_Derived>::adj() const +{ + return adj_impl(IdxList{}, BegDoF{}, LenDoF{}); +} + +template +template +typename BundleBase<_Derived>::Jacobian +BundleBase<_Derived>::adj_impl( + intseq<_Idx...>, intseq<_BegDoF...>, intseq<_LenDoF...>) const +{ + Jacobian adj = Jacobian::Zero(); + // cxx11 "fold expression" + auto l = {((adj.template block<_LenDoF, _LenDoF>(_BegDoF, _BegDoF) = element<_Idx>().adj()), 0) ...}; + static_cast(l); // compiler warning + return adj; +} + +template +template +Eigen::Map::template ElementType<_Idx>> +BundleBase<_Derived>::element() +{ + return Eigen::Map>( + static_cast<_Derived &>(*this).coeffs().data() + + internal::intseq_element<_Idx, BegRep>::value); +} + +template +template +Eigen::Map::template ElementType<_Idx>> +BundleBase<_Derived>::element() const +{ + return Eigen::Map>( + static_cast(*this).coeffs().data() + + internal::intseq_element<_Idx, BegRep>::value); +} + +namespace internal { + +/** + * @brief Random specialization for Bundle objects. + */ +template +struct RandomEvaluatorImpl> +{ + static void run(BundleBase & m) + { + run(m, typename BundleBase::IdxList{}); + } + + template + static void run(BundleBase & m, intseq<_Idx...>) + { + m = typename BundleBase::LieGroup( + BundleBase::template ElementType<_Idx>::Random() ...); + } +}; + +} // namespace internal +} // namespace manif + +#endif // _MANIF_MANIF_BUNDLE_BASE_H_ diff --git a/include/manif/impl/bundle/Bundle_map.h b/include/manif/impl/bundle/Bundle_map.h new file mode 100644 index 00000000..8a1c7a19 --- /dev/null +++ b/include/manif/impl/bundle/Bundle_map.h @@ -0,0 +1,97 @@ +#ifndef _MANIF_MANIF_BUNDLE_MAP_H_ +#define _MANIF_MANIF_BUNDLE_MAP_H_ + +#include "manif/impl/bundle/Bundle.h" + +namespace manif { +namespace internal { + +/** + * @brief traits specialization for Eigen Map + */ +template class ... T> +struct traits, 0>> + : public traits> +{ + using typename traits>::Scalar; + using traits>::RepSize; + using Base = BundleBase, 0>>; + using DataType = Eigen::Map, 0>; +}; + +/** + * @brief traits specialization for Eigen const Map + */ +template class ... T> +struct traits, 0>> + : public traits> +{ + using typename traits>::Scalar; + using traits>::RepSize; + using Base = BundleBase, 0>>; + using DataType = Eigen::Map, 0>; +}; + +} // namespace internal +} // namespace manif + + +namespace Eigen { + +/** + * @brief Specialization of Map for manif::Bundle + */ +template class ... T> +class Map, 0> + : public manif::BundleBase, 0>> +{ + using Base = manif::BundleBase, 0>>; + +public: + + MANIF_COMPLETE_GROUP_TYPEDEF + MANIF_INHERIT_GROUP_API + + using Base::BundleSize; + + Map(Scalar * coeffs) : data_(coeffs) { } + + MANIF_GROUP_MAP_ASSIGN_OP(Bundle) + + DataType & coeffs() {return data_;} + + const DataType & coeffs() const {return data_;} + +protected: + + DataType data_; +}; + +/** + * @brief Specialization of Map for const manif::Bundle + */ +template class ... T> +class Map, 0> + : public manif::BundleBase, 0>> +{ + using Base = manif::BundleBase, 0>>; + +public: + + MANIF_COMPLETE_GROUP_TYPEDEF + MANIF_INHERIT_GROUP_API + + using Base::BundleSize; + + Map(const Scalar * coeffs) : data_(coeffs) { } + + const DataType & coeffs() const {return data_;} + +protected: + + const DataType data_; +}; + +} // namespace Eigen + +#endif // _MANIF_MANIF_BUNDLE_MAP_H_ diff --git a/include/manif/impl/macro.h b/include/manif/impl/macro.h index c01ca735..268fca11 100644 --- a/include/manif/impl/macro.h +++ b/include/manif/impl/macro.h @@ -154,6 +154,7 @@ raise(Args&&... args) X& operator =(Eigen::MatrixBase<_EigenDerived>&& o) { coeffs() = std::move(o); return derived(); } #define MANIF_GROUP_MAP_ASSIGN_OP(X) \ + Map(const Map & o) : Base(), data_(o.coeffs()) { }\ Map& operator=(const Map& o) { coeffs() = o.coeffs(); return *this; }\ template \ Map& operator =(const manif::X##Base<_DerivedOther>& o) { coeffs() = o.coeffs(); return *this; }\ @@ -198,6 +199,7 @@ raise(Args&&... args) X& operator =(Eigen::MatrixBase<_EigenDerived>&& o) MANIF_MOVE_NOEXCEPT { coeffs() = std::move(o); return derived(); } #define MANIF_TANGENT_MAP_ASSIGN_OP(X) \ + Map(const Map & o) : Base(), data_(o.coeffs()) { }\ Map& operator=(const Map& o) { coeffs() = o.coeffs(); return *this; }\ template \ Map& operator =(const manif::X##Base<_DerivedOther>& o) { coeffs() = o.coeffs(); return *this; }\ diff --git a/include/manif/impl/rn/RnTangent.h b/include/manif/impl/rn/RnTangent.h index 094ac921..09cf5a39 100644 --- a/include/manif/impl/rn/RnTangent.h +++ b/include/manif/impl/rn/RnTangent.h @@ -93,15 +93,15 @@ template using R7Tangent = RnTangent<_Scalar, 7>; template using R8Tangent = RnTangent<_Scalar, 8>; template using R9Tangent = RnTangent<_Scalar, 9>; -MANIF_EXTRA_GROUP_TYPEDEF(R1Tangent) -MANIF_EXTRA_GROUP_TYPEDEF(R2Tangent) -MANIF_EXTRA_GROUP_TYPEDEF(R3Tangent) -MANIF_EXTRA_GROUP_TYPEDEF(R4Tangent) -MANIF_EXTRA_GROUP_TYPEDEF(R5Tangent) -MANIF_EXTRA_GROUP_TYPEDEF(R6Tangent) -MANIF_EXTRA_GROUP_TYPEDEF(R7Tangent) -MANIF_EXTRA_GROUP_TYPEDEF(R8Tangent) -MANIF_EXTRA_GROUP_TYPEDEF(R9Tangent) +MANIF_EXTRA_TANGENT_TYPEDEF(R1Tangent) +MANIF_EXTRA_TANGENT_TYPEDEF(R2Tangent) +MANIF_EXTRA_TANGENT_TYPEDEF(R3Tangent) +MANIF_EXTRA_TANGENT_TYPEDEF(R4Tangent) +MANIF_EXTRA_TANGENT_TYPEDEF(R5Tangent) +MANIF_EXTRA_TANGENT_TYPEDEF(R6Tangent) +MANIF_EXTRA_TANGENT_TYPEDEF(R7Tangent) +MANIF_EXTRA_TANGENT_TYPEDEF(R8Tangent) +MANIF_EXTRA_TANGENT_TYPEDEF(R9Tangent) template template diff --git a/include/manif/impl/rn/RnTangent_base.h b/include/manif/impl/rn/RnTangent_base.h index 325bd886..9f884dc3 100644 --- a/include/manif/impl/rn/RnTangent_base.h +++ b/include/manif/impl/rn/RnTangent_base.h @@ -1,7 +1,6 @@ #ifndef _MANIF_MANIF_RNTANGENT_BASE_H_ #define _MANIF_MANIF_RNTANGENT_BASE_H_ -#include "manif/impl/rn/Rn_properties.h" #include "manif/impl/tangent_base.h" namespace manif { @@ -84,7 +83,7 @@ struct RnTangentBase : TangentBase<_Derived> Jacobian rjacinv() const; /** - * @brief Get the inverse of the right Jacobian of Rn. + * @brief Get the inverse of the left Jacobian of Rn. * @note See Eq. (191). * @see ljac. */ diff --git a/include/manif/impl/rn/Rn_base.h b/include/manif/impl/rn/Rn_base.h index 7843d1c0..2b4b5ff9 100644 --- a/include/manif/impl/rn/Rn_base.h +++ b/include/manif/impl/rn/Rn_base.h @@ -1,7 +1,6 @@ #ifndef _MANIF_MANIF_RN_BASE_H_ #define _MANIF_MANIF_RN_BASE_H_ -#include "manif/impl/rn/Rn_properties.h" #include "manif/impl/lie_group_base.h" namespace manif { @@ -33,6 +32,7 @@ struct RnBase : LieGroupBase<_Derived> using Transformation = typename internal::traits<_Derived>::Transformation; // LieGroup common API + protected: using Base::derived; @@ -156,8 +156,6 @@ RnBase<_Derived>::compose( OptJacobianRef J_mc_ma, OptJacobianRef J_mc_mb) const { - using std::abs; - static_assert( std::is_base_of, _DerivedOther>::value, "Argument does not inherit from RnBase !"); diff --git a/include/manif/impl/rn/Rn_properties.h b/include/manif/impl/rn/Rn_properties.h deleted file mode 100644 index 8c4250fe..00000000 --- a/include/manif/impl/rn/Rn_properties.h +++ /dev/null @@ -1,33 +0,0 @@ -#ifndef _MANIF_MANIF_RN_PROPERTIES_H_ -#define _MANIF_MANIF_RN_PROPERTIES_H_ - -#include "manif/impl/traits.h" - -namespace manif { - -// Forward declaration -template struct RnBase; -template struct RnTangentBase; - -namespace internal { - -//! traits specialization -template -struct LieGroupProperties> -{ - static constexpr int Dim = traits<_Derived>::Dim; /// @brief Space dimension - static constexpr int DoF = traits<_Derived>::Dim; /// @brief Degrees of freedom -}; - -//! traits specialization -template -struct LieGroupProperties> -{ - static constexpr int Dim = traits<_Derived>::Dim; /// @brief Space dimension - static constexpr int DoF = traits<_Derived>::Dim; /// @brief Degrees of freedom -}; - -} // namespace internal -} // namespace manif - -#endif // _MANIF_MANIF_RN_PROPERTIES_H_ diff --git a/include/manif/impl/traits.h b/include/manif/impl/traits.h index 93124a82..52a9d7aa 100644 --- a/include/manif/impl/traits.h +++ b/include/manif/impl/traits.h @@ -39,13 +39,17 @@ struct traitscast; /** * @brief Traits to change the scalar type of a template class - * @note given using FooDouble = Foo + * @note given using FooDouble = Foo * using FooFloat = typename traitscast::cast; */ -template