761 lines
30 KiB
OpenEdge ABL
761 lines
30 KiB
OpenEdge ABL
/**
|
|
* Matlab toolbox interface definition for gtsam_unstable
|
|
*/
|
|
|
|
// specify the classes from gtsam we are using
|
|
virtual class gtsam::Value;
|
|
class gtsam::Vector6;
|
|
class gtsam::Point2;
|
|
class gtsam::Point2Vector;
|
|
class gtsam::Rot2;
|
|
class gtsam::Pose2;
|
|
class gtsam::Point3;
|
|
class gtsam::SO3;
|
|
class gtsam::SO4;
|
|
class gtsam::SOn;
|
|
class gtsam::Rot3;
|
|
class gtsam::Pose3;
|
|
virtual class gtsam::noiseModel::Base;
|
|
virtual class gtsam::noiseModel::Gaussian;
|
|
virtual class gtsam::noiseModel::Isotropic;
|
|
virtual class gtsam::imuBias::ConstantBias;
|
|
virtual class gtsam::NonlinearFactor;
|
|
virtual class gtsam::NoiseModelFactor;
|
|
virtual class gtsam::NoiseModelFactorN;
|
|
virtual class gtsam::GaussianFactor;
|
|
virtual class gtsam::HessianFactor;
|
|
virtual class gtsam::JacobianFactor;
|
|
class gtsam::Cal3_S2;
|
|
class gtsam::Cal3DS2;
|
|
class gtsam::GaussianFactorGraph;
|
|
class gtsam::NonlinearFactorGraph;
|
|
class gtsam::Ordering;
|
|
class gtsam::Values;
|
|
class gtsam::Key;
|
|
class gtsam::VectorValues;
|
|
class gtsam::KeyList;
|
|
class gtsam::KeySet;
|
|
class gtsam::KeyVector;
|
|
class gtsam::LevenbergMarquardtParams;
|
|
class gtsam::ISAM2Params;
|
|
class gtsam::GaussianDensity;
|
|
class gtsam::LevenbergMarquardtOptimizer;
|
|
class gtsam::FixedLagSmoother;
|
|
|
|
namespace gtsam {
|
|
|
|
#include <gtsam_unstable/base/Dummy.h>
|
|
class Dummy {
|
|
Dummy();
|
|
void print(string s) const;
|
|
unsigned char dummyTwoVar(unsigned char a) const;
|
|
};
|
|
|
|
#include <gtsam_unstable/dynamics/PoseRTV.h>
|
|
class PoseRTV {
|
|
PoseRTV();
|
|
PoseRTV(gtsam::Vector rtv);
|
|
PoseRTV(const gtsam::Point3& pt, const gtsam::Rot3& rot, const gtsam::Vector& vel);
|
|
PoseRTV(const gtsam::Rot3& rot, const gtsam::Point3& pt, const gtsam::Vector& vel);
|
|
PoseRTV(const gtsam::Pose3& pose, const gtsam::Vector& vel);
|
|
PoseRTV(const gtsam::Pose3& pose);
|
|
PoseRTV(double roll, double pitch, double yaw, double x, double y, double z, double vx, double vy, double vz);
|
|
|
|
// testable
|
|
bool equals(const gtsam::PoseRTV& other, double tol) const;
|
|
void print(string s) const;
|
|
|
|
// access
|
|
gtsam::Point3 translation() const;
|
|
gtsam::Rot3 rotation() const;
|
|
gtsam::Vector velocity() const;
|
|
gtsam::Pose3 pose() const;
|
|
|
|
// gtsam::Vector interfaces
|
|
gtsam::Vector vector() const;
|
|
gtsam::Vector translationVec() const;
|
|
gtsam::Vector velocityVec() const;
|
|
|
|
// manifold/Lie
|
|
static size_t Dim();
|
|
size_t dim() const;
|
|
gtsam::PoseRTV retract(gtsam::Vector v) const;
|
|
gtsam::Vector localCoordinates(const gtsam::PoseRTV& p) const;
|
|
static gtsam::PoseRTV Expmap(gtsam::Vector v);
|
|
static gtsam::Vector Logmap(const gtsam::PoseRTV& p);
|
|
gtsam::PoseRTV inverse() const;
|
|
gtsam::PoseRTV compose(const gtsam::PoseRTV& p) const;
|
|
gtsam::PoseRTV between(const gtsam::PoseRTV& p) const;
|
|
|
|
// measurement
|
|
double range(const gtsam::PoseRTV& other) const;
|
|
gtsam::PoseRTV transformed_from(const gtsam::Pose3& trans) const;
|
|
|
|
// IMU/dynamics
|
|
gtsam::PoseRTV planarDynamics(double vel_rate, double heading_rate, double max_accel, double dt) const;
|
|
gtsam::PoseRTV flyingDynamics(double pitch_rate, double heading_rate, double lift_control, double dt) const;
|
|
gtsam::PoseRTV generalDynamics(gtsam::Vector accel, gtsam::Vector gyro, double dt) const;
|
|
gtsam::Vector imuPrediction(const gtsam::PoseRTV& x2, double dt) const;
|
|
gtsam::Point3 translationIntegration(const gtsam::PoseRTV& x2, double dt) const;
|
|
gtsam::Vector translationIntegrationVec(const gtsam::PoseRTV& x2, double dt) const;
|
|
|
|
void serializable() const; // enabling serialization functionality
|
|
};
|
|
|
|
#include <gtsam_unstable/geometry/Pose3Upright.h>
|
|
class Pose3Upright {
|
|
Pose3Upright();
|
|
Pose3Upright(const gtsam::Pose3Upright& other);
|
|
Pose3Upright(const gtsam::Rot2& bearing, const gtsam::Point3& t);
|
|
Pose3Upright(double x, double y, double z, double theta);
|
|
Pose3Upright(const gtsam::Pose2& pose, double z);
|
|
|
|
void print(string s) const;
|
|
bool equals(const gtsam::Pose3Upright& pose, double tol) const;
|
|
|
|
double x() const;
|
|
double y() const;
|
|
double z() const;
|
|
double theta() const;
|
|
|
|
gtsam::Point2 translation2() const;
|
|
gtsam::Point3 translation() const;
|
|
gtsam::Rot2 rotation2() const;
|
|
gtsam::Rot3 rotation() const;
|
|
gtsam::Pose2 pose2() const;
|
|
gtsam::Pose3 pose() const;
|
|
|
|
size_t dim() const;
|
|
gtsam::Pose3Upright retract(gtsam::Vector v) const;
|
|
gtsam::Vector localCoordinates(const gtsam::Pose3Upright& p2) const;
|
|
|
|
static gtsam::Pose3Upright Identity();
|
|
gtsam::Pose3Upright inverse() const;
|
|
gtsam::Pose3Upright compose(const gtsam::Pose3Upright& p2) const;
|
|
gtsam::Pose3Upright between(const gtsam::Pose3Upright& p2) const;
|
|
|
|
static gtsam::Pose3Upright Expmap(gtsam::Vector xi);
|
|
static gtsam::Vector Logmap(const gtsam::Pose3Upright& p);
|
|
|
|
void serializable() const; // enabling serialization functionality
|
|
}; // \class Pose3Upright
|
|
|
|
#include <gtsam_unstable/geometry/BearingS2.h>
|
|
class BearingS2 {
|
|
BearingS2();
|
|
BearingS2(double azimuth_double, double elevation_double);
|
|
BearingS2(const gtsam::Rot2& azimuth, const gtsam::Rot2& elevation);
|
|
|
|
gtsam::Rot2 azimuth() const;
|
|
gtsam::Rot2 elevation() const;
|
|
|
|
static gtsam::BearingS2 fromDownwardsObservation(const gtsam::Pose3& A, const gtsam::Point3& B);
|
|
static gtsam::BearingS2 fromForwardObservation(const gtsam::Pose3& A, const gtsam::Point3& B);
|
|
|
|
void print(string s) const;
|
|
bool equals(const gtsam::BearingS2& x, double tol) const;
|
|
|
|
size_t dim() const;
|
|
gtsam::BearingS2 retract(gtsam::Vector v) const;
|
|
gtsam::Vector localCoordinates(const gtsam::BearingS2& p2) const;
|
|
|
|
void serializable() const; // enabling serialization functionality
|
|
};
|
|
|
|
|
|
#include <gtsam_unstable/geometry/SimWall2D.h>
|
|
class SimWall2D {
|
|
SimWall2D();
|
|
SimWall2D(const gtsam::Point2& a, const gtsam::Point2& b);
|
|
SimWall2D(double ax, double ay, double bx, double by);
|
|
|
|
void print(string s) const;
|
|
bool equals(const gtsam::SimWall2D& other, double tol) const;
|
|
|
|
gtsam::Point2 a() const;
|
|
gtsam::Point2 b() const;
|
|
|
|
gtsam::SimWall2D scale(double s) const;
|
|
double length() const;
|
|
gtsam::Point2 midpoint() const;
|
|
|
|
bool intersects(const gtsam::SimWall2D& wall) const;
|
|
// bool intersects(const gtsam::SimWall2D& wall, gtsam::Point2* pt = nullptr) const;
|
|
|
|
gtsam::Point2 norm() const;
|
|
gtsam::Rot2 reflection(const gtsam::Point2& init, const gtsam::Point2& intersection) const;
|
|
};
|
|
|
|
#include <gtsam_unstable/geometry/SimPolygon2D.h>
|
|
class SimPolygon2D {
|
|
static void seedGenerator(size_t seed);
|
|
static gtsam::SimPolygon2D createTriangle(const gtsam::Point2& pA, const gtsam::Point2& pB, const gtsam::Point2& pC);
|
|
static gtsam::SimPolygon2D createRectangle(const gtsam::Point2& p, double height, double width);
|
|
|
|
static gtsam::SimPolygon2D randomTriangle(double side_len, double mean_side_len, double sigma_side_len,
|
|
double min_vertex_dist, double min_side_len, const gtsam::SimPolygon2DVector& existing_polys);
|
|
static gtsam::SimPolygon2D randomRectangle(double side_len, double mean_side_len, double sigma_side_len,
|
|
double min_vertex_dist, double min_side_len, const gtsam::SimPolygon2DVector& existing_polys);
|
|
|
|
gtsam::Point2 landmark(size_t i) const;
|
|
size_t size() const;
|
|
gtsam::Point2Vector vertices() const;
|
|
|
|
bool equals(const gtsam::SimPolygon2D& p, double tol) const;
|
|
void print(string s) const;
|
|
|
|
gtsam::SimWall2DVector walls() const;
|
|
bool contains(const gtsam::Point2& p) const;
|
|
bool overlaps(const gtsam::SimPolygon2D& p) const;
|
|
|
|
static bool anyContains(const gtsam::Point2& p, const gtsam::SimPolygon2DVector& obstacles);
|
|
static bool anyOverlaps(const gtsam::SimPolygon2D& p, const gtsam::SimPolygon2DVector& obstacles);
|
|
static bool insideBox(double s, const gtsam::Point2& p);
|
|
static bool nearExisting(const gtsam::Point2Vector& S,
|
|
const gtsam::Point2& p, double threshold);
|
|
|
|
static gtsam::Point2 randomPoint2(double s);
|
|
static gtsam::Rot2 randomAngle();
|
|
static double randomDistance(double mu, double sigma);
|
|
static double randomDistance(double mu, double sigma, double min_dist);
|
|
static gtsam::Point2 randomBoundedPoint2(double boundary_size,
|
|
const gtsam::Point2Vector& landmarks, double min_landmark_dist);
|
|
static gtsam::Point2 randomBoundedPoint2(double boundary_size,
|
|
const gtsam::Point2Vector& landmarks,
|
|
const gtsam::SimPolygon2DVector& obstacles, double min_landmark_dist);
|
|
static gtsam::Point2 randomBoundedPoint2(double boundary_size,
|
|
const gtsam::SimPolygon2DVector& obstacles);
|
|
static gtsam::Point2 randomBoundedPoint2(
|
|
const gtsam::Point2& LL_corner, const gtsam::Point2& UR_corner,
|
|
const gtsam::Point2Vector& landmarks,
|
|
const gtsam::SimPolygon2DVector& obstacles, double min_landmark_dist);
|
|
static gtsam::Pose2 randomFreePose(double boundary_size, const gtsam::SimPolygon2DVector& obstacles);
|
|
};
|
|
|
|
// std::vector<gtsam::SimWall2D>
|
|
class SimWall2DVector
|
|
{
|
|
//Capacity
|
|
size_t size() const;
|
|
size_t max_size() const;
|
|
void resize(size_t sz);
|
|
size_t capacity() const;
|
|
bool empty() const;
|
|
void reserve(size_t n);
|
|
|
|
//Element access
|
|
gtsam::SimWall2D at(size_t n) const;
|
|
gtsam::SimWall2D front() const;
|
|
gtsam::SimWall2D back() const;
|
|
|
|
//Modifiers
|
|
void assign(size_t n, const gtsam::SimWall2D& u);
|
|
void push_back(const gtsam::SimWall2D& x);
|
|
void pop_back();
|
|
};
|
|
|
|
// std::vector<gtsam::SimPolygon2D>
|
|
class SimPolygon2DVector
|
|
{
|
|
//Capacity
|
|
size_t size() const;
|
|
size_t max_size() const;
|
|
void resize(size_t sz);
|
|
size_t capacity() const;
|
|
bool empty() const;
|
|
void reserve(size_t n);
|
|
|
|
//Element access
|
|
gtsam::SimPolygon2D at(size_t n) const;
|
|
gtsam::SimPolygon2D front() const;
|
|
gtsam::SimPolygon2D back() const;
|
|
|
|
//Modifiers
|
|
void assign(size_t n, const gtsam::SimPolygon2D& u);
|
|
void push_back(const gtsam::SimPolygon2D& x);
|
|
void pop_back();
|
|
};
|
|
|
|
// Nonlinear factors from gtsam, for our Value types
|
|
#include <gtsam/nonlinear/PriorFactor.h>
|
|
template<T = {gtsam::PoseRTV}>
|
|
virtual class PriorFactor : gtsam::NoiseModelFactor {
|
|
PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
|
|
|
|
void serializable() const; // enabling serialization functionality
|
|
};
|
|
|
|
#include <gtsam/slam/BetweenFactor.h>
|
|
template<T = {gtsam::PoseRTV}>
|
|
virtual class BetweenFactor : gtsam::NoiseModelFactor {
|
|
BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel);
|
|
|
|
void serializable() const; // enabling serialization functionality
|
|
};
|
|
|
|
#include <gtsam_unstable/slam/BetweenFactorEM.h>
|
|
template<T = {gtsam::Pose2}>
|
|
virtual class BetweenFactorEM : gtsam::NonlinearFactor {
|
|
BetweenFactorEM(size_t key1, size_t key2, const T& relativePose,
|
|
const gtsam::noiseModel::Gaussian* model_inlier, const gtsam::noiseModel::Gaussian* model_outlier,
|
|
double prior_inlier, double prior_outlier);
|
|
|
|
BetweenFactorEM(size_t key1, size_t key2, const T& relativePose,
|
|
const gtsam::noiseModel::Gaussian* model_inlier, const gtsam::noiseModel::Gaussian* model_outlier,
|
|
double prior_inlier, double prior_outlier, bool flag_bump_up_near_zero_probs);
|
|
|
|
gtsam::Vector whitenedError(const gtsam::Values& x);
|
|
gtsam::Vector unwhitenedError(const gtsam::Values& x);
|
|
gtsam::Vector calcIndicatorProb(const gtsam::Values& x);
|
|
gtsam::Pose2 measured(); // TODO: figure out how to output a template instead
|
|
void set_flag_bump_up_near_zero_probs(bool flag);
|
|
bool get_flag_bump_up_near_zero_probs() const;
|
|
|
|
void updateNoiseModels(const gtsam::Values& values, const gtsam::NonlinearFactorGraph& graph);
|
|
void updateNoiseModels_givenCovs(const gtsam::Values& values, gtsam::Matrix cov1, gtsam::Matrix cov2, gtsam::Matrix cov12);
|
|
gtsam::Matrix get_model_inlier_cov();
|
|
gtsam::Matrix get_model_outlier_cov();
|
|
|
|
void serializable() const; // enabling serialization functionality
|
|
};
|
|
|
|
#include <gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h>
|
|
template<T = {gtsam::Pose2}>
|
|
virtual class TransformBtwRobotsUnaryFactorEM : gtsam::NonlinearFactor {
|
|
TransformBtwRobotsUnaryFactorEM(size_t key, const T& relativePose, size_t keyA, size_t keyB,
|
|
const gtsam::Values& valA, const gtsam::Values& valB,
|
|
const gtsam::noiseModel::Gaussian* model_inlier, const gtsam::noiseModel::Gaussian* model_outlier,
|
|
double prior_inlier, double prior_outlier);
|
|
|
|
TransformBtwRobotsUnaryFactorEM(size_t key, const T& relativePose, size_t keyA, size_t keyB,
|
|
const gtsam::Values& valA, const gtsam::Values& valB,
|
|
const gtsam::noiseModel::Gaussian* model_inlier, const gtsam::noiseModel::Gaussian* model_outlier,
|
|
double prior_inlier, double prior_outlier, bool flag_bump_up_near_zero_probs, bool start_with_M_step);
|
|
|
|
gtsam::Vector whitenedError(const gtsam::Values& x);
|
|
gtsam::Vector unwhitenedError(const gtsam::Values& x);
|
|
gtsam::Vector calcIndicatorProb(const gtsam::Values& x);
|
|
void setValAValB(const gtsam::Values valA, const gtsam::Values valB);
|
|
|
|
void updateNoiseModels(const gtsam::Values& values, const gtsam::NonlinearFactorGraph& graph);
|
|
void updateNoiseModels_givenCovs(const gtsam::Values& values, gtsam::Matrix cov1, gtsam::Matrix cov2, gtsam::Matrix cov12);
|
|
gtsam::Matrix get_model_inlier_cov();
|
|
gtsam::Matrix get_model_outlier_cov();
|
|
|
|
void serializable() const; // enabling serialization functionality
|
|
};
|
|
|
|
#include <gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h>
|
|
template<T = {gtsam::Pose2}>
|
|
virtual class TransformBtwRobotsUnaryFactor : gtsam::NonlinearFactor {
|
|
TransformBtwRobotsUnaryFactor(size_t key, const T& relativePose, size_t keyA, size_t keyB,
|
|
const gtsam::Values& valA, const gtsam::Values& valB,
|
|
const gtsam::noiseModel::Gaussian* model);
|
|
|
|
gtsam::Vector whitenedError(const gtsam::Values& x);
|
|
gtsam::Vector unwhitenedError(const gtsam::Values& x);
|
|
void setValAValB(const gtsam::Values valA, const gtsam::Values valB);
|
|
|
|
void serializable() const; // enabling serialization functionality
|
|
};
|
|
|
|
#include <gtsam_unstable/slam/SmartRangeFactor.h>
|
|
virtual class SmartRangeFactor : gtsam::NoiseModelFactor {
|
|
SmartRangeFactor(double s);
|
|
|
|
void addRange(size_t key, double measuredRange);
|
|
gtsam::Point2 triangulate(const gtsam::Values& x) const;
|
|
//void print(string s) const;
|
|
|
|
};
|
|
|
|
#include <gtsam/sam/RangeFactor.h>
|
|
template<POSE, POINT>
|
|
virtual class RangeFactor : gtsam::NoiseModelFactor {
|
|
RangeFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel);
|
|
|
|
void serializable() const; // enabling serialization functionality
|
|
};
|
|
|
|
typedef gtsam::RangeFactor<gtsam::PoseRTV, gtsam::PoseRTV> RangeFactorRTV;
|
|
|
|
double height() const;
|
|
void print(string s) const;
|
|
};
|
|
|
|
class TimeOfArrival {
|
|
|
|
#include <gtsam_unstable/slam/TOAFactor.h>
|
|
virtual class TOAFactor : gtsam::NonlinearFactor {
|
|
// For now, because of overload issues, we only expose constructor with known sensor coordinates:
|
|
TOAFactor(size_t key1, gtsam::Point3 sensor, double measured,
|
|
const gtsam::noiseModel::Base* noiseModel);
|
|
static void InsertEvent(size_t key, const gtsam::Event& event, gtsam::Values* values);
|
|
};
|
|
|
|
#include <gtsam/nonlinear/NonlinearEquality.h>
|
|
template<T = {gtsam::PoseRTV}>
|
|
virtual class NonlinearEquality : gtsam::NoiseModelFactor {
|
|
// Constructor - forces exact evaluation
|
|
NonlinearEquality(size_t j, const T& feasible);
|
|
// Constructor - allows inexact evaluation
|
|
NonlinearEquality(size_t j, const T& feasible, double error_gain);
|
|
|
|
void serializable() const; // enabling serialization functionality
|
|
};
|
|
|
|
#include <gtsam_unstable/dynamics/IMUFactor.h>
|
|
template<POSE = {gtsam::PoseRTV}>
|
|
virtual class IMUFactor : gtsam::NoiseModelFactor {
|
|
/** Standard constructor */
|
|
IMUFactor(gtsam::Vector accel, gtsam::Vector gyro,
|
|
double dt, size_t key1, size_t key2, const gtsam::noiseModel::Base* model);
|
|
|
|
/** Full IMU vector specification */
|
|
IMUFactor(gtsam::Vector imu_vector,
|
|
double dt, size_t key1, size_t key2, const gtsam::noiseModel::Base* model);
|
|
|
|
gtsam::Vector gyro() const;
|
|
gtsam::Vector accel() const;
|
|
gtsam::Vector z() const;
|
|
|
|
template <I = {1, 2}>
|
|
size_t key() const;
|
|
};
|
|
|
|
#include <gtsam_unstable/dynamics/FullIMUFactor.h>
|
|
template<POSE = {gtsam::PoseRTV}>
|
|
virtual class FullIMUFactor : gtsam::NoiseModelFactor {
|
|
/** Standard constructor */
|
|
FullIMUFactor(gtsam::Vector accel, gtsam::Vector gyro,
|
|
double dt, size_t key1, size_t key2, const gtsam::noiseModel::Base* model);
|
|
|
|
/** Single IMU vector - imu = [accel, gyro] */
|
|
FullIMUFactor(gtsam::Vector imu,
|
|
double dt, size_t key1, size_t key2, const gtsam::noiseModel::Base* model);
|
|
|
|
gtsam::Vector gyro() const;
|
|
gtsam::Vector accel() const;
|
|
gtsam::Vector z() const;
|
|
|
|
template <I = {1, 2}>
|
|
size_t key() const;
|
|
};
|
|
|
|
#include <gtsam_unstable/dynamics/DynamicsPriors.h>
|
|
virtual class DHeightPrior : gtsam::NonlinearFactor {
|
|
DHeightPrior(size_t key, double height, const gtsam::noiseModel::Base* model);
|
|
};
|
|
|
|
virtual class DRollPrior : gtsam::NonlinearFactor {
|
|
/** allows for explicit roll parameterization - uses canonical coordinate */
|
|
DRollPrior(size_t key, double wx, const gtsam::noiseModel::Base* model);
|
|
/** Forces roll to zero */
|
|
DRollPrior(size_t key, const gtsam::noiseModel::Base* model);
|
|
};
|
|
|
|
virtual class VelocityPrior : gtsam::NonlinearFactor {
|
|
VelocityPrior(size_t key, gtsam::Vector vel, const gtsam::noiseModel::Base* model);
|
|
};
|
|
|
|
virtual class DGroundConstraint : gtsam::NonlinearFactor {
|
|
// Primary constructor allows for variable height of the "floor"
|
|
DGroundConstraint(size_t key, double height, const gtsam::noiseModel::Base* model);
|
|
// Fully specify vector - use only for debugging
|
|
DGroundConstraint(size_t key, gtsam::Vector constraint, const gtsam::noiseModel::Base* model);
|
|
};
|
|
|
|
#include <gtsam_unstable/dynamics/VelocityConstraint3.h>
|
|
virtual class VelocityConstraint3 : gtsam::NonlinearFactor {
|
|
/** Standard constructor */
|
|
VelocityConstraint3(size_t key1, size_t key2, size_t velKey, double dt);
|
|
|
|
gtsam::Vector evaluateError(const double& x1, const double& x2, const double& v) const;
|
|
};
|
|
|
|
#include <gtsam_unstable/dynamics/Pendulum.h>
|
|
virtual class PendulumFactor1 : gtsam::NonlinearFactor {
|
|
/** Standard constructor */
|
|
PendulumFactor1(size_t k1, size_t k, size_t velKey, double dt);
|
|
|
|
gtsam::Vector evaluateError(const double& qk1, const double& qk, const double& v) const;
|
|
};
|
|
|
|
#include <gtsam_unstable/dynamics/Pendulum.h>
|
|
virtual class PendulumFactor2 : gtsam::NonlinearFactor {
|
|
/** Standard constructor */
|
|
PendulumFactor2(size_t vk1, size_t vk, size_t qKey, double dt, double L, double g);
|
|
|
|
gtsam::Vector evaluateError(const double& vk1, const double& vk, const double& q) const;
|
|
};
|
|
|
|
virtual class PendulumFactorPk : gtsam::NonlinearFactor {
|
|
/** Standard constructor */
|
|
PendulumFactorPk(size_t pk, size_t qk, size_t qk1, double h, double m, double r, double g, double alpha);
|
|
|
|
gtsam::Vector evaluateError(const double& pk, const double& qk, const double& qk1) const;
|
|
};
|
|
|
|
virtual class PendulumFactorPk1 : gtsam::NonlinearFactor {
|
|
/** Standard constructor */
|
|
PendulumFactorPk1(size_t pk1, size_t qk, size_t qk1, double h, double m, double r, double g, double alpha);
|
|
|
|
gtsam::Vector evaluateError(const double& pk1, const double& qk, const double& qk1) const;
|
|
};
|
|
|
|
#include <gtsam_unstable/dynamics/SimpleHelicopter.h>
|
|
virtual class Reconstruction : gtsam::NoiseModelFactor {
|
|
Reconstruction(size_t gKey1, size_t gKey, size_t xiKey, double h);
|
|
|
|
gtsam::Vector evaluateError(const gtsam::Pose3& gK1, const gtsam::Pose3& gK, gtsam::Vector xiK) const;
|
|
};
|
|
|
|
virtual class DiscreteEulerPoincareHelicopter : gtsam::NoiseModelFactor {
|
|
DiscreteEulerPoincareHelicopter(size_t xiKey, size_t xiKey_1, size_t gKey,
|
|
double h, gtsam::Matrix Inertia, gtsam::Vector Fu, double m);
|
|
|
|
gtsam::Vector evaluateError(gtsam::Vector xiK, gtsam::Vector xiK_1, const gtsam::Pose3& gK) const;
|
|
};
|
|
|
|
//*************************************************************************
|
|
// nonlinear
|
|
//*************************************************************************
|
|
#include <gtsam/nonlinear/FixedLagSmoother.h>
|
|
|
|
#include <gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h>
|
|
virtual class ConcurrentFilter {
|
|
void print(string s) const;
|
|
bool equals(const gtsam::ConcurrentFilter& rhs, double tol) const;
|
|
};
|
|
|
|
virtual class ConcurrentSmoother {
|
|
void print(string s) const;
|
|
bool equals(const gtsam::ConcurrentSmoother& rhs, double tol) const;
|
|
};
|
|
|
|
// Synchronize function
|
|
void synchronize(gtsam::ConcurrentFilter& filter, gtsam::ConcurrentSmoother& smoother);
|
|
|
|
#include <gtsam_unstable/nonlinear/ConcurrentBatchFilter.h>
|
|
class ConcurrentBatchFilterResult {
|
|
size_t getIterations() const;
|
|
size_t getLambdas() const;
|
|
size_t getNonlinearVariables() const;
|
|
size_t getLinearVariables() const;
|
|
double getError() const;
|
|
};
|
|
|
|
virtual class ConcurrentBatchFilter : gtsam::ConcurrentFilter {
|
|
ConcurrentBatchFilter();
|
|
ConcurrentBatchFilter(const gtsam::LevenbergMarquardtParams& parameters);
|
|
|
|
gtsam::NonlinearFactorGraph getFactors() const;
|
|
gtsam::Values getLinearizationPoint() const;
|
|
gtsam::Ordering getOrdering() const;
|
|
gtsam::VectorValues getDelta() const;
|
|
|
|
gtsam::ConcurrentBatchFilterResult update();
|
|
gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors);
|
|
gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta);
|
|
gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyList& keysToMove);
|
|
gtsam::Values calculateEstimate() const;
|
|
};
|
|
|
|
#include <gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h>
|
|
class ConcurrentBatchSmootherResult {
|
|
size_t getIterations() const;
|
|
size_t getLambdas() const;
|
|
size_t getNonlinearVariables() const;
|
|
size_t getLinearVariables() const;
|
|
double getError() const;
|
|
};
|
|
|
|
virtual class ConcurrentBatchSmoother : gtsam::ConcurrentSmoother {
|
|
ConcurrentBatchSmoother();
|
|
ConcurrentBatchSmoother(const gtsam::LevenbergMarquardtParams& parameters);
|
|
|
|
gtsam::NonlinearFactorGraph getFactors() const;
|
|
gtsam::Values getLinearizationPoint() const;
|
|
gtsam::Ordering getOrdering() const;
|
|
gtsam::VectorValues getDelta() const;
|
|
|
|
gtsam::ConcurrentBatchSmootherResult update();
|
|
gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors);
|
|
gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta);
|
|
gtsam::Values calculateEstimate() const;
|
|
};
|
|
|
|
//*************************************************************************
|
|
// slam
|
|
//*************************************************************************
|
|
#include <gtsam_unstable/slam/RelativeElevationFactor.h>
|
|
virtual class RelativeElevationFactor: gtsam::NoiseModelFactor {
|
|
RelativeElevationFactor();
|
|
RelativeElevationFactor(size_t poseKey, size_t pointKey, double measured,
|
|
const gtsam::noiseModel::Base* model);
|
|
|
|
double measured() const;
|
|
//void print(string s) const;
|
|
};
|
|
|
|
#include <gtsam_unstable/slam/DummyFactor.h>
|
|
virtual class DummyFactor : gtsam::NonlinearFactor {
|
|
DummyFactor(size_t key1, size_t dim1, size_t key2, size_t dim2);
|
|
};
|
|
|
|
#include <gtsam_unstable/slam/InvDepthFactorVariant1.h>
|
|
virtual class InvDepthFactorVariant1 : gtsam::NoiseModelFactor {
|
|
InvDepthFactorVariant1(size_t poseKey, size_t landmarkKey, const gtsam::Point2& measured, const gtsam::Cal3_S2* K, const gtsam::noiseModel::Base* model);
|
|
};
|
|
|
|
#include <gtsam_unstable/slam/InvDepthFactorVariant2.h>
|
|
virtual class InvDepthFactorVariant2 : gtsam::NoiseModelFactor {
|
|
InvDepthFactorVariant2(size_t poseKey, size_t landmarkKey, const gtsam::Point2& measured, const gtsam::Cal3_S2* K, const gtsam::Point3& referencePoint, const gtsam::noiseModel::Base* model);
|
|
};
|
|
|
|
#include <gtsam_unstable/slam/InvDepthFactorVariant3.h>
|
|
virtual class InvDepthFactorVariant3a : gtsam::NoiseModelFactor {
|
|
InvDepthFactorVariant3a(size_t poseKey, size_t landmarkKey, const gtsam::Point2& measured, const gtsam::Cal3_S2* K, const gtsam::noiseModel::Base* model);
|
|
};
|
|
virtual class InvDepthFactorVariant3b : gtsam::NoiseModelFactor {
|
|
InvDepthFactorVariant3b(size_t poseKey1, size_t poseKey2, size_t landmarkKey, const gtsam::Point2& measured, const gtsam::Cal3_S2* K, const gtsam::noiseModel::Base* model);
|
|
};
|
|
|
|
|
|
#include <gtsam_unstable/slam/Mechanization_bRn2.h>
|
|
class Mechanization_bRn2 {
|
|
Mechanization_bRn2();
|
|
Mechanization_bRn2(gtsam::Rot3& initial_bRn, gtsam::Vector initial_x_g,
|
|
gtsam::Vector initial_x_a);
|
|
gtsam::Vector b_g(double g_e);
|
|
gtsam::Rot3 bRn();
|
|
gtsam::Vector x_g();
|
|
gtsam::Vector x_a();
|
|
static gtsam::Mechanization_bRn2 initialize(gtsam::Matrix U, gtsam::Matrix F, double g_e);
|
|
gtsam::Mechanization_bRn2 correct(gtsam::Vector dx) const;
|
|
gtsam::Mechanization_bRn2 integrate(gtsam::Vector u, double dt) const;
|
|
//void print(string s) const;
|
|
};
|
|
|
|
#include <gtsam_unstable/slam/AHRS.h>
|
|
class AHRS {
|
|
AHRS(gtsam::Matrix U, gtsam::Matrix F, double g_e);
|
|
pair<gtsam::Mechanization_bRn2, gtsam::GaussianDensity*> initialize(double g_e);
|
|
pair<gtsam::Mechanization_bRn2, gtsam::GaussianDensity*> integrate(const gtsam::Mechanization_bRn2& mech, gtsam::GaussianDensity* state, gtsam::Vector u, double dt);
|
|
pair<gtsam::Mechanization_bRn2, gtsam::GaussianDensity*> aid(const gtsam::Mechanization_bRn2& mech, gtsam::GaussianDensity* state, gtsam::Vector f, bool Farrel);
|
|
pair<gtsam::Mechanization_bRn2, gtsam::GaussianDensity*> aidGeneral(const gtsam::Mechanization_bRn2& mech, gtsam::GaussianDensity* state, gtsam::Vector f, gtsam::Vector f_expected, const gtsam::Rot3& increment);
|
|
//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>
|
|
//typedef gtsam::NoiseModelFactorN<gtsam::Pose2, gtsam::Point2> NLPosePose;
|
|
virtual class DeltaFactor : gtsam::NoiseModelFactor {
|
|
DeltaFactor(size_t i, size_t j, const gtsam::Point2& measured,
|
|
const gtsam::noiseModel::Base* noiseModel);
|
|
//void print(string s) const;
|
|
};
|
|
|
|
//typedef gtsam::NoiseModelFactorN<gtsam::Pose2, gtsam::Pose2, gtsam::Pose2,
|
|
// gtsam::Point2> NLPosePosePosePoint;
|
|
virtual class DeltaFactorBase : gtsam::NoiseModelFactor {
|
|
DeltaFactorBase(size_t b1, size_t i, size_t b2, size_t j,
|
|
const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel);
|
|
//void print(string s) const;
|
|
};
|
|
|
|
//typedef gtsam::NoiseModelFactorN<gtsam::Pose2, gtsam::Pose2, gtsam::Pose2,
|
|
// gtsam::Pose2> NLPosePosePosePose;
|
|
virtual class OdometryFactorBase : gtsam::NoiseModelFactor {
|
|
OdometryFactorBase(size_t b1, size_t i, size_t b2, size_t j,
|
|
const gtsam::Pose2& measured, const gtsam::noiseModel::Base* noiseModel);
|
|
//void print(string s) const;
|
|
};
|
|
|
|
#include <gtsam/geometry/Cal3DS2.h>
|
|
#include <gtsam_unstable/slam/ProjectionFactorPPP.h>
|
|
#include <gtsam/geometry/Cal3Fisheye.h>
|
|
template<POSE, LANDMARK, CALIBRATION>
|
|
virtual class ProjectionFactorPPP : gtsam::NoiseModelFactor {
|
|
ProjectionFactorPPP(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel,
|
|
size_t poseKey, size_t transformKey, size_t pointKey, const CALIBRATION* k);
|
|
|
|
ProjectionFactorPPP(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel,
|
|
size_t poseKey, size_t transformKey, size_t pointKey, const CALIBRATION* k, bool throwCheirality, bool verboseCheirality);
|
|
|
|
gtsam::Point2 measured() const;
|
|
CALIBRATION* calibration() const;
|
|
bool verboseCheirality() const;
|
|
bool throwCheirality() const;
|
|
|
|
// enabling serialization functionality
|
|
void serialize() const;
|
|
};
|
|
typedef gtsam::ProjectionFactorPPP<gtsam::Pose3, gtsam::Point3, gtsam::Cal3_S2> ProjectionFactorPPPCal3_S2;
|
|
typedef gtsam::ProjectionFactorPPP<gtsam::Pose3, gtsam::Point3, gtsam::Cal3DS2> ProjectionFactorPPPCal3DS2;
|
|
typedef gtsam::ProjectionFactorPPP<gtsam::Pose3, gtsam::Point3, gtsam::Cal3Fisheye> ProjectionFactorPPPCal3Fisheye;
|
|
|
|
#include <gtsam_unstable/slam/ProjectionFactorPPPC.h>
|
|
template<POSE, LANDMARK, CALIBRATION>
|
|
virtual class ProjectionFactorPPPC : gtsam::NoiseModelFactor {
|
|
ProjectionFactorPPPC(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel,
|
|
size_t poseKey, size_t transformKey, size_t pointKey, size_t calibKey);
|
|
|
|
ProjectionFactorPPPC(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel,
|
|
size_t poseKey, size_t transformKey, size_t pointKey, size_t calibKey, bool throwCheirality, bool verboseCheirality);
|
|
|
|
gtsam::Point2 measured() const;
|
|
bool verboseCheirality() const;
|
|
bool throwCheirality() const;
|
|
|
|
// enabling serialization functionality
|
|
void serialize() const;
|
|
};
|
|
typedef gtsam::ProjectionFactorPPPC<gtsam::Pose3, gtsam::Point3, gtsam::Cal3_S2> ProjectionFactorPPPCCal3_S2;
|
|
typedef gtsam::ProjectionFactorPPPC<gtsam::Pose3, gtsam::Point3, gtsam::Cal3DS2> ProjectionFactorPPPCCal3DS2;
|
|
typedef gtsam::ProjectionFactorPPPC<gtsam::Pose3, gtsam::Point3, gtsam::Cal3Fisheye> ProjectionFactorPPPCCal3Fisheye;
|
|
|
|
#include <gtsam_unstable/slam/ProjectionFactorRollingShutter.h>
|
|
virtual class ProjectionFactorRollingShutter : gtsam::NoiseModelFactor {
|
|
ProjectionFactorRollingShutter(const gtsam::Point2& measured, double alpha, const gtsam::noiseModel::Base* noiseModel,
|
|
size_t poseKey_a, size_t poseKey_b, size_t pointKey, const gtsam::Cal3_S2* K);
|
|
|
|
ProjectionFactorRollingShutter(const gtsam::Point2& measured, double alpha, const gtsam::noiseModel::Base* noiseModel,
|
|
size_t poseKey_a, size_t poseKey_b, size_t pointKey, const gtsam::Cal3_S2* K, gtsam::Pose3& body_P_sensor);
|
|
|
|
ProjectionFactorRollingShutter(const gtsam::Point2& measured, double alpha, const gtsam::noiseModel::Base* noiseModel,
|
|
size_t poseKey_a, size_t poseKey_b, size_t pointKey, const gtsam::Cal3_S2* K, bool throwCheirality,
|
|
bool verboseCheirality);
|
|
|
|
ProjectionFactorRollingShutter(const gtsam::Point2& measured, double alpha, const gtsam::noiseModel::Base* noiseModel,
|
|
size_t poseKey_a, size_t poseKey_b, size_t pointKey, const gtsam::Cal3_S2* K, bool throwCheirality,
|
|
bool verboseCheirality, gtsam::Pose3& body_P_sensor);
|
|
|
|
gtsam::Point2 measured() const;
|
|
double alpha() const;
|
|
gtsam::Cal3_S2* calibration() const;
|
|
bool verboseCheirality() const;
|
|
bool throwCheirality() const;
|
|
|
|
// enabling serialization functionality
|
|
void serialize() const;
|
|
};
|
|
|
|
} //\namespace gtsam
|