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 */ }