00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025 #include "imu_filter_madgwick/stateless_orientation.h"
00026 #include <tf2/LinearMath/Matrix3x3.h>
00027 #include <tf2/convert.h>
00028 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
00029
00030 template<typename T>
00031 static inline void crossProduct(
00032 T ax, T ay, T az,
00033 T bx, T by, T bz,
00034 T& rx, T& ry, T& rz) {
00035 rx = ay*bz - az*by;
00036 ry = az*bx - ax*bz;
00037 rz = ax*by - ay*bx;
00038 }
00039
00040
00041 template<typename T>
00042 static inline T normalizeVector(T& vx, T& vy, T& vz) {
00043 T norm = sqrt(vx*vx + vy*vy + vz*vz);
00044 T inv = 1.0 / norm;
00045 vx *= inv;
00046 vy *= inv;
00047 vz *= inv;
00048 return norm;
00049
00050 }
00051
00052
00053 bool StatelessOrientation::computeOrientation(
00054 WorldFrame::WorldFrame frame,
00055 geometry_msgs::Vector3 A,
00056 geometry_msgs::Vector3 E,
00057 geometry_msgs::Quaternion& orientation) {
00058
00059 float Hx, Hy, Hz;
00060 float Mx, My, Mz;
00061 float normH, invH, invA;
00062
00063
00064 float Ax = A.x, Ay = A.y, Az = A.z;
00065
00066
00067 float Ex = E.x, Ey = E.y, Ez = E.z;
00068
00069
00070
00071 crossProduct(Ex, Ey, Ez, Ax, Ay, Az, Hx, Hy, Hz);
00072
00073
00074 normH = normalizeVector(Hx, Hy, Hz);
00075 if (normH < 1E-7) {
00076
00077
00078
00079 return false;
00080 }
00081
00082
00083 normalizeVector(Ax, Ay, Az);
00084
00085
00086
00087 crossProduct(Ax, Ay, Az, Hx, Hy, Hz, Mx, My, Mz);
00088
00089
00090 tf2::Matrix3x3 R;
00091 switch (frame) {
00092 case WorldFrame::NED:
00093
00094
00095
00096
00097
00098
00099
00100
00101
00102 R[0][0] = Mx; R[0][1] = Hx; R[0][2] = -Ax;
00103 R[1][0] = My; R[1][1] = Hy; R[1][2] = -Ay;
00104 R[2][0] = Mz; R[2][1] = Hz; R[2][2] = -Az;
00105 break;
00106
00107 case WorldFrame::NWU:
00108
00109
00110
00111
00112
00113
00114
00115
00116
00117 R[0][0] = Mx; R[0][1] = -Hx; R[0][2] = Ax;
00118 R[1][0] = My; R[1][1] = -Hy; R[1][2] = Ay;
00119 R[2][0] = Mz; R[2][1] = -Hz; R[2][2] = Az;
00120 break;
00121
00122 default:
00123 case WorldFrame::ENU:
00124
00125
00126
00127
00128
00129
00130
00131
00132
00133 R[0][0] = Hx; R[0][1] = Mx; R[0][2] = Ax;
00134 R[1][0] = Hy; R[1][1] = My; R[1][2] = Ay;
00135 R[2][0] = Hz; R[2][1] = Mz; R[2][2] = Az;
00136 break;
00137 }
00138
00139
00140
00141 tf2::Quaternion q;
00142 R.getRotation(q);
00143 tf2::convert(q.inverse(), orientation);
00144 return true;
00145 }
00146
00147
00148 bool StatelessOrientation::computeOrientation(
00149 WorldFrame::WorldFrame frame,
00150 geometry_msgs::Vector3 A,
00151 geometry_msgs::Quaternion& orientation) {
00152
00153
00154
00155
00156
00157 geometry_msgs::Vector3 E;
00158 if (fabs(A.x) > 0.1 || fabs(A.y) > 0.1) {
00159 E.x = A.y;
00160 E.y = A.x;
00161 E.z = 0.0;
00162 } else if (fabs(A.z) > 0.1) {
00163 E.x = 0.0;
00164 E.y = A.z;
00165 E.z = A.y;
00166 } else {
00167
00168 return false;
00169 }
00170
00171 return computeOrientation(frame, A, E, orientation);
00172 }