TargetPhraseCollection &PhraseDictionaryFuzzyMatch::GetOrCreateTargetPhraseCollection(PhraseDictionaryNodeMemory &rootNode
, const Phrase &source
, const TargetPhrase &target
, const Word *sourceLHS)
{
PhraseDictionaryNodeMemory &currNode = GetOrCreateNode(rootNode, source, target, sourceLHS);
return currNode.GetTargetPhraseCollection();
}
TargetPhraseCollection &PhraseDictionaryMemory::GetOrCreateTargetPhraseCollection(
const Phrase &source
, const TargetPhrase &target
, const Word *sourceLHS)
{
PhraseDictionaryNodeMemory &currNode = GetOrCreateNode(source, target, sourceLHS);
return currNode.GetTargetPhraseCollection();
}
TargetPhraseCollection &RuleTrieCYKPlus::GetOrCreateTargetPhraseCollection(
const Phrase &source, const TargetPhrase &target, const Word *sourceLHS)
{
Node &currNode = GetOrCreateNode(source, target, sourceLHS);
return currNode.GetTargetPhraseCollection();
}
TargetPhraseCollection &RuleTableUTrie::GetOrCreateTargetPhraseCollection(
const Phrase &source, const TargetPhrase &target, const Word *sourceLHS)
{
UTrieNode &currNode = GetOrCreateNode(source, target, sourceLHS);
return currNode.GetOrCreateTargetPhraseCollection(target);
}
TargetPhraseCollection &PhraseDictionarySCFG::GetOrCreateTargetPhraseCollection(const Phrase &source, const TargetPhrase &target)
{
PhraseDictionaryNodeSCFG &currNode = GetOrCreateNode(source, target);
return currNode.GetOrCreateTargetPhraseCollection();
}
/**************************************************************************
CostMap handler
updates costmap for D* path planning
**************************************************************************/
void CostMapHandler(void *data)
{
LARGE_INTEGER frequency, start, finish;
QueryPerformanceFrequency(&frequency);
QueryPerformanceCounter(&start);
CostMapConvert(*(reinterpret_cast<CostMapType*>(data)));
receivedCostMap = true;
receivedNew = true;
static bool **cellsUpdated;
if(costmap == NULL || !costmap->equals(cmpInt))
{
//Todo: actually delete memory here, not just resetting them
cout << "Creating new costmap...\n";
if (costmap != NULL) {
delete costmap;
delete [] cellsUpdated[0];
delete [] cellsUpdated;
}
/* save the original number for back conversion in the end */
CostMapType *costmapUpdate = reinterpret_cast<CostMapType*>(data);
//TODO: Hack this for IGVC 2010
//costmapXMax = costmapUpdate->xMax;
//costmapXMin = costmapUpdate->xMin;
//costmapXUnit = costmapUpdate->xUnitSize;
//costmapYMax = costmapUpdate->yMax;
//costmapYMin = costmapUpdate->yMin;
//costmapYUnit = costmapUpdate->yUnitSize;
xMax = cmpInt->xMax;
yMax = cmpInt->yMax;
costmap = new CostMapInt(xMax+1, yMax+1);
// Variables and arrays to keep track of which cells have been updated and what their old values are
cellsUpdated = new bool*[xMax+1]; //cells changed in both cmpInt and squarecosts
bool *curPtr1 = new bool[(xMax+1) * (yMax+1)]();
for( int i = 0; i < xMax+1; i++) {
*(cellsUpdated + i) = curPtr1;
curPtr1 += yMax + 1;
}
}
static vector<int> cellsUpdatedXs;
static vector<int> cellsUpdatedYs;
int cellUpdateCount = 0; //take out in release!!!
static const double cellsD = 6;
static int cells = 5;
costmap->update(cmpInt);
//QueryPerformanceCounter(&finish);
//double timeelapsed = ((double)(finish.QuadPart - start.QuadPart))/frequency.QuadPart;
//cout << "Map Update Time: " << timeelapsed << " s\n";
// Clears cellsUpdated, cellsUpdatedXs, and cellsUpdatedYs.
//cout << "Cells Updated: " << cellUpdateCount << endl;
if (!isInitialSearch) {
for (size_t i = 0; i < costmap->cellsUpdatedXs.size(); ++i) {
int x = costmap->cellsUpdatedXs[i];
int y = costmap->cellsUpdatedYs[i];
for (int j = 0; j < lattice.update_n; ++j) {
//for each corner of updated cell
int m = x + lattice.update_dx[j];
int n = y + lattice.update_dy[j];
if (!isInMap(m, n)) continue;
if (memory[m][n] == 0) continue;
Node *s = GetOrCreateNode(m, n);
if (s->bptr == 0) continue;
if (s != startNode) {
UpdateRHSandBptr(s);
UpdateState(s);
}
}
//update itself
if (!isInMap(x, y)) continue;
if (memory[x][y] == 0) continue;
Node *s = GetOrCreateNode(x, y);
if (s->bptr == 0) continue;
if (s != startNode) {
UpdateRHSandBptr(s);
UpdateState(s);
}
}
}
//use resize() instead of clear() to keep capacity unchanged
//this is also done in the costmap internal function, but I'm keeping this here in case no points are updated
costmap->cellsUpdatedXs.resize(0);
costmap->cellsUpdatedYs.resize(0);
QueryPerformanceCounter(&finish);
double timeelapsed = ((double)(finish.QuadPart - start.QuadPart))/frequency.QuadPart;
cout << "Map Update Time: " << timeelapsed << " s\n";
}
void FindPath(void *data, unsigned long a, unsigned long b)
{
if (isInitialSearch) {
startX = (int)((stateEstimationUpdate.Easting - costmapXMin) / costmapXUnit);
startY = (int)((stateEstimationUpdate.Northing - costmapYMin) / costmapYUnit);
//startX = 1940;
//startY = 1093;
//goalX = 1850;
//goalY = 2150;
InitialSearch();
} else {
#ifdef DEBUGMAP
static bool searched = false;
if (searched == true) return;
searched = true;
vector<int> cellsUpdatedXs, cellsUpdatedYs;
for (int i = 11; i < 20; ++i)
for (int j = 7; j < 17; ++j) {
cellsUpdatedXs.push_back(i);
cellsUpdatedYs.push_back(j);
costmap->set(i, j, 3000, 0);
}
//for (int i = 2; i < 9; ++i)
// for (int j = 5; j < 9; ++j) {
// cellsUpdatedXs.push_back(i);
// cellsUpdatedYs.push_back(j);
// costmap->set(i, j, 3000, 2);
// }
for (size_t i = 0; i < cellsUpdatedXs.size(); ++i) {
int x = cellsUpdatedXs[i];
int y = cellsUpdatedYs[i];
for (int j = 0; j < lattice.update_n; ++j) {
//for each corner of updated cell
int m = x + lattice.update_dx[j];
int n = y + lattice.update_dy[j];
if (!isInMap(m, n)) return;
Node *s = GetOrCreateNode(m, n);
if (s != startNode)
UpdateRHSandBptr(s);
UpdateState(s);
}
//update itself
if (!isInMap(x, y)) return;
Node *s = GetOrCreateNode(x, y);
if (s != startNode)
UpdateRHSandBptr(s);
UpdateState(s);
}
#else
if (goalX == -1 || goalY == -1) return; //no new waypoints
if (receivedWaypoints) {
InitialSearch();
receivedWaypoints = false;
return;
}
if(!receivedNew || !receivedCostMap || !receivedStateEstimation )
return;
receivedNew = false;
LARGE_INTEGER frequency, start, finish;
QueryPerformanceFrequency(&frequency);
QueryPerformanceCounter(&start);
int oldStartX = startX;
int oldStartY = startY;
startX = (int)((stateEstimationUpdate.Easting - costmapXMin) / costmapXUnit);
startY = (int)((stateEstimationUpdate.Northing - costmapYMin) / costmapYUnit);
//startX = 1940;
//startY = 1093;
//spinCounter++;
//if (spinCounter >= 20) spinCounter = 0;
//if (spinCounter == 0) {
// startX = 2000;
// startY = 2000;
// goalX = 1700;
// goalY = 2300;
// receivedNew = true;
// receivedCostMap = true;
// InitialSearch();
// return;
//} else if (spinCounter == 10) {
// startX = 2000;
// startY = 2000;
// goalX = 2500;
// goalY = 2500;
// receivedNew = true;
// receivedCostMap = true;
// InitialSearch();
// return;
//}
//Avoid Oscillations
int closestX , closestY, closestIndex;
int closestDistSquare = LARGE_INT;
for (int i = 0; i < 50; ++i) {
if (i == prevPath.size()) break;
int prevPathX = int((prevPath[i].x - costmapXMin) / costmapXUnit + 0.5f);
int prevPathY = int((prevPath[i].y - costmapYMin) / costmapYUnit + 0.5f);
int prevDistSquare = (prevPathX - startX) * (prevPathX - startX) + (prevPathY - startY) * (prevPathY - startY);
if (prevDistSquare < closestDistSquare) {
closestDistSquare = prevDistSquare;
closestX = prevPathX;
closestY = prevPathY;
closestIndex = i;
}
}
if (closestDistSquare < 40) {
startX = int((prevPath[max(closestIndex-20, 0)].x - costmapXMin) / costmapXUnit + 0.5f);
startY = int((prevPath[max(closestIndex-20, 0)].y - costmapYMin) / costmapYUnit + 0.5f);
}
// planning failure if we start in an obstacle
if ((*costmap)(startX, startY) > 280){
startX = (int)((stateEstimationUpdate.Easting - costmapXMin) / costmapXUnit);
startY = (int)((stateEstimationUpdate.Northing - costmapYMin) / costmapYUnit);
}
if ((*costmap)(startX, startY) > 280 || (*costmap)(goalX, goalY) > 280) {
cout << "Planning failed before it began." << endl;
return;
}
if (memory[startX][startY] != 0) {
startNode = memory[startX][startY];
} else {
startNode = new Node(startX, startY, DOUBLE_INF, DOUBLE_INF);
memory[startX][startY] = startNode;
usedXs.push_back(startX);
usedYs.push_back(startY);
}
if (memory[oldStartX][oldStartY] != 0 && memory[oldStartX][oldStartY]->used == false) {
usedXs.push_back(oldStartX);
usedYs.push_back(oldStartY);
memory[oldStartX][oldStartY]->used = true;
}
if (startNode->used == false) {
usedXs.push_back(startX);
usedYs.push_back(startY);
memory[startX][startY]->used = true;
}
Node::currStart = startNode;
#endif
//if (Node::epsilon == 1.9)
// Node::epsilon = 1.5;
//else if (Node::epsilon == 1.5)
// Node::epsilon = 1.1;
//else if (Node::epsilon == 1.1)
// Node::epsilon = 1.05;
//else if (Node::epsilon == 1.05)
// Node::epsilon = 1.03;
//else if (Node::epsilon == 1.03)
// Node::epsilon = 1.02;
//else if (Node::epsilon == 1.02)
// Node::epsilon = 1.01;
//else if (Node::epsilon == 1.01)
// Node::epsilon = 1.0;
//else if (Node::epsilon == 1.0) {
// isInitialSearch = true;
// //Node::epsilon = 2.5;
// return;
//}
Node::epsilon = 1.0;
vector<Node*> oldOpen = open->clear();
for (size_t i = 0; i < oldOpen.size(); ++i ) {
oldOpen[i]->recalcKey();
open->add(oldOpen[i]);
}
for (list<Node*>::iterator it = incons.begin(); it != incons.end(); ++it) {
(*it)->inIncons = false;
if (!(*it)->inOpen) {
(*it)->recalcKey();
open->add(*it);
}
}
incons.clear();
for (list<Node*>::iterator it = closed.begin(); it != closed.end(); ++it) {
(*it)->inClosed = false;
}
closed.clear();
//cout << "Replanning..." << endl;
ComputeOrImprovePath();
#ifdef DEBUGMAP
print(memory, 0, 0, xMax, yMax);
#else
QueryPerformanceCounter(&finish);
if (planningFailed) {
planningFailed = false;
cout << "Planning Failed with " << timesExpanded << " times expanded." << endl;
double timeelapsed = ((double)(finish.QuadPart - start.QuadPart))/frequency.QuadPart;
cout << "Time: " << timeelapsed << "s" << endl;
timesExpanded = 0;
return;
}
if (timesExpanded > 0) {
cout << timesExpanded << " times expanded, epsilon = " << Node::epsilon << endl;
timesExpanded = 0;
double timeelapsed = ((double)(finish.QuadPart - start.QuadPart))/frequency.QuadPart;
cout << "Time: " << timeelapsed << "s" << endl;
}
//cout << "Making path..." << endl;
ExtractPath();
#endif
}
}
void ComputeOrImprovePath()
{
while (1) {
if (timesExpanded > MAX_TIMES_EXPANDED) {
planningFailed = true;
return;
}
Node *s1 = open->peekTop();
if (s1 == 0) {
planningFailed = true;
return;
}
if (less_than(s1->key.k1, startNode->key.k1) || (equals(s1->key.k1, startNode->key.k1) && less_than(s1->key.k2, startNode->key.k2))
|| (greater_than(startNode->rhs, startNode->g)) || (startNode->rhs == DOUBLE_INF && startNode->g == DOUBLE_INF))
;
else
break;
Node *s = open->removeTop();
if (greater_than(s->g, s->rhs)) {
s->g = s->rhs;
if (!s->inClosed) {
s->inClosed = true;
closed.push_back(s);
}
for (int i = 0; i < lattice.n; ++i) {
int x = s->x + lattice.dx[i];
int y = s->y + lattice.dy[i];
if (!isInMap(x, y))
continue;
Node *v = GetOrCreateNode(x, y);
if (s->bptr == v) continue;
double newRhs = ComputeCost(v, s->x, s->y);
if (greater_than(v->rhs, newRhs)) {
v->bptr = s;
v->rhs = newRhs;
UpdateState(v);
}
}
++timesExpanded;
} else {
s->g = DOUBLE_INF;
UpdateState(s);
for (int i = 0; i < lattice.n; ++i) {
int x = s->x + lattice.dx[i];
int y = s->y + lattice.dy[i];
Node *v = GetOrCreateNode(x, y);
if (v != goalNode && v->bptr == s) {
UpdateRHSandBptr(v);
UpdateState(v);
}
}
++timesExpanded;
}
}
}
