在MATLAB中使用fminunc最小化重投影错误

Question

我已经获得了一个数据集，我必须在其中运行一个优化函数，以便找到一组给出最小重投影误差的参数。

我必须在MATLAB中使用fminunc()。最小化期限如下

K是相机内在函数，P_j是相机姿势，p_i是3D空间中的要素位置，这三个项相乘会给出要素的投影。 x_ij是要素的2D图像。减去投影和图像会产生重投影错误。

每个相机姿势仅观察一定数量的特征，因此我们无法计算每个姿势的每个特征的重投影误差。因此v_ij变量是一个布尔值，用于确定点i是否在姿势j中可见。在取得标准后，我们​​将每个姿势中每个点的所有重投影误差相加，得出总重投影误差。必须最小化此错误。

问题在于我无法解决如何在我可以传递给fminunc()的函数中编写此函数的问题。

我写了一个函数来计算特定姿势的重投影错误。我现在需要一种方法来为每个姿势做这件事并以某种方式优化错误。这就是我迷路的地方。

function error = reprojectenError(P,K,p,x,m)

% Get current pose
pose = P;

% Get the IDs for the features available for current pose
ids = m;

% Get available 3D features based on the IDs
feature_3d = p(:,ids);

% Get the projected features for the current pose
feature_2d = x;

% Normalize z row of projection to use x and y coordinates.
projection = K*pose*feature_3d;
projection_n = projection(1:2,:)./repmat(projection(3,:),2,1);

calc = projection_n - feature_2d;

% Calculate norm
error = sqrt(sum(calc.^2,1)).';
end

循环每个姿势的代码

options = optimoptions('fminunc','MaxFunEvals',5000);

for k = 1:4
    e = fminunc(@reprojectenError,P{k},options,K,p,x{k},m{k})
    err_min = reprojectenError(e,K,p,x{k},m{k})
end

变量赋值

P = bad.camera_poses;
K = bad.camera_intrinsics;
p = bad.feature_points_3d; % actual location of feature
x = bad.feature_locations; % projected feature
m = bad.feature_ids; % mapping table