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