/*virtual*/ void PCLDepthToMeters::_Initialize (const Arguments& parameters) {
CheckParameters (parameters, _parameters);
_parameters = _runtime.CopyArguments (parameters);
_input_args.push_back (ShortMatrix (GetRuntime()).SetName ("DepthMatrix").SetDescription ("Depth matrix"));
_output_args.push_back (DoubleMatrix (GetRuntime()).SetName ("MetersMatrix").SetDescription ("Real world coordinates matrix in meters"));
}
int ReadDatad(int numdat, double *xin, double *yin, double *zin)
{
double temp[3], minx, maxx, miny, maxy, xtmp, ytmp, ztmp;
double qtxy, qtyx, qtzx, qtzy;
int i0, i1, n0;
bigtri[0][0] = bigtri[0][1] = bigtri[1][1] = bigtri[2][0] = -1;
bigtri[1][0] = bigtri[2][1] = 5;
if (rootdat EQ NULL)
{
rootdat = IMakeDatum();
if (error_status) return (error_status);
rootsimp = IMakeSimp();
if (error_status) return (error_status);
roottemp = IMakeTemp();
if (error_status) return (error_status);
rootneig = IMakeNeig();
if (error_status) return (error_status);
rootdat->values[0] = rootdat->values[1]
= rootdat->values[2]
= 0;
}
else
{
FreeVecti(jndx);
FreeMatrixd(points);
FreeMatrixd(joints);
}
curdat = rootdat;
datcnt = 0;
minx = xstart - horilap; maxx = xend + horilap;
miny = ystart - vertlap; maxy = yend + vertlap;
for (n0 = 0 ; n0 < numdat ; n0++) {
temp[0] = xin[n0];
temp[1] = yin[n0];
temp[2] = zin[n0];
if (temp[0] > minx AND temp[0] < maxx AND
temp[1] > miny AND temp[1] < maxy) {
if (curdat->nextdat EQ NULL)
{
curdat->nextdat = IMakeDatum();
if (error_status) return (error_status);
}
curdat = curdat->nextdat;
datcnt++;
for (i1 = 0; i1 < 3; i1++)
curdat->values[i1] = temp[i1];
}
}
if (datcnt > 3)
{
datcnt3 = datcnt + 3;
jndx = IntVect(datcnt3);
if (error_status) return (error_status);
sumx = sumy = sumz = sumx2 = sumy2 = sumxy = sumxz = sumyz = 0;
iscale = 0;
/*
* Calculate minimums and maximums of the input data accounting for
* the scale factors.
*
* For the initial calculations, we have:
*
* maxxy[0][0] = maximum x input data value
* maxxy[1][0] = minimum x input data value
* maxxy[0][1] = maximum y input data value
* maxxy[1][1] = minimum y input data value
* maxxy[0][2] = maximum z input data value
* maxxy[1][2] = minimum z input data value
*
*/
data_limits:
maxxy[0][0] = maxxy[0][1] = maxxy[0][2] =
-(maxxy[1][0] = maxxy[1][1] = maxxy[1][2] = BIGNUM);
curdat = rootdat->nextdat;
for (i0 = 0; i0 < datcnt; i0++)
{
xtmp = curdat->values[0] * magx;
if (maxxy[0][0] < xtmp)
maxxy[0][0] = xtmp;
if (maxxy[1][0] > xtmp)
maxxy[1][0] = xtmp;
ytmp = curdat->values[1] * magy;
if (maxxy[0][1] < ytmp)
maxxy[0][1] = ytmp;
if (maxxy[1][1] > ytmp)
maxxy[1][1] = ytmp;
ztmp = curdat->values[2] * magz;
if (maxxy[0][2] < ztmp)
maxxy[0][2] = ztmp;
if (maxxy[1][2] > ztmp)
maxxy[1][2] = ztmp;
curdat = curdat->nextdat;
}
/*
* Modify the mins and maxs based on the scale factors and overlap regions.
* to get the actual minimums and maximums of the data under consideration.
*/
if (maxxy[0][0] < maxx * magx)
maxxy[0][0] = maxx * magx;
if (maxxy[1][0] > minx * magx)
maxxy[1][0] = minx * magx;
if (maxxy[0][1] < maxy * magy)
maxxy[0][1] = maxy * magy;
if (maxxy[1][1] > miny * magy)
maxxy[1][1] = miny * magy;
/*
* Calculate the extents in x, y, and z.
*
* maxxy[0][0] = maximum x extent, including overlap regions.
* maxxy[0][1] = maximum y extent, including overlap regions.
* maxxy[0][2] = maximum z extent.
*/
for (i0 = 0 ; i0 < 3 ; i0++)
{
maxxy[0][i0] -= maxxy[1][i0];
}
maxhoriz = maxxy[0][0];
if (maxhoriz < maxxy[0][1])
maxhoriz = maxxy[0][1];
wbit = maxhoriz * EPSILON;
/*
* Calculate the ratio of the x extent by the y extent (qtxy) and
* the y extent by the x extent (qtyx) .
*/
qtxy = maxxy[0][0] / maxxy[0][1];
qtyx = 1./qtxy;
if ( (qtxy > (2.+EPSILON)) OR (qtyx > (2.+EPSILON)) )
{
if (auto_scale)
{
/*
* Readjust the scaling and recompute the data limits.
*/
iscale = 1;
if (qtxy > (2+EPSILON) )
{
magy *= qtxy;
}
else
{
magx *= qtyx;
}
magx_auto = magx;
magy_auto = magy;
magz_auto = magz;
goto data_limits;
}
else
{
/*
* Issue a warning and turn off gradient estimation.
*/
TooNarrow();
}
}
if (igrad)
{
qtzx = maxxy[0][2] / maxxy[0][0];
qtzy = maxxy[0][2] / maxxy[0][1];
if ( (qtzx > 60) OR (qtzy > 60) )
{
if (auto_scale)
{
/*
* Readjust the scaling and recompute the data limits. The X and Y
* scales have been appropriately adjusted by the time you get here,
* so dividing magz by either qtzx or qtzy will bring it in line.
*/
iscale = 1;
magz *= 1./qtzx;
magx_auto = magx;
magy_auto = magy;
magz_auto = magz;
goto data_limits;
}
else
{
/*
* Issue a warning and turn off gradient estimation.
*/
TooSteep();
}
}
if ( (qtzx < .017) OR (qtzy < .017) )
{
if (auto_scale)
{
/*
* Readjust the scaling and recompute the data limits. The X and Y
* scales have been appropriately adjusted by the time you get here,
* so dividing magz by either qtzx or qtzy will bring it in line.
*/
iscale = 1;
magz *= 1./qtzx;
magx_auto = magx;
magy_auto = magy;
magz_auto = magz;
goto data_limits;
}
else
{
/*
* Issue a warning and turn off gradient estimation.
*/
TooShallow();
}
}
}
if (igrad)
{
points = DoubleMatrix(datcnt+4, 6);
if (error_status) return (error_status);
}
else
{
points = DoubleMatrix(datcnt+4, 3);
if (error_status) return (error_status);
}
joints = DoubleMatrix(datcnt3, 2);
if (error_status) return (error_status);
curdat = rootdat->nextdat;
rootdat->nextdat = NULL;
for (i0 = 0; i0 < datcnt; i0++)
{ sumx += points[i0][0] =
curdat->values[0] * magx;
sumx2 += SQ(points[i0][0]);
sumy += points[i0][1] =
curdat->values[1] * magy;
sumy2 += SQ(points[i0][1]);
sumxy += points[i0][0] * points[i0][1];
if (densi) points[i0][2] = 1;
else
{ sumz += points[i0][2] =
curdat->values[2] * magz;
sumxz += points[i0][0] * points[i0][2];
sumyz += points[i0][1] * points[i0][2];
}
holddat = curdat;
curdat = curdat->nextdat;
free(holddat);
}
det = (datcnt * (sumx2 * sumy2 - sumxy * sumxy))
- (sumx * (sumx * sumy2 - sumy * sumxy))
+ (sumy * (sumx * sumxy - sumy * sumx2));
aaa = ((sumz * (sumx2 * sumy2 - sumxy * sumxy))
- (sumxz * (sumx * sumy2 - sumy * sumxy))
+ (sumyz * (sumx * sumxy - sumy * sumx2))) /
det;
bbb =
((datcnt * (sumxz * sumy2 - sumyz * sumxy))
- (sumz * (sumx * sumy2 - sumy * sumxy))
+ (sumy * (sumx * sumyz - sumy * sumxz))) /
det;
ccc =
((datcnt * (sumx2 * sumyz - sumxy * sumxz))
- (sumx * (sumx * sumyz - sumy * sumxz))
+ (sumz * (sumx * sumxy - sumy * sumx2))) /
det;
for (i0 = 0 ; i0 < 3 ; i0++)
{ points[datcnt+i0][0] = maxxy[1][0] +
bigtri[i0][0] * maxxy[0][0] * RANGE;
points[datcnt+i0][1] = maxxy[1][1] +
bigtri[i0][1] * maxxy[0][1] * RANGE;
if (densi)
points[datcnt+i0][2] = 1;
else
points[datcnt+i0][2] =
aaa + bbb * points[datcnt+i0][0] +
ccc * points[datcnt+i0][1];
}
rootdat = NULL;
}
else
{
ErrorHnd(1, "ReadData", filee, "\n");
error_status = 1;
return (error_status);
}
/*
* Determine if any input data coordinates are duplicated.
*/
if (nndup == 1) {
for (i0 = 0 ; i0 < datcnt ; i0++) {
for (i1 = i0+1 ; i1 < datcnt ; i1++) {
if ( (points[i0][0] == points[i1][0]) &&
(points[i0][1] == points[i1][1]) )
{
sprintf(emsg,"\n Coordinates %d and %d are identical.\n",i0,i1);
ErrorHnd(2, "ReadData", filee, emsg);
error_status = 2;
return (error_status);
}
}
}
}
/*
* Introduce a small random perturbation into the coordinate values.
*/
srand(367);
for (i0 = 0 ; i0 < datcnt ; i0++)
{
for (i1 = 0 ; i1 < 2 ; i1++)
{
points[i0][i1] += wbit * (0.5 - (double)rand() / RAND_MAX);
}
}
if (sdip OR igrad)
{
piby2 = 2 * atan(1.0);
nn_pi = piby2 * 2;
piby32 = 3 * piby2;
rad2deg = 90 / piby2;
}
return (0);
}