1 // This is brl/bseg/boxm2/pro/processes/boxm2_add_aux_info_to_ply_process.cxx
2 #include <iostream>
3 #include <fstream>
4 #include <bprb/bprb_func_process.h>
5 //:
6 // \file
7 // \brief A process to ad auxiliary information (vis, prob, nmag) the vertices in a given ply file.
8 //
9 // \author Isabel Restrepo
10 // \date April 30, 2012
11
12 #include <boxm2/boxm2_scene.h>
13 #include <boxm2/boxm2_util.h>
14 #include <boxm2/io/boxm2_cache.h>
15
16 #include <boxm2/cpp/algo/boxm2_mog3_grey_processor.h>
17 #include <boct/boct_bit_tree.h>
18
19 #ifdef _MSC_VER
20 # include "vcl_msvc_warnings.h"
21 #endif
22 #include <cassert>
23
24 #include <rply.h>
25
26 namespace boxm2_add_aux_info_to_ply_process_globals
27 {
28 constexpr unsigned n_inputs_ = 4;
29 constexpr unsigned n_outputs_ = 0;
30
31 //helper class to read in bb from file
32 class ply_points_reader
33 {
34 public:
35 std::vector<vgl_point_3d<double> > points;
36 double p[3];
37 };
38
39 //: Call-back function for a "vertex" element
plyio_vertex_cb(p_ply_argument argument)40 int plyio_vertex_cb(p_ply_argument argument)
41 {
42 long index;
43 void* temp;
44 ply_get_argument_user_data(argument, &temp, &index);
45
46 auto* parsed_ply = (ply_points_reader*) temp;
47
48 switch (index)
49 {
50 case 0: // "x" coordinate
51 parsed_ply->p[0] = (double)ply_get_argument_value(argument);
52 break;
53 case 1: // "y" coordinate
54 parsed_ply->p[1] =(double)ply_get_argument_value(argument);
55 break;
56 case 2: // "z" coordinate
57 parsed_ply->p[2]= (double)ply_get_argument_value(argument);
58 // Insert point
59 parsed_ply->points.emplace_back(parsed_ply->p);
60 break;
61 default:
62 assert(!"This should not happen: index out of range");
63 }
64 return 1;
65 }
66
67 //: The PLY reader of PCL is rather strict, so lets load the cloud on our own
read_points_from_ply(const std::string & filename,std::vector<vgl_point_3d<double>> & points)68 bool read_points_from_ply(const std::string &filename, std::vector<vgl_point_3d<double> > &points)
69 {
70 ply_points_reader parsed_ply;
71 parsed_ply.points = points;
72
73 p_ply ply = ply_open(filename.c_str(), nullptr, 0, nullptr);
74 if (!ply) {
75 std::cout << "File " << filename << " doesn't exist.";
76 return false;
77 }
78 if (!ply_read_header(ply)){
79 std::cout << "File " << filename << " doesn't have header.";
80 return false;
81 }
82
83 // vertex
84 int nvertices = ply_set_read_cb(ply, "vertex", "x", plyio_vertex_cb, (void*) (&parsed_ply), 0);
85 ply_set_read_cb(ply, "vertex", "y", plyio_vertex_cb, (void*) (&parsed_ply), 1);
86 ply_set_read_cb(ply, "vertex", "z", plyio_vertex_cb, (void*) (&parsed_ply), 2);
87
88 std::cout << "Parsed: " << nvertices << "points\n";
89
90 // Read DATA
91 ply_read(ply);
92
93 // CLOSE file
94 ply_close(ply);
95
96 points=parsed_ply.points;
97
98 return true;
99 }
100 }
101
boxm2_add_aux_info_to_ply_process_cons(bprb_func_process & pro)102 bool boxm2_add_aux_info_to_ply_process_cons(bprb_func_process& pro)
103 {
104 using namespace boxm2_add_aux_info_to_ply_process_globals;
105
106 //process takes 4 or 5 inputs and no outputs
107 std::vector<std::string> input_types_(n_inputs_);
108 input_types_[0] = "boxm2_scene_sptr";
109 input_types_[1] = "boxm2_cache_sptr";
110 input_types_[2] = "vcl_string"; //input PLY filename
111 input_types_[3] = "vcl_string"; //output PLY filename
112
113 std::vector<std::string> output_types_(n_outputs_);
114 return pro.set_input_types(input_types_)
115 && pro.set_output_types(output_types_);
116 }
117
boxm2_add_aux_info_to_ply_process(bprb_func_process & pro)118 bool boxm2_add_aux_info_to_ply_process(bprb_func_process& pro)
119 {
120 using namespace boxm2_add_aux_info_to_ply_process_globals;
121
122 if ( pro.n_inputs() < n_inputs_ ) {
123 std::cout << pro.name() << ": The input number should be " << n_inputs_<< std::endl;
124 return false;
125 }
126
127 //get the inputs
128 unsigned i = 0;
129 boxm2_scene_sptr scene = pro.get_input<boxm2_scene_sptr>(i++);
130 boxm2_cache_sptr cache = pro.get_input<boxm2_cache_sptr>(i++);
131 std::string input_mesh_filename = pro.get_input<std::string>(i++);
132 std::string output_mesh_filename = pro.get_input<std::string>(i++);
133
134 std::string data_type;
135 std::vector<std::string> apps = scene->appearances();
136
137 //read incoming ply -- only points are read. Normals, prob, vis and color are read from the scene
138 std::vector<vgl_point_3d<double> > points;
139 read_points_from_ply(input_mesh_filename, points);
140
141 //write outgoing mesh header
142 p_ply oply = ply_create(output_mesh_filename.c_str(), PLY_ASCII, nullptr, 0, nullptr);
143
144 // HEADER SECTION
145 // vertex
146 ply_add_element(oply, "vertex", points.size());
147 ply_add_scalar_property(oply, "x", PLY_FLOAT); //PLY_FLOAT
148 ply_add_scalar_property(oply, "y", PLY_FLOAT); //PLY_FLOAT
149 ply_add_scalar_property(oply, "z", PLY_FLOAT); //PLY_FLOAT
150 ply_add_scalar_property(oply, "nx", PLY_FLOAT); //PLY_FLOAT
151 ply_add_scalar_property(oply, "ny", PLY_FLOAT); //PLY_FLOAT
152 ply_add_scalar_property(oply, "nz", PLY_FLOAT); //PLY_FLOAT
153 ply_add_scalar_property(oply, "prob", PLY_FLOAT); //PLY_FLOAT
154 ply_add_scalar_property(oply, "vis", PLY_FLOAT); //PLY_FLOAT
155 ply_add_scalar_property(oply, "nmag", PLY_FLOAT); //PLY_FLOAT
156 ply_add_scalar_property(oply, "diffuse_red", PLY_UCHAR); //PLY_UCHAR
157 ply_add_scalar_property(oply, "diffuse_green", PLY_UCHAR); //PLY_UCHAR
158 ply_add_scalar_property(oply, "diffuse_blue", PLY_UCHAR); //PLY_UCHAR
159 ply_add_scalar_property(oply, "tree_depth", PLY_UCHAR); //PLY_UCHAR
160
161 // end header
162 ply_write_header(oply);
163
164 std::cout << "Start iterating over pts..." << std::endl;
165
166 //get data sizes
167 std::size_t alphaTypeSize = (int)boxm2_data_info::datasize(boxm2_data_traits<BOXM2_ALPHA>::prefix());
168 //std::size_t pointTypeSize = boxm2_data_info::datasize(boxm2_data_traits<BOXM2_POINT>::prefix()); // UNUSED!! -- fixme
169 std::size_t normalTypeSize = boxm2_data_info::datasize(boxm2_data_traits<BOXM2_NORMAL>::prefix());
170 std::size_t visTypeSize = boxm2_data_info::datasize(boxm2_data_traits<BOXM2_VIS_SCORE>::prefix());
171 int mogSize = (int) boxm2_data_info::datasize(boxm2_data_traits<BOXM2_MOG3_GREY>::prefix());
172
173 //iterate through the points
174 float prob;
175 vgl_point_3d<double> local;
176 boxm2_block_id id;
177 for (auto pt : points) {
178
179 if (!scene->contains(pt, id, local)) {
180 std::cout << "ERROR: point: " << pt << " isn't in scene. Exiting...." << std::endl;
181 return false;
182 }
183
184 int index_x=(int)std::floor(local.x());
185 int index_y=(int)std::floor(local.y());
186 int index_z=(int)std::floor(local.z());
187
188 boxm2_block * blk=cache->get_block(scene,id);
189 boxm2_block_metadata mdata = scene->get_block_metadata_const(id);
190 vnl_vector_fixed<unsigned char,16> treebits=blk->trees()(index_x,index_y,index_z);
191 boct_bit_tree tree(treebits.data_block(),mdata.max_level_);
192 int bit_index=tree.traverse(local);
193
194 int depth=tree.depth_at(bit_index);
195 if (!tree.is_leaf(bit_index)) {
196 std::cout << "ERROR: point: " << pt << " isn't a leaf cell...." << std::endl;
197 //return false;
198 }
199
200 //get the base data
201 int data_offset=tree.get_data_index(bit_index,false);
202 boxm2_data_base * alpha_base = cache->get_data_base(scene,id,boxm2_data_traits<BOXM2_ALPHA>::prefix());
203 int data_buff_length = (int) (alpha_base->buffer_length()/alphaTypeSize);
204
205 // boxm2_data_base * points = cache->get_data_base(id,boxm2_data_traits<BOXM2_POINT>::prefix(), data_buff_length * pointTypeSize); // UNUSED!! -- fixme
206 boxm2_data_base * normals = cache->get_data_base(scene,id,boxm2_data_traits<BOXM2_NORMAL>::prefix(), data_buff_length * normalTypeSize);
207 boxm2_data_base * vis = cache->get_data_base(scene,id,boxm2_data_traits<BOXM2_VIS_SCORE>::prefix(), data_buff_length * visTypeSize);
208 boxm2_data_base * mog = cache->get_data_base(scene,id,boxm2_data_traits<BOXM2_MOG3_GREY>::prefix(), data_buff_length * mogSize);
209
210 //get the actual data
211 auto * alpha_data = (boxm2_data_traits<BOXM2_ALPHA>::datatype*) alpha_base->data_buffer();
212 // boxm2_data_traits<BOXM2_POINT>::datatype * points_data = (boxm2_data_traits<BOXM2_POINT>::datatype*) points->data_buffer(); // UNUSED!! -- fixme
213 auto * normals_data = (boxm2_data_traits<BOXM2_NORMAL>::datatype*) normals->data_buffer();
214 auto * vis_data = (boxm2_data_traits<BOXM2_VIS_SCORE>::datatype*) vis->data_buffer();
215 auto * mog_data = (boxm2_data_traits<BOXM2_MOG3_GREY>::datatype*) mog->data_buffer();
216
217 float alpha=alpha_data[data_offset];
218 double side_len = 1.0 / (double) (1 << depth);
219 //store cell probability
220 prob = 1.0f - (float)std::exp(-alpha * side_len * mdata.sub_block_dim_.x());
221 auto intensity = (unsigned char)(boxm2_mog3_grey_processor::expected_color(mog_data[data_offset])*255.0f);
222
223 ply_write(oply, pt.x());
224 ply_write(oply, pt.y());
225 ply_write(oply, pt.z());
226 ply_write(oply, normals_data[data_offset][0]);
227 ply_write(oply, normals_data[data_offset][1]);
228 ply_write(oply, normals_data[data_offset][2]);
229 ply_write(oply, prob);
230 ply_write(oply, vis_data[data_offset]);
231 ply_write(oply, normals_data[data_offset][3]);
232 ply_write(oply, (unsigned char)(intensity) );
233 ply_write(oply, (unsigned char)(intensity) );
234 ply_write(oply, (unsigned char)(intensity));
235 ply_write(oply, (unsigned char) depth);
236 }
237 std::cout << "Done iterating over pts..." << std::endl;
238
239 // CLOSE PLY FILE
240 ply_close(oply);
241
242 return true;
243 }
244