changed CV camera calibration undistorter to 4-parameter format.

fixed bug when changing image-resolution with callibratino file

README.md

@@ -165,7 +165,7 @@ This one is with no radial distortion, as a special case of ATAN camera model bu
 
 #### Calibration File for OpenCV camera model [untested!]:
 
-    fx fy cx cy d1 d2 d3 d4 d5 d6
+    fx fy cx cy d1 d2 d3 d4
     inputWidth inputHeight
     "crop" / "full" / "none" / "e1 e2 e3 e4 0"
     outputWidth outputHeight

lsd_slam_core/src/main_on_images.cpp

@@ -157,6 +157,10 @@ int main( int argc, char** argv )
 
 	int w = undistorter->getOutputWidth();
 	int h = undistorter->getOutputHeight();
+
+	int w_inp = undistorter->getInputWidth();
+	int h_inp = undistorter->getInputHeight();
+
 	float fx = undistorter->getK().at<double>(0, 0);
 	float fy = undistorter->getK().at<double>(1, 1);
 	float cx = undistorter->getK().at<double>(2, 0);
@@ -219,7 +223,7 @@ int main( int argc, char** argv )
 	{
 		cv::Mat imageDist = cv::imread(files[i], CV_LOAD_IMAGE_GRAYSCALE);
 
-		if(imageDist.rows != h || imageDist.cols != w)
+		if(imageDist.rows != h_inp || imageDist.cols != w_inp)
 		{
 			if(imageDist.rows * imageDist.cols == 0)
 				printf("failed to load image %s! skipping.\n", files[i].c_str());
@@ -258,6 +262,9 @@ int main( int argc, char** argv )
 		}
 
 		ros::spinOnce();
+
+		if(!ros::ok())
+			break;
 	}
 
 

lsd_slam_core/src/util/Undistorter.cpp

@@ -68,9 +68,9 @@ Undistorter* Undistorter::getUndistorterForFile(const char* configFilename)
 
 
 	float ic[10];
-	if(std::sscanf(l1.c_str(), "%f %f %f %f %f %f %f %f %f %f",
+	if(std::sscanf(l1.c_str(), "%f %f %f %f %f %f %f %f",
 			&ic[0], &ic[1], &ic[2], &ic[3], &ic[4],
-			&ic[5], &ic[6], &ic[7], &ic[8], &ic[9]) == 10)
+			&ic[5], &ic[6], &ic[7]) == 8)
 	{
 		printf("found OpenCV camera model, building rectifier.\n");
 		Undistorter* u = new UndistorterOpenCV(completeFileName.c_str());
@@ -428,6 +428,16 @@ int UndistorterPTAM::getOutputHeight() const
 {
 	return out_height;
 }
+int UndistorterPTAM::getInputWidth() const
+{
+	return in_width;
+}
+
+int UndistorterPTAM::getInputHeight() const
+{
+	return in_height;
+}
+
 
 bool UndistorterPTAM::isValid() const
 {
@@ -451,16 +461,16 @@ UndistorterOpenCV::UndistorterOpenCV(const char* configFileName)
 	std::getline(infile,l4);
 
 	// l1 & l2
-	if(std::sscanf(l1.c_str(), "%f %f %f %f %f %f %f %f %f %f",
+	if(std::sscanf(l1.c_str(), "%f %f %f %f %f %f %f %f",
 		&inputCalibration[0], &inputCalibration[1], &inputCalibration[2], &inputCalibration[3], &inputCalibration[4],
-		&inputCalibration[5], &inputCalibration[6], &inputCalibration[7], &inputCalibration[8], &inputCalibration[9]
-  				) == 10 &&
+		&inputCalibration[5], &inputCalibration[6], &inputCalibration[7]
+  				) == 8 &&
 			std::sscanf(l2.c_str(), "%d %d", &in_width, &in_height) == 2)
 	{
 		printf("Input resolution: %d %d\n",in_width, in_height);
-		printf("In: %f %f %f %f %f %f %f %f %f %f\n",
+		printf("In: %f %f %f %f %f %f %f %f\n",
 				inputCalibration[0], inputCalibration[1], inputCalibration[2], inputCalibration[3], inputCalibration[4],
-				inputCalibration[5], inputCalibration[6], inputCalibration[7], inputCalibration[8], inputCalibration[9]);
+				inputCalibration[5], inputCalibration[6], inputCalibration[7]);
 	}
 	else
 	{
@@ -501,12 +511,10 @@ UndistorterOpenCV::UndistorterOpenCV(const char* configFileName)
 		valid = false;
 	}
 
-	cv::Mat distCoeffs = cv::Mat::zeros(8, 1, CV_32F);
-	for (int i = 0; i < 2; ++ i)
-		distCoeffs.at<float>(i, 0) = inputCalibration[4 + i];
+	cv::Mat distCoeffs = cv::Mat::zeros(4, 1, CV_32F);
 	for (int i = 0; i < 4; ++ i)
-		distCoeffs.at<float>(4 + i, 0) = inputCalibration[6 + i];
-	
+		distCoeffs.at<float>(i, 0) = inputCalibration[4 + i];
+
 	originalK_ = cv::Mat(3, 3, CV_64F, cv::Scalar(0));
 	originalK_.at<double>(0, 0) = inputCalibration[0] * in_width;
 	originalK_.at<double>(1, 1) = inputCalibration[1] * in_height;
@@ -521,14 +529,6 @@ UndistorterOpenCV::UndistorterOpenCV(const char* configFileName)
 		cv::initUndistortRectifyMap(originalK_, distCoeffs, cv::Mat(), K_,
 				cv::Size(out_width, out_height), CV_16SC2, map1, map2);
 
-// 		K_.at<double>(0, 0) /= out_width;
-// 		K_.at<double>(0, 2) /= out_width;
-// 		K_.at<double>(1, 1) /= out_height;
-// 		K_.at<double>(1, 2) /= out_height;
-		// TODO: PTAM code uses the following, should here also 0.5 be subtracted?
-// 		K_.at<double>(2, 0) = outputCalibration[2] * out_width - 0.5;
-// 		K_.at<double>(2, 1) = outputCalibration[3] * out_height - 0.5;
-
 		originalK_.at<double>(0, 0) /= in_width;
 		originalK_.at<double>(0, 2) /= in_width;
 		originalK_.at<double>(1, 1) /= in_height;
@@ -567,6 +567,15 @@ int UndistorterOpenCV::getOutputHeight() const
 {
 	return out_height;
 }
+int UndistorterOpenCV::getInputWidth() const
+{
+	return in_width;
+}
+
+int UndistorterOpenCV::getInputHeight() const
+{
+	return in_height;
+}
 
 bool UndistorterOpenCV::isValid() const
 {

lsd_slam_core/src/util/Undistorter.h

@@ -58,6 +58,17 @@ class Undistorter
 	 */
 	virtual int getOutputHeight() const = 0;
 
+	/**
+	 * Returns the width of the input images in pixels.
+	 */
+	virtual int getInputWidth() const = 0;
+
+	/**
+	 * Returns the height of the input images in pixels.
+	 */
+	virtual int getInputHeight() const = 0;
+
+
 	/**
 	 * Returns if the undistorter was initialized successfully.
 	 */
@@ -117,6 +128,17 @@ class UndistorterPTAM : public Undistorter
 	 */
 	int getOutputHeight() const;
 
+	/**
+	 * Returns the width of the input images in pixels.
+	 */
+	int getInputWidth() const;
+
+	/**
+	 * Returns the height of the input images in pixels.
+	 */
+	int getInputHeight() const;
+
+
 	/**
 	 * Returns if the undistorter was initialized successfully.
 	 */
@@ -186,6 +208,17 @@ class UndistorterOpenCV : public Undistorter
 	 */
 	int getOutputHeight() const;
 
+
+	/**
+	 * Returns the width of the input images in pixels.
+	 */
+	int getInputWidth() const;
+
+	/**
+	 * Returns the height of the input images in pixels.
+	 */
+	int getInputHeight() const;
+
 	/**
 	 * Returns if the undistorter was initialized successfully.
 	 */