/*@@@**************************************************************************
* \file  stereoCamera
* \author Hernan Badino
* \date  Thu Jun 11 17:29:31 EDT 2009
* \notes 
*******************************************************************************
******************************************************************************/

/* INCLUDES */
#include "doubleParam.h"
#include "stereoCamera.h"

using namespace VIC;


CStereoCamera::CStereoCamera ( ) 
        : CCamera ( ),
          m_baseline_d (         0. ),
          m_fuB_d (              0. )
{
}

CStereoCamera::CStereoCamera ( const CParamIOHandling & fr_paraReader )
        : CCamera ( ),
          m_baseline_d (         0. ),
          m_fuB_d (              0. )
{
    load ( fr_paraReader );
}

CStereoCamera::~CStereoCamera()
{

}

inline bool
CStereoCamera::load ( const CParamIOHandling & fr_paraReader )
{    
    std::string value_str;
    CDoubleParameter paraDouble;
    
    if ( fr_paraReader.get ( "Baseline", value_str ) && 
         paraDouble.setValueFromString ( value_str ))
        m_baseline_d = paraDouble.getValue();

    bool ret = CCamera::load( fr_paraReader );

    m_fuB_d = m_fu_d * m_baseline_d;

    return ret;
}

inline void
CStereoCamera::setBaseline( double f_baseline_d )
{
    m_baseline_d = f_baseline_d;
    m_fuB_d = m_fu_d * m_baseline_d;
}

inline double
CStereoCamera::getBaseline() const
{
    return m_baseline_d;
}

inline bool
CStereoCamera::world2Image ( double  f_x_d, 
                             double  f_y_d,
                             double  f_z_d,
                             double &fr_u_d,
                             double &fr_v_d ) const
{
    return CCamera::world2Image ( f_x_d, f_y_d, f_z_d, fr_u_d, fr_v_d );
}


inline bool
CStereoCamera::world2Image ( C3DVector  f_point,
                             C3DVector &fr_projection ) const
{
    return world2Image ( f_point.x(), 
                         f_point.y(), 
                         f_point.z(), 
                         fr_projection.at(0),
                         fr_projection.at(1),
                         fr_projection.at(2) );
}


inline bool
CStereoCamera::world2Image ( C3DVector  f_point,
                             double    &fr_u_d,
                             double    &fr_v_d,
                             double    &fr_d_d ) const
{
    return world2Image ( f_point.x(), 
                         f_point.y(), 
                         f_point.z(), 
                         fr_u_d,
                         fr_v_d,
                         fr_d_d );
}

inline bool
CStereoCamera::world2Image ( double  f_x_d, 
                             double  f_y_d,
                             double  f_z_d,
                             double &fr_u_d,
                             double &fr_v_d,
                             double &fr_d_d ) const
{
    bool res_b;
    
    res_b = CCamera::world2Image ( f_x_d, 
                                   f_y_d, 
                                   f_z_d, 
                                   fr_u_d, 
                                   fr_v_d );

    fr_d_d = m_fuB_d / f_z_d;

    return res_b && f_z_d > 0;
}


/// Image position to world position.
inline bool
CStereoCamera::image2World ( double  f_u_d, 
                             double  f_v_d,
                             double  f_d_d,
                             double &fr_x_d,
                             double &fr_y_d,
                             double &fr_z_d ) const
{
    if ( f_d_d <= 0 ) return false;
    
    fr_z_d = m_fuB_d / f_d_d;

    return CCamera::imageAndDistance2World ( f_u_d,
                                             f_v_d,
                                             fr_z_d,
                                             fr_x_d,
                                             fr_y_d );
}

/// Image position to world position.
inline bool
CStereoCamera::image2World ( double     f_u_d, 
                             double     f_v_d,
                             double     f_d_d,
                             C3DVector &f_point ) const
{
    return image2World ( f_u_d,
                         f_v_d,
                         f_d_d,
                         f_point.at(0),
                         f_point.at(1),
                         f_point.at(2) );
}

inline double
CStereoCamera::getDisparityFromDistance ( double f_dist_d ) const
{
    return m_fuB_d / f_dist_d;
}

inline double
CStereoCamera::getDistanceFromDisparity ( double f_disp_d ) const
{
    return m_fuB_d / f_disp_d;
}

/// Image position to world position.
void
CStereoCamera::scale ( double f_scale_d )
{
    CCamera::scale ( f_scale_d );
    m_fuB_d = m_fu_d * m_baseline_d;
}

/// Focal length U.
void
CStereoCamera::setFu ( double f_fu_d )
{
    CCamera::setFu( f_fu_d  );
    m_fuB_d = m_fu_d * m_baseline_d;
}

/* ////////////  Version History ///////////////
 *  $Log: stereoCamera.cpp,v $
 *  Revision 1.2  2009/11/18 15:50:15  badino
 *  badino: global changes.
 *
 *//////////////////////////////////////////////
