mirror of
https://github.com/introlab/rtabmap.git
synced 2026-09-02 01:20:25 +08:00
Version 0.8.9: added FlyCapture2 driver for bumblebee2 (tested only on Windows)
This commit is contained in:
Mathieu Labbé
@@ -108,6 +108,18 @@ IF(DC1394_FOUND)
|
||||
)
|
||||
ENDIF(DC1394_FOUND)
|
||||
|
||||
IF(FlyCapture2_FOUND)
|
||||
ADD_DEFINITIONS("-DWITH_FLYCAPTURE2")
|
||||
SET(INCLUDE_DIRS
|
||||
${INCLUDE_DIRS}
|
||||
${FlyCapture2_INCLUDE_DIRS}
|
||||
)
|
||||
SET(LIBRARIES
|
||||
${LIBRARIES}
|
||||
${FlyCapture2_LIBRARIES}
|
||||
)
|
||||
ENDIF(FlyCapture2_FOUND)
|
||||
|
||||
IF(G2O_FOUND)
|
||||
ADD_DEFINITIONS("-DWITH_G2O")
|
||||
SET(INCLUDE_DIRS
|
||||
|
||||
@@ -63,6 +63,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
#include <OpenNI.h>
|
||||
#endif
|
||||
|
||||
#ifdef WITH_FLYCAPTURE2
|
||||
#include <triclops.h>
|
||||
#include <fc2triclops.h>
|
||||
#endif
|
||||
|
||||
namespace rtabmap
|
||||
{
|
||||
|
||||
@@ -639,10 +644,12 @@ bool CameraOpenNI2::isCalibrated() const
|
||||
|
||||
std::string CameraOpenNI2::getSerial() const
|
||||
{
|
||||
#ifdef WITH_OPENNI2
|
||||
if(_device)
|
||||
{
|
||||
return _device->getDeviceInfo().getName();
|
||||
}
|
||||
#endif
|
||||
return "";
|
||||
}
|
||||
|
||||
@@ -962,7 +969,10 @@ bool CameraFreenect::init(const std::string & calibrationFolder)
|
||||
|
||||
bool CameraFreenect::isCalibrated() const
|
||||
{
|
||||
#ifdef WITH_FREENECT
|
||||
return freenectDevice_ != 0 && freenectDevice_->getDepthFocal() > 0.0f;
|
||||
#endif
|
||||
return false;
|
||||
}
|
||||
|
||||
std::string CameraFreenect::getSerial() const
|
||||
@@ -1162,7 +1172,7 @@ void CameraFreenect2::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, f
|
||||
}
|
||||
|
||||
//
|
||||
// CameraDC1394
|
||||
// CameraStereoDC1394
|
||||
// Inspired from ROS camera1394stereo package
|
||||
//
|
||||
|
||||
@@ -1487,6 +1497,14 @@ bool CameraStereoDC1394::init(const std::string & calibrationFolder)
|
||||
UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!",
|
||||
device_->guid().c_str(), calibrationFolder.c_str());
|
||||
}
|
||||
else
|
||||
{
|
||||
UINFO("Stereo parameters: fx=%f cx=%f cy=%f baseline=%f",
|
||||
stereoModel_.left().fx(),
|
||||
stereoModel_.left().cx(),
|
||||
stereoModel_.left().cy(),
|
||||
stereoModel_.baseline());
|
||||
}
|
||||
}
|
||||
}
|
||||
return ok;
|
||||
@@ -1539,4 +1557,276 @@ void CameraStereoDC1394::captureImage(cv::Mat & left, cv::Mat & right, float & f
|
||||
#endif
|
||||
}
|
||||
|
||||
//
|
||||
// CameraTriclops
|
||||
//
|
||||
CameraStereoFlyCapture2::CameraStereoFlyCapture2(float imageRate, const Transform & localTransform) :
|
||||
CameraRGBD(imageRate, localTransform),
|
||||
camera_(0),
|
||||
triclopsCtx_(0)
|
||||
{
|
||||
#ifdef WITH_FLYCAPTURE2
|
||||
camera_ = new FlyCapture2::Camera();
|
||||
#endif
|
||||
}
|
||||
|
||||
CameraStereoFlyCapture2::~CameraStereoFlyCapture2()
|
||||
{
|
||||
#ifdef WITH_FLYCAPTURE2
|
||||
// Close the camera
|
||||
camera_->StopCapture();
|
||||
camera_->Disconnect();
|
||||
|
||||
// Destroy the Triclops context
|
||||
triclopsDestroyContext( triclopsCtx_ ) ;
|
||||
|
||||
delete camera_;
|
||||
#endif
|
||||
}
|
||||
|
||||
bool CameraStereoFlyCapture2::available()
|
||||
{
|
||||
#ifdef WITH_FLYCAPTURE2
|
||||
return true;
|
||||
#else
|
||||
return false;
|
||||
#endif
|
||||
}
|
||||
|
||||
bool CameraStereoFlyCapture2::init(const std::string & calibrationFolder)
|
||||
{
|
||||
#ifdef WITH_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;
|
||||
}
|
||||
|
||||
float fx, cx, cy, baseline;
|
||||
triclopsGetFocalLength(triclopsCtx_, &fx);
|
||||
triclopsGetImageCenter(triclopsCtx_, &cy, &cx);
|
||||
triclopsGetBaseline(triclopsCtx_, &baseline);
|
||||
UINFO("Stereo parameters: fx=%f cx=%f cy=%f baseline=%f", fx, cx, cy, baseline);
|
||||
|
||||
triclopsSetCameraConfiguration(triclopsCtx_, TriCfg_2CAM_HORIZONTAL_NARROW );
|
||||
UASSERT(triclopsSetResolutionAndPrepare(triclopsCtx_, maxHeight, maxWidth, maxHeight, maxWidth) == Fc2Triclops::ERRORTYPE_OK);
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
bool CameraStereoFlyCapture2::isCalibrated() const
|
||||
{
|
||||
#ifdef WITH_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;
|
||||
}
|
||||
|
||||
std::string CameraStereoFlyCapture2::getSerial() const
|
||||
{
|
||||
#ifdef WITH_FLYCAPTURE2
|
||||
if(camera_ && camera_->IsConnected())
|
||||
{
|
||||
FlyCapture2::CameraInfo camInfo;
|
||||
if(camera_->GetCameraInfo(&camInfo) == FlyCapture2::PGRERROR_OK)
|
||||
{
|
||||
return uNumber2Str(camInfo.serialNumber);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
return "";
|
||||
}
|
||||
|
||||
// struct containing image needed for processing
|
||||
#ifdef WITH_FLYCAPTURE2
|
||||
struct ImageContainer
|
||||
{
|
||||
FlyCapture2::Image tmp[2];
|
||||
FlyCapture2::Image unprocessed[2];
|
||||
} ;
|
||||
#endif
|
||||
|
||||
void CameraStereoFlyCapture2::captureImage(cv::Mat & left, cv::Mat & right, float & fx, float & baseline, float & cx, float & cy)
|
||||
{
|
||||
#ifdef WITH_FLYCAPTURE2
|
||||
left = cv::Mat();
|
||||
right = cv::Mat();
|
||||
fx = 0.0f;
|
||||
baseline = 0.0f;
|
||||
cx = 0.0f;
|
||||
cy = 0.0f;
|
||||
|
||||
if(camera_ && triclopsCtx_ && camera_->IsConnected())
|
||||
{
|
||||
// grab image from camera.
|
||||
// this image contains both right and left images
|
||||
FlyCapture2::Image grabbedImage;
|
||||
if(camera_->RetrieveBuffer(&grabbedImage) == FlyCapture2::PGRERROR_OK)
|
||||
{
|
||||
// right and left image extracted from grabbed image
|
||||
ImageContainer imageCont;
|
||||
|
||||
// generate triclops input from grabbed image
|
||||
FlyCapture2::Image imageRawRight;
|
||||
FlyCapture2::Image imageRawLeft;
|
||||
FlyCapture2::Image * unprocessedImage = imageCont.unprocessed;
|
||||
|
||||
// Convert the pixel interleaved raw data to de-interleaved and color processed data
|
||||
if(Fc2Triclops::unpackUnprocessedRawOrMono16Image(
|
||||
grabbedImage,
|
||||
true /*assume little endian*/,
|
||||
imageRawLeft /* right */,
|
||||
imageRawRight /* left */) == Fc2Triclops::ERRORTYPE_OK)
|
||||
{
|
||||
// convert to color
|
||||
FlyCapture2::Image srcImgRightRef(imageRawRight);
|
||||
FlyCapture2::Image srcImgLeftRef(imageRawLeft);
|
||||
|
||||
bool ok = true;;
|
||||
if ( srcImgRightRef.SetColorProcessing(FlyCapture2::HQ_LINEAR) != FlyCapture2::PGRERROR_OK ||
|
||||
srcImgLeftRef.SetColorProcessing(FlyCapture2::HQ_LINEAR) != FlyCapture2::PGRERROR_OK)
|
||||
{
|
||||
ok = false;
|
||||
}
|
||||
|
||||
if(ok)
|
||||
{
|
||||
FlyCapture2::Image imageColorRight;
|
||||
FlyCapture2::Image imageColorLeft;
|
||||
if ( srcImgRightRef.Convert(FlyCapture2::PIXEL_FORMAT_MONO8, &imageColorRight) != FlyCapture2::PGRERROR_OK ||
|
||||
srcImgLeftRef.Convert(FlyCapture2::PIXEL_FORMAT_BGRU, &imageColorLeft) != FlyCapture2::PGRERROR_OK)
|
||||
{
|
||||
ok = false;
|
||||
}
|
||||
|
||||
if(ok)
|
||||
{
|
||||
//RECTIFY RIGHT
|
||||
TriclopsInput triclopsColorInputs;
|
||||
triclopsBuildRGBTriclopsInput(
|
||||
grabbedImage.GetCols(),
|
||||
grabbedImage.GetRows(),
|
||||
imageColorRight.GetStride(),
|
||||
(unsigned long)grabbedImage.GetTimeStamp().seconds,
|
||||
(unsigned long)grabbedImage.GetTimeStamp().microSeconds,
|
||||
imageColorRight.GetData(),
|
||||
imageColorRight.GetData(),
|
||||
imageColorRight.GetData(),
|
||||
&triclopsColorInputs);
|
||||
|
||||
triclopsRectify(triclopsCtx_, const_cast<TriclopsInput *>(&triclopsColorInputs) );
|
||||
// Retrieve the rectified image from the triclops context
|
||||
TriclopsImage rectifiedImage;
|
||||
triclopsGetImage( triclopsCtx_,
|
||||
TriImg_RECTIFIED,
|
||||
TriCam_REFERENCE,
|
||||
&rectifiedImage );
|
||||
|
||||
right = cv::Mat(rectifiedImage.nrows, rectifiedImage.ncols, CV_8UC1, rectifiedImage.data).clone();
|
||||
|
||||
//RECTIFY LEFT COLOR
|
||||
triclopsBuildPackedTriclopsInput(
|
||||
grabbedImage.GetCols(),
|
||||
grabbedImage.GetRows(),
|
||||
imageColorLeft.GetStride(),
|
||||
(unsigned long)grabbedImage.GetTimeStamp().seconds,
|
||||
(unsigned long)grabbedImage.GetTimeStamp().microSeconds,
|
||||
imageColorLeft.GetData(),
|
||||
&triclopsColorInputs );
|
||||
|
||||
cv::Mat pixelsLeftBuffer( grabbedImage.GetRows(), grabbedImage.GetCols(), CV_8UC4);
|
||||
TriclopsPackedColorImage colorImage;
|
||||
triclopsSetPackedColorImageBuffer(
|
||||
triclopsCtx_,
|
||||
TriCam_LEFT,
|
||||
(TriclopsPackedColorPixel*)pixelsLeftBuffer.data );
|
||||
|
||||
triclopsRectifyPackedColorImage(
|
||||
triclopsCtx_,
|
||||
TriCam_LEFT,
|
||||
&triclopsColorInputs,
|
||||
&colorImage );
|
||||
|
||||
cv::cvtColor(pixelsLeftBuffer, left, CV_RGBA2RGB);
|
||||
|
||||
// Set calibration stuff
|
||||
triclopsGetFocalLength(triclopsCtx_, &fx);
|
||||
triclopsGetImageCenter(triclopsCtx_, &cy, &cx);
|
||||
triclopsGetBaseline(triclopsCtx_, &baseline);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#else
|
||||
UERROR("CameraStereoFlyCapture2: RTAB-Map is not built with Triclops support!");
|
||||
#endif
|
||||
}
|
||||
|
||||
} // namespace rtabmap
|
||||
|
||||
Reference in New Issue
Block a user