#include "camodocal/camera_models/CostFunctionFactory.h"

#include "camodocal/camera_models/CataCamera.h"
#include "camodocal/camera_models/EquidistantCamera.h"
#include "camodocal/camera_models/PinholeCamera.h"
#include "camodocal/camera_models/PinholeFullCamera.h"
#include "camodocal/camera_models/ScaramuzzaCamera.h"
#include "ceres/ceres.h"

namespace camodocal
{

template< typename T >
void
worldToCameraTransform( const T* const q_cam_odo,
                        const T* const t_cam_odo,
                        const T* const p_odo,
                        const T* const att_odo,
                        T* q,
                        T* t,
                        bool optimize_cam_odo_z = true )
{
    Eigen::Quaternion< T > q_z_inv( cos( att_odo[0] / T( 2 ) ), T( 0 ), T( 0 ), -sin( att_odo[0] / T( 2 ) ) );
    Eigen::Quaternion< T > q_y_inv( cos( att_odo[1] / T( 2 ) ), T( 0 ), -sin( att_odo[1] / T( 2 ) ), T( 0 ) );
    Eigen::Quaternion< T > q_x_inv( cos( att_odo[2] / T( 2 ) ), -sin( att_odo[2] / T( 2 ) ), T( 0 ), T( 0 ) );

    Eigen::Quaternion< T > q_zyx_inv = q_x_inv * q_y_inv * q_z_inv;

    T q_odo[4] = { q_zyx_inv.w( ), q_zyx_inv.x( ), q_zyx_inv.y( ), q_zyx_inv.z( ) };

    T q_odo_cam[4] = { q_cam_odo[3], -q_cam_odo[0], -q_cam_odo[1], -q_cam_odo[2] };

    T q0[4];
    ceres::QuaternionProduct( q_odo_cam, q_odo, q0 );

    T t0[3];
    T t_odo[3] = { p_odo[0], p_odo[1], p_odo[2] };

    ceres::QuaternionRotatePoint( q_odo, t_odo, t0 );

    t0[0] += t_cam_odo[0];
    t0[1] += t_cam_odo[1];

    if ( optimize_cam_odo_z )
    {
        t0[2] += t_cam_odo[2];
    }

    ceres::QuaternionRotatePoint( q_odo_cam, t0, t );
    t[0] = -t[0];
    t[1] = -t[1];
    t[2] = -t[2];

    // Convert quaternion from Ceres convention (w, x, y, z)
    // to Eigen convention (x, y, z, w)
    q[0] = q0[1];
    q[1] = q0[2];
    q[2] = q0[3];
    q[3] = q0[0];
}

template< class CameraT >
class ReprojectionError1
{
    public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    ReprojectionError1( const Eigen::Vector3d& observed_P, const Eigen::Vector2d& observed_p )
    : m_observed_P( observed_P )
    , m_observed_p( observed_p )
    , m_sqrtPrecisionMat( Eigen::Matrix2d::Identity( ) )
    {
    }

    ReprojectionError1( const Eigen::Vector3d& observed_P,
                        const Eigen::Vector2d& observed_p,
                        const Eigen::Matrix2d& sqrtPrecisionMat )
    : m_observed_P( observed_P )
    , m_observed_p( observed_p )
    , m_sqrtPrecisionMat( sqrtPrecisionMat )
    {
    }

    ReprojectionError1( const std::vector< double >& intrinsic_params,
                        const Eigen::Vector3d& observed_P,
                        const Eigen::Vector2d& observed_p )
    : m_intrinsic_params( intrinsic_params )
    , m_observed_P( observed_P )
    , m_observed_p( observed_p )
    {
    }

    // variables: camera intrinsics and camera extrinsics
    template< typename T >
    bool operator( )( const T* const intrinsic_params, const T* const q, const T* const t, T* residuals ) const
    {
        Eigen::Matrix< T, 3, 1 > P = m_observed_P.cast< T >( );

        Eigen::Matrix< T, 2, 1 > predicted_p;
        CameraT::spaceToPlane( intrinsic_params, q, t, P, predicted_p );

        Eigen::Matrix< T, 2, 1 > e = predicted_p - m_observed_p.cast< T >( );

        Eigen::Matrix< T, 2, 1 > e_weighted = m_sqrtPrecisionMat.cast< T >( ) * e;

        residuals[0] = e_weighted( 0 );
        residuals[1] = e_weighted( 1 );

        return true;
    }

    // variables: camera-odometry transforms and odometry poses
    template< typename T >
    bool operator( )( const T* const q_cam_odo,
                      const T* const t_cam_odo,
                      const T* const p_odo,
                      const T* const att_odo,
                      T* residuals ) const
    {
        T q[4], t[3];
        worldToCameraTransform( q_cam_odo, t_cam_odo, p_odo, att_odo, q, t );

        Eigen::Matrix< T, 3, 1 > P = m_observed_P.cast< T >( );

        std::vector< T > intrinsic_params( m_intrinsic_params.begin( ), m_intrinsic_params.end( ) );

        // project 3D object point to the image plane
        Eigen::Matrix< T, 2, 1 > predicted_p;
        CameraT::spaceToPlane( intrinsic_params.data( ), q, t, P, predicted_p );

        residuals[0] = predicted_p( 0 ) - T( m_observed_p( 0 ) );
        residuals[1] = predicted_p( 1 ) - T( m_observed_p( 1 ) );

        return true;
    }

    // private:
    // camera intrinsics
    std::vector< double > m_intrinsic_params;

    // observed 3D point
    Eigen::Vector3d m_observed_P;

    // observed 2D point
    Eigen::Vector2d m_observed_p;

    // square root of precision matrix
    Eigen::Matrix2d m_sqrtPrecisionMat;
};

// variables: camera extrinsics, 3D point
template< class CameraT >
class ReprojectionError2
{
    public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    ReprojectionError2( const std::vector< double >& intrinsic_params, const Eigen::Vector2d& observed_p )
    : m_intrinsic_params( intrinsic_params )
    , m_observed_p( observed_p )
    {
    }

    template< typename T >
    bool operator( )( const T* const q, const T* const t, const T* const point, T* residuals ) const
    {
        Eigen::Matrix< T, 3, 1 > P;
        P( 0 ) = T( point[0] );
        P( 1 ) = T( point[1] );
        P( 2 ) = T( point[2] );

        std::vector< T > intrinsic_params( m_intrinsic_params.begin( ), m_intrinsic_params.end( ) );

        // project 3D object point to the image plane
        Eigen::Matrix< T, 2, 1 > predicted_p;
        CameraT::spaceToPlane( intrinsic_params.data( ), q, t, P, predicted_p );

        residuals[0] = predicted_p( 0 ) - T( m_observed_p( 0 ) );
        residuals[1] = predicted_p( 1 ) - T( m_observed_p( 1 ) );

        return true;
    }

    private:
    // camera intrinsics
    std::vector< double > m_intrinsic_params;

    // observed 2D point
    Eigen::Vector2d m_observed_p;
};

template< class CameraT >
class ReprojectionError3
{
    public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    ReprojectionError3( const Eigen::Vector2d& observed_p )
    : m_observed_p( observed_p )
    , m_sqrtPrecisionMat( Eigen::Matrix2d::Identity( ) )
    , m_optimize_cam_odo_z( true )
    {
    }

    ReprojectionError3( const Eigen::Vector2d& observed_p, const Eigen::Matrix2d& sqrtPrecisionMat )
    : m_observed_p( observed_p )
    , m_sqrtPrecisionMat( sqrtPrecisionMat )
    , m_optimize_cam_odo_z( true )
    {
    }

    ReprojectionError3( const std::vector< double >& intrinsic_params, const Eigen::Vector2d& observed_p )
    : m_intrinsic_params( intrinsic_params )
    , m_observed_p( observed_p )
    , m_sqrtPrecisionMat( Eigen::Matrix2d::Identity( ) )
    , m_optimize_cam_odo_z( true )
    {
    }

    ReprojectionError3( const std::vector< double >& intrinsic_params,
                        const Eigen::Vector2d& observed_p,
                        const Eigen::Matrix2d& sqrtPrecisionMat )
    : m_intrinsic_params( intrinsic_params )
    , m_observed_p( observed_p )
    , m_sqrtPrecisionMat( sqrtPrecisionMat )
    , m_optimize_cam_odo_z( true )
    {
    }

    ReprojectionError3( const std::vector< double >& intrinsic_params,
                        const Eigen::Vector3d& odo_pos,
                        const Eigen::Vector3d& odo_att,
                        const Eigen::Vector2d& observed_p,
                        bool optimize_cam_odo_z )
    : m_intrinsic_params( intrinsic_params )
    , m_odo_pos( odo_pos )
    , m_odo_att( odo_att )
    , m_observed_p( observed_p )
    , m_optimize_cam_odo_z( optimize_cam_odo_z )
    {
    }

    ReprojectionError3( const std::vector< double >& intrinsic_params,
                        const Eigen::Quaterniond& cam_odo_q,
                        const Eigen::Vector3d& cam_odo_t,
                        const Eigen::Vector3d& odo_pos,
                        const Eigen::Vector3d& odo_att,
                        const Eigen::Vector2d& observed_p )
    : m_intrinsic_params( intrinsic_params )
    , m_cam_odo_q( cam_odo_q )
    , m_cam_odo_t( cam_odo_t )
    , m_odo_pos( odo_pos )
    , m_odo_att( odo_att )
    , m_observed_p( observed_p )
    , m_optimize_cam_odo_z( true )
    {
    }

    // variables: camera intrinsics, camera-to-odometry transform,
    //            odometry extrinsics, 3D point
    template< typename T >
    bool operator( )( const T* const intrinsic_params,
                      const T* const q_cam_odo,
                      const T* const t_cam_odo,
                      const T* const p_odo,
                      const T* const att_odo,
                      const T* const point,
                      T* residuals ) const
    {
        T q[4], t[3];
        worldToCameraTransform( q_cam_odo, t_cam_odo, p_odo, att_odo, q, t, m_optimize_cam_odo_z );

        Eigen::Matrix< T, 3, 1 > P( point[0], point[1], point[2] );

        // project 3D object point to the image plane
        Eigen::Matrix< T, 2, 1 > predicted_p;
        CameraT::spaceToPlane( intrinsic_params, q, t, P, predicted_p );

        Eigen::Matrix< T, 2, 1 > err          = predicted_p - m_observed_p.cast< T >( );
        Eigen::Matrix< T, 2, 1 > err_weighted = m_sqrtPrecisionMat.cast< T >( ) * err;

        residuals[0] = err_weighted( 0 );
        residuals[1] = err_weighted( 1 );

        return true;
    }

    // variables: camera-to-odometry transform, 3D point
    template< typename T >
    bool operator( )( const T* const q_cam_odo, const T* const t_cam_odo, const T* const point, T* residuals ) const
    {
        T p_odo[3]   = { T( m_odo_pos( 0 ) ), T( m_odo_pos( 1 ) ), T( m_odo_pos( 2 ) ) };
        T att_odo[3] = { T( m_odo_att( 0 ) ), T( m_odo_att( 1 ) ), T( m_odo_att( 2 ) ) };
        T q[4], t[3];

        worldToCameraTransform( q_cam_odo, t_cam_odo, p_odo, att_odo, q, t, m_optimize_cam_odo_z );

        std::vector< T > intrinsic_params( m_intrinsic_params.begin( ), m_intrinsic_params.end( ) );
        Eigen::Matrix< T, 3, 1 > P( point[0], point[1], point[2] );

        // project 3D object point to the image plane
        Eigen::Matrix< T, 2, 1 > predicted_p;
        CameraT::spaceToPlane( intrinsic_params.data( ), q, t, P, predicted_p );

        residuals[0] = predicted_p( 0 ) - T( m_observed_p( 0 ) );
        residuals[1] = predicted_p( 1 ) - T( m_observed_p( 1 ) );

        return true;
    }

    // variables: camera-to-odometry transform, odometry extrinsics, 3D point
    template< typename T >
    bool operator( )( const T* const q_cam_odo,
                      const T* const t_cam_odo,
                      const T* const p_odo,
                      const T* const att_odo,
                      const T* const point,
                      T* residuals ) const
    {
        T q[4], t[3];
        worldToCameraTransform( q_cam_odo, t_cam_odo, p_odo, att_odo, q, t, m_optimize_cam_odo_z );

        std::vector< T > intrinsic_params( m_intrinsic_params.begin( ), m_intrinsic_params.end( ) );
        Eigen::Matrix< T, 3, 1 > P( point[0], point[1], point[2] );

        // project 3D object point to the image plane
        Eigen::Matrix< T, 2, 1 > predicted_p;
        CameraT::spaceToPlane( intrinsic_params.data( ), q, t, P, predicted_p );

        Eigen::Matrix< T, 2, 1 > err          = predicted_p - m_observed_p.cast< T >( );
        Eigen::Matrix< T, 2, 1 > err_weighted = m_sqrtPrecisionMat.cast< T >( ) * err;

        residuals[0] = err_weighted( 0 );
        residuals[1] = err_weighted( 1 );

        return true;
    }

    // variables: 3D point
    template< typename T >
    bool operator( )( const T* const point, T* residuals ) const
    {
        T q_cam_odo[4] = { T( m_cam_odo_q.coeffs( )( 0 ) ),
                           T( m_cam_odo_q.coeffs( )( 1 ) ),
                           T( m_cam_odo_q.coeffs( )( 2 ) ),
                           T( m_cam_odo_q.coeffs( )( 3 ) ) };
        T t_cam_odo[3] = { T( m_cam_odo_t( 0 ) ), T( m_cam_odo_t( 1 ) ), T( m_cam_odo_t( 2 ) ) };
        T p_odo[3]   = { T( m_odo_pos( 0 ) ), T( m_odo_pos( 1 ) ), T( m_odo_pos( 2 ) ) };
        T att_odo[3] = { T( m_odo_att( 0 ) ), T( m_odo_att( 1 ) ), T( m_odo_att( 2 ) ) };
        T q[4], t[3];

        worldToCameraTransform( q_cam_odo, t_cam_odo, p_odo, att_odo, q, t, m_optimize_cam_odo_z );

        std::vector< T > intrinsic_params( m_intrinsic_params.begin( ), m_intrinsic_params.end( ) );
        Eigen::Matrix< T, 3, 1 > P( point[0], point[1], point[2] );

        // project 3D object point to the image plane
        Eigen::Matrix< T, 2, 1 > predicted_p;
        CameraT::spaceToPlane( intrinsic_params.data( ), q, t, P, predicted_p );

        residuals[0] = predicted_p( 0 ) - T( m_observed_p( 0 ) );
        residuals[1] = predicted_p( 1 ) - T( m_observed_p( 1 ) );

        return true;
    }

    private:
    // camera intrinsics
    std::vector< double > m_intrinsic_params;

    // observed camera-odometry transform
    Eigen::Quaterniond m_cam_odo_q;
    Eigen::Vector3d m_cam_odo_t;

    // observed odometry
    Eigen::Vector3d m_odo_pos;
    Eigen::Vector3d m_odo_att;

    // observed 2D point
    Eigen::Vector2d m_observed_p;

    Eigen::Matrix2d m_sqrtPrecisionMat;

    bool m_optimize_cam_odo_z;
};

// variables: camera intrinsics and camera extrinsics
template< class CameraT >
class StereoReprojectionError
{
    public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    StereoReprojectionError( const Eigen::Vector3d& observed_P,
                             const Eigen::Vector2d& observed_p_l,
                             const Eigen::Vector2d& observed_p_r )
    : m_observed_P( observed_P )
    , m_observed_p_l( observed_p_l )
    , m_observed_p_r( observed_p_r )
    {
    }

    template< typename T >
    bool operator( )( const T* const intrinsic_params_l,
                      const T* const intrinsic_params_r,
                      const T* const q_l,
                      const T* const t_l,
                      const T* const q_l_r,
                      const T* const t_l_r,
                      T* residuals ) const
    {
        Eigen::Matrix< T, 3, 1 > P;
        P( 0 ) = T( m_observed_P( 0 ) );
        P( 1 ) = T( m_observed_P( 1 ) );
        P( 2 ) = T( m_observed_P( 2 ) );

        Eigen::Matrix< T, 2, 1 > predicted_p_l;
        CameraT::spaceToPlane( intrinsic_params_l, q_l, t_l, P, predicted_p_l );

        Eigen::Quaternion< T > q_r = Eigen::Quaternion< T >( q_l_r ) * Eigen::Quaternion< T >( q_l );

        Eigen::Matrix< T, 3, 1 > t_r;
        t_r( 0 ) = t_l[0];
        t_r( 1 ) = t_l[1];
        t_r( 2 ) = t_l[2];

        t_r = Eigen::Quaternion< T >( q_l_r ) * t_r;
        t_r( 0 ) += t_l_r[0];
        t_r( 1 ) += t_l_r[1];
        t_r( 2 ) += t_l_r[2];

        Eigen::Matrix< T, 2, 1 > predicted_p_r;
        CameraT::spaceToPlane( intrinsic_params_r, q_r.coeffs( ).data( ), t_r.data( ), P, predicted_p_r );

        residuals[0] = predicted_p_l( 0 ) - T( m_observed_p_l( 0 ) );
        residuals[1] = predicted_p_l( 1 ) - T( m_observed_p_l( 1 ) );
        residuals[2] = predicted_p_r( 0 ) - T( m_observed_p_r( 0 ) );
        residuals[3] = predicted_p_r( 1 ) - T( m_observed_p_r( 1 ) );

        return true;
    }

    private:
    // observed 3D point
    Eigen::Vector3d m_observed_P;

    // observed 2D point
    Eigen::Vector2d m_observed_p_l;
    Eigen::Vector2d m_observed_p_r;
};

template< class CameraT >
class ComprehensionError
{
    public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    ComprehensionError( const Eigen::Vector3d& observed_P, const Eigen::Vector2d& observed_p )
    : m_observed_P( observed_P )
    , m_observed_p( observed_p )
    , m_sqrtPrecisionMat( Eigen::Matrix2d::Identity( ) )
    {
    }

    template< typename T >
    bool operator( )( const T* const intrinsic_params, const T* const q, const T* const t, T* residuals ) const
    {
        {
            Eigen::Matrix< T, 2, 1 > p = m_observed_p.cast< T >( );

            Eigen::Matrix< T, 3, 1 > predicted_img_P;
            CameraT::LiftToSphere( intrinsic_params, p, predicted_img_P );

            Eigen::Matrix< T, 2, 1 > predicted_p;
            CameraT::SphereToPlane( intrinsic_params, predicted_img_P, predicted_p );

            Eigen::Matrix< T, 2, 1 > e = predicted_p - m_observed_p.cast< T >( );

            Eigen::Matrix< T, 2, 1 > e_weighted = m_sqrtPrecisionMat.cast< T >( ) * e;

            residuals[0] = e_weighted( 0 );
            residuals[1] = e_weighted( 1 );
        }

        {
            Eigen::Matrix< T, 3, 1 > P = m_observed_P.cast< T >( );

            Eigen::Matrix< T, 2, 1 > predicted_p;
            CameraT::spaceToPlane( intrinsic_params, q, t, P, predicted_p );

            Eigen::Matrix< T, 2, 1 > e = predicted_p - m_observed_p.cast< T >( );

            Eigen::Matrix< T, 2, 1 > e_weighted = m_sqrtPrecisionMat.cast< T >( ) * e;

            residuals[2] = e_weighted( 0 );
            residuals[3] = e_weighted( 1 );
        }

        return true;
    }

    // private:
    // camera intrinsics
    // std::vector<double> m_intrinsic_params;

    // observed 3D point
    Eigen::Vector3d m_observed_P;

    // observed 2D point
    Eigen::Vector2d m_observed_p;

    // square root of precision matrix
    Eigen::Matrix2d m_sqrtPrecisionMat;
};

boost::shared_ptr< CostFunctionFactory > CostFunctionFactory::m_instance;

CostFunctionFactory::CostFunctionFactory( ) {}

boost::shared_ptr< CostFunctionFactory >
CostFunctionFactory::instance( void )
{
    if ( m_instance.get( ) == 0 )
    {
        m_instance.reset( new CostFunctionFactory );
    }

    return m_instance;
}

ceres::CostFunction*
CostFunctionFactory::generateCostFunction( const CameraConstPtr& camera,
                                           const Eigen::Vector3d& observed_P,
                                           const Eigen::Vector2d& observed_p,
                                           int flags ) const
{
    ceres::CostFunction* costFunction = 0;

    std::vector< double > intrinsic_params;
    camera->writeParameters( intrinsic_params );

    switch ( flags )
    {
        case CAMERA_INTRINSICS | CAMERA_POSE:
            switch ( camera->modelType( ) )
            {
                case Camera::KANNALA_BRANDT:
                    costFunction
                    = new ceres::AutoDiffCostFunction< ReprojectionError1< EquidistantCamera >, 2, 8, 4, 3 >(
                    new ReprojectionError1< EquidistantCamera >( observed_P, observed_p ) );
                    break;
                case Camera::PINHOLE:
                    costFunction
                    = new ceres::AutoDiffCostFunction< ReprojectionError1< PinholeCamera >, 2, 8, 4, 3 >(
                    new ReprojectionError1< PinholeCamera >( observed_P, observed_p ) );
                    break;
                case Camera::PINHOLE_FULL:
                    costFunction
                    = new ceres::AutoDiffCostFunction< ReprojectionError1< PinholeFullCamera >, 2, 12, 4, 3 >(
                    new ReprojectionError1< PinholeFullCamera >( observed_P, observed_p ) );
                    break;
                case Camera::MEI:
                    costFunction
                    = new ceres::AutoDiffCostFunction< ReprojectionError1< CataCamera >, 2, 9, 4, 3 >(
                    new ReprojectionError1< CataCamera >( observed_P, observed_p ) );
                    break;
                case Camera::SCARAMUZZA:
                    costFunction
                    = new ceres::AutoDiffCostFunction< ComprehensionError< OCAMCamera >, 4, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3 >(
                    new ComprehensionError< OCAMCamera >( observed_P, observed_p ) );
                    // new ceres::AutoDiffCostFunction<ReprojectionError1<OCAMCamera>, 2,
                    // SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3>( new
                    // ReprojectionError1<OCAMCamera>(observed_P, observed_p));
                    break;
            }
            break;
        case CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_6D_POSE:
            switch ( camera->modelType( ) )
            {
                case Camera::KANNALA_BRANDT:
                    costFunction
                    = new ceres::AutoDiffCostFunction< ReprojectionError1< EquidistantCamera >, 2, 4, 3, 3, 3 >(
                    new ReprojectionError1< EquidistantCamera >( intrinsic_params, observed_P, observed_p ) );
                    break;
                case Camera::PINHOLE:
                    costFunction
                    = new ceres::AutoDiffCostFunction< ReprojectionError1< PinholeCamera >, 2, 4, 3, 3, 3 >(
                    new ReprojectionError1< PinholeCamera >( intrinsic_params, observed_P, observed_p ) );
                    break;
                case Camera::PINHOLE_FULL:
                    costFunction
                    = new ceres::AutoDiffCostFunction< ReprojectionError1< PinholeFullCamera >, 2, 4, 3, 3, 3 >(
                    new ReprojectionError1< PinholeFullCamera >( intrinsic_params, observed_P, observed_p ) );
                    break;
                case Camera::MEI:
                    costFunction
                    = new ceres::AutoDiffCostFunction< ReprojectionError1< CataCamera >, 2, 4, 3, 3, 3 >(
                    new ReprojectionError1< CataCamera >( intrinsic_params, observed_P, observed_p ) );
                    break;
                case Camera::SCARAMUZZA:
                    costFunction
                    = new ceres::AutoDiffCostFunction< ReprojectionError1< OCAMCamera >, 2, 4, 3, 3, 3 >(
                    new ReprojectionError1< OCAMCamera >( intrinsic_params, observed_P, observed_p ) );
                    break;
            }
            break;
    }

    return costFunction;
}

ceres::CostFunction*
CostFunctionFactory::generateCostFunction( const CameraConstPtr& camera,
                                           const Eigen::Vector3d& observed_P,
                                           const Eigen::Vector2d& observed_p,
                                           const Eigen::Matrix2d& sqrtPrecisionMat,
                                           int flags ) const
{
    ceres::CostFunction* costFunction = 0;

    std::vector< double > intrinsic_params;
    camera->writeParameters( intrinsic_params );

    switch ( flags )
    {
        case CAMERA_INTRINSICS | CAMERA_POSE:
            switch ( camera->modelType( ) )
            {
                case Camera::KANNALA_BRANDT:
                    costFunction
                    = new ceres::AutoDiffCostFunction< ReprojectionError1< EquidistantCamera >, 2, 8, 4, 3 >(
                    new ReprojectionError1< EquidistantCamera >( observed_P, observed_p, sqrtPrecisionMat ) );
                    break;
                case Camera::PINHOLE:
                    costFunction
                    = new ceres::AutoDiffCostFunction< ReprojectionError1< PinholeCamera >, 2, 8, 4, 3 >(
                    new ReprojectionError1< PinholeCamera >( observed_P, observed_p, sqrtPrecisionMat ) );
                    break;
                case Camera::PINHOLE_FULL:
                    costFunction
                    = new ceres::AutoDiffCostFunction< ReprojectionError1< PinholeFullCamera >, 2, 12, 4, 3 >(
                    new ReprojectionError1< PinholeFullCamera >( observed_P, observed_p, sqrtPrecisionMat ) );
                    break;
                case Camera::MEI:
                    costFunction
                    = new ceres::AutoDiffCostFunction< ReprojectionError1< CataCamera >, 2, 9, 4, 3 >(
                    new ReprojectionError1< CataCamera >( observed_P, observed_p, sqrtPrecisionMat ) );
                    break;
                case Camera::SCARAMUZZA:
                    costFunction
                    = new ceres::AutoDiffCostFunction< ReprojectionError1< OCAMCamera >, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3 >(
                    new ReprojectionError1< OCAMCamera >( observed_P, observed_p, sqrtPrecisionMat ) );
                    break;
            }
            break;
    }

    return costFunction;
}

ceres::CostFunction*
CostFunctionFactory::generateCostFunction( const CameraConstPtr& camera,
                                           const Eigen::Vector2d& observed_p,
                                           int flags,
                                           bool optimize_cam_odo_z ) const
{
    ceres::CostFunction* costFunction = 0;

    std::vector< double > intrinsic_params;
    camera->writeParameters( intrinsic_params );

    switch ( flags )
    {
        case CAMERA_POSE | POINT_3D:
            switch ( camera->modelType( ) )
            {
                case Camera::KANNALA_BRANDT:
                    costFunction
                    = new ceres::AutoDiffCostFunction< ReprojectionError2< EquidistantCamera >, 2, 4, 3, 3 >(
                    new ReprojectionError2< EquidistantCamera >( intrinsic_params, observed_p ) );
                    break;
                case Camera::PINHOLE:
                    costFunction
                    = new ceres::AutoDiffCostFunction< ReprojectionError2< PinholeCamera >, 2, 4, 3, 3 >(
                    new ReprojectionError2< PinholeCamera >( intrinsic_params, observed_p ) );
                    break;
                case Camera::PINHOLE_FULL:
                    costFunction
                    = new ceres::AutoDiffCostFunction< ReprojectionError2< PinholeFullCamera >, 2, 4, 3, 3 >(
                    new ReprojectionError2< PinholeFullCamera >( intrinsic_params, observed_p ) );
                    break;
                case Camera::MEI:
                    costFunction
                    = new ceres::AutoDiffCostFunction< ReprojectionError2< CataCamera >, 2, 4, 3, 3 >(
                    new ReprojectionError2< CataCamera >( intrinsic_params, observed_p ) );
                    break;
                case Camera::SCARAMUZZA:
                    costFunction
                    = new ceres::AutoDiffCostFunction< ReprojectionError2< OCAMCamera >, 2, 4, 3, 3 >(
                    new ReprojectionError2< OCAMCamera >( intrinsic_params, observed_p ) );
                    break;
            }
            break;
        case CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_3D_POSE | POINT_3D:
            switch ( camera->modelType( ) )
            {
                case Camera::KANNALA_BRANDT:
                    if ( optimize_cam_odo_z )
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< EquidistantCamera >, 2, 4, 3, 2, 1, 3 >(
                        new ReprojectionError3< EquidistantCamera >( intrinsic_params, observed_p ) );
                    }
                    else
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeCamera >, 2, 4, 2, 2, 1, 3 >(
                        new ReprojectionError3< PinholeCamera >( intrinsic_params, observed_p ) );
                    }
                    break;
                case Camera::PINHOLE:
                    if ( optimize_cam_odo_z )
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeCamera >, 2, 4, 3, 2, 1, 3 >(
                        new ReprojectionError3< PinholeCamera >( intrinsic_params, observed_p ) );
                    }
                    else
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeCamera >, 2, 4, 2, 2, 1, 3 >(
                        new ReprojectionError3< PinholeCamera >( intrinsic_params, observed_p ) );
                    }
                    break;
                case Camera::PINHOLE_FULL:
                    if ( optimize_cam_odo_z )
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeFullCamera >, 2, 4, 3, 2, 1, 3 >(
                        new ReprojectionError3< PinholeFullCamera >( intrinsic_params, observed_p ) );
                    }
                    else
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeFullCamera >, 2, 4, 2, 2, 1, 3 >(
                        new ReprojectionError3< PinholeFullCamera >( intrinsic_params, observed_p ) );
                    }
                    break;
                case Camera::MEI:
                    if ( optimize_cam_odo_z )
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< CataCamera >, 2, 4, 3, 2, 1, 3 >(
                        new ReprojectionError3< CataCamera >( intrinsic_params, observed_p ) );
                    }
                    else
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< CataCamera >, 2, 4, 2, 2, 1, 3 >(
                        new ReprojectionError3< CataCamera >( intrinsic_params, observed_p ) );
                    }
                    break;
                case Camera::SCARAMUZZA:
                    if ( optimize_cam_odo_z )
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< OCAMCamera >, 2, 4, 3, 2, 1, 3 >(
                        new ReprojectionError3< OCAMCamera >( intrinsic_params, observed_p ) );
                    }
                    else
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< OCAMCamera >, 2, 4, 2, 2, 1, 3 >(
                        new ReprojectionError3< OCAMCamera >( intrinsic_params, observed_p ) );
                    }
                    break;
            }
            break;
        case CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_6D_POSE | POINT_3D:
            switch ( camera->modelType( ) )
            {
                case Camera::KANNALA_BRANDT:
                    if ( optimize_cam_odo_z )
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< EquidistantCamera >, 2, 4, 3, 3, 3, 3 >(
                        new ReprojectionError3< EquidistantCamera >( intrinsic_params, observed_p ) );
                    }
                    else
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeCamera >, 2, 4, 2, 3, 3, 3 >(
                        new ReprojectionError3< PinholeCamera >( intrinsic_params, observed_p ) );
                    }
                    break;
                case Camera::PINHOLE:
                    if ( optimize_cam_odo_z )
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeCamera >, 2, 4, 3, 3, 3, 3 >(
                        new ReprojectionError3< PinholeCamera >( intrinsic_params, observed_p ) );
                    }
                    else
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeCamera >, 2, 4, 2, 3, 3, 3 >(
                        new ReprojectionError3< PinholeCamera >( intrinsic_params, observed_p ) );
                    }
                    break;
                case Camera::PINHOLE_FULL:
                    if ( optimize_cam_odo_z )
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeFullCamera >, 2, 4, 3, 3, 3, 3 >(
                        new ReprojectionError3< PinholeFullCamera >( intrinsic_params, observed_p ) );
                    }
                    else
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeFullCamera >, 2, 4, 2, 3, 3, 3 >(
                        new ReprojectionError3< PinholeFullCamera >( intrinsic_params, observed_p ) );
                    }
                    break;
                case Camera::MEI:
                    if ( optimize_cam_odo_z )
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< CataCamera >, 2, 4, 3, 3, 3, 3 >(
                        new ReprojectionError3< CataCamera >( intrinsic_params, observed_p ) );
                    }
                    else
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< CataCamera >, 2, 4, 2, 3, 3, 3 >(
                        new ReprojectionError3< CataCamera >( intrinsic_params, observed_p ) );
                    }
                    break;
                case Camera::SCARAMUZZA:
                    if ( optimize_cam_odo_z )
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< OCAMCamera >, 2, 4, 3, 3, 3, 3 >(
                        new ReprojectionError3< OCAMCamera >( intrinsic_params, observed_p ) );
                    }
                    else
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< OCAMCamera >, 2, 4, 2, 3, 3, 3 >(
                        new ReprojectionError3< OCAMCamera >( intrinsic_params, observed_p ) );
                    }
                    break;
            }
            break;
        case CAMERA_INTRINSICS | CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_3D_POSE | POINT_3D:
            switch ( camera->modelType( ) )
            {
                case Camera::KANNALA_BRANDT:
                    if ( optimize_cam_odo_z )
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< EquidistantCamera >, 2, 8, 4, 3, 2, 1, 3 >(
                        new ReprojectionError3< EquidistantCamera >( observed_p ) );
                    }
                    else
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeCamera >, 2, 8, 4, 2, 2, 1, 3 >(
                        new ReprojectionError3< PinholeCamera >( observed_p ) );
                    }
                    break;
                case Camera::PINHOLE:
                    if ( optimize_cam_odo_z )
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeCamera >, 2, 8, 4, 3, 2, 1, 3 >(
                        new ReprojectionError3< PinholeCamera >( observed_p ) );
                    }
                    else
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeCamera >, 2, 8, 4, 2, 2, 1, 3 >(
                        new ReprojectionError3< PinholeCamera >( observed_p ) );
                    }
                    break;
                case Camera::PINHOLE_FULL:
                    if ( optimize_cam_odo_z )
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeFullCamera >, 2, 12, 4, 3, 2, 1, 3 >(
                        new ReprojectionError3< PinholeFullCamera >( observed_p ) );
                    }
                    else
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeFullCamera >, 2, 12, 4, 2, 2, 1, 3 >(
                        new ReprojectionError3< PinholeFullCamera >( observed_p ) );
                    }
                    break;
                case Camera::MEI:
                    if ( optimize_cam_odo_z )
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< CataCamera >, 2, 9, 4, 3, 2, 1, 3 >(
                        new ReprojectionError3< CataCamera >( observed_p ) );
                    }
                    else
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< CataCamera >, 2, 9, 4, 2, 2, 1, 3 >(
                        new ReprojectionError3< CataCamera >( observed_p ) );
                    }
                    break;
                case Camera::SCARAMUZZA:
                    if ( optimize_cam_odo_z )
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< OCAMCamera >, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3, 2, 1, 3 >(
                        new ReprojectionError3< OCAMCamera >( observed_p ) );
                    }
                    else
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< OCAMCamera >, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 2, 2, 1, 3 >(
                        new ReprojectionError3< OCAMCamera >( observed_p ) );
                    }
                    break;
            }
            break;
        case CAMERA_INTRINSICS | CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_6D_POSE | POINT_3D:
            switch ( camera->modelType( ) )
            {
                case Camera::KANNALA_BRANDT:
                    if ( optimize_cam_odo_z )
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< EquidistantCamera >, 2, 8, 4, 3, 3, 3, 3 >(
                        new ReprojectionError3< EquidistantCamera >( observed_p ) );
                    }
                    else
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeCamera >, 2, 8, 4, 2, 3, 3, 3 >(
                        new ReprojectionError3< PinholeCamera >( observed_p ) );
                    }
                    break;
                case Camera::PINHOLE:
                    if ( optimize_cam_odo_z )
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeCamera >, 2, 8, 4, 3, 3, 3, 3 >(
                        new ReprojectionError3< PinholeCamera >( observed_p ) );
                    }
                    else
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeCamera >, 2, 8, 4, 2, 3, 3, 3 >(
                        new ReprojectionError3< PinholeCamera >( observed_p ) );
                    }
                    break;
                case Camera::PINHOLE_FULL:
                    if ( optimize_cam_odo_z )
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeFullCamera >, 2, 12, 4, 3, 3, 3, 3 >(
                        new ReprojectionError3< PinholeFullCamera >( observed_p ) );
                    }
                    else
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeFullCamera >, 2, 12, 4, 2, 3, 3, 3 >(
                        new ReprojectionError3< PinholeFullCamera >( observed_p ) );
                    }
                    break;
                case Camera::MEI:
                    if ( optimize_cam_odo_z )
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< CataCamera >, 2, 9, 4, 3, 3, 3, 3 >(
                        new ReprojectionError3< CataCamera >( observed_p ) );
                    }
                    else
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< CataCamera >, 2, 9, 4, 2, 3, 3, 3 >(
                        new ReprojectionError3< CataCamera >( observed_p ) );
                    }
                    break;
                case Camera::SCARAMUZZA:
                    if ( optimize_cam_odo_z )
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< OCAMCamera >, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3, 3, 3, 3 >(
                        new ReprojectionError3< OCAMCamera >( observed_p ) );
                    }
                    else
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< OCAMCamera >, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 2, 3, 3, 3 >(
                        new ReprojectionError3< OCAMCamera >( observed_p ) );
                    }
                    break;
            }
            break;
    }

    return costFunction;
}

ceres::CostFunction*
CostFunctionFactory::generateCostFunction( const CameraConstPtr& camera,
                                           const Eigen::Vector2d& observed_p,
                                           const Eigen::Matrix2d& sqrtPrecisionMat,
                                           int flags,
                                           bool optimize_cam_odo_z ) const
{
    ceres::CostFunction* costFunction = 0;

    std::vector< double > intrinsic_params;
    camera->writeParameters( intrinsic_params );

    switch ( flags )
    {
        case CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_6D_POSE | POINT_3D:
            switch ( camera->modelType( ) )
            {
                case Camera::KANNALA_BRANDT:
                    if ( optimize_cam_odo_z )
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< EquidistantCamera >, 2, 4, 3, 3, 3, 3 >(
                        new ReprojectionError3< EquidistantCamera >( intrinsic_params, observed_p, sqrtPrecisionMat ) );
                    }
                    else
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeCamera >, 2, 4, 2, 3, 3, 3 >(
                        new ReprojectionError3< PinholeCamera >( intrinsic_params, observed_p, sqrtPrecisionMat ) );
                    }
                    break;
                case Camera::PINHOLE:
                    if ( optimize_cam_odo_z )
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeCamera >, 2, 4, 3, 3, 3, 3 >(
                        new ReprojectionError3< PinholeCamera >( intrinsic_params, observed_p, sqrtPrecisionMat ) );
                    }
                    else
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeCamera >, 2, 4, 2, 3, 3, 3 >(
                        new ReprojectionError3< PinholeCamera >( intrinsic_params, observed_p, sqrtPrecisionMat ) );
                    }
                    break;
                case Camera::PINHOLE_FULL:
                    if ( optimize_cam_odo_z )
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeFullCamera >, 2, 4, 3, 3, 3, 3 >(
                        new ReprojectionError3< PinholeFullCamera >( intrinsic_params, observed_p, sqrtPrecisionMat ) );
                    }
                    else
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeFullCamera >, 2, 4, 2, 3, 3, 3 >(
                        new ReprojectionError3< PinholeFullCamera >( intrinsic_params, observed_p, sqrtPrecisionMat ) );
                    }
                    break;
                case Camera::MEI:
                    if ( optimize_cam_odo_z )
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< CataCamera >, 2, 4, 3, 3, 3, 3 >(
                        new ReprojectionError3< CataCamera >( intrinsic_params, observed_p, sqrtPrecisionMat ) );
                    }
                    else
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< CataCamera >, 2, 4, 2, 3, 3, 3 >(
                        new ReprojectionError3< CataCamera >( intrinsic_params, observed_p, sqrtPrecisionMat ) );
                    }
                    break;
                case Camera::SCARAMUZZA:
                    if ( optimize_cam_odo_z )
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< OCAMCamera >, 2, 4, 3, 3, 3, 3 >(
                        new ReprojectionError3< OCAMCamera >( intrinsic_params, observed_p, sqrtPrecisionMat ) );
                    }
                    else
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< OCAMCamera >, 2, 4, 2, 3, 3, 3 >(
                        new ReprojectionError3< OCAMCamera >( intrinsic_params, observed_p, sqrtPrecisionMat ) );
                    }
                    break;
            }
            break;
        case CAMERA_INTRINSICS | CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_6D_POSE | POINT_3D:
            switch ( camera->modelType( ) )
            {
                case Camera::KANNALA_BRANDT:
                    if ( optimize_cam_odo_z )
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< EquidistantCamera >, 2, 8, 4, 3, 3, 3, 3 >(
                        new ReprojectionError3< EquidistantCamera >( observed_p, sqrtPrecisionMat ) );
                    }
                    else
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeCamera >, 2, 8, 4, 2, 3, 3, 3 >(
                        new ReprojectionError3< PinholeCamera >( observed_p, sqrtPrecisionMat ) );
                    }
                    break;
                case Camera::PINHOLE:
                    if ( optimize_cam_odo_z )
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeCamera >, 2, 8, 4, 3, 3, 3, 3 >(
                        new ReprojectionError3< PinholeCamera >( observed_p, sqrtPrecisionMat ) );
                    }
                    else
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeCamera >, 2, 8, 4, 2, 3, 3, 3 >(
                        new ReprojectionError3< PinholeCamera >( observed_p, sqrtPrecisionMat ) );
                    }
                    break;
                case Camera::PINHOLE_FULL:
                    if ( optimize_cam_odo_z )
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeFullCamera >, 2, 12, 4, 3, 3, 3, 3 >(
                        new ReprojectionError3< PinholeFullCamera >( observed_p, sqrtPrecisionMat ) );
                    }
                    else
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeFullCamera >, 2, 12, 4, 2, 3, 3, 3 >(
                        new ReprojectionError3< PinholeFullCamera >( observed_p, sqrtPrecisionMat ) );
                    }
                    break;
                case Camera::MEI:
                    if ( optimize_cam_odo_z )
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< CataCamera >, 2, 9, 4, 3, 3, 3, 3 >(
                        new ReprojectionError3< CataCamera >( observed_p, sqrtPrecisionMat ) );
                    }
                    else
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< CataCamera >, 2, 9, 4, 2, 3, 3, 3 >(
                        new ReprojectionError3< CataCamera >( observed_p, sqrtPrecisionMat ) );
                    }
                    break;
                case Camera::SCARAMUZZA:
                    if ( optimize_cam_odo_z )
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< OCAMCamera >, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3, 3, 3, 3 >(
                        new ReprojectionError3< OCAMCamera >( observed_p, sqrtPrecisionMat ) );
                    }
                    else
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< OCAMCamera >, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 2, 3, 3, 3 >(
                        new ReprojectionError3< OCAMCamera >( observed_p, sqrtPrecisionMat ) );
                    }
                    break;
            }
            break;
    }

    return costFunction;
}

ceres::CostFunction*
CostFunctionFactory::generateCostFunction( const CameraConstPtr& camera,
                                           const Eigen::Vector3d& odo_pos,
                                           const Eigen::Vector3d& odo_att,
                                           const Eigen::Vector2d& observed_p,
                                           int flags,
                                           bool optimize_cam_odo_z ) const
{
    ceres::CostFunction* costFunction = 0;

    std::vector< double > intrinsic_params;
    camera->writeParameters( intrinsic_params );

    switch ( flags )
    {
        case CAMERA_ODOMETRY_TRANSFORM | POINT_3D:
            switch ( camera->modelType( ) )
            {
                case Camera::KANNALA_BRANDT:
                    if ( optimize_cam_odo_z )
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< EquidistantCamera >, 2, 4, 3, 3 >(
                        new ReprojectionError3< EquidistantCamera >(
                        intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z ) );
                    }
                    else
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< EquidistantCamera >, 2, 4, 2, 3 >(
                        new ReprojectionError3< EquidistantCamera >(
                        intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z ) );
                    }
                    break;
                case Camera::PINHOLE:
                    if ( optimize_cam_odo_z )
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeCamera >, 2, 4, 3, 3 >(
                        new ReprojectionError3< PinholeCamera >(
                        intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z ) );
                    }
                    else
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeCamera >, 2, 4, 2, 3 >(
                        new ReprojectionError3< PinholeCamera >(
                        intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z ) );
                    }
                    break;
                case Camera::PINHOLE_FULL:
                    if ( optimize_cam_odo_z )
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeFullCamera >, 2, 4, 3, 3 >(
                        new ReprojectionError3< PinholeFullCamera >(
                        intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z ) );
                    }
                    else
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeFullCamera >, 2, 4, 2, 3 >(
                        new ReprojectionError3< PinholeFullCamera >(
                        intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z ) );
                    }
                    break;
                case Camera::MEI:
                    if ( optimize_cam_odo_z )
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< CataCamera >, 2, 4, 3, 3 >(
                        new ReprojectionError3< CataCamera >(
                        intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z ) );
                    }
                    else
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< CataCamera >, 2, 4, 2, 3 >(
                        new ReprojectionError3< CataCamera >(
                        intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z ) );
                    }
                    break;
                case Camera::SCARAMUZZA:
                    if ( optimize_cam_odo_z )
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< OCAMCamera >, 2, 4, 3, 3 >(
                        new ReprojectionError3< OCAMCamera >(
                        intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z ) );
                    }
                    else
                    {
                        costFunction
                        = new ceres::AutoDiffCostFunction< ReprojectionError3< OCAMCamera >, 2, 4, 2, 3 >(
                        new ReprojectionError3< OCAMCamera >(
                        intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z ) );
                    }
                    break;
            }
            break;
    }

    return costFunction;
}

ceres::CostFunction*
CostFunctionFactory::generateCostFunction( const CameraConstPtr& camera,
                                           const Eigen::Quaterniond& cam_odo_q,
                                           const Eigen::Vector3d& cam_odo_t,
                                           const Eigen::Vector3d& odo_pos,
                                           const Eigen::Vector3d& odo_att,
                                           const Eigen::Vector2d& observed_p,
                                           int flags ) const
{
    ceres::CostFunction* costFunction = 0;

    std::vector< double > intrinsic_params;
    camera->writeParameters( intrinsic_params );

    switch ( flags )
    {
        case POINT_3D:
            switch ( camera->modelType( ) )
            {
                case Camera::KANNALA_BRANDT:
                    costFunction
                    = new ceres::AutoDiffCostFunction< ReprojectionError3< EquidistantCamera >, 2, 3 >(
                    new ReprojectionError3< EquidistantCamera >(
                    intrinsic_params, cam_odo_q, cam_odo_t, odo_pos, odo_att, observed_p ) );
                    break;
                case Camera::PINHOLE:
                    costFunction
                    = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeCamera >, 2, 3 >(
                    new ReprojectionError3< PinholeCamera >(
                    intrinsic_params, cam_odo_q, cam_odo_t, odo_pos, odo_att, observed_p ) );
                    break;
                case Camera::PINHOLE_FULL:
                    costFunction
                    = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeFullCamera >, 2, 3 >(
                    new ReprojectionError3< PinholeFullCamera >(
                    intrinsic_params, cam_odo_q, cam_odo_t, odo_pos, odo_att, observed_p ) );
                    break;
                case Camera::MEI:
                    costFunction
                    = new ceres::AutoDiffCostFunction< ReprojectionError3< CataCamera >, 2, 3 >(
                    new ReprojectionError3< CataCamera >(
                    intrinsic_params, cam_odo_q, cam_odo_t, odo_pos, odo_att, observed_p ) );
                    break;
                case Camera::SCARAMUZZA:
                    costFunction
                    = new ceres::AutoDiffCostFunction< ReprojectionError3< OCAMCamera >, 2, 3 >(
                    new ReprojectionError3< OCAMCamera >(
                    intrinsic_params, cam_odo_q, cam_odo_t, odo_pos, odo_att, observed_p ) );
                    break;
            }
            break;
    }

    return costFunction;
}

ceres::CostFunction*
CostFunctionFactory::generateCostFunction( const CameraConstPtr& cameraL,
                                           const CameraConstPtr& cameraR,
                                           const Eigen::Vector3d& observed_P,
                                           const Eigen::Vector2d& observed_p_l,
                                           const Eigen::Vector2d& observed_p_r ) const
{
    ceres::CostFunction* costFunction = 0;

    if ( cameraL->modelType( ) != cameraR->modelType( ) )
    {
        return costFunction;
    }

    switch ( cameraL->modelType( ) )
    {
        case Camera::KANNALA_BRANDT:
            costFunction
            = new ceres::AutoDiffCostFunction< StereoReprojectionError< EquidistantCamera >, 4, 8, 8, 4, 3, 4, 3 >(
            new StereoReprojectionError< EquidistantCamera >( observed_P, observed_p_l, observed_p_r ) );
            break;
        case Camera::PINHOLE:
            costFunction
            = new ceres::AutoDiffCostFunction< StereoReprojectionError< PinholeCamera >, 4, 8, 8, 4, 3, 4, 3 >(
            new StereoReprojectionError< PinholeCamera >( observed_P, observed_p_l, observed_p_r ) );
            break;
        case Camera::PINHOLE_FULL:
            costFunction
            = new ceres::AutoDiffCostFunction< StereoReprojectionError< PinholeFullCamera >, 4, 12, 12, 4, 3, 4, 3 >(
            new StereoReprojectionError< PinholeFullCamera >( observed_P, observed_p_l, observed_p_r ) );
            break;
        case Camera::MEI:
            costFunction
            = new ceres::AutoDiffCostFunction< StereoReprojectionError< CataCamera >, 4, 9, 9, 4, 3, 4, 3 >(
            new StereoReprojectionError< CataCamera >( observed_P, observed_p_l, observed_p_r ) );
            break;
        case Camera::SCARAMUZZA:
            costFunction
            = new ceres::AutoDiffCostFunction< StereoReprojectionError< OCAMCamera >, 4, SCARAMUZZA_CAMERA_NUM_PARAMS, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3, 4, 3 >(
            new StereoReprojectionError< OCAMCamera >( observed_P, observed_p_l, observed_p_r ) );
            break;
    }

    return costFunction;
}
}
