void EnhancedSGM::computeUVCache()
{
for (int y = 0; y < _params.yMax; y++)
{
for (int x = 0; x < _params.xMax; x++)
{
int idx = getLinearIndex(x, y);
if (not _maskVec[idx]) continue;
CurveRasterizer<int, Polynomial2> raster = getCurveRasteriser2(idx);
raster.steps(-DISPARITY_MARGIN);
const int u_vCacheStep = _params.dispMax + 2 * DISPARITY_MARGIN;
int32_t * uPtr = (int32_t *)_uCache.row(y).data + x*u_vCacheStep;
int32_t * vPtr = (int32_t *)_vCache.row(y).data + x*u_vCacheStep;
for (int i = 0; i < u_vCacheStep; i++, raster.step(), uPtr++, vPtr++)
{
if (raster.v < 0 or raster.v >= _params.vMax
or raster.u < 0 or raster.u >= _params.uMax)
{
// coordinate is out of the image
*uPtr = -1;
*vPtr = -1;
}
else
{
// coordinate is within the image
*uPtr = raster.u;
*vPtr = raster.v;
}
}
}
}
}
void Chunk::copyRange( const size_t source_start[], const size_t source_end[], Chunk &dst, const size_t destination[] ) const
{
LOG_IF( ! isInRange( source_start ), Debug, error )
<< "Copy start " << util::vector4<size_t>( source_start )
<< " is out of range (" << getSizeAsString() << ") at the source chunk";
LOG_IF( ! isInRange( source_end ), Debug, error )
<< "Copy end " << util::vector4<size_t>( source_end )
<< " is out of range (" << getSizeAsString() << ") at the source chunk";
LOG_IF( ! dst.isInRange( destination ), Debug, error )
<< "Index " << util::vector4<size_t>( destination )
<< " is out of range (" << getSizeAsString() << ") at the destination chunk";
const size_t sstart = getLinearIndex( source_start );
const size_t send = getLinearIndex( source_end );
const size_t dstart = dst.getLinearIndex( destination );
getValueArrayBase().copyRange( sstart, send, *dst, dstart );
}
EPP_GLOBAL void clearSNSGOutputAll(
unsigned int capacity,
unsigned int *output
) {
unsigned int self = getLinearIndex();
clearSNSGOutput( capacity, output, self );
}
EPP_GLOBAL void getRequiredSNSGSizeAll(
unsigned int *output,
SliceIndex *minmax
) {
unsigned int self = getLinearIndex();
getRequiredSNSGSize( output, minmax, self );
}
EPP_GLOBAL void clearSNSGAll(
unsigned int capacity,
unsigned int *buffer
) {
unsigned int self = getLinearIndex();
clearSNSG( capacity, buffer, self );
}
EPP_GLOBAL void calcOffsetAll(
SliceIndex *minmax,
unsigned int *current_top
) {
unsigned int self = getLinearIndex();
calcOffset( minmax, self, current_top );
}
EPP_GLOBAL void clearMinMaxAll(
unsigned int x_boxel_count,
unsigned int y_boxel_count,
SliceIndex *minmax
) {
unsigned int self = getLinearIndex();
clearMinMax( x_boxel_count, y_boxel_count, minmax, self );
}
size_t Chunk::compareRange( const size_t source_start[], const size_t source_end[], const Chunk &dst, const size_t destination[] ) const
{
LOG_IF( ! isInRange( source_start ), Debug, error )
<< "memcmp start " << util::vector4<size_t>( source_start )
<< " is out of range (" << getSizeAsString() << ") at the first chunk";
LOG_IF( ! isInRange( source_end ), Debug, error )
<< "memcmp end " << util::vector4<size_t>( source_end )
<< " is out of range (" << getSizeAsString() << ") at the first chunk";
LOG_IF( ! dst.isInRange( destination ), Debug, error )
<< "Index " << util::vector4<size_t>( destination )
<< " is out of range (" << getSizeAsString() << ") at the second chunk";
LOG( Debug, verbose_info )
<< "Comparing range from " << util::vector4<size_t>( source_start ) << " to " << util::vector4<size_t>( source_end )
<< " and " << util::vector4<size_t>( destination );
const size_t sstart = getLinearIndex( source_start );
const size_t send = getLinearIndex( source_end );
const size_t dstart = dst.getLinearIndex( destination );
return getValueArrayBase().compare( sstart, send, dst.getValueArrayBase(), dstart );
}
EPP_GLOBAL void detectMinMaxAll(
unsigned int x_boxel_count,
unsigned int y_boxel_count,
unsigned int z_boxel_count,
SliceIndex *minmax,
float boxel_size,
Particle *_particles
) {
unsigned int self = getLinearIndex();
detectMinMax( x_boxel_count, y_boxel_count, z_boxel_count, minmax, boxel_size, _particles[ self ].position );
}
EPP_GLOBAL void getSNSGAll(
float boxel_size, unsigned int capacity,
unsigned int x_boxel_count,
unsigned int y_boxel_count,
unsigned int z_boxel_count,
unsigned int *buffer,
unsigned int *_output, Particle *_particles,
SliceIndex *slice_index
) {
unsigned int self = getLinearIndex();
getSNSG( boxel_size, capacity, x_boxel_count, y_boxel_count, z_boxel_count, buffer, _output + ( capacity * 27 * self ), self, _particles[ self ].position, slice_index );
}
//TODO reconstruct the depth points, not everything
void EnhancedSGM::computeReconstructed()
{
_pointVec1.resize(_params.yMax*_params.xMax);
_pointPxVec1.resize(_params.yMax*_params.xMax);
for (int y = 0; y < _params.yMax; y++)
{
for (int x = 0; x < _params.xMax; x++)
{
int idx = getLinearIndex(x, y);
_pointVec1[idx] = Vector2d(_params.uConv(x), _params.vConv(y));
_pointPxVec1[idx] = Vector2i(_params.uConv(x), _params.vConv(y));
}
}
_camera1->reconstructPointCloud(_pointVec1, _reconstVec, _maskVec);
}
IMDHistoWorkspace_sptr ReflectometryTransform::executeMDNormPoly(
MatrixWorkspace_const_sptr inputWs) const {
auto input_x_dim = inputWs->getXDimension();
MDHistoDimension_sptr dim0 = MDHistoDimension_sptr(new MDHistoDimension(
input_x_dim->getName(), input_x_dim->getDimensionId(),
input_x_dim->getMDFrame(),
static_cast<Mantid::coord_t>(input_x_dim->getMinimum()),
static_cast<Mantid::coord_t>(input_x_dim->getMaximum()),
input_x_dim->getNBins()));
auto input_y_dim = inputWs->getYDimension();
MDHistoDimension_sptr dim1 = MDHistoDimension_sptr(new MDHistoDimension(
input_y_dim->getName(), input_y_dim->getDimensionId(),
input_y_dim->getMDFrame(),
static_cast<Mantid::coord_t>(input_y_dim->getMinimum()),
static_cast<Mantid::coord_t>(input_y_dim->getMaximum()),
input_y_dim->getNBins()));
auto outWs = boost::make_shared<MDHistoWorkspace>(dim0, dim1);
for (size_t nHistoIndex = 0; nHistoIndex < inputWs->getNumberHistograms();
++nHistoIndex) {
const MantidVec X = inputWs->readX(nHistoIndex);
const MantidVec Y = inputWs->readY(nHistoIndex);
const MantidVec E = inputWs->readE(nHistoIndex);
for (size_t nBinIndex = 0; nBinIndex < inputWs->blocksize(); ++nBinIndex) {
auto value_index = outWs->getLinearIndex(nBinIndex, nHistoIndex);
outWs->setSignalAt(value_index, Y[nBinIndex]);
outWs->setErrorSquaredAt(value_index, E[nBinIndex] * E[nBinIndex]);
}
}
return outWs;
}
void EnhancedSGM::computeCurveCost(const Mat8u & img1, const Mat8u & img2)
{
if (_params.verbosity > 0) cout << "EnhancedSGM::computeCurveCost" << endl;
// compute the weights for matching cost
if (_params.salientPoints) _salientBuffer.setTo(0);
for (int y = 0; y < _params.yMax; y++)
{
for (int x = 0; x < _params.xMax; x++)
{
int idx = getLinearIndex(x, y);
if (_params.verbosity > 4)
{
cout << " x: " << x << " y: " << y << " idx: " << idx;
cout << " mask: " << _maskVec[idx] << endl;
}
if (not _maskVec[idx])
{
uint8_t * outPtr = _errorBuffer.row(y).data + x*_params.dispMax;
*outPtr = 0;
fill(outPtr + 1, outPtr + _params.dispMax, 255);
continue;
}
// compute the local image descriptor,
// a piece of the epipolar curve on the first image
vector<uint8_t> descriptor;
CurveRasterizer<int, Polynomial2> descRaster = getCurveRasteriser1(idx);
const int step = _epipolarDescriptor.compute(img1, descRaster, descriptor);
_stepBuffer(y, x) = step;
if (step < 1)
{
//TODO make a function
uint8_t * outPtr = _errorBuffer.row(y).data + x*_params.dispMax;
*outPtr = 0;
fill(outPtr + 1, outPtr + _params.dispMax, 255);
continue;
}
if (_params.imageBasedCost)
{
switch (step)
{
case 1:
_costBuffer(y, x) = _params.lambdaJump;
break;
case 2:
_costBuffer(y, x) = _params.lambdaJump * 3;
break;
default:
_costBuffer(y, x) = _params.lambdaJump * 6;
break;
}
}
//TODO revise the criterion (step == 1)
if (_params.salientPoints and step <= 2)
{
_salientBuffer(y, x) = 1;
}
const int nSteps = ( _params.dispMax + step - 1 ) / step;
//sample the curve
vector<uint8_t> sampleVec(nSteps + MARGIN, 0);
if (_params.useUVCache)
{
const int u_vCacheStep = _params.dispMax + 2 * DISPARITY_MARGIN;
int32_t * uPtr = (int32_t *)_uCache.row(y).data + x*u_vCacheStep;
int32_t * vPtr = (int32_t *)_vCache.row(y).data + x*u_vCacheStep;
uPtr += DISPARITY_MARGIN - HALF_LENGTH * step;
vPtr += DISPARITY_MARGIN - HALF_LENGTH * step;
for (int i = 0; i < nSteps + MARGIN; i++, uPtr += step, vPtr += step)
{
if (*uPtr < 0 or *vPtr < 0) sampleVec[i] = 0;
else sampleVec[i] = img2(*vPtr, *uPtr);
}
}
else
{
CurveRasterizer<int, Polynomial2> raster = getCurveRasteriser2(idx);
raster.setStep(step);
raster.steps(-HALF_LENGTH);
for (int i = 0; i < nSteps + MARGIN; i++, raster.step())
{
if (raster.v < 0 or raster.v >= img2.rows
or raster.u < 0 or raster.u >= img2.cols) sampleVec[i] = 0;
else sampleVec[i] = img2(raster.v, raster.u);
}
}
vector<int> costVec = compareDescriptor(descriptor, sampleVec, _params.flawCost);
if (y == 350 and x > 469 and x < 481)
{
cout << "Point : " << x << " " << y << endl;
cout << "Step : " << step << endl;
cout << "samples :" << endl;
for (auto & x : sampleVec)
{
cout << setw(6) << int(x);
}
cout << endl;
cout << "cost :" << endl;
for (auto & x : costVec)
{
cout << setw(6) << int(x);
}
cout << endl;
cout << "descriptor :" << endl;
for (auto & x : descriptor)
{
cout << setw(6) << int(x);
}
cout << endl;
}
// //compute the bias;
// int sum1 = filter(kernelVec.begin(), kernelVec.end(), descriptor.begin(), 0);
// fill up the cost buffer
uint8_t * outPtr = _errorBuffer.row(y).data + x*_params.dispMax;
auto costIter = costVec.begin() + HALF_LENGTH;
for (int d = 0; d < nSteps; d++, outPtr += step)
{
// int sum2 = filter(kernelVec.begin(), kernelVec.end(), sampleVec.begin() + d, 0);
// int bias = min(_params.maxBias, max(-_params.maxBias, (sum2 - sum1) / LENGTH));
// int acc = biasedAbsDiff(kernelVec.begin(), kernelVec.end(),
// descriptor.begin(), sampleVec.begin() + d, bias);
// *outPtr = acc / NORMALIZER;
*outPtr = *costIter / _params.descLength;
++costIter;
}
if (step > 1) fillGaps(_errorBuffer.row(y).data + x*_params.dispMax, step);
}
}
}
void EnhancedSGM::reconstructDepth(DepthMap & depth) const
{
if (_params.verbosity > 2)
{
cout << "EnhancedSGM::reconstructDepth(DepthMap & depth)" << endl;
}
depth = DepthMap(_camera1, _params, _params.hypMax);
for (int h = 0; h < _params.hypMax; h++)
{
for (int y = 0; y < _params.yMax; y++)
{
for (int x = 0; x < _params.xMax; x++)
{
if (_params.salientPoints and not _salientBuffer(y, x))
{
depth.at(x, y, h) = OUT_OF_RANGE;
depth.sigma(x, y, h) = OUT_OF_RANGE;
depth.cost(x, y, h) = OUT_OF_RANGE;
continue;
}
depth.cost(x, y, h) = _errorBuffer(y, x*_params.dispMax + h);
int idx = getLinearIndex(x, y);
if (not _maskVec[idx])
{
depth.at(x, y, h) = OUT_OF_RANGE;
depth.sigma(x, y, h) = OUT_OF_RANGE;
depth.cost(x, y, h) = OUT_OF_RANGE;
continue;
}
int disparity = _smallDisparity(y, x*_params.hypMax + h);
// point on the first image
const auto & pt1 = _pointVec1[idx];
// to compute point on the second image
// TODO make virtual rasterizer using the cache
int u21, v21, u22, v22;
int step = _stepBuffer(y, x);
if (_params.useUVCache)
{
const int u_vCacheStep = _params.dispMax + 2 * DISPARITY_MARGIN;
u21 = _uCache(y, x*u_vCacheStep + DISPARITY_MARGIN + disparity);
u22 = _uCache(y, x*u_vCacheStep + DISPARITY_MARGIN + disparity + step);
v21 = _vCache(y, x*u_vCacheStep + DISPARITY_MARGIN + disparity);
v22 = _vCache(y, x*u_vCacheStep + DISPARITY_MARGIN + disparity + step);
}
else
{
CurveRasterizer<int, Polynomial2> raster = getCurveRasteriser2(idx);
raster.steps(disparity);
u21 = raster.u;
v21 = raster.v;
raster.steps(step);
u22 = raster.u;
v22 = raster.v;
}
triangulate(pt1[0], pt1[1], u21, v21, u22, v22,
depth.at(x, y, h), depth.sigma(x, y, h));
}
}
}
}
EPP_GLOBAL void calcSizeAll(
SliceIndex *minmax
) {
unsigned int self = getLinearIndex();
calcSize( minmax, self );
}