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

Merge branch 'develop' into ta-add-methods

release/4.3a0
Akshay Krishnan 2022-05-09 22:49:34 -07:00 committed by GitHub
parent e17eddf99f fb1da007f8
commit 3a81d42d2c
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
19 changed files with 265 additions and 69 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
gtsam/discrete/tests/testDiscreteFactorGraph.cpp
View File

@ -415,16 +415,16 @@ TEST(DiscreteFactorGraph, DotWithNames) {
"graph {\n"
" size=\"5,5\";\n"
"\n"
" varC[label=\"C\"];\n"
" varA[label=\"A\"];\n"
" varB[label=\"B\"];\n"
" var0[label=\"C\"];\n"
" var1[label=\"A\"];\n"
" var2[label=\"B\"];\n"
"\n"
" factor0[label=\"\", shape=point];\n"
" varC--factor0;\n"
" varA--factor0;\n"
" var0--factor0;\n"
" var1--factor0;\n"
" factor1[label=\"\", shape=point];\n"
" varC--factor1;\n"
" varB--factor1;\n"
" var0--factor1;\n"
" var2--factor1;\n"
"}\n";
EXPECT(actual == expected);
}

91
gtsam/geometry/geometry.i
View File

@ -547,6 +547,12 @@ class EssentialMatrix {
// Standard Constructors
EssentialMatrix(const gtsam::Rot3& aRb, const gtsam::Unit3& aTb);
// Constructors from Pose3
gtsam::EssentialMatrix FromPose3(const gtsam::Pose3& _1P2_);
gtsam::EssentialMatrix FromPose3(const gtsam::Pose3& _1P2_,
Eigen::Ref<Eigen::MatrixXd> H);
// Testable
void print(string s = "") const;
bool equals(const gtsam::EssentialMatrix& pose, double tol) const;
@ -904,6 +910,12 @@ class PinholeCamera {
Eigen::Ref<Eigen::MatrixXd> Dresult_dp,
Eigen::Ref<Eigen::MatrixXd> Dresult_ddepth,
Eigen::Ref<Eigen::MatrixXd> Dresult_dcal);
gtsam::Point2 reprojectionError(const gtsam::Point3& pw, const gtsam::Point2& measured,
Eigen::Ref<Eigen::MatrixXd> Dpose,
Eigen::Ref<Eigen::MatrixXd> Dpoint,
Eigen::Ref<Eigen::MatrixXd> Dcal);
double range(const gtsam::Point3& point);
double range(const gtsam::Point3& point, Eigen::Ref<Eigen::MatrixXd> Dcamera,
Eigen::Ref<Eigen::MatrixXd> Dpoint);
@ -914,7 +926,74 @@ class PinholeCamera {
// enabling serialization functionality
void serialize() const;
};
// Forward declaration of PinholeCameraCalX is defined here.
#include <gtsam/geometry/SimpleCamera.h>
// Some typedefs for common camera types
// PinholeCameraCal3_S2 is the same as SimpleCamera above
typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;
typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2;
typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
typedef gtsam::PinholeCamera<gtsam::Cal3Fisheye> PinholeCameraCal3Fisheye;
#include <gtsam/geometry/PinholePose.h>
template <CALIBRATION>
class PinholePose {
// Standard Constructors and Named Constructors
PinholePose();
PinholePose(const gtsam::PinholePose<CALIBRATION> other);
PinholePose(const gtsam::Pose3& pose);
PinholePose(const gtsam::Pose3& pose, const CALIBRATION* K);
static This Level(const gtsam::Pose2& pose, double height);
static This Lookat(const gtsam::Point3& eye, const gtsam::Point3& target,
const gtsam::Point3& upVector, const CALIBRATION* K);
// Testable
void print(string s = "PinholePose") const;
bool equals(const This& camera, double tol) const;
// Standard Interface
gtsam::Pose3 pose() const;
CALIBRATION calibration() const;
// Manifold
This retract(Vector d) const;
Vector localCoordinates(const This& T2) const;
size_t dim() const;
static size_t Dim();
// Transformations and measurement functions
static gtsam::Point2 Project(const gtsam::Point3& cameraPoint);
pair<gtsam::Point2, bool> projectSafe(const gtsam::Point3& pw) const;
gtsam::Point2 project(const gtsam::Point3& point);
gtsam::Point2 project(const gtsam::Point3& point,
Eigen::Ref<Eigen::MatrixXd> Dpose,
Eigen::Ref<Eigen::MatrixXd> Dpoint,
Eigen::Ref<Eigen::MatrixXd> Dcal);
gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const;
gtsam::Point3 backproject(const gtsam::Point2& p, double depth,
Eigen::Ref<Eigen::MatrixXd> Dresult_dpose,
Eigen::Ref<Eigen::MatrixXd> Dresult_dp,
Eigen::Ref<Eigen::MatrixXd> Dresult_ddepth,
Eigen::Ref<Eigen::MatrixXd> Dresult_dcal);
double range(const gtsam::Point3& point);
double range(const gtsam::Point3& point, Eigen::Ref<Eigen::MatrixXd> Dcamera,
Eigen::Ref<Eigen::MatrixXd> Dpoint);
double range(const gtsam::Pose3& pose);
double range(const gtsam::Pose3& pose, Eigen::Ref<Eigen::MatrixXd> Dcamera,
Eigen::Ref<Eigen::MatrixXd> Dpose);
// enabling serialization functionality
void serialize() const;
};
typedef gtsam::PinholePose<gtsam::Cal3_S2> PinholePoseCal3_S2;
typedef gtsam::PinholePose<gtsam::Cal3DS2> PinholePoseCal3DS2;
typedef gtsam::PinholePose<gtsam::Cal3Unified> PinholePoseCal3Unified;
typedef gtsam::PinholePose<gtsam::Cal3Bundler> PinholePoseCal3Bundler;
typedef gtsam::PinholePose<gtsam::Cal3Fisheye> PinholePoseCal3Fisheye;
#include <gtsam/geometry/Similarity2.h>
class Similarity2 {
// Standard Constructors
@ -961,16 +1040,6 @@ class Similarity3 {
double scale() const;
};
// Forward declaration of PinholeCameraCalX is defined here.
#include <gtsam/geometry/SimpleCamera.h>
// Some typedefs for common camera types
// PinholeCameraCal3_S2 is the same as SimpleCamera above
typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;
typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2;
typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
typedef gtsam::PinholeCamera<gtsam::Cal3Fisheye> PinholeCameraCal3Fisheye;
template <T>
class CameraSet {
CameraSet();

5
gtsam/inference/BayesNet-inst.h
View File

@ -53,8 +53,9 @@ void BayesNet<CONDITIONAL>::dot(std::ostream& os,
auto frontals = conditional->frontals();
const Key me = frontals.front();
auto parents = conditional->parents();
for (const Key& p : parents)
os << " var" << keyFormatter(p) << "->var" << keyFormatter(me) << "\n";
for (const Key& p : parents) {
os << " var" << p << "->var" << me << "\n";
}
}
os << "}";

8
gtsam/inference/DotWriter.cpp
View File

@ -43,7 +43,7 @@ void DotWriter::drawVariable(Key key, const KeyFormatter& keyFormatter,
const boost::optional<Vector2>& position,
ostream* os) const {
// Label the node with the label from the KeyFormatter
*os << " var" << keyFormatter(key) << "[label=\"" << keyFormatter(key)
*os << " var" << key << "[label=\"" << keyFormatter(key)
<< "\"";
if (position) {
*os << ", pos=\"" << position->x() << "," << position->y() << "!\"";
@ -65,13 +65,13 @@ void DotWriter::DrawFactor(size_t i, const boost::optional<Vector2>& position,
static void ConnectVariables(Key key1, Key key2,
const KeyFormatter& keyFormatter, ostream* os) {
*os << " var" << keyFormatter(key1) << "--"
<< "var" << keyFormatter(key2) << ";\n";
*os << " var" << key1 << "--"
<< "var" << key2 << ";\n";
}
static void ConnectVariableFactor(Key key, const KeyFormatter& keyFormatter,
size_t i, ostream* os) {
*os << " var" << keyFormatter(key) << "--"
*os << " var" << key << "--"
<< "factor" << i << ";\n";
}

4
gtsam/linear/linear.i
View File

@ -671,6 +671,10 @@ virtual class DummyPreconditionerParameters : gtsam::PreconditionerParameters {
DummyPreconditionerParameters();
};
virtual class BlockJacobiPreconditionerParameters : gtsam::PreconditionerParameters {
BlockJacobiPreconditionerParameters();
};
#include <gtsam/linear/PCGSolver.h>
virtual class PCGSolverParameters : gtsam::ConjugateGradientParameters {
PCGSolverParameters();

2
gtsam/nonlinear/Marginals.h
View File

@ -121,7 +121,7 @@ public:
/** Optimize the bayes tree */
VectorValues optimize() const;
protected:
/** Compute the Bayes Tree as a helper function to the constructor */

1
gtsam/nonlinear/PriorFactor.h
View File

@ -94,7 +94,6 @@ namespace gtsam {
Vector evaluateError(const T& x, boost::optional<Matrix&> H = boost::none) const override {
if (H) (*H) = Matrix::Identity(traits<T>::GetDimension(x),traits<T>::GetDimension(x));
// manifold equivalent of z-x -> Local(x,z)
// TODO(ASL) Add Jacobians.
return -traits<T>::Local(x, prior_);
}

16
gtsam/nonlinear/nonlinear.i
View File

@ -226,6 +226,10 @@ class Values {
void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Fisheye>& camera);
void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Unified>& camera);
void insert(size_t j, const gtsam::PinholePose<gtsam::Cal3_S2>& camera);
void insert(size_t j, const gtsam::PinholePose<gtsam::Cal3Bundler>& camera);
void insert(size_t j, const gtsam::PinholePose<gtsam::Cal3Fisheye>& camera);
void insert(size_t j, const gtsam::PinholePose<gtsam::Cal3Unified>& camera);
void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
void insert(size_t j, const gtsam::NavState& nav_state);
void insert(size_t j, double c);
@ -269,6 +273,10 @@ class Values {
void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Fisheye>& camera);
void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Unified>& camera);
void update(size_t j, const gtsam::PinholePose<gtsam::Cal3_S2>& camera);
void update(size_t j, const gtsam::PinholePose<gtsam::Cal3Bundler>& camera);
void update(size_t j, const gtsam::PinholePose<gtsam::Cal3Fisheye>& camera);
void update(size_t j, const gtsam::PinholePose<gtsam::Cal3Unified>& camera);
void update(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
void update(size_t j, const gtsam::NavState& nav_state);
void update(size_t j, Vector vector);
@ -310,6 +318,10 @@ class Values {
void insert_or_assign(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
void insert_or_assign(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Fisheye>& camera);
void insert_or_assign(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Unified>& camera);
void insert_or_assign(size_t j, const gtsam::PinholePose<gtsam::Cal3_S2>& camera);
void insert_or_assign(size_t j, const gtsam::PinholePose<gtsam::Cal3Bundler>& camera);
void insert_or_assign(size_t j, const gtsam::PinholePose<gtsam::Cal3Fisheye>& camera);
void insert_or_assign(size_t j, const gtsam::PinholePose<gtsam::Cal3Unified>& camera);
void insert_or_assign(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
void insert_or_assign(size_t j, const gtsam::NavState& nav_state);
void insert_or_assign(size_t j, Vector vector);
@ -351,6 +363,10 @@ class Values {
gtsam::PinholeCamera<gtsam::Cal3Bundler>,
gtsam::PinholeCamera<gtsam::Cal3Fisheye>,
gtsam::PinholeCamera<gtsam::Cal3Unified>,
gtsam::PinholePose<gtsam::Cal3_S2>,
gtsam::PinholePose<gtsam::Cal3Bundler>,
gtsam::PinholePose<gtsam::Cal3Fisheye>,
gtsam::PinholePose<gtsam::Cal3Unified>,
gtsam::imuBias::ConstantBias,
gtsam::NavState,
Vector,

8
gtsam/sfm/SfmData.h
View File

@ -77,10 +77,14 @@ struct GTSAM_EXPORT SfmData {
size_t numberCameras() const { return cameras.size(); }
/// The track formed by series of landmark measurements
SfmTrack track(size_t idx) const { return tracks[idx]; }
const SfmTrack& track(size_t idx) const { return tracks[idx]; }
/// The camera pose at frame index `idx`
SfmCamera camera(size_t idx) const { return cameras[idx]; }
const SfmCamera& camera(size_t idx) const { return cameras[idx]; }
/// Getters
const std::vector<SfmCamera>& cameraList() const { return cameras; }
const std::vector<SfmTrack>& trackList() const { return tracks; }
/**
* @brief Create projection factors using keys i and P(j)

19
gtsam/sfm/sfm.i
View File

@ -11,6 +11,8 @@ class SfmTrack {
SfmTrack();
SfmTrack(const gtsam::Point3& pt);
const Point3& point3() const;
Point3 p;
double r;
double g;
@ -36,12 +38,15 @@ class SfmData {
static gtsam::SfmData FromBundlerFile(string filename);
static gtsam::SfmData FromBalFile(string filename);
std::vector<gtsam::SfmTrack>& trackList() const;
std::vector<gtsam::PinholeCamera<gtsam::Cal3Bundler>>& cameraList() const;
void addTrack(const gtsam::SfmTrack& t);
void addCamera(const gtsam::SfmCamera& cam);
size_t numberTracks() const;
size_t numberCameras() const;
gtsam::SfmTrack track(size_t idx) const;
gtsam::PinholeCamera<gtsam::Cal3Bundler> camera(size_t idx) const;
gtsam::SfmTrack& track(size_t idx) const;
gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera(size_t idx) const;
gtsam::NonlinearFactorGraph generalSfmFactors(
const gtsam::SharedNoiseModel& model =
@ -99,6 +104,12 @@ class BinaryMeasurementsPoint3 {
size_t size() const;
gtsam::BinaryMeasurement<gtsam::Point3> at(size_t idx) const;
void push_back(const gtsam::BinaryMeasurement<gtsam::Point3>& measurement);
class BinaryMeasurementsRot3 {
BinaryMeasurementsRot3();
size_t size() const;
gtsam::BinaryMeasurement<gtsam::Rot3> at(size_t idx) const;
void push_back(const gtsam::BinaryMeasurement<gtsam::Rot3>& measurement);
};
#include <gtsam/sfm/ShonanAveraging.h>
@ -194,6 +205,10 @@ class ShonanAveraging2 {
};
class ShonanAveraging3 {
ShonanAveraging3(
const std::vector<gtsam::BinaryMeasurement<gtsam::Rot3>>& measurements,
const gtsam::ShonanAveragingParameters3& parameters =
gtsam::ShonanAveragingParameters3());
ShonanAveraging3(string g2oFile);
ShonanAveraging3(string g2oFile,
const gtsam::ShonanAveragingParameters3& parameters);

1
gtsam/slam/dataset.h
View File

@ -224,6 +224,7 @@ parse3DFactors(const std::string &filename,
using BinaryMeasurementsUnit3 = std::vector<BinaryMeasurement<Unit3>>;
using BinaryMeasurementsPoint3 = std::vector<BinaryMeasurement<Point3>>;
using BinaryMeasurementsRot3 = std::vector<BinaryMeasurement<Rot3>>;
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
inline boost::optional<IndexedPose> GTSAM_DEPRECATED

16
gtsam/slam/slam.i
View File

@ -90,6 +90,22 @@ typedef gtsam::GeneralSFMFactor<gtsam::PinholeCamera<gtsam::Cal3Unified>,
gtsam::Point3>
GeneralSFMFactorCal3Unified;
typedef gtsam::GeneralSFMFactor<gtsam::PinholePose<gtsam::Cal3_S2>,
gtsam::Point3>
GeneralSFMFactorPoseCal3_S2;
typedef gtsam::GeneralSFMFactor<gtsam::PinholePose<gtsam::Cal3DS2>,
gtsam::Point3>
GeneralSFMFactorPoseCal3DS2;
typedef gtsam::GeneralSFMFactor<gtsam::PinholePose<gtsam::Cal3Bundler>,
gtsam::Point3>
GeneralSFMFactorPoseCal3Bundler;
typedef gtsam::GeneralSFMFactor<gtsam::PinholePose<gtsam::Cal3Fisheye>,
gtsam::Point3>
GeneralSFMFactorPoseCal3Fisheye;
typedef gtsam::GeneralSFMFactor<gtsam::PinholePose<gtsam::Cal3Unified>,
gtsam::Point3>
GeneralSFMFactorPoseCal3Unified;
template <CALIBRATION = {gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler,
gtsam::Cal3Fisheye, gtsam::Cal3Unified}>
virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor {

40
gtsam/slam/tests/testPriorFactor.cpp
View File

@ -5,12 +5,16 @@
* @date Nov 4, 2014
*/
#include <gtsam/base/Vector.h>
#include <gtsam/nonlinear/PriorFactor.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Vector.h>
#include <gtsam/navigation/ImuBias.h>
#include <gtsam/nonlinear/PriorFactor.h>
#include <gtsam/nonlinear/factorTesting.h>
using namespace std;
using namespace std::placeholders;
using namespace gtsam;
using namespace imuBias;
/* ************************************************************************* */
@ -23,16 +27,44 @@ TEST(PriorFactor, ConstructorScalar) {
// Constructor vector3
TEST(PriorFactor, ConstructorVector3) {
SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 1.0);
PriorFactor<Vector3> factor(1, Vector3(1,2,3), model);
PriorFactor<Vector3> factor(1, Vector3(1, 2, 3), model);
}
// Constructor dynamic sized vector
TEST(PriorFactor, ConstructorDynamicSizeVector) {
Vector v(5); v << 1, 2, 3, 4, 5;
Vector v(5);
v << 1, 2, 3, 4, 5;
SharedNoiseModel model = noiseModel::Isotropic::Sigma(5, 1.0);
PriorFactor<Vector> factor(1, v, model);
}
Vector callEvaluateError(const PriorFactor<ConstantBias>& factor,
const ConstantBias& bias) {
return factor.evaluateError(bias);
}
// Test for imuBias::ConstantBias
TEST(PriorFactor, ConstantBias) {
Vector3 biasAcc(1, 2, 3);
Vector3 biasGyro(0.1, 0.2, 0.3);
ConstantBias bias(biasAcc, biasGyro);
PriorFactor<ConstantBias> factor(1, bias,
noiseModel::Isotropic::Sigma(6, 0.1));
Values values;
values.insert(1, bias);
EXPECT_DOUBLES_EQUAL(0.0, factor.error(values), 1e-8);
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
ConstantBias incorrectBias(
(Vector6() << 1.1, 2.1, 3.1, 0.2, 0.3, 0.4).finished());
values.clear();
values.insert(1, incorrectBias);
EXPECT_DOUBLES_EQUAL(3.0, factor.error(values), 1e-8);
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
}
/* ************************************************************************* */
int main() {
TestResult tr;

18
gtsam/symbolic/tests/testSymbolicBayesNet.cpp
View File

@ -104,16 +104,16 @@ TEST(SymbolicBayesNet, Dot) {
"digraph {\n"
" size=\"5,5\";\n"
"\n"
" vara1[label=\"a1\", pos=\"1,2!\", shape=box];\n"
" vara2[label=\"a2\", pos=\"2,2!\", shape=box];\n"
" varx1[label=\"x1\", pos=\"1,1!\"];\n"
" varx2[label=\"x2\", pos=\"2,1!\"];\n"
" varx3[label=\"x3\", pos=\"3,1!\"];\n"
" var6989586621679009793[label=\"a1\", pos=\"1,2!\", shape=box];\n"
" var6989586621679009794[label=\"a2\", pos=\"2,2!\", shape=box];\n"
" var8646911284551352321[label=\"x1\", pos=\"1,1!\"];\n"
" var8646911284551352322[label=\"x2\", pos=\"2,1!\"];\n"
" var8646911284551352323[label=\"x3\", pos=\"3,1!\"];\n"
"\n"
" varx1->varx2\n"
" vara1->varx2\n"
" varx2->varx3\n"
" vara2->varx3\n"
" var8646911284551352321->var8646911284551352322\n"
" var6989586621679009793->var8646911284551352322\n"
" var8646911284551352322->var8646911284551352323\n"
" var6989586621679009794->var8646911284551352323\n"
"}");
}

10
python/CMakeLists.txt
View File

@ -49,6 +49,7 @@ set(ignore
gtsam::KeyVector
gtsam::BinaryMeasurementsPoint3
gtsam::BinaryMeasurementsUnit3
gtsam::BinaryMeasurementsRot3
gtsam::DiscreteKey
gtsam::KeyPairDoubleMap)
@ -108,6 +109,14 @@ file(GLOB GTSAM_PYTHON_UTIL_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam/utils/*.py"
foreach(util_file ${GTSAM_PYTHON_UTIL_FILES})
configure_file(${util_file} "${GTSAM_MODULE_PATH}/utils/${test_file}" COPYONLY)
endforeach()
file(GLOB GTSAM_PYTHON_PREAMBLE_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam/preamble/*.h")
foreach(util_file ${GTSAM_PYTHON_PREAMBLE_FILES})
configure_file(${util_file} "${GTSAM_MODULE_PATH}/preamble/${test_file}" COPYONLY)
endforeach()
file(GLOB GTSAM_PYTHON_SPECIALIZATION_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam/specializations/*.h")
foreach(util_file ${GTSAM_PYTHON_SPECIALIZATION_FILES})
configure_file(${util_file} "${GTSAM_MODULE_PATH}/specializations/${test_file}" COPYONLY)
endforeach()
# Common directory for data/datasets stored with the package.
# This will store the data in the Python site package directly.
@ -131,6 +140,7 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON)
gtsam::FixedLagSmootherKeyTimestampMapValue
gtsam::BinaryMeasurementsPoint3
gtsam::BinaryMeasurementsUnit3
gtsam::BinaryMeasurementsRot3
gtsam::CameraSetCal3_S2
gtsam::CameraSetCal3Bundler
gtsam::CameraSetCal3Unified

16
python/gtsam/preamble/sfm.h
View File

@ -9,4 +9,18 @@
* automatic STL binding, such that the raw objects can be accessed in Python.
* Without this they will be automatically converted to a Python object, and all
* mutations on Python side will not be reflected on C++.
*/
*/
// Including <stl.h> can cause some serious hard-to-debug bugs!!!
// #include <pybind11/stl.h>
#include <pybind11/stl_bind.h>
PYBIND11_MAKE_OPAQUE(
std::vector<gtsam::SfmTrack>);
PYBIND11_MAKE_OPAQUE(
std::vector<gtsam::SfmCamera>);
PYBIND11_MAKE_OPAQUE(
std::vector<gtsam::BinaryMeasurement<gtsam::Unit3>>);
PYBIND11_MAKE_OPAQUE(
std::vector<gtsam::BinaryMeasurement<gtsam::Rot3>>);

15
python/gtsam/specializations/sfm.h
View File

@ -15,4 +15,19 @@ py::bind_vector<std::vector<gtsam::BinaryMeasurement<gtsam::Point3> > >(
m_, "BinaryMeasurementsPoint3");
py::bind_vector<std::vector<gtsam::BinaryMeasurement<gtsam::Unit3> > >(
m_, "BinaryMeasurementsUnit3");
py::bind_vector<std::vector<gtsam::BinaryMeasurement<gtsam::Rot3> > >(
m_, "BinaryMeasurementsRot3");
py::bind_map<gtsam::KeyPairDoubleMap>(m_, "KeyPairDoubleMap");
py::bind_vector<
std::vector<gtsam::SfmTrack> >(
m_, "SfmTracks");
py::bind_vector<
std::vector<gtsam::SfmCamera> >(
m_, "SfmCameras");
py::bind_vector<
std::vector<std::pair<size_t, gtsam::Point2>>>(
m_, "SfmMeasurementVector"
);

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

@ -11,12 +11,12 @@ Author: Frank Dellaert
# pylint: disable=no-name-in-module, invalid-name
import unittest
import textwrap
import unittest
import gtsam
from gtsam import (DiscreteBayesNet, DiscreteConditional, DiscreteFactorGraph,
DiscreteKeys, DiscreteDistribution, DiscreteValues, Ordering)
from gtsam import (DiscreteBayesNet, DiscreteConditional, DiscreteDistribution,
DiscreteFactorGraph, DiscreteKeys, DiscreteValues, Ordering)
from gtsam.utils.test_case import GtsamTestCase
# Some keys:
@ -152,10 +152,10 @@ class TestDiscreteBayesNet(GtsamTestCase):
var4[label="4"];
var5[label="5"];
var6[label="6"];
vara0[label="a0", pos="0,2!"];
var6989586621679009792[label="a0", pos="0,2!"];
var4->var6
vara0->var3
var6989586621679009792->var3
var3->var5
var6->var5
}"""

40
tests/testNonlinearFactorGraph.cpp
View File

@ -335,21 +335,21 @@ TEST(NonlinearFactorGraph, dot) {
"graph {\n"
" size=\"5,5\";\n"
"\n"
" varl1[label=\"l1\"];\n"
" varx1[label=\"x1\"];\n"
" varx2[label=\"x2\"];\n"
" var7782220156096217089[label=\"l1\"];\n"
" var8646911284551352321[label=\"x1\"];\n"
" var8646911284551352322[label=\"x2\"];\n"
"\n"
" factor0[label=\"\", shape=point];\n"
" varx1--factor0;\n"
" var8646911284551352321--factor0;\n"
" factor1[label=\"\", shape=point];\n"
" varx1--factor1;\n"
" varx2--factor1;\n"
" var8646911284551352321--factor1;\n"
" var8646911284551352322--factor1;\n"
" factor2[label=\"\", shape=point];\n"
" varx1--factor2;\n"
" varl1--factor2;\n"
" var8646911284551352321--factor2;\n"
" var7782220156096217089--factor2;\n"
" factor3[label=\"\", shape=point];\n"
" varx2--factor3;\n"
" varl1--factor3;\n"
" var8646911284551352322--factor3;\n"
" var7782220156096217089--factor3;\n"
"}\n";
const NonlinearFactorGraph fg = createNonlinearFactorGraph();
@ -363,21 +363,21 @@ TEST(NonlinearFactorGraph, dot_extra) {
"graph {\n"
" size=\"5,5\";\n"
"\n"
" varl1[label=\"l1\", pos=\"0,0!\"];\n"
" varx1[label=\"x1\", pos=\"1,0!\"];\n"
" varx2[label=\"x2\", pos=\"1,1.5!\"];\n"
" var7782220156096217089[label=\"l1\", pos=\"0,0!\"];\n"
" var8646911284551352321[label=\"x1\", pos=\"1,0!\"];\n"
" var8646911284551352322[label=\"x2\", pos=\"1,1.5!\"];\n"
"\n"
" factor0[label=\"\", shape=point];\n"
" varx1--factor0;\n"
" var8646911284551352321--factor0;\n"
" factor1[label=\"\", shape=point];\n"
" varx1--factor1;\n"
" varx2--factor1;\n"
" var8646911284551352321--factor1;\n"
" var8646911284551352322--factor1;\n"
" factor2[label=\"\", shape=point];\n"
" varx1--factor2;\n"
" varl1--factor2;\n"
" var8646911284551352321--factor2;\n"
" var7782220156096217089--factor2;\n"
" factor3[label=\"\", shape=point];\n"
" varx2--factor3;\n"
" varl1--factor3;\n"
" var8646911284551352322--factor3;\n"
" var7782220156096217089--factor3;\n"
"}\n";
const NonlinearFactorGraph fg = createNonlinearFactorGraph();