RTABMapRegistration.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
5  *
6  * ArmarX is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License version 2 as
8  * published by the Free Software Foundation.
9  *
10  * ArmarX is distributed in the hope that it will be useful, but
11  * WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program. If not, see <http://www.gnu.org/licenses/>.
17  *
18  * @package VisionX::ArmarXObjects::RTABMapRegistration
19  * @author Markus Grotz ( markus dot grotz at kit dot edu )
20  * @date 2015
21  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
22  * GNU General Public License
23  */
24 
25 #include "RTABMapRegistration.h"
26 
27 #include <rtabmap/core/odometry/OdometryF2M.h>
28 
29 
30 #include <rtabmap/core/util3d.h>
31 #include <rtabmap/core/Transform.h>
32 
33 
34 
35 #include <VisionX/interface/components/PointCloudAndImageAndCalibrationProviderInterface.h>
36 
37 using namespace armarx;
38 using namespace rtabmap;
39 
40 
42 {
44  getConfigIdentifier()));
45 }
46 
48 {
49  agentName = getProperty<std::string>("agentName").getValue();
50  provideGlobalPointCloud = getProperty<bool>("provideGlobalPointCloud").getValue();
51 
52  usingProxy(getProperty<std::string>("RobotStateComponentName").getValue());
53  usingProxy(getProperty<std::string>("WorkingMemoryName").getValue());
54 
55  passThroughFilter.setFilterFieldName("z");
56  passThroughFilter.setFilterLimits(getProperty<float>("MinDistance").getValue(), getProperty<float>("MaxDistance").getValue());
57 
58  updateTask = new PeriodicTask<RTABMapRegistration>(this, &RTABMapRegistration::processData, 1000);
59 
60  ParametersMap parameters;
61  // parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapTimeThr(), "1000"));
62  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemImageKept(), "true"));
63 
64  ULogger::setType(ULogger::kTypeConsole);
65  //ULogger::setLevel(ULogger::kDebug);
66  ULogger::setLevel(ULogger::kWarning);
67 
68  rtabmap = new Rtabmap();
69  rtabmap->init(parameters);
70  rtabmapThread = new RtabmapThread(rtabmap);
71 
72  odomThread = new OdometryThread(new OdometryF2M());
73 
74  odomThread->registerToEventsManager();
75  rtabmapThread->registerToEventsManager();
76 
77  this->registerToEventsManager();
78 
79  UEventsManager::createPipe(this, odomThread, "CameraEvent");
80 }
81 
83 {
84  this->unregisterFromEventsManager();
85 
86  resultCondition.notify_one();
87  dataCondition.notify_one();
88 
89  rtabmapThread->unregisterFromEventsManager();
90  odomThread->unregisterFromEventsManager();
91 
92  odomThread->join(true);
93  rtabmapThread->join(true);
94 
95  rtabmap->close();
96 
97  delete odomThread;
98  delete rtabmapThread;
99  delete rtabmap;
100 }
101 
102 void RTABMapRegistration::onStartCapture(float framesPerSecond)
103 {
104  robotStateComponent = getProxy<RobotStateComponentInterfacePrx>(getProperty<std::string>("RobotStateComponentName").getValue());
105  workingMemory = getProxy<memoryx::WorkingMemoryInterfacePrx>(getProperty<std::string>("WorkingMemoryName").getValue());
106  agentInstancesSegment = workingMemory->getAgentInstancesSegment();
107 
108  SharedRobotInterfacePrx synchronizedRobot = robotStateComponent->getSynchronizedRobot();
109 
110  /*
111  Ice::CommunicatorPtr iceCommunicator = getIceManager()->getCommunicator();
112  agentInstance = new memoryx::AgentInstance(agentName);
113  agentInstance->setSharedRobot(synchronizedRobot);
114  agentInstance->setStringifiedSharedRobotInterfaceProxy(iceCommunicator->proxyToString(synchronizedRobot));
115  agentInstance->setAgentFilePath(robotStateComponent->getRobotFilename());
116  */
117 
118  resultPointCloudPtr.reset(new pcl::PointCloud<pcl::PointXYZRGBA>);
119  hasNewResultPointCloud = false;
120  hasNewData = false;
121 
122  FramedPoseBasePtr poseInGlobalFrame = synchronizedRobot->getRobotNode(sourceFrameName)->getGlobalPose();
123  initialCameraPose = armarx::FramedPosePtr::dynamicCast(poseInGlobalFrame)->toEigen();
124 
125  rtabmapThread->start();
126  odomThread->start();
127 
128  updateTask->start();
129 
130 
131  while (captureEnabled && (!rtabmapThread->isRunning() || !odomThread->isRunning()))
132  {
133  ARMARX_WARNING << " waiting for RTABMap";
134  sleep(1);
135  }
136 
137  // post(new OdometryResetEvent());
138 
139 
140  if (provideGlobalPointCloud)
141  {
142  requestGlobalPointCloud();
143  }
144 }
145 
147 {
148  updateTask->stop();
149 
150  odomThread->kill();
151  rtabmapThread->kill();
152 }
153 
154 void RTABMapRegistration::requestGlobalPointCloud()
155 {
156  UEventsManager::post(new RtabmapEventCmd(RtabmapEventCmd::kCmdPublish3DMap, true, true, false));
157 }
158 
159 
161 {
162  boost::mutex::scoped_lock lock(resultPointCloudMutex);
163 
164  while (captureEnabled && !hasNewResultPointCloud)
165  {
166  resultCondition.wait(lock);
167  }
168 
169  if (!captureEnabled)
170  {
171  return false;
172  }
173 
174  hasNewResultPointCloud = false;
175 
176  if (resultPointCloudPtr->points.size())
177  {
178  providePointCloud(resultPointCloudPtr);
179 
180  return true;
181  }
182  else
183  {
184  return false;
185  }
186 
187 
188 }
189 
190 
192 {
193  if (event->getClassName().compare("RtabmapEvent") == 0)
194  {
195  RtabmapEvent* rtabmapEvent = static_cast<RtabmapEvent*>(event);
196 
197  boost::mutex::scoped_lock lock(RTABDataMutex);
198 
199  /*
200  FramedPoseBasePtr poseInRootFrame = robotStateComponent->getSynchronizedRobot()->getRobotNode(sourceFrameName)->getPoseInRootFrame();
201  Eigen::Matrix4f cameraToRoot = armarx::FramedPosePtr::dynamicCast(poseInRootFrame)->toEigen();
202 
203  Signature s = stats.getSignature();
204 
205  Eigen::Matrix4f transform = s.getPose().toEigen4f() * s.getLocalTransform().toEigen4f();
206  transform.block<3, 1>(0, 3) = transform.block<3, 1>(0, 3) * Eigen::Scaling(1000.0f);
207  transform = initialCameraPose * transform;
208 
209  Eigen::Matrix4f robotPose = transform * cameraToRoot.inverse();
210  updatePosition(robotPose);
211  */
212 
213  if (!provideGlobalPointCloud)
214  {
215  signatures = rtabmapEvent->getStats().getSignatures();
216 
217  hasNewData = true;
218  dataCondition.notify_one();
219  }
220 
221  }
222  else if (event->getClassName().compare("OdometryEvent") == 0)
223  {
224  OdometryEvent* rtabmapEvent = static_cast<OdometryEvent*>(event);
225 
226  if (rtabmapEvent->pose().isNull())
227  {
228  ARMARX_WARNING << deactivateSpam(1) << "unable to register current view. odometry lost.";
229  }
230  }
231  else if (provideGlobalPointCloud && event->getClassName().compare("RtabmapEvent3DMap") == 0)
232  {
233  RtabmapEvent3DMap* rtabmapEvent = static_cast<RtabmapEvent3DMap*>(event);
234 
235  boost::mutex::scoped_lock lock(RTABDataMutex);
236 
237  signatures = rtabmapEvent->getSignatures();
238  poses = rtabmapEvent->getPoses();
239 
240  hasNewData = true;
241  dataCondition.notify_one();
242 
243  }
244 
245  return false;
246 }
247 
248 
249 
250 
251 bool RTABMapRegistration::extractPointCloud(SensorData sensorData, Eigen::Matrix4f transform, pcl::PointCloud<pcl::PointXYZRGBA>::Ptr& tempCloudPtr)
252 {
253  std::vector<CameraModel> cameraModels = sensorData.cameraModels();
254 
255  if (cameraModels.size() != 1)
256  {
257  return false;
258  }
259 
260  CameraModel m = cameraModels[0];
261 
262  transform.block<3, 1>(0, 3) = transform.block<3, 1>(0, 3) * Eigen::Scaling(1000.0f);
263  transform = initialCameraPose * transform;
264 
265  int decimation = (provideGlobalPointCloud) ? 4 : 1;
266 
267  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudPtr = util3d::cloudFromDepthRGB(sensorData.imageRaw(), sensorData.depthOrRightRaw(), m, decimation);
268 
269  pcl::copyPointCloud(*cloudPtr, *tempCloudPtr);
270  passThroughFilter.setInputCloud(tempCloudPtr);
271  passThroughFilter.filter(*tempCloudPtr);
272 
273  Eigen::Affine3f convertUnitToMM = Eigen::Affine3f(Eigen::Scaling(1000.0f));
274  pcl::transformPointCloud(*tempCloudPtr, *tempCloudPtr, convertUnitToMM);
275 
276  pcl::transformPointCloud(*tempCloudPtr, *tempCloudPtr, transform);
277 
278  return true;
279 }
280 
281 void RTABMapRegistration::processData()
282 {
283  boost::mutex::scoped_lock lock(RTABDataMutex);
284 
285  while (captureEnabled && !hasNewData)
286  {
287  dataCondition.wait(lock);
288  }
289 
290  if (!captureEnabled)
291  {
292  return;
293  }
294 
295  hasNewData = false;
296 
297  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr nextCloudPtr(new pcl::PointCloud<pcl::PointXYZRGBA>());
298 
299  for (auto& items : signatures)
300  {
301  const int id = items.first;
302  const Signature signature = items.second;
303 
304  if (signature.isBadSignature())
305  {
306  continue;
307  }
308 
309  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr tempCloudPtr(new pcl::PointCloud<pcl::PointXYZRGBA>());
310 
312 
313  if (signature.isEnabled())
314  {
315  transform = signature.getPose().toEigen4f();
316  }
317  else
318  {
319  if (poses.count(id))
320  {
321  transform = poses.at(id).toEigen4f();
322  }
323  else
324  {
325  continue;
326  }
327  }
328 
329  extractPointCloud(signature.sensorData(), transform, tempCloudPtr);
330 
331  *nextCloudPtr += *tempCloudPtr;
332 
333  /*
334  IceUtil::Time currentTime = IceUtil::Time::now();
335  std::string fileName = "/tmp/cloud" + std::to_string(currentTime.toMilliSeconds()) + ".pcd";
336  pcl::io::savePCDFile(fileName, *temp);
337  */
338  }
339 
340  if (provideGlobalPointCloud)
341  {
342  requestGlobalPointCloud();
343  }
344 
345 
346  boost::mutex::scoped_lock resultLock(resultPointCloudMutex);
347 
348  // resultPointCloudPtr->header.seq = signatures.end()
349 
350  pcl::copyPointCloud(*nextCloudPtr, *resultPointCloudPtr);
351 
352  hasNewResultPointCloud = true;
353  resultCondition.notify_one();
354 }
355 
356 void RTABMapRegistration::updatePosition(Eigen::Matrix4f pose)
357 {
358  Eigen::Matrix3f orientation = pose.block<3, 3>(0, 0);
359  FramedOrientationPtr orientationPtr = new FramedOrientation(orientation, GlobalFrame, agentName);
360  Eigen::Vector3f position = pose.block<3, 1>(0, 3);
361  FramedPositionPtr positionPtr = new FramedPosition(position, GlobalFrame, agentName);
362  agentInstance->setOrientation(orientationPtr);
363  agentInstance->setPosition(positionPtr);
364 
365  if (agentInstanceId.empty() || !agentInstancesSegment->hasEntityById(agentInstanceId))
366  {
367  agentInstanceId = agentInstancesSegment->addEntity(agentInstance);
368  }
369  else
370  {
371  agentInstancesSegment->updateEntity(agentInstanceId, agentInstance);
372  }
373 }
374 
375 
377 {
378  providerName = getProperty<std::string>("providerName").getValue();
379  usingImageProvider(providerName);
380 }
381 
382 
384 {
385  visionx::ImageType imageDisplayType = visionx::tools::typeNameToImageType("rgb");
386 
387  visionx::ImageProviderInfo imageProviderInfo = getImageProvider(providerName, imageDisplayType);
388 
389  int numImages = imageProviderInfo.numberImages;
390 
391  if (numImages != 2)
392  {
393  ARMARX_FATAL << "invalid number of images. aborting";
394  return;
395  }
396 
397  images = new CByteImage*[numImages];
398 
399  for (int i = 0 ; i < numImages ; i++)
400  {
401  images[i] = visionx::tools::createByteImage(imageProviderInfo);
402  }
403 
404  visionx::StereoCalibrationInterfacePrx calibrationProvider = visionx::StereoCalibrationInterfacePrx::checkedCast(imageProviderInfo.proxy);
405  if (!calibrationProvider)
406  {
407  ARMARX_FATAL << "unable to retrieve calibration information. aborting.";
408  return;
409  }
410 
411  sourceFrameName = calibrationProvider->getReferenceFrame();
412 
413  visionx::StereoCalibration calibration = calibrationProvider->getStereoCalibration();
414 
415  float fx = calibration.calibrationRight.cameraParam.focalLength[0];
416  float fy = calibration.calibrationRight.cameraParam.focalLength[1];
417  float cx = calibration.calibrationRight.cameraParam.principalPoint[0];
418  float cy = calibration.calibrationRight.cameraParam.principalPoint[1];
419 
420  // todo test if cameras are rectified
421  float tx = 0.075;
422  //float tx = calibration.calibrationRight.cameraParam.translation[0];
423 
424  cameraModel = CameraModel(fx, fy, cx, cy, Transform::getIdentity(), tx);
425 }
426 
428 {
429  post(new CameraEvent());
430 
431  delete images[0];
432  delete images[1];
433  delete [] images;
434 }
435 
436 
438 {
439  if (!waitForImages(1000))
440  {
441  ARMARX_WARNING << "Timeout or error while waiting for image data";
442  return;
443  }
444  long lastStamp = imageMetaInfo ? imageMetaInfo->timeProvided : 0.0;
445  if (getImages(providerName, images, imageMetaInfo) != 2)
446  {
447  ARMARX_WARNING << "Unable to transfer or read images";
448  return;
449  }
450  if (lastStamp == imageMetaInfo->timeProvided)
451  {
452  ARMARX_INFO << "Last timestamp as before - skipping image";
453  return;
454  }
455 
456  // rgb format is CV_8UC3
457  const cv::Mat tempRGBImage = cv::cvarrToMat(IplImageAdaptor::Adapt(images[0]));
458  cv::Mat RGBImage;
459  cv::cvtColor(tempRGBImage, RGBImage, CV_RGB2BGR);
460 
461 
462  // depth image format is CV_16U1 or CV_32FC1
463 
464  cv::Mat depthImage = cv::Mat(RGBImage.rows, RGBImage.cols, CV_16UC1);
465 
466  for (int j = 0; j < depthImage.rows; j++)
467  {
468  for (int i = 0; i < depthImage.cols; i++)
469  {
470  unsigned short value = images[1]->pixels[3 * (j * images[1]->width + i) + 0]
471  + (images[1]->pixels[3 * (j * images[1]->width + i) + 1] << 8);
472 
473  depthImage.at<unsigned short>(j, i) = value;
474  }
475  }
476 
477 
478  /*
479  cv::Mat disparityImage = cv::Mat(RGBImage.rows, RGBImage.cols, CV_32FC1);
480  for (int j = 0; j < disparityImage.rows; j++)
481  {
482  for (int i = 0; i < disparityImage.cols; i++)
483  {
484  int value = images[1]->pixels[3 * (j * images[1]->width + i) + 0]
485  + (images[1]->pixels[3 * (j * images[1]->width + i) + 1] << 8);
486  + (images[1]->pixels[3 * (j * images[1]->width + i) + 2] << 16);
487 
488  disparityImage.at<float>(j, i) = value;
489  }
490  }
491  const cv::Mat depthImage = util3d::depthFromDisparity(disparityImage, cameraModel.cx(), cameraModel.Tx(), CV_32FC1);
492  */
493 
494  double time = imageMetaInfo->timeProvided / 1000.0;
495  SensorData sensorData(RGBImage, depthImage, cameraModel, ++imageId, time);
496 
497  post(new CameraEvent(sensorData, providerName));
498 }
499 
500 visionx::MetaPointCloudFormatPtr RTABMapRegistration::getDefaultPointCloudFormat()
501 {
502  visionx::MetaPointCloudFormatPtr info = new visionx::MetaPointCloudFormat();
503  info->size = 640 * 480 * sizeof(visionx::ColoredPoint3D);
504 
505  if (provideGlobalPointCloud)
506  {
507  info->capacity = info->size * 50;
508  }
509  else
510  {
511  info->capacity = info->size;
512  }
513 
514  info->type = visionx::PointContentType::eColoredPoints;
515  return info;
516 }
visionx::ImageProviderInfo::numberImages
int numberImages
Number of images.
Definition: ImageProcessor.h:506
armarx::RTABMapRegistration::getDefaultPointCloudFormat
visionx::MetaPointCloudFormatPtr getDefaultPointCloudFormat() override
default point cloud format used to initialize shared memory
Definition: RTABMapRegistration.cpp:500
armarx::RTABMapRegistration::doCapture
bool doCapture() override
Main capturing function.
Definition: RTABMapRegistration.cpp:160
armarx::RTABMapRegistrationPropertyDefinitions
Definition: RTABMapRegistration.h:82
armarx::GlobalFrame
const std::string GlobalFrame
Definition: FramedPose.h:62
armarx::RTABMapRegistration::onExitImageProcessor
void onExitImageProcessor() override
Exit the ImapeProcessor component.
Definition: RTABMapRegistration.cpp:427
armarx::RTABMapRegistration::createPropertyDefinitions
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
Definition: RTABMapRegistration.cpp:41
visionx::tools::createByteImage
CByteImage * createByteImage(const ImageFormatInfo &imageFormat, const ImageType imageType)
Creates a ByteImage for the destination type specified in the given imageProviderInfo.
ARMARX_FATAL
#define ARMARX_FATAL
Definition: Logging.h:192
visionx::ImageProviderInfo
Definition: ImageProcessor.h:466
IceInternal::Handle< FramedOrientation >
deactivateSpam
SpamFilterDataPtr deactivateSpam(SpamFilterDataPtr const &spamFilter, float deactivationDurationSec, const std::string &identifier, bool deactivate)
Definition: Logging.cpp:72
armarx::RTABMapRegistration::onConnectImageProcessor
void onConnectImageProcessor() override
Implement this method in the ImageProcessor in order execute parts when the component is fully initia...
Definition: RTABMapRegistration.cpp:383
cxxopts::value
std::shared_ptr< Value > value()
Definition: cxxopts.hpp:926
armarx::RTABMapRegistration::handleEvent
bool handleEvent(UEvent *event) override
Definition: RTABMapRegistration.cpp:191
armarx::RTABMapRegistration::onInitCapturingPointCloudProvider
void onInitCapturingPointCloudProvider() override
This is called when the Component::onInitComponent() is called.
Definition: RTABMapRegistration.cpp:47
RTABMapRegistration.h
armarx::RTABMapRegistration::onStopCapture
void onStopCapture() override
This is called when the point cloud provider capturing has been stopped.
Definition: RTABMapRegistration.cpp:146
GfxTL::Matrix3f
MatrixXX< 3, 3, float > Matrix3f
Definition: MatrixXX.h:600
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:601
visionx::ImageProviderInfo::proxy
ImageProviderInterfacePrx proxy
proxy to image provider
Definition: ImageProcessor.h:472
armarx::VariantType::FramedOrientation
const VariantTypeId FramedOrientation
Definition: FramedPose.h:40
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
armarx::RTABMapRegistration::process
void process() override
Process the vision component.
Definition: RTABMapRegistration.cpp:437
IceUtil::Handle< class PropertyDefinitionContainer >
armarx::transform
auto transform(const Container< InputT, Alloc > &in, OutputT(*func)(InputT const &)) -> Container< OutputT, typename std::allocator_traits< Alloc >::template rebind_alloc< OutputT > >
Convenience function (with less typing) to transform a container of type InputT into the same contain...
Definition: algorithm.h:315
IceInternal::ProxyHandle< ::IceProxy::armarx::SharedRobotInterface >
visionx::tools::typeNameToImageType
ImageType typeNameToImageType(const std::string &imageTypeName)
Converts an image type name as string into an ImageType integer.
Definition: TypeMapping.cpp:42
armarx::RTABMapRegistration::onStartCapture
void onStartCapture(float framesPerSecond) override
This is called when the point cloud provider capturing has been started.
Definition: RTABMapRegistration.cpp:102
armarx::PeriodicTask
Definition: ArmarXManager.h:70
armarx::VariantType::FramedPosition
const VariantTypeId FramedPosition
Definition: FramedPose.h:39
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:186
armarx::PropertyDefinitionsPtr
IceUtil::Handle< class PropertyDefinitionContainer > PropertyDefinitionsPtr
PropertyDefinitions smart pointer type.
Definition: forward_declarations.h:34
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
armarx::RTABMapRegistration::onExitCapturingPointCloudProvider
void onExitCapturingPointCloudProvider() override
This is called when the Component::onExitComponent() setup is called.
Definition: RTABMapRegistration.cpp:82
armarx::RTABMapRegistration::onInitImageProcessor
void onInitImageProcessor() override
Setup the vision component.
Definition: RTABMapRegistration.cpp:376