Drake
beam_model.h
Go to the documentation of this file.
1 #pragma once
2 
6 #include "drake/systems/sensors/gen/beam_model_params.h"
7 
8 namespace drake {
9 namespace systems {
10 namespace sensors {
11 
12 /// Implements the "Beam Models of Range Finders" from section 6.3 of
13 /// Probabilistic Robotics (2006), by Thrun, Burgard, and Fox
14 ///
15 /// This system takes a depth measurement signal as input, and outputs a noisy
16 /// measurement version of that signal, with some probability of returning the
17 /// true measurement with Gaussian noise, but also with some probability of
18 /// occlusions (short returns), of missed detections (returning the max depth),
19 /// and of returning just a (uniform) random measurement.
20 ///
21 /// Four additional input ports (each of the same dimension as the depth signal)
22 /// are provided for the random inputs: One for determining which of the events
23 /// occurred (true + noise, short return, max return, or uniform return), and
24 /// one
25 /// each for modeling the distribution of short true but noisy returns, short
26 /// returns, and uniform returns).
27 ///
28 /// We deviate from the textbook model in one respect: both here and in the
29 /// textbook, the distribution over short returns and the distribution over
30 /// getting a noisy version of the true return (aka a "hit") are truncated. The
31 /// short returns are from an exponential distribution but truncated to be less
32 /// than the input depth, and "hits" are drawn from a Gaussian centered at the
33 /// input depth but truncated at the maximum range of the sensor. In the book,
34 /// these distributions are normalized so that the total probability of getting
35 /// a short return and/or hit stays constant (independent of the input depth).
36 /// Here we do not normalize, so that the probability of getting a short return
37 /// decreases as the input depth is smaller (there is a modeled obstacle closer
38 /// to the robot), and the tails of the "hit" distribution simply cause more max
39 /// returns as the input depth gets closer to the max range. This was done both
40 /// because it is arguably a better model and because it keeps the code much
41 /// simpler (to allow AutoDiff and Symbolic) given the modeling framework we
42 /// have here that builds the output out of simple (non-truncated) random
43 /// variable inputs.
44 ///
45 /// Instantiated templates for the following kinds of T's are provided:
46 /// - double
47 /// - AutoDiffXd
48 ///
49 /// @ingroup sensor_systems
50 // TODO(russt): Add support for symbolic.
51 template <typename T>
52 class BeamModel final : public LeafSystem<T> {
53  public:
55 
56  BeamModel(int num_depth_readings, double max_range);
57 
58  explicit BeamModel(const DepthSensorSpecification& specification);
59 
60  /// Scalar-converting copy constructor. See @ref system_scalar_conversion.
61  template <typename U>
62  explicit BeamModel(const BeamModel<U>&);
63 
65  return this->get_input_port(0);
66  }
68  return this->get_input_port(1);
69  }
71  return this->get_input_port(2);
72  }
74  return this->get_input_port(3);
75  }
77  return this->get_input_port(4);
78  }
79 
80  BeamModelParams<T>& get_mutable_parameters(Context<T>* context) const;
81 
82  double max_range() const { return max_range_; }
83 
84  private:
85  void CalcOutput(const Context<T>& context, BasicVector<T>* output) const;
86 
87  const double max_range_{1.0};
88 };
89 
90 } // namespace sensors
91 } // namespace systems
92 } // namespace drake
const InputPort< T > & get_event_random_input_port() const
Definition: beam_model.h:67
Definition: bullet_model.cc:22
const InputPort< T > & get_hit_random_input_port() const
Definition: beam_model.h:70
const InputPort< T > & get_short_random_input_port() const
Definition: beam_model.h:73
Implements the "Beam Models of Range Finders" from section 6.3 of Probabilistic Robotics (2006)...
Definition: beam_model.h:52
double max_range() const
Definition: beam_model.h:82
Context is an abstract class template that represents all the typed values that are used in a System&#39;...
Definition: context.h:40
const InputPort< T > & get_input_port(int port_index) const
Returns the typed input port at index port_index.
Definition: system.h:1084
const InputPort< T > & get_depth_input_port() const
Definition: beam_model.h:64
BeamModelParams< T > & get_mutable_parameters(Context< T > *context) const
Definition: beam_model.cc:61
A superclass template that extends System with some convenience utilities that are not applicable to ...
Definition: leaf_system.h:81
BasicVector is a semantics-free wrapper around an Eigen vector that satisfies VectorBase.
Definition: basic_vector.h:25
Holds a DepthSensor&#39;s specification.
Definition: depth_sensor_specification.h:16
BeamModel(const BeamModel &)=delete
#define DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Classname)
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN deletes the special member functions for copy-construction, copy-assignment, move-construction, and move-assignment.
Definition: drake_copyable.h:33
This extends InputPortBase with some scalar type-dependent methods.
Definition: input_port.h:22
Provides careful macros to selectively enable or disable the special member functions for copy-constr...
const InputPort< T > & get_uniform_random_input_port() const
Definition: beam_model.h:76