bool CameraStereoFlyCapture2::isCalibrated() const
{
#ifdef RTABMAP_FLYCAPTURE2
if(triclopsCtx_)
{
float fx, cx, cy, baseline;
triclopsGetFocalLength(triclopsCtx_, &fx);
triclopsGetImageCenter(triclopsCtx_, &cy, &cx);
triclopsGetBaseline(triclopsCtx_, &baseline);
return fx > 0.0f && cx > 0.0f && cy > 0.0f && baseline > 0.0f;
}
#endif
return false;
}
SensorData CameraStereoFlyCapture2::captureImage(CameraInfo * info)
{
SensorData data;
#ifdef RTABMAP_FLYCAPTURE2
if(camera_ && triclopsCtx_ && camera_->IsConnected())
{
// grab image from camera.
// this image contains both right and left imagesCount
FlyCapture2::Image grabbedImage;
if(camera_->RetrieveBuffer(&grabbedImage) == FlyCapture2::PGRERROR_OK)
{
// right and left image extracted from grabbed image
ImageContainer imageCont;
TriclopsColorStereoPair colorStereoInput;
generateColorStereoInput(triclopsCtx_, grabbedImage, imageCont, colorStereoInput);
// rectify images
TriclopsError triclops_status = triclopsColorRectify(triclopsCtx_, &colorStereoInput);
UASSERT_MSG(triclops_status == Fc2Triclops::ERRORTYPE_OK, uFormat("Error: %d", (int)triclops_status).c_str());
// get images
cv::Mat left,right;
TriclopsColorImage color_image;
triclops_status = triclopsGetColorImage(triclopsCtx_, TriImg_RECTIFIED_COLOR, TriCam_LEFT, &color_image);
UASSERT_MSG(triclops_status == Fc2Triclops::ERRORTYPE_OK, uFormat("Error: %d", (int)triclops_status).c_str());
cv::cvtColor(cv::Mat(color_image.nrows, color_image.ncols, CV_8UC4, color_image.data), left, CV_RGBA2RGB);
triclops_status = triclopsGetColorImage(triclopsCtx_, TriImg_RECTIFIED_COLOR, TriCam_RIGHT, &color_image);
UASSERT_MSG(triclops_status == Fc2Triclops::ERRORTYPE_OK, uFormat("Error: %d", (int)triclops_status).c_str());
cv::cvtColor(cv::Mat(color_image.nrows, color_image.ncols, CV_8UC4, color_image.data), right, CV_RGBA2GRAY);
// Set calibration stuff
float fx, cy, cx, baseline;
triclopsGetFocalLength(triclopsCtx_, &fx);
triclopsGetImageCenter(triclopsCtx_, &cy, &cx);
triclopsGetBaseline(triclopsCtx_, &baseline);
cx *= left.cols;
cy *= left.rows;
StereoCameraModel model(
fx,
fx,
cx,
cy,
baseline,
this->getLocalTransform(),
left.size());
data = SensorData(left, right, model, this->getNextSeqID(), UTimer::now());
// Compute disparity
/*triclops_status = triclopsStereo(triclopsCtx_);
UASSERT_MSG(triclops_status == Fc2Triclops::ERRORTYPE_OK, uFormat("Error: %d", (int)triclops_status).c_str());
TriclopsImage16 disparity_image;
triclops_status = triclopsGetImage16(triclopsCtx_, TriImg16_DISPARITY, TriCam_REFERENCE, &disparity_image);
UASSERT_MSG(triclops_status == Fc2Triclops::ERRORTYPE_OK, uFormat("Error: %d", (int)triclops_status).c_str());
cv::Mat depth(disparity_image.nrows, disparity_image.ncols, CV_32FC1);
int pixelinc = disparity_image.rowinc / 2;
float x, y;
for (int i = 0, k = 0; i < disparity_image.nrows; i++) {
unsigned short *row = disparity_image.data + i * pixelinc;
float *rowOut = (float *)depth.row(i).data;
for (int j = 0; j < disparity_image.ncols; j++, k++) {
unsigned short disparity = row[j];
// do not save invalid points
if (disparity < 0xFFF0) {
// convert the 16 bit disparity value to floating point x,y,z
triclopsRCD16ToXYZ(triclopsCtx_, i, j, disparity, &x, &y, &rowOut[j]);
}
}
}
CameraModel model(fx, fx, cx, cy, this->getLocalTransform(), 0, left.size());
data = SensorData(left, depth, model, this->getNextSeqID(), UTimer::now());
*/
}
}
#else
UERROR("CameraStereoFlyCapture2: RTAB-Map is not built with Triclops support!");
#endif
return data;
}
bool CameraStereoFlyCapture2::init(const std::string & calibrationFolder, const std::string & cameraName)
{
#ifdef RTABMAP_FLYCAPTURE2
if(camera_)
{
// Close the camera
camera_->StopCapture();
camera_->Disconnect();
}
if(triclopsCtx_)
{
triclopsDestroyContext(triclopsCtx_);
triclopsCtx_ = 0;
}
// connect camera
FlyCapture2::Error fc2Error = camera_->Connect();
if(fc2Error != FlyCapture2::PGRERROR_OK)
{
UERROR("Failed to connect the camera.");
return false;
}
// configure camera
Fc2Triclops::StereoCameraMode mode = Fc2Triclops::TWO_CAMERA_NARROW;
if(Fc2Triclops::setStereoMode(*camera_, mode ))
{
UERROR("Failed to set stereo mode.");
return false;
}
// generate the Triclops context
FlyCapture2::CameraInfo camInfo;
if(camera_->GetCameraInfo(&camInfo) != FlyCapture2::PGRERROR_OK)
{
UERROR("Failed to get camera info.");
return false;
}
float dummy;
unsigned packetSz;
FlyCapture2::Format7ImageSettings imageSettings;
int maxWidth = 640;
int maxHeight = 480;
if(camera_->GetFormat7Configuration(&imageSettings, &packetSz, &dummy) == FlyCapture2::PGRERROR_OK)
{
maxHeight = imageSettings.height;
maxWidth = imageSettings.width;
}
// Get calibration from th camera
if(Fc2Triclops::getContextFromCamera(camInfo.serialNumber, &triclopsCtx_))
{
UERROR("Failed to get calibration from the camera.");
return false;
}
triclopsSetResolution(triclopsCtx_, maxHeight, maxWidth);
if (triclopsPrepareRectificationData(triclopsCtx_,
maxHeight,
maxWidth,
maxHeight,
maxWidth))
{
UERROR("Failed to prepare rectification matrices.");
return false;
}
triclopsSetCameraConfiguration(triclopsCtx_, TriCfg_2CAM_HORIZONTAL_NARROW);
float fx, cx, cy, baseline;
triclopsGetFocalLength(triclopsCtx_, &fx);
triclopsGetImageCenter(triclopsCtx_, &cy, &cx);
cx *= maxWidth;
cy *= maxHeight;
triclopsGetBaseline(triclopsCtx_, &baseline);
UINFO("Stereo parameters: fx=%f cx=%f cy=%f baseline=%f %dx%d", fx, cx, cy, baseline, maxWidth, maxHeight);
if(camera_->StartCapture() != FlyCapture2::PGRERROR_OK)
{
UERROR("Failed to start capture.");
return false;
}
return true;
#else
UERROR("CameraStereoFlyCapture2: RTAB-Map is not built with Triclops support!");
#endif
return false;
}
int
main( int /* argc */, char** /* argv */ )
{
TriclopsError te;
FlyCaptureError fe;
TriclopsContext triclops;
FlyCaptureContext flycapture;
float baseline;
int nrows;
int ncols;
float focalLength;
TriclopsCameraConfiguration triclopsConfig;
// Create the Flycapture context
fe = flycaptureCreateContext( &flycapture );
_HANDLE_FLYCAPTURE_ERROR( "flycaptureCreateContext()", fe );
// Initialize the camera
fe = flycaptureInitialize( flycapture, 0 );
_HANDLE_FLYCAPTURE_ERROR( "flycaptureInitialize()", fe );
// Get the camera configuration
char* szCalFile;
fe = flycaptureGetCalibrationFileFromCamera( flycapture, &szCalFile );
_HANDLE_FLYCAPTURE_ERROR( "flycaptureGetCalibrationFileFromCamera()", fe );
// Create a Triclops context from the cameras calibration file
te = triclopsGetDefaultContextFromFile( &triclops, szCalFile );
_HANDLE_TRICLOPS_ERROR( "triclopsGetDefaultContextFromFile()", te );
// Destroy the Flycapture context - this program just displays information contained
// in the TriclopsContext
flycaptureDestroyContext( flycapture );
printf( "Triclops Version : %s\n", triclopsVersion() );
// get the camera configuration
te = triclopsGetCameraConfiguration( triclops, &triclopsConfig );
_HANDLE_TRICLOPS_ERROR( "triclopsGetCameraConfiguration()", te );
switch( triclopsConfig )
{
case TriCfg_L:
printf( "Configuration : 3 Camera\n" );
break;
case TriCfg_2CAM_HORIZONTAL:
printf( "Configuration : 2 Camera horizontal\n" );
break;
case TriCfg_2CAM_VERTICAL:
printf( "Configuration : 2 Camera vertical\n" );
break;
default:
printf( "Unrecognized configuration: %d\n", triclopsConfig );
}
// Get the baseline
te = triclopsGetBaseline( triclops, &baseline );
_HANDLE_TRICLOPS_ERROR( "triclopsGetBaseline()", te );
printf( "Baseline : %f cm\n", baseline*100.0 );
// Get the focal length
te = triclopsGetFocalLength( triclops, &focalLength );
_HANDLE_TRICLOPS_ERROR( "triclopsGetFocalLength()", te );
te = triclopsGetResolution( triclops, &nrows, &ncols );
_HANDLE_TRICLOPS_ERROR( "triclopsGetResolution()", te );
printf( "Focal Length : %f pixels for a %d x %d image\n",
focalLength,
ncols,
nrows ) ;
int nRows, nCols;
te = triclopsGetResolution( triclops, &nRows, &nCols );
_HANDLE_TRICLOPS_ERROR( "triclopsGetResolution()", te );
float fCenterRow, fCenterCol;
te = triclopsGetImageCenter( triclops, &fCenterRow, &fCenterCol );
_HANDLE_TRICLOPS_ERROR( "triclopsGetImageCenter()", te );
printf( "The default image resolution for stereo processing is %d x %d\n",
nCols, nRows );
printf( "For this resolution, the 'image center' or 'principal point' "
"is:\n" );
printf( "Center Row = %f\n", fCenterRow );
printf( "Center Col = %f\n", fCenterCol );
// Destroy the Triclops context
triclopsDestroyContext( triclops ) ;
return 0;
}
