StereoImagePointCloudProvider.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::StereoImagePointCloudProvider
19  * @author David Schiebener (schiebener 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 
26 #include "DepthFromStereo.h"
27 
30 
31 #include <Image/ImageProcessor.h>
32 #include <Calibration/StereoCalibration.h>
33 
34 
35 using namespace armarx;
36 using namespace pcl;
37 
38 namespace visionx
39 {
40 
41 
42  void StereoImagePointCloudProvider::onInitComponent()
43  {
44  ImageProcessor::onInitComponent();
45  CapturingPointCloudProvider::onInitComponent();
46  ImageProvider::onInitComponent();
47  downsamplingRate = getProperty<int>("DownsamplingRate").getValue();
48  smoothDisparity = getProperty<bool>("SmoothDisparity").getValue();
49  }
50 
51  void StereoImagePointCloudProvider::onConnectComponent()
52  {
53  ImageProcessor::onConnectComponent();
54  CapturingPointCloudProvider::onConnectComponent();
55  ImageProvider::onConnectComponent();
56  }
57 
58  void StereoImagePointCloudProvider::onDisconnectComponent()
59  {
60  ImageProcessor::onDisconnectComponent();
61  CapturingPointCloudProvider::onDisconnectComponent();
62  ImageProvider::onDisconnectComponent();
63  }
64 
65 
66  void StereoImagePointCloudProvider::onExitComponent()
67  {
68  ImageProvider::onExitComponent();
69  CapturingPointCloudProvider::onExitComponent();
70  ImageProcessor::onExitComponent();
71  }
72 
73  void StereoImagePointCloudProvider::onInitCapturingPointCloudProvider()
74  {
75  }
76 
77 
78  void StereoImagePointCloudProvider::onStartCapture(float frameRate)
79  {
80  }
81 
82 
83  void StereoImagePointCloudProvider::onStopCapture()
84  {
85  }
86 
87 
88  void StereoImagePointCloudProvider::onExitCapturingPointCloudProvider()
89  {
90  }
91 
92 
93  MetaPointCloudFormatPtr StereoImagePointCloudProvider::getDefaultPointCloudFormat()
94  {
95  MetaPointCloudFormatPtr info = new MetaPointCloudFormat();
96  //info->frameId = getProperty<std::string>("frameId").getValue();
97  info->type = PointContentType::eColoredPoints;
98  info->capacity = 1600 * 1200 * sizeof(ColoredPoint3D);// + info->frameId.size();
99  info->size = info->capacity;
100  return info;
101  }
102 
103 
104  void StereoImagePointCloudProvider::onInitImageProcessor()
105  {
106  // set desired image provider
107  providerName = getProperty<std::string>("ImageProviderAdapterName").getValue();
108  usingImageProvider(providerName);
109  }
110 
111 
112  void StereoImagePointCloudProvider::onConnectImageProcessor()
113  {
114  // connect to image provider
115  ARMARX_INFO << getName() << " connecting to " << providerName;
116  visionx::ImageProviderInfo imageProviderInfo = getImageProvider(providerName);
117  imageProviderPrx = getProxy<ImageProviderInterfacePrx>(providerName);
118  setImageFormat(imageProviderInfo.imageFormat.dimension, imageProviderInfo.imageFormat.type, imageProviderInfo.imageFormat.bpType);
119 
120  StereoCalibrationProviderInterfacePrx calibrationProviderPrx1 = StereoCalibrationProviderInterfacePrx::checkedCast(imageProviderPrx);
121  CapturingPointCloudAndImageAndStereoCalibrationProviderInterfacePrx calibrationProviderPrx2 = CapturingPointCloudAndImageAndStereoCalibrationProviderInterfacePrx::checkedCast(imageProviderPrx);
122 
123  if (calibrationProviderPrx1)
124  {
125  stereoCalibration = visionx::tools::convert(calibrationProviderPrx1->getStereoCalibration());
126  imagesAreUndistorted = calibrationProviderPrx1->getImagesAreUndistorted();
127  }
128  else if (calibrationProviderPrx2)
129  {
130  stereoCalibration = visionx::tools::convert(calibrationProviderPrx2->getStereoCalibration());
131  imagesAreUndistorted = calibrationProviderPrx2->getImagesAreUndistorted();
132  }
133  else
134  {
135  ARMARX_WARNING << "Image provider with name " << providerName << " is not a StereoCalibrationProvider";
136  imagesAreUndistorted = true;
137  }
138 
139  cameraImages = new CByteImage*[2];
140  cameraImages[0] = tools::createByteImage(imageProviderInfo);
141  cameraImages[1] = tools::createByteImage(imageProviderInfo);
142  width = imageProviderInfo.imageFormat.dimension.width;
143  height = imageProviderInfo.imageFormat.dimension.height;
144  cameraImagesGrey = new CByteImage*[2];
145  cameraImagesGrey[0] = new CByteImage(width, height, CByteImage::eGrayScale);
146  cameraImagesGrey[1] = new CByteImage(width, height, CByteImage::eGrayScale);
147  disparityImage = new CByteImage(width, height, CByteImage::eGrayScale);
148  disparityImageRGB = new CByteImage(cameraImages[0]);
149  resultImages = new CByteImage*[2];
150  resultImages[0] = cameraImages[0];
151  resultImages[1] = disparityImageRGB;
152  }
153 
154 
155  void StereoImagePointCloudProvider::onExitImageProcessor()
156  {
157  delete cameraImages[0];
158  delete cameraImages[1];
159  delete[] cameraImages;
160  delete cameraImagesGrey[0];
161  delete cameraImagesGrey[1];
162  delete[] cameraImagesGrey;
163  delete disparityImage;
164  delete disparityImageRGB;
165  delete[] resultImages;
166  }
167 
168 
169 
170  void StereoImagePointCloudProvider::onInitImageProvider()
171  {
172  setImageFormat(visionx::ImageDimension(width, height), visionx::eRgb);
173  setNumberImages(2);
174  }
175 
176 
177 
178 
179  MonocularCalibration StereoImagePointCloudProvider::getMonocularCalibration(const Ice::Current& c)
180  {
181  return visionx::tools::convert(*stereoCalibration->GetLeftCalibration());
182  }
183 
184  bool StereoImagePointCloudProvider::doCapture()
185  {
186  std::unique_lock lock(captureLock);
187 
188  if (!waitForImages(8000))
189  {
190  ARMARX_IMPORTANT << "Timeout or error in wait for images";
191  return false;
192  }
193  else
194  {
195  // get images
196  int nNumberImages = ImageProcessor::getImages(cameraImages);
197  ARMARX_DEBUG << getName() << " got " << nNumberImages << " images";
198 
199  pcl::PointCloud<PointXYZRGBA>::Ptr pointcloud(new pcl::PointCloud<PointXYZRGBA>());
200  ::ImageProcessor::ConvertImage(cameraImages[0], cameraImagesGrey[0]);
201  ::ImageProcessor::ConvertImage(cameraImages[1], cameraImagesGrey[1]);
202  depthfromstereo::GetPointsFromDisparity(cameraImages[0], cameraImages[1], cameraImagesGrey[0], cameraImagesGrey[1], downsamplingRate,
203  *pointcloud, disparityImage, width, height, stereoCalibration, imagesAreUndistorted, smoothDisparity);
204 
205  // provide point cloud
206  ARMARX_DEBUG << "providing pointcloud";
207  providePointCloud(pointcloud);
208 
209  for (size_t j = 0; j < pointcloud->height; j++)
210  {
211  for (size_t i = 0; i < pointcloud->width; i++)
212  {
213  int idx = (j * pointcloud->width + i);
214  int value = pointcloud->points[idx].z;
215  // if(value <= max ...)
216 
217  disparityImageRGB->pixels[3 * idx + 0] = value & 0xFF;
218  disparityImageRGB->pixels[3 * idx + 1] = (value >> 8) & 0xFF;
219  }
220  }
221 
222  provideImages(resultImages);
223 
224  ARMARX_DEBUG << "done";
225 
226  return true;
227  }
228  }
229 
230 
231  PropertyDefinitionsPtr StereoImagePointCloudProvider::createPropertyDefinitions()
232  {
234  }
235 
236 }
237 
pcl
Definition: pcl_point_operators.cpp:4
ARMARX_IMPORTANT
#define ARMARX_IMPORTANT
Definition: Logging.h:183
visionx
ArmarX headers.
Definition: OpenPoseStressTest.h:38
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:43
visionx::ImageProviderInfo::imageFormat
ImageFormatInfo imageFormat
Image format struct that contains all necessary image information.
Definition: ImageProcessor.h:496
visionx::tools::createByteImage
CByteImage * createByteImage(const ImageFormatInfo &imageFormat, const ImageType imageType)
Creates a ByteImage for the destination type specified in the given imageProviderInfo.
visionx::ImageProviderInfo
Definition: ImageProcessor.h:466
visionx::tools::convert
CByteImage::ImageType convert(const ImageType visionxImageType)
Converts a VisionX image type into an image type of IVT's ByteImage.
Definition: TypeMapping.cpp:95
visionx::StereoImagePointCloudProviderPropertyDefinitions
Definition: StereoImagePointCloudProvider.h:53
cxxopts::value
std::shared_ptr< Value > value()
Definition: cxxopts.hpp:926
visionx::depthfromstereo::GetPointsFromDisparity
void GetPointsFromDisparity(const CByteImage *pImageLeftColor, const CByteImage *pImageRightColor, const CByteImage *pImageLeftGrey, const CByteImage *pImageRightGrey, const int nDisparityPointDistance, pcl::PointCloud< pcl::PointXYZRGBA > &aPointsFromDisparity, CByteImage *pDisparityImage, const int imageWidth, const int imageHeight, CStereoCalibration *pStereoCalibration, bool imagesAreUndistorted, const bool smoothDisparities)
Definition: DepthFromStereo.cpp:67
ARMARX_DEBUG
#define ARMARX_DEBUG
Definition: Logging.h:177
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
DepthFromStereo.h
IceUtil::Handle< class PropertyDefinitionContainer >
ImageUtil.h
TypeMapping.h
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
StereoImagePointCloudProvider.h