HandModelVisualizer.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 "HandModelVisualizer.h"
25 
26 #include <ArmarXCore/core/logging/Logging.h>
27 
28 #include <Image/ImageProcessor.h>
29 
30 namespace visionx
31 {
32  //**********************************************************************************************
33  // Constructor / destructor
34  //**********************************************************************************************
35 
36 
37  // constructor
38  CHandModelVisualizer::CHandModelVisualizer(CStereoCalibration* pCalibration,
39  bool bUseLeftCamera)
40  {
41 #ifdef DSHT_USE_ICUB
42  m_pHandModelV2 = new HandModeliCub(DSHT_HAND_MODEL_PATH, pCalibration);
43 #else
44  m_pHandModelV2 = new CHandModelV2(DSHT_HAND_MODEL_PATH, pCalibration);
45 #endif
46 
47  if (bUseLeftCamera)
48  {
49  m_fFocalLengthX =
50  pCalibration->GetLeftCalibration()->GetCameraParameters().focalLength.x;
51  m_fFocalLengthY =
52  pCalibration->GetLeftCalibration()->GetCameraParameters().focalLength.y;
53  m_fPrincipalPointX =
54  pCalibration->GetLeftCalibration()->GetCameraParameters().principalPoint.x;
55  m_fPrincipalPointY =
56  pCalibration->GetLeftCalibration()->GetCameraParameters().principalPoint.y;
57  }
58  else
59  {
60  m_fFocalLengthX =
61  pCalibration->GetRightCalibration()->GetCameraParameters().focalLength.x;
62  m_fFocalLengthY =
63  pCalibration->GetRightCalibration()->GetCameraParameters().focalLength.y;
64  m_fPrincipalPointX =
65  pCalibration->GetRightCalibration()->GetCameraParameters().principalPoint.x;
66  m_fPrincipalPointY =
67  pCalibration->GetRightCalibration()->GetCameraParameters().principalPoint.y;
68  }
69 
70  m_pStereoCalibration = pCalibration;
71 
72  m_pMoveMaster = new CMoveMasterModel();
73  if (!m_pMoveMaster->init(DSHT_OI_FILE_PATH, m_fFocalLengthY))
74  {
75  ARMARX_WARNING_S
76  << "CHandModelVisualizer constructor: Could not load OpenInventor-file from "
77  << DSHT_OI_FILE_PATH;
78  delete m_pMoveMaster;
79  return;
80  }
81  }
82 
83  // destructor
84  CHandModelVisualizer::~CHandModelVisualizer()
85  {
86  delete m_pMoveMaster;
87  }
88 
89  //**********************************************************************************************
90  // Update hand configuration
91  //**********************************************************************************************
92 
93  void
94  CHandModelVisualizer::UpdateHandModel(double* pConfig,
95  bool bUpdateOpenInventorModel,
96  bool bDrawCylinderInHand)
97  {
98  //m_pHandModel->UpdateHand(pConfig);
99 
100  m_pHandModelV2->UpdateHand(pConfig);
101 
102  if (bUpdateOpenInventorModel)
103  {
104  ConfigureMoveMasterModel(pConfig, bDrawCylinderInHand);
105  }
106  }
107 
108  void
109  CHandModelVisualizer::ConfigureMoveMasterModel(double* pConfig, bool bDrawCylinderInHand)
110  {
111  /*
112  // --- set object cylinder ---
113 
114  // get positions of palm center, thumb tip, index tip and middle tip
115  Vec3d vPalmCenter, vThumbTip, vIndexTip, vMiddleTip, vIndexAndMiddle, tempVec1, tempVec2;
116 
117  // palm center
118  tempVec1.x = 0;
119  tempVec1.y = 20;
120  tempVec1.z = 0;
121  Math3d::AddVecVec(tempVec1, palm_center_translation, tempVec2);
122  Math3d::MulMatVec(m_pHandModel->GetHandOrientation(), tempVec2, tempVec1);
123  Math3d::AddVecVec(m_pHandModel->GetHandPosition(), tempVec1, vPalmCenter);
124 
125  // thumb tip
126  tempVec1.x = 0;
127  tempVec1.y = -20;
128  tempVec1.z = 0;
129  Math3d::MulMatVec(m_pHandModel->fingers[0]->joint_rot_abs[2], tempVec1, tempVec2);
130  Math3d::AddVecVec(m_pHandModel->fingers[0]->joint_pos_abs[2], tempVec2, vThumbTip);
131 
132  // index tip
133  tempVec1.x = 0;
134  tempVec1.y = -20;
135  tempVec1.z = 0;
136  Math3d::MulMatVec(m_pHandModel->fingers[1]->joint_rot_abs[2], tempVec1, tempVec2);
137  Math3d::AddVecVec(m_pHandModel->fingers[1]->joint_pos_abs[2], tempVec2, vIndexTip);
138 
139  // middle tip
140  tempVec1.x = 0;
141  tempVec1.y = -20;
142  tempVec1.z = 0;
143  Math3d::MulMatVec(m_pHandModel->fingers[2]->joint_rot_abs[2], tempVec1, tempVec2);
144  Math3d::AddVecVec(m_pHandModel->fingers[2]->joint_pos_abs[2], tempVec2, vMiddleTip);
145 
146  // calc average of index and middle finger tip
147  Math3d::AddVecVec(vIndexTip, vMiddleTip, tempVec1);
148  Math3d::MulVecScalar(tempVec1, 0.5, vIndexAndMiddle);
149 
150 
151  // calc circumscribed circle of the three points
152  ColumnVector vPointA(3), vPointB(3), vPointC(3);
153  vPointA << vPalmCenter.x << vPalmCenter.y << vPalmCenter.z;
154  vPointB << vThumbTip.x << vThumbTip.y << vThumbTip.z;
155  vPointC << vIndexAndMiddle.x << vIndexAndMiddle.y << vIndexAndMiddle.z;
156 
157  ColumnVector vDistAB(3), vDistAC(3), vDistBC(3);
158  vDistAB = vPointB - vPointA;
159  vDistAC = vPointC - vPointA;
160  vDistBC = vPointC - vPointB;
161 
162  float alpha = acos( DotProduct(vDistAB, vDistAC) / vDistAB.NormFrobenius() / vDistAC.NormFrobenius() );
163  float beta = acos( DotProduct(-vDistAB, vDistBC) / vDistAB.NormFrobenius() / vDistBC.NormFrobenius() );
164  float gamma = acos( DotProduct(-vDistBC, -vDistAC) / vDistBC.NormFrobenius() / vDistAC.NormFrobenius() );
165 
166  // use the barycentric coordinates (wiki: umkreis) to calc the center of the circle
167  ColumnVector vCenter(3);
168  vCenter = 0.5*(1.0*sin(2*alpha)*vPointA + sin(2*beta)*vPointB + sin(2*gamma)*vPointC);
169  vCenter /= 0.5 * ( 1.0*sin(2*alpha) + sin(2*beta) + sin(2*gamma) );
170 
171  Vec3d vCylinderCenter;
172  vCylinderCenter.x = vCenter(1);
173  vCylinderCenter.y = vCenter(2);
174  vCylinderCenter.z = vCenter(3);
175 
176  float fRadius = 0.9 * (vCenter-vPointA).NormFrobenius();
177  float fHeight = 120;
178 
179  if (!bDrawCylinderInHand)
180  {
181  fRadius = 0;
182  fHeight = 0;
183  }
184 
185  m_pMoveMaster->setObjCylConfig(vCylinderCenter, -pConfig[3], pConfig[4], -pConfig[5], fHeight, fRadius);
186  */
187  if (bDrawCylinderInHand)
188  {
189  printf("Warning: CHandModelVisualizer::ConfigureMoveMasterModel: Cylinder not "
190  "available with new HandmodelV2. Code needs to be changed.\n");
191  }
192 
193  Vec3d vCylinderCenter = {0, 0, -1000};
194  m_pMoveMaster->setObjCylConfig(vCylinderCenter, -pConfig[3], pConfig[4], -pConfig[5], 0, 0);
195 
196 
197  // --- set hand configuration ---
198 
199  Vec3d vConfPos;
200  vConfPos.x = pConfig[0];
201  vConfPos.y = pConfig[1];
202  vConfPos.z = pConfig[2];
203 
204  m_pMoveMaster->setHandPosition(vConfPos);
205 
206  Mat3d m0, m1, m2, m3;
207  Vec3d vAngles;
208  Math3d::SetRotationMat(m0, pConfig[3], pConfig[4], pConfig[5]);
209  Math3d::SetRotationMatX(m1, -M_PI / 2);
210  Math3d::SetRotationMatZ(m2, M_PI / 2);
211  Math3d::MulMatMat(m1, m2, m3);
212  Math3d::MulMatMat(m0, m3, m1);
213  ExtractAnglesFromRotationMatrix(m1, vAngles);
214  m_pMoveMaster->setHandRotation(vAngles.x, vAngles.y, vAngles.z);
215  //m_pMoveMaster->setHandRotation(-pConfig[3], pConfig[4], -pConfig[5]);
216 
217  m_pMoveMaster->setJointAngle(5, pConfig[6]); // finger joints
218  m_pMoveMaster->setJointAngle(6, pConfig[9]);
219  m_pMoveMaster->setJointAngle(7, pConfig[9]);
220  m_pMoveMaster->setJointAngle(8, pConfig[10]);
221  m_pMoveMaster->setJointAngle(9, pConfig[10]);
222  m_pMoveMaster->setJointAngle(10, pConfig[11]);
223  m_pMoveMaster->setJointAngle(11, pConfig[11]);
224  m_pMoveMaster->setJointAngle(12, pConfig[11]);
225  m_pMoveMaster->setJointAngle(13, pConfig[11]);
226  m_pMoveMaster->setJointAngle(14, pConfig[7]);
227  m_pMoveMaster->setJointAngle(15, pConfig[8]);
228 
229  m_pMoveMaster->setJointAngle(16, pConfig[7] / 2); // actuators
230  m_pMoveMaster->setJointAngle(17, pConfig[8] / 2);
231  m_pMoveMaster->setJointAngle(18, pConfig[9] / 2);
232  m_pMoveMaster->setJointAngle(19, pConfig[9] / 2);
233  m_pMoveMaster->setJointAngle(20, pConfig[10] / 2);
234  m_pMoveMaster->setJointAngle(21, pConfig[10] / 2);
235  m_pMoveMaster->setJointAngle(22, pConfig[11] / 2);
236  m_pMoveMaster->setJointAngle(23, pConfig[11] / 2);
237  m_pMoveMaster->setJointAngle(24, pConfig[11] / 2);
238  m_pMoveMaster->setJointAngle(25, pConfig[11] / 2);
239 
240 
241  m_pMoveMaster->update();
242  }
243 
244  //**********************************************************************************************
245  // Visualisations
246  //**********************************************************************************************
247  void
248  CHandModelVisualizer::DrawPolygon(CByteImage* pImage,
249  ConvexPolygonCalculations::Polygon& pPolygon)
250  {
251  const int width = pImage->width;
252  int leftBorderInt, rightBorderInt, up, down, indexLeft, indexRight, indexMain, nTempOffset;
253  float leftBorder = 0, rightBorder = 0, leftOffset = 0, rightOffset = 0;
254 
255  indexLeft = 1;
256  indexRight = 1;
257  for (indexMain = 0; indexMain < pPolygon.nCorners - 1; indexMain++)
258  {
259  if (pPolygon.hull[indexMain].z != 2)
260  {
261  leftBorder = pPolygon.hullLeft[indexLeft].x;
262  leftOffset = (pPolygon.hullLeft[indexLeft + 1].x - pPolygon.hullLeft[indexLeft].x) /
263  (pPolygon.hullLeft[indexLeft + 1].y - pPolygon.hullLeft[indexLeft].y);
264  }
265  if (pPolygon.hull[indexMain].z != 1)
266  {
267  rightBorder = pPolygon.hullRight[indexRight].x;
268  rightOffset =
269  (pPolygon.hullRight[indexRight + 1].x - pPolygon.hullRight[indexRight].x) /
270  (pPolygon.hullRight[indexRight + 1].y - pPolygon.hullRight[indexRight].y);
271  }
272  up = (int)pPolygon.hull[indexMain].y;
273  down = (int)pPolygon.hull[indexMain + 1].y;
274 
275  if (pPolygon.hull[indexMain + 1].z != 2)
276  {
277  indexLeft++;
278  }
279  if (pPolygon.hull[indexMain + 1].z != 1)
280  {
281  indexRight++;
282  }
283 
284  if ((down > DSHT_IMAGE_HEIGHT - 1) || (up < 0))
285  {
286  goto skip_updown; // not in the image
287  }
288 
289  // stay inside image
290  if (down < 0)
291  {
292  down = 0;
293  }
294  if (up > DSHT_IMAGE_HEIGHT - 1)
295  {
296  up = DSHT_IMAGE_HEIGHT - 1;
297  }
298 
299  for (int j = up; j > down; j--, leftBorder -= leftOffset, rightBorder -= rightOffset)
300  {
301  // optim: width/2 sofort dazuaddieren
302  leftBorderInt = (int)leftBorder;
303  rightBorderInt = (int)rightBorder;
304 
305  // stay inside image
306  if ((leftBorderInt > DSHT_IMAGE_WIDTH - 1) || (rightBorderInt < 0))
307  {
308  goto skip_leftright;
309  }
310 
311  if (leftBorderInt < 0)
312  {
313  leftBorderInt = 0;
314  }
315  if (rightBorderInt > DSHT_IMAGE_WIDTH - 1)
316  {
317  rightBorderInt = DSHT_IMAGE_WIDTH - 1;
318  }
319 
320  nTempOffset = j * width;
321  for (int k = leftBorderInt; k < rightBorderInt; k++)
322  {
323  pImage->pixels[3 * (nTempOffset + k) + 0] = 0;
324  pImage->pixels[3 * (nTempOffset + k) + 1] = 0;
325  pImage->pixels[3 * (nTempOffset + k) + 2] = 255;
326  }
327  skip_leftright:;
328  }
329  skip_updown:;
330  }
331  }
332 
333  void
334  CHandModelVisualizer::DrawPolygonGrayscale(CByteImage* pImage,
335  ConvexPolygonCalculations::Polygon& pPolygon)
336  {
337  const int width = pImage->width;
338  int leftBorderInt, rightBorderInt, up, down, indexLeft, indexRight, indexMain, nTempOffset;
339  float leftBorder = 0, rightBorder = 0, leftOffset = 0, rightOffset = 0;
340 
341  indexLeft = 1;
342  indexRight = 1;
343  for (indexMain = 0; indexMain < pPolygon.nCorners - 1; indexMain++)
344  {
345  if (pPolygon.hull[indexMain].z != 2)
346  {
347  leftBorder = pPolygon.hullLeft[indexLeft].x;
348  leftOffset = (pPolygon.hullLeft[indexLeft + 1].x - pPolygon.hullLeft[indexLeft].x) /
349  (pPolygon.hullLeft[indexLeft + 1].y - pPolygon.hullLeft[indexLeft].y);
350  }
351  if (pPolygon.hull[indexMain].z != 1)
352  {
353  rightBorder = pPolygon.hullRight[indexRight].x;
354  rightOffset =
355  (pPolygon.hullRight[indexRight + 1].x - pPolygon.hullRight[indexRight].x) /
356  (pPolygon.hullRight[indexRight + 1].y - pPolygon.hullRight[indexRight].y);
357  }
358  up = (int)pPolygon.hull[indexMain].y;
359  down = (int)pPolygon.hull[indexMain + 1].y;
360 
361  if (pPolygon.hull[indexMain + 1].z != 2)
362  {
363  indexLeft++;
364  }
365  if (pPolygon.hull[indexMain + 1].z != 1)
366  {
367  indexRight++;
368  }
369 
370  if ((down > DSHT_IMAGE_HEIGHT - 1) || (up < 0))
371  {
372  goto skip_updown; // not in the image
373  }
374 
375  // stay inside image
376  if (down < 0)
377  {
378  down = 0;
379  }
380  if (up > DSHT_IMAGE_HEIGHT - 1)
381  {
382  up = DSHT_IMAGE_HEIGHT - 1;
383  }
384 
385  for (int j = up; j > down; j--, leftBorder -= leftOffset, rightBorder -= rightOffset)
386  {
387  // optim: width/2 sofort dazuaddieren
388  leftBorderInt = (int)leftBorder;
389  rightBorderInt = (int)rightBorder;
390 
391  // stay inside image
392  if ((leftBorderInt > DSHT_IMAGE_WIDTH - 1) || (rightBorderInt < 0))
393  {
394  goto skip_leftright;
395  }
396 
397  if (leftBorderInt < 0)
398  {
399  leftBorderInt = 0;
400  }
401  if (rightBorderInt > DSHT_IMAGE_WIDTH - 1)
402  {
403  rightBorderInt = DSHT_IMAGE_WIDTH - 1;
404  }
405 
406  nTempOffset = j * width;
407  for (int k = leftBorderInt; k < rightBorderInt; k++)
408  {
409  pImage->pixels[nTempOffset + k] = 255;
410  }
411  skip_leftright:;
412  }
413  skip_updown:;
414  }
415  }
416 
417  /*
418  void CHandModelVisualizer::DrawFingertips(CByteImage* pImage)
419  {
420  // draw fingertips
421  const int width = pImage->width;
422  int leftBorderInt, rightBorderInt, up, down;
423 
424  int num_polygons = m_pHandModel->GetOverallNumberOfPolygons();
425  Polygon* polygons = m_pHandModel->GetProjectedPolygonsLeftCam();
426 
427  for (int i=0; i<num_polygons; i++)
428  {
429  DrawPolygon(pImage, polygons[i]);
430  }
431 
432 
433 
434  // draw circle for tracking ball
435  double dCenterX, dCenterY, dRadius;
436  m_pHandModel->GetTrackingBallPositionLeftCam(dCenterX, dCenterY, dRadius);
437 
438  up = (int)(dCenterY-dRadius);
439  down = (int)(dCenterY+dRadius);
440  for (int i=up; i<=down; i++)
441  {
442  leftBorderInt = (int)( dCenterX - sqrt(dRadius*dRadius-(i-dCenterY)*(i-dCenterY)) );
443  rightBorderInt = (int)( dCenterX + sqrt(dRadius*dRadius-(i-dCenterY)*(i-dCenterY)) );
444  for (int j=leftBorderInt; j<=rightBorderInt; j++)
445  {
446  if (i>=0 && i<DSHT_IMAGE_HEIGHT && j>=0 && j<DSHT_IMAGE_WIDTH)
447  {
448  pImage->pixels[3 * (i*width + j) + 0] = 0;
449  pImage->pixels[3 * (i*width + j) + 1] = 255;
450  pImage->pixels[3 * (i*width + j) + 2] = 0;
451  }
452  }
453  }
454 
455 
456  // draw center point of the hand
457  Vec3d vHandCenter3D = m_pHandModel->GetHandPosition();
458  Vec2d vHandCenter2D;
459  m_pStereoCalibration->GetLeftCalibration()->CameraToImageCoordinates(vHandCenter3D, vHandCenter2D);
460  //DrawCross(pImage, vHandCenter2D.x, vHandCenter2D.y, 255, 255, 255);
461 
462 
463  // draw coordinate system
464  Vec3d vOffset, vHandCenterPlusOffset;
465  Vec2d vProjectedPoint;
466  Mat3d mHandOrientation = m_pHandModel->GetHandOrientation();
467 
468  Math3d::SetVec(vOffset, 100, 0, 0);
469  Math3d::MulMatVec(mHandOrientation, vOffset, vHandCenter3D, vHandCenterPlusOffset);
470  //Math3d::AddVecVec(vHandCenter3D, vOffset, vHandCenterPlusOffset);
471  m_pStereoCalibration->GetLeftCalibration()->CameraToImageCoordinates(vHandCenterPlusOffset, vProjectedPoint);
472  DrawLineIntoImage(pImage, vHandCenter2D.x, vHandCenter2D.y, vProjectedPoint.x, vProjectedPoint.y, 255, 0, 0);
473  DrawLineIntoImage(pImage, vHandCenter2D.x+1, vHandCenter2D.y, vProjectedPoint.x+1, vProjectedPoint.y, 255, 0, 0);
474  DrawLineIntoImage(pImage, vHandCenter2D.x, vHandCenter2D.y+1, vProjectedPoint.x, vProjectedPoint.y+1, 255, 0, 0);
475  DrawLineIntoImage(pImage, vHandCenter2D.x+1, vHandCenter2D.y+1, vProjectedPoint.x+1, vProjectedPoint.y+1, 255, 0, 0);
476 
477  Math3d::SetVec(vOffset, 0, 100, 0);
478  Math3d::MulMatVec(mHandOrientation, vOffset, vHandCenter3D, vHandCenterPlusOffset);
479  m_pStereoCalibration->GetLeftCalibration()->CameraToImageCoordinates(vHandCenterPlusOffset, vProjectedPoint);
480  DrawLineIntoImage(pImage, vHandCenter2D.x, vHandCenter2D.y, vProjectedPoint.x, vProjectedPoint.y, 0, 255, 0);
481  DrawLineIntoImage(pImage, vHandCenter2D.x+1, vHandCenter2D.y, vProjectedPoint.x+1, vProjectedPoint.y, 0, 0, 255);
482  DrawLineIntoImage(pImage, vHandCenter2D.x, vHandCenter2D.y+1, vProjectedPoint.x, vProjectedPoint.y+1, 0, 255, 0);
483  DrawLineIntoImage(pImage, vHandCenter2D.x+1, vHandCenter2D.y+1, vProjectedPoint.x+1, vProjectedPoint.y+1, 0, 255, 0);
484 
485  Math3d::SetVec(vOffset, 0, 0, 100);
486  Math3d::MulMatVec(mHandOrientation, vOffset, vHandCenter3D, vHandCenterPlusOffset);
487  m_pStereoCalibration->GetLeftCalibration()->CameraToImageCoordinates(vHandCenterPlusOffset, vProjectedPoint);
488  DrawLineIntoImage(pImage, vHandCenter2D.x, vHandCenter2D.y, vProjectedPoint.x, vProjectedPoint.y, 0, 0, 255);
489  DrawLineIntoImage(pImage, vHandCenter2D.x+1, vHandCenter2D.y, vProjectedPoint.x+1, vProjectedPoint.y, 0, 0, 255);
490  DrawLineIntoImage(pImage, vHandCenter2D.x, vHandCenter2D.y+1, vProjectedPoint.x, vProjectedPoint.y+1, 0, 0, 255);
491  DrawLineIntoImage(pImage, vHandCenter2D.x+1, vHandCenter2D.y+1, vProjectedPoint.x+1, vProjectedPoint.y+1, 0, 0, 255);
492  }
493  */
494 
495 
496  void
497  CHandModelVisualizer::DrawHandModelV2(CByteImage* pImage, bool bLeftCameraImage)
498  {
499  const CCalibration* pCalibration = bLeftCameraImage
500  ? m_pStereoCalibration->GetLeftCalibration()
501  : m_pStereoCalibration->GetRightCalibration();
502 
503  // fingertips
504  if (bLeftCameraImage)
505  {
506  for (int i = 0; i < 5; i++)
507  {
508  DrawPolygon(pImage, m_pHandModelV2->m_aFingerTipPolygonsLeftCam.at(i));
509  }
510  }
511  else
512  {
513  for (int i = 0; i < 5; i++)
514  {
515  DrawPolygon(pImage, m_pHandModelV2->m_aFingerTipPolygonsRightCam.at(i));
516  }
517  }
518 
519 
520  // finger kinematics
521 
522  Vec2d vProjectedPoint1, vProjectedPoint2;
523 
524  pCalibration->WorldToImageCoordinates(
525  m_pHandModelV2->m_vHandPosition, vProjectedPoint1, false);
526 
527  // lines from center to finger bases
528  for (int i = 0; i < 5; i++)
529  {
530  pCalibration->WorldToImageCoordinates(
531  m_pHandModelV2->m_aFingerJointsInWorldCS.at(i).at(0), vProjectedPoint2, false);
532  DrawLineIntoImage(pImage,
533  vProjectedPoint1.x,
534  vProjectedPoint1.y,
535  vProjectedPoint2.x,
536  vProjectedPoint2.y,
537  50 * i,
538  50 * i,
539  255 - 50 * i);
540  }
541 
542  // lines along the finger segments
543  for (int i = 0; i < 5; i++)
544  {
545  pCalibration->WorldToImageCoordinates(
546  m_pHandModelV2->m_aFingerJointsInWorldCS.at(i).at(0), vProjectedPoint1, false);
547  for (size_t j = 1; j < m_pHandModelV2->m_aFingerJointsInWorldCS.at(i).size(); j++)
548  {
549  pCalibration->WorldToImageCoordinates(
550  m_pHandModelV2->m_aFingerJointsInWorldCS.at(i).at(j), vProjectedPoint2, false);
551  DrawLineIntoImage(pImage,
552  vProjectedPoint1.x,
553  vProjectedPoint1.y,
554  vProjectedPoint2.x,
555  vProjectedPoint2.y,
556  50 * j,
557  255 - 50 * i,
558  50 * i);
559  Math2d::SetVec(vProjectedPoint1, vProjectedPoint2);
560  }
561  }
562 
563 
564  // draw circle for tracking ball
565  double dCenterX, dCenterY, dRadius;
566  if (bLeftCameraImage)
567  {
568  dCenterX = m_pHandModelV2->m_vTrackingBallPosLeftCam.x;
569  dCenterY = m_pHandModelV2->m_vTrackingBallPosLeftCam.y;
570  dRadius = m_pHandModelV2->m_fTrackingBallRadiusLeftCam;
571  }
572  else
573  {
574  dCenterX = m_pHandModelV2->m_vTrackingBallPosRightCam.x;
575  dCenterY = m_pHandModelV2->m_vTrackingBallPosRightCam.y;
576  dRadius = m_pHandModelV2->m_fTrackingBallRadiusRightCam;
577  }
578 
579  int up = (int)(dCenterY - dRadius);
580  int down = (int)(dCenterY + dRadius);
581  for (int i = up; i <= down; i++)
582  {
583  int leftBorderInt =
584  (int)(dCenterX - sqrt(dRadius * dRadius - (i - dCenterY) * (i - dCenterY)));
585  int rightBorderInt =
586  (int)(dCenterX + sqrt(dRadius * dRadius - (i - dCenterY) * (i - dCenterY)));
587  for (int j = leftBorderInt; j <= rightBorderInt; j++)
588  {
589  if (i >= 0 && i < DSHT_IMAGE_HEIGHT && j >= 0 && j < DSHT_IMAGE_WIDTH)
590  {
591  pImage->pixels[3 * (i * DSHT_IMAGE_WIDTH + j) + 0] = 0;
592  pImage->pixels[3 * (i * DSHT_IMAGE_WIDTH + j) + 1] = 255;
593  pImage->pixels[3 * (i * DSHT_IMAGE_WIDTH + j) + 2] = 0;
594  }
595  }
596  }
597  }
598 
599  void
600  CHandModelVisualizer::DrawHand(CByteImage* pImage)
601  {
602  int principal_point_x = (int)(m_fPrincipalPointX + 0.5);
603  int principal_point_y = (int)(m_fPrincipalPointY + 0.5);
604  unsigned char* pRenderedImage =
605  m_pMoveMaster->m_pOIFwdKinematicsInterface->m_pOffscreenRenderer->getBuffer();
606  int tempIndex, tempIndexX, tempIndexY;
607  int renderSizeX = DSHT_OI_RENDERSIZE_X;
608  int renderSizeY = DSHT_OI_RENDERSIZE_Y;
609 
610  if (!pRenderedImage)
611  {
612  ARMARX_WARNING_S << "pRenderedImage is NULL" << armarx::flush;
613  return;
614  }
615 
616  for (int i = 0; i < renderSizeY; i++)
617  {
618  for (int j = 0; j < renderSizeX; j++)
619  {
620  // if pixel is not red (red => background), draw it into the image
621  if (pRenderedImage[3 * (renderSizeX * (renderSizeY - i) + j)] < 255 ||
622  pRenderedImage[3 * (renderSizeX * (renderSizeY - i) + j) + 1] > 0 ||
623  pRenderedImage[3 * (renderSizeX * (renderSizeY - i) + j) + 2] > 0)
624  {
625  tempIndexX = j - (renderSizeX / 2) + principal_point_x;
626  tempIndexY = i - (renderSizeY / 2) + principal_point_y;
627  if (0 <= tempIndexX && tempIndexX < DSHT_IMAGE_WIDTH && 0 <= tempIndexY &&
628  tempIndexY < DSHT_IMAGE_HEIGHT)
629  {
630  tempIndex = 3 * (DSHT_IMAGE_WIDTH * tempIndexY + tempIndexX);
631 
632  //if (tempIndex >= 0 && tempIndex<3*DSHT_IMAGE_WIDTH*DSHT_IMAGE_HEIGHT)
633  {
634  pImage->pixels[tempIndex + 0] =
635  0.4 *
636  pRenderedImage[3 * (renderSizeX * (renderSizeY - i) + j) + 0] +
637  0.6 * pImage->pixels[tempIndex + 0];
638  pImage->pixels[tempIndex + 1] =
639  0.4 *
640  pRenderedImage[3 * (renderSizeX * (renderSizeY - i) + j) + 1] +
641  0.6 * pImage->pixels[tempIndex + 1];
642  pImage->pixels[tempIndex + 2] =
643  0.4 *
644  pRenderedImage[3 * (renderSizeX * (renderSizeY - i) + j) + 2] +
645  0.6 * pImage->pixels[tempIndex + 2];
646  }
647  }
648  }
649  }
650  }
651  }
652 
653  void
654  CHandModelVisualizer::DrawSegmentedImage(CByteImage* pImage, bool bUseOpenInventorModel)
655  {
656  if (!bUseOpenInventorModel)
657  {
658  DrawSegmentedImageWithoutOpenInventor(pImage);
659  }
660  else
661  {
662  unsigned char* pRenderedImage =
663  m_pMoveMaster->m_pOIFwdKinematicsInterface->m_pOffscreenRenderer->getBuffer();
664 
665  if (!pRenderedImage)
666  {
667  ARMARX_WARNING_S << "pRenderedImage is NULL" << armarx::flush;
668  DrawSegmentedImageWithoutOpenInventor(pImage);
669  }
670  else
671  {
672  // reset image
673  ::ImageProcessor::Zero(pImage);
674 
675  int principal_point_x = (int)(m_fPrincipalPointX + 0.5);
676  int principal_point_y = (int)(m_fPrincipalPointY + 0.5);
677  int tempIndex, tempIndexX, tempIndexY;
678  int renderSizeX = DSHT_OI_RENDERSIZE_X;
679  int renderSizeY = DSHT_OI_RENDERSIZE_Y;
680 
681  for (int i = 0; i < renderSizeY; i++)
682  {
683  for (int j = 0; j < renderSizeX; j++)
684  {
685  // check if pixel is not background
686  if (pRenderedImage[3 * (renderSizeX * (renderSizeY - i) + j)] < 250 ||
687  pRenderedImage[3 * (renderSizeX * (renderSizeY - i) + j) + 1] > 10 ||
688  pRenderedImage[3 * (renderSizeX * (renderSizeY - i) + j) + 2] > 10)
689  {
690  tempIndexX = j - (renderSizeX / 2) + principal_point_x;
691  tempIndexY = i - (renderSizeY / 2) + principal_point_y;
692  // check if pixel is inside the image
693  if ((tempIndexX >= 0) && (tempIndexX < DSHT_IMAGE_WIDTH) &&
694  (tempIndexY >= 0) && (tempIndexY < DSHT_IMAGE_HEIGHT))
695  {
696  tempIndex = DSHT_IMAGE_WIDTH * tempIndexY + tempIndexX;
697  if (tempIndex >= 0 &&
698  tempIndex < DSHT_IMAGE_WIDTH * DSHT_IMAGE_HEIGHT)
699  {
700  // check for object color
701  if (pRenderedImage[3 * (renderSizeX * (renderSizeY - i) + j)] >
702  250 &&
703  pRenderedImage[3 * (renderSizeX * (renderSizeY - i) + j) +
704  1] > 250 &&
705  pRenderedImage[3 * (renderSizeX * (renderSizeY - i) + j) +
706  2] < 10)
707  {
708  pImage->pixels[tempIndex] = 42;
709  }
710  // check for arm color
711  else if (pRenderedImage[3 * (renderSizeX * (renderSizeY - i) +
712  j)] < 10 &&
713  pRenderedImage[3 * (renderSizeX * (renderSizeY - i) +
714  j) +
715  1] > 250 &&
716  pRenderedImage[3 * (renderSizeX * (renderSizeY - i) +
717  j) +
718  2] > 250)
719  {
720  pImage->pixels[tempIndex] = 128;
721  }
722  // otherwise it must be the hand
723  else
724  {
725  pImage->pixels[tempIndex] = 255;
726  }
727  }
728  }
729  }
730  }
731  }
732 
733  if (pImage->type == CByteImage::eRGB24)
734  {
735  for (int i = DSHT_IMAGE_WIDTH * DSHT_IMAGE_HEIGHT - 1; i >= 0; i--)
736  {
737  pImage->pixels[3 * i] = pImage->pixels[i];
738  }
739  for (int i = 0; i < DSHT_IMAGE_WIDTH * DSHT_IMAGE_HEIGHT; i++)
740  {
741  pImage->pixels[3 * i + 1] = pImage->pixels[3 * i];
742  pImage->pixels[3 * i + 2] = pImage->pixels[3 * i];
743  }
744  }
745  }
746  }
747  }
748 
749  void
750  CHandModelVisualizer::DrawSegmentedImageWithoutOpenInventor(CByteImage* pSegmentedImage)
751  {
752  CByteImage* pImage;
753  if (pSegmentedImage->type == CByteImage::eRGB24)
754  {
755  pImage = new CByteImage(
756  pSegmentedImage->width, pSegmentedImage->height, CByteImage::eGrayScale);
757  }
758  else
759  {
760  pImage = pSegmentedImage;
761  }
762  ::ImageProcessor::Zero(pImage);
763 
764  // fingertips
765  for (int i = 0; i < 5; i++)
766  {
767  DrawPolygonGrayscale(pImage, m_pHandModelV2->m_aFingerTipPolygonsLeftCam.at(i));
768  }
769 
770  const float fFingerWidth = 25.0f;
771 
772  // lines from center to finger bases
773  for (int i = 0; i < 5; i++)
774  {
775  DrawQuadrangleAlongA3DLine(pImage,
776  m_pHandModelV2->m_vHandPosition,
777  m_pHandModelV2->m_aFingerJointsInWorldCS.at(i).at(0),
778  fFingerWidth,
779  fFingerWidth);
780  }
781 
782  // lines along the finger segments
783  for (int i = 0; i < 5; i++)
784  {
785  for (size_t j = 0; j < m_pHandModelV2->m_aFingerJointsInWorldCS.at(i).size() - 1; j++)
786  {
787  DrawQuadrangleAlongA3DLine(pImage,
788  m_pHandModelV2->m_aFingerJointsInWorldCS.at(i).at(j),
789  m_pHandModelV2->m_aFingerJointsInWorldCS.at(i).at(j + 1),
790  fFingerWidth,
791  fFingerWidth);
792  }
793  }
794 
795  // draw circle for tracking ball
796  double dCenterX, dCenterY, dRadius;
797  dCenterX = m_pHandModelV2->m_vTrackingBallPosLeftCam.x;
798  dCenterY = m_pHandModelV2->m_vTrackingBallPosLeftCam.y;
799  dRadius = m_pHandModelV2->m_fTrackingBallRadiusLeftCam;
800 
801  int up = (int)(dCenterY - dRadius);
802  int down = (int)(dCenterY + dRadius);
803  for (int i = up; i <= down; i++)
804  {
805  int leftBorderInt =
806  (int)(dCenterX - sqrt(dRadius * dRadius - (i - dCenterY) * (i - dCenterY)));
807  int rightBorderInt =
808  (int)(dCenterX + sqrt(dRadius * dRadius - (i - dCenterY) * (i - dCenterY)));
809  for (int j = leftBorderInt; j <= rightBorderInt; j++)
810  {
811  if (i >= 0 && i < DSHT_IMAGE_HEIGHT && j >= 0 && j < DSHT_IMAGE_WIDTH)
812  {
813  pImage->pixels[i * DSHT_IMAGE_WIDTH + j] = 255;
814  }
815  }
816  }
817 
818  // arm
819  Vec3d vArmLength = {0, 0, -500};
820  Vec3d vArmStart;
821  Math3d::MulMatVec(m_pHandModelV2->m_mHandOrientation,
822  vArmLength,
823  m_pHandModelV2->m_vHandPosition,
824  vArmStart);
825  DrawQuadrangleAlongA3DLine(pImage, vArmStart, m_pHandModelV2->m_vHandPosition, 200, 100);
826 
827  CByteImage* pTempImage = new CByteImage(pImage);
828  //timeval tStart, tEnd;
829  //long tTimeDiff;
830  //gettimeofday(&tStart, 0);
831  ::ImageProcessor::Dilate(pImage, pTempImage, 14);
832  ::ImageProcessor::Dilate(pTempImage, pImage, 14);
833  //gettimeofday(&tEnd, 0);
834  //tTimeDiff = (1000*tEnd.tv_sec+tEnd.tv_usec/1000) - (1000*tStart.tv_sec+tStart.tv_usec/1000);
835  //ARMARX_VERBOSE_S << "Time for dilate: " << tTimeDiff << " ms";
836  delete pTempImage;
837 
838  // return result
839  if (pSegmentedImage->type == CByteImage::eRGB24)
840  {
841  for (int i = 0; i < pSegmentedImage->width * pSegmentedImage->height; i++)
842  {
843  pSegmentedImage->pixels[3 * i + 0] = pImage->pixels[i];
844  pSegmentedImage->pixels[3 * i + 1] = pImage->pixels[i];
845  pSegmentedImage->pixels[3 * i + 2] = pImage->pixels[i];
846  }
847  }
848  }
849 
850  void
851  CHandModelVisualizer::DrawCross(CByteImage* pGreyImage, int x, int y, int nBrightness)
852  {
853  if (x > 2 && y > 2 && x < DSHT_IMAGE_WIDTH - 3 && y < DSHT_IMAGE_HEIGHT - 3)
854  {
855  for (int j = -3; j <= 3; j++)
856  {
857  pGreyImage->pixels[(y + j) * DSHT_IMAGE_WIDTH + x] = nBrightness;
858  pGreyImage->pixels[y * DSHT_IMAGE_WIDTH + x + j] = nBrightness;
859  }
860  }
861  }
862 
863  void
864  CHandModelVisualizer::DrawCross(CByteImage* pColorImage, int x, int y, int r, int g, int b)
865  {
866  if (x > 2 && y > 2 && x < DSHT_IMAGE_WIDTH - 3 && y < DSHT_IMAGE_HEIGHT - 3)
867  {
868  for (int j = -3; j <= 3; j++)
869  {
870  pColorImage->pixels[3 * ((y + j) * DSHT_IMAGE_WIDTH + x)] = r;
871  pColorImage->pixels[3 * ((y + j) * DSHT_IMAGE_WIDTH + x) + 1] = g;
872  pColorImage->pixels[3 * ((y + j) * DSHT_IMAGE_WIDTH + x) + 2] = b;
873 
874  pColorImage->pixels[3 * (y * DSHT_IMAGE_WIDTH + x + j)] = r;
875  pColorImage->pixels[3 * (y * DSHT_IMAGE_WIDTH + x + j) + 1] = g;
876  pColorImage->pixels[3 * (y * DSHT_IMAGE_WIDTH + x + j) + 2] = b;
877  }
878  }
879  }
880 
881  void
882  CHandModelVisualizer::DrawLineIntoImage(CByteImage* pImage,
883  int x1,
884  int y1,
885  int x2,
886  int y2,
887  int red,
888  int green,
889  int blue)
890  {
891  if (x1 < 0 || x1 >= pImage->width || x2 < 0 || x2 >= pImage->width || y1 < 0 ||
892  y1 >= pImage->height || y2 < 0 || y2 >= pImage->height)
893  {
894  return;
895  }
896 
897  if (x1 == x2 && y1 == y2)
898  {
899  pImage->pixels[3 * (y1 * pImage->width + x1)] = red;
900  pImage->pixels[3 * (y1 * pImage->width + x1) + 1] = green;
901  pImage->pixels[3 * (y1 * pImage->width + x1) + 2] = blue;
902  }
903  else
904  {
905  int temp;
906 
907  if (x1 > x2)
908  {
909  temp = x1;
910  x1 = x2;
911  x2 = temp;
912  temp = y1;
913  y1 = y2;
914  y2 = temp;
915  }
916 
917  float divided_diff = (float)(y2 - y1) / (float)(x2 - x1);
918 
919  for (int i = x1; i < x2; i++)
920  {
921  pImage->pixels[3 * (((int)((i - x1) * divided_diff) + y1) * pImage->width + i)] =
922  red;
923  pImage
924  ->pixels[3 * (((int)((i - x1) * divided_diff) + y1) * pImage->width + i) + 1] =
925  green;
926  pImage
927  ->pixels[3 * (((int)((i - x1) * divided_diff) + y1) * pImage->width + i) + 2] =
928  blue;
929  }
930 
931 
932  if (y1 > y2)
933  {
934  temp = x1;
935  x1 = x2;
936  x2 = temp;
937  temp = y1;
938  y1 = y2;
939  y2 = temp;
940  }
941 
942 
943  divided_diff = (float)(x2 - x1) / (float)(y2 - y1);
944 
945  for (int i = y1; i < y2; i++)
946  {
947  pImage->pixels[3 * (i * pImage->width + x1 + (int)((i - y1) * divided_diff))] = red;
948  pImage->pixels[3 * (i * pImage->width + x1 + (int)((i - y1) * divided_diff)) + 1] =
949  green;
950  pImage->pixels[3 * (i * pImage->width + x1 + (int)((i - y1) * divided_diff)) + 2] =
951  blue;
952  }
953  }
954  }
955 
956  void
957  CHandModelVisualizer::DrawQuadrangleAlongA3DLine(CByteImage* pImage,
958  Vec3d vStart,
959  Vec3d vEnd,
960  float fWidthAtStart,
961  float fWidthAtEnd)
962  {
963  Vec3d vDirection;
964  Math3d::SubtractVecVec(vEnd, vStart, vDirection);
965  Vec3d vOrthogonalXY = {-vDirection.y, vDirection.x, 0};
966  if (Math3d::Length(vOrthogonalXY) == 0)
967  {
968  return;
969  }
970  Math3d::NormalizeVec(vOrthogonalXY);
971  Vec3d vOrthogonalStart, vOrthogonalEnd;
972  Math3d::MulVecScalar(vOrthogonalXY, 0.5f * fWidthAtStart, vOrthogonalStart);
973  Math3d::MulVecScalar(vOrthogonalXY, 0.5f * fWidthAtEnd, vOrthogonalEnd);
974 
975  Vec3d p1, p2, p3, p4;
976  Math3d::AddVecVec(vStart, vOrthogonalStart, p1);
977  Math3d::SubtractVecVec(vStart, vOrthogonalStart, p2);
978  Math3d::AddVecVec(vEnd, vOrthogonalEnd, p3);
979  Math3d::SubtractVecVec(vEnd, vOrthogonalEnd, p4);
980 
981  Vec2d vProjPoint1, vProjPoint2, vProjPoint3, vProjPoint4;
982  m_pStereoCalibration->GetLeftCalibration()->WorldToImageCoordinates(p1, vProjPoint1, false);
983  m_pStereoCalibration->GetLeftCalibration()->WorldToImageCoordinates(p2, vProjPoint2, false);
984  m_pStereoCalibration->GetLeftCalibration()->WorldToImageCoordinates(p3, vProjPoint3, false);
985  m_pStereoCalibration->GetLeftCalibration()->WorldToImageCoordinates(p4, vProjPoint4, false);
986 
987  ConvexPolygonCalculations::Polygon pProjectedQuadrangle, pImageCorners,
988  pIntersectingPolygon;
989  Vec3d pPoints[4];
990  pPoints[0].x = vProjPoint1.x;
991  pPoints[0].y = vProjPoint1.y;
992  pPoints[1].x = vProjPoint2.x;
993  pPoints[1].y = vProjPoint2.y;
994  pPoints[2].x = vProjPoint3.x;
995  pPoints[2].y = vProjPoint3.y;
996  pPoints[3].x = vProjPoint4.x;
997  pPoints[3].y = vProjPoint4.y;
998  ConvexPolygonCalculations::CreateConvexPolygonFromHullPoints(
999  pPoints, 4, &pProjectedQuadrangle);
1000 
1001  pPoints[0].x = 0;
1002  pPoints[0].y = 0;
1003  pPoints[1].x = DSHT_IMAGE_WIDTH - 1;
1004  pPoints[1].y = 1;
1005  pPoints[2].x = DSHT_IMAGE_WIDTH - 2;
1006  pPoints[2].y = DSHT_IMAGE_HEIGHT - 1;
1007  pPoints[3].x = 1;
1008  pPoints[3].y = DSHT_IMAGE_HEIGHT - 2;
1009  ConvexPolygonCalculations::CreateConvexPolygonFromHullPoints(pPoints, 4, &pImageCorners);
1010 
1011  ConvexPolygonCalculations::GetPolygonIntersection(
1012  &pProjectedQuadrangle, &pImageCorners, &pIntersectingPolygon);
1013 
1014  if (pImage->type == CByteImage::eGrayScale)
1015  {
1016  DrawPolygonGrayscale(pImage, pIntersectingPolygon);
1017  }
1018  else
1019  {
1020  DrawPolygon(pImage, pIntersectingPolygon);
1021  }
1022  }
1023 
1024  void
1025  CHandModelVisualizer::ExtractAnglesFromRotationMatrix(const Mat3d& mat, Vec3d& angles)
1026  {
1027  angles.y = asin(mat.r3);
1028  angles.x = atan2((-mat.r6), (mat.r9));
1029  angles.z = atan2((-mat.r2), (mat.r1));
1030  }
1031 } // namespace visionx
armarx::view_selection::skills::direction::state::up
state::Type up(state::Type previous)
Definition: LookDirection.cpp:240
visionx::CHandModelV2
Definition: HandModelV2.h:36
DSHT_OI_RENDERSIZE_Y
#define DSHT_OI_RENDERSIZE_Y
Definition: HandLocalisationConstants.h:131
visionx::CMoveMasterModel::m_pOIFwdKinematicsInterface
OIFwdKinematicsInterface * m_pOIFwdKinematicsInterface
Definition: MoveMasterModel.h:48
GfxTL::Vec3d
VectorXD< 3, double > Vec3d
Definition: VectorXD.h:737
visionx
ArmarX headers.
Definition: OpenPoseStressTest.h:38
visionx::CMoveMasterModel
Definition: MoveMasterModel.h:32
visionx::ConvexPolygonCalculations::Polygon::hullLeft
Vec3d hullLeft[2 *DSHT_MAX_POLYGON_CORNERS+1]
Definition: Polygon.h:49
visionx::HandModeliCub
Definition: HandModeliCub.h:30
visionx::CMoveMasterModel::setObjCylConfig
void setObjCylConfig(Vec3d pos, double alpha, double beta, double gamma, float height, float radius)
Definition: MoveMasterModel.cpp:233
visionx::CMoveMasterModel::setHandPosition
void setHandPosition(Vec3d pos)
Definition: MoveMasterModel.cpp:225
DSHT_IMAGE_WIDTH
#define DSHT_IMAGE_WIDTH
Definition: HandLocalisationConstants.h:73
visionx::ConvexPolygonCalculations::GetPolygonIntersection
void GetPolygonIntersection(Polygon *p1, Polygon *p2, Polygon *pInter, Vec3d *pointAccu, bool *boolTable, Vec3d *clockwiseHullPoly1, Vec3d *clockwiseHullPoly2)
Definition: Polygon.cpp:428
visionx::ConvexPolygonCalculations::Polygon::hull
Vec3d hull[2 *DSHT_MAX_POLYGON_CORNERS]
Definition: Polygon.h:37
DSHT_OI_FILE_PATH
#define DSHT_OI_FILE_PATH
Definition: HandLocalisationConstants.h:55
visionx::ConvexPolygonCalculations::Polygon::nCorners
int nCorners
Definition: Polygon.h:34
visionx::ConvexPolygonCalculations::Polygon
Definition: Polygon.h:32
visionx::CHandModelVisualizer::m_fFocalLengthY
float m_fFocalLengthY
Definition: HandModelVisualizer.h:99
visionx::CHandModelVisualizer::m_fFocalLengthX
float m_fFocalLengthX
Definition: HandModelVisualizer.h:98
visionx::CHandModelV2::m_aFingerTipPolygonsRightCam
std::vector< ConvexPolygonCalculations::Polygon > m_aFingerTipPolygonsRightCam
Definition: HandModelV2.h:77
DSHT_HAND_MODEL_PATH
#define DSHT_HAND_MODEL_PATH
Definition: HandLocalisationConstants.h:67
visionx::ConvexPolygonCalculations::CreateConvexPolygonFromHullPoints
void CreateConvexPolygonFromHullPoints(Vec3d *hullpoints, int nPoints, Polygon *pol)
Definition: Polygon.cpp:340
visionx::ConvexPolygonCalculations::Polygon::hullRight
Vec3d hullRight[2 *DSHT_MAX_POLYGON_CORNERS+1]
Definition: Polygon.h:52
visionx::CHandModelV2::m_vTrackingBallPosRightCam
Vec2d m_vTrackingBallPosRightCam
Definition: HandModelV2.h:79
HandModelVisualizer.h
visionx::CHandModelVisualizer::DrawLineIntoImage
static void DrawLineIntoImage(CByteImage *pImage, int x1, int y1, int x2, int y2, int red, int green, int blue)
Definition: HandModelVisualizer.cpp:882
visionx::CHandModelVisualizer::DrawHand
void DrawHand(CByteImage *pImage)
Definition: HandModelVisualizer.cpp:600
visionx::CHandModelVisualizer::m_fPrincipalPointX
float m_fPrincipalPointX
Definition: HandModelVisualizer.h:100
visionx::OIFwdKinematicsInterface::m_pOffscreenRenderer
SoOffscreenRenderer * m_pOffscreenRenderer
Definition: OIFwdKinematicsInterface.h:80
visionx::CHandModelV2::m_aFingerJointsInWorldCS
std::vector< std::vector< Vec3d > > m_aFingerJointsInWorldCS
Definition: HandModelV2.h:70
M_PI
#define M_PI
Definition: MathTools.h:17
armarx::flush
const LogSender::manipulator flush
Definition: LogSender.h:251
visionx::CHandModelVisualizer::DrawSegmentedImageWithoutOpenInventor
void DrawSegmentedImageWithoutOpenInventor(CByteImage *pSegmentedImage)
Definition: HandModelVisualizer.cpp:750
visionx::CHandModelVisualizer::DrawSegmentedImage
void DrawSegmentedImage(CByteImage *pImage, bool bUseOpenInventorModel=true)
Definition: HandModelVisualizer.cpp:654
visionx::CHandModelV2::m_vHandPosition
Vec3d m_vHandPosition
Definition: HandModelV2.h:73
visionx::CHandModelVisualizer::DrawQuadrangleAlongA3DLine
void DrawQuadrangleAlongA3DLine(CByteImage *pImage, Vec3d vStart, Vec3d vEnd, float fWidthAtStart, float fWidthAtEnd)
Definition: HandModelVisualizer.cpp:957
visionx::CHandModelVisualizer::m_pStereoCalibration
CStereoCalibration * m_pStereoCalibration
Definition: HandModelVisualizer.h:97
visionx::CHandModelVisualizer::UpdateHandModel
void UpdateHandModel(double *pConfig, bool bUpdateOpenInventorModel=true, bool bDrawCylinderInHand=false)
Definition: HandModelVisualizer.cpp:94
visionx::CHandModelV2::m_vTrackingBallPosLeftCam
Vec2d m_vTrackingBallPosLeftCam
Definition: HandModelV2.h:79
visionx::CHandModelVisualizer::~CHandModelVisualizer
~CHandModelVisualizer()
Definition: HandModelVisualizer.cpp:84
ARMARX_WARNING_S
#define ARMARX_WARNING_S
Definition: Logging.h:213
visionx::CMoveMasterModel::update
void update()
Definition: MoveMasterModel.cpp:129
armarx::red
QColor red()
Definition: StyleSheets.h:78
GfxTL::Vec2d
VectorXD< 2, double > Vec2d
Definition: VectorXD.h:736
visionx::CHandModelV2::m_fTrackingBallRadiusLeftCam
float m_fTrackingBallRadiusLeftCam
Definition: HandModelV2.h:80
GfxTL::sqrt
VectorXD< D, T > sqrt(const VectorXD< D, T > &a)
Definition: VectorXD.h:704
visionx::CHandModelV2::m_mHandOrientation
Mat3d m_mHandOrientation
Definition: HandModelV2.h:74
float
#define float
Definition: 16_Level.h:22
visionx::CHandModelVisualizer::DrawPolygonGrayscale
void DrawPolygonGrayscale(CByteImage *pImage, ConvexPolygonCalculations::Polygon &pPolygon)
Definition: HandModelVisualizer.cpp:334
visionx::CHandModelVisualizer::ConfigureMoveMasterModel
void ConfigureMoveMasterModel(double *pConfig, bool bDrawCylinderInHand)
Definition: HandModelVisualizer.cpp:109
visionx::CHandModelVisualizer::ExtractAnglesFromRotationMatrix
void ExtractAnglesFromRotationMatrix(const Mat3d &mat, Vec3d &angles)
Definition: HandModelVisualizer.cpp:1025
armarx::view_selection::skills::direction::state::down
state::Type down(state::Type previous)
Definition: LookDirection.cpp:246
visionx::CMoveMasterModel::init
bool init(std::string sFilename, float fFocalLengthY)
Definition: MoveMasterModel.cpp:74
visionx::CHandModelVisualizer::DrawPolygon
void DrawPolygon(CByteImage *pImage, ConvexPolygonCalculations::Polygon &pPolygon)
Definition: HandModelVisualizer.cpp:248
visionx::CHandModelVisualizer::CHandModelVisualizer
CHandModelVisualizer(CStereoCalibration *pCalibration, bool bUseLeftCamera=true)
Definition: HandModelVisualizer.cpp:38
visionx::CHandModelV2::m_fTrackingBallRadiusRightCam
float m_fTrackingBallRadiusRightCam
Definition: HandModelV2.h:80
visionx::CHandModelVisualizer::m_fPrincipalPointY
float m_fPrincipalPointY
Definition: HandModelVisualizer.h:101
visionx::CHandModelVisualizer::DrawHandModelV2
void DrawHandModelV2(CByteImage *pImage, bool bLeftCameraImage=true)
Definition: HandModelVisualizer.cpp:497
Logging.h
visionx::CHandModelVisualizer::m_pMoveMaster
CMoveMasterModel * m_pMoveMaster
Definition: HandModelVisualizer.h:95
visionx::CMoveMasterModel::setJointAngle
void setJointAngle(int nJoint, float fAngleRad)
Definition: MoveMasterModel.cpp:144
DSHT_IMAGE_HEIGHT
#define DSHT_IMAGE_HEIGHT
Definition: HandLocalisationConstants.h:74
visionx::CHandModelV2::UpdateHand
virtual void UpdateHand(double *pConfig)
Definition: HandModelV2.cpp:394
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:27
visionx::CHandModelVisualizer::m_pHandModelV2
CHandModelV2 * m_pHandModelV2
Definition: HandModelVisualizer.h:94
visionx::CHandModelV2::m_aFingerTipPolygonsLeftCam
std::vector< ConvexPolygonCalculations::Polygon > m_aFingerTipPolygonsLeftCam
Definition: HandModelV2.h:76
armarx::green
QColor green()
Definition: StyleSheets.h:72
visionx::CMoveMasterModel::setHandRotation
void setHandRotation(double alpha, double beta, double gamma)
Definition: MoveMasterModel.cpp:191
DSHT_OI_RENDERSIZE_X
#define DSHT_OI_RENDERSIZE_X
Definition: HandLocalisationConstants.h:130
visionx::CHandModelVisualizer::DrawCross
static void DrawCross(CByteImage *pGreyImage, int x, int y, int nBrightness)
Definition: HandModelVisualizer.cpp:851