frame_count.cpp

/*
 *  V4L2 video capture example
 *
 *  This program can be used and distributed without restrictions.
 *
 *      This program is provided with the V4L2 API
 * see https://linuxtv.org/docs.php for more information
 */

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>

#include <getopt.h>             /* getopt_long() */

#include <fcntl.h>              /* low-level i/o */
#include <unistd.h>
#include <errno.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <sys/time.h>
#include <sys/mman.h>
#include <sys/ioctl.h>

#include <linux/videodev2.h>

#include <vector>
#include <string>

#define CLEAR(x) memset(&(x), 0, sizeof(x))
#define MIN_WARMUP_TIME 10.0

struct buffer {
        void   *start;
        size_t  length;
};

double time_diff(const struct timeval& t1, const struct timeval& t0)
{
    double dt = (t1.tv_sec - t0.tv_sec) + (t1.tv_usec - t0.tv_usec) / 1000000.0;
}

class CameraStats {
    public:
        CameraStats() :
            max_gap(0),
            total_num_frames(0),
            total_counted_frames(0),
            effective_frame_rate(0),
            num_slow_10_pct(0),
            num_slow_100_pct(0),
            num_slow_200_pct(0),
            expected_frame_rate(30),
            expected_frame_time(1.0 / 30.0)
        {
            last_read_time.tv_sec = 0;
            last_read_time.tv_usec = 0;

            first_read_time.tv_sec = 0;
            first_read_time.tv_usec = 0;
        }

        void set_frame_rate(double fps)
        {
            expected_frame_rate = fps;
            expected_frame_time = 1.0 / fps;
        }

        void count_frame()
        {
            struct timeval now;
            gettimeofday(&now, NULL);

            if(total_num_frames == 0)
            {
                first_read_time = now;
            }
            else if(time_diff(now, first_read_time) > MIN_WARMUP_TIME)
            {
                double dt = time_diff(now, last_read_time);
                if(dt > max_gap) {
                    max_gap = dt;
                }

                if(dt < 1.1 * expected_frame_time) {
                    fprintf(stderr, ".");
                }
                else if(dt < 2.0 * expected_frame_time) {
                    fprintf(stderr, "+");
                    num_slow_10_pct++;
                }
                else if(dt < 3.0 * expected_frame_time) {
                    fprintf(stderr, "*");
                    num_slow_100_pct++;
                }
                else {
                    fprintf(stderr, "!");
                    num_slow_200_pct++;
                }
                total_counted_frames++;
                effective_frame_rate =
                    total_counted_frames / time_diff(now, first_read_time);
            }
            total_num_frames++;
            last_read_time = now;
        }

        void print_report()
        {
            double capture_time = time_diff(last_read_time, first_read_time);
            fprintf(stdout, "capture time: %f\n", capture_time);
            fprintf(stdout, "effective frame rate: %f\n", effective_frame_rate);
            fprintf(stdout, "number 10%% slow frames: %d\n", num_slow_10_pct);
            fprintf(stdout, "number 100%% slow frames: %d\n", num_slow_100_pct);
            fprintf(stdout, "number >200%% slow frames: %d\n", num_slow_200_pct);
            fprintf(stdout, "maximum gap between frames: %f\n", max_gap);
            //fprintf(stderr, "biggest gap: %f\n", );
            //fprintf(stderr, "number of slow frames: %d\n", num_slow_frames);

        }

    private:
        double  max_gap;
        int     total_num_frames;
        int     total_counted_frames;
        double  effective_frame_rate;
        int num_slow_10_pct;
        int num_slow_100_pct;
        int num_slow_200_pct;
        struct  timeval last_read_time;
        struct  timeval first_read_time;
        double  expected_frame_rate;
        double  expected_frame_time;

};

class Camera {
        public:
                std::string             dev_name_;
                int                     fd_;
                struct buffer           *buffers_;
                unsigned int     n_buffers_;

        Camera();
        ~Camera();

                void open_device(const std::string& dev_name);
                void close_device();

        void init_device();
        void uninit_device();

        void start_capturing();
        void stop_capturing();

        int read_frame();

    private:
        bool open_;
        bool initialized_;
        bool capturing_;

        void init_mmap();
};

static void errno_exit(const char *s)
{
        fprintf(stderr, "%s error %d, %s\\n", s, errno, strerror(errno));
        exit(EXIT_FAILURE);
}

static int xioctl(int fh, int request, void *arg)
{
        int r;

        do {
                r = ioctl(fh, request, arg);
        } while (-1 == r && EINTR == errno);

        return r;
}

Camera::Camera()
    :
    dev_name_(""),
    fd_(-1),
    buffers_(NULL),
    n_buffers_(0),
    open_(false),
    initialized_(false)
{
}

Camera::~Camera()
{
    if(capturing_)
        stop_capturing();
    if(initialized_)
        uninit_device();
    if(open_)
        close_device();
}

void Camera::close_device(void)
{
        if (-1 == close(fd_))
                errno_exit("close");

        fd_ = -1;
    open_ = false;
}

void Camera::open_device(const std::string& dev_name)
{
        struct stat st;

        if (-1 == stat(dev_name.c_str(), &st)) {
                fprintf(stderr, "Cannot identify '%s': %d, %s\n",
                                dev_name.c_str(), errno, strerror(errno));
                exit(EXIT_FAILURE);
        }

        if (!S_ISCHR(st.st_mode)) {
                fprintf(stderr, "%s is no devicen", dev_name.c_str());
                exit(EXIT_FAILURE);
        }

        fd_ = open(dev_name.c_str(), O_RDWR /* required */ | O_NONBLOCK, 0);

        if (-1 == fd_) {
                fprintf(stderr, "Cannot open '%s': %d, %s\n",
                                dev_name.c_str(), errno, strerror(errno));
                exit(EXIT_FAILURE);
        }

        dev_name_ = std::string(dev_name.c_str());
    open_ = true;
}

void Camera::init_device()
{
        struct v4l2_capability cap;
        struct v4l2_cropcap cropcap;
        struct v4l2_crop crop;
        struct v4l2_format fmt;
        unsigned int min;

        if (-1 == xioctl(fd_, VIDIOC_QUERYCAP, &cap)) {
                if (EINVAL == errno) {
                        fprintf(stderr, "%s is no V4L2 device\n",
                                        dev_name_.c_str());
                        exit(EXIT_FAILURE);
                } else {
                        errno_exit("VIDIOC_QUERYCAP");
                }
        }

        if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
                fprintf(stderr, "%s is no video capture device\n",
                                dev_name_.c_str());
                exit(EXIT_FAILURE);
        }

        if (!(cap.capabilities & V4L2_CAP_STREAMING)) {
                fprintf(stderr, "%s does not support streaming i/o\n",
                                dev_name_.c_str());
                exit(EXIT_FAILURE);
        }


        /* Select video input, video standard and tune here. */


        CLEAR(cropcap);

        cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

        if (0 == xioctl(fd_, VIDIOC_CROPCAP, &cropcap)) {
                crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
                crop.c = cropcap.defrect; /* reset to default */

                if (-1 == xioctl(fd_, VIDIOC_S_CROP, &crop)) {
                        switch (errno) {
                                case EINVAL:
                                        /* Cropping not supported. */
                                        break;
                                default:
                                        /* Errors ignored. */
                                        break;
                        }
                }
        } else {
                /* Errors ignored. */
        }


        CLEAR(fmt);

        fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

#if 0
        if (force_format) {
                fmt.fmt.pix.width       = 640;
                fmt.fmt.pix.height      = 480;
                fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
                fmt.fmt.pix.field       = V4L2_FIELD_INTERLACED;

                if (-1 == xioctl(fd_, VIDIOC_S_FMT, &fmt))
                        errno_exit("VIDIOC_S_FMT");

                /* Note VIDIOC_S_FMT may change width and height. */
        }
    else
#endif
    {
                /* Preserve original settings as set by v4l2-ctl for example */
                if (-1 == xioctl(fd_, VIDIOC_G_FMT, &fmt))
                        errno_exit("VIDIOC_G_FMT");
        }

        /* Buggy driver paranoia. */
        min = fmt.fmt.pix.width * 2;
        if (fmt.fmt.pix.bytesperline < min)
                fmt.fmt.pix.bytesperline = min;
        min = fmt.fmt.pix.bytesperline * fmt.fmt.pix.height;
        if (fmt.fmt.pix.sizeimage < min)
                fmt.fmt.pix.sizeimage = min;
#warning "DISABLE"
        init_mmap();

    initialized_ = true;
}

void Camera::init_mmap()
{
    struct v4l2_requestbuffers req;

    CLEAR(req);

    req.count = 4;
    req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    req.memory = V4L2_MEMORY_MMAP;

    if (-1 == xioctl(fd_, VIDIOC_REQBUFS, &req)) {
        if (EINVAL == errno) {
            fprintf(stderr, "%s does not support "
                    "memory mapping", dev_name_.c_str());
            exit(EXIT_FAILURE);
        } else {
            errno_exit("VIDIOC_REQBUFS");
        }
    }

    if (req.count < 2) {
        fprintf(stderr, "Insufficient buffer memory on %s\\n",
                dev_name_.c_str());
        exit(EXIT_FAILURE);
    }

    buffers_ = (buffer *)calloc(req.count, sizeof(*buffers_));

    if (!buffers_) {
        fprintf(stderr, "Out of memory\\n");
        exit(EXIT_FAILURE);
    }

    for (n_buffers_ = 0; n_buffers_ < req.count; ++n_buffers_) {
        struct v4l2_buffer buf;

        CLEAR(buf);

        buf.type        = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        buf.memory      = V4L2_MEMORY_MMAP;
        buf.index       = n_buffers_;

        if (-1 == xioctl(fd_, VIDIOC_QUERYBUF, &buf))
            errno_exit("VIDIOC_QUERYBUF");

        buffers_[n_buffers_].length = buf.length;
        buffers_[n_buffers_].start =
            mmap(NULL /* start anywhere */,
                    buf.length,
                    PROT_READ | PROT_WRITE /* required */,
                    MAP_SHARED /* recommended */,
                    fd_, buf.m.offset);

        if (MAP_FAILED == buffers_[n_buffers_].start)
            errno_exit("mmap");
    }

}

void Camera::uninit_device(void)
{
        unsigned int i;

        for (i = 0; i < n_buffers_; ++i)
                if (-1 == munmap(buffers_[i].start, buffers_[i].length))
                        errno_exit("munmap");

        free(buffers_);

    initialized_ = false;
}

void Camera::start_capturing()
{
        unsigned int i;
        enum v4l2_buf_type type;

        for (i = 0; i < n_buffers_; ++i) {
                struct v4l2_buffer buf;

                CLEAR(buf);
                buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
                buf.memory = V4L2_MEMORY_MMAP;
                buf.index = i;

                if (-1 == xioctl(fd_, VIDIOC_QBUF, &buf))
                        errno_exit("VIDIOC_QBUF");
        }
        type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        if (-1 == xioctl(fd_, VIDIOC_STREAMON, &type))
                errno_exit("VIDIOC_STREAMON");

    capturing_ = true;
}

void Camera::stop_capturing()
{
        enum v4l2_buf_type type;

        type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        if (-1 == xioctl(fd_, VIDIOC_STREAMOFF, &type))
                errno_exit("VIDIOC_STREAMOFF");

    capturing_ = false;
}

int Camera::read_frame()
{
    struct v4l2_buffer buf;
    unsigned int i;

    CLEAR(buf);

    buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        buf.memory = V4L2_MEMORY_MMAP;

        if (-1 == xioctl(fd_, VIDIOC_DQBUF, &buf)) {
                switch (errno) {
                        case EAGAIN:
                                return 0;

                        case EIO:
                                /* Could ignore EIO, see spec. */

                                /* fall through */

                        default:
                                errno_exit("VIDIOC_DQBUF");
                }
        }

        assert(buf.index < n_buffers_);

        //process_image(buffers_[buf.index].start, buf.bytesused);

        if (-1 == xioctl(fd_, VIDIOC_QBUF, &buf))
                errno_exit("VIDIOC_QBUF");

        return 1;
}

static void usage(FILE *fp, int argc, char **argv,
    const std::string& dev, const int run_time)
{
        fprintf(fp,
                        "Usage: %s [options]\n\n"
                        "Version 1.3\n"
                        "Options:\n"
                        "-d | --device name   Video device name [%s]\n"
                        "-h | --help          Print this message\n"
                        "-f | --format        Force format to 640x480 YUYV\n"
                        "-t | --time          Number of seconds to grab [%i]\n"
                        "",
                        argv[0], dev.c_str(), run_time);
}

static const char short_options[] = "d:hft:";

static const struct option
long_options[] = {
        { "device", required_argument, NULL, 'd' },
        { "help",   no_argument,       NULL, 'h' },
        { "format", no_argument,       NULL, 'f' },
        { "time",  required_argument,  NULL, 't' },
        { 0, 0, 0, 0 }
};


void run(const std::vector<Camera*>& cameras, const int num_seconds)
{
    struct timeval t_start, t_now;
    struct timeval timeout;
    double running_time;
    std::vector<CameraStats> stats;

    fd_set master;
    int max_fd = -1;
    FD_ZERO(&master);

    for(std::vector<Camera*>::const_iterator cam_it = cameras.begin();
        cam_it != cameras.end(); ++cam_it)
    {
        (*cam_it)->start_capturing();
        if((*cam_it)->fd_ > max_fd)
        {
            max_fd = (*cam_it)->fd_;
        }
        FD_SET((*cam_it)->fd_, &master);
        fprintf(stderr, "%d\n", (*cam_it)->fd_);

        CameraStats stat;
        stats.push_back(stat);
    }

    gettimeofday(&t_start, NULL);
    running_time = 0;
    while(running_time < (double)num_seconds)
    {
        //fprintf(stderr, "x");
        //fflush(stderr);
        fd_set dup = master;
        //FD_ZERO(&dup);

        timeout.tv_sec = 10;
        timeout.tv_usec = 0;

        int ret = select(max_fd+1, &dup, NULL, NULL, &timeout);

        if(ret < 0) {
            if (EINTR == errno)
                continue;
            errno_exit("select");
        }

        if(ret == 0) {
            fprintf(stderr, "select timeout\n");
            exit(EXIT_FAILURE);
        }

        std::vector<CameraStats>::iterator stat_it = stats.begin();
        std::vector<Camera*>::const_iterator cam_it = cameras.begin();

        for(; cam_it != cameras.end(); ++cam_it, ++stat_it)
        {
            if(FD_ISSET((*cam_it)->fd_, &dup)) {
                (*cam_it)->read_frame();
                stat_it->count_frame();

                fflush(stdout);
                fprintf(stdout, "%d", (*cam_it)->fd_);
                fflush(stdout);
            }
        }

        gettimeofday(&t_now, NULL);
        running_time = (t_now.tv_sec - t_start.tv_sec) + (t_now.tv_usec - t_start.tv_usec) / (1000000.0);
    }

    std::vector<CameraStats>::iterator stat_it = stats.begin();
    std::vector<Camera*>::const_iterator cam_it = cameras.begin();
    for(; cam_it != cameras.end(); ++cam_it, ++stat_it)
    {
        (*cam_it)->stop_capturing();
        fprintf(stdout, "%s\n", (*cam_it)->dev_name_.c_str());
        stat_it->print_report();
    }

}


int main(int argc, char** argv)
{
    std::string dev_name;
    int run_time = 5;

    std::vector<std::string> dev_names;

    for (;;) {
        int idx;
        int c;

        c = getopt_long(argc, argv,
                short_options, long_options, &idx);

        if (-1 == c)
            break;

        switch (c) {
            case 0: /* getopt_long() flag */
                break;

            case 'd':
                dev_name = optarg;
                dev_names.push_back(dev_name);
                break;

            case 'h':
                usage(stdout, argc, argv, dev_name, run_time);
                exit(EXIT_SUCCESS);

            case 't':
                errno = 0;
                run_time = strtol(optarg, NULL, 0);
                if (errno)
                    errno_exit(optarg);
                break;

            default:
                usage(stderr, argc, argv, dev_name, run_time);
                exit(EXIT_FAILURE);
        }
    }

    if(dev_names.empty())
    {
        dev_names.push_back(std::string("/dev/video0"));
    }

    std::vector<Camera*> cameras;
    Camera* c = NULL;

    for(std::vector<std::string>::iterator name_it = dev_names.begin();
        name_it != dev_names.end(); ++name_it)
    {
        c = new Camera();
        c->open_device(*name_it);
        c->init_device();

        cameras.push_back(c);
    }

    run(cameras, run_time);

    for(std::vector<Camera*>::iterator cam_it = cameras.begin();
        cam_it != cameras.end(); ++cam_it)
    {
        (*cam_it)->uninit_device();
        (*cam_it)->close_device();
        delete (*cam_it);
        (*cam_it) = NULL;
    }

    return 0;
}