12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge branch 'develop' into feature/nonlinear-hybrid

release/4.3a0
Varun Agrawal 2022-07-25 23:10:08 -04:00
parent e711a62e2d a700c87504
commit 3780b8c21e
77 changed files with 1816 additions and 593 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
.github/workflows/build-macos.yml vendored
View File

@ -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

15
.github/workflows/build-python.yml vendored
View File

@ -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: |

159
examples/TriangulationLOSTExample.cpp Normal file
View File

@ -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 <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/triangulation.h>
#include <chrono>
#include <iostream>
#include <random>
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<PinholeCamera<Cal3_S2>>* cameras,
std::vector<Pose3>* 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<double> rand_xy(minXY, maxXY);
std::uniform_real_distribution<double> 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<PinholeCamera<Cal3_S2>>* cameras,
std::vector<Pose3>* poses, Point3* point,
Point2Vector* measurements) {
Pose3 pose1;
Pose3 pose2(Rot3(), Point3(5., 0., -5.));
Cal3_S2 identityK;
PinholeCamera<Cal3_S2> camera1(pose1, identityK);
PinholeCamera<Cal3_S2> 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<double> 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<PinholeCamera<Cal3_S2>> cameras;
std::vector<Pose3> 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<Cal3_S2> calib = boost::make_shared<Cal3_S2>();
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<Point3> estimateLOST = triangulatePoint3<Cal3_S2>(
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<Point3> estimateDLT = triangulatePoint3<Cal3_S2>(
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<Point3> estimateDLTOpt = triangulatePoint3<Cal3_S2>(
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");
}

1
gtsam/base/ConcurrentMap.h
View File

@ -113,6 +113,7 @@ private:
template<class Archive>
void load(Archive& ar, const unsigned int /*version*/)
{
this->clear();
// Load into STL container and then fill our map
FastVector<std::pair<KEY, VALUE> > map;
ar & BOOST_SERIALIZATION_NVP(map);

34
gtsam/base/tests/testSerializationBase.cpp
View File

@ -18,6 +18,7 @@
#include <gtsam/inference/Key.h>
#include <gtsam/base/ConcurrentMap.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/MatrixSerialization.h>
#include <gtsam/base/Vector.h>
@ -106,6 +107,39 @@ TEST (Serialization, matrix_vector) {
EXPECT(equalityBinary<Matrix>((Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished()));
}
/* ************************************************************************* */
TEST (Serialization, ConcurrentMap) {
ConcurrentMap<int, std::string> 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<int, std::string> 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));
}
/* ************************************************************************* */

4
gtsam/basis/Chebyshev2.h
View File

@ -36,8 +36,6 @@
#include <gtsam/base/OptionalJacobian.h>
#include <gtsam/basis/Basis.h>
#include <boost/function.hpp>
namespace gtsam {
/**
@ -134,7 +132,7 @@ class GTSAM_EXPORT Chebyshev2 : public Basis<Chebyshev2> {
* Create matrix of values at Chebyshev points given vector-valued function.
*/
template <size_t M>
static Matrix matrix(boost::function<Eigen::Matrix<double, M, 1>(double)> f,
static Matrix matrix(std::function<Eigen::Matrix<double, M, 1>(double)> f,
size_t N, double a = -1, double b = 1) {
Matrix Xmat(M, N);
for (size_t j = 0; j < N; j++) {

2
gtsam/basis/tests/testChebyshev2.cpp
View File

@ -118,7 +118,7 @@ TEST(Chebyshev2, InterpolateVector) {
EXPECT(assert_equal(expected, fx(X, actualH), 1e-9));
// Check derivative
boost::function<Vector2(ParameterMatrix<2>)> f = boost::bind(
std::function<Vector2(ParameterMatrix<2>)> f = boost::bind(
&Chebyshev2::VectorEvaluationFunctor<2>::operator(), fx, _1, boost::none);
Matrix numericalH =
numericalDerivative11<Vector2, ParameterMatrix<2>, 2 * N>(f, X);

9
gtsam/discrete/DecisionTree-inl.h
View File

@ -39,10 +39,10 @@
#include <string>
#include <vector>
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);
}

4
gtsam/discrete/DecisionTree.h
View File

@ -22,7 +22,7 @@
#include <gtsam/base/types.h>
#include <gtsam/discrete/Assignment.h>
#include <boost/function.hpp>
#include <boost/shared_ptr.hpp>
#include <functional>
#include <iostream>
#include <map>
@ -220,7 +220,7 @@ namespace gtsam {
/// @{
/// Make virtual
virtual ~DecisionTree() {}
virtual ~DecisionTree() = default;
/// Check if tree is empty.
bool empty() const { return !root_; }

4
gtsam/discrete/DiscreteFactorGraph.h
View File

@ -209,6 +209,10 @@ class GTSAM_EXPORT DiscreteFactorGraph
/// @}
}; // \ DiscreteFactorGraph
std::pair<DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr> //
EliminateForMPE(const DiscreteFactorGraph& factors,
const Ordering& frontalKeys);
/// traits
template <>
struct traits<DiscreteFactorGraph> : public Testable<DiscreteFactorGraph> {};

2
gtsam/geometry/Point3.h
View File

@ -25,6 +25,7 @@
#include <gtsam/base/VectorSpace.h>
#include <gtsam/base/Vector.h>
#include <gtsam/dllexport.h>
#include <gtsam/base/VectorSerialization.h>
#include <boost/serialization/nvp.hpp>
#include <numeric>
@ -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<Point3, Eigen::aligned_allocator<Point3> > Point3Vector;
// Convenience typedef
using Point3Pair = std::pair<Point3, Point3>;

5
gtsam/geometry/Pose3.cpp
View File

@ -489,6 +489,11 @@ boost::optional<Pose3> 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.

8
gtsam/geometry/Pose3.h
View File

@ -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);

6
gtsam/geometry/Rot3.cpp
View File

@ -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<Unit3, double> Rot3::axisAngle() const {
@ -292,8 +294,8 @@ pair<Matrix3, Vector3> 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);

7
gtsam/geometry/Rot3.h
View File

@ -515,11 +515,16 @@ class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> {
*/
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

60
gtsam/geometry/geometry.i
View File

@ -355,7 +355,7 @@ class Rot3 {
double yaw() const;
pair<gtsam::Unit3, double> 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<Eigen::MatrixXd> H) const;
gtsam::Pose3 compose(const gtsam::Pose3& pose) const;
gtsam::Pose3 compose(const gtsam::Pose3& pose,
Eigen::Ref<Eigen::MatrixXd> H1,
Eigen::Ref<Eigen::MatrixXd> H2) const;
gtsam::Pose3 between(const gtsam::Pose3& pose) const;
gtsam::Pose3 between(const gtsam::Pose3& pose,
Eigen::Ref<Eigen::MatrixXd> H1,
Eigen::Ref<Eigen::MatrixXd> H2) const;
gtsam::Pose3 slerp(double t, const gtsam::Pose3& pose) const;
gtsam::Pose3 slerp(double t, const gtsam::Pose3& pose,
Eigen::Ref<Eigen::MatrixXd> Hx,
Eigen::Ref<Eigen::MatrixXd> 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<Eigen::MatrixXd> Hxi) const;
Vector localCoordinates(const gtsam::Pose3& pose) const;
Vector localCoordinates(const gtsam::Pose3& pose, Eigen::Ref<Eigen::MatrixXd> Hxi) const;
// Lie Group
static gtsam::Pose3 Expmap(Vector v);
static gtsam::Pose3 Expmap(Vector v, Eigen::Ref<Eigen::MatrixXd> Hxi);
static Vector Logmap(const gtsam::Pose3& pose);
static Vector Logmap(const gtsam::Pose3& pose, Eigen::Ref<Eigen::MatrixXd> 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<Eigen::MatrixXd> H_this,
Eigen::Ref<Eigen::MatrixXd> H_xib) const;
Vector AdjointTranspose(Vector xi) const;
Vector AdjointTranspose(Vector xi, Eigen::Ref<Eigen::MatrixXd> H_this,
Eigen::Ref<Eigen::MatrixXd> 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<Eigen::MatrixXd> Hself,
Eigen::Ref<Eigen::MatrixXd> Hpoint) const;
gtsam::Point3 transformTo(const gtsam::Point3& point) const;
gtsam::Point3 transformTo(const gtsam::Point3& point, Eigen::Ref<Eigen::MatrixXd> Hself,
Eigen::Ref<Eigen::MatrixXd> 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<Eigen::MatrixXd> Hself) const;
gtsam::Point3 translation() const;
gtsam::Point3 translation(Eigen::Ref<Eigen::MatrixXd> 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<Eigen::MatrixXd> Hself,
Eigen::Ref<Eigen::MatrixXd> HaTb) const;
gtsam::Pose3 transformPoseTo(const gtsam::Pose3& pose) const;
gtsam::Pose3 transformPoseTo(const gtsam::Pose3& pose, Eigen::Ref<Eigen::MatrixXd> Hself,
Eigen::Ref<Eigen::MatrixXd> HwTb) const;
double range(const gtsam::Point3& point);
double range(const gtsam::Point3& point, Eigen::Ref<Eigen::MatrixXd> Hself,
Eigen::Ref<Eigen::MatrixXd> Hpoint);
double range(const gtsam::Pose3& pose);
double range(const gtsam::Pose3& pose, Eigen::Ref<Eigen::MatrixXd> Hself,
Eigen::Ref<Eigen::MatrixXd> Hpose);
// enabling serialization functionality
void serialize() const;
@ -873,7 +906,7 @@ template <CALIBRATION>
class PinholeCamera {
// Standard Constructors and Named Constructors
PinholeCamera();
PinholeCamera(const gtsam::PinholeCamera<CALIBRATION> 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 <CALIBRATION>
class PinholePose {
// Standard Constructors and Named Constructors
PinholePose();
PinholePose(const gtsam::PinholePose<CALIBRATION> 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<gtsam::Pose2, gtsam::Point2, gtsam::Rot2, double>
BearingRange2D;
typedef gtsam::BearingRange<gtsam::Pose2, gtsam::Pose2, gtsam::Rot2, double>
BearingRangePose2;
typedef gtsam::BearingRange<gtsam::Pose3, gtsam::Point3, gtsam::Unit3, double>
BearingRange3D;
typedef gtsam::BearingRange<gtsam::Pose3, gtsam::Pose3, gtsam::Unit3, double>
BearingRangePose3;
} // namespace gtsam

2
gtsam/geometry/tests/testPoint3.cpp
View File

@ -19,8 +19,6 @@
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Point3.h>
#include <boost/function.hpp>
using namespace std::placeholders;
using namespace gtsam;

296
gtsam/geometry/tests/testTriangulation.cpp
View File

@ -15,6 +15,7 @@
* @date July 30th, 2013
* @author Chris Beall (cbeall3)
* @author Luca Carlone
* @author Akshay Krishnan
*/
#include <CppUnitLite/TestHarness.h>
@ -38,24 +39,24 @@ using namespace boost::assign;
// Some common constants
static const boost::shared_ptr<Cal3_S2> sharedCal = //
static const boost::shared_ptr<Cal3_S2> kSharedCal = //
boost::make_shared<Cal3_S2>(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<Cal3_S2> camera1(pose1, *sharedCal);
static const Pose3 kPose1 = Pose3(upright, gtsam::Point3(0, 0, 1));
static const PinholeCamera<Cal3_S2> 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<Cal3_S2> camera2(pose2, *sharedCal);
static const Pose3 kPose2 = kPose1 * Pose3(Rot3(), Point3(1, 0, 0));
static const PinholeCamera<Cal3_S2> 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<Pose3> 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<Point3> actual1 = //
triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements, rank_tol, optimize);
EXPECT(assert_equal(landmark, *actual1, 1e-7));
triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements, rank_tol, optimize);
EXPECT(assert_equal(kLandmark, *actual1, 1e-7));
// 2. test with optimization on, same answer
optimize = true;
boost::optional<Point3> actual2 = //
triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements, rank_tol, optimize);
EXPECT(assert_equal(landmark, *actual2, 1e-7));
triangulatePoint3<Cal3_S2>(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<Point3> actual3 = //
triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements, rank_tol, optimize);
triangulatePoint3<Cal3_S2>(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<Point3> actual4 = //
triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements, rank_tol, optimize);
triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements, rank_tol, optimize);
EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-4));
}
TEST(triangulation, twoCamerasUsingLOST) {
CameraSet<PinholeCamera<Cal3_S2>> 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<Point3> actual1 = //
triangulatePoint3<PinholeCamera<Cal3_S2>>(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<Point3> actual2 = //
triangulatePoint3<PinholeCamera<Cal3_S2>>(
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<PinholeCamera<Cal3_S2>> 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<Point3> estimateLOST = //
triangulatePoint3<PinholeCamera<Cal3_S2>>(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<Point3> estimateDLT =
triangulatePoint3<Cal3_S2>(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<Cal3DS2>(1500, 1200, 0, 640, 480, -.3, 0.1, 0.0001,
-0.0003);
PinholeCamera<Cal3DS2> camera1Distorted(pose1, *sharedDistortedCal);
PinholeCamera<Cal3DS2> camera1Distorted(kPose1, *sharedDistortedCal);
PinholeCamera<Cal3DS2> camera2Distorted(pose2, *sharedDistortedCal);
PinholeCamera<Cal3DS2> 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<Pose3> 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<Point3> actual1 = //
triangulatePoint3<Cal3DS2>(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<Point3> actual2 = //
triangulatePoint3<Cal3DS2>(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<Calibration>(1500, 1200, .1, 640, 480, -.3, 0.1,
0.0001, -0.0003);
PinholeCamera<Calibration> camera1Distorted(pose1, *sharedDistortedCal);
PinholeCamera<Calibration> camera1Distorted(kPose1, *sharedDistortedCal);
PinholeCamera<Calibration> camera2Distorted(pose2, *sharedDistortedCal);
PinholeCamera<Calibration> 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<Pose3> 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<Point3> actual1 = //
triangulatePoint3<Calibration>(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<Point3> actual2 = //
triangulatePoint3<Calibration>(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<Cal3Bundler> bundlerCal = //
boost::make_shared<Cal3Bundler>(1500, 0.1, 0.2, 640, 480);
PinholeCamera<Cal3Bundler> camera1(pose1, *bundlerCal);
PinholeCamera<Cal3Bundler> camera2(pose2, *bundlerCal);
PinholeCamera<Cal3Bundler> camera1(kPose1, *bundlerCal);
PinholeCamera<Cal3Bundler> 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<Pose3> poses;
Point2Vector measurements;
poses += pose1, pose2;
poses += kPose1, kPose2;
measurements += z1, z2;
bool optimize = true;
@ -232,7 +296,7 @@ TEST(triangulation, twoPosesBundler) {
boost::optional<Point3> actual = //
triangulatePoint3<Cal3Bundler>(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<Pose3> poses;
Point2Vector measurements;
poses += pose1, pose2;
measurements += z1, z2;
poses += kPose1, kPose2;
measurements += kZ1, kZ2;
boost::optional<Point3> actual =
triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements);
EXPECT(assert_equal(landmark, *actual, 1e-2));
triangulatePoint3<Cal3_S2>(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<Point3> actual2 = //
triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements);
EXPECT(assert_equal(landmark, *actual2, 1e-2));
triangulatePoint3<Cal3_S2>(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<Cal3_S2> 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<Cal3_S2> camera3(pose3, *kSharedCal);
Point2 z3 = camera3.project(kLandmark);
poses += pose3;
measurements += z3 + Point2(0.1, -0.1);
boost::optional<Point3> triangulated_3cameras = //
triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements);
EXPECT(assert_equal(landmark, *triangulated_3cameras, 1e-2));
triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements);
EXPECT(assert_equal(kLandmark, *triangulated_3cameras, 1e-2));
// Again with nonlinear optimization
boost::optional<Point3> triangulated_3cameras_opt =
triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements, 1e-9, true);
EXPECT(assert_equal(landmark, *triangulated_3cameras_opt, 1e-2));
triangulatePoint3<Cal3_S2>(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<Cal3_S2> camera4(pose4, *sharedCal);
PinholeCamera<Cal3_S2> 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<Cal3_S2>(poses, sharedCal, measurements),
CHECK_EXCEPTION(triangulatePoint3<Cal3_S2>(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<Cal3_S2> 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<Cal3_S2> camera3(pose3, *kSharedCal);
Point2 z3 = camera3.project(kLandmark);
vector<Pose3> 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<Point3> actual =
triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements);
EXPECT(assert_equal(landmark, *actual, 1e-2));
triangulatePoint3<Cal3_S2>(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<Point3> actual2 = //
triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements);
triangulatePoint3<Cal3_S2>(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<Point3> actual3 =
triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements, 1e-9, true);
triangulatePoint3<Cal3_S2>(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<Point3> actual4 = triangulatePoint3<Cal3_S2>(
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<Cal3_S2> 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<Cal3_S2> camera3(pose3, *kSharedCal);
Point2 z3 = camera3.project(kLandmark);
vector<Pose3> 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<Point3> actual =
triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements);
EXPECT(assert_equal(landmark, *actual, 1e-2));
triangulatePoint3<Cal3_S2>(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<Point3> actual2 = //
triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements);
triangulatePoint3<Cal3_S2>(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<Point3> actual3 =
triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements, 1e-9, true);
triangulatePoint3<Cal3_S2>(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<Point3> actual4 = triangulatePoint3<Cal3_S2>(
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<Cal3_S2> camera1(pose1, K1);
PinholeCamera<Cal3_S2> camera1(kPose1, K1);
// create second camera 1 meter to the right of first camera
Cal3_S2 K2(1600, 1300, 0, 650, 440);
PinholeCamera<Cal3_S2> camera2(pose2, K2);
PinholeCamera<Cal3_S2> 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<PinholeCamera<Cal3_S2>> cameras;
Point2Vector measurements;
@ -407,7 +471,7 @@ TEST(triangulation, fourPoses_distinct_Ks) {
boost::optional<Point3> actual = //
triangulatePoint3<Cal3_S2>(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<Point3> actual2 = //
triangulatePoint3<Cal3_S2>(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<Cal3_S2> camera3(pose3, K3);
Point2 z3 = camera3.project(landmark);
Point2 z3 = camera3.project(kLandmark);
cameras += camera3;
measurements += z3 + Point2(0.1, -0.1);
boost::optional<Point3> triangulated_3cameras = //
triangulatePoint3<Cal3_S2>(cameras, measurements);
EXPECT(assert_equal(landmark, *triangulated_3cameras, 1e-2));
EXPECT(assert_equal(kLandmark, *triangulated_3cameras, 1e-2));
// Again with nonlinear optimization
boost::optional<Point3> triangulated_3cameras_opt =
triangulatePoint3<Cal3_S2>(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<Cal3_S2> 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<Cal3DS2> camera1(pose1, K1);
PinholeCamera<Cal3DS2> 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<Cal3DS2> camera2(pose2, K2);
PinholeCamera<Cal3DS2> 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<PinholeCamera<Cal3DS2>> cameras;
Point2Vector measurements;
@ -473,22 +537,22 @@ TEST(triangulation, fourPoses_distinct_Ks_distortion) {
boost::optional<Point3> actual = //
triangulatePoint3<Cal3DS2>(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<Cal3_S2> camera1(pose1, K1);
PinholeCamera<Cal3_S2> camera1(kPose1, K1);
// create second camera 1 meter to the right of first camera
Cal3_S2 K2(1600, 1300, 0, 650, 440);
PinholeCamera<Cal3_S2> camera2(pose2, K2);
PinholeCamera<Cal3_S2> 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<PinholeCamera<Cal3_S2>> 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<Cal3_S2> 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<Cal3_S2> camera1(pose1, *sharedCal);
PinholeCamera<Cal3_S2> camera1(kPose1, *kSharedCal);
// 1. Project two landmarks into two cameras and triangulate
Point2 z1 = camera1.project(landmark);
Point2 z1 = camera1.project(kLandmark);
vector<Pose3> poses;
Point2Vector measurements;
poses += pose1, pose1;
poses += kPose1, kPose1;
measurements += z1, z1;
CHECK_EXCEPTION(triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements),
CHECK_EXCEPTION(triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements),
TriangulationUnderconstrainedException);
}
@ -565,7 +629,7 @@ TEST(triangulation, onePose) {
poses += Pose3();
measurements += Point2(0, 0);
CHECK_EXCEPTION(triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements),
CHECK_EXCEPTION(triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements),
TriangulationUnderconstrainedException);
}
@ -681,12 +745,12 @@ TEST(triangulation, twoPoses_sphericalCamera) {
std::vector<Unit3> 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<SphericalCamera> 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<SphericalCamera>(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<Point3> actual1 = //
triangulatePoint3<SphericalCamera>(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<Point3> actual2 = //
triangulatePoint3<SphericalCamera>(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<SphericalCamera> cameras;

84
gtsam/geometry/triangulation.cpp
View File

@ -14,6 +14,7 @@
* @brief Functions for triangulation
* @date July 31, 2013
* @author Chris Beall
* @author Akshay Krishnan
*/
#include <gtsam/geometry/triangulation.h>
@ -24,9 +25,9 @@
namespace gtsam {
Vector4 triangulateHomogeneousDLT(
const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>&
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<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>&
projection_matrices,
const std::vector<Unit3>& 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<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
const Point2Vector& measurements, double rank_tol) {
Point3 triangulateLOST(const std::vector<Pose3>& 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<Matrix34, Eigen::aligned_allocator<Matrix34>>&
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<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>&
projection_matrices,
const std::vector<Unit3>& 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<Point3>(landmarkKey);
}
} // \namespace gtsam
} // namespace gtsam

223
gtsam/geometry/triangulation.h
View File

@ -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<Unit3>& 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<Pose3>& 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<NonlinearFactorGraph, Values> triangulationGraph(
const std::vector<Pose3>& poses, boost::shared_ptr<CALIBRATION> 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<CALIBRATION> Camera;
Camera camera_i(pose_i, sharedCal);
graph.emplace_shared<TriangulationFactor<Camera> > //
(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 <class CAMERA>
typename CAMERA::MeasurementVector undistortMeasurements(
const CameraSet<CAMERA>& 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 <class CALIBRATION>
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 <class CAMERA>
inline Point3Vector calibrateMeasurements(
const CameraSet<CAMERA>& 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 <class CAMERA = SphericalCamera>
inline Point3Vector calibrateMeasurements(
const CameraSet<SphericalCamera>& 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<class CALIBRATION>
template <class CALIBRATION>
Point3 triangulatePoint3(const std::vector<Pose3>& poses,
boost::shared_ptr<CALIBRATION> sharedCal,
const Point2Vector& measurements, double rank_tol = 1e-9,
bool optimize = false,
const SharedNoiseModel& model = nullptr) {
boost::shared_ptr<CALIBRATION> 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<CALIBRATION>(*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<CALIBRATION>(*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<CALIBRATION>(*sharedCal, measurements);
point =
triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol);
}
// Then refine using non-linear optimization
if (optimize)
point = triangulateNonlinear<CALIBRATION> //
point = triangulateNonlinear<CALIBRATION> //
(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<Pose3>& 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<class CAMERA>
Point3 triangulatePoint3(
const CameraSet<CAMERA>& cameras,
const typename CAMERA::MeasurementVector& measurements, double rank_tol = 1e-9,
bool optimize = false,
const SharedNoiseModel& model = nullptr) {
template <class CAMERA>
Point3 triangulatePoint3(const CameraSet<CAMERA>& 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<CAMERA>(cameras, measurements);
// construct poses from cameras.
std::vector<Pose3> 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<CAMERA>(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<CAMERA>(cameras, measurements);
point =
triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol);
}
// Then refine using non-linear optimization
if (optimize) {
point = triangulateNonlinear<CAMERA>(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<class CALIBRATION>
Point3 triangulatePoint3(
const CameraSet<PinholeCamera<CALIBRATION> >& cameras,
const Point2Vector& measurements, double rank_tol = 1e-9,
bool optimize = false,
const SharedNoiseModel& model = nullptr) {
return triangulatePoint3<PinholeCamera<CALIBRATION> > //
(cameras, measurements, rank_tol, optimize, model);
template <class CALIBRATION>
Point3 triangulatePoint3(const CameraSet<PinholeCamera<CALIBRATION>>& cameras,
const Point2Vector& measurements,
double rank_tol = 1e-9, bool optimize = false,
const SharedNoiseModel& model = nullptr,
const bool useLOST = false) {
return triangulatePoint3<PinholeCamera<CALIBRATION>> //
(cameras, measurements, rank_tol, optimize, model, useLOST);
}
struct GTSAM_EXPORT TriangulationParameters {

60
gtsam/hybrid/GaussianMixtureConditional.cpp → gtsam/hybrid/GaussianMixture.cpp
View File

@ -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 <gtsam/base/utilities.h>
#include <gtsam/discrete/DecisionTree-inl.h>
#include <gtsam/hybrid/GaussianMixtureConditional.h>
#include <gtsam/discrete/DiscreteValues.h>
#include <gtsam/hybrid/GaussianMixture.h>
#include <gtsam/inference/Conditional-inst.h>
#include <gtsam/linear/GaussianFactorGraph.h>
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<GaussianConditional::shared_ptr> &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<GaussianConditional>(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<const This *>(&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 << "), ";
}

59
gtsam/hybrid/GaussianMixtureConditional.h → gtsam/hybrid/GaussianMixture.h
View File

@ -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 <gtsam/discrete/DecisionTree-inl.h>
#include <gtsam/discrete/DecisionTree.h>
#include <gtsam/discrete/DiscreteKey.h>
#include <gtsam/hybrid/HybridFactor.h>
#include <gtsam/inference/Conditional.h>
#include <gtsam/linear/GaussianConditional.h>
@ -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<HybridFactor, GaussianMixtureConditional> {
public Conditional<HybridFactor, GaussianMixture> {
public:
using This = GaussianMixtureConditional;
using shared_ptr = boost::shared_ptr<GaussianMixtureConditional>;
using This = GaussianMixture;
using shared_ptr = boost::shared_ptr<GaussianMixture>;
using BaseFactor = HybridFactor;
using BaseConditional = Conditional<HybridFactor, GaussianMixtureConditional>;
using BaseConditional = Conditional<HybridFactor, GaussianMixture>;
/// Alias for DecisionTree of GaussianFactorGraphs
using Sum = DecisionTree<Key, GaussianFactorGraph>;
@ -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<GaussianConditional::shared_ptr> &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<GaussianMixture> : public Testable<GaussianMixture> {};
} // namespace gtsam

10
gtsam/hybrid/GaussianMixtureFactor.cpp
View File

@ -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<const This *>(&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;
}
/* *******************************************************************************/

36
gtsam/hybrid/GaussianMixtureFactor.h
View File

@ -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<gtsam::GaussianFactor::shared_ptr>;
/**
* @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<Key, GaussianFactor::shared_ptr>;
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<GaussianFactor::shared_ptr> &factors)
: GaussianMixtureFactor(keys, discreteKeys,
Factors(discreteKeys, factors)) {}
static This FromFactors(
const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys,
const std::vector<GaussianFactor::shared_ptr> &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<GaussianMixtureFactor> : public Testable<GaussianMixtureFactor> {
};
} // namespace gtsam

5
gtsam/hybrid/HybridBayesTree.cpp
View File

@ -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<HybridBayesTreeClique,
GaussianHybridFactorGraph>;
HybridGaussianFactorGraph>;
template class BayesTree<HybridBayesTreeClique>;
/* ************************************************************************* */

9
gtsam/hybrid/HybridBayesTree.h
View File

@ -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 <gtsam/hybrid/GaussianHybridFactorGraph.h>
#include <gtsam/hybrid/HybridBayesNet.h>
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
#include <gtsam/inference/BayesTree.h>
#include <gtsam/inference/BayesTreeCliqueBase.h>
#include <gtsam/inference/Conditional.h>
@ -38,10 +39,10 @@ class VectorValues;
*/
class GTSAM_EXPORT HybridBayesTreeClique
: public BayesTreeCliqueBase<HybridBayesTreeClique,
GaussianHybridFactorGraph> {
HybridGaussianFactorGraph> {
public:
typedef HybridBayesTreeClique This;
typedef BayesTreeCliqueBase<HybridBayesTreeClique, GaussianHybridFactorGraph>
typedef BayesTreeCliqueBase<HybridBayesTreeClique, HybridGaussianFactorGraph>
Base;
typedef boost::shared_ptr<This> shared_ptr;
typedef boost::weak_ptr<This> weak_ptr;

5
gtsam/hybrid/HybridConditional.cpp
View File

@ -54,7 +54,7 @@ HybridConditional::HybridConditional(
/* ************************************************************************ */
HybridConditional::HybridConditional(
boost::shared_ptr<GaussianMixtureConditional> gaussianMixture)
boost::shared_ptr<GaussianMixture> 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<const This *>(&other);
return e != nullptr && BaseFactor::equals(*e, tol);
}
} // namespace gtsam

18
gtsam/hybrid/HybridConditional.h
View File

@ -18,9 +18,9 @@
#pragma once
#include <gtsam/discrete/DiscreteConditional.h>
#include <gtsam/hybrid/GaussianHybridFactorGraph.h>
#include <gtsam/hybrid/GaussianMixtureConditional.h>
#include <gtsam/hybrid/GaussianMixture.h>
#include <gtsam/hybrid/HybridFactor.h>
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
#include <gtsam/inference/Conditional.h>
#include <gtsam/inference/Key.h>
#include <gtsam/linear/GaussianConditional.h>
@ -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<T>` 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<GaussianMixtureConditional> gaussianMixture);
boost::shared_ptr<GaussianMixture> 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<GaussianMixtureConditional>(inner_);
return boost::static_pointer_cast<GaussianMixture>(inner_);
}
/**

6
gtsam/hybrid/HybridDiscreteFactor.cpp
View File

@ -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<const This *>(&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

7
gtsam/hybrid/HybridDiscreteFactor.h
View File

@ -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<HybridDiscreteFactor> : public Testable<HybridDiscreteFactor> {};
} // namespace gtsam

6
gtsam/hybrid/HybridEliminationTree.cpp
View File

@ -21,17 +21,17 @@
namespace gtsam {
// Instantiate base class
template class EliminationTree<HybridBayesNet, GaussianHybridFactorGraph>;
template class EliminationTree<HybridBayesNet, HybridGaussianFactorGraph>;
/* ************************************************************************* */
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) {}
/* ************************************************************************* */

10
gtsam/hybrid/HybridEliminationTree.h
View File

@ -17,8 +17,8 @@
#pragma once
#include <gtsam/hybrid/GaussianHybridFactorGraph.h>
#include <gtsam/hybrid/HybridBayesNet.h>
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
#include <gtsam/inference/EliminationTree.h>
namespace gtsam {
@ -27,12 +27,12 @@ namespace gtsam {
* Elimination Tree type for Hybrid
*/
class GTSAM_EXPORT HybridEliminationTree
: public EliminationTree<HybridBayesNet, GaussianHybridFactorGraph> {
: public EliminationTree<HybridBayesNet, HybridGaussianFactorGraph> {
private:
friend class ::EliminationTreeTester;
public:
typedef EliminationTree<HybridBayesNet, GaussianHybridFactorGraph>
typedef EliminationTree<HybridBayesNet, HybridGaussianFactorGraph>
Base; ///< Base class
typedef HybridEliminationTree This; ///< This class
typedef boost::shared_ptr<This> 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);
/// @}

28
gtsam/hybrid/HybridFactor.cpp
View File

@ -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<const This *>(&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; c<continuousKeys_.size(); c++) {
std::cout << formatter(continuousKeys_.at(c));
if (c < continuousKeys_.size() - 1) {
std::cout << " ";
} else {
std::cout << "; ";
}
}
for(auto && discreteKey: discreteKeys_) {
std::cout << formatter(discreteKey.first) << " ";
}
}
} // namespace gtsam

24
gtsam/hybrid/HybridFactor.h
View File

@ -34,6 +34,7 @@ DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1,
/**
* Base class for hybrid probabilistic factors
*
* Examples:
* - HybridGaussianFactor
* - HybridDiscreteFactor
@ -51,6 +52,9 @@ class GTSAM_EXPORT HybridFactor : public Factor {
protected:
DiscreteKeys discreteKeys_;
/// Record continuous keys for book-keeping
KeyVector continuousKeys_;
public:
// typedefs needed to play nice with gtsam
typedef HybridFactor This; ///< This class
@ -64,13 +68,29 @@ class GTSAM_EXPORT HybridFactor : public Factor {
/** Default constructor creates empty factor */
HybridFactor() = default;
/**
* @brief Construct hybrid factor from continuous keys.
*
* @param keys Vector of continuous keys.
*/
explicit HybridFactor(const KeyVector &keys);
/**
* @brief Construct hybrid factor from discrete keys.
*
* @param keys Vector of discrete keys.
*/
explicit HybridFactor(const DiscreteKeys &discreteKeys);
/**
* @brief Construct a new Hybrid Factor object.
*
* @param continuousKeys Vector of keys for continuous variables.
* @param discreteKeys Vector of keys for discrete variables.
*/
HybridFactor(const KeyVector &continuousKeys,
const DiscreteKeys &discreteKeys);
explicit HybridFactor(const DiscreteKeys &discreteKeys);
/// Virtual destructor
virtual ~HybridFactor() = default;

17
gtsam/hybrid/HybridGaussianFactor.cpp
View File

@ -21,22 +21,27 @@
namespace gtsam {
/* ************************************************************************* */
HybridGaussianFactor::HybridGaussianFactor(GaussianFactor::shared_ptr other)
: Base(other->keys()) {
inner_ = other;
}
: Base(other->keys()), inner_(other) {}
/* ************************************************************************* */
HybridGaussianFactor::HybridGaussianFactor(JacobianFactor &&jf)
: Base(jf.keys()),
inner_(boost::make_shared<JacobianFactor>(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<const This *>(&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

8
gtsam/hybrid/HybridGaussianFactor.h
View File

@ -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<HybridGaussianFactor> : public Testable<HybridGaussianFactor> {};
} // namespace gtsam

156
gtsam/hybrid/GaussianHybridFactorGraph.cpp → gtsam/hybrid/HybridGaussianFactorGraph.cpp
View File

@ -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 <gtsam/discrete/DiscreteEliminationTree.h>
#include <gtsam/discrete/DiscreteFactorGraph.h>
#include <gtsam/discrete/DiscreteJunctionTree.h>
#include <gtsam/hybrid/GaussianHybridFactorGraph.h>
#include <gtsam/hybrid/GaussianMixtureConditional.h>
#include <gtsam/hybrid/GaussianMixture.h>
#include <gtsam/hybrid/GaussianMixtureFactor.h>
#include <gtsam/hybrid/HybridConditional.h>
#include <gtsam/hybrid/HybridDiscreteFactor.h>
#include <gtsam/hybrid/HybridEliminationTree.h>
#include <gtsam/hybrid/HybridFactor.h>
#include <gtsam/hybrid/HybridGaussianFactor.h>
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
#include <gtsam/hybrid/HybridJunctionTree.h>
#include <gtsam/inference/EliminateableFactorGraph-inst.h>
#include <gtsam/inference/Key.h>
@ -53,7 +53,7 @@
namespace gtsam {
template class EliminateableFactorGraph<GaussianHybridFactorGraph>;
template class EliminateableFactorGraph<HybridGaussianFactorGraph>;
/* ************************************************************************ */
static GaussianMixtureFactor::Sum &addGaussian(
@ -77,67 +77,8 @@ static GaussianMixtureFactor::Sum &addGaussian(
}
/* ************************************************************************ */
std::pair<HybridConditional::shared_ptr, HybridFactor::shared_ptr>
continuousElimination(const GaussianHybridFactorGraph &factors,
const Ordering &frontalKeys) {
GaussianFactorGraph gfg;
for (auto &fp : factors) {
auto ptr = boost::dynamic_pointer_cast<HybridGaussianFactor>(fp);
if (ptr) {
gfg.push_back(ptr->inner());
} else {
auto p = boost::static_pointer_cast<HybridConditional>(fp)->inner();
if (p) {
gfg.push_back(boost::static_pointer_cast<GaussianConditional>(p));
} else {
// It is an orphan wrapped conditional
}
}
}
auto result = EliminatePreferCholesky(gfg, frontalKeys);
return std::make_pair(
boost::make_shared<HybridConditional>(result.first),
boost::make_shared<HybridGaussianFactor>(result.second));
}
/* ************************************************************************ */
std::pair<HybridConditional::shared_ptr, HybridFactor::shared_ptr>
discreteElimination(const GaussianHybridFactorGraph &factors,
const Ordering &frontalKeys) {
DiscreteFactorGraph dfg;
for (auto &fp : factors) {
auto ptr = boost::dynamic_pointer_cast<HybridDiscreteFactor>(fp);
if (ptr) {
dfg.push_back(ptr->inner());
} else {
auto p = boost::static_pointer_cast<HybridConditional>(fp)->inner();
if (p) {
dfg.push_back(boost::static_pointer_cast<DiscreteConditional>(p));
} else {
// It is an orphan wrapper
}
}
}
auto result = EliminateDiscrete(dfg, frontalKeys);
return std::make_pair(
boost::make_shared<HybridConditional>(result.first),
boost::make_shared<HybridDiscreteFactor>(result.second));
}
/* ************************************************************************ */
std::pair<HybridConditional::shared_ptr, HybridFactor::shared_ptr>
hybridElimination(const GaussianHybridFactorGraph &factors,
const Ordering &frontalKeys,
const KeySet &continuousSeparator,
const std::set<DiscreteKey> &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<GaussianMixtureFactor>(f);
if (cgmf) {
if (auto cgmf = boost::dynamic_pointer_cast<GaussianMixtureFactor>(f)) {
sum = cgmf->add(sum);
}
auto gm = boost::dynamic_pointer_cast<HybridConditional>(f);
if (gm) {
if (auto gm = boost::dynamic_pointer_cast<HybridConditional>(f)) {
sum = gm->asMixture()->add(sum);
}
@ -179,11 +118,72 @@ hybridElimination(const GaussianHybridFactorGraph &factors,
gttoc(sum);
return sum;
}
/* ************************************************************************ */
std::pair<HybridConditional::shared_ptr, HybridFactor::shared_ptr>
continuousElimination(const HybridGaussianFactorGraph &factors,
const Ordering &frontalKeys) {
GaussianFactorGraph gfg;
for (auto &fp : factors) {
if (auto ptr = boost::dynamic_pointer_cast<HybridGaussianFactor>(fp)) {
gfg.push_back(ptr->inner());
} else if (auto p =
boost::static_pointer_cast<HybridConditional>(fp)->inner()) {
gfg.push_back(boost::static_pointer_cast<GaussianConditional>(p));
} else {
// It is an orphan wrapped conditional
}
}
auto result = EliminatePreferCholesky(gfg, frontalKeys);
return {boost::make_shared<HybridConditional>(result.first),
boost::make_shared<HybridGaussianFactor>(result.second)};
}
/* ************************************************************************ */
std::pair<HybridConditional::shared_ptr, HybridFactor::shared_ptr>
discreteElimination(const HybridGaussianFactorGraph &factors,
const Ordering &frontalKeys) {
DiscreteFactorGraph dfg;
for (auto &fp : factors) {
if (auto ptr = boost::dynamic_pointer_cast<HybridDiscreteFactor>(fp)) {
dfg.push_back(ptr->inner());
} else if (auto p =
boost::static_pointer_cast<HybridConditional>(fp)->inner()) {
dfg.push_back(boost::static_pointer_cast<DiscreteConditional>(p));
} else {
// It is an orphan wrapper
}
}
auto result = EliminateDiscrete(dfg, frontalKeys);
return {boost::make_shared<HybridConditional>(result.first),
boost::make_shared<HybridDiscreteFactor>(result.second)};
}
/* ************************************************************************ */
std::pair<HybridConditional::shared_ptr, HybridFactor::shared_ptr>
hybridElimination(const HybridGaussianFactorGraph &factors,
const Ordering &frontalKeys,
const KeySet &continuousSeparator,
const std::set<DiscreteKey> &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<Key, EliminationPair> 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<GaussianMixtureConditional>(
// Create the GaussianMixture from the conditionals
auto conditional = boost::make_shared<GaussianMixture>(
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<HybridConditional::shared_ptr, HybridFactor::shared_ptr> //
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<HybridGaussianFactor>(std::move(factor)));
}
/* ************************************************************************ */
void GaussianHybridFactorGraph::add(JacobianFactor::shared_ptr factor) {
void HybridGaussianFactorGraph::add(JacobianFactor::shared_ptr factor) {
FactorGraph::add(boost::make_shared<HybridGaussianFactor>(factor));
}
/* ************************************************************************ */
void GaussianHybridFactorGraph::add(DecisionTreeFactor &&factor) {
void HybridGaussianFactorGraph::add(DecisionTreeFactor &&factor) {
FactorGraph::add(boost::make_shared<HybridDiscreteFactor>(std::move(factor)));
}
/* ************************************************************************ */
void GaussianHybridFactorGraph::add(DecisionTreeFactor::shared_ptr factor) {
void HybridGaussianFactorGraph::add(DecisionTreeFactor::shared_ptr factor) {
FactorGraph::add(boost::make_shared<HybridDiscreteFactor>(factor));
}

39
gtsam/hybrid/GaussianHybridFactorGraph.h → gtsam/hybrid/HybridGaussianFactorGraph.h
View File

@ -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<boost::shared_ptr<HybridConditional>, HybridFactor::shared_ptr>
EliminateHybrid(const GaussianHybridFactorGraph& factors, const Ordering& keys);
EliminateHybrid(const HybridGaussianFactorGraph& factors, const Ordering& keys);
/* ************************************************************************* */
template <>
struct EliminationTraits<GaussianHybridFactorGraph> {
struct EliminationTraits<HybridGaussianFactorGraph> {
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<ConditionalType>,
boost::shared_ptr<FactorType> >
@ -73,18 +74,12 @@ struct EliminationTraits<GaussianHybridFactorGraph> {
* 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<GaussianHybridFactorGraph> {
protected:
/// Check if FACTOR type is derived from GaussianFactor.
template <typename FACTOR>
using IsGaussian = typename std::enable_if<
std::is_base_of<GaussianFactor, FACTOR>::value>::type;
class GTSAM_EXPORT HybridGaussianFactorGraph
: public HybridFactorGraph,,
public EliminateableFactorGraph<HybridGaussianFactorGraph> {
public:
using Base = HybridFactorGraph;
using This = GaussianHybridFactorGraph; ///< this class
using This = HybridGaussianFactorGraph; ///< this class
using BaseEliminateable =
EliminateableFactorGraph<This>; ///< for elimination
using shared_ptr = boost::shared_ptr<This>; ///< 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 <class DERIVEDFACTOR>
GaussianHybridFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph)
HybridGaussianFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph)
: Base(graph) {}
/// @}

22
gtsam/hybrid/HybridISAM.cpp → gtsam/hybrid/HybridGaussianISAM.cpp
View File

@ -10,16 +10,16 @@
* -------------------------------------------------------------------------- */
/**
* @file HybridISAM.h
* @file HybridGaussianISAM.h
* @date March 31, 2022
* @author Fan Jiang
* @author Frank Dellaert
* @author Richard Roberts
*/
#include <gtsam/hybrid/GaussianHybridFactorGraph.h>
#include <gtsam/hybrid/HybridBayesTree.h>
#include <gtsam/hybrid/HybridISAM.h>
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
#include <gtsam/hybrid/HybridGaussianISAM.h>
#include <gtsam/inference/ISAM-inst.h>
#include <gtsam/inference/Key.h>
@ -31,15 +31,17 @@ namespace gtsam {
// template class ISAM<HybridBayesTree>;
/* ************************************************************************* */
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);
}

18
gtsam/hybrid/HybridISAM.h → gtsam/hybrid/HybridGaussianISAM.h
View File

@ -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 <gtsam/base/Testable.h>
#include <gtsam/hybrid/GaussianHybridFactorGraph.h>
#include <gtsam/hybrid/HybridBayesTree.h>
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
#include <gtsam/inference/ISAM.h>
namespace gtsam {
class GTSAM_EXPORT HybridISAM : public ISAM<HybridBayesTree> {
class GTSAM_EXPORT HybridGaussianISAM : public ISAM<HybridBayesTree> {
public:
typedef ISAM<HybridBayesTree> Base;
typedef HybridISAM This;
typedef HybridGaussianISAM This;
typedef boost::shared_ptr<This> 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<HybridBayesTree> {
* @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<HybridISAM> : public Testable<HybridISAM> {};
struct traits<HybridGaussianISAM> : public Testable<HybridGaussianISAM> {};
} // namespace gtsam

64
gtsam/hybrid/HybridJunctionTree.cpp
View File

@ -15,8 +15,8 @@
* @author Fan Jiang
*/
#include <gtsam/hybrid/GaussianHybridFactorGraph.h>
#include <gtsam/hybrid/HybridEliminationTree.h>
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
#include <gtsam/hybrid/HybridJunctionTree.h>
#include <gtsam/inference/JunctionTree-inst.h>
#include <gtsam/inference/Key.h>
@ -27,19 +27,19 @@ namespace gtsam {
// Instantiate base classes
template class EliminatableClusterTree<HybridBayesTree,
GaussianHybridFactorGraph>;
template class JunctionTree<HybridBayesTree, GaussianHybridFactorGraph>;
HybridGaussianFactorGraph>;
template class JunctionTree<HybridBayesTree, HybridGaussianFactorGraph>;
struct HybridConstructorTraversalData {
typedef
typename JunctionTree<HybridBayesTree, GaussianHybridFactorGraph>::Node
typename JunctionTree<HybridBayesTree, HybridGaussianFactorGraph>::Node
Node;
typedef
typename JunctionTree<HybridBayesTree,
GaussianHybridFactorGraph>::sharedNode sharedNode;
HybridGaussianFactorGraph>::sharedNode sharedNode;
HybridConstructorTraversalData* const parentData;
sharedNode myJTNode;
sharedNode junctionTreeNode;
FastVector<SymbolicConditional::shared_ptr> childSymbolicConditionals;
FastVector<SymbolicFactor::shared_ptr> 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>(node->key, node->factors);
parentData.myJTNode->addChild(myData.myJTNode);
data.junctionTreeNode = boost::make_shared<Node>(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<HybridEliminationTree::Node>& 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<SymbolicConditional::shared_ptr>& 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<size_t> nrFrontals = node->nrFrontalsOfChildren();
std::vector<size_t> nrChildrenFrontals = node->nrFrontalsOfChildren();
std::vector<bool> 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<typename Base::Node>(); // 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();

8
gtsam/hybrid/HybridJunctionTree.h
View File

@ -17,8 +17,8 @@
#pragma once
#include <gtsam/hybrid/GaussianHybridFactorGraph.h>
#include <gtsam/hybrid/HybridBayesTree.h>
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
#include <gtsam/inference/JunctionTree.h>
namespace gtsam {
@ -49,11 +49,11 @@ class HybridEliminationTree;
* \nosubgrouping
*/
class GTSAM_EXPORT HybridJunctionTree
: public JunctionTree<HybridBayesTree, GaussianHybridFactorGraph> {
: public JunctionTree<HybridBayesTree, HybridGaussianFactorGraph> {
public:
typedef JunctionTree<HybridBayesTree, GaussianHybridFactorGraph>
typedef JunctionTree<HybridBayesTree, HybridGaussianFactorGraph>
Base; ///< Base class
typedef HybridJunctionTree This; ///< This class
typedef HybridJunctionTree This; ///< This class
typedef boost::shared_ptr<This> shared_ptr; ///< Shared pointer to this class
/**

24
gtsam/hybrid/hybrid.i
View File

@ -38,16 +38,16 @@ class GaussianMixtureFactor : gtsam::HybridFactor {
gtsam::DefaultKeyFormatter) const;
};
#include <gtsam/hybrid/GaussianMixtureConditional.h>
class GaussianMixtureConditional : gtsam::HybridFactor {
static GaussianMixtureConditional FromConditionals(
#include <gtsam/hybrid/GaussianMixture.h>
class GaussianMixture : gtsam::HybridFactor {
static GaussianMixture FromConditionals(
const gtsam::KeyVector& continuousFrontals,
const gtsam::KeyVector& continuousParents,
const gtsam::DiscreteKeys& discreteParents,
const std::vector<gtsam::GaussianConditional::shared_ptr>&
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 <gtsam/hybrid/GaussianHybridFactorGraph.h>
class GaussianHybridFactorGraph {
GaussianHybridFactorGraph();
GaussianHybridFactorGraph(const gtsam::HybridBayesNet& bayesNet);
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
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<gtsam::HybridBayesNet*, gtsam::GaussianHybridFactorGraph*>
pair<gtsam::HybridBayesNet*, gtsam::HybridGaussianFactorGraph*>
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<gtsam::HybridBayesTree*, gtsam::GaussianHybridFactorGraph*>
pair<gtsam::HybridBayesTree*, gtsam::HybridGaussianFactorGraph*>
eliminatePartialMultifrontal(const gtsam::Ordering& ordering);
string dot(

8
gtsam/hybrid/tests/Switching.h
View File

@ -18,8 +18,8 @@
#include <gtsam/base/Matrix.h>
#include <gtsam/discrete/DecisionTreeFactor.h>
#include <gtsam/hybrid/GaussianHybridFactorGraph.h>
#include <gtsam/hybrid/GaussianMixtureFactor.h>
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/slam/BetweenFactor.h>
@ -33,10 +33,10 @@ namespace gtsam {
using MotionModel = BetweenFactor<double>;
inline GaussianHybridFactorGraph::shared_ptr makeSwitchingChain(
inline HybridGaussianFactorGraph::shared_ptr makeSwitchingChain(
size_t n, std::function<Key(int)> keyFunc = X,
std::function<Key(int)> 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<GaussianHybridFactorGraph>(std::move(hfg));
return boost::make_shared<HybridGaussianFactorGraph>(std::move(hfg));
}
inline std::pair<KeyVector, std::vector<int>> makeBinaryOrdering(

76
gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp
View File

@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */
/*
* @file testGaussianHybridFactorGraph.cpp
* @file testHybridGaussianFactorGraph.cpp
* @date Mar 11, 2022
* @author Fan Jiang
*/
@ -20,8 +20,7 @@
#include <gtsam/discrete/DecisionTreeFactor.h>
#include <gtsam/discrete/DiscreteKey.h>
#include <gtsam/discrete/DiscreteValues.h>
#include <gtsam/hybrid/GaussianHybridFactorGraph.h>
#include <gtsam/hybrid/GaussianMixtureConditional.h>
#include <gtsam/hybrid/GaussianMixture.h>
#include <gtsam/hybrid/GaussianMixtureFactor.h>
#include <gtsam/hybrid/HybridBayesNet.h>
#include <gtsam/hybrid/HybridBayesTree.h>
@ -29,7 +28,8 @@
#include <gtsam/hybrid/HybridDiscreteFactor.h>
#include <gtsam/hybrid/HybridFactor.h>
#include <gtsam/hybrid/HybridGaussianFactor.h>
#include <gtsam/hybrid/HybridISAM.h>
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
#include <gtsam/hybrid/HybridGaussianISAM.h>
#include <gtsam/inference/BayesNet.h>
#include <gtsam/inference/DotWriter.h>
#include <gtsam/inference/Key.h>
@ -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.h> // ::signal, ::raise
#include <boost/stacktrace.hpp>
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<GaussianConditional>(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);

95
gtsam/hybrid/tests/testGaussianMixture.cpp Normal file
View File

@ -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 <gtsam/discrete/DiscreteValues.h>
#include <gtsam/hybrid/GaussianMixture.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/GaussianConditional.h>
#include <vector>
// Include for test suite
#include <CppUnitLite/TestHarness.h>
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<GaussianConditional>(X(1), d1, R1,
X(2), S1, model),
conditional1 = boost::make_shared<GaussianConditional>(X(1), d2, R2,
X(2), S2, model);
// Create decision tree
DiscreteKey m1(1, 2);
GaussianMixture::Conditionals conditionals(
{m1},
vector<GaussianConditional::shared_ptr>{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);
}
/* ************************************************************************* */

159
gtsam/hybrid/tests/testGaussianMixtureFactor.cpp Normal file
View File

@ -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 <gtsam/base/TestableAssertions.h>
#include <gtsam/discrete/DiscreteValues.h>
#include <gtsam/hybrid/GaussianMixture.h>
#include <gtsam/hybrid/GaussianMixtureFactor.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/GaussianFactorGraph.h>
// Include for test suite
#include <CppUnitLite/TestHarness.h>
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<JacobianFactor>(X(1), A1, X(2), A2, b);
auto f11 = boost::make_shared<JacobianFactor>(X(1), A1, X(2), A2, b);
auto f20 = boost::make_shared<JacobianFactor>(X(1), A1, X(3), A3, b);
auto f21 = boost::make_shared<JacobianFactor>(X(1), A1, X(3), A3, b);
auto f22 = boost::make_shared<JacobianFactor>(X(1), A1, X(3), A3, b);
std::vector<GaussianFactor::shared_ptr> factorsA{f10, f11};
std::vector<GaussianFactor::shared_ptr> 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<Key> 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<JacobianFactor>(X(1), A1, X(2), A2, b);
auto f11 = boost::make_shared<JacobianFactor>(X(1), A1, X(2), A2, b);
std::vector<GaussianFactor::shared_ptr> 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<GaussianConditional>();
GaussianMixture::Conditionals conditionals(gaussians);
GaussianMixture gm({}, keys, dKeys, conditionals);
EXPECT_LONGS_EQUAL(2, gm.discreteKeys().size());
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

2
gtsam/inference/FactorGraph.h
View File

@ -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
*/

11
gtsam/inference/Ordering.cpp
View File

@ -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;

18
gtsam/inference/Ordering.h
View File

@ -70,7 +70,23 @@ public:
boost::assign_detail::call_push_back<This>(*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<Key, size_t>
*/
FastMap<Key, size_t> invert() const;
/// @name Fill-reducing Orderings @{

14
gtsam/inference/inference.i
View File

@ -9,7 +9,7 @@ namespace gtsam {
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/symbolic/SymbolicFactorGraph.h>
#include <gtsam/discrete/DiscreteFactorGraph.h>
#include <gtsam/hybrid/GaussianHybridFactorGraph.h>
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
#include <gtsam/inference/Key.h>
@ -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);

26
gtsam/inference/tests/testOrdering.cpp
View File

@ -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) {

2
gtsam/linear/GaussianBayesNet.h
View File

@ -45,7 +45,7 @@ namespace gtsam {
/// @name Standard Constructors
/// @{
/** Construct empty factor graph */
/** Construct empty bayes net */
GaussianBayesNet() {}
/** Construct from iterator over conditionals */

4
gtsam/linear/GaussianConditional.cpp
View File

@ -15,10 +15,10 @@
* @author Christian Potthast, Frank Dellaert
*/
#include <gtsam/linear/linearExceptions.h>
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/Sampler.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/linearExceptions.h>
#include <boost/format.hpp>
#ifdef __GNUC__

1
gtsam/linear/GaussianConditional.h
View File

@ -24,6 +24,7 @@
#include <gtsam/global_includes.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/inference/Conditional.h>
#include <gtsam/inference/Conditional-inst.h>
#include <gtsam/linear/VectorValues.h>
#include <random> // for std::mt19937_64

6
gtsam/linear/NoiseModel.h
View File

@ -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<Matrix>& A, Vector& b) const override;

2
gtsam/linear/linear.i
View File

@ -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 =

5
gtsam/linear/tests/testNoiseModel.cpp
View File

@ -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);
}

39
gtsam/nonlinear/GncOptimizer.h
View File

@ -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;
}
}
}

2
gtsam/nonlinear/GncParams.h
View File

@ -48,6 +48,8 @@ class GTSAM_EXPORT GncParams {
enum Verbosity {
SILENT = 0,
SUMMARY,
MU,
WEIGHTS,
VALUES
};

5
gtsam/sam/BearingRangeFactor.h
View File

@ -79,9 +79,8 @@ class BearingRangeFactor
{
std::vector<Matrix> 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;

6
gtsam/sam/sam.i
View File

@ -108,5 +108,11 @@ typedef gtsam::BearingRangeFactor<gtsam::Pose2, gtsam::Point2, gtsam::Rot2,
typedef gtsam::BearingRangeFactor<gtsam::Pose2, gtsam::Pose2, gtsam::Rot2,
double>
BearingRangeFactorPose2;
typedef gtsam::BearingRangeFactor<gtsam::Pose3, gtsam::Point3, gtsam::Unit3,
double>
BearingRangeFactor3D;
typedef gtsam::BearingRangeFactor<gtsam::Pose3, gtsam::Pose3, gtsam::Unit3,
double>
BearingRangeFactorPose3;
} // namespace gtsam

29
gtsam/sfm/TranslationRecovery.cpp
View File

@ -119,21 +119,24 @@ void TranslationRecovery::addPrior(
graph->emplace_shared<PriorFactor<Point3>>(edge->key1(), Point3(0, 0, 0),
priorNoiseModel);
// Add between factors for optional relative translations.
for (auto edge : betweenTranslations) {
graph->emplace_shared<BetweenFactor<Point3>>(
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<PriorFactor<Point3>>(
edge->key2(), scale * edge->measured().point3(), edge->noiseModel());
return;
}
// Add between factors for optional relative translations.
for (auto prior_edge : betweenTranslations) {
graph->emplace_shared<BetweenFactor<Point3>>(
prior_edge.key1(), prior_edge.key2(), prior_edge.measured(),
prior_edge.noiseModel());
}
}
Values TranslationRecovery::initializeRandomly(
const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations,
const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
std::mt19937 *rng, const Values &initialValues) const {
uniform_real_distribution<double> 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<BinaryMeasurement<Unit3>> &relativeTranslations,
const std::vector<BinaryMeasurement<Point3>> &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.

14
gtsam/sfm/TranslationRecovery.h
View File

@ -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<BinaryMeasurement<Unit3>> &relativeTranslations,
const std::vector<BinaryMeasurement<Point3>> &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<BinaryMeasurement<Unit3>> &relativeTranslations,
const std::vector<BinaryMeasurement<Point3>> &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

6
gtsam_unstable/slam/PoseToPointFactor.h
View File

@ -61,6 +61,12 @@ class PoseToPointFactor : public NoiseModelFactor2<POSE, POINT> {
traits<POINT>::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>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
/** implement functions needed to derive from Factor */
/** vector of errors

6
gtsam_unstable/timing/timeShonanAveraging.cpp
View File

@ -79,9 +79,9 @@ void saveG2oResult(string name, const Values& values, std::map<Key, Pose3> 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();

3
python/CMakeLists.txt
View File

@ -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})

5
python/gtsam/examples/Pose3ISAM2Example.py
View File

@ -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

5
python/gtsam/examples/VisualISAM2Example.py
View File

@ -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

22
python/gtsam/tests/test_Factors.py
View File

@ -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()

10
python/gtsam/tests/test_HybridFactorGraph.py
View File

@ -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()

117
python/gtsam/tests/test_Pose3.py
View File

@ -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()

28
python/gtsam/tests/test_sam.py
View File

@ -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()

19
python/gtsam/utils/plot.py
View File

@ -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())

53
tests/testRobust.cpp Normal file
View File

@ -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 <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/nonlinear/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
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<double>(x1_key, 0.0, noise);
auto between_factor = BetweenFactor<double>(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);
}

30
tests/testTranslationRecovery.cpp
View File

@ -323,6 +323,36 @@ TEST(TranslationRecovery, ThreePosesWithOneHardConstraint) {
EXPECT(assert_equal(Point3(2, 0, 0), result.at<Point3>(1), 1e-4));
EXPECT(assert_equal(Point3(1, -1, 0), result.at<Point3>(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<Pose3>(0, Pose3(Rot3(), Point3(0, 0, 0)));
poses.insert<Pose3>(1, Pose3(Rot3(), Point3(2, 0, 0)));
poses.insert<Pose3>(3, Pose3(Rot3(), Point3(1, -1, 0)));
poses.insert<Pose3>(4, Pose3(Rot3(), Point3(1, 2, 1)));
auto relativeTranslations = TranslationRecovery::SimulateMeasurements(
poses, {{0, 1}, {0, 3}, {1, 3}});
std::vector<BinaryMeasurement<Point3>> 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<Point3>(0), 1e-4));
EXPECT(assert_equal(Point3(2, 0, 0), result.at<Point3>(1), 1e-4));
EXPECT(assert_equal(Point3(1, -1, 0), result.at<Point3>(3), 1e-4));
EXPECT(assert_equal(Point3(1, 2, 1), result.at<Point3>(4), 1e-4));
}
/* ************************************************************************* */
int main() {
TestResult tr;

7
wrap/cmake/MatlabWrap.cmake
View File

@ -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}