NV_CHAR *get_geotiff (NV_CHAR *mosaic_file, MISC *misc)
{
static NV_CHAR string[512];
strcpy (string, "Success");
if (strstr (mosaic_file, ".tif") || strstr (mosaic_file, ".TIF"))
{
GDALDataset *poDataset;
NV_FLOAT64 adfGeoTransform[6];
GDALAllRegister ();
poDataset = (GDALDataset *) GDALOpen (mosaic_file, GA_ReadOnly);
if (poDataset != NULL)
{
if (poDataset->GetProjectionRef () != NULL)
{
QString projRef = QString (poDataset->GetProjectionRef ());
if (projRef.contains ("GEOGCS"))
{
if (poDataset->GetGeoTransform (adfGeoTransform) == CE_None)
{
misc->lon_step = adfGeoTransform[1];
misc->lat_step = -adfGeoTransform[5];
misc->mosaic_width = poDataset->GetRasterXSize ();
misc->mosaic_height = poDataset->GetRasterYSize ();
misc->geotiff_area.min_x = adfGeoTransform[0];
misc->geotiff_area.max_y = adfGeoTransform[3];
misc->geotiff_area.min_y = misc->geotiff_area.max_y - misc->mosaic_height * misc->lat_step;
misc->geotiff_area.max_x = misc->geotiff_area.min_x + misc->mosaic_width * misc->lon_step;
}
else
{
delete poDataset;
sprintf (string, "File %s contains projected data", gen_basename (mosaic_file));
return (string);
}
}
else
{
delete poDataset;
sprintf (string, "File %s contains Non-geographic coordinate system", gen_basename (mosaic_file));
return (string);
}
}
else
{
delete poDataset;
sprintf (string, "File %s contains no datum/projection information", gen_basename (mosaic_file));
return (string);
}
if (misc->full_res_image != NULL) delete misc->full_res_image;
/* This is how I used to read geoTIFFs but for some reason it stopped working along about Qt 4.7.2 so now I use GDAL to
read it. I really need to keep testing to see if this starts working again because it's a bit faster and probably
a lot more bullet proof.
misc->full_res_image = new QImage (mosaic_file);
if (misc->full_res_image == NULL || misc->full_res_image->width () == 0 || fmisc->ull_res_image->height () == 0)
{
sprintf (string, "Unable to read file %s", gen_basename (mosaic_file));
delete poDataset;
return (string);
}
*/
// This is how I read geoTIFFs now...
GDALRasterBand *poBand[4];
QString dataType[4], colorInt[4];
NV_U_INT32 mult[4] = {0, 0, 0, 0};
NV_INT32 rasterCount = poDataset->GetRasterCount ();
if (rasterCount < 3)
{
delete poDataset;
sprintf (string, "Not enough raster bands in geoTIFF");
return (string);
}
for (NV_INT32 i = 0 ; i < rasterCount ; i++)
{
poBand[i] = poDataset->GetRasterBand (i + 1);
dataType[i] = QString (GDALGetDataTypeName (poBand[i]->GetRasterDataType ()));
colorInt[i] = QString (GDALGetColorInterpretationName (poBand[i]->GetColorInterpretation ()));
// We can only handle Byte data (i.e. RGB or ARGB)
if (dataType[i] != "Byte")
{
delete poDataset;
sprintf (string, "Can only handle Byte data type");
return (string);
}
mult[i] = getColorOffset (colorInt[i]);
}
NV_INT32 nXSize = poBand[0]->GetXSize ();
NV_INT32 nYSize = poBand[0]->GetYSize ();
misc->full_res_image = new QImage (nXSize, nYSize, QImage::Format_ARGB32);
if (misc->full_res_image == NULL || misc->full_res_image->width () == 0 || misc->full_res_image->height () == 0)
{
sprintf (string, "Unable to open image!");
delete poDataset;
return (string);
}
NV_U_INT32 *color = new NV_U_INT32[nXSize];
NV_U_BYTE *pafScanline = (NV_U_BYTE *) CPLMalloc (sizeof (NV_U_BYTE) * nXSize);
for (NV_INT32 i = 0 ; i < nYSize ; i++)
{
// If we don't have an alpha band set it to 255.
for (NV_INT32 k = 0 ; k < nXSize ; k++)
{
if (rasterCount < 4)
{
color[k] = 0xff000000;
}
else
{
color[k] = 0x0;
}
}
// Read the raster bands.
for (NV_INT32 j = 0 ; j < rasterCount ; j++)
{
poBand[j]->RasterIO (GF_Read, 0, i, nXSize, 1, pafScanline, nXSize, 1, GDT_Byte, 0, 0);
for (NV_INT32 k = 0 ; k < nXSize ; k++) color[k] += ((NV_U_INT32) pafScanline[k]) * mult[j];
}
// Set the image pixels.
for (NV_INT32 k = 0 ; k < nXSize ; k++)
{
misc->full_res_image->setPixel (k, i, color[k]);
}
}
delete (color);
CPLFree (pafScanline);
delete poDataset;
}
else
{
sprintf (string, "Unable to open file %s", gen_basename (mosaic_file));
return (string);
}
}
else
{
sprintf (string, "File %s is not a GeoTIFF file", gen_basename (mosaic_file));
return (string);
}
return (string);
}
void
GrabSend::init(zmq::context_t* zmq_context)
{
if(config.cfg_panoramic){
config.cfg_panoramic=false;
printf("Warning: panoramic image creation not supported in grabber...\n");
}
if(config.cfg_transfer_compressed){
config.cfg_transfer_compressed=false;
printf("Warning: compressed image transfer is not supported in grabber...\n");
}
lady = new Ladybug();
lady->init(&config);
//-----------------------------------------------
//create watchdog
//-----------------------------------------------
int val_watchdog = 1;
socket_watchdog = new zmq::socket_t(*zmq_context, ZMQ_PUSH);
socket_watchdog->setsockopt(ZMQ_RCVHWM, &val_watchdog, sizeof(val_watchdog)); //prevent buffer get overfilled
socket_watchdog->setsockopt(ZMQ_SNDHWM, &val_watchdog, sizeof(val_watchdog));
socket_watchdog->connect("inproc://watchdog");
socket_watchdog->send(msg_watchdog, ZMQ_NOBLOCK);
lady->grabImage(&image);
separatedColors = isColorSeparated(&image);
bayer_encoding = getBayerEncoding(&image) + std::to_string(lady->config->get_image_depth());
color_encoding = lady->config->get_color_encoding();
printf("Sending color: %s bayer: %s depth: %ibit\n", color_encoding.c_str(), bayer_encoding.c_str(), lady->config->get_image_depth());
if(separatedColors){
getColorOffset(&image, red_offset, green_offset, blue_offset);
uiRawCols = image.uiFullCols / 2;
uiRawRows = image.uiFullRows / 2;
}
else{
//printf("Exception: only jepeg color sep images are supported in grabber\n");
//throw new std::exception("only jepeg color sep images are supported in grabber");
uiRawCols = image.uiFullCols;
uiRawRows = image.uiFullRows;
}
_TIME
std::string connection;
int socket_type = ZMQ_PUB;
bool zmq_bind = false;
connection = cfg_ros_master.c_str();
status = "connect with zmq to " + connection;
socket = new zmq::socket_t(*zmq_context, socket_type);
int val = 2; //buffer size
socket->setsockopt(ZMQ_RCVHWM, &val, sizeof(val)); //prevent buffer get overfilled
socket->setsockopt(ZMQ_SNDHWM, &val, sizeof(val)); //prevent buffer get overfilled
if(zmq_bind){
socket->bind(connection.c_str());
}else{
socket->connect(connection.c_str());
}
_TIME
socket_watchdog->send(msg_watchdog, ZMQ_NOBLOCK);
for( unsigned int uiCamera = 0; uiCamera < LADYBUG_NUM_CAMERAS; uiCamera++ ){
status = "reading camera extrinics and disortion";
CameraCalibration calib;
lady->getCameraCalibration(uiCamera, &calib);
position[uiCamera].set_rx(calib.rotationX);
position[uiCamera].set_ry(calib.rotationY);
position[uiCamera].set_rz(calib.rotationZ);
position[uiCamera].set_tx(calib.translationX);
position[uiCamera].set_ty(calib.translationY);
position[uiCamera].set_tz(calib.translationZ);
disortion[uiCamera].set_centerx(calib.centerX/2);
disortion[uiCamera].set_centery(calib.centerY/2);
disortion[uiCamera].set_focalx(calib.focal_lenght/2); //TODO: Check it thats right??
disortion[uiCamera].set_focaly(calib.focal_lenght/2);
_TIME
ladybug5_network::pbImage* image_msg = 0;
image_msg = message.add_images();
image_msg->set_type((ladybug5_network::ImageType) ( 1 << uiCamera));
image_msg->set_name(enumToString(image_msg->type()));
image_msg->set_height(uiRawRows);
image_msg->set_width(uiRawCols);
image_msg->set_allocated_distortion(new ladybug5_network::pbDisortion(disortion[uiCamera]));
image_msg->set_allocated_position(new ladybug5_network::pbPosition(position[uiCamera]));
image_msg->set_border_left(image.imageBorder.uiLeftCols/2);
image_msg->set_border_right(image.imageBorder.uiRightCols/2);
image_msg->set_border_top(image.imageBorder.uiTopRows/2);
image_msg->set_border_bottem(image.imageBorder.uiBottomRows/2);
image_msg->set_color_encoding(color_encoding);
image_msg->set_bayer_encoding(bayer_encoding);
image_msg->set_depth(lady->config->get_image_depth());
if(separatedColors){
image_msg->set_packages(3);
}
else{
image_msg->set_packages(1);
}
_TIME
}
unsigned int nr = 0;
double loopstart = t_now = clock();
printf("Running...\n");
socket_watchdog->send(msg_watchdog, ZMQ_NOBLOCK);
}
