potok/opencv
1
0
Fork 0
mirror of https://github.com/opencv/opencv.git synced 2026-07-30 07:43:03 +04:00
Code Packages Releases Wiki Activity

Merge pull request #26221 from asmorkalov:as/refactor_multiview_interface

Browse Source 
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
2024-10-01 16:53:16 +03:00
committed by GitHub
parent 6c743f8e4b
commit 3bcab8db0a
8 changed files with 362 additions and 214 deletions
Download Patch File Download Diff File Expand all files Collapse all files
modules/calib/include/opencv2/calib.hpp
+14 -13
View File
@@ -449,7 +449,7 @@ enum { CALIB_USE_INTRINSIC_GUESS = 0x00001, //!< Use user provided intrinsics as
// for stereo rectification
CALIB_ZERO_DISPARITY = 0x00400, //!< Deprecated synonim of @ref STEREO_ZERO_DISPARITY. See @ref stereoRectify.
CALIB_USE_LU = (1 << 17), //!< use LU instead of SVD decomposition for solving. much faster but potentially less precise
CALIB_USE_EXTRINSIC_GUESS = (1 << 22), //!< For stereo calibration only. Use user provided extrinsics (R, T) as initial point for optimization
CALIB_USE_EXTRINSIC_GUESS = (1 << 22), //!< For stereo and multi-view calibration. Use user provided extrinsics (R, T) as initial point for optimization
// fisheye only flags
CALIB_RECOMPUTE_EXTRINSIC = (1 << 23), //!< For fisheye model only. Recompute board position on each calibration iteration
CALIB_CHECK_COND = (1 << 24), //!< For fisheye model only. Check SVD decomposition quality for each frame during extrinsics estimation
@@ -1236,16 +1236,14 @@ CV_EXPORTS_W double registerCameras( InputArrayOfArrays objectPoints1,
@param[in] imagePoints Detected pattern points on camera images. Expected shape: NUM_CAMERAS x NUM_FRAMES x NUM_POINTS x 2.
This function supports partial observation of the calibration pattern.
To enable this, set the unobserved image points to be invalid points (eg. (-1., -1.)).
@param[in] imageSize Images resolution.
@param[in] imageSize Images resolution array for each camera.
@param[in] detectionMask Pattern detection mask. Each value defines if i-camera observes the calibration pattern in j-th frame.
Expected size: NUM_CAMERAS x NUM_FRAMES. Expected type: CV_8U.
@param[in] isFisheye indicates whether i-th camera is fisheye. In case the input data contains
a mix of pinhole and fisheye cameras Rational distortion model is used. See @ref CALIB_RATIONAL_MODEL
for details. Expected type: CV_8U.
@param[in] useIntrinsicsGuess Use user-specified intrinsic parameters (internal camera matrix and distortion).
If true intrinsics are not estimated during calibration.
@param[in] models indicates camera models for each camera: cv::CALIB_MODEL_PINHOLE or cv::CALIB_MODEL_PINHOLE.
Current implementation does not support mix of different camera models. Expected type: CV_8U.
@param[in] flagsForIntrinsics Flags used for each camera intrinsics calibration.
Use per-camera call and the `useIntrinsicsGuess` flag to get custom intrinsics calibration for each camera.
@param[in] flags Common multiview calibration flags. cv::CALIB_USE_INTRINSIC_GUESS and cv::CALIB_USE_EXTRINSIC_GUESS are supported.
See @ref CALIB_USE_INTRINSIC_GUESS and other `CALIB_` constants. Expected shape: NUM_CAMERAS x 1. Supported data type: CV_32S.
@param[out] Rs Rotation vectors relative to camera 0, where Rs[0] = 0. Output size: NUM_CAMERAS x 3 x 3.
@param[out] Ts Estimated translation vectors relative to camera 0, where Ts[0] = 0. Output size: NUM_CAMERAS x 3 x 1.
@@ -1255,6 +1253,7 @@ See @ref CALIB_USE_INTRINSIC_GUESS and other `CALIB_` constants. Expected shape:
@param[out] distortions Distortion coefficients. Output size: NUM_CAMERAS x NUM_PARAMS.
@param[out] perFrameErrors RMSE value for each visible frame, (-1 for non-visible). Output size: NUM_CAMERAS x NUM_FRAMES.
@param[out] initializationPairs Pairs with camera indices that were used for initial pairwise stereo calibration.
Output size: (NUM_CAMERAS-1) x 2.
@ref tutorial_multiview_camera_calibration provides a detailed tutorial of using this function. Please refer to it for more information.
@@ -1288,12 +1287,14 @@ points in all the available views from all cameras.
@sa findChessboardCorners, findCirclesGrid, calibrateCamera, fisheye::calibrate, registerCameras
*/
CV_EXPORTS_W double calibrateMultiview (InputArrayOfArrays objPoints, const std::vector<std::vector<Mat>> &imagePoints,
const std::vector<Size> &imageSize, InputArray detectionMask,
OutputArrayOfArrays Rs, OutputArrayOfArrays Ts, CV_IN_OUT std::vector<Mat> &Ks, CV_IN_OUT std::vector<Mat> &distortions,
OutputArrayOfArrays rvecs0, OutputArrayOfArrays tvecs0, InputArray isFisheye,
OutputArray perFrameErrors, OutputArray initializationPairs,
bool useIntrinsicsGuess=false, InputArray flagsForIntrinsics=noArray());
CV_EXPORTS_W 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 = 0, InputArray flagsForIntrinsics=noArray(),
OutputArrayOfArrays rvecs0=noArray(), OutputArrayOfArrays tvecs0=noArray(),
OutputArray perFrameErrors=noArray(), OutputArray initializationPairs=noArray());
/** @brief Computes Hand-Eye calibration: \f$_{}^{g}\textrm{T}_c\f$
modules/calib/src/multiview_calibration.cpp
+152 -108
View File
@@ -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> &param, 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> &param, 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 = &param[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);
}
modules/calib/test/test_cameracalibration.cpp
+3 -3
View File
@@ -2482,10 +2482,10 @@ double CV_MultiviewCalibrationTest_CPP::calibrateStereoCamera( const vector<vect
}
std::vector<Size> image_sizes (2, imageSize);
Mat visibility_mat = Mat_<uchar>::ones(2, numImgs);
std::vector<uchar> is_fisheye(2, false);
std::vector<uchar> models(2, cv::CALIB_MODEL_PINHOLE);
std::vector<int> all_flags(2, flags);
double rms = calibrateMultiview(objectPoints, image_points_all, image_sizes, visibility_mat,
Rs, Ts, Ks, distortions, rvecs, tvecs, is_fisheye, errors_mat, noArray(), false, all_flags);
double rms = calibrateMultiview(objectPoints, image_points_all, image_sizes, visibility_mat, models,
Rs, Ts, Ks, distortions, 0, all_flags, rvecs, tvecs, errors_mat);
if (perViewErrors1.size() != (size_t)numImgs)
{
modules/calib/test/test_fisheye.cpp
+4 -4
View File
@@ -615,9 +615,9 @@ TEST_F(fisheyeTest, multiview_calibration)
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()), errors_mat, output_pairs;
cv::Mat visibility_mat = cv::Mat_<uchar>::ones(2, (int)leftPoints.size());
std::vector<cv::Mat> Rs, Ts, Ks, distortions, rvecs0, tvecs0;
std::vector<uchar> is_fisheye(2, true);
std::vector<uchar> models(2, cv::CALIB_MODEL_FISHEYE);
int flag = 0;
flag |= cv::CALIB_RECOMPUTE_EXTRINSIC;
flag |= cv::CALIB_CHECK_COND;
@@ -625,8 +625,8 @@ TEST_F(fisheyeTest, multiview_calibration)
std::vector<int> all_flags(2, flag);
calibrateMultiview (objectPoints, image_points_all, image_sizes, visibility_mat,
Rs, Ts, Ks, distortions, rvecs0, tvecs0, is_fisheye, errors_mat, output_pairs, false, all_flags);
calibrateMultiview(objectPoints, image_points_all, image_sizes, visibility_mat, models,
Rs, Ts, Ks, distortions, 0, all_flags, rvecs0, tvecs0);
cv::Matx33d R_correct( 0.9975587205950972, 0.06953016383322372, 0.006492709911733523,
-0.06956823121068059, 0.9975601387249519, 0.005833595226966235,
-0.006071257768382089, -0.006271040135405457, 0.9999619062167968);
modules/calib/test/test_multiview_calib.cpp
+149 -49
View File
@@ -27,7 +27,7 @@ TEST(multiview_calibration, accuracy) {
board_pattern[j*board_size.width+i] = cv::Vec3f ((float)i, (float)j, 0)*board_len;
}
}
std::vector<bool> is_fisheye(num_cameras, false);
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++) {
@@ -87,7 +87,7 @@ TEST(multiview_calibration, accuracy) {
int num_visible_patterns = 0;
for (int c = 0; c < num_cameras; c++) {
cv::Mat img_pts;
if (is_fisheye[c]) {
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);
@@ -132,9 +132,8 @@ TEST(multiview_calibration, accuracy) {
}
std::vector<cv::Mat> Ks, distortions, Rs, Ts;
cv::Mat errors_mat, output_pairs, rvecs0, tvecs0;
calibrateMultiview (objPoints, image_points_all, image_sizes, visibility_mat,
Rs, Ts, Ks, distortions, rvecs0, tvecs0, is_fisheye, errors_mat, output_pairs, false);
calibrateMultiview(objPoints, image_points_all, image_sizes, visibility_mat,
models, Rs, Ts, Ks, distortions);
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++) {
@@ -197,6 +196,40 @@ struct MultiViewTest : public ::testing::Test
}
}
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)
{
@@ -235,7 +268,7 @@ 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<bool> is_fisheye(3, false);
std::vector<uchar> models(3, cv::CALIB_MODEL_PINHOLE);
double rs_1_gt_data[9] = {
0.9996914489704484, -0.01160060078752197, -0.02196435559568884,
@@ -279,10 +312,113 @@ TEST_F(MultiViewTest, OneLine)
std::vector<int> flagsForIntrinsics(3, CALIB_RATIONAL_MODEL);
std::vector<cv::Mat> Ks, distortions, Rs, Rs_rvec, Ts;
cv::Mat errors_mat, output_pairs, rvecs0, tvecs0;
double rms = calibrateMultiview(objPoints, image_points_all, image_sizes, visibility,
Rs_rvec, Ts, Ks, distortions, rvecs0, tvecs0, is_fisheye, errors_mat, output_pairs,
false, flagsForIntrinsics);
double rms = calibrateMultiview(objPoints, image_points_all, image_sizes, visibility, models,
Rs_rvec, Ts, Ks, distortions, 0, 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;
double rms = calibrateMultiview(objPoints, image_points_all, image_sizes, visibility, models,
Rs_rvec, Ts, Ks, distortions, flags, flagsForIntrinsics);
CV_LOG_INFO(NULL, "RMS: " << rms);
EXPECT_LE(rms, .3);
@@ -303,7 +439,7 @@ 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<bool> is_fisheye(3, false);
std::vector<uchar> models(3, cv::CALIB_MODEL_PINHOLE);
double rs_1_gt_data[9] = {
-0.05217184989559624, 0.6470741242690249, -0.7606399777686852,
@@ -347,10 +483,8 @@ TEST_F(MultiViewTest, CamsToFloor)
std::vector<int> flagsForIntrinsics(3, cv::CALIB_RATIONAL_MODEL);
std::vector<cv::Mat> Ks, distortions, Rs, Rs_rvec, Ts;
cv::Mat errors_mat, output_pairs, rvecs0, tvecs0;
double rms = calibrateMultiview(objPoints, image_points_all, image_sizes, visibility,
Rs_rvec, Ts, Ks, distortions, rvecs0, tvecs0, is_fisheye, errors_mat, output_pairs,
false, flagsForIntrinsics);
double rms = calibrateMultiview(objPoints, image_points_all, image_sizes, visibility, models,
Rs_rvec, Ts, Ks, distortions, 0, flagsForIntrinsics);
CV_LOG_INFO(NULL, "RMS: " << rms);
EXPECT_LE(rms, 1.);
@@ -368,40 +502,6 @@ TEST_F(MultiViewTest, CamsToFloor)
struct RegisterCamerasTest: public MultiViewTest
{
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 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)