Ellipsoid.hpp

#pragma once
 
#include <list>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Sparse>
#include <random>
#include "state_representation/geometry/Shape.hpp"
#include "state_representation/space/cartesian/CartesianPose.hpp"
 
namespace state_representation {
class Ellipsoid : public Shape {
private:
  std::vector<double> axis_lengths_; 
  double rotation_angle_; 
 
public:
  Ellipsoid();
  explicit Ellipsoid(const std::string& name, const std::string& reference_frame = "world");
 
  Ellipsoid(const Ellipsoid& ellipsoid);
 
  Ellipsoid& operator=(const Ellipsoid& state);
 
  const std::vector<double>& get_axis_lengths() const;
 
  double get_axis_length(unsigned int index) const;
 
  void set_axis_lengths(const std::vector<double>& axis_lengths);
 
  void set_axis_lengths(unsigned int index, double axis_length);
 
  double get_rotation_angle() const;
 
  void set_rotation_angle(double rotation_angle);
 
  const CartesianPose get_rotation() const;
 
  const std::list<CartesianPose> sample_from_parameterization(unsigned int nb_samples) const;
 
  static const Ellipsoid from_algebraic_equation(
      const std::string& name, const std::vector<double>& coefficients, const std::string& reference_frame = "world"
  );
 
  static const Ellipsoid fit(
      const std::string& name, const std::list<CartesianPose>& points, const std::string& reference_frame = "world",
      double noise_level = 0.01
  );
 
  const std::vector<double> to_std_vector() const;
 
  virtual void set_data(const Eigen::VectorXd& data) override;
 
  virtual void set_data(const std::vector<double>& data) override;
 
  friend std::ostream& operator<<(std::ostream& os, const Ellipsoid& ellipsoid);
};
 
inline Ellipsoid& Ellipsoid::operator=(const Ellipsoid& state) {
  Shape::operator=(state);
  this->set_axis_lengths(state.get_axis_lengths());
  this->set_rotation_angle(state.get_rotation_angle());
  return (*this);
}
 
inline const std::vector<double>& Ellipsoid::get_axis_lengths() const {
  return this->axis_lengths_;
}
 
inline double Ellipsoid::get_axis_length(unsigned int index) const {
  return this->axis_lengths_[index];
}
 
inline void Ellipsoid::set_axis_lengths(const std::vector<double>& axis_lengths) {
  this->axis_lengths_ = axis_lengths;
  this->set_filled();
}
 
inline void Ellipsoid::set_axis_lengths(unsigned int index, double axis_length) {
  this->axis_lengths_[index] = axis_length;
  this->set_filled();
}
 
inline double Ellipsoid::get_rotation_angle() const {
  return this->rotation_angle_;
}
 
inline void Ellipsoid::set_rotation_angle(double rotation_angle) {
  this->rotation_angle_ = rotation_angle;
  this->set_filled();
}
 
inline const std::vector<double> Ellipsoid::to_std_vector() const {
  if (this->get_center_state().is_empty()) {
    throw exceptions::EmptyStateException("The center state of the Ellipsoid is not set yet.");
  }
  std::vector<double> representation(6);
  // position
  representation[0] = this->get_center_position()(0);
  representation[1] = this->get_center_position()(1);
  representation[2] = this->get_center_position()(2);
  // rotation angle
  representation[3] = this->get_rotation_angle();
  // axis lengths
  representation[4] = this->get_axis_length(0);
  representation[5] = this->get_axis_length(1);
  return representation;
}
 
inline const CartesianPose Ellipsoid::get_rotation() const {
  if (this->get_center_state().is_empty()) {
    throw exceptions::EmptyStateException("The center state of the Ellipsoid is not set yet.");
  }
  Eigen::Quaterniond rotation(Eigen::AngleAxisd(this->rotation_angle_, Eigen::Vector3d::UnitZ()));
  return CartesianPose(
      this->get_center_pose().get_name() + "_rotated", Eigen::Vector3d::Zero(), rotation,
      this->get_center_pose().get_name());
}
}