32 #ifdef RTABMAP_FLYCAPTURE2
33 #include <triclopsrectify.h>
35 #include <fc2triclops.h>
42 Camera(imageRate, localTransform)
43 #ifdef RTABMAP_FLYCAPTURE2
49 #ifdef RTABMAP_FLYCAPTURE2
50 camera_ =
new FlyCapture2::Camera();
56 #ifdef RTABMAP_FLYCAPTURE2
58 camera_->StopCapture();
59 camera_->Disconnect();
62 triclopsDestroyContext( triclopsCtx_ ) ;
70 #ifdef RTABMAP_FLYCAPTURE2
77 #ifdef RTABMAP_FLYCAPTURE2
82 FlyCapture2::Image tmp[2];
83 FlyCapture2::Image unprocessed[2];
87 void generateColorStereoInput(TriclopsContext
const &context,
88 FlyCapture2::Image
const &grabbedImage,
89 ImageContainer &imageCont,
90 TriclopsColorStereoPair &stereoPair)
92 Fc2Triclops::ErrorType fc2TriclopsError;
95 TriclopsColorImage triclopsImageContainer[2];
96 FlyCapture2::Image *tmpImage = imageCont.tmp;
97 FlyCapture2::Image *unprocessedImage = imageCont.unprocessed;
100 fc2TriclopsError = Fc2Triclops::unpackUnprocessedRawOrMono16Image(
106 UASSERT_MSG(fc2TriclopsError == Fc2Triclops::ERRORTYPE_OK,
uFormat(
"Error: %d", (
int)fc2TriclopsError).
c_str());
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());
115 fc2Error = tmpImage[
i].Convert(FlyCapture2::PIXEL_FORMAT_BGRU,
116 &unprocessedImage[i]);
117 UASSERT_MSG(fc2Error == FlyCapture2::PGRERROR_OK, fc2Error.GetDescription());
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(),
134 te = triclopsBuildColorStereoPairFromBuffers(
136 &triclopsImageContainer[1],
137 &triclopsImageContainer[0],
145 #ifdef RTABMAP_FLYCAPTURE2
149 camera_->StopCapture();
150 camera_->Disconnect();
154 triclopsDestroyContext(triclopsCtx_);
159 FlyCapture2::Error fc2Error = camera_->Connect();
160 if(fc2Error != FlyCapture2::PGRERROR_OK)
162 UERROR(
"Failed to connect the camera.");
167 Fc2Triclops::StereoCameraMode
mode = Fc2Triclops::TWO_CAMERA_NARROW;
168 if(Fc2Triclops::setStereoMode(*camera_,
mode ))
170 UERROR(
"Failed to set stereo mode.");
176 if(camera_->GetCameraInfo(&camInfo) != FlyCapture2::PGRERROR_OK)
178 UERROR(
"Failed to get camera info.");
184 FlyCapture2::Format7ImageSettings imageSettings;
187 if(camera_->GetFormat7Configuration(&imageSettings, &packetSz, &
dummy) == FlyCapture2::PGRERROR_OK)
189 maxHeight = imageSettings.height;
190 maxWidth = imageSettings.width;
194 if(Fc2Triclops::getContextFromCamera(camInfo.serialNumber, &triclopsCtx_))
196 UERROR(
"Failed to get calibration from the camera.");
200 triclopsSetResolution(triclopsCtx_, maxHeight, maxWidth);
201 if (triclopsPrepareRectificationData(triclopsCtx_,
207 UERROR(
"Failed to prepare rectification matrices.");
210 triclopsSetCameraConfiguration(triclopsCtx_, TriCfg_2CAM_HORIZONTAL_NARROW);
213 triclopsGetFocalLength(triclopsCtx_, &
fx);
214 triclopsGetImageCenter(triclopsCtx_, &
cy, &
cx);
217 triclopsGetBaseline(triclopsCtx_, &
baseline);
218 UINFO(
"Stereo parameters: fx=%f cx=%f cy=%f baseline=%f %dx%d",
fx,
cx,
cy,
baseline, maxWidth, maxHeight);
220 if(camera_->StartCapture() != FlyCapture2::PGRERROR_OK)
222 UERROR(
"Failed to start capture.");
228 UERROR(
"CameraStereoFlyCapture2: RTAB-Map is not built with Triclops support!");
235 #ifdef RTABMAP_FLYCAPTURE2
239 triclopsGetFocalLength(triclopsCtx_, &
fx);
240 triclopsGetImageCenter(triclopsCtx_, &
cy, &
cx);
241 triclopsGetBaseline(triclopsCtx_, &
baseline);
250 #ifdef RTABMAP_FLYCAPTURE2
251 if(camera_ && camera_->IsConnected())
254 if(camera_->GetCameraInfo(&camInfo) == FlyCapture2::PGRERROR_OK)
266 #ifdef RTABMAP_FLYCAPTURE2
267 if(camera_ && triclopsCtx_ && camera_->IsConnected())
271 FlyCapture2::Image grabbedImage;
272 if(camera_->RetrieveBuffer(&grabbedImage) == FlyCapture2::PGRERROR_OK)
275 ImageContainer imageCont;
277 TriclopsColorStereoPair colorStereoInput;
278 generateColorStereoInput(triclopsCtx_, grabbedImage, imageCont, colorStereoInput);
281 TriclopsError triclops_status = triclopsColorRectify(triclopsCtx_, &colorStereoInput);
286 TriclopsColorImage color_image;
287 triclops_status = triclopsGetColorImage(triclopsCtx_, TriImg_RECTIFIED_COLOR, TriCam_LEFT, &color_image);
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);
292 cv::cvtColor(cv::Mat(color_image.nrows, color_image.ncols, CV_8UC4, color_image.data), right, CV_RGBA2GRAY);
296 triclopsGetFocalLength(triclopsCtx_, &
fx);
297 triclopsGetImageCenter(triclopsCtx_, &
cy, &
cx);
298 triclopsGetBaseline(triclopsCtx_, &
baseline);
342 UERROR(
"CameraStereoFlyCapture2: RTAB-Map is not built with Triclops support!");