3次样条插值的Opencv实现方法

发表评论

本文参考了https://www.cnblogs.com/hehehaha/p/6332245.html 的内容与python对3次样条插值实现的代码。再原有代码的基础上增加了固定边界和非节点边界的算法，并将接口函数的参数类型改写为Mat,方便一次进行多次样条插值的计算。

CubicSpline.h

#ifndef CUBIC_SPLINE_H
#define CUBIC_SPLINE_H

#include <iostream>
#include <vector>
#include <math.h>

#include <opencv2/core.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/imgcodecs.hpp>

using namespace std;
using namespace cv;

class CubicSplineCoeffs_Mat
{
public:
	CubicSplineCoeffs_Mat(const int &count)
	{
		a = std::vector<Mat>(count);
		b = std::vector<Mat>(count);
		c = std::vector<Mat>(count);
		d = std::vector<Mat>(count);
	}
	~CubicSplineCoeffs_Mat()
	{
		std::vector<Mat>().swap(a);
		std::vector<Mat>().swap(b);
		std::vector<Mat>().swap(c);
		std::vector<Mat>().swap(d);
	}

public:
	std::vector<Mat> a, b, c, d;
};

enum CubicSplineMode
{
	CUBIC_NATURAL,    
	CUBIC_CLAMPED,    
	CUBIC_NOT_A_KNOT  
};


class CubicSpline
{
public:
	~CubicSpline();

public:
	void calCubicSplineCoeffs(std::vector<Mat> &input_x,
		std::vector<Mat> &input_y, CubicSplineCoeffs_Mat *&cubicCoeffs,
		CubicSplineMode splineMode = CUBIC_NATURAL);

	void cubicSplineInterpolation2(CubicSplineCoeffs_Mat *&cubicCoeffs,
		std::vector<Mat> &input_x, Mat &x, Mat &y, int targeindex);

	bool caltridiagonalMatrices(cv::Mat &input_a,
		cv::Mat &input_b, cv::Mat &input_c,
		cv::Mat &input_d, cv::Mat &output_x);
};

#endif // CUBIC_SPLINE_H

CubicSpline.cpp

#include "CubicSpline.h"

void CubicSpline::calCubicSplineCoeffs(
	std::vector<Mat> &input_x,
	std::vector<Mat> &input_y,
	CubicSplineCoeffs_Mat *&cubicCoeffs,
	CubicSplineMode splineMode)
{
	int sizeOfx = input_x.size();
	int sizeOfy = input_y.size();

	if (sizeOfx != sizeOfy)
	{
		std::cout << "Data input error!" << std::endl <<
			"Location: CubicSplineInterpolation.cpp" <<
			" -> calCubicSplineCoeffs()" << std::endl;
		return;
	}

	Size matsize = input_x[0].size();
	int matrow = input_x[0].rows;
	int matcol = input_x[0].cols;

	std::vector<Mat> copy_y = input_y;
	const int count = sizeOfx;
	const int count1 = sizeOfx - 1;
	const int count2 = sizeOfx - 2;

	cubicCoeffs = new CubicSplineCoeffs_Mat(count1);

	std::vector<Mat> step_h(count1);
	std::vector<Mat> slope(count1);

	int dims2[] = { count ,matrow,matcol };
	Mat m_a(3, dims2, CV_64F, Scalar(0));
	Mat m_b(3, dims2, CV_64F, Scalar(0));
	Mat m_c(3, dims2, CV_64F, Scalar(0));
	Mat m_d(3, dims2, CV_64F, Scalar(0));
	Mat m_part(3, dims2, CV_64F, Scalar(0));

	// initial step hi
	for (int idx = 0; idx < count1; idx++)
	{
		step_h[idx] = input_x[idx + 1] - input_x[idx];
		slope[idx] = (copy_y[idx + 1] - copy_y[idx]) / step_h[idx];
	}

	// initial coefficients
	for (int idx = 0; idx < count2; idx++)
	{
		step_h[idx + 1].copyTo(Mat(step_h[idx + 1].size(), CV_64F, m_a.ptr(idx)));
		Mat temp = 2 * (step_h[idx] + step_h[idx + 1]);
		temp.copyTo(Mat(step_h[idx].size(), CV_64F, m_b.ptr(idx + 1)));
		step_h[idx].copyTo(Mat(step_h[idx].size(), CV_64F, m_c.ptr(idx + 2)));
	}

	// initial d
	for (int idx = 0; idx < count2; idx++)
	{
		Mat temp, temp2;
		multiply(step_h[idx + 1], slope[idx], temp);
		multiply(step_h[idx], slope[idx + 1], temp2);
		temp = (temp + temp2) * 3;
		temp.copyTo(Mat(step_h[idx].size(), CV_64F, m_d.ptr(idx + 1)));
	}


	if (splineMode == CUBIC_NATURAL)
	{
		Mat temp_m_b_start = 2 * step_h[0];
		temp_m_b_start.copyTo(Mat(step_h[0].size(), CV_64F, m_b.ptr(0)));
		step_h[0].copyTo(Mat(step_h[0].size(), CV_64F, m_c.ptr(1)));
		Mat temp_m_d_start = 3 * (copy_y[1] - copy_y[0]);
		temp_m_d_start.copyTo(Mat(copy_y[0].size(), CV_64F, m_d.ptr(0)));

		Mat temp_m_b_end = 2 * step_h[count1 - 1];
		temp_m_b_end.copyTo(Mat(step_h[count1 - 1].size(), CV_64F, m_b.ptr(count - 1)));
		step_h[count1 - 1].copyTo(Mat(step_h[0].size(), CV_64F, m_a.ptr(count - 2)));
		Mat temp_m_d_end = 3 * (copy_y[count - 1] - copy_y[count - 2]);
		temp_m_d_end.copyTo(Mat(copy_y[count - 1].size(), CV_64F, m_d.ptr(count - 1)));
	}
	else if (splineMode == CUBIC_NOT_A_KNOT) {
		step_h[1].copyTo(Mat(step_h[1].size(), CV_64F, m_b.ptr(0)));
		Mat temp_m_b_start = input_x[2] - input_x[0];
		temp_m_b_start.copyTo(Mat(input_x[0].size(), CV_64F, m_c.ptr(1)));
		Mat d = input_x[2] - input_x[0];
		Mat temp_m_d_start;
		multiply(step_h[0] + 2 * d, step_h[1], temp_m_d_start);
		multiply(temp_m_d_start, slope[0], temp_m_d_start);
		Mat temp;
		cv::pow(step_h[0], 2, temp);
		multiply(temp, slope[1], temp);
		temp_m_d_start = (temp_m_d_start + temp) / d;
		temp_m_d_start.copyTo(Mat(step_h[0].size(), CV_64F, m_d.ptr(0)));

		step_h[count1 - 2].copyTo(Mat(step_h[count1 - 2].size(), CV_64F, m_b.ptr(count - 1)));
		Mat temp_m_a_end = input_x[count - 1] - input_x[count - 3];
		temp_m_a_end.copyTo(Mat(input_x[count - 1].size(), CV_64F, m_a.ptr(count - 2)));
		d = input_x[count - 1] - input_x[count - 3];
		Mat temp_m_d_end;
		pow(step_h[count1 - 1], 2, temp_m_d_end);
		multiply(temp_m_d_end, slope[count1 - 2], temp_m_d_end);
		multiply(2 * d + step_h[count1 - 1], step_h[count1 - 2], temp);

		multiply(temp, slope[count1 - 1], temp);
		temp_m_d_end = (temp_m_d_end + temp) / d;
		temp_m_d_end.copyTo(Mat(step_h[count1 - 1].size(), CV_64F, m_d.ptr(count - 1)));
	}
	else if (splineMode == CUBIC_CLAMPED) {
		Mat(matrow, matcol, CV_64F, m_b.ptr(0)).setTo(1);
		Mat(matrow, matcol, CV_64F, m_c.ptr(1)).setTo(0);
		Mat(matrow, matcol, CV_64F, m_d.ptr(0)).setTo(0);

		Mat(matrow, matcol, CV_64F, m_b.ptr(count - 1)).setTo(1);
		Mat(matrow, matcol, CV_64F, m_a.ptr(count - 2)).setTo(0);
		Mat(matrow, matcol, CV_64F, m_d.ptr(count - 1)).setTo(0);
	}

	bool isSucceed = caltridiagonalMatrices(m_a, m_b, m_c, m_d, m_part);
	if (!isSucceed)
	{
		std::cout << "Calculate tridiagonal matrices failed!" << std::endl <<
			"Location: CubicSplineInterpolation.cpp -> " <<
			"caltridiagonalMatrices()" << std::endl;

		return;
	}

	for (int i = 0; i < count1; i++)
	{
		Mat t = (Mat(matrow, matcol, m_part.type(), m_part.ptr(i)) + Mat(matrow, matcol, m_part.type(), m_part.ptr(i + 1)) - 2 * slope[i]) / step_h[i];
		cubicCoeffs->a[i] = copy_y[i];
		Mat(matrow, matcol, m_part.type(), m_part.ptr(i)).copyTo(cubicCoeffs->b[i]);
		cubicCoeffs->c[i] = (slope[i] - Mat(matrow, matcol, m_part.type(), m_part.ptr(i))) / step_h[i] - t;
		cubicCoeffs->d[i] = t / step_h[i];
	}

}


CubicSpline::~CubicSpline() {

}

void CubicSpline::cubicSplineInterpolation2(CubicSplineCoeffs_Mat *&cubicCoeffs,
	std::vector<Mat> &input_x, Mat &x, Mat &y, int targeindex) {
	Mat cmpresult_self;
	cv::compare(x, input_x[targeindex], cmpresult_self, CMP_GE);
	Mat temp_cmp(cmpresult_self.size(), cmpresult_self.type());
	cmpresult_self.copyTo(temp_cmp);

	Mat temp_a(x.size(), CV_64F), temp_b(x.size(), CV_64F), temp_c(x.size(), CV_64F), temp_d(x.size(), CV_64F), dertx(x.size(), CV_64F);

	int shift_index = 0;
	do {
		if (targeindex + shift_index >= cubicCoeffs->a.size()) {
			break;
		}
		Mat tempx = x - input_x[targeindex + shift_index];
		tempx.copyTo(dertx, temp_cmp);
		cubicCoeffs->a[targeindex + shift_index].copyTo(temp_a, temp_cmp);
		cubicCoeffs->b[targeindex + shift_index].copyTo(temp_b, temp_cmp);
		cubicCoeffs->c[targeindex + shift_index].copyTo(temp_c, temp_cmp);
		cubicCoeffs->d[targeindex + shift_index].copyTo(temp_d, temp_cmp);
		shift_index++;
		if (targeindex + shift_index >= input_x.size()) {
			break;
		}
		cv::compare(x, input_x[targeindex + shift_index], temp_cmp, CMP_GE);
	} while (countNonZero(temp_cmp) > 0);

	shift_index = 1;
	cv::bitwise_not(cmpresult_self, temp_cmp);

	do {
		if (targeindex - shift_index < 0) {
			break;
		}
		Mat tempx = x - input_x[targeindex - shift_index];
		tempx.copyTo(dertx, temp_cmp);
		cubicCoeffs->a[targeindex - shift_index].copyTo(temp_a, temp_cmp);
		cubicCoeffs->b[targeindex - shift_index].copyTo(temp_b, temp_cmp);
		cubicCoeffs->c[targeindex - shift_index].copyTo(temp_c, temp_cmp);
		cubicCoeffs->d[targeindex - shift_index].copyTo(temp_d, temp_cmp);
		shift_index++;
		if (targeindex - shift_index < 0) {
			break;
		}
		cv::compare(x, input_x[targeindex - shift_index], temp_cmp, CMP_LT);
	} while (countNonZero(temp_cmp) > 0);

	y.setTo(-1);
	Mat temp1;
	cv::multiply(temp_b, dertx, temp1);
	Mat temp2;
	cv::pow(dertx, 2, temp2);
	cv::multiply(temp_c, temp2, temp2);
	Mat temp3;
	cv::pow(dertx, 3, temp3);
	cv::multiply(temp_d, temp3, temp3);
	y = temp_a + temp1 + temp2 + temp3;
}


bool CubicSpline::caltridiagonalMatrices(cv::Mat &input_a,
	cv::Mat &input_b, cv::Mat &input_c,
	cv::Mat &input_d, cv::Mat &output_x) {

	const int N = input_a.size[0];
	const int matrow = input_a.size[1];
	const int matcol = input_a.size[2];

	input_d.copyTo(output_x);

	Mat _input_b_0 = Mat(matrow, matcol, input_b.type(), input_b.ptr(0));
	Mat _input_c_1 = Mat(matrow, matcol, input_c.type(), input_c.ptr(1));
	Mat _output_x_0 = Mat(matrow, matcol, output_x.type(), output_x.ptr(0));
	if (cv::countNonZero(_input_b_0) == matrow * matcol)
	{
		_input_c_1 /= _input_b_0;
		_output_x_0 /= _input_b_0;
	}
	else
	{
		return false;
	}

	for (int n = 1; n < N; n++) {
		Mat m;
		cv::multiply(Mat(matrow, matcol, input_a.type(), input_a.ptr(n - 1)), Mat(matrow, matcol, input_c.type(), input_c.ptr(n)), m);
		m = 1.0 / (Mat(matrow, matcol, input_b.type(), input_b.ptr(n)) - m);

		if (n < (N - 1)) {
			multiply(Mat(matrow, matcol, input_c.type(), input_c.ptr(n + 1)), m, Mat(matrow, matcol, input_c.type(), input_c.ptr(n + 1)));
		}
		Mat temp;
		multiply(Mat(matrow, matcol, input_a.type(), input_a.ptr(n - 1)), Mat(matrow, matcol, output_x.type(), output_x.ptr(n - 1)), temp);
		temp = Mat(matrow, matcol, output_x.type(), output_x.ptr(n)) - temp;
		multiply(temp, m, Mat(matrow, matcol, output_x.type(), output_x.ptr(n)));
	}

	for (int n = N - 2; n >= 0; --n) {
		Mat temp;
		multiply(Mat(matrow, matcol, input_c.type(), input_c.ptr(n + 1)), Mat(matrow, matcol, output_x.type(), output_x.ptr(n + 1)), temp);
		Mat(matrow, matcol, output_x.type(), output_x.ptr(n)) = Mat(matrow, matcol, output_x.type(), output_x.ptr(n)) - temp;
	}

	return true;
}

splinetest.cpp

#include "CubicSpline.h"

void main()
{
	std::vector<Mat> input_x;
	std::vector<Mat> input_y;

	cv::Mat input_x_0(1, 1, CV_64F, 1);
	cv::Mat input_x_1(1, 1, CV_64F, 2);
	cv::Mat input_x_2(1, 1, CV_64F, 3);
	cv::Mat input_x_3(1, 1, CV_64F, 5);
	cv::Mat input_x_4(1, 1, CV_64F, 6);
	input_x.push_back(input_x_0);
	input_x.push_back(input_x_1);
	input_x.push_back(input_x_2);
	input_x.push_back(input_x_3);
	input_x.push_back(input_x_4);

	cv::Mat input_y_0(1, 1, CV_64F, 84.4049866);
	cv::Mat input_y_1(1, 1, CV_64F, 96.15193733);
	cv::Mat input_y_2(1, 1, CV_64F, 100.2343023);
	cv::Mat input_y_3(1, 1, CV_64F, 101.0295086);
	cv::Mat input_y_4(1, 1, CV_64F, 100.8174606);
	input_y.push_back(input_y_0);
	input_y.push_back(input_y_1);
	input_y.push_back(input_y_2);
	input_y.push_back(input_y_3);
	input_y.push_back(input_y_4);


	CubicSplineCoeffs_Mat *cubicCoeffs;
	CubicSpline cubicSpline;
	cubicSpline.calCubicSplineCoeffs(input_x, input_y, cubicCoeffs, CUBIC_NATURAL);

	Mat result;
	Mat mat_64(1, 1, CV_64F, Scalar(2.5));
	cubicSpline.cubicSplineInterpolation2(cubicCoeffs, input_x, mat_64, result, 2);

	cout<<"input 2.5 output = " << result.at<double>(0) << endl;
}

运行后输出: input 2.5 output = 98.9983

发表回复

您的电子邮箱地址不会被公开。 必填项已用 * 标注