BitmapPrimitiveShape.cpp
Go to the documentation of this file.
1 #include "BitmapPrimitiveShape.h"
2 #include "Bitmap.h"
3 #include <iostream>
4 #include <fstream>
5 #include <sstream>
6 #include <iomanip>
7 #include "IndexIterator.h"
8 #include <MiscLib/Performance.h>
9 #include <float.h>
10 using namespace MiscLib;
11 
13  const GfxTL::AABox< GfxTL::Vector2Df >& bbox, float epsilon,
14  size_t uextent, size_t vextent, MiscLib::Vector< char >* bmp) const
15 {
16  // the default case is: do nothing
17 }
18 
21  float epsilon, size_t uextent, size_t vextent,
22  MiscLib::Vector< int >* componentImg,
23  MiscLib::Vector< std::pair< int, size_t > >* labels) const
24 {
25  // default: do nothing
26 }
27 
28 bool BitmapPrimitiveShape::Init(bool binary, std::istream* i)
29 {
30  if (binary)
31  {
33  size_t uextent, vextent;
34  // read number of components
35  size_t size;
36  i->read((char*)&size, sizeof(size));
37  if (size)
38  {
39  // read bbox
40  i->read((char*)&bbox, sizeof(bbox));
41  // read uextent and vextent
42  i->read((char*)&uextent, sizeof(uextent));
43  i->read((char*)&vextent, sizeof(vextent));
44  // read every component
45  for (size_t j = 0; j < size; ++j)
46  {
47  // read number of polys in component
48  size_t numPolys;
49  i->read((char*)&numPolys, sizeof(numPolys));
50  for (size_t k = 0; k < numPolys; ++k)
51  {
52  // read number of points in poly
53  size_t numPoints;
54  i->read((char*)&numPoints, sizeof(numPoints));
56  for (size_t l = 0; l < numPoints; ++l)
57  {
58  i->read((char*)&pp, sizeof(pp));
59  }
60  }
61  }
62  }
63  }
64  else
65  {
67  size_t uextent, vextent;
68  // read number of components
69  size_t size;
70  (*i) >> size;
71  if (size)
72  {
73  // read bbox
74  (*i) >> bbox.Min()[0] >> bbox.Max()[0]
75  >> bbox.Min()[1] >> bbox.Max()[1];
76  // read uextent and vextent
77  (*i) >> uextent >> vextent;
78  // read every component
79  for (size_t j = 0; j < size; ++j)
80  {
81  // read number of polys in component
82  size_t numPolys;
83  (*i) >> numPolys;
84  for (size_t k = 0; k < numPolys; ++k)
85  {
86  // read number of points in poly
87  size_t numPoints;
88  (*i) >> numPoints;
90  for (size_t l = 0; l < numPoints; ++l)
91  {
92  (*i) >> pp[0] >> pp[1];
93  }
94  }
95  }
96  }
97  }
98  return true;
99 }
100 
103  MiscLib::Vector< std::pair< int, size_t > >& labels, bool doFiltering)
104 {
105  // first find the extent in the parametrization
106  // but remember the parametrized points for projection into a bitmap
107  size_t size = indices->size();
108  if (!size)
109  {
110  return 0;
111  }
112 
113  // set up bitmap
115 
116  BuildBitmap(pc, &epsilon, indices->begin(), indices->end(), &bitmapInfo.params,
117  &bitmapInfo.bbox, &bitmapInfo.bitmap, &bitmapInfo.uextent, &bitmapInfo.vextent, &bitmapInfo.bmpIdx);
118 
119  /*static int fname_int = 0;
120  std::ostringstream fn;
121  fn << "bitmapImg" << fname_int++ << ".txt";
122  std::ofstream file;
123  file.open(fn.str().c_str(), std::ios::out);
124  for(size_t j = 0; j < vextent; ++j)
125  {
126  for(size_t i = 0; i < uextent; ++i)
127  file << bitmap[j * uextent + i];
128  file << std::endl;
129  }
130  file.close();*/
131 
132  // do a wrapping by copying pixels
133  PreWrapBitmap(bitmapInfo.bbox, epsilon, bitmapInfo.uextent, bitmapInfo.vextent, &bitmapInfo.bitmap);
134 
135  MiscLib::Vector< char > tempBmp(bitmapInfo.bitmap.size()); // temporary bitmap object
136  bool uwrap, vwrap;
137  WrapBitmap(bitmapInfo.bbox, epsilon, &uwrap, &vwrap);
138 
139  if (doFiltering)
140  {
141  // closing
142  DilateCross(bitmapInfo.bitmap, bitmapInfo.uextent, bitmapInfo.vextent, uwrap, vwrap, &tempBmp);
143  ErodeCross(tempBmp, bitmapInfo.uextent, bitmapInfo.vextent, uwrap, vwrap, &bitmapInfo.bitmap);
144  // opening
145  //ErodeCross(bitmap, uextent, vextent, uwrap, vwrap, &tempBmp);
146  //DilateCross(tempBmp, uextent, vextent, uwrap, vwrap, &bitmap);
147  }
148 
149  Components(bitmapInfo.bitmap, bitmapInfo.uextent, bitmapInfo.vextent, uwrap, vwrap, &componentsImg,
150  &labels);
151  if (labels.size() <= 1) // found no connected component!
152  {
153  return 0; // associate no points with this shape
154  }
155 
156  WrapComponents(bitmapInfo.bbox, epsilon, bitmapInfo.uextent, bitmapInfo.vextent, &componentsImg, &labels);
157 
158  return labels.size();
159 }
160 
162  const PointCloud& pc, float epsilon,
163  MiscLib::Vector< size_t >* indices, bool doFiltering, float* borderRatio)
164 {
165  MiscLib::Vector< int > componentsImg;
167 
168  BitmapInfo bitmapInfo;
169  if (AllConnectedComponents(pc, epsilon, bitmapInfo, indices, componentsImg, labels, doFiltering) <= 1)
170  {
171  return 0;
172  }
173 
174  size_t size = indices->size();
176 
177  // find the largest component
178  size_t maxComp = 1;
179  for (size_t i = 2; i < labels.size(); ++i)
180  if (labels[maxComp].second < labels[i].second)
181  {
182  maxComp = i;
183  }
184 
186  bbox.Min() = GfxTL::Vector2Df(
187  std::numeric_limits< float >::infinity(),
188  std::numeric_limits< float >::infinity());
189  bbox.Max() = -bbox.Min();
190  // compute bbox and update indices
191  size_t offset = 0;
192  for (size_t i = 0; i < size; ++i)
193  {
194  if (componentsImg[bitmapInfo.bmpIdx[i]] == labels[maxComp].first)
195  {
196  std::swap(begin[offset], begin[i]);
197  offset++;
198  // update bounding box
199  if (bbox.Min()[0] > bitmapInfo.params[i].first)
200  {
201  bbox.Min()[0] = bitmapInfo.params[i].first;
202  }
203  if (bbox.Max()[0] < bitmapInfo.params[i].first)
204  {
205  bbox.Max()[0] = bitmapInfo.params[i].first;
206  }
207  if (bbox.Min()[1] > bitmapInfo.params[i].second)
208  {
209  bbox.Min()[1] = bitmapInfo.params[i].second;
210  }
211  if (bbox.Max()[1] < bitmapInfo.params[i].second)
212  {
213  bbox.Max()[1] = bitmapInfo.params[i].second;
214  }
215  }
216  }
217 
218  // ratio between border and connected-comp size should be calculated if borderRatio is a valid pointer
219  if (borderRatio)
220  {
221  int borderPixels = 0;
222  int maxLabel = labels[maxComp].first;
223  int row = bitmapInfo.uextent;
224  int pos = 0;
225  char numNeighbours = 0;
226  int ccSize = 0;
227 
228  // test neightbourhood for all bitmappixels that are not marginal
229  for (size_t v = 1; v < bitmapInfo.vextent - 1; ++v)
230  {
231  for (size_t u = 1; u < bitmapInfo.uextent - 1; ++u)
232  {
233  pos = row + u;
234 
235  if (componentsImg[pos] == maxLabel)
236  {
237  ccSize++;
238  numNeighbours = bitmapInfo.bitmap[pos - 1] + bitmapInfo.bitmap[pos + 1] +
239  bitmapInfo.bitmap[pos - bitmapInfo.uextent - 1] + bitmapInfo.bitmap[pos - bitmapInfo.uextent + 1] +
240  bitmapInfo.bitmap[pos + bitmapInfo.uextent - 1] + bitmapInfo.bitmap[pos + bitmapInfo.uextent + 1] +
241  bitmapInfo.bitmap[pos - bitmapInfo.uextent] + bitmapInfo.bitmap[pos + bitmapInfo.uextent];
242 
243  if ((int)numNeighbours != 8)
244  {
245  ++borderPixels;
246  }
247  }
248  }
249  row += bitmapInfo.uextent;
250  }
251 
252  // check left/right margins
253  row = bitmapInfo.uextent;
254  for (size_t v = 1; v < bitmapInfo.vextent - 1; ++v)
255  {
256  ccSize++;
257  if (componentsImg[row] == maxLabel)
258  {
259  ++borderPixels;
260  }
261 
262  ccSize++;
263  if (componentsImg[row + bitmapInfo.uextent - 1] == maxLabel)
264  {
265  ++borderPixels;
266  }
267 
268  row += bitmapInfo.uextent;
269  }
270 
271  // check top/bottom margins
272  row = (bitmapInfo.vextent - 1) * bitmapInfo.uextent;
273  for (size_t u = 0; u < bitmapInfo.uextent; ++u)
274  {
275  ccSize++;
276  if (componentsImg[u] == maxLabel)
277  {
278  ++borderPixels;
279  }
280 
281  ccSize++;
282  if (componentsImg[row + u] == maxLabel)
283  {
284  ++borderPixels;
285  }
286  }
287 
288  *borderRatio = static_cast<float>(borderPixels) / static_cast<float>(ccSize);
289  }
290 
291  m_extBbox = bbox;
292  return offset;
293 }
294 
296  float epsilon, size_t begin, size_t end,
297  std::deque< ComponentPolygons >* polys) const
298 {
300  size_t uextent, vextent;
301  BuildPolygons(pc, epsilon, begin, end, &bbox, &uextent, &vextent, polys);
302 }
303 
305  float epsilon, size_t begin, size_t end, PointCloud* bmpPc) const
306 {
307  // constructing the bitmap is similar to ConnectedComponent
308  MiscLib::Vector< std::pair< float, float > > params, extParams;
312  size_t uextent, vextent;
313  BuildBitmap(pc, &epsilon, IndexIterator(begin), IndexIterator(end), &params,
314  &bbox, &bitmap, &uextent, &vextent, &bmpIdx);
315  m_extBbox = bbox;
316 
317  //// do closing
318  //MiscLib::Vector< char > tempBmp(bitmap.size());
319  //DilateSquare(bitmap, uextent, vextent, false, false, &tempBmp);
320  //ErodeSquare(tempBmp, uextent, vextent, false, false, &bitmap);
321  //// do opening
322  //ErodeSquare(bitmap, uextent, vextent, false, false, &tempBmp);
323  //DilateSquare(tempBmp, uextent, vextent, false, false, &bitmap);
324 
325  if (bmpPc)
326  {
327  size_t count = 0;
328  for (size_t i = 0; i < vextent * uextent; ++i)
329  {
330  // create point on surface
331  if (!bitmap[i])
332  {
333  continue;
334  }
335  if (count >= bmpPc->size())
336  {
337  bmpPc->resize(2 * count + 1);
338  }
339  if (InSpace(i % uextent, i / uextent, epsilon, bbox, uextent,
340  vextent, &(*bmpPc)[count].pos, &(*bmpPc)[count].normal))
341  {
342  ++count;
343  }
344  }
345  bmpPc->resize(count);
346  }
347 }
348 
350  size_t begin, size_t end, GfxTL::AABox< GfxTL::Vector2Df >* bbox,
351  size_t* uextent, size_t* vextent,
352  std::deque< ComponentPolygons >* polys) const
353 {
354  // curves are extracted in the following way:
355  // first the bitmap is constructed
356  // then connected components are found
357  // for each component the curves are found
358 
359  // constructing the bitmap is similar to ConnectedComponent
360  // -> use the same code
364  BuildBitmap(pc, &epsilon, IndexIterator(begin), IndexIterator(end), &params,
365  bbox, &bitmap, uextent, vextent, &bmpIdx);
366 
367  // do closing
368  MiscLib::Vector< char > tempBmp(bitmap.size());
369  DilateCross(bitmap, *uextent, *vextent, false, false, &tempBmp);
370  ErodeCross(tempBmp, *uextent, *vextent, false, false, &bitmap);
371 
372  // find connected components
373  MiscLib::Vector< int > componentsImg;
375  Components(bitmap, *uextent, *vextent, false, false, &componentsImg,
376  &labels);
377  if (labels.size() <= 1) // found no connected component!
378  {
379  return;
380  }
381 
382  // for each component find all polygons
383  for (size_t i = 1; i < labels.size(); ++i)
384  {
385  polys->resize(polys->size() + 1);
386  ComponentLoops(componentsImg, *uextent, *vextent, labels[i].first,
387  false, false, &(*polys)[polys->size() - 1]);
388  }
389 }
GfxTL::VectorXD
Definition: MatrixXX.h:21
DilateCross
void DilateCross(const MiscLib::Vector< char > &bitmap, size_t uextent, size_t vextent, bool uwrap, bool vwrap, MiscLib::Vector< char > *dilated)
Definition: Bitmap.cpp:154
GfxTL::AABox::Max
Point & Max()
Definition: AABox.hpp:74
Performance.h
BitmapPrimitiveShape::PreWrapBitmap
virtual void PreWrapBitmap(const GfxTL::AABox< GfxTL::Vector2Df > &bbox, float epsilon, size_t uextent, size_t vextent, MiscLib::Vector< char > *bmp) const
Definition: BitmapPrimitiveShape.cpp:12
BitmapInfo::bbox
GfxTL::AABox< GfxTL::Vector2Df > bbox
Definition: BitmapPrimitiveShape.h:25
MiscLib::Vector::resize
void resize(size_type s, const value_type &v)
Definition: Vector.h:222
Bitmap.h
BitmapInfo::params
MiscLib::Vector< std::pair< float, float > > params
Definition: BitmapPrimitiveShape.h:23
BitmapInfo::bitmap
MiscLib::Vector< char > bitmap
Definition: BitmapPrimitiveShape.h:24
MiscLib::Vector< char >
BitmapInfo
Definition: BitmapPrimitiveShape.h:21
BitmapPrimitiveShape::BuildPolygons
void BuildPolygons(const PointCloud &pc, float epsilon, size_t begin, size_t end, GfxTL::AABox< GfxTL::Vector2Df > *bbox, size_t *uextent, size_t *vextent, std::deque< ComponentPolygons > *polys) const
Definition: BitmapPrimitiveShape.cpp:349
armarx::armem::client::util::swap
void swap(SubscriptionHandle &first, SubscriptionHandle &second)
Definition: SubscriptionHandle.cpp:66
MiscLib::Vector::size
size_type size() const
Definition: Vector.h:212
BitmapPrimitiveShape::GenerateBitmapPoints
void GenerateBitmapPoints(const PointCloud &pc, float epsilon, size_t begin, size_t end, PointCloud *bmpPc) const
Definition: BitmapPrimitiveShape.cpp:304
BitmapPrimitiveShape::TrimmingPolygons
void TrimmingPolygons(const PointCloud &pc, float epsilon, size_t begin, size_t end, std::deque< ComponentPolygons > *polys) const
Definition: BitmapPrimitiveShape.cpp:295
pcl::graph::indices
pcl::PointIndices::Ptr indices(const PCG &g)
Retrieve the indices of the points of the point cloud stored in a point cloud graph that actually bel...
Definition: point_cloud_graph.h:737
MiscLib
Definition: AlignedAllocator.h:11
IndexIterator.h
BitmapPrimitiveShape::Init
bool Init(bool binary, std::istream *i)
Definition: BitmapPrimitiveShape.cpp:28
BitmapPrimitiveShape.h
BitmapInfo::bmpIdx
MiscLib::Vector< size_t > bmpIdx
Definition: BitmapPrimitiveShape.h:26
for
for(;yybottom<=yytop;yybottom++)
Definition: Grammar.cpp:790
BitmapPrimitiveShape::AllConnectedComponents
size_t AllConnectedComponents(const PointCloud &pc, float epsilon, BitmapInfo &bitmapInfo, MiscLib::Vector< size_t > *indices, MiscLib::Vector< int > &componentsImg, MiscLib::Vector< std::pair< int, size_t > > &labels, bool doFiltering=true)
Definition: BitmapPrimitiveShape.cpp:101
ErodeCross
void ErodeCross(const MiscLib::Vector< char > &bitmap, size_t uextent, size_t vextent, bool uwrap, bool vwrap, MiscLib::Vector< char > *eroded)
Definition: Bitmap.cpp:465
PointCloud
Definition: PointCloud.h:69
armarx::ctrlutil::v
double v(double t, double v0, double a0, double j)
Definition: CtrlUtil.h:39
BitmapInfo::vextent
size_t vextent
Definition: BitmapPrimitiveShape.h:27
GfxTL::Vector2Df
VectorXD< 2, float > Vector2Df
Definition: VectorXD.h:674
GfxTL::AABox::Min
Point & Min()
Definition: AABox.hpp:62
BitmapPrimitiveShape::ConnectedComponent
size_t ConnectedComponent(const PointCloud &pc, float epsilon, MiscLib::Vector< size_t > *indices, bool doFiltering=true, float *borderRatio=0)
Definition: BitmapPrimitiveShape.cpp:161
pc
Introduction Thank you for taking interest in our work and downloading this software This library implements the algorithm described in the paper R R R Klein Efficient RANSAC for Point Cloud Shape in Computer Graphics Blackwell June If you use this software you should cite the aforementioned paper in any resulting publication Please send comments or bug reports to Ruwen Roland BUT NOT LIMITED THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY OR CONSEQUENTIAL WHETHER IN STRICT OR EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE Example usage This section shows how to use the library to detect the shapes in a point cloud PointCloud pc
Definition: ReadMe.txt:68
Components
void Components(const MiscLib::Vector< char > &bitmap, size_t uextent, size_t vextent, bool uwrap, bool vwrap, MiscLib::Vector< int > *componentsImg, MiscLib::Vector< std::pair< int, size_t > > *labels)
Definition: Bitmap.cpp:649
ComponentLoops
void ComponentLoops(const MiscLib::Vector< int > &componentImg, size_t uextent, size_t vextent, int label, bool uwrap, bool vwrap, MiscLib::Vector< MiscLib::Vector< GfxTL::VectorXD< 2, size_t > > > *polys)
Definition: Bitmap.cpp:1000
BitmapPrimitiveShape::WrapComponents
virtual void WrapComponents(const GfxTL::AABox< GfxTL::Vector2Df > &bbox, float epsilon, size_t uextent, size_t vextent, MiscLib::Vector< int > *componentImg, MiscLib::Vector< std::pair< int, size_t > > *labels) const
Definition: BitmapPrimitiveShape.cpp:19
BitmapInfo::uextent
size_t uextent
Definition: BitmapPrimitiveShape.h:27
IndexIterator
Definition: IndexIterator.h:9
GfxTL::AABox
Definition: AABox.h:18