void ProcessImage(Image<Tout>& img_out, const Image<Tin>& img_in, bayer_method_t method, color_filter_t tile)
{
if(method == BAYER_METHOD_NONE) {
PitchedImageCopy(img_out, img_in.template UnsafeReinterpret<Tout>() );
}else if(method == BAYER_METHOD_DOWNSAMPLE_MONO) {
if( sizeof(Tout) == 1) {
DownsampleToMono<int,Tout, Tin>(img_out, img_in);
}else{
DownsampleToMono<double,Tout, Tin>(img_out, img_in);
}
}else if(method == BAYER_METHOD_DOWNSAMPLE) {
DownsampleDebayer(img_out, img_in, tile);
}else{
#ifdef HAVE_DC1394
if(sizeof(Tout) == 1) {
dc1394_bayer_decoding_8bit(
(uint8_t*)img_in.ptr, (uint8_t*)img_out.ptr, img_in.w, img_in.h,
(dc1394color_filter_t)tile, (dc1394bayer_method_t)method
);
}else if(sizeof(Tout) == 2) {
dc1394_bayer_decoding_16bit(
(uint16_t*)img_in.ptr, (uint16_t*)img_out.ptr, img_in.w, img_in.h,
(dc1394color_filter_t)tile, (dc1394bayer_method_t)method,
16
);
}
#endif
}
}
void Libdc1394SequenceGrabber::processCameraImageData( unsigned char* cameraImageData )
{
unsigned char* dest = getImage()->data();
unsigned int w = getImage()->s();
unsigned int h = getImage()->t();
unsigned int bpp = getImage()->getPixelSizeInBits() / 8;
unsigned int size = w * h * bpp;
if( _sourceFormat == DC1394_COLOR_CODING_RAW8 || _sourceFormat == DC1394_COLOR_CODING_MONO8 )
{
if (_grey)
memcpy(dest, cameraImageData, size);
else
dc1394_bayer_decoding_8bit( cameraImageData, dest, w, h, _bayerPattern, _bayerMethod );
}
else if(_sourceFormat == DC1394_COLOR_CODING_MONO16 || _sourceFormat == DC1394_COLOR_CODING_RAW16 )
{
// These are not implemented yet....
if(!_grey)
{
dc1394_convert_to_RGB8(cameraImageData, dest, w, h, 0, _sourceFormat, 16);
}
else {
dc1394_convert_to_MONO8(cameraImageData, dest, w, h, 0, _sourceFormat, 16);
}
}
else if(_sourceFormat == DC1394_COLOR_CODING_YUV411 ||
_sourceFormat == DC1394_COLOR_CODING_YUV422 ||
_sourceFormat == DC1394_COLOR_CODING_YUV444 )
{
if(!_grey )
{
dc1394_convert_to_RGB8( cameraImageData, dest, w, h, DC1394_BYTE_ORDER_UYVY, _sourceFormat, 16);
}
else
{
msg(osg::WARN) << "could not handle sourceformat " << _sourceFormat << std::endl;
}
}
else if( _sourceFormat == DC1394_COLOR_CODING_RGB8 )
{
if( !_grey )
{
memcpy(dest, cameraImageData, size);
}
else
{
msg(osg::WARN) << "could not handle sourceformat " << _sourceFormat << std::endl;
}
}
else
{
msg(osg::WARN) << "could not handle sourceformat " << _sourceFormat << std::endl;
}
}
void
extractImagesColorXB3( PGRStereoCamera_t* stereoCamera,
dc1394bayer_method_t bayerMethod,
unsigned char* pucDeInterleaved,
unsigned char* pucRGB,
unsigned char* pucGreen,
unsigned char** ppucRightRGB,
unsigned char** ppucLeftRGB,
unsigned char** ppucCenterRGB)
{
dc1394error_t err;
dc1394video_frame_t* frame;
err = dc1394_capture_dequeue( stereoCamera->camera,
DC1394_CAPTURE_POLICY_WAIT,
&frame );
if ( err != DC1394_SUCCESS )
{
fprintf( stderr, "extractImagesColor - cannot dequeue image!\n" );
return;
}
unsigned char* pucGrabBuffer = frame->image;
dc1394_deinterlace_rgb( pucGrabBuffer,
pucDeInterleaved,
stereoCamera->nCols,
3*stereoCamera->nRows );
// extract color from the bayer tile image
// note: this will alias colors on the top and bottom rows
dc1394_bayer_decoding_8bit( pucDeInterleaved,
pucRGB,
stereoCamera->nCols,
3*stereoCamera->nRows,
stereoCamera->bayerTile,
bayerMethod );
// now deinterlace the RGB Buffer
dc1394_deinterlace_green( pucRGB,
pucGreen,
stereoCamera->nCols,
9*stereoCamera->nRows );
// NOTE: this code needs to be double checked.
// Currently 3-bytes-per-pixel is not activatable in this example
int iOneBufferPixels = stereoCamera->nRows * stereoCamera->nCols;
*ppucLeftRGB = pucRGB;
*ppucCenterRGB = pucRGB + 3 * iOneBufferPixels;
*ppucRightRGB = pucRGB + 6 * iOneBufferPixels;
// return buffer for use
dc1394_capture_enqueue( stereoCamera->camera, frame );
return;
}
//! Implement VideoInput::GrabNext()
bool DebayerVideo::GrabNext( unsigned char* image, bool wait )
{
if(videoin[0]->GrabNext(buffer,wait)) {
for(size_t s=0; s<streams.size(); ++s) {
Image<unsigned char> img_in = videoin[0]->Streams()[s].StreamImage(buffer);
Image<unsigned char> img_out = Streams()[s].StreamImage(image);
#ifdef HAVE_DC1394
dc1394_bayer_decoding_8bit(
img_in.ptr, img_out.ptr, img_in.w, img_in.h,
(dc1394color_filter_t)tile, (dc1394bayer_method_t)method
);
#else
// use our simple debayering instead
DownsampleDebayer(img_out, img_in, tile);
#endif
}
return true;
}else{
return false;
}
}
//=============================================================================
// extractImagesColor()
//
// De-interleave the stereo images into single bayer patterns.
// De-bayer those images into color images.
// Construct a TriclopsInput for stereo processing from these images.
//
void
extractImagesColor( PGRStereoCamera_t* stereoCamera,
dc1394bayer_method_t bayerMethod,
unsigned char* pucDeInterleaved,
unsigned char* pucRGB,
unsigned char* pucGreen,
unsigned char** ppucRightRGB,
unsigned char** ppucLeftRGB,
unsigned char** ppucCenterRGB)
{
dc1394error_t err;
dc1394video_frame_t* frame;
err = dc1394_capture_dequeue( stereoCamera->camera,
DC1394_CAPTURE_POLICY_WAIT,
&frame );
if ( err != DC1394_SUCCESS )
{
fprintf( stderr, "extractImagesColor - cannot dequeue image!\n" );
return;
}
unsigned char* pucGrabBuffer = frame->image;
if ( stereoCamera->nBytesPerPixel == 2 )
{
// de-interlace the 16 bit data into 2 bayer tile pattern images
dc1394_deinterlace_stereo( pucGrabBuffer,
pucDeInterleaved,
stereoCamera->nCols,
2*stereoCamera->nRows );
// extract color from the bayer tile image
// note: this will alias colors on the top and bottom rows
dc1394_bayer_decoding_8bit( pucDeInterleaved,
pucRGB,
stereoCamera->nCols,
2*stereoCamera->nRows,
stereoCamera->bayerTile,
bayerMethod );
// now deinterlace the RGB Buffer to extract the green channel
// The green channel is a quick and dirty approximation to the mono
// equivalent of the image and can be used for stereo processing
dc1394_deinterlace_green( pucRGB,
pucGreen,
stereoCamera->nCols,
6*stereoCamera->nRows );
*ppucRightRGB = pucRGB;
*ppucLeftRGB = pucRGB + 3 * stereoCamera->nRows * stereoCamera->nCols;
*ppucCenterRGB = *ppucLeftRGB;
}
else
{
dc1394_deinterlace_rgb( pucGrabBuffer,
pucDeInterleaved,
stereoCamera->nCols,
3*stereoCamera->nRows );
// extract color from the bayer tile image
// note: this will alias colors on the top and bottom rows
dc1394_bayer_decoding_8bit( pucDeInterleaved,
pucRGB,
stereoCamera->nCols,
3*stereoCamera->nRows,
stereoCamera->bayerTile,
bayerMethod );
// now deinterlace the RGB Buffer
dc1394_deinterlace_green( pucRGB,
pucGreen,
stereoCamera->nCols,
9*stereoCamera->nRows );
// NOTE: this code needs to be double checked.
// Currently 3-bytes-per-pixel is not activatable in this example
*ppucRightRGB = pucRGB;
*ppucCenterRGB = pucRGB + 3 * stereoCamera->nRows * stereoCamera->nCols;
*ppucLeftRGB = pucRGB + 6 * stereoCamera->nRows * stereoCamera->nCols;
}
// return buffer for use
dc1394_capture_enqueue( stereoCamera->camera, frame );
return;
}
int
main( int argc, char ** argv )
{
uint32_t in_size=0, out_size=0, width=0, height=0, bpp=0;
dc1394color_filter_t first_color = DC1394_COLOR_FILTER_RGGB;
int tiff = 0;
dc1394bayer_method_t method = DC1394_BAYER_METHOD_BILINEAR;
char *infile=NULL, *outfile=NULL;
int input_fd = 0;
int output_fd = 0;
void * bayer = NULL;
void * rgb = NULL, *rgb_start = NULL;
char c;
int optidx = 0;
int swap = 0;
struct option longopt[] = {
{"input",1,NULL,'i'},
{"output",1,NULL,'o'},
{"width",1,NULL,'w'},
{"height",1,NULL,'v'},
{"help",0,NULL,'h'},
{"bpp",1,NULL,'b'},
{"first",1,NULL,'f'},
{"method",1,NULL,'m'},
{"tiff",0,NULL,'t'},
{"swap",0,NULL,'s'},
{0,0,0,0}
};
while ((c=getopt_long(argc,argv,"i:o:w:v:b:f:m:ths",longopt,&optidx)) != -1)
{
switch ( c )
{
case 'i':
infile = strdup( optarg );
break;
case 'o':
outfile = strdup( optarg );
break;
case 'w':
width = strtol( optarg, NULL, 10 );
break;
case 'v':
height = strtol( optarg, NULL, 10 );
break;
case 'b':
bpp = strtol( optarg, NULL, 10 );
break;
case 'f':
first_color = getFirstColor( optarg );
break;
case 'm':
method = getMethod( optarg );
break;
case 's':
swap = 1;
break;
case 't':
tiff = TIFF_HDR_SIZE;
break;
case 'h':
usage(argv[0]);
return 0;
break;
default:
printf("bad arg\n");
usage(argv[0]);
return 1;
}
}
// arguments: infile outfile width height bpp first_color
if( infile == NULL || outfile == NULL || bpp == 0 || width == 0 || height == 0 )
{
printf("Bad parameter\n");
usage(argv[0]);
return 1;
}
input_fd = open(infile, O_RDONLY);
if(input_fd < 0)
{
printf("Problem opening input: %s\n", infile);
return 1;
}
output_fd = open(outfile, O_RDWR | O_CREAT | O_TRUNC, S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH );
if(output_fd < 0)
{
printf("Problem opening output: %s\n", outfile);
return 1;
}
in_size = lseek(input_fd, 0, SEEK_END );
lseek(input_fd, 0, 0);
out_size = width * height * (bpp / 8) * 3 + tiff;
ftruncate(output_fd, out_size );
bayer = mmap(NULL, in_size, PROT_READ | PROT_WRITE, MAP_PRIVATE /*| MAP_POPULATE*/, input_fd, 0);
if( bayer == MAP_FAILED )
{
perror("Faild mmaping input");
return 1;
}
rgb_start = rgb = mmap(NULL, out_size, PROT_READ | PROT_WRITE, MAP_SHARED /*| MAP_POPULATE*/, output_fd, 0);
if( rgb == MAP_FAILED )
{
perror("Faild mmaping output");
return 1;
}
#ifdef DEBUG
printf("%p -> %p\n", bayer, rgb);
printf("%s: %s(%d) %s(%d) %d %d %d, %d %d\n", argv[0], infile, in_size, outfile, out_size, width, height, bpp, first_color, method );
//memset(rgb, 0xff, out_size);//return 1;
#endif
if(tiff)
{
rgb_start = put_tiff((uint8_t*)rgb, width, height, bpp);
}
#if 1
switch(bpp)
{
case 8:
dc1394_bayer_decoding_8bit((const uint8_t*)bayer, (uint8_t*)rgb_start, width, height, first_color, method);
break;
case 16:
dc1394_bayer_decoding_16bit((const uint16_t*)bayer, (uint16_t*)rgb_start, width, height, first_color, method, bpp);
break;
default:
{
uint8_t tmp=0;
uint32_t i=0;
for(i=0;i<in_size;i+=2){
tmp = *(((uint8_t*)bayer)+i);
*(((uint8_t*)bayer)+i) = *(((uint8_t*)bayer)+i+1);
*(((uint8_t*)bayer)+i+1) = tmp;
}
}
dc1394_bayer_decoding_16bit((const uint16_t*)bayer, (uint16_t*)rgb_start, width, height, first_color, method, bpp);
break;
}
#endif
#if DEBUG
printf("Last few In: %x %x %x %x\n",
((uint32_t*)bayer)[0],
((uint32_t*)bayer)[1],
((uint32_t*)bayer)[2],
((uint32_t*)bayer)[3]);
// ((int*)rgb)[2] = 0xadadadad;
printf("Last few Out: %x %x %x %x\n",
((uint32_t*)rgb)[0],
((uint32_t*)rgb)[1],
((uint32_t*)rgb)[2],
((uint32_t*)rgb)[3]);
#endif
munmap(bayer,in_size);
close(input_fd);
if( msync(rgb, out_size, MS_INVALIDATE|MS_SYNC) != 0 )
perror("Problem msyncing");
munmap(rgb,out_size);
if( fsync(output_fd) != 0 )
perror("Problem fsyncing");
close(output_fd);
return 0;
}
void Recombiner::recombine(const stdr_msgs::LadybugImages & raw_images,
std::vector<sensor_msgs::Image::Ptr> & bayer_images)
{
ROS_ASSERT( raw_images.images.size() == 24 );
bayer_images.resize(6);
#if !SAVE_INTERMEDIATE_IMAGES
#pragma omp parallel for
#else
std::cout <<"timestamp: " <<std::setprecision(std::numeric_limits<double>::digits10) <<raw_images.header.stamp.toSec() <<std::endl;
std::cout <<"Saving frame " <<frame_count_ <<std::endl;
#endif
for( unsigned cam=0; cam<6; ++cam )
{
const unsigned c4 = cam * 4;
if( !selector_[cam] )
continue;
const bool valid_camera_img = decompress4(raw_images, c4);
// Prepare the output
// Make sure the image is properly defined and allocated
if( !valid_camera_img ) {
bayer_images[cam].reset();
continue;
}
if( ! bayer_images[cam] )
bayer_images[cam].reset( new sensor_msgs::Image );
sensor_msgs::Image & img = *(bayer_images[cam]);
img.header = raw_images.header;
std::stringstream ss;
ss << "/ladybug/camera" <<cam;
img.header.frame_id = ss.str();
img.height = FULL_HEIGHT;
img.width = FULL_WIDTH;
img.is_bigendian = false;
if( debayer_ ) {
img.encoding = sensor_msgs::image_encodings::RGB8;
img.step = img.width * 3;
}
else {
img.encoding = sensor_msgs::image_encodings::BAYER_GRBG8;
img.step = img.width;
}
img.data.resize(img.height * img.step);
// select the output of the recombining stage
std::vector<unsigned char> & data = debayer_ ? combined_images_[cam] : img.data;
combine(c4, &(data[0]));
if( debayer_ ) {
// TODO: implement debayering with dc1394
dc1394_bayer_decoding_8bit( &(combined_images_[cam][0]), &(img.data[0]),
FULL_WIDTH, FULL_HEIGHT,
DC1394_COLOR_FILTER_GRBG,
debayer_alg_);
#if SAVE_INTERMEDIATE_IMAGES
//save debayered image
cv::Mat mrgb(FULL_HEIGHT, FULL_WIDTH, CV_8UC3, &(img.data[0]));
cv::Mat mbgr(FULL_HEIGHT, FULL_WIDTH, CV_8UC3);
cv::cvtColor(mrgb, mbgr, CV_RGB2BGR);
const std::string name = (boost::format("frame%04d_final_%02d.bmp") % frame_count_ % (c4/4)).str();
cv::imwrite(name, mbgr);
cv::imshow("frame", mbgr);
cv::waitKey();
#endif
}
}
++frame_count_;
}