The scan matcher based on the Monte Carlo simulation. More...
#include <tiny_scan_matcher.h>
Public Member Functions | |
virtual void | reset_state () override |
TinyScanMatcher (ScePtr cost_estimator, unsigned bad_iter, unsigned max_iter, double sigma_coord, double sigma_angle) | |
Public Member Functions inherited from MonteCarloScanMatcher | |
MonteCarloScanMatcher (std::shared_ptr< ScanCostEstimator > estimator, unsigned failed_iter, unsigned max_iter) | |
Initializes the scan matcher with a certain scan cost estimator. More... | |
virtual double | process_scan (const RobotState &init_pose, const TransformedLaserScan &scan, const GridMap &map, RobotState &pose_delta) override |
Public Member Functions inherited from GridScanMatcher | |
GridScanMatcher (std::shared_ptr< ScanCostEstimator > estimator) | |
void | subscribe (std::shared_ptr< GridScanMatcherObserver > obs) |
void | unsubscribe (std::shared_ptr< GridScanMatcherObserver > obs) |
Protected Member Functions | |
virtual unsigned | on_estimate_update (unsigned sample_num, unsigned sample_limit) override |
virtual void | sample_pose (RobotState &base_pose) override |
Protected Member Functions inherited from GridScanMatcher | |
std::shared_ptr< ScanCostEstimator > | cost_estimator () |
Returns a pointer to the cost estimator. More... | |
std::vector< std::weak_ptr< GridScanMatcherObserver > > & | observers () |
Returns a reference to the vector of pointers on observers. More... | |
Private Types | |
using | ScePtr = std::shared_ptr< ScanCostEstimator > |
Private Attributes | |
double | _curr_sigma_angle |
double | _curr_sigma_coord |
double | _sigma_angle |
double | _sigma_coord |
Additional Inherited Members | |
Public Types inherited from MonteCarloScanMatcher | |
using | ObsPtr = std::shared_ptr< GridScanMatcherObserver > |
The scan matcher based on the Monte Carlo simulation.
The robot pose is updated by shifting it on a random vector and rotation by a random angle. The vector distribution is dynamically adjusted.
Definition at line 15 of file tiny_scan_matcher.h.
|
private |
Definition at line 17 of file tiny_scan_matcher.h.
|
inline |
Initializes the scan matcher.
[in] | cost_estimator | - the type of estimator for the robot location. |
[in] | bad_iter,max_iter | - see failed_iter, max_iter in MonteCarloScanMatcher |
[in] | sigma_coord,sigma_angle | - the value of a normal distribution for the random variables. ( , and ). |
Definition at line 28 of file tiny_scan_matcher.h.
|
inlineoverrideprotectedvirtual |
A callback invoked when a better estimate is found.
sample_num | Amount of tries that were complited. |
sample_limit | Totla amount of tries allowed. |
Implements MonteCarloScanMatcher.
Definition at line 54 of file tiny_scan_matcher.h.
|
inlineoverridevirtual |
Resets the scan matcher to the state it had right after the initialization.
Reimplemented from GridScanMatcher.
Definition at line 37 of file tiny_scan_matcher.h.
|
inlineoverrideprotectedvirtual |
Generates the pose of a robot in a vicinity of a base pose.
base_pose | A basical pose of a robot. |
Implements MonteCarloScanMatcher.
Definition at line 43 of file tiny_scan_matcher.h.
|
private |
Definition at line 67 of file tiny_scan_matcher.h.
|
private |
Definition at line 67 of file tiny_scan_matcher.h.
|
private |
Definition at line 66 of file tiny_scan_matcher.h.
|
private |
Definition at line 66 of file tiny_scan_matcher.h.