1 #include "boxm2_export_oriented_point_cloud.h"
2 
3 #include <boxm2/cpp/algo/boxm2_export_oriented_point_cloud_function.h>
4 #include "vgl/vgl_intersection.h"
5 
6 bool boxm2_export_oriented_point_cloud::
export_oriented_point_cloud(boxm2_scene_sptr scene,const boxm2_cache_sptr & cache,const std::string & output_filename,bool output_aux,float vis_t,float nmag_t,float prob_t,float exp_t,const std::string & bb_filename)7 export_oriented_point_cloud(boxm2_scene_sptr scene, const boxm2_cache_sptr& cache,
8                             const std::string& output_filename, bool output_aux,
9                             float vis_t, float nmag_t, float prob_t, float exp_t,
10                             const std::string& bb_filename)
11 {
12   unsigned num_vertices = 0;
13 
14   std::ofstream myfile;
15   myfile.open(output_filename.c_str());
16 
17   //read bb from ply, if any
18   vgl_box_3d<double> original_bb;
19   if (!bb_filename.empty()) {
20     boxm2_export_oriented_point_cloud_function::readBBFromPLY(bb_filename, original_bb);
21     std::cout << "Read bb from PLY: " << original_bb << std::endl;
22   }
23 
24 
25   //zip through each block
26   std::map<boxm2_block_id, boxm2_block_metadata> blocks = scene->blocks();
27   std::map<boxm2_block_id, boxm2_block_metadata>::iterator blk_iter;
28   for (blk_iter = blocks.begin(); blk_iter != blocks.end(); ++blk_iter)
29   {
30     boxm2_block_id id = blk_iter->first;
31     boxm2_block_metadata blk_info= blk_iter->second;
32 
33     if (bb_filename.empty()){
34       original_bb = blk_info.bbox();
35     }
36 
37 
38     double finest_cell_length = (1.0 / (double) ( 1<<3))* blk_info.sub_block_dim_.x();
39     vgl_box_3d<double> bb_expanded = original_bb;
40 
41     bb_expanded.set_min_z(original_bb.min_z() - 10.0 * finest_cell_length);
42     bb_expanded.set_max_z(original_bb.max_z() + 40.0 * finest_cell_length);
43 
44    // bb_expanded.expand_about_centroid(finest_cell_length*10.0);
45 
46 
47     if (vgl_intersection(bb_expanded, blk_info.bbox()).is_empty())
48       continue;
49 
50     std::cout << "Processing Block: "<<id<< " with prob t: " << prob_t << ", vis t: " << vis_t << " and nmag_t: " << nmag_t << " finest cell length: " << finest_cell_length << std::endl;
51     boxm2_block *     blk     = cache->get_block(scene,id);
52 
53     //get data sizes
54     std::size_t alphaTypeSize = (int)boxm2_data_info::datasize(boxm2_data_traits<BOXM2_ALPHA>::prefix());
55     std::size_t pointTypeSize = boxm2_data_info::datasize(boxm2_data_traits<BOXM2_POINT>::prefix());
56     std::size_t normalTypeSize = boxm2_data_info::datasize(boxm2_data_traits<BOXM2_NORMAL>::prefix());
57     std::size_t visTypeSize = boxm2_data_info::datasize(boxm2_data_traits<BOXM2_VIS_SCORE>::prefix());
58     int mogSize = (int) boxm2_data_info::datasize(boxm2_data_traits<BOXM2_MOG3_GREY>::prefix());
59     std::size_t expTypeSize = boxm2_data_info::datasize(boxm2_data_traits<BOXM2_EXPECTATION>::prefix());
60     std::size_t nobsTypeSize = boxm2_data_info::datasize(boxm2_data_traits<BOXM2_NUM_OBS_SINGLE_INT>::prefix());
61     std::size_t raydirTypeSize = boxm2_data_info::datasize(boxm2_data_traits<BOXM2_RAY_DIR>::prefix());
62 
63     // check for invalid parameters
64     if( alphaTypeSize == 0 ) //This should never happen, it will result in division by zero later
65     {
66       std::cout << "ERROR: alphaTypeSize == 0 in " << __FILE__ << __LINE__ << std::endl;
67       return false;
68     }
69 
70     boxm2_data_base * alpha =        cache->get_data_base(scene, id,boxm2_data_traits<BOXM2_ALPHA>::prefix());
71     int data_buff_length    = (int) (alpha->buffer_length()/alphaTypeSize);
72 
73     //specify size to make sure data is right size.
74     boxm2_data_base * points = cache->get_data_base(scene,id,boxm2_data_traits<BOXM2_POINT>::prefix(), data_buff_length * pointTypeSize);
75     boxm2_data_base * normals = cache->get_data_base(scene,id,boxm2_data_traits<BOXM2_NORMAL>::prefix(), data_buff_length * normalTypeSize);
76     boxm2_data_base * vis = cache->get_data_base(scene,id,boxm2_data_traits<BOXM2_VIS_SCORE>::prefix(), data_buff_length * visTypeSize);
77     boxm2_data_base * vis_sum = cache->get_data_base(scene,id,boxm2_data_traits<BOXM2_VIS_SCORE>::prefix("sum"), data_buff_length * visTypeSize);
78     // boxm2_data_base * mog = cache->get_data_base(scene,id,boxm2_data_traits<BOXM2_MOG3_GREY>::prefix(), data_buff_length * mogSize);
79     boxm2_data_base * exp = cache->get_data_base(scene,id,boxm2_data_traits<BOXM2_EXPECTATION>::prefix(), data_buff_length * expTypeSize);
80     boxm2_data_base * nobs = cache->get_data_base(scene,id,boxm2_data_traits<BOXM2_NUM_OBS_SINGLE_INT>::prefix(), data_buff_length * nobsTypeSize);
81     boxm2_data_base * ray_dir_sum = cache->get_data_base(scene,id,boxm2_data_traits<BOXM2_RAY_DIR>::prefix(), data_buff_length * raydirTypeSize);
82     //boxm2_data_base * ray_dir_weighted_sum = cache->get_data_base(scene,id,boxm2_data_traits<BOXM2_RAY_DIR>::prefix(), data_buff_length * raydirTypeSize);
83 
84 
85     boxm2_block_metadata data = blk_iter->second;
86     if (output_filename.substr(output_filename.find_last_of('.') + 1) == "xyz")
87       boxm2_export_oriented_point_cloud_function::exportPointCloudXYZ(scene, data, blk, alpha, vis, vis_sum, exp, nobs, points,normals, ray_dir_sum, myfile, output_aux, vis_t, nmag_t, prob_t, exp_t, bb_expanded);
88  /*   else if (output_filename.substr(output_filename.find_last_of(".") + 1) == "ply")
89       boxm2_export_oriented_point_cloud_function::exportPointCloudPLY(scene, data, blk, alpha, mog, vis,  exp, nobs, points,normals, ray_dir_sum , ray_dir_weighted_sum, myfile, output_aux, vis_t, nmag_t, prob_t, exp_t, bb_expanded, num_vertices);*/
90     else if (output_filename.substr(output_filename.find_last_of('.') + 1) == "ply")
91       boxm2_export_oriented_point_cloud_function::exportPointCloudPLY(scene, data, blk, alpha, vis, points,normals, myfile, output_aux, vis_t, nmag_t, prob_t,  bb_expanded, num_vertices);
92 
93     else
94       std::cout << "UNKNOWN FILE FORMAT..." << std::endl;
95   }
96   myfile.flush();
97   myfile.close();
98 
99   //if ply, have to write annoying header at the beginning
100   if (output_filename.substr(output_filename.find_last_of('.') + 1) == "ply") {
101     std::ifstream myfile_input;
102     myfile_input.open(output_filename.c_str());
103     std::stringstream ss;
104     ss << myfile_input.rdbuf();
105     myfile_input.close();
106     myfile.open(output_filename.c_str());
107     boxm2_export_oriented_point_cloud_function::writePLYHeader(myfile,num_vertices,ss,output_aux);
108     myfile.flush();
109     myfile.close();
110   }
111 
112   return true;
113 }
114