FlyCaptureImageProvider.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::FlyCaptureImageProvider
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 
26 
27 
30 
31 #include <Calibration/StereoCalibration.h>
32 
33 #include <SimoxUtility/algorithm/string/string_tools.h>
34 
35 using namespace armarx;
36 
37 
39 {
40  std::string value = getProperty<std::string>("serialNumbers").getValue();
41  std::vector<std::string> serialNumbers = simox::alg::split(value, "\t ,");
42 
43  visionx::ImageDimension dimensions = getProperty<visionx::ImageDimension>("dimensions").getValue();
44 
45  unsigned int numCameras;
46  if (busManager.GetNumOfCameras(&numCameras) != FlyCapture2::PGRERROR_OK && !numCameras)
47  {
48  ARMARX_FATAL << "no cameras found or unable to query bus";
49  return;
50  }
51 
52  ARMARX_INFO << "found " << numCameras << " cameras";
53 
54  for (std::string& serialNumber : serialNumbers)
55  {
56  FlyCapture2::PGRGuid pGuid;
57 
58  if (busManager.GetCameraFromSerialNumber(std::stoi(serialNumber), &pGuid) != FlyCapture2::PGRERROR_OK)
59  {
60  ARMARX_FATAL << "invalid serial number or camera not found: " << serialNumber;
61  }
62 
64 
65 
66 
67  if (c->Connect(&pGuid) != FlyCapture2::PGRERROR_OK)
68  {
69  ARMARX_FATAL << "unable to connect to camera!";
70  }
71 
72  FlyCapture2::CameraInfo cameraInfo;
73  if (c->GetCameraInfo(&cameraInfo) != FlyCapture2::PGRERROR_OK)
74  {
75  ARMARX_WARNING << "unable to get camera info";
76  }
77  else
78  {
79  ARMARX_INFO << " camera model: " << cameraInfo.modelName << " serial number: `" << cameraInfo.serialNumber;
80  }
81 
82  cameras.push_back(c);
83 
84 
85  colorImages.push_back(new FlyCapture2::Image(dimensions.height, dimensions.width, FlyCapture2::PIXEL_FORMAT_RGB, FlyCapture2::BayerTileFormat::RGGB));
86  }
87 
88 
89  cameraImages = new CByteImage*[cameras.size()];
90  for (size_t i = 0 ; i < cameras.size(); i++)
91  {
92  cameraImages[i] = new CByteImage(dimensions.width, dimensions.height, CByteImage::eRGB24);
93  }
94 
95  rectifyImages = false;
96 
97  undistortion = new CUndistortion();
98  undistortImages = getProperty<bool>("UndistortImages").getValue();
99  undistortion->Init(calibrationFileName.c_str());
100 
101  frameRate = getProperty<float>("FrameRate").getValue();
102 
103  setNumberImages(cameras.size());
104  setImageFormat(dimensions, visionx::eRgb, visionx::eBayerPatternRg);
105  setImageSyncMode(visionx::eCaptureSynchronization);
106 
107 
108 }
109 
111 {
112  for (FlyCapture2::Camera* c : cameras)
113  {
114  if (c->IsConnected())
115  {
116  c->Disconnect();
117  }
118 
119  delete c;
120  }
121 
122  for (FlyCapture2::Image* i : colorImages)
123  {
124  delete i;
125  }
126 
127 
128  for (size_t i = 0; i < cameras.size(); i++)
129  {
130  delete cameraImages[i];
131  }
132 
133  delete [] cameraImages;
134 
135  delete undistortion;
136 }
137 
139 {
140  for (FlyCapture2::Camera* c : cameras)
141  {
142  FlyCapture2::Property p;
143  p.absControl = true;
144  p.onOff = true;
145 
146  p.type = FlyCapture2::AUTO_EXPOSURE;
147  p.absValue = getProperty<float>("Exposure").getValue();
148  p.autoManualMode = getProperty<float>("Exposure").getValue() >= 0;
149  if (c->SetProperty(&p) != FlyCapture2::PGRERROR_OK)
150  {
151  ARMARX_WARNING << "unable to set auto exposure property";
152  }
153 
154  p.type = FlyCapture2::SHUTTER;
155  p.absValue = getProperty<float>("Shutter").getValue();
156  p.autoManualMode = getProperty<float>("Shutter").getValue() >= 0;
157  if (c->SetProperty(&p) != FlyCapture2::PGRERROR_OK)
158  {
159  ARMARX_WARNING << "unable to set shutter property";
160  }
161 
162  p.type = FlyCapture2::GAIN;
163  p.absValue = getProperty<float>("Gain").getValue();
164  p.autoManualMode = getProperty<float>("Gain").getValue() >= 0;
165  if (c->SetProperty(&p) != FlyCapture2::PGRERROR_OK)
166  {
167  ARMARX_WARNING << "unable to set gain property";
168  }
169 
170 
171  FlyCapture2::FrameRate frameRate = FlyCapture2::FRAMERATE_7_5;
172 
173  if (framesPerSecond >= 60.0)
174  {
175  frameRate = FlyCapture2::FRAMERATE_60;
176  }
177  else if (framesPerSecond >= 30.0)
178  {
179  frameRate = FlyCapture2::FRAMERATE_30;
180  }
181  else if (framesPerSecond >= 15.0)
182  {
183  frameRate = FlyCapture2::FRAMERATE_15;
184  }
185  else if (framesPerSecond >= 7.5)
186  {
187  frameRate = FlyCapture2::FRAMERATE_7_5;
188  }
189  else
190  {
191  ARMARX_WARNING << "unsupported frame rate";
192  }
193 
194  FlyCapture2::VideoMode mode = FlyCapture2::VideoMode::VIDEOMODE_640x480Y8;
195 
196 
197  if (getProperty<visionx::ImageDimension>("dimensions").getValue() == visionx::ImageDimension(640, 480))
198  {
199  mode = FlyCapture2::VideoMode::VIDEOMODE_640x480Y8;
200  }
201  else if (getProperty<visionx::ImageDimension>("dimensions").getValue() == visionx::ImageDimension(1600, 1200))
202  {
203  mode = FlyCapture2::VideoMode::VIDEOMODE_1600x1200Y8;
204  }
205  else
206  {
207  ARMARX_WARNING << "unsupported video mode";
208  }
209 
210  FlyCapture2::Error error = c->SetVideoModeAndFrameRate(mode, frameRate);
211 
212  if (error != FlyCapture2::PGRERROR_OK)
213  {
214  ARMARX_WARNING << "unable to set video mode";
215  }
216 
217  if (c->StartCapture() != FlyCapture2::PGRERROR_OK)
218  {
219  ARMARX_WARNING << "unable to start capture!";
220  }
221  }
222 }
223 
225 {
226  for (FlyCapture2::Camera* c : cameras)
227  {
228  c->StopCapture();
229  }
230 }
231 
232 bool FlyCaptureImageProvider::capture(void** ppImageBuffers)
233 {
234  visionx::ImageFormatInfo imageFormat = getImageFormat();
235  const int imageSize = imageFormat.dimension.width * imageFormat.dimension.height * imageFormat.bytesPerPixel;
236 
237  FlyCapture2::Image imageBuffer;
238  for (size_t i = 0; i < cameras.size(); i++)
239  {
240 
241  if (cameras[i]->RetrieveBuffer(&imageBuffer) != FlyCapture2::PGRERROR_OK)
242  {
243  ARMARX_WARNING << "failed to get camera buffer";
244  continue;
245  //return false;
246  }
247 
248  if (imageBuffer.Convert(FlyCapture2::PIXEL_FORMAT_RGB, colorImages[i]) != FlyCapture2::PGRERROR_OK)
249  {
250  ARMARX_WARNING << "failed to convert image";
251  return false;
252  }
253  }
254 
256 
257  for (size_t i = 0; i < cameras.size(); i++)
258  {
259  memcpy(cameraImages[i]->pixels, colorImages[i]->GetData(), imageSize);
260  }
261 
262  if (rectifyImages)
263  {
264  rectification->Rectify(cameraImages, cameraImages);
265  }
266  else if (undistortImages)
267  {
268  undistortion->Undistort(cameraImages, cameraImages);
269  }
270 
271  for (size_t i = 0; i < cameras.size(); i++)
272  {
273  memcpy(ppImageBuffers[i], cameraImages[i]->pixels, imageSize);
274  }
275 
276 
277  return true;
278 }
279 
281 {
284 }
285 
286 
288 {
290 
291  calibrationFileName = getProperty<std::string>("CalibrationFile") .getValue();
292  if (!armarx::ArmarXDataPath::getAbsolutePath(calibrationFileName, calibrationFileName))
293  {
294  ARMARX_ERROR << "" << calibrationFileName.c_str();
295  }
296 
297  CStereoCalibration ivtStereoCalibration;
298  if (!ivtStereoCalibration.LoadCameraParameters(calibrationFileName.c_str(), true))
299  {
300  ARMARX_ERROR << "" << calibrationFileName.c_str();
301  }
302 
303  rectifyImages = getProperty<bool>("RectifyImages").getValue();
304  rectification = new CRectification(true, undistortImages);
305  rectification->Init(calibrationFileName.c_str());
306  stereoCalibration = visionx::tools::convert(ivtStereoCalibration);
307 }
308 
309 
armarx::armem::human::Camera
@ Camera
Definition: util.h:14
armarx::FlyCaptureImageProvider::rectifyImages
bool rectifyImages
Definition: FlyCaptureImageProvider.h:122
armarx::FlyCaptureImageProvider::createPropertyDefinitions
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
Definition: FlyCaptureImageProvider.cpp:280
armarx::IceSharedMemoryProvider::getScopedWriteLock
SharedMemoryScopedWriteLockPtr getScopedWriteLock() const
Retrieve scoped lock for writing to the memory.
Definition: IceSharedMemoryProvider.h:156
armarx::FlyCaptureImageProvider::onStopCapture
void onStopCapture() override
This is called when the image provider capturing has been stopped.
Definition: FlyCaptureImageProvider.cpp:224
armarx::FlyCaptureStereoCameraProvider::onInitCapturingImageProvider
void onInitCapturingImageProvider() override
This is called when the Component::onInitComponent() is called.
Definition: FlyCaptureImageProvider.cpp:287
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:43
FlyCaptureImageProvider.h
armarx::SharedMemoryScopedWriteLockPtr
std::shared_ptr< SharedMemoryScopedWriteLock > SharedMemoryScopedWriteLockPtr
Definition: SharedMemoryProvider.h:46
visionx::ImageProvider::getImageFormat
ImageFormatInfo getImageFormat(const Ice::Current &c=Ice::emptyCurrent) override
Returns the entire image format info struct via Ice.
Definition: ImageProvider.cpp:75
armarx::FlyCaptureImageProvider::onInitCapturingImageProvider
void onInitCapturingImageProvider() override
This is called when the Component::onInitComponent() is called.
Definition: FlyCaptureImageProvider.cpp:38
visionx::ImageProvider::sharedMemoryProvider
armarx::IceSharedMemoryProvider< unsigned char >::pointer_type sharedMemoryProvider
shared memory provider
Definition: ImageProvider.h:256
ARMARX_FATAL
#define ARMARX_FATAL
Definition: Logging.h:192
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
cxxopts::value
std::shared_ptr< Value > value()
Definition: cxxopts.hpp:926
armarx::FlyCaptureImageProvider::onStartCapture
void onStartCapture(float frameRate) override
This is called when the image provider capturing has been started.
Definition: FlyCaptureImageProvider.cpp:138
visionx::CapturingImageProvider::capture
virtual void capture()
Definition: CapturingImageProvider.cpp:106
armarx::FlyCaptureImageProvider::undistortion
CUndistortion * undistortion
Definition: FlyCaptureImageProvider.h:118
armarx::FlyCaptureImageProvider::undistortImages
bool undistortImages
Definition: FlyCaptureImageProvider.h:121
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:189
armarx::FlyCaptureImageProvider::rectification
CRectification * rectification
Definition: FlyCaptureImageProvider.h:119
armarx::Component::getConfigIdentifier
std::string getConfigIdentifier()
Retrieve config identifier for this component as set in constructor.
Definition: Component.cpp:74
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
IceUtil::Handle< class PropertyDefinitionContainer >
TypeMapping.h
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_WARNING
#define ARMARX_WARNING
Definition: Logging.h:186
armarx::PropertyDefinitionsPtr
IceUtil::Handle< class PropertyDefinitionContainer > PropertyDefinitionsPtr
PropertyDefinitions smart pointer type.
Definition: forward_declarations.h:34
ArmarXDataPath.h
armarx::FlyCaptureImageProvider::onExitCapturingImageProvider
void onExitCapturingImageProvider() override
This is called when the Component::onExitComponent() setup is called.
Definition: FlyCaptureImageProvider.cpp:110
armarx::FlyCaptureImageProviderPropertyDefinitions
Definition: FlyCaptureImageProvider.h:49
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
armarx::split
std::vector< std::string > split(const std::string &source, const std::string &splitBy, bool trimElements=false, bool removeEmptyElements=false)
Definition: StringHelpers.cpp:36