camAlgo/camCalib/sourceCode/FitMapParam.cpp

#include <opencv2/opencv.hpp>

#include "FitMapParam.h"

// ʹ<><CAB9><EFBFBD><EFBFBD>С<EFBFBD><D0A1><EFBFBD>˷<EFBFBD><CBB7><EFBFBD><EFBFBD>ж<EFBFBD><D0B6><EFBFBD>ʽ<EFBFBD><CABD><EFBFBD><EFBFBD>
FitParam FittingByPoints(const double* x, const double* y, int count) {
	// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>󷽳<EFBFBD> AW=B
	cv::Mat A(count, FittingOrder + 1, CV_64FC1); // <20><><EFBFBD>ƾ<EFBFBD><C6BE><EFBFBD>
	cv::Mat B(count, 1, CV_64FC1); // <20>۲<EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>

	// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>A<EFBFBD><41>B
	for (int i = 0; i < count; i++) {
		A.ptr<double>(i)[0] = 1; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ1
		for (int n = 1; n <= FittingOrder; n++)
			A.ptr<double>(i)[n] = pow(x[i], n); // x<><78>n<EFBFBD>η<EFBFBD>

		B.ptr<double>(i)[0] = y[i]; // <20><>Ӧ<EFBFBD><D3A6>yֵ
	}

	// <20><>С<EFBFBD><D0A1><EFBFBD>˽⣺W = (A^T*A)^-1 * A^T * B
	cv::Mat W = (A.t() * A).inv() * A.t() * B;

	// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ṹ<EFBFBD><E1B9B9>
	FitParam result;
	for (int n = 0; n <= FittingOrder; n++)
		result.s[n] = W.ptr<double>(0)[n];

	return result;
}

// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ÿһ<C3BF><D2BB>(<28><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>)<29><><EFBFBD>ж<EFBFBD><D0B6><EFBFBD>ʽ<EFBFBD><CABD><EFBFBD>ϣ<EFBFBD><CFA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϲ<EFBFBD><CFB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
cv::Mat GetFitParamMap(const cv::Mat& map, int row_step) {
	// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><C7B7>ܱ<EFBFBD>row_step<65><70><EFBFBD><EFBFBD>
	if (0 != (map.rows % row_step))
		return cv::Mat(); // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ؿվ<D8BF><D5BE><EFBFBD>

	// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	const int rows = 1 == row_step ? map.rows : (map.rows / row_step) + 1;
	cv::Mat fitMap = cv::Mat::zeros(rows, (FittingOrder + 1), map.type());

	for (int _r = 0; _r < map.rows; _r++) {
		// <20><><EFBFBD>㵱ǰ<E3B5B1><C7B0><EFBFBD>ڽ<EFBFBD><DABD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>е<EFBFBD><D0B5>к<EFBFBD>
		int fitRow = _r / row_step;
		if ((1 != row_step) && (_r == (map.rows - 1)))
			fitRow++; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
		else if (0 != (_r % row_step))
			continue; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>

		// ׼<><D7BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
		double* x = new double[map.cols];
		double* y = new double[map.cols];
		for (int _c = 0; _c < map.cols; _c++) {
			x[_c] = _c; // x<><78><EFBFBD><EFBFBD>Ϊ<EFBFBD>к<EFBFBD>
			y[_c] = map.ptr<float>(_r)[_c]; // y<><79><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD>ֵ
		}

		// <20><><EFBFBD>ж<EFBFBD><D0B6><EFBFBD>ʽ<EFBFBD><CABD><EFBFBD><EFBFBD>
		FitParam param = FittingByPoints(x, y, map.cols);
		delete[] x;
		delete[] y;

		// <20>洢<EFBFBD><E6B4A2><EFBFBD>Ͻ<EFBFBD><CFBD><EFBFBD>
		for (int _c = 0; _c <= FittingOrder; _c++)
			fitMap.ptr<float>(fitRow)[_c] = param.s[_c];
	}

	return fitMap;
}

// <20><><EFBFBD><EFBFBD><EFBFBD>ϲ<EFBFBD><CFB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ؽ<EFBFBD>ԭʼͼ<CABC><CDBC>
cv::Mat GetMapFromFitMap(const cv::Mat& map, const cv::Size size, int row_step) {
	// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD>ƥ<EFBFBD><C6A5>
	int rows_needed = size.height / row_step;
	if ((size.height % row_step) > 0)
		rows_needed += 1;
	if (map.rows != rows_needed)
		return cv::Mat();

	// <20><><EFBFBD><EFBFBD>ϡ<EFBFBD><CFA1><EFBFBD><EFBFBD>Map(ֻ<><D6BB><EFBFBD><EFBFBD><EFBFBD>ؼ<EFBFBD><D8BC><EFBFBD>)
	cv::Mat fitMap = cv::Mat::zeros(size.height, size.width, map.type());
	for (int _r = 0; _r < map.rows; _r++) {
		// <20><><EFBFBD><EFBFBD>ʵ<EFBFBD><CAB5><EFBFBD>к<EFBFBD>(<28><><EFBFBD><EFBFBD><EFBFBD>߽<EFBFBD><DFBD><EFBFBD><EFBFBD><EFBFBD>)
		const int real_row = _r * row_step >= size.height ?
			size.height - 1 : _r * row_step;

		// ʹ<>ö<EFBFBD><C3B6><EFBFBD>ʽ<EFBFBD><CABD><EFBFBD><EFBFBD><EFBFBD>ؽ<EFBFBD><D8BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
		for (int _c = 0; _c < size.width; _c++) {
			fitMap.ptr<float>(real_row)[_c] = map.ptr<float>(_r)[0]; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
			for (int n = FittingOrder; n > 0; n--)
				fitMap.ptr<float>(real_row)[_c] += map.ptr<float>(_r)[n] * pow(_c, n); // <20><><EFBFBD>ϸ<EFBFBD><CFB8><EFBFBD><EFBFBD><EFBFBD>
		}
	}

	// <20>Էǹؼ<C7B9><D8BC>н<EFBFBD><D0BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Բ<EFBFBD>ֵ
	if (row_step > 1) {
		for (int _r = 0; _r < size.height; _r++) {
			if (0 == (_r % row_step)) // <20><><EFBFBD><EFBFBD><EFBFBD>ؼ<EFBFBD><D8BC><EFBFBD>
				continue;

			// <20>ҵ<EFBFBD><D2B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ؼ<EFBFBD><D8BC><EFBFBD>
			const int row_l = int(_r / row_step) * row_step;
			int row_h = int(_r / row_step + 1) * row_step;
			row_h = row_h >= map.rows ? map.rows - 1 : row_h;

			// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵȨ<D6B5><C8A8>
			const double sl = double(row_h - _r) / (row_h - row_l);
			const double sh = double(_r - row_l) / (row_h - row_l);

			// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Բ<EFBFBD>ֵ
			for (int _c = 0; _c < map.cols; _c++) {
				fitMap.ptr<float>(_r)[_c] = sl * fitMap.ptr<float>(row_l)[_c]
					+ sh * fitMap.ptr<float>(row_h)[_c];
			}
		}
	}

	return fitMap;
}