BlobRecognition.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 "BlobRecognition.h"
24 
26 
27 #include <RobotAPI/interface/core/RobotStateObserverInterface.h>
30 
31 
32 #include <Image/ByteImage.h>
33 #include <Image/ImageProcessor.h>
34 
35 
36 #include <ObjectFinder/ObjectFinderStereo.h>
37 #include <ObjectFinder/CompactRegionFilter.h>
38 #include <SegmentableRecognition/SegmentableDatabase.h>
39 
43 
44 // 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)!
45 // GLContext.h indirectly includes X.h which does "#define None", colliding with IceUtil::None.
46 #include <gui/GLContext.h>
47 
48 using namespace armarx;
49 using namespace visionx;
50 using namespace memoryx::EntityWrappers;
51 
52 
53 BlobRecognition::BlobRecognition()
54 {
55 }
56 
57 BlobRecognition::~BlobRecognition()
58 {
59 
60 }
61 
62 void BlobRecognition::onExitObjectLocalizerProcessor()
63 {
64 
65 }
66 
67 bool BlobRecognition::initRecognizer()
68 {
69 
70 
71  Eigen::Vector3f minPoint = getProperty<Eigen::Vector3f>("MinPoint").getValue();
72  Eigen::Vector3f maxPoint = getProperty<Eigen::Vector3f>("MaxPoint").getValue();
73 
74  Math3d::SetVec(validResultBoundingBoxMin, minPoint(0), minPoint(1), minPoint(2));
75  Math3d::SetVec(validResultBoundingBoxMax, maxPoint(0), maxPoint(1), maxPoint(2));
76 
77  minPixelsPerRegion = getProperty<float>("MinPixelsPerRegion").getValue();
78  maxEpipolarDistance = getProperty<float>("MaxEpipolarDistance").getValue();
79  std::string colorParameterFilename = getProperty<std::string>("ColorParameterFile").getValue();
80 
81  if (!ArmarXDataPath::getAbsolutePath(colorParameterFilename, colorParameterFilename))
82  {
83  ARMARX_ERROR << "Could not find color parameter file in ArmarXDataPath: " << colorParameterFilename;
84  }
85 
86  if (!objectFinderStereo)
87  {
88  colorParameters.reset(new CColorParameterSet());
89 
90  if (!colorParameters->LoadFromFile(colorParameterFilename.c_str()))
91  {
92  throw armarx::LocalException("Could not read color parameter file.");
93  }
94 
95  objectFinderStereo.reset(new CObjectFinderStereo());
96 
97  ImageFormatInfo imageFormat = getImageFormat();
98  contextGL.reset(new CGLContext());
99  contextGL->CreateContext(imageFormat.dimension.width, imageFormat.dimension.height);
100  contextGL->MakeCurrent();
101 
102  m_pOpenGLVisualizer.reset(new COpenGLVisualizer());
103  m_pOpenGLVisualizer->InitByCalibration(getStereoCalibration()->GetRightCalibration());
104 
105  objectFinderStereo->SetColorParameterSet(colorParameters.get());
106  objectFinderStereo->SetRegionFilter(new CCompactRegionFilter());
107 
108 
109  ARMARX_VERBOSE << "BlobRecognition is initialized";
110  }
111 
112 
113  objectFinderStereo->Init(getStereoCalibration());
114 
115 
116  return true;
117 }
118 
119 bool BlobRecognition::addObjectClass(const memoryx::EntityPtr& objectClassEntity, const memoryx::GridFileManagerPtr& fileManager)
120 {
121  ARMARX_VERBOSE << "Adding object class " << objectClassEntity->getName() << armarx::flush;
122 
123  SegmentableRecognitionWrapperPtr recognitionWrapper = objectClassEntity->addWrapper(new SegmentableRecognitionWrapper(fileManager));
124  std::string dataPath = recognitionWrapper->getDataFiles();
125  ObjectColor color = recognitionWrapper->getObjectColor();
126 
127  std::string colorString;
128  CColorParameterSet::Translate(color, colorString);
129  ARMARX_VERBOSE << "Color: " << (int)color << " (" << colorString << ")";
130 
131  if (dataPath != "")
132  {
133  // remember colors
134  std::string className = objectClassEntity->getName();
135  objectColors.insert(std::make_pair(className, color));
136  colorToObjectClass.insert(std::make_pair(color, className));
137 
138  return true;
139  }
140  else
141  {
142  ARMARX_WARNING << "Datapath is empty for " << objectClassEntity->getName() << " object class - make sure you set the correct model file in PriorKnowledge!";
143  }
144 
145  return false;
146 }
147 
148 memoryx::ObjectLocalizationResultList BlobRecognition::localizeObjectClasses(const std::vector<std::string>& objectClassNames, CByteImage** cameraImages, armarx::MetaInfoSizeBasePtr imageMetaInfo, CByteImage** resultImages)
149 {
150 
151  RemoteRobot::synchronizeLocalCloneToTimestamp(localRobot, robotStateComponent, imageMetaInfo->timeProvided / 1000.0);
152 
153  objectFinderStereo->PrepareImages(cameraImages);
154  objectFinderStereo->ClearObjectList();
155 
156  if (objectClassNames.size() < 1)
157  {
158  ARMARX_WARNING << "objectClassNames.size() = " << objectClassNames.size() << ", something is wrong here! ";
159 
160  memoryx::ObjectLocalizationResultList resultList;
161  return resultList;
162  }
163 
164  std::string allObjectNames;
165  for (size_t i = 0; i < objectClassNames.size(); i++)
166  {
167  allObjectNames.append(" ");
168  allObjectNames.append(objectClassNames.at(i));
169  }
170 
171  contextGL->MakeCurrent();
172 
173 
174  std::string color;
175 
176  for (size_t i = 0; i < objectClassNames.size(); i++)
177  {
178  CColorParameterSet::Translate(objectColors[objectClassNames.at(i)], color);
179  ARMARX_VERBOSE << "Localizing " << objectClassNames.at(i) << ", color: " << color << flush;
180 
181  ObjectColor color = objectColors[objectClassNames.at(i)];
182 
183  objectFinderStereo->FindObjects(cameraImages, resultImages, color, minPixelsPerRegion, true);
184 
185  }
186 
187  objectFinderStereo->Finalize(validResultBoundingBoxMin.z, validResultBoundingBoxMax.z, true, eNone, maxEpipolarDistance, getImagesAreUndistorted());
188  Object3DList objectList = objectFinderStereo->m_objectList;
189 
190  ARMARX_INFO << "Found " << objectList.size() << " objects";
191 
192 
193  // help for problem analysis
194  if (objectList.size() == 0)
195  {
196  ARMARX_INFO << "Looked unsuccessfully for:";
197  std::string color;
198 
199  for (size_t i = 0; i < objectClassNames.size(); i++)
200  {
201  CColorParameterSet::Translate(objectColors[objectClassNames.at(i)], color);
202  ARMARX_INFO << objectClassNames.at(i) << " color: " << color << flush;
203  }
204  }
205 
206  const auto agentName = getProperty<std::string>("AgentName").getValue();
207  memoryx::ObjectLocalizationResultList resultList;
208 
209  for (Object3DEntry const& entry : objectList)
210  {
211  bool queriedClass = false;
212 
213  if (colorToObjectClass.count(entry.color))
214  {
215  queriedClass = (std::find(objectClassNames.begin(), objectClassNames.end(), colorToObjectClass[entry.color]) != objectClassNames.end());
216  }
217 
218  if (queriedClass)
219  {
220 
221  std::string objectName = colorToObjectClass[entry.color];
222  float x = entry.pose.translation.x;
223  float y = entry.pose.translation.y;
224  float z = entry.pose.translation.z;
225 
226  if (seq.count(objectName))
227  {
228  seq[objectName]++;
229  }
230  else
231  {
232  seq[objectName] = 0;
233  }
234 
235 
236  StringVariantBaseMap mapValues;
237  mapValues["x"] = new Variant(x);
238  mapValues["y"] = new Variant(y);
239  mapValues["z"] = new Variant(z);
240  mapValues["name"] = new Variant(objectName);
241  mapValues["sequence"] = new Variant(seq[objectName]);
242  mapValues["timestamp"] = new Variant(imageMetaInfo->timeProvided / 1000.0 / 1000.0);
243  debugObserver->setDebugChannel("ObjectRecognition", mapValues);
244 
245  // only accept realistic positions
246  if (x > validResultBoundingBoxMin.x && y > validResultBoundingBoxMin.y && z > validResultBoundingBoxMin.z &&
247  x < validResultBoundingBoxMax.x && y < validResultBoundingBoxMax.y && z < validResultBoundingBoxMax.z)
248  {
249  // assemble result
250  memoryx::ObjectLocalizationResult result;
251 
252  // position and orientation
253  Eigen::Vector3f positionVec(entry.pose.translation.x, entry.pose.translation.y, entry.pose.translation.z);
254 
255  FramedPositionPtr position = new FramedPosition(positionVec, referenceFrameName, agentName);
256  position->changeToGlobal(localRobot);
257  float expectedHeight = 1020.0;
258  if (std::fabs(position->z - expectedHeight) < 50.0)
259  {
260  // manually update height
261  position->z = expectedHeight;
262  position->changeFrame(localRobot, referenceFrameName);
263 
265  FramedOrientationPtr orientation = new FramedOrientation(m, armarx::GlobalFrame, agentName);
266  orientation->changeFrame(localRobot, referenceFrameName);
267 
268  result.position = position;
269  result.orientation = orientation;
270 
271  // calculate noise
272  result.positionNoise = calculateLocalizationUncertainty(entry.region_left.centroid, entry.region_right.centroid);
273 
274  // calculate recognition certainty
275  result.recognitionCertainty = 0.5f + 0.5f * calculateRecognitionCertainty(objectName, entry);
276  result.objectClassName = objectName;
277  result.timeStamp = new TimestampVariant(imageMetaInfo->timeProvided);
278 
279  resultList.push_back(result);
280  }
281  else
282  {
283  ARMARX_INFO << deactivateSpam(1) << "Ignoring object localization result with height " << position->z;
284  }
285  }
286  else
287  {
288  ARMARX_VERBOSE_S << "Refused unrealistic localization at position: " << x << " " << y << " " << z;
289  }
290  }
291  }
292 
293  ARMARX_VERBOSE << "Finished localizing " << objectClassNames.at(0);
294 
295  return resultList;
296 }
297 
298 
299 
300 float BlobRecognition::calculateRecognitionCertainty(const std::string& objectClassName, const Object3DEntry& entry)
301 {
302  float foundProb = entry.quality * entry.quality2;
303  float notFoundProb = (1 - entry.quality) * (1 - entry.quality2);
304 
305  if (foundProb <= 0)
306  {
307  return 0.0f;
308  }
309 
310  return foundProb / (foundProb + notFoundProb);
311 }
312 
313 
314 
armarx::Variant
The Variant class is described here: Variants.
Definition: Variant.h:224
ARMARX_VERBOSE
#define ARMARX_VERBOSE
Definition: Logging.h:180
armarx::StringVariantBaseMap
std::map< std::string, VariantBasePtr > StringVariantBaseMap
Definition: ManagedIceObject.h:113
visionx
ArmarX headers.
Definition: OpenPoseStressTest.h:38
Pose.h
armarx::GlobalFrame
const std::string GlobalFrame
Definition: FramedPose.h:62
armarx::TimestampVariant
Definition: TimestampVariant.h:54
ObjectRecognitionWrapper.h
memoryx::EntityWrappers::SegmentableRecognitionWrapper
SegmentableRecognitionWrapper offers a simplified access to the data of an object class or instance r...
Definition: ObjectRecognitionWrapper.h:89
IceInternal::Handle
Definition: forward_declarations.h:8
deactivateSpam
SpamFilterDataPtr deactivateSpam(SpamFilterDataPtr const &spamFilter, float deactivationDurationSec, const std::string &identifier, bool deactivate)
Definition: Logging.cpp:72
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:523
FramedPose.h
armarx::flush
const LogSender::manipulator flush
Definition: LogSender.h:251
TimestampVariant.h
MemoryXCoreObjectFactories.h
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:189
GfxTL::Matrix3f
MatrixXX< 3, 3, float > Matrix3f
Definition: MatrixXX.h:600
memoryx::EntityWrappers
Definition: AbstractEntityWrapper.cpp:28
armarx::VariantType::FramedOrientation
const VariantTypeId FramedOrientation
Definition: FramedPose.h:40
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
memoryx::GridFileManagerPtr
std::shared_ptr< GridFileManager > GridFileManagerPtr
Definition: AbstractEntityWrapper.h:32
armarx::RemoteRobot::synchronizeLocalCloneToTimestamp
static bool synchronizeLocalCloneToTimestamp(VirtualRobot::RobotPtr robot, RobotStateComponentInterfacePrx robotStatePrx, Ice::Long timestamp)
Synchronizes a local robot to a robot state at timestamp.
Definition: RemoteRobot.cpp:487
ARMARX_VERBOSE_S
#define ARMARX_VERBOSE_S
Definition: Logging.h:200
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:111
armarx::VariantType::FramedPosition
const VariantTypeId FramedPosition
Definition: FramedPose.h:39
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:186
BlobRecognition.h
ArmarXDataPath.h
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28