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