source: pacpussensors/trunk/Vislab/lib3dv-1.2.0/lib3dv/pose_utils.cc

Last change on this file was 136, checked in by ldecherf, 8 years ago

Doc

File size: 1.5 KB
Line 
1/* pose_utils.cc
2 *
3 * Copyright (C) 2014 VisLab
4 *
5 * This file is part of lib3dv; you can redistribute it and/or modify
6 * it under the terms of the GNU Lesser General Public License as published by
7 * the Free Software Foundation; either version 3 of the License, or (at
8 * your option) any later version.
9 *
10 * This program is distributed in the hope that it will be useful, but
11 * WITHOUT ANY WARRANTY; without even the implied warranty of
12 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13 * General Public License for more details.
14 *
15 * You should have received a copy of the GNU Lesser General Public License
16 * along with this program; if not, see <http://www.gnu.org/licenses/>.
17 */
18
19#include <lib3dv/pose_utils.h>
20
21#include <Eigen/Dense>
22
23#include <cmath>
24
25
26lib3dv::pose_utils::pose_utils(const calibration::position& position, const calibration::orientation& orientation):
27 m_position(position),
28 m_orientation(m_orientation)
29{}
30
31
32void lib3dv::pose_utils::extract_orientation(const lib3dv::pose& pose, float &yaw, float & pitch, float & roll)
33{
34 Eigen::Matrix4d pmatrix = Eigen::Matrix4d::Identity();
35
36 for (unsigned int i = 0; i < pmatrix.rows(); i++)
37 for (unsigned int j = 0; j < pmatrix.cols(); j++)
38 pmatrix(i,j) = pose.m_data[j+(i*pmatrix.cols())];
39
40 Eigen::Matrix4d inv_p = pmatrix.inverse();
41
42 roll = atan2(-inv_p(0,2), -inv_p(1,2));
43 yaw = atan2(inv_p(2,1), inv_p(2,0));
44 pitch = atan2(-inv_p(2,2), sqrt(inv_p(0,2)*inv_p(0,2) + inv_p(1,2)*inv_p(1,2)));
45
46}
47
Note: See TracBrowser for help on using the repository browser.