openni_test1.cpp

Go to the documentation of this file.

#include "openni_cam.h"

#include <stdio.h>
//#include "error.h"

#include "../../thermo_cam/src/camera.h"
#include "../../thermo_cam/src/error.h"
#include "../../thermo_cam/src/include/Imager.h"
#include "../../thermo_cam/src/ImagerUVC.h"
#include "../../thermo_cam/src/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 <string>
#include <iostream>
#include <fstream>
#include <time.h>

#include <cmath>

#include "mex/mex_io.h"
#undef printf

#include <sstream>
#include <pthread.h>


#define NO_EXTTYPE
#include "dynamic/num_array.h"
#include "dynamic/fixed_array2.h"
#include "geom/nmmatrix.h"
#include "dynamic/dynamic.h"
#include "debug/performance.h"

using namespace optris;

class thermo_node {
public:
    //ImagerUVC uvc;
    camera cam;
    Imager * imager;
    unsigned long serial;
    int Width;
    int Height;
    int count;
    bool frame_acquired;
public:
    QLabel * L2;
    QImage im2;
    QLabel * info2;
    float tmax;
    float tmin;
public:

    dynamic::num_array<unsigned short int, 2> IR;
    //unsigned char * buff;

    thermo_node() {
    };

    ~thermo_node() {
        delete info2;
        delete L2;
        delete imager;
    };
    void init(fptrOptrisFrame pOnFrame) {
        cam.find_device();
        cam.init_device();
        Width = cam.width;
        Height = cam.height-1;
        IR.resize(exttype::mint2(Width,Height));
        serial = cam.dev_serial;
        int frequency = cam.frequency;

        imager = new Imager(serial, Width, Height+1, frequency, "../../thermo_cam/data/config/12050017.xml");
        count = 0;
        //imager->setAutoflagControl(true);
//        uvc.Start();
        //buff = new unsigned char[Width*(Height+1)*2];
        imager->setFrameCallback(pOnFrame);
        frame_acquired = false;
        //
        //
        im2 = QImage(Width, Height, QImage::Format_RGB32);
        im2.fill(QColor(0,0,0));
        L2 = new QLabel();
        L2->setScaledContents(true);
        L2->resize(Width*4,Height*4);
        L2->show();
        info2 = new QLabel(L2);
        info2->setText("Info");
        info2->move(10,10);
        info2->show();
    };
    void triger() {
        //frame_acquired = false;
        ++count;
        cam.get_image();
        imager->process((unsigned char *)cam.buffer.start);
        //
        std::stringstream ss;
        ss<<count;
        info2->setText(QString("<font color='white' size='6'>")+ss.str().c_str()+QString("</font>"));
        info2->adjustSize();
        return;
        if(!frame_acquired){
        printf("Thermocam init: ");
        while(!frame_acquired){
            ++count;
//            printf("\b\b\b\b\b\b%4.4i  ",(count%10000));
//            fflush(stdout);
            const char * psym = "-\\|/";
            if(count>1)std::cout<<"\b";
            std::cout<<psym[count % 4];
            fflush(stdout);
            usleep(100);
        };
        printf("\n");
        };
    };

    void show(){
        tmax=-1e6;
        tmin=+1e6;
        QRgb * pimg = (QRgb*)im2.bits();

        float * vals = new float[Width*Height];

        unsigned short * pV = IR.begin();

        for(int i = 0; i < (Width) * (Height); ++i) {
            vals[i] = ((float)(pV[i])-1000.0)*0.1;
        };

        float * vals1 = new float[Width*Height];

        //median filter
        std::vector<float> a;
        a.resize(9);
        memset(vals1,0,Width*Height*sizeof(float));
        for(int i=1; i<Height-1; ++i) {
            for(int j=1; j<Width-1; ++j) {
                int ii = i*Width+j;
                a[0] = vals[ii];
                a[1] = vals[ii+1];
                a[2] = vals[ii-1];
                a[3] = vals[ii-Width];
                a[4] = vals[ii+Width];
                a[5] = vals[ii-1-Width];
                a[6] = vals[ii-1+Width];
                a[7] = vals[ii+1-Width];
                a[8] = vals[ii+1+Width];
                std::nth_element(a.begin(),a.begin()+5,a.end());
                float pixel = a[5];
                //float pixel = vals[ii];
                vals1[ii] = pixel;
                if(pixel > tmax)tmax = pixel;
                if(pixel < tmin)tmin = pixel;
            };
        };
        for(int i = 0; i < (Width) * (Height); ++i) {
            float pixel = vals1[i];
            int a = std::min(239,std::max(0,int((pixel-tmin)*240/(tmax-tmin+1))));
            pimg[i] = qRgb(Iron[a][0],Iron[a][1],Iron[a][2]);
        };
        L2->setPixmap(QPixmap::fromImage(im2));
        delete vals1;
        delete vals;
    };

    void cbOnFrame(unsigned short* image, unsigned int w, unsigned int h) {
        //memcpy(buff,image,Width*Height*2);
        frame_acquired = true;
        memcpy(IR.begin(),image,Width*Height*2);
        show();
    };
};

thermo_node tcam;

class myApp : public QApplication {
public:
    openni_cam::openni_cam cam;
//
    int Width;
    int Height;
    QImage img;
    QLabel label;
    QLabel * info;
    int count;
    int framerate;
    int o_count;
    geom::matrix34 P;
    dynamic::num_array<float,2> IR;
    bool record;
    int r_count;
public:
    myApp(int & argc, char **argv):QApplication(argc,argv) {
        //
        std::ifstream f("../config/ir_geom_cali_P.txt");
        for(int i=0; i<3; ++i) {
            for(int j=0; j<4; ++j) {
                double v;
                f>>v;
                P[i][j] = v;
            };
        };
        //

        cam.init();
        Width = cam.rgb.size()[1];
        Height = cam.rgb.size()[2];
        IR.resize(exttype::mint2(Width,Height));

        img = QImage(Width, Height, QImage::Format_RGB32);
        img.fill(QColor(0,0,0));
        label.setScaledContents(true);
        label.resize(Width*2,Height*2);
        label.show();
        info = new QLabel(&label);
        info->setText("Info");
        info->move(10,10);
        info->show();
        std::cout<<"\n";
        framerate = 20;
        startTimer(1000/framerate);
        count = 0;
        o_count = 0;
        record = false;
    };
    ~myApp() {
    };

        class tpoint;
        class tpoint{
            public:
            exttype::mint2 pt;
            tpoint * next;
        };

    void ir_reproj() {
        dynamic::fixed_array2<tpoint> points;
        //
        dynamic::num_array<tpoint *,2> occ;
        //
        dynamic::num_array<float,2> Z;
        //
        points.resize(IR.size());
        IR << 0;
        Z.resize(tcam.IR.size());
        Z<<1e8;
        occ.resize(tcam.IR.size());
        occ<<0;
        //occ<<exttype::mint2(-1,-1);
        for(int y=0; y<IR.size()[1]; ++y) {
            for(int x=0; x<IR.size()[0]; ++x) {
                double z = cam.depth(x,y);
                if(z<1)continue;
                geom::vectn<4> p(x,y,z,1);
                geom::vectn<3> q;
                q = P*p;
                for(int k=0; k<3; ++k) {
                    int r = 0;
                    for(int l=0; l<4; ++l) {
                        r = r+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>= tcam.IR.size()[0] || j>= tcam.IR.size()[1])continue;
                float delta = 30/1000.0*z;// errors increase with distance
                if(z1<Z(i,j)+delta) {
                    //float v = (float(tcam.IR(tcam.IR.size()[0]-i-1,j))-1000.0)*0.1;
                    float v = (float(tcam.IR(i,j))-1000.0)*0.1;
                    IR(x,y) = v;
                    if(z1<Z(i,j)-delta){//really a different surface
                        //black out all occluded pixels
                        tpoint * p = occ(i,j);
                        while(p){
                            IR[p->pt] = 0; // black out
                            p = p->next;
                        };
                        occ(i,j) = 0;
                    };
                    // add current point to occ
                    tpoint * p = & points[exttype::mint2(x,y)];
                    p->pt = exttype::mint2(x,y);
                    p->next = occ(i,j);
                    occ(i,j) = p;
                };
                if(z1<Z(i,j)) {
                    Z(i,j) = z1;
                };
            };
        };
    };

    /*
        void keyPressEvent(QKeyEvent* e){
              QMessageBox* box = new QMessageBox();
              box->setWindowTitle(QString("Hello"));
              box->setText(QString("You Pressed: ")+ e->text());
              box->show();
        };
    */

    void save_images() {
        dynamic::num_array<unsigned char,4> rec;
        dynamic::num_array<double,1> tt;
        int n_frames = 300;
        rec.resize(exttype::mint4(3,Width,Height,n_frames));
        tt.resize(n_frames);
        debug::PerformanceCounter c1;
        c1.start();
        for(int i=0;i<n_frames;++i){
            cam.get_image();
            tt(i) = c1.time();
            memcpy(&rec(0,0,0,i),cam.rgb.begin(),cam.rgb.byte_size());
        };
        std::cout<<" recording ";
        //fflush(stdout);
        //imager->process((unsigned char *)cam.buffer.start);
        {
            eng_out(rec,"rec");
            eng_out(tt,"tt");
            //eng_out(src.rgb,"X");
            {
                std::stringstream ss;
                ss<<"frames{"<<o_count+1<<"}.brust=rec;";
                engEvalString(matlab,ss.str().c_str());
            };
            {
                std::stringstream ss;
                ss<<"frames{"<<o_count+1<<"}.tt=tt;";
                engEvalString(matlab,ss.str().c_str());
            };
            //engEvalString(matlab,"a=1");
        }
        std::stringstream ss;
        char s[20];
        //
        sprintf(s,"%.4i",o_count);
        ss<<"save('../data/capture/frames_"<<s<<".mat','frames');";
        //ss<<"save('frames_"<<s<<".mat','frames');";
        engEvalString(matlab,ss.str().c_str());
        std::cout<<"ok\n";
        fflush(stdout);
        ++o_count;
        /*
        eng_out(cam.rgb,"X");
        {
            std::stringstream ss;
            ss<<"frames{"<<o_count<<"}.rgb=X;";
            engEvalString(matlab,ss.str().c_str());
        }
        {
            eng_out(cam.depth,"X");
            std::stringstream ss;
            ss<<"frames{"<<o_count<<"}.depth=X;";
            engEvalString(matlab,ss.str().c_str());
        }
        {
            eng_out(tcam.IR,"X");
            std::stringstream ss;
            ss<<"frames{"<<o_count<<"}.IR_src=X;";
            engEvalString(matlab,ss.str().c_str());
        }
        {// save reprojected IR
            eng_out(IR,"X");
            std::stringstream ss;
            ss<<"frames{"<<o_count<<"}.IR=X;";
            engEvalString(matlab,ss.str().c_str());
        }
        engEvalString(matlab,"save('frames.mat','frames');");
        ++o_count;
        */
    };

       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);
            if(e->key() == Qt::Key_Return) {
                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);
    };


/*
    bool notify( QObject * receiver, QEvent * ev) {
        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_Space) {
                save_images();
                // Special tab handling
                return true;
            } else if (e->key() == Qt::Key_Escape) {
                exit(0);
                return true;
            } else if (e->key() == Qt::Key_R) {
                record = !record;
                if(record)r_count = 0;
                return true;
            } else {
                return QApplication::notify(receiver,ev);
            };
            //    return false;
        };
        return QApplication::notify(receiver,ev);
    }
*/

    void timerEvent( QTimerEvent * event ) {
        //
        int recali_time = 30;
        //
        int recali_period = recali_time*framerate;
        if(count%recali_period==0){
            tcam.imager->forceRecalibration();
                };

        tcam.triger();

        ++count;
        cam.get_image();
        std::stringstream ss;
        ss<<"count="<<count<<";";
        engEvalString(matlab,ss.str().c_str());
        //engEvalString(matlab,"pmode client2lab count 1");
        //engEvalString(matlab,"bla=1;");
        //
        //
        //

        ir_reproj();
        //
        const char * psym = "-\\|/";
        //if(count>1)std::cout<<"\b";
        //std::cout<<psym[count % 4];
        //fflush(stdout);

        // display image
        QRgb * pimg = (QRgb*)img.bits();
        unsigned char * prgb = cam.rgb.begin();
        double max_depth = -1e5;
        double min_depth = 1e5;
        for(int i = 0; i < (Width) * (Height); ++i) {
            if(cam.depth[i]>0) {
                double v = 1/(double(cam.depth[i]));
                if(v>max_depth)max_depth = v;
                if(v<min_depth)min_depth = v;
            };
        };
        for(int i = 0; i < (Width) * (Height); ++i) {
            unsigned char * pixel = &prgb[i*3];
            //pimg[i] = qRgb(pixel[0],pixel[1],pixel[2]) + qRgb();
            double V = 1.0;
            if(0 & cam.depth[i]>0) {
                double v = 1/(double(cam.depth[i]));
                //V = floor((1.0 - (v-min_depth)/(max_depth-min_depth))*255);
                V = (v-min_depth)/(max_depth-min_depth);
                pimg[i] = qRgb(floor((V)*255),floor((V)*255),0);
                //pimg[i] = qRgb(std::max(std::min(int(floor((0.5-0.5*V)*255+(0.5*V+0.5)*pixel[0])),255),0),std::max(std::min(int(floor((0.5-0.5*V)*255+(0.5*V+0.5)*pixel[1])),255),0),floor(V*pixel[2]));
                //pimg[i] = qRgb(std::max(std::min(int(floor((1-V)*255)),255),0),std::max(std::min(int(floor((1-V)*255)),255),0),0);
            } else {
                pimg[i] = qRgb(pixel[0],pixel[1],pixel[2]);
            };

            if(IR[i]>0) {
                float pixel = IR[i];
                int a = std::min(239,std::max(0,int((pixel-tcam.tmin)*240/(tcam.tmax-tcam.tmin+1))));
                pimg[i] = qRgb(Iron[a][0],Iron[a][1],Iron[a][2]);
            };

            //pimg[i] = qRgb(std::max(std::min(int(floor((1-V)*255)),255),0),std::max(std::min(int(floor((1-V)*255)),255),0),pixel[2]);
        };
        label.setPixmap(QPixmap::fromImage(img));
        {
            std::stringstream ss;
            ss<<count;
            info->setText(QString("<font color='black' size='6'>")+ss.str().c_str()+QString("</font>"));
            info->adjustSize();
        };
        if(record){
            if(r_count==0){
                engEvalString(matlab,"rec = {};");
            };
            dynamic::num_array<unsigned char,3> wimg;
            wimg.set_ref(img.bits(),exttype::mint3(4,Width,Height));
            eng_out(wimg,"X");
            std::stringstream ss;
            ++r_count;
            ss<<"rec{"<<r_count<<"}=X;";
            engEvalString(matlab,ss.str().c_str());
        }
    };
};


myApp * the_app;

void cbOnFrame(unsigned short* image, unsigned int w, unsigned int h) {
    tcam.cbOnFrame(image,w,h);
};


void eng_thread() {
    //engEvalString(matlab, "desktop");
    /*
    engSetVisible(matlab, true);
    engEvalString(matlab, "commandwindow;");
    */
    //engEvalString(matlab,"com.mathworks.mde.desk.MLDesktop.getInstance.showCommandWindow;");
    //exit(1);

    char str[1024];
    int BUFSIZE = 1<<20;//1Mb
    char buffer[BUFSIZE+1];
    engOutputBuffer(matlab, buffer, BUFSIZE);
    while(1) {
        buffer[0] = '\0';
        fgets(str, 1024, stdin);
        //str[strlen(str)-1]=';';
        engEvalString(matlab, str);
        printf("%s", buffer);
    };
};

pthread_t tid;


int main (int argc, char *argv[]) {
    myApp a(argc, argv);
    tcam.init(cbOnFrame);
    start_engine();
    //engEvalString(matlab, "pmode start 1");
    //engEvalString(matlab, "commandwindow;");
    int err = pthread_create(&tid, NULL, &eng_thread, NULL);
    if(err!= 0)exit(1);
    //engEvalString(matlab, "desktop");
    //engEvalString(matlab, "figure(1); plot(sin([0:0.1:3]));");
    //engEvalString(matlab,"X=magic(3);");
    //engEvalString(matlab,"a=1;");
    //sleep(20);
    /*
    for(int i=0;;++i){
        std::stringstream ss;
        ss<<"b="<<i<<";";
        //std::cout<<i<<"\n";
        engEvalString(matlab,ss.str().c_str());
        engEvalString(matlab,"pmode client2lab b 1");
        usleep(100);
    };
    */
    return a.exec();
};