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 26 namespace android { 27 28 namespace camera3 { 29 30 /** 31 * Metadata keys to correct when adjusting coordinates for distortion correction 32 */ 33 34 // Both capture request and result 35 constexpr 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 42 constexpr std::array<uint32_t, 1> DistortionMapper::kRequestRectsToCorrect = { 43 ANDROID_SCALER_CROP_REGION, 44 }; 45 46 // Only for capture result 47 constexpr std::array<uint32_t, 1> DistortionMapper::kResultRectsToCorrect = { 48 ANDROID_SCALER_CROP_REGION, 49 }; 50 51 // Only for capture result 52 constexpr std::array<uint32_t, 2> DistortionMapper::kResultPointsToCorrectNoClamp = { 53 ANDROID_STATISTICS_FACE_RECTANGLES, // Says rectangles, is really points 54 ANDROID_STATISTICS_FACE_LANDMARKS, 55 }; 56 57 58 DistortionMapper::DistortionMapper() : mValidMapping(false), mValidGrids(false) { 59 } 60 61 bool 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 75 status_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 float arrayX = static_cast<float>(array.data.i32[0]); 83 float arrayY = static_cast<float>(array.data.i32[1]); 84 mArrayWidth = static_cast<float>(array.data.i32[2]); 85 mArrayHeight = static_cast<float>(array.data.i32[3]); 86 87 array = deviceInfo.find(ANDROID_SENSOR_INFO_ACTIVE_ARRAY_SIZE); 88 if (array.count != 4) return BAD_VALUE; 89 90 float activeX = static_cast<float>(array.data.i32[0]); 91 float activeY = static_cast<float>(array.data.i32[1]); 92 mActiveWidth = static_cast<float>(array.data.i32[2]); 93 mActiveHeight = static_cast<float>(array.data.i32[3]); 94 95 mArrayDiffX = activeX - arrayX; 96 mArrayDiffY = activeY - arrayY; 97 98 return updateCalibration(deviceInfo); 99 } 100 101 bool DistortionMapper::calibrationValid() const { 102 std::lock_guard<std::mutex> lock(mMutex); 103 104 return mValidMapping; 105 } 106 107 status_t DistortionMapper::correctCaptureRequest(CameraMetadata *request) { 108 std::lock_guard<std::mutex> lock(mMutex); 109 status_t res; 110 111 if (!mValidMapping) return OK; 112 113 camera_metadata_entry_t e; 114 e = request->find(ANDROID_DISTORTION_CORRECTION_MODE); 115 if (e.count != 0 && e.data.u8[0] != ANDROID_DISTORTION_CORRECTION_MODE_OFF) { 116 for (auto region : kMeteringRegionsToCorrect) { 117 e = request->find(region); 118 for (size_t j = 0; j < e.count; j += 5) { 119 int32_t weight = e.data.i32[j + 4]; 120 if (weight == 0) { 121 continue; 122 } 123 res = mapCorrectedToRaw(e.data.i32 + j, 2, /*clamp*/true); 124 if (res != OK) return res; 125 } 126 } 127 for (auto rect : kRequestRectsToCorrect) { 128 e = request->find(rect); 129 res = mapCorrectedRectToRaw(e.data.i32, e.count / 4, /*clamp*/true); 130 if (res != OK) return res; 131 } 132 } 133 134 return OK; 135 } 136 137 status_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, /*clamp*/true); 160 if (res != OK) return res; 161 } 162 } 163 for (auto rect : kResultRectsToCorrect) { 164 e = result->find(rect); 165 res = mapRawRectToCorrected(e.data.i32, e.count / 4, /*clamp*/true); 166 if (res != OK) return res; 167 } 168 for (auto pts : kResultPointsToCorrectNoClamp) { 169 e = result->find(pts); 170 res = mapRawToCorrected(e.data.i32, e.count / 2, /*clamp*/false); 171 if (res != OK) return res; 172 } 173 } 174 175 return OK; 176 } 177 178 // Utility methods; not guarded by mutex 179 180 status_t DistortionMapper::updateCalibration(const CameraMetadata &result) { 181 camera_metadata_ro_entry_t calib, distortion; 182 183 calib = result.find(ANDROID_LENS_INTRINSIC_CALIBRATION); 184 distortion = result.find(ANDROID_LENS_DISTORTION); 185 186 if (calib.count != 5) return BAD_VALUE; 187 if (distortion.count != 5) return BAD_VALUE; 188 189 // Skip redoing work if no change to calibration fields 190 if (mValidMapping && 191 mFx == calib.data.f[0] && 192 mFy == calib.data.f[1] && 193 mCx == calib.data.f[2] && 194 mCy == calib.data.f[3] && 195 mS == calib.data.f[4]) { 196 bool noChange = true; 197 for (size_t i = 0; i < distortion.count; i++) { 198 if (mK[i] != distortion.data.f[i]) { 199 noChange = false; 200 break; 201 } 202 } 203 if (noChange) return OK; 204 } 205 206 mFx = calib.data.f[0]; 207 mFy = calib.data.f[1]; 208 mCx = calib.data.f[2]; 209 mCy = calib.data.f[3]; 210 mS = calib.data.f[4]; 211 212 mInvFx = 1 / mFx; 213 mInvFy = 1 / mFy; 214 215 for (size_t i = 0; i < distortion.count; i++) { 216 mK[i] = distortion.data.f[i]; 217 } 218 219 mValidMapping = true; 220 // Need to recalculate grid 221 mValidGrids = false; 222 223 return OK; 224 } 225 226 status_t DistortionMapper::mapRawToCorrected(int32_t *coordPairs, int coordCount, 227 bool clamp, bool simple) { 228 if (!mValidMapping) return INVALID_OPERATION; 229 230 if (simple) return mapRawToCorrectedSimple(coordPairs, coordCount, clamp); 231 232 if (!mValidGrids) { 233 status_t res = buildGrids(); 234 if (res != OK) return res; 235 } 236 237 for (int i = 0; i < coordCount * 2; i += 2) { 238 const GridQuad *quad = findEnclosingQuad(coordPairs + i, mDistortedGrid); 239 if (quad == nullptr) { 240 ALOGE("Raw to corrected mapping failure: No quad found for (%d, %d)", 241 *(coordPairs + i), *(coordPairs + i + 1)); 242 return INVALID_OPERATION; 243 } 244 ALOGV("src xy: %d, %d, enclosing quad: (%f, %f), (%f, %f), (%f, %f), (%f, %f)", 245 coordPairs[i], coordPairs[i+1], 246 quad->coords[0], quad->coords[1], 247 quad->coords[2], quad->coords[3], 248 quad->coords[4], quad->coords[5], 249 quad->coords[6], quad->coords[7]); 250 251 const GridQuad *corrQuad = quad->src; 252 if (corrQuad == nullptr) { 253 ALOGE("Raw to corrected mapping failure: No src quad found"); 254 return INVALID_OPERATION; 255 } 256 ALOGV(" corr quad: (%f, %f), (%f, %f), (%f, %f), (%f, %f)", 257 corrQuad->coords[0], corrQuad->coords[1], 258 corrQuad->coords[2], corrQuad->coords[3], 259 corrQuad->coords[4], corrQuad->coords[5], 260 corrQuad->coords[6], corrQuad->coords[7]); 261 262 float u = calculateUorV(coordPairs + i, *quad, /*calculateU*/ true); 263 float v = calculateUorV(coordPairs + i, *quad, /*calculateU*/ false); 264 265 ALOGV("uv: %f, %f", u, v); 266 267 // Interpolate along top edge of corrected quad (which are axis-aligned) for x 268 float corrX = corrQuad->coords[0] + u * (corrQuad->coords[2] - corrQuad->coords[0]); 269 // Interpolate along left edge of corrected quad (which are axis-aligned) for y 270 float corrY = corrQuad->coords[1] + v * (corrQuad->coords[7] - corrQuad->coords[1]); 271 272 // Clamp to within active array 273 if (clamp) { 274 corrX = std::min(mActiveWidth - 1, std::max(0.f, corrX)); 275 corrY = std::min(mActiveHeight - 1, std::max(0.f, corrY)); 276 } 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 285 status_t DistortionMapper::mapRawToCorrectedSimple(int32_t *coordPairs, int coordCount, 286 bool clamp) const { 287 if (!mValidMapping) return INVALID_OPERATION; 288 289 float scaleX = mActiveWidth / mArrayWidth; 290 float scaleY = mActiveHeight / mArrayHeight; 291 for (int i = 0; i < coordCount * 2; i += 2) { 292 float x = coordPairs[i]; 293 float y = coordPairs[i + 1]; 294 float corrX = x * scaleX; 295 float corrY = y * scaleY; 296 if (clamp) { 297 corrX = std::min(mActiveWidth - 1, std::max(0.f, corrX)); 298 corrY = std::min(mActiveHeight - 1, std::max(0.f, corrY)); 299 } 300 coordPairs[i] = static_cast<int32_t>(std::round(corrX)); 301 coordPairs[i + 1] = static_cast<int32_t>(std::round(corrY)); 302 } 303 304 return OK; 305 } 306 307 status_t DistortionMapper::mapRawRectToCorrected(int32_t *rects, int rectCount, bool clamp, 308 bool simple) { 309 if (!mValidMapping) return INVALID_OPERATION; 310 for (int i = 0; i < rectCount * 4; i += 4) { 311 // Map from (l, t, width, height) to (l, t, r, b) 312 int32_t coords[4] = { 313 rects[i], 314 rects[i + 1], 315 rects[i] + rects[i + 2] - 1, 316 rects[i + 1] + rects[i + 3] - 1 317 }; 318 319 mapRawToCorrected(coords, 2, clamp, simple); 320 321 // Map back to (l, t, width, height) 322 rects[i] = coords[0]; 323 rects[i + 1] = coords[1]; 324 rects[i + 2] = coords[2] - coords[0] + 1; 325 rects[i + 3] = coords[3] - coords[1] + 1; 326 } 327 328 return OK; 329 } 330 331 status_t DistortionMapper::mapCorrectedToRaw(int32_t *coordPairs, int coordCount, bool clamp, 332 bool simple) const { 333 return mapCorrectedToRawImpl(coordPairs, coordCount, clamp, simple); 334 } 335 336 template<typename T> 337 status_t DistortionMapper::mapCorrectedToRawImpl(T *coordPairs, int coordCount, bool clamp, 338 bool simple) const { 339 if (!mValidMapping) return INVALID_OPERATION; 340 341 if (simple) return mapCorrectedToRawImplSimple(coordPairs, coordCount, clamp); 342 343 float activeCx = mCx - mArrayDiffX; 344 float activeCy = mCy - mArrayDiffY; 345 for (int i = 0; i < coordCount * 2; i += 2) { 346 // Move to normalized space from active array space 347 float ywi = (coordPairs[i + 1] - activeCy) * mInvFy; 348 float xwi = (coordPairs[i] - activeCx - mS * ywi) * mInvFx; 349 // Apply distortion model to calculate raw image coordinates 350 float rSq = xwi * xwi + ywi * ywi; 351 float Fr = 1.f + (mK[0] * rSq) + (mK[1] * rSq * rSq) + (mK[2] * rSq * rSq * rSq); 352 float xc = xwi * Fr + (mK[3] * 2 * xwi * ywi) + mK[4] * (rSq + 2 * xwi * xwi); 353 float yc = ywi * Fr + (mK[4] * 2 * xwi * ywi) + mK[3] * (rSq + 2 * ywi * ywi); 354 // Move back to image space 355 float xr = mFx * xc + mS * yc + mCx; 356 float yr = mFy * yc + mCy; 357 // Clamp to within pre-correction active array 358 if (clamp) { 359 xr = std::min(mArrayWidth - 1, std::max(0.f, xr)); 360 yr = std::min(mArrayHeight - 1, std::max(0.f, yr)); 361 } 362 363 coordPairs[i] = static_cast<T>(std::round(xr)); 364 coordPairs[i + 1] = static_cast<T>(std::round(yr)); 365 } 366 367 return OK; 368 } 369 370 template<typename T> 371 status_t DistortionMapper::mapCorrectedToRawImplSimple(T *coordPairs, int coordCount, 372 bool clamp) const { 373 if (!mValidMapping) return INVALID_OPERATION; 374 375 float scaleX = mArrayWidth / mActiveWidth; 376 float scaleY = mArrayHeight / mActiveHeight; 377 for (int i = 0; i < coordCount * 2; i += 2) { 378 float x = coordPairs[i]; 379 float y = coordPairs[i + 1]; 380 float rawX = x * scaleX; 381 float rawY = y * scaleY; 382 if (clamp) { 383 rawX = std::min(mArrayWidth - 1, std::max(0.f, rawX)); 384 rawY = std::min(mArrayHeight - 1, std::max(0.f, rawY)); 385 } 386 coordPairs[i] = static_cast<T>(std::round(rawX)); 387 coordPairs[i + 1] = static_cast<T>(std::round(rawY)); 388 } 389 390 return OK; 391 } 392 393 394 status_t DistortionMapper::mapCorrectedRectToRaw(int32_t *rects, int rectCount, bool clamp, 395 bool simple) const { 396 if (!mValidMapping) return INVALID_OPERATION; 397 398 for (int i = 0; i < rectCount * 4; i += 4) { 399 // Map from (l, t, width, height) to (l, t, r, b) 400 int32_t coords[4] = { 401 rects[i], 402 rects[i + 1], 403 rects[i] + rects[i + 2] - 1, 404 rects[i + 1] + rects[i + 3] - 1 405 }; 406 407 mapCorrectedToRaw(coords, 2, clamp, simple); 408 409 // Map back to (l, t, width, height) 410 rects[i] = coords[0]; 411 rects[i + 1] = coords[1]; 412 rects[i + 2] = coords[2] - coords[0] + 1; 413 rects[i + 3] = coords[3] - coords[1] + 1; 414 } 415 416 return OK; 417 } 418 419 status_t DistortionMapper::buildGrids() { 420 if (mCorrectedGrid.size() != kGridSize * kGridSize) { 421 mCorrectedGrid.resize(kGridSize * kGridSize); 422 mDistortedGrid.resize(kGridSize * kGridSize); 423 } 424 425 float gridMargin = mArrayWidth * kGridMargin; 426 float gridSpacingX = (mArrayWidth + 2 * gridMargin) / kGridSize; 427 float gridSpacingY = (mArrayHeight + 2 * gridMargin) / kGridSize; 428 429 size_t index = 0; 430 float x = -gridMargin; 431 for (size_t i = 0; i < kGridSize; i++, x += gridSpacingX) { 432 float y = -gridMargin; 433 for (size_t j = 0; j < kGridSize; j++, y += gridSpacingY, index++) { 434 mCorrectedGrid[index].src = nullptr; 435 mCorrectedGrid[index].coords = { 436 x, y, 437 x + gridSpacingX, y, 438 x + gridSpacingX, y + gridSpacingY, 439 x, y + gridSpacingY 440 }; 441 mDistortedGrid[index].src = &mCorrectedGrid[index]; 442 mDistortedGrid[index].coords = mCorrectedGrid[index].coords; 443 status_t res = mapCorrectedToRawImpl(mDistortedGrid[index].coords.data(), 4, 444 /*clamp*/false, /*simple*/false); 445 if (res != OK) return res; 446 } 447 } 448 449 mValidGrids = true; 450 return OK; 451 } 452 453 const DistortionMapper::GridQuad* DistortionMapper::findEnclosingQuad( 454 const int32_t pt[2], const std::vector<GridQuad>& grid) { 455 const float x = pt[0]; 456 const float y = pt[1]; 457 458 for (const GridQuad& quad : grid) { 459 const float &x1 = quad.coords[0]; 460 const float &y1 = quad.coords[1]; 461 const float &x2 = quad.coords[2]; 462 const float &y2 = quad.coords[3]; 463 const float &x3 = quad.coords[4]; 464 const float &y3 = quad.coords[5]; 465 const float &x4 = quad.coords[6]; 466 const float &y4 = quad.coords[7]; 467 468 // Point-in-quad test: 469 470 // Quad has corners P1-P4; if P is within the quad, then it is on the same side of all the 471 // edges (or on top of one of the edges or corners), traversed in a consistent direction. 472 // This means that the cross product of edge En = Pn->P(n+1 mod 4) and line Ep = Pn->P must 473 // have the same sign (or be zero) for all edges. 474 // For clockwise traversal, the sign should be negative or zero for Ep x En, indicating that 475 // En is to the left of Ep, or overlapping. 476 float s1 = (x - x1) * (y2 - y1) - (y - y1) * (x2 - x1); 477 if (s1 > 0) continue; 478 float s2 = (x - x2) * (y3 - y2) - (y - y2) * (x3 - x2); 479 if (s2 > 0) continue; 480 float s3 = (x - x3) * (y4 - y3) - (y - y3) * (x4 - x3); 481 if (s3 > 0) continue; 482 float s4 = (x - x4) * (y1 - y4) - (y - y4) * (x1 - x4); 483 if (s4 > 0) continue; 484 485 return &quad; 486 } 487 return nullptr; 488 } 489 490 float DistortionMapper::calculateUorV(const int32_t pt[2], const GridQuad& quad, bool calculateU) { 491 const float x = pt[0]; 492 const float y = pt[1]; 493 const float &x1 = quad.coords[0]; 494 const float &y1 = quad.coords[1]; 495 const float &x2 = calculateU ? quad.coords[2] : quad.coords[6]; 496 const float &y2 = calculateU ? quad.coords[3] : quad.coords[7]; 497 const float &x3 = quad.coords[4]; 498 const float &y3 = quad.coords[5]; 499 const float &x4 = calculateU ? quad.coords[6] : quad.coords[2]; 500 const float &y4 = calculateU ? quad.coords[7] : quad.coords[3]; 501 502 float a = (x1 - x2) * (y1 - y2 + y3 - y4) - (y1 - y2) * (x1 - x2 + x3 - x4); 503 float b = (x - x1) * (y1 - y2 + y3 - y4) + (x1 - x2) * (y4 - y1) - 504 (y - y1) * (x1 - x2 + x3 - x4) - (y1 - y2) * (x4 - x1); 505 float c = (x - x1) * (y4 - y1) - (y - y1) * (x4 - x1); 506 507 if (a == 0) { 508 // One solution may happen if edges are parallel 509 float u0 = -c / b; 510 ALOGV("u0: %.9g, b: %f, c: %f", u0, b, c); 511 return u0; 512 } 513 514 float det = b * b - 4 * a * c; 515 if (det < 0) { 516 // Sanity check - should not happen if pt is within the quad 517 ALOGE("Bad determinant! a: %f, b: %f, c: %f, det: %f", a,b,c,det); 518 return -1; 519 } 520 521 // Select more numerically stable solution 522 float sqdet = b > 0 ? -std::sqrt(det) : std::sqrt(det); 523 524 float u1 = (-b + sqdet) / (2 * a); 525 ALOGV("u1: %.9g", u1); 526 if (0 - kFloatFuzz < u1 && u1 < 1 + kFloatFuzz) return u1; 527 528 float u2 = c / (a * u1); 529 ALOGV("u2: %.9g", u2); 530 if (0 - kFloatFuzz < u2 && u2 < 1 + kFloatFuzz) return u2; 531 532 // Last resort, return the smaller-magnitude solution 533 return fabs(u1) < fabs(u2) ? u1 : u2; 534 } 535 536 } // namespace camera3 537 538 } // namespace android 539
