Skip to content

Commit 105817b

Browse files
authored
Hoist camera model dispatch per row in image warps (colmap#4935)
Supersedes colmap#4930. ## Problem `WarpImageBetweenCameras` and `WarpImageWithHomographyBetweenCameras` call `Camera::CamFromImg` and `Camera::ImgFromCam` on every pixel. Both go through the 19-way runtime `switch (model_id)` in `sensor/models/runtime.h` (~24M dispatches for a 12 MP image), which prevents the compiler from inlining the concrete camera model across a scanline, hoisting row-invariant terms, constant-folding unit depth (`w = 1`), and vectorizing the inner loop. ## Fix Hoist the `CAMERA_MODEL_SWITCH_CASES` dispatch out of the inner `x` loop into two scanline helpers (`CamFromImgRow` and `ImgFromCamRow`) in `src/colmap/image/warp.cc`, and unify both warp functions into a single `WarpBetweenCamerasImpl` (+30 net lines in `warp.cc`): - Optimizes both `WarpImageBetweenCameras` and `WarpImageWithHomographyBetweenCameras`, while deduplicating their setup, scanline loop, interpolation dispatch, and rescaling. - Accelerates both `CamFromImg` (target) and `ImgFromCam` (source) for **all** camera models without leaking camera math or parameter indices outside `sensor/models/`. - Kept local to `warp.cc` rather than `sensor/models/runtime.h`: other `CamFromImg` / `ImgFromCam` loops in the codebase (`P3PEstimator::Residuals`, `HomographyMatrixRayEstimator::Residuals`, `UndistortCamera`, `UndistortReconstruction`) operate on 100–10k points (<0.5–3 ms) or 3D rays with `w != 1`, whereas scanline generation (`x + 0.5` with scalar `y`, unit depth `w = 1`, and separate input/output row buffers) is significantly faster than generic in-place point-array batching. ### Comparison with colmap#4930 colmap#4930 special-cases `PinholeCameraModel` in `WarpImageBetweenCameras` by indexing `params[0..3]` directly. Compared to colmap#4930, this PR: - Keeps all camera parameter layout and projection math inside `sensor/models/`. - Works for all 19 camera models on both source and target cameras (not only `PINHOLE` targets). - Covers `WarpImageWithHomographyBetweenCameras` (stereo rectification) as well. - Delivers larger speedups even on `PINHOLE` targets by also eliminating per-pixel dispatch and `w = 1` division in `ImgFromCam`. ## Benchmark 4032×3024 RGB image (12.2 MP), Release build (`g++ 15.2 -O3 -DNDEBUG`) on Intel Xeon @ 2.20 GHz, best of 5 runs after warmup; all outputs are bit-identical to `main`: | Function | Cameras (`source -> target`) | Interpolation | `main` | colmap#4930 | **This PR** | |---|---|---|---|---|---| | `WarpImageBetweenCameras` | `OPENCV -> PINHOLE` | Bilinear | 1175.9 ms | 908.7 ms (−22.7%) | **723.9 ms (−38.4%)** | | `WarpImageBetweenCameras` | `OPENCV -> PINHOLE` | Nearest | 726.3 ms | 447.4 ms (−38.4%) | **274.4 ms (−62.2%)** | | `WarpImageBetweenCameras` | `SIMPLE_RADIAL -> PINHOLE` | Bilinear | 1109.1 ms | 844.7 ms (−23.8%) | **660.3 ms (−40.5%)** | | `WarpImageBetweenCameras` | `OPENCV -> SIMPLE_PINHOLE` | Bilinear | 1217.4 ms | 1180.9 ms (−3.0%) | **669.5 ms (−45.0%)** | | `WarpImageWithHomographyBetweenCameras` | `OPENCV -> PINHOLE` | Bilinear | 1159.3 ms | 1159.3 ms (0.0%) | **767.6 ms (−33.8%)** | ## Tests - `Warp.CameraModelsMatchPerPixelProjection` (`src/colmap/image/warp_test.cc`) checks bit-exact output against per-pixel `Camera::CamFromImg` and `Camera::ImgFromCam` across `OPENCV -> PINHOLE`, `SIMPLE_DIVISION -> FOV` (partial `ImgFromCam` failure), and `SIMPLE_RADIAL -> EQUIRECTANGULAR` (partial `CamFromImg` failure) for both interpolation modes, both direct and intermediate-resolution warp paths, and both `WarpImageBetweenCameras` and `WarpImageWithHomographyBetweenCameras`. - `image/warp_test`, `image/undistortion_test`, `image/line_test`, and `controllers/undistorters_test` pass.
1 parent d0a411f commit 105817b

2 files changed

Lines changed: 295 additions & 124 deletions

File tree

‎src/colmap/image/warp.cc‎

Lines changed: 155 additions & 124 deletions
Original file line numberDiff line numberDiff line change
@@ -2,12 +2,15 @@
22

33
#include "colmap/image/warp.h"
44

5+
#include "colmap/sensor/models.h"
56
#include "colmap/util/eigen_alignment.h"
67
#include "colmap/util/logging.h"
78

89
#include "thirdparty/VLFeat/imopv.h"
910

1011
#include <algorithm>
12+
#include <cstdint>
13+
#include <vector>
1114

1215
#include <Eigen/Geometry>
1316

@@ -61,52 +64,150 @@ bool ShouldWarpDirectly(const Camera& source_camera,
6164
return std::min(scale_x, scale_y) >= options.direct_warp_min_scale;
6265
}
6366

67+
// Unproject a scanline to normalized camera coordinates, dispatching on the
68+
// camera model once per row. If `img_points` is null, unprojects pixel centers
69+
// `(x + 0.5, y)`; otherwise unprojects `img_points[x]` where `valid[x]` holds.
70+
void CamFromImgRow(const Camera& camera,
71+
const double y,
72+
const int width,
73+
const Eigen::Vector2d* img_points,
74+
uint8_t* valid,
75+
Eigen::Vector2d* cam_points) {
76+
const double* params = camera.params.data();
77+
switch (camera.model_id) {
78+
#define CAMERA_MODEL_CASE(CameraModel) \
79+
case CameraModel::model_id: \
80+
if (img_points == nullptr) { \
81+
for (int x = 0; x < width; ++x) { \
82+
valid[x] = CameraModel::CamFromImg( \
83+
params, x + 0.5, y, &cam_points[x].x(), &cam_points[x].y()); \
84+
} \
85+
} else { \
86+
for (int x = 0; x < width; ++x) { \
87+
if (valid[x]) { \
88+
valid[x] = CameraModel::CamFromImg(params, \
89+
img_points[x].x(), \
90+
img_points[x].y(), \
91+
&cam_points[x].x(), \
92+
&cam_points[x].y()); \
93+
} \
94+
} \
95+
} \
96+
break;
97+
98+
CAMERA_MODEL_SWITCH_CASES
99+
100+
#undef CAMERA_MODEL_CASE
101+
}
102+
}
103+
104+
// Project normalized camera coordinates (u, v, 1) along a scanline to image
105+
// coordinates, dispatching on the camera model once per row.
106+
void ImgFromCamRow(const Camera& camera,
107+
const int width,
108+
const Eigen::Vector2d* cam_points,
109+
uint8_t* valid,
110+
Eigen::Vector2d* img_points) {
111+
const double* params = camera.params.data();
112+
switch (camera.model_id) {
113+
#define CAMERA_MODEL_CASE(CameraModel) \
114+
case CameraModel::model_id: \
115+
for (int x = 0; x < width; ++x) { \
116+
if (valid[x]) { \
117+
valid[x] = CameraModel::ImgFromCam(params, \
118+
cam_points[x].x(), \
119+
cam_points[x].y(), \
120+
1.0, \
121+
&img_points[x].x(), \
122+
&img_points[x].y()); \
123+
} \
124+
} \
125+
break;
126+
127+
CAMERA_MODEL_SWITCH_CASES
128+
129+
#undef CAMERA_MODEL_CASE
130+
}
131+
}
132+
64133
template <WarpImageOptions::Interpolation interpolation>
65-
void WarpImageBetweenCamerasImpl(const WarpImageOptions& options,
66-
const Camera& source_camera,
67-
const Camera& target_camera,
68-
const Bitmap& source_image,
69-
Bitmap* target_image) {
134+
void WarpBetweenCamerasImpl(const WarpImageOptions& options,
135+
const Eigen::Matrix3d* H,
136+
const Camera& source_camera,
137+
const Camera& target_camera,
138+
const Bitmap& source_image,
139+
Bitmap* target_image) {
70140
THROW_CHECK_EQ(source_camera.width, source_image.Width());
71141
THROW_CHECK_EQ(source_camera.height, source_image.Height());
72142
THROW_CHECK_NOTNULL(target_image);
143+
THROW_CHECK_EQ(source_camera.params.size(),
144+
CameraModelNumParams(source_camera.model_id));
145+
THROW_CHECK_EQ(target_camera.params.size(),
146+
CameraModelNumParams(target_camera.model_id));
73147

74148
const bool warp_directly =
75149
ShouldWarpDirectly(source_camera, target_camera, options);
76150
Camera warp_target_camera = target_camera;
77-
if (!warp_directly) {
151+
Eigen::Matrix3d warp_H;
152+
if (H != nullptr) {
153+
warp_H = *H;
154+
if (!warp_directly) {
155+
// The intermediate image is at source resolution, but H maps pixels in
156+
// the target camera's coordinate system.
157+
const Eigen::Vector3d target_from_intermediate_scale(
158+
static_cast<double>(target_camera.width) /
159+
static_cast<double>(source_camera.width),
160+
static_cast<double>(target_camera.height) /
161+
static_cast<double>(source_camera.height),
162+
1.0);
163+
warp_H = *H * target_from_intermediate_scale.asDiagonal();
164+
}
165+
} else if (!warp_directly) {
78166
warp_target_camera.Rescale(source_camera.width, source_camera.height);
79167
}
168+
// Bitmap zero-initializes its buffer, so pixels skipped below remain black.
80169
*target_image = Bitmap(static_cast<int>(warp_directly ? target_camera.width
81170
: source_camera.width),
82171
static_cast<int>(warp_directly ? target_camera.height
83172
: source_camera.height),
84173
source_image.IsRGB());
85174

86-
Eigen::Vector2d image_point;
87-
for (int y = 0; y < target_image->Height(); ++y) {
88-
image_point.y() = y + 0.5;
89-
for (int x = 0; x < target_image->Width(); ++x) {
90-
image_point.x() = x + 0.5;
91-
92-
// Camera models assume that the upper left pixel center is (0.5, 0.5).
93-
const std::optional<Eigen::Vector2d> cam_point =
94-
warp_target_camera.CamFromImg(image_point);
95-
if (!cam_point) {
96-
target_image->SetPixel(x, y, BitmapColor<uint8_t>(0));
175+
const int width = target_image->Width();
176+
const int height = target_image->Height();
177+
std::vector<Eigen::Vector2d> img_points(width);
178+
std::vector<Eigen::Vector2d> cam_points(width);
179+
std::vector<uint8_t> valid(width);
180+
for (int y = 0; y < height; ++y) {
181+
// Camera models assume that the upper left pixel center is (0.5, 0.5).
182+
const double y_center = y + 0.5;
183+
if (H != nullptr) {
184+
for (int x = 0; x < width; ++x) {
185+
const Eigen::Vector3d warped_point =
186+
warp_H * Eigen::Vector3d(x + 0.5, y_center, 1.0);
187+
valid[x] = warped_point.z() != 0;
188+
if (valid[x]) {
189+
img_points[x] = warped_point.hnormalized();
190+
}
191+
}
192+
}
193+
CamFromImgRow(warp_target_camera,
194+
y_center,
195+
width,
196+
H != nullptr ? img_points.data() : nullptr,
197+
valid.data(),
198+
cam_points.data());
199+
ImgFromCamRow(source_camera,
200+
width,
201+
cam_points.data(),
202+
valid.data(),
203+
img_points.data());
204+
for (int x = 0; x < width; ++x) {
205+
if (!valid[x]) {
97206
continue;
98207
}
99-
100-
const std::optional<Eigen::Vector2d> source_point =
101-
source_camera.ImgFromCam(cam_point->homogeneous());
102-
103-
const auto color = source_point ? InterpolatePixel<interpolation>(
104-
source_image, *source_point)
105-
: std::nullopt;
106-
if (color) {
208+
if (const auto color =
209+
InterpolatePixel<interpolation>(source_image, img_points[x])) {
107210
target_image->SetPixel(x, y, *color);
108-
} else {
109-
target_image->SetPixel(x, y, BitmapColor<uint8_t>(0));
110211
}
111212
}
112213
}
@@ -116,27 +217,40 @@ void WarpImageBetweenCamerasImpl(const WarpImageOptions& options,
116217
}
117218
}
118219

119-
} // namespace
120-
121-
void WarpImageBetweenCameras(const WarpImageOptions& options,
122-
const Camera& source_camera,
123-
const Camera& target_camera,
124-
const Bitmap& source_image,
125-
Bitmap* target_image) {
220+
void WarpBetweenCameras(const WarpImageOptions& options,
221+
const Eigen::Matrix3d* H,
222+
const Camera& source_camera,
223+
const Camera& target_camera,
224+
const Bitmap& source_image,
225+
Bitmap* target_image) {
126226
switch (options.interpolation) {
127227
case WarpImageOptions::Interpolation::kNearestNeighbor:
128-
return WarpImageBetweenCamerasImpl<
228+
return WarpBetweenCamerasImpl<
129229
WarpImageOptions::Interpolation::kNearestNeighbor>(
130-
options, source_camera, target_camera, source_image, target_image);
230+
options, H, source_camera, target_camera, source_image, target_image);
131231
case WarpImageOptions::Interpolation::kBilinear:
132-
return WarpImageBetweenCamerasImpl<
133-
WarpImageOptions::Interpolation::kBilinear>(
134-
options, source_camera, target_camera, source_image, target_image);
232+
return WarpBetweenCamerasImpl<WarpImageOptions::Interpolation::kBilinear>(
233+
options, H, source_camera, target_camera, source_image, target_image);
135234
}
136235
LOG(FATAL_THROW) << "Invalid warp image interpolation mode: "
137236
<< static_cast<int>(options.interpolation);
138237
}
139238

239+
} // namespace
240+
241+
void WarpImageBetweenCameras(const WarpImageOptions& options,
242+
const Camera& source_camera,
243+
const Camera& target_camera,
244+
const Bitmap& source_image,
245+
Bitmap* target_image) {
246+
WarpBetweenCameras(options,
247+
/*H=*/nullptr,
248+
source_camera,
249+
target_camera,
250+
source_image,
251+
target_image);
252+
}
253+
140254
void WarpImageWithHomography(const Eigen::Matrix3d& H,
141255
const Bitmap& source_image,
142256
Bitmap* target_image) {
@@ -163,97 +277,14 @@ void WarpImageWithHomography(const Eigen::Matrix3d& H,
163277
}
164278
}
165279

166-
namespace {
167-
168-
template <WarpImageOptions::Interpolation interpolation>
169-
void WarpImageWithHomographyBetweenCamerasImpl(const WarpImageOptions& options,
170-
const Eigen::Matrix3d& H,
171-
const Camera& source_camera,
172-
const Camera& target_camera,
173-
const Bitmap& source_image,
174-
Bitmap* target_image) {
175-
THROW_CHECK_EQ(source_camera.width, source_image.Width());
176-
THROW_CHECK_EQ(source_camera.height, source_image.Height());
177-
THROW_CHECK_NOTNULL(target_image);
178-
179-
const bool warp_directly =
180-
ShouldWarpDirectly(source_camera, target_camera, options);
181-
Eigen::Matrix3d warp_H = H;
182-
if (!warp_directly) {
183-
// The intermediate image is at source resolution, but H maps pixels in
184-
// the target camera's coordinate system.
185-
const Eigen::Vector3d target_from_intermediate_scale(
186-
static_cast<double>(target_camera.width) /
187-
static_cast<double>(source_camera.width),
188-
static_cast<double>(target_camera.height) /
189-
static_cast<double>(source_camera.height),
190-
1.0);
191-
warp_H = H * target_from_intermediate_scale.asDiagonal();
192-
}
193-
*target_image = Bitmap(static_cast<int>(warp_directly ? target_camera.width
194-
: source_camera.width),
195-
static_cast<int>(warp_directly ? target_camera.height
196-
: source_camera.height),
197-
source_image.IsRGB());
198-
199-
Eigen::Vector3d image_point(0, 0, 1);
200-
for (int y = 0; y < target_image->Height(); ++y) {
201-
image_point.y() = y + 0.5;
202-
for (int x = 0; x < target_image->Width(); ++x) {
203-
image_point.x() = x + 0.5;
204-
205-
// Camera models assume that the upper left pixel center is (0.5, 0.5).
206-
const Eigen::Vector3d warped_point = warp_H * image_point;
207-
if (warped_point.z() == 0) {
208-
continue;
209-
}
210-
211-
const std::optional<Eigen::Vector2d> cam_point =
212-
target_camera.CamFromImg(warped_point.hnormalized());
213-
if (!cam_point) {
214-
target_image->SetPixel(x, y, BitmapColor<uint8_t>(0));
215-
continue;
216-
}
217-
218-
const std::optional<Eigen::Vector2d> source_point =
219-
source_camera.ImgFromCam(cam_point->homogeneous());
220-
221-
const auto color = source_point ? InterpolatePixel<interpolation>(
222-
source_image, *source_point)
223-
: std::nullopt;
224-
if (color) {
225-
target_image->SetPixel(x, y, *color);
226-
} else {
227-
target_image->SetPixel(x, y, BitmapColor<uint8_t>(0));
228-
}
229-
}
230-
}
231-
232-
if (!warp_directly) {
233-
target_image->Rescale(target_camera.width, target_camera.height);
234-
}
235-
}
236-
237-
} // namespace
238-
239280
void WarpImageWithHomographyBetweenCameras(const WarpImageOptions& options,
240281
const Eigen::Matrix3d& H,
241282
const Camera& source_camera,
242283
const Camera& target_camera,
243284
const Bitmap& source_image,
244285
Bitmap* target_image) {
245-
switch (options.interpolation) {
246-
case WarpImageOptions::Interpolation::kNearestNeighbor:
247-
return WarpImageWithHomographyBetweenCamerasImpl<
248-
WarpImageOptions::Interpolation::kNearestNeighbor>(
249-
options, H, source_camera, target_camera, source_image, target_image);
250-
case WarpImageOptions::Interpolation::kBilinear:
251-
return WarpImageWithHomographyBetweenCamerasImpl<
252-
WarpImageOptions::Interpolation::kBilinear>(
253-
options, H, source_camera, target_camera, source_image, target_image);
254-
}
255-
LOG(FATAL_THROW) << "Invalid warp image interpolation mode: "
256-
<< static_cast<int>(options.interpolation);
286+
WarpBetweenCameras(
287+
options, &H, source_camera, target_camera, source_image, target_image);
257288
}
258289

259290
void ResampleImageBilinear(const float* data,

0 commit comments

Comments
 (0)