StereoImagePointCloudProvider.h
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 
25 #pragma once
26 
27 
32 #include <VisionX/interface/components/PointCloudAndImageAndCalibrationProviderInterface.h>
33 
34 
35 #include <pcl/point_types.h>
36 
37 #include <vector>
38 #include <cmath>
39 #include <climits>
40 #include <mutex>
41 
42 
43 class CStereoCalibration;
44 
45 
46 namespace visionx
47 {
48 
49  /**
50  * @class StereoImagePointCloudProviderPropertyDefinitions
51  * @brief
52  */
55  {
56  public:
59  {
60  defineOptionalProperty<std::string>("ImageProviderAdapterName", "Armar3ImageProvider", "Name of the stereo image provider for the camera images and the calibration");
61  defineOptionalProperty<int>("DownsamplingRate", 1, "Create a point only at every n-th pixel");
62  defineOptionalProperty<bool>("SmoothDisparity", true, "Smooth the disparity image to filter out noise, but loose sharp edges");
63  }
64  };
65 
66  /**
67  * @class StereoImagePointCloudProvider
68  * @ingroup VisionX-Components
69  * @brief A brief description
70  *
71  * Detailed Description
72  */
74  virtual public StereoImagePointCloudProviderInterface,
75  virtual public CapturingPointCloudProvider,
76  virtual public ImageProcessor,
77  virtual public ImageProvider
78  {
79  public:
80  /**
81  * @see armarx::ManagedIceObject::getDefaultName()
82  */
83  std::string getDefaultName() const override
84  {
85  return "StereoImagePointCloudProvider";
86  }
87 
88  MetaPointCloudFormatPtr getDefaultPointCloudFormat() override;
89 
90  protected:
91  /**
92  * @see visionx::PointCloudProviderBase::onInitPointCloudProvider()
93  */
94  void onInitCapturingPointCloudProvider() override;
95 
96  /**
97  * @see visionx::PointCloudProviderBase::onExitPointCloudProvider()
98  */
99  void onExitCapturingPointCloudProvider() override;
100 
101  /**
102  * @see visionx::PointCloudProviderBase::onStartCapture()
103  */
104  void onStartCapture(float frameRate) override;
105 
106  /**
107  * @see visionx::PointCloudProviderBase::onStopCapture()
108  */
109  void onStopCapture() override;
110 
111  /**
112  * @see visionx::PointCloudProviderBase::doCapture()
113  */
114  bool doCapture() override;
115 
116  // ImageProcessor functions
117  void onInitImageProcessor() override;
118  void onConnectImageProcessor() override;
119  void onExitImageProcessor() override;
120  void process() override { }
121 
122  /**
123  * @see visionx::CapturingImageProvider::onInitImageProvider()
124  */
125  void onInitImageProvider() override;
126 
127  /**
128  * @see visionx::CapturingImageProvider::onExitImageProvider()
129  */
130  void onExitImageProvider() override { }
131 
132 
133  /**
134  * @see PropertyUser::createPropertyDefinitions()
135  */
137 
138  // mixed inherited stuff
139  void onInitComponent() override;
140  void onConnectComponent() override;
141  void onDisconnectComponent() override;
142  void onExitComponent() override;
143 
144  bool getImagesAreUndistorted(const ::Ice::Current& c = Ice::emptyCurrent) override
145  {
146  return imagesAreUndistorted;
147  }
148  visionx::MonocularCalibration getMonocularCalibration(const ::Ice::Current& c = Ice::emptyCurrent) override;
149 
150  bool hasSharedMemorySupport(const Ice::Current& c = Ice::emptyCurrent) override
151  {
152  return true;
153  }
154 
155 
156  private:
157 
158  ImageProviderInterfacePrx imageProviderPrx;
159  std::string providerName;
160  CStereoCalibration* stereoCalibration;
161  CByteImage** cameraImages;
162  CByteImage** cameraImagesGrey;
163  CByteImage** resultImages;
164  CByteImage* disparityImage, *disparityImageRGB;
165  float frameRate;
166  int width, height;
167  bool imagesAreUndistorted;
168  int downsamplingRate;
169  bool smoothDisparity;
170 
171  std::mutex captureLock;
172  };
173 }
CapturingPointCloudProvider.h
visionx::StereoImagePointCloudProvider::process
void process() override
Process the vision component.
Definition: StereoImagePointCloudProvider.h:120
visionx
ArmarX headers.
Definition: OpenPoseStressTest.h:38
visionx::ImageProcessor
The ImageProcessor class provides an interface for access to ImageProviders via Ice and shared memory...
Definition: ImageProcessor.h:87
visionx::StereoImagePointCloudProvider::doCapture
bool doCapture() override
Definition: StereoImagePointCloudProvider.cpp:184
visionx::StereoImagePointCloudProvider::onExitImageProvider
void onExitImageProvider() override
Definition: StereoImagePointCloudProvider.h:130
visionx::StereoImagePointCloudProvider::onInitComponent
void onInitComponent() override
Definition: StereoImagePointCloudProvider.cpp:42
visionx::StereoImagePointCloudProvider::createPropertyDefinitions
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
Definition: StereoImagePointCloudProvider.cpp:233
armarx::PropertyDefinitionContainer::prefix
std::string prefix
Prefix of the properties such as namespace, domain, component name, etc.
Definition: PropertyDefinitionContainer.h:333
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:43
visionx::StereoImagePointCloudProvider::onExitComponent
void onExitComponent() override
Definition: StereoImagePointCloudProvider.cpp:66
visionx::StereoImagePointCloudProviderPropertyDefinitions
Definition: StereoImagePointCloudProvider.h:53
visionx::StereoImagePointCloudProvider::getDefaultName
std::string getDefaultName() const override
Definition: StereoImagePointCloudProvider.h:83
ImageProcessor.h
visionx::StereoImagePointCloudProvider::getImagesAreUndistorted
bool getImagesAreUndistorted(const ::Ice::Current &c=Ice::emptyCurrent) override
Definition: StereoImagePointCloudProvider.h:144
visionx::CapturingPointCloudProviderPropertyDefinitions
Definition: CapturingPointCloudProvider.h:38
visionx::StereoImagePointCloudProvider::onConnectImageProcessor
void onConnectImageProcessor() override
Implement this method in the ImageProcessor in order execute parts when the component is fully initia...
Definition: StereoImagePointCloudProvider.cpp:112
visionx::StereoImagePointCloudProvider::onConnectComponent
void onConnectComponent() override
Definition: StereoImagePointCloudProvider.cpp:51
visionx::StereoImagePointCloudProvider
A brief description.
Definition: StereoImagePointCloudProvider.h:73
visionx::StereoImagePointCloudProvider::onExitCapturingPointCloudProvider
void onExitCapturingPointCloudProvider() override
Definition: StereoImagePointCloudProvider.cpp:88
visionx::StereoImagePointCloudProvider::getDefaultPointCloudFormat
MetaPointCloudFormatPtr getDefaultPointCloudFormat() override
default point cloud format used to initialize shared memory
Definition: StereoImagePointCloudProvider.cpp:93
Component.h
visionx::CapturingPointCloudProvider
The CapturingPointCloudProvider provides a callback function to trigger the capturing of point clouds...
Definition: CapturingPointCloudProvider.h:56
visionx::StereoImagePointCloudProvider::getMonocularCalibration
visionx::MonocularCalibration getMonocularCalibration(const ::Ice::Current &c=Ice::emptyCurrent) override
Definition: StereoImagePointCloudProvider.cpp:179
visionx::ImageProvider
ImageProvider abstract class defines a component which provide images via ice or shared memory.
Definition: ImageProvider.h:66
visionx::StereoImagePointCloudProvider::onStartCapture
void onStartCapture(float frameRate) override
Definition: StereoImagePointCloudProvider.cpp:78
visionx::StereoImagePointCloudProvider::onExitImageProcessor
void onExitImageProcessor() override
Exit the ImapeProcessor component.
Definition: StereoImagePointCloudProvider.cpp:155
visionx::StereoImagePointCloudProvider::onInitImageProcessor
void onInitImageProcessor() override
Setup the vision component.
Definition: StereoImagePointCloudProvider.cpp:104
IceUtil::Handle< class PropertyDefinitionContainer >
visionx::StereoImagePointCloudProvider::onStopCapture
void onStopCapture() override
Definition: StereoImagePointCloudProvider.cpp:83
visionx::StereoImagePointCloudProvider::onInitCapturingPointCloudProvider
void onInitCapturingPointCloudProvider() override
Definition: StereoImagePointCloudProvider.cpp:73
visionx::StereoImagePointCloudProviderPropertyDefinitions::StereoImagePointCloudProviderPropertyDefinitions
StereoImagePointCloudProviderPropertyDefinitions(std::string prefix)
Definition: StereoImagePointCloudProvider.h:57
ArmarXDataPath.h
visionx::StereoImagePointCloudProvider::hasSharedMemorySupport
bool hasSharedMemorySupport(const Ice::Current &c=Ice::emptyCurrent) override
Definition: StereoImagePointCloudProvider.h:150
visionx::StereoImagePointCloudProvider::onDisconnectComponent
void onDisconnectComponent() override
Hook for subclass.
Definition: StereoImagePointCloudProvider.cpp:58
visionx::StereoImagePointCloudProvider::onInitImageProvider
void onInitImageProvider() override
Definition: StereoImagePointCloudProvider.cpp:170