result_type
operator () (lhs_op_rhs const & _top) const
{
return dispatcher_(output_.at(_top.lhs_),
_top.operator_,
output_.at(_top.rhs_));
}
EXPORT void arrange_twin(std::deque<HWND> const& hwnds_, long const& width_, long const& height_){
if(0 == hwnds_.size()){
// nop
}
else if(1 == hwnds_.size()){
resize_window(hwnds_.at(0), HWND_NOTOPMOST, SW_SHOWNORMAL, 0, 0, width_, height_);
}
else if(2 == hwnds_.size()){
resize_window(hwnds_.at(0), HWND_NOTOPMOST, SW_SHOWNORMAL, 0, 0, width_ / 2 , height_);
resize_window(hwnds_.at(1), HWND_NOTOPMOST, SW_SHOWNORMAL, width_ / 2, 0, width_ / 2, height_);
}
else{
long const n = hwnds_.size() - 2;
long const sub_width = width_ / n;
resize_window(hwnds_.at(0), HWND_NOTOPMOST, SW_SHOWNORMAL, 0 , 0, width_ / 2, height_ / 4 * 3);
resize_window(hwnds_.at(1), HWND_NOTOPMOST, SW_SHOWNORMAL, width_ / 2, 0, width_ / 2, height_ / 4 * 3);
for(unsigned int i = 2; i < hwnds_.size(); i++){
resize_window(hwnds_.at(i), HWND_NOTOPMOST, SW_SHOWNORMAL,
(sub_width * (i - 2)),
(height_ / 4 * 3),
(sub_width),
(height_ / 4 * 1));
}
}
}
int maximumContiguousSum(std::deque<int> &numbers) {
int max = 0;
std::deque<int>::iterator boundary;
std::deque<int>::iterator adder;
while (numbers.size() > 0) {
for (boundary = numbers.end(); boundary != numbers.begin(); boundary--) {
int testSum = 0;
for (adder = numbers.begin(); adder != boundary; adder++) {
testSum += *adder;
}
if (testSum > max) { max = testSum; }
}
for (boundary = numbers.begin(); boundary != numbers.end(); boundary++) {
int testSum = 0;
for (adder = boundary; adder != numbers.end(); adder++) {
testSum += *adder;
}
if (testSum > max) { max = testSum; }
}
if (!numbers.empty()) { numbers.pop_back(); }
if (!numbers.empty()) { numbers.pop_front(); }
if (numbers.size() == 1) {
if (numbers.at(0) > max) { max = numbers.at(0); }
}
}
return max;
}
//Realiza el algoritmo de Melkman para hallar el convex hull de un poligono simple
// La entrada Q debe ser un poligono simple ordenado
void melkman(std::list<Point2D> & Q, std::deque<Point2D> & D){
bool volar_b = false;
bool volar_t = false;
unsigned int m = 0;
unsigned int b = 0;
unsigned int t = 0;
std::vector<Point2D> V;
std::list<Point2D>::iterator p = Q.begin();
while(p != Q.end()){
V.push_back(*p);
p++;
}
//Inicializa la deque con los primeros 3 en CCW
if(right(V[0], V[1], V[2])){
D.push_front(V[2]);
D.push_front(V[1]);
D.push_front(V[0]);
D.push_front(V[2]);
} else {
D.push_front(V[2]);
D.push_front(V[0]);
D.push_front(V[1]);
D.push_front(V[2]);
}
unsigned int n = Q.size();
unsigned int i = 2;
while((++i) < n){
m = D.size();
b = 0;
t = D.size()-1;
volar_b = right(V[i], D.at(b), D.at(b+1));
volar_t = right(D.at(t-1), D.at(t), V[i]);
if(!volar_b && !volar_t) //En el cono interno, no se agrega
continue;
while(volar_b){
D.pop_front();
volar_b = right(V[i], D.at(b), D.at(b+1));
}
D.push_front(V[i]);//Dentro segun el primer segmento
t = D.size()-1;
volar_t = right(D.at(t-1), D.at(t), V[i]);
while(volar_t){
t--;
D.pop_back();
volar_t = right(D.at(t-1), D.at(t), V[i]);
}
D.push_back(V[i]); //Dentro segun el ultimo segmento
}
}
int main(int, char**)
{
{
std::deque<int> c = make<std::deque<int> >(10);
for (int i = 0; i < 10; ++i)
assert(c[i] == i);
for (int i = 0; i < 10; ++i)
assert(c.at(i) == i);
assert(c.front() == 0);
assert(c.back() == 9);
}
{
const std::deque<int> c = make<std::deque<int> >(10);
for (int i = 0; i < 10; ++i)
assert(c[i] == i);
for (int i = 0; i < 10; ++i)
assert(c.at(i) == i);
assert(c.front() == 0);
assert(c.back() == 9);
}
#if TEST_STD_VER >= 11
{
std::deque<int, min_allocator<int>> c = make<std::deque<int, min_allocator<int>> >(10);
for (int i = 0; i < 10; ++i)
assert(c[i] == i);
for (int i = 0; i < 10; ++i)
assert(c.at(i) == i);
assert(c.front() == 0);
assert(c.back() == 9);
}
{
const std::deque<int, min_allocator<int>> c = make<std::deque<int, min_allocator<int>> >(10);
for (int i = 0; i < 10; ++i)
assert(c[i] == i);
for (int i = 0; i < 10; ++i)
assert(c.at(i) == i);
assert(c.front() == 0);
assert(c.back() == 9);
}
#endif
return 0;
}
int getSpellTypeFromName(std::string name)
{
for(int i = 0;i<SpellNames.size();i++)
{
if(SpellNames.at(i)==name)
{
return i;
}
}
return -1;
}
int main()
{
{
std::deque<int> c = make<std::deque<int> >(10);
for (unsigned i = 0; i < 10; ++i)
assert(c[i] == i);
for (unsigned i = 0; i < 10; ++i)
assert(c.at(i) == i);
assert(c.front() == 0);
assert(c.back() == 9);
}
{
const std::deque<int> c = make<std::deque<int> >(10);
for (unsigned i = 0; i < 10; ++i)
assert(c[i] == i);
for (unsigned i = 0; i < 10; ++i)
assert(c.at(i) == i);
assert(c.front() == 0);
assert(c.back() == 9);
}
#if __cplusplus >= 201103L || defined(_LIBCPP_MSVC)
{
std::deque<int, min_allocator<int>> c = make<std::deque<int, min_allocator<int>> >(10);
for (unsigned i = 0; i < 10; ++i)
assert(c[i] == i);
for (unsigned i = 0; i < 10; ++i)
assert(c.at(i) == i);
assert(c.front() == 0);
assert(c.back() == 9);
}
{
const std::deque<int, min_allocator<int>> c = make<std::deque<int, min_allocator<int>> >(10);
for (unsigned i = 0; i < 10; ++i)
assert(c[i] == i);
for (unsigned i = 0; i < 10; ++i)
assert(c.at(i) == i);
assert(c.front() == 0);
assert(c.back() == 9);
}
#endif
}
void Draw_Buildings(sf::RenderWindow& window)
{
for (unsigned i = 0; i < LowerBuildings.size(); ++i)
{
Building building = LowerBuildings.at(i);
sf::RectangleShape buildingShape(sf::Vector2f(ORIGINAL_WIDTH / 10, building.height));
buildingShape.setFillColor(sf::Color(0, 0, 100));
buildingShape.setPosition(building.x, building.y);
buildingShape.setOutlineColor(sf::Color(0, 0, 0));
buildingShape.setOutlineThickness(1);
window.draw(buildingShape);
}
}
// Checks if buildings collide with player
// TODO: Check for bullets
void Collision_Check_Buildings(void)
{
// Figure out where the helicopter is
// Check the buildings within the same x area to save computation
unsigned x, y;
Player->Get_Location(&x, &y);
int right = x + 70;
int bottom = y + 40;
if (LowerBuildings.size() < 10) // Not a screen full of buildings, just check all buildings
{
for (unsigned i = 0; i < LowerBuildings.size(); ++i)
{
if (LowerBuildings.at(i).x < right)
{
if (LowerBuildings.at(i).y < bottom)
{
// Collision, game over
assert(false);
}
}
}
}
else
{
unsigned buildingIndex = std::max(right / (ORIGINAL_WIDTH / 10), (unsigned)0);
for (unsigned i = 0; i < std::min((unsigned)LowerBuildings.size() - buildingIndex, (unsigned)3); ++i)
{
if (LowerBuildings.at(i + buildingIndex).x < right)
{
if (LowerBuildings.at(i + buildingIndex).y < bottom)
{
// Collision, game over
assert(false);
}
}
}
}
}
void Move_Buildings(void)
{
// We should have created a building before this is called
// Otherwise, we will null pointer exception, for the cost of not checking if LowerBuildings is not empty
for (unsigned i = 0; i < LowerBuildings.size(); ++i)
{
LowerBuildings.at(i).x -= 1;
}
if (LowerBuildings.front().x <= -(int)ORIGINAL_WIDTH / 10)
{
LowerBuildings.pop_front();
}
Collision_Check_Buildings();
}
inline std::vector<std::string>& CsvFile::GetRow(int row)
{
std::vector<std::string>& line = mData.at(row);
while (line.size() < mColCount)
{
line.push_back("");
}
while (line.size() > mColCount)
{
line.pop_back();
}
assert(line.size() == mColCount);
return line;
}
void RunVisualizationOnly()
{
if (!viewer->wasStopped())
{
viewer->spinOnce(100);
boost::mutex::scoped_lock update_pc_lock(update_pc_model_mutex);
if (update_pc)
{
if (!viewer->updatePointCloud(cloud_ptr, "cloud"))
{
viewer->addPointCloud(cloud_ptr, "cloud");
}
update_pc = false;
}
update_pc_lock.unlock();
boost::mutex::scoped_lock update_camera_lock(add_camera_mutex);
if (update_camera)
{
if (aggregate_view_frustrums)
{
camera_count = 0;
for (int i=0; i<cam_meshes.size(); i++)
{
viewer->removePolygonMesh(to_string(camera_count));
viewer->addPolygonMesh(cam_meshes.at(i).second, to_string(camera_count));
camera_count++;
}
}
else
{
viewer->removePolygonMesh(to_string(0));
viewer->addPolygonMesh(cam_meshes.back().second, to_string(0));
}
update_camera = false;
}
update_camera_lock.unlock();
viewer->setRepresentationToWireframeForAllActors();
}
}
cv::Mat CvHelper::convertPoint2fDeque2Mat(std::deque<cv::Point2f> points)
{
cv::Mat mat;
if(points.empty())
return mat;
mat = cv::Mat(points.size(), 2, CV_32F);
for (int i = 0; i < points.size(); i++)
{
cv::Point2f p = points.at(i);
mat.at<float>(i,0)= p.x;
mat.at<float>(i,1)= p.y;
}
return mat;
}
void spnavCallback(const geometry_msgs::WrenchStamped::ConstPtr& msg)
{
if(bRunning)
{
double average_z = 0.0;
if(num_cache == 0)
z_cache.pop_front();
else
num_cache--;
z_cache.push_back(msg->wrench.force.z + offset);
for(unsigned int i = 0; i < z_cache.size(); i++)
average_z += z_cache.at(i);
average_z /= z_cache.size();
geometry_msgs::Twist new_twist;
/*if(msg->wrench.force.x > 5.0)
new_twist.linear.x = 0.01;
if(msg->wrench.force.x < 5.0)
new_twist.linear.x = -0.01;
if(msg->wrench.force.y > 5.0)
new_twist.linear.y = 0.01;
if(msg->wrench.force.y < 5.0)
new_twist.linear.y = -0.01;*/
if(average_z < -1.0)
new_twist.linear.z = -0.01 * average_z;
else
{
if(average_z > 1.0)
new_twist.linear.z = -0.01 * average_z;
else
new_twist.linear.z = 0.0;
}
std::cout << "Sending twist of " << new_twist.linear.z << " current force is " << average_z << "\n";
twist_pub_.publish(new_twist);
}
}
Spell::Spell(int id,int x,int y,lua_State* L) //custom lua state
{
SpellType = id;
sprite = sf::Sprite(SpellTextures.at(id));
sprite.setPosition(x,y);
sprite.setScale(0.6f,0.6f);
cdtext = sf::Text("0",Font,10);
cdtext.setColor(sf::Color::White);
cdtext.setPosition(x+4,y+2);
cdbox = sf::RectangleShape(sf::Vector2f(10,10));
cdbox.setFillColor(sf::Color::Black);
cdbox.setPosition(x+3,y+3);
currCooldown = 0;
lua_getfield(L,-1,"range");
Range = lua_tointeger(L,-1);
//std::cout << "range : " << Range << "\n";
//lua_pop(L,-1);
lua_getfield(L,-2,"manaCost");
ManaCost = lua_tointeger(L,-1);
lua_getfield(L,-3,"moveCost");
MoveCost = lua_tointeger(L,-1);
//std::cout << "movecost : " << MoveCost << "\n";
lua_getfield(L,-4,"cooldown");
Cooldown = lua_tointeger(L,-1);
//std::cout << "cooldown : " << Cooldown << "\n";
lua_getfield(L,-5,"text");
Text = lua_tostring(L,-1);
lua_getfield(L,-6,"name");
Name = lua_tostring(L,-1);
}
void BitLatticeManipulator::sign_reduce_bitstring(
std::deque<bit_lattice> & bitstring,
bool bitstring_is_signed) const
{
THROW_ASSERT(not bitstring.empty(), "");
while (bitstring.size() > 1)
{
if (bitstring_is_signed)
{
if (bitstring.at(0) != bit_lattice::U and bitstring.at(0) == bitstring.at(1))
bitstring.pop_front();
else
break;
}
else
{
if ((bitstring.at(0) == bit_lattice::X and bitstring.at(1) == bit_lattice::X) or
(bitstring.at(0) == bit_lattice::ZERO and bitstring.at(1) != bit_lattice::X))
bitstring.pop_front();
else
break;
}
}
}
Spell::Spell(int id,int x,int y)
{
SpellType = id;
sprite = sf::Sprite(SpellTextures.at(id));
sprite.setPosition(x,y);
sprite.setScale(0.6f,0.6f);
cdtext = sf::Text("0",Font,10);
cdtext.setColor(sf::Color::White);
cdtext.setPosition(x+4,y+2);
cdbox = sf::RectangleShape(sf::Vector2f(10,10));
cdbox.setFillColor(sf::Color::Black);
cdbox.setPosition(x+3,y+3);
lua_State* L = luaOpen("Lua\\Spells.lua");
lua_getglobal(L,SpellNames.at(SpellType).c_str());
lua_getfield(L,1,"range");
Range = lua_tointeger(L,-1);
luaClean(L);
lua_getglobal(L,SpellNames.at(SpellType).c_str());
lua_getfield(L,1,"manaCost");
ManaCost = lua_tointeger(L,-1);
luaClean(L);
lua_getglobal(L,SpellNames.at(SpellType).c_str());
lua_getfield(L,1,"moveCost");
MoveCost = lua_tointeger(L,-1);
luaClean(L);
lua_getglobal(L,SpellNames.at(SpellType).c_str());
lua_getfield(L,1,"cooldown");
Cooldown = lua_tointeger(L,-1);
luaClean(L);
if(isSpellTypeSummon.at(SpellType)==1)
{
lua_getglobal(LuaUnits,SpellNames.at(SpellType).c_str());
lua_getfield(LuaUnits,-1,"text");
Text = lua_tostring(LuaUnits,-1);
lua_pop(LuaUnits,1);
lua_getfield(LuaUnits,-1,"name");
Name = lua_tostring(LuaUnits,-1);
lua_pop(LuaUnits,2);
}
else
{
lua_getglobal(L,SpellNames.at(SpellType).c_str());
lua_getfield(L,1,"text");
Text = lua_tostring(L,-1);
luaClean(L);
lua_getglobal(L,SpellNames.at(SpellType).c_str());
lua_getfield(L,1,"name");
Name = lua_tostring(L,-1);
luaClean(L);
}
currCooldown = 0;
lua_close(L);
}
void Preprocessor::findLineDelimiters (const std::deque<bool>& visited)
{
delimiters_.clear();
// Traverse each row searching non-visited pixels
unsigned int topRowOfTextLine = 0;
bool rowHasInk = false, previousRowHasInk;
for ( unsigned int i = 0; i < clipHeight_; ++i )
{
previousRowHasInk = rowHasInk;
rowHasInk = false;
for ( unsigned int j = 0; j < clipWidth_; ++j )
{
if ( visited.at(i * clipWidth_ + j) )
{
rowHasInk = true;
break;
}
}
if (not rowHasInk )
{
if ( previousRowHasInk )
delimiters_.push_back( LineDelimiter(topRowOfTextLine, i-1) );
else
topRowOfTextLine = i;
}
else
{
if ( not previousRowHasInk )
topRowOfTextLine = i;
}
}
// Make sure the last text line joins with the clip border
if ( rowHasInk )
delimiters_.push_back( LineDelimiter(topRowOfTextLine, clipHeight_-1) );
LineDelimiterIterator previousLineDelimiterIterator = delimiters_.begin();
LineDelimiterIterator currentLineDelimiterIterator = delimiters_.begin();
advance( currentLineDelimiterIterator, 1 );
// Search lines that are too close to each other, probably because there are accents belonging to characters on a single line
while ( currentLineDelimiterIterator != delimiters_.end() )
{
unsigned int currentLineHeight = currentLineDelimiterIterator->second - currentLineDelimiterIterator->first + 1;
unsigned int previousLineHeight = previousLineDelimiterIterator->second - previousLineDelimiterIterator->first + 1;
if ( previousLineHeight > (currentLineHeight / 2) )
{
advance( currentLineDelimiterIterator, 1 );
advance( previousLineDelimiterIterator, 1 );
}
else
{
// A new line delimiter is inserted by joining the two line delimiters explored.
delimiters_.insert( previousLineDelimiterIterator, LineDelimiter(previousLineDelimiterIterator->first, currentLineDelimiterIterator->second) );
LineDelimiterIterator newLineDelimiterIterator = previousLineDelimiterIterator;
advance ( newLineDelimiterIterator, -1 );
// The two old line delimiters are removed from the list
delimiters_.erase( previousLineDelimiterIterator );
previousLineDelimiterIterator = currentLineDelimiterIterator;
advance( currentLineDelimiterIterator, 1 );
delimiters_.erase( previousLineDelimiterIterator );
previousLineDelimiterIterator = newLineDelimiterIterator;
}
}
}
void GetLaneBaseline(const int &sampleIdx,
const int &TIME_SAMPLING_WINDOW,
int &muWindowSize, int &sigmaWindowSize,
const double &lateralOffset,
std::vector<double> &LATSD_Baseline,
std::deque<InfoCar> &lateralOffsetDeque,
std::deque<InfoCar> &LANEXDeque,
std::deque<InfoTLC> &TLCDeque,
LaneFeature &laneFeatures,
const double &intervalTime)
{
laneFeatures.frame = sampleIdx;
laneFeatures.lateralOffset = lateralOffset;
//! lateralOffsetDeque
InfoCar infoLO;
infoLO.lateralOffset = lateralOffset;
infoLO.intervalTime = intervalTime;
if(!lateralOffsetDeque.empty())
{
infoLO.winTime = lateralOffsetDeque.back().winTime + intervalTime;
} else {
infoLO.winTime = intervalTime;
}
while (infoLO.winTime > TIME_SAMPLING_WINDOW && !lateralOffsetDeque.empty())
{
infoLO.winTime -= lateralOffsetDeque.front().intervalTime;
lateralOffsetDeque.pop_front();
}
lateralOffsetDeque.push_back(infoLO);
//! Need to reset \param muWindowSize and \param sigmaWindowSize.
GetEWMA(sampleIdx, muWindowSize, lateralOffset , laneFeatures.LATMEAN);
GetEWVAR(sampleIdx, sigmaWindowSize, lateralOffset , laneFeatures.LATMEAN, laneFeatures.LANEDEV);
//! Standard Deviation of LATSD for Baseline
GetStandardDeviation(lateralOffsetDeque, laneFeatures.LATSD);
LATSD_Baseline.push_back(laneFeatures.LATSD);
sort(LATSD_Baseline.begin(), LATSD_Baseline.end(), LATSD_cmp);
//! Considering the sampling Time of LANEX different that of lateralOffset
//! Rebuild the InfoCar struct
InfoCar infoLANEX;
infoLANEX.lateralOffset = lateralOffset;
infoLANEX.intervalTime = intervalTime;
if(!LANEXDeque.empty())
{
infoLANEX.winTime = LANEXDeque.back().winTime + intervalTime;
} else {
infoLANEX.winTime = intervalTime;
}
while (infoLANEX.winTime > TIME_SAMPLING_WINDOW)
{
infoLANEX.winTime -= LANEXDeque.front().intervalTime;
LANEXDeque.pop_front();
}
LANEXDeque.push_back(infoLANEX);
double sumTime_LANEX = 0;
for(std::deque<InfoCar>::size_type i = 0; i != LANEXDeque.size(); ++i)
{
if(LANEXDeque[i].lateralOffset == 1)
{
sumTime_LANEX += LANEXDeque[i].intervalTime;
}
}
double LANEX = sumTime_LANEX / LANEXDeque.back().winTime;
if(sampleIdx > 1)
{
//! Add TLC into deque. Simple TLC
double TLC = 0, TLC_min = 0;
int TLC_2s = 0, TLC_halfs = 0;
double TLCF_2s = 0, TLCF_halfs = 0;
CV_Assert((int)lateralOffsetDeque.size() > 1);
double lastLateralOffset = lateralOffsetDeque.at(lateralOffsetDeque.size()-2).lateralOffset;
double lateralVelocity = lateralOffset - lastLateralOffset;
double TLC_Frame = 0;
if (lateralVelocity < 0)//direct to left
TLC_Frame = std::abs((1 + lateralOffset) / lateralVelocity);
else if(lateralVelocity > 0)//direct to right
TLC_Frame = std::abs((1 - lateralOffset) / lateralVelocity);
else
TLC_Frame = 10000;//Max TLC shows a safe deviation
TLC = TLC_Frame / intervalTime;//sec
TLC = TLC < 1000 ? TLC : 1000;
InfoTLC infoTLC;
infoTLC.TLC = TLC;
infoTLC.intervalTime = intervalTime;
if(!TLCDeque.empty())
{
infoTLC.winTime = TLCDeque.back().winTime + intervalTime;
} else {
infoTLC.winTime = intervalTime;
}
while (infoTLC.winTime > TIME_SAMPLING_WINDOW)
{
infoTLC.winTime -= TLCDeque.front().intervalTime;
TLCDeque.pop_front();
}
TLCDeque.push_back(infoTLC);
//! Number of times that TLC below a threshold
for(std::deque<double>::size_type i = 0; i != TLCDeque.size(); i++)
{
//!TLC with signal falling below 2s in a given time interval
if(TLCDeque[i].TLC < 2.0)
{
TLC_2s ++;
}
//!TLC with signal falling below 0.5s in a given time interval
if(TLCDeque[i].TLC < 0.5)
{
TLC_halfs ++;
}
}
//! Fraction of TLC_2s and TLC_halfs
TLCF_2s = (double)TLC_2s / (double)TLCDeque.size();
TLCF_halfs = (double)TLC_halfs / (double)TLCDeque.size();
//! Global minimum of TLC over a given time interval;
std::deque<InfoTLC> cmpTLCDeque = TLCDeque;
sort(cmpTLCDeque.begin(), cmpTLCDeque.end(), TLC_cmp);
TLC_min = cmpTLCDeque.back().TLC;
std::deque<InfoTLC>::iterator iter = cmpTLCDeque.end();
while (TLC_min == 0)
{
--iter;
TLC_min = iter->TLC;
}
//! Calculate the Baseline
//!Baseline of LATSD should be the median value.
laneFeatures.LATSD_Baseline = LATSD_Baseline.at(cvRound(LATSD_Baseline.size()/2.0));
//!Others
laneFeatures.LATMEAN_Baseline = laneFeatures.LATMEAN_Baseline > laneFeatures.LATMEAN ? laneFeatures.LATMEAN_Baseline : laneFeatures.LATMEAN;
laneFeatures.LANEDEV_Baseline = laneFeatures.LANEDEV_Baseline > laneFeatures.LANEDEV ? laneFeatures.LANEDEV_Baseline : laneFeatures.LANEDEV;
laneFeatures.LANEX = LANEX;
laneFeatures.TLC = TLC;
laneFeatures.TLC_2s = TLC_2s;
laneFeatures.TLC_halfs = TLC_halfs;
laneFeatures.TLC_min = TLC_min;
laneFeatures.TLCF_2s = TLCF_2s;
laneFeatures.TLCF_halfs = TLCF_halfs;
} else {
laneFeatures.LATMEAN_Baseline = 0;
laneFeatures.LANEDEV_Baseline = 0;
laneFeatures.LATSD_Baseline = 0;
laneFeatures.LANEX = 0;
laneFeatures.TLC = 0;
laneFeatures.TLC_2s = 0;
laneFeatures.TLC_halfs = 0;
laneFeatures.TLC_min = 0;
laneFeatures.TLCF_2s = 0;
laneFeatures.TLCF_halfs = 0;
}
}//end GetLaneBaseline
/***************************************************************
【函数功能】： 查找中间用于视觉校正的路径点
【输 入】： path : 完整路径队列
calibPoints : 输出找到的校正点
bFound : 输出是否找到
【输 出】： 无
【说 明】： 查找范围限定在机器人坐标系前方1m点为中心，0.8m*0.4m的矩形ROI区域
！！！试验过程中，发现路标前停止位置偏远，因此减少中心点距离为0.85米！！！
***************************************************************/
void CPathAgent::FindCalibPoses(std::deque<math::TPoint2D> &path,
std::deque<math::TPose2D> &calibPoses, bool &bFound)
{
std::deque<SPose2LM> dequePose2LM;
//std::deque<int> idLands;
calibPoses.clear();
bFound = false;
if(path.size() < 6) return; // 避免路径终点被分割成小段了
// 遍历路径点，判断在哪个路径点能看到路标
for (unsigned int i=5; i<(path.size()-5); i++) // 路径【头5点】和【尾5点】不参与。保证不会出现路径起末点小路径的情况
{
TPoint2D p = path.at(i); // 本次的点
TPoint2D n = path.at(i+1); // 后一个点
TPose2D p2n( n - p ); // p点到n点的矢量
if (abs(p2n.x) < 0.000001) // x==0
{
p2n.phi = (p2n.y > 0) ? (M_PI/2) : (-M_PI/2);
}
else
{
float k = p2n.y / p2n.x;
p2n.phi = (p2n.x > 0) ? atan(k) : (atan(k) + M_PI);
}
p2n.phi = wrapToPi(p2n.phi); // 机器人在该路径点的理想朝向角
CPose2D robot(p.x, p.y, p2n.phi);// 此点上机器人的大地位姿
CPose2D center2robot(m_dCalibCenterDistance, 0, 0); // 查找范围的中心点在机器人坐标系内的坐标
CPose2D center = robot + center2robot; // 查找范围的中心点在大地坐标系内的坐标
// 遍历路标，找到距离0.4米内的路标
for (int j=0; j<m_iLandMarkNum; j++)
{
TPose2D Land = m_pLandMarkPose[j]; // 路标的大地坐标
////////////// 换算到视野中心点坐标系内判断 ////////////////
CPose2D LMpose(Land);
CPose2D Land2Center = LMpose - center; // 路标在视野中心点坐标系内的坐标
CPose2D LandCen(0.05, 0.05, 0); // 路标帖的中心在路标自己坐标系内的坐标
CPose2D LMC2ViewCenter = Land2Center + LandCen; // 路标贴中心在视野中心坐标系内的坐标
if (abs(LMC2ViewCenter.x()) < abs(m_dCalibCenterDistance-0.7)) // 前后范围符合
{
if ( ((LMC2ViewCenter.x() >= 0) && (abs(LMC2ViewCenter.y()) < 0.5))
|| ((LMC2ViewCenter.x() < 0) && (abs(LMC2ViewCenter.y()) < 0.4)) )
{
// 先记下这个点，这个路标。下面再一个路标选一个最近的路径点
double ddis = LMC2ViewCenter.distance2DTo(0,0);
SPose2LM tpose2LM(robot.x(), robot.y(), robot.phi(), ddis, j);
dequePose2LM.push_back(tpose2LM);
}
}
///////////////// 换算判断结束 ////////////////////////
//double dis = center.distance2DTo(Land.x, Land.y);
//if (dis <= 0.4) // 0.4m半径的圆范围
//{
// // 这个路标点满足基本的距离条件，需要进一步判断是否在矩形ROI区域
// CPoint2D Land2Center(Land.x - center.x(), Land.y - center.y()); // 路标在center坐标系的坐标
// CPose2D TurnPose(0,0,-center.phi()); // 旋转到机器人朝向的center坐标系
// CPoint2D newLand2Center = TurnPose + Land2Center;
// if (abs(newLand2Center.x()) < 0.2) // 在矩形ROI区域
// {
// // 记下这个点，这个路标。一个路标选一个最近的路径点
// SPose2LM tpose2LM(robot.x(), robot.y(), robot.phi(), j);
// dequePose2LM.push_back(tpose2LM);
// }
//}
}
}
if (dequePose2LM.size() <= 0) // 没找到校正点
{
bFound = false;
return;
}
else
bFound = true;
// 每个路标只取一个最近校正点
TPose2D minPose;
double minDis = 10;
for (unsigned int i=0; i<dequePose2LM.size(); i++)
{
TPose2D tpose(dequePose2LM.at(i).x, dequePose2LM.at(i).y, dequePose2LM.at(i).phi);
int indexLM = dequePose2LM.at(i).index;
//CPose2D robot(tpose);
//CPose2D center2robot(m_dCalibCenterDistance, 0, 0); // 查找范围的中心点在机器人坐标系内的坐标
//CPose2D center = robot + center2robot; // 查找范围的中心点在大地坐标系内的坐标
//double dis = center.distance2DTo(m_pLandMarkPose[indexLM].x, m_pLandMarkPose[indexLM].y);
double dis = dequePose2LM.at(i).dis;
if (dis < minDis)
{
minDis = dis;
minPose = tpose;
}
if((i+1) < dequePose2LM.size())
{
if (dequePose2LM.at(i+1).index != indexLM) // 这个路标挑完了
{
calibPoses.push_back(minPose);
minDis = 10;
}
}
}
calibPoses.push_back(minPose); // 最后一个校正点
}
// This function builds the mass function based on lane features
// \param lateralOffset: current lateral offset
// \param intervalTime: the interval time between the two consective frames
//! Assign the mass from lane source
//! Standard deviation reflects the extent of wave about car. It will show the fatigue.
void GenerateLaneIndicators(const int &sampleIdx, const int &TIME_SAMPLING_WINDOW, int &muWindowSize, int &sigmaWindowSize,
const double &lateralOffset, std::deque<InfoCar> &lateralOffsetDeque, std::deque <InfoCar> &LANEXDeque,
std::deque<InfoTLC> &TLCDeque, LaneFeature &laneFeatures, const double &intervalTime)
{
double LATMEAN=laneFeatures.LATMEAN, LANEDEV=laneFeatures.LANEDEV, LATSD=0, LANEX=0, TLC=0, TLC_min=0;
int TLC_2s = 0, TLC_halfs = 0;
double TLCF_2s = 0, TLCF_halfs = 0;
//! lateralOffsetDeque
InfoCar infoLO;
infoLO.lateralOffset = lateralOffset;
infoLO.intervalTime = intervalTime;
if(!lateralOffsetDeque.empty())
{
infoLO.winTime = lateralOffsetDeque.back().winTime + intervalTime;
} else {
infoLO.winTime = intervalTime;
}
while (infoLO.winTime > TIME_SAMPLING_WINDOW && !lateralOffsetDeque.empty())
{
infoLO.winTime -= lateralOffsetDeque.front().intervalTime;
lateralOffsetDeque.pop_front();
}
lateralOffsetDeque.push_back(infoLO);
//! Need to reset \param muWindowSize and \param sigmaWindowSize.
GetEWMA(sampleIdx, muWindowSize, lateralOffset, LATMEAN);
GetEWVAR(sampleIdx, sigmaWindowSize, lateralOffset, LATMEAN, LANEDEV);
GetStandardDeviation(lateralOffsetDeque, LATSD);
std::cout << "LO: " << lateralOffset << "LATSD: " << LATSD << std::endl;
//! Considering the sampling Time of LANEX different that of lateralOffset
//! Rebuild the InfoCar struct
InfoCar infoLANEX;
infoLANEX.lateralOffset = lateralOffset;
infoLANEX.intervalTime = intervalTime;
if(!LANEXDeque.empty())
{
infoLANEX.winTime = LANEXDeque.back().winTime + intervalTime;
} else {
infoLANEX.winTime = intervalTime;
}
while (infoLANEX.winTime > TIME_SAMPLING_WINDOW)
{
infoLANEX.winTime -= LANEXDeque.front().intervalTime;
LANEXDeque.pop_front();
}
LANEXDeque.push_back(infoLANEX);
double sumTime_LANEX = 0, sumTime = 0;
for(std::deque<InfoCar>::size_type i = 0; i != LANEXDeque.size(); ++i)
{
if(LANEXDeque[i].lateralOffset == 1)
{
sumTime_LANEX += LANEXDeque[i].intervalTime;
}
sumTime += LANEXDeque[i].intervalTime;
}
LANEX = sumTime_LANEX / sumTime;
//! Time-to-Lane-Crossing
//! Simple TLC
CV_Assert((int)lateralOffsetDeque.size() > 1);
double lastLateralOffset = lateralOffsetDeque.at(lateralOffsetDeque.size()-2).lateralOffset;
double lateralVelocity = lateralOffset - lastLateralOffset;
double TLC_Frame = 0;
if (lateralVelocity < 0)//direct to left
TLC_Frame = std::abs((1 + lateralOffset) / lateralVelocity);
else if(lateralVelocity > 0)//direct to right
TLC_Frame = std::abs((1 - lateralOffset) / lateralVelocity);
else
TLC_Frame = 10000;//Max TLC shows a safe deviation
TLC = TLC_Frame * intervalTime;//sec
TLC = TLC < 1000 ? TLC : 1000;
InfoTLC infoTLC;
infoTLC.TLC = TLC;
infoTLC.intervalTime = intervalTime;
if(!TLCDeque.empty())
{
infoTLC.winTime = TLCDeque.back().winTime + intervalTime;
} else {
infoTLC.winTime = intervalTime;
}
while (infoTLC.winTime > TIME_SAMPLING_WINDOW)
{
infoTLC.winTime -= TLCDeque.front().intervalTime;
TLCDeque.pop_front();
}
TLCDeque.push_back(infoTLC);
//! Number of times that TLC below a threshold
for(std::deque<double>::size_type i = 0; i != TLCDeque.size(); ++i)
{
//!TLC with signal falling below 2s in a given time interval
if(TLCDeque[i].TLC < 2.0)
{
TLC_2s ++;
}
//!TLC with signal falling below 0.5s in a given time interval
if(TLCDeque[i].TLC < 0.5)
{
TLC_halfs ++;
}
}
//! Fraction of TLC_2s and TLC_halfs
TLCF_2s = (double)TLC_2s / (double)TLCDeque.size();
TLCF_halfs = (double)TLC_halfs / (double)TLCDeque.size();
//! Global minimum of TLC over a given time interval;
std::deque<InfoTLC> cmpTLCDeque = TLCDeque;
sort(cmpTLCDeque.begin(), cmpTLCDeque.end(), TLC_cmp);
TLC_min = cmpTLCDeque.back().TLC;
std::deque<InfoTLC>::iterator iter = cmpTLCDeque.end();
while (TLC_min == 0)
{
--iter;
TLC_min = iter->TLC;
}
//!Update the LaneFeature struct
laneFeatures.frame = sampleIdx;
laneFeatures.lateralOffset = lateralOffset;
laneFeatures.LATMEAN = LATMEAN; //EWMA
laneFeatures.LANEDEV = LANEDEV; //EWVAR
laneFeatures.LATSD = LATSD;
laneFeatures.LANEX = LANEX;
laneFeatures.TLC = TLC;
laneFeatures.TLC_2s = TLC_2s;
laneFeatures.TLC_halfs = TLC_halfs;
laneFeatures.TLC_min = TLC_min;
laneFeatures.TLCF_2s = TLCF_2s;
laneFeatures.TLCF_halfs = TLCF_halfs;
}//end GenerateLaneIndicators
value_type at(size_type index) const
{
return pos_vec.at(index);
};
reference at(size_type index)
{
return pos_vec.at(index);
};