cv::Mat CameraUtils::getImageFromCameraProxy(jderobot::ImageDataPtr dataPtr) {
cv::Mat outImage;
colorspaces::Image::FormatPtr fmt;
fmt = colorspaces::Image::Format::searchFormat(dataPtr->description->format);
if (!fmt)
throw "Format not supported";
if (dataPtr->description->format == colorspaces::ImageRGB8::FORMAT_RGB8_Z.get()->name ||
dataPtr->description->format == colorspaces::ImageRGB8::FORMAT_DEPTH8_16_Z.get()->name )
{
size_t dest_len = dataPtr->description->width*dataPtr->description->height*3;
size_t source_len = dataPtr->pixelData.size();
unsigned char* origin_buf = (uchar*) malloc(dest_len);
int r = uncompress((Bytef *) origin_buf, (uLongf *) &dest_len, (const Bytef *) &(dataPtr->pixelData[0]), (uLong)source_len);
if(r != Z_OK) {
fprintf(stderr, "[CMPR] Error:\n");
switch(r) {
case Z_MEM_ERROR:
fprintf(stderr, "[CMPR] Error: Not enough memory to compress.\n");
break;
case Z_BUF_ERROR:
fprintf(stderr, "[CMPR] Error: Target buffer too small.\n");
break;
case Z_STREAM_ERROR: // Invalid compression level
fprintf(stderr, "[CMPR] Error: Invalid compression level.\n");
break;
}
}
else
{
colorspaces::Image imageRGB(dataPtr->description->width,dataPtr->description->height,colorspaces::ImageRGB8::FORMAT_RGB8,&(origin_buf[0]));
colorspaces::ImageRGB8 img_rgb888(imageRGB);//conversion will happen if needed
cv::Mat(cvSize(img_rgb888.width,img_rgb888.height), CV_8UC3, img_rgb888.data).copyTo(outImage);
img_rgb888.release();
}
if (origin_buf)
free(origin_buf);
}
else if (dataPtr->description->format == colorspaces::ImageRGB8::FORMAT_RGB8.get()->name ||
dataPtr->description->format == colorspaces::ImageRGB8::FORMAT_DEPTH8_16.get()->name )
{
colorspaces::Image imageRGB(dataPtr->description->width,dataPtr->description->height,colorspaces::ImageRGB8::FORMAT_RGB8,&(dataPtr->pixelData[0]));
colorspaces::ImageRGB8 img_rgb888(imageRGB);//conversion will happen if needed
cv::Mat(cvSize(img_rgb888.width,img_rgb888.height), CV_8UC3, img_rgb888.data).copyTo(outImage);
img_rgb888.release();
}
else if (dataPtr->description->format == colorspaces::ImageGRAY8::FORMAT_GRAY8_Z.get()->name) {
//gay compressed
size_t dest_len = dataPtr->description->width*dataPtr->description->height;
size_t source_len = dataPtr->pixelData.size();
unsigned char* origin_buf = (uchar*) malloc(dest_len);
int r = uncompress((Bytef *) origin_buf, (uLongf *) &dest_len, (const Bytef *) &(dataPtr->pixelData[0]), (uLong)source_len);
if(r != Z_OK) {
fprintf(stderr, "[CMPR] Error:\n");
switch(r) {
case Z_MEM_ERROR:
fprintf(stderr, "[CMPR] Error: Not enough memory to compress.\n");
break;
case Z_BUF_ERROR:
fprintf(stderr, "[CMPR] Error: Target buffer too small.\n");
break;
case Z_STREAM_ERROR: // Invalid compression level
fprintf(stderr, "[CMPR] Error: Invalid compression level.\n");
break;
}
}
else
{
colorspaces::Image imageGray(dataPtr->description->width,dataPtr->description->height,colorspaces::ImageGRAY8::FORMAT_GRAY8,&(origin_buf[0]));
colorspaces::ImageGRAY8 img_gray8(imageGray);//conversion will happen if needed
cv::Mat(cvSize(img_gray8.width,img_gray8.height), CV_8UC1, img_gray8.data).copyTo(outImage);
img_gray8.release();
}
if (origin_buf)
free(origin_buf);
}
else if (dataPtr->description->format == colorspaces::ImageGRAY8::FORMAT_GRAY8.get()->name){
colorspaces::Image imageGray(dataPtr->description->width,dataPtr->description->height,colorspaces::ImageGRAY8::FORMAT_GRAY8,&(dataPtr->pixelData[0]));
colorspaces::ImageGRAY8 img_gray8(imageGray);//conversion will happen if needed
cv::Mat(cvSize(img_gray8.width,img_gray8.height), CV_8UC1, img_gray8.data).copyTo(outImage);
img_gray8.release();
}
else{
LOG(ERROR) << "Unkown image format";
}
return outImage;
}
void PDI::mosaicEcualization(cv::Mat &img, int pixels, bool interpolate){
cv::Mat temp;
imageRGB(img);
// Slices (pixels * pixels)
int hor = int(img.cols/pixels);
int ver = int(img.rows/pixels);
int rHor = int(img.cols%pixels);
int rVer = int(img.rows%pixels);
int x, y;
for(int i=0; i<ver; i++){ // Moving in rows
y = i*pixels;
for(int j=0; j<hor; j++){
x = j*pixels;
temp = cv::Mat(img, cvRect(x, y, pixels, pixels));
// Ecualizing subimage
ecualizeImage(temp);
if(rHor>0 && j==hor-1){
temp = cv::Mat(img, cvRect(x+pixels, y, rHor, pixels));
// Ecualizing subimage
ecualizeImage(temp);
}
}
if(rVer>0 && i==ver-1){
for(int j=0; j<hor; j++){
x = j*pixels;
temp = cv::Mat(img, cvRect(x, y+pixels, pixels, rVer));
// Ecualizing subimage
ecualizeImage(temp);
if(rHor>0 && j==hor-1){
temp = cv::Mat(img, cvRect(x+pixels, y+pixels, rHor, rVer));
// Ecualizing subimage
ecualizeImage(temp);
}
}
}
}
if(interpolate){
if(img.cols < 2*pixels || img.rows < 2*pixels ){
std::cout << "ERROR: It's not possible to equalize, try again changing the dimensions of the sub-images." << std::endl;
std::cout << "Image Dimensions: " << img.rows << ", " << img.cols << std::endl;
std::cout << "Sub-image Dimensions: " << pixels << ", " << pixels << std::endl;
}
else{
// Get the image range (to re-convert)
int min, max;
getImageRange(img, min, max);
int subimg = 0;
int f00, f01, f10, f11;
cv::Mat im = convertToDouble(img);
// img: imagen de entrada (en cuadritos)
// pixels: dimensiones del cuadrito
// hor: num de cuadros horizontales
// ver: num de cuadros verticales
// rHor: pixeles horizontales sobrantes
// rVer: pixeles verticales sobrantes
// y: fila inicial para la sub imagen
// x: columna inicial para la sub imagen
std::cout << "Image dims: " << img.rows << " x " << img.cols << std::endl;
for(int v=0; v<ver; v++){ // Moving in rows (Vertical)
y = v*pixels + pixels/2;
for(int h=0; h<hor; h++){ // Moving in colums (Horizontal)
x = h*pixels + pixels/2;
std::cout << "(" << v << ", " << h << "): " << y << ", " << x << std::endl;
if( y+pixels < img.rows && x+pixels < img.cols ){
temp = cv::Mat(img, cvRect(x, y, pixels, pixels));
for(int i=0; i<temp.rows; i++){
for(int j=0; j<temp.cols; j++){
int s = j + 1;
int t = i + 1;
f00 = vHistograms[subimg].HistA[temp.at<uchar>(i, j)];
f01 = vHistograms[subimg+1].HistA[temp.at<uchar>(i, j)];
f10 = vHistograms[subimg+ver].HistA[temp.at<uchar>(i, j)];
f11 = vHistograms[subimg+ver+1].HistA[temp.at<uchar>(i, j)];
temp.at<uchar>(i,j) = (1-s) * (1-t) * f00 + s * (1-t) * f10
+ (1-s) * t+1 * f01 + s * t * f11;
}
}
}
subimg++;
}
}
/*
for(int v=0; v<ver; v++){ // Moving in rows
y = v*pixels;
for(int h=0; h<hor; h++){
x = h*pixels;
// Ecualizing subimage
std::cout << subimg << std::endl;
Hists = vHistograms[subimg];
for(int s=0; s<pixels; s++){
for(int t=0; t<pixels; t++){
int ss = s + v * pixels;
int tt = t + h * pixels;
f00 = vHistograms[subimg].HistA[img.at<uchar>(s, t)];
f01 = vHistograms[subimg+1].HistA[img.at<uchar>(s, t)];
f10 = vHistograms[subimg+ver].HistA[img.at<uchar>(s, t)];
f11 = vHistograms[subimg+ver+1].HistA[img.at<uchar>(s, t)]; // std::cout << "s: " << s << " ss: " << ss << " t: " << t << " tt: " << tt << " => ";
// std::cout << "f00(" << 0+v*pixels << ", " << 0+h*pixels << ") - "
// << "f10(" << 0+v*pixels << ", " << pixels-1+h*pixels << ") - "
// << "f01(" << pixels-1+v*pixels << ", " << 0+h*pixels << ") - "
// << "f11(" << pixels-1+v*pixels << ", " << pixels-1+h*pixels << ")" << std::endl;
im.at<double>(ss,tt) = (1-s) * (1-t) * f00 + s * (1-t) * f10
+ (1-s) * t+1 * f01 + s * t * f11;
}
}
subimg++;
if(rHor>0 && h==hor-1){
std::cout << subimg << std::endl;
Hists = vHistograms[subimg];
// Ecualizing subimage
subimg++;
}
}
if(rVer>0 && v==ver-1){
for(int h=0; h<hor; h++){
x = h*pixels;
std::cout << subimg << std::endl;
Hists = vHistograms[subimg];
// Ecualizing subimage
subimg++;
if(rHor>0 && h==hor-1){
std::cout << subimg << std::endl;
Hists = vHistograms[subimg];
// Ecualizing subimage
subimg++;
}
}
}
}
*/
// Image double converted into integer
// normalizateMatrix(im, min, max);
// cv::Mat im2 = convertToInterger(im);
// im2.copyTo(img);
}
}
}
