TOAFactor.h

Go to the documentation of this file.

/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

#pragma once

#include <gtsam/nonlinear/ExpressionFactor.h>
#include <gtsam_unstable/geometry/Event.h>

namespace gtsam {

class TOAFactor : public ExpressionFactor<double> {
  typedef Expression<double> Double_;

 public:
  TOAFactor(const Expression<Event>& eventExpression,
            const Expression<Point3>& sensorExpression, double toaMeasurement,
            const SharedNoiseModel& model, double speed = 330)
      : ExpressionFactor<double>(
            model, toaMeasurement,
            Double_(TimeOfArrival(speed), eventExpression, sensorExpression)) {}

  TOAFactor(const Expression<Event>& eventExpression, const Point3& sensor,
            double toaMeasurement, const SharedNoiseModel& model,
            double speed = 330)
      : TOAFactor(eventExpression, Expression<Point3>(sensor), toaMeasurement,
                  model, speed) {}

  static void InsertEvent(Key key, const Event& event,
                          std::shared_ptr<Values> values) {
    values->insert(key, event);
  }
};

}  // namespace gtsam