HandLocalisation.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 // *****************************************************************
25 // -----------------------------------------------------------------
26 // HandLocalisation.cpp
27 // Implementation of class CHandLocalisation
28 // -----------------------------------------------------------------
29 // *****************************************************************
30 
31 // *****************************************************************
32 // Author: David Schiebener (Hiwi Kai Welke)
33 // Date: 11.12.2009
34 // *****************************************************************
35 
36 
37 // *****************************************************************
38 // includes
39 // *****************************************************************
40 
41 #include "HandLocalisation.h"
42 
43 #include <cmath>
44 
45 #include "Visualization/HandModelVisualizer.h"
46 
47 // OpenMP
48 #include <omp.h>
49 
50 // Core
51 #include <ArmarXCore/core/logging/Logging.h>
52 
53 namespace visionx
54 {
55  CHandLocalisation::CHandLocalisation(int nNumParticles,
56  int nNumAnnealingRuns,
57  int nPredictionMode,
58  CStereoCalibration* pCalibration,
59  std::string sHandModelFileName)
60  {
61  m_pHSVImageLeft = new CByteImage(DSHT_IMAGE_WIDTH, DSHT_IMAGE_HEIGHT, CByteImage::eRGB24);
62  m_pHSVImageRight = new CByteImage(DSHT_IMAGE_WIDTH, DSHT_IMAGE_HEIGHT, CByteImage::eRGB24);
63  m_pColorFilteredImageLeft =
64  new CByteImage(DSHT_IMAGE_WIDTH, DSHT_IMAGE_HEIGHT, CByteImage::eGrayScale);
65  m_pColorFilteredImageRight =
66  new CByteImage(DSHT_IMAGE_WIDTH, DSHT_IMAGE_HEIGHT, CByteImage::eGrayScale);
67  m_pSobelImageLeft =
68  new CByteImage(DSHT_IMAGE_WIDTH, DSHT_IMAGE_HEIGHT, CByteImage::eGrayScale);
69  m_pSobelImageRight =
70  new CByteImage(DSHT_IMAGE_WIDTH, DSHT_IMAGE_HEIGHT, CByteImage::eGrayScale);
71  m_pSobelXImageLeft =
72  new CByteImage(DSHT_IMAGE_WIDTH, DSHT_IMAGE_HEIGHT, CByteImage::eGrayScale);
73  m_pSobelXImageRight =
74  new CByteImage(DSHT_IMAGE_WIDTH, DSHT_IMAGE_HEIGHT, CByteImage::eGrayScale);
75  m_pSobelYImageLeft =
76  new CByteImage(DSHT_IMAGE_WIDTH, DSHT_IMAGE_HEIGHT, CByteImage::eGrayScale);
77  m_pSobelYImageRight =
78  new CByteImage(DSHT_IMAGE_WIDTH, DSHT_IMAGE_HEIGHT, CByteImage::eGrayScale);
79  m_pTempGrayImage1 =
80  new CByteImage(DSHT_IMAGE_WIDTH, DSHT_IMAGE_HEIGHT, CByteImage::eGrayScale);
81  m_pTempGrayImage2 =
82  new CByteImage(DSHT_IMAGE_WIDTH, DSHT_IMAGE_HEIGHT, CByteImage::eGrayScale);
83  m_pTempGrayImage3 =
84  new CByteImage(DSHT_IMAGE_WIDTH, DSHT_IMAGE_HEIGHT, CByteImage::eGrayScale);
85  m_pTempGrayImage4 =
86  new CByteImage(DSHT_IMAGE_WIDTH, DSHT_IMAGE_HEIGHT, CByteImage::eGrayScale);
87  m_pTempGrayImage5 =
88  new CByteImage(DSHT_IMAGE_WIDTH, DSHT_IMAGE_HEIGHT, CByteImage::eGrayScale);
89  m_pTempGrayImage6 =
90  new CByteImage(DSHT_IMAGE_WIDTH, DSHT_IMAGE_HEIGHT, CByteImage::eGrayScale);
91  m_pTrackingBallImageLeft =
92  new CByteImage(DSHT_IMAGE_WIDTH, DSHT_IMAGE_HEIGHT, CByteImage::eGrayScale);
93  m_pTrackingBallImageRight =
94  new CByteImage(DSHT_IMAGE_WIDTH, DSHT_IMAGE_HEIGHT, CByteImage::eGrayScale);
95 
96  m_pFinalConf = new double[DSHT_NUM_PARAMETERS];
97  m_pOldFinalConf = new double[DSHT_NUM_PARAMETERS];
98  m_pSensorConf = new double[DSHT_NUM_PARAMETERS];
99  m_pOldSensorConf = new double[DSHT_NUM_PARAMETERS];
100  m_pPFConf = new double[DSHT_NUM_PARAMETERS];
101  m_pOldPFConf = new double[DSHT_NUM_PARAMETERS];
102  m_pPredictedConf = new double[DSHT_NUM_PARAMETERS];
103 
104  for (int i = 0; i < DSHT_NUM_PARAMETERS; i++)
105  {
106  m_pFinalConf[i] = 0;
107  m_pOldFinalConf[i] = 0;
108  m_pSensorConf[i] = 0;
109  m_pOldSensorConf[i] = 0;
110  m_pPFConf[i] = 0;
111  m_pOldPFConf[i] = 0;
112  m_pPredictedConf[i] = 0;
113  }
114 
115  m_nNumParticles = nNumParticles;
116  m_nNumAnnealingRuns = nNumAnnealingRuns;
117  m_nPredictionMode = nPredictionMode;
118  m_dVarianceFactor = 1.0;
119 
120  const int nNumThreads = omp_get_num_procs();
121 
122  m_pParticleFilter = new CParticleFilterRobotHandLocalisation(
123  m_nNumParticles, DSHT_NUM_PARAMETERS, nNumThreads, pCalibration, sHandModelFileName);
124  m_pParticleFilter->SetImages(m_pColorFilteredImageLeft,
125  m_pColorFilteredImageRight,
126  m_pSobelImageLeft,
127  m_pSobelImageRight,
128  m_pSobelXImageLeft,
129  m_pSobelXImageRight,
130  m_pSobelYImageLeft,
131  m_pSobelYImageRight);
132 
133  m_pObjectFinderLeft = new CObjectFinder();
134  m_pObjectFinderRight = new CObjectFinder();
135  m_pdTrBallPosXLeft = new double[m_nMaxTrBallRegions];
136  m_pdTrBallPosXRight = new double[m_nMaxTrBallRegions];
137  m_pdTrBallPosYLeft = new double[m_nMaxTrBallRegions];
138  m_pdTrBallPosYRight = new double[m_nMaxTrBallRegions];
139  m_pdTrBallRadiusLeft = new double[m_nMaxTrBallRegions];
140  m_pdTrBallRadiusRight = new double[m_nMaxTrBallRegions];
141 
142 #ifdef DSHT_USE_ICUB
143  m_pHandModel = new HandModeliCub(DSHT_HAND_MODEL_PATH, pCalibration);
144 #else
145  m_pHandModel = new CHandModelV2(DSHT_HAND_MODEL_PATH, pCalibration);
146 #endif
147 
148  m_bFirstRun = true;
149  }
150 
151  CHandLocalisation::~CHandLocalisation()
152  {
153  delete m_pHSVImageLeft;
154  delete m_pHSVImageRight;
155  delete m_pColorFilteredImageLeft;
156  delete m_pColorFilteredImageRight;
157  delete m_pSobelImageLeft;
158  delete m_pSobelImageRight;
159  delete m_pSobelXImageLeft;
160  delete m_pSobelXImageRight;
161  delete m_pSobelYImageLeft;
162  delete m_pSobelYImageRight;
163  delete m_pTempGrayImage1;
164  delete m_pTempGrayImage2;
165  delete m_pTempGrayImage3;
166  delete m_pTempGrayImage4;
167  delete m_pTempGrayImage5;
168  delete m_pTempGrayImage6;
169  delete m_pTrackingBallImageLeft;
170  delete m_pTrackingBallImageRight;
171 
172  delete[] m_pFinalConf;
173  delete[] m_pOldFinalConf;
174  delete[] m_pSensorConf;
175  delete[] m_pOldSensorConf;
176  delete[] m_pPFConf;
177  delete[] m_pOldPFConf;
178  delete[] m_pPredictedConf;
179 
180  delete m_pParticleFilter;
181 
182  delete m_pObjectFinderLeft;
183  delete m_pObjectFinderRight;
184  delete[] m_pdTrBallPosXLeft;
185  delete[] m_pdTrBallPosXRight;
186  delete[] m_pdTrBallPosYLeft;
187  delete[] m_pdTrBallPosYRight;
188  delete[] m_pdTrBallRadiusLeft;
189  delete[] m_pdTrBallRadiusRight;
190 
191  delete m_pHandModel;
192  }
193 
194  inline void
195  ExtractAnglesFromRotationMatrix(const Mat3d& mat, Vec3d& angles)
196  {
197  angles.y = asin(mat.r3);
198  angles.x = atan2((-mat.r6), (mat.r9));
199  angles.z = atan2((-mat.r2), (mat.r1));
200  }
201 
202  void
203  CHandLocalisation::LocaliseHand(const CByteImage* pNewCamImageLeft,
204  const CByteImage* pNewCamImageRight,
205  const Vec3d vPositionFromSensors,
206  const Mat3d mOrientationFromSensors,
207  const double* pSensorConfigFingers,
208  double* pEstimatedConfig,
209  double& dConfidenceRating)
210  {
211  double pSensorConfig[DSHT_NUM_PARAMETERS];
212  Vec3d vRotationAngles;
213  ExtractAnglesFromRotationMatrix(mOrientationFromSensors, vRotationAngles);
214  pSensorConfig[0] = vPositionFromSensors.x;
215  pSensorConfig[1] = vPositionFromSensors.y;
216  pSensorConfig[2] = vPositionFromSensors.z;
217  pSensorConfig[3] = vRotationAngles.x;
218  pSensorConfig[4] = vRotationAngles.y;
219  pSensorConfig[5] = vRotationAngles.z;
220  for (int i = 6; i < DSHT_NUM_PARAMETERS; i++)
221  {
222  pSensorConfig[i] = pSensorConfigFingers[i - 6];
223  }
224 
225  LocaliseHand(pNewCamImageLeft,
226  pNewCamImageRight,
227  pSensorConfig,
228  pEstimatedConfig,
229  dConfidenceRating);
230  }
231 
232  Eigen::Matrix4f
233  CHandLocalisation::GetHandPose()
234  {
235  Eigen::Matrix4f handPose;
236 
237  configMutex.lock();
238 
239  handPose(0, 3) = m_pFinalConf[0];
240  handPose(1, 3) = m_pFinalConf[1];
241  handPose(2, 3) = m_pFinalConf[2];
242 
243  Mat3d orientation;
244  Math3d::SetRotationMat(orientation, m_pFinalConf[3], m_pFinalConf[4], m_pFinalConf[5]);
245 
246  configMutex.unlock();
247 
248  handPose(0, 0) = orientation.r1;
249  handPose(0, 1) = orientation.r2;
250  handPose(0, 2) = orientation.r3;
251  handPose(1, 0) = orientation.r4;
252  handPose(1, 1) = orientation.r5;
253  handPose(1, 2) = orientation.r6;
254  handPose(2, 0) = orientation.r7;
255  handPose(2, 1) = orientation.r8;
256  handPose(2, 2) = orientation.r9;
257 
258  handPose(3, 0) = 0;
259  handPose(3, 1) = 0;
260  handPose(3, 2) = 0;
261  handPose(3, 3) = 1;
262 
263  return handPose;
264  }
265 
266  std::vector<Vec3d>
267  CHandLocalisation::GetFingertipPositions()
268  {
269  configMutex.lock();
270  m_pHandModel->UpdateHand(m_pFinalConf);
271  configMutex.unlock();
272 
273  std::vector<Vec3d> ret;
274  for (size_t i = 0; i < m_pHandModel->m_aFingerTipCornersInWorldCS.size(); i++)
275  {
276  Vec3d vAverage = {0, 0, 0};
277  if (m_pHandModel->m_aFingerTipCornersInWorldCS.at(i).size() > 0)
278  {
279  for (size_t j = 0; j < m_pHandModel->m_aFingerTipCornersInWorldCS.at(i).size(); j++)
280  {
281  Math3d::AddToVec(vAverage,
282  m_pHandModel->m_aFingerTipCornersInWorldCS.at(i).at(j));
283  }
284  Math3d::MulVecScalar(vAverage,
285  1.0f / m_pHandModel->m_aFingerTipCornersInWorldCS.at(i).size(),
286  vAverage);
287  }
288  ret.push_back(vAverage);
289  }
290  return ret;
291  }
292 
293  double*
294  CHandLocalisation::GetResultConfig()
295  {
296  double* ret = new double[DSHT_NUM_PARAMETERS];
297  configMutex.lock();
298  for (int i = 0; i < DSHT_NUM_PARAMETERS; i++)
299  {
300  ret[i] = m_pFinalConf[i];
301  }
302  configMutex.unlock();
303  return ret;
304  }
305 
306  void
307  CHandLocalisation::SetResultConfig(const Vec3d vPosition,
308  const Mat3d mOrientation,
309  const double* pConfigFingers)
310  {
311  configMutex.lock();
312  Vec3d vRotationAngles;
313  ExtractAnglesFromRotationMatrix(mOrientation, vRotationAngles);
314  m_pFinalConf[0] = vPosition.x;
315  m_pFinalConf[1] = vPosition.y;
316  m_pFinalConf[2] = vPosition.z;
317  m_pFinalConf[3] = vRotationAngles.x;
318  m_pFinalConf[4] = vRotationAngles.y;
319  m_pFinalConf[5] = vRotationAngles.z;
320  for (int i = 6; i < DSHT_NUM_PARAMETERS; i++)
321  {
322  m_pFinalConf[i] = pConfigFingers[i - 6];
323  }
324  configMutex.unlock();
325  }
326 
327  double*
328  CHandLocalisation::GetSensorConfig()
329  {
330  double* ret = new double[DSHT_NUM_PARAMETERS];
331  configMutex.lock();
332  for (int i = 0; i < DSHT_NUM_PARAMETERS; i++)
333  {
334  ret[i] = m_pSensorConf[i];
335  }
336  configMutex.unlock();
337  return ret;
338  }
339 
340  void
341  CHandLocalisation::SetSensorConfig(const Vec3d vPosition,
342  const Mat3d mOrientation,
343  const double* pConfigFingers)
344  {
345  configMutex.lock();
346  Vec3d vRotationAngles;
347  ExtractAnglesFromRotationMatrix(mOrientation, vRotationAngles);
348  m_pSensorConf[0] = vPosition.x;
349  m_pSensorConf[1] = vPosition.y;
350  m_pSensorConf[2] = vPosition.z;
351  m_pSensorConf[3] = vRotationAngles.x;
352  m_pSensorConf[4] = vRotationAngles.y;
353  m_pSensorConf[5] = vRotationAngles.z;
354  for (int i = 6; i < DSHT_NUM_PARAMETERS; i++)
355  {
356  m_pSensorConf[i] = pConfigFingers[i - 6];
357  }
358  configMutex.unlock();
359  }
360 
361  void
362  CHandLocalisation::LocaliseHand(const CByteImage* pNewCamImageLeft,
363  const CByteImage* pNewCamImageRight,
364  const double* pSensorConfig,
365  double* pEstimatedConfig,
366  double& dConfidenceRating)
367  {
368  Mat3d mTemp1, mTemp2, mTemp3, mTemp4;
369  Vec3d vTemp1;
370  float fTemp1;
371  double pTempConf[DSHT_NUM_PARAMETERS];
372 
373  ARMARX_VERBOSE_S << "PF: Preparing";
374 
375  // **********************************************************************
376  // update sensor values
377  // **********************************************************************
378 
379  //ARMARX_VERBOSE_S << "Sensor values:";
380  for (int i = 0; i < DSHT_NUM_PARAMETERS; i++)
381  {
382  ARMARX_VERBOSE_S << pSensorConfig[i];
383  }
384 
385  configMutex.lock();
386  for (int i = 0; i < DSHT_NUM_PARAMETERS; i++)
387  {
388  m_pSensorConf[i] = pSensorConfig[i];
389  }
390  configMutex.unlock();
391 
392  m_pParticleFilter->SetSensorConfig(m_pSensorConf);
393 
394 
395 // **********************************************************************
396 // prepare images
397 // **********************************************************************
398 #pragma omp parallel sections
399  {
400 #pragma omp section
401  {
402  ImageProcessor::CalculateHSVImage(pNewCamImageLeft, m_pHSVImageLeft);
403  ImageProcessor::FilterHSV(m_pHSVImageLeft,
404  m_pColorFilteredImageLeft,
405  default_fingertip_hue,
406  default_fingertip_hue_tolerance,
407  default_fingertip_sat_min,
408  default_fingertip_sat_max,
409  default_fingertip_val_min,
410  default_fingertip_val_max);
411  ImageProcessor::Erode(m_pColorFilteredImageLeft, m_pTempGrayImage1);
412  ImageProcessor::Dilate(m_pTempGrayImage1, m_pColorFilteredImageLeft);
413  }
414 
415 #pragma omp section
416  {
417  ImageProcessor::CalculateHSVImage(pNewCamImageRight, m_pHSVImageRight);
418  ImageProcessor::FilterHSV(m_pHSVImageRight,
419  m_pColorFilteredImageRight,
420  default_fingertip_hue,
421  default_fingertip_hue_tolerance,
422  default_fingertip_sat_min,
423  default_fingertip_sat_max,
424  default_fingertip_val_min,
425  default_fingertip_val_max);
426  ImageProcessor::Erode(m_pColorFilteredImageRight, m_pTempGrayImage2);
427  ImageProcessor::Dilate(m_pTempGrayImage2, m_pColorFilteredImageRight);
428  }
429 
430 #pragma omp section
431  {
432  ImageProcessor::ConvertImage(pNewCamImageLeft, m_pTempGrayImage3);
433  ImageProcessor::CalculateGradientImageSobel(m_pTempGrayImage3, m_pTempGrayImage4);
434  for (int i = 0; i < m_pTempGrayImage4->width * m_pTempGrayImage4->height; i++)
435  {
436  m_pTempGrayImage4->pixels[i] = 15 * sqrt(m_pTempGrayImage4->pixels[i]);
437  }
438  ImageProcessor::GaussianSmooth3x3(m_pTempGrayImage4, m_pSobelImageLeft);
439 #ifdef DSHT_USE_EDGE_DIRECTION
440  ImageProcessor::SobelX(m_pTempGrayImage3, m_pTempGrayImage4, false);
441  ImageProcessor::GaussianSmooth3x3(m_pTempGrayImage4, m_pSobelXImageLeft);
442  ImageProcessor::SobelY(m_pTempGrayImage3, m_pTempGrayImage4, false);
443  ImageProcessor::GaussianSmooth3x3(m_pTempGrayImage4, m_pSobelYImageLeft);
444 #endif // DSHT_USE_EDGE_DIRECTION
445  }
446 
447 #pragma omp section
448  {
449  ImageProcessor::ConvertImage(pNewCamImageRight, m_pTempGrayImage5);
450  ImageProcessor::CalculateGradientImageSobel(m_pTempGrayImage5, m_pTempGrayImage6);
451  for (int i = 0; i < m_pTempGrayImage6->width * m_pTempGrayImage6->height; i++)
452  {
453  m_pTempGrayImage6->pixels[i] = 15 * sqrt(m_pTempGrayImage6->pixels[i]);
454  }
455  ImageProcessor::GaussianSmooth3x3(m_pTempGrayImage6, m_pSobelImageRight);
456 #ifdef DSHT_USE_EDGE_DIRECTION
457  ImageProcessor::SobelX(m_pTempGrayImage5, m_pTempGrayImage6, false);
458  ImageProcessor::GaussianSmooth3x3(m_pTempGrayImage6, m_pSobelXImageRight);
459  ImageProcessor::SobelY(m_pTempGrayImage5, m_pTempGrayImage6, false);
460  ImageProcessor::GaussianSmooth3x3(m_pTempGrayImage6, m_pSobelYImageRight);
461 #endif // DSHT_USE_EDGE_DIRECTION
462  }
463 
464 
465  // **********************************************************************
466  // find tracking ball
467  // **********************************************************************
468 
469 #pragma omp section
470  {
471  m_pObjectFinderLeft->PrepareImages(pNewCamImageLeft);
472  m_pObjectFinderLeft->FindObjects(pNewCamImageLeft,
473  NULL,
474  eBlue,
475  default_trball_hue,
476  default_trball_hue_tolerance,
477  default_trball_sat_min,
478  default_trball_sat_max,
479  default_trball_val_min,
480  default_trball_val_max,
481  500,
482  false); // 1000=nMinPointsPerRegion
483  int nNumRegions = m_pObjectFinderLeft->Finalize();
484 
485  // debug
486  //ImageProcessor::FilterHSV(m_pHSVImageLeft, m_pTrackingBallImageLeft, default_trball_hue, default_trball_hue_tolerance,
487  // default_trball_sat_min, default_trball_sat_max, default_trball_val_min, default_trball_val_max);
488 
489  MyRegion* pRegion;
490  for (int i = 0; i < nNumRegions && i < m_nMaxTrBallRegions; i++)
491  {
492  pRegion = &(m_pObjectFinderLeft->m_objectList.at(i).region);
493  m_pdTrBallPosXLeft[i] = 0.5 * (pRegion->min_x + pRegion->max_x);
494  m_pdTrBallPosYLeft[i] = 0.5 * (pRegion->min_y + pRegion->max_y);
495  m_pdTrBallRadiusLeft[i] =
496  (double)(((pRegion->max_x - pRegion->min_x) >
497  (pRegion->max_y - pRegion->min_y))
498  ? 0.5 * (pRegion->max_x - pRegion->min_x)
499  : 0.5 * (pRegion->max_y - pRegion->min_y));
500 
501  // debug
502  //CHandModelVisualizer::DrawCross(m_pTrackingBallImageLeft, m_pdTrBallPosXLeft[i], m_pdTrBallPosYLeft[i], 150);
503  //printf("Tracking ball pos: (%.1f, %.1f)\n", m_pdTrBallPosXLeft[i], m_pdTrBallPosYLeft[i]);
504  }
505  m_pParticleFilter->SetTrackingBallPositions(m_pdTrBallPosXLeft,
506  m_pdTrBallPosYLeft,
507  m_pdTrBallRadiusLeft,
508  nNumRegions,
509  true);
510 
511  // debug
512  //m_pTrackingBallImageLeft->SaveToFile("/home/staff/schieben/TrBall.bmp");
513  }
514 
515 #pragma omp section
516  {
517  m_pObjectFinderRight->PrepareImages(pNewCamImageRight);
518  m_pObjectFinderRight->FindObjects(pNewCamImageRight,
519  NULL,
520  eBlue,
521  default_trball_hue,
522  default_trball_hue_tolerance,
523  default_trball_sat_min,
524  default_trball_sat_max,
525  default_trball_val_min,
526  default_trball_val_max,
527  500,
528  false); // 1000=nMinPointsPerRegion
529  int nNumRegions = m_pObjectFinderRight->Finalize();
530 
531  MyRegion* pRegion;
532  for (int i = 0; i < nNumRegions && i < m_nMaxTrBallRegions; i++)
533  {
534  pRegion = &(m_pObjectFinderRight->m_objectList.at(i).region);
535  m_pdTrBallPosXRight[i] = 0.5 * (pRegion->min_x + pRegion->max_x);
536  m_pdTrBallPosYRight[i] = 0.5 * (pRegion->min_y + pRegion->max_y);
537  m_pdTrBallRadiusRight[i] =
538  (double)(((pRegion->max_x - pRegion->min_x) >
539  (pRegion->max_y - pRegion->min_y))
540  ? 0.5 * (pRegion->max_x - pRegion->min_x)
541  : 0.5 * (pRegion->max_y - pRegion->min_y));
542  }
543  m_pParticleFilter->SetTrackingBallPositions(m_pdTrBallPosXRight,
544  m_pdTrBallPosYRight,
545  m_pdTrBallRadiusRight,
546  nNumRegions,
547  false);
548  }
549  }
550 
551 
552  // debug
553  //m_pColorFilteredImageLeft->SaveToFile("/home/staff/schieben/ColFilImg.bmp");
554  //m_pSobelImageLeft->SaveToFile("/home/staff/schieben/EdgeImg.bmp");
555 
556 
557  // **********************************************************************
558  // predict new pose from old estimation
559  // **********************************************************************
560 
561 
562  // check for nan in old result
563  bool bNan = false;
564  for (int i = 0; i < DSHT_NUM_PARAMETERS; i++)
565  {
566  if (std::isnan(m_pOldFinalConf[i]))
567  {
568  bNan = true;
569  }
570  }
571 
572  if (!m_bFirstRun && !bNan)
573  {
574  // hand position
575  for (int i = 0; i < 3; i++)
576  {
577  m_pPredictedConf[i] = m_pOldFinalConf[i] + m_pSensorConf[i] - m_pOldSensorConf[i];
578  }
579 
580  // finger joints
581  for (int i = 6; i < DSHT_NUM_PARAMETERS; i++)
582  {
583  m_pPredictedConf[i] = m_pOldFinalConf[i] + m_pSensorConf[i] - m_pOldSensorConf[i];
584  if (m_pPredictedConf[i] < 0)
585  {
586  m_pPredictedConf[i] += 2 * M_PI; // avoid angle overflow
587  }
588  else if (m_pPredictedConf[i] >= 2 * M_PI)
589  {
590  m_pPredictedConf[i] -= 2 * M_PI;
591  }
592  }
593 
594  // hand rotation
595  Math3d::SetRotationMat(mTemp1, m_pSensorConf[3], m_pSensorConf[4], m_pSensorConf[5]);
596  Math3d::SetRotationMat(
597  mTemp2, m_pOldSensorConf[3], m_pOldSensorConf[4], m_pOldSensorConf[5]);
598  Math3d::Invert(mTemp2, mTemp3);
599  Math3d::MulMatMat(
600  mTemp1, mTemp3, mTemp4); // this is the trafo from old to new sensor rotation
601  Math3d::SetRotationMat(
602  mTemp1, m_pOldFinalConf[3], m_pOldFinalConf[4], m_pOldFinalConf[5]);
603  Math3d::MulMatMat(mTemp4, mTemp1, mTemp2); // this is the new rotation matrix
604  ExtractAnglesFromRotationMatrix(mTemp2, vTemp1);
605  m_pPredictedConf[3] = vTemp1.x;
606  m_pPredictedConf[4] = vTemp1.y;
607  m_pPredictedConf[5] = vTemp1.z;
608  }
609 
610 
611  // **********************************************************************
612  // apply particle filter
613  // **********************************************************************
614 
615  if ((!m_bFirstRun) && (m_nPredictionMode >= 1) && !bNan)
616  {
617  m_pParticleFilter->SetConfigOfATenthOfTheParticles(0, m_pSensorConf);
618  m_pParticleFilter->SetConfigOfATenthOfTheParticles(1, m_pSensorConf);
619  m_pParticleFilter->SetConfigOfATenthOfTheParticles(2, m_pSensorConf);
620  m_pParticleFilter->SetConfigOfATenthOfTheParticles(3, m_pSensorConf);
621  m_pParticleFilter->SetConfigOfATenthOfTheParticles(4, m_pSensorConf);
622  m_pParticleFilter->SetConfigOfATenthOfTheParticles(5, m_pPredictedConf);
623  m_pParticleFilter->SetConfigOfATenthOfTheParticles(6, m_pPredictedConf);
624  m_pParticleFilter->SetConfigOfATenthOfTheParticles(7, m_pPredictedConf);
625  m_pParticleFilter->SetConfigOfATenthOfTheParticles(8, m_pPredictedConf);
626  m_pParticleFilter->SetConfigOfATenthOfTheParticles(9, m_pPredictedConf);
627  }
628  else
629  {
630  m_pParticleFilter->SetParticleConfig(m_pSensorConf);
631  }
632 
633 
634  double dAnnFact = 1.0; // 1.0
635  double dAnnRedFactor = 0.5;
636  if (m_nNumAnnealingRuns > 4)
637  {
638  dAnnRedFactor =
639  pow(0.1, 1 / (m_nNumAnnealingRuns - 1)); // => annealing factor is never < 0.1
640  }
641  for (int i = 0; i < m_nNumAnnealingRuns; i++)
642  {
643  ARMARX_VERBOSE_S << "PF: Iteration " << i + 1 << " of " << m_nNumAnnealingRuns;
644  m_pParticleFilter->ParticleFilter(m_pPFConf, m_dVarianceFactor * dAnnFact);
645  dAnnFact *= dAnnRedFactor;
646  }
647 
648  //m_pParticleFilter->GetConfiguration(m_pPFConf, 1.0); // get average of particles that are better than 1.0 times the average
649  //m_pParticleFilter->GetBestConfiguration(m_pPFConf);
650 
651  m_pParticleFilter->GetDetailedRating(m_pPFConf, &m_PFRating);
652  m_dRating = (0.5 + 0.5 * m_PFRating.dConfidenceOfRating) * m_PFRating.dRating;
653 
654 
655  // **********************************************************************
656  // if prediction is used, calculate weighted combination of
657  // PF estimation and predicted config
658  // **********************************************************************
659 
660  configMutex.lock();
661 
662  if (m_nPredictionMode == 0 || bNan)
663  {
664  for (int i = 0; i < DSHT_NUM_PARAMETERS; i++)
665  {
666  m_pFinalConf[i] = m_pPFConf[i];
667  }
668  }
669  else if (m_nPredictionMode == 1 || m_nPredictionMode == 2)
670  {
671  ARMARX_VERBOSE_S << "Refining particle filter results with prediction";
672 
673  m_dOldRating *= 0.9;
674 
675  // TODO: include movement credibility rating
676  double dMovementRating = 1;
677 
678 
679  if (m_bFirstRun)
680  {
681  for (int i = 0; i < DSHT_NUM_PARAMETERS; i++)
682  {
683  m_pFinalConf[i] = m_pPFConf[i];
684  }
685  if (m_dRating == 0)
686  {
687  m_dRating = 0.001;
688  }
689  }
690  else if (m_dRating < 0.05 && false) // if new estimation is bad, use predicted config
691  {
692  for (int i = 0; i < DSHT_NUM_PARAMETERS; i++)
693  {
694  m_pFinalConf[i] = m_pPredictedConf[i];
695  }
696  }
697  else // else use weighted combination of new and predicted config
698  {
699  const float fWeightingFactor = 3.0;
700  double dWeight = (fWeightingFactor * m_dRating * dMovementRating /
701  (m_dOldRating + fWeightingFactor * m_dRating * dMovementRating));
702 
703  // hand position
704  for (int i = 0; i < 3; i++)
705  {
706  m_pFinalConf[i] = dWeight * m_pPFConf[i] + (1 - dWeight) * m_pPredictedConf[i];
707  }
708 
709  // hand rotation
710  Math3d::SetRotationMat(
711  mTemp1, m_pPredictedConf[3], m_pPredictedConf[4], m_pPredictedConf[5]);
712  Math3d::SetRotationMat(mTemp2, m_pPFConf[3], m_pPFConf[4], m_pPFConf[5]);
713  Math3d::Invert(mTemp1, mTemp3);
714  Math3d::MulMatMat(
715  mTemp2, mTemp3, mTemp4); // this is the trafo from predicted to PF rotation
716  Math3d::GetAxisAndAngle(mTemp4, vTemp1, fTemp1);
717  fTemp1 *= dWeight;
718  Math3d::SetRotationMatAxis(mTemp3, vTemp1, fTemp1); // trafo with reduced angle
719  Math3d::MulMatMat(mTemp3, mTemp1, mTemp2); // this is the new rotation matrix
720  ExtractAnglesFromRotationMatrix(mTemp2, vTemp1);
721  m_pFinalConf[3] = vTemp1.x;
722  m_pFinalConf[4] = vTemp1.y;
723  m_pFinalConf[5] = vTemp1.z;
724 
725  // finger joints
726  for (int i = 6; i < DSHT_NUM_PARAMETERS; i++)
727  {
728  m_pFinalConf[i] = dWeight * m_pPFConf[i] + (1 - dWeight) * m_pPredictedConf[i];
729  }
730  }
731 
732 
733  if (m_nPredictionMode == 2)
734  {
735  // refine result
736  m_pParticleFilter->SetParticleConfig(m_pFinalConf);
737  m_pParticleFilter->ParticleFilter(
738  (double*)pTempConf, 0.05, m_pParticleFilter->GetNumberOfParticles() / 4);
739  for (int i = 0; i < DSHT_NUM_PARAMETERS; i++)
740  {
741  m_pFinalConf[i] = 0.7 * pTempConf[i] + 0.3 * m_pFinalConf[i];
742  }
743  // update rating
744  m_pParticleFilter->GetDetailedRating(m_pFinalConf, &m_PFRating);
745  m_dRating = (0.5 + 0.5 * m_PFRating.dConfidenceOfRating) * m_PFRating.dRating;
746  }
747  }
748 
749 
750  // **********************************************************************
751  // return estimation, update old configs
752  // **********************************************************************
753 
754  for (int i = 0; i < DSHT_NUM_PARAMETERS; i++)
755  {
756  pEstimatedConfig[i] = m_pFinalConf[i];
757  m_pOldFinalConf[i] = m_pFinalConf[i];
758  m_pOldSensorConf[i] = m_pSensorConf[i];
759  m_pOldPFConf[i] = m_pPFConf[i];
760  }
761  m_dOldRating = m_dRating;
762 
763  configMutex.unlock();
764 
765  m_bFirstRun = false;
766  }
767 } // namespace visionx
visionx::CHandLocalisation::m_pColorFilteredImageLeft
CByteImage * m_pColorFilteredImageLeft
Definition: HandLocalisation.h:105
visionx::CHandModelV2
Definition: HandModelV2.h:36
armarx::aron::ret
ReaderT::InputType T & ret
Definition: rw.h:13
visionx::DetailedPFRating::dConfidenceOfRating
double dConfidenceOfRating
Definition: ParticleFilterRobotHandLocalisation.h:69
GfxTL::Vec3d
VectorXD< 3, double > Vec3d
Definition: VectorXD.h:737
visionx
ArmarX headers.
Definition: OpenPoseStressTest.h:38
visionx::HandModeliCub
Definition: HandModeliCub.h:30
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:650
DSHT_IMAGE_WIDTH
#define DSHT_IMAGE_WIDTH
Definition: HandLocalisationConstants.h:73
visionx::CParticleFilterRobotHandLocalisation::SetTrackingBallPositions
void SetTrackingBallPositions(double *dPosX, double *dPosY, double *dRadius, int nNumTrBallRegions, bool bLeftCamImage)
Definition: ParticleFilterRobotHandLocalisation.cpp:207
visionx::CParticleFilterFrameworkParallelized::GetNumberOfParticles
int GetNumberOfParticles()
Definition: ParticleFilterFrameworkParallelized.h:43
visionx::CParticleFilterRobotHandLocalisation::GetDetailedRating
void GetDetailedRating(double *pConf, DetailedPFRating *pRating)
Definition: ParticleFilterRobotHandLocalisation.cpp:894
visionx::CHandLocalisation::m_pTrackingBallImageRight
CByteImage * m_pTrackingBallImageRight
Definition: HandLocalisation.h:110
DSHT_HAND_MODEL_PATH
#define DSHT_HAND_MODEL_PATH
Definition: HandLocalisationConstants.h:67
HandModelVisualizer.h
visionx::CHandLocalisation::GetFingertipPositions
std::vector< Vec3d > GetFingertipPositions()
Definition: HandLocalisation.cpp:267
visionx::CParticleFilterRobotHandLocalisation
Definition: ParticleFilterRobotHandLocalisation.h:72
M_PI
#define M_PI
Definition: MathTools.h:17
visionx::CHandLocalisation::m_pTrackingBallImageLeft
CByteImage * m_pTrackingBallImageLeft
Definition: HandLocalisation.h:109
visionx::CHandLocalisation::LocaliseHand
void LocaliseHand(const CByteImage *pNewCamImageLeft, const CByteImage *pNewCamImageRight, const double *pSensorConfig, double *pEstimatedConfig, double &dConfidenceRating)
Definition: HandLocalisation.cpp:362
visionx::CParticleFilterRobotHandLocalisation::SetImages
void SetImages(CByteImage *pRegionImageLeft, CByteImage *pRegionImageRight, CByteImage *pSobelImageLeft, CByteImage *pSobelImageRight, CByteImage *pSobelXImageLeft, CByteImage *pSobelXImageRight, CByteImage *pSobelYImageLeft, CByteImage *pSobelYImageRight)
Definition: ParticleFilterRobotHandLocalisation.cpp:124
visionx::CHandLocalisation::m_pSobelImageLeft
CByteImage * m_pSobelImageLeft
Definition: HandLocalisation.h:107
visionx::CHandLocalisation::GetHandPose
Eigen::Matrix4f GetHandPose()
Definition: HandLocalisation.cpp:233
visionx::CHandModelV2::m_aFingerTipCornersInWorldCS
std::vector< std::vector< Vec3d > > m_aFingerTipCornersInWorldCS
Definition: HandModelV2.h:71
visionx::DetailedPFRating::dRating
double dRating
Definition: ParticleFilterRobotHandLocalisation.h:68
GfxTL::sqrt
VectorXD< D, T > sqrt(const VectorXD< D, T > &a)
Definition: VectorXD.h:704
visionx::CHandLocalisation::CHandLocalisation
CHandLocalisation(int nNumParticles, int nNumAnnealingRuns, int nPredictionMode, CStereoCalibration *pCalibration, std::string sHandModelFileName)
Definition: HandLocalisation.cpp:55
visionx::CParticleFilterRobotHandLocalisation::SetParticleConfig
void SetParticleConfig(double *pConfig)
Definition: ParticleFilterRobotHandLocalisation.cpp:146
visionx::CHandLocalisation::SetResultConfig
void SetResultConfig(const Vec3d vPosition, const Mat3d mOrientation, const double *pConfigFingers)
Definition: HandLocalisation.cpp:307
HandLocalisation.h
visionx::ExtractAnglesFromRotationMatrix
void ExtractAnglesFromRotationMatrix(const Mat3d &mat, Vec3d &angles)
Definition: HandLocalisation.cpp:195
DSHT_NUM_PARAMETERS
#define DSHT_NUM_PARAMETERS
Definition: HandLocalisationConstants.h:53
visionx::CHandLocalisation::~CHandLocalisation
~CHandLocalisation()
Definition: HandLocalisation.cpp:151
ARMARX_VERBOSE_S
#define ARMARX_VERBOSE_S
Definition: Logging.h:207
visionx::CParticleFilterRobotHandLocalisation::SetConfigOfATenthOfTheParticles
void SetConfigOfATenthOfTheParticles(int nTenthIndex, double *pConfig)
Definition: ParticleFilterRobotHandLocalisation.cpp:168
Logging.h
visionx::CHandLocalisation::GetResultConfig
double * GetResultConfig()
Definition: HandLocalisation.cpp:294
visionx::CParticleFilterRobotHandLocalisation::SetSensorConfig
void SetSensorConfig(double *sconfig)
Definition: ParticleFilterRobotHandLocalisation.cpp:184
DSHT_IMAGE_HEIGHT
#define DSHT_IMAGE_HEIGHT
Definition: HandLocalisationConstants.h:74
visionx::CHandLocalisation::m_pColorFilteredImageRight
CByteImage * m_pColorFilteredImageRight
Definition: HandLocalisation.h:106
visionx::CHandModelV2::UpdateHand
virtual void UpdateHand(double *pConfig)
Definition: HandModelV2.cpp:394
visionx::CParticleFilterFrameworkParallelized::ParticleFilter
double ParticleFilter(double *pResultMeanConfiguration, double dSigmaFactor, int nNumParticlesToUse=-1)
Definition: ParticleFilterFrameworkParallelized.cpp:44
visionx::CHandLocalisation::SetSensorConfig
void SetSensorConfig(const Vec3d vPosition, const Mat3d mOrientation, const double *pConfigFingers)
Definition: HandLocalisation.cpp:341
visionx::CHandLocalisation::GetSensorConfig
double * GetSensorConfig()
Definition: HandLocalisation.cpp:328
visionx::CHandLocalisation::m_pSobelImageRight
CByteImage * m_pSobelImageRight
Definition: HandLocalisation.h:108