SegmentableTemplateRecognition.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 VisionX::Component
17 * @author Kai Welke <welke at kit dot edu>
18 * @copyright 2013 Humanoids Group, HIS, KIT
19 * @license http://www.gnu.org/licenses/gpl-2.0.txt
20 * GNU General Public License
21 */
22 
23 #include "SegmentableTemplateRecognition.h"
24 
25 #include <fstream>
26 
27 // Core
28 #include <ArmarXCore/core/system/ArmarXDataPath.h>
29 
30 // RobotState
31 #include <RobotAPI/libraries/core/FramedPose.h>
32 #include <RobotAPI/libraries/core/Pose.h>
33 
34 // IVT
35 #include <Helpers/helpers.h>
36 #include <Image/ByteImage.h>
37 #include <Image/ImageProcessor.h>
38 #include <Image/PrimitivesDrawer.h>
39 #include <Math/FloatMatrix.h>
40 
41 // MemoryX
42 #include <ArmarXCore/observers/variant/TimestampVariant.h>
43 
44 #include <MemoryX/core/MemoryXCoreObjectFactories.h>
45 #include <MemoryX/libraries/helpers/ObjectRecognitionHelpers/ObjectRecognitionWrapper.h>
46 
47 // This file must be included after all Ice-related headers when Ice 3.5.x is used (e.g. with Ubuntu 14.04 or Debian 7.0)!
48 // GLContext.h indirectly includes X.h which does "#define None", colliding with IceUtil::None.
49 #include <boost/scope_exit.hpp>
50 
51 #include <SimoxUtility/algorithm/string/string_tools.h>
52 
53 #include <gui/GLContext.h>
54 
55 #pragma GCC diagnostic ignored "-Wmissing-field-initializers"
56 
57 using namespace armarx;
58 using namespace visionx;
59 using namespace memoryx::EntityWrappers;
60 
61 namespace
62 {
63 
64  struct HSVSegmentationParams
65  {
66  int hueValue;
67  int hueTolerance;
68  int saturationMin;
69  int saturationMax;
70  int valueMin;
71  int valueMax;
72 
73  float minRatio = 0.0f;
74  };
75 
76  HSVSegmentationParams
77  getSegmentationParamsFromColor(CColorParameterSet const& colorParameters, ObjectColor color)
78  {
79  HSVSegmentationParams result = {};
80  int const* colorArray = colorParameters.GetColorParameters(color);
81  if (colorArray)
82  {
83  result.hueValue = colorArray[0];
84  result.hueTolerance = colorArray[1];
85  result.saturationMin = colorArray[2];
86  result.saturationMax = colorArray[3];
87  result.valueMin = colorArray[4];
88  result.valueMax = colorArray[5];
89  }
90  else
91  {
92  ARMARX_WARNING << "Unknown color: " << color;
93  }
94  return result;
95  }
96 
97  struct BlobDetector
98  {
99  BlobDetector(int width, int height) :
100  segmented(width, height, CByteImage::eGrayScale),
101  temp(width, height, CByteImage::eGrayScale),
102  hsv(width, height, CByteImage::eRGB24)
103  {
104  }
105 
106  BlobDetector(BlobDetector const&) = delete;
107  BlobDetector& operator=(BlobDetector const&) = delete;
108 
109  Object2DList
110  findColoredRegions(CByteImage* inputImage,
111  int minPixelsPerRegion,
112  std::vector<HSVSegmentationParams>& segmentationParams,
113  int dilationCount = 2)
114  {
115  ::ImageProcessor::CalculateHSVImage(inputImage, &hsv);
116 
117  ::ImageProcessor::Zero(&segmented);
118  for (HSVSegmentationParams const& segParam : segmentationParams)
119  {
120  ::ImageProcessor::FilterHSV(&hsv,
121  &temp,
122  segParam.hueValue,
123  segParam.hueTolerance,
124  segParam.saturationMin,
125  segParam.saturationMax,
126  segParam.valueMin,
127  segParam.valueMax);
128  ::ImageProcessor::Or(&segmented, &temp, &segmented);
129  }
130 
131  for (int i = 0; i < dilationCount / 2; ++i)
132  {
133  ::ImageProcessor::Erode(&segmented, &temp);
134  ::ImageProcessor::Dilate(&temp, &segmented);
135  }
136  for (int i = 0; i < dilationCount / 2; ++i)
137  {
138  ::ImageProcessor::Dilate(&segmented, &temp);
139  ::ImageProcessor::Erode(&temp, &segmented);
140  }
141  if (dilationCount == 0)
142  {
143  ::ImageProcessor::Dilate(&segmented, &temp);
144  ::ImageProcessor::Dilate(&temp, &segmented);
145  ::ImageProcessor::Erode(&segmented, &temp);
146  ::ImageProcessor::CopyImage(&temp, &segmented);
147  }
148 
149  RegionList regions;
150  ::ImageProcessor::FindRegions(&segmented, regions, minPixelsPerRegion);
151 
152  Object2DList objects;
153  objects.reserve(regions.size());
154  int objectId = 0;
155  for (MyRegion const& region : regions)
156  {
157  Object2DEntry object;
158  //object.color = color; // FIXME: We have changed to HSV seg parameters
159  object.region = region;
160 
161  for (HSVSegmentationParams& seg : segmentationParams)
162  {
163  if (seg.minRatio > 0.0f)
164  {
165  int count = 0;
166 
167  int min_hue = (int)seg.hueValue - (int)seg.hueTolerance;
168  int max_hue = (int)seg.hueValue + (int)seg.hueTolerance;
169 
170  if (min_hue < 0)
171  {
172  min_hue += 180;
173  }
174 
175  if (max_hue >= 180)
176  {
177  max_hue -= 180;
178  }
179 
180  for (int y = region.min_y; y < region.max_y; ++y)
181  {
182  for (int x = region.min_x; x < region.max_x; ++x)
183  {
184  int pixelIndex = y * hsv.width + x;
185  int h = hsv.pixels[pixelIndex * 3 + 0];
186  int s = hsv.pixels[pixelIndex * 3 + 1];
187  int v = hsv.pixels[pixelIndex * 3 + 2];
188  if (seg.saturationMin <= s && s <= seg.saturationMax &&
189  seg.valueMin <= v && v <= seg.valueMax)
190  {
191  if (max_hue >= min_hue)
192  {
193  if (min_hue <= h && h <= max_hue)
194  {
195  ++count;
196  }
197  }
198  else
199  {
200  if (h <= max_hue || h >= min_hue)
201  {
202  ++count;
203  }
204  }
205  }
206  }
207  }
208 
209  float ratio = float(count) / region.nPixels;
210  if (ratio < seg.minRatio)
211  {
212  ARMARX_VERBOSE << "Discarding region because ratio too small: "
213  << ratio;
214  continue;
215  }
216  }
217  }
218 
219  object.type = eCompactObject;
220  object.sName = "CompactObject";
221  object.id = objectId++;
222 
223  objects.push_back(object);
224  }
225  return objects;
226  }
227 
228  CByteImage segmented;
229  CByteImage temp;
230  CByteImage hsv;
231  };
232 
233  void
234  CropImageToTemplate(CByteImage* inputImage, MyRegion const& region, CByteImage* outputImage)
235  {
236  int width = region.max_x - region.min_x + 1;
237  int height = region.max_y - region.min_y + 1;
238  int k = outputImage->width;
239 
240 
241  if (width >= height)
242  {
243  int new_height = int((k * height) / float(width) + 0.5f);
244  CByteImage temp_image(k, new_height, CByteImage::eGrayScale);
245 
246  ::ImageProcessor::Resize(inputImage, &temp_image, &region);
247 
248  const int nPixels = k * new_height;
249  unsigned char* output = outputImage->pixels;
250 
251  memcpy(output, temp_image.pixels, nPixels);
252 
253  const int nTotalPixels = k * k;
254 
255  for (int i = nPixels; i < nTotalPixels; i++)
256  {
257  output[i] = 0;
258  }
259  }
260  else
261  {
262  int new_width = int((k * width) / float(height) + 0.5f);
263  CByteImage temp_image(new_width, k, CByteImage::eGrayScale);
264 
265  ::ImageProcessor::Resize(inputImage, &temp_image, &region);
266 
267  const unsigned char* input = temp_image.pixels;
268  unsigned char* output = outputImage->pixels;
269 
270  for (int i = 0, offset = 0, offset2 = 0; i < k; i++)
271  {
272  int j;
273 
274  for (j = 0; j < new_width; j++, offset++, offset2++)
275  {
276  output[offset] = input[offset2];
277  }
278 
279  for (j = new_width; j < k; j++, offset++)
280  {
281  output[offset] = 0;
282  }
283  }
284  }
285  }
286 
287 } // namespace
288 
289 SegmentableTemplateRecognition::SegmentableTemplateRecognition()
290 {
291 }
292 
293 SegmentableTemplateRecognition::~SegmentableTemplateRecognition()
294 {
295 }
296 
297 void
298 SegmentableTemplateRecognition::onInitObjectLocalizerProcessor()
299 {
300  offeringTopic(getProperty<std::string>("DebugObserverName").getValue());
301 
302  paramMinPixelsPerRegion = getProperty<int>("MinPixelsPerRegion").getValue();
303 }
304 
305 void
306 SegmentableTemplateRecognition::onExitObjectLocalizerProcessor()
307 {
308 }
309 
310 std::vector<std::string>
311 getFileList(const std::string& path)
312 {
313  std::vector<std::string> result;
314  if (!path.empty())
315  {
316  namespace fs = std::filesystem;
317 
318  fs::path apk_path(path);
319  fs::recursive_directory_iterator end;
320 
321  for (fs::recursive_directory_iterator i(apk_path); i != end; ++i)
322  {
323  const fs::path cp = (*i);
324  result.push_back(cp.string());
325  }
326  }
327  return result;
328 }
329 
330 bool
331 SegmentableTemplateRecognition::initRecognizer()
332 {
333  ARMARX_VERBOSE << "Initializing SegmentableTemplateRecognition";
334 
335  Eigen::Vector3f minPoint = getProperty<Eigen::Vector3f>("MinPoint").getValue();
336  Eigen::Vector3f maxPoint = getProperty<Eigen::Vector3f>("MaxPoint").getValue();
337 
338  Math3d::SetVec(validResultBoundingBoxMin, minPoint(0), minPoint(1), minPoint(2));
339  Math3d::SetVec(validResultBoundingBoxMax, maxPoint(0), maxPoint(1), maxPoint(2));
340 
341  maxEpipolarDistance = getProperty<float>("MaxEpipolarDistance").getValue();
342  std::string colorParemeterFilename = getProperty<std::string>("ColorParameterFile").getValue();
343 
344  paramTemplateMatchThreshold = getProperty<float>("TemplateMatchThreshold");
345  paramSizeRatioThreshold = getProperty<float>("SizeRatioThreshold");
346  paramCorrelationThreshold = getProperty<float>("CorrelationThreshold");
347  paramSingleInstance = getProperty<bool>("SingleInstance");
348 
349  if (!ArmarXDataPath::getAbsolutePath(colorParemeterFilename, colorParemeterFilename))
350  {
351  ARMARX_ERROR << "Could not find color parameter file in ArmarXDataPath: "
352  << colorParemeterFilename;
353  }
354 
355  templatePath = getProperty<std::string>("TemplatePath").getValue();
356  if (!ArmarXDataPath::getAbsolutePath(templatePath, templatePath))
357  {
358  ARMARX_ERROR << "Could not find template path file in ArmarXDataPath: " << templatePath;
359  }
360 
361  ARMARX_IMPORTANT << "Looking in template path: " << templatePath;
362  auto templateFiles = getFileList(templatePath);
363  for (std::string const& templateFile : templateFiles)
364  {
365  if (!simox::alg::ends_with(templateFile, ".seg"))
366  {
367  continue;
368  }
369 
370  SegmentableTemplateEntry entry;
371 
372  ARMARX_INFO << "Loading template: " << templateFile;
373 
374  FILE* file = fopen(templateFile.c_str(), "rb");
375  BOOST_SCOPE_EXIT((file))
376  {
377  fclose(file);
378  }
379  BOOST_SCOPE_EXIT_END;
380 
381  fseek(file, 0L, SEEK_END);
382  long fileSize = ftell(file);
383  fseek(file, 0L, SEEK_SET);
384 
385  void* memory = operator new(fileSize);
386  entry.data.reset(new (memory) SegmentableTemplateHeader);
387  if (fileSize < (long)sizeof(SegmentableTemplateHeader))
388  {
389  ARMARX_WARNING << "File is too short for template header: " << templateFile
390  << " (expected size: " << sizeof(SegmentableTemplateHeader)
391  << ", but got size: " << fileSize << ")";
392  continue;
393  }
394 
395  long read = fread(memory, 1, fileSize, file);
396  if (read != fileSize)
397  {
398  ARMARX_WARNING << "Could not read template file completely (expected size: " << fileSize
399  << ", but read: " << read << ")";
400  continue;
401  }
402 
403  std::string objectName = entry.data->name;
404  {
405  // Load model
406  std::string modelFilename = templatePath + "/" + objectName + ".mat";
407  entry.model.reset(new CFloatMatrix());
408  if (!entry.model->LoadFromFile(modelFilename.c_str()))
409  {
410  ARMARX_WARNING << "Could not load model file: " << modelFilename;
411  continue;
412  }
413  }
414  std::string colorString;
415  {
416  // Load color
417  std::string colorFilename = templatePath + "/" + objectName + ".color";
418  std::ifstream input(colorFilename.c_str());
419  if (!(input >> colorString))
420  {
421  ARMARX_WARNING << "Could not load color file: " << colorFilename;
422  continue;
423  }
424  ObjectColor color = CColorParameterSet::Translate(colorString.c_str());
425  if (color == eNone)
426  {
427  ARMARX_WARNING << "Unknown color string: " << colorString;
428  continue;
429  }
430  entry.color = color;
431  }
432 
433  ARMARX_INFO << "Inserting template into database: " << objectName
434  << ", template count: " << entry.data->templateCount
435  << ", color: " << colorString << " (" << entry.color << ")";
436  database.emplace(objectName, std::move(entry));
437  }
438 
439  if (!colorParameters.LoadFromFile(colorParemeterFilename.c_str()))
440  {
441  throw armarx::LocalException("Could not read color parameter file.");
442  }
443 
444  // create context
445  ImageFormatInfo imageFormat = getImageFormat();
446  contextGL.reset(new CGLContext());
447  contextGL->CreateContext(imageFormat.dimension.width, imageFormat.dimension.height);
448  contextGL->MakeCurrent();
449 
450  m_pOpenGLVisualizer.reset(new COpenGLVisualizer());
451  m_pOpenGLVisualizer->InitByCalibration(getStereoCalibration()->GetRightCalibration());
452 
453  m_pObjectFinderStereo.reset(new CObjectFinderStereo());
454  m_pObjectFinderStereo->SetColorParameterSet(&colorParameters);
455  m_pObjectFinderStereo->Init(getStereoCalibration());
456 
457  ARMARX_INFO << "SegmentableTemplateRecognition is initialized";
458 
459  return true;
460 }
461 
462 bool
463 SegmentableTemplateRecognition::addObjectClass(const memoryx::EntityPtr& objectClassEntity,
464  const memoryx::GridFileManagerPtr& fileManager)
465 {
466  std::string entityName = objectClassEntity->getName();
467  ARMARX_VERBOSE << "Adding object class " << objectClassEntity->getName() << armarx::flush;
468 
469  auto iter = database.find(entityName);
470  return iter != database.end();
471 }
472 
473 memoryx::ObjectLocalizationResultList
474 SegmentableTemplateRecognition::localizeObjectClasses(
475  const std::vector<std::string>& objectClassNames,
476  CByteImage** cameraImages,
477  armarx::MetaInfoSizeBasePtr imageMetaInfo,
478  CByteImage** resultImages)
479 {
480 
481  if (objectClassNames.size() < 1)
482  {
483  ARMARX_WARNING << "objectClassNames.size() = " << objectClassNames.size()
484  << ", something is wrong here! ";
485 
486  memoryx::ObjectLocalizationResultList resultList;
487  return resultList;
488  }
489 
490  ARMARX_VERBOSE << "Localizing " << objectClassNames;
491 
492  contextGL->MakeCurrent();
493 
494  Object3DList objectList;
495 
496  for (std::string const& className : objectClassNames)
497  {
498  Object3DList classObjectList;
499 
500  int dilationCount = 2;
501 
502  auto classEntryIter = database.find(className);
503  if (classEntryIter == database.end())
504  {
505  ARMARX_WARNING << "Could not find database entry for: " << className;
506  continue;
507  }
508  SegmentableTemplateEntry const& classEntry = classEntryIter->second;
509  ObjectColor color = classEntry.color;
510 
511  std::vector<HSVSegmentationParams> hsvSegmentationParams;
512  // FIXME: Hack to allow two colored objects
513  if (color == eYellow3)
514  {
515  hsvSegmentationParams.push_back(getSegmentationParamsFromColor(colorParameters, eRed));
516  hsvSegmentationParams.push_back(
517  getSegmentationParamsFromColor(colorParameters, eYellow));
518  hsvSegmentationParams.back().minRatio = 0.2f;
519  }
520  else
521  {
522  HSVSegmentationParams hsvSegmentationParam =
523  getSegmentationParamsFromColor(colorParameters, color);
524  hsvSegmentationParams.push_back(hsvSegmentationParam);
525  }
526 
527  {
528  CByteImage* leftInput = cameraImages[0];
529  CByteImage* rightInput = cameraImages[1];
530  int width = leftInput->width;
531  int height = leftInput->height;
532  ARMARX_CHECK_EXPRESSION(rightInput->width == width && rightInput->height == height);
533 
534  CByteImage grayImageBig(width, height, CByteImage::eGrayScale);
535  ::ImageProcessor::ConvertImage(leftInput, &grayImageBig, true);
536 
537  BlobDetector leftBlobDetector(width, height);
538  BlobDetector rightBlobDetector(width, height);
539 
540  Object2DList leftObjects = leftBlobDetector.findColoredRegions(
541  leftInput, paramMinPixelsPerRegion, hsvSegmentationParams, dilationCount);
542  Object2DList rightObjects = rightBlobDetector.findColoredRegions(
543  rightInput, paramMinPixelsPerRegion, hsvSegmentationParams, dilationCount);
544 
545  for (Object2DEntry const& entry : leftObjects)
546  {
547  ::PrimitivesDrawer::DrawRegion(resultImages[0], entry.region, 255, 0, 0, 2);
548  }
549 
550  // Calculate blobs with 3D pose from stereo calibration
551  Object3DList blobList;
552 
553  float maxEpipolarDistance = this->maxEpipolarDistance;
554  CStereoCalibration* stereoCalibration = getStereoCalibration();
555  bool inputImagesAreRectified = false; // FIXME: Should this be true?
556  bool useDistortionParameters = !getImagesAreUndistorted();
557  float minZDistance = 500;
558  float maxZDistance = 3000;
559  {
560  for (Object2DEntry& leftObject : leftObjects)
561  {
562  Object2DEntry const* bestMatch = nullptr;
563  float bestDiff = maxEpipolarDistance;
564  for (Object2DEntry& rightObject : rightObjects)
565  {
566  float pixelRatio =
567  leftObject.region.nPixels < rightObject.region.nPixels
568  ? (float)leftObject.region.nPixels / rightObject.region.nPixels
569  : (float)rightObject.region.nPixels / leftObject.region.nPixels;
570  float aspectRatioRatio =
571  leftObject.region.ratio < rightObject.region.ratio
572  ? (float)leftObject.region.ratio / rightObject.region.ratio
573  : (float)rightObject.region.ratio / leftObject.region.ratio;
574 
575  Vec2d leftCentroid = leftObject.region.centroid;
576  Vec2d rightCentroid = rightObject.region.centroid;
577 
578  float yDiff;
579  if (inputImagesAreRectified)
580  {
581  yDiff = fabsf(leftCentroid.y - rightCentroid.y);
582  }
583  else
584  {
585  yDiff = stereoCalibration->CalculateEpipolarLineInLeftImageDistance(
586  leftCentroid, rightCentroid);
587  yDiff = fabs(yDiff);
588  }
589 
590  Vec3d position;
591  stereoCalibration->Calculate3DPoint(leftCentroid,
592  rightCentroid,
593  position,
594  inputImagesAreRectified,
595  useDistortionParameters);
596 
597 
598  if (pixelRatio > 0.5f && aspectRatioRatio > 0.5f &&
599  yDiff <= maxEpipolarDistance && position.z >= minZDistance &&
600  position.z <= maxZDistance && yDiff <= bestDiff)
601  {
602  bestDiff = yDiff;
603  bestMatch = &rightObject;
604  }
605  }
606 
607  // We now have the best matching right region in bestMatch
608  if (bestMatch)
609  {
610  Object2DEntry const& rightObject = *bestMatch;
611  Object3DEntry entry;
612 
613  entry.region_left = leftObject.region;
614  entry.region_right = rightObject.region;
615  entry.region_id_left = leftObject.id;
616  entry.region_id_right = rightObject.id;
617 
618  stereoCalibration->Calculate3DPoint(entry.region_left.centroid,
619  entry.region_right.centroid,
620  entry.pose.translation,
621  inputImagesAreRectified,
622  useDistortionParameters);
623 
624  entry.world_point = entry.pose.translation;
625 
626  blobList.push_back(entry);
627  }
628  }
629  }
630 
631  // Visualize results
632  CByteImage* segmented = &leftBlobDetector.segmented;
633  CByteImage* resultImage = resultImages[0];
634  {
635  const unsigned char* input = segmented->pixels;
636  unsigned char* output = resultImage->pixels;
637  int nPixels = segmented->width * segmented->height;
638 
639  // FIXME: Make the highlight color configurable (depending on query color?)
640  int r, g, b;
641  hsv2rgb(0, 255, 100, r, g, b);
642 
643  for (int i = 0; i < nPixels; i++)
644  {
645  if (input[i])
646  {
647  const int offset = 3 * i;
648  output[offset + 0] = r;
649  output[offset + 1] = g;
650  output[offset + 2] = b;
651  }
652  }
653  }
654 
655 
656  // Cut out segmented part of the image
657  // segmentedObject contains the grayscale image of the blob (every other pixel is black)
658  CByteImage segmentedObject(width, height, CByteImage::eGrayScale);
659  ::ImageProcessor::ConvertImage(leftInput, &segmentedObject);
660  ::ImageProcessor::And(segmented, &segmentedObject, &segmentedObject);
661 
662  // Try to match templates into the blobs
663  ARMARX_VERBOSE << "Blob count: " << blobList.size();
664  for (Object3DEntry& blob : blobList)
665  {
666  const MyRegion& region = blob.region_left;
667 
668  int length = SegmentableBitmapWidth;
669  CByteImage objectGray(length, length, CByteImage::eGrayScale);
670 
671  //DEBUG_SAVE_IMAGE(*pGrayImage, "00");
672 
673  // Crop the region of the blob
674  CropImageToTemplate(&segmentedObject, region, &objectGray);
675 
676  //DEBUG_SAVE_IMAGE(objectGray, "01");
677 
678  ::ImageProcessor::ThresholdBinarize(&objectGray, &objectGray, 1);
679  //DEBUG_SAVE_IMAGE(objectGray, "03_binarize");
680 
681  // objectGray is now a black/white image (64x64) of the blob
682  CByteImage* input = &objectGray;
683  SegmentableTemplateHeader const& templateHeader = *classEntry.data;
684 
685  int bestOverlap = 0;
686  int total = 0;
687  SegmentableTemplate const* bestTemplate = nullptr;
688  for (std::uint32_t i = 0; i < templateHeader.templateCount; ++i)
689  {
690  SegmentableTemplate const& template_ = templateHeader.templates[i];
691 
692  // Calculate the overlap between input and template_.bitmap
693  int overlap = 0;
694  int stride = input->width;
695  total = 0;
696  for (int y = 0; y < (int)SegmentableBitmapWidth; ++y)
697  {
698  std::uint32_t const* templateRow = template_.bitmap + y * 2;
699  unsigned char const* inputRow = input->pixels + y * stride;
700  for (int x = 0; x < (int)(SegmentableBitmapWidth / 32); ++x)
701  {
702  std::uint32_t compressed = templateRow[x];
703  // Decompress
704  for (int i = 0; i < 32; ++i)
705  {
706  bool inputPixel = inputRow[x * 32 + i] > 0;
707  bool templatePixel = (compressed & (1 << i)) > 0;
708  if ((inputPixel && templatePixel) ||
709  (!inputPixel && !templatePixel))
710  {
711  ++overlap;
712  }
713  ++total;
714  }
715  }
716  }
717 
718  if (overlap > bestOverlap)
719  {
720  bestOverlap = overlap;
721  bestTemplate = &template_;
722  }
723  }
724 
725  float overlapRatio =
726  1.0f * bestOverlap / (SegmentableBitmapWidth * SegmentableBitmapWidth);
727 
728  if (!bestTemplate || overlapRatio < paramTemplateMatchThreshold)
729  {
730  ARMARX_VERBOSE
731  << "Could not find a matching template (overlap ratio: " << overlapRatio
732  << ")";
733  }
734  else
735  {
736  // Calculate pose from angles
737  {
738  Transformation3d& poseData = blob.pose;
739  Vec3d convention;
740  convention.x = bestTemplate->rotationX;
741  convention.y = bestTemplate->rotationY;
742  convention.z = bestTemplate->rotationZ;
743 
744  // calculate rotation matrix
745  Mat3d temp;
746  Math3d::SetRotationMatX(poseData.rotation, convention.x);
747  Math3d::SetRotationMatZ(temp, convention.z);
748  Math3d::MulMatMat(poseData.rotation, temp, poseData.rotation);
749  Math3d::SetRotationMatY(temp, convention.y);
750  Math3d::MulMatMat(poseData.rotation, temp, poseData.rotation);
751  }
752 
753  // Calculate corrected orientation
754  Vec3d position = blob.pose.translation;
755  Mat3d orientation = blob.pose.rotation;
756  Mat3d& resultOrientation = blob.pose.rotation;
757  {
758  Vec3d u0 = {0, 0, 1};
759  Vec3d u = position;
760  Math3d::NormalizeVec(u);
761 
762  // corrective rotation matrix
763  Mat3d correctiveRotation;
764  Vec3d axis;
765  Math3d::CrossProduct(u0, u, axis);
766  float angle = Math3d::Angle(u0, u, axis);
767  Math3d::SetRotationMatAxis(correctiveRotation, axis, angle);
768 
769  // final rotation matrix
770  Math3d::MulMatMat(correctiveRotation, orientation, resultOrientation);
771  }
772 
773  // Calculate corrected position
774  orientation = resultOrientation;
775  float resultCorrelation = 0.0f;
776  float resultSizeRatio = 0.0f;
777  CByteImage* initialMask = &objectGray;
778  int nSize = region.nPixels;
779  int nSimulatedSize = 0;
780  {
781  Vec3d initialPosition = position;
782  // Calculate position in rendered images
783  // Add difference pos_real - pos_sim to pos_real
784  // This results in the simulation producing the expected image
785 
786  CStereoCalibration* stereoCalibration = getStereoCalibration();
787  const int width = stereoCalibration->width;
788  const int height = stereoCalibration->height;
789 
790  Transformation3d pose;
791  Math3d::SetVec(pose.translation, position);
792  Math3d::SetMat(pose.rotation, orientation);
793 
794  CByteImage image_left(width, height, CByteImage::eGrayScale);
795  CByteImage image_right(width, height, CByteImage::eGrayScale);
796  CByteImage* ppImages[2] = {&image_left, &image_right};
797 
798  // render left image
799  m_pOpenGLVisualizer->Clear();
800  m_pOpenGLVisualizer->ActivateShading(false);
801  m_pOpenGLVisualizer->SetProjectionMatrix(
802  stereoCalibration->GetLeftCalibration());
803  //DrawObjectFromFile(m_pOpenGLVisualizer, entry->sOivPath, pose, entry->sName);
804  CFloatMatrix* model = database[className].model.get();
805  m_pOpenGLVisualizer->DrawObject(model, pose);
806  m_pOpenGLVisualizer->GetImage(&image_left);
807  ::ImageProcessor::FlipY(&image_left, &image_left);
808 
809  // render right image
810  m_pOpenGLVisualizer->Clear();
811  m_pOpenGLVisualizer->SetProjectionMatrix(
812  stereoCalibration->GetRightCalibration());
813  //DrawObjectFromFile(m_pOpenGLVisualizer, entry->sOivPath, pose, entry->sName);
814  m_pOpenGLVisualizer->DrawObject(model, pose);
815  m_pOpenGLVisualizer->GetImage(&image_right);
816  ::ImageProcessor::FlipY(&image_right, &image_right);
817 
818  // DEBUG_SAVE_IMAGE(image_left, "04_simulated");
819  // DEBUG_SAVE_IMAGE(image_right, "05_simulated");
820 
821  // calculate stereo
822  m_pObjectFinderStereo->ClearObjectList();
823  m_pObjectFinderStereo->FindObjectsInSegmentedImage(
824  ppImages, 0, eRed, 100, false);
825  m_pObjectFinderStereo->Finalize(0, 10000, false, eRed, 10, false);
826  const Object3DList& objectList = m_pObjectFinderStereo->GetObject3DList();
827 
828  if (objectList.size() == 1)
829  {
830  Object3DEntry const& simEntry = objectList[0];
831 
832  Vec3d positionCorrection;
833  Math3d::SubtractVecVec(
834  position, simEntry.pose.translation, positionCorrection);
835  Math3d::AddVecVec(initialPosition, positionCorrection, position);
836 
837  // calculate quality measure
838  // get simulated size
839  nSimulatedSize = objectList.at(0).region_left.nPixels;
840 
841  // calculate size normalized image
842  const int k = SegmentableBitmapWidth;
843  CByteImage simulatedObject(k, k, CByteImage::eGrayScale);
844  CropImageToTemplate(
845  &image_left, simEntry.region_left, &simulatedObject);
846 
847  int sum = 0;
848  for (std::size_t j = 0; j < SegmentableBitmapSize; j++)
849  {
850  sum += abs(simulatedObject.pixels[j] - initialMask->pixels[j]);
851  }
852 
853  // calculate final measures
854  float divisor = 255.0f * SegmentableBitmapSize;
855  resultCorrelation = 1.0f - sum / divisor;
856  resultSizeRatio = nSimulatedSize < nSize
857  ? float(nSimulatedSize) / nSize
858  : float(nSize) / nSimulatedSize;
859  }
860  else
861  {
862  ARMARX_VERBOSE << "Could not find object '" << className
863  << "' in simulated image";
864  }
865  }
866 
867  if (resultSizeRatio < paramSizeRatioThreshold)
868  {
869  ARMARX_VERBOSE << "Detected blob size is too different in real image "
870  "compared to simulated image ("
871  << "real size: " << nSize
872  << ", simulated size: " << nSimulatedSize
873  << ", ratio: " << resultSizeRatio << ")";
874  }
875  else if (resultCorrelation < paramCorrelationThreshold)
876  {
877  ARMARX_VERBOSE
878  << "Detected blob does not correlate with the simulated blob ("
879  << "ratio: " << resultCorrelation << ")";
880  }
881  else
882  {
883  blob.sName = className;
884  blob.class_id = 0;
885  blob.quality = resultSizeRatio; //fResultRatio;
886  blob.quality2 = resultCorrelation; //fResultCorrelation;
887  blob.localizationValid = true;
888 
889  ARMARX_VERBOSE << "Found a match (overlap ratio: " << overlapRatio
890  << ", size ratio: " << resultSizeRatio
891  << ", correlation: " << resultCorrelation
892  << ", blob size: " << nSize << ")";
893 
894  classObjectList.push_back(blob);
895  }
896  }
897  }
898  if (classObjectList.size() > 1 && paramSingleInstance)
899  {
900  std::sort(classObjectList.begin(),
901  classObjectList.end(),
902  [](Object3DEntry const& left, Object3DEntry const& right)
903  { return left.quality > right.quality; });
904  Object3DEntry& bestMatch = classObjectList.front();
905  objectList.push_back(bestMatch);
906  }
907  else
908  {
909  objectList.insert(objectList.end(), classObjectList.begin(), classObjectList.end());
910  }
911  }
912  }
913 
914  ARMARX_VERBOSE << "Found " << objectList.size() << " object(s)";
915 
916  // help for problem analysis
917  if (objectList.size() == 0)
918  {
919  ARMARX_INFO << "Looked unsuccessfully for:";
920  std::string color;
921 
922  for (std::string const& className : objectClassNames)
923  {
924  CColorParameterSet::Translate(database[className].color, color);
925  ARMARX_INFO << className << " color: " << color << flush;
926  }
927  }
928  else
929  {
930  visualizeResults(objectList, resultImages);
931  }
932 
933  const auto agentName = getProperty<std::string>("AgentName").getValue();
934 
935  memoryx::ObjectLocalizationResultList resultList;
936 
937  for (Object3DList::iterator iter = objectList.begin(); iter != objectList.end(); iter++)
938  {
939  float x = iter->pose.translation.x;
940  float y = iter->pose.translation.y;
941  float z = iter->pose.translation.z;
942 
943  if (seq.count(iter->sName))
944  {
945  seq[iter->sName]++;
946  }
947  else
948  {
949  seq[iter->sName] = 0;
950  }
951 
952  StringVariantBaseMap mapValues;
953  mapValues["x"] = new Variant(x);
954  mapValues["y"] = new Variant(y);
955  mapValues["z"] = new Variant(z);
956  mapValues["name"] = new Variant(iter->sName);
957  mapValues["sequence"] = new Variant(seq[iter->sName]);
958  mapValues["timestamp"] = new Variant(imageMetaInfo->timeProvided / 1000.0 / 1000.0);
959  debugObserver->setDebugChannel("ObjectRecognition", mapValues);
960 
961  // only accept realistic positions
962  if (x > validResultBoundingBoxMin.x && y > validResultBoundingBoxMin.y &&
963  z > validResultBoundingBoxMin.z && x < validResultBoundingBoxMax.x &&
964  y < validResultBoundingBoxMax.y && z < validResultBoundingBoxMax.z)
965  {
966  // assemble result
967  memoryx::ObjectLocalizationResult result;
968 
969  // position and orientation
970  Eigen::Vector3f position(
971  iter->pose.translation.x, iter->pose.translation.y, iter->pose.translation.z);
972  Eigen::Matrix3f orientation;
973  orientation << iter->pose.rotation.r1, iter->pose.rotation.r2, iter->pose.rotation.r3,
974  iter->pose.rotation.r4, iter->pose.rotation.r5, iter->pose.rotation.r6,
975  iter->pose.rotation.r7, iter->pose.rotation.r8, iter->pose.rotation.r9;
976 
977  ARMARX_VERBOSE << "Reporting in frame: " << referenceFrameName;
978  result.position = new armarx::FramedPosition(position, referenceFrameName, agentName);
979  result.orientation =
980  new armarx::FramedOrientation(orientation, referenceFrameName, agentName);
981 
982  // calculate noise
983  result.positionNoise = calculateLocalizationUncertainty(iter->region_left.centroid,
984  iter->region_right.centroid);
985 
986  // calculate recognition certainty
987  result.recognitionCertainty =
988  0.5f + 0.5f * calculateRecognitionCertainty(iter->sName, *iter);
989  result.objectClassName = iter->sName;
990  result.timeStamp = new TimestampVariant(imageMetaInfo->timeProvided);
991 
992  resultList.push_back(result);
993  }
994  else
995  {
996  ARMARX_VERBOSE << "Refused unrealistic localization at position: " << x << " " << y
997  << " " << z;
998  }
999  }
1000 
1001  ARMARX_VERBOSE << "Finished localizing " << objectClassNames.at(0);
1002 
1003  return resultList;
1004 }
1005 
1006 float
1007 SegmentableTemplateRecognition::calculateRecognitionCertainty(const std::string& objectClassName,
1008  const Object3DEntry& entry)
1009 {
1010  float foundProb = entry.quality * entry.quality2;
1011  float notFoundProb = (1 - entry.quality) * (1 - entry.quality2);
1012 
1013  if (foundProb <= 0)
1014  {
1015  return 0.0f;
1016  }
1017 
1018  return foundProb / (foundProb + notFoundProb);
1019 }
1020 
1021 void
1022 SegmentableTemplateRecognition::visualizeResults(const Object3DList& objectList,
1023  CByteImage** resultImages)
1024 {
1025  m_pOpenGLVisualizer->ActivateShading(true);
1026  glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
1027 
1028 
1029  m_pOpenGLVisualizer->SetProjectionMatrix(getStereoCalibration()->GetRightCalibration());
1030 
1031  m_pOpenGLVisualizer->Clear();
1032 
1033 
1034  for (int i = 0; i < (int)objectList.size(); i++)
1035  {
1036  const Object3DEntry& entry = objectList.at(i);
1037  CFloatMatrix* model = database[entry.sName].model.get();
1038  m_pOpenGLVisualizer->DrawObject(model, entry.pose);
1039  }
1040 
1041  const int nImageIndex = 1;
1042 
1043  if (resultImages && resultImages[nImageIndex])
1044  {
1045  CByteImage tempImage(resultImages[nImageIndex]);
1046  m_pOpenGLVisualizer->GetImage(&tempImage);
1047  ::ImageProcessor::FlipY(&tempImage, &tempImage);
1048  const int nBytes = 3 * tempImage.width * tempImage.height;
1049  const unsigned char* pixels = tempImage.pixels;
1050  unsigned char* output = resultImages[nImageIndex]->pixels;
1051 
1052  for (int i = 0; i < nBytes; i += 3)
1053  {
1054  if (pixels[i])
1055  {
1056  const unsigned char g = pixels[i];
1057  output[i] = g;
1058  output[i + 1] = g;
1059  output[i + 2] = g;
1060  }
1061  }
1062  }
1063 }
getFileList
std::vector< std::string > getFileList(const std::string &path)
Definition: SegmentableTemplateRecognition.cpp:311
armarx::Variant
The Variant class is described here: Variants.
Definition: Variant.h:223
armarx::view_selection::skills::direction::state::right
state::Type right(state::Type previous)
Definition: LookDirection.cpp:264
ARMARX_VERBOSE
#define ARMARX_VERBOSE
Definition: Logging.h:187
GfxTL::Vec3d
VectorXD< 3, double > Vec3d
Definition: VectorXD.h:737
armarx::StringVariantBaseMap
std::map< std::string, VariantBasePtr > StringVariantBaseMap
Definition: ManagedIceObject.h:110
ARMARX_IMPORTANT
#define ARMARX_IMPORTANT
Definition: Logging.h:190
visionx
ArmarX headers.
Definition: OpenPoseStressTest.h:38
visionx::SegmentableTemplateEntry::data
std::unique_ptr< SegmentableTemplateHeader > data
Definition: SegmentableTemplateRecognition.h:68
Pose.h
armarx::TimestampVariant
Definition: TimestampVariant.h:54
ObjectRecognitionWrapper.h
visionx::SegmentableTemplateHeader
Definition: SegmentableTemplateRecognition.h:58
armarx::memory
Brief description of class memory.
Definition: memory.h:38
visionx::SegmentableTemplateHeader::templates
SegmentableTemplate templates[1]
Definition: SegmentableTemplateRecognition.h:63
visionx::SegmentableTemplate::rotationZ
float rotationZ
Definition: SegmentableTemplateRecognition.h:54
IceInternal::Handle
Definition: forward_declarations.h:8
visionx::SegmentableTemplateEntry::model
std::unique_ptr< CFloatMatrix > model
Definition: SegmentableTemplateRecognition.h:69
armarx::abs
std::vector< T > abs(const std::vector< T > &v)
Definition: VectorHelpers.h:281
SegmentableTemplateRecognition.h
FramedPose.h
armarx::flush
const LogSender::manipulator flush
Definition: LogSender.h:251
armarx::aron::input
ReaderT::InputType & input
Definition: rw.h:12
TimestampVariant.h
visionx::SegmentableTemplate::rotationX
float rotationX
Definition: SegmentableTemplateRecognition.h:52
armarx::read
void read(auto &eigen, auto *table)
Definition: FTSensorCalibrationGuiWidgetController.cpp:503
MemoryXCoreObjectFactories.h
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:196
GfxTL::Vec2d
VectorXD< 2, double > Vec2d
Definition: VectorXD.h:736
armarx::ctrlutil::v
double v(double t, double v0, double a0, double j)
Definition: CtrlUtil.h:39
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
memoryx::EntityWrappers
Definition: AbstractEntityWrapper.cpp:30
armarx::VariantType::FramedOrientation
const VariantTypeId FramedOrientation
Definition: FramedPose.h:39
float
#define float
Definition: 16_Level.h:22
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:181
memoryx::GridFileManagerPtr
std::shared_ptr< GridFileManager > GridFileManagerPtr
Definition: AbstractEntityWrapper.h:33
visionx::SegmentableTemplateHeader::templateCount
std::uint32_t templateCount
Definition: SegmentableTemplateRecognition.h:62
armarx::ends_with
bool ends_with(const std::string &haystack, const std::string &needle)
Definition: StringHelpers.cpp:53
visionx::SegmentableTemplate
Definition: SegmentableTemplateRecognition.h:50
angle
double angle(const Point &a, const Point &b, const Point &c)
Definition: point.hpp:109
armarx::view_selection::skills::direction::state::left
state::Type left(state::Type previous)
Definition: LookDirection.cpp:258
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
visionx::SegmentableTemplateEntry
Definition: SegmentableTemplateRecognition.h:66
GfxTL::Matrix3f
MatrixXX< 3, 3, float > Matrix3f
Definition: MatrixXX.h:649
armarx::VariantType::FramedPosition
const VariantTypeId FramedPosition
Definition: FramedPose.h:38
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:193
ArmarXDataPath.h
visionx::SegmentableTemplateEntry::color
ObjectColor color
Definition: SegmentableTemplateRecognition.h:70
armarx::ctrlutil::s
double s(double t, double s0, double v0, double a0, double j)
Definition: CtrlUtil.h:33
visionx::SegmentableTemplate::rotationY
float rotationY
Definition: SegmentableTemplateRecognition.h:53
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:27