From 46988ca633b3a470967d37a2f11c1f4b7362b373 Mon Sep 17 00:00:00 2001
From: Vadim Pisarevsky <no@email>
Date: Fri, 1 Oct 2010 09:48:56 +0000
Subject: [PATCH] some more fixes in rectify3 and 3calibration (corrected
 disparity ratio etc.)

---
 modules/calib3d/src/calibration.cpp |  4 ++++
 samples/cpp/3calibration.cpp        | 16 ++++++++++++++--
 2 files changed, 18 insertions(+), 2 deletions(-)

diff --git a/modules/calib3d/src/calibration.cpp b/modules/calib3d/src/calibration.cpp
index 81fd56292b..e4fa225fae 100644
--- a/modules/calib3d/src/calibration.cpp
+++ b/modules/calib3d/src/calibration.cpp
@@ -3533,6 +3533,8 @@ static void adjust3rdMatrix(const vector<vector<Point2f> >& imgpt1_0,
     P3.at<double>(1,1) *= a;
     P3.at<double>(0,2) = P3.at<double>(0,2)*a;
     P3.at<double>(1,2) = P3.at<double>(1,2)*a + b;
+    P3.at<double>(0,3) *= a;
+    P3.at<double>(1,3) *= a;
 }
 
 }
@@ -3587,6 +3589,8 @@ float cv::rectify3( const Mat& cameraMatrix1, const Mat& distCoeffs1,
     P2.copyTo(P3);
     Mat t = P3.col(3);
     t13.copyTo(t);
+    P3.at<double>(0,3) *= P3.at<double>(0,0);
+    P3.at<double>(1,3) *= P3.at<double>(1,1);
     
     if( !imgpt1.empty() && imgpt3.empty() )
         adjust3rdMatrix(imgpt1, imgpt3, cameraMatrix1, distCoeffs1, cameraMatrix3, distCoeffs3, R1, R3, P1, P3);
diff --git a/samples/cpp/3calibration.cpp b/samples/cpp/3calibration.cpp
index 09c54fd599..bc2cefae7b 100644
--- a/samples/cpp/3calibration.cpp
+++ b/samples/cpp/3calibration.cpp
@@ -301,12 +301,11 @@ int main( int argc, char** argv )
     
     fs << "imageWidth" << imageSize.width;
     fs << "imageHeight" << imageSize.height;
-    fs.release();
     
     Mat Q;
     
     // step 3: find rectification transforms
-    rectify3(cameraMatrix[0], distCoeffs[0], cameraMatrix[1],
+    double ratio = rectify3(cameraMatrix[0], distCoeffs[0], cameraMatrix[1],
              distCoeffs[1], cameraMatrix[2], distCoeffs[2],
              imgpt[0], imgpt[2],
              imageSize, R12, T12, R13, T13,
@@ -314,6 +313,19 @@ int main( int argc, char** argv )
              imageSize, 0, 0, CV_CALIB_ZERO_DISPARITY);
     Mat map1[3], map2[3];
     
+    fs << "R1" << R[0];
+    fs << "R2" << R[1];
+    fs << "R3" << R[2];
+    
+    fs << "P1" << P[0];
+    fs << "P2" << P[1];
+    fs << "P3" << P[2];
+    
+    fs << "disparityRatio" << ratio;
+    fs.release();
+    
+    printf("Disparity ratio = %g\n", ratio);
+    
     for( k = 0; k < 3; k++ )
         initUndistortRectifyMap(cameraMatrix[k], distCoeffs[k], R[k], P[k], imageSize, CV_16SC2, map1[k], map2[k]);