void update_arrows() {
geometry_msgs::Point origin, arrow_x_tip, arrow_y_tip, arrow_z_tip;
Eigen::Matrix3d R;
Eigen::Quaterniond quat;
quat.x() = g_stamped_pose.pose.orientation.x;
quat.y() = g_stamped_pose.pose.orientation.y;
quat.z() = g_stamped_pose.pose.orientation.z;
quat.w() = g_stamped_pose.pose.orientation.w;
R = quat.toRotationMatrix();
Eigen::Vector3d x_vec, y_vec, z_vec;
double veclen = 0.2; //make the arrows this long
x_vec = R.col(0) * veclen;
y_vec = R.col(1) * veclen;
z_vec = R.col(2) * veclen;
//update the arrow markers w/ new pose:
origin = g_stamped_pose.pose.position;
arrow_x_tip = origin;
arrow_x_tip.x += x_vec(0);
arrow_x_tip.y += x_vec(1);
arrow_x_tip.z += x_vec(2);
arrow_marker_x.points.clear();
arrow_marker_x.points.push_back(origin);
arrow_marker_x.points.push_back(arrow_x_tip);
arrow_marker_x.header = g_stamped_pose.header;
arrow_y_tip = origin;
arrow_y_tip.x += y_vec(0);
arrow_y_tip.y += y_vec(1);
arrow_y_tip.z += y_vec(2);
arrow_marker_y.points.clear();
arrow_marker_y.points.push_back(origin);
arrow_marker_y.points.push_back(arrow_y_tip);
arrow_marker_y.header = g_stamped_pose.header;
arrow_z_tip = origin;
arrow_z_tip.x += z_vec(0);
arrow_z_tip.y += z_vec(1);
arrow_z_tip.z += z_vec(2);
arrow_marker_z.points.clear();
arrow_marker_z.points.push_back(origin);
arrow_marker_z.points.push_back(arrow_z_tip);
arrow_marker_z.header = g_stamped_pose.header;
}
void Vector3<T>::rotate_inverse(enum Rotation rotation)
{
Vector3<T> x_vec(1.0f,0.0f,0.0f);
Vector3<T> y_vec(0.0f,1.0f,0.0f);
Vector3<T> z_vec(0.0f,0.0f,1.0f);
x_vec.rotate(rotation);
y_vec.rotate(rotation);
z_vec.rotate(rotation);
Matrix3<T> M(
x_vec.x, y_vec.x, z_vec.x,
x_vec.y, y_vec.y, z_vec.y,
x_vec.z, y_vec.z, z_vec.z
);
(*this) = M.mul_transpose(*this);
}
