From 59f6f081c59c81d76cddc7b8b5cef7a26766e868 Mon Sep 17 00:00:00 2001 From: yetongumich Date: Mon, 5 Aug 2024 18:02:34 -0400 Subject: [PATCH 01/52] add definition of constraints --- gtsam/CMakeLists.txt | 1 + gtsam/constraint/CMakeLists.txt | 6 + gtsam/constraint/NonlinearConstraint.h | 65 +++++ .../NonlinearEqualityConstraint-inl.h | 54 ++++ .../NonlinearEqualityConstraint.cpp | 83 ++++++ .../constraint/NonlinearEqualityConstraint.h | 120 ++++++++ .../NonlinearInequalityConstraint.cpp | 158 +++++++++++ .../NonlinearInequalityConstraint.h | 125 ++++++++ gtsam/constraint/RampFunction.cpp | 71 +++++ gtsam/constraint/RampFunction.h | 78 +++++ gtsam/constraint/tests/CMakeLists.txt | 9 + gtsam/constraint/tests/constrainedExample.h | 168 +++++++++++ .../tests/testNonlinearEqualityConstraint.cpp | 267 ++++++++++++++++++ .../testNonlinearInequalityConstraint.cpp | 117 ++++++++ gtsam/constraint/tests/testSimple.cpp | 37 +++ gtsam/nonlinear/Expression.h | 4 + 16 files changed, 1363 insertions(+) create mode 100644 gtsam/constraint/CMakeLists.txt create mode 100644 gtsam/constraint/NonlinearConstraint.h create mode 100644 gtsam/constraint/NonlinearEqualityConstraint-inl.h create mode 100644 gtsam/constraint/NonlinearEqualityConstraint.cpp create mode 100644 gtsam/constraint/NonlinearEqualityConstraint.h create mode 100644 gtsam/constraint/NonlinearInequalityConstraint.cpp create mode 100644 gtsam/constraint/NonlinearInequalityConstraint.h create mode 100644 gtsam/constraint/RampFunction.cpp create mode 100644 gtsam/constraint/RampFunction.h create mode 100644 gtsam/constraint/tests/CMakeLists.txt create mode 100644 gtsam/constraint/tests/constrainedExample.h create mode 100644 gtsam/constraint/tests/testNonlinearEqualityConstraint.cpp create mode 100644 gtsam/constraint/tests/testNonlinearInequalityConstraint.cpp create mode 100644 gtsam/constraint/tests/testSimple.cpp diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index 1fc8e4570..dcc350fe8 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -6,6 +6,7 @@ project(gtsam LANGUAGES CXX) set (gtsam_subdirs base basis + constraint geometry inference symbolic diff --git a/gtsam/constraint/CMakeLists.txt b/gtsam/constraint/CMakeLists.txt new file mode 100644 index 000000000..e4bbd89dd --- /dev/null +++ b/gtsam/constraint/CMakeLists.txt @@ -0,0 +1,6 @@ +# Install headers +file(GLOB constraint_headers "*.h") +install(FILES ${constraint_headers} DESTINATION include/gtsam/constraint) + +# Build tests +add_subdirectory(tests) diff --git a/gtsam/constraint/NonlinearConstraint.h b/gtsam/constraint/NonlinearConstraint.h new file mode 100644 index 000000000..d0e7aef6b --- /dev/null +++ b/gtsam/constraint/NonlinearConstraint.h @@ -0,0 +1,65 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @file NonlinearConstraint.h + * @brief Equality constraints in constrained optimization. + * @author Yetong Zhang, Frank Dellaert + * @date Aug 3, 2024 + */ + +#pragma once + +#include +#include "gtsam/linear/NoiseModel.h" + +namespace gtsam { + +/** + * Base class for nonlinear constraint. + * The constraint is represented as a NoiseModelFactor with Constrained noise model. + * whitenedError() returns The constraint violation vector. + * unwhitenedError() returns the sigma-scaled constraint violation vector. + */ +class NonlinearConstraint : public NoiseModelFactor { + public: + typedef NoiseModelFactor Base; + + /** Use constructors of NoiseModelFactor. */ + using Base::Base; + + /** Destructor. */ + virtual ~NonlinearConstraint() {} + + /** Create a cost factor representing the L2 penalty function with scaling coefficient mu. */ + virtual NoiseModelFactor::shared_ptr penaltyFactor(const double mu = 1.0) const = 0; + + /** Return the norm of the constraint violation vector. */ + virtual double violation(const Values& x) const { return sqrt(2 * error(x)); } + + /** Check if constraint violation is within tolerance. */ + virtual bool feasible(const Values& x, const double tolerance = 1e-5) const { + return violation(x) <= tolerance; + } + + protected: + /** Noise model used for the penalty function. */ + SharedDiagonal penaltyNoise(const double mu) const { + return noiseModel::Diagonal::Sigmas(noiseModel()->sigmas() / sqrt(mu)); + } + + /** Default constrained noisemodel used for construction of constraint. */ + static SharedNoiseModel constrainedNoise(const Vector& sigmas) { + return noiseModel::Constrained::MixedSigmas(1.0, sigmas); + } +}; + +} // namespace gtsam diff --git a/gtsam/constraint/NonlinearEqualityConstraint-inl.h b/gtsam/constraint/NonlinearEqualityConstraint-inl.h new file mode 100644 index 000000000..a594d0b09 --- /dev/null +++ b/gtsam/constraint/NonlinearEqualityConstraint-inl.h @@ -0,0 +1,54 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @file EqualityConstraint-inl.h + * @brief Equality constraints in constrained optimization. + * @author Yetong Zhang, Frank Dellaert + * @date Aug 3, 2024 */ + +#pragma once + +#include + +namespace gtsam { + +/* ************************************************************************* */ +template +ExpressionEqualityConstraint::ExpressionEqualityConstraint(const Expression& expression, + const T& rhs, + const Vector& sigmas) + : Base(constrainedNoise(sigmas), expression.keysAndDims().first), + expression_(expression), + rhs_(rhs), + dims_(expression.keysAndDims().second) {} + +/* ************************************************************************* */ +template +Vector ExpressionEqualityConstraint::unwhitenedError(const Values& x, + OptionalMatrixVecType H) const { + // Copy-paste from ExpressionFactor. + if (H) { + const T value = expression_.valueAndDerivatives(x, keys_, dims_, *H); + return -traits::Local(value, rhs_); + } else { + const T value = expression_.value(x); + return -traits::Local(value, rhs_); + } +} + +/* ************************************************************************* */ +template +NoiseModelFactor::shared_ptr ExpressionEqualityConstraint::penaltyFactor(const double mu) const { + return std::make_shared>(penaltyNoise(mu), rhs_, expression_); +} + +} // namespace gtsam diff --git a/gtsam/constraint/NonlinearEqualityConstraint.cpp b/gtsam/constraint/NonlinearEqualityConstraint.cpp new file mode 100644 index 000000000..06201dc2a --- /dev/null +++ b/gtsam/constraint/NonlinearEqualityConstraint.cpp @@ -0,0 +1,83 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @file EqualityConstraint.cpp + * @brief Equality constraints in constrained optimization. + * @author Yetong Zhang, Frank Dellaert + * @date Aug 3, 2024 */ + +#include + +namespace gtsam { + +/* ************************************************************************* */ +ZeroCostConstraint::ZeroCostConstraint(const NoiseModelFactor::shared_ptr& factor) + : Base(constrainedNoise(factor->noiseModel()->sigmas()), factor->keys()), factor_(factor) {} + +/* ************************************************************************* */ +Vector ZeroCostConstraint::unwhitenedError(const Values& x, OptionalMatrixVecType H) const { + return factor_->unwhitenedError(x, H); +} + +/* ************************************************************************* */ +NoiseModelFactor::shared_ptr ZeroCostConstraint::penaltyFactor(const double mu) const { + return factor_->cloneWithNewNoiseModel(penaltyNoise(mu)); +} + +/* ************************************************************************* */ +NonlinearEqualityConstraints NonlinearEqualityConstraints::FromCostGraph( + const NonlinearFactorGraph& graph) { + NonlinearEqualityConstraints constraints; + for (const auto& factor : graph) { + auto noise_factor = std::dynamic_pointer_cast(factor); + constraints.emplace_shared(noise_factor); + } + return constraints; +} + +/* ************************************************************************* */ +size_t NonlinearEqualityConstraints::dim() const { + size_t dimension = 0; + for (const auto& constraint : *this) { + dimension += constraint->dim(); + } + return dimension; +} + +/* ************************************************************************* */ +Vector NonlinearEqualityConstraints::violationVector(const Values& values, bool whiten) const { + Vector violation(dim()); + size_t start_idx = 0; + for (const auto& constraint : *this) { + size_t dim = constraint->dim(); + violation.middleCols(start_idx, dim) = + whiten ? constraint->whitenedError(values) : constraint->unwhitenedError(values); + start_idx += dim; + } + return violation; +} + +/* ************************************************************************* */ +double NonlinearEqualityConstraints::violationNorm(const Values& values) const { + return violationVector(values, true).norm(); +} + +/* ************************************************************************* */ +NonlinearFactorGraph NonlinearEqualityConstraints::penaltyGraph(const double mu) const { + NonlinearFactorGraph graph; + for (const auto& constraint : *this) { + graph.add(constraint->penaltyFactor(mu)); + } + return graph; +} + +} // namespace gtsam diff --git a/gtsam/constraint/NonlinearEqualityConstraint.h b/gtsam/constraint/NonlinearEqualityConstraint.h new file mode 100644 index 000000000..6d3853030 --- /dev/null +++ b/gtsam/constraint/NonlinearEqualityConstraint.h @@ -0,0 +1,120 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @file EqualityConstraint.h + * @brief Equality constraints in constrained optimization. + * @author Yetong Zhang, Frank Dellaert + * @date Aug 3, 2024 */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + +/** + * Equality constraint base class. + */ +class NonlinearEqualityConstraint : public NonlinearConstraint { + public: + typedef NonlinearConstraint Base; + typedef NonlinearEqualityConstraint This; + typedef std::shared_ptr shared_ptr; + + /** Default constructor. */ + using Base::Base; + + /** Destructor. */ + virtual ~NonlinearEqualityConstraint() {} +}; + +/** Equality constraint that force g(x) = M. */ +template +class ExpressionEqualityConstraint : public NonlinearEqualityConstraint { + public: + typedef NonlinearEqualityConstraint Base; + typedef ExpressionEqualityConstraint This; + typedef std::shared_ptr shared_ptr; + + protected: + Expression expression_; + T rhs_; + FastVector dims_; + + public: + /** + * @brief Constructor. + * + * @param expression expression representing g(x). + * @param tolerance vector representing tolerance in each dimension. + */ + ExpressionEqualityConstraint(const Expression& expression, const T& rhs, const Vector& sigmas); + + virtual Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const override; + + virtual NoiseModelFactor::shared_ptr penaltyFactor(const double mu = 1.0) const override; + + const Expression& expression() const { return expression_; } +}; + +/** Equality constraint that enforce the cost factor with zero error. */ +class ZeroCostConstraint : public NonlinearEqualityConstraint { + public: + typedef NonlinearEqualityConstraint Base; + typedef ZeroCostConstraint This; + typedef std::shared_ptr shared_ptr; + + protected: + NoiseModelFactor::shared_ptr factor_; + + public: + /** + * @brief Constructor. + * + * @param factor NoiseModel factor. + * @param tolerance vector representing tolerance in each dimension. + */ + ZeroCostConstraint(const NoiseModelFactor::shared_ptr& factor); + + virtual Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const override; + + virtual NoiseModelFactor::shared_ptr penaltyFactor(const double mu = 1.0) const override; +}; + +/// Container of NonlinearEqualityConstraint. +class NonlinearEqualityConstraints : public FactorGraph { + public: + typedef std::shared_ptr shared_ptr; + typedef FactorGraph Base; + + public: + using Base::Base; + + /// Create constraints ensuring the cost of factors of a graph is zero. + static NonlinearEqualityConstraints FromCostGraph(const NonlinearFactorGraph& graph); + + size_t dim() const; + + /// Evaluate the constraint violation as a vector + Vector violationVector(const Values& values, bool whiten = true) const; + + /// Evaluate the constraint violation (as 2-norm of the violation vector). + double violationNorm(const Values& values) const; + + NonlinearFactorGraph penaltyGraph(const double mu = 1.0) const; +}; + +} // namespace gtsam + +#include diff --git a/gtsam/constraint/NonlinearInequalityConstraint.cpp b/gtsam/constraint/NonlinearInequalityConstraint.cpp new file mode 100644 index 000000000..f02fd8648 --- /dev/null +++ b/gtsam/constraint/NonlinearInequalityConstraint.cpp @@ -0,0 +1,158 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @file NonlinearInequalityConstraint.cpp + * @brief Nonlinear inequality constraints in constrained optimization. + * @author Yetong Zhang, Frank Dellaert + * @date Aug 3, 2024 + */ + +#include +#include +#include "gtsam/constraint/RampFunction.h" + +namespace gtsam { + +/* ************************************************************************* */ +Vector NonlinearInequalityConstraint::unwhitenedError(const Values& x, + OptionalMatrixVecType H) const { + Vector error = unwhitenedExpr(x, H); + for (size_t i = 0; i < dim(); i++) { + if (error(i) < 0) { + error(i) = 0; + if (H) { + for (Matrix& m : *H) { + m.row(i).setZero(); + } + } + } + } + return error; +} + +/* ************************************************************************* */ +bool NonlinearInequalityConstraint::active(const Values& x) const { + return (unwhitenedExpr(x).array() >= 0).any(); +} + +/* ************************************************************************* */ +NoiseModelFactor::shared_ptr NonlinearInequalityConstraint::penaltyFactorEquality( + const double mu) const { + return createEqualityConstraint()->penaltyFactor(mu); +} + +/* ************************************************************************* */ +ScalarExpressionInequalityConstraint::ScalarExpressionInequalityConstraint( + const Double_& expression, const double& sigma) + : Base(constrainedNoise(Vector1(sigma)), expression.keysAndDims().first), + expression_(expression), + dims_(expression.keysAndDims().second) {} + +/* ************************************************************************* */ +ScalarExpressionInequalityConstraint::shared_ptr ScalarExpressionInequalityConstraint::GeqZero( + const Double_& expression, const double& sigma) { + Double_ neg_expr = Double_(0.0) - expression; + return std::make_shared(neg_expr, sigma); +} + +/* ************************************************************************* */ +ScalarExpressionInequalityConstraint::shared_ptr ScalarExpressionInequalityConstraint::LeqZero( + const Double_& expression, const double& sigma) { + return std::make_shared(expression, sigma); +} + +/* ************************************************************************* */ +Vector ScalarExpressionInequalityConstraint::unwhitenedExpr(const Values& x, + OptionalMatrixVecType H) const { + // Copy-paste from ExpressionFactor. + if (H) { + return Vector1(expression_.valueAndDerivatives(x, keys_, dims_, *H)); + } else { + return Vector1(expression_.value(x)); + } +} + +/* ************************************************************************* */ +NonlinearEqualityConstraint::shared_ptr +ScalarExpressionInequalityConstraint::createEqualityConstraint() const { + return std::make_shared>( + expression_, 0.0, noiseModel()->sigmas()); +} + +/* ************************************************************************* */ +NoiseModelFactor::shared_ptr ScalarExpressionInequalityConstraint::penaltyFactor( + const double mu) const { + Double_ penalty_expression(RampFunction, expression_); + return std::make_shared>(penaltyNoise(mu), 0.0, penalty_expression); +} + +/* ************************************************************************* */ +NoiseModelFactor::shared_ptr ScalarExpressionInequalityConstraint::penaltyFactorSmooth( + SmoothRampFunction::shared_ptr func, const double mu) const { + // TODO(yetong): can we pass the functor directly to construct the expression? + Double_ error(func->function(), expression_); + return std::make_shared>(penaltyNoise(mu), 0.0, error); +} + +/* ************************************************************************* */ +NoiseModelFactor::shared_ptr ScalarExpressionInequalityConstraint::penaltyFactorEquality( + const double mu) const { + return std::make_shared>(penaltyNoise(mu), 0.0, expression_); +} + +/* ************************************************************************* */ +size_t NonlinearInequalityConstraints::dim() const { + size_t dimension = 0; + for (const auto& constraint : *this) { + dimension += constraint->dim(); + } + return dimension; +} + +/* ************************************************************************* */ +Vector NonlinearInequalityConstraints::violationVector(const Values& values, bool whiten) const { + Vector violation(dim()); + size_t start_idx = 0; + for (const auto& constraint : *this) { + size_t dim = constraint->dim(); + violation.middleCols(start_idx, dim) = + whiten ? constraint->whitenedError(values) : constraint->unwhitenedError(values); + start_idx += dim; + } + return violation; +} + +/* ************************************************************************* */ +double NonlinearInequalityConstraints::violationNorm(const Values& values) const { + return violationVector(values, true).norm(); +} + +/* ************************************************************************* */ +NonlinearFactorGraph NonlinearInequalityConstraints::penaltyGraph(const double mu) const { + NonlinearFactorGraph graph; + for (const auto& constraint : *this) { + graph.add(constraint->penaltyFactor(mu)); + } + return graph; +} + +/* ************************************************************************* */ +NonlinearFactorGraph NonlinearInequalityConstraints::penaltyGraphSmooth( + SmoothRampFunction::shared_ptr func, const double mu) const { + NonlinearFactorGraph graph; + for (const auto& constraint : *this) { + graph.add(constraint->penaltyFactorSmooth(func, mu)); + } + return graph; +} + +} // namespace gtsam diff --git a/gtsam/constraint/NonlinearInequalityConstraint.h b/gtsam/constraint/NonlinearInequalityConstraint.h new file mode 100644 index 000000000..7399b30e8 --- /dev/null +++ b/gtsam/constraint/NonlinearInequalityConstraint.h @@ -0,0 +1,125 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @file NonlinearInequalityConstraint.h + * @brief Nonlinear inequality constraints in constrained optimization. + * @author Yetong Zhang, Frank Dellaert + * @date Aug 3, 2024 + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + +/** + * Inequality constraint base class. + */ +class NonlinearInequalityConstraint : public NonlinearConstraint { + public: + typedef NonlinearConstraint Base; + typedef NonlinearInequalityConstraint This; + typedef std::shared_ptr shared_ptr; + + /** Default constructor. */ + using Base::Base; + + /** Destructor. */ + virtual ~NonlinearInequalityConstraint() {} + + virtual Vector unwhitenedExpr(const Values& x, OptionalMatrixVecType H = nullptr) const = 0; + + virtual Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const override; + + virtual bool active(const Values& x) const override; + + virtual NonlinearEqualityConstraint::shared_ptr createEqualityConstraint() const = 0; + + /** Smooth approximation of the ramp function. */ + virtual NoiseModelFactor::shared_ptr penaltyFactorSmooth(SmoothRampFunction::shared_ptr func, + const double mu = 1.0) const = 0; + + /** penalty function as if the constraint is equality, 0.5 * mu * ||g(x)||^2 */ + virtual NoiseModelFactor::shared_ptr penaltyFactorEquality(const double mu = 1.0) const; +}; + +/** Inequality constraint that force g(x) <= 0, where g(x) is a scalar-valued + * function. */ +class ScalarExpressionInequalityConstraint : public NonlinearInequalityConstraint { + public: + typedef NonlinearInequalityConstraint Base; + typedef ScalarExpressionInequalityConstraint This; + typedef std::shared_ptr shared_ptr; + + protected: + Double_ expression_; + FastVector dims_; + + public: + /** + * @brief Constructor. + * + * @param expression expression representing g(x). + * @param sigma scalar representing sigma. + */ + ScalarExpressionInequalityConstraint(const Double_& expression, const double& sigma); + + // Create an inequality constraint g(x)>=0. + static ScalarExpressionInequalityConstraint::shared_ptr GeqZero(const Double_& expression, + const double& sigma); + + // Create an inequality constraint g(x)<=0. + static ScalarExpressionInequalityConstraint::shared_ptr LeqZero(const Double_& expression, + const double& sigma); + + virtual Vector unwhitenedExpr(const Values& x, OptionalMatrixVecType H = nullptr) const override; + + NonlinearEqualityConstraint::shared_ptr createEqualityConstraint() const override; + + NoiseModelFactor::shared_ptr penaltyFactor(const double mu = 1.0) const override; + + NoiseModelFactor::shared_ptr penaltyFactorSmooth(SmoothRampFunction::shared_ptr func, + const double mu = 1.0) const override; + + virtual NoiseModelFactor::shared_ptr penaltyFactorEquality(const double mu = 1.0) const override; + + const Double_& expression() const { return expression_; } +}; + +/// Container of NonlinearInequalityConstraint. +class NonlinearInequalityConstraints : public FactorGraph { + public: + typedef FactorGraph Base; + typedef NonlinearInequalityConstraints This; + typedef std::shared_ptr shared_ptr; + + using Base::Base; + + /// Return the total dimension of constraints. + size_t dim() const; + + /// Evaluate the constraint violation as a vector + Vector violationVector(const Values& values, bool whiten = true) const; + + /// Evaluate the constraint violation (as L2 norm). + double violationNorm(const Values& values) const; + + NonlinearFactorGraph penaltyGraph(const double mu = 1.0) const; + + NonlinearFactorGraph penaltyGraphSmooth(SmoothRampFunction::shared_ptr func, + const double mu = 1.0) const; +}; + +} // namespace gtsam diff --git a/gtsam/constraint/RampFunction.cpp b/gtsam/constraint/RampFunction.cpp new file mode 100644 index 000000000..601416afd --- /dev/null +++ b/gtsam/constraint/RampFunction.cpp @@ -0,0 +1,71 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @file NonlinearInequalityConstraint.h + * @brief Nonlinear inequality constraints in constrained optimization. + * @author Yetong Zhang, Frank Dellaert + * @date Aug 3, 2024 + */ + +#include + +namespace gtsam { + +/* ************************************************************************* */ +double RampFunction(const double x, OptionalJacobian<1, 1> H) { + if (x < 0) { + if (H) { + H->setConstant(0); + } + return 0; + } else { + if (H) { + H->setConstant(1); + } + return x; + } +} + +/* ************************************************************************* */ +SmoothRampFunction::UnaryScalarFunc SmoothRampFunction::function() const { + return [=](const double& x, OptionalJacobian<1, 1> H = {}) -> double { return (*this)(x, H); }; +} + +/* ************************************************************************* */ +double PolynomialRampFunction::operator()(const double& x, OptionalJacobian<1, 1> H) const { + if (x <= 0) { + if (H) { + H->setZero(); + } + return 0; + } else if (x < epsilon_) { + if (H) { + H->setConstant(x / epsilon_); + } + return (x * x) / (2 * epsilon_) + epsilon_ / 2; + } else { + if (H) { + H->setOnes(); + } + return x; + } +} + +/* ************************************************************************* */ +double SoftPlusFunction::operator()(const double& x, OptionalJacobian<1, 1> H) const { + if (H) { + H->setConstant(1 / (1 + std::exp(-k_ * x))); + } + return std::log(1 + std::exp(k_ * x)) / k_; +} + +} // namespace gtsam diff --git a/gtsam/constraint/RampFunction.h b/gtsam/constraint/RampFunction.h new file mode 100644 index 000000000..d04e2dec0 --- /dev/null +++ b/gtsam/constraint/RampFunction.h @@ -0,0 +1,78 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @file NonlinearInequalityConstraint.h + * @brief Nonlinear inequality constraints in constrained optimization. + * @author Yetong Zhang, Frank Dellaert + * @date Aug 3, 2024 + */ + +#pragma once + +#include +#include + +namespace gtsam { + +/// Ramp function for create penalty factors. +double RampFunction(const double x, OptionalJacobian<1, 1> H = {}); + +/** Base class for smooth approximation of the ramp function. */ +class SmoothRampFunction { + public: + typedef std::shared_ptr shared_ptr; + typedef std::function H)> UnaryScalarFunc; + + /** Constructor. */ + SmoothRampFunction() {} + + /** Destructor. */ + virtual ~SmoothRampFunction() {} + + virtual double operator()(const double& x, OptionalJacobian<1, 1> H = {}) const = 0; + + UnaryScalarFunc function() const; +}; + +/** Ramp function approximated with a polynomial of degree 2. + * Function f(x) = 0 for x <= 0 + * x^2/(2*e) + e/2 for 0 < x < epsilon + * x for x >= epsilon + */ +class PolynomialRampFunction : public SmoothRampFunction { + public: + typedef SmoothRampFunction Base; + + protected: + double epsilon_; + + public: + PolynomialRampFunction(const double epsilon = 1) : Base(), epsilon_(epsilon) {} + + virtual double operator()(const double& x, OptionalJacobian<1, 1> H = {}) const override; +}; + +/** Softplus function that implements f(x) = log(1 + exp(k*x)) / k. */ +class SoftPlusFunction : public SmoothRampFunction { + public: + typedef SmoothRampFunction Base; + + protected: + double k_; + + public: + SoftPlusFunction(const double k = 1) : Base(), k_(k) {} + + virtual double operator()(const double& x, OptionalJacobian<1, 1> H = {}) const override; +}; + +} // namespace gtsam diff --git a/gtsam/constraint/tests/CMakeLists.txt b/gtsam/constraint/tests/CMakeLists.txt new file mode 100644 index 000000000..4f364508c --- /dev/null +++ b/gtsam/constraint/tests/CMakeLists.txt @@ -0,0 +1,9 @@ +# if GTSAM_ENABLE_BOOST_SERIALIZATION is OFF then exclude some tests +if (NOT GTSAM_ENABLE_BOOST_SERIALIZATION) + # create a semicolon seperated list of files to exclude + set(EXCLUDE_TESTS "testSerializationConstraint.cpp") +else() + set(EXCLUDE_TESTS "") +endif() + +gtsamAddTestsGlob(constraint "test*.cpp" "${EXCLUDE_TESTS}" "gtsam") diff --git a/gtsam/constraint/tests/constrainedExample.h b/gtsam/constraint/tests/constrainedExample.h new file mode 100644 index 000000000..a8aafb981 --- /dev/null +++ b/gtsam/constraint/tests/constrainedExample.h @@ -0,0 +1,168 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @file constrainedExample.h + * @brief Simple constrained optimization scenarios. + * @author Yetong Zhang + * @date Aug 3, 2024 + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +namespace constrained_example { + +using namespace gtsam; + +/// Exponential function e^x. +double exp_func(const double& x, gtsam::OptionalJacobian<1, 1> H1 = {}) { + double result = exp(x); + if (H1) H1->setConstant(result); + return result; +} + +/// Exponential expression e^x. +Double_ exp(const Double_& x) { return Double_(exp_func, x); } + +/// Pow functor used for pow function. +class PowFunctor { + private: + double c_; + + public: + PowFunctor(const double& c) : c_(c) {} + + double operator()(const double& x, + gtsam::OptionalJacobian<1, 1> H1 = {}) const { + if (H1) H1->setConstant(c_ * pow(x, c_ - 1)); + return pow(x, c_); + } +}; + +/// Pow function. +Double_ pow(const Double_& x, const double& c) { + PowFunctor pow_functor(c); + return Double_(pow_functor, x); +} + +/// Plus between Double expression and double. +Double_ operator+(const Double_& x, const double& d) { return x + Double_(d); } + +/// Negative sign operator. +Double_ operator-(const Double_& x) { return Double_(0.0) - x; } + +/// Keys for creating expressions. +Symbol x1_key('x', 1); +Symbol x2_key('x', 2); +Double_ x1(x1_key), x2(x2_key); + +} // namespace constrained_example + +/* ************************************************************************* */ +/** + * Constrained optimization example in L. Vandenberghe slides: + * https://www.seas.ucla.edu/~vandenbe/133B/lectures/nllseq.pdf + * f(x) = 0.5*||x1 + e^(-x2)||^2 + 0.5*||x1^2 + 2*x2 + 1||^2 + * h(x) = x1 + x1^3 + x2 + x2^2 + */ +namespace e_constrained_example { +using namespace constrained_example; +NonlinearFactorGraph GetCost() { + NonlinearFactorGraph graph; + auto f1 = x1 + exp(-x2); + auto f2 = pow(x1, 2.0) + 2.0 * x2 + 1.0; + auto cost_noise = gtsam::noiseModel::Isotropic::Sigma(1, 1.0); + graph.add(ExpressionFactor(cost_noise, 0., f1)); + graph.add(ExpressionFactor(cost_noise, 0., f2)); + return graph; +} + +NonlinearEqualityConstraints GetConstraints() { + NonlinearEqualityConstraints constraints; + Vector scale = Vector1(0.1); + auto h1 = x1 + pow(x1, 3) + x2 + pow(x2, 2); + constraints.push_back(NonlinearEqualityConstraint::shared_ptr( + new ExpressionEqualityConstraint(h1, 0.0, scale))); + return constraints; +} + +NonlinearFactorGraph cost = GetCost(); +NonlinearEqualityConstraints constraints = GetConstraints(); +} // namespace e_constrained_example + +/* ************************************************************************* */ +/** + * Constrained optimization example with inequality constraints + * f(x) = 0.5 * ||x1-1||^2 + 0.5 * ||x2-1||^2 + * g(x) = 1 - x1^2 - x2^2 + */ +namespace i_constrained_example { +using namespace constrained_example; +NonlinearFactorGraph GetCost() { + NonlinearFactorGraph graph; + auto cost_noise = gtsam::noiseModel::Isotropic::Sigma(1, 1.0); + graph.addPrior(x1_key, 1.0, cost_noise); + graph.addPrior(x2_key, 1.0, cost_noise); + return graph; +} + +// InequalityConstraints GetIConstraints() { +// InequalityConstraints i_constraints; +// Double_ g1 = Double_(1.0) - x1 * x1 - x2 * x2; +// double tolerance = 0.2; +// i_constraints.emplace_shared(g1, tolerance); +// return i_constraints; +// } + +NonlinearFactorGraph cost = GetCost(); +NonlinearEqualityConstraints e_constraints; +// InequalityConstraints i_constraints = GetIConstraints(); +} // namespace i_constrained_example + +/* ************************************************************************* */ +/** + * Constrained optimization example with inequality constraints + * f(x) = 0.5 * ||x1||^2 + 0.5 * ||x2||^2 + * h(x) = x1 + x2 - 1 + */ +namespace e_constrained_example2 { +using namespace constrained_example; +NonlinearFactorGraph GetCost() { + NonlinearFactorGraph graph; + auto cost_noise = gtsam::noiseModel::Isotropic::Sigma(1, 1.0); + graph.addPrior(x1_key, 1.0, cost_noise); + // graph.addPrior(x2_key, 0.0, cost_noise); + // graph.emplace_shared>(x1_key, x2_key, 1.0, cost_noise); + return graph; +} + +NonlinearEqualityConstraints GetEConstraints() { + NonlinearEqualityConstraints e_constraints; + // Double_ h1 = x1 + x2 - Double_(1.0); + // double tolerance = 0.2; + // e_constraints.emplace_shared(h1, tolerance); + return e_constraints; +} + +NonlinearFactorGraph cost = GetCost(); +NonlinearEqualityConstraints e_constraints = GetEConstraints(); +} // namespace i_constrained_example diff --git a/gtsam/constraint/tests/testNonlinearEqualityConstraint.cpp b/gtsam/constraint/tests/testNonlinearEqualityConstraint.cpp new file mode 100644 index 000000000..ca2725d4c --- /dev/null +++ b/gtsam/constraint/tests/testNonlinearEqualityConstraint.cpp @@ -0,0 +1,267 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @file testEqualityConstraint.cpp + * @brief Equality constraints in constrained optimization. + * @author Yetong Zhang + * @date Aug 3, 2024 + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "constrainedExample.h" + +using namespace gtsam; +using constrained_example::pow; +using constrained_example::x1, constrained_example::x2; +using constrained_example::x1_key, constrained_example::x2_key; + +// Test methods of DoubleExpressionEquality. +TEST(ExpressionEqualityConstraint, double) { + // create constraint from double expression + // g(x1, x2) = x1 + x1^3 + x2 + x2^2, from Vanderbergh slides + Vector sigmas = Vector1(0.1); + auto g = x1 + pow(x1, 3) + x2 + pow(x2, 2); + auto constraint = ExpressionEqualityConstraint(g, 0.0, sigmas); + + EXPECT(constraint.noiseModel()->isConstrained()); + EXPECT(assert_equal(sigmas, constraint.noiseModel()->sigmas())); + + // Create 2 sets of values for testing. + Values values1, values2; + values1.insert(x1_key, 0.0); + values1.insert(x2_key, 0.0); + values2.insert(x1_key, 1.0); + values2.insert(x2_key, 1.0); + + // Check that values1 are feasible. + EXPECT(constraint.feasible(values1)); + + // Check that violation evaluates as 0 at values1. + EXPECT(assert_equal(Vector::Zero(1), constraint.unwhitenedError(values1))); + EXPECT(assert_equal(Vector::Zero(1), constraint.whitenedError(values1))); + EXPECT(assert_equal(0.0, constraint.error(values1))); + + // Check that values2 are indeed deemed infeasible. + EXPECT(!constraint.feasible(values2)); + + // Check constraint violation is indeed g(x) at values2. + EXPECT(assert_equal(Vector::Constant(1, 4.0), constraint.unwhitenedError(values2))); + EXPECT(assert_equal(Vector::Constant(1, 40), constraint.whitenedError(values2))); + EXPECT(assert_equal(800, constraint.error(values2))); + + // Check dimension is 1 for scalar g. + EXPECT(constraint.dim() == 1); + + // Check keys include x1, x2. + EXPECT(constraint.keys().size() == 2); + EXPECT(x1_key == *constraint.keys().begin()); + EXPECT(x2_key == *constraint.keys().rbegin()); + + // Generate factor representing the term in merit function. + double mu = 4; + auto merit_factor = constraint.penaltyFactor(mu); + + // Check that noise model sigma == sigmas/sqrt(mu). + auto expected_noise = noiseModel::Diagonal::Sigmas(sigmas / sqrt(mu)); + EXPECT(expected_noise->equals(*merit_factor->noiseModel())); + + // Check that error is equal to 0.5*mu * (g(x)+bias)^2/sigmas^2. + double expected_error1 = 0; // 0.5 * 4 * ||0||_(0.1^2)^2 + EXPECT(assert_equal(expected_error1, merit_factor->error(values1))); + double expected_error2 = 3200; // 0.5 * 4 * ||4||_(0.1^2)^2 + EXPECT(assert_equal(expected_error2, merit_factor->error(values2))); + + // Check jacobian computation is correct. + EXPECT_CORRECT_FACTOR_JACOBIANS(*merit_factor, values1, 1e-7, 1e-5); + EXPECT_CORRECT_FACTOR_JACOBIANS(*merit_factor, values2, 1e-7, 1e-5); +} + +// Test methods of VectorExpressionEquality. +TEST(ExpressionEqualityConstraint, Vector2) { + // g(v1, v2) = v1 + v2, our own example. + Vector2_ x1_vec_expr(x1_key); + Vector2_ x2_vec_expr(x2_key); + auto g = x1_vec_expr + x2_vec_expr; + auto sigmas = Vector2(0.1, 0.5); + auto constraint = ExpressionEqualityConstraint(g, Vector2::Zero(), sigmas); + + EXPECT(constraint.noiseModel()->isConstrained()); + EXPECT(assert_equal(sigmas, constraint.noiseModel()->sigmas())); + + // Create 2 sets of values for testing. + Values values1, values2; + values1.insert(x1_key, Vector2(1, 1)); + values1.insert(x2_key, Vector2(-1, -1)); + values2.insert(x1_key, Vector2(1, 1)); + values2.insert(x2_key, Vector2(1, 1)); + + // Check that values1 are feasible. + EXPECT(constraint.feasible(values1)); + + // Check that violation evaluates as 0 at values1. + auto expected_violation1 = (Vector(2) << 0, 0).finished(); + EXPECT(assert_equal(expected_violation1, constraint.unwhitenedError(values1))); + auto expected_scaled_violation1 = (Vector(2) << 0, 0).finished(); + EXPECT(assert_equal(expected_scaled_violation1, constraint.whitenedError(values1))); + + // Check that values2 are indeed deemed infeasible. + EXPECT(!constraint.feasible(values2)); + + // Check constraint violation is indeed g(x) at values2. + auto expected_violation2 = (Vector(2) << 2, 2).finished(); + EXPECT(assert_equal(expected_violation2, constraint.unwhitenedError(values2))); + + // Check scaled violation is indeed g(x)/sigmas at values2. + auto expected_scaled_violation2 = (Vector(2) << 20, 4).finished(); + EXPECT(assert_equal(expected_scaled_violation2, constraint.whitenedError(values2))); + + // Check dim is the dimension of the vector. + EXPECT(constraint.dim() == 2); + + // Generate factor representing the term in merit function. + double mu = 4; + auto merit_factor = constraint.penaltyFactor(mu); + + // Check that noise model sigma == sigmas/sqrt(mu). + auto expected_noise = noiseModel::Diagonal::Sigmas(sigmas / sqrt(mu)); + EXPECT(expected_noise->equals(*merit_factor->noiseModel())); + + // Check that error is equal to 0.5*mu*||g(x)+bias)||^2_Diag(sigmas^2). + double expected_error1 = 0; // 0.5 * 4 * ||[1, 0.5]||_([0.1,0.5]^2)^2 + EXPECT(assert_equal(expected_error1, merit_factor->error(values1))); + double expected_error2 = 832; // 0.5 * 4 * ||[2, 2]||_([0.1,0.5]^2)^2 + EXPECT(assert_equal(expected_error2, merit_factor->error(values2))); + + // Check jacobian computation is correct. + EXPECT_CORRECT_FACTOR_JACOBIANS(*merit_factor, values1, 1e-7, 1e-5); + EXPECT_CORRECT_FACTOR_JACOBIANS(*merit_factor, values2, 1e-7, 1e-5); +} + + +// Test methods of FactorZeroErrorConstraint. +TEST(ZeroCostConstraint, BetweenFactor) { + Key x1_key = 1; + Key x2_key = 2; + Vector sigmas = Vector2(0.5, 0.1); + auto noise = noiseModel::Diagonal::Sigmas(sigmas); + + auto factor = std::make_shared>(x1_key, x2_key, Vector2(1, 1), noise); + auto constraint = ZeroCostConstraint(factor); + + EXPECT(constraint.noiseModel()->isConstrained()); + EXPECT(assert_equal(sigmas, constraint.noiseModel()->sigmas())); + + // Create 2 sets of values for testing. + Values values1, values2; + values1.insert(x1_key, Vector2(1, 1)); + values1.insert(x2_key, Vector2(2, 2)); + values2.insert(x1_key, Vector2(0, 0)); + values2.insert(x2_key, Vector2(2, 3)); + + // Check that values1 are feasible. + EXPECT(constraint.feasible(values1)); + + // Check that violation evaluates as 0 at values1. + auto expected_violation1 = (Vector(2) << 0, 0).finished(); + EXPECT(assert_equal(expected_violation1, constraint.unwhitenedError(values1))); + auto expected_scaled_violation1 = (Vector(2) << 0, 0).finished(); + EXPECT(assert_equal(expected_scaled_violation1, constraint.whitenedError(values1))); + + // Check that values2 are indeed deemed infeasible. + EXPECT(!constraint.feasible(values2)); + + // Check constraint violation is indeed g(x) at values2. + auto expected_violation2 = (Vector(2) << 1, 2).finished(); + EXPECT(assert_equal(expected_violation2, constraint.unwhitenedError(values2))); + + // Check scaled violation is indeed g(x)/sigmas at values2. + auto expected_scaled_violation2 = (Vector(2) << 2, 20).finished(); + EXPECT(assert_equal(expected_scaled_violation2, constraint.whitenedError(values2))); + + // Check dim is the dimension of the vector. + EXPECT(constraint.dim() == 2); + + // Generate factor representing the term in merit function. + double mu = 4; + auto merit_factor = constraint.penaltyFactor(mu); + + // Check that noise model sigma == sigmas/sqrt(mu). + auto expected_noise = noiseModel::Diagonal::Sigmas(sigmas / sqrt(mu)); + EXPECT(expected_noise->equals(*merit_factor->noiseModel())); + + // Check that error is equal to 0.5*mu*||g(x)+bias)||^2_Diag(sigmas^2). + double expected_error1 = 0; // 0.5 * 4 * ||[0, 0]||_([0.5,0.1]^2)^2 + EXPECT(assert_equal(expected_error1, merit_factor->error(values1))); + double expected_error2 = 808; // 0.5 * 4 * ||[1, 2]||_([0.5,0.1]^2)^2 + EXPECT(assert_equal(expected_error2, merit_factor->error(values2))); + + // Check jacobian computation is correct. + EXPECT_CORRECT_FACTOR_JACOBIANS(*merit_factor, values1, 1e-7, 1e-5); + EXPECT_CORRECT_FACTOR_JACOBIANS(*merit_factor, values2, 1e-7, 1e-5); +} + + + + +TEST(NonlinearEqualityConstraints, Container) { + NonlinearEqualityConstraints constraints; + + Vector sigmas1 = Vector1(10); + auto g1 = x1 + pow(x1, 3) + x2 + pow(x2, 2); + Vector2_ x1_vec_expr(x1_key); + Vector2_ x2_vec_expr(x2_key); + auto g2 = x1_vec_expr + x2_vec_expr; + Vector sigmas2 = Vector2(0.1, 0.5); + + constraints.emplace_shared>(g1, 0.0, sigmas1); + constraints.emplace_shared>(g2, Vector2::Zero(), sigmas2); + + // Check size. + EXPECT_LONGS_EQUAL(2, constraints.size()); + + // Check dimension. + EXPECT_LONGS_EQUAL(3, constraints.dim()); + + // Check keys. + KeySet expected_keys; + expected_keys.insert(x1_key); + expected_keys.insert(x2_key); + EXPECT(assert_container_equality(expected_keys, constraints.keys())); + + // Check VariableIndex. + VariableIndex vi(constraints); + FactorIndices expected_vi_x1{0, 1}; + FactorIndices expected_vi_x2{0, 1}; + EXPECT(assert_container_equality(expected_vi_x1, vi[x1_key])); + EXPECT(assert_container_equality(expected_vi_x2, vi[x2_key])); + + // Check constraint violation. + +} + + +TEST(NonlinearEqualityConstraints, FromCostGraph) { +} + +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} diff --git a/gtsam/constraint/tests/testNonlinearInequalityConstraint.cpp b/gtsam/constraint/tests/testNonlinearInequalityConstraint.cpp new file mode 100644 index 000000000..a73367d68 --- /dev/null +++ b/gtsam/constraint/tests/testNonlinearInequalityConstraint.cpp @@ -0,0 +1,117 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @file NonlinearInequalityConstraint.h + * @brief Nonlinear inequality constraints in constrained optimization. + * @author Yetong Zhang + * @date Aug 3, 2024 + */ + +#include +#include +#include +#include +#include +#include + +#include "constrainedExample.h" + +using namespace gtsam; +using constrained_example::pow; +using constrained_example::x1, constrained_example::x2; +using constrained_example::x1_key, constrained_example::x2_key; + +// Test methods of DoubleExpressionEquality. +TEST(NonlinearInequalityConstraint, ScalarExpressionInequalityConstraint) { + // create constraint from double expression + // g(x1, x2) = x1 + x1^3 + x2 + x2^2, from Vanderbergh slides + double sigma = 0.1; + auto g = x1 + pow(x1, 3) + x2 + pow(x2, 2); + auto constraint_geq = ScalarExpressionInequalityConstraint::GeqZero(g, sigma); + auto constraint_leq = ScalarExpressionInequalityConstraint::LeqZero(g, sigma); + + // Check dimension is 1 for scalar g. + EXPECT_LONGS_EQUAL(1, constraint_geq->dim()); + EXPECT_LONGS_EQUAL(1, constraint_leq->dim()); + + // Check keys include x1, x2. + KeyVector expected_keys{x1_key, x2_key}; + EXPECT(assert_container_equality(expected_keys, constraint_leq->keys())); + + // Create 3 sets of values for testing. + Values values1, values2, values3; + values1.insert(x1_key, -1.0); + values1.insert(x2_key, 1.0); + values2.insert(x1_key, 1.0); + values2.insert(x2_key, 1.0); + values3.insert(x1_key, -2.0); + values3.insert(x2_key, 1.0); + + // Check that expression evaluation at different values. + EXPECT(assert_equal(Vector1(0.0), constraint_leq->unwhitenedExpr(values1))); + EXPECT(assert_equal(Vector1(4.0), constraint_leq->unwhitenedExpr(values2))); + EXPECT(assert_equal(Vector1(-8.0), constraint_leq->unwhitenedExpr(values3))); + EXPECT(assert_equal(Vector1(0.0), constraint_geq->unwhitenedExpr(values1))); + EXPECT(assert_equal(Vector1(-4.0), constraint_geq->unwhitenedExpr(values2))); + EXPECT(assert_equal(Vector1(8.0), constraint_geq->unwhitenedExpr(values3))); + + // Check that constraint violation at different values. + EXPECT(assert_equal(Vector1(0.0), constraint_leq->unwhitenedError(values1))); + EXPECT(assert_equal(Vector1(4.0), constraint_leq->unwhitenedError(values2))); + EXPECT(assert_equal(Vector1(0.0), constraint_leq->unwhitenedError(values3))); + EXPECT(assert_equal(Vector1(0.0), constraint_geq->unwhitenedError(values1))); + EXPECT(assert_equal(Vector1(0.0), constraint_geq->unwhitenedError(values2))); + EXPECT(assert_equal(Vector1(8.0), constraint_geq->unwhitenedError(values3))); + + // Check feasible. + EXPECT(constraint_leq->feasible(values1)); + EXPECT(!constraint_leq->feasible(values2)); + EXPECT(constraint_leq->feasible(values3)); + EXPECT(constraint_geq->feasible(values1)); + EXPECT(constraint_geq->feasible(values2)); + EXPECT(!constraint_geq->feasible(values3)); + + // Check active. + EXPECT(constraint_leq->active(values1)); + EXPECT(constraint_leq->active(values2)); + EXPECT(!constraint_leq->active(values3)); + EXPECT(constraint_geq->active(values1)); + EXPECT(!constraint_geq->active(values2)); + EXPECT(constraint_geq->active(values3)); + + // Check whitenedError of penalty factor. + // Expected to be sqrt(mu) / sigma * ramp(g(x)) + double mu = 9.0; + auto penalty_leq = constraint_leq->penaltyFactor(mu); + auto penalty_geq = constraint_geq->penaltyFactor(mu); + EXPECT(assert_equal(Vector1(0.0), penalty_leq->whitenedError(values1))); + EXPECT(assert_equal(Vector1(120.0), penalty_leq->whitenedError(values2))); + EXPECT(assert_equal(Vector1(0.0), penalty_leq->whitenedError(values3))); + EXPECT(assert_equal(Vector1(0.0), penalty_geq->whitenedError(values1))); + EXPECT(assert_equal(Vector1(0.0), penalty_geq->whitenedError(values2))); + EXPECT(assert_equal(Vector1(240.0), penalty_geq->whitenedError(values3))); + + // Check create equality constraint + auto constraint_eq1 = constraint_leq->createEqualityConstraint(); + auto constraint_eq2 = constraint_geq->createEqualityConstraint(); + EXPECT(assert_equal(0.0, constraint_eq1->error(values1))); + EXPECT(assert_equal(800.0, constraint_eq1->error(values2))); + EXPECT(assert_equal(3200.0, constraint_eq1->error(values3))); + EXPECT(assert_equal(0.0, constraint_eq2->error(values1))); + EXPECT(assert_equal(800.0, constraint_eq2->error(values2))); + EXPECT(assert_equal(3200.0, constraint_eq2->error(values3))); +} + +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} diff --git a/gtsam/constraint/tests/testSimple.cpp b/gtsam/constraint/tests/testSimple.cpp new file mode 100644 index 000000000..800f9ec6c --- /dev/null +++ b/gtsam/constraint/tests/testSimple.cpp @@ -0,0 +1,37 @@ +#include + +#include +#include +#include +#include +#include + +#include +#include + +// Define a functor class +class MyFunctor { +public: + // Overload the function call operator + void operator()(int x) const { + std::cout << "MyFunctor called with " << x << std::endl; + } +}; + + +TEST(InequalityConstraint, DoubleExpressionInequality) { + // Create an instance of the functor + MyFunctor functor; + + // Cast the functor to std::function + std::function func = functor; + + // Call the std::function object + func(42); +} + + +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} diff --git a/gtsam/nonlinear/Expression.h b/gtsam/nonlinear/Expression.h index e5017390d..aa65384c2 100644 --- a/gtsam/nonlinear/Expression.h +++ b/gtsam/nonlinear/Expression.h @@ -34,6 +34,8 @@ namespace gtsam { // Forward declares class Values; template class ExpressionFactor; +template class ExpressionEqualityConstraint; +class ScalarExpressionInequalityConstraint; namespace internal { template class ExecutionTrace; @@ -206,6 +208,8 @@ protected: // be very selective on who can access these private methods: friend class ExpressionFactor ; friend class internal::ExpressionNode; + friend class ExpressionEqualityConstraint; + friend class ScalarExpressionInequalityConstraint; // and add tests friend class ::ExpressionFactorShallowTest; From ba50bd7ad170fdef6c0737d9f858cade25a55dce Mon Sep 17 00:00:00 2001 From: yetongumich Date: Tue, 6 Aug 2024 10:41:18 -0400 Subject: [PATCH 02/52] add serialization --- gtsam/constraint/NonlinearConstraint.h | 19 ++++++- .../constraint/NonlinearEqualityConstraint.h | 51 +++++++++++++++++++ .../NonlinearInequalityConstraint.h | 35 +++++++++++++ 3 files changed, 103 insertions(+), 2 deletions(-) diff --git a/gtsam/constraint/NonlinearConstraint.h b/gtsam/constraint/NonlinearConstraint.h index d0e7aef6b..00bf4d52b 100644 --- a/gtsam/constraint/NonlinearConstraint.h +++ b/gtsam/constraint/NonlinearConstraint.h @@ -19,7 +19,9 @@ #pragma once #include -#include "gtsam/linear/NoiseModel.h" +#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#include +#endif namespace gtsam { @@ -40,7 +42,9 @@ class NonlinearConstraint : public NoiseModelFactor { virtual ~NonlinearConstraint() {} /** Create a cost factor representing the L2 penalty function with scaling coefficient mu. */ - virtual NoiseModelFactor::shared_ptr penaltyFactor(const double mu = 1.0) const = 0; + virtual NoiseModelFactor::shared_ptr penaltyFactor(const double mu = 1.0) const { + return cloneWithNewNoiseModel(penaltyNoise(mu)); + } /** Return the norm of the constraint violation vector. */ virtual double violation(const Values& x) const { return sqrt(2 * error(x)); } @@ -60,6 +64,17 @@ class NonlinearConstraint : public NoiseModelFactor { static SharedNoiseModel constrainedNoise(const Vector& sigmas) { return noiseModel::Constrained::MixedSigmas(1.0, sigmas); } + + private: +#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp("NonlinearConstraint", + boost::serialization::base_object(*this)); + } +#endif }; } // namespace gtsam diff --git a/gtsam/constraint/NonlinearEqualityConstraint.h b/gtsam/constraint/NonlinearEqualityConstraint.h index 6d3853030..9f2305fa0 100644 --- a/gtsam/constraint/NonlinearEqualityConstraint.h +++ b/gtsam/constraint/NonlinearEqualityConstraint.h @@ -20,6 +20,9 @@ #include #include #include +#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#include +#endif namespace gtsam { @@ -37,6 +40,17 @@ class NonlinearEqualityConstraint : public NonlinearConstraint { /** Destructor. */ virtual ~NonlinearEqualityConstraint() {} + + private: +#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp("NonlinearEqualityConstraint", + boost::serialization::base_object(*this)); + } +#endif }; /** Equality constraint that force g(x) = M. */ @@ -66,6 +80,20 @@ class ExpressionEqualityConstraint : public NonlinearEqualityConstraint { virtual NoiseModelFactor::shared_ptr penaltyFactor(const double mu = 1.0) const override; const Expression& expression() const { return expression_; } + + private: +#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp("ExpressionEqualityConstraint", + boost::serialization::base_object(*this)); + ar& BOOST_SERIALIZATION_NVP(expression_); + ar& BOOST_SERIALIZATION_NVP(rhs_); + ar& BOOST_SERIALIZATION_NVP(dims_); + } +#endif }; /** Equality constraint that enforce the cost factor with zero error. */ @@ -90,6 +118,18 @@ class ZeroCostConstraint : public NonlinearEqualityConstraint { virtual Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const override; virtual NoiseModelFactor::shared_ptr penaltyFactor(const double mu = 1.0) const override; + + private: +#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp("ZeroCostConstraint", + boost::serialization::base_object(*this)); + ar& BOOST_SERIALIZATION_NVP(factor_); + } +#endif }; /// Container of NonlinearEqualityConstraint. @@ -113,6 +153,17 @@ class NonlinearEqualityConstraints : public FactorGraph + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp("NonlinearEqualityConstraints", + boost::serialization::base_object(*this)); + } +#endif }; } // namespace gtsam diff --git a/gtsam/constraint/NonlinearInequalityConstraint.h b/gtsam/constraint/NonlinearInequalityConstraint.h index 7399b30e8..ac22df943 100644 --- a/gtsam/constraint/NonlinearInequalityConstraint.h +++ b/gtsam/constraint/NonlinearInequalityConstraint.h @@ -53,6 +53,17 @@ class NonlinearInequalityConstraint : public NonlinearConstraint { /** penalty function as if the constraint is equality, 0.5 * mu * ||g(x)||^2 */ virtual NoiseModelFactor::shared_ptr penaltyFactorEquality(const double mu = 1.0) const; + + private: +#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp("NonlinearInequalityConstraint", + boost::serialization::base_object(*this)); + } +#endif }; /** Inequality constraint that force g(x) <= 0, where g(x) is a scalar-valued @@ -96,6 +107,19 @@ class ScalarExpressionInequalityConstraint : public NonlinearInequalityConstrain virtual NoiseModelFactor::shared_ptr penaltyFactorEquality(const double mu = 1.0) const override; const Double_& expression() const { return expression_; } + + private: +#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp("ExpressionEqualityConstraint", + boost::serialization::base_object(*this)); + ar& BOOST_SERIALIZATION_NVP(expression_); + ar& BOOST_SERIALIZATION_NVP(dims_); + } +#endif }; /// Container of NonlinearInequalityConstraint. @@ -120,6 +144,17 @@ class NonlinearInequalityConstraints : public FactorGraph + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp("NonlinearInequalityConstraints", + boost::serialization::base_object(*this)); + } +#endif }; } // namespace gtsam From 80fcff1ed5af30fcbeeee902c47f0ef4bed1512f Mon Sep 17 00:00:00 2001 From: yetongumich Date: Tue, 6 Aug 2024 10:42:38 -0400 Subject: [PATCH 03/52] remove accidentally added file --- gtsam/constraint/tests/testSimple.cpp | 37 --------------------------- 1 file changed, 37 deletions(-) delete mode 100644 gtsam/constraint/tests/testSimple.cpp diff --git a/gtsam/constraint/tests/testSimple.cpp b/gtsam/constraint/tests/testSimple.cpp deleted file mode 100644 index 800f9ec6c..000000000 --- a/gtsam/constraint/tests/testSimple.cpp +++ /dev/null @@ -1,37 +0,0 @@ -#include - -#include -#include -#include -#include -#include - -#include -#include - -// Define a functor class -class MyFunctor { -public: - // Overload the function call operator - void operator()(int x) const { - std::cout << "MyFunctor called with " << x << std::endl; - } -}; - - -TEST(InequalityConstraint, DoubleExpressionInequality) { - // Create an instance of the functor - MyFunctor functor; - - // Cast the functor to std::function - std::function func = functor; - - // Call the std::function object - func(42); -} - - -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} From 9b0fa1210e9cadb2d06f3dffc30b2712dd3aed1c Mon Sep 17 00:00:00 2001 From: yetongumich Date: Tue, 6 Aug 2024 15:42:09 -0400 Subject: [PATCH 04/52] clean up and refactor NonlinearEquality and BoundingConstraint --- gtsam/constraint/NonlinearConstraint.h | 2 +- .../NonlinearEqualityConstraint-inl.h | 4 +- .../NonlinearEqualityConstraint.cpp | 4 +- .../constraint/NonlinearEqualityConstraint.h | 19 ++- .../NonlinearInequalityConstraint.cpp | 17 ++- .../NonlinearInequalityConstraint.h | 40 ++++-- gtsam/constraint/RampFunction.cpp | 32 ++++- gtsam/constraint/RampFunction.h | 57 ++++++-- .../tests/testNonlinearEqualityConstraint.cpp | 4 +- .../testNonlinearInequalityConstraint.cpp | 4 +- gtsam/constraint/tests/testRampFunction.cpp | 124 ++++++++++++++++++ gtsam/nonlinear/NonlinearEquality.h | 74 +++++++---- gtsam/slam/BoundingConstraint.h | 65 +++++---- tests/testBoundingConstraint.cpp | 14 +- 14 files changed, 363 insertions(+), 97 deletions(-) create mode 100644 gtsam/constraint/tests/testRampFunction.cpp diff --git a/gtsam/constraint/NonlinearConstraint.h b/gtsam/constraint/NonlinearConstraint.h index 00bf4d52b..834ef7991 100644 --- a/gtsam/constraint/NonlinearConstraint.h +++ b/gtsam/constraint/NonlinearConstraint.h @@ -11,7 +11,7 @@ /** * @file NonlinearConstraint.h - * @brief Equality constraints in constrained optimization. + * @brief Nonlinear constraints in constrained optimization. * @author Yetong Zhang, Frank Dellaert * @date Aug 3, 2024 */ diff --git a/gtsam/constraint/NonlinearEqualityConstraint-inl.h b/gtsam/constraint/NonlinearEqualityConstraint-inl.h index a594d0b09..625f1e366 100644 --- a/gtsam/constraint/NonlinearEqualityConstraint-inl.h +++ b/gtsam/constraint/NonlinearEqualityConstraint-inl.h @@ -10,8 +10,8 @@ * -------------------------------------------------------------------------- */ /** - * @file EqualityConstraint-inl.h - * @brief Equality constraints in constrained optimization. + * @file NonlinearEqualityConstraint-inl.h + * @brief Nonlinear equality constraints in constrained optimization. * @author Yetong Zhang, Frank Dellaert * @date Aug 3, 2024 */ diff --git a/gtsam/constraint/NonlinearEqualityConstraint.cpp b/gtsam/constraint/NonlinearEqualityConstraint.cpp index 06201dc2a..0c5b7ea11 100644 --- a/gtsam/constraint/NonlinearEqualityConstraint.cpp +++ b/gtsam/constraint/NonlinearEqualityConstraint.cpp @@ -10,8 +10,8 @@ * -------------------------------------------------------------------------- */ /** - * @file EqualityConstraint.cpp - * @brief Equality constraints in constrained optimization. + * @file NonlinearEqualityConstraint.cpp + * @brief Nonlinear equality constraints in constrained optimization. * @author Yetong Zhang, Frank Dellaert * @date Aug 3, 2024 */ diff --git a/gtsam/constraint/NonlinearEqualityConstraint.h b/gtsam/constraint/NonlinearEqualityConstraint.h index 9f2305fa0..ab7dca2f0 100644 --- a/gtsam/constraint/NonlinearEqualityConstraint.h +++ b/gtsam/constraint/NonlinearEqualityConstraint.h @@ -10,8 +10,8 @@ * -------------------------------------------------------------------------- */ /** - * @file EqualityConstraint.h - * @brief Equality constraints in constrained optimization. + * @file NonlinearEqualityConstraint.h + * @brief Nonlinear equality constraints in constrained optimization. * @author Yetong Zhang, Frank Dellaert * @date Aug 3, 2024 */ @@ -20,9 +20,6 @@ #include #include #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION -#include -#endif namespace gtsam { @@ -81,6 +78,12 @@ class ExpressionEqualityConstraint : public NonlinearEqualityConstraint { const Expression& expression() const { return expression_; } + /// @return a deep copy of this factor + gtsam::NonlinearFactor::shared_ptr clone() const override { + return std::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + private: #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ @@ -119,6 +122,12 @@ class ZeroCostConstraint : public NonlinearEqualityConstraint { virtual NoiseModelFactor::shared_ptr penaltyFactor(const double mu = 1.0) const override; + /// @return a deep copy of this factor + gtsam::NonlinearFactor::shared_ptr clone() const override { + return std::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + private: #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ diff --git a/gtsam/constraint/NonlinearInequalityConstraint.cpp b/gtsam/constraint/NonlinearInequalityConstraint.cpp index f02fd8648..49003f09f 100644 --- a/gtsam/constraint/NonlinearInequalityConstraint.cpp +++ b/gtsam/constraint/NonlinearInequalityConstraint.cpp @@ -17,8 +17,7 @@ */ #include -#include -#include "gtsam/constraint/RampFunction.h" +#include namespace gtsam { @@ -44,6 +43,20 @@ bool NonlinearInequalityConstraint::active(const Values& x) const { return (unwhitenedExpr(x).array() >= 0).any(); } +/* ************************************************************************* */ +NoiseModelFactor::shared_ptr NonlinearInequalityConstraint::penaltyFactorSmooth( + SmoothRampFunction::shared_ptr func, const double mu) const { + /// Default behavior, this function should be overriden. + return penaltyFactor(mu); +} + +/* ************************************************************************* */ +NonlinearEqualityConstraint::shared_ptr NonlinearInequalityConstraint::createEqualityConstraint() + const { + /// Default behavior, this function should be overriden. + return nullptr; +} + /* ************************************************************************* */ NoiseModelFactor::shared_ptr NonlinearInequalityConstraint::penaltyFactorEquality( const double mu) const { diff --git a/gtsam/constraint/NonlinearInequalityConstraint.h b/gtsam/constraint/NonlinearInequalityConstraint.h index ac22df943..1a1813ea3 100644 --- a/gtsam/constraint/NonlinearInequalityConstraint.h +++ b/gtsam/constraint/NonlinearInequalityConstraint.h @@ -25,7 +25,7 @@ namespace gtsam { /** - * Inequality constraint base class. + * Inequality constraint base class, enforcing g(x) <= 0. */ class NonlinearInequalityConstraint : public NonlinearConstraint { public: @@ -39,17 +39,21 @@ class NonlinearInequalityConstraint : public NonlinearConstraint { /** Destructor. */ virtual ~NonlinearInequalityConstraint() {} + /** Return g(x). */ virtual Vector unwhitenedExpr(const Values& x, OptionalMatrixVecType H = nullptr) const = 0; + /** Return ramp(g(x)). */ virtual Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const override; + /** Return true if g(x)>=0 in any dimension. */ virtual bool active(const Values& x) const override; - virtual NonlinearEqualityConstraint::shared_ptr createEqualityConstraint() const = 0; + /** Return an equality constraint corresponding to g(x)=0. */ + virtual NonlinearEqualityConstraint::shared_ptr createEqualityConstraint() const; /** Smooth approximation of the ramp function. */ virtual NoiseModelFactor::shared_ptr penaltyFactorSmooth(SmoothRampFunction::shared_ptr func, - const double mu = 1.0) const = 0; + const double mu = 1.0) const; /** penalty function as if the constraint is equality, 0.5 * mu * ||g(x)||^2 */ virtual NoiseModelFactor::shared_ptr penaltyFactorEquality(const double mu = 1.0) const; @@ -67,7 +71,8 @@ class NonlinearInequalityConstraint : public NonlinearConstraint { }; /** Inequality constraint that force g(x) <= 0, where g(x) is a scalar-valued - * function. */ + * nonlinear function. + */ class ScalarExpressionInequalityConstraint : public NonlinearInequalityConstraint { public: typedef NonlinearInequalityConstraint Base; @@ -82,32 +87,45 @@ class ScalarExpressionInequalityConstraint : public NonlinearInequalityConstrain /** * @brief Constructor. * - * @param expression expression representing g(x). + * @param expression expression representing g(x) (or -g(x) for GeqZero). * @param sigma scalar representing sigma. */ ScalarExpressionInequalityConstraint(const Double_& expression, const double& sigma); - // Create an inequality constraint g(x)>=0. + /** Create an inequality constraint g(x)/sigma >= 0, internally represented as -g(x)/sigma <= 0. + */ static ScalarExpressionInequalityConstraint::shared_ptr GeqZero(const Double_& expression, const double& sigma); - // Create an inequality constraint g(x)<=0. + /** Create an inequality constraint g(x)/sigma <= 0. */ static ScalarExpressionInequalityConstraint::shared_ptr LeqZero(const Double_& expression, const double& sigma); + /** Compute g(x), or -g(x) for objects constructed from GeqZero. */ virtual Vector unwhitenedExpr(const Values& x, OptionalMatrixVecType H = nullptr) const override; + /** Equality constraint representing g(x)/sigma = 0. */ NonlinearEqualityConstraint::shared_ptr createEqualityConstraint() const override; + /** Penalty function 0.5*mu*||ramp(g(x)/sigma||^2. */ NoiseModelFactor::shared_ptr penaltyFactor(const double mu = 1.0) const override; + /** Penalty function using a smooth approxiamtion of the ramp funciton. */ NoiseModelFactor::shared_ptr penaltyFactorSmooth(SmoothRampFunction::shared_ptr func, const double mu = 1.0) const override; + /** Penalty function as if the constraint is equality, 0.5 * mu * ||g(x)/sigma||^2. */ virtual NoiseModelFactor::shared_ptr penaltyFactorEquality(const double mu = 1.0) const override; + /** Return expression g(x), or -g(x) for objects constructed from GeqZero. */ const Double_& expression() const { return expression_; } + /** return a deep copy of this factor. */ + gtsam::NonlinearFactor::shared_ptr clone() const override { + return std::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + private: #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ @@ -131,17 +149,19 @@ class NonlinearInequalityConstraints : public FactorGraph H) const { +double RampFunctionPoly2::operator()(const double& x, OptionalJacobian<1, 1> H) const { if (x <= 0) { if (H) { H->setZero(); @@ -49,9 +49,29 @@ double PolynomialRampFunction::operator()(const double& x, OptionalJacobian<1, 1 return 0; } else if (x < epsilon_) { if (H) { - H->setConstant(x / epsilon_); + H->setConstant(2 * a_ * x); } - return (x * x) / (2 * epsilon_) + epsilon_ / 2; + return a_ * pow(x, 2); + } else { + if (H) { + H->setOnes(); + } + return x - epsilon_ / 2; + } +} + +/* ************************************************************************* */ +double RampFunctionPoly3::operator()(const double& x, OptionalJacobian<1, 1> H) const { + if (x <= 0) { + if (H) { + H->setZero(); + } + return 0; + } else if (x < epsilon_) { + if (H) { + H->setConstant(3 * a_ * pow(x, 2) + 2 * b_ * x); + } + return a_ * pow(x, 3) + b_ * pow(x, 2); } else { if (H) { H->setOnes(); diff --git a/gtsam/constraint/RampFunction.h b/gtsam/constraint/RampFunction.h index d04e2dec0..0722551c5 100644 --- a/gtsam/constraint/RampFunction.h +++ b/gtsam/constraint/RampFunction.h @@ -10,9 +10,9 @@ * -------------------------------------------------------------------------- */ /** - * @file NonlinearInequalityConstraint.h - * @brief Nonlinear inequality constraints in constrained optimization. - * @author Yetong Zhang, Frank Dellaert + * @file RampFunction.h + * @brief Ramp function to compute inequality constraint violations. + * @author Yetong Zhang * @date Aug 3, 2024 */ @@ -20,10 +20,14 @@ #include #include +#include namespace gtsam { -/// Ramp function for create penalty factors. +/** Ramp function f : R -> R. + * f(x) = 0 for x <= 0 + * x for x > 0 + */ double RampFunction(const double x, OptionalJacobian<1, 1> H = {}); /** Base class for smooth approximation of the ramp function. */ @@ -43,20 +47,51 @@ class SmoothRampFunction { UnaryScalarFunc function() const; }; -/** Ramp function approximated with a polynomial of degree 2. - * Function f(x) = 0 for x <= 0 - * x^2/(2*e) + e/2 for 0 < x < epsilon - * x for x >= epsilon +/** Ramp function approximated with a polynomial of degree 2 in [0, epsilon]. + * The coefficients are computed as + * a = 1 / (2 * epsilon) + * Function f(x) = 0 for x <= 0 + * a * x^2 for 0 < x < epsilon + * x - epsilon/2 for x >= epsilon */ -class PolynomialRampFunction : public SmoothRampFunction { +class RampFunctionPoly2 : public SmoothRampFunction { public: typedef SmoothRampFunction Base; + typedef RampFunctionPoly2 This; + typedef std::shared_ptr shared_ptr; protected: double epsilon_; + double a_; public: - PolynomialRampFunction(const double epsilon = 1) : Base(), epsilon_(epsilon) {} + RampFunctionPoly2(const double epsilon = 1) : Base(), epsilon_(epsilon), a_(0.5 / epsilon) {} + + virtual double operator()(const double& x, OptionalJacobian<1, 1> H = {}) const override; +}; + +/** Ramp function approximated with a polynomial of degree 3 in [0, epsilon]. + * The coefficients are computed as + * a = -1 / epsilon^2 + * b = 2 / epsilon + * Function f(x) = 0 for x <= 0 + * a * x^3 + b * x^2 for 0 < x < epsilon + * x for x >= epsilon + */ +class RampFunctionPoly3 : public SmoothRampFunction { + public: + typedef SmoothRampFunction Base; + typedef RampFunctionPoly3 This; + typedef std::shared_ptr shared_ptr; + + protected: + double epsilon_; + double a_; + double b_; + + public: + RampFunctionPoly3(const double epsilon = 1) + : Base(), epsilon_(epsilon), a_(-1 / (epsilon * epsilon)), b_(2 / epsilon) {} virtual double operator()(const double& x, OptionalJacobian<1, 1> H = {}) const override; }; @@ -65,6 +100,8 @@ class PolynomialRampFunction : public SmoothRampFunction { class SoftPlusFunction : public SmoothRampFunction { public: typedef SmoothRampFunction Base; + typedef SoftPlusFunction This; + typedef std::shared_ptr shared_ptr; protected: double k_; diff --git a/gtsam/constraint/tests/testNonlinearEqualityConstraint.cpp b/gtsam/constraint/tests/testNonlinearEqualityConstraint.cpp index ca2725d4c..8309de0b3 100644 --- a/gtsam/constraint/tests/testNonlinearEqualityConstraint.cpp +++ b/gtsam/constraint/tests/testNonlinearEqualityConstraint.cpp @@ -10,8 +10,8 @@ * -------------------------------------------------------------------------- */ /** - * @file testEqualityConstraint.cpp - * @brief Equality constraints in constrained optimization. + * @file testNonlinearEqualityConstraint.cpp + * @brief unit tests for nonlinear equality constraints * @author Yetong Zhang * @date Aug 3, 2024 */ diff --git a/gtsam/constraint/tests/testNonlinearInequalityConstraint.cpp b/gtsam/constraint/tests/testNonlinearInequalityConstraint.cpp index a73367d68..3950bfa03 100644 --- a/gtsam/constraint/tests/testNonlinearInequalityConstraint.cpp +++ b/gtsam/constraint/tests/testNonlinearInequalityConstraint.cpp @@ -10,8 +10,8 @@ * -------------------------------------------------------------------------- */ /** - * @file NonlinearInequalityConstraint.h - * @brief Nonlinear inequality constraints in constrained optimization. + * @file testNonlinearInequalityConstraint.h + * @brief unit tests for nonlinear inequality constraints * @author Yetong Zhang * @date Aug 3, 2024 */ diff --git a/gtsam/constraint/tests/testRampFunction.cpp b/gtsam/constraint/tests/testRampFunction.cpp new file mode 100644 index 000000000..2e8036d74 --- /dev/null +++ b/gtsam/constraint/tests/testRampFunction.cpp @@ -0,0 +1,124 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @file testRampFunction.h + * @brief unit tests for ramp functions + * @author Yetong Zhang + * @date Aug 3, 2024 + */ + +#include +#include +#include +#include +#include + +using namespace gtsam; + +TEST(RampFunction, error_and_jacobian) { + /// Helper function for numerical Jacobian computation. + auto ramp_helper = [&](const double& x) { return RampFunction(x); }; + + /// Create a set of values to test the function. + static std::vector x_vec{-3.0, 0.0, 1.0, 2.0, 3.0}; + static std::vector expected_r_vec{0.0, 0.0, 1.0, 2.0, 3.0}; + + for (size_t i = 0; i < x_vec.size(); i++) { + double x = x_vec.at(i); + Matrix H; + double r = RampFunction(x, H); + + /// Check function evaluation. + EXPECT_DOUBLES_EQUAL(expected_r_vec.at(i), r, 1e-9); + + /// Check derivative. + if (abs(x) > 1e-6) { // function is not smooth at 0, so Jacobian is undefined. + Matrix expected_H = gtsam::numericalDerivative11(ramp_helper, x, 1e-6); + EXPECT(assert_equal(expected_H, H)); + } + } +} + +TEST(RampFunctionPoly2, error_and_jacobian) { + /// Helper function for numerical Jacobian computation. + RampFunctionPoly2 p_ramp(2.0); + auto ramp_helper = [&](const double& x) { return p_ramp(x); }; + + /// Create a set of values to test the function. + static std::vector x_vec{-3.0, 0.0, 1.0, 2.0, 3.0}; + static std::vector expected_r_vec{0.0, 0.0, 0.25, 1.0, 2.0}; + + for (size_t i = 0; i < x_vec.size(); i++) { + double x = x_vec.at(i); + Matrix H; + double r = p_ramp(x, H); + + /// Check function evaluation. + EXPECT_DOUBLES_EQUAL(expected_r_vec.at(i), r, 1e-9); + + /// Check derivative. + Matrix expected_H = gtsam::numericalDerivative11(ramp_helper, x, 1e-6); + EXPECT(assert_equal(expected_H, H, 1e-6)); + } +} + +TEST(RampFunctionPoly3, error_and_jacobian) { + /// Helper function for numerical Jacobian computation. + RampFunctionPoly3 p_ramp(2.0); + auto ramp_helper = [&](const double& x) { return p_ramp(x); }; + + /// Create a set of values to test the function. + static std::vector x_vec{-3.0, 0.0, 1.0, 2.0, 3.0}; + static std::vector expected_r_vec{0.0, 0.0, 0.75, 2.0, 3.0}; + + for (size_t i = 0; i < x_vec.size(); i++) { + double x = x_vec.at(i); + Matrix H; + double r = p_ramp(x, H); + + /// Check function evaluation. + EXPECT_DOUBLES_EQUAL(expected_r_vec.at(i), r, 1e-9); + + /// Check derivative. + Matrix expected_H = gtsam::numericalDerivative11(ramp_helper, x, 1e-6); + EXPECT(assert_equal(expected_H, H, 1e-6)); + } +} + +TEST(SoftPlusFunction, error_and_jacobian) { + /// Helper function for numerical Jacobian computation. + SoftPlusFunction soft_plus(0.5); + auto soft_plus_helper = [&](const double& x) { return soft_plus(x); }; + + /// Create a set of values to test the function. + static std::vector x_vec{-3.0, 0.0, 1.0, 2.0, 3.0}; + static std::vector expected_r_vec{ + 0.40282656, 1.38629436, 1.94815397, 2.62652338, 3.40282656}; + + for (size_t i = 0; i < x_vec.size(); i++) { + double x = x_vec.at(i); + Matrix H; + double r = soft_plus(x, H); + + /// Check function evaluation. + EXPECT_DOUBLES_EQUAL(expected_r_vec.at(i), r, 1e-6); + + /// Check derivative. + Matrix expected_H = gtsam::numericalDerivative11(soft_plus_helper, x, 1e-6); + EXPECT(assert_equal(expected_H, H, 1e-6)); + } +} + +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index f81486b30..c91bddd9c 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -18,6 +18,7 @@ #pragma once #include +#include #include #include @@ -40,14 +41,11 @@ namespace gtsam { * \nosubgrouping */ template -class NonlinearEquality: public NoiseModelFactorN { +class NonlinearEquality: public NonlinearEqualityConstraint { public: typedef VALUE T; - // Provide access to the Matrix& version of evaluateError: - using NoiseModelFactor1::evaluateError; - private: // feasible value @@ -63,7 +61,7 @@ private: using This = NonlinearEquality; // typedef to base class - using Base = NoiseModelFactorN; + using Base = NonlinearEqualityConstraint; public: @@ -88,7 +86,7 @@ public: const CompareFunction &_compare = std::bind(traits::Equals, std::placeholders::_1, std::placeholders::_2, 1e-9)) : Base(noiseModel::Constrained::All(traits::GetDimension(feasible)), - j), feasible_(feasible), allow_error_(false), error_gain_(0.0), // + KeyVector{j}), feasible_(feasible), allow_error_(false), error_gain_(0.0), // compare_(_compare) { } @@ -99,10 +97,14 @@ public: const CompareFunction &_compare = std::bind(traits::Equals, std::placeholders::_1, std::placeholders::_2, 1e-9)) : Base(noiseModel::Constrained::All(traits::GetDimension(feasible)), - j), feasible_(feasible), allow_error_(true), error_gain_(error_gain), // + KeyVector{j}), feasible_(feasible), allow_error_(true), error_gain_(error_gain), // compare_(_compare) { } + Key key() const { + return keys().front(); + } + /// @} /// @name Testable /// @{ @@ -139,7 +141,7 @@ public: } /// Error function - Vector evaluateError(const T& xj, OptionalMatrixType H) const override { + Vector evaluateError(const T& xj, OptionalMatrixType H = nullptr) const { const size_t nj = traits::GetDimension(feasible_); if (allow_error_) { if (H) @@ -158,11 +160,20 @@ public: } } + Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const override { + VALUE x1 = x.at(key()); + if (H) { + return evaluateError(x1, &(H->front())); + } else { + return evaluateError(x1); + } + } + /// Linearize is over-written, because base linearization tries to whiten GaussianFactor::shared_ptr linearize(const Values& x) const override { const T& xj = x.at(this->key()); Matrix A; - Vector b = evaluateError(xj, A); + Vector b = evaluateError(xj, &A); SharedDiagonal model = noiseModel::Constrained::All(b.size()); return GaussianFactor::shared_ptr( new JacobianFactor(this->key(), A, b, model)); @@ -206,16 +217,13 @@ struct traits> : Testable> {}; * Simple unary equality constraint - fixes a value for a variable */ template -class NonlinearEquality1: public NoiseModelFactorN { +class NonlinearEquality1: public NonlinearEqualityConstraint { public: typedef VALUE X; - // Provide access to Matrix& version of evaluateError: - using NoiseModelFactor1::evaluateError; - protected: - typedef NoiseModelFactorN Base; + typedef NonlinearEqualityConstraint Base; typedef NonlinearEquality1 This; /// Default constructor to allow for serialization @@ -240,7 +248,7 @@ public: NonlinearEquality1(const X& value, Key key, double mu = 1000.0) : Base(noiseModel::Constrained::All(traits::GetDimension(value), std::abs(mu)), - key), + KeyVector{key}), value_(value) {} ~NonlinearEquality1() override { @@ -252,14 +260,25 @@ public: gtsam::NonlinearFactor::shared_ptr(new This(*this))); } + Key key() const { return keys().front(); } + /// g(x) with optional derivative - Vector evaluateError(const X& x1, OptionalMatrixType H) const override { + Vector evaluateError(const X& x1, OptionalMatrixType H = nullptr) const { if (H) (*H) = Matrix::Identity(traits::GetDimension(x1),traits::GetDimension(x1)); // manifold equivalent of h(x)-z -> log(z,h(x)) return traits::Local(value_,x1); } + Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const override { + X x1 = x.at(key()); + if (H) { + return evaluateError(x1, &(H->front())); + } else { + return evaluateError(x1); + } + } + /// Print void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { @@ -298,10 +317,10 @@ struct traits > * be the same. */ template -class NonlinearEquality2 : public NoiseModelFactorN { +class NonlinearEquality2 : public NonlinearEqualityConstraint { protected: - using Base = NoiseModelFactorN; - using This = NonlinearEquality2; + typedef NonlinearEqualityConstraint Base; + typedef NonlinearEquality2 This; GTSAM_CONCEPT_MANIFOLD_TYPE(T) @@ -311,9 +330,6 @@ class NonlinearEquality2 : public NoiseModelFactorN { public: typedef std::shared_ptr> shared_ptr; - // Provide access to the Matrix& version of evaluateError: - using Base::evaluateError; - /** * Constructor @@ -323,7 +339,7 @@ class NonlinearEquality2 : public NoiseModelFactorN { */ NonlinearEquality2(Key key1, Key key2, double mu = 1e4) : Base(noiseModel::Constrained::All(traits::dimension, std::abs(mu)), - key1, key2) {} + KeyVector{key1, key2}) {} ~NonlinearEquality2() override {} /// @return a deep copy of this factor @@ -334,13 +350,23 @@ class NonlinearEquality2 : public NoiseModelFactorN { /// g(x) with optional derivative2 Vector evaluateError( - const T& x1, const T& x2, OptionalMatrixType H1, OptionalMatrixType H2) const override { + const T& x1, const T& x2, OptionalMatrixType H1 = nullptr, OptionalMatrixType H2 = nullptr) const { static const size_t p = traits::dimension; if (H1) *H1 = -Matrix::Identity(p, p); if (H2) *H2 = Matrix::Identity(p, p); return traits::Local(x1, x2); } + Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const override { + T x1 = x.at(keys().front()); + T x2 = x.at(keys().back()); + if (H) { + return evaluateError(x1, x2, &(H->front()), &(H->back())); + } else { + return evaluateError(x1, x2); + } + } + GTSAM_MAKE_ALIGNED_OPERATOR_NEW private: diff --git a/gtsam/slam/BoundingConstraint.h b/gtsam/slam/BoundingConstraint.h index a496971e2..d35152adf 100644 --- a/gtsam/slam/BoundingConstraint.h +++ b/gtsam/slam/BoundingConstraint.h @@ -19,6 +19,7 @@ #include #include +#include namespace gtsam { @@ -30,20 +31,17 @@ namespace gtsam { * @ingroup slam */ template -struct BoundingConstraint1: public NoiseModelFactorN { +struct BoundingConstraint1: public NonlinearInequalityConstraint { typedef VALUE X; - typedef NoiseModelFactorN Base; + typedef NonlinearInequalityConstraint Base; typedef std::shared_ptr > shared_ptr; - // Provide access to the Matrix& version of evaluateError: - using Base::evaluateError; - double threshold_; bool isGreaterThan_; /// flag for greater/less than BoundingConstraint1(Key key, double threshold, bool isGreaterThan, double mu = 1000.0) : - Base(noiseModel::Constrained::All(1, mu), key), + Base(noiseModel::Constrained::All(1, mu), KeyVector{key}), threshold_(threshold), isGreaterThan_(isGreaterThan) { } @@ -51,6 +49,7 @@ struct BoundingConstraint1: public NoiseModelFactorN { inline double threshold() const { return threshold_; } inline bool isGreaterThan() const { return isGreaterThan_; } + inline Key key() const { return keys().front(); } /** * function producing a scalar value to compare to the threshold @@ -60,14 +59,23 @@ struct BoundingConstraint1: public NoiseModelFactorN { virtual double value(const X& x, OptionalMatrixType H = OptionalNone) const = 0; - /** active when constraint *NOT* met */ - bool active(const Values& c) const override { - // note: still active at equality to avoid zigzagging - double x = value(c.at(this->key())); - return (isGreaterThan_) ? x <= threshold_ : x >= threshold_; + Vector unwhitenedExpr(const Values& x, OptionalMatrixVecType H = nullptr) const override { + if (H) { + double d = value(x.at(this->key()), &(H->front())); + if (isGreaterThan_) { + H->front() *= -1; + return Vector1(threshold_ - d); + } else { + return Vector1(d - threshold_); + } + } else { + double d = value(x.at(this->key())); + return Vector1((isGreaterThan_) ? threshold_ - d : d - threshold_); + } } - Vector evaluateError(const X& x, OptionalMatrixType H) const override { + /// TODO: This should be deprecated. + Vector evaluateError(const X& x, OptionalMatrixType H = nullptr) const { Matrix D; double error = value(x, &D) - threshold_; if (H) { @@ -102,22 +110,19 @@ private: * to implement for specific systems */ template -struct BoundingConstraint2: public NoiseModelFactorN { +struct BoundingConstraint2: public NonlinearInequalityConstraint { typedef VALUE1 X1; typedef VALUE2 X2; - typedef NoiseModelFactorN Base; + typedef NonlinearInequalityConstraint Base; typedef std::shared_ptr > shared_ptr; - // Provide access to the Matrix& version of evaluateError: - using Base::evaluateError; - double threshold_; bool isGreaterThan_; /// flag for greater/less than BoundingConstraint2(Key key1, Key key2, double threshold, bool isGreaterThan, double mu = 1000.0) - : Base(noiseModel::Constrained::All(1, mu), key1, key2), + : Base(noiseModel::Constrained::All(1, mu), KeyVector{key1, key2}), threshold_(threshold), isGreaterThan_(isGreaterThan) {} ~BoundingConstraint2() override {} @@ -133,15 +138,27 @@ struct BoundingConstraint2: public NoiseModelFactorN { OptionalMatrixType H1 = OptionalNone, OptionalMatrixType H2 = OptionalNone) const = 0; - /** active when constraint *NOT* met */ - bool active(const Values& c) const override { - // note: still active at equality to avoid zigzagging - double x = value(c.at(this->key1()), c.at(this->key2())); - return (isGreaterThan_) ? x <= threshold_ : x >= threshold_; + Vector unwhitenedExpr(const Values& x, OptionalMatrixVecType H = nullptr) const override { + X1 x1 = x.at(keys().front()); + X2 x2 = x.at(keys().back()); + if (H) { + double d = value(x1, x2, &(H->front()), &(H->back())); + if (isGreaterThan_) { + H->front() *= -1; + H->back() *= -1; + return Vector1(threshold_ - d); + } else { + return Vector1(d - threshold_); + } + } else { + double d = value(x1, x2); + return Vector1((isGreaterThan_) ? threshold_ - d : d - threshold_); + } } + /// TODO: This should be deprecated. Vector evaluateError(const X1& x1, const X2& x2, - OptionalMatrixType H1, OptionalMatrixType H2) const override { + OptionalMatrixType H1 = nullptr, OptionalMatrixType H2 = nullptr) const { Matrix D1, D2; double error = value(x1, x2, &D1, &D2) - threshold_; if (H1) { diff --git a/tests/testBoundingConstraint.cpp b/tests/testBoundingConstraint.cpp index 4aa357ba2..ca0ec489d 100644 --- a/tests/testBoundingConstraint.cpp +++ b/tests/testBoundingConstraint.cpp @@ -90,8 +90,8 @@ TEST( testBoundingConstraint, unary_basics_active1 ) { EXPECT(constraint2.active(config)); EXPECT(assert_equal(Vector::Constant(1,-3.0), constraint1.evaluateError(pt2), tol)); EXPECT(assert_equal(Vector::Constant(1,-5.0), constraint2.evaluateError(pt2), tol)); - EXPECT(assert_equal(Vector::Constant(1,-3.0), constraint1.unwhitenedError(config), tol)); - EXPECT(assert_equal(Vector::Constant(1,-5.0), constraint2.unwhitenedError(config), tol)); + EXPECT(assert_equal(Vector::Constant(1, 3.0), constraint1.unwhitenedError(config), tol)); + EXPECT(assert_equal(Vector::Constant(1, 5.0), constraint2.unwhitenedError(config), tol)); EXPECT_DOUBLES_EQUAL(45.0, constraint1.error(config), tol); EXPECT_DOUBLES_EQUAL(125.0, constraint2.error(config), tol); } @@ -103,10 +103,10 @@ TEST( testBoundingConstraint, unary_basics_active2 ) { config.insert(key, pt1); EXPECT(constraint3.active(config)); EXPECT(constraint4.active(config)); - EXPECT(assert_equal(-1.0 * I_1x1, constraint3.evaluateError(pt1), tol)); - EXPECT(assert_equal(-1.0 * I_1x1, constraint4.evaluateError(pt1), tol)); - EXPECT(assert_equal(-1.0 * I_1x1, constraint3.unwhitenedError(config), tol)); - EXPECT(assert_equal(-1.0 * I_1x1, constraint4.unwhitenedError(config), tol)); + // EXPECT(assert_equal(-1.0 * I_1x1, constraint3.evaluateError(pt1), tol)); + // EXPECT(assert_equal(-1.0 * I_1x1, constraint4.evaluateError(pt1), tol)); + EXPECT(assert_equal(1.0 * I_1x1, constraint3.unwhitenedError(config), tol)); + EXPECT(assert_equal(1.0 * I_1x1, constraint4.unwhitenedError(config), tol)); EXPECT_DOUBLES_EQUAL(5.0, constraint3.error(config), tol); EXPECT_DOUBLES_EQUAL(5.0, constraint4.error(config), tol); } @@ -213,7 +213,7 @@ TEST( testBoundingConstraint, MaxDistance_basics) { config1.update(key2, pt4); EXPECT(rangeBound.active(config1)); - EXPECT(assert_equal(-1.0*I_1x1, rangeBound.unwhitenedError(config1))); + EXPECT(assert_equal(1.0*I_1x1, rangeBound.unwhitenedError(config1))); EXPECT_DOUBLES_EQUAL(0.5*mu, rangeBound.error(config1), tol); } From 6caf0a364224355acf0c2a7a2a5e747d448bac91 Mon Sep 17 00:00:00 2001 From: yetongumich Date: Mon, 26 Aug 2024 15:23:39 -0400 Subject: [PATCH 05/52] add GTSAM_EXPORT and refactor --- ...tion.cpp => InequalityPenaltyFunction.cpp} | 28 ++-- ...Function.h => InequalityPenaltyFunction.h} | 81 ++++++---- gtsam/constraint/NonlinearConstraint.h | 9 +- .../NonlinearEqualityConstraint-inl.h | 6 +- .../NonlinearEqualityConstraint.cpp | 16 +- .../constraint/NonlinearEqualityConstraint.h | 8 +- .../NonlinearInequalityConstraint.cpp | 61 ++++---- .../NonlinearInequalityConstraint.h | 23 +-- gtsam/constraint/tests/constrainedExample.h | 141 ++++++++++-------- ....cpp => testInequalityPenaltyFunction.cpp} | 16 +- .../tests/testNonlinearEqualityConstraint.cpp | 10 +- 11 files changed, 225 insertions(+), 174 deletions(-) rename gtsam/constraint/{RampFunction.cpp => InequalityPenaltyFunction.cpp} (75%) rename gtsam/constraint/{RampFunction.h => InequalityPenaltyFunction.h} (51%) rename gtsam/constraint/tests/{testRampFunction.cpp => testInequalityPenaltyFunction.cpp} (84%) diff --git a/gtsam/constraint/RampFunction.cpp b/gtsam/constraint/InequalityPenaltyFunction.cpp similarity index 75% rename from gtsam/constraint/RampFunction.cpp rename to gtsam/constraint/InequalityPenaltyFunction.cpp index 64038a325..8b048686f 100644 --- a/gtsam/constraint/RampFunction.cpp +++ b/gtsam/constraint/InequalityPenaltyFunction.cpp @@ -10,18 +10,23 @@ * -------------------------------------------------------------------------- */ /** - * @file RampFunction.cpp + * @file InequalityPenaltyFunction.cpp * @brief Ramp function to compute inequality constraint violations. * @author Yetong Zhang * @date Aug 3, 2024 */ -#include +#include namespace gtsam { -/* ************************************************************************* */ -double RampFunction(const double x, OptionalJacobian<1, 1> H) { +/* ********************************************************************************************* */ +InequalityPenaltyFunction::UnaryScalarFunc InequalityPenaltyFunction::function() const { + return [=](const double& x, OptionalJacobian<1, 1> H = {}) -> double { return (*this)(x, H); }; +} + +/* ********************************************************************************************* */ +double RampFunction::Ramp(const double x, OptionalJacobian<1, 1> H) { if (x < 0) { if (H) { H->setConstant(0); @@ -35,13 +40,8 @@ double RampFunction(const double x, OptionalJacobian<1, 1> H) { } } -/* ************************************************************************* */ -SmoothRampFunction::UnaryScalarFunc SmoothRampFunction::function() const { - return [=](const double& x, OptionalJacobian<1, 1> H = {}) -> double { return (*this)(x, H); }; -} - -/* ************************************************************************* */ -double RampFunctionPoly2::operator()(const double& x, OptionalJacobian<1, 1> H) const { +/* ********************************************************************************************* */ +double SmoothRampPoly2::operator()(const double& x, OptionalJacobian<1, 1> H) const { if (x <= 0) { if (H) { H->setZero(); @@ -60,8 +60,8 @@ double RampFunctionPoly2::operator()(const double& x, OptionalJacobian<1, 1> H) } } -/* ************************************************************************* */ -double RampFunctionPoly3::operator()(const double& x, OptionalJacobian<1, 1> H) const { +/* ********************************************************************************************* */ +double SmoothRampPoly3::operator()(const double& x, OptionalJacobian<1, 1> H) const { if (x <= 0) { if (H) { H->setZero(); @@ -80,7 +80,7 @@ double RampFunctionPoly3::operator()(const double& x, OptionalJacobian<1, 1> H) } } -/* ************************************************************************* */ +/* ********************************************************************************************* */ double SoftPlusFunction::operator()(const double& x, OptionalJacobian<1, 1> H) const { if (H) { H->setConstant(1 / (1 + std::exp(-k_ * x))); diff --git a/gtsam/constraint/RampFunction.h b/gtsam/constraint/InequalityPenaltyFunction.h similarity index 51% rename from gtsam/constraint/RampFunction.h rename to gtsam/constraint/InequalityPenaltyFunction.h index 0722551c5..baafaef0b 100644 --- a/gtsam/constraint/RampFunction.h +++ b/gtsam/constraint/InequalityPenaltyFunction.h @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file RampFunction.h + * @file InequalityPenaltyFunction.h * @brief Ramp function to compute inequality constraint violations. * @author Yetong Zhang * @date Aug 3, 2024 @@ -20,31 +20,49 @@ #include #include -#include namespace gtsam { +/** Base class for smooth approximation of the ramp function. */ +class GTSAM_EXPORT InequalityPenaltyFunction { + public: + typedef std::shared_ptr shared_ptr; + typedef std::function H)> + UnaryScalarFunc; + + /** Constructor. */ + InequalityPenaltyFunction() {} + + /** Destructor. */ + virtual ~InequalityPenaltyFunction() {} + + virtual double operator()(const double& x, + OptionalJacobian<1, 1> H = {}) const = 0; + + virtual UnaryScalarFunc function() const; +}; + /** Ramp function f : R -> R. * f(x) = 0 for x <= 0 * x for x > 0 */ -double RampFunction(const double x, OptionalJacobian<1, 1> H = {}); - -/** Base class for smooth approximation of the ramp function. */ -class SmoothRampFunction { +class GTSAM_EXPORT RampFunction : public InequalityPenaltyFunction { public: - typedef std::shared_ptr shared_ptr; - typedef std::function H)> UnaryScalarFunc; + typedef InequalityPenaltyFunction Base; + typedef RampFunction This; + typedef std::shared_ptr shared_ptr; - /** Constructor. */ - SmoothRampFunction() {} + public: + RampFunction() : Base() {} - /** Destructor. */ - virtual ~SmoothRampFunction() {} + virtual double operator()(const double& x, + OptionalJacobian<1, 1> H = {}) const override { + return Ramp(x, H); + } - virtual double operator()(const double& x, OptionalJacobian<1, 1> H = {}) const = 0; + virtual UnaryScalarFunc function() const override { return Ramp; } - UnaryScalarFunc function() const; + static double Ramp(const double x, OptionalJacobian<1, 1> H = {}); }; /** Ramp function approximated with a polynomial of degree 2 in [0, epsilon]. @@ -54,10 +72,10 @@ class SmoothRampFunction { * a * x^2 for 0 < x < epsilon * x - epsilon/2 for x >= epsilon */ -class RampFunctionPoly2 : public SmoothRampFunction { +class GTSAM_EXPORT SmoothRampPoly2 : public InequalityPenaltyFunction { public: - typedef SmoothRampFunction Base; - typedef RampFunctionPoly2 This; + typedef InequalityPenaltyFunction Base; + typedef SmoothRampPoly2 This; typedef std::shared_ptr shared_ptr; protected: @@ -65,9 +83,11 @@ class RampFunctionPoly2 : public SmoothRampFunction { double a_; public: - RampFunctionPoly2(const double epsilon = 1) : Base(), epsilon_(epsilon), a_(0.5 / epsilon) {} + SmoothRampPoly2(const double epsilon = 1) + : Base(), epsilon_(epsilon), a_(0.5 / epsilon) {} - virtual double operator()(const double& x, OptionalJacobian<1, 1> H = {}) const override; + virtual double operator()(const double& x, + OptionalJacobian<1, 1> H = {}) const override; }; /** Ramp function approximated with a polynomial of degree 3 in [0, epsilon]. @@ -78,10 +98,10 @@ class RampFunctionPoly2 : public SmoothRampFunction { * a * x^3 + b * x^2 for 0 < x < epsilon * x for x >= epsilon */ -class RampFunctionPoly3 : public SmoothRampFunction { +class GTSAM_EXPORT SmoothRampPoly3 : public InequalityPenaltyFunction { public: - typedef SmoothRampFunction Base; - typedef RampFunctionPoly3 This; + typedef InequalityPenaltyFunction Base; + typedef SmoothRampPoly3 This; typedef std::shared_ptr shared_ptr; protected: @@ -90,16 +110,20 @@ class RampFunctionPoly3 : public SmoothRampFunction { double b_; public: - RampFunctionPoly3(const double epsilon = 1) - : Base(), epsilon_(epsilon), a_(-1 / (epsilon * epsilon)), b_(2 / epsilon) {} + SmoothRampPoly3(const double epsilon = 1) + : Base(), + epsilon_(epsilon), + a_(-1 / (epsilon * epsilon)), + b_(2 / epsilon) {} - virtual double operator()(const double& x, OptionalJacobian<1, 1> H = {}) const override; + virtual double operator()(const double& x, + OptionalJacobian<1, 1> H = {}) const override; }; /** Softplus function that implements f(x) = log(1 + exp(k*x)) / k. */ -class SoftPlusFunction : public SmoothRampFunction { +class GTSAM_EXPORT SoftPlusFunction : public InequalityPenaltyFunction { public: - typedef SmoothRampFunction Base; + typedef InequalityPenaltyFunction Base; typedef SoftPlusFunction This; typedef std::shared_ptr shared_ptr; @@ -109,7 +133,8 @@ class SoftPlusFunction : public SmoothRampFunction { public: SoftPlusFunction(const double k = 1) : Base(), k_(k) {} - virtual double operator()(const double& x, OptionalJacobian<1, 1> H = {}) const override; + virtual double operator()(const double& x, + OptionalJacobian<1, 1> H = {}) const override; }; } // namespace gtsam diff --git a/gtsam/constraint/NonlinearConstraint.h b/gtsam/constraint/NonlinearConstraint.h index 834ef7991..558818803 100644 --- a/gtsam/constraint/NonlinearConstraint.h +++ b/gtsam/constraint/NonlinearConstraint.h @@ -31,7 +31,7 @@ namespace gtsam { * whitenedError() returns The constraint violation vector. * unwhitenedError() returns the sigma-scaled constraint violation vector. */ -class NonlinearConstraint : public NoiseModelFactor { +class GTSAM_EXPORT NonlinearConstraint : public NoiseModelFactor { public: typedef NoiseModelFactor Base; @@ -54,6 +54,13 @@ class NonlinearConstraint : public NoiseModelFactor { return violation(x) <= tolerance; } + const Vector sigmas() const { return noiseModel()->sigmas(); } + + // return the hessian of unwhitened error function in each dimension. + virtual std::vector unwhitenedHessian(const Values& x) const { + throw std::runtime_error("hessian not implemented"); + } + protected: /** Noise model used for the penalty function. */ SharedDiagonal penaltyNoise(const double mu) const { diff --git a/gtsam/constraint/NonlinearEqualityConstraint-inl.h b/gtsam/constraint/NonlinearEqualityConstraint-inl.h index 625f1e366..c228cf97d 100644 --- a/gtsam/constraint/NonlinearEqualityConstraint-inl.h +++ b/gtsam/constraint/NonlinearEqualityConstraint-inl.h @@ -21,7 +21,7 @@ namespace gtsam { -/* ************************************************************************* */ +/* ********************************************************************************************* */ template ExpressionEqualityConstraint::ExpressionEqualityConstraint(const Expression& expression, const T& rhs, @@ -31,7 +31,7 @@ ExpressionEqualityConstraint::ExpressionEqualityConstraint(const Expression Vector ExpressionEqualityConstraint::unwhitenedError(const Values& x, OptionalMatrixVecType H) const { @@ -45,7 +45,7 @@ Vector ExpressionEqualityConstraint::unwhitenedError(const Values& x, } } -/* ************************************************************************* */ +/* ********************************************************************************************* */ template NoiseModelFactor::shared_ptr ExpressionEqualityConstraint::penaltyFactor(const double mu) const { return std::make_shared>(penaltyNoise(mu), rhs_, expression_); diff --git a/gtsam/constraint/NonlinearEqualityConstraint.cpp b/gtsam/constraint/NonlinearEqualityConstraint.cpp index 0c5b7ea11..8705f7db9 100644 --- a/gtsam/constraint/NonlinearEqualityConstraint.cpp +++ b/gtsam/constraint/NonlinearEqualityConstraint.cpp @@ -19,21 +19,21 @@ namespace gtsam { -/* ************************************************************************* */ +/* ********************************************************************************************* */ ZeroCostConstraint::ZeroCostConstraint(const NoiseModelFactor::shared_ptr& factor) : Base(constrainedNoise(factor->noiseModel()->sigmas()), factor->keys()), factor_(factor) {} -/* ************************************************************************* */ +/* ********************************************************************************************* */ Vector ZeroCostConstraint::unwhitenedError(const Values& x, OptionalMatrixVecType H) const { return factor_->unwhitenedError(x, H); } -/* ************************************************************************* */ +/* ********************************************************************************************* */ NoiseModelFactor::shared_ptr ZeroCostConstraint::penaltyFactor(const double mu) const { return factor_->cloneWithNewNoiseModel(penaltyNoise(mu)); } -/* ************************************************************************* */ +/* ********************************************************************************************* */ NonlinearEqualityConstraints NonlinearEqualityConstraints::FromCostGraph( const NonlinearFactorGraph& graph) { NonlinearEqualityConstraints constraints; @@ -44,7 +44,7 @@ NonlinearEqualityConstraints NonlinearEqualityConstraints::FromCostGraph( return constraints; } -/* ************************************************************************* */ +/* ********************************************************************************************* */ size_t NonlinearEqualityConstraints::dim() const { size_t dimension = 0; for (const auto& constraint : *this) { @@ -53,7 +53,7 @@ size_t NonlinearEqualityConstraints::dim() const { return dimension; } -/* ************************************************************************* */ +/* ********************************************************************************************* */ Vector NonlinearEqualityConstraints::violationVector(const Values& values, bool whiten) const { Vector violation(dim()); size_t start_idx = 0; @@ -66,12 +66,12 @@ Vector NonlinearEqualityConstraints::violationVector(const Values& values, bool return violation; } -/* ************************************************************************* */ +/* ********************************************************************************************* */ double NonlinearEqualityConstraints::violationNorm(const Values& values) const { return violationVector(values, true).norm(); } -/* ************************************************************************* */ +/* ********************************************************************************************* */ NonlinearFactorGraph NonlinearEqualityConstraints::penaltyGraph(const double mu) const { NonlinearFactorGraph graph; for (const auto& constraint : *this) { diff --git a/gtsam/constraint/NonlinearEqualityConstraint.h b/gtsam/constraint/NonlinearEqualityConstraint.h index ab7dca2f0..532c3ab38 100644 --- a/gtsam/constraint/NonlinearEqualityConstraint.h +++ b/gtsam/constraint/NonlinearEqualityConstraint.h @@ -26,7 +26,7 @@ namespace gtsam { /** * Equality constraint base class. */ -class NonlinearEqualityConstraint : public NonlinearConstraint { +class GTSAM_EXPORT NonlinearEqualityConstraint : public NonlinearConstraint { public: typedef NonlinearConstraint Base; typedef NonlinearEqualityConstraint This; @@ -52,7 +52,7 @@ class NonlinearEqualityConstraint : public NonlinearConstraint { /** Equality constraint that force g(x) = M. */ template -class ExpressionEqualityConstraint : public NonlinearEqualityConstraint { +class GTSAM_EXPORT ExpressionEqualityConstraint : public NonlinearEqualityConstraint { public: typedef NonlinearEqualityConstraint Base; typedef ExpressionEqualityConstraint This; @@ -100,7 +100,7 @@ class ExpressionEqualityConstraint : public NonlinearEqualityConstraint { }; /** Equality constraint that enforce the cost factor with zero error. */ -class ZeroCostConstraint : public NonlinearEqualityConstraint { +class GTSAM_EXPORT ZeroCostConstraint : public NonlinearEqualityConstraint { public: typedef NonlinearEqualityConstraint Base; typedef ZeroCostConstraint This; @@ -142,7 +142,7 @@ class ZeroCostConstraint : public NonlinearEqualityConstraint { }; /// Container of NonlinearEqualityConstraint. -class NonlinearEqualityConstraints : public FactorGraph { +class GTSAM_EXPORT NonlinearEqualityConstraints : public FactorGraph { public: typedef std::shared_ptr shared_ptr; typedef FactorGraph Base; diff --git a/gtsam/constraint/NonlinearInequalityConstraint.cpp b/gtsam/constraint/NonlinearInequalityConstraint.cpp index 49003f09f..3404a143c 100644 --- a/gtsam/constraint/NonlinearInequalityConstraint.cpp +++ b/gtsam/constraint/NonlinearInequalityConstraint.cpp @@ -17,11 +17,15 @@ */ #include -#include namespace gtsam { -/* ************************************************************************* */ +/* ********************************************************************************************* */ +Vector NonlinearInequalityConstraint::whitenedExpr(const Values& x) const { + return noiseModel()->whiten(unwhitenedExpr(x)); +} + +/* ********************************************************************************************* */ Vector NonlinearInequalityConstraint::unwhitenedError(const Values& x, OptionalMatrixVecType H) const { Vector error = unwhitenedExpr(x, H); @@ -38,52 +42,52 @@ Vector NonlinearInequalityConstraint::unwhitenedError(const Values& x, return error; } -/* ************************************************************************* */ +/* ********************************************************************************************* */ bool NonlinearInequalityConstraint::active(const Values& x) const { return (unwhitenedExpr(x).array() >= 0).any(); } -/* ************************************************************************* */ -NoiseModelFactor::shared_ptr NonlinearInequalityConstraint::penaltyFactorSmooth( - SmoothRampFunction::shared_ptr func, const double mu) const { +/* ********************************************************************************************* */ +NoiseModelFactor::shared_ptr NonlinearInequalityConstraint::penaltyFactorCustom( + InequalityPenaltyFunction::shared_ptr func, const double mu) const { /// Default behavior, this function should be overriden. return penaltyFactor(mu); } -/* ************************************************************************* */ +/* ********************************************************************************************* */ NonlinearEqualityConstraint::shared_ptr NonlinearInequalityConstraint::createEqualityConstraint() const { /// Default behavior, this function should be overriden. return nullptr; } -/* ************************************************************************* */ +/* ********************************************************************************************* */ NoiseModelFactor::shared_ptr NonlinearInequalityConstraint::penaltyFactorEquality( const double mu) const { return createEqualityConstraint()->penaltyFactor(mu); } -/* ************************************************************************* */ +/* ********************************************************************************************* */ ScalarExpressionInequalityConstraint::ScalarExpressionInequalityConstraint( const Double_& expression, const double& sigma) : Base(constrainedNoise(Vector1(sigma)), expression.keysAndDims().first), expression_(expression), dims_(expression.keysAndDims().second) {} -/* ************************************************************************* */ +/* ********************************************************************************************* */ ScalarExpressionInequalityConstraint::shared_ptr ScalarExpressionInequalityConstraint::GeqZero( const Double_& expression, const double& sigma) { Double_ neg_expr = Double_(0.0) - expression; return std::make_shared(neg_expr, sigma); } -/* ************************************************************************* */ +/* ********************************************************************************************* */ ScalarExpressionInequalityConstraint::shared_ptr ScalarExpressionInequalityConstraint::LeqZero( const Double_& expression, const double& sigma) { return std::make_shared(expression, sigma); } -/* ************************************************************************* */ +/* ********************************************************************************************* */ Vector ScalarExpressionInequalityConstraint::unwhitenedExpr(const Values& x, OptionalMatrixVecType H) const { // Copy-paste from ExpressionFactor. @@ -94,35 +98,38 @@ Vector ScalarExpressionInequalityConstraint::unwhitenedExpr(const Values& x, } } -/* ************************************************************************* */ +/* ********************************************************************************************* */ NonlinearEqualityConstraint::shared_ptr ScalarExpressionInequalityConstraint::createEqualityConstraint() const { return std::make_shared>( expression_, 0.0, noiseModel()->sigmas()); } -/* ************************************************************************* */ +/* ********************************************************************************************* */ NoiseModelFactor::shared_ptr ScalarExpressionInequalityConstraint::penaltyFactor( const double mu) const { - Double_ penalty_expression(RampFunction, expression_); + Double_ penalty_expression(RampFunction::Ramp, expression_); return std::make_shared>(penaltyNoise(mu), 0.0, penalty_expression); } -/* ************************************************************************* */ -NoiseModelFactor::shared_ptr ScalarExpressionInequalityConstraint::penaltyFactorSmooth( - SmoothRampFunction::shared_ptr func, const double mu) const { +/* ********************************************************************************************* */ +NoiseModelFactor::shared_ptr ScalarExpressionInequalityConstraint::penaltyFactorCustom( + InequalityPenaltyFunction::shared_ptr func, const double mu) const { + if (!func) { + return penaltyFactor(mu); + } // TODO(yetong): can we pass the functor directly to construct the expression? Double_ error(func->function(), expression_); return std::make_shared>(penaltyNoise(mu), 0.0, error); } -/* ************************************************************************* */ +/* ********************************************************************************************* */ NoiseModelFactor::shared_ptr ScalarExpressionInequalityConstraint::penaltyFactorEquality( const double mu) const { return std::make_shared>(penaltyNoise(mu), 0.0, expression_); } -/* ************************************************************************* */ +/* ********************************************************************************************* */ size_t NonlinearInequalityConstraints::dim() const { size_t dimension = 0; for (const auto& constraint : *this) { @@ -131,7 +138,7 @@ size_t NonlinearInequalityConstraints::dim() const { return dimension; } -/* ************************************************************************* */ +/* ********************************************************************************************* */ Vector NonlinearInequalityConstraints::violationVector(const Values& values, bool whiten) const { Vector violation(dim()); size_t start_idx = 0; @@ -144,12 +151,12 @@ Vector NonlinearInequalityConstraints::violationVector(const Values& values, boo return violation; } -/* ************************************************************************* */ +/* ********************************************************************************************* */ double NonlinearInequalityConstraints::violationNorm(const Values& values) const { return violationVector(values, true).norm(); } -/* ************************************************************************* */ +/* ********************************************************************************************* */ NonlinearFactorGraph NonlinearInequalityConstraints::penaltyGraph(const double mu) const { NonlinearFactorGraph graph; for (const auto& constraint : *this) { @@ -158,12 +165,12 @@ NonlinearFactorGraph NonlinearInequalityConstraints::penaltyGraph(const double m return graph; } -/* ************************************************************************* */ -NonlinearFactorGraph NonlinearInequalityConstraints::penaltyGraphSmooth( - SmoothRampFunction::shared_ptr func, const double mu) const { +/* ********************************************************************************************* */ +NonlinearFactorGraph NonlinearInequalityConstraints::penaltyGraphCustom( + InequalityPenaltyFunction::shared_ptr func, const double mu) const { NonlinearFactorGraph graph; for (const auto& constraint : *this) { - graph.add(constraint->penaltyFactorSmooth(func, mu)); + graph.add(constraint->penaltyFactorCustom(func, mu)); } return graph; } diff --git a/gtsam/constraint/NonlinearInequalityConstraint.h b/gtsam/constraint/NonlinearInequalityConstraint.h index 1a1813ea3..658a29f2f 100644 --- a/gtsam/constraint/NonlinearInequalityConstraint.h +++ b/gtsam/constraint/NonlinearInequalityConstraint.h @@ -18,8 +18,8 @@ #pragma once +#include #include -#include #include namespace gtsam { @@ -27,7 +27,7 @@ namespace gtsam { /** * Inequality constraint base class, enforcing g(x) <= 0. */ -class NonlinearInequalityConstraint : public NonlinearConstraint { +class GTSAM_EXPORT NonlinearInequalityConstraint : public NonlinearConstraint { public: typedef NonlinearConstraint Base; typedef NonlinearInequalityConstraint This; @@ -42,6 +42,8 @@ class NonlinearInequalityConstraint : public NonlinearConstraint { /** Return g(x). */ virtual Vector unwhitenedExpr(const Values& x, OptionalMatrixVecType H = nullptr) const = 0; + virtual Vector whitenedExpr(const Values& x) const; + /** Return ramp(g(x)). */ virtual Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const override; @@ -51,9 +53,9 @@ class NonlinearInequalityConstraint : public NonlinearConstraint { /** Return an equality constraint corresponding to g(x)=0. */ virtual NonlinearEqualityConstraint::shared_ptr createEqualityConstraint() const; - /** Smooth approximation of the ramp function. */ - virtual NoiseModelFactor::shared_ptr penaltyFactorSmooth(SmoothRampFunction::shared_ptr func, - const double mu = 1.0) const; + /** Cost factor using a customized penalty function. */ + virtual NoiseModelFactor::shared_ptr penaltyFactorCustom( + InequalityPenaltyFunction::shared_ptr func, const double mu = 1.0) const; /** penalty function as if the constraint is equality, 0.5 * mu * ||g(x)||^2 */ virtual NoiseModelFactor::shared_ptr penaltyFactorEquality(const double mu = 1.0) const; @@ -73,7 +75,7 @@ class NonlinearInequalityConstraint : public NonlinearConstraint { /** Inequality constraint that force g(x) <= 0, where g(x) is a scalar-valued * nonlinear function. */ -class ScalarExpressionInequalityConstraint : public NonlinearInequalityConstraint { +class GTSAM_EXPORT ScalarExpressionInequalityConstraint : public NonlinearInequalityConstraint { public: typedef NonlinearInequalityConstraint Base; typedef ScalarExpressionInequalityConstraint This; @@ -111,7 +113,7 @@ class ScalarExpressionInequalityConstraint : public NonlinearInequalityConstrain NoiseModelFactor::shared_ptr penaltyFactor(const double mu = 1.0) const override; /** Penalty function using a smooth approxiamtion of the ramp funciton. */ - NoiseModelFactor::shared_ptr penaltyFactorSmooth(SmoothRampFunction::shared_ptr func, + NoiseModelFactor::shared_ptr penaltyFactorCustom(InequalityPenaltyFunction::shared_ptr func, const double mu = 1.0) const override; /** Penalty function as if the constraint is equality, 0.5 * mu * ||g(x)/sigma||^2. */ @@ -141,7 +143,8 @@ class ScalarExpressionInequalityConstraint : public NonlinearInequalityConstrain }; /// Container of NonlinearInequalityConstraint. -class NonlinearInequalityConstraints : public FactorGraph { +class GTSAM_EXPORT NonlinearInequalityConstraints + : public FactorGraph { public: typedef FactorGraph Base; typedef NonlinearInequalityConstraints This; @@ -161,8 +164,8 @@ class NonlinearInequalityConstraints : public FactorGraph -#include -#include -#include -#include +#include #include #include -#include -#include - +#include "gtsam/constraint/NonlinearEqualityConstraint.h" +#include "gtsam/constraint/NonlinearInequalityConstraint.h" namespace constrained_example { using namespace gtsam; /// Exponential function e^x. -double exp_func(const double& x, gtsam::OptionalJacobian<1, 1> H1 = {}) { +inline double exp_func(const double& x, gtsam::OptionalJacobian<1, 1> H1 = {}) { double result = exp(x); if (H1) H1->setConstant(result); return result; } /// Exponential expression e^x. -Double_ exp(const Double_& x) { return Double_(exp_func, x); } +inline Double_ exp(const Double_& x) { return Double_(exp_func, x); } /// Pow functor used for pow function. class PowFunctor { @@ -51,24 +46,23 @@ class PowFunctor { public: PowFunctor(const double& c) : c_(c) {} - double operator()(const double& x, - gtsam::OptionalJacobian<1, 1> H1 = {}) const { + double operator()(const double& x, gtsam::OptionalJacobian<1, 1> H1 = {}) const { if (H1) H1->setConstant(c_ * pow(x, c_ - 1)); return pow(x, c_); } }; /// Pow function. -Double_ pow(const Double_& x, const double& c) { +inline Double_ pow(const Double_& x, const double& c) { PowFunctor pow_functor(c); return Double_(pow_functor, x); } /// Plus between Double expression and double. -Double_ operator+(const Double_& x, const double& d) { return x + Double_(d); } +inline Double_ operator+(const Double_& x, const double& d) { return x + Double_(d); } /// Negative sign operator. -Double_ operator-(const Double_& x) { return Double_(0.0) - x; } +inline Double_ operator-(const Double_& x) { return Double_(0.0) - x; } /// Keys for creating expressions. Symbol x1_key('x', 1); @@ -82,11 +76,11 @@ Double_ x1(x1_key), x2(x2_key); * Constrained optimization example in L. Vandenberghe slides: * https://www.seas.ucla.edu/~vandenbe/133B/lectures/nllseq.pdf * f(x) = 0.5*||x1 + e^(-x2)||^2 + 0.5*||x1^2 + 2*x2 + 1||^2 - * h(x) = x1 + x1^3 + x2 + x2^2 + * h(x) = x1 + x1^3 + x2 + x2^2 = 0 */ -namespace e_constrained_example { +namespace constrained_example1 { using namespace constrained_example; -NonlinearFactorGraph GetCost() { +NonlinearFactorGraph Cost() { NonlinearFactorGraph graph; auto f1 = x1 + exp(-x2); auto f2 = pow(x1, 2.0) + 2.0 * x2 + 1.0; @@ -96,28 +90,49 @@ NonlinearFactorGraph GetCost() { return graph; } -NonlinearEqualityConstraints GetConstraints() { +NonlinearEqualityConstraints EqConstraints() { NonlinearEqualityConstraints constraints; - Vector scale = Vector1(0.1); + Vector sigmas = Vector1(1.0); auto h1 = x1 + pow(x1, 3) + x2 + pow(x2, 2); constraints.push_back(NonlinearEqualityConstraint::shared_ptr( - new ExpressionEqualityConstraint(h1, 0.0, scale))); + new ExpressionEqualityConstraint(h1, 0.0, sigmas))); return constraints; } -NonlinearFactorGraph cost = GetCost(); -NonlinearEqualityConstraints constraints = GetConstraints(); -} // namespace e_constrained_example +Values InitValues() { + Values values; + values.insert(x1_key, -0.2); + values.insert(x2_key, -0.2); + return values; +} + +Values OptimalValues() { + Values values; + values.insert(x1_key, 0.0); + values.insert(x2_key, 0.0); + return values; +} + +NonlinearFactorGraph costs = Cost(); +NonlinearEqualityConstraints e_constraints = EqConstraints(); +NonlinearInequalityConstraints i_constraints; +Values init_values = InitValues(); +ConstrainedOptProblem::shared_ptr problem = + std::make_shared(costs, e_constraints, i_constraints, init_values); +Values optimal_values = OptimalValues(); +} // namespace constrained_example1 /* ************************************************************************* */ /** * Constrained optimization example with inequality constraints + * Approach a point while staying on unit circle, and within an ellipse. * f(x) = 0.5 * ||x1-1||^2 + 0.5 * ||x2-1||^2 - * g(x) = 1 - x1^2 - x2^2 + * h(x) = x1^2 + x2^2 - 1 = 0 + * g(x) = 4*x1^2 + 0.25*x2^2 - 1 <= 0 */ -namespace i_constrained_example { +namespace constrained_example2 { using namespace constrained_example; -NonlinearFactorGraph GetCost() { +NonlinearFactorGraph Cost() { NonlinearFactorGraph graph; auto cost_noise = gtsam::noiseModel::Isotropic::Sigma(1, 1.0); graph.addPrior(x1_key, 1.0, cost_noise); @@ -125,44 +140,42 @@ NonlinearFactorGraph GetCost() { return graph; } -// InequalityConstraints GetIConstraints() { -// InequalityConstraints i_constraints; -// Double_ g1 = Double_(1.0) - x1 * x1 - x2 * x2; -// double tolerance = 0.2; -// i_constraints.emplace_shared(g1, tolerance); -// return i_constraints; -// } - -NonlinearFactorGraph cost = GetCost(); -NonlinearEqualityConstraints e_constraints; -// InequalityConstraints i_constraints = GetIConstraints(); -} // namespace i_constrained_example - -/* ************************************************************************* */ -/** - * Constrained optimization example with inequality constraints - * f(x) = 0.5 * ||x1||^2 + 0.5 * ||x2||^2 - * h(x) = x1 + x2 - 1 - */ -namespace e_constrained_example2 { -using namespace constrained_example; -NonlinearFactorGraph GetCost() { - NonlinearFactorGraph graph; - auto cost_noise = gtsam::noiseModel::Isotropic::Sigma(1, 1.0); - graph.addPrior(x1_key, 1.0, cost_noise); - // graph.addPrior(x2_key, 0.0, cost_noise); - // graph.emplace_shared>(x1_key, x2_key, 1.0, cost_noise); - return graph; +NonlinearEqualityConstraints EqConstraints() { + NonlinearEqualityConstraints constraints; + Vector1 sigmas(1.0); + Double_ h1 = x1 * x1 + x2 * x2; + constraints.emplace_shared>(h1, 1.0, sigmas); + return constraints; } -NonlinearEqualityConstraints GetEConstraints() { - NonlinearEqualityConstraints e_constraints; - // Double_ h1 = x1 + x2 - Double_(1.0); - // double tolerance = 0.2; - // e_constraints.emplace_shared(h1, tolerance); - return e_constraints; +NonlinearInequalityConstraints IneqConstraints() { + NonlinearInequalityConstraints constraints; + Double_ g1 = 4 * x1 * x1 + 0.25 * x2 * x2 - Double_(1.0); + double sigma = 1; + constraints.emplace_shared(g1, sigma); + return constraints; } -NonlinearFactorGraph cost = GetCost(); -NonlinearEqualityConstraints e_constraints = GetEConstraints(); -} // namespace i_constrained_example +Values InitValues() { + Values values; + values.insert(x1_key, -1.0); + values.insert(x2_key, 2.0); + return values; +} + +Values OptimalValues() { + Values values; + values.insert(x1_key, 1.0 / sqrt(5)); + values.insert(x2_key, 2.0 / sqrt(5)); + return values; +} + +NonlinearFactorGraph costs = Cost(); +NonlinearEqualityConstraints e_constraints = EqConstraints(); +NonlinearInequalityConstraints i_constraints = IneqConstraints(); +Values init_values = InitValues(); +ConstrainedOptProblem::shared_ptr problem = + std::make_shared(costs, e_constraints, i_constraints, init_values); +Values optimal_values = OptimalValues(); + +} // namespace constrained_example2 diff --git a/gtsam/constraint/tests/testRampFunction.cpp b/gtsam/constraint/tests/testInequalityPenaltyFunction.cpp similarity index 84% rename from gtsam/constraint/tests/testRampFunction.cpp rename to gtsam/constraint/tests/testInequalityPenaltyFunction.cpp index 2e8036d74..09cd17d5b 100644 --- a/gtsam/constraint/tests/testRampFunction.cpp +++ b/gtsam/constraint/tests/testInequalityPenaltyFunction.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file testRampFunction.h + * @file testInequalityPenaltyFunction.h * @brief unit tests for ramp functions * @author Yetong Zhang * @date Aug 3, 2024 @@ -20,13 +20,14 @@ #include #include #include -#include +#include using namespace gtsam; +/* ********************************************************************************************* */ TEST(RampFunction, error_and_jacobian) { /// Helper function for numerical Jacobian computation. - auto ramp_helper = [&](const double& x) { return RampFunction(x); }; + auto ramp_helper = [&](const double& x) { return RampFunction::Ramp(x); }; /// Create a set of values to test the function. static std::vector x_vec{-3.0, 0.0, 1.0, 2.0, 3.0}; @@ -35,7 +36,7 @@ TEST(RampFunction, error_and_jacobian) { for (size_t i = 0; i < x_vec.size(); i++) { double x = x_vec.at(i); Matrix H; - double r = RampFunction(x, H); + double r = RampFunction::Ramp(x, H); /// Check function evaluation. EXPECT_DOUBLES_EQUAL(expected_r_vec.at(i), r, 1e-9); @@ -48,9 +49,10 @@ TEST(RampFunction, error_and_jacobian) { } } +/* ********************************************************************************************* */ TEST(RampFunctionPoly2, error_and_jacobian) { /// Helper function for numerical Jacobian computation. - RampFunctionPoly2 p_ramp(2.0); + SmoothRampPoly2 p_ramp(2.0); auto ramp_helper = [&](const double& x) { return p_ramp(x); }; /// Create a set of values to test the function. @@ -71,9 +73,10 @@ TEST(RampFunctionPoly2, error_and_jacobian) { } } +/* ********************************************************************************************* */ TEST(RampFunctionPoly3, error_and_jacobian) { /// Helper function for numerical Jacobian computation. - RampFunctionPoly3 p_ramp(2.0); + SmoothRampPoly3 p_ramp(2.0); auto ramp_helper = [&](const double& x) { return p_ramp(x); }; /// Create a set of values to test the function. @@ -94,6 +97,7 @@ TEST(RampFunctionPoly3, error_and_jacobian) { } } +/* ********************************************************************************************* */ TEST(SoftPlusFunction, error_and_jacobian) { /// Helper function for numerical Jacobian computation. SoftPlusFunction soft_plus(0.5); diff --git a/gtsam/constraint/tests/testNonlinearEqualityConstraint.cpp b/gtsam/constraint/tests/testNonlinearEqualityConstraint.cpp index 8309de0b3..70dabff45 100644 --- a/gtsam/constraint/tests/testNonlinearEqualityConstraint.cpp +++ b/gtsam/constraint/tests/testNonlinearEqualityConstraint.cpp @@ -22,7 +22,6 @@ #include #include #include -#include #include #include @@ -155,7 +154,6 @@ TEST(ExpressionEqualityConstraint, Vector2) { EXPECT_CORRECT_FACTOR_JACOBIANS(*merit_factor, values2, 1e-7, 1e-5); } - // Test methods of FactorZeroErrorConstraint. TEST(ZeroCostConstraint, BetweenFactor) { Key x1_key = 1; @@ -218,9 +216,6 @@ TEST(ZeroCostConstraint, BetweenFactor) { EXPECT_CORRECT_FACTOR_JACOBIANS(*merit_factor, values2, 1e-7, 1e-5); } - - - TEST(NonlinearEqualityConstraints, Container) { NonlinearEqualityConstraints constraints; @@ -254,12 +249,9 @@ TEST(NonlinearEqualityConstraints, Container) { EXPECT(assert_container_equality(expected_vi_x2, vi[x2_key])); // Check constraint violation. - } - -TEST(NonlinearEqualityConstraints, FromCostGraph) { -} +TEST(NonlinearEqualityConstraints, FromCostGraph) {} int main() { TestResult tr; From 7bb76f5356f872b4c84c3ef8ea03aa98bc2ac26b Mon Sep 17 00:00:00 2001 From: yetongumich Date: Mon, 26 Aug 2024 15:40:19 -0400 Subject: [PATCH 06/52] adding constrained optimization problem --- gtsam/constraint/ConstrainedOptProblem.cpp | 110 +++++++++++++++++++++ gtsam/constraint/ConstrainedOptProblem.h | 103 +++++++++++++++++++ 2 files changed, 213 insertions(+) create mode 100644 gtsam/constraint/ConstrainedOptProblem.cpp create mode 100644 gtsam/constraint/ConstrainedOptProblem.h diff --git a/gtsam/constraint/ConstrainedOptProblem.cpp b/gtsam/constraint/ConstrainedOptProblem.cpp new file mode 100644 index 000000000..2a7d78135 --- /dev/null +++ b/gtsam/constraint/ConstrainedOptProblem.cpp @@ -0,0 +1,110 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @file ConstrainedOptProblem.h + * @brief Nonlinear constrained optimization problem. + * @author Yetong Zhang, Frank Dellaert + * @date Aug 3, 2024 + */ + +#include +#include +#include +#include "gtsam/constraint/NonlinearEqualityConstraint.h" +#include "gtsam/nonlinear/NonlinearFactor.h" + +namespace gtsam { + +/* ********************************************************************************************* */ +size_t GraphDimension(const NonlinearFactorGraph& graph) { + size_t total_dim = 0; + for (const auto& factor : graph) { + total_dim += factor->dim(); + } + return total_dim; +} + +/* ********************************************************************************************* */ +bool CheckPureCost(const NonlinearFactorGraph& graph) { + for (const auto& factor : graph) { + if (NoiseModelFactor::shared_ptr f = std::dynamic_pointer_cast(factor)) { + if (f->noiseModel()->isConstrained()) { + return false; + } + } + } + return true; +} + +/* ********************************************************************************************* */ +ConstrainedOptProblem::ConstrainedOptProblem(const NonlinearFactorGraph& costs, + const NonlinearEqualityConstraints& e_constraints, + const NonlinearInequalityConstraints& i_constraints, + const Values& values) + : costs_(costs), e_constraints_(e_constraints), i_constraints_(i_constraints), values_(values) { + if (!CheckPureCost(costs)) { + throw std::runtime_error( + "Cost contains factors with constrained noise model. They should be moved to constraints."); + } +} + +/* ********************************************************************************************* */ +std::tuple ConstrainedOptProblem::dim() const { + return { + GraphDimension(costs()), eConstraints().dim(), iConstraints().dim(), initialValues().dim()}; +} + +/* ********************************************************************************************* */ +std::tuple ConstrainedOptProblem::evaluate(const Values& values) const { + return {costs().error(values), + eConstraints().violationNorm(values), + iConstraints().violationNorm(values)}; +} + +/* ********************************************************************************************* */ +ConstrainedOptProblem ConstrainedOptProblem::auxiliaryProblem( + const AuxiliaryKeyGenerator& generator) const { + if (iConstraints().size() == 0) { + return *this; + } + + NonlinearEqualityConstraints new_e_constraints = eConstraints(); + Values new_values = initialValues(); + + size_t k = 0; + for (const auto& i_constraint : iConstraints()) { + if (ScalarExpressionInequalityConstraint::shared_ptr p = + std::dynamic_pointer_cast(i_constraint)) { + // Generate next available auxiliary key. + Key aux_key = generator.generate(k, new_values); + + // Construct auxiliary equality constraint. + Double_ aux_expr(aux_key); + Double_ equality_expr = p->expression() + aux_expr * aux_expr; + new_e_constraints.emplace_shared>( + equality_expr, 0.0, p->noiseModel()->sigmas()); + + // Compute initial value for auxiliary key. + if (!i_constraint->feasible(initialValues())) { + new_values.insert(aux_key, 0.0); + } else { + Vector gap = i_constraint->unwhitenedExpr(initialValues()); + new_values.insert(aux_key, sqrt(-gap(0))); + } + } + } + return ConstrainedOptProblem::EqConstrainedOptProblem(costs_, new_e_constraints, new_values); +} + +/* ********************************************************************************************* */ + +} // namespace gtsam \ No newline at end of file diff --git a/gtsam/constraint/ConstrainedOptProblem.h b/gtsam/constraint/ConstrainedOptProblem.h new file mode 100644 index 000000000..a22de51a1 --- /dev/null +++ b/gtsam/constraint/ConstrainedOptProblem.h @@ -0,0 +1,103 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @file ConstrainedOptProblem.h + * @brief Nonlinear constrained optimization problem. + * @author Yetong Zhang, Frank Dellaert + * @date Aug 3, 2024 + */ + +#pragma once + +#include +#include + +namespace gtsam { + +/** Constrained optimization problem, in the form of + * argmin_x 0.5||f(X)||^2 + * s.t. h(X) = 0 + * g(X) <= 0 + * where X represents the variables, 0.5||f(X)||^2 represents the quadratic cost + * functions, h(X)=0 represents the nonlinear equality constraints, g(x)<=0 represents the + * inequality constraints. + */ +class GTSAM_EXPORT ConstrainedOptProblem { + public: + typedef ConstrainedOptProblem This; + typedef std::shared_ptr shared_ptr; + + protected: + NonlinearFactorGraph costs_; // cost function, ||f(X)||^2 + NonlinearEqualityConstraints e_constraints_; // equality constraints, h(X)=0 + NonlinearInequalityConstraints i_constraints_; // inequality constraints, g(X)<=0 + Values values_; // initial value estimates of X + + public: + /** Constructor with both equality and inequality constraints. */ + ConstrainedOptProblem(const NonlinearFactorGraph& costs, + const NonlinearEqualityConstraints& e_constraints, + const NonlinearInequalityConstraints& i_constraints, + const Values& values = Values()); + + /** Constructor with equality constraints only. */ + static ConstrainedOptProblem EqConstrainedOptProblem( + const NonlinearFactorGraph& costs, + const NonlinearEqualityConstraints& e_constraints, + const Values& values = Values()) { + return ConstrainedOptProblem(costs, e_constraints, NonlinearInequalityConstraints(), values); + } + + /** Member variable access functions. */ + const NonlinearFactorGraph& costs() const { return costs_; } + const NonlinearEqualityConstraints& eConstraints() const { return e_constraints_; } + const NonlinearInequalityConstraints& iConstraints() const { return i_constraints_; } + const Values& initialValues() const { return values_; } + + /** Evaluate cost and constraint violations. + * Return a tuple representing (cost, e-constraint violation, i-constraint violation). + */ + std::tuple evaluate(const Values& values) const; + + /** Return the dimension of the problem, as a tuple of + * total dimension of cost factors, + * total dimension of equality constraints, + * total dimension of inequality constraints, + * total dimension of variables. + */ + std::tuple dim() const; + + /** Base class to generate keys for auxiliary variables. */ + class GTSAM_EXPORT AuxiliaryKeyGenerator { + public: + AuxiliaryKeyGenerator() {} + virtual ~AuxiliaryKeyGenerator() {} + + virtual Key generate(const size_t k) const { return Symbol('u', k); } + + /** generate the next available auxiliary key that is not in values. */ + Key generate(size_t& k, const Values& values) const { + Key key = generate(k++); + while (values.exists(key)) { + key = generate(k++); + } + return key; + } + }; + + /// Equivalent equality-constrained optimization probelm with auxiliary + /// variables z. Inequality constraints g(x)<=0 are transformed into equality + /// constraints g(x)+z^2=0. + ConstrainedOptProblem auxiliaryProblem(const AuxiliaryKeyGenerator& generator) const; +}; + +} // namespace gtsam From 8b09ef1297503721be2e312181d31fe2265c5c18 Mon Sep 17 00:00:00 2001 From: yetongumich Date: Mon, 26 Aug 2024 16:36:32 -0400 Subject: [PATCH 07/52] rm constrainedOptProblem --- gtsam/constraint/ConstrainedOptProblem.cpp | 110 ------------ gtsam/constraint/ConstrainedOptProblem.h | 103 ----------- gtsam/constraint/tests/constrainedExample.h | 186 ++++++++++---------- 3 files changed, 93 insertions(+), 306 deletions(-) delete mode 100644 gtsam/constraint/ConstrainedOptProblem.cpp delete mode 100644 gtsam/constraint/ConstrainedOptProblem.h diff --git a/gtsam/constraint/ConstrainedOptProblem.cpp b/gtsam/constraint/ConstrainedOptProblem.cpp deleted file mode 100644 index 2a7d78135..000000000 --- a/gtsam/constraint/ConstrainedOptProblem.cpp +++ /dev/null @@ -1,110 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 - - * -------------------------------------------------------------------------- */ - -/** - * @file ConstrainedOptProblem.h - * @brief Nonlinear constrained optimization problem. - * @author Yetong Zhang, Frank Dellaert - * @date Aug 3, 2024 - */ - -#include -#include -#include -#include "gtsam/constraint/NonlinearEqualityConstraint.h" -#include "gtsam/nonlinear/NonlinearFactor.h" - -namespace gtsam { - -/* ********************************************************************************************* */ -size_t GraphDimension(const NonlinearFactorGraph& graph) { - size_t total_dim = 0; - for (const auto& factor : graph) { - total_dim += factor->dim(); - } - return total_dim; -} - -/* ********************************************************************************************* */ -bool CheckPureCost(const NonlinearFactorGraph& graph) { - for (const auto& factor : graph) { - if (NoiseModelFactor::shared_ptr f = std::dynamic_pointer_cast(factor)) { - if (f->noiseModel()->isConstrained()) { - return false; - } - } - } - return true; -} - -/* ********************************************************************************************* */ -ConstrainedOptProblem::ConstrainedOptProblem(const NonlinearFactorGraph& costs, - const NonlinearEqualityConstraints& e_constraints, - const NonlinearInequalityConstraints& i_constraints, - const Values& values) - : costs_(costs), e_constraints_(e_constraints), i_constraints_(i_constraints), values_(values) { - if (!CheckPureCost(costs)) { - throw std::runtime_error( - "Cost contains factors with constrained noise model. They should be moved to constraints."); - } -} - -/* ********************************************************************************************* */ -std::tuple ConstrainedOptProblem::dim() const { - return { - GraphDimension(costs()), eConstraints().dim(), iConstraints().dim(), initialValues().dim()}; -} - -/* ********************************************************************************************* */ -std::tuple ConstrainedOptProblem::evaluate(const Values& values) const { - return {costs().error(values), - eConstraints().violationNorm(values), - iConstraints().violationNorm(values)}; -} - -/* ********************************************************************************************* */ -ConstrainedOptProblem ConstrainedOptProblem::auxiliaryProblem( - const AuxiliaryKeyGenerator& generator) const { - if (iConstraints().size() == 0) { - return *this; - } - - NonlinearEqualityConstraints new_e_constraints = eConstraints(); - Values new_values = initialValues(); - - size_t k = 0; - for (const auto& i_constraint : iConstraints()) { - if (ScalarExpressionInequalityConstraint::shared_ptr p = - std::dynamic_pointer_cast(i_constraint)) { - // Generate next available auxiliary key. - Key aux_key = generator.generate(k, new_values); - - // Construct auxiliary equality constraint. - Double_ aux_expr(aux_key); - Double_ equality_expr = p->expression() + aux_expr * aux_expr; - new_e_constraints.emplace_shared>( - equality_expr, 0.0, p->noiseModel()->sigmas()); - - // Compute initial value for auxiliary key. - if (!i_constraint->feasible(initialValues())) { - new_values.insert(aux_key, 0.0); - } else { - Vector gap = i_constraint->unwhitenedExpr(initialValues()); - new_values.insert(aux_key, sqrt(-gap(0))); - } - } - } - return ConstrainedOptProblem::EqConstrainedOptProblem(costs_, new_e_constraints, new_values); -} - -/* ********************************************************************************************* */ - -} // namespace gtsam \ No newline at end of file diff --git a/gtsam/constraint/ConstrainedOptProblem.h b/gtsam/constraint/ConstrainedOptProblem.h deleted file mode 100644 index a22de51a1..000000000 --- a/gtsam/constraint/ConstrainedOptProblem.h +++ /dev/null @@ -1,103 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 - - * -------------------------------------------------------------------------- */ - -/** - * @file ConstrainedOptProblem.h - * @brief Nonlinear constrained optimization problem. - * @author Yetong Zhang, Frank Dellaert - * @date Aug 3, 2024 - */ - -#pragma once - -#include -#include - -namespace gtsam { - -/** Constrained optimization problem, in the form of - * argmin_x 0.5||f(X)||^2 - * s.t. h(X) = 0 - * g(X) <= 0 - * where X represents the variables, 0.5||f(X)||^2 represents the quadratic cost - * functions, h(X)=0 represents the nonlinear equality constraints, g(x)<=0 represents the - * inequality constraints. - */ -class GTSAM_EXPORT ConstrainedOptProblem { - public: - typedef ConstrainedOptProblem This; - typedef std::shared_ptr shared_ptr; - - protected: - NonlinearFactorGraph costs_; // cost function, ||f(X)||^2 - NonlinearEqualityConstraints e_constraints_; // equality constraints, h(X)=0 - NonlinearInequalityConstraints i_constraints_; // inequality constraints, g(X)<=0 - Values values_; // initial value estimates of X - - public: - /** Constructor with both equality and inequality constraints. */ - ConstrainedOptProblem(const NonlinearFactorGraph& costs, - const NonlinearEqualityConstraints& e_constraints, - const NonlinearInequalityConstraints& i_constraints, - const Values& values = Values()); - - /** Constructor with equality constraints only. */ - static ConstrainedOptProblem EqConstrainedOptProblem( - const NonlinearFactorGraph& costs, - const NonlinearEqualityConstraints& e_constraints, - const Values& values = Values()) { - return ConstrainedOptProblem(costs, e_constraints, NonlinearInequalityConstraints(), values); - } - - /** Member variable access functions. */ - const NonlinearFactorGraph& costs() const { return costs_; } - const NonlinearEqualityConstraints& eConstraints() const { return e_constraints_; } - const NonlinearInequalityConstraints& iConstraints() const { return i_constraints_; } - const Values& initialValues() const { return values_; } - - /** Evaluate cost and constraint violations. - * Return a tuple representing (cost, e-constraint violation, i-constraint violation). - */ - std::tuple evaluate(const Values& values) const; - - /** Return the dimension of the problem, as a tuple of - * total dimension of cost factors, - * total dimension of equality constraints, - * total dimension of inequality constraints, - * total dimension of variables. - */ - std::tuple dim() const; - - /** Base class to generate keys for auxiliary variables. */ - class GTSAM_EXPORT AuxiliaryKeyGenerator { - public: - AuxiliaryKeyGenerator() {} - virtual ~AuxiliaryKeyGenerator() {} - - virtual Key generate(const size_t k) const { return Symbol('u', k); } - - /** generate the next available auxiliary key that is not in values. */ - Key generate(size_t& k, const Values& values) const { - Key key = generate(k++); - while (values.exists(key)) { - key = generate(k++); - } - return key; - } - }; - - /// Equivalent equality-constrained optimization probelm with auxiliary - /// variables z. Inequality constraints g(x)<=0 are transformed into equality - /// constraints g(x)+z^2=0. - ConstrainedOptProblem auxiliaryProblem(const AuxiliaryKeyGenerator& generator) const; -}; - -} // namespace gtsam diff --git a/gtsam/constraint/tests/constrainedExample.h b/gtsam/constraint/tests/constrainedExample.h index 47bf771f0..2cc3ba826 100644 --- a/gtsam/constraint/tests/constrainedExample.h +++ b/gtsam/constraint/tests/constrainedExample.h @@ -18,11 +18,11 @@ #pragma once -#include +// #include #include #include -#include "gtsam/constraint/NonlinearEqualityConstraint.h" -#include "gtsam/constraint/NonlinearInequalityConstraint.h" +#include +#include namespace constrained_example { @@ -78,104 +78,104 @@ Double_ x1(x1_key), x2(x2_key); * f(x) = 0.5*||x1 + e^(-x2)||^2 + 0.5*||x1^2 + 2*x2 + 1||^2 * h(x) = x1 + x1^3 + x2 + x2^2 = 0 */ -namespace constrained_example1 { -using namespace constrained_example; -NonlinearFactorGraph Cost() { - NonlinearFactorGraph graph; - auto f1 = x1 + exp(-x2); - auto f2 = pow(x1, 2.0) + 2.0 * x2 + 1.0; - auto cost_noise = gtsam::noiseModel::Isotropic::Sigma(1, 1.0); - graph.add(ExpressionFactor(cost_noise, 0., f1)); - graph.add(ExpressionFactor(cost_noise, 0., f2)); - return graph; -} +// namespace constrained_example1 { +// using namespace constrained_example; +// NonlinearFactorGraph Cost() { +// NonlinearFactorGraph graph; +// auto f1 = x1 + exp(-x2); +// auto f2 = pow(x1, 2.0) + 2.0 * x2 + 1.0; +// auto cost_noise = gtsam::noiseModel::Isotropic::Sigma(1, 1.0); +// graph.add(ExpressionFactor(cost_noise, 0., f1)); +// graph.add(ExpressionFactor(cost_noise, 0., f2)); +// return graph; +// } -NonlinearEqualityConstraints EqConstraints() { - NonlinearEqualityConstraints constraints; - Vector sigmas = Vector1(1.0); - auto h1 = x1 + pow(x1, 3) + x2 + pow(x2, 2); - constraints.push_back(NonlinearEqualityConstraint::shared_ptr( - new ExpressionEqualityConstraint(h1, 0.0, sigmas))); - return constraints; -} +// NonlinearEqualityConstraints EqConstraints() { +// NonlinearEqualityConstraints constraints; +// Vector sigmas = Vector1(1.0); +// auto h1 = x1 + pow(x1, 3) + x2 + pow(x2, 2); +// constraints.push_back(NonlinearEqualityConstraint::shared_ptr( +// new ExpressionEqualityConstraint(h1, 0.0, sigmas))); +// return constraints; +// } -Values InitValues() { - Values values; - values.insert(x1_key, -0.2); - values.insert(x2_key, -0.2); - return values; -} +// Values InitValues() { +// Values values; +// values.insert(x1_key, -0.2); +// values.insert(x2_key, -0.2); +// return values; +// } -Values OptimalValues() { - Values values; - values.insert(x1_key, 0.0); - values.insert(x2_key, 0.0); - return values; -} +// Values OptimalValues() { +// Values values; +// values.insert(x1_key, 0.0); +// values.insert(x2_key, 0.0); +// return values; +// } -NonlinearFactorGraph costs = Cost(); -NonlinearEqualityConstraints e_constraints = EqConstraints(); -NonlinearInequalityConstraints i_constraints; -Values init_values = InitValues(); -ConstrainedOptProblem::shared_ptr problem = - std::make_shared(costs, e_constraints, i_constraints, init_values); -Values optimal_values = OptimalValues(); -} // namespace constrained_example1 +// NonlinearFactorGraph costs = Cost(); +// NonlinearEqualityConstraints e_constraints = EqConstraints(); +// NonlinearInequalityConstraints i_constraints; +// Values init_values = InitValues(); +// ConstrainedOptProblem::shared_ptr problem = +// std::make_shared(costs, e_constraints, i_constraints, init_values); +// Values optimal_values = OptimalValues(); +// } // namespace constrained_example1 -/* ************************************************************************* */ -/** - * Constrained optimization example with inequality constraints - * Approach a point while staying on unit circle, and within an ellipse. - * f(x) = 0.5 * ||x1-1||^2 + 0.5 * ||x2-1||^2 - * h(x) = x1^2 + x2^2 - 1 = 0 - * g(x) = 4*x1^2 + 0.25*x2^2 - 1 <= 0 - */ -namespace constrained_example2 { -using namespace constrained_example; -NonlinearFactorGraph Cost() { - NonlinearFactorGraph graph; - auto cost_noise = gtsam::noiseModel::Isotropic::Sigma(1, 1.0); - graph.addPrior(x1_key, 1.0, cost_noise); - graph.addPrior(x2_key, 1.0, cost_noise); - return graph; -} +// /* ************************************************************************* */ +// /** +// * Constrained optimization example with inequality constraints +// * Approach a point while staying on unit circle, and within an ellipse. +// * f(x) = 0.5 * ||x1-1||^2 + 0.5 * ||x2-1||^2 +// * h(x) = x1^2 + x2^2 - 1 = 0 +// * g(x) = 4*x1^2 + 0.25*x2^2 - 1 <= 0 +// */ +// namespace constrained_example2 { +// using namespace constrained_example; +// NonlinearFactorGraph Cost() { +// NonlinearFactorGraph graph; +// auto cost_noise = gtsam::noiseModel::Isotropic::Sigma(1, 1.0); +// graph.addPrior(x1_key, 1.0, cost_noise); +// graph.addPrior(x2_key, 1.0, cost_noise); +// return graph; +// } -NonlinearEqualityConstraints EqConstraints() { - NonlinearEqualityConstraints constraints; - Vector1 sigmas(1.0); - Double_ h1 = x1 * x1 + x2 * x2; - constraints.emplace_shared>(h1, 1.0, sigmas); - return constraints; -} +// NonlinearEqualityConstraints EqConstraints() { +// NonlinearEqualityConstraints constraints; +// Vector1 sigmas(1.0); +// Double_ h1 = x1 * x1 + x2 * x2; +// constraints.emplace_shared>(h1, 1.0, sigmas); +// return constraints; +// } -NonlinearInequalityConstraints IneqConstraints() { - NonlinearInequalityConstraints constraints; - Double_ g1 = 4 * x1 * x1 + 0.25 * x2 * x2 - Double_(1.0); - double sigma = 1; - constraints.emplace_shared(g1, sigma); - return constraints; -} +// NonlinearInequalityConstraints IneqConstraints() { +// NonlinearInequalityConstraints constraints; +// Double_ g1 = 4 * x1 * x1 + 0.25 * x2 * x2 - Double_(1.0); +// double sigma = 1; +// constraints.emplace_shared(g1, sigma); +// return constraints; +// } -Values InitValues() { - Values values; - values.insert(x1_key, -1.0); - values.insert(x2_key, 2.0); - return values; -} +// Values InitValues() { +// Values values; +// values.insert(x1_key, -1.0); +// values.insert(x2_key, 2.0); +// return values; +// } -Values OptimalValues() { - Values values; - values.insert(x1_key, 1.0 / sqrt(5)); - values.insert(x2_key, 2.0 / sqrt(5)); - return values; -} +// Values OptimalValues() { +// Values values; +// values.insert(x1_key, 1.0 / sqrt(5)); +// values.insert(x2_key, 2.0 / sqrt(5)); +// return values; +// } -NonlinearFactorGraph costs = Cost(); -NonlinearEqualityConstraints e_constraints = EqConstraints(); -NonlinearInequalityConstraints i_constraints = IneqConstraints(); -Values init_values = InitValues(); -ConstrainedOptProblem::shared_ptr problem = - std::make_shared(costs, e_constraints, i_constraints, init_values); -Values optimal_values = OptimalValues(); +// NonlinearFactorGraph costs = Cost(); +// NonlinearEqualityConstraints e_constraints = EqConstraints(); +// NonlinearInequalityConstraints i_constraints = IneqConstraints(); +// Values init_values = InitValues(); +// ConstrainedOptProblem::shared_ptr problem = +// std::make_shared(costs, e_constraints, i_constraints, init_values); +// Values optimal_values = OptimalValues(); -} // namespace constrained_example2 +// } // namespace constrained_example2 From e482c41afd7560decc25b0cd478e8dadbb6102e9 Mon Sep 17 00:00:00 2001 From: yetongumich Date: Tue, 27 Aug 2024 16:25:37 -0400 Subject: [PATCH 08/52] add FactorGraph include --- gtsam/constraint/NonlinearConstraint.h | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/constraint/NonlinearConstraint.h b/gtsam/constraint/NonlinearConstraint.h index 558818803..7ee7ed771 100644 --- a/gtsam/constraint/NonlinearConstraint.h +++ b/gtsam/constraint/NonlinearConstraint.h @@ -18,6 +18,7 @@ #pragma once +#include #include #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION #include From 2516a0a1bbf2b81393e30f47362c82c5c8763f14 Mon Sep 17 00:00:00 2001 From: yetongumich Date: Tue, 27 Aug 2024 16:34:55 -0400 Subject: [PATCH 09/52] add FactorGraph-inst --- gtsam/constraint/NonlinearConstraint.h | 1 + gtsam/constraint/NonlinearEqualityConstraint.cpp | 1 + gtsam/constraint/NonlinearInequalityConstraint.cpp | 1 + 3 files changed, 3 insertions(+) diff --git a/gtsam/constraint/NonlinearConstraint.h b/gtsam/constraint/NonlinearConstraint.h index 7ee7ed771..da13f77c6 100644 --- a/gtsam/constraint/NonlinearConstraint.h +++ b/gtsam/constraint/NonlinearConstraint.h @@ -19,6 +19,7 @@ #pragma once #include +#include #include #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION #include diff --git a/gtsam/constraint/NonlinearEqualityConstraint.cpp b/gtsam/constraint/NonlinearEqualityConstraint.cpp index 8705f7db9..170f2317c 100644 --- a/gtsam/constraint/NonlinearEqualityConstraint.cpp +++ b/gtsam/constraint/NonlinearEqualityConstraint.cpp @@ -16,6 +16,7 @@ * @date Aug 3, 2024 */ #include +#include namespace gtsam { diff --git a/gtsam/constraint/NonlinearInequalityConstraint.cpp b/gtsam/constraint/NonlinearInequalityConstraint.cpp index 3404a143c..2f96f9a29 100644 --- a/gtsam/constraint/NonlinearInequalityConstraint.cpp +++ b/gtsam/constraint/NonlinearInequalityConstraint.cpp @@ -17,6 +17,7 @@ */ #include +#include namespace gtsam { From 037c8b36436993383a9b74e0249fc006d1b1039a Mon Sep 17 00:00:00 2001 From: yetongumich Date: Sun, 17 Nov 2024 23:53:21 -0800 Subject: [PATCH 10/52] add using NonlinearFactor::error --- gtsam/constraint/NonlinearConstraint.h | 2 ++ gtsam/nonlinear/NonlinearFactor.h | 2 ++ 2 files changed, 4 insertions(+) diff --git a/gtsam/constraint/NonlinearConstraint.h b/gtsam/constraint/NonlinearConstraint.h index da13f77c6..a01430bf3 100644 --- a/gtsam/constraint/NonlinearConstraint.h +++ b/gtsam/constraint/NonlinearConstraint.h @@ -40,6 +40,8 @@ class GTSAM_EXPORT NonlinearConstraint : public NoiseModelFactor { /** Use constructors of NoiseModelFactor. */ using Base::Base; + using NonlinearFactor::error; + /** Destructor. */ virtual ~NonlinearConstraint() {} diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 725117748..3830532ae 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -280,6 +280,8 @@ public: */ double weight(const Values& c) const; + using NonlinearFactor::error; + /** * Calculate the error of the factor. * This is the log-likelihood, e.g. \f$ 0.5(h(x)-z)^2/\sigma^2 \f$ in case of Gaussian. From 021d1054289b5e7c44a44ef591d57d21c4bdbef5 Mon Sep 17 00:00:00 2001 From: yetongumich Date: Wed, 20 Nov 2024 11:24:44 -0800 Subject: [PATCH 11/52] fix export issues --- Using-GTSAM-EXPORT.md | 6 +++--- gtsam/constraint/NonlinearEqualityConstraint.h | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/Using-GTSAM-EXPORT.md b/Using-GTSAM-EXPORT.md index 24a29f96b..10dc4a853 100644 --- a/Using-GTSAM-EXPORT.md +++ b/Using-GTSAM-EXPORT.md @@ -3,9 +3,9 @@ To create a DLL in windows, the `GTSAM_EXPORT` keyword has been created and needs to be applied to different methods and classes in the code to expose this code outside of the DLL. However, there are several intricacies that make this more difficult than it sounds. In general, if you follow the following three rules, GTSAM_EXPORT should work properly. The rest of the document also describes (1) the common error message encountered when you are not following these rules and (2) the reasoning behind these usage rules. ## Usage Rules -1. Put `GTSAM_EXPORT` in front of any function that you want exported in the DLL _if and only if_ that function is declared in a .cpp file, not just a .h file. +1. Put `GTSAM_EXPORT` in front of any function that you want exported in the DLL _if and only if_ that function is defined in a .cpp file, not just a .h file. 2. Use `GTSAM_EXPORT` in a class definition (i.e. `class GSTAM_EXPORT MyClass {...}`) only if: - * At least one of the functions inside that class is declared in a .cpp file and not just the .h file. + * At least one of the functions inside that class is defined in a .cpp file and not just the .h file. * You can `GTSAM_EXPORT` any class it inherits from as well. (Note that this implictly requires the class does not derive from a "header-only" class. Note that Eigen is a "header-only" library, so if your class derives from Eigen, _do not_ use `GTSAM_EXPORT` in the class definition!) 3. If you have defined a class using `GTSAM_EXPORT`, do not use `GTSAM_EXPORT` in any of its individual function declarations. (Note that you _can_ put `GTSAM_EXPORT` in the definition of individual functions within a class as long as you don't put `GTSAM_EXPORT` in the class definition.) 4. For template specializations, you need to add `GTSAM_EXPORT` to each individual specialization. @@ -28,7 +28,7 @@ But first, we need to understand exactly what `GTSAM_EXPORT` is. `GTSAM_EXPORT` Rule #1 doesn't seem very bad, until you combine it with rule #2 -***Compiler Rule #2*** Anything declared in a header file is not included in a DLL. +***Compiler Rule #2*** Anything defined in a header file is not included in a DLL. When these two rules are combined, you get some very confusing results. For example, a class which is completely defined in a header (e.g. Foo) cannot use `GTSAM_EXPORT` in its definition. If Foo is defined with `GTSAM_EXPORT`, then the compiler _must_ find Foo in a DLL. Because Foo is a header-only class, however, it can't find it, leading to a very confusing "I can't find this symbol" type of error. Note that the linker says it can't find the symbol even though the compiler found the header file that completely defines the class. diff --git a/gtsam/constraint/NonlinearEqualityConstraint.h b/gtsam/constraint/NonlinearEqualityConstraint.h index 532c3ab38..30e8e52d3 100644 --- a/gtsam/constraint/NonlinearEqualityConstraint.h +++ b/gtsam/constraint/NonlinearEqualityConstraint.h @@ -52,7 +52,7 @@ class GTSAM_EXPORT NonlinearEqualityConstraint : public NonlinearConstraint { /** Equality constraint that force g(x) = M. */ template -class GTSAM_EXPORT ExpressionEqualityConstraint : public NonlinearEqualityConstraint { +class ExpressionEqualityConstraint : public NonlinearEqualityConstraint { public: typedef NonlinearEqualityConstraint Base; typedef ExpressionEqualityConstraint This; From 3c81405a019eddc398a89ba4df70deb3de806297 Mon Sep 17 00:00:00 2001 From: yetongumich Date: Tue, 31 Dec 2024 14:26:31 -0800 Subject: [PATCH 12/52] addressing comments --- gtsam/CMakeLists.txt | 2 +- .../CMakeLists.txt | 2 +- .../InequalityPenaltyFunction.cpp | 2 +- .../InequalityPenaltyFunction.h | 9 + .../NonlinearConstraint.h | 0 .../NonlinearEqualityConstraint-inl.h | 2 +- .../NonlinearEqualityConstraint.cpp | 2 +- .../NonlinearEqualityConstraint.h | 10 +- .../NonlinearInequalityConstraint.cpp | 2 +- .../NonlinearInequalityConstraint.h | 4 +- .../tests/CMakeLists.txt | 0 gtsam/constrained/tests/constrainedExample.h | 73 +++++++ .../tests/testInequalityPenaltyFunction.cpp | 2 +- .../tests/testNonlinearEqualityConstraint.cpp | 2 +- .../testNonlinearInequalityConstraint.cpp | 2 +- gtsam/constraint/tests/constrainedExample.h | 181 ------------------ gtsam/nonlinear/NonlinearEquality.h | 2 +- gtsam/slam/BoundingConstraint.h | 10 +- 18 files changed, 105 insertions(+), 202 deletions(-) rename gtsam/{constraint => constrained}/CMakeLists.txt (93%) rename gtsam/{constraint => constrained}/InequalityPenaltyFunction.cpp (97%) rename gtsam/{constraint => constrained}/InequalityPenaltyFunction.h (93%) rename gtsam/{constraint => constrained}/NonlinearConstraint.h (100%) rename gtsam/{constraint => constrained}/NonlinearEqualityConstraint-inl.h (97%) rename gtsam/{constraint => constrained}/NonlinearEqualityConstraint.cpp (98%) rename gtsam/{constraint => constrained}/NonlinearEqualityConstraint.h (96%) rename gtsam/{constraint => constrained}/NonlinearInequalityConstraint.cpp (99%) rename gtsam/{constraint => constrained}/NonlinearInequalityConstraint.h (98%) rename gtsam/{constraint => constrained}/tests/CMakeLists.txt (100%) create mode 100644 gtsam/constrained/tests/constrainedExample.h rename gtsam/{constraint => constrained}/tests/testInequalityPenaltyFunction.cpp (98%) rename gtsam/{constraint => constrained}/tests/testNonlinearEqualityConstraint.cpp (99%) rename gtsam/{constraint => constrained}/tests/testNonlinearInequalityConstraint.cpp (98%) delete mode 100644 gtsam/constraint/tests/constrainedExample.h diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index dcc350fe8..0911ef550 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -6,7 +6,7 @@ project(gtsam LANGUAGES CXX) set (gtsam_subdirs base basis - constraint + constrained geometry inference symbolic diff --git a/gtsam/constraint/CMakeLists.txt b/gtsam/constrained/CMakeLists.txt similarity index 93% rename from gtsam/constraint/CMakeLists.txt rename to gtsam/constrained/CMakeLists.txt index e4bbd89dd..168744076 100644 --- a/gtsam/constraint/CMakeLists.txt +++ b/gtsam/constrained/CMakeLists.txt @@ -1,6 +1,6 @@ # Install headers file(GLOB constraint_headers "*.h") -install(FILES ${constraint_headers} DESTINATION include/gtsam/constraint) +install(FILES ${constraint_headers} DESTINATION include/gtsam/constrained) # Build tests add_subdirectory(tests) diff --git a/gtsam/constraint/InequalityPenaltyFunction.cpp b/gtsam/constrained/InequalityPenaltyFunction.cpp similarity index 97% rename from gtsam/constraint/InequalityPenaltyFunction.cpp rename to gtsam/constrained/InequalityPenaltyFunction.cpp index 8b048686f..2e331292c 100644 --- a/gtsam/constraint/InequalityPenaltyFunction.cpp +++ b/gtsam/constrained/InequalityPenaltyFunction.cpp @@ -16,7 +16,7 @@ * @date Aug 3, 2024 */ -#include +#include namespace gtsam { diff --git a/gtsam/constraint/InequalityPenaltyFunction.h b/gtsam/constrained/InequalityPenaltyFunction.h similarity index 93% rename from gtsam/constraint/InequalityPenaltyFunction.h rename to gtsam/constrained/InequalityPenaltyFunction.h index baafaef0b..3aef099af 100644 --- a/gtsam/constraint/InequalityPenaltyFunction.h +++ b/gtsam/constrained/InequalityPenaltyFunction.h @@ -83,6 +83,9 @@ class GTSAM_EXPORT SmoothRampPoly2 : public InequalityPenaltyFunction { double a_; public: + /** Constructor. + * @param epsilon parameter for adjusting the smoothness of the function. + */ SmoothRampPoly2(const double epsilon = 1) : Base(), epsilon_(epsilon), a_(0.5 / epsilon) {} @@ -110,6 +113,9 @@ class GTSAM_EXPORT SmoothRampPoly3 : public InequalityPenaltyFunction { double b_; public: + /** Constructor. + * @param epsilon parameter for adjusting the smoothness of the function. + */ SmoothRampPoly3(const double epsilon = 1) : Base(), epsilon_(epsilon), @@ -131,6 +137,9 @@ class GTSAM_EXPORT SoftPlusFunction : public InequalityPenaltyFunction { double k_; public: + /** Constructor. + * @param k parameter for adjusting the smoothness of the function. + */ SoftPlusFunction(const double k = 1) : Base(), k_(k) {} virtual double operator()(const double& x, diff --git a/gtsam/constraint/NonlinearConstraint.h b/gtsam/constrained/NonlinearConstraint.h similarity index 100% rename from gtsam/constraint/NonlinearConstraint.h rename to gtsam/constrained/NonlinearConstraint.h diff --git a/gtsam/constraint/NonlinearEqualityConstraint-inl.h b/gtsam/constrained/NonlinearEqualityConstraint-inl.h similarity index 97% rename from gtsam/constraint/NonlinearEqualityConstraint-inl.h rename to gtsam/constrained/NonlinearEqualityConstraint-inl.h index c228cf97d..14bc42939 100644 --- a/gtsam/constraint/NonlinearEqualityConstraint-inl.h +++ b/gtsam/constrained/NonlinearEqualityConstraint-inl.h @@ -17,7 +17,7 @@ #pragma once -#include +#include namespace gtsam { diff --git a/gtsam/constraint/NonlinearEqualityConstraint.cpp b/gtsam/constrained/NonlinearEqualityConstraint.cpp similarity index 98% rename from gtsam/constraint/NonlinearEqualityConstraint.cpp rename to gtsam/constrained/NonlinearEqualityConstraint.cpp index 170f2317c..effc8774c 100644 --- a/gtsam/constraint/NonlinearEqualityConstraint.cpp +++ b/gtsam/constrained/NonlinearEqualityConstraint.cpp @@ -15,7 +15,7 @@ * @author Yetong Zhang, Frank Dellaert * @date Aug 3, 2024 */ -#include +#include #include namespace gtsam { diff --git a/gtsam/constraint/NonlinearEqualityConstraint.h b/gtsam/constrained/NonlinearEqualityConstraint.h similarity index 96% rename from gtsam/constraint/NonlinearEqualityConstraint.h rename to gtsam/constrained/NonlinearEqualityConstraint.h index 30e8e52d3..017ca1607 100644 --- a/gtsam/constraint/NonlinearEqualityConstraint.h +++ b/gtsam/constrained/NonlinearEqualityConstraint.h @@ -17,7 +17,7 @@ #pragma once -#include +#include #include #include @@ -99,7 +99,10 @@ class ExpressionEqualityConstraint : public NonlinearEqualityConstraint { #endif }; -/** Equality constraint that enforce the cost factor with zero error. */ +/** Equality constraint that enforce the cost factor with zero error. + * e.g., for a factor with unwhitened cost 2x-1, the constraint enforces the + * equlity 2x-1=0. + */ class GTSAM_EXPORT ZeroCostConstraint : public NonlinearEqualityConstraint { public: typedef NonlinearEqualityConstraint Base; @@ -114,7 +117,6 @@ class GTSAM_EXPORT ZeroCostConstraint : public NonlinearEqualityConstraint { * @brief Constructor. * * @param factor NoiseModel factor. - * @param tolerance vector representing tolerance in each dimension. */ ZeroCostConstraint(const NoiseModelFactor::shared_ptr& factor); @@ -177,4 +179,4 @@ class GTSAM_EXPORT NonlinearEqualityConstraints : public FactorGraph +#include diff --git a/gtsam/constraint/NonlinearInequalityConstraint.cpp b/gtsam/constrained/NonlinearInequalityConstraint.cpp similarity index 99% rename from gtsam/constraint/NonlinearInequalityConstraint.cpp rename to gtsam/constrained/NonlinearInequalityConstraint.cpp index 2f96f9a29..b1dcb7534 100644 --- a/gtsam/constraint/NonlinearInequalityConstraint.cpp +++ b/gtsam/constrained/NonlinearInequalityConstraint.cpp @@ -16,7 +16,7 @@ * @date Aug 3, 2024 */ -#include +#include #include namespace gtsam { diff --git a/gtsam/constraint/NonlinearInequalityConstraint.h b/gtsam/constrained/NonlinearInequalityConstraint.h similarity index 98% rename from gtsam/constraint/NonlinearInequalityConstraint.h rename to gtsam/constrained/NonlinearInequalityConstraint.h index 658a29f2f..9dbf1f008 100644 --- a/gtsam/constraint/NonlinearInequalityConstraint.h +++ b/gtsam/constrained/NonlinearInequalityConstraint.h @@ -18,8 +18,8 @@ #pragma once -#include -#include +#include +#include #include namespace gtsam { diff --git a/gtsam/constraint/tests/CMakeLists.txt b/gtsam/constrained/tests/CMakeLists.txt similarity index 100% rename from gtsam/constraint/tests/CMakeLists.txt rename to gtsam/constrained/tests/CMakeLists.txt diff --git a/gtsam/constrained/tests/constrainedExample.h b/gtsam/constrained/tests/constrainedExample.h new file mode 100644 index 000000000..0b708a41f --- /dev/null +++ b/gtsam/constrained/tests/constrainedExample.h @@ -0,0 +1,73 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @file constrainedExample.h + * @brief Simple constrained optimization scenarios. + * @author Yetong Zhang + * @date Aug 3, 2024 + */ + +#pragma once + +// #include +#include +#include +#include +#include + +namespace constrained_example { + +using namespace gtsam; + +/// Exponential function e^x. +inline double exp_func(const double& x, gtsam::OptionalJacobian<1, 1> H1 = {}) { + double result = exp(x); + if (H1) H1->setConstant(result); + return result; +} + +/// Exponential expression e^x. +inline Double_ exp(const Double_& x) { return Double_(exp_func, x); } + +/// Pow functor used for pow function. +class PowFunctor { + private: + double c_; + + public: + PowFunctor(const double& c) : c_(c) {} + + double operator()(const double& x, gtsam::OptionalJacobian<1, 1> H1 = {}) const { + if (H1) H1->setConstant(c_ * pow(x, c_ - 1)); + return pow(x, c_); + } +}; + +/// Pow function. +inline Double_ pow(const Double_& x, const double& c) { + PowFunctor pow_functor(c); + return Double_(pow_functor, x); +} + +/// Plus between Double expression and double. +inline Double_ operator+(const Double_& x, const double& d) { return x + Double_(d); } + +/// Negative sign operator. +inline Double_ operator-(const Double_& x) { return Double_(0.0) - x; } + +/// Keys for creating expressions. +Symbol x1_key('x', 1); +Symbol x2_key('x', 2); +Double_ x1(x1_key), x2(x2_key); + +} // namespace constrained_example + diff --git a/gtsam/constraint/tests/testInequalityPenaltyFunction.cpp b/gtsam/constrained/tests/testInequalityPenaltyFunction.cpp similarity index 98% rename from gtsam/constraint/tests/testInequalityPenaltyFunction.cpp rename to gtsam/constrained/tests/testInequalityPenaltyFunction.cpp index 09cd17d5b..9b41dbe2c 100644 --- a/gtsam/constraint/tests/testInequalityPenaltyFunction.cpp +++ b/gtsam/constrained/tests/testInequalityPenaltyFunction.cpp @@ -20,7 +20,7 @@ #include #include #include -#include +#include using namespace gtsam; diff --git a/gtsam/constraint/tests/testNonlinearEqualityConstraint.cpp b/gtsam/constrained/tests/testNonlinearEqualityConstraint.cpp similarity index 99% rename from gtsam/constraint/tests/testNonlinearEqualityConstraint.cpp rename to gtsam/constrained/tests/testNonlinearEqualityConstraint.cpp index 70dabff45..e09c197a3 100644 --- a/gtsam/constraint/tests/testNonlinearEqualityConstraint.cpp +++ b/gtsam/constrained/tests/testNonlinearEqualityConstraint.cpp @@ -21,7 +21,7 @@ #include #include #include -#include +#include #include #include diff --git a/gtsam/constraint/tests/testNonlinearInequalityConstraint.cpp b/gtsam/constrained/tests/testNonlinearInequalityConstraint.cpp similarity index 98% rename from gtsam/constraint/tests/testNonlinearInequalityConstraint.cpp rename to gtsam/constrained/tests/testNonlinearInequalityConstraint.cpp index 3950bfa03..4d9034daa 100644 --- a/gtsam/constraint/tests/testNonlinearInequalityConstraint.cpp +++ b/gtsam/constrained/tests/testNonlinearInequalityConstraint.cpp @@ -20,7 +20,7 @@ #include #include #include -#include +#include #include #include "constrainedExample.h" diff --git a/gtsam/constraint/tests/constrainedExample.h b/gtsam/constraint/tests/constrainedExample.h deleted file mode 100644 index 2cc3ba826..000000000 --- a/gtsam/constraint/tests/constrainedExample.h +++ /dev/null @@ -1,181 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 - - * -------------------------------------------------------------------------- */ - -/** - * @file constrainedExample.h - * @brief Simple constrained optimization scenarios. - * @author Yetong Zhang - * @date Aug 3, 2024 - */ - -#pragma once - -// #include -#include -#include -#include -#include - -namespace constrained_example { - -using namespace gtsam; - -/// Exponential function e^x. -inline double exp_func(const double& x, gtsam::OptionalJacobian<1, 1> H1 = {}) { - double result = exp(x); - if (H1) H1->setConstant(result); - return result; -} - -/// Exponential expression e^x. -inline Double_ exp(const Double_& x) { return Double_(exp_func, x); } - -/// Pow functor used for pow function. -class PowFunctor { - private: - double c_; - - public: - PowFunctor(const double& c) : c_(c) {} - - double operator()(const double& x, gtsam::OptionalJacobian<1, 1> H1 = {}) const { - if (H1) H1->setConstant(c_ * pow(x, c_ - 1)); - return pow(x, c_); - } -}; - -/// Pow function. -inline Double_ pow(const Double_& x, const double& c) { - PowFunctor pow_functor(c); - return Double_(pow_functor, x); -} - -/// Plus between Double expression and double. -inline Double_ operator+(const Double_& x, const double& d) { return x + Double_(d); } - -/// Negative sign operator. -inline Double_ operator-(const Double_& x) { return Double_(0.0) - x; } - -/// Keys for creating expressions. -Symbol x1_key('x', 1); -Symbol x2_key('x', 2); -Double_ x1(x1_key), x2(x2_key); - -} // namespace constrained_example - -/* ************************************************************************* */ -/** - * Constrained optimization example in L. Vandenberghe slides: - * https://www.seas.ucla.edu/~vandenbe/133B/lectures/nllseq.pdf - * f(x) = 0.5*||x1 + e^(-x2)||^2 + 0.5*||x1^2 + 2*x2 + 1||^2 - * h(x) = x1 + x1^3 + x2 + x2^2 = 0 - */ -// namespace constrained_example1 { -// using namespace constrained_example; -// NonlinearFactorGraph Cost() { -// NonlinearFactorGraph graph; -// auto f1 = x1 + exp(-x2); -// auto f2 = pow(x1, 2.0) + 2.0 * x2 + 1.0; -// auto cost_noise = gtsam::noiseModel::Isotropic::Sigma(1, 1.0); -// graph.add(ExpressionFactor(cost_noise, 0., f1)); -// graph.add(ExpressionFactor(cost_noise, 0., f2)); -// return graph; -// } - -// NonlinearEqualityConstraints EqConstraints() { -// NonlinearEqualityConstraints constraints; -// Vector sigmas = Vector1(1.0); -// auto h1 = x1 + pow(x1, 3) + x2 + pow(x2, 2); -// constraints.push_back(NonlinearEqualityConstraint::shared_ptr( -// new ExpressionEqualityConstraint(h1, 0.0, sigmas))); -// return constraints; -// } - -// Values InitValues() { -// Values values; -// values.insert(x1_key, -0.2); -// values.insert(x2_key, -0.2); -// return values; -// } - -// Values OptimalValues() { -// Values values; -// values.insert(x1_key, 0.0); -// values.insert(x2_key, 0.0); -// return values; -// } - -// NonlinearFactorGraph costs = Cost(); -// NonlinearEqualityConstraints e_constraints = EqConstraints(); -// NonlinearInequalityConstraints i_constraints; -// Values init_values = InitValues(); -// ConstrainedOptProblem::shared_ptr problem = -// std::make_shared(costs, e_constraints, i_constraints, init_values); -// Values optimal_values = OptimalValues(); -// } // namespace constrained_example1 - -// /* ************************************************************************* */ -// /** -// * Constrained optimization example with inequality constraints -// * Approach a point while staying on unit circle, and within an ellipse. -// * f(x) = 0.5 * ||x1-1||^2 + 0.5 * ||x2-1||^2 -// * h(x) = x1^2 + x2^2 - 1 = 0 -// * g(x) = 4*x1^2 + 0.25*x2^2 - 1 <= 0 -// */ -// namespace constrained_example2 { -// using namespace constrained_example; -// NonlinearFactorGraph Cost() { -// NonlinearFactorGraph graph; -// auto cost_noise = gtsam::noiseModel::Isotropic::Sigma(1, 1.0); -// graph.addPrior(x1_key, 1.0, cost_noise); -// graph.addPrior(x2_key, 1.0, cost_noise); -// return graph; -// } - -// NonlinearEqualityConstraints EqConstraints() { -// NonlinearEqualityConstraints constraints; -// Vector1 sigmas(1.0); -// Double_ h1 = x1 * x1 + x2 * x2; -// constraints.emplace_shared>(h1, 1.0, sigmas); -// return constraints; -// } - -// NonlinearInequalityConstraints IneqConstraints() { -// NonlinearInequalityConstraints constraints; -// Double_ g1 = 4 * x1 * x1 + 0.25 * x2 * x2 - Double_(1.0); -// double sigma = 1; -// constraints.emplace_shared(g1, sigma); -// return constraints; -// } - -// Values InitValues() { -// Values values; -// values.insert(x1_key, -1.0); -// values.insert(x2_key, 2.0); -// return values; -// } - -// Values OptimalValues() { -// Values values; -// values.insert(x1_key, 1.0 / sqrt(5)); -// values.insert(x2_key, 2.0 / sqrt(5)); -// return values; -// } - -// NonlinearFactorGraph costs = Cost(); -// NonlinearEqualityConstraints e_constraints = EqConstraints(); -// NonlinearInequalityConstraints i_constraints = IneqConstraints(); -// Values init_values = InitValues(); -// ConstrainedOptProblem::shared_ptr problem = -// std::make_shared(costs, e_constraints, i_constraints, init_values); -// Values optimal_values = OptimalValues(); - -// } // namespace constrained_example2 diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index c91bddd9c..cac54f332 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -18,7 +18,7 @@ #pragma once #include -#include +#include #include #include diff --git a/gtsam/slam/BoundingConstraint.h b/gtsam/slam/BoundingConstraint.h index d35152adf..4c6c775a0 100644 --- a/gtsam/slam/BoundingConstraint.h +++ b/gtsam/slam/BoundingConstraint.h @@ -19,7 +19,7 @@ #include #include -#include +#include namespace gtsam { @@ -59,7 +59,7 @@ struct BoundingConstraint1: public NonlinearInequalityConstraint { virtual double value(const X& x, OptionalMatrixType H = OptionalNone) const = 0; - Vector unwhitenedExpr(const Values& x, OptionalMatrixVecType H = nullptr) const override { + Vector unwhitenedExpr(const Values& x, OptionalMatrixVecType H = {}) const override { if (H) { double d = value(x.at(this->key()), &(H->front())); if (isGreaterThan_) { @@ -75,7 +75,7 @@ struct BoundingConstraint1: public NonlinearInequalityConstraint { } /// TODO: This should be deprecated. - Vector evaluateError(const X& x, OptionalMatrixType H = nullptr) const { + Vector evaluateError(const X& x, OptionalMatrixType H = {}) const { Matrix D; double error = value(x, &D) - threshold_; if (H) { @@ -138,7 +138,7 @@ struct BoundingConstraint2: public NonlinearInequalityConstraint { OptionalMatrixType H1 = OptionalNone, OptionalMatrixType H2 = OptionalNone) const = 0; - Vector unwhitenedExpr(const Values& x, OptionalMatrixVecType H = nullptr) const override { + Vector unwhitenedExpr(const Values& x, OptionalMatrixVecType H = {}) const override { X1 x1 = x.at(keys().front()); X2 x2 = x.at(keys().back()); if (H) { @@ -158,7 +158,7 @@ struct BoundingConstraint2: public NonlinearInequalityConstraint { /// TODO: This should be deprecated. Vector evaluateError(const X1& x1, const X2& x2, - OptionalMatrixType H1 = nullptr, OptionalMatrixType H2 = nullptr) const { + OptionalMatrixType H1 = {}, OptionalMatrixType H2 = {}) const { Matrix D1, D2; double error = value(x1, x2, &D1, &D2) - threshold_; if (H1) { From 3c9b2a2351d767843c009f0c339797c5f7399f0c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 11 Jan 2025 17:48:10 -0500 Subject: [PATCH 13/52] fix examples using discrete factor graph product --- gtsam_unstable/discrete/examples/schedulingExample.cpp | 3 ++- gtsam_unstable/discrete/examples/schedulingQuals12.cpp | 3 ++- gtsam_unstable/discrete/examples/schedulingQuals13.cpp | 3 ++- 3 files changed, 6 insertions(+), 3 deletions(-) diff --git a/gtsam_unstable/discrete/examples/schedulingExample.cpp b/gtsam_unstable/discrete/examples/schedulingExample.cpp index 2b52a3e3a..47bcd0a85 100644 --- a/gtsam_unstable/discrete/examples/schedulingExample.cpp +++ b/gtsam_unstable/discrete/examples/schedulingExample.cpp @@ -108,7 +108,8 @@ void runLargeExample() { // Do brute force product and output that to file if (scheduler.nrStudents() == 1) { // otherwise too slow - DecisionTreeFactor product = scheduler.product(); + DecisionTreeFactor product = + *std::dynamic_pointer_cast(scheduler.product()); product.dot("scheduling-large", DefaultKeyFormatter, false); } diff --git a/gtsam_unstable/discrete/examples/schedulingQuals12.cpp b/gtsam_unstable/discrete/examples/schedulingQuals12.cpp index 913bfb21a..869c13459 100644 --- a/gtsam_unstable/discrete/examples/schedulingQuals12.cpp +++ b/gtsam_unstable/discrete/examples/schedulingQuals12.cpp @@ -108,7 +108,8 @@ void runLargeExample() { // Do brute force product and output that to file if (scheduler.nrStudents() == 1) { // otherwise too slow - DecisionTreeFactor product = scheduler.product(); + DecisionTreeFactor product = + *std::dynamic_pointer_cast(scheduler.product()); product.dot("scheduling-large", DefaultKeyFormatter, false); } diff --git a/gtsam_unstable/discrete/examples/schedulingQuals13.cpp b/gtsam_unstable/discrete/examples/schedulingQuals13.cpp index e52e3d0c6..487215e03 100644 --- a/gtsam_unstable/discrete/examples/schedulingQuals13.cpp +++ b/gtsam_unstable/discrete/examples/schedulingQuals13.cpp @@ -132,7 +132,8 @@ void runLargeExample() { // Do brute force product and output that to file if (scheduler.nrStudents() == 1) { // otherwise too slow - DecisionTreeFactor product = scheduler.product(); + DecisionTreeFactor product = + *std::dynamic_pointer_cast(scheduler.product()); product.dot("scheduling-large", DefaultKeyFormatter, false); } From 5254b4f0b1fba5389ade67586c0f9e03394570e1 Mon Sep 17 00:00:00 2001 From: morten Date: Mon, 13 Jan 2025 16:55:40 +0100 Subject: [PATCH 14/52] adding lever arm optional argument to gps factors --- gtsam/navigation/GPSFactor.cpp | 34 +++++++++++++++++++----- gtsam/navigation/GPSFactor.h | 22 ++++++++++----- gtsam/navigation/tests/testGPSFactor.cpp | 15 ++++++++--- 3 files changed, 55 insertions(+), 16 deletions(-) diff --git a/gtsam/navigation/GPSFactor.cpp b/gtsam/navigation/GPSFactor.cpp index 47de385ef..ff1f9923b 100644 --- a/gtsam/navigation/GPSFactor.cpp +++ b/gtsam/navigation/GPSFactor.cpp @@ -26,20 +26,31 @@ namespace gtsam { void GPSFactor::print(const string& s, const KeyFormatter& keyFormatter) const { cout << (s.empty() ? "" : s + " ") << "GPSFactor on " << keyFormatter(key()) << "\n"; - cout << " GPS measurement: " << nT_ << "\n"; + cout << " GPS measurement: " << nT_.transpose() << "\n"; + cout << " Lever arm: " << B_t_BG_.transpose() << "\n"; noiseModel_->print(" noise model: "); } //*************************************************************************** bool GPSFactor::equals(const NonlinearFactor& expected, double tol) const { const This* e = dynamic_cast(&expected); - return e != nullptr && Base::equals(*e, tol) && traits::Equals(nT_, e->nT_, tol); + return e != nullptr && Base::equals(*e, tol) && + traits::Equals(nT_, e->nT_, tol) && + traits::Equals(B_t_BG_, e->B_t_BG_, tol); } //*************************************************************************** Vector GPSFactor::evaluateError(const Pose3& p, OptionalMatrixType H) const { - return p.translation(H) -nT_; + const Matrix3 rot = p.rotation().matrix(); + if (H) { + H->resize(3, 6); + + H->block<3, 3>(0, 0) = -rot * skewSymmetric(B_t_BG_); + H->block<3, 3>(0, 3) = rot; + } + + return p.translation() - (nT_ - rot * B_t_BG_); } //*************************************************************************** @@ -67,7 +78,8 @@ pair GPSFactor::EstimateState(double t1, const Point3& NED1, //*************************************************************************** void GPSFactor2::print(const string& s, const KeyFormatter& keyFormatter) const { cout << s << "GPSFactor2 on " << keyFormatter(key()) << "\n"; - cout << " GPS measurement: " << nT_.transpose() << endl; + cout << " GPS measurement: " << nT_.transpose() << "\n"; + cout << " Lever arm: " << B_t_BG_.transpose() << "\n"; noiseModel_->print(" noise model: "); } @@ -75,13 +87,23 @@ void GPSFactor2::print(const string& s, const KeyFormatter& keyFormatter) const bool GPSFactor2::equals(const NonlinearFactor& expected, double tol) const { const This* e = dynamic_cast(&expected); return e != nullptr && Base::equals(*e, tol) && - traits::Equals(nT_, e->nT_, tol); + traits::Equals(nT_, e->nT_, tol) && + traits::Equals(B_t_BG_, e->B_t_BG_, tol); } //*************************************************************************** Vector GPSFactor2::evaluateError(const NavState& p, OptionalMatrixType H) const { - return p.position(H) -nT_; + const Matrix3 rot = p.attitude().matrix(); + if (H) { + H->resize(3, 9); + + H->block<3, 3>(0, 0) = -rot * skewSymmetric(B_t_BG_); + H->block<3, 3>(0, 3) = rot; + H->block<3, 3>(0, 6).setZero(); + } + + return p.position() - (nT_ - rot * B_t_BG_); } //*************************************************************************** diff --git a/gtsam/navigation/GPSFactor.h b/gtsam/navigation/GPSFactor.h index 6af184360..e954d719b 100644 --- a/gtsam/navigation/GPSFactor.h +++ b/gtsam/navigation/GPSFactor.h @@ -39,6 +39,7 @@ private: typedef NoiseModelFactorN Base; Point3 nT_; ///< Position measurement in cartesian coordinates + Point3 B_t_BG_; ///< Lever arm between GPS and BODY frame public: @@ -52,7 +53,7 @@ public: typedef GPSFactor This; /** default constructor - only use for serialization */ - GPSFactor(): nT_(0, 0, 0) {} + GPSFactor(): nT_(0, 0, 0), B_t_BG_(0, 0, 0) {} ~GPSFactor() override {} @@ -63,8 +64,8 @@ public: * @param gpsIn measurement already in correct coordinates * @param model Gaussian noise model */ - GPSFactor(Key key, const Point3& gpsIn, const SharedNoiseModel& model) : - Base(model, key), nT_(gpsIn) { + GPSFactor(Key key, const Point3& gpsIn, const SharedNoiseModel& model, const Point3& leverArm) : + Base(model, key), nT_(gpsIn), B_t_BG_(leverArm) { } /// @return a deep copy of this factor @@ -87,6 +88,10 @@ public: return nT_; } + inline const Point3 & leverArm() const { + return B_t_BG_; + } + /** * Convenience function to estimate state at time t, given two GPS * readings (in local NED Cartesian frame) bracketing t @@ -122,6 +127,7 @@ private: typedef NoiseModelFactorN Base; Point3 nT_; ///< Position measurement in cartesian coordinates + Point3 B_t_BG_; ///< Lever arm between GPS and BODY frame public: @@ -135,13 +141,13 @@ public: typedef GPSFactor2 This; /// default constructor - only use for serialization - GPSFactor2():nT_(0, 0, 0) {} + GPSFactor2():nT_(0, 0, 0), B_t_BG_(0, 0, 0) {} ~GPSFactor2() override {} /// Constructor from a measurement in a Cartesian frame. - GPSFactor2(Key key, const Point3& gpsIn, const SharedNoiseModel& model) : - Base(model, key), nT_(gpsIn) { + GPSFactor2(Key key, const Point3& gpsIn, const SharedNoiseModel& model, const Point3& leverArm) : + Base(model, key), nT_(gpsIn), B_t_BG_(leverArm) { } /// @return a deep copy of this factor @@ -164,6 +170,10 @@ public: return nT_; } + inline const Point3 & leverArm() const { + return B_t_BG_; + } + private: #if GTSAM_ENABLE_BOOST_SERIALIZATION diff --git a/gtsam/navigation/tests/testGPSFactor.cpp b/gtsam/navigation/tests/testGPSFactor.cpp index cecfbeaad..2972d0fd6 100644 --- a/gtsam/navigation/tests/testGPSFactor.cpp +++ b/gtsam/navigation/tests/testGPSFactor.cpp @@ -45,6 +45,9 @@ LocalCartesian origin_ENU(lat0, lon0, h0, kWGS84); // Dekalb-Peachtree Airport runway 2L const double lat = 33.87071, lon = -84.30482, h = 274; + +// Random lever arm +const Point3 leverArm(0.1, 0.2, 0.3); } // ************************************************************************* @@ -61,10 +64,12 @@ TEST( GPSFactor, Constructor ) { // Factor Key key(1); SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25); - GPSFactor factor(key, Point3(E, N, U), model); + GPSFactor factor(key, Point3(E, N, U), model, leverArm); // Create a linearization point at zero error - Pose3 T(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(E, N, U)); + const Rot3 rot = Rot3::RzRyRx(0.15, -0.30, 0.45); + const Point3 p = Point3(E, N, U) - rot * leverArm; + Pose3 T(rot, p); EXPECT(assert_equal(Z_3x1,factor.evaluateError(T),1e-5)); // Calculate numerical derivatives @@ -90,10 +95,12 @@ TEST( GPSFactor2, Constructor ) { // Factor Key key(1); SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25); - GPSFactor2 factor(key, Point3(E, N, U), model); + GPSFactor2 factor(key, Point3(E, N, U), model, leverArm); // Create a linearization point at zero error - NavState T(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(E, N, U), Vector3::Zero()); + const Rot3 rot = Rot3::RzRyRx(0.15, -0.30, 0.45); + const Point3 p = Point3(E, N, U) - rot * leverArm; + NavState T(rot, p, Vector3::Zero()); EXPECT(assert_equal(Z_3x1,factor.evaluateError(T),1e-5)); // Calculate numerical derivatives From 088332978bbcd041cca6fbf3a7cb6a52d7fa2ae6 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 13 Jan 2025 13:02:39 -0500 Subject: [PATCH 15/52] Check issue 1452 --- examples/Data/issue1452.txt | 200 +++++++++ .../tests/testIncrementalFixedLagSmoother.cpp | 413 +++++++++++++----- 2 files changed, 492 insertions(+), 121 deletions(-) create mode 100644 examples/Data/issue1452.txt diff --git a/examples/Data/issue1452.txt b/examples/Data/issue1452.txt new file mode 100644 index 000000000..518f1fe3e --- /dev/null +++ b/examples/Data/issue1452.txt @@ -0,0 +1,200 @@ +0 1645005900.600844860077 0 6.217194869853 0.125917563436 0.783594893995 -0.033789843270 0.047134300047 0.266189953411 0.000021415732 0.000001074791 -0.000000147245 -0.000000847030 0.000019167883 0.000016718464 0.000001074791 0.000001605304 -0.000000003066 -0.000001377764 0.000000939474 0.000008913959 -0.000000147245 -0.000000003066 0.000001484791 -0.000000768622 -0.000007854923 -0.000000092444 -0.000000847030 -0.000001377764 -0.000000768622 0.000104453167 0.000003272897 -0.000007611301 0.000019167883 0.000000939474 -0.000007854923 0.000003272897 0.000160225289 0.000014846515 0.000016718464 0.000008913959 -0.000000092444 -0.000007611301 0.000014846515 0.000157951799 +1 1645005900.701431751251 0 1 0.005593021030 0.001526481838 -0.001566024571 -0.000225927310 0.000128593690 -0.003791450430 0.000000179286 -0.000000039948 0.000000006043 -0.000000004575 -0.000000022632 0.000000091709 -0.000000039948 0.000000018979 -0.000000001546 0.000000001926 0.000000006078 -0.000000017221 0.000000006043 -0.000000001546 0.000000015351 -0.000000013913 0.000000001925 0.000000001075 -0.000000004575 0.000000001926 -0.000000013913 0.000000561628 -0.000000028062 0.000000012214 -0.000000022632 0.000000006078 0.000000001925 -0.000000028062 0.000000490365 -0.000000005530 0.000000091709 -0.000000017221 0.000000001075 0.000000012214 -0.000000005530 0.000000550990 +1 1645005900.799895286560 1 2 0.005283034558 0.001722784230 -0.000367567191 0.000386621238 0.000182655752 -0.003429626268 0.000000162458 -0.000000036144 0.000000005112 -0.000000002457 -0.000000020766 0.000000082373 -0.000000036144 0.000000017835 -0.000000001282 0.000000001448 0.000000005345 -0.000000014851 0.000000005112 -0.000000001282 0.000000014551 -0.000000013437 0.000000001790 0.000000000998 -0.000000002457 0.000000001448 -0.000000013437 0.000000523714 -0.000000027843 0.000000011143 -0.000000020766 0.000000005345 0.000000001790 -0.000000027843 0.000000456118 -0.000000005134 0.000000082373 -0.000000014851 0.000000000998 0.000000011143 -0.000000005134 0.000000510306 +1 1645005900.902594566345 2 3 0.003030780414 0.000830079823 0.000494312020 -0.000020397824 0.000094347098 -0.002984943110 0.000000145900 -0.000000032354 0.000000004126 -0.000000001486 -0.000000018203 0.000000070890 -0.000000032354 0.000000016529 -0.000000001105 0.000000001395 0.000000004540 -0.000000012806 0.000000004126 -0.000000001105 0.000000014132 -0.000000012278 0.000000001993 0.000000000686 -0.000000001486 0.000000001395 -0.000000012278 0.000000469953 -0.000000025678 0.000000008948 -0.000000018203 0.000000004540 0.000000001993 -0.000000025678 0.000000410806 -0.000000003776 0.000000070890 -0.000000012806 0.000000000686 0.000000008948 -0.000000003776 0.000000445310 +1 1645005901.003520727158 3 4 -0.000206245469 -0.001102739599 0.000139509744 -0.000044223949 0.000133857483 -0.002842172502 0.000000130290 -0.000000028944 0.000000003338 -0.000000000544 -0.000000016340 0.000000062247 -0.000000028944 0.000000015436 -0.000000000943 0.000000001260 0.000000004005 -0.000000011220 0.000000003338 -0.000000000943 0.000000014049 -0.000000012189 0.000000001599 0.000000000394 -0.000000000544 0.000000001260 -0.000000012189 0.000000464881 -0.000000026439 0.000000008623 -0.000000016340 0.000000004005 0.000000001599 -0.000000026439 0.000000404384 -0.000000003057 0.000000062247 -0.000000011220 0.000000000394 0.000000008623 -0.000000003057 0.000000423376 +1 1645005901.101174592972 4 5 -0.001500063017 -0.002890410973 -0.000122347193 0.000014693626 0.000245148283 -0.002217688406 0.000000119368 -0.000000026625 0.000000002896 0.000000000371 -0.000000014770 0.000000055853 -0.000000026625 0.000000014763 -0.000000000823 0.000000000938 0.000000003581 -0.000000009513 0.000000002896 -0.000000000823 0.000000013680 -0.000000011988 0.000000001262 0.000000000298 0.000000000371 0.000000000938 -0.000000011988 0.000000464346 -0.000000027637 0.000000007687 -0.000000014770 0.000000003581 0.000000001262 -0.000000027637 0.000000402444 -0.000000002216 0.000000055853 -0.000000009513 0.000000000298 0.000000007687 -0.000000002216 0.000000418928 +1 1645005901.201069116592 5 6 0.001046374496 -0.004032584187 -0.000846599681 0.000321181584 -0.000094749466 -0.002057391696 0.000000110823 -0.000000024485 0.000000002417 0.000000001106 -0.000000012269 0.000000051317 -0.000000024485 0.000000014167 -0.000000000692 0.000000000635 0.000000003038 -0.000000008394 0.000000002417 -0.000000000692 0.000000013921 -0.000000011892 0.000000000364 0.000000000007 0.000000001106 0.000000000635 -0.000000011892 0.000000448440 -0.000000027796 0.000000007091 -0.000000012269 0.000000003038 0.000000000364 -0.000000027796 0.000000388841 -0.000000000851 0.000000051317 -0.000000008394 0.000000000007 0.000000007091 -0.000000000851 0.000000405686 +1 1645005901.302946805954 6 7 0.000793901956 -0.003389991954 -0.001184675507 -0.000316637882 0.000125753514 -0.001152057016 0.000000104712 -0.000000022968 0.000000001967 0.000000001772 -0.000000010574 0.000000048337 -0.000000022968 0.000000013707 -0.000000000599 0.000000000391 0.000000002734 -0.000000007830 0.000000001967 -0.000000000599 0.000000014353 -0.000000012287 -0.000000001119 -0.000000000386 0.000000001772 0.000000000391 -0.000000012287 0.000000430486 -0.000000025901 0.000000008441 -0.000000010574 0.000000002734 -0.000000001119 -0.000000025901 0.000000374002 0.000000000168 0.000000048337 -0.000000007830 -0.000000000386 0.000000008441 0.000000000168 0.000000386878 +1 1645005901.403146505356 7 8 0.000422785492 -0.001577894861 -0.000916262031 0.000148651815 -0.000064627432 -0.001220538595 0.000000100167 -0.000000021927 0.000000002289 0.000000000677 -0.000000011203 0.000000045605 -0.000000021927 0.000000013460 -0.000000000705 0.000000000708 0.000000002868 -0.000000007296 0.000000002289 -0.000000000705 0.000000014003 -0.000000011457 -0.000000000961 -0.000000000148 0.000000000677 0.000000000708 -0.000000011457 0.000000410964 -0.000000025258 0.000000008570 -0.000000011203 0.000000002868 -0.000000000961 -0.000000025258 0.000000357815 -0.000000000299 0.000000045605 -0.000000007296 -0.000000000148 0.000000008570 -0.000000000299 0.000000367628 +1 1645005901.502384662628 8 9 -0.002329688882 -0.000140222280 0.000009892231 -0.000239651077 0.000253304248 -0.001105202980 0.000000095969 -0.000000020860 0.000000002507 -0.000000000886 -0.000000012048 0.000000043109 -0.000000020860 0.000000013064 -0.000000000726 0.000000001166 0.000000002962 -0.000000007369 0.000000002507 -0.000000000726 0.000000013480 -0.000000010270 -0.000000000517 0.000000000184 -0.000000000886 0.000000001166 -0.000000010270 0.000000387773 -0.000000024930 0.000000007342 -0.000000012048 0.000000002962 -0.000000000517 -0.000000024930 0.000000338486 -0.000000001780 0.000000043109 -0.000000007369 0.000000000184 0.000000007342 -0.000000001780 0.000000347313 +1 1645005901.599597692490 9 10 -0.004026581327 0.000969420632 0.001171132830 0.000030066830 -0.000042684471 -0.001078015407 0.000000092798 -0.000000020050 0.000000002088 -0.000000000514 -0.000000011837 0.000000040468 -0.000000020050 0.000000012604 -0.000000000661 0.000000001084 0.000000002794 -0.000000007809 0.000000002088 -0.000000000661 0.000000013337 -0.000000009999 -0.000000000185 0.000000000097 -0.000000000514 0.000000001084 -0.000000009999 0.000000364337 -0.000000023644 0.000000006385 -0.000000011837 0.000000002794 -0.000000000185 -0.000000023644 0.000000320215 -0.000000002428 0.000000040468 -0.000000007809 0.000000000097 0.000000006385 -0.000000002428 0.000000324189 +1 1645005901.700814247131 10 11 -0.003099084350 0.000842028850 0.001191700627 -0.000180302292 -0.000304391953 -0.000943844689 0.000000090213 -0.000000019520 0.000000001928 -0.000000000067 -0.000000011416 0.000000037331 -0.000000019520 0.000000012352 -0.000000000665 0.000000000889 0.000000002684 -0.000000007936 0.000000001928 -0.000000000665 0.000000013403 -0.000000009790 0.000000000272 0.000000000027 -0.000000000067 0.000000000889 -0.000000009790 0.000000343339 -0.000000021912 0.000000005787 -0.000000011416 0.000000002684 0.000000000272 -0.000000021912 0.000000304258 -0.000000002229 0.000000037331 -0.000000007936 0.000000000027 0.000000005787 -0.000000002229 0.000000301522 +1 1645005901.799977779388 11 12 -0.001119124717 0.000911769018 0.000896053010 0.000109146445 0.000004749543 -0.001769999687 0.000000088504 -0.000000019214 0.000000002010 -0.000000000364 -0.000000011599 0.000000035324 -0.000000019214 0.000000012281 -0.000000000663 0.000000000986 0.000000002726 -0.000000007878 0.000000002010 -0.000000000663 0.000000013725 -0.000000009806 0.000000000042 0.000000000058 -0.000000000364 0.000000000986 -0.000000009806 0.000000328413 -0.000000018998 0.000000005385 -0.000000011599 0.000000002726 0.000000000042 -0.000000018998 0.000000293251 -0.000000002089 0.000000035324 -0.000000007878 0.000000000058 0.000000005385 -0.000000002089 0.000000286449 +1 1645005901.900312423706 12 13 -0.000402732963 0.000242598646 -0.000072269012 0.000055999966 0.000123729242 -0.002073753983 0.000000087170 -0.000000018926 0.000000002027 -0.000000000440 -0.000000011562 0.000000034056 -0.000000018926 0.000000012165 -0.000000000686 0.000000001149 0.000000002703 -0.000000007746 0.000000002027 -0.000000000686 0.000000013717 -0.000000009355 -0.000000000107 0.000000000029 -0.000000000440 0.000000001149 -0.000000009355 0.000000315929 -0.000000016862 0.000000005085 -0.000000011562 0.000000002703 -0.000000000107 -0.000000016862 0.000000284663 -0.000000002049 0.000000034056 -0.000000007746 0.000000000029 0.000000005085 -0.000000002049 0.000000275236 +1 1645005901.999710559845 13 14 -0.000353031834 0.000520456797 -0.000885039005 0.000175471614 0.000226437074 -0.001781941944 0.000000085576 -0.000000018640 0.000000002007 -0.000000000556 -0.000000011265 0.000000032750 -0.000000018640 0.000000011971 -0.000000000692 0.000000001200 0.000000002700 -0.000000007525 0.000000002007 -0.000000000692 0.000000013341 -0.000000008639 0.000000000083 -0.000000000060 -0.000000000556 0.000000001200 -0.000000008639 0.000000304286 -0.000000016247 0.000000004967 -0.000000011265 0.000000002700 0.000000000083 -0.000000016247 0.000000275471 -0.000000002124 0.000000032750 -0.000000007525 -0.000000000060 0.000000004967 -0.000000002124 0.000000264734 +1 1645005902.100522041321 14 15 -0.000755912182 0.000162757993 -0.000940218374 0.000142873662 0.000156455499 -0.000999359978 0.000000084547 -0.000000018524 0.000000002007 -0.000000001342 -0.000000011413 0.000000031398 -0.000000018524 0.000000011963 -0.000000000637 0.000000001412 0.000000002758 -0.000000007386 0.000000002007 -0.000000000637 0.000000012995 -0.000000008153 0.000000000029 -0.000000000020 -0.000000001342 0.000000001412 -0.000000008153 0.000000294679 -0.000000015478 0.000000004679 -0.000000011413 0.000000002758 0.000000000029 -0.000000015478 0.000000268528 -0.000000002296 0.000000031398 -0.000000007386 -0.000000000020 0.000000004679 -0.000000002296 0.000000256957 +1 1645005902.203018426895 15 16 -0.000439249231 -0.000178254566 -0.000437587780 -0.000084581729 0.000049443904 -0.000079088693 0.000000083829 -0.000000018377 0.000000001938 -0.000000001650 -0.000000011605 0.000000030281 -0.000000018377 0.000000011902 -0.000000000597 0.000000001442 0.000000002713 -0.000000007253 0.000000001938 -0.000000000597 0.000000013076 -0.000000008171 0.000000000037 0.000000000101 -0.000000001650 0.000000001442 -0.000000008171 0.000000287437 -0.000000014704 0.000000003963 -0.000000011605 0.000000002713 0.000000000037 -0.000000014704 0.000000263922 -0.000000002586 0.000000030281 -0.000000007253 0.000000000101 0.000000003963 -0.000000002586 0.000000250716 +1 1645005902.301831007004 16 17 0.000298386384 -0.000560355605 -0.000119106950 0.000156455585 -0.000010964095 -0.000026764071 0.000000083200 -0.000000018095 0.000000001879 -0.000000001309 -0.000000011836 0.000000029821 -0.000000018095 0.000000011725 -0.000000000623 0.000000001381 0.000000002667 -0.000000006986 0.000000001879 -0.000000000623 0.000000013155 -0.000000008373 0.000000000404 0.000000000134 -0.000000001309 0.000000001381 -0.000000008373 0.000000283842 -0.000000014501 0.000000003889 -0.000000011836 0.000000002667 0.000000000404 -0.000000014501 0.000000261244 -0.000000002864 0.000000029821 -0.000000006986 0.000000000134 0.000000003889 -0.000000002864 0.000000247086 +1 1645005902.400156021118 17 18 0.000179886078 -0.000459777112 -0.000358763747 -0.000153694839 0.000090517348 0.000016912731 0.000000081953 -0.000000017650 0.000000001880 -0.000000001460 -0.000000011911 0.000000029712 -0.000000017650 0.000000011542 -0.000000000621 0.000000001450 0.000000002686 -0.000000006772 0.000000001880 -0.000000000621 0.000000012815 -0.000000008204 0.000000000400 0.000000000134 -0.000000001460 0.000000001450 -0.000000008204 0.000000279237 -0.000000013903 0.000000003850 -0.000000011911 0.000000002686 0.000000000400 -0.000000013903 0.000000257257 -0.000000002996 0.000000029712 -0.000000006772 0.000000000134 0.000000003850 -0.000000002996 0.000000243174 +1 1645005902.500535726547 18 19 -0.000677739243 -0.000661804764 -0.000786983512 0.000083215128 -0.000114303323 0.000518990892 0.000000081125 -0.000000017513 0.000000001940 -0.000000001754 -0.000000011942 0.000000029299 -0.000000017513 0.000000011479 -0.000000000613 0.000000001489 0.000000002719 -0.000000006813 0.000000001940 -0.000000000613 0.000000012630 -0.000000007967 0.000000000014 0.000000000174 -0.000000001754 0.000000001489 -0.000000007967 0.000000273527 -0.000000013282 0.000000003515 -0.000000011942 0.000000002719 0.000000000014 -0.000000013282 0.000000252050 -0.000000003271 0.000000029299 -0.000000006813 0.000000000174 0.000000003515 -0.000000003271 0.000000239150 +1 1645005902.600610494614 19 20 -0.001398677644 -0.000281431524 -0.000006292086 -0.000149844144 0.000155174368 0.000310385345 0.000000081164 -0.000000017520 0.000000001984 -0.000000001776 -0.000000012440 0.000000028693 -0.000000017520 0.000000011567 -0.000000000605 0.000000001477 0.000000002823 -0.000000006788 0.000000001984 -0.000000000605 0.000000012243 -0.000000007646 -0.000000000218 0.000000000207 -0.000000001776 0.000000001477 -0.000000007646 0.000000270203 -0.000000013042 0.000000003281 -0.000000012440 0.000000002823 -0.000000000218 -0.000000013042 0.000000249116 -0.000000003680 0.000000028693 -0.000000006788 0.000000000207 0.000000003281 -0.000000003680 0.000000237404 +1 1645005902.700582027435 20 21 -0.001420482214 -0.000263135886 -0.000053375571 -0.000116154210 -0.000141423619 0.000057516294 0.000000080767 -0.000000017432 0.000000001927 -0.000000001700 -0.000000012707 0.000000028372 -0.000000017432 0.000000011550 -0.000000000579 0.000000001468 0.000000002900 -0.000000006723 0.000000001927 -0.000000000579 0.000000011880 -0.000000007357 -0.000000000236 0.000000000169 -0.000000001700 0.000000001468 -0.000000007357 0.000000268424 -0.000000012714 0.000000003151 -0.000000012707 0.000000002900 -0.000000000236 -0.000000012714 0.000000247501 -0.000000003752 0.000000028372 -0.000000006723 0.000000000169 0.000000003151 -0.000000003752 0.000000236011 +1 1645005902.802828073502 21 22 -0.001522845641 -0.000074472171 0.000173559055 -0.000051052057 -0.000127920939 -0.000114638340 0.000000080331 -0.000000017568 0.000000001879 -0.000000001733 -0.000000012773 0.000000027688 -0.000000017568 0.000000011534 -0.000000000541 0.000000001430 0.000000002960 -0.000000006720 0.000000001879 -0.000000000541 0.000000011888 -0.000000007198 -0.000000000298 0.000000000147 -0.000000001733 0.000000001430 -0.000000007198 0.000000266369 -0.000000012230 0.000000003032 -0.000000012773 0.000000002960 -0.000000000298 -0.000000012230 0.000000246106 -0.000000003739 0.000000027688 -0.000000006720 0.000000000147 0.000000003032 -0.000000003739 0.000000233014 +1 1645005902.900591373444 22 23 -0.000662864106 -0.000366807912 -0.000191751859 0.000099874707 0.000044070199 -0.000205737831 0.000000079920 -0.000000017596 0.000000001894 -0.000000001895 -0.000000012932 0.000000026867 -0.000000017596 0.000000011473 -0.000000000544 0.000000001444 0.000000002996 -0.000000006939 0.000000001894 -0.000000000544 0.000000011681 -0.000000007215 -0.000000000519 0.000000000192 -0.000000001895 0.000000001444 -0.000000007215 0.000000264971 -0.000000011324 0.000000002987 -0.000000012932 0.000000002996 -0.000000000519 -0.000000011324 0.000000246113 -0.000000003809 0.000000026867 -0.000000006939 0.000000000192 0.000000002987 -0.000000003809 0.000000230070 +1 1645005902.999960184097 23 24 0.000586459972 -0.000383883637 -0.000657440721 0.000025502157 0.000173096398 -0.000657422729 0.000000078961 -0.000000017438 0.000000001897 -0.000000001864 -0.000000012901 0.000000026255 -0.000000017438 0.000000011356 -0.000000000570 0.000000001429 0.000000002941 -0.000000007077 0.000000001897 -0.000000000570 0.000000011277 -0.000000007081 -0.000000000639 0.000000000279 -0.000000001864 0.000000001429 -0.000000007081 0.000000261573 -0.000000010356 0.000000003006 -0.000000012901 0.000000002941 -0.000000000639 -0.000000010356 0.000000244298 -0.000000003671 0.000000026255 -0.000000007077 0.000000000279 0.000000003006 -0.000000003671 0.000000226695 +1 1645005903.099860429764 24 25 0.001039569017 -0.000664462296 -0.000116140678 -0.000029188969 0.000121233801 -0.000658346529 0.000000078315 -0.000000017330 0.000000001958 -0.000000001934 -0.000000012838 0.000000025439 -0.000000017330 0.000000011376 -0.000000000569 0.000000001357 0.000000002951 -0.000000007067 0.000000001958 -0.000000000569 0.000000011058 -0.000000006788 -0.000000000667 0.000000000313 -0.000000001934 0.000000001357 -0.000000006788 0.000000257200 -0.000000009782 0.000000002843 -0.000000012838 0.000000002951 -0.000000000667 -0.000000009782 0.000000240971 -0.000000003535 0.000000025439 -0.000000007067 0.000000000313 0.000000002843 -0.000000003535 0.000000223420 +1 1645005903.201798200607 25 26 0.000799401277 -0.000715390089 -0.000135250388 -0.000005401473 0.000028317278 -0.000705012899 0.000000078273 -0.000000017284 0.000000001945 -0.000000002002 -0.000000012892 0.000000025018 -0.000000017284 0.000000011434 -0.000000000564 0.000000001376 0.000000002920 -0.000000007058 0.000000001945 -0.000000000564 0.000000011188 -0.000000006747 -0.000000000284 0.000000000359 -0.000000002002 0.000000001376 -0.000000006747 0.000000255919 -0.000000009831 0.000000002438 -0.000000012892 0.000000002920 -0.000000000284 -0.000000009831 0.000000239627 -0.000000003371 0.000000025018 -0.000000007058 0.000000000359 0.000000002438 -0.000000003371 0.000000222579 +1 1645005903.299434661865 26 27 -0.000113261770 0.000023693570 -0.000214375044 -0.000117559997 0.000124720787 -0.000596143614 0.000000078325 -0.000000017305 0.000000001849 -0.000000001888 -0.000000012882 0.000000024989 -0.000000017305 0.000000011476 -0.000000000557 0.000000001385 0.000000002888 -0.000000006966 0.000000001849 -0.000000000557 0.000000011561 -0.000000006807 0.000000000150 0.000000000342 -0.000000001888 0.000000001385 -0.000000006807 0.000000256583 -0.000000010009 0.000000002389 -0.000000012882 0.000000002888 0.000000000150 -0.000000010009 0.000000240356 -0.000000003042 0.000000024989 -0.000000006966 0.000000000342 0.000000002389 -0.000000003042 0.000000222999 +1 1645005903.399843454361 27 28 -0.000958900034 0.000637600325 -0.000214249917 -0.000223045645 -0.000071809246 0.000029335082 0.000000078344 -0.000000017286 0.000000001846 -0.000000001681 -0.000000012791 0.000000024604 -0.000000017286 0.000000011476 -0.000000000598 0.000000001367 0.000000002926 -0.000000006949 0.000000001846 -0.000000000598 0.000000012087 -0.000000006902 0.000000000466 0.000000000207 -0.000000001681 0.000000001367 -0.000000006902 0.000000256077 -0.000000009887 0.000000002620 -0.000000012791 0.000000002926 0.000000000466 -0.000000009887 0.000000240846 -0.000000002625 0.000000024604 -0.000000006949 0.000000000207 0.000000002620 -0.000000002625 0.000000222033 +1 1645005903.503298759460 28 29 -0.000594952889 0.000937675663 0.000350959010 -0.000134047994 0.000112568416 0.000532529091 0.000000077870 -0.000000017120 0.000000001906 -0.000000001471 -0.000000012767 0.000000024191 -0.000000017120 0.000000011459 -0.000000000663 0.000000001370 0.000000002979 -0.000000007036 0.000000001906 -0.000000000663 0.000000012464 -0.000000006974 0.000000000554 0.000000000084 -0.000000001471 0.000000001370 -0.000000006974 0.000000254804 -0.000000009282 0.000000002485 -0.000000012767 0.000000002979 0.000000000554 -0.000000009282 0.000000241012 -0.000000002108 0.000000024191 -0.000000007036 0.000000000084 0.000000002485 -0.000000002108 0.000000220716 +1 1645005903.600738525391 29 30 0.000794414323 0.000340891713 0.000722517724 0.000049267232 -0.000083827634 0.000511848534 0.000000077849 -0.000000017062 0.000000001944 -0.000000001282 -0.000000012931 0.000000024026 -0.000000017062 0.000000011499 -0.000000000672 0.000000001311 0.000000003010 -0.000000007173 0.000000001944 -0.000000000672 0.000000012279 -0.000000007093 0.000000000279 0.000000000086 -0.000000001282 0.000000001311 -0.000000007093 0.000000255543 -0.000000008713 0.000000002207 -0.000000012931 0.000000003010 0.000000000279 -0.000000008713 0.000000242511 -0.000000001802 0.000000024026 -0.000000007173 0.000000000086 0.000000002207 -0.000000001802 0.000000221948 +1 1645005903.702013492584 30 31 0.000724079430 -0.000003591352 0.000270122431 -0.000012632068 -0.000052326779 0.000440294104 0.000000077508 -0.000000016995 0.000000001961 -0.000000001247 -0.000000012890 0.000000023883 -0.000000016995 0.000000011411 -0.000000000627 0.000000001216 0.000000003013 -0.000000007068 0.000000001961 -0.000000000627 0.000000011934 -0.000000007149 -0.000000000010 0.000000000107 -0.000000001247 0.000000001216 -0.000000007149 0.000000254403 -0.000000008732 0.000000001995 -0.000000012890 0.000000003013 -0.000000000010 -0.000000008732 0.000000240911 -0.000000001885 0.000000023883 -0.000000007068 0.000000000107 0.000000001995 -0.000000001885 0.000000221291 +1 1645005903.800774335861 31 32 0.001067662490 -0.000510502525 -0.000301534356 0.000174567922 0.000065476921 -0.000090436674 0.000000077110 -0.000000016957 0.000000001949 -0.000000001388 -0.000000012566 0.000000024020 -0.000000016957 0.000000011352 -0.000000000587 0.000000001174 0.000000002958 -0.000000006851 0.000000001949 -0.000000000587 0.000000011556 -0.000000007045 -0.000000000114 0.000000000144 -0.000000001388 0.000000001174 -0.000000007045 0.000000252509 -0.000000008765 0.000000001947 -0.000000012566 0.000000002958 -0.000000000114 -0.000000008765 0.000000238424 -0.000000002141 0.000000024020 -0.000000006851 0.000000000144 0.000000001947 -0.000000002141 0.000000219630 +1 1645005903.899410247803 32 33 0.000817144389 -0.001122214966 -0.000884357956 -0.000001891347 0.000036718074 0.000252289181 0.000000077446 -0.000000017113 0.000000001881 -0.000000001342 -0.000000012421 0.000000024153 -0.000000017113 0.000000011435 -0.000000000562 0.000000001165 0.000000002871 -0.000000006813 0.000000001881 -0.000000000562 0.000000011375 -0.000000007060 -0.000000000151 0.000000000211 -0.000000001342 0.000000001165 -0.000000007060 0.000000252108 -0.000000008770 0.000000002126 -0.000000012421 0.000000002871 -0.000000000151 -0.000000008770 0.000000237324 -0.000000002342 0.000000024153 -0.000000006813 0.000000000211 0.000000002126 -0.000000002342 0.000000219157 +1 1645005903.999286413193 33 34 0.000286745156 -0.000755201087 -0.000483958459 0.000073063588 -0.000069345847 0.000131138848 0.000000077516 -0.000000017113 0.000000001949 -0.000000001353 -0.000000012656 0.000000023945 -0.000000017113 0.000000011475 -0.000000000564 0.000000001201 0.000000002844 -0.000000006895 0.000000001949 -0.000000000564 0.000000011242 -0.000000006940 -0.000000000110 0.000000000316 -0.000000001353 0.000000001201 -0.000000006940 0.000000251401 -0.000000008774 0.000000002151 -0.000000012656 0.000000002844 -0.000000000110 -0.000000008774 0.000000235544 -0.000000002538 0.000000023945 -0.000000006895 0.000000000316 0.000000002151 -0.000000002538 0.000000218001 +1 1645005904.101194381714 34 35 -0.000092729542 -0.000721176305 -0.000046422003 0.000080600128 -0.000098482186 0.000269102398 0.000000077508 -0.000000016949 0.000000002044 -0.000000001517 -0.000000012907 0.000000023822 -0.000000016949 0.000000011511 -0.000000000628 0.000000001343 0.000000002868 -0.000000006931 0.000000002044 -0.000000000628 0.000000010979 -0.000000006697 -0.000000000142 0.000000000368 -0.000000001517 0.000000001343 -0.000000006697 0.000000251510 -0.000000008827 0.000000002031 -0.000000012907 0.000000002868 -0.000000000142 -0.000000008827 0.000000234984 -0.000000002474 0.000000023822 -0.000000006931 0.000000000368 0.000000002031 -0.000000002474 0.000000217838 +1 1645005904.202036380768 35 36 0.000159501241 -0.000619989049 -0.000182200023 -0.000130638892 -0.000030290097 0.000441918495 0.000000077457 -0.000000016871 0.000000002049 -0.000000001653 -0.000000012863 0.000000024008 -0.000000016871 0.000000011440 -0.000000000666 0.000000001365 0.000000002895 -0.000000006831 0.000000002049 -0.000000000666 0.000000010881 -0.000000006644 -0.000000000477 0.000000000349 -0.000000001653 0.000000001365 -0.000000006644 0.000000252370 -0.000000009051 0.000000002132 -0.000000012863 0.000000002895 -0.000000000477 -0.000000009051 0.000000236368 -0.000000002172 0.000000024008 -0.000000006831 0.000000000349 0.000000002132 -0.000000002172 0.000000219216 +1 1645005904.301526308060 36 37 0.000327836677 -0.000353330603 -0.000334050825 -0.000048653573 -0.000017134077 0.000406296921 0.000000077743 -0.000000017107 0.000000002089 -0.000000001553 -0.000000013005 0.000000024196 -0.000000017107 0.000000011432 -0.000000000665 0.000000001162 0.000000002958 -0.000000006693 0.000000002089 -0.000000000665 0.000000010877 -0.000000006677 -0.000000000574 0.000000000388 -0.000000001553 0.000000001162 -0.000000006677 0.000000252991 -0.000000009267 0.000000002202 -0.000000013005 0.000000002958 -0.000000000574 -0.000000009267 0.000000237868 -0.000000002187 0.000000024196 -0.000000006693 0.000000000388 0.000000002202 -0.000000002187 0.000000220113 +1 1645005904.403170108795 37 38 0.000737980852 -0.000351033561 -0.000295585445 -0.000025607001 -0.000042435192 0.000431463920 0.000000077856 -0.000000017205 0.000000002133 -0.000000001519 -0.000000013172 0.000000024074 -0.000000017205 0.000000011434 -0.000000000607 0.000000001084 0.000000003078 -0.000000006696 0.000000002133 -0.000000000607 0.000000011189 -0.000000006857 -0.000000000557 0.000000000291 -0.000000001519 0.000000001084 -0.000000006857 0.000000252632 -0.000000009363 0.000000002170 -0.000000013172 0.000000003078 -0.000000000557 -0.000000009363 0.000000237409 -0.000000002500 0.000000024074 -0.000000006696 0.000000000291 0.000000002170 -0.000000002500 0.000000219313 +1 1645005904.501605749130 38 39 0.001088348237 -0.000613129911 0.000232566844 0.000179265756 -0.000069468820 -0.000092368701 0.000000077918 -0.000000017330 0.000000002087 -0.000000001727 -0.000000013339 0.000000023890 -0.000000017330 0.000000011400 -0.000000000554 0.000000001171 0.000000003171 -0.000000006655 0.000000002087 -0.000000000554 0.000000011497 -0.000000006951 -0.000000000628 0.000000000205 -0.000000001727 0.000000001171 -0.000000006951 0.000000252754 -0.000000009318 0.000000002086 -0.000000013339 0.000000003171 -0.000000000628 -0.000000009318 0.000000237371 -0.000000002624 0.000000023890 -0.000000006655 0.000000000205 0.000000002086 -0.000000002624 0.000000219216 +1 1645005904.599536895752 39 40 0.000468476145 -0.001272947114 0.000093562933 -0.000116852710 0.000115936099 0.000225755043 0.000000077895 -0.000000017350 0.000000001892 -0.000000001532 -0.000000013201 0.000000023949 -0.000000017350 0.000000011402 -0.000000000560 0.000000001138 0.000000003123 -0.000000006687 0.000000001892 -0.000000000560 0.000000011557 -0.000000006985 -0.000000000681 0.000000000172 -0.000000001532 0.000000001138 -0.000000006985 0.000000252449 -0.000000009335 0.000000002069 -0.000000013201 0.000000003123 -0.000000000681 -0.000000009335 0.000000237483 -0.000000002381 0.000000023949 -0.000000006687 0.000000000172 0.000000002069 -0.000000002381 0.000000218909 +1 1645005904.700893878937 40 41 0.000860573463 -0.001209131350 -0.000501116645 0.000157555541 0.000010319357 -0.000048830165 0.000000077189 -0.000000017111 0.000000001814 -0.000000001358 -0.000000012728 0.000000023995 -0.000000017111 0.000000011307 -0.000000000533 0.000000001084 0.000000002997 -0.000000006770 0.000000001814 -0.000000000533 0.000000011517 -0.000000007033 -0.000000000879 0.000000000155 -0.000000001358 0.000000001084 -0.000000007033 0.000000250581 -0.000000009543 0.000000002194 -0.000000012728 0.000000002997 -0.000000000879 -0.000000009543 0.000000236400 -0.000000002342 0.000000023995 -0.000000006770 0.000000000155 0.000000002194 -0.000000002342 0.000000217604 +1 1645005904.802378892899 41 42 0.000632138609 -0.000790062715 -0.000690771042 0.000034390734 -0.000109221344 0.000029091074 0.000000077001 -0.000000017060 0.000000001956 -0.000000001694 -0.000000012495 0.000000024027 -0.000000017060 0.000000011341 -0.000000000521 0.000000001130 0.000000002989 -0.000000006785 0.000000001956 -0.000000000521 0.000000011397 -0.000000006946 -0.000000000911 0.000000000171 -0.000000001694 0.000000001130 -0.000000006946 0.000000250277 -0.000000009613 0.000000002318 -0.000000012495 0.000000002989 -0.000000000911 -0.000000009613 0.000000236262 -0.000000002568 0.000000024027 -0.000000006785 0.000000000171 0.000000002318 -0.000000002568 0.000000218078 +1 1645005904.902300596237 42 43 0.000802579583 -0.000538433330 -0.000341299568 0.000007003048 -0.000032678783 -0.000429464507 0.000000077294 -0.000000017051 0.000000002005 -0.000000001928 -0.000000012622 0.000000024150 -0.000000017051 0.000000011388 -0.000000000568 0.000000001229 0.000000003054 -0.000000006816 0.000000002005 -0.000000000568 0.000000011301 -0.000000006823 -0.000000000888 0.000000000138 -0.000000001928 0.000000001229 -0.000000006823 0.000000251648 -0.000000009299 0.000000002429 -0.000000012622 0.000000003054 -0.000000000888 -0.000000009299 0.000000237177 -0.000000002580 0.000000024150 -0.000000006816 0.000000000138 0.000000002429 -0.000000002580 0.000000218935 +1 1645005904.999253749847 43 44 0.000501016980 0.000211957757 -0.000005361234 0.000063883092 -0.000036266373 -0.000504158455 0.000000077290 -0.000000017104 0.000000001982 -0.000000001896 -0.000000012750 0.000000024173 -0.000000017104 0.000000011394 -0.000000000596 0.000000001289 0.000000003111 -0.000000006794 0.000000001982 -0.000000000596 0.000000011045 -0.000000006670 -0.000000001063 0.000000000093 -0.000000001896 0.000000001289 -0.000000006670 0.000000252269 -0.000000009368 0.000000002660 -0.000000012750 0.000000003111 -0.000000001063 -0.000000009368 0.000000236670 -0.000000002412 0.000000024173 -0.000000006794 0.000000000093 0.000000002660 -0.000000002412 0.000000218725 +1 1645005905.101718425751 44 45 -0.000024589931 0.000754394551 0.000408641144 -0.000202334514 0.000016741619 -0.000672688351 0.000000076750 -0.000000016988 0.000000001970 -0.000000001786 -0.000000012675 0.000000024005 -0.000000016988 0.000000011345 -0.000000000542 0.000000001221 0.000000003052 -0.000000006750 0.000000001970 -0.000000000542 0.000000010830 -0.000000006614 -0.000000000937 0.000000000130 -0.000000001786 0.000000001221 -0.000000006614 0.000000251233 -0.000000009933 0.000000002642 -0.000000012675 0.000000003052 -0.000000000937 -0.000000009933 0.000000234262 -0.000000002344 0.000000024005 -0.000000006750 0.000000000130 0.000000002642 -0.000000002344 0.000000217464 +1 1645005905.201553344727 45 46 -0.000215612364 0.000834606698 0.000149325139 -0.000059117112 -0.000021374139 -0.000795866284 0.000000076660 -0.000000016912 0.000000001943 -0.000000001779 -0.000000012647 0.000000023970 -0.000000016912 0.000000011337 -0.000000000541 0.000000001223 0.000000003001 -0.000000006808 0.000000001943 -0.000000000541 0.000000011099 -0.000000006710 -0.000000000647 0.000000000160 -0.000000001779 0.000000001223 -0.000000006710 0.000000251867 -0.000000010134 0.000000002780 -0.000000012647 0.000000003001 -0.000000000647 -0.000000010134 0.000000234786 -0.000000002355 0.000000023970 -0.000000006808 0.000000000160 0.000000002780 -0.000000002355 0.000000217751 +1 1645005905.298557758331 46 47 0.000863242831 0.001318878065 -0.000252232506 -0.000118643047 0.000022995563 -0.001197291392 0.000000077249 -0.000000017059 0.000000001925 -0.000000001737 -0.000000012443 0.000000024240 -0.000000017059 0.000000011387 -0.000000000615 0.000000001270 0.000000002946 -0.000000006834 0.000000001925 -0.000000000615 0.000000011672 -0.000000006817 -0.000000000141 0.000000000151 -0.000000001737 0.000000001270 -0.000000006817 0.000000253335 -0.000000010236 0.000000003256 -0.000000012443 0.000000002946 -0.000000000141 -0.000000010236 0.000000237176 -0.000000002293 0.000000024240 -0.000000006834 0.000000000151 0.000000003256 -0.000000002293 0.000000219058 +1 1645005905.399083614349 47 48 0.002259440161 0.001105945552 -0.000750095487 0.000323798878 0.000255879602 -0.002485082179 0.000000077105 -0.000000016955 0.000000001874 -0.000000001539 -0.000000012289 0.000000024392 -0.000000016955 0.000000011391 -0.000000000645 0.000000001253 0.000000002881 -0.000000006702 0.000000001874 -0.000000000645 0.000000012203 -0.000000006954 0.000000000290 0.000000000129 -0.000000001539 0.000000001253 -0.000000006954 0.000000252527 -0.000000010460 0.000000003072 -0.000000012289 0.000000002881 0.000000000290 -0.000000010460 0.000000237679 -0.000000002199 0.000000024392 -0.000000006702 0.000000000129 0.000000003072 -0.000000002199 0.000000218901 +1 1645005905.499478816986 48 49 0.001203207953 0.000792305972 -0.001325455405 0.000034228402 -0.000060355459 -0.004038449411 0.000000076987 -0.000000016850 0.000000001884 -0.000000001681 -0.000000012438 0.000000024471 -0.000000016850 0.000000011498 -0.000000000626 0.000000001299 0.000000002943 -0.000000006512 0.000000001884 -0.000000000626 0.000000012768 -0.000000007178 0.000000000282 0.000000000078 -0.000000001681 0.000000001299 -0.000000007178 0.000000253439 -0.000000010315 0.000000002860 -0.000000012438 0.000000002943 0.000000000282 -0.000000010315 0.000000238861 -0.000000002267 0.000000024471 -0.000000006512 0.000000000078 0.000000002860 -0.000000002267 0.000000220135 +1 1645005905.599982023239 49 50 -0.000427727796 0.001474783510 -0.000181500876 0.000232089095 0.000542383436 -0.007766826389 0.000000077535 -0.000000017038 0.000000001838 -0.000000001917 -0.000000012698 0.000000024720 -0.000000017038 0.000000011645 -0.000000000643 0.000000001385 0.000000003075 -0.000000006360 0.000000001838 -0.000000000643 0.000000012879 -0.000000007370 -0.000000000087 -0.000000000001 -0.000000001917 0.000000001385 -0.000000007370 0.000000256358 -0.000000009978 0.000000002881 -0.000000012698 0.000000003075 -0.000000000087 -0.000000009978 0.000000240935 -0.000000002358 0.000000024720 -0.000000006360 -0.000000000001 0.000000002881 -0.000000002358 0.000000222306 +1 1645005905.699927568436 50 51 -0.003379224466 0.003178186222 -0.000934051059 0.000195152956 0.000761914950 -0.011833240191 0.000000077971 -0.000000017303 0.000000001797 -0.000000001904 -0.000000012847 0.000000024954 -0.000000017303 0.000000011820 -0.000000000663 0.000000001463 0.000000003093 -0.000000006376 0.000000001797 -0.000000000663 0.000000012974 -0.000000007270 0.000000000071 0.000000000011 -0.000000001904 0.000000001463 -0.000000007270 0.000000257764 -0.000000010219 0.000000002728 -0.000000012847 0.000000003093 0.000000000071 -0.000000010219 0.000000242472 -0.000000002420 0.000000024954 -0.000000006376 0.000000000011 0.000000002728 -0.000000002420 0.000000223682 +1 1645005905.802299499512 51 52 -0.003954280518 0.004558444476 -0.000638200694 0.000610985838 0.000592963225 -0.016423441061 0.000000078171 -0.000000017308 0.000000001851 -0.000000001684 -0.000000012949 0.000000025066 -0.000000017308 0.000000012067 -0.000000000670 0.000000001552 0.000000003016 -0.000000006564 0.000000001851 -0.000000000670 0.000000013096 -0.000000007042 0.000000000544 0.000000000071 -0.000000001684 0.000000001552 -0.000000007042 0.000000257460 -0.000000010870 0.000000002616 -0.000000012949 0.000000003016 0.000000000544 -0.000000010870 0.000000243242 -0.000000002361 0.000000025066 -0.000000006564 0.000000000071 0.000000002616 -0.000000002361 0.000000225067 +1 1645005905.903995513916 52 53 -0.002725003716 0.004085843921 -0.000203663179 0.000790836836 0.000587671331 -0.019275453487 0.000000078783 -0.000000017322 0.000000001763 -0.000000001459 -0.000000013309 0.000000025091 -0.000000017322 0.000000012536 -0.000000000705 0.000000001676 0.000000003055 -0.000000006665 0.000000001763 -0.000000000705 0.000000012882 -0.000000007071 0.000000000738 0.000000000012 -0.000000001459 0.000000001676 -0.000000007071 0.000000258028 -0.000000012023 0.000000002543 -0.000000013309 0.000000003055 0.000000000738 -0.000000012023 0.000000244738 -0.000000002147 0.000000025091 -0.000000006665 0.000000000012 0.000000002543 -0.000000002147 0.000000228144 +1 1645005906.006122350693 53 54 -0.001467286616 0.002911939063 0.001069730244 0.000570837090 0.000647669773 -0.017525510312 0.000000079128 -0.000000017240 0.000000001741 -0.000000001521 -0.000000013627 0.000000025287 -0.000000017240 0.000000012719 -0.000000000684 0.000000001673 0.000000003084 -0.000000006545 0.000000001741 -0.000000000684 0.000000012768 -0.000000007310 0.000000001027 0.000000000025 -0.000000001521 0.000000001673 -0.000000007310 0.000000260589 -0.000000013608 0.000000002539 -0.000000013627 0.000000003084 0.000000001027 -0.000000013608 0.000000247534 -0.000000002295 0.000000025287 -0.000000006545 0.000000000025 0.000000002539 -0.000000002295 0.000000231204 +1 1645005906.099501609802 54 55 -0.001059218310 0.001595583380 0.001718691048 0.000383564738 0.000421259981 -0.014061465138 0.000000078834 -0.000000016986 0.000000001758 -0.000000001750 -0.000000013531 0.000000025769 -0.000000016986 0.000000012611 -0.000000000664 0.000000001714 0.000000002983 -0.000000006153 0.000000001758 -0.000000000664 0.000000012509 -0.000000007145 0.000000001254 0.000000000088 -0.000000001750 0.000000001714 -0.000000007145 0.000000263017 -0.000000014906 0.000000002634 -0.000000013531 0.000000002983 0.000000001254 -0.000000014906 0.000000249896 -0.000000002598 0.000000025769 -0.000000006153 0.000000000088 0.000000002634 -0.000000002598 0.000000233378 +1 1645005906.199391126633 55 56 -0.001373475218 0.001308808338 0.000378930768 0.000479365502 0.000490820641 -0.009012238322 0.000000077943 -0.000000016768 0.000000001735 -0.000000001945 -0.000000013077 0.000000026054 -0.000000016768 0.000000012465 -0.000000000630 0.000000001711 0.000000002929 -0.000000005670 0.000000001735 -0.000000000630 0.000000012365 -0.000000007041 0.000000001545 0.000000000018 -0.000000001945 0.000000001711 -0.000000007041 0.000000261574 -0.000000015843 0.000000002953 -0.000000013077 0.000000002929 0.000000001545 -0.000000015843 0.000000248225 -0.000000002881 0.000000026054 -0.000000005670 0.000000000018 0.000000002953 -0.000000002881 0.000000232505 +1 1645005906.297765493393 56 57 -0.001355900545 0.000745568120 -0.000870920659 0.000244984256 0.000234169826 -0.004721749525 0.000000077729 -0.000000016651 0.000000001590 -0.000000002229 -0.000000012620 0.000000026130 -0.000000016651 0.000000012411 -0.000000000629 0.000000001763 0.000000002808 -0.000000005332 0.000000001590 -0.000000000629 0.000000012914 -0.000000007257 0.000000001540 -0.000000000032 -0.000000002229 0.000000001763 -0.000000007257 0.000000261378 -0.000000015902 0.000000003371 -0.000000012620 0.000000002808 0.000000001540 -0.000000015902 0.000000247742 -0.000000003003 0.000000026130 -0.000000005332 -0.000000000032 0.000000003371 -0.000000003003 0.000000232061 +1 1645005906.399662256241 57 58 -0.001773912220 0.001216253031 -0.001438523133 0.000093249864 0.000296808797 -0.001971036703 0.000000077865 -0.000000016538 0.000000001543 -0.000000002529 -0.000000012492 0.000000026251 -0.000000016538 0.000000012416 -0.000000000643 0.000000001937 0.000000002742 -0.000000005129 0.000000001543 -0.000000000643 0.000000013403 -0.000000007617 0.000000001310 -0.000000000089 -0.000000002529 0.000000001937 -0.000000007617 0.000000265989 -0.000000015382 0.000000003738 -0.000000012492 0.000000002742 0.000000001310 -0.000000015382 0.000000252116 -0.000000002953 0.000000026251 -0.000000005129 -0.000000000089 0.000000003738 -0.000000002953 0.000000233770 +1 1645005906.492895603180 58 59 -0.002660918339 0.001015475851 -0.001383065292 0.000330318405 -0.000006555949 -0.001300286378 0.000000077810 -0.000000016434 0.000000001630 -0.000000002398 -0.000000012724 0.000000026385 -0.000000016434 0.000000012330 -0.000000000622 0.000000001970 0.000000002832 -0.000000004942 0.000000001630 -0.000000000622 0.000000013443 -0.000000008032 0.000000001316 -0.000000000234 -0.000000002398 0.000000001970 -0.000000008032 0.000000272129 -0.000000015431 0.000000004625 -0.000000012724 0.000000002832 0.000000001316 -0.000000015431 0.000000257860 -0.000000002721 0.000000026385 -0.000000004942 -0.000000000234 0.000000004625 -0.000000002721 0.000000235343 +1 1645005906.599270582199 59 60 -0.004412984366 -0.001971971670 -0.001231997931 -0.000056688480 0.000101702338 -0.001115010901 0.000000077414 -0.000000016358 0.000000001582 -0.000000001830 -0.000000012780 0.000000026368 -0.000000016358 0.000000012249 -0.000000000629 0.000000001982 0.000000002761 -0.000000004929 0.000000001582 -0.000000000629 0.000000013473 -0.000000007965 0.000000001386 -0.000000000292 -0.000000001830 0.000000001982 -0.000000007965 0.000000274562 -0.000000016461 0.000000005637 -0.000000012780 0.000000002761 0.000000001386 -0.000000016461 0.000000259275 -0.000000002242 0.000000026368 -0.000000004929 -0.000000000292 0.000000005637 -0.000000002242 0.000000234454 +1 1645005906.697822093964 60 61 -0.005188840029 -0.004254111564 -0.000054909271 0.000406498966 -0.000296622837 0.000416988251 0.000000077214 -0.000000016250 0.000000001521 -0.000000001261 -0.000000012771 0.000000026760 -0.000000016250 0.000000012152 -0.000000000625 0.000000001905 0.000000002698 -0.000000005164 0.000000001521 -0.000000000625 0.000000013822 -0.000000008083 0.000000001309 -0.000000000328 -0.000000001261 0.000000001905 -0.000000008083 0.000000278328 -0.000000017447 0.000000005714 -0.000000012771 0.000000002698 0.000000001309 -0.000000017447 0.000000262592 -0.000000002008 0.000000026760 -0.000000005164 -0.000000000328 0.000000005714 -0.000000002008 0.000000235536 +1 1645005906.798193216324 61 62 -0.005350833950 -0.005705298333 0.000741755143 0.000130549378 -0.000347185966 0.003649959900 0.000000077631 -0.000000016247 0.000000001454 -0.000000001004 -0.000000012891 0.000000027268 -0.000000016247 0.000000012128 -0.000000000685 0.000000001939 0.000000002674 -0.000000005295 0.000000001454 -0.000000000685 0.000000014071 -0.000000008387 0.000000001361 -0.000000000365 -0.000000001004 0.000000001939 -0.000000008387 0.000000282769 -0.000000017695 0.000000005791 -0.000000012891 0.000000002674 0.000000001361 -0.000000017695 0.000000267197 -0.000000002088 0.000000027268 -0.000000005295 -0.000000000365 0.000000005791 -0.000000002088 0.000000238789 +1 1645005906.897775650024 62 63 -0.003697841110 -0.006789558112 -0.000589171782 0.000044608399 -0.000455644208 0.005951637027 0.000000077672 -0.000000016231 0.000000001373 -0.000000000697 -0.000000012771 0.000000027509 -0.000000016231 0.000000012025 -0.000000000658 0.000000001829 0.000000002608 -0.000000005327 0.000000001373 -0.000000000658 0.000000013806 -0.000000008294 0.000000001746 -0.000000000347 -0.000000000697 0.000000001829 -0.000000008294 0.000000283100 -0.000000016930 0.000000006179 -0.000000012771 0.000000002608 0.000000001746 -0.000000016930 0.000000267634 -0.000000001907 0.000000027509 -0.000000005327 -0.000000000347 0.000000006179 -0.000000001907 0.000000238791 +1 1645005907.000208616257 63 64 -0.002944765776 -0.006697802741 -0.003116587607 -0.000323320374 -0.000089765171 0.006392215701 0.000000077610 -0.000000016115 0.000000001489 -0.000000000549 -0.000000012506 0.000000027748 -0.000000016115 0.000000011973 -0.000000000586 0.000000001526 0.000000002609 -0.000000005268 0.000000001489 -0.000000000586 0.000000013816 -0.000000008429 0.000000001848 -0.000000000278 -0.000000000549 0.000000001526 -0.000000008429 0.000000282717 -0.000000015968 0.000000006292 -0.000000012506 0.000000002609 0.000000001848 -0.000000015968 0.000000266926 -0.000000001561 0.000000027748 -0.000000005268 -0.000000000278 0.000000006292 -0.000000001561 0.000000238503 +1 1645005907.098475456238 64 65 -0.001290240390 -0.005396750920 -0.004723595682 -0.000007365578 0.000016435762 0.004075705511 0.000000077997 -0.000000016170 0.000000001561 -0.000000000791 -0.000000012603 0.000000027663 -0.000000016170 0.000000012052 -0.000000000605 0.000000001609 0.000000002641 -0.000000005184 0.000000001561 -0.000000000605 0.000000013763 -0.000000008737 0.000000001470 -0.000000000238 -0.000000000791 0.000000001609 -0.000000008737 0.000000281532 -0.000000015977 0.000000006340 -0.000000012603 0.000000002641 0.000000001470 -0.000000015977 0.000000264521 -0.000000001627 0.000000027663 -0.000000005184 -0.000000000238 0.000000006340 -0.000000001627 0.000000238811 +1 1645005907.198553800583 65 66 -0.000669485805 -0.005485448301 -0.002930170548 -0.000146916616 0.000044397139 -0.000044314603 0.000000078888 -0.000000016541 0.000000001493 -0.000000000960 -0.000000013153 0.000000027258 -0.000000016541 0.000000012208 -0.000000000625 0.000000001783 0.000000002743 -0.000000005116 0.000000001493 -0.000000000625 0.000000013535 -0.000000008377 0.000000001173 -0.000000000224 -0.000000000960 0.000000001783 -0.000000008377 0.000000278934 -0.000000015798 0.000000005980 -0.000000013153 0.000000002743 0.000000001173 -0.000000015798 0.000000261815 -0.000000001742 0.000000027258 -0.000000005116 -0.000000000224 0.000000005980 -0.000000001742 0.000000238650 +1 1645005907.299114227295 66 67 -0.000737675673 -0.006602658374 0.001504502005 0.000251984015 0.000332600718 -0.004637221316 0.000000078978 -0.000000016696 0.000000001539 -0.000000001083 -0.000000013528 0.000000027256 -0.000000016696 0.000000012302 -0.000000000624 0.000000001764 0.000000002889 -0.000000005225 0.000000001539 -0.000000000624 0.000000013491 -0.000000008150 0.000000001072 -0.000000000210 -0.000000001083 0.000000001764 -0.000000008150 0.000000275585 -0.000000015902 0.000000005576 -0.000000013528 0.000000002889 0.000000001072 -0.000000015902 0.000000258948 -0.000000001809 0.000000027256 -0.000000005225 -0.000000000210 0.000000005576 -0.000000001809 0.000000238197 +1 1645005907.402397871017 67 68 -0.000551820100 -0.007603293394 0.003319136466 0.000085475476 0.000206125712 -0.005773773594 0.000000078821 -0.000000016809 0.000000001506 -0.000000001311 -0.000000013626 0.000000027682 -0.000000016809 0.000000012396 -0.000000000614 0.000000001766 0.000000002926 -0.000000005476 0.000000001506 -0.000000000614 0.000000013915 -0.000000008554 0.000000000776 -0.000000000165 -0.000000001311 0.000000001766 -0.000000008554 0.000000275078 -0.000000015973 0.000000005780 -0.000000013626 0.000000002926 0.000000000776 -0.000000015973 0.000000258664 -0.000000002050 0.000000027682 -0.000000005476 -0.000000000165 0.000000005780 -0.000000002050 0.000000238900 +1 1645005907.500724315643 68 69 -0.000078335904 -0.007900987662 0.000767252658 0.000325260818 0.000204141643 -0.004069357072 0.000000079410 -0.000000017045 0.000000001346 -0.000000001198 -0.000000013307 0.000000027863 -0.000000017045 0.000000012459 -0.000000000602 0.000000001790 0.000000002868 -0.000000005540 0.000000001346 -0.000000000602 0.000000014320 -0.000000009106 0.000000000386 -0.000000000282 -0.000000001198 0.000000001790 -0.000000009106 0.000000277944 -0.000000015885 0.000000006351 -0.000000013307 0.000000002868 0.000000000386 -0.000000015885 0.000000260452 -0.000000002077 0.000000027863 -0.000000005540 -0.000000000282 0.000000006351 -0.000000002077 0.000000240393 +1 1645005907.605340957642 69 70 0.001043055952 -0.008573117265 -0.001598483094 -0.000097387849 -0.000222906961 -0.000595407429 0.000000080055 -0.000000017438 0.000000001351 -0.000000001308 -0.000000013000 0.000000027705 -0.000000017438 0.000000012484 -0.000000000606 0.000000001915 0.000000002917 -0.000000005219 0.000000001351 -0.000000000606 0.000000014067 -0.000000009196 0.000000000300 -0.000000000458 -0.000000001308 0.000000001915 -0.000000009196 0.000000278309 -0.000000015861 0.000000006592 -0.000000013000 0.000000002917 0.000000000300 -0.000000015861 0.000000260164 -0.000000002000 0.000000027705 -0.000000005219 -0.000000000458 0.000000006592 -0.000000002000 0.000000240578 +1 1645005907.703968524933 70 71 0.001635189256 -0.007661342710 -0.000611987754 0.000195863477 0.000220085415 0.000455212442 0.000000081616 -0.000000018000 0.000000001433 -0.000000001641 -0.000000013192 0.000000027466 -0.000000018000 0.000000012653 -0.000000000591 0.000000001940 0.000000003029 -0.000000005006 0.000000001433 -0.000000000591 0.000000014157 -0.000000009101 0.000000000295 -0.000000000473 -0.000000001641 0.000000001940 -0.000000009101 0.000000278289 -0.000000015793 0.000000006721 -0.000000013192 0.000000003029 0.000000000295 -0.000000015793 0.000000260033 -0.000000002174 0.000000027466 -0.000000005006 -0.000000000473 0.000000006721 -0.000000002174 0.000000241348 +1 1645005907.804278135300 71 72 0.001549220998 -0.007393823942 0.001064792531 -0.000061245968 0.000265809889 -0.000238367926 0.000000084430 -0.000000018916 0.000000001290 -0.000000001506 -0.000000013499 0.000000026763 -0.000000018916 0.000000013096 -0.000000000595 0.000000001859 0.000000003095 -0.000000004652 0.000000001290 -0.000000000595 0.000000014779 -0.000000009309 0.000000000116 -0.000000000490 -0.000000001506 0.000000001859 -0.000000009309 0.000000280877 -0.000000016432 0.000000006827 -0.000000013499 0.000000003095 0.000000000116 -0.000000016432 0.000000260406 -0.000000002092 0.000000026763 -0.000000004652 -0.000000000490 0.000000006827 -0.000000002092 0.000000242565 +1 1645005907.903728008270 72 73 0.002219839781 -0.006729865633 0.001166665107 0.000120456909 0.000039397030 -0.001195289279 0.000000086968 -0.000000019811 0.000000001166 -0.000000001441 -0.000000013646 0.000000025791 -0.000000019811 0.000000013556 -0.000000000638 0.000000001888 0.000000003216 -0.000000004211 0.000000001166 -0.000000000638 0.000000015013 -0.000000009450 -0.000000000298 -0.000000000604 -0.000000001441 0.000000001888 -0.000000009450 0.000000282683 -0.000000017245 0.000000006808 -0.000000013646 0.000000003216 -0.000000000298 -0.000000017245 0.000000258711 -0.000000001579 0.000000025791 -0.000000004211 -0.000000000604 0.000000006808 -0.000000001579 0.000000241863 +1 1645005908.003634452820 73 74 0.003125083194 -0.006057082466 0.000100060086 -0.000042155704 -0.000130882188 -0.000945982049 0.000000088087 -0.000000020274 0.000000001242 -0.000000001785 -0.000000013450 0.000000024803 -0.000000020274 0.000000013671 -0.000000000680 0.000000002063 0.000000003299 -0.000000004076 0.000000001242 -0.000000000680 0.000000014959 -0.000000009260 -0.000000000646 -0.000000000747 -0.000000001785 0.000000002063 -0.000000009260 0.000000284481 -0.000000018051 0.000000007211 -0.000000013450 0.000000003299 -0.000000000646 -0.000000018051 0.000000257869 -0.000000000830 0.000000024803 -0.000000004076 -0.000000000747 0.000000007211 -0.000000000830 0.000000240461 +1 1645005908.103469848633 74 75 0.005239355570 -0.006690197943 -0.001346041897 0.000002110025 0.000383511679 0.000870726890 0.000000088096 -0.000000020346 0.000000001268 -0.000000002250 -0.000000013524 0.000000023906 -0.000000020346 0.000000013591 -0.000000000702 0.000000002211 0.000000003331 -0.000000003807 0.000000001268 -0.000000000702 0.000000014950 -0.000000008801 -0.000000000587 -0.000000000765 -0.000000002250 0.000000002211 -0.000000008801 0.000000284454 -0.000000018287 0.000000007836 -0.000000013524 0.000000003331 -0.000000000587 -0.000000018287 0.000000257037 -0.000000000613 0.000000023906 -0.000000003807 -0.000000000765 0.000000007836 -0.000000000613 0.000000238618 +1 1645005908.202678442001 75 76 0.006817804937 -0.009726863760 -0.000518636839 -0.000056178504 -0.000656898114 0.002883550854 0.000000088802 -0.000000020528 0.000000001285 -0.000000002483 -0.000000014210 0.000000023286 -0.000000020528 0.000000013726 -0.000000000706 0.000000002292 0.000000003404 -0.000000003359 0.000000001285 -0.000000000706 0.000000014784 -0.000000008320 -0.000000000291 -0.000000000649 -0.000000002483 0.000000002292 -0.000000008320 0.000000283431 -0.000000018165 0.000000008157 -0.000000014210 0.000000003404 -0.000000000291 -0.000000018165 0.000000255778 -0.000000000733 0.000000023286 -0.000000003359 -0.000000000649 0.000000008157 -0.000000000733 0.000000239003 +1 1645005908.303349256516 76 77 0.008880185968 -0.011072420154 0.002005117294 0.000364039331 0.000621317606 0.001463440780 0.000000089528 -0.000000020704 0.000000001309 -0.000000002811 -0.000000014365 0.000000022726 -0.000000020704 0.000000013849 -0.000000000704 0.000000002410 0.000000003408 -0.000000003006 0.000000001309 -0.000000000704 0.000000014662 -0.000000008440 -0.000000000182 -0.000000000582 -0.000000002811 0.000000002410 -0.000000008440 0.000000282898 -0.000000018035 0.000000008233 -0.000000014365 0.000000003408 -0.000000000182 -0.000000018035 0.000000254217 -0.000000000493 0.000000022726 -0.000000003006 -0.000000000582 0.000000008233 -0.000000000493 0.000000239589 +1 1645005908.403426647186 77 78 0.013214057998 -0.010022380808 0.003548089026 -0.000420898660 -0.000687846562 0.004288662412 0.000000090426 -0.000000021194 0.000000001135 -0.000000003300 -0.000000014313 0.000000022653 -0.000000021194 0.000000013961 -0.000000000638 0.000000002484 0.000000003448 -0.000000002827 0.000000001135 -0.000000000638 0.000000014468 -0.000000009028 -0.000000000586 -0.000000000561 -0.000000003300 0.000000002484 -0.000000009028 0.000000283166 -0.000000017970 0.000000008066 -0.000000014313 0.000000003448 -0.000000000586 -0.000000017970 0.000000253551 -0.000000000440 0.000000022653 -0.000000002827 -0.000000000561 0.000000008066 -0.000000000440 0.000000240482 +1 1645005908.504360437393 78 79 0.020180381727 -0.009462892108 0.001955139025 0.000042094269 0.000191650148 0.004876347524 0.000000091477 -0.000000021398 0.000000001260 -0.000000003754 -0.000000014396 0.000000022564 -0.000000021398 0.000000014143 -0.000000000654 0.000000002576 0.000000003359 -0.000000002684 0.000000001260 -0.000000000654 0.000000014202 -0.000000008908 -0.000000001365 -0.000000000321 -0.000000003754 0.000000002576 -0.000000008908 0.000000283742 -0.000000018036 0.000000007708 -0.000000014396 0.000000003359 -0.000000001365 -0.000000018036 0.000000252709 -0.000000000672 0.000000022564 -0.000000002684 -0.000000000321 0.000000007708 -0.000000000672 0.000000242092 +1 1645005908.602572441101 79 80 0.027583841746 -0.007492189755 -0.000358945670 -0.000316065525 0.000410807340 0.004627049462 0.000000091890 -0.000000021219 0.000000001348 -0.000000004184 -0.000000014661 0.000000022474 -0.000000021219 0.000000014196 -0.000000000667 0.000000002818 0.000000003383 -0.000000002589 0.000000001348 -0.000000000667 0.000000014612 -0.000000008669 -0.000000001276 -0.000000000350 -0.000000004184 0.000000002818 -0.000000008669 0.000000284516 -0.000000017822 0.000000007904 -0.000000014661 0.000000003383 -0.000000001276 -0.000000017822 0.000000252238 -0.000000001096 0.000000022474 -0.000000002589 -0.000000000350 0.000000007904 -0.000000001096 0.000000242589 +1 1645005908.702723264694 80 81 0.037527173709 -0.002432558917 0.000584410457 -0.000295924796 0.000172057833 0.004510494708 0.000000092503 -0.000000021428 0.000000001442 -0.000000004439 -0.000000015161 0.000000022444 -0.000000021428 0.000000014211 -0.000000000702 0.000000002984 0.000000003573 -0.000000002087 0.000000001442 -0.000000000702 0.000000014735 -0.000000008471 -0.000000001226 -0.000000000479 -0.000000004439 0.000000002984 -0.000000008471 0.000000283202 -0.000000016932 0.000000008633 -0.000000015161 0.000000003573 -0.000000001226 -0.000000016932 0.000000250551 -0.000000001604 0.000000022444 -0.000000002087 -0.000000000479 0.000000008633 -0.000000001604 0.000000241814 +1 1645005908.805076837540 81 82 0.044756539731 -0.000661973435 0.002178494075 -0.000609676361 0.000455606665 0.001515229361 0.000000093517 -0.000000021845 0.000000001449 -0.000000004558 -0.000000015501 0.000000022061 -0.000000021845 0.000000014347 -0.000000000736 0.000000003042 0.000000003685 -0.000000001671 0.000000001449 -0.000000000736 0.000000014651 -0.000000008202 -0.000000002017 -0.000000000480 -0.000000004558 0.000000003042 -0.000000008202 0.000000281630 -0.000000016156 0.000000009346 -0.000000015501 0.000000003685 -0.000000002017 -0.000000016156 0.000000248540 -0.000000001725 0.000000022061 -0.000000001671 -0.000000000480 0.000000009346 -0.000000001725 0.000000241443 +1 1645005908.902604103088 82 83 0.045146081256 -0.002522614562 0.003315811413 0.000215551457 0.001434294478 -0.002791558929 0.000000095523 -0.000000022658 0.000000001480 -0.000000004967 -0.000000015958 0.000000021670 -0.000000022658 0.000000014741 -0.000000000778 0.000000003208 0.000000003828 -0.000000001330 0.000000001480 -0.000000000778 0.000000014730 -0.000000007858 -0.000000002516 -0.000000000414 -0.000000004967 0.000000003208 -0.000000007858 0.000000281929 -0.000000016137 0.000000009579 -0.000000015958 0.000000003828 -0.000000002516 -0.000000016137 0.000000249394 -0.000000001587 0.000000021670 -0.000000001330 -0.000000000414 0.000000009579 -0.000000001587 0.000000243425 +1 1645005909.003700494766 83 84 0.050021001197 -0.004220963579 0.004624075743 0.000077137405 0.000861877284 -0.003931105805 0.000000097156 -0.000000023529 0.000000001589 -0.000000005276 -0.000000016382 0.000000021184 -0.000000023529 0.000000015119 -0.000000000815 0.000000003220 0.000000004081 -0.000000000901 0.000000001589 -0.000000000815 0.000000014639 -0.000000007417 -0.000000002865 -0.000000000427 -0.000000005276 0.000000003220 -0.000000007417 0.000000278551 -0.000000015767 0.000000008985 -0.000000016382 0.000000004081 -0.000000002865 -0.000000015767 0.000000248181 -0.000000001156 0.000000021184 -0.000000000901 -0.000000000427 0.000000008985 -0.000000001156 0.000000244236 +1 1645005909.105985641479 84 85 0.055306161593 -0.005644162807 0.003857391803 0.000176010803 -0.000406336505 -0.002605005858 0.000000098538 -0.000000024037 0.000000001724 -0.000000005135 -0.000000016925 0.000000020936 -0.000000024037 0.000000015386 -0.000000000857 0.000000002936 0.000000004319 -0.000000000573 0.000000001724 -0.000000000857 0.000000014932 -0.000000007133 -0.000000003111 -0.000000000413 -0.000000005135 0.000000002936 -0.000000007133 0.000000274491 -0.000000015304 0.000000008087 -0.000000016925 0.000000004319 -0.000000003111 -0.000000015304 0.000000247261 -0.000000000996 0.000000020936 -0.000000000573 -0.000000000413 0.000000008087 -0.000000000996 0.000000244577 +1 1645005909.204295873642 85 86 0.053724760856 -0.007867742945 0.002362917646 0.000765560740 0.000896643338 -0.004875595418 0.000000100571 -0.000000024471 0.000000001820 -0.000000004997 -0.000000017493 0.000000020789 -0.000000024471 0.000000015583 -0.000000000907 0.000000002811 0.000000004425 -0.000000000103 0.000000001820 -0.000000000907 0.000000014997 -0.000000007018 -0.000000003853 -0.000000000322 -0.000000004997 0.000000002811 -0.000000007018 0.000000274574 -0.000000014663 0.000000007432 -0.000000017493 0.000000004425 -0.000000003853 -0.000000014663 0.000000250668 -0.000000000903 0.000000020789 -0.000000000103 -0.000000000322 0.000000007432 -0.000000000903 0.000000246442 +1 1645005909.301892518997 86 87 0.052598742656 -0.009586402032 -0.001683730482 0.000100902220 0.000694494544 -0.005642354434 0.000000101572 -0.000000024847 0.000000001621 -0.000000004490 -0.000000017492 0.000000020848 -0.000000024847 0.000000015608 -0.000000000748 0.000000002623 0.000000004392 0.000000000187 0.000000001621 -0.000000000748 0.000000014697 -0.000000007106 -0.000000005019 -0.000000000290 -0.000000004490 0.000000002623 -0.000000007106 0.000000274211 -0.000000014372 0.000000006733 -0.000000017492 0.000000004392 -0.000000005019 -0.000000014372 0.000000253261 -0.000000001008 0.000000020848 0.000000000187 -0.000000000290 0.000000006733 -0.000000001008 0.000000245747 +1 1645005909.403104543686 87 88 0.056969463690 -0.013394491540 -0.004907688527 0.000313438663 -0.000019403912 -0.004039402462 0.000000101997 -0.000000025070 0.000000001444 -0.000000003922 -0.000000017659 0.000000021025 -0.000000025070 0.000000015563 -0.000000000684 0.000000002332 0.000000004455 0.000000000663 0.000000001444 -0.000000000684 0.000000015000 -0.000000007663 -0.000000005985 -0.000000000282 -0.000000003922 0.000000002332 -0.000000007663 0.000000273327 -0.000000013785 0.000000006215 -0.000000017659 0.000000004455 -0.000000005985 -0.000000013785 0.000000254655 -0.000000001345 0.000000021025 0.000000000663 -0.000000000282 0.000000006215 -0.000000001345 0.000000243873 +1 1645005909.501364469528 88 89 0.059762523379 -0.018567501801 -0.005975427554 0.000327232703 -0.000227548183 -0.003605858449 0.000000103041 -0.000000025390 0.000000001364 -0.000000003806 -0.000000017859 0.000000021654 -0.000000025390 0.000000015487 -0.000000000758 0.000000002227 0.000000004664 0.000000001355 0.000000001364 -0.000000000758 0.000000015660 -0.000000008246 -0.000000006861 -0.000000000445 -0.000000003806 0.000000002227 -0.000000008246 0.000000275936 -0.000000013446 0.000000006125 -0.000000017859 0.000000004664 -0.000000006861 -0.000000013446 0.000000258726 -0.000000001189 0.000000021654 0.000000001355 -0.000000000445 0.000000006125 -0.000000001189 0.000000245723 +1 1645005909.601603746414 89 90 0.063283735135 -0.025948706002 -0.004113490990 0.000699255821 0.000279829928 -0.004494526640 0.000000104093 -0.000000025791 0.000000001237 -0.000000003691 -0.000000017610 0.000000022878 -0.000000025791 0.000000015579 -0.000000000780 0.000000002254 0.000000004734 0.000000001701 0.000000001237 -0.000000000780 0.000000016004 -0.000000008598 -0.000000007978 -0.000000000633 -0.000000003691 0.000000002254 -0.000000008598 0.000000279345 -0.000000013365 0.000000006383 -0.000000017610 0.000000004734 -0.000000007978 -0.000000013365 0.000000262863 -0.000000000672 0.000000022878 0.000000001701 -0.000000000633 0.000000006383 -0.000000000672 0.000000249192 +1 1645005909.706259727478 90 91 0.064392495618 -0.030468368848 0.003371720855 0.000963763803 0.001428163796 -0.005769357514 0.000000105109 -0.000000026290 0.000000001203 -0.000000003464 -0.000000017388 0.000000024107 -0.000000026290 0.000000015676 -0.000000000845 0.000000002355 0.000000004759 0.000000001923 0.000000001203 -0.000000000845 0.000000016121 -0.000000008819 -0.000000009190 -0.000000000794 -0.000000003464 0.000000002355 -0.000000008819 0.000000281196 -0.000000012993 0.000000007076 -0.000000017388 0.000000004759 -0.000000009190 -0.000000012993 0.000000265075 -0.000000000134 0.000000024107 0.000000001923 -0.000000000794 0.000000007076 -0.000000000134 0.000000251988 +1 1645005909.802580833435 91 92 0.062365238940 -0.025844183621 0.009407834302 0.000193492681 -0.000172715186 -0.001464084852 0.000000108795 -0.000000027506 0.000000001224 -0.000000003570 -0.000000017752 0.000000025071 -0.000000027506 0.000000016130 -0.000000000716 0.000000002314 0.000000004730 0.000000002329 0.000000001224 -0.000000000716 0.000000015944 -0.000000008872 -0.000000010128 -0.000000000541 -0.000000003570 0.000000002314 -0.000000008872 0.000000285835 -0.000000013072 0.000000007112 -0.000000017752 0.000000004730 -0.000000010128 -0.000000013072 0.000000269332 -0.000000000648 0.000000025071 0.000000002329 -0.000000000541 0.000000007112 -0.000000000648 0.000000257045 +1 1645005909.903105735779 92 93 0.068784629133 -0.022584999076 0.007314965158 -0.000173602250 -0.000981733227 0.004132128312 0.000000111499 -0.000000028105 0.000000001268 -0.000000003943 -0.000000017982 0.000000024931 -0.000000028105 0.000000016532 -0.000000000666 0.000000002326 0.000000004775 0.000000003407 0.000000001268 -0.000000000666 0.000000015946 -0.000000009222 -0.000000011103 -0.000000000468 -0.000000003943 0.000000002326 -0.000000009222 0.000000288991 -0.000000012266 0.000000007214 -0.000000017982 0.000000004775 -0.000000011103 -0.000000012266 0.000000270553 -0.000000000807 0.000000024931 0.000000003407 -0.000000000468 0.000000007214 -0.000000000807 0.000000260827 +1 1645005910.002853631973 93 94 0.064100403759 -0.021302988329 0.003328486309 -0.000115742040 0.000269762022 0.004338534235 0.000000113187 -0.000000028301 0.000000001447 -0.000000004451 -0.000000017932 0.000000024371 -0.000000028301 0.000000016578 -0.000000000743 0.000000002509 0.000000004720 0.000000004613 0.000000001447 -0.000000000743 0.000000015755 -0.000000009213 -0.000000011391 -0.000000000441 -0.000000004451 0.000000002509 -0.000000009213 0.000000291379 -0.000000012472 0.000000007953 -0.000000017932 0.000000004720 -0.000000011391 -0.000000012472 0.000000268086 -0.000000000754 0.000000024371 0.000000004613 -0.000000000441 0.000000007953 -0.000000000754 0.000000263152 +1 1645005910.102889776230 94 95 0.058187415660 -0.021959187640 -0.002395190107 -0.000230382660 0.000479950528 0.004722778418 0.000000114411 -0.000000028797 0.000000001597 -0.000000005011 -0.000000018281 0.000000023886 -0.000000028797 0.000000016614 -0.000000000790 0.000000002676 0.000000004786 0.000000005410 0.000000001597 -0.000000000790 0.000000015406 -0.000000009290 -0.000000011420 -0.000000000347 -0.000000005011 0.000000002676 -0.000000009290 0.000000295984 -0.000000013121 0.000000008785 -0.000000018281 0.000000004786 -0.000000011420 -0.000000013121 0.000000268118 -0.000000001232 0.000000023886 0.000000005410 -0.000000000347 0.000000008785 -0.000000001232 0.000000266828 +1 1645005910.203362464905 95 96 0.056619890717 -0.019994351111 -0.005006673376 -0.000078075128 -0.001163126359 0.005855695866 0.000000113848 -0.000000028816 0.000000001425 -0.000000004988 -0.000000018908 0.000000023457 -0.000000028816 0.000000016671 -0.000000000790 0.000000002758 0.000000005082 0.000000006329 0.000000001425 -0.000000000790 0.000000015387 -0.000000009472 -0.000000012713 -0.000000000564 -0.000000004988 0.000000002758 -0.000000009472 0.000000298099 -0.000000012668 0.000000009794 -0.000000018908 0.000000005082 -0.000000012713 -0.000000012668 0.000000271956 -0.000000001334 0.000000023457 0.000000006329 -0.000000000564 0.000000009794 -0.000000001334 0.000000269624 +1 1645005910.304825067520 96 97 0.061086566852 -0.020286198113 -0.004843598885 0.000071324355 -0.000601247565 0.004021009982 0.000000117880 -0.000000029623 0.000000001330 -0.000000004609 -0.000000019305 0.000000024344 -0.000000029623 0.000000017038 -0.000000000772 0.000000002859 0.000000005198 0.000000006765 0.000000001330 -0.000000000772 0.000000015981 -0.000000010001 -0.000000014626 -0.000000000763 -0.000000004609 0.000000002859 -0.000000010001 0.000000301426 -0.000000011273 0.000000010740 -0.000000019305 0.000000005198 -0.000000014626 -0.000000011273 0.000000276684 -0.000000001301 0.000000024344 0.000000006765 -0.000000000763 0.000000010740 -0.000000001301 0.000000271470 +1 1645005910.406254529953 97 98 0.068850355530 -0.020207965588 -0.000165344613 -0.000090050301 -0.000186271132 0.000092130259 0.000000120621 -0.000000030569 0.000000001323 -0.000000004733 -0.000000019353 0.000000024043 -0.000000030569 0.000000017239 -0.000000000763 0.000000002867 0.000000005258 0.000000007204 0.000000001323 -0.000000000763 0.000000016177 -0.000000010171 -0.000000016172 -0.000000000720 -0.000000004733 0.000000002867 -0.000000010171 0.000000306276 -0.000000010789 0.000000010460 -0.000000019353 0.000000005258 -0.000000016172 -0.000000010789 0.000000281858 -0.000000001403 0.000000024043 0.000000007204 -0.000000000720 0.000000010460 -0.000000001403 0.000000273593 +1 1645005910.503567218781 98 99 0.074162405982 -0.018493810987 0.006239807671 -0.000159910511 0.000247959785 -0.004015256391 0.000000119575 -0.000000030905 0.000000001301 -0.000000005274 -0.000000019174 0.000000021991 -0.000000030905 0.000000017327 -0.000000000670 0.000000002923 0.000000005171 0.000000008213 0.000000001301 -0.000000000670 0.000000015751 -0.000000009557 -0.000000017503 -0.000000000420 -0.000000005274 0.000000002923 -0.000000009557 0.000000310028 -0.000000012146 0.000000010051 -0.000000019174 0.000000005171 -0.000000017503 -0.000000012146 0.000000288635 -0.000000001805 0.000000021991 0.000000008213 -0.000000000420 0.000000010051 -0.000000001805 0.000000275801 +1 1645005910.606616973877 99 100 0.080910102851 -0.020164208266 0.008661834846 0.000071976494 0.000898942665 -0.008152706685 0.000000119264 -0.000000031192 0.000000001273 -0.000000006002 -0.000000018615 0.000000020354 -0.000000031192 0.000000017400 -0.000000000693 0.000000003292 0.000000005053 0.000000009012 0.000000001273 -0.000000000693 0.000000015378 -0.000000009114 -0.000000018448 -0.000000000406 -0.000000006002 0.000000003292 -0.000000009114 0.000000310497 -0.000000014070 0.000000010804 -0.000000018615 0.000000005053 -0.000000018448 -0.000000014070 0.000000292764 -0.000000001847 0.000000020354 0.000000009012 -0.000000000406 0.000000010804 -0.000000001847 0.000000274724 +1 1645005910.704123735428 100 101 0.075961027781 -0.021808661780 0.005934638148 0.000450549674 0.001441942574 -0.012050260842 0.000000119262 -0.000000031760 0.000000001275 -0.000000006676 -0.000000018333 0.000000018441 -0.000000031760 0.000000017569 -0.000000000766 0.000000003498 0.000000005201 0.000000009821 0.000000001275 -0.000000000766 0.000000015022 -0.000000009165 -0.000000018987 -0.000000000583 -0.000000006676 0.000000003498 -0.000000009165 0.000000310167 -0.000000014428 0.000000011047 -0.000000018333 0.000000005201 -0.000000018987 -0.000000014428 0.000000295633 -0.000000001178 0.000000018441 0.000000009821 -0.000000000583 0.000000011047 -0.000000001178 0.000000274189 +1 1645005910.803392648697 101 102 0.078482673806 -0.023461910881 0.002857169426 0.000627784303 0.000447074489 -0.011957121987 0.000000119092 -0.000000032121 0.000000001304 -0.000000007124 -0.000000018655 0.000000017333 -0.000000032121 0.000000017789 -0.000000000790 0.000000003564 0.000000005412 0.000000010492 0.000000001304 -0.000000000790 0.000000015124 -0.000000009379 -0.000000019322 -0.000000000660 -0.000000007124 0.000000003564 -0.000000009379 0.000000308153 -0.000000013438 0.000000010595 -0.000000018655 0.000000005412 -0.000000019322 -0.000000013438 0.000000297079 -0.000000000659 0.000000017333 0.000000010492 -0.000000000660 0.000000010595 -0.000000000659 0.000000273340 +1 1645005910.902909994125 102 103 0.083189939682 -0.023490784963 -0.003117914173 0.000480762492 0.000068118626 -0.007186591478 0.000000118524 -0.000000032013 0.000000001297 -0.000000007106 -0.000000018773 0.000000016705 -0.000000032013 0.000000017971 -0.000000000795 0.000000003426 0.000000005515 0.000000011670 0.000000001297 -0.000000000795 0.000000015444 -0.000000009493 -0.000000020369 -0.000000000697 -0.000000007106 0.000000003426 -0.000000009493 0.000000306218 -0.000000012861 0.000000010051 -0.000000018773 0.000000005515 -0.000000020369 -0.000000012861 0.000000299271 -0.000000000380 0.000000016705 0.000000011670 -0.000000000697 0.000000010051 -0.000000000380 0.000000272341 +1 1645005911.004977464676 103 104 0.087315384998 -0.025157189183 -0.000393238317 0.000391193097 0.000467548951 -0.002126631756 0.000000118945 -0.000000031979 0.000000001227 -0.000000007043 -0.000000019158 0.000000016413 -0.000000031979 0.000000018203 -0.000000000668 0.000000003217 0.000000005547 0.000000013380 0.000000001227 -0.000000000668 0.000000015676 -0.000000009973 -0.000000022149 -0.000000000580 -0.000000007043 0.000000003217 -0.000000009973 0.000000309925 -0.000000012331 0.000000009781 -0.000000019158 0.000000005547 -0.000000022149 -0.000000012331 0.000000306142 -0.000000000590 0.000000016413 0.000000013380 -0.000000000580 0.000000009781 -0.000000000590 0.000000276867 +1 1645005911.101893663406 104 105 0.084323245144 -0.023813002632 0.014804471985 0.000199032956 0.000054108090 -0.000198912619 0.000000120217 -0.000000032143 0.000000001107 -0.000000007075 -0.000000019378 0.000000016191 -0.000000032143 0.000000018424 -0.000000000637 0.000000003358 0.000000005540 0.000000015177 0.000000001107 -0.000000000637 0.000000015707 -0.000000010434 -0.000000024174 -0.000000000604 -0.000000007075 0.000000003358 -0.000000010434 0.000000316238 -0.000000012013 0.000000010568 -0.000000019378 0.000000005540 -0.000000024174 -0.000000012013 0.000000316136 -0.000000000812 0.000000016191 0.000000015177 -0.000000000604 0.000000010568 -0.000000000812 0.000000285489 +1 1645005911.202757835388 105 106 0.091890144553 -0.022457558791 0.017747265772 -0.000060833543 -0.000571579936 0.006619650214 0.000000121531 -0.000000032560 0.000000000738 -0.000000006643 -0.000000017693 0.000000015251 -0.000000032560 0.000000018747 -0.000000000671 0.000000003314 0.000000005222 0.000000017170 0.000000000738 -0.000000000671 0.000000016021 -0.000000011220 -0.000000026178 -0.000000000837 -0.000000006643 0.000000003314 -0.000000011220 0.000000321155 -0.000000010921 0.000000011729 -0.000000017693 0.000000005222 -0.000000026178 -0.000000010921 0.000000322863 -0.000000000229 0.000000015251 0.000000017170 -0.000000000837 0.000000011729 -0.000000000229 0.000000293530 +1 1645005911.304598569870 106 107 0.099315592633 -0.013707866919 0.008929200938 -0.000593510612 0.000431991390 0.006542184812 0.000000122613 -0.000000032815 0.000000000312 -0.000000006490 -0.000000015355 0.000000013258 -0.000000032815 0.000000018837 -0.000000000526 0.000000003118 0.000000004545 0.000000018930 0.000000000312 -0.000000000526 0.000000016756 -0.000000012375 -0.000000028786 -0.000000000787 -0.000000006490 0.000000003118 -0.000000012375 0.000000326678 -0.000000008324 0.000000013042 -0.000000015355 0.000000004545 -0.000000028786 -0.000000008324 0.000000328892 0.000000000490 0.000000013258 0.000000018930 -0.000000000787 0.000000013042 0.000000000490 0.000000298943 +1 1645005911.402243375778 107 108 0.104785384813 -0.008105171994 -0.002214641892 -0.000860946379 -0.000993392829 0.010948470463 0.000000123089 -0.000000033137 0.000000000099 -0.000000006942 -0.000000014277 0.000000011341 -0.000000033137 0.000000019181 -0.000000000321 0.000000003202 0.000000004018 0.000000020680 0.000000000099 -0.000000000321 0.000000016999 -0.000000012404 -0.000000030745 -0.000000000434 -0.000000006942 0.000000003202 -0.000000012404 0.000000331673 -0.000000007654 0.000000013557 -0.000000014277 0.000000004018 -0.000000030745 -0.000000007654 0.000000336509 0.000000000255 0.000000011341 0.000000020680 -0.000000000434 0.000000013557 0.000000000255 0.000000304958 +1 1645005911.504294157028 108 109 0.115689898818 -0.005982363005 -0.006109069161 -0.000726627622 0.000973576912 0.010908783105 0.000000124858 -0.000000033368 0.000000000274 -0.000000007206 -0.000000014118 0.000000010002 -0.000000033368 0.000000019303 -0.000000000398 0.000000003340 0.000000003987 0.000000022318 0.000000000274 -0.000000000398 0.000000016946 -0.000000012012 -0.000000031995 -0.000000000446 -0.000000007206 0.000000003340 -0.000000012012 0.000000330527 -0.000000006532 0.000000013696 -0.000000014118 0.000000003987 -0.000000031995 -0.000000006532 0.000000339820 0.000000000103 0.000000010002 0.000000022318 -0.000000000446 0.000000013696 0.000000000103 0.000000309344 +1 1645005911.603650331497 109 110 0.112144073030 -0.007022975029 0.001012219037 -0.001163101342 0.000302771644 0.009647953042 0.000000124674 -0.000000033654 0.000000000385 -0.000000008009 -0.000000015177 0.000000007700 -0.000000033654 0.000000019363 -0.000000000509 0.000000003826 0.000000004570 0.000000023824 0.000000000385 -0.000000000509 0.000000016545 -0.000000011791 -0.000000032496 -0.000000000820 -0.000000008009 0.000000003826 -0.000000011791 0.000000328802 -0.000000004861 0.000000014409 -0.000000015177 0.000000004570 -0.000000032496 -0.000000004861 0.000000338575 0.000000000638 0.000000007700 0.000000023824 -0.000000000820 0.000000014409 0.000000000638 0.000000314003 +1 1645005911.705323696136 110 111 0.108334202057 -0.006199322902 0.015439608211 -0.000499640517 0.000622717034 0.005115544113 0.000000121420 -0.000000033136 0.000000000547 -0.000000008867 -0.000000016030 0.000000005761 -0.000000033136 0.000000019118 -0.000000000490 0.000000003998 0.000000004882 0.000000025082 0.000000000547 -0.000000000490 0.000000015937 -0.000000011322 -0.000000032770 -0.000000000779 -0.000000008867 0.000000003998 -0.000000011322 0.000000326263 -0.000000003896 0.000000014707 -0.000000016030 0.000000004882 -0.000000032770 -0.000000003896 0.000000334604 0.000000000414 0.000000005761 0.000000025082 -0.000000000779 0.000000014707 0.000000000414 0.000000319533 +1 1645005911.805619001389 111 112 0.105563859350 -0.007445366800 0.017090699054 -0.000396553302 -0.000236939023 0.001638354551 0.000000119495 -0.000000032859 0.000000000407 -0.000000008551 -0.000000014394 0.000000004296 -0.000000032859 0.000000019006 -0.000000000444 0.000000003710 0.000000004531 0.000000026291 0.000000000407 -0.000000000444 0.000000015977 -0.000000011729 -0.000000035072 -0.000000000757 -0.000000008551 0.000000003710 -0.000000011729 0.000000324713 -0.000000002118 0.000000014718 -0.000000014394 0.000000004531 -0.000000035072 -0.000000002118 0.000000339187 0.000000000583 0.000000004296 0.000000026291 -0.000000000757 0.000000014718 0.000000000583 0.000000324775 +1 1645005911.905131578445 112 113 0.105669045682 -0.010232842109 0.007581705384 0.000117560962 -0.000845623252 -0.002154597809 0.000000117976 -0.000000032837 0.000000000325 -0.000000008707 -0.000000012159 0.000000002537 -0.000000032837 0.000000019001 -0.000000000403 0.000000003735 0.000000003954 0.000000027237 0.000000000325 -0.000000000403 0.000000015480 -0.000000011530 -0.000000035970 -0.000000000732 -0.000000008707 0.000000003735 -0.000000011530 0.000000324418 -0.000000003537 0.000000014543 -0.000000012159 0.000000003954 -0.000000035970 -0.000000003537 0.000000343391 0.000000000796 0.000000002537 0.000000027237 -0.000000000732 0.000000014543 0.000000000796 0.000000327307 +1 1645005912.005542993546 113 114 0.105408723509 -0.006257586189 -0.004647377317 -0.000008164249 -0.000462734466 -0.006034679601 0.000000115866 -0.000000032739 0.000000000255 -0.000000008904 -0.000000010564 0.000000001256 -0.000000032739 0.000000018798 -0.000000000420 0.000000003723 0.000000003731 0.000000027559 0.000000000255 -0.000000000420 0.000000015409 -0.000000012009 -0.000000036888 -0.000000000887 -0.000000008904 0.000000003723 -0.000000012009 0.000000321068 -0.000000000786 0.000000014100 -0.000000010564 0.000000003731 -0.000000036888 -0.000000000786 0.000000345395 0.000000001508 0.000000001256 0.000000027559 -0.000000000887 0.000000014100 0.000000001508 0.000000326200 +1 1645005912.104353666306 114 115 0.104312051462 -0.005788696135 -0.001086666677 0.000673600379 0.002056576062 -0.010721959912 0.000000113948 -0.000000032429 0.000000000239 -0.000000008558 -0.000000009973 0.000000000000 -0.000000032429 0.000000018879 -0.000000000450 0.000000003631 0.000000003684 0.000000029056 0.000000000239 -0.000000000450 0.000000015680 -0.000000012374 -0.000000038521 -0.000000001026 -0.000000008558 0.000000003631 -0.000000012374 0.000000316878 0.000000003031 0.000000013903 -0.000000009973 0.000000003684 -0.000000038521 0.000000003031 0.000000349891 0.000000002089 0.000000000000 0.000000029056 -0.000000001026 0.000000013903 0.000000002089 0.000000329833 +1 1645005912.204403162003 115 116 0.107279768307 -0.009811669098 0.013827848427 0.000095745570 -0.000565487976 -0.008974907831 0.000000111490 -0.000000032117 0.000000000243 -0.000000008340 -0.000000010631 -0.000000000159 -0.000000032117 0.000000018992 -0.000000000434 0.000000003780 0.000000003786 0.000000030191 0.000000000243 -0.000000000434 0.000000014981 -0.000000011345 -0.000000037764 -0.000000000975 -0.000000008340 0.000000003780 -0.000000011345 0.000000310920 0.000000002683 0.000000013634 -0.000000010631 0.000000003786 -0.000000037764 0.000000002683 0.000000348113 0.000000001943 -0.000000000159 0.000000030191 -0.000000000975 0.000000013634 0.000000001943 0.000000334016 +1 1645005912.304095268250 116 117 0.106183988090 -0.010713891373 0.022572940623 0.000391186142 0.000422801729 -0.011307080283 0.000000109508 -0.000000031579 0.000000000488 -0.000000007880 -0.000000010858 0.000000000029 -0.000000031579 0.000000018604 -0.000000000518 0.000000003456 0.000000003941 0.000000030323 0.000000000488 -0.000000000518 0.000000014084 -0.000000010751 -0.000000036936 -0.000000001088 -0.000000007880 0.000000003456 -0.000000010751 0.000000305213 0.000000003878 0.000000012732 -0.000000010858 0.000000003941 -0.000000036936 0.000000003878 0.000000348036 0.000000002303 0.000000000029 0.000000030323 -0.000000001088 0.000000012732 0.000000002303 0.000000336507 +1 1645005912.402587413788 117 118 0.103151905269 -0.007851188457 0.016492569166 -0.000133641646 -0.000389791053 -0.009425723025 0.000000107590 -0.000000031367 0.000000000521 -0.000000007632 -0.000000009840 -0.000000000231 -0.000000031367 0.000000018521 -0.000000000458 0.000000002965 0.000000003586 0.000000031051 0.000000000521 -0.000000000458 0.000000014103 -0.000000011073 -0.000000039130 -0.000000000784 -0.000000007632 0.000000002965 -0.000000011073 0.000000302435 0.000000007661 0.000000011183 -0.000000009840 0.000000003586 -0.000000039130 0.000000007661 0.000000360891 0.000000001946 -0.000000000231 0.000000031051 -0.000000000784 0.000000011183 0.000000001946 0.000000341818 +1 1645005912.504237890244 118 119 0.102245964023 -0.001922173039 0.001980514565 0.000009290998 0.000695611149 -0.010118021293 0.000000106167 -0.000000031034 0.000000000655 -0.000000007967 -0.000000009572 -0.000000000164 -0.000000031034 0.000000018711 -0.000000000505 0.000000002829 0.000000003587 0.000000033047 0.000000000655 -0.000000000505 0.000000014640 -0.000000011217 -0.000000042101 -0.000000000664 -0.000000007967 0.000000002829 -0.000000011217 0.000000298544 0.000000009862 0.000000010133 -0.000000009572 0.000000003587 -0.000000042101 0.000000009862 0.000000373640 0.000000002635 -0.000000000164 0.000000033047 -0.000000000664 0.000000010133 0.000000002635 0.000000351994 +1 1645005912.604142665863 119 120 0.095380530294 -0.001108012995 -0.003083408103 0.000564478649 0.001636924297 -0.014723225025 0.000000103907 -0.000000030366 0.000000000769 -0.000000008451 -0.000000010034 -0.000000001312 -0.000000030366 0.000000018725 -0.000000000550 0.000000003003 0.000000003747 0.000000034899 0.000000000769 -0.000000000550 0.000000014355 -0.000000010463 -0.000000042267 -0.000000000698 -0.000000008451 0.000000003003 -0.000000010463 0.000000293930 0.000000008522 0.000000009804 -0.000000010034 0.000000003747 -0.000000042267 0.000000008522 0.000000375353 0.000000002938 -0.000000001312 0.000000034899 -0.000000000698 0.000000009804 0.000000002938 0.000000360377 +1 1645005912.702849626541 120 121 0.093286512015 -0.005930663651 0.003868319453 0.000605384071 0.000126378577 -0.016197853324 0.000000099769 -0.000000029448 0.000000000826 -0.000000008585 -0.000000009969 -0.000000002554 -0.000000029448 0.000000018277 -0.000000000493 0.000000003133 0.000000003425 0.000000035023 0.000000000826 -0.000000000493 0.000000013658 -0.000000009979 -0.000000041204 -0.000000000428 -0.000000008585 0.000000003133 -0.000000009979 0.000000289021 0.000000008461 0.000000009199 -0.000000009969 0.000000003425 -0.000000041204 0.000000008461 0.000000371803 0.000000000932 -0.000000002554 0.000000035023 -0.000000000428 0.000000009199 0.000000000932 0.000000361967 +1 1645005912.802519559860 121 122 0.100987173190 0.000714157226 0.010878686943 0.000910503223 -0.000408491888 -0.016040457828 0.000000095734 -0.000000028309 0.000000000990 -0.000000008628 -0.000000009816 -0.000000001987 -0.000000028309 0.000000017686 -0.000000000553 0.000000003081 0.000000003373 0.000000034512 0.000000000990 -0.000000000553 0.000000013034 -0.000000009194 -0.000000040584 -0.000000000408 -0.000000008628 0.000000003081 -0.000000009194 0.000000282506 0.000000007004 0.000000008055 -0.000000009816 0.000000003373 -0.000000040584 0.000000007004 0.000000370148 0.000000000056 -0.000000001987 0.000000034512 -0.000000000408 0.000000008055 0.000000000056 0.000000360170 +1 1645005912.902120351791 122 123 0.105744414544 0.002989777360 0.008055909981 0.000522713016 -0.000712091928 -0.015860847116 0.000000092000 -0.000000026892 0.000000001096 -0.000000007960 -0.000000008934 -0.000000001199 -0.000000026892 0.000000017033 -0.000000000612 0.000000002687 0.000000003198 0.000000034106 0.000000001096 -0.000000000612 0.000000012601 -0.000000008599 -0.000000041253 -0.000000000537 -0.000000007960 0.000000002687 -0.000000008599 0.000000278439 0.000000006637 0.000000006944 -0.000000008934 0.000000003198 -0.000000041253 0.000000006637 0.000000377733 0.000000000207 -0.000000001199 0.000000034106 -0.000000000537 0.000000006944 0.000000000207 0.000000356190 +1 1645005913.005158424377 123 124 0.108371709292 0.001027107202 0.003562412218 0.000635368012 0.002115674008 -0.017401447292 0.000000088452 -0.000000025564 0.000000000949 -0.000000006689 -0.000000006146 -0.000000000882 -0.000000025564 0.000000016427 -0.000000000555 0.000000002086 0.000000002362 0.000000033970 0.000000000949 -0.000000000555 0.000000012646 -0.000000008906 -0.000000043562 -0.000000000533 -0.000000006689 0.000000002086 -0.000000008906 0.000000277482 0.000000008766 0.000000005869 -0.000000006146 0.000000002362 -0.000000043562 0.000000008766 0.000000394236 -0.000000000337 -0.000000000882 0.000000033970 -0.000000000533 0.000000005869 -0.000000000337 0.000000354101 +1 1645005913.103281021118 124 125 0.102758309871 -0.002935497442 -0.002834141245 0.000992244923 0.000603097177 -0.010505902649 0.000000084679 -0.000000024480 0.000000000694 -0.000000006203 -0.000000004434 -0.000000000786 -0.000000024480 0.000000015926 -0.000000000460 0.000000001960 0.000000001912 0.000000034312 0.000000000694 -0.000000000460 0.000000012635 -0.000000008930 -0.000000045449 -0.000000000614 -0.000000006203 0.000000001960 -0.000000008930 0.000000278203 0.000000010319 0.000000005903 -0.000000004434 0.000000001912 -0.000000045449 0.000000010319 0.000000412749 0.000000000610 -0.000000000786 0.000000034312 -0.000000000614 0.000000005903 0.000000000610 0.000000357488 +1 1645005913.203158855438 125 126 0.106915139328 -0.005362769587 0.005671755512 0.000982730101 0.001719758282 -0.007181197185 0.000000079753 -0.000000022565 0.000000000775 -0.000000006067 -0.000000004255 -0.000000000741 -0.000000022565 0.000000015327 -0.000000000457 0.000000001948 0.000000001834 0.000000036065 0.000000000775 -0.000000000457 0.000000012307 -0.000000008166 -0.000000046153 -0.000000000719 -0.000000006067 0.000000001948 -0.000000008166 0.000000275329 0.000000010591 0.000000006327 -0.000000004255 0.000000001834 -0.000000046153 0.000000010591 0.000000425655 0.000000001717 -0.000000000741 0.000000036065 -0.000000000719 0.000000006327 0.000000001717 0.000000367832 +1 1645005913.305967569351 126 127 0.114387020145 -0.004210180518 0.015020722820 -0.000184024553 -0.001805515269 0.000786641202 0.000000073336 -0.000000020355 0.000000000681 -0.000000005855 -0.000000003624 -0.000000000918 -0.000000020355 0.000000014826 -0.000000000406 0.000000001943 0.000000001386 0.000000038378 0.000000000681 -0.000000000406 0.000000012043 -0.000000007381 -0.000000046646 -0.000000000532 -0.000000005855 0.000000001943 -0.000000007381 0.000000270632 0.000000009755 0.000000006505 -0.000000003624 0.000000001386 -0.000000046646 0.000000009755 0.000000431777 0.000000000725 -0.000000000918 0.000000038378 -0.000000000532 0.000000006505 0.000000000725 0.000000380235 +1 1645005913.402428627014 127 128 0.113069559789 -0.000704930784 0.014288948611 -0.000164686473 0.000124000769 0.002486017239 0.000000067177 -0.000000018092 0.000000000770 -0.000000005230 -0.000000002337 -0.000000001447 -0.000000018092 0.000000014158 -0.000000000419 0.000000001621 0.000000000810 0.000000040400 0.000000000770 -0.000000000419 0.000000011887 -0.000000006732 -0.000000048215 -0.000000000372 -0.000000005230 0.000000001621 -0.000000006732 0.000000268588 0.000000010266 0.000000005978 -0.000000002337 0.000000000810 -0.000000048215 0.000000010266 0.000000444703 -0.000000000174 -0.000000001447 0.000000040400 -0.000000000372 0.000000005978 -0.000000000174 0.000000394429 +1 1645005913.504843950272 128 129 0.113183456503 -0.003233214799 0.001167230142 -0.000877923266 -0.000380382302 0.004114345669 0.000000058977 -0.000000015334 0.000000000576 -0.000000004492 -0.000000000609 -0.000000003143 -0.000000015334 0.000000013437 -0.000000000338 0.000000001252 0.000000000348 0.000000043093 0.000000000576 -0.000000000338 0.000000010341 -0.000000006120 -0.000000045149 -0.000000000437 -0.000000004492 0.000000001252 -0.000000006120 0.000000262475 0.000000013212 0.000000005183 -0.000000000609 0.000000000348 -0.000000045149 0.000000013212 0.000000443863 0.000000000910 -0.000000003143 0.000000043093 -0.000000000437 0.000000005183 0.000000000910 0.000000408565 +1 1645005913.601838827133 129 130 0.106480526708 -0.001386640502 -0.011980876123 0.000067753467 -0.000046515530 0.006676571512 0.000000048768 -0.000000012110 0.000000000322 -0.000000003692 -0.000000000074 -0.000000005515 -0.000000012110 0.000000012413 -0.000000000216 0.000000000924 0.000000000176 0.000000045221 0.000000000322 -0.000000000216 0.000000008513 -0.000000005462 -0.000000040526 -0.000000000491 -0.000000003692 0.000000000924 -0.000000005462 0.000000255291 0.000000015909 0.000000004681 -0.000000000074 0.000000000176 -0.000000040526 0.000000015909 0.000000437638 0.000000002802 -0.000000005515 0.000000045221 -0.000000000491 0.000000004681 0.000000002802 0.000000418105 +1 1645005913.703332424164 130 131 0.108468668180 -0.000969605556 -0.002385739928 0.000092510686 0.001524255417 0.001870569352 0.000000039925 -0.000000009382 0.000000000084 -0.000000002806 0.000000000135 -0.000000007330 -0.000000009382 0.000000011452 -0.000000000090 0.000000000564 -0.000000000086 0.000000046436 0.000000000084 -0.000000000090 0.000000007595 -0.000000004539 -0.000000038331 -0.000000000262 -0.000000002806 0.000000000564 -0.000000004539 0.000000246399 0.000000015747 0.000000003991 0.000000000135 -0.000000000086 -0.000000038331 0.000000015747 0.000000435816 0.000000003091 -0.000000007330 0.000000046436 -0.000000000262 0.000000003991 0.000000003091 0.000000424368 +1 1645005913.802533626556 131 132 0.104456210240 -0.004904373024 0.013892430815 -0.000421937248 -0.001963824939 0.001738357414 0.000000033848 -0.000000007783 0.000000000093 -0.000000002527 0.000000000077 -0.000000008221 -0.000000007783 0.000000010852 -0.000000000071 0.000000000554 -0.000000000234 0.000000046899 0.000000000093 -0.000000000071 0.000000007219 -0.000000004204 -0.000000037396 -0.000000000113 -0.000000002527 0.000000000554 -0.000000004204 0.000000236147 0.000000019026 0.000000004261 0.000000000077 -0.000000000234 -0.000000037396 0.000000019026 0.000000432716 0.000000002615 -0.000000008221 0.000000046899 -0.000000000113 0.000000004261 0.000000002615 0.000000427562 +1 1645005913.902822494507 132 133 0.105252268899 -0.005892393425 0.020783173943 -0.000244242694 -0.000148631160 -0.003208653521 0.000000028686 -0.000000006554 0.000000000243 -0.000000002229 -0.000000000013 -0.000000008804 -0.000000006554 0.000000010280 -0.000000000154 0.000000000313 -0.000000000124 0.000000046583 0.000000000243 -0.000000000154 0.000000007239 -0.000000004543 -0.000000037125 -0.000000000235 -0.000000002229 0.000000000313 -0.000000004543 0.000000229949 0.000000026748 0.000000003642 -0.000000000013 -0.000000000124 -0.000000037125 0.000000026748 0.000000428474 0.000000002848 -0.000000008804 0.000000046583 -0.000000000235 0.000000003642 0.000000002848 0.000000427422 +1 1645005914.004937648773 133 134 0.109519905526 -0.010991758570 0.010722232350 0.000333129949 -0.000174623972 -0.002993424422 0.000000024093 -0.000000005515 0.000000000350 -0.000000001671 -0.000000000218 -0.000000008888 -0.000000005515 0.000000009905 -0.000000000203 0.000000000110 -0.000000000103 0.000000046779 0.000000000350 -0.000000000203 0.000000006866 -0.000000005080 -0.000000035414 -0.000000000417 -0.000000001671 0.000000000110 -0.000000005080 0.000000225283 0.000000035405 0.000000003187 -0.000000000218 -0.000000000103 -0.000000035414 0.000000035405 0.000000419305 0.000000003321 -0.000000008888 0.000000046779 -0.000000000417 0.000000003187 0.000000003321 0.000000429289 +1 1645005914.104001283646 134 135 0.110141859157 -0.015877687071 -0.003919154582 0.001363151861 -0.000134739250 0.000119752527 0.000000020081 -0.000000004548 0.000000000494 -0.000000000564 -0.000000000812 -0.000000008086 -0.000000004548 0.000000009513 -0.000000000212 -0.000000000183 -0.000000000079 0.000000046971 0.000000000494 -0.000000000212 0.000000006414 -0.000000005263 -0.000000033246 -0.000000000660 -0.000000000564 -0.000000000183 -0.000000005263 0.000000222499 0.000000041553 0.000000002348 -0.000000000812 -0.000000000079 -0.000000033246 0.000000041553 0.000000404705 0.000000004584 -0.000000008086 0.000000046971 -0.000000000660 0.000000002348 0.000000004584 0.000000434501 +1 1645005914.205260753632 135 136 0.112731195464 -0.016646210746 0.002628785458 0.000209927570 0.000614736609 0.002584125950 0.000000016076 -0.000000003462 0.000000000384 0.000000000165 -0.000000000800 -0.000000006658 -0.000000003462 0.000000009059 -0.000000000133 -0.000000000606 -0.000000000101 0.000000046958 0.000000000384 -0.000000000133 0.000000005887 -0.000000005972 -0.000000030611 -0.000000000603 0.000000000165 -0.000000000606 -0.000000005972 0.000000219212 0.000000046082 -0.000000000162 -0.000000000800 -0.000000000101 -0.000000030611 0.000000046082 0.000000380793 0.000000005566 -0.000000006658 0.000000046958 -0.000000000603 -0.000000000162 0.000000005566 0.000000437954 +1 1645005914.304635286331 136 137 0.110109539783 -0.017998414611 0.012810929145 -0.000245315305 0.000890363266 0.003698770995 0.000000013318 -0.000000002600 0.000000000326 0.000000000152 -0.000000000681 -0.000000005410 -0.000000002600 0.000000008819 -0.000000000077 -0.000000000756 -0.000000000134 0.000000047689 0.000000000326 -0.000000000077 0.000000005647 -0.000000007102 -0.000000029962 -0.000000000471 0.000000000152 -0.000000000756 -0.000000007102 0.000000215325 0.000000049153 -0.000000002005 -0.000000000681 -0.000000000134 -0.000000029962 0.000000049153 0.000000369953 0.000000005821 -0.000000005410 0.000000047689 -0.000000000471 -0.000000002005 0.000000005821 0.000000443851 +1 1645005914.402868032455 137 138 0.111083721384 -0.015259298484 0.014103141251 -0.000356166718 -0.001745079695 0.006088809825 0.000000011488 -0.000000002155 0.000000000288 0.000000000087 -0.000000000634 -0.000000004446 -0.000000002155 0.000000008887 -0.000000000073 -0.000000000761 -0.000000000109 0.000000049551 0.000000000288 -0.000000000073 0.000000005632 -0.000000007521 -0.000000030497 -0.000000000488 0.000000000087 -0.000000000761 -0.000000007521 0.000000212402 0.000000048080 -0.000000002791 -0.000000000634 -0.000000000109 -0.000000030497 0.000000048080 0.000000366926 0.000000005864 -0.000000004446 0.000000049551 -0.000000000488 -0.000000002791 0.000000005864 0.000000458229 +1 1645005914.502975702286 138 139 0.113945330153 -0.011103091303 0.010129852948 -0.000443467816 0.000074664741 0.003419050409 0.000000010923 -0.000000002096 0.000000000221 0.000000000018 -0.000000000160 -0.000000003828 -0.000000002096 0.000000008891 -0.000000000128 -0.000000000737 0.000000000133 0.000000050211 0.000000000221 -0.000000000128 0.000000005571 -0.000000007348 -0.000000031094 -0.000000000767 0.000000000018 -0.000000000737 -0.000000007348 0.000000208241 0.000000044856 -0.000000002913 -0.000000000160 0.000000000133 -0.000000031094 0.000000044856 0.000000367491 0.000000006917 -0.000000003828 0.000000050211 -0.000000000767 -0.000000002913 0.000000006917 0.000000464462 +1 1645005914.606390953064 139 140 0.115763415418 -0.013251559319 -0.002593917162 -0.000387095926 -0.000521830202 0.004558964276 0.000000010125 -0.000000001973 0.000000000276 -0.000000000045 -0.000000000302 -0.000000002573 -0.000000001973 0.000000009055 -0.000000000160 -0.000000000851 0.000000000387 0.000000051580 0.000000000276 -0.000000000160 0.000000005080 -0.000000006897 -0.000000029536 -0.000000000890 -0.000000000045 -0.000000000851 -0.000000006897 0.000000205238 0.000000040500 -0.000000003608 -0.000000000302 0.000000000387 -0.000000029536 0.000000040500 0.000000358067 0.000000007639 -0.000000002573 0.000000051580 -0.000000000890 -0.000000003608 0.000000007639 0.000000474766 +1 1645005914.705168962479 140 141 0.115950679870 -0.007920648289 -0.003581919731 -0.000183384951 -0.000202153026 0.006015096621 0.000000008937 -0.000000001667 0.000000000398 -0.000000000007 -0.000000000886 -0.000000000256 -0.000000001667 0.000000009384 -0.000000000113 -0.000000001058 0.000000000199 0.000000054119 0.000000000398 -0.000000000113 0.000000004302 -0.000000006382 -0.000000025529 -0.000000000504 -0.000000000007 -0.000000001058 -0.000000006382 0.000000203274 0.000000035133 -0.000000005395 -0.000000000886 0.000000000199 -0.000000025529 0.000000035133 0.000000328835 0.000000005386 -0.000000000256 0.000000054119 -0.000000000504 -0.000000005395 0.000000005386 0.000000494144 +1 1645005914.803323030472 141 142 0.113344092687 -0.004513999756 0.003881859525 -0.000081391673 0.001628779102 0.003417929098 0.000000007946 -0.000000001342 0.000000000361 0.000000000164 -0.000000000461 0.000000001892 -0.000000001342 0.000000009145 -0.000000000107 -0.000000001104 0.000000000120 0.000000053134 0.000000000361 -0.000000000107 0.000000003877 -0.000000006332 -0.000000023177 -0.000000000447 0.000000000164 -0.000000001104 -0.000000006332 0.000000198500 0.000000032820 -0.000000006363 -0.000000000461 0.000000000120 -0.000000023177 0.000000032820 0.000000305051 0.000000004342 0.000000001892 0.000000053134 -0.000000000447 -0.000000006363 0.000000004342 0.000000489129 +1 1645005914.904057264328 142 143 0.113932415653 -0.008793837129 0.010399809107 -0.000235218052 -0.001425873177 0.005105286124 0.000000007366 -0.000000001153 0.000000000221 0.000000000379 0.000000000457 0.000000003120 -0.000000001153 0.000000008786 -0.000000000141 -0.000000001114 0.000000000245 0.000000051362 0.000000000221 -0.000000000141 0.000000003860 -0.000000006582 -0.000000022852 -0.000000000726 0.000000000379 -0.000000001114 -0.000000006582 0.000000192639 0.000000031559 -0.000000006610 0.000000000457 0.000000000245 -0.000000022852 0.000000031559 0.000000290550 0.000000005271 0.000000003120 0.000000051362 -0.000000000726 -0.000000006610 0.000000005271 0.000000477340 +1 1645005915.005204200745 143 144 0.114962510911 -0.012358710079 0.012393871611 -0.000551754572 -0.000248518402 0.003903856192 0.000000007433 -0.000000001098 0.000000000173 0.000000000396 0.000000000817 0.000000003762 -0.000000001098 0.000000008813 -0.000000000189 -0.000000001302 0.000000000582 0.000000051927 0.000000000173 -0.000000000189 0.000000003938 -0.000000006922 -0.000000022827 -0.000000001046 0.000000000396 -0.000000001302 -0.000000006922 0.000000188507 0.000000031757 -0.000000007689 0.000000000817 0.000000000582 -0.000000022827 0.000000031757 0.000000280243 0.000000007393 0.000000003762 0.000000051927 -0.000000001046 -0.000000007689 0.000000007393 0.000000482670 +1 1645005915.104339361191 144 145 0.108638769015 -0.013109909901 0.001280725320 0.000360075563 -0.000501910880 0.003610744318 0.000000007793 -0.000000001032 0.000000000172 0.000000000318 0.000000000989 0.000000004597 -0.000000001032 0.000000008853 -0.000000000219 -0.000000001613 0.000000000892 0.000000052595 0.000000000172 -0.000000000219 0.000000004122 -0.000000007546 -0.000000023608 -0.000000001206 0.000000000318 -0.000000001613 -0.000000007546 0.000000185838 0.000000036183 -0.000000009621 0.000000000989 0.000000000892 -0.000000023608 0.000000036183 0.000000280865 0.000000009177 0.000000004597 0.000000052595 -0.000000001206 -0.000000009621 0.000000009177 0.000000490642 +1 1645005915.205692529678 145 146 0.109329578702 -0.013304774566 0.000306145880 -0.000023011733 0.000369480071 0.005562445904 0.000000008439 -0.000000000978 0.000000000142 0.000000000381 0.000000001532 0.000000005640 -0.000000000978 0.000000008798 -0.000000000212 -0.000000001840 0.000000000941 0.000000053029 0.000000000142 -0.000000000212 0.000000004685 -0.000000008117 -0.000000027229 -0.000000001233 0.000000000381 -0.000000001840 -0.000000008117 0.000000182654 0.000000041408 -0.000000011114 0.000000001532 0.000000000941 -0.000000027229 0.000000041408 0.000000305374 0.000000009971 0.000000005640 0.000000053029 -0.000000001233 -0.000000011114 0.000000009971 0.000000499247 +1 1645005915.303473472595 146 147 0.102615336962 -0.011723016179 0.016921966183 -0.000598724662 -0.000883449845 0.009912420693 0.000000009050 -0.000000000931 0.000000000141 0.000000000283 0.000000001632 0.000000006520 -0.000000000931 0.000000008841 -0.000000000254 -0.000000001783 0.000000001294 0.000000053986 0.000000000141 -0.000000000254 0.000000005341 -0.000000008689 -0.000000031566 -0.000000001523 0.000000000283 -0.000000001783 -0.000000008689 0.000000180735 0.000000046782 -0.000000010967 0.000000001632 0.000000001294 -0.000000031566 0.000000046782 0.000000337155 0.000000012174 0.000000006520 0.000000053986 -0.000000001523 -0.000000010967 0.000000012174 0.000000511691 +1 1645005915.403654336929 147 148 0.103466727897 -0.010311299427 0.019111226280 -0.000605924141 -0.000721619272 0.014142457072 0.000000009872 -0.000000000937 0.000000000170 0.000000000104 0.000000001623 0.000000006936 -0.000000000937 0.000000009038 -0.000000000289 -0.000000001859 0.000000001533 0.000000055630 0.000000000170 -0.000000000289 0.000000005662 -0.000000008982 -0.000000033316 -0.000000001699 0.000000000104 -0.000000001859 -0.000000008982 0.000000179291 0.000000050127 -0.000000011628 0.000000001623 0.000000001533 -0.000000033316 0.000000050127 0.000000349687 0.000000013494 0.000000006936 0.000000055630 -0.000000001699 -0.000000011628 0.000000013494 0.000000526085 +1 1645005915.502804994583 148 149 0.102555175381 -0.007097360397 0.012460654451 -0.001109214232 -0.001235813727 0.016759657315 0.000000010898 -0.000000001092 0.000000000180 0.000000000034 0.000000001717 0.000000006432 -0.000000001092 0.000000009110 -0.000000000312 -0.000000001774 0.000000001584 0.000000056414 0.000000000180 -0.000000000312 0.000000005889 -0.000000008997 -0.000000034270 -0.000000001851 0.000000000034 -0.000000001774 -0.000000008997 0.000000178153 0.000000052144 -0.000000011187 0.000000001717 0.000000001584 -0.000000034270 0.000000052144 0.000000356560 0.000000013909 0.000000006432 0.000000056414 -0.000000001851 -0.000000011187 0.000000013909 0.000000534819 +1 1645005915.606675386429 149 150 0.110023677701 -0.005261608351 -0.001025304830 -0.000900306716 -0.001729175039 0.017897628554 0.000000011791 -0.000000001314 0.000000000193 -0.000000000199 0.000000001536 0.000000005499 -0.000000001314 0.000000009095 -0.000000000322 -0.000000001698 0.000000001815 0.000000056489 0.000000000193 -0.000000000322 0.000000006056 -0.000000009260 -0.000000035203 -0.000000001976 -0.000000000199 -0.000000001698 -0.000000009260 0.000000180591 0.000000056392 -0.000000010737 0.000000001536 0.000000001815 -0.000000035203 0.000000056392 0.000000367602 0.000000015758 0.000000005499 0.000000056489 -0.000000001976 -0.000000010737 0.000000015758 0.000000538970 +1 1645005915.706191539764 150 151 0.108997921836 -0.002332104651 -0.007968681270 -0.000641057737 0.000297247851 0.013744307307 0.000000012496 -0.000000001488 0.000000000182 -0.000000000437 0.000000001129 0.000000004476 -0.000000001488 0.000000009204 -0.000000000349 -0.000000001745 0.000000002295 0.000000057627 0.000000000182 -0.000000000349 0.000000006180 -0.000000009543 -0.000000036359 -0.000000002262 -0.000000000437 -0.000000001745 -0.000000009543 0.000000185160 0.000000061278 -0.000000010792 0.000000001129 0.000000002295 -0.000000036359 0.000000061278 0.000000385670 0.000000019334 0.000000004476 0.000000057627 -0.000000002262 -0.000000010792 0.000000019334 0.000000551459 +1 1645005915.803260087967 151 152 0.106514602397 -0.001770007540 -0.000096349759 -0.000752589415 0.000118381130 0.009137181083 0.000000012917 -0.000000001667 0.000000000103 -0.000000000308 0.000000001192 0.000000002903 -0.000000001667 0.000000009141 -0.000000000325 -0.000000001618 0.000000002128 0.000000057675 0.000000000103 -0.000000000325 0.000000006523 -0.000000009497 -0.000000039281 -0.000000002198 -0.000000000308 -0.000000001618 -0.000000009497 0.000000189290 0.000000063224 -0.000000009134 0.000000001192 0.000000002128 -0.000000039281 0.000000063224 0.000000417011 0.000000018931 0.000000002903 0.000000057675 -0.000000002198 -0.000000009134 0.000000018931 0.000000555912 +1 1645005915.905807971954 152 153 0.109997173206 -0.004906461686 0.014498026032 -0.000862247837 0.000085107174 0.006427930427 0.000000013310 -0.000000001919 0.000000000086 -0.000000000244 0.000000001170 0.000000001331 -0.000000001919 0.000000008982 -0.000000000223 -0.000000001470 0.000000001408 0.000000056875 0.000000000086 -0.000000000223 0.000000006917 -0.000000008840 -0.000000042953 -0.000000001544 -0.000000000244 -0.000000001470 -0.000000008840 0.000000192614 0.000000059942 -0.000000007491 0.000000001170 0.000000001408 -0.000000042953 0.000000059942 0.000000453104 0.000000014715 0.000000001331 0.000000056875 -0.000000001544 -0.000000007491 0.000000014715 0.000000552905 +1 1645005916.003596782684 153 154 0.105956714038 -0.007930247692 0.017803331311 -0.000325566987 -0.000424651970 0.000784271079 0.000000013892 -0.000000002247 0.000000000116 -0.000000000302 0.000000000924 -0.000000000604 -0.000000002247 0.000000009117 -0.000000000160 -0.000000001400 0.000000000976 0.000000058527 0.000000000116 -0.000000000160 0.000000007137 -0.000000007719 -0.000000045692 -0.000000001110 -0.000000000302 -0.000000001400 -0.000000007719 0.000000198654 0.000000053623 -0.000000006581 0.000000000924 0.000000000976 -0.000000045692 0.000000053623 0.000000487022 0.000000012262 -0.000000000604 0.000000058527 -0.000000001110 -0.000000006581 0.000000012262 0.000000572349 +1 1645005916.099525690079 154 155 0.106714377159 -0.007351031551 0.007507753034 0.000196149893 -0.001095119436 -0.001110160472 0.000000014587 -0.000000002603 0.000000000179 -0.000000000321 0.000000000579 -0.000000002608 -0.000000002603 0.000000009454 -0.000000000165 -0.000000001330 0.000000001027 0.000000061678 0.000000000179 -0.000000000165 0.000000007113 -0.000000007149 -0.000000046636 -0.000000001161 -0.000000000321 -0.000000001330 -0.000000007149 0.000000206337 0.000000051754 -0.000000005643 0.000000000579 0.000000001027 -0.000000046636 0.000000051754 0.000000508473 0.000000013467 -0.000000002608 0.000000061678 -0.000000001161 -0.000000005643 0.000000013467 0.000000604801 +1 1645005916.201919555664 155 156 0.114214238464 -0.005627023360 -0.006587383337 0.000405811911 0.001118760483 -0.003412120900 0.000000016892 -0.000000003571 0.000000000307 -0.000000000279 0.000000000065 -0.000000005502 -0.000000003571 0.000000010098 -0.000000000203 -0.000000001503 0.000000001164 0.000000066149 0.000000000307 -0.000000000203 0.000000007107 -0.000000007131 -0.000000047845 -0.000000001264 -0.000000000279 -0.000000001503 -0.000000007131 0.000000211613 0.000000054896 -0.000000007056 0.000000000065 0.000000001164 -0.000000047845 0.000000054896 0.000000531070 0.000000014713 -0.000000005502 0.000000066149 -0.000000001264 -0.000000007056 0.000000014713 0.000000647179 +1 1645005916.305266141891 156 157 0.111308342148 -0.009772812253 -0.005809218470 -0.000158830582 -0.000215490923 0.000333338897 0.000000024402 -0.000000006436 0.000000000493 -0.000000000261 -0.000000000377 -0.000000011240 -0.000000006436 0.000000011477 -0.000000000292 -0.000000001398 0.000000001461 0.000000072198 0.000000000493 -0.000000000292 0.000000007568 -0.000000007942 -0.000000052886 -0.000000001576 -0.000000000261 -0.000000001398 -0.000000007942 0.000000218297 0.000000065152 -0.000000006844 -0.000000000377 0.000000001461 -0.000000052886 0.000000065152 0.000000590097 0.000000017471 -0.000000011240 0.000000072198 -0.000000001576 -0.000000006844 0.000000017471 0.000000700700 +1 1645005916.402242660522 157 158 0.098861612320 -0.010532723695 0.012346443386 0.000302995827 0.000414997200 0.001602970315 0.000000037109 -0.000000010805 0.000000000821 -0.000000000804 -0.000000001279 -0.000000016111 -0.000000010805 0.000000012886 -0.000000000505 -0.000000000852 0.000000002599 0.000000074435 0.000000000821 -0.000000000505 0.000000008686 -0.000000010526 -0.000000062013 -0.000000002548 -0.000000000804 -0.000000000852 -0.000000010526 0.000000231195 0.000000086125 -0.000000003957 -0.000000001279 0.000000002599 -0.000000062013 0.000000086125 0.000000677017 0.000000025691 -0.000000016111 0.000000074435 -0.000000002548 -0.000000003957 0.000000025691 0.000000727276 +1 1645005916.503258705139 158 159 0.102502168086 -0.009093252456 0.014363853030 -0.000283008408 -0.000358165826 0.006681060732 0.000000051140 -0.000000015225 0.000000001216 -0.000000002070 -0.000000003404 -0.000000014930 -0.000000015225 0.000000013941 -0.000000000656 -0.000000000227 0.000000003539 0.000000072261 0.000000001216 -0.000000000656 0.000000009605 -0.000000014137 -0.000000068917 -0.000000002838 -0.000000002070 -0.000000000227 -0.000000014137 0.000000246310 0.000000111562 -0.000000001761 -0.000000003404 0.000000003539 -0.000000068917 0.000000111562 0.000000735475 0.000000029040 -0.000000014930 0.000000072261 -0.000000002838 -0.000000001761 0.000000029040 0.000000724038 +1 1645005916.603333711624 159 160 0.110584118510 -0.006969165435 0.006755957308 -0.000547151088 -0.002765490415 0.010723492151 0.000000064963 -0.000000019500 0.000000001613 -0.000000003508 -0.000000006554 -0.000000011097 -0.000000019500 0.000000015237 -0.000000000783 0.000000000383 0.000000004607 0.000000071746 0.000000001613 -0.000000000783 0.000000010020 -0.000000017014 -0.000000072512 -0.000000002905 -0.000000003508 0.000000000383 -0.000000017014 0.000000260411 0.000000131565 -0.000000000900 -0.000000006554 0.000000004607 -0.000000072512 0.000000131565 0.000000769488 0.000000030950 -0.000000011097 0.000000071746 -0.000000002905 -0.000000000900 0.000000030950 0.000000738042 +1 1645005916.703304529190 160 161 0.115867178906 -0.001405365939 -0.006941536777 -0.000217074623 0.000672686205 0.003645953857 0.000000078026 -0.000000023457 0.000000002054 -0.000000004985 -0.000000009324 -0.000000008604 -0.000000023457 0.000000016517 -0.000000000950 0.000000000749 0.000000005758 0.000000073233 0.000000002054 -0.000000000950 0.000000010587 -0.000000019544 -0.000000077702 -0.000000003100 -0.000000004985 0.000000000749 -0.000000019544 0.000000277224 0.000000150069 -0.000000002565 -0.000000009324 0.000000005758 -0.000000077702 0.000000150069 0.000000821178 0.000000033645 -0.000000008604 0.000000073233 -0.000000003100 -0.000000002565 0.000000033645 0.000000772113 +1 1645005916.804172515869 161 162 0.117302909972 0.000295419838 -0.016017415108 -0.000106164346 0.000901201232 -0.001277739925 0.000000088382 -0.000000026988 0.000000002179 -0.000000006324 -0.000000011465 -0.000000008722 -0.000000026988 0.000000017842 -0.000000000989 0.000000001031 0.000000006457 0.000000076083 0.000000002179 -0.000000000989 0.000000010901 -0.000000021540 -0.000000081165 -0.000000002920 -0.000000006324 0.000000001031 -0.000000021540 0.000000295157 0.000000164011 -0.000000004981 -0.000000011465 0.000000006457 -0.000000081165 0.000000164011 0.000000859137 0.000000032540 -0.000000008722 0.000000076083 -0.000000002920 -0.000000004981 0.000000032540 0.000000810795 +1 1645005916.907030344009 162 163 0.117113502778 -0.001332837421 -0.000358495722 0.000214441813 0.001777967441 -0.003905204155 0.000000096512 -0.000000029855 0.000000002263 -0.000000007359 -0.000000013389 -0.000000009255 -0.000000029855 0.000000018930 -0.000000001023 0.000000001324 0.000000007208 0.000000077916 0.000000002263 -0.000000001023 0.000000011112 -0.000000023344 -0.000000084006 -0.000000002951 -0.000000007359 0.000000001324 -0.000000023344 0.000000313829 0.000000177054 -0.000000006430 -0.000000013389 0.000000007208 -0.000000084006 0.000000177054 0.000000893655 0.000000033005 -0.000000009255 0.000000077916 -0.000000002951 -0.000000006430 0.000000033005 0.000000834816 +1 1645005917.005794048309 163 164 0.112162385129 -0.003856647165 0.014586076375 -0.000117380248 -0.001608061862 -0.001170630734 0.000000103324 -0.000000032168 0.000000002342 -0.000000008716 -0.000000016076 -0.000000006607 -0.000000032168 0.000000019618 -0.000000000993 0.000000001842 0.000000007717 0.000000077445 0.000000002342 -0.000000000993 0.000000011325 -0.000000025511 -0.000000087156 -0.000000002530 -0.000000008716 0.000000001842 -0.000000025511 0.000000334949 0.000000195314 -0.000000006232 -0.000000016076 0.000000007717 -0.000000087156 0.000000195314 0.000000934604 0.000000029769 -0.000000006607 0.000000077445 -0.000000002530 -0.000000006232 0.000000029769 0.000000849058 +1 1645005917.104857444763 164 165 0.111741027745 -0.003567459006 0.022616849137 0.000236524934 0.000178204487 -0.002832779338 0.000000108247 -0.000000033657 0.000000002538 -0.000000009343 -0.000000017174 -0.000000003909 -0.000000033657 0.000000020124 -0.000000000963 0.000000001696 0.000000007303 0.000000078131 0.000000002538 -0.000000000963 0.000000011442 -0.000000026989 -0.000000089241 -0.000000001718 -0.000000009343 0.000000001696 -0.000000026989 0.000000352166 0.000000208528 -0.000000009427 -0.000000017174 0.000000007303 -0.000000089241 0.000000208528 0.000000963343 0.000000021895 -0.000000003909 0.000000078131 -0.000000001718 -0.000000009427 0.000000021895 0.000000871104 +1 1645005917.207338571548 165 166 0.113903906676 -0.004144900189 0.009725110224 0.000086239947 -0.000462914316 -0.004704134184 0.000000110910 -0.000000034448 0.000000002432 -0.000000009395 -0.000000015619 -0.000000003444 -0.000000034448 0.000000020431 -0.000000000941 0.000000001671 0.000000006954 0.000000079230 0.000000002432 -0.000000000941 0.000000011953 -0.000000028963 -0.000000094423 -0.000000001837 -0.000000009395 0.000000001671 -0.000000028963 0.000000367870 0.000000226360 -0.000000010076 -0.000000015619 0.000000006954 -0.000000094423 0.000000226360 0.000001016065 0.000000022453 -0.000000003444 0.000000079230 -0.000000001837 -0.000000010076 0.000000022453 0.000000886686 +1 1645005917.305225610733 166 167 0.106350650780 -0.008006138847 -0.004136760500 0.000217735543 0.000793264977 -0.005450629162 0.000000112457 -0.000000034975 0.000000002459 -0.000000009900 -0.000000014669 -0.000000004888 -0.000000034975 0.000000020691 -0.000000001068 0.000000002175 0.000000007608 0.000000081541 0.000000002459 -0.000000001068 0.000000012417 -0.000000030753 -0.000000100017 -0.000000002845 -0.000000009900 0.000000002175 -0.000000030753 0.000000385554 0.000000244403 -0.000000007583 -0.000000014669 0.000000007608 -0.000000100017 0.000000244403 0.000001081297 0.000000030238 -0.000000004888 0.000000081541 -0.000000002845 -0.000000007583 0.000000030238 0.000000913045 +1 1645005917.404884338379 167 168 0.098821449951 -0.014936367447 0.003157378141 0.000960141444 0.000553150047 -0.007218868305 0.000000114058 -0.000000035351 0.000000002309 -0.000000010234 -0.000000013984 -0.000000006367 -0.000000035351 0.000000020849 -0.000000000967 0.000000002122 0.000000006908 0.000000083703 0.000000002309 -0.000000000967 0.000000012562 -0.000000031523 -0.000000102262 -0.000000002356 -0.000000010234 0.000000002122 -0.000000031523 0.000000397207 0.000000252081 -0.000000009475 -0.000000013984 0.000000006908 -0.000000102262 0.000000252081 0.000001111583 0.000000025955 -0.000000006367 0.000000083703 -0.000000002356 -0.000000009475 0.000000025955 0.000000936514 +1 1645005917.504653453827 168 169 0.094186437816 -0.015526879719 0.019416391189 0.000802075926 0.000404113705 -0.007307554869 0.000000114991 -0.000000035620 0.000000002454 -0.000000011103 -0.000000015611 -0.000000006733 -0.000000035620 0.000000020791 -0.000000000965 0.000000002157 0.000000007149 0.000000083370 0.000000002454 -0.000000000965 0.000000012360 -0.000000031514 -0.000000100745 -0.000000002049 -0.000000011103 0.000000002157 -0.000000031514 0.000000399690 0.000000251423 -0.000000012007 -0.000000015611 0.000000007149 -0.000000100745 0.000000251423 0.000001101197 0.000000025036 -0.000000006733 0.000000083370 -0.000000002049 -0.000000012007 0.000000025036 0.000000937610 +1 1645005917.605074167252 169 170 0.100592303433 -0.011605821285 0.019967308871 0.000000189307 -0.002053301407 -0.000766444936 0.000000116137 -0.000000036111 0.000000002855 -0.000000013056 -0.000000020038 -0.000000003199 -0.000000036111 0.000000021073 -0.000000001193 0.000000003208 0.000000009429 0.000000083889 0.000000002855 -0.000000001193 0.000000011993 -0.000000030942 -0.000000098311 -0.000000002956 -0.000000013056 0.000000003208 -0.000000030942 0.000000398169 0.000000245893 -0.000000009126 -0.000000020038 0.000000009429 -0.000000098311 0.000000245893 0.000001086821 0.000000034002 -0.000000003199 0.000000083889 -0.000000002956 -0.000000009126 0.000000034002 0.000000959796 +1 1645005917.706811666489 170 171 0.107970526617 -0.008036241272 0.010229935133 -0.000213881979 -0.001054988989 0.000031095351 0.000000118125 -0.000000036312 0.000000003318 -0.000000014955 -0.000000023459 0.000000001488 -0.000000036312 0.000000021160 -0.000000001345 0.000000003695 0.000000010662 0.000000084616 0.000000003318 -0.000000001345 0.000000011958 -0.000000030573 -0.000000098713 -0.000000003110 -0.000000014955 0.000000003695 -0.000000030573 0.000000397435 0.000000242189 -0.000000009785 -0.000000023459 0.000000010662 -0.000000098713 0.000000242189 0.000001096053 0.000000036591 0.000000001488 0.000000084616 -0.000000003110 -0.000000009785 0.000000036591 0.000000988045 +1 1645005917.804672718048 171 172 0.105731298009 -0.005919607258 -0.005833779357 -0.000045148203 0.001018874621 -0.000751755422 0.000000121189 -0.000000036830 0.000000003468 -0.000000015684 -0.000000023681 0.000000004698 -0.000000036830 0.000000021195 -0.000000001433 0.000000003788 0.000000011194 0.000000084668 0.000000003468 -0.000000001433 0.000000012248 -0.000000031571 -0.000000101807 -0.000000003557 -0.000000015684 0.000000003788 -0.000000031571 0.000000404763 0.000000251181 -0.000000010529 -0.000000023681 0.000000011194 -0.000000101807 0.000000251181 0.000001129929 0.000000041603 0.000000004698 0.000000084668 -0.000000003557 -0.000000010529 0.000000041603 0.000001008043 +1 1645005917.904610872269 172 173 0.103855440730 -0.008847785631 -0.010570833324 0.000073912894 0.000524173989 -0.000244240168 0.000000124632 -0.000000037627 0.000000003556 -0.000000016897 -0.000000025958 0.000000007236 -0.000000037627 0.000000021390 -0.000000001541 0.000000004351 0.000000012531 0.000000085042 0.000000003556 -0.000000001541 0.000000012424 -0.000000032522 -0.000000104071 -0.000000004167 -0.000000016897 0.000000004351 -0.000000032522 0.000000412569 0.000000261172 -0.000000009421 -0.000000025958 0.000000012531 -0.000000104071 0.000000261172 0.000001156601 0.000000046360 0.000000007236 0.000000085042 -0.000000004167 -0.000000009421 0.000000046360 0.000001026787 +1 1645005918.005419969559 173 174 0.100505315538 -0.010103880265 0.006164128240 0.000413598049 0.001946711804 -0.004386893254 0.000000127051 -0.000000038318 0.000000003856 -0.000000018597 -0.000000029537 0.000000006021 -0.000000038318 0.000000021496 -0.000000001582 0.000000004785 0.000000013137 0.000000085273 0.000000003856 -0.000000001582 0.000000012143 -0.000000032181 -0.000000103060 -0.000000003657 -0.000000018597 0.000000004785 -0.000000032181 0.000000415569 0.000000261713 -0.000000011012 -0.000000029537 0.000000013137 -0.000000103060 0.000000261713 0.000001158654 0.000000041063 0.000000006021 0.000000085273 -0.000000003657 -0.000000011012 0.000000041063 0.000001031052 +1 1645005918.106912612915 174 175 0.104243823486 -0.015398520529 0.017609900953 0.000251278556 -0.001321653647 -0.002699090350 0.000000129650 -0.000000039225 0.000000003859 -0.000000019970 -0.000000031925 0.000000007468 -0.000000039225 0.000000021632 -0.000000001548 0.000000005349 0.000000013611 0.000000083970 0.000000003859 -0.000000001548 0.000000012009 -0.000000032668 -0.000000103462 -0.000000003323 -0.000000019970 0.000000005349 -0.000000032668 0.000000423439 0.000000269388 -0.000000010299 -0.000000031925 0.000000013611 -0.000000103462 0.000000269388 0.000001178424 0.000000038174 0.000000007468 0.000000083970 -0.000000003323 -0.000000010299 0.000000038174 0.000001031326 +1 1645005918.205326795578 175 176 0.105018001061 -0.016620276316 0.016583575074 0.000471605107 -0.001302930249 -0.006284028454 0.000000132952 -0.000000039810 0.000000004127 -0.000000022074 -0.000000034193 0.000000010673 -0.000000039810 0.000000021502 -0.000000001615 0.000000005775 0.000000014293 0.000000082301 0.000000004127 -0.000000001615 0.000000012306 -0.000000034229 -0.000000107844 -0.000000003231 -0.000000022074 0.000000005775 -0.000000034229 0.000000435942 0.000000288374 -0.000000012012 -0.000000034193 0.000000014293 -0.000000107844 0.000000288374 0.000001238900 0.000000038299 0.000000010673 0.000000082301 -0.000000003231 -0.000000012012 0.000000038299 0.000001036436 +1 1645005918.304870605469 176 177 0.104912611278 -0.018585854244 0.001515240424 0.000485324157 0.001946316449 -0.010479077493 0.000000136347 -0.000000040849 0.000000004059 -0.000000022351 -0.000000030809 0.000000009278 -0.000000040849 0.000000021957 -0.000000001504 0.000000004860 0.000000012760 0.000000085030 0.000000004059 -0.000000001504 0.000000012626 -0.000000035744 -0.000000112385 -0.000000002494 -0.000000022351 0.000000004860 -0.000000035744 0.000000448023 0.000000308545 -0.000000021449 -0.000000030809 0.000000012760 -0.000000112385 0.000000308545 0.000001300935 0.000000034558 0.000000009278 0.000000085030 -0.000000002494 -0.000000021449 0.000000034558 0.000001068746 +1 1645005918.405547380447 177 178 0.104792166995 -0.023226516703 -0.007056980576 0.000267709258 0.000474336908 -0.007207889728 0.000000138617 -0.000000041762 0.000000003857 -0.000000022163 -0.000000029275 0.000000010678 -0.000000041762 0.000000022377 -0.000000001391 0.000000004532 0.000000011951 0.000000086241 0.000000003857 -0.000000001391 0.000000012725 -0.000000037131 -0.000000114801 -0.000000001984 -0.000000022163 0.000000004532 -0.000000037131 0.000000457244 0.000000327019 -0.000000025297 -0.000000029275 0.000000011951 -0.000000114801 0.000000327019 0.000001339780 0.000000031499 0.000000010678 0.000000086241 -0.000000001984 -0.000000025297 0.000000031499 0.000001092396 +1 1645005918.505361080170 178 179 0.102238431531 -0.023908947381 0.006589654277 0.000655843708 0.000453140817 -0.001821223501 0.000000140246 -0.000000042197 0.000000004132 -0.000000023003 -0.000000031421 0.000000015985 -0.000000042197 0.000000022353 -0.000000001641 0.000000005629 0.000000014034 0.000000084232 0.000000004132 -0.000000001641 0.000000012973 -0.000000038726 -0.000000118299 -0.000000003433 -0.000000023003 0.000000005629 -0.000000038726 0.000000462890 0.000000346326 -0.000000018649 -0.000000031421 0.000000014034 -0.000000118299 0.000000346326 0.000001385059 0.000000044386 0.000000015985 0.000000084232 -0.000000003433 -0.000000018649 0.000000044386 0.000001097883 +1 1645005918.605193614960 179 180 0.107683008826 -0.022764995092 0.017514407486 -0.000368667307 -0.001653800364 0.007067164909 0.000000141481 -0.000000042435 0.000000004360 -0.000000024495 -0.000000034469 0.000000024183 -0.000000042435 0.000000022389 -0.000000001837 0.000000006705 0.000000016054 0.000000082514 0.000000004360 -0.000000001837 0.000000013111 -0.000000040013 -0.000000120651 -0.000000004389 -0.000000024495 0.000000006705 -0.000000040013 0.000000466496 0.000000363613 -0.000000014517 -0.000000034469 0.000000016054 -0.000000120651 0.000000363613 0.000001419210 0.000000053423 0.000000024183 0.000000082514 -0.000000004389 -0.000000014517 0.000000053423 0.000001113406 +1 1645005918.704743146896 180 181 0.111753784251 -0.017339671469 0.014827032018 -0.000550269913 -0.000679567626 0.009481384874 0.000000142307 -0.000000041952 0.000000004661 -0.000000025741 -0.000000036641 0.000000033488 -0.000000041952 0.000000022051 -0.000000001803 0.000000006655 0.000000015521 0.000000081558 0.000000004661 -0.000000001803 0.000000012906 -0.000000039951 -0.000000119907 -0.000000003114 -0.000000025741 0.000000006655 -0.000000039951 0.000000465724 0.000000367203 -0.000000019105 -0.000000036641 0.000000015521 -0.000000119907 0.000000367203 0.000001423450 0.000000042472 0.000000033488 0.000000081558 -0.000000003114 -0.000000019105 0.000000042472 0.000001147641 +1 1645005918.806400060654 181 182 0.113872331121 -0.014001660104 -0.000550563706 -0.000917522817 -0.000618426666 0.010240698071 0.000000143709 -0.000000041830 0.000000004468 -0.000000025299 -0.000000034644 0.000000039964 -0.000000041830 0.000000022150 -0.000000001671 0.000000005770 0.000000014366 0.000000084352 0.000000004468 -0.000000001671 0.000000012897 -0.000000040281 -0.000000121040 -0.000000002385 -0.000000025299 0.000000005770 -0.000000040281 0.000000469448 0.000000373553 -0.000000026765 -0.000000034644 0.000000014366 -0.000000121040 0.000000373553 0.000001447927 0.000000037412 0.000000039964 0.000000084352 -0.000000002385 -0.000000026765 0.000000037412 0.000001214810 +1 1645005918.905241250992 182 183 0.108627949209 -0.010341888855 -0.014273654629 -0.000517403403 0.001641954984 0.006780294180 0.000000145842 -0.000000041995 0.000000004312 -0.000000023934 -0.000000031977 0.000000043324 -0.000000041995 0.000000022211 -0.000000001575 0.000000004796 0.000000013177 0.000000086375 0.000000004312 -0.000000001575 0.000000013350 -0.000000041889 -0.000000127167 -0.000000001918 -0.000000023934 0.000000004796 -0.000000041889 0.000000478700 0.000000395388 -0.000000032141 -0.000000031977 0.000000013177 -0.000000127167 0.000000395388 0.000001528684 0.000000033922 0.000000043324 0.000000086375 -0.000000001918 -0.000000032141 0.000000033922 0.000001262581 +1 1645005919.006230831146 183 184 0.111090918097 -0.011375888755 -0.011195486808 -0.000492323663 -0.000760811117 0.007360552053 0.000000147026 -0.000000042366 0.000000004035 -0.000000022992 -0.000000032173 0.000000045126 -0.000000042366 0.000000022179 -0.000000001481 0.000000004851 0.000000013121 0.000000085324 0.000000004035 -0.000000001481 0.000000013612 -0.000000043406 -0.000000131031 -0.000000001914 -0.000000022992 0.000000004851 -0.000000043406 0.000000485597 0.000000416394 -0.000000028771 -0.000000032173 0.000000013121 -0.000000131031 0.000000416394 0.000001580709 0.000000032816 0.000000045126 0.000000085324 -0.000000001914 -0.000000028771 0.000000032816 0.000001269063 +1 1645005919.103321552277 184 185 0.106064541975 -0.013212314792 0.007933789346 -0.000093460789 -0.000240302902 0.002770392774 0.000000147409 -0.000000042125 0.000000004336 -0.000000023061 -0.000000034649 0.000000046124 -0.000000042125 0.000000021985 -0.000000001589 0.000000004937 0.000000014153 0.000000085861 0.000000004336 -0.000000001589 0.000000013574 -0.000000043836 -0.000000132647 -0.000000002199 -0.000000023061 0.000000004937 -0.000000043836 0.000000489170 0.000000427692 -0.000000028221 -0.000000034649 0.000000014153 -0.000000132647 0.000000427692 0.000001615546 0.000000036449 0.000000046124 0.000000085861 -0.000000002199 -0.000000028221 0.000000036449 0.000001285384 +1 1645005919.204598665237 185 186 0.109583227748 -0.018751742246 0.015930061005 -0.000207377330 -0.000969420842 -0.001611917201 0.000000147737 -0.000000042240 0.000000004321 -0.000000022740 -0.000000033885 0.000000043695 -0.000000042240 0.000000022018 -0.000000001592 0.000000004837 0.000000014065 0.000000087852 0.000000004321 -0.000000001592 0.000000013380 -0.000000043618 -0.000000132567 -0.000000002397 -0.000000022740 0.000000004837 -0.000000043618 0.000000492109 0.000000430403 -0.000000028113 -0.000000033885 0.000000014065 -0.000000132567 0.000000430403 0.000001632040 0.000000039106 0.000000043695 0.000000087852 -0.000000002397 -0.000000028113 0.000000039106 0.000001310151 +1 1645005919.304369688034 186 187 0.105935726310 -0.021962018124 0.012691866608 0.000577732489 0.000249586532 -0.006153890357 0.000000149020 -0.000000042831 0.000000003937 -0.000000021091 -0.000000027669 0.000000039205 -0.000000042831 0.000000022289 -0.000000001497 0.000000004450 0.000000012366 0.000000090284 0.000000003937 -0.000000001497 0.000000013677 -0.000000045058 -0.000000137345 -0.000000002732 -0.000000021091 0.000000004450 -0.000000045058 0.000000500943 0.000000450114 -0.000000025733 -0.000000027669 0.000000012366 -0.000000137345 0.000000450114 0.000001702000 0.000000041449 0.000000039205 0.000000090284 -0.000000002732 -0.000000025733 0.000000041449 0.000001329947 +1 1645005919.404906749725 187 188 0.107178844297 -0.022750150514 -0.003656608090 0.000369946408 -0.000252619318 -0.007040696870 0.000000150210 -0.000000043366 0.000000003672 -0.000000019261 -0.000000023707 0.000000038513 -0.000000043366 0.000000022455 -0.000000001373 0.000000003685 0.000000010707 0.000000090631 0.000000003672 -0.000000001373 0.000000014393 -0.000000048027 -0.000000146585 -0.000000002253 -0.000000019261 0.000000003685 -0.000000048027 0.000000514919 0.000000488132 -0.000000028143 -0.000000023707 0.000000010707 -0.000000146585 0.000000488132 0.000001822771 0.000000035984 0.000000038513 0.000000090631 -0.000000002253 -0.000000028143 0.000000035984 0.000001341511 +1 1645005919.509163618088 188 189 0.111763033254 -0.025831837093 -0.007938767615 0.000112630539 0.001169710732 -0.004771712646 0.000000150729 -0.000000043454 0.000000003900 -0.000000018668 -0.000000025291 0.000000043404 -0.000000043454 0.000000022348 -0.000000001378 0.000000003081 0.000000010657 0.000000089362 0.000000003900 -0.000000001378 0.000000014579 -0.000000049376 -0.000000149721 -0.000000001575 -0.000000018668 0.000000003081 -0.000000049376 0.000000521537 0.000000508382 -0.000000033045 -0.000000025291 0.000000010657 -0.000000149721 0.000000508382 0.000001873121 0.000000031279 0.000000043404 0.000000089362 -0.000000001575 -0.000000033045 0.000000031279 0.000001360429 +1 1645005919.606138467789 189 190 0.103050190411 -0.028243153856 0.009036138321 0.000299620151 -0.000460363273 -0.002126451387 0.000000151210 -0.000000043262 0.000000004079 -0.000000019984 -0.000000029692 0.000000051331 -0.000000043262 0.000000022105 -0.000000001496 0.000000003836 0.000000012813 0.000000087307 0.000000004079 -0.000000001496 0.000000014344 -0.000000049082 -0.000000148242 -0.000000002279 -0.000000019984 0.000000003836 -0.000000049082 0.000000523071 0.000000508358 -0.000000029835 -0.000000029692 0.000000012813 -0.000000148242 0.000000508358 0.000001871353 0.000000041535 0.000000051331 0.000000087307 -0.000000002279 -0.000000029835 0.000000041535 0.000001379713 +1 1645005919.708493471146 190 191 0.110366530571 -0.029472021130 0.018614876772 0.000102110420 0.000167129598 0.001994724913 0.000000150740 -0.000000042621 0.000000004151 -0.000000020036 -0.000000029606 0.000000062230 -0.000000042621 0.000000021889 -0.000000001558 0.000000003923 0.000000013386 0.000000086272 0.000000004151 -0.000000001558 0.000000014122 -0.000000049518 -0.000000146966 -0.000000002704 -0.000000020036 0.000000003923 -0.000000049518 0.000000525905 0.000000515666 -0.000000029326 -0.000000029606 0.000000013386 -0.000000146966 0.000000515666 0.000001869683 0.000000049449 0.000000062230 0.000000086272 -0.000000002704 -0.000000029326 0.000000049449 0.000001414664 +1 1645005919.793772935867 191 192 0.096165140501 -0.018685263170 0.011579795113 -0.000740821107 -0.001031369105 0.004911205760 0.000000150428 -0.000000041858 0.000000003727 -0.000000018419 -0.000000025185 0.000000073735 -0.000000041858 0.000000021544 -0.000000001374 0.000000003222 0.000000011562 0.000000084970 0.000000003727 -0.000000001374 0.000000014202 -0.000000051029 -0.000000149309 -0.000000002193 -0.000000018419 0.000000003222 -0.000000051029 0.000000531889 0.000000537344 -0.000000031680 -0.000000025185 0.000000011562 -0.000000149309 0.000000537344 0.000001910836 0.000000046007 0.000000073735 0.000000084970 -0.000000002193 -0.000000031680 0.000000046007 0.000001453265 +1 1645005919.894334554672 192 193 0.117461686956 -0.016398117007 -0.001090784968 -0.000555133742 -0.000926537311 0.007563804792 0.000000148505 -0.000000040661 0.000000003635 -0.000000017091 -0.000000022791 0.000000084172 -0.000000040661 0.000000020845 -0.000000001307 0.000000002552 0.000000010455 0.000000081570 0.000000003635 -0.000000001307 0.000000014106 -0.000000051158 -0.000000149559 -0.000000001627 -0.000000017091 0.000000002552 -0.000000051158 0.000000525853 0.000000542509 -0.000000035899 -0.000000022791 0.000000010455 -0.000000149559 0.000000542509 0.000001922636 0.000000041308 0.000000084172 0.000000081570 -0.000000001627 -0.000000035899 0.000000041308 0.000001463420 +1 1645005920.004122257233 193 194 0.127889889395 -0.014287189664 -0.018071779409 -0.000603843631 0.001403222454 0.004675372250 0.000000147336 -0.000000039680 0.000000003583 -0.000000015186 -0.000000019399 0.000000089698 -0.000000039680 0.000000020410 -0.000000001303 0.000000001664 0.000000009811 0.000000081704 0.000000003583 -0.000000001303 0.000000014293 -0.000000052484 -0.000000153094 -0.000000001827 -0.000000015186 0.000000001664 -0.000000052484 0.000000530498 0.000000561884 -0.000000039870 -0.000000019399 0.000000009811 -0.000000153094 0.000000561884 0.000001973145 0.000000046528 0.000000089698 0.000000081704 -0.000000001827 -0.000000039870 0.000000046528 0.000001493220 +1 1645005920.104864120483 194 195 0.116168811852 -0.013109825790 -0.012929698648 -0.000005389645 0.000738108287 0.001144078207 0.000000149213 -0.000000040106 0.000000003646 -0.000000015092 -0.000000021339 0.000000090319 -0.000000040106 0.000000020455 -0.000000001340 0.000000001571 0.000000010648 0.000000081937 0.000000003646 -0.000000001340 0.000000014645 -0.000000054570 -0.000000158509 -0.000000002004 -0.000000015092 0.000000001571 -0.000000054570 0.000000553075 0.000000590499 -0.000000041701 -0.000000021339 0.000000010648 -0.000000158509 0.000000590499 0.000002057287 0.000000048248 0.000000090319 0.000000081937 -0.000000002004 -0.000000041701 0.000000048248 0.000001514765 +1 1645005920.206741094589 195 196 0.116606271407 -0.013867701039 0.005805133595 0.000067675702 0.000577447854 -0.002394489670 0.000000149429 -0.000000040285 0.000000003646 -0.000000014456 -0.000000021481 0.000000087876 -0.000000040285 0.000000020545 -0.000000001350 0.000000001645 0.000000010775 0.000000083356 0.000000003646 -0.000000001350 0.000000014509 -0.000000053933 -0.000000159305 -0.000000002159 -0.000000014456 0.000000001645 -0.000000053933 0.000000554499 0.000000591697 -0.000000038609 -0.000000021481 0.000000010775 -0.000000159305 0.000000591697 0.000002096327 0.000000049808 0.000000087876 0.000000083356 -0.000000002159 -0.000000038609 0.000000049808 0.000001532263 +1 1645005920.306432723999 196 197 0.118459510620 -0.015741762731 0.019941986760 -0.000233586310 -0.001780831484 -0.005334381966 0.000000148573 -0.000000040237 0.000000003215 -0.000000013105 -0.000000017644 0.000000084415 -0.000000040237 0.000000020546 -0.000000001290 0.000000001926 0.000000010386 0.000000084228 0.000000003215 -0.000000001290 0.000000014559 -0.000000054075 -0.000000162036 -0.000000003226 -0.000000013105 0.000000001926 -0.000000054075 0.000000552256 0.000000602098 -0.000000029736 -0.000000017644 0.000000010386 -0.000000162036 0.000000602098 0.000002151812 0.000000061306 0.000000084415 0.000000084228 -0.000000003226 -0.000000029736 0.000000061306 0.000001531623 +1 1645005920.396054744720 197 198 0.108833768936 -0.014196265349 0.014957699203 0.000784882132 -0.000440774433 -0.007383134271 0.000000148605 -0.000000040325 0.000000003132 -0.000000012207 -0.000000013711 0.000000083131 -0.000000040325 0.000000020417 -0.000000001368 0.000000002111 0.000000010502 0.000000083174 0.000000003132 -0.000000001368 0.000000015049 -0.000000056948 -0.000000169586 -0.000000004564 -0.000000012207 0.000000002111 -0.000000056948 0.000000566345 0.000000643775 -0.000000023618 -0.000000013711 0.000000010502 -0.000000169586 0.000000643775 0.000002263566 0.000000078674 0.000000083131 0.000000083174 -0.000000004564 -0.000000023618 0.000000078674 0.000001518540 +1 1645005920.505497217178 198 199 0.132822748645 -0.016271936175 -0.000340704148 0.000018558141 0.001135204195 -0.009919704392 0.000000147651 -0.000000040083 0.000000002848 -0.000000010888 -0.000000007401 0.000000081858 -0.000000040083 0.000000020309 -0.000000001301 0.000000001666 0.000000008869 0.000000083786 0.000000002848 -0.000000001301 0.000000015367 -0.000000058874 -0.000000175370 -0.000000004773 -0.000000010888 0.000000001666 -0.000000058874 0.000000570999 0.000000675795 -0.000000023893 -0.000000007401 0.000000008869 -0.000000175370 0.000000675795 0.000002351570 0.000000082187 0.000000081858 0.000000083786 -0.000000004773 -0.000000023893 0.000000082187 0.000001524068 \ No newline at end of file diff --git a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp index cd2ba593b..f025c4470 100644 --- a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp @@ -16,29 +16,37 @@ * @date May 23, 2012 */ - -#include -#include +#include #include -#include -#include -#include -#include +#include #include #include #include -#include +#include +#include +#include +#include +#include +#include +#include +#include // For writeG2o +#include #include +#include +#include + using namespace std; using namespace gtsam; - -Key MakeKey(size_t index) { return Symbol('x', index); } +using symbol_shorthand::X; +using BetweenPoint2 = BetweenFactor; /* ************************************************************************* */ -bool check_smoother(const NonlinearFactorGraph& fullgraph, const Values& fullinit, const IncrementalFixedLagSmoother& smoother, const Key& key) { - +bool check_smoother(const NonlinearFactorGraph& fullgraph, + const Values& fullinit, + const IncrementalFixedLagSmoother& smoother, + const Key& key) { GaussianFactorGraph linearized = *fullgraph.linearize(fullinit); VectorValues delta = linearized.optimize(); Values fullfinal = fullinit.retract(delta); @@ -50,51 +58,46 @@ bool check_smoother(const NonlinearFactorGraph& fullgraph, const Values& fullini } /* ************************************************************************* */ -void PrintSymbolicTreeHelper( - const ISAM2Clique::shared_ptr& clique, const std::string indent = "") { - +void PrintSymbolicTreeHelper(const ISAM2Clique::shared_ptr& clique, + const std::string indent = "") { // Print the current clique std::cout << indent << "P( "; - for(Key key: clique->conditional()->frontals()) { + for (Key key : clique->conditional()->frontals()) { std::cout << DefaultKeyFormatter(key) << " "; } - if (clique->conditional()->nrParents() > 0) - std::cout << "| "; - for(Key key: clique->conditional()->parents()) { + if (clique->conditional()->nrParents() > 0) std::cout << "| "; + for (Key key : clique->conditional()->parents()) { std::cout << DefaultKeyFormatter(key) << " "; } std::cout << ")" << std::endl; // Recursively print all of the children - for(const ISAM2Clique::shared_ptr& child: clique->children) { + for (const ISAM2Clique::shared_ptr& child : clique->children) { PrintSymbolicTreeHelper(child, indent + " "); } } /* ************************************************************************* */ -void PrintSymbolicTree(const ISAM2& isam, - const std::string& label) { +void PrintSymbolicTree(const ISAM2& isam, const std::string& label) { std::cout << label << std::endl; if (!isam.roots().empty()) { - for(const ISAM2::sharedClique& root: isam.roots()) { + for (const ISAM2::sharedClique& root : isam.roots()) { PrintSymbolicTreeHelper(root); } } else std::cout << "{Empty Tree}" << std::endl; } - /* ************************************************************************* */ -TEST( IncrementalFixedLagSmoother, Example ) -{ - // Test the IncrementalFixedLagSmoother in a pure linear environment. Thus, full optimization and - // the IncrementalFixedLagSmoother should be identical (even with the linearized approximations at - // the end of the smoothing lag) +TEST(IncrementalFixedLagSmoother, Example) { + // Test the IncrementalFixedLagSmoother in a pure linear environment. Thus, + // full optimization and the IncrementalFixedLagSmoother should be identical + // (even with the linearized approximations at the end of the smoothing lag) SETDEBUG("IncrementalFixedLagSmoother update", true); // Set up parameters - SharedDiagonal odometerNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1)); + SharedDiagonal odoNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1)); SharedDiagonal loopNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1)); // Create a Fixed-Lag Smoother @@ -110,13 +113,13 @@ TEST( IncrementalFixedLagSmoother, Example ) // Add a prior at time 0 and update the HMF { - Key key0 = MakeKey(0); + Key key0 = X(0); NonlinearFactorGraph newFactors; Values newValues; Timestamps newTimestamps; - newFactors.addPrior(key0, Point2(0.0, 0.0), odometerNoise); + newFactors.addPrior(key0, Point2(0.0, 0.0), odoNoise); newValues.insert(key0, Point2(0.01, 0.01)); newTimestamps[key0] = 0.0; @@ -133,16 +136,17 @@ TEST( IncrementalFixedLagSmoother, Example ) } // Add odometry from time 0 to time 5 - while(i <= 5) { - Key key1 = MakeKey(i-1); - Key key2 = MakeKey(i); + while (i <= 5) { + Key key1 = X(i - 1); + Key key2 = X(i); NonlinearFactorGraph newFactors; Values newValues; Timestamps newTimestamps; - newFactors.push_back(BetweenFactor(key1, key2, Point2(1.0, 0.0), odometerNoise)); - newValues.insert(key2, Point2(double(i)+0.1, -0.1)); + newFactors.emplace_shared(key1, key2, Point2(1.0, 0.0), + odoNoise); + newValues.insert(key2, Point2(double(i) + 0.1, -0.1)); newTimestamps[key2] = double(i); fullgraph.push_back(newFactors); @@ -157,19 +161,22 @@ TEST( IncrementalFixedLagSmoother, Example ) ++i; } - // Add odometry from time 5 to 6 to the HMF and a loop closure at time 5 to the TSM + // Add odometry from time 5 to 6 to the HMF and a loop closure at time 5 to + // the TSM { // Add the odometry factor to the HMF - Key key1 = MakeKey(i-1); - Key key2 = MakeKey(i); + Key key1 = X(i - 1); + Key key2 = X(i); NonlinearFactorGraph newFactors; Values newValues; Timestamps newTimestamps; - newFactors.push_back(BetweenFactor(key1, key2, Point2(1.0, 0.0), odometerNoise)); - newFactors.push_back(BetweenFactor(MakeKey(2), MakeKey(5), Point2(3.5, 0.0), loopNoise)); - newValues.insert(key2, Point2(double(i)+0.1, -0.1)); + newFactors.emplace_shared(key1, key2, Point2(1.0, 0.0), + odoNoise); + newFactors.emplace_shared(X(2), X(5), Point2(3.5, 0.0), + loopNoise); + newValues.insert(key2, Point2(double(i) + 0.1, -0.1)); newTimestamps[key2] = double(i); fullgraph.push_back(newFactors); @@ -185,19 +192,21 @@ TEST( IncrementalFixedLagSmoother, Example ) } // Add odometry from time 6 to time 15 - while(i <= 15) { - Key key1 = MakeKey(i-1); - Key key2 = MakeKey(i); + while (i <= 15) { + Key key1 = X(i - 1); + Key key2 = X(i); NonlinearFactorGraph newFactors; Values newValues; Timestamps newTimestamps; - // Add the odometry factor twice to ensure the removeFactor test below works, - // where we need to keep the connectivity of the graph. - newFactors.push_back(BetweenFactor(key1, key2, Point2(1.0, 0.0), odometerNoise)); - newFactors.push_back(BetweenFactor(key1, key2, Point2(1.0, 0.0), odometerNoise)); - newValues.insert(key2, Point2(double(i)+0.1, -0.1)); + // Add the odometry factor twice to ensure the removeFactor test below + // works, where we need to keep the connectivity of the graph. + newFactors.emplace_shared(key1, key2, Point2(1.0, 0.0), + odoNoise); + newFactors.emplace_shared(key1, key2, Point2(1.0, 0.0), + odoNoise); + newValues.insert(key2, Point2(double(i) + 0.1, -0.1)); newTimestamps[key2] = double(i); fullgraph.push_back(newFactors); @@ -214,112 +223,120 @@ TEST( IncrementalFixedLagSmoother, Example ) // add/remove an extra factor { - Key key1 = MakeKey(i-1); - Key key2 = MakeKey(i); + Key key1 = X(i - 1); + Key key2 = X(i); - NonlinearFactorGraph newFactors; - Values newValues; - Timestamps newTimestamps; + NonlinearFactorGraph newFactors; + Values newValues; + Timestamps newTimestamps; - // add 2 odometry factors - newFactors.push_back(BetweenFactor(key1, key2, Point2(1.0, 0.0), odometerNoise)); - newFactors.push_back(BetweenFactor(key1, key2, Point2(1.0, 0.0), odometerNoise)); - newValues.insert(key2, Point2(double(i)+0.1, -0.1)); - newTimestamps[key2] = double(i); - ++i; + // add 2 odometry factors + newFactors.emplace_shared(key1, key2, Point2(1.0, 0.0), + odoNoise); + newFactors.emplace_shared(key1, key2, Point2(1.0, 0.0), + odoNoise); + newValues.insert(key2, Point2(double(i) + 0.1, -0.1)); + newTimestamps[key2] = double(i); + ++i; - fullgraph.push_back(newFactors); - fullinit.insert(newValues); + fullgraph.push_back(newFactors); + fullinit.insert(newValues); - // Update the smoother - smoother.update(newFactors, newValues, newTimestamps); + // Update the smoother + smoother.update(newFactors, newValues, newTimestamps); - // Check - CHECK(check_smoother(fullgraph, fullinit, smoother, key2)); + // Check + CHECK(check_smoother(fullgraph, fullinit, smoother, key2)); - // now remove one of the two and try again - // empty values and new factors for fake update in which we only remove factors - NonlinearFactorGraph emptyNewFactors; - Values emptyNewValues; - Timestamps emptyNewTimestamps; + // now remove one of the two and try again + // empty values and new factors for fake update in which we only remove + // factors + NonlinearFactorGraph emptyNewFactors; + Values emptyNewValues; + Timestamps emptyNewTimestamps; - size_t factorIndex = 25; // any index that does not break connectivity of the graph - FactorIndices factorToRemove; - factorToRemove.push_back(factorIndex); + size_t factorIndex = + 25; // any index that does not break connectivity of the graph + FactorIndices factorToRemove; + factorToRemove.push_back(factorIndex); - const NonlinearFactorGraph smootherFactorsBeforeRemove = smoother.getFactors(); + const NonlinearFactorGraph smootherFactorsBeforeRemove = + smoother.getFactors(); std::cout << "fullgraph.size() = " << fullgraph.size() << std::endl; std::cout << "smootherFactorsBeforeRemove.size() = " << smootherFactorsBeforeRemove.size() << std::endl; - // remove factor - smoother.update(emptyNewFactors, emptyNewValues, emptyNewTimestamps,factorToRemove); + // remove factor + smoother.update(emptyNewFactors, emptyNewValues, emptyNewTimestamps, + factorToRemove); - // Note: the following test (checking that the number of factor is reduced by 1) - // fails since we are not reusing slots, hence also when removing a factor we do not change - // the size of the factor graph - // size_t nrFactorsAfterRemoval = smoother.getFactors().size(); - // DOUBLES_EQUAL(nrFactorsBeforeRemoval-1, nrFactorsAfterRemoval, 1e-5); + // Note: the following test (checking that the number of factor is reduced + // by 1) fails since we are not reusing slots, hence also when removing a + // factor we do not change the size of the factor graph size_t + // nrFactorsAfterRemoval = smoother.getFactors().size(); + // DOUBLES_EQUAL(nrFactorsBeforeRemoval-1, nrFactorsAfterRemoval, 1e-5); - // check that the factors in the smoother are right - NonlinearFactorGraph actual = smoother.getFactors(); - for(size_t i=0; i< smootherFactorsBeforeRemove.size(); i++){ - // check that the factors that were not removed are there - if(smootherFactorsBeforeRemove[i] && i != factorIndex){ - EXPECT(smootherFactorsBeforeRemove[i]->equals(*actual[i])); - } - else{ // while the factors that were not there or were removed are no longer there - EXPECT(!actual[i]); - } - } + // check that the factors in the smoother are right + NonlinearFactorGraph actual = smoother.getFactors(); + for (size_t i = 0; i < smootherFactorsBeforeRemove.size(); i++) { + // check that the factors that were not removed are there + if (smootherFactorsBeforeRemove[i] && i != factorIndex) { + EXPECT(smootherFactorsBeforeRemove[i]->equals(*actual[i])); + } else { // while the factors that were not there or were removed are no + // longer there + EXPECT(!actual[i]); + } + } } { SETDEBUG("BayesTreeMarginalizationHelper", true); - PrintSymbolicTree(smoother.getISAM2(), "Bayes Tree Before marginalization test:"); + PrintSymbolicTree(smoother.getISAM2(), + "Bayes Tree Before marginalization test:"); // Do pressure test on marginalization. Enlarge max_i to enhance the test. const int max_i = 500; - while(i <= max_i) { - Key key_0 = MakeKey(i); - Key key_1 = MakeKey(i-1); - Key key_2 = MakeKey(i-2); - Key key_3 = MakeKey(i-3); - Key key_4 = MakeKey(i-4); - Key key_5 = MakeKey(i-5); - Key key_6 = MakeKey(i-6); - Key key_7 = MakeKey(i-7); - Key key_8 = MakeKey(i-8); - Key key_9 = MakeKey(i-9); - Key key_10 = MakeKey(i-10); + while (i <= max_i) { + Key key_0 = X(i); + Key key_1 = X(i - 1); + Key key_2 = X(i - 2); + Key key_3 = X(i - 3); + Key key_4 = X(i - 4); + Key key_5 = X(i - 5); + Key key_6 = X(i - 6); + Key key_7 = X(i - 7); + Key key_8 = X(i - 8); + Key key_9 = X(i - 9); + Key key_10 = X(i - 10); NonlinearFactorGraph newFactors; Values newValues; Timestamps newTimestamps; // To make a complex graph - newFactors.push_back(BetweenFactor(key_1, key_0, Point2(1.0, 0.0), odometerNoise)); + const Point2 z(1.0, 0.0); + newFactors.emplace_shared(key_1, key_0, z, odoNoise); if (i % 2 == 0) - newFactors.push_back(BetweenFactor(key_2, key_1, Point2(1.0, 0.0), odometerNoise)); + newFactors.emplace_shared(key_2, key_1, z, odoNoise); if (i % 3 == 0) - newFactors.push_back(BetweenFactor(key_3, key_2, Point2(1.0, 0.0), odometerNoise)); + newFactors.emplace_shared(key_3, key_2, z, odoNoise); if (i % 4 == 0) - newFactors.push_back(BetweenFactor(key_4, key_3, Point2(1.0, 0.0), odometerNoise)); + newFactors.emplace_shared(key_4, key_3, z, odoNoise); if (i % 5 == 0) - newFactors.push_back(BetweenFactor(key_5, key_4, Point2(1.0, 0.0), odometerNoise)); + newFactors.emplace_shared(key_5, key_4, z, odoNoise); if (i % 6 == 0) - newFactors.push_back(BetweenFactor(key_6, key_5, Point2(1.0, 0.0), odometerNoise)); + newFactors.emplace_shared(key_6, key_5, z, odoNoise); if (i % 7 == 0) - newFactors.push_back(BetweenFactor(key_7, key_6, Point2(1.0, 0.0), odometerNoise)); + newFactors.emplace_shared(key_7, key_6, z, odoNoise); if (i % 8 == 0) - newFactors.push_back(BetweenFactor(key_8, key_7, Point2(1.0, 0.0), odometerNoise)); + newFactors.emplace_shared(key_8, key_7, z, odoNoise); if (i % 9 == 0) - newFactors.push_back(BetweenFactor(key_9, key_8, Point2(1.0, 0.0), odometerNoise)); + newFactors.emplace_shared(key_9, key_8, z, odoNoise); if (i % 10 == 0) - newFactors.push_back(BetweenFactor(key_10, key_9, Point2(1.0, 0.0), odometerNoise)); + newFactors.emplace_shared(key_10, key_9, z, odoNoise); - newValues.insert(key_0, Point2(double(i)+0.1, -0.1)); + newValues.insert(key_0, Point2(double(i) + 0.1, -0.1)); newTimestamps[key_0] = double(i); fullgraph.push_back(newFactors); @@ -330,7 +347,9 @@ TEST( IncrementalFixedLagSmoother, Example ) // Check CHECK(check_smoother(fullgraph, fullinit, smoother, key_0)); - PrintSymbolicTree(smoother.getISAM2(), "Bayes Tree marginalization test: i = " + std::to_string(i)); + PrintSymbolicTree( + smoother.getISAM2(), + "Bayes Tree marginalization test: i = " + std::to_string(i)); ++i; } @@ -338,5 +357,157 @@ TEST( IncrementalFixedLagSmoother, Example ) } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +namespace issue1452 { + +// Factor types definition +enum FactorType { PRIOR = 0, BETWEEN = 1 }; + +// Helper function to read covariance matrix from input stream +Matrix6 readCovarianceMatrix(istringstream& iss) { + Matrix6 cov; + for (int r = 0; r < 6; ++r) { + for (int c = 0; c < 6; ++c) { + iss >> cov(r, c); + } + } + return cov; +} + +// Helper function to create pose from parameters +Pose3 createPose(double x, double y, double z, double roll, double pitch, + double yaw) { + return Pose3(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z)); +} + +/** + * Data Format + * PRIOR factor: factor_type timestamp key pose(x y z roll pitch yaw) cov(6*6) + * BETWEEN factor: factor_type timestamp key1 key2 pose(x y z r p y) cov(6*6) + * */ + +TEST(IncrementalFixedLagSmoother, Issue1452) { + // Open factor graph file + auto path = findExampleDataFile("issue1452.txt"); + cout << "path = " << path << endl; + ifstream infile(path); + CHECK(infile.is_open()); + + // Setup ISAM2 parameters for smoother + ISAM2Params isam_parameters; + isam_parameters.relinearizeThreshold = 0.1; + isam_parameters.relinearizeSkip = 1; + // isam_parameters.cacheLinearizedFactors = true; + isam_parameters.findUnusedFactorSlots = true; + // isam_parameters.evaluateNonlinearError = false; + // isam_parameters.enableDetailedResults = true; + + // Initialize fixed-lag smoother with 1-second window + IncrementalFixedLagSmoother smoother(1, isam_parameters); + NonlinearFactorGraph newFactors; + Values newValues, currentEstimate; + FixedLagSmoother::KeyTimestampMap newTimestamps; + Pose3 lastPose; + + // check the isam parameters + isam_parameters.print(); + + string line; + int lineCount = 0; + while (getline(infile, line)) { + if (line.empty()) continue; + istringstream iss(line); + + // if we only want to read less data + // if (lineCount > 100) break; + + cout << "\n========================Processing line " << ++lineCount + << " =========================" << endl; + int factorType; + iss >> factorType; + if (factorType == PRIOR) { + // Read prior factor data, only the first line to fix the coordinate + // system + double timestamp; + int key; + double x, y, z, roll, pitch, yaw; + iss >> timestamp >> key >> x >> y >> z >> roll >> pitch >> yaw; + // Create pose and add prior factor + Pose3 pose = createPose(x, y, z, roll, pitch, yaw); + Matrix6 cov = readCovarianceMatrix(iss); + auto noise = noiseModel::Gaussian::Covariance(cov); + newFactors.add(PriorFactor(X(key), pose, noise)); + if (!newValues.exists(X(key))) { + newValues.insert(X(key), pose); + newTimestamps[X(key)] = timestamp; + } + cout << "Add prior factor " << key << endl; + } else if (factorType == BETWEEN) { + // Read between factor data + double timestamp; + int key1, key2; + // Read timestamps and keys + iss >> timestamp >> key1 >> key2; + // Read relative pose between key1 and key2 + double x1, y1, z1, roll1, pitch1, yaw1; + iss >> x1 >> y1 >> z1 >> roll1 >> pitch1 >> yaw1; + Pose3 relative_pose = createPose(x1, y1, z1, roll1, pitch1, yaw1); + // Read covariance of relative_pose + Matrix6 cov = readCovarianceMatrix(iss); + auto noise = noiseModel::Gaussian::Covariance(cov); + // Add between factor of key1 and key2 + newFactors.add( + BetweenFactor(X(key1), X(key2), relative_pose, noise)); + if (!newValues.exists(X(key2))) { + // Use last optimized pose composed with relative pose for key2 + newValues.insert(X(key2), lastPose.compose(relative_pose)); + newTimestamps[X(key2)] = timestamp; + } + cout << "Add between factor " << key1 << " -> " << key2 << endl; + } + // Print statistics before update + cout << "Before update - Factors: " << smoother.getFactors().size() + << ", NR Factors: " << smoother.getFactors().nrFactors() << endl; + cout << "New factors: " << newFactors.size() + << ", New values: " << newValues.size() << endl; + // Update smoother + try { + smoother.update(newFactors, newValues, newTimestamps); + int max_extra_iterations = 3; + for (size_t n_iter = 1; n_iter < max_extra_iterations; ++n_iter) { + smoother.update(); + } + + cout << "After update - Factors: " << smoother.getFactors().size() + << ", NR Factors: " << smoother.getFactors().nrFactors() << endl; + + // Update current estimate and last pose + currentEstimate = smoother.calculateEstimate(); + if (!currentEstimate.empty()) { + lastPose = currentEstimate.at(currentEstimate.keys().back()); + // Optional: Print the latest pose for debugging + // cout << "Latest pose: " << + // lastPose.translation().transpose() << endl; + } + + // Clear containers for next iteration + newFactors.resize(0); + newValues.clear(); + newTimestamps.clear(); + } catch (const exception& e) { + cerr << "Update failed: " << e.what() << endl; + } + } + + // Check that the number of factors is correct + CHECK_EQUAL(12, smoother.getFactors().size()); + CHECK_EQUAL(11, smoother.getFactors().nrFactors()); + infile.close(); +} + +} // namespace issue1452 +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ From 57b8f6b158e4fb633080443a0d4e0f8715831ab7 Mon Sep 17 00:00:00 2001 From: morten Date: Tue, 14 Jan 2025 12:09:10 +0100 Subject: [PATCH 16/52] fix GPSFactor serialization --- gtsam/navigation/GPSFactor.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/gtsam/navigation/GPSFactor.h b/gtsam/navigation/GPSFactor.h index e954d719b..b180911ae 100644 --- a/gtsam/navigation/GPSFactor.h +++ b/gtsam/navigation/GPSFactor.h @@ -112,6 +112,7 @@ private: & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(nT_); + ar & BOOST_SERIALIZATION_NVP(B_t_BG_); } #endif }; @@ -186,6 +187,7 @@ private: & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(nT_); + ar & BOOST_SERIALIZATION_NVP(B_t_BG_); } #endif }; From 7efcb468f501e046a335ab471b245e0896c4f785 Mon Sep 17 00:00:00 2001 From: morten Date: Tue, 14 Jan 2025 12:09:27 +0100 Subject: [PATCH 17/52] update GPSFactor interface --- gtsam/navigation/navigation.i | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i index ceeab3b35..7300f87ba 100644 --- a/gtsam/navigation/navigation.i +++ b/gtsam/navigation/navigation.i @@ -329,7 +329,8 @@ virtual class Pose3AttitudeFactor : gtsam::NoiseModelFactor { #include virtual class GPSFactor : gtsam::NonlinearFactor{ GPSFactor(size_t key, const gtsam::Point3& gpsIn, - const gtsam::noiseModel::Base* model); + const gtsam::noiseModel::Base* model, + const gtsam::Point3& leverArm); // Testable void print(string s = "", const gtsam::KeyFormatter& keyFormatter = @@ -338,6 +339,7 @@ virtual class GPSFactor : gtsam::NonlinearFactor{ // Standard Interface gtsam::Point3 measurementIn() const; + gtsam::Point3 leverArm() const; // enable serialization functionality void serialize() const; @@ -345,7 +347,8 @@ virtual class GPSFactor : gtsam::NonlinearFactor{ virtual class GPSFactor2 : gtsam::NonlinearFactor { GPSFactor2(size_t key, const gtsam::Point3& gpsIn, - const gtsam::noiseModel::Base* model); + const gtsam::noiseModel::Base* model, + const gtsam::Point3& leverArm); // Testable void print(string s = "", const gtsam::KeyFormatter& keyFormatter = @@ -354,6 +357,7 @@ virtual class GPSFactor2 : gtsam::NonlinearFactor { // Standard Interface gtsam::Point3 measurementIn() const; + gtsam::Point3 leverArm() const; // enable serialization functionality void serialize() const; From a4f88d2d2552fe7334f0b424afa1a8952e2755ad Mon Sep 17 00:00:00 2001 From: JokerJohn <2022087641@qq.com> Date: Tue, 14 Jan 2025 19:44:16 +0800 Subject: [PATCH 18/52] add IncrementalFixedLagExample to showcase how setting addresses the issue #1452 --- .../IncrementalFixedLagSmootherExample.cpp | 273 ++++++++++++++++++ .../IncrementalFixedLagSmootherTestData.txt | 200 +++++++++++++ 2 files changed, 473 insertions(+) create mode 100644 gtsam_unstable/examples/IncrementalFixedLagSmootherExample.cpp create mode 100644 gtsam_unstable/examples/IncrementalFixedLagSmootherTestData.txt diff --git a/gtsam_unstable/examples/IncrementalFixedLagSmootherExample.cpp b/gtsam_unstable/examples/IncrementalFixedLagSmootherExample.cpp new file mode 100644 index 000000000..19f2cc6ab --- /dev/null +++ b/gtsam_unstable/examples/IncrementalFixedLagSmootherExample.cpp @@ -0,0 +1,273 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010-2020, 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 + + * -------------------------------------------------------------------------- */ + +/** +* @file IncrementalFixedLagExample.cpp +* @brief An incremental fixed-lag smoother in GTSAM using real-world data. +* @author Xiangcheng Hu (xhubd@connect.ust.hk), Frank Dellaert, Kevin Doherty +* @date Janaury 2025 +* +* Key objectives: +* - Validate `IncrementalFixedLagSmoother` functionality with real-world data. +* - Showcase how setting `findUnusedFactorSlots = true` addresses the issue #1452 in GTSAM, ensuring +* that unused factor slots (nullptrs) are correctly released when old factors are marginalized. +* +* This example leverages pose measurements from a real scenario. The test data (named "IncrementalFixedLagSmootherTestData.txt") is +* based on the corridor_day sequence from the FusionPortable dataset (https://fusionportable.github.io/dataset/fusionportable/). +* - 1 prior factor derived from point cloud ICP alignment with a prior map. +* - 199 relative pose factors derived from FAST-LIO2 odometry. +* +* Data Format (IncrementalFixedLagSmootherTestData.txt): +* 1) PRIOR factor line: +* @code +* 0 timestamp key x y z roll pitch yaw cov_6x6 +* @endcode +* - "0" indicates PRIOR factor. +* - "timestamp" is the observation time (in seconds). +* - "key" is the integer ID for the Pose3 variable. +* - (x, y, z, roll, pitch, yaw) define the pose. +* - "cov_6x6" is the full 6x6 covariance matrix (row-major). +* +* 2) BETWEEN factor line: +* @code +* 1 timestamp key1 key2 x y z roll pitch yaw cov_6x6 +* @endcode +* - "1" indicates BETWEEN factor. +* - "timestamp" is the observation time (in seconds). +* - "key1" and "key2" are the integer IDs for the connected Pose3 variables. +* - (x, y, z, roll, pitch, yaw) define the relative pose between these variables. +* - "cov_6x6" is the full 6x6 covariance matrix (row-major). +* +* See also: +* - GTSAM Issue #1452: https://github.com/borglab/gtsam/issues/1452 +*/ + +// STL +#include +#include +// GTSAM +#include +#include +#include +#include +#include +#include +#include +#include +#include // for writeG2o + +using namespace std; +using namespace gtsam; +// Shorthand for symbols +using symbol_shorthand::X; // Pose3 (x,y,z, roll, pitch, yaw) + +// Factor types +enum FactorType { + PRIOR = 0, + BETWEEN = 1 +}; + +typedef Eigen::Matrix Matrix6; + +/* ************************************************************************* */ +/** + * @brief Read a 6x6 covariance matrix from an input string stream. + * + * @param iss Input string stream containing covariance entries. + * @return 6x6 covariance matrix. + */ +Matrix6 readCovarianceMatrix(istringstream &iss) { + Matrix6 cov; + for (int r = 0; r < 6; ++r) { + for (int c = 0; c < 6; ++c) { + iss >> cov(r, c); + } + } + return cov; +} + +/* ************************************************************************* */ +/** + * @brief Create a Pose3 object from individual pose parameters. + * + * @param x Translation in x + * @param y Translation in y + * @param z Translation in z + * @param roll Rotation about x-axis + * @param pitch Rotation about y-axis + * @param yaw Rotation about z-axis + * @return Constructed Pose3 object + */ +Pose3 createPose(double x, double y, double z, double roll, double pitch, double yaw) { + return Pose3(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z)); +} + +/* ************************************************************************* */ +/** + * @brief Save the factor graph and estimates to a .g2o file (for visualization/debugging). + * + * @param graph The factor graph + * @param estimate Current estimates of all variables + * @param filename Base filename for saving + * @param iterCount Iteration count to differentiate successive outputs + */ +void saveG2oGraph(const NonlinearFactorGraph &graph, const Values &estimate, + const string &filename, int iterCount) { + // Create zero-padded iteration count + string countStr = to_string(iterCount); + string paddedCount = string(5 - countStr.length(), '0') + countStr; + string fullFilename = filename + "_" + paddedCount + ".g2o"; + // Write graph and estimates to g2o file + writeG2o(graph, estimate, fullFilename); + cout << "\nSaved graph to: " << fullFilename << endl; +} + +/* ************************************************************************* */ +/** + * @brief Main function: Reads poses data from a text file and performs incremental fixed-lag smoothing. + * + * Data Flow: + * 1) Parse lines from "IncrementalFixedLagSmootherTestData.txt". + * 2) For each line: + * - If it's a PRIOR factor, add a PriorFactor with a specified pose and covariance. + * - If it's a BETWEEN factor, add a BetweenFactor with a relative pose and covariance. + * - Insert new variables with initial guesses into the current solution if they don't exist. + * 3) Update the fixed-lag smoother (with iSAM2 inside) to incrementally optimize and marginalize out old poses + * beyond the specified lag window. + * 4) Repeat until all lines are processed. + * 5) Save the resulting factor graph and estimate of the last sliding window to a .g2o file. + */ +int main() { + // Path to the test data file + ifstream infile("IncrementalFixedLagSmootherTestData.txt"); + if (!infile.is_open()) { + cerr << "Failed to open factor graph file: IncrementalFixedLagSmootherTestData.txt" << endl; + return -1; + } + + // Configure ISAM2 parameters for the fixed-lag smoother + ISAM2Params isamParameters; + isamParameters.relinearizeThreshold = 0.1; + isamParameters.relinearizeSkip = 1; + + // Important!!!!!! Key parameter to ensure old factors are released after marginalization + isamParameters.findUnusedFactorSlots = true; + // Initialize fixed-lag smoother with a 1-second lag window + const double lag = 1.0; + IncrementalFixedLagSmoother smoother(lag, isamParameters); + // Print the iSAM2 parameters (optional) + isamParameters.print(); + + // Containers for incremental updates + NonlinearFactorGraph newFactors; + Values newValues; + FixedLagSmoother::KeyTimestampMap newTimestamps; + // For tracking the latest estimate of all states in the sliding window + Values currentEstimate; + Pose3 lastPose; // Used to provide an initial guess for new poses + + // Read and process each line in the factor graph file + string line; + int lineCount = 0; + while (getline(infile, line)) { + if (line.empty()) continue; // Skip empty lines + cout << "\n======================== Processing line " << ++lineCount + << " =========================" << endl; + istringstream iss(line); + int factorType; + iss >> factorType; + // Check if the factor is PRIOR or BETWEEN + if (factorType == PRIOR) { + /** + * Format: PRIOR factor + * factor_type timestamp key pose(x y z roll pitch yaw) cov(6x6) + */ + double timestamp; + int key; + double x, y, z, roll, pitch, yaw; + iss >> timestamp >> key >> x >> y >> z >> roll >> pitch >> yaw; + Pose3 pose = createPose(x, y, z, roll, pitch, yaw); + Matrix6 cov = readCovarianceMatrix(iss); + auto noise = noiseModel::Gaussian::Covariance(cov); + // Add prior factor + newFactors.add(PriorFactor(X(key), pose, noise)); + cout << "Add PRIOR factor on key " << key << endl; + // Provide initial guess if not already in the graph + if (!newValues.exists(X(key))) { + newValues.insert(X(key), pose); + newTimestamps[X(key)] = timestamp; + } + } else if (factorType == BETWEEN) { + /** + * Format: BETWEEN factor + * factor_type timestamp key1 key2 pose(x y z roll pitch yaw) cov(6x6) + */ + double timestamp; + int key1, key2; + iss >> timestamp >> key1 >> key2; + double x1, y1, z1, roll1, pitch1, yaw1; + iss >> x1 >> y1 >> z1 >> roll1 >> pitch1 >> yaw1; + Pose3 relativePose = createPose(x1, y1, z1, roll1, pitch1, yaw1); + Matrix6 cov = readCovarianceMatrix(iss); + auto noise = noiseModel::Gaussian::Covariance(cov); + // Add between factor + newFactors.add(BetweenFactor(X(key1), X(key2), relativePose, noise)); + cout << "Add BETWEEN factor: " << key1 << " -> " << key2 << endl; + // Provide an initial guess if needed + if (!newValues.exists(X(key2))) { + newValues.insert(X(key2), lastPose.compose(relativePose)); + newTimestamps[X(key2)] = timestamp; + } + } else { + cerr << "Unknown factor type: " << factorType << endl; + continue; + } + + // Print some intermediate statistics + cout << "Before update - Graph has " << smoother.getFactors().size() + << " factors, " << smoother.getFactors().nrFactors() << " nr factors." << endl; + cout << "New factors: " << newFactors.size() + << ", New values: " << newValues.size() << endl; + + // Attempt to update the smoother with new factors and values + try { + smoother.update(newFactors, newValues, newTimestamps); + // Optional: Perform extra internal iterations if needed + size_t maxExtraIterations = 3; + for (size_t i = 1; i < maxExtraIterations; ++i) { + smoother.update(); + } + // you may not get expected results if you use the gtsam version lower than 4.3 + cout << "After update - Graph has " << smoother.getFactors().size() + << " factors, " << smoother.getFactors().nrFactors() << " nr factors." << endl; + + // Retrieve the latest estimate + currentEstimate = smoother.calculateEstimate(); + if (!currentEstimate.empty()) { + // Update lastPose to the last key's estimate + Key lastKey = currentEstimate.keys().back(); + lastPose = currentEstimate.at(lastKey); + } + // Clear containers for the next iteration + newFactors.resize(0); + newValues.clear(); + newTimestamps.clear(); + } catch (const exception &e) { + cerr << "Smoother update failed: " << e.what() << endl; + } + } + + // Save final graph and estimates to a g2o file + // The results of the last sliding window are saved to a g2o file for visualization or further analysis. + saveG2oGraph(smoother.getFactors(), currentEstimate, path + "isam", lineCount); + infile.close(); + return 0; +} \ No newline at end of file diff --git a/gtsam_unstable/examples/IncrementalFixedLagSmootherTestData.txt b/gtsam_unstable/examples/IncrementalFixedLagSmootherTestData.txt new file mode 100644 index 000000000..518f1fe3e --- /dev/null +++ b/gtsam_unstable/examples/IncrementalFixedLagSmootherTestData.txt @@ -0,0 +1,200 @@ +0 1645005900.600844860077 0 6.217194869853 0.125917563436 0.783594893995 -0.033789843270 0.047134300047 0.266189953411 0.000021415732 0.000001074791 -0.000000147245 -0.000000847030 0.000019167883 0.000016718464 0.000001074791 0.000001605304 -0.000000003066 -0.000001377764 0.000000939474 0.000008913959 -0.000000147245 -0.000000003066 0.000001484791 -0.000000768622 -0.000007854923 -0.000000092444 -0.000000847030 -0.000001377764 -0.000000768622 0.000104453167 0.000003272897 -0.000007611301 0.000019167883 0.000000939474 -0.000007854923 0.000003272897 0.000160225289 0.000014846515 0.000016718464 0.000008913959 -0.000000092444 -0.000007611301 0.000014846515 0.000157951799 +1 1645005900.701431751251 0 1 0.005593021030 0.001526481838 -0.001566024571 -0.000225927310 0.000128593690 -0.003791450430 0.000000179286 -0.000000039948 0.000000006043 -0.000000004575 -0.000000022632 0.000000091709 -0.000000039948 0.000000018979 -0.000000001546 0.000000001926 0.000000006078 -0.000000017221 0.000000006043 -0.000000001546 0.000000015351 -0.000000013913 0.000000001925 0.000000001075 -0.000000004575 0.000000001926 -0.000000013913 0.000000561628 -0.000000028062 0.000000012214 -0.000000022632 0.000000006078 0.000000001925 -0.000000028062 0.000000490365 -0.000000005530 0.000000091709 -0.000000017221 0.000000001075 0.000000012214 -0.000000005530 0.000000550990 +1 1645005900.799895286560 1 2 0.005283034558 0.001722784230 -0.000367567191 0.000386621238 0.000182655752 -0.003429626268 0.000000162458 -0.000000036144 0.000000005112 -0.000000002457 -0.000000020766 0.000000082373 -0.000000036144 0.000000017835 -0.000000001282 0.000000001448 0.000000005345 -0.000000014851 0.000000005112 -0.000000001282 0.000000014551 -0.000000013437 0.000000001790 0.000000000998 -0.000000002457 0.000000001448 -0.000000013437 0.000000523714 -0.000000027843 0.000000011143 -0.000000020766 0.000000005345 0.000000001790 -0.000000027843 0.000000456118 -0.000000005134 0.000000082373 -0.000000014851 0.000000000998 0.000000011143 -0.000000005134 0.000000510306 +1 1645005900.902594566345 2 3 0.003030780414 0.000830079823 0.000494312020 -0.000020397824 0.000094347098 -0.002984943110 0.000000145900 -0.000000032354 0.000000004126 -0.000000001486 -0.000000018203 0.000000070890 -0.000000032354 0.000000016529 -0.000000001105 0.000000001395 0.000000004540 -0.000000012806 0.000000004126 -0.000000001105 0.000000014132 -0.000000012278 0.000000001993 0.000000000686 -0.000000001486 0.000000001395 -0.000000012278 0.000000469953 -0.000000025678 0.000000008948 -0.000000018203 0.000000004540 0.000000001993 -0.000000025678 0.000000410806 -0.000000003776 0.000000070890 -0.000000012806 0.000000000686 0.000000008948 -0.000000003776 0.000000445310 +1 1645005901.003520727158 3 4 -0.000206245469 -0.001102739599 0.000139509744 -0.000044223949 0.000133857483 -0.002842172502 0.000000130290 -0.000000028944 0.000000003338 -0.000000000544 -0.000000016340 0.000000062247 -0.000000028944 0.000000015436 -0.000000000943 0.000000001260 0.000000004005 -0.000000011220 0.000000003338 -0.000000000943 0.000000014049 -0.000000012189 0.000000001599 0.000000000394 -0.000000000544 0.000000001260 -0.000000012189 0.000000464881 -0.000000026439 0.000000008623 -0.000000016340 0.000000004005 0.000000001599 -0.000000026439 0.000000404384 -0.000000003057 0.000000062247 -0.000000011220 0.000000000394 0.000000008623 -0.000000003057 0.000000423376 +1 1645005901.101174592972 4 5 -0.001500063017 -0.002890410973 -0.000122347193 0.000014693626 0.000245148283 -0.002217688406 0.000000119368 -0.000000026625 0.000000002896 0.000000000371 -0.000000014770 0.000000055853 -0.000000026625 0.000000014763 -0.000000000823 0.000000000938 0.000000003581 -0.000000009513 0.000000002896 -0.000000000823 0.000000013680 -0.000000011988 0.000000001262 0.000000000298 0.000000000371 0.000000000938 -0.000000011988 0.000000464346 -0.000000027637 0.000000007687 -0.000000014770 0.000000003581 0.000000001262 -0.000000027637 0.000000402444 -0.000000002216 0.000000055853 -0.000000009513 0.000000000298 0.000000007687 -0.000000002216 0.000000418928 +1 1645005901.201069116592 5 6 0.001046374496 -0.004032584187 -0.000846599681 0.000321181584 -0.000094749466 -0.002057391696 0.000000110823 -0.000000024485 0.000000002417 0.000000001106 -0.000000012269 0.000000051317 -0.000000024485 0.000000014167 -0.000000000692 0.000000000635 0.000000003038 -0.000000008394 0.000000002417 -0.000000000692 0.000000013921 -0.000000011892 0.000000000364 0.000000000007 0.000000001106 0.000000000635 -0.000000011892 0.000000448440 -0.000000027796 0.000000007091 -0.000000012269 0.000000003038 0.000000000364 -0.000000027796 0.000000388841 -0.000000000851 0.000000051317 -0.000000008394 0.000000000007 0.000000007091 -0.000000000851 0.000000405686 +1 1645005901.302946805954 6 7 0.000793901956 -0.003389991954 -0.001184675507 -0.000316637882 0.000125753514 -0.001152057016 0.000000104712 -0.000000022968 0.000000001967 0.000000001772 -0.000000010574 0.000000048337 -0.000000022968 0.000000013707 -0.000000000599 0.000000000391 0.000000002734 -0.000000007830 0.000000001967 -0.000000000599 0.000000014353 -0.000000012287 -0.000000001119 -0.000000000386 0.000000001772 0.000000000391 -0.000000012287 0.000000430486 -0.000000025901 0.000000008441 -0.000000010574 0.000000002734 -0.000000001119 -0.000000025901 0.000000374002 0.000000000168 0.000000048337 -0.000000007830 -0.000000000386 0.000000008441 0.000000000168 0.000000386878 +1 1645005901.403146505356 7 8 0.000422785492 -0.001577894861 -0.000916262031 0.000148651815 -0.000064627432 -0.001220538595 0.000000100167 -0.000000021927 0.000000002289 0.000000000677 -0.000000011203 0.000000045605 -0.000000021927 0.000000013460 -0.000000000705 0.000000000708 0.000000002868 -0.000000007296 0.000000002289 -0.000000000705 0.000000014003 -0.000000011457 -0.000000000961 -0.000000000148 0.000000000677 0.000000000708 -0.000000011457 0.000000410964 -0.000000025258 0.000000008570 -0.000000011203 0.000000002868 -0.000000000961 -0.000000025258 0.000000357815 -0.000000000299 0.000000045605 -0.000000007296 -0.000000000148 0.000000008570 -0.000000000299 0.000000367628 +1 1645005901.502384662628 8 9 -0.002329688882 -0.000140222280 0.000009892231 -0.000239651077 0.000253304248 -0.001105202980 0.000000095969 -0.000000020860 0.000000002507 -0.000000000886 -0.000000012048 0.000000043109 -0.000000020860 0.000000013064 -0.000000000726 0.000000001166 0.000000002962 -0.000000007369 0.000000002507 -0.000000000726 0.000000013480 -0.000000010270 -0.000000000517 0.000000000184 -0.000000000886 0.000000001166 -0.000000010270 0.000000387773 -0.000000024930 0.000000007342 -0.000000012048 0.000000002962 -0.000000000517 -0.000000024930 0.000000338486 -0.000000001780 0.000000043109 -0.000000007369 0.000000000184 0.000000007342 -0.000000001780 0.000000347313 +1 1645005901.599597692490 9 10 -0.004026581327 0.000969420632 0.001171132830 0.000030066830 -0.000042684471 -0.001078015407 0.000000092798 -0.000000020050 0.000000002088 -0.000000000514 -0.000000011837 0.000000040468 -0.000000020050 0.000000012604 -0.000000000661 0.000000001084 0.000000002794 -0.000000007809 0.000000002088 -0.000000000661 0.000000013337 -0.000000009999 -0.000000000185 0.000000000097 -0.000000000514 0.000000001084 -0.000000009999 0.000000364337 -0.000000023644 0.000000006385 -0.000000011837 0.000000002794 -0.000000000185 -0.000000023644 0.000000320215 -0.000000002428 0.000000040468 -0.000000007809 0.000000000097 0.000000006385 -0.000000002428 0.000000324189 +1 1645005901.700814247131 10 11 -0.003099084350 0.000842028850 0.001191700627 -0.000180302292 -0.000304391953 -0.000943844689 0.000000090213 -0.000000019520 0.000000001928 -0.000000000067 -0.000000011416 0.000000037331 -0.000000019520 0.000000012352 -0.000000000665 0.000000000889 0.000000002684 -0.000000007936 0.000000001928 -0.000000000665 0.000000013403 -0.000000009790 0.000000000272 0.000000000027 -0.000000000067 0.000000000889 -0.000000009790 0.000000343339 -0.000000021912 0.000000005787 -0.000000011416 0.000000002684 0.000000000272 -0.000000021912 0.000000304258 -0.000000002229 0.000000037331 -0.000000007936 0.000000000027 0.000000005787 -0.000000002229 0.000000301522 +1 1645005901.799977779388 11 12 -0.001119124717 0.000911769018 0.000896053010 0.000109146445 0.000004749543 -0.001769999687 0.000000088504 -0.000000019214 0.000000002010 -0.000000000364 -0.000000011599 0.000000035324 -0.000000019214 0.000000012281 -0.000000000663 0.000000000986 0.000000002726 -0.000000007878 0.000000002010 -0.000000000663 0.000000013725 -0.000000009806 0.000000000042 0.000000000058 -0.000000000364 0.000000000986 -0.000000009806 0.000000328413 -0.000000018998 0.000000005385 -0.000000011599 0.000000002726 0.000000000042 -0.000000018998 0.000000293251 -0.000000002089 0.000000035324 -0.000000007878 0.000000000058 0.000000005385 -0.000000002089 0.000000286449 +1 1645005901.900312423706 12 13 -0.000402732963 0.000242598646 -0.000072269012 0.000055999966 0.000123729242 -0.002073753983 0.000000087170 -0.000000018926 0.000000002027 -0.000000000440 -0.000000011562 0.000000034056 -0.000000018926 0.000000012165 -0.000000000686 0.000000001149 0.000000002703 -0.000000007746 0.000000002027 -0.000000000686 0.000000013717 -0.000000009355 -0.000000000107 0.000000000029 -0.000000000440 0.000000001149 -0.000000009355 0.000000315929 -0.000000016862 0.000000005085 -0.000000011562 0.000000002703 -0.000000000107 -0.000000016862 0.000000284663 -0.000000002049 0.000000034056 -0.000000007746 0.000000000029 0.000000005085 -0.000000002049 0.000000275236 +1 1645005901.999710559845 13 14 -0.000353031834 0.000520456797 -0.000885039005 0.000175471614 0.000226437074 -0.001781941944 0.000000085576 -0.000000018640 0.000000002007 -0.000000000556 -0.000000011265 0.000000032750 -0.000000018640 0.000000011971 -0.000000000692 0.000000001200 0.000000002700 -0.000000007525 0.000000002007 -0.000000000692 0.000000013341 -0.000000008639 0.000000000083 -0.000000000060 -0.000000000556 0.000000001200 -0.000000008639 0.000000304286 -0.000000016247 0.000000004967 -0.000000011265 0.000000002700 0.000000000083 -0.000000016247 0.000000275471 -0.000000002124 0.000000032750 -0.000000007525 -0.000000000060 0.000000004967 -0.000000002124 0.000000264734 +1 1645005902.100522041321 14 15 -0.000755912182 0.000162757993 -0.000940218374 0.000142873662 0.000156455499 -0.000999359978 0.000000084547 -0.000000018524 0.000000002007 -0.000000001342 -0.000000011413 0.000000031398 -0.000000018524 0.000000011963 -0.000000000637 0.000000001412 0.000000002758 -0.000000007386 0.000000002007 -0.000000000637 0.000000012995 -0.000000008153 0.000000000029 -0.000000000020 -0.000000001342 0.000000001412 -0.000000008153 0.000000294679 -0.000000015478 0.000000004679 -0.000000011413 0.000000002758 0.000000000029 -0.000000015478 0.000000268528 -0.000000002296 0.000000031398 -0.000000007386 -0.000000000020 0.000000004679 -0.000000002296 0.000000256957 +1 1645005902.203018426895 15 16 -0.000439249231 -0.000178254566 -0.000437587780 -0.000084581729 0.000049443904 -0.000079088693 0.000000083829 -0.000000018377 0.000000001938 -0.000000001650 -0.000000011605 0.000000030281 -0.000000018377 0.000000011902 -0.000000000597 0.000000001442 0.000000002713 -0.000000007253 0.000000001938 -0.000000000597 0.000000013076 -0.000000008171 0.000000000037 0.000000000101 -0.000000001650 0.000000001442 -0.000000008171 0.000000287437 -0.000000014704 0.000000003963 -0.000000011605 0.000000002713 0.000000000037 -0.000000014704 0.000000263922 -0.000000002586 0.000000030281 -0.000000007253 0.000000000101 0.000000003963 -0.000000002586 0.000000250716 +1 1645005902.301831007004 16 17 0.000298386384 -0.000560355605 -0.000119106950 0.000156455585 -0.000010964095 -0.000026764071 0.000000083200 -0.000000018095 0.000000001879 -0.000000001309 -0.000000011836 0.000000029821 -0.000000018095 0.000000011725 -0.000000000623 0.000000001381 0.000000002667 -0.000000006986 0.000000001879 -0.000000000623 0.000000013155 -0.000000008373 0.000000000404 0.000000000134 -0.000000001309 0.000000001381 -0.000000008373 0.000000283842 -0.000000014501 0.000000003889 -0.000000011836 0.000000002667 0.000000000404 -0.000000014501 0.000000261244 -0.000000002864 0.000000029821 -0.000000006986 0.000000000134 0.000000003889 -0.000000002864 0.000000247086 +1 1645005902.400156021118 17 18 0.000179886078 -0.000459777112 -0.000358763747 -0.000153694839 0.000090517348 0.000016912731 0.000000081953 -0.000000017650 0.000000001880 -0.000000001460 -0.000000011911 0.000000029712 -0.000000017650 0.000000011542 -0.000000000621 0.000000001450 0.000000002686 -0.000000006772 0.000000001880 -0.000000000621 0.000000012815 -0.000000008204 0.000000000400 0.000000000134 -0.000000001460 0.000000001450 -0.000000008204 0.000000279237 -0.000000013903 0.000000003850 -0.000000011911 0.000000002686 0.000000000400 -0.000000013903 0.000000257257 -0.000000002996 0.000000029712 -0.000000006772 0.000000000134 0.000000003850 -0.000000002996 0.000000243174 +1 1645005902.500535726547 18 19 -0.000677739243 -0.000661804764 -0.000786983512 0.000083215128 -0.000114303323 0.000518990892 0.000000081125 -0.000000017513 0.000000001940 -0.000000001754 -0.000000011942 0.000000029299 -0.000000017513 0.000000011479 -0.000000000613 0.000000001489 0.000000002719 -0.000000006813 0.000000001940 -0.000000000613 0.000000012630 -0.000000007967 0.000000000014 0.000000000174 -0.000000001754 0.000000001489 -0.000000007967 0.000000273527 -0.000000013282 0.000000003515 -0.000000011942 0.000000002719 0.000000000014 -0.000000013282 0.000000252050 -0.000000003271 0.000000029299 -0.000000006813 0.000000000174 0.000000003515 -0.000000003271 0.000000239150 +1 1645005902.600610494614 19 20 -0.001398677644 -0.000281431524 -0.000006292086 -0.000149844144 0.000155174368 0.000310385345 0.000000081164 -0.000000017520 0.000000001984 -0.000000001776 -0.000000012440 0.000000028693 -0.000000017520 0.000000011567 -0.000000000605 0.000000001477 0.000000002823 -0.000000006788 0.000000001984 -0.000000000605 0.000000012243 -0.000000007646 -0.000000000218 0.000000000207 -0.000000001776 0.000000001477 -0.000000007646 0.000000270203 -0.000000013042 0.000000003281 -0.000000012440 0.000000002823 -0.000000000218 -0.000000013042 0.000000249116 -0.000000003680 0.000000028693 -0.000000006788 0.000000000207 0.000000003281 -0.000000003680 0.000000237404 +1 1645005902.700582027435 20 21 -0.001420482214 -0.000263135886 -0.000053375571 -0.000116154210 -0.000141423619 0.000057516294 0.000000080767 -0.000000017432 0.000000001927 -0.000000001700 -0.000000012707 0.000000028372 -0.000000017432 0.000000011550 -0.000000000579 0.000000001468 0.000000002900 -0.000000006723 0.000000001927 -0.000000000579 0.000000011880 -0.000000007357 -0.000000000236 0.000000000169 -0.000000001700 0.000000001468 -0.000000007357 0.000000268424 -0.000000012714 0.000000003151 -0.000000012707 0.000000002900 -0.000000000236 -0.000000012714 0.000000247501 -0.000000003752 0.000000028372 -0.000000006723 0.000000000169 0.000000003151 -0.000000003752 0.000000236011 +1 1645005902.802828073502 21 22 -0.001522845641 -0.000074472171 0.000173559055 -0.000051052057 -0.000127920939 -0.000114638340 0.000000080331 -0.000000017568 0.000000001879 -0.000000001733 -0.000000012773 0.000000027688 -0.000000017568 0.000000011534 -0.000000000541 0.000000001430 0.000000002960 -0.000000006720 0.000000001879 -0.000000000541 0.000000011888 -0.000000007198 -0.000000000298 0.000000000147 -0.000000001733 0.000000001430 -0.000000007198 0.000000266369 -0.000000012230 0.000000003032 -0.000000012773 0.000000002960 -0.000000000298 -0.000000012230 0.000000246106 -0.000000003739 0.000000027688 -0.000000006720 0.000000000147 0.000000003032 -0.000000003739 0.000000233014 +1 1645005902.900591373444 22 23 -0.000662864106 -0.000366807912 -0.000191751859 0.000099874707 0.000044070199 -0.000205737831 0.000000079920 -0.000000017596 0.000000001894 -0.000000001895 -0.000000012932 0.000000026867 -0.000000017596 0.000000011473 -0.000000000544 0.000000001444 0.000000002996 -0.000000006939 0.000000001894 -0.000000000544 0.000000011681 -0.000000007215 -0.000000000519 0.000000000192 -0.000000001895 0.000000001444 -0.000000007215 0.000000264971 -0.000000011324 0.000000002987 -0.000000012932 0.000000002996 -0.000000000519 -0.000000011324 0.000000246113 -0.000000003809 0.000000026867 -0.000000006939 0.000000000192 0.000000002987 -0.000000003809 0.000000230070 +1 1645005902.999960184097 23 24 0.000586459972 -0.000383883637 -0.000657440721 0.000025502157 0.000173096398 -0.000657422729 0.000000078961 -0.000000017438 0.000000001897 -0.000000001864 -0.000000012901 0.000000026255 -0.000000017438 0.000000011356 -0.000000000570 0.000000001429 0.000000002941 -0.000000007077 0.000000001897 -0.000000000570 0.000000011277 -0.000000007081 -0.000000000639 0.000000000279 -0.000000001864 0.000000001429 -0.000000007081 0.000000261573 -0.000000010356 0.000000003006 -0.000000012901 0.000000002941 -0.000000000639 -0.000000010356 0.000000244298 -0.000000003671 0.000000026255 -0.000000007077 0.000000000279 0.000000003006 -0.000000003671 0.000000226695 +1 1645005903.099860429764 24 25 0.001039569017 -0.000664462296 -0.000116140678 -0.000029188969 0.000121233801 -0.000658346529 0.000000078315 -0.000000017330 0.000000001958 -0.000000001934 -0.000000012838 0.000000025439 -0.000000017330 0.000000011376 -0.000000000569 0.000000001357 0.000000002951 -0.000000007067 0.000000001958 -0.000000000569 0.000000011058 -0.000000006788 -0.000000000667 0.000000000313 -0.000000001934 0.000000001357 -0.000000006788 0.000000257200 -0.000000009782 0.000000002843 -0.000000012838 0.000000002951 -0.000000000667 -0.000000009782 0.000000240971 -0.000000003535 0.000000025439 -0.000000007067 0.000000000313 0.000000002843 -0.000000003535 0.000000223420 +1 1645005903.201798200607 25 26 0.000799401277 -0.000715390089 -0.000135250388 -0.000005401473 0.000028317278 -0.000705012899 0.000000078273 -0.000000017284 0.000000001945 -0.000000002002 -0.000000012892 0.000000025018 -0.000000017284 0.000000011434 -0.000000000564 0.000000001376 0.000000002920 -0.000000007058 0.000000001945 -0.000000000564 0.000000011188 -0.000000006747 -0.000000000284 0.000000000359 -0.000000002002 0.000000001376 -0.000000006747 0.000000255919 -0.000000009831 0.000000002438 -0.000000012892 0.000000002920 -0.000000000284 -0.000000009831 0.000000239627 -0.000000003371 0.000000025018 -0.000000007058 0.000000000359 0.000000002438 -0.000000003371 0.000000222579 +1 1645005903.299434661865 26 27 -0.000113261770 0.000023693570 -0.000214375044 -0.000117559997 0.000124720787 -0.000596143614 0.000000078325 -0.000000017305 0.000000001849 -0.000000001888 -0.000000012882 0.000000024989 -0.000000017305 0.000000011476 -0.000000000557 0.000000001385 0.000000002888 -0.000000006966 0.000000001849 -0.000000000557 0.000000011561 -0.000000006807 0.000000000150 0.000000000342 -0.000000001888 0.000000001385 -0.000000006807 0.000000256583 -0.000000010009 0.000000002389 -0.000000012882 0.000000002888 0.000000000150 -0.000000010009 0.000000240356 -0.000000003042 0.000000024989 -0.000000006966 0.000000000342 0.000000002389 -0.000000003042 0.000000222999 +1 1645005903.399843454361 27 28 -0.000958900034 0.000637600325 -0.000214249917 -0.000223045645 -0.000071809246 0.000029335082 0.000000078344 -0.000000017286 0.000000001846 -0.000000001681 -0.000000012791 0.000000024604 -0.000000017286 0.000000011476 -0.000000000598 0.000000001367 0.000000002926 -0.000000006949 0.000000001846 -0.000000000598 0.000000012087 -0.000000006902 0.000000000466 0.000000000207 -0.000000001681 0.000000001367 -0.000000006902 0.000000256077 -0.000000009887 0.000000002620 -0.000000012791 0.000000002926 0.000000000466 -0.000000009887 0.000000240846 -0.000000002625 0.000000024604 -0.000000006949 0.000000000207 0.000000002620 -0.000000002625 0.000000222033 +1 1645005903.503298759460 28 29 -0.000594952889 0.000937675663 0.000350959010 -0.000134047994 0.000112568416 0.000532529091 0.000000077870 -0.000000017120 0.000000001906 -0.000000001471 -0.000000012767 0.000000024191 -0.000000017120 0.000000011459 -0.000000000663 0.000000001370 0.000000002979 -0.000000007036 0.000000001906 -0.000000000663 0.000000012464 -0.000000006974 0.000000000554 0.000000000084 -0.000000001471 0.000000001370 -0.000000006974 0.000000254804 -0.000000009282 0.000000002485 -0.000000012767 0.000000002979 0.000000000554 -0.000000009282 0.000000241012 -0.000000002108 0.000000024191 -0.000000007036 0.000000000084 0.000000002485 -0.000000002108 0.000000220716 +1 1645005903.600738525391 29 30 0.000794414323 0.000340891713 0.000722517724 0.000049267232 -0.000083827634 0.000511848534 0.000000077849 -0.000000017062 0.000000001944 -0.000000001282 -0.000000012931 0.000000024026 -0.000000017062 0.000000011499 -0.000000000672 0.000000001311 0.000000003010 -0.000000007173 0.000000001944 -0.000000000672 0.000000012279 -0.000000007093 0.000000000279 0.000000000086 -0.000000001282 0.000000001311 -0.000000007093 0.000000255543 -0.000000008713 0.000000002207 -0.000000012931 0.000000003010 0.000000000279 -0.000000008713 0.000000242511 -0.000000001802 0.000000024026 -0.000000007173 0.000000000086 0.000000002207 -0.000000001802 0.000000221948 +1 1645005903.702013492584 30 31 0.000724079430 -0.000003591352 0.000270122431 -0.000012632068 -0.000052326779 0.000440294104 0.000000077508 -0.000000016995 0.000000001961 -0.000000001247 -0.000000012890 0.000000023883 -0.000000016995 0.000000011411 -0.000000000627 0.000000001216 0.000000003013 -0.000000007068 0.000000001961 -0.000000000627 0.000000011934 -0.000000007149 -0.000000000010 0.000000000107 -0.000000001247 0.000000001216 -0.000000007149 0.000000254403 -0.000000008732 0.000000001995 -0.000000012890 0.000000003013 -0.000000000010 -0.000000008732 0.000000240911 -0.000000001885 0.000000023883 -0.000000007068 0.000000000107 0.000000001995 -0.000000001885 0.000000221291 +1 1645005903.800774335861 31 32 0.001067662490 -0.000510502525 -0.000301534356 0.000174567922 0.000065476921 -0.000090436674 0.000000077110 -0.000000016957 0.000000001949 -0.000000001388 -0.000000012566 0.000000024020 -0.000000016957 0.000000011352 -0.000000000587 0.000000001174 0.000000002958 -0.000000006851 0.000000001949 -0.000000000587 0.000000011556 -0.000000007045 -0.000000000114 0.000000000144 -0.000000001388 0.000000001174 -0.000000007045 0.000000252509 -0.000000008765 0.000000001947 -0.000000012566 0.000000002958 -0.000000000114 -0.000000008765 0.000000238424 -0.000000002141 0.000000024020 -0.000000006851 0.000000000144 0.000000001947 -0.000000002141 0.000000219630 +1 1645005903.899410247803 32 33 0.000817144389 -0.001122214966 -0.000884357956 -0.000001891347 0.000036718074 0.000252289181 0.000000077446 -0.000000017113 0.000000001881 -0.000000001342 -0.000000012421 0.000000024153 -0.000000017113 0.000000011435 -0.000000000562 0.000000001165 0.000000002871 -0.000000006813 0.000000001881 -0.000000000562 0.000000011375 -0.000000007060 -0.000000000151 0.000000000211 -0.000000001342 0.000000001165 -0.000000007060 0.000000252108 -0.000000008770 0.000000002126 -0.000000012421 0.000000002871 -0.000000000151 -0.000000008770 0.000000237324 -0.000000002342 0.000000024153 -0.000000006813 0.000000000211 0.000000002126 -0.000000002342 0.000000219157 +1 1645005903.999286413193 33 34 0.000286745156 -0.000755201087 -0.000483958459 0.000073063588 -0.000069345847 0.000131138848 0.000000077516 -0.000000017113 0.000000001949 -0.000000001353 -0.000000012656 0.000000023945 -0.000000017113 0.000000011475 -0.000000000564 0.000000001201 0.000000002844 -0.000000006895 0.000000001949 -0.000000000564 0.000000011242 -0.000000006940 -0.000000000110 0.000000000316 -0.000000001353 0.000000001201 -0.000000006940 0.000000251401 -0.000000008774 0.000000002151 -0.000000012656 0.000000002844 -0.000000000110 -0.000000008774 0.000000235544 -0.000000002538 0.000000023945 -0.000000006895 0.000000000316 0.000000002151 -0.000000002538 0.000000218001 +1 1645005904.101194381714 34 35 -0.000092729542 -0.000721176305 -0.000046422003 0.000080600128 -0.000098482186 0.000269102398 0.000000077508 -0.000000016949 0.000000002044 -0.000000001517 -0.000000012907 0.000000023822 -0.000000016949 0.000000011511 -0.000000000628 0.000000001343 0.000000002868 -0.000000006931 0.000000002044 -0.000000000628 0.000000010979 -0.000000006697 -0.000000000142 0.000000000368 -0.000000001517 0.000000001343 -0.000000006697 0.000000251510 -0.000000008827 0.000000002031 -0.000000012907 0.000000002868 -0.000000000142 -0.000000008827 0.000000234984 -0.000000002474 0.000000023822 -0.000000006931 0.000000000368 0.000000002031 -0.000000002474 0.000000217838 +1 1645005904.202036380768 35 36 0.000159501241 -0.000619989049 -0.000182200023 -0.000130638892 -0.000030290097 0.000441918495 0.000000077457 -0.000000016871 0.000000002049 -0.000000001653 -0.000000012863 0.000000024008 -0.000000016871 0.000000011440 -0.000000000666 0.000000001365 0.000000002895 -0.000000006831 0.000000002049 -0.000000000666 0.000000010881 -0.000000006644 -0.000000000477 0.000000000349 -0.000000001653 0.000000001365 -0.000000006644 0.000000252370 -0.000000009051 0.000000002132 -0.000000012863 0.000000002895 -0.000000000477 -0.000000009051 0.000000236368 -0.000000002172 0.000000024008 -0.000000006831 0.000000000349 0.000000002132 -0.000000002172 0.000000219216 +1 1645005904.301526308060 36 37 0.000327836677 -0.000353330603 -0.000334050825 -0.000048653573 -0.000017134077 0.000406296921 0.000000077743 -0.000000017107 0.000000002089 -0.000000001553 -0.000000013005 0.000000024196 -0.000000017107 0.000000011432 -0.000000000665 0.000000001162 0.000000002958 -0.000000006693 0.000000002089 -0.000000000665 0.000000010877 -0.000000006677 -0.000000000574 0.000000000388 -0.000000001553 0.000000001162 -0.000000006677 0.000000252991 -0.000000009267 0.000000002202 -0.000000013005 0.000000002958 -0.000000000574 -0.000000009267 0.000000237868 -0.000000002187 0.000000024196 -0.000000006693 0.000000000388 0.000000002202 -0.000000002187 0.000000220113 +1 1645005904.403170108795 37 38 0.000737980852 -0.000351033561 -0.000295585445 -0.000025607001 -0.000042435192 0.000431463920 0.000000077856 -0.000000017205 0.000000002133 -0.000000001519 -0.000000013172 0.000000024074 -0.000000017205 0.000000011434 -0.000000000607 0.000000001084 0.000000003078 -0.000000006696 0.000000002133 -0.000000000607 0.000000011189 -0.000000006857 -0.000000000557 0.000000000291 -0.000000001519 0.000000001084 -0.000000006857 0.000000252632 -0.000000009363 0.000000002170 -0.000000013172 0.000000003078 -0.000000000557 -0.000000009363 0.000000237409 -0.000000002500 0.000000024074 -0.000000006696 0.000000000291 0.000000002170 -0.000000002500 0.000000219313 +1 1645005904.501605749130 38 39 0.001088348237 -0.000613129911 0.000232566844 0.000179265756 -0.000069468820 -0.000092368701 0.000000077918 -0.000000017330 0.000000002087 -0.000000001727 -0.000000013339 0.000000023890 -0.000000017330 0.000000011400 -0.000000000554 0.000000001171 0.000000003171 -0.000000006655 0.000000002087 -0.000000000554 0.000000011497 -0.000000006951 -0.000000000628 0.000000000205 -0.000000001727 0.000000001171 -0.000000006951 0.000000252754 -0.000000009318 0.000000002086 -0.000000013339 0.000000003171 -0.000000000628 -0.000000009318 0.000000237371 -0.000000002624 0.000000023890 -0.000000006655 0.000000000205 0.000000002086 -0.000000002624 0.000000219216 +1 1645005904.599536895752 39 40 0.000468476145 -0.001272947114 0.000093562933 -0.000116852710 0.000115936099 0.000225755043 0.000000077895 -0.000000017350 0.000000001892 -0.000000001532 -0.000000013201 0.000000023949 -0.000000017350 0.000000011402 -0.000000000560 0.000000001138 0.000000003123 -0.000000006687 0.000000001892 -0.000000000560 0.000000011557 -0.000000006985 -0.000000000681 0.000000000172 -0.000000001532 0.000000001138 -0.000000006985 0.000000252449 -0.000000009335 0.000000002069 -0.000000013201 0.000000003123 -0.000000000681 -0.000000009335 0.000000237483 -0.000000002381 0.000000023949 -0.000000006687 0.000000000172 0.000000002069 -0.000000002381 0.000000218909 +1 1645005904.700893878937 40 41 0.000860573463 -0.001209131350 -0.000501116645 0.000157555541 0.000010319357 -0.000048830165 0.000000077189 -0.000000017111 0.000000001814 -0.000000001358 -0.000000012728 0.000000023995 -0.000000017111 0.000000011307 -0.000000000533 0.000000001084 0.000000002997 -0.000000006770 0.000000001814 -0.000000000533 0.000000011517 -0.000000007033 -0.000000000879 0.000000000155 -0.000000001358 0.000000001084 -0.000000007033 0.000000250581 -0.000000009543 0.000000002194 -0.000000012728 0.000000002997 -0.000000000879 -0.000000009543 0.000000236400 -0.000000002342 0.000000023995 -0.000000006770 0.000000000155 0.000000002194 -0.000000002342 0.000000217604 +1 1645005904.802378892899 41 42 0.000632138609 -0.000790062715 -0.000690771042 0.000034390734 -0.000109221344 0.000029091074 0.000000077001 -0.000000017060 0.000000001956 -0.000000001694 -0.000000012495 0.000000024027 -0.000000017060 0.000000011341 -0.000000000521 0.000000001130 0.000000002989 -0.000000006785 0.000000001956 -0.000000000521 0.000000011397 -0.000000006946 -0.000000000911 0.000000000171 -0.000000001694 0.000000001130 -0.000000006946 0.000000250277 -0.000000009613 0.000000002318 -0.000000012495 0.000000002989 -0.000000000911 -0.000000009613 0.000000236262 -0.000000002568 0.000000024027 -0.000000006785 0.000000000171 0.000000002318 -0.000000002568 0.000000218078 +1 1645005904.902300596237 42 43 0.000802579583 -0.000538433330 -0.000341299568 0.000007003048 -0.000032678783 -0.000429464507 0.000000077294 -0.000000017051 0.000000002005 -0.000000001928 -0.000000012622 0.000000024150 -0.000000017051 0.000000011388 -0.000000000568 0.000000001229 0.000000003054 -0.000000006816 0.000000002005 -0.000000000568 0.000000011301 -0.000000006823 -0.000000000888 0.000000000138 -0.000000001928 0.000000001229 -0.000000006823 0.000000251648 -0.000000009299 0.000000002429 -0.000000012622 0.000000003054 -0.000000000888 -0.000000009299 0.000000237177 -0.000000002580 0.000000024150 -0.000000006816 0.000000000138 0.000000002429 -0.000000002580 0.000000218935 +1 1645005904.999253749847 43 44 0.000501016980 0.000211957757 -0.000005361234 0.000063883092 -0.000036266373 -0.000504158455 0.000000077290 -0.000000017104 0.000000001982 -0.000000001896 -0.000000012750 0.000000024173 -0.000000017104 0.000000011394 -0.000000000596 0.000000001289 0.000000003111 -0.000000006794 0.000000001982 -0.000000000596 0.000000011045 -0.000000006670 -0.000000001063 0.000000000093 -0.000000001896 0.000000001289 -0.000000006670 0.000000252269 -0.000000009368 0.000000002660 -0.000000012750 0.000000003111 -0.000000001063 -0.000000009368 0.000000236670 -0.000000002412 0.000000024173 -0.000000006794 0.000000000093 0.000000002660 -0.000000002412 0.000000218725 +1 1645005905.101718425751 44 45 -0.000024589931 0.000754394551 0.000408641144 -0.000202334514 0.000016741619 -0.000672688351 0.000000076750 -0.000000016988 0.000000001970 -0.000000001786 -0.000000012675 0.000000024005 -0.000000016988 0.000000011345 -0.000000000542 0.000000001221 0.000000003052 -0.000000006750 0.000000001970 -0.000000000542 0.000000010830 -0.000000006614 -0.000000000937 0.000000000130 -0.000000001786 0.000000001221 -0.000000006614 0.000000251233 -0.000000009933 0.000000002642 -0.000000012675 0.000000003052 -0.000000000937 -0.000000009933 0.000000234262 -0.000000002344 0.000000024005 -0.000000006750 0.000000000130 0.000000002642 -0.000000002344 0.000000217464 +1 1645005905.201553344727 45 46 -0.000215612364 0.000834606698 0.000149325139 -0.000059117112 -0.000021374139 -0.000795866284 0.000000076660 -0.000000016912 0.000000001943 -0.000000001779 -0.000000012647 0.000000023970 -0.000000016912 0.000000011337 -0.000000000541 0.000000001223 0.000000003001 -0.000000006808 0.000000001943 -0.000000000541 0.000000011099 -0.000000006710 -0.000000000647 0.000000000160 -0.000000001779 0.000000001223 -0.000000006710 0.000000251867 -0.000000010134 0.000000002780 -0.000000012647 0.000000003001 -0.000000000647 -0.000000010134 0.000000234786 -0.000000002355 0.000000023970 -0.000000006808 0.000000000160 0.000000002780 -0.000000002355 0.000000217751 +1 1645005905.298557758331 46 47 0.000863242831 0.001318878065 -0.000252232506 -0.000118643047 0.000022995563 -0.001197291392 0.000000077249 -0.000000017059 0.000000001925 -0.000000001737 -0.000000012443 0.000000024240 -0.000000017059 0.000000011387 -0.000000000615 0.000000001270 0.000000002946 -0.000000006834 0.000000001925 -0.000000000615 0.000000011672 -0.000000006817 -0.000000000141 0.000000000151 -0.000000001737 0.000000001270 -0.000000006817 0.000000253335 -0.000000010236 0.000000003256 -0.000000012443 0.000000002946 -0.000000000141 -0.000000010236 0.000000237176 -0.000000002293 0.000000024240 -0.000000006834 0.000000000151 0.000000003256 -0.000000002293 0.000000219058 +1 1645005905.399083614349 47 48 0.002259440161 0.001105945552 -0.000750095487 0.000323798878 0.000255879602 -0.002485082179 0.000000077105 -0.000000016955 0.000000001874 -0.000000001539 -0.000000012289 0.000000024392 -0.000000016955 0.000000011391 -0.000000000645 0.000000001253 0.000000002881 -0.000000006702 0.000000001874 -0.000000000645 0.000000012203 -0.000000006954 0.000000000290 0.000000000129 -0.000000001539 0.000000001253 -0.000000006954 0.000000252527 -0.000000010460 0.000000003072 -0.000000012289 0.000000002881 0.000000000290 -0.000000010460 0.000000237679 -0.000000002199 0.000000024392 -0.000000006702 0.000000000129 0.000000003072 -0.000000002199 0.000000218901 +1 1645005905.499478816986 48 49 0.001203207953 0.000792305972 -0.001325455405 0.000034228402 -0.000060355459 -0.004038449411 0.000000076987 -0.000000016850 0.000000001884 -0.000000001681 -0.000000012438 0.000000024471 -0.000000016850 0.000000011498 -0.000000000626 0.000000001299 0.000000002943 -0.000000006512 0.000000001884 -0.000000000626 0.000000012768 -0.000000007178 0.000000000282 0.000000000078 -0.000000001681 0.000000001299 -0.000000007178 0.000000253439 -0.000000010315 0.000000002860 -0.000000012438 0.000000002943 0.000000000282 -0.000000010315 0.000000238861 -0.000000002267 0.000000024471 -0.000000006512 0.000000000078 0.000000002860 -0.000000002267 0.000000220135 +1 1645005905.599982023239 49 50 -0.000427727796 0.001474783510 -0.000181500876 0.000232089095 0.000542383436 -0.007766826389 0.000000077535 -0.000000017038 0.000000001838 -0.000000001917 -0.000000012698 0.000000024720 -0.000000017038 0.000000011645 -0.000000000643 0.000000001385 0.000000003075 -0.000000006360 0.000000001838 -0.000000000643 0.000000012879 -0.000000007370 -0.000000000087 -0.000000000001 -0.000000001917 0.000000001385 -0.000000007370 0.000000256358 -0.000000009978 0.000000002881 -0.000000012698 0.000000003075 -0.000000000087 -0.000000009978 0.000000240935 -0.000000002358 0.000000024720 -0.000000006360 -0.000000000001 0.000000002881 -0.000000002358 0.000000222306 +1 1645005905.699927568436 50 51 -0.003379224466 0.003178186222 -0.000934051059 0.000195152956 0.000761914950 -0.011833240191 0.000000077971 -0.000000017303 0.000000001797 -0.000000001904 -0.000000012847 0.000000024954 -0.000000017303 0.000000011820 -0.000000000663 0.000000001463 0.000000003093 -0.000000006376 0.000000001797 -0.000000000663 0.000000012974 -0.000000007270 0.000000000071 0.000000000011 -0.000000001904 0.000000001463 -0.000000007270 0.000000257764 -0.000000010219 0.000000002728 -0.000000012847 0.000000003093 0.000000000071 -0.000000010219 0.000000242472 -0.000000002420 0.000000024954 -0.000000006376 0.000000000011 0.000000002728 -0.000000002420 0.000000223682 +1 1645005905.802299499512 51 52 -0.003954280518 0.004558444476 -0.000638200694 0.000610985838 0.000592963225 -0.016423441061 0.000000078171 -0.000000017308 0.000000001851 -0.000000001684 -0.000000012949 0.000000025066 -0.000000017308 0.000000012067 -0.000000000670 0.000000001552 0.000000003016 -0.000000006564 0.000000001851 -0.000000000670 0.000000013096 -0.000000007042 0.000000000544 0.000000000071 -0.000000001684 0.000000001552 -0.000000007042 0.000000257460 -0.000000010870 0.000000002616 -0.000000012949 0.000000003016 0.000000000544 -0.000000010870 0.000000243242 -0.000000002361 0.000000025066 -0.000000006564 0.000000000071 0.000000002616 -0.000000002361 0.000000225067 +1 1645005905.903995513916 52 53 -0.002725003716 0.004085843921 -0.000203663179 0.000790836836 0.000587671331 -0.019275453487 0.000000078783 -0.000000017322 0.000000001763 -0.000000001459 -0.000000013309 0.000000025091 -0.000000017322 0.000000012536 -0.000000000705 0.000000001676 0.000000003055 -0.000000006665 0.000000001763 -0.000000000705 0.000000012882 -0.000000007071 0.000000000738 0.000000000012 -0.000000001459 0.000000001676 -0.000000007071 0.000000258028 -0.000000012023 0.000000002543 -0.000000013309 0.000000003055 0.000000000738 -0.000000012023 0.000000244738 -0.000000002147 0.000000025091 -0.000000006665 0.000000000012 0.000000002543 -0.000000002147 0.000000228144 +1 1645005906.006122350693 53 54 -0.001467286616 0.002911939063 0.001069730244 0.000570837090 0.000647669773 -0.017525510312 0.000000079128 -0.000000017240 0.000000001741 -0.000000001521 -0.000000013627 0.000000025287 -0.000000017240 0.000000012719 -0.000000000684 0.000000001673 0.000000003084 -0.000000006545 0.000000001741 -0.000000000684 0.000000012768 -0.000000007310 0.000000001027 0.000000000025 -0.000000001521 0.000000001673 -0.000000007310 0.000000260589 -0.000000013608 0.000000002539 -0.000000013627 0.000000003084 0.000000001027 -0.000000013608 0.000000247534 -0.000000002295 0.000000025287 -0.000000006545 0.000000000025 0.000000002539 -0.000000002295 0.000000231204 +1 1645005906.099501609802 54 55 -0.001059218310 0.001595583380 0.001718691048 0.000383564738 0.000421259981 -0.014061465138 0.000000078834 -0.000000016986 0.000000001758 -0.000000001750 -0.000000013531 0.000000025769 -0.000000016986 0.000000012611 -0.000000000664 0.000000001714 0.000000002983 -0.000000006153 0.000000001758 -0.000000000664 0.000000012509 -0.000000007145 0.000000001254 0.000000000088 -0.000000001750 0.000000001714 -0.000000007145 0.000000263017 -0.000000014906 0.000000002634 -0.000000013531 0.000000002983 0.000000001254 -0.000000014906 0.000000249896 -0.000000002598 0.000000025769 -0.000000006153 0.000000000088 0.000000002634 -0.000000002598 0.000000233378 +1 1645005906.199391126633 55 56 -0.001373475218 0.001308808338 0.000378930768 0.000479365502 0.000490820641 -0.009012238322 0.000000077943 -0.000000016768 0.000000001735 -0.000000001945 -0.000000013077 0.000000026054 -0.000000016768 0.000000012465 -0.000000000630 0.000000001711 0.000000002929 -0.000000005670 0.000000001735 -0.000000000630 0.000000012365 -0.000000007041 0.000000001545 0.000000000018 -0.000000001945 0.000000001711 -0.000000007041 0.000000261574 -0.000000015843 0.000000002953 -0.000000013077 0.000000002929 0.000000001545 -0.000000015843 0.000000248225 -0.000000002881 0.000000026054 -0.000000005670 0.000000000018 0.000000002953 -0.000000002881 0.000000232505 +1 1645005906.297765493393 56 57 -0.001355900545 0.000745568120 -0.000870920659 0.000244984256 0.000234169826 -0.004721749525 0.000000077729 -0.000000016651 0.000000001590 -0.000000002229 -0.000000012620 0.000000026130 -0.000000016651 0.000000012411 -0.000000000629 0.000000001763 0.000000002808 -0.000000005332 0.000000001590 -0.000000000629 0.000000012914 -0.000000007257 0.000000001540 -0.000000000032 -0.000000002229 0.000000001763 -0.000000007257 0.000000261378 -0.000000015902 0.000000003371 -0.000000012620 0.000000002808 0.000000001540 -0.000000015902 0.000000247742 -0.000000003003 0.000000026130 -0.000000005332 -0.000000000032 0.000000003371 -0.000000003003 0.000000232061 +1 1645005906.399662256241 57 58 -0.001773912220 0.001216253031 -0.001438523133 0.000093249864 0.000296808797 -0.001971036703 0.000000077865 -0.000000016538 0.000000001543 -0.000000002529 -0.000000012492 0.000000026251 -0.000000016538 0.000000012416 -0.000000000643 0.000000001937 0.000000002742 -0.000000005129 0.000000001543 -0.000000000643 0.000000013403 -0.000000007617 0.000000001310 -0.000000000089 -0.000000002529 0.000000001937 -0.000000007617 0.000000265989 -0.000000015382 0.000000003738 -0.000000012492 0.000000002742 0.000000001310 -0.000000015382 0.000000252116 -0.000000002953 0.000000026251 -0.000000005129 -0.000000000089 0.000000003738 -0.000000002953 0.000000233770 +1 1645005906.492895603180 58 59 -0.002660918339 0.001015475851 -0.001383065292 0.000330318405 -0.000006555949 -0.001300286378 0.000000077810 -0.000000016434 0.000000001630 -0.000000002398 -0.000000012724 0.000000026385 -0.000000016434 0.000000012330 -0.000000000622 0.000000001970 0.000000002832 -0.000000004942 0.000000001630 -0.000000000622 0.000000013443 -0.000000008032 0.000000001316 -0.000000000234 -0.000000002398 0.000000001970 -0.000000008032 0.000000272129 -0.000000015431 0.000000004625 -0.000000012724 0.000000002832 0.000000001316 -0.000000015431 0.000000257860 -0.000000002721 0.000000026385 -0.000000004942 -0.000000000234 0.000000004625 -0.000000002721 0.000000235343 +1 1645005906.599270582199 59 60 -0.004412984366 -0.001971971670 -0.001231997931 -0.000056688480 0.000101702338 -0.001115010901 0.000000077414 -0.000000016358 0.000000001582 -0.000000001830 -0.000000012780 0.000000026368 -0.000000016358 0.000000012249 -0.000000000629 0.000000001982 0.000000002761 -0.000000004929 0.000000001582 -0.000000000629 0.000000013473 -0.000000007965 0.000000001386 -0.000000000292 -0.000000001830 0.000000001982 -0.000000007965 0.000000274562 -0.000000016461 0.000000005637 -0.000000012780 0.000000002761 0.000000001386 -0.000000016461 0.000000259275 -0.000000002242 0.000000026368 -0.000000004929 -0.000000000292 0.000000005637 -0.000000002242 0.000000234454 +1 1645005906.697822093964 60 61 -0.005188840029 -0.004254111564 -0.000054909271 0.000406498966 -0.000296622837 0.000416988251 0.000000077214 -0.000000016250 0.000000001521 -0.000000001261 -0.000000012771 0.000000026760 -0.000000016250 0.000000012152 -0.000000000625 0.000000001905 0.000000002698 -0.000000005164 0.000000001521 -0.000000000625 0.000000013822 -0.000000008083 0.000000001309 -0.000000000328 -0.000000001261 0.000000001905 -0.000000008083 0.000000278328 -0.000000017447 0.000000005714 -0.000000012771 0.000000002698 0.000000001309 -0.000000017447 0.000000262592 -0.000000002008 0.000000026760 -0.000000005164 -0.000000000328 0.000000005714 -0.000000002008 0.000000235536 +1 1645005906.798193216324 61 62 -0.005350833950 -0.005705298333 0.000741755143 0.000130549378 -0.000347185966 0.003649959900 0.000000077631 -0.000000016247 0.000000001454 -0.000000001004 -0.000000012891 0.000000027268 -0.000000016247 0.000000012128 -0.000000000685 0.000000001939 0.000000002674 -0.000000005295 0.000000001454 -0.000000000685 0.000000014071 -0.000000008387 0.000000001361 -0.000000000365 -0.000000001004 0.000000001939 -0.000000008387 0.000000282769 -0.000000017695 0.000000005791 -0.000000012891 0.000000002674 0.000000001361 -0.000000017695 0.000000267197 -0.000000002088 0.000000027268 -0.000000005295 -0.000000000365 0.000000005791 -0.000000002088 0.000000238789 +1 1645005906.897775650024 62 63 -0.003697841110 -0.006789558112 -0.000589171782 0.000044608399 -0.000455644208 0.005951637027 0.000000077672 -0.000000016231 0.000000001373 -0.000000000697 -0.000000012771 0.000000027509 -0.000000016231 0.000000012025 -0.000000000658 0.000000001829 0.000000002608 -0.000000005327 0.000000001373 -0.000000000658 0.000000013806 -0.000000008294 0.000000001746 -0.000000000347 -0.000000000697 0.000000001829 -0.000000008294 0.000000283100 -0.000000016930 0.000000006179 -0.000000012771 0.000000002608 0.000000001746 -0.000000016930 0.000000267634 -0.000000001907 0.000000027509 -0.000000005327 -0.000000000347 0.000000006179 -0.000000001907 0.000000238791 +1 1645005907.000208616257 63 64 -0.002944765776 -0.006697802741 -0.003116587607 -0.000323320374 -0.000089765171 0.006392215701 0.000000077610 -0.000000016115 0.000000001489 -0.000000000549 -0.000000012506 0.000000027748 -0.000000016115 0.000000011973 -0.000000000586 0.000000001526 0.000000002609 -0.000000005268 0.000000001489 -0.000000000586 0.000000013816 -0.000000008429 0.000000001848 -0.000000000278 -0.000000000549 0.000000001526 -0.000000008429 0.000000282717 -0.000000015968 0.000000006292 -0.000000012506 0.000000002609 0.000000001848 -0.000000015968 0.000000266926 -0.000000001561 0.000000027748 -0.000000005268 -0.000000000278 0.000000006292 -0.000000001561 0.000000238503 +1 1645005907.098475456238 64 65 -0.001290240390 -0.005396750920 -0.004723595682 -0.000007365578 0.000016435762 0.004075705511 0.000000077997 -0.000000016170 0.000000001561 -0.000000000791 -0.000000012603 0.000000027663 -0.000000016170 0.000000012052 -0.000000000605 0.000000001609 0.000000002641 -0.000000005184 0.000000001561 -0.000000000605 0.000000013763 -0.000000008737 0.000000001470 -0.000000000238 -0.000000000791 0.000000001609 -0.000000008737 0.000000281532 -0.000000015977 0.000000006340 -0.000000012603 0.000000002641 0.000000001470 -0.000000015977 0.000000264521 -0.000000001627 0.000000027663 -0.000000005184 -0.000000000238 0.000000006340 -0.000000001627 0.000000238811 +1 1645005907.198553800583 65 66 -0.000669485805 -0.005485448301 -0.002930170548 -0.000146916616 0.000044397139 -0.000044314603 0.000000078888 -0.000000016541 0.000000001493 -0.000000000960 -0.000000013153 0.000000027258 -0.000000016541 0.000000012208 -0.000000000625 0.000000001783 0.000000002743 -0.000000005116 0.000000001493 -0.000000000625 0.000000013535 -0.000000008377 0.000000001173 -0.000000000224 -0.000000000960 0.000000001783 -0.000000008377 0.000000278934 -0.000000015798 0.000000005980 -0.000000013153 0.000000002743 0.000000001173 -0.000000015798 0.000000261815 -0.000000001742 0.000000027258 -0.000000005116 -0.000000000224 0.000000005980 -0.000000001742 0.000000238650 +1 1645005907.299114227295 66 67 -0.000737675673 -0.006602658374 0.001504502005 0.000251984015 0.000332600718 -0.004637221316 0.000000078978 -0.000000016696 0.000000001539 -0.000000001083 -0.000000013528 0.000000027256 -0.000000016696 0.000000012302 -0.000000000624 0.000000001764 0.000000002889 -0.000000005225 0.000000001539 -0.000000000624 0.000000013491 -0.000000008150 0.000000001072 -0.000000000210 -0.000000001083 0.000000001764 -0.000000008150 0.000000275585 -0.000000015902 0.000000005576 -0.000000013528 0.000000002889 0.000000001072 -0.000000015902 0.000000258948 -0.000000001809 0.000000027256 -0.000000005225 -0.000000000210 0.000000005576 -0.000000001809 0.000000238197 +1 1645005907.402397871017 67 68 -0.000551820100 -0.007603293394 0.003319136466 0.000085475476 0.000206125712 -0.005773773594 0.000000078821 -0.000000016809 0.000000001506 -0.000000001311 -0.000000013626 0.000000027682 -0.000000016809 0.000000012396 -0.000000000614 0.000000001766 0.000000002926 -0.000000005476 0.000000001506 -0.000000000614 0.000000013915 -0.000000008554 0.000000000776 -0.000000000165 -0.000000001311 0.000000001766 -0.000000008554 0.000000275078 -0.000000015973 0.000000005780 -0.000000013626 0.000000002926 0.000000000776 -0.000000015973 0.000000258664 -0.000000002050 0.000000027682 -0.000000005476 -0.000000000165 0.000000005780 -0.000000002050 0.000000238900 +1 1645005907.500724315643 68 69 -0.000078335904 -0.007900987662 0.000767252658 0.000325260818 0.000204141643 -0.004069357072 0.000000079410 -0.000000017045 0.000000001346 -0.000000001198 -0.000000013307 0.000000027863 -0.000000017045 0.000000012459 -0.000000000602 0.000000001790 0.000000002868 -0.000000005540 0.000000001346 -0.000000000602 0.000000014320 -0.000000009106 0.000000000386 -0.000000000282 -0.000000001198 0.000000001790 -0.000000009106 0.000000277944 -0.000000015885 0.000000006351 -0.000000013307 0.000000002868 0.000000000386 -0.000000015885 0.000000260452 -0.000000002077 0.000000027863 -0.000000005540 -0.000000000282 0.000000006351 -0.000000002077 0.000000240393 +1 1645005907.605340957642 69 70 0.001043055952 -0.008573117265 -0.001598483094 -0.000097387849 -0.000222906961 -0.000595407429 0.000000080055 -0.000000017438 0.000000001351 -0.000000001308 -0.000000013000 0.000000027705 -0.000000017438 0.000000012484 -0.000000000606 0.000000001915 0.000000002917 -0.000000005219 0.000000001351 -0.000000000606 0.000000014067 -0.000000009196 0.000000000300 -0.000000000458 -0.000000001308 0.000000001915 -0.000000009196 0.000000278309 -0.000000015861 0.000000006592 -0.000000013000 0.000000002917 0.000000000300 -0.000000015861 0.000000260164 -0.000000002000 0.000000027705 -0.000000005219 -0.000000000458 0.000000006592 -0.000000002000 0.000000240578 +1 1645005907.703968524933 70 71 0.001635189256 -0.007661342710 -0.000611987754 0.000195863477 0.000220085415 0.000455212442 0.000000081616 -0.000000018000 0.000000001433 -0.000000001641 -0.000000013192 0.000000027466 -0.000000018000 0.000000012653 -0.000000000591 0.000000001940 0.000000003029 -0.000000005006 0.000000001433 -0.000000000591 0.000000014157 -0.000000009101 0.000000000295 -0.000000000473 -0.000000001641 0.000000001940 -0.000000009101 0.000000278289 -0.000000015793 0.000000006721 -0.000000013192 0.000000003029 0.000000000295 -0.000000015793 0.000000260033 -0.000000002174 0.000000027466 -0.000000005006 -0.000000000473 0.000000006721 -0.000000002174 0.000000241348 +1 1645005907.804278135300 71 72 0.001549220998 -0.007393823942 0.001064792531 -0.000061245968 0.000265809889 -0.000238367926 0.000000084430 -0.000000018916 0.000000001290 -0.000000001506 -0.000000013499 0.000000026763 -0.000000018916 0.000000013096 -0.000000000595 0.000000001859 0.000000003095 -0.000000004652 0.000000001290 -0.000000000595 0.000000014779 -0.000000009309 0.000000000116 -0.000000000490 -0.000000001506 0.000000001859 -0.000000009309 0.000000280877 -0.000000016432 0.000000006827 -0.000000013499 0.000000003095 0.000000000116 -0.000000016432 0.000000260406 -0.000000002092 0.000000026763 -0.000000004652 -0.000000000490 0.000000006827 -0.000000002092 0.000000242565 +1 1645005907.903728008270 72 73 0.002219839781 -0.006729865633 0.001166665107 0.000120456909 0.000039397030 -0.001195289279 0.000000086968 -0.000000019811 0.000000001166 -0.000000001441 -0.000000013646 0.000000025791 -0.000000019811 0.000000013556 -0.000000000638 0.000000001888 0.000000003216 -0.000000004211 0.000000001166 -0.000000000638 0.000000015013 -0.000000009450 -0.000000000298 -0.000000000604 -0.000000001441 0.000000001888 -0.000000009450 0.000000282683 -0.000000017245 0.000000006808 -0.000000013646 0.000000003216 -0.000000000298 -0.000000017245 0.000000258711 -0.000000001579 0.000000025791 -0.000000004211 -0.000000000604 0.000000006808 -0.000000001579 0.000000241863 +1 1645005908.003634452820 73 74 0.003125083194 -0.006057082466 0.000100060086 -0.000042155704 -0.000130882188 -0.000945982049 0.000000088087 -0.000000020274 0.000000001242 -0.000000001785 -0.000000013450 0.000000024803 -0.000000020274 0.000000013671 -0.000000000680 0.000000002063 0.000000003299 -0.000000004076 0.000000001242 -0.000000000680 0.000000014959 -0.000000009260 -0.000000000646 -0.000000000747 -0.000000001785 0.000000002063 -0.000000009260 0.000000284481 -0.000000018051 0.000000007211 -0.000000013450 0.000000003299 -0.000000000646 -0.000000018051 0.000000257869 -0.000000000830 0.000000024803 -0.000000004076 -0.000000000747 0.000000007211 -0.000000000830 0.000000240461 +1 1645005908.103469848633 74 75 0.005239355570 -0.006690197943 -0.001346041897 0.000002110025 0.000383511679 0.000870726890 0.000000088096 -0.000000020346 0.000000001268 -0.000000002250 -0.000000013524 0.000000023906 -0.000000020346 0.000000013591 -0.000000000702 0.000000002211 0.000000003331 -0.000000003807 0.000000001268 -0.000000000702 0.000000014950 -0.000000008801 -0.000000000587 -0.000000000765 -0.000000002250 0.000000002211 -0.000000008801 0.000000284454 -0.000000018287 0.000000007836 -0.000000013524 0.000000003331 -0.000000000587 -0.000000018287 0.000000257037 -0.000000000613 0.000000023906 -0.000000003807 -0.000000000765 0.000000007836 -0.000000000613 0.000000238618 +1 1645005908.202678442001 75 76 0.006817804937 -0.009726863760 -0.000518636839 -0.000056178504 -0.000656898114 0.002883550854 0.000000088802 -0.000000020528 0.000000001285 -0.000000002483 -0.000000014210 0.000000023286 -0.000000020528 0.000000013726 -0.000000000706 0.000000002292 0.000000003404 -0.000000003359 0.000000001285 -0.000000000706 0.000000014784 -0.000000008320 -0.000000000291 -0.000000000649 -0.000000002483 0.000000002292 -0.000000008320 0.000000283431 -0.000000018165 0.000000008157 -0.000000014210 0.000000003404 -0.000000000291 -0.000000018165 0.000000255778 -0.000000000733 0.000000023286 -0.000000003359 -0.000000000649 0.000000008157 -0.000000000733 0.000000239003 +1 1645005908.303349256516 76 77 0.008880185968 -0.011072420154 0.002005117294 0.000364039331 0.000621317606 0.001463440780 0.000000089528 -0.000000020704 0.000000001309 -0.000000002811 -0.000000014365 0.000000022726 -0.000000020704 0.000000013849 -0.000000000704 0.000000002410 0.000000003408 -0.000000003006 0.000000001309 -0.000000000704 0.000000014662 -0.000000008440 -0.000000000182 -0.000000000582 -0.000000002811 0.000000002410 -0.000000008440 0.000000282898 -0.000000018035 0.000000008233 -0.000000014365 0.000000003408 -0.000000000182 -0.000000018035 0.000000254217 -0.000000000493 0.000000022726 -0.000000003006 -0.000000000582 0.000000008233 -0.000000000493 0.000000239589 +1 1645005908.403426647186 77 78 0.013214057998 -0.010022380808 0.003548089026 -0.000420898660 -0.000687846562 0.004288662412 0.000000090426 -0.000000021194 0.000000001135 -0.000000003300 -0.000000014313 0.000000022653 -0.000000021194 0.000000013961 -0.000000000638 0.000000002484 0.000000003448 -0.000000002827 0.000000001135 -0.000000000638 0.000000014468 -0.000000009028 -0.000000000586 -0.000000000561 -0.000000003300 0.000000002484 -0.000000009028 0.000000283166 -0.000000017970 0.000000008066 -0.000000014313 0.000000003448 -0.000000000586 -0.000000017970 0.000000253551 -0.000000000440 0.000000022653 -0.000000002827 -0.000000000561 0.000000008066 -0.000000000440 0.000000240482 +1 1645005908.504360437393 78 79 0.020180381727 -0.009462892108 0.001955139025 0.000042094269 0.000191650148 0.004876347524 0.000000091477 -0.000000021398 0.000000001260 -0.000000003754 -0.000000014396 0.000000022564 -0.000000021398 0.000000014143 -0.000000000654 0.000000002576 0.000000003359 -0.000000002684 0.000000001260 -0.000000000654 0.000000014202 -0.000000008908 -0.000000001365 -0.000000000321 -0.000000003754 0.000000002576 -0.000000008908 0.000000283742 -0.000000018036 0.000000007708 -0.000000014396 0.000000003359 -0.000000001365 -0.000000018036 0.000000252709 -0.000000000672 0.000000022564 -0.000000002684 -0.000000000321 0.000000007708 -0.000000000672 0.000000242092 +1 1645005908.602572441101 79 80 0.027583841746 -0.007492189755 -0.000358945670 -0.000316065525 0.000410807340 0.004627049462 0.000000091890 -0.000000021219 0.000000001348 -0.000000004184 -0.000000014661 0.000000022474 -0.000000021219 0.000000014196 -0.000000000667 0.000000002818 0.000000003383 -0.000000002589 0.000000001348 -0.000000000667 0.000000014612 -0.000000008669 -0.000000001276 -0.000000000350 -0.000000004184 0.000000002818 -0.000000008669 0.000000284516 -0.000000017822 0.000000007904 -0.000000014661 0.000000003383 -0.000000001276 -0.000000017822 0.000000252238 -0.000000001096 0.000000022474 -0.000000002589 -0.000000000350 0.000000007904 -0.000000001096 0.000000242589 +1 1645005908.702723264694 80 81 0.037527173709 -0.002432558917 0.000584410457 -0.000295924796 0.000172057833 0.004510494708 0.000000092503 -0.000000021428 0.000000001442 -0.000000004439 -0.000000015161 0.000000022444 -0.000000021428 0.000000014211 -0.000000000702 0.000000002984 0.000000003573 -0.000000002087 0.000000001442 -0.000000000702 0.000000014735 -0.000000008471 -0.000000001226 -0.000000000479 -0.000000004439 0.000000002984 -0.000000008471 0.000000283202 -0.000000016932 0.000000008633 -0.000000015161 0.000000003573 -0.000000001226 -0.000000016932 0.000000250551 -0.000000001604 0.000000022444 -0.000000002087 -0.000000000479 0.000000008633 -0.000000001604 0.000000241814 +1 1645005908.805076837540 81 82 0.044756539731 -0.000661973435 0.002178494075 -0.000609676361 0.000455606665 0.001515229361 0.000000093517 -0.000000021845 0.000000001449 -0.000000004558 -0.000000015501 0.000000022061 -0.000000021845 0.000000014347 -0.000000000736 0.000000003042 0.000000003685 -0.000000001671 0.000000001449 -0.000000000736 0.000000014651 -0.000000008202 -0.000000002017 -0.000000000480 -0.000000004558 0.000000003042 -0.000000008202 0.000000281630 -0.000000016156 0.000000009346 -0.000000015501 0.000000003685 -0.000000002017 -0.000000016156 0.000000248540 -0.000000001725 0.000000022061 -0.000000001671 -0.000000000480 0.000000009346 -0.000000001725 0.000000241443 +1 1645005908.902604103088 82 83 0.045146081256 -0.002522614562 0.003315811413 0.000215551457 0.001434294478 -0.002791558929 0.000000095523 -0.000000022658 0.000000001480 -0.000000004967 -0.000000015958 0.000000021670 -0.000000022658 0.000000014741 -0.000000000778 0.000000003208 0.000000003828 -0.000000001330 0.000000001480 -0.000000000778 0.000000014730 -0.000000007858 -0.000000002516 -0.000000000414 -0.000000004967 0.000000003208 -0.000000007858 0.000000281929 -0.000000016137 0.000000009579 -0.000000015958 0.000000003828 -0.000000002516 -0.000000016137 0.000000249394 -0.000000001587 0.000000021670 -0.000000001330 -0.000000000414 0.000000009579 -0.000000001587 0.000000243425 +1 1645005909.003700494766 83 84 0.050021001197 -0.004220963579 0.004624075743 0.000077137405 0.000861877284 -0.003931105805 0.000000097156 -0.000000023529 0.000000001589 -0.000000005276 -0.000000016382 0.000000021184 -0.000000023529 0.000000015119 -0.000000000815 0.000000003220 0.000000004081 -0.000000000901 0.000000001589 -0.000000000815 0.000000014639 -0.000000007417 -0.000000002865 -0.000000000427 -0.000000005276 0.000000003220 -0.000000007417 0.000000278551 -0.000000015767 0.000000008985 -0.000000016382 0.000000004081 -0.000000002865 -0.000000015767 0.000000248181 -0.000000001156 0.000000021184 -0.000000000901 -0.000000000427 0.000000008985 -0.000000001156 0.000000244236 +1 1645005909.105985641479 84 85 0.055306161593 -0.005644162807 0.003857391803 0.000176010803 -0.000406336505 -0.002605005858 0.000000098538 -0.000000024037 0.000000001724 -0.000000005135 -0.000000016925 0.000000020936 -0.000000024037 0.000000015386 -0.000000000857 0.000000002936 0.000000004319 -0.000000000573 0.000000001724 -0.000000000857 0.000000014932 -0.000000007133 -0.000000003111 -0.000000000413 -0.000000005135 0.000000002936 -0.000000007133 0.000000274491 -0.000000015304 0.000000008087 -0.000000016925 0.000000004319 -0.000000003111 -0.000000015304 0.000000247261 -0.000000000996 0.000000020936 -0.000000000573 -0.000000000413 0.000000008087 -0.000000000996 0.000000244577 +1 1645005909.204295873642 85 86 0.053724760856 -0.007867742945 0.002362917646 0.000765560740 0.000896643338 -0.004875595418 0.000000100571 -0.000000024471 0.000000001820 -0.000000004997 -0.000000017493 0.000000020789 -0.000000024471 0.000000015583 -0.000000000907 0.000000002811 0.000000004425 -0.000000000103 0.000000001820 -0.000000000907 0.000000014997 -0.000000007018 -0.000000003853 -0.000000000322 -0.000000004997 0.000000002811 -0.000000007018 0.000000274574 -0.000000014663 0.000000007432 -0.000000017493 0.000000004425 -0.000000003853 -0.000000014663 0.000000250668 -0.000000000903 0.000000020789 -0.000000000103 -0.000000000322 0.000000007432 -0.000000000903 0.000000246442 +1 1645005909.301892518997 86 87 0.052598742656 -0.009586402032 -0.001683730482 0.000100902220 0.000694494544 -0.005642354434 0.000000101572 -0.000000024847 0.000000001621 -0.000000004490 -0.000000017492 0.000000020848 -0.000000024847 0.000000015608 -0.000000000748 0.000000002623 0.000000004392 0.000000000187 0.000000001621 -0.000000000748 0.000000014697 -0.000000007106 -0.000000005019 -0.000000000290 -0.000000004490 0.000000002623 -0.000000007106 0.000000274211 -0.000000014372 0.000000006733 -0.000000017492 0.000000004392 -0.000000005019 -0.000000014372 0.000000253261 -0.000000001008 0.000000020848 0.000000000187 -0.000000000290 0.000000006733 -0.000000001008 0.000000245747 +1 1645005909.403104543686 87 88 0.056969463690 -0.013394491540 -0.004907688527 0.000313438663 -0.000019403912 -0.004039402462 0.000000101997 -0.000000025070 0.000000001444 -0.000000003922 -0.000000017659 0.000000021025 -0.000000025070 0.000000015563 -0.000000000684 0.000000002332 0.000000004455 0.000000000663 0.000000001444 -0.000000000684 0.000000015000 -0.000000007663 -0.000000005985 -0.000000000282 -0.000000003922 0.000000002332 -0.000000007663 0.000000273327 -0.000000013785 0.000000006215 -0.000000017659 0.000000004455 -0.000000005985 -0.000000013785 0.000000254655 -0.000000001345 0.000000021025 0.000000000663 -0.000000000282 0.000000006215 -0.000000001345 0.000000243873 +1 1645005909.501364469528 88 89 0.059762523379 -0.018567501801 -0.005975427554 0.000327232703 -0.000227548183 -0.003605858449 0.000000103041 -0.000000025390 0.000000001364 -0.000000003806 -0.000000017859 0.000000021654 -0.000000025390 0.000000015487 -0.000000000758 0.000000002227 0.000000004664 0.000000001355 0.000000001364 -0.000000000758 0.000000015660 -0.000000008246 -0.000000006861 -0.000000000445 -0.000000003806 0.000000002227 -0.000000008246 0.000000275936 -0.000000013446 0.000000006125 -0.000000017859 0.000000004664 -0.000000006861 -0.000000013446 0.000000258726 -0.000000001189 0.000000021654 0.000000001355 -0.000000000445 0.000000006125 -0.000000001189 0.000000245723 +1 1645005909.601603746414 89 90 0.063283735135 -0.025948706002 -0.004113490990 0.000699255821 0.000279829928 -0.004494526640 0.000000104093 -0.000000025791 0.000000001237 -0.000000003691 -0.000000017610 0.000000022878 -0.000000025791 0.000000015579 -0.000000000780 0.000000002254 0.000000004734 0.000000001701 0.000000001237 -0.000000000780 0.000000016004 -0.000000008598 -0.000000007978 -0.000000000633 -0.000000003691 0.000000002254 -0.000000008598 0.000000279345 -0.000000013365 0.000000006383 -0.000000017610 0.000000004734 -0.000000007978 -0.000000013365 0.000000262863 -0.000000000672 0.000000022878 0.000000001701 -0.000000000633 0.000000006383 -0.000000000672 0.000000249192 +1 1645005909.706259727478 90 91 0.064392495618 -0.030468368848 0.003371720855 0.000963763803 0.001428163796 -0.005769357514 0.000000105109 -0.000000026290 0.000000001203 -0.000000003464 -0.000000017388 0.000000024107 -0.000000026290 0.000000015676 -0.000000000845 0.000000002355 0.000000004759 0.000000001923 0.000000001203 -0.000000000845 0.000000016121 -0.000000008819 -0.000000009190 -0.000000000794 -0.000000003464 0.000000002355 -0.000000008819 0.000000281196 -0.000000012993 0.000000007076 -0.000000017388 0.000000004759 -0.000000009190 -0.000000012993 0.000000265075 -0.000000000134 0.000000024107 0.000000001923 -0.000000000794 0.000000007076 -0.000000000134 0.000000251988 +1 1645005909.802580833435 91 92 0.062365238940 -0.025844183621 0.009407834302 0.000193492681 -0.000172715186 -0.001464084852 0.000000108795 -0.000000027506 0.000000001224 -0.000000003570 -0.000000017752 0.000000025071 -0.000000027506 0.000000016130 -0.000000000716 0.000000002314 0.000000004730 0.000000002329 0.000000001224 -0.000000000716 0.000000015944 -0.000000008872 -0.000000010128 -0.000000000541 -0.000000003570 0.000000002314 -0.000000008872 0.000000285835 -0.000000013072 0.000000007112 -0.000000017752 0.000000004730 -0.000000010128 -0.000000013072 0.000000269332 -0.000000000648 0.000000025071 0.000000002329 -0.000000000541 0.000000007112 -0.000000000648 0.000000257045 +1 1645005909.903105735779 92 93 0.068784629133 -0.022584999076 0.007314965158 -0.000173602250 -0.000981733227 0.004132128312 0.000000111499 -0.000000028105 0.000000001268 -0.000000003943 -0.000000017982 0.000000024931 -0.000000028105 0.000000016532 -0.000000000666 0.000000002326 0.000000004775 0.000000003407 0.000000001268 -0.000000000666 0.000000015946 -0.000000009222 -0.000000011103 -0.000000000468 -0.000000003943 0.000000002326 -0.000000009222 0.000000288991 -0.000000012266 0.000000007214 -0.000000017982 0.000000004775 -0.000000011103 -0.000000012266 0.000000270553 -0.000000000807 0.000000024931 0.000000003407 -0.000000000468 0.000000007214 -0.000000000807 0.000000260827 +1 1645005910.002853631973 93 94 0.064100403759 -0.021302988329 0.003328486309 -0.000115742040 0.000269762022 0.004338534235 0.000000113187 -0.000000028301 0.000000001447 -0.000000004451 -0.000000017932 0.000000024371 -0.000000028301 0.000000016578 -0.000000000743 0.000000002509 0.000000004720 0.000000004613 0.000000001447 -0.000000000743 0.000000015755 -0.000000009213 -0.000000011391 -0.000000000441 -0.000000004451 0.000000002509 -0.000000009213 0.000000291379 -0.000000012472 0.000000007953 -0.000000017932 0.000000004720 -0.000000011391 -0.000000012472 0.000000268086 -0.000000000754 0.000000024371 0.000000004613 -0.000000000441 0.000000007953 -0.000000000754 0.000000263152 +1 1645005910.102889776230 94 95 0.058187415660 -0.021959187640 -0.002395190107 -0.000230382660 0.000479950528 0.004722778418 0.000000114411 -0.000000028797 0.000000001597 -0.000000005011 -0.000000018281 0.000000023886 -0.000000028797 0.000000016614 -0.000000000790 0.000000002676 0.000000004786 0.000000005410 0.000000001597 -0.000000000790 0.000000015406 -0.000000009290 -0.000000011420 -0.000000000347 -0.000000005011 0.000000002676 -0.000000009290 0.000000295984 -0.000000013121 0.000000008785 -0.000000018281 0.000000004786 -0.000000011420 -0.000000013121 0.000000268118 -0.000000001232 0.000000023886 0.000000005410 -0.000000000347 0.000000008785 -0.000000001232 0.000000266828 +1 1645005910.203362464905 95 96 0.056619890717 -0.019994351111 -0.005006673376 -0.000078075128 -0.001163126359 0.005855695866 0.000000113848 -0.000000028816 0.000000001425 -0.000000004988 -0.000000018908 0.000000023457 -0.000000028816 0.000000016671 -0.000000000790 0.000000002758 0.000000005082 0.000000006329 0.000000001425 -0.000000000790 0.000000015387 -0.000000009472 -0.000000012713 -0.000000000564 -0.000000004988 0.000000002758 -0.000000009472 0.000000298099 -0.000000012668 0.000000009794 -0.000000018908 0.000000005082 -0.000000012713 -0.000000012668 0.000000271956 -0.000000001334 0.000000023457 0.000000006329 -0.000000000564 0.000000009794 -0.000000001334 0.000000269624 +1 1645005910.304825067520 96 97 0.061086566852 -0.020286198113 -0.004843598885 0.000071324355 -0.000601247565 0.004021009982 0.000000117880 -0.000000029623 0.000000001330 -0.000000004609 -0.000000019305 0.000000024344 -0.000000029623 0.000000017038 -0.000000000772 0.000000002859 0.000000005198 0.000000006765 0.000000001330 -0.000000000772 0.000000015981 -0.000000010001 -0.000000014626 -0.000000000763 -0.000000004609 0.000000002859 -0.000000010001 0.000000301426 -0.000000011273 0.000000010740 -0.000000019305 0.000000005198 -0.000000014626 -0.000000011273 0.000000276684 -0.000000001301 0.000000024344 0.000000006765 -0.000000000763 0.000000010740 -0.000000001301 0.000000271470 +1 1645005910.406254529953 97 98 0.068850355530 -0.020207965588 -0.000165344613 -0.000090050301 -0.000186271132 0.000092130259 0.000000120621 -0.000000030569 0.000000001323 -0.000000004733 -0.000000019353 0.000000024043 -0.000000030569 0.000000017239 -0.000000000763 0.000000002867 0.000000005258 0.000000007204 0.000000001323 -0.000000000763 0.000000016177 -0.000000010171 -0.000000016172 -0.000000000720 -0.000000004733 0.000000002867 -0.000000010171 0.000000306276 -0.000000010789 0.000000010460 -0.000000019353 0.000000005258 -0.000000016172 -0.000000010789 0.000000281858 -0.000000001403 0.000000024043 0.000000007204 -0.000000000720 0.000000010460 -0.000000001403 0.000000273593 +1 1645005910.503567218781 98 99 0.074162405982 -0.018493810987 0.006239807671 -0.000159910511 0.000247959785 -0.004015256391 0.000000119575 -0.000000030905 0.000000001301 -0.000000005274 -0.000000019174 0.000000021991 -0.000000030905 0.000000017327 -0.000000000670 0.000000002923 0.000000005171 0.000000008213 0.000000001301 -0.000000000670 0.000000015751 -0.000000009557 -0.000000017503 -0.000000000420 -0.000000005274 0.000000002923 -0.000000009557 0.000000310028 -0.000000012146 0.000000010051 -0.000000019174 0.000000005171 -0.000000017503 -0.000000012146 0.000000288635 -0.000000001805 0.000000021991 0.000000008213 -0.000000000420 0.000000010051 -0.000000001805 0.000000275801 +1 1645005910.606616973877 99 100 0.080910102851 -0.020164208266 0.008661834846 0.000071976494 0.000898942665 -0.008152706685 0.000000119264 -0.000000031192 0.000000001273 -0.000000006002 -0.000000018615 0.000000020354 -0.000000031192 0.000000017400 -0.000000000693 0.000000003292 0.000000005053 0.000000009012 0.000000001273 -0.000000000693 0.000000015378 -0.000000009114 -0.000000018448 -0.000000000406 -0.000000006002 0.000000003292 -0.000000009114 0.000000310497 -0.000000014070 0.000000010804 -0.000000018615 0.000000005053 -0.000000018448 -0.000000014070 0.000000292764 -0.000000001847 0.000000020354 0.000000009012 -0.000000000406 0.000000010804 -0.000000001847 0.000000274724 +1 1645005910.704123735428 100 101 0.075961027781 -0.021808661780 0.005934638148 0.000450549674 0.001441942574 -0.012050260842 0.000000119262 -0.000000031760 0.000000001275 -0.000000006676 -0.000000018333 0.000000018441 -0.000000031760 0.000000017569 -0.000000000766 0.000000003498 0.000000005201 0.000000009821 0.000000001275 -0.000000000766 0.000000015022 -0.000000009165 -0.000000018987 -0.000000000583 -0.000000006676 0.000000003498 -0.000000009165 0.000000310167 -0.000000014428 0.000000011047 -0.000000018333 0.000000005201 -0.000000018987 -0.000000014428 0.000000295633 -0.000000001178 0.000000018441 0.000000009821 -0.000000000583 0.000000011047 -0.000000001178 0.000000274189 +1 1645005910.803392648697 101 102 0.078482673806 -0.023461910881 0.002857169426 0.000627784303 0.000447074489 -0.011957121987 0.000000119092 -0.000000032121 0.000000001304 -0.000000007124 -0.000000018655 0.000000017333 -0.000000032121 0.000000017789 -0.000000000790 0.000000003564 0.000000005412 0.000000010492 0.000000001304 -0.000000000790 0.000000015124 -0.000000009379 -0.000000019322 -0.000000000660 -0.000000007124 0.000000003564 -0.000000009379 0.000000308153 -0.000000013438 0.000000010595 -0.000000018655 0.000000005412 -0.000000019322 -0.000000013438 0.000000297079 -0.000000000659 0.000000017333 0.000000010492 -0.000000000660 0.000000010595 -0.000000000659 0.000000273340 +1 1645005910.902909994125 102 103 0.083189939682 -0.023490784963 -0.003117914173 0.000480762492 0.000068118626 -0.007186591478 0.000000118524 -0.000000032013 0.000000001297 -0.000000007106 -0.000000018773 0.000000016705 -0.000000032013 0.000000017971 -0.000000000795 0.000000003426 0.000000005515 0.000000011670 0.000000001297 -0.000000000795 0.000000015444 -0.000000009493 -0.000000020369 -0.000000000697 -0.000000007106 0.000000003426 -0.000000009493 0.000000306218 -0.000000012861 0.000000010051 -0.000000018773 0.000000005515 -0.000000020369 -0.000000012861 0.000000299271 -0.000000000380 0.000000016705 0.000000011670 -0.000000000697 0.000000010051 -0.000000000380 0.000000272341 +1 1645005911.004977464676 103 104 0.087315384998 -0.025157189183 -0.000393238317 0.000391193097 0.000467548951 -0.002126631756 0.000000118945 -0.000000031979 0.000000001227 -0.000000007043 -0.000000019158 0.000000016413 -0.000000031979 0.000000018203 -0.000000000668 0.000000003217 0.000000005547 0.000000013380 0.000000001227 -0.000000000668 0.000000015676 -0.000000009973 -0.000000022149 -0.000000000580 -0.000000007043 0.000000003217 -0.000000009973 0.000000309925 -0.000000012331 0.000000009781 -0.000000019158 0.000000005547 -0.000000022149 -0.000000012331 0.000000306142 -0.000000000590 0.000000016413 0.000000013380 -0.000000000580 0.000000009781 -0.000000000590 0.000000276867 +1 1645005911.101893663406 104 105 0.084323245144 -0.023813002632 0.014804471985 0.000199032956 0.000054108090 -0.000198912619 0.000000120217 -0.000000032143 0.000000001107 -0.000000007075 -0.000000019378 0.000000016191 -0.000000032143 0.000000018424 -0.000000000637 0.000000003358 0.000000005540 0.000000015177 0.000000001107 -0.000000000637 0.000000015707 -0.000000010434 -0.000000024174 -0.000000000604 -0.000000007075 0.000000003358 -0.000000010434 0.000000316238 -0.000000012013 0.000000010568 -0.000000019378 0.000000005540 -0.000000024174 -0.000000012013 0.000000316136 -0.000000000812 0.000000016191 0.000000015177 -0.000000000604 0.000000010568 -0.000000000812 0.000000285489 +1 1645005911.202757835388 105 106 0.091890144553 -0.022457558791 0.017747265772 -0.000060833543 -0.000571579936 0.006619650214 0.000000121531 -0.000000032560 0.000000000738 -0.000000006643 -0.000000017693 0.000000015251 -0.000000032560 0.000000018747 -0.000000000671 0.000000003314 0.000000005222 0.000000017170 0.000000000738 -0.000000000671 0.000000016021 -0.000000011220 -0.000000026178 -0.000000000837 -0.000000006643 0.000000003314 -0.000000011220 0.000000321155 -0.000000010921 0.000000011729 -0.000000017693 0.000000005222 -0.000000026178 -0.000000010921 0.000000322863 -0.000000000229 0.000000015251 0.000000017170 -0.000000000837 0.000000011729 -0.000000000229 0.000000293530 +1 1645005911.304598569870 106 107 0.099315592633 -0.013707866919 0.008929200938 -0.000593510612 0.000431991390 0.006542184812 0.000000122613 -0.000000032815 0.000000000312 -0.000000006490 -0.000000015355 0.000000013258 -0.000000032815 0.000000018837 -0.000000000526 0.000000003118 0.000000004545 0.000000018930 0.000000000312 -0.000000000526 0.000000016756 -0.000000012375 -0.000000028786 -0.000000000787 -0.000000006490 0.000000003118 -0.000000012375 0.000000326678 -0.000000008324 0.000000013042 -0.000000015355 0.000000004545 -0.000000028786 -0.000000008324 0.000000328892 0.000000000490 0.000000013258 0.000000018930 -0.000000000787 0.000000013042 0.000000000490 0.000000298943 +1 1645005911.402243375778 107 108 0.104785384813 -0.008105171994 -0.002214641892 -0.000860946379 -0.000993392829 0.010948470463 0.000000123089 -0.000000033137 0.000000000099 -0.000000006942 -0.000000014277 0.000000011341 -0.000000033137 0.000000019181 -0.000000000321 0.000000003202 0.000000004018 0.000000020680 0.000000000099 -0.000000000321 0.000000016999 -0.000000012404 -0.000000030745 -0.000000000434 -0.000000006942 0.000000003202 -0.000000012404 0.000000331673 -0.000000007654 0.000000013557 -0.000000014277 0.000000004018 -0.000000030745 -0.000000007654 0.000000336509 0.000000000255 0.000000011341 0.000000020680 -0.000000000434 0.000000013557 0.000000000255 0.000000304958 +1 1645005911.504294157028 108 109 0.115689898818 -0.005982363005 -0.006109069161 -0.000726627622 0.000973576912 0.010908783105 0.000000124858 -0.000000033368 0.000000000274 -0.000000007206 -0.000000014118 0.000000010002 -0.000000033368 0.000000019303 -0.000000000398 0.000000003340 0.000000003987 0.000000022318 0.000000000274 -0.000000000398 0.000000016946 -0.000000012012 -0.000000031995 -0.000000000446 -0.000000007206 0.000000003340 -0.000000012012 0.000000330527 -0.000000006532 0.000000013696 -0.000000014118 0.000000003987 -0.000000031995 -0.000000006532 0.000000339820 0.000000000103 0.000000010002 0.000000022318 -0.000000000446 0.000000013696 0.000000000103 0.000000309344 +1 1645005911.603650331497 109 110 0.112144073030 -0.007022975029 0.001012219037 -0.001163101342 0.000302771644 0.009647953042 0.000000124674 -0.000000033654 0.000000000385 -0.000000008009 -0.000000015177 0.000000007700 -0.000000033654 0.000000019363 -0.000000000509 0.000000003826 0.000000004570 0.000000023824 0.000000000385 -0.000000000509 0.000000016545 -0.000000011791 -0.000000032496 -0.000000000820 -0.000000008009 0.000000003826 -0.000000011791 0.000000328802 -0.000000004861 0.000000014409 -0.000000015177 0.000000004570 -0.000000032496 -0.000000004861 0.000000338575 0.000000000638 0.000000007700 0.000000023824 -0.000000000820 0.000000014409 0.000000000638 0.000000314003 +1 1645005911.705323696136 110 111 0.108334202057 -0.006199322902 0.015439608211 -0.000499640517 0.000622717034 0.005115544113 0.000000121420 -0.000000033136 0.000000000547 -0.000000008867 -0.000000016030 0.000000005761 -0.000000033136 0.000000019118 -0.000000000490 0.000000003998 0.000000004882 0.000000025082 0.000000000547 -0.000000000490 0.000000015937 -0.000000011322 -0.000000032770 -0.000000000779 -0.000000008867 0.000000003998 -0.000000011322 0.000000326263 -0.000000003896 0.000000014707 -0.000000016030 0.000000004882 -0.000000032770 -0.000000003896 0.000000334604 0.000000000414 0.000000005761 0.000000025082 -0.000000000779 0.000000014707 0.000000000414 0.000000319533 +1 1645005911.805619001389 111 112 0.105563859350 -0.007445366800 0.017090699054 -0.000396553302 -0.000236939023 0.001638354551 0.000000119495 -0.000000032859 0.000000000407 -0.000000008551 -0.000000014394 0.000000004296 -0.000000032859 0.000000019006 -0.000000000444 0.000000003710 0.000000004531 0.000000026291 0.000000000407 -0.000000000444 0.000000015977 -0.000000011729 -0.000000035072 -0.000000000757 -0.000000008551 0.000000003710 -0.000000011729 0.000000324713 -0.000000002118 0.000000014718 -0.000000014394 0.000000004531 -0.000000035072 -0.000000002118 0.000000339187 0.000000000583 0.000000004296 0.000000026291 -0.000000000757 0.000000014718 0.000000000583 0.000000324775 +1 1645005911.905131578445 112 113 0.105669045682 -0.010232842109 0.007581705384 0.000117560962 -0.000845623252 -0.002154597809 0.000000117976 -0.000000032837 0.000000000325 -0.000000008707 -0.000000012159 0.000000002537 -0.000000032837 0.000000019001 -0.000000000403 0.000000003735 0.000000003954 0.000000027237 0.000000000325 -0.000000000403 0.000000015480 -0.000000011530 -0.000000035970 -0.000000000732 -0.000000008707 0.000000003735 -0.000000011530 0.000000324418 -0.000000003537 0.000000014543 -0.000000012159 0.000000003954 -0.000000035970 -0.000000003537 0.000000343391 0.000000000796 0.000000002537 0.000000027237 -0.000000000732 0.000000014543 0.000000000796 0.000000327307 +1 1645005912.005542993546 113 114 0.105408723509 -0.006257586189 -0.004647377317 -0.000008164249 -0.000462734466 -0.006034679601 0.000000115866 -0.000000032739 0.000000000255 -0.000000008904 -0.000000010564 0.000000001256 -0.000000032739 0.000000018798 -0.000000000420 0.000000003723 0.000000003731 0.000000027559 0.000000000255 -0.000000000420 0.000000015409 -0.000000012009 -0.000000036888 -0.000000000887 -0.000000008904 0.000000003723 -0.000000012009 0.000000321068 -0.000000000786 0.000000014100 -0.000000010564 0.000000003731 -0.000000036888 -0.000000000786 0.000000345395 0.000000001508 0.000000001256 0.000000027559 -0.000000000887 0.000000014100 0.000000001508 0.000000326200 +1 1645005912.104353666306 114 115 0.104312051462 -0.005788696135 -0.001086666677 0.000673600379 0.002056576062 -0.010721959912 0.000000113948 -0.000000032429 0.000000000239 -0.000000008558 -0.000000009973 0.000000000000 -0.000000032429 0.000000018879 -0.000000000450 0.000000003631 0.000000003684 0.000000029056 0.000000000239 -0.000000000450 0.000000015680 -0.000000012374 -0.000000038521 -0.000000001026 -0.000000008558 0.000000003631 -0.000000012374 0.000000316878 0.000000003031 0.000000013903 -0.000000009973 0.000000003684 -0.000000038521 0.000000003031 0.000000349891 0.000000002089 0.000000000000 0.000000029056 -0.000000001026 0.000000013903 0.000000002089 0.000000329833 +1 1645005912.204403162003 115 116 0.107279768307 -0.009811669098 0.013827848427 0.000095745570 -0.000565487976 -0.008974907831 0.000000111490 -0.000000032117 0.000000000243 -0.000000008340 -0.000000010631 -0.000000000159 -0.000000032117 0.000000018992 -0.000000000434 0.000000003780 0.000000003786 0.000000030191 0.000000000243 -0.000000000434 0.000000014981 -0.000000011345 -0.000000037764 -0.000000000975 -0.000000008340 0.000000003780 -0.000000011345 0.000000310920 0.000000002683 0.000000013634 -0.000000010631 0.000000003786 -0.000000037764 0.000000002683 0.000000348113 0.000000001943 -0.000000000159 0.000000030191 -0.000000000975 0.000000013634 0.000000001943 0.000000334016 +1 1645005912.304095268250 116 117 0.106183988090 -0.010713891373 0.022572940623 0.000391186142 0.000422801729 -0.011307080283 0.000000109508 -0.000000031579 0.000000000488 -0.000000007880 -0.000000010858 0.000000000029 -0.000000031579 0.000000018604 -0.000000000518 0.000000003456 0.000000003941 0.000000030323 0.000000000488 -0.000000000518 0.000000014084 -0.000000010751 -0.000000036936 -0.000000001088 -0.000000007880 0.000000003456 -0.000000010751 0.000000305213 0.000000003878 0.000000012732 -0.000000010858 0.000000003941 -0.000000036936 0.000000003878 0.000000348036 0.000000002303 0.000000000029 0.000000030323 -0.000000001088 0.000000012732 0.000000002303 0.000000336507 +1 1645005912.402587413788 117 118 0.103151905269 -0.007851188457 0.016492569166 -0.000133641646 -0.000389791053 -0.009425723025 0.000000107590 -0.000000031367 0.000000000521 -0.000000007632 -0.000000009840 -0.000000000231 -0.000000031367 0.000000018521 -0.000000000458 0.000000002965 0.000000003586 0.000000031051 0.000000000521 -0.000000000458 0.000000014103 -0.000000011073 -0.000000039130 -0.000000000784 -0.000000007632 0.000000002965 -0.000000011073 0.000000302435 0.000000007661 0.000000011183 -0.000000009840 0.000000003586 -0.000000039130 0.000000007661 0.000000360891 0.000000001946 -0.000000000231 0.000000031051 -0.000000000784 0.000000011183 0.000000001946 0.000000341818 +1 1645005912.504237890244 118 119 0.102245964023 -0.001922173039 0.001980514565 0.000009290998 0.000695611149 -0.010118021293 0.000000106167 -0.000000031034 0.000000000655 -0.000000007967 -0.000000009572 -0.000000000164 -0.000000031034 0.000000018711 -0.000000000505 0.000000002829 0.000000003587 0.000000033047 0.000000000655 -0.000000000505 0.000000014640 -0.000000011217 -0.000000042101 -0.000000000664 -0.000000007967 0.000000002829 -0.000000011217 0.000000298544 0.000000009862 0.000000010133 -0.000000009572 0.000000003587 -0.000000042101 0.000000009862 0.000000373640 0.000000002635 -0.000000000164 0.000000033047 -0.000000000664 0.000000010133 0.000000002635 0.000000351994 +1 1645005912.604142665863 119 120 0.095380530294 -0.001108012995 -0.003083408103 0.000564478649 0.001636924297 -0.014723225025 0.000000103907 -0.000000030366 0.000000000769 -0.000000008451 -0.000000010034 -0.000000001312 -0.000000030366 0.000000018725 -0.000000000550 0.000000003003 0.000000003747 0.000000034899 0.000000000769 -0.000000000550 0.000000014355 -0.000000010463 -0.000000042267 -0.000000000698 -0.000000008451 0.000000003003 -0.000000010463 0.000000293930 0.000000008522 0.000000009804 -0.000000010034 0.000000003747 -0.000000042267 0.000000008522 0.000000375353 0.000000002938 -0.000000001312 0.000000034899 -0.000000000698 0.000000009804 0.000000002938 0.000000360377 +1 1645005912.702849626541 120 121 0.093286512015 -0.005930663651 0.003868319453 0.000605384071 0.000126378577 -0.016197853324 0.000000099769 -0.000000029448 0.000000000826 -0.000000008585 -0.000000009969 -0.000000002554 -0.000000029448 0.000000018277 -0.000000000493 0.000000003133 0.000000003425 0.000000035023 0.000000000826 -0.000000000493 0.000000013658 -0.000000009979 -0.000000041204 -0.000000000428 -0.000000008585 0.000000003133 -0.000000009979 0.000000289021 0.000000008461 0.000000009199 -0.000000009969 0.000000003425 -0.000000041204 0.000000008461 0.000000371803 0.000000000932 -0.000000002554 0.000000035023 -0.000000000428 0.000000009199 0.000000000932 0.000000361967 +1 1645005912.802519559860 121 122 0.100987173190 0.000714157226 0.010878686943 0.000910503223 -0.000408491888 -0.016040457828 0.000000095734 -0.000000028309 0.000000000990 -0.000000008628 -0.000000009816 -0.000000001987 -0.000000028309 0.000000017686 -0.000000000553 0.000000003081 0.000000003373 0.000000034512 0.000000000990 -0.000000000553 0.000000013034 -0.000000009194 -0.000000040584 -0.000000000408 -0.000000008628 0.000000003081 -0.000000009194 0.000000282506 0.000000007004 0.000000008055 -0.000000009816 0.000000003373 -0.000000040584 0.000000007004 0.000000370148 0.000000000056 -0.000000001987 0.000000034512 -0.000000000408 0.000000008055 0.000000000056 0.000000360170 +1 1645005912.902120351791 122 123 0.105744414544 0.002989777360 0.008055909981 0.000522713016 -0.000712091928 -0.015860847116 0.000000092000 -0.000000026892 0.000000001096 -0.000000007960 -0.000000008934 -0.000000001199 -0.000000026892 0.000000017033 -0.000000000612 0.000000002687 0.000000003198 0.000000034106 0.000000001096 -0.000000000612 0.000000012601 -0.000000008599 -0.000000041253 -0.000000000537 -0.000000007960 0.000000002687 -0.000000008599 0.000000278439 0.000000006637 0.000000006944 -0.000000008934 0.000000003198 -0.000000041253 0.000000006637 0.000000377733 0.000000000207 -0.000000001199 0.000000034106 -0.000000000537 0.000000006944 0.000000000207 0.000000356190 +1 1645005913.005158424377 123 124 0.108371709292 0.001027107202 0.003562412218 0.000635368012 0.002115674008 -0.017401447292 0.000000088452 -0.000000025564 0.000000000949 -0.000000006689 -0.000000006146 -0.000000000882 -0.000000025564 0.000000016427 -0.000000000555 0.000000002086 0.000000002362 0.000000033970 0.000000000949 -0.000000000555 0.000000012646 -0.000000008906 -0.000000043562 -0.000000000533 -0.000000006689 0.000000002086 -0.000000008906 0.000000277482 0.000000008766 0.000000005869 -0.000000006146 0.000000002362 -0.000000043562 0.000000008766 0.000000394236 -0.000000000337 -0.000000000882 0.000000033970 -0.000000000533 0.000000005869 -0.000000000337 0.000000354101 +1 1645005913.103281021118 124 125 0.102758309871 -0.002935497442 -0.002834141245 0.000992244923 0.000603097177 -0.010505902649 0.000000084679 -0.000000024480 0.000000000694 -0.000000006203 -0.000000004434 -0.000000000786 -0.000000024480 0.000000015926 -0.000000000460 0.000000001960 0.000000001912 0.000000034312 0.000000000694 -0.000000000460 0.000000012635 -0.000000008930 -0.000000045449 -0.000000000614 -0.000000006203 0.000000001960 -0.000000008930 0.000000278203 0.000000010319 0.000000005903 -0.000000004434 0.000000001912 -0.000000045449 0.000000010319 0.000000412749 0.000000000610 -0.000000000786 0.000000034312 -0.000000000614 0.000000005903 0.000000000610 0.000000357488 +1 1645005913.203158855438 125 126 0.106915139328 -0.005362769587 0.005671755512 0.000982730101 0.001719758282 -0.007181197185 0.000000079753 -0.000000022565 0.000000000775 -0.000000006067 -0.000000004255 -0.000000000741 -0.000000022565 0.000000015327 -0.000000000457 0.000000001948 0.000000001834 0.000000036065 0.000000000775 -0.000000000457 0.000000012307 -0.000000008166 -0.000000046153 -0.000000000719 -0.000000006067 0.000000001948 -0.000000008166 0.000000275329 0.000000010591 0.000000006327 -0.000000004255 0.000000001834 -0.000000046153 0.000000010591 0.000000425655 0.000000001717 -0.000000000741 0.000000036065 -0.000000000719 0.000000006327 0.000000001717 0.000000367832 +1 1645005913.305967569351 126 127 0.114387020145 -0.004210180518 0.015020722820 -0.000184024553 -0.001805515269 0.000786641202 0.000000073336 -0.000000020355 0.000000000681 -0.000000005855 -0.000000003624 -0.000000000918 -0.000000020355 0.000000014826 -0.000000000406 0.000000001943 0.000000001386 0.000000038378 0.000000000681 -0.000000000406 0.000000012043 -0.000000007381 -0.000000046646 -0.000000000532 -0.000000005855 0.000000001943 -0.000000007381 0.000000270632 0.000000009755 0.000000006505 -0.000000003624 0.000000001386 -0.000000046646 0.000000009755 0.000000431777 0.000000000725 -0.000000000918 0.000000038378 -0.000000000532 0.000000006505 0.000000000725 0.000000380235 +1 1645005913.402428627014 127 128 0.113069559789 -0.000704930784 0.014288948611 -0.000164686473 0.000124000769 0.002486017239 0.000000067177 -0.000000018092 0.000000000770 -0.000000005230 -0.000000002337 -0.000000001447 -0.000000018092 0.000000014158 -0.000000000419 0.000000001621 0.000000000810 0.000000040400 0.000000000770 -0.000000000419 0.000000011887 -0.000000006732 -0.000000048215 -0.000000000372 -0.000000005230 0.000000001621 -0.000000006732 0.000000268588 0.000000010266 0.000000005978 -0.000000002337 0.000000000810 -0.000000048215 0.000000010266 0.000000444703 -0.000000000174 -0.000000001447 0.000000040400 -0.000000000372 0.000000005978 -0.000000000174 0.000000394429 +1 1645005913.504843950272 128 129 0.113183456503 -0.003233214799 0.001167230142 -0.000877923266 -0.000380382302 0.004114345669 0.000000058977 -0.000000015334 0.000000000576 -0.000000004492 -0.000000000609 -0.000000003143 -0.000000015334 0.000000013437 -0.000000000338 0.000000001252 0.000000000348 0.000000043093 0.000000000576 -0.000000000338 0.000000010341 -0.000000006120 -0.000000045149 -0.000000000437 -0.000000004492 0.000000001252 -0.000000006120 0.000000262475 0.000000013212 0.000000005183 -0.000000000609 0.000000000348 -0.000000045149 0.000000013212 0.000000443863 0.000000000910 -0.000000003143 0.000000043093 -0.000000000437 0.000000005183 0.000000000910 0.000000408565 +1 1645005913.601838827133 129 130 0.106480526708 -0.001386640502 -0.011980876123 0.000067753467 -0.000046515530 0.006676571512 0.000000048768 -0.000000012110 0.000000000322 -0.000000003692 -0.000000000074 -0.000000005515 -0.000000012110 0.000000012413 -0.000000000216 0.000000000924 0.000000000176 0.000000045221 0.000000000322 -0.000000000216 0.000000008513 -0.000000005462 -0.000000040526 -0.000000000491 -0.000000003692 0.000000000924 -0.000000005462 0.000000255291 0.000000015909 0.000000004681 -0.000000000074 0.000000000176 -0.000000040526 0.000000015909 0.000000437638 0.000000002802 -0.000000005515 0.000000045221 -0.000000000491 0.000000004681 0.000000002802 0.000000418105 +1 1645005913.703332424164 130 131 0.108468668180 -0.000969605556 -0.002385739928 0.000092510686 0.001524255417 0.001870569352 0.000000039925 -0.000000009382 0.000000000084 -0.000000002806 0.000000000135 -0.000000007330 -0.000000009382 0.000000011452 -0.000000000090 0.000000000564 -0.000000000086 0.000000046436 0.000000000084 -0.000000000090 0.000000007595 -0.000000004539 -0.000000038331 -0.000000000262 -0.000000002806 0.000000000564 -0.000000004539 0.000000246399 0.000000015747 0.000000003991 0.000000000135 -0.000000000086 -0.000000038331 0.000000015747 0.000000435816 0.000000003091 -0.000000007330 0.000000046436 -0.000000000262 0.000000003991 0.000000003091 0.000000424368 +1 1645005913.802533626556 131 132 0.104456210240 -0.004904373024 0.013892430815 -0.000421937248 -0.001963824939 0.001738357414 0.000000033848 -0.000000007783 0.000000000093 -0.000000002527 0.000000000077 -0.000000008221 -0.000000007783 0.000000010852 -0.000000000071 0.000000000554 -0.000000000234 0.000000046899 0.000000000093 -0.000000000071 0.000000007219 -0.000000004204 -0.000000037396 -0.000000000113 -0.000000002527 0.000000000554 -0.000000004204 0.000000236147 0.000000019026 0.000000004261 0.000000000077 -0.000000000234 -0.000000037396 0.000000019026 0.000000432716 0.000000002615 -0.000000008221 0.000000046899 -0.000000000113 0.000000004261 0.000000002615 0.000000427562 +1 1645005913.902822494507 132 133 0.105252268899 -0.005892393425 0.020783173943 -0.000244242694 -0.000148631160 -0.003208653521 0.000000028686 -0.000000006554 0.000000000243 -0.000000002229 -0.000000000013 -0.000000008804 -0.000000006554 0.000000010280 -0.000000000154 0.000000000313 -0.000000000124 0.000000046583 0.000000000243 -0.000000000154 0.000000007239 -0.000000004543 -0.000000037125 -0.000000000235 -0.000000002229 0.000000000313 -0.000000004543 0.000000229949 0.000000026748 0.000000003642 -0.000000000013 -0.000000000124 -0.000000037125 0.000000026748 0.000000428474 0.000000002848 -0.000000008804 0.000000046583 -0.000000000235 0.000000003642 0.000000002848 0.000000427422 +1 1645005914.004937648773 133 134 0.109519905526 -0.010991758570 0.010722232350 0.000333129949 -0.000174623972 -0.002993424422 0.000000024093 -0.000000005515 0.000000000350 -0.000000001671 -0.000000000218 -0.000000008888 -0.000000005515 0.000000009905 -0.000000000203 0.000000000110 -0.000000000103 0.000000046779 0.000000000350 -0.000000000203 0.000000006866 -0.000000005080 -0.000000035414 -0.000000000417 -0.000000001671 0.000000000110 -0.000000005080 0.000000225283 0.000000035405 0.000000003187 -0.000000000218 -0.000000000103 -0.000000035414 0.000000035405 0.000000419305 0.000000003321 -0.000000008888 0.000000046779 -0.000000000417 0.000000003187 0.000000003321 0.000000429289 +1 1645005914.104001283646 134 135 0.110141859157 -0.015877687071 -0.003919154582 0.001363151861 -0.000134739250 0.000119752527 0.000000020081 -0.000000004548 0.000000000494 -0.000000000564 -0.000000000812 -0.000000008086 -0.000000004548 0.000000009513 -0.000000000212 -0.000000000183 -0.000000000079 0.000000046971 0.000000000494 -0.000000000212 0.000000006414 -0.000000005263 -0.000000033246 -0.000000000660 -0.000000000564 -0.000000000183 -0.000000005263 0.000000222499 0.000000041553 0.000000002348 -0.000000000812 -0.000000000079 -0.000000033246 0.000000041553 0.000000404705 0.000000004584 -0.000000008086 0.000000046971 -0.000000000660 0.000000002348 0.000000004584 0.000000434501 +1 1645005914.205260753632 135 136 0.112731195464 -0.016646210746 0.002628785458 0.000209927570 0.000614736609 0.002584125950 0.000000016076 -0.000000003462 0.000000000384 0.000000000165 -0.000000000800 -0.000000006658 -0.000000003462 0.000000009059 -0.000000000133 -0.000000000606 -0.000000000101 0.000000046958 0.000000000384 -0.000000000133 0.000000005887 -0.000000005972 -0.000000030611 -0.000000000603 0.000000000165 -0.000000000606 -0.000000005972 0.000000219212 0.000000046082 -0.000000000162 -0.000000000800 -0.000000000101 -0.000000030611 0.000000046082 0.000000380793 0.000000005566 -0.000000006658 0.000000046958 -0.000000000603 -0.000000000162 0.000000005566 0.000000437954 +1 1645005914.304635286331 136 137 0.110109539783 -0.017998414611 0.012810929145 -0.000245315305 0.000890363266 0.003698770995 0.000000013318 -0.000000002600 0.000000000326 0.000000000152 -0.000000000681 -0.000000005410 -0.000000002600 0.000000008819 -0.000000000077 -0.000000000756 -0.000000000134 0.000000047689 0.000000000326 -0.000000000077 0.000000005647 -0.000000007102 -0.000000029962 -0.000000000471 0.000000000152 -0.000000000756 -0.000000007102 0.000000215325 0.000000049153 -0.000000002005 -0.000000000681 -0.000000000134 -0.000000029962 0.000000049153 0.000000369953 0.000000005821 -0.000000005410 0.000000047689 -0.000000000471 -0.000000002005 0.000000005821 0.000000443851 +1 1645005914.402868032455 137 138 0.111083721384 -0.015259298484 0.014103141251 -0.000356166718 -0.001745079695 0.006088809825 0.000000011488 -0.000000002155 0.000000000288 0.000000000087 -0.000000000634 -0.000000004446 -0.000000002155 0.000000008887 -0.000000000073 -0.000000000761 -0.000000000109 0.000000049551 0.000000000288 -0.000000000073 0.000000005632 -0.000000007521 -0.000000030497 -0.000000000488 0.000000000087 -0.000000000761 -0.000000007521 0.000000212402 0.000000048080 -0.000000002791 -0.000000000634 -0.000000000109 -0.000000030497 0.000000048080 0.000000366926 0.000000005864 -0.000000004446 0.000000049551 -0.000000000488 -0.000000002791 0.000000005864 0.000000458229 +1 1645005914.502975702286 138 139 0.113945330153 -0.011103091303 0.010129852948 -0.000443467816 0.000074664741 0.003419050409 0.000000010923 -0.000000002096 0.000000000221 0.000000000018 -0.000000000160 -0.000000003828 -0.000000002096 0.000000008891 -0.000000000128 -0.000000000737 0.000000000133 0.000000050211 0.000000000221 -0.000000000128 0.000000005571 -0.000000007348 -0.000000031094 -0.000000000767 0.000000000018 -0.000000000737 -0.000000007348 0.000000208241 0.000000044856 -0.000000002913 -0.000000000160 0.000000000133 -0.000000031094 0.000000044856 0.000000367491 0.000000006917 -0.000000003828 0.000000050211 -0.000000000767 -0.000000002913 0.000000006917 0.000000464462 +1 1645005914.606390953064 139 140 0.115763415418 -0.013251559319 -0.002593917162 -0.000387095926 -0.000521830202 0.004558964276 0.000000010125 -0.000000001973 0.000000000276 -0.000000000045 -0.000000000302 -0.000000002573 -0.000000001973 0.000000009055 -0.000000000160 -0.000000000851 0.000000000387 0.000000051580 0.000000000276 -0.000000000160 0.000000005080 -0.000000006897 -0.000000029536 -0.000000000890 -0.000000000045 -0.000000000851 -0.000000006897 0.000000205238 0.000000040500 -0.000000003608 -0.000000000302 0.000000000387 -0.000000029536 0.000000040500 0.000000358067 0.000000007639 -0.000000002573 0.000000051580 -0.000000000890 -0.000000003608 0.000000007639 0.000000474766 +1 1645005914.705168962479 140 141 0.115950679870 -0.007920648289 -0.003581919731 -0.000183384951 -0.000202153026 0.006015096621 0.000000008937 -0.000000001667 0.000000000398 -0.000000000007 -0.000000000886 -0.000000000256 -0.000000001667 0.000000009384 -0.000000000113 -0.000000001058 0.000000000199 0.000000054119 0.000000000398 -0.000000000113 0.000000004302 -0.000000006382 -0.000000025529 -0.000000000504 -0.000000000007 -0.000000001058 -0.000000006382 0.000000203274 0.000000035133 -0.000000005395 -0.000000000886 0.000000000199 -0.000000025529 0.000000035133 0.000000328835 0.000000005386 -0.000000000256 0.000000054119 -0.000000000504 -0.000000005395 0.000000005386 0.000000494144 +1 1645005914.803323030472 141 142 0.113344092687 -0.004513999756 0.003881859525 -0.000081391673 0.001628779102 0.003417929098 0.000000007946 -0.000000001342 0.000000000361 0.000000000164 -0.000000000461 0.000000001892 -0.000000001342 0.000000009145 -0.000000000107 -0.000000001104 0.000000000120 0.000000053134 0.000000000361 -0.000000000107 0.000000003877 -0.000000006332 -0.000000023177 -0.000000000447 0.000000000164 -0.000000001104 -0.000000006332 0.000000198500 0.000000032820 -0.000000006363 -0.000000000461 0.000000000120 -0.000000023177 0.000000032820 0.000000305051 0.000000004342 0.000000001892 0.000000053134 -0.000000000447 -0.000000006363 0.000000004342 0.000000489129 +1 1645005914.904057264328 142 143 0.113932415653 -0.008793837129 0.010399809107 -0.000235218052 -0.001425873177 0.005105286124 0.000000007366 -0.000000001153 0.000000000221 0.000000000379 0.000000000457 0.000000003120 -0.000000001153 0.000000008786 -0.000000000141 -0.000000001114 0.000000000245 0.000000051362 0.000000000221 -0.000000000141 0.000000003860 -0.000000006582 -0.000000022852 -0.000000000726 0.000000000379 -0.000000001114 -0.000000006582 0.000000192639 0.000000031559 -0.000000006610 0.000000000457 0.000000000245 -0.000000022852 0.000000031559 0.000000290550 0.000000005271 0.000000003120 0.000000051362 -0.000000000726 -0.000000006610 0.000000005271 0.000000477340 +1 1645005915.005204200745 143 144 0.114962510911 -0.012358710079 0.012393871611 -0.000551754572 -0.000248518402 0.003903856192 0.000000007433 -0.000000001098 0.000000000173 0.000000000396 0.000000000817 0.000000003762 -0.000000001098 0.000000008813 -0.000000000189 -0.000000001302 0.000000000582 0.000000051927 0.000000000173 -0.000000000189 0.000000003938 -0.000000006922 -0.000000022827 -0.000000001046 0.000000000396 -0.000000001302 -0.000000006922 0.000000188507 0.000000031757 -0.000000007689 0.000000000817 0.000000000582 -0.000000022827 0.000000031757 0.000000280243 0.000000007393 0.000000003762 0.000000051927 -0.000000001046 -0.000000007689 0.000000007393 0.000000482670 +1 1645005915.104339361191 144 145 0.108638769015 -0.013109909901 0.001280725320 0.000360075563 -0.000501910880 0.003610744318 0.000000007793 -0.000000001032 0.000000000172 0.000000000318 0.000000000989 0.000000004597 -0.000000001032 0.000000008853 -0.000000000219 -0.000000001613 0.000000000892 0.000000052595 0.000000000172 -0.000000000219 0.000000004122 -0.000000007546 -0.000000023608 -0.000000001206 0.000000000318 -0.000000001613 -0.000000007546 0.000000185838 0.000000036183 -0.000000009621 0.000000000989 0.000000000892 -0.000000023608 0.000000036183 0.000000280865 0.000000009177 0.000000004597 0.000000052595 -0.000000001206 -0.000000009621 0.000000009177 0.000000490642 +1 1645005915.205692529678 145 146 0.109329578702 -0.013304774566 0.000306145880 -0.000023011733 0.000369480071 0.005562445904 0.000000008439 -0.000000000978 0.000000000142 0.000000000381 0.000000001532 0.000000005640 -0.000000000978 0.000000008798 -0.000000000212 -0.000000001840 0.000000000941 0.000000053029 0.000000000142 -0.000000000212 0.000000004685 -0.000000008117 -0.000000027229 -0.000000001233 0.000000000381 -0.000000001840 -0.000000008117 0.000000182654 0.000000041408 -0.000000011114 0.000000001532 0.000000000941 -0.000000027229 0.000000041408 0.000000305374 0.000000009971 0.000000005640 0.000000053029 -0.000000001233 -0.000000011114 0.000000009971 0.000000499247 +1 1645005915.303473472595 146 147 0.102615336962 -0.011723016179 0.016921966183 -0.000598724662 -0.000883449845 0.009912420693 0.000000009050 -0.000000000931 0.000000000141 0.000000000283 0.000000001632 0.000000006520 -0.000000000931 0.000000008841 -0.000000000254 -0.000000001783 0.000000001294 0.000000053986 0.000000000141 -0.000000000254 0.000000005341 -0.000000008689 -0.000000031566 -0.000000001523 0.000000000283 -0.000000001783 -0.000000008689 0.000000180735 0.000000046782 -0.000000010967 0.000000001632 0.000000001294 -0.000000031566 0.000000046782 0.000000337155 0.000000012174 0.000000006520 0.000000053986 -0.000000001523 -0.000000010967 0.000000012174 0.000000511691 +1 1645005915.403654336929 147 148 0.103466727897 -0.010311299427 0.019111226280 -0.000605924141 -0.000721619272 0.014142457072 0.000000009872 -0.000000000937 0.000000000170 0.000000000104 0.000000001623 0.000000006936 -0.000000000937 0.000000009038 -0.000000000289 -0.000000001859 0.000000001533 0.000000055630 0.000000000170 -0.000000000289 0.000000005662 -0.000000008982 -0.000000033316 -0.000000001699 0.000000000104 -0.000000001859 -0.000000008982 0.000000179291 0.000000050127 -0.000000011628 0.000000001623 0.000000001533 -0.000000033316 0.000000050127 0.000000349687 0.000000013494 0.000000006936 0.000000055630 -0.000000001699 -0.000000011628 0.000000013494 0.000000526085 +1 1645005915.502804994583 148 149 0.102555175381 -0.007097360397 0.012460654451 -0.001109214232 -0.001235813727 0.016759657315 0.000000010898 -0.000000001092 0.000000000180 0.000000000034 0.000000001717 0.000000006432 -0.000000001092 0.000000009110 -0.000000000312 -0.000000001774 0.000000001584 0.000000056414 0.000000000180 -0.000000000312 0.000000005889 -0.000000008997 -0.000000034270 -0.000000001851 0.000000000034 -0.000000001774 -0.000000008997 0.000000178153 0.000000052144 -0.000000011187 0.000000001717 0.000000001584 -0.000000034270 0.000000052144 0.000000356560 0.000000013909 0.000000006432 0.000000056414 -0.000000001851 -0.000000011187 0.000000013909 0.000000534819 +1 1645005915.606675386429 149 150 0.110023677701 -0.005261608351 -0.001025304830 -0.000900306716 -0.001729175039 0.017897628554 0.000000011791 -0.000000001314 0.000000000193 -0.000000000199 0.000000001536 0.000000005499 -0.000000001314 0.000000009095 -0.000000000322 -0.000000001698 0.000000001815 0.000000056489 0.000000000193 -0.000000000322 0.000000006056 -0.000000009260 -0.000000035203 -0.000000001976 -0.000000000199 -0.000000001698 -0.000000009260 0.000000180591 0.000000056392 -0.000000010737 0.000000001536 0.000000001815 -0.000000035203 0.000000056392 0.000000367602 0.000000015758 0.000000005499 0.000000056489 -0.000000001976 -0.000000010737 0.000000015758 0.000000538970 +1 1645005915.706191539764 150 151 0.108997921836 -0.002332104651 -0.007968681270 -0.000641057737 0.000297247851 0.013744307307 0.000000012496 -0.000000001488 0.000000000182 -0.000000000437 0.000000001129 0.000000004476 -0.000000001488 0.000000009204 -0.000000000349 -0.000000001745 0.000000002295 0.000000057627 0.000000000182 -0.000000000349 0.000000006180 -0.000000009543 -0.000000036359 -0.000000002262 -0.000000000437 -0.000000001745 -0.000000009543 0.000000185160 0.000000061278 -0.000000010792 0.000000001129 0.000000002295 -0.000000036359 0.000000061278 0.000000385670 0.000000019334 0.000000004476 0.000000057627 -0.000000002262 -0.000000010792 0.000000019334 0.000000551459 +1 1645005915.803260087967 151 152 0.106514602397 -0.001770007540 -0.000096349759 -0.000752589415 0.000118381130 0.009137181083 0.000000012917 -0.000000001667 0.000000000103 -0.000000000308 0.000000001192 0.000000002903 -0.000000001667 0.000000009141 -0.000000000325 -0.000000001618 0.000000002128 0.000000057675 0.000000000103 -0.000000000325 0.000000006523 -0.000000009497 -0.000000039281 -0.000000002198 -0.000000000308 -0.000000001618 -0.000000009497 0.000000189290 0.000000063224 -0.000000009134 0.000000001192 0.000000002128 -0.000000039281 0.000000063224 0.000000417011 0.000000018931 0.000000002903 0.000000057675 -0.000000002198 -0.000000009134 0.000000018931 0.000000555912 +1 1645005915.905807971954 152 153 0.109997173206 -0.004906461686 0.014498026032 -0.000862247837 0.000085107174 0.006427930427 0.000000013310 -0.000000001919 0.000000000086 -0.000000000244 0.000000001170 0.000000001331 -0.000000001919 0.000000008982 -0.000000000223 -0.000000001470 0.000000001408 0.000000056875 0.000000000086 -0.000000000223 0.000000006917 -0.000000008840 -0.000000042953 -0.000000001544 -0.000000000244 -0.000000001470 -0.000000008840 0.000000192614 0.000000059942 -0.000000007491 0.000000001170 0.000000001408 -0.000000042953 0.000000059942 0.000000453104 0.000000014715 0.000000001331 0.000000056875 -0.000000001544 -0.000000007491 0.000000014715 0.000000552905 +1 1645005916.003596782684 153 154 0.105956714038 -0.007930247692 0.017803331311 -0.000325566987 -0.000424651970 0.000784271079 0.000000013892 -0.000000002247 0.000000000116 -0.000000000302 0.000000000924 -0.000000000604 -0.000000002247 0.000000009117 -0.000000000160 -0.000000001400 0.000000000976 0.000000058527 0.000000000116 -0.000000000160 0.000000007137 -0.000000007719 -0.000000045692 -0.000000001110 -0.000000000302 -0.000000001400 -0.000000007719 0.000000198654 0.000000053623 -0.000000006581 0.000000000924 0.000000000976 -0.000000045692 0.000000053623 0.000000487022 0.000000012262 -0.000000000604 0.000000058527 -0.000000001110 -0.000000006581 0.000000012262 0.000000572349 +1 1645005916.099525690079 154 155 0.106714377159 -0.007351031551 0.007507753034 0.000196149893 -0.001095119436 -0.001110160472 0.000000014587 -0.000000002603 0.000000000179 -0.000000000321 0.000000000579 -0.000000002608 -0.000000002603 0.000000009454 -0.000000000165 -0.000000001330 0.000000001027 0.000000061678 0.000000000179 -0.000000000165 0.000000007113 -0.000000007149 -0.000000046636 -0.000000001161 -0.000000000321 -0.000000001330 -0.000000007149 0.000000206337 0.000000051754 -0.000000005643 0.000000000579 0.000000001027 -0.000000046636 0.000000051754 0.000000508473 0.000000013467 -0.000000002608 0.000000061678 -0.000000001161 -0.000000005643 0.000000013467 0.000000604801 +1 1645005916.201919555664 155 156 0.114214238464 -0.005627023360 -0.006587383337 0.000405811911 0.001118760483 -0.003412120900 0.000000016892 -0.000000003571 0.000000000307 -0.000000000279 0.000000000065 -0.000000005502 -0.000000003571 0.000000010098 -0.000000000203 -0.000000001503 0.000000001164 0.000000066149 0.000000000307 -0.000000000203 0.000000007107 -0.000000007131 -0.000000047845 -0.000000001264 -0.000000000279 -0.000000001503 -0.000000007131 0.000000211613 0.000000054896 -0.000000007056 0.000000000065 0.000000001164 -0.000000047845 0.000000054896 0.000000531070 0.000000014713 -0.000000005502 0.000000066149 -0.000000001264 -0.000000007056 0.000000014713 0.000000647179 +1 1645005916.305266141891 156 157 0.111308342148 -0.009772812253 -0.005809218470 -0.000158830582 -0.000215490923 0.000333338897 0.000000024402 -0.000000006436 0.000000000493 -0.000000000261 -0.000000000377 -0.000000011240 -0.000000006436 0.000000011477 -0.000000000292 -0.000000001398 0.000000001461 0.000000072198 0.000000000493 -0.000000000292 0.000000007568 -0.000000007942 -0.000000052886 -0.000000001576 -0.000000000261 -0.000000001398 -0.000000007942 0.000000218297 0.000000065152 -0.000000006844 -0.000000000377 0.000000001461 -0.000000052886 0.000000065152 0.000000590097 0.000000017471 -0.000000011240 0.000000072198 -0.000000001576 -0.000000006844 0.000000017471 0.000000700700 +1 1645005916.402242660522 157 158 0.098861612320 -0.010532723695 0.012346443386 0.000302995827 0.000414997200 0.001602970315 0.000000037109 -0.000000010805 0.000000000821 -0.000000000804 -0.000000001279 -0.000000016111 -0.000000010805 0.000000012886 -0.000000000505 -0.000000000852 0.000000002599 0.000000074435 0.000000000821 -0.000000000505 0.000000008686 -0.000000010526 -0.000000062013 -0.000000002548 -0.000000000804 -0.000000000852 -0.000000010526 0.000000231195 0.000000086125 -0.000000003957 -0.000000001279 0.000000002599 -0.000000062013 0.000000086125 0.000000677017 0.000000025691 -0.000000016111 0.000000074435 -0.000000002548 -0.000000003957 0.000000025691 0.000000727276 +1 1645005916.503258705139 158 159 0.102502168086 -0.009093252456 0.014363853030 -0.000283008408 -0.000358165826 0.006681060732 0.000000051140 -0.000000015225 0.000000001216 -0.000000002070 -0.000000003404 -0.000000014930 -0.000000015225 0.000000013941 -0.000000000656 -0.000000000227 0.000000003539 0.000000072261 0.000000001216 -0.000000000656 0.000000009605 -0.000000014137 -0.000000068917 -0.000000002838 -0.000000002070 -0.000000000227 -0.000000014137 0.000000246310 0.000000111562 -0.000000001761 -0.000000003404 0.000000003539 -0.000000068917 0.000000111562 0.000000735475 0.000000029040 -0.000000014930 0.000000072261 -0.000000002838 -0.000000001761 0.000000029040 0.000000724038 +1 1645005916.603333711624 159 160 0.110584118510 -0.006969165435 0.006755957308 -0.000547151088 -0.002765490415 0.010723492151 0.000000064963 -0.000000019500 0.000000001613 -0.000000003508 -0.000000006554 -0.000000011097 -0.000000019500 0.000000015237 -0.000000000783 0.000000000383 0.000000004607 0.000000071746 0.000000001613 -0.000000000783 0.000000010020 -0.000000017014 -0.000000072512 -0.000000002905 -0.000000003508 0.000000000383 -0.000000017014 0.000000260411 0.000000131565 -0.000000000900 -0.000000006554 0.000000004607 -0.000000072512 0.000000131565 0.000000769488 0.000000030950 -0.000000011097 0.000000071746 -0.000000002905 -0.000000000900 0.000000030950 0.000000738042 +1 1645005916.703304529190 160 161 0.115867178906 -0.001405365939 -0.006941536777 -0.000217074623 0.000672686205 0.003645953857 0.000000078026 -0.000000023457 0.000000002054 -0.000000004985 -0.000000009324 -0.000000008604 -0.000000023457 0.000000016517 -0.000000000950 0.000000000749 0.000000005758 0.000000073233 0.000000002054 -0.000000000950 0.000000010587 -0.000000019544 -0.000000077702 -0.000000003100 -0.000000004985 0.000000000749 -0.000000019544 0.000000277224 0.000000150069 -0.000000002565 -0.000000009324 0.000000005758 -0.000000077702 0.000000150069 0.000000821178 0.000000033645 -0.000000008604 0.000000073233 -0.000000003100 -0.000000002565 0.000000033645 0.000000772113 +1 1645005916.804172515869 161 162 0.117302909972 0.000295419838 -0.016017415108 -0.000106164346 0.000901201232 -0.001277739925 0.000000088382 -0.000000026988 0.000000002179 -0.000000006324 -0.000000011465 -0.000000008722 -0.000000026988 0.000000017842 -0.000000000989 0.000000001031 0.000000006457 0.000000076083 0.000000002179 -0.000000000989 0.000000010901 -0.000000021540 -0.000000081165 -0.000000002920 -0.000000006324 0.000000001031 -0.000000021540 0.000000295157 0.000000164011 -0.000000004981 -0.000000011465 0.000000006457 -0.000000081165 0.000000164011 0.000000859137 0.000000032540 -0.000000008722 0.000000076083 -0.000000002920 -0.000000004981 0.000000032540 0.000000810795 +1 1645005916.907030344009 162 163 0.117113502778 -0.001332837421 -0.000358495722 0.000214441813 0.001777967441 -0.003905204155 0.000000096512 -0.000000029855 0.000000002263 -0.000000007359 -0.000000013389 -0.000000009255 -0.000000029855 0.000000018930 -0.000000001023 0.000000001324 0.000000007208 0.000000077916 0.000000002263 -0.000000001023 0.000000011112 -0.000000023344 -0.000000084006 -0.000000002951 -0.000000007359 0.000000001324 -0.000000023344 0.000000313829 0.000000177054 -0.000000006430 -0.000000013389 0.000000007208 -0.000000084006 0.000000177054 0.000000893655 0.000000033005 -0.000000009255 0.000000077916 -0.000000002951 -0.000000006430 0.000000033005 0.000000834816 +1 1645005917.005794048309 163 164 0.112162385129 -0.003856647165 0.014586076375 -0.000117380248 -0.001608061862 -0.001170630734 0.000000103324 -0.000000032168 0.000000002342 -0.000000008716 -0.000000016076 -0.000000006607 -0.000000032168 0.000000019618 -0.000000000993 0.000000001842 0.000000007717 0.000000077445 0.000000002342 -0.000000000993 0.000000011325 -0.000000025511 -0.000000087156 -0.000000002530 -0.000000008716 0.000000001842 -0.000000025511 0.000000334949 0.000000195314 -0.000000006232 -0.000000016076 0.000000007717 -0.000000087156 0.000000195314 0.000000934604 0.000000029769 -0.000000006607 0.000000077445 -0.000000002530 -0.000000006232 0.000000029769 0.000000849058 +1 1645005917.104857444763 164 165 0.111741027745 -0.003567459006 0.022616849137 0.000236524934 0.000178204487 -0.002832779338 0.000000108247 -0.000000033657 0.000000002538 -0.000000009343 -0.000000017174 -0.000000003909 -0.000000033657 0.000000020124 -0.000000000963 0.000000001696 0.000000007303 0.000000078131 0.000000002538 -0.000000000963 0.000000011442 -0.000000026989 -0.000000089241 -0.000000001718 -0.000000009343 0.000000001696 -0.000000026989 0.000000352166 0.000000208528 -0.000000009427 -0.000000017174 0.000000007303 -0.000000089241 0.000000208528 0.000000963343 0.000000021895 -0.000000003909 0.000000078131 -0.000000001718 -0.000000009427 0.000000021895 0.000000871104 +1 1645005917.207338571548 165 166 0.113903906676 -0.004144900189 0.009725110224 0.000086239947 -0.000462914316 -0.004704134184 0.000000110910 -0.000000034448 0.000000002432 -0.000000009395 -0.000000015619 -0.000000003444 -0.000000034448 0.000000020431 -0.000000000941 0.000000001671 0.000000006954 0.000000079230 0.000000002432 -0.000000000941 0.000000011953 -0.000000028963 -0.000000094423 -0.000000001837 -0.000000009395 0.000000001671 -0.000000028963 0.000000367870 0.000000226360 -0.000000010076 -0.000000015619 0.000000006954 -0.000000094423 0.000000226360 0.000001016065 0.000000022453 -0.000000003444 0.000000079230 -0.000000001837 -0.000000010076 0.000000022453 0.000000886686 +1 1645005917.305225610733 166 167 0.106350650780 -0.008006138847 -0.004136760500 0.000217735543 0.000793264977 -0.005450629162 0.000000112457 -0.000000034975 0.000000002459 -0.000000009900 -0.000000014669 -0.000000004888 -0.000000034975 0.000000020691 -0.000000001068 0.000000002175 0.000000007608 0.000000081541 0.000000002459 -0.000000001068 0.000000012417 -0.000000030753 -0.000000100017 -0.000000002845 -0.000000009900 0.000000002175 -0.000000030753 0.000000385554 0.000000244403 -0.000000007583 -0.000000014669 0.000000007608 -0.000000100017 0.000000244403 0.000001081297 0.000000030238 -0.000000004888 0.000000081541 -0.000000002845 -0.000000007583 0.000000030238 0.000000913045 +1 1645005917.404884338379 167 168 0.098821449951 -0.014936367447 0.003157378141 0.000960141444 0.000553150047 -0.007218868305 0.000000114058 -0.000000035351 0.000000002309 -0.000000010234 -0.000000013984 -0.000000006367 -0.000000035351 0.000000020849 -0.000000000967 0.000000002122 0.000000006908 0.000000083703 0.000000002309 -0.000000000967 0.000000012562 -0.000000031523 -0.000000102262 -0.000000002356 -0.000000010234 0.000000002122 -0.000000031523 0.000000397207 0.000000252081 -0.000000009475 -0.000000013984 0.000000006908 -0.000000102262 0.000000252081 0.000001111583 0.000000025955 -0.000000006367 0.000000083703 -0.000000002356 -0.000000009475 0.000000025955 0.000000936514 +1 1645005917.504653453827 168 169 0.094186437816 -0.015526879719 0.019416391189 0.000802075926 0.000404113705 -0.007307554869 0.000000114991 -0.000000035620 0.000000002454 -0.000000011103 -0.000000015611 -0.000000006733 -0.000000035620 0.000000020791 -0.000000000965 0.000000002157 0.000000007149 0.000000083370 0.000000002454 -0.000000000965 0.000000012360 -0.000000031514 -0.000000100745 -0.000000002049 -0.000000011103 0.000000002157 -0.000000031514 0.000000399690 0.000000251423 -0.000000012007 -0.000000015611 0.000000007149 -0.000000100745 0.000000251423 0.000001101197 0.000000025036 -0.000000006733 0.000000083370 -0.000000002049 -0.000000012007 0.000000025036 0.000000937610 +1 1645005917.605074167252 169 170 0.100592303433 -0.011605821285 0.019967308871 0.000000189307 -0.002053301407 -0.000766444936 0.000000116137 -0.000000036111 0.000000002855 -0.000000013056 -0.000000020038 -0.000000003199 -0.000000036111 0.000000021073 -0.000000001193 0.000000003208 0.000000009429 0.000000083889 0.000000002855 -0.000000001193 0.000000011993 -0.000000030942 -0.000000098311 -0.000000002956 -0.000000013056 0.000000003208 -0.000000030942 0.000000398169 0.000000245893 -0.000000009126 -0.000000020038 0.000000009429 -0.000000098311 0.000000245893 0.000001086821 0.000000034002 -0.000000003199 0.000000083889 -0.000000002956 -0.000000009126 0.000000034002 0.000000959796 +1 1645005917.706811666489 170 171 0.107970526617 -0.008036241272 0.010229935133 -0.000213881979 -0.001054988989 0.000031095351 0.000000118125 -0.000000036312 0.000000003318 -0.000000014955 -0.000000023459 0.000000001488 -0.000000036312 0.000000021160 -0.000000001345 0.000000003695 0.000000010662 0.000000084616 0.000000003318 -0.000000001345 0.000000011958 -0.000000030573 -0.000000098713 -0.000000003110 -0.000000014955 0.000000003695 -0.000000030573 0.000000397435 0.000000242189 -0.000000009785 -0.000000023459 0.000000010662 -0.000000098713 0.000000242189 0.000001096053 0.000000036591 0.000000001488 0.000000084616 -0.000000003110 -0.000000009785 0.000000036591 0.000000988045 +1 1645005917.804672718048 171 172 0.105731298009 -0.005919607258 -0.005833779357 -0.000045148203 0.001018874621 -0.000751755422 0.000000121189 -0.000000036830 0.000000003468 -0.000000015684 -0.000000023681 0.000000004698 -0.000000036830 0.000000021195 -0.000000001433 0.000000003788 0.000000011194 0.000000084668 0.000000003468 -0.000000001433 0.000000012248 -0.000000031571 -0.000000101807 -0.000000003557 -0.000000015684 0.000000003788 -0.000000031571 0.000000404763 0.000000251181 -0.000000010529 -0.000000023681 0.000000011194 -0.000000101807 0.000000251181 0.000001129929 0.000000041603 0.000000004698 0.000000084668 -0.000000003557 -0.000000010529 0.000000041603 0.000001008043 +1 1645005917.904610872269 172 173 0.103855440730 -0.008847785631 -0.010570833324 0.000073912894 0.000524173989 -0.000244240168 0.000000124632 -0.000000037627 0.000000003556 -0.000000016897 -0.000000025958 0.000000007236 -0.000000037627 0.000000021390 -0.000000001541 0.000000004351 0.000000012531 0.000000085042 0.000000003556 -0.000000001541 0.000000012424 -0.000000032522 -0.000000104071 -0.000000004167 -0.000000016897 0.000000004351 -0.000000032522 0.000000412569 0.000000261172 -0.000000009421 -0.000000025958 0.000000012531 -0.000000104071 0.000000261172 0.000001156601 0.000000046360 0.000000007236 0.000000085042 -0.000000004167 -0.000000009421 0.000000046360 0.000001026787 +1 1645005918.005419969559 173 174 0.100505315538 -0.010103880265 0.006164128240 0.000413598049 0.001946711804 -0.004386893254 0.000000127051 -0.000000038318 0.000000003856 -0.000000018597 -0.000000029537 0.000000006021 -0.000000038318 0.000000021496 -0.000000001582 0.000000004785 0.000000013137 0.000000085273 0.000000003856 -0.000000001582 0.000000012143 -0.000000032181 -0.000000103060 -0.000000003657 -0.000000018597 0.000000004785 -0.000000032181 0.000000415569 0.000000261713 -0.000000011012 -0.000000029537 0.000000013137 -0.000000103060 0.000000261713 0.000001158654 0.000000041063 0.000000006021 0.000000085273 -0.000000003657 -0.000000011012 0.000000041063 0.000001031052 +1 1645005918.106912612915 174 175 0.104243823486 -0.015398520529 0.017609900953 0.000251278556 -0.001321653647 -0.002699090350 0.000000129650 -0.000000039225 0.000000003859 -0.000000019970 -0.000000031925 0.000000007468 -0.000000039225 0.000000021632 -0.000000001548 0.000000005349 0.000000013611 0.000000083970 0.000000003859 -0.000000001548 0.000000012009 -0.000000032668 -0.000000103462 -0.000000003323 -0.000000019970 0.000000005349 -0.000000032668 0.000000423439 0.000000269388 -0.000000010299 -0.000000031925 0.000000013611 -0.000000103462 0.000000269388 0.000001178424 0.000000038174 0.000000007468 0.000000083970 -0.000000003323 -0.000000010299 0.000000038174 0.000001031326 +1 1645005918.205326795578 175 176 0.105018001061 -0.016620276316 0.016583575074 0.000471605107 -0.001302930249 -0.006284028454 0.000000132952 -0.000000039810 0.000000004127 -0.000000022074 -0.000000034193 0.000000010673 -0.000000039810 0.000000021502 -0.000000001615 0.000000005775 0.000000014293 0.000000082301 0.000000004127 -0.000000001615 0.000000012306 -0.000000034229 -0.000000107844 -0.000000003231 -0.000000022074 0.000000005775 -0.000000034229 0.000000435942 0.000000288374 -0.000000012012 -0.000000034193 0.000000014293 -0.000000107844 0.000000288374 0.000001238900 0.000000038299 0.000000010673 0.000000082301 -0.000000003231 -0.000000012012 0.000000038299 0.000001036436 +1 1645005918.304870605469 176 177 0.104912611278 -0.018585854244 0.001515240424 0.000485324157 0.001946316449 -0.010479077493 0.000000136347 -0.000000040849 0.000000004059 -0.000000022351 -0.000000030809 0.000000009278 -0.000000040849 0.000000021957 -0.000000001504 0.000000004860 0.000000012760 0.000000085030 0.000000004059 -0.000000001504 0.000000012626 -0.000000035744 -0.000000112385 -0.000000002494 -0.000000022351 0.000000004860 -0.000000035744 0.000000448023 0.000000308545 -0.000000021449 -0.000000030809 0.000000012760 -0.000000112385 0.000000308545 0.000001300935 0.000000034558 0.000000009278 0.000000085030 -0.000000002494 -0.000000021449 0.000000034558 0.000001068746 +1 1645005918.405547380447 177 178 0.104792166995 -0.023226516703 -0.007056980576 0.000267709258 0.000474336908 -0.007207889728 0.000000138617 -0.000000041762 0.000000003857 -0.000000022163 -0.000000029275 0.000000010678 -0.000000041762 0.000000022377 -0.000000001391 0.000000004532 0.000000011951 0.000000086241 0.000000003857 -0.000000001391 0.000000012725 -0.000000037131 -0.000000114801 -0.000000001984 -0.000000022163 0.000000004532 -0.000000037131 0.000000457244 0.000000327019 -0.000000025297 -0.000000029275 0.000000011951 -0.000000114801 0.000000327019 0.000001339780 0.000000031499 0.000000010678 0.000000086241 -0.000000001984 -0.000000025297 0.000000031499 0.000001092396 +1 1645005918.505361080170 178 179 0.102238431531 -0.023908947381 0.006589654277 0.000655843708 0.000453140817 -0.001821223501 0.000000140246 -0.000000042197 0.000000004132 -0.000000023003 -0.000000031421 0.000000015985 -0.000000042197 0.000000022353 -0.000000001641 0.000000005629 0.000000014034 0.000000084232 0.000000004132 -0.000000001641 0.000000012973 -0.000000038726 -0.000000118299 -0.000000003433 -0.000000023003 0.000000005629 -0.000000038726 0.000000462890 0.000000346326 -0.000000018649 -0.000000031421 0.000000014034 -0.000000118299 0.000000346326 0.000001385059 0.000000044386 0.000000015985 0.000000084232 -0.000000003433 -0.000000018649 0.000000044386 0.000001097883 +1 1645005918.605193614960 179 180 0.107683008826 -0.022764995092 0.017514407486 -0.000368667307 -0.001653800364 0.007067164909 0.000000141481 -0.000000042435 0.000000004360 -0.000000024495 -0.000000034469 0.000000024183 -0.000000042435 0.000000022389 -0.000000001837 0.000000006705 0.000000016054 0.000000082514 0.000000004360 -0.000000001837 0.000000013111 -0.000000040013 -0.000000120651 -0.000000004389 -0.000000024495 0.000000006705 -0.000000040013 0.000000466496 0.000000363613 -0.000000014517 -0.000000034469 0.000000016054 -0.000000120651 0.000000363613 0.000001419210 0.000000053423 0.000000024183 0.000000082514 -0.000000004389 -0.000000014517 0.000000053423 0.000001113406 +1 1645005918.704743146896 180 181 0.111753784251 -0.017339671469 0.014827032018 -0.000550269913 -0.000679567626 0.009481384874 0.000000142307 -0.000000041952 0.000000004661 -0.000000025741 -0.000000036641 0.000000033488 -0.000000041952 0.000000022051 -0.000000001803 0.000000006655 0.000000015521 0.000000081558 0.000000004661 -0.000000001803 0.000000012906 -0.000000039951 -0.000000119907 -0.000000003114 -0.000000025741 0.000000006655 -0.000000039951 0.000000465724 0.000000367203 -0.000000019105 -0.000000036641 0.000000015521 -0.000000119907 0.000000367203 0.000001423450 0.000000042472 0.000000033488 0.000000081558 -0.000000003114 -0.000000019105 0.000000042472 0.000001147641 +1 1645005918.806400060654 181 182 0.113872331121 -0.014001660104 -0.000550563706 -0.000917522817 -0.000618426666 0.010240698071 0.000000143709 -0.000000041830 0.000000004468 -0.000000025299 -0.000000034644 0.000000039964 -0.000000041830 0.000000022150 -0.000000001671 0.000000005770 0.000000014366 0.000000084352 0.000000004468 -0.000000001671 0.000000012897 -0.000000040281 -0.000000121040 -0.000000002385 -0.000000025299 0.000000005770 -0.000000040281 0.000000469448 0.000000373553 -0.000000026765 -0.000000034644 0.000000014366 -0.000000121040 0.000000373553 0.000001447927 0.000000037412 0.000000039964 0.000000084352 -0.000000002385 -0.000000026765 0.000000037412 0.000001214810 +1 1645005918.905241250992 182 183 0.108627949209 -0.010341888855 -0.014273654629 -0.000517403403 0.001641954984 0.006780294180 0.000000145842 -0.000000041995 0.000000004312 -0.000000023934 -0.000000031977 0.000000043324 -0.000000041995 0.000000022211 -0.000000001575 0.000000004796 0.000000013177 0.000000086375 0.000000004312 -0.000000001575 0.000000013350 -0.000000041889 -0.000000127167 -0.000000001918 -0.000000023934 0.000000004796 -0.000000041889 0.000000478700 0.000000395388 -0.000000032141 -0.000000031977 0.000000013177 -0.000000127167 0.000000395388 0.000001528684 0.000000033922 0.000000043324 0.000000086375 -0.000000001918 -0.000000032141 0.000000033922 0.000001262581 +1 1645005919.006230831146 183 184 0.111090918097 -0.011375888755 -0.011195486808 -0.000492323663 -0.000760811117 0.007360552053 0.000000147026 -0.000000042366 0.000000004035 -0.000000022992 -0.000000032173 0.000000045126 -0.000000042366 0.000000022179 -0.000000001481 0.000000004851 0.000000013121 0.000000085324 0.000000004035 -0.000000001481 0.000000013612 -0.000000043406 -0.000000131031 -0.000000001914 -0.000000022992 0.000000004851 -0.000000043406 0.000000485597 0.000000416394 -0.000000028771 -0.000000032173 0.000000013121 -0.000000131031 0.000000416394 0.000001580709 0.000000032816 0.000000045126 0.000000085324 -0.000000001914 -0.000000028771 0.000000032816 0.000001269063 +1 1645005919.103321552277 184 185 0.106064541975 -0.013212314792 0.007933789346 -0.000093460789 -0.000240302902 0.002770392774 0.000000147409 -0.000000042125 0.000000004336 -0.000000023061 -0.000000034649 0.000000046124 -0.000000042125 0.000000021985 -0.000000001589 0.000000004937 0.000000014153 0.000000085861 0.000000004336 -0.000000001589 0.000000013574 -0.000000043836 -0.000000132647 -0.000000002199 -0.000000023061 0.000000004937 -0.000000043836 0.000000489170 0.000000427692 -0.000000028221 -0.000000034649 0.000000014153 -0.000000132647 0.000000427692 0.000001615546 0.000000036449 0.000000046124 0.000000085861 -0.000000002199 -0.000000028221 0.000000036449 0.000001285384 +1 1645005919.204598665237 185 186 0.109583227748 -0.018751742246 0.015930061005 -0.000207377330 -0.000969420842 -0.001611917201 0.000000147737 -0.000000042240 0.000000004321 -0.000000022740 -0.000000033885 0.000000043695 -0.000000042240 0.000000022018 -0.000000001592 0.000000004837 0.000000014065 0.000000087852 0.000000004321 -0.000000001592 0.000000013380 -0.000000043618 -0.000000132567 -0.000000002397 -0.000000022740 0.000000004837 -0.000000043618 0.000000492109 0.000000430403 -0.000000028113 -0.000000033885 0.000000014065 -0.000000132567 0.000000430403 0.000001632040 0.000000039106 0.000000043695 0.000000087852 -0.000000002397 -0.000000028113 0.000000039106 0.000001310151 +1 1645005919.304369688034 186 187 0.105935726310 -0.021962018124 0.012691866608 0.000577732489 0.000249586532 -0.006153890357 0.000000149020 -0.000000042831 0.000000003937 -0.000000021091 -0.000000027669 0.000000039205 -0.000000042831 0.000000022289 -0.000000001497 0.000000004450 0.000000012366 0.000000090284 0.000000003937 -0.000000001497 0.000000013677 -0.000000045058 -0.000000137345 -0.000000002732 -0.000000021091 0.000000004450 -0.000000045058 0.000000500943 0.000000450114 -0.000000025733 -0.000000027669 0.000000012366 -0.000000137345 0.000000450114 0.000001702000 0.000000041449 0.000000039205 0.000000090284 -0.000000002732 -0.000000025733 0.000000041449 0.000001329947 +1 1645005919.404906749725 187 188 0.107178844297 -0.022750150514 -0.003656608090 0.000369946408 -0.000252619318 -0.007040696870 0.000000150210 -0.000000043366 0.000000003672 -0.000000019261 -0.000000023707 0.000000038513 -0.000000043366 0.000000022455 -0.000000001373 0.000000003685 0.000000010707 0.000000090631 0.000000003672 -0.000000001373 0.000000014393 -0.000000048027 -0.000000146585 -0.000000002253 -0.000000019261 0.000000003685 -0.000000048027 0.000000514919 0.000000488132 -0.000000028143 -0.000000023707 0.000000010707 -0.000000146585 0.000000488132 0.000001822771 0.000000035984 0.000000038513 0.000000090631 -0.000000002253 -0.000000028143 0.000000035984 0.000001341511 +1 1645005919.509163618088 188 189 0.111763033254 -0.025831837093 -0.007938767615 0.000112630539 0.001169710732 -0.004771712646 0.000000150729 -0.000000043454 0.000000003900 -0.000000018668 -0.000000025291 0.000000043404 -0.000000043454 0.000000022348 -0.000000001378 0.000000003081 0.000000010657 0.000000089362 0.000000003900 -0.000000001378 0.000000014579 -0.000000049376 -0.000000149721 -0.000000001575 -0.000000018668 0.000000003081 -0.000000049376 0.000000521537 0.000000508382 -0.000000033045 -0.000000025291 0.000000010657 -0.000000149721 0.000000508382 0.000001873121 0.000000031279 0.000000043404 0.000000089362 -0.000000001575 -0.000000033045 0.000000031279 0.000001360429 +1 1645005919.606138467789 189 190 0.103050190411 -0.028243153856 0.009036138321 0.000299620151 -0.000460363273 -0.002126451387 0.000000151210 -0.000000043262 0.000000004079 -0.000000019984 -0.000000029692 0.000000051331 -0.000000043262 0.000000022105 -0.000000001496 0.000000003836 0.000000012813 0.000000087307 0.000000004079 -0.000000001496 0.000000014344 -0.000000049082 -0.000000148242 -0.000000002279 -0.000000019984 0.000000003836 -0.000000049082 0.000000523071 0.000000508358 -0.000000029835 -0.000000029692 0.000000012813 -0.000000148242 0.000000508358 0.000001871353 0.000000041535 0.000000051331 0.000000087307 -0.000000002279 -0.000000029835 0.000000041535 0.000001379713 +1 1645005919.708493471146 190 191 0.110366530571 -0.029472021130 0.018614876772 0.000102110420 0.000167129598 0.001994724913 0.000000150740 -0.000000042621 0.000000004151 -0.000000020036 -0.000000029606 0.000000062230 -0.000000042621 0.000000021889 -0.000000001558 0.000000003923 0.000000013386 0.000000086272 0.000000004151 -0.000000001558 0.000000014122 -0.000000049518 -0.000000146966 -0.000000002704 -0.000000020036 0.000000003923 -0.000000049518 0.000000525905 0.000000515666 -0.000000029326 -0.000000029606 0.000000013386 -0.000000146966 0.000000515666 0.000001869683 0.000000049449 0.000000062230 0.000000086272 -0.000000002704 -0.000000029326 0.000000049449 0.000001414664 +1 1645005919.793772935867 191 192 0.096165140501 -0.018685263170 0.011579795113 -0.000740821107 -0.001031369105 0.004911205760 0.000000150428 -0.000000041858 0.000000003727 -0.000000018419 -0.000000025185 0.000000073735 -0.000000041858 0.000000021544 -0.000000001374 0.000000003222 0.000000011562 0.000000084970 0.000000003727 -0.000000001374 0.000000014202 -0.000000051029 -0.000000149309 -0.000000002193 -0.000000018419 0.000000003222 -0.000000051029 0.000000531889 0.000000537344 -0.000000031680 -0.000000025185 0.000000011562 -0.000000149309 0.000000537344 0.000001910836 0.000000046007 0.000000073735 0.000000084970 -0.000000002193 -0.000000031680 0.000000046007 0.000001453265 +1 1645005919.894334554672 192 193 0.117461686956 -0.016398117007 -0.001090784968 -0.000555133742 -0.000926537311 0.007563804792 0.000000148505 -0.000000040661 0.000000003635 -0.000000017091 -0.000000022791 0.000000084172 -0.000000040661 0.000000020845 -0.000000001307 0.000000002552 0.000000010455 0.000000081570 0.000000003635 -0.000000001307 0.000000014106 -0.000000051158 -0.000000149559 -0.000000001627 -0.000000017091 0.000000002552 -0.000000051158 0.000000525853 0.000000542509 -0.000000035899 -0.000000022791 0.000000010455 -0.000000149559 0.000000542509 0.000001922636 0.000000041308 0.000000084172 0.000000081570 -0.000000001627 -0.000000035899 0.000000041308 0.000001463420 +1 1645005920.004122257233 193 194 0.127889889395 -0.014287189664 -0.018071779409 -0.000603843631 0.001403222454 0.004675372250 0.000000147336 -0.000000039680 0.000000003583 -0.000000015186 -0.000000019399 0.000000089698 -0.000000039680 0.000000020410 -0.000000001303 0.000000001664 0.000000009811 0.000000081704 0.000000003583 -0.000000001303 0.000000014293 -0.000000052484 -0.000000153094 -0.000000001827 -0.000000015186 0.000000001664 -0.000000052484 0.000000530498 0.000000561884 -0.000000039870 -0.000000019399 0.000000009811 -0.000000153094 0.000000561884 0.000001973145 0.000000046528 0.000000089698 0.000000081704 -0.000000001827 -0.000000039870 0.000000046528 0.000001493220 +1 1645005920.104864120483 194 195 0.116168811852 -0.013109825790 -0.012929698648 -0.000005389645 0.000738108287 0.001144078207 0.000000149213 -0.000000040106 0.000000003646 -0.000000015092 -0.000000021339 0.000000090319 -0.000000040106 0.000000020455 -0.000000001340 0.000000001571 0.000000010648 0.000000081937 0.000000003646 -0.000000001340 0.000000014645 -0.000000054570 -0.000000158509 -0.000000002004 -0.000000015092 0.000000001571 -0.000000054570 0.000000553075 0.000000590499 -0.000000041701 -0.000000021339 0.000000010648 -0.000000158509 0.000000590499 0.000002057287 0.000000048248 0.000000090319 0.000000081937 -0.000000002004 -0.000000041701 0.000000048248 0.000001514765 +1 1645005920.206741094589 195 196 0.116606271407 -0.013867701039 0.005805133595 0.000067675702 0.000577447854 -0.002394489670 0.000000149429 -0.000000040285 0.000000003646 -0.000000014456 -0.000000021481 0.000000087876 -0.000000040285 0.000000020545 -0.000000001350 0.000000001645 0.000000010775 0.000000083356 0.000000003646 -0.000000001350 0.000000014509 -0.000000053933 -0.000000159305 -0.000000002159 -0.000000014456 0.000000001645 -0.000000053933 0.000000554499 0.000000591697 -0.000000038609 -0.000000021481 0.000000010775 -0.000000159305 0.000000591697 0.000002096327 0.000000049808 0.000000087876 0.000000083356 -0.000000002159 -0.000000038609 0.000000049808 0.000001532263 +1 1645005920.306432723999 196 197 0.118459510620 -0.015741762731 0.019941986760 -0.000233586310 -0.001780831484 -0.005334381966 0.000000148573 -0.000000040237 0.000000003215 -0.000000013105 -0.000000017644 0.000000084415 -0.000000040237 0.000000020546 -0.000000001290 0.000000001926 0.000000010386 0.000000084228 0.000000003215 -0.000000001290 0.000000014559 -0.000000054075 -0.000000162036 -0.000000003226 -0.000000013105 0.000000001926 -0.000000054075 0.000000552256 0.000000602098 -0.000000029736 -0.000000017644 0.000000010386 -0.000000162036 0.000000602098 0.000002151812 0.000000061306 0.000000084415 0.000000084228 -0.000000003226 -0.000000029736 0.000000061306 0.000001531623 +1 1645005920.396054744720 197 198 0.108833768936 -0.014196265349 0.014957699203 0.000784882132 -0.000440774433 -0.007383134271 0.000000148605 -0.000000040325 0.000000003132 -0.000000012207 -0.000000013711 0.000000083131 -0.000000040325 0.000000020417 -0.000000001368 0.000000002111 0.000000010502 0.000000083174 0.000000003132 -0.000000001368 0.000000015049 -0.000000056948 -0.000000169586 -0.000000004564 -0.000000012207 0.000000002111 -0.000000056948 0.000000566345 0.000000643775 -0.000000023618 -0.000000013711 0.000000010502 -0.000000169586 0.000000643775 0.000002263566 0.000000078674 0.000000083131 0.000000083174 -0.000000004564 -0.000000023618 0.000000078674 0.000001518540 +1 1645005920.505497217178 198 199 0.132822748645 -0.016271936175 -0.000340704148 0.000018558141 0.001135204195 -0.009919704392 0.000000147651 -0.000000040083 0.000000002848 -0.000000010888 -0.000000007401 0.000000081858 -0.000000040083 0.000000020309 -0.000000001301 0.000000001666 0.000000008869 0.000000083786 0.000000002848 -0.000000001301 0.000000015367 -0.000000058874 -0.000000175370 -0.000000004773 -0.000000010888 0.000000001666 -0.000000058874 0.000000570999 0.000000675795 -0.000000023893 -0.000000007401 0.000000008869 -0.000000175370 0.000000675795 0.000002351570 0.000000082187 0.000000081858 0.000000083786 -0.000000004773 -0.000000023893 0.000000082187 0.000001524068 \ No newline at end of file From c2355fd95a18d80ed051b5c8db72ce92f8fa163f Mon Sep 17 00:00:00 2001 From: JokerJohn <2022087641@qq.com> Date: Tue, 14 Jan 2025 19:47:30 +0800 Subject: [PATCH 19/52] add IncrementalFixedLagExample to show the issue #1452 --- gtsam_unstable/examples/IncrementalFixedLagSmootherExample.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam_unstable/examples/IncrementalFixedLagSmootherExample.cpp b/gtsam_unstable/examples/IncrementalFixedLagSmootherExample.cpp index 19f2cc6ab..ede367f89 100644 --- a/gtsam_unstable/examples/IncrementalFixedLagSmootherExample.cpp +++ b/gtsam_unstable/examples/IncrementalFixedLagSmootherExample.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010-2020, Georgia Tech Research Corporation, + * GTSAM Copyright 2010-2025, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) From 04e04eed52174ca2ad585563951b609079b47fc5 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 15 Jan 2025 00:15:34 -0500 Subject: [PATCH 20/52] inlined ExpmapTranslation --- gtsam/geometry/Pose3.cpp | 74 ++++++++++++++++------------------- gtsam/geometry/Pose3.h | 17 -------- gtsam/navigation/NavState.cpp | 50 +++++++++++++++-------- 3 files changed, 68 insertions(+), 73 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 6d9256f93..9c3bd7733 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -183,16 +183,37 @@ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi) { // Get angular velocity omega and translational velocity v from twist xi const Vector3 w = xi.head<3>(), v = xi.tail<3>(); - // Compute rotation using Expmap - Matrix3 Jr; - Rot3 R = Rot3::Expmap(w, Hxi ? &Jr : nullptr); + // Instantiate functor for Dexp-related operations: + const bool nearZero = (w.dot(w) <= 1e-5); + const so3::DexpFunctor local(w, nearZero); - // Compute translation and optionally its Jacobian Q in w - // The Jacobian in v is the right Jacobian Jr of SO(3), which we already have. - Matrix3 Q; - const Vector3 t = ExpmapTranslation(w, v, Hxi ? &Q : nullptr); + // Compute rotation using Expmap +#ifdef GTSAM_USE_QUATERNIONS + const Rot3 R = traits::Expmap(v); +#else + const Rot3 R(local.expmap()); +#endif + + // The translation t = local.leftJacobian() * v. + // Here we call applyLeftJacobian, which is faster if you don't need + // Jacobians, and returns Jacobian of t with respect to w if asked. + // NOTE(Frank): t = applyLeftJacobian(v) does the same as the intuitive formulas + // t_parallel = w * w.dot(v); // translation parallel to axis + // w_cross_v = w.cross(v); // translation orthogonal to axis + // t = (w_cross_v - Rot3::Expmap(w) * w_cross_v + t_parallel) / theta2; + // but functor does not need R, deals automatically with the case where theta2 + // is near zero, and also gives us the machinery for the Jacobians. + Matrix3 H; + const Vector3 t = local.applyLeftJacobian(v, Hxi ? &H : nullptr); if (Hxi) { + // The Jacobian of expmap is given by the right Jacobian of SO(3): + const Matrix3 Jr = local.rightJacobian(); + // We multiply H, the derivative of applyLeftJacobian in omega, with + // X = Jr * Jl^{-1}, + // which translates from left to right for our right expmap convention: + const Matrix3 X = Jr * local.leftJacobianInverse(); + const Matrix3 Q = X * H; *Hxi << Jr, Z_3x3, // Q, Jr; } @@ -260,45 +281,18 @@ Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi, double nearZeroThreshold) { const auto w = xi.head<3>(); const auto v = xi.tail<3>(); - Matrix3 Q; - ExpmapTranslation(w, v, Q, {}, nearZeroThreshold); - return Q; -} - -/* ************************************************************************* */ -// NOTE(Frank): t = applyLeftJacobian(v) does the same as the intuitive formulas -// t_parallel = w * w.dot(v); // translation parallel to axis -// w_cross_v = w.cross(v); // translation orthogonal to axis -// t = (w_cross_v - Rot3::Expmap(w) * w_cross_v + t_parallel) / theta2; -// but functor does not need R, deals automatically with the case where theta2 -// is near zero, and also gives us the machinery for the Jacobians. -Vector3 Pose3::ExpmapTranslation(const Vector3& w, const Vector3& v, - OptionalJacobian<3, 3> Q, - OptionalJacobian<3, 3> J, - double nearZeroThreshold) { - const double theta2 = w.dot(w); - bool nearZero = (theta2 <= nearZeroThreshold); // Instantiate functor for Dexp-related operations: + bool nearZero = (w.dot(w) <= nearZeroThreshold); so3::DexpFunctor local(w, nearZero); - // Call applyLeftJacobian which is faster than local.leftJacobian() * v if you - // don't need Jacobians, and returns Jacobian of t with respect to w if asked. + // Call applyLeftJacobian to get its Jacobian Matrix3 H; - Vector t = local.applyLeftJacobian(v, Q ? &H : nullptr); + local.applyLeftJacobian(v, H); - // We return Jacobians for use in Expmap, so we multiply with X, that - // translates from left to right for our right expmap convention: - if (Q) { - Matrix3 X = local.rightJacobian() * local.leftJacobianInverse(); - *Q = X * H; - } - - if (J) { - *J = local.rightJacobian(); // = X * local.leftJacobian(); - } - - return t; + // Multiply with X, translates from left to right for our expmap convention: + const Matrix3 X = local.rightJacobian() * local.leftJacobianInverse(); + return X * H; } /* ************************************************************************* */ diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index e4896ae26..af14ac82c 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -220,27 +220,10 @@ public: * (see Chirikjian11book2, pg 44, eq 10.95. * The closed-form formula is identical to formula 102 in Barfoot14tro where * Q_l of the SE3 Expmap left derivative matrix is given. - * This is the Jacobian of ExpmapTranslation and computed there. */ static Matrix3 ComputeQforExpmapDerivative( const Vector6& xi, double nearZeroThreshold = 1e-5); - /** - * Compute the translation part of the exponential map, with Jacobians. - * @param w 3D angular velocity - * @param v 3D velocity - * @param Q Optionally, compute 3x3 Jacobian wrpt w - * @param J Optionally, compute 3x3 Jacobian wrpt v = right Jacobian of SO(3) - * @param nearZeroThreshold threshold for small values - * @note This function returns Jacobians Q and J corresponding to the bottom - * blocks of the SE(3) exponential, and translated from left to right from the - * applyLeftJacobian Jacobians. - */ - static Vector3 ExpmapTranslation(const Vector3& w, const Vector3& v, - OptionalJacobian<3, 3> Q = {}, - OptionalJacobian<3, 3> J = {}, - double nearZeroThreshold = 1e-5); - using LieGroup::inverse; // version with derivative /** diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index bd482132e..d033c96e8 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -117,20 +117,29 @@ NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi) { // Get angular velocity w and components rho (for t) and nu (for v) from xi Vector3 w = xi.head<3>(), rho = xi.segment<3>(3), nu = xi.tail<3>(); - // Compute rotation using Expmap - Matrix3 Jr; - Rot3 R = Rot3::Expmap(w, Hxi ? &Jr : nullptr); + // Instantiate functor for Dexp-related operations: + const bool nearZero = (w.dot(w) <= 1e-5); + const so3::DexpFunctor local(w, nearZero); - // Compute translations and optionally their Jacobians Q in w - // The Jacobians with respect to rho and nu are equal to Jr - Matrix3 Qt, Qv; - Vector3 t = Pose3::ExpmapTranslation(w, rho, Hxi ? &Qt : nullptr); - Vector3 v = Pose3::ExpmapTranslation(w, nu, Hxi ? &Qv : nullptr); + // Compute rotation using Expmap +#ifdef GTSAM_USE_QUATERNIONS + const Rot3 R = traits::Expmap(v); +#else + const Rot3 R(local.expmap()); +#endif + + // Compute translation and velocity. See Pose3::Expmap + Matrix3 H_t_w, H_v_w; + const Vector3 t = local.applyLeftJacobian(rho, Hxi ? &H_t_w : nullptr); + const Vector3 v = local.applyLeftJacobian(nu, Hxi ? &H_v_w : nullptr); if (Hxi) { + // See Pose3::Expamp for explanation of the Jacobians + const Matrix3 Jr = local.rightJacobian(); + const Matrix3 X = Jr * local.leftJacobianInverse(); *Hxi << Jr, Z_3x3, Z_3x3, // - Qt, Jr, Z_3x3, // - Qv, Z_3x3, Jr; + X * H_t_w, Jr, Z_3x3, // + X * H_v_w, Z_3x3, Jr; } return NavState(R, t, v); @@ -231,11 +240,21 @@ Matrix9 NavState::LogmapDerivative(const NavState& state) { const Vector3 w = xi.head<3>(); Vector3 rho = xi.segment<3>(3); Vector3 nu = xi.tail<3>(); - - Matrix3 Qt, Qv; - const Rot3 R = Rot3::Expmap(w); - Pose3::ExpmapTranslation(w, rho, Qt); - Pose3::ExpmapTranslation(w, nu, Qv); + + // Instantiate functor for Dexp-related operations: + const bool nearZero = (w.dot(w) <= 1e-5); + const so3::DexpFunctor local(w, nearZero); + + // Call applyLeftJacobian to get its Jacobians + Matrix3 H_t_w, H_v_w; + local.applyLeftJacobian(rho, H_t_w); + local.applyLeftJacobian(nu, H_v_w); + + // Multiply with X, translates from left to right for our expmap convention: + const Matrix3 X = local.rightJacobian() * local.leftJacobianInverse(); + const Matrix3 Qt = X * H_t_w; + const Matrix3 Qv = X * H_v_w; + const Matrix3 Jw = Rot3::LogmapDerivative(w); const Matrix3 Qt2 = -Jw * Qt * Jw; const Matrix3 Qv2 = -Jw * Qv * Jw; @@ -247,7 +266,6 @@ Matrix9 NavState::LogmapDerivative(const NavState& state) { return J; } - //------------------------------------------------------------------------------ NavState NavState::ChartAtOrigin::Retract(const Vector9& xi, ChartJacobian Hxi) { From 3743b562c1501e7c8a8fb3ac2b5614489c313b24 Mon Sep 17 00:00:00 2001 From: JokerJohn <2022087641@qq.com> Date: Wed, 15 Jan 2025 13:17:21 +0800 Subject: [PATCH 21/52] Move the example to the appropriate folder, and modify the code for reading files and adding factors. --- .../IncrementalFixedLagSmootherExample.cpp | 33 ++- .../IncrementalFixedLagSmootherTestData.txt | 200 ------------------ 2 files changed, 16 insertions(+), 217 deletions(-) rename {gtsam_unstable/examples => examples}/IncrementalFixedLagSmootherExample.cpp (92%) delete mode 100644 gtsam_unstable/examples/IncrementalFixedLagSmootherTestData.txt diff --git a/gtsam_unstable/examples/IncrementalFixedLagSmootherExample.cpp b/examples/IncrementalFixedLagSmootherExample.cpp similarity index 92% rename from gtsam_unstable/examples/IncrementalFixedLagSmootherExample.cpp rename to examples/IncrementalFixedLagSmootherExample.cpp index ede367f89..73beb6a75 100644 --- a/gtsam_unstable/examples/IncrementalFixedLagSmootherExample.cpp +++ b/examples/IncrementalFixedLagSmootherExample.cpp @@ -11,9 +11,9 @@ /** * @file IncrementalFixedLagExample.cpp -* @brief An incremental fixed-lag smoother in GTSAM using real-world data. +* @brief Example of incremental fixed-lag smoother using real-world data. * @author Xiangcheng Hu (xhubd@connect.ust.hk), Frank Dellaert, Kevin Doherty -* @date Janaury 2025 +* @date Janaury 15, 2025 * * Key objectives: * - Validate `IncrementalFixedLagSmoother` functionality with real-world data. @@ -58,7 +58,6 @@ #include #include #include -#include #include #include #include @@ -146,12 +145,9 @@ void saveG2oGraph(const NonlinearFactorGraph &graph, const Values &estimate, * 5) Save the resulting factor graph and estimate of the last sliding window to a .g2o file. */ int main() { - // Path to the test data file - ifstream infile("IncrementalFixedLagSmootherTestData.txt"); - if (!infile.is_open()) { - cerr << "Failed to open factor graph file: IncrementalFixedLagSmootherTestData.txt" << endl; - return -1; - } + string factor_loc = findExampleDataFile("issue1452.txt"); + ifstream factor_file(factor_loc.c_str()); + cout << "Reading factors data file: " << factor_loc << endl; // Configure ISAM2 parameters for the fixed-lag smoother ISAM2Params isamParameters; @@ -172,15 +168,16 @@ int main() { FixedLagSmoother::KeyTimestampMap newTimestamps; // For tracking the latest estimate of all states in the sliding window Values currentEstimate; - Pose3 lastPose; // Used to provide an initial guess for new poses + Pose3 lastPose; // Read and process each line in the factor graph file string line; int lineCount = 0; - while (getline(infile, line)) { + while (getline(factor_file, line)) { if (line.empty()) continue; // Skip empty lines cout << "\n======================== Processing line " << ++lineCount << " =========================" << endl; + istringstream iss(line); int factorType; iss >> factorType; @@ -198,7 +195,7 @@ int main() { Matrix6 cov = readCovarianceMatrix(iss); auto noise = noiseModel::Gaussian::Covariance(cov); // Add prior factor - newFactors.add(PriorFactor(X(key), pose, noise)); + newFactors.addPrior(X(key), pose, noise); cout << "Add PRIOR factor on key " << key << endl; // Provide initial guess if not already in the graph if (!newValues.exists(X(key))) { @@ -219,7 +216,7 @@ int main() { Matrix6 cov = readCovarianceMatrix(iss); auto noise = noiseModel::Gaussian::Covariance(cov); // Add between factor - newFactors.add(BetweenFactor(X(key1), X(key2), relativePose, noise)); + newFactors.emplace_shared>(X(key1), X(key2), relativePose, noise); cout << "Add BETWEEN factor: " << key1 << " -> " << key2 << endl; // Provide an initial guess if needed if (!newValues.exists(X(key2))) { @@ -256,6 +253,7 @@ int main() { Key lastKey = currentEstimate.keys().back(); lastPose = currentEstimate.at(lastKey); } + // Clear containers for the next iteration newFactors.resize(0); newValues.clear(); @@ -265,9 +263,10 @@ int main() { } } - // Save final graph and estimates to a g2o file // The results of the last sliding window are saved to a g2o file for visualization or further analysis. - saveG2oGraph(smoother.getFactors(), currentEstimate, path + "isam", lineCount); - infile.close(); + saveG2oGraph(smoother.getFactors(), currentEstimate, "isam", lineCount); + factor_file.close(); + return 0; -} \ No newline at end of file +} + diff --git a/gtsam_unstable/examples/IncrementalFixedLagSmootherTestData.txt b/gtsam_unstable/examples/IncrementalFixedLagSmootherTestData.txt deleted file mode 100644 index 518f1fe3e..000000000 --- a/gtsam_unstable/examples/IncrementalFixedLagSmootherTestData.txt +++ /dev/null @@ -1,200 +0,0 @@ -0 1645005900.600844860077 0 6.217194869853 0.125917563436 0.783594893995 -0.033789843270 0.047134300047 0.266189953411 0.000021415732 0.000001074791 -0.000000147245 -0.000000847030 0.000019167883 0.000016718464 0.000001074791 0.000001605304 -0.000000003066 -0.000001377764 0.000000939474 0.000008913959 -0.000000147245 -0.000000003066 0.000001484791 -0.000000768622 -0.000007854923 -0.000000092444 -0.000000847030 -0.000001377764 -0.000000768622 0.000104453167 0.000003272897 -0.000007611301 0.000019167883 0.000000939474 -0.000007854923 0.000003272897 0.000160225289 0.000014846515 0.000016718464 0.000008913959 -0.000000092444 -0.000007611301 0.000014846515 0.000157951799 -1 1645005900.701431751251 0 1 0.005593021030 0.001526481838 -0.001566024571 -0.000225927310 0.000128593690 -0.003791450430 0.000000179286 -0.000000039948 0.000000006043 -0.000000004575 -0.000000022632 0.000000091709 -0.000000039948 0.000000018979 -0.000000001546 0.000000001926 0.000000006078 -0.000000017221 0.000000006043 -0.000000001546 0.000000015351 -0.000000013913 0.000000001925 0.000000001075 -0.000000004575 0.000000001926 -0.000000013913 0.000000561628 -0.000000028062 0.000000012214 -0.000000022632 0.000000006078 0.000000001925 -0.000000028062 0.000000490365 -0.000000005530 0.000000091709 -0.000000017221 0.000000001075 0.000000012214 -0.000000005530 0.000000550990 -1 1645005900.799895286560 1 2 0.005283034558 0.001722784230 -0.000367567191 0.000386621238 0.000182655752 -0.003429626268 0.000000162458 -0.000000036144 0.000000005112 -0.000000002457 -0.000000020766 0.000000082373 -0.000000036144 0.000000017835 -0.000000001282 0.000000001448 0.000000005345 -0.000000014851 0.000000005112 -0.000000001282 0.000000014551 -0.000000013437 0.000000001790 0.000000000998 -0.000000002457 0.000000001448 -0.000000013437 0.000000523714 -0.000000027843 0.000000011143 -0.000000020766 0.000000005345 0.000000001790 -0.000000027843 0.000000456118 -0.000000005134 0.000000082373 -0.000000014851 0.000000000998 0.000000011143 -0.000000005134 0.000000510306 -1 1645005900.902594566345 2 3 0.003030780414 0.000830079823 0.000494312020 -0.000020397824 0.000094347098 -0.002984943110 0.000000145900 -0.000000032354 0.000000004126 -0.000000001486 -0.000000018203 0.000000070890 -0.000000032354 0.000000016529 -0.000000001105 0.000000001395 0.000000004540 -0.000000012806 0.000000004126 -0.000000001105 0.000000014132 -0.000000012278 0.000000001993 0.000000000686 -0.000000001486 0.000000001395 -0.000000012278 0.000000469953 -0.000000025678 0.000000008948 -0.000000018203 0.000000004540 0.000000001993 -0.000000025678 0.000000410806 -0.000000003776 0.000000070890 -0.000000012806 0.000000000686 0.000000008948 -0.000000003776 0.000000445310 -1 1645005901.003520727158 3 4 -0.000206245469 -0.001102739599 0.000139509744 -0.000044223949 0.000133857483 -0.002842172502 0.000000130290 -0.000000028944 0.000000003338 -0.000000000544 -0.000000016340 0.000000062247 -0.000000028944 0.000000015436 -0.000000000943 0.000000001260 0.000000004005 -0.000000011220 0.000000003338 -0.000000000943 0.000000014049 -0.000000012189 0.000000001599 0.000000000394 -0.000000000544 0.000000001260 -0.000000012189 0.000000464881 -0.000000026439 0.000000008623 -0.000000016340 0.000000004005 0.000000001599 -0.000000026439 0.000000404384 -0.000000003057 0.000000062247 -0.000000011220 0.000000000394 0.000000008623 -0.000000003057 0.000000423376 -1 1645005901.101174592972 4 5 -0.001500063017 -0.002890410973 -0.000122347193 0.000014693626 0.000245148283 -0.002217688406 0.000000119368 -0.000000026625 0.000000002896 0.000000000371 -0.000000014770 0.000000055853 -0.000000026625 0.000000014763 -0.000000000823 0.000000000938 0.000000003581 -0.000000009513 0.000000002896 -0.000000000823 0.000000013680 -0.000000011988 0.000000001262 0.000000000298 0.000000000371 0.000000000938 -0.000000011988 0.000000464346 -0.000000027637 0.000000007687 -0.000000014770 0.000000003581 0.000000001262 -0.000000027637 0.000000402444 -0.000000002216 0.000000055853 -0.000000009513 0.000000000298 0.000000007687 -0.000000002216 0.000000418928 -1 1645005901.201069116592 5 6 0.001046374496 -0.004032584187 -0.000846599681 0.000321181584 -0.000094749466 -0.002057391696 0.000000110823 -0.000000024485 0.000000002417 0.000000001106 -0.000000012269 0.000000051317 -0.000000024485 0.000000014167 -0.000000000692 0.000000000635 0.000000003038 -0.000000008394 0.000000002417 -0.000000000692 0.000000013921 -0.000000011892 0.000000000364 0.000000000007 0.000000001106 0.000000000635 -0.000000011892 0.000000448440 -0.000000027796 0.000000007091 -0.000000012269 0.000000003038 0.000000000364 -0.000000027796 0.000000388841 -0.000000000851 0.000000051317 -0.000000008394 0.000000000007 0.000000007091 -0.000000000851 0.000000405686 -1 1645005901.302946805954 6 7 0.000793901956 -0.003389991954 -0.001184675507 -0.000316637882 0.000125753514 -0.001152057016 0.000000104712 -0.000000022968 0.000000001967 0.000000001772 -0.000000010574 0.000000048337 -0.000000022968 0.000000013707 -0.000000000599 0.000000000391 0.000000002734 -0.000000007830 0.000000001967 -0.000000000599 0.000000014353 -0.000000012287 -0.000000001119 -0.000000000386 0.000000001772 0.000000000391 -0.000000012287 0.000000430486 -0.000000025901 0.000000008441 -0.000000010574 0.000000002734 -0.000000001119 -0.000000025901 0.000000374002 0.000000000168 0.000000048337 -0.000000007830 -0.000000000386 0.000000008441 0.000000000168 0.000000386878 -1 1645005901.403146505356 7 8 0.000422785492 -0.001577894861 -0.000916262031 0.000148651815 -0.000064627432 -0.001220538595 0.000000100167 -0.000000021927 0.000000002289 0.000000000677 -0.000000011203 0.000000045605 -0.000000021927 0.000000013460 -0.000000000705 0.000000000708 0.000000002868 -0.000000007296 0.000000002289 -0.000000000705 0.000000014003 -0.000000011457 -0.000000000961 -0.000000000148 0.000000000677 0.000000000708 -0.000000011457 0.000000410964 -0.000000025258 0.000000008570 -0.000000011203 0.000000002868 -0.000000000961 -0.000000025258 0.000000357815 -0.000000000299 0.000000045605 -0.000000007296 -0.000000000148 0.000000008570 -0.000000000299 0.000000367628 -1 1645005901.502384662628 8 9 -0.002329688882 -0.000140222280 0.000009892231 -0.000239651077 0.000253304248 -0.001105202980 0.000000095969 -0.000000020860 0.000000002507 -0.000000000886 -0.000000012048 0.000000043109 -0.000000020860 0.000000013064 -0.000000000726 0.000000001166 0.000000002962 -0.000000007369 0.000000002507 -0.000000000726 0.000000013480 -0.000000010270 -0.000000000517 0.000000000184 -0.000000000886 0.000000001166 -0.000000010270 0.000000387773 -0.000000024930 0.000000007342 -0.000000012048 0.000000002962 -0.000000000517 -0.000000024930 0.000000338486 -0.000000001780 0.000000043109 -0.000000007369 0.000000000184 0.000000007342 -0.000000001780 0.000000347313 -1 1645005901.599597692490 9 10 -0.004026581327 0.000969420632 0.001171132830 0.000030066830 -0.000042684471 -0.001078015407 0.000000092798 -0.000000020050 0.000000002088 -0.000000000514 -0.000000011837 0.000000040468 -0.000000020050 0.000000012604 -0.000000000661 0.000000001084 0.000000002794 -0.000000007809 0.000000002088 -0.000000000661 0.000000013337 -0.000000009999 -0.000000000185 0.000000000097 -0.000000000514 0.000000001084 -0.000000009999 0.000000364337 -0.000000023644 0.000000006385 -0.000000011837 0.000000002794 -0.000000000185 -0.000000023644 0.000000320215 -0.000000002428 0.000000040468 -0.000000007809 0.000000000097 0.000000006385 -0.000000002428 0.000000324189 -1 1645005901.700814247131 10 11 -0.003099084350 0.000842028850 0.001191700627 -0.000180302292 -0.000304391953 -0.000943844689 0.000000090213 -0.000000019520 0.000000001928 -0.000000000067 -0.000000011416 0.000000037331 -0.000000019520 0.000000012352 -0.000000000665 0.000000000889 0.000000002684 -0.000000007936 0.000000001928 -0.000000000665 0.000000013403 -0.000000009790 0.000000000272 0.000000000027 -0.000000000067 0.000000000889 -0.000000009790 0.000000343339 -0.000000021912 0.000000005787 -0.000000011416 0.000000002684 0.000000000272 -0.000000021912 0.000000304258 -0.000000002229 0.000000037331 -0.000000007936 0.000000000027 0.000000005787 -0.000000002229 0.000000301522 -1 1645005901.799977779388 11 12 -0.001119124717 0.000911769018 0.000896053010 0.000109146445 0.000004749543 -0.001769999687 0.000000088504 -0.000000019214 0.000000002010 -0.000000000364 -0.000000011599 0.000000035324 -0.000000019214 0.000000012281 -0.000000000663 0.000000000986 0.000000002726 -0.000000007878 0.000000002010 -0.000000000663 0.000000013725 -0.000000009806 0.000000000042 0.000000000058 -0.000000000364 0.000000000986 -0.000000009806 0.000000328413 -0.000000018998 0.000000005385 -0.000000011599 0.000000002726 0.000000000042 -0.000000018998 0.000000293251 -0.000000002089 0.000000035324 -0.000000007878 0.000000000058 0.000000005385 -0.000000002089 0.000000286449 -1 1645005901.900312423706 12 13 -0.000402732963 0.000242598646 -0.000072269012 0.000055999966 0.000123729242 -0.002073753983 0.000000087170 -0.000000018926 0.000000002027 -0.000000000440 -0.000000011562 0.000000034056 -0.000000018926 0.000000012165 -0.000000000686 0.000000001149 0.000000002703 -0.000000007746 0.000000002027 -0.000000000686 0.000000013717 -0.000000009355 -0.000000000107 0.000000000029 -0.000000000440 0.000000001149 -0.000000009355 0.000000315929 -0.000000016862 0.000000005085 -0.000000011562 0.000000002703 -0.000000000107 -0.000000016862 0.000000284663 -0.000000002049 0.000000034056 -0.000000007746 0.000000000029 0.000000005085 -0.000000002049 0.000000275236 -1 1645005901.999710559845 13 14 -0.000353031834 0.000520456797 -0.000885039005 0.000175471614 0.000226437074 -0.001781941944 0.000000085576 -0.000000018640 0.000000002007 -0.000000000556 -0.000000011265 0.000000032750 -0.000000018640 0.000000011971 -0.000000000692 0.000000001200 0.000000002700 -0.000000007525 0.000000002007 -0.000000000692 0.000000013341 -0.000000008639 0.000000000083 -0.000000000060 -0.000000000556 0.000000001200 -0.000000008639 0.000000304286 -0.000000016247 0.000000004967 -0.000000011265 0.000000002700 0.000000000083 -0.000000016247 0.000000275471 -0.000000002124 0.000000032750 -0.000000007525 -0.000000000060 0.000000004967 -0.000000002124 0.000000264734 -1 1645005902.100522041321 14 15 -0.000755912182 0.000162757993 -0.000940218374 0.000142873662 0.000156455499 -0.000999359978 0.000000084547 -0.000000018524 0.000000002007 -0.000000001342 -0.000000011413 0.000000031398 -0.000000018524 0.000000011963 -0.000000000637 0.000000001412 0.000000002758 -0.000000007386 0.000000002007 -0.000000000637 0.000000012995 -0.000000008153 0.000000000029 -0.000000000020 -0.000000001342 0.000000001412 -0.000000008153 0.000000294679 -0.000000015478 0.000000004679 -0.000000011413 0.000000002758 0.000000000029 -0.000000015478 0.000000268528 -0.000000002296 0.000000031398 -0.000000007386 -0.000000000020 0.000000004679 -0.000000002296 0.000000256957 -1 1645005902.203018426895 15 16 -0.000439249231 -0.000178254566 -0.000437587780 -0.000084581729 0.000049443904 -0.000079088693 0.000000083829 -0.000000018377 0.000000001938 -0.000000001650 -0.000000011605 0.000000030281 -0.000000018377 0.000000011902 -0.000000000597 0.000000001442 0.000000002713 -0.000000007253 0.000000001938 -0.000000000597 0.000000013076 -0.000000008171 0.000000000037 0.000000000101 -0.000000001650 0.000000001442 -0.000000008171 0.000000287437 -0.000000014704 0.000000003963 -0.000000011605 0.000000002713 0.000000000037 -0.000000014704 0.000000263922 -0.000000002586 0.000000030281 -0.000000007253 0.000000000101 0.000000003963 -0.000000002586 0.000000250716 -1 1645005902.301831007004 16 17 0.000298386384 -0.000560355605 -0.000119106950 0.000156455585 -0.000010964095 -0.000026764071 0.000000083200 -0.000000018095 0.000000001879 -0.000000001309 -0.000000011836 0.000000029821 -0.000000018095 0.000000011725 -0.000000000623 0.000000001381 0.000000002667 -0.000000006986 0.000000001879 -0.000000000623 0.000000013155 -0.000000008373 0.000000000404 0.000000000134 -0.000000001309 0.000000001381 -0.000000008373 0.000000283842 -0.000000014501 0.000000003889 -0.000000011836 0.000000002667 0.000000000404 -0.000000014501 0.000000261244 -0.000000002864 0.000000029821 -0.000000006986 0.000000000134 0.000000003889 -0.000000002864 0.000000247086 -1 1645005902.400156021118 17 18 0.000179886078 -0.000459777112 -0.000358763747 -0.000153694839 0.000090517348 0.000016912731 0.000000081953 -0.000000017650 0.000000001880 -0.000000001460 -0.000000011911 0.000000029712 -0.000000017650 0.000000011542 -0.000000000621 0.000000001450 0.000000002686 -0.000000006772 0.000000001880 -0.000000000621 0.000000012815 -0.000000008204 0.000000000400 0.000000000134 -0.000000001460 0.000000001450 -0.000000008204 0.000000279237 -0.000000013903 0.000000003850 -0.000000011911 0.000000002686 0.000000000400 -0.000000013903 0.000000257257 -0.000000002996 0.000000029712 -0.000000006772 0.000000000134 0.000000003850 -0.000000002996 0.000000243174 -1 1645005902.500535726547 18 19 -0.000677739243 -0.000661804764 -0.000786983512 0.000083215128 -0.000114303323 0.000518990892 0.000000081125 -0.000000017513 0.000000001940 -0.000000001754 -0.000000011942 0.000000029299 -0.000000017513 0.000000011479 -0.000000000613 0.000000001489 0.000000002719 -0.000000006813 0.000000001940 -0.000000000613 0.000000012630 -0.000000007967 0.000000000014 0.000000000174 -0.000000001754 0.000000001489 -0.000000007967 0.000000273527 -0.000000013282 0.000000003515 -0.000000011942 0.000000002719 0.000000000014 -0.000000013282 0.000000252050 -0.000000003271 0.000000029299 -0.000000006813 0.000000000174 0.000000003515 -0.000000003271 0.000000239150 -1 1645005902.600610494614 19 20 -0.001398677644 -0.000281431524 -0.000006292086 -0.000149844144 0.000155174368 0.000310385345 0.000000081164 -0.000000017520 0.000000001984 -0.000000001776 -0.000000012440 0.000000028693 -0.000000017520 0.000000011567 -0.000000000605 0.000000001477 0.000000002823 -0.000000006788 0.000000001984 -0.000000000605 0.000000012243 -0.000000007646 -0.000000000218 0.000000000207 -0.000000001776 0.000000001477 -0.000000007646 0.000000270203 -0.000000013042 0.000000003281 -0.000000012440 0.000000002823 -0.000000000218 -0.000000013042 0.000000249116 -0.000000003680 0.000000028693 -0.000000006788 0.000000000207 0.000000003281 -0.000000003680 0.000000237404 -1 1645005902.700582027435 20 21 -0.001420482214 -0.000263135886 -0.000053375571 -0.000116154210 -0.000141423619 0.000057516294 0.000000080767 -0.000000017432 0.000000001927 -0.000000001700 -0.000000012707 0.000000028372 -0.000000017432 0.000000011550 -0.000000000579 0.000000001468 0.000000002900 -0.000000006723 0.000000001927 -0.000000000579 0.000000011880 -0.000000007357 -0.000000000236 0.000000000169 -0.000000001700 0.000000001468 -0.000000007357 0.000000268424 -0.000000012714 0.000000003151 -0.000000012707 0.000000002900 -0.000000000236 -0.000000012714 0.000000247501 -0.000000003752 0.000000028372 -0.000000006723 0.000000000169 0.000000003151 -0.000000003752 0.000000236011 -1 1645005902.802828073502 21 22 -0.001522845641 -0.000074472171 0.000173559055 -0.000051052057 -0.000127920939 -0.000114638340 0.000000080331 -0.000000017568 0.000000001879 -0.000000001733 -0.000000012773 0.000000027688 -0.000000017568 0.000000011534 -0.000000000541 0.000000001430 0.000000002960 -0.000000006720 0.000000001879 -0.000000000541 0.000000011888 -0.000000007198 -0.000000000298 0.000000000147 -0.000000001733 0.000000001430 -0.000000007198 0.000000266369 -0.000000012230 0.000000003032 -0.000000012773 0.000000002960 -0.000000000298 -0.000000012230 0.000000246106 -0.000000003739 0.000000027688 -0.000000006720 0.000000000147 0.000000003032 -0.000000003739 0.000000233014 -1 1645005902.900591373444 22 23 -0.000662864106 -0.000366807912 -0.000191751859 0.000099874707 0.000044070199 -0.000205737831 0.000000079920 -0.000000017596 0.000000001894 -0.000000001895 -0.000000012932 0.000000026867 -0.000000017596 0.000000011473 -0.000000000544 0.000000001444 0.000000002996 -0.000000006939 0.000000001894 -0.000000000544 0.000000011681 -0.000000007215 -0.000000000519 0.000000000192 -0.000000001895 0.000000001444 -0.000000007215 0.000000264971 -0.000000011324 0.000000002987 -0.000000012932 0.000000002996 -0.000000000519 -0.000000011324 0.000000246113 -0.000000003809 0.000000026867 -0.000000006939 0.000000000192 0.000000002987 -0.000000003809 0.000000230070 -1 1645005902.999960184097 23 24 0.000586459972 -0.000383883637 -0.000657440721 0.000025502157 0.000173096398 -0.000657422729 0.000000078961 -0.000000017438 0.000000001897 -0.000000001864 -0.000000012901 0.000000026255 -0.000000017438 0.000000011356 -0.000000000570 0.000000001429 0.000000002941 -0.000000007077 0.000000001897 -0.000000000570 0.000000011277 -0.000000007081 -0.000000000639 0.000000000279 -0.000000001864 0.000000001429 -0.000000007081 0.000000261573 -0.000000010356 0.000000003006 -0.000000012901 0.000000002941 -0.000000000639 -0.000000010356 0.000000244298 -0.000000003671 0.000000026255 -0.000000007077 0.000000000279 0.000000003006 -0.000000003671 0.000000226695 -1 1645005903.099860429764 24 25 0.001039569017 -0.000664462296 -0.000116140678 -0.000029188969 0.000121233801 -0.000658346529 0.000000078315 -0.000000017330 0.000000001958 -0.000000001934 -0.000000012838 0.000000025439 -0.000000017330 0.000000011376 -0.000000000569 0.000000001357 0.000000002951 -0.000000007067 0.000000001958 -0.000000000569 0.000000011058 -0.000000006788 -0.000000000667 0.000000000313 -0.000000001934 0.000000001357 -0.000000006788 0.000000257200 -0.000000009782 0.000000002843 -0.000000012838 0.000000002951 -0.000000000667 -0.000000009782 0.000000240971 -0.000000003535 0.000000025439 -0.000000007067 0.000000000313 0.000000002843 -0.000000003535 0.000000223420 -1 1645005903.201798200607 25 26 0.000799401277 -0.000715390089 -0.000135250388 -0.000005401473 0.000028317278 -0.000705012899 0.000000078273 -0.000000017284 0.000000001945 -0.000000002002 -0.000000012892 0.000000025018 -0.000000017284 0.000000011434 -0.000000000564 0.000000001376 0.000000002920 -0.000000007058 0.000000001945 -0.000000000564 0.000000011188 -0.000000006747 -0.000000000284 0.000000000359 -0.000000002002 0.000000001376 -0.000000006747 0.000000255919 -0.000000009831 0.000000002438 -0.000000012892 0.000000002920 -0.000000000284 -0.000000009831 0.000000239627 -0.000000003371 0.000000025018 -0.000000007058 0.000000000359 0.000000002438 -0.000000003371 0.000000222579 -1 1645005903.299434661865 26 27 -0.000113261770 0.000023693570 -0.000214375044 -0.000117559997 0.000124720787 -0.000596143614 0.000000078325 -0.000000017305 0.000000001849 -0.000000001888 -0.000000012882 0.000000024989 -0.000000017305 0.000000011476 -0.000000000557 0.000000001385 0.000000002888 -0.000000006966 0.000000001849 -0.000000000557 0.000000011561 -0.000000006807 0.000000000150 0.000000000342 -0.000000001888 0.000000001385 -0.000000006807 0.000000256583 -0.000000010009 0.000000002389 -0.000000012882 0.000000002888 0.000000000150 -0.000000010009 0.000000240356 -0.000000003042 0.000000024989 -0.000000006966 0.000000000342 0.000000002389 -0.000000003042 0.000000222999 -1 1645005903.399843454361 27 28 -0.000958900034 0.000637600325 -0.000214249917 -0.000223045645 -0.000071809246 0.000029335082 0.000000078344 -0.000000017286 0.000000001846 -0.000000001681 -0.000000012791 0.000000024604 -0.000000017286 0.000000011476 -0.000000000598 0.000000001367 0.000000002926 -0.000000006949 0.000000001846 -0.000000000598 0.000000012087 -0.000000006902 0.000000000466 0.000000000207 -0.000000001681 0.000000001367 -0.000000006902 0.000000256077 -0.000000009887 0.000000002620 -0.000000012791 0.000000002926 0.000000000466 -0.000000009887 0.000000240846 -0.000000002625 0.000000024604 -0.000000006949 0.000000000207 0.000000002620 -0.000000002625 0.000000222033 -1 1645005903.503298759460 28 29 -0.000594952889 0.000937675663 0.000350959010 -0.000134047994 0.000112568416 0.000532529091 0.000000077870 -0.000000017120 0.000000001906 -0.000000001471 -0.000000012767 0.000000024191 -0.000000017120 0.000000011459 -0.000000000663 0.000000001370 0.000000002979 -0.000000007036 0.000000001906 -0.000000000663 0.000000012464 -0.000000006974 0.000000000554 0.000000000084 -0.000000001471 0.000000001370 -0.000000006974 0.000000254804 -0.000000009282 0.000000002485 -0.000000012767 0.000000002979 0.000000000554 -0.000000009282 0.000000241012 -0.000000002108 0.000000024191 -0.000000007036 0.000000000084 0.000000002485 -0.000000002108 0.000000220716 -1 1645005903.600738525391 29 30 0.000794414323 0.000340891713 0.000722517724 0.000049267232 -0.000083827634 0.000511848534 0.000000077849 -0.000000017062 0.000000001944 -0.000000001282 -0.000000012931 0.000000024026 -0.000000017062 0.000000011499 -0.000000000672 0.000000001311 0.000000003010 -0.000000007173 0.000000001944 -0.000000000672 0.000000012279 -0.000000007093 0.000000000279 0.000000000086 -0.000000001282 0.000000001311 -0.000000007093 0.000000255543 -0.000000008713 0.000000002207 -0.000000012931 0.000000003010 0.000000000279 -0.000000008713 0.000000242511 -0.000000001802 0.000000024026 -0.000000007173 0.000000000086 0.000000002207 -0.000000001802 0.000000221948 -1 1645005903.702013492584 30 31 0.000724079430 -0.000003591352 0.000270122431 -0.000012632068 -0.000052326779 0.000440294104 0.000000077508 -0.000000016995 0.000000001961 -0.000000001247 -0.000000012890 0.000000023883 -0.000000016995 0.000000011411 -0.000000000627 0.000000001216 0.000000003013 -0.000000007068 0.000000001961 -0.000000000627 0.000000011934 -0.000000007149 -0.000000000010 0.000000000107 -0.000000001247 0.000000001216 -0.000000007149 0.000000254403 -0.000000008732 0.000000001995 -0.000000012890 0.000000003013 -0.000000000010 -0.000000008732 0.000000240911 -0.000000001885 0.000000023883 -0.000000007068 0.000000000107 0.000000001995 -0.000000001885 0.000000221291 -1 1645005903.800774335861 31 32 0.001067662490 -0.000510502525 -0.000301534356 0.000174567922 0.000065476921 -0.000090436674 0.000000077110 -0.000000016957 0.000000001949 -0.000000001388 -0.000000012566 0.000000024020 -0.000000016957 0.000000011352 -0.000000000587 0.000000001174 0.000000002958 -0.000000006851 0.000000001949 -0.000000000587 0.000000011556 -0.000000007045 -0.000000000114 0.000000000144 -0.000000001388 0.000000001174 -0.000000007045 0.000000252509 -0.000000008765 0.000000001947 -0.000000012566 0.000000002958 -0.000000000114 -0.000000008765 0.000000238424 -0.000000002141 0.000000024020 -0.000000006851 0.000000000144 0.000000001947 -0.000000002141 0.000000219630 -1 1645005903.899410247803 32 33 0.000817144389 -0.001122214966 -0.000884357956 -0.000001891347 0.000036718074 0.000252289181 0.000000077446 -0.000000017113 0.000000001881 -0.000000001342 -0.000000012421 0.000000024153 -0.000000017113 0.000000011435 -0.000000000562 0.000000001165 0.000000002871 -0.000000006813 0.000000001881 -0.000000000562 0.000000011375 -0.000000007060 -0.000000000151 0.000000000211 -0.000000001342 0.000000001165 -0.000000007060 0.000000252108 -0.000000008770 0.000000002126 -0.000000012421 0.000000002871 -0.000000000151 -0.000000008770 0.000000237324 -0.000000002342 0.000000024153 -0.000000006813 0.000000000211 0.000000002126 -0.000000002342 0.000000219157 -1 1645005903.999286413193 33 34 0.000286745156 -0.000755201087 -0.000483958459 0.000073063588 -0.000069345847 0.000131138848 0.000000077516 -0.000000017113 0.000000001949 -0.000000001353 -0.000000012656 0.000000023945 -0.000000017113 0.000000011475 -0.000000000564 0.000000001201 0.000000002844 -0.000000006895 0.000000001949 -0.000000000564 0.000000011242 -0.000000006940 -0.000000000110 0.000000000316 -0.000000001353 0.000000001201 -0.000000006940 0.000000251401 -0.000000008774 0.000000002151 -0.000000012656 0.000000002844 -0.000000000110 -0.000000008774 0.000000235544 -0.000000002538 0.000000023945 -0.000000006895 0.000000000316 0.000000002151 -0.000000002538 0.000000218001 -1 1645005904.101194381714 34 35 -0.000092729542 -0.000721176305 -0.000046422003 0.000080600128 -0.000098482186 0.000269102398 0.000000077508 -0.000000016949 0.000000002044 -0.000000001517 -0.000000012907 0.000000023822 -0.000000016949 0.000000011511 -0.000000000628 0.000000001343 0.000000002868 -0.000000006931 0.000000002044 -0.000000000628 0.000000010979 -0.000000006697 -0.000000000142 0.000000000368 -0.000000001517 0.000000001343 -0.000000006697 0.000000251510 -0.000000008827 0.000000002031 -0.000000012907 0.000000002868 -0.000000000142 -0.000000008827 0.000000234984 -0.000000002474 0.000000023822 -0.000000006931 0.000000000368 0.000000002031 -0.000000002474 0.000000217838 -1 1645005904.202036380768 35 36 0.000159501241 -0.000619989049 -0.000182200023 -0.000130638892 -0.000030290097 0.000441918495 0.000000077457 -0.000000016871 0.000000002049 -0.000000001653 -0.000000012863 0.000000024008 -0.000000016871 0.000000011440 -0.000000000666 0.000000001365 0.000000002895 -0.000000006831 0.000000002049 -0.000000000666 0.000000010881 -0.000000006644 -0.000000000477 0.000000000349 -0.000000001653 0.000000001365 -0.000000006644 0.000000252370 -0.000000009051 0.000000002132 -0.000000012863 0.000000002895 -0.000000000477 -0.000000009051 0.000000236368 -0.000000002172 0.000000024008 -0.000000006831 0.000000000349 0.000000002132 -0.000000002172 0.000000219216 -1 1645005904.301526308060 36 37 0.000327836677 -0.000353330603 -0.000334050825 -0.000048653573 -0.000017134077 0.000406296921 0.000000077743 -0.000000017107 0.000000002089 -0.000000001553 -0.000000013005 0.000000024196 -0.000000017107 0.000000011432 -0.000000000665 0.000000001162 0.000000002958 -0.000000006693 0.000000002089 -0.000000000665 0.000000010877 -0.000000006677 -0.000000000574 0.000000000388 -0.000000001553 0.000000001162 -0.000000006677 0.000000252991 -0.000000009267 0.000000002202 -0.000000013005 0.000000002958 -0.000000000574 -0.000000009267 0.000000237868 -0.000000002187 0.000000024196 -0.000000006693 0.000000000388 0.000000002202 -0.000000002187 0.000000220113 -1 1645005904.403170108795 37 38 0.000737980852 -0.000351033561 -0.000295585445 -0.000025607001 -0.000042435192 0.000431463920 0.000000077856 -0.000000017205 0.000000002133 -0.000000001519 -0.000000013172 0.000000024074 -0.000000017205 0.000000011434 -0.000000000607 0.000000001084 0.000000003078 -0.000000006696 0.000000002133 -0.000000000607 0.000000011189 -0.000000006857 -0.000000000557 0.000000000291 -0.000000001519 0.000000001084 -0.000000006857 0.000000252632 -0.000000009363 0.000000002170 -0.000000013172 0.000000003078 -0.000000000557 -0.000000009363 0.000000237409 -0.000000002500 0.000000024074 -0.000000006696 0.000000000291 0.000000002170 -0.000000002500 0.000000219313 -1 1645005904.501605749130 38 39 0.001088348237 -0.000613129911 0.000232566844 0.000179265756 -0.000069468820 -0.000092368701 0.000000077918 -0.000000017330 0.000000002087 -0.000000001727 -0.000000013339 0.000000023890 -0.000000017330 0.000000011400 -0.000000000554 0.000000001171 0.000000003171 -0.000000006655 0.000000002087 -0.000000000554 0.000000011497 -0.000000006951 -0.000000000628 0.000000000205 -0.000000001727 0.000000001171 -0.000000006951 0.000000252754 -0.000000009318 0.000000002086 -0.000000013339 0.000000003171 -0.000000000628 -0.000000009318 0.000000237371 -0.000000002624 0.000000023890 -0.000000006655 0.000000000205 0.000000002086 -0.000000002624 0.000000219216 -1 1645005904.599536895752 39 40 0.000468476145 -0.001272947114 0.000093562933 -0.000116852710 0.000115936099 0.000225755043 0.000000077895 -0.000000017350 0.000000001892 -0.000000001532 -0.000000013201 0.000000023949 -0.000000017350 0.000000011402 -0.000000000560 0.000000001138 0.000000003123 -0.000000006687 0.000000001892 -0.000000000560 0.000000011557 -0.000000006985 -0.000000000681 0.000000000172 -0.000000001532 0.000000001138 -0.000000006985 0.000000252449 -0.000000009335 0.000000002069 -0.000000013201 0.000000003123 -0.000000000681 -0.000000009335 0.000000237483 -0.000000002381 0.000000023949 -0.000000006687 0.000000000172 0.000000002069 -0.000000002381 0.000000218909 -1 1645005904.700893878937 40 41 0.000860573463 -0.001209131350 -0.000501116645 0.000157555541 0.000010319357 -0.000048830165 0.000000077189 -0.000000017111 0.000000001814 -0.000000001358 -0.000000012728 0.000000023995 -0.000000017111 0.000000011307 -0.000000000533 0.000000001084 0.000000002997 -0.000000006770 0.000000001814 -0.000000000533 0.000000011517 -0.000000007033 -0.000000000879 0.000000000155 -0.000000001358 0.000000001084 -0.000000007033 0.000000250581 -0.000000009543 0.000000002194 -0.000000012728 0.000000002997 -0.000000000879 -0.000000009543 0.000000236400 -0.000000002342 0.000000023995 -0.000000006770 0.000000000155 0.000000002194 -0.000000002342 0.000000217604 -1 1645005904.802378892899 41 42 0.000632138609 -0.000790062715 -0.000690771042 0.000034390734 -0.000109221344 0.000029091074 0.000000077001 -0.000000017060 0.000000001956 -0.000000001694 -0.000000012495 0.000000024027 -0.000000017060 0.000000011341 -0.000000000521 0.000000001130 0.000000002989 -0.000000006785 0.000000001956 -0.000000000521 0.000000011397 -0.000000006946 -0.000000000911 0.000000000171 -0.000000001694 0.000000001130 -0.000000006946 0.000000250277 -0.000000009613 0.000000002318 -0.000000012495 0.000000002989 -0.000000000911 -0.000000009613 0.000000236262 -0.000000002568 0.000000024027 -0.000000006785 0.000000000171 0.000000002318 -0.000000002568 0.000000218078 -1 1645005904.902300596237 42 43 0.000802579583 -0.000538433330 -0.000341299568 0.000007003048 -0.000032678783 -0.000429464507 0.000000077294 -0.000000017051 0.000000002005 -0.000000001928 -0.000000012622 0.000000024150 -0.000000017051 0.000000011388 -0.000000000568 0.000000001229 0.000000003054 -0.000000006816 0.000000002005 -0.000000000568 0.000000011301 -0.000000006823 -0.000000000888 0.000000000138 -0.000000001928 0.000000001229 -0.000000006823 0.000000251648 -0.000000009299 0.000000002429 -0.000000012622 0.000000003054 -0.000000000888 -0.000000009299 0.000000237177 -0.000000002580 0.000000024150 -0.000000006816 0.000000000138 0.000000002429 -0.000000002580 0.000000218935 -1 1645005904.999253749847 43 44 0.000501016980 0.000211957757 -0.000005361234 0.000063883092 -0.000036266373 -0.000504158455 0.000000077290 -0.000000017104 0.000000001982 -0.000000001896 -0.000000012750 0.000000024173 -0.000000017104 0.000000011394 -0.000000000596 0.000000001289 0.000000003111 -0.000000006794 0.000000001982 -0.000000000596 0.000000011045 -0.000000006670 -0.000000001063 0.000000000093 -0.000000001896 0.000000001289 -0.000000006670 0.000000252269 -0.000000009368 0.000000002660 -0.000000012750 0.000000003111 -0.000000001063 -0.000000009368 0.000000236670 -0.000000002412 0.000000024173 -0.000000006794 0.000000000093 0.000000002660 -0.000000002412 0.000000218725 -1 1645005905.101718425751 44 45 -0.000024589931 0.000754394551 0.000408641144 -0.000202334514 0.000016741619 -0.000672688351 0.000000076750 -0.000000016988 0.000000001970 -0.000000001786 -0.000000012675 0.000000024005 -0.000000016988 0.000000011345 -0.000000000542 0.000000001221 0.000000003052 -0.000000006750 0.000000001970 -0.000000000542 0.000000010830 -0.000000006614 -0.000000000937 0.000000000130 -0.000000001786 0.000000001221 -0.000000006614 0.000000251233 -0.000000009933 0.000000002642 -0.000000012675 0.000000003052 -0.000000000937 -0.000000009933 0.000000234262 -0.000000002344 0.000000024005 -0.000000006750 0.000000000130 0.000000002642 -0.000000002344 0.000000217464 -1 1645005905.201553344727 45 46 -0.000215612364 0.000834606698 0.000149325139 -0.000059117112 -0.000021374139 -0.000795866284 0.000000076660 -0.000000016912 0.000000001943 -0.000000001779 -0.000000012647 0.000000023970 -0.000000016912 0.000000011337 -0.000000000541 0.000000001223 0.000000003001 -0.000000006808 0.000000001943 -0.000000000541 0.000000011099 -0.000000006710 -0.000000000647 0.000000000160 -0.000000001779 0.000000001223 -0.000000006710 0.000000251867 -0.000000010134 0.000000002780 -0.000000012647 0.000000003001 -0.000000000647 -0.000000010134 0.000000234786 -0.000000002355 0.000000023970 -0.000000006808 0.000000000160 0.000000002780 -0.000000002355 0.000000217751 -1 1645005905.298557758331 46 47 0.000863242831 0.001318878065 -0.000252232506 -0.000118643047 0.000022995563 -0.001197291392 0.000000077249 -0.000000017059 0.000000001925 -0.000000001737 -0.000000012443 0.000000024240 -0.000000017059 0.000000011387 -0.000000000615 0.000000001270 0.000000002946 -0.000000006834 0.000000001925 -0.000000000615 0.000000011672 -0.000000006817 -0.000000000141 0.000000000151 -0.000000001737 0.000000001270 -0.000000006817 0.000000253335 -0.000000010236 0.000000003256 -0.000000012443 0.000000002946 -0.000000000141 -0.000000010236 0.000000237176 -0.000000002293 0.000000024240 -0.000000006834 0.000000000151 0.000000003256 -0.000000002293 0.000000219058 -1 1645005905.399083614349 47 48 0.002259440161 0.001105945552 -0.000750095487 0.000323798878 0.000255879602 -0.002485082179 0.000000077105 -0.000000016955 0.000000001874 -0.000000001539 -0.000000012289 0.000000024392 -0.000000016955 0.000000011391 -0.000000000645 0.000000001253 0.000000002881 -0.000000006702 0.000000001874 -0.000000000645 0.000000012203 -0.000000006954 0.000000000290 0.000000000129 -0.000000001539 0.000000001253 -0.000000006954 0.000000252527 -0.000000010460 0.000000003072 -0.000000012289 0.000000002881 0.000000000290 -0.000000010460 0.000000237679 -0.000000002199 0.000000024392 -0.000000006702 0.000000000129 0.000000003072 -0.000000002199 0.000000218901 -1 1645005905.499478816986 48 49 0.001203207953 0.000792305972 -0.001325455405 0.000034228402 -0.000060355459 -0.004038449411 0.000000076987 -0.000000016850 0.000000001884 -0.000000001681 -0.000000012438 0.000000024471 -0.000000016850 0.000000011498 -0.000000000626 0.000000001299 0.000000002943 -0.000000006512 0.000000001884 -0.000000000626 0.000000012768 -0.000000007178 0.000000000282 0.000000000078 -0.000000001681 0.000000001299 -0.000000007178 0.000000253439 -0.000000010315 0.000000002860 -0.000000012438 0.000000002943 0.000000000282 -0.000000010315 0.000000238861 -0.000000002267 0.000000024471 -0.000000006512 0.000000000078 0.000000002860 -0.000000002267 0.000000220135 -1 1645005905.599982023239 49 50 -0.000427727796 0.001474783510 -0.000181500876 0.000232089095 0.000542383436 -0.007766826389 0.000000077535 -0.000000017038 0.000000001838 -0.000000001917 -0.000000012698 0.000000024720 -0.000000017038 0.000000011645 -0.000000000643 0.000000001385 0.000000003075 -0.000000006360 0.000000001838 -0.000000000643 0.000000012879 -0.000000007370 -0.000000000087 -0.000000000001 -0.000000001917 0.000000001385 -0.000000007370 0.000000256358 -0.000000009978 0.000000002881 -0.000000012698 0.000000003075 -0.000000000087 -0.000000009978 0.000000240935 -0.000000002358 0.000000024720 -0.000000006360 -0.000000000001 0.000000002881 -0.000000002358 0.000000222306 -1 1645005905.699927568436 50 51 -0.003379224466 0.003178186222 -0.000934051059 0.000195152956 0.000761914950 -0.011833240191 0.000000077971 -0.000000017303 0.000000001797 -0.000000001904 -0.000000012847 0.000000024954 -0.000000017303 0.000000011820 -0.000000000663 0.000000001463 0.000000003093 -0.000000006376 0.000000001797 -0.000000000663 0.000000012974 -0.000000007270 0.000000000071 0.000000000011 -0.000000001904 0.000000001463 -0.000000007270 0.000000257764 -0.000000010219 0.000000002728 -0.000000012847 0.000000003093 0.000000000071 -0.000000010219 0.000000242472 -0.000000002420 0.000000024954 -0.000000006376 0.000000000011 0.000000002728 -0.000000002420 0.000000223682 -1 1645005905.802299499512 51 52 -0.003954280518 0.004558444476 -0.000638200694 0.000610985838 0.000592963225 -0.016423441061 0.000000078171 -0.000000017308 0.000000001851 -0.000000001684 -0.000000012949 0.000000025066 -0.000000017308 0.000000012067 -0.000000000670 0.000000001552 0.000000003016 -0.000000006564 0.000000001851 -0.000000000670 0.000000013096 -0.000000007042 0.000000000544 0.000000000071 -0.000000001684 0.000000001552 -0.000000007042 0.000000257460 -0.000000010870 0.000000002616 -0.000000012949 0.000000003016 0.000000000544 -0.000000010870 0.000000243242 -0.000000002361 0.000000025066 -0.000000006564 0.000000000071 0.000000002616 -0.000000002361 0.000000225067 -1 1645005905.903995513916 52 53 -0.002725003716 0.004085843921 -0.000203663179 0.000790836836 0.000587671331 -0.019275453487 0.000000078783 -0.000000017322 0.000000001763 -0.000000001459 -0.000000013309 0.000000025091 -0.000000017322 0.000000012536 -0.000000000705 0.000000001676 0.000000003055 -0.000000006665 0.000000001763 -0.000000000705 0.000000012882 -0.000000007071 0.000000000738 0.000000000012 -0.000000001459 0.000000001676 -0.000000007071 0.000000258028 -0.000000012023 0.000000002543 -0.000000013309 0.000000003055 0.000000000738 -0.000000012023 0.000000244738 -0.000000002147 0.000000025091 -0.000000006665 0.000000000012 0.000000002543 -0.000000002147 0.000000228144 -1 1645005906.006122350693 53 54 -0.001467286616 0.002911939063 0.001069730244 0.000570837090 0.000647669773 -0.017525510312 0.000000079128 -0.000000017240 0.000000001741 -0.000000001521 -0.000000013627 0.000000025287 -0.000000017240 0.000000012719 -0.000000000684 0.000000001673 0.000000003084 -0.000000006545 0.000000001741 -0.000000000684 0.000000012768 -0.000000007310 0.000000001027 0.000000000025 -0.000000001521 0.000000001673 -0.000000007310 0.000000260589 -0.000000013608 0.000000002539 -0.000000013627 0.000000003084 0.000000001027 -0.000000013608 0.000000247534 -0.000000002295 0.000000025287 -0.000000006545 0.000000000025 0.000000002539 -0.000000002295 0.000000231204 -1 1645005906.099501609802 54 55 -0.001059218310 0.001595583380 0.001718691048 0.000383564738 0.000421259981 -0.014061465138 0.000000078834 -0.000000016986 0.000000001758 -0.000000001750 -0.000000013531 0.000000025769 -0.000000016986 0.000000012611 -0.000000000664 0.000000001714 0.000000002983 -0.000000006153 0.000000001758 -0.000000000664 0.000000012509 -0.000000007145 0.000000001254 0.000000000088 -0.000000001750 0.000000001714 -0.000000007145 0.000000263017 -0.000000014906 0.000000002634 -0.000000013531 0.000000002983 0.000000001254 -0.000000014906 0.000000249896 -0.000000002598 0.000000025769 -0.000000006153 0.000000000088 0.000000002634 -0.000000002598 0.000000233378 -1 1645005906.199391126633 55 56 -0.001373475218 0.001308808338 0.000378930768 0.000479365502 0.000490820641 -0.009012238322 0.000000077943 -0.000000016768 0.000000001735 -0.000000001945 -0.000000013077 0.000000026054 -0.000000016768 0.000000012465 -0.000000000630 0.000000001711 0.000000002929 -0.000000005670 0.000000001735 -0.000000000630 0.000000012365 -0.000000007041 0.000000001545 0.000000000018 -0.000000001945 0.000000001711 -0.000000007041 0.000000261574 -0.000000015843 0.000000002953 -0.000000013077 0.000000002929 0.000000001545 -0.000000015843 0.000000248225 -0.000000002881 0.000000026054 -0.000000005670 0.000000000018 0.000000002953 -0.000000002881 0.000000232505 -1 1645005906.297765493393 56 57 -0.001355900545 0.000745568120 -0.000870920659 0.000244984256 0.000234169826 -0.004721749525 0.000000077729 -0.000000016651 0.000000001590 -0.000000002229 -0.000000012620 0.000000026130 -0.000000016651 0.000000012411 -0.000000000629 0.000000001763 0.000000002808 -0.000000005332 0.000000001590 -0.000000000629 0.000000012914 -0.000000007257 0.000000001540 -0.000000000032 -0.000000002229 0.000000001763 -0.000000007257 0.000000261378 -0.000000015902 0.000000003371 -0.000000012620 0.000000002808 0.000000001540 -0.000000015902 0.000000247742 -0.000000003003 0.000000026130 -0.000000005332 -0.000000000032 0.000000003371 -0.000000003003 0.000000232061 -1 1645005906.399662256241 57 58 -0.001773912220 0.001216253031 -0.001438523133 0.000093249864 0.000296808797 -0.001971036703 0.000000077865 -0.000000016538 0.000000001543 -0.000000002529 -0.000000012492 0.000000026251 -0.000000016538 0.000000012416 -0.000000000643 0.000000001937 0.000000002742 -0.000000005129 0.000000001543 -0.000000000643 0.000000013403 -0.000000007617 0.000000001310 -0.000000000089 -0.000000002529 0.000000001937 -0.000000007617 0.000000265989 -0.000000015382 0.000000003738 -0.000000012492 0.000000002742 0.000000001310 -0.000000015382 0.000000252116 -0.000000002953 0.000000026251 -0.000000005129 -0.000000000089 0.000000003738 -0.000000002953 0.000000233770 -1 1645005906.492895603180 58 59 -0.002660918339 0.001015475851 -0.001383065292 0.000330318405 -0.000006555949 -0.001300286378 0.000000077810 -0.000000016434 0.000000001630 -0.000000002398 -0.000000012724 0.000000026385 -0.000000016434 0.000000012330 -0.000000000622 0.000000001970 0.000000002832 -0.000000004942 0.000000001630 -0.000000000622 0.000000013443 -0.000000008032 0.000000001316 -0.000000000234 -0.000000002398 0.000000001970 -0.000000008032 0.000000272129 -0.000000015431 0.000000004625 -0.000000012724 0.000000002832 0.000000001316 -0.000000015431 0.000000257860 -0.000000002721 0.000000026385 -0.000000004942 -0.000000000234 0.000000004625 -0.000000002721 0.000000235343 -1 1645005906.599270582199 59 60 -0.004412984366 -0.001971971670 -0.001231997931 -0.000056688480 0.000101702338 -0.001115010901 0.000000077414 -0.000000016358 0.000000001582 -0.000000001830 -0.000000012780 0.000000026368 -0.000000016358 0.000000012249 -0.000000000629 0.000000001982 0.000000002761 -0.000000004929 0.000000001582 -0.000000000629 0.000000013473 -0.000000007965 0.000000001386 -0.000000000292 -0.000000001830 0.000000001982 -0.000000007965 0.000000274562 -0.000000016461 0.000000005637 -0.000000012780 0.000000002761 0.000000001386 -0.000000016461 0.000000259275 -0.000000002242 0.000000026368 -0.000000004929 -0.000000000292 0.000000005637 -0.000000002242 0.000000234454 -1 1645005906.697822093964 60 61 -0.005188840029 -0.004254111564 -0.000054909271 0.000406498966 -0.000296622837 0.000416988251 0.000000077214 -0.000000016250 0.000000001521 -0.000000001261 -0.000000012771 0.000000026760 -0.000000016250 0.000000012152 -0.000000000625 0.000000001905 0.000000002698 -0.000000005164 0.000000001521 -0.000000000625 0.000000013822 -0.000000008083 0.000000001309 -0.000000000328 -0.000000001261 0.000000001905 -0.000000008083 0.000000278328 -0.000000017447 0.000000005714 -0.000000012771 0.000000002698 0.000000001309 -0.000000017447 0.000000262592 -0.000000002008 0.000000026760 -0.000000005164 -0.000000000328 0.000000005714 -0.000000002008 0.000000235536 -1 1645005906.798193216324 61 62 -0.005350833950 -0.005705298333 0.000741755143 0.000130549378 -0.000347185966 0.003649959900 0.000000077631 -0.000000016247 0.000000001454 -0.000000001004 -0.000000012891 0.000000027268 -0.000000016247 0.000000012128 -0.000000000685 0.000000001939 0.000000002674 -0.000000005295 0.000000001454 -0.000000000685 0.000000014071 -0.000000008387 0.000000001361 -0.000000000365 -0.000000001004 0.000000001939 -0.000000008387 0.000000282769 -0.000000017695 0.000000005791 -0.000000012891 0.000000002674 0.000000001361 -0.000000017695 0.000000267197 -0.000000002088 0.000000027268 -0.000000005295 -0.000000000365 0.000000005791 -0.000000002088 0.000000238789 -1 1645005906.897775650024 62 63 -0.003697841110 -0.006789558112 -0.000589171782 0.000044608399 -0.000455644208 0.005951637027 0.000000077672 -0.000000016231 0.000000001373 -0.000000000697 -0.000000012771 0.000000027509 -0.000000016231 0.000000012025 -0.000000000658 0.000000001829 0.000000002608 -0.000000005327 0.000000001373 -0.000000000658 0.000000013806 -0.000000008294 0.000000001746 -0.000000000347 -0.000000000697 0.000000001829 -0.000000008294 0.000000283100 -0.000000016930 0.000000006179 -0.000000012771 0.000000002608 0.000000001746 -0.000000016930 0.000000267634 -0.000000001907 0.000000027509 -0.000000005327 -0.000000000347 0.000000006179 -0.000000001907 0.000000238791 -1 1645005907.000208616257 63 64 -0.002944765776 -0.006697802741 -0.003116587607 -0.000323320374 -0.000089765171 0.006392215701 0.000000077610 -0.000000016115 0.000000001489 -0.000000000549 -0.000000012506 0.000000027748 -0.000000016115 0.000000011973 -0.000000000586 0.000000001526 0.000000002609 -0.000000005268 0.000000001489 -0.000000000586 0.000000013816 -0.000000008429 0.000000001848 -0.000000000278 -0.000000000549 0.000000001526 -0.000000008429 0.000000282717 -0.000000015968 0.000000006292 -0.000000012506 0.000000002609 0.000000001848 -0.000000015968 0.000000266926 -0.000000001561 0.000000027748 -0.000000005268 -0.000000000278 0.000000006292 -0.000000001561 0.000000238503 -1 1645005907.098475456238 64 65 -0.001290240390 -0.005396750920 -0.004723595682 -0.000007365578 0.000016435762 0.004075705511 0.000000077997 -0.000000016170 0.000000001561 -0.000000000791 -0.000000012603 0.000000027663 -0.000000016170 0.000000012052 -0.000000000605 0.000000001609 0.000000002641 -0.000000005184 0.000000001561 -0.000000000605 0.000000013763 -0.000000008737 0.000000001470 -0.000000000238 -0.000000000791 0.000000001609 -0.000000008737 0.000000281532 -0.000000015977 0.000000006340 -0.000000012603 0.000000002641 0.000000001470 -0.000000015977 0.000000264521 -0.000000001627 0.000000027663 -0.000000005184 -0.000000000238 0.000000006340 -0.000000001627 0.000000238811 -1 1645005907.198553800583 65 66 -0.000669485805 -0.005485448301 -0.002930170548 -0.000146916616 0.000044397139 -0.000044314603 0.000000078888 -0.000000016541 0.000000001493 -0.000000000960 -0.000000013153 0.000000027258 -0.000000016541 0.000000012208 -0.000000000625 0.000000001783 0.000000002743 -0.000000005116 0.000000001493 -0.000000000625 0.000000013535 -0.000000008377 0.000000001173 -0.000000000224 -0.000000000960 0.000000001783 -0.000000008377 0.000000278934 -0.000000015798 0.000000005980 -0.000000013153 0.000000002743 0.000000001173 -0.000000015798 0.000000261815 -0.000000001742 0.000000027258 -0.000000005116 -0.000000000224 0.000000005980 -0.000000001742 0.000000238650 -1 1645005907.299114227295 66 67 -0.000737675673 -0.006602658374 0.001504502005 0.000251984015 0.000332600718 -0.004637221316 0.000000078978 -0.000000016696 0.000000001539 -0.000000001083 -0.000000013528 0.000000027256 -0.000000016696 0.000000012302 -0.000000000624 0.000000001764 0.000000002889 -0.000000005225 0.000000001539 -0.000000000624 0.000000013491 -0.000000008150 0.000000001072 -0.000000000210 -0.000000001083 0.000000001764 -0.000000008150 0.000000275585 -0.000000015902 0.000000005576 -0.000000013528 0.000000002889 0.000000001072 -0.000000015902 0.000000258948 -0.000000001809 0.000000027256 -0.000000005225 -0.000000000210 0.000000005576 -0.000000001809 0.000000238197 -1 1645005907.402397871017 67 68 -0.000551820100 -0.007603293394 0.003319136466 0.000085475476 0.000206125712 -0.005773773594 0.000000078821 -0.000000016809 0.000000001506 -0.000000001311 -0.000000013626 0.000000027682 -0.000000016809 0.000000012396 -0.000000000614 0.000000001766 0.000000002926 -0.000000005476 0.000000001506 -0.000000000614 0.000000013915 -0.000000008554 0.000000000776 -0.000000000165 -0.000000001311 0.000000001766 -0.000000008554 0.000000275078 -0.000000015973 0.000000005780 -0.000000013626 0.000000002926 0.000000000776 -0.000000015973 0.000000258664 -0.000000002050 0.000000027682 -0.000000005476 -0.000000000165 0.000000005780 -0.000000002050 0.000000238900 -1 1645005907.500724315643 68 69 -0.000078335904 -0.007900987662 0.000767252658 0.000325260818 0.000204141643 -0.004069357072 0.000000079410 -0.000000017045 0.000000001346 -0.000000001198 -0.000000013307 0.000000027863 -0.000000017045 0.000000012459 -0.000000000602 0.000000001790 0.000000002868 -0.000000005540 0.000000001346 -0.000000000602 0.000000014320 -0.000000009106 0.000000000386 -0.000000000282 -0.000000001198 0.000000001790 -0.000000009106 0.000000277944 -0.000000015885 0.000000006351 -0.000000013307 0.000000002868 0.000000000386 -0.000000015885 0.000000260452 -0.000000002077 0.000000027863 -0.000000005540 -0.000000000282 0.000000006351 -0.000000002077 0.000000240393 -1 1645005907.605340957642 69 70 0.001043055952 -0.008573117265 -0.001598483094 -0.000097387849 -0.000222906961 -0.000595407429 0.000000080055 -0.000000017438 0.000000001351 -0.000000001308 -0.000000013000 0.000000027705 -0.000000017438 0.000000012484 -0.000000000606 0.000000001915 0.000000002917 -0.000000005219 0.000000001351 -0.000000000606 0.000000014067 -0.000000009196 0.000000000300 -0.000000000458 -0.000000001308 0.000000001915 -0.000000009196 0.000000278309 -0.000000015861 0.000000006592 -0.000000013000 0.000000002917 0.000000000300 -0.000000015861 0.000000260164 -0.000000002000 0.000000027705 -0.000000005219 -0.000000000458 0.000000006592 -0.000000002000 0.000000240578 -1 1645005907.703968524933 70 71 0.001635189256 -0.007661342710 -0.000611987754 0.000195863477 0.000220085415 0.000455212442 0.000000081616 -0.000000018000 0.000000001433 -0.000000001641 -0.000000013192 0.000000027466 -0.000000018000 0.000000012653 -0.000000000591 0.000000001940 0.000000003029 -0.000000005006 0.000000001433 -0.000000000591 0.000000014157 -0.000000009101 0.000000000295 -0.000000000473 -0.000000001641 0.000000001940 -0.000000009101 0.000000278289 -0.000000015793 0.000000006721 -0.000000013192 0.000000003029 0.000000000295 -0.000000015793 0.000000260033 -0.000000002174 0.000000027466 -0.000000005006 -0.000000000473 0.000000006721 -0.000000002174 0.000000241348 -1 1645005907.804278135300 71 72 0.001549220998 -0.007393823942 0.001064792531 -0.000061245968 0.000265809889 -0.000238367926 0.000000084430 -0.000000018916 0.000000001290 -0.000000001506 -0.000000013499 0.000000026763 -0.000000018916 0.000000013096 -0.000000000595 0.000000001859 0.000000003095 -0.000000004652 0.000000001290 -0.000000000595 0.000000014779 -0.000000009309 0.000000000116 -0.000000000490 -0.000000001506 0.000000001859 -0.000000009309 0.000000280877 -0.000000016432 0.000000006827 -0.000000013499 0.000000003095 0.000000000116 -0.000000016432 0.000000260406 -0.000000002092 0.000000026763 -0.000000004652 -0.000000000490 0.000000006827 -0.000000002092 0.000000242565 -1 1645005907.903728008270 72 73 0.002219839781 -0.006729865633 0.001166665107 0.000120456909 0.000039397030 -0.001195289279 0.000000086968 -0.000000019811 0.000000001166 -0.000000001441 -0.000000013646 0.000000025791 -0.000000019811 0.000000013556 -0.000000000638 0.000000001888 0.000000003216 -0.000000004211 0.000000001166 -0.000000000638 0.000000015013 -0.000000009450 -0.000000000298 -0.000000000604 -0.000000001441 0.000000001888 -0.000000009450 0.000000282683 -0.000000017245 0.000000006808 -0.000000013646 0.000000003216 -0.000000000298 -0.000000017245 0.000000258711 -0.000000001579 0.000000025791 -0.000000004211 -0.000000000604 0.000000006808 -0.000000001579 0.000000241863 -1 1645005908.003634452820 73 74 0.003125083194 -0.006057082466 0.000100060086 -0.000042155704 -0.000130882188 -0.000945982049 0.000000088087 -0.000000020274 0.000000001242 -0.000000001785 -0.000000013450 0.000000024803 -0.000000020274 0.000000013671 -0.000000000680 0.000000002063 0.000000003299 -0.000000004076 0.000000001242 -0.000000000680 0.000000014959 -0.000000009260 -0.000000000646 -0.000000000747 -0.000000001785 0.000000002063 -0.000000009260 0.000000284481 -0.000000018051 0.000000007211 -0.000000013450 0.000000003299 -0.000000000646 -0.000000018051 0.000000257869 -0.000000000830 0.000000024803 -0.000000004076 -0.000000000747 0.000000007211 -0.000000000830 0.000000240461 -1 1645005908.103469848633 74 75 0.005239355570 -0.006690197943 -0.001346041897 0.000002110025 0.000383511679 0.000870726890 0.000000088096 -0.000000020346 0.000000001268 -0.000000002250 -0.000000013524 0.000000023906 -0.000000020346 0.000000013591 -0.000000000702 0.000000002211 0.000000003331 -0.000000003807 0.000000001268 -0.000000000702 0.000000014950 -0.000000008801 -0.000000000587 -0.000000000765 -0.000000002250 0.000000002211 -0.000000008801 0.000000284454 -0.000000018287 0.000000007836 -0.000000013524 0.000000003331 -0.000000000587 -0.000000018287 0.000000257037 -0.000000000613 0.000000023906 -0.000000003807 -0.000000000765 0.000000007836 -0.000000000613 0.000000238618 -1 1645005908.202678442001 75 76 0.006817804937 -0.009726863760 -0.000518636839 -0.000056178504 -0.000656898114 0.002883550854 0.000000088802 -0.000000020528 0.000000001285 -0.000000002483 -0.000000014210 0.000000023286 -0.000000020528 0.000000013726 -0.000000000706 0.000000002292 0.000000003404 -0.000000003359 0.000000001285 -0.000000000706 0.000000014784 -0.000000008320 -0.000000000291 -0.000000000649 -0.000000002483 0.000000002292 -0.000000008320 0.000000283431 -0.000000018165 0.000000008157 -0.000000014210 0.000000003404 -0.000000000291 -0.000000018165 0.000000255778 -0.000000000733 0.000000023286 -0.000000003359 -0.000000000649 0.000000008157 -0.000000000733 0.000000239003 -1 1645005908.303349256516 76 77 0.008880185968 -0.011072420154 0.002005117294 0.000364039331 0.000621317606 0.001463440780 0.000000089528 -0.000000020704 0.000000001309 -0.000000002811 -0.000000014365 0.000000022726 -0.000000020704 0.000000013849 -0.000000000704 0.000000002410 0.000000003408 -0.000000003006 0.000000001309 -0.000000000704 0.000000014662 -0.000000008440 -0.000000000182 -0.000000000582 -0.000000002811 0.000000002410 -0.000000008440 0.000000282898 -0.000000018035 0.000000008233 -0.000000014365 0.000000003408 -0.000000000182 -0.000000018035 0.000000254217 -0.000000000493 0.000000022726 -0.000000003006 -0.000000000582 0.000000008233 -0.000000000493 0.000000239589 -1 1645005908.403426647186 77 78 0.013214057998 -0.010022380808 0.003548089026 -0.000420898660 -0.000687846562 0.004288662412 0.000000090426 -0.000000021194 0.000000001135 -0.000000003300 -0.000000014313 0.000000022653 -0.000000021194 0.000000013961 -0.000000000638 0.000000002484 0.000000003448 -0.000000002827 0.000000001135 -0.000000000638 0.000000014468 -0.000000009028 -0.000000000586 -0.000000000561 -0.000000003300 0.000000002484 -0.000000009028 0.000000283166 -0.000000017970 0.000000008066 -0.000000014313 0.000000003448 -0.000000000586 -0.000000017970 0.000000253551 -0.000000000440 0.000000022653 -0.000000002827 -0.000000000561 0.000000008066 -0.000000000440 0.000000240482 -1 1645005908.504360437393 78 79 0.020180381727 -0.009462892108 0.001955139025 0.000042094269 0.000191650148 0.004876347524 0.000000091477 -0.000000021398 0.000000001260 -0.000000003754 -0.000000014396 0.000000022564 -0.000000021398 0.000000014143 -0.000000000654 0.000000002576 0.000000003359 -0.000000002684 0.000000001260 -0.000000000654 0.000000014202 -0.000000008908 -0.000000001365 -0.000000000321 -0.000000003754 0.000000002576 -0.000000008908 0.000000283742 -0.000000018036 0.000000007708 -0.000000014396 0.000000003359 -0.000000001365 -0.000000018036 0.000000252709 -0.000000000672 0.000000022564 -0.000000002684 -0.000000000321 0.000000007708 -0.000000000672 0.000000242092 -1 1645005908.602572441101 79 80 0.027583841746 -0.007492189755 -0.000358945670 -0.000316065525 0.000410807340 0.004627049462 0.000000091890 -0.000000021219 0.000000001348 -0.000000004184 -0.000000014661 0.000000022474 -0.000000021219 0.000000014196 -0.000000000667 0.000000002818 0.000000003383 -0.000000002589 0.000000001348 -0.000000000667 0.000000014612 -0.000000008669 -0.000000001276 -0.000000000350 -0.000000004184 0.000000002818 -0.000000008669 0.000000284516 -0.000000017822 0.000000007904 -0.000000014661 0.000000003383 -0.000000001276 -0.000000017822 0.000000252238 -0.000000001096 0.000000022474 -0.000000002589 -0.000000000350 0.000000007904 -0.000000001096 0.000000242589 -1 1645005908.702723264694 80 81 0.037527173709 -0.002432558917 0.000584410457 -0.000295924796 0.000172057833 0.004510494708 0.000000092503 -0.000000021428 0.000000001442 -0.000000004439 -0.000000015161 0.000000022444 -0.000000021428 0.000000014211 -0.000000000702 0.000000002984 0.000000003573 -0.000000002087 0.000000001442 -0.000000000702 0.000000014735 -0.000000008471 -0.000000001226 -0.000000000479 -0.000000004439 0.000000002984 -0.000000008471 0.000000283202 -0.000000016932 0.000000008633 -0.000000015161 0.000000003573 -0.000000001226 -0.000000016932 0.000000250551 -0.000000001604 0.000000022444 -0.000000002087 -0.000000000479 0.000000008633 -0.000000001604 0.000000241814 -1 1645005908.805076837540 81 82 0.044756539731 -0.000661973435 0.002178494075 -0.000609676361 0.000455606665 0.001515229361 0.000000093517 -0.000000021845 0.000000001449 -0.000000004558 -0.000000015501 0.000000022061 -0.000000021845 0.000000014347 -0.000000000736 0.000000003042 0.000000003685 -0.000000001671 0.000000001449 -0.000000000736 0.000000014651 -0.000000008202 -0.000000002017 -0.000000000480 -0.000000004558 0.000000003042 -0.000000008202 0.000000281630 -0.000000016156 0.000000009346 -0.000000015501 0.000000003685 -0.000000002017 -0.000000016156 0.000000248540 -0.000000001725 0.000000022061 -0.000000001671 -0.000000000480 0.000000009346 -0.000000001725 0.000000241443 -1 1645005908.902604103088 82 83 0.045146081256 -0.002522614562 0.003315811413 0.000215551457 0.001434294478 -0.002791558929 0.000000095523 -0.000000022658 0.000000001480 -0.000000004967 -0.000000015958 0.000000021670 -0.000000022658 0.000000014741 -0.000000000778 0.000000003208 0.000000003828 -0.000000001330 0.000000001480 -0.000000000778 0.000000014730 -0.000000007858 -0.000000002516 -0.000000000414 -0.000000004967 0.000000003208 -0.000000007858 0.000000281929 -0.000000016137 0.000000009579 -0.000000015958 0.000000003828 -0.000000002516 -0.000000016137 0.000000249394 -0.000000001587 0.000000021670 -0.000000001330 -0.000000000414 0.000000009579 -0.000000001587 0.000000243425 -1 1645005909.003700494766 83 84 0.050021001197 -0.004220963579 0.004624075743 0.000077137405 0.000861877284 -0.003931105805 0.000000097156 -0.000000023529 0.000000001589 -0.000000005276 -0.000000016382 0.000000021184 -0.000000023529 0.000000015119 -0.000000000815 0.000000003220 0.000000004081 -0.000000000901 0.000000001589 -0.000000000815 0.000000014639 -0.000000007417 -0.000000002865 -0.000000000427 -0.000000005276 0.000000003220 -0.000000007417 0.000000278551 -0.000000015767 0.000000008985 -0.000000016382 0.000000004081 -0.000000002865 -0.000000015767 0.000000248181 -0.000000001156 0.000000021184 -0.000000000901 -0.000000000427 0.000000008985 -0.000000001156 0.000000244236 -1 1645005909.105985641479 84 85 0.055306161593 -0.005644162807 0.003857391803 0.000176010803 -0.000406336505 -0.002605005858 0.000000098538 -0.000000024037 0.000000001724 -0.000000005135 -0.000000016925 0.000000020936 -0.000000024037 0.000000015386 -0.000000000857 0.000000002936 0.000000004319 -0.000000000573 0.000000001724 -0.000000000857 0.000000014932 -0.000000007133 -0.000000003111 -0.000000000413 -0.000000005135 0.000000002936 -0.000000007133 0.000000274491 -0.000000015304 0.000000008087 -0.000000016925 0.000000004319 -0.000000003111 -0.000000015304 0.000000247261 -0.000000000996 0.000000020936 -0.000000000573 -0.000000000413 0.000000008087 -0.000000000996 0.000000244577 -1 1645005909.204295873642 85 86 0.053724760856 -0.007867742945 0.002362917646 0.000765560740 0.000896643338 -0.004875595418 0.000000100571 -0.000000024471 0.000000001820 -0.000000004997 -0.000000017493 0.000000020789 -0.000000024471 0.000000015583 -0.000000000907 0.000000002811 0.000000004425 -0.000000000103 0.000000001820 -0.000000000907 0.000000014997 -0.000000007018 -0.000000003853 -0.000000000322 -0.000000004997 0.000000002811 -0.000000007018 0.000000274574 -0.000000014663 0.000000007432 -0.000000017493 0.000000004425 -0.000000003853 -0.000000014663 0.000000250668 -0.000000000903 0.000000020789 -0.000000000103 -0.000000000322 0.000000007432 -0.000000000903 0.000000246442 -1 1645005909.301892518997 86 87 0.052598742656 -0.009586402032 -0.001683730482 0.000100902220 0.000694494544 -0.005642354434 0.000000101572 -0.000000024847 0.000000001621 -0.000000004490 -0.000000017492 0.000000020848 -0.000000024847 0.000000015608 -0.000000000748 0.000000002623 0.000000004392 0.000000000187 0.000000001621 -0.000000000748 0.000000014697 -0.000000007106 -0.000000005019 -0.000000000290 -0.000000004490 0.000000002623 -0.000000007106 0.000000274211 -0.000000014372 0.000000006733 -0.000000017492 0.000000004392 -0.000000005019 -0.000000014372 0.000000253261 -0.000000001008 0.000000020848 0.000000000187 -0.000000000290 0.000000006733 -0.000000001008 0.000000245747 -1 1645005909.403104543686 87 88 0.056969463690 -0.013394491540 -0.004907688527 0.000313438663 -0.000019403912 -0.004039402462 0.000000101997 -0.000000025070 0.000000001444 -0.000000003922 -0.000000017659 0.000000021025 -0.000000025070 0.000000015563 -0.000000000684 0.000000002332 0.000000004455 0.000000000663 0.000000001444 -0.000000000684 0.000000015000 -0.000000007663 -0.000000005985 -0.000000000282 -0.000000003922 0.000000002332 -0.000000007663 0.000000273327 -0.000000013785 0.000000006215 -0.000000017659 0.000000004455 -0.000000005985 -0.000000013785 0.000000254655 -0.000000001345 0.000000021025 0.000000000663 -0.000000000282 0.000000006215 -0.000000001345 0.000000243873 -1 1645005909.501364469528 88 89 0.059762523379 -0.018567501801 -0.005975427554 0.000327232703 -0.000227548183 -0.003605858449 0.000000103041 -0.000000025390 0.000000001364 -0.000000003806 -0.000000017859 0.000000021654 -0.000000025390 0.000000015487 -0.000000000758 0.000000002227 0.000000004664 0.000000001355 0.000000001364 -0.000000000758 0.000000015660 -0.000000008246 -0.000000006861 -0.000000000445 -0.000000003806 0.000000002227 -0.000000008246 0.000000275936 -0.000000013446 0.000000006125 -0.000000017859 0.000000004664 -0.000000006861 -0.000000013446 0.000000258726 -0.000000001189 0.000000021654 0.000000001355 -0.000000000445 0.000000006125 -0.000000001189 0.000000245723 -1 1645005909.601603746414 89 90 0.063283735135 -0.025948706002 -0.004113490990 0.000699255821 0.000279829928 -0.004494526640 0.000000104093 -0.000000025791 0.000000001237 -0.000000003691 -0.000000017610 0.000000022878 -0.000000025791 0.000000015579 -0.000000000780 0.000000002254 0.000000004734 0.000000001701 0.000000001237 -0.000000000780 0.000000016004 -0.000000008598 -0.000000007978 -0.000000000633 -0.000000003691 0.000000002254 -0.000000008598 0.000000279345 -0.000000013365 0.000000006383 -0.000000017610 0.000000004734 -0.000000007978 -0.000000013365 0.000000262863 -0.000000000672 0.000000022878 0.000000001701 -0.000000000633 0.000000006383 -0.000000000672 0.000000249192 -1 1645005909.706259727478 90 91 0.064392495618 -0.030468368848 0.003371720855 0.000963763803 0.001428163796 -0.005769357514 0.000000105109 -0.000000026290 0.000000001203 -0.000000003464 -0.000000017388 0.000000024107 -0.000000026290 0.000000015676 -0.000000000845 0.000000002355 0.000000004759 0.000000001923 0.000000001203 -0.000000000845 0.000000016121 -0.000000008819 -0.000000009190 -0.000000000794 -0.000000003464 0.000000002355 -0.000000008819 0.000000281196 -0.000000012993 0.000000007076 -0.000000017388 0.000000004759 -0.000000009190 -0.000000012993 0.000000265075 -0.000000000134 0.000000024107 0.000000001923 -0.000000000794 0.000000007076 -0.000000000134 0.000000251988 -1 1645005909.802580833435 91 92 0.062365238940 -0.025844183621 0.009407834302 0.000193492681 -0.000172715186 -0.001464084852 0.000000108795 -0.000000027506 0.000000001224 -0.000000003570 -0.000000017752 0.000000025071 -0.000000027506 0.000000016130 -0.000000000716 0.000000002314 0.000000004730 0.000000002329 0.000000001224 -0.000000000716 0.000000015944 -0.000000008872 -0.000000010128 -0.000000000541 -0.000000003570 0.000000002314 -0.000000008872 0.000000285835 -0.000000013072 0.000000007112 -0.000000017752 0.000000004730 -0.000000010128 -0.000000013072 0.000000269332 -0.000000000648 0.000000025071 0.000000002329 -0.000000000541 0.000000007112 -0.000000000648 0.000000257045 -1 1645005909.903105735779 92 93 0.068784629133 -0.022584999076 0.007314965158 -0.000173602250 -0.000981733227 0.004132128312 0.000000111499 -0.000000028105 0.000000001268 -0.000000003943 -0.000000017982 0.000000024931 -0.000000028105 0.000000016532 -0.000000000666 0.000000002326 0.000000004775 0.000000003407 0.000000001268 -0.000000000666 0.000000015946 -0.000000009222 -0.000000011103 -0.000000000468 -0.000000003943 0.000000002326 -0.000000009222 0.000000288991 -0.000000012266 0.000000007214 -0.000000017982 0.000000004775 -0.000000011103 -0.000000012266 0.000000270553 -0.000000000807 0.000000024931 0.000000003407 -0.000000000468 0.000000007214 -0.000000000807 0.000000260827 -1 1645005910.002853631973 93 94 0.064100403759 -0.021302988329 0.003328486309 -0.000115742040 0.000269762022 0.004338534235 0.000000113187 -0.000000028301 0.000000001447 -0.000000004451 -0.000000017932 0.000000024371 -0.000000028301 0.000000016578 -0.000000000743 0.000000002509 0.000000004720 0.000000004613 0.000000001447 -0.000000000743 0.000000015755 -0.000000009213 -0.000000011391 -0.000000000441 -0.000000004451 0.000000002509 -0.000000009213 0.000000291379 -0.000000012472 0.000000007953 -0.000000017932 0.000000004720 -0.000000011391 -0.000000012472 0.000000268086 -0.000000000754 0.000000024371 0.000000004613 -0.000000000441 0.000000007953 -0.000000000754 0.000000263152 -1 1645005910.102889776230 94 95 0.058187415660 -0.021959187640 -0.002395190107 -0.000230382660 0.000479950528 0.004722778418 0.000000114411 -0.000000028797 0.000000001597 -0.000000005011 -0.000000018281 0.000000023886 -0.000000028797 0.000000016614 -0.000000000790 0.000000002676 0.000000004786 0.000000005410 0.000000001597 -0.000000000790 0.000000015406 -0.000000009290 -0.000000011420 -0.000000000347 -0.000000005011 0.000000002676 -0.000000009290 0.000000295984 -0.000000013121 0.000000008785 -0.000000018281 0.000000004786 -0.000000011420 -0.000000013121 0.000000268118 -0.000000001232 0.000000023886 0.000000005410 -0.000000000347 0.000000008785 -0.000000001232 0.000000266828 -1 1645005910.203362464905 95 96 0.056619890717 -0.019994351111 -0.005006673376 -0.000078075128 -0.001163126359 0.005855695866 0.000000113848 -0.000000028816 0.000000001425 -0.000000004988 -0.000000018908 0.000000023457 -0.000000028816 0.000000016671 -0.000000000790 0.000000002758 0.000000005082 0.000000006329 0.000000001425 -0.000000000790 0.000000015387 -0.000000009472 -0.000000012713 -0.000000000564 -0.000000004988 0.000000002758 -0.000000009472 0.000000298099 -0.000000012668 0.000000009794 -0.000000018908 0.000000005082 -0.000000012713 -0.000000012668 0.000000271956 -0.000000001334 0.000000023457 0.000000006329 -0.000000000564 0.000000009794 -0.000000001334 0.000000269624 -1 1645005910.304825067520 96 97 0.061086566852 -0.020286198113 -0.004843598885 0.000071324355 -0.000601247565 0.004021009982 0.000000117880 -0.000000029623 0.000000001330 -0.000000004609 -0.000000019305 0.000000024344 -0.000000029623 0.000000017038 -0.000000000772 0.000000002859 0.000000005198 0.000000006765 0.000000001330 -0.000000000772 0.000000015981 -0.000000010001 -0.000000014626 -0.000000000763 -0.000000004609 0.000000002859 -0.000000010001 0.000000301426 -0.000000011273 0.000000010740 -0.000000019305 0.000000005198 -0.000000014626 -0.000000011273 0.000000276684 -0.000000001301 0.000000024344 0.000000006765 -0.000000000763 0.000000010740 -0.000000001301 0.000000271470 -1 1645005910.406254529953 97 98 0.068850355530 -0.020207965588 -0.000165344613 -0.000090050301 -0.000186271132 0.000092130259 0.000000120621 -0.000000030569 0.000000001323 -0.000000004733 -0.000000019353 0.000000024043 -0.000000030569 0.000000017239 -0.000000000763 0.000000002867 0.000000005258 0.000000007204 0.000000001323 -0.000000000763 0.000000016177 -0.000000010171 -0.000000016172 -0.000000000720 -0.000000004733 0.000000002867 -0.000000010171 0.000000306276 -0.000000010789 0.000000010460 -0.000000019353 0.000000005258 -0.000000016172 -0.000000010789 0.000000281858 -0.000000001403 0.000000024043 0.000000007204 -0.000000000720 0.000000010460 -0.000000001403 0.000000273593 -1 1645005910.503567218781 98 99 0.074162405982 -0.018493810987 0.006239807671 -0.000159910511 0.000247959785 -0.004015256391 0.000000119575 -0.000000030905 0.000000001301 -0.000000005274 -0.000000019174 0.000000021991 -0.000000030905 0.000000017327 -0.000000000670 0.000000002923 0.000000005171 0.000000008213 0.000000001301 -0.000000000670 0.000000015751 -0.000000009557 -0.000000017503 -0.000000000420 -0.000000005274 0.000000002923 -0.000000009557 0.000000310028 -0.000000012146 0.000000010051 -0.000000019174 0.000000005171 -0.000000017503 -0.000000012146 0.000000288635 -0.000000001805 0.000000021991 0.000000008213 -0.000000000420 0.000000010051 -0.000000001805 0.000000275801 -1 1645005910.606616973877 99 100 0.080910102851 -0.020164208266 0.008661834846 0.000071976494 0.000898942665 -0.008152706685 0.000000119264 -0.000000031192 0.000000001273 -0.000000006002 -0.000000018615 0.000000020354 -0.000000031192 0.000000017400 -0.000000000693 0.000000003292 0.000000005053 0.000000009012 0.000000001273 -0.000000000693 0.000000015378 -0.000000009114 -0.000000018448 -0.000000000406 -0.000000006002 0.000000003292 -0.000000009114 0.000000310497 -0.000000014070 0.000000010804 -0.000000018615 0.000000005053 -0.000000018448 -0.000000014070 0.000000292764 -0.000000001847 0.000000020354 0.000000009012 -0.000000000406 0.000000010804 -0.000000001847 0.000000274724 -1 1645005910.704123735428 100 101 0.075961027781 -0.021808661780 0.005934638148 0.000450549674 0.001441942574 -0.012050260842 0.000000119262 -0.000000031760 0.000000001275 -0.000000006676 -0.000000018333 0.000000018441 -0.000000031760 0.000000017569 -0.000000000766 0.000000003498 0.000000005201 0.000000009821 0.000000001275 -0.000000000766 0.000000015022 -0.000000009165 -0.000000018987 -0.000000000583 -0.000000006676 0.000000003498 -0.000000009165 0.000000310167 -0.000000014428 0.000000011047 -0.000000018333 0.000000005201 -0.000000018987 -0.000000014428 0.000000295633 -0.000000001178 0.000000018441 0.000000009821 -0.000000000583 0.000000011047 -0.000000001178 0.000000274189 -1 1645005910.803392648697 101 102 0.078482673806 -0.023461910881 0.002857169426 0.000627784303 0.000447074489 -0.011957121987 0.000000119092 -0.000000032121 0.000000001304 -0.000000007124 -0.000000018655 0.000000017333 -0.000000032121 0.000000017789 -0.000000000790 0.000000003564 0.000000005412 0.000000010492 0.000000001304 -0.000000000790 0.000000015124 -0.000000009379 -0.000000019322 -0.000000000660 -0.000000007124 0.000000003564 -0.000000009379 0.000000308153 -0.000000013438 0.000000010595 -0.000000018655 0.000000005412 -0.000000019322 -0.000000013438 0.000000297079 -0.000000000659 0.000000017333 0.000000010492 -0.000000000660 0.000000010595 -0.000000000659 0.000000273340 -1 1645005910.902909994125 102 103 0.083189939682 -0.023490784963 -0.003117914173 0.000480762492 0.000068118626 -0.007186591478 0.000000118524 -0.000000032013 0.000000001297 -0.000000007106 -0.000000018773 0.000000016705 -0.000000032013 0.000000017971 -0.000000000795 0.000000003426 0.000000005515 0.000000011670 0.000000001297 -0.000000000795 0.000000015444 -0.000000009493 -0.000000020369 -0.000000000697 -0.000000007106 0.000000003426 -0.000000009493 0.000000306218 -0.000000012861 0.000000010051 -0.000000018773 0.000000005515 -0.000000020369 -0.000000012861 0.000000299271 -0.000000000380 0.000000016705 0.000000011670 -0.000000000697 0.000000010051 -0.000000000380 0.000000272341 -1 1645005911.004977464676 103 104 0.087315384998 -0.025157189183 -0.000393238317 0.000391193097 0.000467548951 -0.002126631756 0.000000118945 -0.000000031979 0.000000001227 -0.000000007043 -0.000000019158 0.000000016413 -0.000000031979 0.000000018203 -0.000000000668 0.000000003217 0.000000005547 0.000000013380 0.000000001227 -0.000000000668 0.000000015676 -0.000000009973 -0.000000022149 -0.000000000580 -0.000000007043 0.000000003217 -0.000000009973 0.000000309925 -0.000000012331 0.000000009781 -0.000000019158 0.000000005547 -0.000000022149 -0.000000012331 0.000000306142 -0.000000000590 0.000000016413 0.000000013380 -0.000000000580 0.000000009781 -0.000000000590 0.000000276867 -1 1645005911.101893663406 104 105 0.084323245144 -0.023813002632 0.014804471985 0.000199032956 0.000054108090 -0.000198912619 0.000000120217 -0.000000032143 0.000000001107 -0.000000007075 -0.000000019378 0.000000016191 -0.000000032143 0.000000018424 -0.000000000637 0.000000003358 0.000000005540 0.000000015177 0.000000001107 -0.000000000637 0.000000015707 -0.000000010434 -0.000000024174 -0.000000000604 -0.000000007075 0.000000003358 -0.000000010434 0.000000316238 -0.000000012013 0.000000010568 -0.000000019378 0.000000005540 -0.000000024174 -0.000000012013 0.000000316136 -0.000000000812 0.000000016191 0.000000015177 -0.000000000604 0.000000010568 -0.000000000812 0.000000285489 -1 1645005911.202757835388 105 106 0.091890144553 -0.022457558791 0.017747265772 -0.000060833543 -0.000571579936 0.006619650214 0.000000121531 -0.000000032560 0.000000000738 -0.000000006643 -0.000000017693 0.000000015251 -0.000000032560 0.000000018747 -0.000000000671 0.000000003314 0.000000005222 0.000000017170 0.000000000738 -0.000000000671 0.000000016021 -0.000000011220 -0.000000026178 -0.000000000837 -0.000000006643 0.000000003314 -0.000000011220 0.000000321155 -0.000000010921 0.000000011729 -0.000000017693 0.000000005222 -0.000000026178 -0.000000010921 0.000000322863 -0.000000000229 0.000000015251 0.000000017170 -0.000000000837 0.000000011729 -0.000000000229 0.000000293530 -1 1645005911.304598569870 106 107 0.099315592633 -0.013707866919 0.008929200938 -0.000593510612 0.000431991390 0.006542184812 0.000000122613 -0.000000032815 0.000000000312 -0.000000006490 -0.000000015355 0.000000013258 -0.000000032815 0.000000018837 -0.000000000526 0.000000003118 0.000000004545 0.000000018930 0.000000000312 -0.000000000526 0.000000016756 -0.000000012375 -0.000000028786 -0.000000000787 -0.000000006490 0.000000003118 -0.000000012375 0.000000326678 -0.000000008324 0.000000013042 -0.000000015355 0.000000004545 -0.000000028786 -0.000000008324 0.000000328892 0.000000000490 0.000000013258 0.000000018930 -0.000000000787 0.000000013042 0.000000000490 0.000000298943 -1 1645005911.402243375778 107 108 0.104785384813 -0.008105171994 -0.002214641892 -0.000860946379 -0.000993392829 0.010948470463 0.000000123089 -0.000000033137 0.000000000099 -0.000000006942 -0.000000014277 0.000000011341 -0.000000033137 0.000000019181 -0.000000000321 0.000000003202 0.000000004018 0.000000020680 0.000000000099 -0.000000000321 0.000000016999 -0.000000012404 -0.000000030745 -0.000000000434 -0.000000006942 0.000000003202 -0.000000012404 0.000000331673 -0.000000007654 0.000000013557 -0.000000014277 0.000000004018 -0.000000030745 -0.000000007654 0.000000336509 0.000000000255 0.000000011341 0.000000020680 -0.000000000434 0.000000013557 0.000000000255 0.000000304958 -1 1645005911.504294157028 108 109 0.115689898818 -0.005982363005 -0.006109069161 -0.000726627622 0.000973576912 0.010908783105 0.000000124858 -0.000000033368 0.000000000274 -0.000000007206 -0.000000014118 0.000000010002 -0.000000033368 0.000000019303 -0.000000000398 0.000000003340 0.000000003987 0.000000022318 0.000000000274 -0.000000000398 0.000000016946 -0.000000012012 -0.000000031995 -0.000000000446 -0.000000007206 0.000000003340 -0.000000012012 0.000000330527 -0.000000006532 0.000000013696 -0.000000014118 0.000000003987 -0.000000031995 -0.000000006532 0.000000339820 0.000000000103 0.000000010002 0.000000022318 -0.000000000446 0.000000013696 0.000000000103 0.000000309344 -1 1645005911.603650331497 109 110 0.112144073030 -0.007022975029 0.001012219037 -0.001163101342 0.000302771644 0.009647953042 0.000000124674 -0.000000033654 0.000000000385 -0.000000008009 -0.000000015177 0.000000007700 -0.000000033654 0.000000019363 -0.000000000509 0.000000003826 0.000000004570 0.000000023824 0.000000000385 -0.000000000509 0.000000016545 -0.000000011791 -0.000000032496 -0.000000000820 -0.000000008009 0.000000003826 -0.000000011791 0.000000328802 -0.000000004861 0.000000014409 -0.000000015177 0.000000004570 -0.000000032496 -0.000000004861 0.000000338575 0.000000000638 0.000000007700 0.000000023824 -0.000000000820 0.000000014409 0.000000000638 0.000000314003 -1 1645005911.705323696136 110 111 0.108334202057 -0.006199322902 0.015439608211 -0.000499640517 0.000622717034 0.005115544113 0.000000121420 -0.000000033136 0.000000000547 -0.000000008867 -0.000000016030 0.000000005761 -0.000000033136 0.000000019118 -0.000000000490 0.000000003998 0.000000004882 0.000000025082 0.000000000547 -0.000000000490 0.000000015937 -0.000000011322 -0.000000032770 -0.000000000779 -0.000000008867 0.000000003998 -0.000000011322 0.000000326263 -0.000000003896 0.000000014707 -0.000000016030 0.000000004882 -0.000000032770 -0.000000003896 0.000000334604 0.000000000414 0.000000005761 0.000000025082 -0.000000000779 0.000000014707 0.000000000414 0.000000319533 -1 1645005911.805619001389 111 112 0.105563859350 -0.007445366800 0.017090699054 -0.000396553302 -0.000236939023 0.001638354551 0.000000119495 -0.000000032859 0.000000000407 -0.000000008551 -0.000000014394 0.000000004296 -0.000000032859 0.000000019006 -0.000000000444 0.000000003710 0.000000004531 0.000000026291 0.000000000407 -0.000000000444 0.000000015977 -0.000000011729 -0.000000035072 -0.000000000757 -0.000000008551 0.000000003710 -0.000000011729 0.000000324713 -0.000000002118 0.000000014718 -0.000000014394 0.000000004531 -0.000000035072 -0.000000002118 0.000000339187 0.000000000583 0.000000004296 0.000000026291 -0.000000000757 0.000000014718 0.000000000583 0.000000324775 -1 1645005911.905131578445 112 113 0.105669045682 -0.010232842109 0.007581705384 0.000117560962 -0.000845623252 -0.002154597809 0.000000117976 -0.000000032837 0.000000000325 -0.000000008707 -0.000000012159 0.000000002537 -0.000000032837 0.000000019001 -0.000000000403 0.000000003735 0.000000003954 0.000000027237 0.000000000325 -0.000000000403 0.000000015480 -0.000000011530 -0.000000035970 -0.000000000732 -0.000000008707 0.000000003735 -0.000000011530 0.000000324418 -0.000000003537 0.000000014543 -0.000000012159 0.000000003954 -0.000000035970 -0.000000003537 0.000000343391 0.000000000796 0.000000002537 0.000000027237 -0.000000000732 0.000000014543 0.000000000796 0.000000327307 -1 1645005912.005542993546 113 114 0.105408723509 -0.006257586189 -0.004647377317 -0.000008164249 -0.000462734466 -0.006034679601 0.000000115866 -0.000000032739 0.000000000255 -0.000000008904 -0.000000010564 0.000000001256 -0.000000032739 0.000000018798 -0.000000000420 0.000000003723 0.000000003731 0.000000027559 0.000000000255 -0.000000000420 0.000000015409 -0.000000012009 -0.000000036888 -0.000000000887 -0.000000008904 0.000000003723 -0.000000012009 0.000000321068 -0.000000000786 0.000000014100 -0.000000010564 0.000000003731 -0.000000036888 -0.000000000786 0.000000345395 0.000000001508 0.000000001256 0.000000027559 -0.000000000887 0.000000014100 0.000000001508 0.000000326200 -1 1645005912.104353666306 114 115 0.104312051462 -0.005788696135 -0.001086666677 0.000673600379 0.002056576062 -0.010721959912 0.000000113948 -0.000000032429 0.000000000239 -0.000000008558 -0.000000009973 0.000000000000 -0.000000032429 0.000000018879 -0.000000000450 0.000000003631 0.000000003684 0.000000029056 0.000000000239 -0.000000000450 0.000000015680 -0.000000012374 -0.000000038521 -0.000000001026 -0.000000008558 0.000000003631 -0.000000012374 0.000000316878 0.000000003031 0.000000013903 -0.000000009973 0.000000003684 -0.000000038521 0.000000003031 0.000000349891 0.000000002089 0.000000000000 0.000000029056 -0.000000001026 0.000000013903 0.000000002089 0.000000329833 -1 1645005912.204403162003 115 116 0.107279768307 -0.009811669098 0.013827848427 0.000095745570 -0.000565487976 -0.008974907831 0.000000111490 -0.000000032117 0.000000000243 -0.000000008340 -0.000000010631 -0.000000000159 -0.000000032117 0.000000018992 -0.000000000434 0.000000003780 0.000000003786 0.000000030191 0.000000000243 -0.000000000434 0.000000014981 -0.000000011345 -0.000000037764 -0.000000000975 -0.000000008340 0.000000003780 -0.000000011345 0.000000310920 0.000000002683 0.000000013634 -0.000000010631 0.000000003786 -0.000000037764 0.000000002683 0.000000348113 0.000000001943 -0.000000000159 0.000000030191 -0.000000000975 0.000000013634 0.000000001943 0.000000334016 -1 1645005912.304095268250 116 117 0.106183988090 -0.010713891373 0.022572940623 0.000391186142 0.000422801729 -0.011307080283 0.000000109508 -0.000000031579 0.000000000488 -0.000000007880 -0.000000010858 0.000000000029 -0.000000031579 0.000000018604 -0.000000000518 0.000000003456 0.000000003941 0.000000030323 0.000000000488 -0.000000000518 0.000000014084 -0.000000010751 -0.000000036936 -0.000000001088 -0.000000007880 0.000000003456 -0.000000010751 0.000000305213 0.000000003878 0.000000012732 -0.000000010858 0.000000003941 -0.000000036936 0.000000003878 0.000000348036 0.000000002303 0.000000000029 0.000000030323 -0.000000001088 0.000000012732 0.000000002303 0.000000336507 -1 1645005912.402587413788 117 118 0.103151905269 -0.007851188457 0.016492569166 -0.000133641646 -0.000389791053 -0.009425723025 0.000000107590 -0.000000031367 0.000000000521 -0.000000007632 -0.000000009840 -0.000000000231 -0.000000031367 0.000000018521 -0.000000000458 0.000000002965 0.000000003586 0.000000031051 0.000000000521 -0.000000000458 0.000000014103 -0.000000011073 -0.000000039130 -0.000000000784 -0.000000007632 0.000000002965 -0.000000011073 0.000000302435 0.000000007661 0.000000011183 -0.000000009840 0.000000003586 -0.000000039130 0.000000007661 0.000000360891 0.000000001946 -0.000000000231 0.000000031051 -0.000000000784 0.000000011183 0.000000001946 0.000000341818 -1 1645005912.504237890244 118 119 0.102245964023 -0.001922173039 0.001980514565 0.000009290998 0.000695611149 -0.010118021293 0.000000106167 -0.000000031034 0.000000000655 -0.000000007967 -0.000000009572 -0.000000000164 -0.000000031034 0.000000018711 -0.000000000505 0.000000002829 0.000000003587 0.000000033047 0.000000000655 -0.000000000505 0.000000014640 -0.000000011217 -0.000000042101 -0.000000000664 -0.000000007967 0.000000002829 -0.000000011217 0.000000298544 0.000000009862 0.000000010133 -0.000000009572 0.000000003587 -0.000000042101 0.000000009862 0.000000373640 0.000000002635 -0.000000000164 0.000000033047 -0.000000000664 0.000000010133 0.000000002635 0.000000351994 -1 1645005912.604142665863 119 120 0.095380530294 -0.001108012995 -0.003083408103 0.000564478649 0.001636924297 -0.014723225025 0.000000103907 -0.000000030366 0.000000000769 -0.000000008451 -0.000000010034 -0.000000001312 -0.000000030366 0.000000018725 -0.000000000550 0.000000003003 0.000000003747 0.000000034899 0.000000000769 -0.000000000550 0.000000014355 -0.000000010463 -0.000000042267 -0.000000000698 -0.000000008451 0.000000003003 -0.000000010463 0.000000293930 0.000000008522 0.000000009804 -0.000000010034 0.000000003747 -0.000000042267 0.000000008522 0.000000375353 0.000000002938 -0.000000001312 0.000000034899 -0.000000000698 0.000000009804 0.000000002938 0.000000360377 -1 1645005912.702849626541 120 121 0.093286512015 -0.005930663651 0.003868319453 0.000605384071 0.000126378577 -0.016197853324 0.000000099769 -0.000000029448 0.000000000826 -0.000000008585 -0.000000009969 -0.000000002554 -0.000000029448 0.000000018277 -0.000000000493 0.000000003133 0.000000003425 0.000000035023 0.000000000826 -0.000000000493 0.000000013658 -0.000000009979 -0.000000041204 -0.000000000428 -0.000000008585 0.000000003133 -0.000000009979 0.000000289021 0.000000008461 0.000000009199 -0.000000009969 0.000000003425 -0.000000041204 0.000000008461 0.000000371803 0.000000000932 -0.000000002554 0.000000035023 -0.000000000428 0.000000009199 0.000000000932 0.000000361967 -1 1645005912.802519559860 121 122 0.100987173190 0.000714157226 0.010878686943 0.000910503223 -0.000408491888 -0.016040457828 0.000000095734 -0.000000028309 0.000000000990 -0.000000008628 -0.000000009816 -0.000000001987 -0.000000028309 0.000000017686 -0.000000000553 0.000000003081 0.000000003373 0.000000034512 0.000000000990 -0.000000000553 0.000000013034 -0.000000009194 -0.000000040584 -0.000000000408 -0.000000008628 0.000000003081 -0.000000009194 0.000000282506 0.000000007004 0.000000008055 -0.000000009816 0.000000003373 -0.000000040584 0.000000007004 0.000000370148 0.000000000056 -0.000000001987 0.000000034512 -0.000000000408 0.000000008055 0.000000000056 0.000000360170 -1 1645005912.902120351791 122 123 0.105744414544 0.002989777360 0.008055909981 0.000522713016 -0.000712091928 -0.015860847116 0.000000092000 -0.000000026892 0.000000001096 -0.000000007960 -0.000000008934 -0.000000001199 -0.000000026892 0.000000017033 -0.000000000612 0.000000002687 0.000000003198 0.000000034106 0.000000001096 -0.000000000612 0.000000012601 -0.000000008599 -0.000000041253 -0.000000000537 -0.000000007960 0.000000002687 -0.000000008599 0.000000278439 0.000000006637 0.000000006944 -0.000000008934 0.000000003198 -0.000000041253 0.000000006637 0.000000377733 0.000000000207 -0.000000001199 0.000000034106 -0.000000000537 0.000000006944 0.000000000207 0.000000356190 -1 1645005913.005158424377 123 124 0.108371709292 0.001027107202 0.003562412218 0.000635368012 0.002115674008 -0.017401447292 0.000000088452 -0.000000025564 0.000000000949 -0.000000006689 -0.000000006146 -0.000000000882 -0.000000025564 0.000000016427 -0.000000000555 0.000000002086 0.000000002362 0.000000033970 0.000000000949 -0.000000000555 0.000000012646 -0.000000008906 -0.000000043562 -0.000000000533 -0.000000006689 0.000000002086 -0.000000008906 0.000000277482 0.000000008766 0.000000005869 -0.000000006146 0.000000002362 -0.000000043562 0.000000008766 0.000000394236 -0.000000000337 -0.000000000882 0.000000033970 -0.000000000533 0.000000005869 -0.000000000337 0.000000354101 -1 1645005913.103281021118 124 125 0.102758309871 -0.002935497442 -0.002834141245 0.000992244923 0.000603097177 -0.010505902649 0.000000084679 -0.000000024480 0.000000000694 -0.000000006203 -0.000000004434 -0.000000000786 -0.000000024480 0.000000015926 -0.000000000460 0.000000001960 0.000000001912 0.000000034312 0.000000000694 -0.000000000460 0.000000012635 -0.000000008930 -0.000000045449 -0.000000000614 -0.000000006203 0.000000001960 -0.000000008930 0.000000278203 0.000000010319 0.000000005903 -0.000000004434 0.000000001912 -0.000000045449 0.000000010319 0.000000412749 0.000000000610 -0.000000000786 0.000000034312 -0.000000000614 0.000000005903 0.000000000610 0.000000357488 -1 1645005913.203158855438 125 126 0.106915139328 -0.005362769587 0.005671755512 0.000982730101 0.001719758282 -0.007181197185 0.000000079753 -0.000000022565 0.000000000775 -0.000000006067 -0.000000004255 -0.000000000741 -0.000000022565 0.000000015327 -0.000000000457 0.000000001948 0.000000001834 0.000000036065 0.000000000775 -0.000000000457 0.000000012307 -0.000000008166 -0.000000046153 -0.000000000719 -0.000000006067 0.000000001948 -0.000000008166 0.000000275329 0.000000010591 0.000000006327 -0.000000004255 0.000000001834 -0.000000046153 0.000000010591 0.000000425655 0.000000001717 -0.000000000741 0.000000036065 -0.000000000719 0.000000006327 0.000000001717 0.000000367832 -1 1645005913.305967569351 126 127 0.114387020145 -0.004210180518 0.015020722820 -0.000184024553 -0.001805515269 0.000786641202 0.000000073336 -0.000000020355 0.000000000681 -0.000000005855 -0.000000003624 -0.000000000918 -0.000000020355 0.000000014826 -0.000000000406 0.000000001943 0.000000001386 0.000000038378 0.000000000681 -0.000000000406 0.000000012043 -0.000000007381 -0.000000046646 -0.000000000532 -0.000000005855 0.000000001943 -0.000000007381 0.000000270632 0.000000009755 0.000000006505 -0.000000003624 0.000000001386 -0.000000046646 0.000000009755 0.000000431777 0.000000000725 -0.000000000918 0.000000038378 -0.000000000532 0.000000006505 0.000000000725 0.000000380235 -1 1645005913.402428627014 127 128 0.113069559789 -0.000704930784 0.014288948611 -0.000164686473 0.000124000769 0.002486017239 0.000000067177 -0.000000018092 0.000000000770 -0.000000005230 -0.000000002337 -0.000000001447 -0.000000018092 0.000000014158 -0.000000000419 0.000000001621 0.000000000810 0.000000040400 0.000000000770 -0.000000000419 0.000000011887 -0.000000006732 -0.000000048215 -0.000000000372 -0.000000005230 0.000000001621 -0.000000006732 0.000000268588 0.000000010266 0.000000005978 -0.000000002337 0.000000000810 -0.000000048215 0.000000010266 0.000000444703 -0.000000000174 -0.000000001447 0.000000040400 -0.000000000372 0.000000005978 -0.000000000174 0.000000394429 -1 1645005913.504843950272 128 129 0.113183456503 -0.003233214799 0.001167230142 -0.000877923266 -0.000380382302 0.004114345669 0.000000058977 -0.000000015334 0.000000000576 -0.000000004492 -0.000000000609 -0.000000003143 -0.000000015334 0.000000013437 -0.000000000338 0.000000001252 0.000000000348 0.000000043093 0.000000000576 -0.000000000338 0.000000010341 -0.000000006120 -0.000000045149 -0.000000000437 -0.000000004492 0.000000001252 -0.000000006120 0.000000262475 0.000000013212 0.000000005183 -0.000000000609 0.000000000348 -0.000000045149 0.000000013212 0.000000443863 0.000000000910 -0.000000003143 0.000000043093 -0.000000000437 0.000000005183 0.000000000910 0.000000408565 -1 1645005913.601838827133 129 130 0.106480526708 -0.001386640502 -0.011980876123 0.000067753467 -0.000046515530 0.006676571512 0.000000048768 -0.000000012110 0.000000000322 -0.000000003692 -0.000000000074 -0.000000005515 -0.000000012110 0.000000012413 -0.000000000216 0.000000000924 0.000000000176 0.000000045221 0.000000000322 -0.000000000216 0.000000008513 -0.000000005462 -0.000000040526 -0.000000000491 -0.000000003692 0.000000000924 -0.000000005462 0.000000255291 0.000000015909 0.000000004681 -0.000000000074 0.000000000176 -0.000000040526 0.000000015909 0.000000437638 0.000000002802 -0.000000005515 0.000000045221 -0.000000000491 0.000000004681 0.000000002802 0.000000418105 -1 1645005913.703332424164 130 131 0.108468668180 -0.000969605556 -0.002385739928 0.000092510686 0.001524255417 0.001870569352 0.000000039925 -0.000000009382 0.000000000084 -0.000000002806 0.000000000135 -0.000000007330 -0.000000009382 0.000000011452 -0.000000000090 0.000000000564 -0.000000000086 0.000000046436 0.000000000084 -0.000000000090 0.000000007595 -0.000000004539 -0.000000038331 -0.000000000262 -0.000000002806 0.000000000564 -0.000000004539 0.000000246399 0.000000015747 0.000000003991 0.000000000135 -0.000000000086 -0.000000038331 0.000000015747 0.000000435816 0.000000003091 -0.000000007330 0.000000046436 -0.000000000262 0.000000003991 0.000000003091 0.000000424368 -1 1645005913.802533626556 131 132 0.104456210240 -0.004904373024 0.013892430815 -0.000421937248 -0.001963824939 0.001738357414 0.000000033848 -0.000000007783 0.000000000093 -0.000000002527 0.000000000077 -0.000000008221 -0.000000007783 0.000000010852 -0.000000000071 0.000000000554 -0.000000000234 0.000000046899 0.000000000093 -0.000000000071 0.000000007219 -0.000000004204 -0.000000037396 -0.000000000113 -0.000000002527 0.000000000554 -0.000000004204 0.000000236147 0.000000019026 0.000000004261 0.000000000077 -0.000000000234 -0.000000037396 0.000000019026 0.000000432716 0.000000002615 -0.000000008221 0.000000046899 -0.000000000113 0.000000004261 0.000000002615 0.000000427562 -1 1645005913.902822494507 132 133 0.105252268899 -0.005892393425 0.020783173943 -0.000244242694 -0.000148631160 -0.003208653521 0.000000028686 -0.000000006554 0.000000000243 -0.000000002229 -0.000000000013 -0.000000008804 -0.000000006554 0.000000010280 -0.000000000154 0.000000000313 -0.000000000124 0.000000046583 0.000000000243 -0.000000000154 0.000000007239 -0.000000004543 -0.000000037125 -0.000000000235 -0.000000002229 0.000000000313 -0.000000004543 0.000000229949 0.000000026748 0.000000003642 -0.000000000013 -0.000000000124 -0.000000037125 0.000000026748 0.000000428474 0.000000002848 -0.000000008804 0.000000046583 -0.000000000235 0.000000003642 0.000000002848 0.000000427422 -1 1645005914.004937648773 133 134 0.109519905526 -0.010991758570 0.010722232350 0.000333129949 -0.000174623972 -0.002993424422 0.000000024093 -0.000000005515 0.000000000350 -0.000000001671 -0.000000000218 -0.000000008888 -0.000000005515 0.000000009905 -0.000000000203 0.000000000110 -0.000000000103 0.000000046779 0.000000000350 -0.000000000203 0.000000006866 -0.000000005080 -0.000000035414 -0.000000000417 -0.000000001671 0.000000000110 -0.000000005080 0.000000225283 0.000000035405 0.000000003187 -0.000000000218 -0.000000000103 -0.000000035414 0.000000035405 0.000000419305 0.000000003321 -0.000000008888 0.000000046779 -0.000000000417 0.000000003187 0.000000003321 0.000000429289 -1 1645005914.104001283646 134 135 0.110141859157 -0.015877687071 -0.003919154582 0.001363151861 -0.000134739250 0.000119752527 0.000000020081 -0.000000004548 0.000000000494 -0.000000000564 -0.000000000812 -0.000000008086 -0.000000004548 0.000000009513 -0.000000000212 -0.000000000183 -0.000000000079 0.000000046971 0.000000000494 -0.000000000212 0.000000006414 -0.000000005263 -0.000000033246 -0.000000000660 -0.000000000564 -0.000000000183 -0.000000005263 0.000000222499 0.000000041553 0.000000002348 -0.000000000812 -0.000000000079 -0.000000033246 0.000000041553 0.000000404705 0.000000004584 -0.000000008086 0.000000046971 -0.000000000660 0.000000002348 0.000000004584 0.000000434501 -1 1645005914.205260753632 135 136 0.112731195464 -0.016646210746 0.002628785458 0.000209927570 0.000614736609 0.002584125950 0.000000016076 -0.000000003462 0.000000000384 0.000000000165 -0.000000000800 -0.000000006658 -0.000000003462 0.000000009059 -0.000000000133 -0.000000000606 -0.000000000101 0.000000046958 0.000000000384 -0.000000000133 0.000000005887 -0.000000005972 -0.000000030611 -0.000000000603 0.000000000165 -0.000000000606 -0.000000005972 0.000000219212 0.000000046082 -0.000000000162 -0.000000000800 -0.000000000101 -0.000000030611 0.000000046082 0.000000380793 0.000000005566 -0.000000006658 0.000000046958 -0.000000000603 -0.000000000162 0.000000005566 0.000000437954 -1 1645005914.304635286331 136 137 0.110109539783 -0.017998414611 0.012810929145 -0.000245315305 0.000890363266 0.003698770995 0.000000013318 -0.000000002600 0.000000000326 0.000000000152 -0.000000000681 -0.000000005410 -0.000000002600 0.000000008819 -0.000000000077 -0.000000000756 -0.000000000134 0.000000047689 0.000000000326 -0.000000000077 0.000000005647 -0.000000007102 -0.000000029962 -0.000000000471 0.000000000152 -0.000000000756 -0.000000007102 0.000000215325 0.000000049153 -0.000000002005 -0.000000000681 -0.000000000134 -0.000000029962 0.000000049153 0.000000369953 0.000000005821 -0.000000005410 0.000000047689 -0.000000000471 -0.000000002005 0.000000005821 0.000000443851 -1 1645005914.402868032455 137 138 0.111083721384 -0.015259298484 0.014103141251 -0.000356166718 -0.001745079695 0.006088809825 0.000000011488 -0.000000002155 0.000000000288 0.000000000087 -0.000000000634 -0.000000004446 -0.000000002155 0.000000008887 -0.000000000073 -0.000000000761 -0.000000000109 0.000000049551 0.000000000288 -0.000000000073 0.000000005632 -0.000000007521 -0.000000030497 -0.000000000488 0.000000000087 -0.000000000761 -0.000000007521 0.000000212402 0.000000048080 -0.000000002791 -0.000000000634 -0.000000000109 -0.000000030497 0.000000048080 0.000000366926 0.000000005864 -0.000000004446 0.000000049551 -0.000000000488 -0.000000002791 0.000000005864 0.000000458229 -1 1645005914.502975702286 138 139 0.113945330153 -0.011103091303 0.010129852948 -0.000443467816 0.000074664741 0.003419050409 0.000000010923 -0.000000002096 0.000000000221 0.000000000018 -0.000000000160 -0.000000003828 -0.000000002096 0.000000008891 -0.000000000128 -0.000000000737 0.000000000133 0.000000050211 0.000000000221 -0.000000000128 0.000000005571 -0.000000007348 -0.000000031094 -0.000000000767 0.000000000018 -0.000000000737 -0.000000007348 0.000000208241 0.000000044856 -0.000000002913 -0.000000000160 0.000000000133 -0.000000031094 0.000000044856 0.000000367491 0.000000006917 -0.000000003828 0.000000050211 -0.000000000767 -0.000000002913 0.000000006917 0.000000464462 -1 1645005914.606390953064 139 140 0.115763415418 -0.013251559319 -0.002593917162 -0.000387095926 -0.000521830202 0.004558964276 0.000000010125 -0.000000001973 0.000000000276 -0.000000000045 -0.000000000302 -0.000000002573 -0.000000001973 0.000000009055 -0.000000000160 -0.000000000851 0.000000000387 0.000000051580 0.000000000276 -0.000000000160 0.000000005080 -0.000000006897 -0.000000029536 -0.000000000890 -0.000000000045 -0.000000000851 -0.000000006897 0.000000205238 0.000000040500 -0.000000003608 -0.000000000302 0.000000000387 -0.000000029536 0.000000040500 0.000000358067 0.000000007639 -0.000000002573 0.000000051580 -0.000000000890 -0.000000003608 0.000000007639 0.000000474766 -1 1645005914.705168962479 140 141 0.115950679870 -0.007920648289 -0.003581919731 -0.000183384951 -0.000202153026 0.006015096621 0.000000008937 -0.000000001667 0.000000000398 -0.000000000007 -0.000000000886 -0.000000000256 -0.000000001667 0.000000009384 -0.000000000113 -0.000000001058 0.000000000199 0.000000054119 0.000000000398 -0.000000000113 0.000000004302 -0.000000006382 -0.000000025529 -0.000000000504 -0.000000000007 -0.000000001058 -0.000000006382 0.000000203274 0.000000035133 -0.000000005395 -0.000000000886 0.000000000199 -0.000000025529 0.000000035133 0.000000328835 0.000000005386 -0.000000000256 0.000000054119 -0.000000000504 -0.000000005395 0.000000005386 0.000000494144 -1 1645005914.803323030472 141 142 0.113344092687 -0.004513999756 0.003881859525 -0.000081391673 0.001628779102 0.003417929098 0.000000007946 -0.000000001342 0.000000000361 0.000000000164 -0.000000000461 0.000000001892 -0.000000001342 0.000000009145 -0.000000000107 -0.000000001104 0.000000000120 0.000000053134 0.000000000361 -0.000000000107 0.000000003877 -0.000000006332 -0.000000023177 -0.000000000447 0.000000000164 -0.000000001104 -0.000000006332 0.000000198500 0.000000032820 -0.000000006363 -0.000000000461 0.000000000120 -0.000000023177 0.000000032820 0.000000305051 0.000000004342 0.000000001892 0.000000053134 -0.000000000447 -0.000000006363 0.000000004342 0.000000489129 -1 1645005914.904057264328 142 143 0.113932415653 -0.008793837129 0.010399809107 -0.000235218052 -0.001425873177 0.005105286124 0.000000007366 -0.000000001153 0.000000000221 0.000000000379 0.000000000457 0.000000003120 -0.000000001153 0.000000008786 -0.000000000141 -0.000000001114 0.000000000245 0.000000051362 0.000000000221 -0.000000000141 0.000000003860 -0.000000006582 -0.000000022852 -0.000000000726 0.000000000379 -0.000000001114 -0.000000006582 0.000000192639 0.000000031559 -0.000000006610 0.000000000457 0.000000000245 -0.000000022852 0.000000031559 0.000000290550 0.000000005271 0.000000003120 0.000000051362 -0.000000000726 -0.000000006610 0.000000005271 0.000000477340 -1 1645005915.005204200745 143 144 0.114962510911 -0.012358710079 0.012393871611 -0.000551754572 -0.000248518402 0.003903856192 0.000000007433 -0.000000001098 0.000000000173 0.000000000396 0.000000000817 0.000000003762 -0.000000001098 0.000000008813 -0.000000000189 -0.000000001302 0.000000000582 0.000000051927 0.000000000173 -0.000000000189 0.000000003938 -0.000000006922 -0.000000022827 -0.000000001046 0.000000000396 -0.000000001302 -0.000000006922 0.000000188507 0.000000031757 -0.000000007689 0.000000000817 0.000000000582 -0.000000022827 0.000000031757 0.000000280243 0.000000007393 0.000000003762 0.000000051927 -0.000000001046 -0.000000007689 0.000000007393 0.000000482670 -1 1645005915.104339361191 144 145 0.108638769015 -0.013109909901 0.001280725320 0.000360075563 -0.000501910880 0.003610744318 0.000000007793 -0.000000001032 0.000000000172 0.000000000318 0.000000000989 0.000000004597 -0.000000001032 0.000000008853 -0.000000000219 -0.000000001613 0.000000000892 0.000000052595 0.000000000172 -0.000000000219 0.000000004122 -0.000000007546 -0.000000023608 -0.000000001206 0.000000000318 -0.000000001613 -0.000000007546 0.000000185838 0.000000036183 -0.000000009621 0.000000000989 0.000000000892 -0.000000023608 0.000000036183 0.000000280865 0.000000009177 0.000000004597 0.000000052595 -0.000000001206 -0.000000009621 0.000000009177 0.000000490642 -1 1645005915.205692529678 145 146 0.109329578702 -0.013304774566 0.000306145880 -0.000023011733 0.000369480071 0.005562445904 0.000000008439 -0.000000000978 0.000000000142 0.000000000381 0.000000001532 0.000000005640 -0.000000000978 0.000000008798 -0.000000000212 -0.000000001840 0.000000000941 0.000000053029 0.000000000142 -0.000000000212 0.000000004685 -0.000000008117 -0.000000027229 -0.000000001233 0.000000000381 -0.000000001840 -0.000000008117 0.000000182654 0.000000041408 -0.000000011114 0.000000001532 0.000000000941 -0.000000027229 0.000000041408 0.000000305374 0.000000009971 0.000000005640 0.000000053029 -0.000000001233 -0.000000011114 0.000000009971 0.000000499247 -1 1645005915.303473472595 146 147 0.102615336962 -0.011723016179 0.016921966183 -0.000598724662 -0.000883449845 0.009912420693 0.000000009050 -0.000000000931 0.000000000141 0.000000000283 0.000000001632 0.000000006520 -0.000000000931 0.000000008841 -0.000000000254 -0.000000001783 0.000000001294 0.000000053986 0.000000000141 -0.000000000254 0.000000005341 -0.000000008689 -0.000000031566 -0.000000001523 0.000000000283 -0.000000001783 -0.000000008689 0.000000180735 0.000000046782 -0.000000010967 0.000000001632 0.000000001294 -0.000000031566 0.000000046782 0.000000337155 0.000000012174 0.000000006520 0.000000053986 -0.000000001523 -0.000000010967 0.000000012174 0.000000511691 -1 1645005915.403654336929 147 148 0.103466727897 -0.010311299427 0.019111226280 -0.000605924141 -0.000721619272 0.014142457072 0.000000009872 -0.000000000937 0.000000000170 0.000000000104 0.000000001623 0.000000006936 -0.000000000937 0.000000009038 -0.000000000289 -0.000000001859 0.000000001533 0.000000055630 0.000000000170 -0.000000000289 0.000000005662 -0.000000008982 -0.000000033316 -0.000000001699 0.000000000104 -0.000000001859 -0.000000008982 0.000000179291 0.000000050127 -0.000000011628 0.000000001623 0.000000001533 -0.000000033316 0.000000050127 0.000000349687 0.000000013494 0.000000006936 0.000000055630 -0.000000001699 -0.000000011628 0.000000013494 0.000000526085 -1 1645005915.502804994583 148 149 0.102555175381 -0.007097360397 0.012460654451 -0.001109214232 -0.001235813727 0.016759657315 0.000000010898 -0.000000001092 0.000000000180 0.000000000034 0.000000001717 0.000000006432 -0.000000001092 0.000000009110 -0.000000000312 -0.000000001774 0.000000001584 0.000000056414 0.000000000180 -0.000000000312 0.000000005889 -0.000000008997 -0.000000034270 -0.000000001851 0.000000000034 -0.000000001774 -0.000000008997 0.000000178153 0.000000052144 -0.000000011187 0.000000001717 0.000000001584 -0.000000034270 0.000000052144 0.000000356560 0.000000013909 0.000000006432 0.000000056414 -0.000000001851 -0.000000011187 0.000000013909 0.000000534819 -1 1645005915.606675386429 149 150 0.110023677701 -0.005261608351 -0.001025304830 -0.000900306716 -0.001729175039 0.017897628554 0.000000011791 -0.000000001314 0.000000000193 -0.000000000199 0.000000001536 0.000000005499 -0.000000001314 0.000000009095 -0.000000000322 -0.000000001698 0.000000001815 0.000000056489 0.000000000193 -0.000000000322 0.000000006056 -0.000000009260 -0.000000035203 -0.000000001976 -0.000000000199 -0.000000001698 -0.000000009260 0.000000180591 0.000000056392 -0.000000010737 0.000000001536 0.000000001815 -0.000000035203 0.000000056392 0.000000367602 0.000000015758 0.000000005499 0.000000056489 -0.000000001976 -0.000000010737 0.000000015758 0.000000538970 -1 1645005915.706191539764 150 151 0.108997921836 -0.002332104651 -0.007968681270 -0.000641057737 0.000297247851 0.013744307307 0.000000012496 -0.000000001488 0.000000000182 -0.000000000437 0.000000001129 0.000000004476 -0.000000001488 0.000000009204 -0.000000000349 -0.000000001745 0.000000002295 0.000000057627 0.000000000182 -0.000000000349 0.000000006180 -0.000000009543 -0.000000036359 -0.000000002262 -0.000000000437 -0.000000001745 -0.000000009543 0.000000185160 0.000000061278 -0.000000010792 0.000000001129 0.000000002295 -0.000000036359 0.000000061278 0.000000385670 0.000000019334 0.000000004476 0.000000057627 -0.000000002262 -0.000000010792 0.000000019334 0.000000551459 -1 1645005915.803260087967 151 152 0.106514602397 -0.001770007540 -0.000096349759 -0.000752589415 0.000118381130 0.009137181083 0.000000012917 -0.000000001667 0.000000000103 -0.000000000308 0.000000001192 0.000000002903 -0.000000001667 0.000000009141 -0.000000000325 -0.000000001618 0.000000002128 0.000000057675 0.000000000103 -0.000000000325 0.000000006523 -0.000000009497 -0.000000039281 -0.000000002198 -0.000000000308 -0.000000001618 -0.000000009497 0.000000189290 0.000000063224 -0.000000009134 0.000000001192 0.000000002128 -0.000000039281 0.000000063224 0.000000417011 0.000000018931 0.000000002903 0.000000057675 -0.000000002198 -0.000000009134 0.000000018931 0.000000555912 -1 1645005915.905807971954 152 153 0.109997173206 -0.004906461686 0.014498026032 -0.000862247837 0.000085107174 0.006427930427 0.000000013310 -0.000000001919 0.000000000086 -0.000000000244 0.000000001170 0.000000001331 -0.000000001919 0.000000008982 -0.000000000223 -0.000000001470 0.000000001408 0.000000056875 0.000000000086 -0.000000000223 0.000000006917 -0.000000008840 -0.000000042953 -0.000000001544 -0.000000000244 -0.000000001470 -0.000000008840 0.000000192614 0.000000059942 -0.000000007491 0.000000001170 0.000000001408 -0.000000042953 0.000000059942 0.000000453104 0.000000014715 0.000000001331 0.000000056875 -0.000000001544 -0.000000007491 0.000000014715 0.000000552905 -1 1645005916.003596782684 153 154 0.105956714038 -0.007930247692 0.017803331311 -0.000325566987 -0.000424651970 0.000784271079 0.000000013892 -0.000000002247 0.000000000116 -0.000000000302 0.000000000924 -0.000000000604 -0.000000002247 0.000000009117 -0.000000000160 -0.000000001400 0.000000000976 0.000000058527 0.000000000116 -0.000000000160 0.000000007137 -0.000000007719 -0.000000045692 -0.000000001110 -0.000000000302 -0.000000001400 -0.000000007719 0.000000198654 0.000000053623 -0.000000006581 0.000000000924 0.000000000976 -0.000000045692 0.000000053623 0.000000487022 0.000000012262 -0.000000000604 0.000000058527 -0.000000001110 -0.000000006581 0.000000012262 0.000000572349 -1 1645005916.099525690079 154 155 0.106714377159 -0.007351031551 0.007507753034 0.000196149893 -0.001095119436 -0.001110160472 0.000000014587 -0.000000002603 0.000000000179 -0.000000000321 0.000000000579 -0.000000002608 -0.000000002603 0.000000009454 -0.000000000165 -0.000000001330 0.000000001027 0.000000061678 0.000000000179 -0.000000000165 0.000000007113 -0.000000007149 -0.000000046636 -0.000000001161 -0.000000000321 -0.000000001330 -0.000000007149 0.000000206337 0.000000051754 -0.000000005643 0.000000000579 0.000000001027 -0.000000046636 0.000000051754 0.000000508473 0.000000013467 -0.000000002608 0.000000061678 -0.000000001161 -0.000000005643 0.000000013467 0.000000604801 -1 1645005916.201919555664 155 156 0.114214238464 -0.005627023360 -0.006587383337 0.000405811911 0.001118760483 -0.003412120900 0.000000016892 -0.000000003571 0.000000000307 -0.000000000279 0.000000000065 -0.000000005502 -0.000000003571 0.000000010098 -0.000000000203 -0.000000001503 0.000000001164 0.000000066149 0.000000000307 -0.000000000203 0.000000007107 -0.000000007131 -0.000000047845 -0.000000001264 -0.000000000279 -0.000000001503 -0.000000007131 0.000000211613 0.000000054896 -0.000000007056 0.000000000065 0.000000001164 -0.000000047845 0.000000054896 0.000000531070 0.000000014713 -0.000000005502 0.000000066149 -0.000000001264 -0.000000007056 0.000000014713 0.000000647179 -1 1645005916.305266141891 156 157 0.111308342148 -0.009772812253 -0.005809218470 -0.000158830582 -0.000215490923 0.000333338897 0.000000024402 -0.000000006436 0.000000000493 -0.000000000261 -0.000000000377 -0.000000011240 -0.000000006436 0.000000011477 -0.000000000292 -0.000000001398 0.000000001461 0.000000072198 0.000000000493 -0.000000000292 0.000000007568 -0.000000007942 -0.000000052886 -0.000000001576 -0.000000000261 -0.000000001398 -0.000000007942 0.000000218297 0.000000065152 -0.000000006844 -0.000000000377 0.000000001461 -0.000000052886 0.000000065152 0.000000590097 0.000000017471 -0.000000011240 0.000000072198 -0.000000001576 -0.000000006844 0.000000017471 0.000000700700 -1 1645005916.402242660522 157 158 0.098861612320 -0.010532723695 0.012346443386 0.000302995827 0.000414997200 0.001602970315 0.000000037109 -0.000000010805 0.000000000821 -0.000000000804 -0.000000001279 -0.000000016111 -0.000000010805 0.000000012886 -0.000000000505 -0.000000000852 0.000000002599 0.000000074435 0.000000000821 -0.000000000505 0.000000008686 -0.000000010526 -0.000000062013 -0.000000002548 -0.000000000804 -0.000000000852 -0.000000010526 0.000000231195 0.000000086125 -0.000000003957 -0.000000001279 0.000000002599 -0.000000062013 0.000000086125 0.000000677017 0.000000025691 -0.000000016111 0.000000074435 -0.000000002548 -0.000000003957 0.000000025691 0.000000727276 -1 1645005916.503258705139 158 159 0.102502168086 -0.009093252456 0.014363853030 -0.000283008408 -0.000358165826 0.006681060732 0.000000051140 -0.000000015225 0.000000001216 -0.000000002070 -0.000000003404 -0.000000014930 -0.000000015225 0.000000013941 -0.000000000656 -0.000000000227 0.000000003539 0.000000072261 0.000000001216 -0.000000000656 0.000000009605 -0.000000014137 -0.000000068917 -0.000000002838 -0.000000002070 -0.000000000227 -0.000000014137 0.000000246310 0.000000111562 -0.000000001761 -0.000000003404 0.000000003539 -0.000000068917 0.000000111562 0.000000735475 0.000000029040 -0.000000014930 0.000000072261 -0.000000002838 -0.000000001761 0.000000029040 0.000000724038 -1 1645005916.603333711624 159 160 0.110584118510 -0.006969165435 0.006755957308 -0.000547151088 -0.002765490415 0.010723492151 0.000000064963 -0.000000019500 0.000000001613 -0.000000003508 -0.000000006554 -0.000000011097 -0.000000019500 0.000000015237 -0.000000000783 0.000000000383 0.000000004607 0.000000071746 0.000000001613 -0.000000000783 0.000000010020 -0.000000017014 -0.000000072512 -0.000000002905 -0.000000003508 0.000000000383 -0.000000017014 0.000000260411 0.000000131565 -0.000000000900 -0.000000006554 0.000000004607 -0.000000072512 0.000000131565 0.000000769488 0.000000030950 -0.000000011097 0.000000071746 -0.000000002905 -0.000000000900 0.000000030950 0.000000738042 -1 1645005916.703304529190 160 161 0.115867178906 -0.001405365939 -0.006941536777 -0.000217074623 0.000672686205 0.003645953857 0.000000078026 -0.000000023457 0.000000002054 -0.000000004985 -0.000000009324 -0.000000008604 -0.000000023457 0.000000016517 -0.000000000950 0.000000000749 0.000000005758 0.000000073233 0.000000002054 -0.000000000950 0.000000010587 -0.000000019544 -0.000000077702 -0.000000003100 -0.000000004985 0.000000000749 -0.000000019544 0.000000277224 0.000000150069 -0.000000002565 -0.000000009324 0.000000005758 -0.000000077702 0.000000150069 0.000000821178 0.000000033645 -0.000000008604 0.000000073233 -0.000000003100 -0.000000002565 0.000000033645 0.000000772113 -1 1645005916.804172515869 161 162 0.117302909972 0.000295419838 -0.016017415108 -0.000106164346 0.000901201232 -0.001277739925 0.000000088382 -0.000000026988 0.000000002179 -0.000000006324 -0.000000011465 -0.000000008722 -0.000000026988 0.000000017842 -0.000000000989 0.000000001031 0.000000006457 0.000000076083 0.000000002179 -0.000000000989 0.000000010901 -0.000000021540 -0.000000081165 -0.000000002920 -0.000000006324 0.000000001031 -0.000000021540 0.000000295157 0.000000164011 -0.000000004981 -0.000000011465 0.000000006457 -0.000000081165 0.000000164011 0.000000859137 0.000000032540 -0.000000008722 0.000000076083 -0.000000002920 -0.000000004981 0.000000032540 0.000000810795 -1 1645005916.907030344009 162 163 0.117113502778 -0.001332837421 -0.000358495722 0.000214441813 0.001777967441 -0.003905204155 0.000000096512 -0.000000029855 0.000000002263 -0.000000007359 -0.000000013389 -0.000000009255 -0.000000029855 0.000000018930 -0.000000001023 0.000000001324 0.000000007208 0.000000077916 0.000000002263 -0.000000001023 0.000000011112 -0.000000023344 -0.000000084006 -0.000000002951 -0.000000007359 0.000000001324 -0.000000023344 0.000000313829 0.000000177054 -0.000000006430 -0.000000013389 0.000000007208 -0.000000084006 0.000000177054 0.000000893655 0.000000033005 -0.000000009255 0.000000077916 -0.000000002951 -0.000000006430 0.000000033005 0.000000834816 -1 1645005917.005794048309 163 164 0.112162385129 -0.003856647165 0.014586076375 -0.000117380248 -0.001608061862 -0.001170630734 0.000000103324 -0.000000032168 0.000000002342 -0.000000008716 -0.000000016076 -0.000000006607 -0.000000032168 0.000000019618 -0.000000000993 0.000000001842 0.000000007717 0.000000077445 0.000000002342 -0.000000000993 0.000000011325 -0.000000025511 -0.000000087156 -0.000000002530 -0.000000008716 0.000000001842 -0.000000025511 0.000000334949 0.000000195314 -0.000000006232 -0.000000016076 0.000000007717 -0.000000087156 0.000000195314 0.000000934604 0.000000029769 -0.000000006607 0.000000077445 -0.000000002530 -0.000000006232 0.000000029769 0.000000849058 -1 1645005917.104857444763 164 165 0.111741027745 -0.003567459006 0.022616849137 0.000236524934 0.000178204487 -0.002832779338 0.000000108247 -0.000000033657 0.000000002538 -0.000000009343 -0.000000017174 -0.000000003909 -0.000000033657 0.000000020124 -0.000000000963 0.000000001696 0.000000007303 0.000000078131 0.000000002538 -0.000000000963 0.000000011442 -0.000000026989 -0.000000089241 -0.000000001718 -0.000000009343 0.000000001696 -0.000000026989 0.000000352166 0.000000208528 -0.000000009427 -0.000000017174 0.000000007303 -0.000000089241 0.000000208528 0.000000963343 0.000000021895 -0.000000003909 0.000000078131 -0.000000001718 -0.000000009427 0.000000021895 0.000000871104 -1 1645005917.207338571548 165 166 0.113903906676 -0.004144900189 0.009725110224 0.000086239947 -0.000462914316 -0.004704134184 0.000000110910 -0.000000034448 0.000000002432 -0.000000009395 -0.000000015619 -0.000000003444 -0.000000034448 0.000000020431 -0.000000000941 0.000000001671 0.000000006954 0.000000079230 0.000000002432 -0.000000000941 0.000000011953 -0.000000028963 -0.000000094423 -0.000000001837 -0.000000009395 0.000000001671 -0.000000028963 0.000000367870 0.000000226360 -0.000000010076 -0.000000015619 0.000000006954 -0.000000094423 0.000000226360 0.000001016065 0.000000022453 -0.000000003444 0.000000079230 -0.000000001837 -0.000000010076 0.000000022453 0.000000886686 -1 1645005917.305225610733 166 167 0.106350650780 -0.008006138847 -0.004136760500 0.000217735543 0.000793264977 -0.005450629162 0.000000112457 -0.000000034975 0.000000002459 -0.000000009900 -0.000000014669 -0.000000004888 -0.000000034975 0.000000020691 -0.000000001068 0.000000002175 0.000000007608 0.000000081541 0.000000002459 -0.000000001068 0.000000012417 -0.000000030753 -0.000000100017 -0.000000002845 -0.000000009900 0.000000002175 -0.000000030753 0.000000385554 0.000000244403 -0.000000007583 -0.000000014669 0.000000007608 -0.000000100017 0.000000244403 0.000001081297 0.000000030238 -0.000000004888 0.000000081541 -0.000000002845 -0.000000007583 0.000000030238 0.000000913045 -1 1645005917.404884338379 167 168 0.098821449951 -0.014936367447 0.003157378141 0.000960141444 0.000553150047 -0.007218868305 0.000000114058 -0.000000035351 0.000000002309 -0.000000010234 -0.000000013984 -0.000000006367 -0.000000035351 0.000000020849 -0.000000000967 0.000000002122 0.000000006908 0.000000083703 0.000000002309 -0.000000000967 0.000000012562 -0.000000031523 -0.000000102262 -0.000000002356 -0.000000010234 0.000000002122 -0.000000031523 0.000000397207 0.000000252081 -0.000000009475 -0.000000013984 0.000000006908 -0.000000102262 0.000000252081 0.000001111583 0.000000025955 -0.000000006367 0.000000083703 -0.000000002356 -0.000000009475 0.000000025955 0.000000936514 -1 1645005917.504653453827 168 169 0.094186437816 -0.015526879719 0.019416391189 0.000802075926 0.000404113705 -0.007307554869 0.000000114991 -0.000000035620 0.000000002454 -0.000000011103 -0.000000015611 -0.000000006733 -0.000000035620 0.000000020791 -0.000000000965 0.000000002157 0.000000007149 0.000000083370 0.000000002454 -0.000000000965 0.000000012360 -0.000000031514 -0.000000100745 -0.000000002049 -0.000000011103 0.000000002157 -0.000000031514 0.000000399690 0.000000251423 -0.000000012007 -0.000000015611 0.000000007149 -0.000000100745 0.000000251423 0.000001101197 0.000000025036 -0.000000006733 0.000000083370 -0.000000002049 -0.000000012007 0.000000025036 0.000000937610 -1 1645005917.605074167252 169 170 0.100592303433 -0.011605821285 0.019967308871 0.000000189307 -0.002053301407 -0.000766444936 0.000000116137 -0.000000036111 0.000000002855 -0.000000013056 -0.000000020038 -0.000000003199 -0.000000036111 0.000000021073 -0.000000001193 0.000000003208 0.000000009429 0.000000083889 0.000000002855 -0.000000001193 0.000000011993 -0.000000030942 -0.000000098311 -0.000000002956 -0.000000013056 0.000000003208 -0.000000030942 0.000000398169 0.000000245893 -0.000000009126 -0.000000020038 0.000000009429 -0.000000098311 0.000000245893 0.000001086821 0.000000034002 -0.000000003199 0.000000083889 -0.000000002956 -0.000000009126 0.000000034002 0.000000959796 -1 1645005917.706811666489 170 171 0.107970526617 -0.008036241272 0.010229935133 -0.000213881979 -0.001054988989 0.000031095351 0.000000118125 -0.000000036312 0.000000003318 -0.000000014955 -0.000000023459 0.000000001488 -0.000000036312 0.000000021160 -0.000000001345 0.000000003695 0.000000010662 0.000000084616 0.000000003318 -0.000000001345 0.000000011958 -0.000000030573 -0.000000098713 -0.000000003110 -0.000000014955 0.000000003695 -0.000000030573 0.000000397435 0.000000242189 -0.000000009785 -0.000000023459 0.000000010662 -0.000000098713 0.000000242189 0.000001096053 0.000000036591 0.000000001488 0.000000084616 -0.000000003110 -0.000000009785 0.000000036591 0.000000988045 -1 1645005917.804672718048 171 172 0.105731298009 -0.005919607258 -0.005833779357 -0.000045148203 0.001018874621 -0.000751755422 0.000000121189 -0.000000036830 0.000000003468 -0.000000015684 -0.000000023681 0.000000004698 -0.000000036830 0.000000021195 -0.000000001433 0.000000003788 0.000000011194 0.000000084668 0.000000003468 -0.000000001433 0.000000012248 -0.000000031571 -0.000000101807 -0.000000003557 -0.000000015684 0.000000003788 -0.000000031571 0.000000404763 0.000000251181 -0.000000010529 -0.000000023681 0.000000011194 -0.000000101807 0.000000251181 0.000001129929 0.000000041603 0.000000004698 0.000000084668 -0.000000003557 -0.000000010529 0.000000041603 0.000001008043 -1 1645005917.904610872269 172 173 0.103855440730 -0.008847785631 -0.010570833324 0.000073912894 0.000524173989 -0.000244240168 0.000000124632 -0.000000037627 0.000000003556 -0.000000016897 -0.000000025958 0.000000007236 -0.000000037627 0.000000021390 -0.000000001541 0.000000004351 0.000000012531 0.000000085042 0.000000003556 -0.000000001541 0.000000012424 -0.000000032522 -0.000000104071 -0.000000004167 -0.000000016897 0.000000004351 -0.000000032522 0.000000412569 0.000000261172 -0.000000009421 -0.000000025958 0.000000012531 -0.000000104071 0.000000261172 0.000001156601 0.000000046360 0.000000007236 0.000000085042 -0.000000004167 -0.000000009421 0.000000046360 0.000001026787 -1 1645005918.005419969559 173 174 0.100505315538 -0.010103880265 0.006164128240 0.000413598049 0.001946711804 -0.004386893254 0.000000127051 -0.000000038318 0.000000003856 -0.000000018597 -0.000000029537 0.000000006021 -0.000000038318 0.000000021496 -0.000000001582 0.000000004785 0.000000013137 0.000000085273 0.000000003856 -0.000000001582 0.000000012143 -0.000000032181 -0.000000103060 -0.000000003657 -0.000000018597 0.000000004785 -0.000000032181 0.000000415569 0.000000261713 -0.000000011012 -0.000000029537 0.000000013137 -0.000000103060 0.000000261713 0.000001158654 0.000000041063 0.000000006021 0.000000085273 -0.000000003657 -0.000000011012 0.000000041063 0.000001031052 -1 1645005918.106912612915 174 175 0.104243823486 -0.015398520529 0.017609900953 0.000251278556 -0.001321653647 -0.002699090350 0.000000129650 -0.000000039225 0.000000003859 -0.000000019970 -0.000000031925 0.000000007468 -0.000000039225 0.000000021632 -0.000000001548 0.000000005349 0.000000013611 0.000000083970 0.000000003859 -0.000000001548 0.000000012009 -0.000000032668 -0.000000103462 -0.000000003323 -0.000000019970 0.000000005349 -0.000000032668 0.000000423439 0.000000269388 -0.000000010299 -0.000000031925 0.000000013611 -0.000000103462 0.000000269388 0.000001178424 0.000000038174 0.000000007468 0.000000083970 -0.000000003323 -0.000000010299 0.000000038174 0.000001031326 -1 1645005918.205326795578 175 176 0.105018001061 -0.016620276316 0.016583575074 0.000471605107 -0.001302930249 -0.006284028454 0.000000132952 -0.000000039810 0.000000004127 -0.000000022074 -0.000000034193 0.000000010673 -0.000000039810 0.000000021502 -0.000000001615 0.000000005775 0.000000014293 0.000000082301 0.000000004127 -0.000000001615 0.000000012306 -0.000000034229 -0.000000107844 -0.000000003231 -0.000000022074 0.000000005775 -0.000000034229 0.000000435942 0.000000288374 -0.000000012012 -0.000000034193 0.000000014293 -0.000000107844 0.000000288374 0.000001238900 0.000000038299 0.000000010673 0.000000082301 -0.000000003231 -0.000000012012 0.000000038299 0.000001036436 -1 1645005918.304870605469 176 177 0.104912611278 -0.018585854244 0.001515240424 0.000485324157 0.001946316449 -0.010479077493 0.000000136347 -0.000000040849 0.000000004059 -0.000000022351 -0.000000030809 0.000000009278 -0.000000040849 0.000000021957 -0.000000001504 0.000000004860 0.000000012760 0.000000085030 0.000000004059 -0.000000001504 0.000000012626 -0.000000035744 -0.000000112385 -0.000000002494 -0.000000022351 0.000000004860 -0.000000035744 0.000000448023 0.000000308545 -0.000000021449 -0.000000030809 0.000000012760 -0.000000112385 0.000000308545 0.000001300935 0.000000034558 0.000000009278 0.000000085030 -0.000000002494 -0.000000021449 0.000000034558 0.000001068746 -1 1645005918.405547380447 177 178 0.104792166995 -0.023226516703 -0.007056980576 0.000267709258 0.000474336908 -0.007207889728 0.000000138617 -0.000000041762 0.000000003857 -0.000000022163 -0.000000029275 0.000000010678 -0.000000041762 0.000000022377 -0.000000001391 0.000000004532 0.000000011951 0.000000086241 0.000000003857 -0.000000001391 0.000000012725 -0.000000037131 -0.000000114801 -0.000000001984 -0.000000022163 0.000000004532 -0.000000037131 0.000000457244 0.000000327019 -0.000000025297 -0.000000029275 0.000000011951 -0.000000114801 0.000000327019 0.000001339780 0.000000031499 0.000000010678 0.000000086241 -0.000000001984 -0.000000025297 0.000000031499 0.000001092396 -1 1645005918.505361080170 178 179 0.102238431531 -0.023908947381 0.006589654277 0.000655843708 0.000453140817 -0.001821223501 0.000000140246 -0.000000042197 0.000000004132 -0.000000023003 -0.000000031421 0.000000015985 -0.000000042197 0.000000022353 -0.000000001641 0.000000005629 0.000000014034 0.000000084232 0.000000004132 -0.000000001641 0.000000012973 -0.000000038726 -0.000000118299 -0.000000003433 -0.000000023003 0.000000005629 -0.000000038726 0.000000462890 0.000000346326 -0.000000018649 -0.000000031421 0.000000014034 -0.000000118299 0.000000346326 0.000001385059 0.000000044386 0.000000015985 0.000000084232 -0.000000003433 -0.000000018649 0.000000044386 0.000001097883 -1 1645005918.605193614960 179 180 0.107683008826 -0.022764995092 0.017514407486 -0.000368667307 -0.001653800364 0.007067164909 0.000000141481 -0.000000042435 0.000000004360 -0.000000024495 -0.000000034469 0.000000024183 -0.000000042435 0.000000022389 -0.000000001837 0.000000006705 0.000000016054 0.000000082514 0.000000004360 -0.000000001837 0.000000013111 -0.000000040013 -0.000000120651 -0.000000004389 -0.000000024495 0.000000006705 -0.000000040013 0.000000466496 0.000000363613 -0.000000014517 -0.000000034469 0.000000016054 -0.000000120651 0.000000363613 0.000001419210 0.000000053423 0.000000024183 0.000000082514 -0.000000004389 -0.000000014517 0.000000053423 0.000001113406 -1 1645005918.704743146896 180 181 0.111753784251 -0.017339671469 0.014827032018 -0.000550269913 -0.000679567626 0.009481384874 0.000000142307 -0.000000041952 0.000000004661 -0.000000025741 -0.000000036641 0.000000033488 -0.000000041952 0.000000022051 -0.000000001803 0.000000006655 0.000000015521 0.000000081558 0.000000004661 -0.000000001803 0.000000012906 -0.000000039951 -0.000000119907 -0.000000003114 -0.000000025741 0.000000006655 -0.000000039951 0.000000465724 0.000000367203 -0.000000019105 -0.000000036641 0.000000015521 -0.000000119907 0.000000367203 0.000001423450 0.000000042472 0.000000033488 0.000000081558 -0.000000003114 -0.000000019105 0.000000042472 0.000001147641 -1 1645005918.806400060654 181 182 0.113872331121 -0.014001660104 -0.000550563706 -0.000917522817 -0.000618426666 0.010240698071 0.000000143709 -0.000000041830 0.000000004468 -0.000000025299 -0.000000034644 0.000000039964 -0.000000041830 0.000000022150 -0.000000001671 0.000000005770 0.000000014366 0.000000084352 0.000000004468 -0.000000001671 0.000000012897 -0.000000040281 -0.000000121040 -0.000000002385 -0.000000025299 0.000000005770 -0.000000040281 0.000000469448 0.000000373553 -0.000000026765 -0.000000034644 0.000000014366 -0.000000121040 0.000000373553 0.000001447927 0.000000037412 0.000000039964 0.000000084352 -0.000000002385 -0.000000026765 0.000000037412 0.000001214810 -1 1645005918.905241250992 182 183 0.108627949209 -0.010341888855 -0.014273654629 -0.000517403403 0.001641954984 0.006780294180 0.000000145842 -0.000000041995 0.000000004312 -0.000000023934 -0.000000031977 0.000000043324 -0.000000041995 0.000000022211 -0.000000001575 0.000000004796 0.000000013177 0.000000086375 0.000000004312 -0.000000001575 0.000000013350 -0.000000041889 -0.000000127167 -0.000000001918 -0.000000023934 0.000000004796 -0.000000041889 0.000000478700 0.000000395388 -0.000000032141 -0.000000031977 0.000000013177 -0.000000127167 0.000000395388 0.000001528684 0.000000033922 0.000000043324 0.000000086375 -0.000000001918 -0.000000032141 0.000000033922 0.000001262581 -1 1645005919.006230831146 183 184 0.111090918097 -0.011375888755 -0.011195486808 -0.000492323663 -0.000760811117 0.007360552053 0.000000147026 -0.000000042366 0.000000004035 -0.000000022992 -0.000000032173 0.000000045126 -0.000000042366 0.000000022179 -0.000000001481 0.000000004851 0.000000013121 0.000000085324 0.000000004035 -0.000000001481 0.000000013612 -0.000000043406 -0.000000131031 -0.000000001914 -0.000000022992 0.000000004851 -0.000000043406 0.000000485597 0.000000416394 -0.000000028771 -0.000000032173 0.000000013121 -0.000000131031 0.000000416394 0.000001580709 0.000000032816 0.000000045126 0.000000085324 -0.000000001914 -0.000000028771 0.000000032816 0.000001269063 -1 1645005919.103321552277 184 185 0.106064541975 -0.013212314792 0.007933789346 -0.000093460789 -0.000240302902 0.002770392774 0.000000147409 -0.000000042125 0.000000004336 -0.000000023061 -0.000000034649 0.000000046124 -0.000000042125 0.000000021985 -0.000000001589 0.000000004937 0.000000014153 0.000000085861 0.000000004336 -0.000000001589 0.000000013574 -0.000000043836 -0.000000132647 -0.000000002199 -0.000000023061 0.000000004937 -0.000000043836 0.000000489170 0.000000427692 -0.000000028221 -0.000000034649 0.000000014153 -0.000000132647 0.000000427692 0.000001615546 0.000000036449 0.000000046124 0.000000085861 -0.000000002199 -0.000000028221 0.000000036449 0.000001285384 -1 1645005919.204598665237 185 186 0.109583227748 -0.018751742246 0.015930061005 -0.000207377330 -0.000969420842 -0.001611917201 0.000000147737 -0.000000042240 0.000000004321 -0.000000022740 -0.000000033885 0.000000043695 -0.000000042240 0.000000022018 -0.000000001592 0.000000004837 0.000000014065 0.000000087852 0.000000004321 -0.000000001592 0.000000013380 -0.000000043618 -0.000000132567 -0.000000002397 -0.000000022740 0.000000004837 -0.000000043618 0.000000492109 0.000000430403 -0.000000028113 -0.000000033885 0.000000014065 -0.000000132567 0.000000430403 0.000001632040 0.000000039106 0.000000043695 0.000000087852 -0.000000002397 -0.000000028113 0.000000039106 0.000001310151 -1 1645005919.304369688034 186 187 0.105935726310 -0.021962018124 0.012691866608 0.000577732489 0.000249586532 -0.006153890357 0.000000149020 -0.000000042831 0.000000003937 -0.000000021091 -0.000000027669 0.000000039205 -0.000000042831 0.000000022289 -0.000000001497 0.000000004450 0.000000012366 0.000000090284 0.000000003937 -0.000000001497 0.000000013677 -0.000000045058 -0.000000137345 -0.000000002732 -0.000000021091 0.000000004450 -0.000000045058 0.000000500943 0.000000450114 -0.000000025733 -0.000000027669 0.000000012366 -0.000000137345 0.000000450114 0.000001702000 0.000000041449 0.000000039205 0.000000090284 -0.000000002732 -0.000000025733 0.000000041449 0.000001329947 -1 1645005919.404906749725 187 188 0.107178844297 -0.022750150514 -0.003656608090 0.000369946408 -0.000252619318 -0.007040696870 0.000000150210 -0.000000043366 0.000000003672 -0.000000019261 -0.000000023707 0.000000038513 -0.000000043366 0.000000022455 -0.000000001373 0.000000003685 0.000000010707 0.000000090631 0.000000003672 -0.000000001373 0.000000014393 -0.000000048027 -0.000000146585 -0.000000002253 -0.000000019261 0.000000003685 -0.000000048027 0.000000514919 0.000000488132 -0.000000028143 -0.000000023707 0.000000010707 -0.000000146585 0.000000488132 0.000001822771 0.000000035984 0.000000038513 0.000000090631 -0.000000002253 -0.000000028143 0.000000035984 0.000001341511 -1 1645005919.509163618088 188 189 0.111763033254 -0.025831837093 -0.007938767615 0.000112630539 0.001169710732 -0.004771712646 0.000000150729 -0.000000043454 0.000000003900 -0.000000018668 -0.000000025291 0.000000043404 -0.000000043454 0.000000022348 -0.000000001378 0.000000003081 0.000000010657 0.000000089362 0.000000003900 -0.000000001378 0.000000014579 -0.000000049376 -0.000000149721 -0.000000001575 -0.000000018668 0.000000003081 -0.000000049376 0.000000521537 0.000000508382 -0.000000033045 -0.000000025291 0.000000010657 -0.000000149721 0.000000508382 0.000001873121 0.000000031279 0.000000043404 0.000000089362 -0.000000001575 -0.000000033045 0.000000031279 0.000001360429 -1 1645005919.606138467789 189 190 0.103050190411 -0.028243153856 0.009036138321 0.000299620151 -0.000460363273 -0.002126451387 0.000000151210 -0.000000043262 0.000000004079 -0.000000019984 -0.000000029692 0.000000051331 -0.000000043262 0.000000022105 -0.000000001496 0.000000003836 0.000000012813 0.000000087307 0.000000004079 -0.000000001496 0.000000014344 -0.000000049082 -0.000000148242 -0.000000002279 -0.000000019984 0.000000003836 -0.000000049082 0.000000523071 0.000000508358 -0.000000029835 -0.000000029692 0.000000012813 -0.000000148242 0.000000508358 0.000001871353 0.000000041535 0.000000051331 0.000000087307 -0.000000002279 -0.000000029835 0.000000041535 0.000001379713 -1 1645005919.708493471146 190 191 0.110366530571 -0.029472021130 0.018614876772 0.000102110420 0.000167129598 0.001994724913 0.000000150740 -0.000000042621 0.000000004151 -0.000000020036 -0.000000029606 0.000000062230 -0.000000042621 0.000000021889 -0.000000001558 0.000000003923 0.000000013386 0.000000086272 0.000000004151 -0.000000001558 0.000000014122 -0.000000049518 -0.000000146966 -0.000000002704 -0.000000020036 0.000000003923 -0.000000049518 0.000000525905 0.000000515666 -0.000000029326 -0.000000029606 0.000000013386 -0.000000146966 0.000000515666 0.000001869683 0.000000049449 0.000000062230 0.000000086272 -0.000000002704 -0.000000029326 0.000000049449 0.000001414664 -1 1645005919.793772935867 191 192 0.096165140501 -0.018685263170 0.011579795113 -0.000740821107 -0.001031369105 0.004911205760 0.000000150428 -0.000000041858 0.000000003727 -0.000000018419 -0.000000025185 0.000000073735 -0.000000041858 0.000000021544 -0.000000001374 0.000000003222 0.000000011562 0.000000084970 0.000000003727 -0.000000001374 0.000000014202 -0.000000051029 -0.000000149309 -0.000000002193 -0.000000018419 0.000000003222 -0.000000051029 0.000000531889 0.000000537344 -0.000000031680 -0.000000025185 0.000000011562 -0.000000149309 0.000000537344 0.000001910836 0.000000046007 0.000000073735 0.000000084970 -0.000000002193 -0.000000031680 0.000000046007 0.000001453265 -1 1645005919.894334554672 192 193 0.117461686956 -0.016398117007 -0.001090784968 -0.000555133742 -0.000926537311 0.007563804792 0.000000148505 -0.000000040661 0.000000003635 -0.000000017091 -0.000000022791 0.000000084172 -0.000000040661 0.000000020845 -0.000000001307 0.000000002552 0.000000010455 0.000000081570 0.000000003635 -0.000000001307 0.000000014106 -0.000000051158 -0.000000149559 -0.000000001627 -0.000000017091 0.000000002552 -0.000000051158 0.000000525853 0.000000542509 -0.000000035899 -0.000000022791 0.000000010455 -0.000000149559 0.000000542509 0.000001922636 0.000000041308 0.000000084172 0.000000081570 -0.000000001627 -0.000000035899 0.000000041308 0.000001463420 -1 1645005920.004122257233 193 194 0.127889889395 -0.014287189664 -0.018071779409 -0.000603843631 0.001403222454 0.004675372250 0.000000147336 -0.000000039680 0.000000003583 -0.000000015186 -0.000000019399 0.000000089698 -0.000000039680 0.000000020410 -0.000000001303 0.000000001664 0.000000009811 0.000000081704 0.000000003583 -0.000000001303 0.000000014293 -0.000000052484 -0.000000153094 -0.000000001827 -0.000000015186 0.000000001664 -0.000000052484 0.000000530498 0.000000561884 -0.000000039870 -0.000000019399 0.000000009811 -0.000000153094 0.000000561884 0.000001973145 0.000000046528 0.000000089698 0.000000081704 -0.000000001827 -0.000000039870 0.000000046528 0.000001493220 -1 1645005920.104864120483 194 195 0.116168811852 -0.013109825790 -0.012929698648 -0.000005389645 0.000738108287 0.001144078207 0.000000149213 -0.000000040106 0.000000003646 -0.000000015092 -0.000000021339 0.000000090319 -0.000000040106 0.000000020455 -0.000000001340 0.000000001571 0.000000010648 0.000000081937 0.000000003646 -0.000000001340 0.000000014645 -0.000000054570 -0.000000158509 -0.000000002004 -0.000000015092 0.000000001571 -0.000000054570 0.000000553075 0.000000590499 -0.000000041701 -0.000000021339 0.000000010648 -0.000000158509 0.000000590499 0.000002057287 0.000000048248 0.000000090319 0.000000081937 -0.000000002004 -0.000000041701 0.000000048248 0.000001514765 -1 1645005920.206741094589 195 196 0.116606271407 -0.013867701039 0.005805133595 0.000067675702 0.000577447854 -0.002394489670 0.000000149429 -0.000000040285 0.000000003646 -0.000000014456 -0.000000021481 0.000000087876 -0.000000040285 0.000000020545 -0.000000001350 0.000000001645 0.000000010775 0.000000083356 0.000000003646 -0.000000001350 0.000000014509 -0.000000053933 -0.000000159305 -0.000000002159 -0.000000014456 0.000000001645 -0.000000053933 0.000000554499 0.000000591697 -0.000000038609 -0.000000021481 0.000000010775 -0.000000159305 0.000000591697 0.000002096327 0.000000049808 0.000000087876 0.000000083356 -0.000000002159 -0.000000038609 0.000000049808 0.000001532263 -1 1645005920.306432723999 196 197 0.118459510620 -0.015741762731 0.019941986760 -0.000233586310 -0.001780831484 -0.005334381966 0.000000148573 -0.000000040237 0.000000003215 -0.000000013105 -0.000000017644 0.000000084415 -0.000000040237 0.000000020546 -0.000000001290 0.000000001926 0.000000010386 0.000000084228 0.000000003215 -0.000000001290 0.000000014559 -0.000000054075 -0.000000162036 -0.000000003226 -0.000000013105 0.000000001926 -0.000000054075 0.000000552256 0.000000602098 -0.000000029736 -0.000000017644 0.000000010386 -0.000000162036 0.000000602098 0.000002151812 0.000000061306 0.000000084415 0.000000084228 -0.000000003226 -0.000000029736 0.000000061306 0.000001531623 -1 1645005920.396054744720 197 198 0.108833768936 -0.014196265349 0.014957699203 0.000784882132 -0.000440774433 -0.007383134271 0.000000148605 -0.000000040325 0.000000003132 -0.000000012207 -0.000000013711 0.000000083131 -0.000000040325 0.000000020417 -0.000000001368 0.000000002111 0.000000010502 0.000000083174 0.000000003132 -0.000000001368 0.000000015049 -0.000000056948 -0.000000169586 -0.000000004564 -0.000000012207 0.000000002111 -0.000000056948 0.000000566345 0.000000643775 -0.000000023618 -0.000000013711 0.000000010502 -0.000000169586 0.000000643775 0.000002263566 0.000000078674 0.000000083131 0.000000083174 -0.000000004564 -0.000000023618 0.000000078674 0.000001518540 -1 1645005920.505497217178 198 199 0.132822748645 -0.016271936175 -0.000340704148 0.000018558141 0.001135204195 -0.009919704392 0.000000147651 -0.000000040083 0.000000002848 -0.000000010888 -0.000000007401 0.000000081858 -0.000000040083 0.000000020309 -0.000000001301 0.000000001666 0.000000008869 0.000000083786 0.000000002848 -0.000000001301 0.000000015367 -0.000000058874 -0.000000175370 -0.000000004773 -0.000000010888 0.000000001666 -0.000000058874 0.000000570999 0.000000675795 -0.000000023893 -0.000000007401 0.000000008869 -0.000000175370 0.000000675795 0.000002351570 0.000000082187 0.000000081858 0.000000083786 -0.000000004773 -0.000000023893 0.000000082187 0.000001524068 \ No newline at end of file From fbd45776a69eb4fc3c8d8b49572cfe17c988f647 Mon Sep 17 00:00:00 2001 From: JokerJohn <2022087641@qq.com> Date: Wed, 15 Jan 2025 13:21:32 +0800 Subject: [PATCH 22/52] add a new line at the end of file --- examples/IncrementalFixedLagSmootherExample.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/examples/IncrementalFixedLagSmootherExample.cpp b/examples/IncrementalFixedLagSmootherExample.cpp index 73beb6a75..f7002aabc 100644 --- a/examples/IncrementalFixedLagSmootherExample.cpp +++ b/examples/IncrementalFixedLagSmootherExample.cpp @@ -269,4 +269,3 @@ int main() { return 0; } - From 6795bf5709612c2263796bc3d9d577e7ee44fbaa Mon Sep 17 00:00:00 2001 From: JokerJohn <2022087641@qq.com> Date: Wed, 15 Jan 2025 13:33:53 +0800 Subject: [PATCH 23/52] add a new line at the end of file --- examples/IncrementalFixedLagSmootherExample.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/examples/IncrementalFixedLagSmootherExample.cpp b/examples/IncrementalFixedLagSmootherExample.cpp index f7002aabc..73beb6a75 100644 --- a/examples/IncrementalFixedLagSmootherExample.cpp +++ b/examples/IncrementalFixedLagSmootherExample.cpp @@ -269,3 +269,4 @@ int main() { return 0; } + From d6581cdef98ed6acb263afca14d9ce579038ed47 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 15 Jan 2025 00:45:07 -0500 Subject: [PATCH 24/52] Fixed typo in quaternion path --- gtsam/geometry/Pose3.cpp | 2 +- gtsam/navigation/NavState.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 9c3bd7733..0c47e3eed 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -189,7 +189,7 @@ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi) { // Compute rotation using Expmap #ifdef GTSAM_USE_QUATERNIONS - const Rot3 R = traits::Expmap(v); + const Rot3 R = traits::Expmap(w); #else const Rot3 R(local.expmap()); #endif diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index d033c96e8..d102340ce 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -123,7 +123,7 @@ NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi) { // Compute rotation using Expmap #ifdef GTSAM_USE_QUATERNIONS - const Rot3 R = traits::Expmap(v); + const Rot3 R = traits::Expmap(w); #else const Rot3 R(local.expmap()); #endif From af1e6e34e6d5c7df470bec301f6b203c8d2beace Mon Sep 17 00:00:00 2001 From: morten Date: Wed, 15 Jan 2025 11:54:41 +0100 Subject: [PATCH 25/52] reverting changes to existing factors --- gtsam/navigation/GPSFactor.cpp | 34 +++++------------------- gtsam/navigation/GPSFactor.h | 24 +++++------------ gtsam/navigation/navigation.i | 8 ++---- gtsam/navigation/tests/testGPSFactor.cpp | 15 +++-------- 4 files changed, 18 insertions(+), 63 deletions(-) diff --git a/gtsam/navigation/GPSFactor.cpp b/gtsam/navigation/GPSFactor.cpp index ff1f9923b..47de385ef 100644 --- a/gtsam/navigation/GPSFactor.cpp +++ b/gtsam/navigation/GPSFactor.cpp @@ -26,31 +26,20 @@ namespace gtsam { void GPSFactor::print(const string& s, const KeyFormatter& keyFormatter) const { cout << (s.empty() ? "" : s + " ") << "GPSFactor on " << keyFormatter(key()) << "\n"; - cout << " GPS measurement: " << nT_.transpose() << "\n"; - cout << " Lever arm: " << B_t_BG_.transpose() << "\n"; + cout << " GPS measurement: " << nT_ << "\n"; noiseModel_->print(" noise model: "); } //*************************************************************************** bool GPSFactor::equals(const NonlinearFactor& expected, double tol) const { const This* e = dynamic_cast(&expected); - return e != nullptr && Base::equals(*e, tol) && - traits::Equals(nT_, e->nT_, tol) && - traits::Equals(B_t_BG_, e->B_t_BG_, tol); + return e != nullptr && Base::equals(*e, tol) && traits::Equals(nT_, e->nT_, tol); } //*************************************************************************** Vector GPSFactor::evaluateError(const Pose3& p, OptionalMatrixType H) const { - const Matrix3 rot = p.rotation().matrix(); - if (H) { - H->resize(3, 6); - - H->block<3, 3>(0, 0) = -rot * skewSymmetric(B_t_BG_); - H->block<3, 3>(0, 3) = rot; - } - - return p.translation() - (nT_ - rot * B_t_BG_); + return p.translation(H) -nT_; } //*************************************************************************** @@ -78,8 +67,7 @@ pair GPSFactor::EstimateState(double t1, const Point3& NED1, //*************************************************************************** void GPSFactor2::print(const string& s, const KeyFormatter& keyFormatter) const { cout << s << "GPSFactor2 on " << keyFormatter(key()) << "\n"; - cout << " GPS measurement: " << nT_.transpose() << "\n"; - cout << " Lever arm: " << B_t_BG_.transpose() << "\n"; + cout << " GPS measurement: " << nT_.transpose() << endl; noiseModel_->print(" noise model: "); } @@ -87,23 +75,13 @@ void GPSFactor2::print(const string& s, const KeyFormatter& keyFormatter) const bool GPSFactor2::equals(const NonlinearFactor& expected, double tol) const { const This* e = dynamic_cast(&expected); return e != nullptr && Base::equals(*e, tol) && - traits::Equals(nT_, e->nT_, tol) && - traits::Equals(B_t_BG_, e->B_t_BG_, tol); + traits::Equals(nT_, e->nT_, tol); } //*************************************************************************** Vector GPSFactor2::evaluateError(const NavState& p, OptionalMatrixType H) const { - const Matrix3 rot = p.attitude().matrix(); - if (H) { - H->resize(3, 9); - - H->block<3, 3>(0, 0) = -rot * skewSymmetric(B_t_BG_); - H->block<3, 3>(0, 3) = rot; - H->block<3, 3>(0, 6).setZero(); - } - - return p.position() - (nT_ - rot * B_t_BG_); + return p.position(H) -nT_; } //*************************************************************************** diff --git a/gtsam/navigation/GPSFactor.h b/gtsam/navigation/GPSFactor.h index b180911ae..6af184360 100644 --- a/gtsam/navigation/GPSFactor.h +++ b/gtsam/navigation/GPSFactor.h @@ -39,7 +39,6 @@ private: typedef NoiseModelFactorN Base; Point3 nT_; ///< Position measurement in cartesian coordinates - Point3 B_t_BG_; ///< Lever arm between GPS and BODY frame public: @@ -53,7 +52,7 @@ public: typedef GPSFactor This; /** default constructor - only use for serialization */ - GPSFactor(): nT_(0, 0, 0), B_t_BG_(0, 0, 0) {} + GPSFactor(): nT_(0, 0, 0) {} ~GPSFactor() override {} @@ -64,8 +63,8 @@ public: * @param gpsIn measurement already in correct coordinates * @param model Gaussian noise model */ - GPSFactor(Key key, const Point3& gpsIn, const SharedNoiseModel& model, const Point3& leverArm) : - Base(model, key), nT_(gpsIn), B_t_BG_(leverArm) { + GPSFactor(Key key, const Point3& gpsIn, const SharedNoiseModel& model) : + Base(model, key), nT_(gpsIn) { } /// @return a deep copy of this factor @@ -88,10 +87,6 @@ public: return nT_; } - inline const Point3 & leverArm() const { - return B_t_BG_; - } - /** * Convenience function to estimate state at time t, given two GPS * readings (in local NED Cartesian frame) bracketing t @@ -112,7 +107,6 @@ private: & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(nT_); - ar & BOOST_SERIALIZATION_NVP(B_t_BG_); } #endif }; @@ -128,7 +122,6 @@ private: typedef NoiseModelFactorN Base; Point3 nT_; ///< Position measurement in cartesian coordinates - Point3 B_t_BG_; ///< Lever arm between GPS and BODY frame public: @@ -142,13 +135,13 @@ public: typedef GPSFactor2 This; /// default constructor - only use for serialization - GPSFactor2():nT_(0, 0, 0), B_t_BG_(0, 0, 0) {} + GPSFactor2():nT_(0, 0, 0) {} ~GPSFactor2() override {} /// Constructor from a measurement in a Cartesian frame. - GPSFactor2(Key key, const Point3& gpsIn, const SharedNoiseModel& model, const Point3& leverArm) : - Base(model, key), nT_(gpsIn), B_t_BG_(leverArm) { + GPSFactor2(Key key, const Point3& gpsIn, const SharedNoiseModel& model) : + Base(model, key), nT_(gpsIn) { } /// @return a deep copy of this factor @@ -171,10 +164,6 @@ public: return nT_; } - inline const Point3 & leverArm() const { - return B_t_BG_; - } - private: #if GTSAM_ENABLE_BOOST_SERIALIZATION @@ -187,7 +176,6 @@ private: & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(nT_); - ar & BOOST_SERIALIZATION_NVP(B_t_BG_); } #endif }; diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i index 7300f87ba..ceeab3b35 100644 --- a/gtsam/navigation/navigation.i +++ b/gtsam/navigation/navigation.i @@ -329,8 +329,7 @@ virtual class Pose3AttitudeFactor : gtsam::NoiseModelFactor { #include virtual class GPSFactor : gtsam::NonlinearFactor{ GPSFactor(size_t key, const gtsam::Point3& gpsIn, - const gtsam::noiseModel::Base* model, - const gtsam::Point3& leverArm); + const gtsam::noiseModel::Base* model); // Testable void print(string s = "", const gtsam::KeyFormatter& keyFormatter = @@ -339,7 +338,6 @@ virtual class GPSFactor : gtsam::NonlinearFactor{ // Standard Interface gtsam::Point3 measurementIn() const; - gtsam::Point3 leverArm() const; // enable serialization functionality void serialize() const; @@ -347,8 +345,7 @@ virtual class GPSFactor : gtsam::NonlinearFactor{ virtual class GPSFactor2 : gtsam::NonlinearFactor { GPSFactor2(size_t key, const gtsam::Point3& gpsIn, - const gtsam::noiseModel::Base* model, - const gtsam::Point3& leverArm); + const gtsam::noiseModel::Base* model); // Testable void print(string s = "", const gtsam::KeyFormatter& keyFormatter = @@ -357,7 +354,6 @@ virtual class GPSFactor2 : gtsam::NonlinearFactor { // Standard Interface gtsam::Point3 measurementIn() const; - gtsam::Point3 leverArm() const; // enable serialization functionality void serialize() const; diff --git a/gtsam/navigation/tests/testGPSFactor.cpp b/gtsam/navigation/tests/testGPSFactor.cpp index 2972d0fd6..cecfbeaad 100644 --- a/gtsam/navigation/tests/testGPSFactor.cpp +++ b/gtsam/navigation/tests/testGPSFactor.cpp @@ -45,9 +45,6 @@ LocalCartesian origin_ENU(lat0, lon0, h0, kWGS84); // Dekalb-Peachtree Airport runway 2L const double lat = 33.87071, lon = -84.30482, h = 274; - -// Random lever arm -const Point3 leverArm(0.1, 0.2, 0.3); } // ************************************************************************* @@ -64,12 +61,10 @@ TEST( GPSFactor, Constructor ) { // Factor Key key(1); SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25); - GPSFactor factor(key, Point3(E, N, U), model, leverArm); + GPSFactor factor(key, Point3(E, N, U), model); // Create a linearization point at zero error - const Rot3 rot = Rot3::RzRyRx(0.15, -0.30, 0.45); - const Point3 p = Point3(E, N, U) - rot * leverArm; - Pose3 T(rot, p); + Pose3 T(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(E, N, U)); EXPECT(assert_equal(Z_3x1,factor.evaluateError(T),1e-5)); // Calculate numerical derivatives @@ -95,12 +90,10 @@ TEST( GPSFactor2, Constructor ) { // Factor Key key(1); SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25); - GPSFactor2 factor(key, Point3(E, N, U), model, leverArm); + GPSFactor2 factor(key, Point3(E, N, U), model); // Create a linearization point at zero error - const Rot3 rot = Rot3::RzRyRx(0.15, -0.30, 0.45); - const Point3 p = Point3(E, N, U) - rot * leverArm; - NavState T(rot, p, Vector3::Zero()); + NavState T(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(E, N, U), Vector3::Zero()); EXPECT(assert_equal(Z_3x1,factor.evaluateError(T),1e-5)); // Calculate numerical derivatives From b81593601a1c1c341cbf31561722be17302c108b Mon Sep 17 00:00:00 2001 From: morten Date: Wed, 15 Jan 2025 13:31:38 +0100 Subject: [PATCH 26/52] creating GPSFactorArm and GPSFactor2Arm w/ lever arm arguments --- gtsam/navigation/GPSFactor.cpp | 60 ++++++++++ gtsam/navigation/GPSFactor.h | 145 ++++++++++++++++++++++- gtsam/navigation/tests/testGPSFactor.cpp | 68 ++++++++++- 3 files changed, 270 insertions(+), 3 deletions(-) diff --git a/gtsam/navigation/GPSFactor.cpp b/gtsam/navigation/GPSFactor.cpp index 47de385ef..e4767009b 100644 --- a/gtsam/navigation/GPSFactor.cpp +++ b/gtsam/navigation/GPSFactor.cpp @@ -64,6 +64,37 @@ pair GPSFactor::EstimateState(double t1, const Point3& NED1, return make_pair(nTb, nV); } + +//*************************************************************************** +void GPSFactorArm::print(const string& s, const KeyFormatter& keyFormatter) const { + cout << s << "GPSFactorArm on " << keyFormatter(key()) << "\n"; + cout << " GPS measurement: " << nT_.transpose() << "\n"; + cout << " Lever arm: " << bL_.transpose() << "\n"; + noiseModel_->print(" noise model: "); +} + +//*************************************************************************** +bool GPSFactorArm::equals(const NonlinearFactor& expected, double tol) const { + const This* e = dynamic_cast(&expected); + return e != nullptr && Base::equals(*e, tol) && + traits::Equals(nT_, e->nT_, tol) && + traits::Equals(bL_, e->bL_, tol); +} + +//*************************************************************************** +Vector GPSFactorArm::evaluateError(const Pose3& p, + OptionalMatrixType H) const { + const Matrix3 nRb = p.rotation().matrix(); + if (H) { + H->resize(3, 6); + + H->block<3, 3>(0, 0) = -nRb * skewSymmetric(bL_); + H->block<3, 3>(0, 3) = nRb; + } + + return p.translation() + nRb * bL_ - nT_; +} + //*************************************************************************** void GPSFactor2::print(const string& s, const KeyFormatter& keyFormatter) const { cout << s << "GPSFactor2 on " << keyFormatter(key()) << "\n"; @@ -85,5 +116,34 @@ Vector GPSFactor2::evaluateError(const NavState& p, } //*************************************************************************** +void GPSFactor2Arm::print(const string& s, const KeyFormatter& keyFormatter) const { + cout << s << "GPSFactorArm on " << keyFormatter(key()) << "\n"; + cout << " GPS measurement: " << nT_.transpose() << "\n"; + cout << " Lever arm: " << bL_.transpose() << "\n"; + noiseModel_->print(" noise model: "); +} + +//*************************************************************************** +bool GPSFactor2Arm::equals(const NonlinearFactor& expected, double tol) const { + const This* e = dynamic_cast(&expected); + return e != nullptr && Base::equals(*e, tol) && + traits::Equals(nT_, e->nT_, tol) && + traits::Equals(bL_, e->bL_, tol); +} + +//*************************************************************************** +Vector GPSFactor2Arm::evaluateError(const NavState& p, + OptionalMatrixType H) const { + const Matrix3 nRb = p.attitude().matrix(); + if (H) { + H->resize(3, 9); + + H->block<3, 3>(0, 0) = -nRb * skewSymmetric(bL_); + H->block<3, 3>(0, 3) = nRb; + H->block<3, 3>(0, 6).setZero(); + } + + return p.position() + nRb * bL_ - nT_; +} }/// namespace gtsam diff --git a/gtsam/navigation/GPSFactor.h b/gtsam/navigation/GPSFactor.h index 6af184360..58641f8e7 100644 --- a/gtsam/navigation/GPSFactor.h +++ b/gtsam/navigation/GPSFactor.h @@ -24,7 +24,10 @@ namespace gtsam { /** - * Prior on position in a Cartesian frame. + * Prior on position in a Cartesian frame, assuming position prior is in body + * frame. + * If there exists a non-zero lever arm between body frame and GPS + * antenna, instead use GPSFactorArm. * Possibilities include: * ENU: East-North-Up navigation frame at some local origin * NED: North-East-Down navigation frame at some local origin @@ -112,7 +115,81 @@ private: }; /** - * Version of GPSFactor for NavState + * Version of GPSFactor (for Pose3) with lever arm between GPS and Body frame. + * Because the actual location of the antenna depends on both position and + * attitude, providing a lever arm makes components of the attitude observable + * and accounts for position measurement vs state discrepancies while turning. + * @ingroup navigation + */ +class GTSAM_EXPORT GPSFactorArm: public NoiseModelFactorN { + +private: + + typedef NoiseModelFactorN Base; + + Point3 nT_; ///< Position measurement in cartesian coordinates + Point3 bL_; ///< bL_ is a lever arm in the body frame, denoting the 3D + ///< position of the GPS antenna in the body frame + +public: + + // Provide access to the Matrix& version of evaluateError: + using Base::evaluateError; + + /// shorthand for a smart pointer to a factor + typedef std::shared_ptr shared_ptr; + + /// Typedef to this class + typedef GPSFactorArm This; + + /// default constructor - only use for serialization + GPSFactorArm():nT_(0, 0, 0), bL_(0, 0, 0) {} + + ~GPSFactorArm() override {} + + /// Constructor from a measurement in a Cartesian frame. + GPSFactorArm(Key key, const Point3& gpsIn, const Point3& leverArm, const SharedNoiseModel& model) : + Base(model, key), nT_(gpsIn), bL_(leverArm) { + } + + /// @return a deep copy of this factor + gtsam::NonlinearFactor::shared_ptr clone() const override { + return std::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + /// print + void print(const std::string& s = "", const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const override; + + /// equals + bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override; + + /// vector of errors + Vector evaluateError(const Pose3& p, OptionalMatrixType H) const override; + + inline const Point3 & measurementIn() const { + return nT_; + } + + inline const Point3 & leverArm() const { + return bL_; + } + + /** + * Convenience function to estimate state at time t, given two GPS + * readings (in local NED Cartesian frame) bracketing t + * Assumes roll is zero, calculates yaw and pitch from NED1->NED2 vector. + */ + static std::pair EstimateState(double t1, const Point3& NED1, + double t2, const Point3& NED2, double timestamp); + +}; + +/** + * Version of GPSFactor for NavState, assuming zero lever arm between body frame + * and GPS. If there exists a non-zero lever arm between body frame and GPS + * antenna, instead use GPSFactor2Arm. * @ingroup navigation */ class GTSAM_EXPORT GPSFactor2: public NoiseModelFactorN { @@ -180,4 +257,68 @@ private: #endif }; +/** + * Version of GPSFactor2 with lever arm between GPS and Body frame. + * Because the actual location of the antenna depends on both position and + * attitude, providing a lever arm makes components of the attitude observable + * and accounts for position measurement vs state discrepancies while turning. + * @ingroup navigation + */ +class GTSAM_EXPORT GPSFactor2Arm: public NoiseModelFactorN { + +private: + + typedef NoiseModelFactorN Base; + + Point3 nT_; ///< Position measurement in cartesian coordinates + Point3 bL_; ///< bL_ is a lever arm in the body frame, denoting the 3D + ///< position of the GPS antenna in the body frame + +public: + + // Provide access to the Matrix& version of evaluateError: + using Base::evaluateError; + + /// shorthand for a smart pointer to a factor + typedef std::shared_ptr shared_ptr; + + /// Typedef to this class + typedef GPSFactor2Arm This; + + /// default constructor - only use for serialization + GPSFactor2Arm():nT_(0, 0, 0), bL_(0, 0, 0) {} + + ~GPSFactor2Arm() override {} + + /// Constructor from a measurement in a Cartesian frame. + GPSFactor2Arm(Key key, const Point3& gpsIn, const Point3& leverArm, const SharedNoiseModel& model) : + Base(model, key), nT_(gpsIn), bL_(leverArm) { + } + + /// @return a deep copy of this factor + gtsam::NonlinearFactor::shared_ptr clone() const override { + return std::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + /// print + void print(const std::string& s = "", const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const override; + + /// equals + bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override; + + /// vector of errors + Vector evaluateError(const NavState& p, OptionalMatrixType H) const override; + + inline const Point3 & measurementIn() const { + return nT_; + } + + inline const Point3 & leverArm() const { + return bL_; + } + +}; + } /// namespace gtsam diff --git a/gtsam/navigation/tests/testGPSFactor.cpp b/gtsam/navigation/tests/testGPSFactor.cpp index cecfbeaad..7925f0aeb 100644 --- a/gtsam/navigation/tests/testGPSFactor.cpp +++ b/gtsam/navigation/tests/testGPSFactor.cpp @@ -44,7 +44,11 @@ const double lat0 = 33.86998, lon0 = -84.30626, h0 = 274; LocalCartesian origin_ENU(lat0, lon0, h0, kWGS84); // Dekalb-Peachtree Airport runway 2L -const double lat = 33.87071, lon = -84.30482, h = 274; +const double lat = 33.87071, lon = -84.30482, h = 274;\ + +// Random lever arm +const Point3 leverArm(0.1, 0.2, 0.3); + } // ************************************************************************* @@ -79,6 +83,37 @@ TEST( GPSFactor, Constructor ) { EXPECT(assert_equal(expectedH, actualH, 1e-8)); } +// ************************************************************************* +TEST( GPSFactorArm, Constructor ) { + using namespace example; + + // From lat-lon to geocentric + double E, N, U; + origin_ENU.Forward(lat, lon, h, E, N, U); + + // Factor + Key key(1); + SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25); + GPSFactorArm factor(key, Point3(E, N, U), leverArm, model); + + // Create a linearization point at zero error + const Rot3 nRb = Rot3::RzRyRx(0.15, -0.30, 0.45); + const Point3 np = Point3(E, N, U) - nRb * leverArm; + Pose3 T(nRb, np); + EXPECT(assert_equal(Z_3x1,factor.evaluateError(T),1e-5)); + + // Calculate numerical derivatives + Matrix expectedH = numericalDerivative11( + [&factor](const Pose3& T) { return factor.evaluateError(T); }, T); + + // Use the factor to calculate the derivative + Matrix actualH; + factor.evaluateError(T, actualH); + + // Verify we get the expected error + EXPECT(assert_equal(expectedH, actualH, 1e-8)); +} + // ************************************************************************* TEST( GPSFactor2, Constructor ) { using namespace example; @@ -108,6 +143,37 @@ TEST( GPSFactor2, Constructor ) { EXPECT(assert_equal(expectedH, actualH, 1e-8)); } +// ************************************************************************* +TEST( GPSFactor2Arm, Constructor ) { + using namespace example; + + // From lat-lon to geocentric + double E, N, U; + origin_ENU.Forward(lat, lon, h, E, N, U); + + // Factor + Key key(1); + SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25); + GPSFactor2Arm factor(key, Point3(E, N, U), leverArm, model); + + // Create a linearization point at zero error + const Rot3 nRb = Rot3::RzRyRx(0.15, -0.30, 0.45); + const Point3 np = Point3(E, N, U) - nRb * leverArm; + NavState T(nRb, np, Vector3::Zero()); + EXPECT(assert_equal(Z_3x1,factor.evaluateError(T),1e-5)); + + // Calculate numerical derivatives + Matrix expectedH = numericalDerivative11( + [&factor](const NavState& T) { return factor.evaluateError(T); }, T); + + // Use the factor to calculate the derivative + Matrix actualH; + factor.evaluateError(T, actualH); + + // Verify we get the expected error + EXPECT(assert_equal(expectedH, actualH, 1e-8)); +} + //*************************************************************************** TEST(GPSData, init) { From 94e96ab001a682657e04f7e2524d67335fcb2421 Mon Sep 17 00:00:00 2001 From: morten Date: Wed, 15 Jan 2025 13:38:08 +0100 Subject: [PATCH 27/52] remove EstimateState from GPSFactorArm --- gtsam/navigation/GPSFactor.h | 8 -------- 1 file changed, 8 deletions(-) diff --git a/gtsam/navigation/GPSFactor.h b/gtsam/navigation/GPSFactor.h index 58641f8e7..75249e42e 100644 --- a/gtsam/navigation/GPSFactor.h +++ b/gtsam/navigation/GPSFactor.h @@ -176,14 +176,6 @@ public: return bL_; } - /** - * Convenience function to estimate state at time t, given two GPS - * readings (in local NED Cartesian frame) bracketing t - * Assumes roll is zero, calculates yaw and pitch from NED1->NED2 vector. - */ - static std::pair EstimateState(double t1, const Point3& NED1, - double t2, const Point3& NED2, double timestamp); - }; /** From b0e9b16cc40adcf266fd4e7d153f387097977666 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 15 Jan 2025 18:09:01 -0500 Subject: [PATCH 28/52] expmap now returns Matrix3 --- gtsam/geometry/SO3.cpp | 12 ++++++------ gtsam/geometry/SO3.h | 2 +- gtsam/geometry/tests/testSO3.cpp | 8 ++++---- 3 files changed, 11 insertions(+), 11 deletions(-) diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index 41f7ce810..a1947953b 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -92,7 +92,7 @@ ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle, init(nearZeroApprox); } -SO3 ExpmapFunctor::expmap() const { return SO3(I_3x3 + A * W + B * WW); } +Matrix3 ExpmapFunctor::expmap() const { return I_3x3 + A * W + B * WW; } DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox) : ExpmapFunctor(omega, nearZeroApprox), omega(omega) { @@ -193,7 +193,7 @@ Vector3 DexpFunctor::applyLeftJacobianInverse(const Vector3& v, template <> GTSAM_EXPORT SO3 SO3::AxisAngle(const Vector3& axis, double theta) { - return so3::ExpmapFunctor(axis, theta).expmap(); + return SO3(so3::ExpmapFunctor(axis, theta).expmap()); } //****************************************************************************** @@ -251,11 +251,11 @@ template <> GTSAM_EXPORT SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) { if (H) { - so3::DexpFunctor impl(omega); - *H = impl.dexp(); - return impl.expmap(); + so3::DexpFunctor local(omega); + *H = local.dexp(); + return SO3(local.expmap()); } else { - return so3::ExpmapFunctor(omega).expmap(); + return SO3(so3::ExpmapFunctor(omega).expmap()); } } diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index 36329a19d..1bdcd82c7 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -150,7 +150,7 @@ struct GTSAM_EXPORT ExpmapFunctor { ExpmapFunctor(const Vector3& axis, double angle, bool nearZeroApprox = false); /// Rodrigues formula - SO3 expmap() const; + Matrix3 expmap() const; protected: void init(bool nearZeroApprox = false); diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index c6fd52161..41777ae3a 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -163,22 +163,22 @@ TEST(SO3, ExpmapFunctor) { // axis angle version so3::ExpmapFunctor f1(axis, angle); - SO3 actual1 = f1.expmap(); + SO3 actual1(f1.expmap()); CHECK(assert_equal(expected, actual1.matrix(), 1e-5)); // axis angle version, negative angle so3::ExpmapFunctor f2(axis, angle - 2*M_PI); - SO3 actual2 = f2.expmap(); + SO3 actual2(f2.expmap()); CHECK(assert_equal(expected, actual2.matrix(), 1e-5)); // omega version so3::ExpmapFunctor f3(axis * angle); - SO3 actual3 = f3.expmap(); + SO3 actual3(f3.expmap()); CHECK(assert_equal(expected, actual3.matrix(), 1e-5)); // omega version, negative angle so3::ExpmapFunctor f4(axis * (angle - 2*M_PI)); - SO3 actual4 = f4.expmap(); + SO3 actual4(f4.expmap()); CHECK(assert_equal(expected, actual4.matrix(), 1e-5)); } From fdf8a51a44258f0360bd2ac9a980c93e4f4db09e Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 15 Jan 2025 18:09:26 -0500 Subject: [PATCH 29/52] X = R^T, according to Barfoot Eq 8.83 --- gtsam/geometry/Pose3.cpp | 13 +++++-------- gtsam/navigation/NavState.cpp | 14 +++++++------- 2 files changed, 12 insertions(+), 15 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 0c47e3eed..e9b628985 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -209,11 +209,9 @@ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi) { if (Hxi) { // The Jacobian of expmap is given by the right Jacobian of SO(3): const Matrix3 Jr = local.rightJacobian(); - // We multiply H, the derivative of applyLeftJacobian in omega, with - // X = Jr * Jl^{-1}, - // which translates from left to right for our right expmap convention: - const Matrix3 X = Jr * local.leftJacobianInverse(); - const Matrix3 Q = X * H; + // We multiply H, the derivative of applyLeftJacobian in omega, with R^T + // to translate from left to right for our right expmap convention: + const Matrix3 Q = R.matrix().transpose() * H; *Hxi << Jr, Z_3x3, // Q, Jr; } @@ -290,9 +288,8 @@ Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi, Matrix3 H; local.applyLeftJacobian(v, H); - // Multiply with X, translates from left to right for our expmap convention: - const Matrix3 X = local.rightJacobian() * local.leftJacobianInverse(); - return X * H; + // Multiply with R^T, translates from left to right for our expmap convention: + return local.expmap().transpose() * H; } /* ************************************************************************* */ diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index d102340ce..a0514c679 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -136,10 +136,10 @@ NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi) { if (Hxi) { // See Pose3::Expamp for explanation of the Jacobians const Matrix3 Jr = local.rightJacobian(); - const Matrix3 X = Jr * local.leftJacobianInverse(); + const Matrix3 M = R.matrix(); *Hxi << Jr, Z_3x3, Z_3x3, // - X * H_t_w, Jr, Z_3x3, // - X * H_v_w, Z_3x3, Jr; + M.transpose() * H_t_w, Jr, Z_3x3, // + M.transpose() * H_v_w, Z_3x3, Jr; } return NavState(R, t, v); @@ -250,10 +250,10 @@ Matrix9 NavState::LogmapDerivative(const NavState& state) { local.applyLeftJacobian(rho, H_t_w); local.applyLeftJacobian(nu, H_v_w); - // Multiply with X, translates from left to right for our expmap convention: - const Matrix3 X = local.rightJacobian() * local.leftJacobianInverse(); - const Matrix3 Qt = X * H_t_w; - const Matrix3 Qv = X * H_v_w; + // Multiply with R^T, translates from left to right for our expmap convention: + const Matrix3 R = local.expmap(); + const Matrix3 Qt = R.transpose() * H_t_w; + const Matrix3 Qv = R.transpose() * H_v_w; const Matrix3 Jw = Rot3::LogmapDerivative(w); const Matrix3 Qt2 = -Jw * Qt * Jw; From e3e4736e5e052de05f5d3b43e91a76796ea63263 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 16 Jan 2025 00:41:08 -0500 Subject: [PATCH 30/52] Fix comments about R^T --- gtsam/geometry/Pose3.cpp | 9 +++++---- gtsam/navigation/NavState.cpp | 6 ++++-- 2 files changed, 9 insertions(+), 6 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index e9b628985..0f039d20f 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -209,8 +209,8 @@ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi) { if (Hxi) { // The Jacobian of expmap is given by the right Jacobian of SO(3): const Matrix3 Jr = local.rightJacobian(); - // We multiply H, the derivative of applyLeftJacobian in omega, with R^T - // to translate from left to right for our right expmap convention: + // We are creating a Pose3, so we still need to chain H with R^T, the + // Jacobian of Pose3::Create with respect to t. const Matrix3 Q = R.matrix().transpose() * H; *Hxi << Jr, Z_3x3, // Q, Jr; @@ -288,8 +288,9 @@ Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi, Matrix3 H; local.applyLeftJacobian(v, H); - // Multiply with R^T, translates from left to right for our expmap convention: - return local.expmap().transpose() * H; + // Multiply with R^T to account for the Pose3::Create Jacobian. + const Matrix3 R = local.expmap(); + return R.transpose() * H; } /* ************************************************************************* */ diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index a0514c679..8bdddb198 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -134,8 +134,9 @@ NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi) { const Vector3 v = local.applyLeftJacobian(nu, Hxi ? &H_v_w : nullptr); if (Hxi) { - // See Pose3::Expamp for explanation of the Jacobians const Matrix3 Jr = local.rightJacobian(); + // We are creating a NavState, so we still need to chain H_t_w and H_v_w + // with R^T, the Jacobian of Navstate::Create with respect to both t and v. const Matrix3 M = R.matrix(); *Hxi << Jr, Z_3x3, Z_3x3, // M.transpose() * H_t_w, Jr, Z_3x3, // @@ -250,11 +251,12 @@ Matrix9 NavState::LogmapDerivative(const NavState& state) { local.applyLeftJacobian(rho, H_t_w); local.applyLeftJacobian(nu, H_v_w); - // Multiply with R^T, translates from left to right for our expmap convention: + // Multiply with R^T to account for NavState::Create Jacobian. const Matrix3 R = local.expmap(); const Matrix3 Qt = R.transpose() * H_t_w; const Matrix3 Qv = R.transpose() * H_v_w; + // Now compute the blocks of the LogmapDerivative Jacobian const Matrix3 Jw = Rot3::LogmapDerivative(w); const Matrix3 Qt2 = -Jw * Qt * Jw; const Matrix3 Qv2 = -Jw * Qv * Jw; From 2042977d28963de612fca7c1261eda6ee455c9fc Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 16 Jan 2025 00:54:49 -0500 Subject: [PATCH 31/52] Last comment I hope --- gtsam/geometry/Pose3.cpp | 4 ++-- gtsam/navigation/NavState.cpp | 4 +++- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 0f039d20f..f6d0c43cb 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -212,8 +212,8 @@ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi) { // We are creating a Pose3, so we still need to chain H with R^T, the // Jacobian of Pose3::Create with respect to t. const Matrix3 Q = R.matrix().transpose() * H; - *Hxi << Jr, Z_3x3, // - Q, Jr; + *Hxi << Jr, Z_3x3, // Jr here *is* the Jacobian of expmap + Q, Jr; // Here Jr = R^T * J_l, with J_l the Jacobian of t in v. } return Pose3(R, t); diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index 8bdddb198..116efe90a 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -138,9 +138,11 @@ NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi) { // We are creating a NavState, so we still need to chain H_t_w and H_v_w // with R^T, the Jacobian of Navstate::Create with respect to both t and v. const Matrix3 M = R.matrix(); - *Hxi << Jr, Z_3x3, Z_3x3, // + *Hxi << Jr, Z_3x3, Z_3x3, // Jr here *is* the Jacobian of expmap M.transpose() * H_t_w, Jr, Z_3x3, // M.transpose() * H_v_w, Z_3x3, Jr; + // In the last two rows, Jr = R^T * J_l, see Barfoot eq. (8.83). + // J_l is the Jacobian of applyLeftJacobian in the second argument. } return NavState(R, t, v); From 4737f0d554040ac111c735a5782fc2b6ce47fb4c Mon Sep 17 00:00:00 2001 From: morten Date: Thu, 16 Jan 2025 17:57:29 +0100 Subject: [PATCH 32/52] improving comments --- gtsam/navigation/GPSFactor.h | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/gtsam/navigation/GPSFactor.h b/gtsam/navigation/GPSFactor.h index 75249e42e..d911122e8 100644 --- a/gtsam/navigation/GPSFactor.h +++ b/gtsam/navigation/GPSFactor.h @@ -24,8 +24,7 @@ namespace gtsam { /** - * Prior on position in a Cartesian frame, assuming position prior is in body - * frame. + * Prior on position in a Cartesian frame. * If there exists a non-zero lever arm between body frame and GPS * antenna, instead use GPSFactorArm. * Possibilities include: @@ -41,7 +40,7 @@ private: typedef NoiseModelFactorN Base; - Point3 nT_; ///< Position measurement in cartesian coordinates + Point3 nT_; ///< Position measurement in cartesian coordinates (navigation frame) public: @@ -127,7 +126,7 @@ private: typedef NoiseModelFactorN Base; - Point3 nT_; ///< Position measurement in cartesian coordinates + Point3 nT_; ///< Position measurement in cartesian coordinates (navigation frame) Point3 bL_; ///< bL_ is a lever arm in the body frame, denoting the 3D ///< position of the GPS antenna in the body frame @@ -190,7 +189,7 @@ private: typedef NoiseModelFactorN Base; - Point3 nT_; ///< Position measurement in cartesian coordinates + Point3 nT_; ///< Position measurement in cartesian coordinates (navigation frame) public: @@ -262,7 +261,7 @@ private: typedef NoiseModelFactorN Base; - Point3 nT_; ///< Position measurement in cartesian coordinates + Point3 nT_; ///< Position measurement in cartesian coordinates (navigation frame) Point3 bL_; ///< bL_ is a lever arm in the body frame, denoting the 3D ///< position of the GPS antenna in the body frame From b5349a74f5db644a7928c8998049e666a77a27ba Mon Sep 17 00:00:00 2001 From: morten Date: Thu, 16 Jan 2025 18:12:07 +0100 Subject: [PATCH 33/52] improved documentation, params and getters --- gtsam/navigation/GPSFactor.h | 26 +++++++++++++++++++++++--- 1 file changed, 23 insertions(+), 3 deletions(-) diff --git a/gtsam/navigation/GPSFactor.h b/gtsam/navigation/GPSFactor.h index d911122e8..e043281ce 100644 --- a/gtsam/navigation/GPSFactor.h +++ b/gtsam/navigation/GPSFactor.h @@ -85,6 +85,7 @@ public: /// vector of errors Vector evaluateError(const Pose3& p, OptionalMatrixType H) const override; + /// return the measurement, in the navigation frame inline const Point3 & measurementIn() const { return nT_; } @@ -146,7 +147,12 @@ public: ~GPSFactorArm() override {} - /// Constructor from a measurement in a Cartesian frame. + /** Constructor from a measurement in a Cartesian frame. + * @param key key of the Pose3 variable related to this measurement + * @param gpsIn gps measurement, in Cartesian navigation frame + * @param leverArm translation from the body frame origin to the gps antenna, in body frame + * @param model Gaussian noise model + */ GPSFactorArm(Key key, const Point3& gpsIn, const Point3& leverArm, const SharedNoiseModel& model) : Base(model, key), nT_(gpsIn), bL_(leverArm) { } @@ -167,10 +173,12 @@ public: /// vector of errors Vector evaluateError(const Pose3& p, OptionalMatrixType H) const override; + /// return the measurement, in the navigation frame inline const Point3 & measurementIn() const { return nT_; } + /// return the lever arm, a position in the body frame inline const Point3 & leverArm() const { return bL_; } @@ -207,7 +215,11 @@ public: ~GPSFactor2() override {} - /// Constructor from a measurement in a Cartesian frame. + /** Constructor from a measurement in a Cartesian frame. + * @param key key of the NavState variable related to this measurement + * @param gpsIn gps measurement, in Cartesian navigation frame + * @param model Gaussian noise model + */ GPSFactor2(Key key, const Point3& gpsIn, const SharedNoiseModel& model) : Base(model, key), nT_(gpsIn) { } @@ -228,6 +240,7 @@ public: /// vector of errors Vector evaluateError(const NavState& p, OptionalMatrixType H) const override; + /// return the measurement, in the navigation frame inline const Point3 & measurementIn() const { return nT_; } @@ -281,7 +294,12 @@ public: ~GPSFactor2Arm() override {} - /// Constructor from a measurement in a Cartesian frame. + /** Constructor from a measurement in a Cartesian frame. + * @param key key of the NavState variable related to this measurement + * @param gpsIn gps measurement, in Cartesian navigation frame + * @param leverArm translation from the body frame origin to the gps antenna, in body frame + * @param model noise model for the factor's residual + */ GPSFactor2Arm(Key key, const Point3& gpsIn, const Point3& leverArm, const SharedNoiseModel& model) : Base(model, key), nT_(gpsIn), bL_(leverArm) { } @@ -302,10 +320,12 @@ public: /// vector of errors Vector evaluateError(const NavState& p, OptionalMatrixType H) const override; + /// return the measurement, in the navigation frame inline const Point3 & measurementIn() const { return nT_; } + /// return the lever arm, a position in the body frame inline const Point3 & leverArm() const { return bL_; } From 0c13038ed5dfe73668dd0ee5bd27a64a61169801 Mon Sep 17 00:00:00 2001 From: morten Date: Thu, 16 Jan 2025 21:26:47 +0100 Subject: [PATCH 34/52] typo in print for GPSFactor2Arm --- gtsam/navigation/GPSFactor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/navigation/GPSFactor.cpp b/gtsam/navigation/GPSFactor.cpp index e4767009b..4a6b35117 100644 --- a/gtsam/navigation/GPSFactor.cpp +++ b/gtsam/navigation/GPSFactor.cpp @@ -117,7 +117,7 @@ Vector GPSFactor2::evaluateError(const NavState& p, //*************************************************************************** void GPSFactor2Arm::print(const string& s, const KeyFormatter& keyFormatter) const { - cout << s << "GPSFactorArm on " << keyFormatter(key()) << "\n"; + cout << s << "GPSFactor2Arm on " << keyFormatter(key()) << "\n"; cout << " GPS measurement: " << nT_.transpose() << "\n"; cout << " Lever arm: " << bL_.transpose() << "\n"; noiseModel_->print(" noise model: "); From d1106ae4a36627dfa9926c6255dc985a547070a7 Mon Sep 17 00:00:00 2001 From: morten Date: Thu, 16 Jan 2025 21:27:42 +0100 Subject: [PATCH 35/52] adding GPSFactors with lever arm as state --- gtsam/navigation/GPSFactor.cpp | 65 +++++++++++ gtsam/navigation/GPSFactor.h | 131 +++++++++++++++++++++++ gtsam/navigation/tests/testGPSFactor.cpp | 80 ++++++++++++++ 3 files changed, 276 insertions(+) diff --git a/gtsam/navigation/GPSFactor.cpp b/gtsam/navigation/GPSFactor.cpp index 4a6b35117..f01a16d90 100644 --- a/gtsam/navigation/GPSFactor.cpp +++ b/gtsam/navigation/GPSFactor.cpp @@ -95,6 +95,38 @@ Vector GPSFactorArm::evaluateError(const Pose3& p, return p.translation() + nRb * bL_ - nT_; } +//*************************************************************************** +void GPSFactorArmCalib::print(const string& s, const KeyFormatter& keyFormatter) const { + cout << s << "GPSFactorArmCalib on " << keyFormatter(key()) << "\n"; + cout << " GPS measurement: " << nT_.transpose() << "\n"; + noiseModel_->print(" noise model: "); +} + +//*************************************************************************** +bool GPSFactorArmCalib::equals(const NonlinearFactor& expected, double tol) const { + const This* e = dynamic_cast(&expected); + return e != nullptr && Base::equals(*e, tol) && + traits::Equals(nT_, e->nT_, tol); +} + +//*************************************************************************** +Vector GPSFactorArmCalib::evaluateError(const Pose3& p, const Point3& bL, + OptionalMatrixType H1, OptionalMatrixType H2) const { + const Matrix3 nRb = p.rotation().matrix(); + if (H1) { + H1->resize(3, 6); + + H1->block<3, 3>(0, 0) = -nRb * skewSymmetric(bL); + H1->block<3, 3>(0, 3) = nRb; + } + if (H2){ + H2->resize(3, 3); + *H2 = nRb; + } + + return p.translation() + nRb * bL - nT_; +} + //*************************************************************************** void GPSFactor2::print(const string& s, const KeyFormatter& keyFormatter) const { cout << s << "GPSFactor2 on " << keyFormatter(key()) << "\n"; @@ -146,4 +178,37 @@ Vector GPSFactor2Arm::evaluateError(const NavState& p, return p.position() + nRb * bL_ - nT_; } +//*************************************************************************** +void GPSFactor2ArmCalib::print(const string& s, const KeyFormatter& keyFormatter) const { + cout << s << "GPSFactor2ArmCalib on " << keyFormatter(key()) << "\n"; + cout << " GPS measurement: " << nT_.transpose() << "\n"; + noiseModel_->print(" noise model: "); +} + +//*************************************************************************** +bool GPSFactor2ArmCalib::equals(const NonlinearFactor& expected, double tol) const { + const This* e = dynamic_cast(&expected); + return e != nullptr && Base::equals(*e, tol) && + traits::Equals(nT_, e->nT_, tol); +} + +//*************************************************************************** +Vector GPSFactor2ArmCalib::evaluateError(const NavState& p, const Point3& bL, + OptionalMatrixType H1, OptionalMatrixType H2) const { + const Matrix3 nRb = p.attitude().matrix(); + if (H1) { + H1->resize(3, 9); + + H1->block<3, 3>(0, 0) = -nRb * skewSymmetric(bL); + H1->block<3, 3>(0, 3) = nRb; + H1->block<3, 3>(0, 6).setZero(); + } + if (H2){ + H2->resize(3, 3); + *H2 = nRb; + } + + return p.position() + nRb * bL - nT_; +} + }/// namespace gtsam diff --git a/gtsam/navigation/GPSFactor.h b/gtsam/navigation/GPSFactor.h index e043281ce..4ef7c9794 100644 --- a/gtsam/navigation/GPSFactor.h +++ b/gtsam/navigation/GPSFactor.h @@ -185,6 +185,72 @@ public: }; +/** + * Version of GPSFactorArm (for Pose3) with unknown lever arm between GPS and + * Body frame. Because the actual location of the antenna depends on both + * position and attitude, providing a lever arm makes components of the attitude + * observable and accounts for position measurement vs state discrepancies while + * turning. + * @ingroup navigation + */ +class GTSAM_EXPORT GPSFactorArmCalib: public NoiseModelFactorN { + +private: + + typedef NoiseModelFactorN Base; + + Point3 nT_; ///< Position measurement in cartesian coordinates (navigation frame) + +public: + + // Provide access to the Matrix& version of evaluateError: + using Base::evaluateError; + + /// shorthand for a smart pointer to a factor + typedef std::shared_ptr shared_ptr; + + /// Typedef to this class + typedef GPSFactorArmCalib This; + + /// default constructor - only use for serialization + GPSFactorArmCalib() : nT_(0, 0, 0) {} + + ~GPSFactorArmCalib() override {} + + /** Constructor from a measurement in a Cartesian frame. + * @param key1 key of the Pose3 variable related to this measurement + * @param key2 key of the Point3 variable related to the lever arm + * @param gpsIn gps measurement, in Cartesian navigation frame + * @param leverArm translation from the body frame origin to the gps antenna, in body frame + * @param model Gaussian noise model + */ + GPSFactorArmCalib(Key key1, Key key2, const Point3& gpsIn, const SharedNoiseModel& model) : + Base(model, key1, key2), nT_(gpsIn) { + } + + /// @return a deep copy of this factor + gtsam::NonlinearFactor::shared_ptr clone() const override { + return std::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + /// print + void print(const std::string& s = "", const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const override; + + /// equals + bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override; + + /// vector of errors + Vector evaluateError(const Pose3& p, const Point3& bL, OptionalMatrixType H1, + OptionalMatrixType H2) const override; + + /// return the measurement, in the navigation frame + inline const Point3 & measurementIn() const { + return nT_; + } +}; + /** * Version of GPSFactor for NavState, assuming zero lever arm between body frame * and GPS. If there exists a non-zero lever arm between body frame and GPS @@ -332,4 +398,69 @@ public: }; +/** + * Version of GPSFactor2Arm for an unknown lever arm between GPS and Body frame. + * Because the actual location of the antenna depends on both position and + * attitude, providing a lever arm makes components of the attitude observable + * and accounts for position measurement vs state discrepancies while turning. + * @ingroup navigation + */ +class GTSAM_EXPORT GPSFactor2ArmCalib: public NoiseModelFactorN { + +private: + + typedef NoiseModelFactorN Base; + + Point3 nT_; ///< Position measurement in cartesian coordinates (navigation frame) + +public: + + // Provide access to the Matrix& version of evaluateError: + using Base::evaluateError; + + /// shorthand for a smart pointer to a factor + typedef std::shared_ptr shared_ptr; + + /// Typedef to this class + typedef GPSFactor2ArmCalib This; + + /// default constructor - only use for serialization + GPSFactor2ArmCalib():nT_(0, 0, 0) {} + + ~GPSFactor2ArmCalib() override {} + + /** Constructor from a measurement in a Cartesian frame. + * @param key1 key of the NavState variable related to this measurement + * @param key2 key of the Point3 variable related to the lever arm + * @param gpsIn gps measurement, in Cartesian navigation frame + * @param model Gaussian noise model + */ + GPSFactor2ArmCalib(Key key1, Key key2, const Point3& gpsIn, const SharedNoiseModel& model) : + Base(model, key1, key2), nT_(gpsIn) { + } + + /// @return a deep copy of this factor + gtsam::NonlinearFactor::shared_ptr clone() const override { + return std::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + /// print + void print(const std::string& s = "", const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const override; + + /// equals + bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override; + + /// vector of errors + Vector evaluateError(const NavState& p, const Point3& bL, + OptionalMatrixType H1, + OptionalMatrixType H2) const override; + + /// return the measurement, in the navigation frame + inline const Point3 & measurementIn() const { + return nT_; + } +}; + } /// namespace gtsam diff --git a/gtsam/navigation/tests/testGPSFactor.cpp b/gtsam/navigation/tests/testGPSFactor.cpp index 7925f0aeb..f240e5dbf 100644 --- a/gtsam/navigation/tests/testGPSFactor.cpp +++ b/gtsam/navigation/tests/testGPSFactor.cpp @@ -114,6 +114,46 @@ TEST( GPSFactorArm, Constructor ) { EXPECT(assert_equal(expectedH, actualH, 1e-8)); } +// ************************************************************************* +TEST( GPSFactorArmCalib, Constructor ) { + using namespace example; + + // From lat-lon to geocentric + double E, N, U; + origin_ENU.Forward(lat, lon, h, E, N, U); + + // Factor + Key key1(1), key2(2); + SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25); + GPSFactorArmCalib factor(key1, key2, Point3(E, N, U), model); + + // Create a linearization point at zero error + const Rot3 nRb = Rot3::RzRyRx(0.15, -0.30, 0.45); + const Point3 np = Point3(E, N, U) - nRb * leverArm; + Pose3 T(nRb, np); + EXPECT(assert_equal(Z_3x1,factor.evaluateError(T, leverArm),1e-5)); + + // Calculate numerical derivatives + Matrix expectedH1 = numericalDerivative11( + [&factor, &leverArm](const Pose3& pose_arg) { + return factor.evaluateError(pose_arg, leverArm); + }, + T); + Matrix expectedH2 = numericalDerivative11( + [&factor, &T](const Point3& point_arg) { + return factor.evaluateError(T, point_arg); + }, + leverArm); + + // Use the factor to calculate the derivative + Matrix actualH1, actualH2; + factor.evaluateError(T, leverArm, actualH1, actualH2); + + // Verify we get the expected error + EXPECT(assert_equal(expectedH1, actualH1, 1e-8)); + EXPECT(assert_equal(expectedH2, actualH2, 1e-8)); +} + // ************************************************************************* TEST( GPSFactor2, Constructor ) { using namespace example; @@ -174,6 +214,46 @@ TEST( GPSFactor2Arm, Constructor ) { EXPECT(assert_equal(expectedH, actualH, 1e-8)); } +// ************************************************************************* +TEST( GPSFactor2ArmCalib, Constructor ) { + using namespace example; + + // From lat-lon to geocentric + double E, N, U; + origin_ENU.Forward(lat, lon, h, E, N, U); + + // Factor + Key key1(1), key2(2); + SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25); + GPSFactor2ArmCalib factor(key1, key2, Point3(E, N, U), model); + + // Create a linearization point at zero error + const Rot3 nRb = Rot3::RzRyRx(0.15, -0.30, 0.45); + const Point3 np = Point3(E, N, U) - nRb * leverArm; + NavState T(nRb, np, Vector3::Zero()); + EXPECT(assert_equal(Z_3x1,factor.evaluateError(T, leverArm),1e-5)); + + // Calculate numerical derivatives + Matrix expectedH1 = numericalDerivative11( + [&factor, &leverArm](const NavState& nav_arg) { + return factor.evaluateError(nav_arg, leverArm); + }, + T); + Matrix expectedH2 = numericalDerivative11( + [&factor, &T](const Point3& point_arg) { + return factor.evaluateError(T, point_arg); + }, + leverArm); + + // Use the factor to calculate the derivative + Matrix actualH1, actualH2; + factor.evaluateError(T, leverArm, actualH1, actualH2); + + // Verify we get the expected error + EXPECT(assert_equal(expectedH1, actualH1, 1e-8)); + EXPECT(assert_equal(expectedH2, actualH2, 1e-8)); +} + //*************************************************************************** TEST(GPSData, init) { From 630ee1188591db345b1d66046a103f8f04a75371 Mon Sep 17 00:00:00 2001 From: JokerJohn <2022087641@qq.com> Date: Fri, 17 Jan 2025 23:57:58 +0800 Subject: [PATCH 36/52] remove example --- .../IncrementalFixedLagSmootherExample.cpp | 272 ------------------ 1 file changed, 272 deletions(-) delete mode 100644 examples/IncrementalFixedLagSmootherExample.cpp diff --git a/examples/IncrementalFixedLagSmootherExample.cpp b/examples/IncrementalFixedLagSmootherExample.cpp deleted file mode 100644 index 73beb6a75..000000000 --- a/examples/IncrementalFixedLagSmootherExample.cpp +++ /dev/null @@ -1,272 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010-2025, 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 - - * -------------------------------------------------------------------------- */ - -/** -* @file IncrementalFixedLagExample.cpp -* @brief Example of incremental fixed-lag smoother using real-world data. -* @author Xiangcheng Hu (xhubd@connect.ust.hk), Frank Dellaert, Kevin Doherty -* @date Janaury 15, 2025 -* -* Key objectives: -* - Validate `IncrementalFixedLagSmoother` functionality with real-world data. -* - Showcase how setting `findUnusedFactorSlots = true` addresses the issue #1452 in GTSAM, ensuring -* that unused factor slots (nullptrs) are correctly released when old factors are marginalized. -* -* This example leverages pose measurements from a real scenario. The test data (named "IncrementalFixedLagSmootherTestData.txt") is -* based on the corridor_day sequence from the FusionPortable dataset (https://fusionportable.github.io/dataset/fusionportable/). -* - 1 prior factor derived from point cloud ICP alignment with a prior map. -* - 199 relative pose factors derived from FAST-LIO2 odometry. -* -* Data Format (IncrementalFixedLagSmootherTestData.txt): -* 1) PRIOR factor line: -* @code -* 0 timestamp key x y z roll pitch yaw cov_6x6 -* @endcode -* - "0" indicates PRIOR factor. -* - "timestamp" is the observation time (in seconds). -* - "key" is the integer ID for the Pose3 variable. -* - (x, y, z, roll, pitch, yaw) define the pose. -* - "cov_6x6" is the full 6x6 covariance matrix (row-major). -* -* 2) BETWEEN factor line: -* @code -* 1 timestamp key1 key2 x y z roll pitch yaw cov_6x6 -* @endcode -* - "1" indicates BETWEEN factor. -* - "timestamp" is the observation time (in seconds). -* - "key1" and "key2" are the integer IDs for the connected Pose3 variables. -* - (x, y, z, roll, pitch, yaw) define the relative pose between these variables. -* - "cov_6x6" is the full 6x6 covariance matrix (row-major). -* -* See also: -* - GTSAM Issue #1452: https://github.com/borglab/gtsam/issues/1452 -*/ - -// STL -#include -#include -// GTSAM -#include -#include -#include -#include -#include -#include -#include -#include // for writeG2o - -using namespace std; -using namespace gtsam; -// Shorthand for symbols -using symbol_shorthand::X; // Pose3 (x,y,z, roll, pitch, yaw) - -// Factor types -enum FactorType { - PRIOR = 0, - BETWEEN = 1 -}; - -typedef Eigen::Matrix Matrix6; - -/* ************************************************************************* */ -/** - * @brief Read a 6x6 covariance matrix from an input string stream. - * - * @param iss Input string stream containing covariance entries. - * @return 6x6 covariance matrix. - */ -Matrix6 readCovarianceMatrix(istringstream &iss) { - Matrix6 cov; - for (int r = 0; r < 6; ++r) { - for (int c = 0; c < 6; ++c) { - iss >> cov(r, c); - } - } - return cov; -} - -/* ************************************************************************* */ -/** - * @brief Create a Pose3 object from individual pose parameters. - * - * @param x Translation in x - * @param y Translation in y - * @param z Translation in z - * @param roll Rotation about x-axis - * @param pitch Rotation about y-axis - * @param yaw Rotation about z-axis - * @return Constructed Pose3 object - */ -Pose3 createPose(double x, double y, double z, double roll, double pitch, double yaw) { - return Pose3(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z)); -} - -/* ************************************************************************* */ -/** - * @brief Save the factor graph and estimates to a .g2o file (for visualization/debugging). - * - * @param graph The factor graph - * @param estimate Current estimates of all variables - * @param filename Base filename for saving - * @param iterCount Iteration count to differentiate successive outputs - */ -void saveG2oGraph(const NonlinearFactorGraph &graph, const Values &estimate, - const string &filename, int iterCount) { - // Create zero-padded iteration count - string countStr = to_string(iterCount); - string paddedCount = string(5 - countStr.length(), '0') + countStr; - string fullFilename = filename + "_" + paddedCount + ".g2o"; - // Write graph and estimates to g2o file - writeG2o(graph, estimate, fullFilename); - cout << "\nSaved graph to: " << fullFilename << endl; -} - -/* ************************************************************************* */ -/** - * @brief Main function: Reads poses data from a text file and performs incremental fixed-lag smoothing. - * - * Data Flow: - * 1) Parse lines from "IncrementalFixedLagSmootherTestData.txt". - * 2) For each line: - * - If it's a PRIOR factor, add a PriorFactor with a specified pose and covariance. - * - If it's a BETWEEN factor, add a BetweenFactor with a relative pose and covariance. - * - Insert new variables with initial guesses into the current solution if they don't exist. - * 3) Update the fixed-lag smoother (with iSAM2 inside) to incrementally optimize and marginalize out old poses - * beyond the specified lag window. - * 4) Repeat until all lines are processed. - * 5) Save the resulting factor graph and estimate of the last sliding window to a .g2o file. - */ -int main() { - string factor_loc = findExampleDataFile("issue1452.txt"); - ifstream factor_file(factor_loc.c_str()); - cout << "Reading factors data file: " << factor_loc << endl; - - // Configure ISAM2 parameters for the fixed-lag smoother - ISAM2Params isamParameters; - isamParameters.relinearizeThreshold = 0.1; - isamParameters.relinearizeSkip = 1; - - // Important!!!!!! Key parameter to ensure old factors are released after marginalization - isamParameters.findUnusedFactorSlots = true; - // Initialize fixed-lag smoother with a 1-second lag window - const double lag = 1.0; - IncrementalFixedLagSmoother smoother(lag, isamParameters); - // Print the iSAM2 parameters (optional) - isamParameters.print(); - - // Containers for incremental updates - NonlinearFactorGraph newFactors; - Values newValues; - FixedLagSmoother::KeyTimestampMap newTimestamps; - // For tracking the latest estimate of all states in the sliding window - Values currentEstimate; - Pose3 lastPose; - - // Read and process each line in the factor graph file - string line; - int lineCount = 0; - while (getline(factor_file, line)) { - if (line.empty()) continue; // Skip empty lines - cout << "\n======================== Processing line " << ++lineCount - << " =========================" << endl; - - istringstream iss(line); - int factorType; - iss >> factorType; - // Check if the factor is PRIOR or BETWEEN - if (factorType == PRIOR) { - /** - * Format: PRIOR factor - * factor_type timestamp key pose(x y z roll pitch yaw) cov(6x6) - */ - double timestamp; - int key; - double x, y, z, roll, pitch, yaw; - iss >> timestamp >> key >> x >> y >> z >> roll >> pitch >> yaw; - Pose3 pose = createPose(x, y, z, roll, pitch, yaw); - Matrix6 cov = readCovarianceMatrix(iss); - auto noise = noiseModel::Gaussian::Covariance(cov); - // Add prior factor - newFactors.addPrior(X(key), pose, noise); - cout << "Add PRIOR factor on key " << key << endl; - // Provide initial guess if not already in the graph - if (!newValues.exists(X(key))) { - newValues.insert(X(key), pose); - newTimestamps[X(key)] = timestamp; - } - } else if (factorType == BETWEEN) { - /** - * Format: BETWEEN factor - * factor_type timestamp key1 key2 pose(x y z roll pitch yaw) cov(6x6) - */ - double timestamp; - int key1, key2; - iss >> timestamp >> key1 >> key2; - double x1, y1, z1, roll1, pitch1, yaw1; - iss >> x1 >> y1 >> z1 >> roll1 >> pitch1 >> yaw1; - Pose3 relativePose = createPose(x1, y1, z1, roll1, pitch1, yaw1); - Matrix6 cov = readCovarianceMatrix(iss); - auto noise = noiseModel::Gaussian::Covariance(cov); - // Add between factor - newFactors.emplace_shared>(X(key1), X(key2), relativePose, noise); - cout << "Add BETWEEN factor: " << key1 << " -> " << key2 << endl; - // Provide an initial guess if needed - if (!newValues.exists(X(key2))) { - newValues.insert(X(key2), lastPose.compose(relativePose)); - newTimestamps[X(key2)] = timestamp; - } - } else { - cerr << "Unknown factor type: " << factorType << endl; - continue; - } - - // Print some intermediate statistics - cout << "Before update - Graph has " << smoother.getFactors().size() - << " factors, " << smoother.getFactors().nrFactors() << " nr factors." << endl; - cout << "New factors: " << newFactors.size() - << ", New values: " << newValues.size() << endl; - - // Attempt to update the smoother with new factors and values - try { - smoother.update(newFactors, newValues, newTimestamps); - // Optional: Perform extra internal iterations if needed - size_t maxExtraIterations = 3; - for (size_t i = 1; i < maxExtraIterations; ++i) { - smoother.update(); - } - // you may not get expected results if you use the gtsam version lower than 4.3 - cout << "After update - Graph has " << smoother.getFactors().size() - << " factors, " << smoother.getFactors().nrFactors() << " nr factors." << endl; - - // Retrieve the latest estimate - currentEstimate = smoother.calculateEstimate(); - if (!currentEstimate.empty()) { - // Update lastPose to the last key's estimate - Key lastKey = currentEstimate.keys().back(); - lastPose = currentEstimate.at(lastKey); - } - - // Clear containers for the next iteration - newFactors.resize(0); - newValues.clear(); - newTimestamps.clear(); - } catch (const exception &e) { - cerr << "Smoother update failed: " << e.what() << endl; - } - } - - // The results of the last sliding window are saved to a g2o file for visualization or further analysis. - saveG2oGraph(smoother.getFactors(), currentEstimate, "isam", lineCount); - factor_file.close(); - - return 0; -} - From a9da253d001e533f16988ec077ed54f313de7c81 Mon Sep 17 00:00:00 2001 From: JokerJohn <2022087641@qq.com> Date: Sat, 18 Jan 2025 17:38:34 +0800 Subject: [PATCH 37/52] add IncrementalFixedLagSmootherExample --- .../IncrementalFixedLagSmootherExample.cpp | 272 ++++++++++++++++++ 1 file changed, 272 insertions(+) create mode 100644 examples/IncrementalFixedLagSmootherExample.cpp diff --git a/examples/IncrementalFixedLagSmootherExample.cpp b/examples/IncrementalFixedLagSmootherExample.cpp new file mode 100644 index 000000000..73beb6a75 --- /dev/null +++ b/examples/IncrementalFixedLagSmootherExample.cpp @@ -0,0 +1,272 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010-2025, 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 + + * -------------------------------------------------------------------------- */ + +/** +* @file IncrementalFixedLagExample.cpp +* @brief Example of incremental fixed-lag smoother using real-world data. +* @author Xiangcheng Hu (xhubd@connect.ust.hk), Frank Dellaert, Kevin Doherty +* @date Janaury 15, 2025 +* +* Key objectives: +* - Validate `IncrementalFixedLagSmoother` functionality with real-world data. +* - Showcase how setting `findUnusedFactorSlots = true` addresses the issue #1452 in GTSAM, ensuring +* that unused factor slots (nullptrs) are correctly released when old factors are marginalized. +* +* This example leverages pose measurements from a real scenario. The test data (named "IncrementalFixedLagSmootherTestData.txt") is +* based on the corridor_day sequence from the FusionPortable dataset (https://fusionportable.github.io/dataset/fusionportable/). +* - 1 prior factor derived from point cloud ICP alignment with a prior map. +* - 199 relative pose factors derived from FAST-LIO2 odometry. +* +* Data Format (IncrementalFixedLagSmootherTestData.txt): +* 1) PRIOR factor line: +* @code +* 0 timestamp key x y z roll pitch yaw cov_6x6 +* @endcode +* - "0" indicates PRIOR factor. +* - "timestamp" is the observation time (in seconds). +* - "key" is the integer ID for the Pose3 variable. +* - (x, y, z, roll, pitch, yaw) define the pose. +* - "cov_6x6" is the full 6x6 covariance matrix (row-major). +* +* 2) BETWEEN factor line: +* @code +* 1 timestamp key1 key2 x y z roll pitch yaw cov_6x6 +* @endcode +* - "1" indicates BETWEEN factor. +* - "timestamp" is the observation time (in seconds). +* - "key1" and "key2" are the integer IDs for the connected Pose3 variables. +* - (x, y, z, roll, pitch, yaw) define the relative pose between these variables. +* - "cov_6x6" is the full 6x6 covariance matrix (row-major). +* +* See also: +* - GTSAM Issue #1452: https://github.com/borglab/gtsam/issues/1452 +*/ + +// STL +#include +#include +// GTSAM +#include +#include +#include +#include +#include +#include +#include +#include // for writeG2o + +using namespace std; +using namespace gtsam; +// Shorthand for symbols +using symbol_shorthand::X; // Pose3 (x,y,z, roll, pitch, yaw) + +// Factor types +enum FactorType { + PRIOR = 0, + BETWEEN = 1 +}; + +typedef Eigen::Matrix Matrix6; + +/* ************************************************************************* */ +/** + * @brief Read a 6x6 covariance matrix from an input string stream. + * + * @param iss Input string stream containing covariance entries. + * @return 6x6 covariance matrix. + */ +Matrix6 readCovarianceMatrix(istringstream &iss) { + Matrix6 cov; + for (int r = 0; r < 6; ++r) { + for (int c = 0; c < 6; ++c) { + iss >> cov(r, c); + } + } + return cov; +} + +/* ************************************************************************* */ +/** + * @brief Create a Pose3 object from individual pose parameters. + * + * @param x Translation in x + * @param y Translation in y + * @param z Translation in z + * @param roll Rotation about x-axis + * @param pitch Rotation about y-axis + * @param yaw Rotation about z-axis + * @return Constructed Pose3 object + */ +Pose3 createPose(double x, double y, double z, double roll, double pitch, double yaw) { + return Pose3(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z)); +} + +/* ************************************************************************* */ +/** + * @brief Save the factor graph and estimates to a .g2o file (for visualization/debugging). + * + * @param graph The factor graph + * @param estimate Current estimates of all variables + * @param filename Base filename for saving + * @param iterCount Iteration count to differentiate successive outputs + */ +void saveG2oGraph(const NonlinearFactorGraph &graph, const Values &estimate, + const string &filename, int iterCount) { + // Create zero-padded iteration count + string countStr = to_string(iterCount); + string paddedCount = string(5 - countStr.length(), '0') + countStr; + string fullFilename = filename + "_" + paddedCount + ".g2o"; + // Write graph and estimates to g2o file + writeG2o(graph, estimate, fullFilename); + cout << "\nSaved graph to: " << fullFilename << endl; +} + +/* ************************************************************************* */ +/** + * @brief Main function: Reads poses data from a text file and performs incremental fixed-lag smoothing. + * + * Data Flow: + * 1) Parse lines from "IncrementalFixedLagSmootherTestData.txt". + * 2) For each line: + * - If it's a PRIOR factor, add a PriorFactor with a specified pose and covariance. + * - If it's a BETWEEN factor, add a BetweenFactor with a relative pose and covariance. + * - Insert new variables with initial guesses into the current solution if they don't exist. + * 3) Update the fixed-lag smoother (with iSAM2 inside) to incrementally optimize and marginalize out old poses + * beyond the specified lag window. + * 4) Repeat until all lines are processed. + * 5) Save the resulting factor graph and estimate of the last sliding window to a .g2o file. + */ +int main() { + string factor_loc = findExampleDataFile("issue1452.txt"); + ifstream factor_file(factor_loc.c_str()); + cout << "Reading factors data file: " << factor_loc << endl; + + // Configure ISAM2 parameters for the fixed-lag smoother + ISAM2Params isamParameters; + isamParameters.relinearizeThreshold = 0.1; + isamParameters.relinearizeSkip = 1; + + // Important!!!!!! Key parameter to ensure old factors are released after marginalization + isamParameters.findUnusedFactorSlots = true; + // Initialize fixed-lag smoother with a 1-second lag window + const double lag = 1.0; + IncrementalFixedLagSmoother smoother(lag, isamParameters); + // Print the iSAM2 parameters (optional) + isamParameters.print(); + + // Containers for incremental updates + NonlinearFactorGraph newFactors; + Values newValues; + FixedLagSmoother::KeyTimestampMap newTimestamps; + // For tracking the latest estimate of all states in the sliding window + Values currentEstimate; + Pose3 lastPose; + + // Read and process each line in the factor graph file + string line; + int lineCount = 0; + while (getline(factor_file, line)) { + if (line.empty()) continue; // Skip empty lines + cout << "\n======================== Processing line " << ++lineCount + << " =========================" << endl; + + istringstream iss(line); + int factorType; + iss >> factorType; + // Check if the factor is PRIOR or BETWEEN + if (factorType == PRIOR) { + /** + * Format: PRIOR factor + * factor_type timestamp key pose(x y z roll pitch yaw) cov(6x6) + */ + double timestamp; + int key; + double x, y, z, roll, pitch, yaw; + iss >> timestamp >> key >> x >> y >> z >> roll >> pitch >> yaw; + Pose3 pose = createPose(x, y, z, roll, pitch, yaw); + Matrix6 cov = readCovarianceMatrix(iss); + auto noise = noiseModel::Gaussian::Covariance(cov); + // Add prior factor + newFactors.addPrior(X(key), pose, noise); + cout << "Add PRIOR factor on key " << key << endl; + // Provide initial guess if not already in the graph + if (!newValues.exists(X(key))) { + newValues.insert(X(key), pose); + newTimestamps[X(key)] = timestamp; + } + } else if (factorType == BETWEEN) { + /** + * Format: BETWEEN factor + * factor_type timestamp key1 key2 pose(x y z roll pitch yaw) cov(6x6) + */ + double timestamp; + int key1, key2; + iss >> timestamp >> key1 >> key2; + double x1, y1, z1, roll1, pitch1, yaw1; + iss >> x1 >> y1 >> z1 >> roll1 >> pitch1 >> yaw1; + Pose3 relativePose = createPose(x1, y1, z1, roll1, pitch1, yaw1); + Matrix6 cov = readCovarianceMatrix(iss); + auto noise = noiseModel::Gaussian::Covariance(cov); + // Add between factor + newFactors.emplace_shared>(X(key1), X(key2), relativePose, noise); + cout << "Add BETWEEN factor: " << key1 << " -> " << key2 << endl; + // Provide an initial guess if needed + if (!newValues.exists(X(key2))) { + newValues.insert(X(key2), lastPose.compose(relativePose)); + newTimestamps[X(key2)] = timestamp; + } + } else { + cerr << "Unknown factor type: " << factorType << endl; + continue; + } + + // Print some intermediate statistics + cout << "Before update - Graph has " << smoother.getFactors().size() + << " factors, " << smoother.getFactors().nrFactors() << " nr factors." << endl; + cout << "New factors: " << newFactors.size() + << ", New values: " << newValues.size() << endl; + + // Attempt to update the smoother with new factors and values + try { + smoother.update(newFactors, newValues, newTimestamps); + // Optional: Perform extra internal iterations if needed + size_t maxExtraIterations = 3; + for (size_t i = 1; i < maxExtraIterations; ++i) { + smoother.update(); + } + // you may not get expected results if you use the gtsam version lower than 4.3 + cout << "After update - Graph has " << smoother.getFactors().size() + << " factors, " << smoother.getFactors().nrFactors() << " nr factors." << endl; + + // Retrieve the latest estimate + currentEstimate = smoother.calculateEstimate(); + if (!currentEstimate.empty()) { + // Update lastPose to the last key's estimate + Key lastKey = currentEstimate.keys().back(); + lastPose = currentEstimate.at(lastKey); + } + + // Clear containers for the next iteration + newFactors.resize(0); + newValues.clear(); + newTimestamps.clear(); + } catch (const exception &e) { + cerr << "Smoother update failed: " << e.what() << endl; + } + } + + // The results of the last sliding window are saved to a g2o file for visualization or further analysis. + saveG2oGraph(smoother.getFactors(), currentEstimate, "isam", lineCount); + factor_file.close(); + + return 0; +} + From 6697452954a9b8676d9d33d4ec9294cac5e74dc4 Mon Sep 17 00:00:00 2001 From: Matt Morley Date: Sun, 19 Jan 2025 20:52:25 -0800 Subject: [PATCH 38/52] Boost ifdef -> if macro --- gtsam/base/timing.cpp | 4 ++-- gtsam/constrained/NonlinearConstraint.h | 4 ++-- gtsam/constrained/NonlinearEqualityConstraint.h | 8 ++++---- gtsam/constrained/NonlinearInequalityConstraint.h | 6 +++--- 4 files changed, 11 insertions(+), 11 deletions(-) diff --git a/gtsam/base/timing.cpp b/gtsam/base/timing.cpp index 9e9d1d113..ff68ad4d1 100644 --- a/gtsam/base/timing.cpp +++ b/gtsam/base/timing.cpp @@ -109,7 +109,7 @@ void TimingOutline::print(const std::string& outline) const { /* ************************************************************************* */ void TimingOutline::printCsvHeader(bool addLineBreak) const { -#ifdef GTSAM_USE_BOOST_FEATURES +#if GTSAM_USE_BOOST_FEATURES // Order is (CPU time, number of times, wall time, time + children in seconds, // min time, max time) std::cout << label_ + " cpu time (s)" << "," << label_ + " #calls" << "," @@ -134,7 +134,7 @@ void TimingOutline::printCsvHeader(bool addLineBreak) const { /* ************************************************************************* */ void TimingOutline::printCsv(bool addLineBreak) const { -#ifdef GTSAM_USE_BOOST_FEATURES +#if GTSAM_USE_BOOST_FEATURES // Order is (CPU time, number of times, wall time, time + children in seconds, // min time, max time) std::cout << self() << "," << n_ << "," << wall() << "," << secs() << "," diff --git a/gtsam/constrained/NonlinearConstraint.h b/gtsam/constrained/NonlinearConstraint.h index a01430bf3..f64e8bfc1 100644 --- a/gtsam/constrained/NonlinearConstraint.h +++ b/gtsam/constrained/NonlinearConstraint.h @@ -21,7 +21,7 @@ #include #include #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #endif @@ -77,7 +77,7 @@ class GTSAM_EXPORT NonlinearConstraint : public NoiseModelFactor { } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/constrained/NonlinearEqualityConstraint.h b/gtsam/constrained/NonlinearEqualityConstraint.h index 017ca1607..f8b0205a1 100644 --- a/gtsam/constrained/NonlinearEqualityConstraint.h +++ b/gtsam/constrained/NonlinearEqualityConstraint.h @@ -39,7 +39,7 @@ class GTSAM_EXPORT NonlinearEqualityConstraint : public NonlinearConstraint { virtual ~NonlinearEqualityConstraint() {} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -85,7 +85,7 @@ class ExpressionEqualityConstraint : public NonlinearEqualityConstraint { } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -131,7 +131,7 @@ class GTSAM_EXPORT ZeroCostConstraint : public NonlinearEqualityConstraint { } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -166,7 +166,7 @@ class GTSAM_EXPORT NonlinearEqualityConstraints : public FactorGraph diff --git a/gtsam/constrained/NonlinearInequalityConstraint.h b/gtsam/constrained/NonlinearInequalityConstraint.h index 9dbf1f008..edc2b90e9 100644 --- a/gtsam/constrained/NonlinearInequalityConstraint.h +++ b/gtsam/constrained/NonlinearInequalityConstraint.h @@ -61,7 +61,7 @@ class GTSAM_EXPORT NonlinearInequalityConstraint : public NonlinearConstraint { virtual NoiseModelFactor::shared_ptr penaltyFactorEquality(const double mu = 1.0) const; private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -129,7 +129,7 @@ class GTSAM_EXPORT ScalarExpressionInequalityConstraint : public NonlinearInequa } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -169,7 +169,7 @@ class GTSAM_EXPORT NonlinearInequalityConstraints const double mu = 1.0) const; private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template From b3c02d2b40bc6621fdb111f6c5020ece2efc0d5e Mon Sep 17 00:00:00 2001 From: Matt Morley Date: Sun, 19 Jan 2025 20:55:51 -0800 Subject: [PATCH 39/52] Move shared pointers in dtors --- gtsam/inference/BayesTree-inst.h | 11 +++++------ gtsam/inference/ClusterTree-inst.h | 12 ++++++------ gtsam/inference/EliminationTree-inst.h | 11 +++++------ 3 files changed, 16 insertions(+), 18 deletions(-) diff --git a/gtsam/inference/BayesTree-inst.h b/gtsam/inference/BayesTree-inst.h index fdfa94d2b..0648a90f6 100644 --- a/gtsam/inference/BayesTree-inst.h +++ b/gtsam/inference/BayesTree-inst.h @@ -196,21 +196,20 @@ namespace gtsam { for (auto&& root: roots_) { std::queue bfs_queue; - // first, move the root to the queue - bfs_queue.push(root); - root = nullptr; // now the root node is owned by the queue + // first, steal the root and move it to the queue. This invalidates root + bfs_queue.push(std::move(root)); // do a BFS on the tree, for each node, add its children to the queue, and then delete it from the queue // So if the reference count of the node is 1, it will be deleted, and because its children are in the queue, // the deletion of the node will not trigger a recursive deletion of the children. while (!bfs_queue.empty()) { - // move the ownership of the front node from the queue to the current variable - auto current = bfs_queue.front(); + // move the ownership of the front node from the queue to the current variable, invalidating the sharedClique at the front of the queue + auto current = std::move(bfs_queue.front()); bfs_queue.pop(); // add the children of the current node to the queue, so that the queue will also own the children nodes. for (auto child: current->children) { - bfs_queue.push(child); + bfs_queue.push(std::move(child)); } // leaving the scope of current will decrease the reference count of the current node by 1, and if the reference count is 0, // the node will be deleted. Because the children are in the queue, the deletion of the node will not trigger a recursive // deletion of the children. diff --git a/gtsam/inference/ClusterTree-inst.h b/gtsam/inference/ClusterTree-inst.h index 2a5f0b455..1a0b4470d 100644 --- a/gtsam/inference/ClusterTree-inst.h +++ b/gtsam/inference/ClusterTree-inst.h @@ -114,20 +114,20 @@ ClusterTree::~ClusterTree() { for (auto&& root : roots_) { std::queue bfs_queue; - // first, move the root to the queue - bfs_queue.push(root); - root = nullptr; // now the root node is owned by the queue + + // first, steal the root and move it to the queue. This invalidates root + bfs_queue.push(std::move(root)); // for each node iterated, if its reference count is 1, it will be deleted while its children are still in the queue. // so that the recursive deletion will not happen. while (!bfs_queue.empty()) { - // move the ownership of the front node from the queue to the current variable - auto node = bfs_queue.front(); + // move the ownership of the front node from the queue to the current variable, invalidating the sharedClique at the front of the queue + auto node = std::move(bfs_queue.front()); bfs_queue.pop(); // add the children of the current node to the queue, so that the queue will also own the children nodes. for (auto child : node->children) { - bfs_queue.push(child); + bfs_queue.push(std::move(child)); } // leaving the scope of current will decrease the reference count of the current node by 1, and if the reference count is 0, // the node will be deleted. Because the children are in the queue, the deletion of the node will not trigger a recursive // deletion of the children. diff --git a/gtsam/inference/EliminationTree-inst.h b/gtsam/inference/EliminationTree-inst.h index 16449d0f4..3ab0e9ce4 100644 --- a/gtsam/inference/EliminationTree-inst.h +++ b/gtsam/inference/EliminationTree-inst.h @@ -197,20 +197,19 @@ namespace gtsam { for (auto&& root : roots_) { std::queue bfs_queue; - // first, move the root to the queue - bfs_queue.push(root); - root = nullptr; // now the root node is owned by the queue + // first, steal the root and move it to the queue. This invalidates root + bfs_queue.push(std::move(root)); // for each node iterated, if its reference count is 1, it will be deleted while its children are still in the queue. // so that the recursive deletion will not happen. while (!bfs_queue.empty()) { - // move the ownership of the front node from the queue to the current variable - auto node = bfs_queue.front(); + // move the ownership of the front node from the queue to the current variable, invalidating the sharedClique at the front of the queue + auto node = std::move(bfs_queue.front()); bfs_queue.pop(); // add the children of the current node to the queue, so that the queue will also own the children nodes. for (auto&& child : node->children) { - bfs_queue.push(child); + bfs_queue.push(std::move(child)); } // leaving the scope of current will decrease the reference count of the current node by 1, and if the reference count is 0, // the node will be deleted. Because the children are in the queue, the deletion of the node will not trigger a recursive // deletion of the children. From 71944110e9772eda5c0d0081d5e49a079bc3e557 Mon Sep 17 00:00:00 2001 From: Matt Morley Date: Sun, 19 Jan 2025 21:24:42 -0800 Subject: [PATCH 40/52] Make destructors protected --- gtsam/inference/ClusterTree.h | 2 +- gtsam/inference/EliminationTree.h | 4 +++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/gtsam/inference/ClusterTree.h b/gtsam/inference/ClusterTree.h index 43859f3d3..f81029ba5 100644 --- a/gtsam/inference/ClusterTree.h +++ b/gtsam/inference/ClusterTree.h @@ -166,9 +166,9 @@ class ClusterTree { /// @} + protected: ~ClusterTree(); - protected: /// @name Details /// @{ diff --git a/gtsam/inference/EliminationTree.h b/gtsam/inference/EliminationTree.h index 724c68608..557be1675 100644 --- a/gtsam/inference/EliminationTree.h +++ b/gtsam/inference/EliminationTree.h @@ -117,8 +117,10 @@ namespace gtsam { /// @} - public: + protected: ~EliminationTree(); + + public: /// @name Standard Interface /// @{ From 0959bfef5c7e206a78e38ded84c4397940ea80ac Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 20 Jan 2025 02:27:31 -0500 Subject: [PATCH 41/52] replace #ifdef with #if --- gtsam/constrained/NonlinearConstraint.h | 4 ++-- gtsam/constrained/NonlinearEqualityConstraint.h | 8 ++++---- gtsam/constrained/NonlinearInequalityConstraint.h | 6 +++--- 3 files changed, 9 insertions(+), 9 deletions(-) diff --git a/gtsam/constrained/NonlinearConstraint.h b/gtsam/constrained/NonlinearConstraint.h index a01430bf3..f64e8bfc1 100644 --- a/gtsam/constrained/NonlinearConstraint.h +++ b/gtsam/constrained/NonlinearConstraint.h @@ -21,7 +21,7 @@ #include #include #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #endif @@ -77,7 +77,7 @@ class GTSAM_EXPORT NonlinearConstraint : public NoiseModelFactor { } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/constrained/NonlinearEqualityConstraint.h b/gtsam/constrained/NonlinearEqualityConstraint.h index 017ca1607..f8b0205a1 100644 --- a/gtsam/constrained/NonlinearEqualityConstraint.h +++ b/gtsam/constrained/NonlinearEqualityConstraint.h @@ -39,7 +39,7 @@ class GTSAM_EXPORT NonlinearEqualityConstraint : public NonlinearConstraint { virtual ~NonlinearEqualityConstraint() {} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -85,7 +85,7 @@ class ExpressionEqualityConstraint : public NonlinearEqualityConstraint { } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -131,7 +131,7 @@ class GTSAM_EXPORT ZeroCostConstraint : public NonlinearEqualityConstraint { } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -166,7 +166,7 @@ class GTSAM_EXPORT NonlinearEqualityConstraints : public FactorGraph diff --git a/gtsam/constrained/NonlinearInequalityConstraint.h b/gtsam/constrained/NonlinearInequalityConstraint.h index 9dbf1f008..edc2b90e9 100644 --- a/gtsam/constrained/NonlinearInequalityConstraint.h +++ b/gtsam/constrained/NonlinearInequalityConstraint.h @@ -61,7 +61,7 @@ class GTSAM_EXPORT NonlinearInequalityConstraint : public NonlinearConstraint { virtual NoiseModelFactor::shared_ptr penaltyFactorEquality(const double mu = 1.0) const; private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -129,7 +129,7 @@ class GTSAM_EXPORT ScalarExpressionInequalityConstraint : public NonlinearInequa } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -169,7 +169,7 @@ class GTSAM_EXPORT NonlinearInequalityConstraints const double mu = 1.0) const; private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template From a00335ff4a36200af81f57bbcfcac824bc2a0a83 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 20 Jan 2025 02:32:17 -0500 Subject: [PATCH 42/52] fully declare struct U --- gtsam/base/std_optional_serialization.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/base/std_optional_serialization.h b/gtsam/base/std_optional_serialization.h index 93a5c8dba..265ef1620 100644 --- a/gtsam/base/std_optional_serialization.h +++ b/gtsam/base/std_optional_serialization.h @@ -48,7 +48,7 @@ */ #ifdef __GNUC__ #if __GNUC__ >= 7 && __cplusplus >= 201703L -namespace boost { namespace serialization { struct U; } } +namespace boost { namespace serialization { struct U{}; } } namespace std { template<> struct is_trivially_default_constructible : std::false_type {}; } namespace std { template<> struct is_trivially_copy_constructible : std::false_type {}; } namespace std { template<> struct is_trivially_move_constructible : std::false_type {}; } From aa91159fcbc797730501bcc5861ca969566fbb4c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 20 Jan 2025 02:35:20 -0500 Subject: [PATCH 43/52] add Ubuntu 24.04 to the CI --- .github/workflows/build-linux.yml | 29 +++++++++++++++++++++-------- 1 file changed, 21 insertions(+), 8 deletions(-) diff --git a/.github/workflows/build-linux.yml b/.github/workflows/build-linux.yml index 3c1275a55..317b82dfd 100644 --- a/.github/workflows/build-linux.yml +++ b/.github/workflows/build-linux.yml @@ -2,8 +2,8 @@ name: Linux CI on: [pull_request] -# Every time you make a push to your PR, it cancel immediately the previous checks, -# and start a new one. The other runner will be available more quickly to your PR. +# Every time you make a push to your PR, it cancel immediately the previous checks, +# and start a new one. The other runner will be available more quickly to your PR. concurrency: group: ${{ github.workflow }}-${{ github.head_ref || github.run_id }} cancel-in-progress: true @@ -24,12 +24,15 @@ jobs: matrix: # Github Actions requires a single row to be added to the build matrix. # See https://help.github.com/en/articles/workflow-syntax-for-github-actions. - name: [ - ubuntu-20.04-gcc-9, - ubuntu-20.04-clang-9, - ubuntu-22.04-gcc-12, - ubuntu-22.04-clang-14, - ] + name: + [ + ubuntu-20.04-gcc-9, + ubuntu-20.04-clang-9, + ubuntu-22.04-gcc-12, + ubuntu-22.04-clang-14, + ubuntu-24.04-gcc-13, + ubuntu-24.04-clang-16, + ] build_type: [Debug, Release] build_unstable: [ON] @@ -54,6 +57,16 @@ jobs: compiler: clang version: "14" + - name: ubuntu-24.04-gcc-13 + os: ubuntu-24.04 + compiler: gcc + version: "13" + + - name: ubuntu-24.04-clang-16 + os: ubuntu-24.04 + compiler: clang + version: "16" + steps: - name: Checkout uses: actions/checkout@v4 From 3d116d7fdf6eac746a629953cc4d44dea17b6c6b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 20 Jan 2025 10:04:06 -0500 Subject: [PATCH 44/52] pass in this to lambda expression explicitly --- .../constrained/InequalityPenaltyFunction.cpp | 26 ++++++++++++------- 1 file changed, 16 insertions(+), 10 deletions(-) diff --git a/gtsam/constrained/InequalityPenaltyFunction.cpp b/gtsam/constrained/InequalityPenaltyFunction.cpp index 2e331292c..238733c97 100644 --- a/gtsam/constrained/InequalityPenaltyFunction.cpp +++ b/gtsam/constrained/InequalityPenaltyFunction.cpp @@ -20,12 +20,15 @@ namespace gtsam { -/* ********************************************************************************************* */ -InequalityPenaltyFunction::UnaryScalarFunc InequalityPenaltyFunction::function() const { - return [=](const double& x, OptionalJacobian<1, 1> H = {}) -> double { return (*this)(x, H); }; +/* ************************************************************************* */ +InequalityPenaltyFunction::UnaryScalarFunc InequalityPenaltyFunction::function() + const { + return [this](const double& x, OptionalJacobian<1, 1> H = {}) { + return this->operator()(x, H); + }; } -/* ********************************************************************************************* */ +/* ************************************************************************* */ double RampFunction::Ramp(const double x, OptionalJacobian<1, 1> H) { if (x < 0) { if (H) { @@ -40,8 +43,9 @@ double RampFunction::Ramp(const double x, OptionalJacobian<1, 1> H) { } } -/* ********************************************************************************************* */ -double SmoothRampPoly2::operator()(const double& x, OptionalJacobian<1, 1> H) const { +/* ************************************************************************* */ +double SmoothRampPoly2::operator()(const double& x, + OptionalJacobian<1, 1> H) const { if (x <= 0) { if (H) { H->setZero(); @@ -60,8 +64,9 @@ double SmoothRampPoly2::operator()(const double& x, OptionalJacobian<1, 1> H) co } } -/* ********************************************************************************************* */ -double SmoothRampPoly3::operator()(const double& x, OptionalJacobian<1, 1> H) const { +/* ************************************************************************* */ +double SmoothRampPoly3::operator()(const double& x, + OptionalJacobian<1, 1> H) const { if (x <= 0) { if (H) { H->setZero(); @@ -80,8 +85,9 @@ double SmoothRampPoly3::operator()(const double& x, OptionalJacobian<1, 1> H) co } } -/* ********************************************************************************************* */ -double SoftPlusFunction::operator()(const double& x, OptionalJacobian<1, 1> H) const { +/* ************************************************************************* */ +double SoftPlusFunction::operator()(const double& x, + OptionalJacobian<1, 1> H) const { if (H) { H->setConstant(1 / (1 + std::exp(-k_ * x))); } From 7eb5ad67e3e9b06fc4b9173623dce33c222d6855 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 20 Jan 2025 10:20:01 -0500 Subject: [PATCH 45/52] use reference capture instead of copy capture --- gtsam/geometry/tests/testSO3.cpp | 12 ++++++------ gtsam/navigation/tests/testCombinedImuFactor.cpp | 2 +- gtsam/navigation/tests/testImuFactor.cpp | 4 ++-- .../navigation/tests/testManifoldPreintegration.cpp | 6 +++--- gtsam/navigation/tests/testTangentPreintegration.cpp | 4 ++-- 5 files changed, 14 insertions(+), 14 deletions(-) diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index 41777ae3a..17b27daea 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -333,7 +333,7 @@ TEST(SO3, CrossB) { Matrix aH1; for (bool nearZero : {true, false}) { std::function f = - [=](const Vector3& omega, const Vector3& v) { + [nearZero](const Vector3& omega, const Vector3& v) { return so3::DexpFunctor(omega, nearZero).crossB(v); }; for (const Vector3& omega : test_cases::omegas(nearZero)) { @@ -351,7 +351,7 @@ TEST(SO3, DoubleCrossC) { Matrix aH1; for (bool nearZero : {true, false}) { std::function f = - [=](const Vector3& omega, const Vector3& v) { + [nearZero](const Vector3& omega, const Vector3& v) { return so3::DexpFunctor(omega, nearZero).doubleCrossC(v); }; for (const Vector3& omega : test_cases::omegas(nearZero)) { @@ -369,7 +369,7 @@ TEST(SO3, ApplyDexp) { Matrix aH1, aH2; for (bool nearZero : {true, false}) { std::function f = - [=](const Vector3& omega, const Vector3& v) { + [nearZero](const Vector3& omega, const Vector3& v) { return so3::DexpFunctor(omega, nearZero).applyDexp(v); }; for (const Vector3& omega : test_cases::omegas(nearZero)) { @@ -390,7 +390,7 @@ TEST(SO3, ApplyInvDexp) { Matrix aH1, aH2; for (bool nearZero : {true, false}) { std::function f = - [=](const Vector3& omega, const Vector3& v) { + [nearZero](const Vector3& omega, const Vector3& v) { return so3::DexpFunctor(omega, nearZero).applyInvDexp(v); }; for (const Vector3& omega : test_cases::omegas(nearZero)) { @@ -412,7 +412,7 @@ TEST(SO3, ApplyLeftJacobian) { Matrix aH1, aH2; for (bool nearZero : {true, false}) { std::function f = - [=](const Vector3& omega, const Vector3& v) { + [nearZero](const Vector3& omega, const Vector3& v) { return so3::DexpFunctor(omega, nearZero).applyLeftJacobian(v); }; for (const Vector3& omega : test_cases::omegas(nearZero)) { @@ -433,7 +433,7 @@ TEST(SO3, ApplyLeftJacobianInverse) { Matrix aH1, aH2; for (bool nearZero : {true, false}) { std::function f = - [=](const Vector3& omega, const Vector3& v) { + [nearZero](const Vector3& omega, const Vector3& v) { return so3::DexpFunctor(omega, nearZero).applyLeftJacobianInverse(v); }; for (const Vector3& omega : test_cases::omegas(nearZero)) { diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index c4fefb8ff..80a7b0298 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -144,7 +144,7 @@ TEST(CombinedImuFactor, FirstOrderPreIntegratedMeasurements) { auto p = testing::Params(); testing::SomeMeasurements measurements; - auto preintegrated = [=](const Vector3& a, const Vector3& w) { + auto preintegrated = [&](const Vector3& a, const Vector3& w) { PreintegratedImuMeasurements pim(p, Bias(a, w)); testing::integrateMeasurements(measurements, &pim); return pim.preintegrated(); diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index d4bc763ee..2e1528ae8 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -399,7 +399,7 @@ TEST(ImuFactor, PartialDerivative_wrt_Bias) { Vector3 measuredOmega(0.1, 0, 0); double deltaT = 0.5; - auto evaluateRotation = [=](const Vector3 biasOmega) { + auto evaluateRotation = [&measuredOmega, &deltaT](const Vector3 biasOmega) { return Rot3::Expmap((measuredOmega - biasOmega) * deltaT); }; @@ -424,7 +424,7 @@ TEST(ImuFactor, PartialDerivativeLogmap) { // Measurements Vector3 deltaTheta(0, 0, 0); - auto evaluateLogRotation = [=](const Vector3 delta) { + auto evaluateLogRotation = [&thetaHat](const Vector3 delta) { return Rot3::Logmap( Rot3::Expmap(thetaHat).compose(Rot3::Expmap(delta))); }; diff --git a/gtsam/navigation/tests/testManifoldPreintegration.cpp b/gtsam/navigation/tests/testManifoldPreintegration.cpp index 82f9876fb..f9c6ebacb 100644 --- a/gtsam/navigation/tests/testManifoldPreintegration.cpp +++ b/gtsam/navigation/tests/testManifoldPreintegration.cpp @@ -43,21 +43,21 @@ TEST(ManifoldPreintegration, BiasCorrectionJacobians) { testing::SomeMeasurements measurements; std::function deltaRij = - [=](const Vector3& a, const Vector3& w) { + [&](const Vector3& a, const Vector3& w) { ManifoldPreintegration pim(testing::Params(), Bias(a, w)); testing::integrateMeasurements(measurements, &pim); return pim.deltaRij(); }; std::function deltaPij = - [=](const Vector3& a, const Vector3& w) { + [&](const Vector3& a, const Vector3& w) { ManifoldPreintegration pim(testing::Params(), Bias(a, w)); testing::integrateMeasurements(measurements, &pim); return pim.deltaPij(); }; std::function deltaVij = - [=](const Vector3& a, const Vector3& w) { + [&](const Vector3& a, const Vector3& w) { ManifoldPreintegration pim(testing::Params(), Bias(a, w)); testing::integrateMeasurements(measurements, &pim); return pim.deltaVij(); diff --git a/gtsam/navigation/tests/testTangentPreintegration.cpp b/gtsam/navigation/tests/testTangentPreintegration.cpp index af91f4f2c..73cea9f71 100644 --- a/gtsam/navigation/tests/testTangentPreintegration.cpp +++ b/gtsam/navigation/tests/testTangentPreintegration.cpp @@ -78,7 +78,7 @@ TEST(ImuFactor, BiasCorrectionJacobians) { testing::SomeMeasurements measurements; std::function preintegrated = - [=](const Vector3& a, const Vector3& w) { + [&](const Vector3& a, const Vector3& w) { TangentPreintegration pim(testing::Params(), Bias(a, w)); testing::integrateMeasurements(measurements, &pim); return pim.preintegrated(); @@ -149,7 +149,7 @@ TEST(TangentPreintegration, Compose) { TEST(TangentPreintegration, MergedBiasDerivatives) { testing::SomeMeasurements measurements; - auto f = [=](const Vector3& a, const Vector3& w) { + auto f = [&](const Vector3& a, const Vector3& w) { TangentPreintegration pim02(testing::Params(), Bias(a, w)); testing::integrateMeasurements(measurements, &pim02); testing::integrateMeasurements(measurements, &pim02); From 36758a818fb8e0a1da384eb44e90edb86be10a16 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 20 Jan 2025 10:20:21 -0500 Subject: [PATCH 46/52] add comment and remove unnecessary copy-constructor --- gtsam/base/std_optional_serialization.h | 1 + gtsam/base/tests/testStdOptionalSerialization.cpp | 2 -- 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/gtsam/base/std_optional_serialization.h b/gtsam/base/std_optional_serialization.h index 265ef1620..db5e994e9 100644 --- a/gtsam/base/std_optional_serialization.h +++ b/gtsam/base/std_optional_serialization.h @@ -48,6 +48,7 @@ */ #ifdef __GNUC__ #if __GNUC__ >= 7 && __cplusplus >= 201703L +// Based on https://github.com/borglab/gtsam/issues/1738, we define U as a complete type. namespace boost { namespace serialization { struct U{}; } } namespace std { template<> struct is_trivially_default_constructible : std::false_type {}; } namespace std { template<> struct is_trivially_copy_constructible : std::false_type {}; } diff --git a/gtsam/base/tests/testStdOptionalSerialization.cpp b/gtsam/base/tests/testStdOptionalSerialization.cpp index d9bd1da4a..3c1310aa6 100644 --- a/gtsam/base/tests/testStdOptionalSerialization.cpp +++ b/gtsam/base/tests/testStdOptionalSerialization.cpp @@ -60,8 +60,6 @@ public: TestOptionalStruct() = default; TestOptionalStruct(const int& opt) : opt(opt) {} - // A copy constructor is needed for serialization - TestOptionalStruct(const TestOptionalStruct& other) = default; bool operator==(const TestOptionalStruct& other) const { // check the values are equal return *opt == *other.opt; From c2ee7ff12263699bcb5492b59ff74d86934d5227 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 20 Jan 2025 10:24:57 -0500 Subject: [PATCH 47/52] bracket clang and GCC --- .github/workflows/build-linux.yml | 22 +++++----------------- 1 file changed, 5 insertions(+), 17 deletions(-) diff --git a/.github/workflows/build-linux.yml b/.github/workflows/build-linux.yml index 317b82dfd..b4b391df3 100644 --- a/.github/workflows/build-linux.yml +++ b/.github/workflows/build-linux.yml @@ -24,13 +24,11 @@ jobs: matrix: # Github Actions requires a single row to be added to the build matrix. # See https://help.github.com/en/articles/workflow-syntax-for-github-actions. - name: - [ + name: [ + # "Bracket" the versions from GCC [9-14] and Clang [9-16] ubuntu-20.04-gcc-9, ubuntu-20.04-clang-9, - ubuntu-22.04-gcc-12, - ubuntu-22.04-clang-14, - ubuntu-24.04-gcc-13, + ubuntu-24.04-gcc-14, ubuntu-24.04-clang-16, ] @@ -47,20 +45,10 @@ jobs: compiler: clang version: "9" - - name: ubuntu-22.04-gcc-12 - os: ubuntu-22.04 - compiler: gcc - version: "11" - - - name: ubuntu-22.04-clang-14 - os: ubuntu-22.04 - compiler: clang - version: "14" - - - name: ubuntu-24.04-gcc-13 + - name: ubuntu-24.04-gcc-14 os: ubuntu-24.04 compiler: gcc - version: "13" + version: "14" - name: ubuntu-24.04-clang-16 os: ubuntu-24.04 From 5ce332764c0f466ceddacb18559ecfa5455d1a5d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 20 Jan 2025 10:27:11 -0500 Subject: [PATCH 48/52] don't install boost if no_boost flag is set --- .github/workflows/build-special.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/build-special.yml b/.github/workflows/build-special.yml index 3a7dd974d..eb1d96fdc 100644 --- a/.github/workflows/build-special.yml +++ b/.github/workflows/build-special.yml @@ -116,7 +116,7 @@ jobs: fi - name: Install Boost - if: runner.os == 'Linux' + if: runner.os == 'Linux' && matrix.flag != 'no_boost' run: | sudo apt-get -y install libboost-all-dev From cdb761c603c848088395d5af52f5b92562cbb122 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 20 Jan 2025 10:43:36 -0500 Subject: [PATCH 49/52] update unix.sh to set Boost flags from env variable correctly --- .github/scripts/unix.sh | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.github/scripts/unix.sh b/.github/scripts/unix.sh index 09fcd788b..b2e960fc1 100644 --- a/.github/scripts/unix.sh +++ b/.github/scripts/unix.sh @@ -52,6 +52,8 @@ function configure() -DGTSAM_POSE3_EXPMAP=${GTSAM_POSE3_EXPMAP:-ON} \ -DGTSAM_USE_SYSTEM_EIGEN=${GTSAM_USE_SYSTEM_EIGEN:-OFF} \ -DGTSAM_USE_SYSTEM_METIS=${GTSAM_USE_SYSTEM_METIS:-OFF} \ + -DGTSAM_USE_BOOST_FEATURES=${GTSAM_USE_BOOST_FEATURES:-ON} \ + -DGTSAM_ENABLE_BOOST_SERIALIZATION=${GTSAM_ENABLE_BOOST_SERIALIZATION:-ON} \ -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \ -DGTSAM_SINGLE_TEST_EXE=OFF } From 08fc4885f7d198b2fb4efdd387e8cdd71a2a3b5c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 20 Jan 2025 10:45:04 -0500 Subject: [PATCH 50/52] check for boost all in one step --- .github/workflows/build-special.yml | 22 +++++++++++++--------- 1 file changed, 13 insertions(+), 9 deletions(-) diff --git a/.github/workflows/build-special.yml b/.github/workflows/build-special.yml index eb1d96fdc..d0a182975 100644 --- a/.github/workflows/build-special.yml +++ b/.github/workflows/build-special.yml @@ -2,8 +2,8 @@ name: Special Cases CI on: [pull_request] -# Every time you make a push to your PR, it cancel immediately the previous checks, -# and start a new one. The other runner will be available more quickly to your PR. +# Every time you make a push to your PR, it cancel immediately the previous checks, +# and start a new one. The other runner will be available more quickly to your PR. concurrency: group: ${{ github.workflow }}-${{ github.head_ref || github.run_id }} cancel-in-progress: true @@ -115,19 +115,24 @@ jobs: echo "CXX=clang++-${{ matrix.version }}" >> $GITHUB_ENV fi - - name: Install Boost - if: runner.os == 'Linux' && matrix.flag != 'no_boost' - run: | - sudo apt-get -y install libboost-all-dev - - name: Install (macOS) if: runner.os == 'macOS' run: | - brew install cmake ninja boost + brew install cmake ninja sudo xcode-select -switch /Applications/Xcode_${{ matrix.version }}.app echo "CC=clang" >> $GITHUB_ENV echo "CXX=clang++" >> $GITHUB_ENV + - name: Install Boost + run: | + if [ ${{matrix.flag}} != 'no_boost' ]; then + if [ ${{runner.os}} == 'Linux' ]; then + sudo apt-get -y install libboost-all-dev + elif [ ${{runner.os}} == 'macOS' ]; then + brew install boost + fi + fi + - name: Set Allow Deprecated Flag if: matrix.flag == 'deprecated' run: | @@ -181,7 +186,6 @@ jobs: with: swap-size-gb: 12 - - name: Build & Test run: | bash .github/scripts/unix.sh -t From 49abc02b4ea8962d4a5180e19b9d6f9b4d641b9d Mon Sep 17 00:00:00 2001 From: JokerJohn <2022087641@qq.com> Date: Tue, 21 Jan 2025 04:02:54 +0800 Subject: [PATCH 51/52] remove unitest of issue1452 --- .../tests/testIncrementalFixedLagSmoother.cpp | 150 ------------------ 1 file changed, 150 deletions(-) diff --git a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp index f025c4470..269177b5d 100644 --- a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp @@ -356,156 +356,6 @@ TEST(IncrementalFixedLagSmoother, Example) { } } -/* ************************************************************************* */ -namespace issue1452 { - -// Factor types definition -enum FactorType { PRIOR = 0, BETWEEN = 1 }; - -// Helper function to read covariance matrix from input stream -Matrix6 readCovarianceMatrix(istringstream& iss) { - Matrix6 cov; - for (int r = 0; r < 6; ++r) { - for (int c = 0; c < 6; ++c) { - iss >> cov(r, c); - } - } - return cov; -} - -// Helper function to create pose from parameters -Pose3 createPose(double x, double y, double z, double roll, double pitch, - double yaw) { - return Pose3(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z)); -} - -/** - * Data Format - * PRIOR factor: factor_type timestamp key pose(x y z roll pitch yaw) cov(6*6) - * BETWEEN factor: factor_type timestamp key1 key2 pose(x y z r p y) cov(6*6) - * */ - -TEST(IncrementalFixedLagSmoother, Issue1452) { - // Open factor graph file - auto path = findExampleDataFile("issue1452.txt"); - cout << "path = " << path << endl; - ifstream infile(path); - CHECK(infile.is_open()); - - // Setup ISAM2 parameters for smoother - ISAM2Params isam_parameters; - isam_parameters.relinearizeThreshold = 0.1; - isam_parameters.relinearizeSkip = 1; - // isam_parameters.cacheLinearizedFactors = true; - isam_parameters.findUnusedFactorSlots = true; - // isam_parameters.evaluateNonlinearError = false; - // isam_parameters.enableDetailedResults = true; - - // Initialize fixed-lag smoother with 1-second window - IncrementalFixedLagSmoother smoother(1, isam_parameters); - NonlinearFactorGraph newFactors; - Values newValues, currentEstimate; - FixedLagSmoother::KeyTimestampMap newTimestamps; - Pose3 lastPose; - - // check the isam parameters - isam_parameters.print(); - - string line; - int lineCount = 0; - while (getline(infile, line)) { - if (line.empty()) continue; - istringstream iss(line); - - // if we only want to read less data - // if (lineCount > 100) break; - - cout << "\n========================Processing line " << ++lineCount - << " =========================" << endl; - int factorType; - iss >> factorType; - if (factorType == PRIOR) { - // Read prior factor data, only the first line to fix the coordinate - // system - double timestamp; - int key; - double x, y, z, roll, pitch, yaw; - iss >> timestamp >> key >> x >> y >> z >> roll >> pitch >> yaw; - // Create pose and add prior factor - Pose3 pose = createPose(x, y, z, roll, pitch, yaw); - Matrix6 cov = readCovarianceMatrix(iss); - auto noise = noiseModel::Gaussian::Covariance(cov); - newFactors.add(PriorFactor(X(key), pose, noise)); - if (!newValues.exists(X(key))) { - newValues.insert(X(key), pose); - newTimestamps[X(key)] = timestamp; - } - cout << "Add prior factor " << key << endl; - } else if (factorType == BETWEEN) { - // Read between factor data - double timestamp; - int key1, key2; - // Read timestamps and keys - iss >> timestamp >> key1 >> key2; - // Read relative pose between key1 and key2 - double x1, y1, z1, roll1, pitch1, yaw1; - iss >> x1 >> y1 >> z1 >> roll1 >> pitch1 >> yaw1; - Pose3 relative_pose = createPose(x1, y1, z1, roll1, pitch1, yaw1); - // Read covariance of relative_pose - Matrix6 cov = readCovarianceMatrix(iss); - auto noise = noiseModel::Gaussian::Covariance(cov); - // Add between factor of key1 and key2 - newFactors.add( - BetweenFactor(X(key1), X(key2), relative_pose, noise)); - if (!newValues.exists(X(key2))) { - // Use last optimized pose composed with relative pose for key2 - newValues.insert(X(key2), lastPose.compose(relative_pose)); - newTimestamps[X(key2)] = timestamp; - } - cout << "Add between factor " << key1 << " -> " << key2 << endl; - } - // Print statistics before update - cout << "Before update - Factors: " << smoother.getFactors().size() - << ", NR Factors: " << smoother.getFactors().nrFactors() << endl; - cout << "New factors: " << newFactors.size() - << ", New values: " << newValues.size() << endl; - // Update smoother - try { - smoother.update(newFactors, newValues, newTimestamps); - int max_extra_iterations = 3; - for (size_t n_iter = 1; n_iter < max_extra_iterations; ++n_iter) { - smoother.update(); - } - - cout << "After update - Factors: " << smoother.getFactors().size() - << ", NR Factors: " << smoother.getFactors().nrFactors() << endl; - - // Update current estimate and last pose - currentEstimate = smoother.calculateEstimate(); - if (!currentEstimate.empty()) { - lastPose = currentEstimate.at(currentEstimate.keys().back()); - // Optional: Print the latest pose for debugging - // cout << "Latest pose: " << - // lastPose.translation().transpose() << endl; - } - - // Clear containers for next iteration - newFactors.resize(0); - newValues.clear(); - newTimestamps.clear(); - } catch (const exception& e) { - cerr << "Update failed: " << e.what() << endl; - } - } - - // Check that the number of factors is correct - CHECK_EQUAL(12, smoother.getFactors().size()); - CHECK_EQUAL(11, smoother.getFactors().nrFactors()); - infile.close(); -} - -} // namespace issue1452 -/* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); From 2115cd19d2e76b8f4c42ef3241027310105d4d55 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 20 Jan 2025 19:23:32 -0500 Subject: [PATCH 52/52] special CI build for examples --- .github/scripts/unix.sh | 4 +--- .github/workflows/build-special.yml | 12 ++++++++++++ 2 files changed, 13 insertions(+), 3 deletions(-) diff --git a/.github/scripts/unix.sh b/.github/scripts/unix.sh index b2e960fc1..931495744 100644 --- a/.github/scripts/unix.sh +++ b/.github/scripts/unix.sh @@ -45,7 +45,7 @@ function configure() -DGTSAM_BUILD_TESTS=${GTSAM_BUILD_TESTS:-OFF} \ -DGTSAM_BUILD_UNSTABLE=${GTSAM_BUILD_UNSTABLE:-ON} \ -DGTSAM_WITH_TBB=${GTSAM_WITH_TBB:-OFF} \ - -DGTSAM_BUILD_EXAMPLES_ALWAYS=${GTSAM_BUILD_EXAMPLES_ALWAYS:-ON} \ + -DGTSAM_BUILD_EXAMPLES_ALWAYS=${GTSAM_BUILD_EXAMPLES_ALWAYS:-OFF} \ -DGTSAM_ALLOW_DEPRECATED_SINCE_V43=${GTSAM_ALLOW_DEPRECATED_SINCE_V43:-OFF} \ -DGTSAM_USE_QUATERNIONS=${GTSAM_USE_QUATERNIONS:-OFF} \ -DGTSAM_ROT3_EXPMAP=${GTSAM_ROT3_EXPMAP:-ON} \ @@ -71,7 +71,6 @@ function finish () # compile the code with the intent of populating the cache function build () { - export GTSAM_BUILD_EXAMPLES_ALWAYS=ON export GTSAM_BUILD_TESTS=OFF configure @@ -92,7 +91,6 @@ function build () # run the tests function test () { - export GTSAM_BUILD_EXAMPLES_ALWAYS=OFF export GTSAM_BUILD_TESTS=ON configure diff --git a/.github/workflows/build-special.yml b/.github/workflows/build-special.yml index d0a182975..5ad7039ed 100644 --- a/.github/workflows/build-special.yml +++ b/.github/workflows/build-special.yml @@ -34,6 +34,7 @@ jobs: ubuntu-clang-system-libs, ubuntu-no-boost, ubuntu-no-unstable, + ubuntu-build-examples, ] build_type: [Debug, Release] @@ -81,6 +82,12 @@ jobs: version: "14" flag: no_unstable + - name: ubuntu-build-examples + os: ubuntu-22.04 + compiler: clang + version: "14" + flag: build_examples + steps: - name: Checkout uses: actions/checkout@v4 @@ -158,6 +165,11 @@ jobs: echo "GTSAM_ROT3_EXPMAP=OFF" >> $GITHUB_ENV echo "GTSAM Uses Cayley map for Rot3" + - name: Build Examples + if: matrix.flag == 'build_examples' + run: | + echo "GTSAM_BUILD_EXAMPLES_ALWAYS=ON" >> $GITHUB_ENV + - name: Use system versions of 3rd party libraries if: matrix.flag == 'system' run: |