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); }