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


jsk_perception
Author(s): Manabu Saito, Ryohei Ueda
autogenerated on Tue Jul 2 2019 19:41:07