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
diff --git a/services/camera/libcameraservice/Android.mk b/services/camera/libcameraservice/Android.mk
index 7b86180..5697a87 100644
--- a/services/camera/libcameraservice/Android.mk
+++ b/services/camera/libcameraservice/Android.mk
@@ -51,6 +51,7 @@
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 187bea9..0c738e7 100644
--- a/services/camera/libcameraservice/api1/client2/FrameProcessor.cpp
+++ b/services/camera/libcameraservice/api1/client2/FrameProcessor.cpp
@@ -197,7 +197,6 @@
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 4a58620..d66dec4 100644
--- a/services/camera/libcameraservice/api1/client2/Parameters.cpp
+++ b/services/camera/libcameraservice/api1/client2/Parameters.cpp
@@ -954,13 +954,24 @@
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 @@
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 @@
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 e35b7a4..97f8ea7 100644
--- a/services/camera/libcameraservice/api1/client2/Parameters.h
+++ b/services/camera/libcameraservice/api1/client2/Parameters.h
@@ -177,6 +177,8 @@
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 d9bcba3..543914e 100644
--- a/services/camera/libcameraservice/device3/Camera3Device.cpp
+++ b/services/camera/libcameraservice/device3/Camera3Device.cpp
@@ -249,6 +249,14 @@
}
}
+ 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 @@
}
}
+ // 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 @@
// 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 @@
return INVALID_OPERATION;
}
+ {
+ // Correct metadata regions for distortion correction if enabled
+ sp<Camera3Device> 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 35f799d..d8fe19f 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 <camera_metadata_hidden.h>
@@ -1179,6 +1180,12 @@
/**** End scope for mInFlightLock ****/
+ /**
+ * Distortion correction support
+ */
+
+ camera3::DistortionMapper mDistortionMapper;
+
// Debug tracker for metadata tag value changes
// - Enabled with the -m <taglist> 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 0000000..82923bd
--- /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 <cmath>
+
+#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<uint32_t, 3> DistortionMapper::kMeteringRegionsToCorrect = {
+ ANDROID_CONTROL_AF_REGIONS,
+ ANDROID_CONTROL_AE_REGIONS,
+ ANDROID_CONTROL_AWB_REGIONS
+};
+
+// Only capture request
+constexpr std::array<uint32_t, 1> DistortionMapper::kRequestRectsToCorrect = {
+ ANDROID_SCALER_CROP_REGION,
+};
+
+// Only for capture result
+constexpr std::array<uint32_t, 2> DistortionMapper::kResultRectsToCorrect = {
+ ANDROID_SCALER_CROP_REGION,
+ ANDROID_STATISTICS_FACE_RECTANGLES
+};
+
+// Only for capture result
+constexpr std::array<uint32_t, 1> 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<std::mutex> 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<std::mutex> lock(mMutex);
+
+ return mValidMapping;
+}
+
+status_t DistortionMapper::correctCaptureRequest(CameraMetadata *request) {
+ std::lock_guard<std::mutex> 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<std::mutex> 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<int32_t>(std::round(corrX));
+ coordPairs[i + 1] = static_cast<int32_t>(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<typename T>
+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<T>(std::round(xr));
+ coordPairs[i + 1] = static_cast<T>(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<GridQuad>& 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 0000000..c6d715b
--- /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 <utils/Errors.h>
+#include <array>
+#include <mutex>
+
+#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<typename T>
+ 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<float, 8> 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<GridQuad>& 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<uint32_t, 3> kMeteringRegionsToCorrect;
+
+ // Only capture request
+ static const std::array<uint32_t, 1> kRequestRectsToCorrect;
+
+ // Only capture result
+ static const std::array<uint32_t, 2> kResultRectsToCorrect;
+
+ // Only for capture results
+ static const std::array<uint32_t, 1> 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<GridQuad> mCorrectedGrid;
+ std::vector<GridQuad> mDistortedGrid;
+
+}; // class DistortionMapper
+
+} // namespace camera3
+
+} // namespace android
+
+#endif