namespace mrpt::math
Overview
Math and geometry.
namespace math { // namespaces namespace mrpt::math::detail; namespace mrpt::math::internal; // typedefs typedef CMatrixFixed<double, 2, 2> CMatrixDouble22; typedef CMatrixFixed<double, 2, 3> CMatrixDouble23; typedef CMatrixFixed<double, 3, 2> CMatrixDouble32; typedef CMatrixFixed<double, 3, 3> CMatrixDouble33; typedef CMatrixFixed<double, 4, 4> CMatrixDouble44; typedef CMatrixFixed<double, 6, 6> CMatrixDouble66; typedef CMatrixFixed<double, 7, 7> CMatrixDouble77; typedef CMatrixFixed<double, 1, 3> CMatrixDouble13; typedef CMatrixFixed<double, 3, 1> CMatrixDouble31; typedef CMatrixFixed<double, 3, 4> CMatrixDouble34; typedef CMatrixFixed<double, 3, 6> CMatrixDouble36; typedef CMatrixFixed<double, 1, 2> CMatrixDouble12; typedef CMatrixFixed<double, 2, 1> CMatrixDouble21; typedef CMatrixFixed<double, 6, 1> CMatrixDouble61; typedef CMatrixFixed<double, 1, 6> CMatrixDouble16; typedef CMatrixFixed<double, 7, 1> CMatrixDouble71; typedef CMatrixFixed<double, 1, 7> CMatrixDouble17; typedef CMatrixFixed<double, 5, 1> CMatrixDouble51; typedef CMatrixFixed<double, 1, 5> CMatrixDouble15; typedef CMatrixFixed<double, 4, 1> CMatrixDouble41; typedef CMatrixFixed<double, 4, 3> CMatrixDouble43; typedef CMatrixFixed<double, 6, 12> CMatrixDouble6_12; typedef CMatrixFixed<double, 12, 6> CMatrixDouble12_6; typedef CMatrixFixed<double, 3, 9> CMatrixDouble39; typedef CMatrixFixed<double, 9, 3> CMatrixDouble93; typedef CMatrixFixed<float, 2, 2> CMatrixFloat22; typedef CMatrixFixed<float, 2, 3> CMatrixFloat23; typedef CMatrixFixed<float, 3, 2> CMatrixFloat32; typedef CMatrixFixed<float, 3, 3> CMatrixFloat33; typedef CMatrixFixed<float, 4, 4> CMatrixFloat44; typedef CMatrixFixed<float, 6, 6> CMatrixFloat66; typedef CMatrixFixed<float, 7, 7> CMatrixFloat77; typedef CMatrixFixed<float, 1, 3> CMatrixFloat13; typedef CMatrixFixed<float, 3, 1> CMatrixFloat31; typedef CMatrixFixed<float, 1, 2> CMatrixFloat12; typedef CMatrixFixed<float, 2, 1> CMatrixFloat21; typedef CMatrixFixed<float, 6, 1> CMatrixFloat61; typedef CMatrixFixed<float, 1, 6> CMatrixFloat16; typedef CMatrixFixed<float, 7, 1> CMatrixFloat71; typedef CMatrixFixed<float, 1, 7> CMatrixFloat17; typedef CMatrixFixed<float, 5, 1> CMatrixFloat51; typedef CMatrixFixed<float, 1, 5> CMatrixFloat15; typedef CLevenbergMarquardtTempl<mrpt::math::CVectorDouble> CLevenbergMarquardt; typedef CMatrixDynamic<bool> CMatrixBool; typedef CMatrixDynamic<float> CMatrixFloat; typedef CMatrixDynamic<double> CMatrixDouble; typedef CMatrixDynamic<unsigned int> CMatrixUInt; typedef CMatrixDynamic<uint8_t> CMatrix_u8; typedef CMatrixDynamic<uint16_t> CMatrix_u16; typedef CMatrixDynamic<double> CMatrixLongDouble; typedef CQuaternion<double> CQuaternionDouble; typedef CQuaternion<float> CQuaternionFloat; typedef CVectorDynamic<float> CVectorFloat; typedef CVectorDynamic<double> CVectorDouble; typedef CMatrixFixed<T, N, 1> CVectorFixed; typedef CVectorFixed<float, N> CVectorFixedFloat; typedef CVectorFixed<double, N> CVectorFixedDouble; typedef long int matrix_index_t; typedef long int matrix_dim_t; typedef RANSAC_Template<double> RANSAC; typedef TBoundingBox_<double> TBoundingBox; typedef TBoundingBox_<float> TBoundingBoxf; typedef TOrientedBox_<double> TOrientedBox; typedef TOrientedBox_<float> TOrientedBoxf; typedef TPlane TPlane3D; typedef TPoint2D_<double> TPoint2D; typedef TPoint2D_<float> TPoint2Df; typedef TPoint2D TVector2D; typedef TPoint2Df TVector2Df; typedef TPoint3D_<double> TPoint3D; typedef TPoint3D_<float> TPoint3Df; typedef TPoint3D TVector3D; typedef TPoint3Df TVector3Df; // enums enum GeometricEntity; enum TConstructorFlags_Matrices; enum TConstructorFlags_Quaternions; enum TMatrixTextFileFormat; enum TRobustKernelType; // structs template <typename Scalar, typename Derived> struct ContainerType<mrpt::math::MatrixVectorBase<Scalar, Derived>>; template <typename CONTAINER> struct ContainerType; struct LowPassFilter_IIR1; template < typename Scalar, int NROWS, int NCOLS, typename INFO, bool HAS_REMAP, typename INDEX_REMAP_MAP_IMPL = mrpt::containers::map_as_vector<size_t, size_t> > struct MatrixBlockSparseCols; template <typename T> struct RobustKernel<rkPseudoHuber, T>; template <TRobustKernelType KERNEL_TYPE, typename T = double> struct RobustKernel; template <typename T> struct RobustKernel<rkLeastSquares, T>; template <typename T> struct TBoundingBox_; struct TLine2D; struct TLine3D; struct TObject2D; struct TObject3D; struct TPlane; template <typename T> struct TPoint2D_; template <typename T> struct TPoint2D_data; template <typename T> struct TPoint3D_; template <typename T> struct TPoint3D_data; struct TPointXYZIu8; struct TPointXYZRGBAf; struct TPointXYZRGBu8; struct TPointXYZfIu8; struct TPointXYZfRGBAu8; struct TPointXYZfRGBu8; struct TPose2D; struct TPose3D; struct TPose3DQuat; struct TPoseOrPoint; struct TSegment2D; struct TSegment3D; struct TTwist2D; struct TTwist3D; struct matrix_size_t; // classes class CAtan2LookUpTable; class CAtan2LookUpTableMultiRes; template <typename T, typename U, bool UIsObject = false> class CBinaryRelation; template <class MAT> class CConstMatrixColumnAccessor; template <typename MAT> class CConstMatrixColumnAccessorExtended; template <class MAT> class CConstMatrixRowAccessor; template <class MAT> class CConstMatrixRowAccessorExtended; class CHistogram; template <typename VECTORTYPE = CVectorDouble, class USERPARAM = VECTORTYPE> class CLevenbergMarquardtTempl; class CMatrixB; template <typename MAT> class CMatrixColumnAccessor; template <typename MAT> class CMatrixColumnAccessorExtended; class CMatrixD; template <class T> class CMatrixDynamic; class CMatrixF; template <typename T, matrix_dim_t ROWS, matrix_dim_t COLS> class CMatrixFixed; template <typename MAT> class CMatrixRowAccessor; template <class MAT> class CMatrixRowAccessorExtended; template <typename T, typename NUM, typename OTHER> class CMonteCarlo; class CPolygon; template <class TDATA, size_t STATE_LEN> class CProbabilityDensityFunction; template <class T> class CQuaternion; template <class T> class CSparseMatrixTemplate; template <class T> class CSparseSymmetricalMatrix; class CSplineInterpolator1D; template <class T> class CVectorDynamic; template <class T, int N> class FAddPoint; template < class Derived, typename num_t = float, typename metric_t = nanoflann::L2_Simple_Adaptor<num_t, Derived> > class KDTreeCapable; template <typename Scalar, class Derived> class MatrixBase; template <typename Scalar, class Derived> class MatrixVectorBase; class ModelSearch; template < typename NUMTYPE = double, typename DATASET = CMatrixDynamic<NUMTYPE>, typename MODEL = CMatrixDynamic<NUMTYPE> > class RANSAC_Template; template <typename T> class TOrientedBox_; class TPolygon2D; class TPolygon3D; class TPolygonWithPlane; // global functions template <class VECTORLIKE1, class VECTORLIKE2, class MAT> MAT::Scalar mahalanobisDistanceSq( const VECTORLIKE1& X, const VECTORLIKE2& MU, const MAT& COV ); template <class VECTORLIKE1, class VECTORLIKE2, class MAT> VECTORLIKE1::Scalar mahalanobisDistance( const VECTORLIKE1& X, const VECTORLIKE2& MU, const MAT& COV ); template <class VECTORLIKE, class MAT1, class MAT2, class MAT3> MAT1::Scalar mahalanobisDistanceSq( const VECTORLIKE& mean_diffs, const MAT1& COV1, const MAT2& COV2, const MAT3& CROSS_COV12 ); template <class VECTORLIKE, class MAT1, class MAT2, class MAT3> VECTORLIKE::Scalar mahalanobisDistance( const VECTORLIKE& mean_diffs, const MAT1& COV1, const MAT2& COV2, const MAT3& CROSS_COV12 ); template <class VECTORLIKE, class MATRIXLIKE> MATRIXLIKE::Scalar mahalanobisDistanceSq( const VECTORLIKE& delta_mu, const MATRIXLIKE& cov ); template <class VECTORLIKE, class MATRIXLIKE> MATRIXLIKE::Scalar mahalanobisDistance( const VECTORLIKE& delta_mu, const MATRIXLIKE& cov ); template <typename T> T productIntegralTwoGaussians( const std::vector<T>& mean_diffs, const CMatrixDynamic<T>& COV1, const CMatrixDynamic<T>& COV2 ); template <typename T, size_t DIM> T productIntegralTwoGaussians( const std::vector<T>& mean_diffs, const CMatrixFixed<T, DIM, DIM>& COV1, const CMatrixFixed<T, DIM, DIM>& COV2 ); template <typename T, class VECLIKE, class MATLIKE1, class MATLIKE2> void productIntegralAndMahalanobisTwoGaussians( const VECLIKE& mean_diffs, const MATLIKE1& COV1, const MATLIKE2& COV2, T& maha2_out, T& intprod_out, const MATLIKE1* CROSS_COV12 = nullptr ); template <typename T, class VECLIKE, class MATRIXLIKE> void mahalanobisDistanceSqAndLogPDF( const VECLIKE& diff_mean, const MATRIXLIKE& cov, T& maha2_out, T& log_pdf_out ); template <typename T, class VECLIKE, class MATRIXLIKE> void mahalanobisDistanceSqAndPDF( const VECLIKE& diff_mean, const MATRIXLIKE& cov, T& maha2_out, T& pdf_out ); template < class VECTOR_OF_VECTORS, class MATRIXLIKE, class VECTORLIKE, class VECTORLIKE2, class VECTORLIKE3 > void covariancesAndMeanWeighted( const VECTOR_OF_VECTORS& elements, MATRIXLIKE& covariances, VECTORLIKE& means, const VECTORLIKE2* weights_mean, const VECTORLIKE3* weights_cov, const bool* elem_do_wrap2pi = nullptr ); template <class VECTOR_OF_VECTORS, class MATRIXLIKE, class VECTORLIKE> void covariancesAndMean( const VECTOR_OF_VECTORS& elements, MATRIXLIKE& covariances, VECTORLIKE& means, const bool* elem_do_wrap2pi = nullptr ); template <class VECTORLIKE1, class VECTORLIKE2> void weightedHistogram( const VECTORLIKE1& values, const VECTORLIKE1& weights, float binWidth, VECTORLIKE2& out_binCenters, VECTORLIKE2& out_binValues ); template <class VECTORLIKE1, class VECTORLIKE2> void weightedHistogramLog( const VECTORLIKE1& values, const VECTORLIKE1& log_weights, float binWidth, VECTORLIKE2& out_binCenters, VECTORLIKE2& out_binValues ); double averageLogLikelihood(const CVectorDouble& logLikelihoods); double averageWrap2Pi(const CVectorDouble& angles); double averageLogLikelihood(const CVectorDouble& logWeights, const CVectorDouble& logLikelihoods); bool intersect(const TSegment3D& s1, const TSegment3D& s2, TObject3D& obj); bool intersect(const TSegment3D& s1, const TPlane& p2, TObject3D& obj); bool intersect(const TSegment3D& s1, const TLine3D& r2, TObject3D& obj); bool intersect(const TPlane& p1, const TSegment3D& s2, TObject3D& obj); bool intersect(const TPlane& p1, const TPlane& p2, TObject3D& obj); bool intersect(const TPlane& p1, const TLine3D& p2, TObject3D& obj); bool intersect(const TLine3D& r1, const TSegment3D& s2, TObject3D& obj); bool intersect(const TLine3D& r1, const TPlane& p2, TObject3D& obj); bool intersect(const TLine3D& r1, const TLine3D& r2, TObject3D& obj); bool intersect(const TLine2D& r1, const TLine2D& r2, TObject2D& obj); bool intersect(const TLine2D& r1, const TSegment2D& s2, TObject2D& obj); bool intersect(const TSegment2D& s1, const TLine2D& r2, TObject2D& obj); bool intersect(const TSegment2D& s1, const TSegment2D& s2, TObject2D& obj); double getAngle(const TPlane& p1, const TPlane& p2); double getAngle(const TPlane& p1, const TLine3D& r2); double getAngle(const TLine3D& r1, const TPlane& p2); double getAngle(const TLine3D& r1, const TLine3D& r2); double getAngle(const TLine2D& r1, const TLine2D& r2); bool conformAPlane(const std::vector<TPoint3D>& points); bool conformAPlane(const std::vector<TPoint3D>& points, TPlane& p); bool areAligned(const std::vector<TPoint2D>& points); bool areAligned(const std::vector<TPoint2D>& points, TLine2D& r); bool areAligned(const std::vector<TPoint3D>& points); bool areAligned(const std::vector<TPoint3D>& points, TLine3D& r); void project3D(const TPoint3D& point, const mrpt::math::TPose3D& newXYpose, TPoint3D& newPoint); void project3D(const TSegment3D& segment, const mrpt::math::TPose3D& newXYpose, TSegment3D& newSegment); void project3D(const TLine3D& line, const mrpt::math::TPose3D& newXYpose, TLine3D& newLine); void project3D(const TPlane& plane, const mrpt::math::TPose3D& newXYpose, TPlane& newPlane); void project3D(const TPolygon3D& polygon, const mrpt::math::TPose3D& newXYpose, TPolygon3D& newPolygon); void project3D(const TObject3D& object, const mrpt::math::TPose3D& newXYPose, TObject3D& newObject); template <typename Object> Object project3D(const Object& o, const mrpt::math::TPose3D& newPose); template <class T> void project3D(const T& obj, const TPlane& newXYPlane, T& newObj); template <class T> void project3D( const T& obj, const TPlane& newXYPlane, ] const TPoint3D& newOrigin, T& newObj ); template <class T> void project3D( const std::vector<T>& objs, const mrpt::math::TPose3D& newXYpose, std::vector<T>& newObjs ); void project2D(const TPoint2D& point, const TPose2D& newXpose, TPoint2D& newPoint); void project2D(const TSegment2D& segment, const TPose2D& newXpose, TSegment2D& newSegment); void project2D(const TLine2D& line, const TPose2D& newXpose, TLine2D& newLine); void project2D(const TPolygon2D& polygon, const TPose2D& newXpose, TPolygon2D& newPolygon); void project2D(const TObject2D& object, const TPose2D& newXpose, TObject2D& newObject); template <typename Object> Object project2D(const Object& o, const mrpt::math::TPose2D& newPose); template <class T, class CPOSE2D> void project2D( const T& obj, const TLine2D& newXLine, T& newObj ); template <class T, class CPOSE2D> void project2D( const T& obj, const TLine2D& newXLine, const TPoint2D& newOrigin, T& newObj ); template <class T> void project2D( const std::vector<T>& objs, const TPose2D& newXpose, std::vector<T>& newObjs ); bool intersect(const TPolygon2D& p1, const TSegment2D& s2, TObject2D& obj); bool intersect(const TPolygon2D& p1, const TLine2D& r2, TObject2D& obj); TPolygon2D intersect(const TPolygon2D& subject, const TPolygon2D& clipping); bool intersect( const TPolygon2D& subject, const TPolygon2D& clipping, TObject2D& result ); bool intersect(const TSegment2D& s1, const TPolygon2D& p2, TObject2D& obj); bool intersect(const TLine2D& r1, const TPolygon2D& p2, TObject2D& obj); bool intersect(const TPolygon3D& p1, const TSegment3D& s2, TObject3D& obj); bool intersect(const TPolygon3D& p1, const TLine3D& r2, TObject3D& obj); bool intersect(const TPolygon3D& p1, const TPlane& p2, TObject3D& obj); bool intersect(const TPolygon3D& p1, const TPolygon3D& p2, TObject3D& obj); bool intersect(const TSegment3D& s1, const TPolygon3D& p2, TObject3D& obj); bool intersect(const TLine3D& r1, const TPolygon3D& p2, TObject3D& obj); bool intersect(const TPlane& p1, const TPolygon3D& p2, TObject3D& obj); size_t intersect(const std::vector<TPolygon3D>& v1, const std::vector<TPolygon3D>& v2, CSparseMatrixTemplate<TObject3D>& objs); size_t intersect(const std::vector<TPolygon3D>& v1, const std::vector<TPolygon3D>& v2, std::vector<TObject3D>& objs); template <class T, class U, class O> size_t intersect( const std::vector<T>& v1, const std::vector<U>& v2, CSparseMatrixTemplate<O>& objs ); template <class T, class U, class O> size_t intersect( const std::vector<T>& v1, const std::vector<U>& v2, std::vector<O> objs ); bool intersect(const TObject2D& o1, const TObject2D& o2, TObject2D& obj); bool intersect(const TObject3D& o1, const TObject3D& o2, TObject3D& obj); double distance(const TPoint2D& p1, const TPoint2D& p2); double distance(const TPoint3D& p1, const TPoint3D& p2); double distance(const TLine2D& r1, const TLine2D& r2); double distance(const TLine3D& r1, const TLine3D& r2); double distance(const TPlane& p1, const TPlane& p2); double distance(const TPolygon2D& p1, const TPolygon2D& p2); double distance(const TPolygon2D& p1, const TSegment2D& s2); double distance(const TSegment2D& s1, const TPolygon2D& p2); double distance(const TPolygon2D& p1, const TLine2D& l2); double distance( const TLine2D& l1, const TPolygon2D& p2 ); double distance(const TPolygon3D& p1, const TPolygon3D& p2); double distance(const TPolygon3D& p1, const TSegment3D& s2); double distance(const TSegment3D& s1, const TPolygon3D& p2); double distance(const TPolygon3D& p1, const TLine3D& l2); double distance(const TLine3D& l1, const TPolygon3D& p2); double distance(const TPolygon3D& po, const TPlane& pl); double distance(const TPlane& pl, const TPolygon3D& po); void getRectangleBounds(const std::vector<TPoint2D>& poly, TPoint2D& pMin, TPoint2D& pMax); void getPrismBounds(const std::vector<TPoint3D>& poly, TPoint3D& pMin, TPoint3D& pMax); void createPlaneFromPoseXY(const mrpt::math::TPose3D& pose, TPlane& plane); void createPlaneFromPoseXZ(const mrpt::math::TPose3D& pose, TPlane& plane); void createPlaneFromPoseYZ(const mrpt::math::TPose3D& pose, TPlane& plane); void createPlaneFromPoseAndNormal(const mrpt::math::TPose3D& pose, const double(&) normal [3], TPlane& plane); CMatrixDouble44 generateAxisBaseFromDirectionAndAxis(const mrpt::math::TVector3D& vec, uint8_t coord); double getRegressionLine(const std::vector<TPoint2D>& points, TLine2D& line); double getRegressionLine(const std::vector<TPoint3D>& points, TLine3D& line); double getRegressionPlane(const std::vector<TPoint3D>& points, TPlane& plane); void assemblePolygons(const std::vector<TSegment3D>& segms, std::vector<TPolygon3D>& polys); void assemblePolygons( const std::vector<TSegment3D>& segms, std::vector<TPolygon3D>& polys, std::vector<TSegment3D>& remainder ); void assemblePolygons(const std::vector<TObject3D>& objs, std::vector<TPolygon3D>& polys); void assemblePolygons(const std::vector<TObject3D>& objs, std::vector<TPolygon3D>& polys, std::vector<TObject3D>& remainder); void assemblePolygons( const std::vector<TObject3D>& objs, std::vector<TPolygon3D>& polys, std::vector<TSegment3D>& remainder1, std::vector<TObject3D>& remainder2 ); bool splitInConvexComponents(const TPolygon2D& poly, std::vector<TPolygon2D>& components); bool splitInConvexComponents(const TPolygon3D& poly, std::vector<TPolygon3D>& components); void getSegmentBisector(const TSegment2D& sgm, TLine2D& bis); void getSegmentBisector(const TSegment3D& sgm, TPlane& bis); void getAngleBisector(const TLine2D& l1, const TLine2D& l2, TLine2D& bis); void getAngleBisector(const TLine3D& l1, const TLine3D& l2, TLine3D& bis); bool traceRay(const std::vector<TPolygonWithPlane>& vec, const mrpt::math::TPose3D& pose, double& dist); bool traceRay(const std::vector<TPolygon3D>& vec, const mrpt::math::TPose3D& pose, double& dist); template <class T, class U, class V> void crossProduct3D(const T& v0, const U& v1, V& vOut); template <class T> void crossProduct3D( const std::vector<T>& v0, const std::vector<T>& v1, std::vector<T>& v_out ); template <class VEC1, class VEC2> VEC1 crossProduct3D(const VEC1& v0, const VEC2& v1); template <class VECTOR, class MATRIX> void skew_symmetric3(const VECTOR& v, MATRIX& M); template <class VECTOR> mrpt::math::CMatrixDouble33 skew_symmetric3(const VECTOR& v); template <class VECTOR, class MATRIX> void skew_symmetric3_neg(const VECTOR& v, MATRIX& M); template <class VECTOR> mrpt::math::CMatrixDouble33 skew_symmetric3_neg(const VECTOR& v); template <class T, class U> bool vectorsAreParallel2D(const T& v1, const U& v2); template <class T, class U> bool vectorsAreParallel3D(const T& v1, const U& v2); void closestFromPointToSegment( double Px, double Py, double x1, double y1, double x2, double y2, double& out_x, double& out_y ); mrpt::math::TPoint2D closestFromPointToSegment( const mrpt::math::TPoint2D& query, const mrpt::math::TPoint2D& segPt1, const mrpt::math::TPoint2D& segPt2 ); void closestFromPointToLine( double Px, double Py, double x1, double y1, double x2, double y2, double& out_x, double& out_y ); mrpt::math::TPoint2D closestFromPointToLine( const mrpt::math::TPoint2D& query, const mrpt::math::TPoint2D& linePt1, const mrpt::math::TPoint2D& linePt2 ); double squaredDistancePointToLine(double Px, double Py, double x1, double y1, double x2, double y2); double squaredDistancePointToLine( const mrpt::math::TPoint2D& query, const mrpt::math::TPoint2D& linePt1, const mrpt::math::TPoint2D& linePt2 ); template <typename T> T distanceBetweenPoints(const T x1, const T y1, const T x2, const T y2); template <typename T> T distanceBetweenPoints( const T x1, const T y1, const T z1, const T x2, const T y2, const T z2 ); template <typename T> T distanceSqrBetweenPoints(const T x1, const T y1, const T x2, const T y2); template <typename T> T distanceSqrBetweenPoints( const T x1, const T y1, const T z1, const T x2, const T y2, const T z2 ); template <typename T> double minimumDistanceFromPointToSegment( const double Px, const double Py, const double x1, const double y1, const double x2, const double y2, T& out_x, T& out_y ); template <typename T> bool pointIntoQuadrangle( T x, T y, T v1x, T v1y, T v2x, T v2y, T v3x, T v3y, T v4x, T v4y ); bool RectanglesIntersection( double R1_x_min, double R1_x_max, double R1_y_min, double R1_y_max, double R2_x_min, double R2_x_max, double R2_y_min, double R2_y_max, double R2_pose_x, double R2_pose_y, double R2_pose_phi ); CMatrixDouble33 generateAxisBaseFromDirection(double dx, double dy, double dz); double signedArea(const mrpt::math::TPolygon2D& p); template <mrpt::math::matrix_dim_t NROWS, mrpt::math::matrix_dim_t NCOLS> mrpt::serialization::CArchive& operator >> ( mrpt::serialization::CArchive& in, CMatrixFixed<float, NROWS, NCOLS>& M ); template <mrpt::math::matrix_dim_t NROWS, mrpt::math::matrix_dim_t NCOLS> mrpt::serialization::CArchive& operator >> ( mrpt::serialization::CArchive& in, CMatrixFixed<double, NROWS, NCOLS>& M ); template <mrpt::math::matrix_dim_t NROWS, mrpt::math::matrix_dim_t NCOLS> mrpt::serialization::CArchive& operator << ( mrpt::serialization::CArchive& out, const CMatrixFixed<float, NROWS, NCOLS>& M ); template <mrpt::math::matrix_dim_t NROWS, mrpt::math::matrix_dim_t NCOLS> mrpt::serialization::CArchive& operator << ( mrpt::serialization::CArchive& out, const CMatrixFixed<double, NROWS, NCOLS>& M ); template <typename MAT> void deserializeSymmetricMatrixFrom(MAT& m, mrpt::serialization::CArchive& in); template <typename MAT> void serializeSymmetricMatrixTo(MAT& m, mrpt::serialization::CArchive& out); template <class CONTAINER, typename VALUE> VALUE squareNorm_accum( const VALUE total, const CONTAINER& v ); template <size_t N, class T, class U> T squareNorm(const U& v); template <class CONTAINER1, class CONTAINER2> CONTAINER1::Scalar dotProduct( const CONTAINER1& v1, const CONTAINER1& v2 ); template <size_t N, class T, class U, class V> T dotProduct(const U& v1, const V& v2); template <class CONTAINER> CONTAINER::Scalar sum(const CONTAINER& v); template <typename T> T sum(const std::vector<T>& v); template <class CONTAINER, typename RET> RET sumRetType(const CONTAINER& v); template <class CONTAINER> double mean(const CONTAINER& v); template <typename T, typename Alloc> void minimum_maximum( const std::vector<T, Alloc>& V, T& curMin, T& curMax ); template <class Derived> void minimum_maximum( const Eigen::MatrixBase<Derived>& V, typename Eigen::MatrixBase<Derived>::Scalar& curMin, typename Eigen::MatrixBase<Derived>::Scalar& curMax ); template <class CONTAINER, typename Scalar> void normalize( CONTAINER& c, Scalar valMin, Scalar valMax ); template <class CONTAINER1, class CONTAINER2> size_t countCommonElements( const CONTAINER1& a, const CONTAINER2& b ); template <class CONTAINER> void adjustRange( CONTAINER& m, const typename CONTAINER::Scalar minVal, const typename CONTAINER::Scalar maxVal ); template <class VECTORLIKE> void meanAndStd( const VECTORLIKE& v, double& out_mean, double& out_std, bool unbiased = true ); template <class VECTORLIKE> double stddev(const VECTORLIKE& v, bool unbiased = true); template <class VECTOR_OF_VECTOR, class VECTORLIKE, class MATRIXLIKE> void meanAndCovVec( const VECTOR_OF_VECTOR& v, VECTORLIKE& out_mean, MATRIXLIKE& out_cov ); template <class VECTOR_OF_VECTOR, class RETURN_MATRIX> RETURN_MATRIX covVector(const VECTOR_OF_VECTOR& v); template <class CONT1, class CONT2> double ncc_vector(const CONT1& a, const CONT2& b); template <class VECTOR> VECTOR xcorr( const VECTOR& a, const VECTOR& b, const size_t maxLag, bool normalized = true ); template <typename T1, typename T2> std::vector<T1>& operator *= ( std::vector<T1>& a, const std::vector<T2>& b ); template <typename T1> std::vector<T1>& operator *= (std::vector<T1>& a, const T1 b); template <typename T1, typename T2> std::vector<T1> operator * ( const std::vector<T1>& a, const std::vector<T2>& b ); template <typename T1, typename T2> std::vector<T1>& operator += ( std::vector<T1>& a, const std::vector<T2>& b ); template <typename T1> std::vector<T1>& operator += (std::vector<T1>& a, const T1 b); template <typename T1, typename T2> std::vector<T1> operator + ( const std::vector<T1>& a, const std::vector<T2>& b ); template <typename T1, typename T2> std::vector<T1> operator - ( const std::vector<T1>& v1, const std::vector<T2>& v2 ); template <class CONTAINER, class POINT_OR_POSE> CONTAINER& containerFromPoseOrPoint( CONTAINER& C, const POINT_OR_POSE& p ); template <typename NUMTYPE> void ransac_detect_3D_planes( const CVectorDynamic<NUMTYPE>& x, const CVectorDynamic<NUMTYPE>& y, const CVectorDynamic<NUMTYPE>& z, std::vector<std::pair<size_t, TPlane>>& out_detected_planes, const double threshold, size_t min_inliers_for_valid_plane = 10 ); template <typename NUMTYPE> void ransac_detect_2D_lines( const CVectorDynamic<NUMTYPE>& x, const CVectorDynamic<NUMTYPE>& y, std::vector<std::pair<size_t, TLine2D>>& out_detected_lines, const double threshold, size_t min_inliers_for_valid_line = 5 ); template <class POINTSMAP> void ransac_detect_3D_planes( const POINTSMAP* points_map, std::vector<std::pair<size_t, TPlane>>& out_detected_planes, const double threshold, size_t min_inliers_for_valid_plane ); template <typename T> void slerp( const CQuaternion<T>& q0, const CQuaternion<T>& q1, const double t, CQuaternion<T>& q ); void slerp(const TPose3D& q0, const TPose3D& q1, const double t, TPose3D& p); void slerp_ypr( const mrpt::math::TPose3D& q0, const mrpt::math::TPose3D& q1, const double t, mrpt::math::TPose3D& p ); mrpt::serialization::CArchive& operator >> ( mrpt::serialization::CArchive& in, CMatrixD::Ptr& pObj ); mrpt::serialization::CArchive& operator << ( mrpt::serialization::CArchive& s, const CVectorFloat& a ); mrpt::serialization::CArchive& operator << ( mrpt::serialization::CArchive& s, const CVectorDouble& a ); mrpt::serialization::CArchive& operator >> ( mrpt::serialization::CArchive& in, CVectorDouble& a ); mrpt::serialization::CArchive& operator >> ( mrpt::serialization::CArchive& in, CVectorFloat& a ); double normalPDF(double x, double mu, double std); template <class VECTORLIKE1, class VECTORLIKE2, class MATRIXLIKE> MATRIXLIKE::Scalar normalPDFInf( const VECTORLIKE1& x, const VECTORLIKE2& mu, const MATRIXLIKE& cov_inv, const bool scaled_pdf = false ); template <class VECTORLIKE1, class VECTORLIKE2, class MATRIXLIKE> MATRIXLIKE::Scalar normalPDF( const VECTORLIKE1& x, const VECTORLIKE2& mu, const MATRIXLIKE& cov, const bool scaled_pdf = false ); template <typename VECTORLIKE, typename MATRIXLIKE> MATRIXLIKE::Scalar normalPDF( const VECTORLIKE& d, const MATRIXLIKE& cov ); template < typename VECTORLIKE1, typename MATRIXLIKE1, typename VECTORLIKE2, typename MATRIXLIKE2 > double KLD_Gaussians( const VECTORLIKE1& mu0, const MATRIXLIKE1& cov0, const VECTORLIKE2& mu1, const MATRIXLIKE2& cov1 ); double normalQuantile(double p); double normalCDF(double p); double chi2inv(double P, unsigned int dim = 1); double noncentralChi2CDF(unsigned int degreesOfFreedom, double noncentrality, double arg); double chi2CDF(unsigned int degreesOfFreedom, double arg); double chi2PDF(unsigned int degreesOfFreedom, double arg, double accuracy = 1e-7); std::pair<double, double> noncentralChi2PDF_CDF( unsigned int degreesOfFreedom, double noncentrality, double arg, double eps = 1e-7 ); template <typename CONTAINER, typename T> void confidenceIntervals( const CONTAINER& data, T& out_mean, T& out_lower_conf_interval, T& out_upper_conf_interval, const double confidenceInterval = 0.1, const size_t histogramNumBins = 1000 ); template <typename CONTAINER, typename T> void confidenceIntervalsFromHistogram( const CONTAINER& histogramCoords, const CONTAINER& histogramNormalizedHits, T& out_lower_conf_interval, T& out_upper_conf_interval, const double confidenceInterval = 0.1 ); void setEpsilon(double nE); double getEpsilon(); void fft_real(CVectorFloat& in_realData, CVectorFloat& out_FFT_Re, CVectorFloat& out_FFT_Im, CVectorFloat& out_FFT_Mag); void dft2_real(const CMatrixFloat& in_data, CMatrixFloat& out_real, CMatrixFloat& out_imag); void idft2_real(const CMatrixFloat& in_real, const CMatrixFloat& in_imag, CMatrixFloat& out_data); void dft2_complex(const CMatrixFloat& in_real, const CMatrixFloat& in_imag, CMatrixFloat& out_real, CMatrixFloat& out_imag); void idft2_complex(const CMatrixFloat& in_real, const CMatrixFloat& in_imag, CMatrixFloat& out_real, CMatrixFloat& out_imag); void cross_correlation_FFT(const CMatrixFloat& A, const CMatrixFloat& B, CMatrixFloat& out_corr); double fresnel_sin_integral(double x); double fresnel_cos_integral(double x); long double lfresnel_sin_integral(long double x); long double lfresnel_cos_integral(long double x); template <class MATRIXLIKE1, class MATRIXLIKE2> void homogeneousMatrixInverse( const MATRIXLIKE1& M, MATRIXLIKE2& out_inverse_M ); 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 ); template <class MATRIXLIKE> void homogeneousMatrixInverse(MATRIXLIKE& M); template <class T, class VECTOR> T interpolate( const T& x, const VECTOR& ys, const T& x0, const T& x1 ); double interpolate2points( const double x, const double x0, const double y0, const double x1, const double y1, bool wrap2pi = false ); template <typename NUMTYPE, class VECTORLIKE> NUMTYPE spline( const NUMTYPE t, const VECTORLIKE& x, const VECTORLIKE& y, bool wrap2pi = false ); template <typename NUMTYPE, class VECTORLIKE, int NUM_POINTS = -1> NUMTYPE leastSquareLinearFit( const NUMTYPE t, const VECTORLIKE& x, const VECTORLIKE& y, bool wrap2pi = false ); template <class VECTORLIKE1, class VECTORLIKE2, class VECTORLIKE3, int NUM_POINTS = -1> void leastSquareLinearFit( const VECTORLIKE1& ts, VECTORLIKE2& outs, const VECTORLIKE3& x, const VECTORLIKE3& y, bool wrap2pi = false ); template <class Derived> const Derived& mat2eig(const Eigen::EigenBase<Derived>& m); template <class MAT> auto mat2eig( const MAT& m, typename MAT::eigen_t* = nullptr ); template <typename MAT> CMatrixRowAccessor<MAT> getRowAccessor( MAT& m, size_t rowIdx ); template <typename MAT> CMatrixRowAccessorExtended<MAT> getRowAccessor( MAT& m, size_t rowIdx, size_t offset, size_t space = 1 ); template <typename MAT> CConstMatrixRowAccessor<MAT> getRowAccessor( const MAT& m, size_t rowIdx ); template <typename MAT> CConstMatrixRowAccessorExtended<MAT> getRowAccessor( const MAT& m, size_t rowIdx, size_t offset, size_t space = 1 ); template <typename MAT> CMatrixColumnAccessor<MAT> getColumnAccessor( MAT& m, size_t colIdx ); template <typename MAT> CMatrixColumnAccessorExtended<MAT> getColumnAccessor( MAT& m, size_t colIdx, size_t offset, size_t space = 1 ); template <typename MAT> CConstMatrixColumnAccessor<MAT> getColumnAccessor( const MAT& m, size_t colIdx ); template <typename MAT> CConstMatrixColumnAccessorExtended<MAT> getColumnAccessor( const MAT& m, size_t colIdx, size_t offset, size_t space = 1 ); template <typename DER> void internalAssertEigenDefined(); template <typename Scalar, class Derived> std::ostream& operator << ( std::ostream& o, const MatrixVectorBase<Scalar, Derived>& m ); template < class VECTORLIKE, class VECTORLIKE2, class VECTORLIKE3, class MATRIXLIKE, class USERPARAM > void estimateJacobian( const VECTORLIKE& x, const std::function<void(const VECTORLIKE&x, const USERPARAM&y, VECTORLIKE3&out)>& functor, const VECTORLIKE2& increments, const USERPARAM& userParam, MATRIXLIKE& out_Jacobian ); template <class CONTAINER> std::vector<double> histogram( const CONTAINER& v, double limit_min, double limit_max, size_t number_bins, bool do_normalization = false, std::vector<double>* out_bin_centers = nullptr ); template <class EIGEN_CONTAINER> void resizeLike( EIGEN_CONTAINER& trg, const EIGEN_CONTAINER& src ); template <typename T> void resizeLike( std::vector<T>& trg, const std::vector<T>& src ); template <class CONTAINER1, class CONTAINER2> void cumsum_tmpl( const CONTAINER1& in_data, CONTAINER2& out_cumsum ); template <class CONTAINER1, class CONTAINER2> void cumsum( const CONTAINER1& in_data, CONTAINER2& out_cumsum ); template <class CONTAINER> CONTAINER cumsum(const CONTAINER& in_data); template <class CONTAINER> CONTAINER::Scalar norm_inf(const CONTAINER& v); template <class CONTAINER> CONTAINER::Scalar norm(const CONTAINER& v); template <class CONTAINER, int = CONTAINER::is_mrpt_type> CONTAINER::Scalar maximum(const CONTAINER& v); template <class CONTAINER, int = CONTAINER::is_mrpt_type> CONTAINER::Scalar minimum(const CONTAINER& v); template <class Derived> Derived::Scalar maximum(const Eigen::MatrixBase<Derived>& v); template <class Derived> Derived::Scalar minimum(const Eigen::MatrixBase<Derived>& v); template <typename T> T maximum(const std::vector<T>& v); template <typename T> T minimum(const std::vector<T>& v); template <typename MAT_H, typename MAT_C, typename MAT_R> void multiply_HCHt( const MAT_H& H, const MAT_C& C, MAT_R& R, bool accumResultInOutput = false ); template <matrix_dim_t H_ROWS, matrix_dim_t H_COLS, typename Scalar> mrpt::math::CMatrixFixed<Scalar, H_ROWS, H_ROWS> multiply_HCHt( const mrpt::math::CMatrixFixed<Scalar, H_ROWS, H_COLS>& H, const mrpt::math::CMatrixFixed<Scalar, H_COLS, H_COLS>& C ); template <typename VECTOR_H, typename MAT_C> MAT_C::Scalar multiply_HtCH_scalar( const VECTOR_H& H, const MAT_C& C ); template <typename VECTOR_H, typename MAT_C> MAT_C::Scalar multiply_HCHt_scalar( const VECTOR_H& H, const MAT_C& C ); template <class MAT_IN, class VECTOR, class MAT_OUT> void meanAndCovMat( const MAT_IN& v, VECTOR& out_mean, MAT_OUT& out_cov ); template <class MAT_IN, class VEC> void meanAndStdColumns( const MAT_IN& m, VEC& outMeanVector, VEC& outStdVector, const bool unbiased_variance = true ); template <class MATRIX> CMatrixDouble cov(const MATRIX& v); template <class MAT_A, class SKEW_3VECTOR, class MAT_OUT> void multiply_A_skew3( const MAT_A& A, const SKEW_3VECTOR& v, MAT_OUT& out ); template <class SKEW_3VECTOR, class MAT_A, class MAT_OUT> void multiply_skew3_A( const SKEW_3VECTOR& v, const MAT_A& A, MAT_OUT& out ); template <typename MATIN, typename MATOUT> void laplacian(const MATIN& g, MATOUT& ret); template <std::size_t BLOCKSIZE, typename MAT, typename MATRIX> void extractSubmatrixSymmetricalBlocks( const MAT& m, const std::vector<size_t>& block_indices, MATRIX& out ); template <typename MAT, typename MATRIX> void extractSubmatrixSymmetricalBlocksDyn( const MAT& m, const std::size_t BLOCKSIZE, const std::vector<size_t>& block_indices, MATRIX& out ); template <typename MAT, typename MATRIX> void extractSubmatrixSymmetrical( const MAT& m, const std::vector<size_t>& indices, MATRIX& out ); template <class T> std::ostream& operator << (std::ostream& out, const std::vector<T>& d); template <class T> std::ostream& operator << (std::ostream& out, std::vector<T>* d); template <typename T, matrix_dim_t N> mrpt::serialization::CArchive& operator << (mrpt::serialization::CArchive& ostrm, const CVectorFixed<T, N>& a); template <typename T, matrix_dim_t N> mrpt::serialization::CArchive& operator >> (mrpt::serialization::CArchive& istrm, CVectorFixed<T, N>& a); int solve_poly3(double* x, double a, double b, double c); int solve_poly4(double* x, double a, double b, double c, double d); int solve_poly5(double* x, double a, double b, double c, double d, double e); int solve_poly2(double a, double b, double c, double& r1, double& r2); template <typename T> size_t ransacDatasetSize(const CMatrixDynamic<T>& dataset); void registerAllClasses_mrpt_math(); mrpt::serialization::CArchive& operator >> ( mrpt::serialization::CArchive& in, mrpt::math::TBoundingBoxf& bb ); mrpt::serialization::CArchive& operator << ( mrpt::serialization::CArchive& out, const mrpt::math::TBoundingBoxf& bb ); mrpt::serialization::CArchive& operator >> ( mrpt::serialization::CArchive& in, mrpt::math::TBoundingBox& bb ); mrpt::serialization::CArchive& operator << ( mrpt::serialization::CArchive& out, const mrpt::math::TBoundingBox& bb ); mrpt::serialization::CArchive& operator >> ( mrpt::serialization::CArchive& in, mrpt::math::TLine2D& l ); mrpt::serialization::CArchive& operator << ( mrpt::serialization::CArchive& out, const mrpt::math::TLine2D& l ); std::ostream& operator << (std::ostream& o, const TLine2D& p); mrpt::serialization::CArchive& operator >> ( mrpt::serialization::CArchive& in, mrpt::math::TLine3D& l ); mrpt::serialization::CArchive& operator << ( mrpt::serialization::CArchive& out, const mrpt::math::TLine3D& l ); std::ostream& operator << (std::ostream& o, const TLine3D& p); mrpt::serialization::CArchive& operator >> ( mrpt::serialization::CArchive& in, mrpt::math::TObject2D& o ); mrpt::serialization::CArchive& operator << ( mrpt::serialization::CArchive& out, const mrpt::math::TObject2D& o ); std::ostream& operator << (std::ostream& o, const mrpt::math::TObject2D& obj); mrpt::serialization::CArchive& operator >> ( mrpt::serialization::CArchive& in, mrpt::math::TObject3D& o ); mrpt::serialization::CArchive& operator << ( mrpt::serialization::CArchive& out, const mrpt::math::TObject3D& o ); std::ostream& operator << (std::ostream& o, const mrpt::math::TObject3D& obj); mrpt::serialization::CArchive& operator >> ( mrpt::serialization::CArchive& in, mrpt::math::TOrientedBox& bb ); mrpt::serialization::CArchive& operator << ( mrpt::serialization::CArchive& out, const mrpt::math::TOrientedBox& bb ); mrpt::serialization::CArchive& operator >> ( mrpt::serialization::CArchive& in, mrpt::math::TOrientedBoxf& bb ); mrpt::serialization::CArchive& operator << ( mrpt::serialization::CArchive& out, const mrpt::math::TOrientedBoxf& bb ); mrpt::serialization::CArchive& operator >> ( mrpt::serialization::CArchive& in, mrpt::math::TPlane& p ); mrpt::serialization::CArchive& operator << ( mrpt::serialization::CArchive& out, const mrpt::math::TPlane& p ); std::ostream& operator << (std::ostream& o, const TPlane& p); template <typename T> constexpr TPoint2D_<T> operator - (const TPoint2D_<T>& p1); template < typename T, typename Scalar, std::enable_if_t<std::is_convertible_v<Scalar, T>>* = nullptr > constexpr TPoint2D_<T> operator * ( const Scalar scalar, const TPoint2D_<T>& p ); template <typename T> constexpr bool operator == (const TPoint2D_<T>& p1, const TPoint2D_<T>& p2); template <typename T> constexpr bool operator != (const TPoint2D_<T>& p1, const TPoint2D_<T>& p2); mrpt::serialization::CArchive& operator >> ( mrpt::serialization::CArchive& in, mrpt::math::TPointXYZfRGBu8& p ); mrpt::serialization::CArchive& operator << ( mrpt::serialization::CArchive& out, const mrpt::math::TPointXYZfRGBu8& p ); mrpt::serialization::CArchive& operator >> ( mrpt::serialization::CArchive& in, mrpt::math::TPointXYZfRGBAu8& p ); mrpt::serialization::CArchive& operator << ( mrpt::serialization::CArchive& out, const mrpt::math::TPointXYZfRGBAu8& p ); template <typename T> constexpr TPoint3D_<T> operator - (const TPoint3D_<T>& p1); template < typename T, typename Scalar, std::enable_if_t<std::is_convertible_v<Scalar, T>>* = nullptr > constexpr TPoint3D_<T> operator * ( const Scalar scalar, const TPoint3D_<T>& p ); template <typename T> constexpr bool operator == (const TPoint3D_<T>& p1, const TPoint3D_<T>& p2); template <typename T> constexpr bool operator != (const TPoint3D_<T>& p1, const TPoint3D_<T>& p2); std::ostream& operator << (std::ostream& o, const TPolygon2D& p); std::ostream& operator << (std::ostream& o, const TPolygon3D& p); bool operator == (const TPose2D& p1, const TPose2D& p2); bool operator != (const TPose2D& p1, const TPose2D& p2); TPose3D operator - (const TPose3D& p); TPose3D operator - (const TPose3D& b, const TPose3D& a); bool operator == (const TPose3D& p1, const TPose3D& p2); bool operator != (const TPose3D& p1, const TPose3D& p2); bool operator == (const TPose3DQuat& a, const TPose3DQuat& b); bool operator != ( const TPose3DQuat& a, const TPose3DQuat& b ); template < class PoseOrPoint, typename = std::enable_if_t<std::is_base_of_v<mrpt::math::TPoseOrPoint, PoseOrPoint>> > std::ostream& operator << ( std::ostream& o, const PoseOrPoint& p ); template < class PoseOrPoint, typename = std::enable_if_t<std::is_base_of_v<mrpt::math::TPoseOrPoint, PoseOrPoint>> > mrpt::serialization::CArchive& operator >> ( mrpt::serialization::CArchive& in, PoseOrPoint& o ); template < class PoseOrPoint, typename = std::enable_if_t<std::is_base_of_v<mrpt::math::TPoseOrPoint, PoseOrPoint>> > mrpt::serialization::CArchive& operator << ( mrpt::serialization::CArchive& out, const PoseOrPoint& o ); template < class VECTORLIKE1, class MATLIKE1, class USERPARAM, class VECTORLIKE2, class VECTORLIKE3, class MATLIKE2 > void transform_gaussian_unscented( const VECTORLIKE1& x_mean, const MATLIKE1& x_cov, void(*)(const VECTORLIKE1&x, const USERPARAM&fixed_param, VECTORLIKE3&y) functor, const USERPARAM& fixed_param, VECTORLIKE2& y_mean, MATLIKE2& y_cov, const bool* elem_do_wrap2pi = nullptr, const double alpha = 1e-3, const double K = 0, const double beta = 2.0 ); template < class VECTORLIKE1, class MATLIKE1, class USERPARAM, class VECTORLIKE2, class VECTORLIKE3, class MATLIKE2 > void transform_gaussian_montecarlo( const VECTORLIKE1& x_mean, const MATLIKE1& x_cov, void(*)(const VECTORLIKE1&x, const USERPARAM&fixed_param, VECTORLIKE3&y) functor, const USERPARAM& fixed_param, VECTORLIKE2& y_mean, MATLIKE2& y_cov, const size_t num_samples = 1000, std::vector<VECTORLIKE3>* out_samples_y = nullptr ); template < class VECTORLIKE1, class MATLIKE1, class USERPARAM, class VECTORLIKE2, class VECTORLIKE3, class MATLIKE2 > void transform_gaussian_linear( const VECTORLIKE1& x_mean, const MATLIKE1& x_cov, void(*)(const VECTORLIKE1&x, const USERPARAM&fixed_param, VECTORLIKE3&y) functor, const USERPARAM& fixed_param, VECTORLIKE2& y_mean, MATLIKE2& y_cov, const VECTORLIKE1& x_increments ); bool operator == ( const TSegment2D& s1, const TSegment2D& s2 ); bool operator != ( const TSegment2D& s1, const TSegment2D& s2 ); mrpt::serialization::CArchive& operator >> ( mrpt::serialization::CArchive& in, mrpt::math::TSegment2D& s ); mrpt::serialization::CArchive& operator << ( mrpt::serialization::CArchive& out, const mrpt::math::TSegment2D& s ); std::ostream& operator << (std::ostream& o, const TSegment2D& p); bool operator == ( const TSegment3D& s1, const TSegment3D& s2 ); bool operator != ( const TSegment3D& s1, const TSegment3D& s2 ); mrpt::serialization::CArchive& operator >> ( mrpt::serialization::CArchive& in, mrpt::math::TSegment3D& s ); mrpt::serialization::CArchive& operator << ( mrpt::serialization::CArchive& out, const mrpt::math::TSegment3D& s ); std::ostream& operator << (std::ostream& o, const TSegment3D& p); mrpt::serialization::CArchive& operator >> ( mrpt::serialization::CArchive& in, mrpt::math::TTwist2D& o ); mrpt::serialization::CArchive& operator << ( mrpt::serialization::CArchive& out, const mrpt::math::TTwist2D& o ); mrpt::serialization::CArchive& operator >> ( mrpt::serialization::CArchive& in, mrpt::math::TTwist3D& o ); mrpt::serialization::CArchive& operator << ( mrpt::serialization::CArchive& out, const mrpt::math::TTwist3D& o ); template <class T1, class T2> bool approximatelyEqual(T1 a, T1 b, T2 epsilon); template <class T> bool approximatelyEqual(T a, T b); template <class T> T absDiff(const T& lhs, const T& rhs); bool loadVector(std::istream& f, std::vector<int>& d); bool loadVector(std::istream& f, std::vector<double>& d); void medianFilter( const std::vector<double>& inV, std::vector<double>& outV, int winSize, int numberOfSigmas = 2 ); template <typename T, typename VECTOR> void linspace( T first, T last, size_t count, VECTOR& out_vector ); template <typename T, typename VECTOR = std::vector<T>> VECTOR linspace( T first, T last, size_t count ); template <class T, T STEP> std::vector<T> sequenceStdVec(T first, size_t length); template <class VEC1, class VEC2> void normalize(const VEC1& v, VEC2& out_v); template <class VECTOR_OF_VECTORS, class VECTORLIKE> void extractColumnFromVectorOfVectors( const size_t colIndex, const VECTOR_OF_VECTORS& data, VECTORLIKE& out_column ); uint64_t factorial64(unsigned int n); double factorial(unsigned int n); std::string MATLAB_plotCovariance2D( const CMatrixFloat& cov22, const CVectorFloat& mean, float stdCount, const std::string& style = std::string("b"), size_t nEllipsePoints = 30 ); std::string MATLAB_plotCovariance2D( const CMatrixDouble& cov22, const CVectorDouble& mean, float stdCount, const std::string& style = std::string("b"), size_t nEllipsePoints = 30 ); template <typename VECTOR_T, typename At, size_t N> VECTOR_T& loadVector( VECTOR_T& v, At(&) theArray [N] ); template <typename T, typename At, size_t N> std::vector<T>& loadVector( std::vector<T>& v, At(&) theArray [N] ); template <class TRIPLET> bool saveEigenSparseTripletsToFile( const std::string& sFile, std::vector<TRIPLET>& tri ); template <class T> void wrapTo2PiInPlace(T& a); template <class T> T wrapTo2Pi(T a); template <class T> T wrapToPi(T a); template <class T> void wrapToPiInPlace(T& a); template <class VECTOR> void unwrap2PiSequence(VECTOR& x); template <class T> T angDistance(T from, T to); void project3D( const std::monostate&, const TPose3D&, std::monostate& ); void project2D( const std::monostate&, const TPose3D&, std::monostate& ); template <class T> void removeUnusedVertices(T& poly); template <class T> void removeRepVertices(T& poly); template <typename T> void ransac3Dplane_fit( const CMatrixDynamic<T>& allData, const std::vector<size_t>& useIndices, vector<CMatrixDynamic<T>>& fitModels ); template <typename T> void ransac3Dplane_distance( const CMatrixDynamic<T>& allData, const vector<CMatrixDynamic<T>>& testModels, const T distanceThreshold, unsigned int& out_bestModelIndex, std::vector<size_t>& out_inlierIndices ); template <typename T> bool ransac3Dplane_degenerate( ] const CMatrixDynamic<T>& allData, ] const std::vector<size_t>& useIndices ); template <typename T> void ransac2Dline_fit( const CMatrixDynamic<T>& allData, const std::vector<size_t>& useIndices, vector<CMatrixDynamic<T>>& fitModels ); template <typename T> void ransac2Dline_distance( const CMatrixDynamic<T>& allData, const vector<CMatrixDynamic<T>>& testModels, const T distanceThreshold, unsigned int& out_bestModelIndex, std::vector<size_t>& out_inlierIndices ); template <typename T> bool ransac2Dline_degenerate( ] const CMatrixDynamic<T>& allData, ] const std::vector<size_t>& useIndices ); std::ostream& operator << ( std::ostream& o, const std::monostate& ); } // namespace math
Typedefs
typedef CMatrixDynamic<bool> CMatrixBool
Declares a matrix of booleans (non serializable).
See also:
CMatrixDouble, CMatrixFloat, CMatrixB
typedef CMatrixDynamic<float> CMatrixFloat
Declares a matrix of float numbers (non serializable).
For a serializable version, use math::CMatrixF
See also:
CMatrixDouble, CMatrixF, CMatrixD
typedef CMatrixDynamic<double> CMatrixDouble
Declares a matrix of double numbers (non serializable).
For a serializable version, use math::CMatrixD
See also:
CMatrixFloat, CMatrixF, CMatrixD
typedef CMatrixDynamic<unsigned int> CMatrixUInt
Declares a matrix of unsigned ints (non serializable).
See also:
typedef CMatrixDynamic<uint8_t> CMatrix_u8
matrix of uint8_t (non serializable).
See also:
typedef CMatrixDynamic<uint16_t> CMatrix_u16
matrix of uint16_t (non serializable).
See also:
typedef CMatrixDynamic<double> CMatrixLongDouble
Declares a matrix of “long doubles” (non serializable), or of “doubles” if the compiler does not support “long double”.
See also:
typedef CQuaternion<double> CQuaternionDouble
A quaternion of data type “double”.
typedef CQuaternion<float> CQuaternionFloat
A quaternion of data type “float”.
typedef CMatrixFixed<T, N, 1> CVectorFixed
CVectorFixed is an array for numeric types supporting several mathematical operations (actually, just a wrapper on Eigen::Matrix<T,N,1>)
See also:
CVectorFixedFloat, CVectorFixedDouble, CArray
typedef CVectorFixed<float, N> CVectorFixedFloat
Specialization of CVectorFixed for float numbers.
See also:
typedef CVectorFixed<double, N> CVectorFixedDouble
Specialization of CVectorFixed for double numbers.
See also:
typedef long int matrix_index_t
Index type for matrices.
typedef long int matrix_dim_t
Size type for matrices.
Global Functions
bool conformAPlane(const std::vector<TPoint3D>& points)
Checks whether this polygon or set of points acceptably fits a plane.
See also:
bool conformAPlane(const std::vector<TPoint3D>& points, TPlane& p)
Checks whether this polygon or set of points acceptably fits a plane, and if it’s the case returns it in the second argument.
See also:
bool areAligned(const std::vector<TPoint2D>& points)
Checks whether this set of points acceptably fits a 2D line.
See also:
bool areAligned(const std::vector<TPoint2D>& points, TLine2D& r)
Checks whether this set of points acceptably fits a 2D line, and if it’s the case returns it in the second argument.
See also:
bool areAligned(const std::vector<TPoint3D>& points)
Checks whether this set of points acceptably fits a 3D line.
See also:
bool areAligned(const std::vector<TPoint3D>& points, TLine3D& r)
Checks whether this set of points acceptably fits a 3D line, and if it’s the case returns it in the second argument.
void project3D(const TPoint3D& point, const mrpt::math::TPose3D& newXYpose, TPoint3D& newPoint)
Uses the given pose 3D to project a point into a new base.
void project3D(const TSegment3D& segment, const mrpt::math::TPose3D& newXYpose, TSegment3D& newSegment)
Uses the given pose 3D to project a segment into a new base
void project3D(const TLine3D& line, const mrpt::math::TPose3D& newXYpose, TLine3D& newLine)
Uses the given pose 3D to project a line into a new base.
void project3D(const TPlane& plane, const mrpt::math::TPose3D& newXYpose, TPlane& newPlane)
Uses the given pose 3D to project a plane into a new base.
void project3D(const TPolygon3D& polygon, const mrpt::math::TPose3D& newXYpose, TPolygon3D& newPolygon)
Uses the given pose 3D to project a polygon into a new base.
void project3D(const TObject3D& object, const mrpt::math::TPose3D& newXYPose, TObject3D& newObject)
Uses the given pose 3D to project any 3D object into a new base.
template <typename Object> Object project3D(const Object& o, const mrpt::math::TPose3D& newPose)
Returns the transformed point/line/segment/plane after applying the given SE(3) transformation.
[New in MRPT 2.3.1]
template <class T> void project3D(const T& obj, const TPlane& newXYPlane, T& newObj)
Projects any 3D object into the plane’s base, using its inverse pose.
If the object is exactly inside the plane, this projection will zero its Z coordinates
template <class T> void project3D( const T& obj, const TPlane& newXYPlane, ] const TPoint3D& newOrigin, T& newObj )
Projects any 3D object into the plane’s base, using its inverse pose and forcing the position of the new coordinates origin.
If the object is exactly inside the plane, this projection will zero its Z coordinates
template <class T> void project3D( const std::vector<T>& objs, const mrpt::math::TPose3D& newXYpose, std::vector<T>& newObjs )
Projects a set of 3D objects into the plane’s base.
void project2D(const TPoint2D& point, const TPose2D& newXpose, TPoint2D& newPoint)
Uses the given pose 2D to project a point into a new base.
void project2D(const TSegment2D& segment, const TPose2D& newXpose, TSegment2D& newSegment)
Uses the given pose 2D to project a segment into a new base
void project2D(const TLine2D& line, const TPose2D& newXpose, TLine2D& newLine)
Uses the given pose 2D to project a line into a new base.
void project2D(const TPolygon2D& polygon, const TPose2D& newXpose, TPolygon2D& newPolygon)
Uses the given pose 2D to project a polygon into a new base.
void project2D(const TObject2D& object, const TPose2D& newXpose, TObject2D& newObject)
Uses the given pose 2D to project any 2D object into a new base.
template <typename Object> Object project2D(const Object& o, const mrpt::math::TPose2D& newPose)
Returns the transformed point/line/segment after applying the given SE(2) transformation.
[New in MRPT 2.3.1]
template <class T, class CPOSE2D> void project2D( const T& obj, const TLine2D& newXLine, T& newObj )
Projects any 2D object into the line’s base, using its inverse pose.
If the object is exactly inside the line, this projection will zero its Y coordinate.
Parameters:
CPOSE2D |
set to TPose2D |
template <class T, class CPOSE2D> void project2D( const T& obj, const TLine2D& newXLine, const TPoint2D& newOrigin, T& newObj )
Projects any 2D object into the line’s base, using its inverse pose and forcing the position of the new coordinate origin.
If the object is exactly inside the line, this projection will zero its Y coordinate.
Parameters:
CPOSE2D |
set to TPose2D |
template <class T> void project2D( const std::vector<T>& objs, const TPose2D& newXpose, std::vector<T>& newObjs )
Projects a set of 2D objects into the line’s base.
bool intersect(const TPolygon2D& p1, const TSegment2D& s2, TObject2D& obj)
Gets the intersection between a 2D polygon and a 2D segment.
See also:
bool intersect(const TPolygon2D& p1, const TLine2D& r2, TObject2D& obj)
Gets the intersection between a 2D polygon and a 2D line.
See also:
TPolygon2D intersect(const TPolygon2D& subject, const TPolygon2D& clipping)
The Sutherland-Hodgman algorithm for finding the intersection between two 2D polygons.
Note that at least one of the polygons (clipping) must be convex. The other polygon (subject) may be convex or concave.
See code example: Example: math_polygon_intersection
(New in MRPT 2.4.1)
Returns:
The intersection, or an empty (no points) polygon if there is no intersection at all.
bool intersect(const TSegment2D& s1, const TPolygon2D& p2, TObject2D& obj)
Gets the intersection between a 2D segment and a 2D polygon.
See also:
bool intersect(const TLine2D& r1, const TPolygon2D& p2, TObject2D& obj)
Gets the intersection between a 2D line and a 2D polygon.
See also:
bool intersect(const TPolygon3D& p1, const TSegment3D& s2, TObject3D& obj)
Gets the intersection between a 3D polygon and a 3D segment.
See also:
bool intersect(const TPolygon3D& p1, const TLine3D& r2, TObject3D& obj)
Gets the intersection between a 3D polygon and a 3D line.
See also:
bool intersect(const TPolygon3D& p1, const TPlane& p2, TObject3D& obj)
Gets the intersection between a 3D polygon and a plane.
See also:
bool intersect(const TPolygon3D& p1, const TPolygon3D& p2, TObject3D& obj)
Gets the intersection between two 3D polygons.
See also:
bool intersect(const TSegment3D& s1, const TPolygon3D& p2, TObject3D& obj)
Gets the intersection between a 3D segment and a 3D polygon.
See also:
bool intersect(const TLine3D& r1, const TPolygon3D& p2, TObject3D& obj)
Gets the intersection between a 3D line and a 3D polygon.
See also:
bool intersect(const TPlane& p1, const TPolygon3D& p2, TObject3D& obj)
Gets the intersection between a plane and a 3D polygon.
See also:
size_t intersect(const std::vector<TPolygon3D>& v1, const std::vector<TPolygon3D>& v2, CSparseMatrixTemplate<TObject3D>& objs)
Gets the intersection between two sets of 3D polygons.
The intersection is returned as an sparse matrix with each pair of polygons’ intersections, and the return value is the amount of intersections found.
See also:
TObject3D, CSparseMatrixTemplate
size_t intersect( const std::vector<TPolygon3D>& v1, const std::vector<TPolygon3D>& v2, std::vector<TObject3D>& objs )
Gets the intersection between two sets of 3D polygons.
The intersection is returned as a vector with every intersection found, and the return value is the amount of intersections found.
See also:
template <class T, class U, class O> size_t intersect( const std::vector<T>& v1, const std::vector<U>& v2, CSparseMatrixTemplate<O>& objs )
Gets the intersection between vectors of geometric objects and returns it in a sparse matrix of either TObject2D or TObject3D.
See also:
TObject2D, TObject3D,CSparseMatrix
template <class T, class U, class O> size_t intersect( const std::vector<T>& v1, const std::vector<U>& v2, std::vector<O> objs )
Gets the intersection between vectors of geometric objects and returns it in a vector of either TObject2D or TObject3D.
See also:
bool intersect(const TObject2D& o1, const TObject2D& o2, TObject2D& obj)
Gets the intersection between any pair of 2D objects.
bool intersect(const TObject3D& o1, const TObject3D& o2, TObject3D& obj)
Gets the intersection between any pair of 3D objects.
double distance(const TPoint2D& p1, const TPoint2D& p2)
Gets the distance between two points in a 2D space.
double distance(const TPoint3D& p1, const TPoint3D& p2)
Gets the distance between two points in a 3D space.
double distance(const TLine2D& r1, const TLine2D& r2)
Gets the distance between two lines in a 2D space.
double distance(const TLine3D& r1, const TLine3D& r2)
Gets the distance between two lines in a 3D space.
double distance(const TPlane& p1, const TPlane& p2)
Gets the distance between two planes.
It will be zero if the planes are not parallel.
double distance(const TPolygon2D& p1, const TPolygon2D& p2)
Gets the distance between two polygons in a 2D space.
double distance(const TPolygon2D& p1, const TSegment2D& s2)
Gets the distance between a polygon and a segment in a 2D space.
double distance(const TSegment2D& s1, const TPolygon2D& p2)
Gets the distance between a segment and a polygon in a 2D space.
double distance(const TPolygon2D& p1, const TLine2D& l2)
Gets the distance between a polygon and a line in a 2D space.
double distance(const TPolygon3D& p1, const TPolygon3D& p2)
Gets the distance between two polygons in a 3D space.
double distance(const TPolygon3D& p1, const TSegment3D& s2)
Gets the distance between a polygon and a segment in a 3D space.
double distance(const TSegment3D& s1, const TPolygon3D& p2)
Gets the distance between a segment and a polygon in a 3D space.
double distance(const TPolygon3D& p1, const TLine3D& l2)
Gets the distance between a polygon and a line in a 3D space.
double distance(const TLine3D& l1, const TPolygon3D& p2)
Gets the distance between a line and a polygon in a 3D space.
double distance(const TPolygon3D& po, const TPlane& pl)
Gets the distance between a polygon and a plane.
double distance(const TPlane& pl, const TPolygon3D& po)
Gets the distance between a plane and a polygon.
void getRectangleBounds(const std::vector<TPoint2D>& poly, TPoint2D& pMin, TPoint2D& pMax)
Gets the rectangular bounds of a 2D polygon or set of 2D points.
void getPrismBounds(const std::vector<TPoint3D>& poly, TPoint3D& pMin, TPoint3D& pMax)
Gets the prism bounds of a 3D polygon or set of 3D points.
void createPlaneFromPoseXY(const mrpt::math::TPose3D& pose, TPlane& plane)
Given a pose, creates a plane orthogonal to its Z vector.
See also:
createPlaneFromPoseXZ, createPlaneFromPoseYZ, createPlaneFromPoseAndNormal
void createPlaneFromPoseXZ(const mrpt::math::TPose3D& pose, TPlane& plane)
Given a pose, creates a plane orthogonal to its Y vector.
See also:
createPlaneFromPoseXY, createPlaneFromPoseYZ, createPlaneFromPoseAndNormal
void createPlaneFromPoseYZ(const mrpt::math::TPose3D& pose, TPlane& plane)
Given a pose, creates a plane orthogonal to its X vector.
See also:
createPlaneFromPoseXY, createPlaneFromPoseXZ, createPlaneFromPoseAndNormal
void createPlaneFromPoseAndNormal( const mrpt::math::TPose3D& pose, const double(&) normal [3], TPlane& plane )
Given a pose and any vector, creates a plane orthogonal to that vector in the pose’s coordinates.
See also:
createPlaneFromPoseXY, createPlaneFromPoseXZ, createPlaneFromPoseYZ
CMatrixDouble44 generateAxisBaseFromDirectionAndAxis(const mrpt::math::TVector3D& vec, uint8_t coord)
Creates a homogeneus matrix (4x4) such that the coordinate given (0 for x, 1 for y, 2 for z) corresponds to the provided vector.
Parameters:
vec |
must be a unitary vector |
See also:
generateAxisBaseFromDirectionAndAxis()
double getRegressionLine(const std::vector<TPoint2D>& points, TLine2D& line)
Using eigenvalues, gets the best fitting line for a set of 2D points.
Returns an estimation of the error.
See also:
double getRegressionLine(const std::vector<TPoint3D>& points, TLine3D& line)
Using eigenvalues, gets the best fitting line for a set of 3D points.
Returns an estimation of the error.
See also:
double getRegressionPlane(const std::vector<TPoint3D>& points, TPlane& plane)
Using eigenvalues, gets the best fitting plane for a set of 3D points.
Returns an estimation of the error.
See also:
void assemblePolygons(const std::vector<TSegment3D>& segms, std::vector<TPolygon3D>& polys)
Tries to assemble a set of segments into a set of closed polygons.
void assemblePolygons( const std::vector<TSegment3D>& segms, std::vector<TPolygon3D>& polys, std::vector<TSegment3D>& remainder )
Tries to assemble a set of segments into a set of closed polygons, returning the unused segments as another out parameter.
void assemblePolygons(const std::vector<TObject3D>& objs, std::vector<TPolygon3D>& polys)
Extracts all the polygons, including those formed from segments, from the set of objects.
void assemblePolygons( const std::vector<TObject3D>& objs, std::vector<TPolygon3D>& polys, std::vector<TObject3D>& remainder )
Extracts all the polygons, including those formed from segments, from the set of objects.
void assemblePolygons( const std::vector<TObject3D>& objs, std::vector<TPolygon3D>& polys, std::vector<TSegment3D>& remainder1, std::vector<TObject3D>& remainder2 )
Extracts all the polygons, including those formed from segments, from the set of objects.
bool splitInConvexComponents(const TPolygon2D& poly, std::vector<TPolygon2D>& components)
Splits a 2D polygon into convex components.
See also:
Example: math_polygon_split
bool splitInConvexComponents(const TPolygon3D& poly, std::vector<TPolygon3D>& components)
Splits a 3D polygon into convex components.
Parameters:
std::logic_error |
if the polygon can’t be fit into a plane. |
void getSegmentBisector(const TSegment2D& sgm, TLine2D& bis)
Gets the bisector of a 2D segment.
void getSegmentBisector(const TSegment3D& sgm, TPlane& bis)
Gets the bisector of a 3D segment.
void getAngleBisector(const TLine2D& l1, const TLine2D& l2, TLine2D& bis)
Gets the bisector of two lines or segments (implicit constructor will be used if necessary)
void getAngleBisector(const TLine3D& l1, const TLine3D& l2, TLine3D& bis)
Gets the bisector of two lines or segments (implicit constructor will be used if necessary)
Parameters:
std::logic_error |
if the lines do not fit in a single plane. |
bool traceRay(const std::vector<TPolygonWithPlane>& vec, const mrpt::math::TPose3D& pose, double& dist)
Fast ray tracing method using polygons’ properties.
See also:
CRenderizable::rayTrace
bool traceRay(const std::vector<TPolygon3D>& vec, const mrpt::math::TPose3D& pose, double& dist)
Fast ray tracing method using polygons’ properties.
See also:
CRenderizable::rayTrace
template <class T, class U, class V> void crossProduct3D( const T& v0, const U& v1, V& vOut )
Computes the cross product of two 3D vectors, returning a vector normal to both.
It uses the simple implementation:
template <class T> void crossProduct3D( const std::vector<T>& v0, const std::vector<T>& v1, std::vector<T>& v_out )
This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.
template <class VEC1, class VEC2> VEC1 crossProduct3D( const VEC1& v0, const VEC2& v1 )
overload (returning a vector of size 3 by value).
template <class VECTOR, class MATRIX> void skew_symmetric3( const VECTOR& v, MATRIX& M )
Computes the 3x3 skew symmetric matrix from a 3-vector or 3-array:
template <class VECTOR> mrpt::math::CMatrixDouble33 skew_symmetric3(const VECTOR& v)
This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.
template <class VECTOR, class MATRIX> void skew_symmetric3_neg( const VECTOR& v, MATRIX& M )
Computes the negative version of a 3x3 skew symmetric matrix from a 3-vector or 3-array:
template <class VECTOR> mrpt::math::CMatrixDouble33 skew_symmetric3_neg(const VECTOR& v)
This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.
template <class T, class U> bool vectorsAreParallel2D(const T& v1, const U& v2)
Returns true if two 2D vectors are parallel.
The arguments may be points, arrays, etc.
template <class T, class U> bool vectorsAreParallel3D(const T& v1, const U& v2)
Returns true if two 3D vectors are parallel.
The arguments may be points, arrays, etc.
void closestFromPointToSegment( double Px, double Py, double x1, double y1, double x2, double y2, double& out_x, double& out_y )
Computes the closest point from a given point to a segment.
See also:
void closestFromPointToLine( double Px, double Py, double x1, double y1, double x2, double y2, double& out_x, double& out_y )
Computes the closest point from a given point to a (infinite) line.
See also:
double squaredDistancePointToLine( double Px, double Py, double x1, double y1, double x2, double y2 )
Returns the square distance from a point to a line.
template <typename T> T distanceBetweenPoints( const T x1, const T y1, const T x2, const T y2 )
Returns the distance between 2 points in 2D.
template <typename T> T distanceBetweenPoints( const T x1, const T y1, const T z1, const T x2, const T y2, const T z2 )
Returns the distance between 2 points in 3D.
template <typename T> T distanceSqrBetweenPoints( const T x1, const T y1, const T x2, const T y2 )
Returns the square distance between 2 points in 2D.
template <typename T> T distanceSqrBetweenPoints( const T x1, const T y1, const T z1, const T x2, const T y2, const T z2 )
Returns the square distance between 2 points in 3D.
template <typename T> double minimumDistanceFromPointToSegment( const double Px, const double Py, const double x1, const double y1, const double x2, const double y2, T& out_x, T& out_y )
Computes the closest point from a given point to a segment, and returns that minimum distance.
template <typename T> bool pointIntoQuadrangle( T x, T y, T v1x, T v1y, T v2x, T v2y, T v3x, T v3y, T v4x, T v4y )
Specialized method to check whether a point (x,y) falls into a quadrangle.
See also:
pointIntoPolygon2D
bool RectanglesIntersection( double R1_x_min, double R1_x_max, double R1_y_min, double R1_y_max, double R2_x_min, double R2_x_max, double R2_y_min, double R2_y_max, double R2_pose_x, double R2_pose_y, double R2_pose_phi )
Returns whether two rotated rectangles intersect.
The first rectangle is not rotated and given by (R1_x_min,R1_x_max)-(R1_y_min,R1_y_max). The second rectangle is given is a similar way, but it is internally rotated according to the given coordinates translation (R2_pose_x,R2_pose_y,R2_pose_phi(radians)), relative to the coordinates system of rectangle 1.
CMatrixDouble33 generateAxisBaseFromDirection(double dx, double dy, double dz)
Computes an axis base (a set of three 3D normal vectors) with the given vector being the first of them (“X”) NOTE: Make sure of passing all floats or doubles and that the template of the receiving matrix is of the same type!
If \(d = [ dx ~ dy ~ dz ]\) is the input vector, then this function returns a matrix \(M\) such as:
And the three normal vectors are computed as:
If (dx!=0 or dy!=0):
otherwise (the direction vector is vertical):
And finally, the third vector is the cross product of the others:
(JLB @ 18-SEP-2007)
Returns:
The 3x3 matrix (CMatrixDynamic<T>), containing one vector per column. except Throws an std::exception on invalid input (i.e. null direction vector)
See also:
generateAxisBaseFromDirectionAndAxis()
double signedArea(const mrpt::math::TPolygon2D& p)
Returns the area of a polygon, positive if vertices listed in CCW ordering, negative if CW.
(New in MRPT 2.4.1)
template <class CONTAINER, typename VALUE> VALUE squareNorm_accum( const VALUE total, const CONTAINER& v )
Accumulate the squared-norm of a vector/array/matrix into “total” (this function is compatible with std::accumulate).
template <size_t N, class T, class U> T squareNorm(const U& v)
Compute the square norm of anything implementing [].
See also:
template <class CONTAINER1, class CONTAINER2> CONTAINER1::Scalar dotProduct( const CONTAINER1& v1, const CONTAINER1& v2 )
v1*v2: The dot product of two containers (vectors/arrays/matrices)
template <size_t N, class T, class U, class V> T dotProduct( const U& v1, const V& v2 )
v1*v2: The dot product of any two objects supporting []
template <class CONTAINER> CONTAINER::Scalar sum(const CONTAINER& v)
Computes the sum of all the elements.
If used with containers of integer types (uint8_t, int, etc…) this could overflow. In those cases, use sumRetType the second argument RET to specify a larger type to hold the sum.
See also:
template <typename T> T sum(const std::vector<T>& v)
This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.
template <class CONTAINER, typename RET> RET sumRetType(const CONTAINER& v)
Computes the sum of all the elements, with a custom return type.
See also:
template <class CONTAINER> double mean(const CONTAINER& v)
Computes the mean value of a vector.
Returns:
The mean, as a double number.
See also:
math::stddev, math::meanAndStd
template <typename T, typename Alloc> void minimum_maximum( const std::vector<T, Alloc>& V, T& curMin, T& curMax )
Return the maximum and minimum values of a std::vector.
template <class Derived> void minimum_maximum( const Eigen::MatrixBase<Derived>& V, typename Eigen::MatrixBase<Derived>::Scalar& curMin, typename Eigen::MatrixBase<Derived>::Scalar& curMax )
Return the maximum and minimum values of a Eigen-based vector or matrix.
template <class CONTAINER, typename Scalar> void normalize( CONTAINER& c, Scalar valMin, Scalar valMax )
Scales all elements such as the minimum & maximum values are shifted to the given values.
template <class CONTAINER1, class CONTAINER2> size_t countCommonElements( const CONTAINER1& a, const CONTAINER2& b )
Counts the number of elements that appear in both STL-like containers (comparison through the == operator) It is assumed that no repeated elements appear within each of the containers.
template <class CONTAINER> void adjustRange( CONTAINER& m, const typename CONTAINER::Scalar minVal, const typename CONTAINER::Scalar maxVal )
Adjusts the range of all the elements such as the minimum and maximum values being those supplied by the user.
template <class VECTORLIKE> void meanAndStd( const VECTORLIKE& v, double& out_mean, double& out_std, bool unbiased = true )
Computes the standard deviation of a vector (or all elements of a matrix)
Parameters:
v |
The set of data, either as a vector, or a matrix (arrangement of data is ignored in this function). |
out_mean |
The output for the estimated mean |
out_std |
The output for the estimated standard deviation |
unbiased |
If set to true or false the std is normalized by “N-1” or “N”, respectively. |
See also:
template <class VECTORLIKE> double stddev( const VECTORLIKE& v, bool unbiased = true )
Computes the standard deviation of a vector.
Parameters:
v |
The set of data |
unbiased |
If set to true or false the std is normalized by “N-1” or “N”, respectively. |
See also:
template <class VECTOR_OF_VECTOR, class VECTORLIKE, class MATRIXLIKE> void meanAndCovVec( const VECTOR_OF_VECTOR& v, VECTORLIKE& out_mean, MATRIXLIKE& out_cov )
Computes the mean vector and covariance from a list of values given as a vector of vectors, where each row is a sample.
Parameters:
v |
The set of data, as a vector of N vectors of M elements. |
out_mean |
The output M-vector for the estimated mean. |
out_cov |
The output MxM matrix for the estimated covariance matrix. |
See also:
mrpt::math::meanAndCovMat, math::mean, math::stddev, math::cov
template <class VECTOR_OF_VECTOR, class RETURN_MATRIX> RETURN_MATRIX covVector(const VECTOR_OF_VECTOR& v)
Computes the covariance matrix from a list of values given as a vector of vectors, where each row is a sample.
Parameters:
v |
The set of data, as a vector of N vectors of M elements. |
out_cov |
The output MxM matrix for the estimated covariance matrix. |
RETURN_MATRIX |
The type of the returned matrix, e.g. Eigen::MatrixXd |
See also:
math::mean, math::stddev, math::cov, meanAndCovVec
template <class CONT1, class CONT2> double ncc_vector( const CONT1& a, const CONT2& b )
Normalized Cross Correlation coefficient between two 1-D vectors, returning a single scalar between [-1, 1].
It is equivalent to the following Matlab code:
a = a - mean2(a); b = b - mean2(b); r = sum(sum(a.*b))/sqrt(sum(sum(a.*a))*sum(sum(b.*b)));
Parameters:
CONT1 |
A std::vector<double>, Eigen or mrpt::math vectors. |
CONT2 |
A std::vector<double>, Eigen or mrpt::math vectors. |
See also:
template <class VECTOR> VECTOR xcorr( const VECTOR& a, const VECTOR& b, const size_t maxLag, bool normalized = true )
Normalized Cross Correlation between two 1-D vectors, returning a vector of scalars, with a peak at the position revealing the offset of “b” that makes the two signals most similar:
r = xcorr(a, b, maxLag); lags = mrpt::math::linspace(-maxLag, maxLag);
Where:
aandbare the input signals.ris the output cross correlation vector. Its length ismaxLag*2+1.maxLagis the maximum lag to search for.lags: If needed, it can be generated withlinspace(): it will hold the “delay counts” for each corresponding entry inr, the sequence of integers [-maxLag, maxLag].
(New in MRPT 2.3.3)
Parameters:
VECTOR |
A std::vector<double>, Eigen or mrpt::math vectors. |
See also:
template <class CONTAINER, class POINT_OR_POSE> CONTAINER& containerFromPoseOrPoint( CONTAINER& C, const POINT_OR_POSE& p )
Conversion of poses to MRPT containers (vector/matrix)
template <class MATRIXLIKE1, class MATRIXLIKE2> void homogeneousMatrixInverse( const MATRIXLIKE1& M, MATRIXLIKE2& out_inverse_M )
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: CMatrixDynamic, CMatrixFixed
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 )
This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.
template <class MATRIXLIKE> void homogeneousMatrixInverse(MATRIXLIKE& M)
This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.
template <typename DER> void internalAssertEigenDefined()
Issues a static_assert() error if trying to compile a method that requires Eigen headers, without including them.
template <typename Scalar, class Derived> std::ostream& operator << ( std::ostream& o, const MatrixVectorBase<Scalar, Derived>& m )
Stream as text.
Implemented for all matrices and vectors, except for non-square fixed-size matrices.
template <class CONTAINER> std::vector<double> histogram( const CONTAINER& v, double limit_min, double limit_max, size_t number_bins, bool do_normalization = false, std::vector<double>* out_bin_centers = nullptr )
Computes the normalized or normal histogram of a sequence of numbers given the number of bins and the limits.
In any case this is a “linear” histogram, i.e. for matrices, all the elements are taken as if they were a plain sequence, not taking into account they were in columns or rows. If desired, out_bin_centers can be set to receive the bins centers.
template <class CONTAINER1, class CONTAINER2> void cumsum_tmpl( const CONTAINER1& in_data, CONTAINER2& out_cumsum )
Computes the cumulative sum of all the elements, saving the result in another container.
This works for both matrices (even mixing their types) and vectores/arrays (even mixing types), and even to store the cumsum of any matrix into any vector/array, but not in opposite direction.
See also:
template <class CONTAINER> CONTAINER cumsum(const CONTAINER& in_data)
Computes the cumulative sum of all the elements.
See also:
template <typename MAT_H, typename MAT_C, typename MAT_R> void multiply_HCHt( const MAT_H& H, const MAT_C& C, MAT_R& R, bool accumResultInOutput = false )
R = H * C * H^t.
template <matrix_dim_t H_ROWS, matrix_dim_t H_COLS, typename Scalar> mrpt::math::CMatrixFixed<Scalar, H_ROWS, H_ROWS> multiply_HCHt( const mrpt::math::CMatrixFixed<Scalar, H_ROWS, H_COLS>& H, const mrpt::math::CMatrixFixed<Scalar, H_COLS, H_COLS>& C )
return a fixed-size matrix with the result of: H * C * H^t
template <typename VECTOR_H, typename MAT_C> MAT_C::Scalar multiply_HtCH_scalar( const VECTOR_H& H, const MAT_C& C )
r (scalar) = H^t*C*H (H: column vector, C: symmetric matrix)
template <typename VECTOR_H, typename MAT_C> MAT_C::Scalar multiply_HCHt_scalar( const VECTOR_H& H, const MAT_C& C )
r (scalar) = H*C*H^t (H: row vector, C: symmetric matrix)
template <class MAT_IN, class VECTOR, class MAT_OUT> void meanAndCovMat( const MAT_IN& v, VECTOR& out_mean, MAT_OUT& out_cov )
Computes the mean vector and covariance from a list of samples in an NxM matrix, where each row is a sample, so the covariance is MxM.
Parameters:
v |
The set of data as a NxM matrix, of types: CMatrixDynamic or CMatrixFixed |
out_mean |
The output M-vector for the estimated mean. |
out_cov |
The output MxM matrix for the estimated covariance matrix, this can also be either a fixed-size of dynamic size matrix. |
See also:
mrpt::math::meanAndCovVec, math::mean, math::stddev, math::cov
template <class MAT_IN, class VEC> void meanAndStdColumns( const MAT_IN& m, VEC& outMeanVector, VEC& outStdVector, const bool unbiased_variance = true )
Computes a row with the mean values of each column in the matrix and the associated vector with the standard deviation of each column.
Parameters:
std::exception |
If the matrix/vector is empty. |
unbiased_variance |
Standard deviation is sum(vals-mean)/K, with K=N-1 or N for unbiased_variance=true or false, respectively. |
See also:
mean,meanAndStdAll
template <class MATRIX> CMatrixDouble cov(const MATRIX& v)
Computes the covariance matrix from a list of samples in an NxM matrix, where each row is a sample, so the covariance is MxM.
Parameters:
v |
The set of data, as a NxM matrix. |
out_cov |
The output MxM matrix for the estimated covariance matrix. |
See also:
math::mean, math::stddev, math::cov
template <class MAT_A, class SKEW_3VECTOR, class MAT_OUT> void multiply_A_skew3( const MAT_A& A, const SKEW_3VECTOR& v, MAT_OUT& out )
Only for vectors/arrays “v” of length3, compute out = A * Skew(v), where Skew(v) is the skew symmetric matric generated from v (see mrpt::math::skew_symmetric3)
template <class SKEW_3VECTOR, class MAT_A, class MAT_OUT> void multiply_skew3_A( const SKEW_3VECTOR& v, const MAT_A& A, MAT_OUT& out )
Only for vectors/arrays “v” of length3, compute out = Skew(v) * A, where Skew(v) is the skew symmetric matric generated from v (see mrpt::math::skew_symmetric3)
template <typename MATIN, typename MATOUT> void laplacian( const MATIN& g, MATOUT& ret )
Computes the Laplacian of a square graph weight matrix.
The laplacian matrix is L = D - W, with D a diagonal matrix with the degree of each node, W the edge weights.
template <std::size_t BLOCKSIZE, typename MAT, typename MATRIX> void extractSubmatrixSymmetricalBlocks( const MAT& m, const std::vector<size_t>& block_indices, MATRIX& out )
Get a submatrix from a square matrix, by collecting the elements M(idxs,idxs), where idxs is a sequence {block_indices(i):block_indices(i)+BLOCKSIZE-1} for all “i” up to the size of block_indices.
A perfect application of this method is in extracting covariance matrices of a subset of variables from the full covariance matrix.
See also:
extractSubmatrix, extractSubmatrixSymmetrical
template <typename MAT, typename MATRIX> void extractSubmatrixSymmetrical( const MAT& m, const std::vector<size_t>& indices, MATRIX& out )
Get a submatrix from a square matrix, by collecting the elements M(idxs,idxs), where idxs is the sequence of indices passed as argument.
A perfect application of this method is in extracting covariance matrices of a subset of variables from the full covariance matrix.
See also:
extractSubmatrix, extractSubmatrixSymmetricalBlocks
std::ostream& operator << (std::ostream& o, const TPolygon2D& p)
Text streaming function.
std::ostream& operator << (std::ostream& o, const TPolygon3D& p)
Text streaming function.
bool operator == (const TPose2D& p1, const TPose2D& p2)
Exact comparison between 2D poses, taking possible cycles into account.
bool operator != (const TPose2D& p1, const TPose2D& p2)
Exact comparison between 2D poses, taking possible cycles into account.
TPose3D operator - (const TPose3D& p)
Unary ⊖ operator: returns the SE(3) group inverse T^{-1}.
Note: this is not the elementwise negation of (x,y,z,yaw,pitch,roll).
TPose3D operator - (const TPose3D& b, const TPose3D& a)
Binary ⊖ operator: b ⊖ a = a^{-1} ⊕ b — the pose of b expressed in the frame of a.
In matrix form: T_a^{-1} * T_b.
bool operator == (const TPose3D& p1, const TPose3D& p2)
Exact comparison between 3D poses, taking possible cycles into account.
bool operator != (const TPose3D& p1, const TPose3D& p2)
Exact comparison between 3D poses, taking possible cycles into account.
bool operator == (const TPose3DQuat& a, const TPose3DQuat& b)
Exact equality between two quaternion poses (bitwise, no tolerance).
std::ostream& operator << (std::ostream& o, const TSegment2D& p)
Text streaming function.
std::ostream& operator << (std::ostream& o, const TSegment3D& p)
Text streaming function.
template <class T1, class T2> bool approximatelyEqual(T1 a, T1 b, T2 epsilon)
Compare 2 floats and determine whether they are equal.
Parameters:
a |
Fist num |
b |
Second num |
epsilon |
Difference below which a, b are considered equal |
Returns:
True if equal, false otherwise
template <class T> bool approximatelyEqual(T a, T b)
Compare 2 floats and determine whether they are equal.
Parameters:
a |
Fist num |
b |
Second num |
Returns:
True if equal, false otherwise
template <class T> T absDiff(const T& lhs, const T& rhs)
Absolute difference between two numbers.
template <typename T> bool ransac3Dplane_degenerate( ] const CMatrixDynamic<T>& allData, ] const std::vector<size_t>& useIndices )
Return “true” if the selected points are a degenerate (invalid) case.
template <typename T> bool ransac2Dline_degenerate( ] const CMatrixDynamic<T>& allData, ] const std::vector<size_t>& useIndices )
Return “true” if the selected points are a degenerate (invalid) case.