Estou usando o OpenCV em um algoritmo de estimativa de pose, onde também estou tentando obter uma estimativa da incerteza da pose estimada. Minha estimativa de pose é realizada através de correspondências 3D-2D e do algoritmo PNP. Para obter uma estimativa da incerteza, estou tentando usar a função projectPoints () para reprojetar pontos 3D de volta no plano da imagem e usar a matriz jacobiana computada internamente para obter a covariância.

Meu recurso 'map' contém um conjunto plano de pontos 3D. A trajetória da minha câmera é simples: começo perto dos recursos, passo para trás e, assim, me distendo dos recursos e depois me aproximo novamente até chegar ao ponto inicial. Estou usando um simulador 3D (Unreal Engine) para obter essas imagens e tenho as intrínsecas corretas para a câmera (e ela tem distorção zero). O problema que estou vendo é que a covariância e o subsequente desvio padrão (para tradução) estimado usando projectPoints () não se correlacionam com o erro de posição real. À medida que me afasto, como deveria, a estimativa se torna pior e as poses são erráticas, mas o desvio padrão édiminuindo. Estou anexando um gráfico do erro de distância da pose estimada e, correspondentemente, o desvio padrão estimado, e eles têm perfis opostos. O código em que estou tentando calcular esse desvio padrão é o seguinte.

cv::Mat rvec, J;
cv::eigen2cv(pose.rotation(), rvec);
cv::Mat tvec = cv::Mat(1, 3, CV_32FC1, cv::Scalar::all(0));

tvec.at<float>(0, 0) = pose.center()[0];
tvec.at<float>(0, 1) = pose.center()[1];
tvec.at<float>(0, 2) = pose.center()[2];

cv::Mat p;
cv::projectPoints(objectPoints, rvec, tvec, K, dist, p, J);

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

for (int i = 0; i < p.rows; i++) {
    reprojected.push_back(cv::Point2f(p.at<double>(i, 0), p.at<double>(i, 1)));
}
cv::Mat Sigma = cv::Mat(J.t() * J, cv::Rect(0, 0, 6, 6)).inv();
cv::Mat std_dev;
sqrt(Sigma.diag(), std_dev);

Gráfico de erro de distância da estimativa versus verdade do solo; bem como o desvio padrão estimado:

Qual poderia ser o problema aqui? Entendo que o jacobiano entre uma mudança na pose e a mudança no erro de reprojeção não codificaria totalmente os erros de posição (por exemplo, uma pequena alteração em Z a uma grande distância dos pontos de recurso não mudaria muito o erro de reprojeção), mas como faço para obter estimativas confiáveis da qualidade da minha solução enquanto faço a estimativa de pose?