Revision history [back]

Hi,
I have images from two cameras (left and right). First I calibrate cameras and I get cameras matrices, distortion coefficients and projection matrices.
On images I detect marker and save it positions on left and right image. Next I use cv::triangulatePoints but I get really strange result (I use other library that also allows to get that 3D coords and it shows more likely results).

Here is my code:

int size0 = m_history.getSize(0);
int size1 = m_history.getSize(1);

if(size0 != size1)
{
    setInfo(tr("Cannot calculate triangulation: number of marker positions is different on left and right camera"));
    return false;
}
if(size0 <= 0)
{
    setInfo(tr("No marker postion saved. Cannot calc triangulation."));
    return false;
 }

cv::Mat pointsMat1(2, 1, CV_64F);
cv::Mat pointsMat2(2, 1, CV_64F);

for(int i = 0; i < size0; i++)
{
    cv::Point pt1 = m_history.getOriginalPoint(0, i);
    cv::Point pt2 = m_history.getOriginalPoint(1, i);

    pointsMat1.at<double>(0,0) = pt1.y;
    pointsMat1.at<double>(1,0) = pt1.y;
    pointsMat2.at<double>(0,0) = pt2.x;
    pointsMat2.at<double>(1,0) = pt2.y;

    cv::Mat pnts3D(4, 1, CV_32F);

    cv::triangulatePoints(m_projectionMat1, m_projectionMat2, pointsMat1, pointsMat2, pnts3D);

    CvPoint3D64f point3D;
    point3D.x = pnts3D.at<double>(0, 0);
    point3D.y = pnts3D.at<double>(1, 0);
    point3D.z = pnts3D.at<double>(2, 0);

    point3D.x = point3D.x/pnts3D.at<double>(3, 0);
    point3D.y = point3D.y/pnts3D.at<double>(3, 0);
    point3D.z = point3D.z/pnts3D.at<double>(3, 0);
    m_history.addTriangulatedPoint(point3D);
}

Where I do mistake?

Results from triangulation:

 3D_X              3D_Y              3D_Z

-5.50886e+23    -1.72188e+23    -2.74134e+24
1.45956e+23    -4.99296e+22    7.09517e+23
-1.54104e+23    1.59429e+23    -7.24014e+23
-6.51269e+22    1.10959e+23    -2.96057e+23
3.34095e+21    -7.89224e+21    1.47103e+22
-5.9235e+22    1.15083e+23    -2.63574e+23
-1.97861e+23    2.57096e+23    -9.19323e+23
2.70618e+24    -1.65746e+24    1.30049e+25
-8.69261e+23    -7.53462e+22    -4.27505e+24
-7.2011e+23    -3.12814e+23    -5.17617e+24

And from other library (it seems quite correct):

  3D_X              3D_Y              3D_Z

8.33399      4.74962       62.6119
8.33243       -1.62743       60.8669
8.40008       -8.43666       59.5643
8.47287       -15.1735       58.3184
8.52143       -21.964       57.0896
8.4673       -17.6672       57.6249
8.31039       -10.9452       58.6376
8.39497       -4.27203       60.6766
8.37444       2.56077       62.3287
5.41351       4.73769       62.7424

Hi,
I have images from two cameras (left and right). First I calibrate cameras and I get cameras matrices, distortion coefficients and projection matrices.
On images I detect marker and save it positions on left and right image. Next I use cv::triangulatePoints but I get really strange result (I use other library that also allows to get that 3D coords and it shows more likely results).

Here is my code:

int size0 = m_history.getSize(0);
int size1 = m_history.getSize(1);

if(size0 != size1)
{
    setInfo(tr("Cannot calculate triangulation: number of marker positions is different on left and right camera"));
    return false;
}
if(size0 <= 0)
{
    setInfo(tr("No marker postion saved. Cannot calc triangulation."));
    return false;
 }

cv::Mat pointsMat1(2, 1, CV_64F);
cv::Mat pointsMat2(2, 1, CV_64F);

for(int i = 0; i < size0; i++)
{
    cv::Point pt1 = m_history.getOriginalPoint(0, i);
    cv::Point pt2 = m_history.getOriginalPoint(1, i);

    pointsMat1.at<double>(0,0) = pt1.y;
    pointsMat1.at<double>(1,0) = pt1.y;
    pointsMat2.at<double>(0,0) = pt2.x;
    pointsMat2.at<double>(1,0) = pt2.y;

    cv::Mat pnts3D(4, 1, CV_32F);

    cv::triangulatePoints(m_projectionMat1, m_projectionMat2, pointsMat1, pointsMat2, pnts3D);

    CvPoint3D64f point3D;
    point3D.x = pnts3D.at<double>(0, 0);
    point3D.y = pnts3D.at<double>(1, 0);
    point3D.z = pnts3D.at<double>(2, 0);

    point3D.x = point3D.x/pnts3D.at<double>(3, 0);
    point3D.y = point3D.y/pnts3D.at<double>(3, 0);
    point3D.z = point3D.z/pnts3D.at<double>(3, 0);
    m_history.addTriangulatedPoint(point3D);
}

Where I do mistake?

Results from triangulation:

 3D_X              3D_Y              3D_Z

-5.50886e+23    -1.72188e+23    -2.74134e+24
1.45956e+23    -4.99296e+22    7.09517e+23
-1.54104e+23    1.59429e+23    -7.24014e+23
-6.51269e+22    1.10959e+23    -2.96057e+23
3.34095e+21    -7.89224e+21    1.47103e+22
-5.9235e+22    1.15083e+23    -2.63574e+23
-1.97861e+23    2.57096e+23    -9.19323e+23
2.70618e+24    -1.65746e+24    1.30049e+25
-8.69261e+23    -7.53462e+22    -4.27505e+24
-7.2011e+23    -3.12814e+23    -5.17617e+24
-0.143595   -4.22246    -24.185
2.04136     -0.614643   -15.7484
3.16721     1.25578     -11.3336
3.8125      2.31469     -8.88105
4.22874     3.00726     -7.24257
3.98013     2.59997     -8.18345
3.43011     1.68647     -10.3386
2.53443     0.215475    -13.7499
 0.805428   -2.65543    -20.5206
-0.187007   -5.49902    -31.4968

And from other library (it seems quite correct):

  3D_X              3D_Y              3D_Z

8.33399      4.74962       62.6119
8.33243       -1.62743       60.8669
8.40008       -8.43666       59.5643
8.47287       -15.1735       58.3184
8.52143       -21.964       57.0896
8.4673       -17.6672       57.6249
8.31039       -10.9452       58.6376
8.39497       -4.27203       60.6766
8.37444       2.56077       62.3287
5.41351       4.73769       62.7424

Hi,
I have images from two cameras (left and right). First I calibrate cameras and I get cameras matrices, distortion coefficients and projection matrices.
On images I detect marker and save it positions on left and right image. Next I use cv::triangulatePoints but I get really strange result (I use other library that also allows to get that 3D coords and it shows more likely results).

Here is my code:

int size0 = m_history.getSize(0);
int size1 = m_history.getSize(1);

if(size0 != size1)
{
    setInfo(tr("Cannot calculate triangulation: number of marker positions is different on left and right camera"));
    return false;
}
if(size0 <= 0)
{
    setInfo(tr("No marker postion saved. Cannot calc triangulation."));
    return false;
 }

cv::Mat pointsMat1(2, 1, CV_64F);
cv::Mat pointsMat2(2, 1, CV_64F);

for(int i = 0; i < size0; i++)
{
    cv::Point pt1 = m_history.getOriginalPoint(0, i);
    cv::Point pt2 = m_history.getOriginalPoint(1, i);

    pointsMat1.at<double>(0,0) = pt1.y;
    pointsMat1.at<double>(1,0) = pt1.y;
    pointsMat2.at<double>(0,0) = pt2.x;
    pointsMat2.at<double>(1,0) = pt2.y;

    cv::Mat pnts3D(4, 1, CV_32F);

    cv::triangulatePoints(m_projectionMat1, m_projectionMat2, pointsMat1, pointsMat2, pnts3D);

    CvPoint3D64f point3D;
    point3D.x = pnts3D.at<double>(0, 0);
    point3D.y = pnts3D.at<double>(1, 0);
    point3D.z = pnts3D.at<double>(2, 0);

    point3D.x = point3D.x/pnts3D.at<double>(3, 0);
    point3D.y = point3D.y/pnts3D.at<double>(3, 0);
    point3D.z = point3D.z/pnts3D.at<double>(3, 0);
    m_history.addTriangulatedPoint(point3D);
}

Where I do mistake?

Results from triangulation:

 3D_X              3D_Y              3D_Z

-0.143595   -4.22246    -24.185
2.04136     -0.614643   -15.7484
3.16721     1.25578     -11.3336
3.8125      2.31469     -8.88105
4.22874     3.00726     -7.24257
3.98013     2.59997     -8.18345
3.43011     1.68647     -10.3386
2.53443     0.215475    -13.7499
 0.805428   -2.65543    -20.5206
-0.187007   -5.49902    -31.4968

And from other library (it seems quite correct):

  3D_X              3D_Y              3D_Z

8.33399      4.74962       62.6119
8.33243       -1.62743       60.8669
8.40008       -8.43666       59.5643
8.47287       -15.1735       58.3184
8.52143       -21.964       57.0896
8.4673       -17.6672       57.6249
8.31039       -10.9452       58.6376
8.39497       -4.27203       60.6766
8.37444       2.56077       62.3287
5.41351       4.73769       62.7424

P1 = 
    4.484533e+02         0             3.073146e+02    0 
           0           4.484533e+02    2.473914e+02    0 
           0                  0             1          0

P2 = 
    4.484533e+02         0             3.073146e+02     -8.275082e+02 
           0           4.484533e+02    2.473914e+02          0 
           0                  0              1               0

Hi,
I have images from two cameras (left and right). First I calibrate cameras and I get cameras matrices, distortion coefficients and projection matrices.
On images I detect marker and save it positions on left and right image. Next I use cv::triangulatePoints but I get really strange result (I use other library that also allows to get that 3D coords and it shows more likely results).

Here is my code:

int size0 = m_history.getSize(0);
int size1 = m_history.getSize(1);

if(size0 != size1)
{
    setInfo(tr("Cannot calculate triangulation: number of marker positions is different on left and right camera"));
    return false;
}
if(size0 <= 0)
{
    setInfo(tr("No marker postion saved. Cannot calc triangulation."));
    return false;
 }

cv::Mat pointsMat1(2, 1, CV_64F);
cv::Mat pointsMat2(2, 1, CV_64F);

for(int i = 0; i < size0; i++)
{
    cv::Point pt1 = m_history.getOriginalPoint(0, i);
    cv::Point pt2 = m_history.getOriginalPoint(1, i);

    pointsMat1.at<double>(0,0) = pt1.y;
    pointsMat1.at<double>(1,0) = pt1.y;
    pointsMat2.at<double>(0,0) = pt2.x;
    pointsMat2.at<double>(1,0) = pt2.y;

    cv::Mat pnts3D(4, 1, CV_32F);

    cv::triangulatePoints(m_projectionMat1, m_projectionMat2, pointsMat1, pointsMat2, pnts3D);

    CvPoint3D64f point3D;
    point3D.x = pnts3D.at<double>(0, 0);
    point3D.y = pnts3D.at<double>(1, 0);
    point3D.z = pnts3D.at<double>(2, 0);

    point3D.x = point3D.x/pnts3D.at<double>(3, 0);
    point3D.y = point3D.y/pnts3D.at<double>(3, 0);
    point3D.z = point3D.z/pnts3D.at<double>(3, 0);
    m_history.addTriangulatedPoint(point3D);
}

Where I do mistake?

Results from triangulation:

 3D_X              3D_Y              3D_Z

-0.143595   -4.22246    -24.185
2.04136     -0.614643   -15.7484
3.16721     1.25578     -11.3336
3.8125      2.31469     -8.88105
4.22874     3.00726     -7.24257
3.98013     2.59997     -8.18345
3.43011     1.68647     -10.3386
2.53443     0.215475    -13.7499
 0.805428   -2.65543    -20.5206
-0.187007   -5.49902    -31.4968
1.20151e+24    7.70359e+23     4.41239e+24
-2.23198e+23    -3.11741e+22    -7.98741e+23
3.79697e+22    -1.45599e+22    1.31405e+23
-7.27922e+22    6.40761e+22    -2.45849e+23
7.11722e+22    -9.67023e+22    2.32894e+23
2.14094e+22    -2.26098e+22    7.11648e+22
4.95297e+22    -2.77331e+22    1.70013e+23
2.21597e+23    -1.22193e+22    7.79736e+23
-3.94602e+23    -1.85898e+23    -1.43658e+24
2.56816e+24    2.13037e+24     1.22021e+25

And from other library (it seems quite correct):

  3D_X              3D_Y              3D_Z

8.33399      4.74962       62.6119
8.33243       -1.62743       60.8669
8.40008       -8.43666       59.5643
8.47287       -15.1735       58.3184
8.52143       -21.964       57.0896
8.4673       -17.6672       57.6249
8.31039       -10.9452       58.6376
8.39497       -4.27203       60.6766
8.37444       2.56077       62.3287
5.41351       4.73769       62.7424

My projection matrices:

P1 = 
    4.484533e+02         0             3.073146e+02    0 
           0           4.484533e+02    2.473914e+02    0 
           0                  0             1          0

P2 = 
    4.484533e+02         0             3.073146e+02     -8.275082e+02 
           0           4.484533e+02    2.473914e+02          0 
           0                  0              1               0

Hi,
I have images from two cameras (left and right). First I calibrate cameras and I get cameras matrices, distortion coefficients and projection matrices.
On images I detect marker and save it positions on left and right image. Next I use cv::triangulatePoints but I get really strange result (I use other library that also allows to get that 3D coords and it shows more likely results).

Here is my code:

int size0 = m_history.getSize(0);
int size1 = m_history.getSize(1);

if(size0 != size1)
{
    setInfo(tr("Cannot calculate triangulation: number of marker positions is different on left and right camera"));
    return false;
}
if(size0 <= 0)
{
    setInfo(tr("No marker postion saved. Cannot calc triangulation."));
    return false;
 }

cv::Mat pointsMat1(2, 1, CV_64F);
cv::Mat pointsMat2(2, 1, CV_64F);

for(int i = 0; i < size0; i++)
{
    cv::Point2f pt1 = m_history.getOriginalPoint(0, i);   //before edit, here was cv::Point pt1 = m_history.getOriginalPoint(0, i);
    not cv::Point2f
    cv::Point2f pt2 = m_history.getOriginalPoint(1, i);   //before edit, here was cv::Point pt2 = m_history.getOriginalPoint(1, i);
not cv::Point2f

    pointsMat1.at<double>(0,0) = pt1.y;
    pointsMat1.at<double>(1,0) = pt1.y;
    pointsMat2.at<double>(0,0) = pt2.x;
    pointsMat2.at<double>(1,0) = pt2.y;

    cv::Mat pnts3D(4, 1, CV_32F);

    cv::triangulatePoints(m_projectionMat1, m_projectionMat2, pointsMat1, pointsMat2, pnts3D);

    CvPoint3D64f point3D;
    point3D.x = pnts3D.at<double>(0, 0);
    point3D.y = pnts3D.at<double>(1, 0);
    point3D.z = pnts3D.at<double>(2, 0);

    point3D.x = point3D.x/pnts3D.at<double>(3, 0);
    point3D.y = point3D.y/pnts3D.at<double>(3, 0);
    point3D.z = point3D.z/pnts3D.at<double>(3, 0);
    m_history.addTriangulatedPoint(point3D);
}

Where I do mistake?

Results from triangulation:

 3D_X              3D_Y              3D_Z

1.20151e+24    7.70359e+23     4.41239e+24
-2.23198e+23    -3.11741e+22    -7.98741e+23
3.79697e+22    -1.45599e+22    1.31405e+23
-7.27922e+22    6.40761e+22    -2.45849e+23
7.11722e+22    -9.67023e+22    2.32894e+23
2.14094e+22    -2.26098e+22    7.11648e+22
4.95297e+22    -2.77331e+22    1.70013e+23
2.21597e+23    -1.22193e+22    7.79736e+23
-3.94602e+23    -1.85898e+23    -1.43658e+24
2.56816e+24    2.13037e+24     1.22021e+25

And from other library (it seems quite correct):

  3D_X              3D_Y              3D_Z

8.33399      4.74962       62.6119
8.33243       -1.62743       60.8669
8.40008       -8.43666       59.5643
8.47287       -15.1735       58.3184
8.52143       -21.964       57.0896
8.4673       -17.6672       57.6249
8.31039       -10.9452       58.6376
8.39497       -4.27203       60.6766
8.37444       2.56077       62.3287
5.41351       4.73769       62.7424

My projection matrices:

P1 = 
    4.484533e+02         0             3.073146e+02    0 
           0           4.484533e+02    2.473914e+02    0 
           0                  0             1          0

P2 = 
    4.484533e+02         0             3.073146e+02     -8.275082e+02 
           0           4.484533e+02    2.473914e+02          0 
           0                  0              1               0

3D_X    3D_Y    3D_Z    
138594  107752  -183099
130050  101154  -171815
120401  93695   -159070
111792  87039   -147699
105006  81798.1 -138737
109027  84902.4 -144047
117172  91198.6 -154805
126463  98382   -167077
135853  105637  -179480
135919  105656  -179539

Hi,
I have images from two cameras (left and right). First I calibrate cameras and I get cameras matrices, distortion coefficients and projection matrices.
On images I detect marker and save it positions on left and right image. Next I use cv::triangulatePoints but I get really strange result (I use other library that also allows to get that 3D coords and it shows more likely results).

Here is my code:

int size0 = m_history.getSize(0);
int size1 = m_history.getSize(1);

if(size0 != size1)
{
    setInfo(tr("Cannot calculate triangulation: number of marker positions is different on left and right camera"));
    return false;
}
if(size0 <= 0)
{
    setInfo(tr("No marker postion saved. Cannot calc triangulation."));
    return false;
 }

cv::Mat pointsMat1(2, 1, CV_64F);
cv::Mat pointsMat2(2, 1, CV_64F);

for(int i = 0; i < size0; i++)
{
    cv::Point2f pt1 = m_history.getOriginalPoint(0, i);   //before edit, here was cv::Point not cv::Point2f
    cv::Point2f pt2 = m_history.getOriginalPoint(1, i);   //before edit, here was cv::Point not cv::Point2f

    pointsMat1.at<double>(0,0) = pt1.y;
    pointsMat1.at<double>(1,0) = pt1.y;
    pointsMat2.at<double>(0,0) = pt2.x;
    pointsMat2.at<double>(1,0) = pt2.y;

    cv::Mat pnts3D(4, 1, CV_32F);

    cv::triangulatePoints(m_projectionMat1, m_projectionMat2, pointsMat1, pointsMat2, pnts3D);

    CvPoint3D64f point3D;
    point3D.x = pnts3D.at<double>(0, 0);
    point3D.y = pnts3D.at<double>(1, 0);
    point3D.z = pnts3D.at<double>(2, 0);

    point3D.x = point3D.x/pnts3D.at<double>(3, 0);
    point3D.y = point3D.y/pnts3D.at<double>(3, 0);
    point3D.z = point3D.z/pnts3D.at<double>(3, 0);
    m_history.addTriangulatedPoint(point3D);
}

Where I do mistake?

Results from triangulation:

 3D_X              3D_Y              3D_Z

1.20151e+24    7.70359e+23     4.41239e+24
-2.23198e+23    -3.11741e+22    -7.98741e+23
3.79697e+22    -1.45599e+22    1.31405e+23
-7.27922e+22    6.40761e+22    -2.45849e+23
7.11722e+22    -9.67023e+22    2.32894e+23
2.14094e+22    -2.26098e+22    7.11648e+22
4.95297e+22    -2.77331e+22    1.70013e+23
2.21597e+23    -1.22193e+22    7.79736e+23
-3.94602e+23    -1.85898e+23    -1.43658e+24
2.56816e+24    2.13037e+24     1.22021e+25

And from other library (it seems quite correct):

  3D_X              3D_Y              3D_Z

8.33399      4.74962       62.6119
8.33243       -1.62743       60.8669
8.40008       -8.43666       59.5643
8.47287       -15.1735       58.3184
8.52143       -21.964       57.0896
8.4673       -17.6672       57.6249
8.31039       -10.9452       58.6376
8.39497       -4.27203       60.6766
8.37444       2.56077       62.3287
5.41351       4.73769       62.7424

My projection matrices:

P1 = 
    4.484533e+02         0             3.073146e+02    0 
           0           4.484533e+02    2.473914e+02    0 
           0                  0             1          0

P2 = 
    4.484533e+02         0             3.073146e+02     -8.275082e+02 
           0           4.484533e+02    2.473914e+02          0 
           0                  0              1               0

EDIT:

After correct cv::Point to cv::Point2f and with cv::undistortPoints I get that:that: (I pass to cv::undistortPoints only cameraMatrix and distortinCoeffs)

3D_X    3D_Y    3D_Z    
138594  107752  -183099
130050  101154  -171815
120401  93695   -159070
111792  87039   -147699
105006  81798.1 -138737
109027  84902.4 -144047
117172  91198.6 -154805
126463  98382   -167077
135853  105637  -179480
135919  105656  -179539

3D_X    3D_Y    3D_Z    
11.2226   4.65206     36.4178
10.0705   0.61889     31.2949
8.70359   -2.53934    25.3311
7.23004   -4.29649    19.1758
5.65666   -4.61029    12.9425
6.65943   -4.58273    16.8587
8.18533   -3.32853    23.1648
9.58548   -0.701837   29.1267
10.8634   3.17187     34.7096
9.81334   4.98224     38.1507

Hi,
I have images from two cameras (left and right). First I calibrate cameras and I get cameras matrices, distortion coefficients and projection matrices.
On images I detect marker and save it positions on left and right image. Next I use cv::triangulatePoints but I get really strange result (I use other library that also allows to get that 3D coords and it shows more likely results).

Here is my code:

int size0 = m_history.getSize(0);
int size1 = m_history.getSize(1);

if(size0 != size1)
{
    setInfo(tr("Cannot calculate triangulation: number of marker positions is different on left and right camera"));
    return false;
}
if(size0 <= 0)
{
    setInfo(tr("No marker postion saved. Cannot calc triangulation."));
    return false;
 }

cv::Mat pointsMat1(2, 1, CV_64F);
cv::Mat pointsMat2(2, 1, CV_64F);

for(int i = 0; i < size0; i++)
{
    cv::Point2f pt1 = m_history.getOriginalPoint(0, i);   //before edit, here was cv::Point not cv::Point2f
    cv::Point2f pt2 = m_history.getOriginalPoint(1, i);   //before edit, here was cv::Point not cv::Point2f

    pointsMat1.at<double>(0,0) = pt1.y;
    pointsMat1.at<double>(1,0) = pt1.y;
    pointsMat2.at<double>(0,0) = pt2.x;
    pointsMat2.at<double>(1,0) = pt2.y;

    cv::Mat pnts3D(4, 1, CV_32F);

    cv::triangulatePoints(m_projectionMat1, m_projectionMat2, pointsMat1, pointsMat2, pnts3D);

    CvPoint3D64f point3D;
    point3D.x = pnts3D.at<double>(0, 0);
    point3D.y = pnts3D.at<double>(1, 0);
    point3D.z = pnts3D.at<double>(2, 0);

    point3D.x = point3D.x/pnts3D.at<double>(3, 0);
    point3D.y = point3D.y/pnts3D.at<double>(3, 0);
    point3D.z = point3D.z/pnts3D.at<double>(3, 0);
    m_history.addTriangulatedPoint(point3D);
}

Where I do mistake?

Results from triangulation:

 3D_X              3D_Y              3D_Z

1.20151e+24    7.70359e+23     4.41239e+24
-2.23198e+23    -3.11741e+22    -7.98741e+23
3.79697e+22    -1.45599e+22    1.31405e+23
-7.27922e+22    6.40761e+22    -2.45849e+23
7.11722e+22    -9.67023e+22    2.32894e+23
2.14094e+22    -2.26098e+22    7.11648e+22
4.95297e+22    -2.77331e+22    1.70013e+23
2.21597e+23    -1.22193e+22    7.79736e+23
-3.94602e+23    -1.85898e+23    -1.43658e+24
2.56816e+24    2.13037e+24     1.22021e+25

And from other library (it seems quite correct):

  3D_X              3D_Y              3D_Z

8.33399      4.74962       62.6119
8.33243       -1.62743       60.8669
8.40008       -8.43666       59.5643
8.47287       -15.1735       58.3184
8.52143       -21.964       57.0896
8.4673       -17.6672       57.6249
8.31039       -10.9452       58.6376
8.39497       -4.27203       60.6766
8.37444       2.56077       62.3287
5.41351       4.73769       62.7424

My projection matrices:

P1 = 
    4.484533e+02         0             3.073146e+02    0 
           0           4.484533e+02    2.473914e+02    0 
           0                  0             1          0

P2 = 
    4.484533e+02         0             3.073146e+02     -8.275082e+02 
           0           4.484533e+02    2.473914e+02          0 
           0                  0              1               0

EDIT:

After correct cv::Point to cv::Point2f and with cv::undistortPoints I get that: (I pass to cv::undistortPoints only cameraMatrix and distortinCoeffs)

3D_X    3D_Y    3D_Z    
138594  107752  -183099
130050  101154  -171815
120401  93695   -159070
111792  87039   -147699
105006  81798.1 -138737
109027  84902.4 -144047
117172  91198.6 -154805
126463  98382   -167077
135853  105637  -179480
135919  105656  -179539

EDIT2: EDIT2:

(I pass to cv::undistortPoints not only cameraMatrix and distortinCoeffs but also rotation matrix and projection matrix) - it only looks better but when I try to create graph (XY) I get different figure in comparison to graphs created from original values or from values from other library.

3D_X    3D_Y    3D_Z    
11.2226   4.65206     36.4178
10.0705   0.61889     31.2949
8.70359   -2.53934    25.3311
7.23004   -4.29649    19.1758
5.65666   -4.61029    12.9425
6.65943   -4.58273    16.8587
8.18533   -3.32853    23.1648
9.58548   -0.701837   29.1267
10.8634   3.17187     34.7096
9.81334   4.98224     38.1507