mirrors/opencv
1
0
Fork 0
Code Pull Requests Packages Releases Activity

vendor: OpenCV 5.0.0 snapshot at 40738fb16ceddb5fb3fea747585f7ce6abb0605b

Browse Source
This commit is contained in:
Gitea Mirror Bot
2026-08-22 00:10:33 +08:00
commit f7f077da11
6933 changed files with 2335208 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
modules/calib/test/test_calibration_hand_eye.cpp
+864
View File
@@ -0,0 +1,864 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html.
#include "test_precomp.hpp"
namespace opencv_test { namespace {
static void generatePose(RNG& rng, double min_theta, double max_theta,
double min_tx, double max_tx,
double min_ty, double max_ty,
double min_tz, double max_tz,
Mat& R, Mat& tvec,
bool random_sign)
{
Mat axis(3, 1, CV_64FC1);
for (int i = 0; i < 3; i++)
{
axis.at<double>(i,0) = rng.uniform(-1.0, 1.0);
}
double theta = rng.uniform(min_theta, max_theta);
if (random_sign)
{
theta *= std::copysign(1.0, rng.uniform(-1.0, 1.0));
}
Mat rvec(3, 1, CV_64FC1);
rvec.at<double>(0,0) = theta*axis.at<double>(0,0);
rvec.at<double>(1,0) = theta*axis.at<double>(1,0);
rvec.at<double>(2,0) = theta*axis.at<double>(2,0);
tvec.create(3, 1, CV_64FC1);
tvec.at<double>(0,0) = rng.uniform(min_tx, max_tx);
tvec.at<double>(1,0) = rng.uniform(min_ty, max_ty);
tvec.at<double>(2,0) = rng.uniform(min_tz, max_tz);
if (random_sign)
{
tvec.at<double>(0,0) *= std::copysign(1.0, rng.uniform(-1.0, 1.0));
tvec.at<double>(1,0) *= std::copysign(1.0, rng.uniform(-1.0, 1.0));
tvec.at<double>(2,0) *= std::copysign(1.0, rng.uniform(-1.0, 1.0));
}
cv::Rodrigues(rvec, R);
}
static Mat homogeneousInverse(const Mat& T)
{
CV_Assert( T.rows == 4 && T.cols == 4 );
Mat R = T(Rect(0, 0, 3, 3));
Mat t = T(Rect(3, 0, 1, 3));
Mat Rt = R.t();
Mat tinv = -Rt * t;
Mat Tinv = Mat::eye(4, 4, T.type());
Rt.copyTo(Tinv(Rect(0, 0, 3, 3)));
tinv.copyTo(Tinv(Rect(3, 0, 1, 3)));
return Tinv;
}
static void simulateDataEyeInHand(RNG& rng, int nPoses,
std::vector<Mat> &R_gripper2base, std::vector<Mat> &t_gripper2base,
std::vector<Mat> &R_target2cam, std::vector<Mat> &t_target2cam,
bool noise, Mat& R_cam2gripper, Mat& t_cam2gripper)
{
//to avoid generating values close to zero,
//we use positive range values and randomize the sign
const bool random_sign = true;
generatePose(rng, 10.0*CV_PI/180.0, 50.0*CV_PI/180.0,
0.05, 0.5, 0.05, 0.5, 0.05, 0.5,
R_cam2gripper, t_cam2gripper, random_sign);
Mat R_target2base, t_target2base;
generatePose(rng, 5.0*CV_PI/180.0, 85.0*CV_PI/180.0,
0.5, 3.5, 0.5, 3.5, 0.5, 3.5,
R_target2base, t_target2base, random_sign);
for (int i = 0; i < nPoses; i++)
{
Mat R_gripper2base_, t_gripper2base_;
generatePose(rng, 5.0*CV_PI/180.0, 45.0*CV_PI/180.0,
0.5, 1.5, 0.5, 1.5, 0.5, 1.5,
R_gripper2base_, t_gripper2base_, random_sign);
R_gripper2base.push_back(R_gripper2base_);
t_gripper2base.push_back(t_gripper2base_);
Mat T_cam2gripper = Mat::eye(4, 4, CV_64FC1);
R_cam2gripper.copyTo(T_cam2gripper(Rect(0, 0, 3, 3)));
t_cam2gripper.copyTo(T_cam2gripper(Rect(3, 0, 1, 3)));
Mat T_gripper2base = Mat::eye(4, 4, CV_64FC1);
R_gripper2base_.copyTo(T_gripper2base(Rect(0, 0, 3, 3)));
t_gripper2base_.copyTo(T_gripper2base(Rect(3, 0, 1, 3)));
Mat T_base2cam = homogeneousInverse(T_cam2gripper) * homogeneousInverse(T_gripper2base);
Mat T_target2base = Mat::eye(4, 4, CV_64FC1);
R_target2base.copyTo(T_target2base(Rect(0, 0, 3, 3)));
t_target2base.copyTo(T_target2base(Rect(3, 0, 1, 3)));
Mat T_target2cam = T_base2cam * T_target2base;
if (noise)
{
//Add some noise for the transformation between the target and the camera
Mat R_target2cam_noise = T_target2cam(Rect(0, 0, 3, 3));
Mat rvec_target2cam_noise;
cv::Rodrigues(R_target2cam_noise, rvec_target2cam_noise);
rvec_target2cam_noise.at<double>(0,0) += rng.gaussian(0.002);
rvec_target2cam_noise.at<double>(1,0) += rng.gaussian(0.002);
rvec_target2cam_noise.at<double>(2,0) += rng.gaussian(0.002);
cv::Rodrigues(rvec_target2cam_noise, R_target2cam_noise);
Mat t_target2cam_noise = T_target2cam(Rect(3, 0, 1, 3));
t_target2cam_noise.at<double>(0,0) += rng.gaussian(0.005);
t_target2cam_noise.at<double>(1,0) += rng.gaussian(0.005);
t_target2cam_noise.at<double>(2,0) += rng.gaussian(0.005);
//Add some noise for the transformation between the gripper and the robot base
Mat R_gripper2base_noise = T_gripper2base(Rect(0, 0, 3, 3));
Mat rvec_gripper2base_noise;
cv::Rodrigues(R_gripper2base_noise, rvec_gripper2base_noise);
rvec_gripper2base_noise.at<double>(0,0) += rng.gaussian(0.001);
rvec_gripper2base_noise.at<double>(1,0) += rng.gaussian(0.001);
rvec_gripper2base_noise.at<double>(2,0) += rng.gaussian(0.001);
cv::Rodrigues(rvec_gripper2base_noise, R_gripper2base_noise);
Mat t_gripper2base_noise = T_gripper2base(Rect(3, 0, 1, 3));
t_gripper2base_noise.at<double>(0,0) += rng.gaussian(0.001);
t_gripper2base_noise.at<double>(1,0) += rng.gaussian(0.001);
t_gripper2base_noise.at<double>(2,0) += rng.gaussian(0.001);
}
//Test rvec representation
Mat rvec_target2cam;
cv::Rodrigues(T_target2cam(Rect(0, 0, 3, 3)), rvec_target2cam);
R_target2cam.push_back(rvec_target2cam);
t_target2cam.push_back(T_target2cam(Rect(3, 0, 1, 3)));
}
}
static void simulateDataEyeToHand(RNG& rng, int nPoses,
std::vector<Mat> &R_base2gripper, std::vector<Mat> &t_base2gripper,
std::vector<Mat> &R_target2cam, std::vector<Mat> &t_target2cam,
bool noise, Mat& R_cam2base, Mat& t_cam2base)
{
//to avoid generating values close to zero,
//we use positive range values and randomize the sign
const bool random_sign = true;
generatePose(rng, 10.0*CV_PI/180.0, 50.0*CV_PI/180.0,
0.5, 3.5, 0.5, 3.5, 0.5, 3.5,
R_cam2base, t_cam2base, random_sign);
Mat R_target2gripper, t_target2gripper;
generatePose(rng, 5.0*CV_PI/180.0, 85.0*CV_PI/180.0,
0.05, 0.5, 0.05, 0.5, 0.05, 0.5,
R_target2gripper, t_target2gripper, random_sign);
Mat T_target2gripper = Mat::eye(4, 4, CV_64FC1);
R_target2gripper.copyTo(T_target2gripper(Rect(0, 0, 3, 3)));
t_target2gripper.copyTo(T_target2gripper(Rect(3, 0, 1, 3)));
for (int i = 0; i < nPoses; i++)
{
Mat R_gripper2base_, t_gripper2base_;
generatePose(rng, 5.0*CV_PI/180.0, 45.0*CV_PI/180.0,
0.5, 1.5, 0.5, 1.5, 0.5, 1.5,
R_gripper2base_, t_gripper2base_, random_sign);
Mat R_base2gripper_ = R_gripper2base_.t();
Mat t_base2gripper_ = -R_base2gripper_ * t_gripper2base_;
Mat T_gripper2base = Mat::eye(4, 4, CV_64FC1);
R_gripper2base_.copyTo(T_gripper2base(Rect(0, 0, 3, 3)));
t_gripper2base_.copyTo(T_gripper2base(Rect(3, 0, 1, 3)));
Mat T_cam2base = Mat::eye(4, 4, CV_64FC1);
R_cam2base.copyTo(T_cam2base(Rect(0, 0, 3, 3)));
t_cam2base.copyTo(T_cam2base(Rect(3, 0, 1, 3)));
Mat T_target2cam = homogeneousInverse(T_cam2base) * T_gripper2base * T_target2gripper;
if (noise)
{
//Add some noise for the transformation between the target and the camera
Mat R_target2cam_noise = T_target2cam(Rect(0, 0, 3, 3));
Mat rvec_target2cam_noise;
cv::Rodrigues(R_target2cam_noise, rvec_target2cam_noise);
rvec_target2cam_noise.at<double>(0,0) += rng.gaussian(0.002);
rvec_target2cam_noise.at<double>(1,0) += rng.gaussian(0.002);
rvec_target2cam_noise.at<double>(2,0) += rng.gaussian(0.002);
cv::Rodrigues(rvec_target2cam_noise, R_target2cam_noise);
Mat t_target2cam_noise = T_target2cam(Rect(3, 0, 1, 3));
t_target2cam_noise.at<double>(0,0) += rng.gaussian(0.005);
t_target2cam_noise.at<double>(1,0) += rng.gaussian(0.005);
t_target2cam_noise.at<double>(2,0) += rng.gaussian(0.005);
//Add some noise for the transformation between the robot base and the gripper
Mat rvec_base2gripper_noise;
cv::Rodrigues(R_base2gripper_, rvec_base2gripper_noise);
rvec_base2gripper_noise.at<double>(0,0) += rng.gaussian(0.001);
rvec_base2gripper_noise.at<double>(1,0) += rng.gaussian(0.001);
rvec_base2gripper_noise.at<double>(2,0) += rng.gaussian(0.001);
cv::Rodrigues(rvec_base2gripper_noise, R_base2gripper_);
t_base2gripper_.at<double>(0,0) += rng.gaussian(0.001);
t_base2gripper_.at<double>(1,0) += rng.gaussian(0.001);
t_base2gripper_.at<double>(2,0) += rng.gaussian(0.001);
}
R_base2gripper.push_back(R_base2gripper_);
t_base2gripper.push_back(t_base2gripper_);
//Test rvec representation
Mat rvec_target2cam;
cv::Rodrigues(T_target2cam(Rect(0, 0, 3, 3)), rvec_target2cam);
R_target2cam.push_back(rvec_target2cam);
t_target2cam.push_back(T_target2cam(Rect(3, 0, 1, 3)));
}
}
static std::string getMethodName(HandEyeCalibrationMethod method)
{
std::string method_name = "";
switch (method)
{
case CALIB_HAND_EYE_TSAI:
method_name = "Tsai";
break;
case CALIB_HAND_EYE_PARK:
method_name = "Park";
break;
case CALIB_HAND_EYE_HORAUD:
method_name = "Horaud";
break;
case CALIB_HAND_EYE_ANDREFF:
method_name = "Andreff";
break;
case CALIB_HAND_EYE_DANIILIDIS:
method_name = "Daniilidis";
break;
default:
break;
}
return method_name;
}
static std::string getMethodName(RobotWorldHandEyeCalibrationMethod method)
{
std::string method_name = "";
switch (method)
{
case CALIB_ROBOT_WORLD_HAND_EYE_SHAH:
method_name = "Shah";
break;
case CALIB_ROBOT_WORLD_HAND_EYE_LI:
method_name = "Li";
break;
default:
break;
}
return method_name;
}
static void printStats(const std::string& methodName, const std::vector<double>& rvec_diff, const std::vector<double>& tvec_diff)
{
double max_rvec_diff = *std::max_element(rvec_diff.begin(), rvec_diff.end());
double mean_rvec_diff = std::accumulate(rvec_diff.begin(),
rvec_diff.end(), 0.0) / rvec_diff.size();
double sq_sum_rvec_diff = std::inner_product(rvec_diff.begin(), rvec_diff.end(),
rvec_diff.begin(), 0.0);
double std_rvec_diff = std::sqrt(sq_sum_rvec_diff / rvec_diff.size() - mean_rvec_diff * mean_rvec_diff);
double max_tvec_diff = *std::max_element(tvec_diff.begin(), tvec_diff.end());
double mean_tvec_diff = std::accumulate(tvec_diff.begin(),
tvec_diff.end(), 0.0) / tvec_diff.size();
double sq_sum_tvec_diff = std::inner_product(tvec_diff.begin(), tvec_diff.end(),
tvec_diff.begin(), 0.0);
double std_tvec_diff = std::sqrt(sq_sum_tvec_diff / tvec_diff.size() - mean_tvec_diff * mean_tvec_diff);
std::cout << "Method " << methodName << ":\n"
<< "Max rvec error: " << max_rvec_diff << ", Mean rvec error: " << mean_rvec_diff
<< ", Std rvec error: " << std_rvec_diff << "\n"
<< "Max tvec error: " << max_tvec_diff << ", Mean tvec error: " << mean_tvec_diff
<< ", Std tvec error: " << std_tvec_diff << std::endl;
}
static void loadDataset(std::vector<Mat>& R_target2cam, std::vector<Mat>& t_target2cam,
std::vector<Mat>& R_base2gripper, std::vector<Mat>& t_base2gripper)
{
const std::string camera_poses_filename = findDataFile("cv/robot_world_hand_eye_calibration/cali.txt");
const std::string end_effector_poses = findDataFile("cv/robot_world_hand_eye_calibration/robot_cali.txt");
// Parse camera poses, the pose of the chessboard in the camera frame
{
std::ifstream file(camera_poses_filename);
ASSERT_TRUE(file.is_open());
int ndata = 0;
file >> ndata;
R_target2cam.reserve(ndata);
t_target2cam.reserve(ndata);
std::string image_name;
Matx33d cameraMatrix;
Matx33d R;
Matx31d t;
Matx16d distCoeffs;
Matx13d distCoeffs2;
while (file >> image_name >>
cameraMatrix(0,0) >> cameraMatrix(0,1) >> cameraMatrix(0,2) >>
cameraMatrix(1,0) >> cameraMatrix(1,1) >> cameraMatrix(1,2) >>
cameraMatrix(2,0) >> cameraMatrix(2,1) >> cameraMatrix(2,2) >>
R(0,0) >> R(0,1) >> R(0,2) >>
R(1,0) >> R(1,1) >> R(1,2) >>
R(2,0) >> R(2,1) >> R(2,2) >>
t(0) >> t(1) >> t(2) >>
distCoeffs(0) >> distCoeffs(1) >> distCoeffs(2) >> distCoeffs(3) >> distCoeffs(4) >>
distCoeffs2(0) >> distCoeffs2(1) >> distCoeffs2(2)) {
R_target2cam.push_back(Mat(R));
t_target2cam.push_back(Mat(t));
}
}
// Parse robot poses, the pose of the robot base in the robot hand frame
{
std::ifstream file(end_effector_poses);
ASSERT_TRUE(file.is_open());
int ndata = 0;
file >> ndata;
R_base2gripper.reserve(ndata);
t_base2gripper.reserve(ndata);
Matx33d R;
Matx31d t;
Matx14d last_row;
while (file >>
R(0,0) >> R(0,1) >> R(0,2) >> t(0) >>
R(1,0) >> R(1,1) >> R(1,2) >> t(1) >>
R(2,0) >> R(2,1) >> R(2,2) >> t(2) >>
last_row(0) >> last_row(1) >> last_row(2) >> last_row(3)) {
R_base2gripper.push_back(Mat(R));
t_base2gripper.push_back(Mat(t));
}
}
}
static void loadResults(Matx33d& wRb, Matx31d& wtb, Matx33d& cRg, Matx31d& ctg)
{
const std::string transformations_filename = findDataFile("cv/robot_world_hand_eye_calibration/rwhe_AA_RPI/transformations.txt");
std::ifstream file(transformations_filename);
ASSERT_TRUE(file.is_open());
std::string str;
//Parse X
file >> str;
Matx44d wTb;
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 4; j++)
{
file >> wTb(i,j);
}
}
//Parse Z
file >> str;
int cam_num = 0;
//Parse camera number
file >> cam_num;
Matx44d cTg;
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 4; j++)
{
file >> cTg(i,j);
}
}
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
wRb(i,j) = wTb(i,j);
cRg(i,j) = cTg(i,j);
}
wtb(i) = wTb(i,3);
ctg(i) = cTg(i,3);
}
}
class CV_CalibrateHandEyeTest : public cvtest::BaseTest
{
public:
CV_CalibrateHandEyeTest(bool eyeToHand) : eyeToHandConfig(eyeToHand) {
eps_rvec[CALIB_HAND_EYE_TSAI] = 1.0e-8;
eps_rvec[CALIB_HAND_EYE_PARK] = 1.0e-8;
eps_rvec[CALIB_HAND_EYE_HORAUD] = 1.0e-8;
eps_rvec[CALIB_HAND_EYE_ANDREFF] = 1.0e-8;
eps_rvec[CALIB_HAND_EYE_DANIILIDIS] = 1.0e-8;
eps_tvec[CALIB_HAND_EYE_TSAI] = 1.0e-8;
eps_tvec[CALIB_HAND_EYE_PARK] = 1.0e-8;
eps_tvec[CALIB_HAND_EYE_HORAUD] = 1.0e-8;
eps_tvec[CALIB_HAND_EYE_ANDREFF] = 1.0e-8;
eps_tvec[CALIB_HAND_EYE_DANIILIDIS] = 1.0e-8;
eps_rvec_noise[CALIB_HAND_EYE_TSAI] = 2.0e-2;
eps_rvec_noise[CALIB_HAND_EYE_PARK] = 2.0e-2;
eps_rvec_noise[CALIB_HAND_EYE_HORAUD] = 2.0e-2;
eps_rvec_noise[CALIB_HAND_EYE_ANDREFF] = 1.0e-2;
eps_rvec_noise[CALIB_HAND_EYE_DANIILIDIS] = 1.0e-2;
eps_tvec_noise[CALIB_HAND_EYE_TSAI] = 7.0e-2;
eps_tvec_noise[CALIB_HAND_EYE_PARK] = 7.0e-2;
eps_tvec_noise[CALIB_HAND_EYE_HORAUD] = 7.0e-2;
if (eyeToHandConfig)
{
eps_tvec_noise[CALIB_HAND_EYE_ANDREFF] = 7.0e-2;
}
else
{
eps_tvec_noise[CALIB_HAND_EYE_ANDREFF] = 5.0e-2;
}
eps_tvec_noise[CALIB_HAND_EYE_DANIILIDIS] = 5.0e-2;
}
protected:
virtual void run(int);
bool eyeToHandConfig;
double eps_rvec[5];
double eps_tvec[5];
double eps_rvec_noise[5];
double eps_tvec_noise[5];
};
void CV_CalibrateHandEyeTest::run(int)
{
ts->set_failed_test_info(cvtest::TS::OK);
RNG& rng = cv::theRNG();
std::vector<std::vector<double> > vec_rvec_diff(5);
std::vector<std::vector<double> > vec_tvec_diff(5);
std::vector<std::vector<double> > vec_rvec_diff_noise(5);
std::vector<std::vector<double> > vec_tvec_diff_noise(5);
std::vector<HandEyeCalibrationMethod> methods;
methods.push_back(CALIB_HAND_EYE_TSAI);
methods.push_back(CALIB_HAND_EYE_PARK);
methods.push_back(CALIB_HAND_EYE_HORAUD);
methods.push_back(CALIB_HAND_EYE_ANDREFF);
methods.push_back(CALIB_HAND_EYE_DANIILIDIS);
const int nTests = 100;
for (int i = 0; i < nTests; i++)
{
const int nPoses = 10;
if (eyeToHandConfig)
{
{
//No noise
std::vector<Mat> R_base2gripper, t_base2gripper;
std::vector<Mat> R_target2cam, t_target2cam;
Mat R_cam2base_true, t_cam2base_true;
const bool noise = false;
simulateDataEyeToHand(rng, nPoses, R_base2gripper, t_base2gripper, R_target2cam, t_target2cam, noise,
R_cam2base_true, t_cam2base_true);
for (size_t idx = 0; idx < methods.size(); idx++)
{
Mat rvec_cam2base_true;
cv::Rodrigues(R_cam2base_true, rvec_cam2base_true);
Mat R_cam2base_est, t_cam2base_est;
calibrateHandEye(R_base2gripper, t_base2gripper, R_target2cam, t_target2cam, R_cam2base_est, t_cam2base_est, methods[idx]);
Mat rvec_cam2base_est;
cv::Rodrigues(R_cam2base_est, rvec_cam2base_est);
double rvecDiff = cvtest::norm(rvec_cam2base_true, rvec_cam2base_est, NORM_L2);
double tvecDiff = cvtest::norm(t_cam2base_true, t_cam2base_est, NORM_L2);
vec_rvec_diff[idx].push_back(rvecDiff);
vec_tvec_diff[idx].push_back(tvecDiff);
const double epsilon_rvec = eps_rvec[idx];
const double epsilon_tvec = eps_tvec[idx];
//Maybe a better accuracy test would be to compare the mean and std errors with some thresholds?
if (rvecDiff > epsilon_rvec || tvecDiff > epsilon_tvec)
{
ts->printf(cvtest::TS::LOG, "Invalid accuracy (no noise) for method: %s, rvecDiff: %f, epsilon_rvec: %f, tvecDiff: %f, epsilon_tvec: %f\n",
getMethodName(methods[idx]).c_str(), rvecDiff, epsilon_rvec, tvecDiff, epsilon_tvec);
ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
}
}
}
{
//Gaussian noise on transformations between calibration target frame and camera frame and between robot base and gripper frames
std::vector<Mat> R_base2gripper, t_base2gripper;
std::vector<Mat> R_target2cam, t_target2cam;
Mat R_cam2base_true, t_cam2base_true;
const bool noise = true;
simulateDataEyeToHand(rng, nPoses, R_base2gripper, t_base2gripper, R_target2cam, t_target2cam, noise,
R_cam2base_true, t_cam2base_true);
for (size_t idx = 0; idx < methods.size(); idx++)
{
Mat rvec_cam2base_true;
cv::Rodrigues(R_cam2base_true, rvec_cam2base_true);
Mat R_cam2base_est, t_cam2base_est;
calibrateHandEye(R_base2gripper, t_base2gripper, R_target2cam, t_target2cam, R_cam2base_est, t_cam2base_est, methods[idx]);
Mat rvec_cam2base_est;
cv::Rodrigues(R_cam2base_est, rvec_cam2base_est);
double rvecDiff = cvtest::norm(rvec_cam2base_true, rvec_cam2base_est, NORM_L2);
double tvecDiff = cvtest::norm(t_cam2base_true, t_cam2base_est, NORM_L2);
vec_rvec_diff_noise[idx].push_back(rvecDiff);
vec_tvec_diff_noise[idx].push_back(tvecDiff);
const double epsilon_rvec = eps_rvec_noise[idx];
const double epsilon_tvec = eps_tvec_noise[idx];
//Maybe a better accuracy test would be to compare the mean and std errors with some thresholds?
if (rvecDiff > epsilon_rvec || tvecDiff > epsilon_tvec)
{
ts->printf(cvtest::TS::LOG, "Invalid accuracy (noise) for method: %s, rvecDiff: %f, epsilon_rvec: %f, tvecDiff: %f, epsilon_tvec: %f\n",
getMethodName(methods[idx]).c_str(), rvecDiff, epsilon_rvec, tvecDiff, epsilon_tvec);
ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
}
}
}
}
else
{
{
//No noise
std::vector<Mat> R_gripper2base, t_gripper2base;
std::vector<Mat> R_target2cam, t_target2cam;
Mat R_cam2gripper_true, t_cam2gripper_true;
const bool noise = false;
simulateDataEyeInHand(rng, nPoses, R_gripper2base, t_gripper2base, R_target2cam, t_target2cam, noise,
R_cam2gripper_true, t_cam2gripper_true);
for (size_t idx = 0; idx < methods.size(); idx++)
{
Mat rvec_cam2gripper_true;
cv::Rodrigues(R_cam2gripper_true, rvec_cam2gripper_true);
Mat R_cam2gripper_est, t_cam2gripper_est;
calibrateHandEye(R_gripper2base, t_gripper2base, R_target2cam, t_target2cam, R_cam2gripper_est, t_cam2gripper_est, methods[idx]);
Mat rvec_cam2gripper_est;
cv::Rodrigues(R_cam2gripper_est, rvec_cam2gripper_est);
double rvecDiff = cvtest::norm(rvec_cam2gripper_true, rvec_cam2gripper_est, NORM_L2);
double tvecDiff = cvtest::norm(t_cam2gripper_true, t_cam2gripper_est, NORM_L2);
vec_rvec_diff[idx].push_back(rvecDiff);
vec_tvec_diff[idx].push_back(tvecDiff);
const double epsilon_rvec = eps_rvec[idx];
const double epsilon_tvec = eps_tvec[idx];
//Maybe a better accuracy test would be to compare the mean and std errors with some thresholds?
if (rvecDiff > epsilon_rvec || tvecDiff > epsilon_tvec)
{
ts->printf(cvtest::TS::LOG, "Invalid accuracy (no noise) for method: %s, rvecDiff: %f, epsilon_rvec: %f, tvecDiff: %f, epsilon_tvec: %f\n",
getMethodName(methods[idx]).c_str(), rvecDiff, epsilon_rvec, tvecDiff, epsilon_tvec);
ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
}
}
}
{
//Gaussian noise on transformations between calibration target frame and camera frame and between gripper and robot base frames
std::vector<Mat> R_gripper2base, t_gripper2base;
std::vector<Mat> R_target2cam, t_target2cam;
Mat R_cam2gripper_true, t_cam2gripper_true;
const bool noise = true;
simulateDataEyeInHand(rng, nPoses, R_gripper2base, t_gripper2base, R_target2cam, t_target2cam, noise,
R_cam2gripper_true, t_cam2gripper_true);
for (size_t idx = 0; idx < methods.size(); idx++)
{
Mat rvec_cam2gripper_true;
cv::Rodrigues(R_cam2gripper_true, rvec_cam2gripper_true);
Mat R_cam2gripper_est, t_cam2gripper_est;
calibrateHandEye(R_gripper2base, t_gripper2base, R_target2cam, t_target2cam, R_cam2gripper_est, t_cam2gripper_est, methods[idx]);
Mat rvec_cam2gripper_est;
cv::Rodrigues(R_cam2gripper_est, rvec_cam2gripper_est);
double rvecDiff = cvtest::norm(rvec_cam2gripper_true, rvec_cam2gripper_est, NORM_L2);
double tvecDiff = cvtest::norm(t_cam2gripper_true, t_cam2gripper_est, NORM_L2);
vec_rvec_diff_noise[idx].push_back(rvecDiff);
vec_tvec_diff_noise[idx].push_back(tvecDiff);
const double epsilon_rvec = eps_rvec_noise[idx];
const double epsilon_tvec = eps_tvec_noise[idx];
//Maybe a better accuracy test would be to compare the mean and std errors with some thresholds?
if (rvecDiff > epsilon_rvec || tvecDiff > epsilon_tvec)
{
ts->printf(cvtest::TS::LOG, "Invalid accuracy (noise) for method: %s, rvecDiff: %f, epsilon_rvec: %f, tvecDiff: %f, epsilon_tvec: %f\n",
getMethodName(methods[idx]).c_str(), rvecDiff, epsilon_rvec, tvecDiff, epsilon_tvec);
ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
}
}
}
}
}
for (size_t idx = 0; idx < methods.size(); idx++)
{
std::cout << std::endl;
printStats(getMethodName(methods[idx]), vec_rvec_diff[idx], vec_tvec_diff[idx]);
printStats("(noise) " + getMethodName(methods[idx]), vec_rvec_diff_noise[idx], vec_tvec_diff_noise[idx]);
}
}
///////////////////////////////////////////////////////////////////////////////////////////////////
TEST(Calib3d_CalibrateHandEye, regression_eye_in_hand)
{
//Eye-in-Hand configuration (camera mounted on the robot end-effector observing a static calibration pattern)
const bool eyeToHand = false;
CV_CalibrateHandEyeTest test(eyeToHand);
test.safe_run();
}
TEST(Calib3d_CalibrateHandEye, regression_eye_to_hand)
{
//Eye-to-Hand configuration (static camera observing a calibration pattern mounted on the robot end-effector)
const bool eyeToHand = true;
CV_CalibrateHandEyeTest test(eyeToHand);
test.safe_run();
}
TEST(Calib3d_CalibrateHandEye, regression_17986)
{
std::vector<Mat> R_target2cam, t_target2cam;
// Dataset contains transformation from base to gripper frame since it contains data for AX = ZB calibration problem
std::vector<Mat> R_base2gripper, t_base2gripper;
loadDataset(R_target2cam, t_target2cam, R_base2gripper, t_base2gripper);
std::vector<HandEyeCalibrationMethod> methods = {CALIB_HAND_EYE_TSAI,
CALIB_HAND_EYE_PARK,
CALIB_HAND_EYE_HORAUD,
CALIB_HAND_EYE_ANDREFF,
CALIB_HAND_EYE_DANIILIDIS};
for (auto method : methods) {
SCOPED_TRACE(cv::format("method=%s", getMethodName(method).c_str()));
Matx33d R_cam2base_est;
Matx31d t_cam2base_est;
calibrateHandEye(R_base2gripper, t_base2gripper, R_target2cam, t_target2cam, R_cam2base_est, t_cam2base_est, method);
EXPECT_TRUE(checkRange(R_cam2base_est));
EXPECT_TRUE(checkRange(t_cam2base_est));
}
}
TEST(Calib3d_CalibrateRobotWorldHandEye, regression)
{
std::vector<Mat> R_world2cam, t_worldt2cam;
std::vector<Mat> R_base2gripper, t_base2gripper;
loadDataset(R_world2cam, t_worldt2cam, R_base2gripper, t_base2gripper);
std::vector<Mat> rvec_R_world2cam;
rvec_R_world2cam.reserve(R_world2cam.size());
for (size_t i = 0; i < R_world2cam.size(); i++)
{
Mat rvec;
cv::Rodrigues(R_world2cam[i], rvec);
rvec_R_world2cam.push_back(rvec);
}
std::vector<RobotWorldHandEyeCalibrationMethod> methods = {CALIB_ROBOT_WORLD_HAND_EYE_SHAH,
CALIB_ROBOT_WORLD_HAND_EYE_LI};
Matx33d wRb, cRg;
Matx31d wtb, ctg;
loadResults(wRb, wtb, cRg, ctg);
for (auto method : methods) {
SCOPED_TRACE(cv::format("method=%s", getMethodName(method).c_str()));
Matx33d wRb_est, cRg_est;
Matx31d wtb_est, ctg_est;
calibrateRobotWorldHandEye(rvec_R_world2cam, t_worldt2cam, R_base2gripper, t_base2gripper,
wRb_est, wtb_est, cRg_est, ctg_est, method);
EXPECT_TRUE(checkRange(wRb_est));
EXPECT_TRUE(checkRange(wtb_est));
EXPECT_TRUE(checkRange(cRg_est));
EXPECT_TRUE(checkRange(ctg_est));
//Arbitrary thresholds
const double rotation_threshold = 1.0; //1deg
const double translation_threshold = 50.0; //5cm
//X
//rotation error
Matx33d wRw_est = wRb * wRb_est.t();
Matx31d rvec_wRw_est;
cv::Rodrigues(wRw_est, rvec_wRw_est);
double X_rotation_error = cv::norm(rvec_wRw_est)*180/CV_PI;
//translation error
double X_t_error = cv::norm(wtb_est - wtb);
SCOPED_TRACE(cv::format("X rotation error=%f", X_rotation_error));
SCOPED_TRACE(cv::format("X translation error=%f", X_t_error));
EXPECT_TRUE(X_rotation_error < rotation_threshold);
EXPECT_TRUE(X_t_error < translation_threshold);
//Z
//rotation error
Matx33d cRc_est = cRg * cRg_est.t();
Matx31d rvec_cMc_est;
cv::Rodrigues(cRc_est, rvec_cMc_est);
double Z_rotation_error = cv::norm(rvec_cMc_est)*180/CV_PI;
//translation error
double Z_t_error = cv::norm(ctg_est - ctg);
SCOPED_TRACE(cv::format("Z rotation error=%f", Z_rotation_error));
SCOPED_TRACE(cv::format("Z translation error=%f", Z_t_error));
EXPECT_TRUE(Z_rotation_error < rotation_threshold);
EXPECT_TRUE(Z_t_error < translation_threshold);
}
}
TEST(Calib3d_CalibrateHandEye, regression_24871)
{
std::vector<Mat> R_target2cam, t_target2cam;
std::vector<Mat> R_gripper2base, t_gripper2base;
Mat T_true_cam2gripper;
T_true_cam2gripper = (cv::Mat_<double>(4, 4) << 0, 0, -1, 0.1,
1, 0, 0, 0.2,
0, -1, 0, 0.3,
0, 0, 0, 1);
R_target2cam.push_back((cv::Mat_<double>(3, 3) <<
0.04964505493834381, 0.5136826827431226, 0.8565427426404346,
-0.3923117691818854, 0.7987004864191318, -0.4562554205214679,
-0.9184916136152514, -0.3133809733274676, 0.2411752915926112));
t_target2cam.push_back((cv::Mat_<double>(3, 1) <<
-1.588728904724121,
0.07843752950429916,
-1.002813339233398));
R_gripper2base.push_back((cv::Mat_<double>(3, 3) <<
-0.4143743581399177, -0.6105088815982459, -0.6749613298595637,
-0.1598851232573451, -0.6812625208693498, 0.71436554019614,
-0.895952364066927, 0.4039310376145889, 0.1846864320259794));
t_gripper2base.push_back((cv::Mat_<double>(3, 1) <<
-1.249274406461827,
-1.916570771580279,
2.005069553422765));
R_target2cam.push_back((cv::Mat_<double>(3, 3) <<
-0.3048000068139332, 0.6971848192711539, 0.6488684640388026,
-0.9377589344241749, -0.3387497187353627, -0.07652979135179161,
0.1664486009369332, -0.6318084803439735, 0.7570422097951847));
t_target2cam.push_back((cv::Mat_<double>(3, 1) <<
-1.906493663787842,
-0.07281044125556946,
0.6088893413543701));
R_gripper2base.push_back((cv::Mat_<double>(3, 3) <<
0.7262439860936567, -0.201662933718935, -0.6571923111439066,
-0.4640017362244384, -0.8491808316335328, -0.2521791108852766,
-0.5072199339965884, 0.4880819361030014, -0.7102844234575628));
t_gripper2base.push_back((cv::Mat_<double>(3, 1) <<
-0.7375172846804027,
-2.579760910816792,
1.336561572270101));
R_target2cam.push_back((cv::Mat_<double>(3, 3) <<
-0.590234879685801, -0.7051138289845309, -0.3929850823848928,
0.6017371069678565, -0.7088332765096816, 0.3680595606834615,
-0.5380847896941907, -0.01923211603859842, 0.8426712792141644));
t_target2cam.push_back((cv::Mat_<double>(3, 1) <<
-0.9809040427207947,
-0.2707894444465637,
-0.2577074766159058));
R_gripper2base.push_back((cv::Mat_<double>(3, 3) <<
0.2541996332132083, 0.6186461729765909, 0.7434106934499181,
0.2194912986375709, 0.711701808961156, -0.6673111005698995,
-0.9419161938817396, 0.3328024155303503, 0.04512688689130734));
t_gripper2base.push_back((cv::Mat_<double>(3, 1) <<
-1.040123533893404,
-0.1303773962721222,
1.068029475621886));
R_target2cam.push_back((cv::Mat_<double>(3, 3) <<
0.7643667483125168, -0.08523002870239212, 0.63912386614923,
-0.2583463792779588, 0.8676987164647345, 0.424683512464778,
-0.5907627462764713, -0.489729292214425, 0.6412211770980741));
t_target2cam.push_back((cv::Mat_<double>(3, 1) <<
-1.58987033367157,
-1.924914002418518,
-0.3109001517295837));
R_gripper2base.push_back((cv::Mat_<double>(3, 3) <<
0.116348305340805, -0.9917998080681939, 0.0528792261688552,
-0.2760629007224059, 0.01884966191381591, 0.9609547154213178,
-0.9540714578526358, -0.1264034452126562, -0.2716060057313114));
t_gripper2base.push_back((cv::Mat_<double>(3, 1) <<
-2.551899142554571,
-2.986937398237611,
1.317613923218308));
Mat R_true_cam2gripper;
Mat t_true_cam2gripper;
R_true_cam2gripper = T_true_cam2gripper(Rect(0, 0, 3, 3));
t_true_cam2gripper = T_true_cam2gripper(Rect(3, 0, 1, 3));
std::vector<HandEyeCalibrationMethod> methods = {CALIB_HAND_EYE_TSAI,
CALIB_HAND_EYE_PARK,
CALIB_HAND_EYE_HORAUD,
CALIB_HAND_EYE_ANDREFF,
CALIB_HAND_EYE_DANIILIDIS};
for (auto method : methods) {
SCOPED_TRACE(cv::format("method=%s", getMethodName(method).c_str()));
Matx33d R_cam2gripper_est;
Matx31d t_cam2gripper_est;
calibrateHandEye(R_gripper2base, t_gripper2base, R_target2cam, t_target2cam, R_cam2gripper_est, t_cam2gripper_est, method);
EXPECT_TRUE(cv::norm(R_cam2gripper_est - R_true_cam2gripper) < 1e-9);
EXPECT_TRUE(cv::norm(t_cam2gripper_est - t_true_cam2gripper) < 1e-9);
}
}
}} // namespace
modules/calib/test/test_cameracalibration.cpp
+1901
View File
File diff suppressed because it is too large Load Diff
modules/calib/test/test_cameracalibration_artificial.cpp
+427
View File
@@ -0,0 +1,427 @@
/*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, 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 "test_chessboardgenerator.hpp"
#include "opencv2/objdetect.hpp"
namespace opencv_test { namespace {
//template<class T> ostream& operator<<(ostream& out, const Mat_<T>& mat)
//{
// for(Mat_<T>::const_iterator pos = mat.begin(), end = mat.end(); pos != end; ++pos)
// out << *pos << " ";
// return out;
//}
//ostream& operator<<(ostream& out, const Mat& mat) { return out << Mat_<double>(mat); }
Mat calcRvec(const vector<Point3f>& points, const Size& cornerSize)
{
Point3f p00 = points[0];
Point3f p10 = points[1];
Point3f p01 = points[cornerSize.width];
Vec3d ex(p10.x - p00.x, p10.y - p00.y, p10.z - p00.z);
Vec3d ey(p01.x - p00.x, p01.y - p00.y, p01.z - p00.z);
Vec3d ez = ex.cross(ey);
Mat rot(3, 3, CV_64F);
*rot.ptr<Vec3d>(0) = ex;
*rot.ptr<Vec3d>(1) = ey;
*rot.ptr<Vec3d>(2) = ez * (1.0/cv::norm(ez)); // TODO cvtest
Mat res;
Rodrigues(rot.t(), res);
return res.reshape(1, 1);
}
class CV_CalibrateCameraArtificialTest : public cvtest::BaseTest
{
public:
CV_CalibrateCameraArtificialTest() :
r(0)
{
}
~CV_CalibrateCameraArtificialTest() {}
protected:
int r;
const static int JUST_FIND_CORNERS = 0;
const static int USE_CORNERS_SUBPIX = 1;
const static int USE_4QUAD_CORNERS = 2;
const static int ARTIFICIAL_CORNERS = 4;
bool checkErr(double a, double a0, double eps, double delta)
{
return fabs(a - a0) > eps * (fabs(a0) + delta);
}
void compareCameraMatrs(const Mat_<double>& camMat, const Mat& camMat_est)
{
if ( camMat_est.at<double>(0, 1) != 0 || camMat_est.at<double>(1, 0) != 0 ||
camMat_est.at<double>(2, 0) != 0 || camMat_est.at<double>(2, 1) != 0 ||
camMat_est.at<double>(2, 2) != 1)
{
ts->printf( cvtest::TS::LOG, "Bad shape of camera matrix returned \n");
ts->set_failed_test_info(cvtest::TS::FAIL_MISMATCH);
}
double fx_e = camMat_est.at<double>(0, 0), fy_e = camMat_est.at<double>(1, 1);
double cx_e = camMat_est.at<double>(0, 2), cy_e = camMat_est.at<double>(1, 2);
double fx = camMat(0, 0), fy = camMat(1, 1), cx = camMat(0, 2), cy = camMat(1, 2);
const double eps = 1e-2;
const double dlt = 1e-5;
bool fail = checkErr(fx_e, fx, eps, dlt) || checkErr(fy_e, fy, eps, dlt) ||
checkErr(cx_e, cx, eps, dlt) || checkErr(cy_e, cy, eps, dlt);
if (fail)
{
ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
}
ts->printf( cvtest::TS::LOG, "%d) Expected [Fx Fy Cx Cy] = [%.3f %.3f %.3f %.3f]\n", r, fx, fy, cx, cy);
ts->printf( cvtest::TS::LOG, "%d) Estimated [Fx Fy Cx Cy] = [%.3f %.3f %.3f %.3f]\n", r, fx_e, fy_e, cx_e, cy_e);
}
void compareDistCoeffs(const Mat_<double>& distCoeffs, const Mat& distCoeffs_est)
{
const double *dt_e = distCoeffs_est.ptr<double>();
double k1_e = dt_e[0], k2_e = dt_e[1], k3_e = dt_e[4];
double p1_e = dt_e[2], p2_e = dt_e[3];
double k1 = distCoeffs(0, 0), k2 = distCoeffs(0, 1), k3 = distCoeffs(0, 4);
double p1 = distCoeffs(0, 2), p2 = distCoeffs(0, 3);
const double eps = 5e-2;
const double dlt = 1e-3;
const double eps_k3 = 5;
const double dlt_k3 = 1e-3;
bool fail = checkErr(k1_e, k1, eps, dlt) || checkErr(k2_e, k2, eps, dlt) || checkErr(k3_e, k3, eps_k3, dlt_k3) ||
checkErr(p1_e, p1, eps, dlt) || checkErr(p2_e, p2, eps, dlt);
if (fail)
{
// commented according to vp123's recommendation. TODO - improve accuracy
//ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY); ss
}
ts->printf( cvtest::TS::LOG, "%d) DistCoeff exp=(%.2f, %.2f, %.4f, %.4f %.2f)\n", r, k1, k2, p1, p2, k3);
ts->printf( cvtest::TS::LOG, "%d) DistCoeff est=(%.2f, %.2f, %.4f, %.4f %.2f)\n", r, k1_e, k2_e, p1_e, p2_e, k3_e);
ts->printf( cvtest::TS::LOG, "%d) AbsError = [%.5f %.5f %.5f %.5f %.5f]\n", r, fabs(k1-k1_e), fabs(k2-k2_e), fabs(p1-p1_e), fabs(p2-p2_e), fabs(k3-k3_e));
}
void compareShiftVecs(const vector<Mat>& tvecs, const vector<Mat>& tvecs_est)
{
const double eps = 1e-2;
const double dlt = 1e-4;
int err_count = 0;
const int errMsgNum = 4;
for(size_t i = 0; i < tvecs.size(); ++i)
{
const Point3d& tvec = *tvecs[i].ptr<Point3d>();
const Point3d& tvec_est = *tvecs_est[i].ptr<Point3d>();
double n1 = cv::norm(tvec_est - tvec); // TODO cvtest
double n2 = cv::norm(tvec); // TODO cvtest
if (n1 > eps* (n2 + dlt))
{
if (err_count++ < errMsgNum)
{
if (err_count == errMsgNum)
ts->printf( cvtest::TS::LOG, "%d) ...\n", r);
else
{
ts->printf( cvtest::TS::LOG, "%d) Bad accuracy in returned tvecs. Index = %d\n", r, i);
ts->printf( cvtest::TS::LOG, "%d) norm(tvec_est - tvec) = %f, norm(tvec_exp) = %f \n", r, n1, n2);
}
}
ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
}
}
}
void compareRotationVecs(const vector<Mat>& rvecs, const vector<Mat>& rvecs_est)
{
const double eps = 2e-2;
const double dlt = 1e-4;
Mat rmat, rmat_est;
int err_count = 0;
const int errMsgNum = 4;
for(size_t i = 0; i < rvecs.size(); ++i)
{
Rodrigues(rvecs[i], rmat);
Rodrigues(rvecs_est[i], rmat_est);
if (cvtest::norm(rmat_est, rmat, NORM_L2) > eps* (cvtest::norm(rmat, NORM_L2) + dlt))
{
if (err_count++ < errMsgNum)
{
if (err_count == errMsgNum)
ts->printf( cvtest::TS::LOG, "%d) ...\n", r);
else
{
ts->printf( cvtest::TS::LOG, "%d) Bad accuracy in returned rvecs (rotation matrs). Index = %d\n", r, i);
ts->printf( cvtest::TS::LOG, "%d) norm(rot_mat_est - rot_mat_exp) = %f, norm(rot_mat_exp) = %f \n", r,
cvtest::norm(rmat_est, rmat, NORM_L2), cvtest::norm(rmat, NORM_L2));
}
}
ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
}
}
}
double reprojectErrorWithoutIntrinsics(const vector<Point3f>& cb3d, const vector<Mat>& _rvecs_exp, const vector<Mat>& _tvecs_exp,
const vector<Mat>& rvecs_est, const vector<Mat>& tvecs_est)
{
const static Mat eye33 = Mat::eye(3, 3, CV_64F);
const static Mat zero15 = Mat::zeros(1, 5, CV_64F);
Mat _chessboard3D(cb3d);
vector<Point2f> uv_exp, uv_est;
double res = 0;
for(size_t i = 0; i < rvecs_exp.size(); ++i)
{
projectPoints(_chessboard3D, _rvecs_exp[i], _tvecs_exp[i], eye33, zero15, uv_exp);
projectPoints(_chessboard3D, rvecs_est[i], tvecs_est[i], eye33, zero15, uv_est);
for(size_t j = 0; j < cb3d.size(); ++j)
res += cv::norm(uv_exp[i] - uv_est[i]); // TODO cvtest
}
return res;
}
Size2f sqSile;
vector<Point3f> chessboard3D;
vector<Mat> boards, rvecs_exp, tvecs_exp, rvecs_spnp, tvecs_spnp;
vector< vector<Point3f> > objectPoints;
vector< vector<Point2f> > imagePoints_art;
vector< vector<Point2f> > imagePoints_findCb;
void prepareForTest(const Mat& bg, const Mat& camMat, const Mat& distCoeffs, size_t brdsNum, const ChessBoardGenerator& cbg)
{
sqSile = Size2f(1.f, 1.f);
Size cornersSize = cbg.cornersSize();
chessboard3D.clear();
for(int j = 0; j < cornersSize.height; ++j)
for(int i = 0; i < cornersSize.width; ++i)
chessboard3D.push_back(Point3f(sqSile.width * i, sqSile.height * j, 0));
boards.resize(brdsNum);
rvecs_exp.resize(brdsNum);
tvecs_exp.resize(brdsNum);
objectPoints.clear();
objectPoints.resize(brdsNum, chessboard3D);
imagePoints_art.clear();
imagePoints_findCb.clear();
vector<Point2f> corners_art, corners_fcb;
for(size_t i = 0; i < brdsNum; ++i)
{
for(;;)
{
boards[i] = cbg(bg, camMat, distCoeffs, sqSile, corners_art);
if(findChessboardCorners(boards[i], cornersSize, corners_fcb))
break;
}
//cv::namedWindow("CB"); imshow("CB", boards[i]); cv::waitKey();
imagePoints_art.push_back(corners_art);
imagePoints_findCb.push_back(corners_fcb);
tvecs_exp[i].create(1, 3, CV_64F);
*tvecs_exp[i].ptr<Point3d>() = cbg.corners3d[0];
rvecs_exp[i] = calcRvec(cbg.corners3d, cbg.cornersSize());
}
}
void runTest(const Size& imgSize, const Mat_<double>& camMat, const Mat_<double>& distCoeffs, size_t brdsNum, const Size& cornersSize, int flag = 0)
{
const TermCriteria tc(TermCriteria::EPS|TermCriteria::MAX_ITER, 30, 0.1);
vector< vector<Point2f> > imagePoints;
switch(flag)
{
case JUST_FIND_CORNERS: imagePoints = imagePoints_findCb; break;
case ARTIFICIAL_CORNERS: imagePoints = imagePoints_art; break;
case USE_CORNERS_SUBPIX:
for(size_t i = 0; i < brdsNum; ++i)
{
Mat gray;
cvtColor(boards[i], gray, COLOR_BGR2GRAY);
vector<Point2f> tmp = imagePoints_findCb[i];
cornerSubPix(gray, tmp, Size(5, 5), Size(-1,-1), tc);
imagePoints.push_back(tmp);
}
break;
case USE_4QUAD_CORNERS:
for(size_t i = 0; i < brdsNum; ++i)
{
Mat gray;
cvtColor(boards[i], gray, COLOR_BGR2GRAY);
vector<Point2f> tmp = imagePoints_findCb[i];
find4QuadCornerSubpix(gray, tmp, Size(5, 5));
imagePoints.push_back(tmp);
}
break;
default:
throw std::exception();
}
Mat camMat_est = Mat::eye(3, 3, CV_64F), distCoeffs_est = Mat::zeros(1, 5, CV_64F);
vector<Mat> rvecs_est, tvecs_est;
int flags = /*CALIB_FIX_K3|*/CALIB_FIX_K4|CALIB_FIX_K5|CALIB_FIX_K6; //CALIB_FIX_K3; //CALIB_FIX_ASPECT_RATIO | | CALIB_ZERO_TANGENT_DIST;
TermCriteria criteria = TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 100, DBL_EPSILON);
double rep_error = calibrateCamera(objectPoints, imagePoints, imgSize, camMat_est, distCoeffs_est, rvecs_est, tvecs_est, flags, criteria);
rep_error /= brdsNum * cornersSize.area();
const double thres = 1;
if (rep_error > thres)
{
ts->printf( cvtest::TS::LOG, "%d) Too big reproject error = %f\n", r, rep_error);
ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
}
compareCameraMatrs(camMat, camMat_est);
compareDistCoeffs(distCoeffs, distCoeffs_est);
compareShiftVecs(tvecs_exp, tvecs_est);
compareRotationVecs(rvecs_exp, rvecs_est);
double rep_errorWOI = reprojectErrorWithoutIntrinsics(chessboard3D, rvecs_exp, tvecs_exp, rvecs_est, tvecs_est);
rep_errorWOI /= brdsNum * cornersSize.area();
const double thres2 = 0.01;
if (rep_errorWOI > thres2)
{
ts->printf( cvtest::TS::LOG, "%d) Too big reproject error without intrinsics = %f\n", r, rep_errorWOI);
ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
}
ts->printf( cvtest::TS::LOG, "%d) Testing solvePnP...\n", r);
rvecs_spnp.resize(brdsNum);
tvecs_spnp.resize(brdsNum);
for(size_t i = 0; i < brdsNum; ++i)
solvePnP(objectPoints[i], imagePoints[i], camMat, distCoeffs, rvecs_spnp[i], tvecs_spnp[i]);
compareShiftVecs(tvecs_exp, tvecs_spnp);
compareRotationVecs(rvecs_exp, rvecs_spnp);
}
void run(int)
{
ts->set_failed_test_info(cvtest::TS::OK);
RNG& rng = theRNG();
int progress = 0;
int repeat_num = 3;
for(r = 0; r < repeat_num; ++r)
{
const int brds_num = 20;
Mat bg(Size(640, 480), CV_8UC3);
randu(bg, Scalar::all(32), Scalar::all(255));
GaussianBlur(bg, bg, Size(5, 5), 2);
double fx = 300 + (20 * (double)rng - 10);
double fy = 300 + (20 * (double)rng - 10);
double cx = bg.cols/2 + (40 * (double)rng - 20);
double cy = bg.rows/2 + (40 * (double)rng - 20);
Mat_<double> camMat(3, 3);
camMat << fx, 0., cx, 0, fy, cy, 0., 0., 1.;
double k1 = 0.5 + (double)rng/5;
double k2 = (double)rng/5;
double k3 = (double)rng/5;
double p1 = 0.001 + (double)rng/10;
double p2 = 0.001 + (double)rng/10;
Mat_<double> distCoeffs(1, 5, 0.0);
distCoeffs << k1, k2, p1, p2, k3;
ChessBoardGenerator cbg(Size(9, 8));
cbg.min_cos = 0.9;
cbg.cov = 0.8;
progress = update_progress(progress, r, repeat_num, 0);
ts->printf( cvtest::TS::LOG, "\n");
prepareForTest(bg, camMat, distCoeffs, brds_num, cbg);
ts->printf( cvtest::TS::LOG, "artificial corners\n");
runTest(bg.size(), camMat, distCoeffs, brds_num, cbg.cornersSize(), ARTIFICIAL_CORNERS);
progress = update_progress(progress, r, repeat_num, 0);
ts->printf( cvtest::TS::LOG, "findChessboard corners\n");
runTest(bg.size(), camMat, distCoeffs, brds_num, cbg.cornersSize(), JUST_FIND_CORNERS);
progress = update_progress(progress, r, repeat_num, 0);
ts->printf( cvtest::TS::LOG, "cornersSubPix corners\n");
runTest(bg.size(), camMat, distCoeffs, brds_num, cbg.cornersSize(), USE_CORNERS_SUBPIX);
progress = update_progress(progress, r, repeat_num, 0);
ts->printf( cvtest::TS::LOG, "4quad corners\n");
runTest(bg.size(), camMat, distCoeffs, brds_num, cbg.cornersSize(), USE_4QUAD_CORNERS);
progress = update_progress(progress, r, repeat_num, 0);
}
}
};
TEST(Calib3d_CalibrateCamera_CPP, DISABLED_accuracy_on_artificial_data) { CV_CalibrateCameraArtificialTest test; test.safe_run(); }
}} // namespace
modules/calib/test/test_cameracalibration_badarg.cpp
+396
View File
@@ -0,0 +1,396 @@
/*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.
//
//
// Intel License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2000, Intel Corporation, 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 Intel Corporation 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 "test_chessboardgenerator.hpp"
namespace opencv_test { namespace {
class CV_CameraCalibrationBadArgTest : public cvtest::BadArgTest
{
public:
CV_CameraCalibrationBadArgTest() {}
~CV_CameraCalibrationBadArgTest() {}
protected:
void run(int);
void run_func(void) {}
struct C_Caller
{
_InputArray imgPts_arg;
_InputArray objPts_arg;
_OutputArray rvecs_arg;
_OutputArray tvecs_arg;
_OutputArray newObjPts_arg;
_InputOutputArray cameraMatrix_arg;
_InputOutputArray distCoeffs_arg;
std::vector<std::vector<Point2f> > imgPts;
std::vector<std::vector<Point3f> > objPts;
Size imageSize0, imageSize;
int iFixedPoint0, iFixedPoint;
Mat cameraMatrix;
Mat distCoeffs;
std::vector<Mat> rvecs;
std::vector<Mat> tvecs;
std::vector<Point3f> newObjPts;
int flags0, flags;
void initArgs()
{
imgPts_arg = imgPts;
objPts_arg = objPts;
rvecs_arg = rvecs;
tvecs_arg = tvecs;
newObjPts_arg = newObjPts;
cameraMatrix_arg = cameraMatrix;
distCoeffs_arg = distCoeffs;
imageSize = imageSize0;
flags = flags0;
iFixedPoint = iFixedPoint0;
}
void operator()() const
{
calibrateCameraRO(objPts_arg, imgPts_arg, imageSize, iFixedPoint,
cameraMatrix_arg, distCoeffs_arg, rvecs_arg, tvecs_arg,
newObjPts_arg, flags);
}
};
};
void CV_CameraCalibrationBadArgTest::run( int /* start_from */ )
{
const int M = 2;
Size imgSize(800, 600);
Mat_<float> camMat(3, 3);
Mat_<float> distCoeffs0(1, 5);
camMat << 300.f, 0.f, imgSize.width/2.f, 0, 300.f, imgSize.height/2.f, 0.f, 0.f, 1.f;
distCoeffs0 << 1.2f, 0.2f, 0.f, 0.f, 0.f;
ChessBoardGenerator cbg(Size(8,6));
Size corSize = cbg.cornersSize();
vector<Point2f> corners;
cbg(Mat(imgSize, CV_8U, Scalar(0)), camMat, distCoeffs0, corners);
C_Caller caller;
caller.imageSize0 = imgSize;
caller.iFixedPoint0 = -1;
caller.flags0 = 0;
/////////////////////////////
Mat cameraMatrix_cpp;
Mat distCoeffs_cpp;
Mat rvecs_cpp;
Mat tvecs_cpp;
Mat newObjPts_cpp;
std::vector<Point3f> objPts_cpp;
for(int y = 0; y < corSize.height; ++y)
for(int x = 0; x < corSize.width; ++x)
objPts_cpp.push_back(Point3f((float)x, (float)y, 0.f));
caller.objPts.resize(M);
caller.imgPts.resize(M);
for(int i = 0; i < M; i++)
{
caller.objPts[i] = objPts_cpp;
caller.imgPts[i] = corners;
}
caller.cameraMatrix.create(3, 3, CV_32F);
caller.distCoeffs.create(5, 1, CV_32F);
caller.rvecs.clear();
caller.tvecs.clear();
caller.newObjPts.clear();
/* /*//*/ */
int errors = 0;
caller.initArgs();
caller.objPts_arg = noArray();
errors += run_test_case( cv::Error::StsBadArg, "None passed in objPts", caller);
caller.initArgs();
caller.imgPts_arg = noArray();
errors += run_test_case( cv::Error::StsBadArg, "None passed in imgPts", caller );
caller.initArgs();
caller.cameraMatrix_arg = noArray();
errors += run_test_case( cv::Error::StsBadArg, "Zero passed in cameraMatrix", caller );
caller.initArgs();
caller.distCoeffs_arg = noArray();
errors += run_test_case( cv::Error::StsBadArg, "Zero passed in distCoeffs", caller );
caller.initArgs();
caller.imageSize.width = -1;
errors += run_test_case( cv::Error::StsOutOfRange, "Bad image width", caller );
caller.initArgs();
caller.imageSize.height = -1;
errors += run_test_case( cv::Error::StsOutOfRange, "Bad image height", caller );
caller.initArgs();
caller.imgPts[0].clear();
errors += run_test_case( cv::Error::StsBadSize, "Bad imgpts[0]", caller );
caller.imgPts[0] = caller.imgPts[1];
caller.initArgs();
caller.objPts[1].clear();
errors += run_test_case( cv::Error::StsBadSize, "Bad objpts[1]", caller );
caller.objPts[1] = caller.objPts[0];
caller.initArgs();
Mat badCM = Mat::zeros(4, 4, CV_64F);
caller.cameraMatrix_arg = badCM;
caller.flags = CALIB_USE_INTRINSIC_GUESS;
errors += run_test_case( cv::Error::StsBadArg, "Bad camearaMatrix header", caller );
caller.initArgs();
Mat badDC = Mat::zeros(10, 10, CV_64F);
caller.distCoeffs_arg = badDC;
caller.flags = CALIB_USE_INTRINSIC_GUESS;
errors += run_test_case( cv::Error::StsBadArg, "Bad camearaMatrix header", caller );
if (errors)
ts->set_failed_test_info(cvtest::TS::FAIL_MISMATCH);
else
ts->set_failed_test_info(cvtest::TS::OK);
}
class CV_Rodrigues2BadArgTest : public cvtest::BadArgTest
{
public:
CV_Rodrigues2BadArgTest() {}
~CV_Rodrigues2BadArgTest() {}
protected:
void run_func(void) {}
struct C_Caller
{
_InputArray src_arg;
_OutputArray dst_arg, j_arg;
Mat src;
Mat dst;
Mat jacobian;
void initArgs()
{
src_arg = src;
dst_arg = dst;
j_arg = jacobian;
}
void operator()()
{
cv::Rodrigues(src_arg, dst_arg, j_arg);
}
};
void run(int /* start_from */ )
{
Mat src_cpp(3, 1, CV_32F);
Mat dst_cpp(3, 3, CV_32F);
C_Caller caller;
/*/*//*/*/
int errors = 0;
caller.initArgs();
caller.src_arg = noArray();
errors += run_test_case( cv::Error::StsBadArg, "Src is empty matrix", caller );
caller.initArgs();
caller.src = Mat::zeros(3, 1, CV_8U);
errors += run_test_case( cv::Error::StsUnsupportedFormat, "Bad src formart", caller );
caller.initArgs();
caller.src = Mat::zeros(1, 1, CV_32F);
errors += run_test_case( cv::Error::StsBadSize, "Bad src size", caller );
if (errors)
ts->set_failed_test_info(cvtest::TS::FAIL_MISMATCH);
else
ts->set_failed_test_info(cvtest::TS::OK);
}
};
//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
class CV_ProjectPoints2BadArgTest : public cvtest::BadArgTest
{
public:
CV_ProjectPoints2BadArgTest() : camMat(3, 3), distCoeffs(1, 5)
{
Size imsSize(800, 600);
camMat << 300.f, 0.f, imsSize.width/2.f, 0, 300.f, imsSize.height/2.f, 0.f, 0.f, 1.f;
distCoeffs << 1.2f, 0.2f, 0.f, 0.f, 0.f;
}
~CV_ProjectPoints2BadArgTest() {}
protected:
void run_func(void) {}
Mat_<float> camMat;
Mat_<float> distCoeffs;
struct C_Caller
{
_InputArray objectPoints_arg, rvec_arg, tvec_arg, A_arg, DC_arg;
_OutputArray imagePoints_arg;
Mat objectPoints;
Mat r_vec;
Mat t_vec;
Mat A;
Mat distCoeffs;
Mat imagePoints;
Mat J;
double aspectRatio0, aspectRatio;
void initArgs()
{
objectPoints_arg = objectPoints;
imagePoints_arg = imagePoints;
rvec_arg = r_vec;
tvec_arg = t_vec;
A_arg = A;
DC_arg = distCoeffs;
aspectRatio = aspectRatio0;
}
void operator()()
{
projectPoints(objectPoints_arg, rvec_arg, tvec_arg, A_arg, DC_arg,
imagePoints_arg, J, aspectRatio );
}
};
void run(int /* start_from */ )
{
C_Caller caller;
const int n = 10;
Mat objectPoints_cpp(1, n, CV_32FC3);
randu(objectPoints_cpp, Scalar::all(1), Scalar::all(10));
caller.objectPoints = objectPoints_cpp;
caller.t_vec = Mat::zeros(1, 3, CV_32F);
Rodrigues(Mat::eye(3, 3, CV_32F), caller.r_vec);
caller.A = Mat::eye(3, 3, CV_32F);
caller.distCoeffs = Mat::zeros(1, 5, CV_32F);
caller.aspectRatio0 = 1.0;
/********************/
int errors = 0;
caller.initArgs();
caller.objectPoints_arg = noArray();
errors += run_test_case( cv::Error::StsBadArg, "Zero objectPoints", caller );
caller.initArgs();
caller.rvec_arg = noArray();
errors += run_test_case( cv::Error::StsBadArg, "Zero r_vec", caller );
caller.initArgs();
caller.tvec_arg = noArray();
errors += run_test_case( cv::Error::StsBadArg, "Zero t_vec", caller );
caller.initArgs();
caller.A_arg = noArray();
errors += run_test_case( cv::Error::StsBadArg, "Zero camMat", caller );
caller.initArgs();
caller.imagePoints_arg = noArray();
errors += run_test_case( cv::Error::StsBadArg, "Zero imagePoints", caller );
Mat save_rvec = caller.r_vec;
caller.initArgs();
caller.r_vec.create(2, 2, CV_32F);
errors += run_test_case( cv::Error::StsBadArg, "Bad rvec format", caller );
caller.initArgs();
caller.r_vec.create(1, 3, CV_8U);
errors += run_test_case( cv::Error::StsBadArg, "Bad rvec format", caller );
caller.r_vec = save_rvec;
/****************************/
Mat save_tvec = caller.t_vec;
caller.initArgs();
caller.t_vec.create(3, 3, CV_32F);
errors += run_test_case( cv::Error::StsBadArg, "Bad tvec format", caller );
caller.initArgs();
caller.t_vec.create(1, 3, CV_8U);
errors += run_test_case( cv::Error::StsBadArg, "Bad tvec format", caller );
caller.t_vec = save_tvec;
/****************************/
Mat save_A = caller.A;
caller.initArgs();
caller.A.create(2, 2, CV_32F);
errors += run_test_case( cv::Error::StsBadArg, "Bad A format", caller );
caller.A = save_A;
/****************************/
Mat save_DC = caller.distCoeffs;
caller.initArgs();
caller.distCoeffs.create(3, 3, CV_32F);
errors += run_test_case( cv::Error::StsBadArg, "Bad distCoeffs format", caller );
caller.distCoeffs = save_DC;
if (errors)
ts->set_failed_test_info(cvtest::TS::FAIL_MISMATCH);
else
ts->set_failed_test_info(cvtest::TS::OK);
}
};
TEST(Calib3d_CalibrateCamera_CPP, badarg) { CV_CameraCalibrationBadArgTest test; test.safe_run(); }
TEST(Calib3d_Rodrigues_CPP, badarg) { CV_Rodrigues2BadArgTest test; test.safe_run(); }
TEST(Calib3d_ProjectPoints_CPP, badarg) { CV_ProjectPoints2BadArgTest test; test.safe_run(); }
}} // namespace
modules/calib/test/test_cameracalibration_tilt.cpp
+693
View File
@@ -0,0 +1,693 @@
/*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
namespace opencv_test { namespace {
#define NUM_DIST_COEFF_TILT 14
/**
Some conventions:
- the first camera determines the world coordinate system
- y points down, hence top means minimal y value (negative) and
bottom means maximal y value (positive)
- the field of view plane is tilted around x such that it
intersects the xy-plane in a line with a large (positive)
y-value
- image sensor and object are both modelled in the halfspace
z > 0
**/
class cameraCalibrationTiltTest : public ::testing::Test {
protected:
cameraCalibrationTiltTest()
: m_toRadian(acos(-1.0)/180.0)
, m_toDegree(180.0/acos(-1.0))
{}
virtual void SetUp();
protected:
static const cv::Size m_imageSize;
static const double m_pixelSize;
static const double m_circleConfusionPixel;
static const double m_lensFocalLength;
static const double m_lensFNumber;
static const double m_objectDistance;
static const double m_planeTiltDegree;
static const double m_pointTargetDist;
static const int m_pointTargetNum;
/** image distance corresponding to working distance */
double m_imageDistance;
/** image tilt angle corresponding to the tilt of the object plane */
double m_imageTiltDegree;
/** center of the field of view, near and far plane */
std::vector<cv::Vec3d> m_fovCenter;
/** normal of the field of view, near and far plane */
std::vector<cv::Vec3d> m_fovNormal;
/** points on a plane calibration target */
std::vector<cv::Point3d> m_pointTarget;
/** rotations for the calibration target */
std::vector<cv::Vec3d> m_pointTargetRvec;
/** translations for the calibration target */
std::vector<cv::Vec3d> m_pointTargetTvec;
/** camera matrix */
cv::Matx33d m_cameraMatrix;
/** distortion coefficients */
cv::Vec<double, NUM_DIST_COEFF_TILT> m_distortionCoeff;
/** random generator */
cv::RNG m_rng;
/** degree to radian conversion factor */
const double m_toRadian;
/** radian to degree conversion factor */
const double m_toDegree;
/**
computes for a given distance of an image or object point
the distance of the corresponding object or image point
*/
double opticalMap(double dist) {
return m_lensFocalLength*dist/(dist - m_lensFocalLength);
}
/** magnification of the optical map */
double magnification(double dist) {
return m_lensFocalLength/(dist - m_lensFocalLength);
}
/**
Changes given distortion coefficients randomly by adding
a uniformly distributed random variable in [-max max]
\param coeff input
\param max limits for the random variables
*/
void randomDistortionCoeff(
cv::Vec<double, NUM_DIST_COEFF_TILT>& coeff,
const cv::Vec<double, NUM_DIST_COEFF_TILT>& max)
{
for (int i = 0; i < coeff.rows; ++i)
coeff(i) += m_rng.uniform(-max(i), max(i));
}
/** numerical jacobian */
void numericalDerivative(
cv::Mat& jac,
double eps,
const std::vector<cv::Point3d>& obj,
const cv::Vec3d& rvec,
const cv::Vec3d& tvec,
const cv::Matx33d& camera,
const cv::Vec<double, NUM_DIST_COEFF_TILT>& distor);
/** remove points with projection outside the sensor array */
void removeInvalidPoints(
std::vector<cv::Point2d>& imagePoints,
std::vector<cv::Point3d>& objectPoints);
/** add uniform distribute noise in [-halfWidthNoise, halfWidthNoise]
to the image points and remove out of range points */
void addNoiseRemoveInvalidPoints(
std::vector<cv::Point2f>& imagePoints,
std::vector<cv::Point3f>& objectPoints,
std::vector<cv::Point2f>& noisyImagePoints,
double halfWidthNoise);
};
/** Number of Pixel of the sensor */
const cv::Size cameraCalibrationTiltTest::m_imageSize(1600, 1200);
/** Size of a pixel in mm */
const double cameraCalibrationTiltTest::m_pixelSize(.005);
/** Diameter of the circle of confusion */
const double cameraCalibrationTiltTest::m_circleConfusionPixel(3);
/** Focal length of the lens */
const double cameraCalibrationTiltTest::m_lensFocalLength(16.4);
/** F-Number */
const double cameraCalibrationTiltTest::m_lensFNumber(8);
/** Working distance */
const double cameraCalibrationTiltTest::m_objectDistance(200);
/** Angle between optical axis and object plane normal */
const double cameraCalibrationTiltTest::m_planeTiltDegree(55);
/** the calibration target are points on a square grid with this side length */
const double cameraCalibrationTiltTest::m_pointTargetDist(5);
/** the calibration target has (2*n + 1) x (2*n + 1) points */
const int cameraCalibrationTiltTest::m_pointTargetNum(15);
void cameraCalibrationTiltTest::SetUp()
{
m_imageDistance = opticalMap(m_objectDistance);
m_imageTiltDegree = m_toDegree * atan2(
m_imageDistance * tan(m_toRadian * m_planeTiltDegree),
m_objectDistance);
// half sensor height
double tmp = .5 * (m_imageSize.height - 1) * m_pixelSize
* cos(m_toRadian * m_imageTiltDegree);
// y-Value of tilted sensor
double yImage[2] = {tmp, -tmp};
// change in z because of the tilt
tmp *= sin(m_toRadian * m_imageTiltDegree);
// z-values of the sensor lower and upper corner
double zImage[2] = {
m_imageDistance + tmp,
m_imageDistance - tmp};
// circle of confusion
double circleConfusion = m_circleConfusionPixel*m_pixelSize;
// aperture of the lense
double aperture = m_lensFocalLength/m_lensFNumber;
// near and far factor on the image side
double nearFarFactorImage[2] = {
aperture/(aperture - circleConfusion),
aperture/(aperture + circleConfusion)};
// on the object side - points that determine the field of
// view
std::vector<cv::Vec3d> fovBottomTop(6);
std::vector<cv::Vec3d>::iterator itFov = fovBottomTop.begin();
for (size_t iBottomTop = 0; iBottomTop < 2; ++iBottomTop)
{
// mapping sensor to field of view
*itFov = cv::Vec3d(0,yImage[iBottomTop],zImage[iBottomTop]);
*itFov *= magnification((*itFov)(2));
++itFov;
for (size_t iNearFar = 0; iNearFar < 2; ++iNearFar, ++itFov)
{
// scaling to the near and far distance on the
// image side
*itFov = cv::Vec3d(0,yImage[iBottomTop],zImage[iBottomTop]) *
nearFarFactorImage[iNearFar];
// scaling to the object side
*itFov *= magnification((*itFov)(2));
}
}
m_fovCenter.resize(3);
m_fovNormal.resize(3);
for (size_t i = 0; i < 3; ++i)
{
m_fovCenter[i] = .5*(fovBottomTop[i] + fovBottomTop[i+3]);
m_fovNormal[i] = fovBottomTop[i+3] - fovBottomTop[i];
m_fovNormal[i] = cv::normalize(m_fovNormal[i]);
m_fovNormal[i] = cv::Vec3d(
m_fovNormal[i](0),
-m_fovNormal[i](2),
m_fovNormal[i](1));
// one target position in each plane
m_pointTargetTvec.push_back(m_fovCenter[i]);
cv::Vec3d rvec = cv::Vec3d(0,0,1).cross(m_fovNormal[i]);
rvec = cv::normalize(rvec);
rvec *= acos(m_fovNormal[i](2));
m_pointTargetRvec.push_back(rvec);
}
// calibration target
size_t num = 2*m_pointTargetNum + 1;
m_pointTarget.resize(num*num);
std::vector<cv::Point3d>::iterator itTarget = m_pointTarget.begin();
for (int iY = -m_pointTargetNum; iY <= m_pointTargetNum; ++iY)
{
for (int iX = -m_pointTargetNum; iX <= m_pointTargetNum; ++iX, ++itTarget)
{
*itTarget = cv::Point3d(iX, iY, 0) * m_pointTargetDist;
}
}
// oblique target positions
// approximate distance to the near and far plane
double dist = std::max(
std::abs(m_fovNormal[0].dot(m_fovCenter[0] - m_fovCenter[1])),
std::abs(m_fovNormal[0].dot(m_fovCenter[0] - m_fovCenter[2])));
// maximal angle such that target border "reaches" near and far plane
double maxAngle = atan2(dist, m_pointTargetNum*m_pointTargetDist);
std::vector<double> angle;
angle.push_back(-maxAngle);
angle.push_back(maxAngle);
cv::Matx33d baseMatrix;
cv::Rodrigues(m_pointTargetRvec.front(), baseMatrix);
for (std::vector<double>::const_iterator itAngle = angle.begin(); itAngle != angle.end(); ++itAngle)
{
cv::Matx33d rmat;
for (int i = 0; i < 2; ++i)
{
cv::Vec3d rvec(0,0,0);
rvec(i) = *itAngle;
cv::Rodrigues(rvec, rmat);
rmat = baseMatrix*rmat;
cv::Rodrigues(rmat, rvec);
m_pointTargetTvec.push_back(m_fovCenter.front());
m_pointTargetRvec.push_back(rvec);
}
}
// camera matrix
double cx = .5 * (m_imageSize.width - 1);
double cy = .5 * (m_imageSize.height - 1);
double f = m_imageDistance/m_pixelSize;
m_cameraMatrix = cv::Matx33d(
f,0,cx,
0,f,cy,
0,0,1);
// distortion coefficients
m_distortionCoeff = cv::Vec<double, NUM_DIST_COEFF_TILT>::all(0);
// tauX
m_distortionCoeff(12) = -m_toRadian*m_imageTiltDegree;
}
void cameraCalibrationTiltTest::numericalDerivative(
cv::Mat& jac,
double eps,
const std::vector<cv::Point3d>& obj,
const cv::Vec3d& rvec,
const cv::Vec3d& tvec,
const cv::Matx33d& camera,
const cv::Vec<double, NUM_DIST_COEFF_TILT>& distor)
{
cv::Vec3d r(rvec);
cv::Vec3d t(tvec);
cv::Matx33d cm(camera);
cv::Vec<double, NUM_DIST_COEFF_TILT> dc(distor);
double* param[10+NUM_DIST_COEFF_TILT] = {
&r(0), &r(1), &r(2),
&t(0), &t(1), &t(2),
&cm(0,0), &cm(1,1), &cm(0,2), &cm(1,2),
&dc(0), &dc(1), &dc(2), &dc(3), &dc(4), &dc(5), &dc(6),
&dc(7), &dc(8), &dc(9), &dc(10), &dc(11), &dc(12), &dc(13)};
std::vector<cv::Point2d> pix0, pix1;
double invEps = .5/eps;
for (int col = 0; col < 10+NUM_DIST_COEFF_TILT; ++col)
{
double save = *(param[col]);
*(param[col]) = save + eps;
cv::projectPoints(obj, r, t, cm, dc, pix0);
*(param[col]) = save - eps;
cv::projectPoints(obj, r, t, cm, dc, pix1);
*(param[col]) = save;
std::vector<cv::Point2d>::const_iterator it0 = pix0.begin();
std::vector<cv::Point2d>::const_iterator it1 = pix1.begin();
int row = 0;
for (;it0 != pix0.end(); ++it0, ++it1)
{
cv::Point2d d = invEps*(*it0 - *it1);
jac.at<double>(row, col) = d.x;
++row;
jac.at<double>(row, col) = d.y;
++row;
}
}
}
void cameraCalibrationTiltTest::removeInvalidPoints(
std::vector<cv::Point2d>& imagePoints,
std::vector<cv::Point3d>& objectPoints)
{
// remove object and imgage points out of range
std::vector<cv::Point2d>::iterator itImg = imagePoints.begin();
std::vector<cv::Point3d>::iterator itObj = objectPoints.begin();
while (itImg != imagePoints.end())
{
bool ok =
itImg->x >= 0 &&
itImg->x <= m_imageSize.width - 1.0 &&
itImg->y >= 0 &&
itImg->y <= m_imageSize.height - 1.0;
if (ok)
{
++itImg;
++itObj;
}
else
{
itImg = imagePoints.erase(itImg);
itObj = objectPoints.erase(itObj);
}
}
}
void cameraCalibrationTiltTest::addNoiseRemoveInvalidPoints(
std::vector<cv::Point2f>& imagePoints,
std::vector<cv::Point3f>& objectPoints,
std::vector<cv::Point2f>& noisyImagePoints,
double halfWidthNoise)
{
std::vector<cv::Point2f>::iterator itImg = imagePoints.begin();
std::vector<cv::Point3f>::iterator itObj = objectPoints.begin();
noisyImagePoints.clear();
noisyImagePoints.reserve(imagePoints.size());
while (itImg != imagePoints.end())
{
cv::Point2f pix = *itImg + cv::Point2f(
(float)m_rng.uniform(-halfWidthNoise, halfWidthNoise),
(float)m_rng.uniform(-halfWidthNoise, halfWidthNoise));
bool ok =
pix.x >= 0 &&
pix.x <= m_imageSize.width - 1.0 &&
pix.y >= 0 &&
pix.y <= m_imageSize.height - 1.0;
if (ok)
{
noisyImagePoints.push_back(pix);
++itImg;
++itObj;
}
else
{
itImg = imagePoints.erase(itImg);
itObj = objectPoints.erase(itObj);
}
}
}
TEST_F(cameraCalibrationTiltTest, projectPoints)
{
std::vector<cv::Point2d> imagePoints;
std::vector<cv::Point3d> objectPoints = m_pointTarget;
cv::Vec3d rvec = m_pointTargetRvec.front();
cv::Vec3d tvec = m_pointTargetTvec.front();
cv::Vec<double, NUM_DIST_COEFF_TILT> coeffNoiseHalfWidth(
.1, .1, // k1 k2
.01, .01, // p1 p2
.001, .001, .001, .001, // k3 k4 k5 k6
.001, .001, .001, .001, // s1 s2 s3 s4
.01, .01); // tauX tauY
for (size_t numTest = 0; numTest < 10; ++numTest)
{
// create random distortion coefficients
cv::Vec<double, NUM_DIST_COEFF_TILT> distortionCoeff = m_distortionCoeff;
randomDistortionCoeff(distortionCoeff, coeffNoiseHalfWidth);
// projection
cv::projectPoints(
objectPoints,
rvec,
tvec,
m_cameraMatrix,
distortionCoeff,
imagePoints);
// remove object and imgage points out of range
removeInvalidPoints(imagePoints, objectPoints);
int numPoints = (int)imagePoints.size();
int numParams = 10 + distortionCoeff.rows;
cv::Mat jacobian(2*numPoints, numParams, CV_64FC1);
// projection and jacobian
cv::projectPoints(
objectPoints,
rvec,
tvec,
m_cameraMatrix,
distortionCoeff,
imagePoints,
jacobian);
// numerical derivatives
cv::Mat numericJacobian(2*numPoints, numParams, CV_64FC1);
double eps = 1e-7;
numericalDerivative(
numericJacobian,
eps,
objectPoints,
rvec,
tvec,
m_cameraMatrix,
distortionCoeff);
#if 0
for (size_t row = 0; row < 2; ++row)
{
std::cout << "------ Row = " << row << " ------\n";
for (size_t i = 0; i < 10+NUM_DIST_COEFF_TILT; ++i)
{
std::cout << i
<< " jac = " << jacobian.at<double>(row,i)
<< " num = " << numericJacobian.at<double>(row,i)
<< " rel. diff = " << abs(numericJacobian.at<double>(row,i) - jacobian.at<double>(row,i))/abs(numericJacobian.at<double>(row,i))
<< "\n";
}
}
#endif
// relative difference for large values (rvec and tvec)
cv::Mat check = abs(jacobian(cv::Range::all(), cv::Range(0,6)) - numericJacobian(cv::Range::all(), cv::Range(0,6)))/
(1 + abs(jacobian(cv::Range::all(), cv::Range(0,6))));
double minVal, maxVal;
cv::minMaxIdx(check, &minVal, &maxVal);
EXPECT_LE(maxVal, .01);
// absolute difference for distortion and camera matrix
EXPECT_MAT_NEAR(jacobian(cv::Range::all(), cv::Range(6,numParams)), numericJacobian(cv::Range::all(), cv::Range(6,numParams)), 1e-5);
}
}
TEST_F(cameraCalibrationTiltTest, undistortPoints)
{
cv::Vec<double, NUM_DIST_COEFF_TILT> coeffNoiseHalfWidth(
.2, .1, // k1 k2
.01, .01, // p1 p2
.01, .01, .01, .01, // k3 k4 k5 k6
.001, .001, .001, .001, // s1 s2 s3 s4
.001, .001); // tauX tauY
double step = 99;
double toleranceBackProjection = 1e-5;
for (size_t numTest = 0; numTest < 10; ++numTest)
{
cv::Vec<double, NUM_DIST_COEFF_TILT> distortionCoeff = m_distortionCoeff;
randomDistortionCoeff(distortionCoeff, coeffNoiseHalfWidth);
// distorted points
std::vector<cv::Point2d> distorted;
for (double x = 0; x <= m_imageSize.width-1; x += step)
for (double y = 0; y <= m_imageSize.height-1; y += step)
distorted.push_back(cv::Point2d(x,y));
std::vector<cv::Point2d> normalizedUndistorted;
// undistort
cv::undistortPoints(distorted,
normalizedUndistorted,
m_cameraMatrix,
distortionCoeff);
// copy normalized points to 3D
std::vector<cv::Point3d> objectPoints;
for (std::vector<cv::Point2d>::const_iterator itPnt = normalizedUndistorted.begin();
itPnt != normalizedUndistorted.end(); ++itPnt)
objectPoints.push_back(cv::Point3d(itPnt->x, itPnt->y, 1));
// project
std::vector<cv::Point2d> imagePoints(objectPoints.size());
cv::projectPoints(objectPoints,
cv::Vec3d(0,0,0),
cv::Vec3d(0,0,0),
m_cameraMatrix,
distortionCoeff,
imagePoints);
EXPECT_MAT_NEAR(distorted, imagePoints, toleranceBackProjection);
}
}
template <typename INPUT, typename ESTIMATE>
void show(const std::string& name, const INPUT in, const ESTIMATE est)
{
std::cout << name << " = " << est << " (init = " << in
<< ", diff = " << est-in << ")\n";
}
template <typename INPUT>
void showVec(const std::string& name, const INPUT& in, const cv::Mat& est)
{
for (size_t i = 0; i < in.channels; ++i)
{
std::stringstream ss;
ss << name << "[" << i << "]";
show(ss.str(), in(i), est.at<double>(i));
}
}
/**
For given camera matrix and distortion coefficients
- project point target in different positions onto the sensor
- add pixel noise
- estimate camera model with noisy measurements
- compare result with initial model parameter
Parameter are differently affected by the noise
*/
TEST_F(cameraCalibrationTiltTest, calibrateCamera)
{
cv::Vec<double, NUM_DIST_COEFF_TILT> coeffNoiseHalfWidth(
.2, .1, // k1 k2
.01, .01, // p1 p2
0, 0, 0, 0, // k3 k4 k5 k6
.001, .001, .001, .001, // s1 s2 s3 s4
.001, .001); // tauX tauY
double pixelNoiseHalfWidth = .5;
std::vector<cv::Point3f> pointTarget;
pointTarget.reserve(m_pointTarget.size());
for (std::vector<cv::Point3d>::const_iterator it = m_pointTarget.begin(); it != m_pointTarget.end(); ++it)
pointTarget.push_back(cv::Point3f(
(float)(it->x),
(float)(it->y),
(float)(it->z)));
for (size_t numTest = 0; numTest < 5; ++numTest)
{
// create random distortion coefficients
cv::Vec<double, NUM_DIST_COEFF_TILT> distortionCoeff = m_distortionCoeff;
randomDistortionCoeff(distortionCoeff, coeffNoiseHalfWidth);
// container for calibration data
std::vector<std::vector<cv::Point3f> > viewsObjectPoints;
std::vector<std::vector<cv::Point2f> > viewsImagePoints;
std::vector<std::vector<cv::Point2f> > viewsNoisyImagePoints;
// simulate calibration data with projectPoints
std::vector<cv::Vec3d>::const_iterator itRvec = m_pointTargetRvec.begin();
std::vector<cv::Vec3d>::const_iterator itTvec = m_pointTargetTvec.begin();
// loop over different views
for (;itRvec != m_pointTargetRvec.end(); ++ itRvec, ++itTvec)
{
std::vector<cv::Point3f> objectPoints(pointTarget);
std::vector<cv::Point2f> imagePoints;
std::vector<cv::Point2f> noisyImagePoints;
// project calibration target to sensor
cv::projectPoints(
objectPoints,
*itRvec,
*itTvec,
m_cameraMatrix,
distortionCoeff,
imagePoints);
// remove invisible points
addNoiseRemoveInvalidPoints(
imagePoints,
objectPoints,
noisyImagePoints,
pixelNoiseHalfWidth);
// add data for view
viewsNoisyImagePoints.push_back(noisyImagePoints);
viewsImagePoints.push_back(imagePoints);
viewsObjectPoints.push_back(objectPoints);
}
// Output
std::vector<cv::Mat> outRvecs, outTvecs;
cv::Mat outCameraMatrix(3, 3, CV_64F, cv::Scalar::all(1)), outDistCoeff;
// Stopping criteria
cv::TermCriteria stop(
cv::TermCriteria::COUNT+cv::TermCriteria::EPS,
50000,
1e-14);
// model choice
int flag =
cv::CALIB_FIX_ASPECT_RATIO |
// cv::CALIB_RATIONAL_MODEL |
cv::CALIB_FIX_K3 |
// cv::CALIB_FIX_K6 |
cv::CALIB_THIN_PRISM_MODEL |
cv::CALIB_TILTED_MODEL;
// estimate
double backProjErr = cv::calibrateCamera(
viewsObjectPoints,
viewsNoisyImagePoints,
m_imageSize,
outCameraMatrix,
outDistCoeff,
outRvecs,
outTvecs,
flag,
stop);
EXPECT_LE(backProjErr, pixelNoiseHalfWidth);
#if 0
std::cout << "------ estimate ------\n";
std::cout << "back projection error = " << backProjErr << "\n";
std::cout << "points per view = {" << viewsObjectPoints.front().size();
for (size_t i = 1; i < viewsObjectPoints.size(); ++i)
std::cout << ", " << viewsObjectPoints[i].size();
std::cout << "}\n";
show("fx", m_cameraMatrix(0,0), outCameraMatrix.at<double>(0,0));
show("fy", m_cameraMatrix(1,1), outCameraMatrix.at<double>(1,1));
show("cx", m_cameraMatrix(0,2), outCameraMatrix.at<double>(0,2));
show("cy", m_cameraMatrix(1,2), outCameraMatrix.at<double>(1,2));
showVec("distor", distortionCoeff, outDistCoeff);
#endif
if (pixelNoiseHalfWidth > 0)
{
double tolRvec = pixelNoiseHalfWidth;
double tolTvec = m_objectDistance * tolRvec;
// back projection error
for (size_t i = 0; i < viewsNoisyImagePoints.size(); ++i)
{
double dRvec = cv::norm(m_pointTargetRvec[i],
cv::Vec3d(outRvecs[i].at<double>(0), outRvecs[i].at<double>(1), outRvecs[i].at<double>(2))
);
EXPECT_LE(dRvec, tolRvec);
double dTvec = cv::norm(m_pointTargetTvec[i],
cv::Vec3d(outTvecs[i].at<double>(0), outTvecs[i].at<double>(1), outTvecs[i].at<double>(2))
);
EXPECT_LE(dTvec, tolTvec);
std::vector<cv::Point2f> backProjection;
cv::projectPoints(
viewsObjectPoints[i],
outRvecs[i],
outTvecs[i],
outCameraMatrix,
outDistCoeff,
backProjection);
EXPECT_MAT_NEAR(backProjection, viewsNoisyImagePoints[i], 1.5*pixelNoiseHalfWidth);
EXPECT_MAT_NEAR(backProjection, viewsImagePoints[i], 1.5*pixelNoiseHalfWidth);
}
}
pixelNoiseHalfWidth *= .25;
}
}
}} // namespace
modules/calib/test/test_chessboardgenerator.cpp
+331
View File
@@ -0,0 +1,331 @@
/*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, 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 "test_chessboardgenerator.hpp"
namespace cv {
ChessBoardGenerator::ChessBoardGenerator(const Size& _patternSize) : sensorWidth(32), sensorHeight(24),
squareEdgePointsNum(200), min_cos(std::sqrt(3.f)*0.5f), cov(0.5),
patternSize(_patternSize), rendererResolutionMultiplier(4), tvec(Mat::zeros(1, 3, CV_32F))
{
rvec.create(3, 1, CV_32F);
Rodrigues(Mat::eye(3, 3, CV_32F), rvec);
}
void ChessBoardGenerator::generateEdge(const Point3f& p1, const Point3f& p2, vector<Point3f>& out) const
{
Point3f step = (p2 - p1) * (1.f/squareEdgePointsNum);
for(size_t n = 0; n < squareEdgePointsNum; ++n)
out.push_back( p1 + step * (float)n);
}
Size ChessBoardGenerator::cornersSize() const
{
return Size(patternSize.width-1, patternSize.height-1);
}
struct Mult
{
float m;
Mult(int mult) : m((float)mult) {}
Point2f operator()(const Point2f& p)const { return p * m; }
};
void ChessBoardGenerator::generateBasis(Point3f& pb1, Point3f& pb2) const
{
RNG& rng = theRNG();
Vec3f n;
for(;;)
{
n[0] = rng.uniform(-1.f, 1.f);
n[1] = rng.uniform(-1.f, 1.f);
n[2] = rng.uniform(0.0f, 1.f);
float len = (float)norm(n);
if (len < 1e-3)
continue;
n[0]/=len;
n[1]/=len;
n[2]/=len;
if (n[2] > min_cos)
break;
}
Vec3f n_temp = n; n_temp[0] += 100;
Vec3f b1 = n.cross(n_temp);
Vec3f b2 = n.cross(b1);
float len_b1 = (float)norm(b1);
float len_b2 = (float)norm(b2);
pb1 = Point3f(b1[0]/len_b1, b1[1]/len_b1, b1[2]/len_b1);
pb2 = Point3f(b2[0]/len_b1, b2[1]/len_b2, b2[2]/len_b2);
}
Mat ChessBoardGenerator::generateChessBoard(const Mat& bg, const Mat& camMat, const Mat& distCoeffs,
const Point3f& zero, const Point3f& pb1, const Point3f& pb2,
float sqWidth, float sqHeight, const vector<Point3f>& whole,
vector<Point2f>& corners) const
{
vector< vector<Point> > squares_black;
for(int i = 0; i < patternSize.width; ++i)
for(int j = 0; j < patternSize.height; ++j)
if ( (i % 2 == 0 && j % 2 == 0) || (i % 2 != 0 && j % 2 != 0) )
{
vector<Point3f> pts_square3d;
vector<Point2f> pts_square2d;
Point3f p1 = zero + (i + 0) * sqWidth * pb1 + (j + 0) * sqHeight * pb2;
Point3f p2 = zero + (i + 1) * sqWidth * pb1 + (j + 0) * sqHeight * pb2;
Point3f p3 = zero + (i + 1) * sqWidth * pb1 + (j + 1) * sqHeight * pb2;
Point3f p4 = zero + (i + 0) * sqWidth * pb1 + (j + 1) * sqHeight * pb2;
generateEdge(p1, p2, pts_square3d);
generateEdge(p2, p3, pts_square3d);
generateEdge(p3, p4, pts_square3d);
generateEdge(p4, p1, pts_square3d);
projectPoints(pts_square3d, rvec, tvec, camMat, distCoeffs, pts_square2d);
squares_black.resize(squares_black.size() + 1);
vector<Point2f> temp;
approxPolyDP(pts_square2d, temp, 1.0, true);
transform(temp.begin(), temp.end(), back_inserter(squares_black.back()), Mult(rendererResolutionMultiplier));
}
/* calculate corners */
corners3d.clear();
for(int j = 0; j < patternSize.height - 1; ++j)
for(int i = 0; i < patternSize.width - 1; ++i)
corners3d.push_back(zero + (i + 1) * sqWidth * pb1 + (j + 1) * sqHeight * pb2);
corners.clear();
projectPoints(corners3d, rvec, tvec, camMat, distCoeffs, corners);
vector<Point3f> whole3d;
vector<Point2f> whole2d;
generateEdge(whole[0], whole[1], whole3d);
generateEdge(whole[1], whole[2], whole3d);
generateEdge(whole[2], whole[3], whole3d);
generateEdge(whole[3], whole[0], whole3d);
projectPoints(whole3d, rvec, tvec, camMat, distCoeffs, whole2d);
vector<Point2f> temp_whole2d;
approxPolyDP(whole2d, temp_whole2d, 1.0, true);
vector< vector<Point > > whole_contour(1);
transform(temp_whole2d.begin(), temp_whole2d.end(),
back_inserter(whole_contour.front()), Mult(rendererResolutionMultiplier));
Mat result;
if (rendererResolutionMultiplier == 1)
{
result = bg.clone();
drawContours(result, whole_contour, -1, Scalar::all(255), FILLED, LINE_AA);
drawContours(result, squares_black, -1, Scalar::all(0), FILLED, LINE_AA);
}
else
{
Mat tmp;
resize(bg, tmp, bg.size() * rendererResolutionMultiplier, 0, 0, INTER_LINEAR_EXACT);
drawContours(tmp, whole_contour, -1, Scalar::all(255), FILLED, LINE_AA);
drawContours(tmp, squares_black, -1, Scalar::all(0), FILLED, LINE_AA);
resize(tmp, result, bg.size(), 0, 0, INTER_AREA);
}
return result;
}
Mat ChessBoardGenerator::operator ()(const Mat& bg, const Mat& camMat, const Mat& distCoeffs, vector<Point2f>& corners) const
{
cov = std::min(cov, 0.8);
double fovx, fovy, focalLen;
Point2d principalPoint;
double aspect;
calibrationMatrixValues( camMat, bg.size(), sensorWidth, sensorHeight,
fovx, fovy, focalLen, principalPoint, aspect);
RNG& rng = theRNG();
float d1 = static_cast<float>(rng.uniform(0.1, 10.0));
float ah = static_cast<float>(rng.uniform(-fovx/2 * cov, fovx/2 * cov) * CV_PI / 180);
float av = static_cast<float>(rng.uniform(-fovy/2 * cov, fovy/2 * cov) * CV_PI / 180);
Point3f p;
p.z = std::cos(ah) * d1;
p.x = std::sin(ah) * d1;
p.y = p.z * std::tan(av);
Point3f pb1, pb2;
generateBasis(pb1, pb2);
float cbHalfWidth = static_cast<float>(norm(p) * std::sin( std::min(fovx, fovy) * 0.5 * CV_PI / 180));
float cbHalfHeight = cbHalfWidth * patternSize.height / patternSize.width;
float cbHalfWidthEx = cbHalfWidth * ( patternSize.width + 1) / patternSize.width;
float cbHalfHeightEx = cbHalfHeight * (patternSize.height + 1) / patternSize.height;
vector<Point3f> pts3d(4);
vector<Point2f> pts2d(4);
for(;;)
{
pts3d[0] = p + pb1 * cbHalfWidthEx + cbHalfHeightEx * pb2;
pts3d[1] = p + pb1 * cbHalfWidthEx - cbHalfHeightEx * pb2;
pts3d[2] = p - pb1 * cbHalfWidthEx - cbHalfHeightEx * pb2;
pts3d[3] = p - pb1 * cbHalfWidthEx + cbHalfHeightEx * pb2;
/* can remake with better perf */
projectPoints(pts3d, rvec, tvec, camMat, distCoeffs, pts2d);
bool inrect1 = pts2d[0].x < bg.cols && pts2d[0].y < bg.rows && pts2d[0].x > 0 && pts2d[0].y > 0;
bool inrect2 = pts2d[1].x < bg.cols && pts2d[1].y < bg.rows && pts2d[1].x > 0 && pts2d[1].y > 0;
bool inrect3 = pts2d[2].x < bg.cols && pts2d[2].y < bg.rows && pts2d[2].x > 0 && pts2d[2].y > 0;
bool inrect4 = pts2d[3].x < bg.cols && pts2d[3].y < bg.rows && pts2d[3].x > 0 && pts2d[3].y > 0;
if (inrect1 && inrect2 && inrect3 && inrect4)
break;
cbHalfWidth*=0.8f;
cbHalfHeight = cbHalfWidth * patternSize.height / patternSize.width;
cbHalfWidthEx = cbHalfWidth * ( patternSize.width + 1) / patternSize.width;
cbHalfHeightEx = cbHalfHeight * (patternSize.height + 1) / patternSize.height;
}
Point3f zero = p - pb1 * cbHalfWidth - cbHalfHeight * pb2;
float sqWidth = 2 * cbHalfWidth/patternSize.width;
float sqHeight = 2 * cbHalfHeight/patternSize.height;
return generateChessBoard(bg, camMat, distCoeffs, zero, pb1, pb2, sqWidth, sqHeight, pts3d, corners);
}
Mat ChessBoardGenerator::operator ()(const Mat& bg, const Mat& camMat, const Mat& distCoeffs,
const Size2f& squareSize, vector<Point2f>& corners) const
{
cov = std::min(cov, 0.8);
double fovx, fovy, focalLen;
Point2d principalPoint;
double aspect;
calibrationMatrixValues( camMat, bg.size(), sensorWidth, sensorHeight,
fovx, fovy, focalLen, principalPoint, aspect);
RNG& rng = theRNG();
float d1 = static_cast<float>(rng.uniform(0.1, 10.0));
float ah = static_cast<float>(rng.uniform(-fovx/2 * cov, fovx/2 * cov) * CV_PI / 180);
float av = static_cast<float>(rng.uniform(-fovy/2 * cov, fovy/2 * cov) * CV_PI / 180);
Point3f p;
p.z = std::cos(ah) * d1;
p.x = std::sin(ah) * d1;
p.y = p.z * std::tan(av);
Point3f pb1, pb2;
generateBasis(pb1, pb2);
float cbHalfWidth = squareSize.width * patternSize.width * 0.5f;
float cbHalfHeight = squareSize.height * patternSize.height * 0.5f;
float cbHalfWidthEx = cbHalfWidth * ( patternSize.width + 1) / patternSize.width;
float cbHalfHeightEx = cbHalfHeight * (patternSize.height + 1) / patternSize.height;
vector<Point3f> pts3d(4);
vector<Point2f> pts2d(4);
for(;;)
{
pts3d[0] = p + pb1 * cbHalfWidthEx + cbHalfHeightEx * pb2;
pts3d[1] = p + pb1 * cbHalfWidthEx - cbHalfHeightEx * pb2;
pts3d[2] = p - pb1 * cbHalfWidthEx - cbHalfHeightEx * pb2;
pts3d[3] = p - pb1 * cbHalfWidthEx + cbHalfHeightEx * pb2;
/* can remake with better perf */
projectPoints(pts3d, rvec, tvec, camMat, distCoeffs, pts2d);
bool inrect1 = pts2d[0].x < bg.cols && pts2d[0].y < bg.rows && pts2d[0].x > 0 && pts2d[0].y > 0;
bool inrect2 = pts2d[1].x < bg.cols && pts2d[1].y < bg.rows && pts2d[1].x > 0 && pts2d[1].y > 0;
bool inrect3 = pts2d[2].x < bg.cols && pts2d[2].y < bg.rows && pts2d[2].x > 0 && pts2d[2].y > 0;
bool inrect4 = pts2d[3].x < bg.cols && pts2d[3].y < bg.rows && pts2d[3].x > 0 && pts2d[3].y > 0;
if ( inrect1 && inrect2 && inrect3 && inrect4)
break;
p.z *= 1.1f;
}
Point3f zero = p - pb1 * cbHalfWidth - cbHalfHeight * pb2;
return generateChessBoard(bg, camMat, distCoeffs, zero, pb1, pb2,
squareSize.width, squareSize.height, pts3d, corners);
}
Mat ChessBoardGenerator::operator ()(const Mat& bg, const Mat& camMat, const Mat& distCoeffs,
const Size2f& squareSize, const Point3f& pos, vector<Point2f>& corners) const
{
cov = std::min(cov, 0.8);
Point3f p = pos;
Point3f pb1, pb2;
generateBasis(pb1, pb2);
float cbHalfWidth = squareSize.width * patternSize.width * 0.5f;
float cbHalfHeight = squareSize.height * patternSize.height * 0.5f;
float cbHalfWidthEx = cbHalfWidth * ( patternSize.width + 1) / patternSize.width;
float cbHalfHeightEx = cbHalfHeight * (patternSize.height + 1) / patternSize.height;
vector<Point3f> pts3d(4);
vector<Point2f> pts2d(4);
pts3d[0] = p + pb1 * cbHalfWidthEx + cbHalfHeightEx * pb2;
pts3d[1] = p + pb1 * cbHalfWidthEx - cbHalfHeightEx * pb2;
pts3d[2] = p - pb1 * cbHalfWidthEx - cbHalfHeightEx * pb2;
pts3d[3] = p - pb1 * cbHalfWidthEx + cbHalfHeightEx * pb2;
/* can remake with better perf */
projectPoints(pts3d, rvec, tvec, camMat, distCoeffs, pts2d);
Point3f zero = p - pb1 * cbHalfWidth - cbHalfHeight * pb2;
return generateChessBoard(bg, camMat, distCoeffs, zero, pb1, pb2,
squareSize.width, squareSize.height, pts3d, corners);
}
} // namespace
modules/calib/test/test_chessboardgenerator.hpp
+43
View File
@@ -0,0 +1,43 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html.
#ifndef CV_CHESSBOARDGENERATOR_H143KJTVYM389YTNHKFDHJ89NYVMO3VLMEJNTBGUEIYVCM203P
#define CV_CHESSBOARDGENERATOR_H143KJTVYM389YTNHKFDHJ89NYVMO3VLMEJNTBGUEIYVCM203P
namespace cv
{
using std::vector;
class ChessBoardGenerator
{
public:
double sensorWidth;
double sensorHeight;
size_t squareEdgePointsNum;
double min_cos;
mutable double cov;
Size patternSize;
int rendererResolutionMultiplier;
ChessBoardGenerator(const Size& patternSize = Size(8, 6));
Mat operator()(const Mat& bg, const Mat& camMat, const Mat& distCoeffs, std::vector<Point2f>& corners) const;
Mat operator()(const Mat& bg, const Mat& camMat, const Mat& distCoeffs, const Size2f& squareSize, std::vector<Point2f>& corners) const;
Mat operator()(const Mat& bg, const Mat& camMat, const Mat& distCoeffs, const Size2f& squareSize, const Point3f& pos, std::vector<Point2f>& corners) const;
Size cornersSize() const;
mutable std::vector<Point3f> corners3d;
private:
void generateEdge(const Point3f& p1, const Point3f& p2, std::vector<Point3f>& out) const;
Mat generateChessBoard(const Mat& bg, const Mat& camMat, const Mat& distCoeffs,
const Point3f& zero, const Point3f& pb1, const Point3f& pb2,
float sqWidth, float sqHeight, const std::vector<Point3f>& whole, std::vector<Point2f>& corners) const;
void generateBasis(Point3f& pb1, Point3f& pb2) const;
Mat rvec, tvec;
};
}
#endif
modules/calib/test/test_fisheye.cpp
+767
View File
@@ -0,0 +1,767 @@
/*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);
};
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;
}
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, 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, 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
modules/calib/test/test_main.cpp
+10
View File
@@ -0,0 +1,10 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html.
#include "test_precomp.hpp"
#if defined(HAVE_HPX)
#include <hpx/hpx_main.hpp>
#endif
CV_TEST_MAIN("")
modules/calib/test/test_modelest.cpp
+231
View File
@@ -0,0 +1,231 @@
/*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.
//
//
// Intel License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2000, Intel Corporation, 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 Intel Corporation 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"
#if 0
#include "_modelest.h"
using namespace std;
using namespace cv;
class BareModelEstimator : public CvModelEstimator2
{
public:
BareModelEstimator(int modelPoints, CvSize modelSize, int maxBasicSolutions);
virtual int runKernel( const CvMat*, const CvMat*, CvMat* );
virtual void computeReprojError( const CvMat*, const CvMat*,
const CvMat*, CvMat* );
bool checkSubsetPublic( const CvMat* ms1, int count, bool checkPartialSubset );
};
BareModelEstimator::BareModelEstimator(int _modelPoints, CvSize _modelSize, int _maxBasicSolutions)
:CvModelEstimator2(_modelPoints, _modelSize, _maxBasicSolutions)
{
}
int BareModelEstimator::runKernel( const CvMat*, const CvMat*, CvMat* )
{
return 0;
}
void BareModelEstimator::computeReprojError( const CvMat*, const CvMat*,
const CvMat*, CvMat* )
{
}
bool BareModelEstimator::checkSubsetPublic( const CvMat* ms1, int count, bool checkPartialSubset )
{
checkPartialSubsets = checkPartialSubset;
return checkSubset(ms1, count);
}
class CV_ModelEstimator2_Test : public cvtest::ArrayTest
{
public:
CV_ModelEstimator2_Test();
protected:
void get_test_array_types_and_sizes( int test_case_idx, vector<vector<Size> >& sizes, vector<vector<int> >& types );
void fill_array( int test_case_idx, int i, int j, Mat& arr );
double get_success_error_level( int test_case_idx, int i, int j );
void run_func();
void prepare_to_validation( int test_case_idx );
bool checkPartialSubsets;
int usedPointsCount;
bool checkSubsetResult;
int generalPositionsCount;
int maxPointsCount;
};
CV_ModelEstimator2_Test::CV_ModelEstimator2_Test()
{
generalPositionsCount = get_test_case_count() / 2;
maxPointsCount = 100;
test_array[INPUT].push_back(NULL);
test_array[OUTPUT].push_back(NULL);
test_array[REF_OUTPUT].push_back(NULL);
}
void CV_ModelEstimator2_Test::get_test_array_types_and_sizes( int /*test_case_idx*/,
vector<vector<Size> > &sizes, vector<vector<int> > &types )
{
RNG &rng = ts->get_rng();
checkPartialSubsets = (cvtest::randInt(rng) % 2 == 0);
int pointsCount = cvtest::randInt(rng) % maxPointsCount;
usedPointsCount = pointsCount == 0 ? 0 : cvtest::randInt(rng) % pointsCount;
sizes[INPUT][0] = cvSize(1, pointsCount);
types[INPUT][0] = CV_64FC2;
sizes[OUTPUT][0] = sizes[REF_OUTPUT][0] = cvSize(1, 1);
types[OUTPUT][0] = types[REF_OUTPUT][0] = CV_8UC1;
}
void CV_ModelEstimator2_Test::fill_array( int test_case_idx, int i, int j, Mat& arr )
{
if( i != INPUT )
{
cvtest::ArrayTest::fill_array( test_case_idx, i, j, arr );
return;
}
if (test_case_idx < generalPositionsCount)
{
//generate points in a general position (i.e. no three points can lie on the same line.)
bool isGeneralPosition;
do
{
ArrayTest::fill_array(test_case_idx, i, j, arr);
//a simple check that the position is general:
// for each line check that all other points don't belong to it
isGeneralPosition = true;
for (int startPointIndex = 0; startPointIndex < usedPointsCount && isGeneralPosition; startPointIndex++)
{
for (int endPointIndex = startPointIndex + 1; endPointIndex < usedPointsCount && isGeneralPosition; endPointIndex++)
{
for (int testPointIndex = 0; testPointIndex < usedPointsCount && isGeneralPosition; testPointIndex++)
{
if (testPointIndex == startPointIndex || testPointIndex == endPointIndex)
{
continue;
}
CV_Assert(arr.type() == CV_64FC2);
Point2d tangentVector_1 = arr.at<Point2d>(endPointIndex) - arr.at<Point2d>(startPointIndex);
Point2d tangentVector_2 = arr.at<Point2d>(testPointIndex) - arr.at<Point2d>(startPointIndex);
const float eps = 1e-4f;
//TODO: perhaps it is better to normalize the cross product by norms of the tangent vectors
if (fabs(tangentVector_1.cross(tangentVector_2)) < eps)
{
isGeneralPosition = false;
}
}
}
}
}
while(!isGeneralPosition);
}
else
{
//create points in a degenerate position (there are at least 3 points belonging to the same line)
ArrayTest::fill_array(test_case_idx, i, j, arr);
if (usedPointsCount <= 2)
{
return;
}
RNG &rng = ts->get_rng();
int startPointIndex, endPointIndex, modifiedPointIndex;
do
{
startPointIndex = cvtest::randInt(rng) % usedPointsCount;
endPointIndex = cvtest::randInt(rng) % usedPointsCount;
modifiedPointIndex = checkPartialSubsets ? usedPointsCount - 1 : cvtest::randInt(rng) % usedPointsCount;
}
while (startPointIndex == endPointIndex || startPointIndex == modifiedPointIndex || endPointIndex == modifiedPointIndex);
double startWeight = cvtest::randReal(rng);
CV_Assert(arr.type() == CV_64FC2);
arr.at<Point2d>(modifiedPointIndex) = startWeight * arr.at<Point2d>(startPointIndex) + (1.0 - startWeight) * arr.at<Point2d>(endPointIndex);
}
}
double CV_ModelEstimator2_Test::get_success_error_level( int /*test_case_idx*/, int /*i*/, int /*j*/ )
{
return 0;
}
void CV_ModelEstimator2_Test::prepare_to_validation( int test_case_idx )
{
test_mat[OUTPUT][0].at<uchar>(0) = checkSubsetResult;
test_mat[REF_OUTPUT][0].at<uchar>(0) = test_case_idx < generalPositionsCount || usedPointsCount <= 2;
}
void CV_ModelEstimator2_Test::run_func()
{
//make the input continuous
Mat input = test_mat[INPUT][0].clone();
CvMat _input = input;
RNG &rng = ts->get_rng();
int modelPoints = cvtest::randInt(rng);
CvSize modelSize = cvSize(2, modelPoints);
int maxBasicSolutions = cvtest::randInt(rng);
BareModelEstimator modelEstimator(modelPoints, modelSize, maxBasicSolutions);
checkSubsetResult = modelEstimator.checkSubsetPublic(&_input, usedPointsCount, checkPartialSubsets);
}
TEST(Calib3d_ModelEstimator2, accuracy) { CV_ModelEstimator2_Test test; test.safe_run(); }
#endif
modules/calib/test/test_multiview_calib.cpp
+709
View File
@@ -0,0 +1,709 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html.
#include "test_precomp.hpp"
#include <opencv2/core/utils/logger.hpp>
#include <opencv2/ts/cuda_test.hpp> // EXPECT_MAT_NEAR
namespace opencv_test { namespace {
TEST(multiview_calibration, accuracy) {
// convert euler angles to rotation matrix
const auto euler2rot = [] (double x, double y, double z) {
cv::Matx33d R_x(1, 0, 0, 0, cos(x), -sin(x), 0, sin(x), cos(x));
cv::Matx33d R_y(cos(y), 0, sin(y), 0, 1, 0, -sin(y), 0, cos(y));
cv::Matx33d R_z(cos(z), -sin(z), 0, sin(z), cos(z), 0, 0, 0, 1);
return cv::Mat(R_z * R_y * R_x);
};
const cv::Size board_size (5,4);
cv::RNG rng(0);
const double board_len = 0.08, noise_std = 0.04;
const int num_cameras = 4, num_pts = board_size.area();
std::vector<cv::Vec3f> board_pattern (num_pts);
// fill pattern points
for (int j = 0; j < board_size.height; j++) {
for (int i = 0; i < board_size.width; i++) {
board_pattern[j*board_size.width+i] = cv::Vec3f ((float)i, (float)j, 0)*board_len;
}
}
std::vector<uchar> models(num_cameras, cv::CALIB_MODEL_PINHOLE);
std::vector<cv::Size> image_sizes(num_cameras);
std::vector<cv::Mat> Ks_gt, distortions_gt, Rs_gt, Ts_gt;
for (int c = 0; c < num_cameras; c++) {
// generate intrinsics and extrinsics
image_sizes[c] = cv::Size(rng.uniform(1300, 1500), rng.uniform(900, 1300));
const double focal = rng.uniform(900.0, 1300.0);
cv::Matx33d K(focal, 0, (double)image_sizes[c].width/2.,
0, focal, (double)image_sizes[c].height/2.,
0, 0, 1);
cv::Matx<double, 1, 5> dist (rng.uniform(1e-1, 3e-1), rng.uniform(1e-2, 5e-2), rng.uniform(1e-2, 5e-2), rng.uniform(1e-2, 5e-2), rng.uniform(1e-2, 5e-2));
Ks_gt.emplace_back(cv::Mat(K));
distortions_gt.emplace_back(cv::Mat(dist));
if (c == 0) {
// I | 0
Rs_gt.emplace_back(cv::Mat(cv::Matx33d::eye()));
Ts_gt.emplace_back(cv::Mat(cv::Vec3d::zeros()));
} else {
const double ty_min = -.3, ty_max = .3, tx_min = -.3, tx_max = .3, tz_min = -.1, tz_max = .1;
const double yaw_min = -20, yaw_max = 20, pitch_min = -20, pitch_max = 20, roll_min = -20, roll_max = 20;
Rs_gt.emplace_back(euler2rot(rng.uniform(yaw_min, yaw_max)*M_PI/180,
rng.uniform(pitch_min, pitch_max)*M_PI/180,
rng.uniform(roll_min, roll_max)*M_PI/180));
Ts_gt.emplace_back(cv::Mat(cv::Vec3d(rng.uniform(tx_min, tx_max),
rng.uniform(ty_min, ty_max),
rng.uniform(tz_min, tz_max))));
}
}
const int MAX_SAMPLES = 2000, MAX_FRAMES = 50;
cv::Mat pattern (board_pattern, true/*copy*/);
pattern = pattern.reshape(1, num_pts).t();
pattern.row(2) = 2.0; // set approximate depth of object points
const double ty_min = -2, ty_max = 2, tx_min = -2, tx_max = 2, tz_min = -1, tz_max = 1;
const double yaw_min = -45, yaw_max = 45, pitch_min = -45, pitch_max = 45, roll_min = -45, roll_max = 45;
std::vector<std::vector<cv::Vec3f>> objPoints;
std::vector<std::vector<cv::Mat>> image_points_all(num_cameras);
cv::Mat ones = cv::Mat_<float>::ones(1, num_pts);
std::vector<std::vector<uchar>> visibility;
cv::Mat centroid = cv::Mat(cv::Matx31f(
(float)cv::mean(pattern.row(0)).val[0],
(float)cv::mean(pattern.row(1)).val[0],
(float)cv::mean(pattern.row(2)).val[0]));
for (int f = 0; f < MAX_SAMPLES; f++) {
cv::Mat R = euler2rot(rng.uniform(yaw_min, yaw_max)*M_PI/180,
rng.uniform(pitch_min, pitch_max)*M_PI/180,
rng.uniform(roll_min, roll_max)*M_PI/180);
cv::Mat t = cv::Mat(cv::Matx31f(
(float)rng.uniform(tx_min, tx_max),
(float)rng.uniform(ty_min, ty_max),
(float)rng.uniform(tz_min, tz_max)));
R.convertTo(R, CV_32F);
cv::Mat pattern_new = (R * (pattern - centroid * ones) + centroid * ones + t * ones).t();
std::vector<cv::Mat> img_pts_cams(num_cameras);
std::vector<uchar> visible(num_cameras, (uchar)0);
int num_visible_patterns = 0;
for (int c = 0; c < num_cameras; c++) {
cv::Mat img_pts;
if (models[c] == cv::CALIB_MODEL_FISHEYE) {
cv::fisheye::projectPoints(pattern_new, img_pts, Rs_gt[c], Ts_gt[c], Ks_gt[c], distortions_gt[c]);
} else {
cv::projectPoints(pattern_new, Rs_gt[c], Ts_gt[c], Ks_gt[c], distortions_gt[c], img_pts);
}
// add normal / Gaussian noise to image points
cv::Mat noise (img_pts.rows, img_pts.cols, img_pts.type());
rng.fill(noise, cv::RNG::NORMAL, 0, noise_std);
img_pts += noise;
bool are_all_pts_in_image = true;
const auto * const pts = (float *) img_pts.data;
for (int i = 0; i < num_pts; i++) {
if (pts[i*2 ] < 0 || pts[i*2 ] > (float)image_sizes[c].width ||
pts[i*2+1] < 0 || pts[i*2+1] > (float)image_sizes[c].height) {
are_all_pts_in_image = false;
break;
}
}
if (are_all_pts_in_image) {
visible[c] = 1;
num_visible_patterns += 1;
img_pts.copyTo(img_pts_cams[c]);
}
}
if (num_visible_patterns >= 2) {
objPoints.emplace_back(board_pattern);
visibility.emplace_back(visible);
for (int c = 0; c < num_cameras; c++) {
image_points_all[c].emplace_back(img_pts_cams[c].clone());
}
if (objPoints.size() >= MAX_FRAMES)
break;
}
}
cv::Mat visibility_mat = cv::Mat_<uchar>(num_cameras, (int)objPoints.size());
for (int c = 0; c < num_cameras; c++) {
for (int f = 0; f < (int)objPoints.size(); f++) {
visibility_mat.at<uchar>(c, f) = visibility[f][c];
}
}
std::vector<cv::Mat> Ks, distortions, Rs, Ts;
calibrateMultiview(objPoints, image_points_all, image_sizes, visibility_mat,
models, Ks, distortions, Rs, Ts);
const double K_err_tol = 1e1, dist_tol = 5e-2, R_tol = 1e-2, T_tol = 1e-2;
for (int c = 0; c < num_cameras; c++) {
cv::Mat R;
cv::Rodrigues(Rs[c], R);
EXPECT_MAT_NEAR(Ks_gt[c], Ks[c], K_err_tol);
CV_LOG_INFO(NULL, "true distortions: " << distortions_gt[c]);
CV_LOG_INFO(NULL, "found distortions: " << distortions[c]);
EXPECT_MAT_NEAR(distortions_gt[c], distortions[c], dist_tol);
EXPECT_MAT_NEAR(Rs_gt[c], R, R_tol);
EXPECT_MAT_NEAR(Ts_gt[c], Ts[c], T_tol);
}
}
struct MultiViewTest : public ::testing::Test
{
std::vector<cv::Vec3f> genAsymmetricObjectPoints(cv::Size board_size = cv::Size(8, 11), float square_size = 0.04)
{
std::vector<cv::Vec3f> objectPoints;
objectPoints.reserve(board_size.height*board_size.width);
for( int i = 0; i < board_size.height; i++ )
{
for( int j = 0; j < board_size.width; j++ )
{
objectPoints.push_back(cv::Point3f((2*j + i % 2)*square_size, i*square_size, 0));
}
}
return objectPoints;
}
void loadImagePoints(const std::string& base_dir, const std::vector<std::string> cameras, int frameCount,
std::vector<std::vector<cv::Mat>>& image_points_all, cv::Mat& visibility)
{
image_points_all.clear();
visibility.create(static_cast<int>(cameras.size()), frameCount, CV_BoolC1);
for (int c = 0; c < static_cast<int>(cameras.size()); c++)
{
std::vector<cv::Mat> camera_image_points;
std::string fname = base_dir + cameras[c] + ".json";
FileStorage fs(fname, cv::FileStorage::READ);
ASSERT_TRUE(fs.isOpened()) << "Cannot open points file " << fname;
for (int i = 0; i < frameCount; i++)
{
std::string nodeName = cv::format("frame_%d", i);
FileNode node = fs[nodeName];
if (!node.empty())
{
camera_image_points.push_back(node.mat().reshape(2, 1));
visibility.at<uchar>(c, i) = 1;
}
else
{
camera_image_points.push_back(cv::Mat());
visibility.at<uchar>(c, i) = 0;
}
}
fs.release();
image_points_all.push_back(camera_image_points);
}
}
double calibrateMono(const std::vector<cv::Vec3f>& board_pattern,
const std::vector<cv::Mat>& image_points,
const cv::Size& image_size,
cv::CameraModel model,
int flags,
Mat& K,
Mat& dist)
{
std::vector<cv::Mat> filtered_image_points;
for(size_t i = 0; i < image_points.size(); i++)
{
if(!image_points[i].empty())
filtered_image_points.push_back(image_points[i]);
}
std::vector<std::vector<cv::Vec3f>> objPoints(filtered_image_points.size(), board_pattern);
std::vector<cv::Mat> rvec, tvec;
cv::Mat K1, dist1;
if(model == cv::CALIB_MODEL_PINHOLE)
{
return cv::calibrateCamera(objPoints, filtered_image_points, image_size, K, dist, rvec, tvec, flags);
}
else if(model == cv::CALIB_MODEL_FISHEYE)
{
return cv::fisheye::calibrate(objPoints, filtered_image_points, image_size, K, dist, rvec, tvec, flags);
}
else
{
CV_Error(Error::StsBadArg, "Unsupported camera model!");
}
return FLT_MAX;
}
void validateCameraPose(const Mat& R, Mat T, const Mat& R_gt, const Mat& T_gt,
double angle_tol = 1.*M_PI/180., double pos_tol = 0.01)
{
double cos_r = (cv::trace(R_gt.t() * R)[0] - 1) / 2.;
double angle = std::acos(std::max(std::min(cos_r, 1.), -1.));
cv::Mat dist_mat;
subtract(R_gt.t() * T_gt, R.t() * T, dist_mat);
double dist = cv::norm(dist_mat);
CV_LOG_INFO(NULL, "rotation error: " << angle);
CV_LOG_INFO(NULL, "position error: " << dist);
EXPECT_NEAR(angle, 0., angle_tol);
EXPECT_NEAR(dist, 0., pos_tol);
}
void validateAllPoses(const std::vector<cv::Mat>& Rs_gt,
const std::vector<cv::Mat>& Ts_gt,
const std::vector<cv::Mat>& Rs,
const std::vector<cv::Mat>& Ts,
double angle_tol = 1.*M_PI/180.,
double pos_tol = 0.01)
{
ASSERT_EQ(Rs_gt.size(), Ts_gt.size());
ASSERT_EQ(Rs.size(), Ts.size());
ASSERT_EQ(Rs_gt.size(), Rs.size());
const size_t num_cameras = Rs_gt.size();
for (size_t c = 1; c < num_cameras; c++)
{
validateCameraPose(Rs[c], Ts[c], Rs_gt[c], Ts_gt[c], angle_tol, pos_tol);
double distance0 = cv::norm(Rs[c].t()*Ts[c]);
CV_LOG_INFO(NULL, "distance to camera #0: " << distance0);
}
}
};
TEST_F(MultiViewTest, OneLine)
{
const string root = cvtest::TS::ptr()->get_data_path() + "cv/cameracalibration/multiview/3cams-one-line/";
const std::vector<std::string> cam_names = {"cam_0", "cam_1", "cam_3"};
const std::vector<cv::Size> image_sizes = {{1920, 1080}, {1920, 1080}, {1920, 1080} };
std::vector<uchar> models(3, cv::CALIB_MODEL_PINHOLE);
double rs_1_gt_data[9] = {
0.9996914489704484, -0.01160060078752197, -0.02196435559568884,
0.012283315339906, 0.9994374509454836, 0.03120739995344806,
0.02158997497973892, -0.03146756598408248, 0.9992715673286274
};
double rs_2_gt_data[9] = {
0.9988848194142131, -0.0255827884561986, -0.03968171466355882,
0.0261796234191418, 0.999550713317242, 0.0145944792515729,
0.03929051872229011, -0.0156170561181697, 0.9991057815350362
};
double ts_1_gt_data[3] = {0.5078811293323259, 0.002753469433719865, 0.02413521839310227};
double ts_2_gt_data[3] = {1.007213763725429, 0.01645068247976361, 0.05394643957910365};
std::vector<cv::Mat> Rs_gt = {
cv::Mat::eye(3, 3, CV_64FC1),
cv::Mat(3, 3, CV_64FC1, rs_1_gt_data),
cv::Mat(3, 3, CV_64FC1, rs_2_gt_data)
};
std::vector<cv::Mat> Ts_gt = {
cv::Mat::zeros(3, 1, CV_64FC1),
cv::Mat(3, 1, CV_64FC1, ts_1_gt_data),
cv::Mat(3, 1, CV_64FC1, ts_2_gt_data)
};
const int num_frames = 96;
std::vector<std::vector<cv::Mat>> image_points_all;
cv::Mat visibility;
loadImagePoints(root, cam_names, num_frames, image_points_all, visibility);
ASSERT_EQ(cam_names.size(), image_points_all.size());
ASSERT_TRUE(!image_points_all.empty());
for(size_t i = 0; i < cam_names.size(); i++)
{
EXPECT_TRUE(!image_points_all[i].empty());
}
std::vector<cv::Vec3f> board_pattern = genAsymmetricObjectPoints();
std::vector<std::vector<cv::Vec3f>> objPoints(num_frames, board_pattern);
std::vector<int> flagsForIntrinsics(3, CALIB_RATIONAL_MODEL);
std::vector<cv::Mat> Ks, distortions, Rs, Rs_rvec, Ts;
double rms = calibrateMultiview(objPoints, image_points_all, image_sizes, visibility, models,
Ks, distortions, Rs_rvec, Ts, flagsForIntrinsics);
CV_LOG_INFO(NULL, "RMS: " << rms);
EXPECT_LE(rms, .3);
Rs.resize(Rs_rvec.size());
for(int c = 0; c < 3; c++)
{
cv::Rodrigues(Rs_rvec[c], Rs[c]);
CV_LOG_INFO(NULL, "R" << c << ":" << Rs[c]);
CV_LOG_INFO(NULL, "T" << c << ":" << Ts[c]);
}
validateAllPoses(Rs_gt, Ts_gt, Rs, Ts);
}
TEST_F(MultiViewTest, OneLineInitialGuess)
{
const string root = cvtest::TS::ptr()->get_data_path() + "cv/cameracalibration/multiview/3cams-one-line/";
const std::vector<std::string> cam_names = {"cam_0", "cam_1", "cam_3"};
const std::vector<cv::Size> image_sizes = {{1920, 1080}, {1920, 1080}, {1920, 1080} };
std::vector<uchar> models(3, cv::CALIB_MODEL_PINHOLE);
double rs_1_gt_data[9] = {
0.9996914489704484, -0.01160060078752197, -0.02196435559568884,
0.012283315339906, 0.9994374509454836, 0.03120739995344806,
0.02158997497973892, -0.03146756598408248, 0.9992715673286274
};
double rs_2_gt_data[9] = {
0.9988848194142131, -0.0255827884561986, -0.03968171466355882,
0.0261796234191418, 0.999550713317242, 0.0145944792515729,
0.03929051872229011, -0.0156170561181697, 0.9991057815350362
};
double ts_1_gt_data[3] = {0.5078811293323259, 0.002753469433719865, 0.02413521839310227};
double ts_2_gt_data[3] = {1.007213763725429, 0.01645068247976361, 0.05394643957910365};
std::vector<cv::Mat> Rs_gt = {
cv::Mat::eye(3, 3, CV_64FC1),
cv::Mat(3, 3, CV_64FC1, rs_1_gt_data),
cv::Mat(3, 3, CV_64FC1, rs_2_gt_data)
};
std::vector<cv::Mat> Ts_gt = {
cv::Mat::zeros(3, 1, CV_64FC1),
cv::Mat(3, 1, CV_64FC1, ts_1_gt_data),
cv::Mat(3, 1, CV_64FC1, ts_2_gt_data)
};
const int num_frames = 96;
std::vector<std::vector<cv::Mat>> image_points_all;
cv::Mat visibility;
loadImagePoints(root, cam_names, num_frames, image_points_all, visibility);
EXPECT_EQ(cam_names.size(), image_points_all.size());
for(size_t i = 0; i < cam_names.size(); i++)
{
EXPECT_TRUE(!image_points_all[i].empty());
}
std::vector<cv::Vec3f> board_pattern = genAsymmetricObjectPoints();
std::vector<std::vector<cv::Vec3f>> objPoints(num_frames, board_pattern);
std::vector<int> flagsForIntrinsics(3, CALIB_RATIONAL_MODEL);
std::vector<cv::Mat> Ks, distortions;
std::vector<cv::Mat> Rs(3);
std::vector<cv::Mat> Ts(3);
std::vector<cv::Mat> Rs_rvec(3);
for(int c = 0; c < 3; c++)
{
Mat K, dist;
double mono_rms = calibrateMono(board_pattern, image_points_all[c], image_sizes[c],
cv::CALIB_MODEL_PINHOLE, cv::CALIB_RATIONAL_MODEL,
K, dist);
CV_LOG_INFO(NULL, "K:" << K);
CV_LOG_INFO(NULL, "dist:" << dist);
Ks.push_back(K);
distortions.push_back(dist);
CV_LOG_INFO(NULL, "Calibrate mono RMS #" << c << ": " << mono_rms);
EXPECT_LE(mono_rms, .3);
}
const auto euler2rot = [] (double x, double y, double z) {
cv::Matx33d R_x(1, 0, 0, 0, cos(x), -sin(x), 0, sin(x), cos(x));
cv::Matx33d R_y(cos(y), 0, sin(y), 0, 1, 0, -sin(y), 0, cos(y));
cv::Matx33d R_z(cos(z), -sin(z), 0, sin(z), cos(z), 0, 0, 0, 1);
return cv::Mat(R_z * R_y * R_x);
};
// Introduce small noise by rotating ground truth camera pose a bit
Rs[0] = Rs_gt[0].clone();
Ts[0] = Ts_gt[0].clone();
double sign = 1.;
for (int c = 1; c < 3; c++)
{
Mat noise = euler2rot(0., sign*M_PI/180., 0.);
sign *= -1.;
Rs[c] = noise*Rs_gt[c];
Ts[c] = Ts_gt[c].clone();
cv::Rodrigues(Rs[c], Rs_rvec[c]);
}
int flags = cv::CALIB_USE_EXTRINSIC_GUESS | cv::CALIB_USE_INTRINSIC_GUESS | cv::CALIB_STEREO_REGISTRATION;
double rms = calibrateMultiview(objPoints, image_points_all, image_sizes, visibility, models,
Ks, distortions, Rs_rvec, Ts, flagsForIntrinsics, flags);
CV_LOG_INFO(NULL, "RMS: " << rms);
EXPECT_LE(rms, .3);
Rs.resize(Rs_rvec.size());
for(int c = 0; c < 3; c++)
{
cv::Rodrigues(Rs_rvec[c], Rs[c]);
CV_LOG_INFO(NULL, "R" << c << ":" << Rs[c]);
CV_LOG_INFO(NULL, "T" << c << ":" << Ts[c]);
}
validateAllPoses(Rs_gt, Ts_gt, Rs, Ts);
}
TEST_F(MultiViewTest, CamsToFloor)
{
const string root = cvtest::TS::ptr()->get_data_path() + "cv/cameracalibration/multiview/3cams-to-floor/";
const std::vector<std::string> cam_names = {"cam_0", "cam_1", "cam_2"};
std::vector<cv::Size> image_sizes = {{1920, 1080}, {1920, 1080}, {1280, 720}};
std::vector<uchar> models(3, cv::CALIB_MODEL_PINHOLE);
double rs_1_gt_data[9] = {
-0.05217184989559624, 0.6470741242690249, -0.7606399777686852,
-0.526982982144755, 0.6291523784496631, 0.5713634755748329,
0.8482729717539585, 0.4306534133065782, 0.3081730082260634
};
double rs_2_gt_data[9] = {
0.001580678474783847, -0.62542080411436, 0.7802860496231537,
0.4843796328138114, 0.683118871472744, 0.5465573883435866,
-0.8748564869569847, 0.3770907387072139, 0.304020890746888
};
double ts_1_gt_data[3] = {1.064278166833888, -0.7727142268275895, 1.140555926119704};
double ts_2_gt_data[3] = {-0.9391478506021244, -1.048084838193036, 1.3973875466639};
std::vector<cv::Mat> Rs_gt = {
cv::Mat::eye(3, 3, CV_64FC1),
cv::Mat(3, 3, CV_64FC1, rs_1_gt_data),
cv::Mat(3, 3, CV_64FC1, rs_2_gt_data)
};
std::vector<cv::Mat> Ts_gt = {
cv::Mat::zeros(3, 1, CV_64FC1),
cv::Mat(3, 1, CV_64FC1, ts_1_gt_data),
cv::Mat(3, 1, CV_64FC1, ts_2_gt_data)
};
const int num_frames = 125;
std::vector<std::vector<cv::Mat>> image_points_all;
cv::Mat visibility;
loadImagePoints(root, cam_names, num_frames, image_points_all, visibility);
ASSERT_EQ(cam_names.size(), image_points_all.size());
ASSERT_TRUE(!image_points_all.empty());
for(size_t i = 0; i < cam_names.size(); i++)
{
EXPECT_TRUE(!image_points_all[i].empty());
}
std::vector<cv::Vec3f> board_pattern = genAsymmetricObjectPoints();
std::vector<std::vector<cv::Vec3f>> objPoints(num_frames, board_pattern);
std::vector<int> flagsForIntrinsics(3, cv::CALIB_RATIONAL_MODEL);
std::vector<cv::Mat> Ks, distortions, Rs, Rs_rvec, Ts;
double rms = calibrateMultiview(objPoints, image_points_all, image_sizes, visibility, models,
Ks, distortions, Rs_rvec, Ts, flagsForIntrinsics);
CV_LOG_INFO(NULL, "RMS: " << rms);
EXPECT_LE(rms, 1.);
Rs.resize(Rs_rvec.size());
for(int c = 0; c < 3; c++)
{
cv::Rodrigues(Rs_rvec[c], Rs[c]);
CV_LOG_INFO(NULL, "R" << c << ":" << Rs[c]);
CV_LOG_INFO(NULL, "T" << c << ":" << Ts[c]);
}
validateAllPoses(Rs_gt, Ts_gt, Rs, Ts);
}
TEST_F(MultiViewTest, Hetero)
{
const string root = cvtest::TS::ptr()->get_data_path() + "cv/cameracalibration/multiview/3cams-hetero/";
const std::vector<std::string> cam_names = {"cam_7", "cam_4", "cam_8"};
std::vector<cv::Size> image_sizes = {{1920, 1080}, {1920, 1080}, {2048, 2048}};
std::vector<uchar> models = { cv::CALIB_MODEL_PINHOLE, cv::CALIB_MODEL_PINHOLE, cv::CALIB_MODEL_FISHEYE};
double rs_1_gt_data[9] = {
0.9927140815671712, 0.1070962138895326, 0.05521913824730116,
-0.05355858010980671, -0.01832224712027507, 0.9983966014350634,
0.1079362346706077, -0.994079823872807, -0.0124528315711911
};
double rs_2_gt_data[9] = {
0.9974414183162762, 0.06892036265048015, 0.0189894876008139,
-0.06886936047115397, 0.9976201373221448, -0.003327581349727079,
-0.0191736333413733, 0.002011273594291581, 0.9998141460106571
};
double ts_1_gt_data[3] = {0.5106665738153067, -0.3450096979616873, 0.7854530821015541};
double ts_2_gt_data[3] = {1.01304902944076, 0.01197702701032772, -0.01801263208619407};
std::vector<cv::Mat> Rs_gt = {
cv::Mat::eye(3, 3, CV_64FC1),
cv::Mat(3, 3, CV_64FC1, rs_1_gt_data),
cv::Mat(3, 3, CV_64FC1, rs_2_gt_data)
};
std::vector<cv::Mat> Ts_gt = {
cv::Mat::zeros(3, 1, CV_64FC1),
cv::Mat(3, 1, CV_64FC1, ts_1_gt_data),
cv::Mat(3, 1, CV_64FC1, ts_2_gt_data)
};
const int num_frames = 127;
std::vector<std::vector<cv::Mat>> image_points_all;
cv::Mat visibility;
loadImagePoints(root, cam_names, num_frames, image_points_all, visibility);
ASSERT_EQ(cam_names.size(), image_points_all.size());
ASSERT_TRUE(!image_points_all.empty());
for(size_t i = 0; i < cam_names.size(); i++)
{
EXPECT_TRUE(!image_points_all[i].empty());
}
std::vector<cv::Vec3f> board_pattern = genAsymmetricObjectPoints();
std::vector<std::vector<cv::Vec3f>> objPoints(num_frames, board_pattern);
std::vector<int> flagsForIntrinsics= {
cv::CALIB_RATIONAL_MODEL, cv::CALIB_RATIONAL_MODEL,
cv::CALIB_RECOMPUTE_EXTRINSIC | cv::CALIB_FIX_SKEW};
std::vector<cv::Mat> Ks, distortions, Rs, Rs_rvec, Ts;
double rms = calibrateMultiview(objPoints, image_points_all, image_sizes, visibility, models,
Ks, distortions, Rs_rvec, Ts, flagsForIntrinsics);
CV_LOG_INFO(NULL, "RMS: " << rms);
EXPECT_LE(rms, 2.5);
Rs.resize(Rs_rvec.size());
for(int c = 0; c < 3; c++)
{
cv::Rodrigues(Rs_rvec[c], Rs[c]);
CV_LOG_INFO(NULL, "R" << c << ":" << Rs[c]);
CV_LOG_INFO(NULL, "T" << c << ":" << Ts[c]);
}
validateAllPoses(Rs_gt, Ts_gt, Rs, Ts);
}
struct RegisterCamerasTest: public MultiViewTest
{
void filterPoints(const std::vector<std::vector<cv::Mat>>& image_points_all,
std::vector<cv::Mat>& visible_image_points1,
std::vector<cv::Mat>& visible_image_points2)
{
for (size_t i = 0; i < std::min(image_points_all[0].size(), image_points_all[1].size()); i++)
{
if(!image_points_all[0][i].empty() && !image_points_all[1][i].empty())
{
visible_image_points1.push_back(image_points_all[0][i]);
visible_image_points2.push_back(image_points_all[1][i]);
}
}
}
};
TEST_F(RegisterCamerasTest, hetero1)
{
const string root = cvtest::TS::ptr()->get_data_path() + "cv/cameracalibration/multiview/3cams-hetero/";
const std::vector<std::string> cam_names = {"cam_7", "cam_4"};
std::vector<cv::Size> image_sizes = {{1920, 1080}, {2048, 2048}};
std::vector<cv::CameraModel> models = {cv::CALIB_MODEL_PINHOLE, cv::CALIB_MODEL_FISHEYE};
std::vector<int> flagsForIntrinsics = {cv::CALIB_RATIONAL_MODEL, cv::CALIB_RECOMPUTE_EXTRINSIC | cv::CALIB_FIX_SKEW};
const int num_frames = 127;
std::vector<cv::Vec3f> board_pattern = genAsymmetricObjectPoints();
double rs_1_gt_data[9] = {
0.9923998627583629, 0.1102270543935739, 0.05470382872247866,
-0.05295473891691575, -0.01873572048960163, 0.9984211377990636,
0.1110779367085268, -0.9937298270945939, -0.01275628155556733
};
cv::Mat R_gt(3, 3, CV_64FC1, rs_1_gt_data);
double ts_1_gt_data[3] = {0.5132123397314717, -0.345554256449513, 0.7851208074917889};
cv::Mat T_gt(3, 1, CV_64FC1, ts_1_gt_data);
std::vector<std::vector<cv::Mat>> image_points_all;
cv::Mat visibility;
loadImagePoints(root, cam_names, num_frames, image_points_all, visibility);
ASSERT_EQ(cam_names.size(), image_points_all.size());
ASSERT_TRUE(!image_points_all.empty());
for(size_t i = 0; i < cam_names.size(); i++)
{
ASSERT_TRUE(!image_points_all[i].empty());
}
cv::Mat K1, dist1;
double rms = calibrateMono(board_pattern, image_points_all[0], image_sizes[0], models[0], flagsForIntrinsics[0], K1, dist1);
CV_LOG_INFO(NULL, "Mono #1 RMS: " << rms);
EXPECT_LE(rms, 1.);
cv::Mat K2, dist2;
rms = calibrateMono(board_pattern, image_points_all[1], image_sizes[1], models[1], flagsForIntrinsics[1], K2, dist2);
CV_LOG_INFO(NULL, "Mono #2 RMS: " << rms);
EXPECT_LE(rms, 1.);
std::vector<cv::Mat> visible_image_points1, visible_image_points2;
filterPoints(image_points_all, visible_image_points1, visible_image_points2);
std::vector<std::vector<cv::Vec3f>> object_points(visible_image_points1.size(), board_pattern);
cv::Mat R, T, E, F;
cv::Mat rvec_reg, tvec_reg, per_view_err;
rms = registerCameras(object_points, object_points, visible_image_points1, visible_image_points2,
K1, dist1, cv::CALIB_MODEL_PINHOLE, K2, dist2, cv::CALIB_MODEL_FISHEYE,
R, T, E, F, rvec_reg, tvec_reg, per_view_err);
CV_LOG_INFO(NULL, "Register RMS: " << rms);
EXPECT_LE(rms, 1.);
CV_LOG_INFO(NULL, "R:" << R);
CV_LOG_INFO(NULL, "T:" << T);
validateCameraPose(R, T, R_gt, T_gt);
}
TEST_F(RegisterCamerasTest, hetero2)
{
const string root = cvtest::TS::ptr()->get_data_path() + "cv/cameracalibration/multiview/3cams-hetero/";
const std::vector<std::string> cam_names = {"cam_4", "cam_8"};
std::vector<cv::Size> image_sizes = {{2048, 2048}, {1920, 1080}};
std::vector<cv::CameraModel> models = {cv::CALIB_MODEL_FISHEYE, cv::CALIB_MODEL_PINHOLE};
std::vector<int> flagsForIntrinsics = { cv::CALIB_RECOMPUTE_EXTRINSIC | cv::CALIB_FIX_SKEW, cv::CALIB_RATIONAL_MODEL};
const int num_frames = 127;
std::vector<cv::Vec3f> board_pattern = genAsymmetricObjectPoints();
double rs_1_gt_data[9] = {
0.9987381520324473, -0.03742623778583679, 0.0334870183804049,
0.03272769253311544, -0.02072052049800844, -0.9992494974588425,
0.03809201775004091, 0.999084549352801, -0.01946949994840527
};
cv::Mat R_gt(3, 3, CV_64FC1, rs_1_gt_data);
double ts_1_gt_data[3] = {0.4660746974363485, 0.7703195273112146, 0.3243138654899712};
cv::Mat T_gt(3, 1, CV_64FC1, ts_1_gt_data);
std::vector<std::vector<cv::Mat>> image_points_all;
cv::Mat visibility;
loadImagePoints(root, cam_names, num_frames, image_points_all, visibility);
ASSERT_EQ(cam_names.size(), image_points_all.size());
ASSERT_TRUE(!image_points_all.empty());
for(size_t i = 0; i < cam_names.size(); i++)
{
ASSERT_TRUE(!image_points_all[i].empty());
}
cv::Mat K1, dist1;
double rms = calibrateMono(board_pattern, image_points_all[0], image_sizes[0], models[0], flagsForIntrinsics[0], K1, dist1);
CV_LOG_INFO(NULL, "Mono #1 RMS: " << rms);
EXPECT_LE(rms, 1.);
cv::Mat K2, dist2;
rms = calibrateMono(board_pattern, image_points_all[1], image_sizes[1], models[1], flagsForIntrinsics[1], K2, dist2);
CV_LOG_INFO(NULL, "Mono #2 RMS: " << rms);
EXPECT_LE(rms, 1.);
std::vector<cv::Mat> visible_image_points1, visible_image_points2;
filterPoints(image_points_all, visible_image_points1, visible_image_points2);
std::vector<std::vector<cv::Vec3f>> object_points(visible_image_points1.size(), board_pattern);
cv::Mat R, T, E, F;
cv::Mat rvec_reg, tvec_reg, per_view_err;
rms = registerCameras(object_points, object_points, visible_image_points1, visible_image_points2,
K1, dist1, cv::CALIB_MODEL_FISHEYE, K2, dist2, cv::CALIB_MODEL_PINHOLE,
R, T, E, F, rvec_reg, tvec_reg, per_view_err);
CV_LOG_INFO(NULL, "Register RMS: " << rms);
EXPECT_LE(rms, 1.);
CV_LOG_INFO(NULL, "R:" << R);
CV_LOG_INFO(NULL, "T:" << T);
validateCameraPose(R, T, R_gt, T_gt);
}
}}
modules/calib/test/test_precomp.hpp
+14
View File
@@ -0,0 +1,14 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html.
#ifndef __OPENCV_TEST_PRECOMP_HPP__
#define __OPENCV_TEST_PRECOMP_HPP__
#include <functional>
#include <numeric>
#include "opencv2/ts.hpp"
#include "opencv2/geometry.hpp"
#include "opencv2/calib.hpp"
#endif