前言
需要从配置文件yaml文件中读取相机参数才能用于后续计算。
1.函数声明
读取相机1的参数：
cpp
void Settings::readCamera1(cv::FileStorage &fSettings)如果是双目相机则还要读取相机2的参数：
cpp
void Settings::readCamera2(cv::FileStorage &fSettings)2.函数定义
相机1
1.读取相机模型
3的模型加入了针孔相机模型以及广角相机模型，可以利用模板函数读取参数，来确定相机的类型。
cpp
bool found;
// Read camera model
string cameraModel = readParameter<string>(fSettings, "Camera.type", found);2.如果是针孔相机模型
就进行如下操作：
读取相机参数构造相机模型，针对不同畸变参数的个数采用不同的方式进行读取，如果是单目相机和RGBD相机的话就需要进行去畸变操作。
cpp
vector<float> vCalibration;
if (cameraModel == "PinHole")
{
cameraType_ = PinHole;//设置相机类型
// Read intrinsic parameters读取内参
float fx = readParameter<float>(fSettings, "Camera1.fx", found);
float fy = readParameter<float>(fSettings, "Camera1.fy", found);
float cx = readParameter<float>(fSettings, "Camera1.cx", found);
float cy = readParameter<float>(fSettings, "Camera1.cy", found);
vCalibration = {fx, fy, cx, cy};//储存在校准的变量中
calibration1_ = new Pinhole(vCalibration);
originalCalib1_ = new Pinhole(vCalibration);
// Check if it is a distorted PinHole
// 检查这个针孔相机的配置文件是否有畸变参数
readParameter<float>(fSettings, "Camera1.k1", found, false);
if (found)
{
readParameter<float>(fSettings, "Camera1.k3", found, false);
if (found)
{
vPinHoleDistorsion1_.resize(5);//储存相机畸变参数的向量扩容。
vPinHoleDistorsion1_[4] = readParameter<float>(fSettings, "Camera1.k3", found);
}
else
{
vPinHoleDistorsion1_.resize(4);
}
vPinHoleDistorsion1_[0] = readParameter<float>(fSettings, "Camera1.k1", found);
vPinHoleDistorsion1_[1] = readParameter<float>(fSettings, "Camera1.k2", found);
vPinHoleDistorsion1_[2] = readParameter<float>(fSettings, "Camera1.p1", found);
vPinHoleDistorsion1_[3] = readParameter<float>(fSettings, "Camera1.p2", found);
}
// Check if we need to correct distortion from the images
// 如果是单目相机或者是RGBD相机且有畸变参数则需要去畸变操作。
if (
(sensor_ == System::MONOCULAR ||
sensor_ == System::IMU_MONOCULAR ||
sensor_ == System::RGBD ||
sensor_ == System::IMU_RGBD) &&
vPinHoleDistorsion1_.size() != 0)
{
bNeedToUndistort_ = true;
}
}3.如果是矫正后的图像
此时认为是没有畸变的，就正常读取参数构造相机模型即可。
cpp
//如果是校正后的图像则认为没畸变。
else if (cameraModel == "Rectified")
{
cameraType_ = Rectified;
// Read intrinsic parameters
float fx = readParameter<float>(fSettings, "Camera1.fx", found);
float fy = readParameter<float>(fSettings, "Camera1.fy", found);
float cx = readParameter<float>(fSettings, "Camera1.cx", found);
float cy = readParameter<float>(fSettings, "Camera1.cy", found);
vCalibration = {fx, fy, cx, cy};
calibration1_ = new Pinhole(vCalibration);
originalCalib1_ = new Pinhole(vCalibration);
// Rectified images are assumed to be ideal PinHole images (no distortion)
}4.如果是广角相机模型
需要在读取相机参数构建相机模型的基础上若是双目模式则要加上重叠区域参数读取以及构建重叠区域。
cpp
else if (cameraModel == "KannalaBrandt8")
{
cameraType_ = KannalaBrandt;
// Read intrinsic parameters
// 用模板函数读取yaml文件中的参数
float fx = readParameter<float>(fSettings, "Camera1.fx", found);
float fy = readParameter<float>(fSettings, "Camera1.fy", found);
float cx = readParameter<float>(fSettings, "Camera1.cx", found);
float cy = readParameter<float>(fSettings, "Camera1.cy", found);
float k0 = readParameter<float>(fSettings, "Camera1.k1", found);
float k1 = readParameter<float>(fSettings, "Camera1.k2", found);
float k2 = readParameter<float>(fSettings, "Camera1.k3", found);
float k3 = readParameter<float>(fSettings, "Camera1.k4", found);
vCalibration = {fx, fy, cx, cy, k0, k1, k2, k3};
calibration1_ = new KannalaBrandt8(vCalibration);
originalCalib1_ = new KannalaBrandt8(vCalibration);
//双目相机或IMU双目相机时，读取和设置摄像头的图像重叠区域参数。
if (sensor_ == System::STEREO || sensor_ == System::IMU_STEREO)
{
int colBegin = readParameter<int>(fSettings, "Camera1.overlappingBegin", found);
int colEnd = readParameter<int>(fSettings, "Camera1.overlappingEnd", found);
vector<int> vOverlapping = {colBegin, colEnd};//摄像头重叠区域。
static_cast<KannalaBrandt8 *>(calibration1_)->mvLappingArea = vOverlapping;
}
}5.如果没有读取到相机模型
直接输出错误信息。
cpp
else
{
cerr << "Error: " << cameraModel << " not known" << endl;
exit(-1);
}完整的代码
cpp
void Settings::readCamera1(cv::FileStorage &fSettings)
{
bool found;
// Read camera model
string cameraModel = readParameter<string>(fSettings, "Camera.type", found);
vector<float> vCalibration;
if (cameraModel == "PinHole")
{
cameraType_ = PinHole;//设置相机类型
// Read intrinsic parameters读取内参
float fx = readParameter<float>(fSettings, "Camera1.fx", found);
float fy = readParameter<float>(fSettings, "Camera1.fy", found);
float cx = readParameter<float>(fSettings, "Camera1.cx", found);
float cy = readParameter<float>(fSettings, "Camera1.cy", found);
vCalibration = {fx, fy, cx, cy};//储存在校准的变量中
calibration1_ = new Pinhole(vCalibration);
originalCalib1_ = new Pinhole(vCalibration);
// Check if it is a distorted PinHole
// 检查这个针孔相机的配置文件是否有畸变参数
readParameter<float>(fSettings, "Camera1.k1", found, false);
if (found)
{
readParameter<float>(fSettings, "Camera1.k3", found, false);
if (found)
{
vPinHoleDistorsion1_.resize(5);//储存相机畸变参数的向量扩容。
vPinHoleDistorsion1_[4] = readParameter<float>(fSettings, "Camera1.k3", found);
}
else
{
vPinHoleDistorsion1_.resize(4);
}
vPinHoleDistorsion1_[0] = readParameter<float>(fSettings, "Camera1.k1", found);
vPinHoleDistorsion1_[1] = readParameter<float>(fSettings, "Camera1.k2", found);
vPinHoleDistorsion1_[2] = readParameter<float>(fSettings, "Camera1.p1", found);
vPinHoleDistorsion1_[3] = readParameter<float>(fSettings, "Camera1.p2", found);
}
// Check if we need to correct distortion from the images
// 如果是单目相机或者是RGBD相机且有畸变参数则需要去畸变操作。
if (
(sensor_ == System::MONOCULAR ||
sensor_ == System::IMU_MONOCULAR ||
sensor_ == System::RGBD ||
sensor_ == System::IMU_RGBD) &&
vPinHoleDistorsion1_.size() != 0)
{
bNeedToUndistort_ = true;
}
}
//如果是校正后的图像则认为没畸变。
else if (cameraModel == "Rectified")
{
cameraType_ = Rectified;
// Read intrinsic parameters
float fx = readParameter<float>(fSettings, "Camera1.fx", found);
float fy = readParameter<float>(fSettings, "Camera1.fy", found);
float cx = readParameter<float>(fSettings, "Camera1.cx", found);
float cy = readParameter<float>(fSettings, "Camera1.cy", found);
vCalibration = {fx, fy, cx, cy};
calibration1_ = new Pinhole(vCalibration);
originalCalib1_ = new Pinhole(vCalibration);
// Rectified images are assumed to be ideal PinHole images (no distortion)
}
else if (cameraModel == "KannalaBrandt8")
{
cameraType_ = KannalaBrandt;
// Read intrinsic parameters
// 用模板函数读取yaml文件中的参数
float fx = readParameter<float>(fSettings, "Camera1.fx", found);
float fy = readParameter<float>(fSettings, "Camera1.fy", found);
float cx = readParameter<float>(fSettings, "Camera1.cx", found);
float cy = readParameter<float>(fSettings, "Camera1.cy", found);
float k0 = readParameter<float>(fSettings, "Camera1.k1", found);
float k1 = readParameter<float>(fSettings, "Camera1.k2", found);
float k2 = readParameter<float>(fSettings, "Camera1.k3", found);
float k3 = readParameter<float>(fSettings, "Camera1.k4", found);
vCalibration = {fx, fy, cx, cy, k0, k1, k2, k3};
calibration1_ = new KannalaBrandt8(vCalibration);
originalCalib1_ = new KannalaBrandt8(vCalibration);
//双目相机或IMU双目相机时，读取和设置摄像头的图像重叠区域参数。
if (sensor_ == System::STEREO || sensor_ == System::IMU_STEREO)
{
int colBegin = readParameter<int>(fSettings, "Camera1.overlappingBegin", found);
int colEnd = readParameter<int>(fSettings, "Camera1.overlappingEnd", found);
vector<int> vOverlapping = {colBegin, colEnd};//摄像头重叠区域。
static_cast<KannalaBrandt8 *>(calibration1_)->mvLappingArea = vOverlapping;
}
}
else
{
cerr << "Error: " << cameraModel << " not known" << endl;
exit(-1);
}
}相机2
这时候已经默认是双目模式了，除了要进行相机1的操作，还要额外进行一些操作。
读取基线，计算基线焦距，读取深度。
cpp
// Load stereo extrinsic calibration
if (cameraType_ == Rectified)
{
b_ = readParameter<float>(fSettings, "Stereo.b", found);
bf_ = b_ * calibration1_->getParameter(0);
}
else
{
cv::Mat cvTlr = readParameter<cv::Mat>(fSettings, "Stereo.T_c1_c2", found);
Tlr_ = Converter::toSophus(cvTlr);
// TODO: also search for Trl and invert if necessary
b_ = Tlr_.translation().norm();
bf_ = b_ * calibration1_->getParameter(0);
}
thDepth_ = readParameter<float>(fSettings, "Stereo.ThDepth", found);完整的代码
cpp
void Settings::readCamera2(cv::FileStorage &fSettings)
{
bool found;
vector<float> vCalibration;
if (cameraType_ == PinHole)
{
bNeedToRectify_ = true;
// Read intrinsic parameters
float fx = readParameter<float>(fSettings, "Camera2.fx", found);
float fy = readParameter<float>(fSettings, "Camera2.fy", found);
float cx = readParameter<float>(fSettings, "Camera2.cx", found);
float cy = readParameter<float>(fSettings, "Camera2.cy", found);
vCalibration = {fx, fy, cx, cy};
calibration2_ = new Pinhole(vCalibration);
originalCalib2_ = new Pinhole(vCalibration);
// Check if it is a distorted PinHole
readParameter<float>(fSettings, "Camera2.k1", found, false);
if (found)
{
readParameter<float>(fSettings, "Camera2.k3", found, false);
if (found)
{
vPinHoleDistorsion2_.resize(5);
vPinHoleDistorsion2_[4] = readParameter<float>(fSettings, "Camera2.k3", found);
}
else
{
vPinHoleDistorsion2_.resize(4);
}
vPinHoleDistorsion2_[0] = readParameter<float>(fSettings, "Camera2.k1", found);
vPinHoleDistorsion2_[1] = readParameter<float>(fSettings, "Camera2.k2", found);
vPinHoleDistorsion2_[2] = readParameter<float>(fSettings, "Camera2.p1", found);
vPinHoleDistorsion2_[3] = readParameter<float>(fSettings, "Camera2.p2", found);
}
}
else if (cameraType_ == KannalaBrandt)
{
// Read intrinsic parameters
float fx = readParameter<float>(fSettings, "Camera2.fx", found);
float fy = readParameter<float>(fSettings, "Camera2.fy", found);
float cx = readParameter<float>(fSettings, "Camera2.cx", found);
float cy = readParameter<float>(fSettings, "Camera2.cy", found);
float k0 = readParameter<float>(fSettings, "Camera1.k1", found);
float k1 = readParameter<float>(fSettings, "Camera1.k2", found);
float k2 = readParameter<float>(fSettings, "Camera1.k3", found);
float k3 = readParameter<float>(fSettings, "Camera1.k4", found);
vCalibration = {fx, fy, cx, cy, k0, k1, k2, k3};
calibration2_ = new KannalaBrandt8(vCalibration);
originalCalib2_ = new KannalaBrandt8(vCalibration);
int colBegin = readParameter<int>(fSettings, "Camera2.overlappingBegin", found);
int colEnd = readParameter<int>(fSettings, "Camera2.overlappingEnd", found);
vector<int> vOverlapping = {colBegin, colEnd};
static_cast<KannalaBrandt8 *>(calibration2_)->mvLappingArea = vOverlapping;
}
// Load stereo extrinsic calibration
if (cameraType_ == Rectified)
{
b_ = readParameter<float>(fSettings, "Stereo.b", found);
bf_ = b_ * calibration1_->getParameter(0);
}
else
{
cv::Mat cvTlr = readParameter<cv::Mat>(fSettings, "Stereo.T_c1_c2", found);
Tlr_ = Converter::toSophus(cvTlr);
// TODO: also search for Trl and invert if necessary
b_ = Tlr_.translation().norm();
bf_ = b_ * calibration1_->getParameter(0);
}
thDepth_ = readParameter<float>(fSettings, "Stereo.ThDepth", found);
}结束语
以上就是我学习到的内容，如果对您有帮助请多多支持我，如果哪里有问题欢迎大家在评论区积极讨论，我看到会及时回复。