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