/* pose_utils.cc
 * 
 * Copyright (C) 2014 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/pose_utils.h>

#include <Eigen/Dense>

#include <cmath>


lib3dv::pose_utils::pose_utils(const calibration::position& position, const calibration::orientation& orientation):
    m_position(position),
    m_orientation(m_orientation)
{}
            

void lib3dv::pose_utils::extract_orientation(const lib3dv::pose& pose, float &yaw, float & pitch, float & roll)
{
	  Eigen::Matrix4d pmatrix = Eigen::Matrix4d::Identity();

	  for (unsigned int i = 0; i < pmatrix.rows(); i++)
		  for (unsigned int j = 0; j < pmatrix.cols(); j++)
			  pmatrix(i,j) = pose.m_data[j+(i*pmatrix.cols())];

	  Eigen::Matrix4d inv_p = pmatrix.inverse();

	  roll = atan2(-inv_p(0,2), -inv_p(1,2));
	  yaw = atan2(inv_p(2,1), inv_p(2,0));
	  pitch = atan2(-inv_p(2,2), sqrt(inv_p(0,2)*inv_p(0,2) + inv_p(1,2)*inv_p(1,2)));

}