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

Merge branch 'develop' into feature/nonlinear-hybrid

release/4.3a0
Varun Agrawal 2022-08-03 06:07:12 -04:00
parent 66707792d2 0b9ddb7957
commit 92a586825d
52 changed files with 308 additions and 2516 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -43,46 +43,68 @@ if [ -z ${PYTHON_VERSION+x} ]; then
exit 127
fi
PYTHON="python${PYTHON_VERSION}"
export PYTHON="python${PYTHON_VERSION}"
if [[ $(uname) == "Darwin" ]]; then
function install_dependencies()
{
if [[ $(uname) == "Darwin" ]]; then
brew install wget
else
else
# Install a system package required by our library
sudo apt-get install -y wget libicu-dev python3-pip python3-setuptools
fi
fi
PATH=$PATH:$($PYTHON -c "import site; print(site.USER_BASE)")/bin
export PATH=$PATH:$($PYTHON -c "import site; print(site.USER_BASE)")/bin
[ "${GTSAM_WITH_TBB:-OFF}" = "ON" ] && install_tbb
[ "${GTSAM_WITH_TBB:-OFF}" = "ON" ] && install_tbb
$PYTHON -m pip install -r $GITHUB_WORKSPACE/python/requirements.txt
}
function build()
{
mkdir $GITHUB_WORKSPACE/build
cd $GITHUB_WORKSPACE/build
BUILD_PYBIND="ON"
cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE} \
-DGTSAM_BUILD_TESTS=OFF \
-DGTSAM_BUILD_UNSTABLE=${GTSAM_BUILD_UNSTABLE:-ON} \
-DGTSAM_USE_QUATERNIONS=OFF \
-DGTSAM_WITH_TBB=${GTSAM_WITH_TBB:-OFF} \
-DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF \
-DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \
-DGTSAM_BUILD_PYTHON=${BUILD_PYBIND} \
-DGTSAM_UNSTABLE_BUILD_PYTHON=${GTSAM_BUILD_UNSTABLE:-ON} \
-DGTSAM_PYTHON_VERSION=$PYTHON_VERSION \
-DPYTHON_EXECUTABLE:FILEPATH=$(which $PYTHON) \
-DGTSAM_ALLOW_DEPRECATED_SINCE_V42=OFF \
-DCMAKE_INSTALL_PREFIX=$GITHUB_WORKSPACE/gtsam_install
BUILD_PYBIND="ON"
# Set to 2 cores so that Actions does not error out during resource provisioning.
make -j2 install
sudo $PYTHON -m pip install -r $GITHUB_WORKSPACE/python/requirements.txt
cd $GITHUB_WORKSPACE/build/python
$PYTHON -m pip install --user .
}
mkdir $GITHUB_WORKSPACE/build
cd $GITHUB_WORKSPACE/build
function test()
{
cd $GITHUB_WORKSPACE/python/gtsam/tests
$PYTHON -m unittest discover -v
}
cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE} \
-DGTSAM_BUILD_TESTS=OFF \
-DGTSAM_BUILD_UNSTABLE=${GTSAM_BUILD_UNSTABLE:-ON} \
-DGTSAM_USE_QUATERNIONS=OFF \
-DGTSAM_WITH_TBB=${GTSAM_WITH_TBB:-OFF} \
-DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF \
-DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \
-DGTSAM_BUILD_PYTHON=${BUILD_PYBIND} \
-DGTSAM_UNSTABLE_BUILD_PYTHON=${GTSAM_BUILD_UNSTABLE:-ON} \
-DGTSAM_PYTHON_VERSION=$PYTHON_VERSION \
-DPYTHON_EXECUTABLE:FILEPATH=$(which $PYTHON) \
-DGTSAM_ALLOW_DEPRECATED_SINCE_V42=OFF \
-DCMAKE_INSTALL_PREFIX=$GITHUB_WORKSPACE/gtsam_install
# Set to 2 cores so that Actions does not error out during resource provisioning.
make -j2 install
cd $GITHUB_WORKSPACE/build/python
$PYTHON -m pip install --user .
cd $GITHUB_WORKSPACE/python/gtsam/tests
$PYTHON -m unittest discover -v
# select between build or test
case $1 in
-d)
install_dependencies
;;
-b)
build
;;
-t)
test
;;
esac

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

@ -117,11 +117,12 @@ jobs:
uses: pierotofy/set-swap-space@master
with:
swap-size-gb: 6
- name: Build (Linux)
if: runner.os == 'Linux'
- name: Install Dependencies
run: |
bash .github/scripts/python.sh
- name: Build (macOS)
if: runner.os == 'macOS'
bash .github/scripts/python.sh -d
- name: Build
run: |
bash .github/scripts/python.sh
bash .github/scripts/python.sh -b
- name: Test
run: |
bash .github/scripts/python.sh -t

2347
cmake/FindBoost.cmake
View File

File diff suppressed because it is too large Load Diff

1
cmake/HandlePrintConfiguration.cmake
View File

@ -33,6 +33,7 @@ print_build_options_for_target(gtsam)
print_config("Use System Eigen" "${GTSAM_USE_SYSTEM_EIGEN} (Using version: ${GTSAM_EIGEN_VERSION})")
print_config("Use System Metis" "${GTSAM_USE_SYSTEM_METIS}")
print_config("Using Boost version" "${Boost_VERSION}")
if(GTSAM_USE_TBB)
print_config("Use Intel TBB" "Yes (Version: ${TBB_VERSION})")

2
gtsam/basis/basis.i
View File

@ -137,7 +137,7 @@ class FitBasis {
static gtsam::GaussianFactorGraph::shared_ptr LinearGraph(
const std::map<double, double>& sequence,
const gtsam::noiseModel::Base* model, size_t N);
This::Parameters parameters() const;
gtsam::This::Parameters parameters() const;
};
} // namespace gtsam

8
gtsam/geometry/geometry.i
View File

@ -1126,10 +1126,10 @@ class TriangulationResult {
Status status;
TriangulationResult(const gtsam::Point3& p);
const gtsam::Point3& get() const;
static TriangulationResult Degenerate();
static TriangulationResult Outlier();
static TriangulationResult FarPoint();
static TriangulationResult BehindCamera();
static gtsam::TriangulationResult Degenerate();
static gtsam::TriangulationResult Outlier();
static gtsam::TriangulationResult FarPoint();
static gtsam::TriangulationResult BehindCamera();
bool valid() const;
bool degenerate() const;
bool outlier() const;

5
gtsam/groups.dox
View File

@ -11,4 +11,7 @@
@}
*/
\defgroup SLAM Useful SLAM components
@{ @}
*/

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();

4
gtsam/navigation/CombinedImuFactor.h
View File

@ -123,7 +123,7 @@ public:
* it is received from the IMU) so as to avoid costly integration at time of
* factor construction.
*
* @addtogroup SLAM
* @ingroup SLAM
*/
class GTSAM_EXPORT PreintegratedCombinedMeasurements : public PreintegrationType {
@ -252,7 +252,7 @@ public:
* the correlation between the bias uncertainty and the preintegrated
* measurements uncertainty.
*
* @addtogroup SLAM
* @ingroup SLAM
*/
class GTSAM_EXPORT CombinedImuFactor: public NoiseModelFactor6<Pose3, Vector3, Pose3,
Vector3, imuBias::ConstantBias, imuBias::ConstantBias> {

6
gtsam/navigation/ImuFactor.h
View File

@ -66,7 +66,7 @@ typedef ManifoldPreintegration PreintegrationType;
* as soon as it is received from the IMU) so as to avoid costly integration
* at time of factor construction.
*
* @addtogroup SLAM
* @ingroup SLAM
*/
class GTSAM_EXPORT PreintegratedImuMeasurements: public PreintegrationType {
@ -165,7 +165,7 @@ public:
* (which are usually slowly varying quantities), which is up to the caller.
* See also CombinedImuFactor for a class that does this for you.
*
* @addtogroup SLAM
* @ingroup SLAM
*/
class GTSAM_EXPORT ImuFactor: public NoiseModelFactor5<Pose3, Vector3, Pose3, Vector3,
imuBias::ConstantBias> {
@ -256,7 +256,7 @@ public:
/**
* ImuFactor2 is a ternary factor that uses NavStates rather than Pose/Velocity.
* @addtogroup SLAM
* @ingroup SLAM
*/
class GTSAM_EXPORT ImuFactor2 : public NoiseModelFactor3<NavState, NavState, imuBias::ConstantBias> {
private:

2
gtsam/nonlinear/CustomFactor.h
View File

@ -19,8 +19,6 @@
#include <gtsam/nonlinear/NonlinearFactor.h>
using namespace gtsam;
namespace gtsam {
using JacobianVector = std::vector<Matrix>;

2
gtsam/nonlinear/GncOptimizer.h
View File

@ -42,7 +42,7 @@ static double Chi2inv(const double alpha, const size_t dofs) {
/* ************************************************************************* */
template<class GncParameters>
class GTSAM_EXPORT GncOptimizer {
class GncOptimizer {
public:
/// For each parameter, specify the corresponding optimizer: e.g., GaussNewtonParams -> GaussNewtonOptimizer.
typedef typename GncParameters::OptimizerType BaseOptimizer;

18
gtsam/nonlinear/GncParams.h
View File

@ -39,7 +39,7 @@ enum GncLossType {
};
template<class BaseOptimizerParameters>
class GTSAM_EXPORT GncParams {
class GncParams {
public:
/// For each parameter, specify the corresponding optimizer: e.g., GaussNewtonParams -> GaussNewtonOptimizer.
typedef typename BaseOptimizerParameters::OptimizerType OptimizerType;
@ -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]);
}
@ -163,7 +169,7 @@ class GTSAM_EXPORT GncParams {
std::cout << "knownInliers: " << knownInliers[i] << "\n";
for (size_t i = 0; i < knownOutliers.size(); i++)
std::cout << "knownOutliers: " << knownOutliers[i] << "\n";
baseOptimizerParams.print(str);
baseOptimizerParams.print("Base optimizer params: ");
}
};

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));
}
}
/* ************************************************************************* */

2
gtsam/nonlinear/PriorFactor.h
View File

@ -24,7 +24,7 @@ namespace gtsam {
/**
* A class for a soft prior on any Value type
* @addtogroup SLAM
* @ingroup SLAM
*/
template<class VALUE>
class PriorFactor: public NoiseModelFactor1<VALUE> {

38
gtsam/nonlinear/custom.i Normal file
View File

@ -0,0 +1,38 @@
//*************************************************************************
// Custom Factor wrapping
//*************************************************************************
namespace gtsam {
#include <gtsam/nonlinear/CustomFactor.h>
virtual class CustomFactor : gtsam::NoiseModelFactor {
/*
* Note CustomFactor will not be wrapped for MATLAB, as there is no supporting
* machinery there. This is achieved by adding `gtsam::CustomFactor` to the
* ignore list in `matlab/CMakeLists.txt`.
*/
CustomFactor();
/*
* Example:
* ```
* def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]):
* <calculated error>
* if not H is None:
* <calculate the Jacobian>
* H[0] = J1 # 2-d numpy array for a Jacobian block
* H[1] = J2
* ...
* return error # 1-d numpy array
*
* cf = CustomFactor(noise_model, keys, error_func)
* ```
*/
CustomFactor(const gtsam::SharedNoiseModel& noiseModel,
const gtsam::KeyVector& keys,
const gtsam::CustomErrorFunction& errorFunction);
void print(string s = "",
gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter);
};
}

70
gtsam/nonlinear/nonlinear.i
View File

@ -99,11 +99,11 @@ class NonlinearFactorGraph {
string dot(
const gtsam::Values& values,
const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter,
const GraphvizFormatting& writer = GraphvizFormatting());
const gtsam::GraphvizFormatting& writer = gtsam::GraphvizFormatting());
void saveGraph(
const string& s, const gtsam::Values& values,
const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter,
const GraphvizFormatting& writer = GraphvizFormatting()) const;
const gtsam::GraphvizFormatting& writer = gtsam::GraphvizFormatting()) const;
// enabling serialization functionality
void serialize() const;
@ -135,37 +135,6 @@ virtual class NoiseModelFactor : gtsam::NonlinearFactor {
Vector whitenedError(const gtsam::Values& x) const;
};
#include <gtsam/nonlinear/CustomFactor.h>
virtual class CustomFactor : gtsam::NoiseModelFactor {
/*
* Note CustomFactor will not be wrapped for MATLAB, as there is no supporting
* machinery there. This is achieved by adding `gtsam::CustomFactor` to the
* ignore list in `matlab/CMakeLists.txt`.
*/
CustomFactor();
/*
* Example:
* ```
* def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]):
* <calculated error>
* if not H is None:
* <calculate the Jacobian>
* H[0] = J1 # 2-d numpy array for a Jacobian block
* H[1] = J2
* ...
* return error # 1-d numpy array
*
* cf = CustomFactor(noise_model, keys, error_func)
* ```
*/
CustomFactor(const gtsam::SharedNoiseModel& noiseModel,
const gtsam::KeyVector& keys,
const gtsam::CustomErrorFunction& errorFunction);
void print(string s = "",
gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter);
};
#include <gtsam/nonlinear/Values.h>
class Values {
Values();
@ -544,12 +513,34 @@ virtual class DoglegParams : gtsam::NonlinearOptimizerParams {
};
#include <gtsam/nonlinear/GncParams.h>
enum GncLossType {
GM /*Geman McClure*/,
TLS /*Truncated least squares*/
};
template<PARAMS>
virtual class GncParams {
GncParams(const PARAMS& baseOptimizerParams);
GncParams();
void setVerbosityGNC(const This::Verbosity value);
void print(const string& str) const;
BaseOptimizerParameters baseOptimizerParams;
gtsam::GncLossType lossType;
size_t maxIterations;
double muStep;
double relativeCostTol;
double weightsTol;
Verbosity verbosity;
gtsam::KeyVector knownInliers;
gtsam::KeyVector knownOutliers;
void setLossType(const gtsam::GncLossType type);
void setMaxIterations(const size_t maxIter);
void setMuStep(const double step);
void setRelativeCostTol(double value);
void setWeightsTol(double value);
void setVerbosityGNC(const gtsam::This::Verbosity value);
void setKnownInliers(const gtsam::KeyVector& knownIn);
void setKnownOutliers(const gtsam::KeyVector& knownOut);
void print(const string& str = "GncParams: ") const;
enum Verbosity {
SILENT,
@ -597,6 +588,11 @@ virtual class GncOptimizer {
GncOptimizer(const gtsam::NonlinearFactorGraph& graph,
const gtsam::Values& initialValues,
const PARAMS& params);
void setInlierCostThresholds(const double inth);
const Vector& getInlierCostThresholds();
void setInlierCostThresholdsAtProbability(const double alpha);
void setWeights(const Vector w);
const Vector& getWeights();
gtsam::Values optimize();
};
@ -705,7 +701,7 @@ class ISAM2Result {
/** Getters and Setters for all properties */
size_t getVariablesRelinearized() const;
size_t getVariablesReeliminated() const;
FactorIndices getNewFactorsIndices() const;
gtsam::FactorIndices getNewFactorsIndices() const;
size_t getCliques() const;
double getErrorBefore() const;
double getErrorAfter() const;
@ -873,7 +869,7 @@ template <T = {gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2,
gtsam::PinholeCamera<gtsam::Cal3Unified>,
gtsam::imuBias::ConstantBias}>
virtual class NonlinearEquality2 : gtsam::NoiseModelFactor {
NonlinearEquality2(Key key1, Key key2, double mu = 1e4);
NonlinearEquality2(gtsam::Key key1, gtsam::Key key2, double mu = 1e4);
gtsam::Vector evaluateError(const T& x1, const T& x2);
};

2
gtsam/sam/BearingRangeFactor.h
View File

@ -27,7 +27,7 @@ namespace gtsam {
/**
* Binary factor for a bearing/range measurement
* @addtogroup SLAM
* @ingroup SLAM
*/
template <typename A1, typename A2,
typename B = typename Bearing<A1, A2>::result_type,

2
gtsam/slam/AntiFactor.h
View File

@ -26,7 +26,7 @@ namespace gtsam {
* A class for downdating an existing factor from a graph. The AntiFactor returns the same
* linearized Hessian matrices of the original factor, but with the opposite sign. This effectively
* cancels out any affects of the original factor during optimization."
* @addtogroup SLAM
* @ingroup SLAM
*/
class AntiFactor: public NonlinearFactor {

2
gtsam/slam/BetweenFactor.h
View File

@ -34,7 +34,7 @@ namespace gtsam {
/**
* A class for a measurement predicted by "between(config[key1],config[key2])"
* @tparam VALUE the Value type
* @addtogroup SLAM
* @ingroup SLAM
*/
template<class VALUE>
class BetweenFactor: public NoiseModelFactor2<VALUE, VALUE> {

2
gtsam/slam/BoundingConstraint.h
View File

@ -27,7 +27,7 @@ namespace gtsam {
* greater/less than a fixed threshold. The function
* will need to have its value function implemented to return
* a scalar for comparison.
* @addtogroup SLAM
* @ingroup SLAM
*/
template<class VALUE>
struct BoundingConstraint1: public NoiseModelFactor1<VALUE> {

2
gtsam/slam/EssentialMatrixConstraint.h
View File

@ -25,7 +25,7 @@ namespace gtsam {
/**
* Binary factor between two Pose3 variables induced by an EssentialMatrix measurement
* @addtogroup SLAM
* @ingroup SLAM
*/
class GTSAM_EXPORT EssentialMatrixConstraint: public NoiseModelFactor2<Pose3, Pose3> {

2
gtsam/slam/GeneralSFMFactor.h
View File

@ -54,7 +54,7 @@ namespace gtsam {
/**
* Non-linear factor for a constraint derived from a 2D measurement.
* The calibration is unknown here compared to GenericProjectionFactor
* @addtogroup SLAM
* @ingroup SLAM
*/
template<class CAMERA, class LANDMARK>
class GeneralSFMFactor: public NoiseModelFactor2<CAMERA, LANDMARK> {

2
gtsam/slam/ProjectionFactor.h
View File

@ -33,7 +33,7 @@ namespace gtsam {
* Non-linear factor for a constraint derived from a 2D measurement.
* The calibration is known here.
* The main building block for visual SLAM.
* @addtogroup SLAM
* @ingroup SLAM
*/
template <class POSE = Pose3, class LANDMARK = Point3,
class CALIBRATION = Cal3_S2>

4
gtsam/slam/SmartProjectionPoseFactor.h
View File

@ -24,7 +24,7 @@
namespace gtsam {
/**
*
* @addtogroup SLAM
* @ingroup SLAM
*
* If you are using the factor, please cite:
* L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally
@ -39,7 +39,7 @@ namespace gtsam {
* The factor only constrains poses (variable dimension is 6).
* This factor requires that values contains the involved poses (Pose3).
* If the calibration should be optimized, as well, use SmartProjectionFactor instead!
* @addtogroup SLAM
* @ingroup SLAM
*/
template <class CALIBRATION>
class SmartProjectionPoseFactor

4
gtsam/slam/SmartProjectionRigFactor.h
View File

@ -29,7 +29,7 @@
namespace gtsam {
/**
*
* @addtogroup SLAM
* @ingroup SLAM
*
* If you are using the factor, please cite:
* L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating
@ -46,7 +46,7 @@ namespace gtsam {
* calibration (i.e., are from the same camera), use SmartProjectionPoseFactor
* instead! If the calibration should be optimized, as well, use
* SmartProjectionFactor instead!
* @addtogroup SLAM
* @ingroup SLAM
*/
template <class CAMERA>
class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {

2
gtsam/slam/StereoFactor.h
View File

@ -25,7 +25,7 @@ namespace gtsam {
/**
* A Generic Stereo Factor
* @addtogroup SLAM
* @ingroup SLAM
*/
template<class POSE, class LANDMARK>
class GenericStereoFactor: public NoiseModelFactor2<POSE, LANDMARK> {

2
gtsam/slam/TriangulationFactor.h
View File

@ -27,7 +27,7 @@ namespace gtsam {
/**
* Non-linear factor for a constraint derived from a 2D measurement.
* The calibration and pose are assumed known.
* @addtogroup SLAM
* @ingroup SLAM
*/
template<class CAMERA>
class TriangulationFactor: public NoiseModelFactor1<Point3> {

2
gtsam_unstable/partition/tests/CMakeLists.txt
View File

@ -1,2 +1,2 @@
set(ignore_test "testNestedDissection.cpp")
gtsamAddTestsGlob(partition "test*.cpp" "${ignore_test}" "gtsam_unstable;gtsam;metis-gtsam")
gtsamAddTestsGlob(partition "test*.cpp" "${ignore_test}" "gtsam_unstable;gtsam;metis-gtsam-if")

2
gtsam_unstable/slam/BetweenFactorEM.h
View File

@ -28,7 +28,7 @@ namespace gtsam {
/**
* A class for a measurement predicted by "between(config[key1],config[key2])"
* @tparam VALUE the Value type
* @addtogroup SLAM
* @ingroup SLAM
*/
template<class VALUE>
class BetweenFactorEM: public NonlinearFactor {

2
gtsam_unstable/slam/BiasedGPSFactor.h
View File

@ -25,7 +25,7 @@ namespace gtsam {
/**
* A class to model GPS measurements, including a bias term which models
* common-mode errors and that can be partially corrected if other sensors are used
* @addtogroup SLAM
* @ingroup SLAM
*/
class BiasedGPSFactor: public NoiseModelFactor2<Pose3, Point3> {

2
gtsam_unstable/slam/MultiProjectionFactor.h
View File

@ -29,7 +29,7 @@ namespace gtsam {
/**
* Non-linear factor for a constraint derived from a 2D measurement. The calibration is known here.
* i.e. the main building block for visual SLAM.
* @addtogroup SLAM
* @ingroup SLAM
*/
template<class POSE, class LANDMARK, class CALIBRATION = Cal3_S2>
class MultiProjectionFactor: public NoiseModelFactor {

2
gtsam_unstable/slam/PoseBetweenFactor.h
View File

@ -26,7 +26,7 @@ namespace gtsam {
/**
* A class for a measurement predicted by "between(config[key1],config[key2])"
* @tparam POSE the Pose type
* @addtogroup SLAM
* @ingroup SLAM
*/
template<class POSE>
class PoseBetweenFactor: public NoiseModelFactor2<POSE, POSE> {

2
gtsam_unstable/slam/PosePriorFactor.h
View File

@ -23,7 +23,7 @@ namespace gtsam {
/**
* A class for a soft prior on any Value type
* @addtogroup SLAM
* @ingroup SLAM
*/
template<class POSE>
class PosePriorFactor: public NoiseModelFactor1<POSE> {

2
gtsam_unstable/slam/PoseToPointFactor.h
View File

@ -18,7 +18,7 @@ namespace gtsam {
/**
* A class for a measurement between a pose and a point.
* @addtogroup SLAM
* @ingroup SLAM
*/
template<typename POSE = Pose3, typename POINT = Point3>
class PoseToPointFactor : public NoiseModelFactor2<POSE, POINT> {

2
gtsam_unstable/slam/ProjectionFactorPPP.h
View File

@ -28,7 +28,7 @@ namespace gtsam {
/**
* Non-linear factor for a constraint derived from a 2D measurement. The calibration is known here.
* i.e. the main building block for visual SLAM.
* @addtogroup SLAM
* @ingroup SLAM
*/
template<class POSE, class LANDMARK, class CALIBRATION = Cal3_S2>
class ProjectionFactorPPP: public NoiseModelFactor3<POSE, POSE, LANDMARK> {

2
gtsam_unstable/slam/ProjectionFactorPPPC.h
View File

@ -30,7 +30,7 @@ namespace gtsam {
/**
* Non-linear factor for a constraint derived from a 2D measurement. This factor
* estimates the body pose, body-camera transform, 3D landmark, and calibration.
* @addtogroup SLAM
* @ingroup SLAM
*/
template <class POSE, class LANDMARK, class CALIBRATION = Cal3_S2>
class GTSAM_UNSTABLE_EXPORT ProjectionFactorPPPC

2
gtsam_unstable/slam/ProjectionFactorRollingShutter.h
View File

@ -38,7 +38,7 @@ namespace gtsam {
* define the alpha = (t_p - t_A) / (t_B - t_A), we will use the pose
* interpolated between A and B by the alpha to project the corresponding
* landmark to Point2.
* @addtogroup SLAM
* @ingroup SLAM
*/
class GTSAM_UNSTABLE_EXPORT ProjectionFactorRollingShutter

4
gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h
View File

@ -25,7 +25,7 @@
namespace gtsam {
/**
*
* @addtogroup SLAM
* @ingroup SLAM
*
* If you are using the factor, please cite:
* L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert,
@ -39,7 +39,7 @@ namespace gtsam {
* shutter model of the camera with given readout time. The factor requires that
* values contain (for each pixel observation) two consecutive camera poses from
* which the pixel observation pose can be interpolated.
* @addtogroup SLAM
* @ingroup SLAM
*/
template <class CAMERA>
class GTSAM_UNSTABLE_EXPORT SmartProjectionPoseFactorRollingShutter

2
gtsam_unstable/slam/SmartRangeFactor.h
View File

@ -24,7 +24,7 @@ namespace gtsam {
/**
* Smart factor for range SLAM
* @addtogroup SLAM
* @ingroup SLAM
*/
class SmartRangeFactor: public NoiseModelFactor {
protected:

4
gtsam_unstable/slam/SmartStereoProjectionFactorPP.h
View File

@ -23,7 +23,7 @@
namespace gtsam {
/**
*
* @addtogroup SLAM
* @ingroup SLAM
*
* If you are using the factor, please cite:
* L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert,
@ -38,7 +38,7 @@ namespace gtsam {
* calibration or the same calibration can be shared by multiple cameras. This
* factor requires that values contain the involved poses and extrinsics (both
* are Pose3 variables).
* @addtogroup SLAM
* @ingroup SLAM
*/
class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionFactorPP
: public SmartStereoProjectionFactor {

4
gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h
View File

@ -26,7 +26,7 @@
namespace gtsam {
/**
*
* @addtogroup SLAM
* @ingroup SLAM
*
* If you are using the factor, please cite:
* L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert,
@ -41,7 +41,7 @@ namespace gtsam {
* has its own calibration.
* The factor only constrains poses (variable dimension is 6).
* This factor requires that values contains the involved poses (Pose3).
* @addtogroup SLAM
* @ingroup SLAM
*/
class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionPoseFactor
: public SmartStereoProjectionFactor {

2
gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h
View File

@ -29,7 +29,7 @@ namespace gtsam {
/**
* A class for a measurement predicted by "between(config[key1],config[key2])"
* @tparam VALUE the Value type
* @addtogroup SLAM
* @ingroup SLAM
*/
template<class VALUE>
class TransformBtwRobotsUnaryFactor: public NonlinearFactor { // TODO why not NoiseModelFactor1 ?

2
gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h
View File

@ -31,7 +31,7 @@ namespace gtsam {
/**
* A class for a measurement predicted by "between(config[key1],config[key2])"
* @tparam VALUE the Value type
* @addtogroup SLAM
* @ingroup SLAM
*/
template<class VALUE>
class TransformBtwRobotsUnaryFactorEM: public NonlinearFactor {

2
matlab/CMakeLists.txt
View File

@ -73,6 +73,7 @@ set(interface_files
${GTSAM_SOURCE_DIR}/gtsam/geometry/geometry.i
${GTSAM_SOURCE_DIR}/gtsam/linear/linear.i
${GTSAM_SOURCE_DIR}/gtsam/nonlinear/nonlinear.i
${GTSAM_SOURCE_DIR}/gtsam/nonlinear/custom.i
${GTSAM_SOURCE_DIR}/gtsam/symbolic/symbolic.i
${GTSAM_SOURCE_DIR}/gtsam/sam/sam.i
${GTSAM_SOURCE_DIR}/gtsam/slam/slam.i
@ -98,7 +99,6 @@ endif(GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX)
# Record the root dir for gtsam - needed during external builds, e.g., ROS
set(GTSAM_SOURCE_ROOT_DIR ${GTSAM_SOURCE_DIR})
message(STATUS "GTSAM_SOURCE_ROOT_DIR: [${GTSAM_SOURCE_ROOT_DIR}]")
# Tests message(STATUS "Installing Matlab Toolbox")
install_matlab_scripts("${GTSAM_SOURCE_ROOT_DIR}/matlab/" "*.m;*.fig")

1
python/CMakeLists.txt
View File

@ -61,6 +61,7 @@ set(interface_headers
${PROJECT_SOURCE_DIR}/gtsam/geometry/geometry.i
${PROJECT_SOURCE_DIR}/gtsam/linear/linear.i
${PROJECT_SOURCE_DIR}/gtsam/nonlinear/nonlinear.i
${PROJECT_SOURCE_DIR}/gtsam/nonlinear/custom.i
${PROJECT_SOURCE_DIR}/gtsam/symbolic/symbolic.i
${PROJECT_SOURCE_DIR}/gtsam/sam/sam.i
${PROJECT_SOURCE_DIR}/gtsam/slam/slam.i

12
python/gtsam/preamble/custom.h Normal file
View File

@ -0,0 +1,12 @@
/* Please refer to:
* https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html
* These are required to save one copy operation on Python calls.
*
* NOTES
* =================
*
* `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11
* 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/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++.
*/
*/

12
python/gtsam/specializations/custom.h Normal file
View File

@ -0,0 +1,12 @@
/* Please refer to:
* https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html
* These are required to save one copy operation on Python calls.
*
* NOTES
* =================
*
* `py::bind_vector` and similar machinery gives the std container a Python-like
* 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.
*/

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.
*/
*/

79
python/gtsam/tests/test_NonlinearOptimizer.py
View File

@ -15,10 +15,12 @@ from __future__ import print_function
import unittest
import gtsam
from gtsam import (DoglegOptimizer, DoglegParams, DummyPreconditionerParameters,
GaussNewtonOptimizer, GaussNewtonParams, GncLMParams, GncLMOptimizer,
LevenbergMarquardtOptimizer, LevenbergMarquardtParams, NonlinearFactorGraph,
Ordering, PCGSolverParameters, Point2, PriorFactorPoint2, Values)
from gtsam import (
DoglegOptimizer, DoglegParams, DummyPreconditionerParameters, GaussNewtonOptimizer,
GaussNewtonParams, GncLMParams, GncLossType, GncLMOptimizer, LevenbergMarquardtOptimizer,
LevenbergMarquardtParams, NonlinearFactorGraph, Ordering, PCGSolverParameters, Point2,
PriorFactorPoint2, Values
)
from gtsam.utils.test_case import GtsamTestCase
KEY1 = 1
@ -27,7 +29,6 @@ KEY2 = 2
class TestScenario(GtsamTestCase):
"""Do trivial test with three optimizer variants."""
def setUp(self):
"""Set up the optimization problem and ordering"""
# create graph
@ -83,16 +84,83 @@ class TestScenario(GtsamTestCase):
actual = GncLMOptimizer(self.fg, self.initial_values, gncParams).optimize()
self.assertAlmostEqual(0, self.fg.error(actual))
def test_gnc_params(self):
base_params = LevenbergMarquardtParams()
# Test base params
for base_max_iters in (50, 100):
base_params.setMaxIterations(base_max_iters)
params = GncLMParams(base_params)
self.assertEqual(params.baseOptimizerParams.getMaxIterations(), base_max_iters)
# Test printing
params_str = str(params)
for s in (
"lossType",
"maxIterations",
"muStep",
"relativeCostTol",
"weightsTol",
"verbosity",
):
self.assertTrue(s in params_str)
# Test each parameter
for loss_type in (GncLossType.TLS, GncLossType.GM):
params.setLossType(loss_type) # Default is TLS
self.assertEqual(params.lossType, loss_type)
for max_iter in (1, 10, 100):
params.setMaxIterations(max_iter)
self.assertEqual(params.maxIterations, max_iter)
for mu_step in (1.1, 1.2, 1.5):
params.setMuStep(mu_step)
self.assertEqual(params.muStep, mu_step)
for rel_cost_tol in (1e-5, 1e-6, 1e-7):
params.setRelativeCostTol(rel_cost_tol)
self.assertEqual(params.relativeCostTol, rel_cost_tol)
for weights_tol in (1e-4, 1e-3, 1e-2):
params.setWeightsTol(weights_tol)
self.assertEqual(params.weightsTol, weights_tol)
for i in (0, 1, 2):
verb = GncLMParams.Verbosity(i)
params.setVerbosityGNC(verb)
self.assertEqual(params.verbosity, verb)
for inl in ([], [10], [0, 100]):
params.setKnownInliers(inl)
self.assertEqual(params.knownInliers, inl)
params.knownInliers = []
for out in ([], [1], [0, 10]):
params.setKnownInliers(out)
self.assertEqual(params.knownInliers, out)
params.knownInliers = []
# Test optimizer params
optimizer = GncLMOptimizer(self.fg, self.initial_values, params)
for ict_factor in (0.9, 1.1):
new_ict = ict_factor * optimizer.getInlierCostThresholds()
optimizer.setInlierCostThresholds(new_ict)
self.assertAlmostEqual(optimizer.getInlierCostThresholds(), new_ict)
for w_factor in (0.8, 0.9):
new_weights = w_factor * optimizer.getWeights()
optimizer.setWeights(new_weights)
self.assertAlmostEqual(optimizer.getWeights(), new_weights)
optimizer.setInlierCostThresholdsAtProbability(0.9)
w1 = optimizer.getInlierCostThresholds()
optimizer.setInlierCostThresholdsAtProbability(0.8)
w2 = optimizer.getInlierCostThresholds()
self.assertLess(w2, w1)
def test_iteration_hook(self):
# set up iteration hook to track some testable values
iteration_count = 0
final_error = 0
final_values = None
def iteration_hook(iter, error_before, error_after):
nonlocal iteration_count, final_error, final_values
iteration_count = iter
final_error = error_after
final_values = optimizer.values()
# optimize
params = LevenbergMarquardtParams.CeresDefaults()
params.setOrdering(self.ordering)
@ -104,5 +172,6 @@ class TestScenario(GtsamTestCase):
self.assertEqual(self.fg.error(actual), final_error)
self.assertEqual(optimizer.iterations(), iteration_count)
if __name__ == "__main__":
unittest.main()

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);