point_poses2vectors.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 |
   +------------------------------------------------------------------------+ */
#pragma once

#include <mrpt/math/math_frwds.h>  // forward declarations

namespace mrpt
{
namespace math
{
/** \name Container initializer from pose classes
  * @{
  */

/** Conversion of poses to MRPT containers (vector/matrix) */
template <class CONTAINER, class POINT_OR_POSE>
CONTAINER& containerFromPoseOrPoint(CONTAINER& C, const POINT_OR_POSE& p)
{
        const size_t DIMS = POINT_OR_POSE::static_size;
        C.resize(DIMS, 1);
        for (size_t i = 0; i < DIMS; i++)
                C.coeffRef(i, 0) = static_cast<typename CONTAINER::Scalar>(p[i]);
        return C;
}

/** @} */

}  // End of math namespace
}  // End of mrpt namespace