ros_victim1_node.cpp

Go to the documentation of this file.

#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <sensor_msgs/image_encodings.h>
#include <sensor_msgs/CameraInfo.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf/tf.h>
#include <tf/transform_listener.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>

//messages
#include <openni_cam/thermo_info.h>
#include <vision_msgs/PoseEstimate.h>
#include <sensor_msgs/PointCloud2.h>

//#include <boost/shared_ptr.hpp>

#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <image_transport/subscriber_filter.h>

//#include "IronPalette.h"

#include <QtGui/QApplication>
#include <QtGui/QLabel>
#include <QtCore/QBasicTimer>
#include <QtGui/QPainter>
#include <QtGui/QDialog>
#include <QtGui/QMainWindow>
//#include <QtGui/QMessageBox>
#include <QtGui/QKeyEvent>
#include <QtGui/QMouseEvent>
//
//
//
//#define DISPLAY
//#define MATLAB

#if defined(MATLAB) || defined(MATLAB_MEX_FILE)
#include "mex/mex_io.h"
#undef printf
#endif
//
//
//
#define NO_EXTTYPE
#include <stdio.h>
//#include "exttype/arrayn.h"
#include "exttype/intn.h"
#include "victim1_node.h"
#include "../../thermo_cam/src/IronPalette.h"
#include "geom/nmmatrix.h"
#include "dynamic/fixed_array2.h"
//
//#include <debug/logs.h>
//

#include <sstream>

/*

-lmx
-leng
-L$(MATLAB)/bin/glnx86
-L$(MATLAB)/extern/lib/glnx86
-L$(MATLAB)/sys/os/glnx86
-Wl,-rpath,$(MATLAB)/bin/glnx86
-Wl,-rpath,$(MATLAB)/extern/lib/glnx86
-Wl,-rpath,$(MATLAB)/sys/os/glnx86
-L$(MATLAB)/bin/glnxa64
-L$(MATLAB)/extern/lib/glnxa64
-L$(MATLAB)/sys/os/glnxa64
-Wl,-rpath,$(MATLAB)/bin/glnxa64
-Wl,-rpath,$(MATLAB)/extern/lib/glnxa64
-Wl,-rpath,$(MATLAB)/sys/os/glnxa64

*/


//
image_transport::Publisher publisher1;
ros::Publisher publisher2;
ros::Publisher publisher3;
//
using namespace exttype;
using namespace dynamic;
using namespace sensor_msgs;
using namespace message_filters;

//
class sources {
public:
    struct point {
        geom::vect2 x;
        float depth;
        float conf;
    };
public:
    dynamic::num_array<unsigned char,3> rgb; //rgb image [3 x width x height]
    dynamic::num_array<unsigned short int,2> depth; // [width x height]
    dynamic::num_array<unsigned short int,2> laser_image; // [width x height] but might be [0x0] if empty
//    dynamic::num_array<unsigned short,2> temp;
    //dynamic::num_array<float,2> f0temp;
    dynamic::num_array<float,2> ftemp; //not reprojected IR
    //dynamic::num_array<unsigned char,2> intftemp;
    dynamic::num_array<float,2> IR; // reprojected IR
    dynamic::num_array<float,2> resp1; // responce of human temperature likelihood
    dynamic::num_array<float,3> dresp; // response in the disparity space
    //
    dynamic::num_array<float,2> resp2; // boxed response
    dynamic::num_array<mint2,3> boxes; // boxed response
    std::vector<point> points;
    //
    std_msgs::Header h;
    //
    dynamic::num_array<int,2> fw_IR_map; // map IR cam coords -> kinect coords
    dynamic::num_array<int,2> bw_IR_map; // map IR cam coords -> kinect coords
    array_tn<geom::vect2,4> corners;
    array_tn<geom::vect2,4> Corners;
    //int seq;
    //
    geom::matrix3 K;
    geom::matrix3 Kinv;
    //
    float t_max;
    float t_min;
    float t_med;
    float r1_min;
    float r1_max;
    float r2_max;
    float max_depth;
    float min_depth;
    bool lack_depth;
    std::string sserial;
public:
    sources() {
    };
    int rgb_width()const {
        return rgb.size()[1];
    };
    int rgb_height()const {
        return rgb.size()[2];
    };
};
//________________________________________________________________
class victim1 {
public:
    sources * src;
    geom::matrix34 reproj_P;
    geom::matrix3 Pinv;
    dynamic::num_array<float,2> temp_li; // reprojected IR
    bool initialized;
    //
    victim1() {
        initialized = false;
    };
    //
    void init() {
        //std::stringstream ss;
        //ss<<"../config/<<
        //std::ifstream f("../config/ir_geom_cali_P.txt");
        std::string fname1 = std::string("../config/")+src->sserial+"_ir_geom_cali_P.txt";
        std::ifstream f(fname1.c_str());
        if(!f.is_open()) {
            throw debug_exception(std::string("cant load geometry calibration file "+fname1));
        };
        for(int i=0; i<3; ++i) {
            for(int j=0; j<4; ++j) {
                double v;
                f>>v;
                reproj_P[i][j] = v;
            };
        };
        {
            //std::ifstream f("../config/ir_geom_cali_Pinv.txt");
            std::string fname1 = std::string("../config/")+src->sserial+"_ir_geom_cali_Pinv.txt";
            std::ifstream f(fname1.c_str());
            if(!f.is_open()) {
                throw debug_exception(std::string("cant load geometry calibration file "+fname1));
            };
            for(int i=0; i<3; ++i) {
                for(int j=0; j<3; ++j) {
                    double v;
                    f>>v;
                    Pinv[i][j] = v;
                };
            };
        };
        {
            temp_li.resize(exttype::mint2(64,64));
            std::ifstream f("../config/temp_model.txt");
            if(!f.is_open()) {
                throw debug_exception("cant load geometry calibration file ../config/temp_model.txt");
            };
            for(int j=0; j<64; ++j) {
                for(int i=0; i<64; ++i) {
                    double v;
                    f>>v;
                    temp_li(i,j) = v;
                };
            };
        };
        initialized = true;
    };
    //
    void filter_ir() {
        dynamic::num_array<float,2> a = src->ftemp;
        int med_pos = a.length()/2;
        std::nth_element(a.begin(),a.begin()+med_pos,a.end());
        src->t_med = a[med_pos];
    };

    void fuse_depth() {
        if(!src->laser_image.is_empty()) {
            assert(src->laser_image.size()==src->depth.size());
            //src->depth<<-1;
            int s = src->depth.size()[0];
            for(int i=0; i<src->depth.length(); ++i) {
                //src->depth[i] = (unsigned int)(src->laser_image[i]*1000);
                //if(i/s < i%s){//
                if(src->depth[i] == 0) {
                    src->depth[i] = src->laser_image[i];//(unsigned short int)(src->laser_image[i]*1000);
                };
            };
        };
    };

    void depth_range() {
        src->min_depth = 1e6;
        src->max_depth = -1e6;
        int nnzd = 0;
        for(int i=0; i<src->depth.length(); ++i) {
            double z = src->depth(i);
            if(z>0) {
                ++nnzd;
                if(z < src->min_depth)src->min_depth = z;
                if(z > src->max_depth)src->max_depth = z;
            };
        };
        if(nnzd==0) {
            src->min_depth = 100.0;//10 cm
            src->max_depth = 5000.0;//5  m
        };
        src->lack_depth = nnzd<src->depth.length()/4;

        char s [1024];
        sprintf(s,"depth [%5.2f,%5.2f]",src->min_depth/1000.0,src->max_depth/1000.0);
        std::cout<<s;
        std::cout.flush();

        for(int l=0; l<strlen(s); ++l)std::cout<<"\b";
        std::cout.flush();

    };

    //
    void ir_reproj() {
        dynamic::num_array<int,2> next;
        dynamic::num_array<int,2> occ;
        dynamic::num_array<float,2> Z;
        //
        exttype::mint2 sz1 = src->ftemp.size();
        exttype::mint2 sz2 = src->depth.size();
        //
        src->IR.resize(sz2);
        src->fw_IR_map.resize(sz1);
        src->fw_IR_map << -1;
        src->IR << 0;
        next.resize(sz2);
        next<< -1;
        Z.resize(sz1);
        Z<<1e8;
        occ.resize(sz1);
        occ << -1;

        depth_range();

        for(int y=0; y<sz2[1]; ++y) {
            for(int x=0; x<sz2[0]; ++x) {
                double z = src->depth(x,y);
                if(z<1 && 0 && src->lack_depth) {
                    z = 5000;
                };
                z = 5000;
                if(z<1)continue;
                geom::vectn<4> p(x,y,1,1.0/z);
                geom::vectn<3> q;
                q = reproj_P*p;
                /*
                for(int k=0; k<3; ++k) {
                    int r = 0;
                    for(int l=0; l<4; ++l) {
                        r = r+reproj_P[k][l]*p[l];
                    };
                    q[k] = r;
                };
                */
                double z1 = q[2];
                q/=z1;
                int i = floor(q[0]);
                int j = floor(q[1]);
                if(i<0 || j<0 || i>= sz1[0] || j>= sz1[1])continue;
                z1 = z;
                float delta = 30/1000.0*z;// errors increase with distance
                if(z1<Z(i,j)+delta) {
                    int xy = src->IR.linindex(exttype::mint2(x,y));
                    float v = src->ftemp(i,j);
                    src->IR(x,y) = v;
                    src->fw_IR_map(i,j) = xy;
                    if(z1<Z(i,j)-delta) { //really a different surface
                        //black out all occluded pixels
                        int p = occ(i,j);
                        while(p>0) {
                            src->IR(p) = 0; // black out
                            p = next(p);
                        };
                        occ(i,j) = -1;
                    };
                    // add current point to occ
                    next(xy) = occ(i,j);
                    occ(i,j) = xy;
                };
                if(z1<Z(i,j)) {
                    Z(i,j) = z1;
                };
            };
        };
        src->corners[0] = geom::vect2(0,0);
        src->corners[1] = geom::vect2(sz1[0]-1,0);
        src->corners[2] = geom::vect2(sz1[0]-1,sz1[1]-1);
        src->corners[3] = geom::vect2(0,sz1[1]-1);
        for(int i=0; i<4; ++i) {
            //geom::vect2 v = src->geom::vect2[i];
            geom::vect3 p = src->corners[i]|1.0;//geom::vect3(v[0],v[1],1.0);
            geom::vect3 q = Pinv*p;
            q/=q[2];
            src->Corners[i] = q.sub<2>();
        };
    };
    void process1() {
        src->resp1.resize(src->IR.size());
        src->r1_min = 1e5;
        src->r1_max = -1e5;
        int t_bg = std::max(std::min(int(src->t_med),64),1)-1;
        //int t_bg = std::max(std::min(int(src->t_min),64),1)-1;
        for(int i=0; i<src->IR.length(); ++i) {
            float temp = src->IR[i];
            if(temp<1 || temp>64) {
                src->resp1[i] = -1;
            } else {
                int itemp = int(temp)-1;
                float L = temp_li(itemp,t_bg);
                src->resp1[i] = L;
            };
            if(src->resp1[i] <src->r1_min)src->r1_min = src->resp1[i];
            if(src->resp1[i] >src->r1_max)src->r1_max = src->resp1[i];
        };
    };
    void process2() {
        int D = 30;
        int delta_d = 1;
        int box_size = 50;
        src->dresp.resize(src->resp1.size()|D);
        dynamic::num_array<int,3> nn(src->dresp.size());
        src->dresp << 0;
        nn << 0;
        //src->dresp.cumsum();
        for(iter2 ii(src->resp1.size()); ii.allowed(); ++ii) {
            float z = src->depth[ii];
            if(z==0)continue;
            //float delta = 30/1000.0*z;// errors increase with distance
            //int disp = floor((1/z - 1.0/src->max_depth)/(1.0/src->min_depth -1.0/src->max_depth)*(D-1));
            int disp = floor((z - src->min_depth)/(src->max_depth -src->min_depth)*(D-1));

            if(disp<0 || disp>=D) {
                std::cout<<disp<<"\n";
                std::cout.flush();
                perror("bad disparity");
                throw debug_exception("bad disparity");
                //exit(1);
            };

            src->dresp[ii|disp] = std::max(0.0f,src->resp1[ii]);
            //src->dresp[ii|disp] = src->resp1[ii];
            nn[ii|disp] = 1;
        };
        for(int d = 0; d<D; ++d) {
            src->dresp.subdim<2>(d).cumsum();
            nn.subdim<2>(d).cumsum();
        };

        src->resp2.resize(src->resp1.size());
        src->boxes.resize(src->resp1.size()|2);
        float box = box_size*1000.0; // 10 pixels at 1000 mm
        for(iter2 ii(src->resp1.size()); ii.allowed(); ++ii) {
            //for(int i=0; i<src->resp1.length(); ++i) {
            //float z = src->depth[ii];
            //if(z==0)z = 5000; // 5m
            float z = src->depth[ii];
            //if(z==0)continue;//z = 5000; // 5m
            //float delta = 30/1000.0*z;// errors increase with distance
            //int disp = floor((1/z - 1/src->max_depth)/(1/src->min_depth -1/src->max_depth)*D);
            int disp = floor((z - src->min_depth)/(src->max_depth -src->min_depth)*(D-1));
            float w;
            if(z>0) {
                w = std::max(box/z/2,float(2.0));
            } else {
                w = 10;
            };
            mint2 l = mint2::max(mint2(0,0),mint2(ii[0]-w,ii[1]-w));
            mint2 L = mint2::min(src->resp1.size()-1, mint2(ii[0]+w,ii[1]+w));
            //
            float r = 0;
            int n = 0;
            for(int d=std::max(0,disp-delta_d); d<std::min(D-1,disp+delta_d); ++d) {
                dynamic::num_array<float,2> rs = src->dresp.subdim<2>(d);
                r += rs(l[0],l[1])+rs(L[0],L[1])-rs(L[0],l[1])-rs(l[0],L[1]);
                dynamic::num_array<int,2> rn = nn.subdim<2>(d);
                n += rn(l[0],l[1])+rn(L[0],L[1])-rn(L[0],l[1])-rn(l[0],L[1]);
            };
            //float n = std::max(1,(L-l).prod());
            n = std::max(1,n);
            src->resp2[ii] = r/10;//r/n;
            src->boxes[ii|0] = l;
            src->boxes[ii|1] = L;
        };
    };
    struct keyval {
public:
        float * val;
        int key;
        bool operator < (const keyval & b)const {
            return *val < *b.val;
        };
    };
    void nonmax_supress() {
        src->points.resize(0);
        src->points.reserve(10000);
        src->points.resize(0);
        //
        dynamic::num_array<float,2> resp2 = src->resp2;
        dynamic::fixed_array1<keyval> list(src->resp2.length());
        for(int i=0; i<src->resp1.length(); ++i) {
            list[i].key = i;
            list[i].val = &resp2[i];
        };
        src->r2_max=-1;
        while(true) {
            std::make_heap(list.begin(),list.end());
            //std::sort(list.begin(),list.end());
            while(list.size()>0 && *list.back().val < 0) {
                list.resize(list.size()-1);
            };
            std::pop_heap(list.begin(),list.end());
            keyval kv = list.back();
            list.resize(list.size()-1);
            //float & val = *list[0].val;
            if(*kv.val>src->r2_max) {
                src->r2_max = *kv.val;
            };
            if(*kv.val > 10) {
                int i = kv.key;//list[0].key;
                //save point with confidence
                mint2 ii = src->resp2.mindex(i);
                //debug::stream<<i<<"="<<ii<<"="<<resp2.linindex(ii)<<" ";
                //src->pts.push_back(pt_detection(geom::vect3(ii[0],ii[1],src->depth[i]),val));
                //src->pts.push_back(pt_detection(geom::vect3(ii[0],ii[1],src->depth[i]),val));
                mint2 l = src->boxes[ii|0];
                mint2 L = src->boxes[ii|1];
                //debug::stream<<ii<<" ["<<l<<";"<<L<<"]"<<"\n";
                //supress the neighbors
                // for(range_iter2 jj(l,L); jj.allowed(); ++jj){
                //    resp2[jj] = - 1; // supress
                //};
                //remember this point
                sources::point pt;
                pt.x = geom::vect2(ii[1],ii[0]);
                //pt.depth = src->depth[i]/1000.0; //(in meters)
                int z = src->depth[i];
                if(z>0) {
                    pt.depth = src->depth[i]/1000.0; //(in meters)
                } else {
                    pt.depth = 3;//3 m
                };
                pt.conf = *kv.val;
                src->points.push_back(pt);
                //
                for(int j2=l[1]; j2<=L[1]; ++j2) {
                    float * pv = &resp2(l[0],j2);
                    for(int j1=l[0]; j1<=L[0]; ++j1) {
                        *pv = -1;
                        ++pv;
                        //resp2(j1,j2) = -1;
                        //if(resp2.linindex(mint2(j1,j2))==i){
                        //debug::stream<<i<<"ok"<<"  ";
                        //};
                    };
                }
                if(*kv.val>0) {
                    perror("bad");
                    throw debug_exception("bad");
                };
                //val = -1;
                src->resp2[i] = src->r1_max;
            } else break;
        };
        //for(iter2 ii(src->resp1.size()); ii.allowed();++ii){
    };
    void publish_points() {
        //here loop over all remembered points, make PoseEstimate structure and send it
        for(int i=0; i<std::min((int)src->points.size(),10); ++i) {
            //Compute the outer 3D coords
            vision_msgs::PoseEstimate e;
            e.header = src->h;
            //e.header.frame_id = "openni_camera";
            //e.header.seq = src->seq;
            geom::vect3 v = src->Kinv*(src->points[i].x|1);
            e.pose.position.x = v[0];
            e.pose.position.y = v[1];
            e.pose.position.z = src->points[i].depth;
            e.pose.orientation.x = 0;
            e.pose.orientation.y = 0;
            e.pose.orientation.z = 0;
            e.pose.orientation.w = 1;
            e.id = 12345;
            e.confidence = src->points[i].conf;
            publisher2.publish(e);
            //e.pose
        };
    };
};

class QView {
public:
    QImage img;
    QLabel * label;
    QLabel * info;
public:
    int vmode;
    std::string vmodes[10];
    int count;
public:
    QView():label(0),count(0) {
        vmode = 2;

        vmodes[0] = "outline";
        vmodes[1] = "resp1";
        vmodes[2] = "resp2";
        vmodes[3] = "depth";
        vmodes[4] = "ftemp";

    };

    void init(int Width, int Height) {
        if(!label) {
            img = QImage(Width, Height, QImage::Format_RGB32);
            img.fill(QColor(0,0,0));
            label = new QLabel();
#ifndef DISPLAY
            label->setVisible(false);
#else
            label->show();
#endif
            label->setScaledContents(true);
            label->resize(Width*2,Height*2);
            info = new QLabel(label);
            info->setText("Info");
            info->move(10,10);
            info->show();
        };

    };
    ~QView() {
        if(label) {
            delete info;
            delete label;
        };
    };
    void show(const sources * src) {
        vmode = vmode % 5;
        //++count;
        int Width = src->rgb_width();
        int Height = src->rgb_height();
        if(!label)init(Width,Height);
// display image
        QRgb * pimg = (QRgb*)img.bits();
        const unsigned char * prgb = src->rgb.begin();
        //double max_depth = 1;
        //double min_depth = 1;
        //for(int i = 0; i < (Width) *(Height); ++i) {
        //    if(src->depth[i]>0) {
        //        double v = 1/(double(src->depth[i]));
        //        if(v>max_depth)max_depth = v;
        //        if(v<min_depth)min_depth = v;
        //    };
        //};
        for(int i = 0; i < (Width) *(Height); ++i) {
            const unsigned char * pixel = &prgb[i*3];
            double V = 1.0;
            pimg[i] = qRgb(pixel[0],pixel[1],pixel[2]);
            if(vmode == 0) {
                /*
                if(src->resp1[i]>0) {
                        float pixel = src->resp1[i];
                        int a = std::min(239,std::max(0,int((pixel)*240/(src->r1_max+1))));
                        pimg[i] = qRgb(Iron[a][0],Iron[a][1],Iron[a][2]);
                };
                */
            };
            if(vmode == 1) {//resp1
                if(src->resp1[i]>0) {
                    float pixel = src->resp1[i];
                    int a = std::min(239,std::max(0,int((pixel)*240/(src->r1_max+1))));
                    pimg[i] = qRgb(Iron[a][0],Iron[a][1],Iron[a][2]);
                };
            };
            if(vmode == 2) {//resp2
                if(src->resp2[i]>1) {
                    float pixel = src->resp2[i];
                    int a = std::min(239,std::max(0,int((pixel)*240/(src->r2_max+1))));
                    pimg[i] = qRgb(Iron[a][0],Iron[a][1],Iron[a][2]);
                };
            };
            /*
            if(vmode == 3) {
                if(src->IR[i]>0) {
                    float pixel = src->IR[i];
                    int a = std::min(239,std::max(0,int((pixel-src->t_min)*240/(src->t_max-src->t_min+1))));
                    pimg[i] = qRgb(Iron[a][0],Iron[a][1],Iron[a][2]);
                };
            };
            */
            if(vmode == 3) {// depth
                if(src->depth[i]>0) {
                    double v = 1/(double(src->depth[i]));
                    double V = (v-1/src->max_depth)/(1/src->min_depth-1/src->max_depth);
                    pimg[i] = qRgb(floor((V)*255),floor((V)*255),0);
                };
            };
            if(vmode == 4) {//ftemp
                geom::vect2 p = src->Corners[0];
                int a = (i%Width)-p[0];
                int b = (i/Width)-p[1];
                exttype::mint2 sz1 = src->ftemp.size();
                if(a>=0 && a<=sz1[0] && b>=0 && b<sz1[1]) {
                    float pixel = src->ftemp(a,b);
                    int v = std::min(239,std::max(0,int((pixel-src->t_min)*240/(src->t_max-src->t_min+1))));
                    pimg[i] = qRgb(Iron[v][0],Iron[v][1],Iron[v][2]);
                };
            };
        };
        if(vmode == 0) {
            int nn = 8;
            int shift[nn];
            int k=0;
            for(int k1=-1; k1<2; ++k1) {
                for(int k2=-1; k2<2; ++k2) {
                    if(k1==0 && k2==0)continue;
                    shift[k] = src->resp1.stride(k1,k2);
                    ++k;
                };
            };
            for(int j = 5; j < Height-5; ++j) {
                for(int i = 5; i < Width-5; ++i) {
                    int ii = src->resp1.linindex(mint2(i,j));
                    float v = src->resp1[ii];
                    float z = src->depth[ii];
                    int pc = 0;
                    int nc = 0;
                    for(int k=0; k<nn; ++k) {
                        float v1 = src->resp1[ii+shift[k]];
                        if(v1>0.1) {
                            ++pc;
                        };
                        if(v1<-0.1) {
                            ++nc;
                        };
                    };
                    if(pc>1 && nc>1) {
                        pimg[ii] = qRgb(0,255,0);
                    };
                    //v = src->IR[ii];
                    int pc1 = 0;
                    int pc2 = 0;
                    nc = 0;
                    for(int k=0; k<nn; ++k) {
                        float v1 = src->IR[ii+shift[k]];
                        if(v1>70) {
                            ++pc2;
                        } else if(v1>40) {
                            ++pc1;
                        };
                        if(v1<40) {
                            ++nc;
                        };
                    };
                    if(pc1>1 && nc>1) {
                        pimg[ii] = qRgb(255,255,0);
                    };
                    if(pc2>1 && (pc1+nc)>1) {
                        pimg[ii] = qRgb(255,0,0);
                    };
                };
            };
        };
        //
        QPainter painter(&img);
        //
        for(int i=0; i<4; ++i) {
            geom::vect2 v = src->Corners[(i+3)%4];
            geom::vect2 p = src->Corners[i];
            geom::vect2 q = src->Corners[(i+1)%4];
            geom::vect2 p1 = p+(v-p).normalized()*20;
            geom::vect2 p2 = p+(q-p).normalized()*20;
            painter.setPen(QPen(Qt::black, 1));
            painter.drawLine(p1[0],p1[1],p[0],p[1]);
            painter.drawLine(p[0],p[1],p2[0],p2[1]);
            painter.setPen(QPen(Qt::white, 1));
            painter.drawLine(p1[0]-1,p1[1]-1,p[0]-1,p[1]-1);
            painter.drawLine(p[0]-1,p[1]-1,p2[0]-1,p2[1]-1);
        };

        label->setPixmap(QPixmap::fromImage(img));
        {
            std::stringstream ss;
            //ss<<count;
            ss<<count<<" "<<vmodes[vmode];
            info->setText(QString("<font color='black' size='6'>")+ss.str().c_str()+QString("</font>"));
            info->adjustSize();
        };

    };
};

//___________________________________________________________

//___________________________________________________________

class myApp : public QApplication {
public:
    QView view;
    sources src;
    victim1 victim;
    int count;
    int timer_count;
    int o_count;
    //
    dynamic::num_array<sensor_msgs::ImageConstPtr ,2> msgs;
    mint3 counts;
    //
public:
    myApp(int & argc, char ** argv):QApplication(argc,argv) {
        /*
        msgs.resize(mint2(3,5));
        for(iter2 ii(msgs.size()); ii.allowed(); ++ii) {
            //msgs[ii] = 0;
        };
        counts << 0;
        */
        startTimer(1000/100);
        //victim.init();
        victim.src = &src;
        count = 0;
        timer_count = 0;
        o_count = 1;
    };
    //
    ~myApp() {
    };
    /*
    void shift_msgs() {
        for(int i=0; i<msgs.size()[0]; ++i) {
            for(int j=0; j<counts[i]-1; ++j) {
                msgs(i,j) = msgs(i,j+1);
            };
        };
        for(int i=0; i<msgs.size()[0]; ++i) {
            //msgs(i,counts[j]) = 0;
            counts[i] = std::max(0,counts[i]-1);
        };
    };
    */
    //
    /*
    void imageCallback_rgb(const sensor_msgs::ImageConstPtr & msg) {//rgb
        msgs(0,counts[0]) = msg;
        if(++counts[0] == msgs.size()[1]) { //overflow, shift all
            shift_msgs();
        };
        //std::cout<<"1"; fflush(stdout);
        //
        //++view.count;
        //src.rgb.resize(exttype::mint3(3,msg->width,msg->height));
        //memcpy(src.rgb.begin(),&msg->data[0],src.rgb.byte_size());
    };
    */
    void callback_synch(const ImageConstPtr& rgb, const ImageConstPtr& depth, const CameraInfoConstPtr& cam_info, const ImageConstPtr& ftemp, const boost::shared_ptr<openni_cam::thermo_info const> & info,const ImageConstPtr& laser) {
        //void callback_synch(const ImageConstPtr& rgb, const ImageConstPtr& depth, const CameraInfoConstPtr& cam_info, const ImageConstPtr& ftemp, const boost::shared_ptr<openni_cam::thermo_info const> & info) {
        //std::cout<<"WOW"<<"\n";
        //std::cout.flush();
        //return;
        timer_count = 0;
        {
            const sensor_msgs::ImageConstPtr & msg = rgb;
            int num_bytes = msg->step*msg->height;
            src.rgb.resize(exttype::mint3(3,msg->width,msg->height));
            memcpy(src.rgb.begin(),&msg->data[0],num_bytes);
            //src.seq = msg->header.seq;
            src.h = msg->header;
            std::cout<<msg->header.seq<<"-";
            fflush(stdout);
        }
        {
            const sensor_msgs::ImageConstPtr & msg = laser;
            int num_bytes = msg->step*msg->height;
            src.laser_image.resize(exttype::mint2(msg->width,msg->height));
            memcpy(src.laser_image.begin(),&msg->data[0],num_bytes);
            src.h = msg->header;
            std::cout<<msg->header.seq<<"-";
            fflush(stdout);
        }

        {
            int k=0;
            for(int i=0; i<3; ++i) {
                for(int j=0; j<3; ++j) {
                    src.K(i,j) = cam_info->K[k];
                    ++k;
                };
            };
            src.Kinv = src.K.invert();
        }
        {
            const sensor_msgs::ImageConstPtr & msg = depth;
            int num_bytes = msg->step*msg->height;
            src.depth.resize(exttype::mint2(msg->width,msg->height));
            memcpy(src.depth.begin(),&msg->data[0],num_bytes);
            std::cout<<msg->header.seq<<"-";
            fflush(stdout);
        };
        {
            const sensor_msgs::ImageConstPtr & msg = ftemp;
            int num_bytes = msg->step*msg->height;
            //src.f0temp.resize(exttype::mint2(msg->width,msg->height));
            //src.intftemp.resize(exttype::mint2(msg->width,msg->height));
            src.ftemp.resize(exttype::mint2(msg->width,msg->height));
            memcpy(src.ftemp.begin(),&msg->data[0],num_bytes);

            //memcpy(src.intftemp.begin(),&msg->data[0],num_bytes);
            //for(int i=0;i<src.intftemp.length();++i){
            //    src.f0temp[i] = float(src.intftemp[i]);
            //    src.ftemp[i] = float(src.intftemp[i]);
            //};
            std::cout<<msg->header.seq<<"               \n";
            //std::cout<<" "<<src.ftemp.size()[0]<<"x"<<src.ftemp.size()[1]<<"|"<<src.ftemp.byte_size()<<"\n";
            fflush(stdout);
        };
        src.t_min = info->t_min;
        src.t_max = info->t_max;
        std::stringstream ss;
        ss << info->serial;
        src.sserial = ss.str();
        //std::cout<<"\n";
        //fflush(stdout);
        //
        //
        //
        if(!victim.initialized)victim.init();
        //if(!src.depth.is_empty() && !src.rgb.is_empty() && !src.temp.is_empty()) {
        //view.show(cam);
        victim.filter_ir();
        victim.fuse_depth();
        victim.ir_reproj();
        victim.process1();
        victim.process2();
        victim.nonmax_supress();
        victim.publish_points();
        view.show(&src);
        //
        sensor_msgs::ImagePtr msg(new sensor_msgs::Image());
        msg->header.stamp = ros::Time::now();
        //
        /*
        static tf::TransformBroadcaster br;
        tf::Transform transform;
        transform.setOrigin( tf::Vector3(0, 1.0, 0) );
        transform.setRotation( tf::Quaternion(0, 0, 0) );
        br.sendTransform(tf::StampedTransform(transform, msg->header.stamp, "/base_link", "/thermo_camera"));
        */
        // publish output image

        QImage I = view.img.convertToFormat(QImage::Format_RGB888);
        int num_bytes = I.byteCount();
        msg->data.resize(num_bytes);
        memcpy(&msg->data[0],I.bits(),num_bytes);
        msg->encoding = sensor_msgs::image_encodings::RGB8;
        msg->step = I.bytesPerLine();
        msg->width = I.width();
        msg->height = I.height();
        msg->header.frame_id = cam_info->header.frame_id;
        msg->header.seq = count;
        publisher1.publish(msg);
        ++count;
    };
//
    void timerEvent(QTimerEvent * event) {
        if(!ros::ok()) {
            //ros::shutdown();
            printf("victim1_node quitting\n");
            //fflush(stdout);
            std::cout.flush();
            abort();
        };
        ros::spinOnce();
        //std::cout<<".";
        //std::cout.flush();
        ++timer_count;
        if(true) { //count<1) {
            if(timer_count % 100 ==0) {
                std::cout<<"waiting for inputs\n";
                std::cout.flush();
            };
        };
        //
    };
    //

    void save_images() {
#ifdef MATLAB
        int O_count = o_count;
        //o_count = 1;
        eng_out(src.rgb,"X");
        {
            std::stringstream ss;
            ss<<"frames{"<<o_count<<"}.rgb=X;";
            engEvalString(matlab,ss.str().c_str());
        }
        {
            eng_out(src.depth,"X");
            std::stringstream ss;
            ss<<"frames{"<<o_count<<"}.depth=X;";
            engEvalString(matlab,ss.str().c_str());
        }
        {
            eng_out(src.ftemp,"X");
            std::stringstream ss;
            ss<<"frames{"<<o_count<<"}.IR_src=X;";
            engEvalString(matlab,ss.str().c_str());
        }
        {
            // save reprojected IR
            eng_out(src.IR,"X");
            std::stringstream ss;
            ss<<"frames{"<<o_count<<"}.IR=X;";
            engEvalString(matlab,ss.str().c_str());
        }
        {
            // save reprojected IR
            eng_out(src.resp1,"X");
            std::stringstream ss;
            ss<<"frames{"<<o_count<<"}.resp1=X;";
            engEvalString(matlab,ss.str().c_str());
        };
        {
            // save reprojected IR
            eng_out(src.resp2,"X");
            std::stringstream ss;
            ss<<"frames{"<<o_count<<"}.resp2=X;";
            engEvalString(matlab,ss.str().c_str());
        };
        std::stringstream ss;
        char s[20];
        o_count = O_count;
        //
        sprintf(s,"%.4i",o_count);
        ss<<"save('capture/frames_"<<s<<".mat','frames');";
        engEvalString(matlab,ss.str().c_str());
        ++o_count;
        //if(o_count==30){
        //   o_count = 1;
        //engEvalString(matlab,"save('frames.mat','frames');");
        //};
#endif
    };

    /*
        void mousePressEvent(QMouseEvent * e){
            printf("mouse press 1\n");
        };
    */

    bool notify(QObject * receiver, QEvent * ev) {
        if(ev->type() == QEvent::MouseButtonPress) {
            QMouseEvent * e = static_cast<QMouseEvent *>(ev);
            if(e->button()==Qt::LeftButton) {
                //printf("mouse press Left\n");
                save_images();
                return true;
            };
        };
        if(ev->type() == QEvent::KeyPress) {
            QKeyEvent * e = static_cast<QKeyEvent *>(ev);
            //QMessageBox* box = new QMessageBox();
            //box->setWindowTitle(QString("Hello"));
            //box->setText(QString("You Pressed: ")+ e->text());
            //box->show();
            if(e->key() == Qt::Key_Backspace) {
                save_images();
                // Special tab handling
                return true;
            } else if(e->key() == Qt::Key_Return) {
                //++view.vmode;
                save_images();
                return true;
            } else if(e->key() == Qt::Key_Tab) {
                ++view.vmode;
                return true;
            } else if(e->key() == Qt::Key_Escape) {
                exit(0);
                return true;
            };
        };
        return QApplication::notify(receiver,ev);
    };
};

#ifndef MATLAB_MEX_FILE

int main(int argc, char ** argv) {
    ros::init(argc, argv, "victim1_node");
    ros::NodeHandle nh;
    image_transport::ImageTransport it1(nh);

    publisher1 = it1.advertise("openni_camera/detections", 1);
    publisher2 = nh.advertise<vision_msgs::PoseEstimate>("openni_camera/points", 1);

    message_filters::Subscriber<sensor_msgs::Image> image_rgb(nh, "/openni_camera/rgb", 3);
    message_filters::Subscriber<sensor_msgs::Image> image_depth(nh, "/openni_camera/depth", 3);
    message_filters::Subscriber<sensor_msgs::CameraInfo> cam_info(nh, "/openni_camera/camera_info", 3);
    message_filters::Subscriber<sensor_msgs::Image> image_ftemp(nh, "/thermo_camera/ftemp", 3);
    message_filters::Subscriber<openni_cam::thermo_info> info(nh, "/thermo_camera/info", 3);
    message_filters::Subscriber<sensor_msgs::Image> image_laser(nh, "/openni_camera/laser_image", 3);

    TimeSynchronizer<sensor_msgs::Image,sensor_msgs::Image,sensor_msgs::CameraInfo,sensor_msgs::Image,openni_cam::thermo_info,sensor_msgs::Image> sync(image_rgb,image_depth,cam_info,image_ftemp,info,image_laser, 3);
#ifdef MATLAB
    start_engine();
#endif
    //
    int argc1=0;
    myApp * a = new myApp(argc1, (char**)(0));
    sync.registerCallback(&myApp::callback_synch,a);
    a->exec();
    delete a;
};
#endif


#ifdef MATLAB_MEX_FILE

#include <signal.h>
#include <stdio.h>
#include <stdlib.h>
#include <ucontext.h>

static const char *gregs[] = {
        "GS",
        "FS",
        "ES",
        "DS",
        "EDI",
        "ESI",
        "EBP",
        "ESP",
        "EBX",
        "EDX",
        "ECX",
        "EAX",
        "TRAPNO",
        "ERR",
        "EIP",
        "CS",
        "EFL",
        "UESP",
        "SS"
};

void print_mex(int sig, siginfo_t *info, void *c)
{
        ucontext_t *context = c;

        fprintf(stderr,
                "si_signo:  %d\n"
                "si_code:   %s\n"
                "si_errno:  %d\n"
                "si_pid:    %d\n"
                "si_uid:    %d\n"
                "si_addr:   %p\n"
                "si_status: %d\n"
                "si_band:   %ld\n",
                info->si_signo,
                (info->si_code == SEGV_MAPERR) ? "SEGV_MAPERR" : "SEGV_ACCERR",
                info->si_errno, info->si_pid, info->si_uid, info->si_addr,
                info->si_status, info->si_band
        );

        fprintf(stderr,
                "uc_flags:  0x%x\n"
                "ss_sp:     %p\n"
                "ss_size:   %d\n"
                "ss_flags:  0x%X\n",
                context->uc_flags,
                context->uc_stack.ss_sp,
                context->uc_stack.ss_size,
                context->uc_stack.ss_flags
        );

        fprintf(stderr, "General Registers:\n");
        for(int i = 0; i < 19; i++)
                fprintf(stderr, "\t%7s: 0x%x\n", gregs[i], context->uc_mcontext.gregs[i]);
//      fprintf(stderr, "\tOLDMASK: 0x%x\n", context->uc_mcontext.oldmask);
//      fprintf(stderr, "\t    CR2: 0x%x\n", context->uc_mcontext.cr2);

        throw debug_exception("Segmentation Fault");
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
    using namespace exttype;

//    struct sigaction sa;
//     sa.sa_sigaction=print_mex;// warning here: assignment from incompatible pointer type
//     sa.sa_flags=SA_SIGINFO;
//     sigaction(SIGSEGV,&sa,NULL);


    try {
        if(nrhs!=3) {
            mexErrMsgTxt((std::string("[resp1 resp2] = emaxflow_mex(rgb,depth,ftemp)::input error:\n")+"\n"+
                          "Input: uint8 rgb[3 x width x height], uint16 depth[width x height], float ftemp [width x neight].\n"+
                          "Output: float resp1[ width x height], float resp1[ width x height];").c_str());
            return;
        };

        sources * src = new(sources);
        victim1 * victim = new(victim1);
        victim->src = src;

        src->rgb = mx_array<unsigned char,3>(prhs[0]); // rgb image [3 x width x height]
        src->depth = mx_array<unsigned short int,2>(prhs[1]); // [width x height]
        src->IR = mx_array<float,2>(prhs[2]); // temperature [width x height]
        //
        src->t_max = src->IR.max().first;
        //mexPrintf("Ok\n");
        src->sserial = "12090028";
        //return;
        if(!victim->initialized)victim->init();
        //victim->filter_ir(); // computes t_med;
        {
            dynamic::num_array<float,2> a = src->IR;
            int nz=0;
            src->t_min = src->t_max;
            for(int i=0;i<src->IR.length();++i){
                float t = src->IR[i];
                if(t==0){
                    ++nz;
                    a[i] = -1000;
                };
                if(t<src->t_min){
                    src->t_min = t;
                };
            };
            int med_pos = nz+(a.length()-nz)/2;
            std::nth_element(a.begin(),a.begin()+med_pos,a.end());
            src->t_med = a[med_pos];
        };
        victim->depth_range();
        // detection

        victim->process1();
        victim->process2();
        victim->nonmax_supress();

        //
        plhs[0] = mx_array<float,2>(src->resp1).get_mxArray_andDie();
        plhs[1] = mx_array<float,2>(src->resp2).get_mxArray_andDie();

        delete victim;
        delete src;

    } catch(const std::exception & e) {
        mexErrMsgTxt(e.what());
    };
};

#endif