00001
00002
00003
00004
00005
00006
00007
00008
00009
00010 #ifndef ApproxMVBB_ComputeApproxMVBB_hpp
00011 #define ApproxMVBB_ComputeApproxMVBB_hpp
00012
00013 #include "ApproxMVBB/Config/Config.hpp"
00014 #include "ApproxMVBB/Common/LogDefines.hpp"
00015
00016 #include ApproxMVBB_TypeDefs_INCLUDE_FILE
00017 #include "ApproxMVBB/TypeDefsPoints.hpp"
00018
00019
00020 #include ApproxMVBB_OOBB_INCLUDE_FILE
00021 #include "ApproxMVBB/GreatestCommonDivisor.hpp"
00022 #include "ApproxMVBB/RandomGenerators.hpp"
00023 #include "ApproxMVBB/ProjectedPointSet.hpp"
00024
00025
00026 namespace ApproxMVBB {
00027
00028 ApproxMVBB_DEFINE_MATRIX_TYPES
00029 ApproxMVBB_DEFINE_POINTS_CONFIG_TYPES
00030
00039 template<typename Derived>
00040 APPROXMVBB_EXPORT void samplePointsGrid(Matrix3Dyn & newPoints,
00041 const MatrixBase<Derived> & points,
00042 const unsigned int nPoints,
00043 OOBB & oobb,
00044 std::size_t seed = ApproxMVBB::RandomGenerators::defaultSeed
00045 )
00046 {
00047 using IndexType = typename Derived::Index;
00048
00049 if(nPoints > points.cols() || nPoints < 2) {
00050 ApproxMVBB_ERRORMSG("Wrong arguments!" << "sample nPoints: (>2) " << nPoints << " of points: " << points.cols() << std::endl )
00051 }
00052
00053 newPoints.resize(3,nPoints);
00054
00055
00056 unsigned int gridSize = std::max( static_cast<unsigned int>( std::sqrt( static_cast<double>(nPoints) / 2.0 )) , 1U );
00057
00058
00059
00060 oobb.setZAxisLongest();
00061
00062 IndexType halfSampleSize = gridSize*gridSize;
00063
00064 struct BottomTopPoints{
00065 IndexType bottomIdx = 0;
00066 PREC bottomZ;
00067
00068 IndexType topIdx = 0;
00069 PREC topZ;
00070 };
00071
00072
00073 std::vector< BottomTopPoints > boundaryPoints(halfSampleSize);
00074
00075 using LongInt = long long int;
00076 MyMatrix::Array2<LongInt> idx;
00077
00078
00079 Array2 dxdyInv = Array2(gridSize,gridSize) / oobb.extent().head<2>();
00080 Vector3 K_p;
00081
00082 Matrix33 A_KI(oobb.m_q_KI.matrix().transpose());
00083
00084
00085 IndexType size = points.cols();
00086 for(IndexType i=0; i<size; ++i) {
00087
00088 K_p = A_KI * points.col(i);
00089
00090 idx = ( (K_p - oobb.m_minPoint).head<2>().array() * dxdyInv ).template cast<LongInt>();
00091
00092 idx(0) = std::max( std::min( LongInt(gridSize-1), idx(0)), 0LL );
00093 idx(1) = std::max( std::min( LongInt(gridSize-1), idx(1)), 0LL );
00094
00095
00096
00097
00098
00099 auto & pB = boundaryPoints[idx(0) + idx(1)*gridSize];
00100
00101 if( pB.bottomIdx == 0) {
00102 pB.bottomIdx = pB.topIdx = i+1;
00103 pB.bottomZ = pB.topZ = K_p(2);
00104 } else {
00105 if( pB.topZ < K_p(2) ) {
00106 pB.topIdx = i+1;
00107 pB.topZ = K_p(2);
00108 }else{
00109 if( pB.bottomZ > K_p(2) ) {
00110 pB.bottomIdx = i+1;
00111 pB.bottomZ = K_p(2);
00112 }
00113 }
00114 }
00115 }
00116
00117
00118 IndexType k=0;
00119 ApproxMVBB_MSGLOG_L2("Sampled Points incides: [ ")
00120
00121 for(IndexType i=0; i<halfSampleSize; ++i) {
00122 if( boundaryPoints[i].bottomIdx != 0 ) {
00123
00124
00125
00126 ApproxMVBB_MSGLOG_L2( boundaryPoints[i].topIdx-1 << ", " << ((k%30==0)? "\n" : "") )
00127 newPoints.col(k++) = points.col(boundaryPoints[i].topIdx-1);
00128 if(boundaryPoints[i].topIdx != boundaryPoints[i].bottomIdx) {
00129
00130
00131
00132 ApproxMVBB_MSGLOG_L2( boundaryPoints[i].bottomIdx-1 << ", " )
00133 newPoints.col(k++) = points.col(boundaryPoints[i].bottomIdx-1);
00134 }
00135 }
00136 }
00137
00138
00139
00140 if( k < nPoints ){
00141 RandomGenerators::DefaultRandomGen gen(seed);
00142 RandomGenerators::DefaultUniformUIntDistribution<
00143 typename std::make_unsigned<IndexType>::type
00144 > dis(0, points.cols()-1);
00145 IndexType s;
00146 while( k < nPoints) {
00147 s = dis(gen);
00148 ApproxMVBB_MSGLOG_L2( s << ", " )
00149 newPoints.col(k++) = points.col( s );
00150 }
00151 }
00152 ApproxMVBB_MSGLOG_L2( "]" << std::endl )
00153 }
00154
00163 template<typename Derived>
00164 APPROXMVBB_EXPORT OOBB optimizeMVBB( const MatrixBase<Derived> & points,
00165 OOBB oobb,
00166 unsigned int nLoops = 10,
00167 PREC volumeAcceptFactor = 1e-6,
00168 PREC minBoxExtent = 1e-12
00169 )
00170 {
00171
00172 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived,3,Eigen::Dynamic)
00173
00174 if( oobb.volume() == 0.0 || nLoops == 0) {
00175 return oobb;
00176 }
00177
00178
00179 PREC volumeAcceptTol = oobb.volume() * volumeAcceptFactor;
00180
00181
00182 unsigned int cacheIdx = 0;
00183 Vector3 dirCache[3];
00184
00185 Vector3 dir;
00186 ProjectedPointSet proj;
00187 for(unsigned int loop = 0; loop < nLoops; ++loop ) {
00188
00189
00190
00191 dir = oobb.getDirection(0);
00192
00193
00194 for(unsigned char i=0; i<3 && i<loop; ++i) {
00195 PREC dotp = std::abs(dir.dot(dirCache[i]));
00196 if( std::abs(dotp - 1.0) <= 1e-3 ) {
00197
00198
00199 dir = oobb.getDirection(1);
00200 break;
00201 }
00202 }
00203
00204 dirCache[cacheIdx] = dir;
00205 cacheIdx = (cacheIdx + 1) % 3;
00206
00207
00208 OOBB o = proj.computeMVBB( dir, points);
00209
00210
00211 o.expandToMinExtentAbsolute(minBoxExtent);
00212
00213 if( o.volume() < oobb.volume() && o.volume()>volumeAcceptTol) {
00214 oobb = o;
00215 }
00216 }
00217
00218 return oobb;
00219 }
00220
00221
00233 template<typename Derived>
00234 APPROXMVBB_EXPORT OOBB approximateMVBBGridSearch(const MatrixBase<Derived> & points,
00235 OOBB oobb,
00236 PREC epsilon,
00237 const unsigned int gridSize = 5,
00238 const unsigned int optLoops = 6,
00239 PREC volumeAcceptFactor = 1e-6,
00240 PREC minBoxExtent = 1e-12
00241 )
00242 {
00243 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived,3,Eigen::Dynamic)
00244
00245
00246 oobb.expandToMinExtentAbsolute(minBoxExtent);
00247
00248
00249
00250
00251
00252 Vector3 dir1 = oobb.getDirection(0);
00253 Vector3 dir2 = oobb.getDirection(1);
00254 Vector3 dir3 = oobb.getDirection(2);
00255
00256 Vector3 dir;
00257
00258 ProjectedPointSet proj;
00259
00260 for(int x = -int(gridSize); x <= (int)gridSize; ++x ) {
00261 for(int y = -int(gridSize); y <= (int)gridSize; ++y ) {
00262 for(int z = 0; z <= (int)gridSize; ++z ) {
00263
00264 if( MathFunctions::gcd3(x,y,z)> 1 || ((x==0) && (y==0) && (z==0)) ) {
00265 continue;
00266 }
00267
00268
00269 dir = x*dir1 + y*dir2 + z*dir3;
00270 ApproxMVBB_MSGLOG_L3( "gridSearch: dir: " << dir.transpose() << std::endl; )
00271
00272
00273 auto res = proj.computeMVBB(dir,points);
00274
00275
00276 res.expandToMinExtentAbsolute(minBoxExtent);
00277
00278 if(optLoops){
00279 res = optimizeMVBB(points,res,optLoops,volumeAcceptFactor,minBoxExtent);
00280 }
00281 ApproxMVBB_MSGLOG_L3( "gridSearch: volume: " << res.volume() << std::endl;)
00282 if(res.volume() < oobb.volume() ) {
00283
00284 ApproxMVBB_MSGLOG_L2( "gridSearch: new volume: " << res.volume() << std::endl << "for dir: " << dir.transpose() << std::endl; )
00285 oobb = res;
00286 }
00287
00288 }
00289 }
00290 }
00291 return oobb;
00292 }
00293
00300 template<typename Derived>
00301 APPROXMVBB_EXPORT OOBB approximateMVBBDiam(const MatrixBase<Derived> & points,
00302 const PREC epsilon,
00303 const unsigned int optLoops = 10,
00304 std::size_t seed = ApproxMVBB::RandomGenerators::defaultSeed
00305 )
00306 {
00307 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived,3,Eigen::Dynamic)
00308
00309 using namespace PointFunctions;
00310 auto pp = estimateDiameter<3>(points, epsilon, seed );
00311
00312 ApproxMVBB::MyMatrix::Vector3<ApproxMVBB::TypeDefsPoints::PREC> dirZ = pp.first - pp.second;
00313
00314
00315 if( ( dirZ.array() <= 0.0).all() ) {
00316 dirZ *= -1;
00317 }
00318
00319
00320 if( (dirZ.array() == 0.0).all() ) {
00321 dirZ.setZero();
00322 dirZ(0)= 1;
00323 }
00324 ApproxMVBB_MSGLOG_L1("estimated 3d diameter: " << dirZ.transpose() << " eps: " << epsilon << std::endl;)
00325
00326
00327 ProjectedPointSet proj;
00328
00329
00330 OOBB oobb = proj.computeMVBBApprox(dirZ,points,epsilon);
00331
00332 if(optLoops) {
00333 oobb = optimizeMVBB(points,oobb,optLoops);
00334 }
00335 return oobb;
00336 }
00337
00338 template<typename Derived >
00339 APPROXMVBB_EXPORT OOBB approximateMVBB(const MatrixBase<Derived> & points,
00340 const PREC epsilon,
00341 const unsigned int pointSamples = 400,
00342 const unsigned int gridSize = 5,
00343 const unsigned int mvbbDiamOptLoops = 0,
00344 const unsigned int mvbbGridSearchOptLoops = 6,
00345 std::size_t seed = ApproxMVBB::RandomGenerators::defaultSeed
00346 )
00347 {
00348 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived,3,Eigen::Dynamic)
00349
00350
00351
00352 auto oobb = approximateMVBBDiam(points,epsilon,mvbbDiamOptLoops, seed );
00353
00354
00355 if(pointSamples<points.cols()) {
00356
00357
00358 Matrix3Dyn sampled;
00359 samplePointsGrid(sampled,points,pointSamples,oobb, seed);
00360
00361
00362 oobb = approximateMVBBGridSearch(sampled,oobb,epsilon,gridSize,mvbbGridSearchOptLoops);
00363
00364 } else {
00365
00366 oobb = approximateMVBBGridSearch(points,oobb,epsilon,gridSize,mvbbGridSearchOptLoops);
00367 }
00368
00369 return oobb;
00370 }
00371
00372 }
00373
00374
00375
00376 #endif // ApproxMVBB_hpp
00377