1 //This is brl/bseg/bvxm/pro/processes/bvxm_update_point_cloud_process.cxx 2 #include "bvxm_update_point_cloud_process.h" 3 //: 4 // \file 5 6 #include <brdb/brdb_value.h> 7 #include <bprb/bprb_parameters.h> 8 9 #include "vgl/vgl_point_3d.h" 10 11 #include <bvxm/bvxm_voxel_world.h> 12 #include <bvxm/bvxm_image_metadata.h> 13 #include <bvxm/bvxm_mog_grey_processor.h> 14 15 struct compare_point_3d 16 { operator ()compare_point_3d17 bool operator()(const vgl_point_3d<float> &a, const vgl_point_3d<float> &b) 18 { 19 return a.z() > b.z(); 20 } 21 }; 22 bvxm_update_point_cloud_process_cons(bprb_func_process & pro)23bool bvxm_update_point_cloud_process_cons(bprb_func_process& pro) 24 { 25 using namespace bvxm_update_point_cloud_process_globals; 26 27 //process takes 4inputs 28 //input[0]: filename for input point cloud (x y z) 29 //input[1]: filename for the voxel grid 30 //input[2]: spacing in x-y 31 //input[3]: spacing in z 32 //input[4]: use opinion ? 33 std::vector<std::string> input_types_(n_inputs_); 34 input_types_[0] = "vcl_string"; 35 input_types_[1] = "bvxm_voxel_world_sptr"; 36 input_types_[2] = "float"; 37 input_types_[3] = "float"; 38 input_types_[4] = "bool"; 39 return pro.set_input_types(input_types_); 40 } 41 bvxm_update_point_cloud_process(bprb_func_process & pro)42bool bvxm_update_point_cloud_process(bprb_func_process& pro) 43 { 44 using namespace bvxm_update_point_cloud_process_globals; 45 46 //check number of inputs 47 if (pro.n_inputs()<n_inputs_) 48 { 49 std::cout << pro.name() << " The input number should be " << n_inputs_<< std::endl; 50 return false; 51 } 52 53 //get inputs 54 unsigned i = 0; 55 std::string point_filename = pro.get_input<std::string>(i++); 56 bvxm_voxel_world_sptr world = pro.get_input<bvxm_voxel_world_sptr>(i++); 57 /* float xy_spacing = */ pro.get_input<float>(i++); 58 /* float z_spacing = */ pro.get_input<float>(i++); 59 bool use_opinion = pro.get_input<bool>(i++); 60 61 std::ifstream ifile(point_filename.c_str()); 62 if (!ifile) 63 { 64 std::cout<<"Failed to open "<<point_filename<<std::endl; 65 return false; 66 } 67 68 std::vector<vgl_point_3d<float> > point_cloud; 69 float x,y,z; 70 71 //: read the file to obtain min, max 72 while (ifile) 73 { 74 ifile>>x>>y>>z; 75 vgl_point_3d<float> p3d(x,y,z); 76 point_cloud.push_back(p3d); 77 } 78 std::sort(point_cloud.begin(), point_cloud.end(), compare_point_3d()); 79 bool result=true; 80 if (!use_opinion) 81 { 82 result =result && world->update_point_cloud<OCCUPANCY>(point_cloud); 83 } 84 else { 85 std::cout << "Working with opinion!" << std::endl; 86 result =result && world->update_point_cloud<OCCUPANCY_OPINION>(point_cloud); 87 } 88 89 return result; 90 } 91