Merge branch 'develop' into feature/rollingShutterSmartFactors

release/4.3a0
lcarlone 2021-08-12 17:37:43 -04:00
commit 0b002e6f24
25 changed files with 351 additions and 195 deletions

View File

@ -9,10 +9,10 @@ tar -zxf ${BOOST_FOLDER}.tar.gz
# Bootstrap
cd ${BOOST_FOLDER}/
./bootstrap.sh
./bootstrap.sh --with-libraries=serialization,filesystem,thread,system,atomic,date_time,timer,chrono,program_options,regex
# Build and install
sudo ./b2 install
sudo ./b2 -j$(nproc) install
# Rebuild ld cache
sudo ldconfig

View File

@ -92,7 +92,11 @@ function build ()
configure
make -j2
if [ "$(uname)" == "Linux" ]; then
make -j$(nproc)
elif [ "$(uname)" == "Darwin" ]; then
make -j$(sysctl -n hw.physicalcpu)
fi
finish
}
@ -105,8 +109,12 @@ function test ()
configure
# Actual build:
make -j2 check
# Actual testing
if [ "$(uname)" == "Linux" ]; then
make -j$(nproc)
elif [ "$(uname)" == "Darwin" ]; then
make -j$(sysctl -n hw.physicalcpu)
fi
finish
}

View File

@ -47,8 +47,7 @@ jobs:
- name: Checkout
uses: actions/checkout@v2
- name: Install (Linux)
if: runner.os == 'Linux'
- name: Install Dependencies
run: |
# LLVM (clang) 9 is not in Bionic's repositories so we add the official LLVM repository.
if [ "${{ matrix.compiler }}" = "clang" ] && [ "${{ matrix.version }}" = "9" ]; then
@ -63,7 +62,7 @@ jobs:
fi
sudo apt-get -y update
sudo apt-get install cmake build-essential pkg-config libpython-dev python-numpy libicu-dev
sudo apt-get -y install cmake build-essential pkg-config libpython-dev python-numpy libicu-dev
if [ "${{ matrix.compiler }}" = "gcc" ]; then
sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib
@ -79,7 +78,5 @@ jobs:
run: |
bash .github/scripts/boost.sh
- name: Build and Test (Linux)
if: runner.os == 'Linux'
run: |
bash .github/scripts/unix.sh -t
- name: Build and Test
run: bash .github/scripts/unix.sh -t

View File

@ -34,8 +34,7 @@ jobs:
- name: Checkout
uses: actions/checkout@v2
- name: Install (macOS)
if: runner.os == 'macOS'
- name: Install Dependencies
run: |
brew install cmake ninja
brew install boost
@ -48,7 +47,5 @@ jobs:
echo "CC=clang" >> $GITHUB_ENV
echo "CXX=clang++" >> $GITHUB_ENV
fi
- name: Build and Test (macOS)
if: runner.os == 'macOS'
run: |
bash .github/scripts/unix.sh -t
- name: Build and Test
run: bash .github/scripts/unix.sh -t

View File

@ -81,7 +81,7 @@ jobs:
fi
sudo apt-get -y update
sudo apt install cmake build-essential pkg-config libpython-dev python-numpy libboost-all-dev
sudo apt-get -y install cmake build-essential pkg-config libpython-dev python-numpy libboost-all-dev
if [ "${{ matrix.compiler }}" = "gcc" ]; then
sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib

View File

@ -70,7 +70,7 @@ jobs:
fi
sudo apt-get -y update
sudo apt install cmake build-essential pkg-config libpython-dev python-numpy libicu-dev
sudo apt-get -y install cmake build-essential pkg-config libpython-dev python-numpy libicu-dev
if [ "${{ matrix.compiler }}" = "gcc" ]; then
sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib

View File

@ -12,13 +12,16 @@ jobs:
CTEST_PARALLEL_LEVEL: 2
CMAKE_BUILD_TYPE: ${{ matrix.build_type }}
GTSAM_BUILD_UNSTABLE: ${{ matrix.build_unstable }}
BOOST_VERSION: 1.72.0
BOOST_EXE: boost_1_72_0-msvc-14.2
strategy:
fail-fast: false
matrix:
# Github Actions requires a single row to be added to the build matrix.
# See https://help.github.com/en/articles/workflow-syntax-for-github-actions.
name: [
#TODO This build keeps timing out, need to understand why.
#TODO This build fails, need to understand why.
# windows-2016-cl,
windows-2019-cl,
]
@ -26,28 +29,29 @@ jobs:
build_type: [Debug, Release]
build_unstable: [ON]
include:
#TODO This build keeps timing out, need to understand why.
#TODO This build fails, need to understand why.
# - name: windows-2016-cl
# os: windows-2016
# compiler: cl
# platform: 32
- name: windows-2019-cl
os: windows-2019
compiler: cl
platform: 64
steps:
- name: Checkout
uses: actions/checkout@v2
- name: Install (Windows)
if: runner.os == 'Windows'
- name: Install Dependencies
shell: powershell
run: |
Invoke-Expression (New-Object System.Net.WebClient).DownloadString('https://get.scoop.sh')
scoop install cmake --global # So we don't get issues with CMP0074 policy
scoop install ninja --global
if ("${{ matrix.compiler }}".StartsWith("clang")) {
scoop install llvm --global
}
if ("${{ matrix.compiler }}" -eq "gcc") {
# Chocolatey GCC is broken on the windows-2019 image.
# See: https://github.com/DaanDeMeyer/doctest/runs/231595515
@ -55,27 +59,38 @@ jobs:
scoop install gcc --global
echo "CC=gcc" >> $GITHUB_ENV
echo "CXX=g++" >> $GITHUB_ENV
} elseif ("${{ matrix.compiler }}" -eq "clang") {
echo "CC=clang" >> $GITHUB_ENV
echo "CXX=clang++" >> $GITHUB_ENV
} else {
echo "CC=${{ matrix.compiler }}" >> $GITHUB_ENV
echo "CXX=${{ matrix.compiler }}" >> $GITHUB_ENV
echo "CC=${{ matrix.compiler }}" >> $env:GITHUB_ENV
echo "CXX=${{ matrix.compiler }}" >> $env:GITHUB_ENV
}
# Scoop modifies the PATH so we make the modified PATH global.
echo "$env:PATH" >> $GITHUB_PATH
echo "$env:PATH" >> $env:GITHUB_PATH
- name: Download and install Boost
uses: MarkusJx/install-boost@v1.0.1
id: install-boost
with:
boost_version: 1.72.0
toolset: msvc14.2
- name: Install Boost
shell: powershell
run: |
# Snippet from: https://github.com/actions/virtual-environments/issues/2667
$BOOST_PATH = "C:\hostedtoolcache\windows\Boost\$env:BOOST_VERSION\x86_64"
- name: Build (Windows)
if: runner.os == 'Windows'
env:
BOOST_ROOT: ${{ steps.install-boost.outputs.BOOST_ROOT }}
# Use the prebuilt binary for Windows
$Url = "https://sourceforge.net/projects/boost/files/boost-binaries/$env:BOOST_VERSION/$env:BOOST_EXE-${{matrix.platform}}.exe"
(New-Object System.Net.WebClient).DownloadFile($Url, "$env:TEMP\boost.exe")
Start-Process -Wait -FilePath "$env:TEMP\boost.exe" "/SILENT","/SP-","/SUPPRESSMSGBOXES","/DIR=$BOOST_PATH"
# Set the BOOST_ROOT variable
echo "BOOST_ROOT=$BOOST_PATH" >> $env:GITHUB_ENV
- name: Checkout
uses: actions/checkout@v2
- name: Build
run: |
cmake -E remove_directory build
cmake -B build -S . -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DBOOST_ROOT="${env:BOOST_ROOT}" -DBOOST_INCLUDEDIR="${env:BOOST_ROOT}\boost\include" -DBOOST_LIBRARYDIR="${env:BOOST_ROOT}\lib"

View File

@ -30,7 +30,7 @@ namespace gtsam {
* \nosubgrouping
*/
template<typename Calibration>
class PinholeCamera: public PinholeBaseK<Calibration> {
class GTSAM_EXPORT PinholeCamera: public PinholeBaseK<Calibration> {
public:

View File

@ -31,7 +31,7 @@ namespace gtsam {
* \nosubgrouping
*/
template<typename CALIBRATION>
class PinholeBaseK: public PinholeBase {
class GTSAM_EXPORT PinholeBaseK: public PinholeBase {
private:

View File

@ -982,6 +982,24 @@ gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Fisheye& cameras,
gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Unified& cameras,
const gtsam::Point2Vector& measurements,
double rank_tol, bool optimize);
gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses,
gtsam::Cal3_S2* sharedCal,
const gtsam::Point2Vector& measurements,
const gtsam::Point3& initialEstimate);
gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses,
gtsam::Cal3DS2* sharedCal,
const gtsam::Point2Vector& measurements,
const gtsam::Point3& initialEstimate);
gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses,
gtsam::Cal3Bundler* sharedCal,
const gtsam::Point2Vector& measurements,
const gtsam::Point3& initialEstimate);
gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3_S2& cameras,
const gtsam::Point2Vector& measurements,
const gtsam::Point3& initialEstimate);
gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3Bundler& cameras,
const gtsam::Point2Vector& measurements,
const gtsam::Point3& initialEstimate);
#include <gtsam/geometry/BearingRange.h>
template <POSE, POINT, BEARING, RANGE>

View File

@ -182,6 +182,16 @@ protected:
return item->second;
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_NVP(index_);
ar & BOOST_SERIALIZATION_NVP(nFactors_);
ar & BOOST_SERIALIZATION_NVP(nEntries_);
}
/// @}
};

View File

@ -315,6 +315,26 @@ class GTSAM_EXPORT ISAM2 : public BayesTree<ISAM2Clique> {
void removeVariables(const KeySet& unusedKeys);
void updateDelta(bool forceFullSolve = false) const;
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & boost::serialization::base_object<BayesTree<ISAM2Clique> >(*this);
ar & BOOST_SERIALIZATION_NVP(theta_);
ar & BOOST_SERIALIZATION_NVP(variableIndex_);
ar & BOOST_SERIALIZATION_NVP(delta_);
ar & BOOST_SERIALIZATION_NVP(deltaNewton_);
ar & BOOST_SERIALIZATION_NVP(RgProd_);
ar & BOOST_SERIALIZATION_NVP(deltaReplacedMask_);
ar & BOOST_SERIALIZATION_NVP(nonlinearFactors_);
ar & BOOST_SERIALIZATION_NVP(linearFactors_);
ar & BOOST_SERIALIZATION_NVP(doglegDelta_);
ar & BOOST_SERIALIZATION_NVP(fixedVariables_);
ar & BOOST_SERIALIZATION_NVP(update_count_);
}
}; // ISAM2
/// traits

View File

@ -58,22 +58,23 @@ TEST(Expression, Constant) {
/* ************************************************************************* */
// Leaf
TEST(Expression, Leaf) {
Rot3_ R(100);
const Key key = 100;
Rot3_ R(key);
Values values;
values.insert(100, someR);
values.insert(key, someR);
Rot3 actual2 = R.value(values);
EXPECT(assert_equal(someR, actual2));
}
/* ************************************************************************* */
// Many Leaves
// Test the function `createUnknowns` to create many leaves at once.
TEST(Expression, Leaves) {
Values values;
Point3 somePoint(1, 2, 3);
const Point3 somePoint(1, 2, 3);
values.insert(Symbol('p', 10), somePoint);
std::vector<Point3_> points = createUnknowns<Point3>(10, 'p', 1);
EXPECT(assert_equal(somePoint, points.back().value(values)));
std::vector<Point3_> pointExpressions = createUnknowns<Point3>(10, 'p', 1);
EXPECT(assert_equal(somePoint, pointExpressions.back().value(values)));
}
/* ************************************************************************* */
@ -88,29 +89,34 @@ double f2(const Point3& p, OptionalJacobian<1, 3> H) {
Vector f3(const Point3& p, OptionalJacobian<Eigen::Dynamic, 3> H) {
return p;
}
Point3_ p(1);
Point3_ pointExpression(1);
set<Key> expected = list_of(1);
} // namespace unary
// Create a unary expression that takes another expression as a single argument.
TEST(Expression, Unary1) {
using namespace unary;
Expression<Point2> e(f1, p);
EXPECT(expected == e.keys());
}
TEST(Expression, Unary2) {
using namespace unary;
Double_ e(f2, p);
EXPECT(expected == e.keys());
Expression<Point2> unaryExpression(f1, pointExpression);
EXPECT(expected == unaryExpression.keys());
}
// Check that also works with a scalar return value.
TEST(Expression, Unary2) {
using namespace unary;
Double_ unaryExpression(f2, pointExpression);
EXPECT(expected == unaryExpression.keys());
}
/* ************************************************************************* */
// Unary(Leaf), dynamic
TEST(Expression, Unary3) {
using namespace unary;
// Expression<Vector> e(f3, p);
// TODO(yetongumich): dynamic output arguments do not work yet!
// Expression<Vector> unaryExpression(f3, pointExpression);
// EXPECT(expected == unaryExpression.keys());
}
/* ************************************************************************* */
// Simple test class that implements the `VectorSpace` protocol.
class Class : public Point3 {
public:
enum {dimension = 3};
@ -133,16 +139,20 @@ template<> struct traits<Class> : public internal::VectorSpace<Class> {};
// Nullary Method
TEST(Expression, NullaryMethod) {
// Create expression
Expression<Class> p(67);
Expression<double> norm_(p, &Class::norm);
const Key key(67);
Expression<Class> classExpression(key);
// Make expression from a class method, note how it differs from the function
// expressions by leading with the class expression in the constructor.
Expression<double> norm_(classExpression, &Class::norm);
// Create Values
Values values;
values.insert(67, Class(3, 4, 5));
values.insert(key, Class(3, 4, 5));
// Check dims as map
std::map<Key, int> map;
norm_.dims(map);
norm_.dims(map); // TODO(yetongumich): Change to google style pointer convention.
LONGS_EQUAL(1, map.size());
// Get value and Jacobians
@ -150,9 +160,10 @@ TEST(Expression, NullaryMethod) {
double actual = norm_.value(values, H);
// Check all
EXPECT(actual == sqrt(50));
const double norm = sqrt(3*3 + 4*4 + 5*5);
EXPECT(actual == norm);
Matrix expected(1, 3);
expected << 3.0 / sqrt(50.0), 4.0 / sqrt(50.0), 5.0 / sqrt(50.0);
expected << 3.0 / norm, 4.0 / norm, 5.0 / norm;
EXPECT(assert_equal(expected, H[0]));
}
@ -170,21 +181,21 @@ Point3_ p_cam(x, &Pose3::transformTo, p);
}
/* ************************************************************************* */
// Check that creating an expression to double compiles
// Check that creating an expression to double compiles.
TEST(Expression, BinaryToDouble) {
using namespace binary;
Double_ p_cam(doubleF, x, p);
}
/* ************************************************************************* */
// keys
// Check keys of an expression created from class method.
TEST(Expression, BinaryKeys) {
set<Key> expected = list_of(1)(2);
EXPECT(expected == binary::p_cam.keys());
}
/* ************************************************************************* */
// dimensions
// Check dimensions by calling `dims` method.
TEST(Expression, BinaryDimensions) {
map<Key, int> actual, expected = map_list_of<Key, int>(1, 6)(2, 3);
binary::p_cam.dims(actual);
@ -192,7 +203,7 @@ TEST(Expression, BinaryDimensions) {
}
/* ************************************************************************* */
// dimensions
// Check dimensions of execution trace.
TEST(Expression, BinaryTraceSize) {
typedef internal::BinaryExpression<Point3, Pose3, Point3> Binary;
size_t expectedTraceSize = sizeof(Binary::Record);
@ -247,6 +258,7 @@ TEST(Expression, TreeTraceSize) {
}
/* ************************************************************************* */
// Test compose operation with * operator.
TEST(Expression, compose1) {
// Create expression
Rot3_ R1(1), R2(2);
@ -258,7 +270,7 @@ TEST(Expression, compose1) {
}
/* ************************************************************************* */
// Test compose with arguments referring to the same rotation
// Test compose with arguments referring to the same rotation.
TEST(Expression, compose2) {
// Create expression
Rot3_ R1(1), R2(1);
@ -270,7 +282,7 @@ TEST(Expression, compose2) {
}
/* ************************************************************************* */
// Test compose with one arguments referring to constant rotation
// Test compose with one arguments referring to constant rotation.
TEST(Expression, compose3) {
// Create expression
Rot3_ R1(Rot3::identity()), R2(3);
@ -282,7 +294,7 @@ TEST(Expression, compose3) {
}
/* ************************************************************************* */
// Test with ternary function
// Test with ternary function.
Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, OptionalJacobian<3, 3> H1,
OptionalJacobian<3, 3> H2, OptionalJacobian<3, 3> H3) {
// return dummy derivatives (not correct, but that's ok for testing here)
@ -306,6 +318,7 @@ TEST(Expression, ternary) {
}
/* ************************************************************************* */
// Test scalar multiplication with * operator.
TEST(Expression, ScalarMultiply) {
const Key key(67);
const Point3_ expr = 23 * Point3_(key);
@ -336,6 +349,7 @@ TEST(Expression, ScalarMultiply) {
}
/* ************************************************************************* */
// Test sum with + operator.
TEST(Expression, BinarySum) {
const Key key(67);
const Point3_ sum_ = Point3_(key) + Point3_(Point3(1, 1, 1));
@ -366,6 +380,7 @@ TEST(Expression, BinarySum) {
}
/* ************************************************************************* */
// Test sum of 3 variables with + operator.
TEST(Expression, TripleSum) {
const Key key(67);
const Point3_ p1_(Point3(1, 1, 1)), p2_(key);
@ -387,6 +402,7 @@ TEST(Expression, TripleSum) {
}
/* ************************************************************************* */
// Test sum with += operator.
TEST(Expression, PlusEqual) {
const Key key(67);
const Point3_ p1_(Point3(1, 1, 1)), p2_(key);
@ -461,11 +477,12 @@ TEST(Expression, WeightedSum) {
EXPECT(actual_dims == expected_dims);
Values values;
values.insert<Point3>(key1, Point3(1, 0, 0));
values.insert<Point3>(key2, Point3(0, 1, 0));
const Point3 point1(1, 0, 0), point2(0, 1, 0);
values.insert<Point3>(key1, point1);
values.insert<Point3>(key2, point2);
// Check value
const Point3 expected = 17 * Point3(1, 0, 0) + 23 * Point3(0, 1, 0);
const Point3 expected = 17 * point1 + 23 * point2;
EXPECT(assert_equal(expected, weighted_sum_.value(values)));
// Check value + Jacobians

View File

@ -17,6 +17,7 @@
*/
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Pose2.h>
@ -31,6 +32,30 @@ using namespace std;
using namespace gtsam;
using namespace gtsam::serializationTestHelpers;
/* ************************************************************************* */
// Create GUIDs for Noisemodels
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Base , "gtsam_noiseModel_mEstimator_Base");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Null , "gtsam_noiseModel_mEstimator_Null");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Fair , "gtsam_noiseModel_mEstimator_Fair");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Huber, "gtsam_noiseModel_mEstimator_Huber");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Tukey, "gtsam_noiseModel_mEstimator_Tukey");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic,"gtsam_noiseModel_Isotropic");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Robust, "gtsam_noiseModel_Robust");
BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel");
BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
/* ************************************************************************* */
// Create GUIDs for factors
BOOST_CLASS_EXPORT_GUID(gtsam::PriorFactor<gtsam::Pose3>, "gtsam::PriorFactor<gtsam::Pose3>");
BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor");
BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor");
BOOST_CLASS_EXPORT_GUID(gtsam::GaussianConditional , "gtsam::GaussianConditional");
/* ************************************************************************* */
// Export all classes derived from Value
GTSAM_VALUE_EXPORT(gtsam::Cal3_S2);
@ -82,6 +107,61 @@ TEST (Serialization, TemplatedValues) {
EXPECT(equalsBinary(values));
}
TEST(Serialization, ISAM2) {
gtsam::ISAM2Params parameters;
gtsam::ISAM2 solver(parameters);
gtsam::NonlinearFactorGraph graph;
gtsam::Values initialValues;
initialValues.clear();
gtsam::Vector6 temp6;
for (int i = 0; i < 6; ++i) {
temp6[i] = 0.0001;
}
gtsam::noiseModel::Diagonal::shared_ptr noiseModel = gtsam::noiseModel::Diagonal::Sigmas(temp6);
gtsam::Pose3 pose0(gtsam::Rot3(), gtsam::Point3(0, 0, 0));
gtsam::Symbol symbol0('x', 0);
graph.add(gtsam::PriorFactor<gtsam::Pose3>(symbol0, pose0, noiseModel));
initialValues.insert(symbol0, pose0);
solver.update(graph, initialValues,
gtsam::FastVector<gtsam::FactorIndex>());
std::string binaryPath = "saved_solver.dat";
try {
std::ofstream outputStream(binaryPath);
boost::archive::binary_oarchive outputArchive(outputStream);
outputArchive << solver;
} catch(...) {
EXPECT(false);
}
gtsam::ISAM2 solverFromDisk;
try {
std::ifstream ifs(binaryPath);
boost::archive::binary_iarchive inputArchive(ifs);
inputArchive >> solverFromDisk;
} catch(...) {
EXPECT(false);
}
gtsam::Pose3 p1, p2;
try {
p1 = solver.calculateEstimate<gtsam::Pose3>(symbol0);
} catch(std::exception &e) {
EXPECT(false);
}
try {
p2 = solverFromDisk.calculateEstimate<gtsam::Pose3>(symbol0);
} catch(std::exception &e) {
EXPECT(false);
}
EXPECT(assert_equal(p1, p2));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

View File

@ -955,7 +955,9 @@ static BinaryMeasurement<Rot2> convertPose2ToBinaryMeasurementRot2(
"parseMeasurements<Rot2> can only convert Pose2 measurements "
"with Gaussian noise models.");
const Matrix3 M = gaussian->covariance();
auto model = noiseModel::Gaussian::Covariance(M.block<1, 1>(2, 2));
// the (2,2) entry of Pose2's covariance corresponds to Rot2's covariance
// because the tangent space of Pose2 is ordered as (vx, vy, w)
auto model = noiseModel::Isotropic::Variance(1, M(2, 2));
return BinaryMeasurement<Rot2>(f->key1(), f->key2(), f->measured().rotation(),
model);
}
@ -1001,7 +1003,9 @@ static BinaryMeasurement<Rot3> convert(
"parseMeasurements<Rot3> can only convert Pose3 measurements "
"with Gaussian noise models.");
const Matrix6 M = gaussian->covariance();
auto model = noiseModel::Gaussian::Covariance(M.block<3, 3>(3, 3));
// the upper-left 3x3 sub-block of Pose3's covariance corresponds to Rot3's covariance
// because the tangent space of Pose3 is ordered as (w,T) where w and T are both Vector3's
auto model = noiseModel::Gaussian::Covariance(M.block<3, 3>(0, 0));
return BinaryMeasurement<Rot3>(f->key1(), f->key2(), f->measured().rotation(),
model);
}

View File

@ -164,10 +164,15 @@ namespace gtsam {
}
/** return the calibration object */
inline const boost::shared_ptr<CALIBRATION> calibration() const {
const boost::shared_ptr<CALIBRATION> calibration() const {
return K_;
}
/** return the (optional) sensor pose with respect to the vehicle frame */
const boost::optional<POSE>& body_P_sensor() const {
return body_P_sensor_;
}
/** return verbosity */
inline bool verboseCheirality() const { return verboseCheirality_; }

View File

@ -58,40 +58,42 @@ Point2_ p(2);
TEST(ExpressionFactor, Leaf) {
using namespace leaf;
// Create old-style factor to create expected value and derivatives
// Create old-style factor to create expected value and derivatives.
PriorFactor<Point2> old(2, Point2(0, 0), model);
// Concise version
// Create the equivalent factor with expression.
ExpressionFactor<Point2> f(model, Point2(0, 0), p);
// Check values and derivatives.
EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9);
EXPECT_LONGS_EQUAL(2, f.dim());
boost::shared_ptr<GaussianFactor> gf2 = f.linearize(values);
EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9));
EXPECT(assert_equal(*old.linearize(values), *gf2, 1e-9));
}
/* ************************************************************************* */
// non-zero noise model
// Test leaf expression with noise model of different variance.
TEST(ExpressionFactor, Model) {
using namespace leaf;
SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.01));
// Create old-style factor to create expected value and derivatives
// Create old-style factor to create expected value and derivatives.
PriorFactor<Point2> old(2, Point2(0, 0), model);
// Concise version
// Create the equivalent factor with expression.
ExpressionFactor<Point2> f(model, Point2(0, 0), p);
// Check values and derivatives
// Check values and derivatives.
EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9);
EXPECT_LONGS_EQUAL(2, f.dim());
boost::shared_ptr<GaussianFactor> gf2 = f.linearize(values);
EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9));
EXPECT(assert_equal(*old.linearize(values), *gf2, 1e-9));
EXPECT_CORRECT_FACTOR_JACOBIANS(f, values, 1e-5, 1e-5); // another way
}
/* ************************************************************************* */
// Constrained noise model
// Test leaf expression with constrained noise model.
TEST(ExpressionFactor, Constrained) {
using namespace leaf;
@ -105,7 +107,7 @@ TEST(ExpressionFactor, Constrained) {
EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9);
EXPECT_LONGS_EQUAL(2, f.dim());
boost::shared_ptr<GaussianFactor> gf2 = f.linearize(values);
EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9));
EXPECT(assert_equal(*old.linearize(values), *gf2, 1e-9));
}
/* ************************************************************************* */
@ -129,7 +131,7 @@ TEST(ExpressionFactor, Unary) {
boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
boost::shared_ptr<JacobianFactor> jf = //
boost::dynamic_pointer_cast<JacobianFactor>(gf);
EXPECT( assert_equal(expected, *jf, 1e-9));
EXPECT(assert_equal(expected, *jf, 1e-9));
}
/* ************************************************************************* */
@ -142,11 +144,13 @@ Vector9 wide(const Point3& p, OptionalJacobian<9,3> H) {
if (H) *H << I_3x3, I_3x3, I_3x3;
return v;
}
typedef Eigen::Matrix<double,9,9> Matrix9;
Vector9 id9(const Vector9& v, OptionalJacobian<9,9> H) {
if (H) *H = Matrix9::Identity();
return v;
}
TEST(ExpressionFactor, Wide) {
// Create some values
Values values;
@ -207,6 +211,7 @@ TEST(ExpressionFactor, Binary) {
EXPECT(assert_equal(expected25, (Matrix ) (*r)->dTdA1, 1e-9));
EXPECT(assert_equal(expected22, (Matrix ) (*r)->dTdA2, 1e-9));
}
/* ************************************************************************* */
// Unary(Binary(Leaf,Leaf))
TEST(ExpressionFactor, Shallow) {
@ -263,7 +268,7 @@ TEST(ExpressionFactor, Shallow) {
EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9);
EXPECT_LONGS_EQUAL(2, f2.dim());
boost::shared_ptr<GaussianFactor> gf2 = f2.linearize(values);
EXPECT( assert_equal(*expected, *gf2, 1e-9));
EXPECT(assert_equal(*expected, *gf2, 1e-9));
}
/* ************************************************************************* */
@ -296,7 +301,7 @@ TEST(ExpressionFactor, tree) {
EXPECT_DOUBLES_EQUAL(expected_error, f.error(values), 1e-9);
EXPECT_LONGS_EQUAL(2, f.dim());
boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
EXPECT( assert_equal(*expected, *gf, 1e-9));
EXPECT(assert_equal(*expected, *gf, 1e-9));
// Concise version
ExpressionFactor<Point2> f2(model, measured,
@ -304,14 +309,14 @@ TEST(ExpressionFactor, tree) {
EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9);
EXPECT_LONGS_EQUAL(2, f2.dim());
boost::shared_ptr<GaussianFactor> gf2 = f2.linearize(values);
EXPECT( assert_equal(*expected, *gf2, 1e-9));
EXPECT(assert_equal(*expected, *gf2, 1e-9));
// Try ternary version
ExpressionFactor<Point2> f3(model, measured, project3(x, p, K));
EXPECT_DOUBLES_EQUAL(expected_error, f3.error(values), 1e-9);
EXPECT_LONGS_EQUAL(2, f3.dim());
boost::shared_ptr<GaussianFactor> gf3 = f3.linearize(values);
EXPECT( assert_equal(*expected, *gf3, 1e-9));
EXPECT(assert_equal(*expected, *gf3, 1e-9));
}
/* ************************************************************************* */
@ -332,15 +337,15 @@ TEST(ExpressionFactor, Compose1) {
// Check unwhitenedError
std::vector<Matrix> H(2);
Vector actual = f.unwhitenedError(values, H);
EXPECT( assert_equal(I_3x3, H[0],1e-9));
EXPECT( assert_equal(I_3x3, H[1],1e-9));
EXPECT(assert_equal(I_3x3, H[0],1e-9));
EXPECT(assert_equal(I_3x3, H[1],1e-9));
// Check linearization
JacobianFactor expected(1, I_3x3, 2, I_3x3, Z_3x1);
boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
boost::shared_ptr<JacobianFactor> jf = //
boost::dynamic_pointer_cast<JacobianFactor>(gf);
EXPECT( assert_equal(expected, *jf,1e-9));
EXPECT(assert_equal(expected, *jf,1e-9));
}
/* ************************************************************************* */
@ -362,14 +367,14 @@ TEST(ExpressionFactor, compose2) {
std::vector<Matrix> H(1);
Vector actual = f.unwhitenedError(values, H);
EXPECT_LONGS_EQUAL(1, H.size());
EXPECT( assert_equal(2*I_3x3, H[0],1e-9));
EXPECT(assert_equal(2*I_3x3, H[0],1e-9));
// Check linearization
JacobianFactor expected(1, 2 * I_3x3, Z_3x1);
boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
boost::shared_ptr<JacobianFactor> jf = //
boost::dynamic_pointer_cast<JacobianFactor>(gf);
EXPECT( assert_equal(expected, *jf,1e-9));
EXPECT(assert_equal(expected, *jf,1e-9));
}
/* ************************************************************************* */
@ -391,14 +396,14 @@ TEST(ExpressionFactor, compose3) {
std::vector<Matrix> H(1);
Vector actual = f.unwhitenedError(values, H);
EXPECT_LONGS_EQUAL(1, H.size());
EXPECT( assert_equal(I_3x3, H[0],1e-9));
EXPECT(assert_equal(I_3x3, H[0],1e-9));
// Check linearization
JacobianFactor expected(3, I_3x3, Z_3x1);
boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
boost::shared_ptr<JacobianFactor> jf = //
boost::dynamic_pointer_cast<JacobianFactor>(gf);
EXPECT( assert_equal(expected, *jf,1e-9));
EXPECT(assert_equal(expected, *jf,1e-9));
}
/* ************************************************************************* */
@ -434,16 +439,16 @@ TEST(ExpressionFactor, composeTernary) {
std::vector<Matrix> H(3);
Vector actual = f.unwhitenedError(values, H);
EXPECT_LONGS_EQUAL(3, H.size());
EXPECT( assert_equal(I_3x3, H[0],1e-9));
EXPECT( assert_equal(I_3x3, H[1],1e-9));
EXPECT( assert_equal(I_3x3, H[2],1e-9));
EXPECT(assert_equal(I_3x3, H[0],1e-9));
EXPECT(assert_equal(I_3x3, H[1],1e-9));
EXPECT(assert_equal(I_3x3, H[2],1e-9));
// Check linearization
JacobianFactor expected(1, I_3x3, 2, I_3x3, 3, I_3x3, Z_3x1);
boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
boost::shared_ptr<JacobianFactor> jf = //
boost::dynamic_pointer_cast<JacobianFactor>(gf);
EXPECT( assert_equal(expected, *jf,1e-9));
EXPECT(assert_equal(expected, *jf,1e-9));
}
TEST(ExpressionFactor, tree_finite_differences) {
@ -636,7 +641,7 @@ TEST(ExpressionFactor, MultiplyWithInverseFunction) {
class TestNaryFactor
: public gtsam::ExpressionFactorN<gtsam::Point3 /*return type*/,
gtsam::Rot3, gtsam::Point3,
gtsam::Rot3,gtsam::Point3> {
gtsam::Rot3, gtsam::Point3> {
private:
using This = TestNaryFactor;
using Base =

View File

@ -10,7 +10,7 @@ Parser classes and rules for parsing C++ classes.
Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar, Varun Agrawal, and Frank Dellaert
"""
from typing import Iterable, List, Union
from typing import Any, Iterable, List, Union
from pyparsing import Literal, Optional, ZeroOrMore # type: ignore
@ -48,12 +48,12 @@ class Method:
args_list, t.is_const))
def __init__(self,
template: str,
template: Union[Template, Any],
name: str,
return_type: ReturnType,
args: ArgumentList,
is_const: str,
parent: Union[str, "Class"] = ''):
parent: Union["Class", Any] = ''):
self.template = template
self.name = name
self.return_type = return_type
@ -98,7 +98,7 @@ class StaticMethod:
name: str,
return_type: ReturnType,
args: ArgumentList,
parent: Union[str, "Class"] = ''):
parent: Union["Class", Any] = ''):
self.name = name
self.return_type = return_type
self.args = args
@ -129,7 +129,7 @@ class Constructor:
def __init__(self,
name: str,
args: ArgumentList,
parent: Union["Class", str] = ''):
parent: Union["Class", Any] = ''):
self.name = name
self.args = args
@ -167,7 +167,7 @@ class Operator:
return_type: ReturnType,
args: ArgumentList,
is_const: str,
parent: Union[str, "Class"] = ''):
parent: Union["Class", Any] = ''):
self.name = name
self.operator = operator
self.return_type = return_type
@ -284,7 +284,7 @@ class Class:
properties: List[Variable],
operators: List[Operator],
enums: List[Enum],
parent: str = '',
parent: Any = '',
):
self.template = template
self.is_virtual = is_virtual

View File

@ -169,7 +169,7 @@ class GlobalFunction:
return_type: ReturnType,
args_list: ArgumentList,
template: Template,
parent: str = ''):
parent: Any = ''):
self.name = name
self.return_type = return_type
self.args = args_list

View File

@ -12,7 +12,7 @@ Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar, Varun Agrawal, and Frank Dellae
# pylint: disable=unnecessary-lambda, expression-not-assigned
from typing import Iterable, List, Union
from typing import List, Sequence, Union
from pyparsing import (Forward, Optional, Or, ParseResults, # type: ignore
delimitedList)
@ -49,12 +49,12 @@ class Typename:
def __init__(self,
t: ParseResults,
instantiations: Iterable[ParseResults] = ()):
instantiations: Sequence[ParseResults] = ()):
self.name = t[-1] # the name is the last element in this list
self.namespaces = t[:-1]
if instantiations:
if isinstance(instantiations, Iterable):
if isinstance(instantiations, Sequence):
self.instantiations = instantiations # type: ignore
else:
self.instantiations = instantiations.asList()

View File

@ -1,5 +1,7 @@
"""Mixins for reducing the amount of boilerplate in the main wrapper class."""
from typing import Any, Tuple, Union
import gtwrap.interface_parser as parser
import gtwrap.template_instantiator as instantiator
@ -7,13 +9,14 @@ import gtwrap.template_instantiator as instantiator
class CheckMixin:
"""Mixin to provide various checks."""
# Data types that are primitive types
not_ptr_type = ['int', 'double', 'bool', 'char', 'unsigned char', 'size_t']
not_ptr_type: Tuple = ('int', 'double', 'bool', 'char', 'unsigned char',
'size_t')
# Ignore the namespace for these datatypes
ignore_namespace = ['Matrix', 'Vector', 'Point2', 'Point3']
ignore_namespace: Tuple = ('Matrix', 'Vector', 'Point2', 'Point3')
# Methods that should be ignored
ignore_methods = ['pickle']
ignore_methods: Tuple = ('pickle', )
# Methods that should not be wrapped directly
whitelist = ['serializable', 'serialize']
whitelist: Tuple = ('serializable', 'serialize')
# Datatypes that do not need to be checked in methods
not_check_type: list = []
@ -23,7 +26,7 @@ class CheckMixin:
return True
return False
def is_shared_ptr(self, arg_type):
def is_shared_ptr(self, arg_type: parser.Type):
"""
Determine if the `interface_parser.Type` should be treated as a
shared pointer in the wrapper.
@ -33,7 +36,7 @@ class CheckMixin:
and arg_type.typename.name not in self.ignore_namespace
and arg_type.typename.name != 'string')
def is_ptr(self, arg_type):
def is_ptr(self, arg_type: parser.Type):
"""
Determine if the `interface_parser.Type` should be treated as a
raw pointer in the wrapper.
@ -43,7 +46,7 @@ class CheckMixin:
and arg_type.typename.name not in self.ignore_namespace
and arg_type.typename.name != 'string')
def is_ref(self, arg_type):
def is_ref(self, arg_type: parser.Type):
"""
Determine if the `interface_parser.Type` should be treated as a
reference in the wrapper.
@ -55,7 +58,14 @@ class CheckMixin:
class FormatMixin:
"""Mixin to provide formatting utilities."""
def _clean_class_name(self, instantiated_class):
ignore_namespace: tuple
data_type: Any
data_type_param: Any
_return_count: Any
def _clean_class_name(self,
instantiated_class: instantiator.InstantiatedClass):
"""Reformatted the C++ class name to fit Matlab defined naming
standards
"""
@ -65,23 +75,23 @@ class FormatMixin:
return instantiated_class.name
def _format_type_name(self,
type_name,
separator='::',
include_namespace=True,
constructor=False,
method=False):
type_name: parser.Typename,
separator: str = '::',
include_namespace: bool = True,
is_constructor: bool = False,
is_method: bool = False):
"""
Args:
type_name: an interface_parser.Typename to reformat
separator: the statement to add between namespaces and typename
include_namespace: whether to include namespaces when reformatting
constructor: if the typename will be in a constructor
method: if the typename will be in a method
is_constructor: if the typename will be in a constructor
is_method: if the typename will be in a method
Raises:
constructor and method cannot both be true
"""
if constructor and method:
if is_constructor and is_method:
raise ValueError(
'Constructor and method parameters cannot both be True')
@ -93,9 +103,9 @@ class FormatMixin:
if name not in self.ignore_namespace and namespace != '':
formatted_type_name += namespace + separator
if constructor:
if is_constructor:
formatted_type_name += self.data_type.get(name) or name
elif method:
elif is_method:
formatted_type_name += self.data_type_param.get(name) or name
else:
formatted_type_name += name
@ -106,8 +116,8 @@ class FormatMixin:
template = '{}'.format(
self._format_type_name(type_name.instantiations[idx],
include_namespace=include_namespace,
constructor=constructor,
method=method))
is_constructor=is_constructor,
is_method=is_method))
templates.append(template)
if len(templates) > 0: # If there are no templates
@ -119,15 +129,15 @@ class FormatMixin:
self._format_type_name(type_name.instantiations[idx],
separator=separator,
include_namespace=False,
constructor=constructor,
method=method))
is_constructor=is_constructor,
is_method=is_method))
return formatted_type_name
def _format_return_type(self,
return_type,
include_namespace=False,
separator="::"):
return_type: parser.function.ReturnType,
include_namespace: bool = False,
separator: str = "::"):
"""Format return_type.
Args:
@ -154,18 +164,15 @@ class FormatMixin:
return return_wrap
def _format_class_name(self, instantiated_class, separator=''):
def _format_class_name(self,
instantiated_class: instantiator.InstantiatedClass,
separator: str = ''):
"""Format a template_instantiator.InstantiatedClass name."""
if instantiated_class.parent == '':
parent_full_ns = ['']
else:
parent_full_ns = instantiated_class.parent.full_namespaces()
# class_name = instantiated_class.parent.name
#
# if class_name != '':
# class_name += separator
#
# class_name += instantiated_class.name
parentname = "".join([separator + x
for x in parent_full_ns]) + separator
@ -175,10 +182,12 @@ class FormatMixin:
return class_name
def _format_static_method(self, static_method, separator=''):
"""Example:
gtsamPoint3.staticFunction
def _format_static_method(self,
static_method: parser.StaticMethod,
separator: str = ''):
"""
Example:
gtsam.Point3.staticFunction()
"""
method = ''
@ -188,35 +197,17 @@ class FormatMixin:
return method[2 * len(separator):]
def _format_instance_method(self, instance_method, separator=''):
def _format_global_function(self,
function: Union[parser.GlobalFunction, Any],
separator: str = ''):
"""Example:
gtsamPoint3.staticFunction
"""
method = ''
if isinstance(instance_method, instantiator.InstantiatedMethod):
method_list = [
separator + x
for x in instance_method.parent.parent.full_namespaces()
]
method += "".join(method_list) + separator
method += instance_method.parent.name + separator
method += instance_method.original.name
method += "<" + instance_method.instantiations.to_cpp() + ">"
return method[2 * len(separator):]
def _format_global_method(self, static_method, separator=''):
"""Example:
gtsamPoint3.staticFunction
"""
method = ''
if isinstance(static_method, parser.GlobalFunction):
method += "".join([separator + x for x in static_method.parent.full_namespaces()]) + \
if isinstance(function, parser.GlobalFunction):
method += "".join([separator + x for x in function.parent.full_namespaces()]) + \
separator
return method[2 * len(separator):]

View File

@ -11,8 +11,6 @@ import textwrap
from functools import partial, reduce
from typing import Dict, Iterable, List, Union
from loguru import logger
import gtwrap.interface_parser as parser
import gtwrap.template_instantiator as instantiator
from gtwrap.matlab_wrapper.mixins import CheckMixin, FormatMixin
@ -200,7 +198,7 @@ class MatlabWrapper(CheckMixin, FormatMixin):
check_type = self._format_type_name(
arg.ctype.typename,
separator='.',
constructor=not wrap_datatypes)
is_constructor=not wrap_datatypes)
var_arg_wrap += " && isa(varargin{{{num}}},'{data_type}')".format(
num=i, data_type=check_type)
@ -1090,11 +1088,10 @@ class MatlabWrapper(CheckMixin, FormatMixin):
if method.instantiations:
# method_name += '<{}>'.format(
# self._format_type_name(method.instantiations))
# method_name = self._format_instance_method(method, '::')
method = method.to_cpp()
elif isinstance(method, parser.GlobalFunction):
method_name = self._format_global_method(method, '::')
method_name = self._format_global_function(method, '::')
method_name += method.name
else:

View File

@ -4,7 +4,7 @@
import itertools
from copy import deepcopy
from typing import Iterable, List
from typing import Any, Iterable, List, Sequence
import gtwrap.interface_parser as parser
@ -214,17 +214,17 @@ class InstantiatedMethod(parser.Method):
}
"""
def __init__(self,
original,
original: parser.Method,
instantiations: Iterable[parser.Typename] = ()):
self.original = original
self.instantiations = instantiations
self.template = ''
self.template: Any = ''
self.is_const = original.is_const
self.parent = original.parent
# Check for typenames if templated.
# This way, we can gracefully handle both templated and non-templated methods.
typenames = self.original.template.typenames if self.original.template else []
typenames: Sequence = self.original.template.typenames if self.original.template else []
self.name = instantiate_name(original.name, self.instantiations)
self.return_type = instantiate_return_type(
original.return_type,
@ -348,13 +348,12 @@ class InstantiatedClass(parser.Class):
return "{virtual}Class {cpp_class} : {parent_class}\n"\
"{ctors}\n{static_methods}\n{methods}\n{operators}".format(
virtual="virtual " if self.is_virtual else '',
name=self.name,
cpp_class=self.to_cpp(),
parent_class=self.parent,
ctors="\n".join([repr(ctor) for ctor in self.ctors]),
methods="\n".join([repr(m) for m in self.methods]),
static_methods="\n".join([repr(m)
for m in self.static_methods]),
methods="\n".join([repr(m) for m in self.methods]),
operators="\n".join([repr(op) for op in self.operators])
)

View File

@ -1,3 +1,2 @@
pyparsing
pytest
loguru

View File

@ -11,8 +11,6 @@ import os.path as osp
import sys
import unittest
from loguru import logger
sys.path.append(osp.dirname(osp.dirname(osp.abspath(__file__))))
from gtwrap.matlab_wrapper import MatlabWrapper
@ -44,10 +42,6 @@ class TestWrap(unittest.TestCase):
# Create the `actual/matlab` directory
os.makedirs(self.MATLAB_ACTUAL_DIR, exist_ok=True)
# set the log level to INFO by default
logger.remove() # remove the default sink
logger.add(sys.stderr, format="{time} {level} {message}", level="INFO")
def compare_and_diff(self, file):
"""
Compute the comparison between the expected and actual file,