79 static void getPlanes(
const vector<TPolygon3D> &oldPolys,vector<TPolygonWithPlane> &newPolys);
318 project3D(segment.point1,newXYpose,newSegment.point1);
319 project3D(segment.point2,newXYpose,newSegment.point2);
341 template<
class T>
void project3D(
const T &obj,
const TPlane &newXYPlane,T &newObj) {
361 size_t N=objs.size();
363 for (
size_t i=0;i<N;i++)
project3D(objs[i],newXYpose,newObjs[i]);
396 template<
class T,
class CPOSE2D>
void project2D(
const T &obj,
const TLine2D &newXLine,T &newObj) {
399 project2D(obj,CPOSE2D(0,0,0)-pose,newObj);
406 template<
class T,
class CPOSE2D>
void project2D(
const T &obj,
const TLine2D &newXLine,
const TPoint2D &newOrigin,T &newObj) {
409 project2D(obj,CPOSE2D(0,0,0)-pose,newObj);
416 size_t N=objs.size();
418 for (
size_t i=0;i<N;i++)
project2D(objs[i],newXpose,newObjs[i]);
452 inline bool intersect(
const TLine2D &r1,
const TPolygon2D &p2,TObject2D &obj) {
501 size_t BASE_IMPEXP intersect(
const std::vector<TPolygon3D> &v1,
const std::vector<TPolygon3D> &v2,CSparseMatrixTemplate<TObject3D> &objs);
506 size_t BASE_IMPEXP intersect(
const std::vector<TPolygon3D> &v1,
const std::vector<TPolygon3D> &v2,std::vector<TObject3D> &objs);
518 size_t M=v1.size(),N=v2.size();
522 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;
530 template<
class T,
class U,
class O>
size_t intersect(
const std::vector<T> &v1,
const std::vector<U> &v2,std::vector<O> objs) {
593 inline double distance(
const TLine2D &l1,
const TPolygon2D &p2) {
708 void BASE_IMPEXP assemblePolygons(
const std::vector<TSegment3D> &segms,std::vector<TPolygon3D> &polys,std::vector<TSegment3D> &remainder);
716 void BASE_IMPEXP assemblePolygons(
const std::vector<TObject3D> &objs,std::vector<TPolygon3D> &polys,std::vector<TObject3D> &remainder);
720 void BASE_IMPEXP assemblePolygons(
const std::vector<TObject3D> &objs,std::vector<TPolygon3D> &polys,std::vector<TSegment3D> &remainder1,std::vector<TObject3D> &remainder2);
775 std::vector<TPolygonWithPlane> pwp;
791 template<
class T,
class U,
class V>
793 vOut[0]=v0[1]*v1[2]-v0[2]*v1[1];
794 vOut[1]=v0[2]*v1[0]-v0[0]*v1[2];
795 vOut[2]=v0[0]*v1[1]-v0[1]*v1[0];
801 const std::vector<T> &v0,
802 const std::vector<T> &v1,
803 std::vector<T> &v_out )
808 v_out[0] = v0[1]*v1[2] - v0[2]*v1[1];
809 v_out[1] = -v0[0]*v1[2] + v0[2]*v1[0];
810 v_out[2] = v0[0]*v1[1] - v0[1]*v1[0];
814 template<
class VEC1,
class VEC2>
815 inline Eigen::Matrix<double,3,1>
crossProduct3D(
const VEC1 &v0,
const VEC2 &v1) {
816 Eigen::Matrix<double,3,1> vOut;
817 vOut[0]=v0[1]*v1[2]-v0[2]*v1[1];
818 vOut[1]=v0[2]*v1[0]-v0[0]*v1[2];
819 vOut[2]=v0[0]*v1[1]-v0[1]*v1[0];
832 template<
class VECTOR,
class MATRIX>
836 M.set_unsafe(0,0, 0); M.set_unsafe(0,1, -v[2]); M.set_unsafe(0,2, v[1]);
837 M.set_unsafe(1,0, v[2]); M.set_unsafe(1,1, 0); M.set_unsafe(1,2, -v[0]);
838 M.set_unsafe(2,0, -v[1]); M.set_unsafe(2,1, v[0]); M.set_unsafe(2,2, 0);
841 template<
class VECTOR>
857 template<
class VECTOR,
class MATRIX>
861 M.set_unsafe(0,0, 0); M.set_unsafe(0,1, v[2]); M.set_unsafe(0,2, -v[1]);
862 M.set_unsafe(1,0, -v[2]); M.set_unsafe(1,1, 0); M.set_unsafe(1,2, v[0]);
863 M.set_unsafe(2,0, v[1]); M.set_unsafe(2,1, -v[0]); M.set_unsafe(2,2, 0);
866 template<
class VECTOR>
884 template<
class T,
class U>
954 template <
typename T>
960 template <
typename T>
966 template <
typename T>
972 template <
typename T>
980 const double & x1,
const double & y1,
981 const double & x2,
const double & y2,
982 const double & x3,
const double & y3,
983 const double & x4,
const double & y4,
984 double &ix,
double &iy);
989 const double & x1,
const double & y1,
990 const double & x2,
const double & y2,
991 const double & x3,
const double & y3,
992 const double & x4,
const double & y4,
993 float &ix,
float &iy);
998 bool BASE_IMPEXP pointIntoPolygon2D(
const double & px,
const double & py,
unsigned int polyEdges,
const double *poly_xs,
const double *poly_ys );
1003 template <
typename T>
1013 const T
a1 = atan2( v1y - y , v1x - x );
1014 const T
a2 = atan2( v2y - y , v2x - x );
1015 const T
a3 = atan2( v3y - y , v3x - x );
1016 const T a4 = atan2( v4y - y , v4x - x );
1022 if (
sign(da1)!=
sign(da2))
return false;
1025 if (
sign(da2)!=
sign(da3))
return false;
1043 const double & p1_x,
const double & p1_y,
const double & p1_z,
1044 const double & p2_x,
const double & p2_y,
const double & p2_z,
1045 const double & p3_x,
const double & p3_y,
const double & p3_z,
1046 const double & p4_x,
const double & p4_y,
const double & p4_z,
1047 double &x,
double &y,
double &z,
1057 const double & R1_x_min,
const double & R1_x_max,
1058 const double & R1_y_min,
const double & R1_y_max,
1059 const double & R2_x_min,
const double & R2_x_max,
1060 const double & R2_y_min,
const double & R2_y_max,
1061 const double & R2_pose_x,
1062 const double & R2_pose_y,
1063 const double & R2_pose_phi );
1101 if (dx==0 && dy==0 && dz==0)
1108 T n = sqrt(n_xy+
square(dz));
1115 if (fabs(dx)>1e-4 || fabs(dy)>1e-4)
1117 P(0,1) = -dy / n_xy;
1150 template <
typename VECTOR_LIKE,
typename Precision,
typename MATRIX_LIKE>
1151 inline void rodrigues_so3_exp(
const VECTOR_LIKE& w,
const Precision A,
const Precision B,MATRIX_LIKE & R)
1157 const Precision wx2 = (Precision)w[0]*w[0];
1158 const Precision wy2 = (Precision)w[1]*w[1];
1159 const Precision wz2 = (Precision)w[2]*w[2];
1160 R(0,0) = 1.0 - B*(wy2 + wz2);
1161 R(1,1) = 1.0 - B*(wx2 + wz2);
1162 R(2,2) = 1.0 - B*(wx2 + wy2);
1165 const Precision a = A*w[2];
1166 const Precision b = B*(w[0]*w[1]);
1171 const Precision a = A*w[1];
1172 const Precision b = B*(w[0]*w[2]);
1177 const Precision a = A*w[0];
1178 const Precision b = B*(w[1]*w[2]);
#define ASSERT_EQUAL_(__A, __B)
bool BASE_IMPEXP conformAPlane(const std::vector< TPoint3D > &points)
Checks whether this polygon or set of points acceptably fits a plane.
void rodrigues_so3_exp(const VECTOR_LIKE &w, const Precision A, const Precision B, MATRIX_LIKE &R)
Compute a rotation exponential using the Rodrigues Formula.
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.
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 skew_symmetric3(const VECTOR &v, MATRIX &M)
Computes the 3x3 skew symmetric matrix from a 3-vector or 3-array: .
double BASE_IMPEXP minimumDistanceFromPointToSegment(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, and returns that minimum distance.
size_t getNonNullElements() const
Gets the amount of non-null elements inside the matrix.
#define THROW_EXCEPTION(msg)
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.
struct BASE_IMPEXP TObject3D
bool vectorsAreParallel3D(const T &v1, const U &v2)
Returns true if two 3D vectors are parallel.
struct BASE_IMPEXP TSegment3D
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.
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.
static void getPlanes(const vector< TPolygon3D > &oldPolys, vector< TPolygonWithPlane > &newPolys)
Static method for vectors.
A numeric matrix of compile-time fixed size.
2D segment, consisting of two points.
3D segment, consisting of two points.
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.
CMatrixTemplateNumeric< double > CMatrixDouble
Declares a matrix of double numbers (non serializable).
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
class BASE_IMPEXP TPolygon3D
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.
bool BASE_IMPEXP traceRay(const vector< TPolygonWithPlane > &vec, const mrpt::poses::CPose3D &pose, double &dist)
Fast ray tracing method using polygons' properties.
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.
T square(const T x)
Inline function for the square of a number.
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.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
struct BASE_IMPEXP TLine3D
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.
A matrix of dynamic size.
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 splitInConvexComponents(const TPolygon2D &poly, vector< TPolygon2D > &components)
Splits a 2D polygon into convex components.
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.
TPlane plane
Plane containing the polygon.
void BASE_IMPEXP getSegmentBisector(const TSegment2D &sgm, TLine2D &bis)
Gets the bisector of a 2D segment.
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.
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 rotation matrix so that the coordinate given (0 for x, 1 for y, 2 for z) corresponds to the...
3D line, represented by a base point and a director vector.
2D line without bounds, represented by its equation .