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