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