1 //This is brl/bpro/core/bbas_pro/processes/bpgl_heightmap_from_disparity_process.cxx
2 #include <string>
3 #include <iostream>
4 #include <bprb/bprb_func_process.h>
5 #include <bprb/bprb_parameters.h>
6
7 #ifdef _MSC_VER
8 # include "vcl_msvc_warnings.h"
9 #endif
10
11 #include "vpgl/vpgl_camera_double_sptr.h"
12 #include "vpgl/vpgl_affine_camera.h"
13 #include "vil/vil_image_view_base.h"
14 #include "vil/vil_image_view.h"
15 #include "vil/vil_convert.h"
16 #include "vgl/vgl_box_3d.h"
17 #include "vnl/vnl_math.h"
18
19 #include <bpgl/algo/bpgl_heightmap_from_disparity.h>
20
21
22 //: process to convert heightmap to disparity
23 // template<class CAM_T>
24 // vil_image_view<float>
25 // bpgl_heightmap_from_disparity(CAM_T const& cam1, CAM_T const& cam2,
26 // vil_image_view<float> disparity, vgl_box_3d<double> heightmap_bounds,
27 // double ground_sample_distance);
28
29 namespace bpgl_heightmap_from_disparity_process_globals
30 {
31 unsigned n_inputs_ = 12;
32 unsigned n_outputs_ = 1;
33 }
34
35
bpgl_heightmap_from_disparity_process_cons(bprb_func_process & pro)36 bool bpgl_heightmap_from_disparity_process_cons(bprb_func_process& pro)
37 {
38 using namespace bpgl_heightmap_from_disparity_process_globals;
39
40 std::vector<std::string> input_types_;
41 input_types_.emplace_back("vpgl_camera_double_sptr"); // vpgl_affine_camera<double> camera 1
42 input_types_.emplace_back("vpgl_camera_double_sptr"); // vpgl_affine_camera<double> camera 2
43 input_types_.emplace_back("vil_image_view_base_sptr"); // vil_image_view<float> disparity
44 input_types_.emplace_back("float"); // min point x (e.g. lower left corner of a scene bbox)
45 input_types_.emplace_back("float"); // min point y
46 input_types_.emplace_back("float"); // min point z
47 input_types_.emplace_back("float"); // max point x (e.g. upper right corner of a scene bbox)
48 input_types_.emplace_back("float"); // max point y
49 input_types_.emplace_back("float"); // max point z
50 input_types_.emplace_back("float"); // ground sample distance
51 input_types_.emplace_back("float"); // minimum disparity
52 input_types_.emplace_back("float"); // z-offset
53
54 std::vector<std::string> output_types_;
55 output_types_.emplace_back("vil_image_view_base_sptr"); // vil_image_view<float> heightmap
56
57 return pro.set_input_types(input_types_) && pro.set_output_types(output_types_);
58 }
59
60
bpgl_heightmap_from_disparity_process(bprb_func_process & pro)61 bool bpgl_heightmap_from_disparity_process(bprb_func_process& pro)
62 {
63 using namespace bpgl_heightmap_from_disparity_process_globals;
64
65 // sanity check
66 if (!pro.verify_inputs()) {
67 std::cerr << pro.name() << ": wrong inputs!!!\n";
68 return false;
69 }
70
71 // get inputs
72 unsigned i = 0;
73 vpgl_camera_double_sptr camera1_sptr = pro.get_input<vpgl_camera_double_sptr>(i++);
74 vpgl_camera_double_sptr camera2_sptr = pro.get_input<vpgl_camera_double_sptr>(i++);
75 vil_image_view_base_sptr disparity_sptr = pro.get_input<vil_image_view_base_sptr>(i++);
76 auto min_x = pro.get_input<float>(i++);
77 auto min_y = pro.get_input<float>(i++);
78 auto min_z = pro.get_input<float>(i++);
79 auto max_x = pro.get_input<float>(i++);
80 auto max_y = pro.get_input<float>(i++);
81 auto max_z = pro.get_input<float>(i++);
82 auto gsd = pro.get_input<float>(i++);
83 auto min_disparity = pro.get_input<float>(i++);
84 auto z_offset = pro.get_input<float>(i++);
85
86 // convert cameras
87 auto* camera1_ptr = dynamic_cast<vpgl_affine_camera<double>*> (camera1_sptr.as_pointer());
88 if (!camera1_ptr) {
89 std::cerr << pro.name() << " :-- camera 1 is not affine" << std::endl;
90 return false;
91 }
92 auto* camera2_ptr = dynamic_cast<vpgl_affine_camera<double>*> (camera2_sptr.as_pointer());
93 if (!camera2_ptr) {
94 std::cerr << pro.name() << " :-- camera 2 is not affine" << std::endl;
95 return false;
96 }
97
98 // convert disparity image
99 auto* disparity_ptr = dynamic_cast<vil_image_view<float>*> (disparity_sptr.as_pointer());
100 if (!disparity_ptr) {
101 std::cerr << pro.name() << " :-- disparity is not vil_image_view<float>" << std::endl;
102 return false;
103 } else if (disparity_ptr->nplanes() != 1) {
104 std::cerr << pro.name() << " :-- disparity is not single band" << std::endl;
105 return false;
106 }
107
108 // WORKAROUND: set invalid disparity to NaN
109 // bpgl_heightmap_from_disparity does not test for valid disparity
110 // other than NAN values
111 vil_image_view<float> disparity_nan(disparity_ptr->ni(),disparity_ptr->nj());
112 disparity_nan.deep_copy(*disparity_ptr);
113
114 for (size_t j=0; j<disparity_nan.nj(); ++j) {
115 for (size_t i=0; i<disparity_nan.ni(); ++i) {
116 if (disparity_nan(i,j) < min_disparity) {
117 disparity_nan(i,j) = NAN;
118 }
119 }
120 }
121
122 // bounding box
123 vgl_box_3d<float> heightmap_bounds(min_x,min_y,min_z, max_x,max_y,max_z);
124
125 // process
126 vil_image_view<float> heightmap;
127 try {
128 heightmap = bpgl_heightmap_from_disparity(
129 *camera1_ptr, *camera2_ptr, disparity_nan, heightmap_bounds, gsd);
130 }
131 catch (const std::exception &err) {
132 std::cerr << "EXCEPTION: " << pro.name() << ", " << err.what() << std::endl;
133 return false;
134 }
135
136 // add offset to elevation estimates
137 // Additional WORKAROUND - set invalid to value below min_z (-9999.0)
138 for (size_t j=0; j<heightmap.nj(); ++j) {
139 for (size_t i=0; i<heightmap.ni(); ++i) {
140 if (!vnl_math::isfinite(heightmap(i,j))) {
141 heightmap(i,j) = -9999.0;
142 } else {
143 heightmap(i,j) += z_offset;
144 }
145 }
146 }
147
148 // return
149 pro.set_output_val<vil_image_view_base_sptr>(0, new vil_image_view<float>(heightmap));
150 return true;
151 }
152