Go to the documentation of this file.00001 #include <hrl_phri_2011/utils.h>
00002 #include <hrl_phri_2011/ellipsoid_space.h>
00003 #include <hrl_phri_2011/hsl_rgb_conversions.h>
00004 #include <hrl_phri_2011/EllipsoidParams.h>
00005
00006 int main(int argc, char **argv)
00007 {
00008 ros::init(argc, argv, "function_extractor");
00009 ros::NodeHandle nh;
00010
00011 if(argc < 4 || argc > 4) {
00012 printf("Usage: gray_reg_head head_pc ellipsoid_registration output_bag \n");
00013 return 1;
00014 }
00015
00016 PCRGB head_pc;
00017 Ellipsoid e;
00018 loadRegisteredHead(argv[1], argv[2], head_pc, e);
00019 double h, s, l;
00020 for(size_t i=0;i<head_pc.size();i++) {
00021 extractHSL(head_pc.points[i].rgb, h, s, l);
00022 writeHSL(0, 0, l, head_pc.points[i].rgb);
00023 }
00024
00025 head_pc.header.frame_id = "/base_link";
00026 rosbag::Bag bag;
00027 bag.open(argv[3], rosbag::bagmode::Write);
00028 bag.write("/data_cloud", ros::Time::now(), head_pc);
00029 bag.close();
00030 }