1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
/*
 * Copyright 2022-2024 Riccardo Monica
 * 
 * This software is distributed under the 3-clause BSD license.
 * You should have received a copy of the 3-clause BSD license
 * along with this software. If not, see
 * <https://opensource.org/license/bsd-3-clause>
 */

#include "tracking_library.h"
#include "tracking_commons.h"
#include "oculus_original_tracking.h"
#include "constrained_complementary_tracking.h"
#include "complementary_tracking.h"
#include "tracking_interface.h"
#include "motive_first_tracking.h"
#include "oculus_first_tracking.h"
#include "double_exponential_smoothing.h"
#include "exponential_smoothing.h"
#include "particle_filter.h"
#include "kalman_tracking.h"
#include "kalman_tracking_constrained.h"
#include "directed_complementary_tracking.h"
#include "kalman_tracking_wrapper.h"

#include <Eigen/Dense>
#include <stdint.h>
#include <cmath>
#include <iostream>
#include <memory>

namespace Tracking
{
  ITracking::Ptr current_tracker(new ConstrainedComplementaryTracking(true, true));
}

extern "C"
{
  EXPORT_API void TrackingUpdate(float * motive_mat,
                                 float * oculus_mat)
  {
    Tracking::current_tracker->Update(Tracking::FloatvToAffine3f(motive_mat), Tracking::FloatvToAffine3f(oculus_mat));
  }

  EXPORT_API bool TrackingIsMotiveLost()
  {
    return Tracking::current_tracker->IsMotiveLost();
  }

  EXPORT_API void TrackingReset()
  {
    Tracking::current_tracker->Reset();
  }

  EXPORT_API void TrackingResetWithInit(float * world_to_oculus_mat,
                                        float * world_to_oculus_origin_mat)
  {
    Tracking::current_tracker->Reset(Tracking::FloatvToAffine3f(world_to_oculus_mat),
                                     Tracking::FloatvToAffine3f(world_to_oculus_origin_mat));
  }

  EXPORT_API void TrackingGetEstimatedOculus(float * oculus_mat_out)
  {
    Eigen::Affine3f oculus_mat = Tracking::current_tracker->GetEstimatedOculus();
    Tracking::Affine3fToFloatv(oculus_mat, oculus_mat_out);
  }

  EXPORT_API int32_t TrackingGetPositionStatus()
  {
    return Tracking::current_tracker->GetPositionStatus();
  }

  EXPORT_API int32_t TrackingGetRotationStatus()
  {
    return Tracking::current_tracker->GetRotationStatus();
  }

  EXPORT_API void TrackingSetKalmanWrappedMode(const int mode)
  {
    TrackingSetMode(mode);
    Tracking::current_tracker.reset(new Tracking::KalmanTrackingWrapper(Tracking::current_tracker));
  }

  EXPORT_API void TrackingSetMode(const int mode)
  {
    if (mode == Tracking::Mode::MODE_OCULUS_ORIGIN)
      Tracking::current_tracker.reset(new Tracking::OculusOriginTracking);
    else if (mode == Tracking::Mode::MODE_CONSTR_COMPLEMENTARY)
      Tracking::current_tracker.reset(new Tracking::ConstrainedComplementaryTracking(true, true));
    else if (mode == Tracking::Mode::MODE_CONSTR_COMPLEMENTARY_RO)
      Tracking::current_tracker.reset(new Tracking::ConstrainedComplementaryTracking(false, true));
    else if (mode == Tracking::Mode::MODE_CONSTR_COMPLEMENTARY_TO)
      Tracking::current_tracker.reset(new Tracking::ConstrainedComplementaryTracking(true, false));
    else if (mode == Tracking::Mode::MODE_COMPLEMENTARY)
      Tracking::current_tracker.reset(new Tracking::ComplementaryTracking);
    else if (mode == Tracking::Mode::MODE_MOTIVE_FIRST)
      Tracking::current_tracker.reset(new Tracking::MotiveFirstTracking);
    else if (mode == Tracking::Mode::MODE_DOUBLE_EXP)
      Tracking::current_tracker.reset(new Tracking::DoubleExponentialSmoothing);
    else if (mode == Tracking::Mode::MODE_EXP)
      Tracking::current_tracker.reset(new Tracking::ExponentialSmoothing);
    else if (mode == Tracking::Mode::MODE_PARTICLE_FILTER)
      Tracking::current_tracker.reset(new Tracking::ParticleFilter);
    else if (mode == Tracking::Mode::MODE_KALMAN_FILTER)
      Tracking::current_tracker.reset(new Tracking::KalmanTracking);
    else if (mode == Tracking::Mode::MODE_CONSTR_KALMAN_FILTER)
      Tracking::current_tracker.reset(new Tracking::ConstrainedKalmanTracking);
    else if (mode == Tracking::Mode::MODE_DIR_COMPLEMENTARY)
      Tracking::current_tracker.reset(new Tracking::DirectedComplementaryTracking);
    else if (mode == Tracking::Mode::MODE_OCULUS_FIRST)
      Tracking::current_tracker.reset(new Tracking::OculusFirstTracking);
    else
      std::cout << "TrackingSetMode: Error: unknown mode " << mode << std::endl;
  }

  EXPORT_API bool TrackingSetParameter(const int parameter_id, const float value)
  {
    return Tracking::current_tracker->SetParameter(parameter_id, value);
  }

  EXPORT_API float TrackingGetParameter(const int parameter_id)
  {
    return Tracking::current_tracker->GetParameter(parameter_id);
  }

  EXPORT_API bool TrackingHasConverged()
  {
    return Tracking::current_tracker->HasConverged();
  }
}