Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Valley detection v2 #6

Open
wants to merge 5 commits into
base: master
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion .gitignore
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
.idea
build/
.DS_Store
cmake-build-debug
cmake-build-default/
46 changes: 29 additions & 17 deletions HistogramGrid.h
Original file line number Diff line number Diff line change
Expand Up @@ -34,10 +34,15 @@ class HistogramGrid {
// int histWidth - Width of the entire histogram in meters
// int histLength - Length of the entire histogram in meters
// int nodeSideLen - Side dimension of each node. histWidth and histLength should be divisible by this number
HistogramGrid(int histWidth, int histLength, double nodeSideLen):
iMax((int)(histWidth/nodeSideLen)), jMax((int)(histLength/nodeSideLen)),
nodeSize(nodeSideLen), histGrid(new double*[iMax]), objectGrid(new int*[jMax]),
iSizeActiveRegion(10), jSizeActiveRegion(10)
HistogramGrid(int histWidth, int histLength, double nodeSideLen,
int activeRegionSize_i, int activeRegionSize_j):
iMax((int)(histWidth/nodeSideLen)),
jMax((int)(histLength/nodeSideLen)),
nodeSize(nodeSideLen),
histGrid(new double*[iMax]),
objectGrid(new int*[jMax]),
iSizeActiveRegion(activeRegionSize_i),
jSizeActiveRegion(activeRegionSize_j)
{
std::cout<<"grid: iMax = "<<iMax<<". jMax = "<<jMax<<"\n";
//Initializing the histGrid and objectGrid
Expand All @@ -61,33 +66,40 @@ class HistogramGrid {

//HistogramGrid
//Alternate constructor used for ingesting grid from file. Used only for testing
HistogramGrid(std::string fName, discretePoint robotLocIn)
// HistogramGrid(std::string fName, discretePoint robotLocIn,
// int activeRegionSize_i, int activeRegionSize_j):
// iSizeActiveRegion(activeRegionSize_i),
// jSizeActiveRegion(activeRegionSize_j)
HistogramGrid(std::string fName, discretePoint robotLocIn,
int activeRegionSize_i, int activeRegionSize_j,
int histWidth, int histLength, double nodeSizeIn):
iSizeActiveRegion(activeRegionSize_i),
jSizeActiveRegion(activeRegionSize_j),
nodeSize(nodeSizeIn)
{
//std::cout << "\n\ntesting histogram grid initialization: "<< std::endl;

robotLoc = robotLocIn;
std::string data; //Temporary string to store ingested data
int histWidth;
int histLength;
// int histWidth;
// int histLength;

std::ifstream file(fName);
if (file.is_open())
{
file >> data;
histWidth = std::stoi(data);

file >> data;
histLength = std::stoi(data);

file >> data;
nodeSize = std::stod(data);
// file >> data;
// histWidth = std::stoi(data);
//
// file >> data;
// histLength = std::stoi(data);
//
// file >> data;
// nodeSize = std::stod(data);

iMax = (int)(histWidth/nodeSize);
jMax = (int)(histLength/nodeSize);
histGrid = new double*[iMax];
objectGrid = new int*[jMax];
iSizeActiveRegion = 20;
jSizeActiveRegion = 20;

for(int i = 0; i < iMax; i++)
{
Expand Down
17 changes: 11 additions & 6 deletions Plotter.h
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,7 @@
#include <vector>
#include <thread>
#include "GNUPlot.h"

#include <string>

#ifndef VECTORFIELDHISTOGRAMTESTING_PLOTTER_H
#define VECTORFIELDHISTOGRAMTESTING_PLOTTER_H
Expand All @@ -22,16 +22,20 @@ class Plotter {
public:
Plotter()
{
// TODO: parameterize xrange, yrange according
int xMax = 60;
int yMax = 60;
int pointSize = 7;

std::vector<std::string> script;
f.open(fPath);
f << header << std::endl;
f.close();
std::this_thread::sleep_for(std::chrono::milliseconds(100));


script.push_back("set xrange [0:60];");
script.push_back("set yrange [0:60];");
script.push_back("plot \"" + fPath + "\"" + "with points pointsize 7");
script.push_back("set xrange [0:" + std::to_string(xMax) + "];");
script.push_back("set yrange [0:" + std::to_string(yMax) + "];");
script.push_back("plot \"" + fPath + "\"" + "with points pointsize " + std::to_string(pointSize) + "");

plotter.open();
plotter.execute(script);
Expand All @@ -44,7 +48,7 @@ class Plotter {
plotter.close();
}

void plot(std::vector<discretePoint>& dataIn)
void plot(std::vector<discretePoint>& dataIn, int robotX, int robotY, double angle, int timestep, int numTimesteps)
{
std::vector<std::string> script;
script.push_back("replot");
Expand All @@ -56,6 +60,7 @@ class Plotter {
f << dataIn[j].x << " " << dataIn[j].y << std::endl;
}
f.close();
script.push_back("set label 11 center at graph 0.2,char 1 \"t = " + std::to_string(timestep) + " / " + std::to_string(numTimesteps) + " position = (" + std::to_string(robotX) + ", " + std::to_string(robotY) + "); angle = " + std::to_string(angle) + "\" font \",14\"; set bmargin 4;");
plotter.execute(script);
}
};
Expand Down
13 changes: 10 additions & 3 deletions PolarHistogram.h
Original file line number Diff line number Diff line change
Expand Up @@ -41,6 +41,12 @@ class PolarHistogram {
histogram[9] = 3;
}

//getIndex
//Returns index 0 <= i < nBins that corresponds to a. "Wraps" a around histogram.
int getIndex(int a) {
return ((a % nBins) + nBins) % nBins;
}

//getBinFromAngle
//Returns the index of the bin based on the angle relative to the absolute coordinate system with which
//histogram is represented
Expand All @@ -53,7 +59,7 @@ class PolarHistogram {
//Returns the angle in the middle of the bin
double getAngleFromBin(int bin)
{
return binWidth/2 + binWidth*bin;
return binWidth/2 + binWidth*getIndex(bin);
}

//getNumBins
Expand All @@ -67,7 +73,8 @@ class PolarHistogram {
//Returns the value of the histogram for the specified bin
double getValue(int bin)
{
return histogram[bin];

return histogram[getIndex(bin)];
}

//addValue
Expand Down Expand Up @@ -101,7 +108,7 @@ class PolarHistogram {
{
for(int i = 0; i < nBins; i++)
{
std::cout << getAngleFromBin(i) << " " << histogram[i] << "\n";
std::cout << i <<"th bin: angle = " << getAngleFromBin(i) << " , certainty = " << histogram[i] << "\n";
}
}

Expand Down
7 changes: 6 additions & 1 deletion README.md
Original file line number Diff line number Diff line change
@@ -1 +1,6 @@
"# VectorFieldHistogramTesting"
# VectorFieldHistogramTesting

Bugs

- [] contpoint vs discretepoint angle issue. Error may be due to discretization.
- [] smoothing?
Loading