Robotics Service Bus

#include <iostream>

#include <cstdlib>

#include <math.h>

#include <time.h> 

#include <rsb/Factory.h>

#include <boost/program_options.hpp>

#include <opencv/cv.h>

#include <opencv/highgui.h>

#include <rsb/converter/Repository.h>

#include "HandDetectionHSV.h"

#include "HoughLines2.h"

#include "Util.h"

#include "ChessDetection.h"

#include "BoardLocalizationLines.h"

#include "StrongFeaturesToTrackKeyPoints.h"

#include "FieldOccCanny.h"

#include "FieldOccHoughCircles.h"

#include "IFieldOccDetection.h"

#include "DataReader.h"

#include "ResultPublisher.h"

#ifdef USE_IMAGES_INSTEAD_OF_IMAGEFIXATIONS

#include <rst/converters/opencv/IplImageConverter.h>

#else

#include <rst/converters/opencv/IplImageConverter.h> // We need both here!!

#include <ChessBase\ImageFixationConverter.h>

#endif

using namespace std;

using namespace cv;

namespace po = boost::program_options;

void handleCommandline(int argc, char *argv[]);

string videoPath =  "../../data/thomas-1-recording-fixed.avi";

#ifdef USE_IMAGES_INSTEAD_OF_IMAGEFIXATIONS

        string inScope = "/video";

#else

        string inScope = "/imagefixations";

#endif

string outScopeOccFields = "/occupiedFields";

string outScopeFixatedField = "/fixatedField";

int skipframe = 3*60*30;//skip 3 minutes

int debugMode = 0;

int recordMode = 0;

int videoMode = 0;

int main(int argc, char *argv[]) {

        cout << "chessboardfinder" << endl;

        handleCommandline(argc, argv);

        ostringstream resultdat;

        DataReader dataReader = DataReader();

        //boost::shared_ptr< rsb::converter::ProtocolBufferConverter<rst::generic::ListFloat> >

   //    converter(new rsb::converter::ProtocolBufferConverter<rst::generic::ListFloat>());

   // rsb::converter::converterRepository<std::string>()->registerConverter(converter);

        ResultPublisher resultPublisher(outScopeOccFields,outScopeFixatedField);

        resultPublisher.setupRSB();

        VideoWriter record;

        if(videoMode){

                dataReader.initVidCap(videoPath,skipframe);

        }else{ 

                dataReader.initRSB(inScope);

        }

        if(recordMode){

                resultdat << "../../data/result-" << time(NULL) << ".avi";

                std::string resfile = resultdat.str();

                record = VideoWriter(resfile, CV_FOURCC('D','I','V','X'), 15,Size(400,400), true);

        }

        cv::Mat frame;

#ifndef USE_IMAGES_INSTEAD_OF_IMAGEFIXATIONS

        Fixation fixation;

#endif

        clock_t start, end;

        cout << "Finished preparation" << endl;

        ChessDetection chessDetect = ChessDetection();

        HandDetectionHSV handDetect = HandDetectionHSV();

        handDetect.setSkinPercentage(0.000000000001);

        BoardLocalizationLines boardDetect = BoardLocalizationLines();

        StrongFeaturesToTrackKeyPoints keyPointDetect = StrongFeaturesToTrackKeyPoints();

        vector<pair<IFieldOccDetection*,double>> fieldOccAlgos;

        FieldOccCanny fieldOccCanny = FieldOccCanny();

        fieldOccAlgos.push_back(pair<IFieldOccDetection*,double>(&fieldOccCanny,1.0));

        FieldOccHoughCircles fieldOccHoughCircles = FieldOccHoughCircles();

        fieldOccAlgos.push_back(pair<IFieldOccDetection*,double>(&fieldOccHoughCircles,3.0));

        ImageManager imageManagerInput;

        ImageManager imageManagerUndistorted;

        for(;;){

                if(videoMode){

                        if(!dataReader.readImageFile(&frame))

                                break;

                }else{

                        cout << "PREPOP" << endl;

#ifdef USE_IMAGES_INSTEAD_OF_IMAGEFIXATIONS

                        if(!dataReader.readImageRSB(&frame))

                                break;

#else

                        if(!dataReader.readImageFixationRSB(&frame, &fixation))

                                break;

                        cout << "Yippie, got frame and fixation (Start " << fixation.start << ")" << endl;

#endif

                        cout << "POPPED" << endl;

                }

                //-----------------------------

                //preprocessing

                cout << "//////////////////////////////////////////////////////" << endl;

                //cout << "frame: " << vidCap.get(CV_CAP_PROP_POS_FRAMES) << endl;

                clock_t time_full;                

                clock_t start_full = clock();

                //do processingsteps

                imageManagerInput.setRgbImage(frame);

                //start processing

                bool handinside = chessDetect.detectHands(handDetect,imageManagerInput);

                if(handinside){

                        cout << "skip processing because of hand" << endl;

                        imshow("video", frame); if(cv::waitKey(30) >= 0) break;

                        continue;

                } 

                vector<Point2f> keyPointsSFTT = chessDetect.detectKeyPoints(keyPointDetect,imageManagerInput);

                vector<Point2f> chessBoardCornersSrc = chessDetect.detectChessboardPos(boardDetect,imageManagerInput);

                Mat transformationMat = chessDetect.warpPerspectiveMat(imageManagerInput,&imageManagerUndistorted,chessBoardCornersSrc,Size(400,400),0);

                //cout << "transformationMat " << transformationMat << endl;

                cout << "corners " << Mat(chessBoardCornersSrc) << endl;

                Mat undistorted = imageManagerUndistorted.getRgbImage();

                vector<Point2f> newSFTTUndistorted(keyPointsSFTT.size());

                perspectiveTransform(boardDetect.getLineIntersectionsPoints(),newSFTTUndistorted,transformationMat);

                cv::flann::KDTreeIndexParams indexParams(5);

                Mat query(1,2,CV_32FC1);

                Mat pointMat = Mat(newSFTTUndistorted).reshape(1);

                flann::Index kdtree(pointMat, indexParams);

                vector<Point2f> singleQuery;

                Mat index(1, 4, CV_32S);

                Mat dist(1, 4, CV_32F);

                RNG rng(12345);

                Mat undistBW(400,400,CV_32FC1);

                Mat thMatB(undistorted.size(),(undistorted.depth(), 1));

                Mat undistGray;

                Mat undistorted2 = undistorted.clone();

                cvtColor( undistorted2, undistGray, CV_RGB2GRAY );

                Mat undistGrayC = undistGray.clone();

                threshold(undistGray,thMatB,255,255,THRESH_BINARY);

                Mat undistGrayE;

                imageManagerUndistorted.setRgbImage(undistorted);

                //combine multiple fieldOccupiedVariants

                vector<double> tmp(64);

                fill(tmp.begin(),tmp.end(),0);

                Mat combined(tmp);

                double sumWeights=0;                

                for(vector<pair<IFieldOccDetection*,double> >::iterator fieldOccDect = fieldOccAlgos.begin(); fieldOccDect != fieldOccAlgos.end(); ++fieldOccDect)

                {

                        vector<double> occupiedFields = chessDetect.occupiedFields(*(fieldOccDect->first),imageManagerUndistorted);

                        add(Mat(combined),Mat(occupiedFields)*fieldOccDect->second,combined);

                        sumWeights += fieldOccDect->second;

                }

                combined = combined / sumWeights;

                cout << "combined" << combined << endl;

                vector<float> resultOccFields;

                combined.col(0).copyTo(resultOccFields);

                cout << "resultOccFields.size() " << resultOccFields.size() << endl;

                if(resultOccFields.size()==64)

                        resultPublisher.sendResultOccFields(resultOccFields);//TODO buggy - runtimeError

                Point2f fixatedPoint(fixation.locationX,fixation.locationY);

                vector<Point2f> tmp1,tmp2;

                tmp1.push_back(fixatedPoint);

                perspectiveTransform(tmp1,tmp2,transformationMat);

                fixatedPoint = tmp2[0];

                int fixationFieldX = fixatedPoint.x / 400 *8;

            int fixationFieldY = fixatedPoint.y / 400 *8;

                int fixatedFieldID = fixationFieldX + 8 * fixationFieldY;

                resultPublisher.sendResultFixatedField(fixatedFieldID);

                circle(undistorted, fixatedPoint,5,Scalar(255,0,0),-1);

                //undistGrayC.convertTo(undistGrayC, -1, 2.0f, 10);

                /*int histSize = 60;

                /// Set the ranges ( for B,G,R) )

                float range[] = { 0, 255 } ;

                const float* histRange = { range };

                bool uniform = true; bool accumulate = false;

                Mat hist;

                /// Compute the histograms:

                calcHist( &undistGray, 1, 0, Mat(), hist, 1, &histSize, &histRange, uniform, accumulate );

                int hist_w = 512; int hist_h = 400;

                int bin_w = cvRound( (double) hist_w/histSize );

                Mat histImage( hist_h, hist_w, CV_8UC3, Scalar( 0,0,0) );

                /// Normalize the result to [ 0, histImage.rows ]

                normalize(hist, hist, 0, histImage.rows, NORM_MINMAX, -1, Mat() );

                /// Draw for each channel

                for( int i = 1; i < histSize; i++ )

                {

                cout << "hist.at<float>(i) = " << hist.at<float>(i) << endl;

                if(hist.at<float>(i) > 0.25 * hist_h){

                line( histImage, Point( bin_w*(i), hist_h - cvRound(hist.at<float>(i)) ),Point( bin_w*(i),hist_h ) ,

                Scalar( 255, 0, 0), 3, 8, 0  );

                }

                }

                for( int i = 1; i < histSize; i++ )

                {

                line( histImage, Point( bin_w*(i-1),hist_h - cvRound(hist.at<float>(i-1)) ) ,

                Point( bin_w*(i), hist_h - cvRound(hist.at<float>(i)) ),

                Scalar( 255, 0, 0), 3, 8, 0  );

                }

                /// Display

                namedWindow("calcHist Demo", CV_WINDOW_AUTOSIZE );

                imshow("calcHist Demo", histImage );*/

                /*cout << "newPointsUndistorted = " << newSFTTUndistorted.size() << endl; 

                equalizeHist(undistGray,undistGrayE);

                if(newSFTTUndistorted.size() == 81){

                //single field processing

                for(int i = 0; i < 8; i++){

                Rect roiLine(i*50,0,50,400);

                //equalizeHist(undistGray(roiLine),undistGray(roiLine));                        

                for(int j = 0; j < 8; j++){

                bool whiteField = i%2 ==j%2;

                //cout << "field (" << i << "|"<<j<<") processed" << endl;

                Point2f center = Point2f(400 / 8 * i + 400/8/2,400 / 8 * j + 400/8/2);

                singleQuery.clear();

                singleQuery.push_back(center);

                Mat singleQuerMat = Mat(singleQuery).reshape(1);

                kdtree.knnSearch(singleQuerMat, index, dist, 4, cv::flann::SearchParams(64));

                Point2f max(0,0),min(400,400);

                Scalar color(  rng.uniform(0,255),  rng.uniform(0,255),  rng.uniform(0,255) );

                for(int k = 0; k < 4; k++)

                {

                Point2f startP = newSFTTUndistorted[index.at<int>(0,k)];

                if(startP.x+startP.y > max.x+max.y )max = startP;

                if(startP.x+startP.y < min.x+min.y )min = startP;

                }

                Rect roi(min+Point2f(7,9),max-Point2f(7,3)) ;

                Mat imgRoi = undistorted(roi);

                Mat grayRoi = undistGray(roi);

                Rect roi2(roi);

                roi2.height /= 2;

                Mat imgRoi2 = undistorted(roi2);

                Mat grayRoi2 = undistGray(roi2);

                //not working cause of ilumination?

                Mat grayRoiE = undistGrayE(roi2);

                Mat thMat = thMatB(roi2);

                int th = 30;

                if(whiteField){

                th = 150;

                //cout << "white field" << endl;

                threshold(grayRoiE,thMat,th,255,THRESH_BINARY_INV);

                }

                else{

                th = 140;

                //cout << "black field" << endl;

                threshold(grayRoiE,thMat,th,255,THRESH_BINARY);

                }

                int diffPixel = countNonZero(thMat);

                std::stringstream stst3 ;

                stst3 << diffPixel;

                Scalar m2;

                Scalar stddev;

                meanStdDev(grayRoi2,m2,stddev);

                //cout << "stddev= " << stddev[0] << endl;

                //        if(stddev[0] > 10)

                //        equalizeHist(grayRoi2,grayRoi2);

                //cvtColor( imgRoi, grayRoi, CV_RGB2GRAY );

                Scalar m = mean(grayRoi);

                //TODO doing anything?

                std::stringstream stst2 ;

                stst2 << (int)m[0];

                int th_canny =50;

                if(m[0]<70 || !whiteField){

                //grayRoi.convertTo(grayRoi, -1, 2.0f, 10);

                //imgRoi.convertTo(imgRoi, -1, 2.0f, 10);

                //circle( undistorted, center, 6, Scalar(255,255,0), 3, 8, 0 );

                th_canny = 40;

                }

                //Mat abc = undistGray(roi);

                //Mat abc2 = undistBW(roi);

                // threshold( abc,abc2 ,128,255,CV_THRESH_BINARY | CV_THRESH_OTSU);        

                // cout << "th succes"  << endl;

                //vector<vector<Point> > contours;

                //        vector<Vec4i> hierarchy;

                //        contours.clear();

                //        Mat testRoi = undistBW(roi);

                //        findContours(testRoi , contours, hierarchy,

                //                CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE );

                //        // iterate through all the top-level contours,

                //        // draw each connected component with its own random color

                //        cout << "calc success" << endl;

                //        int idx = 0;

                //        for( ; idx >= 0; idx = hierarchy[idx][0] )

                //        {

                //                Scalar color( rand()&255, rand()&255, rand()&255 );

                //                drawContours( undistorted2(roi), contours, idx, color, CV_FILLED, 8, hierarchy );

                //        }

                Mat cannyRoi;

                //cvtColor( imgRoi2, grayRoi2, CV_RGB2GRAY );

                Canny(grayRoi2,cannyRoi,40,80);

                int canny_nz = countNonZero(cannyRoi);

                std::stringstream stst ;

                stst << canny_nz ;

                vector<Vec3f> circles;

                HoughCircles(grayRoi,circles,CV_HOUGH_GRADIENT, 1, grayRoi.rows/8, 25, 18, 10, 0 );

                if(stddev[0]>10  || canny_nz > th_canny){ //circles.size() > 0 ||

                for( size_t i = 0; i < circles.size(); i++ )

                {

                Point centerP(cvRound(circles[i][0]), cvRound(circles[i][1]));

                int radius = cvRound(circles[i][2]);

                //circle( imgRoi, centerP, 3, Scalar(0,255,0), -1, 8, 0 );

                //circle( imgRoi, centerP, radius, Scalar(0,255,0), 3, 8, 0 );

                }

                if(circles.size() > 0 )

                circle( undistorted, center, 3, Scalar(0,255,0), -1, 8, 0 );

                else

                circle( undistorted, center, 3, Scalar(255,255,0), -1, 8, 0 );

                putText( undistorted, stst.str(),center-Point2f(0,10),0,0.4f, Scalar(0,255,0));

                putText( undistorted, stst2.str(),center+Point2f(0,20),0,0.4f, Scalar(0,255,0));

                putText( undistorted, stst3.str(),center+Point2f(5,5),0,0.4f, Scalar(0,255,0));

                } else {

                //cout << "no circles found" << endl;

                circle( undistorted, center, 3, Scalar(0,0,255), -1, 8, 0 );

                putText( undistorted, stst.str(),center-Point2f(0,10),0,0.4f, Scalar(0,0,255));

                putText( undistorted, stst2.str(),center+Point2f(0,20),0,0.4f, Scalar(0,0,255));

                putText( undistorted, stst3.str(),center+Point2f(5,5),0,0.4f, Scalar(0,0,255));

                }

                rectangle(undistorted,roi,Scalar(255,0,0));

                rectangle(undistorted,roi2,Scalar(255,255,0));

                }

                }

                }else{

                cout << "something went wrong - no full chessboard detected" << endl;

                }

                */

                clock_t end_full = clock();

                time_full = (end_full-start_full);

                //save result

                if(recordMode)

                        record << undistorted;

                //Mat highContrast = undistorted;

                //undistorted.convertTo(highContrast, -1, 1.5f, 20);

                //imshow("videoBW", undistGray);

                //imshow("videoBW2", thMatB);

                //equalizeHist(undistGrayC,undistGrayC);

                //imshow("videoBW2", undistBW);

                //Mat diff = undistGrayC-undistGray;

                //threshold(diff,diff,0,255,CV_THRESH_BINARY | CV_THRESH_OTSU);

                //imshow("videoBW2",diff);

                imshow("video", imageManagerInput.getRgbImage()); imshow("video2", imageManagerUndistorted.getRgbImage());if(cv::waitKey(30) >= 0) break;

                cout << ((double)(time_full)/(double)CLOCKS_PER_SEC)*1000 << " milliseconds for all" << endl;

                //boost::this_thread::sleep(boost::posix_time::milliseconds(10000));

                continue;

                //NOT USED

                //ORB-KEYPOINTS ON CORNERS

                /*cv::Rect chess(frame.rows/2,frame.cols/2,20,20);

                //cout << corners.size() << " corners found! " <<endl;

                if(corners.size()>0){

                cornerSubPix(gray, corners, cv::Size(11, 11), cv::Size(-1, -1),

                cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));

                cout << corners.size() << " corners after refinement! " <<endl;

                vector<cv::KeyPoint> keypoints;

                for(int i=0; i < corners.size(); i++)

                {        

                keypoints.push_back(KeyPoint(corners[i],15));

                }

                cv::Scalar keypointColor = cv::Scalar(255, 0, 0);     

                //drawKeypoints(frame, keypoints, frame, keypointColor, cv::DrawMatchesFlags::DEFAULT);

                imshow( "corners", frame ); if(cv::waitKey(30) >= 0) break;

                continue;

                //cv::Ptr<cv::FeatureDetector> featureDetector = cv::FeatureDetector::create("FAST");

                //cout << "creation complete" << endl;

                //featureDetector->detect(frame, keypoints);

                cout << "keypoints detected" << endl; 

                cv::Mat descriptors;

                cv::Ptr<cv::DescriptorExtractor> featureExtractor = cv::DescriptorExtractor::create("ORB");

                cout << "creation complete" << endl;

                featureExtractor->compute(gray,keypoints,descriptors);

                cout << "descriptors calculated" <<  endl;

                //                        cv::Scalar keypointColor = cv::Scalar(255, 0, 0);     // Blue keypoints.

                // drawKeypoints(frame, keypoints, frame, keypointColor, cv::DrawMatchesFlags::DEFAULT);

                // imshow("video", frame); if(cv::waitKey(30) >= 0) break;

                cout << "before matchercreation " << endl;

                cv::BruteForceMatcher< cv::L2<float> > matcher2 = cv::BruteForceMatcher< cv::L2<float> >();

                cout << "after matchercreation " << endl;

                //cv::Ptr<cv::DescriptorMatcher> matcher = cv::DescriptorMatcher::create("BruteForce");

                vector< cv::DMatch > matches;

                cout << "before matching " << endl;

                matcher2.match(descriptors,descriptors.row(0),matches);

                cout << "before adding trainingsset " << endl;

                // matcher2.add(descriptors);

                // cout << "before matching " << endl;

                // matcher2.match(descriptors.row(1),matches);

                cout << "matches found" << endl;

                double max_dist = 0; double min_dist = 100;

                //-- Quick calculation of max and min distances between keypoints

                for( int i = 0; i < descriptors.rows; i++ )

                { double dist = matches[i].distance;

                if( dist < min_dist ) min_dist = dist;

                if( dist > max_dist ) max_dist = dist;

                }

                printf("-- Max dist : %f \n", max_dist );

                printf("-- Min dist : %f \n", min_dist );

                //-- Draw only "good" matches (i.e. whose distance is less than 2*min_dist )

                //-- PS.- radiusMatch can also be used here.

                std::vector< cv::DMatch > good_matches;

                int maxMatches = 1000;

                int actMatches = 0;

                for( int i = 0; i < matches.size(); i++ )

                { if(actMatches < maxMatches && true)//matches[i].distance < (2*min_dist + max_dist/(double)2)

                { good_matches.push_back( matches[i]);

                actMatches++;

                }

                }

                cout << good_matches.size() << " good matches found" << endl; 

                //-- Draw only "good" matches

                vector<cv::KeyPoint> keypoints2;

                keypoints2.push_back(keypoints[1]);

                cv::Mat img_matches;

                drawMatches( frame, keypoints, frame, keypoints2,

                good_matches, img_matches, cv::Scalar::all(-1), cv::Scalar::all(-1),

                vector<char>(), cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS );

                //-- Show detected matches

                cv::Mat img_matches_small = cv::Mat(1400,700,img_matches.type());

                cv::resize(img_matches,img_matches_small,cv::Size(1400,700));

                imshow( "Good Matches", img_matches_small ); if(cv::waitKey(30) >= 0) break;

                for( int i = 0; i < good_matches.size(); i++ )

                { printf( "-- Good Match [%d] Keypoint 1: %d  -- Keypoint 2: %d with dist: %f \n", i, good_matches[i].queryIdx, good_matches[i].trainIdx,matches[i].distance); }

                boost::this_thread::sleep(boost::posix_time::milliseconds(100));

                clock_t time_all = time_handdetection + time_houghlines + time_corners;

                cout << "full time: " << ((double)(time_houghlines)/(double)CLOCKS_PER_SEC)*1000 <<endl;

                }

                }*/

        }

        cout << "Finished. Press Button to exit" << endl;

        char waitForExit1 = getchar();

        return EXIT_SUCCESS;

}

void handleCommandline(int argc, char *argv[]) {

        po::options_description options("Allowed options");

        options.add_options()

                ("help,h", "Display a help message.")

                ("videopath,v", po::value < string > (&videoPath), "Path to video (from eyetracker) to be processed.")

                ("debugMode,d", po::value < int > (&debugMode), "Trigger for Debugmode.");

        po::variables_map vm;

        po::store( po::command_line_parser(argc, argv).options(options).run(), vm);

        // first, process the help option

        if (vm.count("help")) {

                cout << options << "\n";

                exit(1);

        }

        if (vm.count("videopath")) {

                cout << "videopath:::" << videoPath << "\n";

        }

        if (vm.count("debugMode")) {

                cout << "debugMode:::" << debugMode << "\n";

        }

        // afterwards, let program options handle argument errors

        po::notify(vm);

        cout << "Program started with:" << endl;

        cout << "  videopath=" << videoPath << endl;

}