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
26namespace android {
27
28namespace camera3 {
29
30/**
31 * Metadata keys to correct when adjusting coordinates for distortion correction
32 */
33
34// Both capture request and result
35constexpr 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
42constexpr std::array<uint32_t, 1> DistortionMapper::kRequestRectsToCorrect = {
43    ANDROID_SCALER_CROP_REGION,
44};
45
46// Only for capture result
47constexpr std::array<uint32_t, 1> DistortionMapper::kResultRectsToCorrect = {
48    ANDROID_SCALER_CROP_REGION,
49};
50
51// Only for capture result
52constexpr 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
58DistortionMapper::DistortionMapper() : mValidMapping(false), mValidGrids(false) {
59}
60
61bool 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
75status_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
92bool DistortionMapper::calibrationValid() const {
93    std::lock_guard<std::mutex> lock(mMutex);
94
95    return mValidMapping;
96}
97
98status_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
137status_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
189status_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
235status_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
285status_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
308template<typename T>
309status_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
332template status_t DistortionMapper::mapCorrectedToRaw(int32_t*, int) const;
333template status_t DistortionMapper::mapCorrectedToRaw(float*, int) const;
334
335status_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
359status_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
392const 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
429float 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