white_balance_converter.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <sensor_msgs/Image.h>
00003 #include <sensor_msgs/PointCloud2.h>
00004 
00005 #include <Eigen/Dense>
00006 
00007 #include "jsk_perception/WhiteBalance.h"
00008 #include "jsk_perception/WhiteBalancePoints.h"
00009 
00010 class WhiteBalanceConverter {
00011 
00012 private:
00013   ros::NodeHandle nh_;
00014 
00015   ros::ServiceServer service_img_;
00016   ros::ServiceServer service_pts_;
00017 
00018   double cmat[12]; // convert_matrix
00019   double pmat[48]; // paramteter matrix
00020   //param
00021   int queue_size_;
00022 
00023   typedef union
00024   {
00025     struct /*anonymous*/
00026     {
00027       unsigned char Blue; // Blue channel
00028       unsigned char Green; // Green channel
00029       unsigned char Red; // Red channel
00030       unsigned char Alpha;
00031     };
00032     float float_value;
00033     long long_value;
00034   } RGBValue;
00035 
00036 public:
00037   WhiteBalanceConverter() : nh_ () {
00038     ros::NodeHandle pnh("~");
00039 
00040     if (pnh.hasParam("parameter_matrix")) {
00041       XmlRpc::XmlRpcValue param_val;
00042       pnh.getParam("parameter_matrix", param_val);
00043       // ROS_INFO("MATRIX %d", param_val.size());
00044       if (param_val.getType() == XmlRpc::XmlRpcValue::TypeArray && param_val.size() == 48) {
00045         for ( int i = 0; i < 48; i++) {
00046           pmat[i] = param_val[i];
00047         }
00048       }
00049       for ( int i = 0; i < 12; i++ ) {
00050         ROS_INFO("[%f, %f, %f, %f]", pmat[4*i + 0], pmat[4*i + 1], pmat[4*i + 2], pmat[4*i + 3]);
00051       }
00052     }
00053 
00054     service_img_ = pnh.advertiseService(pnh.resolveName("convert_image"),
00055                                         &WhiteBalanceConverter::imageCallback, this);
00056     service_pts_ = pnh.advertiseService(pnh.resolveName("convert_points"),
00057                                         &WhiteBalanceConverter::pointsCallback, this);
00058   }
00059 
00060   bool imageCallback(jsk_perception::WhiteBalance::Request &req,
00061                      jsk_perception::WhiteBalance::Response &res) {
00062     makeConvertMatrix(req.reference_color[0], req.reference_color[1], req.reference_color[2]);
00063 
00064     return true;
00065   }
00066 
00067   bool pointsCallback(jsk_perception::WhiteBalancePoints::Request &req,
00068                       jsk_perception::WhiteBalancePoints::Response &res) {
00069     makeConvertMatrix(req.reference_color[0], req.reference_color[1], req.reference_color[2]);
00070 
00071     int rgb_offset = -1;
00072     for (size_t i = 0; i < req.input.fields.size(); i++) {
00073       if ( req.input.fields[i].name == "rgb" ) {
00074         rgb_offset = req.input.fields[i].offset;
00075         break;
00076       }
00077     }
00078     //ROS_INFO("offst = %d", rgb_offset);
00079 
00080     if ( rgb_offset < 0 ) {
00081       ROS_WARN("point cloud didn't have rgb field");
00082       return false;
00083     }
00084 
00085     res.output = req.input;
00086     int size = req.input.height * req.input.width;
00087     int step = req.input.point_step;
00088     unsigned char *sdata = (unsigned char *) &(req.input.data[0]);
00089     unsigned char *ddata = (unsigned char *) &(res.output.data[0]);
00090 
00091     for(int i = 0; i < size; i++, sdata += step, ddata += step) {
00092       RGBValue rgb;
00093       rgb.float_value = *((float *)(sdata + rgb_offset));
00094       int r = rgb.Red; int g = rgb.Green; int b = rgb.Blue;
00095       convertColor(r, g, b);
00096       rgb.long_value = 0;
00097       rgb.Red = (unsigned char)r;
00098       rgb.Green = (unsigned char)g;
00099       rgb.Blue = (unsigned char)b;
00100       *((float *)(ddata + rgb_offset)) = rgb.float_value;
00101     }
00102 
00103     return true;
00104   }
00105 
00106   inline void convertColor (int &r, int &g, int &b) {
00107     double rr = ( cmat[0] * r ) + ( cmat[1] * g ) + (  cmat[2] * b ) +  cmat[3]*256;
00108     double gg = ( cmat[4] * r ) + ( cmat[5] * g ) + (  cmat[6] * b ) +  cmat[7]*256;
00109     double bb = ( cmat[8] * r ) + ( cmat[9] * g ) + ( cmat[10] * b ) + cmat[11]*256;
00110 
00111     r = round(rr);
00112     g = round(gg);
00113     b = round(bb);
00114 
00115     if(r > 255) r = 255; if (r < 0) r = 0;
00116     if(g > 255) g = 255; if (g < 0) g = 0;
00117     if(b > 255) b = 255; if (b < 0) b = 0;
00118   }
00119 
00120   void makeConvertMatrix(float refr, float refg, float refb) {
00121     for(int i = 0; i < 12; i++) {
00122       cmat[i] =
00123         ( pmat[4*i + 0] * refr ) +
00124         ( pmat[4*i + 1] * refg ) +
00125         ( pmat[4*i + 2] * refb ) +
00126         pmat[4*i + 3] ;
00127     }
00128     ROS_INFO("%f %f %f %f", cmat[0], cmat[1], cmat[2], cmat[3]);
00129     ROS_INFO("%f %f %f %f", cmat[4], cmat[5], cmat[6], cmat[7]);
00130     ROS_INFO("%f %f %f %f", cmat[8], cmat[9], cmat[10], cmat[11]);
00131 
00132     Eigen::Matrix4d mat;
00133     mat(0,0) = cmat[0]; mat(0,1) = cmat[1]; mat(0,2) = cmat[2];  mat(0,3) = cmat[3];
00134     mat(1,0) = cmat[4]; mat(1,1) = cmat[5]; mat(1,2) = cmat[6];  mat(1,3) = cmat[7];
00135     mat(2,0) = cmat[8]; mat(2,1) = cmat[9]; mat(2,2) = cmat[10]; mat(2,3) = cmat[11];
00136     mat(3,0) = 0.0;     mat(3,1) = 0.0;     mat(3,2) = 0.0;      mat(3,3) = 1.0;
00137 
00138     Eigen::Matrix4d imat = mat.inverse();
00139     cmat[0] = imat(0, 0); cmat[1] = imat(0, 1); cmat[2]  = imat(0, 2); cmat[3]  = imat(0, 3);
00140     cmat[4] = imat(1, 0); cmat[5] = imat(1, 1); cmat[6]  = imat(1, 2); cmat[7]  = imat(1, 3);
00141     cmat[8] = imat(2, 0); cmat[9] = imat(2, 1); cmat[10] = imat(2, 2); cmat[11] = imat(2, 3);
00142 
00143     ROS_INFO("%f %f %f %f", cmat[0], cmat[1], cmat[2], cmat[3]);
00144     ROS_INFO("%f %f %f %f", cmat[4], cmat[5], cmat[6], cmat[7]);
00145     ROS_INFO("%f %f %f %f", cmat[8], cmat[9], cmat[10], cmat[11]);
00146   }
00147 };
00148 
00149 int main (int argc, char** argv)
00150 {
00151   // ROS init
00152   ros::init (argc, argv, "white_balance_converter");
00153 
00154   WhiteBalanceConverter p;
00155   ros::spin ();
00156 
00157   return (0);
00158 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


jsk_perception
Author(s): Manabu Saito
autogenerated on Sat Mar 23 2013 18:21:32