/*!
* \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);
}
/*!
* \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);
}
