void
MapView2D<SLAM>::pollSlam() {
if( !this->m_slam )
return;
Trajectory traj;
TrajectoryVector allTraj;
std::vector<Eigen::Matrix3d> covs;
std::vector<size_t> timePoints;
this->m_slam->lock();
size_t idx = this->m_slam->bestParticleIdx( false );
const MapType map = this->m_slam->map( idx, false );
this->m_bestParticlePos = this->m_slam->pose( idx ).pos();
if( m_showBestPath ) {
traj = this->m_slam->trajectory( idx, false );
if( m_showCovariances && traj.size() > 9 ) {
timePoints.reserve( traj.size() / 3 - 2 );
for( size_t i = 1; i < traj.size() - 1; i += 3 )
timePoints.push_back( i );
covs = this->m_slam->covariances( timePoints, false );
}
}
if( m_showPaths )
allTraj = this->m_slam->trajectories( false );
std::vector<Point2w> posVec;
posVec.reserve( this->m_slam->numParticles() );
for( size_t i = 0; i < this->m_slam->numParticles(); i++ )
posVec.push_back( this->m_slam->pose( i ).pos() );
this->m_slam->unlock();
// The order of drawing is important in order not to overdraw something
drawMap( map );
if( m_showPaths )
for( const auto &t : allTraj )
drawTrajectory( t, Qt::darkYellow );
if( m_showBestPath ) {
if( m_showCovariances )
drawCovariances( traj, covs, timePoints, Qt::darkYellow );
drawTrajectory( traj );
}
drawPosVector( posVec );
}
/* @Core */
Mat PDCounter::detect(Mat& input) {
string log1("Begin detecting...\n");
Mat output = input.clone();
for (vector<PDDetector>::iterator it = Detectors.begin(); it != Detectors.end(); it++) {
(*it).detect(output, Trackers);// NO parallel
}
string log2("All areas are detected...\n");
string lost = i_to_s(Trackers.tracking(output)); // NO parallel
string log3("Tracking completed...\nThe trackers lost " + lost + " pedestrians in this frame.\n");
if (showPedestrian) {
drawFounds(output, Trackers.getCurrRects(), PEDESTRIAN_COLOR);
}
if (showArea) {
for (vector<PDDetector>::iterator it = Detectors.begin(); it != Detectors.end(); it++) {
drawArea(output, (*it).getArea(), AREA_COLOR);
}
}
if (showTrajectory) {
for (int i = 0; i < Trackers.getSize(); i++) {
drawTrajectory(output, Trackers[i].getTrajectory(), Trajectory_COLOR);
}
}
string log4("Drawing completed");
lastLog = log1 + log2 + log3 + log4;
return output;
}
void RNDFEditGLView::paintGL()
{
glClearColor(0, 0, 0, 0);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
activate3DMode();
glDisable(GL_DEPTH_TEST);
glPushMatrix();
// draw imagery
bool show_flat_imagery = true;
double current_time = drc::Time::current();
static double last_time = 0;
if (gui->show_imagery_) {
if (current_time - last_time > 0.05) {
try {
imagery_->update();
}
catch(vlr::Ex<>& e) {
std::cout << "Failed to update imagery :" << e.what() << std::endl;
}
last_time = current_time;
}
glPushMatrix();
{
// glTranslatef(0, 0, -DGC_PASSAT_HEIGHT);
try {
imagery_->draw3D(camera_pose.distance, camera_pose.x_offset,
camera_pose.y_offset, gui->rndf_center.x, gui->rndf_center.y, gui->rndf_center.zone, show_flat_imagery, 1.0, 1);
}
catch(vlr::Ex<>& e) {
std::cout << "Failed to draw imagery :" << e.what() << std::endl;
}
}
glPopMatrix();
}
// draw road network
gui->rn_->draw(gui->rndf_center.x, gui->rndf_center.y, 1, true);
// draw selected waypoint
drawSelected(gui->rndf_center.x, gui->rndf_center.y);
// draw coordinate frame
draw_coordinate_frame(2.0);
drawTrajectory(gui->rndf_center.x, gui->rndf_center.y);
glPopMatrix();
refresh_required=false;
}
