/*! * \return Woolz error code. * \ingroup WlzTransform * \brief Fits a plane to the given vertices using singular value * decomposition. The best fit plane is returned (provided * there is no error) as a unit normal \f$n\f$ and the * centroid of the given vertices ( \f$r_0\f$ ) which in a * point in the plane, with the plane equation * \f[ \vec{n}\cdot(\vec{r} - \vec{r_0}) = \vec{0} \f] * \param vtxType Given vertex type. * \param nVtx Number of given vertices. * \param vtx Given vertices. * \param dstPinP Destination pointer for the return * of the median point (\f$r_0\f$) in * the plane. * \param dstNrm Destination pointer for the unit * normal to the plane (\f$n\f$). */ WlzErrorNum WlzFitPlaneSVD(WlzVertexType vtxType, int nVtx, WlzVertexP vtx, WlzDVertex3 *dstPinP, WlzDVertex3 *dstNrm) { AlgMatrix aMat, vMat; WlzDVertex3 centroid, normal; double *sVec = NULL; AlgError algErr = ALG_ERR_NONE; WlzErrorNum errNum = WLZ_ERR_NONE; aMat.core = NULL; vMat.core = NULL; if(nVtx < 0) { errNum = WLZ_ERR_PARAM_DATA; } else if(vtx.v == NULL) { errNum = WLZ_ERR_PARAM_NULL; } if(errNum == WLZ_ERR_NONE) { if(((sVec = (double *)AlcCalloc(sizeof(double), 3)) == NULL) || ((aMat.rect = AlgMatrixRectNew(nVtx, 3, NULL)) == NULL) || ((vMat.rect = AlgMatrixRectNew(3, 3, NULL)) == NULL)) { errNum = WLZ_ERR_MEM_ALLOC; } } /* Compute a Nx3 matrix from the given vertices then subtract their * centroid. */ if(errNum == WLZ_ERR_NONE) { int i; double *row; WLZ_VTX_3_ZERO(centroid); switch(vtxType) { case WLZ_VERTEX_I3: for(i = 0; i < nVtx; ++i) { WlzIVertex3 *v; v = vtx.i3 + i; row = aMat.rect->array[i]; centroid.vtX += row[0] = v->vtX; centroid.vtY += row[1] = v->vtY; centroid.vtZ += row[2] = v->vtZ; } break; case WLZ_VERTEX_F3: for(i = 0; i < nVtx; ++i) { WlzFVertex3 *v; v = vtx.f3 + i; row = aMat.rect->array[i]; centroid.vtX += row[0] = v->vtX; centroid.vtY += row[1] = v->vtY; centroid.vtZ += row[2] = v->vtZ; } break; case WLZ_VERTEX_D3: for(i = 0; i < nVtx; ++i) { WlzDVertex3 *v; v = vtx.d3 + i; row = aMat.rect->array[i]; centroid.vtX += row[0] = v->vtX; centroid.vtY += row[1] = v->vtY; centroid.vtZ += row[2] = v->vtZ; } break; default: errNum = WLZ_ERR_PARAM_TYPE; break; } if(errNum == WLZ_ERR_NONE) { WLZ_VTX_3_SCALE(centroid, centroid, (1.0 / nVtx)); for(i = 0; i < nVtx; ++i) { row = aMat.rect->array[i]; row[0] -= centroid.vtX; row[1] -= centroid.vtY; row[2] -= centroid.vtZ; } } } /* Compute SVD. */ if(errNum == WLZ_ERR_NONE) { algErr = AlgMatrixSVDecomp(aMat, sVec, vMat); errNum = WlzErrorFromAlg(algErr); } /* Find the vector corresponding to the least singular value. */ if(errNum == WLZ_ERR_NONE) { int i, m; double len; double **ary; const double eps = 1.0e-06; m = 0; for(i = 1; i < 3; ++i) { if(sVec[i] < sVec[m]) { m = i; } } ary = vMat.rect->array; #ifdef WLZ_FITPLANESVD_DEBUG (void )fprintf(stderr, "WlzFitPlaneSVD() singular values = {%lg, %lg, %lg}\n", sVec[0], sVec[1], sVec[2]); (void )fprintf(stderr, "WlzFitPlaneSVD() index of min singular value = %d\n", m); (void )fprintf(stderr, "WlzFitPlaneSVD() singular vectors = \n" "{\n"); for(i = 0; i < 3; ++i) { (void )fprintf(stderr, " {%lg, %lg, %lg}\n", ary[i][0], ary[i][1], ary[i][2]); } (void )fprintf(stderr, "}\n"); #endif /* WLZ_FITPLANESVD_DEBUG */ normal.vtX = ary[0][m]; normal.vtY = ary[1][m]; normal.vtZ = ary[2][m]; len = WLZ_VTX_3_LENGTH(normal); if(len < eps) { errNum = WLZ_ERR_ALG_SINGULAR; } else { WLZ_VTX_3_SCALE(normal, normal, (1.0 / len)); } } AlgMatrixRectFree(aMat.rect); AlgMatrixRectFree(vMat.rect); AlcFree(sVec); if(errNum == WLZ_ERR_NONE) { if(dstNrm) { *dstNrm = normal; } if(dstPinP) { *dstPinP = centroid; } } return(errNum); }
/*! * \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); }
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; }