articulation_structure: utils.cpp Source File

00001 /*
00002  * utils.cpp
00003  *
00004  *  Created on: Jul 25, 2010
00005  *      Author: sturm
00006  */
00007 
00008 #include "utils.h"
00009 using namespace std;
00010 using namespace geometry_msgs;
00011 using namespace sensor_msgs;
00012 using namespace articulation_models;
00013 
00014 std_msgs::ColorRGBA HSV_to_RGB(double h,double s,double v ) {
00015         h -= floor(h);
00016         h *= 6;
00017         int i;
00018         float m, n, f;
00019 
00020         i = floor(h);
00021         f = h - i;
00022         if (!(i & 1))
00023                 f = 1 - f; // if i is even
00024         m = v * (1 - s);
00025         n = v * (1 - s * f);
00026         std_msgs::ColorRGBA r;
00027         r.a = 1.0;
00028         switch (i) {
00029         case 6:
00030         case 0:
00031                 r.r=v; r.g=n; r.b=m; break;
00032         case 1:
00033                 r.r=n; r.g=v; r.b=m; break;
00034         case 2:
00035                 r.r=m; r.g=v; r.b=n; break;
00036         case 3:
00037                 r.r=m; r.g=n; r.b=v; break;
00038         case 4:
00039                 r.r=n; r.g=m; r.b=v; break;
00040         case 5:
00041                 r.r=v; r.g=m; r.b=n; break;
00042         default:
00043                 r.r=1; r.g=0.5; r.b=0.5;
00044         }
00045         return r;
00046 }
00047 
00048 btMatrix3x3 RPY_to_MAT(double roll, double pitch, double yaw){
00049         double sphi   = sin(roll);
00050         double stheta = sin(pitch);
00051         double spsi   = sin(yaw);
00052         double cphi   = cos(roll);
00053         double ctheta = cos(pitch);
00054         double cpsi   = cos(yaw);
00055 
00056     return(btMatrix3x3(
00057       cpsi*ctheta, cpsi*stheta*sphi - spsi*cphi, cpsi*stheta*cphi + spsi*sphi,
00058       spsi*ctheta, spsi*stheta*sphi + cpsi*cphi, spsi*stheta*cphi - cpsi*sphi,
00059           -stheta,                  ctheta*sphi,                  ctheta*cphi
00060     ));
00061 }
00062 
00063 void MAT_to_RPY(const btMatrix3x3& mat, double &roll, double &pitch, double &yaw) {
00064   roll = atan2(mat[2][1],mat[2][2]);
00065   pitch = atan2(-mat[2][0],sqrt(mat[2][1]*mat[2][1] + mat[2][2]* mat[2][2]));
00066   yaw = atan2(mat[1][0],mat[0][0]);
00067 }
00068 
00069 #define VEC(p1) "["<< \
00070 p1.getOrigin().x() << "," << p1.getOrigin().y()<<","<<p1.getOrigin().z()<< \
00071 "/"<< \
00072 p1.getRotation().x() << "," << p1.getRotation().y()<<","<<p1.getRotation().z()<<","<<p1.getRotation().w()<< \
00073 "]"
00074 
00075 string transform_to_string(const btTransform &p) {
00076         stringstream s;
00077         double roll,pitch,yaw;
00078         MAT_to_RPY(p.getBasis(),roll,pitch,yaw);
00079         s <<
00080                         p.getOrigin().x() << " " <<
00081                         p.getOrigin().y() << " " <<
00082                         p.getOrigin().z() << " " <<
00083                         roll << " " <<
00084                         pitch <<" " <<
00085                         yaw;
00086 
00087         return s.str();
00088 }
00089 
00090 string pose_to_string(const Pose &pose) {
00091         btTransform p = poseToTransform(pose);
00092         return transform_to_string(p);
00093 }
00094 
00095 string uncert_to_string(double sigma_position,double sigma_orientation) {
00096         stringstream s;
00097         double ip = 1/SQR(sigma_position);
00098         double io = 1/SQR(sigma_orientation);
00099         s <<
00100                         " " << ip <<" 0 0 0 0 0" <<
00101                         " " << ip <<" 0 0 0 0" <<
00102                         " " << ip <<" 0 0 0" <<
00103                         " " << io <<" 0 0" <<
00104                         " " << io <<" 0" <<
00105                         " " << io;
00106         return s.str();
00107 }
00108 
00109 double getBIC(double loglh, size_t k, size_t n) {
00110         return( -2*( loglh ) + ( k ) * log( n ) );
00111 }