search

xdspacelab / openvslam

OpenVSLAM: A Versatile Visual SLAM Framework
https://openvslam.readthedocs.io/
2.97k stars 869 forks source link

Camera Calibration Conversion #496

Closed ryleymcc closed 3 years ago

ryleymcc commented 3 years ago

I am trying to convert a .yaml camera calibration file that I made with rtabmap to a format the openvslam will accept.

this is the rtabmap calibration file %YAML:1.0

camera_name: "0001" image_width: 1280 image_height: 720 camera_matrix: rows: 3 cols: 3 data: [ 1.0190450565145343e+03, 0., 6.9602573475594977e+02, 0., 1.0121077244488448e+03, 3.8921696658988185e+02, 0., 0., 1. ] distortion_coefficients: rows: 1 cols: 5 data: [ -1.4369180549737193e-01, 1.2992300278716229e-01, -1.0511626654180308e-03, 4.4375902562854615e-03, 2.3044925457766899e-03 ] distortion_model: plumb_bob rectification_matrix: rows: 3 cols: 3 data: [ 1., 0., 0., 0., 1., 0., 0., 0., 1. ] projection_matrix: rows: 3 cols: 4 data: [ 1.0190450565145343e+03, 0., 6.9602573475594977e+02, 2.5066988713517105e+02, 0., 1.0121077244488448e+03, 3.8921696658988185e+02, -2.1812342002665680e-01, 0., 0., 1., 1. ]

fabisant commented 3 years ago

Hello, I have used this code that is developed in C ++, mainly so that you can know what values correspond to the cameraMatrix and distCoeffs. I have shared the code that is used to read the calibration images, and write the config.

I hope I can serve you in something.

/*

include "openvslam/system.h"

include "openvslam/config.h"

include

include

include

include

include <opencv2/core/core.hpp>

include <opencv2/imgcodecs.hpp>

include <opencv2/videoio.hpp>

include <spdlog/spdlog.h>

include

include <opencv2/opencv.hpp>

include <opencv2/calib3d/calib3d.hpp>

include <opencv2/highgui/highgui.hpp>

include <opencv2/imgproc/imgproc.hpp>

include

int write_config_file(cv::Mat cameraMatrix, cv::Mat distCoeffs, cv::Mat R, cv::Mat T); int CHECKERBOARD[2]{ 6, 9 }; extern "C" JNIEXPORT jstring JNICALL Java_nativeJNI_NativeLib_calibrateCamera(JNIEnv* env, jobject, const jstring cfg_file_name, const jstring path_imgs) {

const char* calCharCfgFileName = env->GetStringUTFChars(cfg_file_name, JNI_FALSE);
std::string s_cfg_file_name{ calCharCfgFileName };
const char* calCharPathCfgFile = env->GetStringUTFChars(path_imgs, JNI_FALSE);
std::string s_path_imgs{ calCharPathCfgFile };

std::vector<std::vector<cv::Point3f> > objpoints;

std::vector<std::vector<cv::Point2f> > imgpoints;

std::vector<cv::Point3f> objp;
for (int i{ 0 }; i < CHECKERBOARD[1]; i++) {
    for (int j{ 0 }; j < CHECKERBOARD[0]; j++)
        objp.push_back(cv::Point3f(j, i, 0));
}

std::vector<cv::String> images;

s_path_imgs = s_path_imgs + "*.jpg";

printf(s_path_imgs.c_str(), "\n");

cv::glob(s_path_imgs, images);

cv::Mat frame, gray;

std::vector<cv::Point2f> corner_pts;
bool success;

for (int i{ 0 }; i < images.size(); i++) {
    frame = cv::imread(images[i]);
    cv::cvtColor(frame, gray, cv::COLOR_BGR2GRAY);

    success = cv::findChessboardCorners(gray, cv::Size(CHECKERBOARD[0], CHECKERBOARD[1]), corner_pts, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FAST_CHECK | CV_CALIB_CB_NORMALIZE_IMAGE);

    if (success) {
        cv::TermCriteria criteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 30, 0.001);

        cv::cornerSubPix(gray, corner_pts, cv::Size(11, 11), cv::Size(-1, -1), criteria);

        cv::drawChessboardCorners(frame, cv::Size(CHECKERBOARD[0], CHECKERBOARD[1]), corner_pts, success);

        objpoints.push_back(objp);
        imgpoints.push_back(corner_pts);
    }
}

cv::Mat cameraMatrix, distCoeffs, R, T;

cv::calibrateCamera(objpoints, imgpoints, cv::Size(gray.rows, gray.cols), cameraMatrix, distCoeffs, R, T);

std::cout << "cameraMatrix : " << cameraMatrix << std::endl;
std::cout << "distCoeffs : " << distCoeffs << std::endl;
std::cout << "Rotation vector : " << R << std::endl;
std::cout << "Translation vector : " << T << std::endl;
int calbrate = write_config_file(cameraMatrix, distCoeffs, R, T);
std::string hello = "Calibrado Correctamente";
return env->NewStringUTF((s_path_imgs).c_str());

}

int write_config_file(cv::Mat cameraMatrix, cv::Mat distCoeffs, cv::Mat R, cv::Mat T) {

double fx = cameraMatrix.at<double>(0, 0);
double fy = cameraMatrix.at<double>(1, 1);
double cx = cameraMatrix.at<double>(0, 2);
double cy = cameraMatrix.at<double>(1, 2);

double k1 = distCoeffs.at<double>(0, 0);
double k2 = distCoeffs.at<double>(0, 1);
double p1 = distCoeffs.at<double>(0, 2);
double p2 = distCoeffs.at<double>(0, 3);
double k3 = distCoeffs.at<double>(0, 4);

std::ofstream cfg_file;
cfg_file.open("config.yaml");
cfg_file << "#==============#\n";
cfg_file << "# Camera Model #\n";
cfg_file << "#==============#\n";
cfg_file << "\n";
cfg_file << "Camera.name: "
         << "\"Xiaomi NOte 9 Pro\"\n";
cfg_file << "Camera.setup: "
         << "\"Monocular\"\n";
cfg_file << "Camera.model: "
         << "\"perspective\"\n";
cfg_file << "\n";
cfg_file << "#Camera Matrix\n";
cfg_file << "Camera.fx: " << fx << "\n";
cfg_file << "Camera.fy: " << fy << "\n";
cfg_file << "#cx cy : center of image\n";
cfg_file << "Camera.cx: " << cx << "\n";
cfg_file << "Camera.cy: " << cy << "\n";
cfg_file << "\n";
cfg_file << "Camera.k1: " << k1 << "\n";
cfg_file << "Camera.k2: " << k2 << "\n";
cfg_file << "Camera.p1: " << p1 << "\n";
cfg_file << "Camera.p2: " << p2 << "\n";
cfg_file << "Camera.k3: " << k3 << "\n";
cfg_file << "\n";
cfg_file << "Camera.fps: " << 30.0 << "\n";
cfg_file << "Camera.cols: " << 1024 << "\n";
cfg_file << "Camera.rows: " << 768 << "\n";
cfg_file << "\n";
cfg_file << "Camera.color_order: "
         << "\"RGB\"\n";
cfg_file << "\n";
cfg_file << "#================#\n";
cfg_file << "# ORB Parameters #\n";
cfg_file << "#================#\n";
cfg_file << "\n";
cfg_file << "#=====================#\n";
cfg_file << "# Tracking Parameters #\n";
cfg_file << "#=====================#\n";

return 0;

}