CameraStereoFlyCapture2.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/CameraStereoFlyCapture2.h>
29 #include <rtabmap/utilite/UTimer.h>
30 #include <rtabmap/utilite/UConversion.h>
31 
32 #ifdef RTABMAP_FLYCAPTURE2
33 #include <triclopsrectify.h>
34 #include <triclops.h>
35 #include <fc2triclops.h>
36 #endif
37 
38 namespace rtabmap
39 {
40 
41 CameraStereoFlyCapture2::CameraStereoFlyCapture2(float imageRate, const Transform & localTransform) :
42  Camera(imageRate, localTransform)
43 #ifdef RTABMAP_FLYCAPTURE2
44  ,
45  camera_(0),
46  triclopsCtx_(0)
47 #endif
48 {
49 #ifdef RTABMAP_FLYCAPTURE2
50  camera_ = new FlyCapture2::Camera();
51 #endif
52 }
53 
54 CameraStereoFlyCapture2::~CameraStereoFlyCapture2()
55 {
56 #ifdef RTABMAP_FLYCAPTURE2
57  // Close the camera
58  camera_->StopCapture();
59  camera_->Disconnect();
60 
61  // Destroy the Triclops context
62  triclopsDestroyContext( triclopsCtx_ ) ;
63 
64  delete camera_;
65 #endif
66 }
67 
68 bool CameraStereoFlyCapture2::available()
69 {
70 #ifdef RTABMAP_FLYCAPTURE2
71  return true;
72 #else
73  return false;
74 #endif
75 }
76 
77 #ifdef RTABMAP_FLYCAPTURE2
78 
79 // struct containing image needed for processing
80 struct ImageContainer
81 {
82  FlyCapture2::Image tmp[2];
83  FlyCapture2::Image unprocessed[2];
84 };
85 
86 // generate color stereo input
87 void generateColorStereoInput(TriclopsContext const &context,
88  FlyCapture2::Image const &grabbedImage,
89  ImageContainer &imageCont,
90  TriclopsColorStereoPair &stereoPair)
91 {
92  Fc2Triclops::ErrorType fc2TriclopsError;
93  TriclopsError te;
94 
95  TriclopsColorImage triclopsImageContainer[2];
96  FlyCapture2::Image *tmpImage = imageCont.tmp;
97  FlyCapture2::Image *unprocessedImage = imageCont.unprocessed;
98 
99  // Convert the pixel interleaved raw data to de-interleaved and color processed data
100  fc2TriclopsError = Fc2Triclops::unpackUnprocessedRawOrMono16Image(
101  grabbedImage,
102  true /*assume little endian*/,
103  tmpImage[0],
104  tmpImage[1]);
105 
106  UASSERT_MSG(fc2TriclopsError == Fc2Triclops::ERRORTYPE_OK, uFormat("Error: %d", (int)fc2TriclopsError).c_str());
107 
108  // preprocess color image
109  for (int i = 0; i < 2; ++i) {
110  FlyCapture2::Error fc2Error;
111  fc2Error = tmpImage[i].SetColorProcessing(FlyCapture2::HQ_LINEAR);
112  UASSERT_MSG(fc2Error == FlyCapture2::PGRERROR_OK, fc2Error.GetDescription());
113 
114  // convert preprocessed color image to BGRU format
115  fc2Error = tmpImage[i].Convert(FlyCapture2::PIXEL_FORMAT_BGRU,
116  &unprocessedImage[i]);
117  UASSERT_MSG(fc2Error == FlyCapture2::PGRERROR_OK, fc2Error.GetDescription());
118  }
119 
120  // create triclops image for right and left lens
121  for (size_t i = 0; i < 2; ++i) {
122  TriclopsColorImage *image = &triclopsImageContainer[i];
123  te = triclopsLoadColorImageFromBuffer(
124  reinterpret_cast<TriclopsColorPixel *>(unprocessedImage[i].GetData()),
125  unprocessedImage[i].GetRows(),
126  unprocessedImage[i].GetCols(),
127  unprocessedImage[i].GetStride(),
128  image);
129  UASSERT_MSG(te == Fc2Triclops::ERRORTYPE_OK, uFormat("Error: %d", (int)te).c_str());
130  }
131 
132  // create stereo input from the triclops images constructed above
133  // pack image data into a TriclopsColorStereoPair structure
134  te = triclopsBuildColorStereoPairFromBuffers(
135  context,
136  &triclopsImageContainer[1],
137  &triclopsImageContainer[0],
138  &stereoPair);
139  UASSERT_MSG(te == Fc2Triclops::ERRORTYPE_OK, uFormat("Error: %d", (int)te).c_str());
140 }
141 #endif
142 
143 bool CameraStereoFlyCapture2::init(const std::string & calibrationFolder, const std::string & cameraName)
144 {
145 #ifdef RTABMAP_FLYCAPTURE2
146  if(camera_)
147  {
148  // Close the camera
149  camera_->StopCapture();
150  camera_->Disconnect();
151  }
152  if(triclopsCtx_)
153  {
154  triclopsDestroyContext(triclopsCtx_);
155  triclopsCtx_ = 0;
156  }
157 
158  // connect camera
159  FlyCapture2::Error fc2Error = camera_->Connect();
160  if(fc2Error != FlyCapture2::PGRERROR_OK)
161  {
162  UERROR("Failed to connect the camera.");
163  return false;
164  }
165 
166  // configure camera
167  Fc2Triclops::StereoCameraMode mode = Fc2Triclops::TWO_CAMERA_NARROW;
168  if(Fc2Triclops::setStereoMode(*camera_, mode ))
169  {
170  UERROR("Failed to set stereo mode.");
171  return false;
172  }
173 
174  // generate the Triclops context
175  FlyCapture2::CameraInfo camInfo;
176  if(camera_->GetCameraInfo(&camInfo) != FlyCapture2::PGRERROR_OK)
177  {
178  UERROR("Failed to get camera info.");
179  return false;
180  }
181 
182  float dummy;
183  unsigned packetSz;
184  FlyCapture2::Format7ImageSettings imageSettings;
185  int maxWidth = 640;
186  int maxHeight = 480;
187  if(camera_->GetFormat7Configuration(&imageSettings, &packetSz, &dummy) == FlyCapture2::PGRERROR_OK)
188  {
189  maxHeight = imageSettings.height;
190  maxWidth = imageSettings.width;
191  }
192 
193  // Get calibration from th camera
194  if(Fc2Triclops::getContextFromCamera(camInfo.serialNumber, &triclopsCtx_))
195  {
196  UERROR("Failed to get calibration from the camera.");
197  return false;
198  }
199 
200  triclopsSetResolution(triclopsCtx_, maxHeight, maxWidth);
201  if (triclopsPrepareRectificationData(triclopsCtx_,
202  maxHeight,
203  maxWidth,
204  maxHeight,
205  maxWidth))
206  {
207  UERROR("Failed to prepare rectification matrices.");
208  return false;
209  }
210  triclopsSetCameraConfiguration(triclopsCtx_, TriCfg_2CAM_HORIZONTAL_NARROW);
211 
212  float fx, cx, cy, baseline;
213  triclopsGetFocalLength(triclopsCtx_, &fx);
214  triclopsGetImageCenter(triclopsCtx_, &cy, &cx);
215  cx *= maxWidth;
216  cy *= maxHeight;
217  triclopsGetBaseline(triclopsCtx_, &baseline);
218  UINFO("Stereo parameters: fx=%f cx=%f cy=%f baseline=%f %dx%d", fx, cx, cy, baseline, maxWidth, maxHeight);
219 
220  if(camera_->StartCapture() != FlyCapture2::PGRERROR_OK)
221  {
222  UERROR("Failed to start capture.");
223  return false;
224  }
225 
226  return true;
227 #else
228  UERROR("CameraStereoFlyCapture2: RTAB-Map is not built with Triclops support!");
229 #endif
230  return false;
231 }
232 
233 bool CameraStereoFlyCapture2::isCalibrated() const
234 {
235 #ifdef RTABMAP_FLYCAPTURE2
236  if(triclopsCtx_)
237  {
238  float fx, cx, cy, baseline;
239  triclopsGetFocalLength(triclopsCtx_, &fx);
240  triclopsGetImageCenter(triclopsCtx_, &cy, &cx);
241  triclopsGetBaseline(triclopsCtx_, &baseline);
242  return fx > 0.0f && cx > 0.0f && cy > 0.0f && baseline > 0.0f;
243  }
244 #endif
245  return false;
246 }
247 
248 std::string CameraStereoFlyCapture2::getSerial() const
249 {
250 #ifdef RTABMAP_FLYCAPTURE2
251  if(camera_ && camera_->IsConnected())
252  {
253  FlyCapture2::CameraInfo camInfo;
254  if(camera_->GetCameraInfo(&camInfo) == FlyCapture2::PGRERROR_OK)
255  {
256  return uNumber2Str(camInfo.serialNumber);
257  }
258  }
259 #endif
260  return "";
261 }
262 
263 SensorData CameraStereoFlyCapture2::captureImage(CameraInfo * info)
264 {
265  SensorData data;
266 #ifdef RTABMAP_FLYCAPTURE2
267  if(camera_ && triclopsCtx_ && camera_->IsConnected())
268  {
269  // grab image from camera.
270  // this image contains both right and left imagesCount
271  FlyCapture2::Image grabbedImage;
272  if(camera_->RetrieveBuffer(&grabbedImage) == FlyCapture2::PGRERROR_OK)
273  {
274  // right and left image extracted from grabbed image
275  ImageContainer imageCont;
276 
277  TriclopsColorStereoPair colorStereoInput;
278  generateColorStereoInput(triclopsCtx_, grabbedImage, imageCont, colorStereoInput);
279 
280  // rectify images
281  TriclopsError triclops_status = triclopsColorRectify(triclopsCtx_, &colorStereoInput);
282  UASSERT_MSG(triclops_status == Fc2Triclops::ERRORTYPE_OK, uFormat("Error: %d", (int)triclops_status).c_str());
283 
284  // get images
285  cv::Mat left,right;
286  TriclopsColorImage color_image;
287  triclops_status = triclopsGetColorImage(triclopsCtx_, TriImg_RECTIFIED_COLOR, TriCam_LEFT, &color_image);
288  UASSERT_MSG(triclops_status == Fc2Triclops::ERRORTYPE_OK, uFormat("Error: %d", (int)triclops_status).c_str());
289  cv::cvtColor(cv::Mat(color_image.nrows, color_image.ncols, CV_8UC4, color_image.data), left, CV_RGBA2RGB);
290  triclops_status = triclopsGetColorImage(triclopsCtx_, TriImg_RECTIFIED_COLOR, TriCam_RIGHT, &color_image);
291  UASSERT_MSG(triclops_status == Fc2Triclops::ERRORTYPE_OK, uFormat("Error: %d", (int)triclops_status).c_str());
292  cv::cvtColor(cv::Mat(color_image.nrows, color_image.ncols, CV_8UC4, color_image.data), right, CV_RGBA2GRAY);
293 
294  // Set calibration stuff
295  float fx, cy, cx, baseline;
296  triclopsGetFocalLength(triclopsCtx_, &fx);
297  triclopsGetImageCenter(triclopsCtx_, &cy, &cx);
298  triclopsGetBaseline(triclopsCtx_, &baseline);
299  cx *= left.cols;
300  cy *= left.rows;
301 
302  StereoCameraModel model(
303  fx,
304  fx,
305  cx,
306  cy,
307  baseline,
308  this->getLocalTransform(),
309  left.size());
310  data = SensorData(left, right, model, this->getNextSeqID(), UTimer::now());
311 
312  // Compute disparity
313  /*triclops_status = triclopsStereo(triclopsCtx_);
314  UASSERT_MSG(triclops_status == Fc2Triclops::ERRORTYPE_OK, uFormat("Error: %d", (int)triclops_status).c_str());
315 
316  TriclopsImage16 disparity_image;
317  triclops_status = triclopsGetImage16(triclopsCtx_, TriImg16_DISPARITY, TriCam_REFERENCE, &disparity_image);
318  UASSERT_MSG(triclops_status == Fc2Triclops::ERRORTYPE_OK, uFormat("Error: %d", (int)triclops_status).c_str());
319  cv::Mat depth(disparity_image.nrows, disparity_image.ncols, CV_32FC1);
320  int pixelinc = disparity_image.rowinc / 2;
321  float x, y;
322  for (int i = 0, k = 0; i < disparity_image.nrows; i++) {
323  unsigned short *row = disparity_image.data + i * pixelinc;
324  float *rowOut = (float *)depth.row(i).data;
325  for (int j = 0; j < disparity_image.ncols; j++, k++) {
326  unsigned short disparity = row[j];
327 
328  // do not save invalid points
329  if (disparity < 0xFFF0) {
330  // convert the 16 bit disparity value to floating point x,y,z
331  triclopsRCD16ToXYZ(triclopsCtx_, i, j, disparity, &x, &y, &rowOut[j]);
332  }
333  }
334  }
335  CameraModel model(fx, fx, cx, cy, this->getLocalTransform(), 0, left.size());
336  data = SensorData(left, depth, model, this->getNextSeqID(), UTimer::now());
337  */
338  }
339  }
340 
341 #else
342  UERROR("CameraStereoFlyCapture2: RTAB-Map is not built with Triclops support!");
343 #endif
344  return data;
345 }
346 
347 } // namespace rtabmap
rtabmap::Camera::getLocalTransform
const Transform & getLocalTransform() const
Definition: Camera.h:64
rtabmap::CameraStereoFlyCapture2::captureImage
virtual SensorData captureImage(CameraInfo *info=0)
Definition: CameraStereoFlyCapture2.cpp:263
UConversion.h
Some conversion functions.
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
rtabmap::CameraStereoFlyCapture2::getSerial
virtual std::string getSerial() const
Definition: CameraStereoFlyCapture2.cpp:248
rtabmap::Camera::getNextSeqID
int getNextSeqID()
Definition: Camera.h:84
rtabmap::CameraStereoFlyCapture2::CameraStereoFlyCapture2
CameraStereoFlyCapture2(float imageRate=0.0f, const Transform &localTransform=CameraModel::opticalRotation())
Definition: CameraStereoFlyCapture2.cpp:41
UERROR
#define UERROR(...)
UTimer::now
static double now()
Definition: UTimer.cpp:80
rtabmap::CameraStereoFlyCapture2::init
virtual bool init(const std::string &calibrationFolder=".", const std::string &cameraName="")
Definition: CameraStereoFlyCapture2.cpp:143
rtabmap::Transform::size
int size() const
Definition: Transform.h:90
uFormat
std::string UTILITE_EXP uFormat(const char *fmt,...)
Definition: UConversion.cpp:350
uNumber2Str
std::string UTILITE_EXP uNumber2Str(unsigned int number)
Definition: UConversion.cpp:90
UINFO
#define UINFO(...)

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:34:58