Merge branch 'develop' into feature/lm-iteration-hook-impl
Varun Agrawal
commit
1c087c5196
|
|
@ -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 \
|
||||
|
|
|
|||
|
|
@ -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'
|
||||
|
|
|
|||
|
|
@ -40,12 +40,12 @@ jobs:
|
|||
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'
|
||||
|
|
|
|||
|
|
@ -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,12 +76,12 @@ 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'
|
||||
|
|
@ -92,17 +91,17 @@ jobs:
|
|||
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'
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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: |
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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.")
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -2767,6 +2767,8 @@ virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor {
|
|||
|
||||
class SfmTrack {
|
||||
SfmTrack();
|
||||
SfmTrack(const gtsam::Point3& pt);
|
||||
const Point3& point3() const;
|
||||
|
||||
double r;
|
||||
double g;
|
||||
|
|
@ -2774,20 +2776,24 @@ class SfmTrack {
|
|||
// TODO Need to close wrap#10 to allow this to work.
|
||||
// std::vector<pair<size_t, gtsam::Point2>> measurements;
|
||||
|
||||
Point3 point3() const;
|
||||
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);
|
||||
|
||||
|
|
|
|||
|
|
@ -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));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -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));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -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
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
|
|
|
|||
|
|
@ -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()
|
||||
Loading…
Reference in New Issue