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; }