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

Merge branch 'develop' into improved-hybrid-api

release/4.3a0
Varun Agrawal 2024-09-05 10:04:49 -04:00
parent 8214de04d1 232fa02b19
commit c7e1c60224
17 changed files with 212 additions and 239 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
CMakeLists.txt
View File

@ -1,4 +1,16 @@
cmake_minimum_required(VERSION 3.0)
cmake_minimum_required(VERSION 3.5)
if (POLICY CMP0082)
cmake_policy(SET CMP0082 NEW) # install from sub-directories immediately
endif()
if (POLICY CMP0102)
cmake_policy(SET CMP0102 NEW) # set policy on advanced variables and cmake cache
endif()
if (POLICY CMP0156)
cmake_policy(SET CMP0156 NEW) # new linker strategies
endif()
if (POLICY CMP0167)
cmake_policy(SET CMP0167 OLD) # Don't complain about boost
endif()
# Set the version number for the library
set (GTSAM_VERSION_MAJOR 4)

1
CppUnitLite/CMakeLists.txt
View File

@ -6,6 +6,7 @@ file(GLOB cppunitlite_src "*.cpp")
add_library(CppUnitLite STATIC ${cppunitlite_src} ${cppunitlite_headers})
list(APPEND GTSAM_EXPORTED_TARGETS CppUnitLite)
set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)
target_compile_features(CppUnitLite PUBLIC ${GTSAM_COMPILE_FEATURES_PUBLIC})
gtsam_assign_source_folders("${cppunitlite_headers};${cppunitlite_src}") # MSVC project structure

2
cmake/HandleBoost.cmake
View File

@ -25,7 +25,7 @@ endif()
set(BOOST_FIND_MINIMUM_VERSION 1.65)
set(BOOST_FIND_MINIMUM_COMPONENTS serialization system filesystem thread program_options date_time timer chrono regex)
find_package(Boost ${BOOST_FIND_MINIMUM_VERSION} COMPONENTS ${BOOST_FIND_MINIMUM_COMPONENTS})
find_package(Boost ${BOOST_FIND_MINIMUM_VERSION} COMPONENTS ${BOOST_FIND_MINIMUM_COMPONENTS} REQUIRED)
# Required components
if(NOT Boost_SERIALIZATION_LIBRARY OR NOT Boost_SYSTEM_LIBRARY OR NOT Boost_FILESYSTEM_LIBRARY OR

2
cmake/example_cmake_find_gtsam/CMakeLists.txt
View File

@ -1,7 +1,7 @@
# This file shows how to build and link a user project against GTSAM using CMake
###################################################################################
# To create your own project, replace "example" with the actual name of your project
cmake_minimum_required(VERSION 3.0)
cmake_minimum_required(VERSION 3.5)
project(example CXX)
# Find GTSAM, either from a local build, or from a Debian/Ubuntu package.

41
gtsam/hybrid/GaussianMixture.cpp
View File

@ -330,10 +330,10 @@ AlgebraicDecisionTree<Key> GaussianMixture::logProbability(
return DecisionTree<Key, double>(conditionals_, probFunc);
}
/* *******************************************************************************/
AlgebraicDecisionTree<Key> GaussianMixture::errorTree(
/* ************************************************************************* */
double GaussianMixture::conditionalError(
const GaussianConditional::shared_ptr &conditional,
const VectorValues &continuousValues) const {
auto errorFunc = [&](const GaussianConditional::shared_ptr &conditional) {
// Check if valid pointer
if (conditional) {
return conditional->error(continuousValues) + //
@ -341,8 +341,17 @@ AlgebraicDecisionTree<Key> GaussianMixture::errorTree(
} else {
// If not valid, pointer, it means this conditional was pruned,
// so we return maximum error.
// This way the negative exponential will give
// a probability value close to 0.0.
return std::numeric_limits<double>::max();
}
}
/* *******************************************************************************/
AlgebraicDecisionTree<Key> GaussianMixture::errorTree(
const VectorValues &continuousValues) const {
auto errorFunc = [&](const GaussianConditional::shared_ptr &conditional) {
return conditionalError(conditional, continuousValues);
};
DecisionTree<Key, double> error_tree(conditionals_, errorFunc);
return error_tree;
@ -350,33 +359,9 @@ AlgebraicDecisionTree<Key> GaussianMixture::errorTree(
/* *******************************************************************************/
double GaussianMixture::error(const HybridValues &values) const {
// Check if discrete keys in discrete assignment are
// present in the GaussianMixture
KeyVector dKeys = this->discreteKeys_.indices();
bool valid_assignment = false;
for (auto &&kv : values.discrete()) {
if (std::find(dKeys.begin(), dKeys.end(), kv.first) != dKeys.end()) {
valid_assignment = true;
break;
}
}
// The discrete assignment is not valid so we throw an error.
if (!valid_assignment) {
throw std::runtime_error(
"Invalid discrete values in values. Not all discrete keys specified.");
}
// Directly index to get the conditional, no need to build the whole tree.
auto conditional = conditionals_(values.discrete());
if (conditional) {
return conditional->error(values.continuous()) + //
logConstant_ - conditional->logNormalizationConstant();
} else {
// If not valid, pointer, it means this conditional was pruned,
// so we return maximum error.
return std::numeric_limits<double>::max();
}
return conditionalError(conditional, values.continuous());
}
/* *******************************************************************************/

4
gtsam/hybrid/GaussianMixture.h
View File

@ -256,6 +256,10 @@ class GTSAM_EXPORT GaussianMixture
/// Check whether `given` has values for all frontal keys.
bool allFrontalsGiven(const VectorValues &given) const;
/// Helper method to compute the error of a conditional.
double conditionalError(const GaussianConditional::shared_ptr &conditional,
const VectorValues &continuousValues) const;
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
/** Serialization function */
friend class boost::serialization::access;

9
gtsam/hybrid/HybridFactorGraph.cpp
View File

@ -49,15 +49,6 @@ KeySet HybridFactorGraph::discreteKeySet() const {
return keys;
}
/* ************************************************************************* */
std::unordered_map<Key, DiscreteKey> HybridFactorGraph::discreteKeyMap() const {
std::unordered_map<Key, DiscreteKey> result;
for (const DiscreteKey& k : discreteKeys()) {
result[k.first] = k;
}
return result;
}
/* ************************************************************************* */
const KeySet HybridFactorGraph::continuousKeySet() const {
KeySet keys;

3
gtsam/hybrid/HybridFactorGraph.h
View File

@ -69,9 +69,6 @@ class GTSAM_EXPORT HybridFactorGraph : public FactorGraph<Factor> {
/// Get all the discrete keys in the factor graph, as a set of Keys.
KeySet discreteKeySet() const;
/// Get a map from Key to corresponding DiscreteKey.
std::unordered_map<Key, DiscreteKey> discreteKeyMap() const;
/// Get all the continuous keys in the factor graph.
const KeySet continuousKeySet() const;

20
gtsam/hybrid/HybridGaussianFactorGraph.cpp
View File

@ -312,8 +312,8 @@ using Result = std::pair<std::shared_ptr<GaussianConditional>,
GaussianMixtureFactor::sharedFactor>;
/**
* Compute the probability q(μ;m) = exp(-error(μ;m)) * sqrt(det(2π Σ_m)
* from the residual error at the mean μ.
* Compute the probability p(μ;m) = exp(-error(μ;m)) * sqrt(det(2π Σ_m)
* from the residual error ||b||^2 at the mean μ.
* The residual error contains no keys, and only
* depends on the discrete separator if present.
*/
@ -523,19 +523,9 @@ EliminateHybrid(const HybridGaussianFactorGraph &factors,
std::inserter(continuousSeparator, continuousSeparator.begin()));
// Similarly for the discrete separator.
auto discreteKeySet = factors.discreteKeySet();
// Use set-difference to get the difference in keys
KeySet discreteSeparatorSet;
std::set_difference(
discreteKeySet.begin(), discreteKeySet.end(), frontalKeysSet.begin(),
frontalKeysSet.end(),
std::inserter(discreteSeparatorSet, discreteSeparatorSet.begin()));
// Convert from set of keys to set of DiscreteKeys
std::set<DiscreteKey> discreteSeparator;
auto discreteKeyMap = factors.discreteKeyMap();
for (auto key : discreteSeparatorSet) {
discreteSeparator.insert(discreteKeyMap.at(key));
}
// Since we eliminate all continuous variables first,
// the discrete separator will be *all* the discrete keys.
std::set<DiscreteKey> discreteSeparator = factors.discreteKeys();
return hybridElimination(factors, frontalKeys, continuousSeparator,
discreteSeparator);

8
gtsam/hybrid/tests/testGaussianMixtureFactor.cpp
View File

@ -434,12 +434,12 @@ static HybridBayesNet CreateBayesNet(double mu0, double mu1, double sigma0,
/* ************************************************************************* */
/**
* Test a model P(x0)P(z0|x0)P(x1|x0,m1)P(z1|x1)P(m1).
* Test a model P(z0|x0)P(x1|x0,m1)P(z1|x1)P(m1).
*
* P(f01|x1,x0,m1) has different means and same covariance.
*
* Converting to a factor graph gives us
* P(x0)ϕ(x0)ϕ(x1,x0,m1)ϕ(x1)P(m1)
* ϕ(x0)ϕ(x1,x0,m1)ϕ(x1)P(m1)
*
* If we only have a measurement on z0, then
* the probability of m1 should be 0.5/0.5.
@ -488,12 +488,12 @@ TEST(GaussianMixtureFactor, TwoStateModel) {
/* ************************************************************************* */
/**
* Test a model P(x0)P(z0|x0)P(x1|x0,m1)P(z1|x1)P(m1).
* Test a model P(z0|x0)P(x1|x0,m1)P(z1|x1)P(m1).
*
* P(f01|x1,x0,m1) has different means and different covariances.
*
* Converting to a factor graph gives us
* P(x0)ϕ(x0)ϕ(x1,x0,m1)ϕ(x1)P(m1)
* ϕ(x0)ϕ(x1,x0,m1)ϕ(x1)P(m1)
*
* If we only have a measurement on z0, then
* the P(m1) should be 0.5/0.5.

150
gtsam/hybrid/tests/testMixtureFactor.cpp
View File

@ -118,156 +118,6 @@ TEST(MixtureFactor, Dim) {
EXPECT_LONGS_EQUAL(1, mixtureFactor.dim());
}
/* ************************************************************************* */
// Test components with differing means
TEST(MixtureFactor, DifferentMeans) {
DiscreteKey m1(M(1), 2), m2(M(2), 2);
Values values;
double x1 = 0.0, x2 = 1.75, x3 = 2.60;
values.insert(X(1), x1);
values.insert(X(2), x2);
values.insert(X(3), x3);
auto model0 = noiseModel::Isotropic::Sigma(1, 1e-0);
auto model1 = noiseModel::Isotropic::Sigma(1, 1e-0);
auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-0);
auto f0 = std::make_shared<BetweenFactor<double>>(X(1), X(2), 0.0, model0);
auto f1 = std::make_shared<BetweenFactor<double>>(X(1), X(2), 2.0, model1);
std::vector<NonlinearFactor::shared_ptr> factors{f0, f1};
MixtureFactor mixtureFactor({X(1), X(2)}, {m1}, factors);
HybridNonlinearFactorGraph hnfg;
hnfg.push_back(mixtureFactor);
f0 = std::make_shared<BetweenFactor<double>>(X(2), X(3), 0.0, model0);
f1 = std::make_shared<BetweenFactor<double>>(X(2), X(3), 2.0, model1);
std::vector<NonlinearFactor::shared_ptr> factors23{f0, f1};
hnfg.push_back(MixtureFactor({X(2), X(3)}, {m2}, factors23));
auto prior = PriorFactor<double>(X(1), x1, prior_noise);
hnfg.push_back(prior);
hnfg.emplace_shared<PriorFactor<double>>(X(2), 2.0, prior_noise);
auto hgfg = hnfg.linearize(values);
auto bn = hgfg->eliminateSequential();
HybridValues actual = bn->optimize();
HybridValues expected(
VectorValues{
{X(1), Vector1(0.0)}, {X(2), Vector1(0.25)}, {X(3), Vector1(-0.6)}},
DiscreteValues{{M(1), 1}, {M(2), 0}});
EXPECT(assert_equal(expected, actual));
{
DiscreteValues dv{{M(1), 0}, {M(2), 0}};
VectorValues cont = bn->optimize(dv);
double error = bn->error(HybridValues(cont, dv));
// regression
EXPECT_DOUBLES_EQUAL(1.77418393408, error, 1e-9);
}
{
DiscreteValues dv{{M(1), 0}, {M(2), 1}};
VectorValues cont = bn->optimize(dv);
double error = bn->error(HybridValues(cont, dv));
// regression
EXPECT_DOUBLES_EQUAL(1.77418393408, error, 1e-9);
}
{
DiscreteValues dv{{M(1), 1}, {M(2), 0}};
VectorValues cont = bn->optimize(dv);
double error = bn->error(HybridValues(cont, dv));
// regression
EXPECT_DOUBLES_EQUAL(1.10751726741, error, 1e-9);
}
{
DiscreteValues dv{{M(1), 1}, {M(2), 1}};
VectorValues cont = bn->optimize(dv);
double error = bn->error(HybridValues(cont, dv));
// regression
EXPECT_DOUBLES_EQUAL(1.10751726741, error, 1e-9);
}
}
/* ************************************************************************* */
// Test components with differing covariances
TEST(MixtureFactor, DifferentCovariances) {
DiscreteKey m1(M(1), 2);
Values values;
double x1 = 1.0, x2 = 1.0;
values.insert(X(1), x1);
values.insert(X(2), x2);
double between = 0.0;
auto model0 = noiseModel::Isotropic::Sigma(1, 1e2);
auto model1 = noiseModel::Isotropic::Sigma(1, 1e-2);
auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-3);
auto f0 =
std::make_shared<BetweenFactor<double>>(X(1), X(2), between, model0);
auto f1 =
std::make_shared<BetweenFactor<double>>(X(1), X(2), between, model1);
std::vector<NonlinearFactor::shared_ptr> factors{f0, f1};
// Create via toFactorGraph
using symbol_shorthand::Z;
Matrix H0_1, H0_2, H1_1, H1_2;
Vector d0 = f0->evaluateError(x1, x2, &H0_1, &H0_2);
std::vector<std::pair<Key, Matrix>> terms0 = {{Z(1), gtsam::I_1x1 /*Rx*/},
//
{X(1), H0_1 /*Sp1*/},
{X(2), H0_2 /*Tp2*/}};
Vector d1 = f1->evaluateError(x1, x2, &H1_1, &H1_2);
std::vector<std::pair<Key, Matrix>> terms1 = {{Z(1), gtsam::I_1x1 /*Rx*/},
//
{X(1), H1_1 /*Sp1*/},
{X(2), H1_2 /*Tp2*/}};
auto gm = new gtsam::GaussianMixture(
{Z(1)}, {X(1), X(2)}, {m1},
{std::make_shared<GaussianConditional>(terms0, 1, -d0, model0),
std::make_shared<GaussianConditional>(terms1, 1, -d1, model1)});
gtsam::HybridBayesNet bn;
bn.emplace_back(gm);
gtsam::VectorValues measurements;
measurements.insert(Z(1), gtsam::Z_1x1);
// Create FG with single GaussianMixtureFactor
HybridGaussianFactorGraph mixture_fg = bn.toFactorGraph(measurements);
// Linearized prior factor on X1
auto prior = PriorFactor<double>(X(1), x1, prior_noise).linearize(values);
mixture_fg.push_back(prior);
auto hbn = mixture_fg.eliminateSequential();
VectorValues cv;
cv.insert(X(1), Vector1(0.0));
cv.insert(X(2), Vector1(0.0));
// P(m1) = [0.5, 0.5], so we should pick 0
DiscreteValues dv;
dv.insert({M(1), 0});
HybridValues expected_values(cv, dv);
HybridValues actual_values = hbn->optimize();
EXPECT(assert_equal(expected_values, actual_values));
// Check that we get different error values at the MLE point μ.
AlgebraicDecisionTree<Key> errorTree = hbn->errorTree(cv);
HybridValues hv0(cv, DiscreteValues{{M(1), 0}});
HybridValues hv1(cv, DiscreteValues{{M(1), 1}});
AlgebraicDecisionTree<Key> expectedErrorTree(m1, 9.90348755254,
0.69314718056);
EXPECT(assert_equal(expectedErrorTree, errorTree));
}
/* ************************************************************************* */
int main() {
TestResult tr;

6
gtsam/linear/IterativeSolver.cpp
View File

@ -46,6 +46,12 @@ void IterativeOptimizationParameters::print(ostream &os) const {
<< verbosityTranslator(verbosity_) << endl;
}
/*****************************************************************************/
bool IterativeOptimizationParameters::equals(
const IterativeOptimizationParameters &other, double tol) const {
return verbosity_ == other.verbosity();
}
/*****************************************************************************/
ostream& operator<<(ostream &os, const IterativeOptimizationParameters &p) {
p.print(os);

12
gtsam/linear/IterativeSolver.h
View File

@ -41,13 +41,12 @@ class VectorValues;
* parameters for iterative linear solvers
*/
class IterativeOptimizationParameters {
public:
typedef std::shared_ptr<IterativeOptimizationParameters> shared_ptr;
enum Verbosity {
SILENT = 0, COMPLEXITY, ERROR
} verbosity_;
enum Verbosity { SILENT = 0, COMPLEXITY, ERROR };
protected:
Verbosity verbosity_;
public:
@ -71,6 +70,9 @@ public:
/* virtual print function */
GTSAM_EXPORT virtual void print(std::ostream &os) const;
GTSAM_EXPORT virtual bool equals(const IterativeOptimizationParameters &other,
double tol = 1e-9) const;
/* for serialization */
GTSAM_EXPORT friend std::ostream &operator<<(
std::ostream &os, const IterativeOptimizationParameters &p);

6
gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
View File

@ -236,9 +236,9 @@ bool LevenbergMarquardtOptimizer::tryLambda(const GaussianFactorGraph& linear,
if (currentState->iterations == 0) {
cout << "iter cost cost_change lambda success iter_time" << endl;
}
cout << setw(4) << currentState->iterations << " " << setw(8) << newError << " " << setw(3) << setprecision(2)
<< costChange << " " << setw(3) << setprecision(2) << currentState->lambda << " " << setw(4)
<< systemSolvedSuccessfully << " " << setw(3) << setprecision(2) << iterationTime << endl;
cout << setw(4) << currentState->iterations << " " << setw(12) << newError << " " << setw(12) << setprecision(2)
<< costChange << " " << setw(10) << setprecision(2) << currentState->lambda << " " << setw(6)
<< systemSolvedSuccessfully << " " << setw(10) << setprecision(2) << iterationTime << endl;
}
if (step_is_successful) {
// we have successfully decreased the cost and we have good modelFidelity

22
gtsam/nonlinear/NonlinearOptimizerParams.cpp
View File

@ -123,6 +123,28 @@ void NonlinearOptimizerParams::print(const std::string& str) const {
std::cout.flush();
}
/* ************************************************************************* */
bool NonlinearOptimizerParams::equals(const NonlinearOptimizerParams& other,
double tol) const {
// Check for equality of shared ptrs
bool iterative_params_equal = iterativeParams == other.iterativeParams;
// Check equality of components
if (iterativeParams && other.iterativeParams) {
iterative_params_equal = iterativeParams->equals(*other.iterativeParams);
} else {
// Check if either is null. If both are null, then true
iterative_params_equal = !iterativeParams && !other.iterativeParams;
}
return maxIterations == other.getMaxIterations() &&
std::abs(relativeErrorTol - other.getRelativeErrorTol()) <= tol &&
std::abs(absoluteErrorTol - other.getAbsoluteErrorTol()) <= tol &&
std::abs(errorTol - other.getErrorTol()) <= tol &&
verbosityTranslator(verbosity) == other.getVerbosity() &&
orderingType == other.orderingType && ordering == other.ordering &&
linearSolverType == other.linearSolverType && iterative_params_equal;
}
/* ************************************************************************* */
std::string NonlinearOptimizerParams::linearSolverTranslator(
LinearSolverType linearSolverType) const {

11
gtsam/nonlinear/NonlinearOptimizerParams.h
View File

@ -114,16 +114,7 @@ public:
virtual void print(const std::string& str = "") const;
bool equals(const NonlinearOptimizerParams& other, double tol = 1e-9) const {
return maxIterations == other.getMaxIterations()
&& std::abs(relativeErrorTol - other.getRelativeErrorTol()) <= tol
&& std::abs(absoluteErrorTol - other.getAbsoluteErrorTol()) <= tol
&& std::abs(errorTol - other.getErrorTol()) <= tol
&& verbosityTranslator(verbosity) == other.getVerbosity();
// && orderingType.equals(other.getOrderingType()_;
// && linearSolverType == other.getLinearSolverType();
// TODO: check ordering, iterativeParams, and iterationsHook
}
bool equals(const NonlinearOptimizerParams& other, double tol = 1e-9) const;
inline bool isMultifrontal() const {
return (linearSolverType == MULTIFRONTAL_CHOLESKY)

122
python/gtsam/examples/SelfCalibrationExample.py Normal file
View File

@ -0,0 +1,122 @@
# pylint: disable=unused-import,consider-using-from-import,invalid-name,no-name-in-module,no-member,missing-function-docstring,too-many-locals
"""
Transcription of SelfCalibrationExample.cpp
"""
import math
from gtsam import Cal3_S2
from gtsam.noiseModel import Diagonal, Isotropic
# SFM-specific factors
from gtsam import GeneralSFMFactor2Cal3_S2 # does calibration !
from gtsam import PinholeCameraCal3_S2
# Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y).
from gtsam import Point2
from gtsam import Point3, Pose3, Rot3
# Inference and optimization
from gtsam import NonlinearFactorGraph, DoglegOptimizer, Values
from gtsam.symbol_shorthand import K, L, X
# this is a direct translation of examples/SFMData.h
# which is slightly different from python/gtsam/examples/SFMdata.py.
def createPoints() -> list[Point3]:
"""
Create the set of ground-truth landmarks
"""
return [
Point3(10.0, 10.0, 10.0),
Point3(-10.0, 10.0, 10.0),
Point3(-10.0, -10.0, 10.0),
Point3(10.0, -10.0, 10.0),
Point3(10.0, 10.0, -10.0),
Point3(-10.0, 10.0, -10.0),
Point3(-10.0, -10.0, -10.0),
Point3(10.0, -10.0, -10.0),
]
def createPoses(
init: Pose3 = Pose3(Rot3.Ypr(math.pi / 2, 0, -math.pi / 2), Point3(30, 0, 0)),
delta: Pose3 = Pose3(
Rot3.Ypr(0, -math.pi / 4, 0),
Point3(math.sin(math.pi / 4) * 30, 0, 30 * (1 - math.sin(math.pi / 4))),
),
steps: int = 8,
) -> list[Pose3]:
"""
Create the set of ground-truth poses
Default values give a circular trajectory,
radius 30 at pi/4 intervals, always facing the circle center
"""
poses: list[Pose3] = []
poses.append(init)
for i in range(1, steps):
poses.append(poses[i - 1].compose(delta))
return poses
def main() -> None:
# Create the set of ground-truth
points: list[Point3] = createPoints()
poses: list[Pose3] = createPoses()
# Create the factor graph
graph = NonlinearFactorGraph()
# Add a prior on pose x1.
# 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
poseNoise = Diagonal.Sigmas([0.1, 0.1, 0.1, 0.3, 0.3, 0.3])
graph.addPriorPose3(X(0), poses[0], poseNoise)
# Simulated measurements from each camera pose, adding them to the factor graph
Kcal = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)
measurementNoise = Isotropic.Sigma(2, 1.0)
for i, pose in enumerate(poses):
for j, point in enumerate(points):
camera = PinholeCameraCal3_S2(pose, Kcal)
measurement: Point2 = camera.project(point)
# The only real difference with the Visual SLAM example is that here we
# use a different factor type, that also calculates the Jacobian with
# respect to calibration
graph.add(
GeneralSFMFactor2Cal3_S2(
measurement,
measurementNoise,
X(i),
L(j),
K(0),
)
)
# Add a prior on the position of the first landmark.
pointNoise = Isotropic.Sigma(3, 0.1)
graph.addPriorPoint3(L(0), points[0], pointNoise) # add directly to graph
# Add a prior on the calibration.
calNoise = Diagonal.Sigmas([500, 500, 0.1, 100, 100])
graph.addPriorCal3_S2(K(0), Kcal, calNoise)
# Create the initial estimate to the solution
# now including an estimate on the camera calibration parameters
initialEstimate = Values()
initialEstimate.insert(K(0), Cal3_S2(60.0, 60.0, 0.0, 45.0, 45.0))
for i, pose in enumerate(poses):
initialEstimate.insert(
X(i),
pose.compose(
Pose3(Rot3.Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))
),
)
for j, point in enumerate(points):
initialEstimate.insert(L(j), point + Point3(-0.25, 0.20, 0.15))
# Optimize the graph and print results
result: Values = DoglegOptimizer(graph, initialEstimate).optimize()
result.print("Final results:\n")
if __name__ == "__main__":
main()