Go to the documentation of this file.
79 static void getPlanes(
const vector<TPolygon3D> &oldPolys,vector<TPolygonWithPlane> &newPolys);
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);
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]);
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) {
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>
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]);
CMatrixTemplateNumeric< double > CMatrixDouble
Declares a matrix of double numbers (non serializable).
#define THROW_EXCEPTION(msg)
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:
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.
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.
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.
#define ASSERT_EQUAL_(__A, __B)
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.
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)
void BASE_IMPEXP getSegmentBisector(const TSegment2D &sgm, TLine2D &bis)
Gets the bisector of a 2D segment.
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.
Slightly heavyweight type to speed-up calculations with polygons in 3D.
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.
mrpt::poses::CPose3D inversePose
Plane's inverse pose.
double z
X,Y,Z coordinates.
double BASE_IMPEXP distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
void BASE_IMPEXP createFromPoseX(const mrpt::poses::CPose3D &p, TLine3D &r)
Gets a 3D line corresponding to the X axis in a given pose.
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.
void BASE_IMPEXP createPlaneFromPoseXZ(const mrpt::poses::CPose3D &pose, TPlane &plane)
Given a pose, creates a plane orthogonal to its Y vector.
Standard type for storing any lightweight 2D type.
Standard object for storing any 3D lightweight object.
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.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
void BASE_IMPEXP createPlaneFromPoseXY(const mrpt::poses::CPose3D &pose, TPlane &plane)
Given a pose, creates a plane orthogonal to its Z vector.
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.
const typedef Scalar * const_iterator
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...
void BASE_IMPEXP createFromPoseZ(const mrpt::poses::CPose3D &p, TLine3D &r)
Gets a 3D line corresponding to the Z axis in a given pose.
TPoint3D point1
Origin point.
A matrix of dynamic size.
void getAsPose2D(mrpt::poses::CPose2D &outPose) const
Get a pose2D whose X axis corresponds to the line.
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...
int sign(T x)
Returns the sign of X as "1" or "-1".
double BASE_IMPEXP getRegressionPlane(const std::vector< TPoint3D > &points, TPlane &plane)
Using eigenvalues, gets the best fitting plane for a set of 3D points.
bool BASE_IMPEXP intersect(const TSegment3D &s1, const TSegment3D &s2, TObject3D &obj)
Gets the intersection between two 3D segments.
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...
bool BASE_IMPEXP splitInConvexComponents(const TPolygon2D &poly, vector< TPolygon2D > &components)
Splits a 2D polygon into convex components.
TPoint3D point2
Destiny point.
T distanceSqrBetweenPoints(const T x1, const T y1, const T x2, const T y2)
Returns the square distance between 2 points in 2D.
size_t getNonNullElements() const
Gets the amount of non-null elements inside the matrix.
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
TPolygonWithPlane()
Basic constructor.
double BASE_IMPEXP geometryEpsilon
Global epsilon to overcome small precision errors.
TPolygon3D poly
Actual polygon.
A class used to store a 2D pose.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
mrpt::poses::CPose3D pose
Plane's pose.
T distanceBetweenPoints(const T x1, const T y1, const T x2, const T y2)
Returns the distance between 2 points in 2D.
A sparse matrix container (with cells of any type), with iterators.
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 resize(size_t nRows, size_t nCols)
Changes the size of the matrix.
static void getPlanes(const vector< TPolygon3D > &oldPolys, vector< TPolygonWithPlane > &newPolys)
Static method for vectors.
2D polygon, inheriting from std::vector<TPoint2D>.
2D segment, consisting of two points.
3D polygon, inheriting from std::vector<TPoint3D>
struct BASE_IMPEXP TObject3D
bool vectorsAreParallel3D(const T &v1, const U &v2)
Returns true if two 3D vectors are parallel.
3D segment, consisting of two points.
bool BASE_IMPEXP conformAPlane(const std::vector< TPoint3D > &points)
Checks whether this polygon or set of points acceptably fits a plane.
A numeric matrix of compile-time fixed size.
struct BASE_IMPEXP TSegment3D
TPlane plane
Plane containing the polygon.
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.
void crossProduct3D(const T &v0, const U &v1, V &vOut)
Computes the cross product of two 3D vectors, returning a vector normal to both.
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.
3D Plane, represented by its equation
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 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.
TPolygon2D poly2D
Polygon, after being projected to the plane using inversePose.
void clear()
Completely removes all elements, although maintaining the matrix's size.
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:
T square(const T x)
Inline function for the square of a number.
void getAsPose3D(mrpt::poses::CPose3D &outPose)
Gets a pose whose XY plane corresponds to this plane.
2D line without bounds, represented by its equation .
double BASE_IMPEXP getAngle(const TPlane &p1, const TPlane &p2)
Computes the angle between two planes.
3D line, represented by a base point and a director vector.
void setEpsilon(double nE)
Changes the value of the geometric epsilon.
class BASE_IMPEXP TPolygon3D
bool BASE_IMPEXP traceRay(const vector< TPolygonWithPlane > &vec, const mrpt::poses::CPose3D &pose, double &dist)
Fast ray tracing method using polygons' properties.
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.
TPoint2D point1
Origin point.
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.
TPoint2D point2
Destiny point.
bool BASE_IMPEXP areAligned(const std::vector< TPoint2D > &points)
Checks whether this set of points acceptably fits a 2D line.
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.
bool vectorsAreParallel2D(const T &v1, const U &v2)
Returns true if two 2D vectors are parallel.
struct BASE_IMPEXP TLine3D
double BASE_IMPEXP getRegressionLine(const std::vector< TPoint2D > &points, TLine2D &line)
Using eigenvalues, gets the best fitting line for a set of 2D points.
double getEpsilon()
Gets the value of the geometric epsilon.
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.
Page generated by Doxygen 1.8.17 for MRPT 1.4.0 SVN: at Sat Jan 18 22:37:07 UTC 2020 | | |