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
53namespace 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
DSHT_IMAGE_HEIGHT
#define DSHT_IMAGE_HEIGHT
Definition HandLocalisationConstants.h:74
DSHT_NUM_PARAMETERS
#define DSHT_NUM_PARAMETERS
Definition HandLocalisationConstants.h:53
DSHT_IMAGE_WIDTH
#define DSHT_IMAGE_WIDTH
Definition HandLocalisationConstants.h:73
DSHT_HAND_MODEL_PATH
#define DSHT_HAND_MODEL_PATH
Definition HandLocalisationConstants.h:67
M_PI
#define M_PI
Definition MathTools.h:17
visionx::CHandLocalisation::GetFingertipPositions
std::vector< Vec3d > GetFingertipPositions()
Definition HandLocalisation.cpp:267
visionx::CHandLocalisation::SetSensorConfig
void SetSensorConfig(const Vec3d vPosition, const Mat3d mOrientation, const double *pConfigFingers)
Definition HandLocalisation.cpp:341
visionx::CHandLocalisation::LocaliseHand
void LocaliseHand(const CByteImage *pNewCamImageLeft, const CByteImage *pNewCamImageRight, const double *pSensorConfig, double *pEstimatedConfig, double &dConfidenceRating)
Definition HandLocalisation.cpp:362
visionx::CHandLocalisation::SetResultConfig
void SetResultConfig(const Vec3d vPosition, const Mat3d mOrientation, const double *pConfigFingers)
Definition HandLocalisation.cpp:307
visionx::CHandLocalisation::CHandLocalisation
CHandLocalisation(int nNumParticles, int nNumAnnealingRuns, int nPredictionMode, CStereoCalibration *pCalibration, std::string sHandModelFileName)
Definition HandLocalisation.cpp:55
ARMARX_VERBOSE_S
#define ARMARX_VERBOSE_S
Definition Logging.h:207
visionx
ArmarX headers.
Definition OpenPoseStressTest.h:39
visionx::ExtractAnglesFromRotationMatrix
void ExtractAnglesFromRotationMatrix(const Mat3d &mat, Vec3d &angles)
Definition HandLocalisation.cpp:195