Get camera pos matrix from depth image

Hello, I have got depth image And I need to get Camera pos matrix, I suppose my 2D and 3D points are wrong void getWorldAndImageCoordinates(cv::Mat& image, vector& world_coords, vector& pixel_coords, cv::Mat& IntrinsicMatrix) { float FocalInvertedX = 1/IntrinsicMatrix.at(0,0); // 1/fx float FocalInvertedY = 1/IntrinsicMatrix.at(1,1); // 1/fy cv::Point3d newPoint3D; cv::Point2d newPoint2D; for (int i=0;i(i,j); // if depthValue is not NaN if ((depthValue == depthValue) && (depthValue!=0) && (depthValue!=1)) { // Find 3D position: newPoint3D.z = depthValue; newPoint3D.x = (j - IntrinsicMatrix.at(0,2)) * newPoint3D.z * FocalInvertedX; newPoint3D.y = (i - IntrinsicMatrix.at(1,2)) * newPoint3D.z * FocalInvertedY; world_coords.push_back(newPoint3D); newPoint2D.x = j - IntrinsicMatrix.at(0,2); newPoint2D.y = i - IntrinsicMatrix.at(1,2); pixel_coords.push_back(newPoint2D); } } } }
cv::Mat depthImage = cv::imread(name, CV_LOAD_IMAGE_ANYDEPTH | CV_LOAD_IMAGE_ANYCOLOR); vector world_coords; vector pixel_coords; getWorldAndImageCoordinates(depthImage, world_coords, pixel_coords, IntrinsicMatrix); cv::Mat distCoeffs = (cv::Mat_(1,5) << 0, 0, 0, 0, 0); cv::Mat rotation_vector, translation_vector; solvePnP(world_coords, pixel_coords, IntrinsicMatrix, distCoeffs, rotation_vector, translation_vector, false, 0 ); cv::Mat Rt; cv::Rodrigues(rotation_vector, Rt); cv::Mat R; cv::transpose(Rt, R); cv::Mat coords = -R * translation_vector;