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

linear, navigation, std::optional serialization tests

release/4.3a0
kartik arcot 2023-01-12 13:11:55 -08:00
parent 3bac1efd1f
commit ee65c85442
24 changed files with 208 additions and 104 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
examples/SFMExample_SmartFactor.cpp
View File

@ -114,10 +114,10 @@ int main(int argc, char* argv[]) {
// The graph stores Factor shared_ptrs, so we cast back to a SmartFactor first
SmartFactor::shared_ptr smart = boost::dynamic_pointer_cast<SmartFactor>(graph[j]);
if (smart) {
// The output of point() is in boost::optional<Point3>, as sometimes
// The output of point() is in std::optional<Point3>, as sometimes
// the triangulation operation inside smart factor will encounter degeneracy.
auto point = smart->point(result);
if (point) // ignore if boost::optional return nullptr
if (point) // ignore if std::optional return nullptr
landmark_result.insert(j, *point);
}
}

4
examples/SFMExample_SmartFactorPCG.cpp
View File

@ -110,8 +110,8 @@ int main(int argc, char* argv[]) {
for (size_t j = 0; j < points.size(); ++j) {
auto smart = boost::dynamic_pointer_cast<SmartFactor>(graph[j]);
if (smart) {
boost::optional<Point3> point = smart->point(result);
if (point) // ignore if boost::optional return nullptr
std::optional<Point3> point = smart->point(result);
if (point) // ignore if std::optional return nullptr
landmark_result.insert(j, *point);
}
}

7
examples/TriangulationLOSTExample.cpp
View File

@ -29,6 +29,7 @@
#include <chrono>
#include <iostream>
#include <random>
#include <optional>
using namespace std;
using namespace gtsam;
@ -145,9 +146,9 @@ int main(int argc, char* argv[]) {
cameras, noisyMeasurements, rank_tol, true, measurementNoise, false);
durationDLTOpt += std::chrono::high_resolution_clock::now() - dltOptStart;
errorsLOST.row(i) = *estimateLOST - landmark;
errorsDLT.row(i) = *estimateDLT - landmark;
errorsDLTOpt.row(i) = *estimateDLTOpt - landmark;
errorsLOST.row(i) = estimateLOST - landmark;
errorsDLT.row(i) = estimateDLT - landmark;
errorsDLTOpt.row(i) = estimateDLTOpt - landmark;
}
PrintCovarianceStats(errorsLOST, "LOST");
PrintCovarianceStats(errorsDLT, "DLT");

1
gtsam/base/serializationTestHelpers.h
View File

@ -24,6 +24,7 @@
#include <string>
#include <gtsam/base/serialization.h>
#include <gtsam/base/TestableAssertions.h>
#include <boost/serialization/serialization.hpp>
#include <boost/filesystem.hpp>

30
gtsam/base/std_optional_serialization.h
View File

@ -26,24 +26,11 @@ void save(Archive& ar, const std::optional<T>& t, const unsigned int /*version*/
) {
// It is an inherent limitation to the serialization of optional.hpp
// that the underlying type must be either a pointer or must have a
// default constructor. It's possible that this could change sometime
// in the future, but for now, one will have to work around it. This can
// be done by serialization the optional<T> as optional<T *>
// default constructor.
BOOST_STATIC_ASSERT(boost::serialization::detail::is_default_constructible<T>::value || boost::is_pointer<T>::value);
const bool tflag = t.has_value();
ar << boost::serialization::make_nvp("initialized", tflag);
if (tflag) {
const boost::serialization::item_version_type item_version(version<T>::value);
#if 0
const boost::archive::library_version_type library_version(
ar.get_library_version()
};
if(boost::archive::library_version_type(3) < library_version){
ar << BOOST_SERIALIZATION_NVP(item_version);
}
#else
ar << BOOST_SERIALIZATION_NVP(item_version);
#endif
ar << boost::serialization::make_nvp("value", *t);
}
}
@ -58,16 +45,11 @@ void load(Archive& ar, std::optional<T>& t, const unsigned int /*version*/
return;
}
boost::serialization::item_version_type item_version(0);
boost::archive::library_version_type library_version(ar.get_library_version());
if (boost::archive::library_version_type(3) < library_version) {
ar >> BOOST_SERIALIZATION_NVP(item_version);
if (!t.has_value()) {
// Need to be default constructible
t.emplace();
}
detail::stack_allocate<T> tp;
ar >> boost::serialization::make_nvp("value", tp.reference());
t = std::move(tp.reference());
// hack because std::optional does not have get_ptr fn
ar.reset_object_address(&(t.value()), &tp.reference());
ar >> boost::serialization::make_nvp("value", *t);
}
template <class Archive, class T>
@ -75,6 +57,8 @@ void serialize(Archive& ar, std::optional<T>& t, const unsigned int version) {
boost::serialization::split_free(ar, t, version);
}
// derive boost::xml_archive_impl for archiving std::optional<T> with xml
} // namespace serialization
} // namespace boost

110
gtsam/base/tests/testStdOptionalSerialization.cpp Normal file
View File

@ -0,0 +1,110 @@
#include <CppUnitLite/TestHarness.h>
#include <boost/archive/binary_iarchive.hpp>
#include <boost/archive/binary_oarchive.hpp>
#include <boost/archive/xml_iarchive.hpp>
#include <boost/archive/xml_oarchive.hpp>
#include "gtsam/base/serializationTestHelpers.h"
#include "gtsam/base/std_optional_serialization.h"
using namespace gtsam;
// A test to check that the serialization of std::optional works with boost
TEST(StdOptionalSerialization, SerializeIntOptional) {
// Create an optional
std::optional<int> opt(42);
// Serialize it
std::stringstream ss;
boost::archive::text_oarchive oa(ss);
oa << opt;
// Deserialize it
std::optional<int> opt2;
boost::archive::text_iarchive ia(ss);
ia >> opt2;
// Check that it worked
EXPECT(opt2.has_value());
EXPECT(*opt2 == 42);
}
// A test to check if we serialize an empty optional, it is deserialized as an
// empty optional
TEST(StdOptionalSerialization, SerializeEmptyOptional) {
// Create an optional
std::optional<int> opt;
// Serialize it
std::stringstream ss;
boost::archive::text_oarchive oa(ss);
oa << opt;
// Deserialize it
std::optional<int> opt2 = 43;
boost::archive::text_iarchive ia(ss);
ia >> opt2;
// Check that it worked
EXPECT(!opt2.has_value());
}
/* ************************************************************************* */
// Implement the equals trait for TestOptionalStruct
namespace gtsam {
// A struct which contains an optional
class TestOptionalStruct {
public:
std::optional<int> opt;
TestOptionalStruct() = default;
TestOptionalStruct(const int& opt)
: opt(opt) {}
friend class boost::serialization::access;
template <class Archive>
void serialize(Archive& ar, const unsigned int /*version*/) {
ar& BOOST_SERIALIZATION_NVP(opt);
}
};
template <>
struct traits<TestOptionalStruct> {
static bool Equals(const TestOptionalStruct& q1, const TestOptionalStruct& q2, double) {
// if both have values then compare the values
if (q1.opt.has_value() && q2.opt.has_value()) {
return *q1.opt == *q2.opt;
}
// otherwise check that both are empty
return q1.opt == q2.opt;
}
static void Print(const TestOptionalStruct& m, const std::string& s = "") {
/* std::cout << s << "opt: " << m.opt << std::endl; */
if (m.opt.has_value()) {
std::cout << s << "opt: " << *m.opt << std::endl;
} else {
std::cout << s << "opt: empty" << std::endl;
}
}
};
} // namespace gtsam
// Test for a struct with an initialized optional
TEST(StdOptionalSerialization, SerializTestOptionalStruct) {
TestOptionalStruct optStruct(10);
EXPECT(serializationTestHelpers::equalsObj(optStruct));
EXPECT(serializationTestHelpers::equalsXML(optStruct));
EXPECT(serializationTestHelpers::equalsBinary(optStruct));
}
// Test for a struct with an uninitialized optional
TEST(StdOptionalSerialization, SerializTestOptionalStructUninitialized) {
TestOptionalStruct optStruct;
EXPECT(serializationTestHelpers::equalsObj(optStruct));
EXPECT(serializationTestHelpers::equalsXML(optStruct));
EXPECT(serializationTestHelpers::equalsBinary(optStruct));
}
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}

4
gtsam/linear/AcceleratedPowerMethod.h
View File

@ -60,11 +60,11 @@ class AcceleratedPowerMethod : public PowerMethod<Operator> {
* vector as ritzVector
*/
explicit AcceleratedPowerMethod(
const Operator &A, const boost::optional<Vector> initial = boost::none,
const Operator &A, const std::optional<Vector> initial = {},
double initialBeta = 0.0)
: PowerMethod<Operator>(A, initial) {
// initialize Ritz eigen vector and previous vector
this->ritzVector_ = initial ? initial.get() : Vector::Random(this->dim_);
this->ritzVector_ = initial ? *initial : Vector::Random(this->dim_);
this->ritzVector_.normalize();
previousVector_ = Vector::Zero(this->dim_);

2
gtsam/linear/IterativeSolver.h
View File

@ -23,11 +23,11 @@
#include <boost/tuple/tuple.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/optional.hpp>
#include <iosfwd>
#include <string>
#include <map>
#include <optional>
namespace gtsam {

2
gtsam/linear/JacobianFactor.cpp
View File

@ -264,7 +264,7 @@ void JacobianFactor::JacobianFactorHelper(const GaussianFactorGraph& graph,
// Copy the RHS vectors and sigmas
gttic(copy_vectors);
bool anyConstrained = false;
boost::optional<Vector> sigmas;
std::optional<Vector> sigmas;
// Loop over source jacobians
DenseIndex nextRow = 0;
for (size_t factorI = 0; factorI < jacobians.size(); ++factorI) {

18
gtsam/linear/NoiseModel.cpp
View File

@ -47,7 +47,7 @@ void updateAb(MATRIX& Ab, int j, const Vector& a, const Vector& rd) {
/* ************************************************************************* */
// check *above the diagonal* for non-zero entries
boost::optional<Vector> checkIfDiagonal(const Matrix& M) {
std::optional<Vector> checkIfDiagonal(const Matrix& M) {
size_t m = M.rows(), n = M.cols();
assert(m > 0);
// check all non-diagonal entries
@ -61,7 +61,7 @@ boost::optional<Vector> checkIfDiagonal(const Matrix& M) {
break;
}
if (full) {
return boost::none;
return {};
} else {
Vector diagonal(n);
for (j = 0; j < n; j++)
@ -88,7 +88,7 @@ Gaussian::shared_ptr Gaussian::SqrtInformation(const Matrix& R, bool smart) {
if (m != n)
throw invalid_argument("Gaussian::SqrtInformation: R not square");
if (smart) {
boost::optional<Vector> diagonal = checkIfDiagonal(R);
std::optional<Vector> diagonal = checkIfDiagonal(R);
if (diagonal)
return Diagonal::Sigmas(diagonal->array().inverse(), true);
}
@ -101,7 +101,7 @@ Gaussian::shared_ptr Gaussian::Information(const Matrix& information, bool smart
size_t m = information.rows(), n = information.cols();
if (m != n)
throw invalid_argument("Gaussian::Information: R not square");
boost::optional<Vector> diagonal = boost::none;
std::optional<Vector> diagonal = {};
if (smart)
diagonal = checkIfDiagonal(information);
if (diagonal)
@ -119,7 +119,7 @@ Gaussian::shared_ptr Gaussian::Covariance(const Matrix& covariance,
size_t m = covariance.rows(), n = covariance.cols();
if (m != n)
throw invalid_argument("Gaussian::Covariance: covariance not square");
boost::optional<Vector> variances = boost::none;
std::optional<Vector> variances = {};
if (smart)
variances = checkIfDiagonal(covariance);
if (variances)
@ -426,8 +426,8 @@ Constrained::shared_ptr Constrained::unit() const {
// Check whether column a triggers a constraint and corresponding variable is deterministic
// Return constraint_row with maximum element in case variable plays in multiple constraints
template <typename VECTOR>
boost::optional<size_t> check_if_constraint(VECTOR a, const Vector& invsigmas, size_t m) {
boost::optional<size_t> constraint_row;
std::optional<size_t> check_if_constraint(VECTOR a, const Vector& invsigmas, size_t m) {
std::optional<size_t> constraint_row;
// not zero, so roundoff errors will not be counted
// TODO(frank): that's a fairly crude way of dealing with roundoff errors :-(
double max_element = 1e-9;
@ -437,7 +437,7 @@ boost::optional<size_t> check_if_constraint(VECTOR a, const Vector& invsigmas, s
double abs_ai = std::abs(a(i,0));
if (abs_ai > max_element) {
max_element = abs_ai;
constraint_row.reset(i);
constraint_row = i;
}
}
return constraint_row;
@ -468,7 +468,7 @@ SharedDiagonal Constrained::QR(Matrix& Ab) const {
Eigen::Block<Matrix> a = Ab.block(0, j, m, 1);
// Check whether we need to handle as a constraint
boost::optional<size_t> constraint_row = check_if_constraint(a, invsigmas, m);
std::optional<size_t> constraint_row = check_if_constraint(a, invsigmas, m);
if (constraint_row) {
// Handle this as a constraint, as the i^th row has zero sigma with non-zero entry A(i,j)

10
gtsam/linear/NoiseModel.h
View File

@ -20,6 +20,7 @@
#include <gtsam/base/Testable.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/std_optional_serialization.h>
#include <gtsam/dllexport.h>
#include <gtsam/linear/LossFunctions.h>
@ -27,7 +28,8 @@
#include <boost/serialization/extended_type_info.hpp>
#include <boost/serialization/singleton.hpp>
#include <boost/serialization/shared_ptr.hpp>
#include <boost/serialization/optional.hpp>
#include <optional>
namespace gtsam {
@ -164,7 +166,7 @@ namespace gtsam {
protected:
/** Matrix square root of information matrix (R) */
boost::optional<Matrix> sqrt_information_;
std::optional<Matrix> sqrt_information_;
private:
@ -184,7 +186,7 @@ namespace gtsam {
/** constructor takes square root information matrix */
Gaussian(size_t dim = 1,
const boost::optional<Matrix>& sqrt_information = boost::none)
const std::optional<Matrix>& sqrt_information = {})
: Base(dim), sqrt_information_(sqrt_information) {}
~Gaussian() override {}
@ -732,7 +734,7 @@ namespace gtsam {
};
// Helper function
GTSAM_EXPORT boost::optional<Vector> checkIfDiagonal(const Matrix& M);
GTSAM_EXPORT std::optional<Vector> checkIfDiagonal(const Matrix& M);
} // namespace noiseModel

5
gtsam/linear/PowerMethod.h
View File

@ -26,6 +26,7 @@
#include <Eigen/Sparse>
#include <random>
#include <vector>
#include <optional>
namespace gtsam {
@ -75,10 +76,10 @@ class PowerMethod {
/// Construct from the aim matrix and intial ritz vector
explicit PowerMethod(const Operator &A,
const boost::optional<Vector> initial = boost::none)
const std::optional<Vector> initial = {})
: A_(A), dim_(A.rows()), nrIterations_(0) {
Vector x0;
x0 = initial ? initial.get() : Vector::Random(dim_);
x0 = initial ? *initial : Vector::Random(dim_);
x0.normalize();
// initialize Ritz eigen value

4
gtsam/navigation/AHRSFactor.cpp
View File

@ -185,7 +185,7 @@ Rot3 AHRSFactor::Predict(const Rot3& rot_i, const Vector3& bias,
AHRSFactor::AHRSFactor(Key rot_i, Key rot_j, Key bias,
const PreintegratedAhrsMeasurements& pim,
const Vector3& omegaCoriolis,
const boost::optional<Pose3>& body_P_sensor)
const std::optional<Pose3>& body_P_sensor)
: Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), rot_i, rot_j,
bias),
_PIM_(pim) {
@ -198,7 +198,7 @@ AHRSFactor::AHRSFactor(Key rot_i, Key rot_j, Key bias,
Rot3 AHRSFactor::predict(const Rot3& rot_i, const Vector3& bias,
const PreintegratedAhrsMeasurements& pim,
const Vector3& omegaCoriolis,
const boost::optional<Pose3>& body_P_sensor) {
const std::optional<Pose3>& body_P_sensor) {
auto p = boost::make_shared<PreintegratedAhrsMeasurements::Params>(pim.p());
p->omegaCoriolis = omegaCoriolis;
p->body_P_sensor = body_P_sensor;

6
gtsam/navigation/AHRSFactor.h
View File

@ -24,6 +24,8 @@
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/Pose3.h>
#include <optional>
namespace gtsam {
/**
@ -194,13 +196,13 @@ public:
AHRSFactor(Key rot_i, Key rot_j, Key bias,
const PreintegratedAhrsMeasurements& pim,
const Vector3& omegaCoriolis,
const boost::optional<Pose3>& body_P_sensor = boost::none);
const std::optional<Pose3>& body_P_sensor = {});
/// @deprecated static function, but used in tests.
static Rot3 predict(
const Rot3& rot_i, const Vector3& bias,
const PreintegratedAhrsMeasurements& pim, const Vector3& omegaCoriolis,
const boost::optional<Pose3>& body_P_sensor = boost::none);
const std::optional<Pose3>& body_P_sensor = {});
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
/// @deprecated name

2
gtsam/navigation/ImuBias.cpp
View File

@ -38,7 +38,7 @@ namespace imuBias {
// // H1: Jacobian w.r.t. IMUBias
// // H2: Jacobian w.r.t. pose
// Vector CorrectGyroWithEarthRotRate(Vector measurement, const Pose3& pose, const Vector& w_earth_rate_G,
// boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const {
// Matrix* H1=nullptr, Matrix* H2=nullptr) const {
//
// Matrix R_G_to_I( pose.rotation().matrix().transpose() );
// Vector w_earth_rate_I = R_G_to_I * w_earth_rate_G;

7
gtsam/navigation/MagPoseFactor.h
View File

@ -11,9 +11,12 @@
#pragma once
#include <gtsam/base/std_optional_serialization.h>
#include <gtsam/geometry/concepts.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <optional>
namespace gtsam {
/**
@ -35,7 +38,7 @@ class MagPoseFactor: public NoiseModelFactorN<POSE> {
const Point measured_; ///< The measured magnetometer data in the body frame.
const Point nM_; ///< Local magnetic field (mag output units) in the nav frame.
const Point bias_; ///< The bias vector (mag output units) in the body frame.
boost::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame.
std::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame.
static const int MeasDim = Point::RowsAtCompileTime;
static const int PoseDim = traits<POSE>::dimension;
@ -74,7 +77,7 @@ class MagPoseFactor: public NoiseModelFactorN<POSE> {
const Point& direction,
const Point& bias,
const SharedNoiseModel& model,
const boost::optional<POSE>& body_P_sensor)
const std::optional<POSE>& body_P_sensor)
: Base(model, pose_key),
measured_(body_P_sensor ? body_P_sensor->rotation() * measured : measured),
nM_(scale * direction.normalized()),

2
gtsam/navigation/NavState.cpp
View File

@ -256,7 +256,7 @@ Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder,
//------------------------------------------------------------------------------
Vector9 NavState::correctPIM(const Vector9& pim, double dt,
const Vector3& n_gravity, const boost::optional<Vector3>& omegaCoriolis,
const Vector3& n_gravity, const std::optional<Vector3>& omegaCoriolis,
bool use2ndOrderCoriolis, OptionalJacobian<9, 9> H1,
OptionalJacobian<9, 9> H2) const {
const Rot3& nRb = R_;

24
gtsam/navigation/NavState.h
View File

@ -79,9 +79,9 @@ public:
/// @name Component Access
/// @{
const Rot3& attitude(OptionalJacobian<3, 9> H = boost::none) const;
const Point3& position(OptionalJacobian<3, 9> H = boost::none) const;
const Velocity3& velocity(OptionalJacobian<3, 9> H = boost::none) const;
const Rot3& attitude(OptionalJacobian<3, 9> H = {}) const;
const Point3& position(OptionalJacobian<3, 9> H = {}) const;
const Velocity3& velocity(OptionalJacobian<3, 9> H = {}) const;
const Pose3 pose() const {
return Pose3(attitude(), position());
@ -108,7 +108,7 @@ public:
return v_;
}
// Return velocity in body frame
Velocity3 bodyVelocity(OptionalJacobian<3, 9> H = boost::none) const;
Velocity3 bodyVelocity(OptionalJacobian<3, 9> H = {}) const;
/// Return matrix group representation, in MATLAB notation:
/// nTb = [nRb 0 n_t; 0 nRb n_v; 0 0 1]
@ -156,13 +156,13 @@ public:
/// retract with optional derivatives
NavState retract(const Vector9& v, //
OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 9> H2 =
boost::none) const;
OptionalJacobian<9, 9> H1 = {}, OptionalJacobian<9, 9> H2 =
{}) const;
/// localCoordinates with optional derivatives
Vector9 localCoordinates(const NavState& g, //
OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 9> H2 =
boost::none) const;
OptionalJacobian<9, 9> H1 = {}, OptionalJacobian<9, 9> H2 =
{}) const;
/// @}
/// @name Dynamics
@ -176,14 +176,14 @@ public:
/// Compute tangent space contribution due to Coriolis forces
Vector9 coriolis(double dt, const Vector3& omega, bool secondOrder = false,
OptionalJacobian<9, 9> H = boost::none) const;
OptionalJacobian<9, 9> H = {}) const;
/// Correct preintegrated tangent vector with our velocity and rotated gravity,
/// taking into account Coriolis forces if omegaCoriolis is given.
Vector9 correctPIM(const Vector9& pim, double dt, const Vector3& n_gravity,
const boost::optional<Vector3>& omegaCoriolis, bool use2ndOrderCoriolis =
false, OptionalJacobian<9, 9> H1 = boost::none,
OptionalJacobian<9, 9> H2 = boost::none) const;
const std::optional<Vector3>& omegaCoriolis, bool use2ndOrderCoriolis =
false, OptionalJacobian<9, 9> H1 = {},
OptionalJacobian<9, 9> H2 = {}) const;
/// @}

3
gtsam/navigation/PreintegratedRotation.cpp
View File

@ -115,8 +115,7 @@ void PreintegratedRotation::integrateMeasurement(const Vector3& measuredOmega,
Rot3 PreintegratedRotation::biascorrectedDeltaRij(const Vector3& biasOmegaIncr,
OptionalJacobian<3, 3> H) const {
const Vector3 biasInducedOmega = delRdelBiasOmega_ * biasOmegaIncr;
const Rot3 deltaRij_biascorrected = deltaRij_.expmap(biasInducedOmega,
boost::none, H);
const Rot3 deltaRij_biascorrected = deltaRij_.expmap(biasInducedOmega,{}, H);
if (H)
(*H) *= delRdelBiasOmega_;
return deltaRij_biascorrected;

30
gtsam/navigation/PreintegratedRotation.h
View File

@ -23,6 +23,7 @@
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/std_optional_serialization.h>
namespace gtsam {
@ -32,16 +33,17 @@ struct GTSAM_EXPORT PreintegratedRotationParams {
/// Continuous-time "Covariance" of gyroscope measurements
/// The units for stddev are σ = rad/s/√Hz
Matrix3 gyroscopeCovariance;
boost::optional<Vector3> omegaCoriolis; ///< Coriolis constant
boost::optional<Pose3> body_P_sensor; ///< The pose of the sensor in the body frame
std::optional<Vector3> omegaCoriolis; ///< Coriolis constant
std::optional<Pose3> body_P_sensor; ///< The pose of the sensor in the body frame
PreintegratedRotationParams() : gyroscopeCovariance(I_3x3) {}
PreintegratedRotationParams(const Matrix3& gyroscope_covariance,
boost::optional<Vector3> omega_coriolis)
std::optional<Vector3> omega_coriolis)
: gyroscopeCovariance(gyroscope_covariance) {
if (omega_coriolis)
omegaCoriolis.reset(omega_coriolis.get());
if (omega_coriolis) {
omegaCoriolis = *omega_coriolis;
}
}
virtual ~PreintegratedRotationParams() {}
@ -50,12 +52,12 @@ struct GTSAM_EXPORT PreintegratedRotationParams {
virtual bool equals(const PreintegratedRotationParams& other, double tol=1e-9) const;
void setGyroscopeCovariance(const Matrix3& cov) { gyroscopeCovariance = cov; }
void setOmegaCoriolis(const Vector3& omega) { omegaCoriolis.reset(omega); }
void setBodyPSensor(const Pose3& pose) { body_P_sensor.reset(pose); }
void setOmegaCoriolis(const Vector3& omega) { omegaCoriolis = omega; }
void setBodyPSensor(const Pose3& pose) { body_P_sensor = pose; }
const Matrix3& getGyroscopeCovariance() const { return gyroscopeCovariance; }
boost::optional<Vector3> getOmegaCoriolis() const { return omegaCoriolis; }
boost::optional<Pose3> getBodyPSensor() const { return body_P_sensor; }
std::optional<Vector3> getOmegaCoriolis() const { return omegaCoriolis; }
std::optional<Pose3> getBodyPSensor() const { return body_P_sensor; }
private:
/** Serialization function */
@ -66,8 +68,8 @@ struct GTSAM_EXPORT PreintegratedRotationParams {
ar & BOOST_SERIALIZATION_NVP(gyroscopeCovariance);
ar & BOOST_SERIALIZATION_NVP(body_P_sensor);
// Provide support for Eigen::Matrix in boost::optional
bool omegaCoriolisFlag = omegaCoriolis.is_initialized();
// Provide support for Eigen::Matrix in std::optional
bool omegaCoriolisFlag = omegaCoriolis.has_value();
ar & boost::serialization::make_nvp("omegaCoriolisFlag", omegaCoriolisFlag);
if (omegaCoriolisFlag) {
ar & BOOST_SERIALIZATION_NVP(*omegaCoriolis);
@ -164,12 +166,12 @@ class GTSAM_EXPORT PreintegratedRotation {
/// Calculate an incremental rotation given the gyro measurement and a time interval,
/// and update both deltaTij_ and deltaRij_.
void integrateMeasurement(const Vector3& measuredOmega, const Vector3& biasHat, double deltaT,
OptionalJacobian<3, 3> D_incrR_integratedOmega = boost::none,
OptionalJacobian<3, 3> F = boost::none);
OptionalJacobian<3, 3> D_incrR_integratedOmega = {},
OptionalJacobian<3, 3> F = {});
/// Return a bias corrected version of the integrated rotation, with optional Jacobian
Rot3 biascorrectedDeltaRij(const Vector3& biasOmegaIncr,
OptionalJacobian<3, 3> H = boost::none) const;
OptionalJacobian<3, 3> H = {}) const;
/// Integrate coriolis correction in body frame rot_i
Vector3 integrateCoriolis(const Rot3& rot_i) const;

20
gtsam/navigation/PreintegrationBase.h
View File

@ -129,9 +129,9 @@ class GTSAM_EXPORT PreintegrationBase {
*/
std::pair<Vector3, Vector3> correctMeasurementsBySensorPose(
const Vector3& unbiasedAcc, const Vector3& unbiasedOmega,
OptionalJacobian<3, 3> correctedAcc_H_unbiasedAcc = boost::none,
OptionalJacobian<3, 3> correctedAcc_H_unbiasedOmega = boost::none,
OptionalJacobian<3, 3> correctedOmega_H_unbiasedOmega = boost::none) const;
OptionalJacobian<3, 3> correctedAcc_H_unbiasedAcc = {},
OptionalJacobian<3, 3> correctedAcc_H_unbiasedOmega = {},
OptionalJacobian<3, 3> correctedOmega_H_unbiasedOmega = {}) const;
/**
* Update preintegrated measurements and get derivatives
@ -148,12 +148,12 @@ class GTSAM_EXPORT PreintegrationBase {
/// Given the estimate of the bias, return a NavState tangent vector
/// summarizing the preintegrated IMU measurements so far
virtual Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i,
OptionalJacobian<9, 6> H = boost::none) const = 0;
OptionalJacobian<9, 6> H = {}) const = 0;
/// Predict state at time j
NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i,
OptionalJacobian<9, 9> H1 = boost::none,
OptionalJacobian<9, 6> H2 = boost::none) const;
OptionalJacobian<9, 9> H1 = {},
OptionalJacobian<9, 6> H2 = {}) const;
/// Calculate error given navStates
Vector9 computeError(const NavState& state_i, const NavState& state_j,
@ -167,10 +167,10 @@ class GTSAM_EXPORT PreintegrationBase {
*/
Vector9 computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i,
const Pose3& pose_j, const Vector3& vel_j,
const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H1 =
boost::none, OptionalJacobian<9, 3> H2 = boost::none,
OptionalJacobian<9, 6> H3 = boost::none, OptionalJacobian<9, 3> H4 =
boost::none, OptionalJacobian<9, 6> H5 = boost::none) const;
const imuBias::ConstantBias& bias_i,
OptionalJacobian<9, 6> H1 = {}, OptionalJacobian<9, 3> H2 = {},
OptionalJacobian<9, 6> H3 = {}, OptionalJacobian<9, 3> H4 = {},
OptionalJacobian<9, 6> H5 = {}) const;
private:
/** Serialization function */

7
gtsam/navigation/tests/testAHRSFactor.cpp
View File

@ -140,7 +140,7 @@ TEST( AHRSFactor, PreintegratedAhrsMeasurementsConstructor ) {
EXPECT(assert_equal(gyroscopeCovariance,
actualPim.p().getGyroscopeCovariance(), 1e-6));
EXPECT(assert_equal(omegaCoriolis,
actualPim.p().getOmegaCoriolis().get(), 1e-6));
*(actualPim.p().getOmegaCoriolis()), 1e-6));
EXPECT(assert_equal(bias, actualPim.biasHat(), 1e-6));
DOUBLES_EQUAL(deltaTij, actualPim.deltaTij(), 1e-6);
EXPECT(assert_equal(deltaRij, Rot3(actualPim.deltaRij()), 1e-6));
@ -163,7 +163,7 @@ TEST(AHRSFactor, Error) {
pim.integrateMeasurement(measuredOmega, deltaT);
// Create factor
AHRSFactor factor(X(1), X(2), B(1), pim, kZeroOmegaCoriolis, boost::none);
AHRSFactor factor(X(1), X(2), B(1), pim, kZeroOmegaCoriolis, {});
Vector3 errorActual = factor.evaluateError(x1, x2, bias);
@ -458,8 +458,7 @@ TEST (AHRSFactor, predictTest) {
// PreintegratedAhrsMeasurements::predict
Matrix expectedH = numericalDerivative11<Vector3, Vector3>(
std::bind(&PreintegratedAhrsMeasurements::predict,
&pim, std::placeholders::_1, boost::none), bias);
[&pim](const Vector3& b) { return pim.predict(b, {}); }, bias);
// Actual Jacobians
Matrix H;

8
gtsam/navigation/tests/testMagPoseFactor.cpp
View File

@ -57,8 +57,8 @@ Pose3 body_P3_sensor(Rot3::RzRyRx(Vector3(1.5, 0.9, -1.15)),
// *****************************************************************************
TEST(MagPoseFactor, Constructors) {
MagPoseFactor<Pose2> f2a(Symbol('X', 0), measured2, scale, dir2, bias2, model2, boost::none);
MagPoseFactor<Pose3> f3a(Symbol('X', 0), measured3, scale, dir3, bias3, model3, boost::none);
MagPoseFactor<Pose2> f2a(Symbol('X', 0), measured2, scale, dir2, bias2, model2, {});
MagPoseFactor<Pose3> f3a(Symbol('X', 0), measured3, scale, dir3, bias3, model3, {});
// Try constructing with a body_P_sensor set.
MagPoseFactor<Pose2> f2b = MagPoseFactor<Pose2>(
@ -75,7 +75,7 @@ TEST(MagPoseFactor, JacobianPose2) {
Matrix H2;
// Error should be zero at the groundtruth pose.
MagPoseFactor<Pose2> f(Symbol('X', 0), measured2, scale, dir2, bias2, model2, boost::none);
MagPoseFactor<Pose2> f(Symbol('X', 0), measured2, scale, dir2, bias2, model2, {});
CHECK(gtsam::assert_equal(Z_2x1, f.evaluateError(n_P2_b, H2), 1e-5));
CHECK(gtsam::assert_equal(gtsam::numericalDerivative11<Vector, Pose2> //
([&f] (const Pose2& p) {return f.evaluateError(p);},
@ -88,7 +88,7 @@ TEST(MagPoseFactor, JacobianPose3) {
Matrix H3;
// Error should be zero at the groundtruth pose.
MagPoseFactor<Pose3> f(Symbol('X', 0), measured3, scale, dir3, bias3, model3, boost::none);
MagPoseFactor<Pose3> f(Symbol('X', 0), measured3, scale, dir3, bias3, model3, {});
CHECK(gtsam::assert_equal(Z_3x1, f.evaluateError(n_P3_b, H3), 1e-5));
CHECK(gtsam::assert_equal(gtsam::numericalDerivative11<Vector, Pose3> //
([&f] (const Pose3& p) {return f.evaluateError(p);},

2
gtsam/sfm/ShonanAveraging.cpp
View File

@ -554,7 +554,7 @@ static bool PowerMinimumEigenValue(
}
const Sparse C = pmEigenValue * Matrix::Identity(A.rows(), A.cols()).sparseView() - A;
const boost::optional<Vector> initial = perturb(S.row(0));
const std::optional<Vector> initial = perturb(S.row(0));
AcceleratedPowerMethod<Sparse> apmShiftedOperator(C, initial);
const bool minConverged = apmShiftedOperator.compute(