1 #include <iostream>
2 #include <limits>
3 #include "testlib/testlib_test.h"
4 #ifdef _MSC_VER
5 #  include "vcl_msvc_warnings.h"
6 #endif
7 
8 #include <depth_map/depth_map_scene.h>
9 #include <depth_map/depth_map_region.h>
10 #include <depth_map/depth_map_region_sptr.h>
11 #include "vil/vil_image_view.h"
12 #include <vsol/vsol_polygon_2d.h>
13 #include <vsol/vsol_polygon_3d.h>
14 #include <vsol/vsol_point_2d.h>
15 #include <vsol/vsol_point_3d.h>
16 #include "vpgl/vpgl_perspective_camera.h"
17 #include <bpgl/bpgl_camera_utils.h>
18 #include "vil/vil_save.h"
19 #include "vsl/vsl_binary_io.h"
20 #include "vpl/vpl.h"
21 
test_depth_map()22 static void test_depth_map()
23 {
24   // construct the camera looking along Y with 1.6m height off the ground
25   // focal length = 1126.
26   unsigned ni = 1280, nj = 720;
27   auto nid = static_cast<double>(ni), njd = static_cast<double>(nj);
28   double right_fov = 29.605;
29   double top_fov = 17.725;
30   double altitude = 1.6;
31   double heading = 0.0;
32   double tilt = 90.0;
33  // double tilt = 78.62;
34   double roll = 0.0;
35   vpgl_perspective_camera<double> cam =
36     bpgl_camera_utils::camera_from_kml(nid, njd, right_fov, top_fov,
37                                        altitude, heading, tilt, roll);
38   //construct a ground plane region
39   vsol_point_2d_sptr p0= new vsol_point_2d(0.0, 720.0);
40   vsol_point_2d_sptr p1= new vsol_point_2d(1280.0, 720.0);
41   vsol_point_2d_sptr p2= new vsol_point_2d(1280.0, 361.0);
42   vsol_point_2d_sptr p3= new vsol_point_2d(0.0, 361.0);
43 
44 
45   std::vector<vsol_point_2d_sptr> verts;
46   verts.push_back(p0);   verts.push_back(p1);
47   verts.push_back(p2);   verts.push_back(p3);
48 
49   vsol_polygon_2d_sptr gp = new vsol_polygon_2d(verts);
50   vgl_plane_3d<double> plane(0.0, 0.0, 1.0, 0.0);
51   depth_map_region_sptr gpr =
52     new depth_map_region(gp, plane, "test_region",
53                          depth_map_region::HORIZONTAL);
54   gpr->set_ground_plane_max_depth(10000.0, cam, 3.0);
55   // test ground plane homography
56   vgl_h_matrix_2d<double> H;
57   bool good_H = gpr->img_to_region_plane(cam, H);
58   TEST("ground plane homography", good_H, true);
59   vsol_polygon_2d_sptr r2d = gpr->region_2d();
60   vsol_polygon_3d_sptr r3d = gpr->region_3d();
61   vgl_plane_3d<double> pl3d = r3d->plane();
62   std::cout << "Plane: " << pl3d << '\n';
63   unsigned nv = r2d->size();
64   double erh = 0.0;
65   for (unsigned i = 0; i<nv; ++i) {
66     vsol_point_3d_sptr p3d = r3d->vertex(i);
67     vgl_point_2d<double> pimg = r2d->vertex(i)->get_p();
68     vgl_point_2d<double> pmp_2d = H(vgl_homg_point_2d<double>(pimg.x(), pimg.y()));
69     vgl_point_3d<double> rmap_3d = plane.world_coords(pmp_2d);
70     std::cout << pimg << ' ' << *p3d << ' ' << pmp_2d << ' ' << rmap_3d << '\n';
71     erh += std::fabs(p3d->x()-rmap_3d.x()) +
72       std::fabs(p3d->y()-rmap_3d.y()) +
73       std::fabs(p3d->z()-rmap_3d.z());
74   }
75 
76   TEST_NEAR("planar homography", erh, 0.0, 1e-4);
77   vsol_point_2d_sptr p0v= new vsol_point_2d(0.0, 360.0);
78   vsol_point_2d_sptr p1v= new vsol_point_2d(640.0, 360.0);
79   vsol_point_2d_sptr p2v= new vsol_point_2d(640.0, 0.0);
80   vsol_point_2d_sptr p3v= new vsol_point_2d(0.0, 0.0);
81   std::vector<vsol_point_2d_sptr> vertsv;
82   vertsv.push_back(p0v);   vertsv.push_back(p1v);
83   vertsv.push_back(p2v);   vertsv.push_back(p3v);
84 
85   vsol_polygon_2d_sptr vp = new vsol_polygon_2d(vertsv);
86 
87   double min_depth = 10000, max_depth = 30000;
88   double depth = 10000;
89   std::string name =  "vert_perp";
90   std::string image_path = "dummy_path";
91   depth_map_scene dms(ni, nj, image_path, cam, gpr, nullptr, std::vector<depth_map_region_sptr>());
92   dms.add_ortho_perp_region(vp, min_depth, max_depth, name);
93   /* bool success = */ dms.set_depth(depth, name);
94 
95   vsl_b_ofstream os("./temp.bin");
96   vsl_b_write(os, gpr);
97   os.close();
98   vsl_b_ifstream is("./temp.bin");
99   depth_map_region_sptr r_in;
100   vsl_b_read(is, r_in);
101   bool good = r_in
102            && r_in->name() == gpr->name()
103            && r_in->min_depth() == gpr->min_depth()
104            && r_in->region_2d()->size() == gpr->region_2d()->size();
105   TEST("binary read write - depth_map_region", good, true);
106 
107   vpl_unlink("./temp.bin");
108   depth_map_scene_sptr ssptr = new depth_map_scene(dms);
109   vsl_b_ofstream sos("./temps.bin");
110   vsl_b_write(sos, ssptr);
111   sos.close();
112   vsl_b_ifstream sis("./temps.bin");
113   depth_map_scene_sptr s_in;
114   vsl_b_read(sis, s_in);
115   vpl_unlink("./temps.bin");
116   good = s_in
117       && s_in->ni() == ssptr->ni()
118       && s_in->nj() == ssptr->nj()
119       && s_in->ground_plane()[0]->region_2d()->size() == gpr->region_2d()->size()
120       && s_in->image_path() == image_path;
121   TEST("binary read write - depth_map_scene", good, true);
122 
123   // test ortho perp scene planes, sky region
124   vsol_point_2d_sptr p0s= new vsol_point_2d(640.0, 360.0);
125   vsol_point_2d_sptr p1s= new vsol_point_2d(1280.0, 360.0);
126   vsol_point_2d_sptr p2s= new vsol_point_2d(1280.0, 0.0);
127   vsol_point_2d_sptr p3s= new vsol_point_2d(640.0, 0.0);
128   std::vector<vsol_point_2d_sptr> vertss;
129   vertss.push_back(p0s);   vertss.push_back(p1s);
130   vertss.push_back(p2s);   vertss.push_back(p3s);
131 
132   vsol_polygon_2d_sptr sky = new vsol_polygon_2d(vertss);
133   dms.set_sky(sky);
134 
135 }
136 
137 
138 TESTMAIN(test_depth_map);
139