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