Drake
RgbdCamera Class Reference

An RGB-D camera system that provides RGB, depth and label images using visual elements of RigidBodyTree. More...

#include <drake/systems/sensors/rgbd_camera.h>

Inheritance diagram for RgbdCamera:
[legend]
Collaboration diagram for RgbdCamera:
[legend]

Classes

class  InvalidDepth
 Set of constants used to represent invalid depth values. More...
 
class  Label
 Set of labels used for label image. More...
 

Public Member Functions

 RgbdCamera (const std::string &name, const RigidBodyTree< double > &tree, const Eigen::Vector3d &position, const Eigen::Vector3d &orientation, double depth_range_near=0.5, double depth_range_far=5.0, double fov_y=M_PI_4, bool show_window=true)
 A constructor for RgbdCamera that defines B using Euler angles. More...
 
 RgbdCamera (const std::string &name, const RigidBodyTree< double > &tree, const RigidBodyFrame< double > &frame, double depth_range_near=0.5, double depth_range_far=5.0, double fov_y=M_PI_4, bool show_window=true)
 A constructor for RgbdCamera that defines B using a RigidBodyFrame. More...
 
 ~RgbdCamera ()
 
const CameraInfocolor_camera_info () const
 Reterns the color sensor's info. More...
 
const CameraInfodepth_camera_info () const
 Reterns the depth sensor's info. More...
 
const Eigen::Isometry3d & color_camera_optical_pose () const
 Returns X_BC. More...
 
const Eigen::Isometry3d & depth_camera_optical_pose () const
 Returns X_BD. More...
 
const RigidBodyFrame< double > & frame () const
 Returns the RigidBodyFrame to which this RgbdCamera is attached. More...
 
const RigidBodyTree< double > & tree () const
 Returns the RigidBodyTree to which this RgbdCamera is attached. More...
 
const InputPortDescriptor< double > & state_input_port () const
 Returns a descriptor of the vector valued input port that takes a vector of q, v corresponding to the positions and velocities associated with the RigidBodyTree. More...
 
const OutputPort< double > & color_image_output_port () const
 Returns the abstract valued output port that contains a RGBA image of the type ImageRgba8U. More...
 
const OutputPort< double > & depth_image_output_port () const
 Returns the abstract valued output port that contains an ImageDepth32F. More...
 
const OutputPort< double > & label_image_output_port () const
 Returns the abstract valued output port that contains an label image of the type ImageLabel16I. More...
 
const OutputPort< double > & camera_base_pose_output_port () const
 Returns the vector valued output port that contains a PoseVector. More...
 
Does not allow copy, move, or assignment
 RgbdCamera (const RgbdCamera &)=delete
 
RgbdCameraoperator= (const RgbdCamera &)=delete
 
 RgbdCamera (RgbdCamera &&)=delete
 
RgbdCameraoperator= (RgbdCamera &&)=delete
 
- Public Member Functions inherited from LeafSystem< double >
 ~LeafSystem () override
 
std::unique_ptr< CompositeEventCollection< double > > AllocateCompositeEventCollection () const final
 Allocates a CompositeEventCollection object for this system. More...
 
std::unique_ptr< Context< double > > AllocateContext () const override
 Allocates a context, initialized with the correct numbers of concrete input ports and state variables for this System. More...
 
void SetDefaultState (const Context< double > &context, State< double > *state) const override
 Default implementation: sets all continuous and discrete state variables to zero. More...
 
virtual void SetDefaultParameters (const LeafContext< double > &context, Parameters< double > *parameters) const
 Default implementation: sets all numeric parameters to the model vector given to DeclareNumericParameter, or else if no model was provided sets the numeric parameter to one. More...
 
void SetDefaults (Context< double > *context) const final
 
std::unique_ptr< SystemOutput< double > > AllocateOutput (const Context< double > &context) const final
 Returns a container that can hold the values of all of this System's output ports. More...
 
std::unique_ptr< ContinuousState< double > > AllocateTimeDerivatives () const override
 Returns the AllocateContinuousState value, which must not be nullptr. More...
 
std::unique_ptr< DiscreteValues< double > > AllocateDiscreteVariables () const override
 Returns the AllocateDiscreteState value, which must not be nullptr. More...
 
std::multimap< int, intGetDirectFeedthroughs () const final
 Reports all direct feedthroughs from input ports to output ports. More...
 
 LeafSystem (const LeafSystem &)=delete
 
 LeafSystem (LeafSystem &&)=delete
 
LeafSystemoperator= (const LeafSystem &)=delete
 
LeafSystemoperator= (LeafSystem &&)=delete
 
- Public Member Functions inherited from System< double >
virtual ~System ()
 
void GetWitnessFunctions (const Context< double > &context, std::vector< const WitnessFunction< double > * > *w) const
 Gets the witness functions active at the beginning of a continuous time interval. More...
 
double EvaluateWitness (const Context< double > &context, const WitnessFunction< double > &witness_func) const
 Evaluates a witness function at the given context. More...
 
std::string GetSystemIdString () const
 Returns a string suitable for identifying this particular System in error messages, when it is a subsystem of a larger Diagram. More...
 
 System (const System &)=delete
 
 System (System &&)=delete
 
Systemoperator= (const System &)=delete
 
Systemoperator= (System &&)=delete
 
std::unique_ptr< BasicVector< double > > AllocateInputVector (const InputPortDescriptor< double > &descriptor) const
 Given a port descriptor, allocates the vector storage. More...
 
std::unique_ptr< AbstractValueAllocateInputAbstract (const InputPortDescriptor< double > &descriptor) const
 Given a port descriptor, allocates the abstract storage. More...
 
std::unique_ptr< Context< double > > CreateDefaultContext () const
 This convenience method allocates a context using AllocateContext() and sets its default values using SetDefaults(). More...
 
void AllocateFreestandingInputs (Context< double > *context) const
 For each input port, allocates a freestanding input of the concrete type that this System requires, and binds it to the port, disconnecting any prior input. More...
 
bool HasAnyDirectFeedthrough () const
 Returns true if any of the inputs to the system might be directly fed through to any of its outputs and false otherwise. More...
 
bool HasDirectFeedthrough (int output_port) const
 Returns true if there might be direct-feedthrough from any input port to the given output_port, and false otherwise. More...
 
bool HasDirectFeedthrough (int input_port, int output_port) const
 Returns true if there might be direct-feedthrough from the given input_port to the given output_port, and false otherwise. More...
 
void Publish (const Context< double > &context, const EventCollection< PublishEvent< double >> &events) const
 This method is the public entry point for dispatching all publish event handlers. More...
 
void Publish (const Context< double > &context) const
 Forces a publish on the system, given a context. More...
 
const doubleEvalConservativePower (const Context< double > &context) const
 Returns a reference to the cached value of the conservative power. More...
 
const doubleEvalNonConservativePower (const Context< double > &context) const
 Returns a reference to the cached value of the non-conservative power. More...
 
const Vec< double > * EvalVectorInput (const Context< double > &context, int port_index) const
 Causes the vector-valued input port with the given port_index to become up-to-date, delegating to our parent Diagram if necessary. More...
 
Eigen::VectorBlock< const VectorX< double > > EvalEigenVectorInput (const Context< double > &context, int port_index) const
 Causes the vector-valued input port with the given port_index to become up-to-date, delegating to our parent Diagram if necessary. More...
 
const AbstractValueEvalAbstractInput (const Context< double > &context, int port_index) const
 Causes the abstract-valued input port with the given port_index to become up-to-date, delegating to our parent Diagram if necessary. More...
 
const V * EvalInputValue (const Context< double > &context, int port_index) const
 Causes the abstract-valued input port with the given port_index to become up-to-date, delegating to our parent Diagram if necessary. More...
 
int get_num_constraint_equations (const Context< double > &context) const
 Gets the number of constraint equations for this system using the given context (useful in case the number of constraints is dependent upon the current state (as might be the case with a system modeled using piecewise differential algebraic equations). More...
 
Eigen::VectorXd EvalConstraintEquations (const Context< double > &context) const
 Evaluates the constraint equations for the system at the generalized coordinates and generalized velocity specified by the context. More...
 
Eigen::VectorXd EvalConstraintEquationsDot (const Context< double > &context) const
 Computes the time derivative of each constraint equation, evaluated at the generalized coordinates and generalized velocity specified by the context. More...
 
Eigen::VectorXd CalcVelocityChangeFromConstraintImpulses (const Context< double > &context, const Eigen::MatrixXd &J, const Eigen::VectorXd &lambda) const
 Computes the change in velocity from applying the given constraint forces to the system at the given context. More...
 
double CalcConstraintErrorNorm (const Context< double > &context, const Eigen::VectorXd &error) const
 Computes the norm on constraint error (used as a metric for comparing errors between the outputs of algebraic equations applied to two different state variable instances). More...
 
void CalcTimeDerivatives (const Context< double > &context, ContinuousState< double > *derivatives) const
 Calculates the time derivatives xcdot of the continuous state xc. More...
 
void CalcDiscreteVariableUpdates (const Context< double > &context, const EventCollection< DiscreteUpdateEvent< double >> &events, DiscreteValues< double > *discrete_state) const
 This method is the public entry point for dispatching all discrete variable update event handlers. More...
 
void CalcDiscreteVariableUpdates (const Context< double > &context, DiscreteValues< double > *discrete_state) const
 This method forces a discrete update on the system given a context, and the updated discrete state is stored in discrete_state. More...
 
void CalcUnrestrictedUpdate (const Context< double > &context, const EventCollection< UnrestrictedUpdateEvent< double >> &events, State< double > *state) const
 This method is the public entry point for dispatching all unrestricted update event handlers. More...
 
void CalcUnrestrictedUpdate (const Context< double > &context, State< double > *state) const
 This method forces an unrestricted update on the system given a context, and the updated state is stored in discrete_state. More...
 
double CalcNextUpdateTime (const Context< double > &context, CompositeEventCollection< double > *events) const
 This method is called by a Simulator during its calculation of the size of the next continuous step to attempt. More...
 
void GetPerStepEvents (const Context< double > &context, CompositeEventCollection< double > *events) const
 This method is called by Simulator::Initialize() to gather all update and publish events that are to be handled in StepTo() at the point before Simulator integrates continuous state. More...
 
void CalcOutput (const Context< double > &context, SystemOutput< double > *outputs) const
 Utility method that computes for every output port i the value y(i) that should result from the current contents of the given Context. More...
 
double CalcPotentialEnergy (const Context< double > &context) const
 Calculates and returns the potential energy current stored in the configuration provided in context. More...
 
double CalcKineticEnergy (const Context< double > &context) const
 Calculates and returns the kinetic energy currently present in the motion provided in the given Context. More...
 
double CalcConservativePower (const Context< double > &context) const
 Calculates and returns the rate at which mechanical energy is being converted from potential energy to kinetic energy by this system in the given Context. More...
 
double CalcNonConservativePower (const Context< double > &context) const
 Calculates and returns the rate at which mechanical energy is being generated (positive) or dissipated (negative) other than by conversion between potential and kinetic energy (in the given Context). More...
 
void MapVelocityToQDot (const Context< double > &context, const VectorBase< double > &generalized_velocity, VectorBase< double > *qdot) const
 Transforms a given generalized velocity v to the time derivative qdot of the generalized configuration q taken from the supplied Context. More...
 
void MapVelocityToQDot (const Context< double > &context, const Eigen::Ref< const VectorX< double >> &generalized_velocity, VectorBase< double > *qdot) const
 Transforms the given generalized velocity to the time derivative of generalized configuration. More...
 
void MapQDotToVelocity (const Context< double > &context, const VectorBase< double > &qdot, VectorBase< double > *generalized_velocity) const
 Transforms the time derivative qdot of the generalized configuration q to generalized velocities v. More...
 
void MapQDotToVelocity (const Context< double > &context, const Eigen::Ref< const VectorX< double >> &qdot, VectorBase< double > *generalized_velocity) const
 Transforms the given time derivative qdot of generalized configuration q to generalized velocity v. More...
 
void set_name (const std::string &name)
 Sets the name of the system. More...
 
std::string get_name () const
 Returns the name last supplied to set_name(), or empty if set_name() was never called. More...
 
std::string GetMemoryObjectName () const
 Returns a name for this System based on a stringification of its type name and memory address. More...
 
void GetPath (std::stringstream *output) const
 Writes the full path of this System in the tree of Systems to output. More...
 
std::string GetPath () const
 
int get_num_input_ports () const
 Returns the number of input ports of the system. More...
 
int get_num_output_ports () const
 Returns the number of output ports of the system. More...
 
const InputPortDescriptor< double > & get_input_port (int port_index) const
 Returns the descriptor of the input port at index port_index. More...
 
const OutputPort< double > & get_output_port (int port_index) const
 Returns the output port at index port_index. More...
 
int get_num_constraints () const
 Returns the number of constraints specified for the system. More...
 
const SystemConstraint< double > & get_constraint (SystemConstraintIndex constraint_index) const
 Returns the constraint at index constraint_index. More...
 
int get_num_total_inputs () const
 Returns the total dimension of all of the input ports (as if they were muxed). More...
 
int get_num_total_outputs () const
 Returns the total dimension of all of the output ports (as if they were muxed). More...
 
void CheckValidOutput (const SystemOutput< double > *output) const
 Checks that output is consistent with the number and size of output ports declared by the system. More...
 
void CheckValidContext (const Context< T1 > &context) const
 Checks that context is consistent for this System template. More...
 
VectorX< doubleCopyContinuousStateVector (const Context< double > &context) const
 Returns a copy of the continuous state vector xc into an Eigen vector. More...
 
void set_parent (const detail::InputPortEvaluatorInterface< double > *parent)
 Declares that parent is the immediately enclosing Diagram. More...
 
std::string GetGraphvizString () const
 Returns a Graphviz string describing this System. More...
 
int64_t GetGraphvizId () const
 Returns an opaque integer that uniquely identifies this system in the Graphviz output. More...
 
void FixInputPortsFrom (const System< double > &other_system, const Context< double > &other_context, Context< double > *target_context) const
 Fixes all of the input ports in target_context to their current values in other_context, as evaluated by other_system. More...
 
std::unique_ptr< System< AutoDiffXd > > ToAutoDiffXd () const
 Creates a deep copy of this System, transmogrified to use the autodiff scalar type, with a dynamic-sized vector of partial derivatives. More...
 
std::unique_ptr< System< AutoDiffXd > > ToAutoDiffXdMaybe () const
 Creates a deep copy of this system exactly like ToAutoDiffXd(), but returns nullptr if this System does not support autodiff, instead of throwing an exception. More...
 
std::unique_ptr< System< symbolic::Expression > > ToSymbolic () const
 Creates a deep copy of this System, transmogrified to use the symbolic scalar type. More...
 
std::unique_ptr< System< symbolic::Expression > > ToSymbolicMaybe () const
 Creates a deep copy of this system exactly like ToSymbolic(), but returns nullptr if this System does not support symbolic, instead of throwing an exception. More...
 

Static Public Member Functions

static void ConvertDepthImageToPointCloud (const ImageDepth32F &depth_image, const CameraInfo &camera_info, Eigen::Matrix3Xf *point_cloud)
 Converts a depth image obtained from RgbdCamera to a point cloud. More...
 
- Static Public Member Functions inherited from System< double >
static std::unique_ptr< S< AutoDiffXd > > ToAutoDiffXd (const S< double > &from)
 Creates a deep copy of from, transmogrified to use the autodiff scalar type, with a dynamic-sized vector of partial derivatives. More...
 
static std::unique_ptr< S< symbolic::Expression > > ToSymbolic (const S< double > &from)
 Creates a deep copy of from, transmogrified to use the symbolic scalar type. More...
 

Additional Inherited Members

- Protected Member Functions inherited from LeafSystem< double >
 LeafSystem ()
 Default constructor that declares no inputs, outputs, state, parameters, events, nor scalar-type conversion support (AutoDiff, etc.). More...
 
 LeafSystem (SystemScalarConverter converter)
 Constructor that declares no inputs, outputs, state, parameters, or events, but allows subclasses to declare scalar-type conversion support (AutoDiff, etc.). More...
 
System< AutoDiffXd > * DoToAutoDiffXd () const override
 NVI implementation of ToAutoDiffXdMaybe. More...
 
System< symbolic::Expression > * DoToSymbolic () const override
 NVI implementation of ToSymbolicMaybe. More...
 
virtual std::unique_ptr< LeafContext< double > > DoMakeContext () const
 Provides a new instance of the leaf context for this system. More...
 
double DoEvaluateWitness (const Context< double > &context, const WitnessFunction< double > &witness_func) const final
 Derived classes will implement this method to evaluate a witness function at the given context. More...
 
void AddTriggeredWitnessFunctionToCompositeEventCollection (const WitnessFunction< double > &witness_func, CompositeEventCollection< double > *events) const final
 Add witness_func to events. More...
 
void DoCalcNextUpdateTime (const Context< double > &context, CompositeEventCollection< double > *events, double *time) const override
 Computes the next update time based on the configured periodic events, for scalar types that are arithmetic, or aborts for scalar types that are not arithmetic. More...
 
BasicVector< double > * DoAllocateInputVector (const InputPortDescriptor< double > &descriptor) const override
 Allocates a vector that is suitable as an input value for descriptor. More...
 
AbstractValueDoAllocateInputAbstract (const InputPortDescriptor< double > &descriptor) const override
 Allocates an AbstractValue suitable as an input value for descriptor. More...
 
void GetGraphvizFragment (std::stringstream *dot) const override
 Emits a graphviz fragment for this System. More...
 
void GetGraphvizInputPortToken (const InputPortDescriptor< double > &port, std::stringstream *dot) const final
 Appends a fragment to the dot stream identifying the graphviz node representing port. More...
 
void GetGraphvizOutputPortToken (const OutputPort< double > &port, std::stringstream *dot) const final
 Appends a fragment to the dot stream identifying the graphviz node representing port. More...
 
virtual std::unique_ptr< ContinuousState< double > > AllocateContinuousState () const
 Returns a ContinuousState used to implement both CreateDefaultContext and AllocateTimeDerivatives. More...
 
virtual std::unique_ptr< DiscreteValues< double > > AllocateDiscreteState () const
 Reserves the discrete state as required by CreateDefaultContext. More...
 
virtual std::unique_ptr< AbstractValuesAllocateAbstractState () const
 Reserves the abstract state as required by CreateDefaultContext. More...
 
virtual std::unique_ptr< Parameters< double > > AllocateParameters () const
 Reserves the parameters as required by CreateDefaultContext. More...
 
virtual optional< boolDoHasDirectFeedthrough (int input_port, int output_port) const
 Returns true if there is direct-feedthrough from the given input_port to the given output_port, false if there is not direct-feedthrough, or nullopt if unknown (in which case SystemSymbolicInspector will attempt to measure the feedthrough using symbolic form). More...
 
int DeclareNumericParameter (const BasicVector< double > &model_vector)
 Declares a numeric parameter using the given model_vector. More...
 
const U< double > & GetNumericParameter (const Context< double > &context, int index) const
 Extracts the numeric parameters of type U from the context at index. More...
 
U< double > * GetMutableNumericParameter (Context< double > *context, int index) const
 Extracts the numeric parameters of type U from the context at index. More...
 
void DeclarePeriodicEvent (double period_sec, double offset_sec)
 Declares that this System has a simple, fixed-period event specified with no custom callback function, and its attribute field contains an Event<T>::PeriodicAttribute constructed from the specified period_sec and offset_sec. More...
 
void DeclarePeriodicEvent (double period_sec, double offset_sec, const EventType &event)
 Declares that this System has a simple, fixed-period event specified by event. More...
 
void DeclarePeriodicDiscreteUpdate (double period_sec, double offset_sec=0)
 Declares a periodic discrete update event with period = period_sec and offset = offset_sec. More...
 
void DeclarePeriodicUnrestrictedUpdate (double period_sec, double offset_sec=0)
 Declares a periodic unrestricted update event with period = period_sec and offset = offset_sec. More...
 
void DeclarePeriodicPublish (double period_sec, double offset_sec=0)
 Declares a periodic publish event with period = period_sec and offset = offset_sec. More...
 
void DeclarePerStepEvent (const EventType &event)
 Declares a per-step event using event, which is deep copied (the copy is maintained by this). More...
 
void DeclareContinuousState (int num_state_variables)
 Declares that this System should reserve continuous state with num_state_variables state variables, which have no second-order structure. More...
 
void DeclareContinuousState (int num_q, int num_v, int num_z)
 Declares that this System should reserve continuous state with num_q generalized positions, num_v generalized velocities, and num_z miscellaneous state variables. More...
 
void DeclareContinuousState (const BasicVector< double > &model_vector)
 Declares that this System should reserve continuous state with model_vector.size() miscellaneous state variables, stored in a vector Cloned from model_vector. More...
 
void DeclareContinuousState (const BasicVector< double > &model_vector, int num_q, int num_v, int num_z)
 Declares that this System should reserve continuous state with num_q generalized positions, num_v generalized velocities, and num_z miscellaneous state variables, stored in a vector Cloned from model_vector. More...
 
void DeclareContinuousState (std::unique_ptr< BasicVector< double >> model_vector, int num_q, int num_v, int num_z)
 Declares that this System should reserve continuous state with num_q generalized positions, num_v generalized velocities, and num_z miscellaneous state variables, stored in the a vector Cloned from model_vector. More...
 
void DeclareDiscreteState (int num_state_variables)
 Declares that this System should reserve discrete state with num_state_variables state variables. More...
 
int DeclareAbstractState (std::unique_ptr< AbstractValue > abstract_state)
 Declares an abstract state. More...
 
SystemConstraintIndex DeclareEqualityConstraint (void(MySystem::*calc)(const Context< double > &, VectorX< double > *) const, int count, const std::string &description)
 Declares a system constraint of the form f(context) = 0 by specifying a member function to use to calculate the (VectorX) constraint value with a signature: More...
 
SystemConstraintIndex DeclareEqualityConstraint (typename SystemConstraint< double >::CalcCallback calc, int count, const std::string &description)
 Declares a system constraint of the form f(context) = 0 by specifying a std::function to use to calculate the (Vector) constraint value with a signature: More...
 
SystemConstraintIndex DeclareInequalityConstraint (void(MySystem::*calc)(const Context< double > &, VectorX< double > *) const, int count, const std::string &description)
 Declares a system constraint of the form f(context) ≥ 0 by specifying a member function to use to calculate the (VectorX) constraint value with a signature: More...
 
SystemConstraintIndex DeclareInequalityConstraint (typename SystemConstraint< double >::CalcCallback calc, int count, const std::string &description)
 Declares a system constraint of the form f(context) ≥ 0 by specifying a std::function to use to calculate the (Vector) constraint value with a signature: More...
 
virtual void DoPublish (const Context< double > &context, const std::vector< const PublishEvent< double > * > &events) const
 Derived-class event handler for all simultaneous publish events in events. More...
 
virtual void DoCalcDiscreteVariableUpdates (const Context< double > &context, const std::vector< const DiscreteUpdateEvent< double > * > &events, DiscreteValues< double > *discrete_state) const
 Derived-class event handler for all simultaneous discrete update events. More...
 
virtual void DoCalcUnrestrictedUpdate (const Context< double > &context, const std::vector< const UnrestrictedUpdateEvent< double > * > &events, State< double > *state) const
 Derived-class event handler for all simultaneous unrestricted update events. More...
 
const InputPortDescriptor< double > & DeclareVectorInputPort (const BasicVector< double > &model_vector)
 Declares a vector-valued input port using the given model_vector. More...
 
const InputPortDescriptor< double > & DeclareAbstractInputPort (const AbstractValue &model_value)
 Declares an abstract-valued input port using the given model_value. More...
 
const OutputPort< double > & DeclareVectorOutputPort (const BasicVectorSubtype &model_vector, void(MySystem::*calc)(const Context< double > &, BasicVectorSubtype *) const)
 Declares a vector-valued output port by specifying (1) a model vector of type BasicVectorSubtype derived from BasicVector and initialized to the correct size and desired initial value, and (2) a calculator function that is a class member function (method) with signature: More...
 
const OutputPort< double > & DeclareVectorOutputPort (void(MySystem::*calc)(const Context< double > &, BasicVectorSubtype *) const)
 Declares a vector-valued output port by specifying only a calculator function that is a class member function (method) with signature: More...
 
const OutputPort< double > & DeclareVectorOutputPort (const BasicVector< double > &model_vector, typename LeafOutputPort< double >::CalcVectorCallback vector_calc_function)
 (Advanced) Declares a vector-valued output port using the given model_vector and a function for calculating the port's value at runtime. More...
 
const OutputPort< double > & DeclareAbstractOutputPort (const OutputType &model_value, void(MySystem::*calc)(const Context< double > &, OutputType *) const)
 Declares an abstract-valued output port by specifying a model value of concrete type OutputType and a calculator function that is a class member function (method) with signature: More...
 
const OutputPort< double > & DeclareAbstractOutputPort (void(MySystem::*calc)(const Context< double > &, OutputType *) const)
 Declares an abstract-valued output port by specifying only a calculator function that is a class member function (method) with signature: More...
 
const OutputPort< double > & DeclareAbstractOutputPort (OutputType(MySystem::*make)(const Context< double > &) const, void(MySystem::*calc)(const Context< double > &, OutputType *) const)
 Declares an abstract-valued output port by specifying member functions to use both for the allocator and calculator. More...
 
const OutputPort< double > & DeclareAbstractOutputPort (OutputType(MySystem::*make)() const, void(MySystem::*calc)(const Context< double > &, OutputType *) const)
 Declares an abstract-valued output port by specifying member functions to use both for the allocator and calculator. More...
 
const OutputPort< double > & DeclareAbstractOutputPort (typename LeafOutputPort< double >::AllocCallback alloc_function, typename LeafOutputPort< double >::CalcCallback calc_function)
 (Advanced) Declares an abstract-valued output port using the given allocator and calculator functions provided in their most generic forms. More...
 
- Protected Member Functions inherited from System< double >
virtual void DoGetWitnessFunctions (const Context< double > &, std::vector< const WitnessFunction< double > * > *) const
 Derived classes can override this method to provide witness functions active at the beginning of a continuous time interval. More...
 
SystemConstraintIndex AddConstraint (std::unique_ptr< SystemConstraint< double >> constraint)
 Adds an already-created constraint to the list of constraints for this System. More...
 
const EventCollection< PublishEvent< double > > & get_forced_publish_events () const
 
const EventCollection< DiscreteUpdateEvent< double > > & get_forced_discrete_update_events () const
 
const EventCollection< UnrestrictedUpdateEvent< double > > & get_forced_unrestricted_update_events () const
 
void set_forced_publish_events (std::unique_ptr< EventCollection< PublishEvent< double >>> forced)
 
void set_forced_discrete_update_events (std::unique_ptr< EventCollection< DiscreteUpdateEvent< double >>> forced)
 
void set_forced_unrestricted_update_events (std::unique_ptr< EventCollection< UnrestrictedUpdateEvent< double >>> forced)
 
 System ()
 Constructs an empty System base class object. More...
 
const InputPortDescriptor< double > & DeclareInputPort (PortDataType type, int size)
 Adds a port with the specified type and size to the input topology. More...
 
const InputPortDescriptor< double > & DeclareAbstractInputPort ()
 Adds an abstract-valued port to the input topology. More...
 
void CreateOutputPort (std::unique_ptr< OutputPort< double >> port)
 Adds an already-created output port to this System. More...
 
virtual void DoCalcTimeDerivatives (const Context< double > &context, ContinuousState< double > *derivatives) const
 Override this if you have any continuous state variables xc in your concrete System to calculate their time derivatives. More...
 
virtual double DoCalcPotentialEnergy (const Context< double > &context) const
 Override this method for physical systems to calculate the potential energy currently stored in the configuration provided in the given Context. More...
 
virtual double DoCalcKineticEnergy (const Context< double > &context) const
 Override this method for physical systems to calculate the kinetic energy currently present in the motion provided in the given Context. More...
 
virtual double DoCalcConservativePower (const Context< double > &context) const
 Override this method to return the rate at which mechanical energy is being converted from potential energy to kinetic energy by this system in the given Context. More...
 
virtual double DoCalcNonConservativePower (const Context< double > &context) const
 Override this method to return the rate at which mechanical energy is being generated (positive) or dissipated (negative) other than by conversion between potential and kinetic energy (in the given Context). More...
 
virtual void DoMapQDotToVelocity (const Context< double > &context, const Eigen::Ref< const VectorX< double >> &qdot, VectorBase< double > *generalized_velocity) const
 Provides the substantive implementation of MapQDotToVelocity(). More...
 
virtual void DoMapVelocityToQDot (const Context< double > &context, const Eigen::Ref< const VectorX< double >> &generalized_velocity, VectorBase< double > *qdot) const
 Provides the substantive implementation of MapVelocityToQDot(). More...
 
virtual int do_get_num_constraint_equations (const Context< double > &context) const
 Gets the number of constraint equations for this system from the given context. More...
 
virtual Eigen::VectorXd DoEvalConstraintEquations (const Context< double > &context) const
 Evaluates the constraint equations for the system at the generalized coordinates and generalized velocity specified by the context. More...
 
virtual Eigen::VectorXd DoEvalConstraintEquationsDot (const Context< double > &context) const
 Computes the time derivative of each constraint equation, evaluated at the generalized coordinates and generalized velocity specified by the context. More...
 
virtual Eigen::VectorXd DoCalcVelocityChangeFromConstraintImpulses (const Context< double > &context, const Eigen::MatrixXd &J, const Eigen::VectorXd &lambda) const
 Computes the change in velocity from applying the given constraint forces to the system at the given context. More...
 
virtual double DoCalcConstraintErrorNorm (const Context< double > &context, const Eigen::VectorXd &error) const
 Computes the norm of the constraint error. More...
 
Eigen::VectorBlock< VectorX< double > > GetMutableOutputVector (SystemOutput< double > *output, int port_index) const
 Returns a mutable Eigen expression for a vector valued output port with index port_index in this system. More...
 
void EvalInputPort (const Context< double > &context, int port_index) const
 Causes an InputPortValue in the context to become up-to-date, delegating to the parent Diagram if necessary. More...
 

Detailed Description

An RGB-D camera system that provides RGB, depth and label images using visual elements of RigidBodyTree.

RgbdCamera uses VTK as the rendering backend. Its image resolution is fixed at VGA (640 x 480 pixels) for all three images. The depth sensing range is 0.5 m to 5.0 m.

Let W be the world coordinate system. In addition to W, there are three more coordinate systems that are associated with an RgbdCamera. They are defined as follows:

  • B - The camera's base coordinate system: X-forward, Y-left, and Z-up.
  • C - the camera's color sensor's optical coordinate system: X-right, Y-down and Z-forward.
  • D - the camera's depth sensor's optical coordinate system: X-right, Y-down and Z-forward.

The origins of C and D (i.e., Co and Do, respectively) are offset from B's origin (Bo) by 0 m in B's X-axis, +0.02 m in B's Y-axis, and 0 m in B's Z axis. Since C and D are coincident, the depth image is a "registered depth image" for the RGB image. No disparity between the RGB and depth images are modeled. For more details about the poses of C and D, see the class documentation of CameraInfo.

Output image format:

  • The RGB image has four channels in the following order: red, green blue, alpha. Each channel is represented by a uint8_t.
  • The depth image has a depth channel represented by a float. The value stored in the depth channel holds the Z value in D. Note that this is different from the range data used by laser range finders (like that provided by DepthSensor) in which the depth value represents the distance from the sensor origin to the object's surface.
  • The label image has single channel represented by a int16_t. The value stored in the channel holds a model ID which corresponds to an object in the scene. For the pixels corresponding to no body, namely the sky and the flat terrain, we assign Label::kNoBody and Label::kFlatTerrain, respectively.

Constructor & Destructor Documentation

RgbdCamera ( const RgbdCamera )
delete
RgbdCamera ( RgbdCamera &&  )
delete
RgbdCamera ( const std::string &  name,
const RigidBodyTree< double > &  tree,
const Eigen::Vector3d &  position,
const Eigen::Vector3d &  orientation,
double  depth_range_near = 0.5,
double  depth_range_far = 5.0,
double  fov_y = M_PI_4,
bool  show_window = true 
)

A constructor for RgbdCamera that defines B using Euler angles.

The pose of RgbdCamera will be fixed to the world coordinate system throughout the simulation.

Parameters
nameThe name of the RgbdCamera. This can be any value, but should typically be unique among all sensors attached to a particular model instance.
treeThe RigidBodyTree containing the geometric description of the world. The life span of this parameter must exceed that of this class's instance. The maximum number of bodies in the tree must be less than 1536 based on the number of the colors used for the label image.
positionThe x-y-z position of B in W. This defines the translation component of X_WB.
orientationThe roll-pitch-yaw orientation of B in W. This defines the orientation component of X_WB.
depth_range_nearThe minimum depth distance RgbdCamera can measure. The default is 0.5 meters.
depth_range_farThe maximum depth distance RgbdCamera can measure. The default is 5 meters.
fov_yThe RgbdCamera's vertical field of view. The default is PI / 4.
show_windowA flag for showing a visible window. If this is false, offscreen rendering is executed. This is useful for debugging purposes. The default is true.
RgbdCamera ( const std::string &  name,
const RigidBodyTree< double > &  tree,
const RigidBodyFrame< double > &  frame,
double  depth_range_near = 0.5,
double  depth_range_far = 5.0,
double  fov_y = M_PI_4,
bool  show_window = true 
)

A constructor for RgbdCamera that defines B using a RigidBodyFrame.

The pose of RgbdCamera is fixed to a user-defined frame and will be updated during the simulation.

Parameters
nameThe name of the RgbdCamera. This can be any value, but should typically be unique among all sensors attached to a particular model instance.
treeThe RigidBodyTree containing the geometric description of the world. The life span of this parameter must exceed that of this class's instance. The maximum number of bodies in the tree must be less than 1536 based on the number of the colors used for the label image.
frameThe frame in to which this camera is attached.
depth_range_nearThe minimum depth distance RgbdCamera can measure. The default is 0.5 meters.
depth_range_farThe maximum depth distance RgbdCamera can measure. The default is 5 meters.
fov_yThe RgbdCamera's vertical field of view. The default is PI / 4.
show_windowA flag for showing a visible window. If this is false, offscreen rendering is executed. This is useful for debugging purposes. The default is true.

Here is the call graph for this function:

~RgbdCamera ( )

Member Function Documentation

const OutputPort< double > & camera_base_pose_output_port ( ) const

Returns the vector valued output port that contains a PoseVector.

Here is the caller graph for this function:

const CameraInfo & color_camera_info ( ) const

Reterns the color sensor's info.

const Eigen::Isometry3d & color_camera_optical_pose ( ) const

Returns X_BC.

const OutputPort< double > & color_image_output_port ( ) const

Returns the abstract valued output port that contains a RGBA image of the type ImageRgba8U.

Here is the caller graph for this function:

void ConvertDepthImageToPointCloud ( const ImageDepth32F depth_image,
const CameraInfo camera_info,
Eigen::Matrix3Xf *  point_cloud 
)
static

Converts a depth image obtained from RgbdCamera to a point cloud.

If a pixel in the depth image has NaN depth value, all the (x, y, z) values in the converted point will be NaN. Similarly, if a pixel has either InvalidDepth::kTooFar or InvalidDepth::kTooClose, the converted point will be InvalidDepth::kTooFar. Note that this matches the convention used by the Point Cloud Library (PCL).

Parameters
[in]depth_imageThe input depth image obtained from RgbdCamera.
[in]camera_infoThe input camera info which is used for conversion.
[out]point_cloudThe pointer of output point cloud.

Here is the call graph for this function:

const CameraInfo & depth_camera_info ( ) const

Reterns the depth sensor's info.

const Eigen::Isometry3d & depth_camera_optical_pose ( ) const

Returns X_BD.

const OutputPort< double > & depth_image_output_port ( ) const

Returns the abstract valued output port that contains an ImageDepth32F.

Here is the caller graph for this function:

const RigidBodyFrame< double > & frame ( ) const

Returns the RigidBodyFrame to which this RgbdCamera is attached.

const OutputPort< double > & label_image_output_port ( ) const

Returns the abstract valued output port that contains an label image of the type ImageLabel16I.

Here is the call graph for this function:

Here is the caller graph for this function:

RgbdCamera& operator= ( const RgbdCamera )
delete
RgbdCamera& operator= ( RgbdCamera &&  )
delete
const InputPortDescriptor< double > & state_input_port ( ) const

Returns a descriptor of the vector valued input port that takes a vector of q, v corresponding to the positions and velocities associated with the RigidBodyTree.

Here is the call graph for this function:

Here is the caller graph for this function:

const RigidBodyTree< double > & tree ( ) const

Returns the RigidBodyTree to which this RgbdCamera is attached.


The documentation for this class was generated from the following files: