/** Callback: On recalc local map & publish it */ void onDoPublish(const ros::TimerEvent& ) { mrpt::utils::CTimeLoggerEntry tle(m_profiler,"onDoPublish"); // Purge old observations & latch a local copy: TListObservations obs; { mrpt::utils::CTimeLoggerEntry tle(m_profiler,"onDoPublish.removingOld"); m_hist_obs_mtx.lock(); // Purge old obs: if (!m_hist_obs.empty()) { const double last_time = m_hist_obs.rbegin()->first; TListObservations::iterator it_first_valid = m_hist_obs.lower_bound( last_time-m_time_window ); const size_t nToRemove = std::distance( m_hist_obs.begin(), it_first_valid ); ROS_DEBUG("[onDoPublish] Removing %u old entries, last_time=%lf",static_cast<unsigned int>(nToRemove), last_time); m_hist_obs.erase(m_hist_obs.begin(), it_first_valid); } // Local copy in this thread: obs = m_hist_obs; m_hist_obs_mtx.unlock(); } ROS_DEBUG("Building local map with %u observations.",static_cast<unsigned int>(obs.size())); if (obs.empty()) return; // Build local map(s): // ----------------------------------------------- m_localmap_pts.clear(); mrpt::poses::CPose3D curRobotPose; { mrpt::utils::CTimeLoggerEntry tle2(m_profiler,"onDoPublish.buildLocalMap"); // Get the latest robot pose in the reference frame (typ: /odom -> /base_link) // so we can build the local map RELATIVE to it: try { tf::StampedTransform tx; m_tf_listener.lookupTransform(m_frameid_reference,m_frameid_robot, ros::Time(0), tx); mrpt_bridge::convert(tx,curRobotPose); ROS_DEBUG("[onDoPublish] Building local map relative to latest robot pose: %s", curRobotPose.asString().c_str() ); } catch (tf::TransformException &ex) { ROS_ERROR("%s",ex.what()); return; } // For each observation: compute relative robot pose & insert obs into map: for (TListObservations::const_iterator it=obs.begin();it!=obs.end();++it) { const TInfoPerTimeStep & ipt = it->second; // Relative pose in the past: mrpt::poses::CPose3D relPose(mrpt::poses::UNINITIALIZED_POSE); relPose.inverseComposeFrom( ipt.robot_pose, curRobotPose ); // Insert obs: m_localmap_pts.insertObservationPtr(ipt.observation,&relPose); } // end for } // Publish them: if (m_pub_local_map_pointcloud.getNumSubscribers()>0) { sensor_msgs::PointCloudPtr msg_pts = sensor_msgs::PointCloudPtr( new sensor_msgs::PointCloud); msg_pts->header.frame_id = m_frameid_robot; msg_pts->header.stamp = ros::Time( obs.rbegin()->first ); mrpt_bridge::point_cloud::mrpt2ros(m_localmap_pts,msg_pts->header, *msg_pts); m_pub_local_map_pointcloud.publish(msg_pts); } // Show gui: if (m_show_gui) { if (!m_gui_win) { m_gui_win = mrpt::gui::CDisplayWindow3D::Create("LocalObstaclesNode",800,600); mrpt::opengl::COpenGLScenePtr &scene = m_gui_win->get3DSceneAndLock(); scene->insert( mrpt::opengl::CGridPlaneXY::Create() ); scene->insert( mrpt::opengl::stock_objects::CornerXYZSimple(1.0,4.0) ); mrpt::opengl::CSetOfObjectsPtr gl_obs = mrpt::opengl::CSetOfObjects::Create(); gl_obs->setName("obstacles"); scene->insert( gl_obs ); mrpt::opengl::CPointCloudPtr gl_pts = mrpt::opengl::CPointCloud::Create(); gl_pts->setName("points"); gl_pts->setPointSize(2.0); gl_pts->setColor_u8( mrpt::utils::TColor(0x0000ff) ); scene->insert( gl_pts ); m_gui_win->unlockAccess3DScene(); } mrpt::opengl::COpenGLScenePtr &scene = m_gui_win->get3DSceneAndLock(); mrpt::opengl::CSetOfObjectsPtr gl_obs = mrpt::opengl::CSetOfObjectsPtr( scene->getByName("obstacles") ); ROS_ASSERT(gl_obs.present()); gl_obs->clear(); mrpt::opengl::CPointCloudPtr gl_pts = mrpt::opengl::CPointCloudPtr( scene->getByName("points") ); for (TListObservations::const_iterator it=obs.begin();it!=obs.end();++it) { const TInfoPerTimeStep & ipt = it->second; // Relative pose in the past: mrpt::poses::CPose3D relPose(mrpt::poses::UNINITIALIZED_POSE); relPose.inverseComposeFrom( ipt.robot_pose, curRobotPose ); mrpt::opengl::CSetOfObjectsPtr gl_axis = mrpt::opengl::stock_objects::CornerXYZSimple(0.9,2.0); gl_axis->setPose(relPose); gl_obs->insert(gl_axis); } // end for gl_pts->loadFromPointsMap(&m_localmap_pts); m_gui_win->unlockAccess3DScene(); m_gui_win->repaint(); } } // onDoPublish
// ------------------------------------------------------ // DoTrackingDemo // ------------------------------------------------------ int DoTrackingDemo(CCameraSensorPtr cam, bool DO_SAVE_VIDEO) { win = mrpt::gui::CDisplayWindow3D::Create("Tracked features",800,600); mrpt::vision::CVideoFileWriter vidWritter; bool hasResolution = false; TCamera cameraParams; // For now, will only hold the image resolution on the arrive of the first frame. TSimpleFeatureList trackedFeats; unsigned int step_num = 0; bool SHOW_FEAT_IDS = true; bool SHOW_RESPONSES = true; bool SHOW_FEAT_TRACKS = true; const double SAVE_VIDEO_FPS = 30; // If DO_SAVE_VIDEO=true, the FPS of the video file const char* SAVE_VIDEO_CODEC = "XVID"; // "XVID", "PIM1", "MJPG" bool DO_HIST_EQUALIZE_IN_GRAYSCALE = false; string VIDEO_OUTPUT_FILE = "./tracking_video.avi"; const double MAX_FPS = 5000; // 5.0; // Hz (to slow down visualization). CGenericFeatureTrackerAutoPtr tracker; // "CFeatureTracker_KL" is by far the most robust implementation for now: tracker = CGenericFeatureTrackerAutoPtr( new CFeatureTracker_KL ); tracker->enableTimeLogger(true); // Do time profiling. // Set of parameters common to any tracker implementation: // ------------------------------------------------------------- // To see all the existing params and documentation, see mrpt::vision::CGenericFeatureTracker tracker->extra_params["remove_lost_features"] = 1; // automatically remove out-of-image and badly tracked features tracker->extra_params["add_new_features"] = 1; // track, AND ALSO, add new features tracker->extra_params["add_new_feat_min_separation"] = 32; tracker->extra_params["minimum_KLT_response_to_add"] = 10; tracker->extra_params["add_new_feat_max_features"] = 350; tracker->extra_params["add_new_feat_patch_size"] = 11; tracker->extra_params["update_patches_every"] = 0; // Don't update patches. tracker->extra_params["check_KLT_response_every"] = 5; // Re-check the KLT-response to assure features are in good points. tracker->extra_params["minimum_KLT_response"] = 5; // Specific params for "CFeatureTracker_KL" // ------------------------------------------------------ tracker->extra_params["window_width"] = 5; tracker->extra_params["window_height"] = 5; //tracker->extra_params["LK_levels"] = 3; //tracker->extra_params["LK_max_iters"] = 10; //tracker->extra_params["LK_epsilon"] = 0.1; //tracker->extra_params["LK_max_tracking_error"] = 150; // -------------------------------- // The main loop // -------------------------------- CImage previous_image; TSequenceFeatureObservations feat_track_history; bool save_tracked_history = true; // Dump feat_track_history to a file at the end TCameraPoseID curCamPoseId = 0; cout << endl << "TO END THE PROGRAM: Close the window.\n"; mrpt::opengl::COpenGLViewportPtr gl_view; { mrpt::opengl::COpenGLScenePtr scene = win->get3DSceneAndLock(); gl_view = scene->getViewport("main"); win->unlockAccess3DScene(); } // Aux data for drawing the recent track of features: static const size_t FEATS_TRACK_LEN = 10; std::map<TFeatureID,std::list<TPixelCoord> > feat_tracks; // infinite loop, until we close the win: while( win->isOpen() ) { CObservationPtr obs; try { obs= cam->getNextFrame(); } catch (CExceptionEOF &) { // End of a rawlog file. break; } if (!obs) { cerr << "*Warning* getNextFrame() returned NULL!\n"; mrpt::system::sleep(50); continue; } CImage theImg; // The grabbed image: if (IS_CLASS(obs,CObservationImage)) { CObservationImagePtr o = CObservationImagePtr(obs); theImg.copyFastFrom(o->image); } else if (IS_CLASS(obs,CObservationStereoImages)) { CObservationStereoImagesPtr o = CObservationStereoImagesPtr(obs); theImg.copyFastFrom(o->imageLeft); } else if (IS_CLASS(obs,CObservation3DRangeScan)) { CObservation3DRangeScanPtr o = CObservation3DRangeScanPtr(obs); if (o->hasIntensityImage) theImg.copyFastFrom(o->intensityImage); } else { continue; // Silently ignore non-image observations. } // Make sure the image is loaded (for the case it came from a rawlog file) if (theImg.isExternallyStored()) theImg.loadFromFile( theImg.getExternalStorageFileAbsolutePath()); // Take the resolution upon first valid frame. if (!hasResolution) { hasResolution = true; // cameraParams.scaleToResolution()... cameraParams.ncols = theImg.getWidth(); cameraParams.nrows = theImg.getHeight(); } // Do tracking: if (step_num>1) // we need "previous_image" to be valid. { // This single call makes: detection, tracking, recalculation of KLT_response, etc. tracker->trackFeatures(previous_image, theImg, trackedFeats); } // Save the image for the next step: previous_image = theImg; // Save history of feature observations: tracker->getProfiler().enter("Save history"); for (size_t i=0;i<trackedFeats.size();++i) { TSimpleFeature &f = trackedFeats[i]; const TPixelCoordf pxRaw(f.pt.x,f.pt.y); TPixelCoordf pxUndist; //mrpt::vision::pinhole::undistort_point(pxRaw,pxUndist, cameraParams); pxUndist = pxRaw; feat_track_history.push_back( TFeatureObservation(f.ID,curCamPoseId, pxUndist ) ); } curCamPoseId++; tracker->getProfiler().leave("Save history"); // now that we're done with the image, we can directly write onto it // for the display // ---------------------------------------------------------------- if (DO_HIST_EQUALIZE_IN_GRAYSCALE && !theImg.isColor()) theImg.equalizeHistInPlace(); // Convert to color so we can draw color marks, etc. theImg.colorImageInPlace(); double extra_tim_to_wait=0; { // FPS: static CTicTac tictac; const double T = tictac.Tac(); tictac.Tic(); const double fps = 1.0/(std::max(1e-5,T)); //theImg.filledRectangle(1,1,175,25,TColor(0,0,0)); const int current_adapt_thres = tracker->getDetectorAdaptiveThreshold(); theImg.selectTextFont("6x13B"); theImg.textOut(3,3,format("FPS: %.03f Hz", fps ),TColor(200,200,0) ); theImg.textOut(3,22,format("# feats: %u - Adaptive threshold: %i", (unsigned int)trackedFeats.size(), current_adapt_thres ),TColor(200,200,0) ); theImg.textOut(3,41, format("# raw feats: %u - Removed: %u", (unsigned int)tracker->last_execution_extra_info.raw_FAST_feats_detected, (unsigned int)tracker->last_execution_extra_info.num_deleted_feats ), TColor(200,200,0) ); extra_tim_to_wait = 1.0/MAX_FPS - 1.0/fps; } // Draw feature tracks if (SHOW_FEAT_TRACKS) { // Update new feature coords: tracker->getProfiler().enter("drawFeatureTracks"); std::set<TFeatureID> observed_IDs; for (size_t i=0;i<trackedFeats.size();++i) { const TSimpleFeature &ft = trackedFeats[i]; std::list<TPixelCoord> & seq = feat_tracks[ft.ID]; observed_IDs.insert(ft.ID); if (seq.size()>=FEATS_TRACK_LEN) seq.erase(seq.begin()); seq.push_back(ft.pt); // Draw: if (seq.size()>1) { const std::list<TPixelCoord>::const_iterator it_end = seq.end(); std::list<TPixelCoord>::const_iterator it = seq.begin(); std::list<TPixelCoord>::const_iterator it_prev = it++; for (;it!=it_end;++it) { theImg.line(it_prev->x,it_prev->y,it->x,it->y, TColor(190,190,190) ); it_prev = it; } } } tracker->getProfiler().leave("drawFeatureTracks"); // Purge old data: for (std::map<TFeatureID,std::list<TPixelCoord> >::iterator it=feat_tracks.begin();it!=feat_tracks.end(); ) { if (observed_IDs.find(it->first)==observed_IDs.end()) { std::map<TFeatureID,std::list<TPixelCoord> >::iterator next_it = it; next_it++; feat_tracks.erase(it); it = next_it; } else ++it; } } // Draw Tracked feats: { theImg.selectTextFont("5x7"); tracker->getProfiler().enter("drawFeatures"); theImg.drawFeatures(trackedFeats, TColor(0,0,255), SHOW_FEAT_IDS, SHOW_RESPONSES); tracker->getProfiler().leave("drawFeatures"); } // Update window: win->get3DSceneAndLock(); gl_view->setImageView(theImg); win->unlockAccess3DScene(); win->repaint(); // Save debug output video: // ---------------------------------- if (DO_SAVE_VIDEO) { static bool first = true; if (first) { first=false; if (vidWritter.open( VIDEO_OUTPUT_FILE, SAVE_VIDEO_FPS /* fps */, theImg.getSize(), SAVE_VIDEO_CODEC, true /* force color video */ ) ) { cout << "[track-video] Saving tracking video to: " << VIDEO_OUTPUT_FILE << endl; } else cerr << "ERROR: Trying to create output video: " << VIDEO_OUTPUT_FILE << endl; } vidWritter << theImg; } if (extra_tim_to_wait>0) mrpt::system::sleep(1000*extra_tim_to_wait); step_num++; } // end infinite loop // Save tracked feats: if (save_tracked_history) { cout << "Saving tracked features to: tracked_feats.txt..."; cout.flush(); feat_track_history.saveToTextFile("./tracked_feats.txt"); cout << "Done!\n"; cout.flush(); #if 0 // SBA: cout << "Saving cams.txt & pts.txt files in SBA library format..."; cout.flush(); feat_track_history.removeFewObservedFeatures(3); feat_track_history.decimateCameraFrames(20); feat_track_history.compressIDs(); TLandmarkLocationsVec locs; TFramePosesVec cams; feat_track_history.saveAsSBAFiles(locs,"pts.txt", cams, "cams.txt"); cout << "Done!\n"; cout.flush(); #endif } return 0; // End ok. }