homog_matrices.h

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file     |
   | See: http://www.mrpt.org/Authors - All rights reserved.                |
   | Released under BSD License. See details in http://www.mrpt.org/License |
   +------------------------------------------------------------------------+ */
#ifndef MRPT_HOMOG_MATRICES_H
#define MRPT_HOMOG_MATRICES_H

#include <mrpt/config.h>
#include <mrpt/utils/compiler_fixes.h>
#include <mrpt/utils/mrpt_macros.h>

namespace mrpt
{
namespace math
{
/** Efficiently compute the inverse of a 4x4 homogeneous matrix by only
 * transposing the rotation 3x3 part and solving the translation with dot
 * products.
  *  This is a generic template which works with:
  *    MATRIXLIKE: CMatrixTemplateNumeric, CMatrixFixedNumeric
  */
template <class MATRIXLIKE1, class MATRIXLIKE2>
void homogeneousMatrixInverse(const MATRIXLIKE1& M, MATRIXLIKE2& out_inverse_M)
{
        MRPT_START
        ASSERT_(M.isSquare() && size(M, 1) == 4);

        /* Instead of performing a generic 4x4 matrix inversion, we only need to
          transpose the rotation part, then replace the translation part by
          three dot products. See, for example:
         https://graphics.stanford.edu/courses/cs248-98-fall/Final/q4.html

                [ux vx wx tx] -1   [ux uy uz -dot(u,t)]
                [uy vy wy ty]      [vx vy vz -dot(v,t)]
                [uz vz wz tz]    = [wx wy wz -dot(w,t)]
                [ 0  0  0  1]      [ 0  0  0     1    ]
        */

        out_inverse_M.setSize(4, 4);

        // 3x3 rotation part:
        out_inverse_M.set_unsafe(0, 0, M.get_unsafe(0, 0));
        out_inverse_M.set_unsafe(0, 1, M.get_unsafe(1, 0));
        out_inverse_M.set_unsafe(0, 2, M.get_unsafe(2, 0));

        out_inverse_M.set_unsafe(1, 0, M.get_unsafe(0, 1));
        out_inverse_M.set_unsafe(1, 1, M.get_unsafe(1, 1));
        out_inverse_M.set_unsafe(1, 2, M.get_unsafe(2, 1));

        out_inverse_M.set_unsafe(2, 0, M.get_unsafe(0, 2));
        out_inverse_M.set_unsafe(2, 1, M.get_unsafe(1, 2));
        out_inverse_M.set_unsafe(2, 2, M.get_unsafe(2, 2));

        const double tx = -M.get_unsafe(0, 3);
        const double ty = -M.get_unsafe(1, 3);
        const double tz = -M.get_unsafe(2, 3);

        const double tx_ = tx * M.get_unsafe(0, 0) + ty * M.get_unsafe(1, 0) +
                                           tz * M.get_unsafe(2, 0);
        const double ty_ = tx * M.get_unsafe(0, 1) + ty * M.get_unsafe(1, 1) +
                                           tz * M.get_unsafe(2, 1);
        const double tz_ = tx * M.get_unsafe(0, 2) + ty * M.get_unsafe(1, 2) +
                                           tz * M.get_unsafe(2, 2);

        out_inverse_M.set_unsafe(0, 3, tx_);
        out_inverse_M.set_unsafe(1, 3, ty_);
        out_inverse_M.set_unsafe(2, 3, tz_);

        out_inverse_M.set_unsafe(3, 0, 0);
        out_inverse_M.set_unsafe(3, 1, 0);
        out_inverse_M.set_unsafe(3, 2, 0);
        out_inverse_M.set_unsafe(3, 3, 1);

        MRPT_END
}
//! \overload
template <class IN_ROTMATRIX, class IN_XYZ, class OUT_ROTMATRIX, class OUT_XYZ>
void homogeneousMatrixInverse(
        const IN_ROTMATRIX& in_R, const IN_XYZ& in_xyz, OUT_ROTMATRIX& out_R,
        OUT_XYZ& out_xyz)
{
        MRPT_START
        ASSERT_(in_R.isSquare() && size(in_R, 1) == 3 && in_xyz.size() == 3)
        out_R.setSize(3, 3);
        out_xyz.resize(3);

        // translation part:
        typedef typename IN_ROTMATRIX::Scalar T;
        const T tx = -in_xyz[0];
        const T ty = -in_xyz[1];
        const T tz = -in_xyz[2];

        out_xyz[0] = tx * in_R.get_unsafe(0, 0) + ty * in_R.get_unsafe(1, 0) +
                                 tz * in_R.get_unsafe(2, 0);
        out_xyz[1] = tx * in_R.get_unsafe(0, 1) + ty * in_R.get_unsafe(1, 1) +
                                 tz * in_R.get_unsafe(2, 1);
        out_xyz[2] = tx * in_R.get_unsafe(0, 2) + ty * in_R.get_unsafe(1, 2) +
                                 tz * in_R.get_unsafe(2, 2);

        // 3x3 rotation part: transpose
        out_R = in_R.adjoint();

        MRPT_END
}
//! \overload
template <class MATRIXLIKE>
inline void homogeneousMatrixInverse(MATRIXLIKE& M)
{
        ASSERTDEB_(M.isSquare() && size(M, 1) == 4);
        // translation:
        const double tx = -M(0, 3);
        const double ty = -M(1, 3);
        const double tz = -M(2, 3);
        M(0, 3) = tx * M(0, 0) + ty * M(1, 0) + tz * M(2, 0);
        M(1, 3) = tx * M(0, 1) + ty * M(1, 1) + tz * M(2, 1);
        M(2, 3) = tx * M(0, 2) + ty * M(1, 2) + tz * M(2, 2);
        // 3x3 rotation part:
        double t;  // avoid std::swap() to avoid <algorithm> only for that.
        t = M(1, 0);
        M(1, 0) = M(0, 1);
        M(0, 1) = t;
        t = M(2, 0);
        M(2, 0) = M(0, 2);
        M(0, 2) = t;
        t = M(1, 2);
        M(1, 2) = M(2, 1);
        M(2, 1) = t;
}
}
}
#endif