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)23 bool 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)42 bool 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