Stage& PipelineManager::makeFilter(const std::string& driver, Stage& parent,
Options options)
{
StageCreationOptions ops { "", driver, &parent, options };
return makeFilter(ops);
}
int GroundKernel::execute()
{
PointTable table;
Stage& readerStage(makeReader(m_inputFile, ""));
Options groundOptions;
groundOptions.add("max_window_size", m_maxWindowSize);
groundOptions.add("slope", m_slope);
groundOptions.add("max_distance", m_maxDistance);
groundOptions.add("initial_distance", m_initialDistance);
groundOptions.add("cell_size", m_cellSize);
groundOptions.add("classify", m_classify);
groundOptions.add("extract", m_extract);
groundOptions.add("approximate", m_approximate);
Stage& groundStage = makeFilter("filters.ground", readerStage);
groundStage.addOptions(groundOptions);
// setup the Writer and write the results
Stage& writer(makeWriter(m_outputFile, groundStage, ""));
writer.prepare(table);
// process the data, grabbing the PointViewSet for visualization of the
// resulting PointView
PointViewSet viewSetOut = writer.execute(table);
if (isVisualize())
visualize(*viewSetOut.begin());
//visualize(*viewSetIn.begin(), *viewSetOut.begin());
return 0;
}
bool Utils::OpenMultiFileDialog(CStringList& fileNameList)
{
CFileDialog dlg(TRUE);
std::vector<CString> filter = getOpenFilter();
CString strFilter(makeFilter(filter));
dlg.m_ofn.lpstrFilter = strFilter;
dlg.m_ofn.lpstrDefExt = NULL;
dlg.m_ofn.Flags |= OFN_FILEMUSTEXIST|OFN_ALLOWMULTISELECT;
const int maxChar = 5000;
CString strBuffer;
dlg.m_ofn.lpstrFile = strBuffer.GetBuffer(maxChar);
dlg.m_ofn.nMaxFile = maxChar;
int choice = dlg.DoModal();
strBuffer.ReleaseBuffer();
if (choice == IDOK)
{
POSITION pos = dlg.GetStartPosition();
while (pos != NULL)
{
fileNameList.AddTail(dlg.GetNextPathName(pos));
}
return true;
}
return false;
}
void Textons::makeRFSFilters() {
int SCALEX[3] = {1, 2, 4};
for (int i = 0; i < NF; i++) {
F[i].create(SUP, SUP, CV_32FC1);
}
int hsup = (SUP - 1)/2;
float x[SUP*SUP], y[SUP*SUP];
for (int i = 0; i < SUP; i++) {
for (int j = 0; j < SUP; j++) {
x[i*SUP + j] = -1*hsup + i;
y[j*SUP + i] = hsup - i;
}
}
int count = 0;
for (int scale = 0; scale < NSCALES; scale++) {
for (int orient = 0; orient < NORIENT; orient++) {
float angle = PI*orient/NORIENT;
float c = cos(angle);
float s = sin(angle);
// Calculate rotated points
float rotPtsx[SUP*SUP];
float rotPtsy[SUP*SUP];
for (int i = 0; i < SUP; i++) {
for (int j = 0; j < SUP; j++) {
float x_prime = x[i*SUP+j];
float y_prime = y[i*SUP+j];
rotPtsx[i*SUP+j] = x_prime*c - y_prime*s;
rotPtsy[i*SUP+j] = x_prime*s + y_prime*c;
}
}
makeFilter(&F[count], SCALEX[scale], 0, 1, rotPtsx, rotPtsy, SUP);
makeFilter(&F[count + NEDGE], SCALEX[scale], 0, 2, rotPtsx, rotPtsy, SUP);
count++;
}
}
return;
}
vector<ScanLocalizedObject> ScanLocator::locate(const ScanSegmentation &segmentation, bool floor, const vec3f &targetDimensions)
{
vector<ScanLocalizedObject> result;
if (!skipLocator)
{
ml::util::push_back(result, locate(segmentation, floor, makeFilter(targetDimensions, 0.04f, 0.8f), 1.0f));
//ml::util::push_back(result, locate(segmentation, floor, makeFilter(targetDimensions, 0.075f, 0.75f), 0.5f));
}
return result;
}
cpd::Matrix CpdKernel::readFile(const std::string& filename)
{
Stage& reader = makeReader(filename, "");
PointTable table;
PointViewSet viewSet;
if (!m_bounds.empty())
{
Options boundsOptions;
boundsOptions.add("bounds", m_bounds);
Stage& crop = makeFilter("filters.crop", reader);
crop.setOptions(boundsOptions);
crop.prepare(table);
viewSet = crop.execute(table);
}
else
{
reader.prepare(table);
viewSet = reader.execute(table);
}
cpd::Matrix matrix(0, 3);
for (auto it = viewSet.begin(); it != viewSet.end(); ++it)
{
PointViewPtr view = *it;
point_count_t rowidx;
if (matrix.rows() == 0)
{
rowidx = 0;
matrix.resize(view->size(), 3);
}
else
{
rowidx = matrix.rows();
matrix.conservativeResize(matrix.rows() + view->size(), 3);
}
for (point_count_t bufidx = 0; bufidx < view->size(); ++bufidx, ++rowidx)
{
matrix(rowidx, 0) = view->getFieldAs<double>(Dimension::Id::X, bufidx);
matrix(rowidx, 1) = view->getFieldAs<double>(Dimension::Id::Y, bufidx);
matrix(rowidx, 2) = view->getFieldAs<double>(Dimension::Id::Z, bufidx);
}
}
return matrix;
}
int PCLKernel::execute()
{
PointTable table;
Stage& readerStage(makeReader(m_inputFile, ""));
// go ahead and prepare/execute on reader stage only to grab input
// PointViewSet, this makes the input PointView available to both the
// processing pipeline and the visualizer
readerStage.prepare(table);
PointViewSet viewSetIn = readerStage.execute(table);
// the input PointViewSet will be used to populate a BufferReader that is
// consumed by the processing pipeline
PointViewPtr input_view = *viewSetIn.begin();
std::shared_ptr<BufferReader> bufferReader(new BufferReader);
bufferReader->addView(input_view);
Options filterOptions({"filename", m_pclFile});
Stage& pclStage = makeFilter("filters.pclblock", *bufferReader,
filterOptions);
// the PCLBlock stage consumes the BufferReader rather than the
// readerStage
Options writerOptions;
if (m_bCompress)
writerOptions.add<bool>("compression", true);
if (m_bForwardMetadata)
writerOptions.add("forward_metadata", true);
Stage& writer(makeWriter(m_outputFile, pclStage, "", writerOptions));
writer.prepare(table);
// process the data, grabbing the PointViewSet for visualization of the
// resulting PointView
PointViewSet viewSetOut = writer.execute(table);
if (isVisualize())
visualize(*viewSetOut.begin());
//visualize(*viewSetIn.begin(), *viewSetOut.begin());
return 0;
}
int SortKernel::execute()
{
Stage& readerStage = makeReader(m_inputFile, m_driverOverride);
Stage& sortStage = makeFilter("filters.mortonorder", readerStage);
Options writerOptions;
if (m_bCompress)
writerOptions.add("compression", true);
if (m_bForwardMetadata)
writerOptions.add("forward_metadata", true);
Stage& writer = makeWriter(m_outputFile, sortStage, "", writerOptions);
PointTable table;
writer.prepare(table);
writer.execute(table);
return 0;
}
bool Utils::OpenFileDialog(CString& strFile)
{
CFileDialog dlg(TRUE);
std::vector<CString> filter = getOpenFilter();
CString strFilter(makeFilter(filter));
dlg.m_ofn.lpstrFilter = strFilter;
dlg.m_ofn.lpstrDefExt = NULL;
dlg.m_ofn.Flags |= OFN_FILEMUSTEXIST;
if (dlg.DoModal() == IDOK)
{
strFile = dlg.GetPathName();
return true;
}
return false;
}
Pixel* blur(Pixel* pixelArray, Image * img, double sigma)
{
//prevents bad sigma value from passing
if(sigma <= 0){
printf("Invalid value for sigma!\n");
return pixelArray;
}
//initialize filter struct
Filter* filter;
//get 2D version of image
Pixel** pointerArray = TwoDArray(pixelArray, img);
//get the filter
filter = makeFilter(sigma);
//convolve filter with image, get blurred image
Pixel* blurArray = convolve(pointerArray, img, filter);
//return blurred image
return blurArray;
}
bool Utils::OpenSaveDialog(CString& strFile, const std::vector<CString>& filter, const CString& strInitialDirectory)
{
CFileDialog dlg(true);
CString strFilter(makeFilter(filter));
dlg.m_ofn.lpstrFilter = strFilter;
dlg.m_ofn.lpstrDefExt = NULL;
dlg.m_ofn.lpstrInitialDir = strInitialDirectory;
dlg.m_ofn.Flags |= OFN_FILEMUSTEXIST;
if (dlg.DoModal() == IDOK)
{
strFile = dlg.GetPathName();
return true;
}
return false;
}
int SortKernel::execute()
{
Stage& readerStage = makeReader(m_inputFile, "");
// go ahead and prepare/execute on reader stage only to grab input
// PointViewSet, this makes the input PointView available to both the
// processing pipeline and the visualizer
PointTable table;
readerStage.prepare(table);
PointViewSet viewSetIn = readerStage.execute(table);
// the input PointViewSet will be used to populate a BufferReader that is
// consumed by the processing pipeline
PointViewPtr inView = *viewSetIn.begin();
BufferReader bufferReader;
bufferReader.addView(inView);
Stage& sortStage = makeFilter("filters.mortonorder", bufferReader);
Stage& writer = makeWriter(m_outputFile, sortStage, "");
Options writerOptions;
if (m_bCompress)
writerOptions.add("compression", true);
if (m_bForwardMetadata)
writerOptions.add("forward_metadata", true);
writer.addOptions(writerOptions);
writer.prepare(table);
// process the data, grabbing the PointViewSet for visualization of the
PointViewSet viewSetOut = writer.execute(table);
if (isVisualize())
visualize(*viewSetOut.begin());
return 0;
}
bool Utils::SaveDialog(CString& strFile, FileType& fileType, std::vector<CString>& filter, std::vector<FileType>& fileTypes)
{
CFileDialog dlg(FALSE);
CString strFilter(makeFilter(filter));
dlg.m_ofn.lpstrFilter = strFilter;
dlg.m_ofn.lpstrDefExt = NULL;
CString initalDirectory(getCurrentDirectory());
dlg.m_ofn.lpstrInitialDir = initalDirectory;
dlg.m_ofn.Flags |= OFN_CREATEPROMPT;
if (dlg.DoModal() == IDOK)
{
int typeIndex = dlg.m_ofn.nFilterIndex;
strFile = getFileName(filter, typeIndex, dlg.GetPathName());
fileType = fileTypes[typeIndex - 1];
return true;
}
return false;
}
int SplitKernel::execute()
{
PointTable table;
Stage& reader = makeReader(m_inputFile, m_driverOverride);
Options filterOpts;
std::string driver = (m_length ? "filters.splitter" : "filters.chipper");
if (m_length)
{
filterOpts.add("length", m_length);
filterOpts.add("origin_x", m_xOrigin);
filterOpts.add("origin_y", m_yOrigin);
}
else
{
filterOpts.add("capacity", m_capacity);
}
Stage& f = makeFilter(driver, reader, filterOpts);
f.prepare(table);
PointViewSet pvSet = f.execute(table);
int filenum = 1;
for (auto& pvp : pvSet)
{
BufferReader reader;
reader.addView(pvp);
std::string filename = makeFilename(m_outputFile, filenum++);
Stage& writer = makeWriter(filename, reader, "");
writer.prepare(table);
writer.execute(table);
}
return 0;
}
Stage& PipelineManager::makeFilter(const std::string& driver, Stage& parent)
{
Stage& filter = makeFilter(driver);
filter.setInput(parent);
return filter;
}
//************************************
int sox_init(uint32_t fin, uint32_t fout,ResampleStruct *r)
{
long Xoff, gcdrate;
int i;
memset(r,0,sizeof(ResampleStruct));
/*
r->rolloff = 0.94;
r->quadr = 1;
r->Nmult = 149;*/
r->rolloff = 0.80;
r->quadr = 0;
r->Nmult = 45;
r->beta=16;
if (fout == fin)
{
printf("Input and Output rates must be different to use resample effect");
return(0);
}
printf("Resampling from %lu to %lu\n",fin,fout);
r->Factor = (double)fout / (double)fin;
gcdrate = st_gcd((long)fin, (long)fout);
r->a = fin/ gcdrate;
r->b = fout / gcdrate;
if (r->a <= r->b && r->b <= NQMAX) {
r->quadr = -1; /* exact coeff's */
r->Nq = r->b; /* MAX(r->a,r->b); */
} else {
r->Nq = Nc; /* for now */
}
/* Check for illegal constants */
/* Nwing: # of filter coeffs in right wing */
r->Nwing = r->Nq * (r->Nmult/2+1) + 1;
r->Imp = (Float *)malloc(sizeof(Float) * (r->Nwing+2)) + 1;
/* need Imp[-1] and Imp[Nwing] for quadratic interpolation */
/* returns error # <=0, or adjusted wing-len > 0 */
i = makeFilter(r->Imp, r->Nwing, r->rolloff, r->beta, r->Nq, 1);
if (i <= 0)
{
printf("resample: Unable to make filter\n");
return (0);
}
/*st_report("Nmult: %ld, Nwing: %ld, Nq: %ld\n",r->Nmult,r->Nwing,r->Nq);*/
if (r->quadr < 0) { /* exact coeff's method */
r->Xh = r->Nwing/r->b;
printf("resample: rate ratio %ld:%ld, coeff interpolation not needed\n", r->a, r->b);
} else {
r->dhb = Np; /* Fixed-point Filter sampling-time-increment */
if (r->Factor<1.0) r->dhb = r->Factor*Np + 0.5;
r->Xh = (r->Nwing<<La)/r->dhb;
/* (Xh * dhb)>>La is max index into Imp[] */
}
/* reach of LP filter wings + some creeping room */
Xoff = r->Xh + 10;
r->Xoff = Xoff;
/* Current "now"-sample pointer for input to filter */
r->Xp = Xoff;
/* Position in input array to read into */
r->Xread = Xoff;
/* Current-time pointer for converter */
r->Time = Xoff;
if (r->quadr < 0) { /* exact coeff's method */
r->t = Xoff*r->Nq;
}
i = BUFFSIZE - 2*Xoff;
if (i < r->Factor + 1.0/r->Factor) /* Check input buffer size */
{
printf("Factor is too small or large for BUFFSIZE");
return (0);
}
r->Xsize = 2*Xoff + i/(1.0+r->Factor);
r->Ysize = BUFFSIZE - r->Xsize;
/* st_report("Xsize %d, Ysize %d, Xoff %d",r->Xsize,r->Ysize,r->Xoff); */
r->X = (Float *) malloc(sizeof(Float) * (BUFFSIZE));
r->Y = r->X + r->Xsize;
/* Need Xoff zeros at beginning of sample */
for (i=0; i<Xoff; i++)
r->X[i] = 0;
return (1);
}
/*
* Prepare processing.
*/
int st_resample_start(eff_t effp)
{
resample_t r = (resample_t) effp->priv;
long Xoff, gcdrate;
int i;
if (effp->ininfo.rate == effp->outinfo.rate)
{
st_fail("Input and Output rates must be different to use resample effect");
return(ST_EOF);
}
r->Factor = (double)effp->outinfo.rate / (double)effp->ininfo.rate;
gcdrate = st_gcd((long)effp->ininfo.rate, (long)effp->outinfo.rate);
r->a = effp->ininfo.rate / gcdrate;
r->b = effp->outinfo.rate / gcdrate;
if (r->a <= r->b && r->b <= NQMAX) {
r->quadr = -1; /* exact coeff's */
r->Nq = r->b; /* MAX(r->a,r->b); */
} else {
r->Nq = Nc; /* for now */
}
/* Check for illegal constants */
# if 0
if (Lp >= 16) st_fail("Error: Lp>=16");
if (Nb+Nhg+NLpScl >= 32) st_fail("Error: Nb+Nhg+NLpScl>=32");
if (Nh+Nb > 32) st_fail("Error: Nh+Nb>32");
# endif
/* Nwing: # of filter coeffs in right wing */
r->Nwing = r->Nq * (r->Nmult/2+1) + 1;
r->Imp = (Float *)malloc(sizeof(Float) * (r->Nwing+2)) + 1;
/* need Imp[-1] and Imp[Nwing] for quadratic interpolation */
/* returns error # <=0, or adjusted wing-len > 0 */
i = makeFilter(r->Imp, r->Nwing, r->rolloff, r->beta, r->Nq, 1);
if (i <= 0)
{
st_fail("resample: Unable to make filter\n");
return (ST_EOF);
}
/*st_report("Nmult: %ld, Nwing: %ld, Nq: %ld\n",r->Nmult,r->Nwing,r->Nq);*/
if (r->quadr < 0) { /* exact coeff's method */
r->Xh = r->Nwing/r->b;
st_report("resample: rate ratio %ld:%ld, coeff interpolation not needed\n", r->a, r->b);
} else {
r->dhb = Np; /* Fixed-point Filter sampling-time-increment */
if (r->Factor<1.0) r->dhb = r->Factor*Np + 0.5;
r->Xh = (r->Nwing<<La)/r->dhb;
/* (Xh * dhb)>>La is max index into Imp[] */
}
/* reach of LP filter wings + some creeping room */
Xoff = r->Xh + 10;
r->Xoff = Xoff;
/* Current "now"-sample pointer for input to filter */
r->Xp = Xoff;
/* Position in input array to read into */
r->Xread = Xoff;
/* Current-time pointer for converter */
r->Time = Xoff;
if (r->quadr < 0) { /* exact coeff's method */
r->t = Xoff*r->Nq;
}
i = BUFFSIZE - 2*Xoff;
if (i < r->Factor + 1.0/r->Factor) /* Check input buffer size */
{
st_fail("Factor is too small or large for BUFFSIZE");
return (ST_EOF);
}
r->Xsize = 2*Xoff + i/(1.0+r->Factor);
r->Ysize = BUFFSIZE - r->Xsize;
/* st_report("Xsize %d, Ysize %d, Xoff %d",r->Xsize,r->Ysize,r->Xoff); */
r->X = (Float *) malloc(sizeof(Float) * (BUFFSIZE));
r->Y = r->X + r->Xsize;
/* Need Xoff zeros at beginning of sample */
for (i=0; i<Xoff; i++)
r->X[i] = 0;
return (ST_SUCCESS);
}
Stage& PipelineManager::makeFilter(const std::string& driver)
{
StageCreationOptions ops { "", driver };
return makeFilter(ops);
}
