#ifndef LIB3DV_IPM_H
#define LIB3DV_IPM_H

/* ipm.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/point.h>

#include <lib3dv/3dv_export.h>

#include <boost/shared_ptr.hpp>

#include <vector>

namespace lib3dv
{
    class image;

    /**
     * @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 inverse_perspective_mapping
    {
        public :
            
            /**
            * @brief Setup the conversion from a depth map to a point cloud in sensor coordinates [x -> right, y -> down, z -> forward].
            *
            * @param [in] intrinsics Right camera intrinsic calibration parameters.
            * @param [in] baseline Euclidean distance between the optical centers.
            **/
            inverse_perspective_mapping(const calibration::camera_intrinsics& intrinsics, float baseline);

            /**
            * @brief Setup the conversion from a depth map to a point cloud in world coordinates [x -> forward, y -> left, z -> up].
            *
            * @param [in] intrinsics Right camera intrinsic calibration parameters.
            * @param [in] position Right camera position.
            * @param [in] orientation Right camera orientation. 
            * @param [in] baseline Euclidean distance between the optical centers.
            **/            
            inverse_perspective_mapping(const calibration::camera_intrinsics& intrinsics, const calibration::position& position, const calibration::orientation& orientation, float baseline);
            
            ~inverse_perspective_mapping();
            
            /**
            * @brief Convert a depth map to a point cloud.
            *
            * @param [in] dsi Depth map. Must be a lib3dv::image::type::DSI.
            * @param [out] world_points Point cloud.
            * @return bool True if the operation was successful, false otherwise.
            **/
            bool operator()(boost::shared_ptr<const image> dsi, std::vector<point3>& world_points);
            
        private :
            
            calibration::camera_intrinsics m_intrinsics;
            float m_baseline;
            float* m_transformation_matrix;
    };
}

#endif