Merge branch 'develop' into working-hybrid

release/4.3a0
Varun Agrawal 2024-08-26 16:33:12 -04:00
commit f636212fec
14 changed files with 295 additions and 27 deletions

View File

@ -68,7 +68,7 @@ namespace gtsam {
return fromAngle(theta * degree);
}
/// Named constructor from cos(theta),sin(theta) pair, will *not* normalize!
/// Named constructor from cos(theta),sin(theta) pair
static Rot2 fromCosSin(double c, double s);
/**

View File

@ -375,7 +375,8 @@ virtual class JacobianFactor : gtsam::GaussianFactor {
void serialize() const;
};
pair<gtsam::GaussianConditional, gtsam::JacobianFactor*> EliminateQR(const gtsam::GaussianFactorGraph& factors, const gtsam::Ordering& keys);
pair<gtsam::GaussianConditional*, gtsam::JacobianFactor*> EliminateQR(
const gtsam::GaussianFactorGraph& factors, const gtsam::Ordering& keys);
#include <gtsam/linear/HessianFactor.h>
virtual class HessianFactor : gtsam::GaussianFactor {

View File

@ -424,6 +424,11 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors,
ISAM2Result result(params_.enableDetailedResults);
UpdateImpl update(params_, updateParams);
// Initialize any new variables \Theta_{new} and add
// \Theta:=\Theta\cup\Theta_{new}.
// Needed before delta update if using Dogleg optimizer.
addVariables(newTheta, result.details());
// Update delta if we need it to check relinearization later
if (update.relinarizationNeeded(update_count_))
updateDelta(updateParams.forceFullSolve);
@ -435,9 +440,7 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors,
update.computeUnusedKeys(newFactors, variableIndex_,
result.keysWithRemovedFactors, &result.unusedKeys);
// 2. Initialize any new variables \Theta_{new} and add
// \Theta:=\Theta\cup\Theta_{new}.
addVariables(newTheta, result.details());
// 2. Compute new error to check for relinearization
if (params_.evaluateNonlinearError)
update.error(nonlinearFactors_, calculateEstimate(), &result.errorBefore);
@ -731,6 +734,7 @@ void ISAM2::updateDelta(bool forceFullSolve) const {
effectiveWildfireThreshold, &delta_);
deltaReplacedMask_.clear();
gttoc(Wildfire_update);
} else if (std::holds_alternative<ISAM2DoglegParams>(params_.optimizationParams)) {
// If using Dogleg, do a Dogleg step
const ISAM2DoglegParams& doglegParams =
@ -769,9 +773,8 @@ void ISAM2::updateDelta(bool forceFullSolve) const {
gttic(Copy_dx_d);
// Update Delta and linear step
doglegDelta_ = doglegResult.delta;
delta_ =
doglegResult
.dx_d; // Copy the VectorValues containing with the linear solution
// Copy the VectorValues containing with the linear solution
delta_ = doglegResult.dx_d;
gttoc(Copy_dx_d);
} else {
throw std::runtime_error("iSAM2: unknown ISAM2Params type");

View File

@ -42,6 +42,8 @@ namespace gtsam
// Gather all keys
KeySet allKeys;
for(const std::shared_ptr<FACTOR>& factor: factors) {
// Non-active factors are nullptr
if (factor)
allKeys.insert(factor->begin(), factor->end());
}

View File

@ -671,6 +671,21 @@ class AHRS {
//void print(string s) const;
};
#include <gtsam_unstable/slam/PartialPriorFactor.h>
template <T = {gtsam::Pose2, gtsam::Pose3}>
virtual class PartialPriorFactor : gtsam::NoiseModelFactor {
PartialPriorFactor(gtsam::Key key, size_t idx, double prior,
const gtsam::noiseModel::Base* model);
PartialPriorFactor(gtsam::Key key, const std::vector<size_t>& indices,
const gtsam::Vector& prior,
const gtsam::noiseModel::Base* model);
// enabling serialization functionality
void serialize() const;
const gtsam::Vector& prior();
};
// Tectonic SAM Factors
#include <gtsam_unstable/slam/TSAMFactors.h>

View File

@ -50,9 +50,6 @@ namespace gtsam {
Vector prior_; ///< Measurement on tangent space parameters, in compressed form.
std::vector<size_t> indices_; ///< Indices of the measured tangent space parameters.
/** default constructor - only use for serialization */
PartialPriorFactor() {}
/**
* constructor with just minimum requirements for a factor - allows more
* computation in the constructor. This should only be used by subclasses
@ -65,7 +62,8 @@ namespace gtsam {
// Provide access to the Matrix& version of evaluateError:
using Base::evaluateError;
~PartialPriorFactor() override {}
/** default constructor - only use for serialization */
PartialPriorFactor() {}
/** Single Element Constructor: Prior on a single parameter at index 'idx' in the tangent vector.*/
PartialPriorFactor(Key key, size_t idx, double prior, const SharedNoiseModel& model) :
@ -85,6 +83,8 @@ namespace gtsam {
assert(model->dim() == (size_t)prior.size());
}
~PartialPriorFactor() override {}
/// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override {
return std::static_pointer_cast<gtsam::NonlinearFactor>(

View File

@ -29,11 +29,8 @@ if(POLICY CMP0057)
cmake_policy(SET CMP0057 NEW)
endif()
# Prefer system pybind11 first, if not found, rely on bundled version:
find_package(pybind11 CONFIG QUIET)
if (NOT pybind11_FOUND)
# Use bundled pybind11 version
add_subdirectory(${PROJECT_SOURCE_DIR}/wrap/pybind11 pybind11)
endif()
# Set the wrapping script variable
set(PYBIND_WRAP_SCRIPT "${PROJECT_SOURCE_DIR}/wrap/scripts/pybind_wrap.py")
@ -189,6 +186,8 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON)
gtsam::BinaryMeasurementsPoint3
gtsam::BinaryMeasurementsUnit3
gtsam::BinaryMeasurementsRot3
gtsam::SimWall2DVector
gtsam::SimPolygon2DVector
gtsam::CameraSetCal3_S2
gtsam::CameraSetCal3Bundler
gtsam::CameraSetCal3Unified
@ -264,11 +263,18 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON)
endif()
# Add custom target so we can install with `make python-install`
set(GTSAM_PYTHON_INSTALL_TARGET python-install)
add_custom_target(${GTSAM_PYTHON_INSTALL_TARGET}
if (NOT WIN32) # WIN32=1 is target platform is Windows
add_custom_target(python-install
COMMAND stubgen -q -p gtsam -o ./stubs && cp -a stubs/gtsam/ gtsam && ${PYTHON_EXECUTABLE} -m pip install --user .
DEPENDS ${GTSAM_PYTHON_DEPENDENCIES}
WORKING_DIRECTORY ${GTSAM_PYTHON_BUILD_DIRECTORY})
else()
#TODO(Varun) Find equivalent cp on Windows
add_custom_target(python-install
COMMAND ${PYTHON_EXECUTABLE} -m pip install --user .
DEPENDS ${GTSAM_PYTHON_DEPENDENCIES}
WORKING_DIRECTORY ${GTSAM_PYTHON_BUILD_DIRECTORY})
endif()
# Custom make command to run all GTSAM Python tests
add_custom_target(

View File

@ -1,2 +1,3 @@
-r requirements.txt
pyparsing>=2.4.2
mypy==1.4.1 #TODO(Varun) A bug in mypy>=1.5.0 causes an unresolved placeholder error when importing numpy>=2.0.0 (https://github.com/python/mypy/issues/17396)

View File

@ -17,7 +17,7 @@
| InverseKinematicsExampleExpressions.cpp | |
| ISAM2Example_SmartFactor | |
| ISAM2_SmartFactorStereo_IMU | |
| LocalizationExample | impossible? |
| LocalizationExample | :heavy_check_mark: |
| METISOrderingExample | |
| OdometryExample | :heavy_check_mark: |
| PlanarSLAMExample | :heavy_check_mark: |

View File

@ -0,0 +1,68 @@
"""
A simple 2D pose slam example with "GPS" measurements
- The robot moves forward 2 meter each iteration
- The robot initially faces along the X axis (horizontal, to the right in 2D)
- We have full odometry between pose
- We have "GPS-like" measurements implemented with a custom factor
"""
import numpy as np
import gtsam
from gtsam import BetweenFactorPose2, Pose2, noiseModel
from gtsam_unstable import PartialPriorFactorPose2
def main():
# 1. Create a factor graph container and add factors to it.
graph = gtsam.NonlinearFactorGraph()
# 2a. Add odometry factors
# For simplicity, we will use the same noise model for each odometry factor
odometryNoise = noiseModel.Diagonal.Sigmas(np.asarray([0.2, 0.2, 0.1]))
# Create odometry (Between) factors between consecutive poses
graph.push_back(
BetweenFactorPose2(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise))
graph.push_back(
BetweenFactorPose2(2, 3, Pose2(2.0, 0.0, 0.0), odometryNoise))
# 2b. Add "GPS-like" measurements
# We will use PartialPrior factor for this.
unaryNoise = noiseModel.Diagonal.Sigmas(np.array([0.1,
0.1])) # 10cm std on x,y
graph.push_back(
PartialPriorFactorPose2(1, [0, 1], np.asarray([0.0, 0.0]), unaryNoise))
graph.push_back(
PartialPriorFactorPose2(2, [0, 1], np.asarray([2.0, 0.0]), unaryNoise))
graph.push_back(
PartialPriorFactorPose2(3, [0, 1], np.asarray([4.0, 0.0]), unaryNoise))
graph.print("\nFactor Graph:\n")
# 3. Create the data structure to hold the initialEstimate estimate to the solution
# For illustrative purposes, these have been deliberately set to incorrect values
initialEstimate = gtsam.Values()
initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2))
initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2))
initialEstimate.insert(3, Pose2(4.1, 0.1, 0.1))
initialEstimate.print("\nInitial Estimate:\n")
# 4. Optimize using Levenberg-Marquardt optimization. The optimizer
# accepts an optional set of configuration parameters, controlling
# things like convergence criteria, the type of linear system solver
# to use, and the amount of information displayed during optimization.
# Here we will use the default set of parameters. See the
# documentation for the full set of parameters.
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate)
result = optimizer.optimize()
result.print("Final Result:\n")
# 5. Calculate and print marginal covariances for all variables
marginals = gtsam.Marginals(graph, result)
print("x1 covariance:\n", marginals.marginalCovariance(1))
print("x2 covariance:\n", marginals.marginalCovariance(2))
print("x3 covariance:\n", marginals.marginalCovariance(3))
if __name__ == "__main__":
main()

View File

@ -9,6 +9,7 @@
#include <pybind11/eigen.h>
#include <pybind11/stl_bind.h>
#include <pybind11/stl.h>
#include <pybind11/pybind11.h>
#include <pybind11/functional.h>
#include <pybind11/iostream.h>

View File

@ -13,6 +13,7 @@ package_data = {
"./*.so",
"./*.dll",
"./*.pyd",
"*.pyi", "**/*.pyi", # Add the type hints
]
}

View File

@ -24,10 +24,9 @@
#include <gtsam/nonlinear/DoglegOptimizerImpl.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/GaussianBayesTree.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/slam/SmartProjectionPoseFactor.h>
#include "examples/SFMdata.h"
#include <functional>
@ -36,7 +35,6 @@ using namespace gtsam;
// Convenience for named keys
using symbol_shorthand::X;
using symbol_shorthand::L;
/* ************************************************************************* */
TEST(DoglegOptimizer, ComputeBlend) {
@ -185,6 +183,128 @@ TEST(DoglegOptimizer, Constraint) {
#endif
}
/* ************************************************************************* */
/**
* Test created to fix issue in ISAM2 when using the DogLegOptimizer.
* Originally reported by kvmanohar22 in issue #301
* https://github.com/borglab/gtsam/issues/301
*
* This test is based on a script provided by kvmanohar22
* to help reproduce the issue.
*/
TEST(DogLegOptimizer, VariableUpdate) {
// Make the typename short so it looks much cleaner
typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor;
// create a typedef to the camera type
typedef PinholePose<Cal3_S2> Camera;
// Define the camera calibration parameters
Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
// Define the camera observation noise model
noiseModel::Isotropic::shared_ptr measurementNoise =
noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
// Create the set of ground-truth landmarks and poses
vector<Point3> points = createPoints();
vector<Pose3> poses = createPoses();
// Create a factor graph
NonlinearFactorGraph graph;
ISAM2DoglegParams doglegparams = ISAM2DoglegParams();
doglegparams.verbose = false;
ISAM2Params isam2_params;
isam2_params.evaluateNonlinearError = true;
isam2_params.relinearizeThreshold = 0.0;
isam2_params.enableRelinearization = true;
isam2_params.optimizationParams = doglegparams;
isam2_params.relinearizeSkip = 1;
ISAM2 isam2(isam2_params);
// Simulated measurements from each camera pose, adding them to the factor
// graph
unordered_map<int, SmartFactor::shared_ptr> smart_factors;
for (size_t j = 0; j < points.size(); ++j) {
// every landmark represent a single landmark, we use shared pointer to init
// the factor, and then insert measurements.
SmartFactor::shared_ptr smartfactor(new SmartFactor(measurementNoise, K));
for (size_t i = 0; i < poses.size(); ++i) {
// generate the 2D measurement
Camera camera(poses[i], K);
Point2 measurement = camera.project(points[j]);
// call add() function to add measurement into a single factor, here we
// need to add:
// 1. the 2D measurement
// 2. the corresponding camera's key
// 3. camera noise model
// 4. camera calibration
// add only first 3 measurements and update the later measurements
// incrementally
if (i < 3) smartfactor->add(measurement, i);
}
// insert the smart factor in the graph
smart_factors[j] = smartfactor;
graph.push_back(smartfactor);
}
// Add a prior on pose x0. This indirectly specifies where the origin is.
// 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas(
(Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished());
graph.emplace_shared<PriorFactor<Pose3> >(0, poses[0], noise);
// Because the structure-from-motion problem has a scale ambiguity, the
// problem is still under-constrained. Here we add a prior on the second pose
// x1, so this will fix the scale by indicating the distance between x0 and
// x1. Because these two are fixed, the rest of the poses will be also be
// fixed.
graph.emplace_shared<PriorFactor<Pose3> >(1, poses[1],
noise); // add directly to graph
// Create the initial estimate to the solution
// Intentionally initialize the variables off from the ground truth
Values initialEstimate;
Pose3 delta(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
for (size_t i = 0; i < 3; ++i)
initialEstimate.insert(i, poses[i].compose(delta));
// initialEstimate.print("Initial Estimates:\n");
// Optimize the graph and print results
isam2.update(graph, initialEstimate);
Values result = isam2.calculateEstimate();
// result.print("Results:\n");
// we add new measurements from this pose
size_t pose_idx = 3;
// Now update existing smart factors with new observations
for (size_t j = 0; j < points.size(); ++j) {
SmartFactor::shared_ptr smartfactor = smart_factors[j];
// add the 4th measurement
Camera camera(poses[pose_idx], K);
Point2 measurement = camera.project(points[j]);
smartfactor->add(measurement, pose_idx);
}
graph.resize(0);
initialEstimate.clear();
// update initial estimate for the new pose
initialEstimate.insert(pose_idx, poses[pose_idx].compose(delta));
// this should break the system
isam2.update(graph, initialEstimate);
result = isam2.calculateEstimate();
EXPECT(std::find(result.keys().begin(), result.keys().end(), pose_idx) !=
result.keys().end());
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

View File

@ -994,6 +994,56 @@ TEST(ISAM2, calculate_nnz)
EXPECT_LONGS_EQUAL(expected, actual);
}
class FixActiveFactor : public NoiseModelFactorN<Vector2> {
using Base = NoiseModelFactorN<Vector2>;
bool is_active_;
public:
FixActiveFactor(const gtsam::Key& key, const bool active)
: Base(nullptr, key), is_active_(active) {}
virtual bool active(const gtsam::Values &values) const override {
return is_active_;
}
virtual Vector
evaluateError(const Vector2& x,
Base::OptionalMatrixTypeT<Vector2> H = nullptr) const override {
if (H) {
*H = Vector2::Identity();
}
return Vector2::Zero();
}
};
TEST(ActiveFactorTesting, Issue1596) {
// Issue1596: When a derived Nonlinear Factor is not active, the linearization returns a nullptr (NonlinearFactor.cpp:156), which
// causes an error when `EliminateSymbolic` is called (SymbolicFactor-inst.h:45) due to not checking if the factor is nullptr.
const gtsam::Key key{Symbol('x', 0)};
ISAM2 isam;
Values values;
NonlinearFactorGraph graph;
// Insert an active factor
values.insert<Vector2>(key, Vector2::Zero());
graph.emplace_shared<FixActiveFactor>(key, true);
// No problem here
isam.update(graph, values);
graph = NonlinearFactorGraph();
// Inserting a factor that is never active
graph.emplace_shared<FixActiveFactor>(key, false);
// This call throws the error if the pointer is not validated on (SymbolicFactor-inst.h:45)
isam.update(graph);
// If the bug is fixed, this line is reached.
EXPECT(isam.getFactorsUnsafe().size() == 2);
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */