1 #include "testlib/testlib_test.h"
2 #include "vul/vul_file.h"
3
4 #include "vgl/vgl_point_3d.h"
5 #include "vgl/vgl_vector_3d.h"
6 #include <vgl/algo/vgl_rotation_3d.h>
7
8 #include <bvxm/bvxm_voxel_world.h>
9 #include <bvxm/bvxm_world_params.h>
10 #include <bvxm/bvxm_mog_grey_processor.h>
11 #include "vil/vil_math.h"
12 #include "vil/vil_load.h"
13 #include "vil/vil_image_view.h"
14 #include "vpgl/vpgl_perspective_camera.h"
15 #include "vpl/vpl.h"
16
17
test_voxel_world_mog_image()18 static void test_voxel_world_mog_image()
19 {
20 std::string model_dir("test_world_dir");
21 if (vul_file::is_directory(model_dir))
22 vpl_rmdir(model_dir.c_str());
23 else if (vul_file::exists(model_dir))
24 vpl_unlink(model_dir.c_str());
25 vul_file::make_directory(model_dir);
26
27
28 unsigned nx = 200;
29 unsigned ny = 200;
30 unsigned nz = 4;
31 vgl_point_3d<float> corner(0.f,0.f,0.f);
32 vgl_vector_3d<unsigned> num_voxels(nx,ny,nz);
33 float voxel_length = 1.0f;
34 unsigned scale=0;
35
36 // create a synthetic world
37 bvxm_world_params_sptr params = new bvxm_world_params();
38 params->set_params(model_dir, corner, num_voxels, voxel_length);
39 bvxm_voxel_world_sptr vox_world = new bvxm_voxel_world(params);
40
41 bvxm_voxel_grid_base_sptr ocp_grid_ptr = vox_world->get_grid<OCCUPANCY>(0,scale);
42 auto *ocp_grid = dynamic_cast<bvxm_voxel_grid<float>*>(ocp_grid_ptr.ptr());
43 // fill in grid with zeros to start
44 ocp_grid->initialize_data(0.0f);
45 // now make a ground plane
46 bvxm_voxel_grid<float>::iterator ocp_it = ocp_grid->slab_iterator(nz-1);
47 (*ocp_it).fill(1.0f);
48 // data not written to disk until iterator is iterated
49 ++ocp_it;
50
51 // create a synthetic image to fill layers with
52 bvxm_voxel_slab<float> plane_img(nx,ny,1);
53 for (unsigned i=0; i<nx; ++i) {
54 for (unsigned j=0; j<ny; ++j) {
55 // mark the origin/x axis
56 if ( (i < 20) && (j < 5) ) {
57 plane_img(i,j) = 0.2f;
58 }
59 // just make some squares of constant color
60 else if ( (i > 10) && (i < 90) && (j > 10) && (j < 90) ) {
61 plane_img(i,j) = 0.7f;
62 }
63 else if ((i > 110) && (i < 190) && (j > 10) && (j < 90) ) {
64 plane_img(i,j) = 0.5f;
65 }
66 else if ((i > 10) && (i < 90) && (j > 110) && (j < 190) ) {
67 plane_img(i,j) = 0.3f;
68 }
69 else if ((i > 110) && (i < 190) && (j > 110) && (j < 190) ) {
70 plane_img(i,j) = 0.1f;
71 }
72 else {
73 plane_img(i,j) = 1.0;
74 }
75 #if 0
76 plane_img(i,j) = plane_img(i,j) + ((0.2f*i)/(float)nx + (0.2f*j)/(float)ny);
77 #endif
78 }
79 }
80 typedef bvxm_voxel_traits<APM_MOG_GREY>::voxel_datatype mog_type;
81
82 bvxm_voxel_traits<APM_MOG_GREY>::appearance_processor apm_processor;
83
84 // create a slab of constant weights for update
85 bvxm_voxel_slab<float> ones(nx,ny,1);
86 ones.fill(1.0f);
87
88 // iterate through layers of apm grid and update each level with the same synthetic image
89 // if you want different levels to look different youll have to create a different image for each level
90 bvxm_voxel_grid_base_sptr apm_base = vox_world->get_grid<APM_MOG_GREY>(0,scale);
91 auto *apm_grid = dynamic_cast<bvxm_voxel_grid<mog_type>*>(apm_base.ptr());
92 // initialize the appearance model data to get rid of any previous data on disk
93 apm_grid->initialize_data(bvxm_voxel_traits<APM_MOG_GREY>::initial_val());
94
95 bvxm_voxel_grid<mog_type>::iterator apm_it = apm_grid->begin();
96 for (; apm_it != apm_grid->end(); ++apm_it) {
97 apm_processor.update(*apm_it, plane_img, ones);
98 }
99
100 // now create a couple of cameras and generate the expected images
101 vnl_matrix_fixed<double,3,3> K(0.0);
102 double f = 550.0;
103 double offx = 320.0;
104 double offy = 240.0;
105 K(0,0) = f; K(1,1) = f;
106 K(0,2) = offx; K(1,2) = offy;
107 K(2,2) = 1.0;
108 vgl_point_3d<double> center1(100,-100,275);
109 vgl_rotation_3d<double> rot1(5*vnl_math::pi/6,0.0,0.0);
110
111 vpgl_camera_double_sptr cam1 = new vpgl_perspective_camera<double>(K,center1,rot1);
112 bvxm_image_metadata meta1(vil_image_view_base_sptr(nullptr),cam1);
113 vil_image_view<float> mask(640,480,1);
114 vil_image_view_base_sptr img1 = new vil_image_view<unsigned char>(640,480);
115
116 vox_world->expected_image<APM_MOG_GREY>(meta1,img1,mask);
117
118 // debug: write out images
119 vil_save(*img1,"./expected1.png");
120
121 bvxm_image_metadata meta2(img1,cam1);
122
123 bvxm_voxel_slab_base_sptr mog_image;
124 TEST("testing mixture of gaussian image creation", vox_world->mixture_of_gaussians_image<APM_MOG_GREY>(meta2, mog_image, 0), true);
125 TEST("testing mixture of gaussian image creation", !mog_image, false);
126
127 auto* mog_image_ptr = dynamic_cast<bvxm_voxel_slab<mog_type>*>(mog_image.ptr());
128 TEST("testing mixture of gaussian image creation", !mog_image_ptr, false);
129
130 bvxm_voxel_slab<float> prob = apm_processor.expected_color(*mog_image_ptr);
131
132 vil_image_view_base_sptr expected_img = new vil_image_view<vxl_byte>(640,480,1);
133 bvxm_util::slab_to_img(prob, expected_img);
134 vil_save(*expected_img,"./expected2.png");
135
136 // we want the two expected images to be exactly the same
137 vil_image_view<float> im_dif;
138 vil_image_view<vxl_byte> expected_img_r(*expected_img);
139 vil_image_view<vxl_byte> img1_r(*img1);
140 vil_math_image_difference(expected_img_r, img1_r, im_dif);
141 float sum;
142 vil_math_sum(sum, im_dif, 0);
143 // MAY NEED FIX -- The two expected images differ only by their initial values. Other than that, the rendered results are same
144 //TEST_NEAR("image dif should sum to 0", sum, 0.0f, 5.0f); // imgs look exactly the same but there is dif of 4
145
146 bvxm_voxel_slab_base_sptr mog_image2;
147 TEST("testing mixture of gaussian image creation with samplling", vox_world->mog_image_with_random_order_sampling<APM_MOG_GREY>(meta2, 10, mog_image2, 0), true);
148 TEST("testing mixture of gaussian image creation with sampling", !mog_image2, false);
149
150 mog_image_ptr = dynamic_cast<bvxm_voxel_slab<mog_type>*>(mog_image2.ptr());
151 TEST("testing mixture of gaussian image creation with sampling", !mog_image_ptr, false);
152
153 bvxm_voxel_slab<float> prob2 = apm_processor.expected_color(*mog_image_ptr);
154
155 vil_image_view_base_sptr expected_img2 = new vil_image_view<vxl_byte>(640,480,1);
156 bvxm_util::slab_to_img(prob2, expected_img2);
157 vil_save(*expected_img2,"./expected3.png");
158
159
160 #if 0
161 // create an existing voxel world
162 bvxm_world_params_sptr params3 = new bvxm_world_params();
163 bgeo_lvcs_sptr lvcs = new bgeo_lvcs();
164 params3->set_params("D:\\projects\\change\\plasticville\\world_dir",
165 vgl_point_3d<float> (-10.0f, -10.0f, -5.0f),
166 vgl_vector_3d<unsigned int> (400, 400, 80),
167 0.27f, lvcs, 0.001f, 0.99f, 1);
168 bvxm_voxel_world_sptr vox_world2 = new bvxm_voxel_world;
169 vox_world2->set_params(params3);
170 // load a test image and camera
171 std::string image_fname = "D:/projects/change/plasticville/outseq/DSCN0521.jpg";
172 std::string camera_fname = "D:/projects/change/plasticville/outseq_cams/frame_00.txt";
173 vil_image_view_base_sptr img3 = vil_load(image_fname.c_str());
174 // read projection matrix from the file.
175 std::ifstream ifs(camera_fname.c_str());
176 vnl_matrix_fixed<double,3,4> projection_matrix; ifs >> projection_matrix;
177 vbl_smart_ptr<vpgl_camera<double> > procam3 = new vpgl_proj_camera<double>(projection_matrix);
178 bvxm_image_metadata meta3(img3,procam3);
179
180 bvxm_voxel_slab_base_sptr mog_image2;
181 TEST("testing mixture of gaussian image with sampling", vox_world2->mog_image_with_random_order_sampling<APM_MOG_GREY>(meta3, 5, mog_image2, 0, 0), true);
182 #endif
183 }
184
185 TESTMAIN( test_voxel_world_mog_image );
186
