hrl_phri_2011: utils.cpp Source File

Go to the documentation of this file.
00001 #include<hrl_phri_2011/utils.h>
00002 
00003 
00004 void applyRegistration(const PCRGB& in_pc, const hrl_phri_2011::EllipsoidParams& params, PCRGB& out_pc)
00005 {
00006     PCRGB pc_tf;
00007     tf::Transform tf_tf;
00008     tf::transformMsgToTF(params.e_frame.transform, tf_tf);
00009     Eigen::Affine3d tf_eigen;
00010     tf::TransformTFToEigen(tf_tf, tf_eigen);
00011     tf_eigen = tf_eigen.inverse();
00012     transformPC(in_pc, out_pc, tf_eigen);
00013 }
00014 
00015 void loadRegisteredHead(const string& head_bag, const string& params_bag, PCRGB& out_pc, Ellipsoid& ell)
00016 {
00017     // Load PC bag
00018     vector<PCRGB::Ptr> pc_list;
00019     readBagTopic<PCRGB>(head_bag, pc_list, "/stitched_head");
00020 
00021     // load params
00022     vector<hrl_phri_2011::EllipsoidParams::Ptr> params;
00023     readBagTopic<hrl_phri_2011::EllipsoidParams>(params_bag, params, "/ellipsoid_params");
00024 
00025     applyRegistration(*pc_list[0], *params[0], out_pc);
00026     ell.setParams(*params[0]);
00027 }