AxesArray.h
Go to the documentation of this file.
00001 /*
00002  * AxesArray.h
00003  *
00004  *  Created on: Jul 21, 2011
00005  *      Author: sweiss
00006  */
00007 
00008 #ifndef AXESARRAY_H_
00009 #define AXESARRAY_H_
00010 
00011 #define AX_DIST 0.1
00012 
00013 #include <ros/ros.h>
00014 #include <visualization_msgs/Marker.h>
00015 #include <visualization_msgs/MarkerArray.h>
00016 #include <geometry_msgs/Point.h>
00017 
00018 #include <TooN/TooN.h>
00019 #include <TooN/so3.h>
00020 
00021 class AxesArray
00022 {
00023 private:
00024   visualization_msgs::MarkerArray cubes;
00025   unsigned int ID;
00026   TooN::Matrix<3,3> quaternion2matrix(const double q[4]);
00027 
00028   // temporary variables
00029   geometry_msgs::Point p;
00030   geometry_msgs::Point buffp;
00031   TooN::SO3<double> rot;
00032   TooN::Vector<3,double> center;
00033   TooN::Vector<3,double> buffvec;
00034   visualization_msgs::Marker buffcube;
00035   static const float red[4];
00036   static const float green[4];
00037   static const float blue[4];
00038   static const double dirx[3];
00039   static const double diry[3];
00040   static const double dirz[3];
00041 
00042 
00043 public:
00044   AxesArray(){ID=0;};
00045   void init(double lifetime);   // shall we add here some init arguments?
00046   bool addAxes(const double pos[3], const double att[4],unsigned int id);
00047   void clearAxes() {cubes.markers.clear();};
00048   visualization_msgs::MarkerArray getAxes(){return cubes;};
00049   TooN::Vector<3> getCenter(const double pos[3], const double att[4]);
00050 };
00051 
00052 const float AxesArray::red[4]={1,0,0,1};
00053 const float AxesArray::green[4]={0,1,0,1};
00054 const float AxesArray::blue[4]={0,0,1,1};
00055 const double AxesArray::dirx[3]={AX_DIST,0.01,0.01};
00056 const double AxesArray::diry[3]={0.01,AX_DIST,0.01};
00057 const double AxesArray::dirz[3]={0.01,0.01,AX_DIST};
00058 
00059 void AxesArray::init(double lifetime)
00060 {
00061   clearAxes();
00062   buffcube.lifetime=ros::Duration(lifetime);
00063   buffcube.header.frame_id = "/world";
00064   buffcube.header.stamp = ros::Time::now();
00065   buffcube.ns = "pointcloud_publisher";
00066   buffcube.action = visualization_msgs::Marker::ADD;
00067   buffcube.type = visualization_msgs::Marker::CUBE;
00068   memcpy(&(buffcube.scale.x),dirx,sizeof(double)*3);
00069   memcpy(&(buffcube.color.r),red,sizeof(float)*4);
00070 }
00071 
00072 TooN::Vector<3> AxesArray::getCenter(const double pos[3], const double att[4])
00073 {
00074   rot = quaternion2matrix(att);
00075   //    return -(rot*TooN::makeVector(pos[0],pos[1],pos[2]));
00076   return -(rot.inverse()*TooN::makeVector(pos[0],pos[1],pos[2]));
00077 }
00078 
00079 bool AxesArray::addAxes(const double pos[3], const double att[4], unsigned int id)
00080 {
00081   center = getCenter(pos, att);
00082 
00083   // set cube attitude
00084   buffcube.pose.orientation.w=att[0];
00085   buffcube.pose.orientation.x=-att[1];
00086   buffcube.pose.orientation.y=-att[2];
00087   buffcube.pose.orientation.z=-att[3];
00088   //    memcpy(&(buffcube.pose.orientation.x),&(att[1]),sizeof(double)*3);
00089 
00090   // add x-axis
00091   buffcube.id = 10*id;
00092   buffvec=center+rot.inverse()*TooN::makeVector(AX_DIST/2,0.0,0.0);
00093   memcpy(&(buffcube.pose.position.x),&(buffvec[0]),sizeof(double)*3);
00094   memcpy(&(buffcube.scale.x),dirx,sizeof(double)*3);
00095   memcpy(&(buffcube.color.r),red,sizeof(float)*4);
00096   cubes.markers.push_back(buffcube);
00097 
00098   // add y-axis, keep orientation
00099   buffcube.id = 10*id+1;
00100   buffvec=center+rot.inverse()*TooN::makeVector(0.0, AX_DIST/2,0.0);
00101   memcpy(&(buffcube.pose.position.x),&(buffvec[0]),sizeof(double)*3);
00102   memcpy(&(buffcube.scale.x),diry,sizeof(double)*3);
00103   memcpy(&(buffcube.color.r),green,sizeof(float)*4);
00104   cubes.markers.push_back(buffcube);
00105 
00106   // add z-axis, keep orientation
00107   buffcube.id = 10*id+2;
00108   buffvec=center+rot.inverse()*TooN::makeVector(0.0,0.0,AX_DIST/2);
00109   memcpy(&(buffcube.pose.position.x),&(buffvec[0]),sizeof(double)*3);
00110   memcpy(&(buffcube.scale.x),dirz,sizeof(double)*3);
00111   memcpy(&(buffcube.color.r),blue,sizeof(float)*4);
00112   cubes.markers.push_back(buffcube);
00113 
00114   return true;
00115 }
00116 
00117 TooN::Matrix<3,3> AxesArray::quaternion2matrix(const double q[4])
00118 {
00119   // stolen from Eigen3 and adapted to TooN
00120 
00121   TooN::Matrix<3, 3, double> res;
00122 
00123   const double tx = 2 * q[1];
00124   const double ty = 2 * q[2];
00125   const double tz = 2 * q[3];
00126   const double twx = tx * q[0];
00127   const double twy = ty * q[0];
00128   const double twz = tz * q[0];
00129   const double txx = tx * q[1];
00130   const double txy = ty * q[1];
00131   const double txz = tz * q[1];
00132   const double tyy = ty * q[2];
00133   const double tyz = tz * q[2];
00134   const double tzz = tz * q[3];
00135 
00136   res(0, 0) = 1 - (tyy + tzz);
00137   res(0, 1) = txy - twz;
00138   res(0, 2) = txz + twy;
00139   res(1, 0) = txy + twz;
00140   res(1, 1) = 1 - (txx + tzz);
00141   res(1, 2) = tyz - twx;
00142   res(2, 0) = txz - twy;
00143   res(2, 1) = tyz + twx;
00144   res(2, 2) = 1 - (txx + tyy);
00145 
00146   return res;
00147 }
00148 
00149 
00150 #endif  /// AXESARRAY_H_


ptam
Author(s): Stephan Weiss, Markus Achtelik, Simon Lynen
autogenerated on Tue Jan 7 2014 11:12:22