double
MapImage::getExploredFraction(std::vector<CvPoint> & pathHistory)
{
CvScalar p;
// then also find trapped gray parts, make them zero too
IplImage* im2 = cvCreateImage(cvSize(img->width,img->height), IPL_DEPTH_8U, 1);
im2->widthStep = img->widthStep;
cvCopy(img, im2);
// consider mapped obstacles (180-255) as zeros, so all explored cells are represented
// in a single continous interval
cvThreshold(img, img, 180, 254, CV_THRESH_TOZERO_INV);
// truncate values to get only obstacles
cvThreshold(im2, im2, 180, 254, CV_THRESH_BINARY);
cvDilate(im2, im2);
bool found = false;
CvPoint point;
unsigned int i = 0;
for(i = 0; i < pathHistory.size()-1; i++)
{
p = cvGet2D(im2, pathHistory[i].y, pathHistory[i].x);
if( ((unsigned int)p.val[0]) < 73 )
{
point.x = pathHistory[i].x;
point.y = pathHistory[i].y;
found = true;
break;
}
}
if( !found && i == 0 )
{
point.x = pathHistory[0].x;
point.y = pathHistory[0].y;
found = true;
}
if( !found )
{
std::string msg("error in finding a seed point to make map qt ready");
std::cout << msg << std::endl;
throw MapException(msg.c_str());
}
cvFloodFill(im2, point, cvScalar(127));
std::vector<int> dx;
std::vector<int> dy;
std::vector<CvPoint> neighbors;
PointSet pointSet;
for(int i = 0; i < img->height; i++)
{
dy.push_back(0);
if(i == 0)
dy.push_back(1);
else if(i == img->height-1)
dy.push_back(-1);
else
{
dy.push_back(-1);
dy.push_back(1);
}
for(int j = 0; j < img->width; j++)
{
// p = cvGet2D(im4,i,j);
dx.push_back(0);
if( j == 0 )
dx.push_back(1);
else if(j == img->width-1)
dx.push_back(-1);
else
{
dx.push_back(-1);
dx.push_back(1);
}
p = cvGet2D(im2,i,j);
if( (unsigned int)p.val[0] == 0) // trapped gray cells
{
//cvSet2D(img, i, j, p); // set its neighbors too
for(unsigned int k = 0; k < dy.size(); k++)
{
for(unsigned int l = 0; l < dx.size(); l++)
{
//cvSet2D(img, i+dy[k], j+dx[l], p);
//neighbors.push_back(cvPoint(j+dx[l],i+dy[k]));
pointSet.addPoint(cvPoint(j+dx[l],i+dy[k]));
}
}
}
dx.clear();
}
dy.clear();
}
pointSet.getPointsList(&neighbors);
std::cout << "updating trapped pixels : " << neighbors.size() << std::endl;
for(unsigned int i = 0; i < neighbors.size(); i++)
{
CvPoint pp = neighbors[i];
cvSet2D(img, pp.y, pp.x, cvScalar(0));
}
if( neighbors.size() == (unsigned int)(img->widthStep*img->height)*0.5 ) // if more than 50% of the area is found as trapped, then something is wrong
{
std::cout << point.x << "," << point.y << " - " << (int)im2->imageData[point.y*im2->widthStep + point.x] << std::endl;
cvNamedWindow("make1");
cvNamedWindow("make2");
cvShowImage("make1", im2);
cvWaitKey(0);
cvShowImage("make2", im2);
cvWaitKey(0);
// cvNamedWindow("make3");
// cvShowImage("make3", img);
// cvWaitKey(0);
cvDestroyWindow("make1");
cvDestroyWindow("make2");
// cvDestroyWindow("make3");
}
// little hack to rectify mis-mapping of top most row and right most column
for(int j = 0; j < img->width-1; j++)
{
cvSet2D(img, 0, j, cvGet2D(img, 1, j));
}
for(int i = 0; i < img->height; i++)
{
cvSet2D(img, i, img->width-1, cvGet2D(img, i, img->width-2));
}
int count = 0;
for(int i = 0; i < img->height; i++)
{
for(int j = 0; j < img->widthStep; j++)
{
if( (unsigned int)img->imageData[i*img->widthStep + j] < (unsigned int)50 )
count++;
}
}
cvReleaseImage(&im2);
double totalCells = img->height*img->widthStep;
return ((double)count/totalCells);
}
void PointSets::addPointMeasure(Q3Canvas* canvas, Point* p, QString name, QList<QRect>* updateRectList)
{
PointSet* pointSet = findMeasure(name);
if (pointSet != 0)
pointSet->addPoint(canvas, p, updateRectList);
}
void Mesh::runCDT()
{
PointSet *ps = new PointSet();
for(uint i = 0; i < this->pNum; i++)
ps->addPoint(new Point(i, this->vertexBuffer[i*9], this->vertexBuffer[i*9+1]));
// Start with pointSet sorted by x
ps->sortPSet(X);
this->ps = ps;
printf("Start DT\n");
CDT(*ps);
#ifdef MESHSHADER
this->loadEdgeFromPointSet(*ps);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
this->shader->render(GL_ARRAY_BUFFER, this->VBid, this->assm, 0, this->pNum, true);
glfwSwapBuffers();
printf("End DT\n");
system("pause");
#endif
printf("Start Constrained\n");
boundConstraint(*ps);
#ifdef MESHSHADER
this->loadEdgeFromPointSet(*ps);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
this->shader->render(GL_ARRAY_BUFFER, this->VBid, this->assm, 0, this->pNum, true);
glfwSwapBuffers();
printf("End Constrained\n");
system("pause");
#endif
printf("Creating Spine\n");
int baseID = pruneAndSpine(*ps);
#ifdef MESHSHADER
this->loadEdgeFromPointSet(*ps);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
this->shader->render(GL_ARRAY_BUFFER, this->VBid, this->assm, 0, this->pNum, true);
glfwSwapBuffers();
printf("End Creating\n");
system("pause");
#endif
printf("Bubbling Up\n");
vector<Point*> spine = getSpine(*ps, baseID);
bubbleUp(*ps, spine, 1);
bubbleUp(*ps, spine, -1);
#ifdef MESHSHADER
this->loadEdgeFromPointSet(*ps);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
this->shader->render(GL_ARRAY_BUFFER, this->VBid, this->assm, 0, this->pNum, true);
glfwSwapBuffers();
printf("End Bubbling\n");
system("pause");
#endif
cleanSpineBase(*ps, spine);
setAllPointNormal(*ps);
printf("Finished\n");
this->loadTriangleFromPointSet(*ps);
}
void RangeProfilePlotManager::calculateDifferences()
{
// ensure we have only two objects selected
VERIFYNRV(mpView);
std::list<PlotObject*> selected;
mpView->getSelectedObjects(selected, true);
std::list<PlotObject*>::iterator selIt = selected.begin();
PointSet* pFirst = (selIt == selected.end()) ? NULL : dynamic_cast<PointSet*>(*(selIt++));
PointSet* pSecond = (selIt == selected.end()) ? NULL : dynamic_cast<PointSet*>(*(selIt++));
if (pFirst == NULL || pSecond == NULL)
{
return;
}
// locate the Difference point set
std::list<PlotObject*> allObjects;
mpView->getObjects(POINT_SET, allObjects);
PointSet* pDiffSet = NULL;
for (std::list<PlotObject*>::iterator obj = allObjects.begin(); obj != allObjects.end(); ++obj)
{
PointSet* pSet = static_cast<PointSet*>(*obj);
std::string name;
pSet->getObjectName(name);
if (name == "Difference")
{
pDiffSet = pSet;
pDiffSet->clear(true);
break;
}
}
if (pDiffSet == NULL)
{
pDiffSet = static_cast<PointSet*>(mpView->addObject(POINT_SET, true));
pDiffSet->setObjectName("Difference");
}
// calculate the differences and errors
std::vector<Point*> aPoints = pFirst->getPoints();
std::vector<Point*> bPoints = pSecond->getPoints();
if (aPoints.size() < 2 || bPoints.size() < 2)
{
return;
}
double mae = 0.0;
double mse1 = 0.0;
double mse2 = 0.0;
for (size_t aIdx = 0; aIdx < aPoints.size(); ++aIdx)
{
Point* pA = aPoints[aIdx];
VERIFYNRV(pA);
LocationType aVal = pA->getLocation();
LocationType newVal;
// locate the associated spot in b
for (size_t bIdx = 0; bIdx < bPoints.size(); ++bIdx)
{
Point* pB = bPoints[bIdx];
VERIFYNRV(pB);
LocationType bVal = pB->getLocation();
double diff = aVal.mX - bVal.mX;
if (fabs(diff) < 0.0000001) // a == b use the exact value
{
newVal.mX = aVal.mX;
newVal.mY = bVal.mY - aVal.mY;
break;
}
else if (diff < 0.0) // a < b found the upper point, interpolate
{
newVal.mX = aVal.mX;
LocationType secondBVal;
if (bIdx == 0) // we are at the start so continue the segment from the right
{
Point* pSecondB = bPoints[1];
VERIFYNRV(pSecondB);
secondBVal = pSecondB->getLocation();
}
else // grab the previous point for interpolation
{
Point* pSecondB = bPoints[bIdx-1];
VERIFYNRV(pSecondB);
secondBVal = pSecondB->getLocation();
}
// calculate slope-intercept
double m = (bVal.mY - secondBVal.mY) / (bVal.mX - secondBVal.mX);
double b = bVal.mY - m * bVal.mX;
// find the y corresponding to the interpolated x
newVal.mY = m * newVal.mX + b;
newVal.mY -= aVal.mY;
break;
}
}
mae += fabs(newVal.mY);
mse1 += newVal.mY * newVal.mY;
mse2 += (newVal.mY * newVal.mY) / (aVal.mY * aVal.mY);
pDiffSet->addPoint(newVal.mX, newVal.mY);
}
pDiffSet->setLineColor(ColorType(200, 0, 0));
mpView->refresh();
mae /= aPoints.size();
mse1 /= aPoints.size();
mse2 /= aPoints.size();
QMessageBox::information(mpView->getWidget(), "Comparison metrics",
QString("Mean squared error (method 1): %1\n"
"Mean squared error (method 2): %2\n"
"Mean absolute error: %3").arg(mse1).arg(mse2).arg(mae), QMessageBox::Close);
}
bool RangeProfilePlotManager::plotProfile(Signature* pSignature)
{
VERIFY(mpView && pSignature);
std::string plotName = pSignature->getDisplayName();
if (plotName.empty())
{
plotName = pSignature->getName();
}
if (plotName == "Difference")
{
QMessageBox::warning(Service<DesktopServices>()->getMainWidget(),
"Invalid signature", "Signatures can not be named 'Difference' as this is a reserved "
"name for this plot. Please rename your signature and try again.");
return false;
}
const Units* pXUnits = NULL;
const Units* pYUnits = NULL;
std::vector<double> xData;
std::vector<double> yData;
std::set<std::string> dataNames = pSignature->getDataNames();
for (std::set<std::string>::const_iterator dataName = dataNames.begin();
dataName != dataNames.end() && (pXUnits == NULL || pYUnits == NULL);
++dataName)
{
const Units* pUnits = pSignature->getUnits(*dataName);
if (pUnits == NULL)
{
continue;
}
if (pUnits->getUnitType() == DISTANCE)
{
if (pXUnits == NULL)
{
pXUnits = pUnits;
xData = dv_cast<std::vector<double> >(pSignature->getData(*dataName), std::vector<double>());
}
}
else if (pYUnits == NULL)
{
pYUnits = pUnits;
yData = dv_cast<std::vector<double> >(pSignature->getData(*dataName), std::vector<double>());
}
}
if (xData.empty() || xData.size() != yData.size())
{
QMessageBox::warning(Service<DesktopServices>()->getMainWidget(),
"Invalid signature", QString("Signatures must have a distance axis. '%1' does not and will not be plotted.")
.arg(QString::fromStdString(pSignature->getName())));
return false;
}
std::map<Signature*, std::string>::iterator oldPointSet = mSigPointSets.find(pSignature);
PointSet* pSet = getPointSet(pSignature);
if (pSet != NULL)
{
pSet->clear(true);
}
std::list<PlotObject*> curObjects;
mpView->getObjects(POINT_SET, curObjects);
if (pSet == NULL)
{
std::vector<ColorType> excluded;
excluded.push_back(ColorType(255, 255, 255)); // background
excluded.push_back(ColorType(200, 0, 0)); // color for the difference plot
for (std::list<PlotObject*>::const_iterator cur = curObjects.begin(); cur != curObjects.end(); ++cur)
{
excluded.push_back(static_cast<PointSet*>(*cur)->getLineColor());
}
pSet = static_cast<PointSet*>(mpView->addObject(POINT_SET, true));
mSigPointSets[pSignature] = plotName;
pSignature->attach(SIGNAL_NAME(Subject, Deleted), Slot(this, &RangeProfilePlotManager::signatureDeleted));
pSignature->getDataDescriptor()->attach(SIGNAL_NAME(DataDescriptor, Renamed),
Slot(this, &RangeProfilePlotManager::signatureRenamed));
std::vector<ColorType> colors;
ColorType::getUniqueColors(1, colors, excluded);
if (!colors.empty())
{
pSet->setLineColor(colors.front());
}
}
pSet->setObjectName(plotName);
for (size_t idx = 0; idx < xData.size(); ++idx)
{
pSet->addPoint(xData[idx], yData[idx]);
}
VERIFY(mpPlot);
Axis* pBottom = mpPlot->getAxis(AXIS_BOTTOM);
Axis* pLeft = mpPlot->getAxis(AXIS_LEFT);
VERIFYRV(pBottom && pLeft, NULL);
if (pBottom->getTitle().empty())
{
pBottom->setTitle(pXUnits->getUnitName());
}
if (pLeft->getTitle().empty())
{
pLeft->setTitle(pYUnits->getUnitName());
}
else if (pLeft->getTitle() != pYUnits->getUnitName())
{
Axis* pRight = mpPlot->getAxis(AXIS_RIGHT);
VERIFYRV(pRight, NULL);
if (pRight->getTitle().empty())
{
pRight->setTitle(pYUnits->getUnitName());
}
}
std::string classificationText = dv_cast<std::string>(pSignature->getMetadata()->getAttribute("Raw Classification"),
mpPlot->getClassificationText());
if (classificationText.empty() == false)
{
FactoryResource<Classification> pClassification;
if (pClassification->setClassification(classificationText) == true)
{
mpPlot->setClassification(pClassification.get());
}
else
{
QMessageBox::warning(Service<DesktopServices>()->getMainWidget(), QString::fromStdString(getName()),
"The plot could not be updated with the signature classification. Please ensure that the plot "
"has the proper classification.");
}
}
getDockWindow()->show();
mpView->zoomExtents();
mpView->refresh();
return true;
}
double createPointSets(const ThreadParams& params, bool sample, double stdDev, PointSet& altitude, PointSet& speed)
{
double startTime = 0;
double lastTime = 0;
altitude.clear();
speed.clear();
for(auto i = params.flightProfile.begin(); i != params.flightProfile.end(); ++i)
{
double time;
if(sample)
{
time = i->time.sample();
}
else
{
time = i->time.offsetMean(stdDev);
}
if(i == params.flightProfile.begin())
{
startTime = time;
}
else
{
lastTime = time - startTime;
}
if(!i->altitude.isNull())
{
double alt;
if(sample)
{
alt = i->altitude.sample();
}
else
{
alt = i->altitude.offsetMean(stdDev);
}
altitude.addPoint(time, alt);
}
if(!i->speed.isNull())
{
double spd;
if(sample)
{
spd = i->speed.sample();
}
else
{
spd = i->speed.offsetMean(stdDev);
}
speed.addPoint(time, spd);
}
}
//remove-me
// std::cerr << "altitude" << std::endl;
// for(size_t i = 0; i < altitude.size(); ++i)
// {
// std::cerr << "t=" << altitude[i].x_ << " altitude=" << altitude[i].y_ << std::endl;
// }
// std::cerr << "speed" << std::endl;
// for(size_t i = 0; i < speed.size(); ++i)
// {
// std::cerr << "t=" << speed[i].x_ << " speed=" << speed[i].y_ << std::endl;
// }
// std::cerr << "elapsed=" << lastTime << std::endl;
return lastTime;
}