camera.h

#ifndef CVD_INCLUDE_CAMERA_H
#define CVD_INCLUDE_CAMERA_H

#include <TooN/TooN.h>
#include <TooN/helpers.h>
#include <cmath>

namespace CVD
{
template <class T>
inline T SAT(T x) { return (x < -1.0 / 3 ? -1e9 : x); }

namespace Camera
{

    class Linear
    {
        public:
        static const int num_parameters = 4;

        void load(std::istream& is)
        {
            is >> my_camera_parameters;
        }
        void save(std::ostream& os) const
        {
            os << my_camera_parameters;
        }

        inline TooN::Vector<2> linearproject(const TooN::Vector<2>& camframe, TooN::DefaultPrecision scale) const
        {
            return TooN::Vector<2>(scale * diagmult(camframe, my_camera_parameters.slice<0, 2>()) + my_camera_parameters.slice<2, 2>());
        }
        inline TooN::Vector<2> project(const TooN::Vector<2>& camframe) const
        {
            return TooN::Vector<2>(diagmult(camframe, my_camera_parameters.slice<0, 2>()) + my_camera_parameters.slice<2, 2>());
        }
        inline TooN::Vector<2> unproject(const TooN::Vector<2>& imframe) const
        {
            TooN::Vector<2> my_last_camframe;
            my_last_camframe[0] = (imframe[0] - my_camera_parameters[2]) / my_camera_parameters[0];
            my_last_camframe[1] = (imframe[1] - my_camera_parameters[3]) / my_camera_parameters[1];
            return my_last_camframe;
        }

        TooN::Matrix<2, 2> get_derivative_at(const TooN::Vector<2>&) const
        {
            TooN::Matrix<2, 2> result;
            result(0, 0) = my_camera_parameters[0];
            result(1, 1) = my_camera_parameters[1];
            result(0, 1) = result(1, 0) = 0;
            return result;
        }

        TooN::Vector<num_parameters> get_parameter_derivs(const TooN::Vector<2>& pos, const TooN::Vector<2>& direction) const
        {
            TooN::Vector<num_parameters> result;
            result[0] = pos[0] * direction[0];
            result[1] = pos[1] * direction[1];
            result[2] = direction[0];
            result[3] = direction[1];

            return result;
        }

        TooN::Matrix<num_parameters, 2> get_parameter_derivs(const TooN::Vector<2>& pos) const
        {
            TooN::Matrix<num_parameters, 2> result;
            result(0, 0) = pos[0];
            result(0, 1) = 0;
            result(1, 0) = 0;
            result(1, 1) = pos[1];
            result(2, 0) = 1;
            result(2, 1) = 0;
            result(3, 0) = 0;
            result(3, 1) = 1;
            return result;
        }

        void update(const TooN::Vector<num_parameters>& updates)
        {
            my_camera_parameters += updates;
        }

        inline TooN::Vector<num_parameters>& get_parameters() { return my_camera_parameters; }
        inline const TooN::Vector<num_parameters>& get_parameters() const { return my_camera_parameters; }

        private:
        TooN::Vector<num_parameters> my_camera_parameters;
    };

    class Cubic
    {
        public:
        static const int num_parameters = 5;

        inline void load(std::istream& is);
        inline void save(std::ostream& os) const;

        inline TooN::Vector<2> linearproject(const TooN::Vector<2>& camframe, TooN::DefaultPrecision scale = 1) const;
        inline TooN::Vector<2> project(const TooN::Vector<2>& camframe) const;
        inline TooN::Vector<2> unproject(const TooN::Vector<2>& imframe) const;

        inline TooN::Matrix<2, 2> get_derivative_at(const TooN::Vector<2>&) const;

        inline TooN::Vector<num_parameters> get_parameter_derivs(const TooN::Vector<2>& pos, const TooN::Vector<2>& direction) const;

        inline TooN::Matrix<Camera::Cubic::num_parameters, 2> get_parameter_derivs(const TooN::Vector<2>& pos) const;

        inline void update(const TooN::Vector<num_parameters>& updates);

        inline TooN::Vector<num_parameters>& get_parameters() { return my_camera_parameters; }
        inline const TooN::Vector<num_parameters>& get_parameters() const { return my_camera_parameters; }

        private:
        TooN::Vector<num_parameters> my_camera_parameters; // f_u, f_v, u_0, v_0
    };

    class Quintic
    {
        public:
        static const int num_parameters = 6;

        inline void load(std::istream& is);
        inline void save(std::ostream& os) const;

        inline TooN::Vector<2> linearproject(const TooN::Vector<2>& camframe, TooN::DefaultPrecision scale = 1) const;

        inline TooN::Vector<2> project_vector(const TooN::Vector<2>& x, const TooN::Vector<2>& d) const
        {
            const TooN::DefaultPrecision xsq = x * x;
            const TooN::DefaultPrecision& a = my_camera_parameters[4];
            const TooN::DefaultPrecision& b = my_camera_parameters[5];
            return (2 * (a + 2 * b * xsq) * (x * d) * TooN::diagmult(my_camera_parameters.slice<0, 2>(), x) + (1 + a * xsq + b * xsq * xsq) * TooN::diagmult(my_camera_parameters.slice<0, 2>(), d));
        }

        inline TooN::Vector<2> project_vector(const TooN::Vector<2>& d) const
        {
            return diagmult(my_camera_parameters.slice<0, 2>(), d);
        }
        inline TooN::Vector<2> unproject_vector(const TooN::Vector<2>& d) const
        {
            TooN::Vector<2> v;
            v[0] = d[0] / my_camera_parameters[0];
            v[1] = d[1] / my_camera_parameters[1];
            return v;
        }
        inline TooN::Vector<2> project(const TooN::Vector<2>& camframe) const;
        inline std::pair<TooN::Vector<2>, TooN::Matrix<2>> project(const TooN::Vector<2>& camframe, const TooN::Matrix<2>& R) const;

        inline TooN::Vector<2> unproject(const TooN::Vector<2>& imframe) const;

        inline std::pair<TooN::Vector<2>, TooN::Matrix<2>> unproject(const TooN::Vector<2>& imframe, const TooN::Matrix<2>& R) const;

        inline TooN::Matrix<2, 2> get_derivative() const;
        inline TooN::Matrix<2, 2> get_derivative(const TooN::Vector<2>& x) const;

        inline TooN::Matrix<2, 2> get_inv_derivative() const;
        inline TooN::Matrix<2, 2> get_inv_derivative(const TooN::Vector<2>& x) const;

        inline TooN::Matrix<num_parameters, 2> get_parameter_derivs() const;

        inline TooN::Vector<num_parameters> get_parameter_derivs(const TooN::Vector<2>& direction) const;

        inline void update(const TooN::Vector<num_parameters>& updates);

        inline const TooN::Vector<num_parameters>& get_parameters() const { return my_camera_parameters; }
        inline TooN::Vector<num_parameters>& get_parameters() { return my_camera_parameters; }

        private:
        TooN::Vector<num_parameters> my_camera_parameters; // f_u, f_v, u_0, v_0
        mutable TooN::Vector<2> my_last_camframe;

        inline TooN::DefaultPrecision sat(TooN::DefaultPrecision x)
        {
            TooN::DefaultPrecision a;
            a = (-3 * my_camera_parameters[4] - sqrt(9 * my_camera_parameters[4] * my_camera_parameters[4] - 20 * my_camera_parameters[5])) / (10 * my_camera_parameters[5]);

            if(x < a)
                return x;
            else
                return 1e9; //(inf)
        }
    };

    class Harris
    {

        private:
        TooN::Vector<2> radial_distort(const TooN::Vector<2>& camframe) const
        {
            TooN::DefaultPrecision r2 = camframe * camframe;
            return camframe / sqrt(1 + my_camera_parameters[4] * r2);
        }

        public:
        static const int num_parameters = 5;

        inline void load(std::istream& is)
        {
            is >> my_camera_parameters;
        }

        inline void save(std::ostream& os) const
        {
            os << my_camera_parameters;
        }

        inline TooN::Vector<2> linearproject(const TooN::Vector<2>& camframe, TooN::DefaultPrecision scale = 1) const
        {
            return TooN::Vector<2>(scale * diagmult(camframe, my_camera_parameters.slice<0, 2>()) + my_camera_parameters.slice<2, 2>());
        }

        inline TooN::Vector<2> project(const TooN::Vector<2>& camframe) const
        {
            return linearproject(radial_distort(camframe));
        }

        inline TooN::Vector<2> unproject(const TooN::Vector<2>& imframe) const
        {
            //Undo the focal length and optic axis.
            TooN::Vector<2> mod_camframe;
            mod_camframe[0] = (imframe[0] - my_camera_parameters[2]) / my_camera_parameters[0];
            mod_camframe[1] = (imframe[1] - my_camera_parameters[3]) / my_camera_parameters[1];
            TooN::DefaultPrecision rprime2 = mod_camframe * mod_camframe;
            return mod_camframe / sqrt(1 - my_camera_parameters[4] * rprime2);
        }

        inline TooN::Matrix<2, 2> get_derivative_at(const TooN::Vector<2>& pt) const
        {
            TooN::Matrix<2, 2> J;

            TooN::DefaultPrecision xc = pt[0];
            TooN::DefaultPrecision yc = pt[1];

            TooN::DefaultPrecision fu = my_camera_parameters[0];
            TooN::DefaultPrecision fv = my_camera_parameters[1];
            TooN::DefaultPrecision a = my_camera_parameters[4];

            TooN::DefaultPrecision g = 1 / sqrt(1 + a * (xc * xc + yc * yc));
            TooN::DefaultPrecision g3 = g * g * g;

            J[0][0] = fu * (g - a * xc * xc * g3);
            J[0][1] = -fu * a * xc * yc * g3;
            J[1][0] = -fv * a * xc * yc * g3;
            J[1][1] = fv * (g - a * yc * yc * g3);

            return J;
        }

        inline TooN::Matrix<num_parameters, 2> get_parameter_derivs_at(const TooN::Vector<2>& pt) const
        {
            TooN::Vector<2> mod_camframe = radial_distort(pt);

            TooN::Matrix<5, 2> result;

            TooN::DefaultPrecision xc = pt[0];
            TooN::DefaultPrecision yc = pt[1];
            TooN::DefaultPrecision r2 = xc * xc + yc * yc;

            TooN::DefaultPrecision fu = my_camera_parameters[0];
            TooN::DefaultPrecision fv = my_camera_parameters[1];
            TooN::DefaultPrecision a = my_camera_parameters[4];

            TooN::DefaultPrecision g = 1 / sqrt(1 + a * r2);
            TooN::DefaultPrecision g3 = g * g * g;

            //Derivatives of x_image:
            result[0][0] = mod_camframe[0];
            result[1][0] = 0;
            result[2][0] = 1;
            result[3][0] = 0;
            result[4][0] = -fu * xc * r2 / 2 * g3;

            //Derivatives of y_image:
            result[0][1] = 0;
            result[1][1] = mod_camframe[1];
            result[2][1] = 0;
            result[3][1] = 1;
            result[4][1] = -fv * yc * r2 / 2 * g3;

            return result;
        }

        inline TooN::Vector<num_parameters> get_parameter_derivs(const TooN::Vector<2>& position, const TooN::Vector<2>& direction) const
        {
            return get_parameter_derivs_at(position) * direction;
        }

        //inline void update(const TooN::Vector<num_parameters>& updates);

        inline TooN::Vector<num_parameters>& get_parameters()
        {
            return my_camera_parameters;
        }
        inline const TooN::Vector<num_parameters>& get_parameters() const
        {
            return my_camera_parameters;
        }

        private:
        TooN::Vector<num_parameters> my_camera_parameters; // f_u, f_v, u_0, v_0, alpha
    };

    class SquareHarris
    {

        private:
        TooN::Vector<2> radial_distort(const TooN::Vector<2>& camframe) const
        {
            TooN::DefaultPrecision r2 = camframe * camframe;
            return camframe / sqrt(1 + my_camera_parameters[4] * r2);
        }

        public:
        static const int num_parameters = 5;

        inline void load(std::istream& is)
        {
            is >> my_camera_parameters;
        }

        inline void save(std::ostream& os) const
        {
            os << my_camera_parameters;
        }

        inline TooN::Vector<2> linearproject(const TooN::Vector<2>& camframe, TooN::DefaultPrecision scale = 1) const
        {
            return TooN::Vector<2>(scale * (camframe * my_camera_parameters[0]) + my_camera_parameters.slice<2, 2>());
        }

        inline TooN::Vector<2> project(const TooN::Vector<2>& camframe) const
        {
            my_last_camframe = camframe;
            return linearproject(radial_distort(camframe));
        }

        inline TooN::Vector<2> unproject(const TooN::Vector<2>& imframe) const
        {
            //Undo the focal length and optic axis.
            TooN::Vector<2> mod_camframe;
            mod_camframe[0] = (imframe[0] - my_camera_parameters[2]) / my_camera_parameters[0];
            mod_camframe[1] = (imframe[1] - my_camera_parameters[3]) / my_camera_parameters[0];
            TooN::DefaultPrecision rprime2 = mod_camframe * mod_camframe;
            my_last_camframe = mod_camframe / sqrt(1 - my_camera_parameters[4] * rprime2);
            return my_last_camframe;
        }

        inline TooN::Matrix<2, 2> get_derivative() const
        {
            TooN::Matrix<2, 2> J;

            TooN::DefaultPrecision xc = my_last_camframe[0];
            TooN::DefaultPrecision yc = my_last_camframe[1];

            TooN::DefaultPrecision f = my_camera_parameters[0];
            TooN::DefaultPrecision a = my_camera_parameters[4];

            TooN::DefaultPrecision g = 1 / sqrt(1 + a * (xc * xc + yc * yc));
            TooN::DefaultPrecision g3 = g * g * g;

            J[0][0] = f * (g - 2 * a * xc * xc * g3);
            J[0][1] = -2 * f * a * xc * yc * g3;
            J[1][0] = -2 * f * a * xc * yc * g3;
            J[1][1] = f * (g - 2 * a * yc * yc * g3);

            return J;
        }

        inline TooN::Matrix<num_parameters, 2> get_parameter_derivs() const
        {
            TooN::Vector<2> mod_camframe = radial_distort(my_last_camframe);

            TooN::Matrix<5, 2> result;

            TooN::DefaultPrecision xc = my_last_camframe[0];
            TooN::DefaultPrecision yc = my_last_camframe[1];
            TooN::DefaultPrecision r2 = xc * xc + yc * yc;

            TooN::DefaultPrecision f = my_camera_parameters[0];
            TooN::DefaultPrecision a = my_camera_parameters[4];

            TooN::DefaultPrecision g = 1 / sqrt(1 + a * r2);
            TooN::DefaultPrecision g3 = g * g * g;

            //Derivatives of x_image:
            result[0][0] = mod_camframe[0];
            result[1][0] = 0;
            result[2][0] = 1;
            result[3][0] = 0;
            result[4][0] = -f * xc * r2 / 2 * g3;

            //Derivatives of y_image:
            result[0][1] = mod_camframe[1];
            result[1][1] = 0;
            result[2][1] = 0;
            result[3][1] = 1;
            result[4][1] = -f * yc * r2 / 2 * g3;

            return result;
        }

        inline TooN::Vector<num_parameters> get_parameter_derivs(const TooN::Vector<2>& direction) const
        {
            return get_parameter_derivs() * direction;
        }

        //inline void update(const TooN::Vector<num_parameters>& updates);

        inline TooN::Vector<num_parameters>& get_parameters()
        {
            my_camera_parameters[1] = 0;
            return my_camera_parameters;
        }

        private:
        TooN::Vector<num_parameters> my_camera_parameters; // f_u, f_v, u_0, v_0, alpha
        mutable TooN::Vector<2> my_last_camframe;
    };

    class ArcTan
    {

        private:
        TooN::Vector<2> radial_distort(const TooN::Vector<2>& camframe) const
        {
            const TooN::DefaultPrecision r2 = camframe * camframe;
            const TooN::DefaultPrecision w2 = my_camera_parameters[4] * my_camera_parameters[4];
            const TooN::DefaultPrecision factor = w2 * r2;
            TooN::DefaultPrecision term = 1.0;
            TooN::DefaultPrecision scale = term;
            term *= factor;
            scale -= term / 3.0;
            term *= factor;
            scale += term / 5.0;
            term *= factor;
            scale -= term / 7.0;
            return (scale * camframe);
        }

        public:
        static const int num_parameters = 5;

        inline void load(std::istream& is)
        {
            is >> my_camera_parameters;
        }

        inline void save(std::ostream& os) const
        {
            os << my_camera_parameters;
        }

        inline TooN::Vector<2> linearproject(const TooN::Vector<2>& camframe, TooN::DefaultPrecision scale = 1) const
        {
            return TooN::Vector<2>(scale * diagmult(camframe, my_camera_parameters.slice<0, 2>()) + my_camera_parameters.slice<2, 2>());
        }

        inline TooN::Vector<2> project(const TooN::Vector<2>& camframe) const
        {
            return linearproject(radial_distort(camframe));
        }

        inline TooN::Vector<2> unproject(const TooN::Vector<2>& imframe) const
        {
            //Undo the focal length and optic axis.
            TooN::Vector<2> mod_camframe;
            mod_camframe[0] = (imframe[0] - my_camera_parameters[2]) / my_camera_parameters[0];
            mod_camframe[1] = (imframe[1] - my_camera_parameters[3]) / my_camera_parameters[1];
            const TooN::DefaultPrecision rprime2 = mod_camframe * mod_camframe;

            // first guess
            TooN::DefaultPrecision scale = mod_camframe * mod_camframe;

            const TooN::DefaultPrecision w2 = my_camera_parameters[4] * my_camera_parameters[4];
            const TooN::DefaultPrecision k3 = -w2 / 3.0;
            const TooN::DefaultPrecision k5 = w2 * w2 / 5.0;
            const TooN::DefaultPrecision k7 = -w2 * w2 * w2 / 7.0;

            // 3 iterations of Newton-Raphson
            for(int i = 0; i < 3; i++)
            {
                TooN::DefaultPrecision temp = 1 + scale * (k3 + scale * (k5 + scale * k7));
                TooN::DefaultPrecision error = rprime2 - scale * temp * temp;
                TooN::DefaultPrecision deriv = temp * (temp + 2 * scale * (k3 + 2 * scale * (k5 + 1.5 * scale * k7)));
                scale += error / deriv;
            }
            return mod_camframe / (1 + scale * (k3 + scale * (k5 + scale * k7)));
        }

        inline TooN::Matrix<2, 2> get_derivative(const TooN::Vector<2>& pt) const
        {
            TooN::Matrix<2, 2> J = TooN::Identity;
            TooN::DefaultPrecision r2 = pt * pt;
            const TooN::DefaultPrecision w2 = my_camera_parameters[4] * my_camera_parameters[4];
            const TooN::DefaultPrecision k3 = -w2 / 3.0;
            const TooN::DefaultPrecision k5 = w2 * w2 / 5.0;
            const TooN::DefaultPrecision k7 = -w2 * w2 * w2 / 7.0;
            J *= (1 + k3 * r2 + k5 * r2 * r2 + k7 * r2 * r2 * r2);
            J += ((2 * k3 + 4 * k5 * r2 + 6 * k7 * r2 * r2) * pt.as_col()) * pt.as_row();
            J[0] *= my_camera_parameters[0];
            J[1] *= my_camera_parameters[1];
            return J;
        }

        inline TooN::Matrix<num_parameters, 2> get_parameter_derivs_at(const TooN::Vector<2>& pt) const
        {
            TooN::Vector<2> mod_camframe = radial_distort(pt);

            TooN::Matrix<5, 2> result;

            const TooN::DefaultPrecision xc = pt[0];
            const TooN::DefaultPrecision yc = pt[1];
            const TooN::DefaultPrecision r2 = xc * xc + yc * yc;
            const TooN::DefaultPrecision r4 = r2 * r2;
            const TooN::DefaultPrecision r6 = r4 * r2;

            const TooN::DefaultPrecision fu = my_camera_parameters[0];
            const TooN::DefaultPrecision fv = my_camera_parameters[1];

            const TooN::DefaultPrecision w = my_camera_parameters[4];
            const TooN::DefaultPrecision w3 = w * w * w;
            const TooN::DefaultPrecision w5 = w3 * w * w;

            const TooN::DefaultPrecision k1 = -(2.0 / 3.0) * w * r2;
            const TooN::DefaultPrecision k2 = (4.0 / 5.0) * w3 * r4;
            const TooN::DefaultPrecision k3 = -(6.0 / 7.0) * w5 * r6;

            //Derivatives of x_image:
            result[0][0] = mod_camframe[0];
            result[1][0] = 0;
            result[2][0] = 1;
            result[3][0] = 0;
            result[4][0] = fu * (k1 + k2 + k3) * xc;

            //Derivatives of y_image:
            result[0][1] = 0;
            result[1][1] = mod_camframe[1];
            result[2][1] = 0;
            result[3][1] = 1;
            result[4][1] = fv * (k1 + k2 + k3) * yc;

            return result;
        }

        inline TooN::Vector<num_parameters> get_parameter_derivs(const TooN::Vector<2>& pos, const TooN::Vector<2>& direction) const
        {
            return get_parameter_derivs_at(pos) * direction;
        }

        //inline void update(const TooN::Vector<num_parameters>& updates);

        inline TooN::Vector<num_parameters>& get_parameters()
        {
            return my_camera_parameters;
        }
        inline const TooN::Vector<num_parameters>& get_parameters() const
        {
            return my_camera_parameters;
        }

        private:
        TooN::Vector<num_parameters> my_camera_parameters; // f_u, f_v, u_0, v_0, omega
    };

    template <class C>
    class OldCameraAdapter : public C
    {
        public:
        using C::num_parameters;
        TooN::Vector<2> project(const TooN::Vector<2>& v) const
        {
            my_last_camframe = v;
            return C::project(v);
        }

        TooN::Matrix<2> get_derivative() const
        {
            return C::get_derivative_at(my_last_camframe);
        }

        TooN::Matrix<num_parameters, 2> get_parameter_derivs() const
        {
            return C::get_parameter_derivs_at(my_last_camframe);
        }

        TooN::Vector<2> unproject(const TooN::Vector<2>& v) const
        {
            my_last_camframe = C::unproject(v);
            return my_last_camframe;
        }

        private:
        mutable TooN::Vector<2> my_last_camframe;
    };

}

// Camera::Cubic inline functions //

void Camera::Cubic::load(std::istream& is)
{
    is >> my_camera_parameters;
}

void Camera::Cubic::save(std::ostream& os) const
{
    os << my_camera_parameters;
}

inline TooN::Vector<2> Camera::Cubic::linearproject(const TooN::Vector<2>& camframe, TooN::DefaultPrecision scale) const
{
    return TooN::Vector<2>(scale * diagmult(camframe, my_camera_parameters.slice<0, 2>()) + my_camera_parameters.slice<2, 2>());
}

inline TooN::Vector<2> Camera::Cubic::project(const TooN::Vector<2>& camframe) const
{
    TooN::Vector<2> mod_camframe = camframe * (1 + SAT(my_camera_parameters[4] * (camframe * camframe)));
    return TooN::Vector<2>(diagmult(mod_camframe, my_camera_parameters.slice<0, 2>()) + my_camera_parameters.slice<2, 2>());
}

inline TooN::Vector<2> Camera::Cubic::unproject(const TooN::Vector<2>& imframe) const
{
    TooN::Vector<2> mod_camframe;
    mod_camframe[0] = (imframe[0] - my_camera_parameters[2]) / my_camera_parameters[0];
    mod_camframe[1] = (imframe[1] - my_camera_parameters[3]) / my_camera_parameters[1];

    // first guess
    TooN::DefaultPrecision scale = 1 + my_camera_parameters[4] * (mod_camframe * mod_camframe);

    // 3 iterations of Newton-Rapheson
    for(int i = 0; i < 3; i++)
    {
        TooN::DefaultPrecision error = my_camera_parameters[4] * (mod_camframe * mod_camframe) - scale * scale * (scale - 1);
        TooN::DefaultPrecision deriv = (3 * scale - 2) * scale;
        scale += error / deriv;
    }
    return mod_camframe / scale;
}

TooN::Matrix<2, 2> Camera::Cubic::get_derivative_at(const TooN::Vector<2>& pos) const
{
    TooN::Matrix<2, 2> result = TooN::Identity;
    result *= 1 + my_camera_parameters[4] * (pos * pos);
    result += (2 * my_camera_parameters[4] * pos.as_col()) * pos.as_row();
    result[0] *= my_camera_parameters[0];
    result[1] *= my_camera_parameters[1];
    return result;
}

TooN::Matrix<Camera::Cubic::num_parameters, 2> Camera::Cubic::get_parameter_derivs(const TooN::Vector<2>& pos) const
{
    TooN::Vector<2> mod_camframe = pos * (1 + my_camera_parameters[4] * (pos * pos));
    TooN::Matrix<num_parameters, 2> result;
    result(0, 0) = mod_camframe[0] * my_camera_parameters[0];
    result(0, 1) = 0;
    result(1, 0) = 0;
    result(1, 1) = mod_camframe[1] * my_camera_parameters[1];
    result(2, 0) = 1 * my_camera_parameters[0];
    result(2, 1) = 0;
    result(3, 0) = 0;
    result(3, 1) = 1 * my_camera_parameters[1];
    result[4] = diagmult(pos, my_camera_parameters.slice<0, 2>()) * (pos * pos);
    return result;
}

TooN::Vector<Camera::Cubic::num_parameters> Camera::Cubic::get_parameter_derivs(const TooN::Vector<2>& pos, const TooN::Vector<2>& direction) const
{
    TooN::Vector<2> mod_camframe = pos * (1 + my_camera_parameters[4] * (pos * pos));
    TooN::Vector<num_parameters> result;
    result[0] = mod_camframe[0] * direction[0] * my_camera_parameters[0];
    result[1] = mod_camframe[1] * direction[1] * my_camera_parameters[1];
    result[2] = direction[0] * my_camera_parameters[0];
    result[3] = direction[1] * my_camera_parameters[1];
    result[4] = (diagmult(pos, my_camera_parameters.slice<0, 2>()) * direction) * (pos * pos);
    return result;
}

void Camera::Cubic::update(const TooN::Vector<num_parameters>& updates)
{
    TooN::DefaultPrecision fu = my_camera_parameters[0];
    TooN::DefaultPrecision fv = my_camera_parameters[1];
    my_camera_parameters[0] += fu * updates[0];
    my_camera_parameters[1] += fv * updates[1];
    my_camera_parameters[2] += fu * updates[2];
    my_camera_parameters[3] += fv * updates[3];
    my_camera_parameters[4] += updates[4];
    //my_camera_parameters+=updates;
}

// Camera::Quintic inline functions //

void Camera::Quintic::load(std::istream& is)
{
    is >> my_camera_parameters;
}

void Camera::Quintic::save(std::ostream& os) const
{
    os << my_camera_parameters;
}

inline TooN::Vector<2> Camera::Quintic::linearproject(const TooN::Vector<2>& camframe, TooN::DefaultPrecision scale) const
{
    return TooN::Vector<2>(scale * diagmult(camframe, my_camera_parameters.slice<0, 2>()) + my_camera_parameters.slice<2, 2>());
}

inline TooN::Vector<2> Camera::Quintic::project(const TooN::Vector<2>& camframe) const
{
    my_last_camframe = camframe;
    TooN::DefaultPrecision sc = /*sat*/ (camframe * camframe);
    TooN::Vector<2> mod_camframe = camframe * (1 + sc * (my_camera_parameters[4] + sc * my_camera_parameters[5]));
    return TooN::Vector<2>(diagmult(mod_camframe, my_camera_parameters.slice<0, 2>()) + my_camera_parameters.slice<2, 2>());
}

inline std::pair<TooN::Vector<2>, TooN::Matrix<2>> Camera::Quintic::project(const TooN::Vector<2>& camframe, const TooN::Matrix<2>& R) const
{
    std::pair<TooN::Vector<2>, TooN::Matrix<2>> result;
    result.first = this->project(camframe);
    const TooN::Matrix<2> J = this->get_derivative();
    result.second = J * R * J.T();
    return result;
}

inline TooN::Vector<2> Camera::Quintic::unproject(const TooN::Vector<2>& imframe) const
{
    TooN::Vector<2> mod_camframe;
    mod_camframe[0] = (imframe[0] - my_camera_parameters[2]) / my_camera_parameters[0];
    mod_camframe[1] = (imframe[1] - my_camera_parameters[3]) / my_camera_parameters[1];

    // first guess
    TooN::DefaultPrecision scale = mod_camframe * mod_camframe;

    // 3 iterations of Newton-Rapheson
    for(int i = 0; i < 3; i++)
    {
        TooN::DefaultPrecision temp = 1 + scale * (my_camera_parameters[4] + my_camera_parameters[5] * scale);
        TooN::DefaultPrecision error = mod_camframe * mod_camframe - scale * temp * temp;
        TooN::DefaultPrecision deriv = temp * (temp + 2 * scale * (my_camera_parameters[4] + 2 * my_camera_parameters[5] * scale));
        scale += error / deriv;
    }
    my_last_camframe = mod_camframe / (1 + scale * (my_camera_parameters[4] + my_camera_parameters[5] * scale));

    //std::cout<<"Done inverse on "<<imframe<<" - when reprojected get "<<project(my_last_camframe)<<std::endl;

    return my_last_camframe;
}

inline std::pair<TooN::Vector<2>, TooN::Matrix<2>> Camera::Quintic::unproject(const TooN::Vector<2>& imframe, const TooN::Matrix<2>& R) const
{
    std::pair<TooN::Vector<2>, TooN::Matrix<2>> result;
    result.first = this->unproject(imframe);
    TooN::Matrix<2> J = get_derivative();
    TooN::DefaultPrecision rdet = 1 / (J[0][0] * J[1][1] - J[0][1] * J[1][0]);
    TooN::Matrix<2> Jinv;
    Jinv[0][0] = rdet * J[1][1];
    Jinv[1][1] = rdet * J[0][0];
    Jinv[0][1] = -rdet * J[0][1];
    Jinv[1][0] = -rdet * J[1][0];
    result.second = Jinv * R * Jinv.T();
    return result;
}

TooN::Matrix<2, 2> Camera::Quintic::get_derivative() const
{
    TooN::Matrix<2, 2> result = TooN::Identity;
    TooN::DefaultPrecision temp1 = my_last_camframe * my_last_camframe;
    TooN::DefaultPrecision temp2 = my_camera_parameters[5] * temp1;
    result *= 1 + temp1 * (my_camera_parameters[4] + temp2);
    result += (2 * (my_camera_parameters[4] + 2 * temp2) * my_last_camframe.as_col()) * my_last_camframe.as_row();
    result[0] *= my_camera_parameters[0];
    result[1] *= my_camera_parameters[1];
    return result;
}

TooN::Matrix<2, 2> Camera::Quintic::get_inv_derivative() const
{
    TooN::Matrix<2, 2> result = TooN::Identity;
    TooN::DefaultPrecision temp1 = my_last_camframe * my_last_camframe;
    TooN::DefaultPrecision temp2 = my_camera_parameters[5] * temp1;
    TooN::DefaultPrecision temp3 = 2 * (my_camera_parameters[4] + 2 * temp2);

    result *= 1 + temp1 * (my_camera_parameters[4] + temp2);

    result[0][0] += my_last_camframe[1] * my_last_camframe[1] * temp3;
    result[0][1] = -(temp3 * my_last_camframe[0] * my_last_camframe[1]);

    result[1][1] += my_last_camframe[0] * my_last_camframe[0] * temp3;
    result[1][0] = -(temp3 * my_last_camframe[0] * my_last_camframe[1]);

    (result.T())[0] *= my_camera_parameters[1];
    (result.T())[1] *= my_camera_parameters[0];

    result /= (result[0][0] * result[1][1] - result[1][0] * result[0][1]);

    return result;
}

TooN::Matrix<2, 2> Camera::Quintic::get_inv_derivative(const TooN::Vector<2>& x) const
{

    TooN::Matrix<2, 2> result = TooN::Identity;
    TooN::DefaultPrecision temp1 = x * x;
    TooN::DefaultPrecision temp2 = my_camera_parameters[5] * temp1;
    TooN::DefaultPrecision temp3 = 2 * (my_camera_parameters[4] + 2 * temp2);

    result *= 1 + temp1 * (my_camera_parameters[4] + temp2);
    //Identity(result,1+temp1*(my_camera_parameters[4]+temp2));

    result[0][0] += x[1] * x[1] * temp3;
    result[0][1] = -(temp3 * x[0] * x[1]);

    result[1][1] += x[0] * x[0] * temp3;
    result[1][0] = -(temp3 * x[0] * x[1]);

    (result.T())[0] *= my_camera_parameters[1];
    (result.T())[1] *= my_camera_parameters[0];

    result /= (result[0][0] * result[1][1] - result[1][0] * result[0][1]);

    return result;
}

TooN::Matrix<2, 2> Camera::Quintic::get_derivative(const TooN::Vector<2>& x) const
{
    TooN::Matrix<2, 2> result = TooN::Identity;
    TooN::DefaultPrecision temp1 = x * x;
    TooN::DefaultPrecision temp2 = my_camera_parameters[5] * temp1;
    result *= 1 + temp1 * (my_camera_parameters[4] + temp2);
    //Identity(result,1+temp1*(my_camera_parameters[4]+temp2));
    result += (2 * (my_camera_parameters[4] + 2 * temp2) * x.as_col()) * x.as_row();
    result[0] *= my_camera_parameters[0];
    result[1] *= my_camera_parameters[1];
    return result;
}

TooN::Matrix<Camera::Quintic::num_parameters, 2> Camera::Quintic::get_parameter_derivs() const
{
    TooN::Matrix<num_parameters, 2> result;
    TooN::DefaultPrecision r2 = my_last_camframe * my_last_camframe;
    TooN::DefaultPrecision r4 = r2 * r2;
    TooN::Vector<2> mod_camframe = my_last_camframe * (1 + r2 * (my_camera_parameters[4] + r2 * my_camera_parameters[5]));

    result(0, 0) = mod_camframe[0];
    result(1, 0) = 0;
    result(2, 0) = 1;
    result(3, 0) = 0;
    result(4, 0) = my_camera_parameters[0] * my_last_camframe[0] * r2;
    result(5, 0) = my_camera_parameters[0] * my_last_camframe[0] * r4;

    result(0, 1) = 0;
    result(1, 1) = mod_camframe[1];
    result(2, 1) = 0;
    result(3, 1) = 1;
    result(4, 1) = my_camera_parameters[1] * my_last_camframe[1] * r2;
    result(5, 1) = my_camera_parameters[1] * my_last_camframe[1] * r4;

    //cout<<"Finish me!\n";
    return result;
}

TooN::Vector<Camera::Quintic::num_parameters> Camera::Quintic::get_parameter_derivs(const TooN::Vector<2>& direction) const
{
    //TooN::Vector<num_parameters> result;
    //cout<<"Finish me!\n";
    //FIXME improve this somewhat
    return get_parameter_derivs() * direction;
}

void Camera::Quintic::update(const TooN::Vector<num_parameters>& updates)
{
    TooN::DefaultPrecision fu = my_camera_parameters[0];
    TooN::DefaultPrecision fv = my_camera_parameters[1];

    my_camera_parameters[0] += updates[0] * fu;
    my_camera_parameters[1] += updates[1] * fv;
    my_camera_parameters[2] += updates[2] * fu;
    my_camera_parameters[3] += updates[3] * fv;
    my_camera_parameters[4] += updates[4];
    my_camera_parameters[5] += updates[5];
}
}
#endif