问题
I am using OpenCV in a pose estimation algorithm, where I am also attempting to obtain an estimate of the uncertainty of the estimated pose. My pose estimation is performed through 3D-2D correspondences and the PNP algorithm. To achieve uncertainty estimation, I am trying to use the projectPoints() function to reproject 3D points back on the image plane, and use the internally computed Jacobian matrix to obtain the covariance.
My feature 'map' contains a planar set of 3D points. My camera trajectory is a simple one: I start close to the features, move backwards and thereby far away from the features and then closer again till I reach the start point. I am using a 3D simulator (Unreal Engine) to obtain these images, and I have the correct intrinsics for the camera (and it has zero distortion). The problem I am seeing is that the covariance and the subsequent standard deviation (for translation) estimated using projectPoints() does not correlate with the actual position error. As I move farther away, as it should, the estimation becomes worse and the poses are erratic, but the standard deviation is decreasing. I am attaching a plot of the distance error of the estimated pose, and correspondingly the estimated standard deviation, and they have opposite profiles. The code where I am attempting to compute this standard deviation is as follows.
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);
Plot of distance error of estimation vs ground truth; as well as estimated standard deviation:
What could be the problem here? I understand that the Jacobian between a change in pose and the change in reprojection error would not fully encode position errors (for instance, a small change in Z at a large distance from the feature points would not change the reprojection error by much), but how do I achieve somewhat reliable estimates of my solution quality while doing pose estimation?
来源：https://stackoverflow.com/questions/48711052/opencv-projectpoints-inconsistent-covariance-standard-deviation-of-estimated