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,
                          boost::shared_ptr<Values> values) {
    values->insert(key, event);
  }
};
 
}  // namespace gtsam