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
48namespace visionx
49{
50
51
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
timestamp
std::string timestamp()
Definition CartographerAdapter.cpp:86
deactivateSpam
SpamFilterDataPtr deactivateSpam(SpamFilterDataPtr const &spamFilter, float deactivationDurationSec, const std::string &identifier, bool deactivate)
Definition Logging.cpp:75
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
armarx::Component::getProperty
Property< PropertyType > getProperty(const std::string &name)
Definition PropertyUser.h:180
armarx::FramedOrientation
The FramedOrientation class.
Definition FramedPose.h:216
armarx::FramedPosition
The FramedPosition class.
Definition FramedPose.h:158
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
armarx::ManagedIceObject::getProxy
Ice::ObjectPrx getProxy(long timeoutMs=0, bool waitForScheduler=true) const
Returns the proxy of this object (optionally it waits for the proxy)
Definition ManagedIceObject.cpp:407
armarx::RemoteRobot::createLocalClone
VirtualRobot::RobotPtr createLocalClone()
Clones the structure of this remote robot to a local instance.
Definition RemoteRobot.cpp:381
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
armarx::TimeUtil::GetTime
static IceUtil::Time GetTime(TimeMode timeMode=TimeMode::VirtualTime)
Get the current time.
Definition TimeUtil.cpp:42
armarx::TimestampVariant
Implements a Variant type for timestamps.
Definition TimestampVariant.h:55
memoryx::EntityWrappers::HandMarkerBallWrapper
HandMarkerBallWrapper offers a simplified access to the necessary information for localization of the...
Definition ObjectRecognitionWrapper.h:111
memoryx::MultivariateNormalDistribution
The MultivariateNormalDistribution class.
Definition ProbabilityMeasures.h:288
visionx::HandMarkerLocalization::initRecognizer
bool initRecognizer() override
Initializes segmentable recognition.
Definition HandMarkerLocalization.cpp:77
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::onConnectObjectLocalizerProcessor
void onConnectObjectLocalizerProcessor() override
Initialize stuff here?
Definition HandMarkerLocalization.cpp:69
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
visionx::ObjectLocalizerProcessor::getImagesAreUndistorted
bool getImagesAreUndistorted() const
Retrieve whether images are undistorted.
Definition ObjectLocalizerProcessor.h:368
visionx::ObjectLocalizerProcessor::getResultImagesEnabled
bool getResultImagesEnabled() const
Retrieve whether result images are enabled.
Definition ObjectLocalizerProcessor.h:335
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
visionx::ObjectLocalizerProcessor::imageMetaInfo
armarx::MetaInfoSizeBasePtr imageMetaInfo
Definition ObjectLocalizerProcessor.h:432
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_INFO
#define ARMARX_INFO
The normal logging level.
Definition Logging.h:181
ARMARX_IMPORTANT
#define ARMARX_IMPORTANT
The logging level for always important information, but expected behaviour (in contrast to ARMARX_WAR...
Definition Logging.h:190
ARMARX_ERROR
#define ARMARX_ERROR
The logging level for unexpected behaviour, that must be fixed.
Definition Logging.h:196
ARMARX_DEBUG
#define ARMARX_DEBUG
The logging level for output that is only interesting while debugging.
Definition Logging.h:184
ARMARX_WARNING
#define ARMARX_WARNING
The logging level for unexpected behaviour, but not a serious problem.
Definition Logging.h:193
ARMARX_VERBOSE
#define ARMARX_VERBOSE
The logging level for verbose information.
Definition Logging.h:187
armarx::FramedPositionPtr
IceInternal::Handle< FramedPosition > FramedPositionPtr
Definition FramedPose.h:149
armarx::FramedOrientationPtr
IceInternal::Handle< FramedOrientation > FramedOrientationPtr
Definition FramedPose.h:207
memoryx::EntityWrappers::HandMarkerBallWrapperPtr
IceInternal::Handle< HandMarkerBallWrapper > HandMarkerBallWrapperPtr
Definition ObjectRecognitionWrapper.h:126
memoryx::EntityPtr
IceInternal::Handle< Entity > EntityPtr
Typedef of EntityPtr as IceInternal::Handle<Entity> for convenience.
Definition Entity.h:45
memoryx::GridFileManagerPtr
std::shared_ptr< GridFileManager > GridFileManagerPtr
Definition AbstractEntityWrapper.h:33
visionx
ArmarX headers.
Definition OpenPoseStressTest.h:39
norm
double norm(const Point &a)
Definition point.hpp:102