ProjectedPointSet.hpp
Go to the documentation of this file.
1 // ========================================================================================
2 // ApproxMVBB
3 // Copyright (C) 2014 by Gabriel Nützi <nuetzig (at) imes (d0t) mavt (d0t) ethz (døt) ch>
4 //
5 // This Source Code Form is subject to the terms of the Mozilla Public
6 // License, v. 2.0. If a copy of the MPL was not distributed with this
7 // file, You can obtain one at http://mozilla.org/MPL/2.0/.
8 // ========================================================================================
9 #ifndef ApproxMVBB_ProjectedPointSet_hpp
10 #define ApproxMVBB_ProjectedPointSet_hpp
11 
13 
14 #include ApproxMVBB_AssertionDebug_INCLUDE_FILE
15 #include ApproxMVBB_TypeDefs_INCLUDE_FILE
16 
18 
21 #include ApproxMVBB_OOBB_INCLUDE_FILE
23 
24 //#include "TestFunctions.hpp"
25 
26 namespace ApproxMVBB{
28 public:
29  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
32 
33  template<typename Derived>
35  const MatrixBase<Derived> & points,
36  const PREC epsilon) {
37 
38  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived,3,Eigen::Dynamic)
39 
40  using namespace PointFunctions;
41  m_zDir = zDir;
42  project(points);
43  //std::cout <<"projected points" <<std::endl;
44 
45  // Estimate diameter in 2d projective plane
46  std::pair<Vector2,Vector2> pp = estimateDiameter<2>(m_p,epsilon);
47 
48 
49  Vector2 dirX = pp.first - pp.second;
50  if( ( pp.second.array() >= pp.first.array()).all() ) {
51  dirX *= -1;
52  }
53  //std::cout <<"estimated 2d diameter: " << dirX.transpose() << " eps: " << epsilon << std::endl;
54  // Built Coordinate Trafo from frame K to frame M
55  Matrix22 A2_MK;
56  dirX.normalize();
57 
58  // If normalized direction INf/NaN, use (1,0)
59  if( !dirX.allFinite() ) {
60  dirX.setZero();
61  dirX(0)= 1;
62  }
63 
64  Vector2 dirY(-dirX(1),dirX(0)); // Positive rotation of 90 degrees
65  A2_MK.col(0) = dirX;
66  A2_MK.col(1) = dirY;
67  A2_MK.transposeInPlace();
68 
69  AABB2d aabb;
70  Vector2 p;
71  auto size = m_p.cols();
72  for(unsigned int i=0; i < size; ++i) {
73  p.noalias() = A2_MK * m_p.col(i); // Transform all points
74  aabb.unite(p);
75  }
76 
77 // std::cout << "aabb_min: "<< aabb.m_minPoint.transpose() << std::endl;
78 // std::cout << "aabb_max: "<< aabb.m_maxPoint.transpose() << std::endl;
79 
80  Vector3 M_min;
81  M_min.head<2>() = aabb.m_minPoint;
82  M_min(2) = m_minZValue;
83 
84  Vector3 M_max;
85  M_max.head<2>() = aabb.m_maxPoint;
86  M_max(2) = m_maxZValue;
87 
88  // Make coordinate transformation from M frame (Minimum Rectancle)
89  // to K frame (Projection Plane);
90  Matrix33 A_KM;
91  A_KM.setIdentity();
92  A_KM.block<2,2>(0,0) = A2_MK.transpose();
93 
94 // std::cout << "M_min: "<< M_min.transpose() << std::endl;
95 // std::cout << "M_max: "<< M_max.transpose() << std::endl;
96 
97  return OOBB(M_min,M_max, m_A_KI.transpose()*A_KM );
98  }
99 
104  template<typename Derived>
105  OOBB computeMVBB(const Vector3 & zDir,
106  const MatrixBase<Derived> & points ) {
107 
108  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived,3,Eigen::Dynamic)
109 
110  using namespace CoordinateSystem;
111  m_zDir = zDir;
112  project(points);
113 
114  // compute minimum area rectangle first
115  // std::cout << "Dump points DEBUG:" << std::endl;
116  //TestFunctions::dumpPointsMatrixBinary("DumpedPoints.bin",m_p);
117 
118 
119  MinAreaRectangle mar(m_p);
120  mar.compute();
121  auto rect = mar.getMinRectangle();
122 
123  //std::cout << "Dump RECT DEBUG:" << std::endl;
124  // Vector2List p;
125  //p.push_back( rect.m_p);
126  //p.push_back( rect.m_p + rect.m_u );
127  //p.push_back( rect.m_p + rect.m_u + rect.m_v );
128  //p.push_back( rect.m_p + rect.m_v );
129  //p.push_back( rect.m_p);
130 
131  //TestFunctions::dumpPoints("./MinAreaRectangleTest13" "Out.txt",p);
132 
133 
134  // Box coordinates are in K Frame
135 
136  Matrix22 A2_KM;
137 
138 // std::cout << "u:" << rect.m_u.norm() << std::endl;
139 // std::cout << "v:" << rect.m_v.norm() << std::endl;
140 
141  A2_KM.col(0) = rect.m_u;
142  A2_KM.col(1) = rect.m_v;
143 
144  Vector2 M_p = A2_KM.transpose()*rect.m_p;
145 
146  Vector3 M_min;
147  M_min.head<2>() = M_p;
148  M_min(2) = m_minZValue;
149 
150  Vector3 M_max(rect.m_uL, rect.m_vL, 0.0);
151  M_max.head<2>() += M_p;
152  M_max(2) = m_maxZValue;
153 
154  // Make coordinate transformation from M frame (Minimum Rectancle)
155  // to K frame (Projection Plane);
156  Matrix33 A_IM;
157  // Make A_KM
158  A_IM.setIdentity();
159  A_IM.block<2,2>(0,0) = A2_KM;
160  // Make A_IM;
161  A_IM = m_A_KI.transpose()*A_IM; // A_IM = A_IK * A_KM
162 
163  return OOBB(M_min,M_max, A_IM );
164  }
165 
166 
167 private:
168 
169  template<typename Derived>
170  void project(const MatrixBase<Derived> & points) {
171  using namespace CoordinateSystem;
172 
173  if(points.cols()==0) {
174  ApproxMVBB_ERRORMSG("Point set empty!");
175  }
176 
177  // Generate Orthonormal Bases
178  Vector3 xDir,yDir;
179  //std::cout << "dir: " << m_zDir << std::endl;
180  makeCoordinateSystem(m_zDir,xDir,yDir);
181 
182  //Make coodinate transform from frame I to K!
183  m_A_KI.col(0) = xDir;
184  m_A_KI.col(1) = yDir;
185  m_A_KI.col(2) = m_zDir;
186  m_A_KI.transposeInPlace();
187  ApproxMVBB_ASSERTMSG(checkOrthogonality(xDir,yDir,m_zDir,1e-6),
188  "Not orthogonal: x:"<<xDir.transpose()<< " y: "<< yDir.transpose() << " z: " << m_zDir.transpose());
189 
190  // Project Points onto xDir,yDir Halfspace
191  auto size = points.cols();
192  m_p.resize(2,size);
193  //m_p = m_A_KI * points; // Project points! (below is faster)
194  Vector3 p;
195  m_maxZValue = std::numeric_limits<PREC>::lowest();
196  m_minZValue = std::numeric_limits<PREC>::max();
197  for(decltype(size) i=0; i<size; ++i) {
198  p = m_A_KI*points.col(i);
199  m_p.col(i) = p.head<2>();
200  m_maxZValue = std::max(m_maxZValue,p(2));
201  m_minZValue = std::min(m_minZValue,p(2));
202  }
203 
204 
205  }
206 
208 
211  PREC m_minZValue, m_maxZValue;
212 
213 };
214 }
215 #endif
#define ApproxMVBB_ASSERTMSG(condition, message)
An Assert Macro to use within C++ code.
AABB & unite(const MatrixBase< Derived > &p)
Definition: AABB.hpp:94
VectorStat< Dim > m_minPoint
Definition: AABB.hpp:254
OOBB computeMVBB(const Vector3 &zDir, const MatrixBase< Derived > &points)
These are some container definitions.
Eigen::Matrix< Scalar, 2, 2 > Matrix22
#define ApproxMVBB_ERRORMSG(message)
#define APPROXMVBB_EXPORT
Definition: Platform.hpp:60
VectorStat< Dim > m_maxPoint
Definition: AABB.hpp:255
MatrixStatDyn< 2 > Matrix2Dyn
Eigen::Matrix< Scalar, 3, 3 > Matrix33
void project(const MatrixBase< Derived > &points)
#define ApproxMVBB_DEFINE_POINTS_CONFIG_TYPES
void makeCoordinateSystem(Vector3 &v1, Vector3 &v2, Vector3 &v3)
This function makes an orthogonal normed right-hand coordinate system. If the z-axis is the input...
EIGEN_MAKE_ALIGNED_OPERATOR_NEW ApproxMVBB_DEFINE_MATRIX_TYPES ApproxMVBB_DEFINE_POINTS_CONFIG_TYPES OOBB computeMVBBApprox(const Vector3 &zDir, const MatrixBase< Derived > &points, const PREC epsilon)
bool checkOrthogonality(Vector3 &v1, Vector3 &v2, Vector3 &v3, PREC eps=1e-6)
Eigen::Matrix< Scalar, 2, 1 > Vector2
Matrix2Dyn m_p
Projected points in frame K.
Matrix33 m_A_KI
Transformation from I frame into the projection frame K.
Eigen::Matrix< Scalar, 3, 1 > Vector3
#define ApproxMVBB_DEFINE_MATRIX_TYPES
Definition: TypeDefs.hpp:26


asr_approx_mvbb
Author(s): Gassner Nikolai
autogenerated on Mon Jun 10 2019 12:38:08