make optimization of omnidir::calibrate more stable

modules/ccalib/include/opencv2/ccalib/multiCameraCalibration.hpp

@@ -133,10 +133,10 @@ public:
     @matcher feature matcher.
     */
     multiCameraCalibration(int cameraType, int nCameras, const std::string& fileName, float patternWidth,
-        float patternHeight, int showExtration = 0, int nMiniMatches = 20, int flags = 0,
-        TermCriteria criteria = TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 100, 1e-5),
-        Ptr<FeatureDetector> detector = AKAZE::create(AKAZE::DESCRIPTOR_MLDB, 0, 3, 0.005f),
-        Ptr<DescriptorExtractor> descriptor = AKAZE::create(AKAZE::DESCRIPTOR_MLDB,0, 3, 0.005f),
+        float patternHeight, int verbose = 0, int showExtration = 0, int nMiniMatches = 20, int flags = 0,
+        TermCriteria criteria = TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 200, 1e-7),
+        Ptr<FeatureDetector> detector = AKAZE::create(AKAZE::DESCRIPTOR_MLDB, 0, 3, 0.006f),
+        Ptr<DescriptorExtractor> descriptor = AKAZE::create(AKAZE::DESCRIPTOR_MLDB,0, 3, 0.006f),
         Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-L1"));
 
     /* @brief load images
@@ -189,6 +189,7 @@ private:
     int _nCamera;
     int _nMiniMatches;
     int _flags;
+	int _verbose;
     double _error;
     float _patternWidth, _patternHeight;
     TermCriteria _criteria;

modules/ccalib/include/opencv2/ccalib/omnidir.hpp

@@ -251,10 +251,11 @@ namespace internal
         const Size& size1, const Size& size2, OutputArray om, OutputArray T, OutputArrayOfArrays omL, OutputArrayOfArrays tL, OutputArray K1, OutputArray D1, OutputArray K2, OutputArray D2,
         double &xi1, double &xi2, int flags, OutputArray idx);
 
-    void computeJacobian(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints, InputArray parameters, Mat& JTJ_inv, Mat& JTE, int flags);
+    void computeJacobian(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints, InputArray parameters, Mat& JTJ_inv, Mat& JTE, int flags,
+							double epsilon);
 
     void computeJacobianStereo(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints1, InputArrayOfArrays imagePoints2,
-        InputArray parameters, Mat& JTJ_inv, Mat& JTE, int flags);
+        InputArray parameters, Mat& JTJ_inv, Mat& JTE, int flags, double epsilon);
 
     void encodeParameters(InputArray K, InputArrayOfArrays omAll, InputArrayOfArrays tAll, InputArray distoaration, double xi, OutputArray parameters);
 

modules/ccalib/include/opencv2/ccalib/randomPatten.hpp

@@ -79,9 +79,9 @@ public:
     @matcher feature matcher.
     */
     randomPatternCornerFinder(float patternWidth, float patternHeight,
-        int nminiMatch = 20, int depth = CV_32F, int showExtraction = 0,
-        Ptr<FeatureDetector> detector = AKAZE::create(AKAZE::DESCRIPTOR_MLDB, 0, 3, 0.002f),
-        Ptr<DescriptorExtractor> descriptor = AKAZE::create(AKAZE::DESCRIPTOR_MLDB,0, 3, 0.002f),
+        int nminiMatch = 20, int depth = CV_32F, int verbose = 0, int showExtraction = 0,
+        Ptr<FeatureDetector> detector = AKAZE::create(AKAZE::DESCRIPTOR_MLDB, 0, 3, 0.005f),
+        Ptr<DescriptorExtractor> descriptor = AKAZE::create(AKAZE::DESCRIPTOR_MLDB,0, 3, 0.005f),
         Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-L1"));
 
     /* @brief Load pattern image and compute features for pattern
@@ -122,6 +122,7 @@ private:
     cv::Size _patternImageSize;
     int _nminiMatch;
     int _depth;
+	int _verbose;
 
     Ptr<FeatureDetector> _detector;
     Ptr<DescriptorExtractor> _descriptor;
@@ -141,7 +142,7 @@ private:
         std::vector<DMatch>& filteredMatches12, int knn=1 );
     void drawCorrespondence(const Mat& image1, const std::vector<cv::KeyPoint> keypoint1,
         const Mat& image2, const std::vector<cv::KeyPoint> keypoint2, const std::vector<cv::DMatch> matchces,
-        const Mat& mask1, const Mat& mask2);
+        const Mat& mask1, const Mat& mask2, const int step);
 };
 
 /* @brief Class to generate "random" pattern image that are used for randomPatternCornerFinder

modules/ccalib/sample/multiCamCalib.cpp

@@ -7,7 +7,7 @@ using namespace cv;
 
 const char * usage =
 "\n example command line for multi-camera calibration by using random pattern \n"
-"   multiCamCalib -nc 5 -pw 800 -ph 600 -ct 1 -fe 0 -nm 20 multi_camera_omnidir.xml \n"
+"   multiCamCalib -nc 5 -pw 800 -ph 600 -ct 1 -fe 0 -nm 25 -v 0 multi_camera_omnidir.xml \n"
 "\n"
 " the file multi_camera_omnidir.xml is generated by imagelist_creator as \n"
 " imagelist_creator multi_camera_omnidir.xml *.* \n"
@@ -26,6 +26,7 @@ static void help()
            "    -ct <camera_type> # camera type, 0 for pinhole and 1 for omnidirectional \n"
            "    -fe # whether show feature extraction\n"
            "    -nm # number of minimal matches of an image \n"
+		   "	-v # whether show verbose information \n"
            "    input_data # text file with pattern file names and a list of photo names, the file is generated by imagelist_creator \n");
     printf("\n %s", usage);
 }
@@ -37,7 +38,7 @@ int main(int argc, char** argv)
     int nCamera, nMiniMatches, cameraType;
     const char* outputFilename = "multi-camera-results.xml";
     const char* inputFilename = 0;
-    int showFeatureExtraction;
+    int showFeatureExtraction, verbose;
     if (argc < 2)
     {
         help();
@@ -89,6 +90,13 @@ int main(int argc, char** argv)
                 return fprintf(stderr, "Invalid number of minimal matches \n"), -1;
             }
         }
+		else if ( strcmp( s, "-v" ) == 0 )
+		{
+			if (sscanf( argv[++i], "%u", &verbose) != 1 || (verbose !=1 && verbose !=0) )
+			{
+				return fprintf(stderr, "verbose is not bool value, set to 0 or 1 \n"), -1;
+			}
+		}
         else if( s[0] != '-')
         {
             inputFilename = s;
@@ -100,12 +108,13 @@ int main(int argc, char** argv)
     }
 
     // do multi-camera calibration
-    multiCameraCalibration multiCalib(cameraType, nCamera, inputFilename, patternWidth, patternHeight, showFeatureExtraction, nMiniMatches);
+    multiCameraCalibration multiCalib(cameraType, nCamera, inputFilename, patternWidth, patternHeight, verbose, showFeatureExtraction, nMiniMatches);
 
     multiCalib.loadImages();
     multiCalib.initialize();
     multiCalib.optimizeExtrinsics();
     // the above three lines can be replaced by multiCalib.run();
 
-    multiCalib.writeParameters(outputFilename);
+
+	multiCalib.writeParameters(outputFilename);
 }
\ No newline at end of file

modules/ccalib/src/multiCameraCalibration.cpp

@@ -59,6 +59,7 @@
 
 #include "precomp.hpp"
 #include "opencv2/ccalib/multiCameraCalibration.hpp"
+#include "opencv2/core.hpp"
 #include <string>
 #include <vector>
 #include <queue>
@@ -66,7 +67,7 @@
 using namespace cv;
 
 multiCameraCalibration::multiCameraCalibration(int cameraType, int nCameras, const std::string& fileName,
-    float patternWidth, float patternHeight, int showExtration, int nMiniMatches, int flags, TermCriteria criteria,
+    float patternWidth, float patternHeight, int verbose, int showExtration, int nMiniMatches, int flags, TermCriteria criteria,
     Ptr<FeatureDetector> detector, Ptr<DescriptorExtractor> descriptor,
     Ptr<DescriptorMatcher> matcher)
 {
@@ -89,7 +90,7 @@ multiCameraCalibration::multiCameraCalibration(int cameraType, int nCameras, con
     _detector = detector;
     _descriptor = descriptor;
     _matcher = matcher;
-
+	_verbose = verbose;
     for (int i = 0; i < _nCamera; ++i)
     {
         _vertexList.push_back(vertex());
@@ -133,7 +134,7 @@ void multiCameraCalibration::loadImages()
     Ptr<DescriptorExtractor> descriptor = _descriptor;
     Ptr<DescriptorMatcher> matcher = _matcher;
 
-    randomPatternCornerFinder finder(_patternWidth, _patternHeight, 10, CV_32F, this->_showExtraction, detector, descriptor, matcher);
+    randomPatternCornerFinder finder(_patternWidth, _patternHeight, _nMiniMatches, CV_32F, _verbose, this->_showExtraction, detector, descriptor, matcher);
     Mat pattern = cv::imread(file_list[0]);
     finder.loadPattern(pattern);
 
@@ -170,6 +171,14 @@ void multiCameraCalibration::loadImages()
         for (int imgIdx = 0; imgIdx < (int)filesEachCameraFull[camera].size(); ++imgIdx)
         {
             image = imread(filesEachCameraFull[camera][imgIdx], IMREAD_GRAYSCALE);
+			if (!image.empty() && _verbose)
+			{
+				std::cout << "open image " << filesEachCameraFull[camera][imgIdx] << " successfully" << std::endl;
+			}
+			else if (image.empty() && _verbose)
+			{
+				std::cout << "open image" << filesEachCameraFull[camera][imgIdx] << " failed" << std::endl;
+			}
             std::vector<Mat> imgObj = finder.computeObjectImagePointsForSingle(image);
 			if ((int)imgObj[0].total() > _nMiniMatches)
 			{
@@ -177,6 +186,10 @@ void multiCameraCalibration::loadImages()
 				_objectPointsForEachCamera[camera].push_back(imgObj[1]);
 				timestampAvailable[camera].push_back(timestampFull[camera][imgIdx]);
 			}
+			else if ((int)imgObj[0].total() <= _nMiniMatches && _verbose)
+			{
+				std::cout << "image " << filesEachCameraFull[camera][imgIdx] <<" has too few matched points "<< std::endl;
+			}
         }
 
         // calibrate
@@ -246,6 +259,9 @@ void multiCameraCalibration::loadImages()
 			this->_edgeList.push_back(edge(cameraVertex, photoVertex, idx.at<int>(i), transform));
         }
 		std::cout << "initialized for camera " << camera << " rms = " << rms << std::endl;
+		std::cout << "initialized camera matrix for camera " << camera << " is" << std::endl;
+		std::cout << _cameraMatrix[camera] << std::endl;
+		std::cout << "xi for camera " << camera << " is " << _xi[camera] << std::endl;
     }
 
 }
@@ -310,6 +326,11 @@ void multiCameraCalibration::initialize()
         {
             this->_vertexList[vertexIdx].pose = transform * prePose.inv();
             this->_vertexList[vertexIdx].pose.convertTo(this->_vertexList[vertexIdx].pose, CV_32F);
+			if (_verbose)
+			{
+				std::cout << "initial pose for camera " << vertexIdx << " is " << std::endl;
+				std::cout << this->_vertexList[vertexIdx].pose << std::endl;
+			}
         }
         else
         {
@@ -375,6 +396,11 @@ double multiCameraCalibration::optimizeExtrinsics()
         R.copyTo(pose.colRange(0, 3).rowRange(0, 3));
         Mat(tvecVertex[verIdx-1]).reshape(1, 3).copyTo(pose.rowRange(0, 3).col(3));
         _vertexList[verIdx].pose = pose;
+		if (_verbose && verIdx < _nCamera)
+		{
+			std::cout << "final camera pose of camera " << verIdx << " is" << std::endl;
+			std::cout << pose << std::endl;
+		}
     }
     return error;
 }

modules/ccalib/src/omnidir.cpp

@@ -843,7 +843,7 @@ void cv::omnidir::internal::initializeStereoCalibration(InputArrayOfArrays objec
 /// cv::omnidir::internal::computeJacobian
 
 void cv::omnidir::internal::computeJacobian(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints,
-    InputArray parameters, Mat& JTJ_inv, Mat& JTE, int flags)
+    InputArray parameters, Mat& JTJ_inv, Mat& JTE, int flags, double epsilon)
 {
     CV_Assert(!objectPoints.empty() && objectPoints.type() == CV_64FC3);
     CV_Assert(!imagePoints.empty() && imagePoints.type() == CV_64FC2);
@@ -915,12 +915,21 @@ void cv::omnidir::internal::computeJacobian(InputArrayOfArrays objectPoints, Inp
     subMatrix(JTJ, JTJ, _idx, _idx);
     subMatrix(JTE, JTE, std::vector<int>(1, 1), _idx);
     // in case JTJ is singular
-    double epsilon = 1e-10;
+	//SVD svd(JTJ, SVD::NO_UV);
+	//double cond = svd.w.at<double>(0)/svd.w.at<double>(5);
+
+	//if (cond_JTJ.needed())
+	//{
+	//	cond_JTJ.create(1, 1, CV_64F);
+	//	cond_JTJ.getMat().at<double>(0) = cond;
+	//}
+
+    //double epsilon = 1e-4*std::exp(cond);
     JTJ_inv = Mat(JTJ+epsilon).inv();
 }
 
 void cv::omnidir::internal::computeJacobianStereo(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints1, InputArrayOfArrays imagePoints2,
-    InputArray parameters, Mat& JTJ_inv, Mat& JTE, int flags)
+    InputArray parameters, Mat& JTJ_inv, Mat& JTE, int flags, double epsilon)
 {
     CV_Assert(!objectPoints.empty() && objectPoints.type() == CV_64FC3);
     CV_Assert(!imagePoints1.empty() && imagePoints1.type() == CV_64FC2);
@@ -1000,7 +1009,7 @@ void cv::omnidir::internal::computeJacobianStereo(InputArrayOfArrays objectPoint
     JTE = J.t()*exAll;
     subMatrix(JTJ, JTJ, _idx, _idx);
     subMatrix(JTE, JTE, std::vector<int>(1, 1), _idx);
-    double epsilon = 1e-10;
+    
     JTJ_inv = Mat(JTJ+epsilon).inv();
 }
 
@@ -1168,7 +1177,7 @@ double cv::omnidir::calibrate(InputArray patternPoints, InputArray imagePoints,
     cv::omnidir::internal::encodeParameters(_K, _omAll, _tAll, Mat::zeros(1,4,CV_64F), _xi, currentParam);
 
     // optimization
-    const double alpha_smooth = 0.001;
+    const double alpha_smooth = 0.01;
     //const double thresh_cond = 1e6;
     double change = 1;
     for(int iter = 0; ; ++iter)
@@ -1179,8 +1188,8 @@ double cv::omnidir::calibrate(InputArray patternPoints, InputArray imagePoints,
             break;
         double alpha_smooth2 = 1 - std::pow(1 - alpha_smooth, (double)iter + 1.0);
         Mat JTJ_inv, JTError;
-
-        cv::omnidir::internal::computeJacobian(_patternPoints, _imagePoints, currentParam, JTJ_inv, JTError, flags);
+		double epsilon = 0.01 * std::pow(0.9, (double)iter/10);
+        cv::omnidir::internal::computeJacobian(_patternPoints, _imagePoints, currentParam, JTJ_inv, JTError, flags, epsilon);
 
         // GaussCNewton
         Mat G = alpha_smooth2*JTJ_inv * JTError;
@@ -1330,8 +1339,10 @@ double cv::omnidir::stereoCalibrate(InputOutputArrayOfArrays objectPoints, Input
             break;
         double alpha_smooth2 = 1 - std::pow(1 - alpha_smooth, (double)iter + 1.0);
         Mat JTJ_inv, JTError;
+		double epsilon = 0.01 * std::pow(0.9, (double)iter/10);
 
-        cv::omnidir::internal::computeJacobianStereo(_objectPointsFilt, _imagePoints1Filt, _imagePoints2Filt, currentParam, JTJ_inv, JTError, flags);
+        cv::omnidir::internal::computeJacobianStereo(_objectPointsFilt, _imagePoints1Filt, _imagePoints2Filt, currentParam,
+			JTJ_inv, JTError, flags, epsilon);
 
         // GaussCNewton
         Mat G = alpha_smooth2*JTJ_inv * JTError;
@@ -1835,7 +1846,7 @@ void cv::omnidir::internal::estimateUncertainties(InputArrayOfArrays objectPoint
     double s = sigma_x.at<double>(0);
 
     Mat _JTJ_inv, _JTE;
-    computeJacobian(objectPoints, imagePoints, parameters, _JTJ_inv, _JTE, flags);
+    computeJacobian(objectPoints, imagePoints, parameters, _JTJ_inv, _JTE, flags, 0.0);
     sqrt(_JTJ_inv, _JTJ_inv);
 
     errors = 3 * s * _JTJ_inv.diag();
@@ -1923,7 +1934,7 @@ void cv::omnidir::internal::estimateUncertaintiesStereo(InputArrayOfArrays objec
     double s = sigma_x.at<double>(0);
 
     Mat _JTJ_inv, _JTE;
-    computeJacobianStereo(objectPoints, imagePoints1, imagePoints2, _parameters, _JTJ_inv, _JTE, flags);
+    computeJacobianStereo(objectPoints, imagePoints1, imagePoints2, _parameters, _JTJ_inv, _JTE, flags, 0.0);
     cv::sqrt(_JTJ_inv, _JTJ_inv);
 
     errors = 3 * s * _JTJ_inv.diag();

modules/ccalib/src/randomPatten.cpp

@@ -58,7 +58,7 @@
 using namespace cv;
 using namespace std;
 randomPatternCornerFinder::randomPatternCornerFinder(float patternWidth, float patternHeight,
-    int nminiMatch, int depth, int showExtraction, Ptr<FeatureDetector> detector, Ptr<DescriptorExtractor> descriptor,
+    int nminiMatch, int depth, int verbose, int showExtraction, Ptr<FeatureDetector> detector, Ptr<DescriptorExtractor> descriptor,
     Ptr<DescriptorMatcher> matcher)
 {
     _patternHeight = patternHeight;
@@ -71,6 +71,7 @@ randomPatternCornerFinder::randomPatternCornerFinder(float patternWidth, float p
     _descriptor = descriptor;
     _matcher = matcher;
     _showExtraction = showExtraction;
+	_verbose = verbose;
 }
 
 //void randomPatternCornerFinder::computeObjectImagePoints2(std::vector<cv::Mat> inputImages)
@@ -270,26 +271,26 @@ void randomPatternCornerFinder::crossCheckMatching( Ptr<DescriptorMatcher>& desc
 
 void randomPatternCornerFinder::drawCorrespondence(const Mat& image1, const std::vector<cv::KeyPoint> keypoint1,
     const Mat& image2, const std::vector<cv::KeyPoint> keypoint2, const std::vector<cv::DMatch> matchces,
-    const Mat& mask1, const Mat& mask2)
+    const Mat& mask1, const Mat& mask2, const int step)
 {
     Mat img_corr;
-    if(mask1.empty())
+    if(step == 1)
     {
         drawMatches(image1, keypoint1, image2, keypoint2, matchces, img_corr);
     }
-    else if(!mask1.empty() && mask2.empty())
+    else if(step == 2)
     {
         std::vector<cv::DMatch> matchesFilter;
         for (int i = 0; i < (int)mask1.total(); ++i)
         {
-            if (mask1.at<uchar>(i) == 1)
+            if (!mask1.empty() && mask1.at<uchar>(i) == 1)
             {
                 matchesFilter.push_back(matchces[i]);
             }
         }
         drawMatches(image1, keypoint1, image2, keypoint2, matchesFilter, img_corr);
     }
-    else if(!mask1.empty() && !mask2.empty())
+    else if(step == 3)
     {
         std::vector<cv::DMatch> matchesFilter;
         int j = 0;
@@ -297,7 +298,7 @@ void randomPatternCornerFinder::drawCorrespondence(const Mat& image1, const std:
         {
             if (mask1.at<uchar>(i) == 1)
             {
-                if (mask2.at<uchar>(j) == 1)
+                if (!mask2.empty() && mask2.at<uchar>(j) == 1)
                 {
                     matchesFilter.push_back(matchces[i]);
                 }
@@ -357,8 +358,8 @@ std::vector<cv::Mat> randomPatternCornerFinder::computeObjectImagePointsForSingl
 
     cv::Mat keypointsImageLocation, keypointsPatternLocation;
 
-    crossCheckMatching(this->_matcher, descriptorImage1, this->_descriptorPattern, matchesImgtoPat1, 5);
-    crossCheckMatching(this->_matcher, descriptorImage2, this->_descriptorPattern, matchesImgtoPat2, 5);
+    crossCheckMatching(this->_matcher, descriptorImage1, this->_descriptorPattern, matchesImgtoPat1, 1);
+    crossCheckMatching(this->_matcher, descriptorImage2, this->_descriptorPattern, matchesImgtoPat2, 1);
     if ((int)matchesImgtoPat1.size() > (int)matchesImgtoPat2.size())
     {
         matchesImgtoPat = matchesImgtoPat1;
@@ -382,23 +383,37 @@ std::vector<cv::Mat> randomPatternCornerFinder::computeObjectImagePointsForSingl
     if(this->_showExtraction)
     {
         drawCorrespondence(inputImage, keypointsImage, _patternImage, _keypointsPattern, matchesImgtoPat,
-            innerMask1, innerMask2);
+            innerMask1, innerMask2, 1);
     }
 
-
+	if (_verbose)
+	{
+		std::cout << "number of matched points " << (int)keypointsImageLocation.total() << std::endl;
+	}
     // outlier remove
     findFundamentalMat(keypointsImageLocation, keypointsPatternLocation,
         FM_RANSAC, 1, 0.995, innerMask1);
     getFilteredLocation(keypointsImageLocation, keypointsPatternLocation, innerMask1);
 
-    findHomography(keypointsImageLocation, keypointsPatternLocation, RANSAC, 10*inputImage.cols/1000, innerMask2);
+	if (this->_showExtraction)
+	{
+		drawCorrespondence(inputImage, keypointsImage, _patternImage, _keypointsPattern, matchesImgtoPat,
+			innerMask1, innerMask2, 2);
+	}
+
+    findHomography(keypointsImageLocation, keypointsPatternLocation, RANSAC, 30*inputImage.cols/1000, innerMask2);
     getFilteredLocation(keypointsImageLocation, keypointsPatternLocation, innerMask2);
 
+	if (_verbose)
+	{
+		std::cout << "number of filtered points " << (int)keypointsImageLocation.total() << std::endl;
+	}
+
     // draw filtered correspondence
     if (this->_showExtraction)
     {
         drawCorrespondence(inputImage, keypointsImage, _patternImage, _keypointsPattern, matchesImgtoPat,
-            innerMask1, innerMask2);
+            innerMask1, innerMask2, 3);
     }
 
     std::vector<Vec3d> objectPoints;