CommonPlacesTester.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
5  *
6  * ArmarX is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License version 2 as
8  * published by the Free Software Foundation.
9  *
10  * ArmarX is distributed in the hope that it will be useful, but
11  * WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program. If not, see <http://www.gnu.org/licenses/>.
17  *
18  * @package
19  * @author
20  * @date
21  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
22  * GNU General Public License
23  */
24 
25 
26 #include "CommonPlacesTester.h"
27 
28 #include <RobotAPI/libraries/core/Pose.h>
29 
30 #include <MemoryX/interface/memorytypes/MemoryEntities.h>
31 
32 #include <MemoryX/libraries/memorytypes/entity/ObjectInstance.h>
33 #include <MemoryX/libraries/memorytypes/entity/ObjectClass.h>
34 #include <MemoryX/libraries/helpers/GaussianMixtureHelpers/ISDDistance.h>
35 #include <MemoryX/libraries/helpers/GaussianMixtureHelpers/MahalanobisDistance.h>
36 
37 // Object Factories
38 #include <ArmarXCore/observers/ObserverObjectFactories.h>
39 #include <MemoryX/core/MemoryXCoreObjectFactories.h>
40 #include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
41 
42 #include <Eigen/Core>
43 
44 #include <fstream>
45 #include <iostream>
46 #include <thread>
47 
48 namespace memoryx
49 {
50  void CommonPlacesTester::onInitComponent()
51  {
52  usingProxy("WorkingMemory");
53  usingProxy("LongtermMemory");
54  usingProxy("CommonPlacesLearner");
55 
56  testToRun = getProperty<std::string>("TestToRun").getValue();
57  }
58 
59 
60  void CommonPlacesTester::onConnectComponent()
61  {
62  std::cout << "Starting CommonPlacesTester" << std::endl;
63 
64  const Ice::CommunicatorPtr ic = getIceManager()->getCommunicator();
65 
66 
67  memoryPrx = getProxy<WorkingMemoryInterfacePrx>("WorkingMemory");
68  longtermMemoryPrx = getProxy<LongtermMemoryInterfacePrx>("LongtermMemory");
69  cpLearnerPrx = getProxy<CommonPlacesLearnerInterfacePrx>("CommonPlacesLearner");
70 
71  ltmSegmentName = getProperty<std::string>("LTMSegment").getValue();
72  cpLearnerPrx->setLTMSegmentName(ltmSegmentName);
73 
74  objectToLoad = getProperty<std::string>("ObjectToLoad").getValue();
75 
76  // load snapshot, if specified
77  const std::string snapshotName = getProperty<std::string>("SnapshotToLoad").getValue();
78 
79  if (!snapshotName.empty())
80  {
81  longtermMemoryPrx->loadWorkingMemorySnapshot(snapshotName, memoryPrx);
82  ARMARX_INFO << "Snapshot succcessfully loaded: " << snapshotName;
83  }
84 
85  if (testToRun == "LearnFromSnapshot")
86  {
87  testLearnFromSnapshot();
88  }
89  else if (testToRun == "LearnFromSingleSnapshot")
90  {
91  testLearnFromSingleSnapshot();
92  }
93  else if (testToRun == "LoadLTM")
94  {
95  testLoadLTM();
96  }
97  else if (testToRun == "Clustering")
98  {
99  testClustering();
100  }
101  else if (testToRun == "Merging")
102  {
103  testMerging();
104  }
105  else if (testToRun == "Aging")
106  {
107  testAging();
108  }
109  else if (testToRun == "Aging2")
110  {
111  testAging2();
112  }
113  else if (testToRun == "ClusteringBatch")
114  {
115  testClusteringBatch();
116  }
117  else if (testToRun == "GetClusterLocations")
118  {
119  testGetClusterLocations();
120  }
121 
122  else
123  {
124  std::cout << "Unknown test name: " << testToRun << std::endl;
125  }
126 
127  }
128 
129  void CommonPlacesTester::testLearnFromSnapshot()
130  {
131  const std::string snapshotName = getProperty<std::string>("SnapshotsToLearnFrom").getValue();
132  const bool loadToWM = getProperty<bool>("LoadToWM").getValue();
133  const int initDelay = getProperty<int>("InitialDelay").getValue();
134  const int delay = getProperty<int>("DelayBetweenSnapshots").getValue();
135 
136  if (!snapshotName.empty())
137  {
138  std::this_thread::sleep_for(std::chrono::milliseconds(initDelay));
139 
140  NameList snapNameList = armarx::Split(snapshotName, ",");
141 
142  for (NameList::const_iterator it = snapNameList.begin(); it != snapNameList.end(); ++it)
143  {
144  cpLearnerPrx->learnFromSnapshot(*it);
145  ARMARX_INFO << "Snapshot succcessfully processed by CommonPlacesLearner: " << *it;
146 
147  if (loadToWM)
148  {
149  testLoadLTM();
150  }
151 
152  std::this_thread::sleep_for(std::chrono::milliseconds(delay));
153  }
154  }
155  else
156  {
157  ARMARX_ERROR << "SnapshotsToLearnFrom parameter must be specified! ";
158  }
159  }
160 
161  void CommonPlacesTester::testLearnFromSingleSnapshot()
162  {
163  const std::string snapshotName = getProperty<std::string>("SnapshotsToLearnFrom").getValue();
164  const bool loadToWM = getProperty<bool>("LoadToWM").getValue();
165  const int initDelay = getProperty<int>("InitialDelay").getValue();
166  const int delay = getProperty<int>("DelayBetweenSnapshots").getValue();
167 
168  std::this_thread::sleep_for(std::chrono::milliseconds(initDelay));
169 
170  WorkingMemorySnapshotInterfacePrx snapshot = longtermMemoryPrx->openWorkingMemorySnapshot(snapshotName);
171 
172  if (!snapshot)
173  {
174  ARMARX_ERROR << "Snapshot not found in LTM: " << snapshotName;
175  return;
176  }
177 
178  const std::string segmentName = "objectInstances";
179  PersistentEntitySegmentBasePrx segInstances = snapshot->getSegment(segmentName);
180 
181  if (!segInstances)
182  {
183  ARMARX_ERROR << "Segment not found in snapshot: " << segmentName;
184  return;
185  }
186 
187  EntityIdList ids = segInstances->getAllEntityIds();
188 
189  // sort ids
190  struct EntityIdComparator
191  {
192  static bool compare(const std::string& i, const std::string& j)
193  {
194  return (std::stoi(i) < std::stoi(j));
195  }
196  };
197  std::sort(ids.begin(), ids.end(), EntityIdComparator::compare);
198 
199  for (EntityIdList::const_iterator it = ids.begin(); it != ids.end(); ++it)
200  {
201  EntityBasePtr snapEntity = segInstances->getEntityById(*it);
202  ObjectInstancePtr snapInstance = ObjectInstancePtr::dynamicCast(snapEntity);
203  cpLearnerPrx->learnFromObject(snapInstance);
204  ARMARX_INFO << "Object processed by CommonPlacesLearner: " << *it;
205 
206  if (loadToWM)
207  // testLoadLTM();
208  {
209  testClustering();
210  }
211 
212 
213  std::this_thread::sleep_for(std::chrono::milliseconds(delay));
214  }
215  }
216 
217  void CommonPlacesTester::testLoadLTM()
218  {
219  ObjectInstanceMemorySegmentBasePrx wmObjectsSeg = memoryPrx->getObjectInstancesSegment();
220  PersistentEntitySegmentBasePrx ltmObjectsSeg = longtermMemoryPrx->getCustomInstancesSegment(ltmSegmentName, false);
221 
222  if (ltmObjectsSeg)
223  {
224  if (!objectToLoad.empty())
225  {
226  EntityBasePtr obj = ltmObjectsSeg->getEntityByName(objectToLoad);
227  EntityBasePtr oldWMObj = wmObjectsSeg->getEntityByName(objectToLoad);
228 
229  if (obj)
230  {
231  if (oldWMObj)
232  {
233  wmObjectsSeg->updateEntity(oldWMObj->getId(), obj);
234  }
235  else
236  {
237  wmObjectsSeg->addEntity(obj);
238  }
239 
240  std::cout << "Loaded from LTM: " << ObjectInstancePtr::dynamicCast(obj) << std::endl;
241  }
242  else
243  {
244  ARMARX_ERROR << "Entity not found in LTM: " << objectToLoad;
245  }
246  }
247  else
248  {
249  EntityIdList ids = ltmObjectsSeg->getAllEntityIds();
250 
251  for (EntityIdList::const_iterator it = ids.begin(); it != ids.end(); ++it)
252  {
253  wmObjectsSeg->addEntity(ltmObjectsSeg->getEntityById(*it));
254  }
255  }
256  }
257  else
258  {
259  ARMARX_ERROR << "LTM segment not found: " << ltmSegmentName;
260  }
261  }
262 
263  void CommonPlacesTester::testClustering()
264  {
265  const bool loadToWM = getProperty<bool>("LoadToWM").getValue();
266 
267  if (loadToWM)
268  {
269  testLoadLTM();
270  }
271 
272  float maxDeviation = getProperty<float>("ClusterMaxDeviation").getValue();
273  std::string deviationTypeStr = getProperty<std::string>("ClusterDeviationType").getValue();
274  DeviationType devType = getDeviationTypeFromString(deviationTypeStr);
275 
276  // Cluster3DList posClusters = cpLearnerPrx->getPositionClustersByMaxDeviation(objectToLoad, maxDeviation, devType);
277  // std::cout << "Reduced to clusters: " << std::endl;
278  // int i = 1;
279  // for (Cluster3DList::const_iterator it = posClusters.begin(); it != posClusters.end(); ++it)
280  // {
281  // std::cout << "Cluster " << i <<
282  // ": center = (" << it->center.x << ", " << it->center.y << ", " << it->center.z <<
283  // ") weight = " << it->weight << std::endl;
284  // ++i;
285  // }
286 
287  ObjectInstanceMemorySegmentBasePrx wmObjectsSeg = memoryPrx->getObjectInstancesSegment();
288 
289  mstimer.time_start();
290  GaussianMixtureDistributionPtr posDistr = GaussianMixtureDistributionPtr::dynamicCast(cpLearnerPrx->getPositionReducedByMaxDeviation(objectToLoad, maxDeviation, devType));
291  ARMARX_INFO << "Elapsed time for reduction: " << mstimer.time_stop() << " ms" << std::endl;
292 
293  if (posDistr)
294  {
295  ARMARX_INFO << "Reduced to: " << posDistr->output() << std::endl;
296 
297  EntityBasePtr oldWMObj = wmObjectsSeg->getEntityByName(objectToLoad + "GaussPos");
298 
299  if (oldWMObj)
300  {
301  wmObjectsSeg->removeEntity(oldWMObj->getId());
302  }
303 
304  ObjectInstancePtr obj = new ObjectInstance(objectToLoad + "GaussPos");
305  obj->setClass(objectToLoad, 1.f);
306  NormalDistributionPtr mode = NormalDistributionPtr::dynamicCast(posDistr->getModalComponent().gaussian);
307  armarx::FramedPositionPtr curPos = new armarx::FramedPosition((Eigen::Vector3f) mode->toEigenMean(), armarx::GlobalFrame, "");
308  obj->setPosition(curPos);
309  obj->getPositionAttribute()->setValueWithUncertainty(obj->getPositionAttribute()->getValue(), posDistr);
310 
311  Eigen::Quaternionf quat(0., 0., 0., 1.);
312  armarx::FramedOrientationBasePtr orient = new armarx::FramedOrientation(quat.toRotationMatrix(), armarx::GlobalFrame, "");
313  obj->setOrientation(orient);
314 
315  obj->putAttribute("UncertaintyColorMap", "eHot");
316  obj->putAttribute("UncertaintyTransparency", 0.7f);
317 
318  wmObjectsSeg->addEntity(obj);
319  }
320  else
321  {
322  ARMARX_INFO << "Unable to get object pos gaussian! " << std::endl;
323  }
324 
325  }
326 
327  void CommonPlacesTester::testMerging()
328  {
329  const std::string snapshotName = getProperty<std::string>("SnapshotsToLearnFrom").getValue();
330 
331  //const int sampleSize = getProperty<int>("SampleSize").getValue();
332  const int itCount = getProperty<int>("IterationCount").getValue();
333 
334  const int THRESHOLD_COUNT = 11;
335  const float thresholds[THRESHOLD_COUNT] = {0., 1., 2., 3., 4., 5., 6., 7., 8., 9., 10.};
336  float grandTotalDensity[THRESHOLD_COUNT];
337  float grandTotalKL[THRESHOLD_COUNT];
338  float grandTotalISD[THRESHOLD_COUNT];
339  float grandReductionRate[THRESHOLD_COUNT];
340 
341  for (int i = 0; i < THRESHOLD_COUNT; ++i)
342  {
343  grandTotalDensity[i] = 0;
344  grandTotalKL[i] = 0;
345  grandTotalISD[i] = 0;
346  grandReductionRate[i] = 0;
347  }
348 
349  // RunnallsKLDistance klDistance;
350  ISDDistance isdDistance;
351 
352  NameList snapNameList = armarx::Split(snapshotName, ",");
353 
354  for (NameList::const_iterator it = snapNameList.begin(); it != snapNameList.end(); ++it)
355  {
356  WorkingMemorySnapshotInterfacePrx snapshot = longtermMemoryPrx->openWorkingMemorySnapshot(*it);
357 
358  if (!snapshot)
359  {
360  ARMARX_ERROR << "Snapshot not found in LTM: " << snapshotName;
361  return;
362  }
363 
364  const std::string segmentName = "objectInstances";
365  PersistentEntitySegmentBasePrx segInstances = snapshot->getSegment(segmentName);
366 
367  if (!segInstances)
368  {
369  ARMARX_ERROR << "Segment not found in snapshot: " << segmentName;
370  return;
371  }
372 
373  EntityIdList ids = segInstances->getAllEntityIds();
374 
375  std::ofstream snfile;
376  const std::string fname = "m_" + (*it) + ".csv";
377  snfile.open(fname.c_str());
378 
379  for (int t = 0; t < THRESHOLD_COUNT; ++t)
380  {
381  cpLearnerPrx->setMergingThreshold(thresholds[t]);
382 
383  float totalDensity = 0., totalKL = 0., totalISD = 0.;
384  int origComp = 0, mergedComp = 0;
385 
386  for (int i = 0; i < itCount; ++i)
387  {
388  // EntityIdList ids;
389  // getRandomIds(allIds, ids, sampleSize);
390 
391  std::random_shuffle(ids.begin(), ids.end());
392 
393  longtermMemoryPrx->getCustomInstancesSegment(ltmSegmentName, false)->clear();
394 
395  for (EntityIdList::const_iterator it = ids.begin(); it != ids.end(); ++it)
396  {
397  EntityBasePtr snapEntity = segInstances->getEntityById(*it);
398  ObjectInstancePtr snapInstance = ObjectInstancePtr::dynamicCast(snapEntity);
399  GaussianMixtureDistributionPtr gmm = GaussianMixtureDistributionPtr::dynamicCast(cpLearnerPrx->getPositionFull(snapInstance->getMostProbableClass()));
400 
401  if (gmm)
402  {
403  totalDensity += gmm->getDensity(snapInstance->getPosition()->toEigen());
404  // totalKL += klDistance.getDistance(gmm, GaussianMixtureDistribution::FromProbabilityMeasure(snapInstance->getPositionUncertainty()));
405  // totalISD += isdDistance.getDistance(gmm, GaussianMixtureDistribution::FromProbabilityMeasure(snapInstance->getPositionUncertainty()), true);
406  }
407 
408  cpLearnerPrx->learnFromObject(snapInstance);
409  // ARMARX_INFO << "Object processed by CommonPlacesLearner: " << *it;
410  }
411 
412  GaussianMixtureDistributionPtr mergedGMM = GaussianMixtureDistributionPtr::dynamicCast(cpLearnerPrx->getPositionFull(objectToLoad));
413 
414  // learn w/o merging
415  cpLearnerPrx->setLTMSegmentName(ltmSegmentName + "2");
416  longtermMemoryPrx->getCustomInstancesSegment(ltmSegmentName + "2", false)->clear();
417  cpLearnerPrx->setMergingThreshold(0.);
418 
419  for (EntityIdList::const_iterator it = ids.begin(); it != ids.end(); ++it)
420  {
421  EntityBasePtr snapEntity = segInstances->getEntityById(*it);
422  ObjectInstancePtr snapInstance = ObjectInstancePtr::dynamicCast(snapEntity);
423  cpLearnerPrx->learnFromObject(snapInstance);
424  }
425 
426  GaussianMixtureDistributionPtr origGMM = GaussianMixtureDistributionPtr::dynamicCast(cpLearnerPrx->getPositionFull(objectToLoad));
427  origComp += origGMM->size();
428  mergedComp += mergedGMM->size();
429  totalISD += isdDistance.getDistance(mergedGMM, origGMM, true);
430  cpLearnerPrx->setLTMSegmentName(ltmSegmentName);
431  }
432 
433  totalISD /= itCount;
434  const float redRate = float(mergedComp) / float(origComp);
435 
436  // write snap file
437  snfile << thresholds[t] << "," << totalDensity << "," << totalISD <<
438  "," << float(origComp) / itCount << "," << float(mergedComp) / itCount
439  << "," << float(mergedComp) / float(origComp)
440  << std::endl;
441 
442  grandTotalDensity[t] += totalDensity;
443  grandTotalISD[t] += totalISD;
444  grandTotalKL[t] += totalKL;
445  grandReductionRate[t] += redRate;
446  // ARMARX_INFO << "Total density: " << totalDensity / itCount;
447  }
448 
449  snfile.close();
450  } // end snapshots
451 
452  // write total file
453  std::ofstream totfile;
454  totfile.open("m_totals.csv");
455 
456  for (int t = 0; t < THRESHOLD_COUNT; ++t)
457  totfile << thresholds[t] << "," << (grandTotalDensity[t] / snapNameList.size()) <<
458  "," << (grandTotalISD[t] / snapNameList.size()) <<
459  "," << (grandReductionRate[t] / snapNameList.size())
460  << std::endl;
461 
462  totfile.close();
463 
464  }
465 
466  void CommonPlacesTester::testAging()
467  {
468  const std::string snapshotName = getProperty<std::string>("SnapshotsToLearnFrom").getValue();
469 
470  const int itCount = getProperty<int>("IterationCount").getValue();
471 
472  const int COEFF_COUNT = 10;
473  const float thresholds[COEFF_COUNT] = {1.0, 0.9, 0.8, 0.7, 0.6, 0.5, 0.4, 0.3, 0.2, 0.1};
474  float grandTotalMah[COEFF_COUNT];
475  int grandTotalCorrect[COEFF_COUNT];
476  int totalObjects = 0;
477 
478  for (int i = 0; i < COEFF_COUNT; ++i)
479  {
480  grandTotalMah[i] = 0;
481  grandTotalCorrect[i] = 0;
482  }
483 
484  // RunnallsKLDistance klDistance;
485  ISDDistance isdDistance;
486  MahalanobisDistance mahDistance;
487 
488  NameList snapNameList = armarx::Split(snapshotName, ",");
489 
490  for (NameList::const_iterator it = snapNameList.begin(); it != snapNameList.end(); ++it)
491  {
492  WorkingMemorySnapshotInterfacePrx snapshot = longtermMemoryPrx->openWorkingMemorySnapshot(*it);
493 
494  if (!snapshot)
495  {
496  ARMARX_ERROR << "Snapshot not found in LTM: " << *it;
497  return;
498  }
499 
500  const std::string segmentName = "objectInstances";
501  PersistentEntitySegmentBasePrx segInstances = snapshot->getSegment(segmentName);
502 
503  if (!segInstances)
504  {
505  ARMARX_ERROR << "Segment not found in snapshot: " << segmentName;
506  return;
507  }
508 
509  ARMARX_INFO << "Processing snapshot: " << *it;
510 
511  EntityIdList ids = segInstances->getAllEntityIds();
512 
513  totalObjects += ids.size();
514 
515  for (int t = 0; t < COEFF_COUNT; ++t)
516  {
517  cpLearnerPrx->setAgingFactor(thresholds[t]);
518 
519  float totalMah = 0.;//, totalKL = 0., totalISD = 0.;
520  int corrClusters = 0;
521 
522  for (int i = 0; i < itCount; ++i)
523  {
524  // EntityIdList ids;
525  // getRandomIds(allIds, ids, sampleSize);
526 
527  std::random_shuffle(ids.begin(), ids.end());
528 
529  longtermMemoryPrx->getCustomInstancesSegment(ltmSegmentName, false)->clear();
530 
531  for (EntityIdList::const_iterator it = ids.begin(); it != ids.end(); ++it)
532  {
533  EntityBasePtr snapEntity = segInstances->getEntityById(*it);
534  ObjectInstancePtr snapInstance = ObjectInstancePtr::dynamicCast(snapEntity);
535  GaussianMixtureDistributionPtr gmm = GaussianMixtureDistributionPtr::dynamicCast(cpLearnerPrx->getPositionReducedByMaxDeviation(objectToLoad, 150, eDevAABB));
536 
537  if (gmm)
538  {
539  totalMah += mahDistance.getWeigtedDistance(gmm, snapInstance->getPositionUncertainty());
540 
541  // discrete criterion: cluster is "correct", if observation mean within 3 sigma from cluster mean
542  GaussianMixtureComponent c = gmm->getComponent(0);
543  NormalDistributionPtr g = NormalDistributionPtr::dynamicCast(c.gaussian);
544  const Eigen::VectorXf obsMean = snapInstance->getPosition()->toEigen();
545  Eigen::Vector3f sigma;
546  sigma << sqrtf(g->getCovariance(0, 0)), sqrtf(g->getCovariance(1, 1)), sqrtf(g->getCovariance(2, 2));
547  const Eigen::VectorXf triSigmaPoint = g->toEigenMean() + 3 * sigma;
548 
549  if (g->getDensity(obsMean) > g->getDensity(triSigmaPoint))
550  {
551  corrClusters++;
552  }
553  }
554 
555  cpLearnerPrx->learnFromObject(snapInstance);
556  // ARMARX_INFO << "Object processed by CommonPlacesLearner: " << *it;
557  }
558  }
559 
560  totalMah /= itCount;
561  const float avgCorrect = float(corrClusters) / itCount;
562 
563  grandTotalMah[t] += totalMah;
564  grandTotalCorrect[t] += avgCorrect;
565  // ARMARX_INFO << "Total density: " << totalDensity / itCount;
566  }
567 
568  } // end snapshots
569 
570  // write total file
571  std::ofstream totfile;
572  totfile.open("a_totals.csv");
573 
574  for (int t = 0; t < COEFF_COUNT; ++t)
575  totfile << thresholds[t] << "," << grandTotalMah[t] << // total mah dist
576  "," << grandTotalMah[t] / totalObjects << // avg mah dist/obj
577  "," << grandTotalCorrect[t] << // total correct clusters
578  "," << float(grandTotalCorrect[t]) / totalObjects << // correct cluster rate
579  std::endl;
580 
581  totfile.close();
582 
583  }
584 
585  void CommonPlacesTester::testAging2()
586  {
587  const std::string snapshotName = getProperty<std::string>("SnapshotsToLearnFrom").getValue();
588 
589  const int itCount = getProperty<int>("IterationCount").getValue();
590 
591  const int COEFF_COUNT = 10;
592  const float thresholds[COEFF_COUNT] = {1.0, 0.9, 0.8, 0.7, 0.6, 0.5, 0.4, 0.3, 0.2, 0.1};
593  float grandTotalMah[COEFF_COUNT];
594  int grandTotalCorrect[COEFF_COUNT];
595  int totalObjects = 0;
596 
597  for (int i = 0; i < COEFF_COUNT; ++i)
598  {
599  grandTotalMah[i] = 0;
600  grandTotalCorrect[i] = 0;
601  }
602 
603  MahalanobisDistance mahDistance;
604 
605  NameList snapNameList = armarx::Split(snapshotName, ",");
606 
607  for (int t = 0; t < COEFF_COUNT; ++t)
608  {
609  cpLearnerPrx->setAgingFactor(thresholds[t]);
610 
611  float totalMah = 0.;
612  int corrClusters = 0;
613 
614  for (int i = 0; i < itCount; ++i)
615  {
616  longtermMemoryPrx->getCustomInstancesSegment(ltmSegmentName, false)->clear();
617 
618  for (NameList::const_iterator it = snapNameList.begin(); it != snapNameList.end(); ++it)
619  {
620  WorkingMemorySnapshotInterfacePrx snapshot = longtermMemoryPrx->openWorkingMemorySnapshot(*it);
621 
622  if (!snapshot)
623  {
624  ARMARX_ERROR << "Snapshot not found in LTM: " << *it;
625  return;
626  }
627 
628  const std::string segmentName = "objectInstances";
629  PersistentEntitySegmentBasePrx segInstances = snapshot->getSegment(segmentName);
630 
631  if (!segInstances)
632  {
633  ARMARX_ERROR << "Segment not found in snapshot: " << segmentName;
634  return;
635  }
636 
637  ARMARX_INFO << "Processing snapshot: " << *it;
638 
639  EntityIdList ids = segInstances->getAllEntityIds();
640 
641  if (t == 0 && i == 0)
642  {
643  totalObjects += ids.size();
644  }
645 
646  std::random_shuffle(ids.begin(), ids.end());
647 
648  for (EntityIdList::const_iterator it = ids.begin(); it != ids.end(); ++it)
649  {
650  EntityBasePtr snapEntity = segInstances->getEntityById(*it);
651  ObjectInstancePtr snapInstance = ObjectInstancePtr::dynamicCast(snapEntity);
652  GaussianMixtureDistributionPtr gmm = GaussianMixtureDistributionPtr::dynamicCast(cpLearnerPrx->getPositionReducedByMaxDeviation(objectToLoad, 150, eDevAABB));
653 
654  if (gmm)
655  {
656  totalMah += mahDistance.getWeigtedDistance(gmm, snapInstance->getPositionUncertainty());
657 
658  // discrete criterion: cluster is "correct", if observation mean within 3 sigma from cluster mean
659  GaussianMixtureComponent c = gmm->getModalComponent();
660  NormalDistributionPtr g = NormalDistributionPtr::dynamicCast(c.gaussian);
661  const Eigen::VectorXf obsMean = snapInstance->getPosition()->toEigen();
662  Eigen::Vector3f sigma;
663  sigma << sqrtf(g->getCovariance(0, 0)), sqrtf(g->getCovariance(1, 1)), sqrtf(g->getCovariance(2, 2));
664  const Eigen::VectorXf triSigmaPoint = g->toEigenMean() + 3 * sigma;
665 
666  if (g->getDensity(obsMean) > g->getDensity(triSigmaPoint))
667  {
668  corrClusters++;
669  }
670  }
671 
672  cpLearnerPrx->learnFromObject(snapInstance);
673  // ARMARX_INFO << "Object processed by CommonPlacesLearner: " << *it;
674  }
675  } // end snapshots
676 
677  // ARMARX_INFO << "Total density: " << totalDensity / itCount;
678 
679  } // end iterations
680 
681  totalMah /= itCount;
682  const float avgCorrect = float(corrClusters) / itCount;
683 
684  grandTotalMah[t] += totalMah;
685  grandTotalCorrect[t] += avgCorrect;
686 
687  } // end coefficients
688 
689  // write total file
690  std::ofstream totfile;
691  totfile.open("a_totals.csv");
692 
693  for (int t = 0; t < COEFF_COUNT; ++t)
694  totfile << thresholds[t] << "," << grandTotalMah[t] << // total mah dist
695  "," << grandTotalMah[t] / totalObjects << // avg mah dist/obj
696  "," << grandTotalCorrect[t] << // total correct clusters
697  "," << float(grandTotalCorrect[t]) / totalObjects << // correct cluster rate
698  std::endl;
699 
700  totfile.close();
701 
702  }
703 
704  void CommonPlacesTester::testGetClusterLocations()
705  {
706  Cluster3DList clusterList = cpLearnerPrx->getPositionClustersByComponentCount(objectToLoad, 3);
707 
708  if (clusterList.size() > 0)
709  {
710  for (Cluster3DList::iterator it = clusterList.begin(); it != clusterList.end(); it++)
711  {
712  Cluster3D& entry = *it;
713  ARMARX_IMPORTANT << "object center: x:" << entry.center.x << " y: " << entry.center.y << " with weight: " << entry.weight;
714 
715  }
716  }
717  else
718  {
719  ARMARX_WARNING << "no cluster found for object " << objectToLoad;
720  }
721  }
722 
723  void CommonPlacesTester::testClusteringBatch()
724  {
725  const std::string snapshotName = getProperty<std::string>("SnapshotsToLearnFrom").getValue();
726 
727  const int DEV_COUNT = 11;
728  const float thresholds[DEV_COUNT] = {10., 30., 50., 75., 100., 150., 200., 400., 800., 1000., 2000.};
729 
730  // const int DEV_COUNT = 1;
731  // const float thresholds[DEV_COUNT] = {150.};
732 
733  std::ofstream snfile;
734  const std::string fname = "c_" + ltmSegmentName + "_" + objectToLoad + ".csv";
735  snfile.open(fname.c_str());
736 
737  for (int t = 0; t < DEV_COUNT; ++t)
738  {
739  ARMARX_INFO << "Processing dev: " << thresholds[t];
740 
741  GaussianMixtureDistributionBasePtr gmmAABB = cpLearnerPrx->getPositionReducedByMaxDeviation(objectToLoad, thresholds[t], eDevAABB);
742  GaussianMixtureDistributionBasePtr gmmOBB = cpLearnerPrx->getPositionReducedByMaxDeviation(objectToLoad, thresholds[t], eDevOrientedBBox);
743  GaussianMixtureDistributionBasePtr gmmSphere = cpLearnerPrx->getPositionReducedByMaxDeviation(objectToLoad, thresholds[t], eDevEqualSphere);
744 
745  // write snap file
746  snfile << thresholds[t] << "," << gmmAABB->size() << "," <<
747  gmmOBB->size() << "," <<
748  gmmSphere->size() << "," << std::endl;
749 
750  // ARMARX_INFO << "Total density: " << totalDensity / itCount;
751  }
752 
753  snfile.close();
754  ARMARX_INFO << "Finished: ";
755  }
756 
757  void CommonPlacesTester::getRandomIds(std::vector<std::string>& allIds, std::vector<std::string>& ids, int sampleSize)
758  {
759  if (sampleSize > (int)(allIds.size()))
760  {
761  ids.assign(allIds.begin(), allIds.end());
762  return;
763  }
764 
765  std::set<std::string> idset;
766 
767  while (idset.size() < (size_t) sampleSize)
768  {
769  const size_t ind = (size_t) truncf(((float) rand()) / float(RAND_MAX) * float(allIds.size()));
770  std::cout << "index to add" << ind << std::flush;
771  idset.insert(allIds[ind]);
772  }
773 
774  ids.assign(idset.begin(), idset.end());
775  }
776 
777 
778  DeviationType CommonPlacesTester::getDeviationTypeFromString(const std::string& devTypeStr)
779  {
780 
781  if (devTypeStr == "AABB")
782  {
783  return eDevAABB;
784  }
785  else if (devTypeStr == "OrientedBBox")
786  {
787  return eDevOrientedBBox;
788  }
789  else if (devTypeStr == "EqualSphere")
790  {
791  return eDevEqualSphere;
792  }
793  else
794  {
795  return eDevAABB;
796  }
797  }
798 }
armarx::ManagedIceObject::getIceManager
IceManagerPtr getIceManager() const
Returns the IceManager.
Definition: ManagedIceObject.cpp:353
cyberglove_with_calib_22dof.ic
ic
Definition: cyberglove_with_calib_22dof.py:22
ARMARX_IMPORTANT
#define ARMARX_IMPORTANT
Definition: Logging.h:183
ISDDistance.h
Pose.h
armarx::GlobalFrame
const std::string GlobalFrame
Definition: FramedPose.h:62
armarx::Split
std::vector< std::string > Split(const std::string &source, const std::string &splitBy, bool trimElements=false, bool removeEmptyElements=false)
Definition: StringHelperTemplates.h:35
memoryx::NormalDistributionPtr
IceInternal::Handle< NormalDistribution > NormalDistributionPtr
Definition: ProbabilityMeasures.h:125
memoryx
VirtualRobot headers.
Definition: CommonPlacesTester.cpp:48
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:43
ObjectClass.h
IceInternal::Handle< ::Ice::Communicator >
ObserverObjectFactories.h
MahalanobisDistance.h
armarx::flush
const LogSender::manipulator flush
Definition: LogSender.h:251
memoryx::ObjectInstancePtr
IceInternal::Handle< ObjectInstance > ObjectInstancePtr
Definition: ObjectInstance.h:42
MemoryXCoreObjectFactories.h
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:189
memoryx::CommonPlacesTester::onConnectComponent
void onConnectComponent() override
Pure virtual hook for the subclass.
Definition: CommonPlacesTester.cpp:60
memoryx::GaussianMixtureDistributionPtr
IceInternal::Handle< GaussianMixtureDistribution > GaussianMixtureDistributionPtr
Definition: ProbabilityMeasures.h:293
armarx::detail::compare
int compare(const T &lhs, const T &rhs)
Definition: TreeWidgetBuilder.h:278
ObjectInstance.h
armarx::VariantType::FramedOrientation
const VariantTypeId FramedOrientation
Definition: FramedPose.h:40
float
#define float
Definition: 16_Level.h:22
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
armarx::Quaternion< float, 0 >
memoryx::CommonPlacesTester::onInitComponent
void onInitComponent() override
Pure virtual hook for the subclass.
Definition: CommonPlacesTester.cpp:50
MemoryXTypesObjectFactories.h
CommonPlacesTester.h
armarx::VariantType::FramedPosition
const VariantTypeId FramedPosition
Definition: FramedPose.h:39
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:186
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