Compare commits
7 Commits
2d5baef77d
...
6be62e224e
| Author | SHA1 | Date |
|---|---|---|
|
|
6be62e224e | 5 years ago |
|
|
ca8edc11ff | 6 years ago |
|
|
66ebeb24fb | 6 years ago |
|
|
6e028b49b8 | 6 years ago |
|
|
3b562bbba6 | 6 years ago |
|
|
5c20289472 | 6 years ago |
|
|
65636e447f | 6 years ago |
31 changed files with 959 additions and 572 deletions
@ -0,0 +1,35 @@ |
|||||
|
#ifndef SCANCONVERSIONALG_H |
||||
|
#define SCANCONVERSIONALG_H |
||||
|
|
||||
|
#include "./ScenarioParams.h" |
||||
|
|
||||
|
class ScanConversionAlg |
||||
|
{ |
||||
|
private: |
||||
|
static void linear(Input_t params, QVector<float>& scanXPos, QVector<float>& scanZPos, |
||||
|
QVector<float>& gridPixelXPos, QVector<float>& gridPixelZPos); |
||||
|
|
||||
|
static void virtualConvex(Input_t params, QVector<float>& scanXPos, QVector<float>& scanZPos, |
||||
|
QVector<float>& gridPixelXPos, QVector<float>& gridPixelZPos); |
||||
|
|
||||
|
static void convex(Input_t params, QVector<float>& scanXPos, QVector<float>& scanZPos, |
||||
|
QVector<float>& gridPixelXPos, QVector<float>& gridPixelZPos); |
||||
|
|
||||
|
public: |
||||
|
static void scanConversion(Input_t params, QVector<float>& scanXPos, QVector<float>& scanZPos, |
||||
|
QVector<float>& gridPixelXPos, QVector<float>& gridPixelZPos); |
||||
|
|
||||
|
static void virtualScanConversion(QVector<float> pixelXPos, QVector<float> pixelZPos, |
||||
|
uint width, uint height, |
||||
|
float steering, float virtualOriginalZ, |
||||
|
float startDepth, float depth, float vcMaxTheta, |
||||
|
QVector<float>& gridPixelR, QVector<float>& gridPixelTheta); |
||||
|
|
||||
|
static void convexScanConversion(QVector<float> pixelXPos, QVector<float> pixelZPos, |
||||
|
uint width, uint height, |
||||
|
float steering, float angle, float fieldOfView, |
||||
|
float probeRadius, float startDepth, float depth, |
||||
|
QVector<float>& gridPixelR, QVector<float>& gridPixelTheta); |
||||
|
}; |
||||
|
|
||||
|
#endif // SCANCONVERSIONALG_H
|
||||
@ -0,0 +1,35 @@ |
|||||
|
#ifndef CRI_H |
||||
|
#define CRI_H |
||||
|
|
||||
|
#include <QObject> |
||||
|
#include <QMetaType> |
||||
|
|
||||
|
#include "model/processor/IProcessStrategy.h" |
||||
|
#include "utils/OpenCLHelper.h" |
||||
|
|
||||
|
#define CRI_MAX_BUFFER_SIZE 16 |
||||
|
|
||||
|
|
||||
|
typedef struct Cri_t |
||||
|
{ |
||||
|
cl_int criFilterMode; |
||||
|
cl_int frameCntr; |
||||
|
cl_int scenariFrameNo; |
||||
|
}Cri_t; |
||||
|
|
||||
|
|
||||
|
class Cri : public IProcessStrategy |
||||
|
{ |
||||
|
Q_OBJECT |
||||
|
public: |
||||
|
Q_INVOKABLE Cri(const Context context, const QString kernelPath, const QObject *parent); |
||||
|
virtual void cpuProcess(Input_t parameters) override; |
||||
|
virtual void finalize() override; |
||||
|
|
||||
|
private: |
||||
|
KernelFunctor<Image2DArray, Image2D, Cri_t> _kernelFunctor; |
||||
|
virtual Image* processKernel(Image *frames, Buffer* scratchPad) override; |
||||
|
Cri_t _kernelParameters; |
||||
|
Image2D* _criBuffer[CRI_MAX_BUFFER_SIZE]; |
||||
|
}; |
||||
|
#endif // CRI_H
|
||||
@ -0,0 +1,16 @@ |
|||||
|
|
||||
|
struct input |
||||
|
{ |
||||
|
int criFilterMode; |
||||
|
int frameCntr; |
||||
|
int scenariFrameNo; |
||||
|
}; |
||||
|
|
||||
|
kernel void Cri(read_only image2d_array_t input_frame, read_write image2d_t output_frame, struct input params) |
||||
|
{ |
||||
|
const int rows = get_image_height(input_frame); |
||||
|
const int cols = get_image_width(input_frame); |
||||
|
int4 f = read_imagei(input_frame, (rows,cols,1,1)); |
||||
|
printf("out: %a | ", f.x); |
||||
|
// printf(" :hi! I am the cri kernel"); |
||||
|
} |
||||
@ -0,0 +1,518 @@ |
|||||
|
#include "model/algorithms/ScanConversionAlg.h" |
||||
|
|
||||
|
#include <QtMath> |
||||
|
|
||||
|
void ScanConversionAlg::linear(Input_t params, QVector<float> &scanXPos, QVector<float> &scanZPos, |
||||
|
QVector<float> &gridPixelXPos, QVector<float> &gridPixelZPos) |
||||
|
{ |
||||
|
//just to map to X Z convention
|
||||
|
auto frameMaxXScan = params.bMaxScanAz; |
||||
|
auto frameMinXScan = params.bMinScanAz; |
||||
|
auto frameMaxZScan = params.bMaxScanAx; |
||||
|
auto frameMinZScan = params.bMinScanAx; |
||||
|
|
||||
|
auto finalPixelXNo = params.outputWidth; |
||||
|
auto finalPixelZNo = params.outputHeight; |
||||
|
|
||||
|
auto pixelXStep = (frameMaxXScan - frameMinXScan) / (finalPixelXNo - 1); |
||||
|
auto pixelZStep = (frameMaxZScan - frameMinZScan) / (finalPixelZNo - 1); |
||||
|
|
||||
|
gridPixelXPos.reserve(finalPixelXNo * finalPixelZNo); |
||||
|
gridPixelZPos.reserve(finalPixelXNo * finalPixelZNo); |
||||
|
|
||||
|
auto cap = gridPixelXPos.capacity(); |
||||
|
|
||||
|
for(auto i = 0UL; i < finalPixelZNo; i++) |
||||
|
{ |
||||
|
auto temp = frameMinZScan + i * pixelZStep; |
||||
|
for(auto j = 0UL; j < finalPixelXNo; j++) |
||||
|
{ |
||||
|
gridPixelZPos.push_back(temp / cosf(params.steering)); |
||||
|
gridPixelXPos.push_back((frameMinXScan + j * pixelXStep) - |
||||
|
(temp * tanf(params.steering))); |
||||
|
} |
||||
|
} |
||||
|
|
||||
|
for(auto i = 0; i < params.rxLineNo; i++) |
||||
|
{ |
||||
|
scanXPos.push_back(params.minScanAz + i * params.rxLineDaz); |
||||
|
} |
||||
|
|
||||
|
for(auto i = 0; i < params.rxFocusPointNo; i++) |
||||
|
{ |
||||
|
scanZPos.push_back(params.minScanAx + i * params.rxPointDax); |
||||
|
} |
||||
|
} |
||||
|
|
||||
|
/*************************************************************************************************/ |
||||
|
void ScanConversionAlg::virtualConvex(Input_t params, QVector<float>& scanXPos, |
||||
|
QVector<float>& scanZPos, QVector<float>& gridPixelXPos, |
||||
|
QVector<float>& gridPixelZPos) |
||||
|
{ |
||||
|
auto finalPixelXNo = params.outputWidth; |
||||
|
auto finalPixelZNo = params.outputHeight; |
||||
|
|
||||
|
auto minAbsScanAz = 0.0f; |
||||
|
if (params.bMaxScanAz > 0 && params.bMinScanAz < 0) |
||||
|
{ |
||||
|
minAbsScanAz = 0; |
||||
|
} |
||||
|
else |
||||
|
{ |
||||
|
minAbsScanAz = qMin(abs(params.bMinScanAz), abs(params.bMaxScanAz)); |
||||
|
} |
||||
|
|
||||
|
auto maxAbsScanAz = qMax(abs(params.bMinScanAz), abs(params.bMaxScanAz)); |
||||
|
auto frameMaxZScan = params.bMaxScanAx * |
||||
|
cosf(minAbsScanAz / params.fieldOfView * 2 * params.vcMaxTheta); |
||||
|
auto frameMinZScan = params.bMinScanAx * |
||||
|
cosf(maxAbsScanAz / params.fieldOfView * 2 * params.vcMaxTheta); |
||||
|
auto frameMaxXScan = params.bMaxScanAz + params.bMaxScanAx * |
||||
|
sinf(params.bMaxScanAz / params.fieldOfView * 2 * params.vcMaxTheta); |
||||
|
auto frameMinXScan = params.bMinScanAz + params.bMaxScanAx * |
||||
|
sinf(params.bMinScanAz / params.fieldOfView * 2 * params.vcMaxTheta); |
||||
|
|
||||
|
|
||||
|
auto pixelXStep = (frameMaxXScan - frameMinXScan) / (finalPixelXNo - 1); |
||||
|
auto pixelZStep = (frameMaxZScan - frameMinZScan) / (finalPixelZNo - 1); |
||||
|
|
||||
|
auto pixelXPos = QVector<float>(); |
||||
|
auto pixelZPos = QVector<float>(); |
||||
|
|
||||
|
pixelXPos.reserve(finalPixelXNo * finalPixelZNo); |
||||
|
pixelZPos.reserve(finalPixelXNo * finalPixelZNo); |
||||
|
|
||||
|
for(auto i = 0UL; i < finalPixelZNo; i++) |
||||
|
{ |
||||
|
for(auto j = 0UL; j < finalPixelXNo; j++) |
||||
|
{ |
||||
|
pixelZPos.push_back(frameMinZScan + i * pixelZStep); |
||||
|
pixelXPos.push_back(frameMinXScan + j * pixelXStep); |
||||
|
} |
||||
|
} |
||||
|
|
||||
|
virtualScanConversion(pixelXPos, pixelZPos, finalPixelXNo, finalPixelZNo, params.steering, |
||||
|
params.virtualOriginalZ, params.startDepth, params.depth, |
||||
|
params.vcMaxTheta, gridPixelZPos, gridPixelXPos); |
||||
|
|
||||
|
//scanTheta
|
||||
|
for(auto i = 0; i < params.rxLineNo; i++) |
||||
|
{ |
||||
|
scanXPos.push_back(params.minScanAz + i * params.rxLineDaz); |
||||
|
} |
||||
|
|
||||
|
//scanR
|
||||
|
for(auto i = 0; i < params.rxFocusPointNo; i++) |
||||
|
{ |
||||
|
scanZPos.push_back(params.minScanAx + i * params.rxPointDax); |
||||
|
} |
||||
|
} |
||||
|
|
||||
|
/*************************************************************************************************/ |
||||
|
void ScanConversionAlg::convex(Input_t params, QVector<float>& scanXPos, QVector<float>& scanZPos, |
||||
|
QVector<float>& gridPixelXPos, QVector<float>& gridPixelZPos) |
||||
|
{ |
||||
|
auto finalPixelXNo = params.outputWidth; |
||||
|
auto finalPixelZNo = params.outputHeight; |
||||
|
|
||||
|
auto frameMaxXScan = params.probe.radius * sinf(params.bMaxScanAz) + |
||||
|
params.bMaxScanAx * sinf(params.bMaxScanAz / params.fieldOfView * params.angle); |
||||
|
auto frameMinXScan = params.probe.radius * sinf(params.bMinScanAz) + |
||||
|
params.bMaxScanAx * sinf(params.bMinScanAz / params.fieldOfView * params.angle); |
||||
|
|
||||
|
auto minAbsScanAz = 0.0f; |
||||
|
if (params.bMaxScanAz > 0 && params.bMinScanAz < 0) |
||||
|
{ |
||||
|
minAbsScanAz = 0; |
||||
|
} |
||||
|
else |
||||
|
{ |
||||
|
minAbsScanAz = qMin(abs(params.bMinScanAz), abs(params.bMaxScanAz)); |
||||
|
} |
||||
|
auto maxAbsScanAz = qMax(abs(params.bMinScanAz), abs(params.bMaxScanAz)); |
||||
|
|
||||
|
auto frameMaxZScan = params.probe.radius * cosf(minAbsScanAz) + |
||||
|
params.bMaxScanAx * cosf(minAbsScanAz / params.fieldOfView * params.angle); |
||||
|
auto frameMinZScan = params.probe.radius * cosf(maxAbsScanAz) + |
||||
|
params.bMinScanAx * cosf(maxAbsScanAz / params.fieldOfView * params.angle); |
||||
|
|
||||
|
auto pixelXStep = (frameMaxXScan - frameMinXScan) / (finalPixelXNo - 1); |
||||
|
auto pixelZStep = (frameMaxZScan - frameMinZScan) / (finalPixelZNo - 1); |
||||
|
|
||||
|
auto pixelXPos = QVector<float>(); |
||||
|
auto pixelZPos = QVector<float>(); |
||||
|
|
||||
|
pixelXPos.reserve(finalPixelXNo * finalPixelZNo); |
||||
|
pixelZPos.reserve(finalPixelXNo * finalPixelZNo); |
||||
|
|
||||
|
for(auto i = 0UL; i < finalPixelZNo; i++) |
||||
|
{ |
||||
|
for(auto j = 0UL; j < finalPixelXNo; j++) |
||||
|
{ |
||||
|
pixelZPos.push_back(frameMinZScan + i * pixelZStep); |
||||
|
pixelXPos.push_back(frameMinXScan + j * pixelXStep); |
||||
|
} |
||||
|
} |
||||
|
|
||||
|
convexScanConversion(pixelXPos, pixelZPos, finalPixelXNo, finalPixelZNo, params.steering, |
||||
|
params.angle, params.fieldOfView, params.probe.radius, params.startDepth, |
||||
|
params.depth, gridPixelZPos, gridPixelXPos); |
||||
|
|
||||
|
|
||||
|
//scanTheta
|
||||
|
for(auto i = 0; i < params.rxLineNo; i++) |
||||
|
{ |
||||
|
scanXPos.push_back(params.minScanAz + i * params.rxLineDaz + params.probe.radius); |
||||
|
} |
||||
|
|
||||
|
//scanR
|
||||
|
for(auto i = 0; i < params.rxFocusPointNo; i++) |
||||
|
{ |
||||
|
scanZPos.push_back(params.minScanAx + i * params.rxPointDax); |
||||
|
} |
||||
|
} |
||||
|
|
||||
|
/*************************************************************************************************/ |
||||
|
void ScanConversionAlg::scanConversion(Input_t params, QVector<float>& scanXPos, |
||||
|
QVector<float>& scanZPos, QVector<float>& gridPixelXPos, |
||||
|
QVector<float>& gridPixelZPos) |
||||
|
{ |
||||
|
//convex
|
||||
|
if(!params.probe.linear) |
||||
|
{ |
||||
|
convex(params, scanXPos, scanZPos, gridPixelXPos, gridPixelZPos); |
||||
|
} |
||||
|
//virtual convex
|
||||
|
else if(params.virtualConvex) |
||||
|
{ |
||||
|
virtualConvex(params, scanXPos, scanZPos, gridPixelXPos, gridPixelZPos); |
||||
|
} |
||||
|
//linear
|
||||
|
else |
||||
|
{ |
||||
|
linear(params, scanXPos, scanZPos, gridPixelXPos, gridPixelZPos); |
||||
|
} |
||||
|
} |
||||
|
|
||||
|
/*************************************************************************************************/ |
||||
|
void ScanConversionAlg::virtualScanConversion(QVector<float> pixelXPos, QVector<float> pixelZPos, |
||||
|
uint width, uint height, float steering, |
||||
|
float virtualOriginalZ, float startDepth, float depth, |
||||
|
float vcMaxTheta, QVector<float>& gridPixelR, |
||||
|
QVector<float>& gridPixelTheta) |
||||
|
{ |
||||
|
for(auto i = 0U; i < width * height; i++) |
||||
|
{ |
||||
|
auto x = pixelXPos[i]; |
||||
|
auto z = pixelZPos[i]; |
||||
|
|
||||
|
auto pixelTheta = atanf(x / (z + virtualOriginalZ)); |
||||
|
|
||||
|
if(pixelTheta >= -vcMaxTheta - abs(steering) && |
||||
|
pixelTheta <= vcMaxTheta + abs(steering)) |
||||
|
{ |
||||
|
if(steering == 0.0f) |
||||
|
{ |
||||
|
auto gridPixelAx = sqrtf(powf(x - virtualOriginalZ * tanf(pixelTheta), 2) + |
||||
|
powf(z, 2)); |
||||
|
|
||||
|
if(gridPixelAx >= startDepth && gridPixelAx <= depth) |
||||
|
{ |
||||
|
gridPixelR.push_back(gridPixelAx); |
||||
|
gridPixelTheta.push_back(pixelTheta); |
||||
|
} |
||||
|
else |
||||
|
{ |
||||
|
gridPixelR.push_back(0); |
||||
|
gridPixelTheta.push_back(0); |
||||
|
} |
||||
|
} |
||||
|
else |
||||
|
{ |
||||
|
auto strTan = tanf(steering); |
||||
|
auto a = virtualOriginalZ * strTan; |
||||
|
auto b = x * strTan + virtualOriginalZ + z; |
||||
|
auto c = x - z * strTan; |
||||
|
auto interceptTheta = atanf((b + sqrtf(powf(b, 2) - 4 * a * c)) / (2 * a)); |
||||
|
if(interceptTheta > vcMaxTheta || interceptTheta < -vcMaxTheta) |
||||
|
{ |
||||
|
interceptTheta = atanf((b - sqrtf(powf(b, 2) - 4 * a * c)) / (2 * a)); |
||||
|
if(interceptTheta > vcMaxTheta || interceptTheta < -vcMaxTheta) |
||||
|
{ |
||||
|
gridPixelR.push_back(0); |
||||
|
gridPixelTheta.push_back(0); |
||||
|
} |
||||
|
else |
||||
|
{ |
||||
|
auto gridPixelAx = |
||||
|
sqrtf(powf(x - virtualOriginalZ * tanf(interceptTheta), 2) + |
||||
|
powf(z, 2)); |
||||
|
if(gridPixelAx >= startDepth && gridPixelAx <= depth) |
||||
|
{ |
||||
|
gridPixelR.push_back(gridPixelAx); |
||||
|
gridPixelTheta.push_back(interceptTheta); |
||||
|
} |
||||
|
else |
||||
|
{ |
||||
|
gridPixelR.push_back(0); |
||||
|
gridPixelTheta.push_back(0); |
||||
|
} |
||||
|
} |
||||
|
} |
||||
|
else |
||||
|
{ |
||||
|
auto gridPixelAx = |
||||
|
sqrtf(powf(x - virtualOriginalZ * tanf(interceptTheta), 2) + |
||||
|
powf(z, 2)); |
||||
|
if(gridPixelAx >= startDepth && gridPixelAx <= depth) |
||||
|
{ |
||||
|
gridPixelR.push_back(gridPixelAx); |
||||
|
gridPixelTheta.push_back(interceptTheta); |
||||
|
} |
||||
|
else |
||||
|
{ |
||||
|
gridPixelR.push_back(0); |
||||
|
gridPixelTheta.push_back(0); |
||||
|
} |
||||
|
} |
||||
|
}//steering = 0
|
||||
|
} |
||||
|
else |
||||
|
{ |
||||
|
gridPixelR.push_back(0); |
||||
|
gridPixelTheta.push_back(0); |
||||
|
} |
||||
|
} |
||||
|
} |
||||
|
|
||||
|
/*************************************************************************************************/ |
||||
|
void ScanConversionAlg::convexScanConversion(QVector<float> pixelXPos, QVector<float> pixelZPos, |
||||
|
uint width, uint height, float steering, float angle, |
||||
|
float fieldOfView, float probeRadius, float startDepth, |
||||
|
float depth, QVector<float>& gridPixelR, |
||||
|
QVector<float>& gridPixelTheta) |
||||
|
{ |
||||
|
auto virtualOriginalZ = probeRadius * |
||||
|
(cosf(fieldOfView / 2) - sinf(fieldOfView / 2) / tanf(angle / 2)); |
||||
|
auto virtualOriginalZ2 = powf(virtualOriginalZ, 2); |
||||
|
|
||||
|
auto maxR = probeRadius + depth; |
||||
|
auto minR = probeRadius - virtualOriginalZ + startDepth; |
||||
|
|
||||
|
auto minTheta = -angle / 2 - abs(steering); |
||||
|
auto maxTheta = angle / 2 + abs(steering); |
||||
|
|
||||
|
auto maxInterceptTheta = fieldOfView / 2; |
||||
|
auto radius2 = powf(probeRadius, 2); |
||||
|
|
||||
|
if(steering == 0.0f) |
||||
|
{ |
||||
|
for (auto i = 0U; i < width * height; i++) |
||||
|
{ |
||||
|
auto x = pixelXPos[i]; |
||||
|
auto z = pixelZPos[i]; |
||||
|
|
||||
|
auto pixelTheta = atan2f(x, z - virtualOriginalZ); |
||||
|
auto pixelR = sqrtf(powf(x, 2) + powf(z - virtualOriginalZ, 2)); |
||||
|
|
||||
|
if(pixelR >= minR && pixelR <= maxR && pixelTheta >= minTheta && pixelTheta <= maxTheta) |
||||
|
{ |
||||
|
auto interceptTheta = 0.0f; |
||||
|
auto interceptX = 0.0f; |
||||
|
auto interceptZ = 0.0f; |
||||
|
auto alpha = 0.0f; |
||||
|
auto beta = 0.0f; |
||||
|
|
||||
|
if(x == 0.0f) |
||||
|
interceptTheta = 0; |
||||
|
else |
||||
|
{ |
||||
|
alpha = virtualOriginalZ; |
||||
|
beta = (virtualOriginalZ - z) / x; |
||||
|
interceptX = (alpha * beta + |
||||
|
sqrtf(-1 * powf(alpha, 2) + (powf(beta, 2) + 1) * radius2)) / |
||||
|
(powf(beta, 2) + 1); |
||||
|
interceptZ = alpha - beta * interceptX; |
||||
|
interceptTheta = atan2f(interceptX, interceptZ); |
||||
|
} |
||||
|
|
||||
|
if(interceptTheta > maxInterceptTheta || interceptTheta < -maxInterceptTheta) |
||||
|
{ |
||||
|
interceptX = (alpha * beta - |
||||
|
sqrtf(-1 * powf(alpha, 2) + (powf(beta, 2) + 1) * radius2)) / |
||||
|
(powf(beta, 2) + 1); |
||||
|
interceptZ = alpha - beta * interceptX; |
||||
|
interceptTheta = atan2f(interceptX, interceptZ); |
||||
|
|
||||
|
if(interceptTheta > maxInterceptTheta || interceptTheta < -maxInterceptTheta) |
||||
|
{ |
||||
|
gridPixelR.push_back(0); |
||||
|
gridPixelTheta.push_back(0); |
||||
|
} |
||||
|
else |
||||
|
{ |
||||
|
auto gridPixelAx = sqrtf(powf(x - probeRadius * sinf(interceptTheta), 2) + |
||||
|
powf(z - probeRadius * cosf(interceptTheta), 2)); |
||||
|
if(gridPixelAx >= startDepth && gridPixelAx <= depth) |
||||
|
{ |
||||
|
gridPixelR.push_back(probeRadius + gridPixelAx); |
||||
|
gridPixelTheta.push_back(interceptTheta); |
||||
|
} |
||||
|
else |
||||
|
{ |
||||
|
gridPixelR.push_back(0); |
||||
|
gridPixelTheta.push_back(0); |
||||
|
} |
||||
|
} |
||||
|
} |
||||
|
else |
||||
|
{ |
||||
|
auto gridPixelAx = sqrtf(powf(x - probeRadius * sinf(interceptTheta), 2) + |
||||
|
powf(z - probeRadius * cosf(interceptTheta), 2)); |
||||
|
if(gridPixelAx >= startDepth && gridPixelAx <= depth) |
||||
|
{ |
||||
|
gridPixelR.push_back(probeRadius + gridPixelAx); |
||||
|
gridPixelTheta.push_back(interceptTheta); |
||||
|
} |
||||
|
else |
||||
|
{ |
||||
|
gridPixelR.push_back(0); |
||||
|
gridPixelTheta.push_back(0); |
||||
|
} |
||||
|
} |
||||
|
} |
||||
|
else |
||||
|
{ |
||||
|
gridPixelR.push_back(0); |
||||
|
gridPixelTheta.push_back(0); |
||||
|
} |
||||
|
} |
||||
|
} |
||||
|
else |
||||
|
{ |
||||
|
for (auto i = 0U; i < width * height; i++) |
||||
|
{ |
||||
|
auto x = pixelXPos[i]; |
||||
|
auto z = pixelZPos[i]; |
||||
|
|
||||
|
auto pixelTheta = atan2f(x, z - virtualOriginalZ); |
||||
|
auto pixelR = sqrtf(powf(x, 2) + powf(z - virtualOriginalZ, 2)); |
||||
|
|
||||
|
if(pixelR >= minR && pixelR <= maxR && pixelTheta >= minTheta && pixelTheta <= maxTheta) |
||||
|
{ |
||||
|
auto strSin = 2 * sinf(steering); |
||||
|
auto ro = pixelR / strSin; |
||||
|
auto xo = ro * cosf(steering - pixelTheta); |
||||
|
auto zo = ro * sinf(steering - pixelTheta) + virtualOriginalZ; |
||||
|
if(zo == 0.0f) |
||||
|
{ |
||||
|
auto interceptX = (radius2 - virtualOriginalZ2) / 2 / xo; |
||||
|
auto interceptZ = sqrtf(radius2 - powf(interceptX , 2)); |
||||
|
auto interceptTheta = atan2f(interceptX, interceptZ); |
||||
|
if (interceptTheta > maxInterceptTheta || interceptTheta < -maxInterceptTheta) |
||||
|
{ |
||||
|
interceptZ = -sqrtf(radius2 - powf(interceptX , 2)); |
||||
|
interceptTheta = atan2f(interceptX, interceptZ); |
||||
|
if(interceptTheta > maxInterceptTheta || |
||||
|
interceptTheta < -maxInterceptTheta) |
||||
|
{ |
||||
|
gridPixelR.push_back(0); |
||||
|
gridPixelTheta.push_back(0); |
||||
|
} |
||||
|
else |
||||
|
{ |
||||
|
auto gridPixelAx = |
||||
|
sqrtf(powf(x - probeRadius * sinf(interceptTheta), 2) + |
||||
|
powf(z - probeRadius * cosf(interceptTheta), 2)); |
||||
|
if(gridPixelAx >= startDepth && gridPixelAx <= depth) |
||||
|
{ |
||||
|
gridPixelR.push_back(probeRadius + gridPixelAx); |
||||
|
gridPixelTheta.push_back(interceptTheta); |
||||
|
} |
||||
|
else |
||||
|
{ |
||||
|
gridPixelR.push_back(0); |
||||
|
gridPixelTheta.push_back(0); |
||||
|
} |
||||
|
} |
||||
|
} |
||||
|
else |
||||
|
{ |
||||
|
auto gridPixelAx = sqrtf(powf(x - probeRadius * sinf(interceptTheta), 2) + |
||||
|
powf(z - probeRadius * cosf(interceptTheta), 2)); |
||||
|
if(gridPixelAx >= startDepth && gridPixelAx <= depth) |
||||
|
{ |
||||
|
gridPixelR.push_back(probeRadius + gridPixelAx); |
||||
|
gridPixelTheta.push_back(interceptTheta); |
||||
|
} |
||||
|
else |
||||
|
{ |
||||
|
gridPixelR.push_back(0); |
||||
|
gridPixelTheta.push_back(0); |
||||
|
} |
||||
|
} |
||||
|
} |
||||
|
else |
||||
|
{ |
||||
|
auto alpha = (radius2 - virtualOriginalZ2 + 2 * zo * virtualOriginalZ) / 2 / zo; |
||||
|
auto beta = xo / zo; |
||||
|
|
||||
|
auto interceptX = (alpha * beta + |
||||
|
sqrtf(-1 * powf(alpha, 2) + (powf(beta, 2) + 1) * radius2)) / |
||||
|
(powf(beta, 2) + 1); |
||||
|
auto interceptZ = alpha - beta * interceptX; |
||||
|
auto interceptTheta = atan2f(interceptX, interceptZ); |
||||
|
|
||||
|
if(interceptTheta > maxInterceptTheta || interceptTheta < -maxInterceptTheta) |
||||
|
{ |
||||
|
interceptX = (alpha * beta - |
||||
|
sqrtf(-1 * powf(alpha, 2) + (powf(beta, 2) + 1) * radius2)) / |
||||
|
(powf(beta, 2) + 1); |
||||
|
interceptZ = alpha - beta * interceptX; |
||||
|
interceptTheta = atan2f(interceptX, interceptZ); |
||||
|
if(interceptTheta > maxInterceptTheta || interceptTheta < -maxInterceptTheta) |
||||
|
{ |
||||
|
gridPixelR.push_back(0); |
||||
|
gridPixelTheta.push_back(0); |
||||
|
} |
||||
|
else |
||||
|
{ |
||||
|
auto gridPixelAx = sqrtf(powf(x - probeRadius * sinf(interceptTheta), 2) + |
||||
|
powf(z - probeRadius * cosf(interceptTheta), 2)); |
||||
|
if(gridPixelAx >= startDepth && gridPixelAx <= depth) |
||||
|
{ |
||||
|
gridPixelR.push_back(probeRadius + gridPixelAx); |
||||
|
gridPixelTheta.push_back(interceptTheta); |
||||
|
} |
||||
|
else |
||||
|
{ |
||||
|
gridPixelR.push_back(0); |
||||
|
gridPixelTheta.push_back(0); |
||||
|
} |
||||
|
} |
||||
|
} |
||||
|
else |
||||
|
{ |
||||
|
auto gridPixelAx = sqrtf(powf(x - probeRadius * sinf(interceptTheta), 2) + |
||||
|
powf(z - probeRadius * cosf(interceptTheta), 2)); |
||||
|
if(gridPixelAx >= startDepth && gridPixelAx <= depth) |
||||
|
{ |
||||
|
gridPixelR.push_back(probeRadius + gridPixelAx); |
||||
|
gridPixelTheta.push_back(interceptTheta); |
||||
|
} |
||||
|
else |
||||
|
{ |
||||
|
gridPixelR.push_back(0); |
||||
|
gridPixelTheta.push_back(0); |
||||
|
} |
||||
|
} |
||||
|
} |
||||
|
} |
||||
|
else |
||||
|
{ |
||||
|
gridPixelR.push_back(0); |
||||
|
gridPixelTheta.push_back(0); |
||||
|
} |
||||
|
} |
||||
|
} |
||||
|
} |
||||
@ -0,0 +1,71 @@ |
|||||
|
#include "model/processor/strategies/Cri.h" |
||||
|
#include "model/processor/BIP.h" |
||||
|
#include <QPixmap> |
||||
|
#include <QImage> |
||||
|
|
||||
|
Cri::Cri(const Context context,const QString kernelPath,const QObject *parent = Q_NULLPTR) : |
||||
|
IProcessStrategy(context, kernelPath, "Cri", parent), |
||||
|
_kernelFunctor(KernelFunctor<Image2DArray, Image2D, Cri_t>(_kernel)) |
||||
|
{ |
||||
|
memset(&_kernelParameters, 0, sizeof (Cri_t)); |
||||
|
// _criBuffer = new Image2D()[CRI_MAX_BUFFER_SIZE];
|
||||
|
} |
||||
|
|
||||
|
void Cri::cpuProcess(Input_t params) |
||||
|
{ |
||||
|
_kernelParameters.criFilterMode = params.criFilterMode; |
||||
|
_kernelParameters.scenariFrameNo = params.scenarioFrameNo; |
||||
|
_kernelParameters.frameCntr = params.frameCntr; |
||||
|
} |
||||
|
|
||||
|
void Cri::finalize() |
||||
|
{ |
||||
|
} |
||||
|
|
||||
|
|
||||
|
Image* Cri::processKernel(Image *frames, Buffer* scratchPad) |
||||
|
{ |
||||
|
auto format = frames->getImageInfo<CL_IMAGE_FORMAT>(); |
||||
|
auto width = frames->getImageInfo<CL_IMAGE_WIDTH>(); |
||||
|
auto height = frames->getImageInfo<CL_IMAGE_HEIGHT>(); |
||||
|
|
||||
|
_criBuffer[_kernelParameters.scenariFrameNo] = static_cast<Image2D*>(frames); |
||||
|
int err = 100; |
||||
|
|
||||
|
// Image2DArray bufferframes = Image2DArray(_CLContext, CL_MEM_READ_WRITE, ImageFormat(format.image_channel_order, format.image_channel_data_type),
|
||||
|
// 16,
|
||||
|
// width,
|
||||
|
// height,
|
||||
|
// 0,0);
|
||||
|
// BIP::getInstance()->CLQueue.enqueueWriteImage(*bufferframes, CL_TRUE, array<size_type, 3> {0, 0, 0},
|
||||
|
// array<size_type, 4> {width, height, 1},
|
||||
|
// width * sizeof (myflt),
|
||||
|
// 0,
|
||||
|
// _criBuffer);
|
||||
|
auto imgs = new Image2DArray(_CLContext, CL_MEM_READ_WRITE, ImageFormat(format.image_channel_order, format.image_channel_data_type), |
||||
|
16, |
||||
|
width, |
||||
|
height, |
||||
|
width *4* sizeof (myflt), |
||||
|
0, |
||||
|
_criBuffer, |
||||
|
&err); |
||||
|
qDebug()<<"erro is : " << err; |
||||
|
// cl::enqueueReadImage()
|
||||
|
|
||||
|
auto imageOutput = new Image2D(_CLContext, |
||||
|
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<Image2DArray, Image2D, Cri_t>(_kernelFunctor, |
||||
|
eargs, |
||||
|
*imgs, |
||||
|
*imageOutput, |
||||
|
_kernelParameters); |
||||
|
|
||||
|
delete imgs; |
||||
|
return imageOutput; |
||||
|
} |
||||
Loading…
Reference in new issue