1 // Copyright (c) 2018, ETH Zurich and UNC Chapel Hill.
2 // All rights reserved.
3 //
4 // Redistribution and use in source and binary forms, with or without
5 // modification, are permitted provided that the following conditions are met:
6 //
7 //     * Redistributions of source code must retain the above copyright
8 //       notice, this list of conditions and the following disclaimer.
9 //
10 //     * Redistributions in binary form must reproduce the above copyright
11 //       notice, this list of conditions and the following disclaimer in the
12 //       documentation and/or other materials provided with the distribution.
13 //
14 //     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
15 //       its contributors may be used to endorse or promote products derived
16 //       from this software without specific prior written permission.
17 //
18 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
19 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
21 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
22 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
23 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
24 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
25 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
26 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
27 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
28 // POSSIBILITY OF SUCH DAMAGE.
29 //
30 // Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de)
31 
32 #include "base/warp.h"
33 
34 #include "VLFeat/imopv.h"
35 #include "util/logging.h"
36 
37 namespace colmap {
38 namespace {
39 
GetPixelConstantBorder(const float * data,const int rows,const int cols,const int row,const int col)40 float GetPixelConstantBorder(const float* data, const int rows, const int cols,
41                              const int row, const int col) {
42   if (row >= 0 && col >= 0 && row < rows && col < cols) {
43     return data[row * cols + col];
44   } else {
45     return 0;
46   }
47 }
48 
49 }  // namespace
50 
WarpImageBetweenCameras(const Camera & source_camera,const Camera & target_camera,const Bitmap & source_image,Bitmap * target_image)51 void WarpImageBetweenCameras(const Camera& source_camera,
52                              const Camera& target_camera,
53                              const Bitmap& source_image, Bitmap* target_image) {
54   CHECK_EQ(source_camera.Width(), source_image.Width());
55   CHECK_EQ(source_camera.Height(), source_image.Height());
56   CHECK_NOTNULL(target_image);
57 
58   target_image->Allocate(static_cast<int>(source_camera.Width()),
59                          static_cast<int>(source_camera.Height()),
60                          source_image.IsRGB());
61 
62   // To avoid aliasing, perform the warping in the source resolution and
63   // then rescale the image at the end.
64   Camera scaled_target_camera = target_camera;
65   if (target_camera.Width() != source_camera.Width() ||
66       target_camera.Height() != source_camera.Height()) {
67     scaled_target_camera.Rescale(source_camera.Width(), source_camera.Height());
68   }
69 
70   Eigen::Vector2d image_point;
71   for (int y = 0; y < target_image->Height(); ++y) {
72     image_point.y() = y + 0.5;
73     for (int x = 0; x < target_image->Width(); ++x) {
74       image_point.x() = x + 0.5;
75 
76       // Camera models assume that the upper left pixel center is (0.5, 0.5).
77       const Eigen::Vector2d world_point =
78           scaled_target_camera.ImageToWorld(image_point);
79       const Eigen::Vector2d source_point =
80           source_camera.WorldToImage(world_point);
81 
82       BitmapColor<float> color;
83       if (source_image.InterpolateBilinear(source_point.x() - 0.5,
84                                            source_point.y() - 0.5, &color)) {
85         target_image->SetPixel(x, y, color.Cast<uint8_t>());
86       } else {
87         target_image->SetPixel(x, y, BitmapColor<uint8_t>(0));
88       }
89     }
90   }
91 
92   if (target_camera.Width() != source_camera.Width() ||
93       target_camera.Height() != source_camera.Height()) {
94     target_image->Rescale(target_camera.Width(), target_camera.Height());
95   }
96 }
97 
WarpImageWithHomography(const Eigen::Matrix3d & H,const Bitmap & source_image,Bitmap * target_image)98 void WarpImageWithHomography(const Eigen::Matrix3d& H,
99                              const Bitmap& source_image, Bitmap* target_image) {
100   CHECK_NOTNULL(target_image);
101   CHECK_GT(target_image->Width(), 0);
102   CHECK_GT(target_image->Height(), 0);
103   CHECK_EQ(source_image.IsRGB(), target_image->IsRGB());
104 
105   Eigen::Vector3d target_pixel(0, 0, 1);
106   for (int y = 0; y < target_image->Height(); ++y) {
107     target_pixel.y() = y + 0.5;
108     for (int x = 0; x < target_image->Width(); ++x) {
109       target_pixel.x() = x + 0.5;
110 
111       const Eigen::Vector2d source_pixel = (H * target_pixel).hnormalized();
112 
113       BitmapColor<float> color;
114       if (source_image.InterpolateBilinear(source_pixel.x() - 0.5,
115                                            source_pixel.y() - 0.5, &color)) {
116         target_image->SetPixel(x, y, color.Cast<uint8_t>());
117       } else {
118         target_image->SetPixel(x, y, BitmapColor<uint8_t>(0));
119       }
120     }
121   }
122 }
123 
WarpImageWithHomographyBetweenCameras(const Eigen::Matrix3d & H,const Camera & source_camera,const Camera & target_camera,const Bitmap & source_image,Bitmap * target_image)124 void WarpImageWithHomographyBetweenCameras(const Eigen::Matrix3d& H,
125                                            const Camera& source_camera,
126                                            const Camera& target_camera,
127                                            const Bitmap& source_image,
128                                            Bitmap* target_image) {
129   CHECK_EQ(source_camera.Width(), source_image.Width());
130   CHECK_EQ(source_camera.Height(), source_image.Height());
131   CHECK_NOTNULL(target_image);
132 
133   target_image->Allocate(static_cast<int>(source_camera.Width()),
134                          static_cast<int>(source_camera.Height()),
135                          source_image.IsRGB());
136 
137   // To avoid aliasing, perform the warping in the source resolution and
138   // then rescale the image at the end.
139   Camera scaled_target_camera = target_camera;
140   if (target_camera.Width() != source_camera.Width() ||
141       target_camera.Height() != source_camera.Height()) {
142     scaled_target_camera.Rescale(source_camera.Width(), source_camera.Height());
143   }
144 
145   Eigen::Vector3d image_point(0, 0, 1);
146   for (int y = 0; y < target_image->Height(); ++y) {
147     image_point.y() = y + 0.5;
148     for (int x = 0; x < target_image->Width(); ++x) {
149       image_point.x() = x + 0.5;
150 
151       // Camera models assume that the upper left pixel center is (0.5, 0.5).
152       const Eigen::Vector3d warped_point = H * image_point;
153       const Eigen::Vector2d world_point =
154           target_camera.ImageToWorld(warped_point.hnormalized());
155       const Eigen::Vector2d source_point =
156           source_camera.WorldToImage(world_point);
157 
158       BitmapColor<float> color;
159       if (source_image.InterpolateBilinear(source_point.x() - 0.5,
160                                            source_point.y() - 0.5, &color)) {
161         target_image->SetPixel(x, y, color.Cast<uint8_t>());
162       } else {
163         target_image->SetPixel(x, y, BitmapColor<uint8_t>(0));
164       }
165     }
166   }
167 
168   if (target_camera.Width() != source_camera.Width() ||
169       target_camera.Height() != source_camera.Height()) {
170     target_image->Rescale(target_camera.Width(), target_camera.Height());
171   }
172 }
173 
ResampleImageBilinear(const float * data,const int rows,const int cols,const int new_rows,const int new_cols,float * resampled)174 void ResampleImageBilinear(const float* data, const int rows, const int cols,
175                            const int new_rows, const int new_cols,
176                            float* resampled) {
177   CHECK_NOTNULL(data);
178   CHECK_NOTNULL(resampled);
179   CHECK_GT(rows, 0);
180   CHECK_GT(cols, 0);
181   CHECK_GT(new_rows, 0);
182   CHECK_GT(new_cols, 0);
183 
184   const float scale_r = static_cast<float>(rows) / static_cast<float>(new_rows);
185   const float scale_c = static_cast<float>(cols) / static_cast<float>(new_cols);
186 
187   for (int r = 0; r < new_rows; ++r) {
188     const float r_i = (r + 0.5f) * scale_r - 0.5f;
189     const int r_i_min = std::floor(r_i);
190     const int r_i_max = r_i_min + 1;
191     const float d_r_min = r_i - r_i_min;
192     const float d_r_max = r_i_max - r_i;
193 
194     for (int c = 0; c < new_cols; ++c) {
195       const float c_i = (c + 0.5f) * scale_c - 0.5f;
196       const int c_i_min = std::floor(c_i);
197       const int c_i_max = c_i_min + 1;
198       const float d_c_min = c_i - c_i_min;
199       const float d_c_max = c_i_max - c_i;
200 
201       // Interpolation in column direction.
202       const float value1 =
203           d_c_max * GetPixelConstantBorder(data, rows, cols, r_i_min, c_i_min) +
204           d_c_min * GetPixelConstantBorder(data, rows, cols, r_i_min, c_i_max);
205       const float value2 =
206           d_c_max * GetPixelConstantBorder(data, rows, cols, r_i_max, c_i_min) +
207           d_c_min * GetPixelConstantBorder(data, rows, cols, r_i_max, c_i_max);
208 
209       // Interpolation in row direction.
210       resampled[r * new_cols + c] = d_r_max * value1 + d_r_min * value2;
211     }
212   }
213 }
214 
SmoothImage(const float * data,const int rows,const int cols,const float sigma_r,const float sigma_c,float * smoothed)215 void SmoothImage(const float* data, const int rows, const int cols,
216                  const float sigma_r, const float sigma_c, float* smoothed) {
217   CHECK_NOTNULL(data);
218   CHECK_NOTNULL(smoothed);
219   CHECK_GT(rows, 0);
220   CHECK_GT(cols, 0);
221   CHECK_GT(sigma_r, 0);
222   CHECK_GT(sigma_c, 0);
223   vl_imsmooth_f(smoothed, cols, data, cols, rows, cols, sigma_c, sigma_r);
224 }
225 
DownsampleImage(const float * data,const int rows,const int cols,const int new_rows,const int new_cols,float * downsampled)226 void DownsampleImage(const float* data, const int rows, const int cols,
227                      const int new_rows, const int new_cols,
228                      float* downsampled) {
229   CHECK_NOTNULL(data);
230   CHECK_NOTNULL(downsampled);
231   CHECK_LE(new_rows, rows);
232   CHECK_LE(new_cols, cols);
233   CHECK_GT(rows, 0);
234   CHECK_GT(cols, 0);
235   CHECK_GT(new_rows, 0);
236   CHECK_GT(new_cols, 0);
237 
238   const float scale_c = static_cast<float>(cols) / static_cast<float>(new_cols);
239   const float scale_r = static_cast<float>(rows) / static_cast<float>(new_rows);
240 
241   const float kSigmaScale = 0.5f;
242   const float sigma_c = std::max(std::numeric_limits<float>::epsilon(),
243                                  kSigmaScale * (scale_c - 1));
244   const float sigma_r = std::max(std::numeric_limits<float>::epsilon(),
245                                  kSigmaScale * (scale_r - 1));
246 
247   std::vector<float> smoothed(rows * cols);
248   SmoothImage(data, rows, cols, sigma_r, sigma_c, smoothed.data());
249 
250   ResampleImageBilinear(smoothed.data(), rows, cols, new_rows, new_cols,
251                         downsampled);
252 }
253 
254 }  // namespace colmap
255