VisualSensors.h

#ifndef CONTROL_VISUAL_SENSORS_H
#define CONTROL_VISUAL_SENSORS_H

#include "Sensor.h"
#include <KrisLibrary/camera/viewport.h>
#include <KrisLibrary/math3d/primitives.h>
#include <KrisLibrary/GLdraw/GLRenderToImage.h>
#include <KrisLibrary/GLdraw/GLDisplayList.h>

namespace Klampt {
  using namespace Math3D;

class WorldModel;
class RobotModel;

class LaserRangeSensor : public SensorBase
{
 public:
  LaserRangeSensor();
  virtual ~LaserRangeSensor() {}
  virtual const char* Type() const override { return "LaserRangeSensor"; }
  virtual void Simulate(SimRobotController* robot,Simulator* sim) override;
  virtual void SimulateKinematic(RobotModel& robot,WorldModel& world) override;
  virtual void Advance(double dt) override;
  virtual void Reset() override;
  virtual void MeasurementNames(vector<string>& names) const override;
  virtual void GetMeasurements(vector<double>& values) const override;
  virtual void SetMeasurements(const vector<double>& values) override;
  virtual map<string,string> Settings() const override;
  virtual bool GetSetting(const string& name,string& str) const override;
  virtual bool SetSetting(const string& name,const string& str) override;
  virtual void DrawGL(const RobotModel& robot,const vector<double>& measurements) override;

  int link;
  RigidTransform Tsensor; 
  int measurementCount;   
  double depthResolution; 
  double depthMinimum,depthMaximum;    
  double depthVarianceLinear,depthVarianceConstant;
  enum { SweepSinusoid, SweepTriangular, SweepSawtooth};
  Real xSweepMagnitude,xSweepPeriod,xSweepPhase;
  int xSweepType;
  Real ySweepMagnitude,ySweepPeriod,ySweepPhase;
  int ySweepType; 

  //simulated depth readings
  vector<double> depthReadings;
  //internal state
  Real last_dt,last_t;
};


class CameraSensor : public SensorBase
{
 public:
  CameraSensor();
  virtual ~CameraSensor();
  virtual const char* Type() const override { return "CameraSensor"; }
  virtual void Simulate(SimRobotController* robot,Simulator* sim) override;
  virtual void SimulateKinematic(RobotModel& robot,WorldModel& world) override;
  virtual void Reset() override;
  virtual void MeasurementNames(vector<string>& names) const override;
  virtual void GetMeasurements(vector<double>& values) const override;
  virtual void SetMeasurements(const vector<double>& values) override;
  virtual map<string,string> Settings() const override;
  virtual bool GetSetting(const string& name,string& str) const override;
  virtual bool SetSetting(const string& name,const string& str) override;
  virtual void DrawGL(const RobotModel& robot,const vector<double>& measurements) override;
  void GetViewport(Camera::Viewport& view) const;
  void SetViewport(const Camera::Viewport& view);

  int link;
  RigidTransform Tsensor; 
  bool rgb,depth;  
  int xres,yres;  
  double xfov,yfov; 
  double zmin,zmax;  
  int zresolution;  
  double zvarianceLinear;  
  double zvarianceConstant;  

  //internal: used for OpenGL rendering / buffers
  bool useGLFramebuffers; 
  GLDraw::GLRenderToImage renderer;
  //last measurements
  vector<unsigned char> pixels;
  vector<float> floats;
  //visualization state
  GLDraw::GLDisplayList depthDisplayList;
  unsigned int depthDisplayHash;
};

} //namespace Klampt

#endif