Ellipsoid.hpp

#pragma once
 
#include <list>
 
#include "state_representation/geometry/Shape.hpp"
#include "state_representation/space/cartesian/CartesianPose.hpp"
 
namespace state_representation {
class Ellipsoid : public Shape {
public:
  Ellipsoid();
 
  explicit Ellipsoid(const std::string& name, const std::string& reference_frame = "world");
 
  Ellipsoid(const Ellipsoid& ellipsoid);
 
  static Ellipsoid Unit(const std::string& name, const std::string& reference_frame = "world");
 
  friend void swap(Ellipsoid& state1, Ellipsoid& state2);
 
  Ellipsoid& operator=(const Ellipsoid& state);
 
  const std::vector<double>& get_axis_lengths() const;
 
  double get_axis_length(unsigned int index) const;
 
  double get_rotation_angle() const;
 
  const CartesianPose get_rotation() const;
 
  void set_axis_lengths(const std::vector<double>& axis_lengths);
 
  void set_axis_lengths(unsigned int index, double axis_length);
 
  void set_rotation_angle(double rotation_angle);
 
  void set_data(const Eigen::VectorXd& data) override;
 
  void set_data(const std::vector<double>& data) override;
 
  static const Ellipsoid fit(
      const std::string& name, const std::list<CartesianPose>& points, const std::string& reference_frame = "world",
      double noise_level = 0.01
  );
 
  static const Ellipsoid from_algebraic_equation(
      const std::string& name, const std::vector<double>& coefficients, const std::string& reference_frame = "world"
  );
 
  const std::list<CartesianPose> sample_from_parameterization(unsigned int nb_samples) const;
 
  const std::vector<double> to_std_vector() const;
 
  friend std::ostream& operator<<(std::ostream& os, const Ellipsoid& ellipsoid);
 
private:
  std::vector<double> axis_lengths_; 
  double rotation_angle_; 
};
 
inline void swap(Ellipsoid& state1, Ellipsoid& state2) {
  swap(static_cast<Shape&>(state1), static_cast<Shape&>(state2));
  std::swap(state1.axis_lengths_, state2.axis_lengths_);
  std::swap(state1.rotation_angle_, state2.rotation_angle_);
}
}// namespace state_representation