Common.hpp

//
// Common structures and exceptions for SAL3D.
// Copyright 2008-2010 AQSENSE, S.L.
//
#if !defined (AQSENSE_SAL3DPP_COMMON_HPP)
#define AQSENSE_SAL3DPP_COMMON_HPP

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

#if ! defined(__GNUC__)
    /* msvc and borland */
    #include <float.h>
#endif

#include <cassert>
#include <cmath>
#include <stdexcept>
#include <vector>
#include <memory>
#include <cstdio>
#include <cstring>
#include <cstdarg>
#include <sal3d/zmap.h>
#include <sal3d/metricconfig.h>

namespace sal3d
{
    typedef sal3d_zmap_factors ZMapFactors;

    inline bool isNaN(float f)
    {
#if defined(__GNUC__)
        return std::isnan(f);
#else /* msvc and borland */
        return _isnan(f) != 0;
#endif
    }

    class Error : public std::exception
    {
        public:
        sal3d_error e;

        Error()
        {
            e.value = SAL3D_ERROR_UNKNOWN;
            e.text[0] = '\0';
        }

        Error(const struct sal3d_error &ep)
            :e(ep)
        {
        }

        Error(int val, const char *format, ...)
        {
            va_list ap;

            va_start(ap, format);
            e.value = val;
#ifndef WIN32
            std::
#endif
            vsnprintf(e.text, sizeof(e.text), format, ap);
            va_end(ap);
        }

        Error(int val)
        {
            e.value = val;
            e.text[0] = '\0';
        }

        virtual ~Error () throw() { };

        virtual const char * what () const throw()
        {
            return e.text;
        }

        int
        fill_error(struct sal3d_error *es) const
        {
            if (es)
            {
                es->value = e.value;
                unsigned int i;
                /* sizeof()-1 to make the '\0' always fit */
                for(i=0; i < (sizeof(es->text)-1) && e.text[i] != '\0'; ++i)
                    es->text[i] = e.text[i];
                es->text[i] = '\0';
            }
            return e.value;
        }

        static int set_no_error(struct sal3d_error *e)
        {
            if (e)
            {
                e->value = static_cast<enum SAL3D_ERROR>(0);
                e->text[0] = '\0';
            }
            return SAL3D_ERROR_OK;
        }

        static int set_error_msg(struct sal3d_error *e, int val,
                const char *format, ...)
        {
            va_list ap;

            va_start(ap, format);
            if (e)
            {
                e->value = val;
#ifndef WIN32
                std::
#endif
                vsnprintf(e->text, sizeof(e->text), format, ap);
            }
            va_end(ap);

            return val;
        }

        static int set_unknown_error(struct sal3d_error *e)
        {
            if (e)
            {
                const char text[] = "Unknown error";
                e->value = SAL3D_ERROR_UNKNOWN;
                std::memcpy(e->text, text, sizeof text);
            }
            return SAL3D_ERROR_UNKNOWN;
        }
    };

    template <typename T>
    class Array1D
    {
        public:
            typedef T &reference;
            typedef const T &const_reference;
            typedef T *iterator;
            typedef const T *const_iterator;

            Array1D (T *values, int length):
                _length (length),
                _values (values),
                _end (values + length)
            {
                assert ( values != 0 && "The values pointer is NULL." );
                assert ( length > 0 && "Invalid row length." );
            }

            int
            length () const
            {
                return _length;
            }

            const_reference
            operator[] (int x) const
            {
                assert ( x < length () && "Invalid value index." );
                return *(_values + x);
            }

            reference
            operator[] (int x)
            {
                return const_cast<T &> (
                        static_cast<const Array1D &>(*this)[x]);
            }

            const_iterator
            begin() const
            {
                return _values;
            }

            iterator
            begin()
            {
                return const_cast<iterator>(
                        static_cast<const Array1D &>(*this).begin());
            }

            const_iterator
            end() const
            {
                return _end;
            }

            iterator
            end()
            {
                return const_cast<iterator>(
                        static_cast<const Array1D &>(*this).end());
            }

        private:
            int _length;
            T *_values;
            T *_end;
    };

    template <typename T>
    class Array2D
    {
        public:

            typedef Array1D<T> Row;

            Array2D (T* values, int width, int height, ptrdiff_t pitch):
                _height (height),
                _pitch (pitch),
                _values (values),
                _width (width)
            {
                assert ( 0 != values && "The values pointer is NULL." );
                assert ( width > 0 && "Invalid array's width." );
                assert ( height > 0 && "Invalid array's height." );
            }

            int
            height () const
            {
                return _height;
            }

            const Row
            operator[] (int y) const
            {
                assert ( y < height () && "Invalid row index." );
                return Row (_values + y * _pitch, width ());
            }

            Row
            operator[] (int y)
            {
                return static_cast<const Array2D &>(*this)[y];
            }

            int
            width () const
            {
                return _width;
            }

        private:
            int _height;
            ptrdiff_t _pitch;
            T *_values;
            int _width;
    };

    class Point2D
    {
        public:
            Point2D ():
                _point2d ()
            {}

            explicit Point2D (const sal3d_point2d &point) :
                _point2d (point)
            {}

            Point2D (const Point2D &point):
                _point2d (point._point2d)
            {}

            Point2D (float x, float y) {
                _point2d.p[0] = x;
                _point2d.p[1] = y;
            }

            const sal3d_point2d &
            c_point_2d () const
            {
                return _point2d;
            }

            sal3d_point2d &
            c_point_2d ()
            {
                return _point2d;
            }

            const float& x() const {return _point2d.p[0];}

            float& x() {return _point2d.p[0];}

            const float& y() const {return _point2d.p[1];}

            float& y() {return _point2d.p[1];}

            float norm () const
            {
                return std::sqrt (_point2d.p[0] * _point2d.p[0] +
                        _point2d.p[1] * _point2d.p[1]);
            }

            void normalize ()
            {
                float n = norm();
                _point2d.p[0] /= n;
                _point2d.p[1] /= n;
            }

            Point2D vector (const Point2D& destination) const
            {
                Point2D vector;
                vector[0] = destination[0] - (*this)[0];
                vector[1] = destination[1] - (*this)[1];
                return vector;
            }

            Point2D &
            operator= (const Point2D& rhs)
            {
                for (int i = 0; i < 2; ++i)
                    (*this)[i] = rhs[i];

                return *this;
            }

            Point2D
            operator* (const float rhs) const
            {
                Point2D result;

                for (int i = 0; i < 2; ++i)
                    result[i] = (*this)[i] * rhs;

                return result;
            }

            Point2D &
            operator*= (const float rhs)
            {
                for (int i = 0; i < 2; ++i)
                    (*this)[i] *= rhs;

                return *this;
            }

            const float &
            operator[] (int idx) const
            {
                assert (idx < 2 && "Invalid index");
                return _point2d.p[idx];
            }

            float &
            operator[] (int idx)
            {
                return const_cast<float &> (
                        static_cast<const Point2D &>(*this)[idx]);
            }

            bool
            operator== (const Point2D& rhs)
            {
                return x () == rhs.x () && y () == rhs.y ();
            }

        private:
            sal3d_point2d _point2d;
    };

    class Point3D
    {
        public:
            Point3D ():
                _point3d ()
            {}

            explicit Point3D (const sal3d_point3d &point) :
                _point3d (point)
            {}

            Point3D (const Point3D &point):
                _point3d (point._point3d)
            {}

            Point3D (float x, float y, float z) {
                _point3d.p[0] = x;
                _point3d.p[1] = y;
                _point3d.p[2] = z;
            }

            const sal3d_point3d &
            c_point_3d () const
            {
                return _point3d;
            }

            sal3d_point3d &
            c_point_3d ()
            {
                return _point3d;
            }

            const float& x() const {return _point3d.p[0];}

            float& x() {return _point3d.p[0];}

            const float& y() const {return _point3d.p[1];}

            float& y() {return _point3d.p[1];}

            const float& z() const {return _point3d.p[2];}

            float& z() {return _point3d.p[2];}

            float norm () const
            {
                return std::sqrt (_point3d.p[0] * _point3d.p[0] +
                        _point3d.p[1] * _point3d.p[1] +
                        _point3d.p[2] * _point3d.p[2]);
            }

            void normalize ()
            {
                float n = norm();
                _point3d.p[0] /= n;
                _point3d.p[1] /= n;
                _point3d.p[2] /= n;
            }

            Point3D vector (const Point3D& destination) const
            {
                Point3D vector;
                vector[0] = destination[0] - (*this)[0];
                vector[1] = destination[1] - (*this)[1];
                vector[2] = destination[2] - (*this)[2];
                return vector;
            }

            float dot (const Point3D& rhs) const
            {
                float result (0);
                for (int i = 0; i < 3; ++i)
                    result += (*this)[i] * rhs[i];

                return result;
            }

            Point3D cross (const Point3D& rhs) const
            {
                Point3D result;

                result[0] = (*this)[1] * rhs[2] - ((*this)[2] * rhs[1]);
                result[1] = (*this)[2] * rhs[0] - ((*this)[0] * rhs[2]);
                result[2] = (*this)[0] * rhs[1] - ((*this)[1] * rhs[0]);

                return result;
            }

            Point3D &
            operator= (const Point3D& rhs)
            {
                for (int i = 0; i < 3; ++i)
                    (*this)[i] = rhs[i];

                return *this;
            }

            Point3D
            operator* (const float rhs) const
            {
                Point3D result;

                for (int i = 0; i < 3; ++i)
                    result[i] = (*this)[i] * rhs;

                return result;
            }

            Point3D &
            operator*= (const float rhs)
            {
                for (int i = 0; i < 3; ++i)
                    (*this)[i] *= rhs;

                return *this;
            }

            const float &
            operator[] (int idx) const
            {
                assert (idx < 3 && "Invalid index");
                return _point3d.p[idx];
            }

            float &
            operator[] (int idx)
            {
                return const_cast<float &> (
                        static_cast<const Point3D &>(*this)[idx]);
            }

            bool
            operator== (const Point3D& rhs)
            {
                return x () == rhs.x () && y () == rhs.y ()
                    && z () == rhs.z ();
            }

        private:
            sal3d_point3d _point3d;
    };

    class Polygon2D
    {
        public:
            explicit Polygon2D ()
            {
                Error e;
                sal3d_polygon2d_new (&_polygon2d, &e.e);
                if (e.e.value < 0)
                    throw e;
            }

            explicit Polygon2D (const std::vector<sal3d::Point2D> &points)
            {
                std::vector<sal3d_point2d> c_points (points.size ());
                for (unsigned int i = 0; i < points.size (); ++i)
                    c_points[i] = points[i].c_point_2d ();

                Error e;
                sal3d_polygon2d_new_from_points (&c_points[0],
                        static_cast<int> (c_points.size ()), &_polygon2d, &e.e);
                if (e.e.value < 0)
                    throw e;
            }

            explicit Polygon2D (const sal3d_polygon2d &polygon)
            {
                Error e;
                sal3d_polygon2d_check_validity (polygon, &e.e);
                if (e.e.value < 0)
                    throw e;

                sal3d_polygon2d_copy (polygon,
                        &_polygon2d, &e.e);
                if (e.e.value < 0)
                    throw e;
            }

            Polygon2D (const Polygon2D &polygon)
            {
                Error e;
                sal3d_polygon2d_copy (polygon._polygon2d,
                        &_polygon2d, &e.e);
                if (e.e.value < 0)
                    throw e;
            }

            ~Polygon2D ()
            {
                sal3d_polygon2d_release (_polygon2d);
            }

            sal3d_polygon2d
            c_polygon_2d () const
            {
                return _polygon2d;
            }

            int
            numPoints () const
            {
                int num_points;
                Error e;
                sal3d_polygon2d_num_points (_polygon2d, &num_points, &e.e);
                if (e.e.value < 0)
                    throw e;

                return num_points;
            }

            void
            appendPoint (const sal3d::Point2D &point)
            {
                Error e;
                sal3d_polygon2d_append_point (_polygon2d,
                        point.c_point_2d (), &e.e);
                if (e.e.value < 0)
                    throw e;
            }

            void
            insertPoint (const sal3d::Point2D &point, int pos)
            {
                Error e;
                sal3d_polygon2d_insert_point (_polygon2d,
                        point.c_point_2d (), pos, &e.e);
                if (e.e.value < 0)
                    throw e;
            }

            sal3d::Point2D
            removePoint ()
            {
                sal3d_point2d point;

                Error e;
                sal3d_polygon2d_remove_point_back (_polygon2d,
                        &point, &e.e);
                if (e.e.value < 0)
                    throw e;

                return sal3d::Point2D (point);
            }

            sal3d::Point2D
            removePoint (int pos)
            {
                sal3d_point2d point;

                Error e;
                sal3d_polygon2d_remove_point_pos (_polygon2d, pos,
                        &point, &e.e);
                if (e.e.value < 0)
                    throw e;

                return sal3d::Point2D (point);
            }

            void
            removePoints ()
            {
                Error e;
                sal3d_polygon2d_remove_points (_polygon2d, &e.e);
                if (e.e.value < 0)
                    throw e;
            }

            void
            setPoints (const std::vector<sal3d::Point2D> &points)
            {
                std::vector<sal3d_point2d> c_points (points.size ());
                for (unsigned int i = 0; i < points.size (); ++i)
                    c_points[i] = points[i].c_point_2d ();

                Error e;
                sal3d_polygon2d_set_points (_polygon2d,
                        &c_points[0], static_cast<int> (c_points.size ()), &e.e);
                if (e.e.value < 0)
                    throw e;
            }

            std::vector<sal3d::Point2D>
            getPoints () const
            {
                std::vector<sal3d_point2d> c_points (numPoints ());
                Error e;
                sal3d_polygon2d_get_points (_polygon2d,
                        &c_points[0], &e.e);
                if (e.e.value < 0)
                    throw e;

                std::vector<sal3d::Point2D> points (c_points.size ());
                for (unsigned int i = 0; i < points.size (); ++i)
                    points[i] = sal3d::Point2D (c_points[i]);

                return points;
            }

            sal3d::Point2D
            getPoint (int pos) const
            {
                sal3d_point2d point;
                Error e;
                sal3d_polygon2d_get_point (_polygon2d, pos, &point, &e.e);
                if (e.e.value < 0)
                    throw e;

                return sal3d::Point2D (point);
            }

            bool
            isPointInside (const sal3d::Point2D &point) const
            {
                int inside;
                Error e;
                sal3d_polygon2d_point_inside (point.c_point_2d (),
                        _polygon2d, &inside, &e.e);
                if (e.e.value < 0)
                    throw e;

                return inside ? true : false;
            }

            Polygon2D &
            operator= (const Polygon2D &polygon)
            {
                sal3d_polygon2d tmp = _polygon2d;
                Error e;
                sal3d_polygon2d_copy (polygon._polygon2d, &_polygon2d, &e.e);
                if (e.e.value < 0)
                    throw e;

                sal3d_polygon2d_release (tmp);

                return *this;
            }

        private:
            sal3d_polygon2d _polygon2d;
    };

    //Calibrations can come from outside sal3d, so we put the Model definition
    //here to make it independent of the lenscalib module
    namespace LensDistortion
    {
        typedef sal3d_lens_dist_model_type ModelType;

        class Model
        {
            public:
                static const int TsaiNumCoeffs = 3;
 
                static const int CubicNumCoeffs = 20;
                

                static const int NLPoly3NumCoeffs = 20;

                static const int TsaiRegNumCoeffs = 6;



                Model()
                {
                    _model.type = NoDistortion;
                    _model.error_mean = 0;
                    _model.error_stddev = 0;
                    _model.error_max = 0;
                }

                Model(const sal3d_lens_distortion &model)
                    :_model(model)
                {
                }

                Point2D
                undistort(const Point2D &p) const
                {
                    Error e;
                    Point2D up;
                    sal3d_lens_distortion_undistort(&_model,
                            &p.c_point_2d(), &up.c_point_2d(), &e.e);
                    if (e.e.value < 0)
                        throw e;
                    
                    return up;
                }

                ModelType
                getType() const
                {
                    return _model.type;
                }

                std::vector<float>
                getCoeffs() const
                {
                    int numCoeffs = 0;
                    switch (_model.type)
                    {
                        case NoDistortion:
                            numCoeffs = 0;
                            break;
                        case Tsai:
                            numCoeffs = TsaiNumCoeffs;
                            break;
                        case NLPoly3:
                            numCoeffs = NLPoly3NumCoeffs;
                            break;
                        case TsaiReg:
                            numCoeffs = TsaiRegNumCoeffs;
                            break;
                        case Cubic:
                            numCoeffs = CubicNumCoeffs;
                            break;
                        case MatrixLut:
                            numCoeffs = 0;
                            break;
                    }
                    std::vector<float> coeffs;
                    coeffs.assign(_model.coeffs, _model.coeffs + numCoeffs);

                    return coeffs;
                }

                const sal3d_lens_distortion &
                c_lens_distortion() const
                {
                    return _model;
                }

                static Model
                createTsai(float x, float y, float k)
                {
                    Model m;

                    m._model.type = Tsai;
                    m._model.coeffs[0] = x;
                    m._model.coeffs[1] = y;
                    m._model.coeffs[2] = k;
                    m._model.error_mean = 0;
                    m._model.error_stddev = 0;
                    m._model.error_max = 0;

                    return m;
                }

                float
                getErrorMean() const
                {
                    return _model.error_mean;
                }

                float
                getErrorStdDev() const
                {
                    return _model.error_stddev;
                }

                float
                getErrorMax() const
                {
                    return _model.error_max;
                }

            private:
                sal3d_lens_distortion _model;
        };

    }

    // The Metric::Config is placed here to ensure that users are able to
    // create COPs even if they don't have the Metric module.
    namespace Metric
    {
        /* In Common as it is used by Metric and Angular */
        typedef sal3d_pattern_sp_points PatternSPPoints;

        typedef sal3d_pattern_stp_points PatternSTPPoints;

        class Config
        {
            public:
                Config (float factorU, float factorV, float factorG)
                {
                    _metricConfig.p = 0;
                    Error e;
                    sal3d_metric_config_new_from_factors(
                            factorU, factorV, factorG, &_metricConfig,
                            &e.e);
                    if (e.e.value < 0)
                        throw e;
                }

                explicit Config (
                    const sal3d_metric_config &metricConfig):
                    _metricConfig (metricConfig)
                {
                    Error e;
                    sal3d_metric_config_check_validity (metricConfig, &e.e);
                    if (e.e.value < 0)
                        throw e;
                }

                Config (const Config &mc)
                    : _metricConfig(mc._metricConfig)
                {
                    Error e;
                    sal3d_metric_config_share(_metricConfig, &e.e);
                    if (e.e.value < 0)
                        throw e;
                }

                Config &
                operator= (const Config &mc)
                {
                    _metricConfig = mc._metricConfig;
                    Error e;
                    sal3d_metric_config_share(_metricConfig, &e.e);
                    if (e.e.value < 0)
                        throw e;
                    return *this;
                }

                explicit Config (const std::string &filename)
                {
                    _metricConfig.p = 0;
                    Error e;
                    sal3d_metric_config_new_from_file (
                            filename.c_str (), &_metricConfig, &e.e);
                    if (e.e.value < 0)
                        throw e;
                }

                explicit Config(const std::vector<float> &conf)
                {
                    _metricConfig.p = 0;
                    Error e;
                    sal3d_metric_config_new_from_deserialisation(&conf[0],
                            static_cast<int>(conf.size()), &_metricConfig, &e.e);
                    if (e.e.value < 0)
                        throw e;
                }

                ~Config ()
                {
                    sal3d_metric_config_release (_metricConfig);
                }

                std::vector<float> accuracyMean() const
                {
                    std::vector<float> vec(3);
                    Error e;
                    sal3d_metric_config_get_accuracy_mean(_metricConfig,
                            &vec[0], &vec[1], &vec[2], &e.e);
                    if (e.e.value < 0)
                        throw e;
                    return vec;
                }

                std::vector<float> accuracyStdDev() const
                {
                    std::vector<float> vec(3);
                    Error e;
                    sal3d_metric_config_get_accuracy_stddev(_metricConfig,
                            &vec[0], &vec[1], &vec[2], &e.e);
                    if (e.e.value < 0)
                        throw e;
                    return vec;
                }

                const sal3d_metric_config &
                c_metric_config () const
                {
                    return _metricConfig;
                }

                std::vector<float> patternMisorientation() const
                {
                    std::vector<float> vec(2);
                    Error e;
                    sal3d_metric_config_get_misorientation(_metricConfig,
                            &vec[0], &vec[1], &e.e);
                    if (e.e.value < 0)
                        throw e;
                    return vec;
                }

                void
                saveToFile (const std::string &fileName) const
                {
                    Error e;
                    sal3d_metric_config_save_to_file (_metricConfig,
                            fileName.c_str (), &e.e);
                    if (e.e.value < 0)
                        throw e;
                }

                std::vector<float>
                serialise() const
                {
                    std::vector<float> res(sal3d_metric_config_serialised_size(_metricConfig));

                    Error e;
                    sal3d_metric_config_serialise(_metricConfig,
                            &res[0], &e.e);
                    if (e.e.value < 0)
                        throw e;

                    return res;
                }

                Point3D
                toPoint3D (float u, float v, float g) const
                {
                    sal3d::Point3D point3d;
                    Error e;
                    sal3d_metric_config_point3d_from_range_map_point (u, v, g,
                            _metricConfig, &point3d.c_point_3d(), &e.e);
                    if (e.e.value < 0)
                        throw e;
                    return point3d;
                }

                LensDistortion::Model
                getLensDistortionModel() const
                {
                    sal3d_lens_distortion dm;
                    Error e;
                    sal3d_metric_config_get_lens_distortion(_metricConfig, &dm,
                            &e.e);
                    if (e.e.value < 0)
                        throw e;

                    return LensDistortion::Model(dm);
                }

            private:
                sal3d_metric_config _metricConfig;
        };
    }
}

#endif // !AQSENSE_SAL3DPP_COMMON_HPP