From c5a26aec5974ae0941f47e22933bf915979b9069 Mon Sep 17 00:00:00 2001 From: mkielo3 Date: Thu, 8 May 2025 12:16:36 -0400 Subject: [PATCH] fixed paths to move Event from unstable --- gtsam/geometry/geometry.i | 28 +++++++++++++------ .../examples/TimeOfArrivalExample.cpp | 2 +- gtsam_unstable/gtsam_unstable.i | 13 +-------- .../nonlinear/IncrementalFixedLagSmoother.h | 12 ++++---- gtsam_unstable/slam/TOAFactor.h | 2 +- gtsam_unstable/slam/tests/testTOAFactor.cpp | 2 +- .../examples/TimeOfArrivalExample.py | 4 +-- 7 files changed, 31 insertions(+), 32 deletions(-) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 87bdde7c9..9e3cf1ad1 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -213,7 +213,7 @@ namespace so3 { ExpmapFunctor(const gtsam::Vector3& axis, double angle); gtsam::Matrix3 expmap() const; }; - + virtual class DexpFunctor : gtsam::so3::ExpmapFunctor { const gtsam::Vector3 omega; const double C; // (1 - A) / theta^2 @@ -228,7 +228,7 @@ namespace so3 { gtsam::Vector3 applyRightJacobianInverse(const gtsam::Vector3& v) const; gtsam::Vector3 applyLeftJacobian(const gtsam::Vector3& v) const; gtsam::Vector3 applyLeftJacobianInverse(const gtsam::Vector3& v) const; - }; + }; } class SO3 { @@ -427,7 +427,7 @@ class Rot3 { Eigen::Ref HR, Eigen::Ref Hp) const; gtsam::Unit3 unrotate(const gtsam::Unit3& p) const; - + // Standard Interface gtsam::Matrix matrix() const; gtsam::Matrix transpose() const; @@ -522,7 +522,7 @@ class Pose2 { // enabling serialization functionality void serialize() const; }; - + #include class Pose3 { // Standard Constructors @@ -573,12 +573,12 @@ class Pose3 { static gtsam::Matrix6 LogmapDerivative(const gtsam::Pose3& xi); static gtsam::Pose3 Expmap(gtsam::Vector xi, Eigen::Ref Hxi); static gtsam::Vector Logmap(const gtsam::Pose3& pose, Eigen::Ref Hpose); - + gtsam::Pose3 expmap(gtsam::Vector v); gtsam::Pose3 expmap(gtsam::Vector v, Eigen::Ref H1, Eigen::Ref H2); gtsam::Vector logmap(const gtsam::Pose3& p); gtsam::Vector logmap(const gtsam::Pose3& p, Eigen::Ref H1, Eigen::Ref H2); - + gtsam::Matrix AdjointMap() const; gtsam::Vector Adjoint(gtsam::Vector xi_b) const; gtsam::Vector Adjoint(gtsam::Vector xi_b, Eigen::Ref H_this, @@ -718,7 +718,7 @@ class OrientedPlane3 { static size_t Dim(); size_t dim() const; - + gtsam::OrientedPlane3 retract(const gtsam::Vector3& v) const; gtsam::OrientedPlane3 retract(const gtsam::Vector3& v, Eigen::Ref H) const; @@ -1491,7 +1491,7 @@ gtsam::TriangulationResult triangulateSafe( const gtsam::Point2Vector& measurements, const gtsam::TriangulationParameters& params); -// Cal3Unified versions +// Cal3Unified versions gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, gtsam::Cal3Unified* sharedCal, const gtsam::Point2Vector& measurements, @@ -1514,7 +1514,17 @@ gtsam::TriangulationResult triangulateSafe( const gtsam::Point2Vector& measurements, const gtsam::TriangulationParameters& params); - +#include +class Event { + Event(); + Event(double t, const gtsam::Point3& p); + Event(double t, double x, double y, double z); + double time() const; + gtsam::Point3 location() const; + TimeOfArrival(); + TimeOfArrival(double speed); + double measure(const gtsam::Event& event, const gtsam::Point3& sensor) const; +}; #include template diff --git a/gtsam_unstable/examples/TimeOfArrivalExample.cpp b/gtsam_unstable/examples/TimeOfArrivalExample.cpp index a97aa27be..79dda3c3d 100644 --- a/gtsam_unstable/examples/TimeOfArrivalExample.cpp +++ b/gtsam_unstable/examples/TimeOfArrivalExample.cpp @@ -21,7 +21,7 @@ #include #include #include -#include +#include #include #include diff --git a/gtsam_unstable/gtsam_unstable.i b/gtsam_unstable/gtsam_unstable.i index 5a791e9ed..faf834b07 100644 --- a/gtsam_unstable/gtsam_unstable.i +++ b/gtsam_unstable/gtsam_unstable.i @@ -162,7 +162,7 @@ class BearingS2 { void serializable() const; // enabling serialization functionality }; - + #include class SimWall2D { SimWall2D(); @@ -379,22 +379,11 @@ virtual class RangeFactor : gtsam::NoiseModelFactor { typedef gtsam::RangeFactor RangeFactorRTV; -#include -class Event { - Event(); - Event(double t, const gtsam::Point3& p); - Event(double t, double x, double y, double z); - double time() const; - gtsam::Point3 location() const; double height() const; void print(string s) const; }; class TimeOfArrival { - TimeOfArrival(); - TimeOfArrival(double speed); - double measure(const gtsam::Event& event, const gtsam::Point3& sensor) const; -}; #include virtual class TOAFactor : gtsam::NonlinearFactor { diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h index caf6b7234..e6b8dbb01 100644 --- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h @@ -11,11 +11,11 @@ #pragma once -#ifdef _MSC_VER -#pragma message("IncrementalFixedLagSmoother was moved to the gtsam/nonlinear directory") -#else -#warning "IncrementalFixedLagSmoother was moved to the gtsam/nonlinear directory" -#endif +// #ifdef _MSC_VER +// #pragma message("IncrementalFixedLagSmoother was moved to the gtsam/nonlinear directory") +// #else +// #warning "IncrementalFixedLagSmoother was moved to the gtsam/nonlinear directory" +// #endif -#include \ No newline at end of file +#include diff --git a/gtsam_unstable/slam/TOAFactor.h b/gtsam_unstable/slam/TOAFactor.h index 784bb03ca..e041f9d97 100644 --- a/gtsam_unstable/slam/TOAFactor.h +++ b/gtsam_unstable/slam/TOAFactor.h @@ -20,7 +20,7 @@ #pragma once #include -#include +#include namespace gtsam { diff --git a/gtsam_unstable/slam/tests/testTOAFactor.cpp b/gtsam_unstable/slam/tests/testTOAFactor.cpp index b6628c138..22b403342 100644 --- a/gtsam_unstable/slam/tests/testTOAFactor.cpp +++ b/gtsam_unstable/slam/tests/testTOAFactor.cpp @@ -21,7 +21,7 @@ #include #include #include -#include +#include #include #include diff --git a/python/gtsam_unstable/examples/TimeOfArrivalExample.py b/python/gtsam_unstable/examples/TimeOfArrivalExample.py index 59f008a05..a3f60a557 100644 --- a/python/gtsam_unstable/examples/TimeOfArrivalExample.py +++ b/python/gtsam_unstable/examples/TimeOfArrivalExample.py @@ -12,8 +12,8 @@ Author: Frank Dellaert # pylint: disable=invalid-name, no-name-in-module from gtsam import (LevenbergMarquardtOptimizer, LevenbergMarquardtParams, - NonlinearFactorGraph, Point3, Values, noiseModel) -from gtsam_unstable import Event, TimeOfArrival, TOAFactor + NonlinearFactorGraph, Point3, Values, noiseModel, Event) +from gtsam_unstable import TimeOfArrival, TOAFactor # units MS = 1e-3