#ifndef LIB3DV_POSE_UTILS_H
#define LIB3DV_POSE_UTILS_H

/* pose_utils.h
 * 
 * Copyright (C) 2013 VisLab
 * 
 * This file is part of lib3dv; you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or (at
 * your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 * 
 * You should have received a copy of the GNU Lesser General Public License
 * along with this program; if not, see <http://www.gnu.org/licenses/>.
 */  

#include <lib3dv/calibration.h>
#include <lib3dv/pose.h>

#include <lib3dv/3dv_export.h>

#include <boost/shared_ptr.hpp>

namespace lib3dv
{
    class pose;

    /**
     * @brief Inverse perspective mapping
     * 
     * This class turns raw depth measurements into 3D points. Both sensor and world coordinate systems are supported.
     * 
     * @note Intrinsic camera params must match the DSI resolution, so if the calibration information refers to the full-resolution image and a 2x downsampling has been applied to the depth map,
     * all the fields of calibration::camera_intrinsics will have to be divided by 2 before invoking the constructor.
     **/
    class LIB3DV_EXPORT pose_utils
    {
        public :
            
            /**
            * @brief Setup the conversion from a a pose matrix to orientation data in world coordinates [yaw, pitch, roll].
            *
            * @param [in] position Right camera position.
            * @param [in] orientation Right camera orientation.
            **/
            pose_utils(const calibration::position& position, const calibration::orientation& orientation);
            
            /**
            * @brief Convert a pose matrix to orientation data in world coordinates [yaw, pitch, roll].
            *
            * @param [in] pose Pose data. Must be a lib3dv::pose.
            * @param [out] double yaw.
            * @param [out] double pitch.
            * @param [out] double roll.
            **/
            void extract_orientation(const lib3dv::pose& pose, float &yaw, float & pitch, float & roll);
            
        private :
            
            calibration::position m_position;
            calibration::orientation m_orientation;
    };
}

#endif