<commit_before>#include <unistd.h>
#include <iostream>
#include <fstream>
#include <sstream>
#include <time.h>
#include <string.h>
using namespace std;

#include <FL/Fl.H>
#include <FL/Fl_Double_Window.H>
#include <FL/Fl_Output.H>
#include <FL/Fl_Box.H>
#include <FL/Fl_Button.H>
#include <FL/Fl_Value_Slider.H>
#include <FL/Fl_File_Chooser.H>
#include "Fl_Rotated_Text/Fl_Rotated_Text.H"
#include "cartesian/Cartesian.H"

#include "cvFltkWidget.hh"
#include "ffmpegInterface.hh"
#include "cameraSource.hh"

extern "C"
{
#include "wormProcessing.h"
}

#define DATA_FRAME_RATE_FPS     (1.0 / 15.0) /* I collect at 15 frames per second */
#define PLAYBACK_FRAME_RATE_FPS 15
#define CIRCLE_RADIUS           50
#define CIRCLE_COLOR            CV_RGB(0xFF, 0, 0)
#define POINTED_CIRCLE_COLOR    CV_RGB(0, 0xFF, 0)

#define FRAME_W       480
#define FRAME_H       480
#define CROP_RECT     cvRect(80, 0, FRAME_W, FRAME_H)
#define WINDOW_W      1000
#define WINDOW_H      (FRAME_H + BUTTON_H + PLOT_H + X_AXIS_HEIGHT + AXIS_EXTRA_SPACE)
#define BUTTON_W      100
#define BUTTON_H      30
#define PLOT_W        (WINDOW_W - (Y_AXIS_WIDTH + AXIS_EXTRA_SPACE))
#define PLOT_H        400
#define X_AXIS_HEIGHT 30
#define Y_AXIS_WIDTH  30
#define ACCUM_W       100
#define ACCUM_H       BUTTON_H

// due to a bug (most likely), the axis aren't drawn completely inside their box. Thus I leave a bit
// of extra space to see the labels
#define AXIS_EXTRA_SPACE 30

#define AM_READING_CAMERA (dynamic_cast<CameraSource*>(source) != NULL)

static FrameSource*  source;
static CvFltkWidget* widgetImage;
static Fl_Button*    goResetButton;
static Ca_Canvas*    plot = NULL;
static Ca_X_Axis*    Xaxis;
static Ca_Y_Axis*    Yaxis;

static Fl_Output* leftAccum;
static Fl_Output* rightAccum;
static double     leftAccumValue  = 0.0;
static double     rightAccumValue = 0.0;

// the analysis could be idle, running, or idle examining data (STOPPED)
static enum { RESET, RUNNING, STOPPED } analysisState;

static int           numPoints;
static Ca_LinePoint* lastLeftPoint       = NULL;
static Ca_LinePoint* lastRightPoint      = NULL;
static CvPoint       leftCircleCenter    = cvPoint(-1, -1);
static CvPoint       rightCircleCenter   = cvPoint(-1, -1);
static CvPoint       pointedCircleCenter = cvPoint(-1, -1);

#define HAVE_LEFT_CIRCLE    (leftCircleCenter .x > 0 && leftCircleCenter .y > 0)
#define HAVE_RIGHT_CIRCLE   (rightCircleCenter.x > 0 && rightCircleCenter.y > 0)
#define HAVE_CIRCLES        (HAVE_LEFT_CIRCLE && HAVE_RIGHT_CIRCLE)
#define HAVE_POINTED_CIRCLE (pointedCircleCenter.x > 0 && pointedCircleCenter.y > 0)

void gotNewFrame(IplImage* buffer, uint64_t timestamp_us __attribute__((unused)))
{
    cvMerge(buffer, buffer, buffer, NULL, *widgetImage);

    const CvMat* result = isolateWorms(buffer);
    cvSetImageCOI(*widgetImage, 1);
    cvCopy(result, *widgetImage);
    cvSetImageCOI(*widgetImage, 0);

    if(HAVE_LEFT_CIRCLE)
        cvCircle(*widgetImage, leftCircleCenter,    CIRCLE_RADIUS, CIRCLE_COLOR, 1, 8);

    if(HAVE_RIGHT_CIRCLE)
        cvCircle(*widgetImage, rightCircleCenter,   CIRCLE_RADIUS, CIRCLE_COLOR, 1, 8);

    if(HAVE_POINTED_CIRCLE)
        cvCircle(*widgetImage, pointedCircleCenter, CIRCLE_RADIUS, POINTED_CIRCLE_COLOR, 1, 8);

    // This critical section is likely larger than it needs to be, but this keeps me safe. The
    // analysis state can change in the FLTK thread, so I err on the side of safety
    Fl::lock();
    {
        if(analysisState == RUNNING)
        {
            double leftOccupancy, rightOccupancy;
            computeWormOccupancy(result, &leftCircleCenter, &rightCircleCenter,
                                 CIRCLE_RADIUS,
                                 &leftOccupancy, &rightOccupancy);

            Yaxis->rescale(CA_WHEN_MAX, fmax(leftOccupancy, rightOccupancy) );

            lastLeftPoint  = new Ca_LinePoint(lastLeftPoint,
                                              numPoints, leftOccupancy,  1,FL_RED,   CA_NO_POINT);
            lastRightPoint = new Ca_LinePoint(lastRightPoint,
                                              numPoints, rightOccupancy, 1,FL_GREEN, CA_NO_POINT);
            Xaxis->maximum(numPoints);
            numPoints++;

            leftAccumValue  += leftOccupancy  / DATA_FRAME_RATE_FPS;
            rightAccumValue += rightOccupancy / DATA_FRAME_RATE_FPS;
            char results[128];
            snprintf(results, sizeof(results), "%.3f", leftAccumValue);
            leftAccum->value(results);
            snprintf(results, sizeof(results), "%.3f", rightAccumValue);
            rightAccum->value(results);
        }

        widgetImage->redrawNewFrame();
    }
    Fl::unlock();
}

static void widgetImageCallback(Fl_Widget* widget __attribute__((unused)), void* cookie __attribute__((unused)))
{
    if(Fl::event() == FL_LEAVE)
    {
        // I want to make sure to never miss this, so I handle it unconditionally
        pointedCircleCenter.x = pointedCircleCenter.y = -1;
        return;
    }

    if(analysisState == RUNNING)
    {
        pointedCircleCenter.x = pointedCircleCenter.y = -1;
        return;
    }

    bool onLeftHalf = (Fl::event_x() - widget->x()) < FRAME_W/2;
    switch(Fl::event())
    {
    case FL_MOVE:
        pointedCircleCenter.x = Fl::event_x() - widget->x();
        pointedCircleCenter.y = Fl::event_y() - widget->y();
        break;

    case FL_LEAVE:
        pointedCircleCenter.x = pointedCircleCenter.y = -1;
        break;

    case FL_PUSH:
        if(onLeftHalf)
        {
            leftCircleCenter.x = Fl::event_x() - widget->x();
            leftCircleCenter.y = Fl::event_y() - widget->y();
        }
        else
        {
            rightCircleCenter.x = Fl::event_x() - widget->x();
            rightCircleCenter.y = Fl::event_y() - widget->y();
        }

        pointedCircleCenter.x = pointedCircleCenter.y = -1;
        break;

    default: ;
    }

    if(HAVE_CIRCLES && analysisState == RESET)
        goResetButton->activate();
}

static void setResetAnalysis(void)
{
    goResetButton->labelfont(FL_HELVETICA_BOLD);
    goResetButton->labelcolor(FL_RED);
    goResetButton->type(FL_TOGGLE_BUTTON);
    goResetButton->label("Analyze");

    numPoints       = 0;
    if(plot) plot->clear();
    lastLeftPoint   = lastRightPoint = NULL;
    leftAccumValue  = 0.0;
    rightAccumValue = 0.0;
    leftAccum ->value("0.0");
    rightAccum->value("0.0");

    analysisState = RESET;
}

static void setRunningAnalysis(void)
{
    goResetButton->labelfont(FL_HELVETICA);
    goResetButton->labelcolor(FL_BLACK);
    goResetButton->type(FL_TOGGLE_BUTTON);
    goResetButton->label("Stop analysis");

    pointedCircleCenter.x = pointedCircleCenter.y = -1;

    analysisState = RUNNING;
}

static void setStoppedAnalysis(void)
{
    goResetButton->labelfont(FL_HELVETICA);
    goResetButton->labelcolor(FL_BLACK);
    goResetButton->type(FL_NORMAL_BUTTON);
    goResetButton->label("Reset analysis data");

    analysisState = STOPPED;
}

static void pressedGoReset(Fl_Widget* widget __attribute__((unused)), void* cookie __attribute__((unused)))
{
    if     (analysisState == RUNNING) setStoppedAnalysis();
    else if(analysisState == STOPPED) setResetAnalysis();
    else                              setRunningAnalysis();
}

int main(int argc, char* argv[])
{
    Fl::lock();
    Fl::visual(FL_RGB);

    // open the first source. If there's an argument, assume it's an input video. Otherwise, try
    // reading a camera
    if(argc >= 2) source = new FFmpegDecoder(argv[1], FRAMESOURCE_GRAYSCALE, false, CROP_RECT);
    else          source = new CameraSource (FRAMESOURCE_GRAYSCALE, false, 0, CROP_RECT);

    if(! *source)
    {
        fprintf(stderr, "couldn't open source\n");
        delete source;
        return 0;
    }

    Fl_Double_Window* window =
        new Fl_Double_Window(WINDOW_W, WINDOW_H, "Wormtracker 3");
    widgetImage = new CvFltkWidget(0, 0, source->w(), source->h(),
                                   WIDGET_COLOR);

    widgetImage->callback(widgetImageCallback);

    goResetButton = new Fl_Button( 0, source->h(), BUTTON_W, BUTTON_H);
    goResetButton->callback(pressedGoReset);
    goResetButton->deactivate();

    plot = new Ca_Canvas( Y_AXIS_WIDTH + AXIS_EXTRA_SPACE, goResetButton->y() + goResetButton->h(), PLOT_W, PLOT_H,
                          "Worm occupancy");
    plot->align(FL_ALIGN_TOP);

    // This is extremely important for some reason. Without it the plots do not refresh property and
    // there're artifacts every time the plot is resized
    plot->box(FL_DOWN_BOX);

    Xaxis = new Ca_X_Axis(plot->x(), plot->y() + plot->h(), plot->w(), X_AXIS_HEIGHT, "Points");
    Xaxis->align(FL_ALIGN_BOTTOM);
    Xaxis->minimum(0);
    Xaxis->maximum(1);
    Xaxis->label_format("%g");
    Xaxis->major_step(10);
    Xaxis->label_step(10);
    Xaxis->axis_color(FL_BLACK);
    Xaxis->axis_align(CA_BOTTOM | CA_LINE);

    Yaxis = new Ca_Y_Axis(AXIS_EXTRA_SPACE, plot->y(), Y_AXIS_WIDTH, plot->h());
    Fl_Rotated_Text YaxisLabel("occupancy ratio", FL_HELVETICA, 14, 0, 1);
    Yaxis->image(&YaxisLabel);
    Yaxis->minimum(0);
    Yaxis->maximum(0.01);
    Yaxis->align(FL_ALIGN_LEFT);
    Yaxis->axis_align(CA_LEFT | CA_LINE);
    Yaxis->axis_color(FL_BLACK);

    leftAccum  = new Fl_Output(widgetImage->x() + widgetImage->w(), widgetImage->y(),
                              ACCUM_W, ACCUM_H, "Left accumulator (occupancy-seconds)");
    rightAccum = new Fl_Output(leftAccum->x(), leftAccum->y() + leftAccum->h(),
                              ACCUM_W, ACCUM_H, "Right accumulator (occupancy-seconds)");
    leftAccum ->align(FL_ALIGN_RIGHT);
    rightAccum->align(FL_ALIGN_RIGHT);
    leftAccum ->labelcolor(FL_RED);
    rightAccum->labelcolor(FL_GREEN);

    window->end();
    window->show();

    processingInit(source->w(), source->h());

    setResetAnalysis();

    // I read the data with a tiny delay. This makes sure that I skip old frames (only an issue if I
    // can't keep up with the data rate), but yet got as fast as I can
    IplImage* buffer = cvCreateImage(cvSize(source->w(), source->h()), IPL_DEPTH_8U, 1);
    source->startSourceThread(&gotNewFrame, 1e6/PLAYBACK_FRAME_RATE_FPS, buffer);

    while (Fl::wait())
    {
    }

    Fl::unlock();

    delete source;
    delete window;
    cvReleaseImage(&buffer);

    processingCleanup();
    return 0;
}
<commit_msg>if reading from a stored video file, run as fast as possible<commit_after>#include <unistd.h>
#include <iostream>
#include <fstream>
#include <sstream>
#include <time.h>
#include <string.h>
using namespace std;

#include <FL/Fl.H>
#include <FL/Fl_Double_Window.H>
#include <FL/Fl_Output.H>
#include <FL/Fl_Box.H>
#include <FL/Fl_Button.H>
#include <FL/Fl_Value_Slider.H>
#include <FL/Fl_File_Chooser.H>
#include "Fl_Rotated_Text/Fl_Rotated_Text.H"
#include "cartesian/Cartesian.H"

#include "cvFltkWidget.hh"
#include "ffmpegInterface.hh"
#include "cameraSource.hh"

extern "C"
{
#include "wormProcessing.h"
}

#define DATA_FRAME_RATE_FPS     (1.0 / 15.0) /* I collect at 15 frames per second */
#define PLAYBACK_FRAME_RATE_FPS 15
#define CIRCLE_RADIUS           50
#define CIRCLE_COLOR            CV_RGB(0xFF, 0, 0)
#define POINTED_CIRCLE_COLOR    CV_RGB(0, 0xFF, 0)

#define FRAME_W       480
#define FRAME_H       480
#define CROP_RECT     cvRect(80, 0, FRAME_W, FRAME_H)
#define WINDOW_W      1000
#define WINDOW_H      (FRAME_H + BUTTON_H + PLOT_H + X_AXIS_HEIGHT + AXIS_EXTRA_SPACE)
#define BUTTON_W      100
#define BUTTON_H      30
#define PLOT_W        (WINDOW_W - (Y_AXIS_WIDTH + AXIS_EXTRA_SPACE))
#define PLOT_H        400
#define X_AXIS_HEIGHT 30
#define Y_AXIS_WIDTH  30
#define ACCUM_W       100
#define ACCUM_H       BUTTON_H

// due to a bug (most likely), the axis aren't drawn completely inside their box. Thus I leave a bit
// of extra space to see the labels
#define AXIS_EXTRA_SPACE 30

#define AM_READING_CAMERA (dynamic_cast<CameraSource*>(source) != NULL)

static FrameSource*  source;
static CvFltkWidget* widgetImage;
static Fl_Button*    goResetButton;
static Ca_Canvas*    plot = NULL;
static Ca_X_Axis*    Xaxis;
static Ca_Y_Axis*    Yaxis;

static Fl_Output* leftAccum;
static Fl_Output* rightAccum;
static double     leftAccumValue  = 0.0;
static double     rightAccumValue = 0.0;

// the analysis could be idle, running, or idle examining data (STOPPED)
static enum { RESET, RUNNING, STOPPED } analysisState;

static int           numPoints;
static Ca_LinePoint* lastLeftPoint       = NULL;
static Ca_LinePoint* lastRightPoint      = NULL;
static CvPoint       leftCircleCenter    = cvPoint(-1, -1);
static CvPoint       rightCircleCenter   = cvPoint(-1, -1);
static CvPoint       pointedCircleCenter = cvPoint(-1, -1);

#define HAVE_LEFT_CIRCLE    (leftCircleCenter .x > 0 && leftCircleCenter .y > 0)
#define HAVE_RIGHT_CIRCLE   (rightCircleCenter.x > 0 && rightCircleCenter.y > 0)
#define HAVE_CIRCLES        (HAVE_LEFT_CIRCLE && HAVE_RIGHT_CIRCLE)
#define HAVE_POINTED_CIRCLE (pointedCircleCenter.x > 0 && pointedCircleCenter.y > 0)

void gotNewFrame(IplImage* buffer, uint64_t timestamp_us __attribute__((unused)))
{
    cvMerge(buffer, buffer, buffer, NULL, *widgetImage);

    const CvMat* result = isolateWorms(buffer);
    cvSetImageCOI(*widgetImage, 1);
    cvCopy(result, *widgetImage);
    cvSetImageCOI(*widgetImage, 0);

    if(HAVE_LEFT_CIRCLE)
        cvCircle(*widgetImage, leftCircleCenter,    CIRCLE_RADIUS, CIRCLE_COLOR, 1, 8);

    if(HAVE_RIGHT_CIRCLE)
        cvCircle(*widgetImage, rightCircleCenter,   CIRCLE_RADIUS, CIRCLE_COLOR, 1, 8);

    if(HAVE_POINTED_CIRCLE)
        cvCircle(*widgetImage, pointedCircleCenter, CIRCLE_RADIUS, POINTED_CIRCLE_COLOR, 1, 8);

    // This critical section is likely larger than it needs to be, but this keeps me safe. The
    // analysis state can change in the FLTK thread, so I err on the side of safety
    Fl::lock();
    {
        if(analysisState == RUNNING)
        {
            double leftOccupancy, rightOccupancy;
            computeWormOccupancy(result, &leftCircleCenter, &rightCircleCenter,
                                 CIRCLE_RADIUS,
                                 &leftOccupancy, &rightOccupancy);

            Yaxis->rescale(CA_WHEN_MAX, fmax(leftOccupancy, rightOccupancy) );

            lastLeftPoint  = new Ca_LinePoint(lastLeftPoint,
                                              numPoints, leftOccupancy,  1,FL_RED,   CA_NO_POINT);
            lastRightPoint = new Ca_LinePoint(lastRightPoint,
                                              numPoints, rightOccupancy, 1,FL_GREEN, CA_NO_POINT);
            Xaxis->maximum(numPoints);
            numPoints++;

            leftAccumValue  += leftOccupancy  / DATA_FRAME_RATE_FPS;
            rightAccumValue += rightOccupancy / DATA_FRAME_RATE_FPS;
            char results[128];
            snprintf(results, sizeof(results), "%.3f", leftAccumValue);
            leftAccum->value(results);
            snprintf(results, sizeof(results), "%.3f", rightAccumValue);
            rightAccum->value(results);
        }

        widgetImage->redrawNewFrame();
    }
    Fl::unlock();
}

static void widgetImageCallback(Fl_Widget* widget __attribute__((unused)), void* cookie __attribute__((unused)))
{
    if(Fl::event() == FL_LEAVE)
    {
        // I want to make sure to never miss this, so I handle it unconditionally
        pointedCircleCenter.x = pointedCircleCenter.y = -1;
        return;
    }

    if(analysisState == RUNNING)
    {
        pointedCircleCenter.x = pointedCircleCenter.y = -1;
        return;
    }

    bool onLeftHalf = (Fl::event_x() - widget->x()) < FRAME_W/2;
    switch(Fl::event())
    {
    case FL_MOVE:
        pointedCircleCenter.x = Fl::event_x() - widget->x();
        pointedCircleCenter.y = Fl::event_y() - widget->y();
        break;

    case FL_LEAVE:
        pointedCircleCenter.x = pointedCircleCenter.y = -1;
        break;

    case FL_PUSH:
        if(onLeftHalf)
        {
            leftCircleCenter.x = Fl::event_x() - widget->x();
            leftCircleCenter.y = Fl::event_y() - widget->y();
        }
        else
        {
            rightCircleCenter.x = Fl::event_x() - widget->x();
            rightCircleCenter.y = Fl::event_y() - widget->y();
        }

        pointedCircleCenter.x = pointedCircleCenter.y = -1;
        break;

    default: ;
    }

    if(HAVE_CIRCLES && analysisState == RESET)
        goResetButton->activate();
}

static void setResetAnalysis(void)
{
    goResetButton->labelfont(FL_HELVETICA_BOLD);
    goResetButton->labelcolor(FL_RED);
    goResetButton->type(FL_TOGGLE_BUTTON);
    goResetButton->label("Analyze");

    numPoints       = 0;
    if(plot) plot->clear();
    lastLeftPoint   = lastRightPoint = NULL;
    leftAccumValue  = 0.0;
    rightAccumValue = 0.0;
    leftAccum ->value("0.0");
    rightAccum->value("0.0");

    analysisState = RESET;
}

static void setRunningAnalysis(void)
{
    goResetButton->labelfont(FL_HELVETICA);
    goResetButton->labelcolor(FL_BLACK);
    goResetButton->type(FL_TOGGLE_BUTTON);
    goResetButton->label("Stop analysis");

    pointedCircleCenter.x = pointedCircleCenter.y = -1;

    analysisState = RUNNING;
}

static void setStoppedAnalysis(void)
{
    goResetButton->labelfont(FL_HELVETICA);
    goResetButton->labelcolor(FL_BLACK);
    goResetButton->type(FL_NORMAL_BUTTON);
    goResetButton->label("Reset analysis data");

    analysisState = STOPPED;
}

static void pressedGoReset(Fl_Widget* widget __attribute__((unused)), void* cookie __attribute__((unused)))
{
    if     (analysisState == RUNNING) setStoppedAnalysis();
    else if(analysisState == STOPPED) setResetAnalysis();
    else                              setRunningAnalysis();
}

int main(int argc, char* argv[])
{
    Fl::lock();
    Fl::visual(FL_RGB);

    // open the first source. If there's an argument, assume it's an input video. Otherwise, try
    // reading a camera
    if(argc >= 2) source = new FFmpegDecoder(argv[1], FRAMESOURCE_GRAYSCALE, false, CROP_RECT);
    else          source = new CameraSource (FRAMESOURCE_GRAYSCALE, false, 0, CROP_RECT);

    if(! *source)
    {
        fprintf(stderr, "couldn't open source\n");
        delete source;
        return 0;
    }

    Fl_Double_Window* window =
        new Fl_Double_Window(WINDOW_W, WINDOW_H, "Wormtracker 3");
    widgetImage = new CvFltkWidget(0, 0, source->w(), source->h(),
                                   WIDGET_COLOR);

    widgetImage->callback(widgetImageCallback);

    goResetButton = new Fl_Button( 0, source->h(), BUTTON_W, BUTTON_H);
    goResetButton->callback(pressedGoReset);
    goResetButton->deactivate();

    plot = new Ca_Canvas( Y_AXIS_WIDTH + AXIS_EXTRA_SPACE, goResetButton->y() + goResetButton->h(), PLOT_W, PLOT_H,
                          "Worm occupancy");
    plot->align(FL_ALIGN_TOP);

    // This is extremely important for some reason. Without it the plots do not refresh property and
    // there're artifacts every time the plot is resized
    plot->box(FL_DOWN_BOX);

    Xaxis = new Ca_X_Axis(plot->x(), plot->y() + plot->h(), plot->w(), X_AXIS_HEIGHT, "Points");
    Xaxis->align(FL_ALIGN_BOTTOM);
    Xaxis->minimum(0);
    Xaxis->maximum(1);
    Xaxis->label_format("%g");
    Xaxis->major_step(10);
    Xaxis->label_step(10);
    Xaxis->axis_color(FL_BLACK);
    Xaxis->axis_align(CA_BOTTOM | CA_LINE);

    Yaxis = new Ca_Y_Axis(AXIS_EXTRA_SPACE, plot->y(), Y_AXIS_WIDTH, plot->h());
    Fl_Rotated_Text YaxisLabel("occupancy ratio", FL_HELVETICA, 14, 0, 1);
    Yaxis->image(&YaxisLabel);
    Yaxis->minimum(0);
    Yaxis->maximum(0.01);
    Yaxis->align(FL_ALIGN_LEFT);
    Yaxis->axis_align(CA_LEFT | CA_LINE);
    Yaxis->axis_color(FL_BLACK);

    leftAccum  = new Fl_Output(widgetImage->x() + widgetImage->w(), widgetImage->y(),
                              ACCUM_W, ACCUM_H, "Left accumulator (occupancy-seconds)");
    rightAccum = new Fl_Output(leftAccum->x(), leftAccum->y() + leftAccum->h(),
                              ACCUM_W, ACCUM_H, "Right accumulator (occupancy-seconds)");
    leftAccum ->align(FL_ALIGN_RIGHT);
    rightAccum->align(FL_ALIGN_RIGHT);
    leftAccum ->labelcolor(FL_RED);
    rightAccum->labelcolor(FL_GREEN);

    window->end();
    window->show();

    processingInit(source->w(), source->h());

    setResetAnalysis();

    // I read the data with a tiny delay. This makes sure that I skip old frames (only an issue if I
    // can't keep up with the data rate), but yet got as fast as I can
    IplImage* buffer = cvCreateImage(cvSize(source->w(), source->h()), IPL_DEPTH_8U, 1);

    // If reading from a stored video file, go as fast as possible
    if(AM_READING_CAMERA) source->startSourceThread(&gotNewFrame, 1e6/DATA_FRAME_RATE_FPS, buffer);
    else                  source->startSourceThread(&gotNewFrame, 0,                       buffer);

    while (Fl::wait())
    {
    }

    Fl::unlock();

    delete source;
    delete window;
    cvReleaseImage(&buffer);

    processingCleanup();
    return 0;
}
<|endoftext|>


<commit_before>#include <iostream>
#include <algorithm>
#include <vector>
#include <cmath>
#include <ctime>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

#include "gaussian.h"

#define BLUR_RADIUS 3
#define PATH_COUNT 8
#define MAX_SHORT 65535
#define SMALL_PENALTY 3
#define LARGE_PENALTY 20
#define DEBUG false

struct path {
    short rowDiff;
    short colDiff;
    short index;
};

void printArray(unsigned short ***array, int rows, int cols, int depth) {
    for (int d = 0; d < depth; ++d) {
        std::cout << "disparity: " << d << std::endl;
        for (int row = 0; row < rows; ++row) {
            for (int col = 0; col < cols; ++col) {
                std::cout << "\t" << array[row][col][d];
            }
            std::cout << std::endl;
        }
    }
}

unsigned short calculatePixelCostOneWayBT(int row, int leftCol, int rightCol, const cv::Mat &leftImage, const cv::Mat &rightImage) {

    char leftValue, rightValue, beforeRightValue, afterRightValue, rightValueMinus, rightValuePlus, rightValueMin, rightValueMax;

    leftValue = leftImage.at<uchar>(row, leftCol);
    rightValue = rightImage.at<uchar>(row, rightCol);

    if (rightCol > 0) {
        beforeRightValue = rightImage.at<uchar>(row, rightCol - 1);
    } else {
        beforeRightValue = rightValue;
    }

    if (rightCol + 1 < rightImage.cols) {
        afterRightValue = rightImage.at<uchar>(row, rightCol + 1);
    } else {
        afterRightValue = rightValue;
    }

    rightValueMinus = round((rightValue + beforeRightValue) / 2.f);
    rightValuePlus = round((rightValue + afterRightValue) / 2.f);

    rightValueMin = std::min(rightValue, std::min(rightValueMinus, rightValuePlus));
    rightValueMax = std::max(rightValue, std::max(rightValueMinus, rightValuePlus));

    return std::max(0, std::max((leftValue - rightValueMax), (rightValueMin - leftValue)));
}

inline unsigned short calculatePixelCostBT(int row, int leftCol, int rightCol, const cv::Mat &leftImage, const cv::Mat &rightImage) {
    return std::min(calculatePixelCostOneWayBT(row, leftCol, rightCol, leftImage, rightImage),
        calculatePixelCostOneWayBT(row, rightCol, leftCol, rightImage, leftImage));
}

void calculatePixelCost(cv::Mat &firstImage, cv::Mat &secondImage, int disparityRange, unsigned short ***C) {
    for (int row = 0; row < firstImage.rows; ++row) {
        for (int col = 0; col < firstImage.cols; ++col) {
            for (int d = 0; d < disparityRange; ++d) {
                C[row][col][d] = calculatePixelCostBT(row, col, col - d, firstImage, secondImage);
            }
        }
    }
}

void initializePaths(std::vector<path> &paths, unsigned short pathCount) {
    for (unsigned short i = 0; i < pathCount; ++i) {
        paths.push_back(path());
    }

    if(paths.size() >= 2) {
        paths[0].rowDiff = 0;
        paths[0].colDiff = 1;
        paths[0].index = 0;

        paths[1].rowDiff = 0;
        paths[1].colDiff = -1;
        paths[1].index = 1;
    }

    if(paths.size() >= 4) {
        paths[2].rowDiff = -1;
        paths[2].colDiff = 0;
        paths[2].index = 2;

        paths[3].rowDiff = 1;
        paths[3].colDiff = 0;
        paths[3].index = 3;
    }

    if(paths.size() >= 8) {
        paths[4].rowDiff = -1;
        paths[4].colDiff = 1;
        paths[4].index = 4;

        paths[5].rowDiff = 1;
        paths[5].colDiff = 1;
        paths[5].index = 5;

        paths[6].rowDiff = 1;
        paths[6].colDiff = -1;
        paths[6].index = 6;

        paths[7].rowDiff = -1;
        paths[7].colDiff = -1;
        paths[7].index = 7;
    }

    if(paths.size() >= 16) {
        paths[8].rowDiff = -2;
        paths[8].colDiff = 1;
        paths[8].index = 8;

        paths[9].rowDiff = -2;
        paths[9].colDiff = -1;
        paths[9].index = 9;

        paths[10].rowDiff = 2;
        paths[10].colDiff = 1;
        paths[10].index = 10;

        paths[11].rowDiff = 2;
        paths[11].colDiff = -1;
        paths[11].index = 11;

        paths[12].rowDiff = 1;
        paths[12].colDiff = -2;
        paths[12].index = 12;

        paths[13].rowDiff = -1;
        paths[13].colDiff = -2;
        paths[13].index = 13;

        paths[14].rowDiff = 1;
        paths[14].colDiff = 2;
        paths[14].index = 14;

        paths[15].rowDiff = -1;
        paths[15].colDiff = 2;
        paths[15].index = 15;
    }
}

unsigned short aggregateCost(int row, int col, int d, path &p, int rows, int cols, int disparityRange, unsigned short ***C, unsigned short ****A, unsigned short ***S, unsigned int iter) {
    unsigned short aggregatedCost = 0;
    aggregatedCost += C[row][col][d];

    if(DEBUG) {
        for (unsigned int i = 0; i < iter; ++i) {
            printf(" ");
        }
        printf("{P%d}[%d][%d](d%d)\n", p.index, row, col, d);
    }

    if (A[p.index][row][col][d] != MAX_SHORT) {
        if(DEBUG) {
            for (unsigned int i = 0; i < iter; ++i) {
                printf(" ");
            }
            printf("{P%d}[%d][%d](d%d)-> %d<CACHED>\n", p.index, row, col, d, A[p.index][row][col][d]);
        }
        return A[p.index][row][col][d];
    }

    if (row + p.rowDiff < 0 || row + p.rowDiff >= rows || col + p.colDiff < 0 || col + p.colDiff >= cols) {
        // border
        A[p.index][row][col][d] = aggregatedCost;
        if(DEBUG) {
            for (unsigned int i = 0; i < iter; ++i) {
                printf(" ");
            }
            printf("{P%d}[%d][%d](d%d)-> %d <BORDER>\n", p.index, row, col, d, A[p.index][row][col][d]);
        }
        return A[p.index][row][col][d];

        return aggregatedCost;
    }

    unsigned short minPrev, minPrevOther, prev, prevPlus, prevMinus;
    prev = minPrev = minPrevOther = prevPlus = prevMinus = MAX_SHORT;

    for (int disp = 0; disp < disparityRange; ++disp) {
        unsigned short tmp = aggregateCost(row + p.rowDiff, col + p.colDiff, disp, p, rows, cols, disparityRange, C, A, S, ++iter);
        if(minPrev > tmp) {
            minPrev = tmp;
        }

        if(disp == d) {
            prev = tmp;
        } else if(disp == d + 1) {
            prevPlus = tmp;
        } else if (disp == d - 1) {
            prevMinus = tmp;
        } else {
            if(minPrevOther > tmp + LARGE_PENALTY) {
                minPrevOther = tmp + LARGE_PENALTY;
            }
        }
    }

    aggregatedCost += std::min(std::min((int)prevPlus + SMALL_PENALTY, (int)prevMinus + SMALL_PENALTY), std::min((int)prev, (int)minPrevOther + LARGE_PENALTY));
    aggregatedCost -= minPrev;

    A[p.index][row][col][d] = aggregatedCost;

    if(DEBUG) {
        for (unsigned int i = 0; i < iter; ++i) {
            printf(" ");
        }
        printf("{P%d}[%d][%d](d%d)-> %d<CALCULATED>\n", p.index, row, col, d, A[p.index][row][col][d]);
    }

    return aggregatedCost;
}

void aggregateCosts(int rows, int cols, int disparityRange, std::vector<path> &paths, unsigned short ***C, unsigned short ****A, unsigned short ***S) {
    for (unsigned long p = 0; p < paths.size(); ++p) {
        for (int row = 0; row < rows; ++row) {
            for (int col = 0; col < cols; ++col) {
                for (int d = 0; d < disparityRange; ++d) {
                    S[row][col][d] += aggregateCost(row, col, d, paths[p], rows, cols, disparityRange, C, A, S, 0);
                }
            }
        }
    }
}

void computeDisparity(unsigned short ***S, int rows, int cols, int disparityRange, cv::Mat &disparityMap) {
    unsigned int disparity, minCost;
    for (int row = 0; row < rows; ++row) {
        for (int col = 0; col < cols; ++col) {
            minCost = 65535;
            for (int d = disparityRange - 1; d >= 0; --d) {
                if(minCost > S[row][col][d]) {
                    minCost = S[row][col][d];
                    disparity = d;
                }
            }
            disparityMap.at<uchar>(row, col) = disparity;
        }
    }
}

void showDisparityMap(cv::Mat &disparityMap, int disparityRange) {
    double factor = 256.0 / disparityRange;
    for (int row = 0; row < disparityMap.rows; ++row) {
        for (int col = 0; col < disparityMap.cols; ++col) {
            disparityMap.at<uchar>(row, col) *= factor;
        }
    }
    cv::namedWindow("Disparity map", CV_WINDOW_AUTOSIZE);
    cv::imshow("Disparity map", disparityMap);
    cv::waitKey(0);
}

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

    if (argc != 4) {
        std::cerr << "Usage: " << argv[0] << " <left image> <right image> <disparity range>" << std::endl;
        return -1;
    }

    cv::Mat firstImage;
    cv::Mat secondImage;
    firstImage = cv::imread(argv[1], CV_LOAD_IMAGE_GRAYSCALE);
    secondImage = cv::imread(argv[2], CV_LOAD_IMAGE_GRAYSCALE);

    if(!firstImage.data || !secondImage.data) {
        std::cerr <<  "Could not open or find one of the images!" << std::endl;
        return -1;
    }

    unsigned int disparityRange = atoi(argv[3]);
    unsigned short ***C; // pixel cost array W x H x D
    unsigned short ***S; // aggregated cost array W x H x D
    unsigned short ****A; // path cost array P x W x H x D
    std::vector<path> paths;

    initializePaths(paths, PATH_COUNT);

    /*
    * TODO
    * variable LARGE_PENALTY
    */

    clock_t begin = clock();

    std::cout << "Allocating space..." << std::endl;

    // allocate cost arrays
    C = new unsigned short**[firstImage.rows];
    S = new unsigned short**[firstImage.rows];
    for (int row = 0; row < firstImage.rows; ++row) {
        C[row] = new unsigned short*[firstImage.cols];
        S[row] = new unsigned short*[firstImage.cols]();
        for (int col = 0; col < firstImage.cols; ++col) {
            C[row][col] = new unsigned short[disparityRange];
            S[row][col] = new unsigned short[disparityRange](); // initialize to 0
        }
    }

    A = new unsigned short ***[paths.size()];
    for (unsigned long p = 0; p < paths.size(); ++p) {
        A[p] = new unsigned short **[firstImage.rows];
        for (int row = 0; row < firstImage.rows; ++row) {
            A[p][row] = new unsigned short*[firstImage.cols];
            for (int col = 0; col < firstImage.cols; ++col) {
                A[p][row][col] = new unsigned short[disparityRange];
                for (unsigned int d = 0; d < disparityRange; ++d) {
                    A[p][row][col][d] = MAX_SHORT;
                }
            }
        }
    }

    std::cout << "Smoothing images..." << std::endl;
    grayscaleGaussianBlur(firstImage, firstImage, BLUR_RADIUS);
    grayscaleGaussianBlur(secondImage, secondImage, BLUR_RADIUS);

    std::cout << "Calculating pixel cost for the image..." << std::endl;
    calculatePixelCost(firstImage, secondImage, disparityRange, C);
    if(DEBUG) {
        printArray(C, firstImage.rows, firstImage.cols, disparityRange);
    }
    std::cout << "Aggregating costs..." << std::endl;
    aggregateCosts(firstImage.rows, firstImage.cols, disparityRange, paths, C, A, S);

    cv::Mat disparityMap = cv::Mat(cv::Size(firstImage.cols, firstImage.rows), CV_8UC1, cv::Scalar::all(0));

    std::cout << "Computing disparity..." << std::endl;
    computeDisparity(S, firstImage.rows, firstImage.cols, disparityRange, disparityMap);

    clock_t end = clock();
    double elapsed_secs = double(end - begin) / CLOCKS_PER_SEC;

    printf("Done in %.2lf seconds.\n", elapsed_secs);

    showDisparityMap(disparityMap, disparityRange);

    return 0;
}<commit_msg>Set number of paths to 16<commit_after>#include <iostream>
#include <algorithm>
#include <vector>
#include <cmath>
#include <ctime>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

#include "gaussian.h"

#define BLUR_RADIUS 3
#define PATH_COUNT 16
#define MAX_SHORT 65535
#define SMALL_PENALTY 3
#define LARGE_PENALTY 20
#define DEBUG false

struct path {
    short rowDiff;
    short colDiff;
    short index;
};

void printArray(unsigned short ***array, int rows, int cols, int depth) {
    for (int d = 0; d < depth; ++d) {
        std::cout << "disparity: " << d << std::endl;
        for (int row = 0; row < rows; ++row) {
            for (int col = 0; col < cols; ++col) {
                std::cout << "\t" << array[row][col][d];
            }
            std::cout << std::endl;
        }
    }
}

unsigned short calculatePixelCostOneWayBT(int row, int leftCol, int rightCol, const cv::Mat &leftImage, const cv::Mat &rightImage) {

    char leftValue, rightValue, beforeRightValue, afterRightValue, rightValueMinus, rightValuePlus, rightValueMin, rightValueMax;

    leftValue = leftImage.at<uchar>(row, leftCol);
    rightValue = rightImage.at<uchar>(row, rightCol);

    if (rightCol > 0) {
        beforeRightValue = rightImage.at<uchar>(row, rightCol - 1);
    } else {
        beforeRightValue = rightValue;
    }

    if (rightCol + 1 < rightImage.cols) {
        afterRightValue = rightImage.at<uchar>(row, rightCol + 1);
    } else {
        afterRightValue = rightValue;
    }

    rightValueMinus = round((rightValue + beforeRightValue) / 2.f);
    rightValuePlus = round((rightValue + afterRightValue) / 2.f);

    rightValueMin = std::min(rightValue, std::min(rightValueMinus, rightValuePlus));
    rightValueMax = std::max(rightValue, std::max(rightValueMinus, rightValuePlus));

    return std::max(0, std::max((leftValue - rightValueMax), (rightValueMin - leftValue)));
}

inline unsigned short calculatePixelCostBT(int row, int leftCol, int rightCol, const cv::Mat &leftImage, const cv::Mat &rightImage) {
    return std::min(calculatePixelCostOneWayBT(row, leftCol, rightCol, leftImage, rightImage),
        calculatePixelCostOneWayBT(row, rightCol, leftCol, rightImage, leftImage));
}

void calculatePixelCost(cv::Mat &firstImage, cv::Mat &secondImage, int disparityRange, unsigned short ***C) {
    for (int row = 0; row < firstImage.rows; ++row) {
        for (int col = 0; col < firstImage.cols; ++col) {
            for (int d = 0; d < disparityRange; ++d) {
                C[row][col][d] = calculatePixelCostBT(row, col, col - d, firstImage, secondImage);
            }
        }
    }
}

void initializePaths(std::vector<path> &paths, unsigned short pathCount) {
    for (unsigned short i = 0; i < pathCount; ++i) {
        paths.push_back(path());
    }

    if(paths.size() >= 2) {
        paths[0].rowDiff = 0;
        paths[0].colDiff = 1;
        paths[0].index = 0;

        paths[1].rowDiff = 0;
        paths[1].colDiff = -1;
        paths[1].index = 1;
    }

    if(paths.size() >= 4) {
        paths[2].rowDiff = -1;
        paths[2].colDiff = 0;
        paths[2].index = 2;

        paths[3].rowDiff = 1;
        paths[3].colDiff = 0;
        paths[3].index = 3;
    }

    if(paths.size() >= 8) {
        paths[4].rowDiff = -1;
        paths[4].colDiff = 1;
        paths[4].index = 4;

        paths[5].rowDiff = 1;
        paths[5].colDiff = 1;
        paths[5].index = 5;

        paths[6].rowDiff = 1;
        paths[6].colDiff = -1;
        paths[6].index = 6;

        paths[7].rowDiff = -1;
        paths[7].colDiff = -1;
        paths[7].index = 7;
    }

    if(paths.size() >= 16) {
        paths[8].rowDiff = -2;
        paths[8].colDiff = 1;
        paths[8].index = 8;

        paths[9].rowDiff = -2;
        paths[9].colDiff = -1;
        paths[9].index = 9;

        paths[10].rowDiff = 2;
        paths[10].colDiff = 1;
        paths[10].index = 10;

        paths[11].rowDiff = 2;
        paths[11].colDiff = -1;
        paths[11].index = 11;

        paths[12].rowDiff = 1;
        paths[12].colDiff = -2;
        paths[12].index = 12;

        paths[13].rowDiff = -1;
        paths[13].colDiff = -2;
        paths[13].index = 13;

        paths[14].rowDiff = 1;
        paths[14].colDiff = 2;
        paths[14].index = 14;

        paths[15].rowDiff = -1;
        paths[15].colDiff = 2;
        paths[15].index = 15;
    }
}

unsigned short aggregateCost(int row, int col, int d, path &p, int rows, int cols, int disparityRange, unsigned short ***C, unsigned short ****A, unsigned short ***S, unsigned int iter) {
    unsigned short aggregatedCost = 0;
    aggregatedCost += C[row][col][d];

    if(DEBUG) {
        for (unsigned int i = 0; i < iter; ++i) {
            printf(" ");
        }
        printf("{P%d}[%d][%d](d%d)\n", p.index, row, col, d);
    }

    if (A[p.index][row][col][d] != MAX_SHORT) {
        if(DEBUG) {
            for (unsigned int i = 0; i < iter; ++i) {
                printf(" ");
            }
            printf("{P%d}[%d][%d](d%d)-> %d<CACHED>\n", p.index, row, col, d, A[p.index][row][col][d]);
        }
        return A[p.index][row][col][d];
    }

    if (row + p.rowDiff < 0 || row + p.rowDiff >= rows || col + p.colDiff < 0 || col + p.colDiff >= cols) {
        // border
        A[p.index][row][col][d] = aggregatedCost;
        if(DEBUG) {
            for (unsigned int i = 0; i < iter; ++i) {
                printf(" ");
            }
            printf("{P%d}[%d][%d](d%d)-> %d <BORDER>\n", p.index, row, col, d, A[p.index][row][col][d]);
        }
        return A[p.index][row][col][d];

        return aggregatedCost;
    }

    unsigned short minPrev, minPrevOther, prev, prevPlus, prevMinus;
    prev = minPrev = minPrevOther = prevPlus = prevMinus = MAX_SHORT;

    for (int disp = 0; disp < disparityRange; ++disp) {
        unsigned short tmp = aggregateCost(row + p.rowDiff, col + p.colDiff, disp, p, rows, cols, disparityRange, C, A, S, ++iter);
        if(minPrev > tmp) {
            minPrev = tmp;
        }

        if(disp == d) {
            prev = tmp;
        } else if(disp == d + 1) {
            prevPlus = tmp;
        } else if (disp == d - 1) {
            prevMinus = tmp;
        } else {
            if(minPrevOther > tmp + LARGE_PENALTY) {
                minPrevOther = tmp + LARGE_PENALTY;
            }
        }
    }

    aggregatedCost += std::min(std::min((int)prevPlus + SMALL_PENALTY, (int)prevMinus + SMALL_PENALTY), std::min((int)prev, (int)minPrevOther + LARGE_PENALTY));
    aggregatedCost -= minPrev;

    A[p.index][row][col][d] = aggregatedCost;

    if(DEBUG) {
        for (unsigned int i = 0; i < iter; ++i) {
            printf(" ");
        }
        printf("{P%d}[%d][%d](d%d)-> %d<CALCULATED>\n", p.index, row, col, d, A[p.index][row][col][d]);
    }

    return aggregatedCost;
}

void aggregateCosts(int rows, int cols, int disparityRange, std::vector<path> &paths, unsigned short ***C, unsigned short ****A, unsigned short ***S) {
    for (unsigned long p = 0; p < paths.size(); ++p) {
        for (int row = 0; row < rows; ++row) {
            for (int col = 0; col < cols; ++col) {
                for (int d = 0; d < disparityRange; ++d) {
                    S[row][col][d] += aggregateCost(row, col, d, paths[p], rows, cols, disparityRange, C, A, S, 0);
                }
            }
        }
    }
}

void computeDisparity(unsigned short ***S, int rows, int cols, int disparityRange, cv::Mat &disparityMap) {
    unsigned int disparity, minCost;
    for (int row = 0; row < rows; ++row) {
        for (int col = 0; col < cols; ++col) {
            minCost = 65535;
            for (int d = disparityRange - 1; d >= 0; --d) {
                if(minCost > S[row][col][d]) {
                    minCost = S[row][col][d];
                    disparity = d;
                }
            }
            disparityMap.at<uchar>(row, col) = disparity;
        }
    }
}

void showDisparityMap(cv::Mat &disparityMap, int disparityRange) {
    double factor = 256.0 / disparityRange;
    for (int row = 0; row < disparityMap.rows; ++row) {
        for (int col = 0; col < disparityMap.cols; ++col) {
            disparityMap.at<uchar>(row, col) *= factor;
        }
    }
    cv::namedWindow("Disparity map", CV_WINDOW_AUTOSIZE);
    cv::imshow("Disparity map", disparityMap);
    cv::waitKey(0);
}

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

    if (argc != 4) {
        std::cerr << "Usage: " << argv[0] << " <left image> <right image> <disparity range>" << std::endl;
        return -1;
    }

    cv::Mat firstImage;
    cv::Mat secondImage;
    firstImage = cv::imread(argv[1], CV_LOAD_IMAGE_GRAYSCALE);
    secondImage = cv::imread(argv[2], CV_LOAD_IMAGE_GRAYSCALE);

    if(!firstImage.data || !secondImage.data) {
        std::cerr <<  "Could not open or find one of the images!" << std::endl;
        return -1;
    }

    unsigned int disparityRange = atoi(argv[3]);
    unsigned short ***C; // pixel cost array W x H x D
    unsigned short ***S; // aggregated cost array W x H x D
    unsigned short ****A; // path cost array P x W x H x D
    std::vector<path> paths;

    initializePaths(paths, PATH_COUNT);

    /*
    * TODO
    * variable LARGE_PENALTY
    */

    clock_t begin = clock();

    std::cout << "Allocating space..." << std::endl;

    // allocate cost arrays
    C = new unsigned short**[firstImage.rows];
    S = new unsigned short**[firstImage.rows];
    for (int row = 0; row < firstImage.rows; ++row) {
        C[row] = new unsigned short*[firstImage.cols];
        S[row] = new unsigned short*[firstImage.cols]();
        for (int col = 0; col < firstImage.cols; ++col) {
            C[row][col] = new unsigned short[disparityRange];
            S[row][col] = new unsigned short[disparityRange](); // initialize to 0
        }
    }

    A = new unsigned short ***[paths.size()];
    for (unsigned long p = 0; p < paths.size(); ++p) {
        A[p] = new unsigned short **[firstImage.rows];
        for (int row = 0; row < firstImage.rows; ++row) {
            A[p][row] = new unsigned short*[firstImage.cols];
            for (int col = 0; col < firstImage.cols; ++col) {
                A[p][row][col] = new unsigned short[disparityRange];
                for (unsigned int d = 0; d < disparityRange; ++d) {
                    A[p][row][col][d] = MAX_SHORT;
                }
            }
        }
    }

    std::cout << "Smoothing images..." << std::endl;
    grayscaleGaussianBlur(firstImage, firstImage, BLUR_RADIUS);
    grayscaleGaussianBlur(secondImage, secondImage, BLUR_RADIUS);

    std::cout << "Calculating pixel cost for the image..." << std::endl;
    calculatePixelCost(firstImage, secondImage, disparityRange, C);
    if(DEBUG) {
        printArray(C, firstImage.rows, firstImage.cols, disparityRange);
    }
    std::cout << "Aggregating costs..." << std::endl;
    aggregateCosts(firstImage.rows, firstImage.cols, disparityRange, paths, C, A, S);

    cv::Mat disparityMap = cv::Mat(cv::Size(firstImage.cols, firstImage.rows), CV_8UC1, cv::Scalar::all(0));

    std::cout << "Computing disparity..." << std::endl;
    computeDisparity(S, firstImage.rows, firstImage.cols, disparityRange, disparityMap);

    clock_t end = clock();
    double elapsed_secs = double(end - begin) / CLOCKS_PER_SEC;

    printf("Done in %.2lf seconds.\n", elapsed_secs);

    showDisparityMap(disparityMap, disparityRange);

    return 0;
}<|endoftext|>


<commit_before>#include "circular_vector.h"

#include <iostream>

void Print(circular_vector<int> input);

void TestMethods();
void TestObjectOperators();
void TestIteratorOperators();
void TestIteratorAccess();

int main() {

  TestMethods();

  TestObjectOperators();

  TestIteratorOperators();

  TestIteratorAccess();
}

// Prints all elements in the %circular_vector
void Print(circular_vector<int> input) {
  for (size_t x = 0; x < input.size(); ++x) {
    std::cout << input.at(x) << ",";
  }
  std::cout << std::endl;
}

// Test Methods
void TestMethods() {
  std::cout << "======================================================" << std::endl;
  std::cout << "TESTING CIRCULAR_VECTOR PUSH_BACK METHOD" << std::endl;

  circular_vector<int> test1;

  size_t count = 0;
  std::cout << "Pushing back: ";
  for (int x = 0; x < 13; ++x) {
    test1.push_back(x);
    std::cout << x << ",";
    ++count;
  }
  std::cout << std::endl;
  std::cout << "Printing circular_vector: ";
  Print(test1);
  if (count == test1.size())
    std::cout << "Push_back method passes" << std::endl;
  else
    std::cout << "Push_back method fails" << std::endl;

  std::cout << "======================================================" << std::endl;
  std::cout << "TESTING CIRCULAR_VECTOR POP_BACK METHOD" << std::endl;

  std::cout << "Popping back: ";
  for (int x = 0; x < 3; ++x) {
    test1.pop_back();
    std::cout << x << ",";
    --count;
  }
  std::cout << std::endl;
  std::cout << "Printing circular_vector: ";
  Print(test1);
  if (count == test1.size())
    std::cout << "Push_back method passes" << std::endl;
  else
    std::cout << "Push_back method fails" << std::endl;

  std::cout << "======================================================" << std::endl;
  std::cout << "TESTING CIRCULAR_VECTOR POP_FRONT METHOD" << std::endl;

  std::cout << "Popping front: ";
  for (int x = 0; x < 6; ++x) {
    test1.pop_front();
    std::cout << x << ",";
    --count;
  }
  std::cout << std::endl;
  std::cout << "Printing circular_vector: ";
  Print(test1);
  if (count == test1.size())
    std::cout << "Pop_front method passes" << std::endl;
  else
    std::cout << "Pop_front method fails" << std::endl;

  std::cout << "======================================================" << std::endl;
  std::cout << "TESTING CIRCULAR_VECTOR CLEAR METHOD" << std::endl;

  std::cout << "Clearing CIRCULAR_VECTOR...";
  test1.clear();
  count = 0;
  std::cout << std::endl;
  std::cout << "Printing circular_vector: ";
  Print(test1);
  if (test1.size() == 0)
    std::cout << "Clear method passes" << std::endl;
  else
    std::cout << "Clear method fails" << std::endl;

  std::cout << "======================================================" << std::endl;
  std::cout << "TESTING CIRCULAR_VECTOR PUSH_FRONT METHOD" << std::endl;

  std::cout << "Pushing front: ";
  for (int x = 0; x < 6; ++x) {
    test1.push_front(x);
    std::cout << x << ",";
    ++count;
  }
  std::cout << std::endl;
  std::cout << "Printing circular_vector: ";
  Print(test1);
  if (count == test1.size())
    std::cout << "Push_front method passes" << std::endl;
  else
    std::cout << "Push_front method fails" << std::endl;

  std::cout << "======================================================" << std::endl;
  std::cout << "TESTING CIRCULAR_VECTOR ASSIGN METHOD" << std::endl;

  std::cout << "ASSIGNING CIRCULAR_VECTOR TO: n = 3, val = 4...";
  test1.assign((size_t)3, (int)4);
  std::cout << std::endl;
  std::cout << "Printing circular_vector: ";
  Print(test1);
  if (test1.size() == 3)
    std::cout << "Assign method passes" << std::endl;
  else
    std::cout << "Assign method fails" << std::endl;
  std::cout << "Note: other assign prototype using iterators is used and tested interally" << std::endl;

  std::cout << "======================================================" << std::endl;
  std::cout << "TESTING CIRCULAR_VECTOR RESIZE METHOD" << std::endl;

  std::cout << "RESIZING CIRCULAR_VECTOR TO: n = 1, val = 2...";
  test1.resize(1, 2);
  std::cout << std::endl;
  std::cout << "Printing circular_vector: ";
  Print(test1);
  if (test1.size() == 1)
    std::cout << "Resize method passes" << std::endl;
  else
    std::cout << "Resize method fails" << std::endl;

  std::cout << "RESIZING CIRCULAR_VECTOR TO: n = 6, val = 2...";
  test1.resize(6, 2);
  std::cout << std::endl;
  std::cout << "Printing circular_vector: ";
  Print(test1);
  if (test1.size() == 6)
    std::cout << "Resize method passes" << std::endl;
  else
    std::cout << "Resize method fails" << std::endl;

  std::cout << "======================================================" << std::endl;
  std::cout << "TESTING CIRCULAR_VECTOR RESERVE METHOD" << std::endl;

  std::cout << "RESERVING CIRCULAR_VECTOR TO: n = 30...";
  test1.reserve(30);
  std::cout << std::endl;
  if (test1.capacity() == 30)
    std::cout << "Reserve method passes" << std::endl;
  else
    std::cout << "Reserve method fails" << std::endl;
  std::cout << "Note: tested and used more thoroughly interally" << std::endl;

  std::cout << "======================================================" << std::endl;
  std::cout << "TESTING CIRCULAR_VECTOR SWAP METHOD" << std::endl;

  circular_vector<int> test2;
  std::cout << "PUSHING BACK [0...9) to test2";
  for (int x = 0; x < 9; ++x) {
    test2.push_back(x);
  }
  std::cout << std::endl;
  std::cout << "Printing test1: ";
  Print(test1);
  std::cout << "Printing test2: ";
  Print(test2);
  std::cout << "SWAPPING..." << std::endl;
  test1.swap(test2);
  std::cout << "Printing test1: ";
  Print(test1);
  std::cout << "Printing test2: ";
  Print(test2);
  std::cout << "Note: tested and used more thoroughly interally" << std::endl;

}

// Test %circular_vector object relational operators
void TestObjectOperators() {
  std::cout << "======================================================" << std::endl;
  std::cout << "TESTING CIRCULAR_VECTOR OBJECT RELATIONAL OPERATORS" << std::endl;

  circular_vector<int> test1;
  circular_vector<int> test2;

  // TESTING %circular_vector == operator
  test1.clear();
  test1.resize(5, 1);
  test2.clear();
  test2.resize(5, 1);

  if (test1 == test2)
    std::cout << "== operator passes" << std::endl;
  else
    std::cout << "== operator fails" << std::endl;

  test1.clear();
  test2.clear();
  test1.resize(5,1);
  test2.resize(5,2);

  if (test1 == test2)
    std::cout << "== operator fails" << std::endl;
  else
    std::cout << "== operator passes" << std::endl;

  // TESTING %circular_vector < operator
  test1.clear();
  test2.clear();
  test1.resize(5,1);
  test2.resize(5,2);

  if (test1 < test2)
    std::cout << "< operator passes" << std::endl;
  else
    std::cout << "< operator fails" << std::endl;

  test1.clear();
  test2.clear();
  test1.resize(3,3);
  test2.resize(5,2);

  if (test1 < test2)
    std::cout << "< operator fails" << std::endl;
  else
    std::cout << "< operator passes" << std::endl;

  test1.clear();
  test2.clear();
  test1.resize(5,2);
  test2.resize(5,2);

  if (test1 < test2)
    std::cout << "< operator fails" << std::endl;
  else
    std::cout << "< operator passes" << std::endl;

  // TESTING %circular_vector > operator
  test1.clear();
  test2.clear();
  test1.resize(5,1);
  test2.resize(5,2);

  if (test1 > test2)
    std::cout << "> operator fails" << std::endl;
  else
    std::cout << "> operator passes" << std::endl;

  test1.clear();
  test2.clear();
  test1.resize(3,3);
  test2.resize(5,2);

  if (test1 > test2)
    std::cout << "> operator passes" << std::endl;
  else
    std::cout << "> operator fails" << std::endl;

  test1.clear();
  test2.clear();
  test1.resize(5,2);
  test2.resize(5,2);

  if (test1 > test2)
    std::cout << "> operator fails" << std::endl;
  else
    std::cout << "> operator passes" << std::endl;

  // TESTING %circular_vector <= operator
  test1.clear();
  test2.clear();
  test1.resize(5,1);
  test2.resize(5,2);

  if (test1 <= test2)
    std::cout << "<= operator passes" << std::endl;
  else
    std::cout << "<= operator fails" << std::endl;

  test1.clear();
  test2.clear();
  test1.resize(3,3);
  test2.resize(5,2);

  if (test1 <= test2)
    std::cout << "<= operator fails" << std::endl;
  else
    std::cout << "<= operator passes" << std::endl;

  test1.clear();
  test2.clear();
  test1.resize(5,2);
  test2.resize(5,2);

  if (test1 <= test2)
    std::cout << "<= operator passes" << std::endl;
  else
    std::cout << "<= operator fails" << std::endl;

  // TESTING %circular_vector >= operator
  test1.clear();
  test2.clear();
  test1.resize(5,1);
  test2.resize(5,2);

  if (test1 >= test2)
    std::cout << ">= operator fails" << std::endl;
  else
    std::cout << ">= operator passes" << std::endl;

  test1.clear();
  test2.clear();
  test1.resize(3,3);
  test2.resize(5,2);

  if (test1 >= test2)
    std::cout << ">= operator passes" << std::endl;
  else
    std::cout << ">= operator fails" << std::endl;

  test1.clear();
  test2.clear();
  test1.resize(5,2);
  test2.resize(5,2);

  if (test1 >= test2)
    std::cout << ">= operator passes" << std::endl;
  else
    std::cout << ">= operator fails" << std::endl;
}

void TestIteratorOperators() {
  std::cout << "======================================================" << std::endl;
  std::cout << "TESTING CIRCULAR_VECTOR ITERATOR RELATIONAL OPERATORS" << std::endl;

  circular_vector<int> test1;
  test1.resize(10, 4);

  // Testing == operator
  circular_vector<int>::iterator it = test1.begin();
  circular_vector<int>::iterator it2 = test1.begin()+1;
  if (it == it2)
    std::cout << "iterator == operator fails" << std::endl;
  else
    std::cout << "iterator == operator passes" << std::endl;
  --it2;
  if (it == it2)
    std::cout << "iterator == operator passes" << std::endl;
  else
    std::cout << "iterator == operator fails" << std::endl;

  // Testing > operator
  it = test1.begin();
  it2 = test1.begin();
  if (it > it2)
    std::cout << "iterator > operator fails" << std::endl;
  else
    std::cout << "iterator > operator passes" << std::endl;
  ++it;
  if (it > it2)
    std::cout << "iterator > operator passes" << std::endl;
  else
    std::cout << "iterator > operator fails" << std::endl;
  --it;
  ++it2;
  if (it > it2)
    std::cout << "iterator > operator fails" << std::endl;
  else
    std::cout << "iterator > operator passes" << std::endl;

  // Testing < operator
  it = test1.begin();
  it2 = test1.begin();
  if (it < it2)
    std::cout << "iterator < operator fails" << std::endl;
  else
    std::cout << "iterator < operator passes" << std::endl;
  ++it2;
  if (it < it2)
    std::cout << "iterator < operator passes" << std::endl;
  else
    std::cout << "iterator < operator fails" << std::endl;
  it = test1.begin();
  it2 = test1.begin();
  ++it;
  if (it < it2)
    std::cout << "iterator < operator fails" << std::endl;
  else
    std::cout << "iterator < operator passes" << std::endl;

  // Testing <= operator
  it = test1.begin();
  it2 = test1.begin();
  if (it <= it2)
    std::cout << "iterator <= operator passes" << std::endl;
  else
    std::cout << "iterator <= operator fails" << std::endl;
  ++it2;
  if (it <= it2)
    std::cout << "iterator <= operator passes" << std::endl;
  else
    std::cout << "iterator <= operator fails" << std::endl;
  it = test1.begin();
  it2 = test1.begin();
  ++it;
  if (it <= it2)
    std::cout << "iterator <= operator fails" << std::endl;
  else
    std::cout << "iterator <= operator passes" << std::endl;

  // Testing >= operator
  it = test1.begin();
  it2 = test1.begin();
  if (it >= it2)
    std::cout << "iterator >= operator passes" << std::endl;
  else
    std::cout << "iterator >= operator fails" << std::endl;
  ++it2;
  if (it >= it2)
    std::cout << "iterator >= operator fails" << std::endl;
  else
    std::cout << "iterator >= operator passes" << std::endl;
  it = test1.begin();
  it2 = test1.begin();
  ++it;
  if (it >= it2)
    std::cout << "iterator >= operator passes" << std::endl;
  else
    std::cout << "iterator >= operator fails" << std::endl;

}

void TestIteratorAccess() {
  std::cout << "======================================================" << std::endl;
  std::cout << "TESTING CIRCULAR_VECTOR ITERATOR LOOP ELEMENT ACCESS" << std::endl;

  circular_vector<int> test1;
  test1.resize(6, 4);
  std::cout << "Mutating circular_vector to 6 elements of value 4" << std::endl;

  circular_vector<int>::iterator it = test1.begin();

  std::cout << "Printing circular_vector: ";
  size_t count = 0;
  while (it != test1.end()) {
    std::cout << *it << ",";
    ++it;
    ++count;
  }
  std::cout << std::endl;

  if (count == test1.size())
    std::cout << "Iterator access passes" << std::endl;
  else
    std::cout << "Iterator access fails" << std::endl;

  test1.clear();
  count = 0;
  std::cout << "Mutating circular_vector to [0...13)" << std::endl;
  for (int x = 0; x < 13; ++x) {
    test1.push_back(x);
    std::cout << x << ",";
  }
  it = test1.begin();
  std::cout << std::endl;
  std::cout << "Printing circular_vector: ";
  while (it != test1.end()) {
    std::cout << *it << ",";
    ++it;
    ++count;
  }
  std::cout << std::endl;

  if (count == test1.size())
    std::cout << "Iterator access passes" << std::endl;
  else
    std::cout << "Iterator access fails" << std::endl;
}
<commit_msg>removed duplicated file<commit_after><|endoftext|>


<commit_before>/*
 * Copyright 2016 Konsulko Group
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 *     Unless required by applicable law or agreed to in writing, software
 *     distributed under the License is distributed on an "AS IS" BASIS,
 *     WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *     See the License for the specific language governing permissions and
 *     limitations under the License.
 */

#include <opencv2/core/cuda.hpp>
#include <opencv2/core/ocl.hpp>
#include <opencv2/cudaarithm.hpp>
#include <opencv2/cudaimgproc.hpp>
#include "opencv2/highgui/highgui.hpp"
#include <opencv2/imgproc/imgproc.hpp>
#include <tclap/CmdLine.h>
#include <libconfig.h++>
#include <iostream>
#include <string>

#include "config.h"
#include "fps.h"

using namespace std;
using namespace cv;
using namespace libconfig;

int main(int argc, char* argv[])
{
	string config_file;
	bool display_intermediate;
	bool enable_cuda;
	bool enable_opencl;
	bool enable_display;
	bool write_output;
	bool verbose;

	// Parse command line options
	try {
		TCLAP::CmdLine cmd_line("Lane Departure Warning System", ' ', LDWS_VERSION);
		// FIXME CUDA and OpenCL should be mutually exclusive switches
		TCLAP::SwitchArg enable_cuda_switch("u","enable-cuda","Enable CUDA support", cmd_line, false);
		TCLAP::SwitchArg enable_opencl_switch("o","enable-opencl","Enable OpenCL support", cmd_line, false);
		TCLAP::SwitchArg disable_display_switch("d","disable-display","Disable video display", cmd_line, false);
		TCLAP::SwitchArg display_intermediate_switch("i","display-intermediate","Display intermediate processing steps", cmd_line, false);
		TCLAP::SwitchArg write_output_switch("w","write-video","Write video to a file", cmd_line, false);
		TCLAP::SwitchArg verbose_switch("v","verbose","Verbose messages", cmd_line, false);
		TCLAP::ValueArg<string> config_file_string("c","config-file","Configuration file name", false, "ldws.conf", "filename");
		cmd_line.add(config_file_string);
		cmd_line.parse(argc, argv);

		display_intermediate = display_intermediate_switch.getValue();
		enable_cuda = enable_cuda_switch.getValue();
		enable_opencl = enable_opencl_switch.getValue();
		enable_display = !disable_display_switch.getValue();
		write_output = write_output_switch.getValue();
		verbose = verbose_switch.getValue();
		config_file = config_file_string.getValue();
	} catch (TCLAP::ArgException &e) {
		std::cerr << "error: " << e.error() << " for arg " << e.argId() << std::endl;
	}

	// Parse config file
	Config cfg;
	cfg.readFile(config_file.c_str());
	string video_file = cfg.lookup("video_file");

	// Open video input file/device
	VideoCapture capture(video_file);
	// If file open fails, try finding a camera indicated by an integer argument
	if (!capture.isOpened())
	{capture.open(atoi(video_file.c_str()));}

	// Toggle OpenCL on/off
	if (!enable_cuda)
		cv::ocl::setUseOpenCL(enable_opencl);

	string mode = "CPU";
	if (enable_cuda)
		mode = "CUDA";
	else if (enable_opencl)
		mode = "OpenCL";
	cout << "Mode: " << mode << endl;

	// Report video specs
	double width = capture.get(CV_CAP_PROP_FRAME_WIDTH);
	double height = capture.get(CV_CAP_PROP_FRAME_HEIGHT);
	int ex = static_cast<int>(capture.get(CV_CAP_PROP_FOURCC));
	char fourcc[] = {(char)(ex & 0XFF),(char)((ex & 0XFF00) >> 8),(char)((ex & 0XFF0000) >> 16),(char)((ex & 0XFF000000) >> 24),0};
	Size frame_size(static_cast<int>(width), static_cast<int>(height));
	cout << "Video: frame size " << width << "x" << height << ", codec " << fourcc << endl;

	// Create output window
	string window_name = "Full Video";
	if (enable_display) {
		namedWindow(window_name, CV_WINDOW_KEEPRATIO);
	}

	// FIXME this should be conditional
	VideoWriter output_writer("ldws-full.avi", CV_FOURCC('P','I','M','1'), 30, frame_size, true);

	Mat frame;
	cv::cuda::GpuMat gpu_frame, gpu_gray, gpu_edge, gpu_edge_inv;
	UMat u_frame, u_gray, u_edge, u_edge_inv;
	cv::Ptr<cv::cuda::CannyEdgeDetector> canny = cv::cuda::createCannyEdgeDetector(80, 250, 3, false);

	frame_avg_init();

	while (true)
	{
		capture >> frame;
		if (frame.empty())
			break;

		// Display original frame
		if (display_intermediate) {
			//namedWindow("Original Video", WINDOW_AUTOSIZE);
			imshow("Original Video", frame);
		}

		// TODO Add ROI support

		frame_begin();

		if (enable_cuda) {
			// CUDA
			gpu_frame.upload(frame);
			cv::cuda::cvtColor(gpu_frame, gpu_gray, CV_BGR2GRAY);
			canny->detect(gpu_gray, gpu_edge);
			cv::cuda::threshold(gpu_edge, gpu_edge_inv, 128, 255, THRESH_BINARY_INV);
		} else {
			// TAPI
			frame.copyTo(u_frame);
			cvtColor(u_frame, u_gray, CV_BGR2GRAY);
			Canny(u_frame, u_edge, 80, 250);
			threshold(u_edge, u_edge_inv, 128, 255, THRESH_BINARY_INV);
		}

		// TODO Add line detection

		frame_end();

		// Display Canny image
		if (display_intermediate) {
			//namedWindow("Contours");
			if (enable_cuda)
				imshow("Edges", gpu_edge_inv);
			else
				imshow("Edges", u_edge_inv);
		}

		// Display FPS
		putText(frame, "FPS: " + frame_fps_str(), Point(5,25), FONT_HERSHEY_SIMPLEX, 1., Scalar(255, 100, 0), 2);

		// Display full image
		if (enable_display)
			imshow(window_name, frame);

		// Write frame to output file
		if (write_output)
			output_writer << frame;

		if (waitKey(1) == 27) break;
	}

	cout << "Average FPS: " << frame_fps_avg_str() << endl;
}

<commit_msg>Fix intermediate image display on CUDA<commit_after>/*
 * Copyright 2016 Konsulko Group
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 *     Unless required by applicable law or agreed to in writing, software
 *     distributed under the License is distributed on an "AS IS" BASIS,
 *     WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *     See the License for the specific language governing permissions and
 *     limitations under the License.
 */

#include <opencv2/core/cuda.hpp>
#include <opencv2/core/ocl.hpp>
#include <opencv2/cudaarithm.hpp>
#include <opencv2/cudaimgproc.hpp>
#include "opencv2/highgui/highgui.hpp"
#include <opencv2/imgproc/imgproc.hpp>
#include <tclap/CmdLine.h>
#include <libconfig.h++>
#include <iostream>
#include <string>

#include "config.h"
#include "fps.h"

using namespace std;
using namespace cv;
using namespace libconfig;

int main(int argc, char* argv[])
{
	string config_file;
	bool display_intermediate;
	bool enable_cuda;
	bool enable_opencl;
	bool enable_display;
	bool write_output;
	bool verbose;

	// Parse command line options
	try {
		TCLAP::CmdLine cmd_line("Lane Departure Warning System", ' ', LDWS_VERSION);
		// FIXME CUDA and OpenCL should be mutually exclusive switches
		TCLAP::SwitchArg enable_cuda_switch("u","enable-cuda","Enable CUDA support", cmd_line, false);
		TCLAP::SwitchArg enable_opencl_switch("o","enable-opencl","Enable OpenCL support", cmd_line, false);
		TCLAP::SwitchArg disable_display_switch("d","disable-display","Disable video display", cmd_line, false);
		TCLAP::SwitchArg display_intermediate_switch("i","display-intermediate","Display intermediate processing steps", cmd_line, false);
		TCLAP::SwitchArg write_output_switch("w","write-video","Write video to a file", cmd_line, false);
		TCLAP::SwitchArg verbose_switch("v","verbose","Verbose messages", cmd_line, false);
		TCLAP::ValueArg<string> config_file_string("c","config-file","Configuration file name", false, "ldws.conf", "filename");
		cmd_line.add(config_file_string);
		cmd_line.parse(argc, argv);

		display_intermediate = display_intermediate_switch.getValue();
		enable_cuda = enable_cuda_switch.getValue();
		enable_opencl = enable_opencl_switch.getValue();
		enable_display = !disable_display_switch.getValue();
		write_output = write_output_switch.getValue();
		verbose = verbose_switch.getValue();
		config_file = config_file_string.getValue();
	} catch (TCLAP::ArgException &e) {
		std::cerr << "error: " << e.error() << " for arg " << e.argId() << std::endl;
	}

	// Parse config file
	Config cfg;
	cfg.readFile(config_file.c_str());
	string video_file = cfg.lookup("video_file");

	// Open video input file/device
	VideoCapture capture(video_file);
	// If file open fails, try finding a camera indicated by an integer argument
	if (!capture.isOpened())
	{capture.open(atoi(video_file.c_str()));}

	// Toggle OpenCL on/off
	if (!enable_cuda)
		cv::ocl::setUseOpenCL(enable_opencl);

	string mode = "CPU";
	if (enable_cuda)
		mode = "CUDA";
	else if (enable_opencl)
		mode = "OpenCL";
	cout << "Mode: " << mode << endl;

	// Report video specs
	double width = capture.get(CV_CAP_PROP_FRAME_WIDTH);
	double height = capture.get(CV_CAP_PROP_FRAME_HEIGHT);
	int ex = static_cast<int>(capture.get(CV_CAP_PROP_FOURCC));
	char fourcc[] = {(char)(ex & 0XFF),(char)((ex & 0XFF00) >> 8),(char)((ex & 0XFF0000) >> 16),(char)((ex & 0XFF000000) >> 24),0};
	Size frame_size(static_cast<int>(width), static_cast<int>(height));
	cout << "Video: frame size " << width << "x" << height << ", codec " << fourcc << endl;

	// Create output window
	string window_name = "Full Video";
	if (enable_display) {
		namedWindow(window_name, CV_WINDOW_KEEPRATIO);
	}

	// FIXME this should be conditional
	VideoWriter output_writer("ldws-full.avi", CV_FOURCC('P','I','M','1'), 30, frame_size, true);

	Mat frame, edge_inv;
	cv::cuda::GpuMat gpu_frame, gpu_gray, gpu_edge, gpu_edge_inv;
	UMat u_frame, u_gray, u_edge, u_edge_inv;
	cv::Ptr<cv::cuda::CannyEdgeDetector> canny = cv::cuda::createCannyEdgeDetector(80, 250, 3, false);

	frame_avg_init();

	while (true)
	{
		capture >> frame;
		if (frame.empty())
			break;

		// Display original frame
		if (display_intermediate) {
			//namedWindow("Original Video", WINDOW_AUTOSIZE);
			imshow("Original Video", frame);
		}

		// TODO Add ROI support

		frame_begin();

		if (enable_cuda) {
			// CUDA
			gpu_frame.upload(frame);
			cv::cuda::cvtColor(gpu_frame, gpu_gray, CV_BGR2GRAY);
			canny->detect(gpu_gray, gpu_edge);
			cv::cuda::threshold(gpu_edge, gpu_edge_inv, 128, 255, THRESH_BINARY_INV);
		} else {
			// TAPI
			frame.copyTo(u_frame);
			cvtColor(u_frame, u_gray, CV_BGR2GRAY);
			Canny(u_frame, u_edge, 80, 250);
			threshold(u_edge, u_edge_inv, 128, 255, THRESH_BINARY_INV);
		}

		// TODO Add line detection

		frame_end();

		// Display Canny image
		if (display_intermediate) {
			//namedWindow("Contours");
			if (enable_cuda) {
				gpu_edge_inv.download(edge_inv);
				imshow("Edges", edge_inv);
			} else
				imshow("Edges", u_edge_inv);
		}

		// Display FPS
		putText(frame, "FPS: " + frame_fps_str(), Point(5,25), FONT_HERSHEY_SIMPLEX, 1., Scalar(255, 100, 0), 2);

		// Display full image
		if (enable_display)
			imshow(window_name, frame);

		// Write frame to output file
		if (write_output)
			output_writer << frame;

		if (waitKey(1) == 27) break;
	}

	cout << "Average FPS: " << frame_fps_avg_str() << endl;
}

<|endoftext|>


<commit_before>#include "gmml/internal/geometry.h"

namespace gmml {

RotationMatrix::RotationMatrix(const Coordinate& point, Vector<3> direction,
                               double radians) {
    direction.normalize();
    double u = direction[0];
    double v = direction[1];
    double w = direction[2];

    double a = point.x;
    double b = point.y;
    double c = point.z;

    double u2 = u*u;
    double v2 = v*v;
    double w2 = w*w;
    double cos_t = cos(radians);
    double sin_t = sin(radians);

    matrix_[0][3] = a*(v2 + w2) - u*(b*v + c*w) +
                 (u*(b*v + c*w) - a*(v2 + w2))*cos_t + (b*w - c*v)*sin_t;

    matrix_[1][3] = b*(u2 + w2) - v*(a*u + c*w) +
                 (v*(a*u + c*w) - b*(u2 + w2))*cos_t + (c*u - a*w)*sin_t;

    matrix_[2][3] = c*(u2 + v2) - w*(a*u + b*v) +
                  (w*(a*u + b*v) - c*(u2 + v2))*cos_t + (a*v - b*u)*sin_t;

    matrix_[0][0] = u2 + (v2 + w2)*cos_t;
    matrix_[0][1] = u*v*(1 - cos_t) - w*sin_t;
    matrix_[0][2] = u*w*(1 - cos_t) + v*sin_t;

    matrix_[1][0] = u*v*(1 - cos_t) + w*sin_t;
    matrix_[1][1] = v2 + (u2 + w2)*cos_t;
    matrix_[1][2] = v*w*(1 - cos_t) - u*sin_t;

    matrix_[2][0] = u*w*(1 - cos_t) - v*sin_t;
    matrix_[2][1] = v*w*(1 - cos_t) + u*sin_t;
    matrix_[2][2] = w2 + (u2 + v2)*cos_t;
}

// TODO: fix this up.
Coordinate calculate_point(const Coordinate& a, const Coordinate& b,
                           const Coordinate& c, double angle,
                           double dihedral, double distance) {
    dihedral = kPi - dihedral;

    Matrix<3, 4> m;
    Vector<3> v1(b, a);
    Vector<3> v2(c, b);

    v1.normalize();
    v2.normalize();

    // if v1 is a multiple of v2, a, b, and c are collinear
    // change to numeric utilities
    if (std::abs(v1[0] + v2[0]) < 0.001 &&
            std::abs(v1[1] + v2[1]) < 0.001 &&
            std::abs(v1[2] + v2[2]) < 0.001) {
        // use an arbitary dummy coordinate
        Coordinate a_p(a.x+10, a.y-1, a.z+3);
        v1 = Vector<3>(b, a_p);
    }

    m.set_column(1, cross(v1, v2).normalize());
    m.set_column(2, v2);
    m.set_column(0, cross(m.column(1), m.column(2)));
    m.set_column(3, Vector<3>(c));
    Vector<4> v;
    v[0] = distance*sin(angle)*cos(dihedral);
    v[1] = distance*sin(angle)*sin(dihedral);
    v[2] = distance*cos(angle);
    v[3] = 1;

    Vector<3> ret = m*v;

    return Coordinate(ret[0], ret[1], ret[2]);
}

}  // namespace gmml
<commit_msg>Add reference for algorithm.<commit_after>#include "gmml/internal/geometry.h"

namespace gmml {

RotationMatrix::RotationMatrix(const Coordinate& point, Vector<3> direction,
                               double radians) {
    direction.normalize();
    double u = direction[0];
    double v = direction[1];
    double w = direction[2];

    double a = point.x;
    double b = point.y;
    double c = point.z;

    double u2 = u*u;
    double v2 = v*v;
    double w2 = w*w;
    double cos_t = cos(radians);
    double sin_t = sin(radians);

    matrix_[0][3] = a*(v2 + w2) - u*(b*v + c*w) +
                 (u*(b*v + c*w) - a*(v2 + w2))*cos_t + (b*w - c*v)*sin_t;

    matrix_[1][3] = b*(u2 + w2) - v*(a*u + c*w) +
                 (v*(a*u + c*w) - b*(u2 + w2))*cos_t + (c*u - a*w)*sin_t;

    matrix_[2][3] = c*(u2 + v2) - w*(a*u + b*v) +
                  (w*(a*u + b*v) - c*(u2 + v2))*cos_t + (a*v - b*u)*sin_t;

    matrix_[0][0] = u2 + (v2 + w2)*cos_t;
    matrix_[0][1] = u*v*(1 - cos_t) - w*sin_t;
    matrix_[0][2] = u*w*(1 - cos_t) + v*sin_t;

    matrix_[1][0] = u*v*(1 - cos_t) + w*sin_t;
    matrix_[1][1] = v2 + (u2 + w2)*cos_t;
    matrix_[1][2] = v*w*(1 - cos_t) - u*sin_t;

    matrix_[2][0] = u*w*(1 - cos_t) - v*sin_t;
    matrix_[2][1] = v*w*(1 - cos_t) + u*sin_t;
    matrix_[2][2] = w2 + (u2 + v2)*cos_t;
}

// See the NeRF algorithm in
// Jerod Parsons, J. Bradley Holmes, J. Maurice Rojas, Jerry Tsai, Charlie E. M. Strauss (2005). "Practical conversion from torsion space to Cartesian space for in silico protein synthesis". Journal of Computational Chemistry 26 (10): 1063–1068
// TODO: fix this up.
Coordinate calculate_point(const Coordinate& a, const Coordinate& b,
                           const Coordinate& c, double angle,
                           double dihedral, double distance) {
    dihedral = kPi - dihedral;

    Matrix<3, 4> m;
    Vector<3> v1(b, a);
    Vector<3> v2(c, b);

    v1.normalize();
    v2.normalize();

    // if v1 is a multiple of v2, a, b, and c are collinear
    // change to numeric utilities
    if (std::abs(v1[0] + v2[0]) < 0.001 &&
            std::abs(v1[1] + v2[1]) < 0.001 &&
            std::abs(v1[2] + v2[2]) < 0.001) {
        // use an arbitary dummy coordinate
        Coordinate a_p(a.x+10, a.y-1, a.z+3);
        v1 = Vector<3>(b, a_p);
    }

    m.set_column(1, cross(v1, v2).normalize());
    m.set_column(2, v2);
    m.set_column(0, cross(m.column(1), m.column(2)));
    m.set_column(3, Vector<3>(c));
    Vector<4> v;
    v[0] = distance*sin(angle)*cos(dihedral);
    v[1] = distance*sin(angle)*sin(dihedral);
    v[2] = distance*cos(angle);
    v[3] = 1;

    Vector<3> ret = m*v;

    return Coordinate(ret[0], ret[1], ret[2]);
}

}  // namespace gmml
<|endoftext|>


<commit_before>/*****************************************************************************
 * Media Library
 *****************************************************************************
 * Copyright (C) 2015-2019 Hugo Beauzée-Luyssen, Videolabs, VideoLAN
 *
 * Authors: Hugo Beauzée-Luyssen <hugo@beauzee.fr>
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation; either version 2.1 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with this program; if not, write to the Free Software Foundation,
 * Inc., 51 Franklin Street, Fifth Floor, Boston MA 02110-1301, USA.
 *****************************************************************************/

#if HAVE_CONFIG_H
# include "config.h"
#endif

#include "Directory.h"
#include "Media.h"
#include "Device.h"
#include "filesystem/unix/File.h"
#include "logging/Logger.h"
#include "utils/Filename.h"
#include "utils/Directory.h"
#include "utils/Url.h"
#include "medialibrary/filesystem/Errors.h"

#include <cstring>
#include <dirent.h>
#include <sys/stat.h>
#include <unistd.h>

namespace medialibrary
{

namespace fs
{

Directory::Directory( const std::string& mrl, fs::IFileSystemFactory& fsFactory )
    : CommonDirectory( fsFactory )
{
    m_path = utils::file::toFolderPath(
                utils::fs::toAbsolute( utils::file::toLocalPath( mrl ) ) );
    assert( *m_path.crbegin() == '/' );
    m_mrl = utils::file::toMrl( m_path );
}

const std::string& Directory::mrl() const
{
    return m_mrl;
}

void Directory::read() const
{
    std::unique_ptr<DIR, int(*)(DIR*)> dir( opendir( m_path.c_str() ), closedir );
    if ( dir == nullptr )
    {
        LOG_ERROR( "Failed to open directory ", m_path );
        throw errors::System( errno, "Failed to open directory" );
    }

    dirent* result = nullptr;

    while ( ( result = readdir( dir.get() ) ) != nullptr )
    {
        if ( result->d_name[0] == '.' && strcasecmp( result->d_name, ".nomedia" ) != 0 )
            continue;

        std::string path = m_path + result->d_name;

        struct stat s;
        if ( lstat( path.c_str(), &s ) != 0 )
        {
            if ( errno == EACCES )
                continue;
            // some Android devices will list folder content, but will yield
            // ENOENT when accessing those.
            // See https://trac.videolan.org/vlc/ticket/19909
            if ( errno == ENOENT )
            {
                LOG_WARN( "Ignoring unexpected ENOENT while listing folder content." );
                continue;
            }
            // Ignore EOVERFLOW since we are not (yet?) interested in the file size
            if ( errno != EOVERFLOW )
            {
                LOG_ERROR( "Failed to get file ", path, " info" );
                throw errors::System{ errno, "Failed to get file info" };
            }
        }
        try
        {
            if ( S_ISDIR( s.st_mode ) )
                m_dirs.emplace_back( std::make_shared<Directory>(
                            m_mrl + utils::url::encode( result->d_name ), m_fsFactory ) );
            else
                m_files.emplace_back( std::make_shared<File>( path, s ) );
        }
        catch ( const errors::System& err )
        {
            if ( err.code() == std::errc::no_such_file_or_directory )
            {
                LOG_WARN( "Ignoring ", path, ": ", err.what() );
                continue;
            }
            LOG_ERROR( "Fatal error while reading ", path, ": ", err.what() );
            throw;
        }
    }
}

}

}
<commit_msg>fs: unix directory: Don't ignore EOVERFLOW<commit_after>/*****************************************************************************
 * Media Library
 *****************************************************************************
 * Copyright (C) 2015-2019 Hugo Beauzée-Luyssen, Videolabs, VideoLAN
 *
 * Authors: Hugo Beauzée-Luyssen <hugo@beauzee.fr>
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation; either version 2.1 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with this program; if not, write to the Free Software Foundation,
 * Inc., 51 Franklin Street, Fifth Floor, Boston MA 02110-1301, USA.
 *****************************************************************************/

#if HAVE_CONFIG_H
# include "config.h"
#endif

#include "Directory.h"
#include "Media.h"
#include "Device.h"
#include "filesystem/unix/File.h"
#include "logging/Logger.h"
#include "utils/Filename.h"
#include "utils/Directory.h"
#include "utils/Url.h"
#include "medialibrary/filesystem/Errors.h"

#include <cstring>
#include <dirent.h>
#include <sys/stat.h>
#include <unistd.h>

namespace medialibrary
{

namespace fs
{

Directory::Directory( const std::string& mrl, fs::IFileSystemFactory& fsFactory )
    : CommonDirectory( fsFactory )
{
    m_path = utils::file::toFolderPath(
                utils::fs::toAbsolute( utils::file::toLocalPath( mrl ) ) );
    assert( *m_path.crbegin() == '/' );
    m_mrl = utils::file::toMrl( m_path );
}

const std::string& Directory::mrl() const
{
    return m_mrl;
}

void Directory::read() const
{
    std::unique_ptr<DIR, int(*)(DIR*)> dir( opendir( m_path.c_str() ), closedir );
    if ( dir == nullptr )
    {
        LOG_ERROR( "Failed to open directory ", m_path );
        throw errors::System( errno, "Failed to open directory" );
    }

    dirent* result = nullptr;

    while ( ( result = readdir( dir.get() ) ) != nullptr )
    {
        if ( result->d_name[0] == '.' && strcasecmp( result->d_name, ".nomedia" ) != 0 )
            continue;

        std::string path = m_path + result->d_name;

        struct stat s;
        if ( lstat( path.c_str(), &s ) != 0 )
        {
            if ( errno == EACCES )
                continue;
            // some Android devices will list folder content, but will yield
            // ENOENT when accessing those.
            // See https://trac.videolan.org/vlc/ticket/19909
            if ( errno == ENOENT )
            {
                LOG_WARN( "Ignoring unexpected ENOENT while listing folder content." );
                continue;
            }
            LOG_ERROR( "Failed to get file ", path, " info" );
            throw errors::System{ errno, "Failed to get file info" };
        }
        try
        {
            if ( S_ISDIR( s.st_mode ) )
                m_dirs.emplace_back( std::make_shared<Directory>(
                            m_mrl + utils::url::encode( result->d_name ), m_fsFactory ) );
            else
                m_files.emplace_back( std::make_shared<File>( path, s ) );
        }
        catch ( const errors::System& err )
        {
            if ( err.code() == std::errc::no_such_file_or_directory )
            {
                LOG_WARN( "Ignoring ", path, ": ", err.what() );
                continue;
            }
            LOG_ERROR( "Fatal error while reading ", path, ": ", err.what() );
            throw;
        }
    }
}

}

}
<|endoftext|>