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

Merge branch 'develop' into feature/python-plotting

release/4.3a0
Varun Agrawal 2020-04-08 19:36:47 -04:00
parent a59682e608 5c9c4bed93
commit fec98b5cd4
83 changed files with 558 additions and 227 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
.travis.sh
View File

@ -1,5 +1,37 @@
#!/bin/bash
# install TBB with _debug.so files
function install_tbb()
{
TBB_BASEURL=https://github.com/oneapi-src/oneTBB/releases/download
TBB_VERSION=4.4.2
TBB_DIR=tbb44_20151115oss
TBB_SAVEPATH="/tmp/tbb.tgz"
if [ "$TRAVIS_OS_NAME" == "linux" ]; then
OS_SHORT="lin"
TBB_LIB_DIR="intel64/gcc4.4"
SUDO="sudo"
elif [ "$TRAVIS_OS_NAME" == "osx" ]; then
OS_SHORT="lin"
TBB_LIB_DIR=""
SUDO=""
fi
wget "${TBB_BASEURL}/${TBB_VERSION}/${TBB_DIR}_${OS_SHORT}.tgz" -O $TBB_SAVEPATH
tar -C /tmp -xf $TBB_SAVEPATH
TBBROOT=/tmp/$TBB_DIR
# Copy the needed files to the correct places.
# This works correctly for travis builds, instead of setting path variables.
# This is what Homebrew does to install TBB on Macs
$SUDO cp -R $TBBROOT/lib/$TBB_LIB_DIR/* /usr/local/lib/
$SUDO cp -R $TBBROOT/include/ /usr/local/include/
}
# common tasks before either build or test
function configure()
{
@ -14,6 +46,8 @@ function configure()
rm -fr $BUILD_DIR || true
mkdir $BUILD_DIR && cd $BUILD_DIR
install_tbb
if [ ! -z "$GCC_VERSION" ]; then
export CC=gcc-$GCC_VERSION
export CXX=g++-$GCC_VERSION
@ -24,6 +58,7 @@ function configure()
-DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE:-Debug} \
-DGTSAM_BUILD_TESTS=${GTSAM_BUILD_TESTS:-OFF} \
-DGTSAM_BUILD_UNSTABLE=${GTSAM_BUILD_UNSTABLE:-ON} \
-DGTSAM_WITH_TBB=${GTSAM_WITH_TBB:-OFF} \
-DGTSAM_USE_QUATERNIONS=${GTSAM_USE_QUATERNIONS:-OFF} \
-DGTSAM_BUILD_EXAMPLES_ALWAYS=${GTSAM_BUILD_EXAMPLES_ALWAYS:-ON} \
-DGTSAM_ALLOW_DEPRECATED_SINCE_V4=${GTSAM_ALLOW_DEPRECATED_SINCE_V4:-OFF} \

8
.travis.yml
View File

@ -18,7 +18,7 @@ addons:
- libboost-all-dev
# before_install:
# - if [ "$TRAVIS_OS_NAME" == "osx" ]; then brew update ; fi
# - if [ "$TRAVIS_OS_NAME" == "osx" ]; then brew update; fi
install:
- if [ "$TRAVIS_OS_NAME" == "osx" ]; then HOMEBREW_NO_AUTO_UPDATE=1 brew install ccache ; fi
@ -89,6 +89,12 @@ jobs:
compiler: clang
env: CC=clang-9 CXX=clang++-9 CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_ALLOW_DEPRECATED_SINCE_V4=ON
script: bash .travis.sh -b
# on Linux, with GTSAM_WITH_TBB on to make sure GTSAM still compiles/tests
- stage: special
os: linux
compiler: gcc
env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_WITH_TBB=ON
script: bash .travis.sh -b
# -------- STAGE 2: TESTS -----------
# on Mac, GCC
- stage: test

19
CMakeLists.txt
View File

@ -199,12 +199,17 @@ find_package(TBB 4.4 COMPONENTS tbb tbbmalloc)
# Set up variables if we're using TBB
if(TBB_FOUND AND GTSAM_WITH_TBB)
set(GTSAM_USE_TBB 1) # This will go into config.h
# all definitions and link requisites will go via imported targets:
# tbb & tbbmalloc
list(APPEND GTSAM_ADDITIONAL_LIBRARIES tbb tbbmalloc)
set(GTSAM_USE_TBB 1) # This will go into config.h
if ((${TBB_VERSION_MAJOR} GREATER 2020) OR (${TBB_VERSION_MAJOR} EQUAL 2020))
set(TBB_GREATER_EQUAL_2020 1)
else()
set(TBB_GREATER_EQUAL_2020 0)
endif()
# all definitions and link requisites will go via imported targets:
# tbb & tbbmalloc
list(APPEND GTSAM_ADDITIONAL_LIBRARIES tbb tbbmalloc)
else()
set(GTSAM_USE_TBB 0) # This will go into config.h
set(GTSAM_USE_TBB 0) # This will go into config.h
endif()
###############################################################################
@ -416,6 +421,10 @@ add_subdirectory(CppUnitLite)
# Build wrap
if (GTSAM_BUILD_WRAP)
add_subdirectory(wrap)
# suppress warning of cython line being too long
if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-misleading-indentation")
endif()
endif(GTSAM_BUILD_WRAP)
# Build GTSAM library

5
cython/gtsam/examples/Pose2SLAMExample_g2o.py
View File

@ -53,16 +53,15 @@ graph, initial = gtsam.readG2o(g2oFile, is3D)
assert args.kernel == "none", "Supplied kernel type is not yet implemented"
# Add prior on the pose having index (key) = 0
graphWithPrior = graph
priorModel = gtsam.noiseModel_Diagonal.Variances(vector3(1e-6, 1e-6, 1e-8))
graphWithPrior.add(gtsam.PriorFactorPose2(0, gtsam.Pose2(), priorModel))
graph.add(gtsam.PriorFactorPose2(0, gtsam.Pose2(), priorModel))
params = gtsam.GaussNewtonParams()
params.setVerbosity("Termination")
params.setMaxIterations(maxIterations)
# parameters.setRelativeErrorTol(1e-5)
# Create the optimizer ...
optimizer = gtsam.GaussNewtonOptimizer(graphWithPrior, initial, params)
optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params)
# ... and optimize
result = optimizer.optimize()

9
cython/gtsam/examples/Pose3SLAMExample_g2o.py
View File

@ -43,18 +43,17 @@ priorModel = gtsam.noiseModel_Diagonal.Variances(vector6(1e-6, 1e-6, 1e-6,
1e-4, 1e-4, 1e-4))
print("Adding prior to g2o file ")
graphWithPrior = graph
firstKey = initial.keys().at(0)
graphWithPrior.add(gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel))
graph.add(gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel))
params = gtsam.GaussNewtonParams()
params.setVerbosity("Termination") # this will show info about stopping conds
optimizer = gtsam.GaussNewtonOptimizer(graphWithPrior, initial, params)
optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params)
result = optimizer.optimize()
print("Optimization complete")
print("initial error = ", graphWithPrior.error(initial))
print("final error = ", graphWithPrior.error(result))
print("initial error = ", graph.error(initial))
print("final error = ", graph.error(result))
if args.output is None:
print("Final Result:\n{}".format(result))

9
examples/Data/pose3Localizationexample.txt Normal file
View File

@ -0,0 +1,9 @@
VERTEX_SE3:QUAT 0 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000
VERTEX_SE3:QUAT 1 1.001367 0.015390 0.004948 0.190253 0.283162 -0.392318 0.854230
VERTEX_SE3:QUAT 2 1.993500 0.023275 0.003793 -0.351729 -0.597838 0.584174 0.421446
VERTEX_SE3:QUAT 3 2.004291 1.024305 0.018047 0.331798 -0.200659 0.919323 0.067024
VERTEX_SE3:QUAT 4 0.999908 1.055073 0.020212 -0.035697 -0.462490 0.445933 0.765488
EDGE_SE3:QUAT 0 1 1.001367 0.015390 0.004948 0.190253 0.283162 -0.392318 0.854230 10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 10000.000000 0.000000 10000.000000
EDGE_SE3:QUAT 1 2 0.523923 0.776654 0.326659 0.311512 0.656877 -0.678505 0.105373 10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 10000.000000 0.000000 10000.000000
EDGE_SE3:QUAT 2 3 0.910927 0.055169 -0.411761 0.595795 -0.561677 0.079353 0.568551 10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 10000.000000 0.000000 10000.000000
EDGE_SE3:QUAT 3 4 0.775288 0.228798 -0.596923 -0.592077 0.303380 -0.513226 0.542221 10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 10000.000000 0.000000 10000.000000

5
examples/Pose2SLAMExample_g2o.cpp
View File

@ -63,10 +63,9 @@ int main(const int argc, const char *argv[]) {
}
// Add prior on the pose having index (key) = 0
NonlinearFactorGraph graphWithPrior = *graph;
noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8));
graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
graph->add(PriorFactor<Pose2>(0, Pose2(), priorModel));
std::cout << "Adding prior on pose 0 " << std::endl;
GaussNewtonParams params;
@ -77,7 +76,7 @@ int main(const int argc, const char *argv[]) {
}
std::cout << "Optimizing the factor graph" << std::endl;
GaussNewtonOptimizer optimizer(graphWithPrior, *initial, params);
GaussNewtonOptimizer optimizer(*graph, *initial, params);
Values result = optimizer.optimize();
std::cout << "Optimization complete" << std::endl;

12
examples/Pose2SLAMExample_lago.cpp
View File

@ -42,14 +42,13 @@ int main(const int argc, const char *argv[]) {
boost::tie(graph, initial) = readG2o(g2oFile);
// Add prior on the pose having index (key) = 0
NonlinearFactorGraph graphWithPrior = *graph;
noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8));
graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
graphWithPrior.print();
graph->add(PriorFactor<Pose2>(0, Pose2(), priorModel));
graph->print();
std::cout << "Computing LAGO estimate" << std::endl;
Values estimateLago = lago::initialize(graphWithPrior);
Values estimateLago = lago::initialize(*graph);
std::cout << "done!" << std::endl;
if (argc < 3) {
@ -57,7 +56,10 @@ int main(const int argc, const char *argv[]) {
} else {
const string outputFile = argv[2];
std::cout << "Writing results to file: " << outputFile << std::endl;
writeG2o(*graph, estimateLago, outputFile);
NonlinearFactorGraph::shared_ptr graphNoKernel;
Values::shared_ptr initial2;
boost::tie(graphNoKernel, initial2) = readG2o(g2oFile);
writeG2o(*graphNoKernel, estimateLago, outputFile);
std::cout << "done! " << std::endl;
}

85
examples/Pose3Localization.cpp Normal file
View File

@ -0,0 +1,85 @@
/* ----------------------------------------------------------------------------
* 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 Pose3SLAMExample_initializePose3.cpp
* @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3
* Syntax for the script is ./Pose3SLAMExample_initializePose3 input.g2o output.g2o
* @date Aug 25, 2014
* @author Luca Carlone
*/
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <fstream>
using namespace std;
using namespace gtsam;
int main(const int argc, const char *argv[]) {
// Read graph from file
string g2oFile;
if (argc < 2)
g2oFile = findExampleDataFile("pose3Localizationexample.txt");
else
g2oFile = argv[1];
NonlinearFactorGraph::shared_ptr graph;
Values::shared_ptr initial;
bool is3D = true;
boost::tie(graph, initial) = readG2o(g2oFile, is3D);
// Add prior on the first key
noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
Key firstKey = 0;
for(const Values::ConstKeyValuePair& key_value: *initial) {
std::cout << "Adding prior to g2o file " << std::endl;
firstKey = key_value.key;
graph->add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));
break;
}
std::cout << "Optimizing the factor graph" << std::endl;
GaussNewtonParams params;
params.setVerbosity("TERMINATION"); // this will show info about stopping conditions
GaussNewtonOptimizer optimizer(*graph, *initial, params);
Values result = optimizer.optimize();
std::cout << "Optimization complete" << std::endl;
std::cout << "initial error=" <<graph->error(*initial)<< std::endl;
std::cout << "final error=" <<graph->error(result)<< std::endl;
if (argc < 3) {
result.print("result");
} else {
const string outputFile = argv[2];
std::cout << "Writing results to file: " << outputFile << std::endl;
NonlinearFactorGraph::shared_ptr graphNoKernel;
Values::shared_ptr initial2;
boost::tie(graphNoKernel, initial2) = readG2o(g2oFile);
writeG2o(*graphNoKernel, result, outputFile);
std::cout << "done! " << std::endl;
}
// Calculate and print marginal covariances for all variables
Marginals marginals(*graph, result);
for(const auto& key_value: result) {
auto p = dynamic_cast<const GenericValue<Pose3>*>(&key_value.value);
if (!p) continue;
std::cout << marginals.marginalCovariance(key_value.key) << endl;
}
return 0;
}

10
examples/Pose3SLAMExample_g2o.cpp
View File

@ -41,21 +41,20 @@ int main(const int argc, const char *argv[]) {
boost::tie(graph, initial) = readG2o(g2oFile, is3D);
// Add prior on the first key
NonlinearFactorGraph graphWithPrior = *graph;
noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
Key firstKey = 0;
for(const Values::ConstKeyValuePair& key_value: *initial) {
std::cout << "Adding prior to g2o file " << std::endl;
firstKey = key_value.key;
graphWithPrior.add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));
graph->add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));
break;
}
std::cout << "Optimizing the factor graph" << std::endl;
GaussNewtonParams params;
params.setVerbosity("TERMINATION"); // this will show info about stopping conditions
GaussNewtonOptimizer optimizer(graphWithPrior, *initial, params);
GaussNewtonOptimizer optimizer(*graph, *initial, params);
Values result = optimizer.optimize();
std::cout << "Optimization complete" << std::endl;
@ -67,7 +66,10 @@ int main(const int argc, const char *argv[]) {
} else {
const string outputFile = argv[2];
std::cout << "Writing results to file: " << outputFile << std::endl;
writeG2o(*graph, result, outputFile);
NonlinearFactorGraph::shared_ptr graphNoKernel;
Values::shared_ptr initial2;
boost::tie(graphNoKernel, initial2) = readG2o(g2oFile);
writeG2o(*graphNoKernel, result, outputFile);
std::cout << "done! " << std::endl;
}
return 0;

10
examples/Pose3SLAMExample_initializePose3Chordal.cpp
View File

@ -41,19 +41,18 @@ int main(const int argc, const char *argv[]) {
boost::tie(graph, initial) = readG2o(g2oFile, is3D);
// Add prior on the first key
NonlinearFactorGraph graphWithPrior = *graph;
noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
Key firstKey = 0;
for(const Values::ConstKeyValuePair& key_value: *initial) {
std::cout << "Adding prior to g2o file " << std::endl;
firstKey = key_value.key;
graphWithPrior.add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));
graph->add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));
break;
}
std::cout << "Initializing Pose3 - chordal relaxation" << std::endl;
Values initialization = InitializePose3::initialize(graphWithPrior);
Values initialization = InitializePose3::initialize(*graph);
std::cout << "done!" << std::endl;
if (argc < 3) {
@ -61,7 +60,10 @@ int main(const int argc, const char *argv[]) {
} else {
const string outputFile = argv[2];
std::cout << "Writing results to file: " << outputFile << std::endl;
writeG2o(*graph, initialization, outputFile);
NonlinearFactorGraph::shared_ptr graphNoKernel;
Values::shared_ptr initial2;
boost::tie(graphNoKernel, initial2) = readG2o(g2oFile);
writeG2o(*graphNoKernel, initialization, outputFile);
std::cout << "done! " << std::endl;
}
return 0;

10
examples/Pose3SLAMExample_initializePose3Gradient.cpp
View File

@ -41,20 +41,19 @@ int main(const int argc, const char *argv[]) {
boost::tie(graph, initial) = readG2o(g2oFile, is3D);
// Add prior on the first key
NonlinearFactorGraph graphWithPrior = *graph;
noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
Key firstKey = 0;
for(const Values::ConstKeyValuePair& key_value: *initial) {
std::cout << "Adding prior to g2o file " << std::endl;
firstKey = key_value.key;
graphWithPrior.add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));
graph->add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));
break;
}
std::cout << "Initializing Pose3 - Riemannian gradient" << std::endl;
bool useGradient = true;
Values initialization = InitializePose3::initialize(graphWithPrior, *initial, useGradient);
Values initialization = InitializePose3::initialize(*graph, *initial, useGradient);
std::cout << "done!" << std::endl;
std::cout << "initial error=" <<graph->error(*initial)<< std::endl;
@ -65,7 +64,10 @@ int main(const int argc, const char *argv[]) {
} else {
const string outputFile = argv[2];
std::cout << "Writing results to file: " << outputFile << std::endl;
writeG2o(*graph, initialization, outputFile);
NonlinearFactorGraph::shared_ptr graphNoKernel;
Values::shared_ptr initial2;
boost::tie(graphNoKernel, initial2) = readG2o(g2oFile);
writeG2o(*graphNoKernel, initialization, outputFile);
std::cout << "done! " << std::endl;
}
return 0;

2
examples/SFMExample_SmartFactor.cpp
View File

@ -117,7 +117,7 @@ int main(int argc, char* argv[]) {
// The output of point() is in boost::optional<Point3>, as sometimes
// the triangulation operation inside smart factor will encounter degeneracy.
boost::optional<Point3> point = smart->point(result);
if (point) // ignore if boost::optional return NULL
if (point) // ignore if boost::optional return nullptr
landmark_result.insert(j, *point);
}
}

2
examples/SFMExample_SmartFactorPCG.cpp
View File

@ -114,7 +114,7 @@ int main(int argc, char* argv[]) {
boost::dynamic_pointer_cast<SmartFactor>(graph[j]);
if (smart) {
boost::optional<Point3> point = smart->point(result);
if (point) // ignore if boost::optional return NULL
if (point) // ignore if boost::optional return nullptr
landmark_result.insert(j, *point);
}
}

6
gtsam.h
View File

@ -1563,17 +1563,15 @@ virtual class Robust : gtsam::noiseModel::Base {
#include <gtsam/linear/Sampler.h>
class Sampler {
//Constructors
// Constructors
Sampler(gtsam::noiseModel::Diagonal* model, int seed);
Sampler(Vector sigmas, int seed);
Sampler(int seed);
//Standard Interface
// Standard Interface
size_t dim() const;
Vector sigmas() const;
gtsam::noiseModel::Diagonal* model() const;
Vector sample();
Vector sampleNewModel(gtsam::noiseModel::Diagonal* model);
};
#include <gtsam/linear/VectorValues.h>

26
gtsam/base/OptionalJacobian.h
View File

@ -55,7 +55,7 @@ private:
}
// Private and very dangerous constructor straight from memory
OptionalJacobian(double* data) : map_(NULL) {
OptionalJacobian(double* data) : map_(nullptr) {
if (data) usurp(data);
}
@ -66,25 +66,25 @@ public:
/// Default constructor acts like boost::none
OptionalJacobian() :
map_(NULL) {
map_(nullptr) {
}
/// Constructor that will usurp data of a fixed-size matrix
OptionalJacobian(Jacobian& fixed) :
map_(NULL) {
map_(nullptr) {
usurp(fixed.data());
}
/// Constructor that will usurp data of a fixed-size matrix, pointer version
OptionalJacobian(Jacobian* fixedPtr) :
map_(NULL) {
map_(nullptr) {
if (fixedPtr)
usurp(fixedPtr->data());
}
/// Constructor that will resize a dynamic matrix (unless already correct)
OptionalJacobian(Eigen::MatrixXd& dynamic) :
map_(NULL) {
map_(nullptr) {
dynamic.resize(Rows, Cols); // no malloc if correct size
usurp(dynamic.data());
}
@ -93,12 +93,12 @@ public:
/// Constructor with boost::none just makes empty
OptionalJacobian(boost::none_t /*none*/) :
map_(NULL) {
map_(nullptr) {
}
/// Constructor compatible with old-style derivatives
OptionalJacobian(const boost::optional<Eigen::MatrixXd&> optional) :
map_(NULL) {
map_(nullptr) {
if (optional) {
optional->resize(Rows, Cols);
usurp(optional->data());
@ -110,11 +110,11 @@ public:
/// Constructor that will usurp data of a block expression
/// TODO(frank): unfortunately using a Map makes usurping non-contiguous memory impossible
// template <typename Derived, bool InnerPanel>
// OptionalJacobian(Eigen::Block<Derived,Rows,Cols,InnerPanel> block) : map_(NULL) { ?? }
// OptionalJacobian(Eigen::Block<Derived,Rows,Cols,InnerPanel> block) : map_(nullptr) { ?? }
/// Return true is allocated, false if default constructor was used
operator bool() const {
return map_.data() != NULL;
return map_.data() != nullptr;
}
/// De-reference, like boost optional
@ -173,7 +173,7 @@ public:
/// Default constructor acts like boost::none
OptionalJacobian() :
pointer_(NULL) {
pointer_(nullptr) {
}
/// Construct from pointer to dynamic matrix
@ -186,12 +186,12 @@ public:
/// Constructor with boost::none just makes empty
OptionalJacobian(boost::none_t /*none*/) :
pointer_(NULL) {
pointer_(nullptr) {
}
/// Constructor compatible with old-style derivatives
OptionalJacobian(const boost::optional<Eigen::MatrixXd&> optional) :
pointer_(NULL) {
pointer_(nullptr) {
if (optional) pointer_ = &(*optional);
}
@ -199,7 +199,7 @@ public:
/// Return true is allocated, false if default constructor was used
operator bool() const {
return pointer_!=NULL;
return pointer_!=nullptr;
}
/// De-reference, like boost optional

12
gtsam/base/treeTraversal/parallelTraversalTasks.h
View File

@ -53,7 +53,7 @@ namespace gtsam {
// Run the post-order visitor
(void) visitorPost(treeNode, *myData);
return NULL;
return nullptr;
}
};
@ -88,7 +88,7 @@ namespace gtsam {
{
// Run the post-order visitor since this task was recycled to run the post-order visitor
(void) visitorPost(treeNode, *myData);
return NULL;
return nullptr;
}
else
{
@ -129,14 +129,14 @@ namespace gtsam {
{
// Run the post-order visitor in this task if we have no children
(void) visitorPost(treeNode, *myData);
return NULL;
return nullptr;
}
}
else
{
// Process this node and its children in this task
processNodeRecursively(treeNode, *myData);
return NULL;
return nullptr;
}
}
}
@ -184,8 +184,8 @@ namespace gtsam {
set_ref_count(1 + (int) roots.size());
// Spawn tasks
spawn_and_wait_for_all(tasks);
// Return NULL
return NULL;
// Return nullptr
return nullptr;
}
};

2
gtsam/discrete/AlgebraicDecisionTree.h
View File

@ -101,7 +101,7 @@ namespace gtsam {
/** Create a new function splitting on a variable */
template<typename Iterator>
AlgebraicDecisionTree(Iterator begin, Iterator end, const L& label) :
Super(NULL) {
Super(nullptr) {
this->root_ = compose(begin, end, label);
}

2
gtsam/discrete/DiscreteFactorGraph.cpp
View File

@ -71,7 +71,7 @@ namespace gtsam {
for (size_t i = 0; i < factors_.size(); i++) {
std::stringstream ss;
ss << "factor " << i << ": ";
if (factors_[i] != NULL) factors_[i]->print(ss.str(), formatter);
if (factors_[i] != nullptr) factors_[i]->print(ss.str(), formatter);
}
}

2
gtsam/geometry/tests/testEssentialMatrix.cpp
View File

@ -159,7 +159,7 @@ TEST (EssentialMatrix, rotate) {
Matrix actH1, actH2;
try {
bodyE.rotate(cRb, actH1, actH2);
} catch (exception e) {
} catch (exception& e) {
} // avoid exception
Matrix expH1 = numericalDerivative21(rotate_, bodyE, cRb), //
expH2 = numericalDerivative22(rotate_, bodyE, cRb);

2
gtsam/geometry/tests/testOrientedPlane3.cpp
View File

@ -183,7 +183,7 @@ TEST (OrientedPlane3, jacobian_retract) {
/* ************************************************************************* */
int main() {
srand(time(NULL));
srand(time(nullptr));
TestResult tr;
return TestRegistry::runAllTests(tr);
}

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

@ -1008,7 +1008,14 @@ TEST(Pose3, print) {
// Generate the expected output
std::stringstream expected;
Point3 translation(1, 2, 3);
#ifdef GTSAM_TYPEDEF_POINTS_TO_VECTORS
expected << "1\n"
"2\n"
"3;\n";
#else
expected << '[' << translation.x() << ", " << translation.y() << ", " << translation.z() << "]\';";
#endif
// reset cout to the original stream
std::cout.rdbuf(oldbuf);

2
gtsam/geometry/tests/testUnit3.cpp
View File

@ -495,7 +495,7 @@ TEST(actualH, Serialization) {
/* ************************************************************************* */
int main() {
srand(time(NULL));
srand(time(nullptr));
TestResult tr;
return TestRegistry::runAllTests(tr);
}

4
gtsam/inference/BayesTree-inst.h
View File

@ -125,7 +125,7 @@ namespace gtsam {
void BayesTree<CLIQUE>::addClique(const sharedClique& clique, const sharedClique& parent_clique) {
for(Key j: clique->conditional()->frontals())
nodes_[j] = clique;
if (parent_clique != NULL) {
if (parent_clique != nullptr) {
clique->parent_ = parent_clique;
parent_clique->children.push_back(clique);
} else {
@ -430,7 +430,7 @@ namespace gtsam {
template <class CLIQUE>
void BayesTree<CLIQUE>::removePath(sharedClique clique, BayesNetType* bn,
Cliques* orphans) {
// base case is NULL, if so we do nothing and return empties above
// base case is nullptr, if so we do nothing and return empties above
if (clique) {
// remove the clique from orphans in case it has been added earlier
orphans->remove(clique);

4
gtsam/inference/FactorGraph-inst.h
View File

@ -55,8 +55,8 @@ bool FactorGraph<FACTOR>::equals(const This& fg, double tol) const {
// check whether the factors are the same, in same order.
for (size_t i = 0; i < factors_.size(); i++) {
sharedFactor f1 = factors_[i], f2 = fg.factors_[i];
if (f1 == NULL && f2 == NULL) continue;
if (f1 == NULL || f2 == NULL) return false;
if (f1 == nullptr && f2 == nullptr) continue;
if (f1 == nullptr || f2 == nullptr) return false;
if (!f1->equals(*f2, tol)) return false;
}
return true;

2
gtsam/inference/FactorGraph.h
View File

@ -353,7 +353,7 @@ class FactorGraph {
*/
void resize(size_t size) { factors_.resize(size); }
/** delete factor without re-arranging indexes by inserting a NULL pointer
/** delete factor without re-arranging indexes by inserting a nullptr pointer
*/
void remove(size_t i) { factors_[i].reset(); }

4
gtsam/inference/Ordering.cpp
View File

@ -91,7 +91,7 @@ Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex,
assert((size_t)count == variableIndex.nEntries());
//double* knobs = NULL; /* colamd arg 6: parameters (uses defaults if NULL) */
//double* knobs = nullptr; /* colamd arg 6: parameters (uses defaults if nullptr) */
double knobs[CCOLAMD_KNOBS];
ccolamd_set_defaults(knobs);
knobs[CCOLAMD_DENSE_ROW] = -1;
@ -235,7 +235,7 @@ Ordering Ordering::Metis(const MetisIndex& met) {
int outputError;
outputError = METIS_NodeND(&size, &xadj[0], &adj[0], NULL, NULL, &perm[0],
outputError = METIS_NodeND(&size, &xadj[0], &adj[0], nullptr, nullptr, &perm[0],
&iperm[0]);
Ordering result;

2
gtsam/linear/GaussianBayesNet.h
View File

@ -143,7 +143,7 @@ namespace gtsam {
* allocateVectorValues */
VectorValues gradientAtZero() const;
/** Mahalanobis norm error. */
/** 0.5 * sum of squared Mahalanobis distances. */
double error(const VectorValues& x) const;
/**

2
gtsam/linear/GaussianBayesTree.h
View File

@ -106,7 +106,7 @@ namespace gtsam {
* @return A VectorValues storing the gradient. */
VectorValues gradientAtZero() const;
/** Mahalanobis norm error. */
/** 0.5 * sum of squared Mahalanobis distances. */
double error(const VectorValues& x) const;
/** Computes the determinant of a GassianBayesTree, as if the Bayes tree is reorganized into a

2
gtsam/linear/JacobianFactor.cpp
View File

@ -840,7 +840,7 @@ GaussianConditional::shared_ptr JacobianFactor::splitConditional(size_t nrFronta
if (!model_) {
throw std::invalid_argument(
"JacobianFactor::splitConditional cannot be given a NULL noise model");
"JacobianFactor::splitConditional cannot be given a nullptr noise model");
}
if (nrFrontals > size()) {

2
gtsam/linear/JacobianFactor.h
View File

@ -398,7 +398,7 @@ namespace gtsam {
* @param keys in some order
* @param diemnsions of the variables in same order
* @param m output dimension
* @param model noise model (default NULL)
* @param model noise model (default nullptr)
*/
template<class KEYS, class DIMENSIONS>
JacobianFactor(const KEYS& keys, const DIMENSIONS& dims, DenseIndex m,

16
gtsam/linear/LossFunctions.cpp
View File

@ -153,7 +153,7 @@ void Fair::print(const std::string &s="") const
bool Fair::equals(const Base &expected, double tol) const {
const Fair* p = dynamic_cast<const Fair*> (&expected);
if (p == NULL) return false;
if (p == nullptr) return false;
return std::abs(c_ - p->c_ ) < tol;
}
@ -190,7 +190,7 @@ void Huber::print(const std::string &s="") const {
bool Huber::equals(const Base &expected, double tol) const {
const Huber* p = dynamic_cast<const Huber*>(&expected);
if (p == NULL) return false;
if (p == nullptr) return false;
return std::abs(k_ - p->k_) < tol;
}
@ -223,7 +223,7 @@ void Cauchy::print(const std::string &s="") const {
bool Cauchy::equals(const Base &expected, double tol) const {
const Cauchy* p = dynamic_cast<const Cauchy*>(&expected);
if (p == NULL) return false;
if (p == nullptr) return false;
return std::abs(ksquared_ - p->ksquared_) < tol;
}
@ -266,7 +266,7 @@ void Tukey::print(const std::string &s="") const {
bool Tukey::equals(const Base &expected, double tol) const {
const Tukey* p = dynamic_cast<const Tukey*>(&expected);
if (p == NULL) return false;
if (p == nullptr) return false;
return std::abs(c_ - p->c_) < tol;
}
@ -296,7 +296,7 @@ void Welsch::print(const std::string &s="") const {
bool Welsch::equals(const Base &expected, double tol) const {
const Welsch* p = dynamic_cast<const Welsch*>(&expected);
if (p == NULL) return false;
if (p == nullptr) return false;
return std::abs(c_ - p->c_) < tol;
}
@ -330,7 +330,7 @@ void GemanMcClure::print(const std::string &s="") const {
bool GemanMcClure::equals(const Base &expected, double tol) const {
const GemanMcClure* p = dynamic_cast<const GemanMcClure*>(&expected);
if (p == NULL) return false;
if (p == nullptr) return false;
return std::abs(c_ - p->c_) < tol;
}
@ -372,7 +372,7 @@ void DCS::print(const std::string &s="") const {
bool DCS::equals(const Base &expected, double tol) const {
const DCS* p = dynamic_cast<const DCS*>(&expected);
if (p == NULL) return false;
if (p == nullptr) return false;
return std::abs(c_ - p->c_) < tol;
}
@ -411,7 +411,7 @@ void L2WithDeadZone::print(const std::string &s="") const {
bool L2WithDeadZone::equals(const Base &expected, double tol) const {
const L2WithDeadZone* p = dynamic_cast<const L2WithDeadZone*>(&expected);
if (p == NULL) return false;
if (p == nullptr) return false;
return std::abs(k_ - p->k_) < tol;
}

9
gtsam/linear/NoiseModel.cpp
View File

@ -25,7 +25,6 @@
#include <cmath>
#include <iostream>
#include <limits>
#include <random>
#include <stdexcept>
#include <typeinfo>
@ -134,7 +133,7 @@ void Gaussian::print(const string& name) const {
/* ************************************************************************* */
bool Gaussian::equals(const Base& expected, double tol) const {
const Gaussian* p = dynamic_cast<const Gaussian*> (&expected);
if (p == NULL) return false;
if (p == nullptr) return false;
if (typeid(*this) != typeid(*p)) return false;
//if (!sqrt_information_) return true; // ALEX todo;
return equal_with_abs_tol(R(), p->R(), sqrt(tol));
@ -157,7 +156,7 @@ Vector Gaussian::unwhiten(const Vector& v) const {
}
/* ************************************************************************* */
double Gaussian::Mahalanobis(const Vector& v) const {
double Gaussian::squaredMahalanobisDistance(const Vector& v) const {
// Note: for Diagonal, which does ediv_, will be correct for constraints
Vector w = whiten(v);
return w.dot(w);
@ -573,7 +572,7 @@ void Isotropic::print(const string& name) const {
}
/* ************************************************************************* */
double Isotropic::Mahalanobis(const Vector& v) const {
double Isotropic::squaredMahalanobisDistance(const Vector& v) const {
return v.dot(v) * invsigma_ * invsigma_;
}
@ -625,7 +624,7 @@ void Robust::print(const std::string& name) const {
bool Robust::equals(const Base& expected, double tol) const {
const Robust* p = dynamic_cast<const Robust*> (&expected);
if (p == NULL) return false;
if (p == nullptr) return false;
return noise_->equals(*p->noise_,tol) && robust_->equals(*p->robust_,tol);
}

23
gtsam/linear/NoiseModel.h
View File

@ -207,12 +207,25 @@ namespace gtsam {
virtual Vector unwhiten(const Vector& v) const;
/**
* Mahalanobis distance v'*R'*R*v = <R*v,R*v>
* Squared Mahalanobis distance v'*R'*R*v = <R*v,R*v>
*/
virtual double Mahalanobis(const Vector& v) const;
virtual double squaredMahalanobisDistance(const Vector& v) const;
/**
* Mahalanobis distance
*/
virtual double mahalanobisDistance(const Vector& v) const {
return std::sqrt(squaredMahalanobisDistance(v));
}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
virtual double Mahalanobis(const Vector& v) const {
return squaredMahalanobisDistance(v);
}
#endif
inline virtual double distance(const Vector& v) const {
return Mahalanobis(v);
return squaredMahalanobisDistance(v);
}
/**
@ -564,7 +577,7 @@ namespace gtsam {
}
virtual void print(const std::string& name) const;
virtual double Mahalanobis(const Vector& v) const;
virtual double squaredMahalanobisDistance(const Vector& v) const;
virtual Vector whiten(const Vector& v) const;
virtual Vector unwhiten(const Vector& v) const;
virtual Matrix Whiten(const Matrix& H) const;
@ -616,7 +629,7 @@ namespace gtsam {
virtual bool isUnit() const { return true; }
virtual void print(const std::string& name) const;
virtual double Mahalanobis(const Vector& v) const {return v.dot(v); }
virtual double squaredMahalanobisDistance(const Vector& v) const {return v.dot(v); }
virtual Vector whiten(const Vector& v) const { return v; }
virtual Vector unwhiten(const Vector& v) const { return v; }
virtual Matrix Whiten(const Matrix& H) const { return H; }

34
gtsam/linear/Sampler.cpp
View File

@ -11,6 +11,8 @@
/**
* @file Sampler.cpp
* @brief sampling from a diagonal NoiseModel
* @author Frank Dellaert
* @author Alex Cunningham
*/
@ -18,25 +20,16 @@
namespace gtsam {
/* ************************************************************************* */
Sampler::Sampler(const noiseModel::Diagonal::shared_ptr& model, int32_t seed)
: model_(model), generator_(static_cast<unsigned>(seed))
{
}
Sampler::Sampler(const noiseModel::Diagonal::shared_ptr& model,
uint_fast64_t seed)
: model_(model), generator_(seed) {}
/* ************************************************************************* */
Sampler::Sampler(const Vector& sigmas, int32_t seed)
: model_(noiseModel::Diagonal::Sigmas(sigmas, true)), generator_(static_cast<unsigned>(seed))
{
}
Sampler::Sampler(const Vector& sigmas, uint_fast64_t seed)
: model_(noiseModel::Diagonal::Sigmas(sigmas, true)), generator_(seed) {}
/* ************************************************************************* */
Sampler::Sampler(int32_t seed)
: generator_(static_cast<unsigned>(seed))
{
}
/* ************************************************************************* */
Vector Sampler::sampleDiagonal(const Vector& sigmas) {
Vector Sampler::sampleDiagonal(const Vector& sigmas) const {
size_t d = sigmas.size();
Vector result(d);
for (size_t i = 0; i < d; i++) {
@ -55,18 +48,23 @@ Vector Sampler::sampleDiagonal(const Vector& sigmas) {
}
/* ************************************************************************* */
Vector Sampler::sample() {
Vector Sampler::sample() const {
assert(model_.get());
const Vector& sigmas = model_->sigmas();
return sampleDiagonal(sigmas);
}
/* ************************************************************************* */
Vector Sampler::sampleNewModel(const noiseModel::Diagonal::shared_ptr& model) {
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
Sampler::Sampler(uint_fast64_t seed) : generator_(seed) {}
Vector Sampler::sampleNewModel(
const noiseModel::Diagonal::shared_ptr& model) const {
assert(model.get());
const Vector& sigmas = model->sigmas();
return sampleDiagonal(sigmas);
}
#endif
/* ************************************************************************* */
} // \namespace gtsam
} // namespace gtsam

79
gtsam/linear/Sampler.h
View File

@ -10,9 +10,9 @@
* -------------------------------------------------------------------------- */
/**
* @brief sampling that can be parameterized using a NoiseModel to generate samples from
* @file Sampler.h
* the given distribution
* @brief sampling from a NoiseModel
* @author Frank Dellaert
* @author Alex Cunningham
*/
@ -27,67 +27,74 @@ namespace gtsam {
/**
* Sampling structure that keeps internal random number generators for
* diagonal distributions specified by NoiseModel
*
* This is primarily to allow for variable seeds, and does roughly the same
* thing as sample() in NoiseModel.
*/
class GTSAM_EXPORT Sampler {
protected:
protected:
/** noiseModel created at generation */
noiseModel::Diagonal::shared_ptr model_;
/** generator */
std::mt19937_64 generator_;
mutable std::mt19937_64 generator_;
public:
public:
typedef boost::shared_ptr<Sampler> shared_ptr;
/// @name constructors
/// @{
/**
* Create a sampler for the distribution specified by a diagonal NoiseModel
* with a manually specified seed
*
* NOTE: do not use zero as a seed, it will break the generator
*/
Sampler(const noiseModel::Diagonal::shared_ptr& model, int32_t seed = 42u);
explicit Sampler(const noiseModel::Diagonal::shared_ptr& model,
uint_fast64_t seed = 42u);
/**
* Create a sampler for a distribution specified by a vector of sigmas directly
* Create a sampler for a distribution specified by a vector of sigmas
* directly
*
* NOTE: do not use zero as a seed, it will break the generator
*/
Sampler(const Vector& sigmas, int32_t seed = 42u);
explicit Sampler(const Vector& sigmas, uint_fast64_t seed = 42u);
/**
* Create a sampler without a given noisemodel - pass in to sample
*
* NOTE: do not use zero as a seed, it will break the generator
*/
Sampler(int32_t seed = 42u);
/// @}
/// @name access functions
/// @{
size_t dim() const {
assert(model_.get());
return model_->dim();
}
Vector sigmas() const {
assert(model_.get());
return model_->sigmas();
}
/** access functions */
size_t dim() const { assert(model_.get()); return model_->dim(); }
Vector sigmas() const { assert(model_.get()); return model_->sigmas(); }
const noiseModel::Diagonal::shared_ptr& model() const { return model_; }
/**
* sample from distribution
* NOTE: not const due to need to update the underlying generator
*/
Vector sample();
/// @}
/// @name basic functionality
/// @{
/**
* Sample from noisemodel passed in as an argument,
* can be used without having initialized a model for the system.
*
* NOTE: not const due to need to update the underlying generator
*/
Vector sampleNewModel(const noiseModel::Diagonal::shared_ptr& model);
/// sample from distribution
Vector sample() const;
protected:
/// @}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// @name Deprecated
/// @{
explicit Sampler(uint_fast64_t seed = 42u);
Vector sampleNewModel(const noiseModel::Diagonal::shared_ptr& model) const;
/// @}
#endif
protected:
/** given sigmas for a diagonal model, returns a sample */
Vector sampleDiagonal(const Vector& sigmas);
Vector sampleDiagonal(const Vector& sigmas) const;
};
} // \namespace gtsam
} // namespace gtsam

28
gtsam/linear/VectorValues.cpp
View File

@ -51,7 +51,11 @@ namespace gtsam {
Key key;
size_t n;
boost::tie(key, n) = v;
#ifdef TBB_GREATER_EQUAL_2020
values_.emplace(key, x.segment(j, n));
#else
values_.insert(std::make_pair(key, x.segment(j, n)));
#endif
j += n;
}
}
@ -60,7 +64,11 @@ namespace gtsam {
VectorValues::VectorValues(const Vector& x, const Scatter& scatter) {
size_t j = 0;
for (const SlotEntry& v : scatter) {
#ifdef TBB_GREATER_EQUAL_2020
values_.emplace(v.key, x.segment(j, v.dimension));
#else
values_.insert(std::make_pair(v.key, x.segment(j, v.dimension)));
#endif
j += v.dimension;
}
}
@ -70,7 +78,11 @@ namespace gtsam {
{
VectorValues result;
for(const KeyValuePair& v: other)
#ifdef TBB_GREATER_EQUAL_2020
result.values_.emplace(v.first, Vector::Zero(v.second.size()));
#else
result.values_.insert(std::make_pair(v.first, Vector::Zero(v.second.size())));
#endif
return result;
}
@ -86,7 +98,11 @@ namespace gtsam {
/* ************************************************************************* */
VectorValues::iterator VectorValues::emplace(Key j, const Vector& value) {
#ifdef TBB_GREATER_EQUAL_2020
std::pair<iterator, bool> result = values_.emplace(j, value);
#else
std::pair<iterator, bool> result = values_.insert(std::make_pair(j, value));
#endif
if(!result.second)
throw std::invalid_argument(
"Requested to emplace variable '" + DefaultKeyFormatter(j)
@ -266,7 +282,11 @@ namespace gtsam {
VectorValues result;
// The result.end() hint here should result in constant-time inserts
for(const_iterator j1 = begin(), j2 = c.begin(); j1 != end(); ++j1, ++j2)
#ifdef TBB_GREATER_EQUAL_2020
result.values_.emplace(j1->first, j1->second + j2->second);
#else
result.values_.insert(std::make_pair(j1->first, j1->second + j2->second));
#endif
return result;
}
@ -324,7 +344,11 @@ namespace gtsam {
VectorValues result;
// The result.end() hint here should result in constant-time inserts
for(const_iterator j1 = begin(), j2 = c.begin(); j1 != end(); ++j1, ++j2)
#ifdef TBB_GREATER_EQUAL_2020
result.values_.emplace(j1->first, j1->second - j2->second);
#else
result.values_.insert(std::make_pair(j1->first, j1->second - j2->second));
#endif
return result;
}
@ -340,7 +364,11 @@ namespace gtsam {
{
VectorValues result;
for(const VectorValues::KeyValuePair& key_v: v)
#ifdef TBB_GREATER_EQUAL_2020
result.values_.emplace(key_v.first, a * key_v.second);
#else
result.values_.insert(std::make_pair(key_v.first, a * key_v.second));
#endif
return result;
}

6
gtsam/linear/VectorValues.h
View File

@ -198,7 +198,11 @@ namespace gtsam {
* exist, it is inserted and an iterator pointing to the new element, along with 'true', is
* returned. */
std::pair<iterator, bool> tryInsert(Key j, const Vector& value) {
return values_.emplace(j, value);
#ifdef TBB_GREATER_EQUAL_2020
return values_.emplace(j, value);
#else
return values_.insert(std::make_pair(j, value));
#endif
}
/** Erase the vector with the given key, or throw std::out_of_range if it does not exist */

2
gtsam/linear/tests/testJacobianFactor.cpp
View File

@ -253,7 +253,7 @@ TEST(JacobianFactor, error)
/* ************************************************************************* */
TEST(JacobianFactor, matrices_NULL)
{
// Make sure everything works with NULL noise model
// Make sure everything works with nullptr noise model
JacobianFactor factor(simple::terms, simple::b);
Matrix jacobianExpected(3, 9);

4
gtsam/linear/tests/testNoiseModel.cpp
View File

@ -68,10 +68,10 @@ TEST(NoiseModel, constructors)
for(Gaussian::shared_ptr mi: m)
EXPECT(assert_equal(unwhitened,mi->unwhiten(whitened)));
// test Mahalanobis distance
// test squared Mahalanobis distance
double distance = 5*5+10*10+15*15;
for(Gaussian::shared_ptr mi: m)
DOUBLES_EQUAL(distance,mi->Mahalanobis(unwhitened),1e-9);
DOUBLES_EQUAL(distance,mi->squaredMahalanobisDistance(unwhitened),1e-9);
// test R matrix
for(Gaussian::shared_ptr mi: m)

18
gtsam/linear/tests/testSampler.cpp
View File

@ -10,8 +10,10 @@
* -------------------------------------------------------------------------- */
/**
* @file testSampler
* @file testSampler.cpp
* @brief unit tests for Sampler class
* @author Alex Cunningham
* @author Frank Dellaert
*/
#include <CppUnitLite/TestHarness.h>
@ -22,14 +24,15 @@ using namespace gtsam;
const double tol = 1e-5;
static const Vector3 kSigmas(1.0, 0.1, 0.0);
/* ************************************************************************* */
TEST(testSampler, basic) {
Vector sigmas = Vector3(1.0, 0.1, 0.0);
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(sigmas);
auto model = noiseModel::Diagonal::Sigmas(kSigmas);
char seed = 'A';
Sampler sampler1(model, seed), sampler2(model, 1), sampler3(model, 1);
EXPECT(assert_equal(sigmas, sampler1.sigmas()));
EXPECT(assert_equal(sigmas, sampler2.sigmas()));
EXPECT(assert_equal(kSigmas, sampler1.sigmas()));
EXPECT(assert_equal(kSigmas, sampler2.sigmas()));
EXPECT_LONGS_EQUAL(3, sampler1.dim());
EXPECT_LONGS_EQUAL(3, sampler2.dim());
Vector actual1 = sampler1.sample();
@ -38,5 +41,8 @@ TEST(testSampler, basic) {
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

2
gtsam/navigation/AHRSFactor.cpp
View File

@ -114,7 +114,7 @@ void AHRSFactor::print(const string& s,
//------------------------------------------------------------------------------
bool AHRSFactor::equals(const NonlinearFactor& other, double tol) const {
const This *e = dynamic_cast<const This*>(&other);
return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol);
return e != nullptr && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol);
}
//------------------------------------------------------------------------------

4
gtsam/navigation/AttitudeFactor.cpp
View File

@ -52,7 +52,7 @@ void Rot3AttitudeFactor::print(const string& s,
bool Rot3AttitudeFactor::equals(const NonlinearFactor& expected,
double tol) const {
const This* e = dynamic_cast<const This*>(&expected);
return e != NULL && Base::equals(*e, tol) && this->nZ_.equals(e->nZ_, tol)
return e != nullptr && Base::equals(*e, tol) && this->nZ_.equals(e->nZ_, tol)
&& this->bRef_.equals(e->bRef_, tol);
}
@ -69,7 +69,7 @@ void Pose3AttitudeFactor::print(const string& s,
bool Pose3AttitudeFactor::equals(const NonlinearFactor& expected,
double tol) const {
const This* e = dynamic_cast<const This*>(&expected);
return e != NULL && Base::equals(*e, tol) && this->nZ_.equals(e->nZ_, tol)
return e != nullptr && Base::equals(*e, tol) && this->nZ_.equals(e->nZ_, tol)
&& this->bRef_.equals(e->bRef_, tol);
}

2
gtsam/navigation/CombinedImuFactor.cpp
View File

@ -174,7 +174,7 @@ void CombinedImuFactor::print(const string& s,
//------------------------------------------------------------------------------
bool CombinedImuFactor::equals(const NonlinearFactor& other, double tol) const {
const This* e = dynamic_cast<const This*>(&other);
return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol);
return e != nullptr && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol);
}
//------------------------------------------------------------------------------

4
gtsam/navigation/GPSFactor.cpp
View File

@ -32,7 +32,7 @@ void GPSFactor::print(const string& s, const KeyFormatter& keyFormatter) const {
//***************************************************************************
bool GPSFactor::equals(const NonlinearFactor& expected, double tol) const {
const This* e = dynamic_cast<const This*>(&expected);
return e != NULL && Base::equals(*e, tol) && traits<Point3>::Equals(nT_, e->nT_, tol);
return e != nullptr && Base::equals(*e, tol) && traits<Point3>::Equals(nT_, e->nT_, tol);
}
//***************************************************************************
@ -73,7 +73,7 @@ void GPSFactor2::print(const string& s, const KeyFormatter& keyFormatter) const
//***************************************************************************
bool GPSFactor2::equals(const NonlinearFactor& expected, double tol) const {
const This* e = dynamic_cast<const This*>(&expected);
return e != NULL && Base::equals(*e, tol) &&
return e != nullptr && Base::equals(*e, tol) &&
traits<Point3>::Equals(nT_, e->nT_, tol);
}

2
gtsam/navigation/NavState.h
View File

@ -64,7 +64,7 @@ public:
R_(pose.rotation()), t_(pose.translation()), v_(v) {
}
/// Construct from SO(3) and R^6
NavState(const Matrix3& R, const Vector9 tv) :
NavState(const Matrix3& R, const Vector6& tv) :
R_(R), t_(tv.head<3>()), v_(tv.tail<3>()) {
}
/// Named constructor with derivatives

9
gtsam/navigation/PreintegrationParams.h
View File

@ -29,6 +29,13 @@ struct GTSAM_EXPORT PreintegrationParams: PreintegratedRotationParams {
bool use2ndOrderCoriolis; ///< Whether to use second order Coriolis integration
Vector3 n_gravity; ///< Gravity vector in nav frame
/// Default constructor for serialization only
PreintegrationParams()
: accelerometerCovariance(I_3x3),
integrationCovariance(I_3x3),
use2ndOrderCoriolis(false),
n_gravity(0, 0, -1) {}
/// The Params constructor insists on getting the navigation frame gravity vector
/// For convenience, two commonly used conventions are provided by named constructors below
PreintegrationParams(const Vector3& n_gravity)
@ -60,8 +67,6 @@ struct GTSAM_EXPORT PreintegrationParams: PreintegratedRotationParams {
bool getUse2ndOrderCoriolis() const { return use2ndOrderCoriolis; }
protected:
/// Default constructor for serialization only: uninitialized!
PreintegrationParams() {}
/** Serialization function */
friend class boost::serialization::access;

2
gtsam/navigation/ScenarioRunner.h
View File

@ -48,7 +48,7 @@ class GTSAM_EXPORT ScenarioRunner {
const Bias estimatedBias_;
// Create two samplers for acceleration and omega noise
mutable Sampler gyroSampler_, accSampler_;
Sampler gyroSampler_, accSampler_;
public:
ScenarioRunner(const Scenario& scenario, const SharedParams& p,

6
gtsam/navigation/tests/testImuFactorSerialization.cpp
View File

@ -64,9 +64,9 @@ TEST(ImuFactor, serialization) {
ImuFactor factor(1, 2, 3, 4, 5, pim);
EXPECT(equalsObj(factor));
EXPECT(equalsXML(factor));
EXPECT(equalsBinary(factor));
EXPECT(equalsObj<ImuFactor>(factor));
EXPECT(equalsXML<ImuFactor>(factor));
EXPECT(equalsBinary<ImuFactor>(factor));
}
/* ************************************************************************* */

2
gtsam/nonlinear/ISAM2Params.h
View File

@ -219,7 +219,7 @@ struct GTSAM_EXPORT ISAM2Params {
/// When you will be removing many factors, e.g. when using ISAM2 as a
/// fixed-lag smoother, enable this option to add factors in the first
/// available factor slots, to avoid accumulating NULL factor slots, at the
/// available factor slots, to avoid accumulating nullptr factor slots, at the
/// cost of having to search for slots every time a factor is added.
bool findUnusedFactorSlots;

6
gtsam/nonlinear/NonlinearEquality.h
View File

@ -175,6 +175,8 @@ public:
/// @}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
/** Serialization function */
@ -263,6 +265,8 @@ public:
traits<X>::Print(value_, "Value");
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
/** Serialization function */
@ -327,6 +331,8 @@ public:
return traits<X>::Local(x1,x2);
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
/** Serialization function */

6
gtsam/nonlinear/NonlinearFactorGraph.cpp
View File

@ -54,7 +54,7 @@ void NonlinearFactorGraph::print(const std::string& str, const KeyFormatter& key
for (size_t i = 0; i < factors_.size(); i++) {
stringstream ss;
ss << "Factor " << i << ": ";
if (factors_[i] != NULL) factors_[i]->print(ss.str(), keyFormatter);
if (factors_[i] != nullptr) factors_[i]->print(ss.str(), keyFormatter);
cout << endl;
}
}
@ -67,8 +67,8 @@ void NonlinearFactorGraph::printErrors(const Values& values, const std::string&
for (size_t i = 0; i < factors_.size(); i++) {
stringstream ss;
ss << "Factor " << i << ": ";
if (factors_[i] == NULL) {
cout << "NULL" << endl;
if (factors_[i] == nullptr) {
cout << "nullptr" << endl;
} else {
factors_[i]->print(ss.str(), keyFormatter);
cout << "error = " << factors_[i]->error(values) << endl;

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

@ -98,7 +98,7 @@ struct Record: public internal::CallRecordImplementor<Record, Cols> {
friend struct internal::CallRecordImplementor;
};
internal::JacobianMap & NJM= *static_cast<internal::JacobianMap *>(NULL);
internal::JacobianMap & NJM= *static_cast<internal::JacobianMap *>(nullptr);
/* ************************************************************************* */
typedef Eigen::Matrix<double, Eigen::Dynamic, Cols> DynRowMat;

7
gtsam/nonlinear/utilities.h
View File

@ -237,13 +237,16 @@ Values localToWorld(const Values& local, const Pose2& base,
// if value is a Pose2, compose it with base pose
Pose2 pose = local.at<Pose2>(key);
world.insert(key, base.compose(pose));
} catch (std::exception e1) {
} catch (const std::exception& e1) {
try {
// if value is a Point2, transform it from base pose
Point2 point = local.at<Point2>(key);
world.insert(key, base.transformFrom(point));
} catch (std::exception e2) {
} catch (const std::exception& e2) {
// if not Pose2 or Point2, do nothing
#ifndef NDEBUG
std::cerr << "Values[key] is neither Pose2 nor Point2, so skip" << std::endl;
#endif
}
}
}

2
gtsam/slam/AntiFactor.h
View File

@ -67,7 +67,7 @@ namespace gtsam {
/** equals */
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
const This *e = dynamic_cast<const This*> (&expected);
return e != NULL && Base::equals(*e, tol) && this->factor_->equals(*e->factor_, tol);
return e != nullptr && Base::equals(*e, tol) && this->factor_->equals(*e->factor_, tol);
}
/** implement functions needed to derive from Factor */

2
gtsam/slam/BetweenFactor.h
View File

@ -81,7 +81,7 @@ namespace gtsam {
/** equals */
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
const This *e = dynamic_cast<const This*> (&expected);
return e != NULL && Base::equals(*e, tol) && traits<T>::Equals(this->measured_, e->measured_, tol);
return e != nullptr && Base::equals(*e, tol) && traits<T>::Equals(this->measured_, e->measured_, tol);
}
/** implement functions needed to derive from Factor */

2
gtsam/slam/EssentialMatrixConstraint.cpp
View File

@ -37,7 +37,7 @@ void EssentialMatrixConstraint::print(const std::string& s,
bool EssentialMatrixConstraint::equals(const NonlinearFactor& expected,
double tol) const {
const This *e = dynamic_cast<const This*>(&expected);
return e != NULL && Base::equals(*e, tol)
return e != nullptr && Base::equals(*e, tol)
&& this->measuredE_.equals(e->measuredE_, tol);
}

2
gtsam/slam/JacobianFactorSVD.h
View File

@ -65,7 +65,7 @@ public:
Base() {
size_t numKeys = Enull.rows() / ZDim;
size_t m2 = ZDim * numKeys - 3; // TODO: is this not just Enull.rows()?
// PLAIN NULL SPACE TRICK
// PLAIN nullptr SPACE TRICK
// Matrix Q = Enull * Enull.transpose();
// for(const KeyMatrixZD& it: Fblocks)
// QF.push_back(KeyMatrix(it.first, Q.block(0, 2 * j++, m2, 2) * it.second));

2
gtsam/slam/OrientedPlane3Factor.cpp
View File

@ -31,7 +31,7 @@ void OrientedPlane3DirectionPrior::print(const string& s,
bool OrientedPlane3DirectionPrior::equals(const NonlinearFactor& expected,
double tol) const {
const This* e = dynamic_cast<const This*>(&expected);
return e != NULL && Base::equals(*e, tol)
return e != nullptr && Base::equals(*e, tol)
&& this->measured_p_.equals(e->measured_p_, tol);
}

2
gtsam/slam/PoseRotationPrior.h
View File

@ -62,7 +62,7 @@ public:
/** equals specialized to this factor */
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
const This *e = dynamic_cast<const This*> (&expected);
return e != NULL && Base::equals(*e, tol) && measured_.equals(e->measured_, tol);
return e != nullptr && Base::equals(*e, tol) && measured_.equals(e->measured_, tol);
}
/** print contents */

2
gtsam/slam/PoseTranslationPrior.h
View File

@ -76,7 +76,7 @@ public:
/** equals specialized to this factor */
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
const This *e = dynamic_cast<const This*> (&expected);
return e != NULL && Base::equals(*e, tol) && traits<Translation>::Equals(measured_, e->measured_, tol);
return e != nullptr && Base::equals(*e, tol) && traits<Translation>::Equals(measured_, e->measured_, tol);
}
/** print contents */

2
gtsam/slam/PriorFactor.h
View File

@ -85,7 +85,7 @@ namespace gtsam {
/** equals */
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
const This* e = dynamic_cast<const This*> (&expected);
return e != NULL && Base::equals(*e, tol) && traits<T>::Equals(prior_, e->prior_, tol);
return e != nullptr && Base::equals(*e, tol) && traits<T>::Equals(prior_, e->prior_, tol);
}
/** implement functions needed to derive from Factor */

6
gtsam/slam/dataset.cpp
View File

@ -252,7 +252,7 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID,
is.seekg(0, ios::beg);
// If asked, create a sampler with random number generator
Sampler sampler;
std::unique_ptr<Sampler> sampler;
if (addNoise) {
noiseModel::Diagonal::shared_ptr noise;
if (model)
@ -261,7 +261,7 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID,
throw invalid_argument(
"gtsam::load2D: invalid noise model for adding noise"
"(current version assumes diagonal noise model)!");
sampler = Sampler(noise);
sampler.reset(new Sampler(noise));
}
// Parse the pose constraints
@ -289,7 +289,7 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID,
model = modelInFile;
if (addNoise)
l1Xl2 = l1Xl2.retract(sampler.sample());
l1Xl2 = l1Xl2.retract(sampler->sample());
// Insert vertices if pure odometry file
if (!initial->exists(id1))

2
gtsam/slam/tests/testOrientedPlane3Factor.cpp
View File

@ -175,7 +175,7 @@ TEST( OrientedPlane3DirectionPrior, Constructor ) {
/* ************************************************************************* */
int main() {
srand(time(NULL));
srand(time(nullptr));
TestResult tr;
return TestRegistry::runAllTests(tr);
}

4
gtsam_unstable/base/BTree.h
View File

@ -72,7 +72,7 @@ namespace gtsam {
}; // Node
// We store a shared pointer to the root of the functional tree
// composed of Node classes. If root_==NULL, the tree is empty.
// composed of Node classes. If root_==nullptr, the tree is empty.
typedef boost::shared_ptr<const Node> sharedNode;
sharedNode root_;
@ -223,7 +223,7 @@ namespace gtsam {
/** Return height of the tree, 0 if empty */
size_t height() const {
return (root_ != NULL) ? root_->height_ : 0;
return (root_ != nullptr) ? root_->height_ : 0;
}
/** return size of the tree */

2
gtsam_unstable/linear/tests/testLinearEquality.cpp
View File

@ -136,7 +136,7 @@ TEST(LinearEquality, error)
/* ************************************************************************* */
TEST(LinearEquality, matrices_NULL)
{
// Make sure everything works with NULL noise model
// Make sure everything works with nullptr noise model
LinearEquality factor(simple::terms, simple::b, 0);
Matrix AExpected(3, 9);

6
gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp
View File

@ -39,7 +39,7 @@ bool BatchFixedLagSmoother::equals(const FixedLagSmoother& rhs,
double tol) const {
const BatchFixedLagSmoother* e =
dynamic_cast<const BatchFixedLagSmoother*>(&rhs);
return e != NULL && FixedLagSmoother::equals(*e, tol)
return e != nullptr && FixedLagSmoother::equals(*e, tol)
&& factors_.equals(e->factors_, tol) && theta_.equals(e->theta_, tol);
}
@ -145,7 +145,7 @@ void BatchFixedLagSmoother::removeFactors(
} else {
// TODO: Throw an error??
cout << "Attempting to remove a factor from slot " << slot
<< ", but it is already NULL." << endl;
<< ", but it is already nullptr." << endl;
}
}
}
@ -370,7 +370,7 @@ void BatchFixedLagSmoother::PrintSymbolicFactor(
cout << " " << DefaultKeyFormatter(key);
}
} else {
cout << " NULL";
cout << " nullptr";
}
cout << " )" << endl;
}

4
gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp
View File

@ -38,7 +38,7 @@ void ConcurrentBatchFilter::PrintNonlinearFactor(const NonlinearFactor::shared_p
}
std::cout << ")" << std::endl;
} else {
std::cout << "{ NULL }" << std::endl;
std::cout << "{ nullptr }" << std::endl;
}
}
@ -79,7 +79,7 @@ void ConcurrentBatchFilter::PrintLinearFactor(const GaussianFactor::shared_ptr&
}
std::cout << ")" << std::endl;
} else {
std::cout << "{ NULL }" << std::endl;
std::cout << "{ nullptr }" << std::endl;
}
}

4
gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp
View File

@ -394,7 +394,7 @@ void ConcurrentBatchSmoother::PrintNonlinearFactor(const NonlinearFactor::shared
}
std::cout << ")" << std::endl;
} else {
std::cout << "{ NULL }" << std::endl;
std::cout << "{ nullptr }" << std::endl;
}
}
@ -408,7 +408,7 @@ void ConcurrentBatchSmoother::PrintLinearFactor(const GaussianFactor::shared_ptr
}
std::cout << ")" << std::endl;
} else {
std::cout << "{ NULL }" << std::endl;
std::cout << "{ nullptr }" << std::endl;
}
}

2
gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp
View File

@ -58,7 +58,7 @@ bool IncrementalFixedLagSmoother::equals(const FixedLagSmoother& rhs,
double tol) const {
const IncrementalFixedLagSmoother* e =
dynamic_cast<const IncrementalFixedLagSmoother*>(&rhs);
return e != NULL && FixedLagSmoother::equals(*e, tol)
return e != nullptr && FixedLagSmoother::equals(*e, tol)
&& isam_.equals(e->isam_, tol);
}

6
gtsam_unstable/partition/FindSeparator-inl.h
View File

@ -83,14 +83,14 @@ namespace gtsam { namespace partition {
graph_t *graph;
real_t *tpwgts2;
ctrl_t *ctrl;
ctrl = SetupCtrl(METIS_OP_OMETIS, options, 1, 3, NULL, NULL);
ctrl = SetupCtrl(METIS_OP_OMETIS, options, 1, 3, nullptr, nullptr);
ctrl->iptype = METIS_IPTYPE_GROW;
//if () == NULL)
//if () == nullptr)
// return METIS_ERROR_INPUT;
InitRandom(ctrl->seed);
graph = SetupGraph(ctrl, *nvtxs, 1, xadj, adjncy, vwgt, NULL, NULL);
graph = SetupGraph(ctrl, *nvtxs, 1, xadj, adjncy, vwgt, nullptr, nullptr);
AllocateWorkSpace(ctrl, graph);

2
gtsam_unstable/slam/AHRS.h
View File

@ -77,6 +77,8 @@ public:
void print(const std::string& s = "") const;
virtual ~AHRS();
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} /* namespace gtsam */

2
gtsam_unstable/slam/BiasedGPSFactor.h
View File

@ -66,7 +66,7 @@ namespace gtsam {
/** equals */
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
const This *e = dynamic_cast<const This*> (&expected);
return e != NULL && Base::equals(*e, tol) && traits<Point3>::Equals(this->measured_, e->measured_, tol);
return e != nullptr && Base::equals(*e, tol) && traits<Point3>::Equals(this->measured_, e->measured_, tol);
}
/** implement functions needed to derive from Factor */

2
gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h
View File

@ -155,7 +155,7 @@ public:
/** equals */
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
const This *e = dynamic_cast<const This*> (&expected);
return e != NULL && Base::equals(*e, tol)
return e != nullptr && Base::equals(*e, tol)
&& (delta_pos_in_t0_ - e->delta_pos_in_t0_).norm() < tol
&& (delta_vel_in_t0_ - e->delta_vel_in_t0_).norm() < tol
&& (delta_angles_ - e->delta_angles_).norm() < tol

2
gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h
View File

@ -153,7 +153,7 @@ public:
/** equals */
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
const This *e = dynamic_cast<const This*> (&expected);
return e != NULL && Base::equals(*e, tol)
return e != nullptr && Base::equals(*e, tol)
&& (delta_pos_in_t0_ - e->delta_pos_in_t0_).norm() < tol
&& (delta_vel_in_t0_ - e->delta_vel_in_t0_).norm() < tol
&& (delta_angles_ - e->delta_angles_).norm() < tol

2
gtsam_unstable/slam/GaussMarkov1stOrderFactor.h
View File

@ -81,7 +81,7 @@ public:
/** equals */
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
const This *e = dynamic_cast<const This*> (&expected);
return e != NULL && Base::equals(*e, tol);
return e != nullptr && Base::equals(*e, tol);
}
/** implement functions needed to derive from Factor */

2
gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h
View File

@ -135,7 +135,7 @@ public:
/** equals */
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
const This *e = dynamic_cast<const This*> (&expected);
return e != NULL && Base::equals(*e, tol)
return e != nullptr && Base::equals(*e, tol)
&& (measurement_acc_ - e->measurement_acc_).norm() < tol
&& (measurement_gyro_ - e->measurement_gyro_).norm() < tol
&& (dt_ - e->dt_) < tol

2
gtsam_unstable/slam/PartialPriorFactor.h
View File

@ -100,7 +100,7 @@ namespace gtsam {
/** equals */
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
const This *e = dynamic_cast<const This*> (&expected);
return e != NULL && Base::equals(*e, tol) &&
return e != nullptr && Base::equals(*e, tol) &&
gtsam::equal_with_abs_tol(this->prior_, e->prior_, tol) &&
this->mask_ == e->mask_;
}

2
gtsam_unstable/slam/PoseBetweenFactor.h
View File

@ -79,7 +79,7 @@ namespace gtsam {
/** equals */
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
const This *e = dynamic_cast<const This*> (&expected);
return e != NULL
return e != nullptr
&& Base::equals(*e, tol)
&& this->measured_.equals(e->measured_, tol)
&& ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));

2
gtsam_unstable/slam/PosePriorFactor.h
View File

@ -74,7 +74,7 @@ namespace gtsam {
/** equals */
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
const This* e = dynamic_cast<const This*> (&expected);
return e != NULL
return e != nullptr
&& Base::equals(*e, tol)
&& this->prior_.equals(e->prior_, tol)
&& ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));

2
gtsam_unstable/slam/RelativeElevationFactor.cpp
View File

@ -38,7 +38,7 @@ Vector RelativeElevationFactor::evaluateError(const Pose3& pose, const Point3& p
/* ************************************************************************* */
bool RelativeElevationFactor::equals(const NonlinearFactor& expected, double tol) const {
const This *e = dynamic_cast<const This*> (&expected);
return e != NULL && Base::equals(*e, tol) && std::abs(this->measured_ - e->measured_) < tol;
return e != nullptr && Base::equals(*e, tol) && std::abs(this->measured_ - e->measured_) < tol;
}
/* ************************************************************************* */

8
tests/testExtendedKalmanFilter.cpp
View File

@ -162,7 +162,7 @@ public:
/** Check if two factors are equal. Note type is IndexFactor and needs cast. */
virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const {
const This *e = dynamic_cast<const This*> (&f);
return (e != NULL) && (key1() == e->key1()) && (key2() == e->key2());
return (e != nullptr) && (key1() == e->key1()) && (key2() == e->key2());
}
/**
@ -196,7 +196,7 @@ public:
Vector b = -evaluateError(x1, x2, A1, A2);
SharedDiagonal constrained =
boost::dynamic_pointer_cast<noiseModel::Constrained>(this->noiseModel_);
if (constrained.get() != NULL) {
if (constrained.get() != nullptr) {
return JacobianFactor::shared_ptr(new JacobianFactor(key1(), A1, key2(),
A2, b, constrained));
}
@ -292,7 +292,7 @@ public:
/** Check if two factors are equal. Note type is IndexFactor and needs cast. */
virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const {
const This *e = dynamic_cast<const This*> (&f);
return (e != NULL) && Base::equals(f);
return (e != nullptr) && Base::equals(f);
}
/**
@ -325,7 +325,7 @@ public:
Vector b = -evaluateError(x1, A1);
SharedDiagonal constrained =
boost::dynamic_pointer_cast<noiseModel::Constrained>(this->noiseModel_);
if (constrained.get() != NULL) {
if (constrained.get() != nullptr) {
return JacobianFactor::shared_ptr(new JacobianFactor(key(), A1, b, constrained));
}
// "Whiten" the system before converting to a Gaussian Factor

128
tests/testNonlinearOptimizer.cpp
View File

@ -20,6 +20,7 @@
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/DoglegOptimizer.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
@ -339,31 +340,136 @@ TEST_UNSAFE(NonlinearOptimizer, MoreOptimization) {
}
/* ************************************************************************* */
TEST(NonlinearOptimizer, MoreOptimizationWithHuber) {
TEST(NonlinearOptimizer, Pose2OptimizationWithHuberNoOutlier) {
NonlinearFactorGraph fg;
fg += PriorFactor<Pose2>(0, Pose2(0,0,0), noiseModel::Isotropic::Sigma(3,1));
fg += BetweenFactor<Pose2>(0, 1, Pose2(1,0,M_PI/2),
fg += BetweenFactor<Pose2>(0, 1, Pose2(1,1.1,M_PI/4),
noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(2.0),
noiseModel::Isotropic::Sigma(3,1)));
fg += BetweenFactor<Pose2>(1, 2, Pose2(1,0,M_PI/2),
fg += BetweenFactor<Pose2>(0, 1, Pose2(1,0.9,M_PI/2),
noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(3.0),
noiseModel::Isotropic::Sigma(3,1)));
Values init;
init.insert(0, Pose2(10,10,0));
init.insert(1, Pose2(1,0,M_PI));
init.insert(2, Pose2(1,1,-M_PI));
init.insert(0, Pose2(0,0,0));
init.insert(1, Pose2(0.961187, 0.99965, 1.1781));
Values expected;
expected.insert(0, Pose2(0,0,0));
expected.insert(1, Pose2(1,0,M_PI/2));
expected.insert(2, Pose2(1,1,M_PI));
expected.insert(1, Pose2(0.961187, 0.99965, 1.1781));
LevenbergMarquardtParams lm_params;
auto gn_result = GaussNewtonOptimizer(fg, init).optimize();
auto lm_result = LevenbergMarquardtOptimizer(fg, init, lm_params).optimize();
auto dl_result = DoglegOptimizer(fg, init).optimize();
EXPECT(assert_equal(expected, gn_result, 3e-2));
EXPECT(assert_equal(expected, lm_result, 3e-2));
EXPECT(assert_equal(expected, dl_result, 3e-2));
}
/* ************************************************************************* */
TEST(NonlinearOptimizer, Point2LinearOptimizationWithHuber) {
NonlinearFactorGraph fg;
fg += PriorFactor<Point2>(0, Point2(0,0), noiseModel::Isotropic::Sigma(2,0.01));
fg += BetweenFactor<Point2>(0, 1, Point2(1,1.8),
noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.0),
noiseModel::Isotropic::Sigma(2,1)));
fg += BetweenFactor<Point2>(0, 1, Point2(1,0.9),
noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.0),
noiseModel::Isotropic::Sigma(2,1)));
fg += BetweenFactor<Point2>(0, 1, Point2(1,90),
noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.0),
noiseModel::Isotropic::Sigma(2,1)));
Values init;
init.insert(0, Point2(1,1));
init.insert(1, Point2(1,0));
Values expected;
expected.insert(0, Point2(0,0));
expected.insert(1, Point2(1,1.85));
LevenbergMarquardtParams params;
EXPECT(assert_equal(expected, GaussNewtonOptimizer(fg, init).optimize()));
EXPECT(assert_equal(expected, LevenbergMarquardtOptimizer(fg, init, params).optimize()));
EXPECT(assert_equal(expected, DoglegOptimizer(fg, init).optimize()));
auto gn_result = GaussNewtonOptimizer(fg, init).optimize();
auto lm_result = LevenbergMarquardtOptimizer(fg, init, params).optimize();
auto dl_result = DoglegOptimizer(fg, init).optimize();
EXPECT(assert_equal(expected, gn_result, 1e-4));
EXPECT(assert_equal(expected, lm_result, 1e-4));
EXPECT(assert_equal(expected, dl_result, 1e-4));
}
/* ************************************************************************* */
TEST(NonlinearOptimizer, Pose2OptimizationWithHuber) {
NonlinearFactorGraph fg;
fg += PriorFactor<Pose2>(0, Pose2(0,0, 0), noiseModel::Isotropic::Sigma(3,0.1));
fg += BetweenFactor<Pose2>(0, 1, Pose2(0,9, M_PI/2),
noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(0.2),
noiseModel::Isotropic::Sigma(3,1)));
fg += BetweenFactor<Pose2>(0, 1, Pose2(0, 11, M_PI/2),
noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(0.2),
noiseModel::Isotropic::Sigma(3,1)));
fg += BetweenFactor<Pose2>(0, 1, Pose2(0, 10, M_PI/2),
noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(0.2),
noiseModel::Isotropic::Sigma(3,1)));
fg += BetweenFactor<Pose2>(0, 1, Pose2(0,9, 0),
noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(0.2),
noiseModel::Isotropic::Sigma(3,1)));
Values init;
init.insert(0, Pose2(0, 0, 0));
init.insert(1, Pose2(0, 10, M_PI/4));
Values expected;
expected.insert(0, Pose2(0, 0, 0));
expected.insert(1, Pose2(0, 10, 1.45212));
LevenbergMarquardtParams params;
auto gn_result = GaussNewtonOptimizer(fg, init).optimize();
auto lm_result = LevenbergMarquardtOptimizer(fg, init, params).optimize();
auto dl_result = DoglegOptimizer(fg, init).optimize();
EXPECT(assert_equal(expected, gn_result, 1e-1));
EXPECT(assert_equal(expected, lm_result, 1e-1));
EXPECT(assert_equal(expected, dl_result, 1e-1));
}
/* ************************************************************************* */
TEST(NonlinearOptimizer, RobustMeanCalculation) {
NonlinearFactorGraph fg;
Values init;
Values expected;
auto huber = noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(20),
noiseModel::Isotropic::Sigma(1, 1));
vector<double> pts{-10,-3,-1,1,3,10,1000};
for(auto pt : pts) {
fg += PriorFactor<double>(0, pt, huber);
}
init.insert(0, 100.0);
expected.insert(0, 3.33333333);
LevenbergMarquardtParams params;
auto gn_result = GaussNewtonOptimizer(fg, init).optimize();
auto lm_result = LevenbergMarquardtOptimizer(fg, init, params).optimize();
auto dl_result = DoglegOptimizer(fg, init).optimize();
EXPECT(assert_equal(expected, gn_result, tol));
EXPECT(assert_equal(expected, lm_result, tol));
EXPECT(assert_equal(expected, dl_result, tol));
}
/* ************************************************************************* */