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

Merge branch 'develop' into feature/system-metis-lib

release/4.3a0
Varun Agrawal 2020-11-18 15:18:29 -05:00
parent 9d41178070 653c69fccd
commit 8c7a404ace
37 changed files with 587 additions and 154 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.github/scripts/unix.sh vendored
View File

@ -66,6 +66,8 @@ function configure()
-DGTSAM_BUILD_EXAMPLES_ALWAYS=${GTSAM_BUILD_EXAMPLES_ALWAYS:-ON} \
-DGTSAM_ALLOW_DEPRECATED_SINCE_V41=${GTSAM_ALLOW_DEPRECATED_SINCE_V41:-OFF} \
-DGTSAM_USE_QUATERNIONS=${GTSAM_USE_QUATERNIONS:-OFF} \
-DGTSAM_ROT3_EXPMAP=${GTSAM_ROT3_EXPMAP:-ON} \
-DGTSAM_POSE3_EXPMAP=${GTSAM_POSE3_EXPMAP:-ON} \
-DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \
-DBOOST_ROOT=$BOOST_ROOT \
-DBoost_NO_SYSTEM_PATHS=ON \

12
.github/workflows/build-linux.yml vendored
View File

@ -63,17 +63,17 @@ jobs:
sudo apt install cmake build-essential pkg-config libpython-dev python-numpy
echo "::set-env name=BOOST_ROOT::$(echo $BOOST_ROOT_1_69_0)"
echo "::set-env name=LD_LIBRARY_PATH::$(echo $BOOST_ROOT_1_69_0/lib)"
echo "BOOST_ROOT=$(echo $BOOST_ROOT_1_72_0)" >> $GITHUB_ENV
echo "LD_LIBRARY_PATH=$(echo $BOOST_ROOT_1_72_0/lib)" >> $GITHUB_ENV
if [ "${{ matrix.compiler }}" = "gcc" ]; then
sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib
echo "::set-env name=CC::gcc-${{ matrix.version }}"
echo "::set-env name=CXX::g++-${{ matrix.version }}"
echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV
echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV
else
sudo apt-get install -y clang-${{ matrix.version }} g++-multilib
echo "::set-env name=CC::clang-${{ matrix.version }}"
echo "::set-env name=CXX::clang++-${{ matrix.version }}"
echo "CC=clang-${{ matrix.version }}" >> $GITHUB_ENV
echo "CXX=clang++-${{ matrix.version }}" >> $GITHUB_ENV
fi
- name: Check Boost version
if: runner.os == 'Linux'

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

@ -35,17 +35,19 @@ jobs:
- name: Install (macOS)
if: runner.os == 'macOS'
run: |
brew install cmake ninja boost
brew tap ProfFan/robotics
brew install cmake ninja
brew install ProfFan/robotics/boost
if [ "${{ matrix.compiler }}" = "gcc" ]; then
brew install gcc@${{ matrix.version }}
echo "::set-env name=CC::gcc-${{ matrix.version }}"
echo "::set-env name=CXX::g++-${{ matrix.version }}"
echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV
echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV
else
sudo xcode-select -switch /Applications/Xcode_${{ matrix.version }}.app
echo "::set-env name=CC::clang"
echo "::set-env name=CXX::clang++"
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
bash .github/scripts/unix.sh -t

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

@ -19,7 +19,7 @@ jobs:
# See https://help.github.com/en/articles/workflow-syntax-for-github-actions.
name: [
ubuntu-18.04-gcc-5,
# ubuntu-18.04-gcc-9, # TODO Disabled for now because of timeouts
ubuntu-18.04-gcc-9,
ubuntu-18.04-clang-9,
macOS-10.15-xcode-11.3.1,
ubuntu-18.04-gcc-5-tbb,
@ -33,11 +33,10 @@ jobs:
compiler: gcc
version: "5"
# TODO Disabled for now because of timeouts
# - name: ubuntu-18.04-gcc-9
# os: ubuntu-18.04
# compiler: gcc
# version: "9"
- name: ubuntu-18.04-gcc-9
os: ubuntu-18.04
compiler: gcc
version: "9"
- name: ubuntu-18.04-clang-9
os: ubuntu-18.04
@ -77,30 +76,32 @@ jobs:
if [ "${{ matrix.compiler }}" = "gcc" ]; then
sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib
echo "::set-env name=CC::gcc-${{ matrix.version }}"
echo "::set-env name=CXX::g++-${{ matrix.version }}"
echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV
echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV
else
sudo apt-get install -y clang-${{ matrix.version }} g++-multilib
echo "::set-env name=CC::clang-${{ matrix.version }}"
echo "::set-env name=CXX::clang++-${{ matrix.version }}"
echo "CC=clang-${{ matrix.version }}" >> $GITHUB_ENV
echo "CXX=clang++-${{ matrix.version }}" >> $GITHUB_ENV
fi
- name: Install (macOS)
if: runner.os == 'macOS'
run: |
brew install cmake ninja boost
brew tap ProfFan/robotics
brew install cmake ninja
brew install ProfFan/robotics/boost
if [ "${{ matrix.compiler }}" = "gcc" ]; then
brew install gcc@${{ matrix.version }}
echo "::set-env name=CC::gcc-${{ matrix.version }}"
echo "::set-env name=CXX::g++-${{ matrix.version }}"
echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV
echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV
else
sudo xcode-select -switch /Applications/Xcode_${{ matrix.version }}.app
echo "::set-env name=CC::clang"
echo "::set-env name=CXX::clang++"
echo "CC=clang" >> $GITHUB_ENV
echo "CXX=clang++" >> $GITHUB_ENV
fi
- name: Set GTSAM_WITH_TBB Flag
if: matrix.flag == 'tbb'
run: |
echo "::set-env name=GTSAM_WITH_TBB::ON"
echo "GTSAM_WITH_TBB=ON" >> $GITHUB_ENV
echo "GTSAM Uses TBB"
- name: Build (Linux)
if: runner.os == 'Linux'
@ -109,4 +110,4 @@ jobs:
- name: Build (macOS)
if: runner.os == 'macOS'
run: |
bash .github/scripts/python.sh
bash .github/scripts/python.sh

40
.github/workflows/build-special.yml vendored
View File

@ -24,6 +24,7 @@ jobs:
ubuntu-gcc-deprecated,
ubuntu-gcc-quaternions,
ubuntu-gcc-tbb,
ubuntu-cayleymap,
]
build_type: [Debug, Release]
@ -47,6 +48,12 @@ jobs:
version: "9"
flag: tbb
- name: ubuntu-cayleymap
os: ubuntu-18.04
compiler: gcc
version: "9"
flag: cayley
steps:
- name: Checkout
uses: actions/checkout@master
@ -64,17 +71,17 @@ jobs:
sudo apt install cmake build-essential pkg-config libpython-dev python-numpy
echo "::set-env name=BOOST_ROOT::$(echo $BOOST_ROOT_1_69_0)"
echo "::set-env name=LD_LIBRARY_PATH::$(echo $BOOST_ROOT_1_69_0/lib)"
echo "BOOST_ROOT=$(echo $BOOST_ROOT_1_72_0)" >> $GITHUB_ENV
echo "LD_LIBRARY_PATH=$(echo $BOOST_ROOT_1_72_0/lib)" >> $GITHUB_ENV
if [ "${{ matrix.compiler }}" = "gcc" ]; then
sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib
echo "::set-env name=CC::gcc-${{ matrix.version }}"
echo "::set-env name=CXX::g++-${{ matrix.version }}"
echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV
echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV
else
sudo apt-get install -y clang-${{ matrix.version }} g++-multilib
echo "::set-env name=CC::clang-${{ matrix.version }}"
echo "::set-env name=CXX::clang++-${{ matrix.version }}"
echo "CC=clang-${{ matrix.version }}" >> $GITHUB_ENV
echo "CXX=clang++-${{ matrix.version }}" >> $GITHUB_ENV
fi
- name: Install (macOS)
@ -83,32 +90,39 @@ jobs:
brew install cmake ninja boost
if [ "${{ matrix.compiler }}" = "gcc" ]; then
brew install gcc@${{ matrix.version }}
echo "::set-env name=CC::gcc-${{ matrix.version }}"
echo "::set-env name=CXX::g++-${{ matrix.version }}"
echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV
echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV
else
sudo xcode-select -switch /Applications/Xcode_${{ matrix.version }}.app
echo "::set-env name=CC::clang"
echo "::set-env name=CXX::clang++"
echo "CC=clang" >> $GITHUB_ENV
echo "CXX=clang++" >> $GITHUB_ENV
fi
- name: Set Allow Deprecated Flag
if: matrix.flag == 'deprecated'
run: |
echo "::set-env name=GTSAM_ALLOW_DEPRECATED_SINCE_V41::ON"
echo "GTSAM_ALLOW_DEPRECATED_SINCE_V41=ON" >> $GITHUB_ENV
echo "Allow deprecated since version 4.1"
- name: Set Use Quaternions Flag
if: matrix.flag == 'quaternions'
run: |
echo "::set-env name=GTSAM_USE_QUATERNIONS::ON"
echo "GTSAM_USE_QUATERNIONS=ON" >> $GITHUB_ENV
echo "Use Quaternions for rotations"
- name: Set GTSAM_WITH_TBB Flag
if: matrix.flag == 'tbb'
run: |
echo "::set-env name=GTSAM_WITH_TBB::ON"
echo "GTSAM_WITH_TBB=ON" >> $GITHUB_ENV
echo "GTSAM Uses TBB"
- name: Use Cayley Transform for Rot3
if: matrix.flag == 'cayley'
run: |
echo "GTSAM_POSE3_EXPMAP=OFF" >> $GITHUB_ENV
echo "GTSAM_ROT3_EXPMAP=OFF" >> $GITHUB_ENV
echo "GTSAM Uses Cayley map for Rot3"
- name: Build & Test
run: |
bash .github/scripts/unix.sh -t

25
.github/workflows/build-windows.yml vendored
View File

@ -18,16 +18,19 @@ 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: [
windows-2016-cl,
#TODO This build keeps timing out, need to understand why.
# windows-2016-cl,
windows-2019-cl,
]
build_type: [Debug, Release]
build_unstable: [ON]
include:
- name: windows-2016-cl
os: windows-2016
compiler: cl
#TODO This build keeps timing out, need to understand why.
# - name: windows-2016-cl
# os: windows-2016
# compiler: cl
- name: windows-2019-cl
os: windows-2019
@ -50,17 +53,17 @@ jobs:
# See: https://github.com/DaanDeMeyer/doctest/runs/231595515
# See: https://github.community/t5/GitHub-Actions/Something-is-wrong-with-the-chocolatey-installed-version-of-gcc/td-p/32413
scoop install gcc --global
echo "::set-env name=CC::gcc"
echo "::set-env name=CXX::g++"
echo "CC=gcc" >> $GITHUB_ENV
echo "CXX=g++" >> $GITHUB_ENV
} elseif ("${{ matrix.compiler }}" -eq "clang") {
echo "::set-env name=CC::clang"
echo "::set-env name=CXX::clang++"
echo "CC=clang" >> $GITHUB_ENV
echo "CXX=clang++" >> $GITHUB_ENV
} else {
echo "::set-env name=CC::${{ matrix.compiler }}"
echo "::set-env name=CXX::${{ matrix.compiler }}"
echo "CC=${{ matrix.compiler }}" >> $GITHUB_ENV
echo "CXX=${{ matrix.compiler }}" >> $GITHUB_ENV
}
# Scoop modifies the PATH so we make the modified PATH global.
echo "::set-env name=PATH::$env:PATH"
echo "$env:PATH" >> $GITHUB_PATH
- name: Build (Windows)
if: runner.os == 'Windows'
run: |

15
cmake/GtsamBuildTypes.cmake
View File

@ -1,3 +1,5 @@
include(CheckCXXCompilerFlag) # for check_cxx_compiler_flag()
# Set cmake policy to recognize the AppleClang compiler
# independently from the Clang compiler.
if(POLICY CMP0025)
@ -105,11 +107,14 @@ if(MSVC)
else()
# Common to all configurations, next for each configuration:
if (
((CMAKE_CXX_COMPILER_ID MATCHES "Clang") AND (NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 12.0.0)) OR
(CMAKE_CXX_COMPILER_ID MATCHES "GNU")
)
set(flag_override_ -Wsuggest-override) # -Werror=suggest-override: Add again someday
if (NOT MSVC)
check_cxx_compiler_flag(-Wsuggest-override COMPILER_HAS_WSUGGEST_OVERRIDE)
check_cxx_compiler_flag(-Wmissing COMPILER_HAS_WMISSING_OVERRIDE)
if (COMPILER_HAS_WSUGGEST_OVERRIDE)
set(flag_override_ -Wsuggest-override) # -Werror=suggest-override: Add again someday
elseif(COMPILER_HAS_WMISSING_OVERRIDE)
set(flag_override_ -Wmissing-override) # -Werror=missing-override: Add again someday
endif()
endif()
set(GTSAM_COMPILE_OPTIONS_PRIVATE_COMMON

9
cmake/HandleGeneralOptions.cmake
View File

@ -31,6 +31,15 @@ if(NOT MSVC AND NOT XCODE_VERSION)
option(GTSAM_BUILD_WITH_CCACHE "Use ccache compiler cache" ON)
endif()
# Enable GTSAM_ROT3_EXPMAP if GTSAM_POSE3_EXPMAP is enabled, and vice versa.
if(GTSAM_POSE3_EXPMAP)
message(STATUS "GTSAM_POSE3_EXPMAP=ON, enabling GTSAM_ROT3_EXPMAP as well")
set(GTSAM_ROT3_EXPMAP 1 CACHE BOOL "" FORCE)
elseif(GTSAM_ROT3_EXPMAP)
message(STATUS "GTSAM_ROT3_EXPMAP=ON, enabling GTSAM_POSE3_EXPMAP as well")
set(GTSAM_POSE3_EXPMAP 1 CACHE BOOL "" FORCE)
endif()
# Options relating to MATLAB wrapper
# TODO: Check for matlab mex binary before handling building of binaries
option(GTSAM_INSTALL_MATLAB_TOOLBOX "Enable/Disable installation of matlab toolbox" OFF)

5
cmake/HandlePython.cmake
View File

@ -1,4 +1,5 @@
if(GTSAM_BUILD_PYTHON)
# Set Python version if either Python or MATLAB wrapper is requested.
if(GTSAM_BUILD_PYTHON OR GTSAM_INSTALL_MATLAB_TOOLBOX)
if(${GTSAM_PYTHON_VERSION} STREQUAL "Default")
# Get info about the Python3 interpreter
# https://cmake.org/cmake/help/latest/module/FindPython3.html#module:FindPython3
@ -14,7 +15,9 @@ if(GTSAM_BUILD_PYTHON)
"The version of Python to build the wrappers against."
FORCE)
endif()
endif()
if(GTSAM_BUILD_PYTHON)
if(GTSAM_UNSTABLE_BUILD_PYTHON)
if (NOT GTSAM_BUILD_UNSTABLE)
message(WARNING "GTSAM_UNSTABLE_BUILD_PYTHON requires the unstable module to be enabled.")

2
doc/Doxyfile.in
View File

@ -1188,7 +1188,7 @@ USE_MATHJAX = YES
# MathJax, but it is strongly recommended to install a local copy of MathJax
# before deployment.
MATHJAX_RELPATH = http://cdn.mathjax.org/mathjax/latest
MATHJAX_RELPATH = https://cdn.mathjax.org/mathjax/latest
# The MATHJAX_EXTENSIONS tag can be used to specify one or MathJax extension
# names that should be enabled during MathJax rendering.

41
gtsam/base/TestableAssertions.h
View File

@ -23,6 +23,7 @@
#include <boost/optional.hpp>
#include <map>
#include <iostream>
#include <sstream>
#include <vector>
namespace gtsam {
@ -349,4 +350,44 @@ bool assert_inequal(const V& expected, const V& actual, double tol = 1e-9) {
return false;
}
/**
* Capture std out via cout stream and compare against string.
*/
template<class V>
bool assert_stdout_equal(const std::string& expected, const V& actual) {
// Redirect output to buffer so we can compare
std::stringstream buffer;
// Save the original output stream so we can reset later
std::streambuf* old = std::cout.rdbuf(buffer.rdbuf());
// We test against actual std::cout for faithful reproduction
std::cout << actual;
// Get output string and reset stdout
std::string actual_ = buffer.str();
std::cout.rdbuf(old);
return assert_equal(expected, actual_);
}
/**
* Capture print function output and compare against string.
*/
template<class V>
bool assert_print_equal(const std::string& expected, const V& actual) {
// Redirect output to buffer so we can compare
std::stringstream buffer;
// Save the original output stream so we can reset later
std::streambuf* old = std::cout.rdbuf(buffer.rdbuf());
// We test against actual std::cout for faithful reproduction
actual.print();
// Get output string and reset stdout
std::string actual_ = buffer.str();
std::cout.rdbuf(old);
return assert_equal(expected, actual_);
}
} // \namespace gtsam

12
gtsam/geometry/Cal3Bundler.h
View File

@ -98,6 +98,17 @@ public:
return k2_;
}
/// image center in x
inline double px() const {
return u0_;
}
/// image center in y
inline double py() const {
return v0_;
}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
/// get parameter u0
inline double u0() const {
return u0_;
@ -107,6 +118,7 @@ public:
inline double v0() const {
return v0_;
}
#endif
/**

2
gtsam/geometry/Rot3.h
View File

@ -526,7 +526,7 @@ namespace gtsam {
/**
* @brief Spherical Linear intERPolation between *this and other
* @param s a value between 0 and 1
* @param t a value between 0 and 1
* @param other final point of iterpolation geodesic on manifold
*/
Rot3 slerp(double t, const Rot3& other) const;

12
gtsam/geometry/Rot3M.cpp
View File

@ -176,7 +176,17 @@ Vector3 Rot3::CayleyChart::Local(const Rot3& R, OptionalJacobian<3,3> H) {
if (H) throw std::runtime_error("Rot3::CayleyChart::Local Derivative");
// Create a fixed-size matrix
Matrix3 A = R.matrix();
// Mathematica closed form optimization (procrastination?) gone wild:
// Check if (A+I) is invertible. Same as checking for -1 eigenvalue.
if ((A + I_3x3).determinant() == 0.0) {
throw std::runtime_error("Rot3::CayleyChart::Local Invalid Rotation");
}
// Mathematica closed form optimization.
// The following are the essential computations for the following algorithm
// 1. Compute the inverse of P = (A+I), using a closed-form formula since P is 3x3
// 2. Compute the Cayley transform C = 2 * P^{-1} * (A-I)
// 3. C is skew-symmetric, so we pick out the computations corresponding only to x, y, and z.
const double a = A(0, 0), b = A(0, 1), c = A(0, 2);
const double d = A(1, 0), e = A(1, 1), f = A(1, 2);
const double g = A(2, 0), h = A(2, 1), i = A(2, 2);

11
gtsam/geometry/tests/testCal3_S2.cpp
View File

@ -16,6 +16,7 @@
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Cal3_S2.h>
@ -127,6 +128,16 @@ TEST(Cal3_S2, between) {
}
/* ************************************************************************* */
TEST(Cal3_S2, Print) {
Cal3_S2 cal(5, 5, 5, 5, 5);
std::stringstream os;
os << "{fx: " << cal.fx() << ", fy: " << cal.fy() << ", s:" << cal.skew() << ", px:" << cal.px()
<< ", py:" << cal.py() << "}";
EXPECT(assert_stdout_equal(os.str(), cal));
}
/* ************************************************************************* */
int main() {
TestResult tr;

32
gtsam/geometry/tests/testPose3.cpp
View File

@ -18,6 +18,7 @@
#include <gtsam/geometry/Pose2.h>
#include <gtsam/base/testLie.h>
#include <gtsam/base/lieProxies.h>
#include <gtsam/base/TestableAssertions.h>
#include <boost/assign/std/vector.hpp> // for operator +=
using namespace boost::assign;
@ -906,9 +907,9 @@ TEST(Pose3 , ChartDerivatives) {
Pose3 id;
if (ROT3_DEFAULT_COORDINATES_MODE == Rot3::EXPMAP) {
CHECK_CHART_DERIVATIVES(id,id);
// CHECK_CHART_DERIVATIVES(id,T2);
// CHECK_CHART_DERIVATIVES(T2,id);
// CHECK_CHART_DERIVATIVES(T2,T3);
CHECK_CHART_DERIVATIVES(id,T2);
CHECK_CHART_DERIVATIVES(T2,id);
CHECK_CHART_DERIVATIVES(T2,T3);
}
}
@ -1028,32 +1029,13 @@ TEST(Pose3, Create) {
}
/* ************************************************************************* */
TEST(Pose3, print) {
std::stringstream redirectStream;
std::streambuf* ssbuf = redirectStream.rdbuf();
std::streambuf* oldbuf = std::cout.rdbuf();
// redirect cout to redirectStream
std::cout.rdbuf(ssbuf);
TEST(Pose3, Print) {
Pose3 pose(Rot3::identity(), Point3(1, 2, 3));
// output is captured to redirectStream
pose.print();
// Generate the expected output
std::stringstream expected;
Point3 translation(1, 2, 3);
std::string expected = "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\nt: 1 2 3\n";
// Add expected rotation
expected << "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\n";
expected << "t: 1 2 3\n";
// reset cout to the original stream
std::cout.rdbuf(oldbuf);
// Get substring corresponding to translation part
std::string actual = redirectStream.str();
CHECK_EQUAL(expected.str(), actual);
EXPECT(assert_print_equal(expected, pose));
}
/* ************************************************************************* */

38
gtsam/gtsam.i
View File

@ -597,6 +597,7 @@ class Rot3 {
Rot3(double R11, double R12, double R13,
double R21, double R22, double R23,
double R31, double R32, double R33);
Rot3(double w, double x, double y, double z);
static gtsam::Rot3 Rx(double t);
static gtsam::Rot3 Ry(double t);
@ -980,8 +981,8 @@ class Cal3Bundler {
double fy() const;
double k1() const;
double k2() const;
double u0() const;
double v0() const;
double px() const;
double py() const;
Vector vector() const;
Vector k() const;
Matrix K() const;
@ -2068,7 +2069,7 @@ class NonlinearFactorGraph {
gtsam::KeySet keys() const;
gtsam::KeyVector keyVector() const;
template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::imuBias::ConstantBias}>
template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::PinholeCamera<gtsam::Cal3Bundler>, gtsam::imuBias::ConstantBias}>
void addPrior(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
// NonlinearFactorGraph
@ -2157,12 +2158,13 @@ class Values {
void insert(size_t j, const gtsam::SOn& P);
void insert(size_t j, const gtsam::Rot3& rot3);
void insert(size_t j, const gtsam::Pose3& pose3);
void insert(size_t j, const gtsam::Unit3& unit3);
void insert(size_t j, const gtsam::Cal3_S2& cal3_s2);
void insert(size_t j, const gtsam::Cal3DS2& cal3ds2);
void insert(size_t j, const gtsam::Cal3Bundler& cal3bundler);
void insert(size_t j, const gtsam::EssentialMatrix& essential_matrix);
void insert(size_t j, const gtsam::PinholeCameraCal3_S2& simple_camera);
// void insert(size_t j, const gtsam::PinholeCameraCal3Bundler& camera);
void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
void insert(size_t j, const gtsam::NavState& nav_state);
@ -2175,18 +2177,19 @@ class Values {
void update(size_t j, const gtsam::SOn& P);
void update(size_t j, const gtsam::Rot3& rot3);
void update(size_t j, const gtsam::Pose3& pose3);
void update(size_t j, const gtsam::Unit3& unit3);
void update(size_t j, const gtsam::Cal3_S2& cal3_s2);
void update(size_t j, const gtsam::Cal3DS2& cal3ds2);
void update(size_t j, const gtsam::Cal3Bundler& cal3bundler);
void update(size_t j, const gtsam::EssentialMatrix& essential_matrix);
void update(size_t j, const gtsam::PinholeCameraCal3_S2& simple_camera);
// void update(size_t j, const gtsam::PinholeCameraCal3Bundler& camera);
void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& 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);
void update(size_t j, Matrix matrix);
template<T = {gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Pose2, gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::PinholeCameraCal3_S2, gtsam::imuBias::ConstantBias, gtsam::NavState, Vector, Matrix}>
template<T = {gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Pose2, gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose3, gtsam::Unit3, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::PinholeCameraCal3_S2, gtsam::PinholeCamera<gtsam::Cal3Bundler>, gtsam::imuBias::ConstantBias, gtsam::NavState, Vector, Matrix}>
T at(size_t j);
/// version for double
@ -2490,7 +2493,8 @@ class ISAM2 {
template <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2,
gtsam::Cal3Bundler, gtsam::EssentialMatrix,
gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, Vector, Matrix}>
gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::PinholeCamera<gtsam::Cal3Bundler>,
Vector, Matrix}>
VALUE calculateEstimate(size_t key) const;
gtsam::Values calculateBestEstimate() const;
Matrix marginalCovariance(size_t key) const;
@ -2528,7 +2532,7 @@ class NonlinearISAM {
#include <gtsam/geometry/StereoPoint2.h>
#include <gtsam/nonlinear/PriorFactor.h>
template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::imuBias::ConstantBias}>
template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Unit3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::imuBias::ConstantBias, gtsam::PinholeCamera<gtsam::Cal3Bundler>}>
virtual class PriorFactor : gtsam::NoiseModelFactor {
PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
T prior() const;
@ -2673,6 +2677,7 @@ virtual class GeneralSFMFactor : gtsam::NoiseModelFactor {
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3_S2, gtsam::Point3> GeneralSFMFactorCal3_S2;
//TODO (Issue 237) due to lack of jacobians of Cal3DS2_Base::calibrate, GeneralSFMFactor does not apply to Cal3DS2
//typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCamera<gtsam::Cal3Bundler>, gtsam::Point3> GeneralSFMFactorCal3Bundler;
template<CALIBRATION = {gtsam::Cal3_S2}>
virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor {
@ -2759,21 +2764,36 @@ virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor {
};
#include <gtsam/slam/dataset.h>
class SfmTrack {
Point3 point3() const;
SfmTrack();
SfmTrack(const gtsam::Point3& pt);
const Point3& point3() const;
double r;
double g;
double b;
// TODO Need to close wrap#10 to allow this to work.
// std::vector<pair<size_t, gtsam::Point2>> measurements;
size_t number_measurements() const;
pair<size_t, gtsam::Point2> measurement(size_t idx) const;
pair<size_t, size_t> siftIndex(size_t idx) const;
void add_measurement(size_t idx, const gtsam::Point2& m);
};
class SfmData {
SfmData();
size_t number_cameras() const;
size_t number_tracks() const;
gtsam::PinholeCamera<gtsam::Cal3Bundler> camera(size_t idx) const;
gtsam::SfmTrack track(size_t idx) const;
void add_track(const gtsam::SfmTrack& t) ;
void add_camera(const gtsam::SfmCamera& cam);
};
gtsam::SfmData readBal(string filename);
bool writeBAL(string filename, gtsam::SfmData& data);
gtsam::Values initialCamerasEstimate(const gtsam::SfmData& db);
gtsam::Values initialCamerasAndPointsEstimate(const gtsam::SfmData& db);

10
gtsam/inference/Symbol.h
View File

@ -164,8 +164,16 @@ inline Key Y(std::uint64_t j) { return Symbol('y', j); }
inline Key Z(std::uint64_t j) { return Symbol('z', j); }
}
/** Generates symbol shorthands with alternative names different than the
* one-letter predefined ones. */
class SymbolGenerator {
const char c_;
public:
SymbolGenerator(const char c) : c_(c) {}
Symbol operator()(const std::uint64_t j) const { return Symbol(c_, j); }
};
/// traits
template<> struct traits<Symbol> : public Testable<Symbol> {};
} // \ namespace gtsam

20
gtsam/inference/tests/testKey.cpp
View File

@ -40,6 +40,25 @@ TEST(Key, KeySymbolConversion) {
EXPECT(assert_equal(original, actual))
}
/* ************************************************************************* */
TEST(Key, SymbolGenerator) {
const auto x1 = gtsam::symbol_shorthand::X(1);
const auto v1 = gtsam::symbol_shorthand::V(1);
const auto a1 = gtsam::symbol_shorthand::A(1);
const auto Z = gtsam::SymbolGenerator('x');
const auto DZ = gtsam::SymbolGenerator('v');
const auto DDZ = gtsam::SymbolGenerator('a');
const auto z1 = Z(1);
const auto dz1 = DZ(1);
const auto ddz1 = DDZ(1);
EXPECT(assert_equal(x1, z1));
EXPECT(assert_equal(v1, dz1));
EXPECT(assert_equal(a1, ddz1));
}
/* ************************************************************************* */
template<int KeySize>
Key KeyTestValue();
@ -106,4 +125,3 @@ int main() {
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

4
gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h
View File

@ -200,6 +200,10 @@ boost::tuple<V, int> nonlinearConjugateGradient(const S &system,
currentValues = system.advance(prevValues, alpha, direction);
currentError = system.error(currentValues);
// User hook:
if (params.iterationHook)
params.iterationHook(iteration, prevError, currentError);
// Maybe show output
if (params.verbosity >= NonlinearOptimizerParams::ERROR)
std::cout << "iteration: " << iteration << ", currentError: " << currentError << std::endl;

14
gtsam/nonlinear/NonlinearOptimizer.cpp
View File

@ -88,20 +88,28 @@ void NonlinearOptimizer::defaultOptimize() {
}
// Iterative loop
double newError = currentError; // used to avoid repeated calls to error()
do {
// Do next iteration
currentError = error(); // TODO(frank): don't do this twice at first !? Computed above!
currentError = newError;
iterate();
tictoc_finishedIteration();
// Update newError for either printouts or conditional-end checks:
newError = error();
// User hook:
if (params.iterationHook)
params.iterationHook(iterations(), currentError, newError);
// Maybe show output
if (params.verbosity >= NonlinearOptimizerParams::VALUES)
values().print("newValues");
if (params.verbosity >= NonlinearOptimizerParams::ERROR)
cout << "newError: " << error() << endl;
cout << "newError: " << newError << endl;
} while (iterations() < params.maxIterations &&
!checkConvergence(params.relativeErrorTol, params.absoluteErrorTol, params.errorTol,
currentError, error(), params.verbosity) && std::isfinite(currentError));
currentError, newError, params.verbosity) && std::isfinite(currentError));
// Printing if verbose
if (params.verbosity >= NonlinearOptimizerParams::TERMINATION) {

2
gtsam/nonlinear/NonlinearOptimizer.h
View File

@ -81,7 +81,7 @@ protected:
public:
/** A shared pointer to this class */
typedef boost::shared_ptr<const NonlinearOptimizer> shared_ptr;
using shared_ptr = boost::shared_ptr<const NonlinearOptimizer>;
/// @name Standard interface
/// @{

60
gtsam/nonlinear/NonlinearOptimizerParams.h
View File

@ -38,21 +38,12 @@ public:
SILENT, TERMINATION, ERROR, VALUES, DELTA, LINEAR
};
size_t maxIterations; ///< The maximum iterations to stop iterating (default 100)
double relativeErrorTol; ///< The maximum relative error decrease to stop iterating (default 1e-5)
double absoluteErrorTol; ///< The maximum absolute error decrease to stop iterating (default 1e-5)
double errorTol; ///< The maximum total error to stop iterating (default 0.0)
Verbosity verbosity; ///< The printing verbosity during optimization (default SILENT)
Ordering::OrderingType orderingType; ///< The method of ordering use during variable elimination (default COLAMD)
NonlinearOptimizerParams() :
maxIterations(100), relativeErrorTol(1e-5), absoluteErrorTol(1e-5), errorTol(
0.0), verbosity(SILENT), orderingType(Ordering::COLAMD),
linearSolverType(MULTIFRONTAL_CHOLESKY) {}
virtual ~NonlinearOptimizerParams() {
}
virtual void print(const std::string& str = "") const;
size_t maxIterations = 100; ///< The maximum iterations to stop iterating (default 100)
double relativeErrorTol = 1e-5; ///< The maximum relative error decrease to stop iterating (default 1e-5)
double absoluteErrorTol = 1e-5; ///< The maximum absolute error decrease to stop iterating (default 1e-5)
double errorTol = 0.0; ///< The maximum total error to stop iterating (default 0.0)
Verbosity verbosity = SILENT; ///< The printing verbosity during optimization (default SILENT)
Ordering::OrderingType orderingType = Ordering::COLAMD; ///< The method of ordering use during variable elimination (default COLAMD)
size_t getMaxIterations() const { return maxIterations; }
double getRelativeErrorTol() const { return relativeErrorTol; }
@ -71,6 +62,37 @@ public:
static Verbosity verbosityTranslator(const std::string &s) ;
static std::string verbosityTranslator(Verbosity value) ;
/** Type for an optional user-provided hook to be called after each
* internal optimizer iteration. See iterationHook below. */
using IterationHook = std::function<
void(size_t /*iteration*/, double/*errorBefore*/, double/*errorAfter*/)>;
/** Optional user-provided iteration hook to be called after each
* optimization iteration (Default: none).
* Note that `IterationHook` is defined as a std::function<> with this
* signature:
* \code
* void(size_t iteration, double errorBefore, double errorAfter)
* \endcode
* which allows binding by means of a reference to a regular function:
* \code
* void foo(size_t iteration, double errorBefore, double errorAfter);
* // ...
* lmOpts.iterationHook = &foo;
* \endcode
* or to a C++11 lambda (preferred if you need to capture additional
* context variables, such that the optimizer object itself, the factor graph,
* etc.):
* \code
* lmOpts.iterationHook = [&](size_t iter, double oldError, double newError)
* {
* // ...
* };
* \endcode
* or to the result of a properly-formed `std::bind` call.
*/
IterationHook iterationHook;
/** See NonlinearOptimizerParams::linearSolverType */
enum LinearSolverType {
MULTIFRONTAL_CHOLESKY,
@ -81,10 +103,16 @@ public:
CHOLMOD, /* Experimental Flag */
};
LinearSolverType linearSolverType; ///< The type of linear solver to use in the nonlinear optimizer
LinearSolverType linearSolverType = MULTIFRONTAL_CHOLESKY; ///< The type of linear solver to use in the nonlinear optimizer
boost::optional<Ordering> ordering; ///< The optional variable elimination ordering, or empty to use COLAMD (default: empty)
IterativeOptimizationParameters::shared_ptr iterativeParams; ///< The container for iterativeOptimization parameters. used in CG Solvers.
NonlinearOptimizerParams() = default;
virtual ~NonlinearOptimizerParams() {
}
virtual void print(const std::string& str = "") const;
inline bool isMultifrontal() const {
return (linearSolverType == MULTIFRONTAL_CHOLESKY)
|| (linearSolverType == MULTIFRONTAL_QR);

2
gtsam/nonlinear/tests/testAdaptAutoDiff.cpp
View File

@ -41,7 +41,7 @@ struct Cal3Bundler0 : public Cal3Bundler {
double v0 = 0)
: Cal3Bundler(f, k1, k2, u0, v0) {}
Cal3Bundler0 retract(const Vector& d) const {
return Cal3Bundler0(fx() + d(0), k1() + d(1), k2() + d(2), u0(), v0());
return Cal3Bundler0(fx() + d(0), k1() + d(1), k2() + d(2), px(), py());
}
Vector3 localCoordinates(const Cal3Bundler0& T2) const {
return T2.vector() - vector();

13
gtsam/nonlinear/tests/testFunctorizedFactor.cpp
View File

@ -19,6 +19,7 @@
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/FunctorizedFactor.h>
#include <gtsam/nonlinear/factorTesting.h>
@ -115,16 +116,6 @@ TEST(FunctorizedFactor, Print) {
auto factor =
MakeFunctorizedFactor<Matrix>(key, X, model, MultiplyFunctor(multiplier));
// redirect output to buffer so we can compare
stringstream buffer;
streambuf *old = cout.rdbuf(buffer.rdbuf());
factor.print();
// get output string and reset stdout
string actual = buffer.str();
cout.rdbuf(old);
string expected =
" keys = { X0 }\n"
" noise model: unit (9) \n"
@ -135,7 +126,7 @@ TEST(FunctorizedFactor, Print) {
"]\n"
" noise model sigmas: 1 1 1 1 1 1 1 1 1\n";
CHECK_EQUAL(expected, actual);
EXPECT(assert_print_equal(expected, factor));
}
/* ************************************************************************* */

10
gtsam/nonlinear/tests/testValues.cpp
View File

@ -595,15 +595,7 @@ TEST(Values, Demangle) {
values.insert(key1, v);
string expected = "Values with 1 values:\nValue v1: (Eigen::Matrix<double, 1, 3, 1, 1, 3>)\n[\n 5, 6, 7\n]\n\n";
stringstream buffer;
streambuf * old = cout.rdbuf(buffer.rdbuf());
values.print();
string actual = buffer.str();
cout.rdbuf(old);
EXPECT(assert_equal(expected, actual));
EXPECT(assert_print_equal(expected, values));
}
/* ************************************************************************* */

6
gtsam/sfm/MFAS.cpp
View File

@ -121,8 +121,8 @@ MFAS::MFAS(const TranslationEdges& relativeTranslations,
}
}
vector<Key> MFAS::computeOrdering() const {
vector<Key> ordering; // Nodes in MFAS order (result).
KeyVector MFAS::computeOrdering() const {
KeyVector ordering; // Nodes in MFAS order (result).
// A graph is an unordered map from keys to nodes. Each node contains a list
// of its adjacent nodes. Create the graph from the edgeWeights.
@ -140,7 +140,7 @@ vector<Key> MFAS::computeOrdering() const {
map<MFAS::KeyPair, double> MFAS::computeOutlierWeights() const {
// Find the ordering.
vector<Key> ordering = computeOrdering();
KeyVector ordering = computeOrdering();
// Create a map from the node key to its position in the ordering. This makes
// it easier to lookup positions of different nodes.

2
gtsam/sfm/MFAS.h
View File

@ -84,7 +84,7 @@ class MFAS {
* @brief Computes the 1D MFAS ordering of nodes in the graph
* @return orderedNodes: vector of nodes in the obtained order
*/
std::vector<Key> computeOrdering() const;
KeyVector computeOrdering() const;
/**
* @brief Computes the outlier weights of the graph. We define the outlier

10
gtsam/sfm/tests/testMFAS.cpp
View File

@ -25,7 +25,7 @@ using namespace gtsam;
vector<MFAS::KeyPair> edges = {make_pair(3, 2), make_pair(0, 1), make_pair(3, 1),
make_pair(1, 2), make_pair(0, 2), make_pair(3, 0)};
// nodes in the graph
vector<Key> nodes = {Key(0), Key(1), Key(2), Key(3)};
KeyVector nodes = {Key(0), Key(1), Key(2), Key(3)};
// weights from projecting in direction-1 (bad direction, outlier accepted)
vector<double> weights1 = {2, 1.5, 0.5, 0.25, 1, 0.75};
// weights from projecting in direction-2 (good direction, outlier rejected)
@ -47,10 +47,10 @@ map<MFAS::KeyPair, double> getEdgeWeights(const vector<MFAS::KeyPair> &edges,
TEST(MFAS, OrderingWeights2) {
MFAS mfas_obj(getEdgeWeights(edges, weights2));
vector<Key> ordered_nodes = mfas_obj.computeOrdering();
KeyVector ordered_nodes = mfas_obj.computeOrdering();
// ground truth (expected) ordering in this example
vector<Key> gt_ordered_nodes = {0, 1, 3, 2};
KeyVector gt_ordered_nodes = {0, 1, 3, 2};
// check if the expected ordering is obtained
for (size_t i = 0; i < ordered_nodes.size(); i++) {
@ -77,10 +77,10 @@ TEST(MFAS, OrderingWeights2) {
TEST(MFAS, OrderingWeights1) {
MFAS mfas_obj(getEdgeWeights(edges, weights1));
vector<Key> ordered_nodes = mfas_obj.computeOrdering();
KeyVector ordered_nodes = mfas_obj.computeOrdering();
// "ground truth" expected ordering in this example
vector<Key> gt_ordered_nodes = {3, 0, 1, 2};
KeyVector gt_ordered_nodes = {3, 0, 1, 2};
// check if the expected ordering is obtained
for (size_t i = 0; i < ordered_nodes.size(); i++) {

4
gtsam/slam/dataset.cpp
View File

@ -1164,8 +1164,8 @@ bool writeBAL(const string &filename, SfmData &data) {
for (size_t k = 0; k < track.number_measurements();
k++) { // for each observation of the 3D point j
size_t i = track.measurements[k].first; // camera id
double u0 = data.cameras[i].calibration().u0();
double v0 = data.cameras[i].calibration().v0();
double u0 = data.cameras[i].calibration().px();
double v0 = data.cameras[i].calibration().py();
if (u0 != 0 || v0 != 0) {
cout << "writeBAL has not been tested for calibration with nonzero "

20
gtsam/slam/dataset.h
View File

@ -211,16 +211,18 @@ GTSAM_EXPORT GraphAndValues load3D(const std::string& filename);
/// A measurement with its camera index
typedef std::pair<size_t, Point2> SfmMeasurement;
/// SfmTrack
/// Sift index for SfmTrack
typedef std::pair<size_t, size_t> SiftIndex;
/// Define the structure for the 3D points
struct SfmTrack {
SfmTrack(): p(0,0,0) {}
SfmTrack(const gtsam::Point3& pt) : p(pt) {}
Point3 p; ///< 3D position of the point
float r, g, b; ///< RGB color of the 3D point
std::vector<SfmMeasurement> measurements; ///< The 2D image projections (id,(u,v))
std::vector<SiftIndex> siftIndices;
/// Total number of measurements in this track
size_t number_measurements() const {
return measurements.size();
@ -233,11 +235,17 @@ struct SfmTrack {
SiftIndex siftIndex(size_t idx) const {
return siftIndices[idx];
}
Point3 point3() const {
/// Get 3D point
const Point3& point3() const {
return p;
}
/// Add measurement (camera_idx, Point2) to track
void add_measurement(size_t idx, const gtsam::Point2& m) {
measurements.emplace_back(idx, m);
}
};
/// Define the structure for the camera poses
typedef PinholeCamera<Cal3Bundler> SfmCamera;
@ -260,6 +268,14 @@ struct SfmData {
SfmTrack track(size_t idx) const {
return tracks[idx];
}
/// Add a track to SfmData
void add_track(const SfmTrack& t) {
tracks.push_back(t);
}
/// Add a camera to SfmData
void add_camera(const SfmCamera& cam){
cameras.push_back(cam);
}
};
/**

4
gtsam_unstable/geometry/tests/testSimilarity3.cpp
View File

@ -361,8 +361,12 @@ TEST(Similarity3, AlignPose3) {
vector<Pose3Pair> correspondences{bTa1, bTa2};
// Cayley transform cannot accommodate 180 degree rotations,
// hence we only test for Expmap
#ifdef GTSAM_ROT3_EXPMAP
Similarity3 actual_aSb = Similarity3::Align(correspondences);
EXPECT(assert_equal(expected_aSb, actual_aSb));
#endif
}
//******************************************************************************

2
python/gtsam/examples/README.md
View File

@ -39,7 +39,7 @@
| SelfCalibrationExample | |
| SFMdata | |
| SFMExample_bal_COLAMD_METIS | |
| SFMExample_bal | |
| SFMExample_bal | :heavy_check_mark: |
| SFMExample | :heavy_check_mark: |
| SFMExampleExpressions_bal | |
| SFMExampleExpressions | |

118
python/gtsam/examples/SFMExample_bal.py Normal file
View File

@ -0,0 +1,118 @@
"""
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
Solve a structure-from-motion problem from a "Bundle Adjustment in the Large" file
Author: Frank Dellaert (Python: Akshay Krishnan, John Lambert)
"""
import argparse
import logging
import sys
import matplotlib.pyplot as plt
import numpy as np
import gtsam
from gtsam import (
GeneralSFMFactorCal3Bundler,
PinholeCameraCal3Bundler,
PriorFactorPinholeCameraCal3Bundler,
readBal,
symbol_shorthand
)
C = symbol_shorthand.C
P = symbol_shorthand.P
logging.basicConfig(stream=sys.stdout, level=logging.DEBUG)
def run(args):
""" Run LM optimization with BAL input data and report resulting error """
input_file = gtsam.findExampleDataFile(args.input_file)
# Load the SfM data from file
scene_data = readBal(input_file)
logging.info(f"read {scene_data.number_tracks()} tracks on {scene_data.number_cameras()} cameras\n")
# Create a factor graph
graph = gtsam.NonlinearFactorGraph()
# We share *one* noiseModel between all projection factors
noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # one pixel in u and v
# Add measurements to the factor graph
j = 0
for t_idx in range(scene_data.number_tracks()):
track = scene_data.track(t_idx) # SfmTrack
# retrieve the SfmMeasurement objects
for m_idx in range(track.number_measurements()):
# i represents the camera index, and uv is the 2d measurement
i, uv = track.measurement(m_idx)
# note use of shorthand symbols C and P
graph.add(GeneralSFMFactorCal3Bundler(uv, noise, C(i), P(j)))
j += 1
# Add a prior on pose x1. This indirectly specifies where the origin is.
graph.push_back(
gtsam.PriorFactorPinholeCameraCal3Bundler(
C(0), scene_data.camera(0), gtsam.noiseModel.Isotropic.Sigma(9, 0.1)
)
)
# Also add a prior on the position of the first landmark to fix the scale
graph.push_back(
gtsam.PriorFactorPoint3(
P(0), scene_data.track(0).point3(), gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
)
)
# Create initial estimate
initial = gtsam.Values()
i = 0
# add each PinholeCameraCal3Bundler
for cam_idx in range(scene_data.number_cameras()):
camera = scene_data.camera(cam_idx)
initial.insert(C(i), camera)
i += 1
j = 0
# add each SfmTrack
for t_idx in range(scene_data.number_tracks()):
track = scene_data.track(t_idx)
initial.insert(P(j), track.point3())
j += 1
# Optimize the graph and print results
try:
params = gtsam.LevenbergMarquardtParams()
params.setVerbosityLM("ERROR")
lm = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
result = lm.optimize()
except Exception as e:
logging.exception("LM Optimization failed")
return
# Error drops from ~2764.22 to ~0.046
logging.info(f"final error: {graph.error(result)}")
if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument(
'-i',
'--input_file',
type=str,
default="dubrovnik-3-7-pre",
help='Read SFM data from the specified BAL file'
'The data format is described here: https://grail.cs.washington.edu/projects/bal/.'
'BAL files contain (nrPoses, nrPoints, nrObservations), followed by (i,j,u,v) tuples, '
'then (wx,wy,wz,tx,ty,tz,f,k1,k1) as Bundler camera calibrations w/ Rodrigues vector'
'and (x,y,z) 3d point initializations.'
)
run(parser.parse_args())

79
python/gtsam/tests/test_SfmData.py Normal file
View File

@ -0,0 +1,79 @@
"""
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
Atlanta, Georgia 30332-0415
All Rights Reserved
See LICENSE for the license information
Unit tests for testing dataset access.
Author: Frank Dellaert (Python: Sushmita Warrier)
"""
# pylint: disable=invalid-name, no-name-in-module, no-member
from __future__ import print_function
import unittest
import numpy as np
import gtsam
from gtsam.utils.test_case import GtsamTestCase
class TestSfmData(GtsamTestCase):
"""Tests for SfmData and SfmTrack modules."""
def setUp(self):
"""Initialize SfmData and SfmTrack"""
self.data = gtsam.SfmData()
# initialize SfmTrack with 3D point
self.tracks = gtsam.SfmTrack()
def test_tracks(self):
"""Test functions in SfmTrack"""
# measurement is of format (camera_idx, imgPoint)
# create arbitrary camera indices for two cameras
i1, i2 = 4,5
# create arbitrary image measurements for cameras i1 and i2
uv_i1 = gtsam.Point2(12.6, 82)
# translating point uv_i1 along X-axis
uv_i2 = gtsam.Point2(24.88, 82)
# add measurements to the track
self.tracks.add_measurement(i1, uv_i1)
self.tracks.add_measurement(i2, uv_i2)
# Number of measurements in the track is 2
self.assertEqual(self.tracks.number_measurements(), 2)
# camera_idx in the first measurement of the track corresponds to i1
cam_idx, img_measurement = self.tracks.measurement(0)
self.assertEqual(cam_idx, i1)
np.testing.assert_array_almost_equal(
gtsam.Point3(0.,0.,0.),
self.tracks.point3()
)
def test_data(self):
"""Test functions in SfmData"""
# Create new track with 3 measurements
i1, i2, i3 = 3,5,6
uv_i1 = gtsam.Point2(21.23, 45.64)
# translating along X-axis
uv_i2 = gtsam.Point2(45.7, 45.64)
uv_i3 = gtsam.Point2(68.35, 45.64)
# add measurements and arbitrary point to the track
measurements = [(i1, uv_i1), (i2, uv_i2), (i3, uv_i3)]
pt = gtsam.Point3(1.0, 6.0, 2.0)
track2 = gtsam.SfmTrack(pt)
track2.add_measurement(i1, uv_i1)
track2.add_measurement(i2, uv_i2)
track2.add_measurement(i3, uv_i3)
self.data.add_track(self.tracks)
self.data.add_track(track2)
# Number of tracks in SfmData is 2
self.assertEqual(self.data.number_tracks(), 2)
# camera idx of first measurement of second track corresponds to i1
cam_idx, img_measurement = self.data.track(1).measurement(0)
self.assertEqual(cam_idx, i1)
if __name__ == '__main__':
unittest.main()

52
tests/testNonlinearOptimizer.cpp
View File

@ -566,6 +566,58 @@ TEST( NonlinearOptimizer, logfile )
// EXPECT(actual.str()==expected.str());
}
/* ************************************************************************* */
TEST( NonlinearOptimizer, iterationHook_LM )
{
NonlinearFactorGraph fg(example::createReallyNonlinearFactorGraph());
Point2 x0(3,3);
Values c0;
c0.insert(X(1), x0);
// Levenberg-Marquardt
LevenbergMarquardtParams lmParams;
size_t lastIterCalled = 0;
lmParams.iterationHook = [&](size_t iteration, double oldError, double newError)
{
// Tests:
lastIterCalled = iteration;
EXPECT(newError<oldError);
// Example of evolution printout:
//std::cout << "iter: " << iteration << " error: " << oldError << " => " << newError <<"\n";
};
LevenbergMarquardtOptimizer(fg, c0, lmParams).optimize();
EXPECT(lastIterCalled>5);
}
/* ************************************************************************* */
TEST( NonlinearOptimizer, iterationHook_CG )
{
NonlinearFactorGraph fg(example::createReallyNonlinearFactorGraph());
Point2 x0(3,3);
Values c0;
c0.insert(X(1), x0);
// Levenberg-Marquardt
NonlinearConjugateGradientOptimizer::Parameters cgParams;
size_t lastIterCalled = 0;
cgParams.iterationHook = [&](size_t iteration, double oldError, double newError)
{
// Tests:
lastIterCalled = iteration;
EXPECT(newError<oldError);
// Example of evolution printout:
//std::cout << "iter: " << iteration << " error: " << oldError << " => " << newError <<"\n";
};
NonlinearConjugateGradientOptimizer(fg, c0, cgParams).optimize();
EXPECT(lastIterCalled>5);
}
/* ************************************************************************* */
//// Minimal traits example
struct MyType : public Vector3 {

2
wrap/cmake/PybindWrap.cmake
View File

@ -146,7 +146,7 @@ function(install_python_scripts
else()
set(build_type_tag "")
endif()
# Split up filename to strip trailing '/' in WRAP_CYTHON_INSTALL_PATH if
# Split up filename to strip trailing '/' in GTSAM_PY_INSTALL_PATH if
# there is one
get_filename_component(location "${dest_directory}" PATH)
get_filename_component(name "${dest_directory}" NAME)