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