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