void AprilAnalysis::printDetection(AprilTags::TagDetection& detection) const {
std::cout << " Id: " << detection.id
<< " (Hamming: " << detection.hammingDistance << ")";
Eigen::Vector3d translation;
Eigen::Matrix3d rotation;
detection.getRelativeTranslationRotation(m_tagSize, m_fx, m_fy, m_px, m_py,
translation, rotation);
Eigen::Matrix3d F;
F << 1, 0, 0,
0, -1, 0,
0, 0, 1;
Eigen::Matrix3d fixed_rot = F*rotation;
double yaw, pitch, roll;
wRo_to_euler(fixed_rot, yaw, pitch, roll);
std::cout << " distance = " << translation.norm()
<< "m,\nx = " << translation(0)
<< "\ny = " << translation(1)
<< "\nz = " << translation(2)
<< "\nyaw = " << yaw
<< ", pitch = " << pitch
<< ", roll = " << roll
<< std::endl;
}
void print_detection(AprilTags::TagDetection& detection) {
if(detection.id != TAG_ID)
return;
// recovering the relative pose of a tag:
// NOTE: for this to be accurate, it is necessary to use the
// actual camera parameters here as well as the actual tag size
// (m_fx, m_fy, m_px, m_py, m_tagSize)
Eigen::Vector3d tag_translation; //in camera frame
Eigen::Matrix3d tag_rotation; //in camera frame
detection.getRelativeTranslationRotation(m_tagSize, m_fx, m_fy, m_px, m_py,
tag_translation, tag_rotation);
double tag_roll, tag_pitch, tag_yaw;
wRo_to_euler(tag_rotation, tag_yaw, tag_pitch, tag_roll);
// Note: Roll measures 180 degrees at a neutral attitude for some reason.
// I know this isn't yaw, but the yawWrap function does the bounding we want.
tag_roll = wave::yawWrap(tag_roll + M_PI);
Eigen::Vector3d qr_frame_tag_angles(tag_roll, tag_pitch, tag_yaw);
Eigen::Matrix3d rotation_roll_pi = wave::createEulerRot(M_PI, 0, 0);
qr_frame_tag_angles = rotation_roll_pi * qr_frame_tag_angles;
Eigen::Matrix3d rotation_yaw_half_pi = wave::createEulerRot(0, 0, M_PI/2);
qr_frame_tag_angles = rotation_yaw_half_pi * qr_frame_tag_angles;
tf::Quaternion tag_quaternion;
tag_quaternion.setEulerZYX(qr_frame_tag_angles(2), qr_frame_tag_angles(1),
qr_frame_tag_angles(0));
tf::Matrix3x3 tag_dcm(tag_quaternion);
Eigen::Vector3d copterTranslation = cToM*tag_translation;
double measured_x_position = -copterTranslation(2);
double measured_y_position = -copterTranslation(1);
double measured_z_position = -copterTranslation(0);
Eigen::Matrix4d measured_pose;
measured_pose <<
tag_dcm[0][0], tag_dcm[0][1], tag_dcm[0][2], measured_x_position,
tag_dcm[1][0], tag_dcm[1][1], tag_dcm[1][2], measured_y_position,
tag_dcm[2][0], tag_dcm[2][1], tag_dcm[2][2], measured_z_position,
0, 0, 0, 1;
// Correct for origin offset.
Eigen::Matrix4d origin_pose;
origin_pose <<
__origin_rotation_matrix[0][0], __origin_rotation_matrix[0][1], __origin_rotation_matrix[0][2], __origin_position(0),
__origin_rotation_matrix[1][0], __origin_rotation_matrix[1][1], __origin_rotation_matrix[1][2], __origin_position(1),
__origin_rotation_matrix[2][0], __origin_rotation_matrix[2][1], __origin_rotation_matrix[2][2], __origin_position(2),
0, 0, 0, 1;
Eigen::Matrix4d measured_pose_with_origin_correction;
measured_pose_with_origin_correction = origin_pose.inverse() * measured_pose;
tf::Matrix3x3 rotation_with_origin_correction(measured_pose_with_origin_correction(0,0),
measured_pose_with_origin_correction(0,1), measured_pose_with_origin_correction(0,2),
measured_pose_with_origin_correction(1,0), measured_pose_with_origin_correction(1,1),
measured_pose_with_origin_correction(1,2), measured_pose_with_origin_correction(2,0),
measured_pose_with_origin_correction(2,1), measured_pose_with_origin_correction(2,2));
tf::Quaternion final_quaternion;
rotation_with_origin_correction.getRotation(final_quaternion);
geometry_msgs::Pose to_publish;
to_publish.position.x = measured_pose_with_origin_correction(0, 3);
to_publish.position.y = measured_pose_with_origin_correction(1, 3);
to_publish.position.z = measured_pose_with_origin_correction(2, 3);
tf::quaternionTFToMsg(final_quaternion, to_publish.orientation);
cout<<"x: "<<to_publish.orientation.x<<", y: "<<to_publish.orientation.y<<", z:" << to_publish.orientation.z<<", w: "<<to_publish.orientation.w<<endl;
prev_t = ros::Time::now();
crop_image = true;
april_tag_one_target_ips::ips_msg curpose;
curpose.header.stamp = ros::Time::now();
curpose.tag_id = 0;
curpose.hamming_distance = 0;
curpose.header.frame_id="/map";
curpose.X = to_publish.position.x;
curpose.Y = to_publish.position.y;
curpose.Z = to_publish.position.z;
curpose.Roll = 0; //tf::getRoll(to_publish.orientation);;
curpose.Pitch = 0;//tf::getPitch(to_publish.orientation);;
curpose.Yaw = tf::getYaw(to_publish.orientation); // Robot Yaw
//republish pose for rviz
// send transform
br = new tf::TransformBroadcaster;
tform = new tf::Transform;
tform->setOrigin( tf::Vector3(to_publish.position.x, to_publish.position.y, 0) );
tf::Quaternion q;
q.setEulerZYX(tf::getYaw(to_publish.orientation), 0,0);
tform->setRotation( q );
*tform = tform->inverse();
br->sendTransform(tf::StampedTransform(*tform, ros::Time::now(), "base_footprint", "map"));
pose_pub.publish(curpose);
/*
#ifdef DEBUG_ROS_APRIL
double DEG2RAD = 180.0 / M_PI;
tf::Matrix3x3 m(q);
double roll, pitch, yaw;
m.getRPY(roll, pitch, yaw);
ROS_INFO("Euler Angles: %f, %f, %f", roll*DEG2RAD, pitch*DEG2RAD, yaw*DEG2RAD);
#endif
*/
}
