ColorMarkerObjectLocalizer.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 "ColorMarkerObjectLocalizer.h"
24 
25 // Core
26 #include <ArmarXCore/core/system/ArmarXDataPath.h>
27 
28 // RobotState
29 #include <RobotAPI/libraries/core/Pose.h>
30 #include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
31 
32 // IVT
33 #include <DataProcessing/RANSAC.h>
34 #include <Image/ByteImage.h>
35 #include <Image/ImageProcessor.h>
36 #include <Math/Math3d.h>
37 #include <SegmentableRecognition/SegmentableDatabase.h>
38 
39 // MemoryX
40 #include <MemoryX/core/MemoryXCoreObjectFactories.h>
41 #include <MemoryX/libraries/helpers/ObjectRecognitionHelpers/ObjectRecognitionWrapper.h>
42 
43 // 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)!
44 // GLContext.h indirectly includes X.h which does "#define None", colliding with IceUtil::None.
45 #include <gui/GLContext.h>
46 
47 using namespace armarx;
48 using namespace visionx;
49 using namespace memoryx;
50 using namespace memoryx::EntityWrappers;
51 
52 ColorMarkerObjectLocalizer::ColorMarkerObjectLocalizer()
53 {
54 }
55 
56 ColorMarkerObjectLocalizer::~ColorMarkerObjectLocalizer()
57 {
58 }
59 
60 void
61 ColorMarkerObjectLocalizer::onInitObjectLocalizerProcessor()
62 {
63  usingProxy("RobotStateComponent");
64 }
65 
66 void
67 ColorMarkerObjectLocalizer::onConnectObjectLocalizerProcessor()
68 {
69  RobotStateComponentInterfacePrx robotStateComponent =
70  getProxy<RobotStateComponentInterfacePrx>("RobotStateComponent");
71  remoteRobot.reset(new RemoteRobot(robotStateComponent->getSynchronizedRobot()));
72 }
73 
74 void
75 ColorMarkerObjectLocalizer::onExitObjectLocalizerProcessor()
76 {
77 }
78 
79 bool
80 ColorMarkerObjectLocalizer::initRecognizer()
81 {
82  ARMARX_VERBOSE << "Initializing ColorMarkerObjectLocalizer";
83 
84  minPixelsPerRegion = getProperty<float>("MinPixelsPerRegion").getValue();
85  numObjectMarker = getProperty<int>("NumObjectMarker").getValue();
86  markeredObjectName = getProperty<std::string>("MarkeredObjectName").getValue();
87  maxEpipolarDistance = getProperty<float>("MaxEpipolarDistance").getValue();
88 
89  hue = getProperty<int>("Hue").getValue();
90  hueTol = getProperty<int>("HueTolerance").getValue();
91  minS = getProperty<int>("MinSaturation").getValue();
92  maxS = getProperty<int>("MaxSaturation").getValue();
93  minV = getProperty<int>("MinValue").getValue();
94  maxV = getProperty<int>("MaxValue").getValue();
95  objectMarkerColor =
96  CColorParameterSet::Translate(getProperty<std::string>("MarkerColor").getValue().c_str());
97 
98 
99  if (!segmentableRecognition)
100  {
101  colorParameters.reset(new CColorParameterSet());
102 
103  colorParameters->SetColorParameters(objectMarkerColor, hue, hueTol, minS, maxS, minV, maxV);
104 
105  ARMARX_VERBOSE << "Startup step 1";
106 
107  segmentableRecognition.reset(new CSegmentableRecognition());
108 
109  ARMARX_VERBOSE << "Startup step 2";
110 
111  // create context
112  ImageFormatInfo imageFormat = getImageFormat();
113  contextGL.reset(new CGLContext());
114 
115  ARMARX_VERBOSE << "Startup step 3";
116 
117  contextGL->CreateContext(imageFormat.dimension.width, imageFormat.dimension.height);
118 
119  ARMARX_VERBOSE << "Startup step 4";
120 
121  contextGL->MakeCurrent();
122 
123  m_pOpenGLVisualizer.reset(new COpenGLVisualizer());
124  m_pOpenGLVisualizer->InitByCalibration(getStereoCalibration()->GetRightCalibration());
125 
126  ARMARX_VERBOSE << "Startup step 5";
127 
128  // init segmentable recognition
129  bool success = segmentableRecognition->InitWithoutDatabase(colorParameters.get(),
130  getStereoCalibration());
131 
132  if (!success)
133  {
134  throw armarx::LocalException("Could not initialize segmentable object database.");
135  }
136 
137  ARMARX_VERBOSE << "ColorMarkerObjectLocalizer is initialized";
138 
139  return success;
140  }
141 
142  ARMARX_VERBOSE << "ColorMarkerObjectLocalizer is initialized";
143 
144  return true;
145 }
146 
147 memoryx::ObjectLocalizationResultList
148 ColorMarkerObjectLocalizer::localizeObjectClasses(const std::vector<std::string>& objectClassNames,
149  CByteImage** cameraImages,
150  armarx::MetaInfoSizeBasePtr imageMetaInfo,
151  CByteImage** resultImages)
152 {
153 
154  contextGL->MakeCurrent();
155 
156  Object3DList objectList;
157 
158  ARMARX_VERBOSE << "Localizing everything at once";
159 
160  segmentableRecognition->DoRecognition(cameraImages,
161  resultImages,
162  minPixelsPerRegion,
163  true,
164  maxEpipolarDistance,
165  objectMarkerColor,
166  false,
167  getImagesAreUndistorted());
168 
169  objectList = segmentableRecognition->GetObject3DList();
170 
171  ARMARX_INFO << "Found " << objectList.size() << " objects";
172 
173  memoryx::ObjectLocalizationResultList resultList;
174  // help for problem analysis
175  visualizeResults(objectList, resultImages);
176  std::string refFrame = getProperty<std::string>("ReferenceFrameName").getValue();
177  std::string camFrame("EyeLeftCamera");
178  memoryx::MultivariateNormalDistributionPtr cPositionNoise;
179  Eigen::Vector3f objectPosition(0.0, 0.0, 0.0);
180  Eigen::Matrix3f objectOrientation;
181  bool bChairLocalized = false;
182 
183  if (objectList.size() == numObjectMarker)
184  {
185 
186 
187  CVec3dArray allMarkerPositions;
188 
189 
190  for (Object3DList::iterator iter = objectList.begin(); iter != objectList.end(); iter++)
191  {
192  // assure instance belongs to queried class
193 
194  //if (iter->localizationValid)
195  {
196  // assemble result
197 
198  // position and orientation
199 
200  std::cout << iter->world_point.x << " " << iter->world_point.y << " "
201  << iter->world_point.z << std::endl;
202  std::cout << iter->pose.translation.x << " " << iter->pose.translation.y << " "
203  << iter->pose.translation.z << std::endl;
204  Eigen::Vector3f markerPosition(
205  iter->pose.translation.x, iter->pose.translation.y, iter->pose.translation.z);
206  objectPosition += markerPosition;
207  Vec3d singleMarkerPosition;
208  singleMarkerPosition.x = markerPosition(0);
209  singleMarkerPosition.y = markerPosition(1);
210  singleMarkerPosition.z = markerPosition(2);
211  allMarkerPositions.AddElement(singleMarkerPosition);
212  }
213  }
214 
215  objectPosition /= numObjectMarker;
216 
217  std::vector<int> markerPlacementIndex(numObjectMarker);
218  float min1Distance = 0;
219 
220  for (int i = 0; i < allMarkerPositions.GetSize(); i++)
221  {
222 
223 
224  for (int j = 0; j < allMarkerPositions.GetSize(); j++)
225  {
226  float temp1Distance = 0;
227 
228  if (j == i)
229  {
230  continue;
231  }
232 
233  temp1Distance += (allMarkerPositions[i].x - allMarkerPositions[j].x) *
234  (allMarkerPositions[i].x - allMarkerPositions[j].x);
235  temp1Distance += (allMarkerPositions[i].y - allMarkerPositions[j].y) *
236  (allMarkerPositions[i].y - allMarkerPositions[j].y);
237  temp1Distance += (allMarkerPositions[i].z - allMarkerPositions[j].z) *
238  (allMarkerPositions[i].z - allMarkerPositions[j].z);
239  temp1Distance = sqrt(temp1Distance);
240 
241  if (min1Distance == 0 || temp1Distance < min1Distance)
242  {
243  std::cout << "minDist " << min1Distance << std::endl;
244  min1Distance = temp1Distance;
245 
246  for (int k = 0; k < allMarkerPositions.GetSize(); k++)
247  {
248  markerPlacementIndex[k] = 0;
249  }
250 
251  markerPlacementIndex[i] = 1;
252  markerPlacementIndex[j] = 1;
253  }
254  }
255  }
256 
257  Vec3d minMeanPosition;
258  minMeanPosition.x = 0.0;
259  minMeanPosition.y = 0.0;
260  minMeanPosition.z = 0.0;
261 
262  Vec3d otherMeanPosition;
263  otherMeanPosition.x = 0.0;
264  otherMeanPosition.y = 0.0;
265  otherMeanPosition.z = 0.0;
266 
267  for (int i = 0; i < allMarkerPositions.GetSize(); i++)
268  {
269  std::cout << markerPlacementIndex[i] << " ";
270 
271  if (markerPlacementIndex[i] == 1)
272  {
273  minMeanPosition.x += allMarkerPositions[i].x;
274  minMeanPosition.y += allMarkerPositions[i].y;
275  minMeanPosition.z += allMarkerPositions[i].z;
276  }
277  else
278  {
279  otherMeanPosition.x += allMarkerPositions[i].x;
280  otherMeanPosition.y += allMarkerPositions[i].y;
281  otherMeanPosition.z += allMarkerPositions[i].z;
282  }
283  }
284 
285  std::cout << std::endl;
286  minMeanPosition.x /= 2.0;
287  minMeanPosition.y /= 2.0;
288  minMeanPosition.z /= 2.0;
289  otherMeanPosition.x /= float(numObjectMarker - 2);
290  otherMeanPosition.y /= float(numObjectMarker - 2);
291  otherMeanPosition.z /= float(numObjectMarker - 2);
292  std::cout << "minMean " << minMeanPosition.x << " " << minMeanPosition.y << " "
293  << minMeanPosition.z << std::endl;
294  std::cout << "oterhMean " << otherMeanPosition.x << " " << otherMeanPosition.y << " "
295  << otherMeanPosition.z << std::endl;
296  Vec3d ny, nz, nx;
297  //Vec3d u1, u2;
298  //Math3d::SubtractVecVec(allMarkerPositions[1], allMarkerPositions[0], u1);
299  //Math3d::SubtractVecVec(allMarkerPositions[2], allMarkerPositions[0], u2);
300  //Math3d::CrossProduct(u1, u2, ny);
301  ny.x = minMeanPosition.x - otherMeanPosition.x;
302  ny.z = minMeanPosition.z - otherMeanPosition.z;
303 
304 
305  //interpret camera coordinate in chair
306  nz.x = 0.0;
307  nz.y = -1.0;
308  nz.z = 0.0;
309  //nx.x = -1.0;nx.y = 0.0;nx.z = 0.0;
310  //ny.x = 0.0;ny.y = 0.0;ny.z = -1.0;
311  //std::cout << "nx " << nx.x << " " << nx.y << " " << nx.z << std::endl;
312  std::cout << "ny " << ny.x << " " << ny.y << " " << ny.z << std::endl;
313  //std::cout << "nz " << nz.x << " " << nz.y << " " << nz.z << std::endl;
314  Math3d::NormalizeVec(ny);
315  Math3d::CrossProduct(ny, nz, nx);
316  std::cout << "objectPosition" << std::endl;
317  std::cout << objectPosition << std::endl;
318  objectOrientation << nx.x, ny.x, nz.x, nx.y, ny.y, nz.y, nx.z, ny.z, nz.z;
319  std::cout << "objectOrientation" << std::endl;
320  std::cout << objectOrientation << std::endl;
321  cPositionNoise = calculateLocalizationUncertainty(objectList[0].region_left.centroid,
322  objectList[0].region_right.centroid);
323  ;
324  bChairLocalized = true;
325  ARMARX_VERBOSE << "Finished localizing " << markeredObjectName << " in " << refFrame;
326  }
327  else if (objectList.size() == numObjectMarker - 1)
328  {
329 
330 
331  CVec3dArray allMarkerPositions;
332 
333  for (Object3DList::iterator iter = objectList.begin(); iter != objectList.end(); iter++)
334  {
335  // assure instance belongs to queried class
336 
337  //if (iter->localizationValid)
338  {
339  // assemble result
340 
341  // position and orientation
342 
343  std::cout << iter->world_point.x << " " << iter->world_point.y << " "
344  << iter->world_point.z << std::endl;
345  std::cout << iter->pose.translation.x << " " << iter->pose.translation.y << " "
346  << iter->pose.translation.z << std::endl;
347  Eigen::Vector3f markerPosition(
348  iter->pose.translation.x, iter->pose.translation.y, iter->pose.translation.z);
349  objectPosition += markerPosition;
350  Vec3d singleMarkerPosition;
351  singleMarkerPosition.x = markerPosition(0);
352  singleMarkerPosition.y = markerPosition(1);
353  singleMarkerPosition.z = markerPosition(2);
354  allMarkerPositions.AddElement(singleMarkerPosition);
355  }
356  }
357 
358  objectPosition /= numObjectMarker;
359 
360 
361  Vec3d u1, u2, ny, nz, nx, zAxis;
362  Math3d::SubtractVecVec(allMarkerPositions[1], allMarkerPositions[0], u1);
363  Math3d::SubtractVecVec(allMarkerPositions[2], allMarkerPositions[0], u2);
364  Math3d::CrossProduct(u1, u2, ny);
365  zAxis.x = 0;
366  zAxis.y = 0;
367  zAxis.z = 1;
368 
369  if (Math3d::Angle(zAxis, ny) < M_PI)
370  {
371  ny.x = -ny.x;
372  ny.y = -ny.y;
373  ny.z = -ny.z;
374  }
375 
376 
377  //interpret camera coordinate in chair
378  nz.x = 0.0;
379  nz.y = -1.0;
380  nz.z = 0.0;
381  //nx.x = -1.0;nx.y = 0.0;nx.z = 0.0;
382  //ny.x = 0.0;ny.y = 0.0;ny.z = -1.0;
383  //std::cout << "nx " << nx.x << " " << nx.y << " " << nx.z << std::endl;
384  std::cout << "ny " << ny.x << " " << ny.y << " " << ny.z << std::endl;
385  //std::cout << "nz " << nz.x << " " << nz.y << " " << nz.z << std::endl;
386  Math3d::NormalizeVec(ny);
387  Math3d::CrossProduct(ny, nz, nx);
388  std::cout << "objectPosition" << std::endl;
389  std::cout << objectPosition << std::endl;
390  objectOrientation << nx.x, ny.x, nz.x, nx.y, ny.y, nz.y, nx.z, ny.z, nz.z;
391  std::cout << "objectOrientation" << std::endl;
392  std::cout << objectOrientation << std::endl;
393  cPositionNoise = calculateLocalizationUncertainty(objectList[0].region_left.centroid,
394  objectList[0].region_right.centroid);
395  ;
396  bChairLocalized = true;
397  ARMARX_VERBOSE << "Finished localizing " << markeredObjectName << " in " << refFrame;
398  }
399  else if (objectList.size() == numObjectMarker + 1)
400  {
401 
402 
403  CVec3dArray allMarkerPositions;
404 
405  for (Object3DList::iterator iter = objectList.begin(); iter != objectList.end(); iter++)
406  {
407  // assure instance belongs to queried class
408 
409  //if (iter->localizationValid)
410  {
411  // assemble result
412 
413  // position and orientation
414 
415  std::cout << iter->world_point.x << " " << iter->world_point.y << " "
416  << iter->world_point.z << std::endl;
417  std::cout << iter->pose.translation.x << " " << iter->pose.translation.y << " "
418  << iter->pose.translation.z << std::endl;
419  Eigen::Vector3f markerPosition(
420  iter->pose.translation.x, iter->pose.translation.y, iter->pose.translation.z);
421  objectPosition += markerPosition;
422  Vec3d singleMarkerPosition;
423  singleMarkerPosition.x = markerPosition(0);
424  singleMarkerPosition.y = markerPosition(1);
425  singleMarkerPosition.z = markerPosition(2);
426  allMarkerPositions.AddElement(singleMarkerPosition);
427  }
428  }
429 
430  objectPosition /= numObjectMarker;
431 
432  std::vector<int> markerPlacementIndex(numObjectMarker);
433  float min1Distance = 0;
434 
435  for (int i = 0; i < allMarkerPositions.GetSize(); i++)
436  {
437 
438 
439  for (int j = 0; j < allMarkerPositions.GetSize(); j++)
440  {
441  float temp1Distance = 0;
442 
443  if (j == i)
444  {
445  continue;
446  }
447 
448  temp1Distance += (allMarkerPositions[i].x - allMarkerPositions[j].x) *
449  (allMarkerPositions[i].x - allMarkerPositions[j].x);
450  temp1Distance += (allMarkerPositions[i].y - allMarkerPositions[j].y) *
451  (allMarkerPositions[i].y - allMarkerPositions[j].y);
452  temp1Distance += (allMarkerPositions[i].z - allMarkerPositions[j].z) *
453  (allMarkerPositions[i].z - allMarkerPositions[j].z);
454  temp1Distance = sqrt(temp1Distance);
455 
456  if (min1Distance == 0 || temp1Distance < min1Distance)
457  {
458  std::cout << "minDist " << min1Distance << std::endl;
459  min1Distance = temp1Distance;
460 
461  for (int k = 0; k < allMarkerPositions.GetSize(); k++)
462  {
463  markerPlacementIndex[k] = 0;
464  }
465 
466  markerPlacementIndex[i] = 1;
467  markerPlacementIndex[j] = 1;
468  }
469  }
470  }
471 
472  Vec3d minMeanPosition;
473  minMeanPosition.x = 0.0;
474  minMeanPosition.y = 0.0;
475  minMeanPosition.z = 0.0;
476  CVec3dArray otherMarkerPositions;
477  Vec3d otherMeanPosition;
478  otherMeanPosition.x = 0.0;
479  otherMeanPosition.y = 0.0;
480  otherMeanPosition.z = 0.0;
481 
482  for (int i = 0; i < allMarkerPositions.GetSize(); i++)
483  {
484  std::cout << markerPlacementIndex[i] << " ";
485 
486  if (markerPlacementIndex[i] == 1)
487  {
488  minMeanPosition.x += allMarkerPositions[i].x;
489  minMeanPosition.y += allMarkerPositions[i].y;
490  minMeanPosition.z += allMarkerPositions[i].z;
491  }
492  else
493  {
494  otherMarkerPositions.AddElement(allMarkerPositions[i]);
495  }
496  }
497 
498  std::cout << std::endl;
499  minMeanPosition.x /= 2.0;
500  minMeanPosition.y /= 2.0;
501  minMeanPosition.z /= 2.0;
502  otherMeanPosition.x /= float(numObjectMarker - 2);
503  otherMeanPosition.y /= float(numObjectMarker - 2);
504  otherMeanPosition.z /= float(numObjectMarker - 2);
505  std::cout << "minMean " << minMeanPosition.x << " " << minMeanPosition.y << " "
506  << minMeanPosition.z << std::endl;
507  std::cout << "oterhMean " << otherMeanPosition.x << " " << otherMeanPosition.y << " "
508  << otherMeanPosition.z << std::endl;
509  Vec3d u1, u2, ny, nz, nx, zAxis;
510  Math3d::SubtractVecVec(otherMarkerPositions[1], otherMarkerPositions[0], u1);
511  Math3d::SubtractVecVec(otherMarkerPositions[2], otherMarkerPositions[0], u2);
512  Math3d::CrossProduct(u1, u2, ny);
513  zAxis.x = 0;
514  zAxis.y = 0;
515  zAxis.z = 1;
516  std::cout << "ny " << ny.x << " " << ny.y << " " << ny.z << std::endl;
517 
518  if (Math3d::Angle(zAxis, ny) > M_PI)
519  {
520  ny.x = -ny.x;
521  ny.y = -ny.y;
522  ny.z = -ny.z;
523  }
524 
525 
526  //interpret camera coordinate in chair
527  nz.x = 0.0;
528  nz.y = -1.0;
529  nz.z = 0.0;
530  //nx.x = -1.0;nx.y = 0.0;nx.z = 0.0;
531  //ny.x = 0.0;ny.y = 0.0;ny.z = -1.0;
532  //std::cout << "nx " << nx.x << " " << nx.y << " " << nx.z << std::endl;
533  std::cout << "ny " << ny.x << " " << ny.y << " " << ny.z << std::endl;
534  //std::cout << "nz " << nz.x << " " << nz.y << " " << nz.z << std::endl;
535  Math3d::NormalizeVec(ny);
536  Math3d::CrossProduct(ny, nz, nx);
537  std::cout << "objectPosition" << std::endl;
538  std::cout << objectPosition << std::endl;
539  objectOrientation << nx.x, ny.x, nz.x, nx.y, ny.y, nz.y, nx.z, ny.z, nz.z;
540  std::cout << "objectOrientation" << std::endl;
541  std::cout << objectOrientation << std::endl;
542  cPositionNoise = calculateLocalizationUncertainty(objectList[0].region_left.centroid,
543  objectList[0].region_right.centroid);
544  ;
545  bChairLocalized = true;
546  ARMARX_VERBOSE << "Finished localizing " << markeredObjectName << " in " << refFrame;
547  }
548  else
549  {
550  ARMARX_VERBOSE << "No object localized. Go Closer. ";
551  }
552 
553  if (bChairLocalized)
554  {
555  memoryx::ObjectLocalizationResult result;
556  VirtualRobot::RobotNodePtr rnBase = remoteRobot->getRobotNode(refFrame);
557  VirtualRobot::RobotNodePtr rnCam = remoteRobot->getRobotNode(camFrame);
558  auto agentName = remoteRobot->getName();
559  FramedPositionPtr tmpPosition =
560  new armarx::FramedPosition(objectPosition, camFrame, agentName);
561  FramedOrientationPtr tmpOrientation =
562  new armarx::FramedOrientation(objectOrientation, camFrame, agentName);
563  FramedPosePtr tmpPose = new armarx::FramedPose(
564  tmpOrientation->toEigen(), tmpPosition->toEigen(), camFrame, agentName);
565  tmpPose->changeFrame(remoteRobot, refFrame);
566  Eigen::Vector3f zAxisChair(0.0, 0.0, 1.0);
567  Eigen::Vector3f yAxisChair(
568  (tmpPose->toEigen())(0, 1), (tmpPose->toEigen())(1, 1), (tmpPose->toEigen())(2, 1));
569  Eigen::Vector3f xAxisChair = yAxisChair.cross(zAxisChair);
570  yAxisChair = xAxisChair.cross(zAxisChair);
571  Eigen::Matrix4f objectPoseCorrected = tmpPose->toEigen();
572 
573  objectPoseCorrected(2, 3) = 50.0;
574  objectPoseCorrected(1, 3) += 400.0;
575  objectPoseCorrected.block(0, 0, 3, 1) = xAxisChair;
576  objectPoseCorrected.block(0, 2, 3, 1) = zAxisChair;
577  objectPoseCorrected.block(0, 1, 3, 1) = yAxisChair;
578 
579  result.position = new armarx::FramedPosition(objectPoseCorrected, refFrame, agentName);
580  result.orientation =
581  new armarx::FramedOrientation(objectPoseCorrected, refFrame, agentName);
582 
583  // calculate recognition certainty
584  result.recognitionCertainty = 1.0;
585 
586  result.objectClassName = markeredObjectName;
587  resultList.push_back(result);
588  }
589 
590  return resultList;
591 }
592 
593 void
594 ColorMarkerObjectLocalizer::visualizeResults(const Object3DList& objectList,
595  CByteImage** resultImages)
596 {
597  m_pOpenGLVisualizer->ActivateShading(true);
598  glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
599 
600 
601  m_pOpenGLVisualizer->SetProjectionMatrix(getStereoCalibration()->GetRightCalibration());
602 
603  m_pOpenGLVisualizer->Clear();
604 
605 
606  for (int i = 0; i < (int)objectList.size(); i++)
607  {
608  const Object3DEntry& entry = objectList.at(i);
609  CSegmentableDatabase::DrawObjectFromFile(
610  m_pOpenGLVisualizer.get(), entry.sOivFilePath.c_str(), entry.pose);
611  }
612 
613  const int nImageIndex = 1;
614 
615  if (resultImages && resultImages[nImageIndex])
616  {
617  CByteImage tempImage(resultImages[nImageIndex]);
618  m_pOpenGLVisualizer->GetImage(&tempImage);
619  ::ImageProcessor::FlipY(&tempImage, &tempImage);
620  const int nBytes = 3 * tempImage.width * tempImage.height;
621  const unsigned char* pixels = tempImage.pixels;
622  unsigned char* output = resultImages[nImageIndex]->pixels;
623 
624  for (int i = 0; i < nBytes; i += 3)
625  if (pixels[i])
626  {
627  const unsigned char g = pixels[i];
628  output[i] = g;
629  output[i + 1] = g;
630  output[i + 2] = g;
631  }
632  }
633 }
634 
635 bool
636 ColorMarkerObjectLocalizer::addObjectClass(const memoryx::EntityPtr& objectClassEntity,
637  const memoryx::GridFileManagerPtr& fileManager)
638 {
639  return true;
640 }
RemoteRobot.h
ARMARX_VERBOSE
#define ARMARX_VERBOSE
Definition: Logging.h:187
GfxTL::Vec3d
VectorXD< 3, double > Vec3d
Definition: VectorXD.h:737
ColorMarkerObjectLocalizer.h
visionx
ArmarX headers.
Definition: OpenPoseStressTest.h:38
armarx::VariantType::FramedPose
const VariantTypeId FramedPose
Definition: FramedPose.h:36
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:650
Pose.h
memoryx
VirtualRobot headers.
Definition: CommonPlacesTester.cpp:48
ObjectRecognitionWrapper.h
IceInternal::Handle
Definition: forward_declarations.h:8
M_PI
#define M_PI
Definition: MathTools.h:17
MemoryXCoreObjectFactories.h
GfxTL::sqrt
VectorXD< D, T > sqrt(const VectorXD< D, T > &a)
Definition: VectorXD.h:704
armarx::RemoteRobot
Mimics the behaviour of the VirtualRobot::Robot class while redirecting everything to an Ice proxy.
Definition: RemoteRobot.h:144
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
IceInternal::ProxyHandle<::IceProxy::armarx::RobotStateComponentInterface >
GfxTL::Matrix3f
MatrixXX< 3, 3, float > Matrix3f
Definition: MatrixXX.h:649
armarx::VariantType::FramedPosition
const VariantTypeId FramedPosition
Definition: FramedPose.h:38
ArmarXDataPath.h
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:27
armarx::status::success
@ success