You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
511 lines
18 KiB
511 lines
18 KiB
/*
|
|
* 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 <algorithm>
|
|
#include <cmath>
|
|
|
|
#include "device3/DistortionMapper.h"
|
|
|
|
namespace android {
|
|
|
|
namespace camera3 {
|
|
|
|
|
|
DistortionMapper::DistortionMapper() : mValidMapping(false), mValidGrids(false) {
|
|
}
|
|
|
|
bool DistortionMapper::isDistortionSupported(const CameraMetadata &deviceInfo) {
|
|
bool isDistortionCorrectionSupported = false;
|
|
camera_metadata_ro_entry_t distortionCorrectionModes =
|
|
deviceInfo.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;
|
|
|
|
float arrayX = static_cast<float>(array.data.i32[0]);
|
|
float arrayY = static_cast<float>(array.data.i32[1]);
|
|
mArrayWidth = static_cast<float>(array.data.i32[2]);
|
|
mArrayHeight = static_cast<float>(array.data.i32[3]);
|
|
|
|
array = deviceInfo.find(ANDROID_SENSOR_INFO_ACTIVE_ARRAY_SIZE);
|
|
if (array.count != 4) return BAD_VALUE;
|
|
|
|
float activeX = static_cast<float>(array.data.i32[0]);
|
|
float activeY = static_cast<float>(array.data.i32[1]);
|
|
mActiveWidth = static_cast<float>(array.data.i32[2]);
|
|
mActiveHeight = static_cast<float>(array.data.i32[3]);
|
|
|
|
mArrayDiffX = activeX - arrayX;
|
|
mArrayDiffY = activeY - arrayY;
|
|
|
|
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) {
|
|
int32_t weight = e.data.i32[j + 4];
|
|
if (weight == 0) {
|
|
continue;
|
|
}
|
|
res = mapCorrectedToRaw(e.data.i32 + j, 2, /*clamp*/true);
|
|
if (res != OK) return res;
|
|
}
|
|
}
|
|
for (auto rect : kRectsToCorrect) {
|
|
e = request->find(rect);
|
|
res = mapCorrectedRectToRaw(e.data.i32, e.count / 4, /*clamp*/true);
|
|
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) {
|
|
int32_t weight = e.data.i32[j + 4];
|
|
if (weight == 0) {
|
|
continue;
|
|
}
|
|
res = mapRawToCorrected(e.data.i32 + j, 2, /*clamp*/true);
|
|
if (res != OK) return res;
|
|
}
|
|
}
|
|
for (auto rect : kRectsToCorrect) {
|
|
e = result->find(rect);
|
|
res = mapRawRectToCorrected(e.data.i32, e.count / 4, /*clamp*/true);
|
|
if (res != OK) return res;
|
|
}
|
|
for (auto pts : kResultPointsToCorrectNoClamp) {
|
|
e = result->find(pts);
|
|
res = mapRawToCorrected(e.data.i32, e.count / 2, /*clamp*/false);
|
|
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;
|
|
// Need to recalculate grid
|
|
mValidGrids = false;
|
|
|
|
return OK;
|
|
}
|
|
|
|
status_t DistortionMapper::mapRawToCorrected(int32_t *coordPairs, int coordCount,
|
|
bool clamp, bool simple) {
|
|
if (!mValidMapping) return INVALID_OPERATION;
|
|
|
|
if (simple) return mapRawToCorrectedSimple(coordPairs, coordCount, clamp);
|
|
|
|
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 for (%d, %d)",
|
|
*(coordPairs + i), *(coordPairs + i + 1));
|
|
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]);
|
|
|
|
// Clamp to within active array
|
|
if (clamp) {
|
|
corrX = std::min(mActiveWidth - 1, std::max(0.f, corrX));
|
|
corrY = std::min(mActiveHeight - 1, std::max(0.f, corrY));
|
|
}
|
|
|
|
coordPairs[i] = static_cast<int32_t>(std::round(corrX));
|
|
coordPairs[i + 1] = static_cast<int32_t>(std::round(corrY));
|
|
}
|
|
|
|
return OK;
|
|
}
|
|
|
|
status_t DistortionMapper::mapRawToCorrectedSimple(int32_t *coordPairs, int coordCount,
|
|
bool clamp) const {
|
|
if (!mValidMapping) return INVALID_OPERATION;
|
|
|
|
float scaleX = mActiveWidth / mArrayWidth;
|
|
float scaleY = mActiveHeight / mArrayHeight;
|
|
for (int i = 0; i < coordCount * 2; i += 2) {
|
|
float x = coordPairs[i];
|
|
float y = coordPairs[i + 1];
|
|
float corrX = x * scaleX;
|
|
float corrY = y * scaleY;
|
|
if (clamp) {
|
|
corrX = std::min(mActiveWidth - 1, std::max(0.f, corrX));
|
|
corrY = std::min(mActiveHeight - 1, std::max(0.f, corrY));
|
|
}
|
|
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, bool clamp,
|
|
bool simple) {
|
|
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] - 1,
|
|
rects[i + 1] + rects[i + 3] - 1
|
|
};
|
|
|
|
mapRawToCorrected(coords, 2, clamp, simple);
|
|
|
|
// Map back to (l, t, width, height)
|
|
rects[i] = coords[0];
|
|
rects[i + 1] = coords[1];
|
|
rects[i + 2] = coords[2] - coords[0] + 1;
|
|
rects[i + 3] = coords[3] - coords[1] + 1;
|
|
}
|
|
|
|
return OK;
|
|
}
|
|
|
|
status_t DistortionMapper::mapCorrectedToRaw(int32_t *coordPairs, int coordCount, bool clamp,
|
|
bool simple) const {
|
|
return mapCorrectedToRawImpl(coordPairs, coordCount, clamp, simple);
|
|
}
|
|
|
|
template<typename T>
|
|
status_t DistortionMapper::mapCorrectedToRawImpl(T *coordPairs, int coordCount, bool clamp,
|
|
bool simple) const {
|
|
if (!mValidMapping) return INVALID_OPERATION;
|
|
|
|
if (simple) return mapCorrectedToRawImplSimple(coordPairs, coordCount, clamp);
|
|
|
|
float activeCx = mCx - mArrayDiffX;
|
|
float activeCy = mCy - mArrayDiffY;
|
|
for (int i = 0; i < coordCount * 2; i += 2) {
|
|
// Move to normalized space from active array space
|
|
float ywi = (coordPairs[i + 1] - activeCy) * mInvFy;
|
|
float xwi = (coordPairs[i] - activeCx - 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;
|
|
// Clamp to within pre-correction active array
|
|
if (clamp) {
|
|
xr = std::min(mArrayWidth - 1, std::max(0.f, xr));
|
|
yr = std::min(mArrayHeight - 1, std::max(0.f, yr));
|
|
}
|
|
|
|
coordPairs[i] = static_cast<T>(std::round(xr));
|
|
coordPairs[i + 1] = static_cast<T>(std::round(yr));
|
|
}
|
|
|
|
return OK;
|
|
}
|
|
|
|
template<typename T>
|
|
status_t DistortionMapper::mapCorrectedToRawImplSimple(T *coordPairs, int coordCount,
|
|
bool clamp) const {
|
|
if (!mValidMapping) return INVALID_OPERATION;
|
|
|
|
float scaleX = mArrayWidth / mActiveWidth;
|
|
float scaleY = mArrayHeight / mActiveHeight;
|
|
for (int i = 0; i < coordCount * 2; i += 2) {
|
|
float x = coordPairs[i];
|
|
float y = coordPairs[i + 1];
|
|
float rawX = x * scaleX;
|
|
float rawY = y * scaleY;
|
|
if (clamp) {
|
|
rawX = std::min(mArrayWidth - 1, std::max(0.f, rawX));
|
|
rawY = std::min(mArrayHeight - 1, std::max(0.f, rawY));
|
|
}
|
|
coordPairs[i] = static_cast<T>(std::round(rawX));
|
|
coordPairs[i + 1] = static_cast<T>(std::round(rawY));
|
|
}
|
|
|
|
return OK;
|
|
}
|
|
|
|
status_t DistortionMapper::mapCorrectedRectToRaw(int32_t *rects, int rectCount, bool clamp,
|
|
bool simple) 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] - 1,
|
|
rects[i + 1] + rects[i + 3] - 1
|
|
};
|
|
|
|
mapCorrectedToRaw(coords, 2, clamp, simple);
|
|
|
|
// Map back to (l, t, width, height)
|
|
rects[i] = coords[0];
|
|
rects[i + 1] = coords[1];
|
|
rects[i + 2] = coords[2] - coords[0] + 1;
|
|
rects[i + 3] = coords[3] - coords[1] + 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 = mapCorrectedToRawImpl(mDistortedGrid[index].coords.data(), 4,
|
|
/*clamp*/false, /*simple*/false);
|
|
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
|