OLPTools.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
5  *
6  * ArmarX is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License version 2 as
8  * published by the Free Software Foundation.
9  *
10  * ArmarX is distributed in the hope that it will be useful, but
11  * WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program. If not, see <http://www.gnu.org/licenses/>.
17  *
18  * @package
19  * @author
20  * @date
21  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
22  * GNU General Public License
23  */
24 #include "OLPTools.h"
25 
26 #include "ObjectHypothesis.h"
27 
28 
29 // IVT
30 #include <Calibration/Calibration.h>
31 #include <Calibration/StereoCalibration.h>
32 #include <Image/ByteImage.h>
33 #include <Image/ImageProcessor.h>
34 
35 // OpenCV
36 #include <opencv2/opencv.hpp>
37 
38 #include <ArmarXCore/core/logging/Logging.h>
39 
40 namespace COLPTools
41 {
42  void
43  InterpolateRotation(const Mat3d m1, const Mat3d m2, const float fWeight, Mat3d& mResult)
44  {
45  Mat3d mTemp1, mTemp2;
46  Vec3d vTemp;
47  float fRotationAngle;
48  Math3d::Transpose(m1, mTemp1);
49  Math3d::MulMatMat(m2, mTemp1, mTemp2);
50  Math3d::GetAxisAndAngle(mTemp2, vTemp, fRotationAngle);
51 
52  if (Math3d::ScalarProduct(vTemp, vTemp) < 0.9f) // rotation matrices are identical
53  {
54  Math3d::SetMat(mResult, m1);
55  }
56  else
57  {
58  Math3d::SetRotationMatAxis(mTemp1, vTemp, fWeight * fRotationAngle);
59  Math3d::MulMatMat(mTemp1, m1, mResult);
60  }
61  }
62 
63  void
64  InterpolateTransformation(const Mat3d m1,
65  const Vec3d v1,
66  const Mat3d m2,
67  const Vec3d v2,
68  const float fWeight,
69  Mat3d& mResult,
70  Vec3d& vResult)
71  {
72  InterpolateRotation(m1, m2, fWeight, mResult);
73  Math3d::AddVecVec(v1, v2, vResult);
74  Math3d::MulVecScalar(vResult, fWeight, vResult);
75  }
76 
77  void
78  ClusterXMeans(const std::vector<CHypothesisPoint*>& aPoints,
79  const int nMinNumClusters,
80  const int nMaxNumClusters,
81  const float fBICFactor,
82  std::vector<std::vector<CHypothesisPoint*>>& aaPointClusters)
83  {
84  std::vector<Vec3d> aPointsVec3d;
85  std::vector<std::vector<Vec3d>> aaPointClustersVec3d;
86  std::vector<std::vector<int>> aaOldIndices;
87 
88  for (size_t i = 0; i < aPoints.size(); i++)
89  {
90  aPointsVec3d.push_back(aPoints.at(i)->vPosition);
91  }
92 
93  // call pure 3D point clustering
94  ClusterXMeans(aPointsVec3d,
95  nMinNumClusters,
96  nMaxNumClusters,
97  fBICFactor,
98  aaPointClustersVec3d,
99  aaOldIndices);
100 
101  //ARMARX_VERBOSE_S << "%ld clusters\n", aaPointClustersVec3d.size());
102 
103  // put hypothesis points into clusters according to the indices obtained from the pure 3D point clustering
104  aaPointClusters.clear();
105 
106  for (size_t i = 0; i < aaOldIndices.size(); i++)
107  {
108  std::vector<CHypothesisPoint*> aNewCluster;
109  aaPointClusters.push_back(aNewCluster);
110  }
111 
112  for (size_t i = 0; i < aaOldIndices.size(); i++)
113  {
114  for (size_t j = 0; j < aaOldIndices.at(i).size(); j++)
115  {
116  aaPointClusters.at(i).push_back(aPoints.at(aaOldIndices.at(i).at(j))->GetCopy());
117  }
118  }
119  }
120 
121  void
122  ClusterXMeans(const std::vector<Vec3d>& aPoints,
123  const int nMinNumClusters,
124  const int nMaxNumClusters,
125  const float fBICFactor,
126  std::vector<std::vector<Vec3d>>& aaPointClusters,
127  std::vector<std::vector<int>>& aaOldIndices)
128  {
129  cv::Mat mSamples;
130  cv::Mat mClusterLabels;
131  const int nNumberOfDifferentInitialisations = 7; // 5
132  cv::TermCriteria tTerminationCriteria(
133  cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 50, 0.01);
134 
135  // copy the points
136  const int nNumberOfSamples = aPoints.size();
137  mSamples.create(nNumberOfSamples, 3, CV_32FC1);
138 
139  for (int i = 0; i < nNumberOfSamples; i++)
140  {
141  mSamples.at<float>(i, 0) = aPoints.at(i).x;
142  mSamples.at<float>(i, 1) = aPoints.at(i).y;
143  mSamples.at<float>(i, 2) = aPoints.at(i).z;
144  }
145 
146  mClusterLabels.create(nNumberOfSamples, 1, CV_32SC1);
147 
148  // execute k-means for several values of k and find the value for k that minimises the
149  // Bayesian Information Criterion (BIC)
150  double dMinBIC = FLT_MAX;
151  int nOptK = nMinNumClusters;
152  const int nMaxNumClustersSafe =
153  (10 * nMaxNumClusters < nNumberOfSamples) ? nMaxNumClusters : nNumberOfSamples / 10;
154 
155 #pragma omp parallel for schedule(dynamic, 1)
156  for (int i = nMaxNumClustersSafe; i >= nMinNumClusters; i--)
157  {
158  // maybe we need to make parallel instances of mClusterLabels
159 #ifdef OLP_USE_NEW_OPENCV
160  cv::kmeans(mSamples,
161  i,
162  mClusterLabels,
163  tTerminationCriteria,
164  nNumberOfDifferentInitialisations,
165  cv::KMEANS_RANDOM_CENTERS);
166 #else
167  cv::Mat mClusterCenters;
168  cv::kmeans(mSamples,
169  i,
170  mClusterLabels,
171  tTerminationCriteria,
172  nNumberOfDifferentInitialisations,
173  cv::KMEANS_PP_CENTERS,
174  &mClusterCenters);
175 #endif
176  double dLogVar, dBIC;
177  Vec3d* pvClusterMeans = new Vec3d[nMaxNumClusters];
178  double* pdClusterVariances = new double[nMaxNumClusters];
179  int* pnClusterSizes = new int[nMaxNumClusters];
180 
181  // calculate variances of the clusters
182  double dMLVariance = 0;
183 
184  for (int j = 0; j < i; j++)
185  {
186  pvClusterMeans[j].x = 0;
187  pvClusterMeans[j].y = 0;
188  pvClusterMeans[j].z = 0;
189  pdClusterVariances[j] = 0;
190  pnClusterSizes[j] = 0;
191 
192  for (int l = 0; l < nNumberOfSamples; l++)
193  {
194  if (mClusterLabels.at<int>(l, 0) == j)
195  {
196  pvClusterMeans[j].x += mSamples.at<float>(l, 0);
197  pvClusterMeans[j].y += mSamples.at<float>(l, 1);
198  pvClusterMeans[j].z += mSamples.at<float>(l, 2);
199  pnClusterSizes[j]++;
200  }
201  }
202 
203  pvClusterMeans[j].x /= (float)pnClusterSizes[j];
204  pvClusterMeans[j].y /= (float)pnClusterSizes[j];
205  pvClusterMeans[j].z /= (float)pnClusterSizes[j];
206 
207  for (int l = 0; l < nNumberOfSamples; l++)
208  {
209  if (mClusterLabels.at<int>(l, 0) == j)
210  {
211  pdClusterVariances[j] +=
212  (pvClusterMeans[j].x - mSamples.at<float>(l, 0)) *
213  (pvClusterMeans[j].x - mSamples.at<float>(l, 0)) +
214  (pvClusterMeans[j].y - mSamples.at<float>(l, 1)) *
215  (pvClusterMeans[j].x - mSamples.at<float>(l, 1)) +
216  (pvClusterMeans[j].z - mSamples.at<float>(l, 2)) *
217  (pvClusterMeans[j].x - mSamples.at<float>(l, 2));
218  }
219  }
220 
221  if (pnClusterSizes[j] > 1)
222  {
223  pdClusterVariances[j] /= (float)(pnClusterSizes[j] - 1);
224  }
225  else
226  {
227  pdClusterVariances[j] = 0;
228  }
229 
230  dMLVariance += pdClusterVariances[j];
231  }
232 
233  const int nNumberOfFreeParameters = (i - 1) + (3 * i) + i;
234  //dLogVar = log(dMLVariance);
235  dLogVar = log(dMLVariance / i);
236  dBIC = fBICFactor * 0.35 * dLogVar +
237  ((double)nNumberOfFreeParameters / (double)nNumberOfSamples) *
238  log((double)nNumberOfSamples); // 0.4 // 1.5 with manual variance
239 
240 #pragma omp critical
241  {
242  if (dBIC < dMinBIC)
243  {
244  dMinBIC = dBIC;
245  nOptK = i;
246  }
247  }
248 
249  delete[] pvClusterMeans;
250  delete[] pdClusterVariances;
251  delete[] pnClusterSizes;
252 
253  //ARMARX_VERBOSE_S << "k-means with " << i << " clusters. log(var): " << dLogVar << " BIC: " << dBIC;
254  }
255 
256  // execute k-means with optimal k
257  if (nOptK > 1)
258  {
259 #ifdef OLP_USE_NEW_OPENCV
260  double dKMeansCompactness = cv::kmeans(mSamples,
261  nOptK,
262  mClusterLabels,
263  tTerminationCriteria,
264  nNumberOfDifferentInitialisations,
265  cv::KMEANS_RANDOM_CENTERS);
266 #else
267  cv::Mat mClusterCenters;
268  double dKMeansCompactness = cv::kmeans(mSamples,
269  nOptK,
270  mClusterLabels,
271  tTerminationCriteria,
272  nNumberOfDifferentInitialisations,
273  cv::KMEANS_PP_CENTERS,
274  &mClusterCenters);
275 #endif
276 
277  // copy the points belonging to the clusters
278  Vec3d vPoint;
279  aaPointClusters.clear();
280  aaOldIndices.clear();
281 
282  for (int i = 0; i < nOptK; i++)
283  {
284  std::vector<Vec3d> aNewCluster;
285  aaPointClusters.push_back(aNewCluster);
286  std::vector<int> aClusterIndices;
287  aaOldIndices.push_back(aClusterIndices);
288  }
289 
290  for (int i = 0; i < nNumberOfSamples; i++)
291  {
292  const int nLabel = mClusterLabels.at<int>(i, 0);
293 
294  if ((nLabel >= 0) && (nLabel < nOptK))
295  {
296  vPoint.x = mSamples.at<float>(i, 0);
297  vPoint.y = mSamples.at<float>(i, 1);
298  vPoint.z = mSamples.at<float>(i, 2);
299  aaPointClusters.at(nLabel).push_back(vPoint);
300  aaOldIndices.at(nLabel).push_back(i);
301  }
302  else
303  {
304  ARMARX_WARNING_S << "Invalid cluster label: " << nLabel << "nOptK: " << nOptK
305  << ", i: " << i << ", nNumberOfSamples: " << nNumberOfSamples
306  << ", dKMeansCompactness: " << dKMeansCompactness;
307  break;
308  }
309  }
310  }
311  else
312  {
313  aaPointClusters.clear();
314  std::vector<Vec3d> aNewCluster;
315  aaPointClusters.push_back(aNewCluster);
316  aaOldIndices.clear();
317  std::vector<int> aClusterIndices;
318  aaOldIndices.push_back(aClusterIndices);
319  Vec3d vPoint;
320 
321  for (int i = 0; i < nNumberOfSamples; i++)
322  {
323  vPoint.x = mSamples.at<float>(i, 0);
324  vPoint.y = mSamples.at<float>(i, 1);
325  vPoint.z = mSamples.at<float>(i, 2);
326  aaPointClusters.at(0).push_back(vPoint);
327  aaOldIndices.at(0).push_back(i);
328  }
329  }
330  }
331 
332  void
333  FilterForegroundPoints(const std::vector<CHypothesisPoint*>& aAllPoints,
334  const CByteImage* pForegroundImage,
335  const CCalibration* calibration,
336  std::vector<CHypothesisPoint*>& aForegroundPoints)
337  {
338  Vec2d vImagePoint;
339 
340  for (size_t i = 0; i < aAllPoints.size(); i++)
341  {
342  calibration->WorldToImageCoordinates(aAllPoints.at(i)->vPosition, vImagePoint, false);
343  const int nIndex = ((int)vImagePoint.y) * OLP_IMG_WIDTH + (int)vImagePoint.x;
344 
345  if ((nIndex >= 0) && (nIndex < OLP_IMG_WIDTH * OLP_IMG_HEIGHT))
346  {
347  if (pForegroundImage->pixels[nIndex] > 0)
348  {
349  aForegroundPoints.push_back(aAllPoints.at(i)->GetCopy());
350  }
351  }
352  }
353  }
354 
355  bool
356  PointIsInForeground(const Vec3d vPoint,
357  const CByteImage* pForegroundImage,
358  const CCalibration* calibration)
359  {
360  Vec2d vImagePoint;
361  calibration->WorldToImageCoordinates(vPoint, vImagePoint, false);
362  const int nIndex = ((int)vImagePoint.y) * OLP_IMG_WIDTH + (int)vImagePoint.x;
363 
364  if ((nIndex >= 0) && (nIndex < OLP_IMG_WIDTH * OLP_IMG_HEIGHT))
365  {
366  return (pForegroundImage->pixels[nIndex] > 0);
367  }
368  else
369  {
370  return false;
371  }
372  }
373 
374  bool
375  PointIsInForeground(const CHypothesisPoint* pPoint,
376  const CByteImage* pForegroundImage,
377  const CCalibration* calibration)
378  {
379  return PointIsInForeground(pPoint->vPosition, pForegroundImage, calibration);
380  }
381 
382  void
383  CalculateForegroundRatioOfHypothesis(const CObjectHypothesis* pHypothesis,
384  const CByteImage* pForegroundImage,
385  const CCalibration* calibration,
386  float& fForegroundRatio,
387  int& nNumForegroundPixels)
388  {
389  // get the 2D points
390  int nNumPoints;
391  Vec2d* pPoints2D;
392 
393  if (pHypothesis->aVisibleConfirmedPoints.size() > OLP_MIN_NUM_FEATURES)
394  {
395  nNumPoints = pHypothesis->aVisibleConfirmedPoints.size();
396  pPoints2D = new Vec2d[nNumPoints];
397 
398  for (int i = 0; i < nNumPoints; i++)
399  {
400  calibration->WorldToImageCoordinates(
401  pHypothesis->aVisibleConfirmedPoints.at(i)->vPosition, pPoints2D[i], false);
402  }
403  }
404  else
405  {
406  nNumPoints =
407  pHypothesis->aVisibleConfirmedPoints.size() + pHypothesis->aNewPoints.size();
408  pPoints2D = new Vec2d[nNumPoints];
409 
410  for (size_t i = 0; i < pHypothesis->aNewPoints.size(); i++)
411  {
412  calibration->WorldToImageCoordinates(
413  pHypothesis->aNewPoints.at(i)->vPosition, pPoints2D[i], false);
414  }
415 
416  for (size_t i = 0; i < pHypothesis->aVisibleConfirmedPoints.size(); i++)
417  {
418  calibration->WorldToImageCoordinates(
419  pHypothesis->aVisibleConfirmedPoints.at(i)->vPosition,
420  pPoints2D[pHypothesis->aNewPoints.size() + i],
421  false);
422  }
423  }
424 
425  int nNumForegroundPoints = 0;
426  long nForegroundSum = 0;
427 
428  for (int i = 0; i < nNumPoints; i++)
429  {
430  int nIndex = (OLP_IMG_WIDTH * (int)pPoints2D[i].y) + (int)pPoints2D[i].x;
431 
432  if ((nIndex > 0) && (nIndex < OLP_IMG_WIDTH * OLP_IMG_HEIGHT))
433  {
434  nForegroundSum += pForegroundImage->pixels[nIndex];
435 
436  if (pForegroundImage->pixels[nIndex] > 0)
437  {
438  nNumForegroundPoints++;
439  }
440  }
441  }
442 
443  nNumForegroundPixels = nNumForegroundPoints;
444  fForegroundRatio = (double)nForegroundSum / ((double)nNumPoints * 255.0);
445 
446 
447  /*
448  // calculate foreground ratio of enclosing rectangle
449 
450  // get enclosing rectangle
451  Vec2d p1, p2, p3, p4;
452  bool bRotated;
453  GetEnclosingRectangle(pPoints2d, nNumPoints, true, p1, p2, p3, p4, bRotated);
454 
455  // make sure points are within image
456  p1.x = (p1.x < 0) ? 0 : p1.x;
457  p1.x = (p1.x > OLP_IMG_WIDTH-1) ? OLP_IMG_WIDTH-1 : p1.x;
458  p1.y = (p1.y < 0) ? 0 : p1.y;
459  p1.y = (p1.y > OLP_IMG_HEIGHT-1) ? OLP_IMG_HEIGHT-1 : p1.y;
460  p2.x = (p2.x < 0) ? 0 : p2.x;
461  p2.x = (p2.x > OLP_IMG_WIDTH-1) ? OLP_IMG_WIDTH-1 : p2.x;
462  p2.y = (p2.y < 0) ? 0 : p2.y;
463  p2.y = (p2.y > OLP_IMG_HEIGHT-1) ? OLP_IMG_HEIGHT-1 : p2.y;
464  p3.x = (p3.x < 0) ? 0 : p3.x;
465  p3.x = (p3.x > OLP_IMG_WIDTH-1) ? OLP_IMG_WIDTH-1 : p3.x;
466  p3.y = (p3.y < 0) ? 0 : p3.y;
467  p3.y = (p3.y > OLP_IMG_HEIGHT-1) ? OLP_IMG_HEIGHT-1 : p3.y;
468  p4.x = (p4.x < 0) ? 0 : p4.x;
469  p4.x = (p4.x > OLP_IMG_WIDTH-1) ? OLP_IMG_WIDTH-1 : p4.x;
470  p4.y = (p4.y < 0) ? 0 : p4.y;
471  p4.y = (p4.y > OLP_IMG_HEIGHT-1) ? OLP_IMG_HEIGHT-1 : p4.y;
472 
473  // count the foreground pixels in the rectangle
474  if (!bRotated)
475  {
476  const int nMinX = (int)p1.x;
477  const int nMaxX = (int)p2.x;
478  const int nMinY = (int)p1.y;
479  const int nMaxY = (int)p3.y;
480 
481  int nForegroundSum = 0;
482  for (int i=nMinY; i<=nMaxY; i++)
483  {
484  for (int j=nMinX; j<nMaxX; j++)
485  {
486  nForegroundSum += pForegroundImage->pixels[i*OLP_IMG_WIDTH+j];
487  }
488  }
489  nNumForegroundPixels = nForegroundSum / 255;
490  fForegroundRatio = (float)nNumForegroundPixels / (float)((nMaxX-nMinX)*(nMaxY-nMinY));
491  }
492  else
493  {
494  // x ~ (x+y), y ~ (-x+y) =>
495  // p1 is (min, middle), p2 is (middle, min), p3 is (max, middle), p4 is (middle, max)
496  nNumForegroundPixels = CountForegroundPixelsInRectangle(p2, p1, p3, p4, pForegroundImage);
497  fForegroundRatio = (float)nNumForegroundPixels / ((p3.x-p1.x)*(p4.y-p2.y));
498  }
499  */
500 
501  delete[] pPoints2D;
502  }
503 
504  void
505  GetEnclosingRectangle(const Vec2d* pPoints,
506  const int nNumPoints,
507  const bool bUseSecondMaxPoints,
508  Vec2d& p1,
509  Vec2d& p2,
510  Vec2d& p3,
511  Vec2d& p4,
512  bool& bRotated)
513  {
514  // calculate axis-parallel enclosing rectangle
515  float fMinX = pPoints[0].x;
516  float fMaxX = pPoints[0].x;
517  float fMinY = pPoints[0].y;
518  float fMaxY = pPoints[0].y;
519  float fSecondMinX = pPoints[0].x;
520  float fSecondMaxX = pPoints[0].x;
521  float fSecondMinY = pPoints[0].y;
522  float fSecondMaxY = pPoints[0].y;
523 
524  for (int i = 0; i < nNumPoints; i++)
525  {
526  if (pPoints[i].x < fMinX)
527  {
528  fSecondMinX = fMinX;
529  fMinX = pPoints[i].x;
530  }
531  else if (pPoints[i].x < fSecondMinX)
532  {
533  fSecondMinX = pPoints[i].x;
534  }
535 
536  if (pPoints[i].x > fMaxX)
537  {
538  fSecondMaxX = fMaxX;
539  fMaxX = pPoints[i].x;
540  }
541  else if (pPoints[i].x > fSecondMaxX)
542  {
543  fSecondMaxX = pPoints[i].x;
544  }
545 
546  if (pPoints[i].y < fMinY)
547  {
548  fSecondMinY = fMinY;
549  fMinY = pPoints[i].y;
550  }
551  else if (pPoints[i].y < fSecondMinY)
552  {
553  fSecondMinY = pPoints[i].y;
554  }
555 
556  if (pPoints[i].y > fMaxY)
557  {
558  fSecondMaxY = fMaxY;
559  fMaxY = pPoints[i].y;
560  }
561  else if (pPoints[i].y > fSecondMaxY)
562  {
563  fSecondMaxY = pPoints[i].y;
564  }
565  }
566 
567  // calculate rectangle that is rotated by 45 degrees
568  Vec2d* pRotatedPoints = new Vec2d[nNumPoints];
569 
570  for (int i = 0; i < nNumPoints; i++)
571  {
572  pRotatedPoints[i].x = pPoints[i].x - pPoints[i].y;
573  pRotatedPoints[i].y = pPoints[i].x + pPoints[i].y;
574  }
575 
576  float fMinXr = pRotatedPoints[0].x;
577  float fMaxXr = pRotatedPoints[0].x;
578  float fMinYr = pRotatedPoints[0].y;
579  float fMaxYr = pRotatedPoints[0].y;
580  float fSecondMinXr = pRotatedPoints[0].x;
581  float fSecondMaxXr = pRotatedPoints[0].x;
582  float fSecondMinYr = pRotatedPoints[0].y;
583  float fSecondMaxYr = pRotatedPoints[0].y;
584 
585  for (int i = 0; i < nNumPoints; i++)
586  {
587  if (pRotatedPoints[i].x < fMinXr)
588  {
589  fSecondMinXr = fMinXr;
590  fMinXr = pRotatedPoints[i].x;
591  }
592  else if (pRotatedPoints[i].x < fSecondMinXr)
593  {
594  fSecondMinXr = pRotatedPoints[i].x;
595  }
596 
597  if (pRotatedPoints[i].x > fMaxXr)
598  {
599  fSecondMaxXr = fMaxXr;
600  fMaxXr = pRotatedPoints[i].x;
601  }
602  else if (pRotatedPoints[i].x > fSecondMaxXr)
603  {
604  fSecondMaxXr = pRotatedPoints[i].x;
605  }
606 
607  if (pRotatedPoints[i].y < fMinYr)
608  {
609  fSecondMinYr = fMinYr;
610  fMinYr = pRotatedPoints[i].y;
611  }
612  else if (pRotatedPoints[i].y < fSecondMinYr)
613  {
614  fSecondMinYr = pRotatedPoints[i].y;
615  }
616 
617  if (pRotatedPoints[i].y > fMaxYr)
618  {
619  fSecondMaxYr = fMaxYr;
620  fMaxYr = pRotatedPoints[i].y;
621  }
622  else if (pRotatedPoints[i].y > fSecondMaxYr)
623  {
624  fSecondMaxYr = pRotatedPoints[i].y;
625  }
626  }
627 
628  const float fSqrt2Fact = 0.5f * sqrtf(2.0f);
629  fMinXr *= fSqrt2Fact;
630  fMaxXr *= fSqrt2Fact;
631  fMinYr *= fSqrt2Fact;
632  fMaxYr *= fSqrt2Fact;
633  fSecondMinXr *= fSqrt2Fact;
634  fSecondMaxXr *= fSqrt2Fact;
635  fSecondMinYr *= fSqrt2Fact;
636  fSecondMaxYr *= fSqrt2Fact;
637 
638  if (bUseSecondMaxPoints)
639  {
640  // choose the rectangle with the smaller volume
641  if ((fSecondMaxX - fSecondMinX) * (fSecondMaxY - fSecondMinY) <
642  (fSecondMaxXr - fSecondMinXr) * (fSecondMaxYr - fSecondMinYr))
643  {
644  bRotated = false;
645  Math2d::SetVec(p1, fSecondMinX, fSecondMinY);
646  Math2d::SetVec(p2, fSecondMaxX, fSecondMinY);
647  Math2d::SetVec(p3, fSecondMaxX, fSecondMaxY);
648  Math2d::SetVec(p4, fSecondMinX, fSecondMaxY);
649  }
650  else
651  {
652  bRotated = true;
653  Math2d::SetVec(p1,
654  fSqrt2Fact * (fSecondMinXr + fSecondMinYr),
655  fSqrt2Fact * (-fSecondMinXr + fSecondMinYr));
656  Math2d::SetVec(p2,
657  fSqrt2Fact * (fSecondMaxXr + fSecondMinYr),
658  fSqrt2Fact * (-fSecondMaxXr + fSecondMinYr));
659  Math2d::SetVec(p3,
660  fSqrt2Fact * (fSecondMaxXr + fSecondMaxYr),
661  fSqrt2Fact * (-fSecondMaxXr + fSecondMaxYr));
662  Math2d::SetVec(p4,
663  fSqrt2Fact * (fSecondMinXr + fSecondMaxYr),
664  fSqrt2Fact * (-fSecondMinXr + fSecondMaxYr));
665  }
666  }
667  else
668  {
669  // choose the rectangle with the smaller volume
670  if ((fMaxX - fMinX) * (fMaxY - fMinY) < (fMaxXr - fMinXr) * (fMaxYr - fMinYr))
671  {
672  bRotated = false;
673  Math2d::SetVec(p1, fMinX, fMinY);
674  Math2d::SetVec(p2, fMaxX, fMinY);
675  Math2d::SetVec(p3, fMaxX, fMaxY);
676  Math2d::SetVec(p4, fMinX, fMaxY);
677  }
678  else
679  {
680  bRotated = true;
681  Math2d::SetVec(p1, fSqrt2Fact * (fMinXr + fMinYr), fSqrt2Fact * (-fMinXr + fMinYr));
682  Math2d::SetVec(p2, fSqrt2Fact * (fMaxXr + fMinYr), fSqrt2Fact * (-fMaxXr + fMinYr));
683  Math2d::SetVec(p3, fSqrt2Fact * (fMaxXr + fMaxYr), fSqrt2Fact * (-fMaxXr + fMaxYr));
684  Math2d::SetVec(p4, fSqrt2Fact * (fMinXr + fMaxYr), fSqrt2Fact * (-fMinXr + fMaxYr));
685  }
686  }
687 
688  delete[] pRotatedPoints;
689  }
690 
691  int
692  CountForegroundPixelsInRectangle(const Vec2d vMiddleMin,
693  const Vec2d vMinMiddle,
694  const Vec2d vMaxMiddle,
695  const Vec2d vMiddleMax,
696  const CByteImage* pForegroundImage)
697  {
698  int nStartY = (int)vMiddleMin.y;
699  int nEndY = (int)vMiddleMax.y;
700  int nMiddleY1, nMiddleY2;
701  float fDeltaLeft1, fDeltaLeft2, fDeltaLeft3, fDeltaRight1, fDeltaRight2, fDeltaRight3;
702 
703  int nSum = 0;
704 
705  // check if left or right corner comes first
706  if (vMinMiddle.y < vMaxMiddle.y)
707  {
708  nMiddleY1 = (int)vMinMiddle.y;
709  nMiddleY2 = (int)vMaxMiddle.y;
710  fDeltaLeft1 = (vMinMiddle.x - vMiddleMin.x) / (vMinMiddle.y - vMiddleMin.y);
711  fDeltaLeft2 = (vMiddleMax.x - vMinMiddle.x) / (vMiddleMax.y - vMinMiddle.y);
712  fDeltaLeft3 = fDeltaLeft2;
713  fDeltaRight1 = (vMaxMiddle.x - vMiddleMin.x) / (vMaxMiddle.y - vMiddleMin.y);
714  fDeltaRight2 = fDeltaRight1;
715  fDeltaRight3 = (vMiddleMax.x - vMaxMiddle.x) / (vMiddleMax.y - vMaxMiddle.y);
716  }
717  else
718  {
719  nMiddleY1 = (int)vMaxMiddle.y;
720  nMiddleY2 = (int)vMinMiddle.y;
721  fDeltaLeft1 = (vMinMiddle.x - vMiddleMin.x) / (vMinMiddle.y - vMiddleMin.y);
722  fDeltaLeft2 = fDeltaLeft1;
723  fDeltaLeft3 = (vMiddleMax.x - vMinMiddle.x) / (vMiddleMax.y - vMinMiddle.y);
724  fDeltaRight1 = (vMaxMiddle.x - vMiddleMin.x) / (vMaxMiddle.y - vMiddleMin.y);
725  fDeltaRight2 = (vMiddleMax.x - vMaxMiddle.x) / (vMiddleMax.y - vMaxMiddle.y);
726  fDeltaRight3 = fDeltaRight2;
727  }
728 
729  float fLeft = vMiddleMin.x;
730  float fRight = vMiddleMin.x;
731 
732  // go from start to first corner
733  for (int i = nStartY; i < nMiddleY1; i++)
734  {
735  for (int j = (int)fLeft; j <= (int)fRight; j++)
736  {
737  nSum += pForegroundImage->pixels[i * OLP_IMG_WIDTH + j];
738  }
739 
740  fLeft += fDeltaLeft1;
741  fRight += fDeltaRight1;
742  }
743 
744  // first corner to second corner
745  for (int i = nMiddleY1; i < nMiddleY2; i++)
746  {
747  for (int j = (int)fLeft; j <= (int)fRight; j++)
748  {
749  nSum += pForegroundImage->pixels[i * OLP_IMG_WIDTH + j];
750  }
751 
752  fLeft += fDeltaLeft2;
753  fRight += fDeltaRight2;
754  }
755 
756  // second corner to end
757  for (int i = nMiddleY2; i < nEndY; i++)
758  {
759  for (int j = (int)fLeft; j <= (int)fRight; j++)
760  {
761  nSum += pForegroundImage->pixels[i * OLP_IMG_WIDTH + j];
762  }
763 
764  fLeft += fDeltaLeft3;
765  fRight += fDeltaRight3;
766  }
767 
768  return nSum / 255;
769  }
770 
771  void
772  GetMeanAndVariance(const std::vector<CHypothesisPoint*>& pHypothesisPoints,
773  Vec3d& vMean,
774  float& fVariance)
775  {
776  std::vector<Vec3d> aPoints3d;
777 
778  for (size_t i = 0; i < pHypothesisPoints.size(); i++)
779  {
780  aPoints3d.push_back(pHypothesisPoints.at(i)->vPosition);
781  }
782 
783  GetMeanAndVariance(aPoints3d, vMean, fVariance);
784  }
785 
786  void
787  GetMeanAndVariance(const std::vector<Vec3d>& aPoints, Vec3d& vMean, float& fVariance)
788  {
789  vMean.x = 0;
790  vMean.y = 0;
791  vMean.z = 0;
792  fVariance = 0;
793 
794  const int nNumPoints = aPoints.size();
795 
796  if (nNumPoints > 0)
797  {
798  const float fNumPointsInv = 1.0f / (float)nNumPoints;
799 
800  // calculate mean
801  for (int i = 0; i < nNumPoints; i++)
802  {
803  vMean.x += aPoints.at(i).x;
804  vMean.y += aPoints.at(i).y;
805  vMean.z += aPoints.at(i).z;
806  }
807 
808  vMean.x *= fNumPointsInv;
809  vMean.y *= fNumPointsInv;
810  vMean.z *= fNumPointsInv;
811 
812  // calculate variance
813  for (int i = 0; i < nNumPoints; i++)
814  {
815  fVariance += (vMean.x - aPoints.at(i).x) * (vMean.x - aPoints.at(i).x) +
816  (vMean.y - aPoints.at(i).y) * (vMean.y - aPoints.at(i).y) +
817  (vMean.z - aPoints.at(i).z) * (vMean.z - aPoints.at(i).z);
818  }
819 
820  fVariance *= fNumPointsInv;
821  }
822  else
823  {
824  ARMARX_VERBOSE_S << "GetMeanAndVariance: no points in array!";
825  }
826  }
827 
828  void
829  RemoveOutliers(std::vector<Vec3d>& aPoints,
830  const float fStdDevFactor,
831  std::vector<int>* pOldIndices)
832  {
833  if (aPoints.size() < 9)
834  {
835  if (pOldIndices != NULL)
836  {
837  pOldIndices->clear();
838 
839  for (size_t i = 0; i < aPoints.size(); i++)
840  {
841  pOldIndices->push_back(i);
842  }
843  }
844 
845  return;
846  }
847 
848  Vec3d vMean;
849  float fVariance;
850  GetMeanAndVariance(aPoints, vMean, fVariance);
851  const float fThreshold2 = fStdDevFactor * fStdDevFactor * fVariance;
852 
853  ARMARX_VERBOSE_S << "Mean: (" << vMean.x << ", " << vMean.y << ", " << vMean.z
854  << "), std dev: " << sqrtf(fVariance);
855 
856  // remove all points that are further away from the mean than fStdDevFactor x standard deviation
857  float fDist2;
858 
859  if (pOldIndices != NULL)
860  {
861  pOldIndices->clear();
862 
863  for (size_t i = 0; i < aPoints.size(); i++)
864  {
865  pOldIndices->push_back(i);
866  }
867 
868  for (size_t i = 0; i < aPoints.size(); i++)
869  {
870  fDist2 = (vMean.x - aPoints.at(i).x) * (vMean.x - aPoints.at(i).x) +
871  (vMean.y - aPoints.at(i).y) * (vMean.y - aPoints.at(i).y) +
872  (vMean.z - aPoints.at(i).z) * (vMean.z - aPoints.at(i).z);
873 
874  if (fDist2 > fThreshold2)
875  {
876  Math3d::SetVec(aPoints.at(i), aPoints.at(aPoints.size() - 1));
877  pOldIndices->at(i) = pOldIndices->at(aPoints.size() - 1);
878  aPoints.pop_back();
879  pOldIndices->pop_back();
880  i--;
881  }
882  }
883  }
884  else
885  {
886  for (size_t i = 0; i < aPoints.size(); i++)
887  {
888  fDist2 = (vMean.x - aPoints.at(i).x) * (vMean.x - aPoints.at(i).x) +
889  (vMean.y - aPoints.at(i).y) * (vMean.y - aPoints.at(i).y) +
890  (vMean.z - aPoints.at(i).z) * (vMean.z - aPoints.at(i).z);
891 
892  if (fDist2 > fThreshold2)
893  {
894  aPoints[i] = aPoints[aPoints.size() - 1];
895  aPoints.pop_back();
896  i--;
897  }
898  }
899  }
900  }
901 
902  void
903  RemoveOutliers(std::vector<CHypothesisPoint*>& aPoints, const float fStdDevFactor)
904  {
905  std::vector<Vec3d> aPointsVec3d;
906  std::vector<int> pOldIndices;
907 
908  for (size_t i = 0; i < aPoints.size(); i++)
909  {
910  aPointsVec3d.push_back(aPoints.at(i)->vPosition);
911  }
912 
913  RemoveOutliers(aPointsVec3d, fStdDevFactor, &pOldIndices);
914 
915  ARMARX_VERBOSE_S << "RemoveOutliers: " << pOldIndices.size() << " of " << aPoints.size()
916  << " points left\n";
917 
918  // keep only the points with index contained in pOldIndices
919 
920  std::vector<CHypothesisPoint*> aPointsCopy;
921  std::vector<bool> aPointStillIncluded;
922 
923  for (size_t i = 0; i < aPoints.size(); i++)
924  {
925  aPointsCopy.push_back(aPoints.at(i));
926  aPointStillIncluded.push_back(false);
927  }
928 
929  for (size_t i = 0; i < pOldIndices.size(); i++)
930  {
931  aPointStillIncluded.at(pOldIndices.at(i)) = true;
932  }
933 
934  aPoints.clear();
935 
936  for (size_t i = 0; i < aPointsCopy.size(); i++)
937  {
938  if (aPointStillIncluded.at(i))
939  {
940  aPoints.push_back(aPointsCopy.at(i));
941  }
942  else
943  {
944  delete aPointsCopy.at(i);
945  }
946  }
947  }
948 
949  bool
950  CheckIfPointIsAlreadyInHypothesis(const Vec3d vPoint, const CObjectHypothesis* pHypothesis)
951  {
952  float fMinDist = FLT_MAX;
953  float fDist;
954 
955  for (size_t i = 0; i < pHypothesis->aConfirmedPoints.size(); i++)
956  {
957  fDist = Math3d::Distance(vPoint, pHypothesis->aConfirmedPoints.at(i)->vPosition);
958 
959  if (fDist < fMinDist)
960  {
961  fMinDist = fDist;
962  }
963  }
964 
965  for (size_t i = 0; i < pHypothesis->aDoubtablePoints.size(); i++)
966  {
967  fDist = Math3d::Distance(vPoint, pHypothesis->aDoubtablePoints.at(i)->vPosition);
968 
969  if (fDist < fMinDist)
970  {
971  fMinDist = fDist;
972  }
973  }
974 
975  return (fMinDist < 0.1f * OLP_TOLERANCE_CONCURRENT_MOTION);
976  }
977 
978  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr
979  ConfirmedPointsToPCL(const CObjectHypothesis* pHypothesis)
980  {
981  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGBA>);
982  cloud->resize(pHypothesis->aConfirmedPoints.size());
983 
984  Vec3d vCenter = pHypothesis->vCenter;
985 
986  for (size_t i = 0; i < pHypothesis->aConfirmedPoints.size(); i++)
987  {
988  const CHypothesisPoint* p = pHypothesis->aConfirmedPoints.at(i);
989 
990  cloud->points[i].x = p->vPosition.x - vCenter.x;
991  cloud->points[i].y = p->vPosition.y - vCenter.y;
992  cloud->points[i].z = p->vPosition.z - vCenter.z;
993  cloud->points[i].r = (int)(255.0f * p->fIntensity * p->fColorR);
994  cloud->points[i].g = (int)(255.0f * p->fIntensity * p->fColorG);
995  cloud->points[i].b = (int)(255.0f * p->fIntensity * p->fColorB);
996  cloud->points[i].a = 1;
997  }
998 
999  return cloud;
1000  }
1001 
1002  // stolen from IVT: ImageProcessor
1003  // (1 << 20) / i
1004  static const int division_table[] = {
1005  0, 1048576, 524288, 349525, 262144, 209715, 174762, 149796, 131072, 116508, 104857,
1006  95325, 87381, 80659, 74898, 69905, 65536, 61680, 58254, 55188, 52428, 49932,
1007  47662, 45590, 43690, 41943, 40329, 38836, 37449, 36157, 34952, 33825, 32768,
1008  31775, 30840, 29959, 29127, 28339, 27594, 26886, 26214, 25575, 24966, 24385,
1009  23831, 23301, 22795, 22310, 21845, 21399, 20971, 20560, 20164, 19784, 19418,
1010  19065, 18724, 18396, 18078, 17772, 17476, 17189, 16912, 16644, 16384, 16131,
1011  15887, 15650, 15420, 15196, 14979, 14768, 14563, 14364, 14169, 13981, 13797,
1012  13617, 13443, 13273, 13107, 12945, 12787, 12633, 12483, 12336, 12192, 12052,
1013  11915, 11781, 11650, 11522, 11397, 11275, 11155, 11037, 10922, 10810, 10699,
1014  10591, 10485, 10381, 10280, 10180, 10082, 9986, 9892, 9799, 9709, 9619,
1015  9532, 9446, 9362, 9279, 9198, 9118, 9039, 8962, 8886, 8811, 8738,
1016  8665, 8594, 8525, 8456, 8388, 8322, 8256, 8192, 8128, 8065, 8004,
1017  7943, 7884, 7825, 7767, 7710, 7653, 7598, 7543, 7489, 7436, 7384,
1018  7332, 7281, 7231, 7182, 7133, 7084, 7037, 6990, 6944, 6898, 6853,
1019  6808, 6765, 6721, 6678, 6636, 6594, 6553, 6512, 6472, 6432, 6393,
1020  6355, 6316, 6278, 6241, 6204, 6168, 6132, 6096, 6061, 6026, 5991,
1021  5957, 5924, 5890, 5857, 5825, 5793, 5761, 5729, 5698, 5667, 5637,
1022  5607, 5577, 5548, 5518, 5489, 5461, 5433, 5405, 5377, 5349, 5322,
1023  5295, 5269, 5242, 5216, 5190, 5165, 5140, 5115, 5090, 5065, 5041,
1024  5017, 4993, 4969, 4946, 4922, 4899, 4877, 4854, 4832, 4809, 4788,
1025  4766, 4744, 4723, 4702, 4681, 4660, 4639, 4619, 4599, 4578, 4559,
1026  4539, 4519, 4500, 4481, 4462, 4443, 4424, 4405, 4387, 4369, 4350,
1027  4332, 4315, 4297, 4279, 4262, 4245, 4228, 4211, 4194, 4177, 4161,
1028  4144, 4128, 4112};
1029 
1030  void
1031  ConvertRGB2HSV(const unsigned char r,
1032  const unsigned char g,
1033  const unsigned char b,
1034  unsigned char& h,
1035  unsigned char& s,
1036  unsigned char& v)
1037  {
1038  // stolen from IVT: ImageProcessor
1039  const int max = MY_MAX(MY_MAX(r, g), b);
1040  const int min = MY_MIN(MY_MIN(r, g), b);
1041  const int delta = max - min;
1042 
1043  // unoptimized: 30 * (g - b) / delta (etc.)
1044  int hInt;
1045 
1046  if (r == max)
1047  {
1048  hInt = g > b ? 180 + ((30 * (g - b) * division_table[delta]) >> 20)
1049  : 180 - ((30 * (b - g) * division_table[delta]) >> 20);
1050  }
1051  else if (g == max)
1052  {
1053  hInt = b > r ? 60 + ((30 * (b - r) * division_table[delta]) >> 20)
1054  : 60 - ((30 * (r - b) * division_table[delta]) >> 20);
1055  }
1056  else
1057  {
1058  hInt = r > g ? 120 + ((30 * (r - g) * division_table[delta]) >> 20)
1059  : 120 - ((30 * (g - r) * division_table[delta]) >> 20);
1060  }
1061 
1062  h = (hInt >= 180) ? hInt - 180 : hInt;
1063 
1064  // unoptimized: delta * 255 / max;
1065  s = (255 * delta * division_table[max]) >> 20;
1066 
1067  v = max;
1068  }
1069 
1070  void
1071  CreateHueAndSaturationHistogram(const CObjectHypothesis* pHypothesis,
1072  std::vector<float>& aHueHistogram,
1073  std::vector<float>& aSaturationHistogram)
1074  {
1075  if ((pHypothesis->eType != CObjectHypothesis::eRGBD) &&
1076  (pHypothesis->eType != CObjectHypothesis::eSingleColored) && false)
1077  {
1078  ARMARX_WARNING_S
1079  << "CreateHueHistogram: wrong hypothesis type (not RGBD or SingleColored)!";
1080  return;
1081  }
1082 
1083  aHueHistogram.resize(OLP_SIZE_OBJECT_HUE_HISTOGRAM);
1084  aSaturationHistogram.resize(OLP_SIZE_OBJECT_HUE_HISTOGRAM);
1085  const int nBucketSize = (int)(ceil(256.0f / (float)OLP_SIZE_OBJECT_HUE_HISTOGRAM));
1086 
1087  unsigned char r, g, b, h, s, v;
1088 
1089  // add the values for the confirmed points
1090  for (size_t i = 0; i < pHypothesis->aConfirmedPoints.size(); i++)
1091  {
1092  CHypothesisPoint* pPoint = pHypothesis->aConfirmedPoints.at(i);
1093  r = (unsigned char)(255.0f * pPoint->fColorR * pPoint->fIntensity);
1094  g = (unsigned char)(255.0f * pPoint->fColorG * pPoint->fIntensity);
1095  b = (unsigned char)(255.0f * pPoint->fColorB * pPoint->fIntensity);
1096  COLPTools::ConvertRGB2HSV(r, g, b, h, s, v);
1097 
1098  // saturation-weighted hue values
1099  const float fWeight = 1.0f - 1.0f / (1.0f + 0.0025f * (s * s)); // 0.5 when s=20
1100  const int nHistogramIndexHue = h / nBucketSize;
1101  aHueHistogram.at(nHistogramIndexHue) += fWeight;
1102  const int nHistogramIndexSaturation = s / nBucketSize;
1103  aSaturationHistogram.at(nHistogramIndexSaturation) += 1;
1104  }
1105 
1106  // add the values for the candidate points
1107  const float fCandidatePointWeightFactor = 0.01f; // smaller weight for candidate points
1108 
1109  for (size_t i = 0; i < pHypothesis->aNewPoints.size(); i++)
1110  {
1111  CHypothesisPoint* pPoint = pHypothesis->aNewPoints.at(i);
1112  r = (unsigned char)(255.0f * pPoint->fColorR * pPoint->fIntensity);
1113  g = (unsigned char)(255.0f * pPoint->fColorG * pPoint->fIntensity);
1114  b = (unsigned char)(255.0f * pPoint->fColorB * pPoint->fIntensity);
1115  COLPTools::ConvertRGB2HSV(r, g, b, h, s, v);
1116 
1117  // saturation-weighted hue values
1118  const float fWeight = 1.0f - 1.0f / (1.0f + 0.0025f * (s * s));
1119  const int nHistogramIndexHue = h / nBucketSize;
1120  aHueHistogram.at(nHistogramIndexHue) += fCandidatePointWeightFactor * fWeight;
1121  const int nHistogramIndexSaturation = s / nBucketSize;
1122  aSaturationHistogram.at(nHistogramIndexSaturation) += fCandidatePointWeightFactor;
1123  }
1124 
1125  SmoothHistogram(aHueHistogram);
1126  NormalizeHistogram(aHueHistogram);
1127  SmoothHistogram(aSaturationHistogram);
1128  NormalizeHistogram(aSaturationHistogram);
1129  }
1130 
1131  void
1132  CreateHueAndSaturationHistogramInWindow(const CByteImage* pHSVImage,
1133  const int nMinX,
1134  const int nMinY,
1135  const int nMaxX,
1136  const int nMaxY,
1137  std::vector<float>& aHueHistogram,
1138  std::vector<float>& aSaturationHistogram)
1139  {
1140  aHueHistogram.resize(OLP_SIZE_OBJECT_HUE_HISTOGRAM);
1141  aSaturationHistogram.resize(OLP_SIZE_OBJECT_HUE_HISTOGRAM);
1142  const int nBucketSize = (int)(ceil(256.0f / (float)OLP_SIZE_OBJECT_HUE_HISTOGRAM));
1143 
1144  // accumulate histogram
1145  unsigned char h, s;
1146 
1147  for (int i = nMinY; i < nMaxY; i++)
1148  {
1149  for (int j = nMinX; j < nMaxX; j++)
1150  {
1151  h = pHSVImage->pixels[3 * (i * OLP_IMG_WIDTH + j)];
1152  s = pHSVImage->pixels[3 * (i * OLP_IMG_WIDTH + j) + 1];
1153  // saturation-weighted hue values
1154  const float fWeight = 1.0f - 1.0f / (1.0f + 0.0025f * (s * s)); // 0.5 when s=20
1155  const int nHistogramIndexHue = h / nBucketSize;
1156  aHueHistogram.at(nHistogramIndexHue) += fWeight;
1157  const int nHistogramIndexSaturation = s / nBucketSize;
1158  aSaturationHistogram.at(nHistogramIndexSaturation) += 1;
1159  }
1160  }
1161 
1162  SmoothHistogram(aHueHistogram);
1163  NormalizeHistogram(aHueHistogram);
1164  SmoothHistogram(aSaturationHistogram);
1165  NormalizeHistogram(aSaturationHistogram);
1166  }
1167 
1168  void
1169  SmoothHistogram(std::vector<float>& aHistogram)
1170  {
1171  std::vector<float> aHistogramTemp;
1172  const int nHistogramSize = aHistogram.size();
1173  aHistogramTemp.resize(nHistogramSize);
1174 
1175  for (int i = 0; i < nHistogramSize; i++)
1176  {
1177  aHistogramTemp.at(i) = aHistogram.at(i);
1178  }
1179 
1180  // smooth the histogram
1181  for (int i = 0; i < nHistogramSize; i++)
1182  {
1183  // aHistogram.at(i) = 0.4f * aHistogramTemp.at(i)
1184  // + 0.2f * aHistogramTemp.at((i-1+nHistogramSize)%nHistogramSize)
1185  // + 0.2f * aHistogramTemp.at((i+1)%nHistogramSize)
1186  // + 0.1f * aHistogramTemp.at((i-2+nHistogramSize)%nHistogramSize)
1187  // + 0.1f * aHistogramTemp.at((i+2)%nHistogramSize);
1188  aHistogram.at(i) =
1189  0.7f * aHistogramTemp.at(i) +
1190  0.15f * aHistogramTemp.at((i - 1 + nHistogramSize) % nHistogramSize) +
1191  0.15f * aHistogramTemp.at((i + 1) % nHistogramSize);
1192  }
1193  }
1194 
1195  void
1196  NormalizeHistogram(std::vector<float>& aHistogram)
1197  {
1198  float fSum = 0;
1199 
1200  for (size_t i = 0; i < aHistogram.size(); i++)
1201  {
1202  fSum += aHistogram.at(i);
1203  }
1204 
1205  const float fInverseSum = 1.0f / fSum;
1206 
1207  for (size_t i = 0; i < aHistogram.size(); i++)
1208  {
1209  aHistogram.at(i) *= fInverseSum;
1210  }
1211  }
1212 
1213  float
1214  GetHistogramDistanceL1(const std::vector<float>& aHistogram1,
1215  const std::vector<float>& aHistogram2)
1216  {
1217  const int n = (int)aHistogram1.size();
1218 
1219  float fDist = 0;
1220 
1221  for (int i = 0; i < n; i++)
1222  {
1223  fDist += fabsf(aHistogram1.at(i) - aHistogram2.at(i));
1224  }
1225 
1226  return fDist;
1227  }
1228 
1229  float
1230  GetHistogramDistanceL2(const std::vector<float>& aHistogram1,
1231  const std::vector<float>& aHistogram2)
1232  {
1233  const int n = (int)aHistogram1.size();
1234 
1235  float fDist = 0;
1236 
1237  for (int i = 0; i < n; i++)
1238  {
1239  fDist +=
1240  (aHistogram1.at(i) - aHistogram2.at(i)) * (aHistogram1.at(i) - aHistogram2.at(i));
1241  }
1242 
1243  return sqrtf(fDist);
1244  }
1245 
1246  float
1247  GetHistogramDistanceX2(const std::vector<float>& aHistogram1,
1248  const std::vector<float>& aHistogram2)
1249  {
1250  const int n = (int)aHistogram1.size();
1251 
1252  float fDist = 0;
1253 
1254  for (int i = 0; i < n; i++)
1255  {
1256  fDist += (aHistogram1.at(i) - aHistogram2.at(i)) *
1257  (aHistogram1.at(i) - aHistogram2.at(i)) /
1258  (aHistogram1.at(i) + aHistogram2.at(i) + 0.00001f);
1259  }
1260 
1261  return sqrtf(fDist);
1262  }
1263 
1264  void
1265  DrawCross(CByteImage* pGreyImage, int x, int y, int nBrightness)
1266  {
1267  if (x > 2 && y > 2 && x < OLP_IMG_WIDTH - 3 && y < OLP_IMG_HEIGHT - 3)
1268  {
1269  for (int j = -3; j <= 3; j++)
1270  {
1271  pGreyImage->pixels[(y + j) * OLP_IMG_WIDTH + x] = nBrightness;
1272  pGreyImage->pixels[y * OLP_IMG_WIDTH + x + j] = nBrightness;
1273  }
1274  }
1275  }
1276 
1277  void
1278  DrawCross(CByteImage* pColorImage, int x, int y, int r, int g, int b)
1279  {
1280  if (x > 2 && y > 2 && x < OLP_IMG_WIDTH - 3 && y < OLP_IMG_HEIGHT - 3)
1281  {
1282  for (int j = -3; j <= 3; j++)
1283  {
1284  pColorImage->pixels[3 * ((y + j) * OLP_IMG_WIDTH + x)] = r;
1285  pColorImage->pixels[3 * ((y + j) * OLP_IMG_WIDTH + x) + 1] = g;
1286  pColorImage->pixels[3 * ((y + j) * OLP_IMG_WIDTH + x) + 2] = b;
1287 
1288  pColorImage->pixels[3 * (y * OLP_IMG_WIDTH + x + j)] = r;
1289  pColorImage->pixels[3 * (y * OLP_IMG_WIDTH + x + j) + 1] = g;
1290  pColorImage->pixels[3 * (y * OLP_IMG_WIDTH + x + j) + 2] = b;
1291  }
1292  }
1293  }
1294 
1295  void
1296  DrawFatCross(CByteImage* pColorImage, int x, int y, int r, int g, int b)
1297  {
1298  if (x > 2 && y > 2 && x < OLP_IMG_WIDTH - 5 && y < OLP_IMG_HEIGHT - 5)
1299  {
1300  for (int j = -3; j <= 4; j++)
1301  {
1302  pColorImage->pixels[3 * ((y + j) * OLP_IMG_WIDTH + x)] = r;
1303  pColorImage->pixels[3 * ((y + j) * OLP_IMG_WIDTH + x) + 1] = g;
1304  pColorImage->pixels[3 * ((y + j) * OLP_IMG_WIDTH + x) + 2] = b;
1305 
1306  pColorImage->pixels[3 * ((y + j) * OLP_IMG_WIDTH + x + 1)] = r;
1307  pColorImage->pixels[3 * ((y + j) * OLP_IMG_WIDTH + x + 1) + 1] = g;
1308  pColorImage->pixels[3 * ((y + j) * OLP_IMG_WIDTH + x + 1) + 2] = b;
1309 
1310 
1311  pColorImage->pixels[3 * (y * OLP_IMG_WIDTH + x + j)] = r;
1312  pColorImage->pixels[3 * (y * OLP_IMG_WIDTH + x + j) + 1] = g;
1313  pColorImage->pixels[3 * (y * OLP_IMG_WIDTH + x + j) + 2] = b;
1314 
1315  pColorImage->pixels[3 * ((y + 1) * OLP_IMG_WIDTH + x + j)] = r;
1316  pColorImage->pixels[3 * ((y + 1) * OLP_IMG_WIDTH + x + j) + 1] = g;
1317  pColorImage->pixels[3 * ((y + 1) * OLP_IMG_WIDTH + x + j) + 2] = b;
1318  }
1319  }
1320  }
1321 
1322  void
1323  CreateObjectSegmentationMask(const CObjectHypothesis* pHypothesis,
1324  const CCalibration* calibration,
1325  CByteImage*& pForegroundImage)
1326  {
1327  if (pForegroundImage == NULL)
1328  {
1329  pForegroundImage =
1330  new CByteImage(OLP_IMG_WIDTH, OLP_IMG_HEIGHT, CByteImage::eGrayScale);
1331  }
1332  else if (pForegroundImage->width != OLP_IMG_WIDTH ||
1333  pForegroundImage->height != OLP_IMG_HEIGHT ||
1334  pForegroundImage->type != CByteImage::eGrayScale)
1335  {
1336  delete pForegroundImage;
1337  pForegroundImage =
1338  new CByteImage(OLP_IMG_WIDTH, OLP_IMG_HEIGHT, CByteImage::eGrayScale);
1339  }
1340 
1341  // reset pixels
1342  for (int i = 0; i < OLP_IMG_WIDTH * OLP_IMG_HEIGHT; i++)
1343  {
1344  pForegroundImage->pixels[i] = 0;
1345  }
1346 
1347 
1348  // pixels belonging to the hypothesis are foreground
1349  for (size_t i = 0; i < pHypothesis->aVisibleConfirmedPoints.size(); i++)
1350  {
1351  Vec2d vPoint2D;
1352  calibration->WorldToImageCoordinates(
1353  pHypothesis->aVisibleConfirmedPoints.at(i)->vPosition, vPoint2D, false);
1354  int nIndex = (int)vPoint2D.y * OLP_IMG_WIDTH + (int)vPoint2D.x;
1355 
1356  if (0 <= nIndex && nIndex < OLP_IMG_WIDTH * OLP_IMG_HEIGHT)
1357  {
1358  pForegroundImage->pixels[nIndex] = 255;
1359  }
1360  }
1361 
1362  if (pHypothesis->aVisibleConfirmedPoints.size() < OLP_MIN_NUM_FEATURES)
1363  {
1364  for (size_t i = 0; i < pHypothesis->aNewPoints.size(); i++)
1365  {
1366  Vec2d vPoint2D;
1367  calibration->WorldToImageCoordinates(
1368  pHypothesis->aNewPoints.at(i)->vPosition, vPoint2D, false);
1369  int nIndex = (int)vPoint2D.y * OLP_IMG_WIDTH + (int)vPoint2D.x;
1370 
1371  if (0 <= nIndex && nIndex < OLP_IMG_WIDTH * OLP_IMG_HEIGHT)
1372  {
1373  pForegroundImage->pixels[nIndex] = 255;
1374  }
1375  }
1376  }
1377 
1378 
1379  // apply morphological operators to cover the free space between the points and close holes
1380  CByteImage* pTempImage =
1381  new CByteImage(OLP_IMG_WIDTH, OLP_IMG_HEIGHT, CByteImage::eGrayScale);
1382 
1383  for (int i = 0; i < OLP_DEPTH_MAP_PIXEL_DISTANCE + 1; i++)
1384  {
1385  ImageProcessor::Dilate(pForegroundImage, pTempImage);
1386  ImageProcessor::Dilate(pTempImage, pForegroundImage);
1387  }
1388  //ImageProcessor::Dilate(pForegroundImage, pTempImage);
1389  for (int i = 0; i < OLP_DEPTH_MAP_PIXEL_DISTANCE + 1; i++)
1390  {
1391  ImageProcessor::Erode(pForegroundImage, pTempImage);
1392  ImageProcessor::Erode(pTempImage, pForegroundImage);
1393  }
1394  //ImageProcessor::CopyImage(pTempImage, pForegroundImage);
1395 
1396  delete pTempImage;
1397 
1398  //pForegroundImage->SaveToFile("/home/staff/schieben/datalog/objseg.bmp");
1399  }
1400 
1401  void
1402  CreateSegmentationProbabilityMap(const CObjectHypothesis* pHypothesis,
1403  const CCalibration* calibration,
1404  CByteImage*& pProbabilityImage)
1405  {
1406  if (pProbabilityImage == NULL)
1407  {
1408  pProbabilityImage =
1409  new CByteImage(OLP_IMG_WIDTH, OLP_IMG_HEIGHT, CByteImage::eGrayScale);
1410  }
1411 
1412  // reset pixels
1413  for (int i = 0; i < OLP_IMG_WIDTH * OLP_IMG_HEIGHT; i++)
1414  {
1415  pProbabilityImage->pixels[i] = 0;
1416  }
1417 
1418  // pixels belonging to the hypothesis are foreground
1419  const int probValueConfirmedPoints = 255;
1420  const int probValueCandidatePoints = 51;
1421  Vec2d vPoint2D;
1422 
1423  for (size_t i = 0; i < pHypothesis->aNewPoints.size(); i++)
1424  {
1425  calibration->WorldToImageCoordinates(
1426  pHypothesis->aNewPoints.at(i)->vPosition, vPoint2D, false);
1427  int nIndex = (int)vPoint2D.y * OLP_IMG_WIDTH + (int)vPoint2D.x;
1428 
1429  if (0 <= nIndex && nIndex < OLP_IMG_WIDTH * OLP_IMG_HEIGHT)
1430  {
1431  pProbabilityImage->pixels[nIndex] = probValueCandidatePoints;
1432  }
1433  }
1434 
1435  for (size_t i = 0; i < pHypothesis->aVisibleConfirmedPoints.size(); i++)
1436  {
1437  calibration->WorldToImageCoordinates(
1438  pHypothesis->aVisibleConfirmedPoints.at(i)->vPosition, vPoint2D, false);
1439  int nIndex = (int)vPoint2D.y * OLP_IMG_WIDTH + (int)vPoint2D.x;
1440 
1441  if (0 <= nIndex && nIndex < OLP_IMG_WIDTH * OLP_IMG_HEIGHT)
1442  {
1443  pProbabilityImage->pixels[nIndex] = probValueConfirmedPoints;
1444  }
1445  }
1446 
1447  CByteImage* tempImage = new CByteImage(pProbabilityImage);
1448  const float sigma = OLP_DEPTH_MAP_PIXEL_DISTANCE + 1;
1449  const int nKernelSize = 4 * sigma + 1; // >= 4*sigma+1
1450  ImageProcessor::GaussianSmooth(pProbabilityImage, tempImage, sigma * sigma, nKernelSize);
1451  ImageProcessor::HistogramStretching(tempImage, pProbabilityImage, 0.0f, 1.0f);
1452  ImageProcessor::GaussianSmooth(pProbabilityImage, tempImage, sigma * sigma, nKernelSize);
1453  ImageProcessor::HistogramStretching(tempImage, pProbabilityImage, 0.0f, 1.0f);
1454 
1455  // for (int i = 0; i < OLP_IMG_WIDTH * OLP_IMG_HEIGHT; i++)
1456  // {
1457  // pTempImage->pixels[i] = (pTempImage->pixels[i] > 50) ? 255 : 5 * pTempImage->pixels[i];
1458  // }
1459  // ImageProcessor::GaussianSmooth(pTempImage, pProbabilityImage, sigma * sigma, nKernelSize);
1460  delete tempImage;
1461  }
1462 
1463  void
1464  SetNumberInFileName(std::string& sFileName, int nNumber, int nNumDigits)
1465  {
1466  for (int i = 0; i < nNumDigits; i++)
1467  {
1468  int nDecimalDivisor = 1;
1469 
1470  for (int j = 0; j < i; j++)
1471  {
1472  nDecimalDivisor *= 10;
1473  }
1474 
1475  sFileName.at(sFileName.length() - (5 + i)) = '0' + (nNumber / nDecimalDivisor) % 10;
1476  }
1477  }
1478 
1479  float
1480  GetHypothesesIntersectionRatio(const CObjectHypothesis* pHypothesis1,
1481  const CObjectHypothesis* pHypothesis2,
1482  const CCalibration* calibration)
1483  {
1484  CByteImage* pHypothesisRegionImage1 =
1485  new CByteImage(OLP_IMG_WIDTH, OLP_IMG_HEIGHT, CByteImage::eGrayScale);
1486  CByteImage* pHypothesisRegionImage2 =
1487  new CByteImage(OLP_IMG_WIDTH, OLP_IMG_HEIGHT, CByteImage::eGrayScale);
1488  CreateObjectSegmentationMask(pHypothesis1, calibration, pHypothesisRegionImage1);
1489  CreateObjectSegmentationMask(pHypothesis2, calibration, pHypothesisRegionImage2);
1490 
1491  int nIntersectingPoints = 0;
1492  int nUnionPoints = 0;
1493 
1494  for (int i = 0; i < OLP_IMG_WIDTH * OLP_IMG_HEIGHT; i++)
1495  {
1496  nIntersectingPoints +=
1497  (pHypothesisRegionImage1->pixels[i] * pHypothesisRegionImage2->pixels[i] != 0);
1498  nUnionPoints +=
1499  (pHypothesisRegionImage1->pixels[i] + pHypothesisRegionImage2->pixels[i] != 0);
1500  }
1501 
1502  delete pHypothesisRegionImage1;
1503  delete pHypothesisRegionImage2;
1504 
1505  if (nUnionPoints > 0)
1506  {
1507  return (float)nIntersectingPoints / (float)nUnionPoints;
1508  }
1509  else
1510  {
1511  return 0;
1512  }
1513  }
1514 
1515  void
1516  DrawHypothesis(CObjectHypothesis* pHypothesis,
1517  const CCalibration* calibration,
1518  CByteImage* pImage,
1519  const int nNumFusedHypotheses)
1520  {
1521  // reorder points to draw closest ones last
1522  SortByPositionZ(pHypothesis->aVisibleConfirmedPoints);
1523  // invert order to decreasing
1524  CHypothesisPoint* pTempPoint;
1525  const int nSize = pHypothesis->aVisibleConfirmedPoints.size();
1526 
1527  for (int i = 0; 2 * i < nSize; i++)
1528  {
1529  pTempPoint = pHypothesis->aVisibleConfirmedPoints.at(i);
1530  pHypothesis->aVisibleConfirmedPoints.at(i) =
1531  pHypothesis->aVisibleConfirmedPoints.at(nSize - i - 1);
1532  pHypothesis->aVisibleConfirmedPoints.at(nSize - i - 1) = pTempPoint;
1533  }
1534 
1535  ImageProcessor::Zero(pImage);
1536 
1537  for (size_t i = 0; i < pHypothesis->aVisibleConfirmedPoints.size(); i++)
1538  {
1539  Vec2d vPoint2D;
1540  calibration->WorldToImageCoordinates(
1541  pHypothesis->aVisibleConfirmedPoints.at(i)->vPosition, vPoint2D, false);
1542  int nIndex = (int)vPoint2D.y * OLP_IMG_WIDTH + (int)vPoint2D.x;
1543 
1544  if (0 <= nIndex && nIndex < OLP_IMG_WIDTH * OLP_IMG_HEIGHT)
1545  {
1546  pImage->pixels[3 * nIndex] =
1547  255 * pHypothesis->aVisibleConfirmedPoints.at(i)->fIntensity *
1548  pHypothesis->aVisibleConfirmedPoints.at(i)->fColorR;
1549  pImage->pixels[3 * nIndex + 1] =
1550  255 * pHypothesis->aVisibleConfirmedPoints.at(i)->fIntensity *
1551  pHypothesis->aVisibleConfirmedPoints.at(i)->fColorG;
1552  pImage->pixels[3 * nIndex + 2] =
1553  255 * pHypothesis->aVisibleConfirmedPoints.at(i)->fIntensity *
1554  pHypothesis->aVisibleConfirmedPoints.at(i)->fColorB;
1555  }
1556  }
1557 
1558  // fill holes
1559  CByteImage* pTempImg = new CByteImage(pImage);
1560  FillHolesRGB(pImage, pTempImg, 2);
1561  FillHolesRGB(pTempImg, pImage, 2);
1562  delete pTempImg;
1563 
1564  // remove regions with low foreground probability
1565  CByteImage* pSegmentationProbability = NULL;
1566  CreateSegmentationProbabilityMap(pHypothesis, calibration, pSegmentationProbability);
1567  std::string sFileName = OLP_OBJECT_LEARNING_DIR;
1568  sFileName += "SegmProb.bmp";
1569  pSegmentationProbability->SaveToFile(sFileName.c_str());
1570 
1571  for (int i = 0; i < OLP_IMG_WIDTH * OLP_IMG_HEIGHT; i++)
1572  {
1573  if (pSegmentationProbability->pixels[i] < 1 + nNumFusedHypotheses / 3)
1574  {
1575  pImage->pixels[3 * i] *= 0.3;
1576  pImage->pixels[3 * i + 1] *= 0.3;
1577  pImage->pixels[3 * i + 2] *= 0.3;
1578  }
1579  }
1580  }
1581 
1582  void
1583  FillHolesRGB(const CByteImage* pInputImage, CByteImage* pOutputImage, const int nRadius)
1584  {
1585  for (int i = 0; i < 3 * OLP_IMG_WIDTH * OLP_IMG_HEIGHT; i++)
1586  {
1587  pOutputImage->pixels[i] = pInputImage->pixels[i];
1588  }
1589 
1590  for (int i = nRadius; i < OLP_IMG_HEIGHT - nRadius; i++)
1591  {
1592  for (int j = nRadius; j < OLP_IMG_WIDTH - nRadius; j++)
1593  {
1594  int nIndex = i * OLP_IMG_WIDTH + j;
1595 
1596  if (pInputImage->pixels[3 * nIndex] + pInputImage->pixels[3 * nIndex + 1] +
1597  pInputImage->pixels[3 * nIndex + 2] ==
1598  0)
1599  {
1600  int r, g, b, nNumPixels;
1601  r = g = b = nNumPixels = 0;
1602 
1603  for (int l = -nRadius; l <= nRadius; l++)
1604  {
1605  for (int k = -nRadius; k <= nRadius; k++)
1606  {
1607  int nTempIndex = 3 * (nIndex + l * OLP_IMG_WIDTH + k);
1608 
1609  if (pInputImage->pixels[nTempIndex] +
1610  pInputImage->pixels[nTempIndex + 1] +
1611  pInputImage->pixels[nTempIndex + 2] !=
1612  0)
1613  {
1614  r += pInputImage->pixels[nTempIndex];
1615  g += pInputImage->pixels[nTempIndex + 1];
1616  b += pInputImage->pixels[nTempIndex + 2];
1617  nNumPixels++;
1618  }
1619  }
1620  }
1621 
1622  if (nNumPixels > 0)
1623  {
1624  pOutputImage->pixels[3 * nIndex] = r / nNumPixels;
1625  pOutputImage->pixels[3 * nIndex + 1] = g / nNumPixels;
1626  pOutputImage->pixels[3 * nIndex + 2] = b / nNumPixels;
1627  }
1628  }
1629  }
1630  }
1631  }
1632 
1633  void
1634  FillHolesGray(const CByteImage* pInputImage, CByteImage* pOutputImage, const int nRadius)
1635  {
1636  for (int i = 0; i < OLP_IMG_WIDTH * OLP_IMG_HEIGHT; i++)
1637  {
1638  pOutputImage->pixels[i] = pInputImage->pixels[i];
1639  }
1640 
1641  for (int i = nRadius; i < OLP_IMG_HEIGHT - nRadius; i++)
1642  {
1643  for (int j = nRadius; j < OLP_IMG_WIDTH - nRadius; j++)
1644  {
1645  int nIndex = i * OLP_IMG_WIDTH + j;
1646 
1647  if (pInputImage->pixels[nIndex] == 0)
1648  {
1649  int nSum = 0;
1650  int nNumPixels = 0;
1651 
1652  for (int l = -nRadius; l <= nRadius; l++)
1653  {
1654  for (int k = -nRadius; k <= nRadius; k++)
1655  {
1656  int nTempIndex = nIndex + l * OLP_IMG_WIDTH + k;
1657 
1658  if (pInputImage->pixels[nTempIndex] != 0)
1659  {
1660  nSum += pInputImage->pixels[nTempIndex];
1661  nNumPixels++;
1662  }
1663  }
1664  }
1665 
1666  if (nNumPixels > 0)
1667  {
1668  pOutputImage->pixels[nIndex] = nSum / nNumPixels;
1669  }
1670  }
1671  }
1672  }
1673  }
1674 
1675  // Quicksort from slide of Prof. Sanders
1676  void
1677  SortByPositionZ(std::vector<CHypothesisPoint*>& aHypothesisPoints,
1678  const int nLeftLimit,
1679  const int nRightLimit)
1680  {
1681  if (aHypothesisPoints.size() <= 1 || nRightLimit - nLeftLimit <= 0)
1682  {
1683  return;
1684  }
1685 
1686  float fPivot = aHypothesisPoints.at((nLeftLimit + nRightLimit) / 2)->vPosition.z;
1687 
1688  int nLeftIndex = nLeftLimit;
1689  int nRightIndex = nRightLimit;
1690 
1691  while (nLeftIndex <= nRightIndex)
1692  {
1693  while (aHypothesisPoints.at(nLeftIndex)->vPosition.z < fPivot &&
1694  (nLeftIndex <= nRightIndex))
1695  {
1696  nLeftIndex++;
1697  }
1698 
1699  while (aHypothesisPoints.at(nRightIndex)->vPosition.z > fPivot &&
1700  (nLeftIndex <= nRightIndex))
1701  {
1702  nRightIndex--;
1703  }
1704 
1705  if (nLeftIndex <= nRightIndex)
1706  {
1707  CHypothesisPoint* pTemp = aHypothesisPoints.at(nLeftIndex);
1708  aHypothesisPoints.at(nLeftIndex) = aHypothesisPoints.at(nRightIndex);
1709  aHypothesisPoints.at(nRightIndex) = pTemp;
1710  nLeftIndex++;
1711  nRightIndex--;
1712  }
1713  }
1714 
1715  SortByPositionZ(aHypothesisPoints, nLeftLimit, nRightIndex);
1716  SortByPositionZ(aHypothesisPoints, nLeftIndex, nRightLimit);
1717  }
1718 
1719  void
1720  SortByPositionZ(std::vector<CHypothesisPoint*>& aHypothesisPoints)
1721  {
1722  SortByPositionZ(aHypothesisPoints, 0, aHypothesisPoints.size() - 1);
1723  }
1724 
1725  void
1726  CalculateEdgeImageFromImageAndDisparity(CByteImage* pImageGray,
1727  CByteImage* pDisparity,
1728  CByteImage* pEdgeImage)
1729  {
1730  // edges from the image
1731  CByteImage* pTempImage = new CByteImage(pImageGray);
1732  float pEdgesFromImage[OLP_IMG_WIDTH * OLP_IMG_HEIGHT];
1733  ImageProcessor::GaussianSmooth5x5(pImageGray, pTempImage);
1734  ImageProcessor::CalculateGradientImageSobel(pTempImage, pEdgeImage);
1735 
1736  for (int i = 0; i < OLP_IMG_WIDTH * OLP_IMG_HEIGHT; i++)
1737  {
1738  pEdgesFromImage[i] = pEdgeImage->pixels[i];
1739  }
1740 
1741  std::string sScreenshotFile = OLP_SCREENSHOT_PATH;
1742  sScreenshotFile += "edge-img.bmp";
1743  pEdgeImage->SaveToFile(sScreenshotFile.c_str());
1744 
1745 
1746  // edges from disparity
1747  float pEdgesFromDisparity[OLP_IMG_WIDTH * OLP_IMG_HEIGHT];
1748  // count number of disparity pixels that are zero to allow histogram stretching
1749  int nNumZeroDisparityPixels = 0;
1750 
1751  for (int i = 0; i < OLP_IMG_WIDTH * OLP_IMG_HEIGHT; i++)
1752  {
1753  if (pDisparity->pixels[i] == 0)
1754  {
1755  nNumZeroDisparityPixels++;
1756  }
1757  }
1758 
1759  float fZeroDisparityPixelRatio =
1760  (float)nNumZeroDisparityPixels / (float)(OLP_IMG_WIDTH * OLP_IMG_HEIGHT);
1761  ImageProcessor::HistogramStretching(
1762  pDisparity, pTempImage, fZeroDisparityPixelRatio + 0.1f, 0.9f);
1763  sScreenshotFile = OLP_SCREENSHOT_PATH;
1764  sScreenshotFile += "disp.bmp";
1765  pTempImage->SaveToFile(sScreenshotFile.c_str());
1766 
1767  const float fSigma = 7;
1768  const int nKernelSize = 4 * fSigma + 1; // >= 4*fSigma+1
1769  ImageProcessor::GaussianSmooth(pTempImage, pEdgeImage, fSigma * fSigma, nKernelSize);
1770 
1771  ImageProcessor::CalculateGradientImageSobel(pEdgeImage, pTempImage);
1772 
1773  for (int i = 0; i < OLP_IMG_WIDTH * OLP_IMG_HEIGHT; i++)
1774  {
1775  pEdgesFromDisparity[i] = pTempImage->pixels[i];
1776  }
1777 
1778  sScreenshotFile = OLP_SCREENSHOT_PATH;
1779  sScreenshotFile += "edge-disp.bmp";
1780  pTempImage->SaveToFile(sScreenshotFile.c_str());
1781 
1782 
1783  // combine edges
1784  float fValue;
1785 
1786  for (int i = 0; i < OLP_IMG_WIDTH * OLP_IMG_HEIGHT; i++)
1787  {
1788  fValue =
1789  sqrtf(pEdgesFromImage[i] * pEdgesFromDisparity[i] * sqrtf(pEdgesFromDisparity[i]));
1790  pEdgeImage->pixels[i] = (fValue > 255) ? 255 : fValue;
1791  }
1792 
1793  sScreenshotFile = OLP_SCREENSHOT_PATH;
1794  sScreenshotFile += "edge-full.bmp";
1795  pEdgeImage->SaveToFile(sScreenshotFile.c_str());
1796  }
1797 
1798 } // namespace COLPTools
OLP_IMG_HEIGHT
#define OLP_IMG_HEIGHT
Definition: ObjectLearningByPushingDefinitions.h:69
COLPTools::InterpolateRotation
void InterpolateRotation(const Mat3d m1, const Mat3d m2, const float fWeight, Mat3d &mResult)
Definition: OLPTools.cpp:43
GfxTL::Vec3d
VectorXD< 3, double > Vec3d
Definition: VectorXD.h:737
COLPTools::GetMeanAndVariance
void GetMeanAndVariance(const std::vector< CHypothesisPoint * > &pHypothesisPoints, Vec3d &vMean, float &fVariance)
Definition: OLPTools.cpp:772
OLP_IMG_WIDTH
#define OLP_IMG_WIDTH
Definition: ObjectLearningByPushingDefinitions.h:68
COLPTools::CreateObjectSegmentationMask
void CreateObjectSegmentationMask(const CObjectHypothesis *pHypothesis, const CCalibration *calibration, CByteImage *&pForegroundImage)
Definition: OLPTools.cpp:1323
ObjectHypothesis.h
OLP_DEPTH_MAP_PIXEL_DISTANCE
#define OLP_DEPTH_MAP_PIXEL_DISTANCE
Definition: ObjectLearningByPushingDefinitions.h:269
CHypothesisPoint::fIntensity
float fIntensity
Definition: ObjectHypothesis.h:238
COLPTools::GetHistogramDistanceX2
float GetHistogramDistanceX2(const std::vector< float > &aHistogram1, const std::vector< float > &aHistogram2)
Definition: OLPTools.cpp:1247
COLPTools::SortByPositionZ
void SortByPositionZ(std::vector< CHypothesisPoint * > &aHypothesisPoints, const int nLeftLimit, const int nRightLimit)
Definition: OLPTools.cpp:1677
COLPTools::GetHistogramDistanceL2
float GetHistogramDistanceL2(const std::vector< float > &aHistogram1, const std::vector< float > &aHistogram2)
Definition: OLPTools.cpp:1230
COLPTools::GetHypothesesIntersectionRatio
float GetHypothesesIntersectionRatio(const CObjectHypothesis *pHypothesis1, const CObjectHypothesis *pHypothesis2, const CCalibration *calibration)
Definition: OLPTools.cpp:1480
COLPTools::SetNumberInFileName
void SetNumberInFileName(std::string &sFileName, int nNumber, int nNumDigits)
Definition: OLPTools.cpp:1464
COLPTools::DrawCross
void DrawCross(CByteImage *pGreyImage, int x, int y, int nBrightness)
Definition: OLPTools.cpp:1265
CObjectHypothesis::eType
eObjectType eType
Definition: ObjectHypothesis.h:292
CHypothesisPoint::fColorG
float fColorG
Definition: ObjectHypothesis.h:238
COLPTools::GetHistogramDistanceL1
float GetHistogramDistanceL1(const std::vector< float > &aHistogram1, const std::vector< float > &aHistogram2)
Definition: OLPTools.cpp:1214
COLPTools::CalculateForegroundRatioOfHypothesis
void CalculateForegroundRatioOfHypothesis(const CObjectHypothesis *pHypothesis, const CByteImage *pForegroundImage, const CCalibration *calibration, float &fForegroundRatio, int &nNumForegroundPixels)
Definition: OLPTools.cpp:383
COLPTools::CreateHueAndSaturationHistogramInWindow
void CreateHueAndSaturationHistogramInWindow(const CByteImage *pHSVImage, const int nMinX, const int nMinY, const int nMaxX, const int nMaxY, std::vector< float > &aHueHistogram, std::vector< float > &aSaturationHistogram)
Definition: OLPTools.cpp:1132
CHypothesisPoint::vPosition
Vec3d vPosition
Definition: ObjectHypothesis.h:235
COLPTools::RemoveOutliers
void RemoveOutliers(std::vector< Vec3d > &aPoints, const float fStdDevFactor, std::vector< int > *pOldIndices)
Definition: OLPTools.cpp:829
COLPTools::InterpolateTransformation
void InterpolateTransformation(const Mat3d m1, const Vec3d v1, const Mat3d m2, const Vec3d v2, const float fWeight, Mat3d &mResult, Vec3d &vResult)
Definition: OLPTools.cpp:64
COLPTools::ConfirmedPointsToPCL
pcl::PointCloud< pcl::PointXYZRGBA >::Ptr ConfirmedPointsToPCL(const CObjectHypothesis *pHypothesis)
Definition: OLPTools.cpp:979
COLPTools::NormalizeHistogram
void NormalizeHistogram(std::vector< float > &aHistogram)
Definition: OLPTools.cpp:1196
CObjectHypothesis::eRGBD
@ eRGBD
Definition: ObjectHypothesis.h:287
COLPTools::FillHolesGray
void FillHolesGray(const CByteImage *pInputImage, CByteImage *pOutputImage, const int nRadius)
Definition: OLPTools.cpp:1634
COLPTools::DrawFatCross
void DrawFatCross(CByteImage *pColorImage, int x, int y, int r, int g, int b)
Definition: OLPTools.cpp:1296
COLPTools::PointIsInForeground
bool PointIsInForeground(const Vec3d vPoint, const CByteImage *pForegroundImage, const CCalibration *calibration)
Definition: OLPTools.cpp:356
COLPTools::CalculateEdgeImageFromImageAndDisparity
void CalculateEdgeImageFromImageAndDisparity(CByteImage *pImageGray, CByteImage *pDisparity, CByteImage *pEdgeImage)
Definition: OLPTools.cpp:1726
OLPTools.h
CHypothesisPoint
Definition: ObjectHypothesis.h:166
OLP_MIN_NUM_FEATURES
#define OLP_MIN_NUM_FEATURES
Definition: ObjectLearningByPushingDefinitions.h:109
OLP_TOLERANCE_CONCURRENT_MOTION
#define OLP_TOLERANCE_CONCURRENT_MOTION
Definition: ObjectLearningByPushingDefinitions.h:238
if
if(!yyvaluep)
Definition: Grammar.cpp:645
OLP_SIZE_OBJECT_HUE_HISTOGRAM
#define OLP_SIZE_OBJECT_HUE_HISTOGRAM
Definition: ObjectLearningByPushingDefinitions.h:234
CObjectHypothesis::vCenter
Vec3d vCenter
Definition: ObjectHypothesis.h:311
COLPTools::GetEnclosingRectangle
void GetEnclosingRectangle(const Vec2d *pPoints, const int nNumPoints, const bool bUseSecondMaxPoints, Vec2d &p1, Vec2d &p2, Vec2d &p3, Vec2d &p4, bool &bRotated)
Definition: OLPTools.cpp:505
OLP_OBJECT_LEARNING_DIR
#define OLP_OBJECT_LEARNING_DIR
Definition: ObjectLearningByPushingDefinitions.h:175
CObjectHypothesis::aVisibleConfirmedPoints
std::vector< CHypothesisPoint * > aVisibleConfirmedPoints
Definition: ObjectHypothesis.h:297
EPS
#define EPS
Definition: gdiam.cpp:1945
COLPTools::CreateHueAndSaturationHistogram
void CreateHueAndSaturationHistogram(const CObjectHypothesis *pHypothesis, std::vector< float > &aHueHistogram, std::vector< float > &aSaturationHistogram)
Definition: OLPTools.cpp:1071
max
T max(T t1, T t2)
Definition: gdiam.h:51
COLPTools::CreateSegmentationProbabilityMap
void CreateSegmentationProbabilityMap(const CObjectHypothesis *pHypothesis, const CCalibration *calibration, CByteImage *&pProbabilityImage)
Definition: OLPTools.cpp:1402
COLPTools::ClusterXMeans
void ClusterXMeans(const std::vector< CHypothesisPoint * > &aPoints, const int nMinNumClusters, const int nMaxNumClusters, const float fBICFactor, std::vector< std::vector< CHypothesisPoint * >> &aaPointClusters)
Definition: OLPTools.cpp:78
ARMARX_WARNING_S
#define ARMARX_WARNING_S
Definition: Logging.h:213
CObjectHypothesis::eSingleColored
@ eSingleColored
Definition: ObjectHypothesis.h:286
CHypothesisPoint::fColorR
float fColorR
Definition: ObjectHypothesis.h:238
GfxTL::Vec2d
VectorXD< 2, double > Vec2d
Definition: VectorXD.h:736
COLPTools
Definition: OLPTools.cpp:40
CObjectHypothesis
Definition: ObjectHypothesis.h:244
armarx::ctrlutil::v
double v(double t, double v0, double a0, double j)
Definition: CtrlUtil.h:39
float
#define float
Definition: 16_Level.h:22
CObjectHypothesis::aNewPoints
std::vector< CHypothesisPoint * > aNewPoints
Definition: ObjectHypothesis.h:294
COLPTools::FillHolesRGB
void FillHolesRGB(const CByteImage *pInputImage, CByteImage *pOutputImage, const int nRadius)
Definition: OLPTools.cpp:1583
COLPTools::DrawHypothesis
void DrawHypothesis(CObjectHypothesis *pHypothesis, const CCalibration *calibration, CByteImage *pImage, const int nNumFusedHypotheses)
Definition: OLPTools.cpp:1516
CObjectHypothesis::aDoubtablePoints
std::vector< CHypothesisPoint * > aDoubtablePoints
Definition: ObjectHypothesis.h:296
COLPTools::SmoothHistogram
void SmoothHistogram(std::vector< float > &aHistogram)
Definition: OLPTools.cpp:1169
CObjectHypothesis::aConfirmedPoints
std::vector< CHypothesisPoint * > aConfirmedPoints
Definition: ObjectHypothesis.h:295
ARMARX_VERBOSE_S
#define ARMARX_VERBOSE_S
Definition: Logging.h:207
COLPTools::ConvertRGB2HSV
void ConvertRGB2HSV(const unsigned char r, const unsigned char g, const unsigned char b, unsigned char &h, unsigned char &s, unsigned char &v)
Definition: OLPTools.cpp:1031
COLPTools::FilterForegroundPoints
void FilterForegroundPoints(const std::vector< CHypothesisPoint * > &aAllPoints, const CByteImage *pForegroundImage, const CCalibration *calibration, std::vector< CHypothesisPoint * > &aForegroundPoints)
Definition: OLPTools.cpp:333
OLP_SCREENSHOT_PATH
#define OLP_SCREENSHOT_PATH
Definition: ObjectLearningByPushingDefinitions.h:221
Logging.h
CHypothesisPoint::fColorB
float fColorB
Definition: ObjectHypothesis.h:238
min
T min(T t1, T t2)
Definition: gdiam.h:44
COLPTools::CountForegroundPixelsInRectangle
int CountForegroundPixelsInRectangle(const Vec2d vMiddleMin, const Vec2d vMinMiddle, const Vec2d vMaxMiddle, const Vec2d vMiddleMax, const CByteImage *pForegroundImage)
Definition: OLPTools.cpp:692
COLPTools::CheckIfPointIsAlreadyInHypothesis
bool CheckIfPointIsAlreadyInHypothesis(const Vec3d vPoint, const CObjectHypothesis *pHypothesis)
Definition: OLPTools.cpp:950
armarx::ctrlutil::s
double s(double t, double s0, double v0, double a0, double j)
Definition: CtrlUtil.h:33