1 /*
2 * Copyright (C) 2018 The Android Open Source Project
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16
17 #define LOG_TAG "Camera3-DistMapper"
18 #define ATRACE_TAG ATRACE_TAG_CAMERA
19 //#define LOG_NDEBUG 0
20
21 #include <algorithm>
22 #include <cmath>
23
24 #include "device3/DistortionMapper.h"
25
26 namespace android {
27
28 namespace camera3 {
29
30
DistortionMapper()31 DistortionMapper::DistortionMapper() : mValidMapping(false), mValidGrids(false) {
32 }
33
isDistortionSupported(const CameraMetadata & deviceInfo)34 bool DistortionMapper::isDistortionSupported(const CameraMetadata &deviceInfo) {
35 bool isDistortionCorrectionSupported = false;
36 camera_metadata_ro_entry_t distortionCorrectionModes =
37 deviceInfo.find(ANDROID_DISTORTION_CORRECTION_AVAILABLE_MODES);
38 for (size_t i = 0; i < distortionCorrectionModes.count; i++) {
39 if (distortionCorrectionModes.data.u8[i] !=
40 ANDROID_DISTORTION_CORRECTION_MODE_OFF) {
41 isDistortionCorrectionSupported = true;
42 break;
43 }
44 }
45 return isDistortionCorrectionSupported;
46 }
47
setupStaticInfo(const CameraMetadata & deviceInfo)48 status_t DistortionMapper::setupStaticInfo(const CameraMetadata &deviceInfo) {
49 std::lock_guard<std::mutex> lock(mMutex);
50 camera_metadata_ro_entry_t array;
51
52 array = deviceInfo.find(ANDROID_SENSOR_INFO_PRE_CORRECTION_ACTIVE_ARRAY_SIZE);
53 if (array.count != 4) return BAD_VALUE;
54
55 float arrayX = static_cast<float>(array.data.i32[0]);
56 float arrayY = static_cast<float>(array.data.i32[1]);
57 mArrayWidth = static_cast<float>(array.data.i32[2]);
58 mArrayHeight = static_cast<float>(array.data.i32[3]);
59
60 array = deviceInfo.find(ANDROID_SENSOR_INFO_ACTIVE_ARRAY_SIZE);
61 if (array.count != 4) return BAD_VALUE;
62
63 float activeX = static_cast<float>(array.data.i32[0]);
64 float activeY = static_cast<float>(array.data.i32[1]);
65 mActiveWidth = static_cast<float>(array.data.i32[2]);
66 mActiveHeight = static_cast<float>(array.data.i32[3]);
67
68 mArrayDiffX = activeX - arrayX;
69 mArrayDiffY = activeY - arrayY;
70
71 return updateCalibration(deviceInfo);
72 }
73
calibrationValid() const74 bool DistortionMapper::calibrationValid() const {
75 std::lock_guard<std::mutex> lock(mMutex);
76
77 return mValidMapping;
78 }
79
correctCaptureRequest(CameraMetadata * request)80 status_t DistortionMapper::correctCaptureRequest(CameraMetadata *request) {
81 std::lock_guard<std::mutex> lock(mMutex);
82 status_t res;
83
84 if (!mValidMapping) return OK;
85
86 camera_metadata_entry_t e;
87 e = request->find(ANDROID_DISTORTION_CORRECTION_MODE);
88 if (e.count != 0 && e.data.u8[0] != ANDROID_DISTORTION_CORRECTION_MODE_OFF) {
89 for (auto region : kMeteringRegionsToCorrect) {
90 e = request->find(region);
91 for (size_t j = 0; j < e.count; j += 5) {
92 int32_t weight = e.data.i32[j + 4];
93 if (weight == 0) {
94 continue;
95 }
96 res = mapCorrectedToRaw(e.data.i32 + j, 2, /*clamp*/true);
97 if (res != OK) return res;
98 }
99 }
100 for (auto rect : kRectsToCorrect) {
101 e = request->find(rect);
102 res = mapCorrectedRectToRaw(e.data.i32, e.count / 4, /*clamp*/true);
103 if (res != OK) return res;
104 }
105 }
106
107 return OK;
108 }
109
correctCaptureResult(CameraMetadata * result)110 status_t DistortionMapper::correctCaptureResult(CameraMetadata *result) {
111 std::lock_guard<std::mutex> lock(mMutex);
112 status_t res;
113
114 if (!mValidMapping) return OK;
115
116 res = updateCalibration(*result);
117 if (res != OK) {
118 ALOGE("Failure to update lens calibration information");
119 return INVALID_OPERATION;
120 }
121
122 camera_metadata_entry_t e;
123 e = result->find(ANDROID_DISTORTION_CORRECTION_MODE);
124 if (e.count != 0 && e.data.u8[0] != ANDROID_DISTORTION_CORRECTION_MODE_OFF) {
125 for (auto region : kMeteringRegionsToCorrect) {
126 e = result->find(region);
127 for (size_t j = 0; j < e.count; j += 5) {
128 int32_t weight = e.data.i32[j + 4];
129 if (weight == 0) {
130 continue;
131 }
132 res = mapRawToCorrected(e.data.i32 + j, 2, /*clamp*/true);
133 if (res != OK) return res;
134 }
135 }
136 for (auto rect : kRectsToCorrect) {
137 e = result->find(rect);
138 res = mapRawRectToCorrected(e.data.i32, e.count / 4, /*clamp*/true);
139 if (res != OK) return res;
140 }
141 for (auto pts : kResultPointsToCorrectNoClamp) {
142 e = result->find(pts);
143 res = mapRawToCorrected(e.data.i32, e.count / 2, /*clamp*/false);
144 if (res != OK) return res;
145 }
146 }
147
148 return OK;
149 }
150
151 // Utility methods; not guarded by mutex
152
updateCalibration(const CameraMetadata & result)153 status_t DistortionMapper::updateCalibration(const CameraMetadata &result) {
154 camera_metadata_ro_entry_t calib, distortion;
155
156 calib = result.find(ANDROID_LENS_INTRINSIC_CALIBRATION);
157 distortion = result.find(ANDROID_LENS_DISTORTION);
158
159 if (calib.count != 5) return BAD_VALUE;
160 if (distortion.count != 5) return BAD_VALUE;
161
162 // Skip redoing work if no change to calibration fields
163 if (mValidMapping &&
164 mFx == calib.data.f[0] &&
165 mFy == calib.data.f[1] &&
166 mCx == calib.data.f[2] &&
167 mCy == calib.data.f[3] &&
168 mS == calib.data.f[4]) {
169 bool noChange = true;
170 for (size_t i = 0; i < distortion.count; i++) {
171 if (mK[i] != distortion.data.f[i]) {
172 noChange = false;
173 break;
174 }
175 }
176 if (noChange) return OK;
177 }
178
179 mFx = calib.data.f[0];
180 mFy = calib.data.f[1];
181 mCx = calib.data.f[2];
182 mCy = calib.data.f[3];
183 mS = calib.data.f[4];
184
185 mInvFx = 1 / mFx;
186 mInvFy = 1 / mFy;
187
188 for (size_t i = 0; i < distortion.count; i++) {
189 mK[i] = distortion.data.f[i];
190 }
191
192 mValidMapping = true;
193 // Need to recalculate grid
194 mValidGrids = false;
195
196 return OK;
197 }
198
mapRawToCorrected(int32_t * coordPairs,int coordCount,bool clamp,bool simple)199 status_t DistortionMapper::mapRawToCorrected(int32_t *coordPairs, int coordCount,
200 bool clamp, bool simple) {
201 if (!mValidMapping) return INVALID_OPERATION;
202
203 if (simple) return mapRawToCorrectedSimple(coordPairs, coordCount, clamp);
204
205 if (!mValidGrids) {
206 status_t res = buildGrids();
207 if (res != OK) return res;
208 }
209
210 for (int i = 0; i < coordCount * 2; i += 2) {
211 const GridQuad *quad = findEnclosingQuad(coordPairs + i, mDistortedGrid);
212 if (quad == nullptr) {
213 ALOGE("Raw to corrected mapping failure: No quad found for (%d, %d)",
214 *(coordPairs + i), *(coordPairs + i + 1));
215 return INVALID_OPERATION;
216 }
217 ALOGV("src xy: %d, %d, enclosing quad: (%f, %f), (%f, %f), (%f, %f), (%f, %f)",
218 coordPairs[i], coordPairs[i+1],
219 quad->coords[0], quad->coords[1],
220 quad->coords[2], quad->coords[3],
221 quad->coords[4], quad->coords[5],
222 quad->coords[6], quad->coords[7]);
223
224 const GridQuad *corrQuad = quad->src;
225 if (corrQuad == nullptr) {
226 ALOGE("Raw to corrected mapping failure: No src quad found");
227 return INVALID_OPERATION;
228 }
229 ALOGV(" corr quad: (%f, %f), (%f, %f), (%f, %f), (%f, %f)",
230 corrQuad->coords[0], corrQuad->coords[1],
231 corrQuad->coords[2], corrQuad->coords[3],
232 corrQuad->coords[4], corrQuad->coords[5],
233 corrQuad->coords[6], corrQuad->coords[7]);
234
235 float u = calculateUorV(coordPairs + i, *quad, /*calculateU*/ true);
236 float v = calculateUorV(coordPairs + i, *quad, /*calculateU*/ false);
237
238 ALOGV("uv: %f, %f", u, v);
239
240 // Interpolate along top edge of corrected quad (which are axis-aligned) for x
241 float corrX = corrQuad->coords[0] + u * (corrQuad->coords[2] - corrQuad->coords[0]);
242 // Interpolate along left edge of corrected quad (which are axis-aligned) for y
243 float corrY = corrQuad->coords[1] + v * (corrQuad->coords[7] - corrQuad->coords[1]);
244
245 // Clamp to within active array
246 if (clamp) {
247 corrX = std::min(mActiveWidth - 1, std::max(0.f, corrX));
248 corrY = std::min(mActiveHeight - 1, std::max(0.f, corrY));
249 }
250
251 coordPairs[i] = static_cast<int32_t>(std::round(corrX));
252 coordPairs[i + 1] = static_cast<int32_t>(std::round(corrY));
253 }
254
255 return OK;
256 }
257
mapRawToCorrectedSimple(int32_t * coordPairs,int coordCount,bool clamp) const258 status_t DistortionMapper::mapRawToCorrectedSimple(int32_t *coordPairs, int coordCount,
259 bool clamp) const {
260 if (!mValidMapping) return INVALID_OPERATION;
261
262 float scaleX = mActiveWidth / mArrayWidth;
263 float scaleY = mActiveHeight / mArrayHeight;
264 for (int i = 0; i < coordCount * 2; i += 2) {
265 float x = coordPairs[i];
266 float y = coordPairs[i + 1];
267 float corrX = x * scaleX;
268 float corrY = y * scaleY;
269 if (clamp) {
270 corrX = std::min(mActiveWidth - 1, std::max(0.f, corrX));
271 corrY = std::min(mActiveHeight - 1, std::max(0.f, corrY));
272 }
273 coordPairs[i] = static_cast<int32_t>(std::round(corrX));
274 coordPairs[i + 1] = static_cast<int32_t>(std::round(corrY));
275 }
276
277 return OK;
278 }
279
mapRawRectToCorrected(int32_t * rects,int rectCount,bool clamp,bool simple)280 status_t DistortionMapper::mapRawRectToCorrected(int32_t *rects, int rectCount, bool clamp,
281 bool simple) {
282 if (!mValidMapping) return INVALID_OPERATION;
283 for (int i = 0; i < rectCount * 4; i += 4) {
284 // Map from (l, t, width, height) to (l, t, r, b)
285 int32_t coords[4] = {
286 rects[i],
287 rects[i + 1],
288 rects[i] + rects[i + 2] - 1,
289 rects[i + 1] + rects[i + 3] - 1
290 };
291
292 mapRawToCorrected(coords, 2, clamp, simple);
293
294 // Map back to (l, t, width, height)
295 rects[i] = coords[0];
296 rects[i + 1] = coords[1];
297 rects[i + 2] = coords[2] - coords[0] + 1;
298 rects[i + 3] = coords[3] - coords[1] + 1;
299 }
300
301 return OK;
302 }
303
mapCorrectedToRaw(int32_t * coordPairs,int coordCount,bool clamp,bool simple) const304 status_t DistortionMapper::mapCorrectedToRaw(int32_t *coordPairs, int coordCount, bool clamp,
305 bool simple) const {
306 return mapCorrectedToRawImpl(coordPairs, coordCount, clamp, simple);
307 }
308
309 template<typename T>
mapCorrectedToRawImpl(T * coordPairs,int coordCount,bool clamp,bool simple) const310 status_t DistortionMapper::mapCorrectedToRawImpl(T *coordPairs, int coordCount, bool clamp,
311 bool simple) const {
312 if (!mValidMapping) return INVALID_OPERATION;
313
314 if (simple) return mapCorrectedToRawImplSimple(coordPairs, coordCount, clamp);
315
316 float activeCx = mCx - mArrayDiffX;
317 float activeCy = mCy - mArrayDiffY;
318 for (int i = 0; i < coordCount * 2; i += 2) {
319 // Move to normalized space from active array space
320 float ywi = (coordPairs[i + 1] - activeCy) * mInvFy;
321 float xwi = (coordPairs[i] - activeCx - mS * ywi) * mInvFx;
322 // Apply distortion model to calculate raw image coordinates
323 float rSq = xwi * xwi + ywi * ywi;
324 float Fr = 1.f + (mK[0] * rSq) + (mK[1] * rSq * rSq) + (mK[2] * rSq * rSq * rSq);
325 float xc = xwi * Fr + (mK[3] * 2 * xwi * ywi) + mK[4] * (rSq + 2 * xwi * xwi);
326 float yc = ywi * Fr + (mK[4] * 2 * xwi * ywi) + mK[3] * (rSq + 2 * ywi * ywi);
327 // Move back to image space
328 float xr = mFx * xc + mS * yc + mCx;
329 float yr = mFy * yc + mCy;
330 // Clamp to within pre-correction active array
331 if (clamp) {
332 xr = std::min(mArrayWidth - 1, std::max(0.f, xr));
333 yr = std::min(mArrayHeight - 1, std::max(0.f, yr));
334 }
335
336 coordPairs[i] = static_cast<T>(std::round(xr));
337 coordPairs[i + 1] = static_cast<T>(std::round(yr));
338 }
339
340 return OK;
341 }
342
343 template<typename T>
mapCorrectedToRawImplSimple(T * coordPairs,int coordCount,bool clamp) const344 status_t DistortionMapper::mapCorrectedToRawImplSimple(T *coordPairs, int coordCount,
345 bool clamp) const {
346 if (!mValidMapping) return INVALID_OPERATION;
347
348 float scaleX = mArrayWidth / mActiveWidth;
349 float scaleY = mArrayHeight / mActiveHeight;
350 for (int i = 0; i < coordCount * 2; i += 2) {
351 float x = coordPairs[i];
352 float y = coordPairs[i + 1];
353 float rawX = x * scaleX;
354 float rawY = y * scaleY;
355 if (clamp) {
356 rawX = std::min(mArrayWidth - 1, std::max(0.f, rawX));
357 rawY = std::min(mArrayHeight - 1, std::max(0.f, rawY));
358 }
359 coordPairs[i] = static_cast<T>(std::round(rawX));
360 coordPairs[i + 1] = static_cast<T>(std::round(rawY));
361 }
362
363 return OK;
364 }
365
mapCorrectedRectToRaw(int32_t * rects,int rectCount,bool clamp,bool simple) const366 status_t DistortionMapper::mapCorrectedRectToRaw(int32_t *rects, int rectCount, bool clamp,
367 bool simple) const {
368 if (!mValidMapping) return INVALID_OPERATION;
369
370 for (int i = 0; i < rectCount * 4; i += 4) {
371 // Map from (l, t, width, height) to (l, t, r, b)
372 int32_t coords[4] = {
373 rects[i],
374 rects[i + 1],
375 rects[i] + rects[i + 2] - 1,
376 rects[i + 1] + rects[i + 3] - 1
377 };
378
379 mapCorrectedToRaw(coords, 2, clamp, simple);
380
381 // Map back to (l, t, width, height)
382 rects[i] = coords[0];
383 rects[i + 1] = coords[1];
384 rects[i + 2] = coords[2] - coords[0] + 1;
385 rects[i + 3] = coords[3] - coords[1] + 1;
386 }
387
388 return OK;
389 }
390
buildGrids()391 status_t DistortionMapper::buildGrids() {
392 if (mCorrectedGrid.size() != kGridSize * kGridSize) {
393 mCorrectedGrid.resize(kGridSize * kGridSize);
394 mDistortedGrid.resize(kGridSize * kGridSize);
395 }
396
397 float gridMargin = mArrayWidth * kGridMargin;
398 float gridSpacingX = (mArrayWidth + 2 * gridMargin) / kGridSize;
399 float gridSpacingY = (mArrayHeight + 2 * gridMargin) / kGridSize;
400
401 size_t index = 0;
402 float x = -gridMargin;
403 for (size_t i = 0; i < kGridSize; i++, x += gridSpacingX) {
404 float y = -gridMargin;
405 for (size_t j = 0; j < kGridSize; j++, y += gridSpacingY, index++) {
406 mCorrectedGrid[index].src = nullptr;
407 mCorrectedGrid[index].coords = {
408 x, y,
409 x + gridSpacingX, y,
410 x + gridSpacingX, y + gridSpacingY,
411 x, y + gridSpacingY
412 };
413 mDistortedGrid[index].src = &mCorrectedGrid[index];
414 mDistortedGrid[index].coords = mCorrectedGrid[index].coords;
415 status_t res = mapCorrectedToRawImpl(mDistortedGrid[index].coords.data(), 4,
416 /*clamp*/false, /*simple*/false);
417 if (res != OK) return res;
418 }
419 }
420
421 mValidGrids = true;
422 return OK;
423 }
424
findEnclosingQuad(const int32_t pt[2],const std::vector<GridQuad> & grid)425 const DistortionMapper::GridQuad* DistortionMapper::findEnclosingQuad(
426 const int32_t pt[2], const std::vector<GridQuad>& grid) {
427 const float x = pt[0];
428 const float y = pt[1];
429
430 for (const GridQuad& quad : grid) {
431 const float &x1 = quad.coords[0];
432 const float &y1 = quad.coords[1];
433 const float &x2 = quad.coords[2];
434 const float &y2 = quad.coords[3];
435 const float &x3 = quad.coords[4];
436 const float &y3 = quad.coords[5];
437 const float &x4 = quad.coords[6];
438 const float &y4 = quad.coords[7];
439
440 // Point-in-quad test:
441
442 // Quad has corners P1-P4; if P is within the quad, then it is on the same side of all the
443 // edges (or on top of one of the edges or corners), traversed in a consistent direction.
444 // This means that the cross product of edge En = Pn->P(n+1 mod 4) and line Ep = Pn->P must
445 // have the same sign (or be zero) for all edges.
446 // For clockwise traversal, the sign should be negative or zero for Ep x En, indicating that
447 // En is to the left of Ep, or overlapping.
448 float s1 = (x - x1) * (y2 - y1) - (y - y1) * (x2 - x1);
449 if (s1 > 0) continue;
450 float s2 = (x - x2) * (y3 - y2) - (y - y2) * (x3 - x2);
451 if (s2 > 0) continue;
452 float s3 = (x - x3) * (y4 - y3) - (y - y3) * (x4 - x3);
453 if (s3 > 0) continue;
454 float s4 = (x - x4) * (y1 - y4) - (y - y4) * (x1 - x4);
455 if (s4 > 0) continue;
456
457 return &quad;
458 }
459 return nullptr;
460 }
461
calculateUorV(const int32_t pt[2],const GridQuad & quad,bool calculateU)462 float DistortionMapper::calculateUorV(const int32_t pt[2], const GridQuad& quad, bool calculateU) {
463 const float x = pt[0];
464 const float y = pt[1];
465 const float &x1 = quad.coords[0];
466 const float &y1 = quad.coords[1];
467 const float &x2 = calculateU ? quad.coords[2] : quad.coords[6];
468 const float &y2 = calculateU ? quad.coords[3] : quad.coords[7];
469 const float &x3 = quad.coords[4];
470 const float &y3 = quad.coords[5];
471 const float &x4 = calculateU ? quad.coords[6] : quad.coords[2];
472 const float &y4 = calculateU ? quad.coords[7] : quad.coords[3];
473
474 float a = (x1 - x2) * (y1 - y2 + y3 - y4) - (y1 - y2) * (x1 - x2 + x3 - x4);
475 float b = (x - x1) * (y1 - y2 + y3 - y4) + (x1 - x2) * (y4 - y1) -
476 (y - y1) * (x1 - x2 + x3 - x4) - (y1 - y2) * (x4 - x1);
477 float c = (x - x1) * (y4 - y1) - (y - y1) * (x4 - x1);
478
479 if (a == 0) {
480 // One solution may happen if edges are parallel
481 float u0 = -c / b;
482 ALOGV("u0: %.9g, b: %f, c: %f", u0, b, c);
483 return u0;
484 }
485
486 float det = b * b - 4 * a * c;
487 if (det < 0) {
488 // Sanity check - should not happen if pt is within the quad
489 ALOGE("Bad determinant! a: %f, b: %f, c: %f, det: %f", a,b,c,det);
490 return -1;
491 }
492
493 // Select more numerically stable solution
494 float sqdet = b > 0 ? -std::sqrt(det) : std::sqrt(det);
495
496 float u1 = (-b + sqdet) / (2 * a);
497 ALOGV("u1: %.9g", u1);
498 if (0 - kFloatFuzz < u1 && u1 < 1 + kFloatFuzz) return u1;
499
500 float u2 = c / (a * u1);
501 ALOGV("u2: %.9g", u2);
502 if (0 - kFloatFuzz < u2 && u2 < 1 + kFloatFuzz) return u2;
503
504 // Last resort, return the smaller-magnitude solution
505 return fabs(u1) < fabs(u2) ? u1 : u2;
506 }
507
508 } // namespace camera3
509
510 } // namespace android
511