asr_approx_mvbb: PointFunctions.hpp Source File

Go to the documentation of this file.
00001 // ========================================================================================
00002 //  ApproxMVBB
00003 //  Copyright (C) 2014 by Gabriel Nützi <nuetzig (at) imes (d0t) mavt (d0t) ethz (døt) ch>
00004 //
00005 //  This Source Code Form is subject to the terms of the Mozilla Public
00006 //  License, v. 2.0. If a copy of the MPL was not distributed with this
00007 //  file, You can obtain one at http://mozilla.org/MPL/2.0/.
00008 // ========================================================================================
00009 
00010 #ifndef ApproxMVBB_PointFunctions_hpp
00011 #define ApproxMVBB_PointFunctions_hpp
00012 
00013 #include <string>
00014 
00015 #include "ApproxMVBB/Config/Config.hpp"
00016 #include ApproxMVBB_AssertionDebug_INCLUDE_FILE
00017 #include ApproxMVBB_StaticAssert_INCLUDE_FILE
00018 
00019 #include ApproxMVBB_TypeDefs_INCLUDE_FILE
00020 #include "ApproxMVBB/TypeDefsPoints.hpp"
00021 
00022 #include "ApproxMVBB/Diameter/EstimateDiameter.hpp"
00023 #include "ApproxMVBB/GeometryPredicates/Predicates.hpp"
00024 
00025 #include "ApproxMVBB/Common/FloatingPointComparision.hpp"
00026 
00027 namespace ApproxMVBB{
00028 namespace PointFunctions {
00029 
00030     ApproxMVBB_DEFINE_MATRIX_TYPES
00031     ApproxMVBB_DEFINE_POINTS_CONFIG_TYPES
00032 
00033     template<typename Derived, typename Gen>
00034     void applyRandomRotTrans(MatrixBase<Derived> & points, Gen & g) {
00035         EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived,3, Eigen::Dynamic)
00036         Quaternion q;
00037         q.coeffs() = q.coeffs().unaryExpr(g); // TODO: Check if q=[0,0,0,0] is correctly normalized !! otherwise crash! NaN
00038         q.normalize();
00039         Matrix33 R = q.matrix();
00040         Vector3 trans;
00041         trans = trans.unaryExpr(g);
00042         points = R*points;
00043         points.colwise() += trans;
00044     }
00045 
00046     template<typename Derived>
00047     void applyRandomRotTrans(MatrixBase<Derived> & points) {
00048 
00049         EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived,3, Eigen::Dynamic)
00050         Quaternion q;
00051         q.coeffs().setRandom();
00052         q.normalize();
00053         Matrix33 R = q.matrix();
00054         Vector3 trans;
00055         trans.setRandom();
00056         points = R*points;
00057         points.colwise() += trans;
00058     }
00059 
00060 
00061 
00062     template<typename VecT1, typename VecT2>
00063     inline bool almostEqualAbs(const VecT1  & a, const VecT2  & b, PREC eps = 1.0e-8 ) {
00064           return ((a-b).array().abs() <= eps).all();
00065     }
00066 
00067     template<typename VecT1, typename VecT2>
00068     inline bool almostEqualUlp(const VecT1  & a, const VecT2  & b /*, PREC eps = 1.0e-8*/ ) {
00069         bool ret = true;
00070         for(unsigned int i=0;i<a.size();i++){
00071             ret = ret && FloatingPoint<PREC>(a(i)).AlmostEquals(FloatingPoint<PREC>(b(i)));
00072         }
00073         return ret;
00074     }
00075 
00076 
00077     template<typename VecT1, typename VecT2>
00078     inline bool equal(const VecT1  & a, const VecT2  & b) {
00079         return (a.array() == b.array()).all();
00080     }
00081 
00083     template<typename VecT1, typename VecT2, typename VecT3>
00084     inline int orient2d(const VecT1  & a, const VecT2  & b, const VecT3  & c) {
00085         EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(VecT1,2)
00086         EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(VecT2,2)
00087         EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(VecT3,2)
00088 
00089         PREC f_A = GeometryPredicates::orient2d(const_cast<double*>(a.data()),
00090                                                 const_cast<double*>(b.data()),
00091                                                 const_cast<double*>(c.data()));
00092 
00093         return  ( ( f_A < 0.0 )?   -1   :   ( (f_A > 0.0)? 1 : 0) );
00094 
00095     }
00096 
00098     template<typename VecT1, typename VecT2, typename VecT3>
00099     inline bool leftTurn(const VecT1  & a, const VecT2  & b, const VecT3  & c) {
00100         EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(VecT1,2)
00101         EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(VecT2,2)
00102         EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(VecT3,2)
00103         return orient2d(a,b,c) > 0;
00104     }
00105 
00107     template<typename VecT1, typename VecT2, typename VecT3>
00108     inline int collinearAreOrderedAlongLine(const VecT1  & a, const VecT2  & b, const VecT3  & c) {
00109         EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(VecT1,2)
00110         EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(VecT2,2)
00111         EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(VecT3,2)
00112 
00113           if (a(0) < b(0)) return !(c(0) < b(0));
00114           if (b(0) < a(0)) return !(b(0) < c(0));
00115           if (a(1) < b(1)) return !(c(1) < b(1));
00116           if (b(1) < a(1)) return !(b(1) < c(1));
00117           return true; // a==b
00118 
00119     }
00120 
00121 
00122 
00124     template<typename VecT1, typename VecT2>
00125     inline PREC getAngle(const VecT1  & a, const VecT2 & b) {
00126         EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(VecT1,2)
00127         EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(VecT2,2)
00128         Vector2 t = b - a ;
00129         PREC angle = std::atan2(t(1),t(0));
00130         if(angle<0.0) {
00131             angle += 2*M_PI;
00132         }
00133         return angle;
00134     }
00135 
00136     template<typename VecT1, typename VecT2>
00137     Vector2 intersectLines(const VecT1 & p1, PREC ang1,
00138                            const VecT2 & p2, PREC ang2, PREC eps = 1e-10) {
00139         EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(VecT1,2)
00140         EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(VecT2,2)
00141         using namespace std;
00142         // Two lines p1 + a*t1 =  p2 + b*t2;
00143         // [-a, b]^-1 * (p1-p2) = [t1;t2]
00144         // p = (p1-p2)
00145         // =>
00146         // t1 = px by - bx py / (ay bx - ax by);
00147 
00148         Vector2 a;
00149         a(0) = cos(ang1);
00150         a(1) = sin(ang1);
00151 
00152         PREC bx = cos(ang2);
00153         PREC by = sin(ang2);
00154 
00155         PREC nom = (p1(0)-p2(0))*by  -  (p1(1)-p2(1)) *bx;
00156 
00157         PREC det = a(1)*bx   -  a(0)*by;
00158 
00159         if( det == 0.0 ) { // lines are collinear
00160             if(abs(nom) < eps ) { // if the two lines are almost identical! rot( p1-p2, 90 degrees) almost orthogonal to b
00161                 return p1;
00162             }
00163             a(0) = std::numeric_limits<PREC>::infinity();
00164             a(1) = std::numeric_limits<PREC>::infinity();
00165             return a;
00166         }
00167 
00168         return  (nom / det) *a + p1;
00169     }
00170 
00172     template<typename Derived>
00173     inline unsigned int minPointYX(const MatrixBase<Derived> & points) {
00174         EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived,2, Eigen::Dynamic)
00175         unsigned int index = 0;
00176         for(unsigned int i=1; i<points.cols(); ++i) {
00177             if( points(1,i) < points(1,index) ) {
00178                 index = i;
00179             } else if( points(1,i) == points(1,index)  && points(0,i) <  points(0,index) ) {
00180                 index = i;
00181             }
00182         }
00183         return index;
00184     }
00185 
00187     template<typename Derived>
00188     inline unsigned int minPointXY(const MatrixBase<Derived> & points) {
00189         EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived,2, Eigen::Dynamic)
00190         unsigned int index = 0;
00191         for(unsigned int i=1; i<points.cols(); ++i) {
00192             if( points(0,i) < points(0,index) ) {
00193                 index = i;
00194             } else if( points(0,i) == points(0,index)  && points(1,i) <  points(1,index) ) {
00195                 index = i;
00196             }
00197         }
00198         return index;
00199     }
00200 
00201     template<unsigned int Dimension,
00202              typename Derived>
00203     auto estimateDiameter(  const MatrixBase<Derived> & points,
00204                             const PREC epsilon,
00205                             std::size_t seed = RandomGenerators::defaultSeed) -> std::pair<VectorStat<Dimension>,VectorStat<Dimension> >
00206     {
00207 
00208         ApproxMVBB_STATIC_ASSERTM(Derived::RowsAtCompileTime == Dimension, "input points matrix need to be (Dimension x N) ");
00209         ApproxMVBB_STATIC_ASSERTM((std::is_same<typename Derived::Scalar, PREC>::value), "estimate diameter can only accept double so far")
00210 
00211         /* MatrixBase<Derived> & pp = const_cast< MatrixBase<Derived> &>(points); */
00212 
00213         // Construct pointer list
00214         auto size = points.cols();
00215         PREC const *  *pList = new PREC const*[size];
00216         for(decltype(size) i=0; i<size; ++i) {
00217             pList[i] = points.col(i).data() ;
00218         }
00219 
00220         Diameter::TypeSegment pairP;
00221         DiameterEstimator diamEstimator(seed);
00222         diamEstimator.estimateDiameter(&pairP,pList,0,(int)(size-1),Dimension,epsilon);
00223 
00224         using Vector2d = MyMatrix::VectorStat<double,Dimension>;
00225         const MatrixMap<const Vector2d> p1(pairP.extremity1);
00226         const MatrixMap<const Vector2d> p2(pairP.extremity2);
00227 
00228 
00229 
00230         ApproxMVBB_MSGLOG_L2( "p1: " << p1.transpose() << std::endl
00231                       << "p2: " << p2.transpose() << std::endl
00232                       << " l: " << std::sqrt(pairP.squareDiameter) << std::endl);
00233 
00234         delete[] pList;
00235         return {p1,p2};
00236 
00237     }
00238 
00239     class CompareByAngle {
00240     public:
00241         EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00242         ApproxMVBB_DEFINE_MATRIX_TYPES
00243 
00244 
00245         using PointData = std::pair<unsigned int, bool>;
00246 
00250         template<typename Derived>
00251         CompareByAngle(const MatrixBase<Derived> & points,
00252                        const Vector2 &base,
00253                        unsigned int baseIdx,
00254                        unsigned int & deletedPoints): m_p(points), m_base(base),m_baseIdx(baseIdx), m_deletedPoints(deletedPoints) {
00255             EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived,2, Eigen::Dynamic)
00256         }
00257 
00259         int operator()( const PointData & point1In,
00260                         const PointData & point2In )
00261         {
00262             using namespace PointFunctions;
00263             PointData & point1 = const_cast<PointData &>(point1In);
00264             PointData & point2 = const_cast<PointData &>(point2In);
00265             unsigned int idx1 = point1.first;
00266             unsigned int idx2 = point2.first;
00267 //
00268 //            if(idx1<idx2){
00269 //                if(almostEqualUlp(m_p.col(idx1),m_p.col(idx2))){
00270 //                    if(!point1.second){point1.second = true; ++m_deletedPoints;}
00271 //                    return false;
00272 //                }
00273 //            }else{
00274 //                if(almostEqualUlp(m_p.col(idx2),m_p.col(idx1))){
00275 //                    if(!point2.second){point2.second = true; ++m_deletedPoints;}
00276 //                    return false;
00277 //                }
00278 //            }
00279 
00280             // Compare by Area Sign (by ascending positive (z-Axis Rotation) angle in x-y Plane)
00281             // always  insert the smaller index first , and the larger second (as the function is not completely symmetric!
00282             if(idx1<idx2) {
00283                 int sgn = orient2d( m_base, m_p.col(idx1), m_p.col(idx2) );
00284                 if(sgn != 0){ return (sgn > 0);}
00285             } else {
00286                 int sgn = orient2d( m_base, m_p.col(idx2), m_p.col(idx1) );
00287                 if(sgn != 0) {return (sgn < 0);}
00288 
00289             }
00290             // points are collinear
00291 
00292             if(PointFunctions::equal(m_base, m_p.col(idx1))) return false;
00293             if(PointFunctions::equal(m_base, m_p.col(idx2))) return true;
00294             if(PointFunctions::equal(m_p.col(idx1), m_p.col(idx2))) return false;
00295 
00296             // if idx2 lies between mbase and idx1 then it should go after idx1
00297             return collinearAreOrderedAlongLine(m_base,m_p.col(idx2),m_p.col(idx1));
00298         }
00299     private:
00300         const MatrixRef<const Matrix2Dyn> m_p;
00301         const Vector2 m_base; const unsigned int m_baseIdx;
00302         unsigned int & m_deletedPoints;
00303 
00304     };
00305 
00306 }
00307 }
00308 #endif