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