orientationTracker.cpp

#include <scolPlugin.h>
#include "orientationTracker.h"
#include "tools/SO3Util.h"
#include "SSensor.h"
 
static const float defaultNeckHorizontalOffset = 0.08f;
static const float defaultNeckVerticalOffset = 0.075f;
 
OrientationTracker::OrientationTracker() :
  mDisplayFromDevice(Quaternion(1.0f, 0.0f, 0.0f, 0.0f)),
  mInertialReferenceFrameFromWorld(Quaternion(sqrt(0.5f), -sqrt(0.5f), 0.0f, 0.0f)),
  mLastGyroEventTimestamp(0),
  mOrientationCorrectionAngle(0),
  mNeckModelEnabled(true)
{
  mNeckModelTranslation = Matrix4::IDENTITY;
  Matrix4 translationMatrix;
  translationMatrix.makeTrans(0.0f, - defaultNeckVerticalOffset, defaultNeckHorizontalOffset);
  mNeckModelTranslation = mNeckModelTranslation * translationMatrix;
}
 
OrientationTracker::~OrientationTracker()
{
}
 
void OrientationTracker::startTracking(DeviceOrientation orientation)
{
  updateDeviceOrientation(orientation);
  mBiasEstimator.reset();
  mTracker.reset();
}
 
void OrientationTracker::processGyro(Vector3d gyro, int64_t sensorTimeStamp)
{
  // event.timestamp have no consistent values between constructors ^^;
  mLastGyroEventTimestamp = GetTickCountNano();
  mBiasEstimator.processGyroscope(gyro, sensorTimeStamp);
  mBiasEstimator.getGyroBias(mGyroBias);
 
  Vector3d::sub(gyro, mGyroBias, gyro);
  mTracker.processGyro(gyro, sensorTimeStamp);
}
 
void OrientationTracker::processAcceleration(Vector3d acc, int64_t sensorTimeStamp)
{
  mTracker.processAcc(acc, sensorTimeStamp);
  mBiasEstimator.processAccelerometer(acc, sensorTimeStamp);
}
 
void OrientationTracker::processMag(Vector3d mag, int64_t sensorTimeStamp)
{
  mTracker.processMag(mag, sensorTimeStamp);
}
 
void OrientationTracker::stopTracking()
{
}
 
bool OrientationTracker::isReady()
{
  return mTracker.isReady();
}
 
Quaternion OrientationTracker::lastHeadView(double timeOffsetInSeconds)
{
  if (mLastGyroEventTimestamp == 0)
  {
    return Quaternion::IDENTITY;
  }
 
  double secondsSinceLastGyroEvent = (double)(GetTickCountNano() - mLastGyroEventTimestamp) * 1.e-09;
  double secondsToPredictForward = secondsSinceLastGyroEvent + 0.016;//(timeOffsetInSeconds * 0.5);// use static time like in the original code timeOffsetInSeconds;
 
  double* predictedMat = mTracker.getPredictedMatrix(secondsToPredictForward);
  if (mNeckModelEnabled)
  {
    Matrix4 headmat(predictedMat);
    headmat = mNeckModelTranslation * headmat;
    headmat.convert(predictedMat);
  }
 
  Quaternion frameQuat = Quaternion::FromRotationMatrix(predictedMat);
  if (!isReady())
    return Quaternion::IDENTITY;
  
  frameQuat = mInertialReferenceFrameFromWorld * frameQuat * mDisplayFromDevice;
  return frameQuat;
}
 
void OrientationTracker::updateDeviceOrientation(DeviceOrientation orientation)
{
  if (orientation == ROTATION_0)
  {
    mDisplayFromDevice = Quaternion(1.0f, 0.0f, 0.0f, 0.0f);
  }
  else if (orientation == ROTATION_90) // we invert here 90 / 270
  {
    // Z 270°
    mDisplayFromDevice = Quaternion(sqrt(0.5f), 0.0f, 0.0f, sqrt(0.5f));
    mDisplayFromDevice = Quaternion(0.0f, 0.0f, 0.0f, 1.0f) * mDisplayFromDevice;
  }
  else if (orientation == ROTATION_180)
  {
    mDisplayFromDevice = Quaternion(0.0f, 0.0f, 0.0f, 1.0f);
  }
  else if (orientation == ROTATION_270)
  {
    // Z 90°
    mDisplayFromDevice = Quaternion(sqrt(0.5f), 0.0f, 0.0f, sqrt(0.5f));
  }
}
 
bool OrientationTracker::neckModelEnabled()
{
  return mNeckModelEnabled;
}
 
void OrientationTracker::setNeckModelEnabled(bool enabled)
{
  mNeckModelEnabled = enabled;
}