All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Modules Pages
gc_pointcloud.cc
Go to the documentation of this file.
1 /*
2  * This file is part of the rc_genicam_api package.
3  *
4  * Copyright (c) 2017-2024 Roboception GmbH
5  * All rights reserved
6  *
7  * Author: Heiko Hirschmueller
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions are met:
11  *
12  * 1. Redistributions of source code must retain the above copyright notice,
13  * this list of conditions and the following disclaimer.
14  *
15  * 2. Redistributions in binary form must reproduce the above copyright notice,
16  * this list of conditions and the following disclaimer in the documentation
17  * and/or other materials provided with the distribution.
18  *
19  * 3. Neither the name of the copyright holder nor the names of its contributors
20  * may be used to endorse or promote products derived from this software without
21  * specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
24  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
25  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
26  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
27  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
28  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
29  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
30  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
31  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
32  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  */
35 
36 #include <rc_genicam_api/system.h>
38 #include <rc_genicam_api/device.h>
39 #include <rc_genicam_api/stream.h>
40 #include <rc_genicam_api/buffer.h>
41 #include <rc_genicam_api/image.h>
44 #include <rc_genicam_api/config.h>
45 
47 
48 #include <Base/GCException.h>
49 
50 namespace
51 {
52 
53 /*
54  Print help text on standard output.
55 */
56 
57 void printHelp(const char *prgname)
58 {
59  // show help
60 
61  std::cout << prgname << " -h | [-o <output-filename>] [<interface-id>:]<device-id> [@<file>] [<key>=<value>] ..." << std::endl;
62  std::cout << std::endl;
63  std::cout << "Gets the first synchronized image set of the Roboception rc_visard, consisting of left, disparity, confidence and error image, creates a point cloud and stores it in ply ascii format." << std::endl;
64  std::cout << std::endl;
65  std::cout << "Options:" << std::endl;
66  std::cout << "-h Prints help information and exits" << std::endl;
67  std::cout << "-o <file> Set name of output file (default is 'rc_visard_<timestamp>.ply')" << std::endl;
68  std::cout << std::endl;
69  std::cout << "Parameters:" << std::endl;
70  std::cout << "<interface-id> Optional GenICam ID of interface for connecting to the device" << std::endl;
71  std::cout << "<device-id> GenICam device ID, serial number or user defined name of device" << std::endl;
72  std::cout << "@<file> Optional file with parameters as store with parameter 'gc_info -p ...'" << std::endl;
73  std::cout << "<key>=<value> Optional GenICam parameters to be changed in the given order" << std::endl;
74 }
75 
76 }
77 
78 int main(int argc, char *argv[])
79 {
80  int ret=0;
81 
82  try
83  {
84  // optional parameters
85 
86  std::string name="";
87 
88  int i=1;
89 
90  while (i < argc && argv[i][0] == '-')
91  {
92  if (std::string(argv[i]) == "-o")
93  {
94  i++;
95  name=argv[i++];
96  }
97  else
98  {
99  std::cout << "Unknown parameter: " << argv[i] << std::endl;
100  std::cout << std::endl;
101 
102  printHelp(argv[0]);
103 
104  return 1;
105  }
106  }
107 
108  if (i >= argc || std::string(argv[i]) == "-h")
109  {
110  printHelp(argv[0]);
111  return 1;
112  }
113 
114  // find specific device accross all systems and interfaces and open it
115 
116  std::shared_ptr<rcg::Device> dev=rcg::getDevice(argv[i++]);
117 
118  if (dev)
119  {
120  dev->open(rcg::Device::CONTROL);
121  std::shared_ptr<GenApi::CNodeMapRef> nodemap=dev->getRemoteNodeMap();
122 
123  // get focal length, baseline and disparity scale factor
124 
125  double f=rcg::getFloat(nodemap, "FocalLengthFactor", 0, 0, false);
126  double t=rcg::getFloat(nodemap, "Baseline", 0, 0, true);
127  double scale=rcg::getFloat(nodemap, "Scan3dCoordinateScale", 0, 0, true);
128 
129  // check for special exposure alternate mode of rc_visard and
130  // corresponding filter and set tolerance accordingly
131 
132  // (The exposure alternate mode is typically used with a random dot
133  // projector connected to Out1. Alternate means that Out1 is high for
134  // every second image. The rc_visard calculates disparities from images
135  // with Out1=High. However, if the alternate filter is set to OnlyLow,
136  // then it is gueranteed that Out1=Low (i.e. projector off) for all
137  // rectified images. Thus, rectified images and disparity images are
138  // always around 40 ms appart, which must be taken into account for
139  // synchronization.)
140 
141  uint64_t tol=0;
142 
143  try
144  {
145  rcg::setEnum(nodemap, "LineSelector", "Out1", true);
146  std::string linesource=rcg::getEnum(nodemap, "LineSource", true);
147  std::string filter=rcg::getEnum(nodemap, "AcquisitionAlternateFilter", true);
148 
149  if (linesource == "ExposureAlternateActive" && filter == "OnlyLow")
150  {
151  tol=50*1000*1000; // set tolerance to 50 ms
152  }
153  }
154  catch (const std::exception &)
155  {
156  // ignore possible errors
157  }
158 
159  // sanity check of some parameters
160 
161  rcg::checkFeature(nodemap, "Scan3dOutputMode", "DisparityC");
162  rcg::checkFeature(nodemap, "Scan3dCoordinateOffset", "0");
163  rcg::checkFeature(nodemap, "Scan3dInvalidDataFlag", "1");
164  rcg::checkFeature(nodemap, "Scan3dInvalidDataValue", "0");
165 
166  // set to color format if available
167 
168  rcg::setEnum(nodemap, "PixelFormat", "YCbCr411_8", false);
169 
170  // enable left image, disparity, confidence and error image and disable
171  // all others
172 
173  {
174  std::vector<std::string> component;
175 
176  rcg::getEnum(nodemap, "ComponentSelector", component, true);
177 
178  for (size_t k=0; k<component.size(); k++)
179  {
180  rcg::setEnum(nodemap, "ComponentSelector", component[k].c_str(), true);
181 
182  bool enable=(component[k] == "Intensity" || component[k] == "Disparity" ||
183  component[k] == "Confidence" || component[k] == "Error");
184  rcg::setBoolean(nodemap, "ComponentEnable", enable, true);
185  }
186  }
187 
188  // try getting synchronized data (which only has an effect if the device
189  // and GenTL producer support multipart)
190 
191  rcg::setString(nodemap, "AcquisitionMultiPartMode", "SynchronizedComponents");
192 
193  // set values as given on the command line
194 
195  while (i < argc)
196  {
197  std::string key=argv[i++];
198 
199  if (key.size() > 0 && key[0] == '@')
200  {
201  // load streamable parameters from file into nodemap
202 
203  try
204  {
205  rcg::loadStreamableParameters(nodemap, key.substr(1).c_str(), true);
206  }
207  catch (const std::exception &ex)
208  {
209  std::cerr << "Warning: Loading of parameters from file '" << key.substr(1) <<
210  "' failed at least partially" << std::endl;
211  std::cerr << ex.what() << std::endl;
212  }
213  }
214  else
215  {
216  // split argument in key and value
217 
218  std::string value;
219 
220  size_t k=key.find('=');
221  if (k != std::string::npos)
222  {
223  value=key.substr(k+1);
224  key=key.substr(0, k);
225  }
226 
227  if (value.size() > 0)
228  {
229  rcg::setString(nodemap, key.c_str(), value.c_str(), true);
230  }
231  else
232  {
233  rcg::callCommand(nodemap, key.c_str(), true);
234  }
235  }
236  }
237 
238  // open image stream
239 
240  std::vector<std::shared_ptr<rcg::Stream> > stream=dev->getStreams();
241 
242  if (stream.size() > 0)
243  {
244  // opening first stream
245 
246  stream[0]->open();
247  stream[0]->startStreaming();
248 
249  // prepare buffers for time synchronization of images (buffer at most
250  // 25 images in each list)
251 
252  rcg::ImageList left_list(50);
253  rcg::ImageList disp_list(25);
254  rcg::ImageList conf_list(25);
255  rcg::ImageList error_list(25);
256 
257  bool run=true;
258  int async=0, maxasync=50; // maximum number of asynchroneous images before giving up
259 
260  while (run && async < maxasync)
261  {
262  async++;
263 
264  // grab next image with timeout
265 
266  const rcg::Buffer *buffer=stream[0]->grab(5000);
267  if (buffer != 0)
268  {
269  // check for a complete image in the buffer
270 
271  if (!buffer->getIsIncomplete())
272  {
273  // go through all parts in case of multi-part buffer
274 
275  size_t partn=buffer->getNumberOfParts();
276  for (uint32_t part=0; part<partn; part++)
277  {
278  if (buffer->getImagePresent(part))
279  {
280  // store image in the corresponding list
281 
282  uint64_t left_tol=0;
283  uint64_t disp_tol=0;
284 
285  std::string component=rcg::getComponetOfPart(nodemap, buffer, part);
286 
287  if (component == "Intensity")
288  {
289  left_list.add(buffer, part);
290  disp_tol=tol;
291  }
292  else if (component == "Disparity")
293  {
294  disp_list.add(buffer, part);
295  left_tol=tol;
296  }
297  else if (component == "Confidence")
298  {
299  conf_list.add(buffer, part);
300  left_tol=tol;
301  }
302  else if (component == "Error")
303  {
304  error_list.add(buffer, part);
305  left_tol=tol;
306  }
307 
308  // get corresponding left and disparity images
309 
310  uint64_t timestamp=buffer->getTimestampNS();
311  std::shared_ptr<const rcg::Image> left=left_list.find(timestamp, left_tol);
312  std::shared_ptr<const rcg::Image> disp=disp_list.find(timestamp, disp_tol);
313 
314  // get confidence and error images that correspond to the
315  // disparity image
316 
317  std::shared_ptr<const rcg::Image> conf;
318  std::shared_ptr<const rcg::Image> error;
319 
320  if (disp)
321  {
322  conf=conf_list.find(disp->getTimestampNS());
323  error=error_list.find(disp->getTimestampNS());
324  }
325 
326  if (left && disp && conf && error)
327  {
328  // compute and store point cloud from synchronized image pair
329 
330  rcg::storePointCloud(name, f, t, scale, left, disp, conf, error);
331 
332  // remove all images from the buffer with the current or an
333  // older time stamp
334 
335  async=0;
336  left_list.removeOld(timestamp);
337  disp_list.removeOld(timestamp);
338  conf_list.removeOld(timestamp);
339  error_list.removeOld(timestamp);
340 
341  // in this example, we exit the grabbing loop after receiving
342  // the first synchronized image pair
343 
344  run=false;
345  }
346  }
347  }
348  }
349  }
350  else
351  {
352  std::cerr << "Cannot grab images" << std::endl;
353  break;
354  }
355  }
356 
357  // report if synchronization failed
358 
359  if (async >= maxasync && run)
360  {
361  std::cerr << "Cannot grab synchronized left and disparity image" << std::endl;
362  ret=1;
363  }
364 
365  // stopping and closing image stream
366 
367  stream[0]->stopStreaming();
368  stream[0]->close();
369  }
370  else
371  {
372  std::cerr << "No streams available" << std::endl;
373  ret=1;
374  }
375 
376  // closing the communication to the device
377 
378  dev->close();
379  }
380  else
381  {
382  std::cerr << "Device '" << argv[1] << "' not found!" << std::endl;
383  ret=1;
384  }
385  }
386  catch (const std::exception &ex)
387  {
388  std::cerr << ex.what() << std::endl;
389  ret=2;
390  }
391  catch (const GENICAM_NAMESPACE::GenericException &ex)
392  {
393  std::cerr << "Exception: " << ex.what() << std::endl;
394  ret=2;
395  }
396  catch (...)
397  {
398  std::cerr << "Unknown exception!" << std::endl;
399  ret=2;
400  }
401 
403 
404  return ret;
405 }
rcg::getFloat
double getFloat(const std::shared_ptr< GenApi::CNodeMapRef > &nodemap, const char *name, double *vmin, double *vmax, bool exception, bool igncache)
Get the value of a double feature of the given nodemap.
Definition: config.cc:696
rcg::Buffer::getImagePresent
bool getImagePresent(uint32_t part) const
Returns if a 2D, 3D or confidence image is present in the specified part.
Definition: buffer.cc:472
rcg::Buffer
The buffer class encapsulates a Genicam buffer that is provided by a stream.
Definition: buffer.h:118
rcg::getDevice
std::shared_ptr< Device > getDevice(const char *id, uint64_t timeout)
Searches across all transport layers and interfaces for a device.
Definition: device.cc:726
rcg::Buffer::getTimestampNS
uint64_t getTimestampNS() const
Returns the acquisition timestamp of the data in this buffer in ns.
Definition: buffer.cc:670
rcg::Buffer::getNumberOfParts
uint32_t getNumberOfParts() const
Returns the number of parts, excluding chunk data.
Definition: buffer.cc:203
rcg::setBoolean
bool setBoolean(const std::shared_ptr< GenApi::CNodeMapRef > &nodemap, const char *name, bool value, bool exception)
Set the value of a boolean feature of the given nodemap.
Definition: config.cc:107
rcg::Buffer::getIsIncomplete
bool getIsIncomplete() const
Signals if the buffer is incomplete due to an error.
Definition: buffer.cc:315
device.h
pixel_formats.h
rcg::callCommand
bool callCommand(const std::shared_ptr< GenApi::CNodeMapRef > &nodemap, const char *name, bool exception)
Calls the given command.
Definition: config.cc:61
rcg::ImageList::find
std::shared_ptr< const Image > find(uint64_t timestamp) const
Returns the image that has the given timestamp.
Definition: imagelist.cc:102
main
int main(int argc, char *argv[])
Definition: gc_pointcloud.cc:78
rcg::System::clearSystems
static void clearSystems()
Clears the internal list of systems.
Definition: system.cc:282
rcg::getEnum
std::string getEnum(const std::shared_ptr< GenApi::CNodeMapRef > &nodemap, const char *name, bool exception)
Get the value of an enumeration of the given nodemap.
Definition: config.cc:747
rcg::loadStreamableParameters
bool loadStreamableParameters(const std::shared_ptr< GenApi::CNodeMapRef > &nodemap, const char *name, bool exception)
Load all streamable parameters from file into the nodemap.
Definition: config.cc:1267
interface.h
GENICAM_NAMESPACE::GenericException::what
virtual const char * what() const
Get error description (overwrite from std:exception)
stream.h
image.h
rcg::Device::CONTROL
@ CONTROL
Definition: device.h:60
pointcloud.h
buffer.h
GENICAM_NAMESPACE::GenericException
GenICam's exception class.
Definition: GCException.h:63
system.h
rcg::ImageList::removeOld
void removeOld(uint64_t timestamp)
Removes all images that have a timestamp that is older or equal than the given timestamp.
Definition: imagelist.cc:73
config.h
rcg::checkFeature
void checkFeature(const std::shared_ptr< GenApi::CNodeMapRef > &nodemap, const char *name, const char *value, bool igncache)
Checks the value of given feature and throws an exception in case of a mismatch.
Definition: config.cc:1040
rcg::setString
bool setString(const std::shared_ptr< GenApi::CNodeMapRef > &nodemap, const char *name, const char *value, bool exception)
Set the value of a feature of the given nodemap.
Definition: config.cc:413
rcg::setEnum
bool setEnum(const std::shared_ptr< GenApi::CNodeMapRef > &nodemap, const char *name, const char *value, bool exception)
Set the value of an enumeration of the given nodemap.
Definition: config.cc:302
rcg::getComponetOfPart
std::string getComponetOfPart(const std::shared_ptr< GenApi::CNodeMapRef > &nodemap, const Buffer *buffer, uint32_t ipart)
Definition: config.cc:1077
imagelist.h
GCException.h
rcg::ImageList::add
void add(const std::shared_ptr< const Image > &image)
Adds the given image to the internal list.
Definition: imagelist.cc:53
rcg::ImageList
An object of this class manages a limited number of images.
Definition: imagelist.h:53
left
LOG4CPP_EXPORT CategoryStream & left(CategoryStream &os)
left manipulator
rcg::storePointCloud
void storePointCloud(std::string name, double f, double t, double scale, std::shared_ptr< const Image > left, std::shared_ptr< const Image > disp, std::shared_ptr< const Image > conf, std::shared_ptr< const Image > error)
Definition: pointcloud.cc:86


rc_genicam_api
Author(s): Heiko Hirschmueller
autogenerated on Wed Dec 4 2024 03:10:11