diff --git a/src/graphics/camera.cpp b/src/graphics/camera.cpp index b8f3ca05..07e5fa0e 100644 --- a/src/graphics/camera.cpp +++ b/src/graphics/camera.cpp @@ -38,6 +38,8 @@ using namespace glm; namespace Splash { +constexpr double MAX_REPROJECTION_ERROR = 1000.0; // Maximum admitted reprojection error + /*************/ Camera::Camera(RootObject* root, TreeRegisterStatus registerToTree) : GraphObject(root, registerToTree) @@ -308,17 +310,33 @@ bool Camera::doCalibration() } }; - // We need at least 7 points to get a meaningful calibration + // We need at least 6 points to get enough information to optimize + // for position, orientation, fov and optical center if ((pointsSet < 4) || (!coplanarPoints && pointsSet < 6)) { Log::get() << Log::Warning << "Camera::" << __FUNCTION__ << " - Calibration needs at least 4 coplanar points, or 6 non coplanar points" << Log::endl; return false; } - else if (!coplanarPoints && pointsSet < 7) - { - Log::get() << Log::Warning << "Camera::" << __FUNCTION__ << " - For better calibration results, use at least 7 non coplanar points" << Log::endl; - } + // If we have 6 points or more, at least 2 of them must _not_ be coplanar with the other ones + // To check that, we test coplanarity for all points minus one, for every combination. + // If one of these tests is true, it means only one point is not coplanar with the rest, which is not enough + if (pointsSet >= 6) + { + bool coplanarSelection = false; + for (size_t i = 0; i < worldPoints.size(); ++i) + { + std::vector selectedPoints = worldPoints; + selectedPoints.erase(selectedPoints.begin() + i); + coplanarSelection |= checkPointsCoplanar(selectedPoints, _coplanarTolerance); + } + + if (coplanarSelection) + { + Log::get() << Log::Warning << "Camera::" << __FUNCTION__ << " - Only 1 point is not coplanar with the others, at least 2 are needed" << Log::endl; + return false; + } + } _calibrationCalledOnce = true; @@ -436,7 +454,7 @@ bool Camera::doCalibration() gsl_multimin_fminimizer_free(minimizer); // If the result is good enough, apply it. Otherwise, drop! - if (minValue > 1000.0) + if (minValue > MAX_REPROJECTION_ERROR) { Log::get() << "Camera::" << __FUNCTION__ << " - Minumum found at (fov, cx, cy): " << selectedValues[0] << " " << selectedValues[1] << " " << selectedValues[2] << Log::endl; Log::get() << "Camera::" << __FUNCTION__ << " - Minimum value: " << minValue << Log::endl; @@ -459,9 +477,10 @@ bool Camera::doCalibration() _eye[i] = selectedValues[i + 3]; euler[i] = selectedValues[i + 6]; } - dmat4 rotateMat = yawPitchRoll(euler[0], euler[1], euler[2]); - dvec4 target = rotateMat * dvec4(1.0, 0.0, 0.0, 0.0); - dvec4 up = rotateMat * dvec4(0.0, 0.0, 1.0, 0.0); + + const auto rotateMat = yawPitchRoll(euler[0], euler[1], euler[2]); + const auto target = rotateMat * dvec4(1.0, 0.0, 0.0, 0.0); + const auto up = rotateMat * dvec4(0.0, 0.0, 1.0, 0.0); for (int i = 0; i < 3; ++i) { _target[i] = target[i]; @@ -1050,43 +1069,35 @@ double Camera::calibrationCostFunc(const gsl_vector* v, void* params) if (params == NULL) return 0.0; - Camera& camera = *(Camera*)params; + Camera& camera = *static_cast(params); - double fov = gsl_vector_get(v, 0); - double cx = gsl_vector_get(v, 1); - double cy = gsl_vector_get(v, 2); - - // Check whether the camera parameters are locked - if (camera["fov"].isLocked()) - fov = camera["fov"]()[0].as(); - if (camera["principalPoint"].isLocked()) - { - cx = camera["principalPoint"]()[0].as(); - cy = camera["principalPoint"]()[1].as(); - } + const double fov = camera["fov"].isLocked() ? camera["fov"]()[0].as() : gsl_vector_get(v, 0); + const double cx = camera["principalPoint"].isLocked() ? camera["principalPoint"]()[0].as() : gsl_vector_get(v, 1); + const double cy = camera["principalPoint"].isLocked() ? camera["principalPoint"]()[1].as() : gsl_vector_get(v, 2); // Some limits for the calibration parameters if (fov < 4.0 || fov > 120.0 || abs(cx - 0.5) > 1.0 || abs(cy - 0.5) > 1.0) return std::numeric_limits::max(); dvec3 eye; + dvec3 euler; dvec3 target; dvec3 up; - dvec3 euler; + for (int i = 0; i < 3; ++i) { eye[i] = gsl_vector_get(v, i + 3); euler[i] = gsl_vector_get(v, i + 6); } - dmat4 rotateMat = yawPitchRoll(euler[0], euler[1], euler[2]); - dvec4 targetTmp = rotateMat * dvec4(1.0, 0.0, 0.0, 0.0); - dvec4 upTmp = rotateMat * dvec4(0.0, 0.0, 1.0, 0.0); + + const auto rotateMat = yawPitchRoll(euler[0], euler[1], euler[2]); + const auto targetTmp = rotateMat * dvec4(1.0, 0.0, 0.0, 0.0); + const auto upTmp = rotateMat * dvec4(0.0, 0.0, 1.0, 0.0); for (int i = 0; i < 3; ++i) { - target[i] = targetTmp[i]; + target[i] = targetTmp[i] + eye[i]; up[i] = upTmp[i]; } - target += eye; std::vector objectPoints; std::vector imagePoints; @@ -1106,22 +1117,27 @@ double Camera::calibrationCostFunc(const gsl_vector* v, void* params) << camera._height - cy << Log::endl; #endif - dmat4 lookM = lookAt(eye, target, up); - dmat4 projM = dmat4(getProjectionMatrix(fov, camera._near, camera._far, camera._width, camera._height, cx, cy)); - dvec4 viewport(0, 0, camera._width, camera._height); + const auto lookM = lookAt(eye, target, up); + const auto projM = dmat4(getProjectionMatrix(fov, camera._near, camera._far, camera._width, camera._height, cx, cy)); + const auto viewport = dvec4(0, 0, camera._width, camera._height); // Project all the object points, and measure the distance between them and the image points double summedDistance = 0.0; for (uint32_t i = 0; i < imagePoints.size(); ++i) { - dvec3 projectedPoint; - projectedPoint = project(objectPoints[i], lookM, projM, viewport); - projectedPoint.z = 0.0; - - if (camera._weightedCalibrationPoints) - summedDistance += pointsWeight[i] * pow(imagePoints[i].x - projectedPoint.x, 2.0) + pow(imagePoints[i].y - projectedPoint.y, 2.0); + const auto projectedPoint = project(objectPoints[i], lookM, projM, viewport); + if (projectedPoint.z < 0.0 || projectedPoint.z > 1.0) + { + // Projected point is outside the frustum of the camera, and more precisely _behind_ + // the camera: even if the projected point matches, the camera is definitely not orientated correctly. + // We add twice the MAX_REPROJECTION_ERROR to make sure that this solution is not kept. + summedDistance += 2.0 * MAX_REPROJECTION_ERROR; + } else - summedDistance += pow(imagePoints[i].x - projectedPoint.x, 2.0) + pow(imagePoints[i].y - projectedPoint.y, 2.0); + { + const auto weight = camera._weightedCalibrationPoints ? pointsWeight[i] : 1.0; + summedDistance += weight * pow(imagePoints[i].x - projectedPoint.x, 2.0) + pow(imagePoints[i].y - projectedPoint.y, 2.0); + } } summedDistance /= imagePoints.size();