mirror of
https://github.com/opencv/opencv.git
synced 2026-07-30 15:53:03 +04:00
Merge pull request #26221 from asmorkalov:as/refactor_multiview_interface
Reworked multiview calibration interface #26221 - Use InputArray / OutputArray - Use enum for camera type - Sort parameters according guidelines - Made more outputs optional - Introduce flags and added tests for intrinsics and extrinsics guess. ### Pull Request Readiness Checklist See details at https://github.com/opencv/opencv/wiki/How_to_contribute#making-a-good-pull-request - [x] I agree to contribute to the project under Apache 2 License. - [x] To the best of my knowledge, the proposed patch is not based on a code under GPL or another license that is incompatible with OpenCV - [ ] The PR is proposed to the proper branch - [ ] There is a reference to the original bug report and related work - [ ] There is accuracy test, performance test and test data in opencv_extra repository, if applicable Patch to opencv_extra has the same branch name. - [ ] The feature is well documented and sample code can be built with the project CMake
This commit is contained in:
Alexander Smorkalov
committed by
GitHub
GitHub
parent
6c743f8e4b
commit
3bcab8db0a
@@ -63,7 +63,7 @@ static double robustWrapper (const Mat& ptsErrors, Mat& weights, const RobustFun
|
||||
}
|
||||
|
||||
static double computeReprojectionMSE(const Mat &obj_points_, const Mat &img_points_, const Matx33d &K, const Mat &distortion,
|
||||
const Mat &rvec, const Mat &tvec, InputArray rvec2, InputArray tvec2, bool is_fisheye) {
|
||||
const Mat &rvec, const Mat &tvec, InputArray rvec2, InputArray tvec2, int model) {
|
||||
Mat r, t;
|
||||
if (!rvec2.empty() && !tvec2.empty()) {
|
||||
composeRT(rvec, tvec, rvec2, tvec2, r, t);
|
||||
@@ -71,11 +71,15 @@ static double computeReprojectionMSE(const Mat &obj_points_, const Mat &img_poin
|
||||
r = rvec; t = tvec;
|
||||
}
|
||||
Mat tmpImagePoints, obj_points = obj_points_, img_points = img_points_;
|
||||
if (is_fisheye) {
|
||||
if (model == cv::CALIB_MODEL_FISHEYE) {
|
||||
obj_points = obj_points.reshape(3); // must be 3 channels
|
||||
fisheye::projectPoints(obj_points, tmpImagePoints, r, t, K, distortion);
|
||||
} else
|
||||
} else if (model == cv::CALIB_MODEL_PINHOLE) {
|
||||
projectPoints(obj_points, r, t, K, distortion, tmpImagePoints);
|
||||
} else {
|
||||
CV_Error(Error::StsBadArg, "Unsupported camera model!");
|
||||
}
|
||||
|
||||
if (img_points.channels() != tmpImagePoints.channels())
|
||||
img_points = img_points.reshape(tmpImagePoints.channels());
|
||||
if (img_points.rows != tmpImagePoints.rows)
|
||||
@@ -257,12 +261,24 @@ static void thresholdPatternCameraAngles (int NUM_PATTERN_PTS, double THR_PATTER
|
||||
}
|
||||
|
||||
static void pairwiseStereoCalibration (const std::vector<std::pair<int,int>> &pairs,
|
||||
const std::vector<bool> &is_fisheye_vec, const std::vector<Mat> &objPoints_norm,
|
||||
const cv::Mat &models, const std::vector<Mat> &objPoints_norm,
|
||||
const std::vector<std::vector<Mat>> &imagePoints, const std::vector<std::vector<int>> &overlaps,
|
||||
const std::vector<std::vector<bool>> &detection_mask_mat, const std::vector<Mat> &Ks,
|
||||
const std::vector<Mat> &distortions, std::vector<Matx33d> &Rs_vec, std::vector<Vec3d> &Ts_vec,
|
||||
Mat &flags) {
|
||||
const int NUM_FRAMES = (int) objPoints_norm.size();
|
||||
Mat &intrinsic_flags, int extrinsic_flags = 0) {
|
||||
const int NUM_FRAMES = (int)objPoints_norm.size();
|
||||
const int NUM_CAMERAS = (int)detection_mask_mat.size();
|
||||
std::vector<Matx33d> Rs_prior;
|
||||
std::vector<Vec3d> Ts_prior;
|
||||
if (extrinsic_flags & cv::CALIB_USE_EXTRINSIC_GUESS) {
|
||||
Rs_prior.resize(NUM_CAMERAS);
|
||||
Ts_prior.resize(NUM_CAMERAS);
|
||||
for (int i = 0; i < NUM_CAMERAS; i++) {
|
||||
Rs_vec[i].copyTo(Rs_prior[i]);
|
||||
Ts_vec[i].copyTo(Ts_prior[i]);
|
||||
}
|
||||
}
|
||||
|
||||
for (const auto &pair : pairs) {
|
||||
const int c1 = pair.first, c2 = pair.second, overlap = overlaps[c1][c2];
|
||||
// prepare image points of two cameras and grid points
|
||||
@@ -270,7 +286,8 @@ static void pairwiseStereoCalibration (const std::vector<std::pair<int,int>> &pa
|
||||
grid_points.reserve(overlap);
|
||||
image_points1.reserve(overlap);
|
||||
image_points2.reserve(overlap);
|
||||
const bool are_fisheye_cams = is_fisheye_vec[c1] && is_fisheye_vec[c2];
|
||||
const bool are_fisheye_cams = models.at<uchar>(c1) == cv::CALIB_MODEL_FISHEYE &&
|
||||
models.at<uchar>(c2) == cv::CALIB_MODEL_FISHEYE;
|
||||
for (int f = 0; f < NUM_FRAMES; f++) {
|
||||
if (detection_mask_mat[c1][f] && detection_mask_mat[c2][f]) {
|
||||
grid_points.emplace_back((are_fisheye_cams && objPoints_norm[f].channels() != 3) ?
|
||||
@@ -283,23 +300,30 @@ static void pairwiseStereoCalibration (const std::vector<std::pair<int,int>> &pa
|
||||
}
|
||||
Matx33d R;
|
||||
Vec3d T;
|
||||
|
||||
if (extrinsic_flags & cv::CALIB_USE_EXTRINSIC_GUESS) {
|
||||
R = Rs_prior[c2] * Rs_prior[c1].t();
|
||||
T = -R * Ts_prior[c1] + Ts_prior[c2];
|
||||
}
|
||||
|
||||
// image size does not matter since intrinsics are used
|
||||
if (are_fisheye_cams) {
|
||||
extrinsic_flags |= CALIB_FIX_INTRINSIC;
|
||||
fisheye::stereoCalibrate(grid_points, image_points1, image_points2,
|
||||
Ks[c1], distortions[c1],
|
||||
Ks[c2], distortions[c2],
|
||||
Size(), R, T, CALIB_FIX_INTRINSIC);
|
||||
Size(), R, T, extrinsic_flags);
|
||||
} else {
|
||||
int flags_extrinsics = CALIB_FIX_INTRINSIC;
|
||||
if ((flags.at<int>(c1) & CALIB_RATIONAL_MODEL) || (flags.at<int>(c2) & CALIB_RATIONAL_MODEL))
|
||||
flags_extrinsics += CALIB_RATIONAL_MODEL;
|
||||
if ((flags.at<int>(c1) & CALIB_THIN_PRISM_MODEL) || (flags.at<int>(c2) & CALIB_THIN_PRISM_MODEL))
|
||||
flags_extrinsics += CALIB_THIN_PRISM_MODEL;
|
||||
extrinsic_flags |= CALIB_FIX_INTRINSIC;
|
||||
if ((intrinsic_flags.at<int>(c1) & CALIB_RATIONAL_MODEL) || (intrinsic_flags.at<int>(c2) & CALIB_RATIONAL_MODEL))
|
||||
extrinsic_flags |= CALIB_RATIONAL_MODEL;
|
||||
if ((intrinsic_flags.at<int>(c1) & CALIB_THIN_PRISM_MODEL) || (intrinsic_flags.at<int>(c2) & CALIB_THIN_PRISM_MODEL))
|
||||
extrinsic_flags |= CALIB_THIN_PRISM_MODEL;
|
||||
|
||||
stereoCalibrate(grid_points, image_points1, image_points2,
|
||||
Ks[c1], distortions[c1],
|
||||
Ks[c2], distortions[c2],
|
||||
Size(), R, T, noArray(), noArray(), noArray(), flags_extrinsics);
|
||||
Size(), R, T, noArray(), noArray(), noArray(), extrinsic_flags);
|
||||
}
|
||||
|
||||
// R_0 = I
|
||||
@@ -319,7 +343,7 @@ static void optimizeLM (std::vector<double> ¶m, const RobustFunction &robust
|
||||
const std::vector<bool> &valid_frames, const std::vector<std::vector<bool>> &detection_mask_mat,
|
||||
const std::vector<Mat> &objPoints_norm, const std::vector<std::vector<Mat>> &imagePoints,
|
||||
const std::vector<Mat> &Ks, const std::vector<Mat> &distortions,
|
||||
const std::vector<bool> &is_fisheye_vec, int NUM_PATTERN_PTS) {
|
||||
const Mat& models, int NUM_PATTERN_PTS) {
|
||||
const int NUM_FRAMES = (int) objPoints_norm.size(), NUM_CAMERAS = (int)detection_mask_mat.size();
|
||||
int iters_lm = 0, cnt_valid_frame = 0;
|
||||
auto lmcallback = [&](InputOutputArray _param, OutputArray JtErr_, OutputArray JtJ_, double& errnorm) {
|
||||
@@ -358,7 +382,7 @@ static void optimizeLM (std::vector<double> ¶m, const RobustFunction &robust
|
||||
Mat imgpt_ik = imagePoints[k][i].reshape(2, 1);
|
||||
imgpt_ik.convertTo(imgpt_ik, CV_64FC2);
|
||||
|
||||
if (is_fisheye_vec[k]) {
|
||||
if (models.at<uchar>(k)) {
|
||||
if( JtJ_.needed() || JtErr_.needed() ) {
|
||||
Mat jacobian; // of size num_points*2 x 15 (2 + 2 + 1 + 4 + 3 + 3; // f, c, alpha, k, om, T)
|
||||
fisheye::projectPoints(objpt_i, tmpImagePoints, om[1], T[1], Ks[k], distortions[k], 0, jacobian);
|
||||
@@ -482,43 +506,55 @@ static void checkConnected (const std::vector<std::vector<bool>> &detection_mask
|
||||
}
|
||||
}
|
||||
|
||||
//TODO: use Input/OutputArrays for imagePoints, imageSize(?), Ks, distortions
|
||||
double calibrateMultiview (InputArrayOfArrays objPoints, const std::vector<std::vector<Mat>> &imagePoints,
|
||||
const std::vector<Size> &imageSize, InputArray detectionMask,
|
||||
OutputArrayOfArrays Rs, OutputArrayOfArrays Ts, std::vector<Mat> &Ks, std::vector<Mat> &distortions,
|
||||
OutputArrayOfArrays rvecs0, OutputArrayOfArrays tvecs0, InputArray isFisheye,
|
||||
OutputArray perFrameErrors, OutputArray initializationPairs, bool useIntrinsicsGuess, InputArray flagsForIntrinsics) {
|
||||
//TODO: use Input/OutputArrays for imagePoints(?)
|
||||
double calibrateMultiview(
|
||||
InputArrayOfArrays objPoints, const std::vector<std::vector<Mat>> &imagePoints,
|
||||
const std::vector<cv::Size>& imageSize, InputArray detectionMask, InputArray models,
|
||||
InputOutputArrayOfArrays Rs, InputOutputArrayOfArrays Ts,
|
||||
InputOutputArrayOfArrays Ks, InputOutputArrayOfArrays distortions,
|
||||
int flags, InputArray flagsForIntrinsics,
|
||||
OutputArrayOfArrays rvecs0, OutputArrayOfArrays tvecs0,
|
||||
OutputArray perFrameErrors, OutputArray initializationPairs) {
|
||||
|
||||
CV_CheckEQ((int)objPoints.empty(), 0, "Objects points must not be empty!");
|
||||
CV_CheckEQ((int)imagePoints.empty(), 0, "Image points must not be empty!");
|
||||
CV_CheckEQ((int)imageSize.empty(), 0, "Image size per camera must not be empty!");
|
||||
CV_CheckEQ((int)detectionMask.empty(), 0, "detectionMask matrix must not be empty!");
|
||||
CV_CheckEQ((int)isFisheye.empty(), 0, "Fisheye mask must not be empty!");
|
||||
CV_CheckFalse(objPoints.empty(), "Objects points must not be empty!");
|
||||
CV_CheckFalse(imagePoints.empty(), "Image points must not be empty!");
|
||||
CV_CheckFalse(imageSize.empty(), "Image size per camera must not be empty!");
|
||||
CV_CheckFalse(detectionMask.empty(), "detectionMask matrix must not be empty!");
|
||||
CV_CheckFalse(models.empty(), "Fisheye mask must not be empty!");
|
||||
|
||||
Mat detection_mask_ = detectionMask.getMat();
|
||||
Mat models_mat = models.getMat();
|
||||
|
||||
Mat detection_mask_ = detectionMask.getMat(), is_fisheye_mat = isFisheye.getMat();
|
||||
CV_CheckEQ(detection_mask_.type(), CV_8U, "detectionMask must be of type CV_8U");
|
||||
CV_CheckEQ(is_fisheye_mat.type(), CV_8U, "isFisheye must be of type CV_8U");
|
||||
CV_CheckEQ(models_mat.type(), CV_8U, "models must be of type CV_8U");
|
||||
|
||||
bool is_fisheye = false;
|
||||
bool is_pinhole = false;
|
||||
|
||||
for (int i = 0; i < (int)is_fisheye_mat.total(); i++) {
|
||||
if (is_fisheye_mat.at<uchar>(i)) {
|
||||
for (int i = 0; i < (int)models_mat.total(); i++) {
|
||||
if (models_mat.at<uchar>(i) == cv::CALIB_MODEL_FISHEYE) {
|
||||
is_fisheye = true;
|
||||
} else {
|
||||
} else if (models_mat.at<uchar>(i) == cv::CALIB_MODEL_PINHOLE) {
|
||||
is_pinhole = true;
|
||||
} else {
|
||||
CV_Error(Error::StsBadArg, "Unsupported camera model");
|
||||
}
|
||||
}
|
||||
CV_CheckEQ(is_fisheye && is_pinhole, false, "Mix of pinhole and fisheye cameras is not supported for now");
|
||||
|
||||
// equal number of cameras
|
||||
CV_Assert(imageSize.size() == imagePoints.size());
|
||||
CV_Assert(detection_mask_.rows == std::max(isFisheye.rows(), isFisheye.cols()));
|
||||
CV_Assert(detection_mask_.rows == std::max(models.rows(), models.cols()));
|
||||
CV_Assert(detection_mask_.rows == (int)imageSize.size());
|
||||
CV_Assert(detection_mask_.cols == std::max(objPoints.rows(), objPoints.cols())); // equal number of frames
|
||||
CV_Assert(Rs.isMatVector() == Ts.isMatVector());
|
||||
if (useIntrinsicsGuess) {
|
||||
CV_Assert(Ks.size() == distortions.size() && Ks.size() == imageSize.size());
|
||||
if (flags & cv::CALIB_USE_INTRINSIC_GUESS) {
|
||||
CV_Assert(Ks.isMatVector() && distortions.isMatVector());
|
||||
CV_Assert(Ks.total() == distortions.total() && Ks.total() == imageSize.size());
|
||||
}
|
||||
if (flags & cv::CALIB_USE_EXTRINSIC_GUESS) {
|
||||
CV_Assert(Rs.isMatVector() && Ts.isMatVector());
|
||||
CV_Assert(Rs.total() == Ts.total() && Rs.total() == imageSize.size());
|
||||
}
|
||||
// normalize object points
|
||||
const Mat obj_pts_0 = objPoints.getMat(0);
|
||||
@@ -555,11 +591,9 @@ double calibrateMultiview (InputArrayOfArrays objPoints, const std::vector<std::
|
||||
std::vector<bool> valid_frames(NUM_FRAMES, false);
|
||||
// process input and count all visible frames and points
|
||||
|
||||
std::vector<bool> is_fisheye_vec(NUM_CAMERAS);
|
||||
std::vector<std::vector<bool>> detection_mask_mat(NUM_CAMERAS, std::vector<bool>(NUM_FRAMES));
|
||||
const auto * const detection_mask_ptr = detection_mask_.data, * const is_fisheye_ptr = is_fisheye_mat.data;
|
||||
const auto * const detection_mask_ptr = detection_mask_.data;
|
||||
for (int c = 0; c < NUM_CAMERAS; c++) {
|
||||
is_fisheye_vec[c] = is_fisheye_ptr[c] != 0;
|
||||
int num_visible_frames = 0;
|
||||
for (int f = 0; f < NUM_FRAMES; f++) {
|
||||
detection_mask_mat[c][f] = detection_mask_ptr[c*NUM_FRAMES + f] != 0;
|
||||
@@ -589,11 +623,13 @@ double calibrateMultiview (InputArrayOfArrays objPoints, const std::vector<std::
|
||||
std::vector<int> camera_rt_best(NUM_FRAMES, -1);
|
||||
std::vector<double> camera_rt_errors(NUM_FRAMES, std::numeric_limits<double>::max());
|
||||
const double WARNING_RMSE = 15.;
|
||||
if (!useIntrinsicsGuess) {
|
||||
if ((flags & cv::CALIB_USE_INTRINSIC_GUESS) == 0) {
|
||||
Ks.create(NUM_CAMERAS, 1, CV_64F);
|
||||
distortions.create(NUM_CAMERAS, 1, CV_64F);
|
||||
|
||||
// calibrate each camera independently to find intrinsic parameters - K and distortion coefficients
|
||||
distortions = std::vector<Mat>(NUM_CAMERAS);
|
||||
Ks = std::vector<Mat>(NUM_CAMERAS);
|
||||
for (int camera = 0; camera < NUM_CAMERAS; camera++) {
|
||||
Mat K, dist;
|
||||
Mat rvecs, tvecs;
|
||||
std::vector<Mat> obj_points_, img_points_;
|
||||
std::vector<double> errors_per_view;
|
||||
@@ -601,25 +637,25 @@ double calibrateMultiview (InputArrayOfArrays objPoints, const std::vector<std::
|
||||
img_points_.reserve(num_visible_frames_per_camera[camera]);
|
||||
for (int f = 0; f < NUM_FRAMES; f++) {
|
||||
if (detection_mask_mat[camera][f]) {
|
||||
obj_points_.emplace_back((is_fisheye_vec[camera] && objPoints_norm[f].channels() != 3) ?
|
||||
obj_points_.emplace_back((models_mat.at<uchar>(camera) == cv::CALIB_MODEL_FISHEYE && objPoints_norm[f].channels() != 3) ?
|
||||
objPoints_norm[f].reshape(3): objPoints_norm[f]);
|
||||
img_points_.emplace_back((is_fisheye_vec[camera] && imagePoints[camera][f].channels() != 2) ?
|
||||
img_points_.emplace_back((models_mat.at<uchar>(camera) == cv::CALIB_MODEL_FISHEYE && imagePoints[camera][f].channels() != 2) ?
|
||||
imagePoints[camera][f].reshape(2) : imagePoints[camera][f]);
|
||||
}
|
||||
}
|
||||
double repr_err;
|
||||
if (is_fisheye_vec[camera]) {
|
||||
if (models_mat.at<uchar>(camera) == cv::CALIB_MODEL_FISHEYE) {
|
||||
repr_err = fisheye::calibrate(obj_points_, img_points_, imageSize[camera],
|
||||
Ks[camera], distortions[camera], rvecs, tvecs, flagsForIntrinsics_mat.at<int>(camera));
|
||||
K, dist, rvecs, tvecs, flagsForIntrinsics_mat.at<int>(camera));
|
||||
// calibrate does not compute error per view, so compute it manually
|
||||
errors_per_view = std::vector<double>(obj_points_.size());
|
||||
for (int f = 0; f < (int) obj_points_.size(); f++) {
|
||||
double err2 = multiview::computeReprojectionMSE(obj_points_[f],
|
||||
img_points_[f], Ks[camera], distortions[camera], rvecs.row(f), tvecs.row(f), noArray(), noArray(), true);
|
||||
img_points_[f], K, dist, rvecs.row(f), tvecs.row(f), noArray(), noArray(), cv::CALIB_MODEL_FISHEYE);
|
||||
errors_per_view[f] = sqrt(err2);
|
||||
}
|
||||
} else {
|
||||
repr_err = calibrateCamera(obj_points_, img_points_, imageSize[camera], Ks[camera], distortions[camera],
|
||||
repr_err = calibrateCamera(obj_points_, img_points_, imageSize[camera], K, dist,
|
||||
rvecs, tvecs, noArray(), noArray(), errors_per_view, flagsForIntrinsics_mat.at<int>(camera));
|
||||
}
|
||||
CV_LOG_IF_WARNING(NULL, repr_err > WARNING_RMSE, "Warning! Mean RMSE of intrinsics calibration is higher than "+std::to_string(WARNING_RMSE)+" pixels!");
|
||||
@@ -637,6 +673,11 @@ double calibrateMultiview (InputArrayOfArrays objPoints, const std::vector<std::
|
||||
cnt_visible_frame++;
|
||||
}
|
||||
}
|
||||
|
||||
Ks.create(K.rows, K.cols, CV_64F, camera);
|
||||
distortions.create(dist.rows, dist.cols, CV_64F, camera, true);
|
||||
K.copyTo(Ks.getMat(camera));
|
||||
dist.copyTo(distortions.getMat(camera));
|
||||
}
|
||||
} else {
|
||||
// use PnP to compute rvecs and tvecs
|
||||
@@ -644,10 +685,10 @@ double calibrateMultiview (InputArrayOfArrays objPoints, const std::vector<std::
|
||||
for (int k = 0; k < NUM_CAMERAS; k++) {
|
||||
if (!detection_mask_mat[k][i]) continue;
|
||||
Vec3d rvec, tvec;
|
||||
solvePnP(objPoints_norm[i], imagePoints[k][i], Ks[k], distortions[k], rvec, tvec, false, SOLVEPNP_ITERATIVE);
|
||||
solvePnP(objPoints_norm[i], imagePoints[k][i], Ks.getMat(k), distortions.getMat(k), rvec, tvec, false, SOLVEPNP_ITERATIVE);
|
||||
rvecs_all[k][i] = rvec;
|
||||
tvecs_all[k][i] = tvec;
|
||||
const double err2 = multiview::computeReprojectionMSE(objPoints_norm[i], imagePoints[k][i], Ks[k], distortions[k], Mat(rvec), Mat(tvec), noArray(), noArray(), is_fisheye_vec[k]);
|
||||
const double err2 = multiview::computeReprojectionMSE(objPoints_norm[i], imagePoints[k][i], Ks.getMat(k), distortions.getMat(k), Mat(rvec), Mat(tvec), noArray(), noArray(), models_mat.at<uchar>(k));
|
||||
if (camera_rt_errors[i] > err2) {
|
||||
camera_rt_errors[i] = err2;
|
||||
camera_rt_best[i] = k;
|
||||
@@ -656,6 +697,10 @@ double calibrateMultiview (InputArrayOfArrays objPoints, const std::vector<std::
|
||||
}
|
||||
}
|
||||
|
||||
std::vector<cv::Mat> Ks_vec, distortions_vec;
|
||||
Ks.getMatVector(Ks_vec);
|
||||
distortions.getMatVector(distortions_vec);
|
||||
|
||||
std::vector<std::vector<bool>> is_valid_angle2pattern;
|
||||
multiview::thresholdPatternCameraAngles(NUM_PATTERN_PTS, THR_PATTERN_CAMERA_ANGLES, objPoints_norm, rvecs_all, opt_axes, is_valid_angle2pattern);
|
||||
|
||||
@@ -687,8 +732,8 @@ double calibrateMultiview (InputArrayOfArrays objPoints, const std::vector<std::
|
||||
}
|
||||
pairs_mat.copyTo(initializationPairs);
|
||||
}
|
||||
multiview::pairwiseStereoCalibration(pairs, is_fisheye_vec, objPoints_norm, imagePoints,
|
||||
overlaps, detection_mask_mat, Ks, distortions, Rs_vec, Ts_vec, flagsForIntrinsics_mat);
|
||||
multiview::pairwiseStereoCalibration(pairs, models_mat, objPoints_norm, imagePoints,
|
||||
overlaps, detection_mask_mat, Ks_vec, distortions_vec, Rs_vec, Ts_vec, flagsForIntrinsics_mat);
|
||||
|
||||
const int NUM_VALID_FRAMES = countNonZero(valid_frames);
|
||||
const int nparams = (NUM_VALID_FRAMES + NUM_CAMERAS - 1) * 6; // rvecs + tvecs (6)
|
||||
@@ -739,7 +784,8 @@ double calibrateMultiview (InputArrayOfArrays objPoints, const std::vector<std::
|
||||
TermCriteria termCrit (TermCriteria::COUNT+TermCriteria::EPS, 100, 1e-6);
|
||||
const float RBS_FNC_SCALE = 30;
|
||||
multiview::RobustExpFunction robust_fnc(RBS_FNC_SCALE);
|
||||
multiview::optimizeLM(param, robust_fnc, termCrit, valid_frames, detection_mask_mat, objPoints_norm, imagePoints, Ks, distortions, is_fisheye_vec, NUM_PATTERN_PTS);
|
||||
multiview::optimizeLM(param, robust_fnc, termCrit, valid_frames, detection_mask_mat, objPoints_norm,
|
||||
imagePoints, Ks_vec, distortions_vec, models_mat, NUM_PATTERN_PTS);
|
||||
const auto * const params = ¶m[0];
|
||||
|
||||
// extract extrinsics (R_i, t_i) for i = 1 ... NUM_CAMERAS:
|
||||
@@ -779,67 +825,65 @@ double calibrateMultiview (InputArrayOfArrays objPoints, const std::vector<std::
|
||||
ts.copyTo(Ts);
|
||||
}
|
||||
Mat rvecs0_, tvecs0_;
|
||||
if (rvecs0.needed() || perFrameErrors.needed()) {
|
||||
const bool is_mat_vec = rvecs0.needed() && rvecs0.isMatVector();
|
||||
if (is_mat_vec) {
|
||||
rvecs0.create(NUM_FRAMES, 1, CV_64F);
|
||||
} else {
|
||||
rvecs0_ = Mat_<double>(NUM_FRAMES, 3);
|
||||
}
|
||||
cnt_valid_frame = 0;
|
||||
bool is_mat_vec = rvecs0.needed() && rvecs0.isMatVector();
|
||||
if (is_mat_vec) {
|
||||
rvecs0.create(NUM_FRAMES, 1, CV_64F);
|
||||
} else {
|
||||
rvecs0_ = Mat_<double>(NUM_FRAMES, 3);
|
||||
}
|
||||
cnt_valid_frame = 0;
|
||||
for (int f = 0; f < NUM_FRAMES; f++) {
|
||||
if (!valid_frames[f]) continue;
|
||||
if (is_mat_vec)
|
||||
rvecs0.create(3, 1, CV_64F, f, true);
|
||||
Mat store = is_mat_vec ? rvecs0.getMat(f) : rvecs0_.row(f);
|
||||
memcpy(store.ptr(), params + (cnt_valid_frame + NUM_CAMERAS - 1)*6, 3*sizeof(double));
|
||||
cnt_valid_frame += 1;
|
||||
}
|
||||
if (!is_mat_vec && rvecs0.needed())
|
||||
rvecs0_.copyTo(rvecs0);
|
||||
|
||||
is_mat_vec = tvecs0.needed() && tvecs0.isMatVector();
|
||||
if (is_mat_vec) {
|
||||
tvecs0.create(NUM_FRAMES, 1, CV_64F);
|
||||
} else {
|
||||
tvecs0_ = Mat_<double>(NUM_FRAMES, 3);
|
||||
}
|
||||
cnt_valid_frame = 0;
|
||||
for (int f = 0; f < NUM_FRAMES; f++) {
|
||||
if (!valid_frames[f]) continue;
|
||||
if (is_mat_vec)
|
||||
tvecs0.create(3, 1, CV_64F, f, true);
|
||||
Mat store = is_mat_vec ? tvecs0.getMat(f) : tvecs0_.row(f);
|
||||
memcpy(store.ptr(), params + (cnt_valid_frame + NUM_CAMERAS - 1)*6+3, 3*sizeof(double));
|
||||
store *= scale_3d_pts;
|
||||
cnt_valid_frame += 1;
|
||||
}
|
||||
if (!is_mat_vec && tvecs0.needed())
|
||||
tvecs0_.copyTo(tvecs0);
|
||||
double sum_errors = 0, cnt_errors = 0;
|
||||
|
||||
const bool rvecs_mat_vec = rvecs0.needed() && rvecs0.isMatVector(), tvecs_mat_vec = tvecs0.needed() && tvecs0.isMatVector();
|
||||
const bool r_mat_vec = Rs.isMatVector(), t_mat_vec = Ts.isMatVector();
|
||||
Mat errs = Mat_<double>(NUM_CAMERAS, NUM_FRAMES);
|
||||
auto * errs_ptr = (double *) errs.data;
|
||||
for (int c = 0; c < NUM_CAMERAS; c++) {
|
||||
const Mat rvec = r_mat_vec ? Rs.getMat(c) : Rs.getMat().row(c).t();
|
||||
const Mat tvec = t_mat_vec ? Ts.getMat(c) : Ts.getMat().row(c).t();
|
||||
for (int f = 0; f < NUM_FRAMES; f++) {
|
||||
if (!valid_frames[f]) continue;
|
||||
if (is_mat_vec)
|
||||
rvecs0.create(3, 1, CV_64F, f, true);
|
||||
Mat store = is_mat_vec ? rvecs0.getMat(f) : rvecs0_.row(f);
|
||||
memcpy(store.ptr(), params + (cnt_valid_frame + NUM_CAMERAS - 1)*6, 3*sizeof(double));
|
||||
cnt_valid_frame += 1;
|
||||
if (detection_mask_mat[c][f]) {
|
||||
const Mat rvec0 = rvecs_mat_vec ? rvecs0.getMat(f) : rvecs0_.row(f).t();
|
||||
const Mat tvec0 = tvecs_mat_vec ? tvecs0.getMat(f) : tvecs0_.row(f).t();
|
||||
const double err2 = multiview::computeReprojectionMSE(objPoints.getMat(f), imagePoints[c][f], Ks_vec[c],
|
||||
distortions_vec[c], rvec0, tvec0, rvec, tvec, models_mat.at<uchar>(c));
|
||||
(*errs_ptr++) = sqrt(err2);
|
||||
sum_errors += err2;
|
||||
cnt_errors += 1;
|
||||
} else (*errs_ptr++) = -1.0;
|
||||
}
|
||||
if (!is_mat_vec && rvecs0.needed())
|
||||
rvecs0_.copyTo(rvecs0);
|
||||
}
|
||||
|
||||
if (tvecs0.needed() || perFrameErrors.needed()) {
|
||||
const bool is_mat_vec = tvecs0.needed() && tvecs0.isMatVector();
|
||||
if (is_mat_vec) {
|
||||
tvecs0.create(NUM_FRAMES, 1, CV_64F);
|
||||
} else {
|
||||
tvecs0_ = Mat_<double>(NUM_FRAMES, 3);
|
||||
}
|
||||
cnt_valid_frame = 0;
|
||||
for (int f = 0; f < NUM_FRAMES; f++) {
|
||||
if (!valid_frames[f]) continue;
|
||||
if (is_mat_vec)
|
||||
tvecs0.create(3, 1, CV_64F, f, true);
|
||||
Mat store = is_mat_vec ? tvecs0.getMat(f) : tvecs0_.row(f);
|
||||
memcpy(store.ptr(), params + (cnt_valid_frame + NUM_CAMERAS - 1)*6+3, 3*sizeof(double));
|
||||
store *= scale_3d_pts;
|
||||
cnt_valid_frame += 1;
|
||||
}
|
||||
if (!is_mat_vec && tvecs0.needed())
|
||||
tvecs0_.copyTo(tvecs0);
|
||||
}
|
||||
double sum_errors = 0, cnt_errors = 0;
|
||||
if (perFrameErrors.needed()) {
|
||||
const bool rvecs_mat_vec = rvecs0.needed() && rvecs0.isMatVector(), tvecs_mat_vec = tvecs0.needed() && tvecs0.isMatVector();
|
||||
const bool r_mat_vec = Rs.isMatVector(), t_mat_vec = Ts.isMatVector();
|
||||
Mat errs = Mat_<double>(NUM_CAMERAS, NUM_FRAMES);
|
||||
auto * errs_ptr = (double *) errs.data;
|
||||
for (int c = 0; c < NUM_CAMERAS; c++) {
|
||||
const Mat rvec = r_mat_vec ? Rs.getMat(c) : Rs.getMat().row(c).t();
|
||||
const Mat tvec = t_mat_vec ? Ts.getMat(c) : Ts.getMat().row(c).t();
|
||||
for (int f = 0; f < NUM_FRAMES; f++) {
|
||||
if (detection_mask_mat[c][f]) {
|
||||
const Mat rvec0 = rvecs_mat_vec ? rvecs0.getMat(f) : rvecs0_.row(f).t();
|
||||
const Mat tvec0 = tvecs_mat_vec ? tvecs0.getMat(f) : tvecs0_.row(f).t();
|
||||
const double err2 = multiview::computeReprojectionMSE(objPoints.getMat(f), imagePoints[c][f], Ks[c],
|
||||
distortions[c], rvec0, tvec0, rvec, tvec, is_fisheye_vec[c]);
|
||||
(*errs_ptr++) = sqrt(err2);
|
||||
sum_errors += err2;
|
||||
cnt_errors += 1;
|
||||
} else (*errs_ptr++) = -1.0;
|
||||
}
|
||||
}
|
||||
errs.copyTo(perFrameErrors);
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user