mirrors/opencv
2
0 Fork 0
mirror of https://github.com/opencv/opencv.git synced 2026-10-01 04:03:49 +03:00
Code Issues Packages Projects Releases Wiki Activity
Files
5.x
opencv/modules/calib/test/test_fisheye.cpp
T
Sergiu Deitsch faced50373 calib: improve fisheye extrinsic refinement 
Improve fisheye extrinsic refinement for ill-conditioned views and
poorly scaled object points. Replace the Gauss-Newton update with a
Levenberg-Marquardt solver and normalize translation parameters to make
convergence less sensitive to problem scale.
2026-09-28 20:47:53 +02:00

1125 lines
46 KiB
C++
Raw Permalink Blame History

/*M///////////////////////////////////////////////////////////////////////////////////////
//
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
// By downloading, copying, installing or using the software you agree to this license.
// If you do not agree to this license, do not download, install,
// copy or use the software.
//
//
// License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
// Copyright (C) 2009-2011, Willow Garage Inc., all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
// * Redistribution's of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// * Redistribution's in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
//
// * The name of the copyright holders may not be used to endorse or promote products
// derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/
#include "test_precomp.hpp"
#include <opencv2/ts/cuda_test.hpp> // EXPECT_MAT_NEAR
#include "../src/fisheye.hpp"
#include "opencv2/videoio.hpp"
namespace opencv_test { namespace {
class fisheyeTest : public ::testing::Test {
protected:
const static cv::Size imageSize;
const static cv::Matx33d K;
const static cv::Vec4d D;
const static cv::Matx33d R;
const static cv::Vec3d T;
std::string datasets_repository_path;
virtual void SetUp() {
datasets_repository_path = combine(cvtest::TS::ptr()->get_data_path(), "cv/cameracalibration/fisheye");
}
protected:
std::string combine(const std::string& _item1, const std::string& _item2);
};
static int fixedIntrinsicCalibrationFlags(const bool checkCondition)
{
int flags = cv::fisheye::CALIB_USE_INTRINSIC_GUESS |
cv::fisheye::CALIB_RECOMPUTE_EXTRINSIC |
cv::fisheye::CALIB_FIX_FOCAL_LENGTH |
cv::fisheye::CALIB_FIX_PRINCIPAL_POINT |
cv::fisheye::CALIB_FIX_SKEW |
cv::fisheye::CALIB_FIX_K1 |
cv::fisheye::CALIB_FIX_K2 |
cv::fisheye::CALIB_FIX_K3 |
cv::fisheye::CALIB_FIX_K4;
if (checkCondition)
{
flags |= cv::fisheye::CALIB_CHECK_COND;
}
return flags;
}
static void createNearCollinearView(cv::Matx33d& cameraMatrix,
cv::Vec4d& distortion,
cv::Size& imageSize,
std::vector<cv::Point3d>& objectPoints,
std::vector<cv::Point2d>& imagePoints)
{
const double focalLengthX = 600.0;
const double focalLengthY = 610.0;
const double principalPointX = 320.0;
const double principalPointY = 240.0;
const double lateralPerturbation = 1e-8;
cameraMatrix = cv::Matx33d(focalLengthX, 0.0, principalPointX,
0.0, focalLengthY, principalPointY,
0.0, 0.0, 1.0);
distortion = cv::Vec4d::all(0);
const cv::Vec3d trueR(0.2, -0.15, 0.1);
const cv::Vec3d trueT(0.1, -0.2, 3.0);
imageSize = cv::Size(640, 480);
objectPoints = {
{-0.4, -lateralPerturbation, 0.0},
{-0.3, lateralPerturbation, 0.1},
{-0.2, -lateralPerturbation, 0.2},
{-0.1, lateralPerturbation, 0.3},
{0.0, -lateralPerturbation, 0.4},
{0.1, lateralPerturbation, 0.5},
{0.2, -lateralPerturbation, 0.6},
{0.3, lateralPerturbation, 0.7},
{0.4, -lateralPerturbation, 0.8}};
cv::fisheye::projectPoints(objectPoints, imagePoints, trueR, trueT,
cameraMatrix, distortion);
}
const cv::Size fisheyeTest::imageSize(1280, 800);
const cv::Matx33d fisheyeTest::K(558.478087865323, 0, 620.458515360843,
0, 560.506767351568, 381.939424848348,
0, 0, 1);
const cv::Vec4d fisheyeTest::D(-0.0014613319981768, -0.00329861110580401, 0.00605760088590183, -0.00374209380722371);
const cv::Matx33d fisheyeTest::R ( 9.9756700084424932e-01, 6.9698277640183867e-02, 1.4929569991321144e-03,
-6.9711825162322980e-02, 9.9748249845531767e-01, 1.2997180766418455e-02,
-5.8331736398316541e-04,-1.3069635393884985e-02, 9.9991441852366736e-01);
const cv::Vec3d fisheyeTest::T(-9.9217369356044638e-02, 3.1741831972356663e-03, 1.8551007952921010e-04);
std::string fisheyeTest::combine(const std::string& _item1, const std::string& _item2)
{
std::string item1 = _item1, item2 = _item2;
std::replace(item1.begin(), item1.end(), '\\', '/');
std::replace(item2.begin(), item2.end(), '\\', '/');
if (item1.empty())
return item2;
if (item2.empty())
return item1;
char last = item1[item1.size()-1];
return item1 + (last != '/' ? "/" : "") + item2;
}
class FisheyeExtrinsicRefinementScaleTest
: public fisheyeTest,
public ::testing::WithParamInterface<double>
{
};
TEST_P(FisheyeExtrinsicRefinementScaleTest, IsInvariantToObjectPointScale)
{
const cv::Matx33d cameraMatrix(600.0, 0.0, 320.0,
0.0, 610.0, 240.0,
0.0, 0.0, 1.0);
const cv::Vec4d distortion = cv::Vec4d::all(0);
const std::vector<cv::Vec3d> trueR = {
{0.15, -0.1, 0.08}, {-0.1, 0.12, -0.06}};
const std::vector<cv::Vec3d> trueT = {
{0.2, -0.15, 3.0}, {-0.3, 0.1, 2.5}};
const cv::Size testImageSize(640, 480);
const int maxIterations = 1;
const double scale = GetParam();
const std::vector<cv::Point3d> baseObjectPoints = {
{-0.4, -0.3, 0.0}, {0.0, -0.3, 0.1}, {0.4, -0.3, 0.0},
{-0.4, 0.0, 0.1}, {0.0, 0.0, 0.0}, {0.4, 0.0, 0.1},
{-0.4, 0.3, 0.0}, {0.0, 0.3, 0.1}, {0.4, 0.3, 0.0}};
std::vector<std::vector<cv::Point3d> > objectPoints(2);
std::vector<std::vector<cv::Point2d> > imagePoints(2);
for (size_t view = 0; view < trueR.size(); ++view)
{
for (const cv::Point3d& point : baseObjectPoints)
{
objectPoints[view].emplace_back(point * scale);
}
const cv::Vec3d scaledTrueT = trueT[view] * scale;
cv::fisheye::projectPoints(objectPoints[view], imagePoints[view],
trueR[view], scaledTrueT,
cameraMatrix, distortion);
}
cv::Mat estimatedK = cv::Mat(cameraMatrix);
cv::Mat estimatedD = cv::Mat(distortion);
std::vector<cv::Vec3d> estimatedR;
std::vector<cv::Vec3d> estimatedT;
const int flags = fixedIntrinsicCalibrationFlags(false);
cv::fisheye::calibrate(objectPoints, imagePoints, testImageSize, estimatedK,
estimatedD, estimatedR, estimatedT, flags,
cv::TermCriteria(cv::TermCriteria::COUNT,
maxIterations, 0));
double rms = 0;
for (size_t view = 0; view < objectPoints.size(); ++view)
{
std::vector<cv::Point2d> reprojected;
cv::fisheye::projectPoints(objectPoints[view], reprojected,
estimatedR[view], estimatedT[view],
cameraMatrix, distortion);
rms += cv::norm(cv::Mat(reprojected) - cv::Mat(imagePoints[view]),
cv::NORM_L2SQR);
}
EXPECT_NEAR(std::sqrt(rms / (2 * baseObjectPoints.size())), 0.0, 1e-8);
}
constexpr double normalObjectPointScale = 1.0;
constexpr double smallObjectPointScale = 1e-7;
constexpr double smallestNormalObjectPointScale =
std::numeric_limits<double>::min();
INSTANTIATE_TEST_CASE_P(Fisheye, FisheyeExtrinsicRefinementScaleTest,
::testing::Values(normalObjectPointScale,
smallObjectPointScale,
smallestNormalObjectPointScale));
TEST_F(fisheyeTest, CalibrationHandlesTranslatedObjectPoints)
{
const cv::Matx33d cameraMatrix(600.0, 0.0, 320.0,
0.0, 610.0, 240.0,
0.0, 0.0, 1.0);
const cv::Vec4d distortion = cv::Vec4d::all(0);
const cv::Vec3d trueR(0.15, -0.1, 0.08);
const cv::Vec3d trueT(0.2, -0.15, 3.0);
constexpr double objectPointOffsetMagnitude = 1e6;
constexpr double reprojectionTolerance = 1e-7;
const cv::Vec3d objectPointOffset(objectPointOffsetMagnitude,
-2.0 * objectPointOffsetMagnitude,
3.0 * objectPointOffsetMagnitude);
const cv::Size testImageSize(640, 480);
const int maxIterations = 1;
const std::vector<cv::Point3d> baseObjectPoints = {
{-0.4, -0.3, 0.0}, {0.0, -0.3, 0.1}, {0.4, -0.3, 0.0},
{-0.4, 0.0, 0.1}, {0.0, 0.0, 0.0}, {0.4, 0.0, 0.1},
{-0.4, 0.3, 0.0}, {0.0, 0.3, 0.1}, {0.4, 0.3, 0.0}};
cv::Matx33d rotationMatrix;
cv::Rodrigues(trueR, rotationMatrix);
const cv::Vec3d rotatedOffset = rotationMatrix * objectPointOffset;
const cv::Vec3d shiftedTrueT = trueT - rotatedOffset;
std::vector<cv::Point3d> objectPoints;
for (const cv::Point3d& point : baseObjectPoints)
{
objectPoints.emplace_back(point + cv::Point3d(objectPointOffset));
}
std::vector<cv::Point2d> imagePoints;
cv::fisheye::projectPoints(objectPoints, imagePoints, trueR, shiftedTrueT,
cameraMatrix, distortion);
cv::Mat estimatedK = cv::Mat(cameraMatrix);
cv::Mat estimatedD = cv::Mat(distortion);
std::vector<cv::Vec3d> estimatedR;
std::vector<cv::Vec3d> estimatedT;
const int flags = fixedIntrinsicCalibrationFlags(false);
cv::fisheye::calibrate(
std::vector<std::vector<cv::Point3d> >(1, objectPoints),
std::vector<std::vector<cv::Point2d> >(1, imagePoints),
testImageSize, estimatedK, estimatedD, estimatedR, estimatedT, flags,
cv::TermCriteria(cv::TermCriteria::COUNT, maxIterations, 0));
std::vector<cv::Point2d> reprojected;
cv::fisheye::projectPoints(objectPoints, reprojected, estimatedR[0],
estimatedT[0], cameraMatrix, distortion);
const double rms = cv::norm(cv::Mat(reprojected) - cv::Mat(imagePoints),
cv::NORM_L2SQR);
const double reprojectionRms =
std::sqrt(rms / (2 * baseObjectPoints.size()));
EXPECT_NEAR(reprojectionRms, 0.0, reprojectionTolerance);
}
TEST_F(fisheyeTest, CalibrationRefinesNearCollinearView)
{
cv::Matx33d cameraMatrix;
cv::Vec4d distortion;
cv::Size testImageSize;
std::vector<cv::Point3d> objectPoints;
std::vector<cv::Point2d> imagePoints;
createNearCollinearView(cameraMatrix, distortion, testImageSize,
objectPoints, imagePoints);
cv::Mat estimatedK = cv::Mat(cameraMatrix);
cv::Mat estimatedD = cv::Mat(distortion);
std::vector<cv::Vec3d> estimatedR;
std::vector<cv::Vec3d> estimatedT;
const int flags = fixedIntrinsicCalibrationFlags(false);
const double rms = cv::fisheye::calibrate(
std::vector<std::vector<cv::Point3d> >(1, objectPoints),
std::vector<std::vector<cv::Point2d> >(1, imagePoints),
testImageSize, estimatedK, estimatedD, estimatedR, estimatedT, flags,
cv::TermCriteria(cv::TermCriteria::COUNT, 20, 0));
EXPECT_LT(rms, 1e-8);
}
TEST_F(fisheyeTest, CalibrationCheckCondRejectsNearCollinearView)
{
cv::Matx33d cameraMatrix;
cv::Vec4d distortion;
cv::Size testImageSize;
std::vector<cv::Point3d> objectPoints;
std::vector<cv::Point2d> imagePoints;
createNearCollinearView(cameraMatrix, distortion, testImageSize,
objectPoints, imagePoints);
cv::Mat estimatedK = cv::Mat(cameraMatrix);
cv::Mat estimatedD = cv::Mat(distortion);
const int flags = fixedIntrinsicCalibrationFlags(true);
EXPECT_THROW(
cv::fisheye::calibrate(
std::vector<std::vector<cv::Point3d> >(1, objectPoints),
std::vector<std::vector<cv::Point2d> >(1, imagePoints),
testImageSize, estimatedK, estimatedD, cv::noArray(),
cv::noArray(), flags,
cv::TermCriteria(cv::TermCriteria::COUNT, 20, 0)),
cv::Exception);
}
TEST_F(fisheyeTest, Calibration)
{
const int n_images = 34;
const cv::Matx33d goldK(558.4780870585967, 0, 620.4585053962692,
0, 560.5067667343917, 381.9394122875291,
0, 0, 1);
const cv::Vec4d goldD(-0.00146136, -0.00329847, 0.00605742, -0.00374201);
std::vector<std::vector<cv::Point2d> > imagePoints(n_images);
std::vector<std::vector<cv::Point3d> > objectPoints(n_images);
const std::string folder = combine(datasets_repository_path, "calib-3_stereo_from_JY");
cv::FileStorage fs_left(combine(folder, "left.xml"), cv::FileStorage::READ);
CV_Assert(fs_left.isOpened());
for(int i = 0; i < n_images; ++i)
fs_left[cv::format("image_%d", i )] >> imagePoints[i];
fs_left.release();
cv::FileStorage fs_object(combine(folder, "object.xml"), cv::FileStorage::READ);
CV_Assert(fs_object.isOpened());
for(int i = 0; i < n_images; ++i)
fs_object[cv::format("image_%d", i )] >> objectPoints[i];
fs_object.release();
int flag = 0;
flag |= cv::CALIB_RECOMPUTE_EXTRINSIC;
flag |= cv::CALIB_CHECK_COND;
flag |= cv::CALIB_FIX_SKEW;
cv::Matx33d theK;
cv::Vec4d theD;
cv::fisheye::calibrate(objectPoints, imagePoints, imageSize, theK, theD,
cv::noArray(), cv::noArray(), flag, cv::TermCriteria(3, 20, 1e-6));
EXPECT_MAT_NEAR(theK, goldK, 1e-8);
EXPECT_MAT_NEAR(theD, goldD, 1e-8);
}
TEST_F(fisheyeTest, CalibrationWithFixedFocalLength)
{
const int n_images = 34;
std::vector<std::vector<cv::Point2d> > imagePoints(n_images);
std::vector<std::vector<cv::Point3d> > objectPoints(n_images);
const std::string folder =combine(datasets_repository_path, "calib-3_stereo_from_JY");
cv::FileStorage fs_left(combine(folder, "left.xml"), cv::FileStorage::READ);
CV_Assert(fs_left.isOpened());
for(int i = 0; i < n_images; ++i)
fs_left[cv::format("image_%d", i )] >> imagePoints[i];
fs_left.release();
cv::FileStorage fs_object(combine(folder, "object.xml"), cv::FileStorage::READ);
CV_Assert(fs_object.isOpened());
for(int i = 0; i < n_images; ++i)
fs_object[cv::format("image_%d", i )] >> objectPoints[i];
fs_object.release();
int flag = 0;
flag |= cv::CALIB_RECOMPUTE_EXTRINSIC;
flag |= cv::CALIB_CHECK_COND;
flag |= cv::CALIB_FIX_SKEW;
flag |= cv::CALIB_FIX_FOCAL_LENGTH;
flag |= cv::CALIB_USE_INTRINSIC_GUESS;
cv::Matx33d theK = this->K;
const cv::Matx33d newK(
558.478088, 0.000000, 620.458461,
0.000000, 560.506767, 381.939362,
0.000000, 0.000000, 1.000000);
cv::Vec4d theD;
const cv::Vec4d newD(-0.001461, -0.003298, 0.006057, -0.003742);
cv::fisheye::calibrate(objectPoints, imagePoints, imageSize, theK, theD,
cv::noArray(), cv::noArray(), flag, cv::TermCriteria(3, 20, 1e-6));
// ensure that CALIB_FIX_FOCAL_LENGTH works and focal length has not changed
EXPECT_EQ(theK(0,0), K(0,0));
EXPECT_EQ(theK(1,1), K(1,1));
EXPECT_MAT_NEAR(theK, newK, 1e-6);
EXPECT_MAT_NEAR(theD, newD, 1e-6);
}
TEST_F(fisheyeTest, Homography)
{
const int n_images = 1;
std::vector<std::vector<cv::Point2d> > imagePoints(n_images);
std::vector<std::vector<cv::Point3d> > objectPoints(n_images);
const std::string folder = combine(datasets_repository_path, "calib-3_stereo_from_JY");
cv::FileStorage fs_left(combine(folder, "left.xml"), cv::FileStorage::READ);
CV_Assert(fs_left.isOpened());
for(int i = 0; i < n_images; ++i)
fs_left[cv::format("image_%d", i )] >> imagePoints[i];
fs_left.release();
cv::FileStorage fs_object(combine(folder, "object.xml"), cv::FileStorage::READ);
CV_Assert(fs_object.isOpened());
for(int i = 0; i < n_images; ++i)
fs_object[cv::format("image_%d", i )] >> objectPoints[i];
fs_object.release();
cv::internal::IntrinsicParams param;
param.Init(cv::Vec2d(cv::max(imageSize.width, imageSize.height) / CV_PI, cv::max(imageSize.width, imageSize.height) / CV_PI),
cv::Vec2d(imageSize.width / 2.0 - 0.5, imageSize.height / 2.0 - 0.5));
cv::Mat _imagePoints (imagePoints[0]);
cv::Mat _objectPoints(objectPoints[0]);
cv::Mat imagePointsNormalized = NormalizePixels(_imagePoints, param).reshape(1).t();
_objectPoints = _objectPoints.reshape(1, (int)_objectPoints.total()).t();
cv::Mat objectPointsMean, covObjectPoints;
int Np = imagePointsNormalized.cols;
cv::calcCovarMatrix(_objectPoints, covObjectPoints, objectPointsMean, cv::COVAR_NORMAL | cv::COVAR_COLS);
cv::SVD svd(covObjectPoints);
cv::Mat theR(svd.vt);
if (cv::norm(theR(cv::Rect(2, 0, 1, 2))) < 1e-6)
theR = cv::Mat::eye(3,3, CV_64FC1);
if (cv::determinant(theR) < 0)
theR = -theR;
cv::Mat theT = -theR * objectPointsMean;
cv::Mat X_new = theR * _objectPoints + theT * cv::Mat::ones(1, Np, CV_64FC1);
cv::Mat H = cv::internal::ComputeHomography(imagePointsNormalized, X_new.rowRange(0, 2));
cv::Mat M = cv::Mat::ones(3, X_new.cols, CV_64FC1);
X_new.rowRange(0, 2).copyTo(M.rowRange(0, 2));
cv::Mat mrep = H * M;
cv::divide(mrep, cv::Mat::ones(3,1, CV_64FC1) * mrep.row(2).clone(), mrep);
cv::Mat merr = (mrep.rowRange(0, 2) - imagePointsNormalized).t();
cv::Vec2d std_err;
cv::meanStdDev(merr.reshape(2), cv::noArray(), std_err);
std_err *= sqrt((double)merr.reshape(2).total() / (merr.reshape(2).total() - 1));
cv::Vec2d correct_std_err(0.00516740156010384, 0.00644205331553901);
EXPECT_MAT_NEAR(std_err, correct_std_err, 1e-12);
}
TEST_F(fisheyeTest, InitExtrinsicsIgnoresPointsThatCannotBeUndistorted)
{
// The negative k4 limits the distorted angle to a maximum of about
// 0.98 rad. Image points beyond that radius cannot be undistorted.
const cv::Vec2d focalLength(600.0, 600.0);
const cv::Vec2d principalPoint(640.0, 480.0);
const cv::Vec4d distortion(0.0, 0.0, 0.0, -0.05);
const cv::Matx33d cameraMatrix(focalLength[0], 0.0, principalPoint[0],
0.0, focalLength[1], principalPoint[1],
0.0, 0.0, 1.0);
const cv::Vec3d trueR(0.1, -0.2, 0.05);
const cv::Vec3d trueT(-0.1, 0.05, 2.0);
constexpr int gridSize = 4;
constexpr double gridSpacing = 0.2;
constexpr double undistortionFailureCoordinate = -1e6;
constexpr double poseTolerance = 1e-6;
std::vector<cv::Point3d> objectPoints;
for (int row = 0; row < gridSize; ++row)
{
for (int column = 0; column < gridSize; ++column)
{
objectPoints.emplace_back(column * gridSpacing, row * gridSpacing,
0.0);
}
}
std::vector<cv::Point2d> imagePoints;
cv::fisheye::projectPoints(objectPoints, imagePoints, trueR, trueT,
cameraMatrix, distortion);
const std::vector<cv::Point2d> invalidImagePoints = {
{1340.0, 930.0}, {1300.0, 950.0}, {1260.0, 960.0}};
const std::vector<cv::Point3d> invalidObjectPoints = {
{-0.2, 0.0, 0.0}, {0.0, -0.2, 0.0}, {-0.2, -0.2, 0.0}};
imagePoints.insert(imagePoints.end(), invalidImagePoints.begin(),
invalidImagePoints.end());
objectPoints.insert(objectPoints.end(), invalidObjectPoints.begin(),
invalidObjectPoints.end());
const cv::internal::IntrinsicParams param(focalLength, principalPoint,
distortion);
// InitExtrinsics expects column vectors like CalibrateExtrinsics passes.
const int numberOfPoints = static_cast<int>(imagePoints.size());
const cv::Mat imagePointsMat = cv::Mat(imagePoints).reshape(2, numberOfPoints);
const cv::Mat objectPointsMat =
cv::Mat(objectPoints).reshape(3, numberOfPoints);
const cv::Mat undistorted =
cv::internal::NormalizePixels(imagePointsMat, param);
const size_t numberOfValidPoints = gridSize * gridSize;
for (size_t i = numberOfValidPoints; i < imagePoints.size(); ++i)
{
const cv::Vec2d point = undistorted.at<cv::Vec2d>(static_cast<int>(i));
ASSERT_EQ(point[0], undistortionFailureCoordinate);
ASSERT_EQ(point[1], undistortionFailureCoordinate);
}
cv::Mat rvec;
cv::Mat tvec;
cv::internal::InitExtrinsics(imagePointsMat, objectPointsMat, param, rvec,
tvec);
EXPECT_MAT_NEAR(rvec.reshape(1, 3), cv::Mat(trueR), poseTolerance);
EXPECT_MAT_NEAR(tvec.reshape(1, 3), cv::Mat(trueT), poseTolerance);
}
TEST_F(fisheyeTest, EstimateUncertainties)
{
const int n_images = 34;
std::vector<std::vector<cv::Point2d> > imagePoints(n_images);
std::vector<std::vector<cv::Point3d> > objectPoints(n_images);
const std::string folder =combine(datasets_repository_path, "calib-3_stereo_from_JY");
cv::FileStorage fs_left(combine(folder, "left.xml"), cv::FileStorage::READ);
CV_Assert(fs_left.isOpened());
for(int i = 0; i < n_images; ++i)
fs_left[cv::format("image_%d", i )] >> imagePoints[i];
fs_left.release();
cv::FileStorage fs_object(combine(folder, "object.xml"), cv::FileStorage::READ);
CV_Assert(fs_object.isOpened());
for(int i = 0; i < n_images; ++i)
fs_object[cv::format("image_%d", i )] >> objectPoints[i];
fs_object.release();
int flag = 0;
flag |= cv::CALIB_RECOMPUTE_EXTRINSIC;
flag |= cv::CALIB_CHECK_COND;
flag |= cv::CALIB_FIX_SKEW;
cv::Matx33d theK;
cv::Vec4d theD;
std::vector<cv::Vec3d> rvec;
std::vector<cv::Vec3d> tvec;
cv::fisheye::calibrate(objectPoints, imagePoints, imageSize, theK, theD,
rvec, tvec, flag, cv::TermCriteria(3, 20, 1e-6));
cv::internal::IntrinsicParams param, errors;
cv::Vec2d err_std;
double thresh_cond = 1e6;
int check_cond = 1;
param.Init(cv::Vec2d(theK(0,0), theK(1,1)), cv::Vec2d(theK(0,2), theK(1, 2)), theD);
param.isEstimate = std::vector<uchar>(9, 1);
param.isEstimate[4] = 0;
errors.isEstimate = param.isEstimate;
double rms;
cv::internal::EstimateUncertainties(objectPoints, imagePoints, param, rvec, tvec,
errors, err_std, thresh_cond, check_cond, rms);
EXPECT_MAT_NEAR(errors.f, cv::Vec2d(1.34250246865020720, 1.36037536429654530), 1e-6);
EXPECT_MAT_NEAR(errors.c, cv::Vec2d(0.92070526160049848, 0.84383585812851514), 1e-6);
EXPECT_MAT_NEAR(errors.k, cv::Vec4d(0.0053379581373996041, 0.017389792901700545, 0.022036256089491224, 0.0094714594258908952), 1e-7);
EXPECT_MAT_NEAR(err_std, cv::Vec2d(0.187475975266883, 0.185678953263995), 1e-7);
CV_Assert(fabs(rms - 0.263782587133546) < 1e-10);
CV_Assert(errors.alpha == 0);
}
TEST_F(fisheyeTest, stereoCalibrate)
{
const int n_images = 34;
const std::string folder = combine(datasets_repository_path, "calib-3_stereo_from_JY");
std::vector<std::vector<cv::Point2d> > leftPoints(n_images);
std::vector<std::vector<cv::Point2d> > rightPoints(n_images);
std::vector<std::vector<cv::Point3d> > objectPoints(n_images);
cv::FileStorage fs_left(combine(folder, "left.xml"), cv::FileStorage::READ);
CV_Assert(fs_left.isOpened());
for(int i = 0; i < n_images; ++i)
fs_left[cv::format("image_%d", i )] >> leftPoints[i];
fs_left.release();
cv::FileStorage fs_right(combine(folder, "right.xml"), cv::FileStorage::READ);
CV_Assert(fs_right.isOpened());
for(int i = 0; i < n_images; ++i)
fs_right[cv::format("image_%d", i )] >> rightPoints[i];
fs_right.release();
cv::FileStorage fs_object(combine(folder, "object.xml"), cv::FileStorage::READ);
CV_Assert(fs_object.isOpened());
for(int i = 0; i < n_images; ++i)
fs_object[cv::format("image_%d", i )] >> objectPoints[i];
fs_object.release();
cv::Matx33d K1, K2, theR;
cv::Vec3d theT;
cv::Vec4d D1, D2;
int flag = 0;
flag |= cv::CALIB_RECOMPUTE_EXTRINSIC;
flag |= cv::CALIB_CHECK_COND;
flag |= cv::CALIB_FIX_SKEW;
cv::fisheye::stereoCalibrate(objectPoints, leftPoints, rightPoints,
K1, D1, K2, D2, imageSize, theR, theT, flag,
cv::TermCriteria(3, 12, 0));
cv::Matx33d R_correct( 0.9975587205950972, 0.06953016383322372, 0.006492709911733523,
-0.06956823121068059, 0.9975601387249519, 0.005833595226966235,
-0.006071257768382089, -0.006271040135405457, 0.9999619062167968);
cv::Vec3d T_correct(-0.099402724724121, 0.00270812139265413, 0.00129330292472699);
cv::Matx33d K1_correct (561.195925927249, 0, 621.282400272412,
0, 562.849402029712, 380.555455380889,
0, 0, 1);
cv::Matx33d K2_correct (560.395452535348, 0, 678.971652040359,
0, 561.90171021422, 380.401340535339,
0, 0, 1);
cv::Vec4d D1_correct (-7.44253716539556e-05, -0.00702662033932424, 0.00737569823650885, -0.00342230256441771);
cv::Vec4d D2_correct (-0.0130785435677431, 0.0284434505383497, -0.0360333869900506, 0.0144724062347222);
EXPECT_MAT_NEAR(theR, R_correct, 1e-10);
EXPECT_MAT_NEAR(theT, T_correct, 1e-10);
EXPECT_MAT_NEAR(K1, K1_correct, 1e-10);
EXPECT_MAT_NEAR(K2, K2_correct, 1e-10);
EXPECT_MAT_NEAR(D1, D1_correct, 1e-10);
EXPECT_MAT_NEAR(D2, D2_correct, 1e-10);
}
TEST_F(fisheyeTest, stereoCalibrateFixIntrinsic)
{
const int n_images = 34;
const std::string folder = combine(datasets_repository_path, "calib-3_stereo_from_JY");
std::vector<std::vector<cv::Point2d> > leftPoints(n_images);
std::vector<std::vector<cv::Point2d> > rightPoints(n_images);
std::vector<std::vector<cv::Point3d> > objectPoints(n_images);
cv::FileStorage fs_left(combine(folder, "left.xml"), cv::FileStorage::READ);
CV_Assert(fs_left.isOpened());
for(int i = 0; i < n_images; ++i)
fs_left[cv::format("image_%d", i )] >> leftPoints[i];
fs_left.release();
cv::FileStorage fs_right(combine(folder, "right.xml"), cv::FileStorage::READ);
CV_Assert(fs_right.isOpened());
for(int i = 0; i < n_images; ++i)
fs_right[cv::format("image_%d", i )] >> rightPoints[i];
fs_right.release();
cv::FileStorage fs_object(combine(folder, "object.xml"), cv::FileStorage::READ);
CV_Assert(fs_object.isOpened());
for(int i = 0; i < n_images; ++i)
fs_object[cv::format("image_%d", i )] >> objectPoints[i];
fs_object.release();
cv::Matx33d theR;
cv::Vec3d theT;
int flag = 0;
flag |= cv::CALIB_RECOMPUTE_EXTRINSIC;
flag |= cv::CALIB_CHECK_COND;
flag |= cv::CALIB_FIX_SKEW;
flag |= cv::CALIB_FIX_INTRINSIC;
cv::Matx33d K1 (561.195925927249, 0, 621.282400272412,
0, 562.849402029712, 380.555455380889,
0, 0, 1);
cv::Matx33d K2 (560.395452535348, 0, 678.971652040359,
0, 561.90171021422, 380.401340535339,
0, 0, 1);
cv::Vec4d D1 (-7.44253716539556e-05, -0.00702662033932424, 0.00737569823650885, -0.00342230256441771);
cv::Vec4d D2 (-0.0130785435677431, 0.0284434505383497, -0.0360333869900506, 0.0144724062347222);
cv::fisheye::stereoCalibrate(objectPoints, leftPoints, rightPoints,
K1, D1, K2, D2, imageSize, theR, theT, flag,
cv::TermCriteria(3, 12, 0));
cv::Matx33d R_correct( 0.9975587205950972, 0.06953016383322372, 0.006492709911733523,
-0.06956823121068059, 0.9975601387249519, 0.005833595226966235,
-0.006071257768382089, -0.006271040135405457, 0.9999619062167968);
cv::Vec3d T_correct(-0.099402724724121, 0.00270812139265413, 0.00129330292472699);
EXPECT_MAT_NEAR(theR, R_correct, 1e-10);
EXPECT_MAT_NEAR(theT, T_correct, 1e-10);
}
TEST_F(fisheyeTest, CalibrationWithDifferentPointsNumber)
{
const int n_images = 2;
std::vector<std::vector<cv::Point2d> > imagePoints(n_images);
std::vector<std::vector<cv::Point3d> > objectPoints(n_images);
std::vector<cv::Point2d> imgPoints1(10);
std::vector<cv::Point2d> imgPoints2(15);
std::vector<cv::Point3d> objectPoints1(imgPoints1.size());
std::vector<cv::Point3d> objectPoints2(imgPoints2.size());
for (size_t i = 0; i < imgPoints1.size(); i++)
{
imgPoints1[i] = cv::Point2d((double)i, (double)i);
objectPoints1[i] = cv::Point3d((double)i, (double)i, 10.0);
}
for (size_t i = 0; i < imgPoints2.size(); i++)
{
imgPoints2[i] = cv::Point2d(i + 0.5, i + 0.5);
objectPoints2[i] = cv::Point3d(i + 0.5, i + 0.5, 10.0);
}
imagePoints[0] = imgPoints1;
imagePoints[1] = imgPoints2;
objectPoints[0] = objectPoints1;
objectPoints[1] = objectPoints2;
cv::Matx33d theK = cv::Matx33d::eye();
cv::Vec4d theD;
int flag = 0;
flag |= cv::CALIB_RECOMPUTE_EXTRINSIC;
flag |= cv::CALIB_USE_INTRINSIC_GUESS;
flag |= cv::CALIB_FIX_SKEW;
cv::fisheye::calibrate(objectPoints, imagePoints, cv::Size(100, 100), theK, theD,
cv::noArray(), cv::noArray(), flag, cv::TermCriteria(3, 20, 1e-6));
}
TEST_F(fisheyeTest, NormalMatrixSingularity)
{
// The Jacobian consists of the intrinsic columns followed by six pose
// columns per view. Each view contributes six rows that depend on the
// intrinsics and its own pose only. Additional rows constrain each
// intrinsic parameter. Integer entries and unit pose columns keep all
// products exact such that the singular cases do not depend on rounding.
constexpr int numberOfIntrinsics = 8;
constexpr int numberOfExtrinsics = 6;
constexpr int numberOfViews = 2;
constexpr int couplingModulus = 3;
constexpr int couplingOffset = 1;
const int numberOfRows = numberOfExtrinsics * numberOfViews + numberOfIntrinsics;
const int numberOfColumns = numberOfIntrinsics + numberOfExtrinsics * numberOfViews;
cv::Mat jacobian = cv::Mat::zeros(numberOfRows, numberOfColumns, CV_64FC1);
for (int view = 0; view < numberOfViews; ++view)
{
for (int pose = 0; pose < numberOfExtrinsics; ++pose)
{
const int row = numberOfExtrinsics * view + pose;
jacobian.at<double>(row, numberOfIntrinsics + row) = 1;
for (int intrinsic = 0; intrinsic < numberOfIntrinsics; ++intrinsic)
{
jacobian.at<double>(row, intrinsic) =
(row + intrinsic) % couplingModulus - couplingOffset;
}
}
}
for (int intrinsic = 0; intrinsic < numberOfIntrinsics; ++intrinsic)
{
jacobian.at<double>(numberOfExtrinsics * numberOfViews + intrinsic,
intrinsic) = 1;
}
const auto normalMatrix = [](const cv::Mat& J) { return cv::Mat(J.t() * J); };
EXPECT_FALSE(cv::internal::isNormalMatrixSingular(normalMatrix(jacobian),
numberOfIntrinsics));
// A pose parameter without effect makes the view block singular.
cv::Mat unidentifiablePose = jacobian.clone();
unidentifiablePose.col(numberOfIntrinsics).setTo(0);
EXPECT_TRUE(cv::internal::isNormalMatrixSingular(
normalMatrix(unidentifiablePose), numberOfIntrinsics));
// An intrinsic parameter without effect makes the Schur complement
// singular.
cv::Mat unidentifiableIntrinsic = jacobian.clone();
unidentifiableIntrinsic.col(0).setTo(0);
EXPECT_TRUE(cv::internal::isNormalMatrixSingular(
normalMatrix(unidentifiableIntrinsic), numberOfIntrinsics));
// An intrinsic parameter confined to the six rows of a single view lies in
// the span of its pose columns. Both the intrinsic and the view blocks
// remain regular, while the Schur complement is singular.
cv::Mat coupledIntrinsic = jacobian.clone();
coupledIntrinsic.col(0).rowRange(numberOfExtrinsics, numberOfRows).setTo(0);
coupledIntrinsic.at<double>(0, 0) = 1;
EXPECT_TRUE(cv::internal::isNormalMatrixSingular(
normalMatrix(coupledIntrinsic), numberOfIntrinsics));
}
TEST_F(fisheyeTest, stereoCalibrateWithPerViewTransformations)
{
const int n_images = 34;
const std::string folder = combine(datasets_repository_path, "calib-3_stereo_from_JY");
std::vector<std::vector<cv::Point2d> > leftPoints(n_images);
std::vector<std::vector<cv::Point2d> > rightPoints(n_images);
std::vector<std::vector<cv::Point3d> > objectPoints(n_images);
cv::FileStorage fs_left(combine(folder, "left.xml"), cv::FileStorage::READ);
CV_Assert(fs_left.isOpened());
for(int i = 0; i < n_images; ++i)
fs_left[cv::format("image_%d", i )] >> leftPoints[i];
fs_left.release();
cv::FileStorage fs_right(combine(folder, "right.xml"), cv::FileStorage::READ);
CV_Assert(fs_right.isOpened());
for(int i = 0; i < n_images; ++i)
fs_right[cv::format("image_%d", i )] >> rightPoints[i];
fs_right.release();
cv::FileStorage fs_object(combine(folder, "object.xml"), cv::FileStorage::READ);
CV_Assert(fs_object.isOpened());
for(int i = 0; i < n_images; ++i)
fs_object[cv::format("image_%d", i )] >> objectPoints[i];
fs_object.release();
cv::Matx33d K1, K2, theR;
cv::Vec3d theT;
cv::Vec4d D1, D2;
std::vector<cv::Mat> rvecs, tvecs;
int flag = 0;
flag |= cv::CALIB_RECOMPUTE_EXTRINSIC;
flag |= cv::CALIB_CHECK_COND;
flag |= cv::CALIB_FIX_SKEW;
double rmsErrorStereoCalib = cv::fisheye::stereoCalibrate(objectPoints, leftPoints, rightPoints,
K1, D1, K2, D2, imageSize, theR, theT, rvecs, tvecs, flag,
cv::TermCriteria(3, 12, 0));
std::vector<cv::Point2d> reprojectedImgPts[2] = { std::vector<cv::Point2d>(n_images),
std::vector<cv::Point2d>(n_images) };
size_t totalPoints = 0;
double totalMSError[2] = { 0, 0 };
for( size_t i = 0; i < n_images; i++ )
{
cv::Matx33d viewRotMat1, viewRotMat2;
cv::Vec3d viewT1, viewT2;
cv::Mat rVec;
cv::Rodrigues( rvecs[i], rVec );
rVec.convertTo(viewRotMat1, CV_64F);
tvecs[i].convertTo(viewT1, CV_64F);
viewRotMat2 = theR * viewRotMat1;
cv::Vec3d T2t = theR * viewT1;
viewT2 = T2t + theT;
cv::Vec3d viewRotVec1, viewRotVec2;
cv::Rodrigues(viewRotMat1, viewRotVec1);
cv::Rodrigues(viewRotMat2, viewRotVec2);
double alpha1 = K1(0, 1) / K1(0, 0);
double alpha2 = K2(0, 1) / K2(0, 0);
cv::fisheye::projectPoints(objectPoints[i], reprojectedImgPts[0], viewRotVec1, viewT1, K1, D1, alpha1);
cv::fisheye::projectPoints(objectPoints[i], reprojectedImgPts[1], viewRotVec2, viewT2, K2, D2, alpha2);
double viewMSError[2] = {
cv::norm(leftPoints[i], reprojectedImgPts[0], cv::NORM_L2SQR),
cv::norm(rightPoints[i], reprojectedImgPts[1], cv::NORM_L2SQR)
};
size_t n = objectPoints[i].size();
totalMSError[0] += viewMSError[0];
totalMSError[1] += viewMSError[1];
totalPoints += n;
}
double rmsErrorFromReprojectedImgPts = std::sqrt((totalMSError[0] + totalMSError[1]) / (2 * totalPoints));
cv::Matx33d R_correct( 0.9975587205950972, 0.06953016383322372, 0.006492709911733523,
-0.06956823121068059, 0.9975601387249519, 0.005833595226966235,
-0.006071257768382089, -0.006271040135405457, 0.9999619062167968);
cv::Vec3d T_correct(-0.099402724724121, 0.00270812139265413, 0.00129330292472699);
cv::Matx33d K1_correct (561.195925927249, 0, 621.282400272412,
0, 562.849402029712, 380.555455380889,
0, 0, 1);
cv::Matx33d K2_correct (560.395452535348, 0, 678.971652040359,
0, 561.90171021422, 380.401340535339,
0, 0, 1);
cv::Vec4d D1_correct (-7.44253716539556e-05, -0.00702662033932424, 0.00737569823650885, -0.00342230256441771);
cv::Vec4d D2_correct (-0.0130785435677431, 0.0284434505383497, -0.0360333869900506, 0.0144724062347222);
EXPECT_MAT_NEAR(theR, R_correct, 1e-10);
EXPECT_MAT_NEAR(theT, T_correct, 1e-10);
EXPECT_MAT_NEAR(K1, K1_correct, 1e-10);
EXPECT_MAT_NEAR(K2, K2_correct, 1e-10);
EXPECT_MAT_NEAR(D1, D1_correct, 1e-10);
EXPECT_MAT_NEAR(D2, D2_correct, 1e-10);
EXPECT_NEAR(rmsErrorStereoCalib, rmsErrorFromReprojectedImgPts, 1e-4);
}
TEST_F(fisheyeTest, multiview_calibration)
{
const int n_images = 34;
const std::string folder = combine(datasets_repository_path, "calib-3_stereo_from_JY");
std::vector<std::vector<cv::Point2f> > leftPoints(n_images);
std::vector<std::vector<cv::Point2f> > rightPoints(n_images);
std::vector<std::vector<cv::Point3f> > objectPoints(n_images);
cv::FileStorage fs_left(combine(folder, "left.xml"), cv::FileStorage::READ);
CV_Assert(fs_left.isOpened());
for(int i = 0; i < n_images; ++i)
fs_left[cv::format("image_%d", i )] >> leftPoints[i];
fs_left.release();
cv::FileStorage fs_right(combine(folder, "right.xml"), cv::FileStorage::READ);
CV_Assert(fs_right.isOpened());
for(int i = 0; i < n_images; ++i)
fs_right[cv::format("image_%d", i )] >> rightPoints[i];
fs_right.release();
cv::FileStorage fs_object(combine(folder, "object.xml"), cv::FileStorage::READ);
CV_Assert(fs_object.isOpened());
for(int i = 0; i < n_images; ++i)
fs_object[cv::format("image_%d", i )] >> objectPoints[i];
fs_object.release();
std::vector<std::vector<cv::Mat>> image_points_all(2, std::vector<cv::Mat>(leftPoints.size()));
for (int i = 0; i < (int)leftPoints.size(); i++) {
cv::Mat left_pts(leftPoints[i], false) , right_pts(rightPoints[i], false);
left_pts.copyTo(image_points_all[0][i]);
right_pts.copyTo(image_points_all[1][i]);
}
std::vector<cv::Size> image_sizes(2, imageSize);
cv::Mat visibility_mat = cv::Mat_<uchar>::ones(2, (int)leftPoints.size());
std::vector<cv::Mat> Rs, Ts, Ks, distortions;
std::vector<uchar> models(2, cv::CALIB_MODEL_FISHEYE);
std::vector<int> all_flags(2, cv::CALIB_RECOMPUTE_EXTRINSIC | cv::CALIB_CHECK_COND | cv::CALIB_FIX_SKEW);
calibrateMultiview(objectPoints, image_points_all, image_sizes, visibility_mat,
models, Ks, distortions, Rs, Ts, all_flags);
cv::Matx33d R_correct( 0.9975587205950972, 0.06953016383322372, 0.006492709911733523,
-0.06956823121068059, 0.9975601387249519, 0.005833595226966235,
-0.006071257768382089, -0.006271040135405457, 0.9999619062167968);
cv::Vec3d T_correct(-0.099402724724121, 0.00270812139265413, 0.00129330292472699);
cv::Matx33d K1_correct (561.195925927249, 0, 621.282400272412,
0, 562.849402029712, 380.555455380889,
0, 0, 1);
cv::Matx33d K2_correct (560.395452535348, 0, 678.971652040359,
0, 561.90171021422, 380.401340535339,
0, 0, 1);
cv::Vec4d D1_correct (-7.44253716539556e-05, -0.00702662033932424, 0.00737569823650885, -0.00342230256441771);
cv::Vec4d D2_correct (-0.0130785435677431, 0.0284434505383497, -0.0360333869900506, 0.0144724062347222);
cv::Mat theR;
cv::Rodrigues(Rs[1], theR);
EXPECT_MAT_NEAR(theR, R_correct, 1e-2);
EXPECT_MAT_NEAR(Ts[1], T_correct, 5e-3);
EXPECT_MAT_NEAR(Ks[0], K1_correct, 4);
EXPECT_MAT_NEAR(Ks[1], K2_correct, 5);
EXPECT_MAT_NEAR(distortions[0], D1_correct, 1e-2);
EXPECT_MAT_NEAR(distortions[1], D2_correct, 5e-2);
}
TEST_F(fisheyeTest, cameraRegistrationWithPerViewTransformations)
{
const int n_images = 34;
const std::string folder = combine(datasets_repository_path, "calib-3_stereo_from_JY");
std::vector<std::vector<cv::Point2f> > leftPoints(n_images);
std::vector<std::vector<cv::Point2f> > rightPoints(n_images);
std::vector<std::vector<cv::Point3f> > objectPoints(n_images);
cv::FileStorage fs_left(combine(folder, "left.xml"), cv::FileStorage::READ);
CV_Assert(fs_left.isOpened());
for(int i = 0; i < n_images; ++i)
fs_left[cv::format("image_%d", i )] >> leftPoints[i];
fs_left.release();
cv::FileStorage fs_right(combine(folder, "right.xml"), cv::FileStorage::READ);
CV_Assert(fs_right.isOpened());
for(int i = 0; i < n_images; ++i)
fs_right[cv::format("image_%d", i )] >> rightPoints[i];
fs_right.release();
cv::FileStorage fs_object(combine(folder, "object.xml"), cv::FileStorage::READ);
CV_Assert(fs_object.isOpened());
for(int i = 0; i < n_images; ++i)
fs_object[cv::format("image_%d", i )] >> objectPoints[i];
fs_object.release();
cv::Matx33d K1, K2, theR;
cv::Vec3d theT;
cv::Vec4d D1, D2;
int flag = 0;
flag |= cv::CALIB_RECOMPUTE_EXTRINSIC;
flag |= cv::CALIB_CHECK_COND;
flag |= cv::CALIB_FIX_SKEW;
cv::fisheye::stereoCalibrate(objectPoints, leftPoints, rightPoints,
K1, D1, K2, D2, imageSize, theR, theT,flag, cv::TermCriteria(3, 12, 0));
cv::Mat E, F, perViewErrors;
std::vector<cv::Mat> rvecs, tvecs;
flag = 0;
double rmsErrorRegisterCamera = cv::registerCameras(objectPoints, objectPoints, leftPoints, rightPoints,
K1, D1, CALIB_MODEL_FISHEYE,
K2, D2, CALIB_MODEL_FISHEYE,
theR, theT, E, F, rvecs, tvecs, perViewErrors, flag,
cv::TermCriteria(3, 12, 0));
std::vector<cv::Point2f> reprojectedImgPts[2] = { std::vector<cv::Point2f>(n_images),
std::vector<cv::Point2f>(n_images) };
size_t totalPoints = 0;
double totalMSError[2] = { 0, 0 };
for( size_t i = 0; i < n_images; i++ )
{
cv::Matx33d viewRotMat1, viewRotMat2;
cv::Vec3d viewT1, viewT2;
cv::Mat rVec;
cv::Rodrigues( rvecs[i], rVec );
rVec.convertTo(viewRotMat1, CV_64F);
tvecs[i].convertTo(viewT1, CV_64F);
viewRotMat2 = theR * viewRotMat1;
cv::Vec3d T2t = theR * viewT1;
viewT2 = T2t + theT;
cv::Vec3d viewRotVec1, viewRotVec2;
cv::Rodrigues(viewRotMat1, viewRotVec1);
cv::Rodrigues(viewRotMat2, viewRotVec2);
double alpha1 = K1(0, 1) / K1(0, 0);
double alpha2 = K2(0, 1) / K2(0, 0);
cv::fisheye::projectPoints(objectPoints[i], reprojectedImgPts[0], viewRotVec1, viewT1, K1, D1, alpha1);
cv::fisheye::projectPoints(objectPoints[i], reprojectedImgPts[1], viewRotVec2, viewT2, K2, D2, alpha2);
double viewMSError[2] = {
cv::norm(leftPoints[i], reprojectedImgPts[0], cv::NORM_L2SQR),
cv::norm(rightPoints[i], reprojectedImgPts[1], cv::NORM_L2SQR)
};
size_t n = objectPoints[i].size();
totalMSError[0] += viewMSError[0];
totalMSError[1] += viewMSError[1];
totalPoints += n;
}
double rmsErrorFromReprojectedImgPts = std::sqrt((totalMSError[0] + totalMSError[1]) / (2 * totalPoints));
cv::Matx33d R_correct( 0.9975587205950972, 0.06953016383322372, 0.006492709911733523,
-0.06956823121068059, 0.9975601387249519, 0.005833595226966235,
-0.006071257768382089, -0.006271040135405457, 0.9999619062167968);
cv::Vec3d T_correct(-0.099402724724121, 0.00270812139265413, 0.00129330292472699);
cv::Matx33d K1_correct (561.195925927249, 0, 621.282400272412,
0, 562.849402029712, 380.555455380889,
0, 0, 1);
cv::Matx33d K2_correct (560.395452535348, 0, 678.971652040359,
0, 561.90171021422, 380.401340535339,
0, 0, 1);
cv::Vec4d D1_correct (-7.44253716539556e-05, -0.00702662033932424, 0.00737569823650885, -0.00342230256441771);
cv::Vec4d D2_correct (-0.0130785435677431, 0.0284434505383497, -0.0360333869900506, 0.0144724062347222);
EXPECT_MAT_NEAR(theR, R_correct, 1e-6);
EXPECT_MAT_NEAR(theT, T_correct, 1e-6);
EXPECT_MAT_NEAR(K1, K1_correct, 1e-4);
EXPECT_MAT_NEAR(K2, K2_correct, 1e-4);
EXPECT_MAT_NEAR(D1, D1_correct, 1e-5);
EXPECT_MAT_NEAR(D2, D2_correct, 1e-5);
EXPECT_NEAR(rmsErrorRegisterCamera, rmsErrorFromReprojectedImgPts, 1e-4);
}
}} // namespace
Reference in new issue View Git Blame Copy Permalink