forked from pz4kybsvg/Conception
You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
116 lines
4.2 KiB
116 lines
4.2 KiB
#pragma once
|
|
|
|
#include "drake/common/drake_copyable.h"
|
|
#include "drake/systems/framework/leaf_system.h"
|
|
#include "drake/systems/sensors/gen/beam_model_params.h"
|
|
|
|
namespace drake {
|
|
namespace systems {
|
|
namespace sensors {
|
|
|
|
// TODO(russt): Add support for symbolic.
|
|
/// Implements the "Beam Models of Range Finders" from section 6.3 of
|
|
/// Probabilistic Robotics (2006), by Thrun, Burgard, and Fox
|
|
///
|
|
/// This system takes a depth measurement signal as input, and outputs a noisy
|
|
/// measurement version of that signal, with some probability of returning the
|
|
/// true measurement with Gaussian noise, but also with some probability of
|
|
/// occlusions (short returns), of missed detections (returning the max depth),
|
|
/// and of returning just a (uniform) random measurement.
|
|
///
|
|
/// Four additional input ports (each of the same dimension as the depth
|
|
/// signal) are provided for the random inputs: One for determining which of
|
|
/// the events occurred (true + noise, short return, max return, or uniform
|
|
/// return), and one each for modeling the distribution of short true but noisy
|
|
/// returns, short returns, and uniform returns).
|
|
///
|
|
/// We deviate from the textbook model in one respect: both here and in the
|
|
/// textbook, the distribution over short returns and the distribution over
|
|
/// getting a noisy version of the true return (aka a "hit") are truncated. The
|
|
/// short returns are from an exponential distribution but truncated to be less
|
|
/// than the input depth, and "hits" are drawn from a Gaussian centered at the
|
|
/// input depth but truncated at the maximum range of the sensor. In the book,
|
|
/// these distributions are normalized so that the total probability of getting
|
|
/// a short return and/or hit stays constant (independent of the input depth).
|
|
/// Here we do not normalize, so that the probability of getting a short return
|
|
/// decreases as the input depth is smaller (there is a modeled obstacle closer
|
|
/// to the robot), and the tails of the "hit" distribution simply cause more max
|
|
/// returns as the input depth gets closer to the max range. This was done both
|
|
/// because it is arguably a better model and because it keeps the code much
|
|
/// simpler (to allow AutoDiff and Symbolic) given the modeling framework we
|
|
/// have here that builds the output out of simple (non-truncated) random
|
|
/// variable inputs.
|
|
///
|
|
/// @system
|
|
/// name: BeamModel
|
|
/// input_ports:
|
|
/// - depth
|
|
/// - event
|
|
/// - hit
|
|
/// - short
|
|
/// - uniform
|
|
/// output_ports:
|
|
/// - depth
|
|
/// @endsystem
|
|
///
|
|
/// It is convenient to use `systems::AddRandomInputs()` to supply all the
|
|
/// random input signals:
|
|
/// @code
|
|
/// DiagramBuilder<double> builder;
|
|
/// auto beam_model = builder.AddSystem<BeamModel>(1, 5.0);
|
|
/// builder.ExportInput(beam_model->get_depth_input_port(), "depth");
|
|
/// builder.ExportOutput(beam_model->get_output_port(0), "depth");
|
|
/// AddRandomInputs(0.01, &builder);
|
|
/// auto diagram = builder.Build();
|
|
/// @endcode
|
|
///
|
|
/// @tparam_nonsymbolic_scalar
|
|
/// @ingroup sensor_systems
|
|
template <typename T>
|
|
class BeamModel final : public LeafSystem<T> {
|
|
public:
|
|
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(BeamModel)
|
|
|
|
BeamModel(int num_depth_readings, double max_range);
|
|
|
|
/// Scalar-converting copy constructor. See @ref system_scalar_conversion.
|
|
template <typename U>
|
|
explicit BeamModel(const BeamModel<U>&);
|
|
|
|
const InputPort<T>& get_depth_input_port() const {
|
|
return this->get_input_port(0);
|
|
}
|
|
const InputPort<T>& get_event_random_input_port() const {
|
|
return this->get_input_port(1);
|
|
}
|
|
const InputPort<T>& get_hit_random_input_port() const {
|
|
return this->get_input_port(2);
|
|
}
|
|
const InputPort<T>& get_short_random_input_port() const {
|
|
return this->get_input_port(3);
|
|
}
|
|
const InputPort<T>& get_uniform_random_input_port() const {
|
|
return this->get_input_port(4);
|
|
}
|
|
|
|
BeamModelParams<T>& get_mutable_parameters(Context<T>* context) const;
|
|
|
|
double max_range() const { return max_range_; }
|
|
|
|
private:
|
|
void CalcOutput(const Context<T>& context, BasicVector<T>* output) const;
|
|
|
|
const double max_range_{1.0};
|
|
};
|
|
|
|
} // namespace sensors
|
|
|
|
// Explicitly disable symbolic::Expression (for now).
|
|
namespace scalar_conversion {
|
|
template <>
|
|
struct Traits<sensors::BeamModel> : public NonSymbolicTraits {};
|
|
} // namespace scalar_conversion
|
|
|
|
} // namespace systems
|
|
} // namespace drake
|