From 7b8a1fd27d12d1b3ea711b0edca6ff5b07f5beb1 Mon Sep 17 00:00:00 2001 From: Eino-Ville Talvala Date: Tue, 22 May 2018 15:30:35 -0700 Subject: [PATCH] Camera: Enable distortion correction for API1, map metadata when enabled - API1 + HAL3: Enable HIGH_QUALITY correction for still capture use cases, FAST for others - HAL3: When distortion correction is enabled, map coordinate metadata from corrected to original in capture requests, and from original to corrected in capture results. Test: Camera CTS Bug: 79885994 Change-Id: I79e25d278fe69099770c749f42956fc8e878f7cf --- services/camera/libcameraservice/Android.mk | 1 + .../api1/client2/FrameProcessor.cpp | 1 - .../api1/client2/Parameters.cpp | 24 +- .../api1/client2/Parameters.h | 2 + .../device3/Camera3Device.cpp | 33 +- .../libcameraservice/device3/Camera3Device.h | 7 + .../device3/DistortionMapper.cpp | 444 ++++++++++++++++++ .../device3/DistortionMapper.h | 182 +++++++ 8 files changed, 690 insertions(+), 4 deletions(-) create mode 100644 services/camera/libcameraservice/device3/DistortionMapper.cpp create mode 100644 services/camera/libcameraservice/device3/DistortionMapper.h diff --git a/services/camera/libcameraservice/Android.mk b/services/camera/libcameraservice/Android.mk index 7b86180bce..5697a8739a 100644 --- a/services/camera/libcameraservice/Android.mk +++ b/services/camera/libcameraservice/Android.mk @@ -51,6 +51,7 @@ LOCAL_SRC_FILES := \ device3/StatusTracker.cpp \ device3/Camera3BufferManager.cpp \ device3/Camera3StreamSplitter.cpp \ + device3/DistortionMapper.cpp \ gui/RingBufferConsumer.cpp \ utils/CameraTraces.cpp \ utils/AutoConditionLock.cpp \ diff --git a/services/camera/libcameraservice/api1/client2/FrameProcessor.cpp b/services/camera/libcameraservice/api1/client2/FrameProcessor.cpp index 187bea9894..0c738e7324 100644 --- a/services/camera/libcameraservice/api1/client2/FrameProcessor.cpp +++ b/services/camera/libcameraservice/api1/client2/FrameProcessor.cpp @@ -197,7 +197,6 @@ status_t FrameProcessor::processFaceDetect(const CameraMetadata &frame, faceRects[i*4 + 2], scalerCrop); face.rect[3] = l.mParameters.arrayYToNormalizedWithCrop( faceRects[i*4 + 3], scalerCrop); - face.score = faceScores[i]; if (faceDetectMode == ANDROID_STATISTICS_FACE_DETECT_MODE_FULL) { face.id = faceIds[i]; diff --git a/services/camera/libcameraservice/api1/client2/Parameters.cpp b/services/camera/libcameraservice/api1/client2/Parameters.cpp index 4a58620615..d66dec49e8 100644 --- a/services/camera/libcameraservice/api1/client2/Parameters.cpp +++ b/services/camera/libcameraservice/api1/client2/Parameters.cpp @@ -954,13 +954,24 @@ status_t Parameters::initialize(const CameraMetadata *info, int deviceVersion) { const uint8_t *caps = availableCapabilities.data.u8; for (size_t i = 0; i < availableCapabilities.count; i++) { if (ANDROID_REQUEST_AVAILABLE_CAPABILITIES_PRIVATE_REPROCESSING == - caps[i]) { + caps[i]) { isZslReprocessPresent = true; break; } } } + isDistortionCorrectionSupported = false; + camera_metadata_ro_entry_t distortionCorrectionModes = + staticInfo(ANDROID_DISTORTION_CORRECTION_AVAILABLE_MODES); + for (size_t i = 0; i < distortionCorrectionModes.count; i++) { + if (distortionCorrectionModes.data.u8[i] != + ANDROID_DISTORTION_CORRECTION_MODE_OFF) { + isDistortionCorrectionSupported = true; + break; + } + } + if (isDeviceZslSupported || slowJpegMode || property_get_bool("camera.disable_zsl_mode", false)) { ALOGI("Camera %d: Disabling ZSL mode", cameraId); @@ -1222,7 +1233,7 @@ status_t Parameters::buildFastInfo() { const uint8_t *caps = availableCapabilities.data.u8; for (size_t i = 0; i < availableCapabilities.count; i++) { if (ANDROID_REQUEST_AVAILABLE_CAPABILITIES_PRIVATE_REPROCESSING == - caps[i]) { + caps[i]) { isZslReprocessPresent = true; break; } @@ -2097,15 +2108,24 @@ status_t Parameters::updateRequest(CameraMetadata *request) const { if (intent.count == 0) return BAD_VALUE; + uint8_t distortionMode = ANDROID_DISTORTION_CORRECTION_MODE_OFF; if (intent.data.u8[0] == ANDROID_CONTROL_CAPTURE_INTENT_STILL_CAPTURE) { res = request->update(ANDROID_CONTROL_AE_TARGET_FPS_RANGE, fastInfo.bestStillCaptureFpsRange, 2); + distortionMode = ANDROID_DISTORTION_CORRECTION_MODE_HIGH_QUALITY; } else { res = request->update(ANDROID_CONTROL_AE_TARGET_FPS_RANGE, previewFpsRange, 2); + distortionMode = ANDROID_DISTORTION_CORRECTION_MODE_FAST; } if (res != OK) return res; + if (isDistortionCorrectionSupported) { + res = request->update(ANDROID_DISTORTION_CORRECTION_MODE, + &distortionMode, 1); + if (res != OK) return res; + } + if (autoWhiteBalanceLockAvailable) { uint8_t reqWbLock = autoWhiteBalanceLock ? ANDROID_CONTROL_AWB_LOCK_ON : ANDROID_CONTROL_AWB_LOCK_OFF; diff --git a/services/camera/libcameraservice/api1/client2/Parameters.h b/services/camera/libcameraservice/api1/client2/Parameters.h index e35b7a4f85..97f8ea7f37 100644 --- a/services/camera/libcameraservice/api1/client2/Parameters.h +++ b/services/camera/libcameraservice/api1/client2/Parameters.h @@ -177,6 +177,8 @@ struct Parameters { bool isZslReprocessPresent; // Whether the device supports enableZsl. bool isDeviceZslSupported; + // Whether the device supports geometric distortion correction + bool isDistortionCorrectionSupported; // Overall camera state enum State { diff --git a/services/camera/libcameraservice/device3/Camera3Device.cpp b/services/camera/libcameraservice/device3/Camera3Device.cpp index d9bcba36bb..543914e5a4 100644 --- a/services/camera/libcameraservice/device3/Camera3Device.cpp +++ b/services/camera/libcameraservice/device3/Camera3Device.cpp @@ -249,6 +249,14 @@ status_t Camera3Device::initializeCommonLocked() { } } + if (DistortionMapper::isDistortionSupported(mDeviceInfo)) { + res = mDistortionMapper.setupStaticInfo(mDeviceInfo); + if (res != OK) { + SET_ERR_L("Unable to read necessary calibration fields for distortion correction"); + return res; + } + } + return OK; } @@ -2983,6 +2991,14 @@ void Camera3Device::sendCaptureResult(CameraMetadata &pendingMetadata, } } + // Fix up some result metadata to account for HAL-level distortion correction + status_t res = mDistortionMapper.correctCaptureResult(&captureResult.mMetadata); + if (res != OK) { + SET_ERR("Unable to correct capture result metadata for frame %d: %s (%d)", + frameNumber, strerror(res), res); + return; + } + mTagMonitor.monitorMetadata(TagMonitor::RESULT, frameNumber, timestamp.data.i64[0], captureResult.mMetadata); @@ -4705,13 +4721,13 @@ status_t Camera3Device::RequestThread::prepareHalRequests() { // Insert any queued triggers (before metadata is locked) status_t res = insertTriggers(captureRequest); - if (res < 0) { SET_ERR("RequestThread: Unable to insert triggers " "(capture request %d, HAL device: %s (%d)", halRequest->frame_number, strerror(-res), res); return INVALID_OPERATION; } + int triggerCount = res; bool triggersMixedIn = (triggerCount > 0 || mPrevTriggers > 0); mPrevTriggers = triggerCount; @@ -4731,6 +4747,21 @@ status_t Camera3Device::RequestThread::prepareHalRequests() { return INVALID_OPERATION; } + { + // Correct metadata regions for distortion correction if enabled + sp parent = mParent.promote(); + if (parent != nullptr) { + res = parent->mDistortionMapper.correctCaptureRequest( + &(captureRequest->mSettingsList.begin()->metadata)); + if (res != OK) { + SET_ERR("RequestThread: Unable to correct capture requests " + "for lens distortion for request %d: %s (%d)", + halRequest->frame_number, strerror(-res), res); + return INVALID_OPERATION; + } + } + } + /** * The request should be presorted so accesses in HAL * are O(logn). Sidenote, sorting a sorted metadata is nop. diff --git a/services/camera/libcameraservice/device3/Camera3Device.h b/services/camera/libcameraservice/device3/Camera3Device.h index 35f799d5dd..d8fe19fa66 100644 --- a/services/camera/libcameraservice/device3/Camera3Device.h +++ b/services/camera/libcameraservice/device3/Camera3Device.h @@ -43,6 +43,7 @@ #include "common/CameraDeviceBase.h" #include "device3/StatusTracker.h" #include "device3/Camera3BufferManager.h" +#include "device3/DistortionMapper.h" #include "utils/TagMonitor.h" #include "utils/LatencyHistogram.h" #include @@ -1179,6 +1180,12 @@ class Camera3Device : /**** End scope for mInFlightLock ****/ + /** + * Distortion correction support + */ + + camera3::DistortionMapper mDistortionMapper; + // Debug tracker for metadata tag value changes // - Enabled with the -m option to dumpsys, such as // dumpsys -m android.control.aeState,android.control.aeMode diff --git a/services/camera/libcameraservice/device3/DistortionMapper.cpp b/services/camera/libcameraservice/device3/DistortionMapper.cpp new file mode 100644 index 0000000000..82923bd307 --- /dev/null +++ b/services/camera/libcameraservice/device3/DistortionMapper.cpp @@ -0,0 +1,444 @@ +/* + * Copyright (C) 2018 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#define LOG_TAG "Camera3-DistMapper" +#define ATRACE_TAG ATRACE_TAG_CAMERA +//#define LOG_NDEBUG 0 + +#include + +#include "device3/DistortionMapper.h" + +namespace android { + +namespace camera3 { + +/** + * Metadata keys to correct when adjusting coordinates for distortion correction + */ + +// Both capture request and result +constexpr std::array DistortionMapper::kMeteringRegionsToCorrect = { + ANDROID_CONTROL_AF_REGIONS, + ANDROID_CONTROL_AE_REGIONS, + ANDROID_CONTROL_AWB_REGIONS +}; + +// Only capture request +constexpr std::array DistortionMapper::kRequestRectsToCorrect = { + ANDROID_SCALER_CROP_REGION, +}; + +// Only for capture result +constexpr std::array DistortionMapper::kResultRectsToCorrect = { + ANDROID_SCALER_CROP_REGION, + ANDROID_STATISTICS_FACE_RECTANGLES +}; + +// Only for capture result +constexpr std::array DistortionMapper::kResultPointsToCorrect = { + ANDROID_STATISTICS_FACE_LANDMARKS, +}; + + +DistortionMapper::DistortionMapper() : mValidMapping(false), mValidGrids(false) { +} + +bool DistortionMapper::isDistortionSupported(const CameraMetadata &result) { + bool isDistortionCorrectionSupported = false; + camera_metadata_ro_entry_t distortionCorrectionModes = + result.find(ANDROID_DISTORTION_CORRECTION_AVAILABLE_MODES); + for (size_t i = 0; i < distortionCorrectionModes.count; i++) { + if (distortionCorrectionModes.data.u8[i] != + ANDROID_DISTORTION_CORRECTION_MODE_OFF) { + isDistortionCorrectionSupported = true; + break; + } + } + return isDistortionCorrectionSupported; +} + +status_t DistortionMapper::setupStaticInfo(const CameraMetadata &deviceInfo) { + std::lock_guard lock(mMutex); + camera_metadata_ro_entry_t array; + + array = deviceInfo.find(ANDROID_SENSOR_INFO_PRE_CORRECTION_ACTIVE_ARRAY_SIZE); + if (array.count != 4) return BAD_VALUE; + + mArrayWidth = array.data.i32[2]; + mArrayHeight = array.data.i32[3]; + + return updateCalibration(deviceInfo); +} + +bool DistortionMapper::calibrationValid() const { + std::lock_guard lock(mMutex); + + return mValidMapping; +} + +status_t DistortionMapper::correctCaptureRequest(CameraMetadata *request) { + std::lock_guard lock(mMutex); + status_t res; + + if (!mValidMapping) return OK; + + camera_metadata_entry_t e; + e = request->find(ANDROID_DISTORTION_CORRECTION_MODE); + if (e.count != 0 && e.data.u8[0] != ANDROID_DISTORTION_CORRECTION_MODE_OFF) { + for (auto region : kMeteringRegionsToCorrect) { + e = request->find(region); + for (size_t j = 0; j < e.count; j += 5) { + res = mapCorrectedToRaw(e.data.i32 + j, 2); + if (res != OK) return res; + } + } + for (auto rect : kRequestRectsToCorrect) { + e = request->find(rect); + res = mapCorrectedRectToRaw(e.data.i32, e.count / 4); + if (res != OK) return res; + } + } + + return OK; +} + +status_t DistortionMapper::correctCaptureResult(CameraMetadata *result) { + std::lock_guard lock(mMutex); + status_t res; + + if (!mValidMapping) return OK; + + res = updateCalibration(*result); + if (res != OK) { + ALOGE("Failure to update lens calibration information"); + return INVALID_OPERATION; + } + + camera_metadata_entry_t e; + e = result->find(ANDROID_DISTORTION_CORRECTION_MODE); + if (e.count != 0 && e.data.u8[0] != ANDROID_DISTORTION_CORRECTION_MODE_OFF) { + for (auto region : kMeteringRegionsToCorrect) { + e = result->find(region); + for (size_t j = 0; j < e.count; j += 5) { + res = mapRawToCorrected(e.data.i32 + j, 2); + if (res != OK) return res; + } + } + for (auto rect : kResultRectsToCorrect) { + e = result->find(rect); + res = mapRawRectToCorrected(e.data.i32, e.count / 4); + if (res != OK) return res; + } + for (auto pts : kResultPointsToCorrect) { + e = result->find(pts); + res = mapRawToCorrected(e.data.i32, e.count / 2); + if (res != OK) return res; + } + } + + return OK; +} + +// Utility methods; not guarded by mutex + +status_t DistortionMapper::updateCalibration(const CameraMetadata &result) { + camera_metadata_ro_entry_t calib, distortion; + + calib = result.find(ANDROID_LENS_INTRINSIC_CALIBRATION); + distortion = result.find(ANDROID_LENS_DISTORTION); + + if (calib.count != 5) return BAD_VALUE; + if (distortion.count != 5) return BAD_VALUE; + + // Skip redoing work if no change to calibration fields + if (mValidMapping && + mFx == calib.data.f[0] && + mFy == calib.data.f[1] && + mCx == calib.data.f[2] && + mCy == calib.data.f[3] && + mS == calib.data.f[4]) { + bool noChange = true; + for (size_t i = 0; i < distortion.count; i++) { + if (mK[i] != distortion.data.f[i]) { + noChange = false; + break; + } + } + if (noChange) return OK; + } + + mFx = calib.data.f[0]; + mFy = calib.data.f[1]; + mCx = calib.data.f[2]; + mCy = calib.data.f[3]; + mS = calib.data.f[4]; + + mInvFx = 1 / mFx; + mInvFy = 1 / mFy; + + for (size_t i = 0; i < distortion.count; i++) { + mK[i] = distortion.data.f[i]; + } + + mValidMapping = true; + mValidGrids = false; + + return OK; +} + +status_t DistortionMapper::mapRawToCorrected(int32_t *coordPairs, int coordCount) { + if (!mValidMapping) return INVALID_OPERATION; + + if (!mValidGrids) { + status_t res = buildGrids(); + if (res != OK) return res; + } + + for (int i = 0; i < coordCount * 2; i += 2) { + const GridQuad *quad = findEnclosingQuad(coordPairs + i, mDistortedGrid); + if (quad == nullptr) { + ALOGE("Raw to corrected mapping failure: No quad found"); + return INVALID_OPERATION; + } + ALOGV("src xy: %d, %d, enclosing quad: (%f, %f), (%f, %f), (%f, %f), (%f, %f)", + coordPairs[i], coordPairs[i+1], + quad->coords[0], quad->coords[1], + quad->coords[2], quad->coords[3], + quad->coords[4], quad->coords[5], + quad->coords[6], quad->coords[7]); + + const GridQuad *corrQuad = quad->src; + if (corrQuad == nullptr) { + ALOGE("Raw to corrected mapping failure: No src quad found"); + return INVALID_OPERATION; + } + ALOGV(" corr quad: (%f, %f), (%f, %f), (%f, %f), (%f, %f)", + corrQuad->coords[0], corrQuad->coords[1], + corrQuad->coords[2], corrQuad->coords[3], + corrQuad->coords[4], corrQuad->coords[5], + corrQuad->coords[6], corrQuad->coords[7]); + + float u = calculateUorV(coordPairs + i, *quad, /*calculateU*/ true); + float v = calculateUorV(coordPairs + i, *quad, /*calculateU*/ false); + + ALOGV("uv: %f, %f", u, v); + + // Interpolate along top edge of corrected quad (which are axis-aligned) for x + float corrX = corrQuad->coords[0] + u * (corrQuad->coords[2] - corrQuad->coords[0]); + // Interpolate along left edge of corrected quad (which are axis-aligned) for y + float corrY = corrQuad->coords[1] + v * (corrQuad->coords[7] - corrQuad->coords[1]); + + coordPairs[i] = static_cast(std::round(corrX)); + coordPairs[i + 1] = static_cast(std::round(corrY)); + } + + return OK; +} + +status_t DistortionMapper::mapRawRectToCorrected(int32_t *rects, int rectCount) { + if (!mValidMapping) return INVALID_OPERATION; + for (int i = 0; i < rectCount * 4; i += 4) { + // Map from (l, t, width, height) to (l, t, r, b) + int32_t coords[4] = { + rects[i], + rects[i + 1], + rects[i] + rects[i + 2], + rects[i + 1] + rects[i + 3] + }; + + mapRawToCorrected(coords, 2); + + // Map back to (l, t, width, height) + rects[i] = coords[0]; + rects[i + 1] = coords[1]; + rects[i + 2] = coords[2] - coords[0]; + rects[i + 3] = coords[3] - coords[1]; + } + + return OK; +} + +template +status_t DistortionMapper::mapCorrectedToRaw(T *coordPairs, int coordCount) const { + if (!mValidMapping) return INVALID_OPERATION; + + for (int i = 0; i < coordCount * 2; i += 2) { + // Move to normalized space + float ywi = (coordPairs[i + 1] - mCy) * mInvFy; + float xwi = (coordPairs[i] - mCx - mS * ywi) * mInvFx; + // Apply distortion model to calculate raw image coordinates + float rSq = xwi * xwi + ywi * ywi; + float Fr = 1.f + (mK[0] * rSq) + (mK[1] * rSq * rSq) + (mK[2] * rSq * rSq * rSq); + float xc = xwi * Fr + (mK[3] * 2 * xwi * ywi) + mK[4] * (rSq + 2 * xwi * xwi); + float yc = ywi * Fr + (mK[4] * 2 * xwi * ywi) + mK[3] * (rSq + 2 * ywi * ywi); + // Move back to image space + float xr = mFx * xc + mS * yc + mCx; + float yr = mFy * yc + mCy; + + coordPairs[i] = static_cast(std::round(xr)); + coordPairs[i + 1] = static_cast(std::round(yr)); + } + + return OK; +} + +template status_t DistortionMapper::mapCorrectedToRaw(int32_t*, int) const; +template status_t DistortionMapper::mapCorrectedToRaw(float*, int) const; + +status_t DistortionMapper::mapCorrectedRectToRaw(int32_t *rects, int rectCount) const { + if (!mValidMapping) return INVALID_OPERATION; + + for (int i = 0; i < rectCount * 4; i += 4) { + // Map from (l, t, width, height) to (l, t, r, b) + int32_t coords[4] = { + rects[i], + rects[i + 1], + rects[i] + rects[i + 2], + rects[i + 1] + rects[i + 3] + }; + + mapCorrectedToRaw(coords, 2); + + // Map back to (l, t, width, height) + rects[i] = coords[0]; + rects[i + 1] = coords[1]; + rects[i + 2] = coords[2] - coords[0]; + rects[i + 3] = coords[3] - coords[1]; + } + + return OK; +} + +status_t DistortionMapper::buildGrids() { + if (mCorrectedGrid.size() != kGridSize * kGridSize) { + mCorrectedGrid.resize(kGridSize * kGridSize); + mDistortedGrid.resize(kGridSize * kGridSize); + } + + float gridMargin = mArrayWidth * kGridMargin; + float gridSpacingX = (mArrayWidth + 2 * gridMargin) / kGridSize; + float gridSpacingY = (mArrayHeight + 2 * gridMargin) / kGridSize; + + size_t index = 0; + float x = -gridMargin; + for (size_t i = 0; i < kGridSize; i++, x += gridSpacingX) { + float y = -gridMargin; + for (size_t j = 0; j < kGridSize; j++, y += gridSpacingY, index++) { + mCorrectedGrid[index].src = nullptr; + mCorrectedGrid[index].coords = { + x, y, + x + gridSpacingX, y, + x + gridSpacingX, y + gridSpacingY, + x, y + gridSpacingY + }; + mDistortedGrid[index].src = &mCorrectedGrid[index]; + mDistortedGrid[index].coords = mCorrectedGrid[index].coords; + status_t res = mapCorrectedToRaw(mDistortedGrid[index].coords.data(), 4); + if (res != OK) return res; + } + } + + mValidGrids = true; + return OK; +} + +const DistortionMapper::GridQuad* DistortionMapper::findEnclosingQuad( + const int32_t pt[2], const std::vector& grid) { + const float x = pt[0]; + const float y = pt[1]; + + for (const GridQuad& quad : grid) { + const float &x1 = quad.coords[0]; + const float &y1 = quad.coords[1]; + const float &x2 = quad.coords[2]; + const float &y2 = quad.coords[3]; + const float &x3 = quad.coords[4]; + const float &y3 = quad.coords[5]; + const float &x4 = quad.coords[6]; + const float &y4 = quad.coords[7]; + + // Point-in-quad test: + + // Quad has corners P1-P4; if P is within the quad, then it is on the same side of all the + // edges (or on top of one of the edges or corners), traversed in a consistent direction. + // This means that the cross product of edge En = Pn->P(n+1 mod 4) and line Ep = Pn->P must + // have the same sign (or be zero) for all edges. + // For clockwise traversal, the sign should be negative or zero for Ep x En, indicating that + // En is to the left of Ep, or overlapping. + float s1 = (x - x1) * (y2 - y1) - (y - y1) * (x2 - x1); + if (s1 > 0) continue; + float s2 = (x - x2) * (y3 - y2) - (y - y2) * (x3 - x2); + if (s2 > 0) continue; + float s3 = (x - x3) * (y4 - y3) - (y - y3) * (x4 - x3); + if (s3 > 0) continue; + float s4 = (x - x4) * (y1 - y4) - (y - y4) * (x1 - x4); + if (s4 > 0) continue; + + return &quad; + } + return nullptr; +} + +float DistortionMapper::calculateUorV(const int32_t pt[2], const GridQuad& quad, bool calculateU) { + const float x = pt[0]; + const float y = pt[1]; + const float &x1 = quad.coords[0]; + const float &y1 = quad.coords[1]; + const float &x2 = calculateU ? quad.coords[2] : quad.coords[6]; + const float &y2 = calculateU ? quad.coords[3] : quad.coords[7]; + const float &x3 = quad.coords[4]; + const float &y3 = quad.coords[5]; + const float &x4 = calculateU ? quad.coords[6] : quad.coords[2]; + const float &y4 = calculateU ? quad.coords[7] : quad.coords[3]; + + float a = (x1 - x2) * (y1 - y2 + y3 - y4) - (y1 - y2) * (x1 - x2 + x3 - x4); + float b = (x - x1) * (y1 - y2 + y3 - y4) + (x1 - x2) * (y4 - y1) - + (y - y1) * (x1 - x2 + x3 - x4) - (y1 - y2) * (x4 - x1); + float c = (x - x1) * (y4 - y1) - (y - y1) * (x4 - x1); + + if (a == 0) { + // One solution may happen if edges are parallel + float u0 = -c / b; + ALOGV("u0: %.9g, b: %f, c: %f", u0, b, c); + return u0; + } + + float det = b * b - 4 * a * c; + if (det < 0) { + // Sanity check - should not happen if pt is within the quad + ALOGE("Bad determinant! a: %f, b: %f, c: %f, det: %f", a,b,c,det); + return -1; + } + + // Select more numerically stable solution + float sqdet = b > 0 ? -std::sqrt(det) : std::sqrt(det); + + float u1 = (-b + sqdet) / (2 * a); + ALOGV("u1: %.9g", u1); + if (0 - kFloatFuzz < u1 && u1 < 1 + kFloatFuzz) return u1; + + float u2 = c / (a * u1); + ALOGV("u2: %.9g", u2); + if (0 - kFloatFuzz < u2 && u2 < 1 + kFloatFuzz) return u2; + + // Last resort, return the smaller-magnitude solution + return fabs(u1) < fabs(u2) ? u1 : u2; +} + +} // namespace camera3 + +} // namespace android diff --git a/services/camera/libcameraservice/device3/DistortionMapper.h b/services/camera/libcameraservice/device3/DistortionMapper.h new file mode 100644 index 0000000000..c6d715b5ae --- /dev/null +++ b/services/camera/libcameraservice/device3/DistortionMapper.h @@ -0,0 +1,182 @@ +/* + * Copyright (C) 2018 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef ANDROID_SERVERS_DISTORTIONMAPPER_H +#define ANDROID_SERVERS_DISTORTIONMAPPER_H + +#include +#include +#include + +#include "camera/CameraMetadata.h" + +namespace android { + +namespace camera3 { + +/** + * Utilities to transform between raw (distorted) and warped (corrected) coordinate systems + * for cameras that support geometric distortion + */ +class DistortionMapper { + public: + DistortionMapper(); + + /** + * Check whether distortion correction is supported by the camera HAL + */ + static bool isDistortionSupported(const CameraMetadata &deviceInfo); + + /** + * Update static lens calibration info from camera characteristics + */ + status_t setupStaticInfo(const CameraMetadata &deviceInfo); + + /** + * Return whether distortion correction can be applied currently + */ + bool calibrationValid() const; + + /** + * Correct capture request if distortion correction is enabled + */ + status_t correctCaptureRequest(CameraMetadata *request); + + /** + * Correct capture result if distortion correction is enabled + */ + status_t correctCaptureResult(CameraMetadata *request); + + + public: // Visible for testing. Not guarded by mutex; do not use concurrently + /** + * Update lens calibration from capture results or equivalent + */ + status_t updateCalibration(const CameraMetadata &result); + + /** + * Transform from distorted (original) to corrected (warped) coordinates. + * Coordinates are transformed in-place + * + * coordPairs: A pointer to an array of consecutive (x,y) points + * coordCount: Number of (x,y) pairs to transform + */ + status_t mapRawToCorrected(int32_t *coordPairs, int coordCount); + + /** + * Transform from distorted (original) to corrected (warped) coordinates. + * Coordinates are transformed in-place + * + * rects: A pointer to an array of consecutive (x,y, w, h) rectangles + * rectCount: Number of rectangles to transform + */ + status_t mapRawRectToCorrected(int32_t *rects, int rectCount); + + /** + * Transform from corrected (warped) to distorted (original) coordinates. + * Coordinates are transformed in-place + * + * coordPairs: A pointer to an array of consecutive (x,y) points + * coordCount: Number of (x,y) pairs to transform + */ + template + status_t mapCorrectedToRaw(T* coordPairs, int coordCount) const; + + /** + * Transform from corrected (warped) to distorted (original) coordinates. + * Coordinates are transformed in-place + * + * rects: A pointer to an array of consecutive (x,y, w, h) rectangles + * rectCount: Number of rectangles to transform + */ + status_t mapCorrectedRectToRaw(int32_t *rects, int rectCount) const; + + struct GridQuad { + // Source grid quad, or null + const GridQuad *src; + // x,y coordinates of corners, in + // clockwise order + std::array coords; + }; + + // Find which grid quad encloses the point; returns null if none do + static const GridQuad* findEnclosingQuad( + const int32_t pt[2], const std::vector& grid); + + // Calculate 'horizontal' interpolation coordinate for the point and the quad + // Assumes the point P is within the quad Q. + // Given quad with points P1-P4, and edges E12-E41, and considering the edge segments as + // functions of U: E12(u), where E12(0) = P1 and E12(1) = P2, then we want to find a u + // such that the edge E12(u) -> E43(u) contains point P. + // This can be determined by checking if the cross product of vector [E12(u)-E43(u)] and + // vector [E12(u)-P] is zero. Solving the equation + // [E12(u)-E43(u)] x [E12(u)-P] = 0 gives a quadratic equation in u; the solution in the range + // 0 to 1 is the one chosen. + // If calculateU is true, then an interpolation coordinate for edges E12 and E43 is found; + // if it is false, then an interpolation coordinate for edges E14 and E23 is found. + static float calculateUorV(const int32_t pt[2], const GridQuad& quad, bool calculateU); + + private: + mutable std::mutex mMutex; + + // Number of quads in each dimension of the mapping grids + constexpr static size_t kGridSize = 15; + // Margin to expand the grid by to ensure it doesn't clip the domain + constexpr static float kGridMargin = 0.05f; + // Fuzziness for float inequality tests + constexpr static float kFloatFuzz = 1e-4; + + // Metadata key lists to correct + + // Both capture request and result + static const std::array kMeteringRegionsToCorrect; + + // Only capture request + static const std::array kRequestRectsToCorrect; + + // Only capture result + static const std::array kResultRectsToCorrect; + + // Only for capture results + static const std::array kResultPointsToCorrect; + + // Utility to create reverse mapping grids + status_t buildGrids(); + + + bool mValidMapping; + bool mValidGrids; + + // intrisic parameters, in pixels + float mFx, mFy, mCx, mCy, mS; + // pre-calculated inverses for speed + float mInvFx, mInvFy; + // radial/tangential distortion parameters + float mK[5]; + + // pre-correction active array dimensions + int mArrayWidth, mArrayHeight; + + std::vector mCorrectedGrid; + std::vector mDistortedGrid; + +}; // class DistortionMapper + +} // namespace camera3 + +} // namespace android + +#endif