thermalvis: thermalvis: pose.cpp Source File

00001 
00005 #include "pose.hpp"
00006 
00007 void assignPose(geometry_msgs::PoseStamped& pPose, Mat& C, int idx, ros::Time timestamp) {
00008         
00009         pPose.header.seq = idx;
00010         pPose.header.stamp = timestamp;
00011    
00012         Mat R, t;
00013         Quaterniond Q;
00014         decomposeTransform(C, R, t);
00015         matrixToQuaternion(R, Q);
00016    
00017         // tried: 1,0,2; 1,2,0; 0,2,1; 2,0,1; 2,1,0; 0,1,2
00018         // x-corresponds to graph -x; y to graph -z; z to graph -y
00019    
00020         pPose.pose.position.x = t.at<double>(2,0); //;
00021         pPose.pose.position.y = -t.at<double>(0,0); //t.at<double>(1,0);
00022         pPose.pose.position.z = -t.at<double>(1,0); //t.at<double>(2,0);
00023    
00024         if (abs(pPose.pose.position.x) > MAX_RVIZ_DISPLACEMENT) {
00025                         pPose.pose.position.x = 0.0;
00026         }
00027 
00028         if (abs(pPose.pose.position.y) > MAX_RVIZ_DISPLACEMENT) {
00029                         pPose.pose.position.y = 0.0;
00030         }
00031    
00032         if (abs(pPose.pose.position.z) > MAX_RVIZ_DISPLACEMENT) {
00033                         pPose.pose.position.z = 0.0;
00034         }
00035    
00036         //printf("%s << QUAT = (%f, %f, %f, %f)", __FUNCTION__, Q.x(), Q.y(), Q.z(), Q.w());
00037    
00038         // tried x,y,z,w
00039         pPose.pose.orientation.x = Q.z();
00040         pPose.pose.orientation.y = -Q.x();
00041         pPose.pose.orientation.z = -Q.y();
00042         pPose.pose.orientation.w = Q.w();
00043 }