You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
424 lines
12 KiB
424 lines
12 KiB
#include "model/processor/strategies/ScanConversion.h"
|
|
#include <QPixmap>
|
|
#include <QImage>
|
|
#include "model/processor/BIP.h"
|
|
|
|
ScanConversion::ScanConversion(const Context context,
|
|
const QString kernelPath,
|
|
const QObject *parent = Q_NULLPTR) :
|
|
IProcessStrategy(context, kernelPath, "ScanConversion", parent),
|
|
_kernelFunctor(KernelFunctor<Image2D, Image2D,
|
|
Buffer, Buffer, Buffer, Buffer, ScanConversion_t>(_kernel))
|
|
{
|
|
_gridPixelXPos = Q_NULLPTR;
|
|
_gridPixelZPos = Q_NULLPTR;
|
|
_scanXPos = Q_NULLPTR;
|
|
_scanZPos = Q_NULLPTR;
|
|
|
|
_gridX = Q_NULLPTR;
|
|
_gridZ = Q_NULLPTR;
|
|
_queryX = Q_NULLPTR;
|
|
_queryZ = Q_NULLPTR;
|
|
|
|
_gridPixelR = Q_NULLPTR;
|
|
_gridPixelTheta = Q_NULLPTR;
|
|
}
|
|
|
|
void ScanConversion::cpuProcess(ScenGenOutput_t params)
|
|
{
|
|
auto context = _openCLHelper.getContext();
|
|
auto queue = BIP::getInstance()->CLQueue;
|
|
//virtual convex
|
|
if(params.virtualConvex.value)
|
|
{
|
|
if(params.outputWidth.isUpdated|| params.outputHeight.isUpdated)
|
|
{
|
|
if(_gridPixelXPos)
|
|
delete[] _gridPixelXPos;
|
|
|
|
if(_gridPixelZPos)
|
|
delete[] _gridPixelZPos;
|
|
|
|
_gridPixelXPos = new myflt[params.outputWidth.value *
|
|
params.outputHeight.value];
|
|
_gridPixelZPos = new myflt[params.outputWidth.value *
|
|
params.outputHeight.value];
|
|
|
|
_width = params.outputWidth.value;
|
|
_height = params.outputHeight.value;
|
|
|
|
_kernelParameters.width = _width;
|
|
}
|
|
|
|
if(params.depth.isUpdated || params.startDepth.isUpdated ||
|
|
params.fieldOfView.isUpdated || params.vcMaxTheta.isUpdated ||
|
|
params.outputWidth.isUpdated || params.outputHeight.isUpdated)
|
|
{
|
|
auto maxZ = params.depth.value;
|
|
auto minZ = params.startDepth.value * cosf(params.vcMaxTheta.value);
|
|
auto maxX = params.fieldOfView.value / 2 + maxZ * sinf(params.vcMaxTheta.value);
|
|
auto minX = -1 * maxX;
|
|
|
|
auto baseZ = (maxZ - minZ) / (params.outputHeight.value - 1);
|
|
auto baseX = (maxX - minX) / (params.outputWidth.value - 1);
|
|
|
|
for(auto i = 0; i < params.outputHeight.value; i++)
|
|
{
|
|
auto temp = i * baseZ;
|
|
for(auto j = 0; j < params.outputWidth.value; j++)
|
|
{
|
|
_gridPixelZPos[i * params.outputHeight.value + j] = minZ + temp;
|
|
_gridPixelXPos[i * params.outputHeight.value + j] = minX + (baseX * j);
|
|
}
|
|
}
|
|
|
|
auto virtualOriginZ = params.fieldOfView.value / 2 / tanf(params.vcMaxTheta.value);
|
|
|
|
if(_gridPixelR)
|
|
delete[] _gridPixelR;
|
|
|
|
if(_gridPixelTheta)
|
|
delete[] _gridPixelTheta;
|
|
|
|
_gridPixelR = new myflt[params.outputWidth.value *
|
|
params.outputHeight.value];
|
|
_gridPixelTheta = new myflt[params.outputWidth.value *
|
|
params.outputHeight.value];
|
|
|
|
auto strTan = tanf(params.steering.value);
|
|
|
|
for(auto i = 0; i < params.outputWidth.value *
|
|
params.outputHeight.value; i++)
|
|
{
|
|
auto x = _gridPixelXPos[i];
|
|
auto z = _gridPixelZPos[i];
|
|
|
|
auto pixelTheta = atanf(x / (z + virtualOriginZ));
|
|
|
|
if(pixelTheta >= -params.vcMaxTheta.value - abs(params.steering.value) &&
|
|
pixelTheta <= params.vcMaxTheta.value + abs(params.steering.value))
|
|
{
|
|
if(params.steering.value == 0)
|
|
{
|
|
auto gridPixelAx = sqrtf(powf(x - virtualOriginZ * tanf(pixelTheta), 2) + powf(z, 2));
|
|
if(gridPixelAx >= params.startDepth.value &&
|
|
gridPixelAx <= params.depth.value)
|
|
{
|
|
_gridPixelR[i] = gridPixelAx;
|
|
_gridPixelTheta[i] = pixelTheta;
|
|
}
|
|
else
|
|
{
|
|
_gridPixelR[i] = 0;
|
|
_gridPixelTheta[i] = 0;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
auto a = virtualOriginZ * strTan;
|
|
auto b = x * strTan + virtualOriginZ + z;
|
|
auto c = x - z * strTan;
|
|
auto interceptTheta = atanf((b + sqrtf(powf(b, 2) - 4 * a * c)) / (2 * a));
|
|
if(interceptTheta > params.vcMaxTheta.value ||
|
|
interceptTheta < -params.vcMaxTheta.value)
|
|
{
|
|
interceptTheta = atanf((b - sqrtf(powf(b, 2) - 4 * a * c)) / (2 * a));
|
|
if(interceptTheta > params.vcMaxTheta.value ||
|
|
interceptTheta < -params.vcMaxTheta.value)
|
|
{
|
|
_gridPixelR[i] = 0;
|
|
_gridPixelTheta[i] = 0;
|
|
}
|
|
else
|
|
{
|
|
auto gridPixelAx = sqrtf(powf(x - virtualOriginZ * tanf(interceptTheta), 2) + powf(z, 2));
|
|
if(gridPixelAx >= params.startDepth.value &&
|
|
gridPixelAx <= params.depth.value)
|
|
{
|
|
_gridPixelR[i] = gridPixelAx;
|
|
_gridPixelTheta[i] = interceptTheta;
|
|
}
|
|
else
|
|
{
|
|
_gridPixelR[i] = 0;
|
|
_gridPixelTheta[i] = 0;
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
auto gridPixelAx = sqrtf(powf(x - virtualOriginZ * tanf(interceptTheta), 2) + powf(z, 2));
|
|
if(gridPixelAx >= params.startDepth.value &&
|
|
gridPixelAx <= params.depth.value)
|
|
{
|
|
_gridPixelR[i] = gridPixelAx;
|
|
_gridPixelTheta[i] = interceptTheta;
|
|
}
|
|
else
|
|
{
|
|
_gridPixelR[i] = 0;
|
|
_gridPixelTheta[i] = 0;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
_gridPixelR[i] = 0;
|
|
_gridPixelTheta[i] = 0;
|
|
}
|
|
}
|
|
|
|
if(_queryX)
|
|
delete _queryX;
|
|
|
|
if(_queryZ)
|
|
delete _queryZ;
|
|
_queryX = _openCLHelper.array2CLBuffer(context,
|
|
params.outputWidth.value *
|
|
params.outputHeight.value *
|
|
sizeof (myflt));
|
|
_queryZ = _openCLHelper.array2CLBuffer(context,
|
|
params.outputWidth.value *
|
|
params.outputHeight.value *
|
|
sizeof (myflt));
|
|
|
|
BIP::getInstance()->CLQueue.enqueueWriteBuffer(*_queryZ, CL_TRUE, 0,
|
|
params.outputWidth.value *
|
|
params.outputHeight.value *
|
|
sizeof (myflt),
|
|
_gridPixelR);
|
|
queue.enqueueWriteBuffer(*_queryX, CL_TRUE, 0,
|
|
params.outputWidth.value *
|
|
params.outputHeight.value *
|
|
sizeof (myflt),
|
|
_gridPixelTheta);
|
|
}
|
|
|
|
if(params.minScanAz.isUpdated || params.rxLineNo.isUpdated ||
|
|
params.rxLineDaz.isUpdated)
|
|
{
|
|
if(_scanXPos)
|
|
delete [] _scanXPos;
|
|
|
|
if(_gridX)
|
|
delete _gridX;
|
|
|
|
_scanXPos = new myflt[params.rxLineNo.value];
|
|
for(auto i = 0; i < params.rxLineNo.value; i++)
|
|
{
|
|
_scanXPos[i] = params.minScanAz.value + i * params.rxLineDaz.value;
|
|
}
|
|
|
|
_gridX = _openCLHelper.array2CLBuffer(context,
|
|
static_cast<quint64>(params.rxLineNo.value)
|
|
* sizeof (myflt));
|
|
|
|
queue.enqueueWriteBuffer(*_gridX, CL_TRUE, 0,
|
|
static_cast<unsigned long>(params.rxLineNo.value) * sizeof (myflt),
|
|
_scanXPos);
|
|
|
|
_kernelParameters.gridXSize = params.rxLineNo.value;
|
|
}
|
|
|
|
if(params.rxFocusPointNo.isUpdated || params.minScanAx.isUpdated ||
|
|
params.rxPointDax.isUpdated)
|
|
{
|
|
if(_scanZPos)
|
|
delete[] _scanZPos;
|
|
|
|
if(_gridZ)
|
|
delete _gridZ;
|
|
|
|
_scanZPos = new myflt [params.rxFocusPointNo.value];
|
|
for(auto i = 0; i < params.rxFocusPointNo.value; i++)
|
|
{
|
|
_scanZPos[i] = params.minScanAx.value + i * params.rxPointDax.value;
|
|
}
|
|
|
|
_gridZ = _openCLHelper.array2CLBuffer(context,
|
|
static_cast<quint64>(params.rxFocusPointNo.value)
|
|
* sizeof (myflt));
|
|
|
|
queue.enqueueWriteBuffer(*_gridZ, CL_TRUE, 0,
|
|
static_cast<unsigned long>(params.rxFocusPointNo.value) * sizeof (myflt),
|
|
_scanZPos);
|
|
|
|
_kernelParameters.gridZSize = params.rxFocusPointNo.value;
|
|
}
|
|
}
|
|
//linear non convex
|
|
else
|
|
{
|
|
if(params.outputWidth.isUpdated || params.outputHeight.isUpdated)
|
|
{
|
|
if(_gridPixelXPos)
|
|
delete[] _gridPixelXPos;
|
|
|
|
if(_gridPixelZPos)
|
|
delete[] _gridPixelZPos;
|
|
|
|
_gridPixelXPos = new myflt[params.outputWidth.value *
|
|
params.outputHeight.value];
|
|
_gridPixelZPos = new myflt[params.outputWidth.value *
|
|
params.outputHeight.value];
|
|
|
|
if(_queryX)
|
|
delete _queryX;
|
|
|
|
if(_queryZ)
|
|
delete _queryZ;
|
|
|
|
_queryX = _openCLHelper.array2CLBuffer(context,
|
|
params.outputWidth.value *
|
|
params.outputHeight.value *
|
|
sizeof (myflt));
|
|
_queryZ = _openCLHelper.array2CLBuffer(context,
|
|
params.outputWidth.value *
|
|
params.outputHeight.value *
|
|
sizeof (myflt));
|
|
|
|
_width = params.outputWidth.value;
|
|
_height = params.outputHeight.value;
|
|
|
|
_kernelParameters.width = _width;
|
|
}
|
|
|
|
if(params.depth.isUpdated || params.startDepth.isUpdated ||
|
|
params.fieldOfView.isUpdated || params.steering.isUpdated ||
|
|
params.outputWidth.isUpdated || params.outputHeight.isUpdated)
|
|
{
|
|
auto maxZ = params.depth.value;
|
|
auto minZ = params.startDepth.value;
|
|
auto maxX = params.fieldOfView.value / 2;
|
|
auto minX = -1 * maxX;
|
|
|
|
auto baseZ = (maxZ - minZ) / (params.outputHeight.value - 1);
|
|
auto baseX = (maxX - minX) / (params.outputWidth.value - 1);
|
|
|
|
for(auto i = 0; i < params.outputHeight.value; i++)
|
|
{
|
|
auto temp = i * baseZ;
|
|
auto tan_result = temp * tanf(params.steering.value);
|
|
for(auto j = 0; j < params.outputWidth.value; j++)
|
|
{
|
|
_gridPixelZPos[i * params.outputHeight.value + j] = minZ + temp;
|
|
_gridPixelXPos[i * params.outputHeight.value + j] = minX + (baseX * j) - tan_result;
|
|
}
|
|
}
|
|
|
|
BIP::getInstance()->CLQueue.enqueueWriteBuffer(*_queryX, CL_TRUE, 0,
|
|
params.outputWidth.value *
|
|
params.outputHeight.value *
|
|
sizeof (myflt),
|
|
_gridPixelXPos);
|
|
queue.enqueueWriteBuffer(*_queryZ, CL_TRUE, 0,
|
|
params.outputWidth.value *
|
|
params.outputHeight.value *
|
|
sizeof (myflt),
|
|
_gridPixelZPos);
|
|
}
|
|
|
|
if(params.fieldOfView.isUpdated || params.rxLineNo.isUpdated ||
|
|
params.rxLineDaz.isUpdated)
|
|
{
|
|
if(_scanXPos)
|
|
delete [] _scanXPos;
|
|
|
|
if(_gridX)
|
|
delete _gridX;
|
|
|
|
_scanXPos = new myflt[params.rxLineNo.value];
|
|
for(auto i = 0; i < params.rxLineNo.value; i++)
|
|
{
|
|
_scanXPos[i] = -params.fieldOfView.value / 2 + i * params.rxLineDaz.value;
|
|
}
|
|
|
|
_gridX = _openCLHelper.array2CLBuffer(context,
|
|
static_cast<quint64>(params.rxLineNo.value)
|
|
* sizeof (myflt));
|
|
|
|
queue.enqueueWriteBuffer(*_gridX, CL_TRUE, 0,
|
|
static_cast<unsigned long>(params.rxLineNo.value) * sizeof (myflt),
|
|
_scanXPos);
|
|
|
|
_kernelParameters.gridXSize = params.rxLineNo.value;
|
|
}
|
|
|
|
if(params.rxFocusPointNo.isUpdated || params.rxPointDax.isUpdated)
|
|
{
|
|
if(_scanZPos)
|
|
delete[] _scanZPos;
|
|
|
|
if(_gridZ)
|
|
delete _gridZ;
|
|
|
|
_scanZPos = new myflt [params.rxFocusPointNo.value];
|
|
for(auto i = 0; i < params.rxFocusPointNo.value; i++)
|
|
{
|
|
_scanZPos[i] = params.startDepth.value + i * params.rxPointDax.value;
|
|
}
|
|
|
|
_gridZ = _openCLHelper.array2CLBuffer(context,
|
|
static_cast<quint64>(params.rxFocusPointNo.value)
|
|
* sizeof (myflt));
|
|
|
|
queue.enqueueWriteBuffer(*_gridZ, CL_TRUE, 0,
|
|
static_cast<unsigned long>(params.rxFocusPointNo.value) * sizeof (myflt),
|
|
_scanZPos);
|
|
|
|
_kernelParameters.gridZSize = params.rxFocusPointNo.value;
|
|
}
|
|
}
|
|
}
|
|
|
|
void ScanConversion::finalize()
|
|
{
|
|
delete _gridPixelXPos;
|
|
delete _gridPixelZPos;
|
|
delete _scanXPos;
|
|
delete _scanZPos;
|
|
|
|
delete _gridX;
|
|
delete _gridZ;
|
|
delete _queryX;
|
|
delete _queryZ;
|
|
|
|
_gridPixelXPos = Q_NULLPTR;
|
|
_gridPixelZPos = Q_NULLPTR;
|
|
_scanXPos = Q_NULLPTR;
|
|
_scanZPos = Q_NULLPTR;
|
|
|
|
_gridX = Q_NULLPTR;
|
|
_gridZ = Q_NULLPTR;
|
|
_queryX = Q_NULLPTR;
|
|
_queryZ = Q_NULLPTR;
|
|
}
|
|
|
|
Image* ScanConversion::processKernel(Image *frames, Buffer* scrathPad)
|
|
{
|
|
Context context = _openCLHelper.getContext();
|
|
auto format = frames->getImageInfo<CL_IMAGE_FORMAT>();
|
|
auto imageOutput = new Image2D(context,
|
|
CL_MEM_READ_WRITE,
|
|
ImageFormat(format.image_channel_order, format.image_channel_data_type),
|
|
_width,
|
|
_height);
|
|
cl::EnqueueArgs eargs(BIP::getInstance()->CLQueue, cl::NDRange(_width, _height));
|
|
|
|
_openCLHelper.runKernelFunctor<Image2D, Image2D, Buffer, Buffer, Buffer,
|
|
Buffer, ScanConversion_t>(_kernelFunctor,
|
|
eargs,
|
|
*(Image2D*)(frames),
|
|
*imageOutput,
|
|
*_gridX,
|
|
*_gridZ,
|
|
*_queryX,
|
|
*_queryZ,
|
|
_kernelParameters);
|
|
delete frames;
|
|
|
|
return imageOutput;
|
|
}
|
|
|
|
|
|
|