LensCalibration.hpp

//
// Distortion correction for SAL3D.
// Copyright 2010 AQSENSE, S.L.
//
#if !defined (AQSENSE_SAL3DPP_LENS_DISTORTION_HPP)
#define AQSENSE_SAL3DPP_LENS_DISTORTION_HPP

#if defined (_MSC_VER) && (_MSC_VER >= 1020)
#pragma once
#endif

#include <sal3d/lensdistortion.h>
#include <sal3d/lenscalib.h>
#include <sal3dpp/RangeMap.hpp>
#include <sal3dpp/Common.hpp>
#include <vector>
#include <algorithm>

namespace sal3d
{
    namespace LensDistortion
    {
        inline Model
        calibrateTsai(const std::vector<sal3d::Frame> &frames,
                int patternRows, int patternCols,
                int &nvalid, std::vector<Point2D> &points)
        {
            std::vector<sal3d_frame> c_frames;
            std::vector<sal3d_point2d> c_points2d;

            for(unsigned int i=0; i < frames.size(); ++i)
                c_frames.push_back(frames[i].c_frame());

            c_points2d.resize (patternRows * patternCols);

            sal3d_lens_distortion c_model;
            Error e;
            sal3d_lens_distortion_calibrate_tsai(&c_frames[0],
                    c_frames.size(), patternRows,
                    patternCols, &nvalid, &c_model, &c_points2d[0],
                    &e.e);
            if (e.e.value < 0)
                throw e;

            points.resize(c_points2d.size());
            for(unsigned int i=0; i < points.size(); ++i)
                points[i] = Point2D(c_points2d[i]);

            return Model(c_model);
        }

        inline Model
        calibrateTsai(const std::vector<sal3d::Frame> &frames,
                int patternRows, int patternCols)
        {
            int nvalid = 0;
            std::vector<Point2D> points (patternRows *
                    patternCols);

            return calibrateTsai (frames, patternRows,
                    patternCols, nvalid, points);
        }

        inline Model
        calibrateTsai(const sal3d::Frame &frame, int patternRows,
                int patternCols)
        {
            std::vector<sal3d::Frame> frames;
            frames.push_back (frame);

            return calibrateTsai (frames, patternRows,
                    patternCols);
        }

        inline Model
        calibrateDotsTsai(const std::vector<sal3d::Frame> &frames,
                int &patternRows, int &patternCols,
                int &nvalid, std::vector<Point2D> &points)
        {
            std::vector<sal3d_frame> c_frames;
            std::vector<sal3d_point2d> c_points2d(1);

            for(unsigned int i=0; i < frames.size(); ++i)
                c_frames.push_back(frames[i].c_frame());

            sal3d_lens_distortion c_model;
            Error e;
            sal3d_lens_distortion_calibrate_dots_tsai(&c_frames[0],
                    c_frames.size(), &patternRows,
                    &patternCols, &nvalid, &c_model, &c_points2d[0],
                    &e.e);
            if (e.e.value < 0)
                throw e;

            /* Second time calibration, just to get all points found */
            const int nmax = patternRows * patternCols;
            c_points2d.resize(nmax);
            sal3d_lens_distortion_calibrate_dots_tsai(&c_frames[0],
                    c_frames.size(), &patternRows,
                    &patternCols, &nvalid, &c_model, &c_points2d[0],
                    &e.e);
            if (e.e.value < 0)
                throw e;

            points.resize(nmax);
            for(int i=0; i < nmax; ++i)
                points[i] = Point2D(c_points2d[i]);

            return Model(c_model);
        }

        inline Model
        calibrateDotsTsai(const std::vector<sal3d::Frame> &frames)
        {
            int nvalid = 0;
            int patternRows = 0;
            int patternCols = 0;
            std::vector<Point2D> points;

            return calibrateDotsTsai (frames, patternRows,
                    patternCols, nvalid, points);
        }

        inline Model
        calibrateDotsTsai(const sal3d::Frame &frame)
        {
            std::vector<sal3d::Frame> frames;
            frames.push_back (frame);

            return calibrateDotsTsai (frames);
        }

        inline Model
        calibrateDotsCubic(const sal3d::Frame &frame)
        {
            sal3d_lens_distortion c_model;
            Error e;
            sal3d_lens_distortion_calibrate_dots_cubic(
                    frame.c_frame(), &c_model, 0, 0, 0, &e.e);
            if (e.e.value < 0)
                throw e;

            return Model(c_model);
        }


        inline Model
        calibrateDotsCubic(const sal3d::Frame &frame,
                int &patternRows, int &patternCols,
                std::vector<Point2D> &points)
        {
            /* We allocate as forecasted by the user calling */
            int nmax = patternRows * patternCols;
            std::vector<sal3d_point2d> c_points2d(nmax);

            sal3d_lens_distortion c_model;
            Error e;
            sal3d_lens_distortion_calibrate_dots_cubic(
                    frame.c_frame(),
                    &c_model, &patternRows, &patternCols, &c_points2d[0],
                    &e.e);
            if (e.e.value < 0)
                throw e;

            /* Second time calibration, just to get all points found */
            nmax = patternRows * patternCols;
            c_points2d.resize(nmax);
            sal3d_lens_distortion_calibrate_dots_cubic(
                    frame.c_frame(), 
                    &c_model, &patternRows, &patternCols, &c_points2d[0],
                    &e.e);
            if (e.e.value < 0)
                throw e;

            points.resize(nmax);
            for(int i=0; i < nmax; ++i)
                points[i] = Point2D(c_points2d[i]);

            return Model(c_model);
        }

        inline Model
        calibrateCubic(const sal3d::Frame &frame,
                int patternRows, int patternCols,
                std::vector<Point2D> &points)
        {
            /* We allocate as forecasted by the user calling */
            int nmax = patternRows * patternCols;
            std::vector<sal3d_point2d> c_points2d(nmax);

            sal3d_lens_distortion c_model;

            Error e;
            sal3d_lens_distortion_calibrate_cubic(
                    frame.c_frame(), patternRows, patternCols, &c_model,
                    &c_points2d[0], &e.e);
            if (e.e.value < 0)
                throw e;

            /* If the user forecasted more than needed, return the points as
             * found. If the user forecasted less, return as much as the
             * forecast then, but with a vector size of the new rows*cols,
             * so he can traverse them. */
            nmax = std::min(nmax, patternRows * patternCols);
            points.resize(nmax);
            for(int i=0; i < nmax; ++i)
                points[i] = Point2D(c_points2d[i]);

            return Model(c_model);
        }

    }
}

#endif //AQSENSE_SAL3DPP_LENS_DISTORTION_HPP