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

Merge pull request #2 from varunagrawal/gnc/index-vector 

GNC IndexVector
release/4.3a0
Amado Antonini 2022-07-26 23:36:02 -04:00 committed by GitHub
parent 029631e483 1d51c4e646
commit aeceb450e3
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
11 changed files with 48 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -59,7 +59,7 @@ PATH=$PATH:$($PYTHON -c "import site; print(site.USER_BASE)")/bin
BUILD_PYBIND="ON"
sudo $PYTHON -m pip install -r $GITHUB_WORKSPACE/python/requirements.txt
$PYTHON -m pip install -r $GITHUB_WORKSPACE/python/requirements.txt
mkdir $GITHUB_WORKSPACE/build
cd $GITHUB_WORKSPACE/build

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

@ -19,16 +19,16 @@ jobs:
# Github Actions requires a single row to be added to the build matrix.
# See https://help.github.com/en/articles/workflow-syntax-for-github-actions.
name: [
macOS-10.15-xcode-11.3.1,
macos-11-xcode-13.4.1,
]
build_type: [Debug, Release]
build_unstable: [ON]
include:
- name: macOS-10.15-xcode-11.3.1
os: macOS-10.15
- name: macos-11-xcode-13.4.1
os: macos-11
compiler: xcode
version: "11.3.1"
version: "13.4.1"
steps:
- name: Checkout
@ -43,7 +43,7 @@ jobs:
echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV
echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV
else
sudo xcode-select -switch /Applications/Xcode_${{ matrix.version }}.app
sudo xcode-select -switch /Applications/Xcode.app
echo "CC=clang" >> $GITHUB_ENV
echo "CXX=clang++" >> $GITHUB_ENV
fi

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

@ -22,12 +22,12 @@ jobs:
ubuntu-18.04-gcc-5,
ubuntu-18.04-gcc-9,
ubuntu-18.04-clang-9,
macOS-10.15-xcode-11.3.1,
macOS-11-xcode-13.4.1,
ubuntu-18.04-gcc-5-tbb,
]
build_type: [Debug, Release]
python_version: [3]
python_version: [3.7]
include:
- name: ubuntu-18.04-gcc-5
os: ubuntu-18.04
@ -44,18 +44,10 @@ jobs:
compiler: clang
version: "9"
# NOTE temporarily added this as it is a required check.
- name: ubuntu-18.04-clang-9
os: ubuntu-18.04
compiler: clang
version: "9"
build_type: Debug
python_version: "3"
- name: macOS-10.15-xcode-11.3.1
os: macOS-10.15
- name: macOS-11-xcode-13.4.1
os: macOS-11
compiler: xcode
version: "11.3.1"
version: "13.4.1"
- name: ubuntu-18.04-gcc-5-tbb
os: ubuntu-18.04
@ -65,7 +57,11 @@ jobs:
steps:
- name: Checkout
uses: actions/checkout@v2
uses: actions/checkout@v3
- name: Set up Python
uses: actions/setup-python@v4
with:
python-version: '3.7'
- name: Install (Linux)
if: runner.os == 'Linux'
run: |
@ -103,7 +99,7 @@ jobs:
echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV
echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV
else
sudo xcode-select -switch /Applications/Xcode_${{ matrix.version }}.app
sudo xcode-select -switch /Applications/Xcode.app
echo "CC=clang" >> $GITHUB_ENV
echo "CXX=clang++" >> $GITHUB_ENV
fi

21
gtsam/gtsam.i
View File

@ -66,27 +66,6 @@ class KeySet {
void serialize() const;
};
// Actually a vector<Key>
class KeyVector {
KeyVector();
KeyVector(const gtsam::KeyVector& other);
// Note: no print function
// 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 key) const;
void serialize() const;
};
// Actually a FastMap<Key,int>
class KeyGroupMap {
KeyGroupMap();

14
gtsam/nonlinear/GncParams.h
View File

@ -72,8 +72,14 @@ class GTSAM_EXPORT GncParams {
double relativeCostTol = 1e-5; ///< If relative cost change is below this threshold, stop iterating
double weightsTol = 1e-4; ///< If the weights are within weightsTol from being binary, stop iterating (only for TLS)
Verbosity verbosity = SILENT; ///< Verbosity level
std::vector<size_t> knownInliers = std::vector<size_t>(); ///< Slots in the factor graph corresponding to measurements that we know are inliers
std::vector<size_t> knownOutliers = std::vector<size_t>(); ///< Slots in the factor graph corresponding to measurements that we know are outliers
//TODO(Varun) replace IndexVector with vector<size_t> once pybind11/stl.h is globally enabled.
/// Use IndexVector for inliers and outliers since it is fast + wrapping
using IndexVector = FastVector<uint64_t>;
///< Slots in the factor graph corresponding to measurements that we know are inliers
IndexVector knownInliers = IndexVector();
///< Slots in the factor graph corresponding to measurements that we know are outliers
IndexVector knownOutliers = IndexVector();
/// Set the robust loss function to be used in GNC (chosen among the ones in GncLossType).
void setLossType(const GncLossType type) {
@ -114,7 +120,7 @@ class GTSAM_EXPORT GncParams {
* This functionality is commonly used in SLAM when one may assume the odometry is outlier free, and
* only apply GNC to prune outliers from the loop closures.
* */
void setKnownInliers(const std::vector<size_t>& knownIn) {
void setKnownInliers(const IndexVector& knownIn) {
for (size_t i = 0; i < knownIn.size(); i++){
knownInliers.push_back(knownIn[i]);
}
@ -125,7 +131,7 @@ class GTSAM_EXPORT GncParams {
* corresponds to the slots in the factor graph. For instance, if you have a nonlinear factor graph nfg,
* and you provide knownOut = {0, 2, 15}, GNC will not apply outlier rejection to nfg[0], nfg[2], and nfg[15].
* */
void setKnownOutliers(const std::vector<size_t>& knownOut) {
void setKnownOutliers(const IndexVector& knownOut) {
for (size_t i = 0; i < knownOut.size(); i++){
knownOutliers.push_back(knownOut[i]);
}

3
gtsam/nonlinear/NonlinearFactor.cpp
View File

@ -167,8 +167,9 @@ boost::shared_ptr<GaussianFactor> NoiseModelFactor::linearize(
return GaussianFactor::shared_ptr(
new JacobianFactor(terms, b,
boost::static_pointer_cast<Constrained>(noiseModel_)->unit()));
else
else {
return GaussianFactor::shared_ptr(new JacobianFactor(terms, b));
}
}
/* ************************************************************************* */

8
gtsam/nonlinear/nonlinear.i
View File

@ -529,8 +529,8 @@ virtual class GncParams {
double relativeCostTol;
double weightsTol;
Verbosity verbosity;
std::vector<size_t> knownInliers;
std::vector<size_t> knownOutliers;
gtsam::KeyVector knownInliers;
gtsam::KeyVector knownOutliers;
void setLossType(const gtsam::GncLossType type);
void setMaxIterations(const size_t maxIter);
@ -538,8 +538,8 @@ virtual class GncParams {
void setRelativeCostTol(double value);
void setWeightsTol(double value);
void setVerbosityGNC(const gtsam::This::Verbosity value);
void setKnownInliers(const std::vector<size_t>& knownIn);
void setKnownOutliers(const std::vector<size_t>& knownOut);
void setKnownInliers(const gtsam::KeyVector& knownIn);
void setKnownOutliers(const gtsam::KeyVector& knownOut);
void print(const string& str = "GncParams: ") const;
enum Verbosity {

3
python/CMakeLists.txt
View File

@ -105,7 +105,8 @@ copy_directory("${CMAKE_CURRENT_SOURCE_DIR}/gtsam"
# Hack to get python test and util files copied every time they are modified
file(GLOB GTSAM_PYTHON_TEST_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam/tests/*.py")
foreach(test_file ${GTSAM_PYTHON_TEST_FILES})
configure_file(${test_file} "${GTSAM_MODULE_PATH}/tests/${test_file}" COPYONLY)
get_filename_component(test_file_name ${test_file} NAME)
configure_file(${test_file} "${GTSAM_MODULE_PATH}/tests/${test_file_name}" COPYONLY)
endforeach()
file(GLOB GTSAM_PYTHON_UTIL_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam/utils/*.py")
foreach(util_file ${GTSAM_PYTHON_UTIL_FILES})

2
python/gtsam/preamble/nonlinear.h
View File

@ -9,4 +9,4 @@
* automatic STL binding, such that the raw objects can be accessed in Python.
* Without this they will be automatically converted to a Python object, and all
* mutations on Python side will not be reflected on C++.
*/
*/

2
python/gtsam/specializations/nonlinear.h
View File

@ -9,4 +9,4 @@
* interface, but without the `<pybind11/stl.h>` copying mechanism. Combined
* with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python,
* and saves one copy operation.
*/
*/

22
tests/testGncOptimizer.cpp
View File

@ -567,7 +567,7 @@ TEST(GncOptimizer, optimizeWithKnownInliers) {
Values initial;
initial.insert(X(1), p0);
std::vector<size_t> knownInliers;
GncParams<GaussNewtonParams>::IndexVector knownInliers;
knownInliers.push_back(0);
knownInliers.push_back(1);
knownInliers.push_back(2);
@ -644,7 +644,7 @@ TEST(GncOptimizer, barcsq) {
Values initial;
initial.insert(X(1), p0);
std::vector<size_t> knownInliers;
GncParams<GaussNewtonParams>::IndexVector knownInliers;
knownInliers.push_back(0);
knownInliers.push_back(1);
knownInliers.push_back(2);
@ -691,7 +691,7 @@ TEST(GncOptimizer, setInlierCostThresholds) {
Values initial;
initial.insert(X(1), p0);
std::vector<size_t> knownInliers;
GncParams<GaussNewtonParams>::IndexVector knownInliers;
knownInliers.push_back(0);
knownInliers.push_back(1);
knownInliers.push_back(2);
@ -763,7 +763,7 @@ TEST(GncOptimizer, optimizeSmallPoseGraph) {
// GNC
// Note: in difficult instances, we set the odometry measurements to be
// inliers, but this problem is simple enought to succeed even without that
// assumption std::vector<size_t> knownInliers;
// assumption GncParams<GaussNewtonParams>::IndexVector knownInliers;
GncParams<GaussNewtonParams> gncParams;
auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(*graph, *initial,
gncParams);
@ -784,12 +784,12 @@ TEST(GncOptimizer, knownInliersAndOutliers) {
// nonconvexity with known inliers and known outliers (check early stopping
// when all measurements are known to be inliers or outliers)
{
std::vector<size_t> knownInliers;
GncParams<GaussNewtonParams>::IndexVector knownInliers;
knownInliers.push_back(0);
knownInliers.push_back(1);
knownInliers.push_back(2);
std::vector<size_t> knownOutliers;
GncParams<GaussNewtonParams>::IndexVector knownOutliers;
knownOutliers.push_back(3);
GncParams<GaussNewtonParams> gncParams;
@ -813,11 +813,11 @@ TEST(GncOptimizer, knownInliersAndOutliers) {
// nonconvexity with known inliers and known outliers
{
std::vector<size_t> knownInliers;
GncParams<GaussNewtonParams>::IndexVector knownInliers;
knownInliers.push_back(2);
knownInliers.push_back(0);
std::vector<size_t> knownOutliers;
GncParams<GaussNewtonParams>::IndexVector knownOutliers;
knownOutliers.push_back(3);
GncParams<GaussNewtonParams> gncParams;
@ -841,7 +841,7 @@ TEST(GncOptimizer, knownInliersAndOutliers) {
// only known outliers
{
std::vector<size_t> knownOutliers;
GncParams<GaussNewtonParams>::IndexVector knownOutliers;
knownOutliers.push_back(3);
GncParams<GaussNewtonParams> gncParams;
@ -916,11 +916,11 @@ TEST(GncOptimizer, setWeights) {
// initialize weights and also set known inliers/outliers
{
GncParams<GaussNewtonParams> gncParams;
std::vector<size_t> knownInliers;
GncParams<GaussNewtonParams>::IndexVector knownInliers;
knownInliers.push_back(2);
knownInliers.push_back(0);
std::vector<size_t> knownOutliers;
GncParams<GaussNewtonParams>::IndexVector knownOutliers;
knownOutliers.push_back(3);
gncParams.setKnownInliers(knownInliers);
gncParams.setKnownOutliers(knownOutliers);