i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-03 10:00:23 +08:00
Code Issues Packages Projects Releases Wiki Activity

Updated CameraStereoFlyCapture2 with triclops new API

Browse Source
This commit is contained in:
Mathieu Labbé
2018-11-01 17:17:38 -04:00
parent 4e3e5872fc
commit 961549d375
1 changed files with 138 additions and 110 deletions
Download Patch File Download Diff File Expand all files Collapse all files

248
corelib/src/camera/CameraStereoFlyCapture2.cpp
View File

@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UConversion.h> #include <rtabmap/utilite/UConversion.h>
#ifdef RTABMAP_FLYCAPTURE2 #ifdef RTABMAP_FLYCAPTURE2
#include <triclopsrectify.h>
#include <triclops.h> #include <triclops.h>
#include <fc2triclops.h> #include <fc2triclops.h>
#endif #endif
@@ -73,6 +74,72 @@ bool CameraStereoFlyCapture2::available()
#endif #endif
} }
#ifdef RTABMAP_FLYCAPTURE2
// struct containing image needed for processing
struct ImageContainer
{
FlyCapture2::Image tmp[2];
FlyCapture2::Image unprocessed[2];
};
// generate color stereo input
void generateColorStereoInput(TriclopsContext const &context,
FlyCapture2::Image const &grabbedImage,
ImageContainer &imageCont,
TriclopsColorStereoPair &stereoPair)
{
Fc2Triclops::ErrorType fc2TriclopsError;
TriclopsError te;
TriclopsColorImage triclopsImageContainer[2];
FlyCapture2::Image *tmpImage = imageCont.tmp;
FlyCapture2::Image *unprocessedImage = imageCont.unprocessed;
// Convert the pixel interleaved raw data to de-interleaved and color processed data
fc2TriclopsError = Fc2Triclops::unpackUnprocessedRawOrMono16Image(
grabbedImage,
true /*assume little endian*/,
tmpImage[0],
tmpImage[1]);
UASSERT_MSG(fc2TriclopsError == Fc2Triclops::ERRORTYPE_OK, uFormat("Error: %d", (int)fc2TriclopsError).c_str());
// preprocess color image
for (int i = 0; i < 2; ++i) {
FlyCapture2::Error fc2Error;
fc2Error = tmpImage[i].SetColorProcessing(FlyCapture2::HQ_LINEAR);
UASSERT_MSG(fc2Error == FlyCapture2::PGRERROR_OK, fc2Error.GetDescription());
// convert preprocessed color image to BGRU format
fc2Error = tmpImage[i].Convert(FlyCapture2::PIXEL_FORMAT_BGRU,
&unprocessedImage[i]);
UASSERT_MSG(fc2Error == FlyCapture2::PGRERROR_OK, fc2Error.GetDescription());
}
// create triclops image for right and left lens
for (size_t i = 0; i < 2; ++i) {
TriclopsColorImage *image = &triclopsImageContainer[i];
te = triclopsLoadColorImageFromBuffer(
reinterpret_cast<TriclopsColorPixel *>(unprocessedImage[i].GetData()),
unprocessedImage[i].GetRows(),
unprocessedImage[i].GetCols(),
unprocessedImage[i].GetStride(),
image);
UASSERT_MSG(te == Fc2Triclops::ERRORTYPE_OK, uFormat("Error: %d", (int)te).c_str());
}
// create stereo input from the triclops images constructed above
// pack image data into a TriclopsColorStereoPair structure
te = triclopsBuildColorStereoPairFromBuffers(
context,
&triclopsImageContainer[1],
&triclopsImageContainer[0],
&stereoPair);
UASSERT_MSG(te == Fc2Triclops::ERRORTYPE_OK, uFormat("Error: %d", (int)te).c_str());
}
#endif
bool CameraStereoFlyCapture2::init(const std::string & calibrationFolder, const std::string & cameraName) bool CameraStereoFlyCapture2::init(const std::string & calibrationFolder, const std::string & cameraName)
{ {
#ifdef RTABMAP_FLYCAPTURE2 #ifdef RTABMAP_FLYCAPTURE2
@@ -130,15 +197,26 @@ bool CameraStereoFlyCapture2::init(const std::string & calibrationFolder, const
return false; 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; float fx, cx, cy, baseline;
triclopsGetFocalLength(triclopsCtx_, &fx); triclopsGetFocalLength(triclopsCtx_, &fx);
triclopsGetImageCenter(triclopsCtx_, &cy, &cx); triclopsGetImageCenter(triclopsCtx_, &cy, &cx);
cx *= maxWidth;
cy *= maxHeight;
triclopsGetBaseline(triclopsCtx_, &baseline); triclopsGetBaseline(triclopsCtx_, &baseline);
UINFO("Stereo parameters: fx=%f cx=%f cy=%f baseline=%f", fx, cx, cy, baseline); UINFO("Stereo parameters: fx=%f cx=%f cy=%f baseline=%f %dx%d", fx, cx, cy, baseline, maxWidth, maxHeight);
triclopsSetCameraConfiguration(triclopsCtx_, TriCfg_2CAM_HORIZONTAL_NARROW );
UASSERT(triclopsSetResolutionAndPrepare(triclopsCtx_, maxHeight, maxWidth, maxHeight, maxWidth) == Fc2Triclops::ERRORTYPE_OK);
if(camera_->StartCapture() != FlyCapture2::PGRERROR_OK) if(camera_->StartCapture() != FlyCapture2::PGRERROR_OK)
{ {
UERROR("Failed to start capture."); UERROR("Failed to start capture.");
@@ -182,15 +260,6 @@ std::string CameraStereoFlyCapture2::getSerial() const
return ""; return "";
} }
// struct containing image needed for processing
#ifdef RTABMAP_FLYCAPTURE2
struct ImageContainer
{
FlyCapture2::Image tmp[2];
FlyCapture2::Image unprocessed[2];
} ;
#endif
SensorData CameraStereoFlyCapture2::captureImage(CameraInfo * info) SensorData CameraStereoFlyCapture2::captureImage(CameraInfo * info)
{ {
SensorData data; SensorData data;
@@ -205,108 +274,67 @@ SensorData CameraStereoFlyCapture2::captureImage(CameraInfo * info)
// right and left image extracted from grabbed image // right and left image extracted from grabbed image
ImageContainer imageCont; ImageContainer imageCont;
// generate triclops input from grabbed image TriclopsColorStereoPair colorStereoInput;
FlyCapture2::Image imageRawRight; generateColorStereoInput(triclopsCtx_, grabbedImage, imageCont, colorStereoInput);
FlyCapture2::Image imageRawLeft;
FlyCapture2::Image * unprocessedImage = imageCont.unprocessed;
// Convert the pixel interleaved raw data to de-interleaved and color processed data // rectify images
if(Fc2Triclops::unpackUnprocessedRawOrMono16Image( TriclopsError triclops_status = triclopsColorRectify(triclopsCtx_, &colorStereoInput);
grabbedImage, UASSERT_MSG(triclops_status == Fc2Triclops::ERRORTYPE_OK, uFormat("Error: %d", (int)triclops_status).c_str());
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;; // get images
if ( srcImgRightRef.SetColorProcessing(FlyCapture2::HQ_LINEAR) != FlyCapture2::PGRERROR_OK || cv::Mat left,right;
srcImgLeftRef.SetColorProcessing(FlyCapture2::HQ_LINEAR) != FlyCapture2::PGRERROR_OK) TriclopsColorImage color_image;
{ triclops_status = triclopsGetColorImage(triclopsCtx_, TriImg_RECTIFIED_COLOR, TriCam_LEFT, &color_image);
ok = false; 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);
if(ok) // Set calibration stuff
{ float fx, cy, cx, baseline;
FlyCapture2::Image imageColorRight; triclopsGetFocalLength(triclopsCtx_, &fx);
FlyCapture2::Image imageColorLeft; triclopsGetImageCenter(triclopsCtx_, &cy, &cx);
if ( srcImgRightRef.Convert(FlyCapture2::PIXEL_FORMAT_MONO8, &imageColorRight) != FlyCapture2::PGRERROR_OK || triclopsGetBaseline(triclopsCtx_, &baseline);
srcImgLeftRef.Convert(FlyCapture2::PIXEL_FORMAT_BGRU, &imageColorLeft) != FlyCapture2::PGRERROR_OK) cx *= left.cols;
{ cy *= left.rows;
ok = false;
}
if(ok) StereoCameraModel model(
{ fx,
//RECTIFY RIGHT fx,
TriclopsInput triclopsColorInputs; cx,
triclopsBuildRGBTriclopsInput( cy,
grabbedImage.GetCols(), baseline,
grabbedImage.GetRows(), this->getLocalTransform(),
imageColorRight.GetStride(), left.size());
(unsigned long)grabbedImage.GetTimeStamp().seconds, data = SensorData(left, right, model, this->getNextSeqID(), UTimer::now());
(unsigned long)grabbedImage.GetTimeStamp().microSeconds,
imageColorRight.GetData(),
imageColorRight.GetData(),
imageColorRight.GetData(),
&triclopsColorInputs);
triclopsRectify(triclopsCtx_, const_cast<TriclopsInput *>(&triclopsColorInputs) ); // Compute disparity
// Retrieve the rectified image from the triclops context /*triclops_status = triclopsStereo(triclopsCtx_);
TriclopsImage rectifiedImage; UASSERT_MSG(triclops_status == Fc2Triclops::ERRORTYPE_OK, uFormat("Error: %d", (int)triclops_status).c_str());
triclopsGetImage( triclopsCtx_,
TriImg_RECTIFIED, TriclopsImage16 disparity_image;
TriCam_REFERENCE, triclops_status = triclopsGetImage16(triclopsCtx_, TriImg16_DISPARITY, TriCam_REFERENCE, &disparity_image);
&rectifiedImage ); 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);
cv::Mat left,right; int pixelinc = disparity_image.rowinc / 2;
right = cv::Mat(rectifiedImage.nrows, rectifiedImage.ncols, CV_8UC1, rectifiedImage.data).clone(); float x, y;
for (int i = 0, k = 0; i < disparity_image.nrows; i++) {
//RECTIFY LEFT COLOR unsigned short *row = disparity_image.data + i * pixelinc;
triclopsBuildPackedTriclopsInput( float *rowOut = (float *)depth.row(i).data;
grabbedImage.GetCols(), for (int j = 0; j < disparity_image.ncols; j++, k++) {
grabbedImage.GetRows(), unsigned short disparity = row[j];
imageColorLeft.GetStride(),
(unsigned long)grabbedImage.GetTimeStamp().seconds, // do not save invalid points
(unsigned long)grabbedImage.GetTimeStamp().microSeconds, if (disparity < 0xFFF0) {
imageColorLeft.GetData(), // convert the 16 bit disparity value to floating point x,y,z
&triclopsColorInputs ); triclopsRCD16ToXYZ(triclopsCtx_, i, j, disparity, &x, &y, &rowOut[j]);
}
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
float fx, cy, cx, baseline;
triclopsGetFocalLength(triclopsCtx_, &fx);
triclopsGetImageCenter(triclopsCtx_, &cy, &cx);
triclopsGetBaseline(triclopsCtx_, &baseline);
StereoCameraModel model(
fx,
fx,
cx,
cy,
baseline,
this->getLocalTransform(),
left.size());
data = SensorData(left, right, model, this->getNextSeqID(), UTimer::now());
}
}
} }
CameraModel model(fx, fx, cx, cy, this->getLocalTransform(), 0, left.size());
data = SensorData(left, depth, model, this->getNextSeqID(), UTimer::now());
*/
} }
} }