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