44 static void getPlanes(
const std::vector<TPolygon3D> &oldPolys,std::vector<TPolygonWithPlane> &newPolys);
289 template<
class T>
void project3D(
const T &obj,
const TPlane &newXYPlane,T &newObj) {
305 size_t N=objs.size();
307 for (
size_t i=0;i<N;i++)
project3D(objs[i],newXYpose,newObjs[i]);
327 template<
class T,
class CPOSE2D>
void project2D(
const T &obj,
const TLine2D &newXLine,T &newObj) {
330 project2D(obj,CPOSE2D(0,0,0)-pose,newObj);
336 template<
class T,
class CPOSE2D>
void project2D(
const T &obj,
const TLine2D &newXLine,
const TPoint2D &newOrigin,T &newObj) {
344 size_t N=objs.size();
346 for (
size_t i=0;i<N;i++)
project2D(objs[i],newXpose,newObjs[i]);
394 size_t BASE_IMPEXP intersect(
const std::vector<TPolygon3D> &v1,
const std::vector<TPolygon3D> &v2,std::vector<TObject3D> &objs);
404 size_t M=v1.size(),N=v2.size();
408 for (
size_t i=0;i<M;i++)
for (
size_t j=0;j<M;j++)
if (
intersect(v1[i],v2[j],obj)) objs(i,j)=obj;
414 template<
class T,
class U,
class O>
size_t intersect(
const std::vector<T> &v1,
const std::vector<U> &v2,std::vector<O> objs) {
541 void BASE_IMPEXP assemblePolygons(
const std::vector<TSegment3D> &segms,std::vector<TPolygon3D> &polys,std::vector<TSegment3D> &remainder);
549 void BASE_IMPEXP assemblePolygons(
const std::vector<TObject3D> &objs,std::vector<TPolygon3D> &polys,std::vector<TObject3D> &remainder);
553 void BASE_IMPEXP assemblePolygons(
const std::vector<TObject3D> &objs,std::vector<TPolygon3D> &polys,std::vector<TSegment3D> &remainder1,std::vector<TObject3D> &remainder2);
608 std::vector<TPolygonWithPlane> pwp;
624 template<
class T,
class U,
class V>
626 vOut[0]=v0[1]*v1[2]-v0[2]*v1[1];
627 vOut[1]=v0[2]*v1[0]-v0[0]*v1[2];
628 vOut[2]=v0[0]*v1[1]-v0[1]*v1[0];
634 const std::vector<T> &v0,
635 const std::vector<T> &v1,
636 std::vector<T> &v_out )
641 v_out[0] = v0[1]*v1[2] - v0[2]*v1[1];
642 v_out[1] = -v0[0]*v1[2] + v0[2]*v1[0];
643 v_out[2] = v0[0]*v1[1] - v0[1]*v1[0];
647 template<
class VEC1,
class VEC2>
648 inline Eigen::Matrix<double,3,1>
crossProduct3D(
const VEC1 &v0,
const VEC2 &v1) {
649 Eigen::Matrix<double,3,1> vOut;
650 vOut[0]=v0[1]*v1[2]-v0[2]*v1[1];
651 vOut[1]=v0[2]*v1[0]-v0[0]*v1[2];
652 vOut[2]=v0[0]*v1[1]-v0[1]*v1[0];
665 template<
class VECTOR,
class MATRIX>
669 M.set_unsafe(0,0, 0); M.set_unsafe(0,1, -v[2]); M.set_unsafe(0,2, v[1]);
670 M.set_unsafe(1,0, v[2]); M.set_unsafe(1,1, 0); M.set_unsafe(1,2, -v[0]);
671 M.set_unsafe(2,0, -v[1]); M.set_unsafe(2,1, v[0]); M.set_unsafe(2,2, 0);
674 template<
class VECTOR>
690 template<
class VECTOR,
class MATRIX>
694 M.set_unsafe(0,0, 0); M.set_unsafe(0,1, v[2]); M.set_unsafe(0,2, -v[1]);
695 M.set_unsafe(1,0, -v[2]); M.set_unsafe(1,1, 0); M.set_unsafe(1,2, v[0]);
696 M.set_unsafe(2,0, v[1]); M.set_unsafe(2,1, -v[0]); M.set_unsafe(2,2, 0);
699 template<
class VECTOR>
717 template<
class T,
class U>
719 if (abs(v1[0]*v2[1]-v2[0]*v1[1])>=geometryEpsilon)
return false;
720 if (abs(v1[1]*v2[2]-v2[1]*v1[2])>=geometryEpsilon)
return false;
762 template <
typename T>
768 template <
typename T>
774 template <
typename T>
780 template <
typename T>
787 template <
typename T>
800 out_x =
static_cast<T
>(ox);
801 out_y =
static_cast<T
>(oy);
809 const double x1,
const double y1,
810 const double x2,
const double y2,
811 const double x3,
const double y3,
812 const double x4,
const double y4,
813 double &ix,
double &iy);
818 const double x1,
const double y1,
819 const double x2,
const double y2,
820 const double x3,
const double y3,
821 const double x4,
const double y4,
822 float &ix,
float &iy);
827 bool BASE_IMPEXP pointIntoPolygon2D(
const double & px,
const double & py,
unsigned int polyEdges,
const double *poly_xs,
const double *poly_ys );
832 template <
typename T>
842 const T
a1 =
atan2( v1y - y , v1x - x );
843 const T
a2 =
atan2( v2y - y , v2x - x );
844 const T
a3 =
atan2( v3y - y , v3x - x );
845 const T a4 =
atan2( v4y - y , v4x - x );
851 if (
sign(da1)!=
sign(da2))
return false;
854 if (
sign(da2)!=
sign(da3))
return false;
872 const double & p1_x,
const double & p1_y,
const double & p1_z,
873 const double & p2_x,
const double & p2_y,
const double & p2_z,
874 const double & p3_x,
const double & p3_y,
const double & p3_z,
875 const double & p4_x,
const double & p4_y,
const double & p4_z,
876 double &x,
double &y,
double &
z,
886 const double & R1_x_min,
const double & R1_x_max,
887 const double & R1_y_min,
const double & R1_y_max,
888 const double & R2_x_min,
const double & R2_x_max,
889 const double & R2_y_min,
const double & R2_y_max,
890 const double & R2_pose_x,
891 const double & R2_pose_y,
892 const double & R2_pose_phi );
931 if (dx==0 && dy==0 && dz==0)
938 T n = sqrt(n_xy+
square(dz));
945 if (fabs(dx)>1
e-4 || fabs(dy)>1
e-4)
bool BASE_IMPEXP conformAPlane(const std::vector< TPoint3D > &points)
Checks whether this polygon or set of points acceptably fits a plane.
bool BASE_IMPEXP pointIntoPolygon2D(const double &px, const double &py, unsigned int polyEdges, const double *poly_xs, const double *poly_ys)
Returns true if the 2D point (px,py) falls INTO the given polygon.
double BASE_IMPEXP distancePointToPolygon2D(const double &px, const double &py, unsigned int polyEdges, const double *poly_xs, const double *poly_ys)
Returns the closest distance of a given 2D point to a polygon, or "0" if the point is INTO the polygo...
double BASE_IMPEXP getAngle(const TPlane &p1, const TPlane &p2)
Computes the angle between two planes.
void skew_symmetric3(const VECTOR &v, MATRIX &M)
Computes the 3x3 skew symmetric matrix from a 3-vector or 3-array: .
size_t getNonNullElements() const
Gets the amount of non-null elements inside the matrix.
#define THROW_EXCEPTION(msg)
bool BASE_IMPEXP splitInConvexComponents(const TPolygon2D &poly, std::vector< TPolygon2D > &components)
Splits a 2D polygon into convex components.
Slightly heavyweight type to speed-up calculations with polygons in 3D.
bool vectorsAreParallel2D(const T &v1, const U &v2)
Returns true if two 2D vectors are parallel.
int sign(T x)
Returns the sign of X as "1" or "-1".
Standard type for storing any lightweight 2D type.
void getAsPose3D(mrpt::poses::CPose3D &outPose)
Gets a pose whose XY plane corresponds to this plane.
Standard object for storing any 3D lightweight object.
bool BASE_IMPEXP areAligned(const std::vector< TPoint2D > &points)
Checks whether this set of points acceptably fits a 2D line.
const Scalar * const_iterator
double z
X,Y,Z coordinates.
bool vectorsAreParallel3D(const T &v1, const U &v2)
Returns true if two 3D vectors are parallel.
static void getPlanes(const std::vector< TPolygon3D > &oldPolys, std::vector< TPolygonWithPlane > &newPolys)
Static method for vectors.
A sparse matrix container (with cells of any type), with iterators.
double BASE_IMPEXP getRegressionLine(const std::vector< TPoint2D > &points, TLine2D &line)
Using eigenvalues, gets the best fitting line for a set of 2D points.
void crossProduct3D(const T &v0, const U &v1, V &vOut)
Computes the cross product of two 3D vectors, returning a vector normal to both.
TPoint3D point1
Origin point.
T square(const T x)
Inline function for the square of a number.
TPolygonWithPlane()
Basic constructor.
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: ...
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.
A numeric matrix of compile-time fixed size.
2D segment, consisting of two points.
3D segment, consisting of two points.
bool BASE_IMPEXP 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)
Returns the intersection point, and if it exists, between two segments.
void BASE_IMPEXP createFromPoseAndVector(const mrpt::poses::CPose3D &p, const double(&vector)[3], TLine3D &r)
Gets a 3D line corresponding to any arbitrary vector, in the base given by the pose.
TPoint3D point2
Destiny point.
void BASE_IMPEXP getRectangleBounds(const std::vector< TPoint2D > &poly, TPoint2D &pMin, TPoint2D &pMax)
Gets the rectangular bounds of a 2D polygon or set of 2D points.
TPolygon3D poly
Actual polygon.
double BASE_IMPEXP closestSquareDistanceFromPointToLine(const double &Px, const double &Py, const double &x1, const double &y1, const double &x2, const double &y2)
Returns the square distance from a point to a line.
void BASE_IMPEXP project2D(const TPoint2D &point, const mrpt::poses::CPose2D &newXpose, TPoint2D &newPoint)
Uses the given pose 2D to project a point into a new base.
void setEpsilon(double nE)
Changes the value of the geometric epsilon.
3D Plane, represented by its equation
void BASE_IMPEXP createPlaneFromPoseAndNormal(const mrpt::poses::CPose3D &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...
TPoint2D point2
Destiny point.
TPolygon2D poly2D
Polygon, after being projected to the plane using inversePose.
void composePoint(double lx, double ly, double lz, double &gx, double &gy, double &gz, mrpt::math::CMatrixFixedNumeric< double, 3, 3 > *out_jacobian_df_dpoint=NULL, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dpose=NULL, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dse3=NULL, bool use_small_rot_approx=false) const
An alternative, slightly more efficient way of doing with G and L being 3D points and P this 6D pose...
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
int sign(T x)
Returns the sign of X as "1" or "-1".
mrpt::poses::CPose3D pose
Plane's pose.
void BASE_IMPEXP createFromPoseX(const mrpt::poses::CPose3D &p, TLine3D &r)
Gets a 3D line corresponding to the X axis in a given pose.
TPoint2D point1
Origin point.
void clear()
Completely removes all elements, although maintaining the matrix's size.
void BASE_IMPEXP createFromPoseZ(const mrpt::poses::CPose3D &p, TLine3D &r)
Gets a 3D line corresponding to the Z axis in a given pose.
void BASE_IMPEXP createPlaneFromPoseXZ(const mrpt::poses::CPose3D &pose, TPlane &plane)
Given a pose, creates a plane orthogonal to its Y vector.
bool BASE_IMPEXP minDistBetweenLines(const double &p1_x, const double &p1_y, const double &p1_z, const double &p2_x, const double &p2_y, const double &p2_z, const double &p3_x, const double &p3_y, const double &p3_z, const double &p4_x, const double &p4_y, const double &p4_z, double &x, double &y, double &z, double &dist)
Calculates the minimum distance between a pair of lines.
void getAsPose2D(mrpt::poses::CPose2D &outPose) const
Get a pose2D whose X axis corresponds to the line.
bool BASE_IMPEXP RectanglesIntersection(const double &R1_x_min, const double &R1_x_max, const double &R1_y_min, const double &R1_y_max, const double &R2_x_min, const double &R2_x_max, const double &R2_y_min, const double &R2_y_max, const double &R2_pose_x, const double &R2_pose_y, const double &R2_pose_phi)
Returns whether two rotated rectangles intersect.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
CMatrixTemplateNumeric< T > generateAxisBaseFromDirection(T dx, T dy, T dz)
Computes an axis base (a set of three 3D normal vectors) with the given vector being the first of the...
void BASE_IMPEXP closestFromPointToLine(const double &Px, const double &Py, const double &x1, const double &y1, const double &x2, const double &y2, double &out_x, double &out_y)
Computes the closest point from a given point to a (infinite) line.
void BASE_IMPEXP createPlaneFromPoseXY(const mrpt::poses::CPose3D &pose, TPlane &plane)
Given a pose, creates a plane orthogonal to its Z vector.
void BASE_IMPEXP closestFromPointToSegment(const double &Px, const double &Py, const double &x1, const double &y1, const double &x2, const double &y2, double &out_x, double &out_y)
Computes the closest point from a given point to a segment.
void BASE_IMPEXP assemblePolygons(const std::vector< TSegment3D > &segms, std::vector< TPolygon3D > &polys)
Tries to assemble a set of segments into a set of closed polygons.
double BASE_IMPEXP getRegressionPlane(const std::vector< TPoint3D > &points, TPlane &plane)
Using eigenvalues, gets the best fitting plane for a set of 3D points.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
double getEpsilon()
Gets the value of the geometric epsilon.
void resize(size_t nRows, size_t nCols)
Changes the size of the matrix.
void project3D(const TPoint3D &point, const mrpt::poses::CPose3D &newXYpose, TPoint3D &newPoint)
Uses the given pose 3D to project a point into a new base.
x y t t *t x y t t t x y t t t x *y t *t t x *y t *t t x y t t t x y t t t x(y+z)
mrpt::poses::CPose3D inversePose
Plane's inverse pose.
bool BASE_IMPEXP intersect(const TSegment3D &s1, const TSegment3D &s2, TObject3D &obj)
Gets the intersection between two 3D segments.
bool BASE_IMPEXP traceRay(const std::vector< TPolygonWithPlane > &vec, const mrpt::poses::CPose3D &pose, double &dist)
Fast ray tracing method using polygons' properties.
void getAsPose2DForcingOrigin(const TPoint2D &origin, mrpt::poses::CPose2D &outPose) const
Get a pose2D whose X axis corresponds to the line, forcing the base point to one given.
T distanceBetweenPoints(const T x1, const T y1, const T x2, const T y2)
Returns the distance between 2 points in 2D.
void BASE_IMPEXP getAngleBisector(const TLine2D &l1, const TLine2D &l2, TLine2D &bis)
Gets the bisector of two lines or segments (implicit constructor will be used if necessary) ...
T distanceSqrBetweenPoints(const T x1, const T y1, const T x2, const T y2)
Returns the square distance between 2 points in 2D.
double BASE_IMPEXP geometryEpsilon
Global epsilon to overcome small precision errors (Default=1e-5)
TPlane plane
Plane containing the polygon.
void BASE_IMPEXP getSegmentBisector(const TSegment2D &sgm, TLine2D &bis)
Gets the bisector of a 2D segment.
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...
2D polygon, inheriting from std::vector<TPoint2D>.
void BASE_IMPEXP getPrismBounds(const std::vector< TPoint3D > &poly, TPoint3D &pMin, TPoint3D &pMax)
Gets the prism bounds of a 3D polygon or set of 3D points.
3D polygon, inheriting from std::vector<TPoint3D>
void BASE_IMPEXP createFromPoseY(const mrpt::poses::CPose3D &p, TLine3D &r)
Gets a 3D line corresponding to the Y axis in a given pose.
T atan2(const T v0, const T v1)
void BASE_IMPEXP createPlaneFromPoseYZ(const mrpt::poses::CPose3D &pose, TPlane &plane)
Given a pose, creates a plane orthogonal to its X vector.
double BASE_IMPEXP distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
void BASE_IMPEXP generateAxisBaseFromDirectionAndAxis(const double(&vec)[3], char coord, CMatrixDouble &matrix)
Creates a homogeneus matrix (4x4) such that the coordinate given (0 for x, 1 for y, 2 for z) corresponds to the provided vector.
3D line, represented by a base point and a director vector.
2D line without bounds, represented by its equation .