vrpn_Tracker_Filter.h

Go to the documentation of this file.

 
#pragma once
 
// Internal Includes
#include "quat.h"                       // for q_vec_type
#include "vrpn_Tracker.h"               // for vrpn_Tracker
#include "vrpn_OneEuroFilter.h"
#include <string>

 
class VRPN_API vrpn_Tracker_FilterOneEuro: public vrpn_Tracker {
  public:
    // name is the name that the filtered reports go out under
    // trackercon is the server connection to use to send filtered reports on
    // listen_tracker_name is the name of the tracker we listen to to filter
    //    If the tracker should use the server connection, then put * in
    //    front of the name.
    // channels tells how many channels from the listening tracker to
    //    filter (reports on other channels are ignored by this tracker)
    // The other parameters are passed to each One-Euro filter.
    vrpn_Tracker_FilterOneEuro(const char * name, vrpn_Connection * trackercon,
      const char *listen_tracker_name,
      unsigned channels, vrpn_float64 vecMinCutoff = 1.15,
      vrpn_float64 vecBeta = 0.5, vrpn_float64 vecDerivativeCutoff = 1.2,
      vrpn_float64 quatMinCutoff = 1.5, vrpn_float64 quatBeta = 0.5,
      vrpn_float64 quatDerivativeCutoff = 1.2);
    ~vrpn_Tracker_FilterOneEuro();
 
    virtual void mainloop();
 
  private:
    int  d_channels;                    // How many channels on our tracker?
    vrpn_OneEuroFilterVec *d_filters;   // Set of position filters, one/channel
    vrpn_OneEuroFilterQuat *d_qfilters; // Set of orientation filters, one/channel
    struct timeval  *d_last_report_times;     // Last time of report for each tracker.
    vrpn_Tracker_Remote   *d_listen_tracker;  // Tracker we get our reports from
 
    // Callback handler to deal with getting messages from the tracker we're
    // listening to.  It filters them and then sends them on.
    static void VRPN_CALLBACK handle_tracker_update(void *userdata, const vrpn_TRACKERCB info);
};
 

// of the specified sensors from the specified tracker.  If there are no orientation
// velocity reports, use the two most-recent poses to estimate angular velocity and
// use that to predict.
//  Note: This class does not try to listen for angular acceleration.
//  Note: This class does not try to forward-predict position, it leaves that
// part of the tracker message alone.
 
class VRPN_API vrpn_Tracker_DeadReckoning_Rotation : public vrpn_Tracker_Server
{
  public:
 
    vrpn_Tracker_DeadReckoning_Rotation(
        std::string myName      //< Name of the tracking device we're exposing
        , vrpn_Connection *c    //< Connection to use to send reports on
        , std::string origTrackerName  //< Name of tracker to predict (*Name for one using our connection, Name@URL for one we should connect to)
        , vrpn_int32 numSensors = 1    //< How many sensors to predict for?
        , vrpn_float64 predictionTime = 1.0 / 60.0   //< How far to predict into the future?
        , bool estimateVelocity = true //< Should we estimate angular velocity if we don't get it?
                                       //< If false, this is basically just a pass-through filter, but estimating velocity can be choppy.
        );
 
    ~vrpn_Tracker_DeadReckoning_Rotation();
 
    // Handle ping/pong requests.
    void mainloop();
    
    // prints an error message and returns an integer indicating what happened on failure.
    static int test(void);
 
  protected:
 
    vrpn_float64    d_predictionTime;   //< How far ahead to predict rotation
    vrpn_int32      d_numSensors;       //< How many sensors to predict for?
    vrpn_Tracker_Remote *d_origTracker; //< Original tracker we're predicting for
 
    typedef struct {
        bool d_receivedAngularVelocityReport;   //< If we get these, we don't estimate them
        vrpn_float64 d_rotationAmount[4];       //< How far did we rotate in the specified interval
        double d_rotationInterval;              //< Interval over which we rotated
        vrpn_float64 d_lastPosition[3];         //< What was our last reported position?
        vrpn_float64 d_lastOrientation[4];      //< What was our last orientation?
        struct timeval d_lastReportTime;        //< When did we receive it?
    } RotationState;
    vrpn_vector<RotationState>  d_rotationStates;   //< State of rotation of each sensor.
    
    static void VRPN_CALLBACK handle_tracker_report(void *userdata,
        const vrpn_TRACKERCB info);
    static void VRPN_CALLBACK handle_tracker_velocity_report(void *userdata,
        const vrpn_TRACKERVELCB info);

    // prediction in the future from the original message by the prediction
    // interval.
    void sendNewPrediction(vrpn_int32 sensor);
 
    bool d_estimateVelocity;
};