void AlnGraphBoost::consensus(std::string& cns) {
// get the best scoring path
std::vector<AlnNode> path = bestPath();
cns.reserve(path.size());
std::vector<AlnNode>::iterator curr = path.begin();
for (; curr != path.end(); ++curr) {
AlnNode n = *curr;
if (n.base == _g[_enterVtx].base || n.base == _g[_exitVtx].base)
continue;
cns += n.base;
}
}
void AlnGraphBoost::consensus(std::vector<CnsResult>& seqs, int minWeight, size_t minLen) {
seqs.clear();
// get the best scoring path
std::vector<AlnNode> path = bestPath();
// consensus sequence
std::string cns;
// track the longest consensus path meeting minimum weight
int offs = 0, idx = 0;
bool metWeight = false;
std::vector<AlnNode>::iterator curr = path.begin();
for (; curr != path.end(); ++curr) {
AlnNode n = *curr;
if (n.base == _g[_enterVtx].base || n.base == _g[_exitVtx].base)
continue;
cns += n.base;
// initial beginning of minimum weight section
if (!metWeight && n.weight >= minWeight) {
offs = idx;
metWeight = true;
} else if (metWeight && n.weight < minWeight) {
// concluded minimum weight section, add sequence to supplied vector
metWeight = false;
CnsResult result;
result.range[0] = offs;
result.range[1] = idx;
size_t length = idx - offs;
result.seq = cns.substr(offs, length);
if (length >= minLen) seqs.push_back(result);
}
idx++;
}
// include end of sequence
if (metWeight) {
size_t length = idx - offs;
CnsResult result;
result.range[0] = offs;
result.range[1] = idx;
result.seq = cns.substr(offs, length);
if (length >= minLen) seqs.push_back(result);
}
}
const std::string AlnGraphBoost::consensus(int minWeight) {
// get the best scoring path
std::vector<AlnNode> path = bestPath();
// consensus sequence
std::string cns;
// track the longest consensus path meeting minimum weight
int offs = 0, bestOffs = 0, length = 0, idx = 0;
bool metWeight = false;
std::vector<AlnNode>::iterator curr = path.begin();
for (; curr != path.end(); ++curr) {
AlnNode n = *curr;
if (n.base == _g[_enterVtx].base || n.base == _g[_exitVtx].base)
continue;
cns += n.base;
// initial beginning of minimum weight section
if (!metWeight && n.weight >= minWeight) {
offs = idx;
metWeight = true;
} else if (metWeight && n.weight < minWeight) {
// concluded minimum weight section, update if longest seen so far
if ((idx - offs) > length) {
bestOffs = offs;
length = idx - offs;
}
metWeight = false;
}
idx++;
}
// include end of sequence
if (metWeight && (idx - offs) > length) {
bestOffs = offs;
length = idx - offs;
}
return cns.substr(bestOffs, length);
}
BestPath AltitudeMapPath::findBestPath(bool allowMultiThreading) {
unsigned int threadCount;
unsigned int threadIndex;
// Figure out the number of threads we can spawn
threadCount = std::thread::hardware_concurrency();
if (threadCount && allowMultiThreading) {
// Needs to be even (should be already but just in case)
threadCount = (threadCount % 2 == 0) ? threadCount : (threadCount - 1);
// Shared vectors
bestToPathSum.assign(threadCount, std::vector<unsigned int>(numRows, beyondMaxSum));
bestFromPathSum.assign(threadCount, std::vector<unsigned int>(numRows, beyondMaxSum));
bestToPathCode.assign(threadCount, std::vector<unsigned int>(numRows, 0));
bestFromPathCode.assign(threadCount, std::vector<unsigned int>(numRows, 0));
// Create threads
std::vector<std::thread> threads;
for (threadIndex = 0; threadIndex < threadCount; threadIndex++) {
threads.push_back(std::thread(&AltitudeMapPath::bestPath, this, threadCount, threadIndex));
}
// Return when all threads after they have completed
for (auto& thread : threads) {
thread.join();
}
}
else {
// Shared vectors
bestToPathSum.assign(1, std::vector<unsigned int>(numRows, beyondMaxSum));
bestFromPathSum.assign(1, std::vector<unsigned int>(numRows, beyondMaxSum));
bestToPathCode.assign(1, std::vector<unsigned int>(numRows, 0));
bestFromPathCode.assign(1, std::vector<unsigned int>(numRows, 0));
// No need to create threads just call directly
threadCount = 1;
threadIndex = 0;
bestPath(threadCount, threadIndex);
}
std::vector<unsigned int> combinedBestToPathSum(numRows, beyondMaxSum);
std::vector<unsigned int> combinedBestFromPathSum(numRows, beyondMaxSum);
std::vector<unsigned int> combinedBestToPathCode(numRows, 0);
std::vector<unsigned int> combinedBestFromPathCode(numRows, 0);
// Combined thread results
for (threadIndex = 0; threadIndex < threadCount; threadIndex++) {
for (unsigned int row = 0; row < numRows; row++) {
if (bestToPathSum[threadIndex][row] < combinedBestToPathSum[row]) {
combinedBestToPathSum[row] = bestToPathSum[threadIndex][row];
combinedBestToPathCode[row] = bestToPathCode[threadIndex][row];
}
if (bestFromPathSum[threadIndex][row] < combinedBestFromPathSum[row]) {
combinedBestFromPathSum[row] = bestFromPathSum[threadIndex][row];
combinedBestFromPathCode[row] = bestFromPathCode[threadIndex][row];
}
}
}
// Determine the best of the returned BestPath
unsigned int tempSum;
Coordinates bestDiagCoord;
BestPath path;
path.sum = beyondMaxSum;
unsigned int col = numCols - 1;
for (unsigned int row = 0; row < numRows; row++) {
bestDiagCoord = pathedMap(row, col--);
tempSum = combinedBestToPathSum[row] + combinedBestFromPathSum[row] - bestDiagCoord.z;
if (tempSum < path.sum) {
path.prefix = combinedBestToPathCode[row];
path.suffix = combinedBestFromPathCode[row];
path.sum = tempSum;
}
}
return path;
}
// Get the best available control input (given planning time)
bool Planner3D::getControlSequence(std::list<Control>& clist)
{
m_rrttree.clear();
m_rrtpath.clear();
m_rrtpaths.clear();
m_metric.clear();
PathNode pn;
bool terminate = false;
bool retcode = false;
Matrix<3,1> ctrl;
bool flip = false;
if (m_numIter == 0 || m_replanFlag) {
retcode = buildRRT(m_rrttree, m_rrtpaths, m_plantime);
}
if (retcode)
{
m_solution.clear();
m_currNode = 0;
if (m_mtype == DISTANCE || m_mtype == CLEARANCE) {
bestPath(m_rrttree, m_rrtpaths);
} else {
std::cerr << "Metric type not supported" << std::endl;
std::exit(-1);
}
pn = m_solution[m_currNode];
ctrl = pn.m_u;
//PathNode & pnxt = m_solution[m_currNode+1];
//std::cout << "Expected pose:\n" << pnxt.m_T << std::endl;
displayBestPath();
}
else
{
if (m_currNode < (int)m_solution.size()-2) {
++m_currNode;
pn = m_solution[m_currNode];
if (m_replanFlag) {
std::vector<Matrix<3,1> > uvec;
for(int i = m_currNode; i < (int)m_solution.size()-1; ++i) {
uvec.push_back(m_solution[i].m_u);
}
Matrix<4,4> Tnew;
selectBestTwist(m_npose, uvec, Tnew, ctrl);
m_solution[m_currNode+1].m_T = Tnew;
} else {
ctrl = pn.m_u;
}
} else {
terminate = true;
//std::cout << "TERMINATED!" << std::endl;
}
}
//std::cout << "PAUSED" << std::endl;
//int num;
//std::cin >> num;
if (!terminate) {
std::cout << "Planning matrix:\n" << pn.m_T << std::endl;
getControls(ctrl, flip, clist);
}
++m_numIter;
return terminate;
}
