Skip to content
Snippets Groups Projects

Add ProcessorMotion::processIncomingCapture & Miscellaneous

Merged Jeremie Deray requested to merge miscellaneous into master
All threads resolved!
1 file
+ 4
14
Compare changes
  • Side-by-side
  • Inline
+ 4
14
@@ -386,21 +386,11 @@ inline Eigen::Matrix<T, 3, 3> matrixRollPitchYaw(const T roll,
@@ -386,21 +386,11 @@ inline Eigen::Matrix<T, 3, 3> matrixRollPitchYaw(const T roll,
const T pitch,
const T pitch,
const T yaw)
const T yaw)
{
{
const T c_yaw (cos(yaw));
const Eigen::AngleAxis<T> ax = Eigen::AngleAxis<T>(roll, Eigen::Matrix<T, 3, 1>::UnitX());
const T s_yaw (sin(yaw));
const Eigen::AngleAxis<T> ay = Eigen::AngleAxis<T>(pitch, Eigen::Matrix<T, 3, 1>::UnitY());
 
const Eigen::AngleAxis<T> az = Eigen::AngleAxis<T>(yaw, Eigen::Matrix<T, 3, 1>::UnitZ());
const T c_pitch (cos(pitch));
return (az * ay * ax).toRotationMatrix().matrix();
const T s_pitch (sin(pitch));
const T c_roll (cos(roll));
const T s_roll (sin(roll));
Eigen::Matrix<T, 3, 3> rot;
rot << c_pitch*c_roll , -s_roll*c_pitch , s_pitch,
s_pitch*s_yaw*c_roll + s_roll*c_yaw , -s_pitch*s_roll*s_yaw + c_roll*c_yaw, -s_yaw*c_pitch,
-s_pitch*c_roll*c_yaw + s_roll*s_yaw, s_pitch*s_roll*c_yaw + s_yaw*c_roll, c_pitch*c_yaw;
return rot;
}
}
Loading