Ejemplo n.º 1
0
/*!
* \return	Error code.
* \ingroup      AlgMatrix
* \brief	Solves the matrix equation A.x = b for x, where A is a
*		matrix with at least as many rows as columns.
*		On return the matrix A is overwritten by the matrix U
*		in the singular value decomposition:
*		  A = U.W.V'
* \param	aMat			Matrix A.
* \param	bVec			Column matrix b, overwritten
*					by matrix x on return.
* \param	tol			Tolerance for singular values,
*					1.0e-06 should be suitable as
*					a default value.
* \param	dstIC			Destination pointer for
*					ill-conditioned flag, which may
*					be NULL, but if not NULL is set
*					to the number of singular values
*					which are smaller than the maximum
*					singular value times the given
*					threshold.
*/
AlgError	AlgMatrixSVSolve(AlgMatrix aMat, double *bVec, double tol,
                                 int *dstIC)
{
  int		cnt0 = 0,
  		cntIC = 0;
  size_t	nM,
  		nN;
  double	thresh,
  		wMax = 0.0;
  double	*tDP0,
  		*wVec = NULL;
  AlgMatrix	vMat;
  AlgError	errCode = ALG_ERR_NONE;

  nM = aMat.core->nR;
  nN = aMat.core->nC;
  vMat.core = NULL;
  if((aMat.core == NULL) || (aMat.core->type != ALG_MATRIX_RECT) ||
     (aMat.core->nR <= 0) || (aMat.core->nR < aMat.core->nC) ||
     (bVec == NULL))
  {
    errCode = ALG_ERR_FUNC;
  }
  else if((wVec = (double *)AlcCalloc(sizeof(double), aMat.rect->nC)) == NULL)
  {
    errCode = ALG_ERR_MALLOC;
  }
  else
  {
    vMat.rect = AlgMatrixRectNew(nM, nN, &errCode);
  }
  if(errCode == ALG_ERR_NONE)
  {
    errCode = AlgMatrixSVDecomp(aMat, wVec, vMat);
  }
  if(errCode == ALG_ERR_NONE)
  {
    /* Find maximum singular value. */
    cnt0 = nN;
    cntIC = 0;
    tDP0 = wVec;
    while(cnt0-- > 0)
    {
      if(*tDP0 > wMax)
      {
	++cntIC;
        wMax = *tDP0;
      }
      ++tDP0;
    }
    /* Edit the singular values, replacing any less than tol * max singular
       value with 0.0. */
    cnt0 = nN;
    tDP0 = wVec;
    thresh = tol * wMax;
    while(cnt0-- > 0)
    {
      if(*tDP0 < thresh)
      {
	*tDP0 = 0.0;
      }
      ++tDP0;
    }
    errCode = AlgMatrixSVBackSub(aMat, wVec, vMat, bVec);
  }
  AlcFree(wVec);
  AlgMatrixRectFree(vMat.rect);
  if(dstIC)
  {
    *dstIC = cntIC;
  }
  ALG_DBG((ALG_DBG_LVL_FN|ALG_DBG_LVL_1),
	  ("AlgMatrixSVSolve FX %d\n",
	   (int )errCode));
  return(errCode);
}
Ejemplo n.º 2
0
WlzBasisFnTransform *WlzConfPolyFromCPts(
  WlzFnType		type,
  int 			order,
  int 			nDPts,
  WlzDVertex2 		*dPts,
  int 			nSPts,
  WlzDVertex2 		*sPts,
  WlzErrorNum 		*dstErr)
{
  int  		idM,
  		idN,
		idX,
		nCoef=1;
  double	thresh,
  		wMax;
  double	*bMx = NULL,
  		*wMx = NULL;
  double	**aA, **vA;
  AlgMatrix	aMx, vMx;
  WlzDVertex2	powVx,
  		sVx;
  WlzBasisFnTransform *basis = NULL;
  WlzBasisFn	*basisFn=NULL;
  WlzErrorNum	errNum = WLZ_ERR_NONE;
  int		nPts = nDPts;
  const double	tol = 1.0e-06;

  /* initialise */
  aMx.core = NULL;
  vMx.core = NULL;

  /* allocate space */
  if((order < 0) || (nPts <= 0))
  {
    errNum = WLZ_ERR_PARAM_DATA;
  }
  else if((basis = (WlzBasisFnTransform *)
	           AlcCalloc(sizeof(WlzBasisFnTransform), 1)) == NULL)
  {
    errNum = WLZ_ERR_MEM_ALLOC;
  }
  else
  {
    if((basisFn = (WlzBasisFn *)
	AlcCalloc(sizeof(WlzBasisFn), 1)) == NULL){
      AlcFree(basis);
      basis = NULL;
      errNum = WLZ_ERR_MEM_ALLOC;
    }
    else {
     basis->type = WLZ_TRANSFORM_2D_BASISFN;
     basis->linkcount = 0;
     basis->freeptr = NULL;
     basisFn->type = WLZ_FN_BASIS_2DCONF_POLY;
     basisFn->nPoly = order;
     basisFn->nBasis = 0;
     basisFn->nVtx = 0;
     basisFn->param = AlcMalloc(sizeof(double));
     *((double *) basisFn->param) = 0.0;
    }
  }

  if(errNum == WLZ_ERR_NONE)
  {
    nCoef = (order + 1);
    if(((wMx = (double *)AlcCalloc(sizeof(double), nCoef)) == NULL) ||
       ((bMx = (double *)AlcMalloc(sizeof(double) * nPts)) == NULL) ||
       ((aMx.rect = AlgMatrixRectNew(nPts, nCoef, NULL)) == NULL) ||
       ((vMx.rect = AlgMatrixRectNew(nPts, nCoef, NULL)) == NULL) ||
       ((basisFn->poly.v = AlcMalloc(sizeof(WlzDVertex2) * nCoef)) == NULL))
    {
      errNum = WLZ_ERR_MEM_ALLOC;
    }
  }

  if(errNum == WLZ_ERR_NONE)
  {
    aA = aMx.rect->array;
    vA = vMx.rect->array;
    /* Fill matrix A. */
    for(idM = 0; idM < nPts; ++idM)
    {
      idN = 0;
      powVx.vtX = 1.0;
      sVx = *(sPts + idM);
      for(idX = 0; idX <= basisFn->nPoly; ++idX)
      {
	*(*(aA + idM) + idX) = powVx.vtX;
	powVx.vtX *= sVx.vtX;
      }
    }
    /* Perform singular value decomposition of matrix A. */
    errNum= WlzErrorFromAlg(AlgMatrixSVDecomp(aMx, wMx, vMx));
  }
  if(errNum == WLZ_ERR_NONE)
  {
    /* Edit the singular values. */
    wMax = 0.0;
    for(idN = 0; idN < nCoef; ++idN)
    {
      if(*(wMx + idN) > wMax)
      {
	wMax = *(wMx + idN);
      }
    }
    thresh = tol * wMax;
    for(idN = 0; idN < nCoef; ++idN)
    {
      if(*(wMx + idN) < thresh)
      {
	*(wMx + idN) = 0.0;
      }
    }
    /* Fill matrix b for x coordinate */
    for(idM = 0; idM < nPts; ++idM)
    {
      *(bMx + idM) = (dPts + idM)->vtX;
    }
    /* Solve for x polynomial coefficients. */
    errNum = WlzErrorFromAlg(AlgMatrixSVBackSub(aMx, wMx, vMx,
    					        bMx));
  }
  if(errNum == WLZ_ERR_NONE)
  {
    /* Copy out the x polynomial coefficients, fill matrix b for
       y coordinate and re-solve. */
    for(idN = 0; idN < nCoef; ++idN)
    {
      (basisFn->poly.d2 + idN)->vtX = *(bMx + idN);
    }
    for(idM = 0; idM < nPts; ++idM)
    {
      *(bMx + idM) = (dPts + idM)->vtY;
    }
    errNum = WlzErrorFromAlg(AlgMatrixSVBackSub(aMx, wMx, vMx,
    						bMx));
  }
  if(errNum == WLZ_ERR_NONE)
  {
    /* Copy out the ypolynomial coefficients. */
    for(idN = 0; idN < nCoef; ++idN)
    {
      (basisFn->poly.d2 + idN)->vtY = *(bMx + idN);
    }
  }
  if(bMx)
  {
    AlcFree(bMx);
  }
  if(wMx)
  {
    AlcFree(wMx);
  }
  AlgMatrixFree(aMx);
  AlgMatrixFree(vMx);

  if(errNum != WLZ_ERR_NONE)
  {
    if(basisFn)
    {
      if(basisFn->poly.v)
      {
	AlcFree(basisFn->poly.v);
      }
      if(basisFn->param){
	AlcFree(basisFn->param);
      }
      AlcFree(basisFn);
    }
    if( basis ){
      AlcFree(basis);
      basis = NULL;
    }
  }

  if( dstErr ){
    *dstErr = errNum;
  }
  return basis;
}