I have two cameras with different lenses and resolutions.
I have images, with chessboards visible in both.
I am trying to calculate very accurate extrinsic information between the two cameras, and then have the method repeatable when i change the lens on one camera, and the extrinsic values stay the same across multiple lens calibrations.
I have tried lens calibration and charuco boards, but although this gives me acceptable translation values, the rotation values vary up to a few degrees on each run, depending on the calibration.
I have tried chessboard corners and solvePnP also, but the results vary wildly.
I have tried cv::findEssentialMat on the chessboard corners, then cv::recoverPose, but the results are even worse (I have read that this method does not like planar points).
So, are there any other methods that I can use to find an accurate rotation extrinsic value between two cameras?
Can I use FindHomography somehow to get the relative pose between images?
The code I am using for the solvepnp approach is:
void main()
{
cv::Mat K1, D1, K2, D2;
bool readOk = readCameraParameters("data/zed1920.xml", K1, D1);
if (!readOk) {
cerr << "Invalid camera file" << endl;
}
readOk = readCameraParameters("data/sdi.xml", K2, D2);
if (!readOk) {
cerr << "Invalid camera file" << endl;
}
//set up board
int grid_size_x = 7;
int grid_size_y = 5;
double squareSize = 4.5;
std::vector<cv::Point3d> object_points_;
for (int y = 0; y < grid_size_y; y++)
{
for (int x = 0; x < grid_size_x; x++)
{
object_points_.push_back(cv::Point3d(x * squareSize, y * squareSize, 0));
}
}
cv::Mat imageGray, imageGray2;
//pair 1
cv::Mat frame1_1 = cv::imread("frames/frame1_1.jpg");
cv::Mat frame1_2 = cv::imread("frames/frame2_1.jpg");
cv::cvtColor(frame1_1, imageGray, cv::COLOR_BGR2GRAY);
cv::cvtColor(frame1_2, imageGray2, cv::COLOR_BGR2GRAY);
std::vector<cv::Point2f> corners1, corners2;
//find board in image
bool found_chessboard = cv::findChessboardCorners(frame1, cv::findChessboardCorners(imageGray, cv::Size(grid_size_x, grid_size_y), corners1, cv::CALIB_CB_ADAPTIVE_THRESH);
cv::CALIB_CB_ADAPTIVE_THRESH);// cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_NORMALIZE_IMAGE + cv::CALIB_CB_FAST_CHECK);
bool found_chessboard2 = cv::findChessboardCorners(frame2, cv::findChessboardCorners(imageGray2, cv::Size(grid_size_x, grid_size_y), corners2, cv::CALIB_CB_ADAPTIVE_THRESH);
std::cout << " got frame2: " << found_chessboard2 << " got frame1: " << found_chessboard << std::endl;
cv::CALIB_CB_ADAPTIVE_THRESH);// cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_NORMALIZE_IMAGE + cv::CALIB_CB_FAST_CHECK);
if (found_chessboard && found_chessboard2)
{
std::cout << " got boards " << std::endl;
cv::cornerSubPix(imageGray, corners1, cv::Size(3, 3), cv::Size(-1, -1), cv::TermCriteria(cv::TermCriteria::MAX_ITER + cv::TermCriteria::EPS, 50, 0.01));
cv::cornerSubPix(imageGray2, corners2, cv::Size(3, 3), cv::Size(-1, -1), cv::TermCriteria(cv::TermCriteria::MAX_ITER + cv::TermCriteria::EPS, 50, 0.01));
if (corners1.size() != 0 && corners2.size() != 0)
{
cv::Mat rvec1, tvec1;
cv::solvePnP(object_points_, corners1, K1, D1, rvec1, tvec1, false, cv::SOLVEPNP_ITERATIVE);
cv::Mat rvec2, tvec2;
cv::solvePnP(object_points_, corners2, K2, D2, rvec2, tvec2, false, cv::SOLVEPNP_ITERATIVE);
cv::Mat R1, R2;
cv::Rodrigues(rvec1, R1);
cv::Rodrigues(rvec2, R2);
cv::Mat R_1to2 = R2 * R1.t();
cv::Mat tvec_1to2 = R2 * (-R1.t()*tvec1) + tvec2;
std::cout << tvec_1to2 << std::endl;
Eigen::Matrix<double, 3, 3> Reigen2;
cv2eigen(R_1to2, Reigen2);
Eigen::Quaterniond quaternion2(Reigen2);
auto euler2 = quaternion2.toRotationMatrix().eulerAngles(0, 1, 2);
euler2 = euler2 * 180 / M_PI;
std::cout << " rotation diff pair 1 is: " << euler2.x() << " " << euler2.y() << " " << euler2.z() << std::endl;
}
}
//pair 2
frame1_1 = cv::imread("frames/frame1_2.jpg");
frame1_2 = cv::imread("frames/frame2_2.jpg");
cv::cvtColor(frame1_1, imageGray, cv::COLOR_BGR2GRAY);
cv::cvtColor(frame1_2, imageGray2, cv::COLOR_BGR2GRAY);
corners1.clear();
corners2.clear();
//find board in image
found_chessboard = cv::findChessboardCorners(imageGray, cv::Size(grid_size_x, grid_size_y), corners1, cv::CALIB_CB_ADAPTIVE_THRESH);// cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_NORMALIZE_IMAGE + cv::CALIB_CB_FAST_CHECK);
found_chessboard2 = cv::findChessboardCorners(imageGray2, cv::Size(grid_size_x, grid_size_y), corners2, cv::CALIB_CB_ADAPTIVE_THRESH);// cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_NORMALIZE_IMAGE + cv::CALIB_CB_FAST_CHECK);
if (found_chessboard && found_chessboard2)
{
cv::cornerSubPix(imageGray, corners1, cv::Size(3, 3), cv::Size(-1, -1), cv::TermCriteria(cv::TermCriteria::MAX_ITER + cv::TermCriteria::EPS, 50, 0.01));
cv::cornerSubPix(imageGray2, corners2, cv::Size(3, 3), cv::Size(-1, -1), cv::TermCriteria(cv::TermCriteria::MAX_ITER + cv::TermCriteria::EPS, 50, 0.01));
if (corners1.size() != 0 && corners2.size() != 0)
{
cv::Mat rvec1, tvec1;
cv::solvePnP(object_points_, corners1, K1, D1, rvec1, tvec1, false, cv::SOLVEPNP_ITERATIVE);
cv::Mat rvec2, tvec2;
cv::solvePnP(object_points_, corners2, K2, D2, rvec2, tvec2, false, cv::SOLVEPNP_ITERATIVE);
cv::Mat R1, R2;
cv::Rodrigues(rvec1, R1);
cv::Rodrigues(rvec2, R2);
cv::Mat R_1to2 = R2 * R1.t();
cv::Mat tvec_1to2 = R2 * (-R1.t()*tvec1) + tvec2;
std::cout << tvec_1to2 << std::endl;
Eigen::Matrix<double, 3, 3> Reigen2;
cv2eigen(R_1to2, Reigen2);
Eigen::Quaterniond quaternion2(Reigen2);
auto euler2 = quaternion2.toRotationMatrix().eulerAngles(0, 1, 2);
euler2 = euler2 * 180 / M_PI;
std::cout << " rotation diff pair 2 is: " << euler2.x() << " " << euler2.y() << " " << euler2.z() << std::endl;
}
}
//pair 3
frame1_1 = cv::imread("frames/frame1_3.jpg");
frame1_2 = cv::imread("frames/frame2_3.jpg");
cv::cvtColor(frame1_1, imageGray, cv::COLOR_BGR2GRAY);
cv::cvtColor(frame1_2, imageGray2, cv::COLOR_BGR2GRAY);
corners1.clear();
corners2.clear();
//find board in image
found_chessboard = cv::findChessboardCorners(imageGray, cv::Size(grid_size_x, grid_size_y), corners1, cv::CALIB_CB_ADAPTIVE_THRESH);// cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_NORMALIZE_IMAGE + cv::CALIB_CB_FAST_CHECK);
found_chessboard2 = cv::findChessboardCorners(imageGray2, cv::Size(grid_size_x, grid_size_y), corners2, cv::CALIB_CB_ADAPTIVE_THRESH);// cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_NORMALIZE_IMAGE + cv::CALIB_CB_FAST_CHECK);
if (found_chessboard && found_chessboard2)
{
cv::cornerSubPix(imageGray, corners1, cv::Size(3, 3), cv::Size(-1, -1), cv::TermCriteria(cv::TermCriteria::MAX_ITER + cv::TermCriteria::EPS, 50, 0.01));
cv::cornerSubPix(imageGray2, corners2, cv::Size(3, 3), cv::Size(-1, -1), cv::TermCriteria(cv::TermCriteria::MAX_ITER + cv::TermCriteria::EPS, 50, 0.01));
if (corners1.size() != 0 && corners2.size() != 0)
{
cv::Mat rvec1, tvec1;
cv::solvePnP(object_points_, corners1, K1, D1, rvec1, tvec1, false, cv::SOLVEPNP_ITERATIVE);
cv::Mat rvec2, tvec2;
cv::solvePnP(object_points_, corners2, K2, D2, rvec2, tvec2, false, cv::SOLVEPNP_ITERATIVE);
cv::Mat R1, R2;
cv::Rodrigues(rvec1, R1);
cv::Rodrigues(rvec2, R2);
cv::Mat R_1to2 = R2 * R1.t();
cv::Mat tvec_1to2 = R2 * (-R1.t()*tvec1) + tvec2;
std::cout << tvec_1to2 << std::endl;
Eigen::Matrix<double, 3, 3> Reigen2;
cv2eigen(R_1to2, Reigen2);
Eigen::Quaterniond quaternion2(Reigen2);
auto euler2 = quaternion2.toRotationMatrix().eulerAngles(0, 1, 2);
euler2 = euler2 * 180 / M_PI;
std::cout << " rotation diff pair 3 is: " << euler2.x() << " " << euler2.y() << " " << euler2.z() << std::endl;
}
}
std::cin.get();
}
Thanks!
