ObjectLocalizationMemoryUpdater.cpp
Go to the documentation of this file.
1 /*
2 * This file is part of ArmarX.
3 *
4 * ArmarX is free software; you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License version 2 as
6 * published by the Free Software Foundation.
7 *
8 * ArmarX is distributed in the hope that it will be useful, but
9 * WITHOUT ANY WARRANTY; without even the implied warranty of
10 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11 * GNU General Public License for more details.
12 *
13 * You should have received a copy of the GNU General Public License
14 * along with this program. If not, see <http://www.gnu.org/licenses/>.
15 *
16 * @package MemoryX::Core
17 * @author Kai Welke <welke@kit.edu>
18 * @copyright 2012 Kai Welke
19 * @license http://www.gnu.org/licenses/gpl-2.0.txt
20 * GNU General Public License
21 */
22 
28 #include <VirtualRobot/LinkedCoordinate.h>
29 #include <VirtualRobot/math/Helpers.h>
30 
34 
35 // localization strategies
38 
39 #include <MemoryX/interface/components/WorkingMemoryInterface.h>
43 
44 namespace memoryx
45 {
46  // *******************************************************
47  // public members
48  // *******************************************************
50  classSegmentName("objectClasses"),
51  instanceSegmentName("objectInstances"),
52  agentInstancesSegmentName("agentInstances"),
53  referenceFrameName("Platform")
54  {
55  }
56 
57 
58 
59 
60  void ObjectLocalizationMemoryUpdater::setSegmentNames(std::string classSegmentName, std::string instanceSegmentName)
61  {
62  // retrieve segments
63  this->classSegmentName = classSegmentName;
64  this->instanceSegmentName = instanceSegmentName;
65  }
66 
67 
68 
69 
70  void ObjectLocalizationMemoryUpdater::setReferenceFrame(const std::string& referenceFrameName)
71  {
72  this->referenceFrameName = referenceFrameName;
73  }
74 
75 
76 
77 
78  // *******************************************************
79  // implementation of ManagedIceObject callbacks
80  // *******************************************************
82  {
83  // retrieve relevant segments
84  objectClassSegment = getSegment<ObjectClassMemorySegmentBase>(classSegmentName);
85  objectInstanceSegment = getSegment<ObjectInstanceMemorySegment>(instanceSegmentName);
86  agentInstancesSegment = getSegment<AgentInstancesSegment>(agentInstancesSegmentName);
87 
88  // register for WM updates
89  usingTopic(getWorkingMemory()->getListenerTopicName());
90 
91  // we need a remote robot proxy
92  usingProxy(getProperty<std::string>("RobotStateComponentName").getValue());
93  usingProxy(getProperty<std::string>("LongtermMemoryName").getValue());
94 
95  // this will be used to measure the head motion speed, to decide whether objects can be localized
96  if (!getProperty<std::string>("RobotStateObserverName").getValue().empty())
97  {
98  usingProxy(getProperty<std::string>("RobotStateObserverName").getValue());
99  }
100 
101  if (!getProperty<std::string>("CommonPlacesLearnerName").getValue().empty())
102  {
103  usingProxy(getProperty<std::string>("CommonPlacesLearnerName").getValue());
104  }
105 
106  workingMemoryName = getProperty<std::string>("WorkingMemoryName").getValue();
107  checkFieldOfView = getProperty<bool>("CheckFieldOfView").getValue();
108  cameraOpeningAngle = getProperty<float>("CameraOpeningAngle").getValue();
109  robotNodeNameLeftCamera = getProperty<std::string>("RobotNodeNameLeftCamera").getValue();
110  robotNodeNameRightCamera = getProperty<std::string>("RobotNodeNameRightCamera").getValue();
111  handNodeNameLeft = getProperty<std::string>("HandNodeNameLeft").getValue();
112  handNodeNameRight = getProperty<std::string>("HandNodeNameRight").getValue();
113  headMotionVelocityLimit = getProperty<float>("HeadMotionVelocityLimit").getValue();
114 
115  if (checkFieldOfView)
116  {
117  ARMARX_IMPORTANT << "Objects will only be localized when they are assumed to be in the field of view";
118  }
119  else
120  {
121  ARMARX_IMPORTANT << "Objects will always be localized, even when they are not in the field of view";
122  }
123  }
124 
125 
127  {
128  robotStateInterfacePrx = getProxy<armarx::RobotStateComponentInterfacePrx>(getProperty<std::string>("RobotStateComponentName").getValue());
129  longtermMemoryPrx = getProxy<memoryx::LongtermMemoryInterfacePrx>(getProperty<std::string>("LongtermMemoryName").getValue());
130  auto robot = robotStateInterfacePrx->getSynchronizedRobot();
131  referenceFrameName = robot->getRootNode()->getName();
132  agentName = robot->getName();
133 
134  if (!getProperty<std::string>("RobotStateObserverName").getValue().empty())
135  {
136  robotStateObserverProxy = getProxy<armarx::ObserverInterfacePrx>(getProperty<std::string>("RobotStateObserverName").getValue());
137  headTCPVelocityDatafieldID = new armarx::DataFieldIdentifier(getProperty<std::string>("RobotStateObserverName").getValue(), "TCPVelocities", getProperty<std::string>("GazeTCPName").getValue());
138  }
139 
140  if (!getProperty<std::string>("CommonPlacesLearnerName").getValue().empty())
141  {
142  commonPlacesLearnerProxy = getProxy<memoryx::CommonPlacesLearnerInterfacePrx>(getProperty<std::string>("CommonPlacesLearnerName").getValue());
143  }
144 
145  // start scheduler
146  ARMARX_INFO << "Starting localization jobs";
147  localizationTask = new armarx::PeriodicTask<ObjectLocalizationMemoryUpdater>(this, &ObjectLocalizationMemoryUpdater::runJobs, 10, false, "", false);
148  localizationTask->start();
149  }
150 
151 
153  {
154  if (localizationTask)
155  {
156  localizationTask->stop();
157  }
158  }
159 
160 
161 
162 
163  // *******************************************************
164  // implementation of WorkingMemoryListenerInterface
165  // *******************************************************
166  void ObjectLocalizationMemoryUpdater::reportEntityUpdated(const std::string& segmentName, const EntityBasePtr& entityOld, const EntityBasePtr& entityNew, const Ice::Current&)
167  {
168  ARMARX_DEBUG << deactivateSpam(4) << "ObjectLocalizationMemoryUpdater::reportEntityUpdated()";
169 
170  // only updates from class segment
171  if (segmentName != classSegmentName)
172  {
173  return;
174  }
175 
176  // retrieve queries
177  ObjectClassPtr objectClassOld = ObjectClassPtr::dynamicCast(entityOld);
178  ObjectClassPtr objectClassNew = ObjectClassPtr::dynamicCast(entityNew);
179 
180  if (!objectClassOld || !objectClassNew)
181  {
182  return;
183  }
184 
185  EntityWrappers::ObjectRecognitionWrapperPtr recognitionWrapperOld = objectClassOld->addWrapper(new EntityWrappers::ObjectRecognitionWrapper());
186  EntityWrappers::ObjectRecognitionWrapperPtr recognitionWrappeNew = objectClassNew->addWrapper(new EntityWrappers::ObjectRecognitionWrapper());
187 
188  LocalizationQueryList queriesOld = recognitionWrapperOld->getLocalizationQueries();
189  LocalizationQueryList queriesNew = recognitionWrappeNew->getLocalizationQueries();
190 
191  std::unique_lock lock(jobsMutex);
192 
193  // start new queries
194  LocalizationQueryList queriesCreated = getMissingQueries(queriesOld, queriesNew);
195 
196  for (LocalizationQueryList::iterator iter = queriesCreated.begin(); iter != queriesCreated.end(); iter++)
197  {
198  LocalizationQueryPtr query = LocalizationQueryPtr::dynamicCast(*iter);
199  ARMARX_INFO << "Adding new query " << query->queryName;
200 
201  // add and create jobs
202  std::vector<LocalizationJobPtr> createdjobs = query->createJobs(objectClassSegment);
203 
204  for (LocalizationJobPtr job : createdjobs)
205  {
206  ARMARX_DEBUG << deactivateSpam(1) << "Created job for " << job->getRecognitionMethod();
207  }
208 
209  std::move(createdjobs.begin(), createdjobs.end(), std::back_inserter(jobs));
210  }
211 
212  // stop removed queries
213  LocalizationQueryList queriesRemoved = getMissingQueries(queriesNew, queriesOld);
214 
215  for (LocalizationQueryList::iterator iter = queriesRemoved.begin(); iter != queriesRemoved.end(); iter++)
216  {
217  LocalizationQueryPtr query = LocalizationQueryPtr::dynamicCast(*iter);
218 
219  // remove jobs for this query
220  std::vector<LocalizationJobPtr>::iterator iterJobs = jobs.begin();
221 
222  while (iterJobs != jobs.end())
223  {
224  if ((*iterJobs)->getQueryName() == query->queryName)
225  {
226  iterJobs = jobs.erase(iterJobs);
227  }
228  else
229  {
230  iterJobs++;
231  }
232  }
233 
234  ARMARX_INFO << "removed query " << query->queryName;
235  }
236  }
237 
238 
239 
240 
241  // *******************************************************
242  // private methods
243  // *******************************************************
244  void ObjectLocalizationMemoryUpdater::predictPoses()
245  {
246  std::unique_lock lock(predictionMutex);
247 
248  EntityIdList allEntityIDs = objectInstanceSegment->getAllEntityIds();
249 
250  for (EntityIdList::iterator it = allEntityIDs.begin(); it != allEntityIDs.end(); it++)
251  {
252  auto entity = objectInstanceSegment->getEntityById(*it);
253  if (!entity)
254  {
255  continue;
256  }
257  try
258  {
259  ObjectInstancePtr objectInstance = ObjectInstancePtr::dynamicCast(entity);
260 
261  AbstractMotionModelPtr motionModel = objectInstance->getMotionModel();
262  if (!motionModel)
263  {
264  continue;
265  }
266 
267  ARMARX_DEBUG << deactivateSpam(5, objectInstance->getName()) << "prediction for " << objectInstance->getName() << " motion model: " << objectInstance->getMotionModel()->ice_id();
268 
269  armarx::LinkedPosePtr predictedPose = armarx::LinkedPosePtr::dynamicCast(motionModel->getPredictedPose());
270  MultivariateNormalDistributionBasePtr positionUncertainty = motionModel->getUncertainty();
271 
272  ARMARX_DEBUG << deactivateSpam(1, objectInstance->getName()) << "predicted pose: " << predictedPose->toEigen();
273  ARMARX_DEBUG << deactivateSpam(1, objectInstance->getName()) << "last localization: " << objectInstance->getMotionModel()->getPoseAtLastLocalisation()->output();
274 
275  if (positionUncertainty)
276  {
277  ARMARX_DEBUG << deactivateSpam(1, objectInstance->getName()) << "positionUncertainty->getDimensions(): " << positionUncertainty->getDimensions();
278  Gaussian uncertaintyGaussian = EarlyVisionConverters::convertToGaussian(positionUncertainty);
279  ARMARX_DEBUG << deactivateSpam(1, objectInstance->getName()) << "uncertaintyGaussian: " << uncertaintyGaussian.getCovariance();
280  }
281 
282  if (predictedPose->frame.empty())
283  {
284  ARMARX_VERBOSE << "predicting pose of " << objectInstance->getName() << " position: "
285  << predictedPose->position->x << " " << predictedPose->position->y << " " << predictedPose->position->z << " frame: " << predictedPose->frame;
286  }
287 
288  // ObjectInstancePtr update = new ObjectInstance(objectInstance->getName());
289  objectInstance->setPosition(new armarx::FramedPosition(predictedPose->toEigen(), predictedPose->frame, predictedPose->agent));
290  objectInstance->setOrientation(new armarx::FramedOrientation(predictedPose->toEigen(), predictedPose->frame, predictedPose->agent));
291 
292  if (positionUncertainty)
293  {
294  objectInstance->setPositionUncertainty(positionUncertainty);
295  }
296 
297  objectInstance->setClass(objectInstance->getMostProbableClass(), objectInstance->getClassProbability(objectInstance->getMostProbableClass()));
298  objectInstance->setExistenceCertainty(objectInstance->getExistenceCertainty());
299  // update->setMotionModel(objectInstance->getMotionModel());
300  if (objectInstanceSegment->getListenerProxy() && objectInstanceSegment->hasEntityById(objectInstance->getId(), Ice::emptyCurrent))
301  {
302  auto reportClone = objectInstance->clone();
303  objectInstanceSegment->getListenerProxy()->reportEntityUpdated(objectInstanceSegment->getSegmentName(), reportClone, reportClone);
304  }
305  // objectInstanceSegment->updateEntity(*it, update, Ice::StringSeq{"KalmanFilterFusion"});
306 
307  //objectInstance = ObjectInstancePtr::dynamicCast(objectInstanceSegment->getEntityById(*it));
308  //FramedPositionPtr pos = objectInstance->getPosition();
309  //ARMARX_DEBUG << "new position of " << objectInstance->getName() << ": " << pos->x << " " << pos->y << " " << pos->z << " " << pos->frame;
310  //FramedOrientationPtr ori = objectInstance->getOrientation();
311  //ARMARX_DEBUG << "new orientation of " << objectInstance->getName() << ": " << ori->toEigen();
312  }
313  catch (...)
314  {
315  ARMARX_INFO << deactivateSpam(2, entity->getName()) << "Predicting poses for object '" << entity->getName() << "' failed: " << armarx::GetHandledExceptionString();
316  }
317  }
318  }
319 
320  void ObjectLocalizationMemoryUpdater::scheduleJobs()
321  {
322  //use a lambda so we can quickly leave this block by calling return
323  [&]
324  {
325  std::unique_lock lock(jobsMutex);
326 
327  // collect waiting jobs
328  std::multimap<std::string, LocalizationJobPtr> recognizerJobs;
329  std::set<std::string> recognizerNames;
330 
331  bool aJobIsWaiting = false;
332 
333  for (const LocalizationJobPtr& job : jobs)
334  {
335  if (job->waiting())
336  {
337  recognizerJobs.insert(std::make_pair(job->getRecognitionMethod(), job));
338  recognizerNames.insert(job->getRecognitionMethod());
339  //ARMARX_DEBUG << job->getQueryName() << " is waiting";
340  aJobIsWaiting = true;
341  }
342  else
343  {
344  ARMARX_DEBUG << deactivateSpam(1) << job->getQueryName() << " is not waiting";
345  }
346  }
347 
348  ARMARX_VERBOSE << deactivateSpam(60, armarx::ValueToString(recognizerNames.size())) << "localizer jobs that should be started: " << recognizerNames << " (number of jobs = " << jobs.size() << ")";
349 
350  if (!aJobIsWaiting)
351  {
352  ARMARX_DEBUG << deactivateSpam(5) << "No object needs to be localized";
353  return;
354  }
355 
356  Eigen::Vector3f tcpVelocity;
357  tcpVelocity.setZero();
358 
359  if (robotStateObserverProxy)
360  {
361  if (robotStateObserverProxy->existsDataField(headTCPVelocityDatafieldID->getChannelName(), headTCPVelocityDatafieldID->getDataFieldName()))
362  {
363  tcpVelocity = armarx::VariantPtr::dynamicCast(robotStateObserverProxy->getDataField(headTCPVelocityDatafieldID))->get<armarx::FramedDirection>()->toEigen();
364  }
365  }
366 
367  ARMARX_DEBUG << deactivateSpam(1) << "Head velocity: " << tcpVelocity.norm();
368 
369  // if head speed is too high, don't localize
370  if (tcpVelocity.norm() > headMotionVelocityLimit)
371  {
372  ARMARX_INFO << deactivateSpam(2) << "Not localizing anything because head velocity is too high: " << tcpVelocity.norm();
373  return;
374  }
375 
376  // start jobs, if recognition method is available
377  for (const auto& recognitionMethod : recognizerNames)
378  {
379 
380  // retrieve jobs for recognition method
381  if (isRecognitionMethodRunning(recognitionMethod))
382  {
383  ARMARX_DEBUG << deactivateSpam(0.5) << "Recognition method " << recognitionMethod << " is busy";
384  continue;
385  }
386 
387 
388  // elements contains two iterators, the first one points to the first and the second one to the last element of
389  // the list containing all jobs with the given recognition method
390  std::pair< std::map<std::string, LocalizationJobPtr>::iterator, std::map<std::string, LocalizationJobPtr>::iterator> elements = recognizerJobs.equal_range(recognitionMethod);
391  std::map<std::string, LocalizationJobPtr>::iterator iterElements = elements.first;
392 
393  LocalizationJobContainerPtr container = new LocalizationJobContainer(recognitionMethod);
394  bool hasJob = false;
395  std::unique_lock lockPrediction(predictionMutex, std::defer_lock);
396 
397  if (lockPrediction.try_lock())
398  {
399  while (iterElements != elements.second)
400  {
401  LocalizationJobPtr job = iterElements->second;
402 
403  // filter the objects in the job: only try to localize those that should currently be visible, have not been found yet or were lost again
404  std::vector<std::string> classNames = job->getAllClassNames();
405  std::vector<std::string> classNamesToBeLocalized;
406 
407  for (size_t i = 0; i < classNames.size(); i++)
408  {
409  ObjectInstanceList instances = objectInstanceSegment->getObjectInstancesByClass(classNames.at(i));
410 
411  if (instances.size() != 0)
412  {
413  bool atLeastOneInstanceIsVisibleOrLost = false;
414 
415  for (size_t j = 0; j < instances.size(); j++)
416  {
417  ObjectInstancePtr instance = ObjectInstancePtr::dynamicCast(instances.at(j));
418  float existenceCertainty = instance->getExistenceCertainty();
419  armarx::FramedPositionPtr pos = instance->getPosition();
420 
421  if (checkObjectVisibility(pos) || existenceCertainty < 0.5f)
422  {
423  atLeastOneInstanceIsVisibleOrLost = true;
424  }
425  }
426 
427  if (atLeastOneInstanceIsVisibleOrLost)
428  {
429  for (const auto& instance : instances)
430  {
431  if (ObjectInstancePtr::dynamicCast(instance)->getMotionModel())
432  {
433  ObjectInstancePtr::dynamicCast(instance)->getMotionModel()->savePredictedPoseAtStartOfCurrentLocalization();
434  }
435  }
436 
437  classNamesToBeLocalized.push_back(classNames.at(i));
438  }
439  else
440  {
441  ARMARX_INFO << deactivateSpam(5, classNames.at(i)) << "Not trying to localize " << classNames.at(i) << " because it is currently not in the field of view";
442  }
443  }
444  else
445  {
446  // object has not been found yet, so we will try to localize it
447  classNamesToBeLocalized.push_back(classNames.at(i));
448  }
449  }
450 
451  // if at least one object in the job should be visible, we start the localization
452  if (classNamesToBeLocalized.size() > 0)
453  {
454  job->setClassNamesToBeLocalized(classNamesToBeLocalized);
455  container->addJob(job);
456  hasJob = true;
457  }
458 
459  iterElements++;
460  }
461  }
462 
463  // start recognition method
464  if (hasJob)
465  {
466  startLocalization(container);
467  }
468 
469  }
470  }();
471 
472  try
473  {
474  predictPoses();
475  }
476  catch (...)
477  {
478  ARMARX_ERROR << "predicting poses caused an exception!";
480  }
481  }
482 
483  void ObjectLocalizationMemoryUpdater::runJobs()
484  {
485  try
486  {
487  scheduleJobs();
488  }
489  catch (std::exception&)
490  {
491  ARMARX_ERROR << "Scheduling jobs threw an exception!";
493  }
494  }
495 
496 
497 
498 
499  void ObjectLocalizationMemoryUpdater::startLocalization(const LocalizationJobContainerPtr& jobContainer)
500  {
501  ObjectClassNameList objectClassNames = jobContainer->getClassNamesUnique();
502  std::string recognitionMethod = jobContainer->getRecognitionMethod();
503 
504  ObjectLocalizerInterfacePrx proxy = getProxyCached(recognitionMethod);
505 
506  if (!proxy)
507  {
508  ARMARX_ERROR << deactivateSpam(5) << "No running proxy found for recognition method: " << recognitionMethod;
509  // update queries so that statcharts waiting on queries can continue
510  finishJobContainer(jobContainer);
511  return;
512  }
513 
514 
515 
516  // ARMARX_DEBUG_S << "Got shared robot proxy " << sharedRobotProxy;
517  // make snapshot of robot state at latest possible point
518  auto robotName = robotStateInterfacePrx->getSynchronizedRobot()->getName();
519  armarx::SharedRobotInterfacePrx sharedRobotProxy = robotStateInterfacePrx->getRobotSnapshot(robotName);
520  jobContainer->setRobotState(sharedRobotProxy);
521 
522  // get global pose of robot in world CS, if available
524  robotPose = armarx::PosePtr::dynamicCast(sharedRobotProxy->getGlobalPose());
525 
526 
527  jobContainer->setRobotPose(robotPose);
528 
529 
530 
531  // start jobs
532  setRecognitionMethodRunningState(recognitionMethod, true);
533 
534  jobContainer->startJobs();
535  ARMARX_VERBOSE << deactivateSpam(5, jobContainer->getRecognitionMethod()) << "starting localization of " << objectClassNames << " with " << jobContainer->getRecognitionMethod();
536  proxy->begin_localizeObjectClasses(objectClassNames, Ice::newCallback(this, &ObjectLocalizationMemoryUpdater::localizationFinished), jobContainer);
537  }
538 
539 
540 
541 
542  bool ObjectLocalizationMemoryUpdater::isRecognitionMethodRunning(const std::string& recognitionMethod)
543  {
544  std::unique_lock lock(recognitionMethodStateMutex);
545  std::map<std::string, bool>::iterator iter = recognitionMethodRunning.find(recognitionMethod);
546 
547  if (iter == recognitionMethodRunning.end())
548  {
549  return false;
550  }
551 
552  bool result = recognitionMethodRunning[recognitionMethod];
553  return result;
554  }
555 
556 
557 
558 
559  void ObjectLocalizationMemoryUpdater::setRecognitionMethodRunningState(const std::string& recognitionMethod, bool running)
560  {
561  std::unique_lock lock(recognitionMethodStateMutex);
562  ARMARX_DEBUG << deactivateSpam(1) << "Setting running state of recognition method " << recognitionMethod << " to " << running;
563  recognitionMethodRunning[recognitionMethod] = running;
564  }
565 
566 
567 
568  void ObjectLocalizationMemoryUpdater::finishJobContainer(LocalizationJobContainerPtr jobContainer)
569  {
570  // set all jobs in the container to finished
571  jobContainer->finishJobs();
572  setRecognitionMethodRunningState(jobContainer->getRecognitionMethod(), false);
573  // update queries in object class segment
574  updateQueries(jobContainer);
575  }
576 
577 
578  void ObjectLocalizationMemoryUpdater::localizationFinished(const Ice::AsyncResultPtr& r)
579  {
580 
581  ObjectLocalizerInterfacePrx objectLocalizer = ObjectLocalizerInterfacePrx::uncheckedCast(r->getProxy());
582  LocalizationJobContainerPtr jobContainer = LocalizationJobContainerPtr::dynamicCast(r->getCookie());
583  ObjectLocalizationResultList resultList;
584 
585  try
586  {
587  resultList = objectLocalizer->end_localizeObjectClasses(r);
588 
589 
590  if (jobContainer->getClassNamesUnique().empty())
591  {
592  ARMARX_WARNING << "There is a bug: Returned from localization, but list of class names is empty";
593  finishJobContainer(jobContainer);
594  return;
595  }
596  std::string allObjectNames;
597 
598  for (size_t i = 0; i < jobContainer->getClassNamesUnique().size(); i++)
599  {
600  allObjectNames.append(jobContainer->getClassNamesUnique().at(i));
601  allObjectNames.append(" ");
602  }
603 
604  ARMARX_DEBUG << deactivateSpam(1) << "Localization of " << allObjectNames << " returned - number of detected instances: " << resultList.size();
605 
606 
607  if (resultList.empty())
608  {
609  // if the object was not found although it should have been visible, reduce the existence certainty
610  if (checkFieldOfView)
611  {
612  ObjectInstanceList instances = objectInstanceSegment->getObjectInstancesByClass(jobContainer->getClassNamesUnique().at(0));
613 
614  if (instances.size() == 1)
615  {
616  ObjectInstancePtr instance = ObjectInstancePtr::dynamicCast(instances.at(0));
617  float oldExistenceCertainty = instance->getExistenceCertainty();
618  instance->setExistenceCertainty(getProperty<float>("ExistenceCertaintyReductionFactorWhenLocalizationFailed").getValue() * oldExistenceCertainty);
619  // if(instance->getExistenceCertainty() < 0.1)
620  // instance->setExistenceCertainty(0.1);
621  ARMARX_INFO << deactivateSpam(3, instance->getName()) << instance->getName() << " not found in view altough it should be there - decreasing existence certainty to: " << instance->getExistenceCertainty();
622  if (objectInstanceSegment->getListenerProxy() && objectInstanceSegment->hasEntityById(instance->getId(), Ice::emptyCurrent))
623  {
624  auto reportClone = instance->clone();
625  objectInstanceSegment->getListenerProxy()->reportEntityUpdated(objectInstanceSegment->getSegmentName(), reportClone, reportClone);
626  }
627  }
628  else if (instances.size() > 1)
629  {
630  ARMARX_IMPORTANT << "Dealing with failed localizations is not implemented for the case that multiple instances exist";
631  }
632  }
633  }
634  else
635  {
636  // generate / fuse instances
637  const std::string rootNodeName = jobContainer->getRobotState()->getRootNode()->getName();
638  for (const ObjectLocalizationResult& result : resultList)
639  {
640 
641  if (result.objectClassName.empty())
642  {
643  ARMARX_ERROR << "Object name in localization result is empty";
644  }
645 
646  armarx::SharedRobotInterfacePrx robotState = jobContainer->getRobotState();
647  armarx::PosePtr robotPose = jobContainer->getRobotPose();
648  if (result.timeStamp)
649  {
650  armarx::TimestampVariantPtr timestamp = armarx::TimestampVariantPtr::dynamicCast(result.timeStamp);
652  ARMARX_INFO << deactivateSpam(3) << "object pose is already " << (now - timestamp->toTime()).toMilliSecondsDouble() << "ms old";
653  robotState = robotStateInterfacePrx->getRobotSnapshotAtTimestamp(timestamp->getTimestamp());
654  if (!robotState)
655  {
656  ARMARX_WARNING << deactivateSpam(2) << "getRobotSnapshotAtTimestamp returned NULL, using current robot state "
657  << "(possibly a bug in the simulation, time server, or something else)";
658  robotState = robotStateInterfacePrx->getRobotSnapshot("");
659  }
660  robotPose = armarx::PosePtr::dynamicCast(robotState->getGlobalPose());
661  jobContainer->setRobotState(robotState);
662  jobContainer->setRobotPose(robotPose);
663  }
664  else
665  {
666  ARMARX_WARNING << deactivateSpam(1, jobContainer->getRecognitionMethod()) << "Recognition method " << jobContainer->getRecognitionMethod() << " did not provide a timestamp - using robotstate from before starting recognition";
667  }
668  ARMARX_DEBUG << deactivateSpam(1, result.objectClassName) << "Object: " << result.objectClassName;
669 
670  // new instance for updating
671  ObjectInstancePtr update = new ObjectInstance(result.instanceName.empty() ? result.objectClassName : result.instanceName);
672  update->setLocalizationTimestamp(result.timeStamp);
673  // position and orientation
674  armarx::FramedPositionPtr measuredPosition = armarx::FramedPositionPtr::dynamicCast(result.position);
675  armarx::FramedOrientationPtr measuredOrientation = armarx::FramedOrientationPtr::dynamicCast(result.orientation);
676 
677  Eigen::Vector3f positionBeforeFrameChange = measuredPosition->toEigen();
678  measuredPosition->changeFrame(robotState, rootNodeName);
679  measuredOrientation->changeFrame(robotState, rootNodeName);
680  Eigen::Vector3f positionTranslation = measuredPosition->toEigen() - positionBeforeFrameChange;
681  Eigen::Matrix4f measuredPose = math::Helpers::Pose(measuredPosition->toEigen(),
682  measuredOrientation->toEigen());
683 
684  ARMARX_DEBUG << deactivateSpam(1, result.objectClassName) << result.objectClassName
685  << "New pose from localizer: " << measuredPose << " (in reference frame " << measuredPosition->frame << ")";
686 
687  // now we have new position and orientation in reference frame
688  armarx::FramedPositionPtr posInRef = new armarx::FramedPosition(measuredPose, rootNodeName, measuredPosition->agent);
689  armarx::FramedOrientationPtr orientationInRef = new armarx::FramedOrientation(measuredPose, rootNodeName, measuredPosition->agent);
690  armarx::LinkedPosePtr linkedPose = new armarx::LinkedPose(measuredPose, rootNodeName, robotState);
691 
692  update->setPosition(posInRef);
693  update->setOrientation(orientationInRef);
694 
695  int priority = 0.0;
696  for (LocalizationQueryPtr& q : jobContainer->getQueries())
697  {
698  if (q->className == result.objectClassName)
699  {
700  priority = q->priority;
701  }
702  }
703  update->setLocalizationPriority(priority);
704 
705  // it is not obvious how to rotate the covariance matrix. it seems that it is not as simple as this:
706  // MultivariateNormalDistributionPtr uncertainty = MultivariateNormalDistributionPtr::dynamicCast(result.positionNoise);
707  // Eigen::Matrix3f covariance = uncertainty->toEigenCovariance().block<3,3>(0,0);
708  // Eigen::Matrix3f covarianceInRef = transformationToReferenceFrame.block<3,3>(0,0) * covariance;
709  // MultivariateNormalDistributionPtr uncertaintyInRef = new MultivariateNormalDistribution(posInRef->toEigen(), covarianceInRef);
710  MultivariateNormalDistributionBasePtr uncertaintyInRef = result.positionNoise;
711  auto mean = uncertaintyInRef->getMean();
712  if (mean.size() == 3)
713  {
714  Eigen::Vector3f meanEigen(mean.at(0), mean.at(1), mean.at(2));
715  meanEigen += positionTranslation;
716  uncertaintyInRef->setMean(memoryx::FloatVector {meanEigen(0), meanEigen(1), meanEigen(2)});
717  }
718  update->setPositionUncertainty(uncertaintyInRef);
719 
720  // classes
721  update->addClass(result.objectClassName, result.recognitionCertainty);
722 
723  // existence certainty
724  ARMARX_INFO << deactivateSpam(5, result.objectClassName) << result.objectClassName << ": existence certainty: " << result.recognitionCertainty;
725  update->setExistenceCertainty(result.recognitionCertainty);
726 
727  // find correspondence in ObjectInstanceLayer (TODO: Method not using motion models and correct reference frame (LinkedCoordinate))
728  ObjectInstancePtr correspondingEntity = ObjectInstancePtr::dynamicCast(objectInstanceSegment->getCorrespondingObjectInstance(update));
729 
730  // add entity to or update memory segment
731  if (!correspondingEntity)
732  {
733  ARMARX_INFO << "Adding new entity " << update->getName();
734 
735  if (!update->getMotionModel())
736  {
737  setMotionModel(update);
738  }
739 
740  update->getMotionModel()->setPoseAtLastLocalisation(linkedPose, robotPose, uncertaintyInRef);
741  objectInstanceSegment->addEntity(update);
742  }
743  else
744  {
745  ARMARX_DEBUG << "updating entity: " << update->getName();
746 
747  std::unique_lock lock(predictionMutex);
748 
749  if (!correspondingEntity->getMotionModel())
750  {
751  ARMARX_IMPORTANT << "Corresponding entity has no motion model, creating a new one";
752  setMotionModel(correspondingEntity);
753  correspondingEntity->getMotionModel()->setPoseAtLastLocalisation(linkedPose, robotPose, uncertaintyInRef);
754  }
755 
756  AbstractMotionModelPtr motionModel = ObjectInstancePtr::dynamicCast(correspondingEntity)->getMotionModel();
757 
758  if (motionModel)
759  {
760  // update->setMotionModel(motionModel);
761  // insert into memory to give the fusion methods a chance
762  objectInstanceSegment->updateEntityInternal(correspondingEntity->getId(), update);
763  ARMARX_DEBUG << /*deactivateSpam(1, update->getName())<< */ "Ori of update: " << *update->getOrientation();
764 
765  // update motion model and predict current pose
766  ObjectInstancePtr objectInstance = ObjectInstancePtr::dynamicCast(objectInstanceSegment->getEntityById(correspondingEntity->getId()));
767  // ARMARX_INFO << deactivateSpam(1) << "Object Instance localization timestamp:" << (objectInstance->hasLocalizationTimestamp() ? objectInstance->getLocalizationTimestamp().toDateTime() : "none") << " timestamp of update: " <<
768  // (update->hasLocalizationTimestamp() ? update->getLocalizationTimestamp().toDateTime() : "none") ;
769  ARMARX_DEBUG /*<< deactivateSpam(1, update->getName()) */ << update->getName() << " obj Pose from memory: " << objectInstance->getPose()->output() << " robot global pose: " << jobContainer->getRobotState()->getGlobalPose()->output();
770  objectInstance->setLocalizationTimestamp(result.timeStamp);
771  armarx::LinkedPosePtr linkedObjectPose = new armarx::LinkedPose(*objectInstance->getPose(), robotState);
772  ARMARX_DEBUG << /*deactivateSpam(1, update->getName()) <<*/ "Ori after fusion: " << *objectInstance->getOrientation();
773 
774  motionModel->setPoseAtLastLocalisation(linkedObjectPose, robotPose, objectInstance->getPositionUncertainty());
775  armarx::LinkedPosePtr predictedPose = armarx::LinkedPosePtr::dynamicCast(motionModel->getPredictedPose());
776  ARMARX_DEBUG << /*deactivateSpam(1, update->getName()) <<*/ VAROUT(predictedPose->output());
777  objectInstance->setPose(predictedPose);
778  }
779  else
780  {
781  ARMARX_WARNING << "no motion model for obj instance " << update->getName() << " set";
782  }
783 
784  }
785 
786  if (commonPlacesLearnerProxy)
787  {
788  ARMARX_VERBOSE << "Using object instance " << VAROUT(update) << " to train common places learner";
789  try
790  {
791  commonPlacesLearnerProxy->learnFromObject(update);
792  }
793  catch (...)
794  {
795  ARMARX_INFO << "Learning failed: " << armarx::GetHandledExceptionString();
796  }
797  }
798  }
799  }
800  }
801  catch (std::exception& e)
802  {
803  ARMARX_ERROR << deactivateSpam(2, jobContainer->getRecognitionMethod()) << "Localization failed of type " << jobContainer->getRecognitionMethod() << "\nReason: " << e.what();
804  //handleExceptions();
805  }
806 
807 
808 
809  finishJobContainer(jobContainer);
810  }
811 
812 
813 
814 
815  void ObjectLocalizationMemoryUpdater::updateQueries(const LocalizationJobContainerPtr& jobContainer)
816  {
817  std::vector<LocalizationQueryPtr> queries = jobContainer->getQueries();
818 
819  for (std::vector<LocalizationQueryPtr>::iterator iter = queries.begin() ; iter != queries.end() ; iter++)
820  {
821  LocalizationQueryPtr query = *iter;
822 
823  if (query->getFinished())
824  {
825  // update working memory segment
826  EntityBasePtr entity = objectClassSegment->getEntityByName(query->className, Ice::emptyCurrent);// pass Ice::Current to get a safe clone
827 
828  if (entity)
829  {
830  ObjectClassPtr objectClass = ObjectClassPtr::dynamicCast(entity);
831 
832  if (objectClass)
833  {
834  EntityWrappers::ObjectRecognitionWrapperPtr recognitionWrapper = objectClass->addWrapper(new EntityWrappers::ObjectRecognitionWrapper());
835  recognitionWrapper->updateLocalizationQuery(query->queryName, query);
836  }
837 
838  ARMARX_DEBUG << deactivateSpam(1) << "Updating query " << query->queryName << " with state " << query->getFinished();
839 
840  // update entity in segment with current state
841  objectClassSegment->updateClass(query->className, objectClass);
842  }
843  }
844  }
845  }
846 
847 
848 
849 
850  ObjectLocalizerInterfacePrx ObjectLocalizationMemoryUpdater::getProxyCached(const std::string& recognitionMethod)
851  {
852  // find proxy in list of already obtained proxies
853  std::map<std::string, ObjectLocalizerInterfacePrx>::iterator iter;
854  iter = knownObjectProxies.find(recognitionMethod);
855 
856  if (iter != knownObjectProxies.end())
857  {
858  // TODO: check if proxy is still valid
859  return iter->second;
860  }
861 
862  // create new proxy if not found
863  ObjectLocalizerInterfacePrx objectLocalizerProxy;
864 
865  try
866  {
867  objectLocalizerProxy = getWorkingMemory()->getProxy<ObjectLocalizerInterfacePrx>(recognitionMethod);
868  }
869  catch (...)
870  {
871  // exceptions only indicate an issue with the proxy
872  // this is handled below when the Prx is checked for NULL
873  }
874 
875  if (objectLocalizerProxy)
876  {
877  std::pair<std::string, ObjectLocalizerInterfacePrx> entry;
878  entry.first = recognitionMethod;
879  entry.second = getWorkingMemory()->getProxy<ObjectLocalizerInterfacePrx>(recognitionMethod);
880 
881  knownObjectProxies.insert(entry);
882  }
883 
884  return objectLocalizerProxy;
885  }
886 
887 
888 
889 
890  LocalizationQueryList ObjectLocalizationMemoryUpdater::getMissingQueries(const LocalizationQueryList& queriesBase, const LocalizationQueryList& queriesCompare)
891  {
892  LocalizationQueryList missing;
893 
894  for (LocalizationQueryList::const_iterator iter = queriesCompare.begin() ; iter != queriesCompare.end() ; iter++)
895  {
896  bool found = false;
897 
898  for (LocalizationQueryList::const_iterator iterBase = queriesBase.begin() ; iterBase != queriesBase.end(); iterBase++)
899  {
900  if ((*iterBase)->queryName == (*iter)->queryName)
901  {
902  found = true;
903  }
904  }
905 
906  if (!found)
907  {
908  missing.push_back(*iter);
909  }
910  }
911 
912  return missing;
913  }
914 
915 
916 
917 
918  bool ObjectLocalizationMemoryUpdater::checkObjectVisibility(armarx::FramedPositionPtr pos)
919  {
920  if (!checkFieldOfView)
921  {
922  return true;
923  }
924 
925  auto robot = robotStateInterfacePrx->getSynchronizedRobot();
926  armarx::FramedPositionPtr posInLeftCamCS = armarx::FramedPositionPtr::dynamicCast(pos->clone());
927  posInLeftCamCS->changeFrame(robot, robotNodeNameLeftCamera);
928  Eigen::Vector3f positionInLeftCamCS = posInLeftCamCS->toEigen();
929  armarx::FramedPositionPtr posInRightCamCS = armarx::FramedPositionPtr::dynamicCast(pos->clone());
930  posInRightCamCS->changeFrame(robot, robotNodeNameRightCamera);
931  Eigen::Vector3f positionInRightCamCS = posInRightCamCS->toEigen();
932 
933  // if object is too far away, don't localize
934  if (positionInLeftCamCS(2) > getProperty<float>("MaximalObjectDistance").getValue())
935  {
936  return false;
937  }
938 
939  Eigen::Vector3f viewDirection = Eigen::Vector3f::UnitZ();
940  positionInLeftCamCS.normalize();
941  positionInRightCamCS.normalize();
942  float angleLeft = acos(viewDirection.dot(positionInLeftCamCS));
943  float angleRight = acos(viewDirection.dot(positionInRightCamCS));
944 
945  if (2 * angleLeft < cameraOpeningAngle && 2 * angleRight < cameraOpeningAngle)
946  {
947  return true;
948  }
949  else
950  {
951  return false;
952  }
953  }
954 
955  void ObjectLocalizationMemoryUpdater::setMotionModel(ObjectInstancePtr& objectInstance)
956  {
957  std::string objectClassName = objectInstance->getMostProbableClass();
958  // create a motion model
959  ObjectClassPtr objectClass = ObjectClassPtr::dynamicCast(objectClassSegment->getEntityByName(objectClassName));
960  EntityWrappers::ObjectRecognitionWrapperPtr recognitionWrapper = objectClass->getWrapper<EntityWrappers::ObjectRecognitionWrapper>();
961 
962  if (!recognitionWrapper)
963  {
964  recognitionWrapper = objectClass->addWrapper(new EntityWrappers::ObjectRecognitionWrapper());
965  }
966 
967  std::string motionModelName = recognitionWrapper->getDefaultMotionModel();
968  ARMARX_INFO << "Motion model is " << motionModelName;
970  AbstractMotionModelPtr motionModel;
971 
972  switch (motionModelType)
973  {
975  {
976  motionModel = new MotionModelStaticObject(robotStateInterfacePrx);
977  break;
978  }
979 
981  {
982  std::string handNodeName;
983 
984  if (objectClassName.find("left") != std::string::npos || objectClassName.find("Left") != std::string::npos)
985  {
986  handNodeName = handNodeNameLeft;
987  }
988  else
989  {
990  handNodeName = handNodeNameRight;
991  }
992 
993  ARMARX_INFO << "The hand node name in the kinematic model is assumed to be " << handNodeName;
994  motionModel = new MotionModelRobotHand(robotStateInterfacePrx, handNodeName);
995  break;
996  }
997 
999  {
1000  if (armarx::Contains(objectClassName, "left", true))
1001  {
1002  motionModel = new MotionModelKBM(getProperty<std::string>("KBMReferenceNodeName").getValue(), getProperty<std::string>("KBMLeftArmNodeSetName").getValue(), robotStateInterfacePrx, longtermMemoryPrx);
1003  }
1004  else
1005  {
1006  motionModel = new MotionModelKBM(getProperty<std::string>("KBMReferenceNodeName").getValue(), getProperty<std::string>("KBMRightArmNodeSetName").getValue(), robotStateInterfacePrx, longtermMemoryPrx);
1007  }
1008 
1009  break;
1010  }
1011 
1012  default:
1013  ARMARX_WARNING << "Motion model type " << motionModelName << " not yet implemented here!";
1014  motionModel = new MotionModelStaticObject(robotStateInterfacePrx);
1015  break;
1016  }
1017 
1018  objectInstance->setMotionModel(motionModel);
1019  }
1020 }
MotionModelRobotHand.h
Gaussian::getCovariance
const covariance_type & getCovariance() const
Definition: Gaussian.h:77
memoryx::AbstractMotionModelPtr
IceInternal::Handle< AbstractMotionModel > AbstractMotionModelPtr
Definition: ObjectInstance.h:45
LinkedPose.h
ARMARX_VERBOSE
#define ARMARX_VERBOSE
Definition: Logging.h:180
memoryx::LocalizationJobContainerPtr
IceUtil::Handle< LocalizationJobContainer > LocalizationJobContainerPtr
Definition: LocalizationJobContainer.h:163
ARMARX_IMPORTANT
#define ARMARX_IMPORTANT
Definition: Logging.h:183
armarx::viz::toEigen
Eigen::Matrix4f toEigen(data::GlobalPose const &pose)
Definition: Interaction.h:46
OnScopeExit.h
memoryx::WorkingMemoryUpdater::getWorkingMemory
AbstractWorkingMemoryPtr getWorkingMemory()
Definition: WorkingMemoryUpdater.h:87
armarx::navigation::core::Pose
Eigen::Isometry3f Pose
Definition: basic_types.h:31
ObjectLocalizationMemoryUpdater.h
FileIOException.h
memoryx::LocalizationJobPtr
IceUtil::Handle< LocalizationJob > LocalizationJobPtr
Definition: LocalizationJob.h:67
armarx::Contains
bool Contains(const ContainerType &container, const ElementType &searchElement)
Definition: algorithm.h:295
memoryx::ObjectLocalizationMemoryUpdater::onConnectComponent
void onConnectComponent() override
Pure virtual hook for the subclass.
Definition: ObjectLocalizationMemoryUpdater.cpp:126
memoryx
VirtualRobot headers.
Definition: CommonPlacesTester.cpp:48
ObjectRecognitionWrapper.h
StringHelpers.h
memoryx::EntityWrappers::ObjectRecognitionWrapperPtr
IceInternal::Handle< ObjectRecognitionWrapper > ObjectRecognitionWrapperPtr
Definition: ObjectRecognitionWrapper.h:56
cxxopts::empty
bool empty(const std::string &s)
Definition: cxxopts.hpp:255
memoryx::AbstractMotionModel::getMotionModelTypeByName
static EMotionModelType getMotionModelTypeByName(std::string motionModelName)
Definition: AbstractMotionModel.cpp:89
IceInternal::Handle< ObjectClass >
armarx::GetHandledExceptionString
std::string GetHandledExceptionString()
Definition: Exception.cpp:147
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:523
memoryx::EarlyVisionConverters::convertToGaussian
Gaussian convertToGaussian(const NormalDistributionBasePtr &normalDistribution)
Definition: EarlyVisionConverters.cpp:29
armarx::VariantType::LinkedPose
const VariantTypeId LinkedPose
Definition: LinkedPose.h:43
armarx::mean
std::optional< float > mean(const boost::circular_buffer< NameValueMap > &buffer, const std::string &key)
Definition: KinematicUnitGuiPlugin.cpp:1615
armarx::ValueToString
std::string ValueToString(const T &value)
Definition: StringHelpers.h:58
armarx::FloatVector
::std::vector< ::Ice::Float > FloatVector
Definition: KinematicUnitGuiPlugin.h:327
MotionModelStaticObject.h
ARMARX_DEBUG
#define ARMARX_DEBUG
Definition: Logging.h:177
armarx::FramedPosition
The FramedPosition class.
Definition: FramedPose.h:142
memoryx::ObjectLocalizationMemoryUpdater::ObjectLocalizationMemoryUpdater
ObjectLocalizationMemoryUpdater()
Definition: ObjectLocalizationMemoryUpdater.cpp:49
memoryx::ObjectInstancePtr
IceInternal::Handle< ObjectInstance > ObjectInstancePtr
Definition: ObjectInstance.h:42
memoryx::AbstractMotionModel::eMotionModelRobotHand
@ eMotionModelRobotHand
Definition: AbstractMotionModel.h:45
memoryx::ObjectLocalizationMemoryUpdater::reportEntityUpdated
void reportEntityUpdated(const std::string &segmentName, const EntityBasePtr &entityOld, const EntityBasePtr &entityNew, const Ice::Current &c=Ice::emptyCurrent) override
Definition: ObjectLocalizationMemoryUpdater.cpp:166
memoryx::AbstractMotionModel::eMotionModelKBM
@ eMotionModelKBM
Definition: AbstractMotionModel.h:47
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:189
armarx::FramedOrientation
The FramedOrientation class.
Definition: FramedPose.h:199
q
#define q
armarx::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
armarx::armem::server::ltm::util::mongodb::detail::update
bool update(mongocxx::collection &coll, const nlohmann::json &query, const nlohmann::json &update)
Definition: mongodb.cpp:67
LocalizationStrategyOnce.h
armarx::TimeUtil::GetTime
static IceUtil::Time GetTime(TimeMode timeMode=TimeMode::VirtualTime)
Get the current time.
Definition: TimeUtil.cpp:42
armarx::control::common::getValue
T getValue(nlohmann::json &userConfig, nlohmann::json &defaultConfig, const std::string &entryName)
Definition: utils.h:55
armarx::ManagedIceObject::usingTopic
void usingTopic(const std::string &name, bool orderedPublishing=false)
Registers a proxy for subscription after initialization.
Definition: ManagedIceObject.cpp:248
memoryx::LocalizationQueryPtr
IceInternal::Handle< LocalizationQuery > LocalizationQueryPtr
Definition: LocalizationQuery.h:43
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:601
AbstractMotionModel.h
armarx::VariantType::FramedOrientation
const VariantTypeId FramedOrientation
Definition: FramedPose.h:40
armarx::FramedDirection
FramedDirection is a 3 dimensional direction vector with a reference frame. The reference frame can b...
Definition: FramedPose.h:83
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
VAROUT
#define VAROUT(x)
Definition: StringHelpers.h:182
IceUtil::Handle
Definition: forward_declarations.h:29
EarlyVisionConverters.h
IceInternal::ProxyHandle< ::IceProxy::armarx::SharedRobotInterface >
memoryx::ObjectLocalizationMemoryUpdater::onDisconnectComponent
void onDisconnectComponent() override
Hook for subclass.
Definition: ObjectLocalizationMemoryUpdater.cpp:152
memoryx::EntityWrappers::ObjectRecognitionWrapper
Definition: ObjectRecognitionWrapper.h:40
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
memoryx::AbstractMotionModel::eMotionModelStaticObject
@ eMotionModelStaticObject
Definition: AbstractMotionModel.h:44
memoryx::ObjectLocalizationMemoryUpdater::setSegmentNames
void setSegmentNames(std::string classSegmentName, std::string instanceSegmentName)
Definition: ObjectLocalizationMemoryUpdater.cpp:60
memoryx::ObjectClassPtr
IceInternal::Handle< ObjectClass > ObjectClassPtr
Definition: ObjectClass.h:35
armarx::PeriodicTask
Definition: ArmarXManager.h:70
memoryx::ObjectLocalizationMemoryUpdater::setReferenceFrame
void setReferenceFrame(const std::string &referenceFrameName)
Definition: ObjectLocalizationMemoryUpdater.cpp:70
armarx::VariantType::FramedPosition
const VariantTypeId FramedPosition
Definition: FramedPose.h:39
Gaussian
Definition: Gaussian.h:46
armarx::handleExceptions
void handleExceptions()
Definition: Exception.cpp:141
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:186
LocalizationStrategyRepeated.h
memoryx::ObjectLocalizationMemoryUpdater::onInitComponent
void onInitComponent() override
Pure virtual hook for the subclass.
Definition: ObjectLocalizationMemoryUpdater.cpp:81
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
armarx::DataFieldIdentifier
DataFieldIdentifier provide the basis to identify data field within a distributed ArmarX scenario.
Definition: DataFieldIdentifier.h:48
memoryx::AbstractMotionModel::EMotionModelType
EMotionModelType
Definition: AbstractMotionModel.h:42