img2pano_lut.cpp

Go to the documentation of this file.

/*
 * IMG2PANO_LUT - stitching of incoming IMAGES to PANORAMIC view using pre-computed LOOK-UP TABLES
 * by M. Reinstein, 2011
 *
 * -m  ... mode of operation (1-6 single cam output, 7 panorama using LUT)
 * -t  ... input topic name (default: "/camera/image")
 * -o  ... output topic name (default: "/viz/image_out")
 * -ph ... output panorama height (default: PANO_HEIGHT 375)
 * -pw ... output panorama width (default: IMAGE_WIDTH 808)
 * -ih ... input image height (default: IMAGE_HEIGHT 616)
 * -iw ... input image width (default: IMAGE_WIDTH 808)
 * -r  ... sphere radius [m] (default: PANO_RADIUS 20)
 *
 * LUTs generated for the following parameters:
 * 1. image 808x616, panorama 750x375, radius 20 (default for mode 7, i.e. -m 7)
 *
 * To RUN the node:
 * use img2pano.launch
 * OR
 * rosrun omnicamera img2pano_lut -m 7 -ih 616 -iw 808 -ph 375 -pw 750 -r 20 -path $(find omnicamera)/res/lut
 *
 * To VIEW the panorama using default output:
 * rosrun image_view image_view image:=viz/image_out
 *
*/

//#include <opencv/highgui.h>
#include <opencv/cv.h>
#include <iostream>
#include <sstream>
#include <fstream>
#include <string>
#include <stdio.h>
#include <stdlib.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv/cvwimage.h>
#include <image_transport/image_transport.h>// ROS
#include <ros/ros.h>                                            // ROS
#include <vision_msgs/Rect.h>                           // ROS

#define NCAMS 6 // not to be changed! For single cam output use: -m 1 to -m 6
#define IMAGE_HEIGHT 616
#define IMAGE_WIDTH 808
#define PANO_HEIGHT 375
#define PANO_WIDTH 750
#define PANO_RADIUS 20
using namespace std;

// IMAGE TEMPLATE:
template<class T> class Image
{
private:
  IplImage* imgp;
public:
  Image(IplImage* img=0) {imgp=img;}
  ~Image(){imgp=0;}
  void operator=(IplImage* img) {imgp=img;}
  inline T* operator[](const int rowIndx) {
    return ((T *)(imgp->imageData + rowIndx*imgp->widthStep));}
};
typedef struct{
  unsigned char b,g,r;
} RgbPixel;
typedef struct{
  float b,g,r;
} RgbPixelFloat;

typedef Image<RgbPixel>       RgbImage;
typedef Image<RgbPixelFloat>  RgbImageFloat;
typedef Image<unsigned char>  BwImage;
typedef Image<float>          BwImageFloat;

// LOOK-UP TABLE:
class CLut
{
public:
        CLut(int nRows, int nCols, int nCams);
        ~CLut();
        int getValue(int iRow, int iCol, int iCam);                             // read LUT value
        void setValue(int iRow, int iCol, int iCam, int iVal);  // set LUT value
        int fillArray(string cFileName[]);                                              // fill LUT using txt file of given name
private:
        void initArray();
        int *m_array;
        int m_Rows;// = HEIGHT
        int m_Cols;// = WIDTH
        int m_Cams;// = 6
};
CLut::CLut(int nRows, int nCols, int nCams)
{
m_Rows = nRows;
m_Cols = nCols;
m_Cams = nCams;
initArray();
}
CLut::~CLut(){
        delete m_array;
}
void CLut::initArray()
{
        m_array = new int [m_Rows*m_Cols*NCAMS];
}
inline int CLut::getValue(int iRow, int iCol, int iCam)
{
if (iRow<m_Rows && iCol<m_Cols && iCam <m_Cams)
        return m_array[iCol+iRow*m_Cols+iCam*m_Cols*m_Rows];
else
        return 0;
}
inline void CLut::setValue(int iRow, int iCol, int iCam, int iVal)
{
if (iRow<m_Rows && iCol<m_Cols && iCam <m_Cams)
        m_array[iCol+iRow*m_Cols+iCam*m_Cols*m_Rows] = iVal;
}
int CLut::fillArray(string cFileName[])
{
        const char * pFileName;
        int indCam = 0;
        int value = 0;
        int linenum = 0;
        int itemnum = 0;
        string line, item;


        for(indCam = 0; indCam<6; indCam++)
        {
                linenum = 0;
                pFileName = cFileName[indCam].c_str();
                ifstream inFile(pFileName);
                if (inFile.is_open())
                  {
                        while (getline (inFile, line))
                        {
                                if (linenum <m_Rows)
                                {
                                        istringstream linestream(line);
                                        while (getline (linestream, item, ',') && (itemnum<m_Cols))
                                        {
                                                istringstream is(item);
                                                is >> value;
                                                m_array[itemnum+linenum*m_Cols+indCam*m_Cols*m_Rows] = value-1;
                                                itemnum++;
                                        }
                                }
                        linenum++;
                        itemnum = 0; // set itemnum to zero at the beginning of the new line
                        }
                        inFile.close();
                        cout << "LUT uploaded: " << pFileName << endl;
                  }
                else
                {
                        ROS_ERROR("Unable to open following LUT file: '%s'\n", pFileName);
                        //cout << "HINT: If the LUT file does not exist, use ladybug_lut/main_pano_mesh.m and ladybug_lut/lut2txt.m to generate it." << endl;
                        exit (1);
                }
        }
        return 0;

}
void setlabels(string (&labelset)[NCAMS], char path[], int pheight, int pwidth, int iheight, int iwidth, int prange, char xory)
{
        char label[200];
        for(int i = 1; i <= NCAMS; i++) {
                sprintf(label, "%s/LUT_img_%ix%i_pano_%ix%ir%i_%c%i.txt", path, iwidth, iheight, pwidth, pheight, prange, xory, i);
                labelset[i-1] = label;
        }
}

// IMAGE HANDLING:
int mode = 1;                           // default mode of operation
int imgh = IMAGE_HEIGHT;        // default expected input image height
int imgw = IMAGE_WIDTH;         // default expected input image height
int panoh = PANO_HEIGHT;        // default desired panorama height
int panow = PANO_WIDTH;         // default desired panorama width
int panor = PANO_RADIUS;        // default sphere radius used for stitching

CLut *pLUTX;
CLut *pLUTY;
CLut *pLUTA;
IplImage *frame;
IplImage *out;
image_transport::Publisher pub;
ros::Publisher camera_info_pub;
sensor_msgs::CameraInfo msg_cam;

void imageCallback(const sensor_msgs::ImageConstPtr& msg)
{
  sensor_msgs::CvBridge bridge;
  try  {
        frame = bridge.imgMsgToCv(msg, "bgr8");
        int w,h;
        w = frame->height/6;
        h = frame->width;
        if ((mode>=1)&&(mode<=6)){              // single camera output 1-6
          out = cvCreateImage(cvSize(w, h), IPL_DEPTH_8U, 3);
          RgbImage  imgA(frame);
          RgbImage  imgB(out);
          int i = mode;                                 // image number
          for (int u=0; u<w; u++){
            for (int v=0; v<h; v++){
              imgB[v][w-u-1].b = imgA[u+(i-1)*w][v].b;
              imgB[v][w-u-1].r = imgA[u+(i-1)*w][v].r;
              imgB[v][w-u-1].g = imgA[u+(i-1)*w][v].g;
                }
          }
        }
        if (mode == 7) {                                        // mode for panorama stitching
          out = cvCreateImage(cvSize(panow, panoh), IPL_DEPTH_8U, 3);
          RgbImage imgA(frame);
          RgbImage imgB(out);
          if ((frame->height/6 == imgh) && (frame->width == imgw)) {
            int x = 0;
            int y = 0;
            unsigned int avgb, avgr, avgg, count;
            for (int u = 0; u < panoh; u++) {
              for (int v = 0; v < panow; v++) {
                count = avgb = avgr = avgg =0;
                // TODO: TP: Alpha using the look-up tables.
                for(int c=0; c<NCAMS; c++){
                  x = pLUTX->getValue(u, v, c);
                  y = pLUTY->getValue(u, v, c);
                  //&& x<imgw-1 && y!=imgh && y!=imgh*2 && y!=imgh*3 && y!=imgh*4 && y!=imgh*5 && y!=imgh*6
                  if (x >= 0 && y >= 0){
                    unsigned int a = pLUTA->getValue(u, v, c);
                    avgb = avgb + (unsigned int) imgA[y][x].b * a;
                    avgr = avgr + (unsigned int) imgA[y][x].r * a;
                    avgg = avgg + (unsigned int) imgA[y][x].g * a;
                    count++;
                  }
                }
                if (count > 0) {
                  imgB[u][v].b = (unsigned char) (avgb / 255);
                  imgB[u][v].r = (unsigned char) (avgr / 255);
                  imgB[u][v].g = (unsigned char) (avgg / 255);
                }
              }
            }
          }
          else {
            ROS_ERROR("Input image size '%ix%i' does not match the expected '%ix%i'\n",frame->width, frame->height, imgw, imgh );
            ros::shutdown();

          }
        }
    cv::WImageBuffer3_b image( out );
    sensor_msgs::ImagePtr msg_out = sensor_msgs::CvBridge::cvToImgMsg(image.Ipl(), "bgr8");  
    msg_out->header = msg->header;    // Use appropriate frame_id for separate images (mode 1..6).
    if (mode >= 1 && mode <= 6) {
      char buffer[100];
      sprintf(buffer  ,"tflbcamera%d", mode - 1);
      msg_out->header.frame_id = string(buffer);
        }
    pub.publish(msg_out);
    //msg_cam.header.frame_id = msg_out->header.frame_id;
    msg_cam.header = msg_out->header;
    msg_cam.height = out->height;
    msg_cam.width = out->width;
    camera_info_pub.publish(msg_cam);
  }
  catch (sensor_msgs::CvBridgeException& e)  {
      ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
  }
}

int main(int argc, char ** argv)
{  
  // NODE INTERFACE
  char topic_name[200]; 
  char topic_name_out[200];
  char lutpath[500];
  strcpy( topic_name,  "/camera/image");
  strcpy( topic_name_out,  "/viz/image_out");
  for(int i = 0; i < argc; ++i) {   
    if(strcmp(argv[i], "-t") == 0) {
      if(i == argc - 1) {                
          fprintf(stderr, "Missing input topic name after -t, default topic %s is used.\n", topic_name);
      }
      else {
          ++i;
          strcpy( topic_name, argv[i] );
      }
    }
    if(strcmp(argv[i], "-o") == 0) {
      if(i == argc - 1) {               
          fprintf(stderr, "Missing output topic name after -o, default topic %s is used.\n", topic_name_out);
      }
      else {
          ++i;
          strcpy( topic_name_out, argv[i] );
      }
    }
    if(strcmp(argv[i], "-m") == 0) {
      if(i == argc - 1) {               
          fprintf(stderr, "Missing mode number after -m, default topic %d is used.\n", mode);
      }
      else {
          ++i;
          mode = atoi(argv[i]);
      }
    }
    if(strcmp(argv[i], "-ph") == 0) {
        if(i == argc - 1) {
                fprintf(stderr, "Missing pano_height after -ph, default pano_height %d is used.\n", panoh);
        }
        else {
                ++i;
                panoh = atoi(argv[i]);
        }
    }
    if(strcmp(argv[i], "-pw") == 0) {
                if(i == argc - 1) {
                        fprintf(stderr, "Missing pano_width after -pw, default pano_width %d is used.\n", panow);
                }
                else {
                        ++i;
                        panow = atoi(argv[i]);
                }
        }
    if(strcmp(argv[i], "-ih") == 0) {
        if(i == argc - 1) {
                fprintf(stderr, "Missing image_height after -ih, default image_height %d is used.\n", panoh);
        }
        else {
                ++i;
                imgh = atoi(argv[i]);
        }
    }
    if(strcmp(argv[i], "-iw") == 0) {
                if(i == argc - 1) {
                        fprintf(stderr, "Missing image_width after -iw, default image_width %d is used.\n", imgw);
                }
                else {
                        ++i;
                        imgw = atoi(argv[i]);
                }
        }
    if(strcmp(argv[i], "-r") == 0) {
                if(i == argc - 1) {
                        fprintf(stderr, "Missing camera range  after -r, default camera range %d is used.\n", imgh);
                }
                else {
                        ++i;
                        panor = atoi(argv[i]);
                }
        }
    if(strcmp(argv[i], "-path") == 0) {
                if(i == argc - 1) {
                        fprintf(stderr, "Missing path to LUT files after -path");
                }
                else {
                        ++i;
                        strcpy( lutpath, argv[i] );
                }
        }
  }

  // LUT CREATION
  // Construct file names of look-up tables for x and y coordinates, and alpha.
  string LABELSLUTX[NCAMS], LABELSLUTY[NCAMS], LABELSLUTA[NCAMS];
  setlabels(LABELSLUTX, lutpath, panoh, panow, imgh, imgw, panor, 'x');
  setlabels(LABELSLUTY, lutpath, panoh, panow, imgh, imgw, panor, 'y');
  setlabels(LABELSLUTA, lutpath, panoh, panow, imgh, imgw, panor, 'a');
  CLut LUTX(panoh, panow, NCAMS);                                                               // create LUT instance for X coordinates
  CLut LUTY(panoh, panow, NCAMS);                                                               // create LUT instance for Y coordinates
  CLut LUTA(panoh, panow, NCAMS);               // create LUT instance for alpha channel
  // fill the created LUTs with data according to the labels of the source files
  LUTX.fillArray(LABELSLUTX);
  LUTY.fillArray(LABELSLUTY);
  LUTA.fillArray(LABELSLUTA);
  // enabling access of the imageCallback function to the LUTX, LUTY
  pLUTX = &LUTX;
  pLUTY = &LUTY;
  pLUTA = &LUTA;
  
  // ROS DATA EXCHANGE
  ros::init(argc, argv, "image_listener");      // ROS initialization
  ros::NodeHandle nh;                                           // NODE handle assignment
  image_transport::ImageTransport it(nh);
  image_transport::Subscriber sub = it.subscribe(topic_name, 1, imageCallback);
  image_transport::ImageTransport it2(nh);
  pub = it2.advertise(topic_name_out, 1);
  camera_info_pub = nh.advertise<sensor_msgs::CameraInfo>("/viz/camera_info", 100);
  sensor_msgs::CameraInfo msg_cam;

  ros::spin();

  return 0;
}