-
Notifications
You must be signed in to change notification settings - Fork 0
/
attitude.cpp
37 lines (30 loc) · 784 Bytes
/
attitude.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
#include "attitude.h"
#include "Eigen/Geometry"
Eigen::Quaterniond Attitude::toQuaternion() const
{
return Eigen::AngleAxisd(z_rot, Eigen::Vector3d::UnitZ()) *
Eigen::AngleAxisd(y_rot, Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(x_rot, Eigen::Vector3d::UnitX());
}
Sophus::SO3d Attitude::toSO3() const
{
return Sophus::SO3d(toQuaternion());
}
std::istream& operator>>(std::istream& is, Attitude& att)
{
is >> att.x_rot
>> att.y_rot
>> att.z_rot;
if (!is)
{
throw std::runtime_error("Could not read Attitude data");
}
return is;
}
std::ostream& operator<<(std::ostream& os, const Attitude& att)
{
os << "x_rot: " << att.x_rot << "\n"
<< "y_rot: " << att.y_rot << "\n"
<< "z_rot: " << att.z_rot << "\n";
return os;
}