上面是一台Track设备,现精度比较高的主要是Redspy,Mosys,一般影视用这二种,其底层技术参考SMAL单目+惯性传感器(IMU),因为需要稳定精准的结果,实现上会贴红外反光片,使用红外相机得到这些贴片对应的稳定特征点用于建图(红外相机受外部亮度影响较小,在红外相机里,红外反光贴片相对周边会非常明亮,就像星空里发光的星星一样),也因为单目的原因,传感器本身的尺度与现实其实不对应,需要额外确保传感器本身的尺度与现实对应.
Track跟踪,主要是如下几步,求相机内参,相机与Track相对变换,Track空间与LED空间的相对变换,现在的标定一般来说,都是三步一起解决的.
标定内参,主要还是类似张正友标定法,得到在不同相机姿态态,得到一系列角点的2D-3D关系,其角点的选取可以来自棋盘格,这种方式限制大,需要拍摄全,对相机方向与位置限定了,影响后续手眼标定的精度,所以现在更多选择是的Aruco码,Aruco角点是有索引的,这样就方便找到明确的3D位置,相机只需要拍摄一部分aruco码就可以了,不会对相机距离与方向有限制,不过Aruco角点精度可能比不上圆形块,开了亚像素也不太行,于是像hecoos这样的,会利用视频流打圆形块,根据一系列帧出现/没出现计算出唯一索引,也能确定唯一位置,但是这种方式标定就会比较费时.
得到一系列3D-2D的角点后,代入cv::calibrateCamera求出就可以了,也得到每帧数据相机的变换,一般在记录图片时,也会记录Track的变换,这样就得到一组相机与Track的变换,如果根据这组数据,得到相机与Track相对变换了,这其实就是机器人常见的手眼标定的问题.
其手眼标定(本文限定眼在手上)的算法利用二个坐标系的钢体变化,构建一个AX=XB的问题去求解,先求得相记与Track的变换矩阵,然后得到Track坐标系与LED坐标系的变换,把结果代入记录数组,计算重投影的误差来判断结果的好坏.
实际过程来说,一般是取九张不同姿态的图,然后先算内参,再用手眼标定算相机与Track相对变换,但是直接九张图效果一般不太好,会加上RANSAC算法,选择其中3-5张结合结果,使用SVD得到最优Track空间与LED空间的相对变换,最后根据这二个结果代入之前记录计算重投影拿到最优结果.
这里有个问题,前面提到因为单目的原因,传感器本身的尺度与现实其实不对应,意思Track给出的数据是1m,但是在现实中可能对应10cm,10m,3m这些位移,OpneCV本身手眼标定并不能计算这种与现实的尺度不匹配的数据,现场一般会利用Redspy/Mosys硬件本身的功能纠正,但是使用比较麻烦,比如已经绑在摄像机上需要重新取下,人工取尺输入现实数据移动,精度就和人扯上关系了,和人扯上关系,就容易出问题,能不能改了?其实是可以,还记的当时那段时间刚好新冠被封锁在租的小区里,就仔细看了下opencv里的手眼标定的Tsai算法.
改进的主要就是求得位移的过程,先来看下opencv原手眼标定里求位移相关逻辑.
// 解决追踪器与摄像机使用不同尺寸问题.
Mat A(3 * K, 3, CV_64FC1);
// Will store: Pcij - Pgij
Mat B(3 * K, 1, CV_64FC1);
idx = 0;
for (size_t i = 0; i < Hg.size(); i++)
{
for (size_t j = i+1; j < Hg.size(); j++, idx++)
{
//Defines coordinate transformation from Gi to Gj
//Hgi is from Gi (gripper) to RW (robot base)
//Hgj is from Gj (gripper) to RW (robot base)
Mat Hgij = vec_Hgij[static_cast<size_t>(idx)];
//Defines coordinate transformation from Ci to Cj
//Hci is from CW (calibration target) to Ci (camera)
//Hcj is from CW (calibration target) to Cj (camera)
Mat Hcij = vec_Hcij[static_cast<size_t>(idx)];
//Left-hand side: (Rgij - I)
Mat diff = Hgij(Rect(0,0,3,3)) - Mat::eye(3,3,CV_64FC1);
diff.copyTo(A(Rect(0, idx*3, 3, 3)));
//Right-hand side: Rcg*Tcij - Tgij
diff = Rcg*Hcij(Rect(3, 0, 1, 3)) - Hgij(Rect(3, 0, 1, 3));
diff.copyTo(B(Rect(0, idx*3, 1, 3)));
}
}
Mat Tcg;
//Translation from camera to gripper is obtained from the set of equations:
// (Rgij - I) * Tcg = Rcg*Tcij - Tgij (eq 15)
solve(A, B, Tcg, DECOMP_SVD);
利用等式
其中Rgij表示标定记录中二次记录之间追踪器空间下追踪器之间的旋转,根据记录是已知量.I表示3x3的单位矩阵,是固定量.Tcg表示追踪器空间下,摄像机相对追踪器的位移偏移,也就是我们要求解的值.Rcg表示追踪器空间下,摄像机相对追踪器的旋转偏移.Tcij 表示标定记录中二次记录之间摄像机空间下标定板之间的旋转,根据记录是已知量.Tgij 表示标定记录中二次记录之间追踪器空间下追踪器之间的位移,根据记录是已知量.假定Tracl与现实中的缩放参数是Scale,那么可以重组为如下等式.
原来矩阵算式如下.
现在变换后,矩阵算式如下.
其中新变换里的C表示- Rcg*Tcij,这样就把缩放参数带入要求解的[T0,T1,T2,S3]中的S3里面,新的代码如下.
// 解决追踪器与摄像机使用不同尺寸问题.
Mat TA(3 * K, 4, CV_64FC1);
// Will store: Pcij - Pgij
Mat TB(3 * K, 1, CV_64FC1);
idx = 0;
for (size_t i = 0; i < Hg.size(); i++) {
for (size_t j = i + 1; j < Hg.size(); j++, idx++) {
// Defines coordinate transformation from Gi to Gj
// Hgi is from Gi (gripper) to RW (robot base)
// Hgj is from Gj (gripper) to RW (robot base)
Mat Hgij = vec_Hgij[static_cast<size_t>(idx)];
// Defines coordinate transformation from Ci to Cj
// Hci is from CW (calibration target) to Ci (camera)
// Hcj is from CW (calibration target) to Cj (camera)
Mat Hcij = vec_Hcij[static_cast<size_t>(idx)];
// Left-hand side:3x3_(Rgij - I) 3x1_(Rcg*Tcij)
Mat ldiff1 = Hgij(Rect(0, 0, 3, 3)) - Mat::eye(3, 3, CV_64FC1);
ldiff1.copyTo(TA(Rect(0, idx * 3, 3, 3)));
Mat ldiff2 = -(Rcg * Hcij(Rect(3, 0, 1, 3)));
ldiff2.copyTo(TA(Rect(3, idx * 3, 1, 3)));
// Right-hand side: -Tgij
Mat diff = -Hgij(Rect(3, 0, 1, 3));
diff.copyTo(TB(Rect(0, idx * 3, 1, 3)));
}
}
Mat Tcg;
solve(TA, TB, Tcg, DECOMP_SVD);
其求得的4*1前面三个表示Tcg(摄像机相对追踪器的位移),最后数据表示现实世界相对追踪器坐标系下缩放是scale.
这样后,应该得到类似一个如下结果.
struct ECameraTrack {
// 追踪器坐标系如何变换成标定板坐标系
Eigen::Matrix4d base2target = {};
// 保存摄像机相对track的姿态
Eigen::Matrix4d camera2track = {};
// 追踪器坐标系相对标定板的位移缩放
double scale = 1.0;
};
上面处理后,一般来说,结果平均在8个像素误差左右,用于VP肯定是够了,VP一般是扩展FOV的,但是如果想用于XR拍摄,这个精度就可能会不够,还能不能计算更精准,先看下优化后的效果.
可以看到平均4个像素误差优化到1个左右,实际实现情况大的LED幕墙,原结果平均8个像素(还不稳定)可以优化到稳定的平均3个像素左右误差,精度够XR用了.
那段时间正好在看同事推荐的视觉SLAM十四讲,学习图优化相关框架g2o,可以直接使用测量值比较误差求解或是优化参数,简单总结图优化的步骤,一是确定需要优化的变量,二是确定变量到观测量的计算过程,通过这个过程得到计算结果与观测量比较得到误差,第三步是构建变量与计算过程与观测量误差的图.
前面二步对应图优化框架g2o里的二个概念,分别是顶点和边,其顶点对应的就是需要求解的变量,而边就是由顶点构建计算过程,得到结果并与测量值确定误差,第三步优化过程就是构建顶点,边与测量值的图.
相对常规解法,图优化是直接根据测量值优化结果,只需要构建一个参数能正确和测量值比较误差的模型就行,相比手眼标定算法,需要理解钢体关系多帧间几个变化的相等性到构建AX=XB的处理来说,直接由结果去代入重投影比较误差,然后构建模型会非常容易理解,在3D视觉中,有非常多的这种需求,知道变量,知道变量导致的结果误差,但是不知道怎么求解,其图优化就可以求解这种情况,所以在3D视觉中,其图优化使用非常普遍.
如下有些代码,其变换关系不理解会比较乱,为了方便理解,简单介绍一下结构与命名,比如target说的是屏幕坐标系,camera表示摄像机在屏幕坐标系下的运动,base是追踪器坐标系,track表示追踪器在追踪器坐标系下的运动,这样如base2target表示追踪器坐标系转到屏幕坐标系下的变换,而camera2track表示camera相对track在追踪器坐标系下相对变换,camera2target表示摄像机在屏幕坐标系下的变换,target2camera表示camera2target的逆变换,可以理解成屏幕在摄像机下的变换,track2base表示追踪器在追踪器坐标系下的变换.
如前面介绍图优化的过程,先需要确定求解的值,在这就是camera2track(也就是原手眼标定求解的值),base2target用于把追踪器坐标系转化到屏幕坐标系下,需要注意的是,这里的track给的位移与现实中的位移没有对应,有个缩放关系,在这我们也需要求解这个Scale值,这样就有三个值需要求解,然后假定标定过程使用的Track变换相对真实值有细微误差,针对每个Track记录变换也当做一个需要优化的值,这个处理有兴趣的可以详细参考基于重投影误差最小化的手眼标定,能正确的处理Track异常记录,经实际测试,对于结果的正确性会有很大提高.
需要的结果是二个变换+一个缩放,和中间Track记录变换的优化,变换对应的变量直接使用g2o内置的VertexSE3,截取重要代码如下,有兴趣可以自己去看g2o里的源码,而缩放只需要简单封装一个double就行,简单来说,顶点最主要的是实现方法oplusImpl,告诉顶点如果更新参数.
// 内置钢体变换顶点
class G2O_TYPES_SLAM3D_API VertexSE3 : public BaseVertex<6, Isometry3> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
virtual void setToOriginImpl() { _estimate = Isometry3::Identity(); }
virtual void oplusImpl(const double* update) {
Eigen::Map<const Vector6> v(update);
Isometry3 increment = internal::fromVectorMQT(v);
_estimate = _estimate * increment;
}
}
// 封装缩放的顶点
class ScaleVertex : public g2o::BaseVertex<1, double> {
public:
ScaleVertex() {}
virtual void setToOriginImpl() { _estimate = 1.0; }
virtual void oplusImpl(const double* update) {
_estimate += update[0];
}
virtual bool read(std::istream& is) {
is >> _estimate;
return true;
}
virtual bool write(std::ostream& os) const {
os << _estimate;
return true;
}
};
确定顶点后,然后就是确定边,就如前面所说,边是确定如何优化顶点与测量值的误差变小的,这里使用重投影确定角点UV与测量UV的误差,角点的三维位置与测量UV分别由建立屏幕坐标系时确定以及OPENCV查找角点得到,边的computeError记录了如何把角点的三维位置转化到摄像机下的位置,并得到UV,然后与测量UV比较的过程.
class ProjectionHandEdge
: public g2o::BaseUnaryEdge<2, Eigen::Vector2d, g2o::VertexSE3> {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
public:
ProjectionHandEdge()
: g2o::BaseUnaryEdge<2, Eigen::Vector2d, g2o::VertexSE3>() {
resizeParameters(1);
installParameter(lensModelPar, 0);
}
virtual bool read(std::istream& is) { return false; }
virtual bool write(std::ostream& os) const { return false; };
virtual void computeError() override;
g2o::Vector3 point = {};
protected:
LensModelParameter* lensModelPar = nullptr;
};
void ProjectionHandEdge::computeError() {
const g2o::VertexSE3* target2cameraVec =
dynamic_cast<const g2o::VertexSE3*>(_vertices[0]);
Eigen::Isometry3d target2camera = target2cameraVec->estimate();
// 点在相机下位置
g2o::Vector3 cameraPos = target2camera * point;
// UV
g2o::Vector2 cuv = lensModelPar->map(cameraPos);
// 误差由观测值减预测值
_error = measurement() - cuv;
}
// 使用小孔相机模型确定3维顶点的投影平面
g2o::Vector2 lensMap(const LensModel& lensModel, const g2o::Vector3& pos,
double scale) {
double x = pos[0] / pos[2];
double y = pos[1] / pos[2];
double fx = lensModel.focalLength.x;
double fy = lensModel.focalLength.y;
double cx = lensModel.focalCenter.x;
double cy = lensModel.focalCenter.y;
double k1 = lensModel.K1;
double k2 = lensModel.K2;
double k3 = lensModel.K3;
double p1 = lensModel.P1;
double p2 = lensModel.P2;
// 径向畸变
double r2 = x * x + y * y;
double r4 = r2 * r2;
double r6 = r4 * r2;
// 切向畸变
double a1 = 2 * x * y;
double a2 = r2 + 2 * x * x;
double a3 = r2 + 2 * y * y;
double cdist = 1.0 + k1 * r2 + k2 * r4 + k3 * r6;
x = x * cdist + p1 * a1 + p2 * a2;
y = y * cdist + p1 * a3 + p2 * a1;
// UV
double u = fx * x + cx;
double v = fy * y + cy;
return g2o::Vector2(u * scale, v * scale);
}
每次记录对应的Track变换优化对应设计如下,已经每次记录track2base(track在Track坐标系下的变换),和根据需要求解的变量camera2track,base2target,scale以及摄像机在屏幕坐标系下的变换,得到求解出来的track2base,根据测试出来的track2base,比较误差.
// track姿态优化
class HandEyeEdge : public g2o::BaseMultiEdge<6, Eigen::Isometry3d> {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
public:
HandEyeEdge();
virtual bool read(std::istream& is);
virtual bool write(std::ostream& os) const;
virtual void computeError() override;
};
HandEyeEdge::HandEyeEdge() { resize(4); }
bool HandEyeEdge::read(std::istream& is) {
Vector7 est = {};
bool state = internal::readVector(is, est);
_measurement = internal::fromVectorQT(est);
return readInformationMatrix(is);
}
bool HandEyeEdge::write(std::ostream& os) const {
internal::writeVector(os, internal::toVectorQT(measurement()));
return writeInformationMatrix(os);
}
Eigen::Isometry3d getScaleIsometry3d(const Eigen::Isometry3d& src,
double scale) {
Eigen::Isometry3d result = src;
result.translation() = scale * src.translation();
return result;
};
void HandEyeEdge::computeError() {
// 摄像机相对Track
const g2o::VertexSE3* camera2trackVec =
dynamic_cast<const g2o::VertexSE3*>(_vertices[0]);
// 追踪器坐标系变换到角点坐标系
const g2o::VertexSE3* base2targetVec =
dynamic_cast<const g2o::VertexSE3*>(_vertices[1]);
// 角点
const g2o::VertexSE3* target2cameraVec =
dynamic_cast<const g2o::VertexSE3*>(_vertices[2]);
// 缩放
const ScaleVertex* scaleVec = dynamic_cast<const ScaleVertex*>(_vertices[3]);
// 当前值
Eigen::Isometry3d camera2track = camera2trackVec->estimate();
Eigen::Isometry3d base2target = base2targetVec->estimate();
Eigen::Isometry3d target2camera = target2cameraVec->estimate();
double scale = scaleVec->estimate();
// 优化量计算得到的Track
Eigen::Isometry3d base2track = camera2track * target2camera * base2target;
// 转化成世界尺度下
Eigen::Isometry3d measurementScale = getScaleIsometry3d(_measurement, scale);
// 比较计算的Track与测量的Track姿态
Eigen::Isometry3d delta = measurementScale * base2track;
_error = g2o::internal::toVectorMQT(delta);
}
同上面重投影边一样,在computeError把根据这些结果求得的track变换与测量的track变换比较误差,最后我们把所有已知数据与测量值组成图如下.
CameraTrack HandEyeOptimizer::compute(const HandEyeParamet& handEyeParamet,
const LensModel& lensModel,
const CameraTrack& cameraTrack) {
handEyePar = handEyeParamet;
eigen::ECameraTrack camtrack = {};
eigen::toCameraTrack(cameraTrack, camtrack);
Eigen::Isometry3d camera2track = Eigen::Isometry3d::Identity();
Eigen::Isometry3d base2target = Eigen::Isometry3d::Identity();
camera2track.matrix() = camtrack.camera2track;
base2target.matrix() = camtrack.base2target;
// 设置HandEyeEdge的变化率
Eigen::MatrixXd handeyeInf = Eigen::MatrixXd::Identity(6, 6);
// 移动部分
handeyeInf.topLeftCorner(3, 3) *= 0.01;
// 旋转部分
handeyeInf.bottomRightCorner(3, 3) *= 1.0;
// 优化器
SparseOptimizer optimizer;
using LinearSolver =
g2o::LinearSolverDense<g2o::BlockSolverX::PoseMatrixType>;
OptimizationAlgorithmGaussNewton* solver =
new g2o::OptimizationAlgorithmGaussNewton(
std::make_unique<g2o::BlockSolverX>(
std::make_unique<LinearSolver>()));
// 设置优化方法
optimizer.setAlgorithm(solver);
// 设置镜头参数
LensModelParameter* lensPar = new LensModelParameter(lensModel);
lensPar->uvScale = handEyeParamet.uvScale;
lensPar->setId(0);
optimizer.addParameter(lensPar);
// 优化量camera2Track
VertexSE3* c2tVec = new VertexSE3();
c2tVec->setEstimate(camera2track);
c2tVec->setId(0);
// c2tVec->setFixed(true);
optimizer.addVertex(c2tVec);
// 优化量base2target
VertexSE3* b2tVec = new VertexSE3();
b2tVec->setEstimate(base2target);
b2tVec->setId(1);
// b2tVec->setFixed(true);
optimizer.addVertex(b2tVec);
ScaleVertex* scaleVec = new ScaleVertex();
scaleVec->setEstimate(cameraTrack.scale);
scaleVec->setId(2);
scaleVec->setFixed(handEyePar.bFixScale);
optimizer.addVertex(scaleVec);
// 每条记录(包含摄像机变换,Track变换,角点UV与三维位置)
for (const TrackCorners& trackCorner : trackCorners) {
aoce::Mat4x4d trackPose = trackCorner.trackPose;
aoce::Mat4x4d cameraPose = trackCorner.cameraPose;
Eigen::Isometry3d track2base = Eigen::Isometry3d::Identity();
track2base.matrix() = eigen::toMat(trackPose);
// track转化成真实世界坐标系中
Eigen::Isometry3d track2baseScale =
getScaleIsometry3d(track2base, cameraTrack.scale);
// 角点相对摄像机坐标系的转换
Eigen::Isometry3d camera2target = Eigen::Isometry3d::Identity();
if (trackCorner.cameraPose.valid()) {
camera2target.matrix() = eigen::toMat(trackCorner.cameraPose);
} else {
camera2target = base2target * track2baseScale * camera2track;
}
Eigen::Isometry3d target2camera = camera2target.inverse();
g2o::VertexSE3* t2cVec = new g2o::VertexSE3();
t2cVec->setEstimate(target2camera);
t2cVec->setId(optimizer.vertices().size());
// t2cVec->setFixed(true);
optimizer.addVertex(t2cVec);
// 设置Track姿态边
HandEyeEdge* handEyeEdge = new HandEyeEdge();
handEyeEdge->vertices()[0] = c2tVec;
handEyeEdge->vertices()[1] = b2tVec;
handEyeEdge->vertices()[2] = t2cVec;
handEyeEdge->vertices()[3] = scaleVec;
// 比较实测与观测的Track数据
handEyeEdge->setMeasurement(track2base);
handEyeEdge->setInformation(handeyeInf);
handEyeEdge->setId(optimizer.edges().size());
if (handEyePar.robustHandEye) {
g2o::RobustKernelHuber* kerner = new g2o::RobustKernelHuber();
kerner->setDelta(handEyePar.handEyeDelta);
handEyeEdge->setRobustKernel(kerner);
}
optimizer.addEdge(handEyeEdge);
// 经测试,UV使用的粒度小时,精度会提升
vec2d sizeInv = {
handEyeParamet.uvScale / trackCorner.pointCorners.imageSize.x,
handEyeParamet.uvScale / trackCorner.pointCorners.imageSize.y};
for (int32_t i = 0; i < trackCorner.pointCorners.count; i++) {
vec2f corner = *(trackCorner.pointCorners.corners + i);
vec3f point = *(trackCorner.pointCorners.points + i);
// 设置投影边
ProjectionHandEdge* proEdge = new ProjectionHandEdge();
proEdge->setMeasurement({corner.x * sizeInv.x, corner.y * sizeInv.y});
proEdge->setInformation(Eigen::Matrix2d::Identity() * 0.01);
proEdge->vertices()[0] = t2cVec;
proEdge->setParameterId(0, 0);
proEdge->setId(optimizer.edges().size());
proEdge->point = {point.x, point.y, point.z};
if (handEyePar.projectionHand) {
g2o::RobustKernelHuber* kerner = new g2o::RobustKernelHuber();
kerner->setDelta(handEyePar.projectionHand);
proEdge->setRobustKernel(kerner);
}
optimizer.addEdge(proEdge);
}
}
// 执行优化
bool bInit = optimizer.initializeOptimization();
if (!bInit) {
logMessage(LogLevel::warn,
"ZoomScaleOptimizer::computePoseZoom init optimizer failed");
return cameraTrack;
}
// optimizer.setVerbose(true);
optimizer.optimize(10);
// 检测
camtrack.camera2track = c2tVec->estimate().matrix();
camtrack.base2target = b2tVec->estimate().matrix();
camtrack.scale = scaleVec->estimate();
CameraTrack result = {};
eigen::toCameraTrack(camtrack, result);
return result;
}
整个过程经过优化器多次迭代就能得到更优结果,相对于原始手眼标定的结果来说,优点不少,原始的数据越多,其直接组合一起计算结果很差,只能从各种组合计算结果使用重投影确定最优值,一般最多只选择其中不大于五条记录会是一个最好的结果,大部分记录被排除计算,数据量比较少的情况下,又不能保证结果在10个像素以内,使用图优化的方法,在记录少的情况就能得到非常优秀的结果,在记录多的情况下,更能保证更多的数据得到平均最优的结果,代入以前记录的各组记录数据,在图优化的情况下,能保证所有结果都能得到更好的结果.
在标定Track之后,知道镜头在某个焦段的内外参,扩展到变焦镜头,当镜头的zoom变化后,内参如何变化?
一般相机内参fx/fy,cx/cy,考虑畸变系数K1,K2,P1,P2,K3的畸变模型,与zoom有关的是fx/fy,cx/cy影响不大,畸变P1,P2现在相机在工艺上上,值非常小,对畸变的影响非常小,K3影响也不太大,故需要考虑的fx/fy,K1,K2这四个参数,考虑fx/fy比值固定,只需要考虑fx,k1,k2这三个参数在变焦镜头的zoom变化后,如何变化?
相机zoom变化后,内参不做变化,可以看到匹配的特征误差越来越大,也就是反投影的误差变大,误差优化参数,还得是图优化.
相比上一个手眼标定的图优化过程的点边模型,这个模型会简单不少,假定fx,k1,k2是zoom变化的曲线(ax^2+bx+c=y)变化,最后结果就是求得fx,k1,k2对应的a,b,c的值.对应有二种思路,一种是根据每个zoom下的图,反投影图优化得到fx,k1,k2,最后再拟合多个fx,k1,k2各自的曲线,第二种是直接把各自曲线所有zoom下的图然后反投影图优化一起优化,现二种最终结果相关不大,在定焦三个误差内,变焦后各zoom平均在10个像素,后面也还需要持续优化.
这里列一种处理的部分代码.
void ZoomCurveEdge::computeError() {
const CurveVertex* fxVer = dynamic_cast<const CurveVertex*>(_vertices[0]);
const CurveVertex* k1Ver = dynamic_cast<const CurveVertex*>(_vertices[1]);
const CurveVertex* k2Ver = dynamic_cast<const CurveVertex*>(_vertices[2]);
const ScaleVertex* scaleVer = dynamic_cast<const ScaleVertex*>(_vertices[3]);
const g2o::Vector3 fxCurve = fxVer->estimate();
const g2o::Vector3 k1Curve = k1Ver->estimate();
const g2o::Vector3 k2Curve = k2Ver->estimate();
const double sacle = scaleVer->estimate();
LensModel lensModel = lensModelPar->lensModel;
double aspectRatio = lensModel.focalLength.y / lensModel.focalLength.x;
double fx = getCurveVal(fxCurve, sacle);
lensModel.focalLength.x = fx;
lensModel.focalLength.y = fx * aspectRatio;
lensModel.K1 = getCurveVal(k1Curve, sacle);
lensModel.K2 = getCurveVal(k2Curve, sacle);
g2o::Vector2 estValue = lensMap(lensModel, point);
// 误差由观测值减预测值
_error = measurement() - estValue;
}
bool ZoomLensOptimizer::compute(const ZoomOptParamet& paramet) {
optParamet = paramet;
// 开始优化
SparseOptimizer optimizer;
// LinearSolverDense LinearSolverEigen LinearSolverPCG
using LinearSolver =
g2o::LinearSolverEigen<g2o::BlockSolverX::PoseMatrixType>;
// OptimizationAlgorithmGaussNewton OptimizationAlgorithmLevenberg
// OptimizationAlgorithmDogleg
OptimizationAlgorithmGaussNewton* solver =
new g2o::OptimizationAlgorithmGaussNewton(
std::make_unique<g2o::BlockSolverX>(
std::make_unique<LinearSolver>()));
// 设置优化方法
optimizer.setAlgorithm(solver);
// 设置参数
LensModelParameter* lensPar = new LensModelParameter(lensModel);
lensPar->setId(0);
optimizer.addParameter(lensPar);
// 曲线
CurveVertex* fxCurve = new CurveVertex();
fxCurve->setEstimate({0, 0, lensModel.focalLength.x});
fxCurve->setId(0);
optimizer.addVertex(fxCurve);
CurveVertex* k1Curve = new CurveVertex();
k1Curve->setEstimate({0, 0, lensModel.K1});
k1Curve->setId(1);
k1Curve->setFixed(optParamet.fixDistort);
optimizer.addVertex(k1Curve);
CurveVertex* k2Curve = new CurveVertex();
k2Curve->setEstimate({0, 0, lensModel.K2});
k2Curve->setId(2);
k2Curve->setFixed(true);
optimizer.addVertex(k2Curve);
std::vector<ScaleVertex*> fxList;
// 添加点与边
for (const CameraZoom& poseZoom : poseZooms) {
Eigen::Isometry3d target2camera = Eigen::Isometry3d::Identity();
target2camera.matrix() = eigen::toMat(invPoseMat(poseZoom.cameraPose));
// 假定变焦环的数据并不严谨(推理不出来,只能固定)
ScaleVertex* scaleValue = new ScaleVertex();
scaleValue->setId(optimizer.vertices().size());
scaleValue->setEstimate(poseZoom.zoomScale);
scaleValue->setFixed(true);
optimizer.addVertex(scaleValue);
//
vec2d sizeInv = {1.0 / poseZoom.pointCorners.imageSize.x,
1.0 / poseZoom.pointCorners.imageSize.y};
// 反投影
for (int32_t i = 0; i < poseZoom.pointCorners.count; i++) {
vec2f corner = *(poseZoom.pointCorners.corners + i);
vec3f point = *(poseZoom.pointCorners.points + i);
// 点在相机下位置
g2o::Vector3 cornerPos = {point.x, point.y, point.z};
g2o::Vector3 cameraPos = target2camera * cornerPos;
// 设置边
ZoomCurveEdge* zoomLensEdge = new ZoomCurveEdge();
zoomLensEdge->vertices()[0] = fxCurve;
zoomLensEdge->vertices()[1] = k1Curve;
zoomLensEdge->vertices()[2] = k2Curve;
zoomLensEdge->vertices()[3] = scaleValue;
zoomLensEdge->setMeasurement(
{corner.x * sizeInv.x, corner.y * sizeInv.y});
zoomLensEdge->setInformation(Eigen::Matrix2d::Identity());
zoomLensEdge->setParameterId(0, 0);
zoomLensEdge->setId(optimizer.edges().size());
zoomLensEdge->point = cameraPos;
if (true) {
g2o::RobustKernelHuber* kerner = new g2o::RobustKernelHuber();
kerner->setDelta(1.0);
zoomLensEdge->setRobustKernel(kerner);
}
optimizer.addEdge(zoomLensEdge);
}
}
// 执行优化
bool bInit = optimizer.initializeOptimization();
optimizer.optimize(10);
fxPar = fxCurve->estimate();
k1Par = k1Curve->estimate();
k2Par = k2Curve->estimate();
// 计算误差
LensModel slensModel = lensModel;
for (CameraZoom& poseZoom : poseZooms) {
Eigen::Vector2d offset = {0, 0};
if (getLensModel(poseZoom.zoomScale, slensModel)) {
Mat4x4d target2camera = poseZoom.cameraPose.inverse();
offset = projectOffset(target2camera, slensModel, poseZoom.pointCorners);
}
}
return true;
}
这个结果还需优化,在写到这里的时候仔细想了下,主要可能有几点,一是fx/fy在变焦下比值是否有相对大的变化,二是上述曲线是否有更科学的模型,三是现k2代入后,结果大部分结果更差,导致k2现在用固定的,但是焦距变化比较大后,K2的变化还是比较明显的,后面有机会再想想改进吧.
整个跟踪相关的算法差不多就是这个样子,其实算法占的比例并不大,主要是工程上的各种问题,如各种相机采集(Decklink,MF...),Redspy,Mosys硬件接入,以及给的是欧拉角,如何确定顺序,追踪数据如何通过LiveLink数据发送到UE,不同空间坐标系的变换,FBXMesh的导入与导出,各个模块如何有序的组合,动态链接库与UE使用冲突,等等这些细节问题才是更麻烦的.