opencvのtriangulatePoints関数の返り値が間違っている

格子状に配置された点群をcv::projectPointsで投影し2次元点群にしたものを2つ用意し、その点対応から元の3次元点群を復元したいのですが、cv::triangulatePointsから誤った出力が返ってきます。

入力にしている点群は座標(0,0,0)から(922,622,0)まで格子状に並んだもので、出力も同じ点群になることを期待しているのですが、出力は下記図のようにXY平面に平行でない上、一部が歪んだ格子状の点群になります。

作成したプログラムを以下に示します。
お手数ですが、自分の作成したプログラムの間違いを指摘していただければ幸いです。

なお、ソースコードのカメラ行列は自分の手元にあるカメラでチェスボードを撮影しcv::calibrateCamera関数で得たもので、
設定しているカメラ姿勢はチェスボードを撮影しcv::solvePnPで得たR,tをキリのいい数字に直したものです。

環境：c++17, OpenCV4.3.0

c++
1#include <vector>
2#include <fstream>
3
4#include <opencv2/calib3d.hpp>
5
6int main(){
7    cv::Mat cam_mat = cv::Mat::eye(3,3, CV_64F);
8    cam_mat.at<double>(0,0) = 2273.757852913953; //fx
9    cam_mat.at<double>(1,1) = 2374.806633049482; //fy
10    cam_mat.at<double>(0,2) = 568.627974744841; //cx
11    cam_mat.at<double>(1,2) = 831.8025005895665; //cy
12
13    cv::Mat dist_mat = cv::Mat::zeros(1,5, CV_64F);
14    dist_mat.at<double>(0,0) = 0.2585532699568209; //k1
15    dist_mat.at<double>(0,1) = -0.7173506557751326; //k2
16    dist_mat.at<double>(0,2) = 0.06015190650260366; //p1
17    dist_mat.at<double>(0,3) = -0.02108169455279181; //p2
18    dist_mat.at<double>(0,4) = 2.0048061219599; //k3
19
20    //Set camera poses
21    cv::Mat camera1_rot = (cv::Mat_<double>(1,3) << M_PI_2, -M_PI_2, M_PI/8);
22    cv::Mat camera1_t = (cv::Mat_<double>(1,3) << 100,-200,1300);
23    cv::Mat camera2_rot = (cv::Mat_<double>(1,3) << M_PI_2,-M_PI_2,M_PI/8);
24    cv::Mat camera2_t = (cv::Mat_<double>(1,3) << 200,-200,1300);
25
26    //Correct 3D points
27    std::vector<cv::Point3f> corners;
28    for(int i = 0; i < 10; ++i){
29        for(int j = 0; j < 7; ++j){
30            cv::Point3f p(i * 22, //[mm]
31                          j * 22, //[mm]
32                          0.0);
33            corners.push_back(p);
34        }
35    }
36
37    //Calculate points on the image by projection
38    std::vector<cv::Point2f> projection_points_from_camera1;
39    cv::projectPoints(corners,
40                      camera1_rot,
41                      camera1_t,
42                      cam_mat,
43                      dist_mat,
44                      projection_points_from_camera1
45    );
46
47    std::vector<cv::Point2f> projection_points_from_camera2;
48    cv::projectPoints(corners,
49                      camera2_rot,
50                      camera2_t,
51                      cam_mat,
52                      dist_mat,
53                      projection_points_from_camera2
54    );
55
56    //Create A[R|t] projection matrix
57    cv::Mat project_mat_1;
58    cv::Mat project_mat_2;
59    cv::Rodrigues(camera1_rot, project_mat_1);
60    cv::Rodrigues(camera2_rot, project_mat_2);
61    cv::hconcat(project_mat_1, camera1_t.t(), project_mat_1);
62    cv::hconcat(project_mat_2, camera2_t.t(), project_mat_2);
63    project_mat_1 = cam_mat * project_mat_1;
64    project_mat_2 = cam_mat * project_mat_2;
65
66    //Convert double to float for cv::triangulatePoints
67    project_mat_1.convertTo(project_mat_1, CV_32F);
68    project_mat_2.convertTo(project_mat_2, CV_32F);
69
70    cv::Mat result;
71    cv::triangulatePoints(project_mat_1, project_mat_2, projection_points_from_camera1, projection_points_from_camera2, result);
72
73    //Divide by scale
74    for(int i=0; i<result.cols; i++)
75    {
76        result.at<float>(0,i) /= result.at<float>(3,i);
77        result.at<float>(1,i) /= result.at<float>(3,i);
78        result.at<float>(2,i) /= result.at<float>(3,i);
79        result.at<float>(3,i) /= result.at<float>(3,i);
80    }
81
82    std::ofstream ofs("result.csv", std::ios::out);
83    for(int i=0;i<result.cols; i++)
84    {
85        ofs << result.at<float>(0,i) << ",";
86        ofs << result.at<float>(1,i) << ",";
87        ofs << result.at<float>(2,i) << std::endl;
88    }
89
90    return 0;
91}
92