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
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041 #include <blort/blort/pal_util.h>
00042 #include <opencv2/imgproc/imgproc.hpp>
00043 #include <cmath>
00044 #include <sstream>
00045 #include <iostream>
00046 #include <algorithm>
00047
00048 namespace pal_blort
00049 {
00050
00051 std::string addRoot(const std::string& obj, const std::string& root)
00052 {
00053 std::stringstream ss;
00054 ss << root << "/" << obj;
00055 return ss.str();
00056 }
00057
00058 geometry_msgs::Pose tgPose2RosPose(const TomGine::tgPose &pose)
00059 {
00060 geometry_msgs::Pose res;
00061 res.position.x = pose.t.x;
00062 res.position.y = pose.t.y;
00063 res.position.z = pose.t.z;
00064 res.orientation.x = pose.q.x;
00065 res.orientation.y = pose.q.y;
00066 res.orientation.z = pose.q.z;
00067 res.orientation.w = pose.q.w;
00068 return res;
00069 }
00070
00071 TomGine::tgPose rosPose2TgPose(const geometry_msgs::Pose &pose)
00072 {
00073 TomGine::tgPose res;
00074 res.t.x = pose.position.x;
00075 res.t.y = pose.position.y;
00076 res.t.z = pose.position.z;
00077 res.q.x = pose.orientation.x;
00078 res.q.y = pose.orientation.y;
00079 res.q.z = pose.orientation.z;
00080 res.q.w = pose.orientation.w;
00081 return res;
00082 }
00083
00084 tf::Transform rosPose2TfTransform(const geometry_msgs::Pose &pose)
00085 {
00086 tf::Transform result;
00087 result.setOrigin(tf::Vector3(pose.position.x,
00088 pose.position.y,
00089 pose.position.z));
00090 result.setRotation(tf::Quaternion(pose.orientation.x,
00091 pose.orientation.y,
00092 pose.orientation.z,
00093 pose.orientation.w
00094 ));
00095 return result;
00096 }
00097
00098 geometry_msgs::Pose tfTransform2RosPose(const tf::Transform &transform)
00099 {
00100 geometry_msgs::Pose result;
00101 const tf::Vector3 position = transform.getOrigin();
00102 const tf::Quaternion orientation = transform.getRotation();
00103 result.position.x = position.x();
00104 result.position.y = position.y();
00105 result.position.z = position.z();
00106 result.orientation.x = orientation.x();
00107 result.orientation.y = orientation.y();
00108 result.orientation.z = orientation.z();
00109 result.orientation.w = orientation.w();
00110
00111 return result;
00112 }
00113
00114 geometry_msgs::Pose blortPosesToRosPose(geometry_msgs::Pose reference,
00115 geometry_msgs::Pose target)
00116 {
00117 const tf::Transform blort_reference_frame =
00118 pal_blort::rosPose2TfTransform(reference);
00119 const tf::Transform blort_target_pose =
00120 pal_blort::rosPose2TfTransform(target);
00121
00122 return pal_blort::tfTransform2RosPose(blort_reference_frame.inverse()*blort_target_pose);
00123 }
00124
00125 geometry_msgs::Pose poseAbsDiff(geometry_msgs::Pose pose1, geometry_msgs::Pose pose2)
00126 {
00127 geometry_msgs::Pose result;
00128 result.position.x = abs(pose1.position.x-pose2.position.x);
00129 result.position.y = abs(pose1.position.y-pose2.position.y);
00130 result.position.z = abs(pose1.position.z-pose2.position.z);
00131 result.orientation.x = abs(pose1.orientation.x-pose2.orientation.x);
00132 result.orientation.y = abs(pose1.orientation.y-pose2.orientation.y);
00133 result.orientation.z = abs(pose1.orientation.z-pose2.orientation.z);
00134 result.orientation.w = abs(pose1.orientation.w-pose2.orientation.w);
00135 return result;
00136 }
00137
00138 bool poseValidate(geometry_msgs::Pose known_pose,
00139 geometry_msgs::Pose pose_estimate,
00140 geometry_msgs::Pose max_error)
00141 {
00142 const geometry_msgs::Pose diff = poseAbsDiff(pose_estimate, known_pose);
00143 if((diff.position.x - max_error.position.x < 0) &&
00144 (diff.position.y - max_error.position.y < 0) &&
00145 (diff.position.z - max_error.position.z < 0) &&
00146 (diff.orientation.x - max_error.orientation.x < 0) &&
00147 (diff.orientation.y - max_error.orientation.y < 0) &&
00148 (diff.orientation.z - max_error.orientation.z < 0) &&
00149 (diff.orientation.w - max_error.orientation.w < 0))
00150 {
00151 return true;
00152 }
00153 else
00154 {
00155 return false;
00156 }
00157 }
00158
00159 cv::Mat quaternionTo3x3cvMat(geometry_msgs::Quaternion quaternion)
00160 {
00161 float x2 = quaternion.x * quaternion.x;
00162 float y2 = quaternion.y * quaternion.y;
00163 float z2 = quaternion.z * quaternion.z;
00164 float xy = quaternion.x * quaternion.y;
00165 float xz = quaternion.x * quaternion.z;
00166 float yz = quaternion.y * quaternion.z;
00167 float wx = quaternion.w * quaternion.x;
00168 float wy = quaternion.w * quaternion.y;
00169 float wz = quaternion.w * quaternion.z;
00170
00171 cv::Mat result(3,3, CV_32FC1);
00172 result.at<float>(0,0) = 1.0f - 2.0f * (y2 + z2);
00173 result.at<float>(0,1) = 2.0f * (xy - wz);
00174 result.at<float>(0,2) = 2.0f * (xz + wy);
00175
00176 result.at<float>(1,0) = 2.0f * (xy + wz);
00177 result.at<float>(1,1) = 1.0f - 2.0f * (x2 + z2);
00178 result.at<float>(1,2) = 2.0f * (yz - wx);
00179
00180 result.at<float>(2,0) = 2.0f * (xz - wy);
00181 result.at<float>(2,1) = 2.0f * (yz + wx);
00182 result.at<float>(2,2) = 1.0f - 2.0f * (x2 + y2);
00183
00184 return result;
00185 }
00186 }
