569 lines
25 KiB
C++
569 lines
25 KiB
C++
/*
|
|
* Software License Agreement (BSD License)
|
|
*
|
|
* Point Cloud Library (PCL) - www.pointclouds.org
|
|
* Copyright (c) 2010-2012, Willow Garage, Inc.
|
|
* Copyright (C) 2010 Gael Guennebaud <gael.guennebaud@inria.fr>
|
|
* Copyright (C) 2009 Hauke Heibel <hauke.heibel@gmail.com>
|
|
* Copyright (c) 2012-, Open Perception, Inc.
|
|
*
|
|
* All rights reserved.
|
|
*
|
|
* Redistribution and use in source and binary forms, with or without
|
|
* modification, are permitted provided that the following conditions
|
|
* are met:
|
|
*
|
|
* * Redistributions of source code must retain the above copyright
|
|
* notice, this list of conditions and the following disclaimer.
|
|
* * Redistributions in binary form must reproduce the above
|
|
* copyright notice, this list of conditions and the following
|
|
* disclaimer in the documentation and/or other materials provided
|
|
* with the distribution.
|
|
* * Neither the name of the copyright holder(s) nor the names of its
|
|
* contributors may be used to endorse or promote products derived
|
|
* from this software without specific prior written permission.
|
|
*
|
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
|
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
|
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
* POSSIBILITY OF SUCH DAMAGE.
|
|
*
|
|
* $Id$
|
|
*
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#ifndef NOMINMAX
|
|
#define NOMINMAX
|
|
#endif
|
|
|
|
#if defined __GNUC__
|
|
# pragma GCC system_header
|
|
#elif defined __SUNPRO_CC
|
|
# pragma disable_warn
|
|
#endif
|
|
|
|
#include <pcl/ModelCoefficients.h>
|
|
|
|
#include <Eigen/StdVector>
|
|
#include <Eigen/Geometry>
|
|
#include <Eigen/LU>
|
|
|
|
namespace pcl
|
|
{
|
|
/** \brief Compute the roots of a quadratic polynom x^2 + b*x + c = 0
|
|
* \param[in] b linear parameter
|
|
* \param[in] c constant parameter
|
|
* \param[out] roots solutions of x^2 + b*x + c = 0
|
|
*/
|
|
template <typename Scalar, typename Roots> void
|
|
computeRoots2 (const Scalar &b, const Scalar &c, Roots &roots);
|
|
|
|
/** \brief computes the roots of the characteristic polynomial of the input matrix m, which are the eigenvalues
|
|
* \param[in] m input matrix
|
|
* \param[out] roots roots of the characteristic polynomial of the input matrix m, which are the eigenvalues
|
|
*/
|
|
template <typename Matrix, typename Roots> void
|
|
computeRoots (const Matrix &m, Roots &roots);
|
|
|
|
/** \brief determine the smallest eigenvalue and its corresponding eigenvector
|
|
* \param[in] mat input matrix that needs to be symmetric and positive semi definite
|
|
* \param[out] eigenvalue the smallest eigenvalue of the input matrix
|
|
* \param[out] eigenvector the corresponding eigenvector to the smallest eigenvalue of the input matrix
|
|
* \ingroup common
|
|
*/
|
|
template <typename Matrix, typename Vector> void
|
|
eigen22 (const Matrix &mat, typename Matrix::Scalar &eigenvalue, Vector &eigenvector);
|
|
|
|
/** \brief determine the smallest eigenvalue and its corresponding eigenvector
|
|
* \param[in] mat input matrix that needs to be symmetric and positive semi definite
|
|
* \param[out] eigenvectors the corresponding eigenvector to the smallest eigenvalue of the input matrix
|
|
* \param[out] eigenvalues the smallest eigenvalue of the input matrix
|
|
* \ingroup common
|
|
*/
|
|
template <typename Matrix, typename Vector> void
|
|
eigen22 (const Matrix &mat, Matrix &eigenvectors, Vector &eigenvalues);
|
|
|
|
/** \brief determines the corresponding eigenvector to the given eigenvalue of the symmetric positive semi definite input matrix
|
|
* \param[in] mat symmetric positive semi definite input matrix
|
|
* \param[in] eigenvalue the eigenvalue which corresponding eigenvector is to be computed
|
|
* \param[out] eigenvector the corresponding eigenvector for the input eigenvalue
|
|
* \ingroup common
|
|
*/
|
|
template <typename Matrix, typename Vector> void
|
|
computeCorrespondingEigenVector (const Matrix &mat, const typename Matrix::Scalar &eigenvalue, Vector &eigenvector);
|
|
|
|
/** \brief determines the eigenvector and eigenvalue of the smallest eigenvalue of the symmetric positive semi definite input matrix
|
|
* \param[in] mat symmetric positive semi definite input matrix
|
|
* \param[out] eigenvalue smallest eigenvalue of the input matrix
|
|
* \param[out] eigenvector the corresponding eigenvector for the input eigenvalue
|
|
* \note if the smallest eigenvalue is not unique, this function may return any eigenvector that is consistent to the eigenvalue.
|
|
* \ingroup common
|
|
*/
|
|
template <typename Matrix, typename Vector> void
|
|
eigen33 (const Matrix &mat, typename Matrix::Scalar &eigenvalue, Vector &eigenvector);
|
|
|
|
/** \brief determines the eigenvalues of the symmetric positive semi definite input matrix
|
|
* \param[in] mat symmetric positive semi definite input matrix
|
|
* \param[out] evals resulting eigenvalues in ascending order
|
|
* \ingroup common
|
|
*/
|
|
template <typename Matrix, typename Vector> void
|
|
eigen33 (const Matrix &mat, Vector &evals);
|
|
|
|
/** \brief determines the eigenvalues and corresponding eigenvectors of the symmetric positive semi definite input matrix
|
|
* \param[in] mat symmetric positive semi definite input matrix
|
|
* \param[out] evecs corresponding eigenvectors in correct order according to eigenvalues
|
|
* \param[out] evals resulting eigenvalues in ascending order
|
|
* \ingroup common
|
|
*/
|
|
template <typename Matrix, typename Vector> void
|
|
eigen33 (const Matrix &mat, Matrix &evecs, Vector &evals);
|
|
|
|
/** \brief Calculate the inverse of a 2x2 matrix
|
|
* \param[in] matrix matrix to be inverted
|
|
* \param[out] inverse the resultant inverted matrix
|
|
* \note only the upper triangular part is taken into account => non symmetric matrices will give wrong results
|
|
* \return determinant of the original matrix => if 0 no inverse exists => result is invalid
|
|
* \ingroup common
|
|
*/
|
|
template <typename Matrix> typename Matrix::Scalar
|
|
invert2x2 (const Matrix &matrix, Matrix &inverse);
|
|
|
|
/** \brief Calculate the inverse of a 3x3 symmetric matrix.
|
|
* \param[in] matrix matrix to be inverted
|
|
* \param[out] inverse the resultant inverted matrix
|
|
* \note only the upper triangular part is taken into account => non symmetric matrices will give wrong results
|
|
* \return determinant of the original matrix => if 0 no inverse exists => result is invalid
|
|
* \ingroup common
|
|
*/
|
|
template <typename Matrix> typename Matrix::Scalar
|
|
invert3x3SymMatrix (const Matrix &matrix, Matrix &inverse);
|
|
|
|
/** \brief Calculate the inverse of a general 3x3 matrix.
|
|
* \param[in] matrix matrix to be inverted
|
|
* \param[out] inverse the resultant inverted matrix
|
|
* \return determinant of the original matrix => if 0 no inverse exists => result is invalid
|
|
* \ingroup common
|
|
*/
|
|
template <typename Matrix> typename Matrix::Scalar
|
|
invert3x3Matrix (const Matrix &matrix, Matrix &inverse);
|
|
|
|
/** \brief Calculate the determinant of a 3x3 matrix.
|
|
* \param[in] matrix matrix
|
|
* \return determinant of the matrix
|
|
* \ingroup common
|
|
*/
|
|
template <typename Matrix> typename Matrix::Scalar
|
|
determinant3x3Matrix (const Matrix &matrix);
|
|
|
|
/** \brief Get the unique 3D rotation that will rotate \a z_axis into (0,0,1) and \a y_direction into a vector
|
|
* with x=0 (or into (0,1,0) should \a y_direction be orthogonal to \a z_axis)
|
|
* \param[in] z_axis the z-axis
|
|
* \param[in] y_direction the y direction
|
|
* \param[out] transformation the resultant 3D rotation
|
|
* \ingroup common
|
|
*/
|
|
inline void
|
|
getTransFromUnitVectorsZY (const Eigen::Vector3f& z_axis,
|
|
const Eigen::Vector3f& y_direction,
|
|
Eigen::Affine3f& transformation);
|
|
|
|
/** \brief Get the unique 3D rotation that will rotate \a z_axis into (0,0,1) and \a y_direction into a vector
|
|
* with x=0 (or into (0,1,0) should \a y_direction be orthogonal to \a z_axis)
|
|
* \param[in] z_axis the z-axis
|
|
* \param[in] y_direction the y direction
|
|
* \return the resultant 3D rotation
|
|
* \ingroup common
|
|
*/
|
|
inline Eigen::Affine3f
|
|
getTransFromUnitVectorsZY (const Eigen::Vector3f& z_axis,
|
|
const Eigen::Vector3f& y_direction);
|
|
|
|
/** \brief Get the unique 3D rotation that will rotate \a x_axis into (1,0,0) and \a y_direction into a vector
|
|
* with z=0 (or into (0,1,0) should \a y_direction be orthogonal to \a z_axis)
|
|
* \param[in] x_axis the x-axis
|
|
* \param[in] y_direction the y direction
|
|
* \param[out] transformation the resultant 3D rotation
|
|
* \ingroup common
|
|
*/
|
|
inline void
|
|
getTransFromUnitVectorsXY (const Eigen::Vector3f& x_axis,
|
|
const Eigen::Vector3f& y_direction,
|
|
Eigen::Affine3f& transformation);
|
|
|
|
/** \brief Get the unique 3D rotation that will rotate \a x_axis into (1,0,0) and \a y_direction into a vector
|
|
* with z=0 (or into (0,1,0) should \a y_direction be orthogonal to \a z_axis)
|
|
* \param[in] x_axis the x-axis
|
|
* \param[in] y_direction the y direction
|
|
* \return the resulting 3D rotation
|
|
* \ingroup common
|
|
*/
|
|
inline Eigen::Affine3f
|
|
getTransFromUnitVectorsXY (const Eigen::Vector3f& x_axis,
|
|
const Eigen::Vector3f& y_direction);
|
|
|
|
/** \brief Get the unique 3D rotation that will rotate \a z_axis into (0,0,1) and \a y_direction into a vector
|
|
* with x=0 (or into (0,1,0) should \a y_direction be orthogonal to \a z_axis)
|
|
* \param[in] y_direction the y direction
|
|
* \param[in] z_axis the z-axis
|
|
* \param[out] transformation the resultant 3D rotation
|
|
* \ingroup common
|
|
*/
|
|
inline void
|
|
getTransformationFromTwoUnitVectors (const Eigen::Vector3f& y_direction,
|
|
const Eigen::Vector3f& z_axis,
|
|
Eigen::Affine3f& transformation);
|
|
|
|
/** \brief Get the unique 3D rotation that will rotate \a z_axis into (0,0,1) and \a y_direction into a vector
|
|
* with x=0 (or into (0,1,0) should \a y_direction be orthogonal to \a z_axis)
|
|
* \param[in] y_direction the y direction
|
|
* \param[in] z_axis the z-axis
|
|
* \return transformation the resultant 3D rotation
|
|
* \ingroup common
|
|
*/
|
|
inline Eigen::Affine3f
|
|
getTransformationFromTwoUnitVectors (const Eigen::Vector3f& y_direction,
|
|
const Eigen::Vector3f& z_axis);
|
|
|
|
/** \brief Get the transformation that will translate \a origin to (0,0,0) and rotate \a z_axis into (0,0,1)
|
|
* and \a y_direction into a vector with x=0 (or into (0,1,0) should \a y_direction be orthogonal to \a z_axis)
|
|
* \param[in] y_direction the y direction
|
|
* \param[in] z_axis the z-axis
|
|
* \param[in] origin the origin
|
|
* \param[in] transformation the resultant transformation matrix
|
|
* \ingroup common
|
|
*/
|
|
inline void
|
|
getTransformationFromTwoUnitVectorsAndOrigin (const Eigen::Vector3f& y_direction,
|
|
const Eigen::Vector3f& z_axis,
|
|
const Eigen::Vector3f& origin,
|
|
Eigen::Affine3f& transformation);
|
|
|
|
/** \brief Extract the Euler angles (XYZ-convention) from the given transformation
|
|
* \param[in] t the input transformation matrix
|
|
* \param[in] roll the resulting roll angle
|
|
* \param[in] pitch the resulting pitch angle
|
|
* \param[in] yaw the resulting yaw angle
|
|
* \ingroup common
|
|
*/
|
|
template <typename Scalar> void
|
|
getEulerAngles (const Eigen::Transform<Scalar, 3, Eigen::Affine> &t, Scalar &roll, Scalar &pitch, Scalar &yaw);
|
|
|
|
/** Extract x,y,z and the Euler angles (XYZ-convention) from the given transformation
|
|
* \param[in] t the input transformation matrix
|
|
* \param[out] x the resulting x translation
|
|
* \param[out] y the resulting y translation
|
|
* \param[out] z the resulting z translation
|
|
* \param[out] roll the resulting roll angle
|
|
* \param[out] pitch the resulting pitch angle
|
|
* \param[out] yaw the resulting yaw angle
|
|
* \ingroup common
|
|
*/
|
|
template <typename Scalar> void
|
|
getTranslationAndEulerAngles (const Eigen::Transform<Scalar, 3, Eigen::Affine> &t,
|
|
Scalar &x, Scalar &y, Scalar &z,
|
|
Scalar &roll, Scalar &pitch, Scalar &yaw);
|
|
|
|
/** \brief Create a transformation from the given translation and Euler angles (XYZ-convention)
|
|
* \param[in] x the input x translation
|
|
* \param[in] y the input y translation
|
|
* \param[in] z the input z translation
|
|
* \param[in] roll the input roll angle
|
|
* \param[in] pitch the input pitch angle
|
|
* \param[in] yaw the input yaw angle
|
|
* \param[out] t the resulting transformation matrix
|
|
* \ingroup common
|
|
*/
|
|
template <typename Scalar> void
|
|
getTransformation (Scalar x, Scalar y, Scalar z, Scalar roll, Scalar pitch, Scalar yaw,
|
|
Eigen::Transform<Scalar, 3, Eigen::Affine> &t);
|
|
|
|
inline void
|
|
getTransformation (float x, float y, float z, float roll, float pitch, float yaw,
|
|
Eigen::Affine3f &t)
|
|
{
|
|
return (getTransformation<float> (x, y, z, roll, pitch, yaw, t));
|
|
}
|
|
|
|
inline void
|
|
getTransformation (double x, double y, double z, double roll, double pitch, double yaw,
|
|
Eigen::Affine3d &t)
|
|
{
|
|
return (getTransformation<double> (x, y, z, roll, pitch, yaw, t));
|
|
}
|
|
|
|
/** \brief Create a transformation from the given translation and Euler angles (XYZ-convention)
|
|
* \param[in] x the input x translation
|
|
* \param[in] y the input y translation
|
|
* \param[in] z the input z translation
|
|
* \param[in] roll the input roll angle
|
|
* \param[in] pitch the input pitch angle
|
|
* \param[in] yaw the input yaw angle
|
|
* \return the resulting transformation matrix
|
|
* \ingroup common
|
|
*/
|
|
inline Eigen::Affine3f
|
|
getTransformation (float x, float y, float z, float roll, float pitch, float yaw)
|
|
{
|
|
Eigen::Affine3f t;
|
|
getTransformation<float> (x, y, z, roll, pitch, yaw, t);
|
|
return (t);
|
|
}
|
|
|
|
/** \brief Write a matrix to an output stream
|
|
* \param[in] matrix the matrix to output
|
|
* \param[out] file the output stream
|
|
* \ingroup common
|
|
*/
|
|
template <typename Derived> void
|
|
saveBinary (const Eigen::MatrixBase<Derived>& matrix, std::ostream& file);
|
|
|
|
/** \brief Read a matrix from an input stream
|
|
* \param[out] matrix the resulting matrix, read from the input stream
|
|
* \param[in,out] file the input stream
|
|
* \ingroup common
|
|
*/
|
|
template <typename Derived> void
|
|
loadBinary (Eigen::MatrixBase<Derived> const& matrix, std::istream& file);
|
|
|
|
// PCL_EIGEN_SIZE_MIN_PREFER_DYNAMIC gives the min between compile-time sizes. 0 has absolute priority, followed by 1,
|
|
// followed by Dynamic, followed by other finite values. The reason for giving Dynamic the priority over
|
|
// finite values is that min(3, Dynamic) should be Dynamic, since that could be anything between 0 and 3.
|
|
#define PCL_EIGEN_SIZE_MIN_PREFER_DYNAMIC(a,b) ((int (a) == 0 || int (b) == 0) ? 0 \
|
|
: (int (a) == 1 || int (b) == 1) ? 1 \
|
|
: (int (a) == Eigen::Dynamic || int (b) == Eigen::Dynamic) ? Eigen::Dynamic \
|
|
: (int (a) <= int (b)) ? int (a) : int (b))
|
|
|
|
/** \brief Returns the transformation between two point sets.
|
|
* The algorithm is based on:
|
|
* "Least-squares estimation of transformation parameters between two point patterns",
|
|
* Shinji Umeyama, PAMI 1991, DOI: 10.1109/34.88573
|
|
*
|
|
* It estimates parameters \f$ c, \mathbf{R}, \f$ and \f$ \mathbf{t} \f$ such that
|
|
* \f{align*}
|
|
* \frac{1}{n} \sum_{i=1}^n \vert\vert y_i - (c\mathbf{R}x_i + \mathbf{t}) \vert\vert_2^2
|
|
* \f}
|
|
* is minimized.
|
|
*
|
|
* The algorithm is based on the analysis of the covariance matrix
|
|
* \f$ \Sigma_{\mathbf{x}\mathbf{y}} \in \mathbb{R}^{d \times d} \f$
|
|
* of the input point sets \f$ \mathbf{x} \f$ and \f$ \mathbf{y} \f$ where
|
|
* \f$d\f$ is corresponding to the dimension (which is typically small).
|
|
* The analysis is involving the SVD having a complexity of \f$O(d^3)\f$
|
|
* though the actual computational effort lies in the covariance
|
|
* matrix computation which has an asymptotic lower bound of \f$O(dm)\f$ when
|
|
* the input point sets have dimension \f$d \times m\f$.
|
|
*
|
|
* \param[in] src Source points \f$ \mathbf{x} = \left( x_1, \hdots, x_n \right) \f$
|
|
* \param[in] dst Destination points \f$ \mathbf{y} = \left( y_1, \hdots, y_n \right) \f$.
|
|
* \param[in] with_scaling Sets \f$ c=1 \f$ when <code>false</code> is passed. (default: false)
|
|
* \return The homogeneous transformation
|
|
* \f{align*}
|
|
* T = \begin{bmatrix} c\mathbf{R} & \mathbf{t} \\ \mathbf{0} & 1 \end{bmatrix}
|
|
* \f}
|
|
* minimizing the resudiual above. This transformation is always returned as an
|
|
* Eigen::Matrix.
|
|
*/
|
|
template <typename Derived, typename OtherDerived>
|
|
typename Eigen::internal::umeyama_transform_matrix_type<Derived, OtherDerived>::type
|
|
umeyama (const Eigen::MatrixBase<Derived>& src, const Eigen::MatrixBase<OtherDerived>& dst, bool with_scaling = false);
|
|
|
|
/** \brief Transform a point using an affine matrix
|
|
* \param[in] point_in the vector to be transformed
|
|
* \param[out] point_out the transformed vector
|
|
* \param[in] transformation the transformation matrix
|
|
*
|
|
* \note Can be used with \c point_in = \c point_out
|
|
*/
|
|
template<typename Scalar> inline void
|
|
transformPoint (const Eigen::Matrix<Scalar, 3, 1> &point_in,
|
|
Eigen::Matrix<Scalar, 3, 1> &point_out,
|
|
const Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation)
|
|
{
|
|
Eigen::Matrix<Scalar, 4, 1> point;
|
|
point << point_in, 1.0;
|
|
point_out = (transformation * point).template head<3> ();
|
|
}
|
|
|
|
/** \brief Transform a vector using an affine matrix
|
|
* \param[in] vector_in the vector to be transformed
|
|
* \param[out] vector_out the transformed vector
|
|
* \param[in] transformation the transformation matrix
|
|
*
|
|
* \note Can be used with \c vector_in = \c vector_out
|
|
*/
|
|
template <typename Scalar> inline void
|
|
transformVector (const Eigen::Matrix<Scalar, 3, 1> &vector_in,
|
|
Eigen::Matrix<Scalar, 3, 1> &vector_out,
|
|
const Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation)
|
|
{
|
|
vector_out = transformation.linear () * vector_in;
|
|
}
|
|
|
|
/** \brief Transform a line using an affine matrix
|
|
* \param[in] line_in the line to be transformed
|
|
* \param[out] line_out the transformed line
|
|
* \param[in] transformation the transformation matrix
|
|
*
|
|
* Lines must be filled in this form:\n
|
|
* line[0-2] = Origin coordinates of the vector\n
|
|
* line[3-5] = Direction vector
|
|
*
|
|
* \note Can be used with \c line_in = \c line_out
|
|
*/
|
|
template <typename Scalar> bool
|
|
transformLine (const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &line_in,
|
|
Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &line_out,
|
|
const Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation);
|
|
|
|
/** \brief Transform plane vectors using an affine matrix
|
|
* \param[in] plane_in the plane coefficients to be transformed
|
|
* \param[out] plane_out the transformed plane coefficients to fill
|
|
* \param[in] transformation the transformation matrix
|
|
*
|
|
* The plane vectors are filled in the form ax+by+cz+d=0
|
|
* Can be used with non Hessian form planes coefficients
|
|
* Can be used with \c plane_in = \c plane_out
|
|
*/
|
|
template <typename Scalar> void
|
|
transformPlane (const Eigen::Matrix<Scalar, 4, 1> &plane_in,
|
|
Eigen::Matrix<Scalar, 4, 1> &plane_out,
|
|
const Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation);
|
|
|
|
/** \brief Transform plane vectors using an affine matrix
|
|
* \param[in] plane_in the plane coefficients to be transformed
|
|
* \param[out] plane_out the transformed plane coefficients to fill
|
|
* \param[in] transformation the transformation matrix
|
|
*
|
|
* The plane vectors are filled in the form ax+by+cz+d=0
|
|
* Can be used with non Hessian form planes coefficients
|
|
* Can be used with \c plane_in = \c plane_out
|
|
* \warning ModelCoefficients stores floats only !
|
|
*/
|
|
template<typename Scalar> void
|
|
transformPlane (const pcl::ModelCoefficients::ConstPtr plane_in,
|
|
pcl::ModelCoefficients::Ptr plane_out,
|
|
const Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation);
|
|
|
|
/** \brief Check coordinate system integrity
|
|
* \param[in] line_x the first axis
|
|
* \param[in] line_y the second axis
|
|
* \param[in] norm_limit the limit to ignore norm rounding errors
|
|
* \param[in] dot_limit the limit to ignore dot product rounding errors
|
|
* \return True if the coordinate system is consistent, false otherwise.
|
|
*
|
|
* Lines must be filled in this form:\n
|
|
* line[0-2] = Origin coordinates of the vector\n
|
|
* line[3-5] = Direction vector
|
|
*
|
|
* Can be used like this :\n
|
|
* line_x = X axis and line_y = Y axis\n
|
|
* line_x = Z axis and line_y = X axis\n
|
|
* line_x = Y axis and line_y = Z axis\n
|
|
* Because X^Y = Z, Z^X = Y and Y^Z = X.
|
|
* Do NOT invert line order !
|
|
*
|
|
* Determine whether a coordinate system is consistent or not by checking :\n
|
|
* Line origins: They must be the same for the 2 lines\n
|
|
* Norm: The 2 lines must be normalized\n
|
|
* Dot products: Must be 0 or perpendicular vectors
|
|
*/
|
|
template<typename Scalar> bool
|
|
checkCoordinateSystem (const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &line_x,
|
|
const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &line_y,
|
|
const Scalar norm_limit = 1e-3,
|
|
const Scalar dot_limit = 1e-3);
|
|
|
|
/** \brief Check coordinate system integrity
|
|
* \param[in] origin the origin of the coordinate system
|
|
* \param[in] x_direction the first axis
|
|
* \param[in] y_direction the second axis
|
|
* \param[in] norm_limit the limit to ignore norm rounding errors
|
|
* \param[in] dot_limit the limit to ignore dot product rounding errors
|
|
* \return True if the coordinate system is consistent, false otherwise.
|
|
*
|
|
* Read the other variant for more information
|
|
*/
|
|
template <typename Scalar> inline bool
|
|
checkCoordinateSystem (const Eigen::Matrix<Scalar, 3, 1> &origin,
|
|
const Eigen::Matrix<Scalar, 3, 1> &x_direction,
|
|
const Eigen::Matrix<Scalar, 3, 1> &y_direction,
|
|
const Scalar norm_limit = 1e-3,
|
|
const Scalar dot_limit = 1e-3)
|
|
{
|
|
Eigen::Matrix<Scalar, Eigen::Dynamic, 1> line_x;
|
|
Eigen::Matrix<Scalar, Eigen::Dynamic, 1> line_y;
|
|
line_x << origin, x_direction;
|
|
line_y << origin, y_direction;
|
|
return (checkCoordinateSystem<Scalar> (line_x, line_y, norm_limit, dot_limit));
|
|
}
|
|
|
|
inline bool
|
|
checkCoordinateSystem (const Eigen::Matrix<double, 3, 1> &origin,
|
|
const Eigen::Matrix<double, 3, 1> &x_direction,
|
|
const Eigen::Matrix<double, 3, 1> &y_direction,
|
|
const double norm_limit = 1e-3,
|
|
const double dot_limit = 1e-3)
|
|
{
|
|
Eigen::Matrix<double, Eigen::Dynamic, 1> line_x;
|
|
Eigen::Matrix<double, Eigen::Dynamic, 1> line_y;
|
|
line_x.resize (6);
|
|
line_y.resize (6);
|
|
line_x << origin, x_direction;
|
|
line_y << origin, y_direction;
|
|
return (checkCoordinateSystem<double> (line_x, line_y, norm_limit, dot_limit));
|
|
}
|
|
|
|
inline bool
|
|
checkCoordinateSystem (const Eigen::Matrix<float, 3, 1> &origin,
|
|
const Eigen::Matrix<float, 3, 1> &x_direction,
|
|
const Eigen::Matrix<float, 3, 1> &y_direction,
|
|
const float norm_limit = 1e-3,
|
|
const float dot_limit = 1e-3)
|
|
{
|
|
Eigen::Matrix<float, Eigen::Dynamic, 1> line_x;
|
|
Eigen::Matrix<float, Eigen::Dynamic, 1> line_y;
|
|
line_x.resize (6);
|
|
line_y.resize (6);
|
|
line_x << origin, x_direction;
|
|
line_y << origin, y_direction;
|
|
return (checkCoordinateSystem<float> (line_x, line_y, norm_limit, dot_limit));
|
|
}
|
|
|
|
/** \brief Compute the transformation between two coordinate systems
|
|
* \param[in] from_line_x X axis from the origin coordinate system
|
|
* \param[in] from_line_y Y axis from the origin coordinate system
|
|
* \param[in] to_line_x X axis from the destination coordinate system
|
|
* \param[in] to_line_y Y axis from the destination coordinate system
|
|
* \param[out] transformation the transformation matrix to fill
|
|
* \return true if transformation was filled, false otherwise.
|
|
*
|
|
* Line must be filled in this form:\n
|
|
* line[0-2] = Coordinate system origin coordinates \n
|
|
* line[3-5] = Direction vector (norm doesn't matter)
|
|
*/
|
|
template <typename Scalar> bool
|
|
transformBetween2CoordinateSystems (const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> from_line_x,
|
|
const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> from_line_y,
|
|
const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> to_line_x,
|
|
const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> to_line_y,
|
|
Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation);
|
|
|
|
}
|
|
|
|
#include <pcl/common/impl/eigen.hpp>
|
|
|
|
#if defined __SUNPRO_CC
|
|
# pragma enable_warn
|
|
#endif
|