Calibrate fisheye lens - ofxCv

@abocanegra wrote:

I have successfully integrated the ofxCv libraries calibrate functionality into my project (a threaded multi-camera system for Point Grey GigE Cameras) in Ubuntu 15.04 x64 with OF 0.8.4.

The problem is the camera is a 1/1.2 sensor with a 2.7mm lens. The calibration function does not appear to work with Fisheye lenses directly. I have done quite a bit of research, but admittedly I am new to working with calibration in OpenCv. It appears that I need to set flags to something like:

	int calibFlags = CV_CALIB_FIX_K1 | CV_CALIB_FIX_K2 | CV_CALIB_FIX_K3 | CV_CALIB_FIX_K4 | CV_CALIB_FIX_K5 | CV_CALIB_FIX_K6 | CV_CALIB_FIX_FOCAL_LENGTH | CV_CALIB_USE_INTRINSIC_GUESS;

and apply this to calibrateCamera();

I am adapting the ofxCv code directly, I have backed up the original and when I get it down my plan is to make another header and c++ file to extend it's functionality.

Even with the flags I cannot get the fisheye distortion to clean up like I can lenses in the normal range, it is also apparently unable to automatically calculate the sensor size or the focal length, I could also use help on how to set that manually since I know the numbers.

Essentially, any help I can get in adapting the ofxCv calibration to function with Fisheye lenses would be great, and how to manually handle sensorWidth/sensorHeight, and focalLength.

Also, regarding the settings.yml the xcount and ycount, does the counting start from 0 or 1. my actual x=11 and y=8, but it only works if I list as 10,7.

Here is the calibration.h file I am using in my project to reference the ofxCv calibration:

#include "ofApp.h"

using namespace ofxCv;
using namespace cv;

class GigECalibration{

    public:
        GigECalibration(){
            active = true;
        }

        virtual ~GigECalibration(){
            //calibration.reset();
        }
        void setup(ofImage cam) {
            camImage = cam;
            calibration.setFillFrame(false);
            FileStorage settings(ofToDataPath("settings.yml"), FileStorage::READ);
            if(settings.isOpened()) {
                int xCount = settings["xCount"], yCount = settings["yCount"];
                calibration.setPatternSize(xCount, yCount);
                float squareSize = settings["squareSize"];
                calibration.setSquareSize(squareSize);
                CalibrationPattern patternType;
                switch(settings["patternType"]) {
                    case 0: patternType = CHESSBOARD; break;
                    case 1: patternType = CIRCLES_GRID; break;
                    case 2: patternType = ASYMMETRIC_CIRCLES_GRID; break;
                }
                calibration.setPatternType(patternType);
            }

            imitate(undistorted, camImage);
            imitate(previous, camImage);
            imitate(diff, camImage);

            lastTime = 0;

            //active = true;
        }

            void update(ofImage cam) {
                camImage = cam;
                Mat camMat = toCv(camImage);
                Mat prevMat = toCv(previous);
                Mat diffMat = toCv(diff);

                absdiff(prevMat, camMat, diffMat);
                camMat.copyTo(prevMat);

                diffMean = mean(Mat(mean(diffMat)))[0];

                float curTime = ofGetElapsedTimef();
                if(active && curTime - lastTime > timeThreshold && diffMean < diffThreshold) {
                    if(calibration.add(camMat)) {
                        cout << "re-calibrating" << endl;
                        calibration.calibrate();
                        if(calibration.size() > startCleaning) {
                            calibration.clean();
                        }
                        calibration.save("calibration.yml");
                        lastTime = curTime;
                    }
                }

                if(calibration.size() > 0) {
                    calibration.undistort(toCv(camImage), toCv(undistorted));
                    undistorted.update();
                }

        }

        void draw(int camW, int camH) {
            ofSetColor(255);
            camImage.draw(0, 0,camW/2, camH/2);
            undistorted.draw(camW/2, 0, camW/2,camH/2);

            if(active){
                ofDrawBitmapString("Calibration Active",ofGetWidth()-200,20);
            }else{
                ofDrawBitmapString("Calibration Inactive",ofGetWidth()-200,20);
            }
            stringstream intrinsics;
            intrinsics << "fov: " << toOf(calibration.getDistortedIntrinsics().getFov()) << " distCoeffs: " << calibration.getDistCoeffs();
            drawHighlightString(intrinsics.str(), 10, 20, yellowPrint, ofColor(0));
            drawHighlightString("movement: " + ofToString(diffMean), 10, 40, cyanPrint);
            drawHighlightString("reproj error: " + ofToString(calibration.getReprojectionError()) + " from " + ofToString(calibration.size()), 10, 60, magentaPrint);
            for(int i = 0; i < calibration.size(); i++) {
                drawHighlightString(ofToString(i) + ": " + ofToString(calibration.getReprojectionError(i)), 10, 80 + 16 * i, magentaPrint);
            }
        }


        bool active;
    protected:
        const float diffThreshold = 2.5; // maximum amount of movement
        const float timeThreshold = 1; // minimum time between snapshots
        const int startCleaning = 20; // start cleaning outliers after this many samples

        ofImage camImage;
        ofImage undistorted;
        ofPixels previous;
        ofPixels diff;
        float diffMean;

        float lastTime;

        ofxCv::Calibration calibration;


};

Here is the slightly adapted ofxCv/Calibration.cpp

#include "ofxCv/Calibration.h"
#include "ofxCv/Helpers.h"
#include "ofFileUtils.h"

namespace ofxCv {

	using namespace cv;

	void Intrinsics::setup(Mat cameraMatrix, cv::Size imageSize, cv::Size sensorSize) {
		this->cameraMatrix = cameraMatrix;
		this->imageSize = imageSize;
		this->sensorSize = sensorSize;
		calibrationMatrixValues(cameraMatrix, imageSize, sensorSize.width, sensorSize.height,
														fov.x, fov.y, focalLength, principalPoint, aspectRatio);

	}

	void Intrinsics::setImageSize(cv::Size imgSize) {
		imageSize = imgSize;
	}

	Mat Intrinsics::getCameraMatrix() const {
		return cameraMatrix;
	}

	cv::Size Intrinsics::getImageSize() const {
		return imageSize;
	}

	cv::Size Intrinsics::getSensorSize() const {
		return sensorSize;
	}

	cv::Point2d Intrinsics::getFov() const {
		return fov;
	}

	double Intrinsics::getFocalLength() const {
		return focalLength;
	}

	double Intrinsics::getAspectRatio() const {
		return aspectRatio;
	}

	Point2d Intrinsics::getPrincipalPoint() const {
		return principalPoint;
	}

	void Intrinsics::loadProjectionMatrix(float nearDist, float farDist, cv::Point2d viewportOffset) const {
		ofViewport(viewportOffset.x, viewportOffset.y, imageSize.width, imageSize.height);
		ofSetMatrixMode(OF_MATRIX_PROJECTION);
		ofLoadIdentityMatrix();
		float w = imageSize.width;
		float h = imageSize.height;
		float fx = cameraMatrix.at<double>(0, 0);
		float fy = cameraMatrix.at<double>(1, 1);
		float cx = principalPoint.x;
		float cy = principalPoint.y;

		ofMatrix4x4 frustum;
		frustum.makeFrustumMatrix(
			nearDist * (-cx) / fx, nearDist * (w - cx) / fx,
			nearDist * (cy) / fy, nearDist * (cy - h) / fy,
			nearDist, farDist);
		ofMultMatrix(frustum);

		ofSetMatrixMode(OF_MATRIX_MODELVIEW);
		ofLoadIdentityMatrix();

		ofMatrix4x4 lookAt;
		lookAt.makeLookAtViewMatrix(ofVec3f(0,0,0), ofVec3f(0,0,1), ofVec3f(0,-1,0));
		ofMultMatrix(lookAt);
	}

	Calibration::Calibration() :
		patternType(CHESSBOARD),
		patternSize(cv::Size(10, 7)), // based on Chessboard_A4.pdf, assuming world units are centimeters
		subpixelSize(cv::Size(11,11)),
		squareSize(2.5),
		reprojectionError(0),
		fillFrame(true),
		ready(false) {

	}

	void Calibration::save(string filename, bool absolute) const {
		if(!ready){
			ofLog(OF_LOG_ERROR, "Calibration::save() failed, because your calibration isn't ready yet!");
		}
		FileStorage fs(ofToDataPath(filename, absolute), FileStorage::WRITE);
		cv::Size imageSize = distortedIntrinsics.getImageSize();
		cv::Size sensorSize = distortedIntrinsics.getSensorSize();
		Mat cameraMatrix = distortedIntrinsics.getCameraMatrix();
		fs << "cameraMatrix" << cameraMatrix;
		fs << "imageSize_width" << imageSize.width;
		fs << "imageSize_height" << imageSize.height;
		fs << "sensorSize_width" << sensorSize.width;
		fs << "sensorSize_height" << sensorSize.height;
		fs << "distCoeffs" << distCoeffs;
		fs << "reprojectionError" << reprojectionError;
		fs << "features" << "[";
		for(int i = 0; i < (int)imagePoints.size(); i++) {
			fs << "[:" << imagePoints[i] << "]";
		}
		fs << "]";
	}

	void Calibration::load(string filename, bool absolute) {
		imagePoints.clear();
		FileStorage fs(ofToDataPath(filename, absolute), FileStorage::READ);
		cv::Size imageSize, sensorSize;
		Mat cameraMatrix;
		fs["cameraMatrix"] >> cameraMatrix;
		fs["imageSize_width"] >> imageSize.width;
		fs["imageSize_height"] >> imageSize.height;
		fs["sensorSize_width"] >> sensorSize.width;
		fs["sensorSize_height"] >> sensorSize.height;
		fs["distCoeffs"] >> distCoeffs;
		fs["reprojectionError"] >> reprojectionError;
		FileNode features = fs["features"];
		for(FileNodeIterator it = features.begin(); it != features.end(); it++) {
			vector<Point2f> cur;
			(*it) >> cur;
			imagePoints.push_back(cur);
		}
		addedImageSize = imageSize;
		distortedIntrinsics.setup(cameraMatrix, imageSize, sensorSize);
		updateUndistortion();
		ready = true;
	}
	void Calibration::setIntrinsics(Intrinsics& distortedIntrinsics, Mat& distortionCoefficients){
		this->distortedIntrinsics = distortedIntrinsics;
		this->distCoeffs = distortionCoefficients;
		this->addedImageSize = distortedIntrinsics.getImageSize();
		updateUndistortion();
		this->ready = true;
	}
	void Calibration::reset(){
		this->ready = false;
		this->reprojectionError = 0.0;
		this->imagePoints.clear();
		this->objectPoints.clear();
		this->perViewErrors.clear();
	}
	void Calibration::setPatternType(CalibrationPattern patternType) {
		this->patternType = patternType;
	}
	void Calibration::setPatternSize(int xCount, int yCount) {
		patternSize = cv::Size(xCount, yCount);
	}
	void Calibration::setSquareSize(float squareSize) {
		this->squareSize = squareSize;
	}
	void Calibration::setFillFrame(bool fillFrame) {
		this->fillFrame = fillFrame;
	}
	void Calibration::setSubpixelSize(int subpixelSize) {
		subpixelSize = MAX(subpixelSize,2);
		this->subpixelSize = cv::Size(subpixelSize,subpixelSize);
	}
	bool Calibration::add(Mat img) {
		addedImageSize = img.size();

		vector<Point2f> pointBuf;

		// find corners
		bool found = findBoard(img, pointBuf);

		if (found)
			imagePoints.push_back(pointBuf);
		else
			ofLog(OF_LOG_ERROR, "Calibration::add() failed, maybe your patternSize is wrong or the image has poor lighting?");
		return found;
	}
	bool Calibration::findBoard(Mat img, vector<Point2f>& pointBuf, bool refine) {
		bool found=false;
		if(patternType == CHESSBOARD) {
			// no CV_CALIB_CB_FAST_CHECK, because it breaks on dark images (e.g., dark IR images from kinect)
			int chessFlags = CV_CALIB_CB_ADAPTIVE_THRESH;// | CV_CALIB_CB_FILTER_QUADS;// | CV_CALIB_CB_NORMALIZE_IMAGE;
			found = findChessboardCorners(img, patternSize, pointBuf, chessFlags);

			// improve corner accuracy
			if(found) {
				if(img.type() != CV_8UC1) {
                    copyGray(img, grayMat);
				} else {
					grayMat = img;
				}

				if(refine) {
					// the 11x11 dictates the smallest image space square size allowed
					// in other words, if your smallest square is 11x11 pixels, then set this to 11x11
					cornerSubPix(grayMat, pointBuf, subpixelSize,  cv::Size(-1,-1), TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1 ));
				}
			}
		}
#ifdef USING_OPENCV_2_3
		else {
			int flags = (patternType == CIRCLES_GRID ? CALIB_CB_SYMMETRIC_GRID : CALIB_CB_ASYMMETRIC_GRID); // + CALIB_CB_CLUSTERING
			found = findCirclesGrid(img, patternSize, pointBuf, flags);
		}
#endif
		return found;
	}
	bool Calibration::clean(float minReprojectionError) {
		int removed = 0;
		for(int i = size() - 1; i >= 0; i--) {
			if(getReprojectionError(i) > minReprojectionError) {
				objectPoints.erase(objectPoints.begin() + i);
				imagePoints.erase(imagePoints.begin() + i);
				removed++;
			}
		}
		if(size() > 0) {
			if(removed > 0) {
				return calibrate();
			} else {
				return true;
			}
		} else {
			ofLog(OF_LOG_ERROR, "Calibration::clean() removed the last object/image point pair");
			return false;
		}
	}
	bool Calibration::calibrate() {
		if(size() < 1) {
			ofLog(OF_LOG_ERROR, "Calibration::calibrate() doesn't have any image data to calibrate from.");
			if(ready) {
				ofLog(OF_LOG_ERROR, "Calibration::calibrate() doesn't need to be called after Calibration::load().");
			}
			return ready;
		}

		Mat cameraMatrix = Mat::eye(3, 3, CV_64F);
		distCoeffs = Mat::zeros(8, 1, CV_64F);

		updateObjectPoints();


		//int calibFlags = 0;
		int calibFlags = CV_CALIB_FIX_K1 | CV_CALIB_FIX_K2 | CV_CALIB_FIX_K3 | CV_CALIB_FIX_K4 | CV_CALIB_FIX_K5 | CV_CALIB_FIX_K6 | CV_CALIB_FIX_FOCAL_LENGTH | CV_CALIB_USE_INTRINSIC_GUESS;
		//CV_CALIB_FIX_PRINCIPAL_POINT | CV_CALIB_USE_INTRINSIC_GUESS | CV_CALIB_FIX_ASPECT_RATIO
        //int calibFlags = CV_CALIB_FIX_K1 | CV_CALIB_FIX_K2 | CV_CALIB_FIX_K3 | CV_CALIB_FIX_K4 | CV_CALIB_FIX_K5 | CV_CALIB_FIX_K6 | CV_CALIB_ZERO_TANGENT_DIST | CV_CALIB_USE_INTRINSIC_GUESS |CV_CALIB_ZERO_TANGENT_DIST;

		float rms = calibrateCamera(objectPoints, imagePoints, addedImageSize, cameraMatrix, distCoeffs, boardRotations, boardTranslations, calibFlags);
		ofLog(OF_LOG_VERBOSE, "calibrateCamera() reports RMS error of " + ofToString(rms));

		ready = checkRange(cameraMatrix) && checkRange(distCoeffs);

		if(!ready) {
			ofLog(OF_LOG_ERROR, "Calibration::calibrate() failed to calibrate the camera");
		}

		distortedIntrinsics.setup(cameraMatrix, addedImageSize);
		updateReprojectionError();
		updateUndistortion();

		return ready;
	}

	bool Calibration::isReady(){
		return ready;
	}

	bool Calibration::calibrateFromDirectory(string directory) {
		ofDirectory dirList;
		ofImage cur;
		dirList.listDir(directory);
		for(int i = 0; i < (int)dirList.size(); i++) {
			cur.loadImage(dirList.getPath(i));
			if(!add(toCv(cur))) {
				ofLog(OF_LOG_ERROR, "Calibration::add() failed on " + dirList.getPath(i));
			}
		}
		return calibrate();
	}
	void Calibration::undistort(Mat img, int interpolationMode) {
		img.copyTo(undistortBuffer);
		undistort(undistortBuffer, img, interpolationMode);
	}
	void Calibration::undistort(Mat src, Mat dst, int interpolationMode) {
		remap(src, dst, undistortMapX, undistortMapY, interpolationMode);
	}

	ofVec2f Calibration::undistort(ofVec2f& src) const {
		ofVec2f dst;
		Mat matSrc = Mat(1, 1, CV_32FC2, &src.x);
		Mat matDst = Mat(1, 1, CV_32FC2, &dst.x);;
		undistortPoints(matSrc, matDst, distortedIntrinsics.getCameraMatrix(), distCoeffs);
		return dst;
	}

	void Calibration::undistort(vector<ofVec2f>& src, vector<ofVec2f>& dst) const {
		int n = src.size();
		dst.resize(n);
		Mat matSrc = Mat(n, 1, CV_32FC2, &src[0].x);
		Mat matDst = Mat(n, 1, CV_32FC2, &dst[0].x);
		undistortPoints(matSrc, matDst, distortedIntrinsics.getCameraMatrix(), distCoeffs);
	}

	bool Calibration::getTransformation(Calibration& dst, Mat& rotation, Mat& translation) {
		//if(imagePoints.size() == 0 || dst.imagePoints.size() == 0) {
		if(!ready) {
			ofLog(OF_LOG_ERROR, "getTransformation() requires both Calibration objects to have just been calibrated");
			return false;
		}
		if(imagePoints.size() != dst.imagePoints.size() || patternSize != dst.patternSize) {
			ofLog(OF_LOG_ERROR, "getTransformation() requires both Calibration objects to be trained simultaneously on the same board");
			return false;
		}
		Mat fundamentalMatrix, essentialMatrix;
		Mat cameraMatrix = distortedIntrinsics.getCameraMatrix();
		Mat dstCameraMatrix = dst.getDistortedIntrinsics().getCameraMatrix();
		// uses CALIB_FIX_INTRINSIC by default
		stereoCalibrate(objectPoints,
										imagePoints, dst.imagePoints,
										cameraMatrix, distCoeffs,
										dstCameraMatrix, dst.distCoeffs,
										distortedIntrinsics.getImageSize(), rotation, translation,
										essentialMatrix, fundamentalMatrix);
		return true;
	}
	float Calibration::getReprojectionError() const {
		return reprojectionError;
	}
	float Calibration::getReprojectionError(int i) const {
		return perViewErrors[i];
	}
	const Intrinsics& Calibration::getDistortedIntrinsics() const {
		return distortedIntrinsics;
	}
	const Intrinsics& Calibration::getUndistortedIntrinsics() const {
		return undistortedIntrinsics;
	}
	Mat Calibration::getDistCoeffs() const {
		return distCoeffs;
	}
	int Calibration::size() const {
		return imagePoints.size();
	}
	cv::Size Calibration::getPatternSize() const {
		return patternSize;
	}
	float Calibration::getSquareSize() const {
		return squareSize;
	}
	void Calibration::customDraw() {
		for(int i = 0; i < size(); i++) {
			draw(i);
		}
	}
	void Calibration::draw(int i) const {
		ofPushStyle();
		ofNoFill();
		ofSetColor(ofColor::red);
		for(int j = 0; j < (int)imagePoints[i].size(); j++) {
			ofCircle(toOf(imagePoints[i][j]), 5);
		}
		ofPopStyle();
	}
	// this won't work until undistort() is in pixel coordinates
	/*
	void Calibration::drawUndistortion() const {
		vector<ofVec2f> src, dst;
		cv::Point2i divisions(32, 24);
		for(int y = 0; y < divisions.y; y++) {
			for(int x = 0; x < divisions.x; x++) {
				src.push_back(ofVec2f(
					ofMap(x, -1, divisions.x, 0, addedImageSize.width),
					ofMap(y, -1, divisions.y, 0, addedImageSize.height)));
			}
		}
		undistort(src, dst);
		ofMesh mesh;
		mesh.setMode(OF_PRIMITIVE_LINES);
		for(int i = 0; i < src.size(); i++) {
			mesh.addVertex(src[i]);
			mesh.addVertex(dst[i]);
		}
		mesh.draw();
	}
	*/
	void Calibration::draw3d() const {
		for(int i = 0; i < size(); i++) {
			draw3d(i);
		}
	}
	void Calibration::draw3d(int i) const {
		ofPushStyle();
		ofPushMatrix();
		ofNoFill();

		applyMatrix(makeMatrix(boardRotations[i], boardTranslations[i]));

		ofSetColor(ofColor::fromHsb(255 * i / size(), 255, 255));

		ofDrawBitmapString(ofToString(i), 0, 0);

		for(int j = 0; j < (int)objectPoints[i].size(); j++) {
			ofPushMatrix();
			ofTranslate(toOf(objectPoints[i][j]));
			ofCircle(0, 0, .5);
			ofPopMatrix();
		}

		ofMesh mesh;
		mesh.setMode(OF_PRIMITIVE_LINE_STRIP);
		for(int j = 0; j < (int)objectPoints[i].size(); j++) {
			ofVec3f cur = toOf(objectPoints[i][j]);
			mesh.addVertex(cur);
		}
		mesh.draw();

		ofPopMatrix();
		ofPopStyle();
	}
	void Calibration::updateObjectPoints() {
		vector<Point3f> points = createObjectPoints(patternSize, squareSize, patternType);
		objectPoints.resize(imagePoints.size(), points);
	}
	void Calibration::updateReprojectionError() {
		vector<Point2f> imagePoints2;
		int totalPoints = 0;
		double totalErr = 0;

		perViewErrors.clear();
		perViewErrors.resize(objectPoints.size());

		for(int i = 0; i < (int)objectPoints.size(); i++) {
			projectPoints(Mat(objectPoints[i]), boardRotations[i], boardTranslations[i], distortedIntrinsics.getCameraMatrix(), distCoeffs, imagePoints2);
			double err = norm(Mat(imagePoints[i]), Mat(imagePoints2), CV_L2);
			int n = objectPoints[i].size();
			perViewErrors[i] = sqrt(err * err / n);
			totalErr += err * err;
			totalPoints += n;
			ofLog(OF_LOG_VERBOSE, "view " + ofToString(i) + " has error of " + ofToString(perViewErrors[i]));
		}

		reprojectionError = sqrt(totalErr / totalPoints);

		ofLog(OF_LOG_VERBOSE, "all views have error of " + ofToString(reprojectionError));
	}
	void Calibration::updateUndistortion() {
		Mat undistortedCameraMatrix = getOptimalNewCameraMatrix(distortedIntrinsics.getCameraMatrix(), distCoeffs, distortedIntrinsics.getImageSize(), fillFrame ? 0 : 1);
		initUndistortRectifyMap(distortedIntrinsics.getCameraMatrix(), distCoeffs, Mat(), undistortedCameraMatrix, distortedIntrinsics.getImageSize(), CV_16SC2, undistortMapX, undistortMapY);
		undistortedIntrinsics.setup(undistortedCameraMatrix, distortedIntrinsics.getImageSize());
	}

	vector<Point3f> Calibration::createObjectPoints(cv::Size patternSize, float squareSize, CalibrationPattern patternType) {
		vector<Point3f> corners;
		switch(patternType) {
			case CHESSBOARD:
			case CIRCLES_GRID:
				for(int i = 0; i < patternSize.height; i++)
					for(int j = 0; j < patternSize.width; j++)
						corners.push_back(Point3f(float(j * squareSize), float(i * squareSize), 0));
				break;
			case ASYMMETRIC_CIRCLES_GRID:
				for(int i = 0; i < patternSize.height; i++)
					for(int j = 0; j < patternSize.width; j++)
						corners.push_back(Point3f(float(((2 * j) + (i % 2)) * squareSize), float(i * squareSize), 0));
				break;
		}
		return corners;
	}
}

Posts: 1

Participants: 1

Read full topic