Add the possibility to easily set quaternion and translation in an SE3 object

[GiulioRomualdi]

As far as I understood the only way to set the translation and the quaternion in an SE3 object is to use

DataType& coeffs();

For example, the following lines of code are required to set:

1. the quaternion

   manif::SE3d transform;
   tranform.setIdentity();
   // ....
   // I do some stuff with transform
   Eigen::Quaterniond q = Eigen::AngleAxisd(0.23, Eigen::Vector3d::UnitZ());
   transform.coeff().tail<4>() = q.coeff();

2. the translation

   manif::SE3d transform;
   tranform.setIdentity();
   // ....
   // I do some stuff with transform
   transform.coeff().head<3>() << 1, 0.4, 0.13;

I think that calling coeff() and access to the element of the private storage may lead to undesired bugs and errors.

What I suggest is to change the signature of the two following functions https://github.com/artivis/manif/blob/b1c82befd066fb4e954742bab56c7795168bdaab/include/manif/impl/se3/SE3_base.h#L124-L132 to

/**
 * @brief Get the rotational part of this as a quaternion (const version).
 */
Eigen::Map<const QuaternionDataType> quat() const;

/**
 * @brief Get the rotational part of this as a quaternion.
 */
Eigen::Map<QuaternionDataType> quat();

/**
 * @brief Get the translational part in vector form (const version).
 */
Eigen::Map<const Translation> translation() const;

/**
 * @brief Get the translational part in vector form.
 */
Eigen::Map<Translation> translation();

Thanks to this modification, the Eigen::Map capabilities are used to return a translation and a quaternion, so we can avoid memory allocation. Furthermore the new feature allows to easily set the translation and the quaternion stored in a SE3 object. e.g.

manif::SE3d transform = manif::SE3d::Identity();
transform.translation() << 0.9, 0, 0;
transform.quat() = Eigen::AngleAxisd(1.5708, Eigen::Vector3d::UnitZ());

This commit implements the feature I've just described https://github.com/GiulioRomualdi/manif/commit/513f8f5e902d05d03e3ac1ccc9df40ec1a765486

I also tried to run all the existing tests and all of them pass without problems

@artivis, Let me know if you are interested in this feature.

cc @prashanthr05

[artivis]

Hi @GiulioRomualdi,

This is somewhat related to #132. To summarize, classes in manif were not offering non-const data access until very recently. The rational was to somewhat 'filter' inputs and prevent the user from feeding in invalid values (e.g. non-unit quaternion). Such control is virtually impossible on functions that returns a direct access to the data (reference, pointer, Map...). That is why the current API may seem limited. Note that the point of this control is only to help user figure out this kind of issue as early/easily as possible; it can be much more difficult to debug such issue once manif is integrated into a larger project (speaking from experience).

This being said I understand that experimented users favor neat API over 'safety' and this motivated both #137 and #138. I am not completely sold on your proposal, especially for the quat function. I'm fine giving the user the freedom to shoot himself in the foot calling non-const m.coeffs(), however my feeling is that quat should check for unit quaternion (unless explicitly turned off). It's a matter of semantic, I might be wrong tho... Following this train of thoughts I could imagine setter functions such as

void quat(const quaternion& q);
void quat(const SO3& SO3);

etc... Then the checks can be run and possibly (explicitly) disabled.

In the meantime, for your particular example, SE3 offers plenty of constructors, including SE3(const Translation& t, const Eigen::AngleAxis<Scalar>& angle_axis).

@joansola what's you thoughts on this matter?

[prashanthr05]

In line with @artivis suggestion, we could actually use bool as return which might be false in case user tries to set a non-unit norm quaternion, emphasizing that such a quaternion cannot be set. similarly for rotation matrix, trying to check atleast the unit determinant, if not for orthonormality (due to computational reasons).

Also in my opinion, access to coeffs() is like walking on ice, if the index offsets are not known properly.

[GiulioRomualdi]

@artivis if you agree I can propose a PR with the following methods in the SE3Base class. In each method, I will check if the quaternion has unitary norm by using https://github.com/artivis/manif/blob/b1c82befd066fb4e954742bab56c7795168bdaab/include/manif/impl/se3/SE3.h#L158

void quat(const QuaternionDataType& quaternion);

template <typename _EigenDerived>
void quat(const Eigen::MatrixBase<_EigenDerived> &quaternion);

void quat(const SO3<Scalar>& so3);

cc @S-dafarra @traversaro @prashanthr05

[GiulioRomualdi]

GiulioRomualdi/manif@be76458 Implements the setters proposed, while https://github.com/GiulioRomualdi/manif/commit/23db0f3482a49531e708807d23d65c31ad330d4a is a test for the developed methods

[artivis]

GiulioRomualdi/manif@be76458 Implements the setters proposed, while GiulioRomualdi@23db0f3 is a test for the developed methods

Looks good, I'll have a couple comments tho. Feel free to open a PR!