CameraStereoDC1394.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #include <rtabmap/core/camera/CameraStereoDC1394.h>
29 #include <rtabmap/utilite/UTimer.h>
30 #include <rtabmap/utilite/UConversion.h>
31 #include <opencv2/imgproc/types_c.h>
32 
33 #ifdef RTABMAP_DC1394
34 #include <dc1394/dc1394.h>
35 #endif
36 
37 namespace rtabmap
38 {
39 
40 #ifdef RTABMAP_DC1394
41 class DC1394Device
42 {
43 public:
44  DC1394Device() :
45  camera_(0),
46  context_(0)
47  {
48 
49  }
50  ~DC1394Device()
51  {
52  if (camera_)
53  {
54  if (DC1394_SUCCESS != dc1394_video_set_transmission(camera_, DC1394_OFF) ||
55  DC1394_SUCCESS != dc1394_capture_stop(camera_))
56  {
57  UWARN("unable to stop camera");
58  }
59 
60  // Free resources
61  dc1394_capture_stop(camera_);
62  dc1394_camera_free(camera_);
63  camera_ = NULL;
64  }
65  if(context_)
66  {
67  dc1394_free(context_);
68  context_ = NULL;
69  }
70  }
71 
72  const std::string & guid() const {return guid_;}
73 
74  bool init()
75  {
76  if(camera_)
77  {
78  // Free resources
79  dc1394_capture_stop(camera_);
80  dc1394_camera_free(camera_);
81  camera_ = NULL;
82  }
83 
84  // look for a camera
85  int err;
86  if(context_ == NULL)
87  {
88  context_ = dc1394_new ();
89  if (context_ == NULL)
90  {
91  UERROR( "Could not initialize dc1394_context.\n"
92  "Make sure /dev/raw1394 exists, you have access permission,\n"
93  "and libraw1394 development package is installed.");
94  return false;
95  }
96  }
97 
98  dc1394camera_list_t *list;
99  err = dc1394_camera_enumerate(context_, &list);
100  if (err != DC1394_SUCCESS)
101  {
102  UERROR("Could not get camera list");
103  return false;
104  }
105 
106  if (list->num == 0)
107  {
108  UERROR("No cameras found");
109  dc1394_camera_free_list (list);
110  return false;
111  }
112  uint64_t guid = list->ids[0].guid;
113  dc1394_camera_free_list (list);
114 
115  // Create a camera
116  camera_ = dc1394_camera_new (context_, guid);
117  if (!camera_)
118  {
119  UERROR("Failed to initialize camera with GUID [%016lx]", guid);
120  return false;
121  }
122 
123  uint32_t value[3];
124  value[0]= camera_->guid & 0xffffffff;
125  value[1]= (camera_->guid >>32) & 0x000000ff;
126  value[2]= (camera_->guid >>40) & 0xfffff;
127  guid_ = uFormat("%06x%02x%08x", value[2], value[1], value[0]);
128 
129  UINFO("camera model: %s %s", camera_->vendor, camera_->model);
130 
131  // initialize camera
132  // Enable IEEE1394b mode if the camera and bus support it
133  bool bmode = camera_->bmode_capable;
134  if (bmode
135  && (DC1394_SUCCESS !=
136  dc1394_video_set_operation_mode(camera_,
137  DC1394_OPERATION_MODE_1394B)))
138  {
139  bmode = false;
140  UWARN("failed to set IEEE1394b mode");
141  }
142 
143  // start with highest speed supported
144  dc1394speed_t request = DC1394_ISO_SPEED_3200;
145  int rate = 3200;
146  if (!bmode)
147  {
148  // not IEEE1394b capable: so 400Mb/s is the limit
149  request = DC1394_ISO_SPEED_400;
150  rate = 400;
151  }
152 
153  // round requested speed down to next-lower defined value
154  while (rate > 400)
155  {
156  if (request <= DC1394_ISO_SPEED_MIN)
157  {
158  // get current ISO speed of the device
159  dc1394speed_t curSpeed;
160  if (DC1394_SUCCESS == dc1394_video_get_iso_speed(camera_, &curSpeed) && curSpeed <= DC1394_ISO_SPEED_MAX)
161  {
162  // Translate curSpeed back to an int for the parameter
163  // update, works as long as any new higher speeds keep
164  // doubling.
165  request = curSpeed;
166  rate = 100 << (curSpeed - DC1394_ISO_SPEED_MIN);
167  }
168  else
169  {
170  UWARN("Unable to get ISO speed; assuming 400Mb/s");
171  rate = 400;
172  request = DC1394_ISO_SPEED_400;
173  }
174  break;
175  }
176  // continue with next-lower possible value
177  request = (dc1394speed_t) ((int) request - 1);
178  rate = rate / 2;
179  }
180 
181  // set the requested speed
182  if (DC1394_SUCCESS != dc1394_video_set_iso_speed(camera_, request))
183  {
184  UERROR("Failed to set iso speed");
185  return false;
186  }
187 
188  // set video mode
189  dc1394video_modes_t vmodes;
190  err = dc1394_video_get_supported_modes(camera_, &vmodes);
191  if (err != DC1394_SUCCESS)
192  {
193  UERROR("unable to get supported video modes");
194  return (dc1394video_mode_t) 0;
195  }
196 
197  // see if requested mode is available
198  bool found = false;
199  dc1394video_mode_t videoMode = DC1394_VIDEO_MODE_FORMAT7_3; // bumblebee
200  for (uint32_t i = 0; i < vmodes.num; ++i)
201  {
202  if (vmodes.modes[i] == videoMode)
203  {
204  found = true;
205  }
206  }
207  if(!found)
208  {
209  UERROR("unable to get video mode %d", videoMode);
210  return false;
211  }
212 
213  if (DC1394_SUCCESS != dc1394_video_set_mode(camera_, videoMode))
214  {
215  UERROR("Failed to set video mode %d", videoMode);
216  return false;
217  }
218 
219  // special handling for Format7 modes
220  if (dc1394_is_video_mode_scalable(videoMode) == DC1394_TRUE)
221  {
222  if (DC1394_SUCCESS != dc1394_format7_set_color_coding(camera_, videoMode, DC1394_COLOR_CODING_RAW16))
223  {
224  UERROR("Could not set color coding");
225  return false;
226  }
227  uint32_t packetSize;
228  if (DC1394_SUCCESS != dc1394_format7_get_recommended_packet_size(camera_, videoMode, &packetSize))
229  {
230  UERROR("Could not get default packet size");
231  return false;
232  }
233 
234  if (DC1394_SUCCESS != dc1394_format7_set_packet_size(camera_, videoMode, packetSize))
235  {
236  UERROR("Could not set packet size");
237  return false;
238  }
239  }
240  else
241  {
242  UERROR("Video is not in mode scalable");
243  }
244 
245  // start the device streaming data
246  // Set camera to use DMA, improves performance.
247  if (DC1394_SUCCESS != dc1394_capture_setup(camera_, 4, DC1394_CAPTURE_FLAGS_DEFAULT))
248  {
249  UERROR("Failed to open device!");
250  return false;
251  }
252 
253  // Start transmitting camera data
254  if (DC1394_SUCCESS != dc1394_video_set_transmission(camera_, DC1394_ON))
255  {
256  UERROR("Failed to start device!");
257  return false;
258  }
259 
260  return true;
261  }
262 
263  bool getImages(cv::Mat & left, cv::Mat & right)
264  {
265  if(camera_)
266  {
267  dc1394video_frame_t * frame = NULL;
268  UDEBUG("[%016lx] waiting camera", camera_->guid);
269  dc1394_capture_dequeue (camera_, DC1394_CAPTURE_POLICY_WAIT, &frame);
270  if (!frame)
271  {
272  UERROR("Unable to capture frame");
273  return false;
274  }
275  dc1394video_frame_t frame1 = *frame;
276  // deinterlace frame into two imagesCount one on top the other
277  size_t frame1_size = frame->total_bytes;
278  frame1.image = (unsigned char *) malloc(frame1_size);
279  frame1.allocated_image_bytes = frame1_size;
280  frame1.color_coding = DC1394_COLOR_CODING_RAW8;
281  int err = dc1394_deinterlace_stereo_frames(frame, &frame1, DC1394_STEREO_METHOD_INTERLACED);
282  if (err != DC1394_SUCCESS)
283  {
284  free(frame1.image);
285  dc1394_capture_enqueue(camera_, frame);
286  UERROR("Could not extract stereo frames");
287  return false;
288  }
289 
290  uint8_t* capture_buffer = reinterpret_cast<uint8_t *>(frame1.image);
291  UASSERT(capture_buffer);
292 
293  cv::Mat image(frame->size[1], frame->size[0], CV_8UC3);
294  cv::Mat image2 = image.clone();
295 
296  //DC1394_COLOR_CODING_RAW16:
297  //DC1394_COLOR_FILTER_BGGR
298  cv::cvtColor(cv::Mat(frame->size[1], frame->size[0], CV_8UC1, capture_buffer), left, CV_BayerRG2BGR);
299  cv::cvtColor(cv::Mat(frame->size[1], frame->size[0], CV_8UC1, capture_buffer+image.total()), right, CV_BayerRG2GRAY);
300 
301  dc1394_capture_enqueue(camera_, frame);
302 
303  free(frame1.image);
304 
305  return true;
306  }
307  return false;
308  }
309 
310 private:
311  dc1394camera_t *camera_;
312  dc1394_t *context_;
313  std::string guid_;
314 };
315 #endif
316 
317 bool CameraStereoDC1394::available()
318 {
319 #ifdef RTABMAP_DC1394
320  return true;
321 #else
322  return false;
323 #endif
324 }
325 
326 CameraStereoDC1394::CameraStereoDC1394(float imageRate, const Transform & localTransform) :
327  Camera(imageRate, localTransform)
328 #ifdef RTABMAP_DC1394
329  ,
330  device_(0)
331 #endif
332 {
333 #ifdef RTABMAP_DC1394
334  device_ = new DC1394Device();
335 #endif
336 }
337 
338 CameraStereoDC1394::~CameraStereoDC1394()
339 {
340 #ifdef RTABMAP_DC1394
341  delete device_;
342 #endif
343 }
344 
345 bool CameraStereoDC1394::init(const std::string & calibrationFolder, const std::string & cameraName)
346 {
347 #ifdef RTABMAP_DC1394
348  if(device_)
349  {
350  bool ok = device_->init();
351  if(ok)
352  {
353  // look for calibration files
354  if(!calibrationFolder.empty())
355  {
356  if(!stereoModel_.load(calibrationFolder, cameraName.empty()?device_->guid():cameraName, false))
357  {
358  UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!",
359  cameraName.empty()?device_->guid().c_str():cameraName.c_str(), calibrationFolder.c_str());
360  }
361  else
362  {
363  UINFO("Stereo parameters: fx=%f cx=%f cy=%f baseline=%f",
364  stereoModel_.left().fx(),
365  stereoModel_.left().cx(),
366  stereoModel_.left().cy(),
367  stereoModel_.baseline());
368  }
369  }
370  }
371  return ok;
372  }
373 #else
374  UERROR("CameraDC1394: RTAB-Map is not built with dc1394 support!");
375 #endif
376  return false;
377 }
378 
379 bool CameraStereoDC1394::isCalibrated() const
380 {
381 #ifdef RTABMAP_DC1394
382  return stereoModel_.isValidForProjection();
383 #else
384  return false;
385 #endif
386 }
387 
388 std::string CameraStereoDC1394::getSerial() const
389 {
390 #ifdef RTABMAP_DC1394
391  if(device_)
392  {
393  return device_->guid();
394  }
395 #endif
396  return "";
397 }
398 
399 SensorData CameraStereoDC1394::captureImage(SensorCaptureInfo * info)
400 {
401  SensorData data;
402 #ifdef RTABMAP_DC1394
403  if(device_)
404  {
405  cv::Mat left, right;
406  device_->getImages(left, right);
407 
408  if(!left.empty() && !right.empty())
409  {
410  // Rectification
411  if(stereoModel_.left().isValidForRectification())
412  {
413  left = stereoModel_.left().rectifyImage(left);
414  }
415  if(stereoModel_.right().isValidForRectification())
416  {
417  right = stereoModel_.right().rectifyImage(right);
418  }
419  StereoCameraModel model;
420  if(stereoModel_.isValidForProjection())
421  {
422  model = StereoCameraModel(
423  stereoModel_.left().fx(), //fx
424  stereoModel_.left().fy(), //fy
425  stereoModel_.left().cx(), //cx
426  stereoModel_.left().cy(), //cy
427  stereoModel_.baseline(),
428  this->getLocalTransform(),
429  left.size());
430  }
431  data = SensorData(left, right, model, this->getNextSeqID(), UTimer::now());
432  }
433  }
434 #else
435  UERROR("CameraDC1394: RTAB-Map is not built with dc1394 support!");
436 #endif
437  return data;
438 }
439 
440 } // namespace rtabmap
rtabmap::SensorData
Definition: SensorData.h:51
UINFO
#define UINFO(...)
rtabmap::StereoCameraModel
Definition: StereoCameraModel.h:35
UTimer::now
static double now()
Definition: UTimer.cpp:80
list
rtabmap::CameraStereoDC1394::available
static bool available()
Definition: CameraStereoDC1394.cpp:317
rtabmap::CameraStereoDC1394::isCalibrated
virtual bool isCalibrated() const
Definition: CameraStereoDC1394.cpp:379
uint32_t
::uint32_t uint32_t
this
this
UTimer.h
uint64_t
::uint64_t uint64_t
rtabmap::SensorCaptureInfo
Definition: SensorCaptureInfo.h:36
data
int data[]
UConversion.h
Some conversion functions.
rtabmap::CameraStereoDC1394::captureImage
virtual SensorData captureImage(SensorCaptureInfo *info=0)
Definition: CameraStereoDC1394.cpp:399
UASSERT
#define UASSERT(condition)
rtabmap::Camera
Definition: Camera.h:43
UWARN
#define UWARN(...)
uFormat
std::string UTILITE_EXPORT uFormat(const char *fmt,...)
Definition: UConversion.cpp:365
rtabmap_netvlad.init
def init(descriptorDim)
Definition: rtabmap_netvlad.py:30
rtabmap::Transform
Definition: Transform.h:41
uint8_t
::uint8_t uint8_t
rtabmap::CameraStereoDC1394::getSerial
virtual std::string getSerial() const
Definition: CameraStereoDC1394.cpp:388
rtabmap::CameraStereoDC1394::CameraStereoDC1394
CameraStereoDC1394(float imageRate=0.0f, const Transform &localTransform=Transform::getIdentity())
Definition: CameraStereoDC1394.cpp:326
UDEBUG
#define UDEBUG(...)
NULL
#define NULL
rtabmap::CameraStereoDC1394::~CameraStereoDC1394
virtual ~CameraStereoDC1394()
Definition: CameraStereoDC1394.cpp:338
rtabmap::CameraStereoDC1394::init
virtual bool init(const std::string &calibrationFolder=".", const std::string &cameraName="")
Definition: CameraStereoDC1394.cpp:345
CameraStereoDC1394.h
rtabmap
Definition: CameraARCore.cpp:35
UERROR
#define UERROR(...)
trace.model
model
Definition: trace.py:4
value
value
i
int i
rtabmap::SensorCapture::getNextSeqID
int getNextSeqID()
Definition: SensorCapture.h:83

rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:07