calib.h

足球机器人自动程序

#ifndef CALIB_H
#define CALIB_H

#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include "clapack.h"

inline void calib_pi(const char * const filename) {

	FILE *fp = fopen(filename, "r");
	if (fp == NULL) {
		perror(filename);
		exit(-1);
	}

	unsigned int n;
	if (fscanf(fp, "%u", &n) != 1) {
		fprintf(stderr, "error read calib_n\n");
		exit(-1);
	}
	if (n > 10) {
		fprintf(stderr, "calib_n too big\n");
		exit(-1);
	}

	double a[4*n*n];
	double b[2*n];

	unsigned int i;
	for (i=0; i<n; ++i) {
		double sx, sy, wx, wy;
		if (fscanf(fp, "%lf %lf %lf %lf", &wx, &wy, &sx, &sy) != 4) {
			fprintf(stderr, "error read calib_data\n");
			exit(-1);
		}

		a[2*n*i] = sx;
		a[2*n*i+1] = sy;
		a[2*n*i+2] = 1;
		a[2*n*i+3] = 0;
		a[2*n*i+4] = 0;
		a[2*n*i+5] = 0;
		a[2*n*i+6] = -sx * wx;
		a[2*n*i+7] = -sy * wx;

		a[2*n*i+2*n*n] = 0;
		a[2*n*i+2*n*n+1] = 0;
		a[2*n*i+2*n*n+2] = 0;
		a[2*n*i+2*n*n+3] = sx;
		a[2*n*i+2*n*n+4] = sy;
		a[2*n*i+2*n*n+5] = 1;
		a[2*n*i+2*n*n+6] = -sx * wy;
		a[2*n*i+2*n*n+7] = -sy * wy;

		b[i] = wx;
		b[i+n] = wy;
	}

	fclose(fp);

	printf("b=\n");
	for (i=0; i<10; ++i) {
			printf("%f\n", b[i]);
	}
	
	double atb[8];
	cblas_dgemv(CblasRowMajor, CblasTrans, 2*n, 8, 1.0, a, 2*n, b, 1, 0.0, atb, 1);

	printf("atb=\n");
	for (i=0; i<8; ++i) {
			printf("%f\n", atb[i]);
	}
	printf("\n");

	printf("a=\n");
	for (i=0; i<10; ++i) {
		int j;
		for (j=0; j<8; ++j) {
			printf("%f ", a[i*10+j]);
		}
		printf("\n");
	}

	double ata[64];
	cblas_dsyrk(CblasRowMajor, CblasUpper, CblasTrans, 8, 10, 1.0, a, 10, 0.0, ata, 8);

	printf("ata=\n");
	for (i=0; i<8; ++i) {
		int j;
		for (j=0; j<8; ++j) {
			printf("%.1f ", ata[i*8+j]);
		}
		printf("\n");
	}

	if (clapack_dposv(CblasRowMajor, CblasUpper, 8, 1, ata, 8, atb, 8) != 0) {
		fprintf(stderr, "dgesv error\n");
		exit(-1);
	}
	for (i=0; i<8; ++i) printf("%f\n", atb[i]);
}

#endif