HandMarkerLocalization.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 VisionX::Component
19  * @author David Schiebener (schiebener at kit dot edu)
20  * @date 2013
21  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
22  * GNU General Public License
23  */
24 
25 #include "HandMarkerLocalization.h"
26 
27 // Core
28 #include <ArmarXCore/core/system/ArmarXDataPath.h>
29 
30 #include <RobotAPI/libraries/core/Pose.h>
31 #include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
32 
33 // MemoryX
34 #include <MemoryX/core/entity/Entity.h>
35 #include <MemoryX/core/entity/ProbabilityMeasures.h>
36 #include <MemoryX/libraries/helpers/ObjectRecognitionHelpers/ObjectRecognitionWrapper.h>
37 
38 // IVT
39 #include <ArmarXCore/observers/variant/TimestampVariant.h>
40 
41 #include <Calibration/Calibration.h>
42 #include <Color/ColorParameterSet.h>
43 #include <Image/ImageProcessor.h>
44 
45 // IVTRecognition
46 #include <Locators/VisualTargetLocator.h>
47 
48 namespace visionx
49 {
50 
51 
52  HandMarkerLocalization::HandMarkerLocalization()
53  {
54  numberOfResultImages = 3;
55  }
56 
57  void
58  HandMarkerLocalization::onInitObjectLocalizerProcessor()
59  {
60  usingProxy(getProperty<std::string>("RobotStateComponentName").getValue());
61  tcpNameLeft = getProperty<std::string>("TCPNameLeft").getValue();
62  tcpNameRight = getProperty<std::string>("TCPNameRight").getValue();
63  markerNameLeft = getProperty<std::string>("MarkerNameLeft").getValue();
64  markerNameRight = getProperty<std::string>("MarkerNameRight").getValue();
65  useVision = getProperty<bool>("UseVision").getValue();
66  }
67 
68  void
69  HandMarkerLocalization::onConnectObjectLocalizerProcessor()
70  {
71  robotStateComponent = getProxy<armarx::RobotStateComponentInterfacePrx>(
72  getProperty<std::string>("RobotStateComponentName").getValue());
73  localRobot = armarx::RemoteRobot::createLocalClone(robotStateComponent);
74  }
75 
76  bool
77  HandMarkerLocalization::initRecognizer()
78  {
79  if (useVision)
80  {
81  std::string colorParameterFilename =
82  getProperty<std::string>("ColorParameterFile").getValue();
83 
84  if (!armarx::ArmarXDataPath::getAbsolutePath(colorParameterFilename,
85  colorParameterFilename))
86  {
87  ARMARX_ERROR << "Could not find color parameter file in ArmarXDataPath: "
88  << colorParameterFilename;
89  return false;
90  }
91 
92  ARMARX_VERBOSE << "Color parameter file name: " << colorParameterFilename;
93 
94  visualTargetLocator = new CVisualTargetLocator();
95 
96  if (!visualTargetLocator->Init(colorParameterFilename.c_str(), getStereoCalibration()))
97  {
98  return false;
99  }
100 
101  colorParameterSet = new CColorParameterSet();
102  colorParameterSet->LoadFromFile(colorParameterFilename.c_str());
103  }
104 
105  showSegmentedResultImages = getProperty<bool>("ShowSegmentedResultImages").getValue();
106 
107  if (!showSegmentedResultImages)
108  {
109  ::ImageProcessor::Zero(resultImages[2]);
110  }
111 
112  return true;
113  }
114 
115  bool
116  HandMarkerLocalization::addObjectClass(const memoryx::EntityPtr& objectClassEntity,
117  const memoryx::GridFileManagerPtr& fileManager)
118  {
119  ARMARX_VERBOSE << "Adding object class " << objectClassEntity->getName();
120 
121  memoryx::EntityWrappers::HandMarkerBallWrapperPtr recognitionWrapper =
122  objectClassEntity->addWrapper(
123  new memoryx::EntityWrappers::HandMarkerBallWrapper(fileManager));
124  markerColors[objectClassEntity->getName()] = recognitionWrapper->getObjectColor();
125 
126  return true;
127  }
128 
129  memoryx::ObjectLocalizationResultList
130  HandMarkerLocalization::localizeObjectClasses(const std::vector<std::string>& objectClassNames,
131  CByteImage** cameraImages,
132  armarx::MetaInfoSizeBasePtr imageMetaInfo,
133  CByteImage** resultImages)
134  {
135  // there will be only a valid timestamp in the metainfo if vision is used
136  auto timestamp =
137  useVision ? imageMetaInfo->timeProvided : armarx::TimeUtil::GetTime().toMicroSeconds();
138 
139  const float recognitionCertainty = 0.8f;
140  const float uncertaintyWhenReturningKinematicPosition = 10000;
141  const float maxDistToFwdKinPose =
142  getProperty<float>("MaximalAcceptedDistanceFromForwardKinematics").getValue();
143  if (!armarx::RemoteRobot::synchronizeLocalCloneToTimestamp(
144  localRobot, robotStateComponent, timestamp))
145  {
146  ARMARX_WARNING << deactivateSpam(1)
147  << "Failed to synchronize local robot - results are probably wrong";
148  }
149  const auto agentName = localRobot->getName();
150  std::string firstObjectName = objectClassNames.at(0);
151  bool firstEntryIsLeft = firstObjectName.find("left") != std::string::npos ||
152  firstObjectName.find("Left") != std::string::npos;
153  // get poses of hands and markers
154  Eigen::Matrix4f leftHandPose = localRobot->getRobotNode(tcpNameLeft)->getPoseInRootFrame();
155  Eigen::Matrix4f rightHandPose =
156  localRobot->getRobotNode(tcpNameRight)->getPoseInRootFrame();
157  Eigen::Matrix4f leftMarkerPose =
158  localRobot->getRobotNode(markerNameLeft)->getPoseInRootFrame();
159  Eigen::Matrix4f rightMarkerPose =
160  localRobot->getRobotNode(markerNameRight)->getPoseInRootFrame();
161  // get orientation
162  armarx::FramedOrientationPtr orientationLeftHand = new armarx::FramedOrientation(
163  leftHandPose, localRobot->getRootNode()->getName(), agentName);
164  armarx::FramedOrientationPtr orientationRightHand = new armarx::FramedOrientation(
165  rightHandPose, localRobot->getRootNode()->getName(), agentName);
166 
167  // convert all poses to camera coordinates
168  VirtualRobot::RobotNodePtr rnCamera = localRobot->getRobotNode(referenceFrameName);
169  ARMARX_CHECK_EXPRESSION(rnCamera) << "Reference frame not present in remote robot!";
170  Eigen::Matrix4f cameraPose = rnCamera->getPoseInRootFrame();
171  Eigen::Matrix4f cameraPoseInv = cameraPose.inverse();
172  leftHandPose = cameraPoseInv * leftHandPose;
173  rightHandPose = cameraPoseInv * rightHandPose;
174  leftMarkerPose = cameraPoseInv * leftMarkerPose;
175  rightMarkerPose = cameraPoseInv * rightMarkerPose;
176  // marker offsets to the TCP
177  Eigen::Vector3f markerOffsetToTCPLeft =
178  leftHandPose.block<3, 1>(0, 3) - leftMarkerPose.block<3, 1>(0, 3);
179  Eigen::Vector3f markerOffsetToTCPRight =
180  rightHandPose.block<3, 1>(0, 3) - rightMarkerPose.block<3, 1>(0, 3);
181  // hand orientation from the kinematics will be returned because the localization does not give an orientation
182 
183  memoryx::ObjectLocalizationResultList resultList;
184 
185  if (useVision)
186  {
187  // localize marker(s)
188  Vec3d handMarkerPositionLeft, handMarkerPositionRight;
189  int numFoundMarkers = 0;
190 
191  if (getResultImagesEnabled())
192  {
193  numFoundMarkers =
194  visualTargetLocator->LocateOneOrTwoTargets(cameraImages,
195  resultImages,
196  handMarkerPositionLeft,
197  handMarkerPositionRight,
198  -1,
199  getImagesAreUndistorted(),
200  markerColors[firstObjectName]);
201  // old version of the method
202  //visualTargetLocator->Locate(cameraImages, resultImages, handMarkerPositionLeft, -1, getImagesAreUndistorted(), markerColors[firstObjectName]);
203  }
204  else
205  {
206  numFoundMarkers =
207  visualTargetLocator->LocateOneOrTwoTargets(cameraImages,
208  NULL,
209  handMarkerPositionLeft,
210  handMarkerPositionRight,
211  -1,
212  getImagesAreUndistorted(),
213  markerColors[firstObjectName]);
214  }
215 
216  ARMARX_VERBOSE << "Found " << numFoundMarkers << " hand markers.";
217 
218  if (numFoundMarkers == 0)
219  {
220  ARMARX_INFO << "Marker not found, color: " << markerColors[firstObjectName];
221  }
222 
223  // return found marker(s)
224  if (numFoundMarkers > 0)
225  {
226  // assemble result
227  memoryx::ObjectLocalizationResult result;
228  result.recognitionCertainty = recognitionCertainty;
229  // result.localizationType = eHandMarkerLocalizer;
230  Eigen::Vector3f position;
231 
232  if (numFoundMarkers == 1)
233  {
234  if (objectClassNames.size() == 1)
235  {
236  result.objectClassName = objectClassNames.at(0);
237  position << handMarkerPositionLeft.x, handMarkerPositionLeft.y,
238  handMarkerPositionLeft.z;
239 
240  float distToFwdKinPos;
241  if (firstEntryIsLeft)
242  {
243  distToFwdKinPos = (leftMarkerPose.block<3, 1>(0, 3) - position).norm();
244  position = position + markerOffsetToTCPLeft;
245  result.orientation = orientationLeftHand;
246  }
247  else
248  {
249  distToFwdKinPos = (rightMarkerPose.block<3, 1>(0, 3) - position).norm();
250  position = position + markerOffsetToTCPRight;
251  result.orientation = orientationRightHand;
252  }
253 
254  result.position =
255  new armarx::FramedPosition(position, referenceFrameName, agentName);
256  result.positionNoise = calculateLocalizationUncertainty(position);
257  result.timeStamp =
258  new armarx::TimestampVariant(imageMetaInfo->timeProvided);
259  armarx::FramedPositionPtr pos =
260  armarx::FramedPositionPtr::dynamicCast(result.position);
261  if (firstEntryIsLeft)
262  {
263  ARMARX_VERBOSE << "Position of left hand: "
264  << pos->toRootFrame(localRobot)->output();
265  }
266  else
267  {
268  ARMARX_VERBOSE << "Position of right hand: "
269  << pos->toRootFrame(localRobot)->output();
270  }
271  ARMARX_DEBUG
272  << "Distance to position from forward kinematics: " << distToFwdKinPos
273  << "(max: " << maxDistToFwdKinPose << ")";
274 
275  if (distToFwdKinPos < maxDistToFwdKinPose)
276  {
277  resultList.push_back(result);
278 
279  // mark TCP pose with a cross
280  if (getResultImagesEnabled())
281  {
282  drawCrossInImage(resultImages[0], position, true);
283  drawCrossInImage(resultImages[1], position, false);
284  }
285  }
286  else
287  {
288  ARMARX_VERBOSE << "Refusing found marker because it is too far from "
289  "the position from the kinematic model ("
290  << distToFwdKinPos << ", max: " << maxDistToFwdKinPose
291  << ")";
292  }
293  }
294  else if (objectClassNames.size() == 2)
295  {
296  ARMARX_IMPORTANT
297  << "Found only 1 hand marker, but 2 were searched for. Returning the "
298  "localized marker position as a localization result for the closer "
299  "hand from forward kinematics.";
300 
301  position << handMarkerPositionLeft.x, handMarkerPositionLeft.y,
302  handMarkerPositionLeft.z;
303  float distToFwdKinPosLeft =
304  (leftMarkerPose.block<3, 1>(0, 3) - position).norm();
305  float distToFwdKinPosRight =
306  (rightMarkerPose.block<3, 1>(0, 3) - position).norm();
307 
308  std::string nameLeft, nameRight;
309 
310  if (firstEntryIsLeft)
311  {
312  nameLeft = objectClassNames.at(0);
313  nameRight = objectClassNames.at(1);
314  }
315  else
316  {
317  nameLeft = objectClassNames.at(1);
318  nameRight = objectClassNames.at(0);
319  }
320 
321  ARMARX_VERBOSE << "Distance to position from forward kinematics for "
322  << nameLeft << ": " << distToFwdKinPosLeft
323  << "(max: " << maxDistToFwdKinPose << ")";
324  ARMARX_VERBOSE << "Distance to position from forward kinematics for "
325  << nameRight << ": " << distToFwdKinPosRight
326  << "(max: " << maxDistToFwdKinPose << ")";
327 
328 
329  if (distToFwdKinPosLeft < maxDistToFwdKinPose &&
330  distToFwdKinPosLeft < distToFwdKinPosRight)
331  {
332  result.objectClassName = nameLeft;
333  position = position + markerOffsetToTCPLeft;
334  result.position =
335  new armarx::FramedPosition(position, referenceFrameName, agentName);
336  result.positionNoise = calculateLocalizationUncertainty(position);
337  result.orientation = orientationLeftHand;
338  resultList.push_back(result);
339  }
340 
341  if (distToFwdKinPosRight < maxDistToFwdKinPose &&
342  distToFwdKinPosRight < distToFwdKinPosLeft)
343  {
344  result.objectClassName = nameRight;
345  position = position + markerOffsetToTCPRight;
346  result.position =
347  new armarx::FramedPosition(position, referenceFrameName, agentName);
348  result.positionNoise = calculateLocalizationUncertainty(position);
349  result.orientation = orientationRightHand;
350  resultList.push_back(result);
351  }
352  }
353  }
354  else if (numFoundMarkers == 2)
355  {
356  if (objectClassNames.size() == 1)
357  {
358  ARMARX_IMPORTANT << "Found 2 hand markers, but only 1 was searched for.";
359 
360  result.objectClassName = objectClassNames.at(0);
361 
362  position << handMarkerPositionLeft.x, handMarkerPositionLeft.y,
363  handMarkerPositionLeft.z;
364 
365  float distToFwdKinPosLeft =
366  (leftMarkerPose.block<3, 1>(0, 3) - position).norm();
367  float distToFwdKinPosRight =
368  (rightMarkerPose.block<3, 1>(0, 3) - position).norm();
369 
370  if (firstEntryIsLeft)
371  {
372  ARMARX_VERBOSE << "Distance to position from forward kinematics: "
373  << distToFwdKinPosLeft << "(max: " << maxDistToFwdKinPose
374  << ")";
375 
376  position = position + markerOffsetToTCPLeft;
377  result.position =
378  new armarx::FramedPosition(position, referenceFrameName, agentName);
379  result.positionNoise = calculateLocalizationUncertainty(position);
380  result.orientation = orientationLeftHand;
381 
382  if (distToFwdKinPosLeft < maxDistToFwdKinPose &&
383  distToFwdKinPosLeft < distToFwdKinPosRight)
384  {
385  resultList.push_back(result);
386  }
387  }
388  else
389  {
390  ARMARX_VERBOSE << "Distance to position from forward kinematics: "
391  << distToFwdKinPosRight
392  << "(max: " << maxDistToFwdKinPose << ")";
393 
394  position = position + markerOffsetToTCPRight;
395  result.position =
396  new armarx::FramedPosition(position, referenceFrameName, agentName);
397  result.positionNoise = calculateLocalizationUncertainty(position);
398  result.orientation = orientationRightHand;
399 
400  if (distToFwdKinPosRight < maxDistToFwdKinPose &&
401  distToFwdKinPosRight < distToFwdKinPosLeft)
402  {
403  resultList.push_back(result);
404  }
405  }
406 
407 
408  position << handMarkerPositionRight.x, handMarkerPositionRight.y,
409  handMarkerPositionRight.z;
410 
411  distToFwdKinPosLeft = (leftMarkerPose.block<3, 1>(0, 3) - position).norm();
412  distToFwdKinPosRight =
413  (rightMarkerPose.block<3, 1>(0, 3) - position).norm();
414 
415  if (firstEntryIsLeft)
416  {
417  ARMARX_VERBOSE << "Distance to position from forward kinematics: "
418  << distToFwdKinPosLeft << "(max: " << maxDistToFwdKinPose
419  << ")";
420 
421  position = position + markerOffsetToTCPLeft;
422  result.position =
423  new armarx::FramedPosition(position, referenceFrameName, agentName);
424  result.positionNoise = calculateLocalizationUncertainty(position);
425  result.orientation = orientationLeftHand;
426 
427  if (distToFwdKinPosLeft < maxDistToFwdKinPose &&
428  distToFwdKinPosLeft < distToFwdKinPosRight)
429  {
430  resultList.push_back(result);
431  }
432  }
433  else
434  {
435  ARMARX_VERBOSE << "Distance to position from forward kinematics: "
436  << distToFwdKinPosRight
437  << "(max: " << maxDistToFwdKinPose << ")";
438 
439  position = position + markerOffsetToTCPRight;
440  result.position =
441  new armarx::FramedPosition(position, referenceFrameName, agentName);
442  result.positionNoise = calculateLocalizationUncertainty(position);
443  result.orientation = orientationRightHand;
444 
445  if (distToFwdKinPosRight < maxDistToFwdKinPose &&
446  distToFwdKinPosRight < distToFwdKinPosLeft)
447  {
448  resultList.push_back(result);
449  }
450  }
451  }
452  else if (objectClassNames.size() == 2)
453  {
454  std::string nameLeft, nameRight;
455 
456  if (firstEntryIsLeft)
457  {
458  nameLeft = objectClassNames.at(0);
459  nameRight = objectClassNames.at(1);
460  }
461  else
462  {
463  nameLeft = objectClassNames.at(1);
464  nameRight = objectClassNames.at(0);
465  }
466 
467  result.objectClassName = nameLeft;
468  position << handMarkerPositionLeft.x, handMarkerPositionLeft.y,
469  handMarkerPositionLeft.z;
470  position = position + markerOffsetToTCPLeft;
471  float distToFwdKinPosLeft =
472  (leftMarkerPose.block<3, 1>(0, 3) - position).norm();
473  float distToFwdKinPosRight =
474  (rightMarkerPose.block<3, 1>(0, 3) - position).norm();
475  result.position =
476  new armarx::FramedPosition(position, referenceFrameName, agentName);
477  result.orientation = orientationLeftHand;
478  result.positionNoise = calculateLocalizationUncertainty(position);
479  ARMARX_VERBOSE << "Distance to position from forward kinematics (left): "
480  << distToFwdKinPosLeft << "(max: " << maxDistToFwdKinPose
481  << ")";
482 
483  if (distToFwdKinPosLeft < maxDistToFwdKinPose &&
484  distToFwdKinPosLeft < distToFwdKinPosRight)
485  {
486  resultList.push_back(result);
487  }
488 
489  result.objectClassName = nameRight;
490  position << handMarkerPositionRight.x, handMarkerPositionRight.y,
491  handMarkerPositionRight.z;
492  position = position + markerOffsetToTCPRight;
493  distToFwdKinPosLeft = (leftMarkerPose.block<3, 1>(0, 3) - position).norm();
494  distToFwdKinPosRight =
495  (rightMarkerPose.block<3, 1>(0, 3) - position).norm();
496  result.position =
497  new armarx::FramedPosition(position, referenceFrameName, agentName);
498  result.orientation = orientationRightHand;
499  result.positionNoise = calculateLocalizationUncertainty(position);
500  ARMARX_VERBOSE << "Distance to position from forward kinematics (right): "
501  << distToFwdKinPosRight << "(max: " << maxDistToFwdKinPose
502  << ")";
503 
504  if (distToFwdKinPosRight < maxDistToFwdKinPose &&
505  distToFwdKinPosRight < distToFwdKinPosLeft)
506  {
507  resultList.push_back(result);
508  }
509  }
510  }
511  }
512 
513  // draw color segmented image
514  if (showSegmentedResultImages)
515  {
516  CByteImage* pSegmentedImage = new CByteImage(
517  cameraImages[0]->width, cameraImages[0]->height, CByteImage::eGrayScale);
518  ::ImageProcessor::FilterColor(cameraImages[0],
519  pSegmentedImage,
520  markerColors[firstObjectName],
521  colorParameterSet,
522  false);
523  ::ImageProcessor::ConvertImage(pSegmentedImage, resultImages[2]);
524  delete pSegmentedImage;
525  }
526  }
527 
528  // localization failed or vision is turned off
529  if (resultList.size() < objectClassNames.size())
530  {
531  // return poses from forward kinematics
532  memoryx::ObjectLocalizationResult result;
533  result.recognitionCertainty = recognitionCertainty;
534  // result.localizationType = eForwardKinematics;
535  armarx::FramedPositionPtr positionLeftHand = new armarx::FramedPosition(
536  localRobot->getRobotNode(tcpNameLeft)->getPoseInRootFrame(),
537  localRobot->getRootNode()->getName(),
538  agentName);
539  armarx::FramedPositionPtr positionRightHand = new armarx::FramedPosition(
540  localRobot->getRobotNode(tcpNameRight)->getPoseInRootFrame(),
541  localRobot->getRootNode()->getName(),
542  agentName);
543  Eigen::Matrix3f variances;
544  float var = uncertaintyWhenReturningKinematicPosition *
545  uncertaintyWhenReturningKinematicPosition;
546  variances << var, 0, 0, 0, var, 0, 0, 0, var;
547 
548  if (objectClassNames.size() == 1)
549  {
550  ARMARX_INFO << "Returning hand pose from forward kinematics for one hand: "
551  << objectClassNames.at(0);
552 
553  result.objectClassName = objectClassNames.at(0);
554 
555  if (firstEntryIsLeft)
556  {
557  ARMARX_INFO << "left hand position: " << positionLeftHand->output();
558  result.position = positionLeftHand;
559  result.orientation = orientationLeftHand;
560  Eigen::Vector3f mean = positionLeftHand->toEigen();
561  result.positionNoise =
562  new memoryx::MultivariateNormalDistribution(mean, variances);
563  }
564  else
565  {
566  ARMARX_INFO << "right hand position: " << positionRightHand->output();
567  result.position = positionRightHand;
568  result.orientation = orientationRightHand;
569  Eigen::Vector3f mean = positionRightHand->toEigen();
570  result.positionNoise =
571  new memoryx::MultivariateNormalDistribution(mean, variances);
572  }
573 
574  resultList.push_back(result);
575  }
576  else if (objectClassNames.size() == 2)
577  {
578  std::string leftName =
579  firstEntryIsLeft ? objectClassNames.at(0) : objectClassNames.at(1);
580  std::string rightName =
581  firstEntryIsLeft ? objectClassNames.at(1) : objectClassNames.at(0);
582 
583  if (resultList.size() == 0)
584  {
585  ARMARX_INFO << "Returning hand pose from forward kinematics for two hands: "
586  << objectClassNames.at(0) << ", " << objectClassNames.at(1);
587 
588  result.objectClassName = leftName;
589  result.position = positionLeftHand;
590  result.orientation = orientationLeftHand;
591  Eigen::Vector3f mean = positionLeftHand->toEigen();
592  result.positionNoise =
593  new memoryx::MultivariateNormalDistribution(mean, variances);
594  resultList.push_back(result);
595 
596  result.objectClassName = rightName;
597  result.position = positionRightHand;
598  result.orientation = orientationRightHand;
599  mean = positionRightHand->toEigen();
600  result.positionNoise =
601  new memoryx::MultivariateNormalDistribution(mean, variances);
602  resultList.push_back(result);
603  }
604  else
605  {
606  if (resultList.at(0).objectClassName == leftName)
607  {
608  ARMARX_INFO << "Returning hand pose from recognition for " << leftName
609  << " and from forward kinematics for " << rightName;
610  result.objectClassName = rightName;
611  result.position = positionRightHand;
612  result.orientation = orientationRightHand;
613  Eigen::Vector3f mean = positionRightHand->toEigen();
614  result.positionNoise =
615  new memoryx::MultivariateNormalDistribution(mean, variances);
616  resultList.push_back(result);
617  }
618  else
619  {
620  ARMARX_INFO << "Returning hand pose from recognition for " << rightName
621  << " and from forward kinematics for " << leftName;
622  result.objectClassName = leftName;
623  result.position = positionLeftHand;
624  result.orientation = orientationLeftHand;
625  Eigen::Vector3f mean = positionLeftHand->toEigen();
626  result.positionNoise =
627  new memoryx::MultivariateNormalDistribution(mean, variances);
628  resultList.push_back(result);
629  }
630  }
631  }
632  else
633  {
634  ARMARX_WARNING << "More than 2 hands requested - this case is not handled here";
635  }
636  }
637  for (auto& result : resultList)
638  {
639  result.timeStamp = new armarx::TimestampVariant(timestamp);
640  }
641 
642  ARMARX_DEBUG << "Finished localization, returning.";
643 
644  return resultList;
645  }
646 
647  void
648  HandMarkerLocalization::drawCrossInImage(CByteImage* image,
649  Eigen::Vector3f point,
650  bool leftCamera)
651  {
652  Vec3d positionIVT = {point(0), point(1), point(2)};
653  Vec2d projectedPoint;
654 
655  if (leftCamera)
656  {
657  getStereoCalibration()->GetLeftCalibration()->WorldToImageCoordinates(
658  positionIVT, projectedPoint, getImagesAreUndistorted());
659  }
660  else
661  {
662  getStereoCalibration()->GetRightCalibration()->WorldToImageCoordinates(
663  positionIVT, projectedPoint, getImagesAreUndistorted());
664  }
665 
666  const int size = 3;
667 
668  if (size < projectedPoint.x && size < projectedPoint.y &&
669  projectedPoint.x < image->width - size - 1 &&
670  projectedPoint.y < image->height - size - 3)
671  {
672  for (int i = -size; i <= size; i++)
673  {
674  int indexHorizontalLine =
675  3 * ((int)projectedPoint.y * image->width + (int)projectedPoint.x + i);
676  int indexVerticalLine =
677  3 * (((int)projectedPoint.y + i) * image->width + (int)projectedPoint.x);
678  image->pixels[indexHorizontalLine] = 255;
679  image->pixels[indexHorizontalLine + 1] = 255;
680  image->pixels[indexHorizontalLine + 2] = 255;
681  image->pixels[indexVerticalLine] = 255;
682  image->pixels[indexVerticalLine + 1] = 255;
683  image->pixels[indexVerticalLine + 2] = 255;
684  }
685  }
686  }
687 } // namespace visionx
visionx::HandMarkerLocalization::localizeObjectClasses
memoryx::ObjectLocalizationResultList localizeObjectClasses(const std::vector< std::string > &objectClassNames, CByteImage **cameraImages, armarx::MetaInfoSizeBasePtr imageMetaInfo, CByteImage **resultImages) override
localize one or both hand markers
Definition: HandMarkerLocalization.cpp:130
visionx::HandMarkerLocalization::addObjectClass
bool addObjectClass(const memoryx::EntityPtr &objectClassEntity, const memoryx::GridFileManagerPtr &fileManager) override
Add a memory entity representing the hand marker in order to set its properties.
Definition: HandMarkerLocalization.cpp:116
RemoteRobot.h
HandMarkerLocalization.h
ARMARX_VERBOSE
#define ARMARX_VERBOSE
Definition: Logging.h:187
visionx::ObjectLocalizerProcessor::resultImages
CByteImage ** resultImages
Definition: ObjectLocalizerProcessor.h:431
visionx::HandMarkerLocalization::onInitObjectLocalizerProcessor
void onInitObjectLocalizerProcessor() override
Definition: HandMarkerLocalization.cpp:58
GfxTL::Vec3d
VectorXD< 3, double > Vec3d
Definition: VectorXD.h:737
ARMARX_IMPORTANT
#define ARMARX_IMPORTANT
Definition: Logging.h:190
visionx::ObjectLocalizerProcessor::referenceFrameName
std::string referenceFrameName
Definition: ObjectLocalizerProcessor.h:425
visionx
ArmarX headers.
Definition: OpenPoseStressTest.h:38
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:650
Pose.h
visionx::ObjectLocalizerProcessor::getResultImagesEnabled
bool getResultImagesEnabled() const
Retrieve whether result images are enabled.
Definition: ObjectLocalizerProcessor.h:335
armarx::TimestampVariant
Definition: TimestampVariant.h:54
ObjectRecognitionWrapper.h
visionx::ObjectLocalizerProcessor::imageMetaInfo
armarx::MetaInfoSizeBasePtr imageMetaInfo
Definition: ObjectLocalizerProcessor.h:432
IceInternal::Handle
Definition: forward_declarations.h:8
visionx::ObjectLocalizerProcessor::getImagesAreUndistorted
bool getImagesAreUndistorted() const
Retrieve whether images are undistorted.
Definition: ObjectLocalizerProcessor.h:368
armarx::mean
std::optional< float > mean(const boost::circular_buffer< NameValueMap > &buffer, const std::string &key)
Definition: KinematicUnitGuiPlugin.cpp:1620
visionx::ObjectLocalizerProcessor::cameraImages
CByteImage * cameraImages[2]
Definition: ObjectLocalizerProcessor.h:429
ARMARX_DEBUG
#define ARMARX_DEBUG
Definition: Logging.h:184
TimestampVariant.h
timestamp
std::string timestamp()
Definition: CartographerAdapter.cpp:85
visionx::HandMarkerLocalization::onConnectObjectLocalizerProcessor
void onConnectObjectLocalizerProcessor() override
Initialize stuff here?
Definition: HandMarkerLocalization.cpp:69
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:196
GfxTL::Vec2d
VectorXD< 2, double > Vec2d
Definition: VectorXD.h:736
armarx::TimeUtil::GetTime
static IceUtil::Time GetTime(TimeMode timeMode=TimeMode::VirtualTime)
Get the current time.
Definition: TimeUtil.cpp:42
ProbabilityMeasures.h
visionx::ObjectLocalizerProcessor::getStereoCalibration
CStereoCalibration * getStereoCalibration() const
Retrieve stereo calibration corresponding to image provider.
Definition: ObjectLocalizerProcessor.h:391
ARMARX_CHECK_EXPRESSION
#define ARMARX_CHECK_EXPRESSION(expression)
This macro evaluates the expression and if it turns out to be false it will throw an ExpressionExcept...
Definition: ExpressionException.h:73
armarx::VariantType::FramedOrientation
const VariantTypeId FramedOrientation
Definition: FramedPose.h:39
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:181
memoryx::GridFileManagerPtr
std::shared_ptr< GridFileManager > GridFileManagerPtr
Definition: AbstractEntityWrapper.h:33
armarx::RemoteRobot::createLocalClone
VirtualRobot::RobotPtr createLocalClone()
Clones the structure of this remote robot to a local instance.
Definition: RemoteRobot.cpp:381
Entity.h
armarx::RemoteRobot::synchronizeLocalCloneToTimestamp
static bool synchronizeLocalCloneToTimestamp(VirtualRobot::RobotPtr robot, RobotStateComponentInterfacePrx robotStatePrx, Ice::Long timestamp)
Synchronizes a local robot to a robot state at timestamp.
Definition: RemoteRobot.cpp:566
visionx::HandMarkerLocalization::HandMarkerLocalization
HandMarkerLocalization()
Definition: HandMarkerLocalization.cpp:52
armarx::Logging::deactivateSpam
SpamFilterDataPtr deactivateSpam(float deactivationDurationSec=10.0f, const std::string &identifier="", bool deactivate=true) const
disables the logging for the current line for the given amount of seconds.
Definition: Logging.cpp:99
armarx::ArmarXDataPath::getAbsolutePath
static bool getAbsolutePath(const std::string &relativeFilename, std::string &storeAbsoluteFilename, const std::vector< std::string > &additionalSearchPaths={}, bool verbose=true)
Definition: ArmarXDataPath.cpp:109
GfxTL::Matrix3f
MatrixXX< 3, 3, float > Matrix3f
Definition: MatrixXX.h:649
visionx::ObjectLocalizerProcessor::calculateLocalizationUncertainty
memoryx::MultivariateNormalDistributionPtr calculateLocalizationUncertainty(Vec2d left_point, Vec2d right_point)
Calculate 3D uncertainty from two 2d points in left and right camera.
Definition: ObjectLocalizerProcessor.cpp:221
armarx::VariantType::FramedPosition
const VariantTypeId FramedPosition
Definition: FramedPose.h:38
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:193
ArmarXDataPath.h
armarx::ManagedIceObject::usingProxy
bool usingProxy(const std::string &name, const std::string &endpoints="")
Registers a proxy for retrieval after initialization and adds it to the dependency list.
Definition: ManagedIceObject.cpp:154
visionx::ObjectLocalizerProcessor::numberOfResultImages
int numberOfResultImages
Definition: ObjectLocalizerProcessor.h:433
memoryx::EntityWrappers::HandMarkerBallWrapper
HandMarkerBallWrapper offers a simplified access to the necessary information for localization of the...
Definition: ObjectRecognitionWrapper.h:110
armarx::VariantType::MultivariateNormalDistribution
const armarx::VariantTypeId MultivariateNormalDistribution
Definition: ProbabilityMeasures.h:40
visionx::HandMarkerLocalization::initRecognizer
bool initRecognizer() override
Initializes segmentable recognition.
Definition: HandMarkerLocalization.cpp:77