{"action":"created","comment":{"url":"https://api.github.com/repos/cra-ros-pkg/robot_localization/pulls/comments/55936692","id":55936692,"diff_hunk":"@@ -2747,6 +2798,87 @@ namespace RobotLocalization\n \n     return canTransform;\n   }\n+\n+  template<typename T>\n+  void RosFilter<T>::saveFilterState(FilterBase& filter)\n+  {\n+    FilterState state;\n+    state.state_ = filter.getState();\n+    state.estimate_error_covariance_ = filter.getEstimateErrorCovariance();\n+    state.time_ = filter.getLastMeasurementTime();\n+    filterStateHistory_.push_back(state);\n+    ros::Time ts;\n+    ts.fromSec(state.time_);\n+    RF_DEBUG(\"\\n---- RosFilter::saveFilterState for timestamp \" << ts << \" size = \" << filterStateHistory_.size() << \"----\\n\");\n+  }\n+\n+  template<typename T>\n+  bool RosFilter<T>::restoreFilterAndMeasurementsBefore(const double t)\n+  {\n+    RF_DEBUG(\"\\n----- RosFilter::restoreFilterAndMeasurementsBefore (\" << t << \") -----\\n\");\n+    // Remove all filter states older than t. This yields either the latest\n+    // filter state before t or an empty queue. In that case the smoothing\n+    // window is too small and we cannot reset the filter.\n+    //\n+\n+    int num_popped_filter_updates = 0;\n+    while (!filterStateHistory_.empty()\n+        && filterStateHistory_.back().time_ > t)\n+    {\n+      filterStateHistory_.pop_back();\n+      num_popped_filter_updates++;\n+    }\n+    RF_DEBUG(\"\\n---- Popped \" << num_popped_filter_updates << \" filter updates. Queue is empty/size? \" << filterStateHistory_.empty() << \"/\" << filterStateHistory_.size() << \"\\n\");\n+\n+    if (filterStateHistory_.empty()) return false;\n+\n+    // Reset filter to the latest state from the queue.\n+    const FilterState &old = filterStateHistory_.back();\n+    filter_.setState(old.state_);\n+    filter_.setEstimateErrorCovariance(old.estimate_error_covariance_);\n+    filter_.setLastMeasurementTime(old.time_);\n+\n+    // Find all measurements from the history whose timestamp is > t\n+    // and push them to the measurements queue for integration.\n+    // This re-establishes our invariant: measurementHistory_.back().time_ < measurementQueue_.top()\n+    while (!measurementHistory_.empty()\n+           && measurementHistory_.back().time_ > t)\n+    {\n+      measurementQueue_.push(measurementHistory_.back());\n+      measurementHistory_.pop_back();\n+    }\n+\n+    RF_DEBUG(\"\\n----- Restored measurement queue has \" << measurementQueue_.size() << \" entries now.\\n\");\n+\n+    RF_DEBUG(\"\\n----- /RosFilter::restoreFilterAndMeasurementsBefore\\n\");\n+\n+    return true;\n+  }\n+\n+  template<typename T>\n+  void RosFilter<T>::removeHistoryStatesAndMeasurementsOlderThan(const double t)","path":"src/ros_filter.cpp","position":157,"original_position":157,"commit_id":"199a41b8b8f9208ded17954df879e1ec80d49e72","original_commit_id":"199a41b8b8f9208ded17954df879e1ec80d49e72","user":{"login":"ayrton04","id":5042191,"avatar_url":"https://avatars.githubusercontent.com/u/5042191?v=3","gravatar_id":"","url":"https://api.github.com/users/ayrton04","html_url":"https://github.com/ayrton04","followers_url":"https://api.github.com/users/ayrton04/followers","following_url":"https://api.github.com/users/ayrton04/following{/other_user}","gists_url":"https://api.github.com/users/ayrton04/gists{/gist_id}","starred_url":"https://api.github.com/users/ayrton04/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/ayrton04/subscriptions","organizations_url":"https://api.github.com/users/ayrton04/orgs","repos_url":"https://api.github.com/users/ayrton04/repos","events_url":"https://api.github.com/users/ayrton04/events{/privacy}","received_events_url":"https://api.github.com/users/ayrton04/received_events","type":"User","site_admin":false},"body":"I think the only method that should be permitted to remove states and measurements from their respective histories is this one.","created_at":"2016-03-13T12:50:33Z","updated_at":"2016-03-13T12:50:33Z","html_url":"https://github.com/cra-ros-pkg/robot_localization/pull/266#discussion_r55936692","pull_request_url":"https://api.github.com/repos/cra-ros-pkg/robot_localization/pulls/266","_links":{"self":{"href":"https://api.github.com/repos/cra-ros-pkg/robot_localization/pulls/comments/55936692"},"html":{"href":"https://github.com/cra-ros-pkg/robot_localization/pull/266#discussion_r55936692"},"pull_request":{"href":"https://api.github.com/repos/cra-ros-pkg/robot_localization/pulls/266"}}},"pull_request":{"url":"https://api.github.com/repos/cra-ros-pkg/robot_localization/pulls/266","id":61972886,"html_url":"https://github.com/cra-ros-pkg/robot_localization/pull/266","diff_url":"https://github.com/cra-ros-pkg/robot_localization/pull/266.diff","patch_url":"https://github.com/cra-ros-pkg/robot_localization/pull/266.patch","issue_url":"https://api.github.com/repos/cra-ros-pkg/robot_localization/issues/266","number":266,"state":"open","locked":false,"title":"Implement support for fusing delayed measurements by keeping a state and measurement history","user":{"login":"essinger","id":11777555,"avatar_url":"https://avatars.githubusercontent.com/u/11777555?v=3","gravatar_id":"","url":"https://api.github.com/users/essinger","html_url":"https://github.com/essinger","followers_url":"https://api.github.com/users/essinger/followers","following_url":"https://api.github.com/users/essinger/following{/other_user}","gists_url":"https://api.github.com/users/essinger/gists{/gist_id}","starred_url":"https://api.github.com/users/essinger/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/essinger/subscriptions","organizations_url":"https://api.github.com/users/essinger/orgs","repos_url":"https://api.github.com/users/essinger/repos","events_url":"https://api.github.com/users/essinger/events{/privacy}","received_events_url":"https://api.github.com/users/essinger/received_events","type":"User","site_admin":false},"body":" This implements a smoothing scheme as described in issue 263 and also adresses the issue 262.","created_at":"2016-03-07T20:10:55Z","updated_at":"2016-03-13T12:50:34Z","closed_at":null,"merged_at":null,"merge_commit_sha":"7c15e8cda6de172395e4b8d9a27a1606dfa76264","assignee":null,"milestone":null,"commits_url":"https://api.github.com/repos/cra-ros-pkg/robot_localization/pulls/266/commits","review_comments_url":"https://api.github.com/repos/cra-ros-pkg/robot_localization/pulls/266/comments","review_comment_url":"https://api.github.com/repos/cra-ros-pkg/robot_localization/pulls/comments{/number}","comments_url":"https://api.github.com/repos/cra-ros-pkg/robot_localization/issues/266/comments","statuses_url":"https://api.github.com/repos/cra-ros-pkg/robot_localization/statuses/199a41b8b8f9208ded17954df879e1ec80d49e72","head":{"label":"essinger:feat-temp-smoothing","ref":"feat-temp-smoothing","sha":"199a41b8b8f9208ded17954df879e1ec80d49e72","user":{"login":"essinger","id":11777555,"avatar_url":"https://avatars.githubusercontent.com/u/11777555?v=3","gravatar_id":"","url":"https://api.github.com/users/essinger","html_url":"https://github.com/essinger","followers_url":"https://api.github.com/users/essinger/followers","following_url":"https://api.github.com/users/essinger/following{/other_user}","gists_url":"https://api.github.com/users/essinger/gists{/gist_id}","starred_url":"https://api.github.com/users/essinger/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/essinger/subscriptions","organizations_url":"https://api.github.com/users/essinger/orgs","repos_url":"https://api.github.com/users/essinger/repos","events_url":"https://api.github.com/users/essinger/events{/privacy}","received_events_url":"https://api.github.com/users/essinger/received_events","type":"User","site_admin":false},"repo":{"id":53038170,"name":"robot_localization","full_name":"essinger/robot_localization","owner":{"login":"essinger","id":11777555,"avatar_url":"https://avatars.githubusercontent.com/u/11777555?v=3","gravatar_id":"","url":"https://api.github.com/users/essinger","html_url":"https://github.com/essinger","followers_url":"https://api.github.com/users/essinger/followers","following_url":"https://api.github.com/users/essinger/following{/other_user}","gists_url":"https://api.github.com/users/essinger/gists{/gist_id}","starred_url":"https://api.github.com/users/essinger/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/essinger/subscriptions","organizations_url":"https://api.github.com/users/essinger/orgs","repos_url":"https://api.github.com/users/essinger/repos","events_url":"https://api.github.com/users/essinger/events{/privacy}","received_events_url":"https://api.github.com/users/essinger/received_events","type":"User","site_admin":false},"private":false,"html_url":"https://github.com/essinger/robot_localization","description":"robot_localization is a package of nonlinear state estimation nodes. The package was developed by Charles River Analytics, Inc.","fork":true,"url":"https://api.github.com/repos/essinger/robot_localization","forks_url":"https://api.github.com/repos/essinger/robot_localization/forks","keys_url":"https://api.github.com/repos/essinger/robot_localization/keys{/key_id}","collaborators_url":"https://api.github.com/repos/essinger/robot_localization/collaborators{/collaborator}","teams_url":"https://api.github.com/repos/essinger/robot_localization/teams","hooks_url":"https://api.github.com/repos/essinger/robot_localization/hooks","issue_events_url":"https://api.github.com/repos/essinger/robot_localization/issues/events{/number}","events_url":"https://api.github.com/repos/essinger/robot_localization/events","assignees_url":"https://api.github.com/repos/essinger/robot_localization/assignees{/user}","branches_url":"https://api.github.com/repos/essinger/robot_localization/branches{/branch}","tags_url":"https://api.github.com/repos/essinger/robot_localization/tags","blobs_url":"https://api.github.com/repos/essinger/robot_localization/git/blobs{/sha}","git_tags_url":"https://api.github.com/repos/essinger/robot_localization/git/tags{/sha}","git_refs_url":"https://api.github.com/repos/essinger/robot_localization/git/refs{/sha}","trees_url":"https://api.github.com/repos/essinger/robot_localization/git/trees{/sha}","statuses_url":"https://api.github.com/repos/essinger/robot_localization/statuses/{sha}","languages_url":"https://api.github.com/repos/essinger/robot_localization/languages","stargazers_url":"https://api.github.com/repos/essinger/robot_localization/stargazers","contributors_url":"https://api.github.com/repos/essinger/robot_localization/contributors","subscribers_url":"https://api.github.com/repos/essinger/robot_localization/subscribers","subscription_url":"https://api.github.com/repos/essinger/robot_localization/subscription","commits_url":"https://api.github.com/repos/essinger/robot_localization/commits{/sha}","git_commits_url":"https://api.github.com/repos/essinger/robot_localization/git/commits{/sha}","comments_url":"https://api.github.com/repos/essinger/robot_localization/comments{/number}","issue_comment_url":"https://api.github.com/repos/essinger/robot_localization/issues/comments{/number}","contents_url":"https://api.github.com/repos/essinger/robot_localization/contents/{+path}","compare_url":"https://api.github.com/repos/essinger/robot_localization/compare/{base}...{head}","merges_url":"https://api.github.com/repos/essinger/robot_localization/merges","archive_url":"https://api.github.com/repos/essinger/robot_localization/{archive_format}{/ref}","downloads_url":"https://api.github.com/repos/essinger/robot_localization/downloads","issues_url":"https://api.github.com/repos/essinger/robot_localization/issues{/number}","pulls_url":"https://api.github.com/repos/essinger/robot_localization/pulls{/number}","milestones_url":"https://api.github.com/repos/essinger/robot_localization/milestones{/number}","notifications_url":"https://api.github.com/repos/essinger/robot_localization/notifications{?since,all,participating}","labels_url":"https://api.github.com/repos/essinger/robot_localization/labels{/name}","releases_url":"https://api.github.com/repos/essinger/robot_localization/releases{/id}","deployments_url":"https://api.github.com/repos/essinger/robot_localization/deployments","created_at":"2016-03-03T09:31:46Z","updated_at":"2016-03-03T09:31:47Z","pushed_at":"2016-03-07T20:07:55Z","git_url":"git://github.com/essinger/robot_localization.git","ssh_url":"git@github.com:essinger/robot_localization.git","clone_url":"https://github.com/essinger/robot_localization.git","svn_url":"https://github.com/essinger/robot_localization","homepage":"www.cra.com","size":10651,"stargazers_count":0,"watchers_count":0,"language":"C++","has_issues":false,"has_downloads":true,"has_wiki":true,"has_pages":false,"forks_count":0,"mirror_url":null,"open_issues_count":0,"forks":0,"open_issues":0,"watchers":0,"default_branch":"indigo-devel"}},"base":{"label":"cra-ros-pkg:indigo-devel","ref":"indigo-devel","sha":"bae0d53cd7824be78ea3169d0f500c9f1d53b9ac","user":{"login":"cra-ros-pkg","id":6164287,"avatar_url":"https://avatars.githubusercontent.com/u/6164287?v=3","gravatar_id":"","url":"https://api.github.com/users/cra-ros-pkg","html_url":"https://github.com/cra-ros-pkg","followers_url":"https://api.github.com/users/cra-ros-pkg/followers","following_url":"https://api.github.com/users/cra-ros-pkg/following{/other_user}","gists_url":"https://api.github.com/users/cra-ros-pkg/gists{/gist_id}","starred_url":"https://api.github.com/users/cra-ros-pkg/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/cra-ros-pkg/subscriptions","organizations_url":"https://api.github.com/users/cra-ros-pkg/orgs","repos_url":"https://api.github.com/users/cra-ros-pkg/repos","events_url":"https://api.github.com/users/cra-ros-pkg/events{/privacy}","received_events_url":"https://api.github.com/users/cra-ros-pkg/received_events","type":"Organization","site_admin":false},"repo":{"id":18298873,"name":"robot_localization","full_name":"cra-ros-pkg/robot_localization","owner":{"login":"cra-ros-pkg","id":6164287,"avatar_url":"https://avatars.githubusercontent.com/u/6164287?v=3","gravatar_id":"","url":"https://api.github.com/users/cra-ros-pkg","html_url":"https://github.com/cra-ros-pkg","followers_url":"https://api.github.com/users/cra-ros-pkg/followers","following_url":"https://api.github.com/users/cra-ros-pkg/following{/other_user}","gists_url":"https://api.github.com/users/cra-ros-pkg/gists{/gist_id}","starred_url":"https://api.github.com/users/cra-ros-pkg/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/cra-ros-pkg/subscriptions","organizations_url":"https://api.github.com/users/cra-ros-pkg/orgs","repos_url":"https://api.github.com/users/cra-ros-pkg/repos","events_url":"https://api.github.com/users/cra-ros-pkg/events{/privacy}","received_events_url":"https://api.github.com/users/cra-ros-pkg/received_events","type":"Organization","site_admin":false},"private":false,"html_url":"https://github.com/cra-ros-pkg/robot_localization","description":"robot_localization is a package of nonlinear state estimation nodes. The package was developed by Charles River Analytics, Inc.","fork":false,"url":"https://api.github.com/repos/cra-ros-pkg/robot_localization","forks_url":"https://api.github.com/repos/cra-ros-pkg/robot_localization/forks","keys_url":"https://api.github.com/repos/cra-ros-pkg/robot_localization/keys{/key_id}","collaborators_url":"https://api.github.com/repos/cra-ros-pkg/robot_localization/collaborators{/collaborator}","teams_url":"https://api.github.com/repos/cra-ros-pkg/robot_localization/teams","hooks_url":"https://api.github.com/repos/cra-ros-pkg/robot_localization/hooks","issue_events_url":"https://api.github.com/repos/cra-ros-pkg/robot_localization/issues/events{/number}","events_url":"https://api.github.com/repos/cra-ros-pkg/robot_localization/events","assignees_url":"https://api.github.com/repos/cra-ros-pkg/robot_localization/assignees{/user}","branches_url":"https://api.github.com/repos/cra-ros-pkg/robot_localization/branches{/branch}","tags_url":"https://api.github.com/repos/cra-ros-pkg/robot_localization/tags","blobs_url":"https://api.github.com/repos/cra-ros-pkg/robot_localization/git/blobs{/sha}","git_tags_url":"https://api.github.com/repos/cra-ros-pkg/robot_localization/git/tags{/sha}","git_refs_url":"https://api.github.com/repos/cra-ros-pkg/robot_localization/git/refs{/sha}","trees_url":"https://api.github.com/repos/cra-ros-pkg/robot_localization/git/trees{/sha}","statuses_url":"https://api.github.com/repos/cra-ros-pkg/robot_localization/statuses/{sha}","languages_url":"https://api.github.com/repos/cra-ros-pkg/robot_localization/languages","stargazers_url":"https://api.github.com/repos/cra-ros-pkg/robot_localization/stargazers","contributors_url":"https://api.github.com/repos/cra-ros-pkg/robot_localization/contributors","subscribers_url":"https://api.github.com/repos/cra-ros-pkg/robot_localization/subscribers","subscription_url":"https://api.github.com/repos/cra-ros-pkg/robot_localization/subscription","commits_url":"https://api.github.com/repos/cra-ros-pkg/robot_localization/commits{/sha}","git_commits_url":"https://api.github.com/repos/cra-ros-pkg/robot_localization/git/commits{/sha}","comments_url":"https://api.github.com/repos/cra-ros-pkg/robot_localization/comments{/number}","issue_comment_url":"https://api.github.com/repos/cra-ros-pkg/robot_localization/issues/comments{/number}","contents_url":"https://api.github.com/repos/cra-ros-pkg/robot_localization/contents/{+path}","compare_url":"https://api.github.com/repos/cra-ros-pkg/robot_localization/compare/{base}...{head}","merges_url":"https://api.github.com/repos/cra-ros-pkg/robot_localization/merges","archive_url":"https://api.github.com/repos/cra-ros-pkg/robot_localization/{archive_format}{/ref}","downloads_url":"https://api.github.com/repos/cra-ros-pkg/robot_localization/downloads","issues_url":"https://api.github.com/repos/cra-ros-pkg/robot_localization/issues{/number}","pulls_url":"https://api.github.com/repos/cra-ros-pkg/robot_localization/pulls{/number}","milestones_url":"https://api.github.com/repos/cra-ros-pkg/robot_localization/milestones{/number}","notifications_url":"https://api.github.com/repos/cra-ros-pkg/robot_localization/notifications{?since,all,participating}","labels_url":"https://api.github.com/repos/cra-ros-pkg/robot_localization/labels{/name}","releases_url":"https://api.github.com/repos/cra-ros-pkg/robot_localization/releases{/id}","deployments_url":"https://api.github.com/repos/cra-ros-pkg/robot_localization/deployments","created_at":"2014-03-31T15:54:51Z","updated_at":"2016-02-11T22:03:51Z","pushed_at":"2016-03-07T20:10:55Z","git_url":"git://github.com/cra-ros-pkg/robot_localization.git","ssh_url":"git@github.com:cra-ros-pkg/robot_localization.git","clone_url":"https://github.com/cra-ros-pkg/robot_localization.git","svn_url":"https://github.com/cra-ros-pkg/robot_localization","homepage":"www.cra.com","size":11575,"stargazers_count":29,"watchers_count":29,"language":"C++","has_issues":true,"has_downloads":true,"has_wiki":true,"has_pages":false,"forks_count":47,"mirror_url":null,"open_issues_count":18,"forks":47,"open_issues":18,"watchers":29,"default_branch":"indigo-devel"}},"_links":{"self":{"href":"https://api.github.com/repos/cra-ros-pkg/robot_localization/pulls/266"},"html":{"href":"https://github.com/cra-ros-pkg/robot_localization/pull/266"},"issue":{"href":"https://api.github.com/repos/cra-ros-pkg/robot_localization/issues/266"},"comments":{"href":"https://api.github.com/repos/cra-ros-pkg/robot_localization/issues/266/comments"},"review_comments":{"href":"https://api.github.com/repos/cra-ros-pkg/robot_localization/pulls/266/comments"},"review_comment":{"href":"https://api.github.com/repos/cra-ros-pkg/robot_localization/pulls/comments{/number}"},"commits":{"href":"https://api.github.com/repos/cra-ros-pkg/robot_localization/pulls/266/commits"},"statuses":{"href":"https://api.github.com/repos/cra-ros-pkg/robot_localization/statuses/199a41b8b8f9208ded17954df879e1ec80d49e72"}}}}