diff --git a/.github/workflows/build-macos.yml b/.github/workflows/build-macos.yml index 462723222..7b7646328 100644 --- a/.github/workflows/build-macos.yml +++ b/.github/workflows/build-macos.yml @@ -19,16 +19,16 @@ jobs: # 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: [ - macOS-10.15-xcode-11.3.1, + macos-11-xcode-13.4.1, ] build_type: [Debug, Release] build_unstable: [ON] include: - - name: macOS-10.15-xcode-11.3.1 - os: macOS-10.15 + - name: macos-11-xcode-13.4.1 + os: macos-11 compiler: xcode - version: "11.3.1" + version: "13.4.1" steps: - name: Checkout @@ -43,7 +43,7 @@ jobs: echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV else - sudo xcode-select -switch /Applications/Xcode_${{ matrix.version }}.app + sudo xcode-select -switch /Applications/Xcode.app echo "CC=clang" >> $GITHUB_ENV echo "CXX=clang++" >> $GITHUB_ENV fi diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index 3fc2d662f..f80b9362c 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -22,7 +22,7 @@ jobs: ubuntu-18.04-gcc-5, ubuntu-18.04-gcc-9, ubuntu-18.04-clang-9, - macOS-10.15-xcode-11.3.1, + macOS-11-xcode-13.4.1, ubuntu-18.04-gcc-5-tbb, ] @@ -52,10 +52,10 @@ jobs: build_type: Debug python_version: "3" - - name: macOS-10.15-xcode-11.3.1 - os: macOS-10.15 + - name: macOS-11-xcode-13.4.1 + os: macOS-11 compiler: xcode - version: "11.3.1" + version: "13.4.1" - name: ubuntu-18.04-gcc-5-tbb os: ubuntu-18.04 @@ -103,7 +103,7 @@ jobs: echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV else - sudo xcode-select -switch /Applications/Xcode_${{ matrix.version }}.app + sudo xcode-select -switch /Applications/Xcode.app echo "CC=clang" >> $GITHUB_ENV echo "CXX=clang++" >> $GITHUB_ENV fi @@ -112,6 +112,11 @@ jobs: run: | echo "GTSAM_WITH_TBB=ON" >> $GITHUB_ENV echo "GTSAM Uses TBB" + - name: Set Swap Space + if: runner.os == 'Linux' + uses: pierotofy/set-swap-space@master + with: + swap-size-gb: 6 - name: Build (Linux) if: runner.os == 'Linux' run: | diff --git a/examples/TriangulationLOSTExample.cpp b/examples/TriangulationLOSTExample.cpp new file mode 100644 index 000000000..a862026e0 --- /dev/null +++ b/examples/TriangulationLOSTExample.cpp @@ -0,0 +1,159 @@ +/* ---------------------------------------------------------------------------- + + * 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 TriangulationLOSTExample.cpp + * @author Akshay Krishnan + * @brief This example runs triangulation several times using 3 different + * approaches: LOST, DLT, and DLT with optimization. It reports the covariance + * and the runtime for each approach. + * + * @date 2022-07-10 + */ +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +using namespace std; +using namespace gtsam; + +static std::mt19937 rng(42); + +void PrintCovarianceStats(const Matrix& mat, const std::string& method) { + Matrix centered = mat.rowwise() - mat.colwise().mean(); + Matrix cov = (centered.adjoint() * centered) / double(mat.rows() - 1); + std::cout << method << " covariance: " << std::endl; + std::cout << cov << std::endl; + std::cout << "Trace sqrt: " << sqrt(cov.trace()) << std::endl << std::endl; +} + +void PrintDuration(const std::chrono::nanoseconds dur, double num_samples, + const std::string& method) { + double nanoseconds = dur.count() / num_samples; + std::cout << "Time taken by " << method << ": " << nanoseconds * 1e-3 + << std::endl; +} + +void GetLargeCamerasDataset(CameraSet>* cameras, + std::vector* poses, Point3* point, + Point2Vector* measurements) { + const double minXY = -10, maxXY = 10; + const double minZ = -20, maxZ = 0; + const int nrCameras = 500; + cameras->reserve(nrCameras); + poses->reserve(nrCameras); + measurements->reserve(nrCameras); + *point = Point3(0.0, 0.0, 10.0); + + std::uniform_real_distribution rand_xy(minXY, maxXY); + std::uniform_real_distribution rand_z(minZ, maxZ); + Cal3_S2 identityK; + + for (int i = 0; i < nrCameras; ++i) { + Point3 wti(rand_xy(rng), rand_xy(rng), rand_z(rng)); + Pose3 wTi(Rot3(), wti); + + poses->push_back(wTi); + cameras->emplace_back(wTi, identityK); + measurements->push_back(cameras->back().project(*point)); + } +} + +void GetSmallCamerasDataset(CameraSet>* cameras, + std::vector* poses, Point3* point, + Point2Vector* measurements) { + Pose3 pose1; + Pose3 pose2(Rot3(), Point3(5., 0., -5.)); + Cal3_S2 identityK; + PinholeCamera camera1(pose1, identityK); + PinholeCamera camera2(pose2, identityK); + + *point = Point3(0, 0, 1); + cameras->push_back(camera1); + cameras->push_back(camera2); + *poses = {pose1, pose2}; + *measurements = {camera1.project(*point), camera2.project(*point)}; +} + +Point2Vector AddNoiseToMeasurements(const Point2Vector& measurements, + const double measurementSigma) { + std::normal_distribution normal(0.0, measurementSigma); + + Point2Vector noisyMeasurements; + noisyMeasurements.reserve(measurements.size()); + for (const auto& p : measurements) { + noisyMeasurements.emplace_back(p.x() + normal(rng), p.y() + normal(rng)); + } + return noisyMeasurements; +} + +/* ************************************************************************* */ +int main(int argc, char* argv[]) { + CameraSet> cameras; + std::vector poses; + Point3 landmark; + Point2Vector measurements; + GetLargeCamerasDataset(&cameras, &poses, &landmark, &measurements); + // GetSmallCamerasDataset(&cameras, &poses, &landmark, &measurements); + const double measurementSigma = 1e-2; + SharedNoiseModel measurementNoise = + noiseModel::Isotropic::Sigma(2, measurementSigma); + + const long int nrTrials = 1000; + Matrix errorsDLT = Matrix::Zero(nrTrials, 3); + Matrix errorsLOST = Matrix::Zero(nrTrials, 3); + Matrix errorsDLTOpt = Matrix::Zero(nrTrials, 3); + + double rank_tol = 1e-9; + boost::shared_ptr calib = boost::make_shared(); + std::chrono::nanoseconds durationDLT; + std::chrono::nanoseconds durationDLTOpt; + std::chrono::nanoseconds durationLOST; + + for (int i = 0; i < nrTrials; i++) { + Point2Vector noisyMeasurements = + AddNoiseToMeasurements(measurements, measurementSigma); + + auto lostStart = std::chrono::high_resolution_clock::now(); + boost::optional estimateLOST = triangulatePoint3( + cameras, noisyMeasurements, rank_tol, false, measurementNoise, true); + durationLOST += std::chrono::high_resolution_clock::now() - lostStart; + + auto dltStart = std::chrono::high_resolution_clock::now(); + boost::optional estimateDLT = triangulatePoint3( + cameras, noisyMeasurements, rank_tol, false, measurementNoise, false); + durationDLT += std::chrono::high_resolution_clock::now() - dltStart; + + auto dltOptStart = std::chrono::high_resolution_clock::now(); + boost::optional estimateDLTOpt = triangulatePoint3( + cameras, noisyMeasurements, rank_tol, true, measurementNoise, false); + durationDLTOpt += std::chrono::high_resolution_clock::now() - dltOptStart; + + errorsLOST.row(i) = *estimateLOST - landmark; + errorsDLT.row(i) = *estimateDLT - landmark; + errorsDLTOpt.row(i) = *estimateDLTOpt - landmark; + } + PrintCovarianceStats(errorsLOST, "LOST"); + PrintCovarianceStats(errorsDLT, "DLT"); + PrintCovarianceStats(errorsDLTOpt, "DLT_OPT"); + + PrintDuration(durationLOST, nrTrials, "LOST"); + PrintDuration(durationDLT, nrTrials, "DLT"); + PrintDuration(durationDLTOpt, nrTrials, "DLT_OPT"); +} diff --git a/gtsam/base/ConcurrentMap.h b/gtsam/base/ConcurrentMap.h index 2d7cbd6db..cf4537d4c 100644 --- a/gtsam/base/ConcurrentMap.h +++ b/gtsam/base/ConcurrentMap.h @@ -113,6 +113,7 @@ private: template void load(Archive& ar, const unsigned int /*version*/) { + this->clear(); // Load into STL container and then fill our map FastVector > map; ar & BOOST_SERIALIZATION_NVP(map); diff --git a/gtsam/base/tests/testSerializationBase.cpp b/gtsam/base/tests/testSerializationBase.cpp index f7aa97b31..d2f4d5f51 100644 --- a/gtsam/base/tests/testSerializationBase.cpp +++ b/gtsam/base/tests/testSerializationBase.cpp @@ -18,6 +18,7 @@ #include +#include #include #include #include @@ -106,6 +107,39 @@ TEST (Serialization, matrix_vector) { EXPECT(equalityBinary((Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished())); } +/* ************************************************************************* */ +TEST (Serialization, ConcurrentMap) { + + ConcurrentMap map; + + map.insert(make_pair(1, "apple")); + map.insert(make_pair(2, "banana")); + + std::string binaryPath = "saved_map.dat"; + try { + std::ofstream outputStream(binaryPath); + boost::archive::binary_oarchive outputArchive(outputStream); + outputArchive << map; + } catch(...) { + EXPECT(false); + } + + // Verify that the existing map contents are replaced by the archive data + ConcurrentMap mapFromDisk; + mapFromDisk.insert(make_pair(3, "clam")); + EXPECT(mapFromDisk.exists(3)); + try { + std::ifstream ifs(binaryPath); + boost::archive::binary_iarchive inputArchive(ifs); + inputArchive >> mapFromDisk; + } catch(...) { + EXPECT(false); + } + EXPECT(mapFromDisk.exists(1)); + EXPECT(mapFromDisk.exists(2)); + EXPECT(!mapFromDisk.exists(3)); +} + /* ************************************************************************* */ diff --git a/gtsam/basis/Chebyshev2.h b/gtsam/basis/Chebyshev2.h index e306c93d5..4c3542d56 100644 --- a/gtsam/basis/Chebyshev2.h +++ b/gtsam/basis/Chebyshev2.h @@ -36,8 +36,6 @@ #include #include -#include - namespace gtsam { /** @@ -134,7 +132,7 @@ class GTSAM_EXPORT Chebyshev2 : public Basis { * Create matrix of values at Chebyshev points given vector-valued function. */ template - static Matrix matrix(boost::function(double)> f, + static Matrix matrix(std::function(double)> f, size_t N, double a = -1, double b = 1) { Matrix Xmat(M, N); for (size_t j = 0; j < N; j++) { diff --git a/gtsam/basis/tests/testChebyshev2.cpp b/gtsam/basis/tests/testChebyshev2.cpp index 9090757f4..73339e0f1 100644 --- a/gtsam/basis/tests/testChebyshev2.cpp +++ b/gtsam/basis/tests/testChebyshev2.cpp @@ -118,7 +118,7 @@ TEST(Chebyshev2, InterpolateVector) { EXPECT(assert_equal(expected, fx(X, actualH), 1e-9)); // Check derivative - boost::function)> f = boost::bind( + std::function)> f = boost::bind( &Chebyshev2::VectorEvaluationFunctor<2>::operator(), fx, _1, boost::none); Matrix numericalH = numericalDerivative11, 2 * N>(f, X); diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h index 99f29b8e5..f72743206 100644 --- a/gtsam/discrete/DecisionTree-inl.h +++ b/gtsam/discrete/DecisionTree-inl.h @@ -39,10 +39,10 @@ #include #include -using boost::assign::operator+=; - namespace gtsam { + using boost::assign::operator+=; + /****************************************************************************/ // Node /****************************************************************************/ @@ -291,10 +291,7 @@ namespace gtsam { } os << "\"" << this->id() << "\" -> \"" << branch->id() << "\""; - if (B == 2) { - if (i == 0) os << " [style=dashed]"; - if (i > 1) os << " [style=bold]"; - } + if (B == 2 && i == 0) os << " [style=dashed]"; os << std::endl; branch->dot(os, labelFormatter, valueFormatter, showZero); } diff --git a/gtsam/discrete/DecisionTree.h b/gtsam/discrete/DecisionTree.h index 1f45d320b..dede2a2f0 100644 --- a/gtsam/discrete/DecisionTree.h +++ b/gtsam/discrete/DecisionTree.h @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include #include @@ -220,7 +220,7 @@ namespace gtsam { /// @{ /// Make virtual - virtual ~DecisionTree() {} + virtual ~DecisionTree() = default; /// Check if tree is empty. bool empty() const { return !root_; } diff --git a/gtsam/discrete/DiscreteFactorGraph.h b/gtsam/discrete/DiscreteFactorGraph.h index f962b1802..bfa57e7db 100644 --- a/gtsam/discrete/DiscreteFactorGraph.h +++ b/gtsam/discrete/DiscreteFactorGraph.h @@ -209,6 +209,10 @@ class GTSAM_EXPORT DiscreteFactorGraph /// @} }; // \ DiscreteFactorGraph +std::pair // +EliminateForMPE(const DiscreteFactorGraph& factors, + const Ordering& frontalKeys); + /// traits template <> struct traits : public Testable {}; diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 835c53d7c..13a435c24 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -25,6 +25,7 @@ #include #include #include +#include #include #include @@ -33,6 +34,7 @@ namespace gtsam { /// As of GTSAM 4, in order to make GTSAM more lean, /// it is now possible to just typedef Point3 to Vector3 typedef Vector3 Point3; +typedef std::vector > Point3Vector; // Convenience typedef using Point3Pair = std::pair; diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 2da51a625..2938e90f5 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -489,6 +489,11 @@ boost::optional align(const Point3Pairs &baPointPairs) { } #endif +/* ************************************************************************* */ +Pose3 Pose3::slerp(double t, const Pose3& other, OptionalJacobian<6, 6> Hx, OptionalJacobian<6, 6> Hy) const { + return interpolate(*this, other, t, Hx, Hy); +} + /* ************************************************************************* */ std::ostream &operator<<(std::ostream &os, const Pose3& pose) { // Both Rot3 and Point3 have ostream definitions so we use them. diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index c36047349..33fb55250 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -379,6 +379,14 @@ public: return std::make_pair(0, 2); } + /** + * @brief Spherical Linear interpolation between *this and other + * @param s a value between 0 and 1.5 + * @param other final point of interpolation geodesic on manifold + */ + Pose3 slerp(double t, const Pose3& other, OptionalJacobian<6, 6> Hx = boost::none, + OptionalJacobian<6, 6> Hy = boost::none) const; + /// Output stream operator GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Pose3& p); diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 8f8088a74..6db5e1919 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -228,6 +228,7 @@ double Rot3::yaw(OptionalJacobian<1, 3> H) const { } /* ************************************************************************* */ +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 Vector Rot3::quaternion() const { gtsam::Quaternion q = toQuaternion(); Vector v(4); @@ -237,6 +238,7 @@ Vector Rot3::quaternion() const { v(3) = q.z(); return v; } +#endif /* ************************************************************************* */ pair Rot3::axisAngle() const { @@ -292,8 +294,8 @@ pair RQ(const Matrix3& A, OptionalJacobian<3, 9> H) { (*H)(1, 8) = yHb22 * cx; // Next, calculate the derivate of z. We have - // c20 = a10 * cy + a11 * sx * sy + a12 * cx * sy - // c22 = a11 * cx - a12 * sx + // c10 = a10 * cy + a11 * sx * sy + a12 * cx * sy + // c11 = a11 * cx - a12 * sx const auto c10Hx = (A(1, 1) * cx - A(1, 2) * sx) * sy; const auto c10Hy = A(1, 2) * cx * cy + A(1, 1) * cy * sx - A(1, 0) * sy; Vector9 c10HA = c10Hx * H->row(0) + c10Hy * H->row(1); diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 18bd88b52..01ca7778c 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -515,11 +515,16 @@ class GTSAM_EXPORT Rot3 : public LieGroup { */ gtsam::Quaternion toQuaternion() const; +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 /** * Converts to a generic matrix to allow for use with matlab * In format: w x y z + * @deprecated: use Rot3::toQuaternion() instead. + * If still using this API, remind that in the returned Vector `V`, + * `V.x()` denotes the actual `qw`, `V.y()` denotes 'qx', `V.z()` denotes `qy`, and `V.w()` denotes 'qz'. */ - Vector quaternion() const; + Vector GTSAM_DEPRECATED quaternion() const; +#endif /** * @brief Spherical Linear intERPolation between *this and other diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 8e3c93224..4ebff6c03 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -355,7 +355,7 @@ class Rot3 { double yaw() const; pair axisAngle() const; gtsam::Quaternion toQuaternion() const; - Vector quaternion() const; + // Vector quaternion() const; // @deprecated, see https://github.com/borglab/gtsam/pull/1219 gtsam::Rot3 slerp(double t, const gtsam::Rot3& other) const; // enabling serialization functionality @@ -446,24 +446,43 @@ class Pose3 { // Group static gtsam::Pose3 identity(); gtsam::Pose3 inverse() const; + gtsam::Pose3 inverse(Eigen::Ref H) const; gtsam::Pose3 compose(const gtsam::Pose3& pose) const; + gtsam::Pose3 compose(const gtsam::Pose3& pose, + Eigen::Ref H1, + Eigen::Ref H2) const; gtsam::Pose3 between(const gtsam::Pose3& pose) const; + gtsam::Pose3 between(const gtsam::Pose3& pose, + Eigen::Ref H1, + Eigen::Ref H2) const; + gtsam::Pose3 slerp(double t, const gtsam::Pose3& pose) const; + gtsam::Pose3 slerp(double t, const gtsam::Pose3& pose, + Eigen::Ref Hx, + Eigen::Ref Hy) const; // Operator Overloads gtsam::Pose3 operator*(const gtsam::Pose3& pose) const; // Manifold gtsam::Pose3 retract(Vector v) const; + gtsam::Pose3 retract(Vector v, Eigen::Ref Hxi) const; Vector localCoordinates(const gtsam::Pose3& pose) const; + Vector localCoordinates(const gtsam::Pose3& pose, Eigen::Ref Hxi) const; // Lie Group static gtsam::Pose3 Expmap(Vector v); + static gtsam::Pose3 Expmap(Vector v, Eigen::Ref Hxi); static Vector Logmap(const gtsam::Pose3& pose); + static Vector Logmap(const gtsam::Pose3& pose, Eigen::Ref Hpose); gtsam::Pose3 expmap(Vector v); Vector logmap(const gtsam::Pose3& pose); Matrix AdjointMap() const; Vector Adjoint(Vector xi) const; + Vector Adjoint(Vector xi, Eigen::Ref H_this, + Eigen::Ref H_xib) const; Vector AdjointTranspose(Vector xi) const; + Vector AdjointTranspose(Vector xi, Eigen::Ref H_this, + Eigen::Ref H_x) const; static Matrix adjointMap(Vector xi); static Vector adjoint(Vector xi, Vector y); static Matrix adjointMap_(Vector xi); @@ -476,7 +495,11 @@ class Pose3 { // Group Action on Point3 gtsam::Point3 transformFrom(const gtsam::Point3& point) const; + gtsam::Point3 transformFrom(const gtsam::Point3& point, Eigen::Ref Hself, + Eigen::Ref Hpoint) const; gtsam::Point3 transformTo(const gtsam::Point3& point) const; + gtsam::Point3 transformTo(const gtsam::Point3& point, Eigen::Ref Hself, + Eigen::Ref Hpoint) const; // Matrix versions Matrix transformFrom(const Matrix& points) const; @@ -484,15 +507,25 @@ class Pose3 { // Standard Interface gtsam::Rot3 rotation() const; + gtsam::Rot3 rotation(Eigen::Ref Hself) const; gtsam::Point3 translation() const; + gtsam::Point3 translation(Eigen::Ref Hself) const; double x() const; double y() const; double z() const; Matrix matrix() const; gtsam::Pose3 transformPoseFrom(const gtsam::Pose3& pose) const; + gtsam::Pose3 transformPoseFrom(const gtsam::Pose3& pose, Eigen::Ref Hself, + Eigen::Ref HaTb) const; gtsam::Pose3 transformPoseTo(const gtsam::Pose3& pose) const; + gtsam::Pose3 transformPoseTo(const gtsam::Pose3& pose, Eigen::Ref Hself, + Eigen::Ref HwTb) const; double range(const gtsam::Point3& point); + double range(const gtsam::Point3& point, Eigen::Ref Hself, + Eigen::Ref Hpoint); double range(const gtsam::Pose3& pose); + double range(const gtsam::Pose3& pose, Eigen::Ref Hself, + Eigen::Ref Hpose); // enabling serialization functionality void serialize() const; @@ -873,7 +906,7 @@ template class PinholeCamera { // Standard Constructors and Named Constructors PinholeCamera(); - PinholeCamera(const gtsam::PinholeCamera other); + PinholeCamera(const This other); PinholeCamera(const gtsam::Pose3& pose); PinholeCamera(const gtsam::Pose3& pose, const CALIBRATION& K); static This Level(const CALIBRATION& K, const gtsam::Pose2& pose, @@ -942,7 +975,7 @@ template class PinholePose { // Standard Constructors and Named Constructors PinholePose(); - PinholePose(const gtsam::PinholePose other); + PinholePose(const This other); PinholePose(const gtsam::Pose3& pose); PinholePose(const gtsam::Pose3& pose, const CALIBRATION* K); static This Level(const gtsam::Pose2& pose, double height); @@ -1129,7 +1162,8 @@ gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3_S2& cameras, const gtsam::Point2Vector& measurements, double rank_tol, bool optimize, - const gtsam::SharedNoiseModel& model = nullptr); + const gtsam::SharedNoiseModel& model = nullptr, + const bool useLOST = false); gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, gtsam::Cal3_S2* sharedCal, const gtsam::Point2Vector& measurements, @@ -1151,7 +1185,8 @@ gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3DS2& cameras, const gtsam::Point2Vector& measurements, double rank_tol, bool optimize, - const gtsam::SharedNoiseModel& model = nullptr); + const gtsam::SharedNoiseModel& model = nullptr, + const bool useLOST = false); gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, gtsam::Cal3DS2* sharedCal, const gtsam::Point2Vector& measurements, @@ -1173,7 +1208,8 @@ gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Bundler& cameras, const gtsam::Point2Vector& measurements, double rank_tol, bool optimize, - const gtsam::SharedNoiseModel& model = nullptr); + const gtsam::SharedNoiseModel& model = nullptr, + const bool useLOST = false); gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, gtsam::Cal3Bundler* sharedCal, const gtsam::Point2Vector& measurements, @@ -1195,7 +1231,8 @@ gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Fisheye& cameras, const gtsam::Point2Vector& measurements, double rank_tol, bool optimize, - const gtsam::SharedNoiseModel& model = nullptr); + const gtsam::SharedNoiseModel& model = nullptr, + const bool useLOST = false); gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, gtsam::Cal3Fisheye* sharedCal, const gtsam::Point2Vector& measurements, @@ -1217,7 +1254,8 @@ gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Unified& cameras, const gtsam::Point2Vector& measurements, double rank_tol, bool optimize, - const gtsam::SharedNoiseModel& model = nullptr); + const gtsam::SharedNoiseModel& model = nullptr, + const bool useLOST = false); gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, gtsam::Cal3Unified* sharedCal, const gtsam::Point2Vector& measurements, @@ -1246,5 +1284,11 @@ class BearingRange { typedef gtsam::BearingRange BearingRange2D; +typedef gtsam::BearingRange + BearingRangePose2; +typedef gtsam::BearingRange + BearingRange3D; +typedef gtsam::BearingRange + BearingRangePose3; } // namespace gtsam diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index 315391ac8..e373e1d52 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -19,8 +19,6 @@ #include #include -#include - using namespace std::placeholders; using namespace gtsam; diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index fb66fb6a2..bc843ad75 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -15,6 +15,7 @@ * @date July 30th, 2013 * @author Chris Beall (cbeall3) * @author Luca Carlone + * @author Akshay Krishnan */ #include @@ -38,24 +39,24 @@ using namespace boost::assign; // Some common constants -static const boost::shared_ptr sharedCal = // +static const boost::shared_ptr kSharedCal = // boost::make_shared(1500, 1200, 0.1, 640, 480); // Looking along X-axis, 1 meter above ground plane (x-y) static const Rot3 upright = Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2); -static const Pose3 pose1 = Pose3(upright, gtsam::Point3(0, 0, 1)); -PinholeCamera camera1(pose1, *sharedCal); +static const Pose3 kPose1 = Pose3(upright, gtsam::Point3(0, 0, 1)); +static const PinholeCamera kCamera1(kPose1, *kSharedCal); // create second camera 1 meter to the right of first camera -static const Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); -PinholeCamera camera2(pose2, *sharedCal); +static const Pose3 kPose2 = kPose1 * Pose3(Rot3(), Point3(1, 0, 0)); +static const PinholeCamera kCamera2(kPose2, *kSharedCal); // landmark ~5 meters infront of camera -static const Point3 landmark(5, 0.5, 1.2); +static const Point3 kLandmark(5, 0.5, 1.2); // 1. Project two landmarks into two cameras and triangulate -Point2 z1 = camera1.project(landmark); -Point2 z2 = camera2.project(landmark); +static const Point2 kZ1 = kCamera1.project(kLandmark); +static const Point2 kZ2 = kCamera2.project(kLandmark); //****************************************************************************** // Simple test with a well-behaved two camera situation @@ -63,22 +64,22 @@ TEST(triangulation, twoPoses) { vector poses; Point2Vector measurements; - poses += pose1, pose2; - measurements += z1, z2; + poses += kPose1, kPose2; + measurements += kZ1, kZ2; double rank_tol = 1e-9; // 1. Test simple DLT, perfect in no noise situation bool optimize = false; boost::optional actual1 = // - triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize); - EXPECT(assert_equal(landmark, *actual1, 1e-7)); + triangulatePoint3(poses, kSharedCal, measurements, rank_tol, optimize); + EXPECT(assert_equal(kLandmark, *actual1, 1e-7)); // 2. test with optimization on, same answer optimize = true; boost::optional actual2 = // - triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize); - EXPECT(assert_equal(landmark, *actual2, 1e-7)); + triangulatePoint3(poses, kSharedCal, measurements, rank_tol, optimize); + EXPECT(assert_equal(kLandmark, *actual2, 1e-7)); // 3. Add some noise and try again: result should be ~ (4.995, // 0.499167, 1.19814) @@ -86,16 +87,79 @@ TEST(triangulation, twoPoses) { measurements.at(1) += Point2(-0.2, 0.3); optimize = false; boost::optional actual3 = // - triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize); + triangulatePoint3(poses, kSharedCal, measurements, rank_tol, optimize); EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual3, 1e-4)); // 4. Now with optimization on optimize = true; boost::optional actual4 = // - triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize); + triangulatePoint3(poses, kSharedCal, measurements, rank_tol, optimize); EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-4)); } +TEST(triangulation, twoCamerasUsingLOST) { + CameraSet> cameras; + cameras.push_back(kCamera1); + cameras.push_back(kCamera2); + + Point2Vector measurements = {kZ1, kZ2}; + SharedNoiseModel measurementNoise = noiseModel::Isotropic::Sigma(2, 1e-4); + double rank_tol = 1e-9; + + // 1. Test simple triangulation, perfect in no noise situation + boost::optional actual1 = // + triangulatePoint3>(cameras, measurements, rank_tol, + /*optimize=*/false, + measurementNoise, + /*useLOST=*/true); + EXPECT(assert_equal(kLandmark, *actual1, 1e-12)); + + // 3. Add some noise and try again: result should be ~ (4.995, + // 0.499167, 1.19814) + measurements[0] += Point2(0.1, 0.5); + measurements[1] += Point2(-0.2, 0.3); + boost::optional actual2 = // + triangulatePoint3>( + cameras, measurements, rank_tol, + /*optimize=*/false, measurementNoise, + /*use_lost_triangulation=*/true); + EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual2, 1e-4)); +} + +TEST(triangulation, twoCamerasLOSTvsDLT) { + // LOST has been shown to preform better when the point is much closer to one + // camera compared to another. This unit test checks this configuration. + const Cal3_S2 identityK; + const Pose3 pose1; + const Pose3 pose2(Rot3(), Point3(5., 0., -5.)); + CameraSet> cameras; + cameras.emplace_back(pose1, identityK); + cameras.emplace_back(pose2, identityK); + + Point3 landmark(0, 0, 1); + Point2 x1_noisy = cameras[0].project(landmark) + Point2(0.00817, 0.00977); + Point2 x2_noisy = cameras[1].project(landmark) + Point2(-0.00610, 0.01969); + Point2Vector measurements = {x1_noisy, x2_noisy}; + + const double rank_tol = 1e-9; + SharedNoiseModel measurementNoise = noiseModel::Isotropic::Sigma(2, 1e-2); + + boost::optional estimateLOST = // + triangulatePoint3>(cameras, measurements, rank_tol, + /*optimize=*/false, + measurementNoise, + /*useLOST=*/true); + + // These values are from a MATLAB implementation. + EXPECT(assert_equal(Point3(0.007, 0.011, 0.945), *estimateLOST, 1e-3)); + + boost::optional estimateDLT = + triangulatePoint3(cameras, measurements, rank_tol, false); + + // The LOST estimate should have a smaller error. + EXPECT((landmark - *estimateLOST).norm() <= (landmark - *estimateDLT).norm()); +} + //****************************************************************************** // Simple test with a well-behaved two camera situation with Cal3DS2 calibration. TEST(triangulation, twoPosesCal3DS2) { @@ -103,18 +167,18 @@ TEST(triangulation, twoPosesCal3DS2) { boost::make_shared(1500, 1200, 0, 640, 480, -.3, 0.1, 0.0001, -0.0003); - PinholeCamera camera1Distorted(pose1, *sharedDistortedCal); + PinholeCamera camera1Distorted(kPose1, *sharedDistortedCal); - PinholeCamera camera2Distorted(pose2, *sharedDistortedCal); + PinholeCamera camera2Distorted(kPose2, *sharedDistortedCal); // 0. Project two landmarks into two cameras and triangulate - Point2 z1Distorted = camera1Distorted.project(landmark); - Point2 z2Distorted = camera2Distorted.project(landmark); + Point2 z1Distorted = camera1Distorted.project(kLandmark); + Point2 z2Distorted = camera2Distorted.project(kLandmark); vector poses; Point2Vector measurements; - poses += pose1, pose2; + poses += kPose1, kPose2; measurements += z1Distorted, z2Distorted; double rank_tol = 1e-9; @@ -124,14 +188,14 @@ TEST(triangulation, twoPosesCal3DS2) { boost::optional actual1 = // triangulatePoint3(poses, sharedDistortedCal, measurements, rank_tol, optimize); - EXPECT(assert_equal(landmark, *actual1, 1e-7)); + EXPECT(assert_equal(kLandmark, *actual1, 1e-7)); // 2. test with optimization on, same answer optimize = true; boost::optional actual2 = // triangulatePoint3(poses, sharedDistortedCal, measurements, rank_tol, optimize); - EXPECT(assert_equal(landmark, *actual2, 1e-7)); + EXPECT(assert_equal(kLandmark, *actual2, 1e-7)); // 3. Add some noise and try again: result should be ~ (4.995, // 0.499167, 1.19814) @@ -160,18 +224,18 @@ TEST(triangulation, twoPosesFisheye) { boost::make_shared(1500, 1200, .1, 640, 480, -.3, 0.1, 0.0001, -0.0003); - PinholeCamera camera1Distorted(pose1, *sharedDistortedCal); + PinholeCamera camera1Distorted(kPose1, *sharedDistortedCal); - PinholeCamera camera2Distorted(pose2, *sharedDistortedCal); + PinholeCamera camera2Distorted(kPose2, *sharedDistortedCal); // 0. Project two landmarks into two cameras and triangulate - Point2 z1Distorted = camera1Distorted.project(landmark); - Point2 z2Distorted = camera2Distorted.project(landmark); + Point2 z1Distorted = camera1Distorted.project(kLandmark); + Point2 z2Distorted = camera2Distorted.project(kLandmark); vector poses; Point2Vector measurements; - poses += pose1, pose2; + poses += kPose1, kPose2; measurements += z1Distorted, z2Distorted; double rank_tol = 1e-9; @@ -181,14 +245,14 @@ TEST(triangulation, twoPosesFisheye) { boost::optional actual1 = // triangulatePoint3(poses, sharedDistortedCal, measurements, rank_tol, optimize); - EXPECT(assert_equal(landmark, *actual1, 1e-7)); + EXPECT(assert_equal(kLandmark, *actual1, 1e-7)); // 2. test with optimization on, same answer optimize = true; boost::optional actual2 = // triangulatePoint3(poses, sharedDistortedCal, measurements, rank_tol, optimize); - EXPECT(assert_equal(landmark, *actual2, 1e-7)); + EXPECT(assert_equal(kLandmark, *actual2, 1e-7)); // 3. Add some noise and try again: result should be ~ (4.995, // 0.499167, 1.19814) @@ -213,17 +277,17 @@ TEST(triangulation, twoPosesFisheye) { TEST(triangulation, twoPosesBundler) { boost::shared_ptr bundlerCal = // boost::make_shared(1500, 0.1, 0.2, 640, 480); - PinholeCamera camera1(pose1, *bundlerCal); - PinholeCamera camera2(pose2, *bundlerCal); + PinholeCamera camera1(kPose1, *bundlerCal); + PinholeCamera camera2(kPose2, *bundlerCal); // 1. Project two landmarks into two cameras and triangulate - Point2 z1 = camera1.project(landmark); - Point2 z2 = camera2.project(landmark); + Point2 z1 = camera1.project(kLandmark); + Point2 z2 = camera2.project(kLandmark); vector poses; Point2Vector measurements; - poses += pose1, pose2; + poses += kPose1, kPose2; measurements += z1, z2; bool optimize = true; @@ -232,7 +296,7 @@ TEST(triangulation, twoPosesBundler) { boost::optional actual = // triangulatePoint3(poses, bundlerCal, measurements, rank_tol, optimize); - EXPECT(assert_equal(landmark, *actual, 1e-7)); + EXPECT(assert_equal(kLandmark, *actual, 1e-7)); // Add some noise and try again measurements.at(0) += Point2(0.1, 0.5); @@ -249,12 +313,12 @@ TEST(triangulation, fourPoses) { vector poses; Point2Vector measurements; - poses += pose1, pose2; - measurements += z1, z2; + poses += kPose1, kPose2; + measurements += kZ1, kZ2; boost::optional actual = - triangulatePoint3(poses, sharedCal, measurements); - EXPECT(assert_equal(landmark, *actual, 1e-2)); + triangulatePoint3(poses, kSharedCal, measurements); + EXPECT(assert_equal(kLandmark, *actual, 1e-2)); // 2. Add some noise and try again: result should be ~ (4.995, // 0.499167, 1.19814) @@ -262,37 +326,37 @@ TEST(triangulation, fourPoses) { measurements.at(1) += Point2(-0.2, 0.3); boost::optional actual2 = // - triangulatePoint3(poses, sharedCal, measurements); - EXPECT(assert_equal(landmark, *actual2, 1e-2)); + triangulatePoint3(poses, kSharedCal, measurements); + EXPECT(assert_equal(kLandmark, *actual2, 1e-2)); // 3. Add a slightly rotated third camera above, again with measurement noise - Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); - PinholeCamera camera3(pose3, *sharedCal); - Point2 z3 = camera3.project(landmark); + Pose3 pose3 = kPose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); + PinholeCamera camera3(pose3, *kSharedCal); + Point2 z3 = camera3.project(kLandmark); poses += pose3; measurements += z3 + Point2(0.1, -0.1); boost::optional triangulated_3cameras = // - triangulatePoint3(poses, sharedCal, measurements); - EXPECT(assert_equal(landmark, *triangulated_3cameras, 1e-2)); + triangulatePoint3(poses, kSharedCal, measurements); + EXPECT(assert_equal(kLandmark, *triangulated_3cameras, 1e-2)); // Again with nonlinear optimization boost::optional triangulated_3cameras_opt = - triangulatePoint3(poses, sharedCal, measurements, 1e-9, true); - EXPECT(assert_equal(landmark, *triangulated_3cameras_opt, 1e-2)); + triangulatePoint3(poses, kSharedCal, measurements, 1e-9, true); + EXPECT(assert_equal(kLandmark, *triangulated_3cameras_opt, 1e-2)); // 4. Test failure: Add a 4th camera facing the wrong way Pose3 pose4 = Pose3(Rot3::Ypr(M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); - PinholeCamera camera4(pose4, *sharedCal); + PinholeCamera camera4(pose4, *kSharedCal); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION - CHECK_EXCEPTION(camera4.project(landmark), CheiralityException); + CHECK_EXCEPTION(camera4.project(kLandmark), CheiralityException); poses += pose4; measurements += Point2(400, 400); - CHECK_EXCEPTION(triangulatePoint3(poses, sharedCal, measurements), + CHECK_EXCEPTION(triangulatePoint3(poses, kSharedCal, measurements), TriangulationCheiralityException); #endif } @@ -300,33 +364,33 @@ TEST(triangulation, fourPoses) { //****************************************************************************** TEST(triangulation, threePoses_robustNoiseModel) { - Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); - PinholeCamera camera3(pose3, *sharedCal); - Point2 z3 = camera3.project(landmark); + Pose3 pose3 = kPose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); + PinholeCamera camera3(pose3, *kSharedCal); + Point2 z3 = camera3.project(kLandmark); vector poses; Point2Vector measurements; - poses += pose1, pose2, pose3; - measurements += z1, z2, z3; + poses += kPose1, kPose2, pose3; + measurements += kZ1, kZ2, z3; // noise free, so should give exactly the landmark boost::optional actual = - triangulatePoint3(poses, sharedCal, measurements); - EXPECT(assert_equal(landmark, *actual, 1e-2)); + triangulatePoint3(poses, kSharedCal, measurements); + EXPECT(assert_equal(kLandmark, *actual, 1e-2)); // Add outlier measurements.at(0) += Point2(100, 120); // very large pixel noise! // now estimate does not match landmark boost::optional actual2 = // - triangulatePoint3(poses, sharedCal, measurements); + triangulatePoint3(poses, kSharedCal, measurements); // DLT is surprisingly robust, but still off (actual error is around 0.26m): - EXPECT( (landmark - *actual2).norm() >= 0.2); - EXPECT( (landmark - *actual2).norm() <= 0.5); + EXPECT( (kLandmark - *actual2).norm() >= 0.2); + EXPECT( (kLandmark - *actual2).norm() <= 0.5); // Again with nonlinear optimization boost::optional actual3 = - triangulatePoint3(poses, sharedCal, measurements, 1e-9, true); + triangulatePoint3(poses, kSharedCal, measurements, 1e-9, true); // result from nonlinear (but non-robust optimization) is close to DLT and still off EXPECT(assert_equal(*actual2, *actual3, 0.1)); @@ -334,27 +398,27 @@ TEST(triangulation, threePoses_robustNoiseModel) { auto model = noiseModel::Robust::Create( noiseModel::mEstimator::Huber::Create(1.345), noiseModel::Unit::Create(2)); boost::optional actual4 = triangulatePoint3( - poses, sharedCal, measurements, 1e-9, true, model); + poses, kSharedCal, measurements, 1e-9, true, model); // using the Huber loss we now have a quite small error!! nice! - EXPECT(assert_equal(landmark, *actual4, 0.05)); + EXPECT(assert_equal(kLandmark, *actual4, 0.05)); } //****************************************************************************** TEST(triangulation, fourPoses_robustNoiseModel) { - Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); - PinholeCamera camera3(pose3, *sharedCal); - Point2 z3 = camera3.project(landmark); + Pose3 pose3 = kPose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); + PinholeCamera camera3(pose3, *kSharedCal); + Point2 z3 = camera3.project(kLandmark); vector poses; Point2Vector measurements; - poses += pose1, pose1, pose2, pose3; // 2 measurements from pose 1 - measurements += z1, z1, z2, z3; + poses += kPose1, kPose1, kPose2, pose3; // 2 measurements from pose 1 + measurements += kZ1, kZ1, kZ2, z3; // noise free, so should give exactly the landmark boost::optional actual = - triangulatePoint3(poses, sharedCal, measurements); - EXPECT(assert_equal(landmark, *actual, 1e-2)); + triangulatePoint3(poses, kSharedCal, measurements); + EXPECT(assert_equal(kLandmark, *actual, 1e-2)); // Add outlier measurements.at(0) += Point2(100, 120); // very large pixel noise! @@ -365,14 +429,14 @@ TEST(triangulation, fourPoses_robustNoiseModel) { // now estimate does not match landmark boost::optional actual2 = // - triangulatePoint3(poses, sharedCal, measurements); + triangulatePoint3(poses, kSharedCal, measurements); // DLT is surprisingly robust, but still off (actual error is around 0.17m): - EXPECT( (landmark - *actual2).norm() >= 0.1); - EXPECT( (landmark - *actual2).norm() <= 0.5); + EXPECT( (kLandmark - *actual2).norm() >= 0.1); + EXPECT( (kLandmark - *actual2).norm() <= 0.5); // Again with nonlinear optimization boost::optional actual3 = - triangulatePoint3(poses, sharedCal, measurements, 1e-9, true); + triangulatePoint3(poses, kSharedCal, measurements, 1e-9, true); // result from nonlinear (but non-robust optimization) is close to DLT and still off EXPECT(assert_equal(*actual2, *actual3, 0.1)); @@ -380,24 +444,24 @@ TEST(triangulation, fourPoses_robustNoiseModel) { auto model = noiseModel::Robust::Create( noiseModel::mEstimator::Huber::Create(1.345), noiseModel::Unit::Create(2)); boost::optional actual4 = triangulatePoint3( - poses, sharedCal, measurements, 1e-9, true, model); + poses, kSharedCal, measurements, 1e-9, true, model); // using the Huber loss we now have a quite small error!! nice! - EXPECT(assert_equal(landmark, *actual4, 0.05)); + EXPECT(assert_equal(kLandmark, *actual4, 0.05)); } //****************************************************************************** TEST(triangulation, fourPoses_distinct_Ks) { Cal3_S2 K1(1500, 1200, 0, 640, 480); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - PinholeCamera camera1(pose1, K1); + PinholeCamera camera1(kPose1, K1); // create second camera 1 meter to the right of first camera Cal3_S2 K2(1600, 1300, 0, 650, 440); - PinholeCamera camera2(pose2, K2); + PinholeCamera camera2(kPose2, K2); // 1. Project two landmarks into two cameras and triangulate - Point2 z1 = camera1.project(landmark); - Point2 z2 = camera2.project(landmark); + Point2 z1 = camera1.project(kLandmark); + Point2 z2 = camera2.project(kLandmark); CameraSet> cameras; Point2Vector measurements; @@ -407,7 +471,7 @@ TEST(triangulation, fourPoses_distinct_Ks) { boost::optional actual = // triangulatePoint3(cameras, measurements); - EXPECT(assert_equal(landmark, *actual, 1e-2)); + EXPECT(assert_equal(kLandmark, *actual, 1e-2)); // 2. Add some noise and try again: result should be ~ (4.995, // 0.499167, 1.19814) @@ -416,25 +480,25 @@ TEST(triangulation, fourPoses_distinct_Ks) { boost::optional actual2 = // triangulatePoint3(cameras, measurements); - EXPECT(assert_equal(landmark, *actual2, 1e-2)); + EXPECT(assert_equal(kLandmark, *actual2, 1e-2)); // 3. Add a slightly rotated third camera above, again with measurement noise - Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); + Pose3 pose3 = kPose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); Cal3_S2 K3(700, 500, 0, 640, 480); PinholeCamera camera3(pose3, K3); - Point2 z3 = camera3.project(landmark); + Point2 z3 = camera3.project(kLandmark); cameras += camera3; measurements += z3 + Point2(0.1, -0.1); boost::optional triangulated_3cameras = // triangulatePoint3(cameras, measurements); - EXPECT(assert_equal(landmark, *triangulated_3cameras, 1e-2)); + EXPECT(assert_equal(kLandmark, *triangulated_3cameras, 1e-2)); // Again with nonlinear optimization boost::optional triangulated_3cameras_opt = triangulatePoint3(cameras, measurements, 1e-9, true); - EXPECT(assert_equal(landmark, *triangulated_3cameras_opt, 1e-2)); + EXPECT(assert_equal(kLandmark, *triangulated_3cameras_opt, 1e-2)); // 4. Test failure: Add a 4th camera facing the wrong way Pose3 pose4 = Pose3(Rot3::Ypr(M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); @@ -442,7 +506,7 @@ TEST(triangulation, fourPoses_distinct_Ks) { PinholeCamera camera4(pose4, K4); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION - CHECK_EXCEPTION(camera4.project(landmark), CheiralityException); + CHECK_EXCEPTION(camera4.project(kLandmark), CheiralityException); cameras += camera4; measurements += Point2(400, 400); @@ -455,15 +519,15 @@ TEST(triangulation, fourPoses_distinct_Ks) { TEST(triangulation, fourPoses_distinct_Ks_distortion) { Cal3DS2 K1(1500, 1200, 0, 640, 480, -.3, 0.1, 0.0001, -0.0003); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - PinholeCamera camera1(pose1, K1); + PinholeCamera camera1(kPose1, K1); // create second camera 1 meter to the right of first camera Cal3DS2 K2(1600, 1300, 0, 650, 440, -.2, 0.05, 0.0002, -0.0001); - PinholeCamera camera2(pose2, K2); + PinholeCamera camera2(kPose2, K2); // 1. Project two landmarks into two cameras and triangulate - Point2 z1 = camera1.project(landmark); - Point2 z2 = camera2.project(landmark); + Point2 z1 = camera1.project(kLandmark); + Point2 z2 = camera2.project(kLandmark); CameraSet> cameras; Point2Vector measurements; @@ -473,22 +537,22 @@ TEST(triangulation, fourPoses_distinct_Ks_distortion) { boost::optional actual = // triangulatePoint3(cameras, measurements); - EXPECT(assert_equal(landmark, *actual, 1e-2)); + EXPECT(assert_equal(kLandmark, *actual, 1e-2)); } //****************************************************************************** TEST(triangulation, outliersAndFarLandmarks) { Cal3_S2 K1(1500, 1200, 0, 640, 480); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - PinholeCamera camera1(pose1, K1); + PinholeCamera camera1(kPose1, K1); // create second camera 1 meter to the right of first camera Cal3_S2 K2(1600, 1300, 0, 650, 440); - PinholeCamera camera2(pose2, K2); + PinholeCamera camera2(kPose2, K2); // 1. Project two landmarks into two cameras and triangulate - Point2 z1 = camera1.project(landmark); - Point2 z2 = camera2.project(landmark); + Point2 z1 = camera1.project(kLandmark); + Point2 z2 = camera2.project(kLandmark); CameraSet> cameras; Point2Vector measurements; @@ -501,7 +565,7 @@ TEST(triangulation, outliersAndFarLandmarks) { 1.0, false, landmarkDistanceThreshold); // all default except // landmarkDistanceThreshold TriangulationResult actual = triangulateSafe(cameras, measurements, params); - EXPECT(assert_equal(landmark, *actual, 1e-2)); + EXPECT(assert_equal(kLandmark, *actual, 1e-2)); EXPECT(actual.valid()); landmarkDistanceThreshold = 4; // landmark is farther than that @@ -513,10 +577,10 @@ TEST(triangulation, outliersAndFarLandmarks) { // 3. Add a slightly rotated third camera above with a wrong measurement // (OUTLIER) - Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); + Pose3 pose3 = kPose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); Cal3_S2 K3(700, 500, 0, 640, 480); PinholeCamera camera3(pose3, K3); - Point2 z3 = camera3.project(landmark); + Point2 z3 = camera3.project(kLandmark); cameras += camera3; measurements += z3 + Point2(10, -10); @@ -539,18 +603,18 @@ TEST(triangulation, outliersAndFarLandmarks) { //****************************************************************************** TEST(triangulation, twoIdenticalPoses) { // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - PinholeCamera camera1(pose1, *sharedCal); + PinholeCamera camera1(kPose1, *kSharedCal); // 1. Project two landmarks into two cameras and triangulate - Point2 z1 = camera1.project(landmark); + Point2 z1 = camera1.project(kLandmark); vector poses; Point2Vector measurements; - poses += pose1, pose1; + poses += kPose1, kPose1; measurements += z1, z1; - CHECK_EXCEPTION(triangulatePoint3(poses, sharedCal, measurements), + CHECK_EXCEPTION(triangulatePoint3(poses, kSharedCal, measurements), TriangulationUnderconstrainedException); } @@ -565,7 +629,7 @@ TEST(triangulation, onePose) { poses += Pose3(); measurements += Point2(0, 0); - CHECK_EXCEPTION(triangulatePoint3(poses, sharedCal, measurements), + CHECK_EXCEPTION(triangulatePoint3(poses, kSharedCal, measurements), TriangulationUnderconstrainedException); } @@ -681,12 +745,12 @@ TEST(triangulation, twoPoses_sphericalCamera) { std::vector measurements; // Project landmark into two cameras and triangulate - SphericalCamera cam1(pose1); - SphericalCamera cam2(pose2); - Unit3 u1 = cam1.project(landmark); - Unit3 u2 = cam2.project(landmark); + SphericalCamera cam1(kPose1); + SphericalCamera cam2(kPose2); + Unit3 u1 = cam1.project(kLandmark); + Unit3 u2 = cam2.project(kLandmark); - poses += pose1, pose2; + poses += kPose1, kPose2; measurements += u1, u2; CameraSet cameras; @@ -698,25 +762,25 @@ TEST(triangulation, twoPoses_sphericalCamera) { // 1. Test linear triangulation via DLT auto projection_matrices = projectionMatricesFromCameras(cameras); Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); - EXPECT(assert_equal(landmark, point, 1e-7)); + EXPECT(assert_equal(kLandmark, point, 1e-7)); // 2. Test nonlinear triangulation point = triangulateNonlinear(cameras, measurements, point); - EXPECT(assert_equal(landmark, point, 1e-7)); + EXPECT(assert_equal(kLandmark, point, 1e-7)); // 3. Test simple DLT, now within triangulatePoint3 bool optimize = false; boost::optional actual1 = // triangulatePoint3(cameras, measurements, rank_tol, optimize); - EXPECT(assert_equal(landmark, *actual1, 1e-7)); + EXPECT(assert_equal(kLandmark, *actual1, 1e-7)); // 4. test with optimization on, same answer optimize = true; boost::optional actual2 = // triangulatePoint3(cameras, measurements, rank_tol, optimize); - EXPECT(assert_equal(landmark, *actual2, 1e-7)); + EXPECT(assert_equal(kLandmark, *actual2, 1e-7)); // 5. Add some noise and try again: result should be ~ (4.995, // 0.499167, 1.19814) @@ -761,7 +825,7 @@ TEST(triangulation, twoPoses_sphericalCamera_extremeFOV) { EXPECT(assert_equal(Unit3(Point3(1.0, 0.0, -1.0)), u2, 1e-7)); // behind and to the right of PoseB - poses += pose1, pose2; + poses += kPose1, kPose2; measurements += u1, u2; CameraSet cameras; diff --git a/gtsam/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp index 026afef24..acb0ddf0e 100644 --- a/gtsam/geometry/triangulation.cpp +++ b/gtsam/geometry/triangulation.cpp @@ -14,6 +14,7 @@ * @brief Functions for triangulation * @date July 31, 2013 * @author Chris Beall + * @author Akshay Krishnan */ #include @@ -24,9 +25,9 @@ namespace gtsam { Vector4 triangulateHomogeneousDLT( - const std::vector>& projection_matrices, + const std::vector>& + projection_matrices, const Point2Vector& measurements, double rank_tol) { - // number of cameras size_t m = projection_matrices.size(); @@ -39,6 +40,11 @@ Vector4 triangulateHomogeneousDLT( const Point2& p = measurements.at(i); // build system of equations + // [A_1; A_2; A_3] x = [b_1; b_2; b_3] + // [b_3 * A_1 - b_1 * A_3] x = 0 + // [b_3 * A_2 - b_2 * A_3] x = 0 + // A' x = 0 + // A' 2x4 = [b_3 * A_1 - b_1 * A_3; b_3 * A_2 - b_2 * A_3] A.row(row) = p.x() * projection.row(2) - projection.row(0); A.row(row + 1) = p.y() * projection.row(2) - projection.row(1); } @@ -47,16 +53,15 @@ Vector4 triangulateHomogeneousDLT( Vector v; boost::tie(rank, error, v) = DLT(A, rank_tol); - if (rank < 3) - throw(TriangulationUnderconstrainedException()); + if (rank < 3) throw(TriangulationUnderconstrainedException()); return v; } Vector4 triangulateHomogeneousDLT( - const std::vector>& projection_matrices, + const std::vector>& + projection_matrices, const std::vector& measurements, double rank_tol) { - // number of cameras size_t m = projection_matrices.size(); @@ -66,7 +71,9 @@ Vector4 triangulateHomogeneousDLT( for (size_t i = 0; i < m; i++) { size_t row = i * 2; const Matrix34& projection = projection_matrices.at(i); - const Point3& p = measurements.at(i).point3(); // to get access to x,y,z of the bearing vector + const Point3& p = + measurements.at(i) + .point3(); // to get access to x,y,z of the bearing vector // build system of equations A.row(row) = p.x() * projection.row(2) - p.z() * projection.row(0); @@ -77,34 +84,67 @@ Vector4 triangulateHomogeneousDLT( Vector v; boost::tie(rank, error, v) = DLT(A, rank_tol); - if (rank < 3) - throw(TriangulationUnderconstrainedException()); + if (rank < 3) throw(TriangulationUnderconstrainedException()); return v; } -Point3 triangulateDLT( - const std::vector>& projection_matrices, - const Point2Vector& measurements, double rank_tol) { +Point3 triangulateLOST(const std::vector& poses, + const Point3Vector& calibratedMeasurements, + const SharedIsotropic& measurementNoise) { + size_t m = calibratedMeasurements.size(); + assert(m == poses.size()); - Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements, - rank_tol); + // Construct the system matrices. + Matrix A = Matrix::Zero(m * 2, 3); + Matrix b = Matrix::Zero(m * 2, 1); + + for (size_t i = 0; i < m; i++) { + const Pose3& wTi = poses[i]; + // TODO(akshay-krishnan): are there better ways to select j? + const int j = (i + 1) % m; + const Pose3& wTj = poses[j]; + + const Point3 d_ij = wTj.translation() - wTi.translation(); + + const Point3 wZi = wTi.rotation().rotate(calibratedMeasurements[i]); + const Point3 wZj = wTj.rotation().rotate(calibratedMeasurements[j]); + + // Note: Setting q_i = 1.0 gives same results as DLT. + const double q_i = wZi.cross(wZj).norm() / + (measurementNoise->sigma() * d_ij.cross(wZj).norm()); + + const Matrix23 coefficientMat = + q_i * skewSymmetric(calibratedMeasurements[i]).topLeftCorner(2, 3) * + wTi.rotation().matrix().transpose(); + + A.block<2, 3>(2 * i, 0) << coefficientMat; + b.block<2, 1>(2 * i, 0) << coefficientMat * wTi.translation(); + } + return A.colPivHouseholderQr().solve(b); +} + +Point3 triangulateDLT( + const std::vector>& + projection_matrices, + const Point2Vector& measurements, double rank_tol) { + Vector4 v = + triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol); // Create 3D point from homogeneous coordinates return Point3(v.head<3>() / v[3]); } Point3 triangulateDLT( - const std::vector>& projection_matrices, + const std::vector>& + projection_matrices, const std::vector& measurements, double rank_tol) { - // contrary to previous triangulateDLT, this is now taking Unit3 inputs - Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements, - rank_tol); - // Create 3D point from homogeneous coordinates - return Point3(v.head<3>() / v[3]); + Vector4 v = + triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol); + // Create 3D point from homogeneous coordinates + return Point3(v.head<3>() / v[3]); } -/// /** * Optimize for triangulation * @param graph nonlinear factors for projection @@ -132,4 +172,4 @@ Point3 optimize(const NonlinearFactorGraph& graph, const Values& values, return result.at(landmarkKey); } -} // \namespace gtsam +} // namespace gtsam diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 401fd2d0b..e0473f8c3 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -15,6 +15,7 @@ * @date July 31, 2013 * @author Chris Beall * @author Luca Carlone + * @author Akshay Krishnan */ #pragma once @@ -94,6 +95,20 @@ GTSAM_EXPORT Point3 triangulateDLT( const std::vector& measurements, double rank_tol = 1e-9); +/** + * @brief Triangulation using the LOST (Linear Optimal Sine Triangulation) + * algorithm proposed in https://arxiv.org/pdf/2205.12197.pdf by Sebastien Henry + * and John Christian. + * @param poses camera poses in world frame + * @param calibratedMeasurements measurements in homogeneous coordinates in each + * camera pose + * @param measurementNoise isotropic noise model for the measurements + * @return triangulated point in world coordinates + */ +GTSAM_EXPORT Point3 triangulateLOST(const std::vector& poses, + const Point3Vector& calibratedMeasurements, + const SharedIsotropic& measurementNoise); + /** * Create a factor graph with projection factors from poses and one calibration * @param poses Camera poses @@ -108,17 +123,16 @@ std::pair triangulationGraph( const std::vector& poses, boost::shared_ptr sharedCal, const Point2Vector& measurements, Key landmarkKey, const Point3& initialEstimate, - const SharedNoiseModel& model = nullptr) { + const SharedNoiseModel& model = noiseModel::Unit::Create(2)) { Values values; values.insert(landmarkKey, initialEstimate); // Initial landmark value NonlinearFactorGraph graph; - static SharedNoiseModel unit2(noiseModel::Unit::Create(2)); for (size_t i = 0; i < measurements.size(); i++) { const Pose3& pose_i = poses[i]; typedef PinholePose Camera; Camera camera_i(pose_i, sharedCal); graph.emplace_shared > // - (camera_i, measurements[i], model? model : unit2, landmarkKey); + (camera_i, measurements[i], model, landmarkKey); } return std::make_pair(graph, values); } @@ -302,10 +316,10 @@ template typename CAMERA::MeasurementVector undistortMeasurements( const CameraSet& cameras, const typename CAMERA::MeasurementVector& measurements) { - const size_t num_meas = cameras.size(); - assert(num_meas == measurements.size()); - typename CAMERA::MeasurementVector undistortedMeasurements(num_meas); - for (size_t ii = 0; ii < num_meas; ++ii) { + const size_t nrMeasurements = measurements.size(); + assert(nrMeasurements == cameras.size()); + typename CAMERA::MeasurementVector undistortedMeasurements(nrMeasurements); + for (size_t ii = 0; ii < nrMeasurements; ++ii) { // Calibrate with cal and uncalibrate with pinhole version of cal so that // measurements are undistorted. undistortedMeasurements[ii] = @@ -331,6 +345,65 @@ inline SphericalCamera::MeasurementVector undistortMeasurements( return measurements; } +/** Convert pixel measurements in image to homogeneous measurements in the image + * plane using shared camera intrinsics. + * + * @tparam CALIBRATION Calibration type to use. + * @param cal Calibration with which measurements were taken. + * @param measurements Vector of measurements to undistort. + * @return homogeneous measurements in image plane + */ +template +inline Point3Vector calibrateMeasurementsShared( + const CALIBRATION& cal, const Point2Vector& measurements) { + Point3Vector calibratedMeasurements; + // Calibrate with cal and uncalibrate with pinhole version of cal so that + // measurements are undistorted. + std::transform(measurements.begin(), measurements.end(), + std::back_inserter(calibratedMeasurements), + [&cal](const Point2& measurement) { + Point3 p; + p << cal.calibrate(measurement), 1.0; + return p; + }); + return calibratedMeasurements; +} + +/** Convert pixel measurements in image to homogeneous measurements in the image + * plane using camera intrinsics of each measurement. + * + * @tparam CAMERA Camera type to use. + * @param cameras Cameras corresponding to each measurement. + * @param measurements Vector of measurements to undistort. + * @return homogeneous measurements in image plane + */ +template +inline Point3Vector calibrateMeasurements( + const CameraSet& cameras, + const typename CAMERA::MeasurementVector& measurements) { + const size_t nrMeasurements = measurements.size(); + assert(nrMeasurements == cameras.size()); + Point3Vector calibratedMeasurements(nrMeasurements); + for (size_t ii = 0; ii < nrMeasurements; ++ii) { + calibratedMeasurements[ii] + << cameras[ii].calibration().calibrate(measurements[ii]), + 1.0; + } + return calibratedMeasurements; +} + +/** Specialize for SphericalCamera to do nothing. */ +template +inline Point3Vector calibrateMeasurements( + const CameraSet& cameras, + const SphericalCamera::MeasurementVector& measurements) { + Point3Vector calibratedMeasurements(measurements.size()); + for (size_t ii = 0; ii < measurements.size(); ++ii) { + calibratedMeasurements[ii] << measurements[ii].point3(); + } + return calibratedMeasurements; +} + /** * Function to triangulate 3D landmark point from an arbitrary number * of poses (at least 2) using the DLT. The function checks that the @@ -341,41 +414,55 @@ inline SphericalCamera::MeasurementVector undistortMeasurements( * @param measurements A vector of camera measurements * @param rank_tol rank tolerance, default 1e-9 * @param optimize Flag to turn on nonlinear refinement of triangulation + * @param useLOST whether to use the LOST algorithm instead of DLT * @return Returns a Point3 */ -template +template Point3 triangulatePoint3(const std::vector& poses, - boost::shared_ptr sharedCal, - const Point2Vector& measurements, double rank_tol = 1e-9, - bool optimize = false, - const SharedNoiseModel& model = nullptr) { - + boost::shared_ptr sharedCal, + const Point2Vector& measurements, + double rank_tol = 1e-9, bool optimize = false, + const SharedNoiseModel& model = nullptr, + const bool useLOST = false) { assert(poses.size() == measurements.size()); - if (poses.size() < 2) - throw(TriangulationUnderconstrainedException()); - - // construct projection matrices from poses & calibration - auto projection_matrices = projectionMatricesFromPoses(poses, sharedCal); - - // Undistort the measurements, leaving only the pinhole elements in effect. - auto undistortedMeasurements = - undistortMeasurements(*sharedCal, measurements); + if (poses.size() < 2) throw(TriangulationUnderconstrainedException()); // Triangulate linearly - Point3 point = - triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol); + Point3 point; + if (useLOST) { + // Reduce input noise model to an isotropic noise model using the mean of + // the diagonal. + const double measurementSigma = model ? model->sigmas().mean() : 1e-4; + SharedIsotropic measurementNoise = + noiseModel::Isotropic::Sigma(2, measurementSigma); + // calibrate the measurements to obtain homogenous coordinates in image + // plane. + auto calibratedMeasurements = + calibrateMeasurementsShared(*sharedCal, measurements); + + point = triangulateLOST(poses, calibratedMeasurements, measurementNoise); + } else { + // construct projection matrices from poses & calibration + auto projection_matrices = projectionMatricesFromPoses(poses, sharedCal); + + // Undistort the measurements, leaving only the pinhole elements in effect. + auto undistortedMeasurements = + undistortMeasurements(*sharedCal, measurements); + + point = + triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol); + } // Then refine using non-linear optimization if (optimize) - point = triangulateNonlinear // + point = triangulateNonlinear // (poses, sharedCal, measurements, point, model); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION // verify that the triangulated point lies in front of all cameras - for(const Pose3& pose: poses) { + for (const Pose3& pose : poses) { const Point3& p_local = pose.transformTo(point); - if (p_local.z() <= 0) - throw(TriangulationCheiralityException()); + if (p_local.z() <= 0) throw(TriangulationCheiralityException()); } #endif @@ -392,41 +479,63 @@ Point3 triangulatePoint3(const std::vector& poses, * @param measurements A vector of camera measurements * @param rank_tol rank tolerance, default 1e-9 * @param optimize Flag to turn on nonlinear refinement of triangulation + * @param useLOST whether to use the LOST algorithm instead of + * DLT * @return Returns a Point3 */ -template -Point3 triangulatePoint3( - const CameraSet& cameras, - const typename CAMERA::MeasurementVector& measurements, double rank_tol = 1e-9, - bool optimize = false, - const SharedNoiseModel& model = nullptr) { - +template +Point3 triangulatePoint3(const CameraSet& cameras, + const typename CAMERA::MeasurementVector& measurements, + double rank_tol = 1e-9, bool optimize = false, + const SharedNoiseModel& model = nullptr, + const bool useLOST = false) { size_t m = cameras.size(); assert(measurements.size() == m); - if (m < 2) - throw(TriangulationUnderconstrainedException()); + if (m < 2) throw(TriangulationUnderconstrainedException()); - // construct projection matrices from poses & calibration - auto projection_matrices = projectionMatricesFromCameras(cameras); + // Triangulate linearly + Point3 point; + if (useLOST) { + // Reduce input noise model to an isotropic noise model using the mean of + // the diagonal. + const double measurementSigma = model ? model->sigmas().mean() : 1e-4; + SharedIsotropic measurementNoise = + noiseModel::Isotropic::Sigma(2, measurementSigma); - // Undistort the measurements, leaving only the pinhole elements in effect. - auto undistortedMeasurements = - undistortMeasurements(cameras, measurements); + // construct poses from cameras. + std::vector poses; + poses.reserve(cameras.size()); + for (const auto& camera : cameras) poses.push_back(camera.pose()); - Point3 point = - triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol); + // calibrate the measurements to obtain homogenous coordinates in image + // plane. + auto calibratedMeasurements = + calibrateMeasurements(cameras, measurements); - // The n refine using non-linear optimization - if (optimize) + point = triangulateLOST(poses, calibratedMeasurements, measurementNoise); + } else { + // construct projection matrices from poses & calibration + auto projection_matrices = projectionMatricesFromCameras(cameras); + + // Undistort the measurements, leaving only the pinhole elements in effect. + auto undistortedMeasurements = + undistortMeasurements(cameras, measurements); + + point = + triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol); + } + + // Then refine using non-linear optimization + if (optimize) { point = triangulateNonlinear(cameras, measurements, point, model); + } #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION // verify that the triangulated point lies in front of all cameras - for(const CAMERA& camera: cameras) { + for (const CAMERA& camera : cameras) { const Point3& p_local = camera.pose().transformTo(point); - if (p_local.z() <= 0) - throw(TriangulationCheiralityException()); + if (p_local.z() <= 0) throw(TriangulationCheiralityException()); } #endif @@ -434,14 +543,14 @@ Point3 triangulatePoint3( } /// Pinhole-specific version -template -Point3 triangulatePoint3( - const CameraSet >& cameras, - const Point2Vector& measurements, double rank_tol = 1e-9, - bool optimize = false, - const SharedNoiseModel& model = nullptr) { - return triangulatePoint3 > // - (cameras, measurements, rank_tol, optimize, model); +template +Point3 triangulatePoint3(const CameraSet>& cameras, + const Point2Vector& measurements, + double rank_tol = 1e-9, bool optimize = false, + const SharedNoiseModel& model = nullptr, + const bool useLOST = false) { + return triangulatePoint3> // + (cameras, measurements, rank_tol, optimize, model, useLOST); } struct GTSAM_EXPORT TriangulationParameters { diff --git a/gtsam/hybrid/GaussianMixtureConditional.cpp b/gtsam/hybrid/GaussianMixture.cpp similarity index 63% rename from gtsam/hybrid/GaussianMixtureConditional.cpp rename to gtsam/hybrid/GaussianMixture.cpp index 726af6d5f..b04800d21 100644 --- a/gtsam/hybrid/GaussianMixtureConditional.cpp +++ b/gtsam/hybrid/GaussianMixture.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file GaussianMixtureConditional.cpp + * @file GaussianMixture.cpp * @brief A hybrid conditional in the Conditional Linear Gaussian scheme * @author Fan Jiang * @author Varun Agrawal @@ -19,42 +19,41 @@ */ #include -#include -#include +#include +#include #include #include namespace gtsam { -GaussianMixtureConditional::GaussianMixtureConditional( +GaussianMixture::GaussianMixture( const KeyVector &continuousFrontals, const KeyVector &continuousParents, const DiscreteKeys &discreteParents, - const GaussianMixtureConditional::Conditionals &conditionals) + const GaussianMixture::Conditionals &conditionals) : BaseFactor(CollectKeys(continuousFrontals, continuousParents), discreteParents), BaseConditional(continuousFrontals.size()), conditionals_(conditionals) {} /* *******************************************************************************/ -const GaussianMixtureConditional::Conditionals & -GaussianMixtureConditional::conditionals() { +const GaussianMixture::Conditionals &GaussianMixture::conditionals() { return conditionals_; } /* *******************************************************************************/ -GaussianMixtureConditional GaussianMixtureConditional::FromConditionals( +GaussianMixture GaussianMixture::FromConditionals( const KeyVector &continuousFrontals, const KeyVector &continuousParents, const DiscreteKeys &discreteParents, const std::vector &conditionalsList) { Conditionals dt(discreteParents, conditionalsList); - return GaussianMixtureConditional(continuousFrontals, continuousParents, - discreteParents, dt); + return GaussianMixture(continuousFrontals, continuousParents, discreteParents, + dt); } /* *******************************************************************************/ -GaussianMixtureConditional::Sum GaussianMixtureConditional::add( - const GaussianMixtureConditional::Sum &sum) const { +GaussianMixture::Sum GaussianMixture::add( + const GaussianMixture::Sum &sum) const { using Y = GaussianFactorGraph; auto add = [](const Y &graph1, const Y &graph2) { auto result = graph1; @@ -66,8 +65,7 @@ GaussianMixtureConditional::Sum GaussianMixtureConditional::add( } /* *******************************************************************************/ -GaussianMixtureConditional::Sum -GaussianMixtureConditional::asGaussianFactorGraphTree() const { +GaussianMixture::Sum GaussianMixture::asGaussianFactorGraphTree() const { auto lambda = [](const GaussianFactor::shared_ptr &factor) { GaussianFactorGraph result; result.push_back(factor); @@ -77,20 +75,42 @@ GaussianMixtureConditional::asGaussianFactorGraphTree() const { } /* *******************************************************************************/ -bool GaussianMixtureConditional::equals(const HybridFactor &lf, - double tol) const { - return BaseFactor::equals(lf, tol); +size_t GaussianMixture::nrComponents() const { + size_t total = 0; + conditionals_.visit([&total](const GaussianFactor::shared_ptr &node) { + if (node) total += 1; + }); + return total; } /* *******************************************************************************/ -void GaussianMixtureConditional::print(const std::string &s, - const KeyFormatter &formatter) const { +GaussianConditional::shared_ptr GaussianMixture::operator()( + const DiscreteValues &discreteVals) const { + auto &ptr = conditionals_(discreteVals); + if (!ptr) return nullptr; + auto conditional = boost::dynamic_pointer_cast(ptr); + if (conditional) + return conditional; + else + throw std::logic_error( + "A GaussianMixture unexpectedly contained a non-conditional"); +} + +/* *******************************************************************************/ +bool GaussianMixture::equals(const HybridFactor &lf, double tol) const { + const This *e = dynamic_cast(&lf); + return e != nullptr && BaseFactor::equals(*e, tol); +} + +/* *******************************************************************************/ +void GaussianMixture::print(const std::string &s, + const KeyFormatter &formatter) const { std::cout << s; if (isContinuous()) std::cout << "Continuous "; if (isDiscrete()) std::cout << "Discrete "; if (isHybrid()) std::cout << "Hybrid "; BaseConditional::print("", formatter); - std::cout << "\nDiscrete Keys = "; + std::cout << " Discrete Keys = "; for (auto &dk : discreteKeys()) { std::cout << "(" << formatter(dk.first) << ", " << dk.second << "), "; } diff --git a/gtsam/hybrid/GaussianMixtureConditional.h b/gtsam/hybrid/GaussianMixture.h similarity index 65% rename from gtsam/hybrid/GaussianMixtureConditional.h rename to gtsam/hybrid/GaussianMixture.h index d12fa09d7..fc1eb0f06 100644 --- a/gtsam/hybrid/GaussianMixtureConditional.h +++ b/gtsam/hybrid/GaussianMixture.h @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file GaussianMixtureConditional.h + * @file GaussianMixture.h * @brief A hybrid conditional in the Conditional Linear Gaussian scheme * @author Fan Jiang * @author Varun Agrawal @@ -19,7 +19,9 @@ #pragma once +#include #include +#include #include #include #include @@ -27,20 +29,27 @@ namespace gtsam { /** - * @brief A conditional of gaussian mixtures indexed by discrete variables. + * @brief A conditional of gaussian mixtures indexed by discrete variables, as + * part of a Bayes Network. * * Represents the conditional density P(X | M, Z) where X is a continuous random - * variable, M is the discrete variable and Z is the set of measurements. + * variable, M is the selection of discrete variables corresponding to a subset + * of the Gaussian variables and Z is parent of this node + * + * The probability P(x|y,z,...) is proportional to + * \f$ \sum_i k_i \exp - \frac{1}{2} |R_i x - (d_i - S_i y - T_i z - ...)|^2 \f$ + * where i indexes the components and k_i is a component-wise normalization + * constant. * */ -class GaussianMixtureConditional +class GTSAM_EXPORT GaussianMixture : public HybridFactor, - public Conditional { + public Conditional { public: - using This = GaussianMixtureConditional; - using shared_ptr = boost::shared_ptr; + using This = GaussianMixture; + using shared_ptr = boost::shared_ptr; using BaseFactor = HybridFactor; - using BaseConditional = Conditional; + using BaseConditional = Conditional; /// Alias for DecisionTree of GaussianFactorGraphs using Sum = DecisionTree; @@ -61,19 +70,23 @@ class GaussianMixtureConditional /// @{ /// Defaut constructor, mainly for serialization. - GaussianMixtureConditional() = default; + GaussianMixture() = default; + /** - * @brief Construct a new GaussianMixtureConditional object + * @brief Construct a new GaussianMixture object. * * @param continuousFrontals the continuous frontals. * @param continuousParents the continuous parents. * @param discreteParents the discrete parents. Will be placed last. - * @param conditionals a decision tree of GaussianConditionals. + * @param conditionals a decision tree of GaussianConditionals. The number of + * conditionals should be C^(number of discrete parents), where C is the + * cardinality of the DiscreteKeys in discreteParents, since the + * discreteParents will be used as the labels in the decision tree. */ - GaussianMixtureConditional(const KeyVector &continuousFrontals, - const KeyVector &continuousParents, - const DiscreteKeys &discreteParents, - const Conditionals &conditionals); + GaussianMixture(const KeyVector &continuousFrontals, + const KeyVector &continuousParents, + const DiscreteKeys &discreteParents, + const Conditionals &conditionals); /** * @brief Make a Gaussian Mixture from a list of Gaussian conditionals @@ -88,6 +101,16 @@ class GaussianMixtureConditional const DiscreteKeys &discreteParents, const std::vector &conditionals); + /// @} + /// @name Standard API + /// @{ + + GaussianConditional::shared_ptr operator()( + const DiscreteValues &discreteVals) const; + + /// Returns the total number of continuous components + size_t nrComponents() const; + /// @} /// @name Testable /// @{ @@ -97,7 +120,7 @@ class GaussianMixtureConditional /* print utility */ void print( - const std::string &s = "GaussianMixtureConditional\n", + const std::string &s = "GaussianMixture\n", const KeyFormatter &formatter = DefaultKeyFormatter) const override; /// @} @@ -115,4 +138,8 @@ class GaussianMixtureConditional Sum add(const Sum &sum) const; }; +// traits +template <> +struct traits : public Testable {}; + } // namespace gtsam diff --git a/gtsam/hybrid/GaussianMixtureFactor.cpp b/gtsam/hybrid/GaussianMixtureFactor.cpp index 589e5c660..8f832d8ea 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.cpp +++ b/gtsam/hybrid/GaussianMixtureFactor.cpp @@ -34,7 +34,8 @@ GaussianMixtureFactor::GaussianMixtureFactor(const KeyVector &continuousKeys, /* *******************************************************************************/ bool GaussianMixtureFactor::equals(const HybridFactor &lf, double tol) const { - return Base::equals(lf, tol); + const This *e = dynamic_cast(&lf); + return e != nullptr && Base::equals(*e, tol); } /* *******************************************************************************/ @@ -50,16 +51,19 @@ GaussianMixtureFactor GaussianMixtureFactor::FromFactors( void GaussianMixtureFactor::print(const std::string &s, const KeyFormatter &formatter) const { HybridFactor::print(s, formatter); + std::cout << "]{\n"; factors_.print( - "mixture = ", [&](Key k) { return formatter(k); }, + "", [&](Key k) { return formatter(k); }, [&](const GaussianFactor::shared_ptr &gf) -> std::string { RedirectCout rd; - if (!gf->empty()) + std::cout << ":\n"; + if (gf) gf->print("", formatter); else return {"nullptr"}; return rd.str(); }); + std::cout << "}" << std::endl; } /* *******************************************************************************/ diff --git a/gtsam/hybrid/GaussianMixtureFactor.h b/gtsam/hybrid/GaussianMixtureFactor.h index 494a44ccf..a0a51af55 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.h +++ b/gtsam/hybrid/GaussianMixtureFactor.h @@ -11,7 +11,7 @@ /** * @file GaussianMixtureFactor.h - * @brief A factor that is a function of discrete and continuous variables. + * @brief A set of GaussianFactors, indexed by a set of discrete keys. * @author Fan Jiang * @author Varun Agrawal * @author Frank Dellaert @@ -32,15 +32,16 @@ class GaussianFactorGraph; using GaussianFactorVector = std::vector; /** - * @brief A linear factor that is a function of both discrete and continuous - * variables, i.e. P(X, M | Z) where X is the set of continuous variables, M is - * the set of discrete variables and Z is the measurement set. + * @brief Implementation of a discrete conditional mixture factor. + * Implements a joint discrete-continuous factor where the discrete variable + * serves to "select" a mixture component corresponding to a GaussianFactor type + * of measurement. * * Represents the underlying Gaussian Mixture as a Decision Tree, where the set * of discrete variables indexes to the continuous gaussian distribution. * */ -class GaussianMixtureFactor : public HybridFactor { +class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor { public: using Base = HybridFactor; using This = GaussianMixtureFactor; @@ -52,6 +53,7 @@ class GaussianMixtureFactor : public HybridFactor { using Factors = DecisionTree; private: + /// Decision tree of Gaussian factors indexed by discrete keys. Factors factors_; /** @@ -82,6 +84,19 @@ class GaussianMixtureFactor : public HybridFactor { const DiscreteKeys &discreteKeys, const Factors &factors); + /** + * @brief Construct a new GaussianMixtureFactor object using a vector of + * GaussianFactor shared pointers. + * + * @param keys Vector of keys for continuous factors. + * @param discreteKeys Vector of discrete keys. + * @param factors Vector of gaussian factor shared pointers. + */ + GaussianMixtureFactor(const KeyVector &keys, const DiscreteKeys &discreteKeys, + const std::vector &factors) + : GaussianMixtureFactor(keys, discreteKeys, + Factors(discreteKeys, factors)) {} + static This FromFactors( const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys, const std::vector &factors); @@ -109,6 +124,17 @@ class GaussianMixtureFactor : public HybridFactor { * @return Sum */ Sum add(const Sum &sum) const; + + /// Add MixtureFactor to a Sum, syntactic sugar. + friend Sum &operator+=(Sum &sum, const GaussianMixtureFactor &factor) { + sum = factor.add(sum); + return sum; + } +}; + +// traits +template <> +struct traits : public Testable { }; } // namespace gtsam diff --git a/gtsam/hybrid/HybridBayesTree.cpp b/gtsam/hybrid/HybridBayesTree.cpp index 400cac5e7..d65270f91 100644 --- a/gtsam/hybrid/HybridBayesTree.cpp +++ b/gtsam/hybrid/HybridBayesTree.cpp @@ -11,7 +11,8 @@ /** * @file HybridBayesTree.cpp - * @brief Hybrid Bayes Tree, the result of eliminating a HybridJunctionTree + * @brief Hybrid Bayes Tree, the result of eliminating a + * HybridJunctionTree * @date Mar 11, 2022 * @author Fan Jiang */ @@ -26,7 +27,7 @@ namespace gtsam { // Instantiate base class template class BayesTreeCliqueBase; + HybridGaussianFactorGraph>; template class BayesTree; /* ************************************************************************* */ diff --git a/gtsam/hybrid/HybridBayesTree.h b/gtsam/hybrid/HybridBayesTree.h index 610181458..0b89ca8c4 100644 --- a/gtsam/hybrid/HybridBayesTree.h +++ b/gtsam/hybrid/HybridBayesTree.h @@ -11,15 +11,16 @@ /** * @file HybridBayesTree.h - * @brief Hybrid Bayes Tree, the result of eliminating a HybridJunctionTree + * @brief Hybrid Bayes Tree, the result of eliminating a + * HybridJunctionTree * @date Mar 11, 2022 * @author Fan Jiang */ #pragma once -#include #include +#include #include #include #include @@ -38,10 +39,10 @@ class VectorValues; */ class GTSAM_EXPORT HybridBayesTreeClique : public BayesTreeCliqueBase { + HybridGaussianFactorGraph> { public: typedef HybridBayesTreeClique This; - typedef BayesTreeCliqueBase + typedef BayesTreeCliqueBase Base; typedef boost::shared_ptr shared_ptr; typedef boost::weak_ptr weak_ptr; diff --git a/gtsam/hybrid/HybridConditional.cpp b/gtsam/hybrid/HybridConditional.cpp index 7d1b72067..8e071532d 100644 --- a/gtsam/hybrid/HybridConditional.cpp +++ b/gtsam/hybrid/HybridConditional.cpp @@ -54,7 +54,7 @@ HybridConditional::HybridConditional( /* ************************************************************************ */ HybridConditional::HybridConditional( - boost::shared_ptr gaussianMixture) + boost::shared_ptr gaussianMixture) : BaseFactor(KeyVector(gaussianMixture->keys().begin(), gaussianMixture->keys().begin() + gaussianMixture->nrContinuous()), @@ -101,7 +101,8 @@ void HybridConditional::print(const std::string &s, /* ************************************************************************ */ bool HybridConditional::equals(const HybridFactor &other, double tol) const { - return BaseFactor::equals(other, tol); + const This *e = dynamic_cast(&other); + return e != nullptr && BaseFactor::equals(*e, tol); } } // namespace gtsam diff --git a/gtsam/hybrid/HybridConditional.h b/gtsam/hybrid/HybridConditional.h index 9592b0c69..3ba5da393 100644 --- a/gtsam/hybrid/HybridConditional.h +++ b/gtsam/hybrid/HybridConditional.h @@ -18,9 +18,9 @@ #pragma once #include -#include -#include +#include #include +#include #include #include #include @@ -34,7 +34,7 @@ namespace gtsam { -class GaussianHybridFactorGraph; +class HybridGaussianFactorGraph; /** * Hybrid Conditional Density @@ -42,7 +42,7 @@ class GaussianHybridFactorGraph; * As a type-erased variant of: * - DiscreteConditional * - GaussianConditional - * - GaussianMixtureConditional + * - GaussianMixture * * The reason why this is important is that `Conditional` is a CRTP class. * CRTP is static polymorphism such that all CRTP classes, while bearing the @@ -128,16 +128,16 @@ class GTSAM_EXPORT HybridConditional * HybridConditional. */ HybridConditional( - boost::shared_ptr gaussianMixture); + boost::shared_ptr gaussianMixture); /** - * @brief Return HybridConditional as a GaussianMixtureConditional + * @brief Return HybridConditional as a GaussianMixture * - * @return GaussianMixtureConditional::shared_ptr + * @return GaussianMixture::shared_ptr */ - GaussianMixtureConditional::shared_ptr asMixture() { + GaussianMixture::shared_ptr asMixture() { if (!isHybrid()) throw std::invalid_argument("Not a mixture"); - return boost::static_pointer_cast(inner_); + return boost::static_pointer_cast(inner_); } /** diff --git a/gtsam/hybrid/HybridDiscreteFactor.cpp b/gtsam/hybrid/HybridDiscreteFactor.cpp index 989127a28..0455e1e90 100644 --- a/gtsam/hybrid/HybridDiscreteFactor.cpp +++ b/gtsam/hybrid/HybridDiscreteFactor.cpp @@ -38,14 +38,16 @@ HybridDiscreteFactor::HybridDiscreteFactor(DecisionTreeFactor &&dtf) /* ************************************************************************ */ bool HybridDiscreteFactor::equals(const HybridFactor &lf, double tol) const { - return Base::equals(lf, tol); + const This *e = dynamic_cast(&lf); + // TODO(Varun) How to compare inner_ when they are abstract types? + return e != nullptr && Base::equals(*e, tol); } /* ************************************************************************ */ void HybridDiscreteFactor::print(const std::string &s, const KeyFormatter &formatter) const { HybridFactor::print(s, formatter); - inner_->print("inner: ", formatter); + inner_->print("\n", formatter); }; } // namespace gtsam diff --git a/gtsam/hybrid/HybridDiscreteFactor.h b/gtsam/hybrid/HybridDiscreteFactor.h index 572ddfbcd..9cbea8170 100644 --- a/gtsam/hybrid/HybridDiscreteFactor.h +++ b/gtsam/hybrid/HybridDiscreteFactor.h @@ -29,7 +29,7 @@ namespace gtsam { * us to hide the implementation of DiscreteFactor and thus avoid diamond * inheritance. */ -class HybridDiscreteFactor : public HybridFactor { +class GTSAM_EXPORT HybridDiscreteFactor : public HybridFactor { private: DiscreteFactor::shared_ptr inner_; @@ -61,4 +61,9 @@ class HybridDiscreteFactor : public HybridFactor { /// Return pointer to the internal discrete factor DiscreteFactor::shared_ptr inner() const { return inner_; } }; + +// traits +template <> +struct traits : public Testable {}; + } // namespace gtsam diff --git a/gtsam/hybrid/HybridEliminationTree.cpp b/gtsam/hybrid/HybridEliminationTree.cpp index 148685327..c2df2dd60 100644 --- a/gtsam/hybrid/HybridEliminationTree.cpp +++ b/gtsam/hybrid/HybridEliminationTree.cpp @@ -21,17 +21,17 @@ namespace gtsam { // Instantiate base class -template class EliminationTree; +template class EliminationTree; /* ************************************************************************* */ HybridEliminationTree::HybridEliminationTree( - const GaussianHybridFactorGraph& factorGraph, + const HybridGaussianFactorGraph& factorGraph, const VariableIndex& structure, const Ordering& order) : Base(factorGraph, structure, order) {} /* ************************************************************************* */ HybridEliminationTree::HybridEliminationTree( - const GaussianHybridFactorGraph& factorGraph, const Ordering& order) + const HybridGaussianFactorGraph& factorGraph, const Ordering& order) : Base(factorGraph, order) {} /* ************************************************************************* */ diff --git a/gtsam/hybrid/HybridEliminationTree.h b/gtsam/hybrid/HybridEliminationTree.h index 04bd9cd35..77a84fea8 100644 --- a/gtsam/hybrid/HybridEliminationTree.h +++ b/gtsam/hybrid/HybridEliminationTree.h @@ -17,8 +17,8 @@ #pragma once -#include #include +#include #include namespace gtsam { @@ -27,12 +27,12 @@ namespace gtsam { * Elimination Tree type for Hybrid */ class GTSAM_EXPORT HybridEliminationTree - : public EliminationTree { + : public EliminationTree { private: friend class ::EliminationTreeTester; public: - typedef EliminationTree + typedef EliminationTree Base; ///< Base class typedef HybridEliminationTree This; ///< This class typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class @@ -49,7 +49,7 @@ class GTSAM_EXPORT HybridEliminationTree * named constructor instead. * @return The elimination tree */ - HybridEliminationTree(const GaussianHybridFactorGraph& factorGraph, + HybridEliminationTree(const HybridGaussianFactorGraph& factorGraph, const VariableIndex& structure, const Ordering& order); /** Build the elimination tree of a factor graph. Note that this has to @@ -57,7 +57,7 @@ class GTSAM_EXPORT HybridEliminationTree * this precomputed, use the other constructor instead. * @param factorGraph The factor graph for which to build the elimination tree */ - HybridEliminationTree(const GaussianHybridFactorGraph& factorGraph, + HybridEliminationTree(const HybridGaussianFactorGraph& factorGraph, const Ordering& order); /// @} diff --git a/gtsam/hybrid/HybridFactor.cpp b/gtsam/hybrid/HybridFactor.cpp index 815ea0415..8df2d524f 100644 --- a/gtsam/hybrid/HybridFactor.cpp +++ b/gtsam/hybrid/HybridFactor.cpp @@ -50,7 +50,10 @@ DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1, /* ************************************************************************ */ HybridFactor::HybridFactor(const KeyVector &keys) - : Base(keys), isContinuous_(true), nrContinuous_(keys.size()) {} + : Base(keys), + isContinuous_(true), + nrContinuous_(keys.size()), + continuousKeys_(keys) {} /* ************************************************************************ */ HybridFactor::HybridFactor(const KeyVector &continuousKeys, @@ -60,17 +63,22 @@ HybridFactor::HybridFactor(const KeyVector &continuousKeys, isContinuous_((continuousKeys.size() != 0) && (discreteKeys.size() == 0)), isHybrid_((continuousKeys.size() != 0) && (discreteKeys.size() != 0)), nrContinuous_(continuousKeys.size()), - discreteKeys_(discreteKeys) {} + discreteKeys_(discreteKeys), + continuousKeys_(continuousKeys) {} /* ************************************************************************ */ HybridFactor::HybridFactor(const DiscreteKeys &discreteKeys) : Base(CollectKeys({}, discreteKeys)), isDiscrete_(true), - discreteKeys_(discreteKeys) {} + discreteKeys_(discreteKeys), + continuousKeys_({}) {} /* ************************************************************************ */ bool HybridFactor::equals(const HybridFactor &lf, double tol) const { - return Base::equals(lf, tol); + const This *e = dynamic_cast(&lf); + return e != nullptr && Base::equals(*e, tol) && + isDiscrete_ == e->isDiscrete_ && isContinuous_ == e->isContinuous_ && + isHybrid_ == e->isHybrid_ && nrContinuous_ == e->nrContinuous_; } /* ************************************************************************ */ @@ -80,7 +88,17 @@ void HybridFactor::print(const std::string &s, if (isContinuous_) std::cout << "Continuous "; if (isDiscrete_) std::cout << "Discrete "; if (isHybrid_) std::cout << "Hybrid "; - this->printKeys("", formatter); + for (size_t c=0; ckeys()) { - inner_ = other; -} + : Base(other->keys()), inner_(other) {} +/* ************************************************************************* */ HybridGaussianFactor::HybridGaussianFactor(JacobianFactor &&jf) : Base(jf.keys()), inner_(boost::make_shared(std::move(jf))) {} -bool HybridGaussianFactor::equals(const HybridFactor &lf, double tol) const { - return false; +/* ************************************************************************* */ +bool HybridGaussianFactor::equals(const HybridFactor &other, double tol) const { + const This *e = dynamic_cast(&other); + // TODO(Varun) How to compare inner_ when they are abstract types? + return e != nullptr && Base::equals(*e, tol); } + +/* ************************************************************************* */ void HybridGaussianFactor::print(const std::string &s, const KeyFormatter &formatter) const { HybridFactor::print(s, formatter); - inner_->print("inner: ", formatter); + inner_->print("\n", formatter); }; } // namespace gtsam diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h index 4c8ede12c..7ffe39e79 100644 --- a/gtsam/hybrid/HybridGaussianFactor.h +++ b/gtsam/hybrid/HybridGaussianFactor.h @@ -25,7 +25,8 @@ namespace gtsam { /** * A HybridGaussianFactor is a layer over GaussianFactor so that we do not have - * a diamond inheritance. + * a diamond inheritance i.e. an extra factor type that inherits from both + * HybridFactor and GaussianFactor. */ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor { private: @@ -64,4 +65,9 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor { GaussianFactor::shared_ptr inner() const { return inner_; } }; + +// traits +template <> +struct traits : public Testable {}; + } // namespace gtsam diff --git a/gtsam/hybrid/GaussianHybridFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp similarity index 83% rename from gtsam/hybrid/GaussianHybridFactorGraph.cpp rename to gtsam/hybrid/HybridGaussianFactorGraph.cpp index 3354d5b4d..88730cae9 100644 --- a/gtsam/hybrid/GaussianHybridFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file GaussianHybridFactorGraph.cpp + * @file HybridGaussianFactorGraph.cpp * @brief Hybrid factor graph that uses type erasure * @author Fan Jiang * @author Varun Agrawal @@ -23,14 +23,14 @@ #include #include #include -#include -#include +#include #include #include #include #include #include #include +#include #include #include #include @@ -53,7 +53,7 @@ namespace gtsam { -template class EliminateableFactorGraph; +template class EliminateableFactorGraph; /* ************************************************************************ */ static GaussianMixtureFactor::Sum &addGaussian( @@ -77,67 +77,8 @@ static GaussianMixtureFactor::Sum &addGaussian( } /* ************************************************************************ */ -std::pair -continuousElimination(const GaussianHybridFactorGraph &factors, - const Ordering &frontalKeys) { - GaussianFactorGraph gfg; - for (auto &fp : factors) { - auto ptr = boost::dynamic_pointer_cast(fp); - if (ptr) { - gfg.push_back(ptr->inner()); - } else { - auto p = boost::static_pointer_cast(fp)->inner(); - if (p) { - gfg.push_back(boost::static_pointer_cast(p)); - } else { - // It is an orphan wrapped conditional - } - } - } - - auto result = EliminatePreferCholesky(gfg, frontalKeys); - return std::make_pair( - boost::make_shared(result.first), - boost::make_shared(result.second)); -} - -/* ************************************************************************ */ -std::pair -discreteElimination(const GaussianHybridFactorGraph &factors, - const Ordering &frontalKeys) { - DiscreteFactorGraph dfg; - for (auto &fp : factors) { - auto ptr = boost::dynamic_pointer_cast(fp); - if (ptr) { - dfg.push_back(ptr->inner()); - } else { - auto p = boost::static_pointer_cast(fp)->inner(); - if (p) { - dfg.push_back(boost::static_pointer_cast(p)); - } else { - // It is an orphan wrapper - } - } - } - - auto result = EliminateDiscrete(dfg, frontalKeys); - - return std::make_pair( - boost::make_shared(result.first), - boost::make_shared(result.second)); -} - -/* ************************************************************************ */ -std::pair -hybridElimination(const GaussianHybridFactorGraph &factors, - const Ordering &frontalKeys, - const KeySet &continuousSeparator, - const std::set &discreteSeparatorSet) { - // NOTE: since we use the special JunctionTree, - // only possiblity is continuous conditioned on discrete. - DiscreteKeys discreteSeparator(discreteSeparatorSet.begin(), - discreteSeparatorSet.end()); - +GaussianMixtureFactor::Sum sumFrontals( + const HybridGaussianFactorGraph &factors) { // sum out frontals, this is the factor on the separator gttic(sum); @@ -146,13 +87,11 @@ hybridElimination(const GaussianHybridFactorGraph &factors, for (auto &f : factors) { if (f->isHybrid()) { - auto cgmf = boost::dynamic_pointer_cast(f); - if (cgmf) { + if (auto cgmf = boost::dynamic_pointer_cast(f)) { sum = cgmf->add(sum); } - auto gm = boost::dynamic_pointer_cast(f); - if (gm) { + if (auto gm = boost::dynamic_pointer_cast(f)) { sum = gm->asMixture()->add(sum); } @@ -179,11 +118,72 @@ hybridElimination(const GaussianHybridFactorGraph &factors, gttoc(sum); + return sum; +} + +/* ************************************************************************ */ +std::pair +continuousElimination(const HybridGaussianFactorGraph &factors, + const Ordering &frontalKeys) { + GaussianFactorGraph gfg; + for (auto &fp : factors) { + if (auto ptr = boost::dynamic_pointer_cast(fp)) { + gfg.push_back(ptr->inner()); + } else if (auto p = + boost::static_pointer_cast(fp)->inner()) { + gfg.push_back(boost::static_pointer_cast(p)); + } else { + // It is an orphan wrapped conditional + } + } + + auto result = EliminatePreferCholesky(gfg, frontalKeys); + return {boost::make_shared(result.first), + boost::make_shared(result.second)}; +} + +/* ************************************************************************ */ +std::pair +discreteElimination(const HybridGaussianFactorGraph &factors, + const Ordering &frontalKeys) { + DiscreteFactorGraph dfg; + for (auto &fp : factors) { + if (auto ptr = boost::dynamic_pointer_cast(fp)) { + dfg.push_back(ptr->inner()); + } else if (auto p = + boost::static_pointer_cast(fp)->inner()) { + dfg.push_back(boost::static_pointer_cast(p)); + } else { + // It is an orphan wrapper + } + } + + auto result = EliminateDiscrete(dfg, frontalKeys); + + return {boost::make_shared(result.first), + boost::make_shared(result.second)}; +} + +/* ************************************************************************ */ +std::pair +hybridElimination(const HybridGaussianFactorGraph &factors, + const Ordering &frontalKeys, + const KeySet &continuousSeparator, + const std::set &discreteSeparatorSet) { + // NOTE: since we use the special JunctionTree, + // only possiblity is continuous conditioned on discrete. + DiscreteKeys discreteSeparator(discreteSeparatorSet.begin(), + discreteSeparatorSet.end()); + + // sum out frontals, this is the factor on the separator + GaussianMixtureFactor::Sum sum = sumFrontals(factors); + using EliminationPair = GaussianFactorGraph::EliminationResult; KeyVector keysOfEliminated; // Not the ordering KeyVector keysOfSeparator; // TODO(frank): Is this just (keys - ordering)? + // This is the elimination method on the leaf nodes auto eliminate = [&](const GaussianFactorGraph &graph) -> GaussianFactorGraph::EliminationResult { if (graph.empty()) { @@ -201,14 +201,16 @@ hybridElimination(const GaussianHybridFactorGraph &factors, return result; }; + // Perform elimination! DecisionTree eliminationResults(sum, eliminate); + // Separate out decision tree into conditionals and remaining factors. auto pair = unzip(eliminationResults); const GaussianMixtureFactor::Factors &separatorFactors = pair.second; - // Create the GaussianMixtureConditional from the conditionals - auto conditional = boost::make_shared( + // Create the GaussianMixture from the conditionals + auto conditional = boost::make_shared( frontalKeys, keysOfSeparator, discreteSeparator, pair.first); // If there are no more continuous parents, then we should create here a @@ -236,7 +238,7 @@ hybridElimination(const GaussianHybridFactorGraph &factors, } /* ************************************************************************ */ std::pair // -EliminateHybrid(const GaussianHybridFactorGraph &factors, +EliminateHybrid(const HybridGaussianFactorGraph &factors, const Ordering &frontalKeys) { // NOTE: Because we are in the Conditional Gaussian regime there are only // a few cases: @@ -262,7 +264,7 @@ EliminateHybrid(const GaussianHybridFactorGraph &factors, // Because of all these reasons, we carefully consider how to // implement the hybrid factors so that we do not get poor performance. - // The first thing is how to represent the GaussianMixtureConditional. + // The first thing is how to represent the GaussianMixture. // A very possible scenario is that the incoming factors will have different // levels of discrete keys. For example, imagine we are going to eliminate the // fragment: $\phi(x1,c1,c2)$, $\phi(x1,c2,c3)$, which is perfectly valid. @@ -345,22 +347,22 @@ EliminateHybrid(const GaussianHybridFactorGraph &factors, } /* ************************************************************************ */ -void GaussianHybridFactorGraph::add(JacobianFactor &&factor) { +void HybridGaussianFactorGraph::add(JacobianFactor &&factor) { FactorGraph::add(boost::make_shared(std::move(factor))); } /* ************************************************************************ */ -void GaussianHybridFactorGraph::add(JacobianFactor::shared_ptr factor) { +void HybridGaussianFactorGraph::add(JacobianFactor::shared_ptr factor) { FactorGraph::add(boost::make_shared(factor)); } /* ************************************************************************ */ -void GaussianHybridFactorGraph::add(DecisionTreeFactor &&factor) { +void HybridGaussianFactorGraph::add(DecisionTreeFactor &&factor) { FactorGraph::add(boost::make_shared(std::move(factor))); } /* ************************************************************************ */ -void GaussianHybridFactorGraph::add(DecisionTreeFactor::shared_ptr factor) { +void HybridGaussianFactorGraph::add(DecisionTreeFactor::shared_ptr factor) { FactorGraph::add(boost::make_shared(factor)); } diff --git a/gtsam/hybrid/GaussianHybridFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h similarity index 80% rename from gtsam/hybrid/GaussianHybridFactorGraph.h rename to gtsam/hybrid/HybridGaussianFactorGraph.h index 6998ff899..b98654b92 100644 --- a/gtsam/hybrid/GaussianHybridFactorGraph.h +++ b/gtsam/hybrid/HybridGaussianFactorGraph.h @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file GaussianHybridFactorGraph.h + * @file HybridGaussianFactorGraph.h * @brief Linearized Hybrid factor graph that uses type erasure * @author Fan Jiang * @date Mar 11, 2022 @@ -29,7 +29,7 @@ namespace gtsam { // Forward declarations -class GaussianHybridFactorGraph; +class HybridGaussianFactorGraph; class HybridConditional; class HybridBayesNet; class HybridEliminationTree; @@ -39,26 +39,27 @@ class DecisionTreeFactor; class JacobianFactor; -/** Main elimination function for GaussianHybridFactorGraph */ +/** Main elimination function for HybridGaussianFactorGraph */ GTSAM_EXPORT std::pair, HybridFactor::shared_ptr> -EliminateHybrid(const GaussianHybridFactorGraph& factors, const Ordering& keys); +EliminateHybrid(const HybridGaussianFactorGraph& factors, const Ordering& keys); /* ************************************************************************* */ template <> -struct EliminationTraits { +struct EliminationTraits { typedef HybridFactor FactorType; ///< Type of factors in factor graph - typedef GaussianHybridFactorGraph + typedef HybridGaussianFactorGraph FactorGraphType; ///< Type of the factor graph (e.g. - ///< GaussianHybridFactorGraph) + ///< HybridGaussianFactorGraph) typedef HybridConditional ConditionalType; ///< Type of conditionals from elimination typedef HybridBayesNet BayesNetType; ///< Type of Bayes net from sequential elimination typedef HybridEliminationTree - EliminationTreeType; ///< Type of elimination tree - typedef HybridBayesTree BayesTreeType; ///< Type of Bayes tree - typedef HybridJunctionTree JunctionTreeType; ///< Type of Junction tree + EliminationTreeType; ///< Type of elimination tree + typedef HybridBayesTree BayesTreeType; ///< Type of Bayes tree + typedef HybridJunctionTree + JunctionTreeType; ///< Type of Junction tree /// The default dense elimination function static std::pair, boost::shared_ptr > @@ -73,18 +74,12 @@ struct EliminationTraits { * This is the linearized version of a hybrid factor graph. * Everything inside needs to be hybrid factor or hybrid conditional. */ -class GaussianHybridFactorGraph - : public HybridFactorGraph, - public EliminateableFactorGraph { - protected: - /// Check if FACTOR type is derived from GaussianFactor. - template - using IsGaussian = typename std::enable_if< - std::is_base_of::value>::type; - +class GTSAM_EXPORT HybridGaussianFactorGraph + : public HybridFactorGraph,, + public EliminateableFactorGraph { public: using Base = HybridFactorGraph; - using This = GaussianHybridFactorGraph; ///< this class + using This = HybridGaussianFactorGraph; ///< this class using BaseEliminateable = EliminateableFactorGraph; ///< for elimination using shared_ptr = boost::shared_ptr; ///< shared_ptr to This @@ -95,7 +90,7 @@ class GaussianHybridFactorGraph /// @name Constructors /// @{ - GaussianHybridFactorGraph() = default; + HybridGaussianFactorGraph() = default; /** * Implicit copy/downcast constructor to override explicit template container @@ -103,7 +98,7 @@ class GaussianHybridFactorGraph * `cachedSeparatorMarginal_.reset(*separatorMarginal)` * */ template - GaussianHybridFactorGraph(const FactorGraph& graph) + HybridGaussianFactorGraph(const FactorGraph& graph) : Base(graph) {} /// @} diff --git a/gtsam/hybrid/HybridISAM.cpp b/gtsam/hybrid/HybridGaussianISAM.cpp similarity index 84% rename from gtsam/hybrid/HybridISAM.cpp rename to gtsam/hybrid/HybridGaussianISAM.cpp index 476149126..7783a88dd 100644 --- a/gtsam/hybrid/HybridISAM.cpp +++ b/gtsam/hybrid/HybridGaussianISAM.cpp @@ -10,16 +10,16 @@ * -------------------------------------------------------------------------- */ /** - * @file HybridISAM.h + * @file HybridGaussianISAM.h * @date March 31, 2022 * @author Fan Jiang * @author Frank Dellaert * @author Richard Roberts */ -#include #include -#include +#include +#include #include #include @@ -31,15 +31,17 @@ namespace gtsam { // template class ISAM; /* ************************************************************************* */ -HybridISAM::HybridISAM() {} +HybridGaussianISAM::HybridGaussianISAM() {} /* ************************************************************************* */ -HybridISAM::HybridISAM(const HybridBayesTree& bayesTree) : Base(bayesTree) {} +HybridGaussianISAM::HybridGaussianISAM(const HybridBayesTree& bayesTree) + : Base(bayesTree) {} /* ************************************************************************* */ -void HybridISAM::updateInternal(const GaussianHybridFactorGraph& newFactors, - HybridBayesTree::Cliques* orphans, - const HybridBayesTree::Eliminate& function) { +void HybridGaussianISAM::updateInternal( + const HybridGaussianFactorGraph& newFactors, + HybridBayesTree::Cliques* orphans, + const HybridBayesTree::Eliminate& function) { // Remove the contaminated part of the Bayes tree BayesNetType bn; const KeySet newFactorKeys = newFactors.keys(); @@ -90,8 +92,8 @@ void HybridISAM::updateInternal(const GaussianHybridFactorGraph& newFactors, } /* ************************************************************************* */ -void HybridISAM::update(const GaussianHybridFactorGraph& newFactors, - const HybridBayesTree::Eliminate& function) { +void HybridGaussianISAM::update(const HybridGaussianFactorGraph& newFactors, + const HybridBayesTree::Eliminate& function) { Cliques orphans; this->updateInternal(newFactors, &orphans, function); } diff --git a/gtsam/hybrid/HybridISAM.h b/gtsam/hybrid/HybridGaussianISAM.h similarity index 76% rename from gtsam/hybrid/HybridISAM.h rename to gtsam/hybrid/HybridGaussianISAM.h index fae7efafd..d5b6271da 100644 --- a/gtsam/hybrid/HybridISAM.h +++ b/gtsam/hybrid/HybridGaussianISAM.h @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file HybridISAM.h + * @file HybridGaussianISAM.h * @date March 31, 2022 * @author Fan Jiang * @author Frank Dellaert @@ -20,33 +20,33 @@ #pragma once #include -#include #include +#include #include namespace gtsam { -class GTSAM_EXPORT HybridISAM : public ISAM { +class GTSAM_EXPORT HybridGaussianISAM : public ISAM { public: typedef ISAM Base; - typedef HybridISAM This; + typedef HybridGaussianISAM This; typedef boost::shared_ptr shared_ptr; /// @name Standard Constructors /// @{ /** Create an empty Bayes Tree */ - HybridISAM(); + HybridGaussianISAM(); /** Copy constructor */ - HybridISAM(const HybridBayesTree& bayesTree); + HybridGaussianISAM(const HybridBayesTree& bayesTree); /// @} private: /// Internal method that performs the ISAM update. void updateInternal( - const GaussianHybridFactorGraph& newFactors, + const HybridGaussianFactorGraph& newFactors, HybridBayesTree::Cliques* orphans, const HybridBayesTree::Eliminate& function = HybridBayesTree::EliminationTraitsType::DefaultEliminate); @@ -58,13 +58,13 @@ class GTSAM_EXPORT HybridISAM : public ISAM { * @param newFactors Factor graph of new factors to add and eliminate. * @param function Elimination function. */ - void update(const GaussianHybridFactorGraph& newFactors, + void update(const HybridGaussianFactorGraph& newFactors, const HybridBayesTree::Eliminate& function = HybridBayesTree::EliminationTraitsType::DefaultEliminate); }; /// traits template <> -struct traits : public Testable {}; +struct traits : public Testable {}; } // namespace gtsam diff --git a/gtsam/hybrid/HybridJunctionTree.cpp b/gtsam/hybrid/HybridJunctionTree.cpp index 8fa3aa033..7725742cf 100644 --- a/gtsam/hybrid/HybridJunctionTree.cpp +++ b/gtsam/hybrid/HybridJunctionTree.cpp @@ -15,8 +15,8 @@ * @author Fan Jiang */ -#include #include +#include #include #include #include @@ -27,19 +27,19 @@ namespace gtsam { // Instantiate base classes template class EliminatableClusterTree; -template class JunctionTree; + HybridGaussianFactorGraph>; +template class JunctionTree; struct HybridConstructorTraversalData { typedef - typename JunctionTree::Node + typename JunctionTree::Node Node; typedef typename JunctionTree::sharedNode sharedNode; + HybridGaussianFactorGraph>::sharedNode sharedNode; HybridConstructorTraversalData* const parentData; - sharedNode myJTNode; + sharedNode junctionTreeNode; FastVector childSymbolicConditionals; FastVector childSymbolicFactors; KeySet discreteKeys; @@ -57,24 +57,24 @@ struct HybridConstructorTraversalData { // On the pre-order pass, before children have been visited, we just set up // a traversal data structure with its own JT node, and create a child // pointer in its parent. - HybridConstructorTraversalData myData = + HybridConstructorTraversalData data = HybridConstructorTraversalData(&parentData); - myData.myJTNode = boost::make_shared(node->key, node->factors); - parentData.myJTNode->addChild(myData.myJTNode); + data.junctionTreeNode = boost::make_shared(node->key, node->factors); + parentData.junctionTreeNode->addChild(data.junctionTreeNode); for (HybridFactor::shared_ptr& f : node->factors) { for (auto& k : f->discreteKeys()) { - myData.discreteKeys.insert(k.first); + data.discreteKeys.insert(k.first); } } - return myData; + return data; } // Post-order visitor function static void ConstructorTraversalVisitorPostAlg2( const boost::shared_ptr& ETreeNode, - const HybridConstructorTraversalData& myData) { + const HybridConstructorTraversalData& data) { // In this post-order visitor, we combine the symbolic elimination results // from the elimination tree children and symbolically eliminate the current // elimination tree node. We then check whether each of our elimination @@ -87,50 +87,50 @@ struct HybridConstructorTraversalData { // Do symbolic elimination for this node SymbolicFactors symbolicFactors; symbolicFactors.reserve(ETreeNode->factors.size() + - myData.childSymbolicFactors.size()); + data.childSymbolicFactors.size()); // Add ETree node factors symbolicFactors += ETreeNode->factors; // Add symbolic factors passed up from children - symbolicFactors += myData.childSymbolicFactors; + symbolicFactors += data.childSymbolicFactors; Ordering keyAsOrdering; keyAsOrdering.push_back(ETreeNode->key); - SymbolicConditional::shared_ptr myConditional; - SymbolicFactor::shared_ptr mySeparatorFactor; - boost::tie(myConditional, mySeparatorFactor) = + SymbolicConditional::shared_ptr conditional; + SymbolicFactor::shared_ptr separatorFactor; + boost::tie(conditional, separatorFactor) = internal::EliminateSymbolic(symbolicFactors, keyAsOrdering); // Store symbolic elimination results in the parent - myData.parentData->childSymbolicConditionals.push_back(myConditional); - myData.parentData->childSymbolicFactors.push_back(mySeparatorFactor); - myData.parentData->discreteKeys.merge(myData.discreteKeys); + data.parentData->childSymbolicConditionals.push_back(conditional); + data.parentData->childSymbolicFactors.push_back(separatorFactor); + data.parentData->discreteKeys.merge(data.discreteKeys); - sharedNode node = myData.myJTNode; + sharedNode node = data.junctionTreeNode; const FastVector& childConditionals = - myData.childSymbolicConditionals; - node->problemSize_ = (int)(myConditional->size() * symbolicFactors.size()); + data.childSymbolicConditionals; + node->problemSize_ = (int)(conditional->size() * symbolicFactors.size()); // Merge our children if they are in our clique - if our conditional has // exactly one fewer parent than our child's conditional. - const size_t myNrParents = myConditional->nrParents(); + const size_t nrParents = conditional->nrParents(); const size_t nrChildren = node->nrChildren(); assert(childConditionals.size() == nrChildren); // decide which children to merge, as index into children - std::vector nrFrontals = node->nrFrontalsOfChildren(); + std::vector nrChildrenFrontals = node->nrFrontalsOfChildren(); std::vector merge(nrChildren, false); - size_t myNrFrontals = 1; + size_t nrFrontals = 1; for (size_t i = 0; i < nrChildren; i++) { // Check if we should merge the i^th child - if (myNrParents + myNrFrontals == childConditionals[i]->nrParents()) { + if (nrParents + nrFrontals == childConditionals[i]->nrParents()) { const bool myType = - myData.discreteKeys.exists(myConditional->frontals()[0]); + data.discreteKeys.exists(conditional->frontals()[0]); const bool theirType = - myData.discreteKeys.exists(childConditionals[i]->frontals()[0]); + data.discreteKeys.exists(childConditionals[i]->frontals()[0]); if (myType == theirType) { // Increment number of frontal variables - myNrFrontals += nrFrontals[i]; + nrFrontals += nrChildrenFrontals[i]; merge[i] = true; } } @@ -156,7 +156,7 @@ HybridJunctionTree::HybridJunctionTree( // as we go. Gather the created junction tree roots in a dummy Node. typedef HybridConstructorTraversalData Data; Data rootData(0); - rootData.myJTNode = + rootData.junctionTreeNode = boost::make_shared(); // Make a dummy node to gather // the junction tree roots treeTraversal::DepthFirstForest(eliminationTree, rootData, @@ -164,7 +164,7 @@ HybridJunctionTree::HybridJunctionTree( Data::ConstructorTraversalVisitorPostAlg2); // Assign roots from the dummy node - this->addChildrenAsRoots(rootData.myJTNode); + this->addChildrenAsRoots(rootData.junctionTreeNode); // Transfer remaining factors from elimination tree Base::remainingFactors_ = eliminationTree.remainingFactors(); diff --git a/gtsam/hybrid/HybridJunctionTree.h b/gtsam/hybrid/HybridJunctionTree.h index ce9b818e6..cad1e15a1 100644 --- a/gtsam/hybrid/HybridJunctionTree.h +++ b/gtsam/hybrid/HybridJunctionTree.h @@ -17,8 +17,8 @@ #pragma once -#include #include +#include #include namespace gtsam { @@ -49,11 +49,11 @@ class HybridEliminationTree; * \nosubgrouping */ class GTSAM_EXPORT HybridJunctionTree - : public JunctionTree { + : public JunctionTree { public: - typedef JunctionTree + typedef JunctionTree Base; ///< Base class - typedef HybridJunctionTree This; ///< This class + typedef HybridJunctionTree This; ///< This class typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class /** diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i index 612f3abc5..bbe1e2400 100644 --- a/gtsam/hybrid/hybrid.i +++ b/gtsam/hybrid/hybrid.i @@ -38,16 +38,16 @@ class GaussianMixtureFactor : gtsam::HybridFactor { gtsam::DefaultKeyFormatter) const; }; -#include -class GaussianMixtureConditional : gtsam::HybridFactor { - static GaussianMixtureConditional FromConditionals( +#include +class GaussianMixture : gtsam::HybridFactor { + static GaussianMixture FromConditionals( const gtsam::KeyVector& continuousFrontals, const gtsam::KeyVector& continuousParents, const gtsam::DiscreteKeys& discreteParents, const std::vector& conditionalsList); - void print(string s = "GaussianMixtureConditional\n", + void print(string s = "GaussianMixture\n", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; }; @@ -98,15 +98,15 @@ class HybridBayesNet { const gtsam::DotWriter& writer = gtsam::DotWriter()) const; }; -#include -class GaussianHybridFactorGraph { - GaussianHybridFactorGraph(); - GaussianHybridFactorGraph(const gtsam::HybridBayesNet& bayesNet); +#include +class HybridGaussianFactorGraph { + HybridGaussianFactorGraph(); + HybridGaussianFactorGraph(const gtsam::HybridBayesNet& bayesNet); // Building the graph void push_back(const gtsam::HybridFactor* factor); void push_back(const gtsam::HybridConditional* conditional); - void push_back(const gtsam::GaussianHybridFactorGraph& graph); + void push_back(const gtsam::HybridGaussianFactorGraph& graph); void push_back(const gtsam::HybridBayesNet& bayesNet); void push_back(const gtsam::HybridBayesTree& bayesTree); void push_back(const gtsam::GaussianMixtureFactor* gmm); @@ -120,13 +120,13 @@ class GaussianHybridFactorGraph { const gtsam::HybridFactor* at(size_t i) const; void print(string s = "") const; - bool equals(const gtsam::GaussianHybridFactorGraph& fg, double tol = 1e-9) const; + bool equals(const gtsam::HybridGaussianFactorGraph& fg, double tol = 1e-9) const; gtsam::HybridBayesNet* eliminateSequential(); gtsam::HybridBayesNet* eliminateSequential( gtsam::Ordering::OrderingType type); gtsam::HybridBayesNet* eliminateSequential(const gtsam::Ordering& ordering); - pair + pair eliminatePartialSequential(const gtsam::Ordering& ordering); gtsam::HybridBayesTree* eliminateMultifrontal(); @@ -134,7 +134,7 @@ class GaussianHybridFactorGraph { gtsam::Ordering::OrderingType type); gtsam::HybridBayesTree* eliminateMultifrontal( const gtsam::Ordering& ordering); - pair + pair eliminatePartialMultifrontal(const gtsam::Ordering& ordering); string dot( diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h index 42c61767e..4b1c27637 100644 --- a/gtsam/hybrid/tests/Switching.h +++ b/gtsam/hybrid/tests/Switching.h @@ -18,8 +18,8 @@ #include #include -#include #include +#include #include #include #include @@ -33,10 +33,10 @@ namespace gtsam { using MotionModel = BetweenFactor; -inline GaussianHybridFactorGraph::shared_ptr makeSwitchingChain( +inline HybridGaussianFactorGraph::shared_ptr makeSwitchingChain( size_t n, std::function keyFunc = X, std::function dKeyFunc = C) { - GaussianHybridFactorGraph hfg; + HybridGaussianFactorGraph hfg; hfg.add(JacobianFactor(keyFunc(1), I_3x3, Z_3x1)); @@ -55,7 +55,7 @@ inline GaussianHybridFactorGraph::shared_ptr makeSwitchingChain( } } - return boost::make_shared(std::move(hfg)); + return boost::make_shared(std::move(hfg)); } inline std::pair> makeBinaryOrdering( diff --git a/gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp b/gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp index 853353278..552bb18f5 100644 --- a/gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp +++ b/gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /* - * @file testGaussianHybridFactorGraph.cpp + * @file testHybridGaussianFactorGraph.cpp * @date Mar 11, 2022 * @author Fan Jiang */ @@ -20,8 +20,7 @@ #include #include #include -#include -#include +#include #include #include #include @@ -29,7 +28,8 @@ #include #include #include -#include +#include +#include #include #include #include @@ -57,31 +57,17 @@ using gtsam::symbol_shorthand::D; using gtsam::symbol_shorthand::X; using gtsam::symbol_shorthand::Y; -#ifdef HYBRID_DEBUG -#define BOOST_STACKTRACE_GNU_SOURCE_NOT_REQUIRED - -#include // ::signal, ::raise - -#include - -void my_signal_handler(int signum) { - ::signal(signum, SIG_DFL); - std::cout << boost::stacktrace::stacktrace(); - ::raise(SIGABRT); -} -#endif - /* ************************************************************************* */ -TEST(GaussianHybridFactorGraph, creation) { +TEST(HybridGaussianFactorGraph, creation) { HybridConditional test; - GaussianHybridFactorGraph hfg; + HybridGaussianFactorGraph hfg; hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1))); - GaussianMixtureConditional clgc( + GaussianMixture clgc( {X(0)}, {X(1)}, DiscreteKeys(DiscreteKey{C(0), 2}), - GaussianMixtureConditional::Conditionals( + GaussianMixture::Conditionals( C(0), boost::make_shared(X(0), Z_3x1, I_3x3, X(1), I_3x3), @@ -91,8 +77,8 @@ TEST(GaussianHybridFactorGraph, creation) { } /* ************************************************************************* */ -TEST(GaussianHybridFactorGraph, eliminate) { - GaussianHybridFactorGraph hfg; +TEST(HybridGaussianFactorGraph, eliminate) { + HybridGaussianFactorGraph hfg; hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1))); @@ -102,8 +88,8 @@ TEST(GaussianHybridFactorGraph, eliminate) { } /* ************************************************************************* */ -TEST(GaussianHybridFactorGraph, eliminateMultifrontal) { - GaussianHybridFactorGraph hfg; +TEST(HybridGaussianFactorGraph, eliminateMultifrontal) { + HybridGaussianFactorGraph hfg; DiscreteKey c(C(1), 2); @@ -119,8 +105,8 @@ TEST(GaussianHybridFactorGraph, eliminateMultifrontal) { } /* ************************************************************************* */ -TEST(GaussianHybridFactorGraph, eliminateFullSequentialEqualChance) { - GaussianHybridFactorGraph hfg; +TEST(HybridGaussianFactorGraph, eliminateFullSequentialEqualChance) { + HybridGaussianFactorGraph hfg; DiscreteKey c1(C(1), 2); @@ -143,8 +129,8 @@ TEST(GaussianHybridFactorGraph, eliminateFullSequentialEqualChance) { } /* ************************************************************************* */ -TEST(GaussianHybridFactorGraph, eliminateFullSequentialSimple) { - GaussianHybridFactorGraph hfg; +TEST(HybridGaussianFactorGraph, eliminateFullSequentialSimple) { + HybridGaussianFactorGraph hfg; DiscreteKey c1(C(1), 2); @@ -171,8 +157,8 @@ TEST(GaussianHybridFactorGraph, eliminateFullSequentialSimple) { } /* ************************************************************************* */ -TEST(GaussianHybridFactorGraph, eliminateFullMultifrontalSimple) { - GaussianHybridFactorGraph hfg; +TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalSimple) { + HybridGaussianFactorGraph hfg; DiscreteKey c1(C(1), 2); @@ -204,8 +190,8 @@ TEST(GaussianHybridFactorGraph, eliminateFullMultifrontalSimple) { } /* ************************************************************************* */ -TEST(GaussianHybridFactorGraph, eliminateFullMultifrontalCLG) { - GaussianHybridFactorGraph hfg; +TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalCLG) { + HybridGaussianFactorGraph hfg; DiscreteKey c(C(1), 2); @@ -240,8 +226,8 @@ TEST(GaussianHybridFactorGraph, eliminateFullMultifrontalCLG) { * This test is about how to assemble the Bayes Tree roots after we do partial * elimination */ -TEST(GaussianHybridFactorGraph, eliminateFullMultifrontalTwoClique) { - GaussianHybridFactorGraph hfg; +TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalTwoClique) { + HybridGaussianFactorGraph hfg; hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1)); hfg.add(JacobianFactor(X(1), I_3x3, X(2), -I_3x3, Z_3x1)); @@ -290,7 +276,7 @@ TEST(GaussianHybridFactorGraph, eliminateFullMultifrontalTwoClique) { GTSAM_PRINT(ordering_full); HybridBayesTree::shared_ptr hbt; - GaussianHybridFactorGraph::shared_ptr remaining; + HybridGaussianFactorGraph::shared_ptr remaining; std::tie(hbt, remaining) = hfg.eliminatePartialMultifrontal(ordering_full); GTSAM_PRINT(*hbt); @@ -309,7 +295,7 @@ TEST(GaussianHybridFactorGraph, eliminateFullMultifrontalTwoClique) { /* ************************************************************************* */ // TODO(fan): make a graph like Varun's paper one -TEST(GaussianHybridFactorGraph, Switching) { +TEST(HybridGaussianFactorGraph, Switching) { auto N = 12; auto hfg = makeSwitchingChain(N); @@ -381,7 +367,7 @@ TEST(GaussianHybridFactorGraph, Switching) { GTSAM_PRINT(ordering_full); HybridBayesTree::shared_ptr hbt; - GaussianHybridFactorGraph::shared_ptr remaining; + HybridGaussianFactorGraph::shared_ptr remaining; std::tie(hbt, remaining) = hfg->eliminatePartialMultifrontal(ordering_full); // GTSAM_PRINT(*hbt); @@ -417,7 +403,7 @@ TEST(GaussianHybridFactorGraph, Switching) { /* ************************************************************************* */ // TODO(fan): make a graph like Varun's paper one -TEST(GaussianHybridFactorGraph, SwitchingISAM) { +TEST(HybridGaussianFactorGraph, SwitchingISAM) { auto N = 11; auto hfg = makeSwitchingChain(N); @@ -473,7 +459,7 @@ TEST(GaussianHybridFactorGraph, SwitchingISAM) { GTSAM_PRINT(ordering_full); HybridBayesTree::shared_ptr hbt; - GaussianHybridFactorGraph::shared_ptr remaining; + HybridGaussianFactorGraph::shared_ptr remaining; std::tie(hbt, remaining) = hfg->eliminatePartialMultifrontal(ordering_full); // GTSAM_PRINT(*hbt); @@ -499,10 +485,10 @@ TEST(GaussianHybridFactorGraph, SwitchingISAM) { } auto new_fg = makeSwitchingChain(12); - auto isam = HybridISAM(*hbt); + auto isam = HybridGaussianISAM(*hbt); { - GaussianHybridFactorGraph factorGraph; + HybridGaussianFactorGraph factorGraph; factorGraph.push_back(new_fg->at(new_fg->size() - 2)); factorGraph.push_back(new_fg->at(new_fg->size() - 1)); isam.update(factorGraph); @@ -512,7 +498,7 @@ TEST(GaussianHybridFactorGraph, SwitchingISAM) { } /* ************************************************************************* */ -TEST(GaussianHybridFactorGraph, SwitchingTwoVar) { +TEST(HybridGaussianFactorGraph, SwitchingTwoVar) { const int N = 7; auto hfg = makeSwitchingChain(N, X); hfg->push_back(*makeSwitchingChain(N, Y, D)); @@ -582,7 +568,7 @@ TEST(GaussianHybridFactorGraph, SwitchingTwoVar) { } { HybridBayesNet::shared_ptr hbn; - GaussianHybridFactorGraph::shared_ptr remaining; + HybridGaussianFactorGraph::shared_ptr remaining; std::tie(hbn, remaining) = hfg->eliminatePartialSequential(ordering_partial); diff --git a/gtsam/hybrid/tests/testGaussianMixture.cpp b/gtsam/hybrid/tests/testGaussianMixture.cpp new file mode 100644 index 000000000..420e22315 --- /dev/null +++ b/gtsam/hybrid/tests/testGaussianMixture.cpp @@ -0,0 +1,95 @@ +/* ---------------------------------------------------------------------------- + + * 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 testGaussianMixture.cpp + * @brief Unit tests for GaussianMixture class + * @author Varun Agrawal + * @author Fan Jiang + * @author Frank Dellaert + * @date December 2021 + */ + +#include +#include +#include +#include + +#include + +// Include for test suite +#include + +using namespace std; +using namespace gtsam; +using noiseModel::Isotropic; +using symbol_shorthand::M; +using symbol_shorthand::X; + +/* ************************************************************************* */ +/* Check construction of GaussianMixture P(x1 | x2, m1) as well as accessing a + * specific mode i.e. P(x1 | x2, m1=1). + */ +TEST(GaussianMixture, Equals) { + // create a conditional gaussian node + Matrix S1(2, 2); + S1(0, 0) = 1; + S1(1, 0) = 2; + S1(0, 1) = 3; + S1(1, 1) = 4; + + Matrix S2(2, 2); + S2(0, 0) = 6; + S2(1, 0) = 0.2; + S2(0, 1) = 8; + S2(1, 1) = 0.4; + + Matrix R1(2, 2); + R1(0, 0) = 0.1; + R1(1, 0) = 0.3; + R1(0, 1) = 0.0; + R1(1, 1) = 0.34; + + Matrix R2(2, 2); + R2(0, 0) = 0.1; + R2(1, 0) = 0.3; + R2(0, 1) = 0.0; + R2(1, 1) = 0.34; + + SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector2(1.0, 0.34)); + + Vector2 d1(0.2, 0.5), d2(0.5, 0.2); + + auto conditional0 = boost::make_shared(X(1), d1, R1, + X(2), S1, model), + conditional1 = boost::make_shared(X(1), d2, R2, + X(2), S2, model); + + // Create decision tree + DiscreteKey m1(1, 2); + GaussianMixture::Conditionals conditionals( + {m1}, + vector{conditional0, conditional1}); + GaussianMixture mixtureFactor({X(1)}, {X(2)}, {m1}, conditionals); + + // Let's check that this worked: + DiscreteValues mode; + mode[m1.first] = 1; + auto actual = mixtureFactor(mode); + EXPECT(actual == conditional1); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp new file mode 100644 index 000000000..36477218b --- /dev/null +++ b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp @@ -0,0 +1,159 @@ +/* ---------------------------------------------------------------------------- + + * 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 GaussianMixtureFactor.cpp + * @brief Unit tests for GaussianMixtureFactor + * @author Varun Agrawal + * @author Fan Jiang + * @author Frank Dellaert + * @date December 2021 + */ + +#include +#include +#include +#include +#include +#include + +// Include for test suite +#include + +using namespace std; +using namespace gtsam; +using noiseModel::Isotropic; +using symbol_shorthand::M; +using symbol_shorthand::X; + +/* ************************************************************************* */ +// Check iterators of empty mixture. +TEST(GaussianMixtureFactor, Constructor) { + GaussianMixtureFactor factor; + GaussianMixtureFactor::const_iterator const_it = factor.begin(); + CHECK(const_it == factor.end()); + GaussianMixtureFactor::iterator it = factor.begin(); + CHECK(it == factor.end()); +} + +/* ************************************************************************* */ +// "Add" two mixture factors together. +TEST(GaussianMixtureFactor, Sum) { + DiscreteKey m1(1, 2), m2(2, 3); + + auto A1 = Matrix::Zero(2, 1); + auto A2 = Matrix::Zero(2, 2); + auto A3 = Matrix::Zero(2, 3); + auto b = Matrix::Zero(2, 1); + Vector2 sigmas; + sigmas << 1, 2; + auto model = noiseModel::Diagonal::Sigmas(sigmas, true); + + auto f10 = boost::make_shared(X(1), A1, X(2), A2, b); + auto f11 = boost::make_shared(X(1), A1, X(2), A2, b); + auto f20 = boost::make_shared(X(1), A1, X(3), A3, b); + auto f21 = boost::make_shared(X(1), A1, X(3), A3, b); + auto f22 = boost::make_shared(X(1), A1, X(3), A3, b); + std::vector factorsA{f10, f11}; + std::vector factorsB{f20, f21, f22}; + + // TODO(Frank): why specify keys at all? And: keys in factor should be *all* + // keys, deviating from Kevin's scheme. Should we index DT on DiscreteKey? + // Design review! + GaussianMixtureFactor mixtureFactorA({X(1), X(2)}, {m1}, factorsA); + GaussianMixtureFactor mixtureFactorB({X(1), X(3)}, {m2}, factorsB); + + // Check that number of keys is 3 + EXPECT_LONGS_EQUAL(3, mixtureFactorA.keys().size()); + + // Check that number of discrete keys is 1 // TODO(Frank): should not exist? + EXPECT_LONGS_EQUAL(1, mixtureFactorA.discreteKeys().size()); + + // Create sum of two mixture factors: it will be a decision tree now on both + // discrete variables m1 and m2: + GaussianMixtureFactor::Sum sum; + sum += mixtureFactorA; + sum += mixtureFactorB; + + // Let's check that this worked: + Assignment mode; + mode[m1.first] = 1; + mode[m2.first] = 2; + auto actual = sum(mode); + EXPECT(actual.at(0) == f11); + EXPECT(actual.at(1) == f22); +} + +TEST(GaussianMixtureFactor, Printing) { + DiscreteKey m1(1, 2); + auto A1 = Matrix::Zero(2, 1); + auto A2 = Matrix::Zero(2, 2); + auto b = Matrix::Zero(2, 1); + auto f10 = boost::make_shared(X(1), A1, X(2), A2, b); + auto f11 = boost::make_shared(X(1), A1, X(2), A2, b); + std::vector factors{f10, f11}; + + GaussianMixtureFactor mixtureFactor({X(1), X(2)}, {m1}, factors); + + std::string expected = + R"(Hybrid x1 x2; 1 ]{ + Choice(1) + 0 Leaf : + A[x1] = [ + 0; + 0 +] + A[x2] = [ + 0, 0; + 0, 0 +] + b = [ 0 0 ] + No noise model + + 1 Leaf : + A[x1] = [ + 0; + 0 +] + A[x2] = [ + 0, 0; + 0, 0 +] + b = [ 0 0 ] + No noise model + +} +)"; + EXPECT(assert_print_equal(expected, mixtureFactor)); +} + +TEST_UNSAFE(GaussianMixtureFactor, GaussianMixture) { + KeyVector keys; + keys.push_back(X(0)); + keys.push_back(X(1)); + + DiscreteKeys dKeys; + dKeys.emplace_back(M(0), 2); + dKeys.emplace_back(M(1), 2); + + auto gaussians = boost::make_shared(); + GaussianMixture::Conditionals conditionals(gaussians); + GaussianMixture gm({}, keys, dKeys, conditionals); + + EXPECT_LONGS_EQUAL(2, gm.discreteKeys().size()); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ \ No newline at end of file diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index afea63da8..101134c83 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -361,7 +361,7 @@ class FactorGraph { * less than the original, factors at the end will be removed. If the new * size is larger than the original, null factors will be appended. */ - void resize(size_t size) { factors_.resize(size); } + virtual void resize(size_t size) { factors_.resize(size); } /** delete factor without re-arranging indexes by inserting a nullptr pointer */ diff --git a/gtsam/inference/Ordering.cpp b/gtsam/inference/Ordering.cpp index 2ac2c0dde..7b7ff1403 100644 --- a/gtsam/inference/Ordering.cpp +++ b/gtsam/inference/Ordering.cpp @@ -283,6 +283,17 @@ void Ordering::print(const std::string& str, cout.flush(); } +/* ************************************************************************* */ +Ordering::This& Ordering::operator+=(KeyVector& keys) { + this->insert(this->end(), keys.begin(), keys.end()); + return *this; +} + +/* ************************************************************************* */ +bool Ordering::contains(const Key& key) const { + return std::find(this->begin(), this->end(), key) != this->end(); +} + /* ************************************************************************* */ bool Ordering::equals(const Ordering& other, double tol) const { return (*this) == other; diff --git a/gtsam/inference/Ordering.h b/gtsam/inference/Ordering.h index a2d165792..b366b3b3a 100644 --- a/gtsam/inference/Ordering.h +++ b/gtsam/inference/Ordering.h @@ -70,7 +70,23 @@ public: boost::assign_detail::call_push_back(*this))(key); } - /// Invert (not reverse) the ordering - returns a map from key to order position + /** + * @brief Append new keys to the ordering as `ordering += keys`. + * + * @param key + * @return The ordering variable with appended keys. + */ + This& operator+=(KeyVector& keys); + + /// Check if key exists in ordering. + bool contains(const Key& key) const; + + /** + * @brief Invert (not reverse) the ordering - returns a map from key to order + * position. + * + * @return FastMap + */ FastMap invert() const; /// @name Fill-reducing Orderings @{ diff --git a/gtsam/inference/inference.i b/gtsam/inference/inference.i index e8d918a1d..fbdd70fdf 100644 --- a/gtsam/inference/inference.i +++ b/gtsam/inference/inference.i @@ -9,7 +9,7 @@ namespace gtsam { #include #include #include -#include +#include #include @@ -107,36 +107,36 @@ class Ordering { template < FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph, - gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::GaussianHybridFactorGraph}> + gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridGaussianFactorGraph}> static gtsam::Ordering Colamd(const FACTOR_GRAPH& graph); template < FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph, - gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::GaussianHybridFactorGraph}> + gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridGaussianFactorGraph}> static gtsam::Ordering ColamdConstrainedLast( const FACTOR_GRAPH& graph, const gtsam::KeyVector& constrainLast, bool forceOrder = false); template < FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph, - gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::GaussianHybridFactorGraph}> + gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridGaussianFactorGraph}> static gtsam::Ordering ColamdConstrainedFirst( const FACTOR_GRAPH& graph, const gtsam::KeyVector& constrainFirst, bool forceOrder = false); template < FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph, - gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::GaussianHybridFactorGraph}> + gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridGaussianFactorGraph}> static gtsam::Ordering Natural(const FACTOR_GRAPH& graph); template < FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph, - gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::GaussianHybridFactorGraph}> + gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridGaussianFactorGraph}> static gtsam::Ordering Metis(const FACTOR_GRAPH& graph); template < FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph, - gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::GaussianHybridFactorGraph}> + gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridGaussianFactorGraph}> static gtsam::Ordering Create(gtsam::Ordering::OrderingType orderingType, const FACTOR_GRAPH& graph); diff --git a/gtsam/inference/tests/testOrdering.cpp b/gtsam/inference/tests/testOrdering.cpp index 6fdca0d89..1afa1cfde 100644 --- a/gtsam/inference/tests/testOrdering.cpp +++ b/gtsam/inference/tests/testOrdering.cpp @@ -199,6 +199,32 @@ TEST(Ordering, csr_format_3) { EXPECT(adjExpected == adjAcutal); } +/* ************************************************************************* */ +TEST(Ordering, AppendVector) { + using symbol_shorthand::X; + Ordering actual; + KeyVector keys = {X(0), X(1), X(2)}; + actual += keys; + + Ordering expected; + expected += X(0); + expected += X(1); + expected += X(2); + EXPECT(assert_equal(expected, actual)); +} + +/* ************************************************************************* */ +TEST(Ordering, Contains) { + using symbol_shorthand::X; + Ordering ordering; + ordering += X(0); + ordering += X(1); + ordering += X(2); + + EXPECT(ordering.contains(X(1))); + EXPECT(!ordering.contains(X(4))); +} + /* ************************************************************************* */ #ifdef GTSAM_SUPPORT_NESTED_DISSECTION TEST(Ordering, csr_format_4) { diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h index 940ffd882..e4f2950ed 100644 --- a/gtsam/linear/GaussianBayesNet.h +++ b/gtsam/linear/GaussianBayesNet.h @@ -45,7 +45,7 @@ namespace gtsam { /// @name Standard Constructors /// @{ - /** Construct empty factor graph */ + /** Construct empty bayes net */ GaussianBayesNet() {} /** Construct from iterator over conditionals */ diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index 6199f91a7..951577641 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -15,10 +15,10 @@ * @author Christian Potthast, Frank Dellaert */ -#include #include -#include #include +#include +#include #include #ifdef __GNUC__ diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index b2b616dab..11fe1f318 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -24,6 +24,7 @@ #include #include #include +#include #include #include // for std::mt19937_64 diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 5c379beb8..5a29e5d7d 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -697,6 +697,12 @@ namespace gtsam { return robust_->loss(std::sqrt(squared_distance)); } + // NOTE: This is special because in whiten the base version will do the reweighting + // which is incorrect! + double squaredMahalanobisDistance(const Vector& v) const override { + return noise_->squaredMahalanobisDistance(v); + } + // These are really robust iterated re-weighting support functions virtual void WhitenSystem(Vector& b) const; void WhitenSystem(std::vector& A, Vector& b) const override; diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index 943b661d8..fdf156ff9 100644 --- a/gtsam/linear/linear.i +++ b/gtsam/linear/linear.i @@ -279,7 +279,6 @@ virtual class GaussianFactor { virtual class JacobianFactor : gtsam::GaussianFactor { //Constructors JacobianFactor(); - JacobianFactor(const gtsam::GaussianFactor& factor); JacobianFactor(Vector b_in); JacobianFactor(size_t i1, Matrix A1, Vector b, const gtsam::noiseModel::Diagonal* model); @@ -295,6 +294,7 @@ virtual class JacobianFactor : gtsam::GaussianFactor { JacobianFactor(const gtsam::GaussianFactorGraph& graph, const gtsam::Ordering& ordering, const gtsam::VariableSlots& p_variableSlots); + JacobianFactor(const gtsam::GaussianFactor& factor); //Testable void print(string s = "", const gtsam::KeyFormatter& keyFormatter = diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index b974b6cd5..f83ba7831 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -661,12 +661,13 @@ TEST(NoiseModel, robustNoiseDCS) TEST(NoiseModel, robustNoiseL2WithDeadZone) { double dead_zone_size = 1.0; - SharedNoiseModel robust = noiseModel::Robust::Create( + auto robust = noiseModel::Robust::Create( noiseModel::mEstimator::L2WithDeadZone::Create(dead_zone_size), Unit::Create(3)); for (int i = 0; i < 5; i++) { - Vector3 error = Vector3(i, 0, 0); + Vector error = Vector3(i, 0, 0); + robust->WhitenSystem(error); DOUBLES_EQUAL(std::fmax(0, i - dead_zone_size) * i, robust->squaredMahalanobisDistance(error), 1e-8); } diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index cc3fdaf34..eb1c0233f 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -207,9 +207,11 @@ class GTSAM_EXPORT GncOptimizer { std::cout << "GNC Optimizer stopped because all measurements are already known to be inliers or outliers" << std::endl; } + if (params_.verbosity >= GncParameters::Verbosity::MU) { + std::cout << "mu: " << mu << std::endl; + } if (params_.verbosity >= GncParameters::Verbosity::VALUES) { result.print("result\n"); - std::cout << "mu: " << mu << std::endl; } return result; } @@ -218,12 +220,16 @@ class GTSAM_EXPORT GncOptimizer { for (iter = 0; iter < params_.maxIterations; iter++) { // display info - if (params_.verbosity >= GncParameters::Verbosity::VALUES) { + if (params_.verbosity >= GncParameters::Verbosity::MU) { std::cout << "iter: " << iter << std::endl; - result.print("result\n"); std::cout << "mu: " << mu << std::endl; + } + if (params_.verbosity >= GncParameters::Verbosity::WEIGHTS) { std::cout << "weights: " << weights_ << std::endl; } + if (params_.verbosity >= GncParameters::Verbosity::VALUES) { + result.print("result\n"); + } // weights update weights_ = calculateWeights(result, mu); @@ -255,10 +261,12 @@ class GTSAM_EXPORT GncOptimizer { if (params_.verbosity >= GncParameters::Verbosity::SUMMARY) { std::cout << "final iterations: " << iter << std::endl; std::cout << "final mu: " << mu << std::endl; - std::cout << "final weights: " << weights_ << std::endl; std::cout << "previous cost: " << prev_cost << std::endl; std::cout << "current cost: " << cost << std::endl; } + if (params_.verbosity >= GncParameters::Verbosity::WEIGHTS) { + std::cout << "final weights: " << weights_ << std::endl; + } return result; } @@ -293,6 +301,11 @@ class GTSAM_EXPORT GncOptimizer { std::min(mu_init, barcSq_[k] / (2 * rk - barcSq_[k]) ) : mu_init; } } + if (mu_init >= 0 && mu_init < 1e-6){ + mu_init = 1e-6; // if mu ~ 0 (but positive), that means we have measurements with large errors, + // i.e., rk > barcSq_[k] and rk very large, hence we threshold to 1e-6 to avoid mu = 0 + } + return mu_init > 0 && !std::isinf(mu_init) ? mu_init : -1; // if mu <= 0 or mu = inf, return -1, // which leads to termination of the main gnc loop. In this case, all residuals are already below the threshold // and there is no need to robustify (TLS = least squares) @@ -340,8 +353,10 @@ class GTSAM_EXPORT GncOptimizer { bool checkCostConvergence(const double cost, const double prev_cost) const { bool costConverged = std::fabs(cost - prev_cost) / std::max(prev_cost, 1e-7) < params_.relativeCostTol; - if (costConverged && params_.verbosity >= GncParameters::Verbosity::SUMMARY) - std::cout << "checkCostConvergence = true " << std::endl; + if (costConverged && params_.verbosity >= GncParameters::Verbosity::SUMMARY){ + std::cout << "checkCostConvergence = true (prev. cost = " << prev_cost + << ", curr. cost = " << cost << ")" << std::endl; + } return costConverged; } @@ -436,18 +451,16 @@ class GTSAM_EXPORT GncOptimizer { return weights; } case GncLossType::TLS: { // use eq (14) in GNC paper - double upperbound = (mu + 1) / mu * barcSq_.maxCoeff(); - double lowerbound = mu / (mu + 1) * barcSq_.minCoeff(); for (size_t k : unknownWeights) { if (nfg_[k]) { double u2_k = nfg_[k]->error(currentEstimate); // squared (and whitened) residual - if (u2_k >= upperbound) { + double upperbound = (mu + 1) / mu * barcSq_[k]; + double lowerbound = mu / (mu + 1) * barcSq_[k]; + weights[k] = std::sqrt(barcSq_[k] * mu * (mu + 1) / u2_k) - mu; + if (u2_k >= upperbound || weights[k] < 0) { weights[k] = 0; - } else if (u2_k <= lowerbound) { + } else if (u2_k <= lowerbound || weights[k] > 1) { weights[k] = 1; - } else { - weights[k] = std::sqrt(barcSq_[k] * mu * (mu + 1) / u2_k) - - mu; } } } diff --git a/gtsam/nonlinear/GncParams.h b/gtsam/nonlinear/GncParams.h index 086f08acc..1f324ae38 100644 --- a/gtsam/nonlinear/GncParams.h +++ b/gtsam/nonlinear/GncParams.h @@ -48,6 +48,8 @@ class GTSAM_EXPORT GncParams { enum Verbosity { SILENT = 0, SUMMARY, + MU, + WEIGHTS, VALUES }; diff --git a/gtsam/sam/BearingRangeFactor.h b/gtsam/sam/BearingRangeFactor.h index da352f2e9..9874760c4 100644 --- a/gtsam/sam/BearingRangeFactor.h +++ b/gtsam/sam/BearingRangeFactor.h @@ -79,9 +79,8 @@ class BearingRangeFactor { std::vector Hs(2); const auto &keys = Factor::keys(); - const Vector error = unwhitenedError( - {{keys[0], genericValue(a1)}, {keys[1], genericValue(a2)}}, - Hs); + const Vector error = this->unwhitenedError( + {{keys[0], genericValue(a1)}, {keys[1], genericValue(a2)}}, Hs); if (H1) *H1 = Hs[0]; if (H2) *H2 = Hs[1]; return error; diff --git a/gtsam/sam/sam.i b/gtsam/sam/sam.i index 90c319ede..a46a6de9e 100644 --- a/gtsam/sam/sam.i +++ b/gtsam/sam/sam.i @@ -108,5 +108,11 @@ typedef gtsam::BearingRangeFactor BearingRangeFactorPose2; +typedef gtsam::BearingRangeFactor + BearingRangeFactor3D; +typedef gtsam::BearingRangeFactor + BearingRangeFactorPose3; } // namespace gtsam diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp index 810fe7de9..c7ef2e526 100644 --- a/gtsam/sfm/TranslationRecovery.cpp +++ b/gtsam/sfm/TranslationRecovery.cpp @@ -119,21 +119,24 @@ void TranslationRecovery::addPrior( graph->emplace_shared>(edge->key1(), Point3(0, 0, 0), priorNoiseModel); - // Add between factors for optional relative translations. - for (auto edge : betweenTranslations) { - graph->emplace_shared>( - edge.key1(), edge.key2(), edge.measured(), edge.noiseModel()); - } - // Add a scale prior only if no other between factors were added. if (betweenTranslations.empty()) { graph->emplace_shared>( edge->key2(), scale * edge->measured().point3(), edge->noiseModel()); + return; + } + + // Add between factors for optional relative translations. + for (auto prior_edge : betweenTranslations) { + graph->emplace_shared>( + prior_edge.key1(), prior_edge.key2(), prior_edge.measured(), + prior_edge.noiseModel()); } } Values TranslationRecovery::initializeRandomly( const std::vector> &relativeTranslations, + const std::vector> &betweenTranslations, std::mt19937 *rng, const Values &initialValues) const { uniform_real_distribution randomVal(-1, 1); // Create a lambda expression that checks whether value exists and randomly @@ -155,14 +158,20 @@ Values TranslationRecovery::initializeRandomly( insert(edge.key1()); insert(edge.key2()); } + // There may be nodes in betweenTranslations that do not have a measurement. + for (auto edge : betweenTranslations) { + insert(edge.key1()); + insert(edge.key2()); + } return initial; } Values TranslationRecovery::initializeRandomly( const std::vector> &relativeTranslations, + const std::vector> &betweenTranslations, const Values &initialValues) const { - return initializeRandomly(relativeTranslations, &kRandomNumberGenerator, - initialValues); + return initializeRandomly(relativeTranslations, betweenTranslations, + &kRandomNumberGenerator, initialValues); } Values TranslationRecovery::run( @@ -187,8 +196,8 @@ Values TranslationRecovery::run( &graph); // Uses initial values from params if provided. - Values initial = - initializeRandomly(nonzeroRelativeTranslations, initialValues); + Values initial = initializeRandomly( + nonzeroRelativeTranslations, nonzeroBetweenTranslations, initialValues); // If there are no valid edges, but zero-distance edges exist, initialize one // of the nodes in a connected component of zero-distance edges. diff --git a/gtsam/sfm/TranslationRecovery.h b/gtsam/sfm/TranslationRecovery.h index 7863f5133..44a5ef43e 100644 --- a/gtsam/sfm/TranslationRecovery.h +++ b/gtsam/sfm/TranslationRecovery.h @@ -112,12 +112,15 @@ class TranslationRecovery { * * @param relativeTranslations unit translation directions between * translations to be estimated + * @param betweenTranslations relative translations (with scale) between 2 + * points in world coordinate frame known a priori. * @param rng random number generator * @param intialValues (optional) initial values from a prior * @return Values */ Values initializeRandomly( const std::vector> &relativeTranslations, + const std::vector> &betweenTranslations, std::mt19937 *rng, const Values &initialValues = Values()) const; /** @@ -125,11 +128,14 @@ class TranslationRecovery { * * @param relativeTranslations unit translation directions between * translations to be estimated + * @param betweenTranslations relative translations (with scale) between 2 + * points in world coordinate frame known a priori. * @param initialValues (optional) initial values from a prior * @return Values */ Values initializeRandomly( const std::vector> &relativeTranslations, + const std::vector> &betweenTranslations, const Values &initialValues = Values()) const; /** @@ -137,11 +143,15 @@ class TranslationRecovery { * * @param relativeTranslations the relative translations, in world coordinate * frames, vector of BinaryMeasurements of Unit3, where each key of a - * measurement is a point in 3D. + * measurement is a point in 3D. If a relative translation magnitude is zero, + * it is treated as a hard same-point constraint (the result of all nodes + * connected by a zero-magnitude edge will be the same). * @param scale scale for first relative translation which fixes gauge. * The scale is only used if betweenTranslations is empty. * @param betweenTranslations relative translations (with scale) between 2 - * points in world coordinate frame known a priori. + * points in world coordinate frame known a priori. Unlike + * relativeTranslations, zero-magnitude betweenTranslations are not treated as + * hard constraints. * @param initialValues intial values for optimization. Initializes randomly * if not provided. * @return Values diff --git a/gtsam_unstable/slam/PoseToPointFactor.h b/gtsam_unstable/slam/PoseToPointFactor.h index cab48e506..b9b2ad5ce 100644 --- a/gtsam_unstable/slam/PoseToPointFactor.h +++ b/gtsam_unstable/slam/PoseToPointFactor.h @@ -61,6 +61,12 @@ class PoseToPointFactor : public NoiseModelFactor2 { traits::Equals(this->measured_, e->measured_, tol); } + /// @return a deep copy of this factor + gtsam::NonlinearFactor::shared_ptr clone() const override { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + /** implement functions needed to derive from Factor */ /** vector of errors diff --git a/gtsam_unstable/timing/timeShonanAveraging.cpp b/gtsam_unstable/timing/timeShonanAveraging.cpp index e932b6400..74dd04a78 100644 --- a/gtsam_unstable/timing/timeShonanAveraging.cpp +++ b/gtsam_unstable/timing/timeShonanAveraging.cpp @@ -79,9 +79,9 @@ void saveG2oResult(string name, const Values& values, std::map poses myfile << "VERTEX_SE3:QUAT" << " "; myfile << i << " "; myfile << poses[i].x() << " " << poses[i].y() << " " << poses[i].z() << " "; - Vector quaternion = Rot3(R).quaternion(); - myfile << quaternion(3) << " " << quaternion(2) << " " << quaternion(1) - << " " << quaternion(0); + Quaternion quaternion = Rot3(R).toQuaternion(); + myfile << quaternion.x() << " " << quaternion.y() << " " << quaternion.z() + << " " << quaternion.w(); myfile << "\n"; } myfile.close(); diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index cba206d11..0d63c6c54 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -104,7 +104,8 @@ copy_directory("${CMAKE_CURRENT_SOURCE_DIR}/gtsam" # Hack to get python test and util files copied every time they are modified file(GLOB GTSAM_PYTHON_TEST_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam/tests/*.py") foreach(test_file ${GTSAM_PYTHON_TEST_FILES}) - configure_file(${test_file} "${GTSAM_MODULE_PATH}/tests/${test_file}" COPYONLY) + get_filename_component(test_file_name ${test_file} NAME) + configure_file(${test_file} "${GTSAM_MODULE_PATH}/tests/${test_file_name}" COPYONLY) endforeach() file(GLOB GTSAM_PYTHON_UTIL_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam/utils/*.py") foreach(util_file ${GTSAM_PYTHON_UTIL_FILES}) diff --git a/python/gtsam/examples/Pose3ISAM2Example.py b/python/gtsam/examples/Pose3ISAM2Example.py index cb71813c5..986bd5b9c 100644 --- a/python/gtsam/examples/Pose3ISAM2Example.py +++ b/python/gtsam/examples/Pose3ISAM2Example.py @@ -34,7 +34,10 @@ def report_on_progress(graph: gtsam.NonlinearFactorGraph, current_estimate: gtsa # Plot the newly updated iSAM2 inference. fig = plt.figure(0) - axes = fig.gca(projection='3d') + if not fig.axes: + axes = fig.add_subplot(projection='3d') + else: + axes = fig.axes[0] plt.cla() i = 1 diff --git a/python/gtsam/examples/VisualISAM2Example.py b/python/gtsam/examples/VisualISAM2Example.py index 4b480fab7..60d4fb376 100644 --- a/python/gtsam/examples/VisualISAM2Example.py +++ b/python/gtsam/examples/VisualISAM2Example.py @@ -33,7 +33,10 @@ def visual_ISAM2_plot(result): fignum = 0 fig = plt.figure(fignum) - axes = fig.gca(projection='3d') + if not fig.axes: + axes = fig.add_subplot(projection='3d') + else: + axes = fig.axes[0] plt.cla() # Plot points diff --git a/python/gtsam/tests/test_Factors.py b/python/gtsam/tests/test_Factors.py index 3ec8648a4..a5688eec8 100644 --- a/python/gtsam/tests/test_Factors.py +++ b/python/gtsam/tests/test_Factors.py @@ -11,9 +11,8 @@ Author: Varun Agrawal """ import unittest -import numpy as np - import gtsam +import numpy as np from gtsam.utils.test_case import GtsamTestCase @@ -21,6 +20,7 @@ class TestNonlinearEquality2Factor(GtsamTestCase): """ Test various instantiations of NonlinearEquality2. """ + def test_point3(self): """Test for Point3 version.""" factor = gtsam.NonlinearEquality2Point3(0, 1) @@ -30,5 +30,23 @@ class TestNonlinearEquality2Factor(GtsamTestCase): np.testing.assert_allclose(error, np.zeros(3)) +class TestJacobianFactor(GtsamTestCase): + """Test JacobianFactor""" + + def test_gaussian_factor_graph(self): + """Test construction from GaussianFactorGraph.""" + gfg = gtsam.GaussianFactorGraph() + jf = gtsam.JacobianFactor(gfg) + self.assertIsInstance(jf, gtsam.JacobianFactor) + + nfg = gtsam.NonlinearFactorGraph() + nfg.push_back(gtsam.PriorFactorDouble(1, 0.0, gtsam.noiseModel.Isotropic.Sigma(1, 1.0))) + values = gtsam.Values() + values.insert(1, 0.0) + gfg = nfg.linearize(values) + jf = gtsam.JacobianFactor(gfg) + self.assertIsInstance(jf, gtsam.JacobianFactor) + + if __name__ == "__main__": unittest.main() diff --git a/python/gtsam/tests/test_HybridFactorGraph.py b/python/gtsam/tests/test_HybridFactorGraph.py index 895c9e14e..781cfd924 100644 --- a/python/gtsam/tests/test_HybridFactorGraph.py +++ b/python/gtsam/tests/test_HybridFactorGraph.py @@ -20,8 +20,8 @@ from gtsam.symbol_shorthand import C, X from gtsam.utils.test_case import GtsamTestCase -class TestGaussianHybridFactorGraph(GtsamTestCase): - """Unit tests for GaussianHybridFactorGraph.""" +class TestHybridGaussianFactorGraph(GtsamTestCase): + """Unit tests for HybridGaussianFactorGraph.""" def test_create(self): """Test contruction of hybrid factor graph.""" @@ -36,20 +36,20 @@ class TestGaussianHybridFactorGraph(GtsamTestCase): gmf = gtsam.GaussianMixtureFactor.FromFactors([X(0)], dk, [jf1, jf2]) - hfg = gtsam.GaussianHybridFactorGraph() + hfg = gtsam.HybridGaussianFactorGraph() hfg.add(jf1) hfg.add(jf2) hfg.push_back(gmf) hbn = hfg.eliminateSequential( - gtsam.Ordering.ColamdConstrainedLastGaussianHybridFactorGraph( + gtsam.Ordering.ColamdConstrainedLastHybridGaussianFactorGraph( hfg, [C(0)])) # print("hbn = ", hbn) self.assertEqual(hbn.size(), 2) mixture = hbn.at(0).inner() - self.assertIsInstance(mixture, gtsam.GaussianMixtureConditional) + self.assertIsInstance(mixture, gtsam.GaussianMixture) self.assertEqual(len(mixture.keys()), 2) discrete_conditional = hbn.at(hbn.size() - 1).inner() diff --git a/python/gtsam/tests/test_Pose3.py b/python/gtsam/tests/test_Pose3.py index cde71de53..4464e8d47 100644 --- a/python/gtsam/tests/test_Pose3.py +++ b/python/gtsam/tests/test_Pose3.py @@ -19,6 +19,62 @@ from gtsam import Point3, Pose3, Rot3, Point3Pairs from gtsam.utils.test_case import GtsamTestCase +def numerical_derivative_pose(pose, method, delta=1e-5): + jacobian = np.zeros((6, 6)) + for idx in range(6): + xplus = np.zeros(6) + xplus[idx] = delta + xminus = np.zeros(6) + xminus[idx] = -delta + pose_plus = pose.retract(xplus).__getattribute__(method)() + pose_minus = pose.retract(xminus).__getattribute__(method)() + jacobian[:, idx] = pose_minus.localCoordinates(pose_plus) / (2 * delta) + return jacobian + + +def numerical_derivative_2_poses(pose, other_pose, method, delta=1e-5, inputs=()): + jacobian = np.zeros((6, 6)) + other_jacobian = np.zeros((6, 6)) + for idx in range(6): + xplus = np.zeros(6) + xplus[idx] = delta + xminus = np.zeros(6) + xminus[idx] = -delta + + pose_plus = pose.retract(xplus).__getattribute__(method)(*inputs, other_pose) + pose_minus = pose.retract(xminus).__getattribute__(method)(*inputs, other_pose) + jacobian[:, idx] = pose_minus.localCoordinates(pose_plus) / (2 * delta) + + other_pose_plus = pose.__getattribute__(method)(*inputs, other_pose.retract(xplus)) + other_pose_minus = pose.__getattribute__(method)(*inputs, other_pose.retract(xminus)) + other_jacobian[:, idx] = other_pose_minus.localCoordinates(other_pose_plus) / (2 * delta) + return jacobian, other_jacobian + + +def numerical_derivative_pose_point(pose, point, method, delta=1e-5): + jacobian = np.zeros((3, 6)) + point_jacobian = np.zeros((3, 3)) + for idx in range(6): + xplus = np.zeros(6) + xplus[idx] = delta + xminus = np.zeros(6) + xminus[idx] = -delta + + point_plus = pose.retract(xplus).__getattribute__(method)(point) + point_minus = pose.retract(xminus).__getattribute__(method)(point) + jacobian[:, idx] = (point_plus - point_minus) / (2 * delta) + + if idx < 3: + xplus = np.zeros(3) + xplus[idx] = delta + xminus = np.zeros(3) + xminus[idx] = -delta + point_plus = pose.__getattribute__(method)(point + xplus) + point_minus = pose.__getattribute__(method)(point + xminus) + point_jacobian[:, idx] = (point_plus - point_minus) / (2 * delta) + return jacobian, point_jacobian + + class TestPose3(GtsamTestCase): """Test selected Pose3 methods.""" @@ -30,6 +86,47 @@ class TestPose3(GtsamTestCase): actual = T2.between(T3) self.gtsamAssertEquals(actual, expected, 1e-6) + #test jacobians + jacobian = np.zeros((6, 6), order='F') + jacobian_other = np.zeros((6, 6), order='F') + T2.between(T3, jacobian, jacobian_other) + jacobian_numerical, jacobian_numerical_other = numerical_derivative_2_poses(T2, T3, 'between') + self.gtsamAssertEquals(jacobian, jacobian_numerical) + self.gtsamAssertEquals(jacobian_other, jacobian_numerical_other) + + def test_inverse(self): + """Test between method.""" + pose = Pose3(Rot3.Rodrigues(0, 0, -math.pi/2), Point3(2, 4, 0)) + expected = Pose3(Rot3.Rodrigues(0, 0, math.pi/2), Point3(4, -2, 0)) + actual = pose.inverse() + self.gtsamAssertEquals(actual, expected, 1e-6) + + #test jacobians + jacobian = np.zeros((6, 6), order='F') + pose.inverse(jacobian) + jacobian_numerical = numerical_derivative_pose(pose, 'inverse') + self.gtsamAssertEquals(jacobian, jacobian_numerical) + + def test_slerp(self): + """Test slerp method.""" + pose0 = gtsam.Pose3() + pose1 = Pose3(Rot3.Rodrigues(0, 0, -math.pi/2), Point3(2, 4, 0)) + actual_alpha_0 = pose0.slerp(0, pose1) + self.gtsamAssertEquals(actual_alpha_0, pose0) + actual_alpha_1 = pose0.slerp(1, pose1) + self.gtsamAssertEquals(actual_alpha_1, pose1) + actual_alpha_half = pose0.slerp(0.5, pose1) + expected_alpha_half = Pose3(Rot3.Rodrigues(0, 0, -math.pi/4), Point3(0.17157288, 2.41421356, 0)) + self.gtsamAssertEquals(actual_alpha_half, expected_alpha_half, tol=1e-6) + + # test jacobians + jacobian = np.zeros((6, 6), order='F') + jacobian_other = np.zeros((6, 6), order='F') + pose0.slerp(0.5, pose1, jacobian, jacobian_other) + jacobian_numerical, jacobian_numerical_other = numerical_derivative_2_poses(pose0, pose1, 'slerp', inputs=[0.5]) + self.gtsamAssertEquals(jacobian, jacobian_numerical) + self.gtsamAssertEquals(jacobian_other, jacobian_numerical_other) + def test_transformTo(self): """Test transformTo method.""" pose = Pose3(Rot3.Rodrigues(0, 0, -math.pi/2), Point3(2, 4, 0)) @@ -37,6 +134,15 @@ class TestPose3(GtsamTestCase): expected = Point3(2, 1, 10) self.gtsamAssertEquals(actual, expected, 1e-6) + #test jacobians + point = Point3(3, 2, 10) + jacobian_pose = np.zeros((3, 6), order='F') + jacobian_point = np.zeros((3, 3), order='F') + pose.transformTo(point, jacobian_pose, jacobian_point) + jacobian_numerical_pose, jacobian_numerical_point = numerical_derivative_pose_point(pose, point, 'transformTo') + self.gtsamAssertEquals(jacobian_pose, jacobian_numerical_pose) + self.gtsamAssertEquals(jacobian_point, jacobian_numerical_point) + # multi-point version points = np.stack([Point3(3, 2, 10), Point3(3, 2, 10)]).T actual_array = pose.transformTo(points) @@ -51,6 +157,15 @@ class TestPose3(GtsamTestCase): expected = Point3(3, 2, 10) self.gtsamAssertEquals(actual, expected, 1e-6) + #test jacobians + point = Point3(3, 2, 10) + jacobian_pose = np.zeros((3, 6), order='F') + jacobian_point = np.zeros((3, 3), order='F') + pose.transformFrom(point, jacobian_pose, jacobian_point) + jacobian_numerical_pose, jacobian_numerical_point = numerical_derivative_pose_point(pose, point, 'transformFrom') + self.gtsamAssertEquals(jacobian_pose, jacobian_numerical_pose) + self.gtsamAssertEquals(jacobian_point, jacobian_numerical_point) + # multi-point version points = np.stack([Point3(2, 1, 10), Point3(2, 1, 10)]).T actual_array = pose.transformFrom(points) @@ -122,4 +237,4 @@ class TestPose3(GtsamTestCase): if __name__ == "__main__": - unittest.main() + unittest.main() \ No newline at end of file diff --git a/python/gtsam/tests/test_sam.py b/python/gtsam/tests/test_sam.py index e01b9c1d1..fd8da5308 100644 --- a/python/gtsam/tests/test_sam.py +++ b/python/gtsam/tests/test_sam.py @@ -50,6 +50,34 @@ class TestSam(GtsamTestCase): self.assertEqual(range_measurement, measurement.range()) self.gtsamAssertEquals(bearing_measurement, measurement.bearing()) + def test_BearingRangeFactor3D(self): + """ + Test that `measured` works as expected for BearingRangeFactor3D. + """ + bearing_measurement = gtsam.Unit3() + range_measurement = 10.0 + factor = gtsam.BearingRangeFactor3D( + 1, 2, bearing_measurement, range_measurement, + gtsam.noiseModel.Isotropic.Sigma(3, 1)) + measurement = factor.measured() + + self.assertEqual(range_measurement, measurement.range()) + self.gtsamAssertEquals(bearing_measurement, measurement.bearing()) + + def test_BearingRangeFactorPose3(self): + """ + Test that `measured` works as expected for BearingRangeFactorPose3. + """ + range_measurement = 10.0 + bearing_measurement = gtsam.Unit3() + factor = gtsam.BearingRangeFactorPose3( + 1, 2, bearing_measurement, range_measurement, + gtsam.noiseModel.Isotropic.Sigma(3, 1)) + measurement = factor.measured() + + self.assertEqual(range_measurement, measurement.range()) + self.gtsamAssertEquals(bearing_measurement, measurement.bearing()) + if __name__ == "__main__": unittest.main() diff --git a/python/gtsam/utils/plot.py b/python/gtsam/utils/plot.py index a4d19f72b..880c436e8 100644 --- a/python/gtsam/utils/plot.py +++ b/python/gtsam/utils/plot.py @@ -333,7 +333,10 @@ def plot_point3( """ fig = plt.figure(fignum) - axes = fig.gca(projection='3d') + if not fig.axes: + axes = fig.add_subplot(projection='3d') + else: + axes = fig.axes[0] plot_point3_on_axes(axes, point, linespec, P) axes.set_xlabel(axis_labels[0]) @@ -388,7 +391,7 @@ def plot_3d_points(fignum, fig = plt.figure(fignum) fig.suptitle(title) - fig.canvas.set_window_title(title.lower()) + fig.canvas.manager.set_window_title(title.lower()) def plot_pose3_on_axes(axes, pose, axis_length=0.1, P=None, scale=1): @@ -490,7 +493,10 @@ def plot_trajectory( axis_labels (iterable[string]): List of axis labels to set. """ fig = plt.figure(fignum) - axes = fig.gca(projection='3d') + if not fig.axes: + axes = fig.add_subplot(projection='3d') + else: + axes = fig.axes[0] axes.set_xlabel(axis_labels[0]) axes.set_ylabel(axis_labels[1]) @@ -522,7 +528,7 @@ def plot_trajectory( plot_pose3_on_axes(axes, pose, P=covariance, axis_length=scale) fig.suptitle(title) - fig.canvas.set_window_title(title.lower()) + fig.canvas.manager.set_window_title(title.lower()) def plot_incremental_trajectory(fignum: int, @@ -545,7 +551,10 @@ def plot_incremental_trajectory(fignum: int, Used to create animation effect. """ fig = plt.figure(fignum) - axes = fig.gca(projection='3d') + if not fig.axes: + axes = fig.add_subplot(projection='3d') + else: + axes = fig.axes[0] poses = gtsam.utilities.allPose3s(values) keys = gtsam.KeyVector(poses.keys()) diff --git a/tests/testRobust.cpp b/tests/testRobust.cpp new file mode 100644 index 000000000..ad361f6d9 --- /dev/null +++ b/tests/testRobust.cpp @@ -0,0 +1,53 @@ +/* ---------------------------------------------------------------------------- + + * 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 testRobust.cpp + * @brief Unit tests for Robust loss functions + * @author Fan Jiang + * @author Yetong Zhang + * @date Apr 7, 2022 + **/ + +#include +#include +#include +#include +#include +#include + +using namespace gtsam; + +TEST(RobustNoise, loss) { + // Keys. + gtsam::Key x1_key = 1; + gtsam::Key x2_key = 2; + + auto gm = noiseModel::mEstimator::GemanMcClure::Create(1.0); + auto noise = noiseModel::Robust::Create(gm, noiseModel::Unit::Create(1)); + + auto factor = PriorFactor(x1_key, 0.0, noise); + auto between_factor = BetweenFactor(x1_key, x2_key, 0.0, noise); + + Values values; + values.insert(x1_key, 10.0); + values.insert(x2_key, 0.0); + + EXPECT_DOUBLES_EQUAL(0.49505, factor.error(values), 1e-5); + EXPECT_DOUBLES_EQUAL(0.49505, between_factor.error(values), 1e-5); + EXPECT_DOUBLES_EQUAL(0.49505, gm->loss(10.0), 1e-5); +} + +int main() { + TestResult tr; + + return TestRegistry::runAllTests(tr); +} diff --git a/tests/testTranslationRecovery.cpp b/tests/testTranslationRecovery.cpp index 5dd319d30..15f1caa1b 100644 --- a/tests/testTranslationRecovery.cpp +++ b/tests/testTranslationRecovery.cpp @@ -323,6 +323,36 @@ TEST(TranslationRecovery, ThreePosesWithOneHardConstraint) { EXPECT(assert_equal(Point3(2, 0, 0), result.at(1), 1e-4)); EXPECT(assert_equal(Point3(1, -1, 0), result.at(3), 1e-4)); } + +TEST(TranslationRecovery, NodeWithBetweenFactorAndNoMeasurements) { + // Checks that valid results are obtained when a between translation edge is + // provided with a node that does not have any other relative translations. + Values poses; + poses.insert(0, Pose3(Rot3(), Point3(0, 0, 0))); + poses.insert(1, Pose3(Rot3(), Point3(2, 0, 0))); + poses.insert(3, Pose3(Rot3(), Point3(1, -1, 0))); + poses.insert(4, Pose3(Rot3(), Point3(1, 2, 1))); + + auto relativeTranslations = TranslationRecovery::SimulateMeasurements( + poses, {{0, 1}, {0, 3}, {1, 3}}); + + std::vector> betweenTranslations; + betweenTranslations.emplace_back(0, 1, Point3(2, 0, 0), + noiseModel::Constrained::All(3, 1e2)); + // Node 4 only has this between translation prior, no relative translations. + betweenTranslations.emplace_back(0, 4, Point3(1, 2, 1)); + + TranslationRecovery algorithm; + auto result = + algorithm.run(relativeTranslations, /*scale=*/0.0, betweenTranslations); + + // Check result + EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-4)); + EXPECT(assert_equal(Point3(2, 0, 0), result.at(1), 1e-4)); + EXPECT(assert_equal(Point3(1, -1, 0), result.at(3), 1e-4)); + EXPECT(assert_equal(Point3(1, 2, 1), result.at(4), 1e-4)); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/wrap/cmake/MatlabWrap.cmake b/wrap/cmake/MatlabWrap.cmake index 3cb058102..eaffcc059 100644 --- a/wrap/cmake/MatlabWrap.cmake +++ b/wrap/cmake/MatlabWrap.cmake @@ -242,6 +242,13 @@ function(wrap_library_internal interfaceHeader moduleName linkLibraries extraInc find_package(PythonInterp ${WRAP_PYTHON_VERSION} EXACT) find_package(PythonLibs ${WRAP_PYTHON_VERSION} EXACT) + # Set the path separator for PYTHONPATH + if(UNIX) + set(GTWRAP_PATH_SEPARATOR ":") + else() + set(GTWRAP_PATH_SEPARATOR ";") + endif() + add_custom_command( OUTPUT ${generated_cpp_file} DEPENDS ${interfaceHeader} ${module_library_target} ${otherLibraryTargets}