BigBowlLocalization.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 2015
21  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
22  * GNU General Public License
23  */
24 
25 #include "BigBowlLocalization.h"
26 
27 #include <VirtualRobot/MathTools.h>
28 
29 // Core
30 #include <ArmarXCore/core/system/ArmarXDataPath.h>
31 #include <ArmarXCore/observers/variant/TimestampVariant.h>
32 
33 #include <RobotAPI/libraries/core/Pose.h>
34 #include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
35 
36 // MemoryX
37 #include <MemoryX/core/entity/Entity.h>
38 #include <MemoryX/core/entity/ProbabilityMeasures.h>
39 #include <MemoryX/libraries/helpers/ObjectRecognitionHelpers/ObjectRecognitionWrapper.h>
40 
41 // IVT
42 #include "Image/ByteImage.h"
43 #include "Image/ImageProcessor.h"
44 #include <Calibration/Calibration.h>
45 #include <Color/ColorParameterSet.h>
46 
47 // IVTRecognition
48 #include "Locators/VisualTargetLocator.h"
49 
50 namespace visionx
51 {
52 
53 
54  BigBowlLocalization::BigBowlLocalization()
55  {
56  numberOfResultImages = 3;
57  imageCounter = 1;
58  }
59 
60  void
61  BigBowlLocalization::onInitObjectLocalizerProcessor()
62  {
63  usingProxy("RobotStateComponent");
64  useVision = getProperty<bool>("UseVision").getValue();
65  objectColor = getProperty<ObjectColor>("BlobColor").getValue();
66 
67  // writing to the DebugDrawer topic. In ArmarX this topic's default name is DebugDrawerUpdates
68  offeringTopic("DebugDrawerUpdates");
69 
70  stereoMatcher = new CStereoMatcher();
71  }
72 
73  void
74  BigBowlLocalization::onConnectObjectLocalizerProcessor()
75  {
76  robotStateComponent =
77  getProxy<armarx::RobotStateComponentInterfacePrx>("RobotStateComponent");
78  localRobot =
79  armarx::RemoteRobot::createLocalClone(robotStateComponent->getSynchronizedRobot());
80 
81  // retrieve the proxy to the debug drawer
82  debugDrawerPrx = getTopic<armarx::DebugDrawerInterfacePrx>("DebugDrawerUpdates");
83 
84  stereoMatcher->InitCameraParameters(getStereoCalibration(), false);
85  }
86 
87  void
88  BigBowlLocalization::onExitObjectLocalizerProcessor()
89  {
90  delete stereoMatcher;
91  }
92 
93  bool
94  BigBowlLocalization::initRecognizer()
95  {
96  if (useVision)
97  {
98  ARMARX_IMPORTANT << " xxxxxxxxxxx BigBowlLocalization initRecognizer!!!!!!!!!!";
99 
100  std::string colorParameterFilename =
101  getProperty<std::string>("ColorParameterFile").getValue();
102 
103  if (!armarx::ArmarXDataPath::getAbsolutePath(colorParameterFilename,
104  colorParameterFilename))
105  {
106  ARMARX_ERROR << "Could not find color parameter file in ArmarXDataPath: "
107  << colorParameterFilename;
108  return false;
109  }
110 
111  ARMARX_VERBOSE << "Color parameter file name: " << colorParameterFilename;
112 
113  visualTargetLocator = new CVisualTargetLocator();
114 
115  if (!visualTargetLocator->Init(colorParameterFilename.c_str(), getStereoCalibration()))
116  {
117  return false;
118  }
119  int nMinPixelsPerRegion = getProperty<int>("nMinPixelsPerRegion").getValue();
120  double dMaxEpipolarDiff = getProperty<double>("dMaxEpipolarDiff").getValue();
121  double dMinFilledRatio = getProperty<double>("dMinFilledRatio").getValue();
122  double dMaxSideRatio = getProperty<double>("dMaxSideRatio").getValue();
123  double dMinSize = getProperty<double>("dMinSize").getValue();
124  double dMaxSize = getProperty<double>("dMaxSize").getValue();
125 
126  visualTargetLocator->SetParameters(nMinPixelsPerRegion,
127  dMaxEpipolarDiff,
128  dMinFilledRatio,
129  dMaxSideRatio,
130  dMinSize,
131  dMaxSize);
132 
133  colorParameterSet = new CColorParameterSet();
134  colorParameterSet->LoadFromFile(colorParameterFilename.c_str());
135  }
136  else
137  {
138  ARMARX_IMPORTANT << "Not using vision, will just return a fake result";
139  }
140 
141  showSegmentedResultImages = getProperty<bool>("ShowSegmentedResultImages").getValue();
142 
143  if (!showSegmentedResultImages)
144  {
145  ::ImageProcessor::Zero(resultImages[2]);
146  }
147 
148  return true;
149  }
150 
151  memoryx::ObjectLocalizationResultList
152  BigBowlLocalization::localizeObjectClasses(const std::vector<std::string>& objectClassNames,
153  CByteImage** cameraImages,
154  armarx::MetaInfoSizeBasePtr imageMetaInfo,
155  CByteImage** resultImages)
156  {
157  ARMARX_CHECK_EXPRESSION(imageMetaInfo->timeProvided > 0);
158  ARMARX_DEBUG << deactivateSpam(3) << "Image is already "
159  << (IceUtil::Time::now() -
160  IceUtil::Time::microSeconds(imageMetaInfo->timeProvided))
161  .toMilliSecondsDouble()
162  << "ms old";
163  if (!armarx::RemoteRobot::synchronizeLocalCloneToTimestamp(
164  localRobot, robotStateComponent, imageMetaInfo->timeProvided))
165  {
166  ARMARX_WARNING << deactivateSpam(1)
167  << "Failed to synchronize local robot - results are probably wrong";
168  }
169 
170  // check if reference frame is valid
171  auto rnCamera = localRobot->getRobotNode(referenceFrameName);
172  ARMARX_CHECK_EXPRESSION(rnCamera) << "Reference frame not present in remote robot!";
173 
174  const std::string agentName = localRobot->getName();
175 
176  Eigen::Vector3f expectedPositionVec;
177  expectedPositionVec << 0, 600, 1000;
178  const std::string rootFrameName = localRobot->getRootNode()->getName();
179  armarx::FramedPositionPtr expectedPosition =
180  new armarx::FramedPosition(expectedPositionVec, rootFrameName, agentName);
181  expectedPosition->changeFrame(localRobot, referenceFrameName);
182 
183  memoryx::ObjectLocalizationResultList resultList;
184 
185  if (useVision)
186  {
187  // localize marker(s)
188  Vec3d bigBowlPosition;
189  int numFoundBlobs = 0;
190 
191  numFoundBlobs = locateBowl(cameraImages, bigBowlPosition, resultImages);
192  ARMARX_INFO << "Found " << numFoundBlobs << " blobs.";
193 
194  if (numFoundBlobs == 0)
195  {
196  ARMARX_INFO << deactivateSpam(1) << "Object not found, color: " << objectColor;
197  }
198  else
199  {
200  // check if position is realistic
201  Eigen::Vector3f position;
202  position << bigBowlPosition.x, bigBowlPosition.y, bigBowlPosition.z;
203  float diffToExpectedPosition = (position - expectedPosition->toEigen()).norm();
204 
205  if (diffToExpectedPosition < 1200)
206  {
207  // assemble result
208  memoryx::ObjectLocalizationResult result;
209  result.recognitionCertainty = 0.96f;
210  result.objectClassName = objectClassNames.at(0);
211 
212  armarx::FramedPositionPtr positionPtr =
213  new armarx::FramedPosition(position, referenceFrameName, agentName);
214  positionPtr->changeFrame(localRobot, localRobot->getRootNode()->getName());
215  positionPtr->z = 1030;
216  result.position = positionPtr;
217  Eigen::Matrix4f orientation = Eigen::Matrix4f::Identity();
218  VirtualRobot::MathTools::rpy2eigen4f(-M_PI / 2, 0, 0, orientation);
219  armarx::FramedOrientationPtr ori =
220  new armarx::FramedOrientation(orientation, armarx::GlobalFrame, "");
221  ori->changeFrame(localRobot, referenceFrameName);
222  result.orientation = ori;
223  result.positionNoise = calculateLocalizationUncertainty(position);
224  result.timeStamp = new armarx::TimestampVariant(imageMetaInfo->timeProvided);
225 
226  resultList.push_back(result);
227 
228  ARMARX_VERBOSE << deactivateSpam(1) << "Object found at position " << position;
229  ARMARX_INFO << "position >> x: " << positionPtr->x << " y: " << positionPtr->y
230  << " z: " << positionPtr->z << " frame: " << positionPtr->frame;
231  armarx::FramedPositionPtr globalPosPtr = positionPtr->toGlobal(localRobot);
232  ARMARX_INFO << "globalposition >> x: " << globalPosPtr->x
233  << " y: " << globalPosPtr->y << " z: " << globalPosPtr->z
234  << " frame: " << globalPosPtr->frame;
235 
236 
237  // setup a 4x4 homogeneous matrix
238  Eigen::Matrix4f currPose;
239  currPose.setIdentity();
240  // set rotating coordinates (mm)
241  currPose(0, 3) = globalPosPtr->x;
242  currPose(1, 3) = globalPosPtr->y;
243  currPose(2, 3) = globalPosPtr->z;
244  armarx::PosePtr poseGlobal(new armarx::Pose(currPose));
245 
246  // size of the box in mm
247  Eigen::Vector3f v(100, 100, 100);
248  armarx::Vector3Ptr s(new armarx::Vector3(v));
249  // draw/update red box
250  debugDrawerPrx->clearLayer("objLocalizationLayer");
251  debugDrawerPrx->setBoxDebugLayerVisu(
252  "objLocalizationLayer", poseGlobal, s, armarx::DrawColor{1, 0, 0, 1});
253 
254  // mark TCP pose with a cross
255  if (getResultImagesEnabled())
256  {
257  drawCrossInImage(resultImages[0], position, true);
258  }
259  }
260  else
261  {
262  ARMARX_INFO << deactivateSpam(1)
263  << "Object is too far away from expected position: "
264  << (int)diffToExpectedPosition;
265  }
266  }
267  }
268  else
269  {
270  ARMARX_INFO << "Vision is switched off, returning fake result";
271 
272  memoryx::ObjectLocalizationResult result;
273  result.recognitionCertainty = 0.8f;
274  result.objectClassName = objectClassNames.at(0);
275 
276  result.position = expectedPosition;
277 
278  Eigen::Matrix3f orientation = Eigen::Matrix3f::Identity();
279  result.orientation =
280  new armarx::FramedOrientation(orientation, rootFrameName, agentName);
281  result.positionNoise = calculateLocalizationUncertainty(expectedPosition->toEigen());
282 
283  resultList.push_back(result);
284  }
285 
286  ARMARX_VERBOSE << "Finished localization, returning.";
287 
288  return resultList;
289  }
290 
291  void
292  BigBowlLocalization::drawCrossInImage(CByteImage* image, Eigen::Vector3f point, bool leftCamera)
293  {
294  Vec3d positionIVT = {point(0), point(1), point(2)};
295  Vec2d projectedPoint;
296 
297  if (leftCamera)
298  {
299  getStereoCalibration()->GetLeftCalibration()->WorldToImageCoordinates(
300  positionIVT, projectedPoint, getImagesAreUndistorted());
301  }
302  else
303  {
304  getStereoCalibration()->GetRightCalibration()->WorldToImageCoordinates(
305  positionIVT, projectedPoint, getImagesAreUndistorted());
306  }
307 
308  const int size = 15;
309  ARMARX_INFO << "projectedPoint.x :" << projectedPoint.x
310  << " projectedPoint.y: " << projectedPoint.y;
311 
312  if (size < projectedPoint.x && size < projectedPoint.y &&
313  projectedPoint.x < image->width - size - 1 &&
314  projectedPoint.y < image->height - size - 3)
315  {
316  for (int i = -size; i <= size; i++)
317  {
318  int indexHorizontalLine =
319  3 * ((int)projectedPoint.y * image->width + (int)projectedPoint.x + i);
320  int indexVerticalLine =
321  3 * (((int)projectedPoint.y + i) * image->width + (int)projectedPoint.x);
322  image->pixels[indexHorizontalLine] = 255;
323  image->pixels[indexHorizontalLine + 1] = 255;
324  image->pixels[indexHorizontalLine + 2] = 255;
325  image->pixels[indexVerticalLine] = 255;
326  image->pixels[indexVerticalLine + 1] = 255;
327  image->pixels[indexVerticalLine + 2] = 255;
328  }
329  }
330  }
331 
332  int
333  BigBowlLocalization::locateBowl(CByteImage** cameraImages,
334  Vec3d& bowlPosition,
335  CByteImage** resultImages)
336  {
337  int numberOfSegments = 0;
338  CByteImage* imageHSVLeft =
339  new CByteImage(cameraImages[0]->width, cameraImages[0]->height, CByteImage::eRGB24);
340  CByteImage* imageHSVRight =
341  new CByteImage(cameraImages[1]->width, cameraImages[1]->height, CByteImage::eRGB24);
342  CByteImage* imageFiltered =
343  new CByteImage(cameraImages[0]->width, cameraImages[0]->height, CByteImage::eRGB24);
344  CByteImage* imgLeftGray =
345  new CByteImage(cameraImages[0]->width, cameraImages[1]->height, CByteImage::eGrayScale);
346  CByteImage* imgRightGray =
347  new CByteImage(cameraImages[1]->width, cameraImages[1]->height, CByteImage::eGrayScale);
348 
349  ::ImageProcessor::CalculateHSVImage(cameraImages[0], imageHSVLeft);
350  ::ImageProcessor::CalculateHSVImage(cameraImages[1], imageHSVRight);
351  ::ImageProcessor::ConvertImage(cameraImages[0], imgLeftGray);
352  ::ImageProcessor::ConvertImage(cameraImages[1], imgRightGray);
353  ::ImageProcessor::Zero(imageFiltered);
354 
355  int ThesholdHMinValue = 4;
356  int ThesholdHMaxValue = 20;
357  int ThesholdSMinValue = 220;
358  int ThesholdSMaxValue = 255;
359 
360  int imagePosXLeft = 0;
361  int imagePosYLeft = 0;
362  int totalPixelLeft = 0;
363  int imagePosXRight = 0;
364  int imagePosYRight = 0;
365  int totalPixelRight = 0;
366  int deltaROI = 50;
367 
368  // scan the Left HSV image and compute the histogram
369  for (int i = deltaROI; i < imageHSVLeft->width - deltaROI; i++)
370  {
371  for (int j = deltaROI; j < imageHSVLeft->height - deltaROI; j++)
372  {
373  int pixelIndex = (i + j * imageHSVLeft->width) * imageHSVLeft->bytesPerPixel;
374  double hVal = (double)imageHSVLeft->pixels[pixelIndex];
375  double sVal = (double)imageHSVLeft->pixels[pixelIndex + 1];
376 
377  if ((hVal > ThesholdHMinValue && hVal < ThesholdHMaxValue) &&
378  (sVal > ThesholdSMinValue && sVal < ThesholdSMaxValue))
379  {
380  imageFiltered->pixels[pixelIndex] = cameraImages[0]->pixels[pixelIndex];
381  imageFiltered->pixels[pixelIndex + 1] = cameraImages[0]->pixels[pixelIndex + 1];
382  imageFiltered->pixels[pixelIndex + 2] = cameraImages[0]->pixels[pixelIndex + 2];
383  imagePosXLeft += i;
384  imagePosYLeft += j;
385  totalPixelLeft++;
386  }
387  }
388  }
389  ARMARX_INFO << "totalPixel >> left " << totalPixelLeft;
390 
391 
392  // scan the Right HSV image and compute the histogram
393  for (int i = deltaROI; i < imageHSVRight->width - deltaROI; i++)
394  {
395  for (int j = deltaROI; j < imageHSVRight->height - deltaROI; j++)
396  {
397  int pixelIndex = (i + j * imageHSVRight->width) * imageHSVRight->bytesPerPixel;
398  double hVal = (double)imageHSVRight->pixels[pixelIndex];
399  double sVal = (double)imageHSVRight->pixels[pixelIndex + 1];
400 
401  if ((hVal > ThesholdHMinValue && hVal < ThesholdHMaxValue) &&
402  (sVal > ThesholdSMinValue && sVal < ThesholdSMaxValue))
403  {
404  imagePosXRight += i;
405  imagePosYRight += j;
406  totalPixelRight++;
407  }
408  }
409  }
410  ARMARX_INFO << "totalPixel >> right " << totalPixelRight;
411 
412  if (totalPixelLeft > 10000 && totalPixelRight > 10000)
413  {
414  numberOfSegments = 1;
415  }
416  else
417  {
418  resultImages[0] = cameraImages[0];
419  resultImages[1] = cameraImages[1];
420  ::ImageProcessor::Zero(imageFiltered);
421  resultImages[2] = imageFiltered;
422  numberOfSegments = 0;
423  return numberOfSegments;
424  }
425 
426  Vec2d imageCoorLeft, imageCoorRight;
427  imageCoorLeft.x = (int)imagePosXLeft / totalPixelLeft;
428  imageCoorLeft.y = (int)imagePosYLeft / totalPixelLeft;
429  imageCoorRight.x = (int)imagePosXRight / totalPixelRight;
430  imageCoorRight.y = (int)imagePosYRight / totalPixelRight;
431 
432  ARMARX_INFO << "imageCoorLeft x: " << imageCoorLeft.x << " y: " << imageCoorLeft.y;
433  ARMARX_INFO << "imageCoorRight x: " << imageCoorRight.x << " y: " << imageCoorRight.y;
434  // bowl must be somewhere in the middle in the left camera
435  if (imageCoorLeft.x < 250 || imageCoorLeft.x > 470 || imageCoorLeft.y < 150 ||
436  imageCoorLeft.y > 320)
437  {
438  resultImages[0] = cameraImages[0];
439  resultImages[1] = cameraImages[1];
440  ::ImageProcessor::Zero(imageFiltered);
441  resultImages[2] = imageFiltered;
442  numberOfSegments = 0;
443  return numberOfSegments;
444  }
445 
446  Vec2d vCorrespondingPointRight;
447  const int nDispMin = stereoMatcher->GetDisparityEstimate(10000);
448  const int nDispMax = stereoMatcher->GetDisparityEstimate(500);
449 
450  // img l, img r, px, py, size of correlation window, min disparity, max disparity,
451  // corresponding point 2d, 3d point, correlation threshold, images are undistorted
452  int nMatchingResult = stereoMatcher->Match(imgLeftGray,
453  imgRightGray,
454  (int)imageCoorLeft.x,
455  (int)imageCoorLeft.y,
456  50,
457  nDispMin,
458  nDispMax,
459  vCorrespondingPointRight,
460  bowlPosition,
461  0.7f,
462  true);
463 
464 
465  ARMARX_INFO << "nMatchingResult " << nMatchingResult;
466  //bowlPosition.x -= 0.2;
467  //ARMARX_INFO << "worldCoor x: " << bowlPosition.x << " y: " << bowlPosition.y << " z: " << bowlPosition.z;
468 
469  if (nMatchingResult <= 0)
470  {
471  resultImages[0] = cameraImages[0];
472  resultImages[1] = cameraImages[1];
473  ::ImageProcessor::Zero(imageFiltered);
474  resultImages[2] = imageFiltered;
475  numberOfSegments = 0;
476  return numberOfSegments;
477  }
478 
479  // draw a white dot in the object center
480  int deltaCenter = 10;
481  for (int i = imageCoorLeft.x - deltaCenter; i < imageCoorLeft.x + deltaCenter; i++)
482  {
483  for (int j = imageCoorLeft.y - deltaCenter; j < imageCoorLeft.y + deltaCenter; j++)
484  {
485  int pixelIndex = (i + j * imageHSVLeft->width) * imageHSVLeft->bytesPerPixel;
486  imageFiltered->pixels[pixelIndex] = 255;
487  imageFiltered->pixels[pixelIndex + 1] = 255;
488  imageFiltered->pixels[pixelIndex + 2] = 255;
489  }
490  }
491 
492  /*
493  std::stringstream str;
494  str << "/common/homes/staff/aksoy/Desktop/testBowlImages/org_" << imageCounter++ << ".bmp";
495  cameraImages[0]->SaveToFile(str.str().c_str());
496  std::stringstream strFlt;
497  strFlt << "/common/homes/staff/aksoy/Desktop/testBowlImages/flt_" << imageCounter++ << ".bmp";
498  imageFiltered->SaveToFile(strFlt.str().c_str());
499 
500  ARMARX_INFO << " xxxxxxxxxxx Images are saved!!!!!!!!!!" << str.str().c_str();
501  */
502 
503  resultImages[0] = cameraImages[0];
504  resultImages[1] = cameraImages[1];
505  resultImages[2] = imageFiltered;
506  return numberOfSegments;
507  }
508 } // namespace visionx
RemoteRobot.h
ARMARX_VERBOSE
#define ARMARX_VERBOSE
Definition: Logging.h:187
visionx::ObjectLocalizerProcessor::resultImages
CByteImage ** resultImages
Definition: ObjectLocalizerProcessor.h:431
visionx::BigBowlLocalization::onConnectObjectLocalizerProcessor
void onConnectObjectLocalizerProcessor() override
Initialize stuff here?
Definition: BigBowlLocalization.cpp:74
GfxTL::Vec3d
VectorXD< 3, double > Vec3d
Definition: VectorXD.h:737
ARMARX_IMPORTANT
#define ARMARX_IMPORTANT
Definition: Logging.h:190
visionx::ObjectLocalizerProcessor::referenceFrameName
std::string referenceFrameName
Definition: ObjectLocalizerProcessor.h:425
visionx
ArmarX headers.
Definition: OpenPoseStressTest.h:38
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:650
Pose.h
visionx::ObjectLocalizerProcessor::getResultImagesEnabled
bool getResultImagesEnabled() const
Retrieve whether result images are enabled.
Definition: ObjectLocalizerProcessor.h:335
armarx::GlobalFrame
const std::string GlobalFrame
Definition: FramedPose.h:65
armarx::TimestampVariant
Definition: TimestampVariant.h:54
ObjectRecognitionWrapper.h
visionx::BigBowlLocalization::onInitObjectLocalizerProcessor
void onInitObjectLocalizerProcessor() override
Definition: BigBowlLocalization.cpp:61
visionx::BigBowlLocalization::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: BigBowlLocalization.cpp:152
visionx::ObjectLocalizerProcessor::imageMetaInfo
armarx::MetaInfoSizeBasePtr imageMetaInfo
Definition: ObjectLocalizerProcessor.h:432
IceInternal::Handle< FramedPosition >
visionx::BigBowlLocalization::initRecognizer
bool initRecognizer() override
Initializes segmentable recognition.
Definition: BigBowlLocalization.cpp:94
visionx::ObjectLocalizerProcessor::getImagesAreUndistorted
bool getImagesAreUndistorted() const
Retrieve whether images are undistorted.
Definition: ObjectLocalizerProcessor.h:368
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:570
M_PI
#define M_PI
Definition: MathTools.h:17
if
if(!yyvaluep)
Definition: Grammar.cpp:645
visionx::ObjectLocalizerProcessor::cameraImages
CByteImage * cameraImages[2]
Definition: ObjectLocalizerProcessor.h:429
ARMARX_DEBUG
#define ARMARX_DEBUG
Definition: Logging.h:184
TimestampVariant.h
armarx::Vector3
The Vector3 class.
Definition: Pose.h:112
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:196
GfxTL::Vec2d
VectorXD< 2, double > Vec2d
Definition: VectorXD.h:736
visionx::BigBowlLocalization::onExitObjectLocalizerProcessor
void onExitObjectLocalizerProcessor() override
Definition: BigBowlLocalization.cpp:88
ProbabilityMeasures.h
armarx::Pose
The Pose class.
Definition: Pose.h:242
armarx::ctrlutil::v
double v(double t, double v0, double a0, double j)
Definition: CtrlUtil.h:39
visionx::ObjectLocalizerProcessor::getStereoCalibration
CStereoCalibration * getStereoCalibration() const
Retrieve stereo calibration corresponding to image provider.
Definition: ObjectLocalizerProcessor.h:391
ARMARX_CHECK_EXPRESSION
#define ARMARX_CHECK_EXPRESSION(expression)
This macro evaluates the expression and if it turns out to be false it will throw an ExpressionExcept...
Definition: ExpressionException.h:73
armarx::VariantType::FramedOrientation
const VariantTypeId FramedOrientation
Definition: FramedPose.h:39
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:181
armarx::RemoteRobot::createLocalClone
VirtualRobot::RobotPtr createLocalClone()
Clones the structure of this remote robot to a local instance.
Definition: RemoteRobot.cpp:381
armarx::ManagedIceObject::offeringTopic
void offeringTopic(const std::string &name)
Registers a topic for retrival after initialization.
Definition: ManagedIceObject.cpp:300
Entity.h
armarx::RemoteRobot::synchronizeLocalCloneToTimestamp
static bool synchronizeLocalCloneToTimestamp(VirtualRobot::RobotPtr robot, RobotStateComponentInterfacePrx robotStatePrx, Ice::Long timestamp)
Synchronizes a local robot to a robot state at timestamp.
Definition: RemoteRobot.cpp:566
armarx::Logging::deactivateSpam
SpamFilterDataPtr deactivateSpam(float deactivationDurationSec=10.0f, const std::string &identifier="", bool deactivate=true) const
disables the logging for the current line for the given amount of seconds.
Definition: Logging.cpp:99
armarx::ArmarXDataPath::getAbsolutePath
static bool getAbsolutePath(const std::string &relativeFilename, std::string &storeAbsoluteFilename, const std::vector< std::string > &additionalSearchPaths={}, bool verbose=true)
Definition: ArmarXDataPath.cpp:109
GfxTL::Matrix3f
MatrixXX< 3, 3, float > Matrix3f
Definition: MatrixXX.h:649
visionx::ObjectLocalizerProcessor::calculateLocalizationUncertainty
memoryx::MultivariateNormalDistributionPtr calculateLocalizationUncertainty(Vec2d left_point, Vec2d right_point)
Calculate 3D uncertainty from two 2d points in left and right camera.
Definition: ObjectLocalizerProcessor.cpp:221
armarx::VariantType::FramedPosition
const VariantTypeId FramedPosition
Definition: FramedPose.h:38
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:193
BigBowlLocalization.h
visionx::BigBowlLocalization::BigBowlLocalization
BigBowlLocalization()
Definition: BigBowlLocalization.cpp:54
ArmarXDataPath.h
armarx::ManagedIceObject::usingProxy
bool usingProxy(const std::string &name, const std::string &endpoints="")
Registers a proxy for retrieval after initialization and adds it to the dependency list.
Definition: ManagedIceObject.cpp:154
armarx::ctrlutil::s
double s(double t, double s0, double v0, double a0, double j)
Definition: CtrlUtil.h:33
visionx::BigBowlLocalization::debugDrawerPrx
armarx::DebugDrawerInterfacePrx debugDrawerPrx
Definition: BigBowlLocalization.h:199
visionx::ObjectLocalizerProcessor::numberOfResultImages
int numberOfResultImages
Definition: ObjectLocalizerProcessor.h:433
norm
double norm(const Point &a)
Definition: point.hpp:102