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::kResultPointsToCorrect = {
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 mArrayWidth = array.data.i32[2];
83 mArrayHeight = array.data.i32[3];
84
85 array = deviceInfo.find(ANDROID_SENSOR_INFO_ACTIVE_ARRAY_SIZE);
86 mActiveWidth = array.data.i32[2];
87 mActiveHeight = array.data.i32[3];
88
89 return updateCalibration(deviceInfo);
90 }
91
calibrationValid() const92 bool DistortionMapper::calibrationValid() const {
93 std::lock_guard<std::mutex> lock(mMutex);
94
95 return mValidMapping;
96 }
97
correctCaptureRequest(CameraMetadata * request)98 status_t DistortionMapper::correctCaptureRequest(CameraMetadata *request) {
99 std::lock_guard<std::mutex> lock(mMutex);
100 status_t res;
101
102 if (!mValidMapping) return OK;
103
104 camera_metadata_entry_t e;
105 e = request->find(ANDROID_DISTORTION_CORRECTION_MODE);
106 if (e.count != 0 && e.data.u8[0] != ANDROID_DISTORTION_CORRECTION_MODE_OFF) {
107 for (auto region : kMeteringRegionsToCorrect) {
108 e = request->find(region);
109 for (size_t j = 0; j < e.count; j += 5) {
110 int32_t weight = e.data.i32[j + 4];
111 if (weight == 0) {
112 continue;
113 }
114 res = mapCorrectedToRaw(e.data.i32 + j, 2);
115 if (res != OK) return res;
116 for (size_t k = 0; k < 4; k+=2) {
117 int32_t& x = e.data.i32[j + k];
118 int32_t& y = e.data.i32[j + k + 1];
119 // Clamp to within active array
120 x = std::max(0, x);
121 x = std::min(mActiveWidth - 1, x);
122 y = std::max(0, y);
123 y = std::min(mActiveHeight - 1, y);
124 }
125 }
126 }
127 for (auto rect : kRequestRectsToCorrect) {
128 e = request->find(rect);
129 res = mapCorrectedRectToRaw(e.data.i32, e.count / 4);
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);
160 if (res != OK) return res;
161 for (size_t k = 0; k < 4; k+=2) {
162 int32_t& x = e.data.i32[j + k];
163 int32_t& y = e.data.i32[j + k + 1];
164 // Clamp to within active array
165 x = std::max(0, x);
166 x = std::min(mActiveWidth - 1, x);
167 y = std::max(0, y);
168 y = std::min(mActiveHeight - 1, y);
169 }
170 }
171 }
172 for (auto rect : kResultRectsToCorrect) {
173 e = result->find(rect);
174 res = mapRawRectToCorrected(e.data.i32, e.count / 4);
175 if (res != OK) return res;
176 }
177 for (auto pts : kResultPointsToCorrect) {
178 e = result->find(pts);
179 res = mapRawToCorrected(e.data.i32, e.count / 2);
180 if (res != OK) return res;
181 }
182 }
183
184 return OK;
185 }
186
187 // Utility methods; not guarded by mutex
188
updateCalibration(const CameraMetadata & result)189 status_t DistortionMapper::updateCalibration(const CameraMetadata &result) {
190 camera_metadata_ro_entry_t calib, distortion;
191
192 calib = result.find(ANDROID_LENS_INTRINSIC_CALIBRATION);
193 distortion = result.find(ANDROID_LENS_DISTORTION);
194
195 if (calib.count != 5) return BAD_VALUE;
196 if (distortion.count != 5) return BAD_VALUE;
197
198 // Skip redoing work if no change to calibration fields
199 if (mValidMapping &&
200 mFx == calib.data.f[0] &&
201 mFy == calib.data.f[1] &&
202 mCx == calib.data.f[2] &&
203 mCy == calib.data.f[3] &&
204 mS == calib.data.f[4]) {
205 bool noChange = true;
206 for (size_t i = 0; i < distortion.count; i++) {
207 if (mK[i] != distortion.data.f[i]) {
208 noChange = false;
209 break;
210 }
211 }
212 if (noChange) return OK;
213 }
214
215 mFx = calib.data.f[0];
216 mFy = calib.data.f[1];
217 mCx = calib.data.f[2];
218 mCy = calib.data.f[3];
219 mS = calib.data.f[4];
220
221 mInvFx = 1 / mFx;
222 mInvFy = 1 / mFy;
223
224 for (size_t i = 0; i < distortion.count; i++) {
225 mK[i] = distortion.data.f[i];
226 }
227
228 mValidMapping = true;
229 // Need to recalculate grid
230 mValidGrids = false;
231
232 return OK;
233 }
234
mapRawToCorrected(int32_t * coordPairs,int coordCount)235 status_t DistortionMapper::mapRawToCorrected(int32_t *coordPairs, int coordCount) {
236 if (!mValidMapping) return INVALID_OPERATION;
237
238 if (!mValidGrids) {
239 status_t res = buildGrids();
240 if (res != OK) return res;
241 }
242
243 for (int i = 0; i < coordCount * 2; i += 2) {
244 const GridQuad *quad = findEnclosingQuad(coordPairs + i, mDistortedGrid);
245 if (quad == nullptr) {
246 ALOGE("Raw to corrected mapping failure: No quad found for (%d, %d)",
247 *(coordPairs + i), *(coordPairs + i + 1));
248 return INVALID_OPERATION;
249 }
250 ALOGV("src xy: %d, %d, enclosing quad: (%f, %f), (%f, %f), (%f, %f), (%f, %f)",
251 coordPairs[i], coordPairs[i+1],
252 quad->coords[0], quad->coords[1],
253 quad->coords[2], quad->coords[3],
254 quad->coords[4], quad->coords[5],
255 quad->coords[6], quad->coords[7]);
256
257 const GridQuad *corrQuad = quad->src;
258 if (corrQuad == nullptr) {
259 ALOGE("Raw to corrected mapping failure: No src quad found");
260 return INVALID_OPERATION;
261 }
262 ALOGV(" corr quad: (%f, %f), (%f, %f), (%f, %f), (%f, %f)",
263 corrQuad->coords[0], corrQuad->coords[1],
264 corrQuad->coords[2], corrQuad->coords[3],
265 corrQuad->coords[4], corrQuad->coords[5],
266 corrQuad->coords[6], corrQuad->coords[7]);
267
268 float u = calculateUorV(coordPairs + i, *quad, /*calculateU*/ true);
269 float v = calculateUorV(coordPairs + i, *quad, /*calculateU*/ false);
270
271 ALOGV("uv: %f, %f", u, v);
272
273 // Interpolate along top edge of corrected quad (which are axis-aligned) for x
274 float corrX = corrQuad->coords[0] + u * (corrQuad->coords[2] - corrQuad->coords[0]);
275 // Interpolate along left edge of corrected quad (which are axis-aligned) for y
276 float corrY = corrQuad->coords[1] + v * (corrQuad->coords[7] - corrQuad->coords[1]);
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
mapRawRectToCorrected(int32_t * rects,int rectCount)285 status_t DistortionMapper::mapRawRectToCorrected(int32_t *rects, int rectCount) {
286 if (!mValidMapping) return INVALID_OPERATION;
287 for (int i = 0; i < rectCount * 4; i += 4) {
288 // Map from (l, t, width, height) to (l, t, r, b)
289 int32_t coords[4] = {
290 rects[i],
291 rects[i + 1],
292 rects[i] + rects[i + 2],
293 rects[i + 1] + rects[i + 3]
294 };
295
296 mapRawToCorrected(coords, 2);
297
298 // Map back to (l, t, width, height)
299 rects[i] = coords[0];
300 rects[i + 1] = coords[1];
301 rects[i + 2] = coords[2] - coords[0];
302 rects[i + 3] = coords[3] - coords[1];
303 }
304
305 return OK;
306 }
307
308 template<typename T>
mapCorrectedToRaw(T * coordPairs,int coordCount) const309 status_t DistortionMapper::mapCorrectedToRaw(T *coordPairs, int coordCount) const {
310 if (!mValidMapping) return INVALID_OPERATION;
311
312 for (int i = 0; i < coordCount * 2; i += 2) {
313 // Move to normalized space
314 float ywi = (coordPairs[i + 1] - mCy) * mInvFy;
315 float xwi = (coordPairs[i] - mCx - mS * ywi) * mInvFx;
316 // Apply distortion model to calculate raw image coordinates
317 float rSq = xwi * xwi + ywi * ywi;
318 float Fr = 1.f + (mK[0] * rSq) + (mK[1] * rSq * rSq) + (mK[2] * rSq * rSq * rSq);
319 float xc = xwi * Fr + (mK[3] * 2 * xwi * ywi) + mK[4] * (rSq + 2 * xwi * xwi);
320 float yc = ywi * Fr + (mK[4] * 2 * xwi * ywi) + mK[3] * (rSq + 2 * ywi * ywi);
321 // Move back to image space
322 float xr = mFx * xc + mS * yc + mCx;
323 float yr = mFy * yc + mCy;
324
325 coordPairs[i] = static_cast<T>(std::round(xr));
326 coordPairs[i + 1] = static_cast<T>(std::round(yr));
327 }
328
329 return OK;
330 }
331
332 template status_t DistortionMapper::mapCorrectedToRaw(int32_t*, int) const;
333 template status_t DistortionMapper::mapCorrectedToRaw(float*, int) const;
334
mapCorrectedRectToRaw(int32_t * rects,int rectCount) const335 status_t DistortionMapper::mapCorrectedRectToRaw(int32_t *rects, int rectCount) const {
336 if (!mValidMapping) return INVALID_OPERATION;
337
338 for (int i = 0; i < rectCount * 4; i += 4) {
339 // Map from (l, t, width, height) to (l, t, r, b)
340 int32_t coords[4] = {
341 rects[i],
342 rects[i + 1],
343 rects[i] + rects[i + 2],
344 rects[i + 1] + rects[i + 3]
345 };
346
347 mapCorrectedToRaw(coords, 2);
348
349 // Map back to (l, t, width, height)
350 rects[i] = coords[0];
351 rects[i + 1] = coords[1];
352 rects[i + 2] = coords[2] - coords[0];
353 rects[i + 3] = coords[3] - coords[1];
354 }
355
356 return OK;
357 }
358
buildGrids()359 status_t DistortionMapper::buildGrids() {
360 if (mCorrectedGrid.size() != kGridSize * kGridSize) {
361 mCorrectedGrid.resize(kGridSize * kGridSize);
362 mDistortedGrid.resize(kGridSize * kGridSize);
363 }
364
365 float gridMargin = mArrayWidth * kGridMargin;
366 float gridSpacingX = (mArrayWidth + 2 * gridMargin) / kGridSize;
367 float gridSpacingY = (mArrayHeight + 2 * gridMargin) / kGridSize;
368
369 size_t index = 0;
370 float x = -gridMargin;
371 for (size_t i = 0; i < kGridSize; i++, x += gridSpacingX) {
372 float y = -gridMargin;
373 for (size_t j = 0; j < kGridSize; j++, y += gridSpacingY, index++) {
374 mCorrectedGrid[index].src = nullptr;
375 mCorrectedGrid[index].coords = {
376 x, y,
377 x + gridSpacingX, y,
378 x + gridSpacingX, y + gridSpacingY,
379 x, y + gridSpacingY
380 };
381 mDistortedGrid[index].src = &mCorrectedGrid[index];
382 mDistortedGrid[index].coords = mCorrectedGrid[index].coords;
383 status_t res = mapCorrectedToRaw(mDistortedGrid[index].coords.data(), 4);
384 if (res != OK) return res;
385 }
386 }
387
388 mValidGrids = true;
389 return OK;
390 }
391
findEnclosingQuad(const int32_t pt[2],const std::vector<GridQuad> & grid)392 const DistortionMapper::GridQuad* DistortionMapper::findEnclosingQuad(
393 const int32_t pt[2], const std::vector<GridQuad>& grid) {
394 const float x = pt[0];
395 const float y = pt[1];
396
397 for (const GridQuad& quad : grid) {
398 const float &x1 = quad.coords[0];
399 const float &y1 = quad.coords[1];
400 const float &x2 = quad.coords[2];
401 const float &y2 = quad.coords[3];
402 const float &x3 = quad.coords[4];
403 const float &y3 = quad.coords[5];
404 const float &x4 = quad.coords[6];
405 const float &y4 = quad.coords[7];
406
407 // Point-in-quad test:
408
409 // Quad has corners P1-P4; if P is within the quad, then it is on the same side of all the
410 // edges (or on top of one of the edges or corners), traversed in a consistent direction.
411 // This means that the cross product of edge En = Pn->P(n+1 mod 4) and line Ep = Pn->P must
412 // have the same sign (or be zero) for all edges.
413 // For clockwise traversal, the sign should be negative or zero for Ep x En, indicating that
414 // En is to the left of Ep, or overlapping.
415 float s1 = (x - x1) * (y2 - y1) - (y - y1) * (x2 - x1);
416 if (s1 > 0) continue;
417 float s2 = (x - x2) * (y3 - y2) - (y - y2) * (x3 - x2);
418 if (s2 > 0) continue;
419 float s3 = (x - x3) * (y4 - y3) - (y - y3) * (x4 - x3);
420 if (s3 > 0) continue;
421 float s4 = (x - x4) * (y1 - y4) - (y - y4) * (x1 - x4);
422 if (s4 > 0) continue;
423
424 return &quad;
425 }
426 return nullptr;
427 }
428
calculateUorV(const int32_t pt[2],const GridQuad & quad,bool calculateU)429 float DistortionMapper::calculateUorV(const int32_t pt[2], const GridQuad& quad, bool calculateU) {
430 const float x = pt[0];
431 const float y = pt[1];
432 const float &x1 = quad.coords[0];
433 const float &y1 = quad.coords[1];
434 const float &x2 = calculateU ? quad.coords[2] : quad.coords[6];
435 const float &y2 = calculateU ? quad.coords[3] : quad.coords[7];
436 const float &x3 = quad.coords[4];
437 const float &y3 = quad.coords[5];
438 const float &x4 = calculateU ? quad.coords[6] : quad.coords[2];
439 const float &y4 = calculateU ? quad.coords[7] : quad.coords[3];
440
441 float a = (x1 - x2) * (y1 - y2 + y3 - y4) - (y1 - y2) * (x1 - x2 + x3 - x4);
442 float b = (x - x1) * (y1 - y2 + y3 - y4) + (x1 - x2) * (y4 - y1) -
443 (y - y1) * (x1 - x2 + x3 - x4) - (y1 - y2) * (x4 - x1);
444 float c = (x - x1) * (y4 - y1) - (y - y1) * (x4 - x1);
445
446 if (a == 0) {
447 // One solution may happen if edges are parallel
448 float u0 = -c / b;
449 ALOGV("u0: %.9g, b: %f, c: %f", u0, b, c);
450 return u0;
451 }
452
453 float det = b * b - 4 * a * c;
454 if (det < 0) {
455 // Sanity check - should not happen if pt is within the quad
456 ALOGE("Bad determinant! a: %f, b: %f, c: %f, det: %f", a,b,c,det);
457 return -1;
458 }
459
460 // Select more numerically stable solution
461 float sqdet = b > 0 ? -std::sqrt(det) : std::sqrt(det);
462
463 float u1 = (-b + sqdet) / (2 * a);
464 ALOGV("u1: %.9g", u1);
465 if (0 - kFloatFuzz < u1 && u1 < 1 + kFloatFuzz) return u1;
466
467 float u2 = c / (a * u1);
468 ALOGV("u2: %.9g", u2);
469 if (0 - kFloatFuzz < u2 && u2 < 1 + kFloatFuzz) return u2;
470
471 // Last resort, return the smaller-magnitude solution
472 return fabs(u1) < fabs(u2) ? u1 : u2;
473 }
474
475 } // namespace camera3
476
477 } // namespace android
478