Merge remote-tracking branch 'origin/develop' into feature/improvementsIncrementalFilter
Frank Dellaert
commit
67b65f9845
|
|
@ -1,4 +1,5 @@
|
|||
/build*
|
||||
/debug*
|
||||
.idea
|
||||
*.pyc
|
||||
*.DS_Store
|
||||
|
|
|
|||
|
|
@ -84,6 +84,12 @@ set(GTSAM_PYTHON_VERSION "Default" CACHE STRING "The version of python to build
|
|||
if((GTSAM_INSTALL_MATLAB_TOOLBOX OR GTSAM_INSTALL_CYTHON_TOOLBOX) AND NOT GTSAM_BUILD_WRAP)
|
||||
message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX or GTSAM_INSTALL_CYTHON_TOOLBOX is enabled, please also enable GTSAM_BUILD_WRAP")
|
||||
endif()
|
||||
if((GTSAM_INSTALL_MATLAB_TOOLBOX OR GTSAM_INSTALL_CYTHON_TOOLBOX) AND GTSAM_BUILD_TYPE_POSTFIXES)
|
||||
set(CURRENT_POSTFIX ${CMAKE_${CMAKE_BUILD_TYPE}_POSTFIX})
|
||||
if(NOT "${CURRENT_POSTFIX}" STREQUAL "")
|
||||
message(FATAL_ERROR "Cannot use executable postfixes with the matlab or cython wrappers. Please disable GTSAM_BUILD_TYPE_POSTFIXES")
|
||||
endif()
|
||||
endif()
|
||||
if(GTSAM_INSTALL_WRAP AND NOT GTSAM_BUILD_WRAP)
|
||||
message(FATAL_ERROR "GTSAM_INSTALL_WRAP is enabled, please also enable GTSAM_BUILD_WRAP")
|
||||
endif()
|
||||
|
|
@ -130,6 +136,14 @@ if(MSVC)
|
|||
endif()
|
||||
endif()
|
||||
|
||||
# If building DLLs in MSVC, we need to avoid EIGEN_STATIC_ASSERT()
|
||||
# or explicit instantiation will generate build errors.
|
||||
# See: https://bitbucket.org/gtborg/gtsam/issues/417/fail-to-build-on-msvc-2017
|
||||
#
|
||||
if(MSVC AND BUILD_SHARED_LIBS)
|
||||
list(APPEND GTSAM_COMPILE_DEFINITIONS_PUBLIC EIGEN_NO_STATIC_ASSERT)
|
||||
endif()
|
||||
|
||||
# Store these in variables so they are automatically replicated in GTSAMConfig.cmake and such.
|
||||
set(BOOST_FIND_MINIMUM_VERSION 1.43)
|
||||
set(BOOST_FIND_MINIMUM_COMPONENTS serialization system filesystem thread program_options date_time timer chrono regex)
|
||||
|
|
@ -142,22 +156,43 @@ if(NOT Boost_SERIALIZATION_LIBRARY OR NOT Boost_SYSTEM_LIBRARY OR NOT Boost_FILE
|
|||
message(FATAL_ERROR "Missing required Boost components >= v1.43, please install/upgrade Boost or configure your search paths.")
|
||||
endif()
|
||||
|
||||
option(GTSAM_DISABLE_NEW_TIMERS "Disables using Boost.chrono for timing" OFF)
|
||||
# Allow for not using the timer libraries on boost < 1.48 (GTSAM timing code falls back to old timer library)
|
||||
option(GTSAM_DISABLE_NEW_TIMERS "Disables using Boost.chrono for timing" OFF)
|
||||
|
||||
# JLBC: This was once updated to target-based names (Boost::xxx), but it caused
|
||||
# problems with Boost versions newer than FindBoost.cmake was prepared to handle,
|
||||
# so we downgraded this to classic filenames-based variables, and manually adding
|
||||
# the target_include_directories(xxx ${Boost_INCLUDE_DIR})
|
||||
set(GTSAM_BOOST_LIBRARIES
|
||||
Boost::serialization
|
||||
Boost::system
|
||||
Boost::filesystem
|
||||
Boost::thread
|
||||
Boost::date_time
|
||||
Boost::regex
|
||||
optimized
|
||||
${Boost_SERIALIZATION_LIBRARY_RELEASE}
|
||||
${Boost_SYSTEM_LIBRARY_RELEASE}
|
||||
${Boost_FILESYSTEM_LIBRARY_RELEASE}
|
||||
${Boost_THREAD_LIBRARY_RELEASE}
|
||||
${Boost_DATE_TIME_LIBRARY_RELEASE}
|
||||
${Boost_REGEX_LIBRARY_RELEASE}
|
||||
debug
|
||||
${Boost_SERIALIZATION_LIBRARY_DEBUG}
|
||||
${Boost_SYSTEM_LIBRARY_DEBUG}
|
||||
${Boost_FILESYSTEM_LIBRARY_DEBUG}
|
||||
${Boost_THREAD_LIBRARY_DEBUG}
|
||||
${Boost_DATE_TIME_LIBRARY_DEBUG}
|
||||
${Boost_REGEX_LIBRARY_DEBUG}
|
||||
)
|
||||
message(STATUS "GTSAM_BOOST_LIBRARIES: ${GTSAM_BOOST_LIBRARIES}")
|
||||
if (GTSAM_DISABLE_NEW_TIMERS)
|
||||
message("WARNING: GTSAM timing instrumentation manually disabled")
|
||||
list(APPEND GTSAM_COMPILE_DEFINITIONS_PUBLIC DGTSAM_DISABLE_NEW_TIMERS)
|
||||
else()
|
||||
if(Boost_TIMER_LIBRARY)
|
||||
list(APPEND GTSAM_BOOST_LIBRARIES Boost::timer Boost::chrono)
|
||||
list(APPEND GTSAM_BOOST_LIBRARIES
|
||||
optimized
|
||||
${Boost_TIMER_LIBRARY_RELEASE}
|
||||
${Boost_CHRONO_LIBRARY_RELEASE}
|
||||
debug
|
||||
${Boost_TIMER_LIBRARY_DEBUG}
|
||||
${Boost_CHRONO_LIBRARY_DEBUG}
|
||||
)
|
||||
else()
|
||||
list(APPEND GTSAM_BOOST_LIBRARIES rt) # When using the header-only boost timer library, need -lrt
|
||||
message("WARNING: GTSAM timing instrumentation will use the older, less accurate, Boost timer library because boost older than 1.48 was found.")
|
||||
|
|
|
|||
|
|
@ -6,7 +6,7 @@ file(GLOB cppunitlite_src "*.cpp")
|
|||
add_library(CppUnitLite STATIC ${cppunitlite_src} ${cppunitlite_headers})
|
||||
list(APPEND GTSAM_EXPORTED_TARGETS CppUnitLite)
|
||||
set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)
|
||||
target_link_libraries(CppUnitLite PUBLIC Boost::boost) # boost/lexical_cast.h
|
||||
target_include_directories(CppUnitLite PUBLIC ${Boost_INCLUDE_DIR}) # boost/lexical_cast.h
|
||||
|
||||
gtsam_assign_source_folders("${cppunitlite_headers};${cppunitlite_src}") # MSVC project structure
|
||||
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* 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)
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* 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)
|
||||
|
|
@ -36,7 +36,7 @@ Test *Test::getNext() const
|
|||
}
|
||||
|
||||
void Test::setNext(Test *test)
|
||||
{
|
||||
{
|
||||
next_ = test;
|
||||
}
|
||||
|
||||
|
|
@ -46,9 +46,9 @@ bool Test::check(long expected, long actual, TestResult& result, const std::stri
|
|||
return true;
|
||||
result.addFailure (
|
||||
Failure (
|
||||
name_,
|
||||
name_,
|
||||
boost::lexical_cast<std::string> (__FILE__),
|
||||
__LINE__,
|
||||
__LINE__,
|
||||
boost::lexical_cast<std::string> (expected),
|
||||
boost::lexical_cast<std::string> (actual)));
|
||||
|
||||
|
|
@ -63,10 +63,10 @@ bool Test::check(const std::string& expected, const std::string& actual, TestRes
|
|||
return true;
|
||||
result.addFailure (
|
||||
Failure (
|
||||
name_,
|
||||
name_,
|
||||
boost::lexical_cast<std::string> (__FILE__),
|
||||
__LINE__,
|
||||
expected,
|
||||
__LINE__,
|
||||
expected,
|
||||
actual));
|
||||
|
||||
return false;
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* 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)
|
||||
|
|
@ -12,7 +12,7 @@
|
|||
///////////////////////////////////////////////////////////////////////////////
|
||||
//
|
||||
// TEST.H
|
||||
//
|
||||
//
|
||||
// This file contains the Test class along with the macros which make effective
|
||||
// in the harness.
|
||||
//
|
||||
|
|
@ -66,7 +66,7 @@ protected:
|
|||
virtual ~testGroup##testName##Test () {};\
|
||||
void run (TestResult& result_);} \
|
||||
testGroup##testName##Instance; \
|
||||
void testGroup##testName##Test::run (TestResult& result_)
|
||||
void testGroup##testName##Test::run (TestResult& result_)
|
||||
|
||||
/**
|
||||
* Declare friend in a class to test its private methods
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* 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)
|
||||
|
|
@ -12,7 +12,7 @@
|
|||
///////////////////////////////////////////////////////////////////////////////
|
||||
//
|
||||
// TESTHARNESS.H
|
||||
//
|
||||
//
|
||||
// The primary include file for the framework.
|
||||
//
|
||||
///////////////////////////////////////////////////////////////////////////////
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* 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)
|
||||
|
|
@ -18,26 +18,26 @@
|
|||
#include "TestResult.h"
|
||||
#include "TestRegistry.h"
|
||||
|
||||
void TestRegistry::addTest (Test *test)
|
||||
void TestRegistry::addTest (Test *test)
|
||||
{
|
||||
instance ().add (test);
|
||||
}
|
||||
|
||||
|
||||
int TestRegistry::runAllTests (TestResult& result)
|
||||
int TestRegistry::runAllTests (TestResult& result)
|
||||
{
|
||||
return instance ().run (result);
|
||||
}
|
||||
|
||||
|
||||
TestRegistry& TestRegistry::instance ()
|
||||
TestRegistry& TestRegistry::instance ()
|
||||
{
|
||||
static TestRegistry registry;
|
||||
return registry;
|
||||
}
|
||||
|
||||
|
||||
void TestRegistry::add (Test *test)
|
||||
void TestRegistry::add (Test *test)
|
||||
{
|
||||
if (tests == 0) {
|
||||
test->setNext(0);
|
||||
|
|
@ -52,7 +52,7 @@ void TestRegistry::add (Test *test)
|
|||
}
|
||||
|
||||
|
||||
int TestRegistry::run (TestResult& result)
|
||||
int TestRegistry::run (TestResult& result)
|
||||
{
|
||||
result.testsStarted ();
|
||||
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* 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)
|
||||
|
|
@ -12,9 +12,9 @@
|
|||
///////////////////////////////////////////////////////////////////////////////
|
||||
//
|
||||
// TESTREGISTRY.H
|
||||
//
|
||||
// TestRegistry is a singleton collection of all the tests to run in a system.
|
||||
//
|
||||
//
|
||||
// TestRegistry is a singleton collection of all the tests to run in a system.
|
||||
//
|
||||
///////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
#ifndef TESTREGISTRY_H
|
||||
|
|
@ -38,7 +38,7 @@ private:
|
|||
void add (Test *test);
|
||||
int run (TestResult& result);
|
||||
|
||||
|
||||
|
||||
Test *tests;
|
||||
Test *lastTest;
|
||||
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* 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)
|
||||
|
|
@ -22,12 +22,12 @@ TestResult::TestResult ()
|
|||
}
|
||||
|
||||
|
||||
void TestResult::testsStarted ()
|
||||
void TestResult::testsStarted ()
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
void TestResult::addFailure (const Failure& failure)
|
||||
void TestResult::addFailure (const Failure& failure)
|
||||
{
|
||||
if (failure.lineNumber < 0) // allow for no line number
|
||||
fprintf (stdout, "%s%s%s%s\n",
|
||||
|
|
@ -48,7 +48,7 @@ void TestResult::addFailure (const Failure& failure)
|
|||
}
|
||||
|
||||
|
||||
void TestResult::testsEnded ()
|
||||
void TestResult::testsEnded ()
|
||||
{
|
||||
if (failureCount > 0)
|
||||
fprintf (stdout, "There were %d failures\n", failureCount);
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* 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)
|
||||
|
|
@ -12,10 +12,10 @@
|
|||
///////////////////////////////////////////////////////////////////////////////
|
||||
//
|
||||
// TESTRESULT.H
|
||||
//
|
||||
//
|
||||
// A TestResult is a collection of the history of some test runs. Right now
|
||||
// it just collects failures.
|
||||
//
|
||||
//
|
||||
///////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
#ifndef TESTRESULT_H
|
||||
|
|
@ -26,12 +26,12 @@ class Failure;
|
|||
class TestResult
|
||||
{
|
||||
public:
|
||||
TestResult ();
|
||||
TestResult ();
|
||||
virtual ~TestResult() {};
|
||||
virtual void testsStarted ();
|
||||
virtual void addFailure (const Failure& failure);
|
||||
virtual void testsEnded ();
|
||||
|
||||
|
||||
int getFailureCount() {return failureCount;}
|
||||
|
||||
private:
|
||||
|
|
|
|||
27
INSTALL.md
27
INSTALL.md
|
|
@ -166,3 +166,30 @@ Another useful debugging symbol is _GLIBCXX_DEBUG, which enables debug checks an
|
|||
NOTE: The native Snow Leopard g++ compiler/library contains a bug that makes it impossible to use _GLIBCXX_DEBUG. MacPorts g++ compilers do work with it though.
|
||||
|
||||
NOTE: If _GLIBCXX_DEBUG is used to compile gtsam, anything that links against gtsam will need to be compiled with _GLIBCXX_DEBUG as well, due to the use of header-only Eigen.
|
||||
|
||||
|
||||
## Installing MKL on Linux
|
||||
|
||||
Intel has a guide for installing MKL on Linux through APT repositories at <https://software.intel.com/en-us/articles/installing-intel-free-libs-and-python-apt-repo>.
|
||||
|
||||
After following the instructions, add the following to your `~/.bashrc` (and afterwards, open a new terminal before compiling GTSAM):
|
||||
`LD_PRELOAD` need only be set if you are building the cython wrapper to use GTSAM from python.
|
||||
```sh
|
||||
source /opt/intel/mkl/bin/mklvars.sh intel64
|
||||
export LD_PRELOAD="$LD_PRELOAD:/opt/intel/mkl/lib/intel64/libmkl_core.so:/opt/intel/mkl/lib/intel64/libmkl_sequential.so"
|
||||
```
|
||||
To use MKL in GTSAM pass the flag `-DGTSAM_WITH_EIGEN_MKL=ON` to cmake.
|
||||
|
||||
|
||||
The `LD_PRELOAD` fix seems to be related to a well known problem with MKL which leads to lots of undefined symbol errors, for example:
|
||||
- <https://software.intel.com/en-us/forums/intel-math-kernel-library/topic/300857>
|
||||
- <https://software.intel.com/en-us/forums/intel-distribution-for-python/topic/628976>
|
||||
- <https://groups.google.com/a/continuum.io/forum/#!topic/anaconda/J3YGoef64z8>
|
||||
|
||||
Failing to specify `LD_PRELOAD` may lead to errors such as:
|
||||
`ImportError: /opt/intel/mkl/lib/intel64/libmkl_vml_avx2.so: undefined symbol: mkl_serv_getenv`
|
||||
or
|
||||
`Intel MKL FATAL ERROR: Cannot load libmkl_avx2.so or libmkl_def.so.`
|
||||
when importing GTSAM using the cython wrapper in python.
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -36,7 +36,9 @@ Prerequisites:
|
|||
Optional prerequisites - used automatically if findable by CMake:
|
||||
|
||||
- [Intel Threaded Building Blocks (TBB)](http://www.threadingbuildingblocks.org/) (Ubuntu: `sudo apt-get install libtbb-dev`)
|
||||
- [Intel Math Kernel Library (MKL)](http://software.intel.com/en-us/intel-mkl)
|
||||
- [Intel Math Kernel Library (MKL)](http://software.intel.com/en-us/intel-mkl) (Ubuntu: [installing using APT](https://software.intel.com/en-us/articles/installing-intel-free-libs-and-python-apt-repo))
|
||||
- See [INSTALL.md](INSTALL.md) for more installation information
|
||||
- Note that MKL may not provide a speedup in all cases. Make sure to benchmark your problem with and without MKL.
|
||||
|
||||
GTSAM 4 Compatibility
|
||||
---------------------
|
||||
|
|
|
|||
|
|
@ -30,9 +30,12 @@ find_package(GTSAM REQUIRED) # Uses installed package
|
|||
###################################################################################
|
||||
# Build static library from common sources
|
||||
set(CONVENIENCE_LIB_NAME ${PROJECT_NAME})
|
||||
add_library(${CONVENIENCE_LIB_NAME} STATIC example/PrintExamples.h example/PrintExamples.cpp)
|
||||
add_library(${CONVENIENCE_LIB_NAME} SHARED example/PrintExamples.h example/PrintExamples.cpp)
|
||||
target_link_libraries(${CONVENIENCE_LIB_NAME} gtsam)
|
||||
|
||||
# Install library
|
||||
install(TARGETS ${CONVENIENCE_LIB_NAME} LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin)
|
||||
|
||||
###################################################################################
|
||||
# Build tests (CMake tracks the dependecy to link with GTSAM through our project's static library)
|
||||
gtsamAddTestsGlob("example" "tests/test*.cpp" "" "${CONVENIENCE_LIB_NAME}")
|
||||
|
|
|
|||
|
|
@ -0,0 +1,32 @@
|
|||
# MATLAB Wrapper Example Project
|
||||
|
||||
This project serves as a lightweight example for demonstrating how to wrap C++ code in MATLAB using GTSAM.
|
||||
|
||||
## Compiling
|
||||
|
||||
We follow the regular build procedure inside the `example_project` directory:
|
||||
|
||||
```sh
|
||||
mkdir build && cd build
|
||||
cmake ..
|
||||
make -j8
|
||||
sudo make install
|
||||
|
||||
sudo ldconfig # ensures the shared object file generated is correctly loaded
|
||||
```
|
||||
|
||||
## Usage
|
||||
|
||||
Now you can open MATLAB and add the `gtsam_toolbox` to the MATLAB path
|
||||
|
||||
```matlab
|
||||
addpath('/usr/local/gtsam_toolbox')
|
||||
```
|
||||
|
||||
At this point you are ready to run the example project. Starting from the `example_project` directory inside MATLAB, simply run code like regular MATLAB, e.g.
|
||||
|
||||
```matlab
|
||||
pe = example.PrintExamples();
|
||||
pe.sayHello();
|
||||
pe.sayGoodbye();
|
||||
```
|
||||
|
|
@ -18,11 +18,14 @@
|
|||
// This is an interface file for automatic MATLAB wrapper generation. See
|
||||
// gtsam.h for full documentation and more examples.
|
||||
|
||||
#include <example/PrintExamples.h>
|
||||
|
||||
namespace example {
|
||||
|
||||
class PrintExamples {
|
||||
void sayHello() const;
|
||||
void sayGoodbye() const;
|
||||
PrintExamples();
|
||||
void sayHello() const;
|
||||
void sayGoodbye() const;
|
||||
};
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -17,6 +17,7 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <iostream>
|
||||
#include <string>
|
||||
|
||||
namespace example {
|
||||
|
|
|
|||
|
|
@ -62,7 +62,7 @@ Examples: noiseModel_Gaussian, noiseModel_mEstimator_Tukey
|
|||
- Casting from a base class to a derive class must be done explicitly.
|
||||
Examples:
|
||||
```Python
|
||||
noiseBase = factor.get_noiseModel()
|
||||
noiseBase = factor.noiseModel()
|
||||
noiseGaussian = dynamic_cast_noiseModel_Gaussian_noiseModel_Base(noiseBase)
|
||||
```
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,118 @@
|
|||
"""
|
||||
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
|
||||
Atlanta, Georgia 30332-0415
|
||||
All Rights Reserved
|
||||
|
||||
See LICENSE for the license information
|
||||
|
||||
Example comparing DoglegOptimizer with Levenberg-Marquardt.
|
||||
Author: Frank Dellaert
|
||||
"""
|
||||
# pylint: disable=no-member, invalid-name
|
||||
|
||||
import math
|
||||
import argparse
|
||||
|
||||
import gtsam
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
|
||||
|
||||
def run(args):
|
||||
"""Test Dogleg vs LM, inspired by issue #452."""
|
||||
|
||||
# print parameters
|
||||
print("num samples = {}, deltaInitial = {}".format(
|
||||
args.num_samples, args.delta))
|
||||
|
||||
# Ground truth solution
|
||||
T11 = gtsam.Pose2(0, 0, 0)
|
||||
T12 = gtsam.Pose2(1, 0, 0)
|
||||
T21 = gtsam.Pose2(0, 1, 0)
|
||||
T22 = gtsam.Pose2(1, 1, 0)
|
||||
|
||||
# Factor graph
|
||||
graph = gtsam.NonlinearFactorGraph()
|
||||
|
||||
# Priors
|
||||
prior = gtsam.noiseModel_Isotropic.Sigma(3, 1)
|
||||
graph.add(gtsam.PriorFactorPose2(11, T11, prior))
|
||||
graph.add(gtsam.PriorFactorPose2(21, T21, prior))
|
||||
|
||||
# Odometry
|
||||
model = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.01, 0.01, 0.3]))
|
||||
graph.add(gtsam.BetweenFactorPose2(11, 12, T11.between(T12), model))
|
||||
graph.add(gtsam.BetweenFactorPose2(21, 22, T21.between(T22), model))
|
||||
|
||||
# Range
|
||||
model_rho = gtsam.noiseModel_Isotropic.Sigma(1, 0.01)
|
||||
graph.add(gtsam.RangeFactorPose2(12, 22, 1.0, model_rho))
|
||||
|
||||
params = gtsam.DoglegParams()
|
||||
params.setDeltaInitial(args.delta) # default is 10
|
||||
|
||||
# Add progressively more noise to ground truth
|
||||
sigmas = [0.01, 0.1, 0.2, 0.5, 1, 2, 5, 10, 20]
|
||||
n = len(sigmas)
|
||||
p_dl, s_dl, p_lm, s_lm = [0]*n, [0]*n, [0]*n, [0]*n
|
||||
for i, sigma in enumerate(sigmas):
|
||||
dl_fails, lm_fails = 0, 0
|
||||
# Attempt num_samples optimizations for both DL and LM
|
||||
for _attempt in range(args.num_samples):
|
||||
initial = gtsam.Values()
|
||||
initial.insert(11, T11.retract(np.random.normal(0, sigma, 3)))
|
||||
initial.insert(12, T12.retract(np.random.normal(0, sigma, 3)))
|
||||
initial.insert(21, T21.retract(np.random.normal(0, sigma, 3)))
|
||||
initial.insert(22, T22.retract(np.random.normal(0, sigma, 3)))
|
||||
|
||||
# Run dogleg optimizer
|
||||
dl = gtsam.DoglegOptimizer(graph, initial, params)
|
||||
result = dl.optimize()
|
||||
dl_fails += graph.error(result) > 1e-9
|
||||
|
||||
# Run
|
||||
lm = gtsam.LevenbergMarquardtOptimizer(graph, initial)
|
||||
result = lm.optimize()
|
||||
lm_fails += graph.error(result) > 1e-9
|
||||
|
||||
# Calculate Bayes estimate of success probability
|
||||
# using a beta prior of alpha=0.5, beta=0.5
|
||||
alpha, beta = 0.5, 0.5
|
||||
v = args.num_samples+alpha+beta
|
||||
p_dl[i] = (args.num_samples-dl_fails+alpha)/v
|
||||
p_lm[i] = (args.num_samples-lm_fails+alpha)/v
|
||||
|
||||
def stddev(p):
|
||||
"""Calculate standard deviation."""
|
||||
return math.sqrt(p*(1-p)/(1+v))
|
||||
|
||||
s_dl[i] = stddev(p_dl[i])
|
||||
s_lm[i] = stddev(p_lm[i])
|
||||
|
||||
fmt = "sigma= {}:\tDL success {:.2f}% +/- {:.2f}%, LM success {:.2f}% +/- {:.2f}%"
|
||||
print(fmt.format(sigma,
|
||||
100*p_dl[i], 100*s_dl[i],
|
||||
100*p_lm[i], 100*s_lm[i]))
|
||||
|
||||
if args.plot:
|
||||
fig, ax = plt.subplots()
|
||||
dl_plot = plt.errorbar(sigmas, p_dl, yerr=s_dl, label="Dogleg")
|
||||
lm_plot = plt.errorbar(sigmas, p_lm, yerr=s_lm, label="LM")
|
||||
plt.title("Dogleg emprical success vs. LM")
|
||||
plt.legend(handles=[dl_plot, lm_plot])
|
||||
ax.set_xlim(0, sigmas[-1]+1)
|
||||
ax.set_ylim(0, 1)
|
||||
plt.show()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
parser = argparse.ArgumentParser(
|
||||
description="Compare Dogleg and LM success rates")
|
||||
parser.add_argument("-n", "--num_samples", type=int, default=1000,
|
||||
help="Number of samples for each sigma")
|
||||
parser.add_argument("-d", "--delta", type=float, default=10.0,
|
||||
help="Initial delta for dogleg")
|
||||
parser.add_argument("-p", "--plot", action="store_true",
|
||||
help="Flag to plot results")
|
||||
args = parser.parse_args()
|
||||
run(args)
|
||||
|
|
@ -115,7 +115,7 @@ class ThreeLinkArm(object):
|
|||
|
||||
l1Zl1 = expmap(0.0, 0.0, q[0])
|
||||
l2Zl2 = expmap(0.0, self.L1, q[1])
|
||||
l3Zl3 = expmap(0.0, 7.0, q[2])
|
||||
l3Zl3 = expmap(0.0, self.L1+self.L2, q[2])
|
||||
return compose(l1Zl1, l2Zl2, l3Zl3, self.sXt0)
|
||||
|
||||
def ik(self, sTt_desired, e=1e-9):
|
||||
|
|
@ -297,12 +297,18 @@ def run_example():
|
|||
""" Use trajectory interpolation and then trajectory tracking a la Murray
|
||||
to move a 3-link arm on a straight line.
|
||||
"""
|
||||
# Create arm
|
||||
arm = ThreeLinkArm()
|
||||
|
||||
# Get initial pose using forward kinematics
|
||||
q = np.radians(vector3(30, -30, 45))
|
||||
sTt_initial = arm.fk(q)
|
||||
|
||||
# Create interpolated trajectory in task space to desired goal pose
|
||||
sTt_goal = Pose2(2.4, 4.3, math.radians(0))
|
||||
poses = trajectory(sTt_initial, sTt_goal, 50)
|
||||
|
||||
# Setup figure and plot initial pose
|
||||
fignum = 0
|
||||
fig = plt.figure(fignum)
|
||||
axes = fig.gca()
|
||||
|
|
@ -310,6 +316,8 @@ def run_example():
|
|||
axes.set_ylim(0, 10)
|
||||
gtsam_plot.plot_pose2(fignum, arm.fk(q))
|
||||
|
||||
# For all poses in interpolated trajectory, calculate dq to move to next pose.
|
||||
# We do this by calculating the local Jacobian J and doing dq = inv(J)*delta(sTt, pose).
|
||||
for pose in poses:
|
||||
sTt = arm.fk(q)
|
||||
error = delta(sTt, pose)
|
||||
|
|
|
|||
|
|
@ -30,9 +30,9 @@ class TestPose3(GtsamTestCase):
|
|||
self.gtsamAssertEquals(actual, expected, 1e-6)
|
||||
|
||||
def test_transform_to(self):
|
||||
"""Test transform_to method."""
|
||||
"""Test transformTo method."""
|
||||
transform = Pose3(Rot3.Rodrigues(0, 0, -1.570796), Point3(2, 4, 0))
|
||||
actual = transform.transform_to(Point3(3, 2, 10))
|
||||
actual = transform.transformTo(Point3(3, 2, 10))
|
||||
expected = Point3(2, 1, 10)
|
||||
self.gtsamAssertEquals(actual, expected, 1e-6)
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,38 @@
|
|||
"""
|
||||
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
|
||||
Atlanta, Georgia 30332-0415
|
||||
All Rights Reserved
|
||||
|
||||
See LICENSE for the license information
|
||||
|
||||
Unit tests for Disjoint Set Forest.
|
||||
Author: Frank Dellaert & Varun Agrawal
|
||||
"""
|
||||
# pylint: disable=invalid-name, no-name-in-module, no-member
|
||||
|
||||
from __future__ import print_function
|
||||
|
||||
import unittest
|
||||
|
||||
import gtsam
|
||||
from gtsam.utils.test_case import GtsamTestCase
|
||||
|
||||
|
||||
class TestDSFMap(GtsamTestCase):
|
||||
"""Tests for DSFMap."""
|
||||
|
||||
def test_all(self):
|
||||
"""Test everything in DFSMap."""
|
||||
def key(index_pair):
|
||||
return index_pair.i(), index_pair.j()
|
||||
|
||||
dsf = gtsam.DSFMapIndexPair()
|
||||
pair1 = gtsam.IndexPair(1, 18)
|
||||
self.assertEqual(key(dsf.find(pair1)), key(pair1))
|
||||
pair2 = gtsam.IndexPair(2, 2)
|
||||
dsf.merge(pair1, pair2)
|
||||
self.assertTrue(dsf.find(pair1), dsf.find(pair1))
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
unittest.main()
|
||||
|
|
@ -35,7 +35,7 @@ inline PyArrayObject *_ndarray_view<double>(double *data, long rows, long cols,
|
|||
return ndarray_double_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1);
|
||||
} else {
|
||||
// Eigen column-major mode: row_stride=outer_stride, and col_stride=inner_stride
|
||||
// If no stride is given, the cow_stride is set to the number of rows.
|
||||
// If no stride is given, the cow_stride is set to the number of rows.
|
||||
return ndarray_double_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows);
|
||||
}
|
||||
}
|
||||
|
|
@ -355,7 +355,7 @@ public:
|
|||
FlattenedMap()
|
||||
: Base(NULL, 0, 0),
|
||||
object_(NULL) {}
|
||||
|
||||
|
||||
FlattenedMap(Scalar *data, long rows, long cols, long outer_stride=0, long inner_stride=0)
|
||||
: Base(data, rows, cols,
|
||||
Eigen::Stride<_StrideOuter, _StrideInner>(outer_stride, inner_stride)),
|
||||
|
|
@ -363,7 +363,7 @@ public:
|
|||
}
|
||||
|
||||
FlattenedMap(PyArrayObject *object)
|
||||
: Base((Scalar *)((PyArrayObject*)object)->data,
|
||||
: Base((Scalar *)((PyArrayObject*)object)->data,
|
||||
// : Base(_from_numpy<Scalar>((PyArrayObject*)object),
|
||||
(((PyArrayObject*)object)->nd == 2) ? ((PyArrayObject*)object)->dimensions[0] : 1,
|
||||
(((PyArrayObject*)object)->nd == 2) ? ((PyArrayObject*)object)->dimensions[1] : ((PyArrayObject*)object)->dimensions[0],
|
||||
|
|
@ -390,7 +390,7 @@ public:
|
|||
|
||||
return *this;
|
||||
}
|
||||
|
||||
|
||||
operator Base() const {
|
||||
return static_cast<Base>(*this);
|
||||
}
|
||||
|
|
@ -429,7 +429,7 @@ public:
|
|||
(ColsAtCompileTime == Eigen::Dynamic) ? 0 : ColsAtCompileTime),
|
||||
object_(NULL) {
|
||||
}
|
||||
|
||||
|
||||
Map(Scalar *data, long rows, long cols)
|
||||
: Base(data, rows, cols),
|
||||
object_(NULL) {}
|
||||
|
|
@ -456,7 +456,7 @@ public:
|
|||
throw std::invalid_argument("Numpy array must be a in one contiguous segment to be able to be transferred to a Eigen Map.");
|
||||
Py_XINCREF(object_);
|
||||
}
|
||||
|
||||
|
||||
Map &operator=(const Map &other) {
|
||||
if (other.object_) {
|
||||
new (this) Map(other.object_);
|
||||
|
|
@ -474,7 +474,7 @@ public:
|
|||
MapBase<MatrixType>::operator=(other);
|
||||
return *this;
|
||||
}
|
||||
|
||||
|
||||
operator Base() const {
|
||||
return static_cast<Base>(*this);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,5 +1,5 @@
|
|||
// add unary measurement factors, like GPS, on all three poses
|
||||
noiseModel::Diagonal::shared_ptr unaryNoise =
|
||||
noiseModel::Diagonal::shared_ptr unaryNoise =
|
||||
noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1)); // 10cm std on x,y
|
||||
graph.add(boost::make_shared<UnaryFactor>(1, 0.0, 0.0, unaryNoise));
|
||||
graph.add(boost::make_shared<UnaryFactor>(2, 2.0, 0.0, unaryNoise));
|
||||
|
|
|
|||
|
|
@ -5,7 +5,7 @@ public:
|
|||
UnaryFactor(Key j, double x, double y, const SharedNoiseModel& model):
|
||||
NoiseModelFactor1<Pose2>(model, j), mx_(x), my_(y) {}
|
||||
|
||||
Vector evaluateError(const Pose2& q,
|
||||
Vector evaluateError(const Pose2& q,
|
||||
boost::optional<Matrix&> H = boost::none) const
|
||||
{
|
||||
if (H) (*H) = (Matrix(2,3)<< 1.0,0.0,0.0, 0.0,1.0,0.0).finished();
|
||||
|
|
|
|||
|
|
@ -3,13 +3,13 @@ NonlinearFactorGraph graph;
|
|||
|
||||
// Add a Gaussian prior on pose x_1
|
||||
Pose2 priorMean(0.0, 0.0, 0.0);
|
||||
noiseModel::Diagonal::shared_ptr priorNoise =
|
||||
noiseModel::Diagonal::shared_ptr priorNoise =
|
||||
noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
|
||||
graph.add(PriorFactor<Pose2>(1, priorMean, priorNoise));
|
||||
|
||||
// Add two odometry factors
|
||||
Pose2 odometry(2.0, 0.0, 0.0);
|
||||
noiseModel::Diagonal::shared_ptr odometryNoise =
|
||||
noiseModel::Diagonal::shared_ptr odometryNoise =
|
||||
noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
|
||||
graph.add(BetweenFactor<Pose2>(1, 2, odometry, odometryNoise));
|
||||
graph.add(BetweenFactor<Pose2>(2, 3, odometry, odometryNoise));
|
||||
|
|
|
|||
|
|
@ -1,10 +1,10 @@
|
|||
NonlinearFactorGraph graph;
|
||||
noiseModel::Diagonal::shared_ptr priorNoise =
|
||||
noiseModel::Diagonal::shared_ptr priorNoise =
|
||||
noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
|
||||
graph.add(PriorFactor<Pose2>(1, Pose2(0, 0, 0), priorNoise));
|
||||
|
||||
// Add odometry factors
|
||||
noiseModel::Diagonal::shared_ptr model =
|
||||
noiseModel::Diagonal::shared_ptr model =
|
||||
noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
|
||||
graph.add(BetweenFactor<Pose2>(1, 2, Pose2(2, 0, 0 ), model));
|
||||
graph.add(BetweenFactor<Pose2>(2, 3, Pose2(2, 0, M_PI_2), model));
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* 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)
|
||||
|
|
|
|||
|
|
@ -0,0 +1,20 @@
|
|||
NAME HS21
|
||||
ROWS
|
||||
N OBJ.FUNC
|
||||
G R------1
|
||||
COLUMNS
|
||||
C------1 R------1 0.100000e+02
|
||||
C------2 R------1 -.100000e+01
|
||||
RHS
|
||||
RHS OBJ.FUNC 0.100000e+03
|
||||
RHS R------1 0.100000e+02
|
||||
RANGES
|
||||
BOUNDS
|
||||
LO BOUNDS C------1 0.200000e+01
|
||||
UP BOUNDS C------1 0.500000e+02
|
||||
LO BOUNDS C------2 -.500000e+02
|
||||
UP BOUNDS C------2 0.500000e+02
|
||||
QUADOBJ
|
||||
C------1 C------1 0.200000e-01
|
||||
C------2 C------2 0.200000e+01
|
||||
ENDATA
|
||||
|
|
@ -0,0 +1,55 @@
|
|||
NAME HS268
|
||||
ROWS
|
||||
N OBJ.FUNC
|
||||
G R------1
|
||||
G R------2
|
||||
G R------3
|
||||
G R------4
|
||||
G R------5
|
||||
COLUMNS
|
||||
C------1 OBJ.FUNC 0.183400e+05 R------1 -.100000e+01
|
||||
C------1 R------2 0.100000e+02 R------3 -.800000e+01
|
||||
C------1 R------4 0.800000e+01 R------5 -.400000e+01
|
||||
C------2 OBJ.FUNC -.341980e+05 R------1 -.100000e+01
|
||||
C------2 R------2 0.100000e+02 R------3 0.100000e+01
|
||||
C------2 R------4 -.100000e+01 R------5 -.200000e+01
|
||||
C------3 OBJ.FUNC 0.454200e+04 R------1 -.100000e+01
|
||||
C------3 R------2 -.300000e+01 R------3 -.200000e+01
|
||||
C------3 R------4 0.200000e+01 R------5 0.300000e+01
|
||||
C------4 OBJ.FUNC 0.867200e+04 R------1 -.100000e+01
|
||||
C------4 R------2 0.500000e+01 R------3 -.500000e+01
|
||||
C------4 R------4 0.500000e+01 R------5 -.500000e+01
|
||||
C------5 OBJ.FUNC 0.860000e+02 R------1 -.100000e+01
|
||||
C------5 R------2 0.400000e+01 R------3 0.300000e+01
|
||||
C------5 R------4 -.300000e+01 R------5 0.100000e+01
|
||||
RHS
|
||||
RHS OBJ.FUNC -.144630e+05
|
||||
RHS R------1 -.500000e+01
|
||||
RHS R------2 0.200000e+02
|
||||
RHS R------3 -.400000e+02
|
||||
RHS R------4 0.110000e+02
|
||||
RHS R------5 -.300000e+02
|
||||
RANGES
|
||||
BOUNDS
|
||||
FR BOUNDS C------1
|
||||
FR BOUNDS C------2
|
||||
FR BOUNDS C------3
|
||||
FR BOUNDS C------4
|
||||
FR BOUNDS C------5
|
||||
QUADOBJ
|
||||
C------1 C------1 0.203940e+05
|
||||
C------1 C------2 -.249080e+05
|
||||
C------1 C------3 -.202600e+04
|
||||
C------1 C------4 0.389600e+04
|
||||
C------1 C------5 0.658000e+03
|
||||
C------2 C------2 0.418180e+05
|
||||
C------2 C------3 -.346600e+04
|
||||
C------2 C------4 -.982800e+04
|
||||
C------2 C------5 -.372000e+03
|
||||
C------3 C------3 0.351000e+04
|
||||
C------3 C------4 0.217800e+04
|
||||
C------3 C------5 -.348000e+03
|
||||
C------4 C------4 0.303000e+04
|
||||
C------4 C------5 -.440000e+02
|
||||
C------5 C------5 0.540000e+02
|
||||
ENDATA
|
||||
|
|
@ -0,0 +1,20 @@
|
|||
NAME HS35
|
||||
ROWS
|
||||
N OBJ.FUNC
|
||||
G R------1
|
||||
COLUMNS
|
||||
C------1 OBJ.FUNC -.800000e+01 R------1 -.100000e+01
|
||||
C------2 OBJ.FUNC -.600000e+01 R------1 -.100000e+01
|
||||
C------3 OBJ.FUNC -.400000e+01 R------1 -.200000e+01
|
||||
RHS
|
||||
RHS OBJ.FUNC -.900000e+01
|
||||
RHS R------1 -.300000e+01
|
||||
RANGES
|
||||
BOUNDS
|
||||
QUADOBJ
|
||||
C------1 C------1 0.400000e+01
|
||||
C------1 C------2 0.200000e+01
|
||||
C------1 C------3 0.200000e+01
|
||||
C------2 C------2 0.400000e+01
|
||||
C------3 C------3 0.200000e+01
|
||||
ENDATA
|
||||
|
|
@ -0,0 +1,21 @@
|
|||
NAME HS35MOD
|
||||
ROWS
|
||||
N OBJ.FUNC
|
||||
G R------1
|
||||
COLUMNS
|
||||
C------1 OBJ.FUNC -.800000e+01 R------1 -.100000e+01
|
||||
C------2 OBJ.FUNC -.600000e+01 R------1 -.100000e+01
|
||||
C------3 OBJ.FUNC -.400000e+01 R------1 -.200000e+01
|
||||
RHS
|
||||
RHS OBJ.FUNC -.900000e+01
|
||||
RHS R------1 -.300000e+01
|
||||
RANGES
|
||||
BOUNDS
|
||||
FX BOUNDS C------2 0.500000e+00
|
||||
QUADOBJ
|
||||
C------1 C------1 0.400000e+01
|
||||
C------1 C------2 0.200000e+01
|
||||
C------1 C------3 0.200000e+01
|
||||
C------2 C------2 0.400000e+01
|
||||
C------3 C------3 0.200000e+01
|
||||
ENDATA
|
||||
|
|
@ -0,0 +1,33 @@
|
|||
NAME HS51
|
||||
ROWS
|
||||
N OBJ.FUNC
|
||||
E R------1
|
||||
E R------2
|
||||
E R------3
|
||||
COLUMNS
|
||||
C------1 R------1 0.100000e+01
|
||||
C------2 OBJ.FUNC -.400000e+01 R------1 0.300000e+01
|
||||
C------2 R------3 0.100000e+01
|
||||
C------3 OBJ.FUNC -.400000e+01 R------2 0.100000e+01
|
||||
C------4 OBJ.FUNC -.200000e+01 R------2 0.100000e+01
|
||||
C------5 OBJ.FUNC -.200000e+01 R------2 -.200000e+01
|
||||
C------5 R------3 -.100000e+01
|
||||
RHS
|
||||
RHS OBJ.FUNC -.600000e+01
|
||||
RHS R------1 0.400000e+01
|
||||
RANGES
|
||||
BOUNDS
|
||||
FR BOUNDS C------1
|
||||
FR BOUNDS C------2
|
||||
FR BOUNDS C------3
|
||||
FR BOUNDS C------4
|
||||
FR BOUNDS C------5
|
||||
QUADOBJ
|
||||
C------1 C------1 0.200000e+01
|
||||
C------1 C------2 -.200000e+01
|
||||
C------2 C------2 0.400000e+01
|
||||
C------2 C------3 0.200000e+01
|
||||
C------3 C------3 0.200000e+01
|
||||
C------4 C------4 0.200000e+01
|
||||
C------5 C------5 0.200000e+01
|
||||
ENDATA
|
||||
|
|
@ -0,0 +1,32 @@
|
|||
NAME HS52
|
||||
ROWS
|
||||
N OBJ.FUNC
|
||||
E R------1
|
||||
E R------2
|
||||
E R------3
|
||||
COLUMNS
|
||||
C------1 R------1 0.100000e+01
|
||||
C------2 OBJ.FUNC -.400000e+01 R------1 0.300000e+01
|
||||
C------2 R------3 0.100000e+01
|
||||
C------3 OBJ.FUNC -.400000e+01 R------2 0.100000e+01
|
||||
C------4 OBJ.FUNC -.200000e+01 R------2 0.100000e+01
|
||||
C------5 OBJ.FUNC -.200000e+01 R------2 -.200000e+01
|
||||
C------5 R------3 -.100000e+01
|
||||
RHS
|
||||
RHS OBJ.FUNC -.600000e+01
|
||||
RANGES
|
||||
BOUNDS
|
||||
FR BOUNDS C------1
|
||||
FR BOUNDS C------2
|
||||
FR BOUNDS C------3
|
||||
FR BOUNDS C------4
|
||||
FR BOUNDS C------5
|
||||
QUADOBJ
|
||||
C------1 C------1 0.320000e+02
|
||||
C------1 C------2 -.800000e+01
|
||||
C------2 C------2 0.400000e+01
|
||||
C------2 C------3 0.200000e+01
|
||||
C------3 C------3 0.200000e+01
|
||||
C------4 C------4 0.200000e+01
|
||||
C------5 C------5 0.200000e+01
|
||||
ENDATA
|
||||
|
|
@ -0,0 +1,19 @@
|
|||
NAME QP example
|
||||
ROWS
|
||||
N obj
|
||||
G r1
|
||||
L r2
|
||||
COLUMNS
|
||||
c1 r1 2.0 r2 -1.0
|
||||
c1 obj 1.5
|
||||
c2 r1 1.0 r2 2.0
|
||||
c2 obj -2.0
|
||||
RHS
|
||||
rhs1 r1 2.0 r2 6.0
|
||||
BOUNDS
|
||||
UP bnd1 c1 20.0
|
||||
QUADOBJ
|
||||
c1 c1 8.0
|
||||
c1 c2 2.0
|
||||
c2 c2 10.0
|
||||
ENDATA
|
||||
File diff suppressed because it is too large
Load Diff
|
|
@ -0,0 +1,169 @@
|
|||
<?xml version="1.0" encoding="UTF-8" standalone="yes" ?>
|
||||
<!DOCTYPE boost_serialization>
|
||||
<boost_serialization signature="serialization::archive" version="17">
|
||||
<graph class_id="0" tracking_level="0" version="0">
|
||||
<Base class_id="1" tracking_level="0" version="0">
|
||||
<factors_ class_id="2" tracking_level="0" version="0">
|
||||
<count>2</count>
|
||||
<item_version>1</item_version>
|
||||
<item class_id="3" tracking_level="0" version="1">
|
||||
<px class_id="4" class_name="JacobianFactor" tracking_level="1" version="0" object_id="_0">
|
||||
<Base class_id="5" tracking_level="0" version="0">
|
||||
<Base class_id="6" tracking_level="0" version="0">
|
||||
<keys_>
|
||||
<count>2</count>
|
||||
<item_version>0</item_version>
|
||||
<item>0</item>
|
||||
<item>1</item>
|
||||
</keys_>
|
||||
</Base>
|
||||
</Base>
|
||||
<Ab_ class_id="8" tracking_level="0" version="0">
|
||||
<matrix_ class_id="9" tracking_level="0" version="0">
|
||||
<rows>9</rows>
|
||||
<cols>7</cols>
|
||||
<data>
|
||||
<item>1.22427709071730959e+01</item>
|
||||
<item>1.51514613104920048e+01</item>
|
||||
<item>3.60934366857813060e+00</item>
|
||||
<item>-1.18407259026383116e+01</item>
|
||||
<item>7.84826117220921216e+00</item>
|
||||
<item>1.23509165494819051e+01</item>
|
||||
<item>-6.09875015991639735e+00</item>
|
||||
<item>6.16547190708139126e-01</item>
|
||||
<item>3.94972084922329048e+00</item>
|
||||
<item>-4.89208482920378174e+00</item>
|
||||
<item>3.02091647632478866e+00</item>
|
||||
<item>-8.95328692238917512e+00</item>
|
||||
<item>7.89831607220345955e+00</item>
|
||||
<item>-2.36793602009719084e+00</item>
|
||||
<item>1.48517612051941725e+01</item>
|
||||
<item>-3.97284286249233731e-01</item>
|
||||
<item>-1.95744531643153863e+01</item>
|
||||
<item>-3.85954855417462017e+00</item>
|
||||
<item>4.79268277145419042e+00</item>
|
||||
<item>-9.01707953629520453e+00</item>
|
||||
<item>1.37848069005841385e+01</item>
|
||||
<item>1.04829326688375950e+01</item>
|
||||
<item>-5.00630568442241675e+00</item>
|
||||
<item>4.70463561852773182e+00</item>
|
||||
<item>-1.59179134598689274e+01</item>
|
||||
<item>-2.04767784956723942e+00</item>
|
||||
<item>9.54135497908261954e+00</item>
|
||||
<item>0.00000000000000000e+00</item>
|
||||
<item>0.00000000000000000e+00</item>
|
||||
<item>-0.00000000000000000e+00</item>
|
||||
<item>-1.76201757312015372e+00</item>
|
||||
<item>1.98634190821282672e+01</item>
|
||||
<item>1.52966546661624236e+00</item>
|
||||
<item>1.94817649567575373e+01</item>
|
||||
<item>1.39684693294110307e+00</item>
|
||||
<item>4.30228460420588288e+00</item>
|
||||
<item>1.76201757312015372e+00</item>
|
||||
<item>-1.98634190821282672e+01</item>
|
||||
<item>-1.52966546661624236e+00</item>
|
||||
<item>-0.00000000000000000e+00</item>
|
||||
<item>0.00000000000000000e+00</item>
|
||||
<item>0.00000000000000000e+00</item>
|
||||
<item>4.16606867942304948e+00</item>
|
||||
<item>1.86906420801308037e+00</item>
|
||||
<item>-1.94717865319198360e+01</item>
|
||||
<item>-1.94817649567575373e+01</item>
|
||||
<item>-1.39684693294110307e+00</item>
|
||||
<item>-4.30228460420588288e+00</item>
|
||||
<item>-4.16606867942304948e+00</item>
|
||||
<item>-1.86906420801308037e+00</item>
|
||||
<item>1.94717865319198360e+01</item>
|
||||
<item>0.00000000000000000e+00</item>
|
||||
<item>0.00000000000000000e+00</item>
|
||||
<item>0.00000000000000000e+00</item>
|
||||
<item>1.00891462904330531e+01</item>
|
||||
<item>-1.08132497987816869e+01</item>
|
||||
<item>8.66487736568128497e+00</item>
|
||||
<item>2.88370015604634311e+01</item>
|
||||
<item>1.89391698948574643e+01</item>
|
||||
<item>2.12398960190661290e+00</item>
|
||||
<item>1.22150946112124039e+01</item>
|
||||
<item>-2.33658532501596561e+01</item>
|
||||
<item>1.51576204760307363e+01</item>
|
||||
</data>
|
||||
</matrix_>
|
||||
<variableColOffsets_>
|
||||
<count>4</count>
|
||||
<item_version>0</item_version>
|
||||
<item>0</item>
|
||||
<item>3</item>
|
||||
<item>6</item>
|
||||
<item>7</item>
|
||||
</variableColOffsets_>
|
||||
<rowStart_>0</rowStart_>
|
||||
<rowEnd_>9</rowEnd_>
|
||||
<blockStart_>0</blockStart_>
|
||||
</Ab_>
|
||||
<model_ class_id="11" tracking_level="0" version="1">
|
||||
<px class_id="-1"></px>
|
||||
</model_>
|
||||
</px>
|
||||
</item>
|
||||
<item>
|
||||
<px class_id_reference="4" object_id="_1">
|
||||
<Base>
|
||||
<Base>
|
||||
<keys_>
|
||||
<count>2</count>
|
||||
<item_version>0</item_version>
|
||||
<item>0</item>
|
||||
<item>1</item>
|
||||
</keys_>
|
||||
</Base>
|
||||
</Base>
|
||||
<Ab_>
|
||||
<matrix_>
|
||||
<rows>3</rows>
|
||||
<cols>7</cols>
|
||||
<data>
|
||||
<item>1.00000000000000000e+00</item>
|
||||
<item>0.00000000000000000e+00</item>
|
||||
<item>0.00000000000000000e+00</item>
|
||||
<item>0.00000000000000000e+00</item>
|
||||
<item>1.00000000000000000e+00</item>
|
||||
<item>0.00000000000000000e+00</item>
|
||||
<item>0.00000000000000000e+00</item>
|
||||
<item>0.00000000000000000e+00</item>
|
||||
<item>1.00000000000000000e+00</item>
|
||||
<item>1.00000000000000000e+00</item>
|
||||
<item>0.00000000000000000e+00</item>
|
||||
<item>0.00000000000000000e+00</item>
|
||||
<item>0.00000000000000000e+00</item>
|
||||
<item>1.00000000000000000e+00</item>
|
||||
<item>0.00000000000000000e+00</item>
|
||||
<item>0.00000000000000000e+00</item>
|
||||
<item>0.00000000000000000e+00</item>
|
||||
<item>1.00000000000000000e+00</item>
|
||||
<item>0.00000000000000000e+00</item>
|
||||
<item>0.00000000000000000e+00</item>
|
||||
<item>0.00000000000000000e+00</item>
|
||||
</data>
|
||||
</matrix_>
|
||||
<variableColOffsets_>
|
||||
<count>4</count>
|
||||
<item_version>0</item_version>
|
||||
<item>0</item>
|
||||
<item>3</item>
|
||||
<item>6</item>
|
||||
<item>7</item>
|
||||
</variableColOffsets_>
|
||||
<rowStart_>0</rowStart_>
|
||||
<rowEnd_>3</rowEnd_>
|
||||
<blockStart_>0</blockStart_>
|
||||
</Ab_>
|
||||
<model_>
|
||||
<px class_id="-1"></px>
|
||||
</model_>
|
||||
</px>
|
||||
</item>
|
||||
</factors_>
|
||||
</Base>
|
||||
</graph>
|
||||
</boost_serialization>
|
||||
|
||||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* 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)
|
||||
|
|
|
|||
|
|
@ -91,7 +91,7 @@ int main(int argc, char* argv[])
|
|||
cout << "initial state:\n" << initial_state.transpose() << "\n\n";
|
||||
|
||||
// Assemble initial quaternion through gtsam constructor ::quaternion(w,x,y,z);
|
||||
Rot3 prior_rotation = Rot3::Quaternion(initial_state(6), initial_state(3),
|
||||
Rot3 prior_rotation = Rot3::Quaternion(initial_state(6), initial_state(3),
|
||||
initial_state(4), initial_state(5));
|
||||
Point3 prior_point(initial_state.head<3>());
|
||||
Pose3 prior_pose(prior_rotation, prior_point);
|
||||
|
|
@ -102,7 +102,7 @@ int main(int argc, char* argv[])
|
|||
int correction_count = 0;
|
||||
initial_values.insert(X(correction_count), prior_pose);
|
||||
initial_values.insert(V(correction_count), prior_velocity);
|
||||
initial_values.insert(B(correction_count), prior_imu_bias);
|
||||
initial_values.insert(B(correction_count), prior_imu_bias);
|
||||
|
||||
// Assemble prior noise model and add it the graph.
|
||||
noiseModel::Diagonal::shared_ptr pose_noise_model = noiseModel::Diagonal::Sigmas((Vector(6) << 0.01, 0.01, 0.01, 0.5, 0.5, 0.5).finished()); // rad,rad,rad,m, m, m
|
||||
|
|
@ -138,7 +138,7 @@ int main(int argc, char* argv[])
|
|||
p->biasAccCovariance = bias_acc_cov; // acc bias in continuous
|
||||
p->biasOmegaCovariance = bias_omega_cov; // gyro bias in continuous
|
||||
p->biasAccOmegaInt = bias_acc_omega_int;
|
||||
|
||||
|
||||
#ifdef USE_COMBINED
|
||||
imu_preintegrated_ = new PreintegratedCombinedMeasurements(p, prior_imu_bias);
|
||||
#else
|
||||
|
|
@ -154,7 +154,7 @@ int main(int argc, char* argv[])
|
|||
double current_position_error = 0.0, current_orientation_error = 0.0;
|
||||
|
||||
double output_time = 0.0;
|
||||
double dt = 0.005; // The real system has noise, but here, results are nearly
|
||||
double dt = 0.005; // The real system has noise, but here, results are nearly
|
||||
// exactly the same, so keeping this for simplicity.
|
||||
|
||||
// All priors have been set up, now iterate through the data file.
|
||||
|
|
@ -203,8 +203,8 @@ int main(int argc, char* argv[])
|
|||
*preint_imu);
|
||||
graph->add(imu_factor);
|
||||
imuBias::ConstantBias zero_bias(Vector3(0, 0, 0), Vector3(0, 0, 0));
|
||||
graph->add(BetweenFactor<imuBias::ConstantBias>(B(correction_count-1),
|
||||
B(correction_count ),
|
||||
graph->add(BetweenFactor<imuBias::ConstantBias>(B(correction_count-1),
|
||||
B(correction_count ),
|
||||
zero_bias, bias_noise_model));
|
||||
#endif
|
||||
|
||||
|
|
@ -215,7 +215,7 @@ int main(int argc, char* argv[])
|
|||
gps(2)), // D,
|
||||
correction_noise);
|
||||
graph->add(gps_factor);
|
||||
|
||||
|
||||
// Now optimize and compare results.
|
||||
prop_state = imu_preintegrated_->predict(prev_state, prev_bias);
|
||||
initial_values.insert(X(correction_count), prop_state.pose());
|
||||
|
|
@ -250,10 +250,10 @@ int main(int argc, char* argv[])
|
|||
// display statistics
|
||||
cout << "Position error:" << current_position_error << "\t " << "Angular error:" << current_orientation_error << "\n";
|
||||
|
||||
fprintf(fp_out, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n",
|
||||
fprintf(fp_out, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n",
|
||||
output_time, gtsam_position(0), gtsam_position(1), gtsam_position(2),
|
||||
gtsam_quat.x(), gtsam_quat.y(), gtsam_quat.z(), gtsam_quat.w(),
|
||||
gps(0), gps(1), gps(2),
|
||||
gps(0), gps(1), gps(2),
|
||||
gps_quat.x(), gps_quat.y(), gps_quat.z(), gps_quat.w());
|
||||
|
||||
output_time += 1.0;
|
||||
|
|
|
|||
|
|
@ -0,0 +1,92 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file InverseKinematicsExampleExpressions.cpp
|
||||
* @brief Implement inverse kinematics on a three-link arm using expressions.
|
||||
* @date April 15, 2019
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#include <gtsam/geometry/Pose2.h>
|
||||
#include <gtsam/nonlinear/ExpressionFactorGraph.h>
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
#include <gtsam/nonlinear/Marginals.h>
|
||||
#include <gtsam/nonlinear/expressions.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/slam/PriorFactor.h>
|
||||
#include <gtsam/slam/expressions.h>
|
||||
|
||||
#include <cmath>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
// Scalar multiplication of a vector, with derivatives.
|
||||
inline Vector3 scalarMultiply(const double& s, const Vector3& v,
|
||||
OptionalJacobian<3, 1> Hs,
|
||||
OptionalJacobian<3, 3> Hv) {
|
||||
if (Hs) *Hs = v;
|
||||
if (Hv) *Hv = s * I_3x3;
|
||||
return s * v;
|
||||
}
|
||||
|
||||
// Expression version of scalar product, using above function.
|
||||
inline Vector3_ operator*(const Double_& s, const Vector3_& v) {
|
||||
return Vector3_(&scalarMultiply, s, v);
|
||||
}
|
||||
|
||||
// Expression version of Pose2::Expmap
|
||||
inline Pose2_ Expmap(const Vector3_& xi) { return Pose2_(&Pose2::Expmap, xi); }
|
||||
|
||||
// Main function
|
||||
int main(int argc, char** argv) {
|
||||
// Three-link planar manipulator specification.
|
||||
const double L1 = 3.5, L2 = 3.5, L3 = 2.5; // link lengths
|
||||
const Pose2 sXt0(0, L1 + L2 + L3, M_PI / 2); // end-effector pose at rest
|
||||
const Vector3 xi1(0, 0, 1), xi2(L1, 0, 1),
|
||||
xi3(L1 + L2, 0, 1); // screw axes at rest
|
||||
|
||||
// Create Expressions for unknowns
|
||||
using symbol_shorthand::Q;
|
||||
Double_ q1(Q(1)), q2(Q(2)), q3(Q(3));
|
||||
|
||||
// Forward kinematics expression as product of exponentials
|
||||
Pose2_ l1Zl1 = Expmap(q1 * Vector3_(xi1));
|
||||
Pose2_ l2Zl2 = Expmap(q2 * Vector3_(xi2));
|
||||
Pose2_ l3Zl3 = Expmap(q3 * Vector3_(xi3));
|
||||
Pose2_ forward = compose(compose(l1Zl1, l2Zl2), compose(l3Zl3, Pose2_(sXt0)));
|
||||
|
||||
// Create a factor graph with a a single expression factor.
|
||||
ExpressionFactorGraph graph;
|
||||
Pose2 desiredEndEffectorPose(3, 2, 0);
|
||||
auto model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
|
||||
graph.addExpressionFactor(forward, desiredEndEffectorPose, model);
|
||||
|
||||
// Create initial estimate
|
||||
Values initial;
|
||||
initial.insert(Q(1), 0.1);
|
||||
initial.insert(Q(2), 0.2);
|
||||
initial.insert(Q(3), 0.3);
|
||||
initial.print("\nInitial Estimate:\n"); // print
|
||||
GTSAM_PRINT(forward.value(initial));
|
||||
|
||||
// Optimize the initial values using a Levenberg-Marquardt nonlinear optimizer
|
||||
LevenbergMarquardtParams params;
|
||||
params.setlambdaInitial(1e6);
|
||||
LevenbergMarquardtOptimizer optimizer(graph, initial, params);
|
||||
Values result = optimizer.optimize();
|
||||
result.print("Final Result:\n");
|
||||
|
||||
GTSAM_PRINT(forward.value(result));
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* 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)
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* 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)
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* 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)
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* 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)
|
||||
|
|
|
|||
|
|
@ -34,47 +34,47 @@ int main(int argc, char* argv[]) {
|
|||
// Concatenate poses to create trajectory
|
||||
poses.insert( poses.end(), poses2.begin(), poses2.end() );
|
||||
poses.insert( poses.end(), poses3.begin(), poses3.end() ); // std::vector of Pose3
|
||||
auto points = createPoints(); // std::vector of Point3
|
||||
auto points = createPoints(); // std::vector of Point3
|
||||
|
||||
// (ground-truth) sensor pose in body frame, further an unknown variable
|
||||
Pose3 body_T_sensor_gt(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
|
||||
|
||||
|
||||
// The graph
|
||||
ExpressionFactorGraph graph;
|
||||
|
||||
|
||||
// Specify uncertainty on first pose prior and also for between factor (simplicity reasons)
|
||||
auto poseNoise = noiseModel::Diagonal::Sigmas((Vector(6)<<0.3,0.3,0.3,0.1,0.1,0.1).finished());
|
||||
|
||||
|
||||
// Uncertainty bearing range measurement;
|
||||
auto bearingRangeNoise = noiseModel::Diagonal::Sigmas((Vector(3)<<0.01,0.03,0.05).finished());
|
||||
|
||||
|
||||
// Expressions for body-frame at key 0 and sensor-tf
|
||||
Pose3_ x_('x', 0);
|
||||
Pose3_ body_T_sensor_('T', 0);
|
||||
|
||||
|
||||
// Add a prior on the body-pose
|
||||
graph.addExpressionFactor(x_, poses[0], poseNoise);
|
||||
|
||||
graph.addExpressionFactor(x_, poses[0], poseNoise);
|
||||
|
||||
// Simulated measurements from pose
|
||||
for (size_t i = 0; i < poses.size(); ++i) {
|
||||
auto world_T_sensor = poses[i].compose(body_T_sensor_gt);
|
||||
for (size_t j = 0; j < points.size(); ++j) {
|
||||
|
||||
|
||||
// This expression is the key feature of this example: it creates a differentiable expression of the measurement after being displaced by sensor transform.
|
||||
auto prediction_ = Expression<BearingRange3D>( BearingRange3D::Measure, Pose3_('x',i)*body_T_sensor_, Point3_('l',j));
|
||||
|
||||
|
||||
// Create a *perfect* measurement
|
||||
auto measurement = BearingRange3D(world_T_sensor.bearing(points[j]), world_T_sensor.range(points[j]));
|
||||
|
||||
|
||||
// Add factor
|
||||
graph.addExpressionFactor(prediction_, measurement, bearingRangeNoise);
|
||||
}
|
||||
|
||||
|
||||
// and add a between factor to the graph
|
||||
if (i > 0)
|
||||
{
|
||||
// And also we have a *perfect* measurement for the between factor.
|
||||
graph.addExpressionFactor(between(Pose3_('x', i-1),Pose3_('x', i)), poses[i-1].between(poses[i]), poseNoise);
|
||||
graph.addExpressionFactor(between(Pose3_('x', i-1),Pose3_('x', i)), poses[i-1].between(poses[i]), poseNoise);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -82,12 +82,12 @@ int main(int argc, char* argv[]) {
|
|||
Values initial;
|
||||
Pose3 delta(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
|
||||
for (size_t i = 0; i < poses.size(); ++i)
|
||||
initial.insert(Symbol('x', i), poses[i].compose(delta));
|
||||
initial.insert(Symbol('x', i), poses[i].compose(delta));
|
||||
for (size_t j = 0; j < points.size(); ++j)
|
||||
initial.insert<Point3>(Symbol('l', j), points[j] + Point3(-0.25, 0.20, 0.15));
|
||||
|
||||
|
||||
// Initialize body_T_sensor wrongly (because we do not know!)
|
||||
initial.insert<Pose3>(Symbol('T',0), Pose3());
|
||||
initial.insert<Pose3>(Symbol('T',0), Pose3());
|
||||
|
||||
std::cout << "initial error: " << graph.error(initial) << std::endl;
|
||||
Values result = LevenbergMarquardtOptimizer(graph, initial).optimize();
|
||||
|
|
|
|||
|
|
@ -79,7 +79,7 @@ int main(const int argc, const char *argv[]) {
|
|||
key2 = pose3Between->key2() - firstKey;
|
||||
}
|
||||
NonlinearFactor::shared_ptr simpleFactor(
|
||||
new BetweenFactor<Pose3>(key1, key2, pose3Between->measured(), pose3Between->get_noiseModel()));
|
||||
new BetweenFactor<Pose3>(key1, key2, pose3Between->measured(), pose3Between->noiseModel()));
|
||||
simpleGraph.add(simpleFactor);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -72,7 +72,7 @@ int main(int argc, char* argv[]) {
|
|||
Point2 measurement = camera.project(points[j]);
|
||||
// Below an expression for the prediction of the measurement:
|
||||
Point3_ p('l', j);
|
||||
Point2_ prediction = uncalibrate(cK, project(transform_to(x, p)));
|
||||
Point2_ prediction = uncalibrate(cK, project(transformTo(x, p)));
|
||||
// Again, here we use an ExpressionFactor
|
||||
graph.addExpressionFactor(prediction, measurement, measurementNoise);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -52,9 +52,9 @@ std::vector<gtsam::Point3> createPoints() {
|
|||
/* ************************************************************************* */
|
||||
std::vector<gtsam::Pose3> createPoses(
|
||||
const gtsam::Pose3& init = gtsam::Pose3(gtsam::Rot3::Ypr(M_PI/2,0,-M_PI/2), gtsam::Point3(30, 0, 0)),
|
||||
const gtsam::Pose3& delta = gtsam::Pose3(gtsam::Rot3::Ypr(0,-M_PI/4,0), gtsam::Point3(sin(M_PI/4)*30, 0, 30*(1-sin(M_PI/4)))),
|
||||
const gtsam::Pose3& delta = gtsam::Pose3(gtsam::Rot3::Ypr(0,-M_PI/4,0), gtsam::Point3(sin(M_PI/4)*30, 0, 30*(1-sin(M_PI/4)))),
|
||||
int steps = 8) {
|
||||
|
||||
|
||||
// Create the set of ground-truth poses
|
||||
// Default values give a circular trajectory, radius 30 at pi/4 intervals, always facing the circle center
|
||||
std::vector<gtsam::Pose3> poses;
|
||||
|
|
@ -63,6 +63,6 @@ std::vector<gtsam::Pose3> createPoses(
|
|||
for(; i < steps; ++i) {
|
||||
poses.push_back(poses[i-1].compose(delta));
|
||||
}
|
||||
|
||||
|
||||
return poses;
|
||||
}
|
||||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* 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)
|
||||
|
|
|
|||
|
|
@ -361,8 +361,8 @@ void runIncremental()
|
|||
const auto& measured = factor->measured();
|
||||
Rot2 measuredBearing = measured.bearing();
|
||||
double measuredRange = measured.range();
|
||||
newVariables.insert(lmKey,
|
||||
pose.transform_from(measuredBearing.rotate(Point2(measuredRange, 0.0))));
|
||||
newVariables.insert(lmKey,
|
||||
pose.transformFrom(measuredBearing.rotate(Point2(measuredRange, 0.0))));
|
||||
}
|
||||
}
|
||||
else
|
||||
|
|
|
|||
|
|
@ -22,7 +22,7 @@
|
|||
* -moves forward 1 meter
|
||||
* -takes stereo readings on three landmarks
|
||||
*/
|
||||
|
||||
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/geometry/Cal3_S2Stereo.h>
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
|
|
@ -40,7 +40,7 @@ int main(int argc, char** argv){
|
|||
NonlinearFactorGraph graph;
|
||||
Pose3 first_pose;
|
||||
graph.emplace_shared<NonlinearEquality<Pose3> >(1, Pose3());
|
||||
|
||||
|
||||
//create factor noise model with 3 sigmas of value 1
|
||||
const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,1);
|
||||
//create stereo camera calibration object with .2m between cameras
|
||||
|
|
|
|||
|
|
@ -50,7 +50,7 @@ int main(int argc, char** argv){
|
|||
string calibration_loc = findExampleDataFile("VO_calibration.txt");
|
||||
string pose_loc = findExampleDataFile("VO_camera_poses_large.txt");
|
||||
string factor_loc = findExampleDataFile("VO_stereo_factors_large.txt");
|
||||
|
||||
|
||||
//read camera calibration info from file
|
||||
// focal lengths fx, fy, skew s, principal point u0, v0, baseline b
|
||||
double fx, fy, s, u0, v0, b;
|
||||
|
|
@ -60,7 +60,7 @@ int main(int argc, char** argv){
|
|||
|
||||
//create stereo camera calibration object
|
||||
const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fx,fy,s,u0,v0,b));
|
||||
|
||||
|
||||
ifstream pose_file(pose_loc.c_str());
|
||||
cout << "Reading camera poses" << endl;
|
||||
int pose_id;
|
||||
|
|
@ -72,7 +72,7 @@ int main(int argc, char** argv){
|
|||
}
|
||||
initial_estimate.insert(Symbol('x', pose_id), Pose3(m));
|
||||
}
|
||||
|
||||
|
||||
// camera and landmark keys
|
||||
size_t x, l;
|
||||
|
||||
|
|
@ -89,8 +89,8 @@ int main(int argc, char** argv){
|
|||
//if the landmark variable included in this factor has not yet been added to the initial variable value estimate, add it
|
||||
if (!initial_estimate.exists(Symbol('l', l))) {
|
||||
Pose3 camPose = initial_estimate.at<Pose3>(Symbol('x', x));
|
||||
//transform_from() transforms the input Point3 from the camera pose space, camPose, to the global space
|
||||
Point3 worldPoint = camPose.transform_from(Point3(X, Y, Z));
|
||||
//transformFrom() transforms the input Point3 from the camera pose space, camPose, to the global space
|
||||
Point3 worldPoint = camPose.transformFrom(Point3(X, Y, Z));
|
||||
initial_estimate.insert(Symbol('l', l), worldPoint);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -36,7 +36,7 @@ int main(int argc, char** argv) {
|
|||
|
||||
// Each node takes 1 of 7 possible states denoted by 0-6 in following order:
|
||||
// ["VideoGames" "Industry" "GradSchool" "VideoGames(with PhD)"
|
||||
// "Industry(with PhD)" "Academia" "Deceased"]
|
||||
// "Industry(with PhD)" "Academia" "Deceased"]
|
||||
const size_t nrStates = 7;
|
||||
|
||||
// define variables
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* 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)
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* 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)
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* 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)
|
||||
|
|
@ -140,7 +140,7 @@ int main() {
|
|||
const GaussianConditional::shared_ptr& cg0 = linearBayesNet->back();
|
||||
assert(cg0->nrFrontals() == 1);
|
||||
assert(cg0->nrParents() == 0);
|
||||
linearFactorGraph->add(0, cg0->get_R(), cg0->get_d() - cg0->get_R()*result[ordering->at(x1)], noiseModel::Diagonal::Sigmas(cg0->get_sigmas(), true));
|
||||
linearFactorGraph->add(0, cg0->R(), cg0->d() - cg0->R()*result[ordering->at(x1)], noiseModel::Diagonal::Sigmas(cg0->get_sigmas(), true));
|
||||
|
||||
// Create the desired ordering
|
||||
ordering = Ordering::shared_ptr(new Ordering);
|
||||
|
|
|
|||
73
gtsam.h
73
gtsam.h
|
|
@ -209,11 +209,60 @@ class KeyGroupMap {
|
|||
bool insert2(size_t key, int val);
|
||||
};
|
||||
|
||||
// Actually a FastSet<FactorIndex>
|
||||
class FactorIndexSet {
|
||||
FactorIndexSet();
|
||||
FactorIndexSet(const gtsam::FactorIndexSet& set);
|
||||
|
||||
// common STL methods
|
||||
size_t size() const;
|
||||
bool empty() const;
|
||||
void clear();
|
||||
|
||||
// structure specific methods
|
||||
void insert(size_t factorIndex);
|
||||
bool erase(size_t factorIndex); // returns true if value was removed
|
||||
bool count(size_t factorIndex) const; // returns true if value exists
|
||||
};
|
||||
|
||||
// Actually a vector<FactorIndex>
|
||||
class FactorIndices {
|
||||
FactorIndices();
|
||||
FactorIndices(const gtsam::FactorIndices& other);
|
||||
|
||||
// common STL methods
|
||||
size_t size() const;
|
||||
bool empty() const;
|
||||
void clear();
|
||||
|
||||
// structure specific methods
|
||||
size_t at(size_t i) const;
|
||||
size_t front() const;
|
||||
size_t back() const;
|
||||
void push_back(size_t factorIndex) const;
|
||||
};
|
||||
|
||||
//*************************************************************************
|
||||
// base
|
||||
//*************************************************************************
|
||||
|
||||
/** gtsam namespace functions */
|
||||
|
||||
#include <gtsam/base/DSFMap.h>
|
||||
class IndexPair {
|
||||
IndexPair();
|
||||
IndexPair(size_t i, size_t j);
|
||||
size_t i() const;
|
||||
size_t j() const;
|
||||
};
|
||||
|
||||
template<KEY = {gtsam::IndexPair}>
|
||||
class DSFMap {
|
||||
DSFMap();
|
||||
KEY find(const KEY& key) const;
|
||||
void merge(const KEY& x, const KEY& y);
|
||||
};
|
||||
|
||||
#include <gtsam/base/Matrix.h>
|
||||
bool linear_independent(Matrix A, Matrix B, double tol);
|
||||
|
||||
|
|
@ -583,8 +632,8 @@ class Pose2 {
|
|||
static Matrix wedge(double vx, double vy, double w);
|
||||
|
||||
// Group Actions on Point2
|
||||
gtsam::Point2 transform_from(const gtsam::Point2& p) const;
|
||||
gtsam::Point2 transform_to(const gtsam::Point2& p) const;
|
||||
gtsam::Point2 transformFrom(const gtsam::Point2& p) const;
|
||||
gtsam::Point2 transformTo(const gtsam::Point2& p) const;
|
||||
|
||||
// Standard Interface
|
||||
double x() const;
|
||||
|
|
@ -636,8 +685,8 @@ class Pose3 {
|
|||
static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz);
|
||||
|
||||
// Group Action on Point3
|
||||
gtsam::Point3 transform_from(const gtsam::Point3& point) const;
|
||||
gtsam::Point3 transform_to(const gtsam::Point3& point) const;
|
||||
gtsam::Point3 transformFrom(const gtsam::Point3& point) const;
|
||||
gtsam::Point3 transformTo(const gtsam::Point3& point) const;
|
||||
|
||||
// Standard Interface
|
||||
gtsam::Rot3 rotation() const;
|
||||
|
|
@ -646,7 +695,8 @@ class Pose3 {
|
|||
double y() const;
|
||||
double z() const;
|
||||
Matrix matrix() const;
|
||||
gtsam::Pose3 transform_to(const gtsam::Pose3& pose) const; // FIXME: shadows other transform_to()
|
||||
gtsam::Pose3 transformPoseFrom(const gtsam::Pose3& pose) const;
|
||||
gtsam::Pose3 transformPoseTo(const gtsam::Pose3& pose) const;
|
||||
double range(const gtsam::Point3& point);
|
||||
double range(const gtsam::Pose3& pose);
|
||||
|
||||
|
|
@ -1219,7 +1269,7 @@ namespace noiseModel {
|
|||
#include <gtsam/linear/NoiseModel.h>
|
||||
virtual class Base {
|
||||
void print(string s) const;
|
||||
// Methods below are available for all noise models. However, can't add them
|
||||
// Methods below are available for all noise models. However, can't add them
|
||||
// because wrap (incorrectly) thinks robust classes derive from this Base as well.
|
||||
// bool isConstrained() const;
|
||||
// bool isUnit() const;
|
||||
|
|
@ -1257,7 +1307,7 @@ virtual class Diagonal : gtsam::noiseModel::Gaussian {
|
|||
Vector sigmas() const;
|
||||
Vector invsigmas() const;
|
||||
Vector precisions() const;
|
||||
|
||||
|
||||
// enabling serialization functionality
|
||||
void serializable() const;
|
||||
};
|
||||
|
|
@ -1730,7 +1780,7 @@ virtual class SubgraphSolverParameters : gtsam::ConjugateGradientParameters {
|
|||
|
||||
virtual class SubgraphSolver {
|
||||
SubgraphSolver(const gtsam::GaussianFactorGraph &A, const gtsam::SubgraphSolverParameters ¶meters, const gtsam::Ordering& ordering);
|
||||
SubgraphSolver(const gtsam::GaussianFactorGraph &Ab1, const gtsam::GaussianFactorGraph &Ab2, const gtsam::SubgraphSolverParameters ¶meters, const gtsam::Ordering& ordering);
|
||||
SubgraphSolver(const gtsam::GaussianFactorGraph &Ab1, const gtsam::GaussianFactorGraph* Ab2, const gtsam::SubgraphSolverParameters ¶meters, const gtsam::Ordering& ordering);
|
||||
gtsam::VectorValues optimize() const;
|
||||
};
|
||||
|
||||
|
|
@ -1867,7 +1917,6 @@ virtual class NonlinearFactor {
|
|||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
virtual class NoiseModelFactor: gtsam::NonlinearFactor {
|
||||
bool equals(const gtsam::NoiseModelFactor& other, double tol) const;
|
||||
gtsam::noiseModel::Base* get_noiseModel() const; // deprecated by below
|
||||
gtsam::noiseModel::Base* noiseModel() const;
|
||||
Vector unwhitenedError(const gtsam::Values& x) const;
|
||||
Vector whitenedError(const gtsam::Values& x) const;
|
||||
|
|
@ -2053,7 +2102,7 @@ virtual class LevenbergMarquardtParams : gtsam::NonlinearOptimizerParams {
|
|||
bool getUseFixedLambdaFactor();
|
||||
string getLogFile() const;
|
||||
string getVerbosityLM() const;
|
||||
|
||||
|
||||
void setDiagonalDamping(bool flag);
|
||||
void setlambdaFactor(double value);
|
||||
void setlambdaInitial(double value);
|
||||
|
|
@ -2091,7 +2140,7 @@ virtual class NonlinearOptimizer {
|
|||
double error() const;
|
||||
int iterations() const;
|
||||
gtsam::Values values() const;
|
||||
void iterate() const;
|
||||
gtsam::GaussianFactorGraph* iterate() const;
|
||||
};
|
||||
|
||||
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
|
||||
|
|
@ -2209,8 +2258,6 @@ class ISAM2Result {
|
|||
size_t getCliques() const;
|
||||
};
|
||||
|
||||
class FactorIndices {};
|
||||
|
||||
class ISAM2 {
|
||||
ISAM2();
|
||||
ISAM2(const gtsam::ISAM2Params& params);
|
||||
|
|
|
|||
|
|
@ -101,7 +101,10 @@ message(STATUS "Building GTSAM - shared: ${BUILD_SHARED_LIBS}")
|
|||
|
||||
# BUILD_SHARED_LIBS automatically defines static/shared libs:
|
||||
add_library(gtsam ${gtsam_srcs})
|
||||
# Boost:
|
||||
target_link_libraries(gtsam PUBLIC ${GTSAM_BOOST_LIBRARIES})
|
||||
target_include_directories(gtsam PUBLIC ${Boost_INCLUDE_DIR})
|
||||
# Other deps:
|
||||
target_link_libraries(gtsam PUBLIC ${GTSAM_ADDITIONAL_LIBRARIES})
|
||||
target_compile_definitions(gtsam PRIVATE ${GTSAM_COMPILE_DEFINITIONS_PRIVATE})
|
||||
target_compile_definitions(gtsam PUBLIC ${GTSAM_COMPILE_DEFINITIONS_PUBLIC})
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* 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)
|
||||
|
|
@ -18,9 +18,9 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <cstdlib> // Provides size_t
|
||||
#include <map>
|
||||
#include <set>
|
||||
#include <cstdlib> // Provides size_t
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -29,11 +29,9 @@ namespace gtsam {
|
|||
* Uses rank compression and union by rank, iterator version
|
||||
* @addtogroup base
|
||||
*/
|
||||
template<class KEY>
|
||||
template <class KEY>
|
||||
class DSFMap {
|
||||
|
||||
protected:
|
||||
|
||||
protected:
|
||||
/// We store the forest in an STL map, but parents are done with pointers
|
||||
struct Entry {
|
||||
typename std::map<KEY, Entry>::iterator parent_;
|
||||
|
|
@ -41,7 +39,7 @@ protected:
|
|||
Entry() {}
|
||||
};
|
||||
|
||||
typedef typename std::map<KEY, Entry> Map;
|
||||
typedef typename std::map<KEY, Entry> Map;
|
||||
typedef typename Map::iterator iterator;
|
||||
mutable Map entries_;
|
||||
|
||||
|
|
@ -62,8 +60,7 @@ protected:
|
|||
iterator find_(const iterator& it) const {
|
||||
// follow parent pointers until we reach set representative
|
||||
iterator& parent = it->second.parent_;
|
||||
if (parent != it)
|
||||
parent = find_(parent); // not yet, recurse!
|
||||
if (parent != it) parent = find_(parent); // not yet, recurse!
|
||||
return parent;
|
||||
}
|
||||
|
||||
|
|
@ -73,13 +70,11 @@ protected:
|
|||
return find_(initial);
|
||||
}
|
||||
|
||||
public:
|
||||
|
||||
public:
|
||||
typedef std::set<KEY> Set;
|
||||
|
||||
/// constructor
|
||||
DSFMap() {
|
||||
}
|
||||
DSFMap() {}
|
||||
|
||||
/// Given key, find the representative key for the set in which it lives
|
||||
inline KEY find(const KEY& key) const {
|
||||
|
|
@ -89,12 +84,10 @@ public:
|
|||
|
||||
/// Merge two sets
|
||||
void merge(const KEY& x, const KEY& y) {
|
||||
|
||||
// straight from http://en.wikipedia.org/wiki/Disjoint-set_data_structure
|
||||
iterator xRoot = find_(x);
|
||||
iterator yRoot = find_(y);
|
||||
if (xRoot == yRoot)
|
||||
return;
|
||||
if (xRoot == yRoot) return;
|
||||
|
||||
// Merge sets
|
||||
if (xRoot->second.rank_ < yRoot->second.rank_)
|
||||
|
|
@ -117,7 +110,14 @@ public:
|
|||
}
|
||||
return sets;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
/// Small utility class for representing a wrappable pairs of ints.
|
||||
class IndexPair : public std::pair<size_t,size_t> {
|
||||
public:
|
||||
IndexPair(): std::pair<size_t,size_t>(0,0) {}
|
||||
IndexPair(size_t i, size_t j) : std::pair<size_t,size_t>(i,j) {}
|
||||
inline size_t i() const { return first; };
|
||||
inline size_t j() const { return second; };
|
||||
};
|
||||
} // namespace gtsam
|
||||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* 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)
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* 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)
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* 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)
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* 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)
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* 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)
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* 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)
|
||||
|
|
@ -170,7 +170,7 @@ struct FixedDimension {
|
|||
|
||||
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
|
||||
/// Helper class to construct the product manifold of two other manifolds, M1 and M2
|
||||
/// Deprecated because of limited usefulness, maximum obfuscation
|
||||
/// Deprecated because of limited usefulness, maximum obfuscation
|
||||
template<typename M1, typename M2>
|
||||
class ProductManifold: public std::pair<M1, M2> {
|
||||
BOOST_CONCEPT_ASSERT((IsManifold<M1>));
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* 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)
|
||||
|
|
@ -540,7 +540,7 @@ Matrix cholesky_inverse(const Matrix &A)
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Semantics:
|
||||
// Semantics:
|
||||
// if B = inverse_square_root(A), then all of the following are true:
|
||||
// inv(B) * inv(B)' == A
|
||||
// inv(B' * B) == A
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* 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)
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* 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)
|
||||
|
|
|
|||
|
|
@ -65,8 +65,9 @@ Matrix SymmetricBlockMatrix::block(DenseIndex I, DenseIndex J) const {
|
|||
void SymmetricBlockMatrix::choleskyPartial(DenseIndex nFrontals) {
|
||||
gttic(VerticalBlockMatrix_choleskyPartial);
|
||||
DenseIndex topleft = variableColOffsets_[blockStart_];
|
||||
if (!gtsam::choleskyPartial(matrix_, offset(nFrontals) - topleft, topleft))
|
||||
if (!gtsam::choleskyPartial(matrix_, offset(nFrontals) - topleft, topleft)) {
|
||||
throw CholeskyFailed();
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* 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)
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* 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)
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* 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)
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* 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)
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* 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)
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* 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)
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* 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)
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* 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)
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* 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)
|
||||
|
|
@ -36,7 +36,7 @@ struct GTSAM_EXPORT LieVector : public Vector {
|
|||
|
||||
/** initialize from a normal vector */
|
||||
LieVector(const Vector& v) : Vector(v) {}
|
||||
|
||||
|
||||
template <class V>
|
||||
LieVector(const V& v) : Vector(v) {}
|
||||
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* 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)
|
||||
|
|
|
|||
|
|
@ -67,28 +67,29 @@ struct FixedSizeMatrix {
|
|||
}
|
||||
|
||||
/**
|
||||
* Numerically compute gradient of scalar function
|
||||
* @brief Numerically compute gradient of scalar function
|
||||
* @return n-dimensional gradient computed via central differencing
|
||||
* Class X is the input argument
|
||||
* The class X needs to have dim, expmap, logmap
|
||||
* int N is the dimension of the X input value if variable dimension type but known at test time
|
||||
*/
|
||||
template<class X>
|
||||
typename internal::FixedSizeMatrix<X>::type numericalGradient(boost::function<double(const X&)> h, const X& x,
|
||||
double delta = 1e-5) {
|
||||
|
||||
template <class X, int N = traits<X>::dimension>
|
||||
typename Eigen::Matrix<double, N, 1> numericalGradient(
|
||||
boost::function<double(const X&)> h, const X& x, double delta = 1e-5) {
|
||||
double factor = 1.0 / (2.0 * delta);
|
||||
|
||||
BOOST_STATIC_ASSERT_MSG(
|
||||
(boost::is_base_of<manifold_tag, typename traits<X>::structure_category>::value),
|
||||
"Template argument X must be a manifold type.");
|
||||
static const int N = traits<X>::dimension;
|
||||
BOOST_STATIC_ASSERT_MSG(N>0, "Template argument X must be fixed-size type.");
|
||||
|
||||
typedef typename traits<X>::TangentVector TangentX;
|
||||
BOOST_STATIC_ASSERT_MSG(N>0, "Template argument X must be fixed-size type or N must be specified.");
|
||||
|
||||
// Prepare a tangent vector to perturb x with, only works for fixed size
|
||||
TangentX d;
|
||||
typename traits<X>::TangentVector d;
|
||||
d.setZero();
|
||||
|
||||
Eigen::Matrix<double,N,1> g; g.setZero(); // Can be fixed size
|
||||
Eigen::Matrix<double,N,1> g;
|
||||
g.setZero();
|
||||
for (int j = 0; j < N; j++) {
|
||||
d(j) = delta;
|
||||
double hxplus = h(traits<X>::Retract(x, d));
|
||||
|
|
@ -108,37 +109,34 @@ typename internal::FixedSizeMatrix<X>::type numericalGradient(boost::function<do
|
|||
* @param delta increment for numerical derivative
|
||||
* Class Y is the output argument
|
||||
* Class X is the input argument
|
||||
* int N is the dimension of the X input value if variable dimension type but known at test time
|
||||
* @return m*n Jacobian computed via central differencing
|
||||
*/
|
||||
|
||||
template<class Y, class X>
|
||||
template <class Y, class X, int N = traits<X>::dimension>
|
||||
// TODO Should compute fixed-size matrix
|
||||
typename internal::FixedSizeMatrix<Y,X>::type numericalDerivative11(boost::function<Y(const X&)> h, const X& x,
|
||||
double delta = 1e-5) {
|
||||
|
||||
typename internal::FixedSizeMatrix<Y, X>::type numericalDerivative11(
|
||||
boost::function<Y(const X&)> h, const X& x, double delta = 1e-5) {
|
||||
typedef typename internal::FixedSizeMatrix<Y,X>::type Matrix;
|
||||
|
||||
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
|
||||
"Template argument Y must be a manifold type.");
|
||||
typedef traits<Y> TraitsY;
|
||||
typedef typename TraitsY::TangentVector TangentY;
|
||||
|
||||
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X>::structure_category>::value),
|
||||
"Template argument X must be a manifold type.");
|
||||
static const int N = traits<X>::dimension;
|
||||
BOOST_STATIC_ASSERT_MSG(N>0, "Template argument X must be fixed-size type.");
|
||||
BOOST_STATIC_ASSERT_MSG(N>0, "Template argument X must be fixed-size type or N must be specified.");
|
||||
typedef traits<X> TraitsX;
|
||||
typedef typename TraitsX::TangentVector TangentX;
|
||||
|
||||
// get value at x, and corresponding chart
|
||||
const Y hx = h(x);
|
||||
|
||||
// Bit of a hack for now to find number of rows
|
||||
const TangentY zeroY = TraitsY::Local(hx, hx);
|
||||
const typename TraitsY::TangentVector zeroY = TraitsY::Local(hx, hx);
|
||||
const size_t m = zeroY.size();
|
||||
|
||||
// Prepare a tangent vector to perturb x with, only works for fixed size
|
||||
TangentX dx;
|
||||
Eigen::Matrix<double, N, 1> dx;
|
||||
dx.setZero();
|
||||
|
||||
// Fill in Jacobian H
|
||||
|
|
@ -146,9 +144,9 @@ typename internal::FixedSizeMatrix<Y,X>::type numericalDerivative11(boost::funct
|
|||
const double factor = 1.0 / (2.0 * delta);
|
||||
for (int j = 0; j < N; j++) {
|
||||
dx(j) = delta;
|
||||
const TangentY dy1 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx)));
|
||||
const auto dy1 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx)));
|
||||
dx(j) = -delta;
|
||||
const TangentY dy2 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx)));
|
||||
const auto dy2 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx)));
|
||||
dx(j) = 0;
|
||||
H.col(j) << (dy1 - dy2) * factor;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* 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)
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* 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)
|
||||
|
|
@ -16,7 +16,7 @@
|
|||
* @brief unit tests for DSFMap
|
||||
*/
|
||||
|
||||
#include <gtsam_unstable/base/DSFMap.h>
|
||||
#include <gtsam/base/DSFMap.h>
|
||||
|
||||
#include <boost/assign/std/list.hpp>
|
||||
#include <boost/assign/std/set.hpp>
|
||||
|
|
@ -115,7 +115,6 @@ TEST(DSFMap, mergePairwiseMatches2) {
|
|||
TEST(DSFMap, sets){
|
||||
// Create some "matches"
|
||||
typedef pair<size_t,size_t> Match;
|
||||
typedef pair<size_t, set<size_t> > key_pair;
|
||||
list<Match> matches;
|
||||
matches += Match(1,2), Match(2,3), Match(4,5), Match(4,6);
|
||||
|
||||
|
|
@ -128,18 +127,24 @@ TEST(DSFMap, sets){
|
|||
set<size_t> s1, s2;
|
||||
s1 += 1,2,3;
|
||||
s2 += 4,5,6;
|
||||
|
||||
|
||||
/*for(key_pair st: sets){
|
||||
cout << "Set " << st.first << " :{";
|
||||
for(const size_t s: st.second)
|
||||
cout << s << ", ";
|
||||
cout << "}" << endl;
|
||||
}*/
|
||||
|
||||
|
||||
EXPECT(s1 == sets[1]);
|
||||
EXPECT(s2 == sets[4]);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(DSFMap, findIndexPair) {
|
||||
DSFMap<IndexPair> dsf;
|
||||
EXPECT(dsf.find(IndexPair(1,2))==IndexPair(1,2));
|
||||
EXPECT(dsf.find(IndexPair(1,2)) != dsf.find(IndexPair(1,3)));
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* 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)
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* 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)
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* 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)
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* 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)
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* 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)
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* 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)
|
||||
|
|
@ -275,7 +275,7 @@ TEST(Matrix, stream_read ) {
|
|||
-0.3, -8e-2, 5.1, 9.0,
|
||||
1.2, 3.4, 4.5, 6.7).finished();
|
||||
|
||||
string matrixAsString =
|
||||
string matrixAsString =
|
||||
"1.1 2.3 4.2 7.6\n"
|
||||
"-0.3 -8e-2 5.1 9.0\n\r" // Test extra spaces and windows newlines
|
||||
"1.2 \t 3.4 4.5 6.7"; // Test tab as separator
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* 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)
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* 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)
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* 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)
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* 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)
|
||||
|
|
|
|||
|
|
@ -56,6 +56,9 @@ namespace gtsam {
|
|||
/// Integer nonlinear key type
|
||||
typedef std::uint64_t Key;
|
||||
|
||||
/// Integer nonlinear factor index type
|
||||
typedef std::uint64_t FactorIndex;
|
||||
|
||||
/// The index type for Eigen objects
|
||||
typedef ptrdiff_t DenseIndex;
|
||||
|
||||
|
|
|
|||
|
|
@ -355,7 +355,7 @@ namespace gtsam {
|
|||
NodePtr choose(const L& label, size_t index) const {
|
||||
if (label_ == label)
|
||||
return branches_[index]; // choose branch
|
||||
|
||||
|
||||
// second case, not label of interest, just recurse
|
||||
boost::shared_ptr<Choice> r(new Choice(label_, branches_.size()));
|
||||
for(const NodePtr& branch: branches_)
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* 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)
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* 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)
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* 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)
|
||||
|
|
@ -31,7 +31,7 @@ namespace gtsam {
|
|||
typedef EliminationTree<DiscreteBayesNet, DiscreteFactorGraph> Base; ///< Base class
|
||||
typedef DiscreteEliminationTree This; ///< This class
|
||||
typedef boost::shared_ptr<This> shared_ptr; ///< Shared pointer to this class
|
||||
|
||||
|
||||
/**
|
||||
* Build the elimination tree of a factor graph using pre-computed column structure.
|
||||
* @param factorGraph The factor graph for which to build the elimination tree
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* 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)
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* 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)
|
||||
|
|
@ -51,7 +51,7 @@ namespace gtsam {
|
|||
typedef JunctionTree<DiscreteBayesTree, DiscreteFactorGraph> Base; ///< Base class
|
||||
typedef DiscreteJunctionTree This; ///< This class
|
||||
typedef boost::shared_ptr<This> shared_ptr; ///< Shared pointer to this class
|
||||
|
||||
|
||||
/**
|
||||
* Build the elimination tree of a factor graph using precomputed column structure.
|
||||
* @param factorGraph The factor graph for which to build the elimination tree
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* 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)
|
||||
|
|
|
|||
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue