OpenCVで3次スプライン補間

#include <stdio.h>
#include <string.h>
#include <cv.h>
#include <highgui.h>
void SetPixelToBGR(IplImage *image, int x, int y, int b, int g, int r)
{
  assert(image->nChannels == 3);
  if(!image) return;
  if(( 0 <= x && x <= image->width -1 )&& (0 <= y && y <= image->height -1 ) )
  {
    b = b > 255 ? 255 : b; b = b < 0 ? 0 : b;
    g = g > 255 ? 255 : g; g = g < 0 ? 0 : g;
    r = r > 255 ? 255 : r; r = r < 0 ? 0 : r;
    image->imageData[y *image->widthStep+ x * image->nChannels + 2] = r;
    image->imageData[y *image->widthStep+ x * image->nChannels + 1] = g;
    image->imageData[y *image->widthStep+ x * image->nChannels] = b;
  }
}

void SolveSpline3D(int n, CvPoint2D32f* src_points, CvMat* dst)
{
  int i,j;
  double* h = (double*) malloc(sizeof(double)*n);
  CvMat* A = cvCreateMat(n+1, n+1, CV_32F);
  CvMat* b = cvCreateMat(n+1, 1, CV_32F);
  for(i=0; i<n; i++){
    h[i] = src_points[i+1].x - src_points[i].x;
  }
  //set A param
  cvmSet(A, 0, 0, 1.0);
  for(j=1; j<n+1; j++){
    cvmSet(A, 0, j, 0.0);
  }
  for(i=1; i<n; i++){
    for(j=0; j<i-1; j++){
      cvmSet(A, i, j, 0.0);
    }
    cvmSet(A, i, i-1, h[i-1]);
    cvmSet(A, i, i, 2*(h[i-1] + h[i]));
    cvmSet(A, i, i+1, h[i]);
    for(j=i+2; j<n+1; j++){
      cvmSet(A, i, j, 0.0);
    }
  }
  for(j=0; j<n; j++){
    cvmSet(A, n, j, 0.0);
  }
  cvmSet(A, n, n, 1.0);
  //set b param
  cvmSet(b, 0, 0, 0.0);
  for(i=1; i<n; i++){
    cvmSet(b, i, 0, (3.0*(src_points[i+1].y - src_points[i].y)/h[i]) - (3.0*(src_points[i].y - src_points[i-1].y)/h[i-1]));
  }
  cvmSet(b, n, 0, 0.0);
  cvSolve(A, b, dst, CV_LU);
  cvReleaseMat(&A);
  cvReleaseMat(&b);
}

float FuncSpline3D(int n, float x, const CvPoint2D32f* src_points, const CvMat* c_mat)
{
  assert(x >= src_points[0].x);
  assert(x <= src_points[n].x );
  int i;
  for(i=0; i<n; i++){
    if((src_points[i].x <= x) && (x <= src_points[i+1].x)){
      break;
    }
  }
  assert(i < n);
  float h_i = src_points[i+1].x - src_points[i].x;
  float c = cvmGet(c_mat, i, 0);
  float c2 = cvmGet(c_mat, i+1, 0);
  float b = (src_points[i+1].y - src_points[i].y)/h_i - h_i*(2.0*c + c2)/3.0;
  float d = (c2 - c)/(3.0*h_i);
  float h = x - src_points[i].x;
  return (src_points[i].y + b*h + c *h*h + d*h*h*h);
}
int main(void)
{
  int i,j;
  int n=13;
  CvPoint2D32f* points = (CvPoint2D32f*) malloc(sizeof(CvPoint2D32f)*(n+1));
  points[0].x = 9; points[0].y = 130;
  points[1].x = 13; points[1].y = 150;
  points[2].x = 19; points[2].y = 185;
  points[3].x = 21; points[3].y = 210;
  points[4].x = 26; points[4].y = 260;
  points[5].x = 30; points[5].y = 270;
  points[6].x = 39; points[6].y = 240;
  points[7].x = 44; points[7].y = 215;
  points[8].x = 47; points[8].y = 205;
  points[9].x = 50; points[9].y = 210;
  points[10].x = 60; points[10].y = 225;
  points[11].x = 70; points[11].y = 230;
  points[12].x = 80; points[12].y = 225;
  points[13].x = 92; points[13].y = 195;
  IplImage* image = cvCreateImage(cvSize(100, 200), IPL_DEPTH_8U, 3);
  CvMat* c_mat = cvCreateMat(n+1, 1, CV_32F);
  SolveSpline3D(n, points, c_mat);
  for(i=0; i<n-1; i++){
    double c = cvmGet(c_mat, i, 0);
    double c2 = cvmGet(c_mat, i+1, 0);
    double b = (points[i+1].y - points[i].y)/(points[i+1].x - points[i].x) - (points[i+1].x - points[i].x)*(2.0*c + c2)/3.0;
    double d = (c2 - c)/(3.0*(points[i+1].x - points[i].x));
    printf("b = %f c = %f d=%f \n", b, c, d);
  }
  cvSet(image, cvScalar(255, 255, 255));
  int x;
  for(x=10; x<=90; x++){
    float y = FuncSpline3D(n, x, points, c_mat);
    SetPixelToBGR(image, x, y, 0, 0, 0);
  }
  cvSaveImage("test.bmp", image);
  cvReleaseMat(&c_mat);
  cvReleaseImage(&image);
  free(points);
}