matmath_c.txt

LM算法的实现并不难,这里给出了LM算法C语言实现

  rot[1][1] = 1.0 - 2.0 * (q22 + q33);
  rot[1][2] =     - 2.0 * (q03 - q12);
  rot[1][3] =       2.0 * (q02 + q13);
  rot[2][1] =       2.0 * (q03 + q12);
  rot[2][2] = 1.0 - 2.0 * (q22 + q33);
  rot[2][3] =     - 2.0 * (q01 - q23);
  rot[3][1] =     - 2.0 * (q02 - q13);
  rot[3][2] =       2.0 * (q01 + q23);
  rot[3][3] = 1.0 - 2.0 * (q11 + q22);
}

/**********************************************************

  Vector and Matrix allocation and deallocation routines

*/

void nrerror(char error_text[])
/* Numerical Recipes standard error handler */
{
	fprintf(stderr,"Numerical Recipes run-time error...\n");
	fprintf(stderr,"%s\n",error_text);
	fprintf(stderr,"...now exiting to system...\n");
	exit(1);
}

m_elem *vector(long nl, long nh)
/* allocate a vector with subscript range v[nl..nh] */
{
	m_elem *v;

	v=(m_elem *)malloc((size_t) ((nh-nl+1+NR_END) *
					   sizeof(m_elem)));
	if (!v) nrerror("allocation failure in vector()");
	return v-nl+NR_END;
}

float *fvector(long nl, long nh)
/* allocate a vector with subscript range v[nl..nh] */
{
	float *v;

	v=(float *)malloc((size_t) ((nh-nl+1+NR_END) *
					   sizeof(float)));
	if (!v) nrerror("allocation failure in vector()");
	return v-nl+NR_END;
}

int *ivector(long nl, long nh)
/* allocate an int vector with subscript range v[nl..nh] */
{
	int *v;

	v=(int *)malloc((size_t) ((nh-nl+1+NR_END)*sizeof(int)));
	if (!v) nrerror("allocation failure in ivector()");
	return v-nl+NR_END;
}

unsigned char *cvector(long nl, long nh)
/* allocate an unsigned char vector with subscript range v[nl..nh] */
{
	unsigned char *v;

	v=(unsigned char *)malloc((size_t) ((nh-nl+1+NR_END)*sizeof(unsigned char)));
	if (!v) nrerror("allocation failure in cvector()");
	return v-nl+NR_END;
}

unsigned long *lvector(long nl, long nh)
/* allocate an unsigned long vector with subscript range v[nl..nh] */
{
	unsigned long *v;

	v=(unsigned long *)malloc((size_t) ((nh-nl+1+NR_END)*sizeof(long)));
	if (!v) nrerror("allocation failure in lvector()");
	return v-nl+NR_END;
}

double *dvector(long nl, long nh)
/* allocate a double vector with subscript range v[nl..nh] */
{
	double *v;

	v=(double *)malloc((size_t) ((nh-nl+1+NR_END)*sizeof(double)));
	if (!v) nrerror("allocation failure in dvector()");
	return v-nl+NR_END;
}

m_elem **matrix(long nrl, long nrh, long ncl, long nch)
/* allocate a m_elem matrix with subscript range m[nrl..nrh][ncl..nch] */
{
	long i, nrow=nrh-nrl+1,ncol=nch-ncl+1;
	m_elem **m;

	/* allocate pointers to rows */
	m=(m_elem **)malloc((size_t)((nrow+NR_END)
					    *sizeof(m_elem *)));
	if (!m) nrerror("allocation failure 1 in matrix()");
	m += NR_END;
	m -= nrl;

	/* allocate rows and set pointers to them */
	m[nrl]=(m_elem *)malloc((size_t)((nrow*ncol+NR_END) 
					       *sizeof(m_elem)));
	if (!m[nrl]) nrerror("allocation failure 2 in matrix()");
	m[nrl] += NR_END;
	m[nrl] -= ncl;

	for(i=nrl+1;i<=nrh;i++) m[i]=m[i-1]+ncol;

	/* return pointer to array of pointers to rows */
	return m;
}


float **fmatrix(long nrl, long nrh, long ncl, long nch)
/* allocate a float matrix with subscript range m[nrl..nrh][ncl..nch] */
{
	long i, nrow=nrh-nrl+1,ncol=nch-ncl+1;
	float **m;

	/* allocate pointers to rows */
	m=(float **) malloc((size_t)((nrow+NR_END)*sizeof(float*)));
	if (!m) nrerror("allocation failure 1 in matrix()");
	m += NR_END;
	m -= nrl;

	/* allocate rows and set pointers to them */
	m[nrl]=(float *) malloc((size_t)((nrow*ncol+NR_END)*sizeof(float)));
	if (!m[nrl]) nrerror("allocation failure 2 in matrix()");
	m[nrl] += NR_END;
	m[nrl] -= ncl;

	for(i=nrl+1;i<=nrh;i++) m[i]=m[i-1]+ncol;

	/* return pointer to array of pointers to rows */
	return m;
}

double **dmatrix(long nrl, long nrh, long ncl, long nch)
/* allocate a double matrix with subscript range m[nrl..nrh][ncl..nch] */
{
	long i, nrow=nrh-nrl+1,ncol=nch-ncl+1;
	double **m;

	/* allocate pointers to rows */
	m=(double **) malloc((size_t)((nrow+NR_END)*sizeof(double*)));
	if (!m) nrerror("allocation failure 1 in matrix()");
	m += NR_END;
	m -= nrl;

	/* allocate rows and set pointers to them */
	m[nrl]=(double *) malloc((size_t)((nrow*ncol+NR_END)*sizeof(double)));
	if (!m[nrl]) nrerror("allocation failure 2 in matrix()");
	m[nrl] += NR_END;
	m[nrl] -= ncl;

	for(i=nrl+1;i<=nrh;i++) m[i]=m[i-1]+ncol;

	/* return pointer to array of pointers to rows */
	return m;
}

int **imatrix(long nrl, long nrh, long ncl, long nch)
/* allocate a int matrix with subscript range m[nrl..nrh][ncl..nch] */
{
	long i, nrow=nrh-nrl+1,ncol=nch-ncl+1;
	int **m;

	/* allocate pointers to rows */
	m=(int **) malloc((size_t)((nrow+NR_END)*sizeof(int*)));
	if (!m) nrerror("allocation failure 1 in matrix()");
	m += NR_END;
	m -= nrl;


	/* allocate rows and set pointers to them */
	m[nrl]=(int *) malloc((size_t)((nrow*ncol+NR_END)*sizeof(int)));
	if (!m[nrl]) nrerror("allocation failure 2 in matrix()");
	m[nrl] += NR_END;
	m[nrl] -= ncl;

	for(i=nrl+1;i<=nrh;i++) m[i]=m[i-1]+ncol;

	/* return pointer to array of pointers to rows */
	return m;
}

m_elem **submatrix(m_elem **a,
			 long oldrl, long oldrh, long oldcl, long oldch,
	long newrl, long newcl)
/* point a submatrix [newrl..][newcl..] to a[oldrl..oldrh][oldcl..oldch] */
{
	long i,j,nrow=oldrh-oldrl+1,ncol=oldcl-newcl;
	m_elem **m;

	/* allocate array of pointers to rows */
	m=(m_elem **) malloc((size_t) ((nrow+NR_END)
					     *sizeof(m_elem*)));
	if (!m) nrerror("allocation failure in submatrix()");
	m += NR_END;
	m -= newrl;

	/* set pointers to rows */
	for(i=oldrl,j=newrl;i<=oldrh;i++,j++) m[j]=a[i]+ncol;

	/* return pointer to array of pointers to rows */
	return m;
}

m_elem **convert_matrix(m_elem *a,
			      long nrl, long nrh, long ncl, long nch)
/* allocate a m_elem matrix m[nrl..nrh][ncl..nch] that points
to the matrix declared in the standard C manner as a[nrow][ncol], where
nrow=nrh-nrl+1 and ncol=nch-ncl+1. The routine should be called with the
address &a[0][0] as the first argument. */
{
	long i,j,nrow=nrh-nrl+1,ncol=nch-ncl+1;
	m_elem **m;

	/* allocate pointers to rows */
	m=(m_elem **)malloc((size_t)((nrow+NR_END)
					   *sizeof(m_elem*)));
	if (!m) nrerror("allocation failure in convert_matrix()");
	m += NR_END;
	m -= nrl;

	/* set pointers to rows */
	m[nrl]=a-ncl;
	for(i=1,j=nrl+1;i<nrow;i++,j++) m[j]=m[j-1]+ncol;
	/* return pointer to array of pointers to rows */
	return m;
}

void free_vector(m_elem *v, long nl, long nh)
/* free a vector allocated with vector() */
{
	free((FREE_ARG) (v+nl-NR_END));
}

void free_fvector(float *v, long nl, long nh)
/* free a float vector allocated with fvector() */
{
	free((FREE_ARG) (v+nl-NR_END));
}

void free_ivector(int *v, long nl, long nh)
/* free an int vector allocated with ivector() */
{
	free((FREE_ARG) (v+nl-NR_END));
}

void free_cvector(unsigned char *v, long nl, long nh)
/* free an unsigned char vector allocated with cvector() */
{
	free((FREE_ARG) (v+nl-NR_END));
}

void free_lvector(unsigned long *v, long nl, long nh)
/* free an unsigned long vector allocated with lvector() */
{
	free((FREE_ARG) (v+nl-NR_END));
}

void free_dvector(double *v, long nl, long nh)
/* free a double vector allocated with dvector() */
{
	free((FREE_ARG) (v+nl-NR_END));
}

void free_matrix(m_elem  **m, long nrl, long nrh, long ncl, long nch)
/* free a matrix allocated by matrix() */
{
	free((FREE_ARG) (m[nrl]+ncl-NR_END));
	free((FREE_ARG) (m+nrl-NR_END));
}

void free_fmatrix(float **m, long nrl, long nrh, long ncl, long nch)
/* free a float matrix allocated by fmatrix() */
{
	free((FREE_ARG) (m[nrl]+ncl-NR_END));
	free((FREE_ARG) (m+nrl-NR_END));
}

void free_dmatrix(double **m, long nrl, long nrh, long ncl, long nch)
/* free a double matrix allocated by dmatrix() */
{
	free((FREE_ARG) (m[nrl]+ncl-NR_END));
	free((FREE_ARG) (m+nrl-NR_END));
}

void free_imatrix(int **m, long nrl, long nrh, long ncl, long nch)
/* free an int matrix allocated by imatrix() */
{
	free((FREE_ARG) (m[nrl]+ncl-NR_END));
	free((FREE_ARG) (m+nrl-NR_END));
}

void free_submatrix(float **b, long nrl, long nrh, long ncl, long nch)
/* free a submatrix allocated by submatrix() */
{
	free((FREE_ARG) (b+nrl-NR_END));
}

void free_convert_matrix(float **b, long nrl, long nrh, long ncl, long nch)
/* free a matrix allocated by convert_matrix() */
{
	free((FREE_ARG) (b+nrl-NR_END));
}

/***************************************************

  Matrix and Vector Printing routines

*/


void print_vector( char *str, m_elem *x, int n )
{
  int     i;

  printf( "%s:\n", str );
  for( i = 1; i <= n; i++ )
    {
      if( (x[ i ] > 1) && (x[i] < 999) )
	printf( " %3.1lf", x[ i ] );
      else
	printf( " %2.2lg", x[ i ] );
    }
  printf( "\n" );
}

void print_quaternion( char *str, m_elem *x )
{
  int     i;

  printf( "%s:\n", str );
  for( i = 0; i < QUATERNION_SIZE; i++ )
    {
      if( (x[ i ] > 1) && (x[i] < 999) )
	printf( " %3.1lf", x[ i ] );
      else
	printf( " %2.2lg", x[ i ] );
    }
  printf( "\n" );
}

void print_matrix( char *str, m_elem **A, int m, int n )
{
  int     i, j;

  printf( "%s:  (%d x %d)\n", str, m, n );
  for( i = 1; i <= m; i++ )
    {
      printf( ">" );
      for( j = 1; j <= n; j++ )
	{
	  printf( " %2.2lg", A[ i ][ j ] );
	}
      printf( "\n" );
    }
}