namespace mrpt::math¶
This base provides a set of functions for maths stuff.
mrpt_math_grp
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 RANSAC_Template<double> RANSAC; 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; typedef float FFT_TYPE; // enums enum TConstructorFlags_Matrices; enum TConstructorFlags_Quaternions; enum TMatrixTextFileFormat; enum TRobustKernelType; // structs struct CExceptionNotDefPos; template <typename CONTAINER> struct ContainerType; template <typename Scalar, typename Derived> struct ContainerType<mrpt::math::MatrixVectorBase<Scalar, Derived>>; 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 <typename T> struct RobustKernel<rkLeastSquares, T>; template <TRobustKernelType KERNEL_TYPE, typename T = double> struct RobustKernel; 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, std::size_t ROWS, std::size_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; class CSparseMatrix; 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; class TPolygon2D; class TPolygon3D; class TPolygonWithPlane; // global variables static constexpr unsigned char GEOMETRIC_TYPE_POINT = 0; static constexpr unsigned char GEOMETRIC_TYPE_SEGMENT = 1; static constexpr unsigned char GEOMETRIC_TYPE_LINE = 2; static constexpr unsigned char GEOMETRIC_TYPE_POLYGON = 3; static constexpr unsigned char GEOMETRIC_TYPE_PLANE = 4; static constexpr unsigned char GEOMETRIC_TYPE_UNDEFINED = 255; // global functions template <class VECTORLIKE1, class VECTORLIKE2, class MAT> MAT::Scalar mahalanobisDistance2( 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 mahalanobisDistance2( 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 mahalanobisDistance2( 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 mahalanobisDistance2AndLogPDF( const VECLIKE& diff_mean, const MATRIXLIKE& cov, T& maha2_out, T& log_pdf_out ); template <typename T, class VECLIKE, class MATRIXLIKE> void mahalanobisDistance2AndPDF( 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); void createFromPoseX(const mrpt::math::TPose3D& p, TLine3D& r); void createFromPoseY(const mrpt::math::TPose3D& p, TLine3D& r); void createFromPoseZ(const mrpt::math::TPose3D& p, TLine3D& r); void createFromPoseAndVector(const mrpt::math::TPose3D& p, const double(&) vector [3], TLine3D& r); void createFromPoseX(const TPose2D& p, TLine2D& r); void createFromPoseY(const TPose2D& p, TLine2D& r); void createFromPoseAndVector(const TPose2D& p, const double(&) vector [2], TLine2D& r); 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 <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 <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); bool intersect(const TPolygon2D& p1, const TPolygon2D& p2, TObject2D& obj); 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 ); void closestFromPointToLine( double Px, double Py, double x1, double y1, double x2, double y2, double& out_x, double& out_y ); double closestSquareDistanceFromPointToLine(double Px, double Py, double x1, double y1, double x2, double y2); 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 ); bool SegmentsIntersection( const double x1, const double y1, const double x2, const double y2, const double x3, const double y3, const double x4, const double y4, double& ix, double& iy ); bool SegmentsIntersection( const double x1, const double y1, const double x2, const double y2, const double x3, const double y3, const double x4, const double y4, float& ix, float& iy ); bool pointIntoPolygon2D( double px, double py, unsigned int polyEdges, const double* poly_xs, const double* poly_ys ); 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 ); double distancePointToPolygon2D( double px, double py, unsigned int polyEdges, const double* poly_xs, const double* poly_ys ); bool minDistBetweenLines( double p1_x, double p1_y, double p1_z, double p2_x, double p2_y, double p2_z, double p3_x, double p3_y, double p3_z, double p4_x, double p4_y, double p4_z, double& x, double& y, double& z, double& dist ); 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); template <class LIST_OF_VECTORS1, class LIST_OF_VECTORS2> double kmeans( const size_t k, const LIST_OF_VECTORS1& points, std::vector<int>& assignments, LIST_OF_VECTORS2* out_centers = nullptr, const size_t attempts = 3 ); template <class LIST_OF_VECTORS1, class LIST_OF_VECTORS2 = LIST_OF_VECTORS1> double kmeanspp( const size_t k, const LIST_OF_VECTORS1& points, std::vector<int>& assignments, LIST_OF_VECTORS2* out_centers = nullptr, const size_t attempts = 3 ); template <size_t NROWS, size_t NCOLS> mrpt::serialization::CArchive& operator >> (mrpt::serialization::CArchive& in, CMatrixFixed<float, NROWS, NCOLS>& M); template <size_t NROWS, size_t NCOLS> mrpt::serialization::CArchive& operator >> (mrpt::serialization::CArchive& in, CMatrixFixed<double, NROWS, NCOLS>& M); template <size_t NROWS, size_t NCOLS> mrpt::serialization::CArchive& operator << ( mrpt::serialization::CArchive& out, const CMatrixFixed<float, NROWS, NCOLS>& M ); template <size_t NROWS, size_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> void minimum_maximum(const std::vector<T>& 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& patch1, const CONT2& patch2 ); 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, const 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, const 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, const 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 ); template <class MATRIXLIKE> size_t size(const MATRIXLIKE& m, const int dim); mrpt::serialization::CArchive& operator >> ( mrpt::serialization::CArchive& in, CMatrixD::Ptr& pObj ); mrpt::serialization::CArchive& operator >> ( mrpt::serialization::CArchive& in, CMatrixF::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 ); 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, typename = std::enable_if_t<Derived::RowsAtCompileTime == Derived::ColsAtCompileTime ||(Derived::ColsAtCompileTime == 1)> > 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 <std::size_t H_ROWS, std::size_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, size_t N> mrpt::serialization::CArchive& operator << (mrpt::serialization::CArchive& ostrm, const CVectorFixed<T, N>& a); template <typename T, size_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_poly4Bi(double* x, double b, double d); int solve_poly4De(double* x, double b, double c, double d); int solve_poly2(double a, double b, double c, double& r1, double& r2); template <typename T> size_t ransacDatasetSize(const CMatrixDynamic<T>& dataset); 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 ); 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 ); 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 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> 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); 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); 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 ); 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 ); 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 <typename MATRIX> mxArray* convertToMatlab(const MATRIX& mat); template <typename CONTAINER> mxArray* convertVectorToMatlab(const CONTAINER& vec); 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); static void four1( float data [], unsigned long nn, int isign ); static void realft( float data [], unsigned long n ); static void makewt( int nw, int* ip, FFT_TYPE* w ); static void bitrv2(int n, int* ip, FFT_TYPE* a); static void cftbsub( int n, FFT_TYPE* a, FFT_TYPE* w ); static void cftfsub( int n, FFT_TYPE* a, FFT_TYPE* w ); static void rftfsub( int n, FFT_TYPE* a, int nc, FFT_TYPE* c ); static void rftbsub( int n, FFT_TYPE* a, int nc, FFT_TYPE* c ); static void makect(int nc, int* ip, FFT_TYPE* c); static void cdft(int n, int isgn, FFT_TYPE* a, int* ip, FFT_TYPE* w); static void rdft( int n, int isgn, FFT_TYPE* a, int* ip, FFT_TYPE* w ); static void rdft2d(int n1, int n2, int isgn, FFT_TYPE** a, FFT_TYPE* t, int* ip, FFT_TYPE* w); static void cdft2d(int n1, int n2, int isgn, FFT_TYPE** a, FFT_TYPE* t, int* ip, FFT_TYPE* w); 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 ); } // namespace math
Typedefs¶
typedef CLevenbergMarquardtTempl<mrpt::math::CVectorDouble> CLevenbergMarquardt
The default name for the LM class is an instantiation for “double”.
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 TPoint2D TVector2D
Useful type alias for double 2-vectors.
typedef TPoint2Df TVector2Df
Useful type alias for float 2-vectors.
typedef TPoint3D_<double> TPoint3D
Lightweight 3D point.
Allows coordinate access using [] operator. (1-byte memory packed, no padding).
See also:
mrpt::poses::CPoint3D, mrpt::math::TPoint3Df
typedef TPoint3D TVector3D
Useful type alias for 3-vectors.
(1-byte memory packed, no padding)
typedef float FFT_TYPE
Copyright(C) 1997 Takuya OOURA (email: ooura@mmm.t.u-tokyo.ac.jp).
You may use, copy, modify this code for any purpose and without fee. You may distribute this ORIGINAL package.
Global Variables¶
static constexpr unsigned char GEOMETRIC_TYPE_POINT = 0
Object type identifier for TPoint2D or TPoint3D.
See also:
static constexpr unsigned char GEOMETRIC_TYPE_SEGMENT = 1
Object type identifier for TSegment2D or TSegment3D.
See also:
static constexpr unsigned char GEOMETRIC_TYPE_LINE = 2
Object type identifier for TLine2D or TLine3D.
See also:
static constexpr unsigned char GEOMETRIC_TYPE_POLYGON = 3
Object type identifier for TPolygon2D or TPolygon3D.
See also:
static constexpr unsigned char GEOMETRIC_TYPE_PLANE = 4
Object type identifier for TPlane.
See also:
static constexpr unsigned char GEOMETRIC_TYPE_UNDEFINED = 255
Object type identifier for empty TObject2D or TObject3D.
See also:
Global Functions¶
template <class LIST_OF_VECTORS1, class LIST_OF_VECTORS2> double kmeans( const size_t k, const LIST_OF_VECTORS1& points, std::vector<int>& assignments, LIST_OF_VECTORS2* out_centers = nullptr, const size_t attempts = 3 )
k-means algorithm to cluster a list of N points of arbitrary dimensionality into exactly K clusters.
The list of input points can be any template CONTAINER<POINT> with:
CONTAINER can be: Any STL container: std::vector,std::list,std::deque,…
POINT can be:
std::vector<double/float>
CVectorFixedDouble<N> / CVectorFixedFloat<N>
Uses the kmeans++ implementation by David Arthur (2009, http://www.stanford.edu/~darthur/kmpp.zip).
Parameters:
k |
[IN] Number of cluster to look for. |
points |
[IN] The list of N input points. It can be any STL-like containers of std::vector<float/double>, for example a std::vector<mrpt::math::CVectorDouble>, a std::list<CVectorFloat>, etc… |
assignments |
[OUT] At output it will have a number [0,k-1] for each of the N input points. |
out_centers |
[OUT] If not nullptr, at output will have the centers of each group. Can be of any of the supported types of “points”, but the basic coordinates should be float or double exactly as in “points”. |
attempts |
[IN] Number of attempts. |
See also:
A more advanced algorithm, see: kmeanspp
template <class LIST_OF_VECTORS1, class LIST_OF_VECTORS2 = LIST_OF_VECTORS1> double kmeanspp( const size_t k, const LIST_OF_VECTORS1& points, std::vector<int>& assignments, LIST_OF_VECTORS2* out_centers = nullptr, const size_t attempts = 3 )
k-means++ algorithm to cluster a list of N points of arbitrary dimensionality into exactly K clusters.
The list of input points can be any template CONTAINER<POINT> with:
CONTAINER can be: Any STL container: std::vector,std::list,std::deque,…
POINT can be:
std::vector<double/float>
CVectorFixedDouble<N> / CVectorFixedFloat<N>
Uses the kmeans++ implementation by David Arthur (2009, http://www.stanford.edu/~darthur/kmpp.zip).
Parameters:
k |
[IN] Number of cluster to look for. |
points |
[IN] The list of N input points. It can be any STL-like containers of std::vector<float/double>, for example a std::vector<mrpt::math::CVectorDouble>, a std::list<CVectorFloat>, etc… |
assignments |
[OUT] At output it will have a number [0,k-1] for each of the N input points. |
out_centers |
[OUT] If not nullptr, at output will have the centers of each group. Can be of any of the supported types of “points”, but the basic coordinates should be float or double exactly as in “points”. |
attempts |
[IN] Number of attempts. |
See also:
The standard kmeans algorithm, see: kmeans
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> void minimum_maximum( const std::vector<T>& 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& patch1, const CONT2& patch2 )
Normalised Cross Correlation between two vector patches The Matlab code for this is a = a - mean2(a); b = b - mean2(b); r = sum(sum(a.
*b))/sqrt(sum(sum(a.*a))*sum(sum(b.*b)));
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 MATRIXLIKE> size_t size(const MATRIXLIKE& m, const int dim)
Returns the size of the matrix in the i’th dimension: 1=rows, 2=columns (MATLAB-compatible function)
Template argument MATRIXLIKE can be: mrpt::math::CMatrixDynamic, mrpt::math::CMatrixDynamic, mrpt::math::CMatrixFixed
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, typename = std::enable_if_t<Derived::RowsAtCompileTime == Derived::ColsAtCompileTime ||(Derived::ColsAtCompileTime == 1)> > 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 <std::size_t H_ROWS, std::size_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 extractSubmatrixSymmetricalBlocksDyn( const MAT& m, const std::size_t BLOCKSIZE, const std::vector<size_t>& block_indices, MATRIX& out )
This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.
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 TLine2D& p)
Text streaming function.
std::ostream& operator << (std::ostream& o, const TLine3D& p)
Text streaming function.
std::ostream& operator << (std::ostream& o, const TPlane& p)
Text streaming function.
template <typename T> constexpr bool operator == (const TPoint2D_<T>& p1, const TPoint2D_<T>& p2)
Exact comparison between 2D points.
template <typename T> constexpr bool operator != (const TPoint2D_<T>& p1, const TPoint2D_<T>& p2)
Exact comparison between 2D points.
template <typename T> constexpr TPoint3D_<T> operator - (const TPoint3D_<T>& p1)
Unary minus operator for 3D points.
template <typename T> constexpr bool operator == (const TPoint3D_<T>& p1, const TPoint3D_<T>& p2)
Exact comparison between 3D points.
template <typename T> constexpr bool operator != (const TPoint3D_<T>& p1, const TPoint3D_<T>& p2)
Exact comparison between 3D points.
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: computes inverse SE(3) element.
TPose3D operator - (const TPose3D& b, const TPose3D& a)
Binary $$ operator: $b a$ computes the relative SE(3) pose of b “as seen from” a
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.
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 )
Text streaming function.
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 )
Binary streaming function.
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 )
Binary 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.
static void bitrv2(int n, int* ip, FFT_TYPE* a)
Copyright(C) 1997 Takuya OOURA (email: ooura@mmm.t.u-tokyo.ac.jp).
You may use, copy, modify this code for any purpose and without fee. You may distribute this ORIGINAL package.
static void makect(int nc, int* ip, FFT_TYPE* c)
Copyright(C) 1997 Takuya OOURA (email: ooura@mmm.t.u-tokyo.ac.jp).
You may use, copy, modify this code for any purpose and without fee. You may distribute this ORIGINAL package.
static void cdft(int n, int isgn, FFT_TYPE* a, int* ip, FFT_TYPE* w)
Copyright(C) 1997 Takuya OOURA (email: ooura@mmm.t.u-tokyo.ac.jp).
You may use, copy, modify this code for any purpose and without fee. You may distribute this ORIGINAL package.
static void rdft2d(int n1, int n2, int isgn, FFT_TYPE** a, FFT_TYPE* t, int* ip, FFT_TYPE* w)
Copyright(C) 1997 Takuya OOURA (email: ooura@mmm.t.u-tokyo.ac.jp).
You may use, copy, modify this code for any purpose and without fee. You may distribute this ORIGINAL package.
—– Real DFT / Inverse of Real DFT —– [definition] <case1> RDFT R[k1][k2] = sum_j1=0^n1-1 sum_j2=0^n2-1 a[j1][j2] * cos(2*pi*j1*k1/n1 + 2*pi*j2*k2/n2), 0<=k1<n1, 0<=k2<n2 I[k1][k2] = sum_j1=0^n1-1 sum_j2=0^n2-1 a[j1][j2] * sin(2*pi*j1*k1/n1 + 2*pi*j2*k2/n2), 0<=k1<n1, 0<=k2<n2 <case2> IRDFT (excluding scale) a[k1][k2] = (1/2) * sum_j1=0^n1-1 sum_j2=0^n2-1 (R[j1][j2] * cos(2*pi*j1*k1/n1 + 2*pi*j2*k2/n2) + I[j1][j2] * sin(2*pi*j1*k1/n1 + 2*pi*j2*k2/n2)), 0<=k1<n1, 0<=k2<n2 (notes: R[n1-k1][n2-k2] = R[k1][k2], I[n1-k1][n2-k2] = -I[k1][k2], R[n1-k1][0] = R[k1][0], I[n1-k1][0] = -I[k1][0], R[0][n2-k2] = R[0][k2], I[0][n2-k2] = -I[0][k2], 0<k1<n1, 0<k2<n2) [usage] <case1> ip[0] = 0; // first time only rdft2d(n1, n2, 1, a, t, ip, w); <case2> ip[0] = 0; // first time only rdft2d(n1, n2, -1, a, t, ip, w); [parameters] n1 :data length (int) n1 >= 2, n1 = power of 2 n2 :data length (int) n2 >= 2, n2 = power of 2 a[0…n1-1][0…n2-1] :input/output data (FFT_TYPE **) <case1> output data a[k1][2*k2] = R[k1][k2] = R[n1-k1][n2-k2], a[k1][2*k2+1] = I[k1][k2] = -I[n1-k1][n2-k2], 0<k1<n1, 0<k2<n2/2, a[0][2*k2] = R[0][k2] = R[0][n2-k2], a[0][2*k2+1] = I[0][k2] = -I[0][n2-k2], 0<k2<n2/2, a[k1][0] = R[k1][0] = R[n1-k1][0], a[k1][1] = I[k1][0] = -I[n1-k1][0], a[n1-k1][1] = R[k1][n2/2] = R[n1-k1][n2/2], a[n1-k1][0] = -I[k1][n2/2] = I[n1-k1][n2/2], 0<k1<n1/2, a[0][0] = R[0][0], a[0][1] = R[0][n2/2], a[n1/2][0] = R[n1/2][0], a[n1/2][1] = R[n1/2][n2/2] <case2> input data a[j1][2*j2] = R[j1][j2] = R[n1-j1][n2-j2], a[j1][2*j2+1] = I[j1][j2] = -I[n1-j1][n2-j2], 0<j1<n1, 0<j2<n2/2, a[0][2*j2] = R[0][j2] = R[0][n2-j2], a[0][2*j2+1] = I[0][j2] = -I[0][n2-j2], 0<j2<n2/2, a[j1][0] = R[j1][0] = R[n1-j1][0], a[j1][1] = I[j1][0] = -I[n1-j1][0], a[n1-j1][1] = R[j1][n2/2] = R[n1-j1][n2/2], a[n1-j1][0] = -I[j1][n2/2] = I[n1-j1][n2/2], 0<j1<n1/2, a[0][0] = R[0][0], a[0][1] = R[0][n2/2], a[n1/2][0] = R[n1/2][0], a[n1/2][1] = R[n1/2][n2/2] t[0…2*n1-1] :work area (FFT_TYPE ) ip[0…] :work area for bit reversal (int ) length of ip >= 2+sqrt(n) ; if n % 4 == 0 2+sqrt(n/2); otherwise (n = max(n1, n2/2)) ip[0],ip[1] are pointers of the cos/sin table. w[0…] :cos/sin table (FFT_TYPE *) length of w >= max(n1/2, n2/4) + n2/4 w[],ip[] are initialized if ip[0] == 0. [remark] Inverse of rdft2d(n1, n2, 1, a, t, ip, w); is rdft2d(n1, n2, -1, a, t, ip, w); for (j1 = 0; j1 <= n1 - 1; j1++) { for (j2 = 0; j2 <= n2 - 1; j2++) { a[j1][j2] *= 2.0 / (n1 * n2); } }
static void cdft2d(int n1, int n2, int isgn, FFT_TYPE** a, FFT_TYPE* t, int* ip, FFT_TYPE* w)
Copyright(C) 1997 Takuya OOURA (email: ooura@mmm.t.u-tokyo.ac.jp).
You may use, copy, modify this code for any purpose and without fee. You may distribute this ORIGINAL package.
—– Complex DFT (Discrete Fourier Transform) —– [definition] <case1> X[k1][k2] = sum_j1=0^n1-1 sum_j2=0^n2-1 x[j1][j2] * exp(2*pi*i*j1*k1/n1) * exp(2*pi*i*j2*k2/n2), 0<=k1<n1, 0<=k2<n2 <case2> X[k1][k2] = sum_j1=0^n1-1 sum_j2=0^n2-1 x[j1][j2] * exp(-2*pi*i*j1*k1/n1) * exp(-2*pi*i*j2*k2/n2), 0<=k1<n1, 0<=k2<n2 (notes: sum_j=0^n-1 is a summation from j=0 to n-1) [usage] <case1> ip[0] = 0; // first time only cdft2d(n1, 2*n2, 1, a, t, ip, w); <case2> ip[0] = 0; // first time only cdft2d(n1, 2*n2, -1, a, t, ip, w); [parameters] n1 :data length (int) n1 >= 1, n1 = power of 2 2*n2 :data length (int) n2 >= 1, n2 = power of 2 a[0…n1-1][0…2*n2-1] :input/output data (double **) input data a[j1][2*j2] = Re(x[j1][j2]), a[j1][2*j2+1] = Im(x[j1][j2]), 0<=j1<n1, 0<=j2<n2 output data a[k1][2*k2] = Re(X[k1][k2]), a[k1][2*k2+1] = Im(X[k1][k2]), 0<=k1<n1, 0<=k2<n2 t[0…2*n1-1] :work area (double ) ip[0…] :work area for bit reversal (int ) length of ip >= 2+sqrt(n) ; if n % 4 == 0 2+sqrt(n/2); otherwise (n = max(n1, n2)) ip[0],ip[1] are pointers of the cos/sin table. w[0…] :cos/sin table (double *) length of w >= max(n1/2, n2/2) w[],ip[] are initialized if ip[0] == 0. [remark] Inverse of cdft2d(n1, 2*n2, -1, a, t, ip, w); is cdft2d(n1, 2*n2, 1, a, t, ip, w); for (j1 = 0; j1 <= n1 - 1; j1++) { for (j2 = 0; j2 <= 2 * n2 - 1; j2++) { a[j1][j2] *= 1.0 / (n1 * n2); } }
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.