Get camera pos matrix from depth image

edit

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<cv::Point3d>& world_coords, 
     vector<cv::Point2d>& pixel_coords, cv::Mat& IntrinsicMatrix)
{
    float FocalInvertedX = 1/IntrinsicMatrix.at<float>(0,0);    // 1/fx
    float FocalInvertedY = 1/IntrinsicMatrix.at<float>(1,1);    // 1/fy

    cv::Point3d newPoint3D;
    cv::Point2d newPoint2D;
    for (int i=0;i<image.rows;i++)
    {
        for (int j=0;j<image.cols;j++)
        {
            float depthValue = image.at<float>(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<float>(0,2)) * newPoint3D.z * FocalInvertedX;
                newPoint3D.y = (i - IntrinsicMatrix.at<float>(1,2)) * newPoint3D.z * FocalInvertedY;
                world_coords.push_back(newPoint3D);

                newPoint2D.x =  j - IntrinsicMatrix.at<float>(0,2);
                newPoint2D.y =  i - IntrinsicMatrix.at<float>(1,2);
                pixel_coords.push_back(newPoint2D);
            }

        }
    }
}

    cv::Mat depthImage = cv::imread(name, CV_LOAD_IMAGE_ANYDEPTH | CV_LOAD_IMAGE_ANYCOLOR);

        vector<cv::Point3d>     world_coords;
    vector<cv::Point2d>     pixel_coords;
    getWorldAndImageCoordinates(depthImage, world_coords, pixel_coords, IntrinsicMatrix);
    cv::Mat distCoeffs = (cv::Mat_<double>(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;