Control Libraries 7.4.0
|
Public Member Functions | |
Ellipsoid () | |
Empty constructor. | |
Ellipsoid (const std::string &name, const std::string &reference_frame="world") | |
Constructor with name and reference frame. | |
Ellipsoid (const Ellipsoid &ellipsoid) | |
Copy constructor from another Ellipsoid. | |
Ellipsoid & | operator= (const Ellipsoid &state) |
Copy assignment operator that has to be defined to the custom assignment operator. | |
const std::vector< double > & | get_axis_lengths () const |
Getter of the axis lengths. | |
double | get_axis_length (unsigned int index) const |
Getter of the axis length in one direction. | |
double | get_rotation_angle () const |
Getter of the rotation angle. | |
const CartesianPose | get_rotation () const |
Getter of the rotation as a pose. | |
void | set_axis_lengths (const std::vector< double > &axis_lengths) |
Setter of the axis lengths. | |
void | set_axis_lengths (unsigned int index, double axis_length) |
Setter of the axis length at given index. | |
void | set_rotation_angle (double rotation_angle) |
Setter of the rotation angle. | |
void | set_data (const Eigen::VectorXd &data) override |
Set the ellipsoid data from an Eigen vector. | |
void | set_data (const std::vector< double > &data) override |
Set the ellipsoid data from a std vector. | |
const std::list< CartesianPose > | sample_from_parameterization (unsigned int nb_samples) const |
Function to sample an obstacle from its parameterization. | |
const std::vector< double > | to_std_vector () const |
Convert the Ellipsoid to an std vector representation of its parameter. | |
Public Member Functions inherited from state_representation::Shape | |
Shape () | |
Empty constructor. | |
Shape (const std::string &name, const std::string &reference_frame="world") | |
Constructor with name and reference frame. | |
Shape (const Shape &shape) | |
Copy constructor from another Shape. | |
Shape & | operator= (const Shape &state) |
Copy assignment operator that has to be defined to the custom assignment operator. | |
const CartesianState & | get_center_state () const |
Getter of the state. | |
const CartesianPose & | get_center_pose () const |
Getter of the pose from the state. | |
const Eigen::Vector3d | get_center_position () const |
Getter of the position from the state. | |
const Eigen::Quaterniond | get_center_orientation () const |
Getter of the orientation from the state. | |
const CartesianTwist & | get_center_twist () const |
Getter of the twist from the state. | |
void | set_center_state (const CartesianState &state) |
Setter of the state. | |
void | set_center_pose (const CartesianPose &pose) |
Setter of the pose. | |
void | set_center_position (const Eigen::Vector3d &position) |
Setter of the position. | |
void | set_center_orientation (const Eigen::Quaterniond &orientation) |
Setter of the pose. | |
Public Member Functions inherited from state_representation::State | |
State () | |
Empty constructor. | |
State (const std::string &name) | |
Constructor with name specification. | |
State (const State &state) | |
Copy constructor from another state. | |
virtual | ~State ()=default |
Virtual destructor. | |
State & | operator= (const State &state) |
Copy assignment operator that has to be defined to the custom assignment operator. | |
const StateType & | get_type () const |
Getter of the type attribute. | |
const std::string & | get_name () const |
Getter of the name attribute. | |
bool | is_empty () const |
Getter of the empty attribute. | |
const std::chrono::time_point< std::chrono::steady_clock > & | get_timestamp () const |
Getter of the timestamp attribute. | |
virtual void | set_name (const std::string &name) |
Setter of the name attribute. | |
void | reset_timestamp () |
Reset the timestamp attribute to now. | |
virtual void | set_data (const Eigen::MatrixXd &data) |
Set the data of the state from an Eigen matrix. | |
double | get_age () const |
Get the age of the state, i.e. the time since the last modification. | |
virtual bool | is_incompatible (const State &state) const |
Check if the state is incompatible for operations with the state given as argument. | |
bool | is_deprecated (double time_delay) const |
Check if the state is deprecated given a certain time delay. | |
template<typename DurationT > | |
bool | is_deprecated (const std::chrono::duration< int64_t, DurationT > &time_delay) const |
Check if the state is deprecated given a certain time delay. | |
virtual void | reset () |
Reset the object to a post-construction state. | |
operator bool () const noexcept | |
Boolean operator for the truthiness of a state. | |
Static Public Member Functions | |
static Ellipsoid | Unit (const std::string &name, const std::string &reference_frame="world") |
Constructor for a Ellipsoid with identity state, unit axis lengths and zero rotation angle. | |
static const Ellipsoid | fit (const std::string &name, const std::list< CartesianPose > &points, const std::string &reference_frame="world", double noise_level=0.01) |
Fit an Ellipsoid on a set of points This method uses direct least square fitting from Fitzgibbon, A., et al. (1999). "Direct least square fitting of ellipses." IEEE Transactions on pattern analysis and machine intelligence 21(5) | |
static const Ellipsoid | from_algebraic_equation (const std::string &name, const std::vector< double > &coefficients, const std::string &reference_frame="world") |
Compute an Ellipsoid from its algebraic equation ax2 + bxy + cy2 + cx + ey + f. | |
Static Public Member Functions inherited from state_representation::Shape | |
static Shape | Unit (const std::string &name, const std::string &reference_frame="world") |
Constructor for a Shape with identity state. | |
Friends | |
void | swap (Ellipsoid &state1, Ellipsoid &state2) |
Swap the values of the two Ellipsoids. | |
std::ostream & | operator<< (std::ostream &os, const Ellipsoid &ellipsoid) |
Overload the ostream operator for printing. | |
Additional Inherited Members | |
Protected Member Functions inherited from state_representation::Shape | |
std::string | to_string () const override |
Convert the state to its string representation. | |
Protected Member Functions inherited from state_representation::State | |
void | set_type (const StateType &type) |
Setter of the state type attribute. | |
void | set_empty (bool empty=true) |
Setter of the empty attribute. | |
void | assert_not_empty () const |
Throw an exception if the state is empty. | |
Definition at line 12 of file Ellipsoid.hpp.
state_representation::Ellipsoid::Ellipsoid | ( | ) |
Empty constructor.
Definition at line 12 of file Ellipsoid.cpp.
|
explicit |
Constructor with name and reference frame.
name | Name of the Ellipsoid and its state |
reference_frame | The reference frame in which the state is expressed (default is "world") |
Definition at line 16 of file Ellipsoid.cpp.
state_representation::Ellipsoid::Ellipsoid | ( | const Ellipsoid & | ellipsoid | ) |
Copy constructor from another Ellipsoid.
shape | The Ellipsoid to copy from |
Definition at line 21 of file Ellipsoid.cpp.
|
static |
Fit an Ellipsoid on a set of points This method uses direct least square fitting from Fitzgibbon, A., et al. (1999). "Direct least square fitting of ellipses." IEEE Transactions on pattern analysis and machine intelligence 21(5)
Definition at line 93 of file Ellipsoid.cpp.
|
static |
Compute an Ellipsoid from its algebraic equation ax2 + bxy + cy2 + cx + ey + f.
Definition at line 176 of file Ellipsoid.cpp.
double state_representation::Ellipsoid::get_axis_length | ( | unsigned int | index | ) | const |
Getter of the axis length in one direction.
index | The index of the length (0 for x, 1 for y and 2 for z) |
Definition at line 46 of file Ellipsoid.cpp.
const std::vector< double > & state_representation::Ellipsoid::get_axis_lengths | ( | ) | const |
const CartesianPose state_representation::Ellipsoid::get_rotation | ( | ) | const |
Getter of the rotation as a pose.
Definition at line 56 of file Ellipsoid.cpp.
double state_representation::Ellipsoid::get_rotation_angle | ( | ) | const |
Getter of the rotation angle.
Definition at line 51 of file Ellipsoid.cpp.
Copy assignment operator that has to be defined to the custom assignment operator.
state | The Ellipsoid with value to assign |
Definition at line 35 of file Ellipsoid.cpp.
const std::list< CartesianPose > state_representation::Ellipsoid::sample_from_parameterization | ( | unsigned int | nb_samples | ) | const |
Function to sample an obstacle from its parameterization.
nb_samples | The number of sample points to generate |
Definition at line 222 of file Ellipsoid.cpp.
void state_representation::Ellipsoid::set_axis_lengths | ( | const std::vector< double > & | axis_lengths | ) |
Setter of the axis lengths.
axis_lengths | The new values |
Definition at line 64 of file Ellipsoid.cpp.
void state_representation::Ellipsoid::set_axis_lengths | ( | unsigned int | index, |
double | axis_length | ||
) |
Setter of the axis length at given index.
index | The desired index |
axis_length | The new length |
Definition at line 69 of file Ellipsoid.cpp.
|
overridevirtual |
Set the ellipsoid data from an Eigen vector.
The | data vector with [center_position, rotation_angle, axis_lengths] |
Reimplemented from state_representation::State.
Definition at line 79 of file Ellipsoid.cpp.
|
overridevirtual |
Set the ellipsoid data from a std vector.
The | data vector with [center_position, rotation_angle, axis_lengths] |
Reimplemented from state_representation::State.
Definition at line 89 of file Ellipsoid.cpp.
void state_representation::Ellipsoid::set_rotation_angle | ( | double | rotation_angle | ) |
Setter of the rotation angle.
rotation_angle | The rotation angle |
Definition at line 74 of file Ellipsoid.cpp.
const std::vector< double > state_representation::Ellipsoid::to_std_vector | ( | ) | const |
Convert the Ellipsoid to an std vector representation of its parameter.
Definition at line 243 of file Ellipsoid.cpp.
|
static |
Constructor for a Ellipsoid with identity state, unit axis lengths and zero rotation angle.
name | Name of the Ellipsoid and its state |
reference_frame | The reference frame in which the state is expressed (default is "world") |
Definition at line 29 of file Ellipsoid.cpp.
|
friend |
Overload the ostream operator for printing.
os | The ostream to append the string representing the state |
state | The state to print |
Definition at line 258 of file Ellipsoid.cpp.
Swap the values of the two Ellipsoids.
Definition at line 153 of file Ellipsoid.hpp.