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
47using namespace armarx;
48using namespace visionx;
49using namespace memoryx;
50using namespace memoryx::EntityWrappers;
51
55
59
60void
65
66void
67ColorMarkerObjectLocalizer::onConnectObjectLocalizerProcessor()
68{
69 RobotStateComponentInterfacePrx robotStateComponent =
70 getProxy<RobotStateComponentInterfacePrx>("RobotStateComponent");
71 remoteRobot.reset(new RemoteRobot(robotStateComponent->getSynchronizedRobot()));
72}
73
74void
78
79bool
80ColorMarkerObjectLocalizer::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
147memoryx::ObjectLocalizationResultList
148ColorMarkerObjectLocalizer::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
593void
594ColorMarkerObjectLocalizer::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
635bool
636ColorMarkerObjectLocalizer::addObjectClass(const memoryx::EntityPtr& objectClassEntity,
637 const memoryx::GridFileManagerPtr& fileManager)
638{
639 return true;
640}
float
#define float
Definition 16_Level.h:22
M_PI
#define M_PI
Definition MathTools.h:17
armarx::Component::getProperty
Property< PropertyType > getProperty(const std::string &name)
Definition PropertyUser.h:180
armarx::FramedOrientation
The FramedOrientation class.
Definition FramedPose.h:216
armarx::FramedPose
The FramedPose class.
Definition FramedPose.h:281
armarx::FramedPosition
The FramedPosition class.
Definition FramedPose.h:158
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::ManagedIceObject::getProxy
Ice::ObjectPrx getProxy(long timeoutMs=0, bool waitForScheduler=true) const
Returns the proxy of this object (optionally it waits for the proxy)
Definition ManagedIceObject.cpp:407
armarx::RemoteRobot
Mimics the behaviour of the VirtualRobot::Robot class while redirecting everything to an Ice proxy.
Definition RemoteRobot.h:145
visionx::ColorMarkerObjectLocalizer::initRecognizer
bool initRecognizer() override
Initializes segmentable recognition.
Definition ColorMarkerObjectLocalizer.cpp:80
visionx::ColorMarkerObjectLocalizer::localizeObjectClasses
memoryx::ObjectLocalizationResultList localizeObjectClasses(const std::vector< std::string > &objectClassNames, CByteImage **cameraImages, armarx::MetaInfoSizeBasePtr imageMetaInfo, CByteImage **resultImages) override
localizes object markers instances
Definition ColorMarkerObjectLocalizer.cpp:148
visionx::ColorMarkerObjectLocalizer::onConnectObjectLocalizerProcessor
void onConnectObjectLocalizerProcessor() override
Initializes the CSegmentableRecognition.
Definition ColorMarkerObjectLocalizer.cpp:67
visionx::ColorMarkerObjectLocalizer::addObjectClass
bool addObjectClass(const memoryx::EntityPtr &objectClassEntity, const memoryx::GridFileManagerPtr &fileManager) override
ObjectLocalizerProcessor interface: The addObjectClass method needs to be implemented by any ObjectLo...
Definition ColorMarkerObjectLocalizer.cpp:636
visionx::ObjectLocalizerProcessor::getImageFormat
ImageFormatInfo getImageFormat() const
Retrieve format of input images.
Definition ObjectLocalizerProcessor.h:357
visionx::ObjectLocalizerProcessor::getImagesAreUndistorted
bool getImagesAreUndistorted() const
Retrieve whether images are undistorted.
Definition ObjectLocalizerProcessor.h:368
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
visionx::ObjectLocalizerProcessor::imageMetaInfo
armarx::MetaInfoSizeBasePtr imageMetaInfo
Definition ObjectLocalizerProcessor.h:432
visionx::ObjectLocalizerProcessor::getStereoCalibration
CStereoCalibration * getStereoCalibration() const
Retrieve stereo calibration corresponding to image provider.
Definition ObjectLocalizerProcessor.h:391
ARMARX_INFO
#define ARMARX_INFO
The normal logging level.
Definition Logging.h:181
ARMARX_VERBOSE
#define ARMARX_VERBOSE
The logging level for verbose information.
Definition Logging.h:187
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition ArmarXTimeserver.cpp:28
armarx::FramedPositionPtr
IceInternal::Handle< FramedPosition > FramedPositionPtr
Definition FramedPose.h:149
armarx::FramedOrientationPtr
IceInternal::Handle< FramedOrientation > FramedOrientationPtr
Definition FramedPose.h:207
armarx::RobotStateComponentInterfacePrx
::IceInternal::ProxyHandle<::IceProxy::armarx::RobotStateComponentInterface > RobotStateComponentInterfacePrx
Definition RobotVisuWidget.h:63
armarx::FramedPosePtr
IceInternal::Handle< FramedPose > FramedPosePtr
Definition FramedPose.h:272
memoryx
VirtualRobot headers.
Definition CommonPlacesTester.cpp:49
memoryx::MultivariateNormalDistributionPtr
IceInternal::Handle< MultivariateNormalDistribution > MultivariateNormalDistributionPtr
Definition ProbabilityMeasures.h:279
memoryx::EntityPtr
IceInternal::Handle< Entity > EntityPtr
Typedef of EntityPtr as IceInternal::Handle<Entity> for convenience.
Definition Entity.h:45
memoryx::GridFileManagerPtr
std::shared_ptr< GridFileManager > GridFileManagerPtr
Definition AbstractEntityWrapper.h:33
visionx
ArmarX headers.
Definition OpenPoseStressTest.h:39